结合执行器动力学的运载火箭IMU故障检测仿真

先说结论：这套仿真验证了在运载火箭俯仰控制回路里，把执行器动力学纳入参考模型，能让基于IMU观测残差的故障检测在机动段依然保持稳定阈值，明显降低动态误报率。我按标题思路在Matlab里搭了一套桌面级仿真环境，参数全部按某型模拟项目的归一化单位制处理，重点验证故障检测逻辑，而不是弹道精度。实际跑下来，真正难的不是卡尔曼滤波本身，而是把所有模型参数、故障注入方式和阈值判定统一到同一个框架里。

这篇文章会把俯仰通道模型、IMU故障建模、执行器动力学、残差生成、Matlab代码和调试经验完整展开。适合正在做飞控故障检测仿真、或者准备在姿控回路里加解析冗余设计的工程师参考；如果你刚接触基于模型的故障检测，这篇文章也能帮你理解残差方法到底解决什么问题。

1. 项目核心：为什么要把IMU故障检测放在俯仰通道做

1.1 故障检测在姿控系统里的定位

运载火箭主动段的姿态控制，本质上是一个闭环反馈问题。俯仰通道控制律根据当前姿态角和姿态角速度，计算摆动发动机或栅格舵的偏转指令，执行器响应后产生控制力矩，再通过刚体动力学改变姿态，最后由IMU把姿态反馈回控制器。这个回路里任何一个环节出问题，都会直接影响飞行稳定。传统做法是依赖硬件冗余，比如多个惯性器件表决，但硬件冗余也有两个现实问题：一是成本和质量代价高，二是相同设计缺陷可能同时影响多个器件。所以近几年工程上更重视解析冗余——用系统模型去检验测量信号的一致性，IMU故障检测就是典型场景。

IMU放在箭体上，测量的是角速度和比力，姿态解算后作为俯仰角反馈。它的故障模式大多是慢变的，比如陀螺漂移缓慢增大、常值偏置、噪声方差变大，这类故障在短时间任务里不容易被操作手的肉眼看出，但会影响制导精度甚至闭环稳定裕度。故障检测的目的不是替代硬件冗余，而是在故障发生初期就给出报警，为后续的隔离和重构争取时间。

1.2 基于残差的解析冗余原理

残差检测的基本思想很朴素：建立系统模型，根据控制输入预测传感器应该看到什么，拿预测值和真实测量值做差，这个差值就叫残差。系统正常情况下，残差只在噪声水平附近波动；传感器出故障时，测量值偏离模型预测，残差会出现明显变化。

用一个生活类比：你约了朋友到某个地标见面，你知道他正常步行速度大约多长时间能到。你每隔一分钟看他手机定位，如果定位突然出现在几条街外，你不需要问他也能判断“这个定位不对劲”。模型就相当于你对朋友速度的估计，定位就是传感器测量，偏差就是残差。飞行器上也是如此，残差包含了模型误差、测量噪声和故障信息。我们需要做的，是设计一种方法把故障信息从噪声里捞出来。

在实现上，卡尔曼滤波器天然适合做残差生成器。它通过状态预测和实测更新之间的新息序列，不断估计当前状态，同时给出一步预测误差。新息就是残差，而且它的协方差还可以用来做白化处理。这也是为什么很多故障检测方案都围绕Kalman滤波来设计，而不是直接对传感器原始信号做阈值判断——因为飞行器姿态本身在变化，在时域里直接给姿态角设一个固定上下限，要么把正常机动误判成故障，要么把小幅漂移漏掉。

1.3 为什么必须结合执行器动力学

这是整个方案里最容易被忽略、但对检测性能影响最大的一点。很多人做IMU故障检测时，直接拿简化刚体运动学来预测姿态，比如假设控制力矩是瞬间响应的。实际上，从控制器输出偏转指令，到执行器真正摆到指定角度，中间有伺服机构的时间常数，还有速率限制和机械饱和。如果模型里不包含这一段动态，预测姿态就会比真实姿态“超前”或“滞后”，残差在指令变化段会出现大幅尖峰。

