自动驾驶控制算法——MPC控制算法

自动驾驶控制算法——MPC控制算法

一、MPC 是什么？

1.1 定义与核心思想

**模型预测控制（Model Predictive Control, MPC）**是一种基于系统预测模型、通过滚动优化方式求解最优控制输入的先进控制算法。

其核心思想包括：

• 使用数学模型预测未来系统状态演化；
• 定义优化目标函数，度量“预测轨迹”与“期望轨迹”的偏差；
• 在满足约束的前提下，求解控制输入序列以最小化目标函数；
• 每个控制周期仅执行第一步控制，然后滚动更新。

简而言之，MPC 是一种“我预测未来 → 优化选择 → 控制当前 → 重复预测”的闭环优化控制策略。

---

1.2 MPC 与传统控制器（如 PID）的区别

对比维度	PID 控制器	MPC 控制器
控制原理	基于误差反馈进行比例-积分-微分调节	基于预测模型进行多步优化控制
未来状态预测	无预测能力	利用模型预测未来多个时刻状态
控制输入设计	当前误差决定当前控制量	优化未来整个控制序列，仅执行第一个控制量
约束处理	无法显式处理约束	可直接嵌入输入/状态/路径等各种物理约束
复杂度	简单、易部署	模型依赖强、需实时求解优化问题
应用场景	适用于简单、线性、约束少的系统	更适合非线性、多变量、约束复杂的系统

MPC 相比 PID 更智能、鲁棒、灵活，特别适用于自动驾驶等对安全性与动态可控性要求极高的场景。

---

1.3 自动驾驶中 MPC 的典型应用场景

MPC 在自动驾驶系统中有以下典型应用：

🚗 轨迹跟踪控制（Trajectory Tracking）

• 控制车辆精确跟随一条参考轨迹；
• 可在考虑约束（如转角、加速度限制）的前提下实现平滑转向。

🛣️ 车道保持与变道决策（Lane Keeping / Changing）

• 保持车辆稳定行驶在车道中央；
• 支持动态规划与自主变道行为控制。

🧠 联合纵横向控制

• 同时控制加减速与转向行为，实现更协调的行驶动态。

🅿️ 自动泊车系统

• 在狭窄环境中规划最优路径并控制执行；
• 可规避障碍并实现低速高精度控制。

🧭 动态路径规划与避障

• 在动态障碍环境中进行预测式规避与实时调整；
• 保证路径安全性与可达性。

二、MPC 的原理（参考图文重构讲解）

MPC 的控制策略可以归纳为四步：

感知当前 → 预测未来 → 规划最优动作 → 只执行一步 → 重复整个过程

我们逐步讲解。

---

2.1 闭环结构总览：MPC 控制器如 何作用于系统？

从下图中我们可以看出，MPC 控制器运行在闭环系统中：

• $r_k$：目标参考值；
• $y_k$：系统实际输出；
• $e_k=r_k-y_k$：当前误差；
• MPC 控制器根据 $e_k$ 和模型预测，输出最优控制量 $u_k$​ 作用于系统。

控制流程：

1. 误差计算
   系统计算当前误差 $e_k=r_k-y_k$，并基于此来进行优化。
2. 预测与优化
   MPC 控制器根据系统动态模型预测未来一段时间内的系统行为，并在满足约束条件下最小化代价函数，选择最优控制序列。
3. 控制输出
   在计算得到最优控制序列后，MPC 只会选择第一个控制量 $u_k$​ 进行执行。
4. 实时调整
   由于系统和环境的变化，MPC 会持续地重新获取当前状态，重新计算最优控制动作，逐步修正控制策略。

总结：

MPC 控制器通过实时测量系统输出与目标之间的误差，根据预测模型预见未来行为，优化控制输入以最小化误差，从而实现系统的精准控制。它通过闭环结构，确保了控制系统在动态环境中持续跟踪并修正偏差。

---

2.2 预测未来：构建状态空间模型

在模型预测控制（MPC）中，控制器的核心任务之一是预测未来行为。为了做到这一点，MPC 依赖于系统的数学模型来推测未来几步的状态变化，并基于这些预测来优化当前的控制输入。为了“未雨绸缪”，MPC 在每个时刻都要使用当前的系统状态 $x_k$ 来预测未来的系统行为。

2.2.1 系统模型：离散状态空间模型

MPC 中使用的离散状态空间模型是将系统的动态过程通过数学方程表示出来。系统的状态空间模型通常是以下的形式：

$\begin{array}{rl}{x}_{k+1}& =A{x}_k+B{u}_k\\ y_k& =C{x}_k+D{u}_k\end{array}$

其中：

