ITOMP: Incremental Trajectory Optimization for Real-Time Replanning in Dynamic Environments

We present a novel optimization-based algorithm for motion planning in dynamic environments. Our approach uses a stochastic trajectory optimization framework to avoid collisions and satisfy smoothness and dynamics constraints. Our algorithm does not require a priori knowledge about global motion or trajectories of dynamic obstacles. Rather, we compute a conservative local bound on the position or trajectory of each obstacle over a short time and use the bound to compute a collision-free trajectory for the robot in an incremental manner. Moreover, we interleave planning and execution of the robot in an adaptive manner to balance between the planning horizon and responsiveness to obstacle. We highlight the performance of our planner in a simulated dynamic environment with the 7-DOF PR2 robot arm and dynamic obstacles.

[1]  Oussama Khatib,et al.  Elastic bands: connecting path planning and control , 1993, [1993] Proceedings IEEE International Conference on Robotics and Automation.

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

[3]  Paolo Fiorini,et al.  Motion Planning in Dynamic Environments Using Velocity Obstacles , 1998, Int. J. Robotics Res..

[4]  Yong K. Hwang,et al.  SANDROS: a dynamic graph search algorithm for motion planning , 1998, IEEE Trans. Robotics Autom..

[5]  Steven M. LaValle,et al.  RRT-connect: An efficient approach to single-query path planning , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[6]  Jean-Claude Latombe,et al.  Randomized Kinodynamic Motion Planning with Moving Obstacles , 2002, Int. J. Robotics Res..

[7]  Ming C. Lin,et al.  Constraint-Based Motion Planning Using Voronoi Diagrams , 2002, WAFR.

[8]  Sven Koenig,et al.  Performance bounds for planning in unknown terrain , 2003, Artif. Intell..

[9]  O. Brock,et al.  Elastic Strips: A Framework for Motion Generation in Human Environments , 2002, Int. J. Robotics Res..

[10]  Dinesh Manocha,et al.  DiFi: Fast 3D Distance Field Computation Using Graphics Hardware , 2004, Comput. Graph. Forum.

[11]  Thierry Fraichard,et al.  Safe motion planning in dynamic environments , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[12]  Mark H. Overmars,et al.  Roadmap-based motion planning in dynamic environments , 2005, IEEE Trans. Robotics.

[13]  Sebastian Thrun,et al.  Anytime Dynamic A*: An Anytime, Replanning Algorithm , 2005, ICAPS.

[14]  Steven M. LaValle,et al.  Planning algorithms , 2006 .

[15]  Kostas E. Bekris,et al.  Greedy but Safe Replanning under Kinodynamic Constraints , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

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

[17]  Dinesh Manocha,et al.  Generalized velocity obstacles , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[18]  Oliver Brock,et al.  Planning Long Dynamically-Feasible Maneuvers for Autonomous Vehicles , 2009 .

[19]  Dinesh Manocha,et al.  g-Planner: Real-time Motion Planning and Global Navigation using GPUs , 2010, AAAI.

[20]  Daniel D. Lee,et al.  Learning Dimensional Descent for Optimal Motion Planning in High-dimensional Spaces , 2011, AAAI.

[21]  Stefan Schaal,et al.  STOMP: Stochastic trajectory optimization for motion planning , 2011, 2011 IEEE International Conference on Robotics and Automation.

[22]  Didier Devaurs,et al.  Parallelizing RRT on distributed-memory architectures , 2011, 2011 IEEE International Conference on Robotics and Automation.

[23]  Maxim Likhachev,et al.  Planning in Domains with Cost Function Dependent Actions , 2011, SOCS.

[24]  Maxim Likhachev,et al.  SIPP: Safe interval path planning for dynamic environments , 2011, 2011 IEEE International Conference on Robotics and Automation.

[25]  Siddhartha S. Srinivasa,et al.  Manipulation planning with goal sets using constrained trajectory optimization , 2011, 2011 IEEE International Conference on Robotics and Automation.

[26]  P. Abbeel,et al.  Collision-Free and Curvature-Continuous Path Smoothing in Cluttered Environments , 2012 .

[27]  Kris K. Hauser.  On responsiveness, safety, and completeness in real-time motion planning , 2012, Auton. Robots.

[28]  Chonhyon Park,et al.  Real-time optimization-based planning in dynamic environments using GPUs , 2013, 2013 IEEE International Conference on Robotics and Automation.

[29]  M. Levas.  OBBTree : A Hierarchical Structure for Rapid Interference Detection , .