CN109946732B - 一种基于多传感器数据融合的无人车定位方法 - Google Patents

Abstract

本发明公开了一种基于多传感器数据融合的无人车定位方法，该方法充分利用厘米级高精度地图、视觉和激光雷达传感器数据，在卫星定位信号被遮挡区域实现高精度定位。本发明通过实时监测卫星定位数据有效性，不满足无人车定位要求时，利用厘米级高精度地图、激光雷达、里程计、视觉多传感器数据融合对卫星定位数据进行辅助，同时，对惯性导航累积误差进行修正，提高定位系统的可靠性、连续性和精度，且不需要增加传感器硬件成本。

Description

一种基于多传感器数据融合的无人车定位方法

技术领域

本发明涉及无人驾驶汽车技术中的定位与导航领域，尤其涉及一种基于多传感器数据融合的无人车定位方法。

背景技术

随着DARPA重启无人车技术研究，并先后于2004年、2005年和2007年举办了三届智能车挑战赛，谷歌大力投入自动驾驶技术研究，近年来全球范围内掀起了自动驾驶技术研究的热潮。参与者既有谷歌、苹果、Uber、BAT等顶尖互联网企业，也有特斯拉、奔驰、宝马、通用、凯迪拉克等传统车企，同时，英特尔、英伟达、博世、Mobileye等零部件供应商也参与其中，另外自动驾驶领域创业公司也如雨后春笋般应运而生。

自动驾驶技术经过十多年发展，逐渐从实验室研究、到封闭环境技术竞赛、再到固定场景示范应用，虽然取得了长足的进展，但距离实际应用仍有较大距离。车企普遍采用的由低级辅助驾驶到高级辅助驾驶，再到限定场景自动驾驶，最后到达全自动驾驶的路途并不十分顺利，例如特斯拉公司车辆配备的辅助驾驶系统，至今仍事故频出。谷歌、BAT互联网公司及创业公司采用的由低速、封闭场景逐渐向外延伸的技术路线发展也并不理想，目前仍然只能在封闭场景低速驾驶状态下执行简单确定性任务，并且可靠性较低。

无人车定位是限制自动驾驶技术发展的关键和瓶颈技术之一，依靠单一传感器难以达到理想的定位效果。高精度卫星导航接收机、惯性导航设备、激光雷达、视觉等传感器可以实现绝对或相对定位，但各有优缺点，多传感器融合技术是实现无人车精确、稳定、可靠定位的有效方法。

发明内容

针对目前无人车定位传感器存在的卫星导航接收机连续性差、惯性导航设备误差随时间累积、激光雷达和视觉传感器实时性差等诸多难题，本发明提出一种基于多传感器融合的无人车定位方法。

本发明所采用的技术方案如下：一种基于多传感器数据融合的无人车定位方法，包括如下步骤：

(1)通过卫星导航接收机输出的定位数据，实时监测数据有效性，当出现PDOP>5、可视星数目小于等于4两种情况中任意一种时，判别为卫星定位数据可用性下降；

(2)当卫星定位数据可用性下降时，无人车进行制动，制动过程中，通过里程计和惯导数据融合，对定位位置进行更新，车辆静止后对惯导设备的俯仰角误差、横滚角误差、航向角误差、加速度计和陀螺仪器件常值误差进行修正；

(3)利用激光雷达和视觉传感器分别获取道路边界和车道线在车体坐标系中的二次函数表示，计算车头方向相对于厘米级高精度地图中对应目标点航向的偏移角，并据此求取基于厘米级高精度地图的车辆航向角，对步骤(2)中的航向角进行更新；

(4)通过激光雷达点云、视觉、厘米级高精度地图对无人车航向、俯仰、横滚、定位信息更新后，无人车恢复运行，通过里程计和惯导数据融合进行实时位置更新，同时激光雷达、视觉厘米级高精度地图数据融合获取车辆航向，对惯导航向角参数进行修正。

进一步的，所述步骤(2)中车辆静止后对惯导设备的俯仰角误差、横滚角误差、航向角误差、加速度计和陀螺仪器件常值误差进行修正，具体过程如下：

车辆静止后真实的速度为0，惯导设备输出的速度即为误差，并以此建立误差模型，利用kalman滤波器对姿态失准角、水平速度误差、加速度计和陀螺仪器件常值进行估计，状态向量X如下所示：

式中，φ_E、φ_N、φ_U分别为俯仰、横滚和航向失准角，δV_E、δV_N分别为东向、北向速度误差，ε_x、ε_y、ε_z分别为三个陀螺仪常值误差，

分别为三个加速度计常值误差；

姿态失准角和速度误差对应的状态方程如下：

公式(2)-(6)中，ω_ie为地球自转角速度，L为纬度，V_E为东向速度，R_M为子午圈曲率半径、R_N为卯酉圈曲率半径，V_N为北向速度，h为高度，f_i(i＝E,N,U)为加速度计输出的比力在导航系下的投影；

T为姿态矩阵，具体表达式如下：

其中γ为横滚角，φ为航向角，θ为俯仰角。

进一步的，所述步骤(3)具体计算过程如下：

