💡 导语
在系列笔记的前两篇《微分动态规划 (DDP) 全景探秘》与《非线性约束微分动态规划 (CDDP) 深度剖析》中，我们分别探讨了欧氏空间下的标准 DDP 基础与非线性约束处理机制。然而，在航天器六自由度交会对接、机械臂空间姿态操作以及无人机敏捷机动等物理任务中，系统的位形空间位于矩阵李群（如 $\mathrm{SO}(3),\mathrm{SE}(3)$）之上。若强行使用欧拉角或局部欧氏坐标，不可避免会遭遇万向节死锁（Gimbal Lock）、坐标奇异性以及数值漂移；若采用四元数，又必须引入繁琐的单位范数约束。
本文以 Gokhan Alcan 等人的前沿成果为核心，深入剖析流形上的约束李代数微分动态规划 (Constrained Lie-Algebraic DDP)：如何在反向传播中将优化问题提升至局部切空间（李代数），将约束纳入增广拉格朗日（Augmented Lagrangian）框架；在正向传播中利用群指数映射缩回流形，天然保持李群代数拓扑结构。本文同时结合我们在开源工程中的 Python/JAX 复现试验，深入分析该方法在三维刚体避障、外部随机扰动抑制与真实四旋翼极端机动下的表现，并揭示论文中关于矩阵对数二阶近似的关键边界。

---

0 论文摘要与核心读者批注

---

1 引言

许多物理系统（如机器人）具有非欧几里得位形空间。使用局部坐标建模这些空间会导致奇异性和退化问题，某些系统位形可能无法表示。例如，用欧拉角表示旋转会出现“万向节锁”现象——三个旋转轴中的两个对齐，导致失去一个自由度🌹 对应精读批注 5 ↗，使得特定运动跟踪或位形转换无法实现[1,2]。此类奇异性会使系统位形间的转换难以准确表示，可能需要额外技术来避免或缓解。

🎬 点击展开：3D 万向节锁动态运行机理演示 (GIF) ▶ 点击展开 ▼ 点击收起

图 1.1：万向节锁 (Gimbal Lock) 3D 动态演示。当中间绿色俯仰环抬起至 $\theta = 90^\circ$ 奇点时，外层红色偏航环与内层蓝色滚转环轴线完全共线重合，系统瞬时退化为仅能在一个平面旋转，丧失了侧向横滚机动自由度（详见 8.10 节数学推导）。

矩阵李群可有效建模位形空间的几何结构，为理解底层系统的结构和运动提供了结构化连续框架[3,4]。近年来，几何控制技术在机器人与控制系统中备受关注，它无缝融合了微分几何与控制理论[5,6]。然而，现有几何最优控制问题的求解方法要么依赖特定问题细节简化最优性条件[7,8]，要么随时间 horizon 扩展计算复杂度急剧增加[9,10]。为克服这些限制，微分动态规划（DDP）成为数值求解此类问题的有前景方法。但现有基于DDP的几何控制技术存在局限性，包括仅显式考虑特定矩阵李群[11]、忽略状态约束[12,13,14]或约束处理局限于特定群[15]。

本文提出一种基于DDP的矩阵李群几何控制方法，纳入一般非线性约束（图1），克服了上述局限。

图1：所提方法图解。a) 使用给定的标称输入序列在位形流形上生成轨迹。b) 在局部正切空间内计算Q函数的导数。c) 后向传播中，位形和速度约束的影响也包含在局部正切空间内。d) 前向传播采用闭环控制更新控制输入。更新后的轨迹作为下一次迭代的新标称轨迹。主要贡献包括：

(1) 针对矩阵李群轨迹优化，提出一种新颖的基于增广拉格朗日的约束DDP算法。

(2) 为一般矩阵李群的非线性约束处理提供原理性方法，克服了文献[15]仅处理SO(3)约束的局限。

(3) 在SE(3)刚体动力学的简单机械系统中评估了所提方法处理约束的能力，对比了其与现有优化求解器的计算效率，并在真实四旋翼场景中测试了性能。

本文结构如下：第2节讨论从经典控制到光滑流形系统约束最优控制的相关工作；第3节提供矩阵李群的预备知识；第4节定义矩阵李群的轨迹优化问题；第5节详细介绍所提方法；第6节通过SE(3)和真实四旋翼动力学的数值仿真实验验证方法有效性；第7节总结并提出未来工作方向。

2 相关工作

近年来，将现代控制理论融入光滑流形系统的研究取得了显著进展，产生了稳定性分析、可控性和反馈控制设计等多样化理论成果[5,6]。早期研究中，经典PID型控制器已成功应用于全驱动机械系统[5]，利用黎曼流形上定义的位形和速度误差。近年，研究人员考虑SO(3)和SE(3)上刚体动力学，为无人机开发了局部指数稳定的几何跟踪控制器[16,17]。尽管这些方法擅长点镇定，但无法生成最优轨迹，需预定义参考轨迹实现复杂任务。

基于几何方法的优化控制旨在黎曼流形上生成最优轨迹。Bloch等[7]研究了SO(3)李群上表示3D刚体动力学的变分最优控制问题，Kobilarov和Marsden[8]推导了对应高阶系统离散测地线的最优轨迹必要条件，并开发了数值计算方法。Teng等[13]为连通矩阵李群上的系统跟踪控制开发了几何误差状态模型预测控制（MPC）。尽管这些方法考虑了动态和运动学约束，但缺乏处理障碍/位形避障等一般状态约束的能力。Saccon等[18]推导了李群上的投影算子框架，将其应用于连续时间轨迹优化问题，并利用障碍函数实现SE(3)上多车辆运动规划的碰撞避障[19]。

在轨迹优化领域，各类研究以不同方式处理约束。最直接的方法是构建非线性规划，在满足所有边界条件和约束的同时最小化成本函数。Lee等[20]提出SO(3)群上航天器姿态控制的非线性MPC框架，考虑了控制和禁飞区约束。类似地，Hong等[21]将高斯-牛顿法与近端算子结合，处理SO(3)上轨迹优化的不等式约束。部分研究者聚焦于其他矩阵李群的轨迹优化，如Kalabic等[22]设计SE(3)上的MPC，仅考虑状态和输入的边界约束；Ding等[23]提出SE(3)群的无表示MPC。但这些方法存在局限，如限于特定矩阵李群，且随时间步数增加复杂度上升。最近，Watterson等[24]提出流形上基于图搜索的安全走廊方法，优化带障碍的黎曼流形轨迹。尽管在某些机器人应用中成功，但其对参数化的敏感性影响了最优性和计算成本。

不同于上述基于MPC的直接方法，DDP是通过优化控制输入求解最优控制问题的间接数值方法。DDP由Mayne和Jacobson[25]最初提出，已应用于广泛的复杂高维系统[26]。其关键优势包括可扩展性（处理多自由度大型复杂系统）、快速收敛率（快速找到近最优解），以及生成反馈控制策略（实时实现最优控制）。

早期多项研究探索了DDP在几何控制中的应用[11,27]，聚焦于特定矩阵李群（如SE(3)），推导DDP算法的最终形式用于几何控制。Boutselis和Theodorou[12]通过对李群上定义的成本函数和动力学使用二次展开方案扩展了原始DDP方法，证明DDP比序列二次规划（SQP）方法收敛率显著更优。Teng等[14]通过在李代数中设计控制目标，进一步改善了矩阵群DDP的收敛性能。这两种方法[12,14]均在无约束框架下构建矩阵李群的轨迹优化。为解决这一局限，Liu等[15]通过施加SO(3)约束扩展了文献[12]的工作，但该方法无法推广到一般矩阵李群的非线性约束。本文旨在通过扩展李代数成本定义思想[14]，开发适用于非线性约束下矩阵李群的DDP方法，解决一般约束处理问题。

3 预备知识

设$\mathcal{G}$为n维矩阵李群，$\mathfrak{g}$为其关联李代数（即单位元处的切空间）。通过以下算子可定义向量空间${\mathbb{R}}^n$与$\mathfrak{g}$的同构：

$$\begin{array}{rl}(\cdot {)}^{\wedge }& :{\mathbb{R}}^n\mapsto \mathfrak{g}\\ (\cdot {)}^{\vee }& :\mathfrak{g}\mapsto {\mathbb{R}}^n\end{array}\text{\hspace{1em}(1)}$$

对任意$\varphi \in {\mathbb{R}}^n$和$\mathcal{X}\in \mathcal{G}$，${\mathbb{R}}^n$与$\mathcal{G}$间的映射可通过函数$\exp (\cdot ):{\mathbb{R}}^n\mapsto \mathcal{G}$和$\log (\cdot ):\mathcal{G}\mapsto {\mathbb{R}}^n$定义为：

$$\begin{array}{r}\mathrm{Exp}(\varphi )=\mathrm{expm}({\varphi }^{\wedge })=\mathcal{X}\\ \log (\mathcal{X})=\mathrm{logm}(\mathcal{X}{)}^{\vee }=\varphi \end{array}\text{\hspace{1em}(2)}$$

其中$\mathrm{expm}(\cdot )$和$\mathrm{logm}(\cdot )$分别为矩阵指数和对数。

伴随作用记为${\mathrm{Ad}}_{\mathcal{X}}:\mathfrak{g}\mapsto \mathfrak{g}$（对任意$\mathcal{X}\in \mathcal{G}$），是允许框架变换的李代数同构。给定$\varphi ,\xi ,\eta \in {\mathbb{R}}^n$和${\varphi }^{\wedge },{\xi }^{\wedge },{\eta }^{\wedge }\in \mathfrak{g}$，伴随作用可表示为函数形式：

$${\mathrm{Ad}}_{\mathcal{X}}(\varphi )=\mathcal{X}{\varphi }^{\wedge }{\mathcal{X}}^{-1}\text{\hspace{1em}(3)}$$

或矩阵形式：

$$({\mathrm{Ad}}_{\mathcal{X}}\varphi {)}^{\wedge }=\mathcal{X}{\varphi }^{\wedge }{\mathcal{X}}^{-1}\text{\hspace{1em}(4)}$$

伴随映射是伴随作用关于$\mathcal{X}$在单位元处的导数，定义为：

$${\mathrm{ad}}_{\xi }\eta =[{\xi }^{\wedge },{\eta }^{\wedge }]\text{\hspace{1em}(5)}$$

其中$[{\xi }^{\wedge },{\eta }^{\wedge }]$为李括号，定义为：

$$[{\xi }^{\wedge },{\eta }^{\wedge }]={\xi }^{\wedge }{\eta }^{\wedge }-{\eta }^{\wedge }{\xi }^{\wedge }\text{\hspace{1em}(6)}$$

4 问题定义

考虑状态位于矩阵李群切丛的系统，这类系统[8]的状态可表示为对$\{\mathcal{X},{\xi }^{\wedge }\}\in \mathcal{G}\times \mathfrak{g}$，其中$\mathcal{X}$表示位形，${\xi }^{\wedge }$表示该位形的速度（变化率）。此类系统的连续时间运动方程可写为：

$$\begin{array}{rl}{\dot{\mathcal{X}}}_t& ={\mathcal{X}}_t{\xi }_t^{\wedge }\\ {\dot{\xi }}_t& =f({\mathcal{X}}_t,{\xi }_t,u_t)\end{array}\text{\hspace{1em}(7)}$$

其中$u_t\in {\mathbb{R}}^m$为广义控制输入，$f(\cdot )$为速度动力学函数。给定初始状态$\{{\mathcal{X}}_0,{\xi }_0\}$、目标状态$\{{\mathcal{X}}_g,{\xi }_g\}$和时间 horizon $N$，定义离散时间约束最优控制问题为：

$$\begin{array}{rlrl}\underset{u_0,\dots ,u_{N-1}}{\min }& {\ell }^f({\mathcal{X}}_N,{\xi }_N)+\sum _{k=0}^{N-1}\ell ({\mathcal{X}}_k,{\xi }_k,u_k)& & \\ \text{s.t.}& {\mathcal{X}}_{k+1}=F_{\mathcal{X}}({\mathcal{X}}_k,{\xi }_k),& & k=0,\dots ,N-1,\\ & {\xi }_{k+1}=F_{\xi }({\mathcal{X}}_k,{\xi }_k,u_k),& & k=0,\dots ,N-1,\\ & u_{\min }\le u_k\le u_{\max },& & k=0,\dots ,N-1,\\ & \mathbf{g}({\mathcal{X}}_k,{\xi }_k,u_k)\le 0,& & k=0,\dots ,N-1,\\ & {\mathcal{X}}_0,{\xi }_0\text{ 给定}& & \end{array}\text{\hspace{1em}(8)}$$

其中${\ell }^f:\mathcal{G}\times {\mathbb{R}}^n\mapsto \mathbb{R}$和$\ell :\mathcal{G}\times {\mathbb{R}}^n\times {\mathbb{R}}^m\mapsto \mathbb{R}$分别为终端成本和运行成本，$F_{\mathcal{X}}$和$F_{\xi }$为位形和速度动力学的离散形式（可通过固定时间步长$\Delta t$的零阶保持或一阶欧拉积分获得），$\mathbf{g}$为$p$个约束的向量，以可微非线性函数形式表示状态约束。

📚 论文发表写作范式参考：
若您正在撰写或准备向 IEEE TAC / Automatica / IEEE T-RO / TAES 投稿六自由度几何控制（NMPC / CDDP）相关论文，我们已将本节的符号系统与问题描述进一步系统化升华、补全对偶伴随 ${\mathrm{ad}}^{\ast }$ 与保流形离散化，封装为可直接采纳复制的标准论文第二、三节模板，详见独立专篇：
👉 《现代几何最优控制论文写作规范与范式手册：从抽象李群符号到刚体最优控制问题描述》。

5 提出的方法

第4节概述的一般问题解析解通常困难，数值寻找全局最小值耗时且可能不可行，尤其对高维非线性系统。因此，本文提出一种寻找可行解（即使非全局最优）的方法，利用DDP框架[25]迭代求解反向传播中的子优化问题，并基于找到的最优策略在正向传播中生成新轨迹，逼近局部最优🌌 对应精读批注 7 ↗。

为处理状态位于矩阵李群切丛的系统约束，我们优化增广拉格朗日函数，将轨迹成本与约束惩罚结合。该方法包括：在反向传播阶段通过在对应李代数中计算成本函数梯度，将问题提升至李代数；正向传播阶段利用反向传播获得的最优策略积分动力学，缩回至流形。

5.1 切空间动力学

DDP的核心思想是：每次迭代中，所有非线性约束和目标用一阶或二阶泰勒展开近似，使作用于标称轨迹偏差的近似函数可通过离散线性二次调节器（LQR）技术求解。为在李代数中定义成本和约束函数，需确定位形的误差动力学。为获得扰动状态动力学，采用Teng等[13]的方法，此处为完整性概述必要步骤，感兴趣读者可参考[13,14]🏛️ 对应学术谱系考据 8 ↗。

考虑标称状态$\{\mathcal{X},{\xi }^{\wedge }\}$附近的扰动状态$\{{\mathcal{X}}_p,{\xi }_p^{\wedge }\}$，位形误差$\Psi$定义为：

$$\Psi ={\mathcal{X}}^{-1}{\mathcal{X}}_p\in \mathcal{G}\text{\hspace{1em}(9)}$$

对(9)两边求导得位形误差动力学：

