Extracting Optimal Paths from Roadmaps for Motion Planning

Transcription

1 Extracting Otimal Paths from Roadmas for Motion Planning Jinsuck Kim Roger A. Pearce Nancy M. Amato Deartment of Comuter Science Texas A&M University College Station, TX 843 Abstract We resent methods for extracting otimal aths from motion lanning roadmas. Our system enables any combination of otimization criteria, such as collision detection, kinematic/dynamic constraints, or minimum clearance, and relaxed definitions of the state, to be used when selecting aths from roadmas. Our algorithm is an augmented version of Dijkstra s shortest ath algorithm which allows edge weights to be defined relative to the current ath. We resent simulation results maximizing minimum ath clearance, minimizing localization effort, and enforcing kinematic/dynamic constraints. I. INTRODUCTION Automatic motion lanning has been used in many areas such as robotics and comuter aided design (CAD) to find aths in the resence of obstacles. Though originating in robotics, motion lanning techniques have been adated to areas such as autonomous transortation systems for automobiles or aircraft, military unmanned vehicles that oerate in the air or underwater, and comuter animations in the entertainment industry. In these alications, aths must be found quickly in large search saces. Roadma based lanners are ideal for such scenarios [4]. A roadma containing reresentative aths is comuted during a rerocessing ste, and aths can be quickly extracted from the roadma during query rocessing. In articular, the roadma is a grah reresenting the connectivity of the free configuration sace, where nodes are robot configurations and edges are aths comuted by a simle and deterministic local lanner. The strength of roadma based lanners is that the roadma is a comact aroximation of the connectivity of the lanning sace. While roadma based lanners are extremely effective in roviding feasible solution aths for arbitrary queries, generally no guarantees can be rovided regarding the quality of the aths. In articular, aths extracted from roadmas seldom rovide otimal solutions because they are restricted to the nodes and edges in the roadmas. In many cases, this is not a concern because the roblem of interest is simly finding a feasible ath. For this reason, otimizing aths has received little attention for roadma based lanners. In this aer, we consider the roblem of extracting an otimal ath from among all aths contained in the roadma. There are two main issues that are of concern. First, roadmas contain many ossible routes connecting two different nodes. Deending on the grah search algorithm and the criteria alied, different aths connecting the same and nodes can be found. Second, a ath extracted from a roadma is comosed of many short line segments and its quality is likely lower than a smoothed ath obtained by exhaustive numerical otimization. These two roerties are inherent in roadma based methods. We call the first a macroscoic roerty because the chosen search method can result in large scale changes in the ath. We refer to the second as a microscoic roerty because tyically there are no toological differences between the extracted ath and the otimal ath. A number of techniques have been roosed to imrove the solution aths extracted from roadmas (the microscoic roerty). Common aroaches are to ostrocess the ath by converting the ath to a curve [12], moving existing nodes, or adding additional nodes to the subotimal ath [10]. In this aer, we focus on the macroscoic roerty and rovide a method to quickly comute an otimal ath from among all aths contained in the roadma. Our method is based on an augmented version of Dijkstra s shortest ath algorithm which enables one to consider more general otimization criteria and relaxed definitions of the state. II. RELATED WORK Previous research shows that alying common otimization techniques to robotics motion lanning is not straightforward because the collision free requirement renders it difficult to solve analytically or numerically [2], [11]. Figure 1(a) shows a ath extracted from a roadma ( ) and aths numerically generated by general otimization techniques ( ). Figure 1(b) shows two regions searated by an obstacle. To solve two oint boundary value otimization roblems, an initial guess of the solution must be given []. If the initial guess is, then the solution cannot be imroved beyond without understanding the discontinuity of the search sace. However, the subotimal ath can be transformed to the otimal. This exlains the imortance of the search of an otimal ath in the macroscoic way.

