Greedy but Safe Replanning under Kinodynamic Constraints

Transcription

1 Intl. Conf. on Robotic and Automation, IEEE pre, Rome, Italy, p , 2007 Greedy but Safe Replanning under Kinodynamic Contraint Kota E. Bekri Lydia E. Kavraki Abtract We conider motion planning problem for a vehicle with kinodynamic contraint, where there i partial knowledge about the environment and replanning i required. We preent a new tree-baed planner that explicitly deal with kinodynamic contraint and addree the afety iue when planning under finite computation time, meaning that the vehicle avoid colliion in it evolving configuration pace. In order to achieve good performance we incrementally update a tree data-tructure by retaining information from previou tep and we bia the earch of the planner with a greedy, yet probabilitically complete tate pace exploration trategy. Moreover, the number of colliion check required to guarantee afety i kept to a minimum. We compare our technique with alternative approache a a tandalone planner and how that it achieve favorable performance when planning with dynamic. We have applied the planner to olve a challenging replanning problem involving the mapping of an unknown workpace with a non-holonomic platform. I. INTRODUCTION A automobile and other mobile platform achieve a higher degree of autonomy, generating afe and effective motion for autonomou vehicle emerge a a great application for motion planning. In realitic tak, however, vehicle have only partial information about their environment. Solving uch problem require interleaving ening, planning and execution, where a planner i called frequently and ha finite time to replan a trajectory [1] [3]. Moreover, vehicle exhibit kinodynamic contraint that retrict their motion, which mut be accounted for at the motion planning tage o that a planned trajectory can be followed. In thi paper, a treebaed planner i preented that repect uch contraint and generate afe path under finite computation time. Safety mean that the vehicle doe not collide with obtacle even when the configuration pace of the robot change. Thi i an important conideration when replanning for real vehicle a a colliion-free trajectory may bring the ytem cloe to an obtacle with high-velocity and no maneuver to avoid colliion [4], [5]. For replanning application, the computational performance of the planner i of primary importance. A low planner delay taking into account new ening information and the vehicle doe not react on time to change in the workpace. In the propoed framework, the planner itelf elect the duration of the planning cycle, which allow the implementation of a lazy evaluation approach to reduce the Work on thi paper ha been upported in part by NSF , NSF , and a Sloan Fellowhip to LEK. Experiment reported in thi paper ha been obtained on equipment upported by NSF CNS , NSF CNS in partnerhip with Rice Univerity, AMD, and Cray. Kota E. Bekri and Lydia E. Kavraki are with the Computer Science Department, Rice Univerity, Houton, TX, 77005, USA {bekri,kavraki}@rice.edu Fig. 1. Mapping an unknown pace with an acceleration controlled car. overhead of colliion checking. It alo ue computation executed during previou planning period and an effective tate pace exploration cheme to bia it earch. Thi paper alo decribe the application of the planner to a tak that require replanning: mapping of unknown environment. Initially the vehicle know only a mall part of the pace but it mut eventually cover the entire workpace. Fig. 1 how an example from our imulation with a car-like robot with bounded acceleration. We have experimented with variou mobile platform on different cene type and compared the approach a a tandalone planner againt alternative planner. We preent reult that demontrate the algorithm favorable performance and it effectivene in replanning under kinodynamic contraint, limited computation time and a dynamic repreentation of the workpace. A. Related Literature Sampling-baed planner have proved effective in dealing with high-dimenional problem [6] and, in particular, treelike planner [7], [8], have been ucceful dealing with dynamic contraint. Epecially, the Path-Directed Subdiviion Tree (PDST) planner [9] ha hown good performance in uch planning intance and allow biaing the earch of the algorithm while providing probabilitic completene. Typical tree-baed planner aume complete workpace knowledge. Hu et al. [8] have experimented with a real robot that navigate among moving obtacle by replanning from cratch. Bruce and Veloo [1] decribe way of uing previou call to the RRT algorithm to bia the exploration of a new tree. Ferguon et al. [2] repair RRT while replanning by creating a probabilitic analog to the D* family of determinitic replanning method [10]. Van den Berg et al. [3] create a roadmap that cover the planning pace and later