$$\begin{array}{rl}\dot{\Psi }& ={\mathcal{X}}^{-1}\frac{d}{dt}\left({\mathcal{X}}_p\right)+\frac{d}{dt}\left({\mathcal{X}}^{-1}\right){\mathcal{X}}_p\\ & ={\mathcal{X}}^{-1}{\dot{\mathcal{X}}}_p-{\mathcal{X}}^{-1}\dot{\mathcal{X}}{\mathcal{X}}^{-1}{\mathcal{X}}_p\\ & ={\mathcal{X}}^{-1}{\mathcal{X}}_p{\xi }_p^{\wedge }-{\mathcal{X}}^{-1}\mathcal{X}{\xi }^{\wedge }{\mathcal{X}}^{-1}{\mathcal{X}}_p\\ & =\Psi {\xi }_p^{\wedge }-{\xi }^{\wedge }\Psi \end{array}\text{\hspace{1em}(10)}$$

定义${\mathbb{R}}^n$中的向量$\psi$，使${\psi }^{\wedge }$的矩阵指数对应$\Psi$，即$\Psi =\mathrm{expm}({\psi }^{\wedge })$。利用矩阵指数的一阶近似$\mathrm{expm}({\psi }^{\wedge })\approx I_n+{\psi }^{\wedge }$，(10)中的位形误差动力学可线性化为：

$$\begin{array}{rl}\dot{\Psi }& =\Psi {\xi }_p^{\wedge }-{\xi }^{\wedge }\Psi \\ & \approx (I_n+{\psi }^{\wedge }){\xi }_p^{\wedge }-{\xi }^{\wedge }(I_n+{\psi }^{\wedge })\\ & ={\xi }_p^{\wedge }-{\xi }^{\wedge }+{\psi }^{\wedge }{\xi }_p^{\wedge }-{\xi }^{\wedge }{\psi }^{\wedge }\\ & ={\xi }_p^{\wedge }-{\xi }^{\wedge }+{\psi }^{\wedge }{\xi }^{\wedge }-{\xi }^{\wedge }{\psi }^{\wedge }+{\psi }^{\wedge }({\xi }_p^{\wedge }-{\xi }^{\wedge })\\ & \approx {\xi }_p^{\wedge }-{\xi }^{\wedge }+[{\psi }^{\wedge },{\xi }^{\wedge }]\\ & ={\xi }_p^{\wedge }-{\xi }^{\wedge }+{\mathrm{ad}}_{\psi }\xi \\ & =({\xi }_p-\xi {)}^{\wedge }-{\mathrm{ad}}_{\xi }\psi \\ ⟹\dot{\psi }& =\delta \xi -{\mathrm{ad}}_{\xi }\psi \end{array}\text{\hspace{1em}(11)}$$

注意，为获得位形误差的线性动力学，舍弃了${\psi }^{\wedge }({\xi }_p^{\wedge }-{\xi }^{\wedge })$的二阶项[13, 14]🔬 对应精度妥协与离散化考据 9 ↗。(11)中的$\psi$是李代数中表示的扰动位形，扰动速度和控制输入定义为：

$$\delta \xi ={\xi }_p-\xi ,\text{and}\delta u=u_p-u\text{\hspace{1em}(12)}$$

扰动速度动力学可近似至二阶：

$$\begin{array}{rl}\delta {\dot{\xi }}_t& ={\mathbf{\Gamma }}_t\delta {\xi }_t+{\mathbf{A}}_t\delta u_t\\ & +\frac{1}{2}(H_{{\xi }_t}(\delta {\xi }_t\otimes \delta {\xi }_t)+H_{u_t}(\delta u_t\otimes \delta u_t))\\ & +H_{{\xi }_t{u}_t}(\delta {\xi }_t\otimes \delta u_t)\end{array}\text{\hspace{1em}(13)}$$

其中${\mathbf{\Gamma }}_t$和${\mathbf{A}}_t$为雅可比矩阵，$H_{{\xi }_t},H_{u_t},H_{{\xi }_t,u_t}$为在标称轨迹处评估的速度动力学$(F_{\xi })$的海森矩阵，张量积$\otimes$表示扰动向量的外积。

最后，定义扰动状态（如[13]定义的误差状态）为拼接：

$$\mathbf{x}=\left[\begin{array}{c}\psi \\ \delta \xi \end{array}\right],\overline{\mathbf{u}}=\delta \mathbf{u}\text{\hspace{1em}(14)}$$

注意，扰动状态动力学$\dot{\mathbf{x}}=h(\mathbf{x},\mathbf{u})$在位形状态下是线性的，但在速度动力学下是非线性的。忽略海森矩阵，可得线性扰动状态动力学：

$$\dot{\mathbf{x}}=\underset{\triangleq A_t}{\underbrace{\left[\begin{array}{cc}-{\mathrm{ad}}_{\xi }& I_n\\ 0& {\mathbf{\Gamma }}_t\end{array}\right]}}\mathbf{x}+\underset{\triangleq B_t}{\underbrace{\left[\begin{array}{c}{0}_{n\times m}\\ {\mathbf{A}}_t\end{array}\right]}}\overline{\mathbf{u}}\text{\hspace{1em}(15)}$$

在DDP框架中使用线性动力学即迭代LQR。

矩阵${\mathbf{A}}_t$和$B_t$的离散时间对应形式可通过固定时间步长$\Delta t$的零阶保持或一阶欧拉积分简单获得。

5.2 约束处理

为在DDP框架中处理约束，将约束近似至二阶：🐞 对应符号捉虫与脉络溯源 10 ↗

$$\begin{array}{rl}\overline{\mathbf{c}}(\mathbf{x}+\delta \mathbf{x},\mathbf{u}+\delta \mathbf{u})& \approx \overline{\mathbf{c}}(\mathbf{x},\mathbf{u})+{\overline{\mathbf{c}}}_{\mathbf{x}}\delta \mathbf{x}+{\overline{\mathbf{c}}}_{\mathbf{u}}\delta \mathbf{u}\\ & +\frac{1}{2}\left(\delta {\mathbf{x}}^{\top }{\overline{\mathbf{c}}}_{\mathbf{x}\mathbf{x}}\delta \mathbf{x}+\delta {\mathbf{u}}^{\top }{\overline{\mathbf{c}}}_{\mathbf{u}\mathbf{u}}\delta \mathbf{u}\right)+\delta {\mathbf{x}}^{\top }{\overline{\mathbf{c}}}_{\mathbf{x}\mathbf{u}}\delta \mathbf{u}\end{array}\text{\hspace{1em}(16)}$$

其中${\overline{\mathbf{c}}}_{\mathbf{x}},{\overline{\mathbf{c}}}_{\mathbf{u}}$为$\overline{\mathbf{c}}$的一阶导数，${\overline{\mathbf{c}}}_{\mathbf{x}\mathbf{x}},{\overline{\mathbf{c}}}_{\mathbf{x}\mathbf{u}},{\overline{\mathbf{c}}}_{\mathbf{u}\mathbf{u}}$为在标称状态轨迹处评估的二阶导数。

位形状态中两个群元素的差在群中产生测地线。为处理此，我们提出将距离测地线映射至当前时刻位形的切空间，并在该向量空间中处理约束。

例如，位形避障约束通常可使用n-球面函数表示为不等式约束，n-球中心位于需避开的位形$({\mathcal{X}}_c)$，半径$(r_c)$定义受限区域🎯 对应几何避障与截断考据 11 ↗，从而指定应避开的位形区域。标称位形与受限位形的距离${\Psi }^c={\mathcal{X}}^{-1}{\mathcal{X}}_c$，可在标称轨迹的切空间中近似为：

$${\psi }^c=\mathrm{logm}\left({\Psi }^c\right)\approx {\left(\left({\Psi }^c-I\right)-0.5{\left({\Psi }^c-I\right)}^2\right)}^{\vee }\text{\hspace{1em}(17)}$$

其中$I$为与${\Psi }^c$同尺寸的单位矩阵。则位形避障约束可写为：

$${\overline{\mathbf{c}}}^{\mathcal{X}}\left({\psi }^c\right)=\left(r_c^2-{\Vert {\psi }^c\Vert }_2\right)\le 0\text{\hspace{1em}(18)}$$

此方法中，我们对n维球的每个轴考虑相同受限区域，但也可对每个轴指定不同半径，形成n维椭球受限区域，本文方法也可处理此类位形约束。

5.3 增广拉格朗日公式

求解约束优化问题的有效方法是将约束纳入目标函数，并迭代增加违反或接近约束的惩罚，此技术称为惩罚法，保证当惩罚系数趋近无穷时收敛至最优解。但由于有限精度算术限制，这在数值优化例程中可能不实用。增广拉格朗日方法[29]提供替代方案，通过维持与约束相关的拉格朗日乘子估计，允许收敛至最优解而无需惩罚项无限增加。此处获得增广拉格朗日为：

$$\begin{array}{rl}{\mathcal{L}}_A& ={\mathcal{L}}_N\left({\mathbf{x}}_N\right)+\sum _{k=0}^{N-1}{\mathcal{L}}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\\ {\mathcal{L}}_N& \left({\mathbf{x}}_N\right)={\bar{\ell }}^f\left({\mathbf{x}}_N\right)+{\left({\lambda }_N+\frac{\mu }{2}{\overline{\mathbf{g}}}^f\left({\mathbf{x}}_N\right)\odot {\beta }_N\right)}^{\top }{\overline{\mathbf{g}}}^f\left({\mathbf{x}}_N\right)\\ {\mathcal{L}}_k& \left({\mathbf{x}}_k,{\mathbf{u}}_k\right)=\bar{\ell }\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+{\left({\lambda }_k+\frac{\mu }{2}\overline{\mathbf{g}}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\odot {\beta }_k\right)}^{\top }\overline{\mathbf{g}}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\end{array}\text{\hspace{1em}(19)}$$

其中${\bar{\ell }}^f:{\mathbb{R}}^{2n}\mapsto \mathbb{R}$和$\bar{\ell }:{\mathbb{R}}^{2n}\times {\mathbb{R}}^m\mapsto \mathbb{R}$分别为扰动系统动力学的终端成本和运行成本函数，此类函数的典型设计为二次型：

$$\begin{array}{c}{\bar{\ell }}^f(\mathbf{x})=\frac{1}{2}\Vert \delta \mathbf{x}{\Vert }_{S_V}\\ \bar{\ell }(\mathbf{x},\mathbf{u})=\frac{1}{2}\Vert \delta \mathbf{x}{\Vert }_{S_Q}+\frac{1}{2}\Vert \delta \mathbf{u}{\Vert }_{S_U}\end{array}\text{\hspace{1em}(20)}$$

其中$S_V\in {\mathbb{R}}^{2n\times 2n},S_Q\in {\mathbb{R}}^{2n\times 2n}$和$S_U\in {\mathbb{R}}^{m\times m}$为用户指定的常数成本矩阵。

在(19)中，${\overline{\mathbf{g}}}^f$和$\overline{\mathbf{g}}$为(16)中引入的扰动状态动力学终端状态和运行状态-输入的$p$个约束向量（即局部切空间中表示的约束），$\odot$为元素wise乘法算子，${\lambda }_k\in {\mathbb{R}}^p$为拉格朗日乘子向量（对偶变量），$\mu \in \mathbb{R}$为惩罚参数，${\beta }_k\in {\mathbb{R}}^p$为指示活动约束的二元控制向量，定义为：

