CN112729336A - 一种基于高精度矢量地图的车道级导航定位评价方法 - Google Patents

Abstract

本发明公开了一种基于高精度矢量地图的车道级导航定位评价方法，首先，在极坐标系下设立原点，确定极轴和正方向；然后，高精度矢量地图车道中心线上的点利用极坐标表示，将车载导航传感器获得的自动驾驶汽车的地理位置通过坐标转换利用极坐标表示；接着，利用车道中心线信息辅助匹配定位，建立极坐标表征函数；最后，利用匹配算法获取当前时刻定位结果给出车载导航定位精度评价，当前时刻定位精度评价结束后，进行下一时刻车载导航定位精度评价，直到整个驾驶过程结束，从而做出全程导航定位精度评价。上述评价方法，可以显著提高自动驾驶汽车的车载导航定位精度，实现厘米级的定位要求，从而可以利用高精度的位姿信息来实现汽车的自主驾驶。

Description

一种基于高精度矢量地图的车道级导航定位评价方法

技术领域

本发明涉及车载导航定位技术领域，尤其涉及一种基于高精度矢量地图的车道级导航定位评价方法。

背景技术

随着汽车驾驶自动化、智能化技术的不断发展，驾驶领域对车载传感器的性能要求越来越高，尤其对车载导航的定位精度要求越来越高。因此，需要建立车载导航定位精度评价指标，以此来衡量车载导航的定位性能。

自动驾驶技术的兴起对汽车导航的精度要求越来越高，需要对车载移动测量系统获取的导航数据产品精度做出合理评价。车载导航是为汽车提供具体位姿信息的必要手段，尤其是自动驾驶汽车，需要利用高精度的位姿信息来实现汽车的自主驾驶。汽车在路径规划与决策方面，对导航精度的依赖性很强，若车载导航定位精度过低，自动驾驶汽车将可能导致行为决策的判断失误，甚至导致交通事故的发生。因此，为避免这类型事件的发生，需要建立一个完整的汽车导航定位精度评价指标来衡量车载导航系统的定位性能。这就需要有一个评价手段在汽车出厂前对汽车导航性能进行合理的评估，为自动驾驶提供精度测试和精度评价参数等可靠性依据，建立一个完整的车载导航精度评价体系。

一个完整的导航精度评价体系是促进自动驾驶汽车发展的必要条件，提供一套完整的自动驾驶车道级导航定位精度评价指标，不仅是对汽车导航性能的评估，还可以为购买汽车的消费者提供一个重要参考指标。因此，这一技术的研究是不可或缺的。

发明内容

有鉴于此，本发明提供了一种基于高精度矢量地图的车道级导航定位评价方法，用以满足自动驾驶汽车行驶过程中的导航定位精度要求。

本发明提供的一种基于高精度矢量地图的车道级导航定位评价方法，包括如下步骤：

S1：将高精度矢量地图中整条车道中心线的数据提取出来，对所述整条车道中心线的数据进行等距离间隔采样，将等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组；

S2：对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组；

S3：将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组，将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组；

S4：根据车载导航传感器获取的当前时刻自动驾驶汽车所在位置，对车载导航误差进行粗估计，确定匹配搜索区域；其中，所述匹配搜索区域的长度为当前时刻车载导航误差的粗估计值，所述匹配搜索区域的宽度为当前时刻自动驾驶汽车所在道路的宽度；

S5：将所述第一表征函数数组中与所述匹配搜索区域对应的表征函数提取出来构成匹配序列，在所述第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长的表征函数构成搜索序列；

S6：利用所述搜索序列对所述匹配搜索区域进行遍历搜索，计算所述搜索序列与所述匹配序列的相似度；

S7：在遍历搜索结束后，将获得的所有相似度组成相似度数组，在所述相似度数组中选取数值最小的相似度对应的匹配搜索位置作为当前时刻的最佳匹配搜索位置；

S8：从所述匹配序列中提取与所述最佳匹配搜索位置对应的表征函数序列，根据提取出的表征函数序列获得所述最佳匹配搜索位置对应的极坐标序列，从所述极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标，并将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标；

S9：从所述极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据，并将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据；

S10：将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差，并根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方误差；

S11：返回步骤S4，重复执行步骤S4～步骤S10，计算下一时刻车载导航定位精度的均方误差，直至完成对整条车道上所有时刻车载导航定位精度均方误差的计算；

