第8章：轨迹优化

章节概要

轨迹优化是机器人运动规划的核心技术，它将路径规划问题转化为数学优化问题，通过求解约束优化得到满足动力学、运动学和任务要求的最优轨迹。本章深入探讨三大主流轨迹优化方法：直接配点法、直接射击法和微分动态规划，以及时间最优轨迹生成技术。我们将从数学原理出发，分析各方法的优缺点、适用场景和实现细节，并通过无人机穿越动态障碍的案例展示轨迹优化在复杂场景中的应用。

学习目标

完成本章学习后，您将能够：

• 掌握轨迹优化问题的数学表述和求解框架
• 理解直接法与间接法的本质区别
• 实现基于配点法和射击法的轨迹优化算法
• 运用DDP解决非线性系统的轨迹优化问题
• 设计满足时间最优或能量最优等目标的轨迹
• 处理状态约束、控制约束和路径约束
• 评估不同方法的计算效率和数值稳定性

8.1 轨迹优化问题表述

8.1.1 基本问题定义

轨迹优化是机器人控制的核心优化问题，它寻求在满足系统动力学和各种约束的前提下，找到从初始状态到目标状态的最优轨迹。与传统的路径规划不同，轨迹优化同时考虑路径的几何特性和时间演化，生成动力学可行的运动轨迹。

轨迹优化的一般形式可以表述为：

$$ \begin{aligned} \min_{\mathbf{x}(\cdot), \mathbf{u}(\cdot)} \quad & J = \phi(\mathbf{x}(T)) + \int_0^T L(\mathbf{x}(t), \mathbf{u}(t), t) dt \\ \text{s.t.} \quad & \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t), t) \\ & \mathbf{x}(0) = \mathbf{x}_0 \\ & \mathbf{g}(\mathbf{x}(t), \mathbf{u}(t), t) \leq \mathbf{0} \\ & \mathbf{h}(\mathbf{x}(t), \mathbf{u}(t), t) = \mathbf{0} \end{aligned} $$

其中：

• $\mathbf{x}(t) \in \mathbb{R}^n$ 是状态轨迹，描述系统在配置空间中的演化
• $\mathbf{u}(t) \in \mathbb{R}^m$ 是控制轨迹，代表施加在系统上的输入
• $\phi(\cdot)$ 是终端代价（Mayer项），评估最终状态的质量
• $L(\cdot)$ 是运行代价（Lagrange项），积分形式的路径代价
• $f(\cdot)$ 是系统动力学，描述状态演化规律
• $\mathbf{g}(\cdot)$ 是不等式约束（如避障、关节限位、速度限制）
• $\mathbf{h}(\cdot)$ 是等式约束（如终端条件、路径约束）

这个问题的复杂性在于它是一个无穷维优化问题——我们需要在连续函数空间中搜索最优解。实际求解时，必须将其转化为有限维问题。

8.1.2 问题类型与特性

轨迹优化问题可以根据不同特性分类：

根据目标函数：

• Bolza问题：同时包含Mayer项和Lagrange项，最一般的形式
• Mayer问题：仅有终端代价 $J = \phi(\mathbf{x}(T))$
• Lagrange问题：仅有积分代价 $J = \int_0^T L(\mathbf{x}, \mathbf{u}) dt$

这三种形式在数学上是等价的，可以通过引入辅助状态变量相互转换。例如，Lagrange问题可以通过引入状态 $z(t) = \int_0^t L(\mathbf{x}, \mathbf{u}) d\tau$ 转换为Mayer问题。

根据边界条件：

• 固定端点：$\mathbf{x}(0) = \mathbf{x}_0$，$\mathbf{x}(T) = \mathbf{x}_f$
• 自由端点：终端状态无约束或在某个集合内
• 固定时间 vs 自由时间：终端时间T可以固定或作为优化变量

根据系统特性：

• 线性 vs 非线性：动力学方程的线性性
• 凸 vs 非凸：目标函数和约束集的凸性
• 确定性 vs 随机：是否考虑不确定性

8.1.3 离散化与转录

连续时间问题需要离散化才能数值求解。离散化将无穷维问题转换为有限维非线性规划（NLP）问题，这个过程称为转录（transcription）。

欧拉法（一阶精度）： $$\mathbf{x}_{k+1} = \mathbf{x}_k + h \cdot f(\mathbf{x}_k, \mathbf{u}_k, t_k)$$ 欧拉法是最简单的离散化方法，具有显式形式，但精度较低，需要较小的时间步长以保证精度。对于刚性系统可能产生数值不稳定。

