轨迹优化是机器人运动规划的核心技术，它将路径规划问题转化为数学优化问题，通过求解约束优化得到满足动力学、运动学和任务要求的最优轨迹。本章深入探讨三大主流轨迹优化方法：直接配点法、直接射击法和微分动态规划，以及时间最优轨迹生成技术。我们将从数学原理出发，分析各方法的优缺点、适用场景和实现细节，并通过无人机穿越动态障碍的案例展示轨迹优化在复杂场景中的应用。
完成本章学习后，您将能够：
轨迹优化是机器人控制的核心优化问题，它寻求在满足系统动力学和各种约束的前提下，找到从初始状态到目标状态的最优轨迹。与传统的路径规划不同，轨迹优化同时考虑路径的几何特性和时间演化，生成动力学可行的运动轨迹。
轨迹优化的一般形式可以表述为：
\[\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}\]其中：
这个问题的复杂性在于它是一个无穷维优化问题——我们需要在连续函数空间中搜索最优解。实际求解时，必须将其转化为有限维问题。
轨迹优化问题可以根据不同特性分类：
根据目标函数：
这三种形式在数学上是等价的，可以通过引入辅助状态变量相互转换。例如，Lagrange问题可以通过引入状态 $z(t) = \int_0^t L(\mathbf{x}, \mathbf{u}) d\tau$ 转换为Mayer问题。
根据边界条件：
根据系统特性：
连续时间问题需要离散化才能数值求解。离散化将无穷维问题转换为有限维非线性规划（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}]\)
这个方法在直接配点法中广泛使用，因为它提供了良好的精度-复杂度平衡。
不同的转录方法产生不同结构的NLP问题：
| 方法 | 变量数 | 约束数 | 精度 | 稳定性 | 适用场景 |
|---|---|---|---|---|---|
| 显式欧拉 | 少 | 少 | $O(h)$ | 条件稳定 | 简单系统、快速原型 |
| 隐式欧拉 | 中 | 中 | $O(h)$ | 无条件稳定 | 刚性系统 |
| 梯形法 | 中 | 中 | $O(h^2)$ | A-稳定 | 一般非线性系统 |
| RK4 | 少 | 少 | $O(h^4)$ | 条件稳定 | 高精度需求 |
| Hermite-Simpson | 多 | 多 | $O(h^3)$ | 良好 | 直接配点优化 |
从变分法的角度看，轨迹优化问题可以通过拉格朗日乘子法处理约束：
增广目标函数（哈密顿函数）： \(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），可以通过间接法求解，但数值困难促使人们更多使用直接法。
直接配点法是轨迹优化最成功的方法之一，它将连续轨迹优化问题转化为大规模但结构化的非线性规划（NLP）问题。与射击法不同，配点法同时将状态和控制作为优化变量，避免了数值积分的累积误差，提供了更好的数值稳定性。
核心思想包含三个关键步骤：
决策变量向量的组织形式： \(\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）上强制满足动力学约束。
梯形配点是最简单的配点方案之一，使用梯形积分规则离散化动力学约束。其基本思想是用梯形面积近似曲线下的积分。
动力学约束的离散化： \(\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}\)
注意目标函数中的积分也使用梯形规则离散化，保持了一致性。
梯形配点的特性：
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插值一致。
控制参数化选择：
Hermite-Simpson的优势：
直接配点法产生的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
└─────┴─────┴─────┴─────┴─────┴─────┘
其中：
Hessian矩阵的稀疏性： Hessian矩阵 $\nabla^2 \mathcal{L}$ 具有箭头结构：
利用稀疏性的求解策略：
初始猜测策略：
约束规范化： 为改善数值条件，应当规范化所有约束： \(\tilde{\mathbf{c}}_k = \mathbf{D}_c^{-1} \mathbf{c}_k\) 其中 $\mathbf{D}_c$ 是特征尺度对角矩阵。
自适应网格细化： 在轨迹变化剧烈的区域自动加密网格：
数值微分 vs 自动微分：
直接射击法仅将控制序列作为优化变量，通过前向积分动力学获得状态轨迹：
\[\mathbf{x}_{k+1} = \Phi(\mathbf{x}_k, \mathbf{u}_k)\]其中 $\Phi$ 是积分算子（如RK4）。
单射击（Single Shooting）：
多重射击在多个时间点引入状态变量，结合了配点法和射击法的优点：
决策变量： \(\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]})\)
计算目标函数和约束对控制变量的梯度需要敏感度分析：
前向敏感度： \(\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）： 通过反向积分伴随方程计算梯度，内存效率更高。
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]\)
在标称轨迹 $(\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}\)
反向过程：从终端时刻向初始时刻计算最优控制增量和值函数近似
控制增量： \(\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}\)
前向过程：使用计算出的控制策略更新轨迹
\[\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]$ 以保证收敛。
迭代线性二次调节器（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\)
虽然收敛速度较慢，但计算更稳定，适用于高维系统。
时间最优控制问题寻找最短时间内完成任务的轨迹：
\[\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}\]根据最大值原理，时间最优控制通常是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$ 是共态变量。
对于机械系统，可将时间最优问题分解为：
速度优化问题： \(\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$ 是伪速度的平方。
通过变量替换可以将时间最优问题凸化：
令 $\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}\)
通过适当的松弛和线性化，可以得到凸优化子问题。
状态约束 $\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}))\)
控制饱和约束的处理：
投影方法： \(\mathbf{u} = \text{proj}_{[\mathbf{u}_{\min}, \mathbf{u}_{\max}]}(\mathbf{u}_{\text{unconstrained}})\)
平滑近似： 使用双曲正切或sigmoid函数平滑化： \(u = u_{\max} \cdot \tanh(v)\)
时间相关的路径约束需要特殊处理：
积分约束： \(\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\)
SQP是求解轨迹优化NLP的主流方法：
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}\)
内点法通过障碍函数将不等式约束转化为目标函数：
\[L_{\mu} = J(\mathbf{z}) - \mu \sum_i \log(s_i)\]其中 $s_i$ 是松弛变量。通过逐渐减小 $\mu$ 逼近原问题解。
结合罚函数和拉格朗日乘子：
\[\mathcal{L}_{\rho} = J + \lambda^T \mathbf{c} + \frac{\rho}{2}\|\mathbf{c}\|^2\]交替优化原始变量和对偶变量：
考虑四旋翼无人机在动态环境中的轨迹规划：
系统模型： \(\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}}\)
使用直接配点法with时间优化：
决策变量：
目标函数： \(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\)
采用模型预测控制框架实现实时避障：
算法：实时轨迹重规划
1. 初始化：计算初始轨迹 τ₀
2. While 未到达目标:
a. 感知：更新障碍物状态预测
b. 热启动：基于当前轨迹生成初始猜测
c. 优化：求解有限时域轨迹优化
d. 执行：跟踪优化轨迹的第一段
e. 滑动时域窗口
计算加速技巧：
鲁棒性增强：
本章系统介绍了轨迹优化的核心理论与实践方法：
轨迹优化基本问题： \(\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}\)
| 方法 | 优点 | 缺点 | 适用场景 | |——|——|——|———-| | 直接配点 | 稀疏结构、数值稳定 | 变量多、内存需求大 | 复杂约束、长时域 | | 直接射击 | 变量少、精度高 | 敏感性高、初值依赖 | 短时域、简单约束 | | DDP/iLQR | 快速收敛、处理非线性 | 局部最优、约束处理难 | 非线性系统、实时控制 | | 时间最优 | 时间最短 | 控制不连续、实现难 | 快速机动、紧急避障 |
习题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 推导时间和燃料最优的多目标轨迹优化问题，设计帕累托前沿的计算方法。
习题8.8 设计一个自适应网格细化算法，在轨迹曲率大或接近约束边界时自动加密时间网格。
问题：随机初始化导致不收敛或收敛到差解 解决：
问题：变量尺度差异大导致数值误差 解决：
问题：过约束导致无可行解 解决：
问题：陷入次优解，特别是非凸问题 解决：
问题：计算时间超过控制周期 解决：