2 ' (a) Fig roadma 3 r 1 r 2 (b) Otimizing ath with initial guesses Recently, two different aroaches have been develoed to obtain otimal aths in robotics alications. Imroving robot aths. Many recent methods for motion lanning are exlicitly/imlicitly based on roadmas. Several methods consider the roblem of otimizing or imroving an existing ath, for examle, grids [9], visibility grahs [1], [12], and growing control oints in barycentric coordinates [8]. All of the aroaches above use deterministic roadmas. Probabilistic roadmas encoding hysical constraints have been studied in [10] where the roadma is customized for various alications, and aths are imroved in the query ste. Finding otimal aths. Otimal aths can be obtained by modifying general otimization or otimal control techniques for motion lanning. Because the methods are not based on roadmas, collision checking needs to be geometrically and/or mathematically formulated, and is relatively comlex and inefficient. In [11], the constraints of the otimization roblem are extended to AND and OR logic, which are referred to as generalized constraints and deal with olygonal obstacles. Modification of genetic algorithms was attemted in [3] to imrove the ath using using Gram Schmidt orthogonalization. Dijkstra s algorithm. Our otimization method is based on Dijkstra s shortest ath algorithm. Dijkstra s algorithm searches for a shortest ath in a weighted directed grah where all edge weights are non negative. Dijkstra s algorithm is widely used in many areas where the ath cost needs to be minimized, for examle in wireless network alications [13] where the edge cost is an estimation of the required transmission ower and the roagation delay. III. ISSUES IN PATH OPTIMIZATION In this section, we discuss useful roerties and requirements for comuting otimal aths in robotics that have not been addressed in revious work. Standard cost function. The otimization of certain values for a hysical system that moves from an initial state at time 0 to a final state at time, while subject to constraints, is described by the roblem of minimizing a cost function. The standard cost function in otimal control theory [] is described by!"#$ (1) where " is the state at time and is the control inut at time. The necessary condition at the final time # is described by! %. This is Markov in the sense that & (') is determined by the value of which is evaluated for time only. Non Markov otimization criteria. Comared to our work, revious work with roadma based methods lacks two imortant roerties needed for real alications. The first is the need for non Markovian states, i.e., states which deend on information from a range of revious states. For examle, to maximize clearance, it is clear that a cost function will contain the recirocal of clearance if the otimizer minimizes. We denote the recirocal of the clearance as.-0/ If we let ".-5/-3244 in Equation 1, then the resulting ath will maximize the accumulated clearance from the to. In most cases, the objective is to otimize the ath for maximum safety and the roer criterion is maximizing the minimum ath clearance, not maximizing the accumulated clearance. This requires a modified cost function 6!"#$ 98 ; where (<>=? #)@ such that 98 is minimum. Goal sets flexible final states. The second issue that has not been addressed in revious work is a flexible definition of the final necessary condition. If the final condition at is equivalently described using an equality condition!"?, then the!"#$ term is removed from Equation 1. Unfortunately, in a grah search based ath lanner such as Dijkstra s algorithm, it is difficult to find a node that satisfies!"#$? unless some of the nodes are generated exactly on the surface with the condition satisfied. So, we exand the surface using an inequality condition. A!" DC " 1.-0/1-32B44 The inequality condition is used to terminate the grah search if any node satisfying! C is reached. We call this set of nodes a set. (2) (3)

