CN107704821B - 一种弯道的车辆位姿计算方法 - Google Patents

Abstract

本发明涉及一种弯道的车辆位姿计算方法，该方法是基于GPS定位与基于弯道路面特征的车辆视觉位姿计算相结合的方法，首先利用当前车辆的GPS位置信息进行初定位，再将弯道路面的测试图像与弯道地图数据库进行特征匹配，最后根据匹配的局部特征点进行视觉位姿计算，得到车辆在弯道的高精度位姿信息。该方法克服了现有使用单一的GPS定位精度差和视觉位姿计算方法在弯道位姿计算不恰当的缺陷，同时能解决弯道车辆位姿计算存在的误差较大、成本高的问题，从而提高车辆弯道位姿计算的精度。

Description

一种弯道的车辆位姿计算方法

技术领域

本发明涉及为了车辆驾驶安全的辅助驾驶技术，具体地说是一种弯道的车辆位姿计算方法。

背景技术

弯道的车辆位姿是指在弯道行驶时车辆的位置和姿态，在行驶中车辆的位姿是影响驾驶安全的重要因素之一，对车辆进行实时位姿计算有助于驾驶员根据位姿状态做出实时调整，以减少交通事故发生的概率。因此，实时准确的计算车辆的位姿在车道保持和辅助驾驶方面有重大意义。车辆定位仅是对车辆位置的判断，在弯道时车辆位置计算称为弯道车辆的定位，而弯道车辆的定位和弯道车辆姿态的计算一起称为弯道的车辆位姿计算。车辆行驶的公路可分为直道和弯道两种类型。在直道行驶时可根据车道线对车辆的位姿进行精确计算，但当车辆处于弯道时无法检测车道线，且安装在车辆上的激光仅能监测与其它车辆的距离，不能用于判断车辆的位姿。弯道车辆的定位尤其是对度量级位姿计算的准确性需求也日益剧增，弯道车辆的定位中目前最主要的定位方式是运用GPS实现定位，然而该方法精度约为5m，不能满足车辆的位置计算精度的要求，且弯道车辆姿态的计算还未能实现。

从目前公开的文献和技术手段来看，研发较多的车辆弯道位姿计算技术有：GPS定位技术，该技术基于24颗工作卫星，使得在地球上何时何地都可观测到4颗及其以上的卫星，可测量出卫星到车载接收机的距离，然后综合多颗卫星的数据即可确定当前车辆的具体位置。车辆在路面行驶时GPS信号精度及可靠性会受到很大影响：接收的GPS信号会受到遮挡物的影响，如高楼，大树等物体；有些弯道GPS信号强度不高也会影响。故单一使用GPS实现弯道车辆位姿计算的方法，不可避免的会致使误差比较大。

目前，车辆在弯道车辆的定位方面已有一些成果，绝大部分都是对弯道车辆定位还停留在GPS初定位或者图像级定位阶段，还未能达到车辆度量级的位姿计算阶段；而弯道车辆姿态的计算还未有涉及。车辆度量级的位姿计算为车辆的高精度位姿计算，其可达到分米级甚至是厘米级别。例如CN104326566A公开了一种利用智能手机的地图匹配方法，该方法利用手机的加速传感器进行弯道检测，再根据检测结果对GPS导航系统进行修正。利用手机加速传感器仅可检测到车辆所处的车道是否属于弯道，以此来对GPS导航系统进行修正，其修正结果精度较低，未能达到车辆度量级的位姿计算阶段；CN106494406A披露了一种弯道导向方法、弯道导向装置、电子装置及程序,可获得与车辆所行驶的道路相对应的路段信息和车辆性能信息，即道路标识符、宽度、长度等路段属性信息和车辆的速度及位置信息，再基于所获得的路段信息来对未来时间点确定路段中的车辆位置以及利用所确定的位置及基准时间点的车辆的速度来计算车辆的离心力，进而判断规定时间之后车辆所要行驶的弯道区间的危险度，以此来对车辆行驶在弯道区域的危险性进行估计，车辆的位置无法精度计算；CN105882661A公开了机动车弯道驾驶辅助系统及运行方式，GPS信息确定车辆的位置信息，再将车辆行驶时采集的前端图像进行处理分析，通过与完全障碍背景的比较判断车辆否进入弯道和分析计算获得弯道的曲率半径信息，达到辅助驾驶车辆的目的，但道路环境复杂多变完全障碍背景的判断容易出现误差，且其未能精确计算车辆的位姿信息。CN106289271A公开了一种弯道车辆定位装置及方法，根据激光采集的弯道区域测距波形，确定车辆区域对应的位置信息，并将当前帧车辆区域与前一帧的进行区域匹配,确定所述当前帧车辆区域对应弯道新进车辆或者弯道区域行进车辆的扫描点位置信息,对车辆进行实时定位，该方法仅利用激光进行弯道车辆的定位误差大。

综上所述，目前尚无经济且成熟的弯道车辆位姿计算技术。随着互联网、无线通信、计算机技术、测绘技术和设备制造等关键技术的飞速发展，弯道车辆位姿将向着不同车辆位姿计算技术互补结合的方向发展。通过不同车辆位姿计算技术互补结合的方式弥补某一项车辆位姿计算方法的劣势，如何将各种不同的车辆位姿计算技术有机地结合起来将是该技术领域研究的热点。

