Trajectory Optimization Formulation with Smooth Analytical Derivatives for Track-leg and Wheel-leg Ground Robots

Tracks, wheels, and legs are all useful locomotion modes for Unmanned Ground Vehicles (UGVs), and ground robots that combine these mechanisms have the potential to climb over large obstacles. As robot morphologies include more degrees of freedom and obstacles become increasingly large and complex, UGVs will need to rely on automatic motion planning to compute the joint trajectories for traversal. This article presents a trajectory optimization formulation for multibody UGVs with combined wheel-leg and track-leg designs. We derive the dynamics and constraints for rolling wheels and circulating elliptical tracks. Using direct collocation, we formulate a model-based trajectory optimization where all constraints and objectives are written in closed-form with smooth and exact derivatives for tractable computation times with existing large-scale nonlinear optimization solvers (<1 minute). We demonstrate the trajectory optimization on numerous simulated planar wheel-leg and track-leg morphologies completing locomotion tasks, demonstrating full body dynamic coupling for the multibody system. Future work will extend this formulation to 3D and include contact planning.