$${\beta }_k[i]=\{\begin{array}{ll}0& \text{if}{\overline{\mathbf{g}}}_k[i]<0\text{and}{\lambda }_k[i]\sim 0,\\ 1& \text{otherwise}\end{array}\text{\hspace{1em}(21)}$$

这样，若对应对偶变量接近零，满足的约束不会因惩罚量产生额外成本。一般而言，惩罚参数$\mu$可随时间变化，但为简单起见，我们在同一迭代的每个时间步保持其恒定。

增广拉格朗日方法包括外层循环：对固定$\lambda$和$\mu$参数形成组合成本函数(19)，将问题(8)转化为无约束问题，用内层求解器（如DDP/iLQR）求解。外层循环的每一步，按如下方式应用递增惩罚：

$$\begin{array}{rl}{\lambda }_k^{+}& =\max \left(0,{\lambda }_k+\mu {\overline{\mathbf{g}}}_k\left({\mathbf{x}}_k^{\ast },{\mathbf{u}}_k^{\ast }\right)\right)\forall k\in \{0,\dots ,N-1\}\\ {\mu }^{+}& =\gamma \mu ,\gamma >0\end{array}\text{\hspace{1em}(22)}$$

其中$\left\{\left({\mathbf{x}}_0^{\ast },{\mathbf{u}}_0^{\ast }\right),\dots ,\left({\mathbf{x}}_{N-1}^{\ast },{\mathbf{u}}_{N-1}^{\ast }\right)\right\}$为从内层求解器获得的最优状态和输入轨迹，$\gamma$为固定惩罚更新率。若迭代次数小于指定最大值且约束未在期望容差阈值内满足，外层循环通常继续。约束满足性通常通过以下方式测试：

$$\begin{array}{rl}\text{MCV}& =\underset{k=0,\dots ,N-1}{\max }\left(\max \left(0,{\overline{\mathbf{g}}}_k\left({\mathbf{x}}_k^{\ast },{\mathbf{u}}_k^{\ast }\right)\right)\right)\\ \text{MCD}& =\underset{k=0,\dots ,N-1}{\max }\left(\left|{\overline{\mathbf{g}}}_k\left({\mathbf{x}}_k^{\ast },{\mathbf{u}}_k^{\ast }\right)\odot {\lambda }_k\right|\right)\end{array}\text{\hspace{1em}(23)}$$

其中若MCV（最大约束违反）或MCD（最大互补对偶）高于小阈值${\epsilon }_c$，则需要进一步改善约束满足性，外层循环继续：

$$\text{ConsViolCheck}=\{\begin{array}{ll}1& \text{if}\mathrm{MCV}>{\epsilon }_c\vee \mathrm{MCD}>{\epsilon }_c,\\ 0& \text{otherwise}\end{array}\text{\hspace{1em}(24)}$$

整个算法如算法1所述。

5.4 微分动态规划

给定外层循环的增广拉格朗日函数(19)，定义值函数和动作值函数$V$和$Q$为：

$$\begin{array}{rl}{V}_k\left({\mathbf{x}}_k\right)& =\underset{{\mathbf{u}}_k}{\min }\left\{{\mathcal{L}}_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\right\}+V_{k+1}\left({\mathbf{A}}_k{\mathbf{x}}_k+B_k{\mathbf{u}}_k\right)\\ & =\underset{{\mathbf{u}}_k}{\min }Q\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\end{array}\text{\hspace{1em}(25)}$$

矩阵${\mathbf{A}}_k$和$B_k$表示(15)式中${\mathbf{A}}_t$和$B_t$的离散形式。值函数的二阶泰勒展开可写为：

$$\delta V_k(\mathbf{x})\approx \frac{1}{2}\delta {\mathbf{x}}_k^{\top }{V}_{\mathbf{x}\mathbf{x},k}\delta {\mathbf{x}}_k+V_{\mathbf{x},k}^{\top }\delta {\mathbf{x}}_k\text{\hspace{1em}(26)}$$

其中$V_{\mathbf{x}\mathbf{x},k}$和$V_{\mathbf{x},k}$分别为时间步$k$的值函数的海森矩阵和梯度。(25)中定义的动作值函数也可近似为二次函数：

$$\begin{array}{rl}Q(\mathbf{x}+\delta \mathbf{x},\mathbf{u}+\delta \mathbf{u})& \approx Q(\mathbf{x},\mathbf{u})+Q_{\mathbf{x}}^{\top }\delta \mathbf{x}+Q_{\mathbf{u}}^{\top }\delta \mathbf{u}\\ & +\frac{1}{2}\left(\delta {\mathbf{x}}^{\top }{Q}_{\mathbf{x}\mathbf{x}}\delta \mathbf{x}+\delta {\mathbf{u}}^{\top }{Q}_{\mathbf{u}\mathbf{u}}\delta \mathbf{u}\right)+\delta {\mathbf{u}}^{\top }{Q}_{\mathbf{u}\mathbf{x}}\delta \mathbf{x}\end{array}\text{\hspace{1em}(27)}$$

为计算(27)中的导数矩阵：

$$\begin{array}{rl}{Q}_{\mathbf{x}}& ={\mathcal{L}}_{\mathbf{x}}+{\mathbf{A}}^{\top }{V}_{\mathbf{x}}^{\prime }\\ Q_{\mathbf{u}}& ={\mathcal{L}}_{\mathbf{u}}+{\mathbf{B}}^{\top }{V}_{\mathbf{x}}^{\prime }\\ Q_{\mathbf{x}\mathbf{x}}& ={\mathcal{L}}_{\mathbf{x}\mathbf{x}}+{\mathbf{A}}^{\top }{V}_{\mathbf{x}\mathbf{x}}^{\prime }\mathbf{A}+V_{\mathbf{x}}^{\prime }{h}_{\mathbf{x}\mathbf{x}}\\ Q_{\mathbf{u}\mathbf{u}}& ={\mathcal{L}}_{\mathbf{u}\mathbf{u}}+{\mathbf{B}}^{\top }{V}_{\mathbf{x}\mathbf{x}}^{\prime }\mathbf{B}+V_{\mathbf{x}}^{\prime }{h}_{\mathbf{u}\mathbf{u}}\\ Q_{\mathbf{u}\mathbf{x}}& ={\mathcal{L}}_{\mathbf{u}\mathbf{x}}+{\mathbf{B}}^{\top }{V}_{\mathbf{x}\mathbf{x}}^{\prime }\mathbf{A}+V_{\mathbf{x}}^{\prime }{h}_{\mathbf{u}\mathbf{x}}\end{array}\text{\hspace{1em}(28)}$$

为简化符号，所有变量的时间索引均省略，此表达式中所有变量在时间步$k$处评估，除标记${}^{\prime }$的变量在时间步$k+1$处评估。

计算状态动力学的完整二阶展开$(h_{\mathbf{x}\mathbf{x}},h_{\mathbf{u}\mathbf{u}},h_{\mathbf{u}\mathbf{x}})$计算成本高，尤其对复杂动力学和高维状态系统。此时，采用迭代LQR中的一阶近似是替代方案，虽通过提供真实海森矩阵的高斯-牛顿近似降低计算成本，但也降低了局部保真度，增加了收敛所需迭代次数。

对$\delta \mathbf{u}$最小化(27)得到仿射控制器：

$$\delta {\mathbf{u}}^{\ast }=-Q_{\mathbf{u}\mathbf{u}}^{-1}\left(Q_{\mathbf{u}\mathbf{x}}\delta \mathbf{x}+Q_{\mathbf{u}}\right)\triangleq \mathbf{K}\delta \mathbf{x}+\mathbf{d}\text{\hspace{1em}(29)}$$

在$Q_{\mathbf{u}\mathbf{u}}$秩亏时，通常正则化为$Q_{\mathbf{u}\mathbf{u}}+\rho I$，其中$\rho$为小正常数，$I$为单位矩阵。将$\delta {\mathbf{u}}^{\ast }$代入(27)，得时间步$k$的值函数导数与动作值函数导数的关系：

$$\begin{array}{rl}{V}_{\mathbf{x}}& =Q_{\mathbf{x}}+\mathbf{K}{Q}_{\mathbf{u}}+{\mathbf{K}}^{\top }{Q}_{\mathbf{u}\mathbf{u}}\mathbf{d}+Q_{\mathbf{u}\mathbf{x}}^{\top }\mathbf{d},\\ V_{\mathbf{x}\mathbf{x}}& =Q_{\mathbf{x}\mathbf{x}}+{\mathbf{K}}^{\top }{Q}_{\mathbf{u}\mathbf{u}}\mathbf{K}+{\mathbf{K}}^{\top }{Q}_{\mathbf{u}\mathbf{x}}+Q_{\mathbf{u}\mathbf{x}}^{\top }\mathbf{K}\end{array}\text{\hspace{1em}(30)}$$

在终端时间步，$V_{\mathbf{x}}$和$V_{\mathbf{x}\mathbf{x}}$可轻松计算为终端拉格朗日函数$({\mathcal{L}}_N)$的一阶和二阶导数。这样，动作值函数的导数(28)及每步的局部最优控制策略(29)可从终端步开始反向计算。

确定每步最优控制策略后，通过从初始状态开始在流形上正向仿真动力学更新标称轨迹：

$$\delta {\mathbf{x}}_k=\left[\begin{array}{c}\mathrm{logm}\left({\mathcal{X}}_k^{-1}{\overline{\mathcal{X}}}_k\right)\\ {\overline{\xi }}_k-{\xi }_k\end{array}\right]\text{\hspace{1em}(31)}$$ $$\begin{array}{c}\delta {\mathbf{u}}_k={\mathbf{K}}_k\delta {\mathbf{x}}_k+\alpha {\mathbf{d}}_k\\ {\overline{\mathbf{u}}}_k={\mathbf{u}}_k+\delta {\mathbf{u}}_k\end{array}\text{\hspace{1em}(32)}$$ $$\begin{array}{rl}{\overline{\xi }}_{k+1}& ={\overline{\xi }}_k+f\left(\xi ,{\overline{\mathbf{u}}}_k\right)\Delta t\\ {\overline{\mathcal{X}}}_{k+1}& ={\overline{\mathcal{X}}}_k\exp \mathrm{m}\left({\overline{\xi }}_{k+1}\Delta t\right)\end{array}\text{\hspace{1em}(33)}$$

其中$\{{\mathcal{X}}_k,{\xi }_k,{\mathbf{u}}_k\}$和$\{{\overline{\mathcal{X}}}_k,{\overline{\xi }}_k,{\overline{\mathbf{u}}}_k\}$分别表示时间步$k$的标称状态-动作和更新后的状态-动作，(32)中$\alpha$为前馈项简单线性搜索的缩放项。

DDP/iLQR方法的收敛高度依赖搜索方向和步长[31]，可分别通过$\rho$和$\alpha$正则化。尽管对此类约束非线性优化问题，确定最佳搜索方向和步长无证明的最优方法，但有渐进增加正则化和采用Armijo线搜索策略等有效实践[29, 32]。

本文在反向传播阶段(算法2)通过变化$\rho$并行生成多个控制器：

$$\begin{array}{c}{\mathbf{K}}^i=-{\left(Q_{\mathbf{u}\mathbf{u}}+{\rho }_{i}I\right)}^{-1}{Q}_{\mathbf{u}\mathbf{x}}\\ {\mathbf{d}}^i=-{\left(Q_{\mathbf{u}\mathbf{u}}+{\rho }_{i}I\right)}^{-1}{Q}_{\mathbf{u}}\end{array}\text{\hspace{1em}(34)}$$

其中从集合$\mathcal{P}$中选择${\rho }_i$。$\rho$小时，正则化项影响小，控制更新类似无正则化的激进更新，严重依赖$Q_{\mathbf{u}\mathbf{u}}$信息；$\rho$大时，正则化项主导，控制更新更保守，确保更好数值稳定性但收敛更慢。此行为类似Levenberg-Marquardt方法，随正则化增加从高斯-牛顿法过渡到梯度下降。

一旦在反向传播中获得从激进到保守的多个控制器，在正向传播中用每个控制器和$\alpha =1$闭环展开轨迹并计算成本(算法3)。若成本低于前次迭代，取成本最小的轨迹作为下次DDP迭代的标称轨迹。

5.5 实现实践

所提方法使用Python的jax库[33]实现，因其易于并行化和自动微分。扰动状态动力学的雅可比和海森函数，以及拉格朗日函数的一阶和二阶导数，均通过jax的自动微分获得。初始惩罚参数$\mu$决定算法从无约束(小$\mu$)还是约束可行(大$\mu$)开始，通常设置$\mu =1$并迭代乘以$\gamma =10$达到平衡。约束满足容差阈值$({\epsilon }_c)$设为$1\mathrm{e}-3$。为反向传播中变化正则化，使用$n_{\rho }=7$和$\mathcal{P}=\{{\rho }_i=10^{i-4}|i=0,\dots ,n_{\rho }\}$，生成从激进(${\rho }_0=1\mathrm{e}-4$)到保守(${\rho }_7=1000$)的变化搜索方向和控制更新。

6 实验

本文使用所提方法优化状态位于矩阵李群切丛的系统轨迹，实验1从以下方面评估方法：

• 处理SE(3)中刚体动力学特征的简单机械系统约束，
• 作为李代数反馈控制策略应用时缓解外部扰动，
• 与现有优化求解器的计算效率对比，
• 真实四旋翼场景的轨迹优化性能。

考虑SE(3)中的3D刚体，系统状态可由旋转矩阵

$$R\in \mathrm{SO}(3)\equiv \left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid R^{\top }R=I_3,\det (R)=1\right\}\text{\hspace{1em}(35)}$$

和位置$p\in {\mathbb{R}}^3$表示。SE(3)中群元素的齐次表示为：

$$\mathcal{X}=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\in \mathrm{SE}(3)\text{\hspace{1em}(36)}$$

SE(3)中的速度向量$\xi$称为“螺旋”，由体坐标系中的角速度($\omega$)和线速度($v$)组成：

$$\xi =\left[\begin{array}{c}\omega \\ v\end{array}\right]\in {\mathbb{R}}^6,{\xi }^{\wedge }=\left[\begin{array}{cc}{\omega }^{\wedge }& v\\ 0& 0\end{array}\right]\in \mathfrak{se}(3)\text{\hspace{1em}(37)}$$

强迫欧拉-庞加莱方程[28]定义螺旋动力学为：

$$J_b\dot{\xi }={\mathrm{ad}}_{\xi }^{\ast }{J}_b\xi +u\text{\hspace{1em}(38)}$$

其中$J_b$为体固定主轴的广义惯性矩阵，$u\in {\mathfrak{g}}^{\ast }$为广义控制输入（包括作用于这些轴的力矩和力$u_{\tau }$和$u_f$），${\mathfrak{g}}^{\ast }$表示余切空间，余伴随映射表示为${\mathrm{ad}}_{\xi }^{\ast }$。李代数中伴随作用(${\mathrm{ad}}_{\xi }$)和余伴随映射(${\mathrm{ad}}_{\xi }^{\ast }$)的矩阵表示为：

$${\mathrm{ad}}_{\xi }=\left[\begin{array}{cc}{\omega }^{\wedge }& 0\\ v^{\wedge }& {\omega }^{\wedge }\end{array}\right],{\mathrm{ad}}_{\xi }^{\ast }={\mathrm{ad}}_{\xi }^{\top }=-\left[\begin{array}{cc}{\omega }^{\wedge }& v^{\wedge }\\ 0& {\omega }^{\wedge }\end{array}\right]\text{\hspace{1em}(39)}$$

最后，SE(3)群的连续运动方程(7)可写为：

$$\begin{array}{c}\dot{\mathcal{X}}=\mathcal{X}{\xi }^{\wedge },\\ \dot{\xi }=J_b^{-1}\left({\mathrm{ad}}_{\xi }^{\ast }{J}_b\xi +u\right)\end{array}\text{\hspace{1em}(40)}$$

6.1.1 约束处理

测试所提算法为SE(3)中刚体规划安全路径，任务是在6秒内将刚体从单位位形旋转至$R_z(170^{\circ })$，从初始位置$(2,2,2)$平移至$(6,2,2)$，固定时间步长$\Delta t=0.1$。体固定坐标系$x$、$y$和$z$轴的旋转分别记为$R_x(\cdot )$、$R_y(\cdot )$和$R_z(\cdot )$。

运动中，位形$R_z(90^{\circ })$视为不安全需避开，同时存在位于$(4,2,2)$、半径1的球形障碍也需避开。所有输入限制在-7.5和7.5之间，当位置$x$小于3时角速度限制为0.5，否则上限为2.0。这些约束对所有离散步$k$公式化为：

$$\begin{array}{rl}\text{障碍物避障 (Obstacle Avoid):}& \Vert p_k-[4,2,2{]}^{\top }{\Vert }_2\ge 1.0\\ \text{位形禁飞区 (Conf. Avoid):}& \Vert \mathrm{logm}(R_z(90^{\circ }{)}^{-1}{R}_k){\Vert }_2\ge 0.4\\ \text{执行器幅值 (Input Limits):}& -7.5\le u_k\le 7.5\\ \text{角速度约束 (Velocity Bound):}& {\omega }_k\le {\bar{\omega }}_k,\text{其中 }{\bar{\omega }}_k=\{\begin{array}{ll}0.5,& x_k\le 3\\ 2.0,& \text{其他}\end{array}\end{array}\text{\hspace{1em}(41)}$$

其中位形避障约束中的logm运算按(17)式用其二阶近似实现。

位形轨迹通过$R_x(\cdot )R_y(\cdot )R_z(\cdot )$旋转顺序转换为欧拉角($\varphi ,\theta ,\psi$，单位：度)。在结果轨迹中，观察到绕$x$和$y$轴的旋转($\varphi$和$\theta$角变化)避开了$R_x(90^{\circ })$的不安全位形(图2)。考虑位形约束时，机器人沿不同轴改变运动以绕过不安全方向。球形障碍的存在要求更迂回的路径，沿各轴偏离以实现无碰撞轨迹(图2)。

图2：所提约束DDP方法有效优化了SE(3)对象的轨迹，同时考虑了位置、方向、速度和输入的约束。此外，角速度${\omega }_z$在2-2.5秒间饱和至0.5，满足位置相关的速度约束。

图3：在所提DDP方法中实现李代数反馈控制，在受到随机扰动时，显着降低了状态方差，特别是在目标点附近。

6.1.2 扰动抑制

为评估所提DDP方法处理外部扰动的有效性，将[12]的分析扩展至SE(3)，并将所提DDP方法用作李代数反馈控制：

$$u_k^{fb}:=u_k^{\ast }+K_k\mathrm{logm}\left({\left({\mathcal{X}}_k^{\ast }\right)}^{-1}{\mathcal{X}}_k^{ϵ}\right)\text{\hspace{1em}(42)}$$

其中$u_k^{\ast }$和$K_k$分别为通过DDP获得的（次）最优控制序列和时变反馈增益，(29)中的前馈项$d_k$未在反馈策略(42)中显式显示，因其已包含在$u_k^{\ast }$中。变量${\mathcal{X}}_k^{\ast }$和${\mathcal{X}}_k^{ϵ}$分别表示通过DDP获得的（次）最优状态和扰动引起的扰动状态。

在SE(3)动力学的随机版本上测试策略(42)：

$$\begin{array}{rl}{\mathcal{X}}_{k+1}^{ϵ}& ={\mathcal{X}}_k^{ϵ}\exp {\mathrm{m}}_{\mathrm{SE}(3)}\left({\xi }_k^{ϵ}\Delta t\right)\\ {\xi }_{k+1}^{ϵ}& ={\xi }_k^{ϵ}+f\left({\xi }_k^{ϵ},u_k\right)\Delta t+{\sigma }_w\mathbf{w}\end{array}\text{\hspace{1em}(43)}$$

其中$\mathbf{w}$假设为空间不相关独立同分布噪声，取自零均值高斯分布$\mathbf{w}\sim \mathcal{N}\left(0_{6\times 1},I_6\right)$，${\sigma }_w=0.001$。通过让优化器在确定性系统上收敛，然后在随机系统上测试性能，评估所提DDP方法在随机条件下的表现。图3显示了此对比结果，使用噪声动力学下的1000条采样轨迹，比较开环策略$u_k^{\ast }$与反馈策略$u_k^{fb}$(42)。结果表明，如预期，所获得的反馈增益显著降低了状态方差，尤其在目标点附近。

图4：标准约束IPOPT、SQP与所提约束DDP方法的输入收敛性比较，展示了本文方法实现的显著更快的收敛。

6.1.3 计算效率

求解离散最优控制问题的常用方法是利用现成优化求解器，稍作修改即可应用，将系统动力学作为等式约束，状态和输入约束作为不等式约束，生成可行轨迹。

为建立基准对比，我们使用Python优化器，包括SciPy优化模块的序列最小二乘规划(SLSQP)算法[34]和内点优化器(IPOPT)[35]，对6.1.1节描述的任务用这些方法测试。为加速收敛，提供了成本函数以及等式和不等式约束的导数信息。

图4展示了标准约束IPOPT、SQP与所提约束DDP方法的收敛对比。DDP在少于40次迭代内收敛，而IPOPT和SQP分别需要125和250次迭代。差异的主要原因是直接优化方法随horizon扩展增加决策变量数量，导致在有众多等式和不等式约束的空间中搜索。

6.2 四旋翼轨迹优化

考虑四旋翼动力学作为3D刚体，系统状态由旋转矩阵$R\in \mathrm{SO}(3)$、位置向量$p\in {\mathbb{R}}^3$、角速度向量$\omega \in {\mathbb{R}}^3$和线速度向量$v\in {\mathbb{R}}^3$表示[36]。不同于SE(3)动力学，四旋翼是欠驱动机器人系统，即缺乏直接控制所有六自由度的广义控制输入，而是由四个电机驱动，假设电机速度(${\Phi }_i,i=0,1,2,3$)可近瞬时控制[37]，产生力矩和力如下：

$$\begin{array}{c}{\tau }_{\text{in}}=\left[\begin{array}{c}\left({\Phi }_1^2-{\Phi }_3^2\right)k_{F}L\\ \left({\Phi }_2^2-{\Phi }_0^2\right)k_{F}L\\ {\sum }_{i=0}^3(-1{)}^{i+1}{k}_T{\Phi }_i^2\end{array}\right]\\ F_{\text{in}}=R\cdot \left[0,0,k_F\sum _{i=0}^3{\Phi }_i^2\right]\end{array}\text{\hspace{1em}(44)}$$

其中$L$为臂长，$k_F,k_T$为常数。此处考虑${}^{+}$构型四旋翼，其$x$和$y$轴与电机位置对齐，(44)中诱导的力矩和力因此在全局框架中产生线加速度，在局部框架中产生角速度变化：

$$\begin{array}{c}\dot{v}=m^{-1}\left(F_{\text{in}}-[0,0,mg]\right)\\ \dot{\omega }=J^{-1}\left({\tau }_{\text{in}}-\omega \times (J\omega )\right)\end{array}\text{\hspace{1em}(45)}$$

其中$m,g$和$J$分别为质量、重力加速度和惯性。最后，四旋翼在SE(3)中的位置和方向动力学为：

$$\dot{p}=v,\dot{R}=R{\omega }^{\wedge }\text{\hspace{1em}(46)}$$

测试所提方法优化此四旋翼动力学的安全轨迹，任务(见图5)是在4秒内从初始位置$(3,0.25,2)$移动至$(2,5,2)$，固定时间步长$\Delta t=0.02$。两个尺寸(1.5, 0.1, 3.0)的矩形盒位于$(0,2.5,3.0)$和(3.6, 2.5, 3.0)，需避开。除障碍避障外，纳入位形到达约束测试万向节锁，强制机器人在通过盒子间时实现$R_y(90)$。此外，由于执行器特性，旋翼速度限制在0和最大rpm之间。

图5：所提方法成功为非线性四旋翼动力学优化轨迹，在避开障碍物且不发生万向节死锁的情况下，实现绕 y 轴纯 90 度旋转。

图6显示了使用所提方法的结果轨迹，机器人成功实现$R_y(90)$旋转($\theta$角)，在1.2-1.9秒间无输入时未出现万向节锁。尽管机器人初始和目标z位置相同，它在轨迹开始时增加z位置，防止因零输入期坠落(图6)。

图6：所提方法展示的四旋翼优化轨迹，表现出绕 y 轴平滑的 90 度旋转，同时保持稳定性并避开障碍物。

7 结论

矩阵李群上的控制策略优化是控制与机器人领域的重要挑战，具有众多实际应用。本文提出一种新颖方法，利用基于增广拉格朗日的约束离散DDP算法解决此问题。方法包括：在反向传播阶段将优化问题提升至李代数，便于在对应切空间计算目标和约束函数的梯度；正向传播阶段通过积分反向传播获得的最优策略动力学，缩回至流形。

本文的关键贡献是开发了在DDP框架中处理广泛矩阵李群非线性约束的通用方法，超越了限于特定矩阵李群的以往DDP方法，提供了更通用的解决方案。此外，与现有直接优化方法相比，所提方法计算效率更高，因内在纳入动力学并利用二阶信息实现更准确更新，收敛更快、迭代更少。我们还通过作为SE(3)上李代数反馈控制策略的应用，展示了方法处理外部扰动的有效性，结果表明方法能有效处理约束，并在外部扰动下保持稳定性。

本文工作的局限之一是假设受控系统的确定性转换动力学已知，未来研究可通过在矩阵李群表示中纳入这些动力学的学习来解决。此外，将动态模型和测量的不确定性纳入规划，以及研究类似[26]中欧几里得模型的闭环不确定性传播，可显著增强所提方法的鲁棒性，进一步使方法在复杂、动态和未知环境中更有效。

---

8 复现验证与工程实战总结

为了严谨评估原论文方法的算法正确性与工程实用价值，我们在 Python + JAX 环境下对 Alcan 2023 进行了端到端的结构化复现与实验基准测试。以下是核心复现结论与工程洞见：

8.1 算法实现完整性与流形几何保真度

• 流形正交性严格保持：通过李群指数映射 ${\mathcal{X}}_{k+1}={\mathcal{X}}_k\exp ({\xi }_{k+1}\Delta t)$ 进行前向积分，在整个 6 秒仿真时域（60 个控制步）内，旋转矩阵的 $\mathrm{SO}(3)$ 正交性残差均保持在 $\Vert R^{T}R-I\Vert \le 2.8\times 10^{-15}$，行列式残差 $|\det (R)-1|\le 2.3\times 10^{-15}$，彻底杜绝了欧氏积分器带来的数值奇异与流形漂移。
• 并行正则化机制：利用 JAX vmap 并行计算 $\rho \in [10^{-4},10^3]$ 8 组候选控制器并并行回滚，在兼顾积极收敛步与保守安全步的同时，完全消除了传统单步回溯线搜索对串行回滚的依赖。

8.2 论文式(17)矩阵对数二阶近似的关键缺陷发现

论文在第 5.2 节式 (17) 建议使用矩阵对数的二阶泰勒展开近似：

$${\psi }^c\approx {\left(({\Psi }^c-I)-0.5({\Psi }^c-I{)}^2\right)}^{\vee }$$

用于位形避障约束 $\Vert {\psi }^c\Vert \ge r_c$（实验中 $r_c=0.4$）。 我们在消融实验中发现了该近似的关键安全隐患：

• 在二阶对数近似模式（paper_second_order）下，优化器最终报告的约束距离为 $0.4002\ge 0.40$，增广拉格朗日外循环判定约束已满足；
• 然而，当我们使用真实的李代数测地线对数映射（exact $\mathrm{logm}$）重新度量该轨迹时，真实物理距离仅为 $0.3813<0.40$，刚体实际上已经侵入了禁飞姿态锥！
• 工程建议：在大角度姿态机动场景下，严禁使用低阶截断近似替代真实黎曼对数映射，必须直接采用数值稳定且带有零点光滑分支保护的精确李代数映射。

8.3 一阶 Lie-iLQR vs 全二阶 Full-DDP 对比

• 一阶 Lie-iLQR (Gauss-Newton)：忽略动力学海森张量收缩项 $V_x^{\prime }\cdot h_{xx},V_x^{\prime }\cdot h_{uu},V_x^{\prime }\cdot h_{ux}$，单次迭代无需计算高阶动力学自动微分，32 次内循环迭代耗时约 27.9s，最终目标代价值为 $1.547759$。
• 全二阶 Full-DDP：完整计算三项张量收缩，内循环迭代 36 次，耗时约 41.3s，最终代价值略降至 $1.547306$。
• 结论：对于全驱动刚体动力学，一阶 Lie-iLQR 在计算速度与收敛精度之间达到了极佳平衡，是实时 MPC 部署的首选方案。

8.4 独立单打靶求解器基线对比 (DDP vs SLSQP vs IPOPT)

在完全相同的动力学模型、目标函数、初始控制序列和约束条件下，我们构建了独立的 SciPy SLSQP 与 CasADi IPOPT 单打靶基线：

• Lie-DDP：仅需 36 次迭代即达到 $10^{-5}$ 量级约束容差并成功收敛；
• SLSQP：迭代达到设定的 250 次上限仍未收敛（约束违规残差停滞在 $10^{-8}$ 但梯度未能清零）；
• IPOPT：迭代达到 125 次上限未收敛。 这充分印证了 DDP 通过动态规划逆向分解消除等式动力学约束、从而大幅降低决策变量搜索空间的理论优势。

8.5 欠驱动四旋翼任务收敛边界与工程启示

对于第 6.2 节的四旋翼挑战任务（$N=200$，欠驱动六自由度系统）：

• 在固定前向步长 $\alpha =1.0$ 的条件下，四旋翼在 8 阶超椭球箱体避障、电机转速上限（900 rad/s）与地面防撞上均实现 100% 满足；
• 1.2~1.9s 之间的 $R_y(90^{\circ })$ 姿态翻转窗口实际达到约 $8.30^{\circ }$ 精度（距预设 $8.0^{\circ }$ 阈值最大超限仅 $0.00515$ rad，约 $0.29^{\circ }$）；
• 当外循环惩罚因子 $\mu$ 达到 $10^6$ 以上时，欠驱动系统海森矩阵条件数恶化，固定 $\alpha =1.0$ 步长难以在离散正则化集合中找到持续递减方向。在工程实际中，建议结合自适应 Armijo 步长缩放或二次规划活跃集求解以实现深层残差压制。

8.6 欧氏空间对照基准 (Howell 2019 ALTRO 与 Alcan 2022)

针对读者关心的“增广拉格朗日约束离散 DDP 在欧氏空间对应的奠基之作”，我们在此系统整理其跨空间算法对比与数学映射：

维度 / 算法特性	欧式空间代表作：Howell et al. 2019 (ALTRO / AL-iLQR)	矩阵李群流形本篇：Alcan et al. 2023 (Lie-DDP)
状态空间表征	$x\in {\mathbb{R}}^n,u\in {\mathbb{R}}^m$ (连续欧氏平直空间)	$\mathcal{X}\in \mathrm{SE}(3),\xi \in {\mathbb{R}}^6,u\in {\mathbb{R}}^6$ (矩阵李群切丛)
外环增广拉格朗日函数	${\mathcal{L}}_A=\ell (x,u)+{\lambda }^{T}c(x,u)+\frac{\mu }{2}c(x,u{)}^T{I}_{\mu }c(x,u)$	式 (19)：${\mathcal{L}}_A=\ell (x,u)+(\lambda +\frac{\mu }{2}\overline{g}\odot \beta {)}^T\overline{g}$
乘子一阶更新律	${\lambda }^{+}=\max (0,\lambda +\mu c(x,u))$	式 (22)：${\lambda }^{+}=\max (0,\lambda +\mu \overline{g}(x,u))$ (完全一致)
罚因子单调递增律	${\mu }^{+}=\gamma \mu (\gamma >1)$	式 (22)：${\mu }^{+}=\gamma \mu (\gamma >1)$ (完全一致)
外环收敛停机条件	$\max (c)<ϵ$ 且互补松弛残差 $<ϵ$	式 (23)：$\mathrm{MCV}<{ϵ}_c$ 且 $\mathrm{MCD}<{ϵ}_c$ (完全一致)
后向传播 (Backward Pass)	直接在 ${\mathbb{R}}^n$ 中计算对欧氏状态的雅可比 $A_k,B_k$ 与海森矩阵	Lifting (提拉)：通过对数映射 $\psi =\mathrm{logm}({\mathcal{X}}^{-1}\bar{\mathcal{X}}{)}^{\vee }$ 提拉到切空间，利用伴随矩阵 ${\mathrm{ad}}_{\xi }$ 计算切空间动力学
前向仿真 (Forward Pass)	欧氏向量简单加法：$x_{k+1}=x_k+f(x_k,u_k)\Delta t$	Retraction (缩回)：利用群指数映射 ${\mathcal{X}}_{k+1}={\mathcal{X}}_k\mathrm{expm}({\xi }_{k+1}^{\wedge }\Delta t)$ 积分，严格保证流形正交性
姿态与位形约束处理	采用欧拉角（存在万向节死锁奇异性）或单位四元数（须额外施加 $\Vert q\Vert =1$ 约束）	直接在李代数测地线空间定义超球约束：$\Vert \mathrm{logm}(R_c^{-1}R){\Vert }_2\ge r_c$，无奇点、无冗余约束
工程实现源码对照	本地路径：Howell_2019_ALTRO/src/solver.py	本地路径：Alcan_2023_DDP_Constraints/src/solver.py

由此可见，Alcan 2023 与 Howell 2019 (ALTRO) 在外环增广拉格朗日乘子更新体系上是完全等价的理论孪生；其最核心的突破与创新，就在于打通了非平直矩阵李群流形与欧氏切空间之间的提拉（Lifting）与缩回（Retraction）桥梁。

---

8.7 理论深度辨析：“提升 (Lifting)”与“缩回 (Retraction)”的严格公理体系

为了让读者在理论研究和控制工程实践中彻底廓清李群与李代数之间的映射公理，我们在此归纳该对偶体系在现代几何控制论中的公理化定义：

       局部李代数切空间 g ≅ Rⁿ  [平直向量空间: 满足向量加法, 可做二阶泰勒展开与 LQR]
              ▲                             │
              │  提升 (Lifting)             │  缩回 (Retraction)
              │  逆映射/对数映射 log(·)      │  指数映射 exp(·)
              │                             ▼
       底流形: 矩阵李群 G (如 SE(3)) [非线性弯曲流形: 无向量加法, 存在非欧曲率]

1. 公理 1 (Retraction 缩回)： 设 $\mathcal{M}$ 为光滑流形，$T_x\mathcal{M}$ 为点 $x$ 处的切空间。映射 $R_x:T_x\mathcal{M}\to \mathcal{M}$ 称为流形上的缩回，若满足：

   • $R_x(0_x)=x$（零切向量映射回原切点）；
   • 微分映射导数为单位算子：$\mathrm{D}{R}_x(0_x)={\mathrm{id}}_{T_x\mathcal{M}}$（局部一阶保真性）。 对于矩阵李群，群指数映射 ${\mathrm{Exp}}_x(\xi )=x\mathrm{expm}({\xi }^{\wedge })$ 是天然且最标准的 Retraction。

2. 公理 2 (Lifting 提升 / 提拉)： 在局部邻域内，Retraction 的光滑局部逆映射 $R_x^{-1}={\mathrm{Log}}_x:U_x\to T_x\mathcal{M}$ 称为局部提升映射。 通过该映射，定义在弯曲流形上的非凸目标函数与约束泛函被“提升”为切空间上的复合函数： ${\hat{\mathcal{L}}}_A(\delta x,\delta u)={\mathcal{L}}_A\circ R_{\bar{x}}(\delta x,\delta u)$ 所有经典的贝尔曼逆向传播展开、高斯-牛顿近似、Riccati 递推求解，本质上都是在该提升后的线性切空间中严谨进行的。

---

8.8 计算效率理论推导：DDP 与直接法的 $\mathcal{O}(\cdot )$ 复杂度分析

针对读者关心的“计算效率量化评测与 $\mathcal{O}(\cdot )$ 复杂度推导”，我们在此系统整理其数值代数机理：

1. 渐进代数复杂度推导对比

设时域步数为 $N$，状态空间维度为 $n$，控制输入维度为 $m$（通常 $N\gg n\ge m$）：

• 微分动态规划 (DDP / iLQR)： DDP 利用贝尔曼最优性原理，将全局最优化问题通过时间逆向分解为 $N$ 个顺序单步二次子问题。 在每一个离散时间节点 $k$，求解控制更新方程： $\delta u_k^{\ast }=-Q_{uu,k}^{-1}(Q_{ux,k}\delta x_k+Q_{u,k})$ 求逆算子的维度严格限定在控制维度 $m\times m$（在刚体动力学中 $m=6$，在四旋翼中仅为 $m=4$），与离散时间步长 $N$ 完全无关！

  • 单步 Cholesky 分解开销：$\frac{1}{3}{m}^3$；
  • 伴随雅可比与值函数海森传递矩阵乘法：$\mathcal{O}(m^{2}n+m{n}^2+n^3)$；
  • 整条轨迹总代数复杂度： ${\mathcal{O}}_{\text{DDP}}=\mathcal{O}(N\cdot (m^3+m{n}^2+n^3\left)\right)\approx \mathcal{O}(\mathbf{N}\cdot {\mathbf{n}}^3)$ 其关于时间视界 $N$ 是严格一阶线性的。

• 直接优化法 (Direct Methods: 全离散化 NLP + SQP / 内点法 IPOPT)： 直接法将所有时刻的状态和输入作为未知决策变量堆叠： $Z=[x_0,u_0,x_1,u_1,\dots ,x_N{]}^{\top }\in {\mathbb{R}}^{N(n+m)+n}$ 并将 $N\times n$ 维动力学方程作为等式约束，形成庞大的稀疏非线性规划（NLP）。

  • 稠密线性求解器：全时域 KKT 矩阵直接因式分解的复杂度高达 $\mathcal{O}({\mathbf{N}}^3(\mathbf{n}+\mathbf{m}{)}^3)$，随时间步长呈三次方爆炸；
  • 带稀疏块三对角分解（如 IPOPT + MA27/MUMPS）：利用时间局部稀疏性后，线性求解器理论下限为 $\mathcal{O}(\mathbf{N}(\mathbf{n}+\mathbf{m}{)}^3)$。但每个时间节点求逆分解的局部矩阵块维度为 $(n+m)$。当 $n=12,m=6$ 时，$(18{)}^3=5832$，而 DDP 仅为 $6^3=216$，局部浮点运算常数项相差 27 倍之巨！

2. 工业界量化轨迹优化求解器效率的 5 大维度

量化评估维度	指标含义与定义	DDP / iLQR 表现	直接优化求解器 (SQP / IPOPT) 表现
1. 渐进时间复杂度	随时间视界 $N$ 扩展的运算量增长阶数	严格 $\mathcal{O}(N)$	稀疏结构 $\mathcal{O}(N)$，稠密结构 $\mathcal{O}(N^3)$
2. 单步代数规模 (FLOPs)	核心求逆矩阵尺寸与 CPU L1 缓存亲和性	极小 ($m\times m$)，常驻 CPU 缓存，零额外内存搬运	庞大稀疏结构，频繁动态寻址与非连续内存访问
3. 收敛所需迭代次数	达到约束容差 $\epsilon \le 10^{-3}$ 所需的总迭代数	极少（通常 $<40$ 次迭代）	较多（通常 $100\sim 300$ 次迭代）
4. 求解挂钟时间 (Latency)	达到收敛的实际物理运行耗时（P95 / P99 延迟）	毫秒级 ($10\text{Hz}\sim 100\text{Hz}$)	秒级至数十秒级
5. 闭环 MPC 实时可行性	能否在控制周期 $\Delta t$（如 20ms）内完成热启动刷新	极易在单核嵌入式平台上实现实时控制	极难在单核平台上满足毫秒级硬实时要求

3. 实验数据印证（原论文图 4 与本地独立复现）

• 收敛迭代次数对比：在论文 6.1.3 节实验中，DDP 在第 36 次迭代即达到最优性与约束容差，而 IPOPT 需要 125 次迭代，SLSQP 需要 250 次迭代（见论文图 4 输入范数收敛平稳曲线）；
• 动力学可行性保持：DDP 在每一次迭代回滚（Rollout）时都是通过非线性真实动力学进行积分前向仿真，中间轨迹始终保持动力学 100% 可行；而直接法在收敛前动力学等式约束均存在缺陷违规，一旦未完全收敛，中间轨迹无法直接输入物理系统执行。

---

8.9 控制理论深度审视：鲁棒性 vs 稳定性学术辨析与认证体系

针对读者精读提出的“鲁棒性与稳定性是否存在混用、Monte Carlo 实验与理论认证有效性”，我们在控制理论学术严谨性视角下展开深度审视：

1. 严格控制论边界：标称稳定性 vs 鲁棒稳定性 vs 鲁棒约束满足

在 IEEE TAC、Automatica 等控制理论顶刊中，三者具有不可混淆的数理定义：

核心控制属性	严格数学定义与收敛保证	本文 Alcan 2023 实际现状	审稿人视角的真实结论
标称渐进稳定性 (Nominal Stability)	针对确定性系统：存在离散李雅普诺夫函数 $V$，满足 $\Delta V(x)\le -\alpha (\Vert x\Vert )$，状态渐进收敛到零平衡点。	仅在 §6.1.1 给出确定性无扰动仿真曲线，未推导任何李雅普诺夫降落证明，未分析反向闭环极点配置。	仿真收敛不等同于理论稳定；数值收敛是必要条件而非充分证明。
鲁棒稳定性 (Robust Stability / ISS)	针对持续非零外部扰动 $w_k\in \mathcal{W}$：证明系统为输入-状态稳定 (ISS) 或最终一致有界 (UUB)：$\Vert x_k\Vert \le \beta (\Vert x_0\Vert ,k)+\gamma (\Vert w{\Vert }_{\infty })$。	作者在摘要中声称“面对外部扰动仍能保持稳定性”，但在持续扰动下不可能渐进稳定到原点。	属于工程语意混用；应严格表述为“经验性方差抑制”或“有界扰动下的轨迹邻域跟踪”。
鲁棒约束满足 (Robust Constraint)	在任何扰动下，系统状态与输入必须 100% 严格落在安全硬约束集内（$\forall w\in \mathcal{W},x_k\in \mathcal{X},u_k\in \mathcal{U}$）。	反馈策略式 (42) 直接叠加线性增益 $u_k^{fb}=u_k^{\ast }+K_k\log (\cdot )$，未做执行器饱和截断投影，未对几何障碍物做紧缩。	无法保证鲁棒约束满足；一旦出现推向障碍物的扰动，物理系统将直接撞障。

2. 原文 Monte Carlo 实验的深度复盘与三大局限

论文在第 6.1.2 节确实针对 SE(3) 刚体开展了 1000 次蒙特卡洛随机采样（图 3），对比了开环策略 $u_k^{\ast }$ 与李代数反馈策略 $u_k^{fb}$。然而从工业落地与控制理论审查来看，该实验存在三大局限：

1. 扰动量级极弱：高斯白噪声标准差仅为 ${\sigma }_w=0.001$。在实际航天器或无人机飞行中，气动力矩、推力不对准或风切变扰动通常比这大数个数量级，千分之一标准差更接近于浮点运算的数值微扰；
2. 缺乏物理参数不确定性：未开展质量 $m$（如 $\pm 10\%$）与转动惯量矩阵 $J$（如 $\pm 20\%$）的模型参数摄动压力测试；
3. 未统计约束违规率：测试仅绘制了终点位姿的散布方差椭球，未给出“撞障概率”或“控制超限比例”的量化置信区间。

3. 严格鲁棒 MPC 必须具备的五大理论闭环证据链 (Claim Gates)

若要在学术界严格声称“鲁棒稳定与安全约束保证”，必须构建以下五大闭环认证（本文 Alcan 2023 全部缺失，但在高级鲁棒控制如 Tube-NMPC 中为标配）：

证据链层级	理论定义与数学判据	闭环认证功能与 Alcan 2023 现状
1. 递归可行性 (Recursive Feasibility)	$\exists u_{0:N-1}^{\ast }(x_k)⟹\exists {\tilde{u}}_{0:N-1}(x_{k+1})\in {\mathcal{U}}_{\text{feas}}$	核心功能：保证在任意未知有界扰动下，下一步永远存在可行安全控制律，系统绝不陷入求解器死锁。 ❌ Alcan 2023 未证明
2. 离散李雅普诺夫降落 (Lyapunov Decrease)	$V_N^{\ast }(x_{k+1})-V_N^{\ast }(x_k)\le -\ell (x_k,u_k^{\ast })+\gamma (w_k)$	核心功能：构造最优成本作为控制 Lyapunov 函数，严格保证闭环系统渐进稳定或输入-状态稳定 (ISS)。 ❌ Alcan 2023 未构建
3. 终端不变集与局部控制律 (Terminal Set & Controller)	$\forall x\in {\Omega }_{ϵ}⟹f(x,{\kappa }_f(x))\in {\Omega }_{ϵ},{\kappa }_f(x)\in \mathcal{U}$	核心功能：求解离散代数 Riccati 方程 (DARE) 获得局部稳定反馈 $K_f$ 与高维椭球不变吸引域 ${\Omega }_{ϵ}$。 ❌ Alcan 2023 仅有终端权重，无终端集
4. 鲁棒正不变管 (RPI Tube Invariance)	$e_{k+1}=(A-B{K}_v)e_k+w_k⟹e_k\in \mathcal{Z},\forall k$	核心功能：在扰动紧致集 $w_k\in \mathbb{W}$ 下，构建真实状态围绕标称轨迹的紧致不变误差管道。 ❌ Alcan 2023 无误差管设计
5. 几何约束紧缩 (Pontryagin Difference)	$\bar{\mathcal{X}}=\mathcal{X}\ominus \mathcal{Z},\bar{\mathcal{U}}=\mathcal{U}\ominus K_v\mathcal{Z}$	核心功能：标称规划在收缩后的几何集合内优化，保证真实轨迹在扰动下 100% 绝不突破原始物理与安全硬约束。 ❌ Alcan 2023 采用硬约束直接求解

4. 总结与工程启发

Alcan 2023 是一篇在流形数值轨迹优化计算机制上极具突破性的工程论文，它以优异的阶段解耦实现了 $<40$ 次迭代的超高计算效率；但文中所述的“稳定性”和“鲁棒性”属于机器人实验领域的经验定性描述。在航天器交会对接或高可靠性自主无人系统研发中，必须进一步结合 非线性扰动观测器 (NDOB) 实时在线补偿未知力矩，并配合 几何 Tube-NMPC 约束紧缩，才能真正提供具备数学证明的严格鲁棒安全保障。

---

8.10 姿态运动学深度溯源：万向节锁推导与全参数化缺陷对比

针对读者精读时提出的“万向节锁严格数学实例、四元数解绕与 MRPs 影子集切换”，我们在此进行全景式的姿态动力学数理剖析：

1. 3D 万向节锁 (Gimbal Lock) 动态几何机制

💡 3D 运转动态演示：已在前文 图 1.1 万向节锁 3D 运转动图 中可视化直观呈现，可点击直接跳转回放。

如图 1.1 所示，三环万向节系统由三个嵌套的正交旋转环构成：

• 外环（红色）：绕世界系 $Z$ 轴转动（偏航 Yaw，$\psi$）；
• 中环（绿色）：绕机体系 $Y$ 轴转动（俯仰 Pitch，$\theta$）；
• 内环（蓝色）：绕机体系 $X$ 轴转动（滚转 Roll，$\varphi$）。 当俯仰角 $\theta \to 90^{\circ }$ 时，绿色环旋转带动内层蓝色环立起，使得蓝色环的转轴与外层红色环的转轴在三维空间中完全重合共线！此时转动红色环与转动蓝色环的效果完全相同，物体失去了绕第三根独立正交轴旋转的能力。

2. $Z-Y-X$ 欧拉角锁死的数学推导

设旋转复合顺序为：偏航 $\psi$（绕 $Z$）、俯仰 $\theta$（绕 $Y$）、滚转 $\varphi$（绕 $X$）。总体旋转矩阵为：

$$R(\psi ,\theta ,\varphi )=R_z(\psi )R_y(\theta )R_x(\varphi )=\left[\begin{array}{ccc}\cos \psi \cos \theta & \cos \psi \sin \theta \sin \varphi -\sin \psi \cos \varphi & \cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \\ \sin \psi \cos \theta & \sin \psi \sin \theta \sin \varphi +\cos \psi \cos \varphi & \sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right]$$

• 旋转矩阵代数退化：当飞机直冲云霄机头朝天（$\theta =90^{\circ }=\frac{\pi }{2}$）时，$\cos \theta =0,\sin \theta =1$。利用三角和差化积化简可得：

  $$R\left(\psi ,\frac{\pi }{2},\varphi \right)=\left[\begin{array}{ccc}0& \sin (\psi -\varphi )& \cos (\psi -\varphi )\\ 0& \cos (\psi -\varphi )& -\sin (\psi -\varphi )\\ -1& 0& 0\end{array}\right]$$

  此时整个姿态矩阵的所有元素仅取决于单一标量组合 $(\psi -\varphi )$！增加偏航角与反向减少滚转角在物理上完全等效，无法独立控制。

• 速度级运动学雅可比奇异降秩：机体角速度与欧拉角变化率之间的转换关系为：

  $${\omega }_b=J(\theta ,\varphi )\left[\begin{array}{c}\dot{\psi }\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]=\left[\begin{array}{ccc}-\sin \theta & 0& 1\\ \cos \theta \sin \varphi & \cos \varphi & 0\\ \cos \theta \cos \varphi & -\sin \varphi & 0\end{array}\right]\left[\begin{array}{c}\dot{\psi }\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]$$

  其行列式为： $\det (J(\theta ,\varphi ))=-\cos \theta$ 当 $\theta \to 90^{\circ }$ 时，$\det (J)=0⟹\mathrm{rank}(J)=2<3$。反求角速率时出现除以零奇异：

  $$\left[\begin{array}{c}\dot{\psi }\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]=\frac{1}{\cos \theta }\left[\begin{array}{ccc}0& \sin \varphi & \cos \varphi \\ 0& \cos \varphi \cos \theta & -\sin \varphi \cos \theta \\ \cos \theta & \sin \varphi \sin \theta & \cos \varphi \sin \theta \end{array}\right]{\omega }_b$$

  物理空间中任何微小的横向角速度 ${\omega }_y\ne 0$，都会导致计算出的欧拉角速率 $\dot{\psi },\dot{\varphi }\to \pm \infty$ 趋向无穷大，飞控算法直接由于浮点数溢出崩溃。

3. 其他常用三维姿态表示的“拓扑诅咒”

根据微分拓扑中的 Stuelpnagel 定理 (1964)，三维旋转群 $\mathrm{SO}(3)$ 在微分流形上同胚于实射影空间 ${\mathbb{RP}}^3$。不存在任何定义在三维欧氏空间中全局连续且无奇异的姿态坐标系！

1. 单位四元数 ($S^3$ 双覆盖与 Unwinding 解绕灾难)：

   • 4 维单位四元数满足 $q\in S^3$。其对 $\mathrm{SO}(3)$ 是 $2:1$ 的双覆盖（Double Cover），即 $q$ 与 $-q$ 代表完全相同的物理旋转；
   • 解绕问题 (Unwinding Phenomenon)：若定义误差四元数 $q_e=q_d^{-1}\otimes q$，控制器期望收敛到 $q_e=[\pm 1,0,0,0{]}^{\top }$。若刚体当前姿态距目标仅差 $5^{\circ }$，但符号处于 $-q_d$ 分支，未加严密内积符号判断的控制器会将误差误判为“相差 $355^{\circ }$ 的巨幅偏差”，从而驱使刚体在空间中凭空翻滚一整圈 ($360^{\circ }$) 再回到原位！

2. 经典罗德里格斯参数 (Gibbs Vector $\mathbf{\rho }=\mathbf{n}\tan (\theta /2)$)：

   • 极小 3 维表示，但在旋转角度达到 $\theta =180^{\circ }$ 时，$\tan (\pi /2)\to \infty$ 发生直接除以零数值崩溃，完全无法胜任半周以上大角度旋转。

3. 修正罗德里格斯参数 (MRPs $\mathbf{\sigma }=\mathbf{n}\tan (\theta /4)$ 与影子集跳变)：

   • 极小 3 维表示，将奇异点推迟至 $\theta =360^{\circ }$；
   • 为避免 $360^{\circ }$ 奇点，工程上规定当 $\Vert \mathbf{\sigma }\Vert >1$（旋转角 $>180^{\circ }$）时必须离散切换到影子集 (Shadow Set)： ${\mathbf{\sigma }}^S=-\frac{\mathbf{\sigma }}{\Vert \mathbf{\sigma }{\Vert }^2}$
   • 优化致命伤：影子集的切换属于非光滑的离散跳变（Hybrid Jump），彻底破坏了目标函数在切换面上的连续可微性。自动微分引擎（Autodiff / JAX）在切换边界处导数不存在，DDP / iLQR 赖以生存的连续二阶海森矩阵 $Q_{xx}$ 在此剧烈震荡跳变，极易导致优化求解器瞬间发散！

4. 全姿态参数化横向终极对比矩阵

姿态参数化方式	空间与参数维度	奇点发生位置	代数约束	核心缺陷与工程痛点
欧拉角 (Euler Angles)	${\mathbb{R}}^3$ (极小化)	中间轴 $\pm 90^{\circ }$	无	• 万向节死锁：失去一个自由度； • 雅可比矩阵降秩奇异，角速度反求发散到 $\infty$。
经典罗德里格斯 (Gibbs)	${\mathbb{R}}^3$ (极小化)	转角 $\pm 180^{\circ }$	无	• $\tan (\theta /2)$ 在半周翻转时直接除以零数值崩溃，无法处理大角度旋转。
修正罗德里格斯 (MRPs)	${\mathbb{R}}^3$ (极小化)	转角 $\pm 360^{\circ }$	无	• 必须引入 影子集离散跳变，彻底破坏平滑性与自动微分 (Autodiff)，DDP 优化极易发散。
单位四元数 (Quaternions)	$S^3\subset {\mathbb{R}}^4$ (过参数化)	全局无奇点	$\Vert q{\Vert }_2=1$	• 双覆盖 ($q\equiv -q$)； • 解绕问题 (Unwinding)：导致系统多翻滚大圈 $360^{\circ }$； • 优化须增加非凸等式硬约束。
矩阵李群 $\mathrm{SO}(3)/\mathrm{SE}(3)$ ⭐ 本篇采用	内蕴群流形	全局无奇点	内蕴保正交群结构	• 拓扑最完美：无双覆盖、无解绕大翻滚、无影子集离散跳变； • 配合局部李代数切空间提升 (Lifting) + 缩回 (Retraction)，兼具全局无奇点与平直欧氏计算的高效性！

---

8.11 经典力学概念溯源：完整系统 vs 非完整系统与空间交会对接动力学本质

针对读者精读文献 [3]（A. M. Bloch, Nonholonomic Mechanics and Control）时提出的“Nonholonomic 含义、机械臂与空间交会对接系统属性界定”，我们在此系统梳理分析力学与控制论的分类本质：

1. 完整约束 (Holonomic) vs 非完整约束 (Nonholonomic)

约束维度	完整约束 (Holonomic Constraints)	非完整约束 (Nonholonomic Constraints)
数学方程形式	纯位形坐标与时间的代数方程： $f(q_1,q_2,\dots ,q_n,t)=0$	无法积分为纯位置关系的速度微分方程（Pfaffian 约束）： $A(q)\dot{q}=0(\text{不可积，即不存在 }\nabla h=A)$
几何物理图像	系统被刚性“捆绑”在位形空间中的某个低维流形曲面上活动。	系统在某一瞬时拥有“不可行进的速度方向”，但在全局位形空间中所有位姿皆可达！
位形自由度影响	真实减少自由度：$n$ 个坐标受 $k$ 个独立约束，系统有效自由度降为 $n-k$。	不减少全局位形自由度：系统依然可以在完整的 $n$ 维流形空间中机动。
日常经典范例	• 单摆受绳长约束在球面上； • 轨道列车受限于铁轨； • 固定基座串联机械臂。	• 汽车/独轮车的无侧滑纯滚动（小车不能横向平移，但通过倒车揉库仍能停入任意车位）； • 冰刀在冰面滑行； • 自由落体中无推力翻转的猫。

2. 机械臂到底是不是 Nonholonomic 系统？

明确结论：常规固定基座工业机械臂是标准的“完整系统（Holonomic System）”。

• 正运动学直接代数确定：设机械臂各个关节的角度为 $q=[{\theta }_1,{\theta }_2,\dots ,{\theta }_n{]}^{\top }$。末端执行器（End-Effector）在三维空间中的位姿 $T_{ee}\in \mathrm{SE}(3)$ 与关节角之间存在完全确定的正向运动学代数方程： $T_{ee}=f({\theta }_1,{\theta }_2,\dots ,{\theta }_n)$
• 无不可积速度约束：每个关节均由独立电机直接驱动，任何关节运动都可以独立积分，不存在类似“轮式侧滑”的速度级非完整约束。
• 特例延伸：
  • 若将机械臂固定在两轮差速或阿克曼移动小车底盘上（移动机械臂 Mobile Manipulator），移动底盘的无侧滑约束会使整个系统变为非完整系统；
  • 若机械臂的某些关节无电机驱动（被动自由摆动关节，Underactuated Manipulator），角动量守恒的不可积性会呈现非完整力学特性。

3. 空间交会对接 (Chu et al., 2024 / 2026) 是不是 Holonomic 系统？

明确结论：我们论文中所研究的空间交会对接系统是标准的“完整系统（Holonomic System）”，且是“全驱动系统（Fully Actuated System）”！

1. 执行机构配置与控制输入维度：
   在 Chu et al. 的系统架构中，追踪飞行器配置了完备的 6-DoF 执行机构，控制输入向量为： $u=[{\tau }^{\top },f^{\top }{]}^{\top }\in {\mathbb{R}}^6$

   • 姿态执行机构（反作用飞轮 / 姿控喷气）：提供三轴独立力矩 $\tau =[{\tau }_x,{\tau }_y,{\tau }_z{]}^{\top }\in {\mathbb{R}}^3$，直接控制 Roll, Pitch, Yaw；
   • 轨道执行机构（反作用控制系统 RCS 推进器）：提供三轴独立推力 $f=[f_x,f_y,f_z{]}^{\top }\in {\mathbb{R}}^3$，直接控制空间三维平移。 飞行器在太空中处于微重力真空环境，不存在任何地面车辆“不能横向侧滑”的速度限制，推进器可以随时随地向任意方向输出合力与合力矩。

2. 为什么航天器经常与“Nonholonomic”联系在一起？
   在航天动力学中，有一种特殊的航天器属于经典的非完整系统——“零角动量无外推力漂移卫星”：

   • 当航天器发动机熄火、无向外喷气推力时，系统总角动量守恒： $I(q)\omega +\sum h_{\text{wheel}}=0$
   • 这个角动量守恒方程是一阶速度微分关系，但由于李代数非对易性（Lie Bracket $[f_i,f_j]\ne 0$），该方程在数学上不可积为纯姿态角的代数方程！宇航员在太空中扑腾四肢、猫咪在空中翻身（Falling Cat Problem），本质都是通过肢体形状改变转动惯量矩阵实现无力矩姿态重构，这正是 Bloch 专著 [3] 重点讨论的非完整几何相位（Geometric Phase / Holonomy）问题。

3. Chu et al. 论文的核心科学挑战究竟是什么？
   既然我们的交会对接飞行器是完整全驱动系统，那么本论文的攻坚核心就不在于“非完整速度约束规划”，而在于：

   • 流形非欧拓扑与运动耦合（$\mathrm{SE}(3)$ Lie Group Manifold）：大范围姿态旋转与相对平移运动紧密非线性耦合，传统欧氏解耦控制易引发奇异或姿态漂移；
   • 非合作目标高动态翻滚（Non-cooperative Tumbling Target）：目标卫星在故障状态下以高达 $0.5\text{rad/s}$ 速度翻滚，相对动力学产生强烈的科氏力加速度耦合项；
   • 严苛的安全走廊硬约束（Line-of-Sight Docking Corridor & Speed Envelope）：对接机构视场对准锥、接近速度刹车包络线等高维非线性状态与输入约束的在线实时求解与递归可行性证明！

---

8.12 最优化与控制理论辨析：次优解、流形非凸性与 NMPC 有限滚动时域本质

针对读者精读时提出的“局部最优解能否称为次优解、优化简化问题算不算次优、无法求得全局最优是否因约束惩罚引起、以及 NMPC 有限滚动时域与次优性的关系”，我们在此系统廓清最优化与现代控制理论中的关键边界：

1. 严格辨析：“全局最优解”、“局部最优解”与“次优解”

在数学规划与最优控制领域，这三者具有严谨的集合隶属关系：

概念分类	数学定义与几何直觉	在工程系统中的判定
全局最优解 (Globally Optimal)	在所有满足物理硬约束的可行解集合 ${\mathcal{X}}_{\text{feas}}$ 中，目标代价值绝对最低： $\forall u\in {\mathcal{U}}_{\text{feas}},J(u^{\ast })\le J(u)$	在高维非凸问题中，除非使用耗时极长、指数级复杂度的全局分支定界（Branch-and-Bound）或凸松弛（Convex Relaxation），否则极难解析求得。
局部最优解 (Locally Optimal)	仅在当前解的一个微小开球邻域 $B_{ϵ}(\hat{u})$ 内成本最低，满足一阶 KKT 驻点条件与二阶充分条件： $\forall u\in {\mathcal{U}}_{\text{feas}}\cap B_{ϵ}(\hat{u}),J(\hat{u})\le J(u)$	梯度下降法、牛顿法、SQP、DDP / iLQR 等基于导数的局部优化器收敛到的终点。
次优解 (Suboptimal Solution)	广义定义：任何严格满足所有物理与安全硬约束（Primal Feasible 可行解），但其目标代价未能达到全局绝对最低点（$J(\tilde{u})>J(u^{\ast })$）的解。 狭义/算法控制论定义：在求解器尚未完全收敛到 KKT 容差前**提前终止（Early Termination / RTI）**所输出的可行解。	在全局尺度下，所有的“局部最优解”本质上都属于“次优解”！ 此外，求解简化/松弛模型（如丢弃二阶张量项、凸松弛）得到的解也是原物理问题的次优解。

2. Alcan 2023 无法保证“全局最优”的真实原因是什么？

核心结论：绝不是因为引入了“增广拉格朗日约束惩罚”，其根本罪魁祸首是“流形与动力学的本质非凸性（Non-convexity）”！

1. 即使是纯无约束系统，流形优化也是极度非凸的：

   • 刚体的位形空间位于非欧紧流形 $\mathrm{SE}(3)$ 上，具有内在测地线曲率；
   • 相对运动学 ${\dot{g}}_e=g_e{\xi }_e^{\wedge }$ 包含矩阵乘法非线性，欧拉-庞加莱动力学 $\mathbb{I}\dot{\xi }={\mathrm{ad}}_{\xi }^{\ast }(\mathbb{I}\xi )+u$ 包含角速度二次张量耦合；
   • 目标代价值函数的高维曲面充满极其复杂的局部极小值山谷（Local Minima）、鞍点（Saddle Points）与狭长弯曲峡谷。DDP / iLQR 作为二阶局部梯度下降法，本质上就像一颗顺着重力滚动的弹珠，从初始标称轨迹滚入哪一个山谷，就只能收敛到哪一个山谷底部的局部极小值，它根本没有跨越崇山峻岭全局跳跃的机制。

2. 增广拉格朗日法在理论上是精确的（Exact Penalty via Multipliers）：

   • 简单的外罚函数法（Penalty Method）由于惩罚权重 $\mu \to \infty$ 会引发严重的 Hessian 条件数病态，且破坏最优性；
   • 但增广拉格朗日法（Augmented Lagrangian）通过引入对偶乘子 $\lambda$ 迭代估计，在乘子更新收敛时，其解严格精确地满足原始有约束 NLP 的一阶 KKT 极值条件与二阶互补松弛性！
   • 因此，增广拉格朗日完全继承了原问题的局部最优性，并未人为制造次优解；无法获得全局最优完全源自物理问题本身的非凸性。

3. NMPC 求解中“次优解”的双重本质根源

在非线性模型预测控制（NMPC）中，所谓“次优性”是由时间维度与空间维度两个完全独立的理论机制共同造成的：

🧩 NMPC 次优性 (Suboptimality) 的双重理论根源解构

理论维度	产生机理与工程表现	现代控制理论闭环应对方案
【维度一：时间截断次优】 (Horizon Truncation) Receding Horizon	• 终极理论目标：无限时域贝尔曼最优 ($N\to \infty$) • NMPC 现实局限：每次仅预测优化未来有限步 ($N<\infty$，如 3 秒) • 短视次优本质：即便单次时域内求得绝对全局最优，跨越时域仍属贪心截断	🛡️ 终端代价与终端不变集： 通过构造终端控制 Lyapunov 函数 $V_f(x_N)$ 与控制正不变集 ${\Omega }_{ϵ}$，在数学上解析吸收未来 $[N,\infty )$ 剩余截断误差，确立闭环渐进稳定证据链！
【维度二：在线计算受限次优】 (Computation Limitation) Suboptimal MPC / RTI	• 实时性严苛：控制周期极短（如 $10\sim 50\text{ms}$），求解器来不及收敛至 KKT 梯度容差 $<10^{-6}$ 便提前输出 • 空间非凸性：$\mathrm{SE}(3)$ 流形曲率与障碍物导致多局部极小值山谷，DDP/SQP 仅收敛到局部谷底	🛡️ 递归可行性与降落证书： 经典理论（Scokaert & Mayne 1999, Diehl 2005 RTI）严格证明：只要当前次优解满足物理硬约束且保证 Lyapunov 函数单调下降，闭环系统依然 100% 保证渐进稳定！

1. 维度一：有限时域滚动截断带来的“短视次优性” (Receding Horizon Suboptimality)：

   • 贝尔曼动态规划（Bellman’s Principle of Optimality）的绝对全局最优，是指无限时域总代价最小： $J_{\infty }^{\ast }(x_0)={\min }_{u_{[0,\infty )}}{\int }_0^{\infty }\ell (x(t),u(t))dt$
   • 但现实中算力有限，NMPC 必须截断为有限预测时域（例如 $N=30$ 步，仅看未来 3 秒）；
   • 哪怕你在当前这 3 秒内用超级计算机算出了这 3 秒时间内的“绝对全局最优解”，放在整个任务周期（$0$ 到 $\infty$）的宏观尺度下，它依然是次优的！ 因为系统具有“短视性（Myopic Planning）”，它看不到 3 秒之后潜伏的巨大障碍物或动力学突变；
   • （控制理论闭环）：现代严格稳定 NMPC（如 Mayne 2000 经典框架）之所以必须煞费苦心地设计终端惩罚代价 $V_f(x_N)$ 与终端控制不变集 ${\Omega }_{ϵ}$，本质就是为了用解析方法严密吸收掉未来 $[N,\infty )$ 剩余截断误差，从而在有限滚动时域下提供无限时域的渐进稳定性闭环证明！

2. 维度二：在线时间受限导致的“求解器提前终止次优性” (Suboptimal MPC)：

   • 在高动态刚体飞行或四旋翼机动中，控制频率高达 $50\text{Hz}\sim 100\text{Hz}$，单步计算时间预算仅有 $10\text{ms}\sim 20\text{ms}$；
   • 在这极短的时间窗口内，数值求解器（无论是 IPOPT 内点法还是 DDP）往往只来得及进行 $1\sim 3$ 次迭代，远远无法达到梯度残差 $<10^{-6}$ 的完全收敛标准；
   • 现代控制理论大师（Scokaert & Mayne 1999 年《Suboptimal Model Predictive Control (Feasibility and Stability)》以及 Moritz Diehl 提出的 Real-Time Iteration (RTI) 方案）已经给出了决定性的理论证明：在闭环控制中，完全不需要每次在线求解都收敛到局部最优极值点！只要当前输出的控制量满足系统物理与防碰撞硬约束（递归可行性），并且其对应的值函数满足离散李雅普诺夫单调递减条件，该次优解依然能 100% 严格保证闭环系统的全局或局部渐进稳定性！

---

8.13 学术源流考据：密歇根学派 (Teng [13, 14]) 与阿尔托学派 (Alcan [26]) 的理论接力与创新分工

针对读者在 5.1 节切空间动力学中提出的敏锐疑问，本节对 Teng 2022 [13, 14] 与 Alcan 2023 [26] 的学术渊源与理论分工进行深度考据：

1. 5.1 节切空间动力学与 Teng 2022 [13] 的数学同构验证

在 Alcan 2023 的第 5.1 节中，作者在正文第二段写道：

“To obtain the perturbed state dynamics, we adopt the approach of Teng et al. [13]. For completeness, we outline the necessary steps here, and the interested reader is referred to [13, 14].”

我们对比两篇论文的原始推导：

• Teng et al. 2022 [13] (IROS 2022) 式 (12)-(15)： 定义位形误差：$\Psi =X_d^{-1}X$； 微分运动学：$\dot{\Psi }=\Psi (\xi -{\mathrm{Ad}}_{{\Psi }^{-1}}{\xi }_d{)}^{\wedge }$； 在李代数切空间代入一阶近似 $\Psi \approx I+{\psi }^{\wedge }$ 展开，得到： $\dot{\psi }=-{\mathrm{ad}}_{{\xi }_d}\psi +(\xi -{\xi }_d)$
• Alcan et al. 2023 [26] (IEEE T-RO) 式 (9)-(11)： 定义位形误差：$\Psi ={\mathcal{X}}^{-1}{\mathcal{X}}_p$； 时间求导展开：$\dot{\Psi }=-{\xi }^{\wedge }\Psi +\Psi {\xi }_p^{\wedge }$； 利用 $\Psi =\mathrm{expm}({\psi }^{\wedge })\approx I+{\psi }^{\wedge }$ 展开，得到： $\dot{\psi }=\delta \xi -{\mathrm{ad}}_{\xi }\psi$

考据判定：两者的代数结构是完全同构的（Teng 用伴随运输矩阵，Alcan 用群乘积微分，二者在李代数上等价）。Alcan 严格遵守了学术引用规范，明确指出了这是 Teng 的方法。因此，连通矩阵李群切代数误差状态微分方程的原创版权属于 Teng 团队。

2. Teng 2022 姊妹篇 [13] 与 [14] 的精妙分工

读者提问：“[14] 也是这么推导的吗？”——答案是：[14] 不推导动力学，它专攻代价函数与曲率性质！

论文标识与出处	核心研究课题与理论突破	在 Alcan 2023 框架中的映射
[13] Teng et al. (IROS 2022)	连通李群误差状态动力学与 MPC： 首次推导了切空间误差方程 $\dot{\psi }=\delta \xi -{\mathrm{ad}}_{\xi }\psi$，并用于 MIT Mini Cheetah 四足机器人的姿态平衡控制。	映射到 第 5.1 节：提供 DDP 反向传播所需的一阶系统雅可比矩阵 $A_t,B_t$。
[14] Teng et al. (CDC 2022)	李代数对数代价函数设计与几何曲率： 系统证明了在李代数切空间定义二次代价 $\frac{1}{2}\Vert \mathrm{Log}(g){\Vert }_Q^2$ 比传统矩阵迹代价拥有更广的全局收敛域，能导出二次 Lyapunov 指数收敛，并初步构建了无约束流形 iLQR。	映射到 第 5.2 节：提供 DDP 阶段代价与终端代价的二阶切空间展开 Hessian 矩阵 $Q_t,R_t$。

Alcan 将 [13] 和 [14] 并列引用，是因为 DDP 算法的反向传播既需要动力学一阶/二阶导数（来自 [13]），也需要代价函数的一阶/二阶导数（来自 [14]）。

3. 团队学术谱系与“接力式创新”真相

维度	美国密歇根大学安娜堡分校 (UMich) 学派	芬兰阿尔托大学 (Aalto University) 学派
代表人物	Sangli Teng (滕桑立)、Maani Ghaffari (导师) Anthony Bloch (几何力学泰斗)、Ram Vasudevan	Gökhan Alcan、Fares J. Abu-Dakka Ville Kyrki (机器人学教授，IRG 负责人)
核心学术风格	偏向数理公理化、微分几何基础与严格 Lyapunov 控制证明	偏向机器人运动规划求解器、数值优化算法与真实硬件实验
历史贡献局限	Teng 2022 奠定了切代数展开理论，但完全没有处理非线性状态硬约束（无避障、无视场锥）	Alcan 2023 引入增广拉格朗日攻克了流形硬约束，但完全缺失扰动观测与鲁棒控制闭环认证

经全面文献考据，Alcan 与 Teng 之间没有任何共同挂名论文（No Co-authorship），不存在直接学术交集。
这是学术界最典型的良性接力式创新：

1. 密歇根学派 (Teng 2022)：打造出了轻盈优美的切代数骨架；
2. 阿尔托学派 (Alcan 2023)：发现机器人真实场景最怕撞障，于是把骨架装进了增广拉格朗日框架，让 DDP 能够处理任意状态硬约束；
3. 本课题论文 (Chu et al. 2024 / 2026)：进一步指出了 Alcan 在空间高动态非合作交会对接中的致命短板（无 NDOB 扰动补偿、无双刚体翻滚牵连项、无 Tube 鲁棒不变性证明），完成了从“经验数值算法”到“严格稳定鲁棒几何控制”的第三次理论跃迁！

---

8.14 流形算法深层辨析：Hessian 复杂性、非对称阶次近似与 LtD vs DtL 离散化路线之争

针对读者在 5.1 节中敏锐指出的“为什么位形一阶速度二阶、先线性化后离散化 (LtD) 是否会放大误差、以及与 Boutselis 2018 的对比”，我们在此结合 AstroMPC 本地工程仿真进行深层解构：

1. 为什么流形位形是一阶，欧氏速度能到二阶？

在微分几何中，对李群位形计算二阶导数的代价与欧氏空间完全不在同一个量级：

1. 流形上的二阶黎曼 Hessian 计算复杂度极高：
   对于紧致李群（如 $\mathrm{SO}(3),\mathrm{SE}(3)$），若要对位形误差求严格二阶切空间展开，必须指定对称仿射联络 $\nabla$。此时，位形二阶微扰展开不仅包含李括号自同构，还会引入指数映射右微分切映射 ${\mathrm{dexp}}_{\psi }$ 的导数项，以及无穷级数形式的 Baker-Campbell-Hausdorff (BCH) 二阶交互张量。若每一迭代步均计算流形全二阶 Hessian，单步计算耗时将激增一个数量级；
2. 欧氏速度空间的 Hessian 是平凡平坦的：
   角速度与线速度向量生活在平坦的欧氏切空间 ${\mathbb{R}}^6$ 中，其协变联络与经典普通微分重合（Christoffel 符号为 0）。因此，速度的二阶张量 $H_{{\xi }_t},H_{u_t}$ 就是经典多元微积分的对称方阵，可以通过矩阵自动微分快速获取；
3. Alcan 2023 的实际实现：
   因此，Alcan 在式 (11) 中舍弃了涉及流形与速度微扰交叉的二阶小量 ${\psi }^{\wedge }({\xi }_p^{\wedge }-{\xi }^{\wedge })=\mathcal{O}(\Vert \psi \Vert \Vert \delta \xi \Vert )$。更关键的是，在式 (15) 中，作者承认：“忽略海森矩阵，可得线性扰动状态动力学 (15)”——在真实算例和开源代码中，Alcan 连速度的二阶海森矩阵也未激活，实质上运行的是纯一阶的 Lie-iLQR！

2. 先线性化后离散化 (LtD) vs 先离散化后线性化 (DtL)

在几何最优控制领域，动力学求导存在两条互不兼容的方法论路线：

比较维度	先离散化后线性化 (DtL) (Discretize-then-Linearize)	先线性化后离散化 (LtD) (Linearize-then-Discretize)
代表文献	Boutselis & Theodorou 2018 (IEEE T-RO)	Teng 2022 (IROS) / Alcan 2023 (IEEE T-RO)
计算流程	1. 连续方程 $\dot{g}=g{\xi }^{\wedge },\dot{\xi }=f$； 2. 保流形离散化 $g_{k+1}=g_k\mathrm{expm}(\Delta t{\xi }_k^{\wedge })$； 3. 对离散保结构映射求解析/数值偏导 $A_d=\frac{\partial f_d}{\partial x_k}$。	1. 连续方程 $\dot{g}=g{\xi }^{\wedge },\dot{\xi }=f$； 2. 在连续时间下忽略二阶项求连续切空间雅可比 $A_t,B_t$； 3. 采用一阶欧拉近似 $A_d\approx I+A_t\Delta t,B_d\approx B_t\Delta t$。
数值自洽性	100% 精确代数自洽：反向传播计算的梯度严格等于当前离散优化命题的解析梯度。	存在非零交换子失配（Gradient Mismatch）：反向传播预测的线性下降方向与前向真实非线性积分存在 $\mathcal{O}(\Delta t^2)$ 漂移。
步长敏感度	对大时间步长 $\Delta t$ 极具鲁棒性，线搜索步长 $\alpha \approx 1$。	步长 $\Delta t$ 稍大（$>0.05\text{s}$）时，线搜索回退（Backtracking）次数显著增加，收敛变慢。
实现复杂度	较高（需用 BCH 公式或对李群步进算子做代数展开/自动微分）。	极低（直接套用经典连续定常系统离散化公式）。

3. AstroMPC 与 OCJax 本地工程验证证据

读者关于“LtD 会进一步增大模型误差”的敏锐直觉，在 AstroMPC 与 OCJax 本地工程中拥有无可辩驳的实测数据支撑：

• 在本地测试脚本 OCJax/example/relative_se3/torch/validate_relative_se3_linearization.py 中，针对相对 SE(3) 双刚体动力学，分别对比了：
  1. analytical_euler_linearization（对离散 Lie-Euler 映射求离散解析导数，DtL）；
  2. autodiff_linearization（通过 PyTorch / JAX 对离散步进算子求自动微分，DtL）；
  3. 连续系统线性化后零阶保持近似（LtD）；
• 实测残差结论：DtL 解析求导与自动微分之间的最大主方向误差达到了机算浮点极限 $1.55\times 10^{-13}$；而 LtD 近似与真实离散步进算子之间存在明显的 $\mathcal{O}(\Delta t^2)$ 截断漂移。
• 这也是 AstroMPC/AGENTS.md 将“严禁使用连续 CARE 替代离散真实步进 DARE”列为一票否决项的理论根由！

4. 三大代表性成果长短板互补矩阵

算法框架	离散与求导机制	二阶曲率深度	约束处理能力	扰动鲁棒闭环认证
Boutselis 2018	保结构 DtL（严谨）	完整二阶 BCH 张量展开	❌ 仅限无约束轨迹优化	❌ 零鲁棒认证
Alcan 2023	工程级 LtD（妥协）	实际上退化为一阶 iLQR	✅ 增广拉格朗日处理非线性硬约束	❌ 零鲁棒认证
Chu 2026 (本课题)	保结构 DtL / 伪谱配点	精确解析/JAX 二阶梯度	✅ 增广拉格朗日 + 视场对准锥 + 速度包络	✅ NDOB 观测器 + 几何 Tube 不变管

---

8.15 原文符号断层与严谨性捉虫：5.2 节 $\overline{\mathbf{c}}$ 符号的空降本质、提升映射机制与式 (16)/(19) 符号打架

针对读者在 5.2 节中提出的“$\overline{\mathbf{c}}(\mathbf{x}+\delta \mathbf{x},\mathbf{u}+\delta \mathbf{u})$ 是否突然出现、前文是否有交代”这一极具洞察力的质询，本节进行彻底的符号谱系考据与作者写作失误还原：

1. 前文交代考据：确实是完全空降的符号

纵观 Alcan 2023 全文的前 4 个章节：

• 在 第 4 节“问题定义”式 (8) 中，作者给出的原始优化问题约束为：

  $$\mathbf{g}({\mathcal{X}}_k,{\xi }_k,u_k)\le 0,k=0,\dots ,N-1$$

  这里的自变量明确是流形群元素 ${\mathcal{X}}_k\in \mathcal{G}$、速度向量 ${\xi }_k\in {\mathbb{R}}^n$ 与控制输入 $u_k\in {\mathbb{R}}^m$。

• 从引言到第 5.1 节，全文从未定义过任何名为 $\mathbf{c}$ 或 $\overline{\mathbf{c}}$ 的约束函数！

• 到了第 5.2 节首行，作者没有任何铺垫，直接写道：

  “In order to handle the constraints in DDP framework, we approximate the constraints up to second-order as: $\overline{\mathbf{c}}(\mathbf{x}+\delta \mathbf{x},\mathbf{u}+\delta \mathbf{u})\approx \dots$ (16)”

2. 作者内心隐性完成的“流形提升空间大挪移”

尽管作者未在正文中交代，但在几何控制数学逻辑上，作者在这里执行了以下空间变换：

1. 自变量降维至切空间误差向量：在第 5.1 节式 (14) 中，作者定义了合成误差状态 $\mathbf{x}=[{\psi }^{\top },\delta {\xi }^{\top }{]}^{\top }\in {\mathbb{R}}^{2n}$。优化问题在反向传播时不再作用于非欧流形 $\mathcal{G}$，而是作用于向量空间 ${\mathbb{R}}^{2n}$；
2. 上横线 $\overline{(\cdot )}$ 的代数含义：上横线代表**“将流形上的几何约束通过群对数映射（Logarithmic Map / Retraction）提升并投影至局部切空间后的等效欧氏约束函数”**；
3. 字母随意更换：作者从 General Inequality $\mathbf{g}$ 随手换成了 Constraint $\mathbf{c}$。

3. 铁证笔误：式 (16) 与式 (19) 的“符号打架”

最能证明作者写作仓促与符号断层的，是紧随其后的第 5.3 节。我们把式 (16)、式 (18) 和式 (19) 放在一起对比：

章节与公式编号	原文印制的公式表达式	正文中的文字声明	符号矛盾诊断
第 5.2 节 式 (16)	$\overline{\mathbf{c}}(\mathbf{x}+\delta \mathbf{x},\mathbf{u}+\delta \mathbf{u})\approx \overline{\mathbf{c}}(\mathbf{x},\mathbf{u})+\dots$	“为了在 DDP 框架中处理约束……”	首次空降引入 $\overline{\mathbf{c}}$
第 5.2 节 式 (18)	${\overline{\mathbf{c}}}^{\mathcal{X}}\left({\psi }^c\right)=\left(r_c^2-{\Vert {\psi }^c\Vert }_2\right)\le 0$	“则位形避障约束可写为……”	沿用位形约束符号 ${\overline{\mathbf{c}}}^{\mathcal{X}}$
第 5.3 节 式 (19)	${\mathcal{L}}_k=\bar{\ell }+{\left({\lambda }_k+\frac{\mu }{2}\overline{\mathbf{g}}\odot {\beta }_k\right)}^{\top }\overline{\mathbf{g}}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)$	“In (19), $\bar{\mathbf{g}}$ are vectors of $p$ constraints as introduced in (16)…”	🚨 笔误实锤：正文声称 $\overline{\mathbf{g}}$ 由式 (16) 引入，但式 (16) 分明写的是 $\overline{\mathbf{c}}$！

结论：这是多作者协作或将会议论文修改为长文期刊时，未统一符号全局检索替换留下的典型笔误。

4. 顶级期刊（TAC / Automatica）投稿时的严谨定义范式

若在 TAC 或 Automatica 投稿中出现此种符号跳跃，极易被苛刻的审稿人判定为“数学不自洽”。规范论文中必须给出严格的拉回/提升定义（Pull-back / Lifted Constraint Formulation）：

“Given the original state-input inequality constraints on the Lie manifold $\mathbf{g}(\mathcal{X},\xi ,u)\le 0$ from (8), within the tangent-space DDP backward pass around the nominal trajectory $({\bar{\mathcal{X}}}_k,{\bar{\xi }}_k,{\bar{u}}_k)$, we define the locally lifted constraint function $\overline{\mathbf{g}}:{\mathbb{R}}^{2n}\times {\mathbb{R}}^m\to {\mathbb{R}}^p$ via the exponential retraction:” $\overline{\mathbf{g}}({\mathbf{x}}_k,{\mathbf{u}}_k)\triangleq \mathbf{g}\left({\bar{\mathcal{X}}}_k\mathrm{expm}({\psi }_k^{\wedge }),{\bar{\xi }}_k+\delta {\xi }_k,{\bar{u}}_k+\delta u_k\right)\le 0$ “This explicitly bridges the global manifold formulation and the local tangent quadratic Taylor expansion.”

---

8.16 几何避障建模深层辨析：式 (17) 麦卡托对数截断的局限性、式 (18) 李代数超球禁区的量纲硬伤与空间航天任务升级范式

针对读者在 5.2 节中提出的“如何理解公式 (17) 与 (18)、以及能否直接拓展到 Chu 2026 航天器交会对接 CDDP 论文”的深刻问题，本节从微分几何数学原理与空间工程物理约束两方面进行全面解剖：

1. 公式 (17) 的数学本质：麦卡托对数截断与其工程缺陷

公式 (17) 的数学本质是实方阵对数函数的二阶麦卡托级数截断（2nd-order Mercator Series Approximation）：

$$\mathrm{logm}(A)=\ln (I+(A-I))\approx (A-I)-\frac{1}{2}(A-I{)}^2$$

在现代几何控制视角下，这种写法存在严重的技术局限：

1. 收敛域极其受限且大角度严重畸变：麦卡托级数要求矩阵谱半径 $\Vert A-I\Vert <1$。对于旋转群 $\mathrm{SO}(3)$，当旋转角 $\theta >\pi /3=60^{\circ }$ 时误差呈指数上升；当 $\theta \to 90^{\circ }$ 时（Alcan 在算例 6.1.1 中要避开的正是不安全位形 $R_z(90^{\circ })$！），二阶截断矩阵 $(R-I)-0.5(R-I{)}^2$ 甚至不再具备反对称反对易性，导致映射回切向量 ${\psi }^c$ 时产生不可忽略的切代数失真；

2. 现代求解器的落后妥协：对于旋转矩阵 $R\in \mathrm{SO}(3)$，人类早在数十年前就已经掌握了 100% 精确的闭式罗德里格斯逆公式（Rodrigues Inverse Formula）：

   $$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right),\mathrm{logm}(R{)}^{\vee }=\frac{\theta }{2\sin \theta }(R-R^{\top })$$

   在 JAX / CasADi 等现代具备计算图优化与自动微分的工具中，直接调用闭式罗德里格斯逆映射不仅精度达到双精度极限，且执行速度远快于矩阵乘方与加减。式 (17) 是作者为了适应无闭式逆公式的一般抽象李群而做的过度简化。

