Model Predictive Control(MPC,模型预测控制) 是一种基于系统动力学模型、有限时域最优控制和滚动反馈优化的先进控制方法。
与传统 PID 不同,MPC 不仅考虑系统当前的误差,还会预测未来一段时间内系统的运动状态,并在满足速度、加速度、力矩、位置等约束的前提下,计算最优控制序列。
在机器人、无人机、自动驾驶、工业过程控制等领域,MPC 的价值是:将轨迹跟踪、动态性能、多变量耦合和物理约束统一到一个在线优化问题中。
一、MPC定义
1.1 从 PID 控制说起
考虑一个场景:机械臂末端沿预定轨迹运动,要求位置误差尽可能小,同时不能超过机械臂的最大速度和加速度。
传统 PID 控制器根据当前误差产生控制量:
( e ( t ) ) = ( r ( t ) ) − ( y ( t ) ) (e(t))=(r(t))-(y(t)) (e(t))=(r(t))−(y(t))
( u ( t ) ) = K p ( e ( t ) ) + K i ∫ 0 t e ( τ ) d τ + K d d ( e ( t ) ) d t (u(t))=K_p(e(t))+K_i\int_0^t e(\tau)d\tau+K_d\frac{d(e(t))}{dt} (u(t))=Kp(e(t))+Ki∫0te(τ)dτ+Kddtd(e(t))
其中:
- ( r ( t ) ) (r(t)) (r(t)):目标值
- ( y ( t ) ) (y(t)) (y(t)):实际输出
- ( e ( t ) ) (e(t)) (e(t)):跟踪误差
- ( u ( t ) ) (u(t)) (u(t)):控制器输出
PID 的特点是实现简单、计算成本低,适合大量工业控制场景。但基本 PID 并不显式预测未来轨迹,也不直接求解带约束的多步最优控制问题。
例如,机械臂正在高速接近目标位置:
如果只考虑当前的位置误差,控制器可能持续要求机械臂向目标加速,直到距离目标很近时才开始制动。
但此时由于惯性、最大减速度和执行器延迟等限制,即使已经发出制动指令,也可能发生超调。
MPC 处理这个问题的方式不同。
MPC 会预测:如果现在继续加速,那么未来 0.5 秒、1 秒乃至更长时间内,机械臂将运动到什么位置?是否还能及时减速?是否会违反速度或位置约束?
于是,MPC 可以根据未来的预测结果,提前调整当前控制量。
需要注意,经过合理设计的 PID、前馈控制和状态反馈控制同样能够实现提前制动。MPC 的区别不只是提前控制,而是把预测与约束显式地纳入优化过程。
1.2 MPC 的三个核心思想
MPC 可以概括为三个步骤:
第一,预测未来(Prediction)
通过动力学模型,计算系统在不同控制输入下的未来状态。
第二,优化控制(Optimization)
寻找一组控制输入,使未来预测轨迹尽可能接近目标,同时满足系统约束。
第三,滚动执行(Receding Horizon)
只执行优化得到的第一个控制输入,然后重新测量状态、重新预测和求解。
这就是 MPC 最重要的特点:每次计算一段未来,但只执行当前一步。
1.3 MPC 的运行机制
假设当前是时刻 k k k,MPC 预测未来 N N N 个控制周期:
x ^ ∗ k + 1 ∣ k , x ^ ∗ k + 2 ∣ k , ... , x ^ k + N ∣ k \hat{x}*{k+1|k},\hat{x}*{k+2|k},\ldots,\hat{x}_{k+N|k} x^∗k+1∣k,x^∗k+2∣k,...,x^k+N∣k
这里的下标 ( k + i ) ∣ k (k+i)|k (k+i)∣k 表示:基于时刻 k k k 已知的信息,对时刻 ( k + i ) (k+i) (k+i) 的状态所作的预测。
优化器得到未来控制序列:
U k ∗ = u k ∣ k ∗ u k + 1 ∣ k ∗ ⋮ u k + N − 1 ∣ k ∗ U_k^*= \begin{bmatrix} u_{k|k}^*\ u_{k+1|k}^*\ \vdots\ u_{k+N-1|k}^* \end{bmatrix} Uk∗=uk∣k∗ uk+1∣k∗ ⋮ uk+N−1∣k∗
但并不一次性执行全部控制序列,而是只执行:
u k = u k ∣ k ∗ \boxed{u_k=u_{k|k}^*} uk=uk∣k∗
进入下一个控制周期后,根据最新状态重新求解。
#mermaid-svg-XkZ98sCLVObOjgqY{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-XkZ98sCLVObOjgqY .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-XkZ98sCLVObOjgqY .error-icon{fill:#552222;}#mermaid-svg-XkZ98sCLVObOjgqY .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-XkZ98sCLVObOjgqY .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-XkZ98sCLVObOjgqY .marker{fill:#333333;stroke:#333333;}#mermaid-svg-XkZ98sCLVObOjgqY .marker.cross{stroke:#333333;}#mermaid-svg-XkZ98sCLVObOjgqY svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-XkZ98sCLVObOjgqY p{margin:0;}#mermaid-svg-XkZ98sCLVObOjgqY .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-XkZ98sCLVObOjgqY .cluster-label text{fill:#333;}#mermaid-svg-XkZ98sCLVObOjgqY .cluster-label span{color:#333;}#mermaid-svg-XkZ98sCLVObOjgqY .cluster-label span p{background-color:transparent;}#mermaid-svg-XkZ98sCLVObOjgqY .label text,#mermaid-svg-XkZ98sCLVObOjgqY span{fill:#333;color:#333;}#mermaid-svg-XkZ98sCLVObOjgqY .node rect,#mermaid-svg-XkZ98sCLVObOjgqY .node circle,#mermaid-svg-XkZ98sCLVObOjgqY .node ellipse,#mermaid-svg-XkZ98sCLVObOjgqY .node polygon,#mermaid-svg-XkZ98sCLVObOjgqY .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-XkZ98sCLVObOjgqY .rough-node .label text,#mermaid-svg-XkZ98sCLVObOjgqY .node .label text,#mermaid-svg-XkZ98sCLVObOjgqY .image-shape .label,#mermaid-svg-XkZ98sCLVObOjgqY .icon-shape .label{text-anchor:middle;}#mermaid-svg-XkZ98sCLVObOjgqY .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-XkZ98sCLVObOjgqY .rough-node .label,#mermaid-svg-XkZ98sCLVObOjgqY .node .label,#mermaid-svg-XkZ98sCLVObOjgqY .image-shape .label,#mermaid-svg-XkZ98sCLVObOjgqY .icon-shape .label{text-align:center;}#mermaid-svg-XkZ98sCLVObOjgqY .node.clickable{cursor:pointer;}#mermaid-svg-XkZ98sCLVObOjgqY .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-XkZ98sCLVObOjgqY .arrowheadPath{fill:#333333;}#mermaid-svg-XkZ98sCLVObOjgqY .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-XkZ98sCLVObOjgqY .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-XkZ98sCLVObOjgqY .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-XkZ98sCLVObOjgqY .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-XkZ98sCLVObOjgqY .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-XkZ98sCLVObOjgqY .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-XkZ98sCLVObOjgqY .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-XkZ98sCLVObOjgqY .cluster text{fill:#333;}#mermaid-svg-XkZ98sCLVObOjgqY .cluster span{color:#333;}#mermaid-svg-XkZ98sCLVObOjgqY div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-XkZ98sCLVObOjgqY .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-XkZ98sCLVObOjgqY rect.text{fill:none;stroke-width:0;}#mermaid-svg-XkZ98sCLVObOjgqY .icon-shape,#mermaid-svg-XkZ98sCLVObOjgqY .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-XkZ98sCLVObOjgqY .icon-shape p,#mermaid-svg-XkZ98sCLVObOjgqY .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-XkZ98sCLVObOjgqY .icon-shape .label rect,#mermaid-svg-XkZ98sCLVObOjgqY .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-XkZ98sCLVObOjgqY .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-XkZ98sCLVObOjgqY .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-XkZ98sCLVObOjgqY :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;}#mermaid-svg-XkZ98sCLVObOjgqY .state>*{fill:#dbeafe!important;stroke:#3b82f6!important;color:#1e3a8a!important;}#mermaid-svg-XkZ98sCLVObOjgqY .state span{fill:#dbeafe!important;stroke:#3b82f6!important;color:#1e3a8a!important;}#mermaid-svg-XkZ98sCLVObOjgqY .state tspan{fill:#1e3a8a!important;}#mermaid-svg-XkZ98sCLVObOjgqY .model>*{fill:#f8fafc!important;stroke:#94a3b8!important;color:#0f172a!important;}#mermaid-svg-XkZ98sCLVObOjgqY .model span{fill:#f8fafc!important;stroke:#94a3b8!important;color:#0f172a!important;}#mermaid-svg-XkZ98sCLVObOjgqY .model tspan{fill:#0f172a!important;}#mermaid-svg-XkZ98sCLVObOjgqY .opt>*{fill:#fef3c7!important;stroke:#f59e0b!important;color:#78350f!important;}#mermaid-svg-XkZ98sCLVObOjgqY .opt span{fill:#fef3c7!important;stroke:#f59e0b!important;color:#78350f!important;}#mermaid-svg-XkZ98sCLVObOjgqY .opt tspan{fill:#78350f!important;}#mermaid-svg-XkZ98sCLVObOjgqY .action>*{fill:#dcfce7!important;stroke:#22c55e!important;color:#14532d!important;}#mermaid-svg-XkZ98sCLVObOjgqY .action span{fill:#dcfce7!important;stroke:#22c55e!important;color:#14532d!important;}#mermaid-svg-XkZ98sCLVObOjgqY .action tspan{fill:#14532d!important;}#mermaid-svg-XkZ98sCLVObOjgqY .cycle>*{fill:#f3e8ff!important;stroke:#a855f7!important;color:#581c87!important;}#mermaid-svg-XkZ98sCLVObOjgqY .cycle span{fill:#f3e8ff!important;stroke:#a855f7!important;color:#581c87!important;}#mermaid-svg-XkZ98sCLVObOjgqY .cycle tspan{fill:#581c87!important;} 当前状态 x(k)
系统动力学模型
预测未来 N 步状态
求解约束优化问题
得到最优控制序列 U*
仅执行第一个 u*
系统产生实际运动
传感器反馈最新状态
进入下一周期
因此,MPC 是一种在线反复执行优化的反馈控制方法。
二、MPC 的数学基础:状态空间模型
MPC 的起点是受控系统的动力学模型。
如果无法合理描述当前控制输入如何影响未来状态,就难以建立有效的预测控制器。
2.1 连续时间状态空间模型
对于线性时不变系统(Linear Time-Invariant System,LTI),连续时间状态方程为:
x ˙ ( t ) = A x ( t ) + B ( u ( t ) ) \boxed{\dot{x}(t)=Ax(t)+B(u(t))} x˙(t)=Ax(t)+B(u(t))
输出方程为:
( y ( t ) ) = C x ( t ) + D ( u ( t ) ) (y(t))=Cx(t)+D(u(t)) (y(t))=Cx(t)+D(u(t))
其中:
| 符号 | 含义 |
|---|---|
| x ( t ) ∈ R n x(t)\in\mathbb{R}^{n} x(t)∈Rn | 系统状态向量 |
| ( u ( t ) ) ∈ R m (u(t))\in\mathbb{R}^{m} (u(t))∈Rm | 控制输入向量 |
| ( y ( t ) ) ∈ R p (y(t))\in\mathbb{R}^{p} (y(t))∈Rp | 系统输出向量 |
| A ∈ R n × n A\in\mathbb{R}^{n\times n} A∈Rn×n | 系统状态矩阵 |
| B ∈ R n × m B\in\mathbb{R}^{n\times m} B∈Rn×m | 输入矩阵 |
| ( C , D ) (C,D) (C,D) | 输出相关矩阵 |
从控制理论角度,状态向量必须包含足够的信息,使得已知当前状态和未来输入后,可以预测系统的后续演化。
例如,一维移动机构的状态可以定义为:
x ( t ) = p ( t ) v ( t ) x(t)= \begin{bmatrix} p(t)\ v(t) \end{bmatrix} x(t)=p(t) v(t)
其中 p p p 为位置, v v v 为速度。
假设控制输入是加速度:
( u ( t ) ) = a ( t ) (u(t))=a(t) (u(t))=a(t)
由运动学关系:
p ˙ ( t ) = v ( t ) \dot p(t)=v(t) p˙(t)=v(t)
v ˙ ( t ) = a ( t ) \dot v(t)=a(t) v˙(t)=a(t)
可得:
p ˙ v ˙ = 0 1 0 0 p v + 0 1 u \begin{bmatrix} \dot p\ \dot v \end{bmatrix} = \begin{bmatrix} 0&1\ 0&0 \end{bmatrix} \begin{bmatrix} p\ v \end{bmatrix} + \begin{bmatrix} 0\ 1 \end{bmatrix}u p˙ v˙=01 00p v+0 1u
这就是一个典型的双积分器模型(Double Integrator),广泛用于理解位置和速度控制。
必须强调:这里假设加速度可以被理想执行。对于实际伺服机构,若驱动器存在明显的带宽、延迟或饱和,还需要补充执行器模型。
2.2 为什么要离散化?
计算机控制器并不是连续地计算控制量,而是在一个个控制周期上更新。
假设控制周期为:
T s = 0.01 s T_s=0.01\ \text{s} Ts=0.01 s
即控制频率为:
f s = 1 T s = 100 Hz f_s=\frac{1}{T_s}=100\ \text{Hz} fs=Ts1=100 Hz
在零阶保持(Zero-Order Hold,ZOH)假设下,输入在一个采样周期内保持不变。
离散系统可以写成:
x k + 1 = A d x k + B d u k \boxed{x_{k+1}=A_dx_k+B_du_k} xk+1=Adxk+Bduk
对于连续线性系统,其精确的零阶保持离散化形式是:
A d = e A T s A_d=e^{AT_s} Ad=eATs
B d = ∫ 0 T s e A τ B , d τ B_d=\int_0^{T_s}e^{A\tau}B,d\tau Bd=∫0TseAτB,dτ
对于刚才的双积分器模型,可以得到:
p k + 1 v k + 1 = 1 T s 0 1 p k v k + 1 2 T s 2 T s u k \boxed{ \begin{bmatrix} p_{k+1}\ v_{k+1} \end{bmatrix} = \begin{bmatrix} 1&T_s\ 0&1 \end{bmatrix} \begin{bmatrix} p_k\ v_k \end{bmatrix} + \begin{bmatrix} \frac{1}{2}T_s^2\ T_s \end{bmatrix}u_k } pk+1 vk+1=1Ts 01pk vk+21Ts2 Tsuk
展开:
p k + 1 = p k + T s v k + 1 2 T s 2 u k p_{k+1}=p_k+T_sv_k+\frac12T_s^2u_k pk+1=pk+Tsvk+21Ts2uk
v k + 1 = v k + T s u k v_{k+1}=v_k+T_su_k vk+1=vk+Tsuk
从工程上理解,这个模型表达的是:
已知现在的位置、速度以及未来一个周期内施加的加速度,就能预测下一个周期的位置和速度。
MPC 正是利用这种数学关系不断向未来推演。
2.3 从预测一步到预测多步
假设离散动力学模型为:
x k + 1 = A x k + B u k x_{k+1}=Ax_k+Bu_k xk+1=Axk+Buk
为了简化符号,下文中的 ( A , B ) (A,B) (A,B) 均表示离散时间矩阵。
预测一步:
x k + 1 = A x k + B u k x_{k+1}=Ax_k+Bu_k xk+1=Axk+Buk
预测两步:
x k + 2 = A x k + 1 + B u k + 1 x_{k+2}=Ax_{k+1}+Bu_{k+1} xk+2=Axk+1+Buk+1
将第一步代入:
x k + 2 = A 2 x k + A B u k + B u k + 1 x_{k+2}=A^2x_k+ABu_k+Bu_{k+1} xk+2=A2xk+ABuk+Buk+1
预测三步:
x k + 3 = A 3 x k + A 2 B u k + A B u k + 1 + B u k + 2 x_{k+3}=A^3x_k+A^2Bu_k+ABu_{k+1}+Bu_{k+2} xk+3=A3xk+A2Buk+ABuk+1+Buk+2
依此类推:
x ( k + i ) = A i x k + ∑ j = 0 i − 1 A i − 1 − j B u k + j \boxed{ x_{(k+i)} = A^ix_k+ \sum_{j=0}^{i-1}A^{i-1-j}Bu_{k+j} } x(k+i)=Aixk+j=0∑i−1Ai−1−jBuk+j
这个公式说明:
未来状态由当前状态,以及从现在到未来这一段时间内的全部控制输入共同决定。
这也是 MPC 和单步反馈控制的重要区别。
2.4 构建多步预测矩阵
将未来 N N N 步状态堆叠成向量:
X = x k + 1 x k + 2 ⋮ x k + N X= \begin{bmatrix} x_{k+1}\ x_{k+2}\ \vdots\ x_{k+N} \end{bmatrix} X=xk+1 xk+2 ⋮ xk+N
将待优化控制量堆叠为:
U = u k u k + 1 ⋮ u k + N − 1 U= \begin{bmatrix} u_k\ u_{k+1}\ \vdots\ u_{k+N-1} \end{bmatrix} U=uk uk+1 ⋮ uk+N−1
可以写成统一的矩阵形式:
X = S x x k + S u U \boxed{X=S_xx_k+S_uU} X=Sxxk+SuU
其中:
S x = A A 2 ⋮ A N S_x= \begin{bmatrix} A\ A^2\ \vdots\ A^N \end{bmatrix} Sx=A A2 ⋮ AN
S u = B 0 ⋯ 0 A B B ⋯ 0 A 2 B A B ⋱ 0 ⋮ ⋮ ⋱ B S_u= \begin{bmatrix} B&0&\cdots&0\ AB&B&\cdots&0\ A^2B&AB&\ddots&0\ \vdots&\vdots&\ddots&B \end{bmatrix} Su=B0⋯0 ABB⋯0 A2BAB⋱0 ⋮⋮⋱B
这一步非常关键,因为它把未来所有状态都表示成当前状态和待优化控制序列的函数。
于是,MPC 的求解就从模拟系统运动,进一步转换为一个标准的数学优化问题。
MPC 的预测能力是通过数学模型,根据当前状态和候选控制输入推算未来状态。
三、MPC 的目标函数:如何定义"最优控制"
有了预测模型,我们就能计算未来状态。但控制器还需要回答另一个问题:
在许多可能的控制输入序列中,究竟应该选择哪一组?
MPC 使用目标函数(Cost Function / Objective Function) 来衡量控制方案的好坏。
3.1 MPC 优化问题
对于线性离散系统,可以构建如下有限时域最优控制问题:
min U J = ∑ i = 0 N − 1 ∣ x ( k + i ) ∣ k − r ( k + i ) ∣ ∗ Q 2 + ∑ ∗ i = 0 N − 1 ∣ u ( k + i ) ∣ k − u r e f , ( k + i ) ∣ ∗ R 2 + ∣ x ∗ k + N ∣ k − r k + N ∣ P 2 \boxed{ \begin{aligned} \min_{U}\quad J={}& \sum_{i=0}^{N-1} |x_{(k+i)|k}-r_{(k+i)}|*Q^2\ &+\sum*{i=0}^{N-1} |u_{(k+i)|k}-u_{\mathrm{ref},(k+i)}|*R^2\ &+|x*{k+N|k}-r_{k+N}|_P^2 \end{aligned} } UminJ=i=0∑N−1∣x(k+i)∣k−r(k+i)∣∗Q2 +∑∗i=0N−1∣u(k+i)∣k−uref,(k+i)∣∗R2 +∣x∗k+N∣k−rk+N∣P2
满足约束:
x ( k + i ) + 1 ∣ k = A x ( k + i ) ∣ k + B u ( k + i ) ∣ k x_{(k+i)+1|k}=Ax_{(k+i)|k}+Bu_{(k+i)|k} x(k+i)+1∣k=Ax(k+i)∣k+Bu(k+i)∣k
x min ≤ x ( k + i ) ∣ k ≤ x max x_{\min}\leq x_{(k+i)|k}\leq x_{\max} xmin≤x(k+i)∣k≤xmax
u min ≤ u ( k + i ) ∣ k ≤ u max u_{\min}\leq u_{(k+i)|k}\leq u_{\max} umin≤u(k+i)∣k≤umax
其中,符号 ∣ z ∣ Q 2 |z|_Q^2 ∣z∣Q2 的定义为:
∣ z ∣ Q 2 = z T Q z |z|_Q^2=z^TQz ∣z∣Q2=zTQz
矩阵 ( Q , R , P ) (Q,R,P) (Q,R,P) 分别表示状态跟踪权重、控制输入权重和终端状态权重。
这是典型的受约束线性二次 MPC 形式,OSQP 官方示例也采用这一基本结构。
3.2 状态误差惩罚:Q 矩阵
假设一维移动机构的状态为:
x = p v x= \begin{bmatrix}p\\v\end{bmatrix} x=pv
期望目标为:
r = p r e f v r e f r= \begin{bmatrix}p_{\mathrm{ref}}\\v_{\mathrm{ref}}\end{bmatrix} r=prefvref
定义:
Q = q p 0 0 q v Q= \begin{bmatrix} q_p&0\ 0&q_v \end{bmatrix} Q=qp0 0qv
那么状态误差的代价为:
J x = q p ( p − p r e f ) 2 + q v ( v − v r e f ) 2 J_x=q_p(p-p_{\mathrm{ref}})^2 +q_v(v-v_{\mathrm{ref}})^2 Jx=qp(p−pref)2+qv(v−vref)2
其中:
- q p q_p qp 较大:强调位置跟踪精度
- q v q_v qv 较大:强调速度跟踪精度
例如,在机械臂末端接近目标位置时,如果只惩罚位置误差,不重视末端速度,控制器可能允许机械臂以较高速度经过目标。
提高速度权重,能够使优化器更重视到达目标时的速度状态。
但权重并非越大越好。Q 矩阵决定不同控制目标之间的相对优先级,而非直接保证某个性能指标一定满足。
对于必须满足的速度上限,更可靠的建模方式是将它表示成硬约束。
3.3 控制输入惩罚:R 矩阵
状态跟踪精度并不是唯一需要考虑的目标。
例如,机械臂为了快速消除微小位置误差,如果不断输出很大的加速度命令,就可能导致机构振动、执行器频繁饱和,甚至加速机械磨损。
因此引入控制输入惩罚:
J u = u T R u J_u=u^TRu Ju=uTRu
如果控制输入是一维加速度:
J u = r u 2 J_u=ru^2 Ju=ru2
较大的 r r r 通常使控制器更保守,较小的 r r r 允许更积极的控制输入。
但如果控制输入本身代表保持某个平衡点所必需的非零力矩,例如机械臂抗重力力矩,就不应该简单地一直惩罚 u 2 u^2 u2。
更合理的是:
J u = ( u − u r e f ) T R ( u − u r e f ) J_u=(u-u_{\mathrm{ref}})^TR(u-u_{\mathrm{ref}}) Ju=(u−uref)TR(u−uref)
其中 u r e f u_{\mathrm{ref}} uref 是目标轨迹对应的期望输入或平衡输入。
3.4 控制增量惩罚:抑制抖动
工业控制中,经常希望控制输出尽可能平滑。
定义控制增量:
Δ u k = u k − u k − 1 \Delta u_k=u_k-u_{k-1} Δuk=uk−uk−1
加入代价项:
J Δ u = ∑ i = 0 N − 1 ∣ Δ u ( k + i ) ∣ k ∣ ∗ R ∗ Δ 2 J_{\Delta u}= \sum_{i=0}^{N-1} |\Delta u_{(k+i)|k}|*{R*\Delta}^2 JΔu=i=0∑N−1∣Δu(k+i)∣k∣∗R∗Δ2
完整代价函数就可以同时考虑跟踪误差、控制幅度和控制变化率。
如果 u u u 表示加速度,那么:
j k ≈ u k − u k − 1 T s j_k\approx \frac{u_k-u_{k-1}}{T_s} jk≈Tsuk−uk−1
便是离散意义上的 jerk(加加速度)。
因此,可以通过惩罚加速度变化率或显式加入 jerk 约束,使运动更加平滑。
注意:惩罚控制增量并不等于保证 jerk 不超限。对安全或机构寿命具有严格要求的指标,仍应建立明确的约束。
3.5 Q、R、P 如何选择?
这些权重不应该脱离变量量纲随意设定。
例如:
- 位置误差使用米
- 速度误差使用米每秒
- 加速度输入使用米每二次方秒
不同变量的数值尺度相差很大。如果直接设定所有权重为 1,得到的控制偏好未必符合设计目标。
工程中可以先进行归一化:
e ~ p = e p e p , s c a l e \tilde e_p=\frac{e_p}{e_{p,\mathrm{scale}}} e~p=ep,scaleep
e ~ v = e v e v , s c a l e \tilde e_v=\frac{e_v}{e_{v,\mathrm{scale}}} e~v=ev,scaleev
再在归一化空间设置权重。
终端权重 P P P 在跟踪性能之外还可能承担稳定性设计作用。对于适当的线性二次控制问题,可由离散代数 Riccati 方程计算与 LQR 相关的终端代价矩阵。
四、MPC 可以转化成二次规划
前面已经推导:
X = S x x k + S u U X=S_xx_k+S_uU X=Sxxk+SuU
假设先采用最简单的目标函数:
J = ( X − X r e f ) T Q ˉ ( X − X r e f ) + U T R ˉ U J=(X-X_{\mathrm{ref}})^T\bar Q(X-X_{\mathrm{ref}}) +U^T\bar RU J=(X−Xref)TQˉ(X−Xref)+UTRˉU
这里 Q ˉ \bar Q Qˉ 是整个预测时域的状态权重矩阵, R ˉ \bar R Rˉ 是控制输入权重矩阵。
代入预测方程:
J = ( S x x k + S u U − X r e f ) T Q ˉ ( S x x k + S u U − X r e f ) + U T R ˉ U J= (S_xx_k+S_uU-X_{\mathrm{ref}})^T \bar Q (S_xx_k+S_uU-X_{\mathrm{ref}}) +U^T\bar RU J=(Sxxk+SuU−Xref)TQˉ(Sxxk+SuU−Xref)+UTRˉU
展开:
J = U T ( S u T Q ˉ S u + R ˉ ) U + 2 ( S x x k − X r e f ) T Q ˉ S u U + constant \begin{aligned} J={}& U^T(S_u^T\bar QS_u+\bar R)U\ &+2(S_xx_k-X_{\mathrm{ref}})^T\bar QS_uU\ &+\text{constant} \end{aligned} J=UT(SuTQˉSu+Rˉ)U +2(Sxxk−Xref)TQˉSuU +constant
定义:
H = 2 ( S u T Q ˉ S u + R ˉ ) H=2(S_u^T\bar QS_u+\bar R) H=2(SuTQˉSu+Rˉ)
g = 2 S u T Q ˉ ( S x x k − X r e f ) g=2S_u^T\bar Q(S_xx_k-X_{\mathrm{ref}}) g=2SuTQˉ(Sxxk−Xref)
则可以整理为:
min U 1 2 U T H U + g T U \boxed{\min_U\frac12U^THU+g^TU} Umin21UTHU+gTU
这是标准的二次规划问题(Quadratic Programming,QP)。
对于线性系统,如果目标函数是凸二次型,约束是线性的,便能形成凸 QP。通常要求 H ⪰ 0 H\succeq0 H⪰0,若 H ≻ 0 H\succ0 H≻0,则在可行域非空的情况下,最优解唯一。
4.1 约束如何进入 QP?
以控制输入约束为例:
u min ≤ u k ≤ u max u_{\min}\leq u_k\leq u_{\max} umin≤uk≤umax
扩展到预测时域就是:
U min ≤ U ≤ U max U_{\min}\leq U\leq U_{\max} Umin≤U≤Umax
对于状态约束:
X min ≤ X ≤ X max X_{\min}\leq X\leq X_{\max} Xmin≤X≤Xmax
代入预测方程:
X min ≤ S x x k + S u U ≤ X max X_{\min}\leq S_xx_k+S_uU\leq X_{\max} Xmin≤Sxxk+SuU≤Xmax
整理:
X min − S x x k ≤ S u U ≤ X max − S x x k X_{\min}-S_xx_k \leq S_uU \leq X_{\max}-S_xx_k Xmin−Sxxk≤SuU≤Xmax−Sxxk
把输入和状态约束组合起来:
min U 1 2 U T H U + g T U s.t. l ≤ C U ≤ h \boxed{ \begin{aligned} \min_U \quad &\frac12U^THU+g^TU\ \text{s.t.}\quad &l\leq CU\leq h \end{aligned} } Umin21UTHU+gTU s.t.l≤CU≤h
这里 C C C 是约束矩阵, ( l , h ) (l,h) (l,h) 分别为上下界。
这正是 OSQP 等 QP 求解器所处理的问题形式。
4.2 MPC 和 LQR 关系
线性二次调节器(Linear Quadratic Regulator,LQR)也通过最小化状态与输入代价来产生控制。
典型无限时域离散 LQR 的代价函数为:
J = ∑ k = 0 ∞ ( x k T Q x k + u k T R u k ) J=\sum_{k=0}^{\infty} (x_k^TQx_k+u_k^TRu_k) J=k=0∑∞(xkTQxk+ukTRuk)
它在适当假设下得到线性状态反馈:
u k = − K x k u_k=-Kx_k uk=−Kxk
而 MPC 每个周期求解一个有限时域问题,并能够显式处理约束。
如果是无约束线性二次问题,且终端代价等条件选择适当,有限时域 MPC 可以得到与无限时域 LQR 相同的反馈控制律。
LQR 提供了最优状态反馈的重要理论基础,MPC 将有限时域优化、滚动反馈与约束处理结合起来。
五、MPC 的滚动时域:采样周期与预测窗口
| 参数 | 含义 | 影响 |
|---|---|---|
| T s T_s Ts | 控制采样周期 | 控制更新频率、离散模型精度 |
| N p N_p Np | 预测时域步数 | 系统预测未来多远 |
| N c N_c Nc | 控制时域步数 | 优化器具有多少控制自由度 |
其中:
T p r e d i c t i o n = N p T s T_{\mathrm{prediction}}=N_pT_s Tprediction=NpTs
假设:
T s = 0.02 s , N p = 25 T_s=0.02\ \text{s},\quad N_p=25 Ts=0.02 s,Np=25
则预测时间长度为:
T p r e d i c t i o n = 0.5 s T_{\mathrm{prediction}}=0.5\ \text{s} Tprediction=0.5 s
也就是说,控制器每隔 20ms 重新计算一次,同时预测未来 500ms 的系统行为。
5.1 预测步数越多越好吗?
不一定。
增加 N p N_p Np 能够让控制器提前考虑更远处的制动、障碍和状态约束。
但预测步数增加也会导致优化问题规模增大,计算开销上升,并且较远时刻的模型预测可能具有更大不确定性。
预测时域太短,则可能出现控制器无法提前制动或无法及时识别即将触发的约束等情况。
预测时域应足以覆盖系统主要动态行为,并能够提前反映约束影响,而非单纯追求尽可能长。
5.2 控制时域为什么可以小于预测时域?
在某些 MPC 实现中,可以设置:
N c < N p N_c<N_p Nc<Np
例如:
N p = 30 , N c = 5 N_p=30,\quad N_c=5 Np=30,Nc=5
代表预测未来 30 步,但只把前 5 步控制量作为独立优化变量,其余控制量按预设的参数化方式处理,例如保持最后一个控制量不变。
这种做法能够减少优化变量数量。
需要说明的是,控制时域较短和控制序列被整体执行是两回事。即使 N c = 5 N_c=5 Nc=5,本周期仍然只执行第一个控制量。
5.3 采样周期与实时性的权衡
MPC 的采样周期越小,往往越有利于及时响应扰动,但优化器必须更频繁地计算。
如果:
T s = 10 ms T_s=10\ \text{ms} Ts=10 ms
而求解器需要 15ms 才能完成一次优化,就无法直接满足同步 100Hz 控制周期的实时截止要求。
如果求解耗时波动很大,则还可能出现部分周期正常、部分周期超时的问题。
工程设计因此应满足:
T e s t i m a t e + T s o l v e + T c o m m u n i c a t i o n + T o t h e r ≤ T d e a d l i n e T_{\mathrm{estimate}} + T_{\mathrm{solve}} + T_{\mathrm{communication}} + T_{\mathrm{other}} \leq T_{\mathrm{deadline}} Testimate+Tsolve+Tcommunication+Tother≤Tdeadline
这里要特别关注最坏情况下的执行时间或经过充分测试的尾部延迟,而不只是平均耗时。
对于高频实时系统,能否稳定按时完成优化,往往比能否获得略低的目标函数值更加重要。
六、MPC 的稳定性
MPC 会在每个控制周期计算最优控制序列,但这不代表闭环系统必然稳定。
原因在于:有限时域内最优的控制行为,并不一定在无限长时间范围内保持稳定。
例如,对于一个预测窗口很短的控制器,它可能认为未来几步内继续加速是最优的,却没有充分考虑更远处的制动需求。
6.1 引入终端代价与终端约束
经典稳定性 MPC 通常考虑:
J N = ∑ i = 0 N − 1 ℓ ( x i , u i ) + V f ( x N ) J_N= \sum_{i=0}^{N-1}\ell(x_i,u_i)+V_f(x_N) JN=i=0∑N−1ℓ(xi,ui)+Vf(xN)
其中:
- ℓ ( x i , u i ) \ell(x_i,u_i) ℓ(xi,ui) 称为阶段代价(Stage Cost)
- V f ( x N ) V_f(x_N) Vf(xN) 称为终端代价(Terminal Cost)
再引入终端约束:
x N ∈ X f x_N\in X_f xN∈Xf
这里的 X f X_f Xf 称为终端集合(Terminal Set)。
直观理解:
- 阶段代价:要求预测过程中的状态和控制输入尽可能好
- 终端代价:对预测结束时仍未消除的状态误差进行评价
- 终端约束:要求预测结束时系统进入一个能够继续安全、稳定控制的区域
6.2 使用 Lyapunov 思想理解稳定性
假设在终端区域内存在局部控制律:
u = κ f ( x ) u=\kappa_f(x) u=κf(x)
并且满足:
V f ( f ( x , κ f ( x ) ) ) − V f ( x ) ≤ − ℓ ( x , κ f ( x ) ) V_f(f(x,\kappa_f(x)))-V_f(x) \leq-\ell(x,\kappa_f(x)) Vf(f(x,κf(x)))−Vf(x)≤−ℓ(x,κf(x))
这意味着,进入终端区域后,存在一个能够使终端代价持续下降的控制方式。
如果还满足合适的正定性、约束可行性和终端集合不变性等条件,可以证明:
V N ∗ ( x k + 1 ) ≤ V N ∗ ( x k ) − ℓ ( x k , u k ∗ ) \boxed{ V_N^*(x_{k+1}) \leq V_N^*(x_k)-\ell(x_k,u_k^*) } VN∗(xk+1)≤VN∗(xk)−ℓ(xk,uk∗)
这里 V N ∗ ( x k ) V_N^*(x_k) VN∗(xk) 表示时刻 k k k 的最优有限时域代价。
证明思路是:
上一周期获得长度为 N N N 的可行最优序列,下一周期将该序列向前移动一步,再在末尾补入局部终端控制律。
这样便构造了下一周期的一组可行候选解。
新的最优解,其代价不会比这组候选解更大。结合终端代价下降条件,就能得到上述不等式。
在适当条件下,最优代价可以作为 Lyapunov 函数,进一步证明闭环渐近稳定性。
但这个结论依赖模型、约束、终端条件和优化求解等假设;不能直接推广为所有 MPC 都稳定。
6.3 递归可行性(Recursive Feasibility)
当前周期优化问题有解,下一周期是否仍然有解?
例如,某移动机器人高速靠近障碍物。
当前状态虽然尚未越界,但如果速度过高、剩余制动距离不足,未来可能不存在任何同时满足位置约束和最大减速度约束的控制序列。这就是不可行问题(Infeasibility)。
递归可行性要求在相应条件下:
x k ∈ X f e a s i b l e ⟹ x k + 1 ∈ X f e a s i b l e x_k\in X_{\mathrm{feasible}} \Longrightarrow x_{k+1}\in X_{\mathrm{feasible}} xk∈Xfeasible⟹xk+1∈Xfeasible
终端可行集合、不变集合和合适的控制设计是实现递归可行性的常见理论工具。
还需要区分两种约束:
- 硬约束(Hard Constraint):必须严格满足,例如真正的安全边界、执行器物理极限
- 软约束(Soft Constraint):允许通过松弛变量违反部分要求,并承担惩罚
g ( x , u ) ≤ ϵ , ϵ ≥ 0 g(x,u)\leq\epsilon,\qquad \epsilon\geq0 g(x,u)≤ϵ,ϵ≥0
J s o f t = J + ρ ϵ 2 J_{\mathrm{soft}}=J+\rho\epsilon^2 Jsoft=J+ρϵ2
软约束能改善部分优化问题的可行性,但不能用它随意放松真正的安全约束。
七、非线性 MPC 与鲁棒 MPC
前面分析的线性 MPC,使用的是:
x k + 1 = A x k + B u k x_{k+1}=Ax_k+Bu_k xk+1=Axk+Buk
但真实机器人通常具有非线性动力学。
例如,机械臂动力学:
( M ( q ) ) q ¨ + C ( q , q ˙ ) q ˙ + ( g ( q ) ) = τ \boxed{ (M(q))\ddot q+C(q,\dot q)\dot q+(g(q))=\tau } (M(q))q¨+C(q,q˙)q˙+(g(q))=τ
其中:
- q q q 为关节位置向量
- ( M ( q ) ) (M(q)) (M(q)) 为惯性矩阵
- C ( q , q ˙ ) q ˙ C(q,\dot q)\dot q C(q,q˙)q˙ 代表科里奥利力与离心力相关项
- ( g ( q ) ) (g(q)) (g(q)) 为重力项
- τ \tau τ 为关节驱动力矩
这个系统显然不再是简单的线性状态方程。
7.1 非线性模型预测控制(NMPC)
非线性 MPC 使用:
x k + 1 = f ( x k , u k ) \boxed{x_{k+1}=f(x_k,u_k)} xk+1=f(xk,uk)
优化问题相应变为:
min x i , u i ∑ i = 0 N − 1 ℓ ( x i , u i ) + V f ( x N ) s.t. x i + 1 = f ( x i , u i ) g ( x i , u i ) ≤ 0 \begin{aligned} \min_{{x_i,u_i}}\quad& \sum_{i=0}^{N-1}\ell(x_i,u_i)+V_f(x_N)\ \text{s.t.}\quad& x_{i+1}=f(x_i,u_i)\ &g(x_i,u_i)\leq0 \end{aligned} xi,uimini=0∑N−1ℓ(xi,ui)+Vf(xN) s.t.xi+1=f(xi,ui) g(xi,ui)≤0
这通常属于非线性规划(Nonlinear Programming,NLP)。
与凸 QP 不同,NMPC 可能存在局部最优解、求解不收敛以及计算成本较高的问题。
因此,工程系统中经常在非线性模型准确性与求解速度之间做取舍。
7.2 LTV-MPC:线性时变模型预测控制
不一定所有系统都必须直接运行 NMPC。
可以在当前状态和参考轨迹附近,对非线性模型进行线性化:
δ x k + 1 ≈ A k δ x k + B k δ u k \delta x_{k+1} \approx A_k\delta x_k+B_k\delta u_k δxk+1≈Akδxk+Bkδuk
其中:
A k = ∂ f ∂ x ∣ ∗ ( x ˉ k , u ˉ k ) A_k=\left.\frac{\partial f}{\partial x}\right|*{(\bar x_k,\bar u_k)} Ak=∂x∂f ∗(xˉk,uˉk)
B k = ∂ f ∂ u ∣ ∗ ( x ˉ k , u ˉ k ) B_k=\left.\frac{\partial f}{\partial u}\right|*{(\bar x_k,\bar u_k)} Bk=∂u∂f ∗(xˉk,uˉk)
由于线性化工作点可能沿着预测时域变化,矩阵 A k , B k A_k,B_k Ak,Bk 也随时间变化。
于是,可以把部分非线性轨迹跟踪问题转化为一系列线性时变优化问题。
这种方式通常具有较好的实时性,但其有效性取决于局部线性化误差与工作范围。
7.3 鲁棒 MPC:模型预测不可能完全准确
真实系统通常还存在扰动:
x k + 1 = A x k + B u k + w k x_{k+1}=Ax_k+Bu_k+w_k xk+1=Axk+Buk+wk
其中 w k ∈ W w_k\in W wk∈W 表示模型误差、外部扰动以及未建模动态等。
普通的标称 MPC(Nominal MPC)通常基于无扰动模型:
x ^ k + 1 = A x ^ k + B u ^ k \hat x_{k+1}=A\hat x_k+B\hat u_k x^k+1=Ax^k+Bu^k
如果模型与真实系统偏差较大,那么优化得到的预测轨迹可能偏离实际轨迹。
一种经典处理方法是 Tube MPC(管状模型预测控制)。
它可以将实际控制输入设计为:
u k = v k + K ( x k − z k ) \boxed{u_k=v_k+K(x_k-z_k)} uk=vk+K(xk−zk)
其中:
- z k z_k zk 为标称状态
- v k v_k vk 为 MPC 优化出的标称控制输入
- K ( x k − z k ) K(x_k-z_k) K(xk−zk) 为纠正真实状态偏差的辅助反馈
标称系统满足:
z k + 1 = A z k + B v k z_{k+1}=Az_k+Bv_k zk+1=Azk+Bvk
定义误差:
e k = x k − z k e_k=x_k-z_k ek=xk−zk
则:
e k + 1 = ( A + B K ) e k + w k e_{k+1}=(A+BK)e_k+w_k ek+1=(A+BK)ek+wk
如果能够为误差系统构建合适的鲁棒不变集合 Z Z Z,则可以使用收紧后的约束:
z k ∈ X ⊖ Z z_k\in X\ominus Z zk∈X⊖Z
v k ∈ U ⊖ K Z v_k\in U\ominus KZ vk∈U⊖KZ
其中 ⊖ \ominus ⊖ 为 Pontryagin 差运算。
直观上,就是不给标称预测轨迹使用全部安全空间,而是预留一部分用于吸收外部扰动。
八、MPC 的求解
MPC 的难点之一是将理论优化问题转换为能够在控制周期内稳定完成的计算任务。
8.1 线性 MPC 的求解器
对于凸 QP,常用求解方法包含内点法、主动集方法以及基于算子分裂的算法。
例如 OSQP 使用算子分裂方法,支持问题数据更新、热启动,以及不可行性检测。
在连续控制周期中,系统矩阵通常不需要每次完全重新构建。
如果只有当前状态和参考轨迹发生变化,常常只需要更新 QP 中的线性项与部分约束边界。
OSQP Python 接口允许:
solver.update(q=q_new, l=l_new, u=u_new)
并支持使用前一个问题的解进行热启动,大幅提升求解速度。
8.2 非线性 MPC 的数值求解
NMPC 常用的方法包括:
Sequential Quadratic Programming(SQP,序列二次规划)
对非线性优化问题逐步构建局部 QP,并不断更新优化变量。
Real-Time Iteration(RTI,实时迭代)
针对实时最优控制场景,通过结构化计算与有限迭代减少在线反馈计算时间。
8.3 主流 MPC 软件对比
| 工具 | 主要定位 | 应用 |
|---|---|---|
| OSQP | 凸二次规划求解器 | 线性 MPC、凸 QP |
| acados | 高性能最优控制求解框架 | 实时 NMPC、嵌入式控制 |
| CasADi | 自动微分、符号优化建模 | NMPC 建模、轨迹优化 |
| do-mpc | Python MPC/MHE 工具箱 | 仿真、非线性与鲁棒控制 |
| MATLAB MPC Toolbox | 控制器设计、仿真、部署 | 工业控制与快速原型开发 |
需要区分建模框架 和数值求解器。
例如,CasADi 本身并不是一个单一的 MPC 控制器。它提供符号建模、自动微分、最优控制问题构建等能力,实际优化通常还需要底层 NLP 或 QP 求解器。
acados 则更接近面向实时最优控制问题的一套高性能求解框架,支持使用高级语言建立问题并生成可部署的 C 代码。
对于以 C++、Linux、ROS 2 为主要开发环境的机器人系统,建议先使用 OSQP 理解线性 MPC,再使用 CasADi 建立非线性控制模型,最后评估是否需要 acados 等面向实时部署的方案。
九、MPC 的 Python 实践:实现一个一维位置控制器
下面用前面推导的双积分器模型,建立一个完整的线性 MPC 仿真。
控制目标:让一个初始静止的移动机构,从 0m 移动到 1m,同时控制终点速度接近 0。
设定如下示例参数:
- 控制频率:10 Hz
- 预测时域:3.0 s
- 最大速度:0.6 m/s
- 最大加速度:0.8 m/s²
优化目标为:
min U ∑ i = 1 N ( x ( k + i ) − r ) T Q i ( x ( k + i ) − r ) + ∑ i = 0 N − 1 R u ( k + i ) 2 \min_U\sum_{i=1}^{N} (x_{(k+i)}-r)^TQ_i(x_{(k+i)}-r) +\sum_{i=0}^{N-1}Ru_{(k+i)}^2 Umini=1∑N(x(k+i)−r)TQi(x(k+i)−r)+i=0∑N−1Ru(k+i)2
约束:
∣ u k ∣ ≤ 0.8 |u_k|\leq0.8 ∣uk∣≤0.8
∣ v k ∣ ≤ 0.6 |v_k|\leq0.6 ∣vk∣≤0.6
这里使用 OSQP,依照其官方 MPC 示例中的 QP 数据构造及重复求解方式编写。
9.1 安装依赖
bash
pip install numpy scipy osqp
9.2 完整示例代码
python
import numpy as np
import osqp
from scipy import sparse
# ========== 1. 定义离散系统:位置 + 速度 ==========
Ts = 0.1 # 采样周期 100ms
N = 30 # 预测步数 30 步 = 3s
A = np.array([
[1.0, Ts],
[0.0, 1.0]
])
B = np.array([
[0.5 * Ts**2],
[Ts]
])
nx = A.shape[0] # 状态维度
nu = B.shape[1] # 输入维度
# ========== 2. 定义优化权重 ==========
Q = np.diag([20.0, 2.0]) # 状态权重:位置 > 速度
Qf = np.diag([40.0, 4.0]) # 终端权重
R = 0.2 # 输入权重
# ========== 3. 构建预测矩阵 X = F @ x + G @ U ==========
F = np.zeros((N * nx, nx))
G = np.zeros((N * nx, N * nu))
for i in range(N):
F[i*nx : (i+1)*nx] = np.linalg.matrix_power(A, i+1)
for j in range(i+1):
G[i*nx : (i+1)*nx, j*nu : (j+1)*nu] = (
np.linalg.matrix_power(A, i-j) @ B
)
# ========== 4. 构造 QP 矩阵 ==========
# 目标函数: 0.5 * U^T H U + g^T U
Q_bar = sparse.block_diag([sparse.kron(sparse.eye(N-1), Q), Qf])
R_bar = sparse.kron(sparse.eye(N), R)
H = G.T @ Q_bar @ G + R_bar
H = sparse.csc_matrix(H)
# 约束
u_max = 0.8
u_min = -u_max
v_max = 0.6
v_min = -v_max
# 输入约束
U_min = np.ones(N * nu) * u_min
U_max = np.ones(N * nu) * u_max
# 状态约束矩阵
x_min = np.array([-np.inf, v_min])
x_max = np.array([ np.inf, v_max])
X_min = np.kron(np.ones(N), x_min)
X_max = np.kron(np.ones(N), x_max)
# 组合约束: l <= Aineq * U <= u
Aineq = sparse.vstack([
sparse.eye(N * nu), # 输入约束
G # 状态约束
])
l = np.hstack([U_min, X_min - F @ np.zeros(nx)])
u = np.hstack([U_max, X_max - F @ np.zeros(nx)])
# ========== 5. 初始化求解器 ==========
solver = osqp.OSQP()
solver.setup(H, np.zeros(N*nu), Aineq, l, u, warm_start=True, verbose=False)
# ========== 6. 闭环仿真 ==========
x = np.array([0.0, 0.0]) # 初始状态:位置0,速度0
r = np.array([1.0, 0.0]) # 目标状态:位置1,速度0
history_x = [x.copy()]
history_u = []
for step in range(60):
# 更新线性项和约束边界
q = G.T @ Q_bar @ (F @ x - np.kron(np.ones(N), r))
l[:N*nx] = X_min - F @ x
u[:N*nx] = X_max - F @ x
solver.update(q=q, l=l, u=u)
res = solver.solve()
if res.info.status != 'solved':
raise ValueError(f"求解失败: {res.info.status}")
# 只执行第一个控制量
u0 = res.x[0]
history_u.append(u0)
# 系统演化
x = A @ x + B[:, 0] * u0
history_x.append(x.copy())
# 结果可自行绘图查看
print(f"最终位置: {x[0]:.4f} m, 最终速度: {x[1]:.4f} m/s")
这段代码最核心的不是 OSQP 的接口调用,而是下面的滚动优化逻辑:
python
for step in range(60):
# 根据当前状态更新优化问题
solver.update(q=q, l=l, u=u)
result = solver.solve()
# 只执行第一步控制量
u0 = result.x[0]
# 系统演化并进入下一控制周期
x = A @ x + B[:, 0] * u0
它完整体现了 MPC 的滚动优化机制。
对于这个理想模型,正常闭环响应应体现出先加速、接近速度上限、再减速并逐渐稳定在目标附近的行为。
注意:这份示例使用已知的理想状态更新,不包含状态估计器、传感器噪声、控制延迟和求解失败后的实际安全控制策略,只适用于学习与仿真
十、MPC 在机器人中的应用
对于机器人开发而言,MPC 一般不会单独构成完整控制系统。
实际架构通常包含感知、状态估计、参考轨迹生成、MPC 优化以及底层运动控制。
10.1 机器人控制系统的架构
#mermaid-svg-YURC0aPkV2olDZFR{font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-YURC0aPkV2olDZFR .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-YURC0aPkV2olDZFR .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-YURC0aPkV2olDZFR .error-icon{fill:hsl(220.5882352941, 100%, 98.3333333333%);}#mermaid-svg-YURC0aPkV2olDZFR .error-text{fill:rgb(8.5000000002, 5.7500000001, 0);stroke:rgb(8.5000000002, 5.7500000001, 0);}#mermaid-svg-YURC0aPkV2olDZFR .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-YURC0aPkV2olDZFR .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-YURC0aPkV2olDZFR .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-YURC0aPkV2olDZFR .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-YURC0aPkV2olDZFR .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-YURC0aPkV2olDZFR .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-YURC0aPkV2olDZFR .marker{fill:#0b0b0b;stroke:#0b0b0b;}#mermaid-svg-YURC0aPkV2olDZFR .marker.cross{stroke:#0b0b0b;}#mermaid-svg-YURC0aPkV2olDZFR svg{font-size:16px;}#mermaid-svg-YURC0aPkV2olDZFR p{margin:0;}#mermaid-svg-YURC0aPkV2olDZFR .label{color:#333;}#mermaid-svg-YURC0aPkV2olDZFR .cluster-label text{fill:rgb(8.5000000002, 5.7500000001, 0);}#mermaid-svg-YURC0aPkV2olDZFR .cluster-label span{color:rgb(8.5000000002, 5.7500000001, 0);}#mermaid-svg-YURC0aPkV2olDZFR .cluster-label span p{background-color:transparent;}#mermaid-svg-YURC0aPkV2olDZFR .label text,#mermaid-svg-YURC0aPkV2olDZFR span{fill:#333;color:#333;}#mermaid-svg-YURC0aPkV2olDZFR .node rect,#mermaid-svg-YURC0aPkV2olDZFR .node circle,#mermaid-svg-YURC0aPkV2olDZFR .node ellipse,#mermaid-svg-YURC0aPkV2olDZFR .node polygon,#mermaid-svg-YURC0aPkV2olDZFR .node path{fill:#fff4dd;stroke:hsl(40.5882352941, 60%, 83.3333333333%);stroke-width:1px;}#mermaid-svg-YURC0aPkV2olDZFR .rough-node .label text,#mermaid-svg-YURC0aPkV2olDZFR .node .label text,#mermaid-svg-YURC0aPkV2olDZFR .image-shape .label,#mermaid-svg-YURC0aPkV2olDZFR .icon-shape .label{text-anchor:middle;}#mermaid-svg-YURC0aPkV2olDZFR .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-YURC0aPkV2olDZFR .rough-node .label,#mermaid-svg-YURC0aPkV2olDZFR .node .label,#mermaid-svg-YURC0aPkV2olDZFR .image-shape .label,#mermaid-svg-YURC0aPkV2olDZFR .icon-shape .label{text-align:center;}#mermaid-svg-YURC0aPkV2olDZFR .node.clickable{cursor:pointer;}#mermaid-svg-YURC0aPkV2olDZFR .root .anchor path{fill:#0b0b0b!important;stroke-width:0;stroke:#0b0b0b;}#mermaid-svg-YURC0aPkV2olDZFR .arrowheadPath{fill:#0b0b0b;}#mermaid-svg-YURC0aPkV2olDZFR .edgePath .path{stroke:#0b0b0b;stroke-width:2.0px;}#mermaid-svg-YURC0aPkV2olDZFR .flowchart-link{stroke:#0b0b0b;fill:none;}#mermaid-svg-YURC0aPkV2olDZFR .edgeLabel{background-color:hsl(-79.4117647059, 100%, 93.3333333333%);text-align:center;}#mermaid-svg-YURC0aPkV2olDZFR .edgeLabel p{background-color:hsl(-79.4117647059, 100%, 93.3333333333%);}#mermaid-svg-YURC0aPkV2olDZFR .edgeLabel rect{opacity:0.5;background-color:hsl(-79.4117647059, 100%, 93.3333333333%);fill:hsl(-79.4117647059, 100%, 93.3333333333%);}#mermaid-svg-YURC0aPkV2olDZFR .labelBkg{background-color:rgba(243.9999999999, 220.9999999998, 255, 0.5);}#mermaid-svg-YURC0aPkV2olDZFR .cluster rect{fill:hsl(220.5882352941, 100%, 98.3333333333%);stroke:hsl(220.5882352941, 60%, 88.3333333333%);stroke-width:1px;}#mermaid-svg-YURC0aPkV2olDZFR .cluster text{fill:rgb(8.5000000002, 5.7500000001, 0);}#mermaid-svg-YURC0aPkV2olDZFR .cluster span{color:rgb(8.5000000002, 5.7500000001, 0);}#mermaid-svg-YURC0aPkV2olDZFR div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-size:12px;background:hsl(220.5882352941, 100%, 98.3333333333%);border:1px solid hsl(220.5882352941, 60%, 88.3333333333%);border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-YURC0aPkV2olDZFR .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-YURC0aPkV2olDZFR rect.text{fill:none;stroke-width:0;}#mermaid-svg-YURC0aPkV2olDZFR .icon-shape,#mermaid-svg-YURC0aPkV2olDZFR .image-shape{background-color:hsl(-79.4117647059, 100%, 93.3333333333%);text-align:center;}#mermaid-svg-YURC0aPkV2olDZFR .icon-shape p,#mermaid-svg-YURC0aPkV2olDZFR .image-shape p{background-color:hsl(-79.4117647059, 100%, 93.3333333333%);padding:2px;}#mermaid-svg-YURC0aPkV2olDZFR .icon-shape .label rect,#mermaid-svg-YURC0aPkV2olDZFR .image-shape .label rect{opacity:0.5;background-color:hsl(-79.4117647059, 100%, 93.3333333333%);fill:hsl(-79.4117647059, 100%, 93.3333333333%);}#mermaid-svg-YURC0aPkV2olDZFR .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-YURC0aPkV2olDZFR .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-YURC0aPkV2olDZFR :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;}#mermaid-svg-YURC0aPkV2olDZFR .sensor>*{fill:#e3f2fd!important;stroke:#1976d2!important;stroke-width:2px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .sensor span{fill:#e3f2fd!important;stroke:#1976d2!important;stroke-width:2px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .sensor tspan{fill:#333!important;}#mermaid-svg-YURC0aPkV2olDZFR .estimator>*{fill:#e0f7fa!important;stroke:#0097a7!important;stroke-width:2px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .estimator span{fill:#e0f7fa!important;stroke:#0097a7!important;stroke-width:2px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .estimator tspan{fill:#333!important;}#mermaid-svg-YURC0aPkV2olDZFR .mpc>*{fill:#e0f2f1!important;stroke:#00796b!important;stroke-width:3px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .mpc span{fill:#e0f2f1!important;stroke:#00796b!important;stroke-width:3px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .mpc tspan{fill:#333!important;}#mermaid-svg-YURC0aPkV2olDZFR .system>*{fill:#f5f7fa!important;stroke:#78909c!important;stroke-width:2px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .system span{fill:#f5f7fa!important;stroke:#78909c!important;stroke-width:2px!important;color:#333!important;rx:8px!important;ry:8px!important;}#mermaid-svg-YURC0aPkV2olDZFR .system tspan{fill:#333!important;} 相机 / IMU / 编码器
传感器反馈
参考轨迹 / 目标
位置、速度、约束
状态估计器
EKF / MHE
MPC 优化器
预测 · 约束 · 最优输入
底层伺服控制器
位置 / 速度 / 电流环
机器人 / 物理系统
这套架构中,MPC 主要回答:根据当前估计状态、未来目标轨迹以及机器人运动约束,下一步应该执行什么控制输入?
而状态估计器回答:机器人现在处于什么状态?
实际系统中,状态估计、MPC 和底层伺服控制可能以不同频率运行。
10.2 MPC 融合视觉和 IMU
可以构建视觉、IMU 与 MPC 联合工作的系统,但需要准确理解不同模块的职责。
视觉与 IMU 融合的核心通常是状态估计,而 MPC 是利用状态预测未来并优化控制。
对于视觉惯性系统:
z c a m e r a = h c ( x ) + v c z_{\mathrm{camera}}=h_c(x)+v_c zcamera=hc(x)+vc
z i m u = h i ( x ) + v i z_{\mathrm{imu}}=h_i(x)+v_i zimu=hi(x)+vi
可以通过 EKF、UKF、因子图优化或 MHE 等方法融合测量数据,获得当前状态估计:
x ^ k \hat x_k x^k
然后 MPC 使用这个估计结果:
x ^ k ⟶ MPC ⟶ u k \hat x_k\longrightarrow \text{MPC}\longrightarrow u_k x^k⟶MPC⟶uk
MHE(Moving Horizon Estimation,移动时域估计) 与 MPC 在数学上有相似之处:二者都在有限时间窗口内求解优化问题。
但 MHE 主要利用过去一段时间内的测量数据估计状态;MPC 主要优化未来一段时间内的控制动作。
十一、从仿真走向落地
实际项目中,一个数学上能够求出控制量的 MPC,不等于一个可以安全长期运行的控制器。
11.1 模型误差和状态估计误差
首先要验证预测模型。
可以使用实际采集数据,将每个时刻的状态和输入送入模型:
x ^ k + 1 = f ( x ^ k , u k ) \hat x_{k+1}=f(\hat x_k,u_k) x^k+1=f(x^k,uk)
然后与真实采集的下一时刻状态比较。
但不能只评估单步预测误差,还应该评估整个 MPC 预测时域内的多步误差。
例如:
E N = 1 N ∑ i = 1 N ∣ x ( k + i ) − x ^ ( k + i ) ∣ k ∣ 2 E_N= \frac1N\sum_{i=1}^{N} |x_{(k+i)}-\hat x_{(k+i)|k}|^2 EN=N1i=1∑N∣x(k+i)−x^(k+i)∣k∣2
如果模型只有第一步预测准确,经过 20 步便明显偏离真实运动,那么即使优化器完全正确,MPC 也可能选择不合理的控制动作。
11.2 不可行与超时问题
实际系统中,QP/NLP 求解器可能返回:
- 成功求解
- 原问题不可行
- 达到最大迭代次数
- 超过求解时间限制
- 数值精度不足
OSQP 接口明确区分这些不同的求解状态。
因此,控制代码不应该无条件读取求解结果并下发。
需要检查求解状态、数值有效性与约束满足程度。
失败时可根据系统安全要求切换到经过验证的备用控制器、受控减速、保持状态或安全停止策略。
备用动作不能简单地统一设定为零输入,因为对于某些系统,零输入本身可能不是安全状态。
11.3 控制频率不是越高越好
设机械系统物理响应相对缓慢,即使让 MPC 以 1kHz 运行,也不一定能改善实际闭环性能。
可能真正决定性能的是:
T s e n s o r , T c o m m u n i c a t i o n , T a c t u a t o r , T d y n a m i c s T_{\mathrm{sensor}} ,\quad T_{\mathrm{communication}} ,\quad T_{\mathrm{actuator}} ,\quad T_{\mathrm{dynamics}} Tsensor,Tcommunication,Tactuator,Tdynamics
MPC 控制周期应结合系统带宽、执行器能力、传感器更新率和计算资源共同选择。
对于已有高速底层伺服环的系统,较低频率的外层 MPC 加高速内部闭环,可能比在整个系统中统一使用高频 NMPC 更合适。
11.4 ROS 2 与 C++ 工程部署建议
可以将 MPC 设计为独立控制模块,内部维护状态估计、优化器工作区、约束参数和上一周期的控制解。
例如:
cpp
class MPCController
{
public:
bool initialize();
void setReference(const ReferenceTrajectory& ref);
void updateState(const EstimatedState& state);
ControlCommand solve();
private:
// 动力学模型
// QP / NLP 求解器实例
// 预测时域参数
// 控制约束参数
// Warm Start 缓存数据
// 求解失败处理逻辑
};
实际实现中,建议区分非实时配置通路 与周期性控制通路。
非实时通路负责参数加载、矩阵结构初始化和日志输出;控制通路尽量复用预分配的求解器工作区,避免不必要的动态内存分配以及不可控的阻塞操作。
尤其需要记录并监控:
T s o l v e , T j i t t e r , N i t e r a t i o n s , solver status T_{\mathrm{solve}} ,\quad T_{\mathrm{jitter}} ,\quad N_{\mathrm{iterations}} ,\quad \text{solver status} Tsolve,Tjitter,Niterations,solver status
其中求解器状态、求解时间分布、最大超时比例和约束违反情况,都是判断能否投产的重要指标。
总结
MPC 的知识体系可以浓缩为:
动力学建模 → 未来状态预测 → 约束优化 → 滚动反馈 → 闭环验证 \boxed{ \text{动力学建模} \rightarrow \text{未来状态预测} \rightarrow \text{约束优化} \rightarrow \text{滚动反馈} \rightarrow \text{闭环验证} } 动力学建模→未来状态预测→约束优化→滚动反馈→闭环验证
- 从控制算法角度,它通过数学模型预测未来,并计算有限时域内的最优控制动作。
- 从优化理论角度,它将跟踪误差、控制代价和系统约束放入统一的 QP 或 NLP 框架。
- 从控制理论角度,MPC 是否具有稳定性、递归可行性和鲁棒性,取决于模型、代价函数、约束以及相应的理论设计条件。
- 从软件工程角度,MPC 是一个需要在每个控制周期内可靠执行的在线优化任务,求解时间、数值稳定性、失败回退和接口延迟都属于控制系统设计的一部分。
- 从机器人系统架构角度,MPC 通常位于状态估计和底层伺服控制之间,与 EKF、MHE、轨迹规划器和执行器控制器协同工作。