MPC模型预测控制算法

1. 什么是MPC

MPC，Model Predictive Control，中文通常叫做模型预测控制。

它的核心思想很直接：

先建立系统模型。
在当前时刻，根据模型预测未来一段时间内系统状态如何变化。
通过求解一个优化问题，得到未来一串控制量。
只执行第一个控制量。
到下一个采样时刻重新测量状态，再重复上面的过程。

这个过程也叫做：

滚动优化
滑动时域优化
Receding Horizon Control

MPC和传统PID最大的区别在于：

PID更多依赖当前误差
MPC会显式预测未来
MPC可以自然处理输入约束和状态约束

2. 基本组成

一个标准MPC控制器通常由四部分组成：

2.1 预测模型

最常见的是离散时间状态空间模型：

$$
x_{k+1} = A x_k + B u_k
$$

如果有输出方程，还可以写成：

$$
y_k = C x_k + D u_k
$$

其中：

$x_k$ 是系统在时刻 $k$ 的状态
$u_k$ 是控制输入
$y_k$ 是输出
$A, B, C, D$ 是系统模型矩阵

2.2 预测时域

MPC不会只看下一步，而是看未来若干步。

$N_p$：预测时域，predict horizon
$N_c$：控制时域，control horizon

通常满足：

$$
N_c \le N_p
$$

含义是：

预测未来 $N_p$ 步状态
优化未来 $N_c$ 步控制输入

2.3 代价函数

MPC的目标是让系统既能跟踪参考轨迹，又不要控制过猛。

一个常见的二次型代价函数为：

$$
J = \sum_{i=1}^{N_p} (x_{k+i|k} - x_{ref,k+i})^T Q (x_{k+i|k} - x_{ref,k+i})

\sum_{i=0}^{N_c-1} u_{k+i|k}^T R u_{k+i|k}
\sum_{i=0}^{N_c-1} \Delta u_{k+i|k}^T S \Delta u_{k+i|k}
$$

其中：

第一项：惩罚状态误差，强调跟踪性能
第二项：惩罚控制输入过大
第三项：惩罚控制变化过快，让控制更平滑

权重矩阵的作用：

$Q$ 越大，越重视跟踪误差
$R$ 越大，越不愿意使用过大的控制量
$S$ 越大，控制输入变化越平稳

2.4 约束条件

MPC很重要的一个优势是可以直接把约束写进优化问题：

$$
u_{min} \le u_k \le u_{max}
$$

$$
x_{min} \le x_k \le x_{max}
$$

$$
\Delta u_{min} \le \Delta u_k \le \Delta u_{max}
$$

实际中常见的约束有：

电机最大力矩
方向盘最大转角
车辆最大加速度
无人机姿态角限制
速度和位置安全边界

3. MPC的工作流程

在第 $k$ 个采样时刻，MPC通常按下面步骤工作：

读取当前状态 $x_k$
根据系统模型预测未来状态
构造优化目标函数 $J$
加入输入和状态约束
求解最优控制序列
取最优序列中的第一个控制量 $u_k^\ast$
执行该控制量
时刻更新到 $k+1$，重新优化

这里最关键的一点是：

虽然求出来的是一串控制量，但真正执行的只有第一项。

这就是“反馈”产生的来源，因为每一步都会重新根据最新状态修正控制。

4. 线性MPC的预测模型推导

下面用一个最简单的离散系统说明预测是怎么展开的。

设系统为：

$$
x_{k+1} = x_k + u_k
$$

这相当于：

$$
A = 1,\quad B = 1
$$

如果预测两步，那么有：

$$
x_{k+1|k} = x_k + u_k
$$

$$
x_{k+2|k} = x_{k+1|k} + u_{k+1} = x_k + u_k + u_{k+1}
$$

定义控制向量：

$$
U =
\begin{bmatrix}
u_k \
u_{k+1}
\end{bmatrix}
$$

定义预测状态向量：

$$
X =
\begin{bmatrix}
x_{k+1|k} \
x_{k+2|k}
\end{bmatrix}
$$

则可以写成矩阵形式：

$$
X =
\begin{bmatrix}
1 \
1
\end{bmatrix} x_k
+
\begin{bmatrix}
1 & 0 \
1 & 1
\end{bmatrix}
\begin{bmatrix}
u_k \
u_{k+1}
\end{bmatrix}
$$

记为：

$$
X = F x_k + G U
$$

其中：

$$
F =
\begin{bmatrix}
1 \
1
\end{bmatrix},
\quad
G =
\begin{bmatrix}
1 & 0 \
1 & 1
\end{bmatrix}
$$

这个形式非常重要，因为一旦写成：

$$
X = F x_k + G U
$$

就可以把未来状态全部表示成“当前状态 + 控制序列”的函数，从而把问题变成标准优化问题。

