最优化方法在轨迹优化与路径优化中的应用：综述
摘要
轨迹优化（Trajectory Optimization）与路径规划（Path Planning）是自主系统、机器人和航空航天领域的核心问题，旨在满足运动约束的条件下求解最优运动方案。本文系统梳理了互联网上高质量的专业资源，深入分析线性规划（LP）、混合整数线性规划（MILP）、二次规划（QP）、序列二次规划（SQP）、非线性规划（NLP）、凸优化、动态规划（DP）及各类元启发式算法（遗传算法GA、粒子群优化PSO等）在轨迹与路径优化中的具体应用。通过对 MIT、Stanford、Berkeley 等顶尖学术机构教程及工业界开源项目的综合分析，本文阐述了各类方法的数学原理、实现机制、优缺点对比及典型应用案例，旨在为研究者和工程师提供兼具学术深度与实践指导价值的参考文档。
关键词：轨迹优化；路径规划；最优化方法；凸优化；模型预测控制；进化算法
目录
- 引言
- 最优化方法分类与数学基础
- 线性规划与整数规划方法
- 非线性规划与序列二次规划
- 凸优化方法
- 二次规划方法
- 动态规划与图搜索方法
- 元启发式算法
- 曲线优化与几何方法
- 强化学习方法
- 非线性规划与直接法
- 序列凸规划与微分动态规划
- 方法对比与选型指南
- 典型案例分析
- 总结与展望
1. 引言
1.1 问题背景
路径规划是指寻找一条从起点到终点的无碰撞几何路径的过程；轨迹规划则进一步引入时间参数，为已规划出的路径分配速度、加速度等动力学信息。两者构成了自主系统运动规划的两个基本层次------前者解决"去哪儿"的问题，后者解决"怎么去"的问题。
在复杂环境中进行轨迹优化和路径规划面临多重挑战：高维状态空间的搜索难度、运动学与动力学约束的耦合、多目标权衡、不确定环境下的实时性与安全性需求等。这些问题本质上构成了一类多约束、非线性、非凸的优化问题。
1.2 最优化方法的分类体系
本综述采用以下分类框架来组织轨迹优化与路径规划中的最优化方法：
| 类别 | 代表性方法 | 典型特征 |
|---|---|---|
| 经典数学规划 | LP、MILP、QP、SQP | 理论完备，求解可靠 |
| 非线性/凸优化 | NLP、GCS、SCP | 适应非线性问题，全局/局部最优 |
| 动态规划 | DP、DDP | 多阶段决策，时间最优 |
| 元启发式算法 | GA、PSO、DE、ACO | 全局搜索，适应黑箱问题 |
| 强化学习 | DRL、RL-with-Guidance | 自适应，在线规划 |
| 曲线与几何方法 | Bézier、B-spline、Dubins | 平滑性与几何最优性 |
| 收敛性/梯度优化 | Gradient-based methods | 局部快速收敛 |
各类方法在不同场景下各有优势，也各有局限。以下章节将对每类方法逐一展开详细讨论。
2. 最优化方法分类与数学基础
2.1 轨迹优化问题的标准数学表述
轨迹优化的核心问题可表述为：给定系统动力学 x ˙ = f ( x , u , t ) \dot{x} = f(x, u, t) x˙=f(x,u,t)，寻找控制轨迹 u ( t ) u(t) u(t) 和状态轨迹 x ( t ) x(t) x(t)，使得性能指标最小化：
min x ( ⋅ ) , u ( ⋅ ) J = φ ( x ( t f ) , t f ) + ∫ t 0 t f L ( x ( t ) , u ( t ) , t ) d t \min_{x(\cdot), u(\cdot)} J = \varphi(x(t_f), t_f) + \int_{t_0}^{t_f} L(x(t), u(t), t) dt x(⋅),u(⋅)minJ=φ(x(tf),tf)+∫t0tfL(x(t),u(t),t)dt
约束条件包括：
- 动力学约束 : x ˙ ( t ) = f ( x ( t ) , u ( t ) , t ) \dot{x}(t) = f(x(t), u(t), t) x˙(t)=f(x(t),u(t),t)
- 路径约束 : h ( x ( t ) , u ( t ) ) ≤ 0 h(x(t), u(t)) \leq 0 h(x(t),u(t))≤0
- 边界约束 : ψ ( x ( t 0 ) , x ( t f ) ) = 0 \psi(x(t_0), x(t_f)) = 0 ψ(x(t0),x(tf))=0
- 控制约束 : u min ≤ u ( t ) ≤ u max u_{\min} \leq u(t) \leq u_{\max} umin≤u(t)≤umax
2.2 方法分类体系
最优化方法在轨迹/路径优化中的应用
├── 离散/组合方法
│ ├── 动态规划 (Dynamic Programming)
│ ├── 图搜索算法 (A*, Dijkstra, D*)
│ └── 混合整数规划 (MILP, MIQP)
├── 连续优化方法
│ ├── 凸优化
│ │ ├── 线性规划 (LP)
│ │ ├── 二次规划 (QP)
│ │ └── 二阶锥规划 (SOCP)
│ └── 非凸优化
│ ├── 非线性规划 (NLP)
│ ├── 序列凸规划 (SCP)
│ └── 微分动态规划 (DDP/iLQR)
└── 混合方法
├── 采样+优化 (RRT* + 优化)
├── 搜索+优化 (Lattice + QP)
└── 图+凸集 (GCS)3. 线性规划与整数规划方法
3.1 线性规划 (LP) 在轨迹优化中的应用
3.1.1 数学基础
线性规划（LP） 的一般形式为：
min x c T x s.t. A x ≤ b , x ≥ 0 \min_x \; c^T x \quad \text{s.t.} \quad Ax \leq b, \; x \geq 0 xmincTxs.t.Ax≤b,x≥0
线性规划的标准形式：
min x c T x \min_{\mathbf{x}} \mathbf{c}^T \mathbf{x} xmincTx
s.t. A x ≤ b , A e q x = b e q \text{s.t. } A\mathbf{x} \leq \mathbf{b}, \quad A_{eq}\mathbf{x} = \mathbf{b}_{eq} s.t. Ax≤b,Aeqx=beq
3.1.2 应用场景：数据驱动的时间最优轨迹
根据 arXiv 论文 Minimum-Time Trajectory Optimization With Data-Based Models: A Linear Programming Approach，LP 可用于端到端数据驱动的时间最优规划：
核心思想: 利用指数加权方案将最小时间轨迹规划重构为双层优化，最终形式为线性规划。
优势:
- 计算效率高，适合实时应用
- 无需精确的动力学模型
- 可利用数据驱动方法处理复杂系统
局限:
- 只能处理线性目标与约束
- 难以直接处理碰撞避免等非线性约束
3.2 混合整数线性规划 (MILP) 在路径规划中的应用
3.2.1 数学形式
当决策变量中同时包含连续变量和整数变量时，问题演变为混合整数线性规划（MILP）：
min x , z c T x + d T z s.t. A x + B z ≤ b , x ∈ R n , z ∈ Z m \min_{x, z} \; c^T x + d^T z \quad \text{s.t.} \quad Ax + Bz \leq b, \; x \in \mathbb{R}^n, \; z \in \mathbb{Z}^m x,zmincTx+dTzs.t.Ax+Bz≤b,x∈Rn,z∈Zm
在路径规划领域，整数变量通常用于编码离散选择------例如障碍物避碰的"左绕/右绕"选择、路径段的离散选择，或目标点之间的逻辑时序约束。
3.2.2 应用场景
无人机物流路径规划：有研究者提出了基于MILP的概率约束路径规划方法，将定位不确定性转化为二元约束，利用Gurobi优化器求解。仿真显示，80%任务成功率需要104,946 m的路径长度和9次中继修正；100%成功率时路径长度增加到105,874 m，但修正次数保持不变。
刚体拥挤空间导航：MIT团队提出三阶段MILP算法：(1) 在无碰撞工作空间生成凸多面体图；(2) 在相邻多面体之间求解小规模MILP以获取可行路径段；(3) 在线查询端到端可行路径。该方法将每个MILP子问题的变量规模限制在2-3个自由多面体内，大幅提升了大规模环境的可扩展性。
线性时序逻辑约束路径规划：LTL形式的任务规范通过LTL模型检测确保全局路径满足复杂时序和避障约束，在移动机器人、药品配送等场景有实际应用。
3.2.3 避障约束的 MILP 建模
将障碍物表示为多面体，引入二进制变量 b i , j b_{i,j} bi,j 表示轨迹点 i i i 位于障碍物 j j j 的哪一侧：
a j , k T x i ≤ d j , k + M ( 1 − b i , j , k ) , ∀ i , j , k \mathbf{a}{j,k}^T \mathbf{x}i \leq d{j,k} + M(1 - b{i,j,k}), \quad \forall i, j, k aj,kTxi≤dj,k+M(1−bi,j,k),∀i,j,k
∑ k b i , j , k ≥ 1 , ∀ i , j \sum_k b_{i,j,k} \geq 1, \quad \forall i, j k∑bi,j,k≥1,∀i,j
其中 M M M 为大常数，此为大-M 法 (Big-M method)。
3.2.4 典型案例
pseudocode
# 算法1：三阶段MILP刚体路径规划（简化版）
输入：起始位姿 q_start, 目标位姿 q_goal, 障碍物环境 E
输出：可行路径序列 P
阶段1：无碰撞凸分解
G = 构建凸多面体图(E) # 将自由空间分解为凸多面体集合
阶段2：离线MILP子问题
FOR EACH 相邻多面体对 (P_i, P_j) IN G:
MILP_i_j = 构建避碰MILP(P_i, P_j) # 每个子问题≤3个多面体
Path_i_j = 求解(MILP_i_j)
阶段3：在线查询
查询MILPs(q_start, q_goal) → 拼接路径段 → 返回 P3.2.5 优缺点分析
| 维度 | LP/MILP方法 |
|---|---|
| 优点 | 理论成熟，可给出全局最优解；商业求解器（Gurobi、CPLEX）效率高；数学严格性保证安全性 |
| 缺点 | MILP为NP-hard，规模增大时求解时间指数增长；需要线性化非线性动力学；整数变量带来组合爆炸 |
MILP方法对小型问题非常有效，但其可扩展性一直是限制其应用的核心瓶颈。
4. 非线性规划与序列二次规划
4.1 问题建模
实际问题中，系统动力学和任务约束往往是非线性的。非线性规划（NLP） 的一般形式为：
min x f ( x ) s.t. g i ( x ) ≤ 0 , h j ( x ) = 0 \min_x \; f(x) \quad \text{s.t.} \quad g_i(x) \leq 0, \; h_j(x) = 0 xminf(x)s.t.gi(x)≤0,hj(x)=0
其中 f f f, g i g_i gi, h j h_j hj 中至少有一个为非线性函数。在轨迹优化中， f f f 通常表示时间、能量或平滑度指标，约束 g g g 和 h h h 包括系统动力学方程、障碍物避碰条件和控制量边界。
4.2 序列二次规划（SQP）
SQP是解决NLP最有效的方法之一。其核心思想是在当前迭代点将原问题近似为一个QP子问题：
min d ∇ f ( x k ) T d + 1 2 d T H k d s.t. ∇ g ( x k ) T d + g ( x k ) ≤ 0 , ∇ h ( x k ) T d + h ( x k ) = 0 \min_d \; \nabla f(x_k)^T d + \frac{1}{2} d^T H_k d \quad \text{s.t.} \quad \nabla g(x_k)^T d + g(x_k) \leq 0, \; \nabla h(x_k)^T d + h(x_k) = 0 dmin∇f(xk)Td+21dTHkds.t.∇g(xk)Td+g(xk)≤0,∇h(xk)Td+h(xk)=0
其中 H k H_k Hk 为拉格朗日函数的Hessian近似（常用BFGS更新）。
4.3 应用场景
航天器再入轨迹优化：有研究采用正交配点参数化结合SQP方法计算航天飞机再入轨迹。通过正交配点离散化控制变量，再利用SQP在全局设计空间中高效搜索最优决策变量，满足燃料最优和终端状态约束等任务目标。
空间机器人轨迹规划：DLR的研究将自由浮动空间机器人（7-DOF）的轨迹优化问题建模为非线性约束优化，通过引入静态计算的奇异位形图为不等式约束，确保计算出的轨迹与奇异位形保持安全距离。
月球上升器轨迹优化：使用Gauss配点法和SQP，构建以燃料消耗最小为目标、满足入轨约束的轨迹优化模型。
4.4 典型案例
pseudocode
# 算法2：SQP轨迹优化（通用框架）
输入：初始猜测 x_0，收敛容差 ε
输出：最优轨迹 x*
k = 0
WHILE ||∇L(x_k, λ_k)|| > ε:
# Step 1：构建并求解QP子问题
H_k = 更新Hessian近似(x_k, λ_k) # BFGS或精确Hessian
d_k = 求解QP_subproblem(H_k, ∇f(x_k), ∇g(x_k), g(x_k))
# Step 2：线搜索
α_k = 线搜索(x_k, d_k) # 确保充分下降和约束可行性
# Step 3：更新
x_{k+1} = x_k + α_k * d_k
λ_{k+1} = 更新拉格朗日乘子(x_{k+1})
k = k + 1
RETURN x*4.5 NLMPC的应用
非线性模型预测控制（NLMPC）在工业自动化中展现了强大能力。有研究提出基于NLMPC的取放操作轨迹优化框架，通过在目标函数中加入终端代价项（惩罚末端状态的速度和加速度），使欧氏距离误差减少35.9%（无障碍场景）和10.6%（有障碍场景），平均计算时间仅0.045秒/步。
4.6 优缺点分析
| 维度 | 非线性规划/SQP方法 |
|---|---|
| 优点 | 适应复杂非线性动力学；SQP收敛速度快（超线性）；可处理一般约束 |
| 缺点 | 需要良好初始猜测；仅保证局部最优；Hessian计算代价高；非凸问题可能发散 |
5. 凸优化方法
5.1 问题转化与凸化技术
凸优化是轨迹规划中最具吸引力的范式之一：一旦问题被表述为凸形式，即可在多项式时间内求得全局最优解。其核心挑战在于，避障约束和大多数动力学系统自然构成的是非凸优化问题。
连续时间序列凸化（ct-SCVX） 是一种实时可解的约束轨迹优化方法，通过连续时间约束满足和保证收敛性来实现。其主要技术手段是将非线性动力学沿名义轨迹线性化，将非凸障碍物约束转换为凸近似，在信赖域约束下迭代求解。
5.2 凸集图（GCS）方法
MIT CSAIL提出的凸集图（GCS） 轨迹优化方法是近年来的标志性突破。GCS结合了图搜索的完备性和凸优化的最优性：
- 阶段1：将自由空间分解为凸集（多面体/椭球）
- 阶段2：在凸集图上构建混合整数凸优化问题
- 阶段3：在顶点（凸集）内和边（凸集交集）上分配路径段，求解全局最优轨迹
该方法已在7-DoF机械臂等复杂系统上验证，在计算效率和轨迹质量方面优于PRM*、RRT*等传统方法。GCSGC扩展进一步通过ReLU神经网络近似非凸代价函数，结合McCormick松弛实现凸化。
贝塞尔曲线 + 凸优化：贝塞尔曲线在多项式轨迹优化中有重要应用。其凸包特性使得：若所有控制点位于安全区域内，则整条曲线（凸包内）也位于该区域内，从而可将连续避碰约束转化为离散的控制点线性约束。五阶贝塞尔曲线进一步提供曲率连续（G²连续）的轨迹，适合非完整约束车辆。有研究通过构造旋转矩形框，将避碰约束转化为严格的线性等式和不等式约束，配合二次型优化指标构成标准凸优化模型。
5.3 应用场景
- 自动驾驶车辆：MIT在2D和7D配置空间中验证了GCSGC规划器，与PRM*、TrajOpt、BIT*等方法对比展现了显著优势
- 类车机器人：CES算法将形状优化和速度优化均转化为凸规划问题，在数百毫秒内返回高质量解，适用于实时实现
- 多AGV调度：混合整数二次规划（MIQP）用于构建时空安全走廊，在多AGV协同场景下保证避碰和轨迹平滑性
5.4 典型案例
pseudocode
# 算法3：凸弹性平滑（CES）算法
输入：初始锯齿轨迹 τ_init（由采样规划器生成）
输出：平滑且速度优化的轨迹 τ*
τ = τ_init # 当前轨迹
REPEAT:
# Step 1：形状优化（凸规划）
# - 目标：最小化路径长度/偏离原始路径的惩罚
# - 约束：保持与障碍物的安全距离 → 线性约束
τ_shape = 解凸规划_shape(τ)
# Step 2：速度优化（凸规划）
# - 目标：最小化时间或能量
# - 约束：速度、加速度边界 → 线性约束
τ = 解凸规划_speed(τ_shape)
UNTIL 收敛
RETURN τ5.5 优缺点分析
| 维度 | 凸优化方法 |
|---|---|
| 优点 | 全局最优保证；多项式时间求解；无需初始猜测；理论完备 |
| 缺点 | 需要将原问题转化为凸形式（可能改变优化本质）；凸化过程可能损失精度；序列凸化仅保证局部最优 |
6. 二次规划方法
6.1 数学基础
二次规划（QP） 是目标函数为二次型、约束为线性的一类特殊凸优化问题：
min x 1 2 x T Q x + c T x s.t. A x ≤ b , C x = d \min_x \; \frac{1}{2} x^T Q x + c^T x \quad \text{s.t.} \quad Ax \leq b, \; Cx = d xmin21xTQx+cTxs.t.Ax≤b,Cx=d
当 Q ⪰ 0 Q \succeq 0 Q⪰0（半正定）时，QP是凸问题，可在多项式时间内精确求解。QP在轨迹优化中的地位特殊：许多轨迹平滑、速度规范和MPC控制问题可直接或间接转化为QP。
6.2 Apollo 自动驾驶中的 QP 路径优化
根据 Apollo 官方文档及 CSDN 技术解析：
问题构造:
在 Frenet 坐标系下，优化变量为横向偏移 l ( s ) l(s) l(s) 及其导数 l ′ , l ′ ′ l', l'' l′,l′′。
采用分段五次多项式：
l i ( s ) = a i 0 + a i 1 s + a i 2 s 2 + a i 3 s 3 + a i 4 s 4 + a i 5 s 5 l_i(s) = a_{i0} + a_{i1}s + a_{i2}s^2 + a_{i3}s^3 + a_{i4}s^4 + a_{i5}s^5 li(s)=ai0+ai1s+ai2s2+ai3s3+ai4s4+ai5s5
目标函数:
J = w 1 ∑ i ( l i − l i , r e f ) 2 + w 2 ∑ i ( l i ′ ) 2 + w 3 ∑ i ( l i ′ ′ ) 2 + w 4 ∑ i ( l i ′ ′ ′ ) 2 J = w_1 \sum_i (l_i - l_{i,ref})^2 + w_2 \sum_i (l_i')^2 + w_3 \sum_i (l_i'')^2 + w_4 \sum_i (l_i''')^2 J=w1i∑(li−li,ref)2+w2i∑(li′)2+w3i∑(li′′)2+w4i∑(li′′′)2
- 跟踪项：保持与参考线接近
- 平滑项：最小化曲率及曲率变化率
- 紧凑项：避免点集过度分散
约束条件:
- 连续性约束: 位置、速度、加速度在节点处连续
- 边界约束 : l min ≤ l i ≤ l max l_{\min} \leq l_i \leq l_{\max} lmin≤li≤lmax
- 动力学约束: 曲率约束、加速度约束
算法伪代码:
算法: Apollo QP Path Optimizer
输入: 参考线点集 {r_i}, 障碍物信息, 车辆状态
输出: 平滑路径 {l_i}
1. 构建优化变量: x = [l_1, l'_1, l''_1, ..., l_n, l'_n, l''_n]^T
2. 构造代价矩阵 Q:
- Q_l: 横向偏移权重 (跟踪参考线)
- Q_l': 一阶导权重 (航向角平滑)
- Q_l'': 二阶导权重 (曲率平滑)
- Q_l''': 三阶导权重 (jerk 平滑)
3. 构造约束矩阵:
- A_eq: 连续性约束 (位置、导数连续)
- A_ineq: 边界约束 (车道线、障碍物)
4. 调用 OSQP/Eigen-QP 求解器
5. 将解 {l_i} 转换回笛卡尔坐标系
6. 返回平滑路径6.3 二阶锥规划 (SOCP) 在足式机器人中的应用
6.3.1 数学形式
min x f T x \min_{\mathbf{x}} \mathbf{f}^T \mathbf{x} xminfTx
s.t. ∥ A i x + b i ∥ 2 ≤ c i T x + d i , i = 1 , ... , m \text{s.t. } \|A_i \mathbf{x} + \mathbf{b}_i\|_2 \leq \mathbf{c}_i^T \mathbf{x} + d_i, \quad i = 1, \ldots, m s.t. ∥Aix+bi∥2≤ciTx+di,i=1,...,m
6.3.2 足式机器人 MPC 中的 SOCP
根据 arXiv 论文 Convex Optimization in Legged Robots：
单刚体动力学模型 (SRBD):
m p ¨ = ∑ i = 1 n c f i − m g m\ddot{\mathbf{p}} = \sum_{i=1}^{n_c} \mathbf{f}_i - m\mathbf{g} mp¨=i=1∑ncfi−mg
I ω ˙ = ∑ i = 1 n c ( r i − p ) × f i \mathbf{I}\dot{\boldsymbol{\omega}} = \sum_{i=1}^{n_c} (\mathbf{r}_i - \mathbf{p}) \times \mathbf{f}_i Iω˙=i=1∑nc(ri−p)×fi
摩擦锥约束的 SOCP 形式:
接触力需满足摩擦锥约束：
f i , x 2 + f i , y 2 ≤ μ f i , z \sqrt{f_{i,x}^2 + f_{i,y}^2} \leq \mu f_{i,z} fi,x2+fi,y2 ≤μfi,z
这天然是二阶锥约束。
MIT Cheetah 的 Convex MPC:
将非线性 MPC 简化为线性化模型，使得每步优化为凸 QP/SOCP：
min f i ∑ k = 1 N ∥ x k − x k , r e f ∥ Q 2 + ∑ k = 0 N − 1 ∥ f k ∥ R 2 \min_{\mathbf{f}i} \sum{k=1}^{N} \|\mathbf{x}k - \mathbf{x}{k,ref}\|Q^2 + \sum{k=0}^{N-1} \|\mathbf{f}_k\|_R^2 fimink=1∑N∥xk−xk,ref∥Q2+k=0∑N−1∥fk∥R2
s.t. x k + 1 = A k x k + B k f k \text{s.t. } \mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{f}_k s.t. xk+1=Akxk+Bkfk
∥ f i , x y ∥ 2 ≤ μ f i , z \|\mathbf{f}_{i,xy}\|2 \leq \mu \mathbf{f}{i,z} ∥fi,xy∥2≤μfi,z
6.4 应用场景
车辆轨迹平滑：有研究提出两步QP方法：第一步对齐平滑速度与物理规律（车辆动力学约束），第二步约束加速度范围符合驾驶员行为，用于清理纵向车辆轨迹中的感知噪声和误差。
轨迹跟踪控制：基于李雅普诺夫引导的QP方法用于反馈线性化系统的输出空间轨迹跟踪，在有限计算资源下实现了比纯追踪控制更好的跟踪精度。
基于高斯过程的MPC（GP-MPC）：通过在没有线性化的情况下构建一系列替代QP子问题，迭代求解包含高斯过程不确定性的非线性最优控制问题，显著加速了预测控制的求解过程。
MIQP多AGV轨迹规划：利用混合整数二次规划，在时空安全走廊框架下同时规划多辆自主导引车（AGV）的轨迹，确保避碰和运动平滑性。
6.5 典型案例------MPC中的QP
在实时MPC中，每个控制周期求解一个QP：
pseudocode
# 算法4：MPC-QP轨迹跟踪控制循环
初始化：系统状态 x_0，参考轨迹 τ_ref
FOR t = 0 TO T DO:
# 测量当前状态
x_meas = 传感器读取(t)
# 构建QP（线性化动力学 + 预测时域内二次代价）
QP = 构建MPC_QP(x_meas, τ_ref, N_horizon, Q, R)
# 求解QP（使用qpOASES、OSQP等专用求解器）
u_seq = solveQP(QP)
# 应用第一步控制
执行(u_seq[0])求解器选择：DAQP、qpOASES和OSQP等专用QP求解器已在实际卡车-拖车系统上得到验证。
6.6 优缺点分析
| 维度 | QP方法 |
|---|---|
| 优点 | 求解速度快（毫秒级）；凸QP保证全局最优；适合实时MPC |
| 缺点 | 需要模型为线性或可线性化；QP约束必须为线性；非线性系统需借助SQP框架 |
7. 动态规划与图搜索方法
7.1 动态规划 (DP) 原理
7.1.1 Bellman 最优性原理
J ∗ ( x , t ) = min u [ L ( x , u , t ) Δ t + J ∗ ( x + f ( x , u ) Δ t , t + Δ t ) ] J^*(x, t) = \min_u \left[ L(x, u, t) \Delta t + J^*(x + f(x,u)\Delta t, t + \Delta t) \right] J∗(x,t)=umin[L(x,u,t)Δt+J∗(x+f(x,u)Δt,t+Δt)]
离散动态规划（DP） 基于Bellman最优性原理：
V k ( x k ) = min u k { g k ( x k , u k ) + V k + 1 ( f k ( x k , u k ) ) } V_k(x_k) = \min_{u_k} \{ g_k(x_k, u_k) + V_{k+1}(f_k(x_k, u_k)) \} Vk(xk)=ukmin{gk(xk,uk)+Vk+1(fk(xk,uk))}
其中 V k ( ⋅ ) V_k(\cdot) Vk(⋅) 为值函数（从阶段 k k k 到终点的最优代价）， f k f_k fk 为状态转移方程， g k g_k gk 为阶段代价函数。
微分动态规划（DDP） 是DP在连续最优控制中的经典推广，通过对系统沿名义轨迹进行二次展开，利用局部线性二次调节器（LQR）迭代改进解质量。
7.1.2 在自动驾驶中的应用：Lattice Planner
根据 CSDN 技术博客对自动驾驶规划算法的分析：
Lattice Planner 的 DP 阶段:
在 Frenet 坐标系下，将路径-速度解耦：
-
路径规划 (SL 图):
- 在 station-lateral 网格上执行动态规划
- 代价函数： C t o t a l = C s m o o t h + C o b s + C g u i d e C_{total} = C_{smooth} + C_{obs} + C_{guide} Ctotal=Csmooth+Cobs+Cguide
- 平滑代价： C s m o o t h = w 1 ∑ ( l ′ ′ ) 2 + w 2 ∑ ( l ′ ′ ′ ) 2 C_{smooth} = w_1 \sum (l'')^2 + w_2 \sum (l''')^2 Csmooth=w1∑(l′′)2+w2∑(l′′′)2
- 障碍物代价： C o b s = ∑ dist ( x i , obs ) C_{obs} = \sum \text{dist}(x_i, \text{obs}) Cobs=∑dist(xi,obs)
-
速度规划 (ST 图):
- 在 station-time 网格上执行动态规划
- 考虑跟车、限速、红绿灯等约束
算法伪代码:
算法: Lattice Planner (DP Stage)
输入: 参考线, 障碍物信息, 车辆状态
输出: 粗路径/速度曲线
1. 在 Frenet 坐标系下建立 SL/ST 网格
2. 对每个网格节点 (s_i, l_j):
- 计算从上一列节点转移的代价
- 代价 = 平滑代价 + 障碍物代价 + 参考线偏离代价
3. 使用动态规划递推:
- cost[i][j] = min_k {cost[i-1][k] + transition_cost(k→j)}
- parent[i][j] = argmin_k
4. 从终点回溯得到最优粗路径
5. 对粗路径进行多项式插值7.2 图搜索算法
7.2.1 A* 算法
评价函数 : f ( n ) = g ( n ) + h ( n ) f(n) = g(n) + h(n) f(n)=g(n)+h(n)
- g ( n ) g(n) g(n): 从起点到节点 n n n 的实际代价
- h ( n ) h(n) h(n): 从节点 n n n 到目标的启发式估计（需满足可采纳性： h ( n ) ≤ h ∗ ( n ) h(n) \leq h^*(n) h(n)≤h∗(n)）
常用启发函数:
| 启发函数 | 公式 | 适用场景 |
|---|---|---|
| 欧几里得距离 | h = ( x − x g ) 2 + ( y − y g ) 2 h = \sqrt{(x-x_g)^2 + (y-y_g)^2} h=(x−xg)2+(y−yg)2 | 自由空间 |
| 曼哈顿距离 | $h = | x-x_g |
| 切比雪夫距离 | $h = \max( | x-x_g |
7.2.2 D* Lite 算法
适用于动态环境的增量式重规划：
key ( s ) = [ min ( g ( s ) , r h s ( s ) ) + h ( s s t a r t , s ) ; min ( g ( s ) , r h s ( s ) ) ] \text{key}(s) = [\min(g(s), rhs(s)) + h(s_{start}, s); \min(g(s), rhs(s))] key(s)=[min(g(s),rhs(s))+h(sstart,s);min(g(s),rhs(s))]
优势: 环境变化时仅需局部更新，计算效率远高于完全重规划。
7.3 应用场景
无人机航迹规划：有研究运用动态规划方法，将多约束条件下的无人机航迹规划问题划分为三个子问题：校正点选择、转向路径规划、路径搜寻终止条件。对每个子问题分别建立数学模型，实验验证表明该方法具有简单、易操作的特点，在时间复杂度和空间复杂度方面表现良好。
差分进化滚动优化（DEROA）：针对大规模无人机编队协同感知场景，DEROA方法结合差分进化的全局搜索能力与滚动时域优化的实时调整能力，通过贝叶斯推理动态更新网格目标存在概率，实现分布式预测控制。实验表明，高危区域覆盖率提升65.7%-106.6%，目标发现概率达到0.62（提升3.3%-37.8%）。
闭环动态SQP：在DDP的框架下，MIT团队提出了一种基于动态规划的约束集递归方法，将经典DDP的"cost-to-go"反向传播与约束集传播结合，使间接法也能处理一般性约束。
7.4 典型案例
pseudocode
# 算法5：无人机DP航迹规划
输入：航路地图 M，起点 S，目标 T，多个约束条件
输出：最优航迹 P*
Phase 1：校正点选择（DP子问题）
D_corr = DP_stage1(M, S, T) # 选择最佳校正点序列
Phase 2：转向路径规划（DP子问题）
P_turn = DP_stage2(D_corr, M) # 在相邻校正点间规划转向路径
Phase 3：路径终止条件
P* = DP_stage3(P_turn, T) # 确定到达终点的最优路径
Phase 4：路径平滑（可选）
P* = 贝塞尔曲线/B样条平滑(P*)7.5 方法对比
| 方法 | 最优性 | 实时性 | 适用维度 | 动态环境适应性 |
|---|---|---|---|---|
| Dijkstra | 全局最优 | 慢 | 低维 | 差 |
| A* | 全局最优 | 中等 | 低-中维 | 差 |
| D* Lite | 全局最优 | 快 | 低-中维 | 优秀 |
| Lattice DP | 网格最优 | 快 | 中维 | 中等 |
7.6 优缺点分析
| 维度 | 动态规划方法 |
|---|---|
| 优点 | 提供全局最优解（离散DP）；适用于多阶段决策问题；DDP收敛速度通常超线性 |
| 缺点 | "维数灾难"：状态空间增长时计算量指数上升；DDP需要系统具有良好的光滑性；DP需离散化，可能引入误差 |
8. 元启发式算法
8.1 遗传算法（GA）
GA通过模拟自然进化的选择、交叉和变异操作来搜索最优路径。在路径规划中，每条染色体编码一条完整路径（从起点到目标点的路径点序列）。
改进GA路径规划 ：针对GA在全局静态环境中种群初始化效率低、收敛速度慢、易陷入局部最优的缺点，有研究提出改进方案：采用网格法建立环境模型，染色体 P ( p 1 , p 2 , . . . , p n ) P(p_1, p_2, ..., p_n) P(p1,p2,...,pn) 表示从起点S到目标点T的完整路径。GIT*算法进一步发展了GA与采样规划的结合，通过整合环境的斥力信息和动态路径段重要性来增强引导启发式。
GA-SQP混合优化：有研究将改进GA与SQP结合用于可重复使用运载火箭再入轨迹优化：GA（结合模拟退火罚函数）进行全局设计空间搜索，SQP进行局部精化------这种"全局+局部"混合策略兼具GA的全局搜索优势和SQP的局部高效收敛特性。
8.2 粒子群优化（PSO）
PSO通过粒子在搜索空间中的速度和位置更新来寻找最优解，在连续空间路径优化中表现优异。
改进PSO三维轨迹规划：将无人机三维轨迹规划问题转化为多约束目标优化问题，构建包含轨迹长度、障碍物威胁、平滑性和高度变化的综合适应度函数。改进PSO引入Lévy飞行策略增强跳出局部最优的能力，结果显示轨迹代价更低、质量更高。
球面向量PSO（SPSO）动态路径规划：使用球面向量编码路径，使PSO产生满足飞行准则的最优UAV路径。
8.3 差分进化（DE）
DE通过差分向量（种群成员间的差异）生成变异向量，在连续参数空间具有强全局搜索能力和旋转不变性。
DEROA方法：将DE的全局搜索与滚动时域优化的实时调整相结合，通过分布式预测控制实现网格化动态路径规划。该方法在大型区域多UAV协同感知中展现了极强的鲁棒性和适应性。
8.4 其他算法：蚁群优化（ACO）
ACO模仿蚂蚁通过信息素通信找到最优路径，特别适用于离散组合优化问题。改进的ACO结合B样条曲线可实现路径光滑化，使规划路径满足机器人最大曲率约束。
8.5 优缺点分析
| 维度 | GA | PSO | ACO | DE |
|---|---|---|---|---|
| 优点 | 全局搜索强，适合离散路径；可与梯度方法混合 | 收敛快，参数少，适合高维连续空间 | 天然适合TSP类问题；适应性强 | 旋转不变，对噪声鲁棒 |
| 缺点 | 收敛慢，参数调试困难 | 易早熟收敛，后期多样性差 | 计算量大，信息素更新复杂 | 局部搜索能力弱于SQP |
| 典型应用 | 机器人全局路径 | UAV三维轨迹 | TSP路径 | 多UAV协同 |
9. 曲线优化与几何方法
9.1 Bézier曲线与B-spline
曲线参数化是路径平滑的基础工具，也是连接离散路径点和连续轨迹优化的桥梁。
贝塞尔曲线轨迹优化源于其凸包性------若所有控制点位于安全区域内，整条曲线必在其中。图论视角下的贝塞尔曲线优化已在无人机、自动驾驶和机械臂等系统中得到广泛应用。
分段五次贝塞尔曲线平滑：通过构造旋转矩形框保证曲线安全性，建立仅需满足线性等式/不等式约束的凸优化模型。
B-spline具有局部支撑性（调整单个控制点只影响有限区间），在轨迹规划中更具灵活性。应用包括：
- B样条参数化 + 进化优化的编队运动规划
- 基于距离梯度的B样条无人机避障轨迹规划，使用L-BFGS算法高效求解局部最优路径
- 五次B样条 + 遗传算法的时间最优轨迹优化
9.1.1 B 样条的凸包性质
B 样条曲线的关键性质：曲线始终位于控制点的凸包内。
C ( t ) = ∑ i = 0 n P i N i , p ( t ) \mathbf{C}(t) = \sum_{i=0}^{n} \mathbf{P}i N{i,p}(t) C(t)=i=0∑nPiNi,p(t)
其中 N i , p ( t ) N_{i,p}(t) Ni,p(t) 为 p p p 次 B 样条基函数。
9.1.2 Fast-Planner 中的 B 样条优化
根据 CSDN 博客对 Fast-Planner 的解读：
目标函数:
J = λ 1 J s m o o t h + λ 2 J c o l l i s i o n + λ 3 J f e a s i b i l i t y J = \lambda_1 J_{smooth} + \lambda_2 J_{collision} + \lambda_3 J_{feasibility} J=λ1Jsmooth+λ2Jcollision+λ3Jfeasibility
- 平滑项 : J s m o o t h = ∑ i ∥ P i + 1 − 2 P i + P i − 1 ∥ 2 J_{smooth} = \sum_i \|\mathbf{P}_{i+1} - 2\mathbf{P}i + \mathbf{P}{i-1}\|^2 Jsmooth=∑i∥Pi+1−2Pi+Pi−1∥2
- 碰撞项 : J c o l l i s i o n = ∑ i d ( P i , obs ) 2 J_{collision} = \sum_i d(\mathbf{P}_i, \text{obs})^2 Jcollision=∑id(Pi,obs)2 (若控制点在障碍物内)
- 可行性项: 速度、加速度约束
优势: 利用 B 样条凸包性质，只需保证控制点在安全区域，即可保证整条曲线安全。
9.2 Dubins路径
Dubins路径是曲率有界条件下两点间的最短路径，由圆弧段和直线段组合而成，对非完整约束（如汽车、固定翼UAV）具有特殊的理论意义。
时间最优Dubins轨迹：在考虑移动障碍物的前提下，研究两点间的时间最优Dubins轨迹，采用离散化动态规划框架处理静态障碍物，保证路径的曲率连续性和运动可行性。
球面Dubins问题 ：在单位球面上的Dubins车辆运动规划中，路径半径 r r r 受最大曲率约束下限，最优路径类型为CG、CC或其退化形式。
9.3 优缺点分析
| 维度 | 曲线优化方法 |
|---|---|
| 优点 | 保证轨迹平滑性和运动可行性；与凸规划可以自然集成；参数少、实现简单 |
| 缺点 | 仅解决局部轨迹生成；需与全局规划器配合；高阶曲线计算开销较大 |
10. 强化学习方法
10.1 深度强化学习轨迹优化
强化学习（RL）为轨迹优化提供了数据驱动、无需精确模型的替代范式。在未知或动态环境中，RL智能体通过与环境的交互学习最优策略。
模型自优化回放机制：针对DRL在轨迹规划中存在的经验利用效率低、收敛慢、奖励函数设计不合理等问题，有研究设计了基于神经网络的专家引导三重经验回放机制（NETM），并提出了适应动态环境的改进奖励函数。
自适应性大脑启发的HG²P方法：借鉴海马体-纹状体双控制器假说，通过HRL分解复杂目标任务序列为简单子目标任务，解决了大规模环境中的长时序规划挑战。
10.2 RL与传统优化的结合
LearnOpTra项目结合RL与基于优化的方法，采用离线RL提高数据效率，利用人类演示和现有数据集训练智能体。
自适应动态规划（ADP）：基于IRL和神经网络的ADP方法为不确定非线性系统的轨迹跟踪提供了新的理论框架。
10.3 优缺点分析
| 维度 | 强化学习方法 |
|---|---|
| 优点 | 自适应未知环境；不需要精确模型；在线学习能力 |
| 缺点 | 需要大量训练数据；训练不稳定；缺乏最优性保证；可解释性差 |
11. 非线性规划与直接法
11.1 直接法概述
直接法 (Direct Methods) 将连续时间最优控制问题转录 (transcribe) 为有限维非线性规划 (NLP) 问题：
min z F ( z ) \min_{\mathbf{z}} F(\mathbf{z}) zminF(z)
s.t. g ( z ) = 0 , h ( z ) ≤ 0 \text{s.t. } \mathbf{g}(\mathbf{z}) = 0, \quad \mathbf{h}(\mathbf{z}) \leq 0 s.t. g(z)=0,h(z)≤0
11.2 直接打靶法 (Direct Shooting)
11.2.1 原理
仅将控制轨迹离散化为决策变量：
U = [ u 0 , u 1 , ... , u N − 1 ] \mathbf{U} = [u_0, u_1, \ldots, u_{N-1}] U=[u0,u1,...,uN−1]
状态轨迹通过前向积分获得：
x k + 1 = f ( x k , u k ) , x 0 given x_{k+1} = f(x_k, u_k), \quad x_0 \text{ given} xk+1=f(xk,uk),x0 given
11.2.2 优缺点
| 优点 | 缺点 |
|---|---|
| 决策变量少 | 对初始猜测敏感 |
| 动力学约束自动满足 | 梯度计算需反向传播 |
| 适合无状态约束的问题 | 状态约束难以处理 |
11.3 直接转录法 (Direct Transcription)
11.3.1 原理
同时将状态和控制作为决策变量：
Z = [ x 0 , u 0 , x 1 , u 1 , ... , x N ] \mathbf{Z} = [x_0, u_0, x_1, u_1, \ldots, x_N] Z=[x0,u0,x1,u1,...,xN]
动力学作为等式约束：
x k + 1 − f ( x k , u k ) = 0 x_{k+1} - f(x_k, u_k) = 0 xk+1−f(xk,uk)=0
11.3.2 线性系统的凸优化形式
对于线性系统 x k + 1 = A x k + B u k x_{k+1} = Ax_k + Bu_k xk+1=Axk+Buk，动力学约束为线性约束：
x k + 1 − A x k − B u k = 0 x_{k+1} - Ax_k - Bu_k = 0 xk+1−Axk−Buk=0
若目标函数为二次型，则整体为 QP 问题，可全局高效求解。
11.4 直接配点法 (Direct Collocation)
11.4.1 梯形配点法 (Trapezoidal Collocation)
根据 SIAM 教程 An Introduction to Trajectory Optimization：
在区间 [ t k , t k + 1 ] [t_k, t_{k+1}] [tk,tk+1] 上，状态用分段线性近似，控制用分段常数近似。
动力学缺陷约束 (defect constraint)：
ζ k = x k + 1 − x k − h k 2 ( f k + f k + 1 ) = 0 \zeta_k = x_{k+1} - x_k - \frac{h_k}{2}(f_k + f_{k+1}) = 0 ζk=xk+1−xk−2hk(fk+fk+1)=0
其中 f k = f ( x k , u k ) f_k = f(x_k, u_k) fk=f(xk,uk)， h k = t k + 1 − t k h_k = t_{k+1} - t_k hk=tk+1−tk。
精度 : O ( h 2 ) O(h^2) O(h2)
11.4.2 Hermite-Simpson 配点法
状态用分段三次多项式近似，在每个区间中点增加配点。
x c = 1 2 ( x k + x k + 1 ) + h 8 ( f k − f k + 1 ) x_c = \frac{1}{2}(x_k + x_{k+1}) + \frac{h}{8}(f_k - f_{k+1}) xc=21(xk+xk+1)+8h(fk−fk+1)
ζ k = x k + 1 − x k − h 6 ( f k + 4 f c + f k + 1 ) = 0 \zeta_k = x_{k+1} - x_k - \frac{h}{6}(f_k + 4f_c + f_{k+1}) = 0 ζk=xk+1−xk−6h(fk+4fc+fk+1)=0
精度 : O ( h 4 ) O(h^4) O(h4)
11.4.3 算法伪代码
算法: Direct Collocation (Trapezoidal)
输入: 初始状态 x_0, 目标状态 x_f, 时间区间 [0, T], 段数 N
输出: 最优轨迹 {x_k, u_k}
1. 离散化时间: t_k = k * h, h = T/N
2. 初始化猜测: {x_k^0}, {u_k^0}
3. 定义决策变量: Z = [x_0, u_0, x_1, u_1, ..., x_N]
4. 构建 NLP:
- 目标: min Σ L(x_k, u_k) + φ(x_N)
- 等式约束:
* 动力学缺陷: x_{k+1} - x_k - h/2*(f_k + f_{k+1}) = 0
* 边界条件: x_0 = x_init, x_N = x_goal
- 不等式约束:
* 控制限幅: u_min ≤ u_k ≤ u_max
* 状态限幅: x_min ≤ x_k ≤ x_max
5. 调用 NLP 求解器 (IPOPT/SNOPT)
6. 返回最优解11.5 直接多重打靶法 (Direct Multiple Shooting)
11.5.1 原理
将轨迹分为 N N N 段，每段独立积分，通过连续性约束连接：
x k + 1 − ϕ ( x k , u k ; h k ) = 0 \mathbf{x}_{k+1} - \boldsymbol{\phi}(\mathbf{x}_k, \mathbf{u}_k; h_k) = 0 xk+1−ϕ(xk,uk;hk)=0
其中 ϕ \boldsymbol{\phi} ϕ 为数值积分结果。
11.5.2 与直接配点法对比
| 特性 | 直接配点法 | 直接多重打靶法 |
|---|---|---|
| 动力学精度 | 近似满足 | 精确满足（通过积分） |
| 问题规模 | 大 | 较小 |
| 稀疏性 | 高度稀疏 | 中等稀疏 |
| 收敛性 | 对初猜敏感 | 较鲁棒 |
| 适用场景 | 平滑轨迹 | 刚性动力学 |
12. 序列凸规划与微分动态规划
12.1 序列凸规划 (SCP)
12.1.1 核心思想
迭代线性化非凸问题，在每步求解凸子问题：
min x f ( x k ) + ∇ f ( x k ) T ( x − x k ) \min_{\mathbf{x}} f(\mathbf{x}_k) + \nabla f(\mathbf{x}_k)^T (\mathbf{x} - \mathbf{x}_k) xminf(xk)+∇f(xk)T(x−xk)
s.t. g ( x k ) + ∇ g ( x k ) T ( x − x k ) ≤ 0 \text{s.t. } g(\mathbf{x}_k) + \nabla g(\mathbf{x}_k)^T (\mathbf{x} - \mathbf{x}_k) \leq 0 s.t. g(xk)+∇g(xk)T(x−xk)≤0
∥ x − x k ∥ ≤ ρ k (信赖域) \|\mathbf{x} - \mathbf{x}_k\| \leq \rho_k \quad \text{(信赖域)} ∥x−xk∥≤ρk(信赖域)
12.1.2 碰撞避免的凸松弛
根据 MIT 教程，碰撞避免约束可松弛为：
∥ x − x o b s ∥ 2 ≥ r s a f e \|\mathbf{x} - \mathbf{x}_{obs}\|2 \geq r{safe} ∥x−xobs∥2≥rsafe
松弛为线性约束：
( x k − x o b s ) T ( x − x o b s ) ≥ r s a f e ∥ x k − x o b s ∥ 2 (\mathbf{x}k - \mathbf{x}{obs})^T (\mathbf{x} - \mathbf{x}{obs}) \geq r{safe} \|\mathbf{x}k - \mathbf{x}{obs}\|_2 (xk−xobs)T(x−xobs)≥rsafe∥xk−xobs∥2
12.1.3 CHOMP 算法
协变哈密顿优化运动规划 (Covariant Hamiltonian Optimization for Motion Planning)：
J C H O M P = λ o b s ∑ t = 0 T c ( x t ) + λ s m o o t h ∑ t = 0 T − 1 ∥ x ˙ t ∥ 2 J_{CHOMP} = \lambda_{obs} \sum_{t=0}^{T} c(x_t) + \lambda_{smooth} \sum_{t=0}^{T-1} \|\dot{x}_t\|^2 JCHOMP=λobst=0∑Tc(xt)+λsmootht=0∑T−1∥x˙t∥2
使用函数梯度下降优化轨迹：
ξ k + 1 = ξ k − α k M ~ − 1 ∇ J ( ξ k ) \xi_{k+1} = \xi_k - \alpha_k \tilde{M}^{-1} \nabla J(\xi_k) ξk+1=ξk−αkM~−1∇J(ξk)
其中 M ~ \tilde{M} M~ 为度量张量，确保优化具有坐标不变性。
12.2 微分动态规划 (DDP) 与 iLQR
12.2.1 DDP 原理
DDP 通过在标称轨迹附近进行二次展开来迭代优化：
前向展开:
x k + 1 = f ( x k , u k ) x_{k+1} = f(x_k, u_k) xk+1=f(xk,uk)
后向递推 (Riccati 方程):
Q x = ℓ x + f x T V x ′ Q_x = \ell_x + f_x^T V_{x}' Qx=ℓx+fxTVx′
Q u = ℓ u + f u T V x ′ Q_u = \ell_u + f_u^T V_{x}' Qu=ℓu+fuTVx′
Q x x = ℓ x x + f x T V x x ′ f x Q_{xx} = \ell_{xx} + f_x^T V_{xx}' f_x Qxx=ℓxx+fxTVxx′fx
Q u u = ℓ u u + f u T V x x ′ f u Q_{uu} = \ell_{uu} + f_u^T V_{xx}' f_u Quu=ℓuu+fuTVxx′fu
Q u x = ℓ u x + f u T V x x ′ f x Q_{ux} = \ell_{ux} + f_u^T V_{xx}' f_x Qux=ℓux+fuTVxx′fx
最优控制增量：
δ u ∗ = − Q u u − 1 ( Q u + Q u x δ x ) \delta u^* = -Q_{uu}^{-1} (Q_u + Q_{ux} \delta x) δu∗=−Quu−1(Qu+Quxδx)
12.2.2 iLQR (迭代 LQR)
iLQR 忽略 DDP 中的二阶动力学项（Hessian），仅保留代价函数的 Hessian：
Q u u ≈ ℓ u u + f u T V x x ′ f u Q_{uu} \approx \ell_{uu} + f_u^T V_{xx}' f_u Quu≈ℓuu+fuTVxx′fu
优势: 每步计算更快，适合高维系统。
12.2.3 AL-iLQR (增广拉格朗日 iLQR)
处理约束的扩展形式：
L ( x , u , λ , μ ) = ℓ ( x , u ) + λ T c ( x , u ) + 1 2 c ( x , u ) T I μ c ( x , u ) \mathcal{L}(x, u, \lambda, \mu) = \ell(x, u) + \lambda^T c(x, u) + \frac{1}{2} c(x, u)^T I_{\mu} c(x, u) L(x,u,λ,μ)=ℓ(x,u)+λTc(x,u)+21c(x,u)TIμc(x,u)
算法伪代码:
算法: AL-iLQR
输入: 初始轨迹 {x_k, u_k}, 约束 c(x,u) ≤ 0
输出: 最优轨迹
1. 初始化: λ = 0, μ = μ_init
2. while 未收敛:
a. // 后向传递
for k = N-1 downto 0:
- 计算 Q 函数展开 (一阶、二阶)
- 计算反馈增益: K_k = -Q_uu^{-1} Q_ux
- 计算前馈项: d_k = -Q_uu^{-1} Q_u
- 更新 Value 函数: V_k, V_xk, V_xxk
b. // 前向模拟
for k = 0 to N-1:
- δx = x_k - x_k_nom
- u_k = u_k_nom + K_k δx + α d_k (线搜索)
- x_{k+1} = f(x_k, u_k)
c. // 更新拉格朗日乘子
if ||c|| < ε:
λ += μ * c
else:
μ *= β
3. 返回收敛轨迹13. 方法对比与选型指南
13.1 综合对比表
| 方法类别 | 代表算法 | 问题类型 | 收敛性 | 计算效率 | 约束处理 | 典型应用 |
|---|---|---|---|---|---|---|
| 线性规划 | LP, MILP | 凸/组合 | 全局最优 | 高 | 线性 | UAV 路径、调度 |
| 动态规划 | Dijkstra, A* | 离散图 | 全局最优 | 中 | 硬约束 | 游戏 AI、地图导航 |
| 二次规划 | QP, OSQP | 凸 | 全局最优 | 很高 | 线性 | 自动驾驶平滑 |
| 二阶锥规划 | SOCP | 凸 | 全局最优 | 高 | 锥约束 | 足式机器人 MPC |
| 非线性规划 | IPOPT, SNOPT | 非凸 | 局部最优 | 中 | 一般 | 机器人轨迹 |
| 直接配点法 | Trapezoidal, H-S | 非凸 | 局部最优 | 中 | 等式/不等式 | 双足步态 |
| 序列凸规划 | SCP, CHOMP | 非凸→凸 | 局部最优 | 较高 | 可凸化 | 机械臂避障 |
| 微分动态规划 | DDP, iLQR | 非凸 | 局部最优 | 很高 | 需扩展 | 实时控制 |
13.2 多维度综合评价
| 方法 | 全局最优 | 计算效率 | 模型要求 | 可扩展性 | 实时性 | 典型适用场景 |
|---|---|---|---|---|---|---|
| LP/MILP | ★★★★★ | ★★★ | 线性模型 | ★★ | ★★ | 物流路径、小规模避障 |
| NLP/SQP | ★★★ | ★★★★ | 可微模型 | ★★★ | ★★★ | 航天器轨迹、机械臂 |
| 凸优化 | ★★★★★ | ★★★★ | 凸结构 | ★★★★ | ★★★ | 机器人运动规划 |
| QP | ★★★★★ | ★★★★★ | 二次模型 | ★★★ | ★★★★★ | MPC、轨迹平滑 |
| DP | ★★★★★ | ★★★ | 阶段划分 | ★★ | ★★ | 多阶段航迹、资源分配 |
| GA | ★★ | ★★ | 黑盒模型 | ★★★ | ★ | 全局路径搜索 |
| PSO | ★★ | ★★★ | 黑盒模型 | ★★★ | ★★ | 连续空间轨迹优化 |
| DE | ★★★ | ★★★ | 黑盒模型 | ★★★ | ★★ | 多UAV协同、动态环境 |
| 强化学习 | ★★ | ★★★ | 无/学习 | ★★★ | ★★★★★ | 未知动态环境、自适应 |
| 曲线优化 | ★★★★ | ★★★★★ | 路径点 | ★ | ★★★★★ | 轨迹平滑、局部优化 |
13.3 选型决策树
是否需要全局最优?
├── 是 → 问题是否凸?
│ ├── 是 → 使用 QP/SOCP (OSQP, ECOS)
│ └── 否 → 问题是否离散?
│ ├── 是 → 使用 A*, Dijkstra, MILP
│ └── 否 → 使用多起点优化、分支定界
└── 否 → 实时性要求?
├── 高 (< 1ms) → 使用 iLQR/DDP, 凸 MPC
├── 中 (1-100ms) → 使用 SCP, QP + 预测模型
└── 低 (> 100ms) → 使用直接配点法, NLP13.4 混合策略实践
实际工程中，最优方法通常是混合的：
- 全局粗规划 + 局部精细优化：A*（全局）→ Bézier/B-spline平滑（局部）
- 元启发式 + 梯度优化：GA（全局搜索）→ SQP（局部精化）
- 图搜索 + 凸优化：GCS框架将两者有机融合
- RL + 优化引导：专家策略引导 + 优化模板约束
14. 典型案例分析
14.1 案例一：自动驾驶 Apollo EM Planner
架构: DP + QP 两段式优化
E-Step (路径评估):
- 固定速度曲线，在 SL 空间采样多条路径
- 使用 DP 评估每条路径的代价（平滑性、安全性、舒适性）
M-Step (速度优化):
- 固定最优路径，优化速度曲线
- 使用 QP 求解满足动力学约束的最优速度剖面
数学形式:
min s ( t ) ∫ 0 T [ w 1 ( s − s r e f ) 2 + w 2 s ¨ 2 + w 3 s ... 2 ] d t \min_{s(t)} \int_0^T \left[ w_1 (s - s_{ref})^2 + w_2 \ddot{s}^2 + w_3 \dddot{s}^2 \right] dt s(t)min∫0T[w1(s−sref)2+w2s¨2+w3s...2]dt
s.t. 0 ≤ s ˙ ≤ v m a x , ∣ s ¨ ∣ ≤ a m a x , ∣ s ... ∣ ≤ j m a x \text{s.t. } 0 \leq \dot{s} \leq v_{max}, \quad |\ddot{s}| \leq a_{max}, \quad |\dddot{s}| \leq j_{max} s.t. 0≤s˙≤vmax,∣s¨∣≤amax,∣s...∣≤jmax
14.2 案例二：MIT Cheetah 四足机器人 Convex MPC
模型简化: 单刚体动力学 (SRBD)
优化问题:
min f i ∑ k = 1 N ∥ p k − p r e f , k ∥ 2 + ∥ Θ k − Θ r e f , k ∥ 2 + λ ∑ i , k ∥ f i , k ∥ 2 \min_{\mathbf{f}i} \sum{k=1}^{N} \|\mathbf{p}k - \mathbf{p}{ref,k}\|^2 + \|\boldsymbol{\Theta}k - \boldsymbol{\Theta}{ref,k}\|^2 + \lambda \sum_{i,k} \|\mathbf{f}_{i,k}\|^2 fimink=1∑N∥pk−pref,k∥2+∥Θk−Θref,k∥2+λi,k∑∥fi,k∥2
s.t. m p ¨ = ∑ i f i − m g \text{s.t. } m\ddot{\mathbf{p}} = \sum_i \mathbf{f}_i - m\mathbf{g} s.t. mp¨=i∑fi−mg
I ω ˙ = ∑ i ( r i − p ) × f i \mathbf{I}\dot{\boldsymbol{\omega}} = \sum_i (\mathbf{r}_i - \mathbf{p}) \times \mathbf{f}_i Iω˙=i∑(ri−p)×fi
∥ f i , x y ∥ 2 ≤ μ f i , z , f i , z ≥ 0 \|\mathbf{f}{i,xy}\|2 \leq \mu f{i,z}, \quad f{i,z} \geq 0 ∥fi,xy∥2≤μfi,z,fi,z≥0
求解: 每步转化为 SOCP，使用 ECOS 或自定义内点法求解器，在 1kHz 下实时运行。
14.3 案例三：SpaceX 火箭回收轨迹优化
方法: 直接配点法 + 伪谱法
挑战:
- 强非线性动力学（气动力、推力矢量）
- 严格的状态/控制约束
- 终端约束（精确着陆）
实现: 使用 GPOPS-II 离线生成最优轨迹，在线使用 MPC 跟踪。
14.4 案例四：机械臂运动规划 (CHOMP)
问题: 在高维配置空间中找到无碰撞、平滑的轨迹。
目标函数:
U [ ξ ] = λ o b s ∑ t = 0 T c ~ ( ξ ( t ) ) + λ s m o o t h ∑ t = 0 T − 1 ∥ ξ ( t + 1 ) − ξ ( t ) ∥ 2 U[\xi] = \lambda_{obs} \sum_{t=0}^{T} \tilde{c}(\xi(t)) + \lambda_{smooth} \sum_{t=0}^{T-1} \|\xi(t+1) - \xi(t)\|^2 U[ξ]=λobst=0∑Tc~(ξ(t))+λsmootht=0∑T−1∥ξ(t+1)−ξ(t)∥2
优化: 使用协变梯度下降，在轨迹空间直接优化。
15. 总结与展望
15.1 核心结论
-
凸优化方法（QP、SOCP）在实时性要求高的场景（自动驾驶、足式机器人）中占据主导地位，得益于其可证明的全局最优性和高效求解。
-
直接法（配点法、多重打靶法）是处理复杂动力学约束的标准工具，广泛应用于机器人学和航空航天领域。
-
序列方法（SCP、DDP/iLQR）在局部优化和实时控制中表现出色，是连接离线规划与在线控制的桥梁。
-
混合方法（DP+QP、GCS）结合多种方法优势，是工业级系统的典型架构。
15.2 发展趋势
-
在线自适应轨迹优化：克服MILP/NLP的离线计算瓶颈，发展亚毫秒级实时规划方法仍是核心挑战。
-
高维空间可扩展性：结合图分解（如GCS）和凸优化的分级框架代表了有希望的方向。
-
多目标协同优化：帕累托最优前沿方法与传统优化器的整合，实现在安全性、效率、舒适性等多目标间的自适应权衡。
-
不确定性处理：鲁棒优化（如ADP、随机MPC）用于解决定位精度、模型误差和环境不确定性。
-
学习与优化的深度融合：结合DRL的探索能力与传统优化的收敛保证，发展真正的"智能规划"技术。
-
可微分优化: 将优化问题作为神经网络层进行端到端训练，实现感知-规划一体化。
-
图+凸集 (GCS): MIT 提出的新型运动规划框架，将轨迹优化转化为图上的凸优化问题，兼具全局最优性和计算效率。
-
数据驱动优化: 利用深度学习近似值函数或策略，减少在线计算负担。
-
嵌入式求解器: 针对边缘设备定制的轻量级 QP/SOCP 求解器（如 HPIPM、Fatrop）。
参考文献与资源链接
学术教程
- MIT Underactuated Robotics - Trajectory Optimization
- Stanford AA203: Optimal and Learning-Based Control
- Czech Technical University - Numerical Methods for Direct Approach
- SIAM: An Introduction to Trajectory Optimization
中文资源
开源框架
学术论文
- Horizon: A Trajectory Optimization Framework
- Convex Optimization in Legged Robots
- Finding Locally Optimal Trajectories with Sequential Convex Optimization
- AL-iLQR Tutorial
扩展参考
-
路径规划与轨迹优化基础入门（中文）
-
机器人操作中的运动规划（Chapter 8: Motion Planning）
-
GCS: Motion Planning around Obstacles with Convex Optimization
T. Marcucci, M. Petersen, D. von Wrangel, R. Tedrake (Science Robotics, 2023) -
GCS‑GC: Global Path Planning via Graph Search and Convex Optimization
T. Marcucci et al. (IEEE T‑RO, 2024) -
CHOMP: Covariant Hamiltonian Optimization for Motion Planning
M. Zucker et al. (IJRR, 2013) -
TrajOpt: Motion Planning with Sequential Convex Optimization and Convex Collision Checking
J. Schulman et al. (IJRR, 2014) -
CES: Convex Elastic Smoothing for Motion Planning
Z. Marinho et al. (ICRA, 2016) -
Planner MiLP：混合整数线性规划在路径规划中的应用综述
T. Schouwenaars, "Safe Trajectory Planning of Autonomous Vehicles" (PhD Thesis, MIT, 2006) -
微分动态规划 DDP （原始文献）
D. H. Jacobson and D. Q. Mayne, "Differential Dynamic Programming", 1970现代实现参考：https://github.com/ethz-adrl/control-toolbox （含 iLQR/DDP）
-
轨迹优化开源库 OptimTraj (MATLAB)
Matthew P. Kellyhttps://github.com/MatthewPeterKelly/OptimTraj （包含直接转录法、配置法、Hermite--Simpson 等）
-
CasADi‑非线性优化与最优控制框架
https://web.casadi.org/ （支持 SQP、IPOPT，广泛用于轨迹优化）
-
OMPL: Open Motion Planning Library
https://ompl.kavrakilab.org/ （涵盖 PRM*、RRT*、BIT* 等多种采样规划算法，可与轨迹优化联合使用）
-
贝塞尔曲线在避碰轨迹规划中的应用
Z. Han et al., "Efficient Trajectory Planning using Bezier Curves with Convex Hull Property" (2020) -
基于强化学习的轨迹优化综述
J. Duan et al., "A survey of deep reinforcement learning for motion planning" (2022)