Searchbased Path Planning with Homotopy Class Constraints University
Search-based Path Planning with Homotopy Class Constraints University of Subhrajit Bhattacharya | Vijay Kumar | Maxim Likhachev GRASP Pennsylvania LABORATORY Definition of Homotopy Class Motivational Examples Homotopy class in literature final ? ? Trajectories in same homotopy classses ? ? initial Trajectories in different homotopy classses Set of trajectories joining same start and goal points that can be smoothly deformed into one another without intersecting obstacles Deploying multiple agents: • Searching/exploring the map • Pursuing an agent with uncertain paths • Geometric approach [Hershberger et al. ; Grigoriev et al. ] - Not well-suited for graph representation - Inefficient for planning with homotopy class constraints • Triangulation based method [Demyen et al. ] - Not suitable for non-Euclidean cost functions - Requires triangulation-based discretization schemes. - Complexity increases significantly if environment contains many small obstacles. - Cannot be easily used with an arbitrary graph search and arbitrarily discretization. goal start Predicting: • Possible paths of an agent with uncertainty in behaviors • Possible paths taken by an agent when only start and final positions are known Constraints: • Avoid high-risk regions and homotopy classes • Follow a known homotopy class in order to perform certain tasks Our Approach – Exploit Theorems from Complex Analysis Goal: To plan least-cost paths for arbitrary cost functions (not necessarily Euclidean distances) within a particular homotopy class or while avoiding certain homotopy classes. Homotopy class constraint To develop efficient representation of homotopy classes that supports planning using arbitrary discretizations and graph representations, (uniform discretization, unstructured discretization , triangulation, visibility graph, etc. ) and using any standard graph search algorithm. (Dijkstra’s, A*, D*, ARA*, etc. ) Basic Principle: f 0 , for example, can be any arbitrary polynomial in z ζ 1 Complex Analytic Function ≡ Complex Differentiable F(z) ≡ F (x + iy) ≡ u(x, y) + i v(x, y) Equivalently, F ( xy) = ( u(x, y) v(x, y)) with u, v following certain properties ( 2 u = 2 v = 0) which are guaranteed when x & y are implicitly used within z in construction of F. Im ζ 2 ζ 3 Define an Obstacle Marker function such that it is Complex Analytic everywhere, Re except for having poles (singularities) at the representative points Represent the X-Y plane by a complex plane i. e. A point (x, y) is represented as z = x + iy A direct consequence of Cauchy Integral Theorem and Residue Theorem τ1 = τ2 ζ 1 Place “representative points”, ζi, inside significant obstacles τ2 τ1 ζ 2 The value of ζ 3 Can be computed numerically by further discretizing e z 1 uniquely defines the homotopy class of a trajectory τ Computing L(e) for any straight line segment e (e. g. edge of a graph laid down on the environment) e τ3 τ3 (we can ignore small obstacles which we don’t want to contribute towards homotopy classes) z 2 ≠ L-augmented graph Given the graph laid upon the environment, G we construct L by augmenting each state z with L-value of trajectory leading to it from start coordinate z in G If e is “small”, and f 0 an order-N-1 polynomial, this can be computed analytically in a fast and efficient way. Augment each node, z, with distinct L-values of trajectories leading to it from start. {z, L(zs→z)} in G L kl = argminkl Integrating L-values along paths by adding up L-values of the edges Experimental Analysis “Visibility” constraint translates to homotopy class constraint Non-Euclidean cost function Homotopy class exploration A – set of allowed homotopy classes B – set of blocked homotopy classes Cost function remains same Illustration of the effect of augmenting L -values with state coordinates: z 2 e 1 start ζ 1 zs zg e 2 Implementation on a Visibility Graph (polygonal obstacles) Planning in X-Y-Time Exploring 20 homotopy classes in a 1000 x 1000 uniformly discritized environment A homotopy class blocked: G unique goal state e 4 z 1 {z 2 , L(e 1)} e 1 start {zs , 0+0 i} GL No homotopy class constraint: Insight into graph topology e 3 ζ 1 e 2 e 4 (z 1 , L(e 2)) {zg , L(e 1)+L(e 3)} ≠ {zg , L(e 2)+L(e 4)} Goal states are distinguished based on the path taken to reach it Conclusions: Developed a compact and efficient representation of homotopy classes, using which homotopy class constraints can be imposed on existing graph search-based planning methods. Acknowledgements We gratefully acknowledge support from ONR grant no. N 00014 -09 -1 -1052, NSF grant no. IIS-0427313, ARO grant no. W 911 NF-05 -1 -0219, ONR grants no. N 00014 -07 -1 -0829 and N 00014 -08 -1 -0696, and ARL grant no. W 911 NF-08 -2 -0004.
Addendum For the simple cases in 2 -dimensions we have not distinguished between homotopy and homology. The distinction however does exist even in 2 -d. See our more recent [AURO 2012] paper or [RSS 2011] paper for a comprehensive discussion on the distinction between homotopy and homology, examples illustrating the distinction, and its implications in robot planning problems. [AURO 2012] Subhrajit Bhattacharya, Maxim Likhachev and Vijay Kumar (2012) "Topological Constraints in Searchbased Robot Path Planning". Autonomous Robots, 33(3): 273 -290, October, Springer Netherlands. DOI: 10. 1007/s 10514 -012 -9304 -1. [RSS 2011] Subhrajit Bhattacharya, Maxim Likhachev and Vijay Kumar (2011) "Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3 D". [Original title: "Identifying Homotopy Classes of Trajectories for Robot Exploration and Path Planning"]. In Proceedings of Robotics: Science and Systems. 27 -30 June.
- Slides: 2