模型预测控制MPC具身智能详解_mpc控制原理-CSDN博客

本文链接：https://blog.csdn.net/lovely_yoshino/article/details/147227028

0. 简介

模型预测控制（Model Predictive Control，简称MPC）是自动驾驶和机器人领域中一种强大的控制方法，它通过优化未来一段时间内的系统行为来实现控制目标。本文将从控制理论和优化的角度，深入浅出地介绍MPC及其在自动驾驶领域的应用。

MPC由三个主要部分构成：
PID图所示，两个回路之间没有交叉，就像二者是相互独立一样,如果系统（Plant）足够大，因为调参十分困难

MPC的优势：可以处理多变量问题，并且可以添加约束制输入，并将第一个控制输入应用到系统中。

1. 控制理论与优化的交叉

控制理论和优化之间有着密切的联系，而模型预测控制恰好位于这两个领域的交叉点。MPC实际上是以优化方法来求解控制问题，或者说是以优化问题的求解来给出控制器的动作。MPC外层是控制的框架，内部融合了优化的思想。

1.1 模型预测控制的三大要素

MPC由四个主要部分构成：

预测模型：预测模型能够在短时间内根据历史信息、当前输入很好地预测系统状态的变化。需要一个模型能够基于历史信息和当前状态，来预测未来输出，这就涉及状态量的描述，非线性模型的线性化，从而确保预测输出最大限度接近期望值
优化控制序列：在预测范围内，通过数学优化选择一个最优的控制序列，使某个性能指标最小化（如能耗、跟踪误差）。
在线滚动优化：通过某种最优化算法（QP）来反复优化未来一段短时间的控制输入，使得在这种控制输入下预测模型的输出与参考值的差距最小，由于外部干扰，模型系统误差等原因，预测输出与实际存在偏差，滚动优化要做的就是找到每个时刻下的局部最优解，一般会设计一个损失函数，转化为二次规划问题，找到最优解
反馈校正：基于测量对模型预测进行修正，并在下一个时间点根据新的状态重新进行预测和优化

1.2 MPC基本工作原理

以车辆沿中线行驶为例，MPC的工作流程如下：

预测未来路径：基于当前状态，预测p步后可达到中线的路径
优化选择：从无穷多条可能路径中，选择"代价"最小的一条
执行第一步：仅执行优化路径中的第一步，舍弃后面步骤
滚动优化：在新的起点重复以上过程

1.3 MPC代价函数详解

在MPC中，代价函数(也称目标函数)是优化问题的核心，它定义了我们希望系统优化的目标。典型的MPC代价函数可以表示为：

$\sum_{i=0}^{N-1} x_{k+i}^T Q x_{k+i} + u_{k+i}^T R u_{k+i} + x_{k+N}^T F x_{k+N}$

其中：

$x_{k+i}$ 是预测的系统状态向量（包含位置、速度等状态变量）
$u_{k+i}$ 是控制输入向量（如方向盘角度、加速度等）
$Q$ 是状态权重矩阵，调整不同状态变量的重要性
$R$ 是控制输入权重矩阵，调整对控制输入幅值的惩罚程度
$F$ 是终端状态权重矩阵，用于保证系统在预测期末状态的稳定性
$N$ 是预测步数

对于车辆路径跟踪问题，这个代价函数实际上反映了两个主要目标：

跟踪精度：使车辆尽可能接近参考轨迹（状态误差最小化）
控制平滑性：避免控制输入的剧烈变化，保证舒适性和安全性

简化的代价函数形式也常见，特别是在实际应用中：

$J=\sum_{i=1}^{p}{w}_{e}{e}_{k+i}^{2}+\sum_{i=0}^{p-1}{w}_{\Delta u}{\Delta u}_{k+i}^{2}$

这里：

${e}_{k+i}$ 代表第k+i时刻的轨迹偏差（车辆位置与参考路径间的误差）
${\Delta u}_{k+i}$ 代表控制输入的变化量（例如方向盘转角的变化率）
${w}_{e}$ 和 ${w}_{\Delta u}$ 分别是对应的权重系数

