基于三次B样条多信息融合实时地图匹配方法

陈文杰，李浩，沈勇

(同济大学汽车学院，上海 201804)

0 引言

车辆智能驾驶、车道级导航、智能交通等，均要求运动载体有极高的定位精度以及可靠性，载体的高精度定位是实现智能化交通的核心问题之一[1-2]。目前车辆获取高精度位置一般通过GPS、实时动态定位技术(Real Time Kinematic, RTK)、惯性定位(Inertial Navigation System, INS)、航位推算(Dead Reckoning, DR)等之间相互组合获得[3-5]。这些组合定位技术或多或少均存在一定的定位误差，因此利用电子地图提供的绝对精度数据，通过地图匹配算法，能够减小定位误差，达到精确定位的目的。在地图匹配定位技术领域，有效的地图匹配算法可以显著地提高定位精度。常见的地图匹配算法有拓扑关系算法、模糊逻辑算法、概率统计算法、神经网络算法等[6-9]，这些算法要么匹配的精度不高，要么原理复杂，实现难度大，对硬件要求较高，不适用于实时地图匹配。实时地图匹配算法要求实时性、鲁棒性、精确性[10]。匹配算法的实时性主要体现在匹配候选区域的时间以及匹配规则的复杂度，鲁棒性体现在算法对于各种异常情况做出正确的判断和处理，精确性体现在导航定位误差、电子地图精度误差、算法计算误差。

文中根据实时地图匹配算法的要求，提出一种基于三次B样条的多信息融合实时地图匹配方法。首先对采集的高精度定位数据进行区域划分，接着采用带权约束三次B样条压缩算法对道路信息进行压缩，获得适用于基于三次B样条多信息融合匹配方法的控制点序列和节点矢量，创建适用于嵌入式定位系统的电子地图；然后求解三次B样条，以点间距离、航向、磁场、坡度、曲率因素为权重的参数值，进行地图匹配，并对地图匹配修正量进行更新和校正，最终实现车辆在道路上的实时高精度匹配定位。

1 创建适用嵌入式定位系统的电子地图

电子地图是指用数字的形式来描述地图要素，文中创建的电子地图的地图要素包含：定位位置、地磁基准及特征、坡度特征。创建适用于嵌入式定位系统的电子地图流程如图1所示，创建的过程分为3个部分：

(1)采集道路信息。采用高精度定位模块，利用GPS/RTK采集道路位置信息、磁强计采集道路磁场特征信息、加速度计采集道路坡度特征信息。

(2)道路区域划分。道路区域划分用于将道路信息的特征进行分组，以便于后续的地图匹配。

(3)道路信息压缩。采集的原始道路信息的数据量非常庞大，不仅存在不必要的信息冗余，而且对硬件的存储要求较高，同时不利于对电子地图的快速搜索，因此需要对采集的原始道路信息进行压缩。文中采用带权约束的三次B样条压缩算法，在保证一定精度范围的前提下，压缩率尽可能小。

图1 嵌入式定位系统电子地图绘制流程

1.1 道路区域划分

为了便于道路特征的分组，并且保证航向、曲率特征的唯一性，对道路进行区域划分，如图2所示，对同济大学嘉定校区部分道路进行区域划分，同时为了使轨迹点能够更快速且准确地搜索到所处道路区域，取每个区域的中心点作为该区域的位置标记。通过计算轨迹点到位置标记的距离，可以判定轨迹点处在哪个匹配区域。

图2 道路区域划分

1.2 带权约束三次B样条压缩算法

传统压缩算法有Douglas-Peucker压缩算法、分段多项式曲线压缩算法、垂距限值法、光栏法等[11]，然而这些压缩算法无法在较低压缩率的情况下，保证一定的精度。文中采用带权约束三次B样条压缩算法，该算法可以在较低压缩率的情况下，保证一定的精度，获得用于基于三次B样条多信息融合匹配方法的控制点序列和节点矢量。

1.2.1 算法实现过程

首先确定采集的道路信息数据所对应的参数值以及节点矢量：

(1)

其次根据曲率来确定各个数据点的约束权值，采用的方式如下：

(2)

式中：KC为各个数据点的约束权值；Cr为数据点的曲率大小；ρCr为数据点曲率变化率。

最后引入目标函数，求解压缩后的控制点序列。这里令S为非约束数据(数据点约束权值为1)集合，T为带权约束数据集合，W为各个数据权值的集合，N为非约束数据对应的基函数集合，M为带权约束数据的基函数集合，P为所求控制点集合。

