0 引　言

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

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

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

1 基于EKF的地磁/陀螺信息融合姿态测量算法设计 1.1 坐标系的定义及坐标系转换

发射坐标系 $O{x_{\rm{n}}}{y_{\rm{n}}}{{\textit z}_{\rm{n}}}$ 如图1所示。发射系原点位于载体上与载体固连，发射系不随载体转动。 ${x_{\rm{n}}}O{y_{\rm{n}}}$ 与地理水平面重合， $O{x_{\rm n}}$ 轴水平指向发射方向， $O{{\textit z}_{\rm n}}$ 轴垂直向下指向地心， $O{y_{\rm n}}$ 轴与之形成右手坐标系。

弹体坐标系 $O{x_{\rm{b}}}{y_{\rm{b}}}{{\textit z}_{\rm{b}}}$ 如图1所示。弹体坐标系与弹体固连，随弹体转动，其原点位于弹体质心。 $O{x_{\rm{b}}}$ 轴与弹轴重合，方向指向弹轴前方， $O{y_{\rm{b}}}$ 轴垂直于 $O{x_{\rm{b}}}$ 轴方向向右，而 $O{{\textit z}_{\rm{b}}}$ 轴垂直于 ${x_{\rm{b}}}O{{\textit y}_{\rm{b}}}$ 平面向下。

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

在图2中，发射坐标系经三次变化即可得到载体坐标系。在初始状态下，载体坐标系与发射坐标系完全重合。第一次转动后，发射坐标系 $O{x_{\rm{n}}}{y_{\rm{n}}}{{\textit z}_{\rm{n}}}$ 绕z轴旋转航向角 $\varphi $ ，得到 $O{x_1}{y_1}{{\textit z}_{\rm{n}}}$ ；第二次转动后，坐标系 $O{x_1}{y_1}{{\textit z}_{\rm{n}}}$ 绕y轴旋转俯仰角 $\theta $ ，得到 $O{x_{\rm b}}{y_1}{{\textit z}_1}$ ；第三次转动后，坐标系 $O{x_{\rm b}}{y_1}{{\textit z}_1}$ 绕x轴旋转滚转角 $\gamma $ ，得到 $O{x_{\rm{b}}}{y_{\rm{b}}}{{\textit z}_{\rm{b}}}$ 。

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

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

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

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

地磁传感器的测量值是地磁场在载体坐标系下的三分量 ${{H}}_{\rm m}^{\rm b} = {\left[ {\begin{array}{*{20}{c}} { {H_x} }&{{H_y}}&{{H_{\textit z}}} \end{array}} \right]^{\rm T}}$ ，它与地磁场在发射坐标下三分量 ${{H}}_{\rm e}^{\rm n} = {\left[ {\begin{array}{*{20}{c}} { H_x^{\rm n} }&{H_y^{\rm n}}&{H_{\textit z}^{\rm n}} \end{array}} \right]^{\rm T}}$ 可以通过姿态矩阵进行转换，即有：

其中 ${ C}_{\rm n}^{\rm b}$ 是从地理坐标系到弹体坐标系的转换矩阵，即：

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

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

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

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

式（4）可以转化为：

取 ${{Q}}(t) = {\left[ {\begin{array}{*{20}{c}} {{q_0}(t)}&{{q_1}(t)}&{{q_2}(t)}&{{q_3}(t)} \end{array}} \right]^{\rm T}}$ ，则式（5）所示方程连续，求解齐次线性方程解为：

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

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

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

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

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

其中 ${{x}}\left( k \right) = {\left[ {\begin{array}{*{20}{c}} {{q_0}(k)}&{{q_1}(k)}&{{q_2}(k)}&{{q_3}(k)} \end{array}} \right]^{\rm T}}$ ， ${{v}}\left( k \right)$ 为均值为零的高斯白噪声，其协方差为：

以式（3）所示的磁传感器四元数姿态测量模型建立观测方程。因为式（3）是非线性，因此建立非线性系统观测方程为：

其中：

${{n}}\left( k \right)$ 为均值为零的高斯白噪声，其协方差为：

从而得到如下形式的非线性系统模型为：

其中 ${{v}}\left( k \right)$ 、 ${{n}}\left( k \right)$ 互不相关。对于这种模型形式的过程，采用扩展Kalman滤波器（EKF）。在扩展Kalman滤波器中，状态更新方程式基于非线性模型，误差协方差矩阵的更新则基于式（4）的Taylor级数展开^[12]。状态的预报为：

协方差的更新需要每步都计算Jacobian矩阵，进一步计算Jacobian矩阵：

随后，协方差矩阵 ${{P}}\left( {k + 1} \right)$ 的更新方程和增益矩阵 ${{K}}\left( {k + 1} \right)$ 的计算可写成：

状态估计是利用真实非线性关系进行校正的，写成：

根据式（21），最后得出状态四元数估计值，根据四元数与姿态转换矩阵的关系求得载体的姿态角为：

2 试验验证 2.1 计算机仿真

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

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

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

根据图7~图9对于导弹飞行模型仿真结果显示，制导弹飞行60 s落地，全程导弹轴向正转，无反转。对比EKF融合算法和纯地磁解算算法，EKF解算滚转角的误差均值为0.113 3°，标准差为0.058 5°，而纯地磁解算滚转角的误差标准差为−3.377 6°，标准差为2.129 5°；所以EKF融合算法的滚转角解算精度提高一个数量级以上，此外纯地磁解算滚转角误差最大为−9.158°，而EKF算法的滚转角误差在最后阶段最大仅为0.267 1°，滚转角解算精度较高。此外EKF解算俯仰角的误差均值为−0.253 6°，标准差为0.104 2°，纯地磁解算俯仰角的误差均值为0.174 4°，标准差为0.365 2°，所以EKF融合算法的噪声水平降低约1/3，在57.5 s地磁解算俯仰角误差最大达到2.013°，而EKF解算俯仰角误差均不超过−0.6°。纯地磁无法解算偏航角，而EKF解算偏航角的误差均值为0.276 7°，标准差为0.171 7°，全程最大不超过1°，解算偏航角精度较高。此融合算法不仅可以解算弹载3个姿态角，弥补了地磁传感器滚转角、俯仰角测量受偏航影响的缺陷，而且解算得到的姿态角均具有较高的精度和较低的噪声水平。

2.2 半物理试验验证

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

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

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

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

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

3 结束语

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