简介：面向自动控制、机器人及无人机方向的本科生与研究生，这是一份基于MATLAB/Simulink的四旋翼飞行器建模仿真综合实验项目资料，源自北京航空航天大学课程实践，重点解决六自由度（6DOF）动力学建模、姿态解算与控制器设计仿真问题。包内共26个文件，以15个m脚本和4个slx模型为核心：m脚本覆盖状态初始化、四元数/欧拉角/旋转矩阵相互转换、3D轨迹绘制与动画显示等工具链，slx模型则分别搭建定点悬停控制、三维航路跟踪、多机编队飞行（线性编队与圆形编队）三套可运行仿真环境；另有PDF实验报告、mlapp交互式应用、README说明及docx附赠资料，便于按图索骥，整体压缩包约5.3MB。目前已有81人学习下载。资源按实验一至实验三组织，结构清晰，附带完整的模型与脚本，可直接在Simulink中复现北航综合实验流程，也可帮助读者理解坐标变换、四旋翼运动学与动力学、控制器整定、编队协同等关键知识点，对课程设计、毕业设计或无人机竞赛备赛都有实用参考价值。
1. 从 6DOF 状态方程入手，让四旋翼仿真不再只是"给个阶跃响应"
拿到这份四旋翼综合实验的压缩包时，很多人的第一反应是解压后找到 .slx 文件直接双击运行，看到曲线动起来就算完成。但这类实验真正要交的东西，是你能说清楚那 12 个状态是怎么从电机转速一路积分出来的——这也是 6DOF 建模和"随便拉个传递函数"之间的分界线。基于 MATLAB 和 Simulink 的四旋翼仿真，数学上并不复杂，真正的复杂度来自坐标变换的次序和控制器与模型之间的信号契约。标题里列出的定点悬停、三维航路跟踪、多机编队，本质上是同一个模型在不同参考输入下的表现：悬停是目标点不变，航路是目标点随时间滑动，编队是目标点换成"领航者位置加偏置"。适合正在做毕设、准备课程综合实验、以及想把 Simulink 里的控制律搬到真实机架上的工程师。读完这篇文章，你能把一个只有 PID 模块的演示模型，改造成自己可以改参数、换控制器的完整仿真环境。
2. 6DOF 动力学建模：力与力矩怎么写进 Simulink
2.1 参考系与姿态描述：为什么悬停模型里 ZYX 欧拉角最顺手
四旋翼的 6DOF 指机体在空间中的六个自由度：位置 x、y、z 和姿态 φ、θ、ψ。姿态用什么参数化，直接决定后面 Simulink 里被积分的量是什么。我一般会先在纸面用 ZYX 欧拉角把旋转矩阵写完整，不跳过这一步；很多仿真发散都是因为姿态更新用了小角度近似，却在控制器里给了大角度参考。
旋转矩阵把机体坐标系下的矢量转换到惯性系，写法如下，其中 c 代表 cos，s 代表 sin：
$$ R_B^E = \begin{bmatrix} c_\theta c_\psi & s_\phi s_\theta c_\psi - c_\phi s_\psi & c_\phi s_\theta c_\psi + s_\phi s_\psi \ c_\theta s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & c_\phi s_\theta s_\psi - s_\phi c_\psi \ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{bmatrix} $$
这个矩阵的每一列可以读作"对应机体轴在惯性系下的投影"。后面所有需要把升力方向从机体换到惯性系的环节，都要用到它。ZYX 欧拉角在俯仰角接近 ±90° 时出现奇异，悬停和常规航路跟踪不会飞这种姿态，所以这套表示足够；只有做全姿态翻转之类的机动，才需要换四元数。
2.2 平动方程与转动方程：升力、反扭矩与角速度耦合
四旋翼的合外力只有重力和四个旋翼产生的升力。定义机体轴总升力 T = kΣωᵢ²，水平运动靠倾斜机体让升力产生水平分量，所以力的方程一定写为：
$$ m \dot{\mathbf{v}}^E = R_B^E \begin{bmatrix}0\0\T\end{bmatrix} - m g \mathbf{e}_z $$
转动方程则要考虑机体轴角速度 ω = [p q r]ᵀ：
$$ I \dot{\omega} = \mathbf{M} - \omega \times (I \omega) $$
这里的 M 是滚转、俯仰、偏航三个通道的控制力矩。四旋翼的偏航力矩来自两组正反桨的差速，所以建模时应该把 T 和 M 分开作为动力学模块的输入，而不是直接把四个电机转速接进去。这样后续把 PID 输出换成滑模控制器或者模型预测控制器的输出时，动力学部分一行都不用改。
方程里的ω × Iω是陀螺耦合项。悬停转速下它很小，但做大偏航率航路时会明显影响响应；不少简化模型省掉这一项后，偏航加转速度的仿真相较真实情况会显得"过于稳定"。保留它，后面做编队转弯时能少踩一个坑。
2.3 用 MATLAB Function 封装刚体动力学：可直接抄的最小 6DOF 模型
在 Simulink 里搭刚体动力学，常见做法不是把十几个 Gain 和积分器用线连起来，而是写一个 MATLAB Function 块，把 12 维状态作为输入、返回状态导数，再接给积分器。代码要点是旋转矩阵和欧拉角速率转换各出现一次，其他地方只做代数运算。
function sDot = quad6DofDynamics(s, T, M, p) % 四旋翼 6DOF 刚体动力学状态导数 % s = [x y z vx vy vz phi theta psi p q r]' 12 维状态 % T = 总升力 (N)，M = 机体轴力矩 [Mx My Mz] (N*m) % p = 参数结构体，包含 m、Ixx、Iyy、Izz、g v = s(4:6); eul = s(7:9); % [phi theta psi] ZYX 顺序 om = s(10:12); % 机体轴角速度 [p q r] phi = eul(1); theta = eul(2); psi = eul(3); sP = sin(phi); cP = cos(phi); sT = sin(theta); cT = cos(theta); sY = sin(psi); cY = cos(psi); % 旋转矩阵，把机体系矢量换到惯性系 R_EB = [cT*cY, sP*sT*cY-cP*sY, cP*sT*cY+sP*sY; cT*sY, sP*sT*sY+cP*cY, cP*sT*sY-sP*cY; -sT, sP*cT, cP*cT]; % 力的方程：升力换到惯性系后与重力相加 aE = R_EB * [0; 0; T] / p.m - [0; 0; p.g]; % 转动方程：I 为对角阵，注意陀螺耦合项 I = diag([p.Ixx, p.Iyy, p.Izz]); omDot = I \ (M - cross(om, I * om)); % 欧拉角速率转换矩阵（ZYX），俯仰角接近 90° 时奇异 Einv = [1, sP*tT, cP*tT; 0, cP, -sP; 0, sP/cT, cP/cT]; eulDot = Einv * om; sDot = [v; aE; eulDot; omDot]; end解释几个关键点。omDot里的cross(om, I*om)就是 2.2 节说的陀螺耦合项，小姿态工况下去掉它看不出差别，但编队飞行中做快速偏航转向时，少了它会让模型少一个真实的角速度阻尼作用。Einv第三行带着1/cT，这就是俯仰角接近 90° 时姿态积分发散的原因，所以实验里的参考轨迹要避开垂直方向的大俯仰机动。
把sDot积分出来需要 12 个积分器，在 Simulink 里可以摆一组 Integrator 模块接 Demux，也可以直接用状态空间形式的积分器组。注意 MATLAB Function 块里不要写离散递推逻辑，否则后面换固定步长求解器导出代码时，会和你手写的离散积分混在一起，产生双倍积分误差。
2.4 模型参数表与仿真步长：开环曲线先不要指望它稳
搭建完成后，先用一个常数升力 T = m*g 的悬停配平值去跑开环。电机动力学用一阶惯性环节近似，给一组能起飞的默认参数。常见的小型实验四旋翼参数可以按下面的表设置：
| 符号 | 含义 | 常见取值 | 说明 |
|---|---|---|---|
| m | 整机质量 | 1.2 kg | 决定升力配平值的大小 |
| Ixx / Iyy | 滚转/俯仰惯量 | 0.015 kg·m² | 通道响应速度的关键 |
| Izz | 偏航惯量 | 0.024 kg·m² | 通常比横滚轴大 |
| g | 重力加速度 | 9.8 m/s² | 配平和姿态解算都要用它 |
| k | 拉力系数 | 1e-5 N·s²/rad² | 旋翼转速平方得到升力 |
| b | 反扭矩系数 | 0.2 * k | 偏航通道配平与姿态耦合 |
| τ_motor | 电机一阶时间常数 | 0.02 s | 比姿态环响应快一个量级即可 |
步长设置我一般先统一用固定步长 1 ms，对应 1000 Hz 控制频率，求解器选 ode4。这个配置在 6DOF 连续模型和后续 C 代码生成之间不会出现行为跳变；变步长 ode45 在分钟级长航路仿真里能省时间，但步长突变会掩盖控制器某些通道的高频抖动。开环仿真时飞机会立刻倒扣，这属于正常现象，标志是俯仰和滚转角速度先发散、位置还没来得及变化——这正是验证模型里欧拉角更新方向是否正确的简单办法。
3. 定点悬停的串级 PID：内环外环时间尺度分离与参数落点
3.1 为什么悬停控制器要拆两层：位置环的带宽容不下姿态振荡
定点悬停看起来只是让 x、y、z 保持常数，但水平方向根本没有直接执行器：要让飞机回到目标点上方，必须先让机体倾斜，等加速度出现后再回平。这个"倾斜产生水平加速度"的物理过程决定了控制必然分两层：外环位置环算期望加速度，内环姿态环去产生那个倾斜角。
内环和外环的时间尺度分离原则是这套控制器能工作的前提。姿态环闭环带宽要做到位置环的 5 到 10 倍，姿态响应相对位置运动来说就是瞬时的；反过来如果两层带宽接近，两个 PID 会在回路里互相激励，表现出来是悬停点附近持续的小幅振荡，频率低于姿态环单独调出来的振荡频率。仿真验收里最常见的失败就是内环还没调好直接整定外环，曲线看起来在动但永远收敛不到 ±10 cm。
3.2 串级 PID 的 Simulink 接线：从期望位置到期望力矩的完整链路
3.2.1 位置环：算期望加速度，再转成期望姿态角
位置环的输入是目标位置和飞机当前位置、速度，输出是期望加速度。悬停工况下目标速度为 0，控制律写成：
$$ a_d = K_{p,pos}(r_d - r) + K_{d,pos}(0 - v) $$
z 通道单独加上重力配平项 m*g。得到惯性系 a_x、a_y 后，用 ZYX 欧拉角反解期望姿态角，小角度假设下可以直接用：
$$ \phi_d = \frac{a_x \sin\psi - a_y \cos\psi}{g}, \quad \theta_d = \frac{a_x \cos\psi + a_y \sin\psi}{g} $$
这里的 ψ 取当前偏航角。因为位置环 P 控制器给出的 a_x、a_y 通常小于 2 m/s²，小角度近似造成的误差在悬停指标内可以忽略；如果做大过载机动，就要换成 atan2 形式的完整反解。
3.2.2 姿态环：滚转俯仰用 PD，偏航用普通 PID
姿态环输入是期望姿态角，输出是力矩。滚转通道的常用结构是比例加角速度阻尼：
$$ M_x = K_{p,\phi}(\phi_d - \phi) - K_{d,\phi} p $$
角速度反馈项是负号，因为阻尼要抵消速率本身，而不是角度误差的导数。三个通道分别接饱和模块，限制最大力矩，避免仿真里出现几十牛米的控制量——那说明误差已经大到超出模型有效范围了。
Simulink 里这一步最省事的做法是摆三个 PID Controller 模块，控制周期填 1 ms 与模型一致，输出接到 2.3 节那个动力学模块的 M 端。积分项输出要做抗饱和，否则电机进入饱和区后积分还在涨，恢复时会出现明显超调。姿态环理想情况下应当只用 PD 就能达到悬停精度，积分项是给常值风扰留的余量。
3.3 参数整定的顺序与一组可用的初始值
仿真里调 PID 的顺序和实物调试一致：先只调内环 Kp，找到临界增益再调 Kd 和 Ki。具体做法是把外环位置控制设为开环，直接把期望姿态给常数，姿态角参考给一个小阶跃，比如 θ_d = 5°。Kp 从 0.5 开始每次增加一倍，记录第一次出现等幅振荡时的临界增益和振荡周期，再用 Ziegler-Nichols 经验公式得到初值，最后靠 Kd 把超调压到 5% 以内。
下表是对 2.4 节质量惯量参数下能直接跑起来的基准值。如果换成 0.5 kg 的小机架，Kp 普遍要往下缩 30% 左右：
| 控制环 | Kp | Ki | Kd | 输出限幅 |
|---|---|---|---|---|
| 姿态角环（φ/θ） | 6.0 | 2.0 | 1.2 | ±0.8 N·m |
| 姿态角环（ψ） | 3.0 | 1.0 | 0.8 | ±0.4 N·m |
| 位置环（x/y） | 1.5 | 0.2 | 0.8 | 期望姿态限 ±15° |
| 位置环（z） | 2.0 | 0.3 | 1.0 | 推力增量限 ±4 N |
整定时还要同时看两个带宽：姿态环闭环穿越频率落到 2 到 4 Hz，位置环 0.5 到 1 Hz。Simulink 可以对模型做线性化，但更快的办法是在悬停平衡点给位置一个 0.1 m 的窄脉冲扰动，看状态恢复曲线：姿态环超调且振荡超过两次，说明 Kd 不够；位置环跟着姿态一起抖，说明外环带宽压得太高。
3.4 悬停验收：观测哪些量才算"真的悬停住"
验收不能只看最终位置误差。我习惯把三项指标同时打出来：稳态位置误差 ±10 cm，姿态角稳态误差 ±2°，以及从扰动开始到进入误差带的恢复时间。综合实验里的验收也基本对应这三项。
另一个必须观测的量是电机转速指令。如果四个转速指令里有两个持续贴在上限，模型参数一定有问题，通常是 m 与升力配平不对，而不是 PID 增益不够。把电机转速、期望姿态角、实际姿态角都接入记录模块，后面航路跟踪和编队出现异常时，看这几个信号的分叉点比看位置误差图直接得多。
4. 三维航路跟踪与多机编队：轨迹前馈和领航-跟随误差回路的 Simulink 实现
4.1 三维航路怎么变成参考输入：时间参数化的三次样条
定点悬停只给了常值参考，航路跟踪则要让每个时刻都有独立的期望位置、期望速度和期望加速度。最省事的做法是把航点写成一个 MATLAB 脚本，用interp1做插值生成时间表：
t_wp = [0, 5, 10, 15]; % 航点时间 wpts = [0 0 0; 5 3 1; 8 -2 3; 0 0 0]; % 每行一个 [x y z] t = (0:0.01:15)'; posRef = interp1(t_wp, wpts, t, 'pchip'); velRef = zeros(size(posRef)); accRef = zeros(size(posRef)); for i = 2:size(t,1)-1 dt = t(i+1) - t(i-1); velRef(i,:) = (posRef(i+1,:) - posRef(i-1,:)) / dt; accRef(i,:) = (velRef(i+1,:) - velRef(i-1,:)) / dt; end refTable = timetable(seconds(t), ... posRef(:,1), posRef(:,2), posRef(:,3), ... velRef(:,1), velRef(:,2), velRef(:,3), ... accRef(:,1), accRef(:,2), accRef(:,3), ... 'VariableNames', {'x','y','z','vx','vy','vz', ... 'ax','ay','az'});这段脚本做了三件事：用 pchip 插值避免直线航点间出现过冲；用中心差分求速度和加速度；把结果打包成timetable，供 Simulink 的 From Workspace 模块直接读取。pchip 相比 spline 的优点是航段之间不会在靠近航点时出现明显超调，这对"沿直线飞"类的航路更友好；如果希望轨迹尽量圆滑，可以换spline，代价是可能飞出航段包络。
在 Simulink 里打开 From Workspace 模块后，注意采样时间和插值方式。对timetable类型，模块默认按表格时间做线性插值，不需要额外处理。但输出端口顺序要和总线定义一致，我常常在这里把 x 和 vx 接反，导致位置环速度反馈变成正的，整条轨迹直接发散。
4.2 航路跟踪的位置环：把前馈加速度加进去
定点悬停里的 PD 位置环直接换到跟踪场景，会出现一个明显问题：转弯时跟踪误差方向总是滞后于航路方向，因为 P 项只有在产生误差之后才有输出。解决办法是把参考加速度作为前馈叠加到控制律里：
$$ a_d = K_{p,pos}(r_d - r) + K_{d,pos}(v_d - v) + a_{ref} $$
在 Simulink 里，就是从 4.1 节的时间表把 ax、ay、az 接进期望加速度计算模块，与反馈项相加后再过期望姿态角解算。前馈加对了的判别标准：航路跟踪最大横向误差如果能从 0.3 m 量级降到 0.1 m 以内，说明前馈节点接对了；如果误差反而振荡，先检查 accRef 是否含噪声。中心差分在 0.01 s 间隔下对平滑轨迹产生的噪声很小，但如果是实测轨迹，必须先用滤波函数处理。
4.3 领航-跟随编队：误差定义是这份实验里最容易错的地方
多机编队最常用的实验实现是领航-跟随。领航者按 4.1 节的航路飞行，跟随者的期望位置等于领航者当前位置加上期望机间偏移。误差不再是与固定目标点的差，而是：
$$ e = (r_f - r_l) - d_{ref} $$
其中 r_f 是跟随者位置，r_l 是领航者位置，d_ref 是编队向量。这里的坑在于 d_ref 定义在哪个坐标系。如果把 d_ref 定义为惯性系常数，比如前方 2 m、右侧 1 m，那么领航者掉头时整个队形跟着整体平移，这在仿真验收里最简单；如果要求队形方向随着领航者航向旋转，那么 d_ref 要先乘领航者的偏航旋转矩阵，多一次坐标变换，很多模型在这里把某个轴的方向弄反，表现是两机在转弯时距离先变小后拉开。
编队控制器可以复用 3.2 节位置环的结构，只是把误差来源从固定目标点换成上面的 e。在三机编队的 Simulink 模型里，我一般把三套控制器画在同一模型的三个子系统中，而不是三个独立文件。领航者状态打包成总线信号，跟随者一侧用 Selector 模块取出总线里第 1 到 3 维作为位置输入。
% 在跟随者控制器的 MATLAB Function 块里读总线数据 % busIn 按 [pos_l(3), vel_l(3), eul_l(3)] 顺序打包 posL = busIn(1:3); velL = busIn(4:6); posRef = posL + formationOffset'; % formationOffset = [2; 1; 0]这段代码只处理了位置参考。跟随者的姿态环和单机悬停完全一致。编队中的稳定性和单机一致，唯一要额外注意的是领航者和跟随者如果没有同时启动，初始间距和目标编队向量不匹配，会让控制器在一开始就输出大姿态指令。所有飞机的起始位置要在初始化脚本里按相对位置摆放，而不是都放在原点后让控制器自己拉回队形。
4.4 同一模型下跑多机的三种做法与选型
| 做法 | 对模型结构的要求 | 适合场景 |
|---|---|---|
| 复制三份控制器子系统 | 状态独立但参数共用同一结构体 | 三架同机型、仿真时长较短 |
| Subsystem Reference 复用控制器 | 控制器存为独立引用模块 | 控制器改动频繁、需要版本管理 |
| 两个模型加 parsim 并行仿真 | 领航者和三机模型分开 | 做扰动对比、批量参数扫描 |
复制子系统最简单，但要注意控制器内部的积分器状态必须独立，不能把三架飞机的姿态积分器连到同一个状态端口。Subsystem Reference 是中等规模方案的常见选择，代价是模型引用链不能出现循环依赖。如果做 5 架以上的编队，更建议把个体模型封装成同一个底层函数，用数组信号总线接入，这时需要按索引从数组中取出某架飞机状态，Selector 模块的索引配置要跟打包顺序严格对应，否则飞机会突然跳到另一架飞机的位置。
编队的避碰可以先不写复杂逻辑，用"间距小于阈值时临时退出编队模式回到悬停"的简单状态机处理。综合实验一般不会测到快速汇聚场景，但答辩时被问到防碰撞设计，需要能讲出这条保护链的触发条件和恢复策略。
5. 把实验包移植到自己的四旋翼：参数、总线与换控制器的三个可复用技巧
5.1 做一个单一参数入口，比改模块里的数字靠谱
把 2.4 节参数表里的 m、惯量、电机时间常数统一做成p结构体，在 MATLAB 脚本里定义，模型顶层参数引用它。模型内部不写任何数字常量，换机型时只动这一个脚本。方法是在模型资源管理器里把 MATLAB Function 块的参数端口绑定到p，而不是在每个 Gain 模块里填 0.015。这样还能用set_param写批处理脚本，一次跑出不同惯量下的响应，不需要进界面改参数。
模型内部的积分器初值也要从这个结构体读取。比如从p.pos0读取第 i 架飞机的初始位置，而不是在积分器对话框里手填 0。旧版本 MATLAB 直接打开新模型时，Model Advisor 会报连续 PID 被替换成离散块的警告，积分器初值配置也常常丢，这些都是打开实验包后最先要检查的地方。
5.2 用 Bus 代替到处飞线的 Goto 和 From
超过三个子系统的模型再用 Goto 和 From，排错阶段几乎无法跟踪信号。做法是先定义逻辑上完整的那几个组：状态总线（12 个状态）、参考总线（位置、速度、加速度、偏航角）、控制总线（升力、力矩、电机转速）。在 MATLAB 里用Simulink.Bus.createObject从结构体生成 Bus 类型，然后在总线模块里把类型关联过去。这样输出端口改名时不会断线，代码生成也能得到对应的 C 结构体字段，后面做硬件在环或者代码导出对接时省时间。
5.3 PID 换滑模或自适应控制器时，保持动力学接口不变
最后一个技巧是把动力学模块当成独立网关，所有控制器都只往 T 和 M 两端送信号。PID 换成滑模控制器、LQR 或者模型预测控制，动力学模块不动。Simulink 模型可以生成 C 代码，也可以把控制模型导出为 FMU 供其他工具联调；这些流程的共同前置要求，就是控制器和模型之间的端口契约足够干净。
把 5.1 节的参数脚本存成initParams.m，以后每换一架飞机只需要改这三个惯量值和机体质量，模型里的数字一个都不用动。
本文还有配套的精品资源，点击获取
