一种双频弹性干扰下角速度滤波方法
【技术领域】
[0001] 本发明涉及一种角速度滤波方法，特别是涉及一种角速度扩展卡尔曼滤波方法。
【背景技术】
[0002] 同陀螺器件相比，角加速度计具有体积小、成本降低空间大、无漂移、安装限制 少的特点。角加速度计通常被用来组建无陀螺仪的惯性测量单元（gyro-freeinertial measurementunitGF-IMU)，在航天飞行器中具有很好的应用前景。在一些工程实践中将 飞行器作为刚体模型来处理，认为角加速度计只受到测量噪声的影响。实际上，飞行器是一 个弹性体，除了测量噪声外还受到飞行器自身弹性振动的影响。这种弹性振动对角速度测 量有着很大的影响。针对该问题，在以往工程实践中，将弹性振动建模为不同频率和幅度的 正弦信号，并且假定该干扰信号的频率是已知且固定的；干扰信号的幅度和角速度信号幅 度的比例关系是已知的，使用传递函数式的数字滤波器对角速度信号的测量值进行滤波。 这种方法存在两个缺陷。第一，滤波器传递函数的形式和弹性干扰信号的频率有关，在不同 的场合下，弹性干扰的频率可能不同，导致滤波器需要重新设计。这使得该方法适用面比较 窄。第二，传递函数形式的滤波器虽然对弹性干扰和测量噪声进行了滤除，但是会产生相位 延迟，导致对角速度信号估计值不准确。
【发明内容】
[0003] 本发明为了解决现有的传递函数形式滤波器在面临不同频率的弹性干扰时需要 重新设计的问题和进行弹性干扰和测量噪声滤除时产生相位延迟而导致的角速度信号估 计值不准确的问题。
[0004] 一种双频弹性干扰下角速度滤波方法，包括下述步骤：
[0005] 步骤1、针对弹性干扰下角速度信号动态过程进行建模：
[0006] 飞行器的角速度动态过程为
[0008] 其中，ω代表角速度，a代表角加速度，Μ为转动力矩，J为转动惯量；
[0009] 弹性干扰信号为两个正弦信号叠加而成，设两个正弦信号的频率分别为％和 ω2,单位为rad/s;两个正弦信号的相位分别为料和0，幅度分别为匕和b2;
[0010] 弹性干扰信号的形式如下
[0011]
[0012] 将弹性干扰信号中的Μsin(呌/ +仍）进行处理得到弹性干扰状态Vl、v2，将弹性干 扰信号中的(哗+奶）进行处理得到弹性干扰状态v3、v4;
[0013] 将干扰频率％和ω2当做状态进行估计；设状态为X=[χv1v2v3v4c0iω2] T，其中x为角速度信号，即x=ω;则角速度和干扰信号的状态方程为
[0015] 其中，az角加速度的测量值；
[0016] 角速度的测量方程为
[0017] z=x+v1+v3+w (4)
[0018] 式中，z为角速度的测量值；w是零均值高斯白噪声；
[0019] 测量矩阵为
[0020]Η= [1 1 0 1 0 0 0] (5)
[0021 ] 步骤2、设计状态转移矩阵：
[0022] 由状态方程（3)描述的系统的状态转移矩阵为
[0033] 其中，Τ为测量周期；
[0034] 步骤3、设计Kalman滤波方程：
[0035] 角速度滤波器采用EKF(扩展卡尔曼滤波器）设计，Kalman滤波器方程为
[0039] Pk+1,k+1= (I_Kk+1H)Pk+1,k (24)
[0040] 式中，是对k+1时刻状态的估计值；i:u是对k+1时刻状态的预报值；Kk 和Kk+1分别是k时刻和k+1时刻的滤波器增益；Zk+1为k+1时刻的测量值；Φk+1,Ak时刻到k+1时刻的状态转移矩阵；pk+lik为k+1时刻状态预报的协方差矩阵；PkidPPk+lik+1分别是k 时刻和k+1时刻状态估计协方差矩阵；Q为过程噪声矩阵；R为测量噪声矩阵；I为7X7维 单位矩阵；
[0041] 步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行 滤波处理。
[0042] 本发明具有以下有益效果：
[0043] 本发明设计了一种基于扩展卡尔曼滤波的角速度测量信号的滤波方法，适用于两 个任意低频正弦弹性干扰下的角速度测量值的干扰和测量噪声滤除问题；设计好的滤波器 不需要根据不同的弹性干扰频率重新设计或者修改，可以适用于任意低频弹性干扰信号， 使得本发明的滤波方法适用范围较广；而且本发明的滤波方法可以估计出弹性干扰信号的 频率。同时本发明的滤波方法针对角速度信号的估计值基本上没有相位滞后，使得估计结 果更加准确，估计误差标准差相对于测量误差标准差减小了 70 %以上。
【附图说明】
[0044] 图1为实施例中角速度信号真值、测量值和估计值的对比图；
[0045] 图2为实施例中弹性干扰状态Vl下的估计仿真结果；
[0046] 图3为实施例中弹性干扰状态v2下的估计仿真结果；
[0047] 图4为实施例中弹性干扰状态v3下的估计仿真结果；
[0048] 图5为实施例中弹性干扰状态v4下的估计仿真结果；
[0049] 图6为实施例中对两个弹性干扰信号频率的估计仿真结果。
【具体实施方式】
【具体实施方式】 [0050] 一：
[0051] -种双频弹性干扰下角速度滤波方法，包括下述步骤：
[0052] 步骤1、针对弹性干扰下角速度信号动态过程进行建模：
[0053] 飞行器的角速度动态过程为
[0055] 其中，ω代表角速度，a代表角加速度，Μ为转动力矩，J为转动惯量；
[0056] 弹性干扰信号为两个正弦信号叠加而成，设两个正弦信号的频率分别为％和 ω2,单位为rad/s;两个正弦信号的相位分别为fV和朽，幅度分别为匕和b2;
[0057] 弹性干扰信号的形式如下
[0058]
[0059] 将弹性干扰信号中的邮+ ?〇进行处理得到弹性干扰状态A、v2，将弹性干 扰信号中的hsin (>/ +的）进行处理得到弹性干扰状态v3、v4;
[0060] 将干扰频率ω^Ρω2当做状态进行估计；设状态为X=[χv1v2v3v4c0iω2] T，其中x为角速度信号，即x=ω;则角速度和干扰信号的状态方程为
[0062] 其中，az角加速度的测量值；
[0063] 角速度的测量方程为
[0064]z=x+v1+v3+w (4)
[0065] 式中，z为角速度的测量值；w是零均值高斯白噪声；
[0066] 测量矩阵为
[0067]Η= [1 1 0 1 0 0 0] (5)
[0068] 步骤2、设计状态转移矩阵：
[0069] 由状态方程（3)描述的系统的状态转移矩阵为
[0080] 其中，τ为测量周期；
[0081] 步骤3、设计Kalman滤波方程：
[0082] 角速度滤波器采用EKF(扩展卡尔曼滤波器）设计，Kalman滤波器方程为
[0087] 式中，氧+_是对k+Ι时刻状态的估计值；ii+u是对k+Ι时刻状态的预报值；心和 Kk+1分别是k时刻和k+Ι时刻的滤波器增益；Zk+1为k+Ι时刻的测量值；Φk+1,Ak时刻到k+1时刻的状态转移矩阵；Pk+lik为k+1时刻状态预报的协方差矩阵；P^和Pk+lik+1分别是k 时刻和k