Motion Planning for Nonlinear Robotic System based on ADMM and Convex Feasible Set Algorithm

In the motion planning of robot, nonlinear dynamic constraints and obstacle avoidance constraints make the problem highly non-convex and difficult to solve. In this paper, a general algorithmic framework for trajectory optimization is proposed based on alternating direction method of multipliers (ADMM) and convex feasible set algorithm (CFS). The ADMM framework decomposes the original problem into an optimal control sub-problem with only dynamic constraints and a subproblem with only obstacle avoidance constraints, while the sub-problem with obstacle avoidance constraints is solved via CFS based on the geometric properties of the feasible region. The convergence of proposed algorithm is proven, and the simulation results in different cases verify the feasibility and effectiveness of proposed algorithm.