基于自适应互补滤波的滚转角测量算法

陈 督，霍鹏飞，陈 超，雷泷杰

(西安机电信息技术研究所，陕西 西安 710065)

0 引言

弹体滚转姿态角是制导弹药判定实际飞行弹道的基本原始信息之一，为了提高测量精度和抗干扰能力，近年来普遍采用多传感器方案进行姿态测量，其姿态解算的关键技术为数据融合技术[1]。基于MEMS的传感器具有可靠性高、小体积、低成本和高集成度等特点[2]，随着MEMS技术的迅速发展，MEMS传感器的精度不断提高，其在多传感器组合方面的应用也随之不断增加。文献[3]对扩展卡尔曼滤波器(EKF)和互补滤波器进行了比较验证，表明了互补滤波器相对于EKF具有较快的收敛速度，但是由于只限于两种传感器，其精度还是有限。文献[4]利用三轴陀螺仪提出四元数扩展卡尔曼滤波器，但是未对地磁矢量加以考虑，磁场的干扰仍未得到有效的解决。国内外在互补滤波器的研究方面，对于两种不同传感器数据融合颇为注重[5]，致使多传感器数据资源没有得到充分的利用，此外，还多用陀螺仪辅助磁罗盘完成故障检测[6]，对陀螺仪的精度要求较高。本文针对上述问题，提出了基于自适应互补滤波的滚转角测量算法。

1 滚转角测量数学模型和互补滤波理论

基于互补滤波器的多传感器姿态信息融合，首先在传感器级别利用惯性传感器进行姿态解算，得到各个传感器解算的结果，随后利用互补滤波器进行数据融合得到载体的姿态信息。

1.1 双轴磁传感器测量滚转角数学模型

弹体的速度矢量如图1所示，地磁要素如图2所示。

图1 地面坐标系下的速度矢量Fig.1 Velocity vector in ground coordinate system

图2 地磁要素图Fig.2 Geomagnetic feature map

设地面坐标系下的磁场向量Mg为(mxg,myg,mzg)，双轴磁强计测得弹体坐标系下磁场向量Mb为(x,myb,mzb)，其中x与解算滚转角无关。由文献[7]知Mg与Mb关系如下：

(1)

令:

k1=-sinθcosψmxg+cosθmyg+sinθsinψmzg

k2=sinψmxg+cosψmzg

k3=sinψmxg+cosψmzg

k4=sinθcosψmxg-cosθmyg-sinθsinψmzg

由速度矢量可以解出θ和ψ，则由式(1)可解出：

(2)

由式(2)可根据磁强计信息解出滚转角。

1.2 单轴陀螺测量滚转角数学模型

设ωx是陀螺仪的输出，ω0是陀螺输出的零偏。通过下式可以由陀螺信息解算滚转角。

(3)

当采样率足够高时，式(3)可以近似为:

γn=(ωx-ω0)ΔT+γn-1

(4)

通过算法确定当前零偏量和滚转角初始值即可计算当前的滚转角度。

1.3 互补滤波理论

假如我们对一个物理量有两种观测手段，一种容易受到高频干扰，另一种容易受到低频干扰，那么我们就可以设计两个互补的滤波器F1和F2。F1是低通滤波器,F2是高通滤波器，且F1+F2≡1[8]。

互补滤波算法是将干扰信号在频率上具有互补特性两个信号通过上述的一组互补滤波器，然后加权求和从而提取有用信息的方法。基于互补滤波的滚转角测量公式如下：

γ=K·γmag+(1-K)·γL

(5)

式(5)中，γ是当前滚转角，γmag是F1滤波器输出的测量结果，γL是滤波器F2输出的测量结果，K是互补滤波算法调节参数，它的取值直接影响到滤波精度。

2 基于自适应互补滤波的滚转角测量

