Real time trajectory optimization for nonlinear robotic systems: Relaxation and convexification

Real time trajectory optimization is critical for robotic systems. Due to nonlinear system dynamics and obstacles in the environment, the trajectory optimization problems are highly nonlinear and non convex, hence hard to be computed online. Liu, Lin and Tomizuka proposed the convex feasible set algorithm (CFS) to handle the non convex optimization in real time by convexification. However, one limitation of CFS is that it will not converge to local optima when there are nonlinear equality constraints. In this paper, the slack convex feasible set algorithm (SCFS) is proposed to handle the nonlinear equality constraints, e.g. nonlinear system dynamics, by introducing slack variables to relax the constraints. The geometric interpretation of the method is discussed. The feasibility and convergence of the SCFS algorithm is proved. It is demonstrated that SCFS performs better than existing non convex optimization methods such as interior-point, active set and sequential quadratic programming, as it requires less computation time and converges faster.