2. 公式 (18) 的本质：李代数超球禁区及其在 $\mathrm{SE}(3)$ 下的量纲硬伤

公式 (18) 的形式为：

$${\overline{\mathbf{c}}}^{\mathcal{X}}({\psi }^c)=\left(r_c^2-\Vert {\psi }^c{\Vert }_2\right)\le 0$$

• 数学本质：它并非三维物理空间防撞，而是**“位形/姿态禁飞区（Attitude Exclusion Zone）”**在李代数向量空间 ${\mathbb{R}}^n$ 中的超球表达。

• 在航天刚体切丛 $\mathrm{SE}(3)$ 上的致命缺陷——量纲混乱（Dimensional Inconsistency）： 在空间六自由度刚体系统中，切空间相对位形向量 ${\psi }^c\in {\mathbb{R}}^6$ 是旋转与平移的半直积拼接：

  $${\psi }^c=\left[\begin{array}{c}\eta \\ p\end{array}\right],\eta \in {\mathbb{R}}^3(\text{rad}),p\in {\mathbb{R}}^3(\text{m})$$

  如果直接计算 2-范数：

  $$\Vert {\psi }^c{\Vert }_2=\sqrt{\Vert \eta {\Vert }_{\text{rad}}^2+\Vert p{\Vert }_{\text{m}}^2}$$

  米（$\text{m}$）与弧度（$\text{rad}$）在代数上直接平方相加，物理量纲完全错误！

  • 严重后果：由于 $1\text{rad}\approx 57.3^{\circ }$ 是很大的角度，航天器微小的姿态转动（如 $30^{\circ }\approx 0.52\text{rad}$）在模长中贡献巨大，而末端对接时几厘米（$0.05\text{m}$）的真实碰撞风险反而被完全淹没！这会导致优化器判定“已经成功避障”，实际上却发生了刚体物理撞击。

