Adaptive Gaussian Process based Stochastic Trajectory Optimization for Motion Planning

We propose a new formulation of optimal motion planning (OMP) algorithm for robots operating in a hazardous environment, called adaptive Gaussian-process based stochastic trajectory optimization (AGP-STO). It first restarts the accelerated gradient descent with the reestimated Lipschitz constant (LreAGD) to improve the computation efficiency, only requiring 1storder momentum. However, it still cannot infer a global optimum of the nonconvex problem, informed by the prior information of Gaussian-process (GP) and obstacles. So it then integrates the adaptive stochastic trajectory optimization (ASTO) in the L-reestimation process to learn the GP-prior rewarded by the important samples via accelerated moving averaging (AMA). Moreover, we introduce the incremental optimal motion planning (iOMP) to upgrade AGP-STO to iAGP-STO. It interpolates the trajectory incrementally among the previously optimized waypoints to ensure time-continuous safety. Finally, we benchmark iAGP-STO against the numerical (CHOMP, TrajOpt, GPMP) and sampling (STOMP, RRT-Connect) methods and conduct the tuning experiment of key parameters to show how the integration of L-reAGD, ASTO, and iOMP elevates computation efficiency and reliability. Moreover, the implementation of iAGPSTO on LBR-iiwa, multi-AGV, and rethink-Baxter demonstrates its application in manipulation, collaboration, and assistance.

[1]  Ivan Markovic,et al.  Stochastic Optimization for Trajectory Planning with Heteroscedastic Gaussian Processes , 2019, 2019 European Conference on Mobile Robots (ECMR).

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

[3]  Lydia E. Kavraki,et al.  Randomized preprocessing of configuration for fast path planning , 1994, Proceedings of the 1994 IEEE International Conference on Robotics and Automation.

[4]  Leslie Pack Kaelbling,et al.  FFRob: Leveraging symbolic planning for efficient task and motion planning , 2016, Int. J. Robotics Res..

[5]  Joachim Denzler,et al.  Large-Scale Gaussian Process Inference with Generalized Histogram Intersection Kernels for Visual Recognition Tasks , 2016, International Journal of Computer Vision.

[6]  Emmanuel J. Candès,et al.  Adaptive Restart for Accelerated Gradient Schemes , 2012, Foundations of Computational Mathematics.

[7]  New York Dover,et al.  ON THE CONVERGENCE PROPERTIES OF THE EM ALGORITHM , 1983 .

[8]  Jorge Nocedal,et al.  Algorithm 778: L-BFGS-B: Fortran subroutines for large-scale bound-constrained optimization , 1997, TOMS.

[9]  Sergey Levine,et al.  Trust Region Policy Optimization , 2015, ICML.

[10]  Jieping Ye,et al.  An efficient algorithm for a class of fused lasso problems , 2010, KDD.

[11]  Liam Paull,et al.  Area coverage planning that accounts for pose uncertainty with an AUV seabed surveying application , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[12]  Sean Quinlan.  Real-time modification of collision-free paths , 1994 .

[13]  S. LaValle.  Rapidly-exploring random trees : a new tool for path planning , 1998 .

[14]  Pieter Abbeel,et al.  Motion planning with sequential convex optimization and convex collision checking , 2014, Int. J. Robotics Res..

[15]  Dimitris N. Metaxas,et al.  Human Motion Planning Based on Recursive Dynamics and Optimal Control Techniques , 2002 .

[16]  Jieping Ye,et al.  An accelerated gradient method for trace norm minimization , 2009, ICML '09.

[17]  Geoffrey E. Hinton,et al.  Using Expectation-Maximization for Reinforcement Learning , 1997, Neural Computation.

[18]  Masayoshi Tomizuka,et al.  Algorithmic safety measures for intelligent industrial co-robots , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[19]  Johan Meyers,et al.  Sequential Quadratic Programming (SQP) for optimal control in direct numerical simulation of turbulent flow , 2014, J. Comput. Phys..

[20]  M. Al-Baali.  Descent Property and Global Convergence of the Fletcher—Reeves Method with Inexact Line Search , 1985 .

[21]  F. Dellaert.  Factor Graphs and GTSAM: A Hands-on Introduction , 2012 .

[22]  Siddhartha S. Srinivasa,et al.  CHOMP: Covariant Hamiltonian optimization for motion planning , 2013, Int. J. Robotics Res..

[23]  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).

[24]  Tomomichi Sugihara,et al.  Solvability-Unconcerned Inverse Kinematics by the Levenberg–Marquardt Method , 2011, IEEE Transactions on Robotics.

[25]  Jun Kinugawa,et al.  Adaptive Motion Planning for a Collaborative Robot Based on Prediction Uncertainty to Enhance Human Safety and Work Efficiency , 2019, IEEE Transactions on Robotics.

[26]  Xiaoqian Jiang,et al.  Adaptive Gaussian Process for Short-Term Wind Speed Forecasting , 2010, ECAI.