我在仿真里刚把这段补上，效果非常明显。原来不加执行器动态时，一个0.5 rad的俯仰阶跃指令，残差峰值能冲到几十毫弧度，导致虚警；加入一阶惯性环节后，动态段的残差收敛到接近白噪声，阈值就能压得很低。所以标题里“结合执行器动力学”并不是一句修饰，而是让残差生成器在飞行过程中保持可信的前提。

2. 俯仰通道建模与故障模式定义

2.1 俯仰通道刚体动力学

运载火箭俯仰通道的完整模型很复杂，涉及弹性振动、液体晃动、气动系数随马赫数变化等因素。但故障检测方案的第一版验证，不需要那么完整的非线性环境。工程上通常先用简化模型把检测逻辑跑通，再逐步往里面加扰动项。我用的刚体动力学是线性化后的俯仰姿态方程：

[ \ddot{\theta} = -a_1 \dot{\theta} + b \delta ]

其中θ为俯仰姿态角，δ为等效执行器偏转角，a1为气动阻尼归一化系数，b为执行器偏转产生的力矩增益。这个式子的物理含义很直接：角加速度由气动阻尼力矩和控制力矩共同决定。在主动段，箭体本身是不稳定的，姿态角偏离程序角后会形成气动力矩，所以在控制律设计时要引入反馈项来稳定。

对应的控制律采用PD形式：

[ \delta_{cmd} = K_p (\theta_{cmd} - \theta) - K_d \dot{\theta} ]

比例项保证姿态角跟踪指令，微分项提供阻尼。Kp和Kd的选取要保证闭环特征根落在合适的区域，既不能太慢，也不能因为增益过高激励出弹性模态。我这里取Kp=2.0、Kd=0.6，在简化刚体环境下闭环稳定。

2.2 执行器动力学与故障模式

执行器这里指的是摆动发动机或舵机的等效输出。它的输入是控制律给的偏转指令δ_cmd，输出是实际偏转角δ。一阶惯性模型可以写成：

[ \dot{\delta} = \frac{\delta_{cmd} - \delta}{\tau} ]

τ是等效时间常数，反映伺服机构的响应快慢。我仿真里取τ=0.05s，意味着指令发出后大约200ms后基本到位。对运载火箭来说这个时间常数并不夸张，伺服机构带宽通常也就十几赫兹。

执行器故障是姿控系统的重要故障源，本文虽然主要检测IMU故障，但建模时必须把执行器故障模式定义清楚，因为执行器故障同样会改变IMU测量值，如果检测逻辑不能区分，就会误报警。我把执行器故障归纳为四类：卡死、松浮、效率下降和饱和提前。卡死指偏转角固定不再响应指令；松浮指输出力矩接近零，等效于偏转角失效；效率下降指实际偏转只有指令的一部分；饱和提前指偏转角被限制在更小的范围内。

2.3 IMU测量模型与典型故障模式

IMU安装在箭体上，经过姿态解算输出俯仰角。理想情况下测量输出等于真实姿态角，加上测量噪声：

[ \theta_m = \theta + v ]

v是零均值高斯噪声。但在实际惯性器件里，故障往往表现为非理想项，我建模时把以下几类故障考虑进去。

第一类是常值偏置，表现为测量输出始终叠加一个有界常数。这是陀螺或加速度计最常见的故障模式，可能是温度变化、刻度因子漂移或者硬件损伤导致。第二类是漂移，偏置随时间线性增大或随机游走，短时间内不明显，积分后会越来越大。第三类是噪声方差增大，可能是器件内部电路异常或者电源噪声耦合，测量信号变得很“脏”。第四类是卡死，测量输出恒定在某个值附近完全失效。

2.4 状态空间表示与离散化

把刚体姿态动力学和执行器动力学写成一个三阶状态方程，状态向量x=[θ; ω; δ]，其中ω=θdot。连续时间模型为：

