2. 数学基础

轨迹优化本质上是把“让机器人/飞行器沿期望路径运动”这一工程目标，翻译成一个带约束的数学优化问题。要读懂后续的求解器、实时性与收敛性讨论，必须先掌握三类基础工具：凸优化提供可全局求解的“安全区”，非线性优化处理真实世界中的非凸性，最优控制则把连续时间动力学与性能指标统一到变分框架中。本节围绕拉格朗日乘子法、KKT条件、庞特里亚金极小值原理以及轨迹优化的标准形式展开。

一、核心原理：从约束优化到最优控制

一般约束优化问题写作：

minimize    f(x)
subject to  g_i(x) <= 0,  i = 1..m
            h_j(x) = 0,   j = 1..p

拉格朗日乘子法的核心思想是：把等式约束“吸收”进目标函数，构造拉格朗日函数 L(x, λ) = f(x) + Σ λ_j h_j(x)。在正则条件下，原问题的最优解对应 L 的鞍点，从而把有约束问题转化为无约束的驻点问题。对于不等式约束，引入乘子 μ_i ≥ 0，并附加互补松弛条件 μ_i g_i(x) = 0，即“约束不起作用时乘子为零，约束起作用时乘子非负”。

把这些条件系统化，就得到 KKT 条件：

• 平稳性：∇f(x) + Σ μ_i ∇g_i(x) + Σ λ_j ∇h_j(x) = 0；
• 原始可行性：g_i(x) ≤ 0，h_j(x) = 0；
• 对偶可行性：μ_i ≥ 0；
• 互补松弛：μ_i g_i(x) = 0。

KKT 条件是非线性规划最优解的必要条件；当问题为凸问题（凸目标、凸不等式约束、仿射等式约束）时，KKT 条件同时是充分条件，且任何局部最优都是全局最优。这正是凸优化在轨迹优化中被高度重视的原因：它给出可验证、可复现的全局解。

二、方法流程：最优控制与极小值原理

轨迹优化通常写成连续时间最优控制问题：

minimize    J = Φ(x(t_f), t_f) + ∫ L(x(t), u(t), t) dt
subject to  ẋ(t) = f(x(t), u(t), t)          (动力学)
            x(t_0) = x_0,  x(t_f) ∈ X_f      (边界条件)
            u(t) ∈ U,  x(t) ∈ X              (路径约束)

庞特里亚金极小值原理（PMP）通过引入协态变量 λ(t)，构造哈密顿函数 H = L + λᵀ f，把最优控制问题转化为逐点最优条件：

• 协态方程：λ̇ = -∂H/∂x；
• 状态方程：ẋ = ∂H/∂λ = f；
• 极小值条件：u*(t) = argmin_{u∈U} H(x*, λ*, u, t)；
• 横截条件：λ(t_f) = ∂Φ/∂x(t_f)。

PMP 与 KKT 的关系可以这样理解：KKT 是静态约束优化的最优性条件，PMP 是其在连续时间动态系统中的推广，协态变量 λ(t) 扮演了“随时间变化的拉格朗日乘子”角色。二者共同构成轨迹优化求解的理论骨架。

三、案例/场景：无人机定点悬停的轨迹优化

考虑四旋翼从起点 x₀ 飞向目标点 x_f，要求最小化能量消耗并满足推力上限。其标准形式可写为：

min  J = ∫ (uᵀ R u) dt
s.t. ẋ = v
     v̇ = g + (1/m) R(q) u
     q̇ = 0.5 q ⊗ ω
     ω̇ = I⁻¹(τ - ω × Iω)
     x(0)=x₀, x(T)=x_f
     u_min ≤ u ≤ u_max

若把动力学线性化并忽略姿态耦合，问题可近似为凸二次规划，用 KKT 条件直接求解；若保留完整非线性动力学，则需用非线性优化或 PMP 打靶法。实际工程中常采用“先凸化求初值，再非线性精修”的两阶段策略，兼顾实时性与精度。

四、误区与复盘

五、关键概念对照清单

概念	数学对象	在轨迹优化中的作用	典型求解方式
拉格朗日乘子法	L(x, λ, μ)	将等式/不等式约束并入目标	解析法、增广拉格朗日
KKT 条件	平稳性 + 可行性 + 互补松弛	静态约束优化的最优性判据	内点法、SQP
庞特里亚金极小值原理	H = L + λᵀf	连续时间最优控制的逐点最优条件	打靶法、间接法
轨迹优化标准形式	min J, s.t. ẋ=f, 边界/路径约束	统一描述规划与控制问题	直接配点、直接打靶