控制算法：MPC（模型预测控制）算法

什么是控制算法？

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

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

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

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

MPC控制算法：

MPC是一类控制算法。

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

MPC控制算法的几大特性：

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

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

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

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

特性	PID (比例-积分-微分)	LQR (线性二次调节器)	MPC (模型预测控制)
核心思想	消除误差：根据过去和现在的误差来调整输出。	能量最优：在无限时间内寻找系统状态与控制能量的最佳平衡。	预测未来：在有限时间内通过滚动优化寻找满足约束的最优解。
对模型的依赖	无需模型	强依赖：需要精确的线性状态空间模型	强依赖：需要系统模型，但要求不高
处理约束的能力	几乎没有	中：设计时考虑了能量权衡，但不能直接加入硬约束。	极强
多变量系统 (MIMO)	困难：多个回路之间存在严重的耦合干扰。	擅长：天然支持多输入多输出系统，通过矩阵统一计算。	最擅长：能同时处理多个目标和复杂的变量耦合。
计算复杂度	极低：简单的代数运算，单片机轻松跑。	低：设计阶段解代数黎卡提方程，运行时仅为矩阵乘法。	极高：每个采样周期都要解一个带约束的最优化问题（QP）。
主要应用场景	电机转速、恒温控制、简单平衡。	航天器姿态控制、精密工业伺服。	自动驾驶避障、无人机轨迹规划、化工过程。