基于路网匹配的多源自主组合导航技术研究

李 旦，刘 明，胡华峰

(湖北航天技术研究院总体设计所，武汉 430040)

0 引言

基于惯性导航的多源信息融合组合导航技术已广泛应用于海、陆、空、天等各领域的组合导航系统，全球导航卫星系统(Global Navigation Satellite System，GNSS)、星敏感器、测距仪、测速仪、里程计、气压计、激光雷达、视觉导航、声学传感器等辅助导航设备的发展和应用，对提升组合导航性能和精度起到了关键性的作用[1-3]。

对于陆地车辆而言，惯导/GNSS/里程计/气压计组合导航系统是目前比较成熟的方案，它能够提供精确的位置、姿态和速度信息，可以提高车辆的机动性和灵活性[4]。在卫星信号受干扰或无卫星信号可用的环境下，陆地车辆特别是陆地战车的自主导航能力对于提升自身的生成能力和快速反应能力具有至关重要的作用。

惯导/里程计/气压计自主组合导航方法能提供数十米级别的水平位置精度和10 m级别的高程位置精度，但里程计的测量误差受环境影响较大[5]，海拔、气候、温湿度、路面粗糙程度、载重等都会影响里程计测量的准确性，从而影响自主组合导航系统的精度。为了提高自主组合导航系统的精度，本文将路网匹配算法与惯性/里程计组合方法相结合[6-8]，一方面惯性/里程计组合可以为路网匹配提供高精度的导航轨迹数据，另一方面路网匹配根据导航轨迹数据与事先采集的路网数据进行对比匹配，得到高精度的匹配结果用以修正惯导/里程计导航误差，如此往复迭代修正，能够进一步提高组合导航的精度。

1 路网匹配组合导航

1.1 路网匹配介绍

路网匹配最早与地理信息系统(Geographic Information System，GIS)结合使用，以提高定位车辆在电子地图中的位置精度[9-11]。路网匹配作为一种软件纠错技术，将导航定位系统采集的位置信息与路网数据库中的位置信息进行比较，计算出导航系统在路网中的匹配位置[12]；由于路网数据库中的位置信息往往精度较高，通过匹配后能够用来修正车载导航系统的定位误差，提高车载导航的精度。

点到线路网匹配方法将方向、点到道路距离等因素通过各自加权系数进行累加，运用权重的策略将点匹配到道路上。然而这种点到线的匹配由于缺乏对整体轨迹趋势的考虑，在复杂的路网环境下容易导致误匹配。现如今大多数针对轨迹数据的地图匹配方法采用基于曲线相似度的全局匹配方法[13]，曲线匹配与点和线的匹配有差异，需要使用距离准则来定义2条曲线间的相似程度。

近年来，数学学科得到快速发展，涌现出了多种判别曲线相似度的方法，使用较广泛的是相似性函数判别方法以及特征值判别方法[14]。其中Hausdorff距离主要用于计算点集之间的相似度，Frechet距离用于计算曲线之间的相似度。Frechet方法选取Frechet距离作为路段的权重，构建网络图，计算最短路径，从而获得最佳的匹配路段，但是Frechet距离是考虑距离集合中的最大值，因此很容易受到异常值的影响。

本文面向的对象主要是陆地战车等军事化装备，它们行驶的路网与城市车辆道路路网不同，城市道路路网交汇点和分节点多，而且路网纵横交错，用来描述路网的数据库表现形式复杂[15-17]；而陆地战车所需要覆盖的路网较为单一，主要围绕营区或训练区域，路网数量和复杂程度有所降低。因此本文在路网匹配算法的基础上，根据实际使用情况，应用路网匹配算法与惯性组合导航历史数据相结合，考虑车辆行驶轨迹曲线的整体变化趋势，基于全局匹配的思想提出了惯性/里程计/路网匹配组合导航算法，解决了陆地战车自主导航相关技术问题。

1.2 改进匹配方案

首先，为了实现高精度匹配定位的可靠性，对路网数据库进行优化处理，即将路网数据处理成10m等间隔的点集合，并在此基础上进行后续路网匹配。

