一种提高捷联惯性/里程计组合导航系统定位精度的后处理方法

李 昂，张忆欣，季 阳，郭元江，张 泽

(北京自动化控制设备研究所，北京 100074)

0 引言

管道机器人可通过超声波探头等传感器实现对海底光缆、石油天然气等管道的无损检测，并依靠内置的惯性测量装置实现管道内裂纹的精准定位，在人员、卫星信号无法到达的环境中有着广泛的应用[1-2]。现代管道铺设时，每隔一段距离会预置一个Mark点：当管道机器人在管道内移动至Mark点时，可获取Mark点的位置信息。考虑管道铺设成本等因素，Mark点不会过于密集，因而Mark点间的位置需要由惯性测量装置的导航解算获得。

为限制惯性测量装置在纯惯性导航状态下速度位置发散较快的问题，通常引入里程计，为系统提供相对可信的速度基准。文献[3]利用里程计与捷联惯导中的陀螺仪进行高精度航位推算，建立航位推算的误差模型；文献[4]将里程计引入管道测量装置，初步解决了单纯依靠惯导系统检测管道精度发散的问题；文献[5]针对复杂应用环境下由于里程计测量信息不准而导致组合导航系统精度下降的问题，利用滤波实现了故障检测和隔离，提高了系统导航精度。此类测绘用捷联惯性/里程计组合导航系统可实现自主连续的定位，具有较高的定位精度。

测绘用捷联惯性/里程计组合导航系统对实时性要求较低，但对精度要求较高，可采取离线后处理方式对数据进行充分挖掘，以获得更高的定位精度[6]。离线后处理不仅可以相应放宽对惯性器件的精度要求，节约测绘用组合导航系统的研制成本,而且可以在满足系统精度指标的前提下，适当减少待测路径中预置的Mark点数量[7]，缩减工程建造费用。更为重要的是，精准定位可大幅降低相关设施的维护保养难度。

本文构建了捷联惯性/里程计组合导航系统的速度匹配Kalman滤波模型[8]和航位推算模型；基于离线后处理技术，提出了正反双向滤波、姿态矩阵迭代和里程计刻度系数迭代相结合的后处理算法；设计了数据离线处理流程，并进行了试验验证。结果表明，与正向滤波方式相比，在离线处理过程中采用双向滤波、姿态矩阵迭代和里程计刻度系数迭代算法，可将位置解算精度提高95%以上。

1 Kalman滤波模型

在传统速度匹配滤波模型的基础上，在状态变量中引入里程计刻度系数误差和安装误差，构建16维速度匹配Kalman滤波模型[9-10]。此模型中的里程计刻度系数误差、航向安装误差角和俯仰安装误差角均有较高的可观测性；在滤波过程中，此模型可消除上述误差对系统姿态、速度和位置解算的不利影响。

系统状态变量为

εxεyεzΘYΘZΔkΔt]T

(1)

捷联惯导系统姿态和速度误差模型如下

(2)

(3)

以捷联惯导系统与里程计的北向、天向、东向速度之差作为观测量[15]

(4)

惯导系统在地理坐标系下的速度为

(5)

里程计的刻度系数误差和安装误差已事先完成出厂标定[13]，可保证上述误差均为小量。故里程计在地理坐标系下的速度为

(6)

联立式(5)和式(6)，可得

记观测矩阵

根据状态变量和观测量，建立Kalman滤波微分方程

(7)

Z(t)=H(t)X(t)+V(t)

(8)

式中，F(t)为状态微分方程系数矩阵，由式(2)和式(3)得出;L(t)为白噪声系数矩阵;W(t)为惯性器件噪声;H(t)为观测矩阵;V(t)为观测噪声，呈高斯分布。对式(7)和式(8)进行离散化处理，可得Kalman滤波状态方程和观测方程，见式(9)和式(10)

xk=Φxk-1+LWk

(9)

zk=Hkxk+vk

(10)

其中，Φ为k-1时刻至k时刻的状态转移矩阵。

根据本节的Kalman滤波模型，在初始对准结束后，依次进行正向和反向导航及滤波解算。

2 航位推算模型

(11)

(12)

(13)

航位推算在第二次正向滤波结束后进行。从起始时刻逐段进行位置推算，并在Mark点处进行位置校正，同时对航位推算信息的误差进行精度判断。

3 正反双向滤波处理

传统的正向滤波是按时间顺序，从起始点处理采样数据直至终点。正反双向滤波则是在正向滤波结束后，按时间倒序处理采样数据至导航起始时刻[14-15]。如此一来，等效为采样数据量翻倍，有利于滤波器状态变量进一步分离，从而提高导航解算精度。

通过晃动基座粗对准，可获取系统的初始姿态信息。随后，构建16维速度匹配Kalman滤波模型，进行正向滤波。捷联惯导系统正向姿态、速度和位置解算公式为