中点法（二阶精度）： $$\mathbf{x}_{k+1} = \mathbf{x}_k + h \cdot f\left(\frac{\mathbf{x}_k + \mathbf{x}_{k+1}}{2}, \mathbf{u}_k, t_k + \frac{h}{2}\right)$$ 中点法是隐式方法，需要迭代求解，但具有更好的数值稳定性和精度。它保持了哈密顿系统的辛结构，适合长时间仿真。

Runge-Kutta方法： 经典的RK4方法提供四阶精度： $$\mathbf{x}_{k+1} = \mathbf{x}_k + \frac{h}{6}(k_1 + 2k_2 + 2k_3 + k_4)$$ 其中： $$\begin{aligned} k_1 &= f(\mathbf{x}_k, \mathbf{u}_k, t_k) \\ k_2 &= f(\mathbf{x}_k + \frac{h}{2}k_1, \mathbf{u}_k, t_k + \frac{h}{2}) \\ k_3 &= f(\mathbf{x}_k + \frac{h}{2}k_2, \mathbf{u}_k, t_k + \frac{h}{2}) \\ k_4 &= f(\mathbf{x}_k + hk_3, \mathbf{u}_k, t_k + h) \end{aligned}$$ Hermite-Simpson配点（三阶精度）： $$\mathbf{x}_{k+1} = \mathbf{x}_k + \frac{h}{6}[f_k + 4f_m + f_{k+1}]$$ 其中 $f_m = f(\mathbf{x}_m, \mathbf{u}_m, t_k + h/2)$，中点状态满足： $$\mathbf{x}_m = \frac{\mathbf{x}_k + \mathbf{x}_{k+1}}{2} + \frac{h}{8}[f_k - f_{k+1}]$$ 这个方法在直接配点法中广泛使用，因为它提供了良好的精度-复杂度平衡。

8.1.4 转录方法比较

不同的转录方法产生不同结构的NLP问题：

| 方法 | 变量数 | 约束数 | 精度 | 稳定性 | 适用场景 |

8.1.5 变分原理视角

从变分法的角度看，轨迹优化问题可以通过拉格朗日乘子法处理约束：

增广目标函数（哈密顿函数）： $$H(\mathbf{x}, \mathbf{u}, \boldsymbol{\lambda}, t) = L(\mathbf{x}, \mathbf{u}, t) + \boldsymbol{\lambda}^T f(\mathbf{x}, \mathbf{u}, t)$$ 其中 $\boldsymbol{\lambda}(t)$ 是共态变量（拉格朗日乘子）。

最优性必要条件（欧拉-拉格朗日方程）： $$\begin{aligned} \dot{\mathbf{x}} &= \frac{\partial H}{\partial \boldsymbol{\lambda}} = f(\mathbf{x}, \mathbf{u}, t) \quad \text{（状态方程）} \\ -\dot{\boldsymbol{\lambda}} &= \frac{\partial H}{\partial \mathbf{x}} = \frac{\partial L}{\partial \mathbf{x}} + \boldsymbol{\lambda}^T \frac{\partial f}{\partial \mathbf{x}} \quad \text{（共态方程）} \\ \mathbf{0} &= \frac{\partial H}{\partial \mathbf{u}} = \frac{\partial L}{\partial \mathbf{u}} + \boldsymbol{\lambda}^T \frac{\partial f}{\partial \mathbf{u}} \quad \text{（最优控制条件）} \end{aligned}$$ 这形成了一个两点边值问题（TPBVP），可以通过间接法求解，但数值困难促使人们更多使用直接法。

8.2 直接配点法（Direct Collocation）

8.2.1 方法原理

直接配点法是轨迹优化最成功的方法之一，它将连续轨迹优化问题转化为大规模但结构化的非线性规划（NLP）问题。与射击法不同，配点法同时将状态和控制作为优化变量，避免了数值积分的累积误差，提供了更好的数值稳定性。

核心思想包含三个关键步骤：

1. 时间离散化：将时间区间 $[0, T]$ 离散为 $N$ 个节点 $\{t_0, t_1, \ldots, t_N\}$
2. 参数化：在每个节点上同时优化状态 $\mathbf{x}_k$ 和控制 $\mathbf{u}_k$ 变量
3. 配点约束：使用数值积分规则确保轨迹满足动力学约束