3. Chu 2026 几何 CDDP 论文的正统升级范式

在空间航天器非合作交会对接中，我们坚决不照搬 Alcan 的 (17) 和 (18)，而是建立符合物理现实与空间几何规范的四大半直积解耦约束：

约束类型	物理场景与工程意义	严格数学表达（无量纲混淆）	在几何 CDDP 中的处理方案
视场进近走廊锥 (FOV Cone)	保证追踪星从对接口正前方安全锥角内接近	$p_{e,x}\ge \Vert p_e{\Vert }_2\cos {\theta }_{\text{cone}}$	纯位置平移约束，投影至切空间自动微分
航天器本体/太阳翼防撞	避开非合作翻滚目标的太阳翼障碍物	$(p_e-p_{\text{obs}}{)}^{\top }E(p_e-p_{\text{obs}})\ge 1$	纯空间位置椭球防撞，与旋转解耦
光学敏感器防灼伤约束	星敏感器/相机光轴禁止直射太阳强光源	${\mathbf{b}}_s^{\top }{R}_e{\mathbf{s}}_t\le \cos {\varphi }_{\text{safe}}$	纯姿态双球内积约束，严格保流形
近场制动速度包络线	保证推力器失效时具备足够距离刹停	$\Vert v_e{\Vert }_2\le \min \left(v_{\max },\sqrt{2a_{\text{brake}}(d-d_{\text{safe}})}\right)$	相对距离-速度动态非线性耦合约束

