非线性约束微分动态规划 (Nonlinear Constrained DDP, CDDP) 深度剖析

Differential Dynamic Programming with Nonlinear Constraints: Theory, Active-Set Sensitivities, Stage-wise QP & Benchmarks
基于 Zhaoming Xie, C. Karen Liu, Kris Hauser (ICRA 2017 / IEEE TRO) 经典论文 · 结合全库标准化符号规范与工程实践

---

💡 导语
在第一篇笔记《微分动态规划 (DDP) 全景探秘》中，我们详细推导了标准 DDP 和 iLQR 的二阶展开与 Riccati 反向传播。然而，在自动驾驶车辆避障、空间飞行器视场角安全走廊（Safe Corridors）以及无人机/双足机器人多约束穿越等严苛物理场景中，系统往往面临着极其复杂的非线性状态与控制不等式约束。
传统 DDP 无法直接处理非线性约束；对数障碍（Log-Barrier）方法在边界附近严重病态且极易陷入较差的局部极小；而直接配点法（Direct Collocation / SQP）求解单块巨型非线性规划（NLP）则随着时域长度 $N$ 出现计算开销激增，且无法直接获得反馈增益矩阵 ${\mathbf{K}}_k$。
本文以 Zhaoming Xie 等人的奠基性论文为核心，深入剖析约束微分动态规划 (Constrained DDP, CDDP)：如何在反向传播中解析引入活跃集一阶扰动灵敏度（Active-Set Sensitivity）与对偶乘子符号判定机制，并在前向传播中结合阶段二次规划（Stage-wise QP）与自适应盒式信赖域，在保持 $\mathcal{O}(N)$ 线性时间复杂度的同时，实现受限非线性轨迹的严格可行收敛。

---

---

目录

• 1. 约束最优控制的困境与 CDDP 核心动机
  • 1.1 传统约束处理机制的瓶颈对比
  • 1.2 CDDP 的核心设计哲学
• 2. 标准非线性约束轨迹优化问题表述
• 3. 活跃约束对 Bellman 局部模型的拓扑剪裁
• 4. 受约束反向传播 (Constrained Backward Pass) 严格数学推导
  • 4.1 候选活跃集选取与局部线性化
  • 4.2 阶段等式约束 QP 与 KKT 线性系统
  • 4.3 对偶乘子符号判定与非活跃约束释放机制
  • 4.4 受约束前馈步长与反馈增益闭式解
  • 4.5 受约束值函数二阶模型逆向更新
• 5. 受约束前向回放 (Constrained Forward Pass) 与信赖域管理
  • 5.1 为什么线性回放会失效？阶段 QP 的必要性
  • 5.2 阶段 QP 形式与自适应盒式信赖域
  • 5.3 不可行回退重启与单调代价接受准则
• 6. 正则化策略与动力学离散化避坑工程指南
  • 6.1 状态与控制双重 LM 阻尼正则化
  • 6.2 动力学相对度（Relative Degree）与离散化陷阱
• 7. 三大经典基准物理系统实验深度剖析
  • 7.1 二维质点避障（Point Mass Double Integrator）
  • 7.2 二维非完整约束车辆动态避障（2D Car with Kinematic Constraints）
  • 7.3 三维十二状态四旋翼飞行器（3D Quadcopter 6-DOF Dynamics）
  • 7.4 CDDP vs 对数障碍 DDP vs SNOPT/IPOPT 全景横向评测
• 8. 完整 Python / JAX 算法实战架构
• 9. 现代约束 DDP 四大流派横评与 Chu 2026 几何 CDDP 论文升级范式
• 10. 权威参考文献与延伸阅读

---

1. 约束最优控制的困境与 CDDP 核心动机

在控制与机器人学中，绝大多数真实物理系统都不可避免地带有各种约束：

1. 控制执行机构边界：电机最大扭矩、油门开度、推力非负性；
2. 状态安全走廊与几何包络：避免撞击静止或移动障碍物、保持在车道线内、飞行器视场角限制；
3. 状态-控制耦合动力学限制：摩擦锥（Friction Cone）约束、倾角与升力耦合边界。

1.1 传统约束处理机制的瓶颈对比

为了在轨迹优化中处理这些约束，学术界与工业界曾探索过数种主流路径：

约束处理范式	典型代表算法	核心数学机制	核心优势	严重物理/数值缺陷
罚函数与对数障碍法	Log-Barrier DDP, Interior Point	在目标函数中追加 $-t\sum \log (-g_i(x,u))$ 或 $\rho \Vert \max (0,g){\Vert }^2$	可直接复用无约束 DDP 求解器代码	边界处 Hessian 矩阵严重病态，极易卡在较差局部极小，中间迭代违反约束
控制受限投影法	Box-QP iLQR (Tassa 2014)	反向传递求解带边界的单步控制 QP，行截断更新增益	计算极快（微秒级），适合电机扭矩限制	完全无法处理状态不等式约束 $g(x)\le 0$ 与耦合约束
直接配点法 / 全局 NLP	Direct Collocation + SNOPT / IPOPT	将全时域状态和控制同时离散为决策变量，求解巨型稀疏 NLP	能通用处理任意非线性硬约束	求解单块巨型稀疏 KKT 系统，开销大，不输出闭环时变反馈增益 $K_k$
约束 DDP (CDDP)	Xie 2017 CDDP	活跃集反向灵敏度解析求解 + 前向阶段 QP 滚动与信赖域重算	严格可行收敛、保持 $O(N)$ 复杂度、天然伴生受约束闭环反馈增益	需严格物理可行初始轨迹热启动，反向要求 $Q_{uu}$ 正定

graph TD
    Problem["受约束非线性轨迹优化问题 (Nonlinear Constrained OCP)"] --> BranchA["罚函数 / 障碍函数法 (Log-Barrier)"]
    Problem --> BranchB["全轨迹直接转录法 (Direct Collocation / SQP)"]
    Problem --> BranchC["约束微分动态规划 (Xie 2017 CDDP)"]
    
    BranchA --> FlawA["⚠️ 缺陷: 边界 Hessian 严重病态，难以严格满足硬约束"]
    BranchB --> FlawB["⚠️ 缺陷: 决策变量维度随 $N$ 线性膨胀，无天然伴生反馈增益"]
    BranchC --> WinC["✅ 优势: 时域 $O(N)$ 递归分解 + 活跃集解析灵敏度 + 逐阶段 QP 严格保可行性"]

1.2 CDDP 的核心设计哲学

Zhaoming Xie, C. Karen Liu 与 Kris Hauser 在 2017 年提出的 CDDP (Constrained DDP) 提出了一种优雅的结构性平衡：

• 在反向传递（Backward Pass）中：识别候选活跃约束，通过局部一阶泰勒线性化建立阶段等式约束 QP，应用 KKT 条件解析求解出受约束的开环前馈控制增量 ${\mathbf{k}}_k$ 与闭环状态反馈增益 ${\mathbf{K}}_k$，并利用拉格朗日乘子符号判定释放非绑定约束；
• 在前向回放（Forward Pass）中：鉴于非线性约束曲率的存在，不直接采用纯仿射反馈，而是在每个时间步求解一个极小规模（仅 $n_u$ 维变量）的阶段 QP，并配以自适应盒式信赖域与不可行回退重启机制；
• 整体保持严格可行性：算法从一条可行（但次优）的初始标称轨迹出发，在每一次外层迭代中只接受既严格满足非线性动力学与非线性不等式约束、又实现总代价单调下降的新轨迹。

---

2. 标准非线性约束轨迹优化问题表述

依据全库统一符号标准化规范，我们考虑离散时间有限时域非线性系统：

$$\begin{array}{r}{\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k),k=0,1,\dots ,N-1\end{array}\text{\hspace{1em}(2-1)}$$

其中：

• 状态向量 ${\mathbf{x}}_k\in {\mathbb{R}}^{n_x}$；
• 控制输入向量 ${\mathbf{u}}_k\in {\mathbb{R}}^{n_u}$；
• $\mathbf{f}:{\mathbb{R}}^{n_x}\times {\mathbb{R}}^{n_u}\to {\mathbb{R}}^{n_x}$ 为二次连续可微的系统动力学映射。

系统在全时域内的累积性能指标泛函（Cost Functional）定义为：

