四旋翼悬停控制：PID、LQR与MPC的Matlab仿真对比

去年组里做无人机控制方向的师弟跑过来问我：“师兄，悬停控制到底该选哪个控制器？”他说自己看了几篇论文，有的用 PID，有的用 LQR，最近又开始看到 MPC，感觉都能做悬停，但一到实际调参就不知道差别在哪。这个问题其实不是他一个人会问，很多刚接触四旋翼控制的人都会卡在同一个地方：控制器名字都认识，代码也会抄，就是不清楚它们之间真正的差异是什么，以及什么时候该用哪一个。

我当时给他的建议是：与其看各种博客上的理论对比，不如直接拿一个统一的对象，把 PID、LQR、MPC 三种控制器都在同一个四旋翼悬停线性化模型上跑一遍，用 Matlab 做仿真对比，看它们的响应曲线、控制量变化、抗扰能力和实现复杂度。这样得来的认知，比读十篇综述都管用。

这篇博客就把这条思路完整展开。我会从数学模型开始，再到三种控制器的设计思路、Matlab 实现结构和最终对比结论，最后给出一个小型可复用框架，帮助你在自己的项目里做控制方案选型。文章里的代码结构、参数设计和比较方法都是按工程实践的路子走的，你拿到之后可以先复现，再按自己的对象去调整。

1. 先搞清楚四旋翼悬停问题为什么会成为控制算法对比的“标准赛道”

悬停看起来是最简单的飞行模式，无人机停在半空中不动就行了，但控制上它反而很适合用来对比算法。原因有三个：状态空间模型明确、控制目标清晰、性能指标容易量化。

1.1 线性化模型：把非线性飞行器变成可设计控制器的“线性对象”

四旋翼本身是一个强耦合、非线性的系统，直接拿非线性模型去设计控制器会很复杂。工程上和学术里常用的做法是，先建立完整的非线性动力学模型，然后在悬停工作点附近做小扰动线性化，得到一个线性时不变状态空间方程。这个线性模型的形式是：

dx/dt = A x + B u y = C x + D u

其中状态向量通常选取位置、速度、姿态角和姿态角速度，输入向量是期望姿态角或电机转速增量。对于悬停工况，可以假设飞行器在平衡点附近，俯仰和滚转角度很小，从而把 sin 和 cos 近似成角度本身，把非线性项退化成线性项。

这一步是整篇文章的基石，因为 PID、LQR、MPC 三种控制器，如果在理论推导和代码实现上要对比，就必须建立在同一个线性模型上。否则你很难判断性能差异是来自控制器本身，还是来自被控对象不一样。

1.2 为什么悬停线性化模型能成为对比基准

从实操角度看，悬停有三个非常适合做算法对比的特点：

• 工作点固定，不需要考虑轨迹跟踪时的前瞻和路径规划。
• 状态方程阶次适中，既能体现多变量耦合，又不至于让代码复杂到失去可读性。
• 性能指标易于定义，比如上升时间、超调量、稳态误差、控制能量，都可以从仿真曲线里读出来。

模型本身虽然简化了，但控制器的设计思路和实现方式可以完整保留。你学会在这个线性模型上做三种控制器的对比，后续迁移到姿态控制、轨迹跟踪、抗风扰场景时，核心逻辑是一样的。

2. 三种控制器不是三个版本，而是三种控制思路

很多人第一次接触 PID、LQR、MPC 时，容易把它们理解成“三个都能实现悬停的控制器 A/B/C”，好像差别只在公式形式。但实际上，这三种方法代表了三种完全不同的控制哲学。

2.1 PID：不需要模型，靠误差反馈“摸着石头过河”

PID 是三种方法里唯一一个不依赖被控对象精确模型的控制器。它的工作逻辑非常简单：看当前输出和目标之间的误差，然后根据误差、误差积分、误差变化率来调整控制量。

在四旋翼悬停里，通常的做法是串级 PID，内环控制姿态角速度，外环控制姿态角或位置。比如你希望无人机保持高度不变，外环把“期望高度 - 当前高度”转换成期望垂直速度或期望推力增量，内环再把姿态误差转换成电机转速增量。

PID 的优点大家都很熟悉：结构简单、直观、计算量小，特别适合做底层姿态环，以及在没有精确模型的环境里快速调通。缺点也明确：多变量耦合比较强的时候，PID 参数往往要靠手动试凑，而且很难同时兼顾多个状态的性能。

