一种基于GNSS和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法与流程

本发明涉及无人驾驶车载组合导航技术领域，具体涉及一种基于gnss、视觉里程计、和车道线检测的融合导航方法。

背景技术：

随着汽车的普及和使用频率的上升，道路拥堵、环境污染与交通事故已经成为人们不得不面对的问题。发展无人驾驶车辆及研究车辆的自主行驶系统是目前实现安全、高效交通的最佳选择。定位作为无人驾驶车辆的关键技术，是目前车载导航系统亟待解决的问题。早期的车辆定位系统中常使用的是全球卫星导航系统gnss(gps，glonass，galileo,beidou)。gnss可以提供全球性的、高精度的定位服务，但是受到卫星轨道误差、时钟误差以及信号传播误差等的影响，gnss的定位精度只能达到米级。虽然通过载波相位差分技术可以将定位精度提高到厘米级，但是在建筑物密集的城市区域，由于卫星信号受到阻挡以及多路径效应的其它因素干扰，gnss往往无法满足无人驾驶汽车的定位需求。

为了弥补gnss的缺陷，通常采用组合导航的方式来提高车载导航系统的精度与鲁棒性。例如自主导航系统，基于车辆的相对运动模型，可以由车辆的上一个位置信息计算出车辆当前的位置。早期车辆自主定位系统常使用轮速编码器来进行航迹推算。然而，轮速编码器存在原理性的累积误差，并且在某些特殊环境下(如土质疏松、轮胎打滑等)会出现失误，不能确保得到精确的车辆位置与姿态估计。另一种常用的基于航位推算的自主定位方法是惯性导航系统(inertialnavigationsystem，ins)，通过测量载体相对于惯性空间的角速度和加速度，在对测量的值进行积分来推算而获得载体的导航参数并实时输出。但是，在gnss信号受干扰区域gnss/ins组合系统的误差会随着时间而逐渐积累^[4],无法完成无人驾驶汽车的精准定位。且高精度的惯性传感器价格昂贵，不利于无人驾驶汽车的产业化发展。

技术实现要素：

针对以上现有技术问题，本发明的目的是针对目前组合导航系统价格昂贵、精度低、可靠性差的问题，提出一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法。具体技术方案如下：

一种车道线辅助视觉里程计和gnss融合的组合导航方法，所述方法的步骤为：

(1)基于单目视觉的车道线检测；

(2)单目视觉里程计定位精度优化；

(3)gnss与视觉里程计组合导航系统的定位精度矫正；

进一步的，步骤(1)中基于单目视觉的车道线检测的方法为：读取一帧图像，对图像逐行处理，对图像中各行，采用对水平方向敏感的sobel算法增强边缘，然后对行信息进行处理，进行二值化，对二值化后数据采用车道线内边缘提取算法，提取车道线内边缘点，采用hough变换进行拟合车道线，得到车道线极坐标参数，提取车辆在在道路坐标系下的航向角ψd和横向位置yd；

进一步的，视觉里程计是由多个模块组合得到的系统，包括图像采集模块、特征选择和关联模块、位姿估计模块以及可选的局部优化模块；

进一步的，特征选择与关联、帧间位姿估计这两个模块子在获取每一帧图像时都需要进行计算，是构成视觉里程计的必要模块；

进一步的，基于特征的视觉里程计的算法流程为：

(1)从车载相机采集的视频中读取一系列的图像序列ik，对某一时刻的单帧图像进行特征点检测，寻找那些比较突出的像素，即根据特定响应度计分函数计算得到的特征度要明显高于其邻域像素，用不同的高斯差来对图像进行滤波并检测局部极值，以获得图像对尺度的不变性，在每个不同的尺度都用高斯差滤波器来对图像做卷积：

通过用不同的高斯核g(σ)对图像做卷积运算；

(2)计算卷积图像的差选取局部极值作为特征候选点，只是该处的局部极值不仅要与当前尺度图像in中的8个邻近点比较，还要与“上层”图像in-1和“下层”图像in+1中的18个临近点进行比较；

(3)用抛物线拟合来对特征候选点位置进行亚像素细化，再用对图像强度的阈值化方法来剔除对比度过小的候选点；公式如下：

