基于EKF的地磁／陀螺信息融合姿态测量算法研究

张晓明 陈雷 张莺莺 檀杰 朱孟龙

摘要：针对智能弹药机动飞行中仅利用地磁信息制导时无法实现全姿态角解算的问题，提出一种采用三轴陀螺仪角速率信息辅助三轴磁传感器信息进行弹体姿态角解算的EKF融合算法。算法利用磁传感器测量模型和四元数微分方程建立观测方程和状态方程，并分别对非线性的系统进行线性化得到卡尔曼滤波方程。通过在高速飞行仿真转台上进行半物理仿真试验，最终全姿态角的解算实现对地磁/陀螺信息的融合。经过对仿真信号的处理，在弹体俯仰角士300变化的情况下，该EKF融合算法解算滚转角和俯仰角比传统单纯依靠地磁信息进行滚转角和俯仰角解算的精度提高近一个数量级，并且解算偏航角误差在10以内。

中图分类号：TN919.5 文献标志码：A文章编号1674-5124（2019）05-0010-07

关键词：四元数;全姿态角解算;地磁/陀螺信息融合：EKF

0 引言

现代战争要求进攻武器具备精确打击能力和快速响应能力，传统的常规弹药无法满足这种要求，必须对常规弹药进行制导化改造，而改造的核心为弹体姿态的准确测量。由于受常规弹药高旋转、高冲击、小体积的应用环境限制，传统的弹载姿态测量方案中，单一使用磁传感器无法实现全姿态角的实时解算，并且其精度易受弹体俯仰方向变化和干扰磁场的影响[1-2]。而常规组合导航算法计算量大，数据更新率低，信息融合精度低，不能满足制导弹药的实时性、可靠性和精度要求[3-5]。因此需要一种高精度的信息融合姿态测试方案，以满足全姿态角高精度实时解算和适应复杂的弹载环境要求。

对于上述问题，文献[6]提出了由单轴陀螺和三轴地磁传感器组合姿态测量方案，利用单轴陀螺积分得到轴向滚转角，进而由滚转角结合地磁传感器信息解算得到俯仰角和偏航角;该解算虽然方法简单，但是滚转角初值不易获取，并且精度受到限制。文献[7]采用的方法由于基于实际弹道可以较好匹配模版弹道，从而获得一些飞行特征参数，在发射前将其装订供弹载飞行使用，发射后将弹道参数与三轴磁传感器信息结合从而解算姿态角，最后用一个扩展卡尔曼滤波器融合磁传感器和陀螺仪的信息，提高解算精度和系统稳定性;这种方法在实际弹道模型准确的情况下具有较高精度，但是其过程复杂，实际弹道与匹配弹道总有偏差，而无法获得较高的精度。文献[8]采用权系数和卡尔曼滤波算法对地磁/陀螺组合测姿中的仿真信号进行处理，得到较好的结果，证明其算法的适用性;但卡尔曼滤波算法是针对线性高斯的情况，仅用于线性系统处理。以上方案分别从传感器组合、弹道匹配、算法处理3个方面对信息融合姿态进行了测试，但均不能满足复杂弹载环境全姿态角实时解算要求。

本文提出一种基于EKF的地磁/陀螺信息融合姿态测量方法。利用三轴陀螺仪和三轴磁传感器对弹体姿态角进行解算，避免了精度受限的问题且滚转角易获取，利用EKF将非线性方程线性化，进而对仿真信号进行处理，避免了弹道匹配的偏差且可以处理非线性系统。

1 基于EKF的地磁/陀螺信息融合姿态测量算法设计

1.1 坐标系的定义及坐标系转换

发射坐标系Ox_nv_nz_n如图1所示。发射系原点位于载体上与载体固连，发射系不随载体转动。X_nOy_n与地理水平面重合，Ox_n轴水平指向发射方向，Oz_n轴垂直向下指向地心，Oy_n轴与之形成右手坐标系。

弹体坐标系OxbybZb如图1所示。弹体坐标系与弹体固连，随弹体转动，其原点位于弹体质心。Ox_b轴与弹轴重合，方向指向弹轴前方，Oy_b轴垂直于Ox_b轴方向向右，而Oz_b轴垂直于x_bOy_b平面向下。

为了得到弹体的姿态角，需要研究发射坐标系与载体坐标系之间的转换关系。载体坐标系固联在载体上，当弹体在飞行时载体坐标系也会发生变化，载体坐标系与发射坐标系之间的夹角就是弹体运动的姿态角[9]。设φ为偏航角，θ为俯仰角，γ为滚转角，转换关系如图2所示。

在图2中，发射坐标系经三次变化即可得到載体坐标系。在初始状态下，载体坐标系与发射坐标系完全重合。第一次转动后，发射坐标系OxnYnZn绕Z轴旋转航向角φ，得到Ox₁y₁z_n;第二次转动后，坐标系Ox₁y₁zn绕Y轴旋转俯仰角θ，得到Ox_by₁z₁;第三次转动后，坐标系Ox_by₁z₁绕x轴旋转滚转角γ，得到Ox_by_bz_b。