带权约束三次B样条压缩算法的思想在于在满足约束条件MP=T的前提条件下，使得非约束方程组NP=S的残差S-NP的加权平方和最小。引入拉格朗日乘子A=[λi]，根据拉格朗日乘子法，得出如下目标函数：

f(P,A)=(ST-PTNT)W(S-NP)+AT(MP-T)

(3)

分别对A和P求导，并令其等于零，得到：

(4)

联立方程组可以先求得A,然后通过A求得P:

(5)

最终道路信息数据压缩为控制点序列P及节点矢量U，由P和U即可解压为一条逼近非约束数据项，并插值约束数据项的一条曲线：

(6)

为了验证带权约束三次B样条压缩算法的优点，将其与Douglas-Peucker压缩算法进行对比，如图3、图4所示，在压缩率均取5%的情况下，对区域1进行压缩，可以看出带权约束三次B样条压缩算法在距离误差和方向误差上较小，其距离误差的最大值为4.81 cm，方向误差的最大值为2.13°，并且其误差变化稳定，不存在突变。

图3 两种压缩算法距离误差比较

图4 两种压缩算法方向误差比较

1.2.2 压缩率的选取

压缩率选取得恰当与否直接影响到压缩的质量，过低的压缩率，无法保证一定的精度，而过高的压缩率又会导致数据的冗余。矢量数据压缩的原则为：在采用尽可能小压缩率的同时，保证较小的极差δM和离差σP[12]。

根据这个压缩原则，如图5、图6所示，采用带权约束三次B样条压缩算法对区域1以不同的压缩率(0～20%)，分别计算极差和离差。可以看出：当压缩率为4%时，极差和离差的大小已经满足了数据压缩精度的要求，并且其减小的趋势趋于平稳状态，再加大压缩率对于其减小的效果不明显。

对其他几个区域采用相同的方式得出极差、离差和压缩率的关系，可以得到相同的结果，故文中对道路信息数据采用4%压缩率带权约束三次B样条压缩算法，在保证较小极差、离差的前提下，以最小的压缩率压缩数据。

图5 区域1压缩率α和极差δM的关系

图6 区域1压缩率α和离差σP的关系

2 基于三次B样条多信息融合匹配方法

在车辆运行过程中，综合考虑定位系统输出的实时位置、航向、加速度、磁场以及由历史轨迹推出的轨迹曲率，作者设计了一种基于三次B样条的多信息融合实时匹配方法。算法的流程如图7所示，该算法分为3个阶段：

(1)初始阶段。由于在初始时刻，车辆处于由静止状态转为运动状态的过程。在静止状态，此时所得到的汽车航向信息并不准确，匹配区域的确定容易出现误判定，同时存储的历史轨迹序列在这一阶段为无效数据，由于后续轨迹点的匹配将用到初始匹配信息，为了保证后续轨迹点的匹配准确性，在车辆静止状态直接将轨迹点作为匹配点输出。在运动状态将进行匹配区域的确定以及历史数据的存储。

(2)匹配阶段。综合利用历史轨迹序列，得到航向变化率、曲率大小及变化率、坡度，对满足特征条件的轨迹点，确定以航向、曲率、坡度、点间距离、磁场强度为加权的参数值进行特征路段匹配；对于不满足特征条件的匹配点，首先对轨迹点进行预估校正，然后确定以点间距离和磁场强度加权的参数值进行非特征路段匹配。

(3)更新校正阶段。对地图匹配修正量e(k)的更新和校正用于减小轨迹点与车辆真实位置之间的偏差，从而提高匹配精度。当轨迹点处于特征路段匹配阶段时，由于此时的匹配精度较高，将对e(k)进行更新；而当轨迹点处于非特征路段匹配时，只对e(k)进行校正而不进行更新。

图7 地图匹配算法流程图

2.1 匹配区域确定

匹配初始阶段时，主要是确定车辆行驶的匹配区域。初始阶段下，为了减少误匹配，车辆静止状态下的轨迹点不进行匹配，直接将轨迹点作为匹配点输出。为了保证初始匹配区域的准确性，在车辆运动状态下，如果6个历史轨迹点的匹配区域均为一个区域，则确定该匹配区域为初始匹配区域。

匹配区域的确定。首先分别计算轨迹点到各个区域的距离权值、磁场权值以及方向权值，加权得出各匹配区域的总权值，取权值最大的区域作为匹配区域(这里约定当某项权值为负数时，该项权值为零)。

候选区域Ri的总权值WRi定义为：

WRi=WDi+WBi+WHi

(7)

式中：距离权值WDi表示轨迹点与各匹配区域的接近程度，接近程度越大则距离权值也越大；磁场权值WBi表示轨迹点的磁场与各区域的磁场基准的相近程度，越相近则磁场权值越大；方向权值WHi表示轨迹点的方向与各区域的投影点方向的一致程度。