在悬停线性化模型上，PID 通常可以按通道独立设计，比如俯仰通道、滚转通道、偏航通道、高度通道分别调参。这样做的好处是工程上容易实施，坏处是忽略了通道间的耦合，在大机动或者强扰动场景下，性能可能会明显退化。

2.2 LQR：基于模型的最优控制，用代价函数把“调参”变成“加权”

LQR 全称是 Linear Quadratic Regulator，也就是线性二次型调节器。它和 PID 最大的区别在于，控制器的设计不是靠直觉去试参数，而是通过优化一个二次型代价函数来得到状态反馈增益矩阵 K。

代价函数通常是这样的形式：

J = ∫ ( x^T Q x + u^T R u ) dt

其中 Q 矩阵表示你对状态偏差的重视程度，R 矩阵表示你对控制量变化的惩罚程度。解这个优化问题会得到一个增益矩阵 K，控制律就是：

u = -K x

在四旋翼悬停场景下，Q 和 R 的选择直接决定了闭环系统的性能。如果你希望无人机姿态恢复快，就把对应的姿态状态权重调大；如果担心电机转速饱和或能耗过高，就把 R 调大。相比 PID 的多个增益参数，LQR 的参数数量看起来更少，但 Q 和 R 往往是矩阵，维度高起来之后，调参的心智负担并不小。

LQR 的强项是对多变量系统有良好的理论支撑，能一次性处理多个状态之间的权衡。缺点是它本质上是一个状态反馈调节器，没有显式地考虑输入约束和状态约束。也就是说，LQR 计算出来的控制量可能很大，而真实电机不可能无限加速，这时容易产生饱和。

2.3 MPC：带约束的“预测-优化-执行”控制器

MPC 全称是 Model Predictive Control，模型预测控制。它和前两种方法差异更大。MPC 不是一次性算出一个固定的反馈增益，而是在每一个控制周期，基于当前状态和系统模型，往前预测未来 N 步的行为，然后在线求解一个有限时域优化问题，只执行第一步，等下一个周期再重新计算。

这套“预测-优化-执行”的滚动机制，让 MPC 能非常自然地处理约束。比如你可以直接告诉控制器“电机转速增量不能超过某个值”“姿态角不能超过 30 度”，MPC 在求解优化问题时会把这些约束作为硬性或软性条件加进去。

在四旋翼悬停控制里，MPC 的典型用法有两种：一种是用它做位置环或速度环的轨迹规划，把输出的期望加速度或期望姿态角传给底层 PID；另一种是直接做全状态 MPC，把姿态和位置都纳入优化问题。后者对模型精度和算力要求更高，但在仿真中更容易展示 MPC 的潜力。

需要特别注意：在悬停线性化模型上，MPC 的“预测”能力并不会体现出太剧烈的优势，因为工作点固定，未来行为基本可以预测得很准。但 MPC 的价值在于它能在每个采样时刻重新优化，因此处理突加扰动和约束违例的能力更强。

3. Matlab 代码实现：从模型搭建到三种控制器对比

这一部分是整篇博客的实操重点。我会按一个完整的对比流程来写，先从模型参数和环境准备开始，然后分别给出 PID、LQR、MPC 在 Matlab 里的实现结构，最后展示怎么对比和读取结果。

3.1 环境准备与模型参数

Matlab 里做这个对比，建议使用较新的版本，至少包含 Control System Toolbox，最好有 Model Predictive Control Toolbox，因为带约束的 MPC 如果用 YALMIP 或自己写 QP 求解器，复杂度会高很多，不适合作为入门对比。

模型参数建议取一套常见的四旋翼参数，比如：

% 四旋翼悬停线性化模型参数示例 m = 1.2; % 质量 kg g = 9.81; % 重力加速度 m/s^2 Ixx = 0.015; % 滚转惯量 kg.m^2 Iyy = 0.015; % 俯仰惯量 kg.m^2 Izz = 0.025; % 偏航惯量 kg.m^2

这些参数需要和你的真实无人机做匹配，不同机型差异很大。如果你只是学习对比方法，用一组合理的默认参数即可，但不要直接拿别人的结果作为自己硬件上的性能结论。

线性化状态空间模型在 Matlab 里可以用 ss 对象表达。常见的状态选取是：

