基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法

摘要

本发明公开了一种基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法，该方法利用SURF匹配算法和FLANN快速搜索算法对定位的处理时间行了改善，有效地降低了双目视觉SLAM算法的处理时延带来的影响，鲁棒性强；通过利用霍夫变换来识别道路两侧的现状对象，从而判断车辆是否发生左右偏移并进行实时纠偏，算法简单易行，实时性高。本发明依靠自动驾驶车辆和双目视觉传感器，不需要对车辆进行大幅度改装，降低了车辆的复杂性与成本，且在定位和纠偏过程，从双目摄像头采集图像，到目标车辆执行控制指令，全程无需人工参与，可以实现真正意义上的定位和纠偏自动化。

说明书

技术领域

本发明属于图像处理技术领域，具体涉及一种基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法。

背景技术

随着自动驾驶技术的普及，高精度、低成本、高效率的技术正成为研究的热潮，目前在自动驾驶车辆自定位方面有多种方法，其中主要分为两个方向：内传感器定位和外传感器定位。

内传感器通过感知自身的运动变化计量累计位姿变化，主要应用于准确的已知出发点和固定不变的速度和方向的自动驾驶系统中，其中主要有两种：里程计和惯性传感器(陀螺仪、加速度计)；轮子码盘(又叫轮式里程计)通过事先知道车轮的直径计数车轮转速得出速度和位移；以差分两轮为例子，根据两个轮子的转圈数差可以推算出运动主体角度的变化。惯性传感器可以测量出线性加速度和角速度，通过积分可以推算出累计位移和角度变化。内传感器的定位都依赖于一个起始位姿通过不停积分，在起始位姿基础上合并相对位姿，进而推算出新时刻的位姿。内传感器定位方法存在以下不足：

(1)不依赖于外部环境，不对外部环境做先验假设，因而无法感知车辆方向的变化，若果车辆和指定的行驶方向发生偏差无法纠正。

(2)它推算的是累计位姿变化，所以绝对定位需要一个准确的起始位姿，起始不对，后面定位都不准。

(3)该方法既然是累计变化，如果每一步累计引入误差，不管误差多小，在后续长时间的积累下，都会是一个很大的误差，累计误差到最后将会导致无法估量的错误。

而在外传感器定位方面上，原理主要是通过外传感器通过感知周遭环境，来辅助定位自己的位姿，其中分类主要有GPS接收、2D单目摄像头、双目摄像头、深度摄像头、激光雷达等，其事先在车载电子系统中安装有GPS或者激光雷达、摄像头等装置，并通过车载装置接收传感器信息来获取自身位置信息。上述外传感器单独使用存在以下不足：

(1)GPS获取位置信息的频率不会很快，大概10Hz的样子，对于运动速度很慢的运动主体，还凑合能用，对于自动驾驶这个高速行驶的运动主体，需要更高的频率获取位姿；GPS信号很容易被遮挡，室内定位基本用不上GPS，汽车过隧道的时候，会有相当长的时间无GPS信号；GPS浮动误差较大，精度智能保证在几十米以内的范围，对室外定位精度要求比较高的场景，GPS无法单独解决定位问题。

(2)激光雷达在自动驾驶领域，高精地图的采集及定位应用，一般使用的是多线激光雷达方案，缺点是价格过于昂贵，无法再普通自动驾驶车辆上部署并且处理算法时间过长，不满足高精度、低成本、高效率的需求。

以上描述了用于定位的主流传感器，可以看到单一传感器在解决定位问题上都有自己的优缺点；在实际应用中，需要结合多个传感器联合解决定位问题，以下针对几个典型场景，描述一下多传感器融合的情况：

(1)GPS+IMU+里程计。

GPS给出的全局锚定，可以消除累计误差问题，不过它的更新频率低，并且信号容易被遮挡；IMU(惯性测量单元)和轮盘里程计更新频率高，不过有累计误差问题，最容易想到的是收到GPS定位，使用GPS位置信息，误差就是GPS的精度，在下一次收到GPS定位间隔中，使用IMU(角度累加)和里程计(位移累加)进行位姿累加，中间的位姿误差是初值GPS定位误差和中间累加误差的积累。

缺点：误差等于GPS的精度，不满足高精度要求，累计误差无法通过GPS消除，没有实时的偏移纠正。

(2)GPS+多线雷达+高精地图匹配。

GPS给出全局锚定，中间使用雷达SLAM前端里程计做累加，可以配合高精地图的图像匹配，做类似后端回环优化的方式，将GPS、激光雷达及已知地图进行融合定位。

缺点：价格昂贵，处理时间不够快，没有实时的偏移纠正。

发明内容

