CN107246876A - 一种无人驾驶汽车自主定位与地图构建的方法及系统 - Google Patents

Abstract

本发明公开了发明一种无人驾驶汽车自主定位与地图构建的方法及系统，利用SLAM的技术，将多种传感器数据进行融合，采用新型的算法结构，构建稳定有效的框架。利用粒子滤波器优化三维激光雷达的数据，将三维激光雷达的数据转换成视觉模型，利用词袋模型进行闭环检测，对无人驾驶汽车进行稳定有效的自主定位与地图构建，提高运算效率和运行速度，应用到无人驾驶汽车系统上，进行大规模应用。

Description

一种无人驾驶汽车自主定位与地图构建的方法及系统

技术领域

本发明涉及机器人及控制领域，尤其涉及一种无人驾驶汽车自主定位与地图构建的方法。

背景技术

近年来，互联网技术的迅速发展给汽车制造工业带来了革命性变化的机会。与此同时，汽车智能化技术正逐步得到广泛应用，这项技术简化了汽车的驾驶操作并提高了行驶安全性。而其中最典型也是最热门的未来应用就是无人驾驶汽车。在人工智能技术的加持下，无人驾驶高速发展，正在改变人类的出行方式，进而会大规模改变相关行业格局。

对于行驶在未知区域中的无人驾驶汽车而言，由于楼宇、树木遮挡等原因，汽车常处于无信号或弱信号的状态，无法提供有效定位；在环境恶劣的地方，因天气等原因GPS或北斗导航系统信号微弱，无法对无人驾驶汽车进行有效的定位。为此，无人驾驶汽车必须具有自主定位与地图构建的能力。通过实时的自主定位与地图构建，获取周围环境信息，确定无人驾驶汽车所处的位置，为路径规划提供重要的依据。

在机器人领域，同时定位与地图构建(simultaneous localization andmapping，SLAM)技术能够对机器人进行实时定位与地图构建，是当今的主要研究对象。

然而如今的无人驾驶汽车大多仍存在于辅助驾驶阶段，缺乏自主定位与地图构建的能力。同时较少的无人驾驶汽车采用单一传感器进行自主定位与地图构建，定位与地图构建精度较低，不能有效的将多种传感器进行融合，或者能够将多种传感器数据进行融合，但是不能够对无人驾驶汽车进行稳定有效的自主定位与地图构建，并且不能大规模普及。

为此发明一种无人驾驶汽车自主定位与地图构建的方法，利用SLAM的技术，将多种传感器数据进行融合，采用新型的算法结构，构建稳定有效的框架，对无人驾驶汽车进行稳定有效的自主定位与地图构建，应用到无人驾驶汽车系统上，进行大规模应用。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种无人驾驶汽车自主定位与地图构建的方法，利用无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的数据，通过数据融合及优化，对无人驾驶汽车进行自主定位与地图构建。

为实现上述目的，本发明采用的技术方案为：

一种无人驾驶汽车自主定位与地图构建的方法，包括以下步骤：

步骤一、初始化无人驾驶汽车的位姿为x₀，无人驾驶汽车行驶的轨迹为X_1:t＝{x₁,x₂...x_t}，校正无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的时间，使无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的数据保持时间一致性；

步骤二、采集无人驾驶汽车车轮里程计数据U_1:t＝{u₁,u₂...u_t}，IMU惯性测量单元的数据I_1:t＝{i₁,i₂...i_t}，全景摄像头的数据C_1:t＝{c₁,c₂...c_t}，三维激光雷达的数据R_1:t＝{r₁,r₂...r_t}，汽车大脑通过全景摄像头和三维激光雷达的数据计算出无人驾驶汽车周围所有环境路标为L_1:t＝{l₁,l₂...l_t}；

步骤三、利用评价函数F评价全景摄像头采集数据C_1:t的复杂程度，根据评价指标选择fast、orb、surf、sift特征点的一种，进一步利用PCL库将数据C_1:t转换成点云P₁:t；

步骤四、利用粒子滤波器去除三维激光雷达的数据R_1:t异常值，与步骤三生成的点云数据P₁:t、无人驾驶汽车车轮里程计数据U_1:t＝{u₁,u₂...u_t}，IMU惯性测量单元的数据I_1:t＝{i₁,i₂...i_t}进行数据关联；

