五分钟入门控制算法：MPC（模型预测控制）算法

什么是控制算法？

        比如我现在的无人机悬浮在空中的某个位置，我想要让他以最短时间抬升悬浮到上方10m的位置，那我要具体如何去调整输入（如电流、油门、功率），以最好的性能（时间最短）来达到预期的目标呢？那就需要控制算法来求解，来调整这些输入。

        控制算法（Control Algorithm）本质上是一套控制机械系统运作的“数学指挥指令”。它告诉机器（如无人机、恒温空调、机械臂）如何根据目前的状态，通过调整输入（如电流、油门、功率）来达到预期的目标。

        不同的算法有不同的使用场景与特性，有些适用于动态系统，有些适用于静态。有些适用于低阶系统，有些适用于高阶系统。有些计算量小，有些计算量大。所以衍生出了很多种控制算法。

        如何根据不同的场景选择合适的控制算法，创造更厉害的控制算法，调整控制算法的参数使得任务完成的效果更好；如何让实时波形图（如 rqt_plot）更加贴合跟踪曲线；如何对机械系统编写“保护逻辑” ；如何处理传感器噪声与延迟，用一些滤波算法（卡尔曼滤波）做更好的状态估计。如何增加前馈（Feedforward）**或优化阻尼比来让动作丝滑 就是控制算法工程师的工作。

MPC控制算法：

MPC是一类控制算法。

如果 PID 是根据“现在的误差”在做反应，那么 MPC 就是在**“预测未来”并做决策。你可以把它想象成一个会推算未来几秒钟走势的驾驶员**。

MPC控制算法的几大特性：

（1）预测模型：模型是 MPC 的灵魂，它是对物理世界规律的数学表达。

（2）预测：根据预测模型推算出未来N个步长里系统的状态

（3）滚动优化：虽然我算出了未来 10 步的完美计划，但我只走第 1 步，然后扔掉剩下的，重新计算。走一步，看十步。每一个采样周期都在解一个最优问题。虽然计算量大，但它能保证在任何时刻，输出的操作都是基于当前最新状态的最优选择。

PID vs. LQR vs. MPC 核心对比表

普适的MPC算法推导流程：

如果针对某一个问题不好简历数学模型，我们可以不构造数学模型，可以通过一些手段直接跳过数学建模获得预测模型：

总结：

根据原始问题满足的数学物理原理，状态变量，输入，去建立连续动力学模型

根据连续动力学模型构建连续状态空间方程

将状态空间方程离散化

通过离散状态空间方程预测未来N步的状态，并整理为矩阵形式预测方程

构造代价函数，将预测方程代入得到一个QP问题，再通过QP求解器求解

所以本质上是在求解一个受约束的优化问题（如果实在处理线性系统那就是是 QP问题）。输入是表征位置，速度的预测方程跟期望的轨迹。输出是N的步长的控制量。

控制序列 u 是有物理单位的实际物理量（力、扭矩、电流、流速，加速度等等）。

假如u=2.5，那就让pwm占空比为50%，就可以控制机器人踩下一半的油门了。

以一维小车位置控制（多大力从A走到B）为例使用MPC控制：

MPC算法调参：

调节 MPC（模型预测控制）参数的过程，就像是在“计算性能”、“控制精度”和“运行稳定性”之间找平衡。根据不同的场景，有不同的性能需求，自然需要调节不同的参数

1. 时域参数 (Horizon Parameters)

Np：预测步长。     Nc：控制步长

什么是状态空间方程：

状态空间方程是描述系统动态特性的核心工具。传递函数也能表征系统动态特性，但传递函数是“黑盒”，只关注输入输出；状态空间是“白盒”，可以实时观测机器人的每一个关节状态。

1. 核心直觉：什么是“状态”？

想象你在开一辆车（或者控制你的仓库机器人）：

• 如果你只知道机器人现在的位置，你没法预判它下一秒在哪，因为你不知道它的速度。
• 如果你知道位置 + 速度，你就掌握了它的完整“状态”。

2. 数学定义

一个线性定常系统的状态空间模型由两个方程组成：状态方程和输出方程。状态空间方程的核心思想： 只要知道了现在的状态和接下来的动作，就能算出未来的状态。