控制算法：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）。 |
| 主要应用场景 | 电机转速、恒温控制、简单平衡。 | 航天器姿态控制、精密工业伺服。 | 自动驾驶避障、无人机轨迹规划、化工过程。 |
普适的MPC算法推导流程：
如果针对某一个问题不好简历数学模型，我们可以不构造数学模型，可以通过一些手段直接跳过数学建模获得预测模型：
总结：
建立连续动力学模型
根据连续动力学模型构建连续状态空间方程
将状态空间方程离散化
通过离散状态空间方程预测未来N步的状态，并整理为矩阵形式预测方程
构造代价函数，将预测方程代入得到一个QP问题，再通过QP求解器求解
以一维小车位置控制为例使用mpc控制的流程：
MPC算法调参：
调节 MPC（模型预测控制）参数的过程，就像是在“计算性能”、“控制精度”和“运行稳定性”之间找平衡。根据不同的场景，有不同的性能需求，自然需要调节不同的参数
1. 时域参数 (Horizon Parameters)
Np：预测步长。 Nc：控制步长