基于位置复合匹配的惯性/地图匹配组合定位算法

张新岗，韩勇强，谢志超，张路成，黄吴淼

（北京理工大学自动化学院，北京 100081）

惯性导航系统（Inertial Navigation System，INS）凭借其完全自主、高稳定性、大带宽和导航信息丰富等优势，在陆用移动平台的导航领域占据着重要地位。但惯性导航存在定位误差随时间积累的问题，当引入其他方法对其进行辅助校正时，其可在保持独立连续制导的同时，准确地进行定位。常用的方法有全球定位系统、里程计技术、航位推算及地图匹配技术等。近年来，随着定位技术和位置服务应用的发展，高精度电子地图的精度达到厘米级。地图匹配技术的研究不断推进，且其作为一种软件技术，不需要添加新硬件，在低成本的同时可以有效地抑制惯性导航的误差发散。利用地图匹配技术辅助惯性导航已经成为当前研究的热点问题。

针对这一问题，国内外学者相继提出了许多地图匹配算法[1]。地图匹配的种类可分为几何特征匹配[2]、概率统计[3]、权重算法[4]、D-S 证据推理[5]、贝叶斯[6]以及高级地图匹配[7]等。但是上述方法主要对全球定位系统（Global Position System,GPS）数据进行处理，与INS 结合的方法较少。邓志红等[8]根据惯导输出的信息引入“平移向量”的概念，使用了改进的投影匹配法对INS 误差进行修正；
李磊磊等[9]使用多种不同的地图匹配算法，对惯导提供的导航信息和车辆行驶的道路进行对比，根据不同的道路情景选择不同的匹配算法，但是该方法对于道路特征的划分有着严格的要求，较为复杂的地段匹配效果不高；
刘峰等[10]采用道路的几何特征对惯导轨迹进行修正，放弃了点到线式的点匹配方法，全程采用线到线式的段匹配法，计算量较大，匹配实时性不高；
Han 等[11]则采用了点匹配方法，且结合光栏法对轨迹路线进行了压缩，匹配速度有较大的提升，但是匹配效果不佳，容易出现“野点”，匹配错误率较高。

不同于GPS 数据，车辆在使用INS 导航时，短时间内惯性导航的轨迹往往为一段连续平滑的曲线，且轨迹与实际路线的一致性较高。根据这一特点，本文设计了一种“点段”结合的地图匹配方法：在道路特征不明显的路段使用点匹配，既能保证较高的精度，又能降低计算量，提高算法效率；
在岔路或者复杂路段引入“裁决系统”，充分利用历史匹配信息，对相似的匹配路径进行段对比，既能够弥补点匹配容易出现误匹配的缺陷，同时又降低了整体的计算量，在保证实时性的同时有着更强的鲁棒性。

2.1 基于Light-Barrier 法的特征点检测

电子地图中的路网信息是通过连续的折线近似为道路曲线，采样点一定程度上可以表征路网的几何形状：在直线路线中，采样点分布较为稀疏；
而在转弯，岔路等复杂路线中，采样点分布较为密集。在这种情况下，路段的几何特征较为明显。在对定位轨迹和路网对比前，可以在尽可能保证原有轨迹特征没有过多改变的同时，对定位轨迹进行数据压缩。本文采用Light-Barrier 法[11]检测惯导轨迹中的特征点，其反映了轨迹局部的变化特征，可以在维持曲线本身形状的同时压缩大量数据，降低计算量，保证算法实时性。Light-Barrier 原理如图1 所示，假设p1p2～pn为某路线轨迹上的采样点，特征点的具体的识别过程如下。

图1 Light-Barrier 法原理图Fig. 1 Schematic diagram of the Light-Barrier

（1）确定光栏的阈值d，连接p1p2，过p2作该线段的垂线，在垂线上取两个点，记为a1和a2，满足a1p2=a2p2=d/2，以p1作为顶点，p1a1、p1a2分别作为两条边，形成的扇形区域称为光栏。