距离权值定义为：

(8)

式中：Dmax是轨迹点到各匹配区域投影距离的最大值；Di是轨迹点到各匹配区域的投影距离；Dk是距离权值系数(文中Dk取0.4)。

磁场权值定义为：

(9)

式中：Bmax是轨迹点的磁场强度与各区域的磁场基准差值的最大值；Bi是轨迹点的磁场强度与各区域的磁场基准的差值；Bk是磁场权值系数(文中Bk取0.2)。

方向权值定义为：

WHi=(v-3)·|cos(Ac-Ai)|·εH·Hk

(10)

式中：v是车辆行驶速度；Ac是车辆轨迹的航迹角；Ai是轨迹点在各区域投影点的方向或反方向；εH是车辆航向角的变化率；Hk是方向权值系数(文中Hk取0.4)。在一定范围内，车速越快且车头朝向轨迹点对区域投影点的方向越接近，其方向权值越大。

2.2 地图匹配过程

为了保证存储历史轨迹点的连续性，同时节约存储空间，采用滑窗移动(窗口大小为30)的方式存储满足一定车速的(v≥3 m/s)历史轨迹点，当存储的历史轨迹序列中的轨迹点数目达到30，则判断该历史轨迹序列有效。当历史轨迹序列判断为无效以及从有效历史轨迹序列中得到的航向变化率、曲率大小及变化率、坡度不满足特征条件时，确定以点间距离、磁场强度为加权的参数值对非特征路段进行匹配；当从有效历史轨迹序列中得到的航向变化率、曲率大小及变化率、坡度满足特征条件时，确定以航向、曲率、坡度、点间距离、磁场强度为加权的参数值对特征路段进行匹配。

2.2.1 参数值的求解

参数值的求解如图8所示，这里对轨迹点采用三次B样条投影算法，其基本思想在于利用Newton下山法来最小化轨迹点特征T(k)(位置、航向变化率、曲率及变化率、磁场强度、坡度)和C(u)的距离，当其距离小于一个事先指定的精度ε，则认为C(u)为投影点，其u即为轨迹点特征的参数值。

图8 参数值求解示意图

利用数量积定义函数：

f(u)=C′(u)·[C(u)-T(k)]

(11)

式中：C′(u)为曲线在参数值u的一阶导矢；C(u)为投影点；T(k)为轨迹点特征。

当f(u)=0时，轨迹点特征T(k)到C(u)的距离达到最小，根据Newton下山法，可得迭代函数：

(12)

式中：ui表示第i次Newton下山法迭代所得到的参数值；λ为牛顿下山法迭代参数；C″(ui)是曲线在参数值ui处的二阶导矢。

如果f(ui+1)小于一个事先指定的精度(ε=1×10-5)，则ui+1为轨迹点特征的参数值。

考虑到车辆运行过程中总有一些偶然误差(例如信号的干扰等)，单独采用式(12)可能会导致参数值产生比较大的扰动。为了平滑这种扰动，采用前3个历史轨迹点特征所得到的参数值的平均值来修正：

(13)

2.2.2 非特征路段地图匹配

对于非特征路段，确定以点间距离和磁场强度为加权的参考值进行匹配。

图9 非特征路段匹配过程示意图

然后用该预测轨迹点确定点间距离以及磁场强度加权参数值：

u=WDuD+WMuM

(14)

式中：u为点间距离以及磁场强度加权参数值；WD为点间距离匹配权值；uD为预测轨迹点的点间距离参数值；WM为磁场强度匹配权值；uM为轨迹点磁场强度与区域磁场序列的磁场强度参数值。

WD、WM定义为：

(15)

式中：Dmin为预测轨迹点到匹配区域的最小距离。这样就保证了只有在点间距离相对较大的情况，才加大磁场序列匹配的权值。

最后获得匹配点C(u)同时对匹配修正量e(k)进行实时矫正，同样为了平滑这种扰动，采用前3个历史数据的平均值来修正：

(16)

2.2.3 特征路段地图匹配

由于位于特征路段的轨迹点特征非常明显，故而进行特征路段的匹配时，不需要获得该时刻的预测轨迹点。

对特征路段的轨迹点直接以点间距离、航向、曲率、坡度、磁场为加权的参数值进行匹配。

u=WDuD+WMuM+WHuH+WCuC+WSuS

(17)

式中：u为点间距离、航向、曲率、坡度、磁场为加权的参数值；WD、WM、WH、WC、WS分别为点间距离匹配权值、磁场强度匹配权值、航向匹配权值、曲率匹配权值、坡度匹配权值；uD、uM、uH、uC、uS分别为点间距离参数值、磁场强度参数值、航向参数值、曲率参数值、坡道参数值。