• $x_k$ 表示系统在时刻 $k$ 的状态向量，包含系统的内部变量（如位置、速度、角度等）。对于自动驾驶系统，状态 $x_k$ 可能包括车辆的位置、速度、航向角等；
• $u_k$ 是控制输入，例如加速度、转向角或其它驱动系统的控制量。在自动驾驶中，这通常是控制系统根据目标轨迹或目标行为所施加的指令；
• $y_k$ 是系统输出，表示系统在当前时刻的观测量。它是通过系统状态 $x_k$ 计算或测量得到的，通常是车辆的实时位置、速度等；
• $A,B,C,D$ 是系统参数矩阵，它们决定了状态与控制输入之间的关系，具体含义如下：
  • $A$ 是状态转移矩阵，它描述了在没有控制输入 $u_k$ 的情况下，系统状态如何从时刻 $k$ 变化到时刻$k+1$；
  • $B$ 是控制矩阵，它描述了控制输入$u_k$ 如何影响系统状态的变化；
  • $C$ 是输出矩阵，它将系统状态映射到观测值$y_k$；
  • $D$ 是输入矩阵，它描述了控制输入对系统输出的直接影响。

---

2.2.2 状态空间模型在 MPC 中的作用

MPC 控制器利用状态空间模型预测系统在未来 $N$ 步的状态演变。这些预测将用于优化控制输入 $u_k$，以实现最小化目标函数（通常是误差、能耗等）。

1. 状态更新
   MPC 通过模型对未来系统状态进行预测。根据当前时刻的状态 $x_k$​ 和控制输入 $u_k$​，通过状态转移矩阵 $A$ 和控制矩阵 $B$，MPC 预测出未来的状态 $x_{k+1},x_{k+2},\dots ,x_{k+N}$：

   $x_{k+1}=A{x}_k+B{u}_k$

   在每一个时刻，控制器都使用这个模型来更新状态，直到预测时域结束。

2. 输出预测
   同时，MPC 还预测系统的输出 $y_k$，这些输出将用来衡量控制效果，确保系统的行为尽可能接近目标状态：

   $y_k=C{x}_k+D{u}_k$

   在自动驾驶中，输出通常是车辆的实际位置、速度等，可以通过传感器测得。

---

2.3 控制预测序列：MPC 是怎么“看未来”的？

模型预测控制（MPC）之所以被称为“预测控制”，关键就在于它具备对未来进行规划与模拟的能力。与传统反馈控制器（如 PID）只基于当前误差进行调节不同，MPC 在每一个控制周期都会主动预测未来若干步的系统状态和控制行为，以达到更优的控制效果。

---

2.3.1 展望未来：预测控制输入序列

在当前时刻$k$，MPC 不仅仅为当前时刻计算一个控制输入 $u_k$，而是会规划一个未来 N 步的完整控制输入序列：

$u_k,u_{k+1},u_{k+2},\dots ,u_{k+N-1}$

这个序列代表了 MPC 预测在接下来 $N$ 个时间步内，系统应采取的最优控制动作。这个控制序列并不是立即全部执行，而是作为一个整体输入到优化器中，通过目标函数与约束条件综合评估，最终仅执行其中的第一个控制量 $u_k$，其余部分在下一时刻重新计算。

---

2.3.2 模型驱动的状态输出预测

有了完整的控制输入序列，MPC 就可以基于系统的状态空间模型（见 2.2 节）计算出系统的未来状态与输出：

$x_{k+1},x_{k+2},\dots ,x_{k+N},y_{k+1},y_{k+2},\dots ,y_{k+N}$

其中：

• 每一个未来的状态 $x_{k+i}$ 是在已知当前状态 $x_k$ 和未来控制量 $u_{k+i-1}$ 的基础上，逐步迭代预测得到的；
• 对应地，系统的输出 $y_{k+i}$ 可根据预测状态和控制量通过输出方程求得；
• 预测结果用于估计未来的控制误差，从而指导优化器进行控制策略选择。

---

2.3.3 控制目标：用预测误差最小化优化函数

为了使系统输出尽可能贴近参考轨迹 $r_k$，MPC 会在优化过程中计算每一个未来时间步上的误差项：

$e_{k+i}=r_{k+i}-y_{k+i}$

所有这些预测误差将被代入目标函数，常见的目标函数形式如下：

$J={\sum }_{i=0}^{N-1}\left(e_{k+i}^{T}Q{e}_{k+i}+u_{k+i}^{T}R{u}_{k+i}\right)$

通过使该代价函数最小，MPC 控制器能够在精度与能耗之间取得最优平衡。

---

2.3.4 可视化理解

为了更直观地理解 MPC 是如何“看未来”的，下面是一幅典型的控制预测示意图：

• 上图为预测控制输入 $u_k\sim u_{k+N-1}$，下图展示系统响应 $y_k\sim y_{k+N+1}$ 以及误差项 $e_k$。误差箭头以橙色标注，参考轨迹 $r$ 为虚线。

---

2.3.5 MPC 与传统控制的本质区别

传统 PID 控制器只看“当前误差”，是一个“点”上的反馈；
而 MPC 看的是“未来轨迹”，是一段时间上的最优规划。

这种全局考虑、局部执行的机制，使得 MPC 能够提前判断系统趋势，在不确定或动态环境中展现出更高的鲁棒性和性能。