决策变量向量的组织形式： $$\mathbf{z} = [\mathbf{x}_0^T, \mathbf{u}_0^T, \mathbf{x}_1^T, \mathbf{u}_1^T, \ldots, \mathbf{x}_N^T, \mathbf{u}_N^T]^T \in \mathbb{R}^{(N+1)(n+m)}$$ 这种参数化将原始的函数空间优化问题转化为有限维优化问题。配点法的名称来源于它在特定的配点（collocation points）上强制满足动力学约束。

8.2.2 梯形配点

梯形配点是最简单的配点方案之一，使用梯形积分规则离散化动力学约束。其基本思想是用梯形面积近似曲线下的积分。

动力学约束的离散化： $$\mathbf{x}_{k+1} = \mathbf{x}_k + \frac{h}{2}[f(\mathbf{x}_k, \mathbf{u}_k) + f(\mathbf{x}_{k+1}, \mathbf{u}_{k+1})]$$ 这是一个隐式约束，因为 $\mathbf{x}_{k+1}$ 同时出现在等式两边。这种隐式性质提供了更好的数值稳定性，特别是对于刚性系统。

完整的NLP问题表述： $$ \begin{aligned} \min_{\mathbf{z}} \quad & J = \sum_{k=0}^{N-1} \frac{h}{2}[L(\mathbf{x}_k, \mathbf{u}_k) + L(\mathbf{x}_{k+1}, \mathbf{u}_{k+1})] + \phi(\mathbf{x}_N) \\ \text{s.t.} \quad & \mathbf{x}_{k+1} - \mathbf{x}_k - \frac{h}{2}[f(\mathbf{x}_k, \mathbf{u}_k) + f(\mathbf{x}_{k+1}, \mathbf{u}_{k+1})] = \mathbf{0}, \quad k = 0, \ldots, N-1 \\ & \mathbf{x}_0 = \mathbf{x}_{\text{init}} \\ & \mathbf{g}_k(\mathbf{x}_k, \mathbf{u}_k) \leq \mathbf{0}, \quad k = 0, \ldots, N \\ & \mathbf{u}_{\min} \leq \mathbf{u}_k \leq \mathbf{u}_{\max} \end{aligned} $$ 注意目标函数中的积分也使用梯形规则离散化，保持了一致性。

梯形配点的特性：

• 精度：二阶精度，局部误差 $O(h^3)$，全局误差 $O(h^2)$
• 稳定性：A-稳定，适合刚性问题
• 守恒性：保持线性不变量（如动量守恒）
• 计算成本：每个时间段产生 $n$ 个等式约束

8.2.3 Hermite-Simpson配点

Hermite-Simpson配点通过在每个时间段中引入中点变量来提高精度。这种方法结合了Hermite插值和Simpson积分规则的优点。

扩展的变量结构：

时间段 [t_k, t_{k+1}]:
    节点: t_k -------- t_m -------- t_{k+1}
    状态: x_k          x_m          x_{k+1}  
    控制: u_k          u_m          u_{k+1}

每个时间段引入额外的中点变量 $(\mathbf{x}_m, \mathbf{u}_m)$，使得总变量数增加到约 $1.5(N+1)(n+m)$。

配点约束（Simpson积分）： $$\mathbf{x}_{k+1} = \mathbf{x}_k + \frac{h}{6}[f_k + 4f_m + f_{k+1}]$$ 中点一致性约束： $$\mathbf{x}_m = \frac{\mathbf{x}_k + \mathbf{x}_{k+1}}{2} + \frac{h}{8}[f_k - f_{k+1}]$$ 这个约束确保中点状态与端点状态通过三次Hermite插值一致。

控制参数化选择：

• 线性插值：$\mathbf{u}_m = \frac{\mathbf{u}_k + \mathbf{u}_{k+1}}{2}$（减少变量）
• 独立优化：$\mathbf{u}_m$ 作为自由变量（增加灵活性）

Hermite-Simpson的优势：

1. 高精度：三阶精度配点，四阶精度积分
2. 光滑性：生成 $C^1$ 连续的状态轨迹
3. 鲁棒性：对初始猜测不太敏感
4. 物理保真：更好地捕捉系统动力学特征

8.2.4 稀疏性结构与高效求解

直接配点法产生的NLP问题虽然规模大，但具有高度结构化的稀疏性，这是其计算可行性的关键。

