Riemannian Direct Trajectory Optimization of Rigid Bodies on Matrix Lie Groups

2025-05-05

AI Summary

In rigid-body trajectory optimization, improper rotation parameterization often induces topological violations and slow convergence. To address this, we propose a geometrically consistent direct optimization framework based on matrix Lie groups. Our method is the first to integrate Lie-group variational integrators with the Riemannian interior-point method (RIPM), yielding closed-form Riemannian first- and second-order derivatives of the dynamics constraints—enabling singularity-free, structure-preserving optimization. The SO(3) topology is rigorously preserved throughout optimization, with correctness inherently embedded in the algorithmic design. Computational complexity scales linearly with the number of planning steps and degrees of freedom. Simulation results demonstrate an order-of-magnitude improvement in convergence speed over conventional approaches, significantly enhancing the efficiency of high-precision robotic trajectory generation.

Technology Category

• Intelligent Robots: Learning & Optimization for ROB
• Search and Optimization: Sampling/Simulation-based Search
• Constraint Satisfaction and Optimization: Constraint Optimization

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Experiences and lessons learnt from Web-based algorithms and system deployments
• Responsible Web: Human-perceived consequences of algorithmic deployment on the web

Abstract

Designing dynamically feasible trajectories for rigid bodies is a fundamental problem in robotics. Although direct trajectory optimization is widely applied to solve this problem, inappropriate parameterizations of rigid body dynamics often result in slow convergence and violations of the intrinsic topological structure of the rotation group. This paper introduces a Riemannian optimization framework for direct trajectory optimization of rigid bodies. We first use the Lie Group Variational Integrator to formulate the discrete rigid body dynamics on matrix Lie groups. We then derive the closed-form first- and second-order Riemannian derivatives of the dynamics. Finally, this work applies a line-search Riemannian Interior Point Method (RIPM) to perform trajectory optimization with general nonlinear constraints. As the optimization is performed on matrix Lie groups, it is correct-by-construction to respect the topological structure of the rotation group and be free of singularities. The paper demonstrates that both the derivative evaluations and Newton steps required to solve the RIPM exhibit linear complexity with respect to the planning horizon and system degrees of freedom. Simulation results illustrate that the proposed method is faster than conventional methods by an order of magnitude in challenging robotics tasks.

Problem

• Designing dynamically feasible trajectories for rigid bodies in robotics
• Addressing slow convergence and rotation group structure violations in optimization
• Performing trajectory optimization with nonlinear constraints on matrix Lie groups

Innovation

• Uses Lie Group Variational Integrator for dynamics
• Derives Riemannian derivatives in closed-form
• Applies Riemannian Interior Point Method for optimization