[ \dot{x} = F x + B \delta_{cmd} ]

[ F = \begin{bmatrix} 0 & 1 & 0 \ 0 & -a_1 & b \ 0 & 0 & -1/\tau \end{bmatrix} ]

[ B = \begin{bmatrix} 0 \ 0 \ 1/\tau \end{bmatrix} ]

IMU测量方程只取姿态角：

[ y = H x + v = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} x + v ]

这里有一个设计选择需要说明：为什么不直接把IMU的角速度测量也放进测量方程？从传感器角度，陀螺确实能输出角速度。但把角速度也作为测量会让Kalman滤波器同时依赖两个通道，故障源难以隔离。第一版检测方案先用姿态角单测量，把残差做干净，后续再扩展多测量通道。另外，状态向量里包含执行器状态δ，这意味着我们假设执行器模型足够可信，它的估计值和真实偏转角一致。执行器故障会导致这个假设破坏，从而也可能被检测到——这是一个相对复杂的问题，后文会专门讨论。

离散化直接用一阶欧拉法，仿真步长dt=0.001s，在俯仰单通道低频环境下精度够用。状态转移矩阵和输入矩阵按((F_d = I + F dt, B_d = B dt))计算。如果想更精确，可以用零阶保持或四阶Runge-Kutta，但故障检测验证阶段并不需要。

3. 残差生成与故障检测方案设计

3.1 卡尔曼滤波器作为残差生成器

Kalman滤波在这里做的不是“估计姿态然后显示给用户”，而是作为模型基准，在故障发生前持续预测IMU本应输出的姿态角。滤波器的预测环节完全基于标称模型，不掺入测量信息；更新环节才使用IMU测量值修正状态估计。残差取滤波新息：

[ r_k = y_k - H \hat{x}_{k|k-1} ]

其中(\hat{x}_{k|k-1})是根据上一时刻估计和控制输入计算出的当前预测。如果IMU正常，r_k是在零附近的白噪声；如果IMU出现偏置或漂移，r_k会表现出均值偏离；如果噪声增大，r_k的方差会明显变大。

这里有个工程直觉需要培养：滤波器增益K的大小决定了状态估计对测量的信任程度。K大，意味着系统更相信测量，残差会被迅速“吸收”，状态估计跟着测量走，故障可能被掩盖；K小，意味着系统更相信模型，残差会一直拖着，故障容易暴露但正常动态偏差也会残留下拉不下去。所以Q和R的取值直接影响检测灵敏度和虚警率，这是后文要反复调的核心参数。

3.2 滑动窗口统计与自适应阈值

单个时刻的残差包含了噪声成分，直接在样本级别设阈值容易抖动。工程上更稳妥的做法是看一段窗口内的统计量。我采用的是滑动窗口内的残差均值和残差平方均值两个指标，分别对均值类和方差类故障敏感。窗口长度取50个采样点，也就是50ms。

[ m_k = \frac{1}{W} \sum_{i=k-W+1}^{k} r_i ]

[ s_k = \frac{1}{W} \sum_{i=k-W+1}^{k} r_i^2 ]

正常飞行时，用一段标称仿真数据统计出m_k和s_k的均值和方差，然后设定阈值为“标称均值+3倍标准差”。这个3σ准则来自统计过程控制，在样本足够多且近似正态的情况下，理论虚警率在0.3%左右。

不能只设一个固定阈值的原因很简单：飞行过程中指令变化会导致模型预测误差短暂变大，即使加入了执行器动力学，也不可能等于真实机械和气动的全部细节。用标称动态段而不是稳态段来标定阈值，可以让阈值本身包含模型失配带来的残差背景。实际操作时可以先把动态段和稳态段截出来分别统计，再取包络作为最终阈值。

3.3 执行器故障与IMU故障的区分问题

这里必须诚实说：仅凭“IMU测量与模型预测的残差大小”，很难严格区分IMU故障和执行器故障。因为执行器卡死会导致实际姿态偏离指令，IMU作为测量源会忠实地把偏差报出来，残差一样会变大。