---

2.4 优化目标函数：选择“最优”控制序列

MPC 的本质是一个在线优化问题。在每一个控制周期，MPC 都会预测未来一段时间内系统可能的运行轨迹，并在所有可能的控制序列中，选择出一组最优的控制动作序列，使得系统尽可能精确、平稳地跟踪目标轨迹，并在此过程中满足各种约束条件。

为了实现这一目的，MPC 引入了一个优化目标函数，该函数评估控制行为的“好坏”，并以此为依据选择控制输入。

---

2.4.1 目标函数定义

在标准的 MPC 框架中，目标函数一般被设计为如下二次型形式：

$J={\sum }_{i=0}^{N-1}\left(e_{k+i}^{T}Q{e}_{k+i}+u_{k+i}^{T}R{u}_{k+i}\right)+e_{k+N}^{T}F{e}_{k+N}$

其中：

• $J$：整体控制代价，优化器的最小化目标；
• $N$：预测时域长度，即 MPC“看未来”的步数；
• $e_{k+i}=r_{k+i}-y_{k+i}$：第 $i$ 步的预测误差；
• $u_{k+i}$：第 $i$ 步的控制输入；
• $Q$：状态误差的权重矩阵；
• $R$：控制输入的权重矩阵；
• $F$：终端状态的权重矩阵（用于强化最终收敛性）。

---

2.4.2 每项的含义与作用

项目	数学表达	作用说明
状态误差项	$e_{k+i}^{T}Q{e}_{k+i}$	惩罚输出偏离目标的行为，提高跟踪精度
控制能耗项	$u_{k+i}^{T}R{u}_{k+i}$	限制控制动作幅度，避免剧烈变化，提高舒适性或降低能耗
终端误差项	$e_{k+N}^{T}F{e}_{k+N}$	强化末尾状态的精度，提升系统长期稳定性（可选）

这种结构使得 MPC 能够在控制性能（精度）和执行代价（能耗或平滑性）之间灵活权衡。

---

2.4.3 权重矩阵 Q、R、F 的设计原则

1）$Q$：误差权重矩阵

• 权重越大，控制器越重视状态精度；
• 可以设置为对角矩阵，例如只关注某几个关键状态（如横向偏移、航向角）；
• 在自动驾驶中，通常对路径跟踪误差、角度偏差等状态加高权重。

2）$R$：控制量权重矩阵

• 权重越大，控制器越“吝啬”使用控制力，动作更保守；
• 可用于限制加速度、转向速度等不宜剧烈变化的物理量；
• 可增强系统平滑性与舒适性（尤其适合自动驾驶中的乘坐体验优化）。

3）$F$：终端状态权重矩阵

• 用于强化系统在预测末端的目标精度；
• 在有限预测窗口内具有重要作用，帮助系统更早收敛；
• 在飞行器/机器人路径规划中常常用于控制最终位姿误差。

---

2.4.4 优化目标函数的作用总结

通过设计目标函数，MPC 能够：

1. 引导系统准确地跟踪目标参考轨迹；
2. 限制控制行为的幅度和变化速度，提升舒适性或稳定性；
3. 在复杂系统中平衡多个目标，例如效率、精度与能耗之间的折衷；
4. 提供灵活的可调结构，便于在不同任务场景下进行参数微调。

---

2.4.5 工业与自动驾驶应用示例

场景	状态误差设计	控制输入设计	目标函数效果
自动驾驶跟踪车道	路径偏移、航向角误差加权	控制转向角、加速度的幅值限制	提高跟踪精度、避免抖动
四旋翼控制	空间位置误差加权	限制电机输出	平衡稳定飞行与电量使用
自动泊车	最终停靠点精度加权	控制动作最小化	提高泊车精度并限制动作次数

---

小结

MPC 控制器通过优化一个代价函数，在考虑未来系统误差、控制能耗、终端状态等多个维度的同时，动态规划控制策略。代价函数的构造体现了“系统想要什么”的核心思想，是 MPC 智能决策的基础。

你可以将目标函数理解为 MPC 控制器的“行为指南针”——它不直接告诉你怎么做，而是告诉你什么是“好”的控制行为，然后让优化器自己去找最优方案。

---

2.5 控制器只执行第一个动作：滚动优化思想

模型预测控制（MPC）之所以具有高鲁棒性与适应性，关键就在于它不是一次性计划所有未来的控制动作，而是采用一种“滚动优化（Receding Horizon Optimization）”的策略。

尽管在每一个控制周期，MPC 都会预测未来 $N$ 步的系统行为并生成一整段控制序列：

Uk={uk∗,uk+1∗,…,uk+N−1∗}U_k = \{u_k^*, u_{k+1}^*, \dots, u_{k+N-1}^*\}Uk​={uk∗​,uk+1∗​,…,uk+N−1∗​}

但它并不会将整个控制序列一次性全部执行，而是只执行其中的第一个控制量 $u_k^{\ast }$。然后：