(14)

(15)

(16)

(17)

(18)

(19)

(20)

(21)

(22)

(23)

在正反双向滤波结束后，可重置惯导速度和位置，并补偿陀螺漂移、加表零偏和里程计刻度系数误差等。通过上述处理，提高了系统再次正向滤波时的导航解算精度，从而提高了后续航位推算精度。

4 姿态矩阵迭代和里程计刻度系数迭代

从起始时刻起，依次解算相邻两个Mark点间的位置,进行航位推算，并观测Mark点处的位置误差。在每个Mark点处校正航位推算的位置，并计算里程计剩余刻度系数δk、航向剩余安装误差δφu和俯仰剩余安装误差δφx

(24)

其中，SDn、SDu、SDe、SDL分别为相邻两个Mark点之间里程计航位推算获得的北向、垂向、东向以及径向位移；dLD、dhD、dλD分别为里程计航位推算得到的纬度、高度、经度与Mark点的纬度、高度、经度的差值。

为逼近真实位置信息，在得到δk、δφu和δφx后，迭代上述系数，迭代后的系数为dk、dφu、dφx，迭代方式如下

(25)

ΔSi=(1+dk)ΔSi

(26)

(27)

因dφx和dφu均为小角度，由方向余弦矩阵，有

在上述系数完成迭代后，根据预设的位置误差门限，判断是否再次进行此阶段的航位推算：若位置精度不满足要求，利用迭代得到的三个参数更新姿态矩阵，重新进行此段位置计算；若位置精度满足要求，此段离线处理工作完成，进行下一段Mark点间采样数据的位置解算。

按此方式处理所有Mark点间的数据，直至数据结束。

5 后处理算法流程设计

本文后处理算法分为正向滤波、反向滤波、再次正向滤波、相邻Mark点间的航位推算、姿态矩阵迭代和里程计刻度系数迭代等模块：

首先，构建16维速度匹配Kalman滤波器，进行正向滤波，见第1节；以正向滤波结束点为起点，进行反向滤波至初始位置，见第3节；在正反双向滤波器期间，估计惯性器件和里程计相关误差。待反向滤波结束后，再次进行正向滤波，记录各时刻姿态信息。最后，利用各时刻记录的姿态信息进行航位推算，并进行姿态矩阵迭代和里程计刻度系数迭代，见第2节和第4节。依次处理所有相邻Mark点，直至数据结束。图1所示为后处理算法框图。

图1 后处理算法框图

6 试验结果及分析

为验证数学模型和后处理技术的正确性，进行了试验验证。试验路线如下：待测路径全程8100m，高度变化100m，每1000m设置一个Mark点。起始点经度116.1526°，纬度38.8138°；结束点经度116.2012°，纬度39.8462°。待测路径平面图如图2所示。

图2 待测路径平面图

设置里程计脉冲更新频率为10Hz，惯性器件采样频率为200Hz。

Kalman滤波周期为1s。分别采用正向滤波(forward filter)、正反双向滤波(forward and reverse filter)、正反双向滤波加姿态矩阵和里程计刻度系数迭代(forward and reverse filter+itera-tion)三种算法，分离器件误差并补偿。随后，均按时间顺序进行导航解算，并存储姿态信息。最后，进行航位推算，以获取定位结果。根据本系统惯性器件精度，预设Mark点位置误差门限为0.1m。比较正向滤波、正反双向滤波和本文方法三种算法的定位精度，如图3所示。

图3 不同离线处理方法的位置精度对比

由图3可知：

1)正向滤波和正反双向滤波两种方案在Mark点处对系统进行位置校正，一定程度上控制了系统的位置误差，但随时间累积，位置误差增长明显。

2)只进行惯性/里程计组合导航算法的正向滤波处理时，最大位置误差为1.72m。

3)引入反向滤波后，最大位置误差减小至0.39m。

4)在正反双向滤波的基础上，引入姿态矩阵迭代和里程计刻度系数迭代技术，使全程的定位误差控制在0.1m以内，从而实现厘米级的定位精度。

与正向滤波方式相比，正反双向滤波方式对陀螺和加速度计的采样数据进行了更充分的挖掘，使状态变量估计更精确；迭代姿态误差矩阵和里程计刻度系数进一步提高了航位推算精度。

7 结论

本文针对测绘用捷联惯性/里程计组合导航系统，在传统速度匹配Kalman滤波器的基础上，引入里程计相关误差项，构建了16维滤波模型。提出了正反双向滤波算法，提高了数据利用率；提出了姿态矩阵迭代和里程计刻度系数迭代算法，优化了姿态解算精度，从而提高定位精度。试验结果表明，与传统正向滤波相比，正反双向滤波融合加姿态矩阵迭代和里程计刻度系数迭代算法的定位精度更高，系统定位精度提高95%以上。