融合激光雷达和深度相机的地图构建方法及装置

技术领域

本发明属于定位与地图构建技术领域，尤其是涉及一种融合激光雷达和深度相机的地图构建方法及装置。

背景技术

同步定位与地图构建（SLAM）技术是实现无人平台自主运动的重要技术之一，在自主机器人、无人驾驶、无人机等领域被广泛应用。目前，根据用于获取障碍物信息的传感器的不同，SLAM主要分为激光SLAM、视觉SLAM和多传感器融合的SLAM。其中，激光点云所包含的信息仅有三维空间信息，故非常容易造成回环失效，而且价格昂贵。与激光SLAM相比，价格便宜、包含信息丰富的视觉传感器SLAM的则是目前SLAM领域的热门研究内容，但也存在探测范围小、易受环境光影响的问题。正因如此，同时采用多种传感器，充分发挥两种传感器的优势，弥补两种传感器的缺点，提高整个系统的准确性和鲁棒性将是SLAM发展的主流方向。二维激光雷达只能获取某平面环境信息，无法反映障碍物其它高度信息，故单纯的二维激光雷达易导致误撞现象。而基于二维激光雷达和深度相机融合的SLAM，既能建立更为全面的环境地图，且不会因使用多线激光雷达而造成成本大幅增加。但是，现有的二维激光雷达和深度相机融合方案仍存在一定的局限性：数据层融合的方案不会过多增加计算量，但由于相机探测角度小于激光雷达探测角度，为保证深度相机检测的障碍物不被激光雷达覆盖掉，会牺牲激光雷达一定的检测范围；决策层融合的方案不会牺牲激光雷达的探测范围，但会因为对深度相机的位姿估计而大大增加计算量。

研究发现，现有的决策层融合能够建立较为完整的环境栅格地图，但构建深度相机栅格地图时没有充分利用定位和建图精度较高的激光SLAM信息，而是通过视觉里程计等方法重新对深度相机进行位姿估计，两次位姿估计严重降低了无人平台定位和地图构建效率。

发明内容

本发明要解决的技术问题是怎样克服在两种传感器数据融合方案中牺牲激光雷达探测角度和决策融合方案中大幅增加计算量，从而使得因计算量大而导致计算效率低的问题，提出了一种融合激光雷达和深度相机的地图构建方法及装置。

为解决上述技术问题，本发明所采用的技术方案是：

一种融合激光雷达和深度相机的地图构建方法，包括以下步骤：步骤1：根据安装在无人平台上的轮式里程计和惯性导航模块所采集的数据，基于扩展卡尔曼滤波预测所述无人平台的初步位姿；

步骤2：根据安装在所述无人平台上的二维激光雷达所采集的二维激光点云数据，将所述初步位姿及所述二维激光点云数据输入Gmapping算法，构建出所述无人平台周围环境的激光雷达栅格地图，同时也得到无人平台的优化位姿数据；

步骤3：将安装在所述无人平台上的深度相机所采集的深度点云数据转换为二维伪激光点云数据，得到所述无人平台周围环境的障碍物不同高度平面信息，结合经Gmapping算法得到的无人平台的优化位姿数据，通过占用栅格地图算法构建出无人平台周围环境的含有障碍物不同高度平面信息的深度相机栅格地图；

步骤4：将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。

进一步地，步骤3中将深度点云数据转换为二维伪激光点云数据的方法是：将所述深度相机所测量的深度点云数据，截取所述深度点云数据中间的一部分行数的像素点，选取这一部分行数的像素点中每列数据中深度值最小的点作为二维伪激光点云数据。

进一步地，步骤4中将激光雷达栅格地图和深度相机栅格进行融合的方法是：

步骤4.1：将由同一时刻所采集的二维激光点云数据所得到的激光雷达栅格地图，以及由相同时刻所采集的深度点云数据所得到的深度相机栅格地图进行原点定位对齐，所述激光雷达栅格地图中的栅格与深度相机栅格地图中的栅格大小一致，所述激光雷达栅格地图中的栅格与深度相机栅格地图中的地图原点、地图分辨率、地图尺寸参数保持一致；

步骤4.2：构建融合栅格地图，所述融合栅格地图的地图原点、地图分辨率、地图尺寸参数与激光雷达地图的参数保持一致；

步骤4.3：当激光雷达栅格地图与深度相机栅格地图对应栅格状态相同时，融合栅格地图对应的栅格状态与当前状态相同；当激光雷达栅格地图或深度相机栅格地图中对应的栅格状态为障碍物占据状态，则融合栅格地图对应的栅格状态为占据状态；当激光雷达栅格地图与深度相机栅格地图所对应的栅格状态不同且均不为占据状态时，融合地图栅格状态与激光雷达栅格状态相同。

进一步地，截取所述深度点云数据中间的一部分行数的像素点是指截取滤掉所述深度点云数据中与地面相关的部分行数数据。

进一步地，截取所述深度点云数据中间一部分行数的像素点是指截取深度点云数据中从上到下第120行附近的像素点数据。

进一步地，步骤3中将深度点云数据转换为二维伪激光点云数据的模型为：