雅可比矩阵的块结构：

约束雅可比矩阵 ∂c/∂z：
    时刻:  0      1      2     ...   N-1    N
         ┌─────┬─────┬─────┬─────┬─────┬─────┐
    动力 │ B₀  │ C₀  │     │     │     │     │  段0: x₁ = f(x₀,u₀,x₁,u₁)
    约束 │─────┼─────┼─────┼─────┼─────┼─────│
         │ A₁  │ B₁  │ C₁  │     │     │     │  段1: x₂ = f(x₁,u₁,x₂,u₂)
         │─────┼─────┼─────┼─────┼─────┼─────│
         │     │ A₂  │ B₂  │ C₂  │     │     │  段2: x₃ = f(x₂,u₂,x₃,u₃)
         │─────┼─────┼─────┼─────┼─────┼─────│
         │     │     │ ··· │ ··· │ ··· │     │
         │─────┼─────┼─────┼─────┼─────┼─────│
         │     │     │     │Aₙ₋₁ │Bₙ₋₁ │Cₙ₋₁ │  段N-1
         └─────┴─────┴─────┴─────┴─────┴─────┘

其中：

• $A_k = -\partial \mathbf{c}_k/\partial \mathbf{x}_k$：前一时刻状态的影响
• $B_k = \partial \mathbf{c}_k/\partial [\mathbf{x}_k^T, \mathbf{u}_k^T]^T$：当前时刻变量的影响
• $C_k = \partial \mathbf{c}_k/\partial [\mathbf{x}_{k+1}^T, \mathbf{u}_{k+1}^T]^T$：后一时刻变量的影响

Hessian矩阵的稀疏性： Hessian矩阵 $\nabla^2 \mathcal{L}$ 具有箭头结构：

• 块对角部分：来自局部代价和约束
• 边界耦合：来自动力学约束的二阶导数

利用稀疏性的求解策略：

1. 稀疏线性代数： - 使用专门的稀疏求解器（MA27、MA57、MUMPS） - 只存储和操作非零元素 - 复杂度从 $O(N^3n^3)$ 降至 $O(Nn^3)$

2. 块消元技术： - 利用块三对角结构进行递归消元 - 类似于Riccati递归，但适用于非线性情况 - 特别适合长时域问题

3. 并行化： - 约束雅可比计算完全并行 - 每个时间段的局部计算独立 - GPU加速矩阵运算

4. 预条件技术： - 使用问题结构构造预条件子 - 加速迭代求解器收敛 - 改善数值条件数

8.2.5 实施细节与数值技巧

初始猜测策略：

1. 线性插值：简单连接初始和目标状态
2. 前向仿真：使用简单控制策略积分
3. 逆运动学：对于机械臂等系统
4. 热启动：使用相似问题的解

约束规范化： 为改善数值条件，应当规范化所有约束： $$\tilde{\mathbf{c}}_k = \mathbf{D}_c^{-1} \mathbf{c}_k$$ 其中 $\mathbf{D}_c$ 是特征尺度对角矩阵。

自适应网格细化： 在轨迹变化剧烈的区域自动加密网格：

• 监控配点误差估计
• 使用二分法局部细化
• 保持相邻段的平滑过渡

数值微分 vs 自动微分：

• 有限差分：简单但有截断误差
• 复步微分：高精度但需要复数运算
• 自动微分：精确且高效，推荐使用

8.3 直接射击法（Direct Shooting）

8.3.1 方法原理

直接射击法仅将控制序列作为优化变量，通过前向积分动力学获得状态轨迹： $$\mathbf{x}_{k+1} = \Phi(\mathbf{x}_k, \mathbf{u}_k)$$ 其中 $\Phi$ 是积分算子（如RK4）。

单射击（Single Shooting）：

• 决策变量：$\mathbf{z} = [\mathbf{u}_0^T, \mathbf{u}_1^T, \ldots, \mathbf{u}_{N-1}^T]^T$
• 优点：变量少，无需处理动力学约束
• 缺点：对初始猜测敏感，数值不稳定

8.3.2 多重射击（Multiple Shooting）

多重射击在多个时间点引入状态变量，结合了配点法和射击法的优点：

1. 将轨迹分为M段
2. 每段起点的状态作为决策变量
3. 段内使用前向积分
4. 段间添加连续性约束