1. 系统状态随时间前进（例如，车辆移动了一个时间步）；
2. 重新测量当前状态 $x_{k+1}$；
3. 重新进行预测与优化，生成新的控制序列 $U_{k+1}$；
4. 再次只执行第一个控制量 $u_{k+1}^{\ast }$；
5. 重复以上步骤，直到任务完成。

---

2.5.1 为什么只执行第一个动作？

虽然看似每次只执行一小步，实则是整个控制策略中最关键的稳定性保障机制。这是因为：

(1) 实际系统存在不确定性

• 例如外部扰动、传感器噪声、建模误差等；
• 如果一次执行完整个控制序列，将无法应对这些实时变化，易导致偏差累积或失稳。

(2) 控制目标可能发生变化

• 自动驾驶中，前车变道、障碍物出现、限速改变等都需要快速响应；
• 滚动优化能实时对目标变化做出调整。

(3) 状态预测具有时效性

• 预测时间越久，误差可能越大；
• 每次更新能保证 MPC 的预测是“基于最新现实”的。

因此，MPC 选择只执行当前最优动作，相当于在“步步为营”地调整整个系统的运行轨迹。

---

2.5.2 滚动优化流程图示

下图清晰地说明了 MPC 控制器在两个连续时刻的运行逻辑：

• 上图（时刻 $k$）：系统获取当前状态 $x_k$，预测未来 $N$ 步状态与输出，优化得到控制序列 $U_k$，并执行其中的第一个控制动作 $u_k^{\ast }$；
• 下图（时刻 $k+1$）：系统前进一步，状态更新为 $x_{k+1}$，重新预测与优化得到新的控制序列 $U_{k+1}$，并执行 $u_{k+1}^{\ast }$。

---

2.5.3 滚动优化带来的优势

优势	描述
实时性强	控制器每一时刻都基于最新观测和状态进行优化，适应动态环境
鲁棒性高	能处理建模误差、外部扰动和目标变化，防止失控
反馈性好	每步都使用新观测形成闭环反馈，实现“预测 + 纠正”的动态结合
控制平滑	连续优化产生平稳控制序列，避免动作突变

---

2.5.4 举例说明：自动驾驶中的滚动控制

假设自动驾驶车辆正在沿着一条弯道行驶，目标是精准贴合路面中心：

• 时刻 $k$：车辆刚进入弯道，MPC 提前预测了 10 米内的路径，规划了一组加速度和转向角；
• 系统执行第一个控制量，车辆向左转了一点；
• 由于前方突然出现一个障碍物，原先的控制序列失效；
• 时刻 $k+1$：MPC 重新获取新状态，发现障碍，重新规划路线；
• 系统立即切换新的控制序列，做出避让。

如果当初 MPC 一次性执行了全部 10 步控制，车辆可能会因为缺乏应变而撞上障碍物。

---

小结

MPC 的滚动优化思想体现的是一种“谨慎而智能的控制策略”：

“我会看得很远，但只走一步，走一步再重新看一遍。”

它融合了前瞻性规划与实时纠偏，使控制系统具备计划性、灵活性、鲁棒性与稳定性的多重优点，正是这种思想让 MPC 在自动驾驶、工业机器人、能源调度等高要求场景中得到广泛应用。

2.6 MPC 的工作流程总结

MPC 的控制方式融合了预测规划、在线优化和实时反馈，其本质可以看作是一个循环进行的“观察 → 预测 → 优化 → 执行”过程。每一次迭代，控制器都以“未来的最优”作为导向，以“当前的行动”作为执行策略。

---

2.6.1 控制流程结构图

这张图展示了 MPC 控制器在一个完整的控制周期中的各个步骤，以及它如何在每一时刻进行实时反馈和滚动优化。

---

2.6.2 MPC 每一时刻的完整工作步骤

Step 1：获取系统当前状态 $x_k$

• 通过传感器、估计器（如 Kalman 滤波器）等方式获取系统的当前状态；
• 状态可能包括位置、速度、角度、电流、电压等信息；
• 状态精度直接影响预测准确性，因此可靠的状态估计机制至关重要。

Step 2：构建预测模型，设定控制序列 Uk={uk,...,uk+N−1}U_k = \{u_k, ..., u_{k+N-1}\}Uk​={uk​,...,uk+N−1​}

• 基于系统建模结果构建状态空间模型；
• 在已知当前状态 $x_k$ 的前提下，控制器假设未来将使用控制序列 $U_k$，并据此预测系统状态 $x_{k+1},...,x_{k+N}$ 与输出 $y_{k+1},...,y_{k+N}$。

Step 3：通过优化求解最优控制序列 $U_k^{\ast }$

• 构建优化目标函数（见 2.4），综合考虑跟踪误差、控制能耗与约束限制；
• 采用优化算法（如 QP、IPM、SQP 等）进行数值求解；
• 结果是未来 $N$ 步的最优控制输入序列 $U_k^{\ast }$，其中包含当前时刻到未来时域内的所有控制策略。

