基于逆向Kalman滤波的行进间对准

王东升，何光宇，姜希会

（1. 天津航海仪器研究所，天津 300131；2. 中国船舶集团有限公司，北京 100097）

行进间对准作为一种能显著提高车辆机动性的技术，而受到国内外学者的广泛关注。文献[1]通过车辆行驶前和行驶途中停车状态下的重力矢量，确定初始时刻载体坐标系和中间时刻过渡坐标系的姿态关系间接计算出姿态矩阵，完成初始对准。文献[2][3]在组合导航系统中使用自适应Kalman 滤波实现行进间精对准。这些方法均需要在车辆行驶前进行一段时间的静态粗对准，限制了车辆的机动性。为解决此问题，文献[4-6]将惯性系对准的思想应用在行进间对准中，但是在对准过程中没有GPS 辅助的情况下难以进行精确的导航定位，因而在对准结束时要通过地标点进行位置修正。文献[7]提出了基于一种逆向数据处理的行进间对准方案，通过逆向导航算法将动基座初始对准与位置导航有机结合起来，先进行捷联罗经动基座姿态初始对准和传感器采样数据存储，再利用逆向航位推算算法和正向航位推算算法获得位置信息，解决了以往行进间对准结束后需要利用地标点修正位置的问题，为捷联惯导的初始对准提供了一个新的方法。近年来，文献[8]将逆向导航算法应用在捷联惯导的静基座对准中，可以有效缩短对准时间并提高对准精度。本文受文献[7]的启发，并引入逆向Kalman 滤波技术，提出了一种行进间对准方案，通过正逆向Kalman 滤波算法对采集的历史数据进行多次解算，取消了静态粗对准过程，节约了对准时间，同时也获得了较高的姿态和位置对准精度。

1 坐标系定义

导航坐标系（n系）：原点on为载体中心，o n xn轴指向地理东向，o n yn轴指向地理北向，on zn轴指向天向。

载体坐标系（b系）：原点ob为载体中心，o b xb轴沿载体横轴向右，ob yb轴沿载体纵轴向前，ob zb轴沿载体立轴向上。

地心惯性系（i系）：原点oi为地球质心，o i xi轴在赤道平面内指向春分点，o i yi轴在赤道平面垂直于o i xi轴，oi zi轴为地球自转轴。

初始时刻地心惯性系（i0系）：与对准初始时刻地心惯性系（i系）重合。

初始时刻载体惯性系（ib0系）：与对准初始时刻载体坐标系（b系）重合。

2 行进间对准方案设计

在惯性/里程计组合导航系统中使用Kalman 滤波实现行进间精对准时，往往需要一段时间的静态粗对准来满足实施Kalman 滤波的线性化条件。同时，起始阶段姿态误差大会引起较大的位置误差，导致车辆需要行驶到地标点进行位置修正。

针对上述问题，本文提出了一种基于逆向Kalman滤波的行进间对准方案，在整个初始对准过程中进行数据采集和存储，并将这些历史数据同时用于粗对准和精对准。通过惯性系对准方法，使用里程计作为外界速度测量装置，完成行进间粗对准过程。对于已采集的数据信息，利用导航计算机的高速运算能力通过逆向导航算法将系统状态在短时间内由粗对准结束时刻t1 逆向解算至对准起始时刻t0（起始时刻车辆为静止状态），同时使用逆向Kalman 滤波回溯估算出的姿态信息结合起点处已知的速度位置信息对惯导进行校正，完成逆向精对准过程并得到具有一定精度的姿态信息。紧接着进行快速正向组合导航使系统状态追赶上正常的实时计算直至对准结束时刻t2，完成全部精对准过程并获得较高精度的姿态和位置信息。整个行进间对准方案如图1 所示。

图1 行进间对准方案Fig.1 The scheme of in-motion alignment

2.1 粗对准

式中，表示导航系n到初始时刻惯性系i0的变换矩阵，通过对准时的实时位置变化和地球自转角速度ωie确定，由于粗对准过程中难以获得准确的位置信息，在车辆行驶距离不长的情况下忽略位置变化的影响并使用初始时刻的位置代替计算。表示初始时刻载体惯性系ib0到载体坐标系b的变换矩阵，由陀螺输出的角速率得到。示初始时刻惯性系i0到初始时刻载体惯性系ib0的变换矩阵，容易通过加速度计输出fb、里程计速度和重力矢量gn之间的转换关系求得。

2.2 逆向导航和航位推算算法

在实际计算时，捷联惯导的姿态、速度和位置更新方程一般可表示为

为得到逆向解算时的惯导姿态、速度和位置更新方程[7]，对式(2)-(6)进行移项变换，如下：

对比式(2)-(6)和式(7)-(11)可知，通过对地球自转角速度导航系相对地球的旋转角速度、重力加速度gn、陀螺输出的角速率加速度计输出的比力fb和位置更新方程中的位置变化量都进行取反处理，实现了逆向导航。

对正向航位推算算法进行简单的变换即可得到逆向航位推算算法，如下所示：

通过以上推导不难发现，正向与逆向导航过程中相同时刻的姿态、速度、位置都可基本保持一致，便可对同一段导航数据行进重复解算，并利用起点处的已知信息进行校正，来提高对准精度。

