本发明涉及为车辆定位的安全辅助驾驶领域，具体地说是一种基于视觉、GPS与高精度地图融合的车辆定位方法。
背景技术：
国家机动业的提升带动了汽车产业的蓬勃发展，据统计，到2016年底，国内汽车拥有量约为1.94亿，如此庞大的数量引起的城市交通问题也渐渐凸显，精确实时的车辆定位对于缓解行车安全、自主导航定位、交通拥堵等问题具有重大意义。目前GPS定位核心技术成熟且使用广泛，但其定位误差为10m左右，精度较低，在高大建筑物林立、树木密集、桥梁隧道等环境下存在信号盲区，通常结合高精度组合惯导系统提高GPS定位精度，但因成本较高无法普遍使用。而视觉定位具有成本低、精度高、信息多等优点，近年来在车辆定位方向展现出了非常好的应用前景，并系统性地发展出一些如视觉同时定位与制图以及视觉里程计等先进的视觉定位技术，但存在计算复杂度高、稳定性差等缺点，且无法避免目标拍摄不完全的问题。
结合各个车辆定位方法的优势，到目前为止，融合定位提高车辆定位精度已经有了广泛的应用。CN106256644A公开了一种识别车辆位置和方向的方法，利用GPS获取车辆位置并使用雷达传感器计算车辆至位置处静态对象的距离，并通过目标检测获取视觉提示，但该方法仅限于特定静态环境下的应用，即车辆处于静止状态时才能定位识别。CN107664500A公开了一种预先对停车位进行编号、记录行车道信息生成车库的电子地图，然后根据识别地图中车位编号的顺序确定车辆的行驶方向和路线的方法，此方法需实时更新停车位的闲置状态,且路线及行驶方向是根据识别的停车位编号排序确定的，存在前后时刻的关联。综上所述，通过融合多种定位技术实现车辆定位的准确性、实时性、鲁棒性的方法还有待研究。
技术实现要素：
针对现有技术的不足，本发明所要解决的技术问题是：提供一种基于视觉、GPS与高精度地图融合的车辆定位方法。该方法在离线阶段事先生成路面箭头的高精度地图，以路面箭头为核心元素的地图表征，即从路面箭头的图像特征、地理位置信息以及几何结构信息来对路面路标进行表征，使之适用于智能车高精度定位；在线定位阶段，将车载GPS坐标转化为以路面箭头为空间平面坐标系下的平面坐标，然后利用几何结构计算箭头各边的方程及距车辆的距离，最后利用Kalman滤波实现视觉定位和GPS定位的信息融合，克服了传统的基于点与点之间的距离的非线性约束，利用点到箭头各边之间的距离作为约束，构造线性Kalman滤波器，提高了系统的精度，得到车辆在全局坐标系下的更加精确位置坐标。该方法中地图的构建解决了某些复杂路段地图数据缺失和路标磨损的问题，融合定位克服了GPS定位精度低和视觉定位存在累积误差、无法全局定位的问题，从而达到厘米级定位精度。
本发明解决所述技术问题采用的技术方案是：提供一种基于视觉、GPS与高精度地图融合的车辆定位方法，该方法首先将地图中惯导数据和车载GPS坐标转换为空间平面坐标，匹配得出距离当前车辆位置最近的路面箭头，确定视觉定位范围，然后使用Kalman滤波器将GPS定位与视觉定位结果融合，利用视觉定位数据修正GPS定位结果，得到车辆在全局坐标系下更精确的位置信息，完成定位，具体步骤是：
第一步，构建高精度路面箭头地图：
1-1、地图数据采集：
高精度路面箭头地图构建过程中以路面箭头为核心元素，通过测量工具测量箭头的几何结构信息，运用车载摄像机对路面箭头进行图像采集，同时运用GPS接收机、组合惯导系统以及DGPS基站采集该箭头的位置信息，位置信息数据采集以路面箭头的几何中心为准，位置信息组成的惯导数据记为G＝{(Bm1，Lm1)，(Bm2，Lm2)，(Bm3，Lm3)，…}，Bmi，Lmi分别代表第i个箭头的纬度值与经度值，以箭头的几何结构信息、图像信息、惯导数据三部分生成高精度地图，使得地图中的每一个箭头均包含一帧图像信息、高精度惯导数据及几何结构信息；
1-2、惯导数据坐标转换：
将采集的惯导数据转换为空间平面坐标系下的坐标Xmi＝(xmi，ymi)＝trans(Bmi，Lmi)，其中(xmi，ymi)、(Bmi，Lmi)分别为第i个箭头的空间平面坐标系坐标和惯导数据，trans()为由惯导数据转化为空间平面坐标的转换公式；
第二步，GPS定位：
2-1、车载GPS坐标转换：
将当前车辆测试位置记为点P，利用车载GPS采集设备采集点P的GPS坐标为(B，L)，B代表纬度值，L代表经度值；根据转换公式trans()将GPS坐标转化为空间平面坐标系下的坐标Xp，即Xp＝(xp，yp)＝trans(B，L)，(xp，yp)为采集点P的空间平面坐标；同时利用车载摄像机在当前位置采集箭头图像；
2-2、GPS定位选定最近参照路标：
将步骤2-1得到的点P处的空间平面坐标与步骤1-2高精度路面箭头地图中惯导数据的空间平面坐标进行GPS信息匹配，选取地图中采样点处惯导数据与当前车辆位置点P最相近的n个箭头，根据二者转换后的空间平面坐标计算当前车辆位置与选取箭头的距离，距离D最小的作为接下来视觉定位的参照箭头，D＝min{||Xmi-Xp‖2}，即为最近参照路标；
第三步，视觉定位：
以步骤2-2选出的参照箭头所在路面建立空间平面坐标系，根据地图中采集的箭头的几何结构信息写出箭头各角点的空间平面坐标，由两点式直线方程分别计算箭头各边的方程li：aix+biy+ci＝0，然后根据当前车辆位置点P的空间平面坐标Xp计算其到路面箭头各边li的直线距离
第四步，Kalman滤波器融合定位：
4-1、GPS定位数据作为Kalman滤波器的预测数据：
针对单帧非连续图像，不存在时刻的关联，将车载GPS坐标转化后的空间平面坐标Xp作为Kalman滤波器的预测值设定预测矩阵为预测误差的协方差矩阵为其中Q为GPS定位精度；
4-2、视觉定位数据作为Kalman滤波器的观测数据：
将当前车辆位置的空间平面坐标Xp及其到路面箭头各边li的直线距离di作为Kalman滤波器的观测值z，
其中，H为观测矩阵，记为
其中lix，liy分别为当前车辆位置空间平面坐标(xp，yp)到直线li的距离关于横轴与纵轴的关系式分量，
多次测量车到各边距离的均值zk与观测值z的平均误差为观测误差的协方差矩阵为Rk，
其中为多次测量的各测量误差数据的方差；
4-3、Kalman滤波信息融合：
将步骤4-1的GPS定位得到的预测数据和步骤4-2的视觉定位得到的观测数据作为卡尔曼滤波器的输入，由Kalman滤波公式得到融合后的定位结果，该融合后的定位结果即为精确车辆平面坐标Xp′，再将该平面坐标按照(B′，L′)＝trans-1(xp′，yp′)转化为GPS坐标，其中x′p，y′p为Xp′的横轴与纵轴的坐标值，得到更加精确的全局坐标系下的坐标(B′，L′)，至此，基于视觉、GPS与高精度地图融合的车辆定位方法已经完成；其中Kalman滤波公式为：
其中Xp'为融合定位后当前车辆位置的空间平面坐标；Rk观测误差的协方差矩阵，K为Kalman滤波器的增益，为视觉测量误差均值。
与现有技术相比，本发明的有益效果是：
本发明的突出的实质性特点如下：
(1)本发明一种基于视觉、GPS与高精度地图融合的车辆定位方法，离线阶段使用高精度惯导、卷尺、直角尺等工具构建路面箭头地图。在线定位首先利用当前车辆的GPS位置信息实现坐标转换，并与地图中惯导数据匹配确定视觉定位范围，然后利用视觉计算当前车辆到箭头各边的距离选定最近参照路标，最后利用Kalman滤波器融合定位数据信息。实验结果表明，本发明提出的三种定位技术相融合的定位方法具有成本低、速度快、精度高、鲁棒性强的显著性优势。
(2)本发明方法中，三种定位方法的互补之处在于：高精度地图的构建含有路面箭头的几何尺寸信息，解决了因长期暴晒、雨雪等环境因素导致路标模糊不清影响视觉定位的问题，同时解决了某些复杂地段在世界地图中没有数据的缺陷；GPS定位误差在10m左右，精度较低，难以满足如今智能车定位精度要求，可利用视觉定位来修正GPS定位结果；视觉定位一般需要选取参照物进行局部定位，仅能得到车辆在局部坐标系下的相对位置关系，无法得到车辆在全局坐标系下的位置，且存在误差积累，而利用GPS定位结果首先选取地图中的参照箭头，减小了视觉计算的时间和误差。故本发明创造性地利用了三种定位的优势，为智能车定位提供了一种新思想。
本发明的显著进步如下：
(1)本发明创新地将GPS定位、视觉定位、地图定位相结合，扬长避短，可方便地实现高精度车辆定位。
(2)本发明方法可以有效提高车载GPS定位精度，传统的GPS定位精度为10m，融合定位后的定位误差为厘米级。
(3)本发明方法在高精度地图的构建过程中，提出了以路面箭头为核心元素的地图表征，即从路面箭头的图像特征、地理位置信息以及几何结构信息来对路面路标进行表征，使之适用于智能车高精度定位，且充分利用了路面箭头的几何信息，减小了环境因素造成路标视野不完全对视觉定位的影响，弥补了世界地图存在道路数据空缺的问题。
(4)本发明方法首先通过GPS、地图定位确定了视觉定位范围，避免了视觉定位大量冗余计算，提高了运算速度。
(5)本发明方法在视觉定位部分，充分利用路面箭头的几何结构信息构建空间平面坐标系，根据路面箭头的角点坐标直接计算箭头各边的直线方程及车到各边的距离，减小视觉累积误差。
(6)本发明方法使用Kalman滤波器实现GPS与视觉定位的融合，在Kalman滤波框架中，克服了传统的基于点与点之间的距离的非线性约束，创新性的提出利用点到路面箭头各边之间的距离作为约束，构造线性Kalman滤波器，提高了系统的精度。
(7)本发明方法实现了全局坐标系下的定位，为车辆在实时复杂道路环境中提供准确决策和规划。
(8)本发明方法以路面箭头为参照路标，在众多道路资产中具有种类、颜色、尺寸、角度较为统一的优势，适用于所有包含箭头的路段，适用范围更加广泛。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1为本发明方法的步骤流程示意框图。
图2为本发明方法的GPS定位示意图。
图3为本发明方法的路面箭头空间平面坐标系的建立。
图4为本发明方法中GPS定位与融合定位后的定位误差对比图。
具体实施方式
下面结合附图和实施例进一步说明本发明，但并不以此作为对本申请保护范围的限定。
本发明一种基于视觉、GPS与高精度地图融合的车辆定位方法(参见图1)，首先将地图中惯导数据和车载GPS坐标转换为空间平面坐标，匹配得出距离当前车辆位置最近的路面箭头，确定视觉定位范围，然后使用Kalman滤波器将GPS定位与视觉定位结果融合，利用视觉定位数据修正GPS定位结果，得到车辆在全局坐标系下更精确的位置信息，完成定位，具体步骤是：
第一步，构建高精度路面箭头地图：
坐标统一说明：构建地图时箭头的位置信息称为惯导数据，GPS定位时确定车辆位置信息称为GPS坐标，这两个位置信息的单位都是由经纬度表示，只是采集的设备不同，由经纬度坐标转换为空间平面坐标的转换公式均为trans()。
1-1、地图数据采集：
高精度路面箭头地图构建过程中以路面箭头为核心元素，当路面有指示箭头存在时，一方面通过测量工具(卷尺、直角尺等)测量箭头的几何结构信息，另一方面运用车载摄像机进行图像采集，同时运用GPS接收机、组合惯导系统以及DGPS基站采集该箭头的位置信息，值得说明的是，位置信息数据采集以路面箭头的几何中心为准，位置信息组成的惯导数据记为G＝{(Bm1,Lm1),(Bm2,Lm2),(Bm3,Lm3),···}，Bmi,Lmi分别代表第i个箭头的纬度值与经度值，以箭头的几何结构信息、图像信息、惯导数据三部分生成高精度地图，使得地图中的每一个箭头均包含一帧图像信息、高精度惯导数据及几何结构信息；
1-2、惯导数据坐标转换：
将采集的惯导数据转换为空间平面坐标系下的坐标Xmi＝(xmi,ymi)＝trans(Bmi,Lmi)，其中(xmi,ymi)、(Bmi,Lmi)分别为第i个箭头的空间平面坐标系坐标和惯导数据，trans()为由惯导数据转化为空间平面坐标的转换公式；
第二步，GPS定位：
2-1、车载GPS坐标转换：
将当前车辆测试位置记为点P，利用车载GPS采集设备采集点P的GPS坐标为(B，L)，B代表纬度值，L代表经度值；同时转化为空间平面坐标系下的坐标为Xp＝(xp，yp)＝trans(B，L)，trans()为由GPS坐标转化为空间平面坐标的转换公式，同时利用车载摄像机在当前位置采集箭头图像；
2-2、GPS定位选定最近参照路标：
将步骤2-1得到的点P处的空间平面坐标与步骤1-2高精度路面箭头地图中惯导数据的空间平面坐标进行GPS信息匹配，由于GPS定位精度较低，定位误差Q为10m左右，故选取地图中采样点处惯导数据与当前车辆位置点P最相近的n个箭头(3≤n≤5)，根据二者转换后的空间平面坐标计算当前车辆位置与选取箭头的距离，距离D最小的作为接下来视觉定位的参照箭头，D＝min{||Xmi-Xp‖2}，即为最近参照路标；
第三步，视觉定位：
以步骤2-2选出的参照箭头所在路面建立空间平面坐标系，根据地图中采集的箭头的几何结构信息写出箭头各角点的空间平面坐标，由两点式直线方程分别计算箭头各边的方程li：aix+biy+ci＝0，然后根据当前车辆位置点P的空间平面坐标Xp计算其到路面箭头各边li的直线距离
第四步，Kalman滤波器融合定位：
4-1、GPS定位数据作为Kalman滤波器的预测数据：
针对单帧非连续图像，不存在时刻的关联，将车载GPS坐标转化后的空间平面坐标Xp作为Kalman滤波器的预测值两坐标值间不存在约束关系，故设定预测矩阵为
由于GPS本身存在定位误差，设预测误差的协方差矩阵为Q为GPS定位精度；
4-2、视觉定位数据作为Kalman滤波器的观测数据：
将当前车辆位置的空间平面坐标Xp及其到路面箭头各边li的直线距离di作为Kalman滤波器的观测值z，
其中，H为观测矩阵，记为
其中lix，liy分别为当前车辆位置空间平面坐标(xp，yp)到直线li的距离关于横轴与纵轴的关系式分量，观测值z和观测矩阵H确定的优势在于，克服了传统的基于点与点之间的距离的非线性约束，创新性的利用点到路面箭头各边之间的距离作为约束，构造线性Kalman滤波器，提高了系统的精度。
路面箭头的形状、角度、尺寸在道路资产中大部分都是统一的，但在人工绘制时难免存在角度偏差、不对称等人为因素的影响，存有噪声，使用卷尺、直角尺等工具多次测量出当前车辆位置的空间平面坐标(xp，yp)到箭头各边li的距离即为测量数据zk，测量数据zk与观测值z的平均误差为各测量数据误差之间相互独立互不相关，其协方差矩阵Rk对角线数据即为各测量误差的方差，
其中为多次测量的各测量误差数据的方差；
4-3、Kalman滤波信息融合：
将步骤4-1的GPS定位得到的预测数据和步骤4-2的视觉定位得到的观测数据作为卡尔曼滤波器的输入，由Kalman滤波公式得到融合后的定位结果，该融合后的定位结果即为精确车辆平面坐标Xp′，再将该平面坐标按照(B′，L′)＝trans-1(xp′，yp′)转化为GPS坐标，其中x′p，y′p为Xp′的横轴与纵轴的坐标值，得到更加精确的全局坐标系下的坐标(B′，L′)，至此，基于视觉、GPS与高精度地图融合的车辆定位方法已经完成；
其中Xp′为融合定位后当前车辆位置的空间平面坐标，误差精度为厘米级，满足当今智能车定位精度的需求；Pk为预测误差的协方差矩阵、H为观测矩阵、Rk观测误差的协方差矩阵，K为Kalman滤波器的增益，为视觉测量误差均值，zk为多次测量车到各边距离的均值；
将观测值z和视觉误差的和作为测量数据车到各边距离的均值zk，其中视觉测量误差的引入，提高了视觉定位的结果，减少了直线方程求取过程对实验结果的影响，融合定位后得到精度更高的位置坐标Xp'＝Xp+K(zk-H*Xp)，再经过平面坐标与GPS坐标的转化，得到更加精确的全局坐标系下的坐标，(B',L')＝trans-1(xp',yp')。
GPS、视觉定位数据的融合，充分利用二者定位优势，解决了GPS定位精度低的问题，同时弥补了视觉定位计算复杂度高和局部定位的不足。
图2所示实施例表明，GPS初始匹配结果为Mj-2到Mj+2，最近参照路标为Mj。Mj代表步骤2-2从地图中选出的与当前车辆位置点P的最近参照路面箭头，其他为地图中与Mj相邻的箭头。
图3所示实施例表明,建立路面箭头空间平面坐标系，选取顶点和箭头边缘直线li。
图4所示实施例表明,GPS定位与融合定位的误差对比图。
本发明方法最近参照箭头无论是直行箭头还是右转、左转、双转箭头等均可适用，且定位精度高。
实施例1
本实施例以河北工业大学酬勤道和学熙路为试验场地，以该场地的直行箭头作为实验参照物，车载摄像机系统拍摄的所有图片均为640*480(像素)。
本实施例基于视觉、GPS与高精度地图融合的车辆定位方法，包括以下步骤：
第一步，构建高精度路面箭头地图：
1-1、地图数据采集：
将车载摄像机至于挡风玻璃后，高123cm，俯视向下倾斜约30度角，设实验过程中摄像机保持姿势不动。当路面有指示箭头存在时进行拍摄，同时运用GPS接收机、组合惯导系统以及DGPS基站采集箭头的高精度惯导数据，并利用测量工具(卷尺、直角尺等)记录对应箭头的几何结构信息，使得地图中的每一个箭头均包含一帧图像信息、高精度惯导数据及几何结构信息。实验路段共有15个路面箭头采集点，故采集15组地图数据，每一组数据由图像信息、几何结构信息及高精度惯导数据组成，15组惯导数据序列记为G15＝{(Bm1,Lm1),(Bm2,Lm2),···(Bm15,Lm15)}；
1-2、惯导数据坐标转换：
将采集的惯导数据转换为空间平面坐标系下的坐标Xmi＝(xmi,ymi)＝trans(Bmi,Lmi)(i＝15)，其中(xmi,ymi)、(Bmi,Lmi)分别为惯导数据的平面坐标系坐标和惯导数据，trans()为由惯导数据转化为空间平面坐标的转换公式；
第二步，GPS定位：
2-1、车载坐标转换：
测试位置即为当前车辆所在位置，记为点P，利用车载GPS采集设备采集车辆位置P的GPS坐标为(39.232444°，117.049094°)，利用公式Xp＝(xp,yp)＝trans(B,L)将GPS坐标转换为空间平面坐标系下的坐标。同时利用车载摄像机在当前位置采集箭头图像，得到测试位置的箭头图像称为待测试图像；
2-2、GPS定位选定最近参照路标：
将步骤2-1得到的点P处的空间平面坐标与路面箭头地图中惯导数据平面坐标进行匹配，因为GPS采集设备本身存在10m左右定位误差，故选取地图中采样点处惯导数据与当前车辆位置点P最相近的3个路面箭头，根据转换后的平面坐标计算当前车辆位置与选取箭头的距离D＝min{||Xmj-1-Xp‖2，‖Xmj-Xp‖2，‖Xmj+1-Xp‖2}，距离最小的作为接下来视觉定位的参照箭头。这3个采样点中中间采样箭头与当前车辆测试位置GPS坐标最匹配，选作参照箭头，其对应地图中的惯导数据的坐标为(39.232447°，117.049144°)；
第三步，视觉定位：
查询地图中步骤2-2选出的最近参照箭头的几何结构信息，以其所在路面建立空间平面坐标系，见附图3，直行箭头的五个角点分别记为ABCDE，以直行箭头底边AB的中点为原点O，向右为x轴正方向，箭头所指方向为y轴正方向建立空间平面直角坐标系，结合地图中几何结构信息写出箭头顶点ABCDE的平面坐标(单位：厘米)分别为：A＝(-7.75，0)，B＝(7.75，0)，C＝(22.25，155.5)，D＝(0，276)，E＝(-22.25，15 5.5)，由两点式直线方程分别计算出箭头各边的方程l1：x＝-7.75；l2：x＝7.75；l3：y＝0；l4：y＝155.5；本实施例中不考虑箭头三角的两边，使计算更简单。
第四步，Kalman滤波器融合定位：
4-1、GPS定位数据作为Kalman滤波器的预测数据：
步骤2-1坐标转换后测试位置的平面坐标Xp仅存在两个互不相关的坐标分量xp、yp，且采集的待测试图像为单帧非连续图像，仅由该时刻的位置坐标确定，不存在时刻的关联，将当前车辆测试位置的平面坐标记为Kalman滤波器的预测值其预测矩阵为又因为车载GPS采集设备本身存在约10m的误差，故转换后的空间平面坐标存在噪声误差，误差的协方差矩阵记为
4-2、视觉定位数据作为Kalman滤波器的观测数据：
第三步视觉定位已求出箭头各边li的直线方程，将步骤2-1得到坐标转换后测试位置的平面坐标为(xp，yp)带入点到直线的距离方程式计算车到参照箭头各边的距离为d1＝10.75，d2＝4.75，d3＝151，d4＝306.5(单位：厘米)，当前车辆测试位置平面坐标和车到箭头各边距离作为Kalman滤波器的观测值z＝[3 -151 10.75 4.75 151 306.5]T，观测矩阵为
使用卷尺、直角尺等工具测量7次当前车辆测试位置到参照箭头各边的距离，测量数据的平均值矩阵记为zk，误差均值和协方差矩阵分别记为Rk，
其中，矩阵每一行分别代表各个状态量的平均观测误差，各测量误差数据之间相互独立互不相关，协方差矩阵对角线数据即为各测量误差的方差：
至此，Kalman滤波器的输入数据已经全部确定。
4-3、数据融合：
将步骤4-2的GPS定位得出的预测数据作为Kalman滤波器的一次输入，步骤4-3的视觉定位得出的观测数据作为Kalman滤波器的另一输入，由预测误差的协方差矩阵Pk、观测矩阵H和观测误差的协方差矩阵Rk计算Kalman滤波器的增益K＝PkHT(HPKHT+Rk)-1。
由Kalman滤波公式得到融合定位后更加准确的车辆位置坐标
Xp′为融合定位后当前车辆测试位置的平面坐标，误差精度为厘米级，满足当今智能车定位精度的需求。其中视觉测量误差的引入，提高了视觉定位的结果，减少了直线方程求取过程对实验结果的影响。
将融合定位后的空间平面坐标转换为GPS坐标，即可得到更加精确的全局坐标系下的坐标。
图4为最近参照路标为直行箭头情况下的定位误差对比图，虚线为GPS定位后的误差，实线为融合后的定位误差，明显看出融合定位后误差有所减小。
实施例2
选取河北工业大学自强道、承学路、学熙路为试验场地，除了包含直行箭头以外还包含右转、左转、双转、右转加直行等多种其他种类箭头，分别以除直行箭头以外的其他种类箭头为最近参照路标进行融合定位，在不同地点对每种箭头各进行15组定位实验，其他实验步骤同实施例1。
经过一系列以箭头为参照的车辆定位实验，比较GPS定位和融合定位的误差对比结果，证明本发明方法能够提高车辆定位精度，定位精度达到厘米级。
本发明未述及之处适用于现有技术。