（2）判断下一个点p3是否存在于该扇形区域内。p3处于该光栏内，则舍去p2。再连接p1p3，过p3作线段p1p3的垂线，这条垂线和第一步中定义的光栏交于c1和c2。再在这条垂线上取两个点，记为b1和b2，满足b1p3=b2p3=d/2。从b1、c1和b2、c2中分别选择距离p3最近的两个点（图中选择的是b1、c2）定义为新光栏。

（3）重复步骤（2），当有采样点位于定义的光栏外时，进行步骤（4）。

（4）当采样点位于光栏外，如图中的p4点，此时将把p3点作为特征点进行保留，并且将p3作为新的起点，重复步骤（1）～（3）。

2.2 原始INS 轨迹的收束校正

虽然随着时间的增加，车辆装载惯性导航系统的导航位置会出现偏差，但是它输出的位置仍然是连续变化的。因此，可以通过匹配点和特征点以及导航初始点对INS 输出的位置进行收束校正处理，收束后的位置会更偏向于车辆的真实位置，从而可以极大地缩小候选道路的搜索范围，提高效率。

INS 的轨迹、匹配点和初始点的位置关系可以用旋转参数φ和拉伸参数λ表示，其中旋转参数φ可用方向夹角[7]进行求取，如图2 所示。惯导轨迹与路网道路与北向夹角可分别表示为1φ、2φ，拉伸参数λ可用轨迹点、匹配点分别与初始点间的欧式距离dP0P1、dP0P1"的比值求取

图2 INS 轨迹收束Fig. 2 Bundle of INS trajectories

由惯导轨迹点的原始北向角0α以及与起始点的原始距离0D推算收束校正后的待匹配点的北向角α及距离D

进而可推导待匹配点所在经度和纬度方向上的长度为

进一步推导待匹配点所在纬度的纬度圈半径，如图3 所示。a

图3 纬度圈半径计算Fig. 3 Latitude circle radius calculation

R代表长轴（赤道）半径，bR表短轴半径，PI代表圆周率，由于地球是一个近似球体，假设初始点纬度为La0t，经度为Lo0n，则球半径的长度cE及纬度圈的半径dE为

根据以上求得的球半径和纬度圈半径，可以进一步推导经度跨度和纬度跨度，进而求出收束校正后的经纬度坐标

2.3 候选道路的搜索

电子矢量地图中的道路纷繁复杂，搜索道路的基本思路是：确定车辆当前的定位点，在能够确保正确道路在搜索范围内的同时，尽可能地减少候选道路的数量，在后续对比时提高匹配效率。以收束校正后的待匹配点作为搜索中心，构建搜索圈。如图4 所示，搜索半径初始值R0= 50 m ，将搜索圈中包含的所有道路编号及方向进行保存。若搜索圈内没有道路，逐次增加搜索半径ΔR= 30 m 。扩大半径两次后若仍没有道路，则等待下一个INS 信息重新搜索。搜索圈半径设计为

图4 候选道路示意图Fig. 4 Schematic diagram of candidate roads

在预定的范围内将所有路径搜索后，需对搜索到的道路进行进一步筛选，选出车辆最有可能在的道路。影响候选道路选择的因素主要有以下两个。

（1）待匹配点的航向与N条道路方向的夹角βi(i=1,2,3, …,N)。如图5 所示，计算航向角与道路方向夹角的公式为

图5 航向角与道路方向夹角Fig. 5 Angle between Heading and road position

式(7)中，Ha为待匹配点的航向角，ir为对应第i条道路，而ih代表第i条道路的方向，定义iβ为航向角与道路平移向量的夹角。

在预定的范围内搜索出所有路径后，可将定位路线和行驶路线航向角相差过大的道路，缩小候选道路范围，如图4 所示，路线1r和8r为选择出的候选道路。在此基础上，为了保证候选道路中包含正确路径，对剩余道路的两端分别进行延伸扩展，延伸距离以特征点间距为参考进行自适应调整，特征点密集时距离缩小，特征点稀疏时距离延长。

（2）待匹配点与N条候选道路的投影距离δi(i=1,2,3, …,N)。通过βi确定道路方向基本正确的候选道路后，计算垂直投影待匹配点与候选道路的投影距离δi，如图6 所示。针对待匹配点与道路可能会出现的 3 种情况用点线相关度进行区分处理。如图7 所示，假设定位点P坐标为（x,y），AB路线的两个首尾点的坐标分别记为A(x1,y1),B(x2,y2)，那么点P和路线AB的点线相关度[12]表达式为