发明内容

针对现有技术的不足，本发明所要解决的技术问题是：提供一种弯道的车辆位姿计算方法。该方法采用基于GPS定位与基于弯道路面特征的车辆视觉位姿计算相结合的方法，可得到车辆在弯道的高精度位姿信息，克服了现有使用单一的GPS定位精度差和视觉位姿计算方法在弯道位姿计算不恰当的缺陷，同时能解决弯道车辆位姿计算存在的误差较大、成本高的问题，从而提高车辆弯道位姿计算的精度。

本发明解决所述技术问题采用的技术方案是：提供一种弯道的车辆位姿计算方法，该方法的具体步骤如下：

第一步，构建弯道地图数据库

1-1、采集弯道GPS信息序列：

基于车载GPS采集设备获取车辆当前的GPS值，即纬度与经度，记为[n,e]；车辆在弯道的道路中心处且车身平行于弯道的切线，每隔0.5-1.5m采集一次GPS信息，采集GPS的位置作为采样点，遍历所有采样点便可获得弯道GPS信息序列；

1-2、采集弯道路面图像：

采集弯道路面图像由车载摄像机系统完成，车载摄像机系统由车载摄像机、控制设备和弯道路面图像存储器和传输器组成。车载摄像机安装在车辆的前盖的横向中心位置，车载摄像机俯角拍摄弯道路面，车载摄像机的光轴到路面的投影与车的纵向平行；利用张正友标定法对车载摄像机进行标定，得到车载摄像机的内参数矩阵K和外参数、弯道路面坐标系与弯道路面图像坐标系间的单应性矩阵H₁及弯道路面图像的中心点(u₀，v₀)；外参数包括旋转矩阵R和平移向量T，弯道路面坐标系与车载摄像机采集的弯道路面图像坐标系的关系满足式(1)：

[u v 1]^T＝H₁[X_w Y_w 1]^T (1)

其中(u v)为车载摄像机采集的弯道路面图像的像素坐标，(X_w Y_w)为路面坐标；

在上述步骤1-1进行时由车载摄像机系统在采样点采集弯道路面的图像，得一系列弯道路面图像，每张弯道路面图像对应唯一的GPS值；将弯道路面图像的中心点带入式(1)得到车载摄像机的光轴与路面的交点记为A,A＝(X₀ Y₀)，车载摄像机投影到弯道路面的点与A点的连线即为光轴投影到弯道路面的线段，记为l；

1-3、采样点处车辆的位姿信息序列：

每个采样点的坐标根据路测激光确定；在步骤1-1和步骤1-2进行时路测激光实时采集激光数据，根据激光数据确定车辆的位姿信息，即车辆所处的位置和车辆偏转角度，第i个采样点处车辆的位置坐标为M_i(i＝1,2,3,……)，从而得到采样点处车辆的位姿信息序列；

1-4、弯道地图数据库：

每一张弯道路面图像对应唯一步骤1-1中采集的GPS值，对应唯一唯一步骤1-3中位姿信息，每个采样点利用已安装的车载GPS采集设备获取车辆当前的GPS，将弯道GPS信息序列、一系列弯道路面图像以及车辆的位姿信息序列共同存入弯道地图数据库，由此完成弯道地图数据库的构建；

第二步，GPS初定位：

2-1、采集测试位置车辆的GPS信息与弯道路面图像：

测试位置即为车辆所在的当前位置，记为点P，在弯道内利用车载GPS采集设备采集测试位置车辆的GPS值，得到点P处GPS值的纬度与经度；利用车载摄像机系统在当前位置采集弯道路面的图像，得到测试位置的弯道路面图像，称为被测试图像；

2-2、GPS初定位阶段：

将步骤2-1得到的点P处的GPS值与弯道地图数据库中GPS值进行匹配，选取弯道地图数据库中采样点处的GPS值与点P处的GPS值最相近的n个采样点，n不小于5，且不大于10，n个采样点按弯道位置排序分别记为M_j+1，M_j+2，M_j+3，M_j+4，M_j+5……M_j+n，即可实现GPS初定位；从弯道地图数据库调取n个采样点处采集的图像组成图像训练集，当弯道不足n个采样点时GPS初定位结果为弯道地图数据库中全部的采样点；

第三步，图像级定位：

首先分别将上述步骤2-1和步骤2-2得到的被测试图像和图像训练集分别进行预处理，所述预处理包括归一化处理、均衡化和灰度化处理，将预处理后图像的中心作为ORB特征点，把该整个图像作为此特征点的邻域区域，再计算得到ORB全局特征描述符，以该ORB全局特征描述符作为图像的ORB全局特征；

使用WH-KNN算法按照式(2)分别计算被测试图像与图像训练集中每张图像的汉明距离，

其中，H¹为被测试图像的ORB全局特征描述符字符串，H²为图像训练集中一张弯道路面图像的ORB全局特征描述符字符串，

和

分别为ORB全局特征描述符256位字符串的第i位、D_R(H¹,H²)为两张图像间的汉明距离、xor为位异或运算；

比较所有汉明距离的大小，找出汉明距离最小的一张图像作为与被测试图像最相近的图像训练集中的一张图像，称为图像级定位的最匹配图像，此最匹配图像对应的位置坐标即为图像级定位坐标(x_v,y_v)，由此完成图像级定位；

