一种基于高精度三维地图的视觉定位系统及方法与流程

本发明涉及智能交通系统领域，尤其涉及一种基于高精度三维地图的视觉定位系统及方法。

背景技术：

高精度车辆定位是实现自动驾驶的基础。传统定位是全球卫星导航系统(gnss)，但该系统定位精度在10m左右，难以达到无人车高精度定位的要求，使得高速行驶的无人车存在着安全隐患。若要提高定位精度，目前能够达到高精度定位的系统大多成本高昂，比如使用惯性导航系统(ins)与gnss组合导航系统定位能达到20cm左右的定位精度，但惯性导航系统成本大概在20万元以上，远高于现在受众最普遍的汽车成本。

为了使定位导航精度达到亚米级，国内外学者各有研究。目前美国谷歌公司主要通过gps与激光雷达、声纳等传感器数据融合的定位方法来实现无人车的导航定位，德国已经实现了利用周围分布的wifi信号匹配实现高精度定位，国内邓中亮提出的“羲和”系统可以解决gnss定位信号定位精度问题从而实现亚米级定位。然而，以上的高精度定位方法要么或单个成本高昂，要么或需要政府投入大量财政进行基础公共设备。

技术实现要素：

本发明要解决的技术问题在于针对现有技术中高精度的惯导定位系统成本高，而普通的gnss定位系统定位精度较低的缺陷，提供一种基于高精度三维地图的视觉定位系统及方法。

本发明解决其技术问题所采用的技术方案是：

本发明提供一种基于高精度三维地图的视觉定位系统，包括数据采集模块、采集辅助模块、数据处理模块和电源模块；其中：

数据采集模块，包括车载摄像机和gnss接收机，用于在车辆行驶到待定位位置时，通过车载摄像机采集当前位置的图像数据，并通过gnss接收机获取相应的gnss数据；

采集辅助模块，包括网络传输线和gnss数据传输线，网络传输线用于将采集到的图像数据传输给数据处理模块，gnss数据传输线用于将gnss数据传输给数据处理模块；

电源模块，用于为数据采集模块、采集辅助模块和数据处理模块供电；

数据处理模块中存储有包含惯导数据、图像特征数据和三维信息的高精度三维视觉地图；数据处理模块用于将gnss数据与高精度三维视觉地图中的惯导数据进行匹配，得到初步定位结果；提取当前位置的图像数据中的全局特征，将其与高精度三维视觉地图中初步定位结果范围内的图像特征数据进行匹配，得到图像级定位结果；提取当前位置的图像数据中的局部特征，结合高精度三维视觉地图中图像级定位结果对应的三维信息进行计算，得到当前车辆的位姿，进而对图像级定位结果进行修正得到最终定位结果。

进一步地，本发明的数据处理模块中的高精度三维视觉地图的获取方法为：

在待定位路段上，以等距的方式在每一个采集点，用车载摄像机与惯导导航系统组合载波差分系统采集惯导数据、图像特征数据和三维信息数据制作高精度三维视觉地图，图像特征数据包括全局特征数据和局部特征数据。

本发明提供一种基于高精度三维地图的视觉定位方法，该方法包括以下步骤：

s1、车辆行驶至待定位位置时，采集当前位置的图像数据，并获取相应的gnss数据，将gnss数据与高精度三维视觉地图中的惯导数据进行匹配，得到初步定位结果，初步定位结果包括高精度三维视觉地图中的多个数据采集点；

s2、提取当前位置的图像数据中的全局特征，将其与初步定位结果中多个数据采集点的全局图像特征数据进行匹配，得到距离最近的数据采集点作为图像级定位结果；

s3、提取当前位置的图像数据中的局部特征，与图像级定位结果对应存储在高精度三维视觉地图中的局部特征相匹配，结合匹配到的局部特征点对应的三维信息进行计算，得到当前车辆的位姿，进而对图像级定位结果进行修正得到最终定位结果。

进一步地，本发明的步骤s1中计算初步定位结果的方法具体为：

将当前位置的gnss数据转换为欧式坐标，计算当前位置与高精度三维视觉地图中的各个数据采集点之间的欧式距离，其公式为：

di＝[||an1-bqi||2，||an2-bqi||2，||an3-bqi||2…]

其中，anj＝(xnj，ynj)表示高精度三维视觉地图中的各个数据采集点的惯导数据，bqi＝(xqi，yqi)表示当前位置的gnss数据对应的欧式坐标，x、y为经纬度信息；

结合gnss信号精度和高精度三维视觉地图的采集频率，选取高精度三维视觉地图中相邻的若干个数据采集点作为初步定位结果。

进一步地，本发明的步骤s2中计算图像级定位结果的方法具体为：