2 replan uing thi graph. Van den Verg and Overmar [11] provide algorithm for computing hortet afe path amidt moving obtacle. Safety iue related to replanning and kinodynamic contraint arie when colliion-free tate are Inevitable Colliion State (ICS). The notion of ICS ha been employed in the partial motion planning framework of Fraichard et al. [4], [12]. Frazolli et al. [5] introduced the notion of τ-afety and ued RRT that guaranteed tate along the produced tree were τ-afe. B. Contribution Our goal i to combine the above contribution with new idea to define a ingle, efficient framework for replanning under kinodynamic contraint. In particular: (a) We follow an incremental approach imilar to method that repair RRT [1], [2], but we deal explicitly with kinodynamic contraint and afety iue. (b) The planner ue the ICS formalization [4] and provide afety guarantee imilar to τ-afety [5]. It reduce, however, the cot of achieving afety by controlling the duration of the planning cycle and employing lazy evaluation [13]. In thi way, the number of tate that mut be checked for afety are reduced, reulting in coniderable peedup a our experiment how. (c) A tate pace exploration cheme imilar to PDST [9] i applied, but the earch i biaed uing information eaily extracted directly from enor data. Our experiment how that the propoed algorithm perform favorably againt the Voronoi-bia election trategie of RRT given the ame metric [7]. In the following ection we formally decribe the problem that the propoed motion planner olve. A detailed decription of the planer, called GRIP for GReedy, Incremental, Path-directed planner, i provided in ection III. Section IV illutrate the application of the planner to a mapping tak with non-holonomic robot. The etup for our experiment and our reult are preented in ection V and VI. II. PROBLEM OVERVIEW Aume a drift-le non-holonomic robot whoe motion i governed by: q = f(q, u) and g(q, q) 0, where q Q i a tate, u i a control and f, g are mooth. Q i the tate pace of the robot with a metric ρ(q 1, q 2 ), where q 1, q 2 Q. The robot i in a workpace W, equipped with a enor of limited range. The robot ue it enor to update an evolving workpace repreentation (W, t). A tate q Q free i conidered colliion-free at time t if it place the robot chai in the known colliion-free part of (W, t). Our framework i applicable to multiple tak where the workpace change dynamically. We focu on the cae the robot doe not know anything about the workpace and mut cover it in order to build a map. A imilar dynamic tak not covered here due to pace limitation i planning among dynamic obtacle. Our approach toward thee high-level tak i to break them into a equence of maller planning problem. To achieve thi, we ynchronize the mapping unit, the planner and the motion controller a hown by Fig. 2. Fig. 2. The robot ynchronization cheme. Then for the planning cycle (t i 1 : t i ), the following tep are executed: A repreentation (W, t i 1 ) built in the previou cycle. A target F ti 1 Q i defined for the current cycle. A motion planner compute a plan. The plan will be executed during cycle (t i : t i+1 ). A plan i a time-equence of control: p(dt) = {(u 1, dt 1 ),..., (u n, dt n )} where dt = i dti. When a plan p(dt) of duration dt i executed at tate q, it define a trajectory tr(q, p(dt)), which i the equence of tate propagated according to q = f(q, u). A trajectory that repect g(q, q) 0 i called feaible, and if it lie in Q free i colliion free. The motion planner olve the following problem: Replanning under Kinodynamic Contraint: Given: the latet repreentation (W, t i 1 ) a et of target tate F ti 1 Q the initial robot tate q at time t i the tree tructure from the previou cycle (t i 2 : t i 1 ). Compute plan p(t) to execute during (t i : t i+1 ) that produce tr(q, p(t)) which i: (a) colliion-free, (b) lead to F ti 1, (c) minimize the ditance traveled by the robot and (d) doe not lead to Inevitable Colliion State [4] pat t i+1. III. THE PLANNER: GRIP The GRIP (Greedy, Incremental, Path-directed) planner expand greedily and incrementally a tree data tructure in the tate-time pace and return afe path for kinodynamic ytem. The planner contain: 1) A retainment tep that reue part of the previou tree (III-A). 2) An exploration trategy that achieve probabilitic completene but allow greedy earch (III-B). 3) Safety checking o that the elected plan do not lead to inevitable colliion (III-C). A. Tree Retainment The algorithm ue a planning horizon longer than the duration of the next planning cycle, e.g. (t i : t i+1 ). Thi implie that a large part of the tree contructed during the previou planning cycle may till be valid and it can be ued to accelerate the earch for a new path given new enor data imilarly to Dynamic RRT [2]. Note, however, that kinodynamic contraint add an additional limitation, ince it i not poible to go backward along the tree. The ubtree of the initial robot tate q for the new planning cycle i valid for planning and everything above it i unreachable and mut be dicarded. Trimming other part of the tree that are invalid due to unexpected colliion can imilarly be executed. Moreover, if a path to the deired target exit in the remaining tree, every path that doe not lead to the