这个简化形式直观地表达了两个关键目标之间的权衡：一方面我们希望车辆准确跟踪路径（第一项），另一方面我们希望控制动作平滑（第二项）。通过调整权重 ${w}_{e}$ 和 ${w}_{\Delta u}$ ，我们可以在跟踪精度和控制平滑性之间找到合适的平衡点。

在矩阵形式中，整个预测范围内的代价函数可以表示为：

$J = (X - X_{ref})^T Q (X - X_{ref}) + U^T R U$

其中：

$X$ 是包含预测范围内所有状态的向量 $x_{k+1}^T, x_{k+2}^T, ..., x_{k+N}^T]^T$
$X_{ref}$ 是参考轨迹
$U$ 是包含预测范围内所有控制输入的向量 $u_k^T, u_{k+1}^T, ..., u_{k+N-1}^T]^T$
$Q$ 和 $R$ 是相应的权重矩阵

在状态空间表示中，系统的状态转移方程为：

$x_{k+1} = Ax_k + Bu_k$

通过这个方程，可以将系统未来N步的状态预测表示为当前状态和未来控制输入的函数：

$\mathcal{A}x_k + \mathcal{B}U$

其中 $\mathcal{A}$ 和 $\mathcal{B}$ 是适当维度的矩阵，可以从系统动态模型导出。

将状态预测表达式代入代价函数，可以得到仅关于控制输入序列 $U$ 的二次函数，然后使用二次规划求解器求解最优控制序列。

2. MPC的数学原理

2.1 预测模型

以车辆运动学自行车模型为例，状态量更新公式：

$\begin{aligned} x_{k+1} &= x_k + v_k\cos(\theta_k)dt \\ y_{k+1} &= y_k + v_k\sin(\theta_k)dt \\ \theta_{k+1} &= \theta_k + v_k\frac{\tan(\delta_k)}{L}dt \\ v_{k+1} &= v_k + a_kdt \end{aligned}$

其中：

$(x, y)$ ：车辆位置
$\theta$ ：车辆航向角
$v$ ：车辆速度
$\delta$ ：前轮转向角
$a$ ：加速度
$L$ ：轴距
$d t$ ：时间步长

2.2 损失函数设计

MPC优化问题的损失函数通常包含以下几个部分：

横向偏移误差：轨迹和参考线的差距（CTE）
$\sum_{k=1}^{N}w_{cte}||cte_k||^2$
航向偏差：当前航向与期望航向的差距
$\sum_{k=1}^{N}w_{epsi}||epsi_k||^2$
速度误差：当前速度和期望速度的差距
$\sum_{k=1}^{N}w_v||v_k-v_{ref}||^2$
控制量约束：限制控制输入的幅值
$\sum_{k=1}^{N-1}(w_{\delta}||\delta_k||^2+w_a||a_k||^2)$
控制量变化率约束：使控制更加平滑
$\sum_{k=1}^{N-2}(w_{\delta_{rate}}||\delta_{k+1}-\delta_k||^2+w_{a_{rate}}||a_{k+1}-a_k||^2)$

2.3 约束条件

