A Roadmap-Path Reshaping Algorithm for Real-Time Motion Planning

Real-time motion planning is a vital function of robotic systems. Different from existing roadmap algorithms which first determine the free space and then determine the collision-free path, researchers recently proposed several convex relaxation based smoothing algorithms which first select an initial path to link the starting configuration and the goal configuration and then reshape this path to meet other requirements (e.g., collision-free conditions) by using convex relaxation. However, convex relaxation based smoothing algorithms often fail to give a satisfactory path, since the initial paths are selected randomly. Moreover, the curvature constraints were not considered in the existing convex relaxation based smoothing algorithms. In this paper, we show that we can first grid the whole configuration space to pick a candidate path and reshape this shortest path to meet our goal. This new algorithm inherits the merits of the roadmap algorithms and the convex feasible set algorithm. We further discuss how to meet the curvature constraints by using both the Beamlet algorithm to select a better initial path and an iterative optimization algorithm to adjust the curvature of the path. Theoretical analyzing and numerical testing results show that it can almost surely find a feasible path and use much less time than the recently proposed convex feasible set algorithm.

[1]  Yukinori Kobayashi,et al.  Path Smoothing Techniques in Robot Navigation: State-of-the-Art, Current and Future Challenges , 2018, Sensors.

[2]  Zhaodan Kong,et al.  A Survey of Motion Planning Algorithms from the Perspective of Autonomous UAV Guidance , 2010, J. Intell. Robotic Syst..

[3]  B. Faverjon,et al.  Probabilistic Roadmaps for Path Planning in High-Dimensional Con(cid:12)guration Spaces , 1996 .

[4]  Fredrik Kahl,et al.  Shortest Paths with Higher-Order Regularization , 2015, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[5]  Jean-Claude Latombe,et al.  Robot motion planning , 1970, The Kluwer international series in engineering and computer science.

[6]  Mark H. Overmars,et al.  Creating High-quality Paths for Motion Planning , 2007, Int. J. Robotics Res..

[7]  Subir Kumar Ghosh,et al.  Visibility Algorithms in the Plane , 2007 .

[8]  Thierry Fraichard,et al.  From Reeds and Shepp's to continuous-curvature paths , 1999, IEEE Transactions on Robotics.

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

[10]  E. H. Lockwood,et al.  A Book of Curves , 1963, The Mathematical Gazette.

[11]  Masayoshi Tomizuka,et al.  Designing Robot Behavior in Human-Robot Interactions , 2019 .

[12]  Marco Pavone,et al.  A convex optimization approach to smooth trajectories for motion planning with car-like robots , 2015, 2015 54th IEEE Conference on Decision and Control (CDC).

[13]  L. Piegl,et al.  The NURBS Book , 1995, Monographs in Visual Communications.

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

[15]  Franz Aurenhammer,et al.  Voronoi diagrams—a survey of a fundamental geometric data structure , 1991, CSUR.

[16]  Masayoshi Tomizuka,et al.  Convex feasible set algorithm for constrained trajectory smoothing , 2017, 2017 American Control Conference (ACC).

[17]  Panagiotis Tsiotras,et al.  Solving shortest path problems with curvature constraints using beamlets , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[18]  Ana Paula Teixeira,et al.  On the Complexity of a Mehrotra-Type Predictor-Corrector Algorithm , 2012, ICCSA.

[19]  Michael Ian Shamos,et al.  Computational geometry: an introduction , 1985 .

[20]  AurenhammerFranz.  Voronoi diagramsa survey of a fundamental geometric data structure , 1991 .

[21]  Lydia E. Kavraki,et al.  Probabilistic roadmaps for path planning in high-dimensional configuration spaces , 1996, IEEE Trans. Robotics Autom..

[22]  Narendra Karmarkar,et al.  A new polynomial-time algorithm for linear programming , 1984, STOC '84.

[23]  Masayoshi Tomizuka,et al.  Real time trajectory optimization for nonlinear robotic systems: Relaxation and convexification , 2017, Syst. Control. Lett..

[24]  L. Dubins.  On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents , 1957 .

[25]  Xiaoming Huo,et al.  Beamlets and Multiscale Image Analysis , 2002 .

[26]  John Shier.  Filling Space with Random Fractal Non-Overlapping Simple Shapes , 2011 .

[27]  Masayoshi Tomizuka,et al.  The Convex Feasible Set Algorithm for Real Time Optimization in Motion Planning , 2017, SIAM J. Control. Optim..

[28]  Siddhartha S. Srinivasa,et al.  CHOMP: Gradient optimization techniques for efficient motion planning , 2009, 2009 IEEE International Conference on Robotics and Automation.

[29]  P. Abbeel,et al.  LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information , 2011 .