Matrix Lie groups are an important class of manifolds commonly used in control and robotics, and the optimization of control policies on these manifolds is a fundamental problem. In this work, we propose a novel approach for trajectory optimization on matrix Lie groups using an augmented Lagrangian-based constrained discrete Differential Dynamic Programming. The method involves lifting the optimization problem to the Lie algebra in the backward pass and retracting back to the manifold in the forward pass. In contrast to previous approaches which only addressed constraint handling for specific classes of matrix Lie groups, the proposed method provides a general approach for nonlinear constraint handling for generic matrix Lie groups. We also demonstrate the effectiveness of the method in handling external disturbances through its application as a Lie-algebraic feedback control policy on SE(3). Experiments show that the approach is able to effectively handle configuration, velocity and input constraints and maintain stability in the presence of external disturbances.

翻译：矩阵李群是控制与机器人领域中一类重要的流形，而在此类流形上优化控制策略是一个基础性问题。本文提出了一种新颖的矩阵李群轨迹优化方法，采用基于增广拉格朗日的约束离散微分动态规划。该方法在反向传播过程中将优化问题提升至李代数，并在正向传播中回缩至流形。与先前仅针对特定矩阵李群类别处理约束的方法不同，所提方法为通用矩阵李群提供了处理非线性约束的通用框架。我们还通过将其作为SE(3)上的李代数反馈控制策略，验证了该方法在处理外部扰动方面的有效性。实验表明，该方法能够有效处理位形、速度及输入约束，并在存在外部扰动时保持系统稳定性。