WD、WH的选取与非特征路段匹配一致，只是其占比仅为10%。WH、WC、WS定义为：

(18)

式中：ρH为航向变化率；Cr为曲率的大小；ρCr为曲率的变化率；αS为坡度。对WD、WM、WH、WC、WS进行归一化处理，得到带权的参数值u，最后获得特征匹配点CT(u)。

通过特征路段匹配得到的匹配点相对而言比较准确，故而在从特征路段过渡到非特征路段过程中，应对地图匹配修正量进行更新。其更新方式如式(19)所示：

enew(k)=CT(u)-g(k)

(19)

式中：enew(k)是更新后的地图匹配修正量；CT(u)为特征匹配点；g(k)为轨迹点。

3 实验结果及分析

为了验证文中提出的实时地图匹配算法的匹配精度，首先将同济大学嘉定校区部分道路的电子地图以及地图匹配算法烧录到DSP中，进行实车实验。其中DSP芯片为F28035，GPS接收机为u-blox NEO-M8N，加速度计为MEMS三轴加速度计，陀螺仪为MEMS三轴磁力计。同时为了计算匹配精度，将在同一位置安装一高精度RTK接收机。实验结果如图10所示，ublox轨迹均匹配到相应的路网，其匹配轨迹和高精度RTK轨迹基本重合，绝大部分匹配点的误差为±10 cm，可以看出文中的实时地图匹配算法在确定匹配区域以及轨迹点的位置匹配时效果理想，能够真实反映出车辆运行的轨迹，实现了实时高精度定位。

图10 同济大学嘉定校区实车匹配局部放大结果

参考文献:

[1]AMINI A,VAGHEFI R M,de la GARZA J M,et al.Improving GPS-based Vehicle Positioning for Intelligent Transportation Systems[C]//Intelligent Vehicles Symposium.Michigan,2014:1023-1029.

[2]TOLEDO-MOREO R,BETAILLE D,PEYRET F,et al.Fusing GNSS,Dead-reckoning,and Enhanced Maps for Road Vehicle Lane-level Navigation[J].IEEE Journal of Selected Topics in Signal Processing,2009,3(5):798-809.

[3]JI C,DU X.GPS RTK Applied in Land Reclamation in Mining Area[C]//International Conference on Remote Sensing,Environment and Transportation Engineering.IEEE,2012:1-4.

[4]LEE I,LI H,HOANG N,et al.Navigation System Development of the Underwater Vehicles Using the GPS/INS Sensor Fusion[C]//International Conference on Control,Automation and System.Gyeonggi-do,2014:610-612.

[5]LU W,ZHANG Y.Research on Algorithm of GPS/DR Integrated

Vehicle Navigation System[C]//International Conference on Electric Information & Control Engineering.Wuhan，2011:6056-6060.

[6]SANTOS M M D,BALLESTER P,ZAFFARI G B,et al.A Topological Descriptor of Acoustic Images for Navigation and Mapping[C]//Latin American Robotics Symposium.Uberlandia,2016:1-6.

[7]YANG Y,YE H,FEI S.Integrated Map-matching Algorithm Based on Fuzzy Logic and Dead Reckoning[C]//International Conference on Control Automation & Systems.Gyeonggi-do，2010:1139-1142.

[8]ZHANG B，TAO B，GAO L，et al.Matching Probability of Visible Image Based on the Side Information of Buildings[C]//IEEI Chinese Guidance,Navigation & Control Conference.Nanjing,2017:878-880.

[9]WINTER M,TAYLOR G.Modular Neural Networks for Map-matched GPS Positioning[C]//International Conference on Web Information Systems Engineering Workshops.Roma，2003:106-111.

[10]李星军.车辆定位导航系统中地图匹配算法研究[D].西安：西安电子科技大学软件工程,2015.

[11]杨建宇,杨崇俊,明冬萍,等.WebGIS系统中矢量数据的压缩与化简方法综述[J].计算机工程与应用,2004，40(32):36-38.

YANG J Y,YANG C J,MING D P,et al.Review on Vector Data Compression and Simplification of WebGIS[J].Computer Engineering and Applications,2004，40(32):36-38.

[12]刘文强,杨海忠.一种应用于电子地图道路特征点提取的新方法[J].科学技术与工程,2012,12(30):7911-7914.

LIU W Q,YANG H Z.A New Method of Road Feature Point Extraction Application in Electronic Map[J].Science Technology and Engineering,2012,12(30):7911-7914.