% 状态: [x, y, z, phi, theta, psi, vx, vy, vz, p, q, r] % 输入: [u1, u2, u3, u4] 对应油门和三个力矩增量

为了直观，也可以把模型拆成俯仰-纵向通道和滚转-横向通道来分别设计，先验证每个通道的 PID 和 LQR，再组合成完整状态空间模型做 MPC。这种“先单通道验证，再全状态融合”的做法在工程里非常常见，能有效减少调试难度。

3.2 PID 控制器的代码设计：串级结构是工程标配

在悬停控制里，PID 我更建议采用串级结构，而不是单环结构。单环 PID 面对姿态和位置耦合时会比较吃力，而串级 PID 可以明显提高响应速度。

一个经典的串级 PID 伪代码如下：

% 外环：位置/高度控制，输出期望姿态角或期望速度 error_z = z_ref - z; vz_ref = Kp_z * error_z + Ki_z * integral_error_z + Kd_z * dz; % 内环：姿态控制，输出力矩指令 error_phi = phi_ref - phi; p_ref = Kp_phi * error_phi + Ki_phi * integral_error_phi + Kd_phi * dphi; moment_x = Kp_p * (p_ref - p);

实际代码里还要加积分限幅、输出限幅和微分滤波，否则积分饱和会带来很大的超调。下面是一个更完整的 Matlab 函数结构示例：

function u = pid_controller(x, ref, params) % x: 当前状态向量 % ref: 参考悬停状态 % params: PID 参数结构体 % 位置环 position_error = ref(1:3) - x(1:3); velocity_ref = params.Kp_pos .* position_error; % 姿态环 attitude_error = pos2attitude(position_error, x); moment = params.Kp_att .* attitude_error + ... params.Kd_att .* (-x(10:12)); % 总推力 thrust = params.Kp_z * position_error(3) + params.Kd_z * (-x(9)) + m * g; u = [thrust; moment]; end

这段代码只是用来展示结构，具体参数需要你根据模型来调。调 PID 时有一个经验顺序：先只调比例，让系统稳定；再加微分，减小超调；最后加积分，消除稳态误差。不要一开始就三个参数一起调。

3.3 LQR 控制器的代码设计：核心在 K 矩阵和加权矩阵

LQR 的代码实现比 PID 简单，因为它最后的输出就是一个增益矩阵 K，控制律是 u = -K x。难的是如何选择合适的 Q 和 R。

在 Matlab 里，求解 LQR 增益非常直接：

% 状态空间模型 A = ...; % 状态矩阵 B = ...; % 输入矩阵 % 权重矩阵 Q = diag([1 1 1 10 10 10 1 1 1 1 1 1]); R = diag([0.1 0.1 0.1 0.1]); % 求解 LQR 增益 K = lqr(A, B, Q, R);

这里的 Q 和 R 设置很关键。Q 中姿态角的权重通常要高于位置，因为位置变化是通过姿态变化来间接控制的；R 不能设得太小，否则控制量会过大，容易饱和。

LQR 控制器在 Matlab 仿真的闭环响应可以直接用 lsim 来跑：

