Real-time Robot Arm Motion Planning and Control with Nonlinear Model Predictive Control using Augmented Lagrangian on a First-Order Solver

In this work we implement motion planning and control of a robot arm with nonlinear model predictive control using the optimization algorithm PANOC. PANOC is a first order nonlinear optimization solver, with convergence guarantees, that is matrix-free unlike the popular sequential quadratic programming and nonlinear interior-point methods. We extend this solver to deal with hard constraints using an augmented Lagrangian method. This is used to implement a multipleshooting MPC algorithm with collision avoidance capabilities on a robot arm. The computational time is benchmarked against other nonlinear optimization solvers. The algorithm is validated with simulations.

[1]  Pantelis Sopasakis,et al.  A simple and efficient algorithm for nonlinear model predictive control , 2017, 2017 IEEE 56th Annual Conference on Decision and Control (CDC).

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

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

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

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

[6]  MORITZ DIEHL,et al.  A Real-Time Iteration Scheme for Nonlinear Optimization in Optimal Feedback Control , 2005, SIAM J. Control. Optim..

[7]  Dimitri P. Bertsekas,et al.  Constrained Optimization and Lagrange Multiplier Methods , 1982 .

[8]  Pantelis Sopasakis,et al.  Aerial navigation in obstructed environments with embedded nonlinear model predictive control , 2018, 2019 18th European Control Conference (ECC).

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

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

[11]  Panagiotis Patrinos,et al.  Forward–backward quasi-Newton methods for nonsmooth optimization problems , 2016, Computational Optimization and Applications.

[12]  Giorgio Fasano,et al.  Modeling and Optimization in Space Engineering , 2012, Springer Optimization and Its Applications.

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

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

[15]  Panagiotis Patrinos,et al.  Forward-Backward Envelope for the Sum of Two Nonconvex Functions: Further Properties and Nonmonotone Linesearch Algorithms , 2016, SIAM J. Optim..

[16]  Christof Büskens,et al.  The ESA NLP Solver WORHP , 2012 .

[17]  Lorenz T. Biegler,et al.  On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming , 2006, Math. Program..

[18]  Goele Pipeleers,et al.  Embedded nonlinear model predictive control for obstacle avoidance using PANOC , 2018, 2018 European Control Conference (ECC).