Designing dynamically feasible trajectories for rigid bodies is a fundamental problem in robotics. While direct methods are widely used, the existing constrained optimizers typically operate in Euclidean space and ignore the manifold structure of rigid body motions. This mismatch may introduce singularities or lead to poorly conditioned optimization problems. To bridge this gap, we develop a structure-aware framework for constrained trajectory optimization directly on matrix Lie groups. Our approach is based on the second-order rigid body models utilizing Lie group structures, which enables efficient Newton-type updates while preserving the underlying geometry. Building on this model, we propose a line-search Lie Group Interior Point Method (LieIPM) to handle constraints on the manifolds. We instantiate the framework for rigid body motion planning using Lie group variational integrators and derive closed-form intrinsic derivatives that exploit group symmetries. The LieIPM preserves the topology of rotation motions by construction and avoids singularities. Numerical results demonstrate superior robustness and faster convergence compared to general-purpose solvers and structure-exploiting optimal control methods.

翻译：为刚体设计动力学可行轨迹是机器人学中的基本问题。尽管直接方法被广泛使用，但现有约束优化器通常作用于欧氏空间，忽略了刚体运动的流形结构。这种不匹配可能导致奇异点或导致条件数较差的优化问题。为此，我们提出了一种直接在矩阵李群上进行约束轨迹优化的结构感知框架。本方法基于利用李群结构的二阶刚体模型，能够在保持底层几何特性的同时实现高效的牛顿型更新。在此模型基础上，我们提出了一种线搜索李群内点法（LieIPM）以处理流形上的约束。我们利用李群变分积分器将框架具体应用于刚体运动规划，并推导出利用群对称性的闭式本征导数。LieIPM通过构造保持了旋转运动的拓扑结构并避免奇异点。数值结果表明，与通用求解器和结构感知最优控制方法相比，本方法具有更优的鲁棒性和更快的收敛速度。