第四步，车辆度量级的位姿计算：

将第三步得到的图像级定位的最匹配图像与当前预处理后的被测试图像进行ORB局部特征匹配，再利用RANSAC算法去除误匹配，分别得到最匹配图像与当前预处理后的被测试图像对应的两组特征点序列，且两组特征点序列之间的关系满足式(3)，

[u v 1]^T＝H₀[u₁ v₁ 1]^T (3)

其中H₀为预处理后的最匹配图像与当前被测试图像之间对应的单应性矩阵，(u,v)是最匹配图像的弯道路面图像像素坐标，最匹配的弯道路面图像是弯道地图数据库中的一张图像，(u₁,v₁)被测试图像的弯道路面图像像素坐标；

再联立式(1)和式(3)得到式(4)，

[u₁ v₁ 1]^T＝H₀ ^-1H₁[X_w Y_w 1]^T (4)

其中，H₀ ^-1H₁为弯道路面坐标系与被测试图像坐标系对应的单应性矩阵，H₀ ^-1H₁记为H；

再根据张正友标定法中单应性矩阵H与旋转矩阵R、平移向量T之间的关系，计算出拍摄被测试图像时车载摄像机的外参数，即旋转矩阵R和平移向量T；然后根据-R^-1T计算车载摄像机所在世界坐标系的位置，判断拍摄当前被测试图像和最匹配图像的车载摄像机位置的差，记为车辆的偏离距离，将第三步得到的图像级定位坐标(x_v,y_v)加上车载摄像机位置的差，即为拍摄被测试图像时车辆的当前位置坐标，由此完成在弯道行驶时车辆位置坐标的计算；

拍摄被测试图像时车载摄相机投影到弯道路面的点与A点的连线即光轴投影到弯道路面上的线段，记为l₁；再在弯道地图数据库中找出最匹配图像的光轴投影到弯道路面的线段，两线段的夹角即为车辆偏转角度；至此完成弯道车辆度量级的位姿计算。

与现有技术相比，本发明的有益效果是：

本发明的突出的实质性特点如下：

(1)本发明一种弯道的车辆位姿计算方法采用基于GPS定位与弯道路面特征的视觉位姿计算相结合的方法，首先利用当前车辆的GPS位置信息进行初定位，再将弯道路面的测试图像与弯道地图数据库进行ORB全局特征匹配确定弯道地图数据库中最匹配的一张图像，最后再根据匹配的ORB局部特征点进行视觉位姿计算，得到车辆在弯道的高精度位姿信息，创造出一种基于GPS定位与视觉融合的高精度弯道位姿计算方法。实验结果表明，本发明提出的两种定位技术相结合的定位方法准确率高，误差较小。

(2)本发明方法中，GPS定位与视觉位姿计算的互补之处在于：仅利用GPS定位得到的结果误差在5m左右，正确率低，对智能车辆的辅助驾驶毫无帮助，可利用视觉位姿计算来修正GPS定位结果；但当只利用视觉位姿计算来判断车辆的位姿会有两个方面的问题：一是计算时间长，当弯道的采样点过多时没有GPS的初定位，将会使图像级定位的训练图像增多至全部弯道地图数据库中的图像，致使计算量增大、时间增长。二是当视觉位姿计算出现较大的误差时，即匹配发生错误时，若只利用视觉位姿计算来计算车辆的位姿将会引起不可挽回的错误。故本发明创造性地用视觉位姿计算结果达到修正GPS定位结果的目的，实现弯道车辆位姿计算的高精度，本发明方法克服了现有GPS定位技术存在精度低的缺陷，同时也克服了单一利用视觉位姿计算来判断车辆的位姿的困难。

本发明的显著进步如下：

(1)本发明创新地将GPS定位与弯道路面特征的车辆视觉位姿计算相结合，扬长避短，可方便地实现高精度的弯道车辆位姿计算。

(2)本发明方法可以有效提高弯道车辆位姿计算的精度，传统的基于GPS定位的弯道车辆定位方法的精度一般在5m左右，本发明方法将GPS定位与视觉位姿计算相结合，定位精度可达到10cm以内。

(3)本发明方法首先利用GPS定位得到初定位范围确定5-10个采样点，为之后的图像匹配以及视觉视觉位姿计算确定了大致位置范围，当GPS定位不准时本发明方法同样适用。

(4)本发明方法可将视觉位姿计算结果修正GPS定位结果，实现弯道车辆位姿计算的精度，达到度量级。

(5)本发明方法利用弯道路面不随时间变化、唯一性和特征旋转与平移不发生变化的特征进行检测和图像匹配，适用于所有的弯道，适用范围广。

(6)本发明方法可计算在弯道行驶时车辆的位姿信息，包括车载摄像机拍摄测试图像时车辆的位置坐标和车辆的偏转角度。

(7)本发明方法在弯道区域克服了传统的视觉或者激光方法会失效的缺陷。在直道行驶时传统的视觉方法可根据车道线对车辆的位姿进行精确计算，在弯道时无法检测车道线，故传统的视觉车辆位姿计算方法在弯道上将会失效；在直道行驶时车辆的位姿计算的传统激光方法可通过监测与其它车辆的位置关系确定，当处于弯道时当前被计算位姿的车辆与其它车辆行驶的公路不平行，故弯道时传统的激光方法不能用于计算弯道的车辆位姿。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1为本发明方法的步骤流程示意框图。