Step 4：只执行第一个控制量 KaTeX parse error: Undefined control sequence: \* at position 5: u_k^\̲*̲

• MPC 只执行预测序列中第一个控制量 $u_k^{\ast }$，其余部分不会直接采用；
• 保留滚动优化机制的弹性，以应对未来扰动与变化；
• 保证控制系统的反馈性与稳定性。

Step 5：系统进入下一个控制时刻 $k+1$，重复整个流程

• 系统受控后进入新状态 $x_{k+1}$；
• 控制器重新观测、预测、优化，并输出新的控制策略；
• 整个过程构成一个连续、动态、滚动执行的优化控制闭环。

---

2.6.3 MPC 工作流程的优势总结

特性	描述
闭环控制	每一步都基于新状态重新优化，确保系统实时自适应变化
预测前瞻性	控制器不仅考虑当前误差，还考虑未来误差趋势，具备全局视野
局部执行策略	只执行最优序列中第一个控制动作，避免过度依赖预测假设，提升鲁棒性
强约束处理能力	可灵活添加物理约束、状态边界、安全距离等，使控制行为“可行且可控”
易于参数调整与调优	权重矩阵、预测步长、控制频率等可调节，以满足不同性能需求和资源限制

---

2.6.4 一句话总结

“MPC 就像是驾驶员边开车边预测前方道路，并不断微调方向盘与油门，只为每一刻都处在最优决策上。”

这种**“规划一步，执行一步”**的滚动机制，让 MPC 成为面对动态环境与多约束系统时最灵活、最可靠的控制策略之一。

三、无约束线性系统下的 MPC 控制

3.1 系统模型与假设

我们从最经典的系统出发，考虑如下线性离散时间、无约束系统：

$\begin{array}{rl}{x}_{k+1}& =A{x}_k+B{u}_k\\ y_k& =C{x}_k\end{array}$

3.1.1 假设条件说明：

• 线性：系统的动态行为满足线性状态空间模型；
• 无约束：对输入 $u_k$ 和状态 $x_k$ 没有任何限制；
• 全状态可测：可通过传感器直接获得 $x_k$，或能从$y_k$ 中准确估计。

由于满足上述条件，该类问题可构建为凸二次规划问题，且具有解析解，是最易入门的 MPC 实现形式。

---

3.2 方法一：解析推导法

我们希望求解未来 $N$ 步的最优控制序列：

$U=\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]\in {\mathbb{R}}^{Nm}$

使得如下代价函数 JJJ 最小：

$J={\sum }_{i=0}^{N-1}\left(x_i^{\top }Q{x}_i+u_i^{\top }R{u}_i\right)+x_N^{\top }P{x}_N$

其中：

• $Q\succcurlyeq 0$：状态加权矩阵；
• $R\succ 0$：输入加权矩阵；
• $P\succcurlyeq 0$：终端状态加权矩阵。

---

Step 1：构建代价函数矩阵形式

将代价函数转写为向量二次型形式：

$J=X^{\top }\bar{Q}X+U^{\top }\bar{R}U$

其中：

• $X={\left[\begin{array}{cccc}{x}_1^{\top }& x_2^{\top }& \dots & x_N^{\top }\end{array}\right]}^{\top }$
• $\bar{Q}=\text{diag}(Q,Q,\dots ,Q,P)$
• $\bar{R}=\text{diag}(R,R,\dots ,R)$

又由于系统状态满足递推关系：

$X=\bar{T}U+\bar{S}{x}_0$

其中：

• $\bar{T}\in {\mathbb{R}}^{nN\times mN}$：输入响应矩阵；
• $\bar{S}\in {\mathbb{R}}^{nN\times n}$：状态响应矩阵（类似 $\mathcal{A}$）；

将其代入目标函数中，得：

$J=(\bar{T}U+\bar{S}{x}_0{)}^{\top }\bar{Q}(\bar{T}U+\bar{S}{x}_0)+U^{\top }\bar{R}U$

整理为标准二次型：

$J(U)=\frac{1}{2}{U}^{\top }HU+x_0^{\top }FU+\text{const}$

其中：

• $H=2({\bar{T}}^{\top }\bar{Q}\bar{T}+\bar{R})$：正定矩阵；
• $F=2{\bar{S}}^{\top }\bar{Q}\bar{T}$

---

Step 2：求解最优控制量

因为是无约束问题，所以一阶导数为零即可得最优解：

$\frac{\partial J}{\partial U}=HU+F^{\top }{x}_0=0\Rightarrow U^{\ast }=-H^{-1}{F}^{\top }{x}_0$

此时的最优控制序列 $U^{\ast }\in {\mathbb{R}}^{Nm}$，我们只需取其中第一段：

$u_0^{\ast }=K{x}_0,\text{其中}K=-H^{-1}{F}^{\top }\text{的首行块}$