3 Fig. 3. An illutration of GRIP exploration. A dicrete potential function i ued for ρ(e, F). The number denote edge prioritie p(e). The econd figure how an edge broken into: e 1 and e 2, while a new edge e i propagated from tate qr. The lat figure how the final tree that reache the target. target can alo be pruned. In thi way, when a path ha been found, the technique focue on obtaining plan of increaing quality. There are alo computation during tree retainment that are related to tate-pace exploration and plan afety, which will be decribed in the following ection. B. Selection/Propagation: Completene with Bia We follow the election/propagation cheme for exploring the tate-pace, typical for tree-baed planner [7]. A tate from the tree i elected for propagating a trajectory forward in time. Our approach follow the PDST algorithm [9], which determinitically biae the earch, o a to earch intereting part of the tate pace fater. Although the earch i biaed, the approach i till probabilitically complete. The baic ampling module i a path, which correpond to an edge of the tree data tructure. In thi way, all the tate along the tree are candidate for propagation. The algorithm firt determinitically elect an edge e to expand from. The actual tate q r ued for propagation i randomly ampled along the edge e. Edge e i then broken into two part e 1 and e 2 by creating a node at the elected tate qr. For the new edge e, a random plan p(t max ) of maximum duration t max produce a trajectory tr(q r, p(t max )). Thi trajectory i checked for colliion o a to calculate t free, the lat point in time that the trajectory i colliion-free. Figure 3 provide an illutration of the election/propagation procedure. The election of edge e i baed on a combination of metric information to greedily bia the earch toward the target and a priority cheme that guarantee that eventually all the edge will be elected for propagation. For a new edge e, tate along e are ampled. For each tate, we compute the ditance to the target et F, o a to get a ditance etimate between the edge and the target: ρ(e, F) = {min(ρ(q, f)), q e and f F }. (1) Edge alo have priority counter. A newly expanded edge e ha priority equal to the iteration counter: p(e) = i, where i i the number of edge propagated before e in the current cycle. The two part of the old edge e 1 and e 2 get a lower priority that decreae exponentially when the ame ample i elected for propagation, e.g.: p(e 1 ) = p(e 2 ) = 2 p(e ) + 1. (2) The overall core of an edge i: (e) = α p(e) ρ(e, F). (3) where parameter α i greater than 1: α > 1 and control the importance of the bia v. the priority counter. The edge are tored in a heap that return the minimum core edge. The coring function mut alo be recomputed during tree retainment. The ditance etimate ρ(e, F) i updated for all retained edge given the newly acquired repreentation: (W, t i 1 ). An advantage of the approach, i that with the ue of imple metric intead of path ubdiviion a in PDST [9], thi update i inexpenive. The priority counter of retained edge, which are children of the initial robot tate, are et to 1 and the priority counter along the remaining tree are recurively et by Eq. 2. Thi choice tend to produce increaingly moother path toward the goal during conecutive planning cycle, when a olution path ha already been found. Notice that intead of random control, propagation could be achieved with control elected from an appropriate et pecifically contructed for the ytem (e.g., maneuver automata [14], Reed and Shepp curve [15] etc.). C. Efficient Safety Checking The fact that tr(q r, p(t free )) i colliion-free doe not guarantee afety, ince tr(q r, p(t free )) may lead to an Inevitable Colliion State (ICS) [4]. Computing whether a tate i ICS i difficult, ince it require to conider the et of all poible future trajectorie. Taking a conervative approach, however, a uperet of ICS can be computed much fater by uing only a mall et of contingency plan Γ and define a tate q to be unafe iff: γ Γ.t.: tr(q, γ(t)) i colliion free. (4) Example of contingency plan for tatic workpace are breaking maneuver that bring the robot to a complete top. The time it take to execute the contingency plan i unrelated to the duration of the planning cycle. Note that trajectorie leading to afe tate are alo afe. Fraichard et al. [12] and Frazolli et al. [5] ued thi property to reduce the overhead of afety checking. We can ue the fact that the planner can elect the duration of the conecutive planning cycle to further reduce the overhead of afety checking. In thi way, only tate that are reached at the end of the next planning cycle have to be checked for afety. Theorem: Aume a drift-le non-holonomic ytem executing a replanning tak with GRIP in a tatic environment. The planner elect duration T for the next planning cycle. It i then ufficient to guarantee colliion avoidance to produce trajectorie during the current cycle that are afe only for time