S12：根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差，完成对整条车道车载导航定位精度的评价。

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S1，将高精度矢量地图中整条车道中心线的数据提取出来，对所述整条车道中心线的数据进行等距离间隔采样，将等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组，具体包括：

将高精度矢量地图中整条车道中心线的数据提取出来，对所述整条车道中心线的数据进行等距离间隔采样，将n个等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中得到：

map[(lon₁,lat₁) (lon₂,lat₂) (lon₃,lat₃) … (lon_n-1,lat_n-1) (lon_n,lat_n)] (1)

其中，(lon_i,lat_i)表示第i个等距离间隔采样点的地理坐标，i＝1,2,…,n；

设原点坐标为(lon₀,lat₀)，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组：

map[(r₁,θ₁) (r₂,θ₂) (r₃,θ₃) ... (r_n-1,θ_n-1) (r_n,θ_n)] (2)

其中，

其中，(r_i,θ_i)表示第i个等距离间隔采样点的极坐标；r_i表示第i个等距离间隔采样点在极坐标系下的极径，θ_i表示第i个等距离间隔采样点的极径与极轴的夹角；Δx_i表示第i个等距离间隔采样点与原点在经度方向上的距离，Δy_i表示第i个等距离间隔采样点与原点在纬度方向上的距离；R_N表示地球经度圈半径，R_M表示地球纬度圈半径。

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S2，对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组，具体包括：

对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，得到：

nav[(Lon₁,Lat₁) (Lon₂,Lat₂) (Lon₃,Lat₃) … (Lon_m-1,Lat_m-1) (Lon_m,Lat_m)] (5)

其中，(Lon_j,Lat_j)表示第j个等时间间隔采样点的地理坐标，j＝1,2,…,m；

将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组：

nav[(R₁,Θ₁) (R₂,Θ₂) (R₃,Θ₃) … (R_n-1,Θ_n-1) (R_n,Θ_n)] (6)

其中，

其中，(R_j,Θ_j)表示第j个等时间间隔采样点的极坐标；R_j表示第j个等时间间隔采样点在极坐标系下的极径，Θ_j表示第j个等时间间隔采样点的极径与极轴的夹角；Δx_j表示第j个等时间间隔采样点与原点在经度方向上的距离，Δy_j表示第j个等时间间隔采样点与原点在纬度方向上的距离。

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S3，将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组，将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组，具体包括：

将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组：

map[P₁(r₁,θ₁) P₂(r₂,θ₂) P₃(r₃,θ₃) ... P_n-1(r_n-1,θ_n-1) P_n(r_n,θ_n)] (9)

其中，P_i(r_i,θ_i)＝r_iθ_i；

将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组：

nav[P₁(R₁,Θ₁) P₂(R₂,Θ₂) P₃(R₃,Θ₃) … P_m-1(R_m-1,Θ_m-1) P_m(R_m,Θ_m)] (10)

其中，P_j(R_j,Θ_j)＝R_jΘ_j。

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，设步骤S4确定的匹配搜索区域为a×b，其中，a为当前时刻车载导航误差的粗估计值，b为当前时刻自动驾驶汽车所在道路的宽度；

步骤S5，将所述第一表征函数数组中与所述匹配搜索区域对应的表征函数提取出来构成匹配序列，在所述第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长的表征函数构成搜索序列，具体包括：

将所述第一表征函数数组中与所述匹配搜索区域对应的表征函数提取出来，构成匹配序列：

[P_1,1 P_1,2 P_1,3 ... P_1,a×b-1 P_1,a×b] (11)

其中，a×b<n；

在所述第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长为M的表征函数，构成搜索序列：

[P_2,1 P_2,2 P_2,3 ... P_2,M-1 P_2,M] (12)

其中，P_2,1表示所述第二表征函数数组中与当前时刻对应的表征函数；M<m且M<a×b。

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S6，利用所述搜索序列对所述匹配搜索区域进行遍历搜索，计算所述搜索序列与所述匹配序列的相似度，具体包括：

利用下式计算所述搜索序列与所述匹配序列的相似度：

其中，l表示所述搜索序列在所述匹配搜索区域内平移的次数，l＝a×b；h＝1,2,…,M。

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S8，从所述匹配序列中提取与所述最佳匹配搜索位置对应的表征函数序列，根据提取出的表征函数序列获得所述最佳匹配搜索位置对应的极坐标序列，从所述极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标，并将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标，具体包括：