对当前位置的图像数据进行灰度化，将灰度化后的图像重置为标准图像尺寸，其分辨率为n×n像素，并计算当前位置的全局特征；将得到的全局特征与初步定位结果中多个数据采集点对应的图像特征数据进行匹配，计算它们之间的海明距离，其公式为：

其中，e表示当前位置的图像数据的全局特征，f表示初步定位结果中数据采集点对应的图像特征数据；

选取海明距离最短的数据采集点为图像级定位结果。

进一步地，本发明的步骤s3中计算最终定位结果的方法具体为：

将当前位置的图像数据重置为标准图像尺寸，其分辨率为m×n像素，提取图像数据中的局部特征，将当前位置图像与图像级定位结果存储在高精度三维视觉地图中的局部特征相匹配，得到多个匹配的局部特征点，并提取匹配到的局部特征点所对应的三维信息；

结合当前位置图像中的局部特征与高精度三维视觉地图中的三维信息计算车辆在高精度三维视觉地图中的位姿。

进一步地，本发明的根据计算得到的位姿对图像级定位结果进行验证和修正的方法具体为：

根据计算得到的位姿计算车辆当前位置与图像级定位结果对应的采集点之间的距离，结合高精度三维视觉地图数据采集频率，如果该距离大于采集频率的一半，表明得到的图像级定位结果不是距离汽车当前位置最近的采集点，根据计算得到的位姿决定是选择图像级定位结果前一个采集点还是后一个采集点作为新的待定位结果，然后验证新的待定位结果是否符合精度要求，如果符合，则确定该采集点为测量级定位结果即最终定位结果。

本发明产生的有益效果是：本发明的基于高精度三维地图的视觉定位系统及方法，以高精度三维视觉地图为基础，首先进行gnss信息匹配的初步定位，其次进行全局特征匹配的图像级定位，最后根据当前位置图像中的局部特征点与图像级定位结果中的三维信息计算两者的位姿关系，并对图像级定位结果进行验证和修正得到最终定位结果。与现有技术相比较，本发明在gnss初步定位后，增加了图像级定位和测量级定位从而实现精确定位，在不需要大幅增加硬件成本的基础上，提高了车辆定位的精度，降低了定位成本。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的结构示意图；

图2是本发明实施例的方法流程图；

图中:1.车载计算机；2.车载摄像机；3.gnss接收机；4.稳压直流电源；5.网络传输线；6.gnss数据传输线；7.电源线。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，本发明实施例的基于高精度三维地图的视觉定位系统，包括数据采集模块、采集辅助模块、数据处理模块和电源模块；其中：

数据采集模块，包括车载摄像机和gnss接收机，用于在车辆行驶到待定位位置时，通过车载摄像机采集当前位置的图像数据，并通过gnss接收机获取相应的gnss数据；

采集辅助模块，包括网络传输线和gnss数据传输线，网络传输线用于将采集到的图像数据传输给数据处理模块，gnss数据传输线用于将gnss数据传输给数据处理模块；

电源模块，用于为数据采集模块、采集辅助模块和数据处理模块供电；

数据处理模块中存储有包含惯导数据、图像特征数据和三维信息的高精度三维视觉地图；数据处理模块用于将gnss数据与高精度三维视觉地图中的惯导数据进行匹配，得到初步定位结果；提取当前位置的图像数据中的全局特征，将其与高精度三维视觉地图中初步定位结果范围内的图像特征数据进行匹配，得到图像级定位结果；提取当前位置的图像数据中的局部特征，结合高精度三维视觉地图中图像级定位结果对应的三维信息进行计算，得到当前车辆的位姿，进而对图像级定位结果进行修正得到最终定位结果。

数据处理模块中的高精度三维视觉地图的获取方法为：

在待定位路段上，以等距的方式在每一个采集点，用车载摄像机与惯导导航系统组合载波差分系统采集惯导数据、图像特征数据和三维信息数据制作高精度三维视觉地图，图像特征数据包括全局特征数据和局部特征数据。

如图2所示，本发明实施例的基于高精度三维地图的视觉定位方法，包括以下步骤：

s1、车辆行驶至待定位位置时，采集当前位置的图像数据，并获取相应的gnss数据，将gnss数据与高精度三维视觉地图中的惯导数据进行匹配，得到初步定位结果，初步定位结果包括高精度三维视觉地图中的多个数据采集点；

s2、提取当前位置的图像数据中的全局特征，将其与初步定位结果中多个数据采集点的全局图像特征数据进行匹配，得到距离最近的数据采集点作为图像级定位结果；

s3、提取当前位置的图像数据中的局部特征，与图像级定位结果对应存储在高精度三维视觉地图中的局部特征相匹配，结合匹配到的局部特征点对应的三维信息进行计算，得到当前车辆的位姿，进而对图像级定位结果进行修正得到最终定位结果。