决策变量： $$\mathbf{z} = [\mathbf{s}_0^T, \mathbf{u}_{[0]}^T, \mathbf{s}_1^T, \mathbf{u}_{[1]}^T, \ldots, \mathbf{s}_M^T]^T$$ 连续性约束： $$\mathbf{s}_{i+1} = \Phi_i(\mathbf{s}_i, \mathbf{u}_{[i]})$$

8.3.3 敏感度分析

计算目标函数和约束对控制变量的梯度需要敏感度分析：

前向敏感度： $$\frac{\partial \mathbf{x}_{k+1}}{\partial \mathbf{u}_j} = \frac{\partial f}{\partial \mathbf{x}}\bigg|_k \frac{\partial \mathbf{x}_k}{\partial \mathbf{u}_j} + \delta_{kj}\frac{\partial f}{\partial \mathbf{u}}\bigg|_k$$ 伴随法（Adjoint Method）： 通过反向积分伴随方程计算梯度，内存效率更高。

8.4 微分动态规划（DDP）

8.4.1 动态规划原理

DDP基于贝尔曼最优性原理，通过局部二次近似求解非线性最优控制问题。

值函数定义： $$V_k(\mathbf{x}_k) = \min_{\mathbf{u}_{k:N-1}} \left[ \sum_{i=k}^{N-1} L_i(\mathbf{x}_i, \mathbf{u}_i) + V_N(\mathbf{x}_N) \right]$$ 贝尔曼方程： $$V_k(\mathbf{x}_k) = \min_{\mathbf{u}_k} \left[ L_k(\mathbf{x}_k, \mathbf{u}_k) + V_{k+1}(f(\mathbf{x}_k, \mathbf{u}_k)) \right]$$

8.4.2 二次近似

在标称轨迹 $(\bar{\mathbf{x}}, \bar{\mathbf{u}})$ 附近进行二次展开：

状态-动作值函数： $$Q_k(\delta\mathbf{x}, \delta\mathbf{u}) = L_k + V_{k+1} \approx \text{const} + \begin{bmatrix} \delta\mathbf{x} \\ \delta\mathbf{u} \end{bmatrix}^T \begin{bmatrix} Q_x \\ Q_u \end{bmatrix} + \frac{1}{2}\begin{bmatrix} \delta\mathbf{x} \\ \delta\mathbf{u} \end{bmatrix}^T \begin{bmatrix} Q_{xx} & Q_{xu} \\ Q_{ux} & Q_{uu} \end{bmatrix} \begin{bmatrix} \delta\mathbf{x} \\ \delta\mathbf{u} \end{bmatrix}$$ 其中： $$ \begin{aligned} Q_x &= L_x + f_x^T V'_x \\ Q_u &= L_u + f_u^T V'_x \\ Q_{xx} &= L_{xx} + f_x^T V'_{xx} f_x + V'_x \cdot f_{xx} \\ Q_{uu} &= L_{uu} + f_u^T V'_{xx} f_u + V'_x \cdot f_{uu} \\ Q_{ux} &= L_{ux} + f_u^T V'_{xx} f_x + V'_x \cdot f_{ux} \end{aligned} $$

8.4.3 反向传播

反向过程：从终端时刻向初始时刻计算最优控制增量和值函数近似

控制增量： $$\delta\mathbf{u}_k^* = -Q_{uu}^{-1}(Q_u + Q_{ux}\delta\mathbf{x}_k) = \mathbf{k}_k + \mathbf{K}_k\delta\mathbf{x}_k$$ 值函数更新： $$ \begin{aligned} V_x &= Q_x - Q_u^T Q_{uu}^{-1} Q_{ux} \\ V_{xx} &= Q_{xx} - Q_{xu}^T Q_{uu}^{-1} Q_{ux} \end{aligned} $$

8.4.4 前向传播与线搜索

前向过程：使用计算出的控制策略更新轨迹 $$ \begin{aligned} \mathbf{u}_k &= \bar{\mathbf{u}}_k + \alpha \mathbf{k}_k + \mathbf{K}_k(\mathbf{x}_k - \bar{\mathbf{x}}_k) \\ \mathbf{x}_{k+1} &= f(\mathbf{x}_k, \mathbf{u}_k) \end{aligned} $$ 线搜索确定步长 $\alpha \in (0, 1]$ 以保证收敛。

8.4.5 iLQR变体