从所述匹配序列中提取与所述最佳匹配搜索位置对应的表征函数序列：

其中，k表示所述搜索序列在所述匹配搜索区域内到达所述最佳匹配搜索位置所平移的次数；t表示当前时刻；P_t ^k表示在平移搜索k次后获得的当前时刻最佳匹配搜索位置的表征函数；

根据提取出的表征函数序列获得所述最佳匹配搜索位置对应的极坐标序列：

其中，

q＝t,t-1,t-2,...,t-M+2,t-M+1；

从所述极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标为

将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标：

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S9，从所述极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据，并将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据，具体包括：

从所述极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据(R_t,Θ_t)，将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据：

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S10，将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差，并根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方差，具体包括：

将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差：

其中，Δx_t表示当前时刻经度方向的定位误差；Δy_t表示当前时刻纬度方向的定位误差；

根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方误差：

在一种可能的实现方式中，在本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法中，步骤S12，根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差，完成对整条车道车载导航定位精度的评价，具体包括：

根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差：

其中，T表示自动驾驶汽车经过整条车道所花费的时间。

本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法，首先，在极坐标系下，设立一个公共原点，并确定一条极轴，规定逆时针为正方向；然后，高精度矢量地图车道中心线上的点便可以利用极坐标唯一表示，并且，利用车载导航传感器获得的自动驾驶汽车的地理位置，也可以通过坐标转换，利用极坐标表示；接着，为利用车道中心线信息辅助匹配定位，建立极坐标表征函数；最后，利用匹配算法获取的当前时刻定位结果给出车载导航定位精度评价，当前时刻的车载导航定位精度评价结束后，进行下一时刻车载导航定位精度的评价，直到整个驾驶过程结束，从而做出车载导航系统的全程导航精度评价。上述评价方法，可以显著提高自动驾驶汽车的车载导航定位精度，实现厘米级的定位要求，从而可以利用高精度的位姿信息来实现汽车的自主驾驶。在汽车的路径规划与决策方面，高精度的车载导航定位技术可以辅助自动驾驶汽车做出正确的行为决策，从而避免交通事故的发生。

附图说明

图1为本发明提供的一种基于高精度矢量地图的车道级导航定位评价方法的流程图；

图2为极坐标系下位置信息的表征示意图。

具体实施方式

下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整的描述，显然，所描述的实施方式仅仅是作为例示，并非用于限制本发明。

本发明提供的一种基于高精度矢量地图的车道级导航定位评价方法，如图1所示，包括如下步骤：

S1：将高精度矢量地图中整条车道中心线的数据提取出来，对整条车道中心线的数据进行等距离间隔采样，将等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组；

S2：对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组；

S3：将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组，将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组；

S4：根据车载导航传感器获取的当前时刻自动驾驶汽车所在位置，对车载导航误差进行粗估计，确定匹配搜索区域；其中，匹配搜索区域的长度为当前时刻车载导航误差的粗估计值，匹配搜索区域的宽度为当前时刻自动驾驶汽车所在道路的宽度；

S5：将第一表征函数数组中与匹配搜索区域对应的表征函数提取出来构成匹配序列，在第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长的表征函数构成搜索序列；

S6：利用搜索序列对匹配搜索区域进行遍历搜索，计算搜索序列与匹配序列的相似度；

S7：在遍历搜索结束后，将获得的所有相似度组成相似度数组，在相似度数组中选取数值最小的相似度对应的匹配搜索位置作为当前时刻的最佳匹配搜索位置；

S8：从匹配序列中提取与最佳匹配搜索位置对应的表征函数序列，根据提取出的表征函数序列获得最佳匹配搜索位置对应的极坐标序列，从极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标，并将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标；

S9：从极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据，并将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据；

S10：将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差，并根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方误差；

S11：返回步骤S4，重复执行步骤S4～步骤S10，计算下一时刻车载导航定位精度的均方误差，直至完成对整条车道上所有时刻车载导航定位精度均方误差的计算；

S12：根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差，完成对整条车道车载导航定位精度的评价。

下面通过一个具体的实施例对本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法的具体实施进行详细说明。

实施例1：

在介绍实施例1的方法步骤前，先介绍下极坐标系下位置信息的表征原理，如图2所示，在平面内取一个定点O为极点，在极点O的向正北方向引一条射线OX为极轴，确定一个长度单位(比如，以米为一个单位步长)和正方向(通常选取逆时针)，对于平面内任意一点M，利用有限长线段r和∠MOX(即θ)就可以唯一表示在平面内M点相对极点O的唯一位置关系，即在极坐标系下平面内任意一点M的表示方法为：(r,θ)。

