第四章：混合约束处理——SQP、内点法、可行方向法
好，咱们进入第四章。这一章我打算聊聊混合约束处理。说白了，就是当你的飞行轨迹问题里既有等式约束，又有不等式约束，甚至还有非线性约束时，该怎么办。
我在做无人机避障规划时，遇到过最头疼的情况：一边要满足动力学方程（等式），一边要躲开禁飞区（不等式），还得让控制量平滑。这时候单一方法根本搞不定。嗯，那就得上混合约束处理了。
4.1 序列二次规划（SQP）
SQP 是我个人比较偏爱的方法。为什么？因为它把非线性问题，拆成一系列二次规划子问题来解。你想想看，二次规划好解啊，成熟工具包一大堆。
核心思想：在每次迭代中，用二次模型近似目标函数，用线性模型近似约束。
数学上长这样：
min ½ d^T H_k d + ∇f(x_k)^T d
s.t. c_i(x_k) + ∇c_i(x_k)^T d = 0, i ∈ E
c_i(x_k) + ∇c_i(x_k)^T d ≤ 0, i ∈ I这里的 H_k 是拉格朗日函数的 Hessian 矩阵。我一般用 BFGS 来近似它，省得算二阶导。
关键点：SQP 的收敛性依赖于初始点。选不好，可能发散。
我在项目中遇到过一件事：有一次给一个高超声速飞行器做再入轨迹规划，初始点随便选了一个，结果 SQP 迭代了 50 步还不收敛。后来我改用可行性恢复阶段（feasibility restoration phase），先找到一个可行点，再跑 SQP，效果立竿见影。
SQP 的典型流程
- 给定初始点 x_0，设置 k=0
- 计算目标梯度 ∇f(x_k) 和约束雅可比矩阵
- 求解 QP 子问题，得到搜索方向 d_k
- 进行线搜索，确定步长 α_k
- 更新 x_{k+1} = x_k + α_k d_k
- 检查收敛条件，不满足则 k=k+1 返回步骤 2
我的小技巧：线搜索时别用单纯的目标函数下降，要用 merit function。我习惯用 l1 罚函数，简单粗暴有效。
4.2 内点法
内点法，也叫障碍法。它跟 SQP 的思路完全不同。它把不等式约束变成障碍项，加到目标函数里。
你想想看，原本的不等式约束 c_i(x) ≤ 0，我们改成在目标函数里加一项 -μ log(-c_i(x))。当 x 靠近约束边界时，log 项会趋向无穷大，把解「推」回可行域内部。
数学形式：
min f(x) - μ Σ log(-c_i(x))
s.t. c_i(x) = 0, i ∈ E这里的 μ 是障碍参数，随着迭代逐渐减小到 0。
内点法的优势：不需要主动识别哪些约束是活跃的。它天然地保持迭代点在可行域内部。
我曾经用内点法做再入飞行器的热流约束处理。热流约束是高度非线性的，SQP 经常在边界附近振荡。换成内点法后，迭代路径平滑多了。当然，代价是每次迭代要解一个更大的 KKT 系统。
注意：内点法对初始点有要求——必须严格可行。如果初始点不满足不等式约束，你得先跑一个可行性恢复阶段。
SQP vs 内点法：怎么选？
| 对比维度 | SQP | 内点法 |
|---|---|---|
| 迭代点位置 | 可在边界上 | 始终在内部 |
| 约束处理方式 | 显式线性化 | 隐式障碍项 |
| 适合场景 | 约束数量少，非线性弱 | 约束多，非线性强 |
| 收敛速度 | 超线性/二次收敛 | 超线性收敛 |
| 实现难度 | 中等 | 较高（需处理病态） |
我个人习惯：如果问题规模在几百个变量以内，约束不太多，我首选 SQP。如果约束成百上千，或者约束函数很「扭曲」，我会用内点法。
4.3 可行方向法
可行方向法，名字就说明了它的特点：每一步都保证迭代点可行。
它的思路是：在当前点 x_k，找一个方向 d_k，使得沿这个方向走一小步，目标函数下降，同时不违反约束。
对于线性约束问题，可行方向法特别高效。因为可行方向集是一个多面锥，很容易计算。
数学上，我们需要解这样一个子问题：
min ∇f(x_k)^T d
s.t. A d ≤ 0 (对活跃约束)
||d|| ≤ 1这里的 A 是活跃约束的系数矩阵。解出 d 后，做线搜索，保证新点仍在可行域内。
核心难点：当约束非线性时，可行方向的计算变得复杂。你需要考虑约束曲率。
我记得有一次做旋翼机的轨迹规划，约束主要是电机转速上下限（线性）和避障球（非线性）。我一开始用可行方向法处理线性约束部分，效果很好。但加上避障约束后，方向计算变得不稳定。后来我改用梯度投影法来处理非线性约束，算是可行方向法的一个变种。
三种方法的知识体系
下面这张图，是我梳理的三种方法的核心逻辑和适用场景：
4.4 实际应用中的选择策略
讲完理论，咱们聊聊实际中怎么选。我总结了几条经验：
- 问题规模：变量少于 500，用 SQP。变量上千，用内点法。
- 约束特性：如果大部分约束是线性的，可行方向法很香。如果全是非线性，别犹豫，上内点法。
- 实时性要求：SQP 每次迭代快，适合在线规划。内点法迭代次数少但每次计算量大，适合离线。
- 初始点质量：有好初始点，SQP 飞起。没有好初始点，内点法更鲁棒。
避坑指南：我曾经在一个项目里，为了追求理论上的二次收敛速度，硬上 SQP。结果约束函数高度非线性，线性化误差太大，迭代根本不收敛。后来换成内点法，虽然每次迭代慢一点，但整体收敛稳定。所以别迷信收敛阶，实用才是王道。
嗯，这一章的内容就到这。混合约束处理没有银弹，关键是根据问题特点选对工具。下一章咱们会聊更具体的轨迹参数化方法，到时候再结合这些约束处理方法一起用。
公众号:蓝海资料掘金营，微信deep3321