图6 垂直投影Fig. 6 Vertical projection

图7 点线相关度Fig. 7 Correlation between points and lines

2.4 候选道路加权选择

在求得候选道路的投影距离iδ与道路夹角iβ后，使用零均值的高斯分布概率密度函数来表征两个因素对于候选道路准确程度的价值[13]，其公式为

此外，考虑到定位轨迹与路网路线的一致性较高的特点，通过判断INS 轨迹与路网的相关性来对候选道路进行加权处理。设车载导航输出的轨迹为W= [w1,w2, … ,wk]，其中wk包含每个点的经纬度，即wk=（Lon k,Latk），候选道路为ri(i=1,2, …,N)，则轨迹与路网的相关性系数为

式中，k为采样点序号，σW,σri分别为轨迹序列W和路网序列ri(k)的标准差。p(εi)的值越大代表两条轨迹的相关性越强，车辆更可能在该道路上行驶。

最终，计算各候选道路的匹配加权值

式中，Γi为第i条道路的加权值，ρ1,ρ2,ρ3分别为距离和夹角以及相关性的权值系数，可以根据待匹配点所在的路网结构的复杂程度进行动态调整：在路网较为单一，搜索圈内确定的候选道路较少且平行路段（道路方向角变化程度小）较多时，取ρ1=0.45,ρ2=0.25,ρ3=0.3；
而当路网复杂，候选道路数量较多且路段中有较多弯路（道路方向角变化程度较大）时，取ρ1=0.2,ρ2=0.5,ρ3=0.3。Γ值越大，代表车辆位于该道路上的可能性更高。

由此，在匹配的过程中，计算出N条符合条件的候选道路的匹配权值Γi(i= 1,2, …,N)，然后找出最大权值Γmax与次大权值Γslv[14]。将Γmax与设定的阈值Uth1和Uth2进行比较，（其中，Uth1为判断路段可信度所设计的阈值，Uth2为判断两条路段是否相近匹配所设计的阈值，取Uth1=0.85，Uth2=0.15）。若Γmax≥Uth1且Γmax-Γslv＞Uth2，证明权值Γmax对应的道路为最佳匹配道路，将Γmax对应的路段作为当前定位点的匹配路段，相应的投影点作为最终的匹配位置。

2.5 裁决系统

车辆在行驶到两条或多条道路汇集的岔路口时，存在Γmax-Γslv＜Uth2的情景，这极易导致部分定位点出现错误匹配，此时单独的点匹配的方式已经不再适用。如图8 所示，车辆在R1道路上行驶，在经过岔路口时，由于曲率较小，在岔口附近的两条路相似度较高，出现了先投影到R2道路上（图中所示的红色点为错误匹配点，理论匹配点应为蓝色虚点），后续又再次回到R1道路上的情况。

图8 岔路错误匹配Fig. 8 False matching at a fork in the road

为了解决这个问题，本文引入“裁决系统”，对两条相似程度较高的路线通过段匹配的方式进行进一步裁决，确定最合适的道路。本文使用Fréchet 距离[15]对道路进行裁决判断，如图9 所示，两条曲线通过判断Fréchet 距离的大小来判断相似性。Fréchet 距离越小，代表两条路线的相似性越高，取最小的候选道路为最终的匹配路段。

图9 Fréchet 距离Fig. 9 Fréchet distance

规则：当采样点到达岔路交界处时，进入裁决系统，开始记录后续车载导航的输出轨迹A与候选路径Bi(i= 1,2)，每次采样的离散遍历区间为［ 0,1］ ，得到离散的采样点，分别记为A(μ(t)),Bi(η(t))，对于路径A和Bi的Fréchet 距离定义为

通过计算，得到轨迹A与候选道路iB的Fréchet 距离，选择相对Fréchet 距离较小的候选道路作为匹配路段，将所有的轨迹点直接投影到该匹配路段上后，退出裁决系统。由此，在遇到相似路段时，可以在不增加过多计算量的同时，提高匹配准确度。

