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

这次我们来看一个经常被问到的四旋翼控制仿真项目：基于悬停线性化模型，用 Matlab 同时实现 PID、LQR 和 MPC 三种控制器，并给出性能对比。项目代码带详细注释，适合做毕业设计、课程作业，也适合刚入门四旋翼控制、想把理论模型和仿真代码对应起来的工程师。

先说结论：如果你正在纠结“PID、LQR、MPC 到底选哪个”，或者“论文里需要一张三种控制器的对比曲线”，这个项目可以直接作为起点。它不是一个人工搭建的复杂非线性仿真平台，而是把四旋翼在悬停点附近做小扰动线性化，得到标准的线性状态空间模型，然后分别设计三种控制器。好处很明显：模型简单、状态方程直观、控制器参数调节逻辑清晰、Matlab 代码可读性强。

本文会按以下顺序展开：第一，核心能力速览；第二，四旋翼悬停线性化模型的推导思路和状态空间方程；第三，三种控制器的设计原理和 Matlab 实现示例；第四，环境准备和运行方式；第五，功能测试与结果对比；第六，常见问题排查；最后给出最佳实践建议。

1. 核心能力速览

从材料看，项目的核心价值不在于“飞起来”，而在于把悬停控制这个物理问题抽象成线性系统，然后用三种控制算法做横向对比。对于想理解 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 = 0，phi = theta = psi = 0，p = q = r = 0。控制输入的配平值是四个旋翼等速旋转，产生的升力等于重力。

2.3 常见参数表

四旋翼悬停线性化模型中常用到的物理参数如下，实际数值以项目代码为准：

注意：不同论文中推力系数、扭矩系数的定义方式不同，有的用“转速平方 × 系数”，有的用 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 调参是四旋翼控制中最耗时间的一步。从项目角度，建议按以下顺序调整：

1. 先调高度通道（Z 轴），其他通道保持 0 参考输入。
2. 先加比例系数，观察响应速度，再逐渐加积分消除稳态误差。
3. 最后加微分，抑制超调。
4. 姿态通道调参同理，但要注意角度环和角速度环的带宽匹配关系。

PID 的优点是实现简单、计算量小、工程上容易调参，缺点是没有显式的约束处理能力，无法保证在电机转速饱和时依然有良好的性能。

4. LQR 控制器设计与 Matlab 实现

4.1 设计思路

LQR（Linear 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 设计思路

MPC（Model Predictive Control，模型预测控制）的核心思想是：在每个控制周期，基于当前状态和系统模型，求解一个有限时域的优化问题，得到未来 N 步的最优控制序列，然后只取第一步执行。

MPC 相比 PID 和 LQR 的最大优势是：可以显式处理约束。比如电机转速限制、控制输入饱和、姿态角限制等，都可以写成优化问题的约束条件。

在四旋翼悬停控制中，MPC 的优化目标通常包括：

• 状态误差尽快趋于零
• 控制输入尽量小
• 满足控制输入上界/下界约束

5.2 Matlab 代码示例

Matlab 中实现 MPC 通常有两种方式：

1. 使用 MPC Toolbox 的mpc对象。
2. 手动编写二次规划求解代码（使用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 的基本步骤如下：

1. 将预测模型离散化。
2. 根据预测时域 N，构建预测方程。
3. 将优化问题写成标准二次规划形式。
4. 调用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 操作步骤

1. 打开params.m，检查模型参数。
2. 打开main.m，确认仿真的参考输入和初始状态设置。
3. 运行main.m。
4. 观察命令窗口输出数据，记录三种控制器的调节时间、超调量。
5. 查看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. 常见问题与排查方法

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 增加控制输入约束，对比有约束和无约束时的表现。

建议收藏备用。如果你在运行过程中遇到报错，先对照常见问题排查表逐项检查，确认模型路径和工具箱都正常，再考虑是否调整控制器参数。