步骤s1中计算初步定位结果的方法具体为：

将当前位置的gnss数据转换为欧式坐标，计算当前位置与高精度三维视觉地图中的各个数据采集点之间的欧式距离，其公式为：

di＝[||an1-bqi||2，||an2-bqi||2，||an3-bqi||2…]

其中，anj＝(xnj，ynj)表示高精度三维视觉地图中的各个数据采集点的惯导数据，bqi＝(xqi，yqi)表示当前位置的gnss数据对应的欧式坐标，x、y为经纬度信息；

结合gnss信号精度和高精度三维视觉地图的采集频率，选取高精度三维视觉地图中相邻的若干个数据采集点作为初步定位结果。

步骤s2中计算图像级定位结果的方法具体为：

对当前位置的图像数据进行灰度化，将灰度化后的图像重置为标准图像尺寸，其分辨率为n×n像素，并计算当前位置的全局特征；将得到的全局特征与初步定位结果中多个数据采集点对应的图像特征数据进行匹配，计算它们之间的海明距离，其公式为：

其中，e表示当前位置的图像数据的全局特征，f表示初步定位结果中数据采集点对应的图像特征数据；

选取海明距离最短的数据采集点为图像级定位结果。

步骤s3中计算最终定位结果的方法具体为：

将当前位置的图像数据重置为标准图像尺寸，其分辨率为m×n像素，提取图像数据中的局部特征，将当前位置图像与图像级定位结果存储在高精度三维视觉地图中的局部特征相匹配，得到多个匹配的局部特征点，并提取匹配到的局部特征点所对应的三维信息；

结合当前位置图像中的局部特征与高精度三维视觉地图中的三维信息计算车辆在高精度三维视觉地图中的位姿；

根据计算得到的位姿计算车辆当前位置与图像级定位结果对应的采集点之间的距离，结合高精度三维视觉地图数据采集频率，如果该距离大于采集频率的一半，表明得到的图像级定位结果不是距离汽车当前位置最近的采集点，根据计算得到的位姿决定是选择图像级定位结果前一个采集点还是后一个采集点作为新的待定位结果，然后验证新的待定位结果是否符合精度要求，如果符合，则确定该采集点为测量级定位结果即最终定位结果。

在本发明的另一个具体实施例中：

基于高精度三维地图的视觉定位系统，包括数据采集模块、采集辅助模块、数据处理模块和电源模块；其中

本实施例中，数据采集模块包括车载摄像机2和gnss接收机3；

在定位系统中，车辆每行驶到一个任意待定位位置，由车载摄像机2接收图像信息，由gnss接收机3接收gnss信息(经纬度信息)，并将其传输至计算机1；

本实施例中，采集辅助模块包括网络传输线5、gnss数据传输线6以及电源线7；

在定位系统中，图像信息通过网络传输网线5传输至计算机1，gnss信息通过gnss数据传输线6传输至计算机1，稳压直流电源通过各类电源线7向车载计算机1、车载单目摄像机2、gnss接收机3供电；

本实施例中，数据处理模块主要由车载计算机1组成；

在定位系统中，计算机1存储高精度三维视觉地图信息，接收并处理由车载摄像机2和gnss接收机3传输的图像信息和gnss信息，包括初步定位中的gnss数据匹配，图像级定位中的全局特征点提取与匹配，测量级定位中的三维局部特征点匹配并计算车辆当前位姿，最终实现定位功能；

本实施例中，电源模块主要包括稳压直流电源4；

在定位系统中，稳压直流电源4可以为车载电瓶，可以通过各类电源线7向车载计算机1、车载单目摄像机2、gnss接收机3供电。

在实际应用中，gnss定位精度在10m左右，尚未达到高精度定位的要求，使得在高速行驶的道路上智能车辆存在着重大安全隐患。本发明所述的基于高精度三维地图的视觉定位方法以高精度三维视觉地图为基础，根据定位系统数据采集模块所采集的信息，可以将定位过程分为三部分：(1)通过gnss数据与视觉地图中高精度惯导数据匹配实现初步定位，(2)通过当前位置图像全局特征与初步定位结果中的每帧图像特征相匹配实现图像级定位，(3)通过待定位结果图像与当前位置图像的三维局部特征匹配实现测量级定位。

利用上述基于高精度三维地图的视觉定位方法，包括以下步骤：

s1、通过gnss数据与视觉地图中高精度惯导数据匹配实现初步定位。

gnss数据和高精度惯导数据都是在地理坐标系下采集的，计算两者之间的距离需要转换为欧式坐标系，其中视觉地图中存储的是已经转换为了欧式坐标的高精度惯导数据，可以通过公式trans()将gnss数据转换为欧式坐标。如下式所示：