检测到特征点p后，要为其计算一个具有尺度是旋转不变性的环装多维描述符，首先为p分配一个方向αp使描述符具备对图像旋转的不变性：为围绕p的每一个像素都要计算梯度值g和方向α，并用g和围绕p的高斯窗口计算的加权直方图分布，αp的选择就是该直方图的峰值位置，随后，用欧式距离对描述符进行相似度度量，在用相似度度量比较了两帧图像之间的特征描述符之后，第一帧图像在第二帧图像中的最好关联将被选为最接近的描述符；

(4)位姿估计是以特征检测与关联的输出为输入的模块，是由内层的外数据移除和基于全部内数据的外层细化计算组成的二步过程，用随机采样一致性方法从点关联集合中随机采样最小子集来计算模型假设，然后在点关联集合的其他数据上验证假设，与其他数据表现出最高一致性的假设将被选为优胜解；随后，用通用求解器计算相对位姿，采用高效5点算法，获得的是最小二乘解，扩展到用n对点关联对进行计算，其中n>5，随后通过图像匹配求解车辆运动的6自由度运动参数；

(5)利用车辆在在道路坐标系下的航向角ψd和横向位置yd修正基于视觉里程计方法求解的6自由度参数，提高车辆在车道上横向位置上的定位精度，抑制视觉里程计的漂移误差；

进一步的，在完成单目视觉里程计相对定位的基础上，利用卡尔曼滤波器将无人驾驶汽车的视觉定位坐标与gnss定位坐标进行融合矫正，在融合导航系统gnss的信号可以抑制vo的漂移，当gnss信号受遮挡而无法正常工作时，vo可以提供连续的导航结果；

进一步的，首先在无人驾驶汽车车载平台上建立动力学模型，该模型用于预测车辆新的运动状态，根据gnss与视觉定位结果建立gnss/vo系统的误差模型；其次根据上一时刻的坐标向量与下一时刻的坐标向量的转换关系建立系统状态方程；最后根据状态向量和观测向量之间的关系建立观测方程，而gnss与视觉系统将作为观测向量中的成员同时对整个系统进行数据融合，卡尔曼滤波器接收gnss与vo两个导航子系统对车辆位置参数输出值的差值，经过滤波计算，估计出各误差量的最优估计值，用vo导航系统误差的估计值去校正vo系统特征匹配中的相应六自由度导航参数，即将误差的最优估计值反馈到vo导航系统的动力学模型中进行导航参数修正，最后输出经反馈校正后的定位坐标。

本发明与现有技术相比，具有如下优点和效果：

利用车道线辅助视觉里程计，然后与gnss定位进行融合以提高车辆定位系统的可靠性。视觉里程计(vo)是利用车载相机采集到的图像信息来恢复车体本身的6自由度信息，包括3自由度的旋转和3自由度的平移。视觉传感器可以提供丰富的感知信息，既可以满足车辆的自定位需求，同时可以为其它功能提供信息，如车道线检测、避障等，路标识别等。而且，视觉传感器还具有成本低、体积小等优点。gnss/vo具有很强的互补特性，gnss可以获得长时间稳定的定位结果，而vo可以获取短期高精度定位数据，利用两种传感器对位置测量值的差值进行滤波计算，使用vo系统误差的估计值去校正vo系统的误差，实现利用gnss数据限制vo数据长时间漂移的目的。

附图说明

图1为车道线检测步骤

图2为基于特征的视觉里程计的算法流程

图3为ransac算法流程示意图

图4为gnss和视觉组合导航的原理框图

具体实施方式

下面根据附图对本发明进行详细描述，其为本发明多种实施方式中的一种优选实施例。

(1)基于单目视觉的车道线检测

读取一帧图像，对图像逐行处理。对图像中各行，首先采用对水平方向敏感的sobel算法增强边缘。然后对行信息进行处理，进行二值化，对二值化后数据采用车道线内边缘提取算法，提取车道线内边缘点，采用hough变换进行拟合车道线，得到车道线极坐标参数(极坐标半径和角度)。提取车辆在在道路坐标系下的航向角ψd和横向位置yd。车道线检测流程如图1所示。

(2)单目视觉里程计定位精度优化

视觉里程计是由多个模块组合得到的系统，包括图像采集模块、特征选择和关联模块、位姿估计模块以及可选的局部优化模块。基于两帧框架的视觉里程计系统，其工作流程一般包括特征的选择与关联，以及基于所获得的特征关联集合的帧间位姿估计。其中，特征选择与关联、帧间位姿估计这两个模块子在获取每一帧图像时都学要进行计算，是构成视觉里程计的必要模块；在视觉里程计定位的基础上，利用车道线参数对视觉定位结果进行局部优化。图2展示了基于特征的视觉里程计的算法流程。