其中，

进一步地，步骤3中深度相机栅格地图的构建方法是：

步骤3.1：对齐二维伪激光点云与所述优化位姿数据的时间戳，对于每一帧优化位姿数据，提取一段时间内与二维伪激光点云时间戳最相近的一帧作为二维伪激光点云数据与之对应；

步骤3.2：将每一帧的优化位姿数据通过静态坐标转换为深度相机在地图坐标系下的位姿；

步骤3.3：将二维伪激光点云数据以及优化位姿数据转换后的位姿数据输入到占用栅格地图构建算法中，得到深度相机栅格地图。

本发明还提供了一种融合激光雷达和深度相机的地图构建装置，包括以下模块：

初步位姿预测模块：用于根据安装在无人平台上的轮式里程计和惯性导航模块所采集的数据，基于扩展卡尔曼滤波预测所述无人平台的初步位姿；

激光雷达栅格地图构建模块：用于根据安装在所述无人平台上的二维激光雷达所采集的二维激光点云数据，将所述初步位姿及所述二维激光点云数据输入Gmapping算法，构建出所述无人平台周围环境的激光雷达栅格地图，同时也得到无人平台的优化位姿数据；

深度相机栅格地图构建模块：用于将安装在所述无人平台上的深度相机所采集的深度点云数据转换为二维伪激光点云数据，得到所述无人平台周围环境的障碍物不同高度平面信息，结合经Gmapping算法得到的无人平台的优化位姿数据，通过占用栅格地图算法构建出无人平台周围环境的含有障碍物不同高度平面信息的深度相机栅格地图；

融合地图输出模块：用于将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。

采用上述技术方案，本发明具有如下有益效果：

本发明提供的一种融合激光雷达和深度相机的同步定位与地图构建方法及装置，通过将深度相机所获取的深度点云信息进行处理，得到二维伪激光点云信息，然后根据二维伪激光点云与经Gmapping算法优化后的无人平台优化位姿信息，得到深度相机栅格地图信息，由于深度相机栅格地图中含有障碍物的不同高度平面信息，将其与激光雷达栅格地图进行融合，避免了因二维激光雷达只能获取单一平面障碍物信息而导致的误撞现象。此外由于本发明的方法中，深度相机的位姿估计是基于激光雷达的位姿估计经静态坐标转换而来的，故本质上只进行了一次位姿估计，相对应传统方法中需要进行激光雷达和深度相机两次位姿估计而言，节省了一次位姿估计的计算量，提高了计算速度，进而提高了地图的构建效率，改善了定位与建图的实时性。

附图说明

图1为本发明的整体流程框图；

图2为本发明具体实施例的流程图；

图3为本发明具体实施例的实验环境；

图4为本发明具体实施例运动模型；

图5为本发明具体实施例构建的激光雷达栅格地图；

图6为本发明具体实施例构建的融合栅格地图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1至图6示出了本发明一种融合激光雷达和深度相机的同步定位与地图构建方法的一种具体实施例，实施例的实验环境，如图3所示，为无人平台搭载了思岚A2激光雷达、AstraPro深度相机和一台工控机，工控机使用Ubuntu 18.06 LTS系统，并以ROS作为无人平台的开发平台。实施环境为走廊环境，走廊宽为3m，并人为设置二维激光雷达无法完整建图的目标障碍物（仅有少量的支撑柱的凳子）。

一种融合激光雷达和深度相机的地图构建方法，如图1和图2所示，包括以下步骤：

步骤1：根据安装在无人平台上的轮式里程计和惯性导航模块所采集的数据，基于扩展卡尔曼滤波预测所述无人平台的初步位姿。

本实施例中，利用轮式里程计和惯性导航模块IMU所采集的数据信息来对无人平台的初步位姿进行预测。具体方法是：

无人平台的运动模型为：

则得到：

进一步可以得到无人平台下一时刻t+1的位姿预测模型为：

当

则

如图4所示，

基于扩展卡尔曼滤波的融合轮式里程计和IMU的位姿预测方法中，预测模型为:

其中，

轮式里程计的观测模型为：

其中，

IMU的观测模型为：

其中，

将以上轮式里程计观测模型和IMU观测模型结合，总观测模型为：

无人平台初步位姿的预测过程为：

无人平台初步位姿的更新过程为：

步骤2：根据安装在所述无人平台上的二维激光雷达所采集的二维激光点云数据，将所述初步位姿及所述二维激光点云数据输入Gmapping算法，构建出所述无人平台周围环境的激光雷达栅格地图，同时也得到无人平台的优化位姿数据。Gmapping算法来自于文献1“Grisetti G , Stachniss C , Burgard W . Improved Techniques for Grid MappingWith Rao-Blackwellized Particle Filters[J]. IEEE Transactions on Robotics,2007, 23(1):34-46.”。

步骤3：将安装在所述无人平台上的深度相机所采集的深度点云数据转换为二维伪激光点云数据，得到所述无人平台周围环境的障碍物不同高度平面信息，结合经Gmapping算法得到的无人平台的优化位姿数据，通过占用栅格地图算法构建出无人平台周围环境的含有障碍物不同高度平面信息的深度相机栅格地图；

