简介面向自动控制、机器人及无人机方向的本科生与研究生这是一份基于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 动力学建模力与力矩怎么写进 Simulink2.1 参考系与姿态描述为什么悬停模型里 ZYX 欧拉角最顺手四旋翼的 6DOF 指机体在空间中的六个自由度位置 x、y、z 和姿态 φ、θ、ψ。姿态用什么参数化直接决定后面 Simulink 里被积分的量是什么。我一般会先在纸面用 ZYX 欧拉角把旋转矩阵写完整不跳过这一步很多仿真发散都是因为姿态更新用了小角度近似却在控制器里给了大角度参考。旋转矩阵把机体坐标系下的矢量转换到惯性系写法如下其中 c 代表 coss 代表 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*cYsP*sY; cT*sY, sP*sT*sYcP*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% 左右控制环KpKiKd输出限幅姿态角环φ/θ6.02.01.2±0.8 N·m姿态角环ψ3.01.00.8±0.4 N·m位置环x/y1.50.20.8期望姿态限 ±15°位置环z2.00.31.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(i1) - t(i-1); velRef(i,:) (posRef(i1,:) - posRef(i-1,:)) / dt; accRef(i,:) (velRef(i1,:) - 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以后每换一架飞机只需要改这三个惯量值和机体质量模型里的数字一个都不用动。本文还有配套的精品资源点击获取 基于Simulink的四旋翼6DOF建模与串级PID控制仿真指南
简介面向自动控制、机器人及无人机方向的本科生与研究生这是一份基于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 动力学建模力与力矩怎么写进 Simulink2.1 参考系与姿态描述为什么悬停模型里 ZYX 欧拉角最顺手四旋翼的 6DOF 指机体在空间中的六个自由度位置 x、y、z 和姿态 φ、θ、ψ。姿态用什么参数化直接决定后面 Simulink 里被积分的量是什么。我一般会先在纸面用 ZYX 欧拉角把旋转矩阵写完整不跳过这一步很多仿真发散都是因为姿态更新用了小角度近似却在控制器里给了大角度参考。旋转矩阵把机体坐标系下的矢量转换到惯性系写法如下其中 c 代表 coss 代表 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*cYsP*sY; cT*sY, sP*sT*sYcP*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% 左右控制环KpKiKd输出限幅姿态角环φ/θ6.02.01.2±0.8 N·m姿态角环ψ3.01.00.8±0.4 N·m位置环x/y1.50.20.8期望姿态限 ±15°位置环z2.00.31.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(i1) - t(i-1); velRef(i,:) (posRef(i1,:) - posRef(i-1,:)) / dt; accRef(i,:) (velRef(i1,:) - 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以后每换一架飞机只需要改这三个惯量值和机体质量模型里的数字一个都不用动。本文还有配套的精品资源点击获取