图2为本发明方法的GPS初定位示意图。

图3为本发明方法的ORB局部特征匹配图。

具体实施方式

本发明弯道的车辆位姿计算方法(简称方法，参见图1-3)，是基于GPS定位与基于弯道路面特征的车辆视觉位姿计算相结合的方法，首先利用当前车辆的GPS位置信息进行初定位，再将弯道路面的测试图像与弯道地图数据库进行特征匹配，最后根据匹配的局部特征点进行视觉位姿计算，得到车辆在弯道的高精度位姿信息；具体步骤如下：

第一步，构建弯道地图数据库

1-1、采集弯道GPS信息序列：

基于车载GPS采集设备获取车辆当前的GPS值，即纬度与经度，记为[n,e]；车辆在弯道的道路中心处且车身平行于弯道的切线，每隔0.5-1.5m采集一次GPS信息，采集GPS的位置作为采样点，遍历所有采样点便可获得弯道GPS信息序列；

1-2、采集弯道路面图像：

采集弯道路面图像由车载摄像机系统完成，车载摄像机系统由车载摄像机、控制设备和弯道路面图像存储器和传输器组成。车载摄像机安装在车辆的前盖的横向中心位置，车载摄像机俯角拍摄弯道路面，车载摄像机的光轴到路面的投影与车的纵向平行。利用张正友标定法对车载摄像机进行标定，得到车载摄像机的内参数矩阵K和外参数(旋转矩阵R和平移向量T)、弯道路面坐标系与弯道路面图像坐标系间的单应性矩阵H₁及弯道路面图像的中心点(u₀，v₀)；弯道路面坐标系与车载摄像机采集的弯道路面图像坐标系的关系满足式(1)：

[u v 1]^T＝H₁[X_w Y_w 1]^T (1)

其中(u v)为车载摄像机采集的弯道路面图像的像素坐标，(X_w Y_w)为路面坐标；

在上述步骤1-1进行时由车载摄像机系统在采样点采集弯道路面的图像，得一系列弯道路面图像，每张弯道路面图像对应唯一的GPS值；将弯道路面图像的中心点带入式(1)得到车载摄像机的光轴与路面的交点记为A,A＝(X₀ Y₀)，车载摄像机投影到弯道路面的点与A点的连线即为光轴投影到弯道路面的线段，记为l；

1-3、采样点处车辆的位姿信息序列：

每个采样点的坐标根据路测激光确定，路测激光的精度要求在200m内误差不大于2mm；在步骤1-1和步骤1-2进行时路测激光实时采集激光数据，根据激光数据确定车辆的位姿信息，即车辆所处的位置和车辆偏转角度，第i个采样点处车辆的位置坐标为M_i(i＝1,2,3,……)，从而得到采样点处车辆的位姿信息序列；

1-4、弯道地图数据库：

每一张弯道路面图像对应唯一步骤1-1中采集的GPS值，对应唯一步骤1-3中采集的位姿信息，每个采样点利用已安装的车载GPS采集设备获取车辆当前的GPS，将弯道GPS信息序列、一系列弯道路面图像以及车辆的位姿信息序列共同存入弯道地图数据库，由此完成弯道地图数据库的构建；

第二步，GPS初定位：

2-1、采集测试位置车辆的GPS信息与弯道路面图像：

测试位置即为车辆所在的当前位置，记为点P，在弯道内利用车载GPS采集设备采集测试位置车辆的GPS值，得到点P处GPS值的纬度与经度；利用车载摄像机系统在当前位置采集弯道路面的图像，得到测试位置的弯道路面图像，称为被测试图像；

2-2、GPS初定位阶段：

将步骤2-1得到的点P处的GPS值与弯道地图数据库中GPS值进行匹配，选取弯道地图数据库中采样点处的GPS值与点P处的GPS值最相近的n个采样点，n不小于5，且不大于10，n个采样点按弯道位置排序分别记为M_j+1，M_j+2，M_j+3，M_j+4，M_j+5……M_j+n，即可实现GPS初定位。从弯道地图数据库调取n个采样点处采集的图像组成图像训练集，当弯道不足n个采样点时GPS初定位结果为弯道地图数据库中全部的采样点；

第三步，图像级定位：

图像级定位包括两部分，第一部分是计算被测试图像和图像训练集的ORB全局特征描述符；第二部分是利用WH-KNN算法在ORB匹配空间计算一个近邻，即计算图像训练集中与被测试图像的ORB全局特征描述符的汉明距离最小的一张图像。与被测试图像最相近的图像训练集中的一张图像，称为图像级定位的最匹配图像，完成图像级定位；具体步骤是：

首先分别将上述步骤2-1和步骤2-2得到的被测试图像和图像训练集进行预处理，所述预处理包括归一化处理、均衡化和灰度化处理，然后计算预处理后的被测试图像和图像训练集的ORB全局特征描述符，包括三部分，第一部分是定义特征点的方向，第二部分创建BRIEF特征描述子，第三部分计算ORB全局特征描述符；将预处理后图像的中心作为ORB特征点，把整个图像作为此特征点的邻域区域，再计算得到ORB全局特征描述符，以该ORB全局特征描述符作为图像的ORB全局特征；

使用WH-KNN算法按照式(2)分别计算被测试图像与图像训练集中每张图像的汉明距离，