互补滤波器是针对同一个信号的不同传感器测量得到的噪声特性,从频率域分辨和消除噪声。相比于时域的滤波器,并不需要对信号进行精确建模,而且操作简单易实现[9]。

互补滤波的关键是设置合适的滤波器参数K。针对引信转速高，干扰特征变化明显的问题，本文利用磁测数据γmag计算出角速度ωm，然后计算ωm与陀螺仪输出ω的差值eω，最后通过eω由式(6)—式(8)自适应调整互补滤波的系数K，从而改善在不同干扰下滚转角算法的适应性。

(6)

eω=ωm-ω

(7)

算法需要根据陀螺角速度误差和当前转速自动调整滤波器系数K从而达到自动适应各种场景的目的。当引信转速降低，陀螺的输出误差随之减小；当引信进入修正阶段，磁强计受到舵机的干扰，误差随之增大。自适应系统将根据式(8)自动分辨当前的干扰情况，采取更恰当的K值，从而提高解算精度。

(8)

同时由于设计的滤波器在通带内存在t秒的群延时，产生的延时误差wt在高转速情况下会更加显著。因此在高转速且eω较小的情况下对滤波器输出进行延时补偿可以显著降低延时带来的误差。

自适应互补滤波系统框图如图3所示。

图3 系统框图Fig.3 System block diagram

3 仿真验证

为了验证算法在高转速、转速减小、低转速弹道修正等环境下的测量精度和稳定性，分别在三个场景采用K取1，0.5，0.1等定值的传统互补滤波算法和自适应互补滤波算法进行仿真对比。

考虑到高速旋转以及减速旋转时舵机处于静止状态，噪声主要是高斯白噪声，弹道修正阶段，舵机噪声强且同时存在高斯白噪声，设计以下三个场景进行仿真验证。

在高速旋转场景下使转速维持在300 r/s，陀螺仪引入31.4 rad/s的零偏，双轴磁强计和陀螺仪各引入1 dBm/Hz的高斯噪声；减速环境下，转速100 r/s减到0 r/s，陀螺仪引入31.4 rad/s零偏，各传感器各加1 dBm/Hz的高斯噪声；弹道修正阶段，陀螺加上0.3 rad/s的零偏，传感器加上从实验样机提取的实际舵机噪声。图4—图6为上述各场景下的滤波误差对比。

图4 高转速下的几种情况误差对比Fig.4 Comparison of several algorithm errors at high speeds

图5 减旋阶段的误差对比Fig.5 Comparison of errors in the derotation phase

图6 修正阶段的误差对比Fig.6 Comparison of errors during attitude correction

由图4可见，传统互补滤波器在高转速时滤波器延时带来的误差十分显著。图5显示，在转速减小的场景下传统互补滤波器误差随着转速减小而减小。图4、图5中的自适应互补滤波误差均小于0.12 rad，显著优于传统互补滤波器。

图6表明传统互补滤波器在低转速下由滤波器延时带来的误差已经可以忽略不记，且随着K值减小，误差有所减小，而自适应互补滤波误差小于0.03 rad，精度比传统互补滤波提高了2～3倍。可见在全弹道环境下，自适应互补滤波算法均具有稳定的测量效果。

4 结论

本文提出了基于自适应互补滤波的滚转角测量算法。该方法采用互补滤波器对速率陀螺仪、磁强计信息进行了融合，针对引信在出炮口时刻高速旋转，卫星对准时刻的滚转减旋以及弹道修正时刻的滚转角稳定等不同弹道特征点处的干扰特性，设计了参数自适应调整的互补滤波器，能够有效克服传统互补滤波器无法适应引信的全部工作场景的困难。理论分析及仿真结果表明，该算法能够有效补偿高速旋转情况下滤波器延时带来的延时误差、陀螺零偏漂移、舵机高速运动引入的磁场干扰等，能够在干扰存在的情况下较传统固定系数互补滤波器滚转角测量精度提升2～3倍，具有更强的环境适应性和工程可实现性。