迭代线性二次调节器（iLQR）是DDP的一阶近似版本，忽略动力学的二阶项： $$Q_{xx} = L_{xx} + f_x^T V'_{xx} f_x$$ $$Q_{uu} = L_{uu} + f_u^T V'_{xx} f_u$$ $$Q_{ux} = L_{ux} + f_u^T V'_{xx} f_x$$ 虽然收敛速度较慢，但计算更稳定，适用于高维系统。

8.5 时间最优轨迹生成

8.5.1 时间最优问题表述

时间最优控制问题寻找最短时间内完成任务的轨迹： $$ \begin{aligned} \min_{\mathbf{u}(\cdot), T} \quad & T \\ \text{s.t.} \quad & \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)) \\ & \mathbf{x}(0) = \mathbf{x}_0, \quad \mathbf{x}(T) = \mathbf{x}_f \\ & |\mathbf{u}(t)| \leq \mathbf{u}_{\max} \end{aligned} $$

8.5.2 庞特里亚金最大值原理

根据最大值原理，时间最优控制通常是bang-bang控制： $$u_i(t) = \begin{cases} u_{i,\max} & \text{if } \lambda_i(t) > 0 \\ u_{i,\min} & \text{if } \lambda_i(t) < 0 \\ \text{singular} & \text{if } \lambda_i(t) = 0 \end{cases}$$ 其中 $\lambda$ 是共态变量。

8.5.3 路径-速度分解

对于机械系统，可将时间最优问题分解为：

1. 路径规划：生成几何路径 $\mathbf{q}(s)$
2. 速度优化：沿路径的时间最优速度剖面

速度优化问题： $$ \begin{aligned} \min_{b(s)} \quad & \int_0^{s_f} \frac{1}{\sqrt{2b(s)}} ds \\ \text{s.t.} \quad & \dot{b}(s) = a(s) \\ & a_{\min}(s, b) \leq a(s) \leq a_{\max}(s, b) \end{aligned} $$ 其中 $b(s) = \dot{s}^2$ 是伪速度的平方。

8.5.4 凸化方法

通过变量替换可以将时间最优问题凸化：

令 $\tau = 1/T$ 为时间缩放因子，原问题变为： $$ \begin{aligned} \max_{\mathbf{u}(\cdot), \tau} \quad & \tau \\ \text{s.t.} \quad & \mathbf{x}'(s) = \frac{1}{\tau}f(\mathbf{x}(s), \mathbf{u}(s)) \\ & \tau \in [0, \tau_{\max}] \end{aligned} $$ 通过适当的松弛和线性化，可以得到凸优化子问题。

8.6 约束处理技术

8.6.1 状态约束

状态约束 $\mathbf{g}(\mathbf{x}) \leq \mathbf{0}$ 的处理方法：

硬约束：直接在NLP中添加约束

• 优点：严格满足约束
• 缺点：可能导致不可行

软约束：通过罚函数松弛 $$L_{\text{aug}} = L + \rho \cdot \max(0, g(\mathbf{x}))^2$$ 障碍函数： $$L_{\text{barrier}} = L - \mu \sum_i \log(-g_i(\mathbf{x}))$$

8.6.2 控制约束

控制饱和约束的处理：

投影方法： $$\mathbf{u} = \text{proj}_{[\mathbf{u}_{\min}, \mathbf{u}_{\max}]}(\mathbf{u}_{\text{unconstrained}})$$ 平滑近似： 使用双曲正切或sigmoid函数平滑化： $$u = u_{\max} \cdot \tanh(v)$$

8.6.3 路径约束

时间相关的路径约束需要特殊处理：

积分约束： $$\int_0^T g(\mathbf{x}(t), \mathbf{u}(t)) dt \leq C$$ 转化为状态扩增： $$\dot{z} = g(\mathbf{x}, \mathbf{u}), \quad z(0) = 0, \quad z(T) \leq C$$

8.7 数值优化算法

8.7.1 序列二次规划（SQP）

SQP是求解轨迹优化NLP的主流方法：

1. 在当前点线性化约束，二次近似目标
2. 求解QP子问题获得搜索方向
3. 线搜索确定步长
4. 更新变量和拉格朗日乘子

QP子问题： $$ \begin{aligned} \min_{\Delta \mathbf{z}} \quad & \nabla J^T \Delta \mathbf{z} + \frac{1}{2}\Delta \mathbf{z}^T \mathbf{H} \Delta \mathbf{z} \\ \text{s.t.} \quad & \mathbf{A}\Delta \mathbf{z} + \mathbf{c} = \mathbf{0} \\ & \mathbf{G}\Delta \mathbf{z} + \mathbf{g} \leq \mathbf{0} \end{aligned} $$