状态方程约束：
$\begin{aligned} x_{k+1} &= x_k + v_k\cos(\theta_k)dt \\ y_{k+1} &= y_k + v_k\sin(\theta_k)dt \\ \theta_{k+1} &= \theta_k + v_k\frac{\tan(\delta_k)}{L}dt \\ v_{k+1} &= v_k + a_kdt \\ cte_{k+1} &= f(x_k) - y_k + v_k\sin(\theta_k)dt \\ epsi_{k+1} &= arctan(f'(x_k)) - \theta_k + v_k\frac{\tan(\delta_k)}{L}dt \end{aligned}$
执行器约束：
$\begin{aligned} \delta &\in [\delta_{min}, \delta_{max}] \\ a &\in [a_{min}, a_{max}] \end{aligned}$

2.4 MPC的数学构建与代价函数推导

为了系统地构建MPC控制器，我们需要详细理解其数学基础，尤其是代价函数的构造和雅可比矩阵的推导过程。

2.4.1 MPC问题的形式化表述

首先，我们将MPC问题形式化为一个有限时域最优控制问题。对于离散时间系统，控制目标是在预测范围内最小化代价函数，同时满足系统动态约束和其他约束条件。

对于离散时间系统模型：
$x_{k+1} = f(x_k, u_k)$

其中 $x_k$ 是系统状态， $u_k$ 是控制输入。

2.4.2 代价函数构建的理论基础

MPC的代价函数通常包含两个主要部分：

状态偏差惩罚 - 衡量系统状态与参考轨迹的偏差
控制量惩罚 - 限制控制输入的大小以避免剧烈控制动作

标准形式的代价函数为：

$(\mathbf{X}-\mathbf{X}_{ref})^TQ(\mathbf{X}-\mathbf{X}_{ref}) + \mathbf{U}^TR\mathbf{U}$

这个函数的设计基于最优控制理论中的线性二次型调节器(LQR)思想，其理论基础可以追溯到贝尔曼最优性原理。代价函数中：

$\mathbf{X}$ 表示预测范围内的所有状态向量 $x_{k+1}^T, x_{k+2}^T, ..., x_{k+N}^T]^T$
$\mathbf{X}_{ref}$ 表示参考轨迹
$\mathbf{U}$ 表示控制输入序列 $u_k^T, u_{k+1}^T, ..., u_{k+N-1}^T]^T$
$Q$ 和 $R$ 是权重矩阵，用于平衡状态跟踪精度和控制代价

2.4.3 雅可比矩阵构建的必要性

当系统模型为非线性时，为了应用基于梯度的优化方法求解MPC问题，我们需要计算代价函数关于控制变量的梯度。这就需要构建雅可比矩阵。

雅可比矩阵本质上描述了系统状态对控制输入的敏感度。对于系统状态方程 $x_{k+1} = f(x_k, u_k)$ ，雅可比矩阵定义为：

$J_u = \frac{\partial f}{\partial u}$

它表示状态变化率对控制输入的偏导数。

2.4.4 雅可比矩阵的推导过程

以自行车模型为例，我们可以展示雅可比矩阵的推导过程：

首先列出系统状态方程：
$\begin{aligned} \dot{x} &= v\cos(\theta) \\ \dot{y} &= v\sin(\theta) \\ \dot{\theta} &= \frac{v\tan(\delta)}{L} \\ \dot{v} &= a \end{aligned}$
将连续时间模型离散化（使用欧拉前向差分法）：
$\begin{aligned} x_{k+1} &= x_k + v_k\cos(\theta_k)dt \\ y_{k+1} &= y_k + v_k\sin(\theta_k)dt \\ \theta_{k+1} &= \theta_k + \frac{v_k\tan(\delta_k)}{L}dt \\ v_{k+1} &= v_k + a_kdt \end{aligned}$
计算状态方程关于控制输入u的偏导数，得到雅可比矩阵：
$\begin{aligned} \frac{\partial x_{k+1}}{\partial \delta_k} &= 0 \\ \frac{\partial x_{k+1}}{\partial a_k} &= 0 \\ \frac{\partial y_{k+1}}{\partial \delta_k} &= 0 \\ \frac{\partial y_{k+1}}{\partial a_k} &= 0 \\ \frac{\partial \theta_{k+1}}{\partial \delta_k} &= \frac{v_kdt}{L\cos^2(\delta_k)} \\ \frac{\partial \theta_{k+1}}{\partial a_k} &= 0 \\ \frac{\partial v_{k+1}}{\partial \delta_k} &= 0 \\ \frac{\partial v_{k+1}}{\partial a_k} &= dt \end{aligned}$