这说明：无约束线性系统的 MPC 控制器可以退化为线性反馈形式 $u=Kx$，结构与 LQR 本质相同。

3.3 方法二：基于动态规划（Dynamic Programming, DP）推导

针对无约束线性系统的最优控制问题，除了使用解析解，还可以使用 动态规划法（DP）。该方法不仅能避免直接求解矩阵逆，还能揭示控制问题的结构性本质。

---

Step 0：理论基础 —— Bellman 最优性原理

最优性原理（Principle of Optimality）
“无论起点如何，余下的控制策略一定是当前状态下的最优解。”

通俗理解：

• 只要我们从某一时刻处于某个状态开始，后续每一步都选择最优控制策略；
• 那么整体形成的策略就是最优的。

---

Step 1：将代价函数拆解为递归结构

MPC 优化问题如下：

$J={\sum }_{k=0}^{N-1}\left(x_k^{\top }Q{x}_k+u_k^{\top }R{u}_k\right)+x_N^{\top }P{x}_N$

我们将其拆为 $N$ 个子阶段，设第 kkk 步的阶段成本函数为：

$J_k(x_k)={\min }_{u_k}\left[x_k^{\top }Q{x}_k+u_k^{\top }R{u}_k+J_{k+1}(x_{k+1})\right]\text{with}{x}_{k+1}=A{x}_k+B{u}_k$

---

Step 2：反向迭代推导值函数

边界条件（终端）：

$J_N(x_N)=x_N^{\top }P{x}_N$

倒数第一步 $k=N-1$：

$J_{N-1}(x_{N-1})={\min }_{u_{N-1}}\left[x_{N-1}^{\top }Q{x}_{N-1}+u_{N-1}^{\top }R{u}_{N-1}+(A{x}_{N-1}+B{u}_{N-1}{)}^{\top }P(A{x}_{N-1}+B{u}_{N-1})\right]$

整理后仍为二次型：

$J_{N-1}(x_{N-1})=x_{N-1}^{\top }{P}_{N-1}{x}_{N-1}$

通过展开并配平方项可以求出$P_{N-1}$ 与反馈矩阵 $K_{N-1}$。

继续递归下去，可得每一阶段的：

• 值函数矩阵 $P_k$；
• 最优控制律 $u_k^{\ast }=-K_k{x}_k$

---

Step 3：归纳结构性结果

对于所有 $k=0\sim N-1$，最优控制策略均为：

$u_k^{\ast }=-K_k{x}_k\text{with}{K}_k=(R+B^{\top }{P}_{k+1}B{)}^{-1}{B}^{\top }{P}_{k+1}A$

终端条件：$P_N=P$，向前递推：

$P_k=Q+A^{\top }{P}_{k+1}A-A^{\top }{P}_{k+1}B(R+B^{\top }{P}_{k+1}B{)}^{-1}{B}^{\top }{P}_{k+1}A$

当 $N\to \infty$，递推收敛：即为 LQR 中的黎卡提方程解。

---

3.4 MPC 与 LQR 的关系对比

对比维度	MPC（有限时域）	LQR（无限时域）
优化范围	$k\sim k+N$	$k\sim \infty$
权重矩阵 PPP	每轮重算（动态）	固定稳定解
控制律 KKK	实时更新	固定反馈矩阵
是否在线	✅ 每轮优化	❌ 离线设计
是否支持约束	✅ 支持	❌ 不支持
数学结构	二次规划	黎卡提方程

📌 总结：

LQR 是线性无约束 MPC 的无限时域极限形式。
其控制律结构一致：$u_k=-K{x}_k$​，但 LQR 不具备对约束与非线性建模的能力。

四、带约束系统下的 MPC 控制设计

4.1 控制目标与求解方式概述

在现实控制系统中，MPC（Model Predictive Control）最突出的能力之一就是可以显式处理约束条件。这使得 MPC 在工程实践中，如自动驾驶、工业过程控制等领域中展现出比传统 PID 和 LQR 更强的适应能力。

本节将从问题建模、求解形式与优化器集成三方面，介绍带约束系统下 MPC 的基本求解逻辑。

---

4.1.1 有约束系统的 MPC 控制问题形式

带约束系统的 MPC 通常可统一表示为一个带有约束的有限时域最优化问题，其目标是在给定当前状态 $x_k$ 和参考轨迹 $r_k$ 的前提下，预测未来 $N$ 步的控制输入 $u_k,...,u_{k+N-1}$，使得系统的输出尽可能逼近目标轨迹，并满足所有物理与安全约束。

控制目标函数（带终端项）：

$J={\sum }_{i=0}^{N-1}\left(e_{k+i}^{T}Q{e}_{k+i}+u_{k+i}^{T}R{u}_{k+i}\right)+e_{k+N}^{T}F{e}_{k+N}$

其中，

• $e_{k+i}=r_{k+i}-y_{k+i}$：跟踪误差；
• $Q,R,F$：权重矩阵，分别衡量误差、控制量、终端性能的影响。

约束条件：

