NUROA: A numerical roadmap algorithm

Motion planning has been studied for nearly four decades now. Complete, combinatorial motion planning approaches are theoretically well-rooted with completeness guarantees but they are hard to implement. Sampling-based and heuristic methods are easy to implement and quite simple to customize but they lack completeness guarantees. Can the best of both worlds be ever achieved, particularly for mission critical applications such as robotic surgery, space explorations, and handling hazardous material? In this paper, we answer affirmatively to that question. We present a new methodology, NUROA, to numerically approximate the Canny's roadmap, which is a network of one-dimensional algebraic curves. Our algorithm encloses the roadmap with a chain of tiny boxes each of which contains a piece of the roadmap and whose connectivity captures the roadmap connectivity. It starts by enclosing the entire space with a box. In each iteration, remaining boxes are shrunk on all sides and then split into smaller sized boxes. Those boxes that are empty are detected in the shrink phase and removed. The algorithm terminates when all remaining boxes are smaller than a resolution that can be either given as input or automatically computed using root separation lower bounds. Shrink operation is cast as a polynomial optimization with semialgebraic constraints, which is in turn transformed into a (series of) semidefinite programs (SDP) using the Lasserre's approach. NUROA's success is due to fast SDP solvers. NUROA correctly captured the connectivity of multiple curves/skeletons whereas competitors such as IBEX and Realpaver failed in some cases. Since boxes are independent from one another, NUROA can be parallelized particularly on GPUs. NUROA is available as an open source package at http://nuroa.sourceforge.net/.

[1]  Yong K. Hwang,et al.  SANDROS: a motion planner with performance proportional to task difficulty , 1992, Proceedings 1992 IEEE International Conference on Robotics and Automation.

[2]  Micha Sharir,et al.  Planning, geometry, and complexity of robot motion , 1986 .

[3]  Koichi Kondo,et al.  Motion planning with six degrees of freedom by multistrategic bidirectional heuristic free-space enumeration , 1991, IEEE Trans. Robotics Autom..

[4]  George E. Collins,et al.  Quantifier Elimination for Real Closed Fields by Cylindrical Algebraic Decomposition: a synopsis , 1976, SIGS.

[5]  Jean B. Lasserre,et al.  Global Optimization with Polynomials and the Problem of Moments , 2000, SIAM J. Optim..

[6]  Éric Schost,et al.  A Baby Step–Giant Step Roadmap Algorithm for General Algebraic Sets , 2012, Found. Comput. Math..

[7]  Bernard Mourrain,et al.  A symbolic-numeric silhouette algorithm , 2000, Proceedings. 2000 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2000) (Cat. No.00CH37113).

[8]  Federico Thomas,et al.  A Linear Relaxation Technique for the Position Analysis of Multiloop Linkages , 2009, IEEE Transactions on Robotics.

[9]  Tomás Lozano-Pérez,et al.  An algorithm for planning collision-free paths among polyhedral obstacles , 1979, CACM.

[10]  Xiong Zhang,et al.  Solving Large-Scale Sparse Semidefinite Programs for Combinatorial Optimization , 1999, SIAM J. Optim..

[11]  Frédéric Benhamou,et al.  Algorithm 852: RealPaver: an interval solver using constraint satisfaction techniques , 2006, TOMS.

[12]  Makoto Yamashita,et al.  Algorithm 925: Parallel Solver for Semidefinite Programming Problem having Sparse Schur Complement Matrix , 2012, TOMS.

[13]  J. T. Shwartz,et al.  On the Piano Movers' Problem : III , 1983 .

[14]  D. S. Arnon,et al.  Algorithms in real algebraic geometry , 1988 .

[15]  Oussama Khatib,et al.  Real-Time Obstacle Avoidance for Manipulators and Mobile Robots , 1985, Autonomous Robot Vehicles.

[16]  George E. Collins,et al.  Polynomial Minimum Root Separation , 2001, J. Symb. Comput..

[17]  Lydia E. Kavraki,et al.  Random networks in configuration space for fast path planning , 1994 .

[18]  J. Schwartz,et al.  On the “piano movers'” problem I. The case of a two‐dimensional rigid polygonal body moving amidst polygonal barriers , 1983 .

[19]  S. Basu,et al.  COMPUTING ROADMAPS OF SEMI-ALGEBRAIC SETS ON A VARIETY , 1999 .

[20]  Dinesh Manocha,et al.  FCL: A general purpose library for collision and proximity queries , 2012, 2012 IEEE International Conference on Robotics and Automation.

[21]  Masakazu Kojima,et al.  Exploiting sparsity in linear and nonlinear matrix inequalities via positive semidefinite matrix completion , 2011, Math. Program..

[22]  J. Schwartz,et al.  On the Piano Movers' Problem: III. Coordinating the Motion of Several Independent Bodies: The Special Case of Circular Bodies Moving Amidst Polygonal Barriers , 1983 .

[23]  Mohammad T. Manzuri Shalmani,et al.  AMF: A novel reactive approach for motion planning of mobile robots in unknown dynamic environments , 2009, 2009 IEEE International Conference on Robotics and Biomimetics (ROBIO).

[24]  Dimos V. Dimarogonas,et al.  Decentralized feedback stabilization of multiple nonholonomic agents , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[25]  John H. Reif,et al.  Complexity of the mover's problem and generalizations , 1979, 20th Annual Symposium on Foundations of Computer Science (sfcs 1979).

[26]  Gilles Trombettoni,et al.  Upper bounding in inner regions for global optimization under inequality constraints , 2014, J. Glob. Optim..

[27]  Kenneth Y. Goldberg,et al.  Motion Planning Under Uncertainty for Image-guided Medical Needle Steering , 2008, Int. J. Robotics Res..

[28]  J. Schwartz,et al.  On the “piano movers” problem. II. General techniques for computing topological properties of real algebraic manifolds , 1983 .

[29]  Howie Choset,et al.  Principles of Robot Motion: Theory, Algorithms, and Implementation ERRATA!!!! 1 , 2007 .

[30]  Masakazu Kojima,et al.  A parallel primal-dual interior-point method for semidefinite programs using positive definite matrix completion , 2006, Parallel Comput..

[31]  Juan Jesús Pérez,et al.  Complete maps of molecular‐loop conformational spaces , 2008, J. Comput. Chem..

[32]  John Canny,et al.  The complexity of robot motion planning , 1988 .

[33]  M.T. Manzuri-Shalmani,et al.  A New Fuzzy-Based Spatial Model for Robot Navigation among Dynamic Obstacles , 2007, 2007 IEEE International Conference on Control and Automation.

[34]  Markus Schweighofer,et al.  Optimization of Polynomials on Compact Semialgebraic Sets , 2005, SIAM J. Optim..

[35]  Stephen P. Boyd,et al.  Semidefinite Programming , 1996, SIAM Rev..

[36]  Brian Borchers,et al.  Implementation of a primal–dual method for SDP on a shared memory parallel architecture , 2007, Comput. Optim. Appl..

[37]  Steven M. LaValle,et al.  Rapidly-Exploring Random Trees: Progress and Prospects , 2000 .

[38]  J. Davenport A "Piano Movers" Problem. , 1986 .

[39]  M. Mignotte Some Useful Bounds , 1983 .