路网匹配以惯性/里程计组合导航历史轨迹数据作为输入，构建导航数据缓冲区，按照匹配准则获取匹配路段及匹配点。整个匹配过程及步骤如下：

步骤1：获取惯性/里程计组合导航定位点的位置信息，计算惯性定位行驶里程，若小于最短里程计要求(例如3km)，则不进行匹配；若满足最短里程要求，则开始匹配。

步骤2：首先在惯性/里程计组合导航定位点误差范围(例如100m)内搜寻待匹配的路网，从路网中第一条道路开始，遍历该条道路中所有点到导航定位点的距离，取距离最小值，若距离最小值大于误差范围(100m)，则该条道路不在待选择范围内；若距离最小值小于误差范围，则该条道路为待选择道路。遍历路网中的所有道路，列出所有待选择道路

Rds={Rd1,Rd2,Rd3，…，Rdn}

(1)

步骤3：针对一条待选择道路，将其中与定位点距离小于误差范围的点构成一个点集

Pts={Pt1,Pt2,Pt3，…，Ptn}

(2)

步骤4：从点集中第一个点开始，根据惯导的历史轨迹数据(例如数据长度3000m，同样需要对历史轨迹数据进行优化处理得到10m等间隔点集)与路网数据进行比较，构造惯导数据与路网数据的方位角差值函数和距离差值函数。方位角计算方法如式(3)所示。

(3)

其中，Δλ表示前后2个点的经度之差，ΔL表示前后2个点的纬度之差。惯导点与路网点的距离计算方法如下

(4)

步骤5：构造匹配准则，方位角差值函数值小于阈值，并且同时距离差值函数值也小于阈值。

步骤6：对路网数据库中每条道路的点集Pts按照匹配准则进行遍历，得到满足匹配准则的匹配点，一般情况下满足条件的匹配点不止一个，选择距离差值函数最小的点作为匹配点。如果不存在满足条件的匹配点，则匹配结果为空。

步骤7：匹配结束，匹配完成后根据路网匹配的结果修正惯导位置误差，以惯导位置与匹配点位置之差作为卡尔曼滤波器的量测输入值。

步骤8：进行卡尔曼滤波，使用滤波估计值校正惯导误差，并开始下一次匹配。

2 惯性/里程计/路网匹配组合导航

组合导航滤波器状态向量选取惯导姿态误差、速度误差、位置误差、陀螺误差、加速度计误差共15维向量，即

(5)

根据惯性导航误差方程得到状态转移矩阵。量测值的选取根据惯导位置与路网匹配位置来确定，即以惯导位置与路网匹配位置之差作为卡尔曼滤波器的量测输入

Zk=δP=δPSINS-δPMAP
=H(t)X+v(t)

(6)

式中v(t)表示量测噪声，且有

(7)

组合导航采用反馈校正的方式，通过滤波估计出姿态误差、速度误差、位置误差及器件误差后实时反馈修正惯导的姿态、速度、位置及器件参数，其结构原理如图 1所示。

图1 惯性/里程计/路网匹配组合导航结构Fig.1 Flow chart of SINS/odometer/road network matching integrated navigation

3 路网匹配试验验证

首先对高精度定位定向系统(Position and Orientation System, POS)采集的道路轨迹数据进行优化处理，得到等间隔的路网数据库，比较路网中前后点距离间隔如图 2所示。从图2中可以看出，路网数据库中各个点等间隔均匀分布。

图2 路网数据库间隔距离Fig.2 Distance of road network points

本文的路网匹配验证分2个步骤进行，首先仅根据惯性/里程计组合导航的结果进行路网匹配，匹配的结果不参与后续组合导航解算，即开环验证，主要目的是比较在组合导航基础上路网匹配的精度；第二步是根据组合导航的结果进行路网匹配，并将匹配结果进行反馈用于修正惯导误差，通过不断迭代和闭环修正减少惯导误差。

3.1 开环验证

