[发明专利]一种弯道的车辆位姿计算方法

摘要

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

主权项

一种弯道的车辆位姿计算方法，该方法的具体步骤如下：第一步，构建弯道地图数据库1‑1、采集弯道GPS信息序列：基于车载GPS采集设备获取车辆当前的GPS值，即纬度与经度，记为[n,e]；车辆在弯道的道路中心处且车身平行于弯道的切线，每隔0.5‑1.5m采集一次GPS信息，采集GPS的位置作为采样点，遍历所有采样点便可获得弯道GPS信息序列；1‑2、采集弯道路面图像：采集弯道路面图像由车载摄像机系统完成，车载摄像机系统由车载摄像机、控制设备和弯道路面图像存储器和传输器组成，车载摄像机安装在车辆的前盖的横向中心位置，车载摄像机俯角拍摄弯道路面，车载摄像机的光轴到路面的投影与车的纵向平行；利用张正友标定法对车载摄像机进行标定，得到车载摄像机的内参数矩阵K和外参数、弯道路面坐标系与弯道路面图像坐标系间的单应性矩阵H1及弯道路面图像的中心点(u0，v0)；外参数包括旋转矩阵R和平移向量T，弯道路面坐标系与车载摄像机采集的弯道路面图像坐标系的关系满足式(1)：[u v 1]T＝H1[Xw Yw 1]T    (1)其中(u v)为车载摄像机采集的弯道路面图像的像素坐标，(Xw Yw)为路面坐标；在上述步骤1‑1进行时由车载摄像机系统在采样点采集弯道路面的图像，得一系列弯道路面图像，每张弯道路面图像对应唯一的GPS值；将弯道路面图像的中心点带入式(1)得到车载摄像机的光轴与路面的交点记为A,A＝(X0 Y0)，车载摄像机投影到弯道路面的点与A点的连线即为光轴投影到弯道路面的线段，记为l；1‑3、采样点处车辆的位姿信息序列：每个采样点的坐标根据路测激光确定；在步骤1‑1和步骤1‑2进行时路测激光实时采集激光数据，根据激光数据确定车辆的位姿信息，即车辆所处的位置和车辆偏转角度，第i个采样点处车辆的位置坐标为Mi(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个采样点按弯道位置排序分别记为Mj+1，Mj+2，Mj+3，Mj+4，Mj+5……Mj+n，即可实现GPS初定位；从弯道地图数据库调取n个采样点处采集的图像组成图像训练集，当弯道不足n个采样点时GPS初定位结果为弯道地图数据库中全部的采样点；第三步，图像级定位：首先分别将上述步骤2‑1和步骤2‑2得到的被测试图像和图像训练集分别进行预处理，所述预处理包括归一化处理、均衡化和灰度化处理，将预处理后图像的中心作为ORB特征点，把该整个图像作为此特征点的邻域区域，再计算得到ORB全局特征描述符，以该ORB全局特征描述符作为图像的ORB全局特征；使用WH‑KNN算法按照式(2)分别计算被测试图像与图像训练集中每张图像的汉明距离，DR(H1,H2)=Σi=1256xor(Hi1,Hi2)---(2)]]>其中，H1为被测试图像的ORB全局特征描述符字符串，H2为图像训练集中一张弯道路面图像的ORB全局特征描述符字符串，和分别为ORB全局特征描述符256位字符串的第i位、DR(H1,H2)为两张图像间的汉明距离、xor为位异或运算；比较所有汉明距离的大小，找出汉明距离最小的一张图像作为与被测试图像最相近的图像训练集中的一张图像，称为图像级定位的最匹配图像，此最匹配图像对应的位置坐标即为图像级定位坐标(xv,yv)，由此完成图像级定位；第四步，车辆度量级的位姿计算：将第三步得到的图像级定位的最匹配图像与当前预处理后的被测试图像进行ORB局部特征匹配，再利用RANSAC算法去除误匹配，分别得到最匹配图像与当前预处理后的被测试图像对应的两组特征点序列，且两组特征点序列之间的关系满足式(3)，[u v 1]T＝H0[u1 v1 1]T    (3)其中H0为预处理后的最匹配图像与当前被测试图像之间对应的单应性矩阵，(u,v)是最匹配图像的弯道路面图像像素坐标，最匹配的弯道路面图像是弯道地图数据库中的一张图像，(u1,v1)被测试图像的弯道路面图像像素坐标；再联立式(1)和式(3)得到式(4)，[u1 v1 1]T＝H0‑1H1[Xw Yw 1]T     (4)其中，H0‑1H1为弯道路面坐标系与被测试图像坐标系对应的单应性矩阵，H0‑1H1记为H；再根据张正友标定法中单应性矩阵H与旋转矩阵R、平移向量T之间的关系，计算出拍摄被测试图像时车载摄像机的外参数，即旋转矩阵R和平移向量T；然后根据‑R‑1T计算车载摄像机所在世界坐标系的位置，判断拍摄当前被测试图像和最匹配图像的车载摄像机位置的差，记为车辆的偏离距离，将第三步得到的图像级定位坐标(xv,yv)加上车载摄像机位置的差，即为拍摄被测试图像时车辆的当前位置坐标，由此完成在弯道行驶时车辆位置坐标的计算；拍摄被测试图像时车载摄相机投影到弯道路面的点与A点的连线即光轴投影到弯道路面上的线段，记为l1；再在弯道地图数据库中找出最匹配图像的光轴投影到弯道路面的线段，两线段的夹角即为车辆偏转角度；至此完成弯道车辆度量级的位姿计算。