[发明专利]一种基于视觉、GPS与高精度地图融合的车辆定位方法

摘要

本发明为一种基于视觉、GPS与高精度地图融合的车辆定位方法，该方法首先将地图中惯导数据和车载GPS坐标转换为空间平面坐标，匹配得出距离当前车辆位置最近的路面箭头，确定视觉定位范围，然后使用Kalman滤波器将GPS定位与视觉定位结果融合，利用视觉定位数据修正GPS定位结果，得到车辆在全局坐标系下更精确的位置信息，完成定位。该方法克服了传统的基于点与点之间的距离的非线性约束，利用点到箭头各边之间的距离作为约束，构造线性Kalman滤波器，提高了系统的精度，地图的构建解决了某些复杂路段地图数据缺失和路标磨损的问题，融合定位克服了GPS定位精度低和视觉定位存在累积误差、无法全局定位的问题，从而达到厘米级定位精度。

主权项

1.一种基于视觉、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选出的参照箭头所在路面建立空间平面坐标系，根据地图中采集的箭头的几何结构信息写出箭头各角点的空间平面坐标，由两点式直线方程分别计算箭头各边的方程l_i：a_ix+b_iy+c_i＝0，然后根据当前车辆位置点P的空间平面坐标X_p计算其到路面箭头各边l_i的直线距离第四步，Kalman滤波器融合定位：4‑1、GPS定位数据作为Kalman滤波器的预测数据：针对单帧非连续图像，不存在时刻的关联，将车载GPS坐标转化后的空间平面坐标X_p作为Kalman滤波器的预测值设定预测矩阵为预测误差的协方差矩阵为其中Q为GPS定位精度；4‑2、视觉定位数据作为Kalman滤波器的观测数据：将当前车辆位置的空间平面坐标Xp及其到路面箭头各边li的直线距离di作为Kalman滤波器的观测值z，其中，H为观测矩阵，记为其中lix,liy分别为当前车辆位置空间平面坐标(xp,yp)到直线li的距离关于横轴与纵轴的关系式分量，多次测量车到各边距离的均值z_k与观测值z的平均误差为观测误差的协方差矩阵为R_k，其中为多次测量的各测量误差数据的方差；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滤波公式为：其中X_p'为融合定位后当前车辆位置的空间平面坐标；R_k观测误差的协方差矩阵，K为Kalman滤波器的增益，为视觉测量误差均值。