sys_cl = ss(A - B*K, B, eye(size(A)), 0); t = 0:0.01:5; x0 = [0.1; 0; 0; 0.05; 0; 0; 0; 0; 0; 0; 0; 0]; % 初始姿态偏差 lsim(sys_cl, zeros(size(t')), t, x0);

通过这个仿真可以直观看到，LQR 对初始偏差的收敛是多状态同时进行的，不像 PID 那样可以清楚拆成“位置环”和“姿态环”。这也是为什么 LQR 在多变量耦合系统里理论性能更好，但调试时不够直观。

3.4 MPC 控制器的代码设计：用 Matlab 自带工具箱还是自己写

带约束的 MPC 实现可以分为三种路径，复杂度递增：

1. 使用 Model Predictive Control Toolbox 的 mpc 对象。
2. 用 YALMIP 或 CVX 做优化建模，调用 QP 求解器。
3. 自己写 MPC 的预测和 QP 求解循环。

对于入门理解，我建议先走路径 1，因为代码短、不易出错，能让你把注意力放在 MPC 的行为而不是求解器细节上。

用工具箱实现 MPC 的常见写法：

% 建立 MPC 对象 mpcobj = mpc(sys, Ts, PredictionHorizon, ControlHorizon); % 设置约束 mpcobj.MV(1).Min = -0.5; mpcobj.MV(1).Max = 0.5; mpcobj.MV(2).Min = -0.5; mpcobj.MV(2).Max = 0.5; % 设置权重 mpcobj.Weights.OutputVariables = [1 1 1 10 10 10 1 1 1 1 1 1]; mpcobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1]; % 仿真 sim(mpcobj, 5, ref, disturbance);

这里有几个参数需要重点解释：

• PredictionHorizon：预测时域，表示 MPC 会往前看多少步。时域越长，MPC 对未来考虑越充分，但计算量也越大。
• ControlHorizon：控制时域，表示未来多少步的控制量会被当作优化变量。一般比预测时域短很多。
• MV.Min 和 MV.Max：操纵变量约束，对应真实电机或控制量的饱和边界。

如果你希望更深入理解 MPC 的内部机制，我推荐第二种路径。用 YALMIP 写一个 QP 格式的 MPC，代码会多几十行，但你能清楚看到预测方程、代价函数和约束是怎么拼起来的。这个过程能帮你理解一个事实：MPC 本质上是把控制问题转化成了一个在线优化问题。

4. 对比实验：同一模型、同一扰动、同一评价指标

三种控制器都搭好之后，关键就是怎么设计对比实验。这里最忌“每种控制器用每个不同的场景、不同的指标”，那样比出来的结果没有说服力。

4.1 对比实验设计：固定初始偏差与突加扰动

我建议设计两组实验：

第一组是初始状态偏差实验。给一个设定好的初始姿态角偏差或高度偏差，观察三种控制器把状态拉回悬停点的速度和过程。

初始偏差示例：z 方向偏差 0.3m，俯仰角偏差 5 度

第二组是突加扰动实验。在仿真运行到 2 秒时，给高度通道或俯仰通道加入一个持续时间很短的外部扰动力或力矩，观察控制器抑制扰动的能力和恢复时间。

扰动示例：在 t=2s 时，给 z 轴施加 2N 的向上扰动力，持续 0.5s

通过两组实验，可以分别考察控制器在“初始误差消除”和“外部扰动抑制”两个不同层面的表现。

4.2 性能指标：不要只看上升时间

对比控制器性能时，常用的指标包括：

这五个指标要一起看。单纯看上升时间，PID 可能并不慢；但综合控制能量或约束满足情况，MPC 的优势就体现出来了。

4.3 三种控制器在仿真中通常会呈现出的差异

基于我的经验和常见测试结果，可以给一个保守的预期，但你要在自己的模型上验证：

• PID 在初始偏差消除上往往响应很快，但超调和控制量峰值可能偏高。如果参数调得比较激进，还会在某些通道上出现轻微振荡。
• LQR 在初始偏差实验中通常比 PID 更平滑，多状态收敛更协调。但要注意，它的控制量没有显式约束，如果 Q 和 R 设置不当，控制量峰值会很大。
• MPC 在带有输入约束和状态约束时表现最好，能在大偏差下依然保持控制量在安全范围内，但因为每个周期要在线求解优化，仿真耗时明显高于另外两者。

还有一个重要的体感差异：PID 和 LQR 在仿真中速度很快，几百毫秒就能跑完；MPC 因为每个采样周期都要求解一次优化问题，仿真时间可能要多出几倍甚至一个数量级。这个差异在仿真里不致命，但放到真实飞控上，算力消耗就会变成关键因素。

4.4 常见错误与排查链路

如果你在复现时发现结果不对，别急着怀疑控制器本身，按下面的排查顺序走：

1. 先看状态空间模型是否稳定可控。计算一下 det(ctrb(A,B)) 或直接用 ctrb 函数检查可控性。
2. 再看单位是否统一。角度用弧度还是度，力用 N 还是 kgf，这些不一致会直接导致控制器输出异常。
3. 然后看权重矩阵维度和模型维度是否对齐。Q 的维度应该等于状态数量，R 的维度应该等于输入数量。
4. 接着看 MPC 的采样时间 Ts 是否和模型一致。Ts 太大，预测质量差；Ts 太小，计算量上升。
5. 最后看约束设置。如果约束太紧，MPC 可能无解或响应过慢；如果约束太松，MPC 就退化成没有约束的 LQR。

这个五步排查法，是我后续做控制方案对比时基本都会先做一遍的流程。它能避免大多数因为建模、维度、单位、权重和约束引入的隐性错误。

5. 控制方案的选型：不是“哪个最好”，而是“哪个匹配你的约束”

很多人看完整套对比后，会问一个很自然的问题：那到底用哪个？

这个问题的答案取决于你的约束条件，而不是哪个控制器理论性能更好。

5.1 选型框架：从算力、模型精度、约束需求三个维度判断

我建议用一个简单的三问法来做控制方案选型：

1. 你的控制器跑在什么硬件上？算力是否紧张？
2. 你对被控对象的模型有多了解？模型误差有多大？
3. 你的任务里是否真的存在必须满足的约束？

如果把这三问放在一起，大多数情况都能得到清晰判断：

这个表格不是最终答案，但它能帮你快速排除不合适的选项。比如你手上只有一块非常便宜、主频很低的小型飞控，那么即使 MPC 在仿真里表现最好，也不适合现在就上；你要么换硬件，要么先用 PID 或 LQR 做底层，把 MPC 留给上层规划。

5.2 长期使用的工程提醒：控制器之外的东西更重要

在实际项目里，控制器的选择只是整个系统的一部分。即使你最终选了 LQR 或 MPC，下面这些问题一样会影响最终效果：

• 状态估计。控制器需要完整状态反馈，但真实无人机上很多状态不能直接测量，需要 EKF 或互补滤波器估计。估计误差会直接削弱 LQR 和 MPC 的模型优势。
• 执行器饱和。LQR 如果遇到电机饱和，性能会明显下降；MPC 可以建模约束，但模型不准确时约束也可能设错。
• 采样时间与延迟。真实控制系统的传感器采样、通信和计算延迟，都会让控制器性能偏离仿真结果。
• 参数鲁棒性。线性化模型只是在悬停点附近有效，如果无人机负载变化或重心偏移，三种控制器的表现都会变化，需要在多个工作点做验证。

所以我的建议是：先用仿真把三种控制器的差异刻在脑子里，然后回到你的真实系统，从最底层、最容易验证的 PID 开始，逐步升级到 LQR 和 MPC。每一步升级都走“建模-验证-仿真-实验”的闭环，不要因为 MPC 听起来更高级就直接跳到 MPC。

6. 把这次对比沉淀成一套可复用流程

最后，我想把这篇文章的实践过程浓缩成一个可复用的方法。未来你面对的不一定是四旋翼悬停，可能是机械臂、移动机器人、倒立摆，甚至某个过程控制系统，但对比控制方案的思路是一样的。

6.1 四步控制方案对比流程

1. 建立基准模型。先找一个能代表系统核心特性的模型，并明确线性化工作点。
2. 搭建统一仿真环境。确定控制器接口、状态量、输入量、参考轨迹、扰动形式和性能指标。
3. 逐类实现控制器。从简单到复杂，先 PID、再 LQR、最后 MPC，每一步都验证闭环稳定性。
4. 统一实验并对比。用相同的初始偏差、相同的扰动、相同的指标来比较，记录差异，并分析原因。

这套流程的价值在于，它把“选哪个控制器”这个问题从“谁更强”转变成“在什么条件下，谁更匹配”。这是工程判断和论文阅读之间最大的差别。

6.2 学习路线建议

如果你是想通过这个项目入门四旋翼控制，我建议的学习顺序是：

• 第一步：把非线性模型和线性化过程完全弄懂，不要只会用系统辨识或工具箱。
• 第二步：用 PID 把悬停控制跑通，知道串级结构和积分限幅的含义。
• 第三步：用 LQR 重新设计同一个模型，感受状态反馈和权重矩阵的影响。
• 第四步：用 MPC 做带约束的控制，理解预测时域、控制时域和约束设置。
• 第五步：回到实物或高保真仿真，对比理论差异和工程差异，积累自己的判断经验。

如果你能走完这五步，你得到的不是一个“最优控制方案”，而是一套能判断任何控制方案是否适合自己的方法论。这个能力比记住某个控制器能跑通更重要。

回到开头师弟的问题。我当时给他的原话是：PID 是保底方案，LQR 是入门最优控制的好起点，MPC 是应对约束和不确定性的更现代工具。但真正决定你项目成败的，不是控制器名字有多高级，而是你对模型、状态估计和执行器了解得有多深。先把这个认知立住，再去动 Matlab，你的收获会大得多。