鉴于上述，本发明提供了一种基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法，通过里程计和IMU的高频惯性测量单元和低频率的SURF特征提取算法进行传感器数据融合，以实现沿轨车道上己方车辆的高精度定位和实时纠偏。

一种基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法，包括如下步骤：

(1)在指定道路上以固定距离间隔设置若干参考点并使车辆在该道路上低速行驶，利用双目视觉传感器、里程计以及GPS采集车辆在每一参考点处对应的道路两侧环境图像、行驶里程以及经纬度，从而构建图像-经纬度数据集以及里程-经纬度数据集；

(2)实时获取车辆的行驶里程，根据车辆的当前行驶里程distance从图像-经纬度数据集中提取相近的环境图像组成图像集Ω；

(3)利用双目视觉传感器获取当前道路两侧环境图像，由于两侧双目共得到4张图像P₁～P₄；对于其中任一张图像P_i，通过特征提取使其与图像集Ω中每一张图像进行特征匹配，i为自然数且1≤i≤4；

(4)从图像集Ω中提取与图像P_i特征匹配结果最优的两张图像Q₁和Q₂，并从图像-经纬度数据集中查询得到图像Q₁和Q₂对应参考点的经纬度Z₁和Z₂，进而对经纬度Z₁和Z₂加权求和得到图像P_i的定位结果；遍历得到4张图像P₁～P₄的定位结果并结合双目视觉传感器相对车辆几何中心的位置关系，从而求得当前车辆几何中心的地理坐标X；

(5)对图像P₁～P₄进行线段分析并通过偏移识别求得当前车辆的偏移角Δα和偏移量Δβ，进而根据Δα和Δβ计算判别出当前车辆的偏移状态、偏移距离ΔL以及实际方向角α_pm。

进一步地，所述步骤(1)的具体实现过程为：将两个双目视觉传感器分别安装于车身两侧，在每个参考点处采集道路两侧环境图像并对图像进行直方图均衡化处理以调整图像饱和度和亮度，进而将每个参考点处对应处理后的环境图像加上经纬度组成样本存入图像-经纬度数据集中，同时将每个参考点处对应的行驶里程加上经纬度组成样本存入里程-经纬度数据集中，两个数据集的样本个数相同即对应为参考点个数。

进一步地，所述步骤(2)的具体实现过程为：首先从里程-经纬度数据集中提取出最接近distance的n个参考点对应的样本，并对这些样本的经纬度求平均得到车辆的当前经纬度Z；同时从图像-经纬度数据集中提取出这n个参考点对应的样本，并将这些样本的环境图像组成图像集Ω，n为大于1的自然数。