8.7.2 内点法

内点法通过障碍函数将不等式约束转化为目标函数： $$L_{\mu} = J(\mathbf{z}) - \mu \sum_i \log(s_i)$$ 其中 $s_i$ 是松弛变量。通过逐渐减小 $\mu$ 逼近原问题解。

8.7.3 增广拉格朗日方法

结合罚函数和拉格朗日乘子： $$\mathcal{L}_{\rho} = J + \lambda^T \mathbf{c} + \frac{\rho}{2}|\mathbf{c}|^2$$ 交替优化原始变量和对偶变量：

1. 固定 $\lambda$，最小化 $\mathcal{L}_{\rho}$ 关于 $\mathbf{z}$
2. 更新乘子：$\lambda \leftarrow \lambda + \rho \mathbf{c}$
3. 可选：增加罚参数 $\rho$

8.8 案例研究：无人机穿越动态障碍

8.8.1 问题设定

考虑四旋翼无人机在动态环境中的轨迹规划：

系统模型： $$ \begin{aligned} \dot{\mathbf{p}} &= \mathbf{v} \\ \dot{\mathbf{v}} &= \mathbf{g} + \frac{1}{m}\mathbf{R}(\boldsymbol{\theta})\begin{bmatrix} 0 \\ 0 \\ u_T \end{bmatrix} \\ \dot{\boldsymbol{\theta}} &= \boldsymbol{\omega} \\ \mathbf{J}\dot{\boldsymbol{\omega}} &= \boldsymbol{\tau} - \boldsymbol{\omega} \times \mathbf{J}\boldsymbol{\omega} \end{aligned} $$ 动态障碍： 移动障碍物位置：$\mathbf{o}_i(t) = \mathbf{o}_{i,0} + \mathbf{v}_{o,i} \cdot t$

避障约束： $$|\mathbf{p}(t) - \mathbf{o}_i(t)| \geq r_{\text{safe}}$$

8.8.2 轨迹优化方案

使用直接配点法with时间优化：

决策变量：

• 状态序列：$\{\mathbf{p}_k, \mathbf{v}_k, \boldsymbol{\theta}_k, \boldsymbol{\omega}_k\}_{k=0}^N$
• 控制序列：$\{u_{T,k}, \boldsymbol{\tau}_k\}_{k=0}^{N-1}$
• 时间间隔：$\{h_k\}_{k=0}^{N-1}$

目标函数： $$J = w_T \sum_{k} h_k + w_u \sum_{k} h_k |\mathbf{u}_k|^2 + w_{\text{smooth}} \sum_{k} |\mathbf{u}_{k+1} - \mathbf{u}_k|^2$$

8.8.3 实时重规划

采用模型预测控制框架实现实时避障：

算法：实时轨迹重规划

1. 初始化：计算初始轨迹 τ₀
2. While 未到达目标:
   a. 感知：更新障碍物状态预测
   b. 热启动：基于当前轨迹生成初始猜测
   c. 优化：求解有限时域轨迹优化
   d. 执行：跟踪优化轨迹的第一段
   e. 滑动时域窗口

8.8.4 性能优化

计算加速技巧：

1. 稀疏性利用：使用稀疏线性求解器（如MA57）
2. 并行化：约束雅可比计算可并行
3. 自适应网格：在关键区域加密网格
4. 热启动：利用上一时刻解初始化

鲁棒性增强：

• 障碍物速度不确定性：使用鲁棒优化或机会约束
• 模型误差：添加安全边界
• 计算延迟：预测补偿

本章小结

本章系统介绍了轨迹优化的核心理论与实践方法：

关键概念

1. 问题表述：将轨迹规划转化为约束优化问题，包含动力学约束、边界条件和路径约束
2. 直接配点法：同时优化状态和控制，利用稀疏结构高效求解大规模NLP
3. 直接射击法：仅优化控制变量，通过积分获得状态，适合低维控制空间
4. 微分动态规划：基于动态规划原理的局部迭代优化，适合非线性系统
5. 时间最优控制：通过庞特里亚金原理或路径-速度分解求解最短时间轨迹

核心公式