5. 代价函数示例

假设希望系统跟踪参考值 $r$，则可以构造代价函数：

$$
J = (x_{k+1|k} - r)^2 + (x_{k+2|k} - r)^2 + \lambda (u_k^2 + u_{k+1}^2)
$$

这里：

前两项是跟踪误差
最后一项是控制代价
$\lambda > 0$ 是控制加权系数

性质可以这样理解：

$\lambda$ 较小：控制更激进，响应更快
$\lambda$ 较大：控制更保守，更平滑

如果写成矩阵形式，通常可以整理为：

$$
J = (F x_k + G U - R)^T Q (F x_k + G U - R) + U^T H U
$$

进一步可以化为标准二次规划形式：

$$
\min_U \frac{1}{2} U^T P U + q^T U
$$

如果还有线性不等式约束：

$$
M U \le b
$$

那么问题就变成标准QP问题，这也是线性MPC最常见的求解形式。

6. 为什么MPC适合机器人

机器人控制里，很多问题天然带有约束：

电机推力有限
舵机角度有限
速度和加速度有限
路径跟踪需要尽量平滑
需要提前预判未来轨迹偏差

MPC非常适合这类问题，因为它：

能提前预测未来误差，而不是只看当前误差
能显式加入速度、加速度、输入饱和等约束
能在轨迹跟踪和平滑控制之间做平衡
很适合做多变量耦合系统控制

比如无人车横向控制时，MPC通常可以同时考虑：

横向偏差
航向角误差
转角大小
转角变化率

这比单纯PID更系统，也更容易处理约束。

7. MPC的分类

按模型和求解形式，常见可以分成几类：

7.1 线性MPC

模型是线性的：

$$
x_{k+1} = A x_k + B u_k
$$

特点：

建模和求解相对简单
通常能转化为QP问题
实时性较好

适合：

中低速车辆控制
工作点附近的小范围控制
线性化后的无人机姿态或位置控制

7.2 非线性MPC

系统模型为：

$$
x_{k+1} = f(x_k, u_k)
$$

特点：

更接近真实系统
求解更复杂
计算量明显更大

适合：

高动态无人机
机械臂强非线性运动
复杂约束和复杂动力学场景

7.3 鲁棒MPC

针对模型不确定性、外部扰动进行设计，目标是在误差和扰动存在时仍保持稳定和约束满足。

7.4 自适应MPC

在线更新模型参数，使控制器适应系统参数变化。

8. MPC的优点与缺点

优点

能显式处理约束
适合多输入多输出系统
能利用未来参考轨迹进行预测控制
跟踪性能和平滑性通常较好
对复杂控制问题表达能力强

缺点

对模型依赖较强
每个采样周期都要在线优化
计算量比PID大很多
参数整定比PID更复杂

所以工程上常见做法是：

先做线性化
先降低模型维度
选较短预测时域保证实时性
使用高效QP求解器

9. 在ROS/自动驾驶/无人机中的典型应用

9.1 轨迹跟踪

给定一条参考轨迹，MPC根据当前状态预测未来位置误差，求解最优控制输入，让机器人尽量贴合轨迹。

9.2 速度控制

在速度规划完成后，MPC负责根据速度误差和约束输出油门、刹车或推力指令。

9.3 姿态与位置控制

无人机中常见的做法是：

外环MPC控制位置或速度
内环控制姿态和角速度

也有更高级的做法直接使用全状态MPC。

10. 一个标准MPC优化问题

线性离散系统中，一个典型MPC问题可以写为：

$$
\min_{u_{k|k},\dots,u_{k+N_c-1|k}}
\sum_{i=1}^{N_p}
|x_{k+i|k} - x_{ref,k+i}|Q^2
+
\sum{i=0}^{N_c-1}
|u_{k+i|k}|_R^2
$$

约束为：

$$
x_{k+i+1|k} = A x_{k+i|k} + B u_{k+i|k}
$$

$$
x_{min} \le x_{k+i|k} \le x_{max}
$$

$$
u_{min} \le u_{k+i|k} \le u_{max}
$$

初始条件：

$$
x_{k|k} = x_k
$$

求解后得到最优控制序列：

$$
U^\ast =
\begin{bmatrix}
u_{k|k}^\ast \
u_{k+1|k}^\ast \
\vdots \
u_{k+N_c-1|k}^\ast
\end{bmatrix}
$$

实际只执行第一项：

$$
u_k = u_{k|k}^\ast
$$

11. 总结

MPC的本质可以概括成一句话：

利用模型预测未来，在满足约束的条件下，通过在线优化得到当前最优控制输入。

它的三个关键词是：

预测
优化
约束

对于机器人控制来说，MPC是一种非常强的工具，尤其适合：

轨迹跟踪
多变量耦合控制
有输入/状态约束的场景