第一步：获取极坐标系下的车道中心线数据数组。

具体地，将高精度矢量地图中整条车道中心线的数据提取出来，综合考虑数据存储问题以及极坐标地图的制作成本问题，对整条车道中心线的数据进行等距离间隔采样，将n个等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中得到：

map[(lon₁,lat₁) (lon₂,lat₂) (lon₃,lat₃) … (lon_n-1,lat_n-1) (lon_n,lat_n)] (1)

其中，(lon_i,lat_i)表示第i个等距离间隔采样点的地理坐标，i＝1,2,…,n；

设原点坐标为(lon₀,lat₀)，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组：

map[(r₁,θ₁) (r₂,θ₂) (r₃,θ₃) ... (r_n-1,θ_n-1) (r_n,θ_n)] (2)

其中，

其中，(r_i,θ_i)表示第i个等距离间隔采样点的极坐标；r_i表示第i个等距离间隔采样点在极坐标系下的极径，θ_i表示第i个等距离间隔采样点的极径与极轴的夹角；Δx_i表示第i个等距离间隔采样点与原点在经度方向上的距离，Δy_i表示第i个等距离间隔采样点与原点在纬度方向上的距离；R_N表示地球经度圈半径，R_M表示地球纬度圈半径。

第二步：获取极坐标系下的车载导航数据数组。

具体地，对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，得到：

nav[(Lon₁,Lat₁) (Lon₂,Lat₂) (Lon₃,Lat₃) … (Lon_m-1,Lat_m-1) (Lon_m,Lat_m)] (5)

其中，(Lon_j,Lat_j)表示第j个等时间间隔采样点的地理坐标，j＝1,2,…,m；

将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组：

nav[(R₁,Θ₁) (R₂,Θ₂) (R₃,Θ₃) … (R_n-1,Θ_n-1) (R_n,Θ_n)] (6)

其中，

其中，(R_j,Θ_j)表示第j个等时间间隔采样点的极坐标；R_j表示第j个等时间间隔采样点在极坐标系下的极径，Θ_j表示第j个等时间间隔采样点的极径与极轴的夹角；Δx_j表示第j个等时间间隔采样点与原点在经度方向上的距离，Δy_j表示第j个等时间间隔采样点与原点在纬度方向上的距离。

第三步：建立极坐标系下车道中心线数据数组和车载导航数据数组的表征函数数组。

为了便于地图匹配搜索过程中的相似性计算，利用极坐标的两个参数(极径与极角)建立表征函数。将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组：

map[P₁(r₁,θ₁) P₂(r₂,θ₂) P₃(r₃,θ₃) … P_n-1(r_n-1,θ_n-1) P_n(r_n,θ_n)] (9)

其中，P_i(r_i,θ_i)＝r_iθ_i；

将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组：

nav[P₁(R₁,Θ₁) P₂(R₂,Θ₂) P₃(R₃,Θ₃) … P_m-1(R_m-1,Θ_m-1) P_m(R_m,Θ_m)] (10)

其中，P_j(R_j,Θ_j)＝R_jΘ_j。

第四步：根据车载导航传感器获取的当前时刻自动驾驶汽车所在位置，对车载导航误差进行粗估计，确定匹配搜索区域；其中，匹配搜索区域的长度为当前时刻车载导航误差的粗估计值，匹配搜索区域的宽度为当前时刻自动驾驶汽车所在道路的宽度。例如，可以假设确定的匹配搜索区域为a×b，其中，a为当前时刻车载导航误差的粗估计值，b为当前时刻自动驾驶汽车所在道路的宽度。

具体地，当前时刻自动驾驶汽车所在道路的宽度，通过以下方式获得：首先，利用车载导航传感器获取的当前时刻自动驾驶汽车所在位置，通过查表法确定当前时刻自动驾驶汽车所在位置的所在道路，然后，以所在道路的宽度作为匹配搜索区域的宽度。

