Trajectory planning for manipulators based on the optimal concatenation of LQ control primitives

A trajectory planning method for robotic systems consisting of kinematic chains is introduced based on the concatenation of control primitives. The parameterized, optimal motion primitives are derived from a parametric, linear-quadratic optimal control problem, which is formulated for the input-to-state and input-to-output linearized robot dynamics. The primitives can be concatenated, such that the resulting trajectory is optimal with respect to desired intermediate points. Here, sub-optimal intermediate points are found by a heuristic motion planning algorithm and are iteratively inserted, if necessary, to avoid collisions with obstacles in the robot workspace. All parameters for concatenated primitives are uniquely determined by the solution of a system of parameterized linear equations. In comparison to ordinary approaches based on optimal control, the computational effort for trajectory planning is reduced, since the system of linear equations can be solved on-line by algebraic computations.