这次我们来看一个偏控制工程方向的经典项目：四旋翼无人机悬停线性化模型的 PID、LQR 与 MPC 控制性能对比。项目给的是一整套 Matlab 代码加详细注释，适合做飞控算法入门、控制理论课设、本科毕设，或者提前调研飞控控制器选型的同学。它解决的问题很直接：同一架四旋翼，同一个悬停线性化模型，分别用 PID、LQR、MPC 去控制，谁的超调更小、谁的抗扰更强、谁的控制量更稳定、谁的代码更复杂。
在飞控圈子里，PID 是默认基线，LQR 提供全状态最优反馈，MPC 能显式处理约束。这三个控制器单独拿出来都有大量资料，但是放在同一套模型里做公平对比的代码并不多。这个项目的价值就在于它把建模、线性化、控制器设计、仿真对比串成了一条完整链路，而且代码注释很细，适合对照论文逐步理解。本文会带你拆解这套项目的建模思路、三种控制器的设计方法、关键 Matlab 代码，以及如何设计实验把结果对比清楚。
如果不想只停留在“能跑出图”的阶段，建议先做一件事：把非线性模型、悬停点线性化、控制器权重这三个环节单独拆开看。下面先给出一份核心能力速览，再逐项展开。
1. 核心能力速览
| 能力项 | 说明 |
|---|---|
| 项目类型 | 四旋翼无人机控制算法仿真与对比 |
| 模型基础 | 四旋翼刚体动力学模型，悬停点小扰动线性化 |
| 控制器覆盖 | PID、LQR、MPC 三种控制器 |
| 运行环境 | Matlab，建议 R2020b 及以上，实际以本机能安装的版本为准 |
| 需要硬件 | 不需要真实无人机，纯 Matlab 仿真可完成 |
| 代码形态 | 建模脚本、控制器函数、主仿真对比脚本，带详细注释 |
| 输出结果 | 状态响应曲线、控制量曲线、性能指标对比表 |
| 上手难度 | 中等，需要理解状态空间方程和反馈控制基本概念 |
| 适合场景 | 课设、毕设、飞控算法选型预研、控制理论入门 |
需要注意一个关键点：这个项目的所有结论都是针对“悬停点线性化模型”的。在这个模型下，PID、LQR、MPC 的对比是公平的、可复现的，但结论不能直接外推到真机大机动飞行场景。真机还会遇到执行器饱和、气动扰动、传感器噪声、模型不确定性，这些在悬停线性化模型里不一定完整体现。
2. 适用场景与使用边界
先说适合谁。如果你是自动化、控制工程、航空航天相关专业的学生，正在做四旋翼方向课设或者毕设，这套代码可以作为基础框架。它能帮你解决三件事：第一，理解四旋翼的动力学模型怎么建立；第二，学会在悬停点附近做线性化；第三，把 PID、LQR、MPC 放在同一个模型下对比，给自己的论文提供定量结果。
如果你是在做飞控相关竞赛项目，也可以用它做算法预研。先在线性化模型上调通控制逻辑，再迁移到半物理仿真或者实际飞控里。但要注意，实际工程里飞控通常用的是串级 PID 或者加入总能量控制、自抗扰控制等方案，这里的单层 PID 设计更多是教学基线。
这个项目不适合的场景也很明确：不适合直接用于真机部署。线性化模型只在悬停点附近有效，当无人机做大角度机动、高速飞行或者遇到阵风时，线性化模型的误差会明显增大。此时必须回到非线性模型，或者改用在模型预测控制中引入非线性预测模型与约束处理。实际飞行还涉及传感器融合、状态估计、执行器调度、航线规划等模块，这套代码只覆盖了姿态和位置控制最核心的控制律部分。
另外需要提醒使用边界：仿真用的控制器参数和真实飞行参数之间没有直接映射关系。真机需要根据桨叶、电机、机体惯量实测数据进行建模和调参。如果后续要做实物验证，必须遵守当地空域管理规定，在合法合规的场地进行，飞行前检查电量、GPS、遥控器对频和紧急停桨逻辑。
3. 四旋翼无人机建模与悬停点线性化
3.1 四旋翼动力学模型
四旋翼无人机通常建模为一个六自由度刚体。机体由四个旋翼提供升力和力矩，控制输入是四个电机的转速或者等效推力。常见做法是把四个旋翼的推力组合成四个虚拟控制量：总升力、俯仰力矩、滚转力矩、偏航力矩，然后分别控制高度、俯仰角、滚转角和偏航角。
在 Matlab 中建立非线性模型时，最简单的形式是使用欧拉角描述姿态。忽略空气阻力等未建模扰动时，位置和速度方程可以写成：
% 状态向量 x = [x; y; z; vx; vy; vz; phi; theta; psi; p; q; r] % 控制输入 u = [U1; U2; U3; U4] 分别对应总升力、滚转力矩、俯仰力矩、偏航力矩 % 以下为简化非线性模型的核心函数框架 function dx = quadrotor_dynamics(x, u, params) g = params.g; m = params.m; Ixx = params.Ixx; Iyy = params.Iyy; Izz = params.Izz; % 欧拉角和角速度 phi = x(7); theta = x(8); psi = x(9); p = x(10); q = x(11); r = x(12); U1 = u(1); % 总升力 U2 = u(2); % 滚转力矩 U3 = u(3); % 俯仰力矩 U4 = u(4); % 偏航力矩 % 加速度方程（简化形式） ax = (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) * U1 / m; ay = (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) * U1 / m; az = (cos(phi)*cos(theta)) * U1 / m - g; % 角加速度方程（简化形式） dp = (Iyy - Izz)/Ixx * q * r + U2 / Ixx; dq = (Izz - Ixx)/Iyy * p * r + U3 / Iyy; dr = (Ixx - Iyy)/Izz * p * q + U4 / Izz; dx = zeros(12, 1); dx(1:3) = x(4:6); dx(4:6) = [ax; ay; az]; dx(7:9) = [p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r; cos(phi)*q - sin(phi)*r; sin(phi)/cos(theta)*q + cos(phi)/cos(theta)*r]; dx(10:12) = [dp; dq; dr]; end这段代码是标准的刚体动力学框架，测试时可以直接作为非线性被控对象使用。
3.2 悬停点线性化
悬停状态下，无人机的平衡条件是：位置不变、速度为 0、姿态角接近 0、角速度为 0。此时四个旋翼产生的总升力约等于重力，即 U1 = m * g，其余力矩为 0。
在这个平衡点附近做一阶泰勒展开，就能得到线性状态空间模型：
x_dot = A * x + B * u
Matlab 里既可以用符号推导或者数值差分得到 A 和 B 矩阵，也可以在 simulink 里用 linmod 做线性化。比较可复现的方式是用数值扰动法：
% 数值线性化框架：在悬停平衡点附近对非线性模型求雅可比矩阵 % 状态量 12 维，控制量 4 维 % 平衡点 x0 = zeros(12, 1); u0 = [m*g; 0; 0; 0]; % 对状态求偏导 A A = zeros(12, 12); dx0 = quadrotor_dynamics(x0, u0, params); for i = 1:12 xp = x0; xp(i) = x0(i) + eps; xn = x0; xn(i) = x0(i) - eps; A(:, i) = (quadrotor_dynamics(xp, u0, params) - quadrotor_dynamics(xn, u0, params)) / (2*eps); end % 对控制求偏导 B B = zeros(12, 4); for j = 1:4 up = u0; up(j) = u0(j) + eps; un = u0; un(j) = u0(j) - eps; B(:, j) = (quadrotor_dynamics(x0, up, params) - quadrotor_dynamics(x0, un, params)) / (2*eps); end % 输出离散化模型 C = eye(12); D = zeros(12, 4); sys_c = ss(A, B, C, D); Ts = 0.01; sys_d = c2d(sys_c, Ts); Ad = sys_d.A; Bd = sys_d.B;这里给的是数值线性化模板，实际项目里需要把 eps 调整到合适的步长。更稳妥的判断是：线性化之后的模型矩阵应该满足悬停点附近的物理直觉，比如位置状态对应速度状态的一阶导数关系、输入扭矩对应角加速度的惯性比。拿到 A、B 矩阵后，控制器设计就可以开始。
4. 三种控制器设计思路
4.1 PID 控制设计
PID 控制器在飞控里最常见，优点是直观、计算量小、不依赖精确模型。悬停线性化模型下，可以把高度通道和三个姿态通道分开设计 PID。例如高度控制使用高度误差和垂向速度误差作为反馈，姿态控制使用目标角度与实际角度的误差。
在单层 PID 设计里，每个通道用标准 PID 公式控制，控制量 = Kp * 误差 + Ki * 误差积分 + Kd * 误差微分。仿真时需要对微分项做滤波处理，避免高频噪声被放大。
PID 参数的调优可以从 P 开始：先加入比例项让系统稳定，然后加 D 抑制超调，最后加 I 消除稳态误差。初始参数建议给到比较保守的范围，然后逐步加大。
4.2 LQR 控制设计
LQR 是线性二次型最优控制，核心是把控制问题转化为一个二次型性能指标优化。对于线性状态空间模型 x_dot = A x + B u，LQR 设计需要定义状态权重矩阵 Q 和控制权重矩阵 R：
J = ∫ (x^T Q x + u^T R u) dt
Q 矩阵越大，系统越强调快速回到平衡点；R 矩阵越大，系统越强调节省控制能量。Matlab 中直接使用 lqr 函数求解反馈增益 K。由于悬停模型是 12 维状态、4 维控制，通常会把 Q 设计成对角矩阵，对角元素分别对应位置、速度、姿态角、角速度的权重，R 对应四个虚拟控制量的权重。
LQR 的优势是全状态反馈，耦合处理比 PID 好；劣势是如果状态量不全可测，需要设计观测器或者做输出反馈。
4.3 MPC 控制设计
MPC 模型预测控制的核心思想是：在每个采样时刻，基于当前状态和预测模型，在预测时域内求解一个带约束的有限时域优化问题，得到一组最优控制序列，然后只执行第一步，到下一个采样时刻重新求解。
MPC 相比 PID 和 LQR 的明显优势是能处理约束。真实无人机的执行器是限幅的，电机转速有上下限，电流有上限，这些约束在 PID 和 LQR 里不好直接显式建模，但在 MPC 里可以作为约束条件直接写进优化问题。代价是计算量大，采样周期短的时候对实时性压力很高。
在悬停线性化模型中，MPC 可以使用离散线性模型作为预测模型，目标函数为预测状态偏差与控制增量的二次型，然后用二次规划求解器求解。Matlab 里可以用 mpc 工具箱，也可以自己写一个用 quadprog 或 fmincon 求解的简化版本。
5. Matlab 代码实现
5.1 PID 控制器代码
以下是一个高度通道 PID 的示例实现，姿态通道可以按同样结构扩展。
% 离散 PID 控制器，带微分滤波 classdef PIDController < handle properties Kp Ki Kd Ts integral prev_error diff_error filter_alpha end methods function obj = PIDController(Kp, Ki, Kd, Ts) obj.Kp = Kp; obj.Ki = Ki; obj.Kd = Kd; obj.Ts = Ts; obj.integral = 0; obj.prev_error = 0; obj.diff_error = 0; obj.filter_alpha = 0.1; % 微分滤波器系数，需要按实际调 end function u = update(obj, error) obj.integral = obj.integral + error * obj.Ts; % 简单微分滤波：一阶低通 obj.diff_error = obj.filter_alpha * (error - obj.prev_error) / obj.Ts ... + (1 - obj.filter_alpha) * obj.diff_error; obj.prev_error = error; u = obj.Kp * error + obj.Ki * obj.integral + obj.Kd * obj.diff_error; end end end使用示例：
% 创建高度 PID alt_pid = PIDController(8.0, 0.5, 2.0, Ts); % 每个控制周期调用 update，输入为高度误差 u_alt = alt_pid.update(z_ref - z_cur);这里 PID 参数是示例值，实际需要根据模型参数调整。初始调试可以先让 Ki = 0、Kd = 0，只调 Kp，直到系统稳定但不理想；再逐步加入 D 和 I。
5.2 LQR 控制器代码
LQR 的设计核心是选择权重矩阵。以下代码是 LQR 增益计算的示例：
% 基于悬停线性化模型设计 LQR % 假设已经得到连续或离散线性模型 sys_c / sys_d A = sys_c.A; B = sys_c.B; % 状态权重矩阵 Q，需要根据物理量纲和性能需求设置 % 这里给出一种对角配置示例：位置权重 10，速度权重 5，姿态权重 20，角速率权重 1 Q = diag([10*ones(1,3), 5*ones(1,3), 20*ones(1,3), 1*ones(1,3)]); % 控制权重矩阵 R，对应四个虚拟控制量 R = diag([0.1, 0.1, 0.1, 0.1]); % LQR 求解反馈增益矩阵 K [K, S, e] = lqr(A, B, Q, R); % 控制律 u = -K * x % 这里通常需要转换为跟踪参考输入的形式： % u = -K * (x - x_ref) + u_ref如果采用离散模型，可用 dlqr 函数。LQR 反馈增益 K 会把所有状态量加权组合到控制量上，所以需要确保所有状态可测或者有估计值。在仿真中这不成问题，在真机上则需要搭配状态估计器。
5.3 MPC 控制器代码
这里给出一个简化 MPC 的二次规划实现。核心是构建预测矩阵和目标函数，然后调用 quadprog 求解。
% 简化 MPC：线性模型 + 二次目标 + 控制量约束 % 离散模型 Ad, Bd 由 c2d 得到 % 预测时域 Np，控制时域 Nc，状态维度 nx，控制维度 nu function u_opt = mpc_control(Ad, Bd, x0, x_ref, Np, Nc, Q_mpc, R_mpc, u_min, u_max) nx = size(Ad, 1); nu = size(Bd, 2); % 构建预测系统矩阵 [F, G] = build_prediction_matrix(Ad, Bd, Np, Nc); % 目标函数转换为二次规划标准型：0.5 * u' * H * u + f' * u % 这里假设只优化控制序列 U 的长度为 Nc * nu Q_mpc_kron = kron(eye(Np), Q_mpc); R_mpc_kron = kron(eye(Nc), R_mpc); H = 2 * (G' * Q_mpc_kron * G + R_mpc_kron); f = 2 * G' * Q_mpc_kron * (F * x0 - kron(ones(Np, 1), x_ref)); % 控制量约束 lb = repmat(u_min, Nc, 1); ub = repmat(u_max, Nc, 1); opts = optimoptions('quadprog', 'Display', 'off'); U_opt = quadprog(H, f, [], [], [], [], lb, ub, [], opts); % 取第一步控制量 u_opt = U_opt(1:nu); end function [F, G] = build_prediction_matrix(Ad, Bd, Np, Nc) nx = size(Ad, 1); nu = size(Bd, 2); F = zeros(nx * Np, nx); G = zeros(nx * Np, nu * Nc); Ai = eye(nx); for i = 1:Np F((i-1)*nx+1:i*nx, :) = Ai; Ai = Ai * Ad; end % 构建 G 矩阵需要按预测时域和控制时域展开 % 这里给出标准 Givens 形式的构建逻辑 for i = 1:Np Ai = eye(nx); for j = 1:min(i, Nc) if i == 1 && j == 1 G((i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu) = Bd; elseif j == i G((i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu) = Bd; elseif i > j % 需要累乘 Ad tmp = Ad^(i-j) * Bd; G((i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu) = tmp; end end end end这段代码是简化教学实现，效率和数值稳定性都不如 Matlab MPC Toolbox，但逻辑清楚，适合理解 MPC 的核心预测优化结构。实际项目中如果使用 MPC Toolbox，可以用 mpc 对象直接搭建，更方便处理硬约束和软约束。
5.4 主仿真对比脚本
主脚本负责串联整个流程：
% 主仿真脚本：PID / LQR / MPC 对比 % 1. 初始化参数 params = struct('g', 9.81, 'm', 1.2, 'Ixx', 0.01, 'Iyy', 0.01, 'Izz', 0.02); Ts = 0.01; Tfinal = 10; t = 0:Ts:Tfinal; N = length(t); % 2. 得到线性模型 % A, B, sys_c, sys_d 由之前线性化代码得到 % ... % 3. 控制器初始化 pid_z = PIDController(8.0, 0.5, 2.0, Ts); % LQR 增益 Q_lqr = diag([10*ones(1,3), 5*ones(1,3), 20*ones(1,3), 1*ones(1,3)]); R_lqr = diag([0.1, 0.1, 0.1, 0.1]); [K_lqr, ~, ~] = lqr(sys_c.A, sys_c.B, Q_lqr, R_lqr); % MPC 参数 Np = 20; Nc = 5; Q_mpc = diag([5*ones(1,3), 2*ones(1,3), 10*ones(1,3), 0.5*ones(1,3)]); R_mpc = diag([0.5, 0.5, 0.5, 0.5]); u_min = -10*ones(4,1); u_max = 10*ones(4,1); % 4. 仿真循环 x = zeros(12, 1); x_history = zeros(12, N); u_history = zeros(4, N); for k = 1:N % 高度参考值，例如 1 米阶跃 z_ref = 1.0; % 控制量计算 % PID u_pid = zeros(4,1); u_pid(1) = params.m * params.g + pid_z.update(z_ref - x(3)); % LQR u_lqr = -K_lqr * (x - x_ref) + u_ref; % MPC u_mpc = mpc_control(Ad, Bd, x, x_ref, Np, Nc, Q_mpc, R_mpc, u_min, u_max); % 这里选择其中之一用于非线性模型仿真 u = u_lqr; % 切换控制器 % 使用 RK4 或 ode45 更新非线性模型 x = rk4_step(x, u, Ts, params); x_history(:, k) = x; u_history(:, k) = u; end % 5. 绘图对比 figure; subplot(3,1,1); plot(t, x_history(3,:)); title('高度响应'); subplot(3,1,2); plot(t, x_history(7,:)); title('俯仰角响应'); subplot(3,1,3); plot(t, u_history(1,:)); title('总升力控制量');这个脚本框架把三种控制器放在同一个非线性模型上做测试，所以结论更接近真实情况。实际项目里，如果需要单独对比线性模型与非线性模型的差异，可以将被控对象切换为线性模型。
6. 仿真实验设计与性能对比方法
有了一套能跑的 Matlab 代码框架，接下来的重点不是让代码跑通，而是设计出有说服力的对比实验。建议至少设计三组实验。
6.1 实验一：悬停阶跃响应
给高度通道一个阶跃参考，比如从 0 米到 1 米，同时保持姿态角参考为 0。三种控制器分别运行，记录高度响应、姿态角响应和控制量。这一步重点看：谁的超调量小、谁的调节时间短、谁的稳态误差接近零。
对比指标可以做成表格：
| 控制器 | 超调量 | 调节时间(2%) | 稳态误差 | 控制量峰值 |
|---|---|---|---|---|
| PID | 待仿真记录 | 待仿真记录 | 待仿真记录 | 待仿真记录 |
| LQR | 待仿真记录 | 待仿真记录 | 待仿真记录 | 待仿真记录 |
| MPC | 待仿真记录 | 待仿真记录 | 待仿真记录 | 待仿真记录 |
表中的数值需要自己跑仿真后填写，不要照抄其他论文的参数。
6.2 实验二：抗扰动性能
在仿真到第 5 秒时，给水平方向施加一个持续 1 秒的常值扰动力，模拟阵风。记录无人机在扰动下的位置偏移和恢复时间。这个实验的关键差异最容易暴露：PID 如果没有针对该通道做积分，会有明显稳态偏移；LQR 和 MPC 的表现取决于 Q 权重里对位置误差的惩罚强度；MPC 能在预测模型中显式看到扰动的影响，但前提是扰动可预测或者需要设计扰动观测器。
6.3 实验三：控制量约束对比
给控制量加上相同的限幅约束，比如总升力在 [0, 2mg] 范围内。这时 PID 和 LQR 如果直接输出超出限幅的控制量，会被截断，可能出现积分饱和或者响应变慢；MPC 因为优化时已经把约束写进去了，会提前规划控制轨迹，避免超限。这个实验最值得做，因为它能直观看出 MPC 的工程优势。
三组实验做完，用绘图脚本把曲线集中在一张图里对比，同时计算每次实验的性能指标，整理成表格，这就是论文里非常有说服力的结果页。
7. 结果解读与参数调优思路
7.1 PID 调参思路
PID 参数在悬停模型里不要一开始就同时调三个参数。建议先用 Ziegler-Nichols 或者手动递增 Kp，直到系统出现等幅振荡，记录振荡周期，再根据经验表给出 Ki 和 Kd。在仿真环境里没有传感器噪声，Kd 可以大一点，但在真机上 Kd 过大会放大测量噪声。
如果高度通道有稳态误差，先加 Ki。但注意 Ki 过大会导致超调和低频振荡。控制量饱和时，还需要考虑积分限幅或者积分分离，否则会积分饱和。
7.2 LQR 调参思路
LQR 的调参核心是 Q 和 R。比较高效的做法是从一个保守的 Q、R 出发，用仿真看状态响应，然后调整权重比。一般原则是：
- 如果位置误差收敛太慢，增大位置对应的 Q 权重；
- 如果控制量过大或者动作过猛，增大对应 R 权重；
- 如果姿态角波动明显，增大姿态角对应的 Q 权重。
Q、R 是相对权重，实际起作用的是两者比值，不是绝对值。另外要注意，如果 Q 矩阵中出现跨状态耦合项的设置，要结合物理意义，不要随意设置非对角项。
7.3 MPC 调参思路
MPC 的可调参数更多，包括预测时域 Np、控制时域 Nc、Q、R、约束范围。建议先固定 Q、R，调 Np 和 Nc。Np 太小会丢失远期信息，导致稳定性下降；Np 太大计算量增加，在仿真里问题不大，在实时系统里会拖慢控制周期。Nc 一般取 3 到 10，大于 Nc 之后控制增量收益会递减。
约束范围的设置要基于执行器实际能力，不要为了好看把约束设得很宽。如果 MPC 频繁无解或者求解时间超过控制周期，优先放宽控制量约束，或者把约束改成软约束。
7.4 三种控制器的结果差异预期
基于控制理论的基本结论，可以预期：
- 在无约束问题中，LQR 和 MPC 的差别不大，因为 LQR 本身就是一个无限时域的最优控制；MPC 的优势主要体现在约束场景。
- PID 在单通道独立控制下也能做到不错的效果，但耦合较强时调参会比较累。
- MPC 控制量最平缓，但计算复杂度显著高于 PID 和 LQR。
更具体的结论需要以实际仿真结果为准。跑完三组实验，把曲线和指标摆在一起，是这套项目最有价值的部分。
8. 常见问题与排查方法
| 问题现象 | 可能原因 | 排查方式 | 解决方案 |
|---|---|---|---|
| 线性化后 A、B 矩阵有 NaN | 数值差分步长选择不合理 / 平衡点设置不准确 | 检查 x0、u0 是否满足平衡条件 | 调整 eps 步长；用符号求导验证 |
| PID 控制发散 | Kp 过大 / Kd 过小 / 积分饱和 | 先关掉 KI，只调 KP，逐步增大 KD | 从保守参数开始，观察响应趋势 |
| LQR 计算结果返回 NaN | Q、R 非正定 / 系统不可控 | 检查 Q、R 是否正定；计算可控性矩阵 | 调 Q、R 为严格对角正定矩阵；确认可控性 |
| MPC 优化无解 | 约束过紧 / 预测时域设置不合理 | 查看 quadprog 返回信息 | 放宽约束或增大 Np、Nc |
| 仿真速度太慢 | MPC 每个采样点都调用优化求解器 | 查看 MPC 求解耗时 | 减少 Np、Nc；改用 MPC Toolbox 的 codegen 加速 |
| 三组实验结果差异不明显 | 控制器参数没有调到各自最优 | 检查各控制器是否已经调参 | 分别把三种控制器调到较优状态再对比 |
| 高度通道持续偏移 | 未加入积分项 / LQR 无积分项 | 查看稳态误差 | 加入积分项或者使用扩展状态模型 |
| 姿态角出现高频抖动 | 微分项滤波不够 / 采样周期不当 | 查看控制量高频振荡 | 增加滤波系数，降低 Kd |
如果在 Matlab 启动阶段就遇到工具箱缺失，比如 quadprog 不在路径中，需要检查 Optimization Toolbox 是否安装。可以用ver命令确认。lqr 函数依赖 Control System Toolbox，mpc 相关函数如果自己用 quadprog 实现，则只依赖 Optimization Toolbox。
9. 最佳实践与使用建议
第一，建议把代码拆成四个文件管理：参数初始化、模型定义、控制器定义、主仿真脚本。这样当你想换一组控制器参数时，不需要在主脚本里翻找。模型文件保持纯函数形式，方便后续复制到 Simulink 做更复杂的仿真。
第二，参数调整要有记录。控制器的每次改动，记录下 Q、R、PID 参数和对应的性能指标，做到改动可回退。这里建议建一个 params 结构体统一管理，不要散落在脚本里。
第三，实验顺序固定。先测阶跃响应，再测抗扰动，最后测约束场景。每一组实验都要把控制器的参数记录在实验结果旁边，否则隔几天再看就不知道参数来自哪次调整。
第四，合规和授权。纯 Matlab 仿真不涉及真实飞行器和空域问题，但如果后续把控制算法放到真机上验证，必须遵守当地飞行管理规定，选择合法场地，做好飞行前安全检查和紧急停止机制。代码中使用的人名、论文数据如果来自其他作者，需要保留引用来源。
第五，如果用于课程报告或者论文配图，建议把仿真图中的三条曲线用不同线型和颜色区分，并附上性能指标表格，这样比单纯贴代码更有说服力。图表的轴标签建议加上单位，统一中英文命名规范。
10. 总结与下一步
这个项目的核心价值不是“哪个控制器最好”，而是提供了一套统一建模、统一对比的完整链路。从四旋翼非线性动力学方程出发，推导悬停点线性化模型，再在同一个模型上分别设计 PID、LQR、MPC，最后用仿真实验把它们的阶跃响应、抗扰性能和约束处理能力放在同一张图里比较。这套思路本身就是一个完成度很高的控制类课设或论文工作。
建议拿到代码后，第一个要验证的功能是高度通道的阶跃响应。先跑通非线性模型和线性化脚本，再切换三种控制器看输出差异。最容易踩的坑是线性化时平衡点设置不准确，导致 A 矩阵不合理，后面所有控制器的表现都会异常。如果发现控制器效果都不好，先回头检查模型，而不是急着调参数。
接下来可以考虑的方向有三个：一是对非线性模型做串级 PID 结构，贴近真实飞控；二是在 MPC 里加入状态估计器处理不可测状态；三是把控制器迁移到 Simulink 中做基于 Pixhawk 的半物理仿真，进一步对标实机调参流程。对于正在做毕设或者课设的同学，这套代码值得作为起点，往任一个方向深入都有足够的延展空间。