但在本文设计的场景里，检测目标不是区分故障类型，而是“检测回路里是否存在导致测量与模型不一致的故障”。执行器动力学被纳入状态向量后，执行器故障实际上也被这张网罩住了。如果要进一步隔离，需要在执行器端加位置反馈，或者在残差生成器中分别建立“执行器模型+刚体模型”和“仅刚体模型”两组并行滤波器，通过比较两组残差的差异来判断故障源。这个扩展思路我放在第6章单独讲。

4. Matlab仿真实现与关键步骤拆解

4.1 仿真框架与参数设置

我把整个仿真做成一个主脚本，方便反复调整故障注入时间和参数。初始状态全零，程序角θ_cmd设计成一段矩形波：2秒时从0阶跃到0.5 rad，5秒时回到0。这样仿真段内同时包含动态跟踪段和稳态保持段，能够验证执行器动力学对残差的影响。

下面是主脚本的参数设置部分，完整复现建议直接抄：

clear; clc; close all; dt = 0.001; % 仿真步长 1ms T = 10.0; % 总时长 10s tvec = 0:dt:T; N = length(tvec); % 标称模型参数 a1_nom = 1.5; % 气动阻尼归一化系数 b_nom = 8.0; % 控制力矩增益 tau = 0.05; % 执行器时间常数 % 控制律参数 Kp = 2.0; Kd = 0.6; % 俯仰角指令：2s~5s内为0.5 rad theta_cmd = zeros(1, N); theta_cmd(tvec >= 2 & tvec < 5) = 0.5; % 噪声参数 Q = 1e-6 * eye(3); % 模型噪声协方差 R = 1e-4; % IMU量测噪声方差 % 故障注入参数 imu_bias_time = 3.0; % 在3s注入常值偏置 imu_bias_value = 0.05; % 偏置幅度 0.05 rad

这里噪声参数的选择可以解释一下。R取1e-4，对应IMU姿态角测量噪声标准差10mrad，这个量级对惯性级器件来说是正常水平。Q的物理意义是模型不确定性的等效加速度噪声，我取1e-6，相当于对模型比较信任，但又不至于完全忽略测量。

4.2 主循环：执行器动力学、Kalman滤波与残差记录

仿真主循环做四件事：计算控制指令、更新真实状态、生成IMU测量、更新Kalman滤波并记录残差。下面这段是核心逻辑：

