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

Abstract

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.

Introduction

Although great progresses have been made in motion planning for robotic systems [1], challenges remain in real time planning in dynamic uncertain environment. The applications include but are not limited to real time navigation [2], autonomous driving [3], robot arm manipulation and human robot cooperation [4]. To achieve real time safety and efficiency, the robot motion should be re-planned from time to time when new information is obtained during operation, which requires the motion planning algorithms to run fast enough online.

This paper focuses on optimization-based motion planning, where an ideal low level tracking controller is assumed. The method fits into the framework of model-predictive control (MPC) [5], where an optimal trajectory is obtained by solving a constrained optimization or optimal control problem in receding horizons. The optimization problem may be highly nonlinear due to the dynamic constraints, and highly non convex due to the constraints for obstacle avoidance, which makes it hard to be solved online.

Convexification [6] is a popular way to deal with non convexity by transforming the non convex problem into a convex one. One popular convexification method is the sequential quadratic programming (SQP), which approximates the non convex problem as a sequence of quadratic programming (QP) problems and solves them iteratively. References for SQP can be founded in [7] and [8]. The method has been successfully applied to offline robot motion planning as discussed in [9] and [10]. However, as SQP is designed for general purpose, the unique geometric structure of the motion planning problems is neglected. As a consequence, it is hard to use for real time applications.

In practice, the cost function for motion planning is usually designed to be convex [11], [12], while the non convexity mainly comes from the physical constraints, e.g. obstacles. Regarding this observation, the convex feasible set algorithm (CFS) [13] was proposed to handle motion planning problems with convex objective functions and non convex inequality constraints. The idea of the CFS algorithm is to transform the origin problem into a sequence of convex subproblems by obtaining convex feasible sets within the non convex inequality constraints, and then iteratively solve the convex subproblems until solutions converge. The difference between CFS and SQP lies in the methods in obtaining the convex subproblems, where the geometric structure of the motion planning problem is fully considered in CFS.

However, one limitation of CFS is that it may not converge to local optima under nonlinear equality constraints (such as nonlinear dynamic constraints) as the convex feasible set for a nonlinear equality constraint may reduce to a singleton point.

In this paper, the slack convex feasible set algorithm (SCFS) is introduced to handle optimization problems with convex cost functions and non convex equality and inequality constraints. The idea is to relax the nonlinear equality constraints to several nonlinear inequality constraints using slack variables and then solve the relaxed problem using CFS. The feasibility, convergence and optimality of the algorithm will be proved in the paper. The performance of SCFS will be compared to that of SQP as well as other existing non convex optimization algorithms.

The remainder of the paper will be organized as follows: in Section 2, a benchmark motion planning problem is proposed; Section 3 reviews the convex feasible set algorithm; Section 4 introduces the slack convex feasible set algorithm; Section 5 illustrates the performance of SCFS; Section 6 concludes the paper.