Smooth proximity computation for collision-free optimal control of multiple robotic manipulators

This paper presents a novel approach for trajectory planning of multiple robot manipulators operating amongst obstacles. Karush-Kuhn-Tucker (KKT) conditions are exploited to compute the proximity between line-swept sphere (LSS) bounding volumes used to model potentially colliding objects. The KKT multipliers and the parameters giving the minimum distance between LSS volumes are augmented into the manipulator trajectory planning problem as dummy control variables. These extra variables allow the planning problem to be cast as a standard nonlinear optimal control problem with smooth path constraints, which is then solved using the pseudospectral method. The utility of the approach is demonstrated by a trajectory planning example involving stationary workspace obstacles and for a centralized multi-robot system in which each robot acts as a dynamic obstacle that the other should avoid. The optimal control formulation incorporates practical constraints on the manipulator joint angles, velocities and accelerations as well as limits on the control torque. The computed collision-free optimal trajectories are executed on a pair of experimental robots to verify the feasibility of the numerical results.