3 Given environment, and, find a ath minimize 9 where 9 % subject to )9 = ) 98?, ' such that others (e.g., time, energy, ) and by the final condition # < 8 2 where 8 2 # # Fig. 2. "! Path otimization roblem IV. SYSTEM DESCRIPTION Our ath otimization system is based on the roadma method and Dijkstra s shortest ath algorithm. To address the issues mentioned in the revious section, we designed an augmented version of Dijkstra s algorithm and cost comutation. A. Problem Formulation Before exlaining the details of our framework, we reformat the mathematical descrition (in Equation 3) to a seudo code friendly version. Figure 2 describes our ath otimization roblem of minimizing the cost of a given ath. Oerators )9 and # C denote the and end vertex of edge, resectively, and the cost functions and! denote the functions and! in Equation 3, resectively. # is a node in the roadma, and the final condition secified by a constant is internally transformed to a set, 8 2, that will terminate the search when reached. In Section IV-D, seudo code is used to describe this in detail. B. Markov like Otimization Ideal Markov Function. The issue of maximizing minimum clearance was introduced in Section III, and the cost function including a non Markovian state is 6 %$" (4) where $> is a general non Markovian cost function and < =? In Equation 2, $" was.-0/1-32b 44. Discretization. Since we are using a grah search algorithm which is similar to dynamic rogramming in classic otimization theory, Equation 4 can be reresented by a discretized version & %$ (< B? ' /. (5) where. is the total number of time stes, is the time ste corresonding to, and is a discrete time state udate equation of the system dynamics. ('*) - Markov like Cost - Function. Now, we relace with so that both revious and current states are used for comuting the cost. The revious state is obtained using the arent data structure in the search tree of Dijkstra s algorithm (see Figure 3). We call this aroach Markov like because using is not Markov in a strict sense but and can be denoted by a comound state 0. The general cost function is ('*) 210 3$ B New State Udate Equation. We added to the cost function with the intention of eliminating $ B, - and the state equation needs to be changed accordingly. The idea is that 0 should contain the entire history of the non Markovian roerty. For examle, to maximize the minimum ath clearance, an element in 0 will indicate the minimum clearance from to time ste. Now, we denote the minimum clearance state by and add it to 0. ('*) ; () =< The state equation returns that is lower than only if 98, the clearance of, is smaller than. - Otherwise, must equal because the clearance of the current state is not smaller than the minimum clearance discovered so far (see Figure ). -?> 98 if 8 A@ otherwise - New Cost Function. Next, we focus on which is a art of 1 (0 and corresonds to the state. It comares the difference between and, and should return a nonzero ositive value if. Otherwise, it returns zero so that does not increase. So, we have - > - B DC if? otherwise (9) where is a constant. This technique for minimum clearance can be alied to other non Markovian otimization values with the suerscrit changed in Equations, 8 and 9. 8 (6) (8)

4 ) ) C C. Flexible Final Condition (0 D. Augmented Dijkstra s Algorithm We aly the modified final condition shown in Equation 3 to our new cost function in Equation, which is the final form('*) of the cost function that we seek (10) DC Dijkstra s algorithm is augmented to reflect the changes, and its seudo code is shown in Figure 3. Since was introduced in Equation 6, is added in line and 8. The cost function! in line 10 checks if a node is in the set using. AUGMENTED DIJKSTRA( ) 1. for (each "!$# 2. %&'! 0 3. (*)+! PriorityQueue of ordered by 4. while ((*)-,.+/ ) 5. 01!2(*).dequeue 6. for each 31(*) adjacent to 0. if ( 54+6 &89<;=>?@60A %B CB&D;=E" 0 ])) 8. "!F &89<;=>?@60A %B CB&D;=E" 0 ]) 9. CB&D;=E" G!F0 10. if (HI JB LKM= ) return 11. (*).reorder Fig. 3. The augmented Dijkstra s algorithm V. MOBILE ROBOT APPLICATIONS In this section we rovide some robotic examles that benefit from the ath otimization methods described. They utilize our roadma based mobile robot system described in [5], [6]. It uses feature based localization and sonar range sensors. A T shaed environment and roadma are shown in Figure 4 where five nodes in the set are marked. node Fig. 4. e i 1 e i obstacle roadma edges set searched edges of of Dijkstra s algorithm Environment, roadma and ath searching. Avoiding Localization Failure. In this case, we assume that the robot s sensors have range limits and always fail to localize if no feature exists within the range. The locations of all features in the environment are assumed to be known. In Figure 6, we use BDN visibility of (11) 9 where visibility of is the exected number of features to be scanned by the robot when it is on edge. The function N converts the visibility of edge into a scalar as shown in Figure 5(a). Note that the otimal ath can traverse a region with no features if necessary Fig features (a) infinity 10 0 B A 0 10 infinity turning radius(m) (b) Cost functions, (a) for features and (b) for turning radius. Kinematic Constraints. If the robot has constraints on its turning radius, two adjacent edges and are needed to comute the required turning radius to obtain the cost of. The weight function now uses two edges (or three vertices) as shown in the seudo code in Figure 3. In Figure 6, which reflects the modified weight comutation, we use 9 where N, cf Otimal Path BDN turn radius of and (12) is an aroriate linear or nonlinear function. Fig. 6. Augmented Dijkstra s Algorithm edges e i, ei 1 cost( ei ) Weight function with two adjacent edges. Weight Function Figure 5(b) shows an examle of a nonlinear function that maximizes turning radius (region A) and rohibits from being used if it violates the kinematic constraint of a turning radius of less than 10 meters (region B). Maximizing Minimum Clearance. As discussed in Section IV-B, the minimum clearance is a non increasing variable and is shown as a solid line in Figure. To imlement this in the augmented Dijkstra s algorithm framework, we add the new variable as auxiliary data as in Figure 8. The data is maintained according to the rule shown in Equation 8. The edge cost comutation equivalent to Equation 9 is described by PO QAB if 9 98? otherwise (13)

5 O C distance cost Fig.. edge clearance minimum ath clearance cost( ei) Cost function for edge clearance where 98 is the auxiliary data and 98 is the clearance of edge. Initially, 98 is set to the clearance of the node., cf Otimal Path Fig. 8. Augmented Dijkstra s Algorithm edges e i, ei 1 cost( ei ) Auxiliary data edge ei 1 data Weight Function Weighting with two adjacent edges and related data. Combination of Criteria. Combining various costs into one function results in the simultaneous otimization for multile values, and is useful in many alications. The combined cost of an edge is the weighted sum of individual costs. 9 B VI. SIMULATION RESULTS (14) To generate roadmas for simulations, we used a class of roadma based lanning methods which are called robabilistic roadma methods (PRMs) that have roven to be very successful in efficiently solving high dimensional roblems in comlex environments [4]. Three different ossible routes exist in the environment using the roadma shown in Figure 9(a) from the to area in Figure 9(c). Paths going through corridor A or C in Figure 9(c) are obtained by maximizing the minimum clearance or minimizing ath length, resectively. Figure 9(c) shows the ath going through corridor B; this results from the combination of minimizing ath length and maximizing minimum ath clearance. 9?? 8!? if 98? otherwise (15) Search tree edges of Dijkstra algorithm are illustrated in Figure 9(b) by arrows reresenting the direction of the search from the node. Fig. 9. (a) (b) environment roadma search tree otimal ath A (c) C B Maximizing clearance and combination of criteria. Several simulations in the same environment are resented in Table I using another arameter, turning radius. Then, the 9 is comuted using three constant weights 2 and 2. 2 is the cost for turning radius and enalizes the edge with a shar turn. The second row shows that the smoothest ath is obtained by going though region B, which is shown in Figure 9(c). The second and third rows, and Equation 15 show that different combinations of weight constants can result in similar aths. H Route HI HI A B B C TABLE I SIMULATIONS WITH DIFFERENT PARAMETERS VII. CONCLUSIONS A framework for extracting an otimal ath in a motion lanning roadma has been roosed. Our framework combines the mathematical flexibility of general otimization techniques and comutational efficiency of roadma based methods. We designed an augmented Dijkstra s shortest ath algorithm that uses Markov like states and sets. Using PRMs, the ath can be efficiently otimized in a large sace for several values including kinematic/dynamic constraints and minimum clearance. Simulation results were resented to illustrate the feasibility of our aroach.

6 Future work consists of exerimenting with robots with high degrees of freedom, efficient algorithms for ath smoothing, and hardware exeriments using mobile robots. VIII. REFERENCES [1] C. Ahrikencheikh and A. Seireg. Otimized-Motion Planning. John Wiley & Sons, Inc., [2] S. Akella and S. Hutchinson. Coordinating the motions of multile robots with secified trajectories. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), ages , [3] C. Hocaoglu and A. Sanderson. Planning multile aths with evolutionary seciation. In IEEE transactions on evolutionary comutation, volume 5, ages , [4] L. Kavraki, M. Kolountzakis, and J.-C. Latombe. Analysis of robabilistic roadmas for ath lanning. In IEEE Trans. Robot. Automat., volume 14, ages , [5] J. Kim, N.M. Amato, and S. Lee. An integrated mobile robot ath (re)lanner and localizer for ersonal robots. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), ages , [6] J. Kim, R.A. Pearce, and N.M. Amato. Robust geometric based localization in indoor environments using sonar range sensors. In Proc. IEEE Int. Conf. Intel. Rob. Syst. (IROS), To aear. [] Donald E. Kirk. Otimal Control Theory An Introduction. Prentice Hall, 190. [8] P. Konkimalla and S.M. LaValle. Efficient comutation of otimal navigation functions for nonholonomic lanning. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), ages , [9] Z. Shiller, K. Yamane, and Y. Nakamura. Planning motion atterns of human figures using a multi-layered grid and the dynamics filter. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), ages 1 8, [10] G. Song, S. L. Miller, and N. M. Amato. Customizing PRM roadmas at query time. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), ages , [11] Y. Wang and D. Lane. Solving a generalized constrainted otimization roblem with both logic AND and OR relationshis by mathematical transformation and its alication to robot motion lanning. In IEEE transactions on systems, man and and cybernetics, art C, ages , [12] M. Yamamoto, M. Iwamura, and A. Mohri. Quasi-timeotimal motion lanning of mobile latforms in the resence of obstacles. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), ages , [13] M. A. Youssef, M. F. Younis, and K. A. Arisha. A constrained shortest-ath energy-aware routing algorithm for wireless sensor networks. In IEEE Wireless Communications and Networking Conference (WCNC), ages 94 99, 2002.