4、约束处理：MPC如何处理执行器饱和、状态约束与安全边界
各位同学，咱们今天聊个硬核话题——约束处理。
说实话，我最早做PID的时候，最头疼的就是限幅。你调好一组参数，飞得好好的，突然来个急转弯，电机直接饱和，飞机就开始抖。那时候我只能靠经验，把限幅值设得保守一点，但性能就下去了。
MPC不一样。它天生就带着约束处理的能力。说白了，MPC在规划未来轨迹的时候，就已经把「电机能转多快」「角度不能超过多少」这些限制考虑进去了。这是PID做不到的。
4.1 执行器饱和：别让电机硬扛
执行器饱和，就是电机转速到了极限，你给再大的控制量也没用。
PID遇到饱和，通常的做法是积分限幅、抗饱和。但MPC的做法更优雅——它直接在优化问题里加约束。
比如，你的电机PWM范围是1000到2000us。在MPC里，你只需要写：
u_min ≤ u(k) ≤ u_max
其中 u_min = 1000, u_max = 2000。
我在项目中遇到过一个问题：有个四旋翼，电机响应有延迟。单纯加约束还不够，因为MPC算出来的控制量，电机实际执行时可能因为动态响应而超调。嗯，这里要注意——
所以，执行器饱和的处理，不只是加个上下限那么简单。你还要考虑：
- 速率约束：电机转速变化率也有限制
- 动态余量：给执行器响应留出缓冲
- 优先级：多个执行器饱和时，哪个优先？
4.2 状态约束：别让飞机飞出安全区
状态约束，就是限制飞机的姿态、位置、速度等状态变量。
比如，你不想让飞机倾斜超过45度。在PID里，你只能靠限幅指令来实现。但MPC可以直接把角度约束写进优化问题：
|φ(k)| ≤ 45° (滚转角约束)
|θ(k)| ≤ 45° (俯仰角约束)
为什么这很重要？
你想想看，PID是「事后补救」——角度超了，我才往回拉。MPC是「事前规划」——它知道未来可能会超，提前就调整了。
我个人习惯把状态约束分成两类：
| 约束类型 | 示例 | 处理方式 |
|---|---|---|
| 硬约束 | 位置边界、障碍物 | 必须满足，违反即危险 |
| 软约束 | 姿态角、速度上限 | 允许轻微违反，但需惩罚 |
硬约束和软约束的区别，我举个例子你就明白了。
有一次做室内编队飞行，墙壁就是硬约束——撞上就炸机。但姿态角是软约束——偶尔超过45度，只要快速恢复，问题不大。
软约束的实现方式，是在代价函数里加一个惩罚项：
J = J_original + ρ * max(0, |φ| - 45°)^2
这个ρ是惩罚系数。ρ越大，约束越硬。我一般从100开始调，看效果再增减。
4.3 安全边界：给MPC加个「保险丝」
安全边界，说白了就是「最后一道防线」。
MPC再厉害，也是基于模型的。模型有误差，传感器有噪声，执行器有故障。万一MPC算出来的轨迹不安全怎么办？
我见过一个案例：某无人机在强风下，MPC预测的轨迹和实际偏差越来越大，最后撞树了。原因就是模型失配，MPC以为自己能飞过去，实际飞不过去。
所以，安全边界的设计，我建议分三层：
- 预测层：MPC内部约束，保证规划轨迹安全
- 监控层：实时检查状态是否接近边界
- 应急层：一旦越界，立即切换保护模式
应急层怎么做？我常用的方法是「安全备份轨迹」。比如，如果MPC失效，就执行一个预设的减速悬停轨迹。这个轨迹要保证：
- 在任何状态下都能安全执行
- 不依赖模型预测
- 响应时间在50ms以内
4.4 约束处理的工程实现
理论说完了，咱们看看代码怎么写。
以OSQP求解器为例，约束矩阵的构建是这样的：
# 状态约束：角度不超过45度
A_ub = np.array([[0, 0, 1, 0, 0, 0], # 滚转角
[0, 0, 0, 1, 0, 0]]) # 俯仰角
l_ub = np.array([-45, -45]) # 下界
u_ub = np.array([45, 45]) # 上界
# 控制约束：PWM 1000-2000
A_ctl = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
l_ctl = np.array([1000, 1000, 1000, 1000])
u_ctl = np.array([2000, 2000, 2000, 2000])
这里要注意一个细节：约束矩阵的维度要和预测时域匹配。比如预测时域是N=10，那约束矩阵就要重复10次。
我刚开始做的时候，经常在这里出错。矩阵维度不对，求解器直接报错。后来我写了个辅助函数，自动扩展约束矩阵：
def expand_constraints(A, l, u, N):
"""将单步约束扩展为预测时域约束"""
A_exp = np.kron(np.eye(N), A)
l_exp = np.tile(l, N)
u_exp = np.tile(u, N)
return A_exp, l_exp, u_exp
嗯，这个小工具帮我省了不少调试时间。
4.5 约束冲突怎么办？
这是MPC里最头疼的问题——约束之间互相矛盾。
比如，你想让飞机快速减速，但电机转速有下限，不能反转。这时候「快速减速」和「电机下限」就冲突了。
怎么办？
我的做法是：给约束排优先级。
- 一级约束：安全相关，必须满足（如位置边界）
- 二级约束：性能相关，尽量满足（如姿态角）
- 三级约束：舒适性相关，可以放松（如加速度变化率）
实现方式是用「松弛变量」。给低优先级的约束加一个松弛变量，允许它在一定范围内被违反：
# 软约束：允许姿态角最多超过5度
|φ| ≤ 45° + ε
ε ≥ 0
代价函数中加惩罚：ρ * ε^2
这个ε就是松弛变量。ρ越大，约束越硬。我一般把ρ设成1000，这样ε基本不会超过0.1度。
4.6 本章小结
好了，咱们把约束处理的核心内容过了一遍。
执行器饱和、状态约束、安全边界，这三者是MPC相比PID的最大优势。但优势背后也有代价——约束越多，计算越慢，调试越复杂。
我个人的建议是：
- 先从硬约束开始，保证安全
- 再逐步加软约束，提升性能
- 最后加安全边界，兜底保护
别想着一步到位。我见过太多人，一上来就加十几个约束，结果求解器跑不动，最后又全部删掉。慢慢来，比较快。