4 T, given contingency plan that are breaking maneuver. Proof Sketch: Aume the vehicle i afe at time t i 1 and ha a plan p(t) of duration t > T that i afe at leat for T. The quetion i whether reaching ICS can be avoided while replanning. There are two cae: (a) The planner will either ucceed in producing a new afe plan for the next planning period and at time t i the robot will again have a colliion-free plan to follow. (b) The planner fail to compute a plan for the next period. However, the previou plan p(t) wa afe for at leat T, which mean that there i at leat one plan γ(t) Γ that can be executed after p(t), which i colliion-free and bring the robot to a complete top in a colliion-free configuration. So, in every cae there i a colliion-free plan. The contingency plan do not have to be breaking maneuver. Looping maneuver would work a well, given the aumption. If we provide τ-afety guarantee in the cae of moving obtacle, which mean that colliion avoidance i guaranteed only for a time period τ after the failure of the algorithm, then we can ue any plan of duration τ. Conequently, the planner doe not check for afety pat time T. During propagation of tr(q r, p(t free )), we compute whether it interect time T at a tate q. If it doe not or if q i afe given et Γ, then the complete tr(q r, p(t free )) i added a a new edge. Otherwie, the part of the trajectory pat q i pruned, o that all the trajectorie tored in the tree are afe at leat for time T. A poible drawback of the approach i that the robot might tart following a path that i not afe pat time T. Thi i not a afety rik ince the unafe part of the trajectory will be pruned during tree retainment when it will fall within the planning period T. It could be an undeired effect, however, ince it could reult in often election of contingency plan. But we can check for afety only the olution trajectorie and accept them a olution only if they are completely afe, following the lazy evaluation approach, where a path i checked for validity only after it i conidered a a good candidate for a olution [13]. D. GRIP Algorithm 1 provide an overview of the complete GRIP algorithm. The main loop elect greedily the edge that ha the maximum core and expand the tree from a tate along the elected edge. An implementation choice i related to the metric ρ(q 1, q 2 ). A poible olution could be the cot-to-go function in an obtacle free pace, which can be computed with dynamic programming a decribed by Frazzoli et al. [5]. Lavalle and Kuffner [7] ued a weighted Euclidean metric. In our implementation, we ue an occupancy grid to repreent the workpace and define the metric ρ A (q 1, q 2 ) uing the A* holonomic ditance between the coordinate of the two tate q 1 and q 2. Although ρ A ( ) carrie poor ditance information for high-dimenional problem, the planner achieve good performance for the dynamically contrained platform we have experimented with and outperform RRT when the ame metric i ued. Algorithm 1 GRIP ( tree, q, (W, t i 1 ), F ) TREE RETAINMENT (initialization) Compute duration T of conecutive planning cycle Set tree root = q and prune everything before it (afety check for retained tree) for each edge e tree do if e contain tate q reachable from q a time T then extend contingency trajectory tr(q, γ(t)) if tr(q, γ(t)) i not colliion-free then Prune everything below q end for (focu on moothing if path exit) if a path to F exit in tree then Prune every path on the tree that doe not lead to F (update core for exiting edge) Set the priority of edge that are children of q to 1. Recurively update the prioritie of edge a in Eq. 2. Initialize an empty heap for all the edge e tree do compute ρ(e, F) for the new target F given Eq. 1 update (e) according to Eq. 3 add e in heap end for Main loop: SELECTION-PROPAGATION while time i le than t i do (determinitic election) Remove from heap edge e = argmin((e)) e heap (random propagation) Select random tate q r along e Break edge e at tate q r into edge e 1 and e 2 Select random plan p(t max ) for a planning duration t max Create trajectory tr(q r, p(t max )) Compute duration t free that tr(q r, p(t max )) i free (afety check) if tr(q r, p(t free )) contain q reachable from q a time T then extend contingency trajectory tr(q, γ(t)) if tr(q, γ(t)) i not colliion-free then Prune everything below q and adjut t free (priority cheme) if t free i above a minimum threhold then Create edge e from the remaining part of tr(q r, p(t free )) et prioritie: p(e) = i++ and p(e 1 ) = p(e 2 ) a in Eq. 2. compute ρ(, F) and core ( ) for e, e 1, e 2 add e, e 1, e 2 in heap end while return tree