带约束系统通常包括以下限制项：

• 控制输入约束（如最大加速度、转角限制）：

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

• 系统状态约束（如车道边界、障碍物区域）：

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

• 终端状态约束（可选）：

  $x_{k+N}\in {\mathcal{X}}_f$

---

4.1.2 二次规划（QP）求解器简介

由于系统模型与代价函数都保持二次型，且约束为线性不等式，因此上述问题可归结为一个标准的带约束二次优化问题（Quadratic Programming, QP）：

$\begin{array}{rl}& \underset{z}{\min }\frac{1}{2}{z}^{T}Hz+z^{T}F{x}_0+\frac{1}{2}{x}_0^{T}E{x}_0\\ & \text{s.t.}Gz\le W+S{x}_0\end{array}$

其中，向量 $z=[u_k^T,...,u_{k+N-1}^T{]}^T$ 为待优化的控制输入序列。

常用 QP 求解器包括：

• OSQP：稀疏优化专用，速度快、适用于嵌入式系统
• qpOASES：适合小型问题，适配自动驾驶控制
• ECOS / CVXOPT：适用于带锥形约束的 SOCP 问题
• GUROBI / MOSEK：通用商用求解器，精度高但需许可证

---

4.1.3 MPC 与优化器的集成逻辑

MPC 控制器在每个时刻的控制流程可概括如下：

1. 获取当前状态 $x_k$；
2. 构造带约束的优化问题（目标函数 + 状态/输入约束）；
3. 调用 QP 求解器，计算最优控制输入序列 KaTeX parse error: Undefined control sequence: \* at position 3: z^\̲*̲；
4. 仅执行第一个控制输入 KaTeX parse error: Undefined control sequence: \* at position 9: u_k = z^\̲*̲_0，丢弃其余，等待下一步更新。

这种滚动优化 + 实时重规划机制使得 MPC 可以动态应对约束变化，并适配非平稳系统。

4.2 有约束线性系统的 MPC

带约束线性系统的 MPC 是目前最常见、应用最广泛的一类模型预测控制器，其核心在于：

• 系统模型是线性的（状态转移与输出方程）；
• 约束为线性等式或不等式；
• 目标函数为二次型代价函数。

这类问题具有良好的凸性结构，因此可以高效地使用标准二次规划（Quadratic Programming, QP）方法进行实时求解。

---

4.2.1 系统模型与控制问题回顾

我们考虑如下的离散时间线性系统（状态空间形式）：

$\begin{array}{rl}{x}_{k+1}& =A{x}_k+B{u}_k\\ y_k& =C{x}_k+D{u}_k\end{array}$

系统状态向量 $x_k\in {\mathbb{R}}^n$、控制输入 $u_k\in {\mathbb{R}}^m$、输出 $y_k\in {\mathbb{R}}^p$。

---

4.2.2 线性约束形式建模

MPC 在控制序列的设计中需满足一系列线性约束，包括但不限于：

• 控制输入约束（硬件物理限制）：

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

• 状态约束（安全性、物理边界）：

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

• 终端约束（收敛性或稳定性）（可选）：

  $x_{k+N}\in {\mathcal{X}}_f$

所有上述约束都可以统一写成矩阵形式：

$Gz\le W+S{x}_k$

其中 $z={\left[\begin{array}{ccc}{u}_k^T& \cdots & u_{k+N-1}^T\end{array}\right]}^T$，为控制优化向量。

---

4.2.3 QP 问题构建与求解过程

为了进行最优控制，需要构建如下标准 QP 问题：

目标函数（带预测模型）：

$J(z)=\frac{1}{2}{z}^{T}Hz+z^{T}F{x}_k+\frac{1}{2}{x}_k^{T}E{x}_k$

• $H$：与 $Q,R,F$ 有关的正定矩阵；
• $F$：状态相关权重项；
• $E$：终端状态权重项（不影响求解，但影响代价评估）；

约束形式：

$Gz\le W+S{x}_k$

这是一个标准的凸二次规划问题，可调用高效的 QP 求解器（如 OSQP、qpOASES、GUROBI）获得全局最优解。

---

4.2.4 控制流程图（逻辑结构）

如下图所示，带约束的线性 MPC 控制器运行流程如下：

🧭 逻辑流程结构图（可视化）

这种结构保证了：

• 实时计算；
• 明确可调约束；
• 高鲁棒性与可解释性。

4.3 有约束非线性系统的 MPC

在实际工程场景中，系统通常既存在非线性动力学，又需满足输入和状态的物理约束，例如：

• 无人车的转角、加速度受限；
• 飞行器的角速率、推力不能超过极限；
• 机械臂关节的活动范围有限，且动力学模型为非线性。

这种背景下，必须采用有约束非线性系统的 MPC 控制策略。

---

4.3.1 非线性系统建模形式

控制对象的非线性状态空间模型可形式化为：

$\begin{array}{rl}{x}_{k+1}& =f(x_k,u_k)\\ y_k& =g(x_k,u_k)\end{array}$

