Energy-efficient collision-free trajectory planning using Alternating Quadratic Programming

This paper considers the planning of collision-free and energy-optimal trajectories for linear systems with decoupled dynamics for different degrees of freedom. A direct transcription of such a problem generally results in a non-convex problem due to the collision avoidance constraint. In this paper we propose a novel Alternating Quadratic Programming (AQP) algorithm to deal with the non-convex collision avoidance constraint, and generate a suboptimal solution for the original problem by alternatively solving a number of subproblems. It is proved that the AQP algorithm is guaranteed to converge, and the solution is locally optimal when the boundary of the collision-free region satisfies certain properties. The speed and energy-saving performance of the proposed method are demonstrated by numerical examples.