$$\begin{array}{r}\mathcal{J}({\mathbf{x}}_0,\mathbf{U})=\sum _{k=0}^{N-1}{\ell }_k({\mathbf{x}}_k,{\mathbf{u}}_k)+\varphi ({\mathbf{x}}_N)\end{array}\text{\hspace{1em}(2-2)}$$

其中 ${\ell }_k$ 为各阶段运行代价（Running Cost），$\varphi ({\mathbf{x}}_N)$ 为终端代价（Terminal Cost）。

系统面临通用可微非线性不等式约束：

$$\begin{array}{r}{\mathbf{g}}_k({\mathbf{x}}_k,{\mathbf{u}}_k)\le 0,k=0,1,\dots ,N-1\end{array}\text{\hspace{1em}(2-3)}$$

以及终端约束 ${\mathbf{g}}_N({\mathbf{x}}_N)\le 0$（或等式目标 ${\mathbf{h}}_N({\mathbf{x}}_N)=0$）。这里 ${\mathbf{g}}_k:{\mathbb{R}}^{n_x}\times {\mathbb{R}}^{n_u}\to {\mathbb{R}}^{p_k}$ 包含了任意可微状态不等式约束（如与静态/动态障碍物的欧氏安全距离）、控制限幅约束以及状态-控制耦合约束。

值函数与 Bellman 最优性原理

定义从时刻 $k$、状态 $\mathbf{x}$ 出发到时域终点的最优剩余代价为值函数（Value Function / Cost-to-Go）：

$$\begin{array}{r}{V}_k(\mathbf{x})\triangleq \underset{\{{\mathbf{u}}_j{\}}_{j=k}^{N-1}}{\min }\left[\sum _{j=k}^{N-1}{\ell }_j({\mathbf{x}}_j,{\mathbf{u}}_j)+\varphi ({\mathbf{x}}_N)\right]\text{s.t.}{\mathbf{g}}_j({\mathbf{x}}_j,{\mathbf{u}}_j)\le 0,{\mathbf{x}}_{j+1}=\mathbf{f}({\mathbf{x}}_j,{\mathbf{u}}_j)\end{array}\text{\hspace{1em}(2-4)}$$

根据 Bellman 最优性原理，逆向动态规划递推方程为：

$$\begin{array}{r}{V}_k(\mathbf{x})=\underset{\mathbf{u}}{\min }\left[{\ell }_k(\mathbf{x},\mathbf{u})+V_{k+1}(\mathbf{f}(\mathbf{x},\mathbf{u}))\right]\text{s.t.}{\mathbf{g}}_k(\mathbf{x},\mathbf{u})\le 0\end{array}\text{\hspace{1em}(2-5)}$$

终端边界条件为：

$$\begin{array}{r}{V}_N(\mathbf{x})=\varphi (\mathbf{x})\end{array}\text{\hspace{1em}(2-6)}$$

定义动作-状态价值函数（Action-Value / Q-function）：

$$\begin{array}{r}{Q}_k(\mathbf{x},\mathbf{u})\triangleq {\ell }_k(\mathbf{x},\mathbf{u})+V_{k+1}(\mathbf{f}(\mathbf{x},\mathbf{u}))\end{array}\text{\hspace{1em}(2-7)}$$

则阶段最优控制子问题等价于：

$$\begin{array}{r}{\mathbf{u}}_k^{\ast }(\mathbf{x})=\arg \underset{\mathbf{u}}{\min }{Q}_k(\mathbf{x},\mathbf{u})\text{s.t.}{\mathbf{g}}_k(\mathbf{x},\mathbf{u})\le 0\end{array}\text{\hspace{1em}(2-8)}$$ $$\begin{array}{r}{V}_k(\mathbf{x})=Q_k(\mathbf{x},{\mathbf{u}}_k^{\ast }(\mathbf{x}))\end{array}\text{\hspace{1em}(2-9)}$$

---

3. 活跃约束对 Bellman 局部模型的拓扑剪裁