这样就得到了单步雅可比矩阵：
$$
J_u =
\begin{bmatrix}
\frac{\partial x_{k+1}}{\partial \delta_k} & \frac{\partial x_{k+1}}{\partial a_k} \
\frac{\partial y_{k+1}}{\partial \delta_k} & \frac{\partial y_{k+1}}{\partial a_k} \
\frac{\partial \theta_{k+1}}{\partial \delta_k} & \frac{\partial \theta_{k+1}}{\partial a_k} \
\frac{\partial v_{k+1}}{\partial \delta_k} & \frac{\partial v_{k+1}}{\partial a_k}
\end{bmatrix}

\begin{bmatrix}
0 & 0 \
0 & 0 \
\frac{v_kdt}{L\cos^2(\delta_k)} & 0 \
0 & dt
\end{bmatrix}
$$

2.4.5 多步预测下的雅可比矩阵构建

在MPC中，我们关心的是整个预测范围内的状态对当前控制输入的敏感度。由于状态传播的链式效应，多步雅可比矩阵的构建变得复杂。

假设我们预测N步，控制序列为 $\mathbf{U} = [u_k, u_{k+1}, ..., u_{k+N-1}]$ ，
预测状态为 $\mathbf{X} = [x_{k+1}, x_{k+2}, ..., x_{k+N}]$ ，
那么完整的雅可比矩阵为：

$\frac{\partial \mathbf{X}}{\partial \mathbf{U}} = \begin{bmatrix} \frac{\partial x_{k+1}}{\partial u_k} & 0 & \cdots & 0 \\ \frac{\partial x_{k+2}}{\partial u_k} & \frac{\partial x_{k+2}}{\partial u_{k+1}} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial x_{k+N}}{\partial u_k} & \frac{\partial x_{k+N}}{\partial u_{k+1}} & \cdots & \frac{\partial x_{k+N}}{\partial u_{k+N-1}} \end{bmatrix}$

这个矩阵是下三角形式，因为未来状态只依赖于当前和过去的控制输入。对于非线性系统，这些偏导数通常需要数值计算或使用自动微分技术得到。下面的代价函数 $(\mathbf{X}-\mathbf{X}_{ref})^TQ(\mathbf{X}-\mathbf{X}_{ref}) + \mathbf{U}^TR\mathbf{U}$ 就是使用QR分解得到的约束，从而得到当前帧的雅可比矩阵。雅可比分解原理

2.4.6 雅可比矩阵、状态预测方程与代价函数的关系

在MPC中，我们有状态预测方程 $\mathbf{X} = \mathcal{A}x_k + \mathcal{B}\mathbf{U} + \mathcal{C}$ 和代价函数 $(\mathbf{X}-\mathbf{X}_{ref})^TQ(\mathbf{X}-\mathbf{X}_{ref}) + \mathbf{U}^TR\mathbf{U}$ ，状态预测方程与代价函数的关系：

状态预测方程 $\mathbf{X} = \mathcal{A}x_k + \mathcal{B}\mathbf{U} + \mathcal{C}$ 描述了控制输入 $\mathbf{U}$ 如何影响未来状态 $\mathbf{X}$
代价函数 $(\mathbf{X}-\mathbf{X}_{ref})^TQ(\mathbf{X}-\mathbf{X}_{ref}) + \mathbf{U}^TR\mathbf{U}$ 评估控制方案的好坏
将状态预测方程代入代价函数，可以得到仅关于控制输入 $\mathbf{U}$ 的函数：
$J(\mathbf{U}) = ((\mathcal{A}x_k + \mathcal{B}\mathbf{U} + \mathcal{C})-\mathbf{X}_{ref})^TQ((\mathcal{A}x_k + \mathcal{B}\mathbf{U} + \mathcal{C})-\mathbf{X}_{ref}) + \mathbf{U}^TR\mathbf{U}$