步骤五、估计无人驾驶汽车轨迹和周围环境三维地图的后验概率P(X_1:t，L_1:t|C_1:t，R_1:t，U_1:t，I_1:t，x₀)，进一步将函数转化成最小二乘法问题进行求解，得出无人驾驶汽车实时位姿x_t’；

步骤六、根据步骤五中的无人驾驶汽车实时位姿x_t’，利用图优化的方法将步骤三中的点云和步骤四优化的三维激光雷达的数据R_1:t实时构建无人驾驶汽车周围环境三维地图；

步骤七、利用词袋模型进行闭环检测，进一步提高人驾驶汽车周围环境三维地图的精度。

优选的是，在步骤三中的环境复杂评价函数，F＝G+H，G为环境的亮度函数，H为sift特征点数量函数，

优选的是，在步骤四中，将三维激光雷达的数据的坐标系均转换为世界坐标系下的数据，根据时间戳进行数据关联；

数据关联函数:

优选的是，在步骤五中的最小二乘法问题中，具体采用g2o模块中Levenberg-Marqudart方法进行求解。

优选的是，在步骤五中，转化后的公式为

其中h_i,j(Q_i-L_j)为关联数据在路标平面图像上投影位置的函数。

优选的是，在步骤六的无人驾驶汽车周围环境三维地图构建中，针对稀疏矩阵采用cholesky分解进行求解，提高运算速度。

优选的是，在步骤七的的闭环检测中，将步骤四优化的三维激光雷达的数据R_1:t转换成视觉模型，同样利用词袋模型进行闭环检测。

一种无人驾驶汽车自主定位与地图构建的系统，该系统包括汽车大脑、车轮里程计、全景摄像头、IMU惯性测量单元和三维激光雷达；所述汽车大脑作为整个系统的核心部件，分别与车轮里程计、全景摄像头、IMU惯性测量单元和三维激光雷达相连；所述车轮里程计采集汽车行程数据；所述全景摄像头观测汽车周围视觉环境；所述IMU惯性测量单元采集测量汽车三轴姿态角以及加速度；所述三维激光雷达采集汽车周围环境的点云信息。

本发明有益效果：

本发明提供的一种无人驾驶汽车自主定位与地图构建的方法，利用SLAM的技术，将多种传感器数据进行融合，采用新型的算法结构，构建稳定有效的框架，对无人驾驶汽车进行稳定有效的自主定位与地图构建，应用到无人驾驶汽车系统上，进行大规模应用。

附图说明

图1为本发明的无人驾驶汽车自主定位与地图构建的方法示意图；

图2为本发明的无人驾驶汽车自主定位与地图构建的系统框图；

图3为本发明实现自主定位的图示；

图4、图5分别为本发明实现地图构建的图示。

具体实施方式

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

如图1，一种无人驾驶汽车自主定位与地图构建的方法，包括以下步骤：

步骤一、初始化无人驾驶汽车的位姿为x₀，无人驾驶汽车行驶的轨迹为X_1:t＝{x₁,x₂...x_t}，校正无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的时间，使无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的数据保持时间一致性；

步骤二、采集无人驾驶汽车车轮里程计数据U_1:t＝{u₁,u₂...u_t}，IMU惯性测量单元的数据I_1:t＝{i₁,i₂...i_t}，全景摄像头的数据C_1:t＝{c₁,c₂...c_t}，三维激光雷达的数据R_1:t＝{r₁,r₂...r_t}，汽车大脑通过全景摄像头和三维激光雷达的数据计算出无人驾驶汽车周围所有环境路标为L_1:t＝{l₁,l₂...l_t}；

步骤三、利用评价函数F评价全景摄像头采集数据C_1:t的复杂程度，根据评价指标选择fast、orb、surf、sift特征点的一种，进一步利用PCL库将数据C_1:t转换成点云P₁:t；PLC库是一个依赖的开源库，可以当成是一种软件。

在步骤三中的环境复杂评价函数，F＝G+H，G为环境的亮度函数，H为sift特征点数量函数，

步骤四、利用粒子滤波器去除三维激光雷达的数据R_1:t异常值,与步骤三生成的点云数据P₁:t、无人驾驶汽车车轮里程计数据U_1:t＝{u₁,u₂...u_t}，IMU惯性测量单元的数据I_1:t＝{i₁,i₂...i_t}进行数据关联；