其中，H¹为被测试图像的ORB全局特征描述符字符串，H²为图像训练集中一张弯道路面图像的ORB全局特征描述符字符串，

和

分别为ORB全局特征描述符256位字符串的第i位、D_R(H¹,H²)为两张图像间的汉明距离、xor为位异或运算；

以被测试图像与图像训练集中一张图像的ORB全局特征描述符之间的汉明距离为度量值，若汉明距离越小，则图像匹配度越高；比较所有汉明距离的大小，找出汉明距离最小的一张图像作为与被测试图像最相近的图像训练集中的一张图像，称为图像级定位的最匹配图像，此最匹配图像对应的位置坐标即为图像级定位坐标(x_v,y_v)，由此完成图像级定位；

第四步，车辆度量级的位姿计算：

在第三步得到图像级定位的最匹配图像后，若直接将最匹配图像的位置坐标(x_v,y_v)作为当前车辆位置坐标将会带来较大的误差。为减少上述所述的误差，提高车辆位姿计算的精度，满足车辆行驶中的位姿信息需求，必须将车辆位姿计算提升到度量级的高度。

将第三步得到的经过预处理图像级定位的最匹配图像与当前预处理后的被测试图像进行ORB局部特征匹配，再利用RANSAC算法去除误匹配，分别得到最匹配图像与当前预处理后的被测试图像对应的两组特征点序列，且两组特征点序列之间的关系满足式(3)，

[u v 1]^T＝H₀[u₁ v₁ 1]^T (3)

其中H₀为预处理后的最匹配图像与当前被测试图像之间对应的单应性矩阵，(u,v)是最匹配图像的弯道路面图像像素坐标，最匹配的弯道路面图像是弯道地图数据库中的一张图像，(u₁,v₁)是被测试图像的弯道路面图像像素坐标；

再联立式(1)和式(3)得到式(4)

[u₁ v₁ 1]^T＝H₀ ^-1H₁[X_w Y _w1]^T (4)

其中，H₀ ^-1H₁为弯道路面坐标系与被测试图像坐标系的对应的单应性矩阵，H₀ ^-1H₁记为H；

再根据张正友标定法中单应性矩阵H与旋转矩阵R、平移向量T之间的关系，计算出拍摄被测试图像时车载摄像机的外参数，即旋转矩阵R和平移向量T；然后根据-R^-1T计算车载摄像机所在世界坐标系的位置((X_c Y_c Z_c)＝-R^-1T，(X_c Y_c Z_c)为车载摄像机所在世界坐标系的位置坐标)，判断拍摄当前被测试图像和最匹配图像的车载摄像机位置的差，记为车辆的偏离距离，将第三步得到的图像级定位坐标(x_v,y_v)加上车载摄像机位置的差，即为拍摄被测试图像时车辆的当前位置坐标，由此完成在弯道行驶时车辆位置坐标的计算；

拍摄被测试图像时车载摄相机投影到弯道路面的点与A点的连线即光轴投影到弯道路面上的线段，记为l₁；再在弯道地图数据库中找出最匹配图像的光轴投影到弯道路面的线段，两线段的夹角即为车辆偏转角度；至此完成弯道车辆度量级的位姿计算。

本发明中所述车辆度量级的位姿为拍摄被测试图像时车辆的位置坐标和车辆的偏转角度，能够实现分米级甚至厘米级的计算。

本发明弯道的车辆位姿计算方法的所述第一步中张正友标定法中利用车载摄像机拍摄的放置在地面的棋盘格实现摄像机的内外参数的标定，车辆在行驶过程中车载摄像机安放的位置不变，故摄像机内外参数不变；本发明中所述的张正友标定法、WH-KNN算法、ORB全局特征、ORB局部特征匹配、RANSAC算法均为现有方法。

图1所示实施例表明，本发明方法的步骤流程是：构建弯道地图数据库→GPS初定位→图像级定位→车辆度量级位姿计算。

图2所示实施例表明，GPS初定位位置坐标结果为M_j+1到M_j+n。

图3所示实施例表明,图像级定位的最匹配图像与被测试图像进行ORB局部特征匹配的效果图，左侧为图像级定位的最匹配图像，右侧为被测试图像。

实施例1

本实施例以武汉理工大学航海楼门前弯道为试验场地，车载摄像机系统拍摄的所有图片均为1920*1200(像素)。

本实施例弯道的车辆位姿计算方法，包括以下步骤：

第一步，构建弯道地图数据库：

1-1、采集弯道GPS信息序列：

利用车载GPS采集设备获取车辆当前的GPS值。车辆在弯道的道路中心处且车身平行于弯道的切线每隔1m采集一次GPS信息，共计采集25次GPS值。采样点数也为25个，遍历所有采样点便可获得弯道的GPS序列，其由25组经纬度值；

1-2、采集弯道路面图像：

采集弯道路面图像由车载摄像机系统完成，车载摄像机系统由车载摄像机、控制设备和弯道路面图像存储器和传输器组成，其主要为车载摄像机。车载摄像机安装在车辆的前盖的横向中心位置，车载摄像机俯角拍摄弯道路面，车载摄像机的光轴到路面的投影与车的纵向平行。利用张正友标定法对车载摄像机进行标定，车载摄像机采集的视频每帧图像尺寸为1920×1200(像素)，主点位置位于[930.74641.00](像素)，并得到车载摄像机的内参数(内参数矩阵K)和外参数(旋转矩阵R和平移向量T)及弯道路面坐标系与弯道路面图像坐标系间的单应性矩阵为H₁。