由于极坐标系下表征函数与地理坐标并非一一对应，因此，无法实现单点匹配搜索，需要将连续采样点组成匹配序列，利用形状匹配原理建立表征函数的连续序列函数，使该连续序列函数与高精度矢量地图建立一一对应的关系。具体地，将匹配搜索区域内车道中心线的表征参数提取出来，组成匹配序列数组；同时，将自动驾驶汽车行驶过程中的解算轨迹点组成搜索序列数组；然后利用相似度算法进行最小距离度量，将遍历整个搜索区域，在所有能够取到的子集中，找到与匹配序列最相似的搜索序列的位置作为最佳匹配搜索位置。具体步骤如下面的第五步至第七步。

第五步：生成匹配序列和搜索序列。

具体地，将第一表征函数数组中与匹配搜索区域对应的表征函数提取出来，构成匹配序列：

[P_1,1 P_1,2 P_1,3 ... P_1,a×b-1 P_1,a×b] (11)

其中，a×b<n；

在第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长为M的表征函数，构成搜索序列：

[P_2,1 P_2,2 P_2,3 … P_2,M-1 P_2,M] (12)

其中，P_2,1表示第二表征函数数组中与当前时刻对应的表征函数；M<m且M<a×b。

第六步：采用遍历搜索策略。

在匹配搜索区域确保包含自动驾驶汽车所在真实位置的条件下，为了防止遗漏最佳匹配搜索位置，可以采用遍历搜索的方式。具体地，利用搜索序列对匹配搜索区域进行遍历搜索，计算搜索序列与匹配序列的相似度，具体包括：

利用下式计算搜索序列与匹配序列的相似度：

其中，l表示搜索序列在匹配搜索区域内平移的次数，l＝a×b；h＝1,2,…,M。

第七步：确定最佳匹配搜索位置。

在遍历搜索结束后，将获得的所有相似度组成相似度数组D，在相似度数组中选取数值最小的相似度对应的匹配搜索位置作为当前时刻的最佳匹配搜索位置：d＝min D；其中，d表示最相似误差，d对应的匹配搜索位置为最佳匹配搜索位置。

第八步：获取当前时刻最佳匹配搜索位置对应的极坐标序列。

具体地，从匹配序列中提取与最佳匹配搜索位置对应的表征函数序列：

其中，k表示搜索序列在匹配搜索区域内到达最佳匹配搜索位置所平移的次数；t表示当前时刻；P_t ^k表示在平移搜索k次后获得的当前时刻最佳匹配搜索位置的表征函数；

根据提取出的表征函数序列获得最佳匹配搜索位置对应的极坐标序列：

其中，

q＝t,t-1,t-2,...,t-M+2,t-M+1；

从式(15)可以看出，当前时刻最佳匹配搜索位置的极坐标为

将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标：

第九步：获取当前时刻直角坐标系下的车载导航数据。

具体地，从极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据(R_t,Θ_t)，将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据：

第十步：评价当前时刻的车载导航定位精度。

具体地，将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差：

其中，Δx_t表示当前时刻经度方向的定位误差，单位为m；Δy_t表示当前时刻纬度方向的定位误差，单位为m；

根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方误差(RMS)：

第十一步：返回第四步，重复执行第四步至第十步，计算下一时刻车载导航定位精度的均方误差，直至完成对整条车道上所有时刻车载导航定位精度均方误差的计算。

第十二步：评价整条车道的车载导航定位精度。

具体地，根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差，即自动驾驶汽车全程车载导航定位精度的均方误差：

其中，T表示自动驾驶汽车经过整条车道所花费的时间，即自动驾驶汽车整个行驶过程所花费的时间。

本发明提供的上述基于高精度矢量地图的车道级导航定位评价方法，首先，在极坐标系下，设立一个公共原点，并确定一条极轴，规定逆时针为正方向；然后，高精度矢量地图车道中心线上的点便可以利用极坐标唯一表示，并且，利用车载导航传感器获得的自动驾驶汽车的地理位置，也可以通过坐标转换，利用极坐标表示；接着，为利用车道中心线信息辅助匹配定位，建立极坐标表征函数；最后，利用匹配算法获取的当前时刻定位结果给出车载导航定位精度评价，当前时刻的车载导航定位精度评价结束后，进行下一时刻车载导航定位精度的评价，直到整个驾驶过程结束，从而做出车载导航系统的全程导航精度评价。上述评价方法，可以显著提高自动驾驶汽车的车载导航定位精度，实现厘米级的定位要求，从而可以利用高精度的位姿信息来实现汽车的自主驾驶。在汽车的路径规划与决策方面，高精度的车载导航定位技术可以辅助自动驾驶汽车做出正确的行为决策，从而避免交通事故的发生。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种基于高精度矢量地图的车道级导航定位评价方法，其特征在于，包括如下步骤：

