基于卡尔曼滤波法的船载惯性测量单元设计*

孔令哲

(中国电子科技集团公司第三十九研究所 陕西省天线与控制技术重点实验室，西安 710065)

0 引言

卫星技术的进步和成本降低将让未来卫星带宽资源越来越丰富，加上廉价大数据以及物联网的广泛应用，基于机载、车载和船载动中通设备的需求量日愈剧增。惯性测量单元(IMU)作为动中通设备的核心部件，是决定动中通设备跟踪精度和市场竞争力的关键[1]。但常规姿态测量单元价格昂贵，简易姿态测量单元精度不足，需要通过合理的算法弥补彼此的不足[2]。本文将介绍我所在民用船载动中通领域如何运用常规的微电子(MEMS)传感器测量数据构建惯性测量单元(IMU)并通过卡尔曼滤波法与四元数获得稳定、可靠的欧拉角姿态信息。此方法简单易行，通过数据融合提升了常规的微电子传感器对欧拉角姿态信息测量的精度，可广泛的应用于各个行业，尤其为动中通系列产品及类似设备的民用化、普遍化提供了可能。

1 姿态信息与欧拉角

在介绍姿态板构成前首先要说明一下什么是姿态。此处的姿态源于飞行器姿态[3]，它着力描述的是研究对象体轴相对于地面的角位置，通常用偏航角、俯仰角与横滚角三个角度表示。偏航角(yaw)，研究对象机体纵轴在水平面上的投影与该面上参数线之间的夹角；俯仰角(pitch)，研究对象机体纵轴与水平面的夹角；滚转角(roll)，研究对象对称平面与通过其机体纵轴的铅垂平面间的夹角。

为了更为形象的表述，本方案选用了欧拉角(Eulerian angles)描述姿态。对于在三维空间里的一个参考系，任何研究对象坐标系的取向都可以用一组有序的欧拉角来表现[4]。参考系是静止不动的，而研究对象坐标系则固定于刚体，随着刚体的旋转而相对参考系旋转。任何研究对象坐标系均可通过右手笛卡尔坐标系(Cartesian coordinates)描述，因此，在参考系内，研究对象坐标系的两个表征向量确定后，此研究对象坐标系即可确定，反之亦然[5]。

2 MEMS姿态信息

姿态信息指的是研究对象坐标系相对于参考系的欧拉角信息。在非标定的前提下，为迅速有效的获得此信息，首先我们得选择可供微电子传感器直接测量的参考系两个表征向量[6]。于是，重力加速度与磁场成为了最佳的选择。依据右手笛卡尔坐标系法则，我们规定重力加速度的指向为参考系Z轴反方向，水平面上磁场北指向为参考系X轴方向，则此参考系为磁场指北坐标系。在惯性测量单元三个标志向量X、Y与Z上分别安装测量重力加速度与磁场的微电子传感器，即可获取重力加速度与磁场在此测量坐标系下的测量向量(gxgygz)与(magxmagymagz)，归一量化后为(GxGyGz)与(MagxMagyMagz)，利用向量叉乘获得第三个单位表征向量(SxSySz)。由此构成姿态单位标准矩阵Mc，

(1)

此矩阵即可描述研究对象坐标系与参考坐标系间的关系。

鉴于欧拉角所描述的旋转矩阵Me同样是单位标准矩阵，因此可获得联立关系式。

(2)

由此，可通过微电子传感器获得的Mc矩阵求得偏航角(Y)、俯仰角(P)与滚转角(R)三个旋转欧拉角，继而获得姿态信息。

但仅依靠此方法获得的姿态信息精度完全取决于微电子测量单元的测量精度，在震荡、抖动与电磁干扰等诸多不利环境下，如若想获得高精度姿态信息，单单靠提升微电子传感器的测量精度与抗干扰性成本极大且效果欠佳。因此，需寻求一类滤波方案使测量结果精度高、实时性好，卡尔曼滤波法成了最佳选择。

3 卡尔曼滤波法

卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法[7-8]。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。其实时性好、精度高，普遍应用于航天航空领域，是姿态信息滤波法的不二选择。但欧拉角并非线性系统，广义上不存在系统状态方程。因此，使用卡尔曼滤波法前需引入另一个描述旋转的数学模型，即四元数。

3.1 四元数

四元数(quaternions)也是一种重要的描述姿态的方法[9]。早在19世纪，爱尔兰数学家哈密顿(Hamilton，1805—1865)在1843年首先在数学中引入了四元数的概念，并建立了四元数理论，后来Klein等又做了进一步的研究，但该理论一直未得到实际应用。自20世纪60年代以来，随着航空航天领域中飞行器姿态控制系统研究的迅速发展，才开始广泛采用四元数法。用四元数法来描述姿态只需要四个参数，并且没有奇异点。