在上述步骤1-1进行时由车载摄像机系统在采样点采集弯道路面的图像，可得一系列弯道路面图像。每张弯道路面图像对应唯一的GPS值。由车载摄像机采集的视频每帧图像可得弯道路面图像的中心点(930.74641.00)，车载摄像机的光轴与路面的交点记为A，车载摄像机投影到弯道路面的点与A点的连线即为光轴投影到弯道路面的线段l，车载摄像机的光轴到路面的投影与弯道的切线平行；

1-3、采样点处车辆的位姿信息序列：

每个采样点的坐标根据路测激光确定，路测激光的精度为200m，误差为2mm；在步骤1-1和步骤1-2进行时路测激光实时采集激光数据，根据激光数据可确定车辆的位姿信息(即车辆所处的位置和姿态)，第i个采样点处车辆的位置坐标为M_i(i＝1,2,3,……)，从而得到采样点处车辆的位姿信息序列；

1-4、弯道地图数据库：

每一张弯道路面图像对应唯一的步骤1-1中采集的GPS值，对应唯一位姿信息，每个采样点利用已安装的车载GPS采集设备获取车辆当前的GPS，将弯道GPS信息序列、一系列弯道路面图像以及车辆的位姿信息序列共同存入弯道地图数据库，由此完成弯道地图数据库的构建；

第二步，GPS初定位：

2-1、采集测试位置车辆的GPS信息与弯道路面图像：

测试位置即为车辆所在的当前位置，记为点P。由上述步骤1-1所述，在弯道内利用车载GPS采集设备采集测试位置车辆的GPS值，为北纬30.608405°、东经114.363922°。利用车载摄像机系统在当前位置采集的弯道路面图像，得到测试位置的路面图像，称为被测试图像；

2-2、GPS初定位阶段：

将步骤2-1得到的点P处的GPS值与弯道地图数据库中GPS值的纬度与经度进行匹配，选取弯道地图数据库中采样点处的GPS值与点P处的GPS值最相近的5个采样点，即可实现GPS初定位；从弯道地图数据库调取5个采样点处采集的图像组成图像训练集，其可以从弯道地图数据库调取。这5个采样点的中间采样点的位置为GPS最匹配的位置(当GPS最匹配的位置位于采样点情况除外)，位置坐标为M_j+3(北纬30.608409°、东经114.363930°)；

第三步，图像级定位：

首先分别将上述步骤2-1和步骤2-2得到的被测试图像和图像训练集分别进行预处理，包括归一化处理、均衡化和灰度化处理，然后计算被测试图像和图像训练集的ORB全局特征描述符。将预处理后图像的中心作为ORB特征点，把整个图像作为此特征点的邻域区域，再计算得到ORB全局特征描述符，以ORB全局特征描述符作为图像的ORB全局特征；

使用WH-KNN算法按照式(2)分别计算被测试图像与图像训练集中每张图像的汉明距离，

其中，H¹为被测试图像的ORB全局特征描述符字符串，H²为图像训练集中一张弯道路面图像的ORB全局特征描述符字符串，

和

分别为ORB全局特征描述符256位字符串的第i位、D_R(H¹,H²)为两张图像间的汉明距离、xor为位异或运算；

以被测试图像与图像训练集中一张图像的ORB全局特征描述符之间的汉明距离为度量值，若汉明距离越小，则图像匹配度越高；比较所有汉明距离的大小，找出汉明距离最小的一张图像作为与被测试图像最相近的图像训练集中的一张图像，称为图像级定位的最匹配图像，此最匹配图像对应的位置坐标即为图像级定位坐标(x_v,y_v)，图像级定位坐标为(9.53，0.42)(单位m)，由此完成图像级定位；

第四步，车辆度量级的位姿计算：

在上述第三步得到图像级定位的最匹配图像后，若直接将最匹配图像的位置坐标(9.53，0.42)(单位m)作为当前车辆位置坐标将会带来较大的误差。为减少上述所述的误差，提高车辆位姿计算的精度，满足车辆行驶中的位姿信息需求，必须将车辆位姿计算提升到度量级的高度。

将第三步得到的经过预处理图像级定位的最匹配图像与当前预处理后的被测试图像进行ORB局部特征匹配，再利用RANSAC算法去除误匹配，匹配图如图3所示其中ORB局部匹配特征点486个，去除误匹配后还剩92个，分别得到最匹配图像与当前预处理后的被测试图像对应的两组特征点序列，且两组特征点序列之间的关系满足式(3)，

[u v 1]^T＝H₀[u₁ v₁ 1]^T (3)

其中H₀为预处理后的最匹配图像与当前被测试图像之间对应的单应性矩阵，(u,v)是最匹配图像的弯道路面图像像素坐标，最匹配的弯道路面图像是弯道地图数据库中的一张图像，(u₁,v₁)是被测试图像的弯道路面图像像素坐标；

再联立式(1)和式(3)得到式(4)

[u₁ v₁ 1]^T＝H₀ ^-1H₁[X_w Y_w 1]^T (4)

其中，H₀ ^-1H₁为弯道路面坐标系与被测试图像坐标系对应的单应性矩阵，H₀ ^-1H₁记为H；

