A Multi-stage Probabilistic Algorithm for Dynamic Path-Planning

A Multi-stage Pr obabilistic Algorithm f or Dynamic Path-Planning Nicolas A. Barriga Departamento de Inform ´ atica Universidad T ´ ecnica F ederico Santa Mar ´ ıa V alparaiso, Chile nbarriga@inf.utfsm.cl Mauricio Araya-L ´ opez MAIA Equipe INRIA/LORIA Nancy , F rance mauricio.araya@loria.fr Abstract —Probabilistic sampling methods hav e become very popular to solve single-shot path planning problems. Rapidly- exploring Random T rees (RR Ts) in particular have been shown to be efficient in solving high dimensional problems. Ev en though several RRT variants hav e been proposed for dynamic replanning, these methods only perform well in en vir onments with infrequent changes. This paper addr esses the dynamic path planning problem by combining simple techniques in a multi-stage probabilistic algorithm. This algorithm uses RR Ts for initial planning and informed local search for navigation. W e show that this combination of simple techniques provides better responses to highly dynamic en vir onments than the RR T extensions. Keyw ords -artificial intelligence; motion planning; RR T ; Multi-stage; local search; greedy heuristics; I . I N T RO D U C T I O N The dynamic path-planning problem consists in finding a suitable plan for each new configuration of the en vironment by recomputing a free-collision path using the new informa- tion av ailable at each time step [5]. This kind of problem can be found for example by a robot trying to navigate through an area crowded with people, such as a shopping mall or supermarket. The problem has been addressed widely in its sev eral flavors, such as cellular decomposition of the con- figuration space [12], partial en vironmental knowledge [11], high-dimensional configuration spaces [6] or planning with non-holonomic constraints [8]. Howe v er , simpler v ariations of this problem are complex enough that cannot be solved with deterministic techniques, and therefore they are worthy to study . This paper is focused on finding and tra versing a collision- free path in two dimensional space, for a holonomic robot 1 , without kinodynamic restrictions 2 , in two dif ferent scenar- ios: • sev eral unpredictably moving obstacles or adversaries. • partially known environment, when at some point in time, a new obstacle is found. 1 A holonomic robot is a robot in which the controllable degrees of freedom is equal to the total degrees of freedom. 2 Kinodynamic planning is a problem in which v elocity and acceleration bounds must be satisfied Besides from one (or fe w) ne w obstacle(s) in the second scenario we assume that we have perfect information of the en vironment at all times. W e will focus on continuous space algorithms and won’t consider algorithms that use discretized representations of the configuration space, such as D* [12], because for high dimensional problems, the configuration space becomes in- tractable in terms of both memory and computation time, and there is the extra difficulty of calculating the discretization size, trading off accuracy versus computational cost. The offline RR T is efficient at finding solutions but they are far from being optimal, and must be post-processed for shortening, smoothing or other qualities that might be de- sirable in each particular problem. Furthermore, replanning RR Ts are costly in terms of computation time, as well as ev olutionary and cell-decomposition approaches. Therefore, the novelty of this work is the mixture of the feasibility benefits of the RR Ts, the repairing capabilities of local search, and the computational inexpensi v eness of greedy algorithms, into our lightweight multi-stage algorithm. In the follo wing sections, we present se v eral path planning methods that can be applied to the problem described abov e. In section II-A we revie w the basic offline, single-query RR T , a probabilistic method that builds a tree along the free configuration space until it reaches the goal state. Afterwards, we introduce the most popular replanning vari- ants of the RR T : ERR T in section II-B, DRR T in section II-C and MP-RR T in section II-D. Then, in section III we present our ne w hybrid multi-stage algorithm with the experimental results and comparisons in section IV. At last, the conclusions and further work are discussed in section V. I I . P R E V I O U S A N D R E L A T E D W O R K A. Rapidly-Exploring Random T r ee One of the most successful probabilistic sampling meth- ods for offline path planning currently in use, is the Rapidly- exploring Random T ree (RR T), a single-query planner for static en vironments, first introduced in [9]. RR Ts work tow ards finding a continuous path from a state q init to a state q g oal in the free configuration space C f ree , by building a tree rooted at q init . A ne w state q rand is uniformly sampled at random from the configuration space C . Then the nearest node, q near , in the tree is located, and if q rand and the shortest path from q rand to q near are in C f ree , then q rand is added to the tree. The tree growth is stopped when a node is found near q g oal . T o speed up con v ergence, the search is usually biased to q g oal with a small probability . In [7], tw o new features are added to RR Ts. First, the EXTEND function is introduced, which, instead of trying to add directly q rand to the tree, mak es a motion towards q rand and tests for collisions. Then a greedier approach is introduced, which repeats EXTEND until an obstacle is reached. This ensures that most of the time, we will be adding states to the tree, instead of just rejecting new random states. The second extension is the use of two trees, rooted at q init and q g oal , which are grown tow ards each other . This significantly decreases the time needed to find a path. B. ERRT The execution extended RR T presented in [3] introduces two RR Ts extensions to build an on-line planner: the way- point cache and the adaptive cost penalty searc h , which improv es re-planning efficiency and the quality of generated paths. The waypoint cache is implemented by keeping a constant size array of states, and whenever a plan is found, all the states in the plan are placed in the cache with random replacement. Then, when the tree is no longer valid, a new tree must be grown, and there are three possibilities for choosing a ne w target state. W ith probability P[ goal ], the goal is chosen as the target; W ith probability P[ waypoint ], a random waypoint is chosen, and with remaining probability a uniform state is chosen as before. V alues used in [3] are P[ goal ] = 0 . 1 and P[ waypoint ] = 0 . 6 . In the other extension — the adaptiv e cost penalty search — the planner dynamically modifies a parameter β to help it finding shorter paths. A value of 1 for β will always extend from the root node, while a value of 0 is equiv alent to the original algorithm. Unfortunately , the solution presented in [3] lacks of implementation details and experimental results on this extension. C. Dynamic RRT The Dynamic Rapidly-exploring Random Tree (DRR T) described in [4] is a probabilistic analog to the widely used D* family of algorithms. It works by gro wing a tree from q g oal to q init . The principal advantage is that the root of the tree does not hav e to be changed during the lifetime of the planning and execution. Also, in some problem classes the robot has limited range sensors, thus moving obstacles (or new ones) are typically near the robot and not near the goal. In general, this strategy attempts to trim smaller branches and farther aw ay from the root. When new information concerning the configuration space is receiv ed, the algorithm remov es the ne wly-in v alid branches of the tree, and grows the remaining tree, focusing, with a certain probability(empirically tuned to 0 . 4 in [4]) to a vicinity of the recently trimmed branches, by using the a similar structure to the waypoint cache of the ERR T . In experimental results DRR T vastly outperforms ERR T . D. MP-RRT The Multipartite RR T presented in [14] is another RR T variant which supports planning in unknown or dynamic en vironments. The MP-RR T maintains a forest F of dis- connected sub-trees which lie in C f ree , b ut which are not connected to the root node q root of T , the main tree. At the start of a given planning iteration, any nodes of T and F which are no longer valid are deleted, and any disconnected sub-trees which are created as a result are placed into F . W ith giv en probabilities, the algorithm tries to connect T to a new random state, to the goal state, or to the root of a tree in F . In [14], a simple greedy smoothing heuristic is used, that tries to shorten paths by skipping intermediate nodes. The MP-RR T is compared to an iterated RR T , ERR T and DRR T , in 2D, 3D and 4D problems, with and without smoothing. For most of the experiments, MP-RR T modestly outperforms the other algorithms, but in the 4D case with smoothing, the performance gap in fa vor of MP-RR T is much larger . The authors explained this fact due to MP-RR T being able to construct much more robust plans in the face of dynamic obstacle motion. Another algorithm that utilizes the concept of forests is the Reconfigurable Random Forests (RRF) presented in [10], but without the success of MP-RR T . I I I . A M U LT I - S T AG E P RO B A B I L I S T I C A L G O R I T H M In highly dynamic en vironments, with many (or a few but fast) relativ ely small moving obstacles, regro wing trees are pruned too fast, cutting aw ay important parts of the trees before they can be replaced. This reduce dramatically the performance of the algorithms, making them unsuitable for these class of problems. W e believ e that a better performance could be obtained by slightly modifying a RR T solution using simple obstacle-av oidance operations on the new col- liding points of the path by informed local search. Then, the path could be greedily optimized if the path has reached the feasibility condition. A. Pr oblem F ormulation At each time-step, the proposed problem could be defined as an optimization problem with satisfiability constraints. Therefore, giv en a path our objective is to minimize an ev aluation function (i.e. distance, time, or path-points), with the C f ree constraint. Formally , let the path ρ = p 1 p 2 . . . p n a sequence of points, where p i ∈ R n a n -dimensional point ( p 1 = q init , p n = q g oal ), O t ∈ O the set of obstacles positions at time t , and ev al : R n × O 7→ R an ev aluation function of the path depending on the object positions. Then, our ideal objectiv e is to obtain the optimum ρ ∗ path that minimize our ev al function within a feasibility restriction in the form ρ ∗ = ar g min ρ [ ev al ( ρ, O t )] with f eas ( ρ, O t ) = C f ree (1) where f eas ( · , · ) is a feasibility function that equals to C f ree iff the path ρ is collision free for the obstacles O t . For simplicity , we use very nai ve ev al ( · , · ) and f eas ( · , · ) functions, but this could be extended easily to more complex ev aluation and feasibility functions. The used f eas ( ρ, O t ) function assumes that the robot is a punctual object (dimen- sionless) in the space, and therefore, if all segments − − − − → p i p i +1 of the path do not collide with any object o j ∈ O t , we say that the path is in C f ree . The ev al ( ρ, O t ) function will be the points count of ρ , assuming that similar paths with less points are shorter . This could be easily changed to the euclidean distance, time, smoothness, clearness or several other optimization criterions. B. A Multi-stage Pr obabilistic Strate gy If solving equation 1 is not a simple task in static en vironments, solving dynamic versions turns out to be ev en more difficult. In dynamic path planning we cannot wait until reaching the optimal solution because we must deliv er a “good enough” plan within some time quantum. Then, a heuristic approach must be developed to tackle the on-line nature of the problem. The heuristic algorithms presented in sections II-B, II-C and II-D, extend a method developed for static en vironments, which produce a poor response to highly dynamic environments and an unwanted complexity of the algorithms. W e propose a multi-stage combination of three simple heuristic probabilistic techniques to solv e each part of the problem: feasibility , initial solution and optimization. 1) F easibility: The key point in this problem is the hard constraint in equation 1 which must be met before ev en thinking about optimizing. The problem is that in highly dynamic environments a path turns rapidly from feasible to unfeasible — and the other way around — even if our path does not change. W e propose a simple informed local sear ch to obtain paths in C f ree . The idea is to randomly search for a C f ree path by modifying the nearest colliding segment of the path. As we include in the search some knowledge of the problem, the informed term is coined to distinguish it from blind local search. The details of the operators used for the modification of the path are described in section III-C. 2) Initial Solution: The problem with local search algo- rithms is that they repair a solution that it is assumed to be near the feasibility condition. Trying to produce feasible paths from scratch with local search (or even with ev olution- ary algorithms [13]) is not a good idea due the randomness of the initial solution. Therefore, we propose feeding the informed local search with a standard RRT solution at the start of the planning, as can be seen in figure 1. Figure 1. A Multi-stage Strategy for Dynamic Path Planning . This figure describes the life-cycle of the multi-stage algorithm presented here. The RR T , informed local search, and greedy heuristic are combined to produce an expensi veness solution to the dynamic path planning problem. 3) Optimization: W ithout an optimization criteria, the path could grow infinitely lar ge in time or size. Therefore, the eval ( · , · ) function must be minimized when a (tempo- rary) feasible path is obtained. A simple gr eedy technique is used here: we test each point in the solution to check if it can be removed maintaining feasibility , if so, we remove it and check the follo wing point, continuing until reaching the last one. C. Algorithm Implementation Algorithm 1. Main() Require: q robot ← is the current robot position Require: q g oal ← is the goal position 1: while q robot 6 = q g oal do 2: updateW orld ( time ) 3: process ( time ) The multi-stage algorithm proposed in this paper w orks by alternating en vironment updates and path planning, as can be seen in Algorithm 1. The first stage of the path planning (see Algorithm 2) is to find an initial path using a RR T technique, ignoring any cuts that might happen during en vironment updates. Thus, the RR T ensures that the path found does not collide with static obstacles, b ut might collide with dynamic obstacles in the future. When a first path is found, the navigation is done by alternating a simple informed local search and a simple greedy heuristic as is shown in Figure 1. Algorithm 2. process ( time ) Require: q robot ← is the current robot position Require: q start ← is the starting position Require: q g oal ← is the goal position Require: T init ← is the tree rooted at the robot position Require: T g oal ← is the tree rooted at the goal position Require: path ← is the path extracted from the merged RR Ts 1: q robot ← q start 2: T init .init ( q robot ) 3: T g oal .init ( q g oal ) 4: while time elapsed < time do 5: if first path not found then 6: RR T ( T init , T g oal ) 7: else 8: if path is not collision free then 9: firstCol ← collision point closest to robot 10: arc ( path, f ir stC ol ) 11: mut ( path, f ir stC ol ) 12: postProcess ( path ) Figure 2. The arc operator . This operator draws an of fset value ∆ over a fixed interval called vicinity . Then, one of the two axises is selected to perform the arc and two new consecutive points are added to the path. n 1 is placed at a ± ∆ of the point b and n 2 at ± ∆ of point c , both of them over the same selected axis. The axis, sign and value of ∆ are chosen randomly from an uniform distribution. The second stage is the informed local search, which is a two step function composed by the arc and mutate operators (Algorithms 3 and 4). The first one tries to build a square arc around an obstacle, by inserting two new points Figure 3. The mutation operator . This operator dra ws two offset v alues ∆ x and ∆ y over a vicinity re gion. Then the same point b is moved in both axises from b = [ b x , b y ] to b 0 = [ b x ± ∆ x , b y ± ∆ y ] , where the sign and offset v alues are chosen randomly from an uniform distribution. between two points in the path that form a segment colliding with an obstacle, as is shown in Figure 2. The second step in the function is a mutation operator that mov es a point close to an obstacle to a random point in the vicinity , as is graphically explained in Figure 3. The mutation operator is inspired by the ones used in the Adaptiv e Ev olutionary Planner/Navigator(EP/N) presented in [13], while the arc operator is deriv ed from the arc operator in the Evolutionary Algorithm presented in [1]. Algorithm 3. arc ( path, f irstC ol ) Require: vicinity ← some vicinity size 1: randDev ← random ( − v icinity , v icinity ) 2: point1 ← path[firstCol] 3: point2 ← path[firstCol+1] 4: if random ()%2 then 5: newPoint1 ← (point1[X]+randDev ,point1[Y]) 6: newPoint2 ← (point2[X]+randDev ,point2[Y]) 7: else 8: newPoint1 ← (point1[X],point1[Y]+randDev) 9: newPoint2 ← (point2[X],point2[Y]+randDev) 10: if path se gments point1-ne wPoint1-ne wPoint2-point2 are collision free then 11: add new points between point1 and point2 12: else 13: drop new point2 The third and last stage is the greedy optimization heuristic, which can be seen as a post-processing for path shortening, that eliminates intermediate nodes if doing so does not create collisions, as is described in the Algorithm 5. I V . E X P E R I M E N T S A N D R E S U LTS The multi-stage strategy proposed here has been dev el- oped to navig ate highly-dynamic en vironments, and there- fore, our experiments should be aimed towards that purpose. Algorithm 4. mut ( path, f ir stC ol ) Require: vicinity ← some vicinity size 1: path[firstCol][X] + = random ( − v icinity , v icinity ) 2: path[firstCol][Y] + = random ( − v icinity , v icinity ) 3: if path segments before and after path[firstCol] are collision free then 4: accept new point 5: else 6: reject new point Algorithm 5. postProcess ( path ) 1: i ← 0 2: while i < path.size () -2 do 3: if segment path[i] to path[i+2] is collision free then 4: delete path[i+1] 5: else 6: i ← i+1 Therefore, we have tested our algorithm in two highly- dynamic situations, both of them over a map representing an office b uilding or shopping mall (i.e. with some static walls). Also, we have ran the DRR T and MP-RR T algorithms ov er the same situations in order to compare the performance of our proposal. A. Experimental Setup Figure 4. The dynamic environment. The gr een square is our robot, currently at the start position. The blue squares are the moving obstacles. The blue cross is the goal. The first environment for our e xperiments consists on a map with 30 moving obstacles the same size of the robot, with a random speed between 10% and 55% the speed of the robot. This dynamic en vir onment is shown in figure 4. Figure 5. The partially know environment. The gr een square is our robot, currently at the start position. The black squares are the suddenly appearing obstacles. The blue cross is the goal. The second en vironment uses the same map, but with six obstacles, three to four times the size of the robot, appearing at a predefined time and position. This partially known en vir onment is shown in figure 5. The three algorithms were ran a hundred times in each en vironment. The cutoff time was five minutes for the first en vironment and one minute for the second, after which, the robot was considered not to have reached the goal. B. Implementation Details The algorithms where implemented in C++ using a framew ork 3 dev eloped by the same authors. There are several variations that can be found in the liter- ature when implementing RR Ts. For all our RR T v ariants, the following are the details on where we departed from the basics: • W e always use two trees rooted at q init and q g oal . • Our EXTEND function, if the point cannot be added without collisions to a tree, adds the mid point between the nearest tree node and the nearest collision point to it. • In each iteration, we try to add the new randomly generated point to both trees, and if successful in both, the trees are merged, as proposed in [7]. • W e found that the success rate was somewhat lower if we allow the robot to advance towards the node nearest to the goal when the trees are disconnected, as proposed in [14]. The problem is that the robot would become stuck if it enters a small concave zone of the 3 MoPa homepage: https://csrg.inf.utfsm.cl/twiki4/bin/view/CSRG/MoP a en vironment(like a room in a building) while there are moving obstacles inside that zone. Therefore our robot only mov es when the trees are connected. In MP-RR T , the forest was handled simply replacing the oldest tree in it if the forest had reached the maximum size allowed. Concerning the parameter selection, the probability for selecting a point in the vicinity of a point in the w aypoint cache in DRR T was set to 0.4 as suggested in [4]. The probability for trying to reuse a sub tree in MP-RR T was set to 0.1 as suggested in [14]. Also, the forest size was set to 25 and the minimum size of a tree to be sav ed in the forest was set to 5 nodes. C. Dynamic En vir onment Results The results in table I show that it takes our algorithm around a third of the time it takes the DRR T and MP-RR T to get to the goal, with f ar less collision checks. It w as e xpected that nearest neighbor lookups would be much lo wer in the multi-stage algorithm than in the other two, because they are only performed in the RR T phase, not during navigation. Howe ver , the multi-stage algorithm seems to be slighty less dependable, as it arri ved to the goal 98 out of 100 times, while the other two managed to arrive always. T able I D Y N AM I C E N V I RO N ME N T R E S U L TS . A V E RA G E R E SU LT S OV E R 1 0 0 RU N S , W I T H 5 M IN U T E S C U TO FF Algorithm Success % Coll. Checks Nearest Neigh. Time[s] Multi-stage 98 24364 1468 7.08 DRR T 100 92569 4536 19.81 MP-RR T 100 97517 4408 21.53 D. P artially Known En vir onment Results The results in table II show that our multi-stage algorithm is very undependable, though faster than the other two when it actually reaches the goal. Due to the simplicity of our local search, and that it basically just avoids obstacles by stepping to the side or letting the obstacle move out of the way , when the changes to the environment are significant and obstacles do not mov e, it is very prone to getting stuck. T able II P A RT IA L LY K N OW N E N VI R ON M E N T R E SU LT S . A V E R AG E R E S ULT S OV E R 1 0 0 R UN S , W I T H 1 MI N U T E C U TO FF Algorithm Success % Coll. Checks Nearest Neigh. Time[s] Multi-stage 44 4856 673 5.95 DRR T 100 9845 1037 7.25 MP-RR T 98 17029 1156 8.13 V . C O N C L U S I O N S The new multi-stage algorithm proposed here has a very good performance in very dynamic environments. It behaves particularly well when sev eral small obstacles are moving around seemingly randomly . It’ s major shortcoming is that it gets easily stuck when significant changes to the environ- ment are made, such as big static obstacles appearing near the robot, a situation usually considered as a partially known en vironment. A. Futur e W ork There are sev eral areas of improv ement for the work presented in this paper . First of all, the multi-stage algorithm must recognize a situation where it is stuck, and restart an RR T from the current location, before continuing with the na vigation phase. The detection could be as simple as recognizing that the robot has not mov ed out of a certain vicinity for a gi ven period of time, or that the next collision in the planned path has been against the same obstacle during a giv en period of time, meaning that the local search has been unable to find a path around it. This will yield a much more dependable algorithm in different kinds of en vironments. A second area of improvement is to experiment with different on-line planners such as the EP/N presented in [13], a version of the EvP([1] and [2]) modified to work in continuous configuration space or a potential field navigator . Also, the local search presented here, could benefit from the use of more sophisticated operators. A third area of research that could be tackled is extending this algorithm to other types of en vironments, ranging from totally known and very dynamic, to static partially known or unknown environments. An extension to higher dimensional problems would be one logical way to go, as RR Ts are know to work well in higher dimensions. Finally , as RR Ts are suitable for kinodynamic planning, we only need to adapt the on-line stage of the algorithm to hav e a ne w multi-stage planner for problem with kinody- namic constraints. R E F E R E N C E S [1] T . Alfaro and M. Riff. An On-the-fly Evolutionary Algorithm for Robot Motion Planning. Lectur e Notes in Computer Science , 3637:119, 2005. [2] T . Alfaro and M. Riff. An Evolutionary Navigator for Autonomous Agents on Unknown Large-Scale En vironments. INTELLIGENT A UTOMA TION AND SOFT COMPUTING , 14(1):105, 2008. [3] J. Bruce and M. V eloso. Real-time randomized path planning for robot navigation. Intelligent Robots and System, 2002. IEEE/RSJ International Confer ence on , 3:2383–2388 v ol.3, 2002. [4] D. Ferguson, N. Kalra, and A. Stentz. Replanning with rrts. Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Confer ence on , pages 1243–1248, 15-19, 2006. [5] Y . K. Hw ang and N. Ahuja. Gross motion planning—a surv ey . A CM Comput. Surv . , 24(3):219–291, 1992. [6] L. Kavraki, P . Svestka, J.-C. Latombe, and M. Ov ermars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. Robotics and Automation, IEEE T rans- actions on , 12(4):566–580, Aug 1996. [7] J. Kuf fner , J.J. and S. LaV alle. Rrt-connect: An ef ficient approach to single-query path planning. Robotics and Au- tomation, 2000. Pr oceedings. ICRA ’00. IEEE International Confer ence on , 2:995–1001 vol.2, 2000. [8] S. LaV alle and J. Ku. Randomized kinodynamic planning, 1999. [9] S. M. Lav alle. Rapidly-exploring random trees: A new tool for path planning. T echnical report, Computer Science Dept., Iow a State Univ ., 1998. [10] T .-Y . Li and Y .-C. Shie. An incremental learning approach to motion planning with roadmap management. Robotics and Automation, 2002. Pr oceedings. ICRA ’02. IEEE Inter- national Conference on , 4:3411–3416 vol.4, 2002. [11] A. Stentz. Optimal and efficient path planning for partially- knownen vironments. In 1994 IEEE International Confer- ence on Robotics and Automation, 1994. Pr oceedings. , pages 3310–3317, 1994. [12] A. Stentz. The Focussed Dˆ* Algorithm for Real-T ime Replanning. In International Joint Conference on Artificial Intelligence , volume 14, pages 1652–1659. LA WRENCE ERLB A UM ASSOCIA TES L TD, 1995. [13] J. Xiao, Z. Michalewicz, L. Zhang, and K. Trojano wski. Adaptiv e evolutionary planner/navigator for mobile robots. Evolutionary Computation, IEEE T ransactions on , 1(1):18– 28, Apr 1997. [14] M. Zucker , J. Kuf fner , and M. Branicky . Multipartite rrts for rapid replanning in dynamic environments. Robotics and Automation, 2007 IEEE International Conference on , pages 1603–1609, April 2007.