1)利用视觉传感器获取当前车道线的二次函数表示，并投影到车体坐标系O-XY平面，计算二次函数和X轴交点的切线斜率k₀，进而计算得到切线和Y轴夹角β₀；

2)利用激光雷达扫描周边环境，提取出边界特征点，然后投影到车体坐标系O-XY平面，剔除野值点，并利用最小二乘算法拟合出道路边界在二维平面中的二次函数表示，计算该二次函数和

的交点坐标A₀(x₀,y₀)，该点切线和Y轴夹角β₁；

3)根据夹角β₁和厘米级高精度地图对应坐标点航向

可计算基于定位和厘米级高精度地图的当前航向角，公式如下：

式中，

表示基于定位和厘米级高精度地图的车辆航向。

本发明的有益效果如下：定位传感器是无人车的重要感知设备，定位数据是无人车轨迹规划与决策功能模块的重要输入，但是，由于无人车运行的实际道路环境非常复杂，依靠惯导、卫星导航、激光雷达、视觉等单一传感器难以持续获得稳定可靠定位数据。本发明公开了一种基于多传感器融合的无人车定位方法，利用该方法可以有效解决隧道、“城市峡谷”、树冠遮挡道路环境下卫星导航定位数据不可用问题，为无人车提供持续、稳定、可靠的高精度定位数据。

附图说明

图1为基于传感器数据融合和车辆航向计算方法示意图；

图2为多传感器融合的自动驾驶车辆定位方法流程图；

图3为RTK异常状态下多传感器融合定位方法原理图。

具体实施方式

下面结合附图对本发明做进一步的说明。

一种基于多传感器数据融合的无人车定位方法，包括如下步骤：

(1)通过卫星导航接收机输出的定位数据，实时监测数据有效性，当出现PDOP>5、可视星数目小于等于4两种情况中任意一种时，判别为卫星定位数据可用性下降；

(2)当卫星定位数据可用性下降时，无人车进行制动，制动过程中，通过里程计和惯导数据融合，对定位位置进行更新，车辆静止后对惯导设备的俯仰角误差、横滚角误差、航向角误差、加速度计和陀螺仪器件常值误差进行修正，具体过程如下：

车辆静止后真实的速度为0，惯导设备输出的速度即为误差，并以此建立误差模型，利用kalman滤波器对姿态失准角、水平速度误差、加速度计和陀螺仪器件常值进行估计，状态向量如下所示：

式中，φ_E、φ_N、φ_U分别为俯仰、横滚和航向失准角，δV_E、δV_N分别为东向、北向速度误差，ε_x、ε_y、ε_z分别为三个陀螺仪常值误差，

分别为三个加速度计常值误差。

姿态失准角和速度误差对应的状态方程如下：

公式(2)-(6)中，ω_ie为地球自转角速度，R_M、R_N为地球半径参数，V_E为东向速度，V_N为北向速度，h为高度，L为纬度，f_i(i＝E,N,U)为加速度计输出的比力在导航系下的投影，T_ij为姿态矩阵中的元素，具体表达式如下：

(3)利用激光雷达和视觉传感器分别获取道路边界和车道线在车体坐标系中的二次函数表示，计算车头方向相对于厘米级高精度地图标注对应点航向的偏移角，并据此求取基于厘米级高精度地图的车辆航向角，对步骤(2)中的航向角进行更新，具体计算过程如下：

1)利用视觉传感器获取当前车道线的二次函数表示，并投影到车体坐标系O-XY平面，Y轴为车头方向，计算二次函数和X轴交点的切线斜率k₀，进而计算得到切线和车头方向夹角β₀(逆时针为负，顺时针为正，下同)。

2)利用16线激光雷达(VLP-16)扫描周边环境，提取出边界特征点，然后投影到车体坐标系O-XY平面，剔除野值点，并利用最小二乘算法拟合出道路边界在二维平面中的二次函数表示，计算该二次函数和

的交点坐标A₀(x₀,y₀)，进而计算该点切线和车头方向的夹角β₁；

3)根据夹角β₁和厘米级高精度地图对应坐标点航向

可计算基于定位和厘米级高精度地图的当前航向角，公式如下：

式中，

表示基于定位和厘米级高精度地图的车辆航向。

(4)通过激光雷达点云、视觉、厘米级高精度地图对无人车航向、俯仰、横滚、定位信息更新后，无人车恢复运行，通过里程计和惯导数据融合进行实时位置更新，同时激光雷达、视觉厘米级高精度地图数据融合获取车辆航向，对惯导航向进行修正。

根据定位信息、视觉识别车道线信息、激光雷达识别道路边界信息、厘米级高精度地图数据获取车辆当前航向计算方法见附图1。附图1中，OXY坐标系为车体坐标系在水平方向的投影，原点O位于车头顶端中央，高度和地面平行，Y轴为车头正前方，X轴垂直于Y轴，指向车体右侧。β₀为车道线函数和X轴交点切线和车头方向夹角，因此，垂直于此切线的直线y和道路边界函数的交点A₀即为厘米级高精度地图中和车辆位置对应的道路边界点。y_bj函数在A₀点的切线和Y轴的夹角β₁，即为车体航向角和道路边界A₀点厘米级高精度地图中航向夹角，据此可以求解基于定位和厘米级高精度地图的可靠地车辆航向角。