2.3 逆向导航系统误差模型与逆向Kalman 滤波模型

根据捷联惯导误差方程对逆向导航系统的误差方程进行推导[9]，过程如下：

实际计算的姿态矩阵与理想姿态矩阵存在小角度的姿态误差角φ，那么可表示为：

计算姿态矩阵一般通过如下微分方程求解：

由上文可知，实现逆向导航过程需要对陀螺输出角速度和导航系的旋转角速度进行取反，即

将式(18)(19)和式(16)的微分代入式(17)中，得

对式(21)进行化简便得逆向导航的惯导姿态误差方程，如下所示：

根据逆向导航的速度更新方程式(8)，逆向导航的惯导速度微分方程可以表示为

实际的计算速度通过式(24)得到

将式(16)代入式(25)中，展开得

对式(26)进行化简，略去二阶小量误差和重力误差δ gn，得到逆向导航的速度误差方程如下：

由式(9)-(11)可知，逆向导航的位置更新微分方程可写为

通过位置更新微分方程式(28)-(30)可得逆向导航的位置误差方程如下：

综合上文推导的逆向导航的误差方程，考虑到里程计的安装误差角和标度因数会受到车辆的振动、轮胎的形变等因素的影响。相应的惯性/里程计逆向组合导航系统的系统误差方程如下：

式中，φ、δ v和δ p分别表示东北天三个方向上惯导的姿态误差、速度误差和位置误差，ε和∇分别为陀螺漂移和加速度计零位误差，δ pD表示航位推算的位置误差，δ K为里程计的标度因数误差，α ψ为里程计和惯导的安装方位误差。

根据式(34)可推出惯性/里程计组合导航系统的逆向Kalman 滤波模型为

其中，X为20 维的状态变量,Z为观测向量，F和H分别表示系统矩阵和观测矩阵，G为噪声分配矩阵，W和V分别表示系统噪声和观测噪声。它们定义如下：

状态变量

逆向Kalman 滤波中的系统矩阵分为两部分，分别由捷联惯导的误差方程和航位推算的误差方程得出，如式(37)-(39)所示。

噪声分配矩阵

系统噪声

观测向量

观测矩阵

根据以上公式，通过逆向Kalman 滤波方法将惯导和里程计进行组合，对系统状态进行逆向解算便可得到较为准确的姿态信息。

3 车载行进间对准实验

为验证该行进间对准方案的有效性，本文开展了车载行进间对准实验。实验设备主要由激光捷联惯导、里程计、GPS 和猎豹实验车组成。其中激光陀螺的精度为0.002 °/h ，加速度计的精度为50 ug，GPS 定位误差小于0.1 m，惯性元件的采样频率和惯导解算频率均为200 Hz。实验过程如下：

（1）车辆静止时启动GPS，记录起点处的位置信息。

（2）启动惯导，开始行进间粗对准的同时进行数据存储，车辆行驶速度约为30 km/h 至50 km/h。

（3）车辆行驶到500 s 时，开始逆向精对准，导航计算机进行高速运算，将车辆的状态回溯至初始时刻，并对惯导进行校正，整个过程耗时约40 s，期间车辆继续行驶。

（4）逆向精对准结束后，再进行约60 s 的正向组合导航直至行进间对准结束，整个过程中导航计算机仍保持高速运算，确保在对准结束前追赶上系统正常的实时计算。

（5）车辆行驶到600 s 对准完成时停车，得到此时的航向ψ′和位置p′，再多次静基座对准得到均值ψ，将ψ作为参考航向基准，并记录终点处的位置信息p作为参考定位基准。

行车轨迹如图2 所示，全程约7 km，道路为柏油马路。

图2 行车轨迹Fig.2 Test trajectory

图3、图4 和图5 为整个行进间对准过程中三个姿态失准角的Kalman 滤波估计曲线。

图3 北向失准角估计曲线Fig.3 Estimation of north misalignment

图4 东向失准角估计曲线Fig.4 Estimation of east misalignment

图5 天向失准角估计曲线Fig.5 Estimation of azimuth misalignment

从图3-5 可以看出，利用逆向Kalman 滤波回溯至初始时刻，用估计结果对惯导进行校正后，修正了大部分的姿态失准角，之后的正向组合对准过程中，滤波曲线收敛，可以保证对准精度。一共进行了9 组行进间对准实验，实验结果如表1 所示。

表1 行进间对准实验结果Tab.1 Results of in-motion alignment

由表1 可知，应用逆向Kalman 滤波方案，系统行进间对准的航向误差为0.042°(σ1 )，位置误差为5.99 m(RMS)。

4 结 论

本文将逆向数据处理的思维方式引入到行进间对准中，通过对历史采样数据的分时复用，在粗对准结果的基础上进行逆向精对准和正向组合导航，完成了两次精对准过程。不仅取消了传统的静态粗对准过程，而且提高了定向和定位精度，保证了在粗对准较差的情况下仍能获得较高精度的姿态和位置信息。最后，通过车载对准实验验证了所提出的行进间对准方案的正确性和有效性。该算法的提出为后续的工程应用奠定了基础。