选取一组定位导航试验数据，首先根据GNSS数据生成路网数据库，然后进行惯性/里程计组合导航解算，并得到组合结果。根据组合导航结果进行路网匹配，以采集的GNSS位置信息作为参考基准，比较组合导航结果与路网匹配结果，统计组合导航误差与路网匹配误差，组合导航轨迹及匹配误差如图 3所示。

(a)组合导航轨迹

(b)导航误差及匹配误差

(c)误差直方图图3 组合导航及路网匹配误差示意图(开环)Fig.3 Error of integrated navigation and road network matching (open loop revise)

根据组合导航结果及开环匹配结果，导航轨迹包含东西向及南北向路线，组合导航结果最大误差约40m，经过路网匹配后，根据导航点与匹配的误差曲线可以看出，匹配过程识别出了导航误差，从而匹配点与参考点误差大部分在10m左右。根据误差直方图3(c)可以看出，经过路网匹配后导航误差分布明显减小，相比于惯性/里程计组合导航结果，匹配效果显著。

3.2 闭环验证

闭环验证采用与开环验证同样的试验数据，匹配开始后根据匹配结果修正惯导误差及惯性器件误差，作为下一次惯导计算的初值，通过不断地迭代修正，得到全程的导航定位误差，试验结果如图 4所示。

(a)组合导航轨迹

(b)导航误差及匹配误差

(c)误差直方图图4 组合导航及路网匹配误差示意图(闭环)Fig.4 Errors of integrated navigation and road network matching(closed loop revise)

根据闭环试验验证结果，经过路网匹配对惯性导航结果的迭代修正后，惯性/里程计/路网匹配组合导航结果定位误差明显减小，大部分误差控制在10m左右。对比开环试验验证与闭环试验验证的误差直方图，将路网匹配与惯性组合导航相结合，能大幅度提高组合导航系统的精度。并且路网匹配与惯性组合导航相辅相成，路网匹配能够提供高精度的位置信息，用于修正惯性组合导航结果；而惯性组合导航通过匹配修正后能够减少误差，同样为路网匹配提供高精度的待匹配点位置信息，降低匹配误差。

惯性器件误差估计结果如图 5所示。从估计曲线可以看出，经过惯性/里程计/路网匹配组合导航后加表零偏误差与陀螺零偏误差估计稳定，间接表明了组合导航的性能。

图5 惯性器件误差估计曲线Fig.5 Estimation curve of sensor of IMU

为了进一步验证路网匹配的精度，本文选取一组长时间的试验数据，并且人为增大惯性/里程计组合导航的误差，得到路网匹配修正前后组合导航位置误差曲线如图 6所示。

根据路网匹配的试验结果，路网匹配修正前，惯性/里程计组合导航误差较大且不断发散，已无法提供准确的导航信息;经过路网匹配修正后，组合导航误差明显减小，且位置误差保持稳定，不存在发散趋势，进一步证明了路网匹配与惯性组合导航结合的优势。

(a)匹配前：惯性/里程计组合误差

(b)匹配后：惯性/里程计/路网匹配组合误差图6 路网匹配结果对比Fig.6 Relatively result of map matching

3.3 试验小结

本节试验过程中先后通过开环验证试验和闭环验证试验验证了路网匹配的精度，从不同侧面证明了路网匹配的精度及路网匹配与惯性组合导航相结合的优势，通过对比试验与不同试验数据的验证试验，证明了惯性/里程计/路网匹配组合导航的精度与性能。

4 结论

本文针对惯性/里程计组合导航定位精度易受环境影响的情况，提出了采用路网匹配方法与组合导航相结合，以提高导航定位精度的方法。首先对路网数据库进行优化处理，得到等间隔的路网点集；路网匹配采用组合导航的历史轨迹数据进行全局匹配，同时对历史轨迹数据进行等间隔处理，提高了匹配的精度与可靠度；经过匹配修正后，组合导航能够通过不断迭代修正定位误差，从而提高精度。通过试验验证表明，惯性/里程计/路网匹配组合导航的定位精度有大幅度提升，且可靠性高，是陆地车载自主导航系统的一种有效导航方式。