结论：
Alcan 2023 为我们提供了优秀的增广拉格朗日（AL）处理流形非线性硬约束的通用数值优化骨架；而在具体的约束函数设计上，Chu_2026 论文必须采用上述物理意义严密的半直积解耦约束体系，并借助 JAX / CasADi 精确解析自动微分彻底淘汰式 (17) 的麦卡托截断！

---

参考文献 {.unnumbered}

[1] M. D. Shuster, “A survey of attitude representations.” Navigation, 8(9), 439–517, 1993.

[2] E. G. Hemingway and O. M. O’Reilly, “Perspectives on Euler angle singularities, gimbal lock, and the orthogonality of applied forces and applied moments.” Multibody System Dynamics, 44, 31-–56, 2018.

[3] A.M. Bloch, Nonholonomic Mechanics and Control, Springer, 2015.

[4] K. M. Lynch and F. C. Park, Modern robotics, Cambridge University Press, 2017.

[5] F. Bullo and R. M. Murray, “Tracking for fully actuated mechanical systems: a geometric framework,” Automatica, 35(1), 17–34, 1999.

[6] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control Systems, Springer, 2019.

[7] A. M. Bloch, I. I. Hussein, M. Leok, and A. K. Sanyal, “Geometric structure-preserving optimal control of a rigid body.” Journal of Dynamical and Control Systems, 15(3), 307– 330, 2009.