5 IV. APPLICATION TO MAPPING Thi ection decribe how GRIP can olve the tak of mapping a workpace while repecting dynamic. 1) Workpace repreentation and colliion: An occupancy grid i ued that ha 3 value: explored free pace, obtacle and unexplored pace. Given a tate, the robot i poitioned on the map and if the chai interect an obtacle or an unexplored pace cell, it i in colliion. Figure 1 how an example of mapping in our imulator. 2) Selection of target F for the planner: We follow a frontier-baed approach to elect a target for the motion planner at each cycle. A frontier i the boundary between the free explored pace and the unexplored one. Neighboring frontier cell are grouped by applying a flooding operation on the grid and the earch i biaed toward a particular frontier during each planning period. There are many alternative heuritic approache for electing target frontier [16]. In our implementation, we elect frontier that are: (a) cloe to the initial robot tate given ρ A and (b) mall in ize, to avoid returning to mall unexplored region after covering large ditance. Figure 4 how the A* ditance on the grid map from a frontier and a tree expanded toward the frontier. 3) Path election given a GRIP tree: The objective in path election i to maximize viibility of the unexplored pace and minimize the length of the trajectory. Candidate trajectorie are all thoe that initiate from the root to a node of the contructed tree. A weight w(tr) for every candidate trajectory tr i defined a: w(tr) = e (d(tr)+λ l(tr)) where d(tr) decribe how cloe the trajectory i to the elected frontier and l(tr) expree the trajectory length. Parameter λ expree the importance of the path length over the ditance to the frontier. There are two ditinct cae, however, for the ditance parameter d(tr) depending on whether the algorithm ha managed to produce tate that can ene the frontier or not. In the firt cae, for a trajectory tr and a cell c in the target frontier group we define the ditance a: { ρa (q, c) if q tr.t. c i viible from q d(tr, c) = otherwie d max Fig. 4. Biaed tree expanion. The A* ditance ued to bia the earch i hown in the background. The light colored triangle correpond to vehicle configuration. The darker trajectory i the elected path. 0 ẋ ẏ θ V L V R 0 Differential Drive 1 0 co θ R 2 (V 1 L + V R ) C A = inθ R 2 (V L + V R ) R B 2 (V R V L ) C α A L α R ẋ ẏ θ V ṡ Car-like co θ co V inθ co V C A = B in v α A t TABLE I V 3 m θ 20 deg α 0.6 m2 θ 3 deg2 0.5 m V 3 m 4 deg α 0.6 m2 t 1 deg2 STATE UPDATE EQUATIONS AND CONTROL LIMITS. where d max i the ening radiu of the robot. Then d(tr) = c d(tr, c). In thi way, trajectorie that ee a large number of frontier cell and which are cloer to them have a maller ditance parameter. If there i no tate along the tree able to ene the frontier, we define d(tr) a the minimum ditance between the end tate of the trajectory and a frontier cell according to ρ A. Both l(tr) and d(tr) can be computed recurively during the tree contruction. The trajectory of maximum weight that ha duration at leat T i finally returned. If no uch trajectory exit, a colliion-free contingency plan i guaranteed to exit by the theorem. V. EXPERIMENTAL SETUP We have experimented with three ytem: (1) A differential drive robot (DD-robot) with velocity control V L, V R. Thi platform i reducible to a impler holonomic robot, ince we can retain the entire tree at each time tep and the contingency plan i trivial: V L = V R = 0. (2) A DDrobot with acceleration control α L, α R. The contingency plan i elected o that the wheel with the larget velocity magnitude i aigned maximum de-acceleration. The econd wheel acceleration i et o that it velocity reache zero a the ame time the firt wheel top. (3) A car-like robot that move backward and forward with acceleration control α and teering velocity t. The contingency plan et the deacceleration parameter to it maximum value o a to reach a configuration with zero forward and teering velocitie. Table I provide the tate update equation for the lat two ytem and the bound we have ued for the control. Parameter R and L are the radiu of the wheel and the ditance between a wheel and the robot center. The three cene we have ued for our experiment can be een in Figure 8. The total area that the robot mut ene in all of the cene i comparable. The ening radiu of the robot i equal to one tenth the width of the cene. The imulation component of our program i reponible for updating the map and tranmitting it over ocket communication to the planner. GRIP i executed on a different proceor than the imulator and after the computation of a plan the planner communicate a equence of control back to the imulator. The planner wa teted on an Athlon 1900MP with one gigabyte of RAM.

6 (a) (b) (c) (d) (e) (f) (g) (h) Fig. 5. Goal finding in (top row) cene meandro with a 2nd order DD-robot and (bottom row) cene labyrinth with an acceleration-bounded car-like robot: (a-e) a trivial random tree doe not find the target after 100,000 iteration, (b-f) an RRT-EXTEND election trategy find the target after (top) 48,410 iteration and (bottom) 51,245 iteration (c-g) RRT-EXTEND-BIAS, where 20% of the time the target i the attractor, find the target after (top) 42,855 iteration and (bottom) 17,212 iteration (d-h) GRIP reache the target after (top) 13,774 edge and (bottom) 4,363 repectively. VI. RESULTS Thi ection ummarize experiment conducted with GRIP for the three non-holonomic platform. A. GRIP a a tand-alone planner The firt et of experiment correpond to typical motion planning problem with dynamic that do not make ue of replanning, o a to compare againt the Voronoi-bia election trategie of RRT. In thee experiment, the ame programming infratructure and parameter have been ued but different election trategie are teted. Figure 5 diplay the reulting tree for different election trategie. Figure 6 provide average over 10 experiment in thee two cene. All trategie are uing the ame metric ρ A ( ). A trivial random election policy fail to produce any path after 100,000 edge have been added to the tree. In order to implement the Voronoi-bia approach, we randomly ample point in the free part of the workpace and ue ρ A to elect the cloet edge to them. The RRT approach offer good coverage of the tate pace but it i low in reaching the target configuration. We have experimented with a verion of RRT that i biaed to promote exploration toward the target. In thi verion, 20% of the time the tate that i ued to elect the cloet edge i a tate in the target et. The value 20% gave the bet reult over different cene. Although there i an improvement compared to the trictly exploring verion of the RRT algorithm, the approach i till low in reaching the goal configuration. On the other hand, the GRIP algorithm manage to aggreively earch the tate pace toward the goal configuration. Conidering the poor quality of the metric ued, thi i a poitive reult. Thi behavior wa conitent acro all experiment. Figure 7 how the benefit of replanning with a elected duration for the next planning cycle. We have compared our algorithm that tet for afety only tate that are T away from the root node of the tree with an approach that Fig. 6. Comparion between the GRIP election/propagation cheme and Voronoi biaed election. Time DD-velocity DD-acceleration Car-like Average Time in ec Maximum Time in ec TABLE II AVER. COST IN SECONDS TO PRODUCE 250 EDGES. produce a tree where all the leaf node are afe. If the two approache are provided with the ame planning period, then GRIP produce a much bigger tree, which allow the planner to better earch the tate-time pace. The difference in the tree ize i mainly due to the additional colliion checking neceary to provide afety in the econd cae. Fig. 7. Replanning with a known duration reduce the overhead of guaranteeing afety. For the ame planning period GRIP build bigger tree.

7 Fig. 8. Exploration of cene (from left to right) meandro, room with a DD-robot, labyrinth and room again with a car-like robot. Fig. 9. The velocity profile for the car exploring room in Figure 8(d). tree retainment, the initial robot tate mut properly be reconnected with the exiting tree. Moreover, the notion of colliion afety i no longer determinitic but each tate ha an aociated probability of whether it collide with an obtacle. We are actively reearching extenion of the framework to addre planning under uncertainty without acrificing the nice propertie of high-peed afe motion. B. Performance for high-level tak: mapping Figure 8 provide a qualitative evaluation of the exploration path produced by GRIP. The robot i initially poitioned at the bottom left corner of a cene and know only the part of the environment that it can ene. No colliion wa oberved during our experiment. If the ICS avoidance tep i removed from the planner, however, then the vehicle collide within a few econd of execution. The path appear mooth and the robot do not unnecearily reviit part of the pace that are already covered. Figure 9 diplay a velocity profile for an exploration procedure. The robot velocity remain for a large duration of the exploration procedure cloe to it maximum value and doe not fluctuate coniderably. Table II preent computational performance tatitic. We ran 10 experiment for each robot and cene type and meaure the time it take for the planner to compute tree with 250 edge. We preent: (a) the average time, (b) and the maximum time, that the planner require to produce the tree. A expected ytem with bounded acceleration are more difficult to plan for. VII. DISCUSSION AND EXTENSIONS Thi paper decribe a tree-baed planner for replanning under dynamic contraint for tak with partial obervability. The algorithm provide afety guarantee for colliion avoidance even under limited computation time. Thi work provide a general framework for uch application by extending previou work on kinodynamic planning [2], [4], [5], [8], [9], [17] and ue new idea to achieve good performance. An efficient election/propagation trategy manage to bia tate pace exploration with the aid of imple metric. The planner reduce the amount of colliion checking neceary for providing afety guarantee and reue computation from previou cycle. The imulated experiment ugget favorable computational performance againt popular alternative and return mooth, afe path. An important extenion i to take into account enor noie and poitioning error. One of the problem in thi cae i that the robot initial tate q during each cycle doe not necearily lie on the previou tree. To achieve REFERENCES [1] J. Bruce and M. Veloo, Safe multi-robot navigation within dynamic contraint, Proc. of the IEEE, vol. 94(7), pp , [2] D. Ferguon, N. Kalra, and A. Stentz, Replanning with rrt, in IEEE ICRA, May 2006, pp [3] J. v. d. Berg, D. Ferguon, and J. Kuffner, Anytime path planning and replanning in dynamic environment, in IEEE ICRA, May 2006, pp [4] T. Fraichard and H. Aama, Inevitable colliion tate - a tep toward afer robot? Advanced Robotic, vol. 18(10), pp , [5] E. Frazzoli, M. A. Dahleh, and E. Feron, Real-time motion planning for agile autonomou vehicle, Journal of Guidance, Control and Dynamic, vol. 25, no. 1, pp , [6] L. E. Kavraki, P. Svetka, J.-C. Latombe, and M. Overmar, Probabilitic roadmap for path planning in high-dimenional configuration pace, IEEE TRA, vol. 12, no. 4, pp , Aug [7] S. M. LaValle and J. J. Kuffner, Randomized kinodynamic planning, IJRR, vol. 20, no. 5, pp , May [8] D. Hu, R. Kindel, J.-C. Latombe, and S. Rock, Randomized kinodynamic motion planning with moving obtacle, IJRR, vol. 21, no. 3, pp , [9] A. M. Ladd and L. E. Kavraki, Fat tree-baed exploration of tate pace for robot with dynamic, in WAFR, 2005, pp [10] A. Stentz, The focued d* algorithm for real-time replanning, in IJCAI, Augut 1995, pp [11] J. v. d. Berg and M. Overmar, Planning the hortet afe path amidt unpredictably moving obtacle, in WAFR, July [12] S. Petti and T. Fraichard, Partial motion planning framework for reactive planning within dynamic environment, in AAAI Intl. Conf. ICAR, Barcelona, Spain, September [13] R. Bohlin and L. E. Kavraki, Path planning uing lazy prm, in IEEE ICRA, San Franico, CA, April 2000, pp [14] E. Frazzoli, M. A. Dahleh, and E. Feron, Maneuver-baed motion planning for nonlinear ytem with ymmetrie, IEEE TR, vol. 21, no. 6, pp , December [15] J. A. Reed and L. A. Shepp, Optimal path for a car that goe both forward and backward, Pacific J. Math., vol. 145, no. 2, pp , [16] W. Burgard, M. Moor, C. Stachni, and F. Schneider, Coordinated multi-robot exploration, IEEE TR, vol. 21, no. 3, [17] S. LaValle and J. Kuffner, Rapidly exploring random tree: Progre and propect, in WAFR, 2001, pp