Constrained Trajectory Optimization on Matrix Lie Groups via Lie-Algebraic Differential Dynamic Programming

Abstract

Matrix Lie groups are an important class of manifolds commonly used in control and robotics, and optimizing control policies on these manifolds is a fundamental problem. In this work, we propose a novel augmented Lagrangian-based constrained Differential Dynamic Programming (DDP) approach specifically designed for trajectory optimization on matrix Lie groups. Our method formulates the optimization problem in the error-state space, employs automatic differentiation during the backward pass, and ensures manifold consistency through discrete-time Lie-group integration during the forward pass. Unlike previous methods limited to specific manifold classes, our approach robustly handles generic nonlinear constraints across arbitrary matrix Lie groups and exhibits resilience to constraint violations during training. We evaluate the proposed DDP algorithm through extensive experiments, demonstrating its efficacy in managing constraints within a rigid-body mechanical system on SE(3), its computational superiority compared to existing optimization solvers, robustness under external disturbances as a Lie-algebraic feedback controller, and effectiveness in trajectory optimization tasks including realistic quadrotor scenarios as underactuated systems and deformable objects whose deformation dynamics are represented in SL(2). The experimental results validate the generality, stability, and computational efficiency of our proposed method.