% 状态向量: x_true = [theta; omega; delta] F = [0 1 0; 0 -a1_nom b_nom; 0 0 -1/tau]; B = [0; 0; 1/tau]; H = [1 0 0]; x_true = zeros(3, 1); x_hat = zeros(3, 1); P = eye(3) * 1e-3; res = zeros(1, N); fault_flag = zeros(1, N); for k = 1:N t_k = tvec(k); theta = x_true(1); omega = x_true(2); delta = x_true(3); % 1. 控制律 delta_cmd = Kp * (theta_cmd(k) - theta) - Kd * omega; % 2. 真实模型递推（一阶欧拉） xdot = F * x_true + B * delta_cmd; x_true = x_true + xdot * dt; % 3. IMU测量：注入常值偏置故障 b_bias = (t_k >= imu_bias_time) * imu_bias_value; theta_m = x_true(1) + b_bias + sqrt(R) * randn(); % 4. Kalman滤波预测 x_hat = x_hat + (F * x_hat + B * delta_cmd) * dt; P = P + (F * P + P * F' + Q) * dt; % 5. Kalman滤波更新，残差取新息 r_k = theta_m - H * x_hat; S = H * P * H' + R; K_gain = P * H' / S; x_hat = x_hat + K_gain * r_k; P = (eye(3) - K_gain * H) * P; res(k) = r_k; end

运行之前提醒一个细节：卡尔曼滤波的预测中使用的是控制律输出delta_cmd，但真实状态里执行器状态delta是内部状态。在Matlab脚本里，delta_cmd是控制器算出来的指令，它同时也被送入执行器模型和滤波器预测。如果执行器存在故障，真实delta会偏离标称模型的delta状态，这时候残差会变大。但在第一版检测方案里，滤波器内部始终认为执行器模型是准确的。

这段代码跑出来的残差，在故障注入前基本落在±0.02 rad以内；3秒注入0.05 rad偏置后，残差会立刻出现一个朝向正方向的跳变，随后被滤波器部分吸收，稳定在0.03~0.05 rad附近。这就是检测的依据。

4.3 残差统计与故障判定逻辑

单个残差跳变还不足以直接报警，因为噪声尖峰也可能触发。我写了一个滑动窗口统计函数，并加入连续性判断——连续多个窗口超过阈值才置位故障标志，避免单点干扰造成瞬时虚警。

win = 50; % 50ms窗口 thr_m = 0.03; % 由标称仿真统计得到 thr_s = 0.002; % 由标称仿真统计得到 hold_count = 0; for k = win:N seg = res(k-win+1:k); m_val = abs(mean(seg)); s_val = mean(seg.^2); if (m_val > thr_m) || (s_val > thr_s) hold_count = hold_count + 1; else hold_count = 0; end % 连续5个窗口超限才判定故障 if hold_count >= 5 fault_flag(k) = 1; break; end end

阈值可以直接手动给，也可以从标称仿真自动统计。我的做法是先跑一遍完全无故障的仿真，把窗口统计量的分布记下来，然后用“3σ上限”作为阈值。这里有个容易被忽视的问题：标称仿真本身也包含动态段，所以残差统计量中混合了机动瞬态的峰值。直接用全局3σ会导致阈值偏大、灵敏度下降；如果只截取稳态段标定，机动段又会虚警。更稳妥的方法是分别对稳态段和动态段标定，然后取包络，或者直接使用整个任务期间的保守上限，优先保证不漏报。

4.4 典型仿真结果与性能分析

我在同一套框架里跑了三组工况：

第一组是无故障标称，残差窗口均值不超过0.02 rad，窗口功率稳定在0.0008以下，不触发检测。第二组是在3秒注入0.05 rad常值偏置，检测逻辑在3.025秒附近报警，延迟约25ms。第三组是把偏置减小到0.015 rad，由于这个量级和噪声标准差接近，检测延迟拉长到250ms左右，但最终仍能检出。

执行器卡死的工况值得多说一句：卡死发生后，姿态响应偏离指令，残差同样变大。但因为状态向量里含执行器状态，滤波器会试图用测量去修正被执行器卡死“污染”的状态估计，残差的增长会比IMU故障更平滑。这从侧面印证了第3章提到的隔离难度——单靠一个残差通道无法利落区分故障源。

5. 仿真调试中的常见问题与避坑记录

5.1 数值离散化带来的隐蔽问题

一阶欧拉法虽然简单，但有一类典型问题：当执行器时间常数很小、仿真步长不够小时，状态变量会出现数值振荡。我一开始把τ设为0.01s、dt设为0.005s，结果残差序列出现明显的锯齿状振荡，检测逻辑几乎一直在报警。后来把dt缩小到0.001s，振荡消失，但计算总点数也随之到了10000个，循环时间还可以接受。

另一个隐蔽问题是执行器状态和姿态角状态的量纲差异。姿态角是弧度，偏转角也是弧度，数值上都在0.01到1之间，这反而掩盖了一个事实：它们的动态尺度不同。姿态角响应是秒级，执行器响应是毫秒级到百毫秒级。在设置过程噪声Q时，如果所有状态都给同一个值，执行器状态会被“额外放大”，滤波器对执行器状态的估计可能抖得很厉害。我最终把Q设置为对角阵，姿态角通道给1e-6，执行器通道给1e-5，才让滤波后的执行器状态平滑下来。

5.2 阈值标定到底用什么数据

这是整个方案里最容易返工的地方。一开始我在0到2秒的稳态段上统计残差均值和方差，得到的阈值很漂亮，结果指令在2秒阶跃后，残差窗口均值直接超限，系统虚警。原因很简单：阶跃指令激发了闭环动态，模型即使包含执行器一阶惯性，也和真实刚体响应之间存在相位差，这个瞬态残差被忽略时，任何正常飞行都会报错。

正确做法是直接拿一个完整的标称任务剖面，包括所有机动指令段，统计整个飞行过程中残差窗口统计量的最大值，再用这个值乘以1.2到1.5作为安全余量。如果检测灵敏度不够，可以再折中：把动态段的瞬态窗口剔除，只保留指令完成后100ms后的样本，这样阈值能压得更低。

5.3 检测逻辑的出场与沉默策略

我踩过的一个坑是：检测逻辑只做“数据超限就报警”，结果在故障注入边界附近出现反复报警、复位、再报警的抖动。原因是残差窗口统计量可能刚好在阈值附近来回穿越。加入滞回机制后，问题就解决了：一旦报警，需要连续若干窗口回落到阈值的一半以下才清除故障标志。这在工程化时非常实用，否则报警状态会严重干扰后续系统重构逻辑。

还有一个值得提的细节：故障注入时间最好避开指令切换瞬间。为了验证检测性能，我在仿真里刻意让IMU偏置故障发生在3秒，也就是指令稳态段，这能最清晰地观察检测延迟。如果故障发生在机动段，残差背景本身处于动态峰值，检测延迟会明显拉长。这不代表算法失效，而是模型残差在动态段本身的信噪比更差。实际飞行中如果要提升机动段的检测灵敏度，需要引入多模型滤波或者对动态残差做在线补偿，这就是更高阶的话题了。

6. 可扩展方向与个人体会

6.1 从单通道扩展到三通道

本文所有推导和仿真都基于俯仰单通道，但运载火箭的姿态控制是滚转、俯仰、偏航三通道耦合的。三通道扩展并不是简单地把三个滤波器并联，因为通道间存在惯性耦合项，尤其在姿态角速度较大时，滚转和偏航会互相感应。扩展时最直接的做法是先保持执行器动力学解耦模型，分别建立三个独立的Kalman滤波器，然后在真实动力学里加入耦合项，观察残差是否会因为未建模耦合而变大。如果耦合造成的残差背景明显，就需要考虑全状态滤波或通道间前馈补偿。

6.2 从故障检测到故障隔离与重构

本文的方案能够给出“测量与模型不一致”的报警，但不能直接告诉你到底是IMU的哪个轴出了问题。要做隔离，常用的方法是分组残差法：用不同测量子集的组合分别生成残差，通过残差的组合模式反推故障源。比如把姿态角测量和角速度测量拆成两个残差通道，IMU偏置故障会让角度通道残差异常，而角速度通道可能正常；执行器卡死则会让两个通道同时出现关联异常。隔离出故障源之后，重构策略就是切掉故障通道、改用剩余余度或者降级控制。这套逻辑我在另一个模拟项目里验证过，确实能工作，但代码量和调试难度比本文的示例高一个量级。

6.3 仿真之外的一点工程体会

仿真做到最后，我发现自己很少再碰检测算法本身，时间基本花在模型参数标定和故障场景合理安排上。有些参数看起来“差不多”，比如执行器时间常数从0.05改成0.08，标称仿真里的残差背景就会变大，原来合格的阈值直接失效。做这类仿真最忌讳的就是拿一组参数一顿跑，然后宣布方法有效。更健康的工作方式是：给出一组标称参数和一组不确定性范围，把阈值设计成对这些偏差不敏感，再把故障检测的性能统计做出来。

另外，我建议刚开始做这个方向的朋友先把“残差长什么样”可视化出来，再上检测逻辑。我调试初期就是先把标称残差、故障残差和阈值三条线画在同一张图上，看到残差在指令切换瞬间的尖峰后，我才真正明白为什么要把执行器动力学放进状态向量。纸上谈兵和看图理解是完全不同的体验，这一步省不了。