A Bi-Level Optimization Approach to Joint Trajectory Optimization for Redundant Manipulators

In this work, we present an approach to minimizing the time necessary for the end-effector of a redundant robot manipulator to traverse a Cartesian path by optimizing the trajectory of its joints. Each joint has limits in the ranges of position, velocity and acceleration, the latter making jerks in joint space undesirable. The proposed approach takes this nonlinear optimization problem whose variables are path speed and joint trajectory and reformulates it into a bi-level problem. The lower-level formulation is a convex subproblem that considers a fixed joint trajectory and maximizes path speed while considering all joint velocity and acceleration constraints. Under particular conditions, this subproblem has a closed-form solution. Then, we solve a higher-level subproblem by leveraging the directional derivative of the lower-level value with respect to the joint trajectory parameters. In particular, we use this direction to implement a Primal-Dual method that considers the path accuracy and joint position constraints. We show the efficacy of our proposed approach with simulations and experimental results.