S1：将高精度矢量地图中整条车道中心线的数据提取出来，对所述整条车道中心线的数据进行等距离间隔采样，将等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组；

S2：对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组；

S3：将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组，将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组；

S4：根据车载导航传感器获取的当前时刻自动驾驶汽车所在位置，对车载导航误差进行粗估计，确定匹配搜索区域；其中，所述匹配搜索区域的长度为当前时刻车载导航误差的粗估计值，所述匹配搜索区域的宽度为当前时刻自动驾驶汽车所在道路的宽度；

S5：将所述第一表征函数数组中与所述匹配搜索区域对应的表征函数提取出来构成匹配序列，在所述第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长的表征函数构成搜索序列；

S6：利用所述搜索序列对所述匹配搜索区域进行遍历搜索，计算所述搜索序列与所述匹配序列的相似度；

S7：在遍历搜索结束后，将获得的所有相似度组成相似度数组，在所述相似度数组中选取数值最小的相似度对应的匹配搜索位置作为当前时刻的最佳匹配搜索位置；

S8：从所述匹配序列中提取与所述最佳匹配搜索位置对应的表征函数序列，根据提取出的表征函数序列获得所述最佳匹配搜索位置对应的极坐标序列，从所述极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标，并将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标；

S9：从所述极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据，并将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据；

S10：将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差，并根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方误差；

S11：返回步骤S4，重复执行步骤S4～步骤S10，计算下一时刻车载导航定位精度的均方误差，直至完成对整条车道上所有时刻车载导航定位精度均方误差的计算；

S12：根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差，完成对整条车道车载导航定位精度的评价。

2.如权利要求1所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S1，将高精度矢量地图中整条车道中心线的数据提取出来，对所述整条车道中心线的数据进行等距离间隔采样，将等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组，具体包括：

将高精度矢量地图中整条车道中心线的数据提取出来，对所述整条车道中心线的数据进行等距离间隔采样，将n个等距离间隔采样点的地理坐标存放到直角坐标系下的车道中心线数据数组中得到：

map[(lon₁,lat₁) (lon₂,lat₂) (lon₃,lat₃)...(lon_n-1,lat_n-1) (lon_n,lat_n)] (1)

其中，(lon_i,lat_i)表示第i个等距离间隔采样点的地理坐标，i＝1,2,…,n；

设原点坐标为(lon₀,lat₀)，将直角坐标系下的车道中心线数据数组转换为极坐标系下的车道中心线数据数组：

map[(r₁,θ₁) (r₂,θ₂) (r₃,θ₃)…(r_n-1,θ_n-1) (r_n,θ_n)] (2)

其中，

其中，(r_i,θ_i)表示第i个等距离间隔采样点的极坐标；r_i表示第i个等距离间隔采样点在极坐标系下的极径，θ_i表示第i个等距离间隔采样点的极径与极轴的夹角；Δx_i表示第i个等距离间隔采样点与原点在经度方向上的距离，Δy_i表示第i个等距离间隔采样点与原点在纬度方向上的距离；R_N表示地球经度圈半径，R_M表示地球纬度圈半径。

3.如权利要求2所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S2，对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组，具体包括：

对车载导航传感器获取的自动驾驶汽车所在位置的地理坐标进行等时间间隔采样，将等时间间隔采样点的地理坐标存放到直角坐标系下的车载导航数据数组中，得到：

nav[(Lon₁,Lat₁) (Lon₂,Lat₂) (Lon₃,Lat₃)...(Lon_m-1,Lat_m-1) (Lon_m,Lat_m)] (5)

其中，(Lon_j,Lat_j)表示第j个等时间间隔采样点的地理坐标，j＝1,2,…,m；

将直角坐标系下的车载导航数据数组转换为极坐标系下的车载导航数据数组：

nav[(R₁,Θ₁) (R₂,Θ₂) (R₃,Θ₃)…(R_n-1,Θ_n-1) (R_n,Θ_n)] (6)

其中，

其中，(R_j,Θ_j)表示第j个等时间间隔采样点的极坐标；R_j表示第j个等时间间隔采样点在极坐标系下的极径，Θ_j表示第j个等时间间隔采样点的极径与极轴的夹角；Δx_j表示第j个等时间间隔采样点与原点在经度方向上的距离，Δy_j表示第j个等时间间隔采样点与原点在纬度方向上的距离。