再根据张正友标定法中单应性矩阵H与旋转矩阵R、平移向量T之间的关系，计算出拍摄被测试图像时车载摄像机的外参数，即旋转矩阵R和平移向量T；然后根据-R^-1T计算车载摄像机所在世界坐标系的位置((X_c Y_c Z_c)＝-R^-1T，(X_c Y_c Z_c)为车载摄像机所在世界坐标系的位置坐标)，判断拍摄当前被测试图像和最匹配图像的车载摄像机位置的差，记为车辆的偏离距离，将第三步得到的图像级定位坐标(x_v,y_v)加上车载摄像机位置的差，即为采集被测试图像时车辆的当前位置坐标(9.21，1.27)(单位m)，由此完成在弯道行驶时车辆位置坐标的计算。

路测激光测得采集被测试图像时车辆的当前位置坐标为(9.17，1.34)(单位m)，位置坐标绝对误差为(0.04，0.07)(单位m)。

拍摄被测试图像时车载摄相机投影到弯道路面的点与A点的连线即光轴投影到弯道路面上的线段，记为l₁；再在弯道地图数据库中找出最匹配图像的光轴投影到弯道路面的线段，两线段的夹角即为车辆偏转角度，至此完成弯道车辆度量级的位姿计算。本实施例方法计算得到的车辆偏转角度为4.75°。

路测激光测得的车辆偏转角度为5.13°，车辆的偏转角度绝对误差为0.38°。

车辆度量级的位姿为采集被测试图像时车辆的位置坐标和车辆的偏转角度，有上述方法可实现弯道车辆度量级的位姿计算，位姿为：位置坐标为(9.21，1.27)(单位m)，车辆的偏转角度为4.75°。

车辆度量级的位姿计算为采集测试图像时车辆的位置坐标和车辆的偏转角度的计算。

实施例2

除“选取武汉理工大学航海楼门前弯道区域为定位区域，在该区域中，在弯道的道路中心处每隔0.5米采集一次GPS信息，采集GPS的位置作为采样点，在采样点处采集将弯道的GPS信息序列、弯道采样点的路面图像系列以及弯道采样点车辆位姿信息序列共同存入弯道地图数据库，由此构建完成弯道地图数据库，并在不同地点进行40组测试实验”之外，其他同实施例1。

实施例3

除“选取河北工业大学承孝路的弯道区域为定位区域，在该区域中，在弯道的道路中心处每隔0.5米采集一次GPS信息，采集GPS的位置作为采样点，在采样点处采集将弯道的GPS信息序列、弯道采样点的路面图像系列以及弯道采样点车辆位姿信息序列共同存入弯道地图数据库，由此构建完成弯道地图数据库，并在不同地点进行25组测试实验”之外，其他同实施例1。

经过一系列待定位点真实坐标与实施例1-3方法计算的位置坐标对比，证明本发明方法，能够实现高精度的弯道车辆位姿计算，且计算结果均为度量级(厘米级)。

本发明未述及之处适用于现有技术。

Claims (2)

1.一种弯道的车辆位姿计算方法，该方法的具体步骤如下：

第一步，构建弯道地图数据库

1-1、采集弯道GPS信息序列：

基于车载GPS采集设备获取车辆当前的GPS值，即纬度与经度，记为[n,e]；车辆在弯道的道路中心处且车身平行于弯道的切线，每隔0.5-1.5m采集一次GPS信息，采集GPS的位置作为采样点，遍历所有采样点便可获得弯道GPS信息序列；

1-2、采集弯道路面图像：

采集弯道路面图像由车载摄像机系统完成，车载摄像机系统由车载摄像机、控制设备和弯道路面图像存储器和传输器组成，车载摄像机安装在车辆的前盖的横向中心位置，车载摄像机俯角拍摄弯道路面，车载摄像机的光轴到路面的投影与车的纵向平行；利用张正友标定法对车载摄像机进行标定，得到车载摄像机的内参数矩阵K和外参数、弯道路面坐标系与弯道路面图像坐标系间的单应性矩阵H₁及弯道路面图像的中心点(u₀，v₀)；外参数包括旋转矩阵R和平移向量T，弯道路面坐标系与车载摄像机采集的弯道路面图像坐标系的关系满足式(1)：

[u v 1]^T＝H₁[X_w Y_w 1]^T (1)

其中(u v)为车载摄像机采集的弯道路面图像的像素坐标，(X_w Y_w)为路面坐标；

在上述步骤1-1进行时由车载摄像机系统在采样点采集弯道路面的图像，得一系列弯道路面图像，每张弯道路面图像对应唯一的GPS值；将弯道路面图像的中心点带入式(1)得到车载摄像机的光轴与路面的交点记为A,A＝(X₀ Y₀)，车载摄像机投影到弯道路面的点与A点的连线即为光轴投影到弯道路面的线段，记为l；

1-3、采样点处车辆的位姿信息序列：

每个采样点的坐标根据路测激光确定；在步骤1-1和步骤1-2进行时路测激光实时采集激光数据，根据激光数据确定车辆的位姿信息，即车辆所处的位置和车辆偏转角度，由于步骤1-1中设置了车辆在弯道的道路中心处且车身平行于弯道的切线，因此路测激光测得的此时车辆偏转角度均为0，第i个采样点处车辆的位置坐标为M_i(i＝1,2,3,……)，从而得到采样点处车辆的位姿信息序列；

