Constrained motion planning for multiple vehicles on SE(3)

This paper proposes a computational method to solve constrained cooperative motion planning problems for multiple vehicles undergoing translational and rotational motions. The problem is solved by means of the Lie group projection operator approach, a recently developed optimization strategy for solving continuous-time optimal control problems on Lie groups. State constraints (for collision avoidance) are handled by means of a set of barrier functions, turning the optimization approach into an interior point method. A sample computation is shown to demonstrate the effectiveness of the method.