[8] M. B. Kobilarov and J. E. Marsden, “Discrete Geometric Optimal Control on Lie Groups,” IEEE Transactions on Robotics, 27(4), 641–655, 2011.

[9] M. Kobilarov, M. Desbrun, J. E. Marsden, and G. S. Sukhatme, “A discrete geometric optimal control framework for systems with symmetries,” Robotics: Science and Systems, 161–168, 2008.

[10] S. Patil, J. Pan, P. Abbeel and K. Goldberg, “Planning Curvature and Torsion Constrained Ribbons in 3D With Application to Intracavitary Brachytherapy,” IEEE Transactions on Automation Science and Engineering, 12(4), 1332–1345, 2015.

[11] M. Kobilarov, “Discrete Optimal Control on Lie Groups and Applications to Robotic Vehicles,” IEEE International Conference on Robotics and Automation, 5523–5529, 2014.

[12] G.I. Boutselis, E. Theodorou, “Discrete-time differential dynamic programming on lie groups: Derivation, convergence analysis, and numerical results.” IEEE Transactions on Automatic Control, 66(10), 4636–4651, 2020.

[13] S. Teng, D. Chen, W. Clark and M. Ghaffari, “An Error-State Model Predictive Control on Connected Matrix Lie Groups for Legged Robot Control,” IEEE/RSJ International Conference on Intelligent Robots and Systems, 8850–8857, 2022.

