基于强跟踪滤波的捷联惯导/里程计组合导航

陈雨，王健博，张凌东，曹全，陈世业，刘宇航

(1.北京航天发射技术研究所，北京 100076；2.火箭军驻211厂军事代表室，北京 100076)

0 引言

现代战争要求车载发射平台具有自主定位定向能力，由于捷联惯导系统具有自主性、隐蔽性、抗干扰性以及可以在全球范围内连续提供运载体全部运动参数的特点，车载发射平台普遍配备捷联惯导系统。但捷联惯导系统导航误差随时间发散，通常可以采用卫星导航或里程计与捷联惯导系统进行组合以限制惯导系统导航误差或限制其发散速度。卫星导航信号容易受遮挡，且容易受电子信号干扰，限制了其在军事上的使用。相对于卫星导航，里程计与捷联惯导组合导航仍具有自主性和抗干扰能力强的特点，因此，捷联惯导/里程计组合导航在车载发射平台中得到了广泛的应用。

捷联惯导/里程计组合导航通常采用速度匹配方法[1-3]、位置匹配方法[4-6]或里程增量匹配方法[7-11]。速度匹配需要对里程计信号进行微分，当里程计的脉冲当量大时，造成很大的量化误差，导致观测量不准确，影响滤波精度。位置匹配时，需要将航位推算的位置误差扩充到组合导航状态变量中，增加了状态变量维数，造成计算量增加。为了克服速度匹配时量化误差对观测量的影响，文献[7-10]提出采用导航系下的里程增量匹配方法，文献[11]提出采用载体系下的里程增量匹配方法。通常捷联惯导IMU(inertial measurement unit)敏感中心与里程计速度测量点之间存在一定长度的杆臂，当载车有角运动时，捷联惯导IMU敏感中心与里程计速度测量点的速度有差异。组合导航时，由杆臂引起的速度差异与组合导航系统的误差传播无关，如不消除就会影响组合导航精度。文献[12]提出将杆臂扩充到捷联惯导/里程计组合导航状态中的杆臂在线补偿算法以消除杆臂对组合导航的影响。

捷联惯导/里程计组合导航通常将里程计刻度系数误差建模为随机常值，由于受轮胎胎压、温度、复杂路面特性等的影响，刻度系数实时变化。本文提出将强跟踪滤波引入组合导航，以跟踪里程计刻度系数的变化，同时，采用平均速度匹配方法，降低载车振动及里程计量化误差对组合导航的影响。将杆臂误差列入状态变量，进行在线估计。建立了状态变量包括杆臂误差在内的平均速度匹配组合导航模型，并通过实际跑车验证了本文方法的有效性。

1 组合导航模型

1.1 里程计测速模型

安装于载车底盘的里程计以脉冲形式输出非转向轮轴中心点在地面投影的前向速度。采样间隔内的脉冲频率大小代表了载车前进的速度大小，定义载车坐标系(m系)为右前上坐标系，考虑载车正常行驶时的运动学约束，里程计的输出可以写成矢量形式为

式中：Ni为里程计在采样周期内输出的脉冲数；k为里程计刻度系数；Ts为采样周期。

考虑到捷联惯导载体坐标系(b系)与m系不重合，则b系和导航坐标系(n系)下的里程计的输出分别为

忽略二阶小量，式(4)可以写成

1.2 杆臂速度模型

捷联惯导IMU敏感中心与里程计速度测量点之间存在一定长度的杆臂，当载车有角运动时，捷联惯导IMU敏感中心与里程计速度测量点的速度有差异。捷联惯导IMU敏感中心与里程计速度测量点的速度存在关系：

杆臂速度为

实际组合导航时，根据捷联惯导姿态矩阵与标称杆臂计算杆臂速度，由于惯导姿态矩阵和标称杆臂存在误差，实际计算的杆臂速度为

忽略二阶小量，式(8)可以写成

式中：δlb为杆臂距离误差。

1.3 状态方程

组合导航使用前可以对捷联惯导的安装误差、里程计的刻度系数和杆臂进行标定，由于刻度系数与轮胎的直径相关，受轮胎气压等的影响，刻度系数存在误差，安装误差和杆臂也可能标定不准确，因此，将里程计刻度系数误差、捷联惯导安装误差、杆臂误差都列为状态。捷联惯导/里程计组合导航系统状态定义为

将里程计刻度系数误差、捷联惯导俯仰安装误差、捷联惯导方位安装误差和杆臂距离误差都建模为随机常值。状态方程包括捷联惯导误差方程、惯性器件误差模型、里程计刻度系数误差模型、捷联惯导安装误差模型、杆臂误差模型，可以写成

式中：RM为地球子午圈曲率半径；h为高度；L为纬度；vN为北向速度；vE为东向速度；RN为地球卯酉圈半径。

