第195篇 MPC约束处理——输入/状态/输出约束的工程实现

上篇聊了MPC的核心原理——预测、优化、滚动执行。MPC相比LQR最大的优势就是能处理约束。但能处理约束这句话说起来轻松真正落地的时候约束的类型、建模方式、求解难度每个环节都有坑。之前做四足机器人步态规划的团队就踩过一个大坑他们在MPC里只加了力矩约束没加关节角度约束。结果优化出来的控制量让髋关节转到了极限位置电机直接堵转报错。约束这东西漏一个就可能出事故。今天就把MPC中的约束处理掰开揉碎讲一遍。一、三类约束输入、状态、输出MPC中的约束分三大类输入约束|u| u_max。这是最常见的物理意义也最明确——电机最大力矩、阀门最大开度、推力器最大推力。输入约束是硬的执行器物理上就做不到你必须尊重。状态约束x_min x x_max。比如关节角度限位、飞行器最大倾斜角、电池SOC上下限。状态约束比输入约束更难处理因为它约束的是系统内部变量有些状态你还测不到。输出约束y_min y y_max。输出y Cx Du所以输出约束本质上是状态和输入的联合约束。比如机械臂末端位置不能超过某个区域无人机飞行高度不能超过限高。从数学上看这三类约束在MPC的优化问题中都是线性不等式约束。只要系统是线性的、代价函数是二次的整个优化问题就是一个标准的QP二次规划。二、输入约束的实现输入约束最简单直接写就行u_min u(ki|k) u_max, i 0, 1, ..., N-1实际工程中输入约束经常是不对称的。比如电机的正转力矩和反转力矩不一样大或者液压缸的推力和拉力不同。这时候约束写成u_min_j u_j(ki|k) u_max_j每个通道j的上下限可以不同。还有一种常见情况是输入变化率约束input rate constraint|u(k) - u(k-1)| delta_u_max这个约束限制控制量的变化速度防止执行器被突然的大信号打坏。在MPC中实现也不难只需要把u(k) - u(k-1)作为一个新的约束加进去。讲真输入约束是MPC最自然的约束类型。很多团队选择MPC就是因为它能优雅地处理输入约束——LQR做不到这一点。三、状态约束的 tricky 之处状态约束比输入约束麻烦得多。原因有几个第一状态约束需要所有状态都可观测。如果某些状态你测不到也估不准状态约束就没法严格执行。实际中你可能需要用软约束soft constraint来处理——允许少量违反但在代价函数中严重惩罚。第二状态约束可能导致优化问题不可行。你想想如果当前状态已经违反了某个约束比如关节角度已经超了限位那优化问题一开始就无解了。这时候你需要做一些特殊处理比如松弛约束或者切换到安全模式。第三状态约束可能和稳定性冲突。为了保证闭环稳定性MPC通常需要终端约束或者终端代价。如果状态约束太紧可能把终端不变集压缩到零导致稳定性无法保证。软约束的实现方式引入松弛变量s把硬约束x_min x x_max变成x_min - s x x_max s, s 0然后在代价函数中加上rho * s^2rho是一个很大的权重。这样优化器会尽量不违反约束但在极端情况下允许少量违反避免优化问题无解。四、代码示例带完整约束的MPCimport cvxpy as cp import numpy as np def mpc_with_constraints(x_current, N, A_d, B_d, Q, R, P, u_max, x_max, delta_u_max): nx, nu 4, 1 X cp.Variable((nx, N1)) U cp.Variable((nu, N)) cost 0 constraints [] for i in range(N): cost cp.quad_form(X[:,i], Q) cp.quad_form(U[:,i], R) constraints.append(X[:,i1] A_d X[:,i] B_d U[:,i]) # 输入约束 constraints.append(U[:,i] u_max) constraints.append(U[:,i] -u_max) # 状态约束软约束 constraints.append(X[:,i1] x_max) constraints.append(X[:,i1] -x_max) # 输入变化率约束 if i 0: constraints.append(U[:,i] - U[:,i-1] delta_u_max) constraints.append(U[:,i-1] - U[:,i] delta_u_max) cost cp.quad_form(X[:,N], P) # 终端代价 constraints.append(X[:,0] x_current) prob cp.Problem(cp.Minimize(cost), constraints) prob.solve(solvercp.OSQP, warm_startTrue) if prob.status infeasible: print(警告MPC优化问题不可行) return np.zeros(nu) return U[:,0].value注意代码里的几个细节输入约束是硬约束直接加不等式状态约束这里也写成硬约束实际中可能需要改成软约束输入变化率约束只在i0时加第一步没有前一步warm_startTrue很重要——用上一次的解作为初始猜测能大幅加速求解不可行检测如果prob.status是infeasible说明约束冲突了五、约束处理的工程经验经验一约束优先级。不同约束的重要程度不一样。力矩约束是硬的违反了会烧电机关节角度约束是软的少量违反可以接受。在MPC中可以用不同的松弛权重来区分优先级。经验二约束太多会拖慢求解。每加一个约束QP问题的约束矩阵就多几行。约束数量从50增加到200求解时间可能翻3-5倍。所以要精简约束只保留真正必要的。经验三不可行处理是必须的。实际运行中MPC优化问题偶尔会不可行传感器跳变、外部冲击等。你必须有一个fallback策略——要么松弛约束重新求解要么切换到备份控制器。不做不可行处理系统就会卡死。之前有个做无人机配送的团队MPC里加了20多个约束速度、加速度、倾斜角、角速度、电机转速...结果在阵风干扰下偶尔不可行无人机直接自由落体。后来加了软约束松弛和不可行检测才解决这个问题。六、面试实战QMPC中硬约束和软约束有什么区别A硬约束必须满足否则优化问题无解。软约束允许违反但会在代价函数中惩罚。工程中通常输入约束用硬约束状态约束用软约束保证优化问题始终可行。Q约束太多怎么办A几个策略1减少预测时域N2用活动集法active set method只处理活跃约束3约束裁剪constraint tightening提前去掉明显不会激活的约束。QMPC优化问题不可行怎么办A首先检查约束设置是否合理。如果约束本身没问题用软约束松弛保证可行性。同时加一个fallback控制器在MPC不可行时接管。小结MPC的约束处理是它最大的优势也是工程落地最复杂的部分。输入约束最直接状态约束最tricky输出约束是前两者的组合。工程上记住三点硬约束保安全软约束保可行不可行处理保命。这三点做到了MPC的约束处理基本就稳了。下篇聊MPC的实时求解——怎么让QP在几毫秒内解完以及怎么把MPC部署到嵌入式平台上。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第194篇 模型预测控制MPC原理——预测优化滚动执行下一篇预告第196篇 MPC实时求解——计算效率和嵌入式部署的挑战有任何问题欢迎评论区留言我会尽量回复。