首先，从车载相机采集的视频中读取一系列的图像序列ik。对某一时刻的单帧图像进行特征点检测，其基本的思想是寻找那些比较突出的像素，即根据特定响应度计分函数计算得到的特征度要明显高于其邻域像素。用不同的高斯差(differenceofgaussians,dog)来对图像进行滤波并检测局部极值，以获得图像对尺度的不变性，也即构建图像金字塔，在每个不同的尺度都用高斯差滤波器来对图像做卷积：

通过用不同的高斯核g(σ)对图像做卷积运算，然后计算卷积图像的差选取局部极值作为特征候选点，只是该处的局部极值不仅要与当前尺度图像in中的8个邻近点比较，还要与“上层”图像in-1和“下层”图像in+1中的18个临近点进行比较。接下来，还可以用抛物线拟合来对特征候选点位置进行亚像素细化，再用对图像强度的阈值化方法来剔除对比度过小的候选点。如题公式如下：

检测到特征点p后，要为其计算一个具有尺度是旋转不变性的环装多维描述符。首先为p分配一个方向αp使描述符具备对图像旋转的不变性：为围绕p的每一个像素都要计算梯度值g和方向α，并用g和围绕p的高斯窗口计算的加权直方图分布。αp的选择就是该直方图的峰值位置。随后，用欧式距离对描述符进行相似度度量。在用相似度度量比较了两帧图像之间的特征描述符之后，第一帧图像在第二帧图像中的最好关联将被选为最接近的描述符。

位姿估计是以特征检测与关联的输出为输入的模块，是由内层的外数据移除和基于全部内数据的外层细化计算组成的二步过程。用随机采样一致性方法从点关联集合中随机采样最小子集来计算模型假设，然后在点关联集合的其他数据上验证假设，与其他数据表现出最高一致性的假设将被选为优胜解。其算法流程如图3所示。

随后，用通用求解器计算相对位姿，相对定姿问题的代表性最小集求解器是nister的高校五点算法。高效5点算法本质意义上获得的是最小二乘解，因此很容易扩展到用n(n>5)对点关联对进行计算。随后通过图像匹配求解车辆运动的6自由度运动参数。最后，利用车辆在在道路坐标系下的航向角ψd和横向位置yd修正基于视觉里程计方法求解的6自由度参数，提高车辆在车道上横向位置上的定位精度，抑制视觉里程计的漂移误差。

(3)gnss/vo组合导航系统的定位精度矫正方法

在完成单目视觉里程计相对定位的基础上，利用卡尔曼滤波器(kalmanfilter)将无人驾驶汽车的视觉定位坐标与gnss定位坐标进行融合矫正。在融合导航系统gnss的信号可以抑制vo的漂移，当gnss信号受遮挡而无法正常工作时，vo可以提供连续的导航结果。

gnss与视觉定位系统具有良好的互补性，gnss可以提供比较稳定的长时间范围内的定位结果，而视觉里程计在短时间内可以提供高精度的定位输出。利用最优估计法将无人驾驶汽车的视觉定位坐标与gnss定位坐标进行融合矫正，多传感器信息融合系统的最优估计常采用的技术是卡尔曼滤波。由于视觉定位与gnss定位的输出都是非线性系统，所以采用间接法滤波。首先在无人驾驶汽车车载平台上建立动力学模型，该模型用于预测车辆新的运动状态。根据gnss与视觉定位结果建立gnss/vo系统的误差模型；其次根据上一时刻的坐标向量与下一时刻的坐标向量的转换关系建立系统状态方程；最后根据状态向量和观测向量之间的关系建立观测方程。而gnss与视觉系统将作为观测向量中的成员同时对整个系统进行数据融合。卡尔曼滤波器接收gnss与vo两个导航子系统对车辆位置参数输出值的差值，经过滤波计算，估计出各误差量的最优估计值。用vo导航系统误差的估计值去校正vo系统特征匹配中的相应六自由度导航参数，即将误差的最优估计值反馈到vo导航系统的动力学模型中进行导航参数修正，最后输出经反馈校正后的定位坐标。间接法滤波原理如图4所示。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种改进，或未经改进直接应用于其它场合的，均在本发明的保护范围之内。