2.6 匹配过程

车载导航行驶过程中输出INS 信息，由设备接收后首先对原始轨迹进行收束校正处理，得到接近真实位置的待匹配点。通过Light-Barrier 对轨迹进行特征点选择，若为特征点，在投影后更新旋转参数φ和拉伸参数λ；
非特征点根据上一次投影后的点确定下一个待匹配点。在确定待匹配点后进行道路搜索，并计算投影距离、夹角及相关性等匹配参数，根据加权公式得到每条道路的加权值Γi，通过设定的阈值判断该道路是否可以作为最佳匹配道路。遇到两条道路极为相似的情况，采用裁决系统对道路进行进一步判断，提高道路匹配的准确度，匹配过程如图10 所示。

图10 匹配过程流程图Fig. 10 Flow chart of matching process

为了检验该方法对惯性导航累积误差的抑制效果和投影准确情况，在北京部分地区进行了跑车试验。试验车搭载某型号激光陀螺双轴转位捷联惯性导航系统（Strap-down Inertial Navigation System, SINS），陀螺仪与加速度计参数如表1 所示。实验路线为三环内北京理工大学6 号楼至六环的北清路的往返路线，全长约 55 km，耗时约1.2 h，平均速度约50 km/h，采样数据点接近4400，以随车 GPS 输出的位置作为车辆所处的真实参考位置。采用MapInfo 格式的北京市电子地图，使用MapX4.5 平台插件进行算法测试。

表1 惯导系统参数Table 1 Inertial navigation system parameters

3.1 全程路段分析

将车辆行驶全程的INS 轨迹（红色）与匹配后的轨迹（蓝色）进行对比，如图11 所示，随着时间的增加，INS 轨迹已经逐渐偏离正确的行驶道路，经过算法校正后的轨迹基本符合实际运行路线。随着时间推移，惯性导航的误差逐渐发散，而匹配误差呈现上下浮动的现象，如图12 所示，整体误差都保持在15 m 左右。

图11 全程路段匹配图Fig. 11 Whole road section matching map

数据处理结果如表2 所示：惯性导航的平均误差值远远高于匹配误差，并且匹配误差的标准差远远低于惯性导航误差，说明经过地图匹配算法校正后，惯性导航的累积误差被极大地抑制并维持在较小的误差范围内，证明该算法的稳定性良好，对惯性导航信息能够实现实时地补偿修正。

表2 误差对比Table 2 Error comparison

3.2 特殊路段分析

如图13 所示，校正后的转弯路线和地图数据十分契合，反观惯性导航的轨迹，已经偏离正确路线较远。车辆运行到复杂路段时，惯性导航轨迹已经严重偏离正确路线，而经过算法改良后可以准确地找到所在道路。图14 给出了裁决系统的成功应用场景，图中显示车辆在进入岔路时出现了误匹配，但是经过裁决系统后成功投影回正确的道路上，证明了裁决系统在解决相似道路投影错误的问题上有较好的效果，体现了该方法具有较强的鲁棒性。

图13 转弯路段（左）及复杂路段（右）匹配图Fig. 13 Turning section (left) and complex road section (right) matching map

图14 裁决系统应用场景Fig. 14 Application scenario of adjudication system

跑车试验证明，该算法可以有效地适应不同类型的道路，抑制惯性导航系统的误差发散，具有较高的匹配精度。

随着高精度电子地图研究的蓬勃发展，地图匹配作为一种软件手段，可有效辅助陆用惯性导航系统实现长时间大范围、卫星拒止环境下高精度自主精确定位定向。本文设计了一种点段复合匹配的地图匹配方法，通过实际车载实验证明，该方法在保持导航系统自主性的同时，能够有效地抑制惯性导航误差的发散，对惯性导航产生的漂移误差有较好的补偿修正效果。当前的地图匹配领域研究多集中在矢量道路平面几何特征层面，对三维特征、拓扑关系、人工智能等方面的研究较少。在后续研究中，可综合考虑地图构成、行驶规则等多维度匹配方法以进一步提高地图匹配的准确性。