四元数是具有四个元素的超复数，它可以描述一个坐标系或一个矢量相对于某一坐标系的旋转，定义为：

(3)

(4)

(5)

旋转矩阵与欧拉角的关系是

(6)

欧拉角与四元数的关系是

(7)

由此可见旋转矩阵、欧拉角与四元数都可以描述同一个目标姿态，且彼此可以相互转换。相比而言，旋转矩阵普遍存在于空间姿态转换运算中，但有九个参量[10]，虽然彼此有限制但仍不宜于描述；欧拉角由三个参量表述，形象直观地描述了姿态的旋转，但由于三角函数非线性，因此在数学应用上存在局限性；四元数由四个参量组成，描述姿态既不直观，其运算过程又复杂，但它存在状态方程。因此，通过四元数状态方程，借助卡尔曼滤波是可以实现姿态信息滤波的。

3.2 卡尔曼滤波方案

卡尔曼滤波法的实质是通过多渠道测量，在预先知晓各测量方案的噪音状态情况下获得概率意义上的测量数据[11]。因此，为配合卡尔曼滤波法，需构建有别于直接测量的其它测量姿态通道。在惯性测量单元三个标志向量X、Y与Z上分别安装角速度计、加速度计，实现一步预测，结合卡尔曼滤波法，通过姿态旋转四元数状态方程，对比预估姿态与实际测量姿态获得概率意义上的低噪音、高精度姿态信息，从而获得快速、准确的滤波结果。过程可分为五步：

第一步：状态预测

设在Tk-1时刻，即在没有姿态测量输出时，由姿态旋转四元数状态方程运算获得四元数姿态信息预估值：

(8)

第二步：一步预测误差方差阵

(9)

其中，Qk-1为四元素旋转矩阵干扰参数阵，与安装精度与微电子测速单元的测量精度有关，可预先测得[12]，Pk-1为Tk-1时刻的预测误差方差阵，Pk，k-1为下一时刻的预测误差方差阵。

第三步：滤波增益矩阵

Kk=Pk，k-1HkT[HkPk，k-1HkT]-1

(10)

其中，Hk为观测阵，此算法如选用姿态矩阵则为单位对角阵。

第四步：状态估计

在Tk时刻，由姿态测量输出与预估值，通过增益矩阵运算，获得概率意义上的真值：

(11)

第五步：估计误差方差阵

(12)

其中，I为单位对角阵。

由此五步，可及时准确的获得经卡尔曼滤波法处理的以四元数形式表述的姿态信息，从而获得旋转矩阵与姿态欧拉角信息。

3.3 基于卡尔曼滤波的惯性测量实际效果

以某型号产品实测数据为例，如图1所示。

(a)角速度数据 (b)加速度数据

此运动由三轴测试转台实现，其运动过程与空间姿态可知。由于噪音、温漂等因素干扰，测量信息模糊。

如仅通过三轴加速度计以常规测量方式获得姿态信息，如图2所示。

(a)横摇角度 (b)纵摇角度 (c)偏摇角度

由图2可知，测量的横摇、纵摇虽然逼近理论值，但噪音大，如用于姿态标校则将引起控制信息震荡，另外偏航运动不能通过三轴加速度计信息测得，因此，常规测量方式获得姿态信息不可直接应用。

如仅通过三轴角速度计以积分方式获得姿态信息，如图3所示。

(a) 横摇角度 (b) 纵摇角度 (c) 偏摇角度

由图可知，积分的横摇、纵摇、偏航角度虽然连续平滑，但由于残差、温漂、离散处理等不可避免因素影响，在运动过程中，角度均有可能偏离理论值甚至发散。因此，积分姿态信息不可长时间无修正使用。

使用卡尔曼滤波法将测量的姿态信息与积分的姿态信息综合处理，获得统计的姿态信息如图4所示。

(a) 横摇角度 (b) 纵摇角度 (c) 偏摇角度

由图可知，通过卡尔曼滤波法获得统计的横摇、纵摇角度不但相对连续平滑，而且可长时间逼近理论值，较为及时、准确的反映了真实的姿态信息，满足动中通姿态标定及速度前馈的需求。

4 结论

基于微电子器件的惯性测量单元，其价格低廉、构成简单、易于实现，配合上卡尔曼滤波法，可消除大部分的噪音干扰与累积误差，进一步提升了其测量精度与性能，最大限度的发掘了测量芯片的能力，使其可满足多数动中通设备姿态测量的需求[13-15]，尤其是在船载动中通领域，其测量精度与实时性基本可满足各类民用船载动中通天线姿态标定及速度前馈需求。