[14] S. Teng, W. Clark, A. Bloch, R. Vasudevan and M. Ghaffari, “Lie Algebraic Cost Function Design for Control on Lie Groups,” IEEE Conference on Decision and Control, 1867– 1874, 2022.

[15] S. Liu, and D. Liu, “Discrete-Time Differential Dynamic Programming on SO(3) With Pose Constraints.” IEEE Access, 10, 112921–112933, 2022.

[16] T. Lee, “Geometric Tracking Control of the Attitude Dynamics of a Rigid Body on SO(3).” IEEE American Control Conference, 1200–1205, 2011.

[17] T. Lee, M. Leok and N. H. McClamroch, “Geometric Tracking Control of a Quadrotor UAV on SE(3),” IEEE Conference on Decision and Control, 5420–5425, 2010.

[18] A. Saccon, J. Hauser and A. P. Aguiar, “Optimal Control on Lie Groups: The Projection Operator Approach,” IEEE Transactions on Automatic Control, 58(9), 2230–2245, 2013.

[19] A. Saccon, A. P. Aguiar, A. J. H¨ausler, J. Hauser, A. M. Pascoal, “Constrained motion planning for multiple vehicles on SE(3),” IEEE Conference on Decision and Control, 5637– 5642, 2012.

[20] D. Y. Lee, R. Gupta, U. V. Kalabic, S. Di Cairano, A. M. Bloch, J. W. Cutler and I. V. Kolmanovsky, “Geometric mechanics based nonlinear model predictive spacecraft attitude control with reaction wheels,” Journal of Guidance, Control, and Dynamics, 40(2), 309–319, 2017.

[21] S. Hong, J. H. Kim and H. W. Park, “Real-Time Constrained Nonlinear Model Predictive Control on SO(3) for Dynamic Legged Locomotion,” IEEE/RSJ International Conference on Intelligent Robots and Systems, 3982–3989, 2020.

[22] U. Kalabic, R. Gupta, S. Di Cairano, A. Bloch and I. Kolmanovsky, “MPC on manifolds with an application to SE(3),” IEEE American Control Conference, 7–12, 2016.

[23] Y. Ding, A. Pandala, C. Li, Y. H. Shin and H. W. Park, “Representation-Free Model Predictive Control for Dynamic Motions in Quadrupeds,” IEEE Transactions on Robotics, 37(4), 1154–1171, 2021.

[24] M. Watterson, S. Liu, K. Sun, T. Smith and V. Kumar, “Trajectory optimization on manifolds with applications to quadrotor systems,” The International Journal of Robotics Research, 39(2-3), 303–320, 2020.

[25] D. H. Jacobson and D. Q. Mayne, “Differential Dynamic Programming.” New York, NY, USA: Elsevier, 1970.

[26] G. Alcan and V. Kyrki, “Differential Dynamic Programming With Nonlinear Safety Constraints Under System Uncertainties,” IEEE Robotics and Automation Letters, 7(2), 1760–1767, 2022.

[27] M. Kobilarov, D.N. Ta, and F. Dellaert, “Differential Dynamic Programming for Optimal Estimation,” IEEE International Conference on Robotics and Automation, 863- –869, 2015.

[28] A. Bloch, P. S. Krishnaprasad, J. E. Marsden, and T. S. Ratiu, “The Euler-Poincare´ equations and double bracket dissipation,” Communications in Mathematical Physics, 175(1), 1—42, 1996.

[29] T. A. Howell, B. E. Jackson and Z. Manchester, “ALTRO: A Fast Solver for Constrained Trajectory Optimization,” IEEE/RSJ International Conference on Intelligent Robots and Systems, 7674–7679, 2019.

[30] M. Ghaffari, R. Zhang, M. Zhu, C. E. Lin, T. Y. Lin, S. Teng and J. Song, “Progress in Symmetry Preserving Robot Perception and Control Through Geometry and Learning.” Frontiers in Robotics and AI, 9, 2022.

[31] V. Roulet, S. Srinivasa, M. Fazel, Z. Harchaoui, “Iterative linear quadratic optimization for nonlinear control: Differentiable programming algorithmic templates.” arXiv:2207.06362, 2022.

[32] Z. Xie, C. K. Liu, K. Hauser, “Differential dynamic programming with nonlinear constraints.” IEEE International Conference on Robotics and Automation, 695–702, 2017.

[33] J. Bradbury, R. Frostig, P. Hawkins, M. J. Johnson, C. Leary, D. Maclaurin, G. Necula, A. Paszke, J. VanderPlas, S. Wanderman-Milne, and Q. Zhang, “JAX: composable transformations of Python+NumPy programs.” Version 0.4.20. url: http://github.com/google/jax, 2018.

[34] D. Kraft, “A Software Package for Sequential Quadratic Programming.” Forschungsbericht- Deutsche Forschungs- und Versuchsanstalt fur Luft- und Raumfahrt, 1988.

[35] R.H.Byrd, M.E.Hribar, J.Nocedal, An interior point algorithm for large-scale nonlinear programming. SIAM Journal on Optimization, 9(4), 877-900, 1999.

[36] G. Alcan, M. Unel, “Robust hovering control of a quadrotor using acceleration feedback.” IEEE International Conference on Unmanned Aircraft Systems, 1455–1462, 2017.

[37] J. Panerati, H. Zheng, S. Zhou, J. Xu, A. Prorok, A.P. Schoellig, “Learning to fly—a gym environment with pybullet physics for reinforcement learning of multi-agent quadcopter control.” IEEE/RSJ International Conference on Intelligent Robots and Systems, 7512–7519, 2021.