去年组里做无人机控制方向的师弟跑过来问我“师兄悬停控制到底该选哪个控制器”他说自己看了几篇论文有的用 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 实现可以分为三种路径复杂度递增使用 Model Predictive Control Toolbox 的 mpc 对象。用 YALMIP 或 CVX 做优化建模调用 QP 求解器。自己写 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 秒时给高度通道或俯仰通道加入一个持续时间很短的外部扰动力或力矩观察控制器抑制扰动的能力和恢复时间。扰动示例在 t2s 时给 z 轴施加 2N 的向上扰动力持续 0.5s通过两组实验可以分别考察控制器在“初始误差消除”和“外部扰动抑制”两个不同层面的表现。4.2 性能指标不要只看上升时间对比控制器性能时常用的指标包括指标含义读取方式上升时间从初始偏差到首次进入目标范围的时间响应曲线直接读取超调量超过目标值的最大百分比响应曲线直接读取调节时间状态稳定在目标范围所需时间响应曲线直接读取控制能量控制量平方的积分从控制输入序列计算最大控制量控制过程峰值从控制输入序列读取这五个指标要一起看。单纯看上升时间PID 可能并不慢但综合控制能量或约束满足情况MPC 的优势就体现出来了。4.3 三种控制器在仿真中通常会呈现出的差异基于我的经验和常见测试结果可以给一个保守的预期但你要在自己的模型上验证PID 在初始偏差消除上往往响应很快但超调和控制量峰值可能偏高。如果参数调得比较激进还会在某些通道上出现轻微振荡。LQR 在初始偏差实验中通常比 PID 更平滑多状态收敛更协调。但要注意它的控制量没有显式约束如果 Q 和 R 设置不当控制量峰值会很大。MPC 在带有输入约束和状态约束时表现最好能在大偏差下依然保持控制量在安全范围内但因为每个周期要在线求解优化仿真耗时明显高于另外两者。还有一个重要的体感差异PID 和 LQR 在仿真中速度很快几百毫秒就能跑完MPC 因为每个采样周期都要求解一次优化问题仿真时间可能要多出几倍甚至一个数量级。这个差异在仿真里不致命但放到真实飞控上算力消耗就会变成关键因素。4.4 常见错误与排查链路如果你在复现时发现结果不对别急着怀疑控制器本身按下面的排查顺序走先看状态空间模型是否稳定可控。计算一下 det(ctrb(A,B)) 或直接用 ctrb 函数检查可控性。再看单位是否统一。角度用弧度还是度力用 N 还是 kgf这些不一致会直接导致控制器输出异常。然后看权重矩阵维度和模型维度是否对齐。Q 的维度应该等于状态数量R 的维度应该等于输入数量。接着看 MPC 的采样时间 Ts 是否和模型一致。Ts 太大预测质量差Ts 太小计算量上升。最后看约束设置。如果约束太紧MPC 可能无解或响应过慢如果约束太松MPC 就退化成没有约束的 LQR。这个五步排查法是我后续做控制方案对比时基本都会先做一遍的流程。它能避免大多数因为建模、维度、单位、权重和约束引入的隐性错误。5. 控制方案的选型不是“哪个最好”而是“哪个匹配你的约束”很多人看完整套对比后会问一个很自然的问题那到底用哪个这个问题的答案取决于你的约束条件而不是哪个控制器理论性能更好。5.1 选型框架从算力、模型精度、约束需求三个维度判断我建议用一个简单的三问法来做控制方案选型你的控制器跑在什么硬件上算力是否紧张你对被控对象的模型有多了解模型误差有多大你的任务里是否真的存在必须满足的约束如果把这三问放在一起大多数情况都能得到清晰判断场景推荐方案理由低成本飞控、底层姿态环、算力有限PID计算量小调参直观工程成熟模型已知、多变量耦合、需要平滑收敛LQR理论完备多状态协调实现简单强约束、大偏差、硬件算力充足MPC能显式处理约束预测能力强但代价是算力和模型依赖学习与科研对比三种都做相互印证理解各自边界这个表格不是最终答案但它能帮你快速排除不合适的选项。比如你手上只有一块非常便宜、主频很低的小型飞控那么即使 MPC 在仿真里表现最好也不适合现在就上你要么换硬件要么先用 PID 或 LQR 做底层把 MPC 留给上层规划。5.2 长期使用的工程提醒控制器之外的东西更重要在实际项目里控制器的选择只是整个系统的一部分。即使你最终选了 LQR 或 MPC下面这些问题一样会影响最终效果状态估计。控制器需要完整状态反馈但真实无人机上很多状态不能直接测量需要 EKF 或互补滤波器估计。估计误差会直接削弱 LQR 和 MPC 的模型优势。执行器饱和。LQR 如果遇到电机饱和性能会明显下降MPC 可以建模约束但模型不准确时约束也可能设错。采样时间与延迟。真实控制系统的传感器采样、通信和计算延迟都会让控制器性能偏离仿真结果。参数鲁棒性。线性化模型只是在悬停点附近有效如果无人机负载变化或重心偏移三种控制器的表现都会变化需要在多个工作点做验证。所以我的建议是先用仿真把三种控制器的差异刻在脑子里然后回到你的真实系统从最底层、最容易验证的 PID 开始逐步升级到 LQR 和 MPC。每一步升级都走“建模-验证-仿真-实验”的闭环不要因为 MPC 听起来更高级就直接跳到 MPC。6. 把这次对比沉淀成一套可复用流程最后我想把这篇文章的实践过程浓缩成一个可复用的方法。未来你面对的不一定是四旋翼悬停可能是机械臂、移动机器人、倒立摆甚至某个过程控制系统但对比控制方案的思路是一样的。6.1 四步控制方案对比流程建立基准模型。先找一个能代表系统核心特性的模型并明确线性化工作点。搭建统一仿真环境。确定控制器接口、状态量、输入量、参考轨迹、扰动形式和性能指标。逐类实现控制器。从简单到复杂先 PID、再 LQR、最后 MPC每一步都验证闭环稳定性。统一实验并对比。用相同的初始偏差、相同的扰动、相同的指标来比较记录差异并分析原因。这套流程的价值在于它把“选哪个控制器”这个问题从“谁更强”转变成“在什么条件下谁更匹配”。这是工程判断和论文阅读之间最大的差别。6.2 学习路线建议如果你是想通过这个项目入门四旋翼控制我建议的学习顺序是第一步把非线性模型和线性化过程完全弄懂不要只会用系统辨识或工具箱。第二步用 PID 把悬停控制跑通知道串级结构和积分限幅的含义。第三步用 LQR 重新设计同一个模型感受状态反馈和权重矩阵的影响。第四步用 MPC 做带约束的控制理解预测时域、控制时域和约束设置。第五步回到实物或高保真仿真对比理论差异和工程差异积累自己的判断经验。如果你能走完这五步你得到的不是一个“最优控制方案”而是一套能判断任何控制方案是否适合自己的方法论。这个能力比记住某个控制器能跑通更重要。回到开头师弟的问题。我当时给他的原话是PID 是保底方案LQR 是入门最优控制的好起点MPC 是应对约束和不确定性的更现代工具。但真正决定你项目成败的不是控制器名字有多高级而是你对模型、状态估计和执行器了解得有多深。先把这个认知立住再去动 Matlab你的收获会大得多。