1-4、弯道地图数据库：

每一张弯道路面图像对应唯一步骤1-1中采集的GPS值，对应唯一唯一步骤1-3中位姿信息，每个采样点利用已安装的车载GPS采集设备获取车辆当前的GPS，将弯道GPS信息序列、一系列弯道路面图像以及车辆的位姿信息序列共同存入弯道地图数据库，由此完成弯道地图数据库的构建；

第二步，GPS初定位：

2-1、采集测试位置车辆的GPS信息与弯道路面图像：

测试位置即为车辆所在的当前位置，记为点P，在弯道内利用车载GPS采集设备采集测试位置车辆的GPS值，得到点P处GPS值的纬度与经度；利用车载摄像机系统在当前位置采集弯道路面的图像，得到测试位置的弯道路面图像，称为被测试图像；

2-2、GPS初定位阶段：

将步骤2-1得到的点P处的GPS值与弯道地图数据库中GPS值进行匹配，选取弯道地图数据库中采样点处的GPS值与点P处的GPS值最相近的n个采样点，n不小于5，且不大于10，n个采样点按弯道位置排序分别记为M_j+1，M_j+2，M_j+3，M_j+4，M_j+5……M_j+n，即可实现GPS初定位；从弯道地图数据库调取n个采样点处采集的图像组成图像训练集，该图像训练集中的每个采样点含有GPS信息序列、弯道路面图像以及由路测激光确定的车辆的位姿信息的；当弯道不足n个采样点时GPS初定位结果为弯道地图数据库中全部的采样点；

第三步，图像级定位：

首先分别将上述步骤2-1和步骤2-2得到的被测试图像和图像训练集分别进行预处理，所述预处理包括归一化处理、均衡化和灰度化处理，将预处理后图像的中心作为ORB特征点，把该整个图像作为此特征点的邻域区域，再计算得到ORB全局特征描述符，以该ORB全局特征描述符作为图像的ORB全局特征；

使用WH-KNN算法按照式(2)分别计算被测试图像与图像训练集中每张图像的汉明距离，

其中，H¹为被测试图像的ORB全局特征描述符字符串，H²为图像训练集中一张弯道路面图像的ORB全局特征描述符字符串，

和

分别为ORB全局特征描述符256位字符串的第i位、D_R(H¹,H²)为两张图像间的汉明距离、xor为位异或运算；

比较所有汉明距离的大小，找出汉明距离最小的一张图像作为与被测试图像最相近的图像训练集中的一张图像，称为图像级定位的最匹配图像，确定图像训练集中的最匹配图像后，就能确定出该最匹配图像下所对应的由路测激光确定的车辆的位姿信息，因为图像训练集中的采样点图像在弯道地图数据库中有其对应的由路测激光确定的车辆的位姿信息，此最匹配图像对应的位置坐标即为图像级定位坐标(x_v,y_v)，这个位置坐标就是弯道地图数据库中由路测激光确定的车辆所处的位置，由此完成图像级定位；

第四步，车辆度量级的位姿计算：

将第三步得到的图像级定位的最匹配图像与当前预处理后的被测试图像进行ORB局部特征匹配，再利用RANSAC算法去除误匹配，分别得到最匹配图像与当前预处理后的被测试图像对应的两组特征点序列，且两组特征点序列之间的关系满足式(3)，

[u v 1]^T＝H₀[u₁ v₁ 1]^T (3)

其中H₀为预处理后的最匹配图像与当前被测试图像之间对应的单应性矩阵，(u,v)是最匹配图像的弯道路面图像像素坐标，最匹配的弯道路面图像是弯道地图数据库中的一张图像，(u₁,v₁)被测试图像的弯道路面图像像素坐标；

再联立式(1)和式(3)得到式(4)，

[u₁ v₁ 1]^T＝H₀ ^-1H₁[X_w Y_w 1]^T (4)

其中，H₀ ^-1H₁为弯道路面坐标系与被测试图像坐标系对应的单应性矩阵，H₀ ^-1H₁记为H；

再根据张正友标定法中单应性矩阵H与旋转矩阵R、平移向量T之间的关系，计算出拍摄被测试图像时车载摄像机的外参数，即旋转矩阵R和平移向量T；然后根据-R^-1T计算车载摄像机所在世界坐标系的位置，判断拍摄当前被测试图像和最匹配图像的车载摄像机位置的差，记为车辆的偏离距离，将第三步得到的图像级定位坐标(x_v,y_v)也就是对应最匹配图像的由路测激光确定的车辆所处的位置加上车载摄像机位置的差，即为拍摄被测试图像时车辆的当前位置坐标，由此完成在弯道行驶时车辆位置坐标的计算；

拍摄被测试图像时车载摄相机投影到弯道路面的点与A点的连线即光轴投影到弯道路面上的线段，记为l₁；再在弯道地图数据库中找出最匹配图像的光轴投影到弯道路面的线段，由于设置弯道地图数据库中车辆偏转角度均为0，则两线段的夹角即为车辆偏转角度；至此完成弯道车辆度量级的位姿计算。

2.根据权利要求1所述的弯道的车辆位姿计算方法，其特征在于所述路测激光的精度要求在200m内，误差不大于2mm。

Images