这次我们来看一个经常被问到的四旋翼控制仿真项目基于悬停线性化模型用 Matlab 同时实现 PID、LQR 和 MPC 三种控制器并给出性能对比。项目代码带详细注释适合做毕业设计、课程作业也适合刚入门四旋翼控制、想把理论模型和仿真代码对应起来的工程师。先说结论如果你正在纠结“PID、LQR、MPC 到底选哪个”或者“论文里需要一张三种控制器的对比曲线”这个项目可以直接作为起点。它不是一个人工搭建的复杂非线性仿真平台而是把四旋翼在悬停点附近做小扰动线性化得到标准的线性状态空间模型然后分别设计三种控制器。好处很明显模型简单、状态方程直观、控制器参数调节逻辑清晰、Matlab 代码可读性强。本文会按以下顺序展开第一核心能力速览第二四旋翼悬停线性化模型的推导思路和状态空间方程第三三种控制器的设计原理和 Matlab 实现示例第四环境准备和运行方式第五功能测试与结果对比第六常见问题排查最后给出最佳实践建议。1. 核心能力速览能力项说明项目类型四旋翼无人机悬停控制仿真Matlab 代码 详细注释模型类型悬停点附近小扰动线性化模型状态空间形式控制器类型PID、LQR、MPC 三种控制器对比主要功能三通道姿态/位置控制仿真绘制响应曲线分析控制性能运行环境Matlab需安装 Control System Toolbox、MPC Toolbox仿真语言Matlab 脚本.m 文件是否需要 Simulink不需要脚本仿真即可运行硬件要求普通电脑即可不需要 GPU不需要嵌入式硬件是否支持 API不支持属于离线仿真项目是否支持批量任务可通过脚本循环批量跑不同控制器参数但无任务队列适合场景控制课程作业、本科毕业设计、控制算法入门对比研究从材料看项目的核心价值不在于“飞起来”而在于把悬停控制这个物理问题抽象成线性系统然后用三种控制算法做横向对比。对于想理解 PID、LQR、MPC 差异的读者这种对比比孤立看理论公式更直观。需要注意如果做实际飞行器实验或硬件在环测试还需要考虑执行器饱和、姿态解算、传感器噪声、模型不确定性等因素这个线性化模型适合作为算法研究和教学验证的基础。2. 四旋翼悬停线性化模型与状态空间方程2.1 为什么要在悬停点线性化四旋翼是一个典型的欠驱动、非线性、强耦合系统。完整描述它的运动需要刚体动力学方程、运动学方程、空气动力学模型直接做非线性控制会非常复杂。但是在悬停状态附近飞行器的姿态角很小线速度和角速度都接近零此时可以将非线性方程展开成线性模型这就是小扰动线性化。悬停线性化模型的工程意义在于很多实际飞控系统的姿态内环就是在悬停点附近工作的。只要飞行器不做大机动飞行动作线性模型已经能够反映主要动态特性并且在设计控制器时可以使用成熟的线性系统理论。2.2 状态空间模型的一般形式对于悬停状态下的四旋翼常见做法是把状态变量选为位置和速度、姿态角和角速度的组合。一个典型的线性化状态空间模型可以写成x_dot A * x B * u y C * x D * u其中状态向量通常包含位置x、y、z线速度vx、vy、vz姿态角phi滚转角、theta俯仰角、psi偏航角角速度p、q、r控制输入 u 通常是四个旋翼产生的升力或转速的线性组合也可以直接使用总升力、滚转力矩、俯仰力矩、偏航力矩作为虚拟控制输入。在悬停线性化模型中配平状态是x y z 0 附近vx vy vz 0phi theta psi 0p q r 0。控制输入的配平值是四个旋翼等速旋转产生的升力等于重力。2.3 常见参数表四旋翼悬停线性化模型中常用到的物理参数如下实际数值以项目代码为准参数符号单位说明质量mkg飞行器总质量重力加速度gm/s²取 9.81机臂长度lm旋翼中心到机体中心距离转动惯量Ixx, Iyy, Izzkg·m²三轴转动惯量推力系数kTN/(rad/s)²旋翼升力系数扭矩系数kMN·m/(rad/s)²旋翼反扭矩系数注意不同论文中推力系数、扭矩系数的定义方式不同有的用“转速平方 × 系数”有的用 PWM 信号映射。使用项目代码时建议先检查参数定义是否和你的仿真需求一致。2.4 代码示例构建状态空间模型在 Matlab 中构建 A、B、C、D 矩阵的常见方式如下。下面代码是通用示例展示了如何把参数赋值、矩阵构造和状态空间模型封装在一起具体矩阵元素需要按照项目的动力学方程填充。% 四旋翼悬停线性化模型参数 m 1.0; % 质量 kg g 9.81; % 重力加速度 m/s^2 l 0.25; % 机臂长度 m Ixx 0.03; % 滚转轴转动惯量 kg*m^2 Iyy 0.03; % 俯仰轴转动惯量 kg*m^2 Izz 0.06; % 偏航轴转动惯量 kg*m^2 % 状态向量: [x y z vx vy vz phi theta psi p q r] % 控制输入: [u1 u2 u3 u4] 分别对应总升力、滚转力矩、俯仰力矩、偏航力矩 A zeros(12, 12); B zeros(12, 4); % 位置与速度关系 A(1,4) 1; A(2,5) 1; A(3,6) 1; % 速度与姿态角关系悬停点小角度近似 A(4,8) g; % x 方向速度受俯仰角影响 A(5,7) -g; % y 方向速度受滚转角影响 % 姿态角与角速度关系 A(7,10) 1; A(8,11) 1; A(9,12) 1; % 角速度与力矩关系 B(11,2) l / Ixx; B(12,3) l / Iyy; % 输出矩阵默认输出全部状态 C eye(12); D zeros(12, 4); % 构建状态空间模型 quad_model ss(A, B, C, D); disp(quad_model);这个示例中的矩阵填充方式是悬停线性化模型的典型结构。实际上Z 轴方向的垂直运动通常由总升力和重力差决定所以状态矩阵中 z、vz 与总升力输入 u1 的关系也需要填入。不同项目对输入的定义不同有些直接用四旋翼四个电机的转速平方作为控制输入这种情况下 B 矩阵的写法会不同。3. PID 控制器设计与 Matlab 实现3.1 设计思路PID 控制器的核心思路是根据误差的比例、积分、微分来生成控制量。在四旋翼悬停控制中常见的做法是采用串级 PID外环位置环输出期望姿态角内环姿态环输出期望力矩对于悬停线性化模型可以先垂直通道用单环 PID 控制高度水平通道用位置-速度-姿态的串级结构。不过在很多论文中为了简化对比会在不同通道上直接使用独立的 PID 控制器。3.2 Matlab 代码示例下面是一个高度通道 PID 控制的示例用于说明如何在 Matlab 中实现 PID 控制律。% 高度通道 PID 控制器参数 Kp_z 4.0; Ki_z 0.5; Kd_z 1.5; % 目标高度和当前高度 z_des 0; z_cur 1.0; % 初始化积分项和上一时刻误差 integral_z 0; prev_error_z 0; dt 0.01; for k 1:1000 error_z z_des - z_cur; integral_z integral_z error_z * dt; derivative_z (error_z - prev_error_z) / dt; % 控制量总升力增量需要加上重力补偿 u1 Kp_z * error_z Ki_z * integral_z Kd_z * derivative_z m * g; % 更新状态这里需要调用四旋翼状态更新函数 % z_cur, vz_cur update_state(z_cur, vz_cur, u1, dt); prev_error_z error_z; end实际项目中PID 控制器的实现会更完整。通常会把 PID 类封装成函数便于在位置环、速度环、姿态环中复用。3.3 PID 调参重点PID 调参是四旋翼控制中最耗时间的一步。从项目角度建议按以下顺序调整先调高度通道Z 轴其他通道保持 0 参考输入。先加比例系数观察响应速度再逐渐加积分消除稳态误差。最后加微分抑制超调。姿态通道调参同理但要注意角度环和角速度环的带宽匹配关系。PID 的优点是实现简单、计算量小、工程上容易调参缺点是没有显式的约束处理能力无法保证在电机转速饱和时依然有良好的性能。4. LQR 控制器设计与 Matlab 实现4.1 设计思路LQRLinear Quadratic Regulator线性二次型调节器的核心思想是给定线性状态空间模型设计一个状态反馈控制器 u -Kx使得一个二次型性能指标最小。性能指标一般写为J integral( x * Q * x u * R * u ) dt其中 Q 是状态权重矩阵R 是控制输入权重矩阵。LQR 的设计过程就是求解 Riccati 方程得到最优反馈增益矩阵 K。在悬停控制中Q 矩阵中位置误差、姿态误差的权重越大系统响应越快R 矩阵中控制输入的权重越大控制能量越小响应越平缓。4.2 Matlab 代码示例Matlab 的lqr函数可以直接计算反馈增益矩阵% LQR 权重矩阵 Q diag([10, 10, 10, 2, 2, 2, 5, 5, 5, 1, 1, 1]); R diag([1, 1, 1, 1]); % 计算 LQR 反馈增益 K_lqr lqr(quad_model.A, quad_model.B, Q, R); disp(LQR 反馈增益矩阵:); disp(K_lqr);得到 K_lqr 之后控制输入可以写成u -K_lqr * x;在仿真中每个控制周期根据当前状态向量 x 计算控制输入然后把 u 输入给四旋翼模型更新状态。4.3 LQR 参数调节原则LQR 的调参相比 PID 要“数学化”一些但 Q 和 R 的选取仍和工程经验相关状态量权重 Q 越大状态收敛越快但控制量可能过大。控制量权重 R 越大控制动作越平缓但响应变慢。一般先固定 R调整 Q观察仿真曲线是否满足超调量、调节时间的要求。如果某个通道响应太慢增大该状态对应的 Q 值即可。如果控制量饱和增大对应的 R 值。LQR 的缺点是它是状态反馈控制器要求所有状态可测或可估计。在仿真中这很容易做到因为仿真中所有状态都是已知的。在实际飞行器中可能需要加入状态观测器如卡尔曼滤波。5. MPC 控制器设计与 Matlab 实现5.1 设计思路MPCModel Predictive Control模型预测控制的核心思想是在每个控制周期基于当前状态和系统模型求解一个有限时域的优化问题得到未来 N 步的最优控制序列然后只取第一步执行。MPC 相比 PID 和 LQR 的最大优势是可以显式处理约束。比如电机转速限制、控制输入饱和、姿态角限制等都可以写成优化问题的约束条件。在四旋翼悬停控制中MPC 的优化目标通常包括状态误差尽快趋于零控制输入尽量小满足控制输入上界/下界约束5.2 Matlab 代码示例Matlab 中实现 MPC 通常有两种方式使用 MPC Toolbox 的mpc对象。手动编写二次规划求解代码使用quadprog。先看 MPC Toolbox 的方式% MPC 预测模型将连续模型离散化 Ts 0.05; quad_model_d c2d(quad_model, Ts); % 创建 MPC 控制器 mpcobj mpc(quad_model_d, Ts); mpcobj.PredictionHorizon 20; mpcobj.ControlHorizon 5; % 设置控制输入约束 mpcobj.MV(1).Min 0; mpcobj.MV(1).Max 20; mpcobj.MV(2).Min -5; mpcobj.MV(2).Max 5; % 设置权重 mpcobj.Weights.OutputVariables [10, 10, 10, 2, 2, 2, 5, 5, 5, 1, 1, 1]; mpcobj.Weights.ManipulatedVariables [1, 1, 1, 1]; mpcobj.Weights.ManipulatedVariablesRate [0.1, 0.1, 0.1, 0.1]; % 仿真 x zeros(12, 1); for k 1:200 % 计算 MPC 控制量 u mpcstep(mpcobj, x); % 更新状态 % x_new quad_model_discrete_update(x, u); end注意mpcstep是 MPC 仿真中使用的函数实际调用方式需要结合 MPC Toolbox 的版本和具体代码。如果不想用 MPC Toolbox可以使用quadprog手动实现虽然代码量多一些但能更清楚地理解 MPC 的优化机制。5.3 手动实现 MPC手动实现 MPC 的基本步骤如下将预测模型离散化。根据预测时域 N构建预测方程。将优化问题写成标准二次规划形式。调用quadprog求解。伪代码如下% 离散化模型 Ad quad_model_d.A; Bd quad_model_d.B; % 初始化参数 N 20; % 预测时域 Nc 5; % 控制时域 Q diag([10, 10, 10, 2, 2, 2, 5, 5, 5, 1, 1, 1]); R diag([1, 1, 1, 1]); % 每个控制周期构建并求解二次规划 for k 1:200 % 构建预测矩阵 Phi 和 Gamma % Phi [Ad; Ad^2; ...; Ad^N] % Gamma 对应的输入输出映射矩阵 % 定义二次规划目标: min 0.5 * dU * H * dU f * dU H Gamma * kron(eye(N), Q) * Gamma kron(eye(Nc), R); f Gamma * kron(eye(N), Q) * (Phi * x); % 约束: 控制输入上下界 Aineq []; bineq []; lb -5 * ones(Nc * 4, 1); ub 5 * ones(Nc * 4, 1); % 求解 dU quadprog(H, f, Aineq, bineq, [], [], lb, ub); % 取第一个控制增量 u dU(1:4); % 更新状态 x Ad * x Bd * u; end这里kron函数用来构造预测时域内的块对角权重矩阵。需要注意的是构造预测矩阵Phi和Gamma时不同状态维度下矩阵尺寸会很大需要仔细核对维度。如果不装 MPC Toolbox手动实现方式完全够用而且能帮助理解 MPC 的原理。6. 环境准备与运行方式6.1 Matlab 版本要求项目是纯脚本仿真对 Matlab 版本要求不算苛刻。从代码常见函数来看如果只用 PID 和 LQR基础 Matlab Control System Toolbox 即可。如果使用 MPC Toolbox 的mpc对象则必须安装 Model Predictive Control Toolbox。如果使用quadprog手动实现 MPC需要 Optimization Toolbox。推荐使用 Matlab R2021a 或更高版本。早期版本也能运行但mpc对象的某些属性命名可能略有差异需要看具体代码的兼容范围。6.2 安装工具箱检查在 Matlab 命令窗口中输入ver查看是否安装 Control System Toolbox、Optimization Toolbox、Model Predictive Control Toolbox。如果没有安装可以在 Add-On Explorer 中搜索并安装。教育版账号通常可以免费使用所有工具箱。6.3 代码目录结构一个完整的项目通常包含以下文件quadrotor_hover_control/ ├── main.m % 主脚本运行三种控制器对比 ├── quad_model.m % 四旋翼悬停线性化模型 ├── pid_controller.m % PID 控制器 ├── lqr_controller.m % LQR 控制器 ├── mpc_controller.m % MPC 控制器 ├── plot_results.m % 绘图脚本 ├── params.m % 模型参数 └── README.md % 说明文档将整个文件夹添加到 Matlab 路径中即可运行。6.4 启动运行在 Matlab 中打开main.m点击运行按钮或者直接在命令窗口输入main如果代码没有错误程序会依次执行三种控制器的仿真并在最后输出对比曲线图。7. 功能测试与效果验证7.1 测试目的运行项目的核心目的是验证三种控制器在相同初始偏差下能否让四旋翼回到悬停状态并对比调节时间、超调量、稳态误差和控制能量。建议测试以下场景高度通道初始偏差 1m观察高度响应。水平位置 x 通道初始偏差 0.5m观察位置响应。姿态角初始偏差 5 度观察姿态响应。加入控制输入饱和约束后观察 MPC 和 PID/LQR 的表现差异。7.2 操作步骤打开params.m检查模型参数。打开main.m确认仿真的参考输入和初始状态设置。运行main.m。观察命令窗口输出数据记录三种控制器的调节时间、超调量。查看plot_results.m生成的曲线图。7.3 预期结果以典型悬停线性化模型为例合理预期是PID实现简单调参合适时能稳定悬停但可能出现超调对约束处理能力较弱。LQR响应较快状态收敛平稳但需要调节 Q 和 R 矩阵且控制量可能超出实际电机能力。MPC能够显式处理控制量约束控制效果更平滑但计算耗时明显增加。注意实际仿真曲线取决于参数设置以上只是常见规律不是固定结论。7.4 判断是否成功判断三种控制器是否有效的标准状态最终是否收敛到 0 附近。稳态误差是否在设定范围内。控制输入是否在设定的约束范围内。曲线是否平滑是否有剧烈振荡。如果 PID 和 LQR 都稳定但 MPC 无法求解优先检查预测时域和控制时域的参数是否合理以及二次规划问题的约束是否矛盾。8. 接口 API 与批量任务说明8.1 接口 API这个项目是离线 Matlab 仿真项目没有提供 API 接口。如果需要对外提供服务可以考虑以下扩展方式使用 Matlab Compiler SDK 将仿真函数打包成 DLL 或 Java/Python 可调用的组件。使用 Matlab Web App Server 发布交互式 Web 应用。将核心控制算法移植到 Python使用 Flask/FastAPI 提供 REST API。对于课程作业和毕业论文接口不是必须的。8.2 批量任务项目支持通过脚本循环批量运行不同控制器参数。例如要对比多组 PID 参数的效果可以写一个循环脚本% 批量测试不同 PID 参数 Kp_list [2.0, 4.0, 6.0]; Ki_list [0.1, 0.5, 1.0]; for i 1:length(Kp_list) for j 1:length(Ki_list) Kp Kp_list(i); Ki Ki_list(j); % 设置参数并运行仿真 % run_simulation(Kp, Ki); % 保存结果 % save(fullfile(results, sprintf(pid_kp_%.1f_ki_%.1f.mat, Kp, Ki)), result); end end批量运行的核心要点是每个仿真用独立的输出文件名避免覆盖。在循环内清空工作区变量或使用独立函数封装仿真逻辑。记录每组参数对应的性能指标便于后续分析和绘图。8.3 参数扫描的工程建议如果要做参数扫描建议把仿真逻辑封装成函数function result run_simulation(controller_type, params, initial_state, total_time) % 运行一次仿真返回时间序列、状态序列、控制输入序列和性能指标 end这样可以在不污染全局工作区的前提下批量调用并保存结果。9. 资源占用与性能观察9.1 资源占用纯 Matlab 离线仿真的资源占用很低普通笔记本即可流畅运行。三种控制器中PID计算量最小每条指令只有几个乘加运算。LQR计算量也很小状态反馈控制只是常数矩阵乘法。MPC计算量最大因为每个控制周期需要求解一个约束优化问题。预测时域越长、控制时域越长求解时间越长。如果仿真时间设为 10 秒采样时间为 0.01 秒总共 1000 个控制周期。PID 和 LQR 几乎瞬间完成MPC 可能需要几秒到几十秒具体取决于预测时域和优化问题规模。9.2 如何观察仿真耗时在 Matlab 中可以用tic和toc计时tic; run_simulation(MPC, params, x0, 10); elapsed toc; fprintf(MPC 仿真耗时: %.3f 秒\n, elapsed);9.3 降低 MPC 计算量的方法如果 MPC 计算太慢可以减小预测时域PredictionHorizon比如从 20 降到 10。减小控制时域ControlHorizon比如从 5 降到 3。增大采样时间 Ts从 0.01 增大到 0.02 或 0.05。关闭非必要的输出约束减少约束数量。10. 常见问题与排查方法问题现象可能原因排查方式解决方案运行 main.m 报错“未定义函数或变量”项目文件夹未添加到 Matlab 路径在命令窗口输入addpath(genpath(项目路径))添加路径后重新运行lqr函数报错未安装 Control System Toolbox输入ver查看工具箱列表安装 Control System Toolboxmpc对象无法创建未安装 Model Predictive Control Toolbox输入ver查看工具箱列表安装 MPC Toolboxquadprog报错未安装 Optimization Toolbox输入ver查看工具箱列表安装 Optimization ToolboxMPC 求解失败或求解时间过长预测时域过大或约束过紧检查 MPC 参数设置减小预测时域、放宽约束系统响应不稳定控制器参数不合适检查 PID 参数、LQR 权重矩阵、MPC 权重调参或对比默认参数状态不能收敛到零初始状态与参考输入不匹配检查初始状态设置修正初始状态绘图结果无曲线仿真循环未执行或数据未保存检查循环条件、数据存储方式加入hold on、plot调用检查控制量出现 NaN模型矩阵包含 NaN 或无穷大检查 A、B 矩阵构造修正矩阵参数电机转速出现负数控制输入未加非负约束检查饱和和约束设置增加输入下限约束不同版本 Matlab 运行报错工具箱函数的命名或属性不同查看 Matlab 版本和文档调整函数调用方式11. 最佳实践与使用建议11.1 先跑通默认参数再调参第一次运行项目时不要急着改参数。先使用默认参数跑通整个流程观察曲线和输出数据确认模型、控制器、绘图逻辑都正常然后再调整参数测试。11.2 把控制器封装成独立函数建议将三种控制器分别封装成函数输入是当前状态和参考状态输出是控制量。这样便于做批量测试也便于以后迁移到其他平台。11.3 保存中间结果批量测试时建议把每次仿真的性能指标保存到表格中% 创建结果表 results_table table(); % 每次仿真结束后添加一行 % results_table [results_table; table(Kp, Ki, settling_time, overshoot, mpc_time)];这样便于对比和撰写报告。11.4 合规使用提醒本项目是面向仿真研究的控制算法对比代码。如果后续进行真实无人机飞行测试必须注意飞行测试需要遵守当地空域管理法规在合法合规的场地进行。确保飞行器设备安全配备必要的保护措施。如有采集人脸、声音等敏感数据要提前获得授权。发布论文或商用项目中引用代码和模型需遵循原作者的开源协议。11.5 扩展方向如果没有做实物测试的需求可以在这个项目基础上继续扩展加入风扰动模型测试三种控制器的抗扰性能。加入传感器噪声对比鲁棒性。将 MPC 的约束扩展到姿态角和电机转速。用 Simulink 搭建非线性四旋翼模型将线性控制器和非线性模型联合仿真观察线性化误差的影响。将代码迁移到 Python结合 numpy 和 scipy 复现同一套对比流程。12. 总结与下一步这个项目的最大价值是把三种常用的控制算法放在同一个线性模型下做横向对比。对初学者来说它可以直观看到 PID、LQR、MPC 在响应速度、控制能量、约束处理能力上的差异对做毕业设计的同学来说它提供了完整的 Matlab 代码框架方便在此基础上替换模型、增加扰动、扩展算法。最先要跑通的是main.m确认三种控制器都能输出响应曲线最容易踩的坑是缺少工具箱尤其是 MPC Toolbox 和 Optimization Toolbox建议提前用ver检查。接下来可以尝试修改 Q 和 R 矩阵观察 LQR 响应变化再给 MPC 增加控制输入约束对比有约束和无约束时的表现。建议收藏备用。如果你在运行过程中遇到报错先对照常见问题排查表逐项检查确认模型路径和工具箱都正常再考虑是否调整控制器参数。
四旋翼悬停控制仿真：PID、LQR与MPC对比实现
这次我们来看一个经常被问到的四旋翼控制仿真项目基于悬停线性化模型用 Matlab 同时实现 PID、LQR 和 MPC 三种控制器并给出性能对比。项目代码带详细注释适合做毕业设计、课程作业也适合刚入门四旋翼控制、想把理论模型和仿真代码对应起来的工程师。先说结论如果你正在纠结“PID、LQR、MPC 到底选哪个”或者“论文里需要一张三种控制器的对比曲线”这个项目可以直接作为起点。它不是一个人工搭建的复杂非线性仿真平台而是把四旋翼在悬停点附近做小扰动线性化得到标准的线性状态空间模型然后分别设计三种控制器。好处很明显模型简单、状态方程直观、控制器参数调节逻辑清晰、Matlab 代码可读性强。本文会按以下顺序展开第一核心能力速览第二四旋翼悬停线性化模型的推导思路和状态空间方程第三三种控制器的设计原理和 Matlab 实现示例第四环境准备和运行方式第五功能测试与结果对比第六常见问题排查最后给出最佳实践建议。1. 核心能力速览能力项说明项目类型四旋翼无人机悬停控制仿真Matlab 代码 详细注释模型类型悬停点附近小扰动线性化模型状态空间形式控制器类型PID、LQR、MPC 三种控制器对比主要功能三通道姿态/位置控制仿真绘制响应曲线分析控制性能运行环境Matlab需安装 Control System Toolbox、MPC Toolbox仿真语言Matlab 脚本.m 文件是否需要 Simulink不需要脚本仿真即可运行硬件要求普通电脑即可不需要 GPU不需要嵌入式硬件是否支持 API不支持属于离线仿真项目是否支持批量任务可通过脚本循环批量跑不同控制器参数但无任务队列适合场景控制课程作业、本科毕业设计、控制算法入门对比研究从材料看项目的核心价值不在于“飞起来”而在于把悬停控制这个物理问题抽象成线性系统然后用三种控制算法做横向对比。对于想理解 PID、LQR、MPC 差异的读者这种对比比孤立看理论公式更直观。需要注意如果做实际飞行器实验或硬件在环测试还需要考虑执行器饱和、姿态解算、传感器噪声、模型不确定性等因素这个线性化模型适合作为算法研究和教学验证的基础。2. 四旋翼悬停线性化模型与状态空间方程2.1 为什么要在悬停点线性化四旋翼是一个典型的欠驱动、非线性、强耦合系统。完整描述它的运动需要刚体动力学方程、运动学方程、空气动力学模型直接做非线性控制会非常复杂。但是在悬停状态附近飞行器的姿态角很小线速度和角速度都接近零此时可以将非线性方程展开成线性模型这就是小扰动线性化。悬停线性化模型的工程意义在于很多实际飞控系统的姿态内环就是在悬停点附近工作的。只要飞行器不做大机动飞行动作线性模型已经能够反映主要动态特性并且在设计控制器时可以使用成熟的线性系统理论。2.2 状态空间模型的一般形式对于悬停状态下的四旋翼常见做法是把状态变量选为位置和速度、姿态角和角速度的组合。一个典型的线性化状态空间模型可以写成x_dot A * x B * u y C * x D * u其中状态向量通常包含位置x、y、z线速度vx、vy、vz姿态角phi滚转角、theta俯仰角、psi偏航角角速度p、q、r控制输入 u 通常是四个旋翼产生的升力或转速的线性组合也可以直接使用总升力、滚转力矩、俯仰力矩、偏航力矩作为虚拟控制输入。在悬停线性化模型中配平状态是x y z 0 附近vx vy vz 0phi theta psi 0p q r 0。控制输入的配平值是四个旋翼等速旋转产生的升力等于重力。2.3 常见参数表四旋翼悬停线性化模型中常用到的物理参数如下实际数值以项目代码为准参数符号单位说明质量mkg飞行器总质量重力加速度gm/s²取 9.81机臂长度lm旋翼中心到机体中心距离转动惯量Ixx, Iyy, Izzkg·m²三轴转动惯量推力系数kTN/(rad/s)²旋翼升力系数扭矩系数kMN·m/(rad/s)²旋翼反扭矩系数注意不同论文中推力系数、扭矩系数的定义方式不同有的用“转速平方 × 系数”有的用 PWM 信号映射。使用项目代码时建议先检查参数定义是否和你的仿真需求一致。2.4 代码示例构建状态空间模型在 Matlab 中构建 A、B、C、D 矩阵的常见方式如下。下面代码是通用示例展示了如何把参数赋值、矩阵构造和状态空间模型封装在一起具体矩阵元素需要按照项目的动力学方程填充。% 四旋翼悬停线性化模型参数 m 1.0; % 质量 kg g 9.81; % 重力加速度 m/s^2 l 0.25; % 机臂长度 m Ixx 0.03; % 滚转轴转动惯量 kg*m^2 Iyy 0.03; % 俯仰轴转动惯量 kg*m^2 Izz 0.06; % 偏航轴转动惯量 kg*m^2 % 状态向量: [x y z vx vy vz phi theta psi p q r] % 控制输入: [u1 u2 u3 u4] 分别对应总升力、滚转力矩、俯仰力矩、偏航力矩 A zeros(12, 12); B zeros(12, 4); % 位置与速度关系 A(1,4) 1; A(2,5) 1; A(3,6) 1; % 速度与姿态角关系悬停点小角度近似 A(4,8) g; % x 方向速度受俯仰角影响 A(5,7) -g; % y 方向速度受滚转角影响 % 姿态角与角速度关系 A(7,10) 1; A(8,11) 1; A(9,12) 1; % 角速度与力矩关系 B(11,2) l / Ixx; B(12,3) l / Iyy; % 输出矩阵默认输出全部状态 C eye(12); D zeros(12, 4); % 构建状态空间模型 quad_model ss(A, B, C, D); disp(quad_model);这个示例中的矩阵填充方式是悬停线性化模型的典型结构。实际上Z 轴方向的垂直运动通常由总升力和重力差决定所以状态矩阵中 z、vz 与总升力输入 u1 的关系也需要填入。不同项目对输入的定义不同有些直接用四旋翼四个电机的转速平方作为控制输入这种情况下 B 矩阵的写法会不同。3. PID 控制器设计与 Matlab 实现3.1 设计思路PID 控制器的核心思路是根据误差的比例、积分、微分来生成控制量。在四旋翼悬停控制中常见的做法是采用串级 PID外环位置环输出期望姿态角内环姿态环输出期望力矩对于悬停线性化模型可以先垂直通道用单环 PID 控制高度水平通道用位置-速度-姿态的串级结构。不过在很多论文中为了简化对比会在不同通道上直接使用独立的 PID 控制器。3.2 Matlab 代码示例下面是一个高度通道 PID 控制的示例用于说明如何在 Matlab 中实现 PID 控制律。% 高度通道 PID 控制器参数 Kp_z 4.0; Ki_z 0.5; Kd_z 1.5; % 目标高度和当前高度 z_des 0; z_cur 1.0; % 初始化积分项和上一时刻误差 integral_z 0; prev_error_z 0; dt 0.01; for k 1:1000 error_z z_des - z_cur; integral_z integral_z error_z * dt; derivative_z (error_z - prev_error_z) / dt; % 控制量总升力增量需要加上重力补偿 u1 Kp_z * error_z Ki_z * integral_z Kd_z * derivative_z m * g; % 更新状态这里需要调用四旋翼状态更新函数 % z_cur, vz_cur update_state(z_cur, vz_cur, u1, dt); prev_error_z error_z; end实际项目中PID 控制器的实现会更完整。通常会把 PID 类封装成函数便于在位置环、速度环、姿态环中复用。3.3 PID 调参重点PID 调参是四旋翼控制中最耗时间的一步。从项目角度建议按以下顺序调整先调高度通道Z 轴其他通道保持 0 参考输入。先加比例系数观察响应速度再逐渐加积分消除稳态误差。最后加微分抑制超调。姿态通道调参同理但要注意角度环和角速度环的带宽匹配关系。PID 的优点是实现简单、计算量小、工程上容易调参缺点是没有显式的约束处理能力无法保证在电机转速饱和时依然有良好的性能。4. LQR 控制器设计与 Matlab 实现4.1 设计思路LQRLinear Quadratic Regulator线性二次型调节器的核心思想是给定线性状态空间模型设计一个状态反馈控制器 u -Kx使得一个二次型性能指标最小。性能指标一般写为J integral( x * Q * x u * R * u ) dt其中 Q 是状态权重矩阵R 是控制输入权重矩阵。LQR 的设计过程就是求解 Riccati 方程得到最优反馈增益矩阵 K。在悬停控制中Q 矩阵中位置误差、姿态误差的权重越大系统响应越快R 矩阵中控制输入的权重越大控制能量越小响应越平缓。4.2 Matlab 代码示例Matlab 的lqr函数可以直接计算反馈增益矩阵% LQR 权重矩阵 Q diag([10, 10, 10, 2, 2, 2, 5, 5, 5, 1, 1, 1]); R diag([1, 1, 1, 1]); % 计算 LQR 反馈增益 K_lqr lqr(quad_model.A, quad_model.B, Q, R); disp(LQR 反馈增益矩阵:); disp(K_lqr);得到 K_lqr 之后控制输入可以写成u -K_lqr * x;在仿真中每个控制周期根据当前状态向量 x 计算控制输入然后把 u 输入给四旋翼模型更新状态。4.3 LQR 参数调节原则LQR 的调参相比 PID 要“数学化”一些但 Q 和 R 的选取仍和工程经验相关状态量权重 Q 越大状态收敛越快但控制量可能过大。控制量权重 R 越大控制动作越平缓但响应变慢。一般先固定 R调整 Q观察仿真曲线是否满足超调量、调节时间的要求。如果某个通道响应太慢增大该状态对应的 Q 值即可。如果控制量饱和增大对应的 R 值。LQR 的缺点是它是状态反馈控制器要求所有状态可测或可估计。在仿真中这很容易做到因为仿真中所有状态都是已知的。在实际飞行器中可能需要加入状态观测器如卡尔曼滤波。5. MPC 控制器设计与 Matlab 实现5.1 设计思路MPCModel Predictive Control模型预测控制的核心思想是在每个控制周期基于当前状态和系统模型求解一个有限时域的优化问题得到未来 N 步的最优控制序列然后只取第一步执行。MPC 相比 PID 和 LQR 的最大优势是可以显式处理约束。比如电机转速限制、控制输入饱和、姿态角限制等都可以写成优化问题的约束条件。在四旋翼悬停控制中MPC 的优化目标通常包括状态误差尽快趋于零控制输入尽量小满足控制输入上界/下界约束5.2 Matlab 代码示例Matlab 中实现 MPC 通常有两种方式使用 MPC Toolbox 的mpc对象。手动编写二次规划求解代码使用quadprog。先看 MPC Toolbox 的方式% MPC 预测模型将连续模型离散化 Ts 0.05; quad_model_d c2d(quad_model, Ts); % 创建 MPC 控制器 mpcobj mpc(quad_model_d, Ts); mpcobj.PredictionHorizon 20; mpcobj.ControlHorizon 5; % 设置控制输入约束 mpcobj.MV(1).Min 0; mpcobj.MV(1).Max 20; mpcobj.MV(2).Min -5; mpcobj.MV(2).Max 5; % 设置权重 mpcobj.Weights.OutputVariables [10, 10, 10, 2, 2, 2, 5, 5, 5, 1, 1, 1]; mpcobj.Weights.ManipulatedVariables [1, 1, 1, 1]; mpcobj.Weights.ManipulatedVariablesRate [0.1, 0.1, 0.1, 0.1]; % 仿真 x zeros(12, 1); for k 1:200 % 计算 MPC 控制量 u mpcstep(mpcobj, x); % 更新状态 % x_new quad_model_discrete_update(x, u); end注意mpcstep是 MPC 仿真中使用的函数实际调用方式需要结合 MPC Toolbox 的版本和具体代码。如果不想用 MPC Toolbox可以使用quadprog手动实现虽然代码量多一些但能更清楚地理解 MPC 的优化机制。5.3 手动实现 MPC手动实现 MPC 的基本步骤如下将预测模型离散化。根据预测时域 N构建预测方程。将优化问题写成标准二次规划形式。调用quadprog求解。伪代码如下% 离散化模型 Ad quad_model_d.A; Bd quad_model_d.B; % 初始化参数 N 20; % 预测时域 Nc 5; % 控制时域 Q diag([10, 10, 10, 2, 2, 2, 5, 5, 5, 1, 1, 1]); R diag([1, 1, 1, 1]); % 每个控制周期构建并求解二次规划 for k 1:200 % 构建预测矩阵 Phi 和 Gamma % Phi [Ad; Ad^2; ...; Ad^N] % Gamma 对应的输入输出映射矩阵 % 定义二次规划目标: min 0.5 * dU * H * dU f * dU H Gamma * kron(eye(N), Q) * Gamma kron(eye(Nc), R); f Gamma * kron(eye(N), Q) * (Phi * x); % 约束: 控制输入上下界 Aineq []; bineq []; lb -5 * ones(Nc * 4, 1); ub 5 * ones(Nc * 4, 1); % 求解 dU quadprog(H, f, Aineq, bineq, [], [], lb, ub); % 取第一个控制增量 u dU(1:4); % 更新状态 x Ad * x Bd * u; end这里kron函数用来构造预测时域内的块对角权重矩阵。需要注意的是构造预测矩阵Phi和Gamma时不同状态维度下矩阵尺寸会很大需要仔细核对维度。如果不装 MPC Toolbox手动实现方式完全够用而且能帮助理解 MPC 的原理。6. 环境准备与运行方式6.1 Matlab 版本要求项目是纯脚本仿真对 Matlab 版本要求不算苛刻。从代码常见函数来看如果只用 PID 和 LQR基础 Matlab Control System Toolbox 即可。如果使用 MPC Toolbox 的mpc对象则必须安装 Model Predictive Control Toolbox。如果使用quadprog手动实现 MPC需要 Optimization Toolbox。推荐使用 Matlab R2021a 或更高版本。早期版本也能运行但mpc对象的某些属性命名可能略有差异需要看具体代码的兼容范围。6.2 安装工具箱检查在 Matlab 命令窗口中输入ver查看是否安装 Control System Toolbox、Optimization Toolbox、Model Predictive Control Toolbox。如果没有安装可以在 Add-On Explorer 中搜索并安装。教育版账号通常可以免费使用所有工具箱。6.3 代码目录结构一个完整的项目通常包含以下文件quadrotor_hover_control/ ├── main.m % 主脚本运行三种控制器对比 ├── quad_model.m % 四旋翼悬停线性化模型 ├── pid_controller.m % PID 控制器 ├── lqr_controller.m % LQR 控制器 ├── mpc_controller.m % MPC 控制器 ├── plot_results.m % 绘图脚本 ├── params.m % 模型参数 └── README.md % 说明文档将整个文件夹添加到 Matlab 路径中即可运行。6.4 启动运行在 Matlab 中打开main.m点击运行按钮或者直接在命令窗口输入main如果代码没有错误程序会依次执行三种控制器的仿真并在最后输出对比曲线图。7. 功能测试与效果验证7.1 测试目的运行项目的核心目的是验证三种控制器在相同初始偏差下能否让四旋翼回到悬停状态并对比调节时间、超调量、稳态误差和控制能量。建议测试以下场景高度通道初始偏差 1m观察高度响应。水平位置 x 通道初始偏差 0.5m观察位置响应。姿态角初始偏差 5 度观察姿态响应。加入控制输入饱和约束后观察 MPC 和 PID/LQR 的表现差异。7.2 操作步骤打开params.m检查模型参数。打开main.m确认仿真的参考输入和初始状态设置。运行main.m。观察命令窗口输出数据记录三种控制器的调节时间、超调量。查看plot_results.m生成的曲线图。7.3 预期结果以典型悬停线性化模型为例合理预期是PID实现简单调参合适时能稳定悬停但可能出现超调对约束处理能力较弱。LQR响应较快状态收敛平稳但需要调节 Q 和 R 矩阵且控制量可能超出实际电机能力。MPC能够显式处理控制量约束控制效果更平滑但计算耗时明显增加。注意实际仿真曲线取决于参数设置以上只是常见规律不是固定结论。7.4 判断是否成功判断三种控制器是否有效的标准状态最终是否收敛到 0 附近。稳态误差是否在设定范围内。控制输入是否在设定的约束范围内。曲线是否平滑是否有剧烈振荡。如果 PID 和 LQR 都稳定但 MPC 无法求解优先检查预测时域和控制时域的参数是否合理以及二次规划问题的约束是否矛盾。8. 接口 API 与批量任务说明8.1 接口 API这个项目是离线 Matlab 仿真项目没有提供 API 接口。如果需要对外提供服务可以考虑以下扩展方式使用 Matlab Compiler SDK 将仿真函数打包成 DLL 或 Java/Python 可调用的组件。使用 Matlab Web App Server 发布交互式 Web 应用。将核心控制算法移植到 Python使用 Flask/FastAPI 提供 REST API。对于课程作业和毕业论文接口不是必须的。8.2 批量任务项目支持通过脚本循环批量运行不同控制器参数。例如要对比多组 PID 参数的效果可以写一个循环脚本% 批量测试不同 PID 参数 Kp_list [2.0, 4.0, 6.0]; Ki_list [0.1, 0.5, 1.0]; for i 1:length(Kp_list) for j 1:length(Ki_list) Kp Kp_list(i); Ki Ki_list(j); % 设置参数并运行仿真 % run_simulation(Kp, Ki); % 保存结果 % save(fullfile(results, sprintf(pid_kp_%.1f_ki_%.1f.mat, Kp, Ki)), result); end end批量运行的核心要点是每个仿真用独立的输出文件名避免覆盖。在循环内清空工作区变量或使用独立函数封装仿真逻辑。记录每组参数对应的性能指标便于后续分析和绘图。8.3 参数扫描的工程建议如果要做参数扫描建议把仿真逻辑封装成函数function result run_simulation(controller_type, params, initial_state, total_time) % 运行一次仿真返回时间序列、状态序列、控制输入序列和性能指标 end这样可以在不污染全局工作区的前提下批量调用并保存结果。9. 资源占用与性能观察9.1 资源占用纯 Matlab 离线仿真的资源占用很低普通笔记本即可流畅运行。三种控制器中PID计算量最小每条指令只有几个乘加运算。LQR计算量也很小状态反馈控制只是常数矩阵乘法。MPC计算量最大因为每个控制周期需要求解一个约束优化问题。预测时域越长、控制时域越长求解时间越长。如果仿真时间设为 10 秒采样时间为 0.01 秒总共 1000 个控制周期。PID 和 LQR 几乎瞬间完成MPC 可能需要几秒到几十秒具体取决于预测时域和优化问题规模。9.2 如何观察仿真耗时在 Matlab 中可以用tic和toc计时tic; run_simulation(MPC, params, x0, 10); elapsed toc; fprintf(MPC 仿真耗时: %.3f 秒\n, elapsed);9.3 降低 MPC 计算量的方法如果 MPC 计算太慢可以减小预测时域PredictionHorizon比如从 20 降到 10。减小控制时域ControlHorizon比如从 5 降到 3。增大采样时间 Ts从 0.01 增大到 0.02 或 0.05。关闭非必要的输出约束减少约束数量。10. 常见问题与排查方法问题现象可能原因排查方式解决方案运行 main.m 报错“未定义函数或变量”项目文件夹未添加到 Matlab 路径在命令窗口输入addpath(genpath(项目路径))添加路径后重新运行lqr函数报错未安装 Control System Toolbox输入ver查看工具箱列表安装 Control System Toolboxmpc对象无法创建未安装 Model Predictive Control Toolbox输入ver查看工具箱列表安装 MPC Toolboxquadprog报错未安装 Optimization Toolbox输入ver查看工具箱列表安装 Optimization ToolboxMPC 求解失败或求解时间过长预测时域过大或约束过紧检查 MPC 参数设置减小预测时域、放宽约束系统响应不稳定控制器参数不合适检查 PID 参数、LQR 权重矩阵、MPC 权重调参或对比默认参数状态不能收敛到零初始状态与参考输入不匹配检查初始状态设置修正初始状态绘图结果无曲线仿真循环未执行或数据未保存检查循环条件、数据存储方式加入hold on、plot调用检查控制量出现 NaN模型矩阵包含 NaN 或无穷大检查 A、B 矩阵构造修正矩阵参数电机转速出现负数控制输入未加非负约束检查饱和和约束设置增加输入下限约束不同版本 Matlab 运行报错工具箱函数的命名或属性不同查看 Matlab 版本和文档调整函数调用方式11. 最佳实践与使用建议11.1 先跑通默认参数再调参第一次运行项目时不要急着改参数。先使用默认参数跑通整个流程观察曲线和输出数据确认模型、控制器、绘图逻辑都正常然后再调整参数测试。11.2 把控制器封装成独立函数建议将三种控制器分别封装成函数输入是当前状态和参考状态输出是控制量。这样便于做批量测试也便于以后迁移到其他平台。11.3 保存中间结果批量测试时建议把每次仿真的性能指标保存到表格中% 创建结果表 results_table table(); % 每次仿真结束后添加一行 % results_table [results_table; table(Kp, Ki, settling_time, overshoot, mpc_time)];这样便于对比和撰写报告。11.4 合规使用提醒本项目是面向仿真研究的控制算法对比代码。如果后续进行真实无人机飞行测试必须注意飞行测试需要遵守当地空域管理法规在合法合规的场地进行。确保飞行器设备安全配备必要的保护措施。如有采集人脸、声音等敏感数据要提前获得授权。发布论文或商用项目中引用代码和模型需遵循原作者的开源协议。11.5 扩展方向如果没有做实物测试的需求可以在这个项目基础上继续扩展加入风扰动模型测试三种控制器的抗扰性能。加入传感器噪声对比鲁棒性。将 MPC 的约束扩展到姿态角和电机转速。用 Simulink 搭建非线性四旋翼模型将线性控制器和非线性模型联合仿真观察线性化误差的影响。将代码迁移到 Python结合 numpy 和 scipy 复现同一套对比流程。12. 总结与下一步这个项目的最大价值是把三种常用的控制算法放在同一个线性模型下做横向对比。对初学者来说它可以直观看到 PID、LQR、MPC 在响应速度、控制能量、约束处理能力上的差异对做毕业设计的同学来说它提供了完整的 Matlab 代码框架方便在此基础上替换模型、增加扰动、扩展算法。最先要跑通的是main.m确认三种控制器都能输出响应曲线最容易踩的坑是缺少工具箱尤其是 MPC Toolbox 和 Optimization Toolbox建议提前用ver检查。接下来可以尝试修改 Q 和 R 矩阵观察 LQR 响应变化再给 MPC 增加控制输入约束对比有约束和无约束时的表现。建议收藏备用。如果你在运行过程中遇到报错先对照常见问题排查表逐项检查确认模型路径和工具箱都正常再考虑是否调整控制器参数。