4.如权利要求3所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S3，将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组，将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组，具体包括：

将极坐标系下车道中心线数据数组中每个极坐标的极径与极角相乘，得到第一表征函数数组：

map[P₁(r₁,θ₁) P₂(r₂,θ₂) P₃(r₃,θ₃)…P_n-1(r_n-1,θ_n-1) P_n(r_n,θ_n)] (9)

其中，P_i(r_i,θ_i)＝r_iθ_i；

将极坐标系下车载导航数据数组中每个极坐标的极径与极角相乘，得到第二表征函数数组：

nav[P₁(R₁,Θ₁) P₂(R₂,Θ₂) P₃(R₃,Θ₃)…P_m-1(R_m-1,Θ_m-1) P_m(R_m,Θ_m)] (10)

其中，P_j(R_j,Θ_j)＝R_jΘ_j。

5.如权利要求4所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，设步骤S4确定的匹配搜索区域为a×b，其中，a为当前时刻车载导航误差的粗估计值，b为当前时刻自动驾驶汽车所在道路的宽度；

步骤S5，将所述第一表征函数数组中与所述匹配搜索区域对应的表征函数提取出来构成匹配序列，在所述第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长的表征函数构成搜索序列，具体包括：

将所述第一表征函数数组中与所述匹配搜索区域对应的表征函数提取出来，构成匹配序列：

[P_1,1 P_1,2 P_1,3…P_1,a×b-1 P_1,a×b] (11)

其中，a×b<n；

在所述第二表征函数数组中从与当前时刻对应的表征函数开始向后选取预设时长为M的表征函数，构成搜索序列：

[P_2,1 P_2,2 P_2,3…P_2,M-1 P_2,M] (12)

其中，P_2,1表示所述第二表征函数数组中与当前时刻对应的表征函数；M<m且M<a×b。

6.如权利要求5所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S6，利用所述搜索序列对所述匹配搜索区域进行遍历搜索，计算所述搜索序列与所述匹配序列的相似度，具体包括：

利用下式计算所述搜索序列与所述匹配序列的相似度：

其中，l表示所述搜索序列在所述匹配搜索区域内平移的次数，l＝a×b；h＝1,2,…,M。

7.如权利要求6所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S8，从所述匹配序列中提取与所述最佳匹配搜索位置对应的表征函数序列，根据提取出的表征函数序列获得所述最佳匹配搜索位置对应的极坐标序列，从所述极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标，并将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标，具体包括：

从所述匹配序列中提取与所述最佳匹配搜索位置对应的表征函数序列：

其中，k表示所述搜索序列在所述匹配搜索区域内到达所述最佳匹配搜索位置所平移的次数；t表示当前时刻；P_t ^k表示在平移搜索k次后获得的当前时刻最佳匹配搜索位置的表征函数；

根据提取出的表征函数序列获得所述最佳匹配搜索位置对应的极坐标序列：

其中，

从所述极坐标序列中得到当前时刻最佳匹配搜索位置的极坐标为

将当前时刻最佳匹配搜索位置的极坐标转换为当前时刻最佳匹配搜索位置的直角坐标：

8.如权利要求7所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S9，从所述极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据，并将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据，具体包括：

从所述极坐标系下的车载导航数据数组中提取出当前时刻极坐标系下的车载导航数据(R_t,Θ_t)，将当前时刻极坐标系下的车载导航数据转换为当前时刻直角坐标系下的车载导航数据：

9.如权利要求8所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S10，将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差，并根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方差，具体包括：

将当前时刻最佳匹配搜索位置的直角坐标与当前时刻直角坐标系下的车载导航数据作差，得到当前时刻的定位误差：

其中，Δx_t表示当前时刻经度方向的定位误差；Δy_t表示当前时刻纬度方向的定位误差；

根据当前时刻的定位误差，计算当前时刻车载导航定位精度的均方误差：

10.如权利要求9所述的基于高精度矢量地图的车道级导航定位评价方法，其特征在于，步骤S12，根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差，完成对整条车道车载导航定位精度的评价，具体包括：

根据所有时刻车载导航定位精度的均方误差，计算整条车道车载导航定位精度的均方误差：

其中，T表示自动驾驶汽车经过整条车道所花费的时间。

Images