将三维激光雷达、的数据的坐标系均转换为世界坐标系下的数据，根据时间戳进行数据关联。

数据关联函数

步骤五、估计无人驾驶汽车轨迹和周围环境三维地图的后验概率函数进一步将该函数转化成最小二乘法问题进行求解，得出无人驾驶汽车实时位姿x_t’；

转化后的公式为

其中h_i,j(Q_i-L_j)为关联数据在路标平面图像上投影位置的函数

在步骤五中的最小二乘法问题中，具体采用g2o模块中Levenberg-Marqudart方法进行求解；

步骤六、根据步骤五中的无人驾驶汽车实时位姿x_t’，根据同一时间t下三维激光雷达数据和x_t’在中间坐标中位置，进行有序堆积，利用图优化的方法将步骤三中的点云和步骤四优化的三维激光雷达的数据实时构建无人驾驶汽车周围环境三维地图；

在步骤六的无人驾驶汽车周围环境三维地图构建中，针对稀疏矩阵采用cholesky分解进行求解，将一个矩阵分解为若干个矩阵的乘积可以大大降低存储空间，提高运算速度；

步骤七、利用词袋模型进行闭环检测，当遇到重复或已知地域时，能够将运行轨迹进行回环处理，进一步提高人驾驶汽车周围环境三维地图的精度；

在步骤七的的闭环检测中，将步骤四优化的三维激光雷达的数据R_1:t转换成视觉模型，同样利用词袋模型进行闭环检测。

如图2所示，一种无人驾驶汽车自主定位与地图构建的系统，该系统包括汽车大脑1、车轮里程计2、全景摄像头3、IMU惯性测量单元4和三维激光雷达5；

所述汽车大脑1作为整个系统的核心部件，分别与车轮里程计2、全景摄像头3、IMU惯性测量单元4和三维激光雷达5相连；所述车轮里程计2采集汽车行程数据；所述全景摄像头3观测汽车周围视觉环境；所述IMU惯性测量单元4采集测量汽车三轴姿态角以及加速度；所述三维激光雷达5采集汽车周围环境的点云信息。其中汽车大脑1配备高性能的CPU和GPU.

如图3所示，给出了本发明实现自主定位的图示，图中的直线为汽车定位的位姿。

图4、图5分别为本发明实现地图构建的图示。本发明提供的一种无人驾驶汽车自主定位与地图构建的方法，利用SLAM的技术，将多种传感器数据进行融合，采用新型的算法结构，构建稳定有效的框架，对无人驾驶汽车进行稳定有效的自主定位与地图构建，应用到无人驾驶汽车系统上，进行大规模应用。

以上仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (8)

1.一种无人驾驶汽车自主定位与地图构建的方法，其特征在于，包括以下步骤：

步骤一、初始化无人驾驶汽车的位姿为x₀，无人驾驶汽车行驶的轨迹为X_1:t＝{x₁,x₂...x_t}，校正无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的时间，使无人驾驶汽车车轮里程计、IMU惯性测量单元、全景摄像头、三维激光雷达的数据保持时间一致性；

步骤二、采集无人驾驶汽车车轮里程计数据U_1:t＝{u₁,u₂...u_t}，IMU惯性测量单元的数据I_1:t＝{i₁,i₂...i_t}，全景摄像头的数据C_1:t＝{c₁,c₂...c_t}，三维激光雷达的数据R_1:t＝{r₁,r₂...r_t}，汽车大脑通过全景摄像头和三维激光雷达的数据计算出无人驾驶汽车周围所有环境路标为L_1:t＝{l₁,l₂...l_t}；

步骤三、利用评价函数F评价全景摄像头采集数据C_1:t的复杂程度，根据评价指标选择fast、orb、surf、sift特征点的一种，进一步利用PCL库将数据C_1:t转换成点云P₁:t；

步骤四、利用粒子滤波器去除三维激光雷达的数据R_1:t异常值，与步骤三生成的点云数据P₁:t、无人驾驶汽车车轮里程计数据U_1:t＝{u₁,u₂...u_t}，IMU惯性测量单元的数据I_1:t＝{i₁,i₂...i_t}进行数据关联；

步骤五、估计无人驾驶汽车轨迹和周围环境三维地图的后验概率P(X_1:t，L_1:t|C_1:t，R_1:t，U_1:t，I_1:t，x₀)，进一步将函数转化成最小二乘法问题进行求解，得出无人驾驶汽车实时位姿x_t’；

