很多刚开始接触四旋翼无人机的朋友，第一反应都是找个Matlab仿真跑一跑。搜一圈下来，PID控制、串级控制、Simulink模型满天飞，但真正能看懂、能自己改参数、能复现整个闭环过程的完整代码其实不多。更常见的情况是：模型文件一大堆，参数脚本上百行，PID藏在某个S-function里，点一下运行倒是能出图，可是每条曲线代表什么、为什么会振荡、参数改了有没有用，完全是一头雾水。
这篇文章打算把整个流程拆开，从最底层讲起。我会用纯Matlab脚本加一个动力学函数，不依赖Simulink，搭一个完整的四旋翼无人机PID控制仿真。代码会完整贴在正文里，每一段都会解释在干什么、为什么这么写。物理模型用12维状态方程描述，控制结构按悬停场景设计成串级PID：外环跟踪姿态角，内环跟踪角速度，高度环独立闭环。仿真目标很简单——初始时刻给无人机一个姿态扰动，让它通过PID控制自动恢复悬停，并完成高度爬升到期望值。
无论你是做毕业设计、准备电赛，还是想为后面玩PX4、ArduPilot这类开源飞控打基础，这套代码都可以作为你第一次亲手搭起来的完整闭环。它不花哨，但每个环节都能讲出道理，这比跑通一个别人写好的复杂工程有用得多。
1. 仿真不是Makeup：先想清楚控制回路里谁管谁
1.1 为什么"找个现成模型直接跑"对新手最坑
我见过太多这种场景：从开源社区下载一个四旋翼仿真工程，里面有十几二十个.m文件，跑之前还要配置路径、加载参数脚本、初始化Simulink模型。好不容易跑通了，改一个PID参数，曲线完全没反应，或者直接发散到NaN，然后就没有然后了。
问题不在于代码写得不好，而在于你跳过了"建模"和"控制器设计"这两步最关键的学习过程。你没有亲手定义过状态量，就不知道仿真结果里哪些量是可信的；你没有亲手推过模型方程，就不知道某些简化假设会带来什么后果。最后论文里能写的只是"采用了PID控制方法进行仿真验证"，问细节就露馅。
所以我强烈建议新手从最简版本开始搭。所谓"最简"，不是功能少，而是每一行代码你都看得懂、都能解释清楚。等这个基础闭环跑通，再去研究更复杂的工程化模型也不迟。
1.2 从外到内理一遍：位置、姿态、角速度、电机
四旋翼的控制回路，本质上是一条"误差逐级消除"的链条。外环的期望值是位置和航向，但四旋翼这个执行机构有个特点：它不能直接产生水平方向的作用力，只能靠倾斜机体来获得水平分量。所以要改变水平位置，必须先改变姿态角。
这条因果链是这样的：
- 期望位置 与 实际位置 的偏差，换算成期望姿态角；
- 期望姿态角 与 实际姿态角 的偏差，换算成期望角速度；
- 期望角速度 与 实际角速度 的偏差，换算成期望力矩；
- 期望力矩 加上 期望升力，通过混控器变成四个电机的拉力，最终由电机转速变化实现。
理解这条链是今天所有内容的地基。很多人调PID越调越乱，就是因为把外环的误差当成内环的输入去用，或者反过来把内环的误差直接作用到电机上，回路层次一乱，参数就永远调不好。
1.3 本文仿真的边界：悬停工况下的最小完整闭环
这里先明确仿真边界，避免期待错位。本文不做航迹跟踪，不做复杂机动，不做风场干扰。只做一件事：四旋翼在某个初始姿态扰动和高度的偏差下，通过PID闭环自动回到期望悬停状态。
期望值设定为：高度10米，滚转角、俯仰角、偏航角全部为0。初始状态设为高度8米，滚转角0.3弧度，俯仰角-0.25弧度，偏航角0.1弧度，角速度和线速度都是0。这种情况下，你会看到非常直观的控制过程：无人机先偏着机身，然后逐渐摆正，同时缓缓爬升到10米高度，最后稳定悬停。
这个场景虽然简单，但完整覆盖了四旋翼仿真的所有核心要素：动力学模型、串级PID、控制分配、数值积分、调参分析。把这套逻辑跑通，后面做轨迹跟踪只是在这个基础上再加一个位置外环而已。
2. 动力学模型：一个能看懂的12维状态方程
2.1 状态向量：位置、速度、姿态、角速度
四旋翼在三维空间里的运动状态，可以用12个变量完整描述。这套状态定义几乎是行业通用的，后面你去看PX4的EKF源码、看任何一篇四旋翼动力学论文，都能对上号。
状态向量state = [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]，含义如下：
| 变量 | 含义 | 单位 |
|---|---|---|
| x, y, z | 机体质心在世界坐标系中的位置 | m |
| vx, vy, vz | 三个方向的线速度 | m/s |
| phi, theta, psi | 滚转角、俯仰角、偏航角（欧拉角） | rad |
| p, q, r | 机体坐标系下的角速度分量 | rad/s |
注意姿态角用的是欧拉角，这在悬停小角度工况下非常方便，人能直观理解。但如果后面要做大机动，欧拉角会遇到万向锁问题，那就得换四元数，这个我们在扩展部分再讲。
2.2 加速度怎么算：牛顿-欧拉方程的小角度近似
动力学方程是整个仿真的"发动机"，它告诉我们在给定控制输入下，状态量会怎么变化。
把四旋翼当成一个刚体，控制输入为总推力 u1 和三轴力矩 u2、u3、u4。总推力方向沿机体Z轴，所以把推力从机体坐标系映射到世界坐标系时，要经过姿态角的旋转。这里我直接写出完整的坐标变换关系：
ax = u1/m * (sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi)) ay = u1/m * (-sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi)) az = -g + u1/m * (cos(phi)*cos(theta))三个角加速度方程在小角度假设下可以简化为：
p_dot = u2 / Ixx q_dot = u3 / Iyy r_dot = u4 / Izz姿态角速率和角速度的关系，我直接用了最简形式phi_dot = p、theta_dot = q、psi_dot = r。严格来说，欧拉角导数和角速度之间有一个矩阵变换关系，中间还包含sin(phi)*tan(theta)这样的项，但悬停附近角度很小，这些高阶项可以忽略。
这就是"小角度近似"的含义。它牺牲了一定的精确性，换来了直观性和可读性。对新手来说，先通过这个模型把控制逻辑吃透，比一上来就怼四元数微分方程要友好得多。
2.3 控制分配：如何把油门和三轴力矩反解成四个电机推力
PID控制器算出来的是总推力和三个轴的力矩，但真正的执行机构是四个电机。从"总推力+力矩"到"四个电机拉力"的变换，在飞控领域叫混控器（Mixer），原理是把期望的合力与合力矩分配到各个电机上。
假设一个X型四旋翼，四个电机的布局为：M1在前，M2在右，M3在后，M4在左。定义电机编号沿机架顺时针排列，则控制分配公式为：
F1 = u1/4 - u3/(4*l) - u4/(4*km) F2 = u1/4 + u2/(4*l) + u4/(4*km) F3 = u1/4 + u3/(4*l) - u4/(4*km) F4 = u1/4 - u2/(4*l) + u4/(4*km)其中 l 是机臂长度，km 是偏航力矩系数。式子里的加减号反映了：俯仰力矩主要由前后两个电机的推力差产生，滚转力矩由左右两个电机的推力差产生，偏航力矩由两组反向旋转电机的转速差产生。
这里特别提醒一句：混控器公式与电机编号、旋向定义强相关。不同开源飞控的规定不一样，你在我这个仿真里看到的是这套定义，换到PX4里可能是另一套符号。但只要逻辑一致，控制效果等价。
2.4 物理参数表与选择依据
仿真里用到的物理参数，多数来自小型四旋翼的典型值，比如常用轴距450毫米左右、起飞重量1到1.5公斤的机架，可参考如下取值：
| 参数 | 数值 | 说明 |
|---|---|---|
| m | 1.2 kg | 起飞总质量 |
| g | 9.8 m/s^2 | 重力加速度 |
| Ixx | 0.03 kg*m^2 | 绕机体X轴转动惯量 |
| Iyy | 0.03 kg*m^2 | 绕机体Y轴转动惯量 |
| Izz | 0.05 kg*m^2 | 绕机体Z轴转动惯量 |
| l | 0.25 m | 电机到质心的距离 |
| km | 0.012 | 偏航力矩与推力差的换算系数 |
注意一个关键约束：悬停时总推力必须等于重力，也就是 u1 约等于 11.76 N。对应四个电机，每个电机需要输出约 2.94 N 的拉力。如果电机推力上限设得太小，仿真一开始就会因为推力饱和而爬不上去。我在代码里把单电机的推力范围设为 0.1 到 15 N，留出足够余量，同时避免仿真中推力出现负值这种物理上不可能的情况。
3. 串级PID：外环给角速度、内环给力矩，为什么不能只用一个PID
3.1 单环PID为什么在四旋翼上行不通
如果只用一个角度环PID，把期望姿态角和实际姿态角的误差直接映射成电机力矩，会面临一个尴尬的问题：悬停在半空中的四旋翼，当机体受到扰动开始转动时，你测量到的角速度响应和角度响应是耦合在一起的。角度变化是角速度的积分，也就是说角度环PID实际上在控制一个二阶积分对象，比例增益稍微给大一点就容易振荡，给小了又回正得很慢。
更关键的是，角速度这个中间量本身存在高频扰动。比如电机振动、突风引起的瞬间力矩变化，都会首先反映在角速度上，然后才慢慢积累成角度偏差。如果只用角度环，等到角度误差大到能产生足够的控制量时，扰动已经作用了很久了。
3.2 内环快、外环慢：时间尺度分离的本质
串级PID的思路是把过程中的中间状态也闭环起来，让控制回路变成一个梯队：
- 内环（角速度环）负责把角速度控制到期望值。它直接面对高频扰动，响应快，是系统稳定性的核心。
- 外环（角度环）负责把姿态角控制到期望值。它的输出不是执行机构的指令，而是给内环的期望角速度。
- 因为内环比外环快得多，我们可以认为当外环在计算时，内环已经完全跟踪上了目标。"时间尺度分离"是串级控制能成立的前提。
这个设计和开车非常像。你不会直接打死方向盘来修正行驶路线，而是先控制车头的转动速度，再根据车头方向不断微调方向盘。内环是手，外环是眼睛，手必须比眼睛反应快。
3.3 三通道统一套路与积分限幅
滚转、俯仰、偏航三个通道，都可以套用同一个套路：
角度误差 -> 角度环PID -> 期望角速度 -> 角速度误差 -> 角速度环PID -> 期望力矩高度通道也类似：
高度误差 -> 高度环PID -> 期望垂向加速度 -> u1 = m*(g + az_cmd)代码里积分项都加了限幅。这是新手最容易忽略的地方。积分项的作用是消除稳态误差，但如果误差一直存在，积分项会无限累加，导致控制量长期处于饱和状态，等误差反向时系统却反应不过来，这就是"积分饱和"。限幅的意义在于，把积分项的贡献限制在一个可控范围内，物理意义是"我最多容忍积分项给你加这么多额外输出"，超过这个范围就封顶。
4. 完整代码：主脚本、动力学函数与控制分配逐段拆解
4.1 不依赖Simulink的脚本组织方式
这套代码由两个文件组成：主脚本quad_sim_main.m和动力学函数quadRotorDynamics.m。把动力学单独写成函数，是为了让主循环代码更清爽，也方便你以后替换成更复杂的模型。运行环境要求Matlab R2016b以上，不需要任何额外的工具箱，纯基础函数就能跑。
主脚本完成五件事：定义参数、定义PID增益、初始化状态、运行仿真循环、绘制曲线。动力学函数只负责一件事：输入当前状态和控制量，输出状态导数。
4.2 完整主脚本
%% 四旋翼无人机 PID 控制仿真（从零手写版） % 状态量: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r] % 控制目标: 期望高度10m，期望姿态角0，从初始扰动恢复悬停 clear; clc; close all; %% 1. 物理参数与仿真参数 param.m = 1.2; % 质量 kg param.g = 9.8; % 重力加速度 m/s^2 param.Ixx = 0.03; % X轴转动惯量 kg*m^2 param.Iyy = 0.03; % Y轴转动惯量 kg*m^2 param.Izz = 0.05; % Z轴转动惯量 kg*m^2 param.l = 0.25; % 机臂长度 m param.km = 0.012; % 偏航力矩系数 param.u1_min = 4; % 总推力下限 N param.u1_max = 18; % 总推力上限 N param.u2_max = 2; % 横滚/俯仰力矩限幅 N*m param.u4_max = 0.5; % 偏航力矩限幅 N*m dt = 0.01; % 仿真步长 s T = 15; % 仿真时间 s N = round(T / dt); % 总步数 %% 2. 期望值 desired.z = 10; desired.phi = 0; desired.theta = 0; desired.psi = 0; %% 3. PID 参数 % 高度环（外环产生期望垂向加速度） ctrl.Kp_z = 8; ctrl.Ki_z = 1; ctrl.Kd_z = 5; % 滚转角环（外环产生期望角速度 p_des） ctrl.Kp_phi = 5; ctrl.Ki_phi = 0.5; ctrl.Kd_phi = 0.3; % 滚转角速度环（内环产生力矩 u2） ctrl.Kp_p = 15; ctrl.Ki_p = 3; ctrl.Kd_p = 0.5; % 俯仰角环与角速度环 ctrl.Kp_theta = 5; ctrl.Ki_theta = 0.5; ctrl.Kd_theta = 0.3; ctrl.Kp_q = 15; ctrl.Ki_q = 3; ctrl.Kd_q = 0.5; % 偏航角环与角速度环 ctrl.Kp_psi = 4; ctrl.Ki_psi = 0.5; ctrl.Kd_psi = 0.2; ctrl.Kp_r = 10; ctrl.Ki_r = 2; ctrl.Kd_r = 0.4; %% 4. 初始状态 state = [0; 0; 8; 0; 0; 0; 0.3; -0.25; 0.1; 0; 0; 0]; %% 5. 存储数据 t_arr = zeros(1, N + 1); state_arr = zeros(12, N + 1); u_arr = zeros(4, N + 1); F_arr = zeros(4, N + 1); state_arr(:, 1) = state; t_arr(1) = 0; % PID 积分与微分变量初始化 err_z_i = 0; err_z_prev = 0; err_phi_i = 0; err_phi_prev = 0; err_p_i = 0; err_p_prev = 0; err_theta_i = 0; err_theta_prev = 0; err_q_i = 0; err_q_prev = 0; err_psi_i = 0; err_psi_prev = 0; err_r_i = 0; err_r_prev = 0; %% 6. 主仿真循环 for k = 1:N t = k * dt; x = state(1); y = state(2); z = state(3); vx = state(4); vy = state(5); vz = state(6); phi = state(7); theta = state(8); psi = state(9); p = state(10); q = state(11); r = state(12); % ------------------- 高度控制 ------------------- err_z = desired.z - z; err_z_i = err_z_i + err_z * dt; err_z_i = max(-2, min(2, err_z_i)); % 积分限幅 err_z_d = (err_z - err_z_prev) / dt; az_cmd = ctrl.Kp_z * err_z + ctrl.Ki_z * err_z_i + ctrl.Kd_z * err_z_d; u1 = param.m * (param.g + az_cmd); u1 = max(param.u1_min, min(param.u1_max, u1)); err_z_prev = err_z; % ------------------- 滚转控制：phi -> p -> 力矩 ------------------- err_phi = desired.phi - phi; err_phi_i = err_phi_i + err_phi * dt; err_phi_i = max(-0.5, min(0.5, err_phi_i)); err_phi_d = (err_phi - err_phi_prev) / dt; p_des = ctrl.Kp_phi * err_phi + ctrl.Ki_phi * err_phi_i ... + ctrl.Kd_phi * err_phi_d; p_des = max(-1, min(1, p_des)); % 期望角速度限幅 err_p = p_des - p; err_p_i = err_p_i + err_p * dt; err_p_i = max(-2, min(2, err_p_i)); err_p_d = (err_p - err_p_prev) / dt; u2 = ctrl.Kp_p * err_p + ctrl.Ki_p * err_p_i + ctrl.Kd_p * err_p_d; u2 = max(-param.u2_max, min(param.u2_max, u2)); err_phi_prev = err_phi; err_p_prev = err_p; % ------------------- 俯仰控制：theta -> q -> 力矩 ------------------- err_theta = desired.theta - theta; err_theta_i = err_theta_i + err_theta * dt; err_theta_i = max(-0.5, min(0.5, err_theta_i)); err_theta_d = (err_theta - err_theta_prev) / dt; q_des = ctrl.Kp_theta * err_theta + ctrl.Ki_theta * err_theta_i ... + ctrl.Kd_theta * err_theta_d; q_des = max(-1, min(1, q_des)); err_q = q_des - q; err_q_i = err_q_i + err_q * dt; err_q_i = max(-2, min(2, err_q_i)); err_q_d = (err_q - err_q_prev) / dt; u3 = ctrl.Kp_q * err_q + ctrl.Ki_q * err_q_i + ctrl.Kd_q * err_q_d; u3 = max(-param.u2_max, min(param.u2_max, u3)); err_theta_prev = err_theta; err_q_prev = err_q; % ------------------- 偏航控制：psi -> r -> 力矩 ------------------- err_psi = desired.psi - psi; err_psi_i = err_psi_i + err_psi * dt; err_psi_i = max(-0.5, min(0.5, err_psi_i)); err_psi_d = (err_psi - err_psi_prev) / dt; r_des = ctrl.Kp_psi * err_psi + ctrl.Ki_psi * err_psi_i ... + ctrl.Kd_psi * err_psi_d; r_des = max(-1, min(1, r_des)); err_r = r_des - r; err_r_i = err_r_i + err_r * dt; err_r_i = max(-2, min(2, err_r_i)); err_r_d = (err_r - err_r_prev) / dt; u4 = ctrl.Kp_r * err_r + ctrl.Ki_r * err_r_i + ctrl.Kd_r * err_r_d; u4 = max(-param.u4_max, min(param.u4_max, u4)); err_psi_prev = err_psi; err_r_prev = err_r; % ------------------- 控制分配：反解四个电机推力 ------------------- F1 = u1/4 - u3/(4*param.l) - u4/(4*param.km); F2 = u1/4 + u2/(4*param.l) + u4/(4*param.km); F3 = u1/4 + u3/(4*param.l) - u4/(4*param.km); F4 = u1/4 - u2/(4*param.l) + u4/(4*param.km); F = [F1; F2; F3; F4]; F = max(0.1, min(15, F)); % 单电机推力饱和 % ------------------- 状态更新（欧拉法） ------------------- dstate = quadRotorDynamics(state, [u1; u2; u3; u4], param); state = state + dt * dstate; % ------------------- 存储 ------------------- state_arr(:, k + 1) = state; u_arr(:, k + 1) = [u1; u2; u3; u4]; F_arr(:, k + 1) = F; t_arr(k + 1) = t; end %% 7. 绘图 figure('Name', '四旋翼PID仿真结果', 'Color', 'w'); subplot(2, 2, 1); plot(t_arr, state_arr(3, :), 'b-', 'LineWidth', 1.5); hold on; plot(t_arr, desired.z * ones(size(t_arr)), 'r--', 'LineWidth', 1); xlabel('时间/s'); ylabel('高度/m'); title('高度跟踪'); legend('实际高度', '期望高度', 'Location', 'best'); grid on; subplot(2, 2, 2); plot(t_arr, state_arr(7, :) * 180/pi, 'r-', 'LineWidth', 1.2); hold on; plot(t_arr, state_arr(8, :) * 180/pi, 'g-', 'LineWidth', 1.2); plot(t_arr, state_arr(9, :) * 180/pi, 'b-', 'LineWidth', 1.2); xlabel('时间/s'); ylabel('角度/deg'); title('姿态角'); legend('滚转 phi', '俯仰 theta', '偏航 psi', 'Location', 'best'); grid on; subplot(2, 2, 3); plot(t_arr, state_arr(10, :) * 180/pi, 'r-', 'LineWidth', 1.2); hold on; plot(t_arr, state_arr(11, :) * 180/pi, 'g-', 'LineWidth', 1.2); plot(t_arr, state_arr(12, :) * 180/pi, 'b-', 'LineWidth', 1.2); xlabel('时间/s'); ylabel('角速度/deg/s'); title('机体角速度'); legend('p', 'q', 'r', 'Location', 'best'); grid on; subplot(2, 2, 4); plot(t_arr, F_arr(1, :), 'LineWidth', 1.2); hold on; plot(t_arr, F_arr(2, :), 'LineWidth', 1.2); plot(t_arr, F_arr(3, :), 'LineWidth', 1.2); plot(t_arr, F_arr(4, :), 'LineWidth', 1.2); xlabel('时间/s'); ylabel('推力/N'); title('四个电机推力'); legend('F1', 'F2', 'F3', 'F4', 'Location', 'best'); grid on;4.3 动力学子函数
新建一个文件，命名为quadRotorDynamics.m，放在和主脚本同一个目录下。
function dstate = quadRotorDynamics(state, u, param) % 四旋翼动力学：小角度近似下的12维状态导数 % u = [u1总推力, u2滚转力矩, u3俯仰力矩, u4偏航力矩] x = state(1); y = state(2); z = state(3); vx = state(4); vy = state(5); vz = state(6); phi = state(7); theta = state(8); psi = state(9); p = state(10); q = state(11); r = state(12); u1 = u(1); u2 = u(2); u3 = u(3); u4 = u(4); m = param.m; g = param.g; Ixx = param.Ixx; Iyy = param.Iyy; Izz = param.Izz; % 线性加速度（小角度近似） ax = u1/m * (sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi)); ay = u1/m * (-sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi)); az = -g + u1/m * (cos(phi)*cos(theta)); % 姿态运动学与角加速度 phi_dot = p; theta_dot = q; psi_dot = r; p_dot = u2 / Ixx; q_dot = u3 / Iyy; r_dot = u4 / Izz; dstate = [vx; vy; vz; ax; ay; az; phi_dot; theta_dot; psi_dot; p_dot; q_dot; r_dot]; end4.4 为什么不用ode45而是自己写积分循环
这里有一个很多人会问的问题：Matlab明明有强大的求解器，为什么非要用最原始的欧拉法？
原因有两层。第一，PID控制器是离散的，它需要在每一个控制周期内读取状态、计算误差、累加积分项，然后输出控制量。我用固定步长循环，天然就是一个"控制周期"的结构。如果换成ode45，整个状态更新时间步由求解器自动决定，PID的积分项积分项就不知道在哪里更新——你只能把控制器放在一个事件函数里，或者极不优雅地强行固定步长，这完全违背了使用变步长求解器的初衷。
第二，真实飞控从来不用变步长求解器。飞控是一个实时的、固定周期控制系统，主频就是控制频率，通常250到1000赫兹。我的仿真步长取0.01秒，对应100赫兹，这个频率对本文的悬停场景完全够用。用欧拉法虽然数值精度不如高阶求解器，但只要步长足够小、系统不发散，结果就是可信的。工程仿真不是越精确越好，而是满足分析需求即可。
4.5 中文乱码问题
代码和绘图里的中文标签，在Matlab老版本里可能会显示为乱码。如果出现这种情况，检查文件编码是否为UTF-8，或者把绘图标签改成英文，不影响运行。高版本Matlab里如果注释乱码，一般是因为操作系统区域设置和文件编码不一致，改成UTF-8编码保存即可解决。
5. 调参实战：初始值、发散原因、从曲线反推问题
5.1 一组能用的初始参数与运行结果预期
上面代码里给的那组PID参数，直接复制运行就能得到一个稳定的结果。运行后你会看到什么？
高度曲线大约在3到4秒内从8米爬升到10米，基本无超调或者最多轻微超调0.1米，然后稳定在10米。姿态角曲线会更快收敛，滚转角和俯仰角在1到2秒内回到0度附近，中间有一次小幅回摆，之后彻底稳定。偏航角收敛得稍微慢一点，但也在4秒内归零。四个电机推力曲线在悬停稳定后都趋近于2.94牛，这个值正好是重力平均到四个电机上的结果，验证了模型的正确性。
这个结果说明，PID参数的量级和系统的物理参数是匹配的。如果换成其他的转动惯量或质量，这套参数不一定还能稳定，这就是为什么后面要学会自己调参。
5.2 调参顺序：先内环后外环，先比例后微分
调PID参数最忌讳一上来就六个环路一起调。正确顺序是：从最内层开始，一层层往外调。
第一步调角速度环。把外环角度环的增益先设为0，也就是让期望角速度恒为0，然后给一个初始角速度扰动，观察角速度能不能快速归零。这个环节只用调内环的Kp_p和Kd_p。Kp_p决定角速度被拉回来的力度，Kd_p决定对突变响应的抑制作用。只要角速度能在0.5秒内稳定归零，内环就算合格。
第二步调角度环。把外环增益恢复，期望角度设为0，给一个初始角度扰动，观察角度回正过程。这时候如果角度曲线出现明显的超调和回摆，说明外环的Kp_phi偏大；如果回正很慢，说明Kp_phi偏小，适当加Kd_phi可以加快收敛并抑制超调。
第三步调高度环，调法和角度环类似。高度环属于最外层，响应最慢，给它足够的反应时间。
调参的口诀是"先比例、后积分、再加微分"。先把P调到临界振荡，再稍微减小一点，然后加D抑制超调，最后用I消除稳态误差。这套方法在仿真里可以直接用，因为你在电脑上能看到完整曲线。
5.3 从仿真曲线反向定位问题
很多时候系统发散或者效果差，不是参数调不好，而是不知道问题出在哪个环。这里给你几个最常见的曲线特征，以及对应的原因：
| 仿真现象 | 可能原因 | 检查优先级 |
|---|---|---|
| 角速度曲线高频剧烈振荡 | 内环Kp_p过大，或dt过大 | 最高 |
| 角速度缓慢爬坡、长时间不回零 | 内环Kp_p过小 | 最高 |
| 角度回正很慢、无超调 | 外环Kp_phi不足 | 中 |
| 角度大幅反复超调 | 外环Kp_phi过大，或内环响应太慢 | 中 |
| 高度始终有稳态误差 | 高度环Ki_z不足 | 中 |
| 仿真初期推力冲到上限然后NaN | 初始姿态角过大或u1限幅设置过宽 | 低 |
有一个非常容易忽略的问题：微分项对噪声敏感。如果你的状态量是从传感器模型里读出来的，带测量噪声，那么直接对误差做差分会把噪声放大得非常厉害。真实飞控里常用角速度反馈代替角度误差的微分量，因为角速度本身就是角度误差微分的最直接体现。本文的仿真没有加传感器噪声，所以用差分法没问题，但如果你要做更真实的仿真，建议把err_phi_d直接换成-p参与运算，效果会干净很多。
5.4 新手最容易踩的三个坑
第一个坑是单位错误。期望角度和初始角度必须用弧度，很多人赋值时顺手写了30，系统直接以为你让它转30弧度，结果当然发散。建议在代码注释里反复标注单位，养成习惯。
第二个坑是忘记加饱和。控制量必须限幅，不仅是为了符合物理约束，更是为了让仿真数值稳定。没有饱和的PID，在初期大误差下可能产生一个天大的控制量，欧拉积分一步就把状态推到不可思议的值，之后怎么调都救不回来。所以我的代码里对u1、u2、u4以及期望角速度都做了限幅，这是仿真稳定性的第一道防线。
第三个坑是积分项不设限幅。高度从8米爬到10米需要好几秒时间，期间高度误差一直存在，如果积分项无限累积，到达10米时积分项已经攒了很多"额外出力"，系统会直接冲过去形成很大的超调。我的代码里对所有积分项都加了max和min限幅，你可以试着把限幅值调大，观察超调明显增大的现象，这样对积分饱和的理解会非常深刻。
6. 从悬停到航线：这套仿真还能往哪个方向扩展
6.1 加上水平位置控制：从姿态控制到位置控制的关键一步
这篇文章的仿真只做了高度和姿态闭环，水平位置完全没有控。这也意味着初始状态里vx和vy即使为0，x和y的位置误差也不会被自动修正。真正做航迹飞行的四旋翼，必须在姿态环外面再套一个位置环，逻辑和高度环几乎一样：
- x方向误差经过位置环PID，产生一个期望滚转角（注意方向）；
- y方向误差经过位置环PID，产生一个期望俯仰角；
- 把这两个期望角度作为姿态环的输入，姿态环再继续往下走。
加上这个位置环之后，整套控制结构就变成三层串级：位置环 → 姿态环 → 角速度环。层次多了，但每个环的职责依旧清晰。你可以在这份代码的基础上，给x和y也加上PID，观察无人机能否自动回到原点，这比Simulink里拖模块更能加深理解。
6.2 模型升级：全量姿态运动学与四元数
小角度近似终究有适用范围。当无人机做大角度机动时，比如快速翻滚、大俯仰爬升，phi_dot = p这种近似就不准了，需要换成完整的欧拉角运动学方程，甚至直接用四元数避免万向锁。此外，真实四旋翼还存在陀螺力矩、电机动态响应滞后、旋翼入流效应等，这些都可以逐步加进quadRotorDynamics.m里。每加一项，你都会对四旋翼的真实行为多一分理解。
从脚本迁移到Simulink也是个自然的扩展方向。Simulink的优势在于可视化搭积木，调试方便，你可以在Simulink里保留我这里串级PID的结构，用PID Controller模块代替手写循环。但建议至少先把脚本版本吃透再迁移，否则只是从一个黑盒换到另一个黑盒。
6.3 下一步：从仿真到实物飞控
等你把Matlab仿真完全调明白，可以再去看PX4或ArduPilot这类开源飞控的代码。你会发现，它们飞行模式的底层逻辑其实就是这套串级PID，只是增加了更完整的姿态表示、更复杂的滤波和状态估计、以及庞大的参数配置系统。有了这个仿真基础，至少看到MPC_XY_P、MC_ROLL_P、MC_PITCHRATE_P这些参数时，你能立刻知道它们在哪个环路、主要影响什么。这就是仿真学习最大的回报。
我自己在带新人的时候，一直坚持一个观点：仿真能帮你学会"控制逻辑"和"调参手感"，但替代不了对真实系统的敬畏。真实飞行中还有电池电压下降、振动、风扰、GPS漂移、电机一致性等问题，仿真给不了的，就只能靠多炸机、多复盘去积累了。希望这套代码能成为你从零开始的第一块垫脚石。
