简介来自北航可靠飞行控制研究组的四旋翼容灾控制Simulink仿真与半物理仿真源码属于RflySim课程容错控制教学内容同时包含论文相关代码面向从事无人机容错/降级控制算法验证与开发的科研人员、研究生、竞赛团队及高年级本科生。资源完整覆盖Model、Control、MIL、HIL等目录Model提供四旋翼模型、参数脚本与RflySim3D显示接口Control包含控制器及QGC地面站参数定义MIL与HIL分别对应软件仿真和半物理仿真流程PriviteLib还收录了若干自编函数。压缩包共142个文件、约683KB以xml配置117个、Simulink模型slx10个、MATLAB脚本m7个为主另有dll、p、c、mat等辅助文件目录结构清晰可按Model、Control、MIL、HIL、PriviteLib模块分层查阅。已有941人学习下载。借助H250模型DLL与配套参数脚本可快速复现四旋翼部分失效场景下的降级控制效果附带的源码说明对MATLAB 2020b、Pixhawk 4与PX4 1.8.2等软硬件环境有明确交代对教学演示、算法对比与工程落地均有参考价值。1. 一个旋翼停转后四旋翼怎么才能不摔机四旋翼在空中坏掉一个电机是不是只能眼睁睁看它坠机我在 RflySimDegradedControl 这套基于 MATLAB/Simulink 的四旋翼容灾控制仿真例程里反复跑过单旋翼失效场景结论是多数情况能救回来但前提是控制器早就写好容灾逻辑而不是等故障发生后临时抱佛脚。这套例程由某高校可靠飞行控制研究组发布解决的问题非常聚焦当某个旋翼因堵转、断桨或电机烧毁彻底失去推力时机体如何通过控制分配重构和旋转降级策略保持最少可控性并完成安全降落。它把故障检测、控制器切换、Simulink 仿真和半实物验证串在一条链路上特别适合正在做飞控容错算法的开发者以及想验证冗余控制逻辑的高年级研究生。对新手来说它让你在不动真实飞机的情况下把常见故障模式反复撞一遍建立故障现象与控制器行为的对应关系。对熟手来说它提供了一套可以替换自己控制器的框架关键参数全部暴露在模型和脚本里适合做算法预研和半实物验证前的桌面排查。下面先从故障为什么危险讲起再拆部署、注入、调参和踩坑。2. 容灾控制的理论底子四旋翼为什么怕丢旋翼四旋翼的稳定靠四个电机同时出力缺一个控制通道立刻少一条。理解这一点才能明白 RflySimDegradedControl 里的控制器为什么不是简单修一下 PID而是要把整个控制分配和模式逻辑都换掉。2.1 旋翼失效的本质控制通道从四个变成三个四旋翼有六个自由度却只有四个执行器本身就是一个欠驱动系统。它之所以能飞得稳是因为四个电机可以通过产生总推力和三个方向的力矩把姿态与位置运动解耦控制。健康状态下控制分配矩阵是一个 4×4 矩阵把期望的总推力和三个力矩映射到四个电机的转速指令。当一个旋翼完全停转这个矩阵的某一列变成零。数值上矩阵的秩从 4 降到 3意味着系统失去了一个独立的控制通道物理上四旋翼变成了一个不对称的三旋翼布局。这时候你会发现想同时稳住滚转、俯仰和偏航三个姿态角是做不到的因为三旋翼的力矩合成在静态配平上不可解。更直观的物理直觉是一个旋转刚体如果三个旋翼产生的净力矩不为零它一定开始转动而转动以后合力的方向会周期性变化控制器必须利用这种周期性才能维持姿态。这就是工程上常见的旋转降级思路的来源不再追求所有姿态角静止而是让机体绕某个轴以恒定角速度旋转把不可控的通道“转起来”让剩余三个电机的推力在旋转坐标系里被周期调制重新恢复对滚转和俯仰的可控性。RflySimDegradedControl 的核心策略正是围绕这种思路实现的。2.2 三种主流容灾策略重构分配、旋转降级、在线诊断容灾控制并不只有一种做法。根据故障程度不同我一般会把策略分成三档。第一档是控制分配重构适合部分失效的情况。比如某个电机因为供电波动推力掉了三成但还没有完全停转。这时不需要改变控制结构只需要在分配矩阵里把损坏电机的列缩小同时把它的任务分摊给对面和对角的电机。这种策略实现简单代价是剩余电机转速普遍提升会更快接近饱和。第二档是旋转降级适合完全失效的情况。此时静态配平无解只能让机体进入自旋模式偏航从角度控制切换为角速度控制滚转和俯仰继续用剩余的交互力矩关系来稳定。这需要在飞行状态机里增加一个降级模式并处理切换瞬间的控制量突变。第三档是故障检测与诊断它不直接产生控制量但决定了前两档策略什么时候被激活。常见做法是残差检测比较模型预测出的角加速度和 IMU 实测值残差超过阈值并持续一段时间就判定对应执行器失效。检测延迟通常要控制在 0.05 到 0.2 秒之间太慢会错过最佳修正时机太快会被噪声带偏。在仿真实操里三档策略要配合使用先诊断再决定是重构分配还是进入旋转降级。把这套逻辑在 Simulink 里跑通远比只调一个控制器有意义。2.3 分配矩阵重构脚本从 4×4 到秩 3这一步怎么跨过去理解容灾控制最好的切入方式是亲手把分配矩阵打印出来看一眼。下面是四旋翼控制分配的典型 MATLAB 写法电机布局以 X 型为例符号方向请以你自己的机架为准。% allocation_matrix_rebuild.m % 演示单电机失效对控制分配矩阵的影响 % X 型布局FL1, FR2, RR3, RL4 % % 参数定义 d 0.225; % 旋翼中心到机体中心距离单位 m kF 1.2e-5; % 推力系数单位 N/(rad/s)^2 kM 3.0e-7; % 反扭矩系数单位 N*m/(rad/s)^2 % 健康四旋翼的 4x4 分配矩阵 % 行顺序总推力、滚转力矩、俯仰力矩、偏航力矩 % 列顺序电机 1~4 M [ kF, kF, kF, kF; % 总推力 -d*kF, d*kF, d*kF, -d*kF; % 滚转力矩 d*kF, d*kF, -d*kF, -d*kF; % 俯仰力矩 -kM, kM, -kM, kM]; % 偏航力矩 % 模拟 2 号电机完全失效对应列置零 M_degraded M; M_degraded(:, 2) 0; % 查看秩的变化 fprintf(健康矩阵秩: %d\n, rank(M_degraded)); fprintf(失效后矩阵秩: %d\n, rank(M_degraded));这段代码的意义在于提前暴露系统的物理极限。当秩降为 3 时直接用伪逆求控制量仍能得到一个数学解但这个解在物理上根本无法同时满足四个控制目标。常见的处理是去掉偏航力矩通道让偏航不再跟踪角度指令只跟踪一个目标角速度把节省出的控制自由度用于维持推力和滚转俯仰力矩。参数方面最需要注意的是 kF 和 kM 的比值。它们决定了反扭矩在偏航通道里的权重如果设置偏差过大仿真里会出现偏航漂移但滚转俯仰正常的诡异现象。我会在初始化脚本里单独打印这两个系数和电机测试数据比对后再整机联调。3. 把 RflySimDegradedControl 跑起来部署方式与文件结构拿到源码包之后第一步不是急着改模型而是先把目录结构摸清。这类例程包通常有固定的组织方式看懂分层可以省掉很多查找时间。3.1 拿到手先看什么目录分层和入口脚本RflySimDegradedControl 作为一套挂接在现有仿真链路里的例程目录一般会按功能分割。常见结构大致是这样根目录放着 README 和 startup 脚本用来初始化路径models 或 Simulink 文件夹放主模型scripts 放故障注入、参数初始化和数据后处理脚本results 或 logs 放仿真输出。我拿到任何仿真包第一步是打开根目录下的启动脚本看它 addpath 了哪些子目录。很多莫名其妙报“找不到模型文件”就是因为跳过了启动脚本工作目录不在包根路径下。第二步是全局搜索“set_param”和“sim(”这些是仿真入口和参数设置的核心位置能帮助你快速定位所有可调项。一个值得注意的习惯是不要把模型文件和解算脚本拆开放到不同盘符。Simulink 模型里的回调命令依赖相对路径一旦移动根目录整套模型会变成黑匣子报错都看不懂。3.2 最快跑通一个仿真场景的最小命令用命令行启动比在 Simulink 界面里点按钮更可控也更容易记录和复现问题。下面是一段最短启动流程% 1. 进入例程根目录并加载全部子路径 cd(RflySimDegradedControl); addpath(genpath(pwd)); % 2. 打开主模型以实际包内文件名为准 open_system(RflySimDegradedControl.slx); % 3. 强制使用离散固定步长2ms 是四旋翼仿真的常见选择 set_param(RflySimDegradedControl, ... SolverType, Fixed-step, ... Solver, FixedStepDiscrete, ... FixedStep, 0.002); % 4. 一键运行 30 秒仿真 simOut sim(RflySimDegradedControl, StopTime, 30);第一行是路径准备缺失会导致模型依赖的回调找不到初始化参数第二步的固定步长设置是关键四旋翼电机动力学时间常数很小变步长很容易在过零检测处引入数值抖动后面踩坑章节还会细说最后的 simOut 变量会把仿真结果写回工作区方便后续分析。如果运行时出现 MATLAB 版本相关的模块替换提示建议先跑一遍包内自带的版本兼容检查脚本而不是直接点“Update Model”。某些老模型里的模糊逻辑或状态流模块在不同版本下的升级行为会改变故障切换时序这一点容易被忽略。3.3 与容灾控制强相关的模块清单打开主模型后不要一头扎进姿态控制器里。先按下面的模块清单把信号流拉通就知道整套模型是怎么组织和切换的了。模块类别作用故障模式下的表现故障注入模块对外部故障标志做出响应切断或衰减指定电机的指令通道置位后对应支路输出清零或限幅残差计算与观测器对比模型预测角速度和实测角速度估算故障系数输出残差变大触发故障诊断逻辑控制分配重构负责期望力与力矩到四个电机指令的映射检测故障后切换分配矩阵或改变伪逆权重旋转降级状态机监控系统状态决定是否进入自旋模式切换反馈通道偏航改为角速度控制执行器饱和模块限制电机指令范围防止出现负推力或超转故障后更容易触发上限需在分配端处理这套模块清单的逻辑顺序是故障注入在最前模拟物理层的损坏观测器在中间负责感知状态机和分配重构在最后负责响应。我一般会在 Simulink 里把这三个环节用不同颜色高亮排查问题时直接沿着颜色走比到处找模块快得多。4. 搭一个单旋翼失效场景故障注入、参数表和结果判读理论准备够了目录也摸清了接下来要真正制造一次故障。单旋翼失效场景是容灾控制最典型的测试用例也是这套 RflySimDegradedControl 例程的核心验证对象。4.1 三种故障注入方式执行器层注入最接近真实在 Simulink 里模拟旋翼失效常见有三种做法。第一种是在电机转速指令通路上加 Switch 模块当外部故障标志为 1 时直接把目标电机的指令切换成 0。这种方式最直接操作也简单适合考察控制器对转速指令突变的响应。第二种是给目标电机加一个大幅限幅模块模拟堵转或供电中断后电机无法响应指令的情况。这种注入更接近真实的桨叶卡死现象因为故障电机仍然存在只是输出被物理约束在零附近电机模型的反电动势和阻力矩会继续影响系统。第三种是把故障标志通过外部输入端口注入到观测器里让观测器认为某个执行器的效率系数下降。这种方式适合验证故障诊断算法本身。我实际仿真时更常用第二种因为它保留了故障电机对机体的阻力影响更接近真实失效尤其适合半实物阶段复现。执行器层注入还有一个好处故障点距离分配器很远能够完整体验“故障发生到诊断出来再到控制重构”的全链路延迟。注入点放在姿态环以内的做法都属于偷懒测出来的过渡过程偏乐观。下面是一段把某个电机指令在故障时刻直接切断的脚本片段% fault_inject_step.m % 在指定时刻切断 2 号电机的转速指令 t_fault 10.0; % 故障发生时刻单位 s enabled true; % 总开关 if enabled t t_fault % 故障电机指令直接置 0 u_motor(2) 0.0; end % 其余电机指令不变交给控制分配器输出 u_motor signed_saturation(u_motor, 0, 1);这段代码把故障作用在四路指令的最终输出上而不是改变控制器内部的期望力矩这样控制器看不到故障只能通过姿态反馈间接感知逼着诊断和重构模块发挥作用。signed_saturation 用于把指令限制在 0 到 1 之间防止切换瞬间出现负油门。4.2 必调的四个参数故障阈值、观测器带宽、旋转速率、分配权重四个参数决定了整套容灾控制是“救回来”还是“带着隐患坚持飞”。它们之间有明显的耦合调参顺序很重要。参数建议量级调太小的后果调太大的后果故障检测阈值归一化残差0.05~0.2噪声引起误切换控制器反复重启检测延迟增大系统长时间带病运行故障观测器带宽3~10 Hz故障系数估计滞后重构不及时高频噪声放大误判频发旋转降级目标角速度60~180 deg/s力矩调制周期太长姿态发散电机转速振荡加剧姿态耦合严重分配矩阵推力通道权重1~3 倍于力矩权重故障后掉高明显垂直速度失控滚转俯仰控制力度不足故障检测阈值要放在故障注入实验前去调。先把故障标志强制置 1观察残差信号的幅值和噪声底座阈值取两者中间偏上一般能避开大部分误报。观测器带宽要和姿态环带宽错开通常取姿态环带宽的 1.5 到 2 倍太低会把观测延迟引入控制环。旋转降级速率是四旋翼容灾里最有“玄学”味道的参数。速率太低剩余三电机产生的交互力矩在旋转周期内来不及平均滚转俯仰会发散速率太高电机响应滞后开始显现相位滞后让姿态环产生振荡。常用做法是机身转动惯量偏大的取 90 deg/s 左右轻小机型从 120 到 150 deg/s 起步。分配权重的调整体现在伪逆矩阵里下面是一段带权重的分配重构代码。% weighted_pinv_allocator.m % 带权重的控制分配失效后低权重抑制高力矩输出 A M_degraded; % 失效后的分配矩阵 W diag([1.0 1.0 1.0 1.0]); % 权重对角阵 % 推力通道权重加大避免故障后掉高过快 W(1, 1) 2.0; u W \ (A / (A / W * A)) * tau_cmd; % 限幅保护 u max(0, min(1, u));这段代码的关键在最后两行先做矩阵运算再做非对称限幅。如果省略限幅伪逆算法可能给出负指令Simulink 里的电机模型按负转速计算会产生不存在的反推力仿真结果直接失真。4.3 结果判读三条曲线加一个恢复时间指标跑完故障注入仿真我一般不看整段曲线而是把故障时刻附近的窗口拉出来只看三个信号姿态角误差、垂直高度、四个电机转速。姿态角误差能直接反映欠驱动后的稳定能力垂直高度反映推力通道有没有被过度侵占电机转速反映分配器是否已经触碰饱和边界。三者都合格才算容灾控制生效。为了快速量化结果我会在仿真结束后计算恢复时间故障发生后到姿态角误差回落到阈值以内所需的时间。% compute_recovery_time.m % 输入仿真时间、横滚角误差曲线、故障发生时刻 idx_fault find(time t_fault, 1); idx_settle find(abs(roll_err(idx_fault:end)) deg2rad(3), 1); recovery_time time(idx_settle idx_fault - 1) - t_fault; fprintf(恢复时间: %.2f s\n, recovery_time);恢复时间不是越小越好。切换太快往往意味着控制器在观测器还没完全收敛时就强行介入容易造成二次冲击。我通常会把恢复时间和切换瞬间的电机超调一起看两者兼顾才算好结果。5. 避坑与排查这套仿真最容易翻车的五个环节仿真跑起来不卡不代表结果可靠。这一章列出的五个问题是我在类似容灾控制例程上反复踩过的坑每一条都有真实的现象、原因和解决办法。5.1 仿真跑出 NaN——先查步长和电机限幅现象模型运行到 0.3 秒左右Scope 曲线突然跳到 NaN 或 Inf整个仿真报错终止。原因最常见的是求解器被设成了变步长而四旋翼动力学模型里包含高速旋转的电机和刚体姿态积分变步长在过零检测时容易漏掉关键状态导致数值发散。次常见的原因是电机指令出现负值进入电机模型后转速取平方根负数直接变成 NaN。解决把仿真步长改成离散固定步长取值 1 到 2 毫秒同时给所有电机指令加 0 到 1 的非负限幅。这一步做完90% 的 NaN 问题能消失。5.2 故障注入切了等于没切——注入点选错了层现象外部故障标志已经置 1但是姿态响应完全正常Scope 里看不到任何变化。原因故障注入模块被放在了姿态控制器的内部等于控制器先收到了一个额外扰动然后又用健康电机的输出去补偿。故障在闭环内部被直接消化掉了没有真正模拟执行器失效。解决把故障注入点移到四个电机各自的指令通路上并且让故障标志同步触发状态机切换。这样故障才不会被控制器“偷偷修掉”诊断模块才能看到真实的残差变化。5.3 半实物模式下姿态抖得像筛糠——时间基准没对齐现象纯 Simulink 仿真收敛得很干净一旦切换到半实物模式姿态曲线开始高频抖动甚至发散。原因半实物仿真中Simulink 模型和真实飞控各自维护一套时间基准。如果 Simulink 侧还在用变步长或者固定步长与飞控主频不一致观测器会看到一个持续变化的额外扰动误以为外部力矩异常。解决Simulink 求解器设为与飞控主频严格一致的离散固定步长通常取 1 毫秒或 2 毫秒。时间同步以飞控发送的数据包为准Simulink 不再自行积分时间通信异常时直接丢弃当前包不做插值补偿。5.4 切换降级模式的瞬间跳变——积分项没做无扰处理现象故障发生后控制器按预期切换到旋转降级模式但切换瞬间电机转速突然顶到上限机体姿态出现明显抖动。原因切换前姿态环的积分项里累积了针对健康状态的控制量直接沿用到退化控制器相当于叠加了一个很大的常数干扰。分配矩阵切换也是一步到位控制输出出现阶跃。解决切换时清空或按比例缩放积分项分配矩阵变化用一阶低通过渡时间常数取 0.1 到 0.3 秒。不要追求切换瞬间完全保留原控制量退化状态下的控制律本身就应该从头建立。5.5 伪逆分配算出负油门——约束条件没进分配器现象故障后某一个电机的指令变成负数执行器限幅把它钳到 0实际输出与分配器解算不一致机体出现无法收敛的单向偏差。原因普通伪逆是数学上的最小范数解不考虑电机只能正向出力的物理约束。故障后推力补偿任务压在剩余电机上解耦矩阵很容易算出负指令。解决在分配求解时显式加入非负约束使用约束最小二乘替代普通伪逆。如果不想引入额外工具箱可以在解后做加权投影把负指令按推力比例转移到同排电机上再重新验证力矩是否守恒。6. 从 Simulink 走到半实物自检清单和旋转速率整定技巧半实物仿真不是简单把 Simulink 模型下载到飞控上就行它有一个承上启下的作用承的是纯仿真里调好的算法启的是后面真机试飞。这一步做得好能省掉大量外场时间。6.1 半实物验证前的一分钟自检表每次接半实物链路之前我会强制自己按下面这张表逐项确认不跳过不将就。检查项判断标准求解器步长与飞控主频一致通常固定为 1ms 或 2ms故障注入时机故障时刻由外部指令触发和仿真时间戳无耦合执行器限幅所有电机指令范围严格控制在 0 到 1观测器启动状态起飞前即运行残差基线接近零日志记录时间戳以飞控侧数据为主Simulink 侧不独立计时这五项里故障注入时机最容易被忽略。半实物模式下如果用仿真时间触发故障会因为通讯延迟导致不同设备上看到的时间不一致后续分析故障响应曲线时很难对齐。我一般会用飞控发送的指令通道来触发故障标志。6.2 旋转降级速率怎么从 120 deg/s 继续往细里调半实物验证跑通之后值得花时间把旋转降级速率从初始值往细里调。常见做法是设置 60、90、120、150 deg/s 四档速率分别采集姿态误差的均方根值取最小的一档作为起点再微调。若发现误差曲线在某个速率附近出现明显拐点说明角速度环相位滞后开始主导这时候加控制器增益解决不了问题反而会放大噪声正确的做法是把观测器带宽同步调高再重新扫档。我第一次做这个实验时直接上了固定速度没有扫档结果高度掉了接近三米才反应过来是旋转速率和机身惯量不匹配。后来我养成一个习惯任何降级模式参数跑完仿真先看一眼分配器和电机转速曲线再决定动不动飞行器这套检查流程每次都能提前暴露问题。希望这个习惯对你也能有帮助。本文还有配套的精品资源点击获取
