Simplicial Dijkstra and A∗ Algorithms: From Graphs to Continuous Spaces

Abstract This paper considers the optimal feedback planning problem of a point robot among polygonal obstacles in . In this problem, the Euclidean distance traveled by the robot is minimized. The approximate optimal feedback plan is computed using a piecewise linear approximation of the cost-to-go function. The approximate cost-to-go function, in turn, satisfies the discrete version of dynamic programming principle defined using a simplicial decomposition of the environment. Adaptations of Dijkstra’s and A∗ algorithms are introduced that solve the nonlinear system of discrete dynamic programming equations. Interpolation methods are carefully designed and analyzed so that they are proven to converge numerically. As a result, the computed feedback plan produces approximately optimal trajectories. The methods are implemented and demonstrated on 2D and 3D examples. As expected, the simplicial A∗ algorithm significantly improves performance over the simplicial Dijkstra’s algorithm.

[1]  Chee-Keng Yap,et al.  Approximate Euclidean shortest path in 3-space , 1994, SCG '94.

[2]  Joseph S. B. Mitchell,et al.  Shortest paths among obstacles in the plane , 1993, SCG '93.

[3]  Alexander Vladimirsky,et al.  Ordered Upwind Methods for Static Hamilton-Jacobi Equations: Theory and Algorithms , 2003, SIAM J. Numer. Anal..

[4]  Edsger W. Dijkstra,et al.  A note on two problems in connexion with graphs , 1959, Numerische Mathematik.

[5]  A. Kelly,et al.  Fast and Feasible Deliberative Motion Planner for Dynamic Environments , 2009 .

[6]  Nils J. Nilsson,et al.  A Formal Basis for the Heuristic Determination of Minimum Cost Paths , 1968, IEEE Trans. Syst. Sci. Cybern..

[7]  Ariel Felner,et al.  Theta*: Any-Angle Path Planning on Grids , 2007, AAAI.

[8]  Joseph S. B. Mitchell,et al.  An Efficient Algorithm for Euclidean Shortest Paths Among Polygonal Obstacles in the Plane , 1997, Discret. Comput. Geom..

[9]  Daniel E. Koditschek,et al.  Exact robot navigation using artificial potential functions , 1992, IEEE Trans. Robotics Autom..

[10]  R. Bellman.  Dynamic programming. , 1957, Science.

[11]  Sven Koenig,et al.  Improved fast replanning for robot navigation in unknown terrain , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[12]  Dimitri P. Bertsekas,et al.  Dynamic programming & optimal control , volume i , 2014 .

[13]  Christophe Geuzaine,et al.  Gmsh: A 3‐D finite element mesh generator with built‐in pre‐ and post‐processing facilities , 2009 .

[14]  J A Sethian,et al.  Computing geodesic paths on manifolds. , 1998, Proceedings of the National Academy of Sciences of the United States of America.

[15]  J. Tsitsiklis,et al.  Efficient algorithms for globally optimal trajectories , 1994, Proceedings of 1994 33rd IEEE Conference on Decision and Control.

[16]  Subhash Suri,et al.  Efficient computation of Euclidean shortest paths in the plane , 1993, Proceedings of 1993 IEEE 34th Annual Foundations of Computer Science.

[17]  Anthony Stentz,et al.  Field D*: An Interpolation-Based Path Planner and Replanner , 2005, ISRR.

[18]  J. Sethian,et al.  Numerical Schemes for the Hamilton-Jacobi and Level Set Equations on Triangulated Domains , 1998 .

[19]  Christos H. Papadimitriou,et al.  An Algorithm for Shortest-Path Motion in Three Dimensions , 1985, Inf. Process. Lett..

[20]  Joseph S. B. Mitchell,et al.  The Discrete Geodesic Problem , 1987, SIAM J. Comput..

[21]  Aleksej F. Filippov,et al.  Differential Equations with Discontinuous Righthand Sides , 1988, Mathematics and Its Applications.

[22]  John F. Canny,et al.  New lower bound techniques for robot motion planning problems , 1987, 28th Annual Symposium on Foundations of Computer Science (sfcs 1987).

[23]  Josef Stoer,et al.  Numerische Mathematik 1 , 1989 .