• $x_k\in {\mathbb{R}}^n$：系统状态变量；
• $u_k\in {\mathbb{R}}^m$：控制输入；
• $f,g$：非线性函数（如自行车模型中的三角函数、航向角积分等）。

常见非线性系统示例包括：车体运动学模型、无人机六自由度方程、机械臂动力学方程等。

---

4.3.2 控制约束建模方式

为确保系统运行安全可靠，需对以下内容设置约束：

• 输入约束（控制量约束）：

  uk∈U={u∣umin⁡≤u≤umax⁡}u_k \in \mathcal{U} = \{ u \,|\, u_{\min} \leq u \leq u_{\max} \}uk​∈U={u∣umin​≤u≤umax​}

• 状态约束（物理/安全约束）：

  xk∈X={x∣xmin⁡≤x≤xmax⁡}x_k \in \mathcal{X} = \{ x \,|\, x_{\min} \leq x \leq x_{\max} \}xk​∈X={x∣xmin​≤x≤xmax​}

• 非线性约束项（如避障、稳定性边界）：

  $h(x_k,u_k)\le 0,\forall k$

这些约束可组合为统一的非线性约束表达形式，在优化中严格满足。

---

4.3.3 优化问题构建与求解方法

在每一控制周期，MPC 构建一个有限时域的非线性优化问题（NLP），其目标函数为：

min⁡{uk}k=0N−1J=∑i=0N−1(ek+i⊤Qek+i+uk+i⊤Ruk+i)+ek+N⊤Fek+Nukk\min_{\{u_k\}_{k=0}^{N-1}} \; J = \sum_{i=0}^{N-1} \left( e_{k+i}^\top Q e_{k+i} + u_{k+i}^\top R u_{k+i} \right) + e_{k+N}^\top F e_{k+N}{uk}kmin{uk​}k=0N−1​​J=∑i=0N−1​(ek+i⊤​Qek+i​+uk+i⊤​Ruk+i​)+ek+N⊤​Fek+N​ukk

约束条件为：

$\{\begin{array}{l}{x}_{k+i+1}=f(x_{k+i},u_{k+i})\\ x_{k+i}\in \mathcal{X}\\ u_{k+i}\in \mathcal{U}\\ h(x_{k+i},u_{k+i})\le 0\end{array}\text{for }i=0,...,N-1$

该优化问题可通过以下求解器实现：

• IPOPT（Interior Point Optimizer）—支持大规模非线性优化；
• CasADi + IPOPT — 适合构建和自动求导；
• ACADO / FORCES Pro — 实时嵌入式 MPC；
• Optuna / NLopt / SciPy — 通用 NLP 工具箱。

4.4 使用 MPC 控制车辆的系统架构

在自动驾驶系统中，MPC（Model Predictive Control）因其强大的预测能力与约束处理能力被广泛应用于车辆路径规划与控制。然而，相较于传统控制器（如 PID、Stanley 或 LQR），MPC 控制器的系统架构存在明显差异，尤其在执行效率与实时性方面。

4.4.1 系统架构组成

使用 MPC 控制车辆运动时，系统通常由以下几个关键模块构成：

• 车辆动力学建模：建立准确的车辆状态空间模型，支持预测未来行为；
• 代价函数设计：明确优化目标，如误差最小、控制平滑、能耗低等；
• 约束条件构建：包括控制输入约束（转角、加速度）和状态约束（车道边界、障碍物）；
• 底层控制器（Low-Level Controller）：如 PID 控制器，承担实时补偿作用；
• MPC 控制器（High-Level Planner）：执行预测、优化与决策，输出理想控制动作。

4.4.2 为什么需要“底层控制器”？

传统控制器如 PID、LQR 具有计算复杂度低、响应速度快的优势，得到的控制量可直接下发车辆执行。但 MPC 控制器在每个时刻都需预测 N 步未来状态并求解一个优化问题，当系统模型为非线性时，求解效率会进一步降低，甚至不能保证在一个控制周期内完成计算。

这就带来了关键问题：

在控制时刻 $k$，MPC 控制器可能要等到时刻 $k+3$ 才能求解出 $u_k$，而车辆控制系统必须在 $k$ 时刻立即响应控制命令。

为了解决该问题，工程实践中普遍采用双层控制结构：

• MPC 作为高层控制器（High-Level），用于策略规划与决策；
• 低层控制器（如 PID）作为执行器接口，在 MPC 无法及时响应时快速接管控制，保证系统的实时性与稳定性。

4.4.3 控制结构图

下图展示了典型的 MPC 控制车辆系统结构：

结构说明：

• 状态反馈来自车辆传感器，如位置、航向、速度；
• MPC 控制器负责每轮预测与优化，输出最优控制策略；
• 若 MPC 输出延迟或失败，底层控制器可依据上一次输出状态快速调整当前控制命令；
• 控制输出最终通过执行机构（转向、油门、制动等）作用于车辆。