自动驾驶车辆多传感器融合定位整体逻辑流程见附图2，RTK异常状态下，基于厘米级高精度地图的激光雷达、视觉、惯导和里程计多传感器融合定位原理见附图3。

如附图2所示，通过卫星导航接收机发送的导航数据中的有效标志位、PDOP值判别RTK工作状态，当标志位无效或者PDOP值大于5时，判定RTK工作异常，车辆自主制动，同时，利用惯导/里程计组合，对定位信息进行更新，直至通过里程计判别车速为0km/h。车辆静止后，利用零速修正算法对惯导姿态误差、航向误差进行估计，并将修正后的定位和航向角数据用于辅助激光雷达和视觉，获取基于厘米级高精度地图的融合定位和航向信息，两种定位信息相差不大于30米、航向信息相差不大于5°，则将该信息更新至惯导，无人车启动，否则，重新进行融合定位和航向信息解算流程。

如附图3所示，视觉传感器对车道线进行识别，并在车体坐标系中用拟合的二次函数表示，利用车头对应点的车道线函数切线，获得车头方向相对于车道线的偏离角；利用惯导/里程计的位置信息，激光雷达点云信息和厘米级高精度地图进行局部信息匹配，解算获得基于厘米级高精度地图的融合定位信息，同时，获得道路边界点，并拟合出道路边界在车体坐标系的二次函数表示；利用车道线偏离角得到的车头方向偏转角，计算得到无人车当前位置对应的道路边界点，基于厘米级高精度地图数据得到无人车航向角，融合定位信息和惯导数据相差不大于30米、航向信息和惯导数据相差不大于5°，则认定本次融合定位数据有效。

Claims (1)

1.一种基于多传感器数据融合的无人车定位方法，其特征在于，包括如下步骤：

步骤(1)通过卫星导航接收机输出的定位数据，实时监测数据有效性，当出现PDOP>5、可视星数目小于等于4两种情况中任意一种时，判别为卫星定位数据可用性下降；

步骤(2)当卫星定位数据可用性下降时，无人车进行制动，制动过程中，通过里程计和惯导数据融合，对定位位置进行更新，车辆静止后对惯导设备的俯仰角误差、横滚角误差、航向角误差、加速度计和陀螺仪器件常值误差进行修正；

步骤(3)利用激光雷达和视觉传感器分别获取道路边界和车道线在车体坐标系中的二次函数表示，计算车头方向相对于厘米级高精度地图中对应目标点航向的偏移角，并据此求取基于厘米级高精度地图的车辆航向角，对步骤(2)中的航向角进行更新；

步骤(4)通过激光雷达、视觉传感器、厘米级高精度地图对无人车航向、俯仰、横滚、定位信息更新后，无人车恢复运行，通过里程计和惯导数据融合进行实时位置更新，同时激光雷达、视觉传感器、厘米级高精度地图数据融合获取车辆航向，对惯导航向角参数进行修正；

所述步骤(2)中车辆静止后对惯导设备的俯仰角误差、横滚角误差、航向角误差、加速度计和陀螺仪器件常值误差进行修正，具体过程如下：

车辆静止后真实的速度为0，惯导设备输出的速度即为误差，并以此建立误差模型，利用kalman滤波器对姿态失准角、水平速度误差、加速度计和陀螺仪器件常值进行估计，状态向量X如下所示：

式中，φ_E、φ_N、φ_U分别为俯仰、横滚和航向失准角，δV_E、δV_N分别为东向、北向速度误差，ε_x、ε_y、ε_z分别为三个陀螺仪常值误差，

分别为三个加速度计常值误差；

姿态失准角和速度误差对应的状态方程如下：

公式(2)-(6)中，ω_ie为地球自转角速度，L为纬度，V_E为东向速度，R_M为子午圈曲率半径、R_N为卯酉圈曲率半径，V_N为北向速度，h为高度，f_i(i＝E,N,U)为加速度计输出的比力在导航系下的投影；

T为姿态矩阵，具体表达式如下：

其中γ为横滚角，φ为航向角，θ为俯仰角；

所述步骤(3)具体计算过程如下：

1)利用视觉传感器获取当前车道线的二次函数表示，并投影到车体坐标系O-XY平面，计算二次函数和X轴交点的切线斜率k₀，进而计算得到切线和Y轴夹角β₀；

2)利用激光雷达扫描周边环境，提取出边界特征点，然后投影到车体坐标系O-XY平面，剔除野值点，并利用最小二乘算法拟合出道路边界在二维平面中的二次函数表示，计算该二次函数和

的交点坐标A₀(x₀,y₀)，该点切线和Y轴夹角β₁；

3)根据夹角β₁和厘米级高精度地图对应坐标点航向

可计算基于定位和厘米级高精度地图的当前航向角，公式如下：

式中，

表示基于定位和厘米级高精度地图的车辆航向。

Images