步骤六、根据步骤五中的无人驾驶汽车实时位姿x_t’，利用图优化的方法将步骤三中的点云和步骤四优化的三维激光雷达的数据R_1:t实时构建无人驾驶汽车周围环境三维地图；

步骤七、利用词袋模型进行闭环检测，进一步提高人驾驶汽车周围环境三维地图的精度。

2.根据权利要求1所述的无人驾驶汽车自主定位与地图构建的方法，其特征在于：在步骤三中的环境复杂评价函数，F＝G+H，G为环境的亮度函数，H为sift特征点数量函数，

3.根据权利要求1所述的无人驾驶汽车自主定位与地图构建的方法，其特征在于：在步骤四中，将三维激光雷达的数据的坐标系均转换为世界坐标系下的数据，根据时间戳进行数据关联；

<mrow> <msubsup> <mi>R</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mi>T</mi> <mi>r</mi> <mi>w</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msub> <mi>R</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>;</mo> </mrow>

<mrow> <msubsup> <mi>P</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mi>T</mi> <mi>p</mi> <mi>w</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msub> <mi>P</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>;</mo> </mrow>

<mrow> <msubsup> <mi>U</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mi>T</mi> <mi>u</mi> <mi>w</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msub> <mi>U</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>;</mo> </mrow>

<mrow> <msubsup> <mi>I</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mi>T</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msub> <mi>I</mi> <mrow> <mn>1</mn> <mo>:</mo> <mi>t</mi> </mrow> </msub> <mo>;</mo> </mrow>

数据关联函数:

<mrow> <msub> <mi>Q</mi> <mi>t</mi> </msub> <mo>=</mo> <mi>q</mi> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>t</mi> <mi>w</mi> </msubsup> <mo>,</mo> <msubsup> <mi>p</mi> <mi>t</mi> <mi>w</mi> </msubsup> <mo>,</mo> <msubsup> <mi>u</mi> <mi>t</mi> <mi>w</mi> </msubsup> <mo>,</mo> <msubsup> <mi>i</mi> <mi>t</mi> <mi>w</mi> </msubsup> <mo>)</mo> </mrow> <mo>.</mo> </mrow>

4.根据权利要求1所述的无人驾驶汽车自主定位与地图构建的方法，其特征在于：在步骤五中的最小二乘法问题中，具体采用g2o模块中Levenberg-Marqudart方法进行求解。

5.根据权利要求1所述的无人驾驶汽车自主定位与地图构建的方法，其特征在于：在步骤五中，转化后的公式为

<mrow> <munder> <mi>arg</mi> <mrow> <msub> <mi>Q</mi> <mn>1</mn> </msub> <msub> <mi>KQ</mi> <mi>m</mi> </msub> </mrow> </munder> <munder> <mi>min</mi> <mrow> <msub> <mi>L</mi> <mn>1</mn> </msub> <mn>...</mn> <msub> <mi>L</mi> <mi>n</mi> </msub> </mrow> </munder> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <mi>h</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>L</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <msup> <mi>x</mi> <mo>,</mo> </msup> <mi>t</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow>

其中h_i,j(Q_i-L_j)为关联数据在路标平面图像上投影位置的函数。

6.根据权利要求1所述的无人驾驶汽车自主定位与地图构建的方法，其特征在于：在步骤六的无人驾驶汽车周围环境三维地图构建中，针对稀疏矩阵采用cholesky分解进行求解，提高运算速度。

7.根据权利要求1所述的无人驾驶汽车自主定位与地图构建的方法，其特征在于：在步骤七的的闭环检测中，将步骤四优化的三维激光雷达的数据R_1:t转换成视觉模型，同样利用词袋模型进行闭环检测。

8.一种无人驾驶汽车自主定位与地图构建的系统，其特征在于：该系统包括汽车大脑、车轮里程计、全景摄像头、IMU惯性测量单元和三维激光雷达；

所述汽车大脑作为整个系统的核心部件，分别与车轮里程计、全景摄像头、IMU惯性测量单元和三维激光雷达相连；

所述车轮里程计采集汽车行程数据；所述全景摄像头观测汽车周围视觉环境；所述IMU惯性测量单元采集测量汽车三轴姿态角以及加速度；所述三维激光雷达采集汽车周围环境的点云信息。