Constraint-Based Prioritized Trajectory Planning for Multibody Systems

This paper presents a trajectory-planning method for multibody systems. Trajectory planning of a multibody system is formulated as a constraint-solving problem on a set of variables expressing the motion of the multibody system over a finite-time interval. Constraints express the dynamics of rigid bodies, kinematic conditions of joints, various range limitations, as well as achievement of tasks, and they can be assigned different priority levels. The prioritized constraint-solving problem is then treated under the framework of lexicographical goal programming, where the local optimality of the problem is characterized in terms of Pareto efficiency condition. Based on this observation, an algorithm that iteratively updates the variables toward a locally optimal solution is derived. The proposed method is evaluated in simulation examples.