2.4.7 代价函数的统一简化表示

首先，MPC的代价函数表示为关于控制增量 $\Delta u$ 和状态向量 $\mathbf{X}$ 的二次函数：

$(\mathbf{X}-\mathbf{X}_{ref})^TQ(\mathbf{X}-\mathbf{X}_{ref}) + \Delta u^T R \Delta u$

代入状态预测方程 $\mathbf{X} = \mathcal{B}\Delta u + \mathbf{X}_0$ 后（其中 $\mathbf{X}_0$ 代表不受当前控制影响的状态部分）：

$((\mathcal{B}\Delta u + \mathbf{X}_0)-\mathbf{X}_{ref})^TQ((\mathcal{B}\Delta u + \mathbf{X}_0)-\mathbf{X}_{ref}) + \Delta u^T R \Delta u$

实际上我们在真正使用的时候会使用 $\Delta u$ 来作为控制，而不是直接 $\mathbf{U}$ ，需要通过迭代完成修正

$\mathbf{U}$ （绝对控制输入）
表示控制变量的实际值
通常定义为预测时域内所有控制输入的向量： $\mathbf{U} = [u_k^T, u_{k+1}^T, ..., u_{k+N-1}^T]^T$
在代价函数中以绝对形式出现： $\mathbf{U}^TR\mathbf{U}$
状态预测方程表示为： $\mathbf{X} = \mathcal{A}x_k + \mathcal{B}\mathbf{U} + \mathcal{C}$

$\Delta u$ （控制增量）
表示控制变量的变化量，即 $\Delta u_k = u_k - u_{k-1}$
强调控制信号的平滑变化
在代价函数中以增量形式出现： $\Delta u^T R \Delta u$
状态预测方程表示为： $\mathbf{X} = \mathcal{B}\Delta u + \mathbf{X}_0$

2.4.8 求解最优控制

当MPC问题没有约束条件时，我们通过微分求最优解：

对代价函数 $J$ 关于 $\Delta u$ 求偏导数：
$\frac{\partial J}{\partial \Delta u} = 2\mathcal{B}^TQ(\mathcal{B}\Delta u + \mathbf{X}_0 - \mathbf{X}_{ref}) + 2R\Delta u$
令偏导数等于零：
$\frac{\partial J}{\partial \Delta u} = 0$
求解得到最优控制增量：
$(\mathcal{B}^TQ\mathcal{B} + R)\Delta u = -\mathcal{B}^TQ(\mathbf{X}_0 - \mathbf{X}_{ref})$
$\Delta u = -(\mathcal{B}^TQ\mathcal{B} + R)^{-1}\mathcal{B}^TQ(\mathbf{X}_0 - \mathbf{X}_{ref})$

2.4.9 实际应用

在有约束条件的情况下，我们将代价函数转换为标准二次规划形式：

$\frac{1}{2}\Delta u^T H \Delta u + f^T\Delta u + c$

其中：

$2(\mathcal{B}^TQ\mathcal{B} + R)$
$2\mathcal{B}^TQ(\mathbf{X}_0 - \mathbf{X}_{ref})$
$c$ 为常数项，不影响优化结果

这种形式可以直接输入到QP求解器中，得到满足各种约束条件下的最优控制序列。

3. MPC参数设计

3.1 基本参数选择

采样时间 $T_s$ ：
- 太大：无法快速处理扰动
- 太小：计算负荷过大
- 建议： $\frac{T_r}{20} \leq T_s \leq \frac{T_r}{10}$ ，其中 $T_r$ 为系统开环响应时间
预测范围：
- 太短：缺乏前瞻性（例如制动距离不足）
- 太长：计算开销大，易受突发情况影响导致无效预测
- 建议：在开环响应时间内采样20-30个样本
控制范围：
- 控制范围过小：控制效果不佳
- 控制范围过大：后半部分效果有限但计算开销大
- 建议：控制范围应在预测范围的10~20%之间，至少2-3步