步骤3中将深度点云数据转换为二维伪激光点云数据的方法是：将所述深度相机所测量的深度点云数据，截取所述深度点云数据中间的一部分行数的像素点，选取这一部分行数的像素点中每列数据中深度值最小的点作为二维伪激光点云数据。

本实施例中，截取所述深度点云数据中间的一部分行数的像素点是指截取滤掉所述深度点云数据中与地面相关的部分行数数据。既避免因将深度图像中的地面作为障碍物而导致的建图失败，又在保留障碍物不同高度平面信息的基础上，减少了数据处理量，节省了计算资源。

深度相机模型为：

其中，

本实施例中，深度相机所采集的深度图像由高到低对应0~475行像素点，为避免将地面深度点云作为障碍物，只截取120行附近的像素点，并保存每列深度值最小的点，作为二维伪激光点云。将深度点云转换为激光点云的模型为：

步骤3中将深度点云数据转换为二维伪激光点云数据的模型为：

其中，

本实施例中，截取所述深度点云数据中间的一部分行数的像素点是指截取深度点云数据中从上到下第120行附近的像素点数据。经试验验证，截取深度点云数据中从上到下第120行附近的像素点数据作为进行二维伪激光点云的转换数据，可以更好的排除地面的数据信息，效果更好。

本实施例中，深度相机栅格地图的构建方法是：

步骤3.1：对齐二维伪激光点云与所述优化位姿数据的时间戳，对于每一帧优化位姿数据，提取一段时间内与二维伪激光点云时间戳最相近的一帧作为二维伪激光点云数据与之对应；

步骤3.2：将每一帧的优化位姿数据通过静态坐标转换为深度相机在地图坐标系下的位姿；

步骤3.3：将二维伪激光点云数据以及优化位姿数据转换后的位姿数据输入到占用栅格地图构建算法中，得到深度相机栅格地图。占用栅格地图算法来自于文献2“(Thrun,Sebastian. Probabilistic robotics[J]. Communications of the Acm, 2005, 45(3):52-57.)”。

步骤4：将激光雷达栅格地图和深度相机栅格地图进行融合，得到无人平台周围环境的完整栅格地图。

本实施例中，将激光雷达栅格地图和深度相机栅格进行融合的方法是：

步骤4.1：将由同一时刻所采集的二维激光点云数据所得到的激光雷达栅格地图，以及由相同时刻所采集的深度点云数据所得到的深度相机栅格地图进行原点定位对齐，所述激光雷达栅格地图中的栅格与深度相机栅格地图中的栅格大小一致，所述激光雷达栅格地图中的栅格与深度相机栅格地图中的地图原点、地图分辨率、地图尺寸参数保持一致；

步骤4.2：构建融合栅格地图，所述融合栅格地图的地图原点、地图分辨率、地图尺寸参数与激光雷达地图的参数保持一致；

步骤4.3：当激光雷达栅格地图与深度相机栅格地图对应栅格状态相同时，融合栅格地图对应的栅格状态与当前状态相同；当激光雷达栅格地图或深度相机栅格地图中对应的栅格状态为障碍物占据状态，则融合栅格地图对应的栅格状态为占据状态；当激光雷达栅格地图与深度相机栅格地图所对应的栅格状态不同且均不为占据状态时，融合地图栅格状态与激光雷达栅格状态相同。

图5为融合前的激光雷达栅格地图，由于激光雷达只能获取某一平面的障碍物信息，此栅格地图仅包含障碍物少量的支撑柱特征（四个点）；图6为融合后的栅格地图，由于深度相机可获取不同高度平面的障碍物信息，故融合后的地图完整地包含了障碍物的整体特征（四边形）。两图对比可知，只用激光雷达建图不能完整表达走廊中间障碍物的信息，采用本发明提出的一种融合激光雷达和深度相机的同步定位与地图构建方法，能够将被激光雷达忽略掉的障碍物信息完整地表达出来，提高了多传感器融合建图效率，改善了环境地图的完整性和精确性。

本发明还提供了一种融合激光雷达和深度相机的地图构建装置，包括以下模块：

初步位姿预测模块：用于根据安装在无人平台上的轮式里程计和惯性导航模块所采集的数据，基于扩展卡尔曼滤波预测所述无人平台的初步位姿；

激光雷达栅格地图构建模块：用于根据安装在所述无人平台上的二维激光雷达所采集的二维激光点云数据，将所述初步位姿及所述二维激光点云数据输入Gmapping算法，构建出所述无人平台周围环境的激光雷达栅格地图，同时也得到无人平台的优化位姿数据；

深度相机栅格地图构建模块：用于将安装在所述无人平台上的深度相机所采集的深度点云数据转换为二维伪激光点云数据，得到所述无人平台周围环境的障碍物深度方向信息，结合经Gmapping算法得到的无人平台的优化位姿数据，通过占用栅格地图算法构建出无人平台周围环境的含有障碍物不同高度平面信息的深度相机栅格地图；

融合地图输出模块：用于将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。