• 轨迹优化基本问题： $$\min J = \phi(\mathbf{x}(T)) + \int_0^T L(\mathbf{x}, \mathbf{u}) dt$$

• DDP控制更新： $$\delta\mathbf{u}^* = -Q_{uu}^{-1}(Q_u + Q_{ux}\delta\mathbf{x})$$

• 时间最优bang-bang控制： $$u_i = \text{sign}(\lambda_i) \cdot u_{i,\max}$$

方法对比

| 方法 | 优点 | 缺点 | 适用场景 |

实践要点

• 选择合适的离散化方法平衡精度与效率
• 利用问题结构（稀疏性、分解性）加速求解
• 通过热启动、并行化提升实时性能
• 结合MPC框架实现闭环轨迹优化

练习题

基础题（理解概念）

习题8.1 比较欧拉法、梯形法和Hermite-Simpson法的局部截断误差阶数。对于时间步长$h$，分析各方法的精度。

习题8.2 证明时间最优控制问题中，对于双积分系统$\ddot{x} = u$，$|u| \leq 1$，最优控制必为bang-bang形式。

习题8.3 解释为什么直接配点法的NLP问题具有块三对角结构，以及如何利用这种结构加速求解。

习题8.4 在DDP算法中，为什么需要正则化项$\mu I$添加到$Q_{uu}$？这如何影响收敛性？

挑战题（深入思考）

习题8.5 设计一个算法，将非凸避障约束$|\mathbf{p} - \mathbf{o}| \geq r$转化为凸约束序列，并证明收敛性。

习题8.6 对于欠驱动系统（如倒立摆），比较直接配点法和DDP的性能。分析各自的优势和局限。

习题8.7 推导时间和燃料最优的多目标轨迹优化问题，设计帕累托前沿的计算方法。

算法：

1. 求解纯时间最优：得到T_min, E_max
2. 求解纯燃料最优：得到T_max, E_min
3. For α = linspace(0, 1, N):
   - 求解加权问题
   - 记录(T*, E*)
4. 去除被支配解
5. 拟合帕累托曲线

习题8.8 设计一个自适应网格细化算法，在轨迹曲率大或接近约束边界时自动加密时间网格。

算法：自适应时间网格
输入：初始网格 t₀ < t₁ < ... < tₙ
输出：细化网格

1. 求解初始网格上的轨迹优化
2. For each segment [tₖ, tₖ₊₁]:
   a. 计算误差指标：

      - 动力学残差：||x_mid - Φ(xₖ, uₖ, h/2)||
      - 约束裕度：min(g(x_mid))
      - 控制变化：||uₖ₊₁ - uₖ||
   b. 综合指标：εₖ = w₁·残差 + w₂/裕度 + w₃·变化

3. If εₖ > ε_threshold:
   - 插入中点：t_new = (tₖ + tₖ₊₁)/2
   - 插值状态和控制作为初始猜测
4. 重新求解细化后的问题
5. 迭代直到所有εₖ < ε_threshold

常见陷阱与调试技巧

陷阱1：初始猜测不当

问题：随机初始化导致不收敛或收敛到差解 解决：

• 使用简单轨迹（直线、多项式）初始化
• 逐步增加约束复杂度（continuation方法）
• 从相似问题的解开始（warm start）

陷阱2：数值病态

问题：变量尺度差异大导致数值误差 解决：

• 归一化状态和控制变量
• 时间归一化到[0, 1]
• 使用自动微分避免数值微分误差

陷阱3：约束冲突

问题：过约束导致无可行解 解决：

• 检查约束相容性
• 使用软约束松弛
• 分层优化（先可行后最优）

陷阱4：局部最优

问题：陷入次优解，特别是非凸问题 解决：

• 多起点优化
• 使用全局优化方法初始化
• 添加探索噪声

陷阱5：实时性能

问题：计算时间超过控制周期 解决：

• 降低问题规模（减少节点数）
• 使用快速近似求解器
• 实现增量优化（只优化变化部分）

调试技巧

1. 可视化检查： - 绘制轨迹、控制和约束 - 检查违反约束的时刻 - 观察优化迭代过程

2. 渐进式调试： - 先解无约束问题 - 逐步添加约束 - 从短时域到长时域

3. 数值检验： - 验证梯度计算（有限差分对比） - 检查KKT条件 - 监控条件数

4. 性能分析： - 分析时间瓶颈 - 检查内存使用 - 评估并行化效果