在无约束 DDP 中，我们在当前名义轨迹 $({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)$ 周围对 $Q_k$ 做二阶泰勒展开，由于控制量在全空间无障碍自由变化，一阶最优性条件 ${\nabla }_{\delta \mathbf{u}}{Q}_k=0$ 能够解析导出控制增量是状态偏差的无约束仿射函数 $\delta \mathbf{u}=-Q_{\mathbf{u}\mathbf{u}}^{-1}{Q}_{\mathbf{u}}-Q_{\mathbf{u}\mathbf{u}}^{-1}{Q}_{\mathbf{u}\mathbf{x}}\delta \mathbf{x}$。

但在约束存在时，这一无约束解析律彻底破裂： 当系统状态处于约束边界（例如飞行器紧贴障碍物边缘、或电机输出达到最大饱和幅值）时，上游状态产生微小扰动 $\delta \mathbf{x}$，最优下游控制 $\delta \mathbf{u}$ 不能沿任意无约束梯度方向自由调整，而必须受制于约束切平面的投影流动，强制停留在可行域内部或滑移在约束超曲面上。

因此，反向传播必须显式求解：在约束切空间投影下的局部最优解对于状态偏差 $\delta \mathbf{x}$ 的灵敏度（Sensitivity Derivative）！

---

4. 受约束反向传播 (Constrained Backward Pass) 严格数学推导

设当前已有可行名义轨迹 $(\bar{\mathbf{X}},\bar{\mathbf{U}})=\{({\bar{\mathbf{x}}}_0,{\bar{\mathbf{u}}}_0),\dots ,({\bar{\mathbf{x}}}_{N-1},{\bar{\mathbf{u}}}_{N-1}),{\bar{\mathbf{x}}}_N\}$。定义偏差量：

$$\begin{array}{r}\delta {\mathbf{x}}_k\triangleq {\mathbf{x}}_k-{\bar{\mathbf{x}}}_k,\delta {\mathbf{u}}_k\triangleq {\mathbf{u}}_k-{\bar{\mathbf{u}}}_k\end{array}\text{\hspace{1em}(4-1)}$$

$Q_k$ 在名义点二阶多元泰勒展开为：

$$\begin{array}{r}{Q}_k({\bar{\mathbf{x}}}_k+\delta {\mathbf{x}}_k,{\bar{\mathbf{u}}}_k+\delta {\mathbf{u}}_k)\approx Q_k({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+Q_{\mathbf{x}}^{\top }\delta {\mathbf{x}}_k+Q_{\mathbf{u}}^{\top }\delta {\mathbf{u}}_k+\frac{1}{2}\delta {\mathbf{x}}_k^{\top }{Q}_{\mathbf{x}\mathbf{x}}\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^{\top }{Q}_{\mathbf{u}\mathbf{u}}\delta {\mathbf{u}}_k+\delta {\mathbf{u}}_k^{\top }{Q}_{\mathbf{u}\mathbf{x}}\delta {\mathbf{x}}_k\end{array}\text{\hspace{1em}(4-2)}$$

其中一阶与二阶展开导数（利用链式法则，上标撇号表示在 $k+1$ 时刻评估）：

$$\begin{array}{r}{Q}_{\mathbf{x}}={\ell }_{\mathbf{x}}+{\mathbf{f}}_{\mathbf{x}}^{\top }{V}_{\mathbf{x}}^{\prime }\end{array}\text{\hspace{1em}(4-3)}$$ $$\begin{array}{r}{Q}_{\mathbf{u}}={\ell }_{\mathbf{u}}+{\mathbf{f}}_{\mathbf{u}}^{\top }{V}_{\mathbf{x}}^{\prime }\end{array}\text{\hspace{1em}(4-4)}$$ $$\begin{array}{r}{Q}_{\mathbf{x}\mathbf{x}}={\ell }_{\mathbf{x}\mathbf{x}}+{\mathbf{f}}_{\mathbf{x}}^{\top }{V}_{\mathbf{x}\mathbf{x}}^{\prime }{\mathbf{f}}_{\mathbf{x}}+V_{\mathbf{x}}^{\prime }\cdot {\mathbf{f}}_{\mathbf{x}\mathbf{x}}\end{array}\text{\hspace{1em}(4-5)}$$ $$\begin{array}{r}{Q}_{\mathbf{u}\mathbf{u}}={\ell }_{\mathbf{u}\mathbf{u}}+{\mathbf{f}}_{\mathbf{u}}^{\top }{V}_{\mathbf{x}\mathbf{x}}^{\prime }{\mathbf{f}}_{\mathbf{u}}+V_{\mathbf{x}}^{\prime }\cdot {\mathbf{f}}_{\mathbf{u}\mathbf{u}}\end{array}\text{\hspace{1em}(4-6)}$$ $$\begin{array}{r}{Q}_{\mathbf{u}\mathbf{x}}={\ell }_{\mathbf{u}\mathbf{x}}+{\mathbf{f}}_{\mathbf{u}}^{\top }{V}_{\mathbf{x}\mathbf{x}}^{\prime }{\mathbf{f}}_{\mathbf{x}}+V_{\mathbf{x}}^{\prime }\cdot {\mathbf{f}}_{\mathbf{u}\mathbf{x}}\end{array}\text{\hspace{1em}(4-7)}$$

注：在 iLQR 高斯-牛顿近似下，可忽略动力学二阶张量项 $V_{\mathbf{x}}^{\prime }\cdot {\mathbf{f}}_{\mathbf{x}\mathbf{x}}\approx 0$；而在 Full DDP 下则完整保留。

4.1 候选活跃集选取与局部线性化

在时刻 $k$，对于不等式约束向量 ${\mathbf{g}}_k(\mathbf{x},\mathbf{u})\le 0$，我们选取处于“近活跃（Near-Active）”状态的分量。设定微小正容差 $ϵ>0$（例如 $ϵ=10^{-3}$），定义候选活跃集 ${\hat{\mathbf{g}}}_k$：

$$\begin{array}{r}{\hat{\mathbf{g}}}_k\triangleq \left\{g_{k,i}({\mathbf{x}}_k,{\mathbf{u}}_k)|g_{k,i}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\ge -ϵ\right\}\end{array}\text{\hspace{1em}(4-8)}$$

在名义点 $({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)$ 处将候选活跃约束一阶线性化：

$$\begin{array}{r}{\hat{\mathbf{g}}}_k({\bar{\mathbf{x}}}_k+\delta {\mathbf{x}}_k,{\bar{\mathbf{u}}}_k+\delta {\mathbf{u}}_k)\approx {\hat{\mathbf{g}}}_k({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\hat{\mathbf{g}}}_{\mathbf{x},k}\delta {\mathbf{x}}_k+{\hat{\mathbf{g}}}_{\mathbf{u},k}\delta {\mathbf{u}}_k\le 0\end{array}\text{\hspace{1em}(4-9)}$$

对于已经紧贴边界的活跃约束（${\hat{\mathbf{g}}}_k({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\approx 0$），其局部演化必须严格满足切平面等式：

$$\begin{array}{r}{\mathbf{C}}_k\delta {\mathbf{u}}_k={\mathbf{D}}_k\delta {\mathbf{x}}_k\end{array}\text{\hspace{1em}(4-10)}$$

其中定义雅可比矩阵：

$$\begin{array}{r}{\mathbf{C}}_k\triangleq {\hat{\mathbf{g}}}_{\mathbf{u},k}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\in {\mathbb{R}}^{\hat{p}\times n_u},{\mathbf{D}}_k\triangleq -{\hat{\mathbf{g}}}_{\mathbf{x},k}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\in {\mathbb{R}}^{\hat{p}\times n_x}\end{array}\text{\hspace{1em}(4-11)}$$

4.2 阶段等式约束 QP 与 KKT 线性系统

将第 $k$ 步的最优控制扰动寻优建模为受约束二次规划：

$$\begin{array}{r}\underset{\delta {\mathbf{u}}_k}{\min }\frac{1}{2}\delta {\mathbf{u}}_k^{\top }{Q}_{\mathbf{u}\mathbf{u},k}\delta {\mathbf{u}}_k+\delta {\mathbf{u}}_k^{\top }{Q}_{\mathbf{u}\mathbf{x},k}\delta {\mathbf{x}}_k+Q_{\mathbf{u},k}^{\top }\delta {\mathbf{u}}_k\end{array}\text{\hspace{1em}(4-12)}$$ $$\begin{array}{r}\text{s.t.}{\mathbf{C}}_k\delta {\mathbf{u}}_k={\mathbf{D}}_k\delta {\mathbf{x}}_k\end{array}\text{\hspace{1em}(4-13)}$$

引入拉格朗日乘子向量 ${\mathbf{\lambda }}_k\in {\mathbb{R}}^{\hat{p}}$，构建拉格朗日函数：

$$\begin{array}{r}\mathcal{L}(\delta {\mathbf{u}}_k,{\mathbf{\lambda }}_k)=\frac{1}{2}\delta {\mathbf{u}}_k^{\top }{Q}_{\mathbf{u}\mathbf{u},k}\delta {\mathbf{u}}_k+\delta {\mathbf{u}}_k^{\top }{Q}_{\mathbf{u}\mathbf{x},k}\delta {\mathbf{x}}_k+Q_{\mathbf{u},k}^{\top }\delta {\mathbf{u}}_k+{\mathbf{\lambda }}_k^{\top }({\mathbf{C}}_k\delta {\mathbf{u}}_k-{\mathbf{D}}_k\delta {\mathbf{x}}_k)\end{array}\text{\hspace{1em}(4-14)}$$

对 $\delta {\mathbf{u}}_k$ 和 ${\mathbf{\lambda }}_k$ 求偏导并令其为 0，得到一阶 Karush-Kuhn-Tucker (KKT) 方程组：

$$\begin{array}{r}\{\begin{array}{l}{\nabla }_{\delta \mathbf{u}}\mathcal{L}=Q_{\mathbf{u}\mathbf{u},k}\delta {\mathbf{u}}_k+Q_{\mathbf{u}\mathbf{x},k}\delta {\mathbf{x}}_k+Q_{\mathbf{u},k}+{\mathbf{C}}_k^{\top }{\mathbf{\lambda }}_k=0\\ {\nabla }_{\mathbf{\lambda }}\mathcal{L}={\mathbf{C}}_k\delta {\mathbf{u}}_k-{\mathbf{D}}_k\delta {\mathbf{x}}_k=0\end{array}\end{array}\text{\hspace{1em}(4-15)}$$

写成标准分块矩阵 KKT 系统：

$$\begin{array}{r}\left[\begin{array}{cc}{Q}_{\mathbf{u}\mathbf{u},k}& {\mathbf{C}}_k^{\top }\\ {\mathbf{C}}_k& 0\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{u}}_k\\ {\mathbf{\lambda }}_k\end{array}\right]=\left[\begin{array}{c}-Q_{\mathbf{u}\mathbf{x},k}\delta {\mathbf{x}}_k-Q_{\mathbf{u},k}\\ {\mathbf{D}}_k\delta {\mathbf{x}}_k\end{array}\right]\end{array}\text{\hspace{1em}(4-16)}$$

4.3 对偶乘子符号判定与非活跃约束释放机制

在 KKT 最优性理论中，对于不等式约束 $g_i\le 0$，其对偶拉格朗日乘子必须满足非负互补条件 ${\lambda }_i\ge 0$。

• 若 ${\lambda }_i>0$：表明该约束处于**绑定（Binding / Active）**状态，阻止了目标函数进一步下降，该约束对最优解有实质性限制；
• 若某个候选约束解出的 ${\lambda }_i<0$：表明目标函数梯度的下降方向是指向可行域内部的！此时如果强制将该约束当作等式锁定（$C_i\delta u=D_i\delta x$），将错误地阻碍系统向代价更低的内部区域移动！

CDDP 采用如下活跃集动态释放机制：

1. 首先在名义无状态扰动点 $\delta {\mathbf{x}}_k=0$ 处求解 KKT 系统的标称拉格朗日乘子： 由式 (4-15) 第一式得 $\delta {\mathbf{u}}_k=-Q_{\mathbf{u}\mathbf{u},k}^{-1}(Q_{\mathbf{u},k}+{\mathbf{C}}_k^{\top }{\mathbf{\lambda }}_k)$，代入第二式：

$$-{\mathbf{C}}_k{Q}_{\mathbf{u}\mathbf{u},k}^{-1}{Q}_{\mathbf{u},k}-{\mathbf{C}}_k{Q}_{\mathbf{u}\mathbf{u},k}^{-1}{\mathbf{C}}_k^{\top }{\mathbf{\lambda }}_k=0$$ $$\begin{array}{r}{\bar{\mathbf{\lambda }}}_k=-{\left({\mathbf{C}}_k{Q}_{\mathbf{u}\mathbf{u},k}^{-1}{\mathbf{C}}_k^{\top }\right)}^{-1}{\mathbf{C}}_k{Q}_{\mathbf{u}\mathbf{u},k}^{-1}{Q}_{\mathbf{u},k}\end{array}\text{\hspace{1em}(4-17)}$$

2. 逐行检查 ${\bar{\mathbf{\lambda }}}_k$：剔除所有 ${\bar{\lambda }}_{k,i}<0$ 的不等式约束行（释放约束），仅保留严格正乘子行与原有等式约束；
3. 形成修剪后的约化约束矩阵 ${\hat{\mathbf{C}}}_k$ 与 ${\hat{\mathbf{D}}}_k$。

4.4 受约束前馈步长与反馈增益闭式解

在剔除负乘子后，设剩余有效活跃约束数量为 $p\le n_u$，定义如下两个核心受约束算子矩阵：

$$\begin{array}{r}{\mathbf{W}}_k\triangleq {\left({\hat{\mathbf{C}}}_k{Q}_{\mathbf{u}\mathbf{u},k}^{-1}{\hat{\mathbf{C}}}_k^{\top }\right)}^{-1}{\hat{\mathbf{C}}}_k{Q}_{\mathbf{u}\mathbf{u},k}^{-1}\in {\mathbb{R}}^{p\times n_u}\end{array}\text{\hspace{1em}(4-18)}$$ $$\begin{array}{r}{\mathbf{H}}_k\triangleq Q_{\mathbf{u}\mathbf{u},k}^{-1}\left({\mathbf{I}}_{n_u}-{\hat{\mathbf{C}}}_k^{\top }{\mathbf{W}}_k\right)\in {\mathbb{R}}^{n_u\times n_u}\end{array}\text{\hspace{1em}(4-19)}$$

💡 数学性质洞察：

• 矩阵 ${\mathbf{H}}_k$ 实质上是 $Q_{\mathbf{u}\mathbf{u},k}^{-1}$ 在由约束诱导的切空间（Null-space of ${\hat{\mathbf{C}}}_k$）上的正交投影算子；
• 当不存在活跃约束时（${\hat{\mathbf{C}}}_k=\varnothing$），${\mathbf{W}}_k=0$，${\mathbf{H}}_k=Q_{\mathbf{u}\mathbf{u},k}^{-1}$，公式无缝退化为经典无约束 DDP！

将 ${\mathbf{W}}_k$ 和 ${\mathbf{H}}_k$ 代入 KKT 方程组求解，得到最优控制扰动对于状态扰动的解析闭式解：

$$\begin{array}{r}\delta {\mathbf{u}}_k^{\ast }(\delta {\mathbf{x}}_k)={\mathbf{k}}_k+{\mathbf{K}}_k\delta {\mathbf{x}}_k\end{array}\text{\hspace{1em}(4-20)}$$

其中：

• 受约束开环前馈控制修正量 (Constrained Feedforward Gain)：

$$\begin{array}{r}{\mathbf{k}}_k=-{\mathbf{H}}_k{Q}_{\mathbf{u},k}\end{array}\text{\hspace{1em}(4-21)}$$

• 受约束闭环状态反馈增益矩阵 (Constrained Feedback Gain)：

$$\begin{array}{r}{\mathbf{K}}_k=-{\mathbf{H}}_k{Q}_{\mathbf{u}\mathbf{x},k}+{\mathbf{W}}_k^{\top }{\hat{\mathbf{D}}}_k\end{array}\text{\hspace{1em}(4-22)}$$

重点解析式 (4-22)：反馈增益 ${\mathbf{K}}_k$ 由两部分构成：

1. 第一项 $-{\mathbf{H}}_k{Q}_{\mathbf{u}\mathbf{x},k}$：目标函数在约束切平面上的投影下降反馈；
2. 第二项 ${\mathbf{W}}_k^{\top }{\hat{\mathbf{D}}}_k$：约束沿流形法向的几何恢复项！当状态偏差 $\delta \mathbf{x}$ 试图突破约束曲面时，该项产生反向补偿控制，强制将系统拉回约束边界内！

4.5 受约束值函数二阶模型逆向更新

将受约束最优控制律 (4-20) 代回二次模型 (4-2)，并对 $\delta {\mathbf{x}}_k$ 重新整理二次项与一次项，可得当前时刻值函数 $V_k(\mathbf{x})$ 的二阶导数逆向递推公式：

$$\begin{array}{r}{V}_{\mathbf{x}}(k)=Q_{\mathbf{x}}+{\mathbf{K}}_k^{\top }{Q}_{\mathbf{u}}+{\mathbf{K}}_k^{\top }{Q}_{\mathbf{u}\mathbf{u}}{\mathbf{k}}_k+Q_{\mathbf{u}\mathbf{x}}^{\top }{\mathbf{k}}_k\end{array}\text{\hspace{1em}(4-23)}$$ $$\begin{array}{r}{V}_{\mathbf{x}\mathbf{x}}(k)=Q_{\mathbf{x}\mathbf{x}}+{\mathbf{K}}_k^{\top }{Q}_{\mathbf{u}\mathbf{u}}{\mathbf{K}}_k+{\mathbf{K}}_k^{\top }{Q}_{\mathbf{u}\mathbf{x}}+Q_{\mathbf{u}\mathbf{x}}^{\top }{\mathbf{K}}_k\end{array}\text{\hspace{1em}(4-24)}$$

为了保证数值稳定性，每步对 $V_{\mathbf{x}\mathbf{x}}$ 实施对称化操作：$V_{\mathbf{x}\mathbf{x}}\leftarrow \frac{1}{2}(V_{\mathbf{x}\mathbf{x}}+V_{\mathbf{x}\mathbf{x}}^{\top })$。

终端边界初始化条件为：

$$\begin{array}{r}{V}_{\mathbf{x}}(N)={\nabla }_{\mathbf{x}}\varphi ({\bar{\mathbf{x}}}_N),V_{\mathbf{x}\mathbf{x}}(N)={\nabla }_{\mathbf{x}\mathbf{x}}^2\varphi ({\bar{\mathbf{x}}}_N)\end{array}\text{\hspace{1em}(4-25)}$$

从 $k=N-1$ 逆向循环递推至 $k=0$，即可完成整个受约束反向传播。

flowchart TD
    Start["终端初始化: $V_{\boldsymbol{x}}(N) = \nabla \phi, V_{\boldsymbol{x}\boldsymbol{x}}(N) = \nabla^2 \phi$"] --> Loop["逆向循环: $k = N-1, \dots, 0$"]
    Loop --> CalcQ["计算 Q 矩阵导数: $Q_{\boldsymbol{x}}, Q_{\boldsymbol{u}}, Q_{\boldsymbol{x}\boldsymbol{x}}, Q_{\boldsymbol{u}\boldsymbol{u}}, Q_{\boldsymbol{u}\boldsymbol{x}}$ 并加入 LM 阻尼"]
    CalcQ --> ActiveSet["判定候选活跃约束: $g_{k, i}(\bar{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k) \ge -\epsilon$"]
    ActiveSet --> Multiplier["求解 $\delta \boldsymbol{x}=\boldsymbol{0}$ 标称对偶乘子 $\bar{\boldsymbol{\lambda}}_k$"]
    Multiplier --> Filter["过滤释放 $\bar{\lambda}_i < 0$ 的非活跃行，保留有效矩阵 $\hat{\boldsymbol{C}}_k, \hat{\boldsymbol{D}}_k$"]
    Filter --> Gains["计算受约束算子 $\boldsymbol{W}, \boldsymbol{H}$ 及增益 $\boldsymbol{k}_k, \boldsymbol{K}_k$"]
    Gains --> ValueUpdate["更新值函数导数: $V_{\boldsymbol{x}}(k), V_{\boldsymbol{x}\boldsymbol{x}}(k)$"]
    ValueUpdate --> Next["$k \leftarrow k-1$ 递推至初始时刻 $k=0$"]

---

5. 受约束前向回放 (Constrained Forward Pass) 与信赖域管理

5.1 为什么线性回放会失效？阶段 QP 的必要性

在无约束 DDP 中，前向传播只需简单地执行仿真：

$${\mathbf{u}}_k^{\mathrm{new}}={\bar{\mathbf{u}}}_k+\alpha {\mathbf{k}}_k+{\mathbf{K}}_k({\mathbf{x}}_k^{\mathrm{new}}-{\bar{\mathbf{x}}}_k),{\mathbf{x}}_{k+1}^{\mathrm{new}}=\mathbf{f}({\mathbf{x}}_k^{\mathrm{new}},{\mathbf{u}}_k^{\mathrm{new}})$$

但在强非线性约束下，纯线性回放会直接导致严重越界或碰撞：

1. 反向传播仅仅是基于当前名义点的局部一阶线性化，而真实的非线性障碍物表面（如球面、圆柱面）是弯曲的；
2. 在全时域积分过程中，非线性动力学演化会导致状态累积漂移，使得线性增益给出的控制量 ${\mathbf{u}}_k^{\mathrm{new}}$ 无法精确满足非线性约束 ${\mathbf{g}}_k({\mathbf{x}}_k^{\mathrm{new}},{\mathbf{u}}_k^{\mathrm{new}})\le 0$；
3. 一旦在某个中间时刻发生越界，下游状态将严重偏离可行通道，导致整个轨迹发散。

因此，CDDP 在前向传播中采用了创新的“逐阶段 QP 重解算 (Stage-wise QP Rollout)”。

5.2 阶段 QP 形式与自适应盒式信赖域

在前向仿真的每一个时刻 $k$，系统已经获得了当前真实的前向状态 ${\mathbf{x}}_k^{\mathrm{temp}}$。定义状态偏差 $\delta {\mathbf{x}}_k={\mathbf{x}}_k^{\mathrm{temp}}-{\bar{\mathbf{x}}}_k$。

CDDP 在当前状态处求解一个小型凸二次规划（QP）来决定当前最优控制扰动 $\delta {\mathbf{u}}_k^{\ast }$：

$$\begin{array}{r}\underset{\delta \mathbf{u}}{\min }\frac{1}{2}\delta {\mathbf{u}}^{\top }{Q}_{\mathbf{u}\mathbf{u},k}\delta \mathbf{u}+\delta {\mathbf{u}}^{\top }{Q}_{\mathbf{u}\mathbf{x},k}\delta {\mathbf{x}}_k+Q_{\mathbf{u},k}^{\top }\delta \mathbf{u}\end{array}\text{\hspace{1em}(5-1)}$$ $$\begin{array}{r}\text{s.t.}{\mathbf{g}}_k({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\nabla }_{\mathbf{x}}{\mathbf{g}}_k({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}{\mathbf{g}}_k({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta \mathbf{u}\le 0\end{array}\text{\hspace{1em}(5-2)}$$ $$\begin{array}{r}-e\le \delta \mathbf{u}\le e(\text{盒式信赖域 Box Trust Region})\end{array}\text{\hspace{1em}(5-3)}$$

其中 $e>0$ 为自适应盒式信赖域半径。

💡 阶段 QP 的极速求解特性：
注意到该阶段 QP 的优化变量维度仅仅为控制输入维度 $n_u$（对于质点为 2，车辆为 2，四旋翼为 4）。对于如此小规模的 QP，使用确定性积极集法（Active-Set QP）仅需几微秒即可精确求解，绝不会成为计算瓶颈！

解出阶段最优 $\delta {\mathbf{u}}^{\ast }$ 后，更新控制并传播真实非线性动力学：

$$\begin{array}{r}{\mathbf{u}}_k^{\mathrm{temp}}={\bar{\mathbf{u}}}_k+\delta {\mathbf{u}}^{\ast },{\mathbf{x}}_{k+1}^{\mathrm{temp}}=\mathbf{f}({\mathbf{x}}_k^{\mathrm{temp}},{\mathbf{u}}_k^{\mathrm{temp}})\end{array}\text{\hspace{1em}(5-4)}$$

5.3 不可行回退重启与单调代价接受准则

在前向仿真过程中，CDDP 严格执行如下两重安全把关机制：

graph TD
    Start["开始前向滚动: $x \leftarrow x_0, k = 0$"] --> SolveQP["求解第 $k$ 步阶段 QP (含信赖域 $|\delta u| \le e$)"]
    SolveQP --> CheckFeasible{"阶段 QP 是否可行<br/>且满足非线性约束?"}
    
    CheckFeasible -- "❌ 不可行 (Infeasible)" --> Shrink["信赖域收缩: $e \leftarrow \alpha e$ (如 $\alpha=0.5$)<br/>重置回 $k=0$ 重新仿真"]
    Shrink --> Start
    
    CheckFeasible -- "✅ 可行 (Feasible)" --> Propagate["非线性动力学前向推进: $x_{k+1} = f(x_k, u_k)$"]
    Propagate --> CheckEnd{"是否已推至 $k=N$?"}
    CheckEnd -- "否" --> NextK["$k \leftarrow k+1$"] --> SolveQP
    
    CheckEnd -- "是" --> CheckCost{"评估总代价:<br/>$J_{\mathrm{temp}} < J_{\mathrm{init}}$ ?"}
    CheckCost -- "✅ 代价单调下降" --> Accept["接受新轨迹: $(\bar{\boldsymbol{X}}, \bar{\boldsymbol{U}}) \leftarrow (\boldsymbol{X}^{\mathrm{temp}}, \boldsymbol{U}^{\mathrm{temp}})$<br/>减小正则化阻尼: $\mu \leftarrow \beta \mu$"]
    CheckCost -- "❌ 代价上升" --> Reject["拒绝新轨迹<br/>增大正则化阻尼: $\mu \leftarrow \alpha_{\mu} \mu$"]

1. 不可行回退与信赖域收缩 (Trust-Region Shrinking)： 如果在某一步 $k$，阶段 QP 无解（出现不可行），或者数值积分后的真实状态严重违背了非线性物理约束，则判定当前步长 $e$ 过大导致线性化模型失真。此时立即中断滚动，将信赖域半径缩减 $e\leftarrow \alpha e$（$\alpha =0.5$），并完全重置回 $k=0$ 从头重新仿真。
2. 单调代价改善准则 (Monotone Cost Improvement)： 当成功推进到时域终点 $N$ 且全程物理可行后，计算整条候选轨迹的真实累积代价 ${\mathcal{J}}_{\mathrm{temp}}$。
   • 若 ${\mathcal{J}}_{\mathrm{temp}}<{\mathcal{J}}_{\mathrm{init}}$：接受新轨迹，更新标称轨迹，并降低 LM 阻尼权重（${\mu }_1\leftarrow {\beta }_1{\mu }_1,{\mu }_2\leftarrow {\beta }_2{\mu }_2$）；
   • 若代价未改善：拒绝新轨迹，增大 LM 阻尼权重（${\mu }_1\leftarrow {\alpha }_1{\mu }_1,{\mu }_2\leftarrow {\alpha }_2{\mu }_2$），迫使下一次迭代更加偏向保守的受约束梯度流。

---

6. 正则化策略与动力学离散化避坑工程指南

6.1 状态与控制双重 LM 阻尼正则化

为了保证逆向递推时 Hessian 矩阵 $Q_{\mathbf{u}\mathbf{u}}$ 严格正定可逆，同时防止新轨迹过度偏离旧轨迹，CDDP 采纳了 Tassa 等人的双重 Levenberg-Marquardt (LM) 正则化方案：

$$\begin{array}{r}{Q}_{\mathbf{x}\mathbf{x},k}={\ell }_{\mathbf{x}\mathbf{x},k}+{\mathbf{f}}_{\mathbf{x}}^{\top }(V_{\mathbf{x}\mathbf{x},k+1}+{\mu }_1{\mathbf{I}}_{n_x}){\mathbf{f}}_{\mathbf{x}}+V_{\mathbf{x},k+1}^{\prime }\cdot {\mathbf{f}}_{\mathbf{x}\mathbf{x}}\end{array}\text{\hspace{1em}(6-1)}$$ $$\begin{array}{r}{Q}_{\mathbf{u}\mathbf{u},k}={\ell }_{\mathbf{u}\mathbf{u},k}+{\mathbf{f}}_{\mathbf{u}}^{\top }(V_{\mathbf{x}\mathbf{x},k+1}+{\mu }_1{\mathbf{I}}_{n_x}){\mathbf{f}}_{\mathbf{u}}+V_{\mathbf{x},k+1}^{\prime }\cdot {\mathbf{f}}_{\mathbf{u}\mathbf{u}}+{\mu }_2{\mathbf{I}}_{n_u}\end{array}\text{\hspace{1em}(6-2)}$$ $$\begin{array}{r}{Q}_{\mathbf{u}\mathbf{x},k}={\ell }_{\mathbf{u}\mathbf{x},k}+{\mathbf{f}}_{\mathbf{u}}^{\top }(V_{\mathbf{x}\mathbf{x},k+1}+{\mu }_1{\mathbf{I}}_{n_x}){\mathbf{f}}_{\mathbf{x}}+V_{\mathbf{x},k+1}^{\prime }\cdot {\mathbf{f}}_{\mathbf{u}\mathbf{x}}\end{array}\text{\hspace{1em}(6-3)}$$

参数调节经验取值：

• 衰减系数：${\beta }_1={\beta }_2=0.95$；
• 放大系数：${\alpha }_1={\alpha }_2=1.05$；
• 信赖域缩放系数：$\alpha =0.5$。

6.2 动力学相对度（Relative Degree）与离散化陷阱

[!CAUTION] 致命工程避坑：欧拉离散化导致的约束灵敏度退化！

在多刚体动力学与质点系统中，系统的控制量通常是加速度/力/力矩 ${\mathbf{a}}_k$，而避障约束通常是关于空间几何位置 ${\mathbf{p}}_k$ 的函数：

$$g({\mathbf{p}}_k)=r_{obs}^2-\Vert {\mathbf{p}}_k-{\mathbf{p}}_{obs}{\Vert }^2\le 0$$

若在离散化时采用简单的标准显式欧拉法（Forward Euler）：

$${\mathbf{p}}_{k+1}={\mathbf{p}}_k+h{\mathbf{v}}_k,{\mathbf{v}}_{k+1}={\mathbf{v}}_k+h{\mathbf{a}}_k$$

此时，位置 ${\mathbf{p}}_k$ 对当步控制输入 ${\mathbf{a}}_k$ 的一阶偏导恒等于零：

$$\frac{\partial g}{\partial {\mathbf{a}}_k}=\frac{\partial g}{\partial {\mathbf{p}}_k}\frac{\partial {\mathbf{p}}_k}{\partial {\mathbf{a}}_k}=0⟹{\mathbf{C}}_k={\hat{\mathbf{g}}}_{\mathbf{u},k}\equiv 0$$

后果是灾难性的： ${\mathbf{C}}_k=0$ 意味着约束矩阵在控制空间完全没有投影分量，反向传播与前向阶段 QP 均无法感知到当步控制对避碰约束的任何调整作用，导致求解器退化或奇异！

工业级解决方案： 采用具有二阶精度的零阶保持器 (Zero-Order Hold, ZOH) 或中点法 (Midpoint Runge-Kutta) 进行状态离散化：

$${\mathbf{p}}_{k+1}={\mathbf{p}}_k+h{\mathbf{v}}_k+\frac{1}{2}{h}^2{\mathbf{a}}_k⟹\frac{\partial {\mathbf{p}}_{k+1}}{\partial {\mathbf{a}}_k}=\frac{1}{2}{h}^2\mathbf{I}\ne 0$$

将下一时刻的状态安全约束 $g({\mathbf{x}}_{k+1})=g(\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k))\le 0$ 作为当前步的转移约束，此时 ${\mathbf{C}}_k=\nabla g\cdot {\mathbf{f}}_{\mathbf{u}}\ne 0$，约束灵敏度得以完美保持！

---

7. 三大经典基准物理系统实验深度剖析

论文与本地复现工程在三大典型欠驱动与受限非线性系统上对 CDDP 进行了严格的数值仿真与基准对标：

7.1 二维质点避障（Point Mass Double Integrator）

• 状态与控制：$\mathbf{x}=[x,y,v_x,v_y{]}^{\top }\in {\mathbb{R}}^4,\mathbf{u}=[a_x,a_y{]}^{\top }\in {\mathbb{R}}^2$
• 时域参数：$h=0.05\text{s},N=300$；
• 几何障碍物：
  • 单障碍物：圆心位于 $(1,1)$、半径 $r=0.5$，即 $(x-1{)}^2+(y-1{)}^2\ge 0.25$；
  • 双障碍物：追加圆心位于 $(1.5,2.2)$、半径 $r=0.5$ 的第二圆形障碍物；
• 初始与目标：${\mathbf{x}}_0=[0,0,0,0{]}^{\top },{\mathbf{x}}_{\mathrm{goal}}=[3,3,0,0{]}^{\top }$；
• 表现：CDDP 顺利计算出平滑绕行切线轨迹，且在双障碍物狭窄通道中展现出优异的贴边过渡特性。

图 7.1.1：本地复现 CDDP 在二维质点单圆形障碍物避障任务中的轨迹迭代与收敛历程。

图 7.1.2：本地复现 CDDP 在双障碍物狭窄通道穿越任务中的精准贴边无碰撞轨迹。

图 7.1.3：原论文图 2，二维质点双障碍物避障轨迹与初值对比。

---

7.2 二维非完整约束车辆动态避障（2D Car with Kinematic Constraints）

• 状态与控制：$\mathbf{x}=[x,y,\theta ,v{]}^{\top }\in {\mathbb{R}}^4,\mathbf{u}=[u^{\theta },u^v{]}^{\top }\in {\mathbb{R}}^2$（转向角速度与纵向加速度）
• 动力学：

$$\dot{x}=v\sin \theta ,\dot{y}=v\cos \theta ,\dot{\theta }=u^{\theta }v,\dot{v}=u^v$$

• 控制限幅硬约束：转向输入满足 $u^{\theta }\in [-\pi /2,\pi /2]$；
• 动态障碍物场景：一个半径为 $1$ 的圆形障碍物自 $(-1,1.2)$ 以 $0.5\text{m/s}$ 向右匀速穿行（$N=200$）；
• 机动策略涌现：CDDP 自主规划出了极其智能的**“先低速等待避让，待动态障碍物穿过十字交叉口后急加速穿过”**的非凸时空机动行为！

图 7.2.1：本地复现非完整约束车辆绕行固定障碍物的平滑可行轨迹。

图 7.2.2：本地复现车辆面对动态横穿障碍物时的“减速礼让-加速超车”时空速度规划图。

图 7.2.3：原论文图 3，车辆避开固定障碍物的几何规划结果。

图 7.2.4：原论文图 4，车辆与动态穿行圆形障碍物在不同时间步的位置关系全貌。

图 7.2.5：原论文图 5，动态避障过程的时序动作分解图。

---

7.3 三维十二状态四旋翼飞行器（3D Quadcopter 6-DOF Dynamics）

• 状态与控制：12 维非线性状态（位置 $\mathbf{p}\in {\mathbb{R}}^3$、速度 $\mathbf{v}\in {\mathbb{R}}^3$、欧拉角 $\mathbf{\theta }\in {\mathbb{R}}^3$、机体系角速度 $\mathbf{\omega }\in {\mathbb{R}}^3$），4 维控制（电机平方转速 $\mathbf{u}=[u_1^2,u_2^2,u_3^2,u_4^2{]}^{\top }$）；
• 执行机构单向推力约束：$u_i\ge 0,i=1,\dots ,4$；
• 三维双动态障碍物避碰：
  • 障碍物 1：从 $[-1.5,0,2{]}^{\top }$ 沿 $-z$ 轴匀速下坠；
  • 障碍物 2：从 $[1,-2.5,0{]}^{\top }$ 沿 $+y$ 轴匀速横切；
• 求解效果：在 12 维高维欠驱动强耦合非线性空间中，CDDP 在 5 秒计算预算内以终值代价 49.36 成功收敛并生成完美的三维避让航线。

图 7.3.1：原论文图 1，四旋翼飞行器在三维空间穿越两个运动球体障碍物的复杂机动轨迹。

图 7.3.2：原论文图 6，四旋翼固定球形障碍物避障的三维几何视角。

---

7.4 CDDP vs 对数障碍 DDP vs SNOPT/IPOPT 全景横向评测

论文在相同硬件平台与 5 秒统一计算预算下，对三大算法在不同时域长度下的收敛总代价进行了严谨对比：

测试算例与时域参数	约束 DDP (CDDP)	对数障碍 DDP (Log-Barrier)	直接 SQP (SNOPT)	深度结果剖析
质点，单圆障碍 ($h=0.05,N=300$)	0.065	0.042	0.073	短时域简单凸障碍，三者均能平稳收敛
质点，双圆障碍 ($h=0.05,N=300$)	0.28	0.64	0.43	对数障碍开始出现边界振荡与较差局部解
车辆，动态圆障碍 ($h=0.05,N=200$)	0.10	0.32	0.08	CDDP 成功捕捉“减速等待-加速突破”非凸机动
质点，单圆障碍 ($h=0.03,N=500$)	0.071	18.30	0.32	长时域下对数障碍彻底发散；SNOPT 变慢
车辆，动态圆障碍 ($h=0.02,N=500$)	0.21	4.80	86.00	SNOPT 在长时域巨型稀疏 NLP 下陷入局部停滞
四旋翼，固定球 ($h=0.01,N=400$)	53.69	635.53	98.28	CDDP 收敛速度明显超越 SNOPT，代价低 45%
四旋翼，双动态球 ($h=0.01,N=400$)	52.45	182.01	49.12	CDDP 展现出卓越的高维多动态约束解算能力

图 7.4.1：原论文图 7，CDDP、对数障碍法与通用 NLP 直接配点法（SNOPT）在各算例下的收敛速度与约束满足度横向对比曲线。

---

8. 完整 Python / JAX 算法实战架构

以下为严格依循 Xie 2017 理论公式与本地工程库 differential-dynamic-programming/Xie_2017_Nonlinear_Constrained_DDP/src/ 架构编写的轻量级、内置确定性 Active-Set QP 解算器的工业级 CDDP 核心解算器：

import numpy as np

def solve_stage_convex_qp(H, g, C, d_bound, lower, upper, tol=1e-8):
    """
    微型确定性 Active-Set QP 求解器 (专门针对 n_u = 2~4 的控制维度设计)
    求解: min 0.5 * u^T H u + g^T u   s.t.  C u <= d_bound,  lower <= u <= upper
    """
    nu = g.size
    H = 0.5 * (H + H.T)
    
    # 构造统一不等式矩阵 A_ineq @ u <= b_ineq
    A_list = [C] if C.size else []
    b_list = [d_bound] if C.size else []
    
    # 盒式信赖域边界 [-e, e]
    A_list.extend([np.eye(nu), -np.eye(nu)])
    b_list.extend([upper, -lower])
    
    A_all = np.vstack(A_list)
    b_all = np.concatenate(b_list)
    m = A_all.shape[0]
    
    best_u = None
    best_obj = float('inf')
    found = False
    
    # 枚举可能的活跃约束集组合 (0 到 nu 个约束激活)
    from itertools import combinations
    for k in range(nu + 1):
        for combo in combinations(range(m), k):
            if k == 0:
                try:
                    u_cand = -np.linalg.solve(H, g)
                except np.linalg.LinAlgError:
                    continue
                lam = np.array([])
            else:
                A_act = A_all[list(combo)]
                b_act = b_all[list(combo)]
                KKT = np.block([[H, A_act.T], [A_act, np.zeros((k, k))]])
                rhs = np.concatenate([-g, b_act])
                try:
                    sol = np.linalg.solve(KKT, rhs)
                    u_cand = sol[:nu]
                    lam = sol[nu:]
                except np.linalg.LinAlgError:
                    continue
                if np.any(lam < -tol):  # 对偶乘子必须非负
                    continue
            
            # 校验全局可行性
            if np.all(A_all @ u_cand <= b_all + tol):
                obj = 0.5 * u_cand @ H @ u_cand + g @ u_cand
                if obj < best_obj:
                    best_obj = obj
                    best_u = u_cand
                    found = True
        if found and k > 0:
            break
            
    return best_u, found

class ConstrainedDDPSolver:
    """
    非线性约束微分动态规划 (Nonlinear CDDP) 求解器
    严格依据 Zhaoming Xie et al. (ICRA 2017) 闭式灵敏度与阶段 QP 实现
    """
    def __init__(self, dynamics, cost, constraint, nx, nu, horizon):
        self.dynamics = dynamics       # f(x, u), fx, fu, fxx, fuu, fux
        self.cost = cost               # l(x, u), lx, lu, lxx, luu, lux, lf, lfx, lfxx
        self.constraint = constraint   # g(x, u) <= 0, gx, gu
        self.nx = nx
        self.nu = nu
        self.N = horizon

        # 正则化参数与信赖域参数
        self.mu1 = 1e-3
        self.mu2 = 1e-3
        self.beta1 = 0.95
        self.beta2 = 0.95
        self.alpha1 = 1.05
        self.alpha2 = 1.05
        self.eps_active = 1e-3        # 候选活跃约束判定容差
        self.trust_region_init = 2.0  # 初始盒式信赖域半径 e
        self.alpha_trust = 0.5        # 信赖域缩减系数

    def solve(self, x0, u_nominal, max_iters=50, tol=1e-4):
        x_nom = np.zeros((self.N + 1, self.nx))
        u_nom = np.array(u_nominal)
        x_nom[0] = x0
        for k in range(self.N):
            x_nom[k + 1] = self.dynamics.f(x_nom[k], u_nom[k])

        current_cost = self._calc_total_cost(x_nom, u_nom)

        for iteration in range(max_iters):
            # 1. 受约束反向传播 (Constrained Backward Pass)
            k_seq, K_seq, success = self._backward_pass(x_nom, u_nom)
            if not success:
                self.mu1 *= self.alpha1
                self.mu2 *= self.alpha2
                continue

            # 2. 受约束前向回放 (Constrained Forward Pass with Stage-wise QP)
            x_new, u_new, new_cost, accepted = self._forward_pass(
                x_nom, u_nom, k_seq, K_seq, current_cost
            )

            if accepted:
                cost_reduction = current_cost - new_cost
                x_nom, u_nom, current_cost = x_new, u_new, new_cost
                self.mu1 = max(1e-6, self.mu1 * self.beta1)
                self.mu2 = max(1e-6, self.mu2 * self.beta2)
                if cost_reduction < tol:
                    print(f"[CDDP] 收敛于第 {iteration + 1} 次迭代，总代价: {current_cost:.6f}")
                    break
            else:
                self.mu1 *= self.alpha1
                self.mu2 *= self.alpha2

        return x_nom, u_nom, current_cost

    def _backward_pass(self, x_nom, u_nom):
        k_seq = np.zeros((self.N, self.nu))
        K_seq = np.zeros((self.N, self.nu, self.nx))

        Vx = self.cost.lfx(x_nom[self.N])
        Vxx = self.cost.lfxx(x_nom[self.N])

        for k in reversed(range(self.N)):
            xk, uk = x_nom[k], u_nom[k]
            fx, fu = self.dynamics.fx(xk, uk), self.dynamics.fu(xk, uk)

            Qx = self.cost.lx(xk, uk) + fx.T @ Vx
            Qu = self.cost.lu(xk, uk) + fu.T @ Vx
            Qxx = self.cost.lxx(xk, uk) + fx.T @ (Vxx + self.mu1 * np.eye(self.nx)) @ fx
            Quu = self.cost.luu(xk, uk) + fu.T @ (Vxx + self.mu1 * np.eye(self.nx)) @ fu + self.mu2 * np.eye(self.nu)
            Qux = self.cost.lux(xk, uk) + fu.T @ (Vxx + self.mu1 * np.eye(self.nx)) @ fx

            # Full DDP 动力学曲率项
            if hasattr(self.dynamics, 'fxx'):
                Qxx += self.dynamics.contract_tensor(Vx, self.dynamics.fxx(xk, uk))
                Quu += self.dynamics.contract_tensor(Vx, self.dynamics.fuu(xk, uk))
                Qux += self.dynamics.contract_tensor(Vx, self.dynamics.fux(xk, uk))

            try:
                Quu_inv = np.linalg.inv(Quu)
            except np.linalg.LinAlgError:
                return None, None, False

            # 候选活跃约束
            g_val = self.constraint.g(xk, uk)
            gx = self.constraint.gx(xk, uk)
            gu = self.constraint.gu(xk, uk)

            active_idx = np.where(g_val >= -self.eps_active)[0]

            if len(active_idx) == 0:
                k_k = -Quu_inv @ Qu
                K_k = -Quu_inv @ Qux
            else:
                C = gu[active_idx, :]
                D = -gx[active_idx, :]

                # 求解 dx = 0 标称乘子并过滤负乘子
                M = C @ Quu_inv @ C.T
                try:
                    lam = -np.linalg.solve(M, C @ Quu_inv @ Qu)
                except np.linalg.LinAlgError:
                    lam = np.zeros(len(active_idx))

                valid_mask = lam > 0
                if not np.any(valid_mask):
                    k_k = -Quu_inv @ Qu
                    K_k = -Quu_inv @ Qux
                else:
                    C_hat = C[valid_mask, :]
                    D_hat = D[valid_mask, :]
                    M_hat = C_hat @ Quu_inv @ C_hat.T
                    
                    W = np.linalg.solve(M_hat, C_hat @ Quu_inv)
                    H = Quu_inv @ (np.eye(self.nu) - C_hat.T @ W)

                    k_k = -H @ Qu
                    K_k = -H @ Qux + W.T @ D_hat

            Vx = Qx + K_k.T @ Qu + K_k.T @ Quu @ k_k + Qux.T @ k_k
            Vxx = Qxx + K_k.T @ Quu @ K_k + K_k.T @ Qux + Qux.T @ K_k
            Vxx = 0.5 * (Vxx + Vxx.T)

            k_seq[k] = k_k
            K_seq[k] = K_k

        return k_seq, K_seq, True

    def _forward_pass(self, x_nom, u_nom, k_seq, K_seq, current_cost):
        e = self.trust_region_init
        max_retries = 8

        for _ in range(max_retries):
            x_temp = np.zeros_like(x_nom)
            u_temp = np.zeros_like(u_nom)
            x_temp[0] = x_nom[0]
            rollout_feasible = True

            for k in range(self.N):
                dx = x_temp[k] - x_nom[k]
                
                # 真实阶段 QP：min 0.5 du^T Quu du + (Qu + Qux dx)^T du
                # s.t. gu du <= -g_val - gx dx,  -e <= du <= e
                gk = self.constraint.g(x_nom[k], u_nom[k])
                gx_k = self.constraint.gx(x_nom[k], u_nom[k])
                gu_k = self.constraint.gu(x_nom[k], u_nom[k])
                
                # 目标函数梯度在当前偏差处评估
                H_qp = self.cost.luu(x_nom[k], u_nom[k]) + self.mu2 * np.eye(self.nu)
                g_qp = self.cost.lu(x_nom[k], u_nom[k]) + self.cost.lux(x_nom[k], u_nom[k]) @ dx
                
                C_qp = gu_k
                d_bound = -gk - gx_k @ dx
                lower = -e * np.ones(self.nu)
                upper = e * np.ones(self.nu)

                delta_u, qp_ok = solve_stage_convex_qp(H_qp, g_qp, C_qp, d_bound, lower, upper)
                if not qp_ok:
                    rollout_feasible = False
                    break

                u_temp[k] = u_nom[k] + delta_u
                x_temp[k + 1] = self.dynamics.f(x_temp[k], u_temp[k])

                # 校验非线性约束是否满足
                if np.any(self.constraint.g(x_temp[k + 1], u_temp[k]) > 1e-4):
                    rollout_feasible = False
                    break

            if not rollout_feasible:
                e *= self.alpha_trust
                continue

            new_cost = self._calc_total_cost(x_temp, u_temp)
            if new_cost < current_cost:
                return x_temp, u_temp, new_cost, True
            else:
                return None, None, current_cost, False

        return None, None, current_cost, False

    def _calc_total_cost(self, x, u):
        total = self.cost.lf(x[self.N])
        for k in range(self.N):
            total += self.cost.l(x[k], u[k])
        return total

---

9. 现代约束 DDP 四大流派横评与 Chu 2026 几何 CDDP 论文升级范式

在轨迹优化演进史中，约束处理经历了数次深刻的范式转移。下表总结了四大主流约束 DDP 流派与我们在 Chu_2026_Geometric_CDDP_on_SE3 论文中确立的终极技术定位：

对比维度	Xie 2017 CDDP	Tassa 2014 Box-QP	Howell 2019 ALTRO	Jallet 2022 PROX-DDP	Chu 2026 (本课题范式)
约束类型覆盖	通用非线性不等式/等式	仅限控制输入盒式边界	通用非线性状态/输入约束	通用非线性状态/输入约束	空间非合作多物理硬约束
核心算法机制	反向解析灵敏度 + 前向阶段 QP	反向投影牛顿法 + 增益行截断	AL-iLQR 粗解 + 投影精修	近端主-对偶增广拉格朗日 (PDAL)	对角 AL + 几何走廊 + 速度包络
流形结构支持	❌ 仅限欧氏空间 ${\mathbb{R}}^n$	❌ 仅限欧氏空间 ${\mathbb{R}}^n$	⚠️ 误差四元数切空间投影	⚠️ 支持流形缩回	✅ 严格保李群 $\mathrm{SE}(3)$ DtL 步进
初值可行性依赖	❌ 必须提供严格可行初值	❌ 必须提供动力学初值	✅ 允许不可行初值启动	✅ 允许不可行初值启动	✅ 支持两点边值不可行几何初值
数值病态抵抗	依赖 LM 阻尼与盒式信赖域	极佳 (微秒级)	极佳 (平方根 QR 分解)	极致 (近端正则化抗奇异)	极致 (平方根 QR + NDOB 鲁棒管)
在航天交会中的局限	欧拉角万向节死锁，阶段 QP 易失配	无法处理视场角与太阳翼避碰	缺少非合作旋转伴随动力学显式推导	缺少航天近场制动包络硬约束证明	全闭环：几何保流形 + 鲁棒管不变性

对 Chu 2026 几何 CDDP 论文的三大终极技术反哺：

1. 批判性吸纳 Xie 2017 约束灵敏度反馈： Xie 2017 提出的反馈增益分解式 ${\mathbf{K}}_k=-\mathbf{H}{Q}_{\mathbf{u}\mathbf{x}}+{\mathbf{W}}^{\top }\hat{\mathbf{D}}$ 为流形约束反馈提供了极佳的几何直觉；
2. 摒弃前向阶段 QP，采用 ALTRO 两阶段求解范式： 彻底废弃 Xie 2017 容易导致策略失配与频繁信赖域回退的前向阶段 QP 滚动，采用 Howell 2019 的“增广拉格朗日粗解快速锁定活跃集 + 有效集牛顿投影高精度二次精修”；
3. 升华航天半直积物理约束： 摒弃平坦欧氏障碍物模型，严格建立空间非合作目标翻滚交会下的视场进近走廊锥、太阳翼防撞椭球与接触速度安全制动包络，完成现代几何约束 DDP 的理论闭环！

---

10. 权威参考文献与延伸阅读

1. Xie, Zhaoming, Liu, C. Karen, and Hauser, Kris. (2017). “Differential Dynamic Programming with Nonlinear Constraints.” In IEEE International Conference on Robotics and Automation (ICRA), pp. 695–702. (本文核心奠基文献)
2. Tassa, Yuval, Mansard, Nicolas, and Todorov, Emanuel. (2014). “Control-limited differential dynamic programming.” In IEEE International Conference on Robotics and Automation (ICRA), pp. 1168–1175.
3. Howell, Taylor A., Jackson, Brian E., and Manchester, Zachary. (2019). “ALTRO: A fast solver for constrained trajectory optimization.” In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 7674–7681.
4. Pavlov, Andrei, Shames, Iman, and Manzie, Chris. (2021). “Interior point differential dynamic programming.” IEEE Transactions on Robotics (T-RO), 37(6):2105–2120.
5. Jallet, Wilson, Mansard, Nicolas, and Todorov, Emanuel. (2022). “PROX-DDP: Proximal constrained differential dynamic programming.” In IEEE International Conference on Robotics and Automation (ICRA).
6. Boutselis, George I., and Theodorou, Evangelos. (2018). “Discrete-time Differential Dynamic Programming on Lie Groups: Derivation, Convergence Analysis and Numerical Results.” arXiv:1809.07883.
7. Alcan, Gokhan, Risiglione, Martino, Jackson, Brian E., and Howell, Taylor A. (2023). “Constrained differential dynamic programming on manifolds.” IEEE Transactions on Robotics (T-RO).
8. Kim, Beomjoon, Fan, David, and Choi, Jongeun. (2024). “MPPI-IPDDP: Real-time nonlinear model predictive control via sample-based and gradient-based optimization.” IEEE Robotics and Automation Letters (RA-L).