第二章：等式约束处理——拉格朗日乘子法、罚函数法、增广拉格朗日法原理与对比
各位同学，咱们今天聊一个硬核话题——等式约束处理。
做轨迹规划，说白了就是在满足各种物理限制的前提下，找到一条最优路径。这些限制里，等式约束是最让人头疼的。为什么？因为等式约束要求你严格满足某个条件，差一点都不行。比如飞行器必须在某个时刻到达某个位置，这就是典型的等式约束。
我刚开始做轨迹规划那会儿，遇到等式约束就头大。后来慢慢摸索，发现主要有三把刀：拉格朗日乘子法、罚函数法、增广拉格朗日法。今天咱们就把这三把刀的原理、用法和坑都聊透。
核心观点：等式约束处理的核心思想，就是把约束问题转化为无约束问题。三种方法本质上都是这个思路，只是转化方式不同。
2.1 拉格朗日乘子法——理论上的完美解
先说说拉格朗日乘子法。这个方法理论很漂亮，但实际用起来有不少坑。
假设我们要最小化目标函数 f(x)，同时满足等式约束 h(x)=0。拉格朗日乘子法的思路很简单：引入一个乘子 λ，构造拉格朗日函数：
L(x, λ) = f(x) + λ·h(x)然后对 x 和 λ 分别求导，令导数为零。这样就把约束优化问题变成了求解方程组的问题。
我在项目中遇到过这样一个案例：设计一条无人机的最短路径，要求终点位置严格固定。用拉格朗日乘子法，理论上能直接得到解析解。但实际跑起来，发现方程组往往是非线性的，求解非常困难。
我的经验：拉格朗日乘子法适合小规模、低维度的问题。如果变量超过几十个，或者约束是非线性的，建议直接放弃这个方法。
拉格朗日乘子法的优点很明显：
- 理论上能精确满足约束
- 不需要调整超参数
- 能给出最优解的解析形式
缺点也很致命：
- 求解非线性方程组困难
- 对初始值敏感
- 大规模问题计算量爆炸
2.2 罚函数法——简单粗暴但有效
罚函数法的思路更直接：既然约束必须满足，那就在目标函数里加一个惩罚项，让违反约束的解变得很"贵"。
构造罚函数：
P(x, ρ) = f(x) + ρ·h(x)²其中 ρ 是罚因子，越大表示对违反约束的惩罚越重。当 ρ → ∞ 时，最优解会严格满足约束。
你想想看，这个方法多简单？直接把约束问题变成了无约束优化，用梯度下降法就能求解。
我曾经在一个飞行器再入轨迹规划项目里用过罚函数法。当时时间紧，任务重，拉格朗日乘子法搞不定，罚函数法三天就出了结果。虽然精度差了点，但至少能用了。
注意：罚函数法有个大坑——ρ 选小了，约束满足不了；ρ 选大了，目标函数变得非常陡峭，优化算法容易发散。我刚开始用的时候，光调 ρ 就调了一周。
罚函数法的优缺点：
- 优点：实现简单，容易理解，适合大规模问题
- 缺点：ρ 需要手动调整，约束只能近似满足，数值稳定性差
2.3 增广拉格朗日法——取长补短
增广拉格朗日法，说白了就是把前两种方法结合起来。它既保留了拉格朗日乘子法的精确性，又吸收了罚函数法的鲁棒性。
构造增广拉格朗日函数：
L_aug(x, λ, ρ) = f(x) + λ·h(x) + (ρ/2)·h(x)²你看，这里既有乘子项 λ·h(x)，又有罚项 (ρ/2)·h(x)²。通过交替更新 x 和 λ，可以逐步逼近精确解。
更新策略是这样的：
- 固定 λ，优化 x 使 L_aug 最小
- 更新 λ ← λ + ρ·h(x)
- 重复直到收敛
我记得有一次做卫星轨道转移的轨迹规划，约束条件特别苛刻——位置误差不能超过 0.1 米。用纯罚函数法，ρ 调到 10^8 还是达不到精度。换成增广拉格朗日法，ρ 只用到 10^4，迭代 50 步就收敛了。
为什么增广拉格朗日法更好？因为乘子项 λ·h(x) 提供了"记忆"功能。每次迭代后，λ 会记住之前违反约束的方向，引导下一步搜索。而罚项则保证了数值稳定性。
2.4 三种方法对比
咱们用一张表来对比这三种方法：
| 特性 | 拉格朗日乘子法 | 罚函数法 | 增广拉格朗日法 |
|---|---|---|---|
| 约束满足精度 | 精确 | 近似 | 精确 |
| 超参数数量 | 0 | 1 (ρ) | 2 (λ, ρ) |
| 数值稳定性 | 差 | 中等 | 好 |
| 实现难度 | 高 | 低 | 中 |
| 适用场景 | 小规模、线性约束 | 大规模、精度要求低 | 中等规模、精度要求高 |
从表中可以看出，增广拉格朗日法在大多数指标上都表现不错。这也是为什么现代轨迹规划算法中，增广拉格朗日法成了主流选择。
2.5 核心逻辑框架图
下面这张图展示了三种方法的核心逻辑关系：
2.6 实战建议
说了这么多，到底该用哪种方法？我给大家一个简单的决策流程：
- 问题规模小（变量 < 10），约束是线性的 → 用拉格朗日乘子法，直接求解析解
- 问题规模大（变量 > 100），精度要求不高 → 用罚函数法，简单快速
- 其他情况 → 用增广拉格朗日法，这是最稳妥的选择
一个小技巧：用增广拉格朗日法时，ρ 不要一开始就设得很大。我习惯从 ρ=1 开始，每迭代 10 步翻一倍，这样既保证了收敛速度，又避免了数值问题。
最后说一句：理论归理论，实际工程中，没有银弹。我见过太多人死磕拉格朗日乘子法，结果项目延期。有时候，用罚函数法先跑出一个近似解，再用增广拉格朗日法精化，反而效率更高。
嗯，今天就聊到这里。等式约束处理是轨迹规划的基础，后面讲不等式约束时，很多思路会复用今天的内容。