1.4 量测方程

由于捷联惯导与里程计测量点非理想刚性连接，载车机动时，惯导安装处存在振动，该振动引起捷联惯导速度与里程计测量速度存在误差，并且里程计采样周期很短，造成里程计输出的瞬时速度存在很大的量化误差。为了消除振动及量化误差对组合导航的影响，采用滤波周期内的平均速度作为观测量，即对速度观测量在一段时间进行积分，然后除以滤波周期，得到平均观测量，即

考虑到实际卡尔曼滤波周期比较短，在滤波周期内捷联惯导速度误差、失准角、里程计刻度系数误差、捷联惯导安装误差、杆臂误差近似为常值，将式(15)与式(9)代入式(14)，可得观测方程为

(16)

实际应用时，卡尔曼滤波周期的选择要综合考虑里程计量化误差、惯导误差在滤波周期内是否近似为常值误差及振动环境。若滤波周期选择为1 s，则本文提出的采用平均速度作为观测量的方法与文献[7，9]提出的采用单位时间内位移增量之差作为观测量的方法等价。

2 强跟踪滤波

将捷联惯导/里程计组合导航的状态方程、观测方程写成离散化形式有

(17)

式中：wk，vk均是离散化的零均值白噪声，其协方差阵分别为Qk，Rk。

由于里程计刻度系数受轮胎胎压、温度、载荷、轮胎打滑等的影响实时变化，因此，本文提出采用具有突变状态跟踪能力的滤波器进行组合导航。而强跟踪滤波正是为解决模型不确定性与状态突变在卡尔曼滤波算法基础上改进的一种滤波算法[13-15]。卡尔曼滤波方程为：

时间更新方程

量测更新方程

强跟踪滤波利用卡尔曼滤波残差序列相互正交的性质，在线实时调整渐消因子。在标准卡尔曼滤波方程用式(23)替换卡尔曼滤波方程中预测方差矩阵更新方程式(19)，则得到强跟踪滤波基本方程。

式中：Dk为渐消因子矩阵。

渐消因子矩阵的计算方法如下[9-11]：

(25)

(29)

3 试验验证

为了验证本文提出的捷联惯导/里程计组合导航方法的有效性，进行了实际跑车验证。捷联惯导使用的激光陀螺零偏稳定性约为0.002 (°)/h，石英挠性加速度计零偏稳定性约为20 μg。捷联惯导完成多位置初始对准后，进行组合导航跑车试验，试验过程中采集高精度卫星导航定位数据作为捷联惯导/里程计组合导航位置基准，卫星导航定位精度为3 m(CEP)。跑车试验里程为48 km，跑车试验持续时间3 848 s。强跟踪滤波器中遗忘因子ρ取为0.99，弱化因子lk取为1，除刻度系数误差的ηi取为1.1外，其他状态都取为1。跑车试验轨迹如图1所示。

跑车试验结果如图2～5所示，其中，图2为定位误差曲线，图3为里程计刻度系数误差和捷联惯导安装角估计曲线，图4为加速度计零偏估计曲线，图5为杆臂估计曲线。从图2可见，定位误差总的趋势是随里程的增加而增大，这是因为里程计只能提供速度信息，不能提供直接位置观测，里程计的作用是降低惯性定位误差的发散速度，整个试验过程定位误差在3 800 s达到最大38 m，行驶里程为35 km，定位精度约为1‰D，随后误差有所减小，总行驶里程数为48 km，因此定位终点的定位精度优于1‰D。从图3可见，里程计刻度系数在整个试验过程中变化约5‰，相对于定位精度1‰D，可知该方法能有效跟踪里程计刻度系数的变化，捷联惯导的俯仰安装角和方位安装角估计值相对于标定值在实时变化。由于可以对里程计刻度系数和捷联惯导的安装误差进行实时估计，因此可以有效延长组合导航系统的标定周期。从图4可以看出，组合导航能实时对加速度计零偏进行估计，其中加速度计零偏始初估计值为初始对准对加速度计零偏的估计值。从图5可以看出相对于杆臂的标定值，组合导航能估计出杆臂误差。

4 结束语

里程计刻度系数受环境的影响实时变化，本文提出在捷联惯导/里程计组合导航时利用强跟踪滤波对里程计刻度系数进行强跟踪估计。推导了状态变量包括杆臂误差在内的平均速度匹配组合导航模型。平均速度匹配可以降低里程计量化误差对组合导航的影响，特别是对于里程计分辨率较低的情况。实际跑车试验结果表明，该组合导航方法定位精度约为1‰D，且能估计组合导航中主要的误差源，为捷联惯导/里程计组合导航工程应用提供了一种有效的方法。