进一步地，所述步骤(3)的具体实现过程为：对于图像P_i，截取其中间1/3宽度的图像区域，通过SURF特征提取算法求得该图像区域的特征矢量，并根据该特征矢量通过基于Kd-Tree的FLANN(Fast>

进一步地，所述步骤(4)中与图像P_i特征匹配结果最优的两张图像Q₁和Q₂即图像集Ω中与图像P_i特征点匹配个数最多的两张图像；对经纬度Z₁和Z₂加权求和时，经纬度Z₁和Z₂的权重由对应图像Q₁和Q₂与图像P_i的特征点匹配个数决定，特征点匹配个数越多则相应的权重越大。

进一步地，所述步骤(5)中求取当前车辆偏移角Δα和偏移量Δβ的具体过程如下：

5.1基于霍夫变换对图像P₁～P₄进行线段检测，得到每一线段在图像中的角度α以及线段中点位置，检测得到的线段需满足以下条件：|tanα|＜tan5°且线段长度大于50个像素；

5.2以线段归一化后的长度作为权重，对图像P₁～P₄中左侧两张图像中所有线段的角度α加权求和得到Δα_L，对图像P₁～P₄中右侧两张图像中所有线段角度α加权求和得到Δα_R，进而对Δα_L和Δα_R求平均即得到偏移角Δα；

5.3根据线段中点位置求得每一线段中点在图像中相对道路中心线的横向距离β，以线段归一化后的长度作为权重，对图像P₁～P₄中左侧两张图像中所有线段的横向距离β加权求和得到Δβ_L，对图像P₁～P₄中右侧两张图像中所有线段的横向距离β加权求和得到Δβ_R，进而求得偏移量Δβ＝Δβ_L+Δβ_R。

进一步地，所述步骤(5)中若偏移角Δα为正，则表明当前车辆为右偏；若偏移角Δα为负，则表明当前车辆为左偏；所述偏移距离ΔL则通过以下算式求得：

其中：H为道路宽度，h为车辆宽度。

进一步地，所述步骤(5)中根据车辆最近两个轨迹点连线的方向角α_m，使其与偏移角Δα相加即得到当前车辆的实际方向角α_pm。

与现有技术相比，本发明具有以下有益技术效果：

(1)本发明利用内传感器和外传感器数据融合，结合内、外传感器的优点，弥补了单一传感器的不足，提高了准确率和精度。

(2)本发明利用SURF匹配算法和基于KD-TREE的FLANN快速搜索算法对实时道路两侧图像进行匹配搜索，在图像定位点有效地降低了因单一里程计累计误差带来的定位影响，将精度提高到厘米级别，并结合左右偏移识别部分得到制定车道上车辆的精准定位，在车道上的前后左右距离都可以准确计算得到，鲁棒性强。

(3)本发明通过利用偏移识别和沿轨定位，从而判断待检测车辆是否和指定自动驾驶轨迹发生偏差，并计算左右偏差的距离和角度Δα，算法简单易行，实时性高。

附图说明

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

具体实施方式

为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。

如图1所示，本发明基于图像匹配的自动驾驶车辆定位和纠偏方法，包括如下步骤：

步骤1：利用两个双目视觉传感器采集当前指定道路两侧环境的图像，采集固定距离设置参考点并在每个参考点用视觉传感器获取图像，并对图像进行直方图均衡化处理，调整图像饱和度和亮度，然后加上经纬度标签作为环境图像-经纬度数据集以便于后续识别；利用里程计或者速度传感器加上GPS定位数据获得在指定道路上的里程-经纬度数据集。

本发明采集道路两侧图像，根据图像采集设备在车上的安装位置，截取实景图像中的马路区域的中间部分作为目标区域，如将图像的中间1/3部分作为目标区域等，然后对目标区域进行直方图均衡化处理，调整图像饱和度和亮度。直方图均衡化是一种利用图像直方图对对比度进行调整的方法。其基本思想是把原始图像的直方图变换为均匀分布的形式，对图像进行非线性拉伸，重新分配图像像素值，使一定灰度范围内的像素数量大致相同，这样就增加了图像灰度值的动态范围，从而可以用于增强局部的对比度而不影响整体的对比度。截取图像的中间1/3部分作为匹配，加速了图像匹配的过程，较少了处理时间，而且规避了连续两幅图像因摄像头广角限制带来的连续的两幅图像上边缘空间重叠和扭而导致匹配失准的现象。

本实施方式以固定间距的参考点作为采集环境图像的参考点，两个双目视觉传感器分别装在车辆两侧使其与路沿的举例在1.5米到2米之间，并且分别朝向左右侧的路沿，使路沿在视野高度的中间范围内，包括左边的传感器L1和L2，面朝道路左边缘，以及右边的传感器R1和R2，面朝道路右边缘；L1与L2相距10cm，R1与R2相距10cm；图像大小640×480；沿道路每隔0.1米左右设置一个惯导定位点，沿道路每隔5～10米设置一个图像定位点，在每个图像定位点用视觉传感器获取图像，加入环境数据集，参考点坐标与环境数据集中的图像一一对应。为了加大环境图像之间的区别性可以沿路沿装饰无规则的纹理或者图案；准备里程计/速度传感器：用里程计记录路程或者通过速度传感器记录的速度来累积路程，在构建图像到经纬度位置的数据集的同时，构建路程到经纬度位置的里程数据集。

构建数据集具体步骤：沿指定道路上，以1m/s移动速度移动，同时：

——以10Hz频率，记录GPS轨迹数据；

——以100Hz频率，记录累积路程数据；

——每隔10s，以2Hz的频率，记录两侧图像数据，持续3s。

步骤2：在指定道路上实时使用里程计获得实时累计路程数据distance，再根据数据集中最接近distance的两行数据以及distance和经纬度的关系，加权计算当前经纬度Z，从环境数据集里面获取Z范围内的图像集合Ω＝{Q₁，Q₂，...，Q_4n}。

通过里程计获得准确的累计路程数据，在通过预先准备的路程-经纬度数据集获取低精度的GPS位置信息，属于一个精度为十米左右的范围，然后根据这个范围得到一系列道路两侧环境图像。因为采样时是以10Hz频率，记录GPS轨迹数据；以100Hz频率记录累积路程数据，每隔10s以2Hz的频率记录两侧图像数据，持续3s，因而里程计对应的GPS位置信息是一定存在的。

本实施方式中在指定道路上实时使用里程计得到车辆的累计路程distance，再根据查询里程-经纬度数据集得到对应的经纬度位置Z；若查询到的这两行数据的其中一行的T_{1_id}值(记为t_{1_id})在图像-经纬度数据库table2的的T_{1_id}列中存在，则开启一次图像定位，计算当前经纬度；否则，只做惯导定位，不处理图像信息。

步骤3：使用两个双目视觉传感器获取当前车侧视野图像P＝{P₁，P₂，P₃，P₄}，截取其宽度的中间1/3得到P’＝{P₁’，P₂’，P₃’，P₄’}；基于SURF依次获得P_m’(1≤m≤4)与Q_i(1≤i≤4n)的特征矢量，并用KD-TREE快速搜索方法依次匹配P_m’与Q_i的特征矢量。

本实施方式基于SURF特征和KD-TREE快速搜素算法得到最近的图像匹配，需要获取从左右两侧的双目摄像头获取实时的车侧图像，主要步骤如下：

3.1在数据库的table2中，把T_{1_id}列的值等于t_{1_id}的几行数据从table2中取出，每一行数据作为集合row＝{row₁,row₂,…,row_4n}的一个元素。

3.2根据这几行数据的img_addr列，获得附近的左右侧环境图像集合QL＝{QL₁,QL₂,...,QL_2n}，QR＝{QR₁,QR₂,...,QR_2n}。

3.3通过左右摄像头获取当前车侧图像PL＝{PL₁,PL₂}，PR＝{PR₁,PR₂}。

3.4基于SURF特征与KD-TREE快速搜索，匹配PL与QL的特征矢量；PR与QR同理。

3.5为PL与QL、PR与QR的特征匹配定义一个评价函数如下，评价匹配结果的最优程度；函数值越小，则越优。

式中：n是匹配点个数，和分别为两张图中相匹配的像素点，与分别为两张图中正中间点的坐标(这样做是为了坐标归一化)，C是常数。

步骤4：根据与P_m’匹配结果最优的两张环境图像Q_mj、Q_mk的编号，查找该编号对应的参考点坐标D_mj、D_mk，加权计算获得Pm’的定位结果；根据P_m图像获得者(传感器的某个眼睛)相对车辆几何中心的坐标，以及计算得到的各个传感器对应的定位结果，计算车辆几何中心的地理坐标。

本实施方式计算车辆几何中心的地理坐标，具体过程如下：

4.1假设与PL匹配结果最优的两张环境图像是QR_j、QR_k，对应row集合中的row_j和row_k，通过row_j和row_k的经纬度信息，以评价函数值的倒数作为权重，加权计算获得PL的定位结果；PR同理。

4.2根据PL、PR的定位结果以及PL₁、PL₂、PR₁、PR₂相对车辆几何中心的坐标，计算车辆几何中心的地理坐标。

步骤5：使用“偏移识别”中的左右偏移距离信息，修正轨迹法向的定位位置，得到最终定位结果；把此处的前后轨迹点的连线的方向角α_m和“偏移识别”中的左右偏移角度Δα相加，得到此时的定位点上的实际方向角α_pm＝α_m+Δα(m＝b，r)。

本实施方式中使用“偏移识别”中的左右偏移距离信息，修正轨迹法向的定位位置，得到最终定位结果，其中：

输入：当前图像；

输出：左右偏移角度值，左右偏移的距离值；

精度：①角度值，假设特定线状对象长度下限是L，则角度识别的最小单位是②距离值，在高宽为a×b的分辨率下，若道路宽W，车宽w，则距离值的最小单位是

具体过程如下：

5.1左右的摄像头以10Hz频率记录实时图像ML₁、ML₂、MR₁、MR₂。

5.2基于霍夫变换(Hough Transform)识别ML₁、ML₂、MR₁、MR₂中的特定线状“对象”，获得这些对象的角度、在图中的位置(即线段中点位置，进而得到其相对道路中心线的横向距离)，这些线状对象的识别条件是斜率绝对值小于arctan(5°)、且长度大于50个像素。

5.3以四张图中“对象”们的长度作为权重，计算“对象”们加权后的角度值，作为左右偏移角度Δα；负数为左偏，正数为右偏；相关的具体计算公式为：同理可求得偏移距离Δβ＝Δβ_L+Δβ_R。

5.4以四张图中“对象”们的长度作为权重，分别计算左边和右边图像中“对象”们加权后的位置值的y向分量L_y与R_y。

5.5根据实际空间与图像像素空间的相对关系，计算作为左右偏移的距离，Δα＜arctan(5°)，负数为左偏，正数为右偏；以(Δβ，Δγ)表示车辆在βγ坐标系下相对目标轨迹的位置偏移，这里我们以Δβ为沿轨法向方向上的偏移量，也就是车辆行驶方向的左右偏移。

上述对实施例的描述是为便于本技术领域的普通技术人员能理解和应用本发明。熟悉本领域技术的人员显然可以容易地对上述实施例做出各种修改，并把在此说明的一般原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于上述实施例，本领域技术人员根据本发明的揭示，对于本发明做出的改进和修改都应该在本发明的保护范围之内。