bqi＝(xqi，yqi)＝trans(xqi，yqi)

其中，sqi＝(xqi，yqi)为gnss数据的地理坐标，bqi＝(xqi，yqi)表示gnss数据经公式trans()转换后的欧式坐标。

然后用距离公式计算视觉地图中各采集点存储在视觉地图中惯导数据与当前位置图像的gnss数据的欧式距离，如下式所示：

di＝[||an1-bqi||2，||an2-bqi||2，||an3-bqi||2…]

其中，anj＝(xnj，ynj)是高精度惯导数据的欧式坐标。由式(1)选取距离车辆当前位置最近的采集点。

本实施例中，由于gnss定位精度在10m左右，结合制作视觉地图时的数据采集频率，本实施例中取为2m/帧，故选取距离当前gnss数据最近采集点图像及其前后2帧图像采集点共5帧图像作为初步定位结果。

s2、通过当前位置图像全局特征与初步定位结果中的每帧图像特征相匹配实现图像级定位。

本实施例中，所用车载单目摄像机所拍摄的图片尺寸为m×n(pixel)。首先，由于图片色彩对结果影响不大，可以对当前位置图像进行灰度化处理，其次为了排除图像尺寸对结果的影响，将当前位置图像尺寸重置为标准图像尺寸即视觉地图中的标准图像尺寸，本实施例中取为63×63(pixel)，然后利用计算机视觉技术提取当前位置图像的全局特征描述符fq。

最后提取初步定位结果每帧图像存储在视觉地图中的全局特征，并与当前位置图像的全局特征相匹配，即分别计算每帧图像的全局特征与当前位置图像的全局特征间的海明距离，海明距离计算公式如下式所示：

式(2)中，e,f分别表示当前位置图像与初步定位结果中图像的全局特征描述符；选取初步定位结果中与当前位置图像海明距离最近的那一帧图像作为图像级定位结果，如下式所示：

其中，fi(i＝1，2，...，n)为初定位结果中每帧图像的全局特征，fq为当前位置图像的全局特征。

s3、通过图像级定位结果与当前位置图像的三维局部特征匹配实现测量级定位。

本实施例中，首先，为了排除图像尺寸对结果的影响，将当前位置图像尺寸重置为标准图像尺寸m×n(pixel)，采用计算机视觉技术提取当前位置图像局部特征，并将图像级定位结果存储在视觉地图中的局部特征与当前位置图像进行局部特征匹配，如果两个图像的各个局部特征点相匹配，则提取图像级定位结果存储在视觉地图中的相对应的三维信息。

然后，通过相对应的局部特征与三维特征点，可以计算两者之间的位姿关系，如下式所示：

其中，[wjkj1]^t为当前位置图像所对应的第j个局部特征点坐标；[ajbjcj1]^t为图像级定位结果图像中对应的第j个三维特征点，p为车载摄像机的内参数，为3×3矩阵；q和s分别表示当前位置图像与图像级定位结果间的旋转与平移关系，其中，q为旋转矩阵，尺寸为3×3，s为平移向量，尺寸为3×1，这两个值表示了车辆当前位置与图像级定位结果图像的位姿关系；表示线性关系，即等式左边与右边相差一个尺度因子，尺度因子可由对应特征点求解。

根据(3)式求得q和s后即得到位姿关系后，可通过平移向量s来计算车辆当前位置与图像级定位结果对应的采集点之间的距离：

本实施例中，最后根据(4)式得到的l验证待定位结果是否符合精度要求，结合高精度三维视觉地图数据采集频率，本实施例中取为2m/帧，所以如果l大于1m，表明得到的图像级定位结果不是距离汽车当前位置最近的采集点，然后由q与s决定是选择图像级定位结果前一个采集点还是后一个采集点作为新的待定位结果，然后验证新的待定位结果是否符合精度要求，如果符合，则确定该采集点为测量级定位结果即最终定位结果。

因此，本发明能够在gnss初步定位后利用计算机视觉技术进一步定位，将车辆定位精度提高至亚米级。

本发明提出的基于高精度三维地图的视觉定位系统与方法，该方法将计算机视觉技术与gnss相结合，运用价格低廉的车载单目相机，采用多尺度匹配的方法实现亚米级车辆定位精度。具体而言，首先运用普通gnss信息与视觉地图中的位置信息匹配实现初步定位，其次，提取道路场景的图像特征并与视觉地图中的图像特征信息进行匹配，实现图像级定位，最后，运用三维特征匹配实现测量级定位，得到车辆在地图中的位置。由于定位数据和图像数据的采集工具只需普通gnss接收机和车载摄像头，本发明所需的成本远低于目前其他高精度定位系统的成本，这有利于自动驾驶系统在车辆上的大面积推广。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。