1.2 基于EKF的地磁/陀螺信息融合姿态测量原理

地磁/陀螺信息融合姿态测量原理如图3所示，其核心是设计一个扩展卡尔曼滤波器对姿态四元数进行估计，最终再由姿态四元数换算得到姿态角。首先依载体姿态角作为原始输入，由陀螺仪的测量可以得到载体坐标系相对发射坐标系的三轴角速率，再利用四元数微分方程得到姿态四元数信息。图3中的卡尔曼滤波器中，采用四元数微分方程作为状态方程，以姿态四元数为系统观测量，ω_x、ω_y、ω_z作为状态方程的输入量。姿态输入的另一个路径是，直接由姿态角换行到姿态四元数，即系统状态量，经地磁测量得到载体坐标系下的地磁三分量Hx、Hy、Hz，以此作为对系统状态量的观测反馈到EKF，系统观测方程以发射坐标系和载体坐标系下的地磁三分量转化关系建立的。由于系统观测方程是非线性的，因此采用扩展卡尔曼滤波器进行迭代估计。

1.3 地磁/陀螺信息融合的扩展卡尔曼滤波器设计

1.3.1 基于四元数的地磁姿态测量原理

地磁传感器的测量值是地磁场在载体坐标系下的三分量H_m^b=[HxHyHz]^T，它与地磁场在发射坐标下三分量H_eⁿ=[H_xⁿH_yⁿH_zⁿ]^T可以通过姿态矩阵进行转换，即有：

H_m^b=C_n^bH_eⁿ（1）其中C_n^b是从地理坐标系到弹体坐标系的转换矩阵，即：

由于转换矩阵C_n^b线性相关，因此利用纯地磁信息无法完成全姿态解算，必须由其他传感器辅助给出其中一个姿态角，才能利用地磁信息得到另外两个姿态角[10]。

用四元数代替姿态角表示从地理坐标系到弹体坐标系的转换矩阵[11]，有：

1.3.2 基于四元数的陀螺姿态测量原理

根据载体坐标系和发射坐标系的定义，可以得到描述角速率和角位置关系的四元数微分方程：式（4）可以转化为：

Q=1/2M^*（ω_nb^b）Q（5）取Q（t）=[q₀（t）q₁（t）q₂（t）q₃（t）]^T，则式（5）所示方程连续，求解齐次线性方程解为：

将其离散化，假设一个周期内的陀螺仪的角速度为一固定值，令：

则得到四元数离散化的齐次线性方程解为：

已知载体初始四元数和任何时刻下陀螺仪的输出角速度数据，就能求得更新后的四元数。

1.3.3 EKF状态方程和观测方程的建立

以姿态四元数Q为状态量，建立如下的状态方程为：

2 试验验证

2.1 计算机仿真

依据弹道质心运动学方程和弹丸绕质心转动的运动学方程，由于质心运动分析可以忽略其他星球对制导弹的影响，导弹仅仅受到地球引力作用，不考虑地球自转及其绕太阳的公转，并且认为地球为一个质量分布均匀的圆球体，因此导弹轨迹可以简化，不影响炮弹运动要求的精度[13]。设定初始条件，t=0时，弹丸初速ν₀=870m/s，弹道初始倾角θ₀=7.5°，即可计算得到弹丸飞行过程的速度三分量，进一步解算出弹丸任意时刻的偏航角φ和俯仰角。为：

此外弹体轴向变化根据实际某型号弹设定参数，将其积分得到滚转角的姿态信息。理想姿态角的变化曲线如图4所示，由姿态角生成的仿真三轴磁场值和三轴陀螺如图5和图6所示。

根据生成的三轴磁场信息和三轴陀螺信息运用EKF融合算法对其信息融合进行偏航角、俯仰角、滚转角的计算，并且与地磁解算的俯仰角和滚转角对比，得到如图7一图9所示误差曲线。

根据图7～图9对于导弹飞行模型仿真结果显示，制导弹飞行60s落地，全程导弹轴向正转，无反转。对比EKF融合算法和纯地磁解算算法，EKF解算滚转角的误差均值为0.1133°，标准差为0.0585°，而纯地磁解算滚转角的误差标准差为一3-3776°，标准差为2.1295°;所以EKF融合算法的滚转角解算精度提高一个数量级以上，此外纯地磁解算滚转角误差最大为-9.158°，而EKF算法的滚转角误差在最后阶段最大仅为0.2671°，滚转角解算精度较高。此外EKF解算俯仰角的误差均值为-0.2536°，标准差为0.1042°，纯地磁解算俯仰角的误差均值为0.1744°，标准差为0.3652°，所以EKF融合算法的噪声水平降低约1/3，在57-5s地磁解算俯仰角误差最大达到2.013°，而EKF解算俯仰角誤差均不超过-0.6°。纯地磁无法解算偏航角，而EKF解算偏航角的误差均值为0.2767°，标准差为0.1717°，全程最大不超过1°，解算偏航角精度较高。此融合算法不仅可以解算弹载3个姿态角，弥补了地磁传感器滚转角、俯仰角测量受偏航影响的缺陷，而且解算得到的姿态角均具有较高的精度和较低的噪声水平。