[27]  Saeed Ghadimi,et al.  Accelerated gradient methods for nonconvex nonlinear and stochastic programming , 2013, Mathematical Programming.

[28]  D K Smith,et al.  Numerical Optimization , 2001, J. Oper. Res. Soc..

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

[30]  Leslie Pack Kaelbling,et al.  LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics , 2012, 2012 IEEE International Conference on Robotics and Automation.

[31]  Kenneth Levenberg.  A METHOD FOR THE SOLUTION OF CERTAIN NON – LINEAR PROBLEMS IN LEAST SQUARES , 1944 .

[32]  Pieter Abbeel,et al.  Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization , 2013, Robotics: Science and Systems.

[33]  Jimmy Ba,et al.  Adam: A Method for Stochastic Optimization , 2014, ICLR.

[34]  Z. J. Shi,et al.  New Inexact Line Search Method for Unconstrained Optimization , 2005 .

[35]  Jorge Cortés,et al.  Nesterov Acceleration for Equality-Constrained Convex Optimization via Continuously Differentiable Penalty Functions , 2021, IEEE Control Systems Letters.

[36]  Riadh Omheni,et al.  A globally and quadratically convergent primal–dual augmented Lagrangian algorithm for equality constrained optimization , 2017, Optim. Methods Softw..

[37]  Léon Bottou,et al.  On-line learning and stochastic approximations , 1999 .

[38]  Jin Wang,et al.  Incrementally Stochastic and Accelerated Gradient Information mixed Optimization for Manipulator Motion Planning , 2021, ArXiv.

[39]  Byron Boots,et al.  Continuous-time Gaussian process motion planning via probabilistic inference , 2017, Int. J. Robotics Res..

[40]  Sethu Vijayakumar,et al.  Online Hybrid Motion Planning for Dyadic Collaborative Manipulation via Bilevel Optimization , 2020, IEEE Transactions on Robotics.

[41]  Tim Mitchell,et al.  A BFGS-SQP method for nonsmooth, nonconvex, constrained optimization and its evaluation using relative minimization profiles , 2017, Optim. Methods Softw..

[42]  James T. Kwok,et al.  Accelerated Gradient Methods for Stochastic Optimization and Online Learning , 2009, NIPS.

[43]  Takayuki Osa.  Multimodal trajectory optimization for motion planning , 2020, Int. J. Robotics Res..

[44]  Ronald Fedkiw,et al.  Signed Distance Functions , 2003 .

[45]  Frank Dellaert,et al.  iSAM2: Incremental smoothing and mapping using the Bayes tree , 2012, Int. J. Robotics Res..

[46]  Michael I. Jordan,et al.  Accelerated Gradient Descent Escapes Saddle Points Faster than Gradient Descent , 2017, COLT.

[47]  Chonhyon Park,et al.  ITOMP: Incremental Trajectory Optimization for Real-Time Replanning in Dynamic Environments , 2012, ICAPS.

[48]  Marc Toussaint,et al.  A Bayesian View on Motor Control and Planning , 2010, From Motor Learning to Interaction Learning in Robots.

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

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

[51]  Jun Kinugawa,et al.  Incremental Learning of Spatial-Temporal Features in Human Motion Patterns with Mixture Model for Planning Motion of a Collaborative Robot in Assembly Lines , 2019, 2019 International Conference on Robotics and Automation (ICRA).

[52]  A. Wade.  Explicit laws of large numbers for random nearest-neighbour-type graphs , 2006, Advances in Applied Probability.

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

[54]  Lydia E. Kavraki,et al.  Analysis of probabilistic roadmaps for path planning , 1998, IEEE Trans. Robotics Autom..

[55]  Byron Boots,et al.  Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs , 2016, Robotics: Science and Systems.

[56]  Emilio Frazzoli,et al.  Sampling-based algorithms for optimal motion planning , 2011, Int. J. Robotics Res..

[57]  Trevor Darrell,et al.  Gaussian Processes for Object Categorization , 2010, International Journal of Computer Vision.

[58]  Yoram Singer,et al.  Adaptive Subgradient Methods for Online Learning and Stochastic Optimization , 2011, J. Mach. Learn. Res..

[59]  John J. Leonard,et al.  Probabilistic cooperative mobile robot area coverage and its application to autonomous seabed mapping , 2018, Int. J. Robotics Res..

[60]  Y. Nesterov.  A method for unconstrained convex minimization problem with the rate of convergence o(1/k^2) , 1983 .

[61]  Frank Hutter,et al.  SGDR: Stochastic Gradient Descent with Warm Restarts , 2016, ICLR.

[62]  Byron Boots,et al.  Motion planning with graph-based trajectories and Gaussian process inference , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[63]  Jorge Nocedal,et al.  On the limited memory BFGS method for large scale optimization , 1989, Math. Program..