3、二次规划求解：QP问题标准化、有效集法、内点法、嵌入式QP求解器选型
各位同学，欢迎来到第三章。前两章我们把MPC的模型和预测搞清楚了，但真正让MPC跑起来的核心，其实是求解一个优化问题。说白了，就是解一个二次规划（QP）。
我刚开始做MPC的时候，觉得模型建好了就万事大吉。结果一上硬件，求解器跑不动，控制周期直接超时。嗯，那时候我才意识到——QP求解才是MPC落地的拦路虎。
这一章，我们就来啃这块硬骨头。我会带你从标准化开始，再到两种经典算法，最后聊聊嵌入式环境下的求解器选型。都是我在项目里踩过的坑，你拿回去就能用。
3.1 QP问题的标准化形式
先问个问题：为什么要把QP标准化？
原因很简单——所有求解器都认标准形式。你写个花里胡哨的QP，求解器不认识，它只认下面这种：
minimize 0.5 * x^T * H * x + f^T * x
subject to A * x <= b
Aeq * x = beq
lb <= x <= ub
这里H是Hessian矩阵，必须是对称半正定的。f是线性项系数。A和b是不等式约束，Aeq和beq是等式约束，lb和ub是边界。
我在项目中遇到过一个问题：明明模型没问题，但求解器报错说H不是半正定。查了半天，原来是数值误差导致H矩阵不对称了。后来我养成了一个习惯——每次构造完H，先检查对称性。
重要提醒：MPC问题中，H矩阵通常是H = B^T * Q * B + R。如果Q和R都是正定的，H就是正定的。但数值计算中，一定要加一个小的正则项，比如H = H + 1e-8 * I，防止奇异。
标准化还有一个好处：方便调试。你把MPC问题写成标准形式，就能用MATLAB的quadprog或者Python的cvxopt直接验证结果。我建议你刚开始做的时候，先用这些工具跑通，再移植到嵌入式平台。
3.2 有效集法（Active Set Method）
有效集法，说白了就是猜哪些约束是起作用的。你想想看，一个QP问题可能有几十上百个约束，但真正在最优解处起作用的，往往只有几个。有效集法就是不断猜、不断验证。
算法流程大致是这样的：
- 从一个可行点开始，猜一个有效集（起作用的约束集合）
- 在有效集上解一个等式约束的QP
- 检查解是否满足所有约束
- 如果不满足，调整有效集，重复
我记得第一次实现有效集法时，卡在了步骤3。解出来的点明明满足等式约束，但不等式约束却超了。后来发现是初始点选得不好。这里有个小技巧：用最小二乘法先求一个无约束解，再投影到可行域内作为初始点。
个人经验：有效集法在约束数量少的时候特别快。如果你的MPC问题只有10个以内的约束，有效集法是不二之选。我曾经在一个电机控制项目里用有效集法，控制周期做到了500微秒。
但有效集法有个致命弱点——约束多了就慢。当约束数量超过50个，每次迭代都要解一个线性方程组，计算量会爆炸。这时候，就该内点法登场了。
3.3 内点法（Interior Point Method）
内点法和有效集法的思路完全不同。有效集法是沿着边界走，内点法是从内部逼近最优解。它通过引入障碍函数，把不等式约束变成目标函数的一部分。
核心思想就一句话：把约束问题变成无约束问题。
具体做法是：
minimize 0.5 * x^T * H * x + f^T * x - μ * Σ log(s_i)
subject to A * x + s = b
s >= 0
这里的μ是障碍参数，s是松弛变量。当μ趋近于0时，解就趋近于原问题的最优解。
内点法的好处是：迭代次数基本固定，一般20-40次就能收敛，和约束数量关系不大。我在一个轨迹规划项目里用过，约束有200多个，内点法依然稳定收敛。
注意：内点法对数值精度要求高。μ的下降速度要控制好，降太快会不收敛，降太慢又浪费时间。我一般用μ_{k+1} = 0.2 * μ_k，这个比例在大多数嵌入式平台上表现不错。
但内点法也有缺点——内存占用大。每次迭代都要解一个大型线性方程组，对内存有限的嵌入式平台是个挑战。
3.4 嵌入式QP求解器选型
好了，理论讲完了，我们来点实际的。嵌入式平台资源有限，选求解器要综合考虑三点：速度、内存、精度。
我整理了一个对比表，方便你选型：
| 求解器 | 算法 | 适用场景 | 内存需求 | 速度 | 精度 |
|---|---|---|---|---|---|
| qpOASES | 有效集法 | 小规模、约束少 | 低 | 快 | 高 |
| OSQP | ADMM | 中等规模 | 中 | 中 | 中 |
| HPIPM | 内点法 | 大规模、约束多 | 高 | 中 | 高 |
| CVXGEN | 自定义代码生成 | 固定问题结构 | 极低 | 极快 | 高 |
| ACADO | 多种算法 | 复杂MPC | 中 | 中 | 高 |
我个人习惯是：问题规模固定且约束少，用qpOASES。它基于有效集法，代码量小，适合ARM Cortex-M系列。我曾经在一个STM32F4上跑qpOASES，控制周期1ms，稳得很。
如果约束多，比如超过50个，我建议用HPIPM。它是专门为嵌入式MPC设计的内点法求解器，支持代码生成，内存可以提前分配。
还有一个选择是CVXGEN。它把你的QP问题编译成C代码，运行时就是一堆矩阵运算，没有迭代过程。速度极快，但问题是——问题结构变了就得重新生成代码。适合产品定型后的量产阶段。
选型建议：
- 原型验证阶段：用MATLAB的quadprog或Python的cvxopt
- 嵌入式移植阶段：用qpOASES（小规模）或HPIPM（大规模）
- 产品量产阶段：用CVXGEN生成定制代码
我曾经犯过一个错误：在原型阶段用了一个很重的求解器，结果移植到嵌入式平台时发现内存不够。后来学乖了，从一开始就考虑嵌入式平台的限制。比如，提前算好H矩阵的稀疏性，用稀疏存储格式；约束矩阵A也尽量用稀疏格式。
最后，给你一个避坑指南：
- 我曾经因为H矩阵条件数太大，导致求解器迭代不收敛。后来加了正则化项，问题解决。
- 我曾经在有效集法里忘了处理退化的约束，导致死循环。后来加了最大迭代次数限制。
- 我曾经在内点法里μ下降太快，结果解直接飞了。后来改成自适应下降策略。
好了，这一章的内容就到这里。下一章我们会讲MPC的稳定性分析，到时候你会看到，QP求解的精度直接影响闭环稳定性。所以，这一章的基础一定要打牢。
有什么问题，欢迎在课程群里讨论。我们下章见。