2.2 半物理试验验证

把集成了EIMC 1043地磁传感器、ITG-3701陀螺仪（两个传感器的测量精度均在1mV以内）的系统捷联安装于转台上（见图10），在转台的零点位置上（内框、中框、外框均为转台零位）预先装载地磁场初始三分量，上电内框在is内加速到5r/s，外框在1s内转动到30°，之后内框以5r/s的速度转动，中框和外框均按照30sin（2πt）的规律运动，此阶段20s，实验全程共21s。以三轴高速飞行仿真转台的反馈信息为基准信号，每组试验后分别对试验数据进行读取、保存、处理，通过转台反馈数据对比EKF融合算法和纯地磁解算姿态角的精度，误差如图11、图12和图13所示。

由图11可以看出，EKF解算滚转角相比于纯地磁解算滚转角，精度得到明显改善，抗偏航和俯仰扰动能力提升，机动性较好。具体比较数据如表1所示。

由表1可得月：KF解算滚转角相比于纯地磁解算滚转角精度提高一个数量级以上，且标准差也减小为原来的1/10左右，噪声水平提高近一个数量级。

此外，由图12可以得出纯地磁解算俯仰角和EFK解算俯仰角均值相差不多，但从标准差和最大偏差来看，融合算法的俯仰角精度提高一个数量级以上，如表2所示。

从图13可以得出，EKF解算偏航角的误差均值为0.2807°，标准差为0.2228°，误差均在1°以内，进一步证明了EKF对于偏航角解算的准确度，可以融合地磁和陀螺信息实现全姿态角高精度解算。

结束语

本文提出一种基于EKF的简单、高效、实时性、自适应的地磁/陀螺信息融合姿态测量方法。得到主要结论为：1）方法解算精度比地磁解算的滚转角和俯仰角提高一个数量级以上，而偏航角的解算误差在1°以内;2）采用信息融合后，外界干扰和全姿态机动的情况对解算精度影响较小，且解算姿态角误差较为稳定，变化幅度更小。

参考文献

[1]赵鑫炉.基于MARL传感器的车辆姿态测量系统设计[D].太原：中北大学，2014.

[2]韩艳.制导炮弹飞行姿态的陀螺/磁阻传感器组合测量方法研究[D].南京：南京理工大学，2010.

[3]鲍亚琪，陈国光，吴坤，等.基于磁强计和MEMS陀螺的弹箭全姿态探测[J].兵工学报，2008，29（10）：1227-1231.

[4]赵鑫炉，张晓明，龙达峰，等.旋转弹用滚转角磁测系统设计[J].传感技术学报，2013，26（9）：1309-1313.

[5]龙达峰，刘俊，张晓明，等.高速旋转弹载体干扰磁场补偿算法[J].弹箭与制导学报，2016，36（2）：151-156.

[6]曹红松，冯顺山，赵捍东，等.地磁陀螺组合弹药姿态探测技术研究[J].弹箭与制导学报，2006，26（3）：142-145.

[7]王嘉雨.地磁/陀螺信息融合的姿态解算算法研究[D].太原：中北大学，2015

[8]马秀丽.基于磁阻月沱螺组合测量弹体飞行姿态的数据融合方法研究[D].南京：南京理工大学，2012.

[9]代刚，李枚，苏伟，等.自旋导弹捷联式陀螺/地磁姿态测量方法[J].中国惯性技术学报，2010，18（6）：702-705.

[10]刘凯，梁晓庚.基于陀螺仪和磁强计的姿态解算方法研究[J].计算机仿真，2014（5）：39-41.

[11]赵鑫炉，张晓明，白诸锉，等.基于磁阻传感器的航姿测量系统罗差补偿技术研究[J].传感技术学报，2013（11）：1504-1507.

[12]GRANDVALLET B，ZEMOUCHE A，BOUTAYEB M，et al.Real-time attitude-independent three-axis magnetometercalibration for spinning projectiles：A sliding windowapproach[J].IEEE Transactions on Control SystemsTechnology，2013，22（1）：255-264.

[13]JULIER S J，UHLMANN J K，DURRANT-WHYTE H F.Anew method of the nonlinear transformation of means andcovariance in filters and estimation[J].IEEE Transactions onAutomatic Control，2000，45（3）：477-428.

（編辑：李刚）

收稿日期：2018-10-07;收到修改稿日期：2018-11-13

基金项目：国家自然科学基金（61873247）

作者简介：张晓明（1976-），男，山西新绛县人，教授，博士，研究方向为动态测试与组合导航。