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

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

Info

Publication number
CN113253297A
CN113253297A CN202110683274.5A CN202110683274A CN113253297A CN 113253297 A CN113253297 A CN 113253297A CN 202110683274 A CN202110683274 A CN 202110683274A CN 113253297 A CN113253297 A CN 113253297A
Authority
CN
China
Prior art keywords
point cloud
map
grid map
depth camera
grid
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110683274.5A
Other languages
English (en)
Other versions
CN113253297B (zh
Inventor
王东
张萌
李国喜
赵存
杜日煌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202110683274.5A priority Critical patent/CN113253297B/zh
Publication of CN113253297A publication Critical patent/CN113253297A/zh
Application granted granted Critical
Publication of CN113253297B publication Critical patent/CN113253297B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Abstract

本发明提供一种融合激光雷达和深度相机的地图构建方法及装置，基于扩展卡尔曼滤波预测无人平台的初步位姿；基于二维激光点云数据构建出激光雷达栅格地图，同时得到无人平台的优化位姿数据；将深度点云数据转换为二维伪激光点云数据，构建出含有障碍物不同高度平面信息的深度相机栅格地图；将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。通过将深度相机栅格地图与激光雷达栅格地图进行融合，避免因二维激光雷达只能获取单一平面障碍物信息而导致的误撞现象。由于只需要对无人平台进行一次位姿估计，与现有方法相比，节省一次位姿估计的计算量，提高了计算速度，进而提高地图的构建效率。

Description

融合激光雷达和深度相机的地图构建方法及装置
技术领域
本发明属于定位与地图构建技术领域，尤其是涉及一种融合激光雷达和深度相机的地图构建方法及装置。
背景技术
同步定位与地图构建（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中将深度点云数据转换为二维伪激光点云数据的模型为：
Figure 738877DEST_PATH_IMAGE001
Figure 853464DEST_PATH_IMAGE002
其中，
Figure 666961DEST_PATH_IMAGE003
、
Figure 308027DEST_PATH_IMAGE004
分别为深度点云数据在相机坐标系下的横坐标值及深度值，
Figure 680102DEST_PATH_IMAGE005
为深度点云与
Figure 780520DEST_PATH_IMAGE003
轴的角度，
Figure 630665DEST_PATH_IMAGE006
为深度点云到深度相机的距离。
进一步地，步骤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所采集的数据信息来对无人平台的初步位姿进行预测。具体方法是：
无人平台的运动模型为：
Figure 532762DEST_PATH_IMAGE007
Figure 810159DEST_PATH_IMAGE008
Figure 791016DEST_PATH_IMAGE009
则得到：
Figure 444851DEST_PATH_IMAGE010
Figure 201454DEST_PATH_IMAGE011
Figure 419727DEST_PATH_IMAGE012
Figure 261781DEST_PATH_IMAGE013
为无人平台的运动半径，
Figure 719307DEST_PATH_IMAGE014
分别为轮式里程计中左右两驱动轮的运动半径，
Figure 330417DEST_PATH_IMAGE015
为无人平台中心点，
Figure 716661DEST_PATH_IMAGE016
为无人平台角速度，
Figure 46012DEST_PATH_IMAGE017
是无人平台线速度，
Figure 307229DEST_PATH_IMAGE018
分别为两驱动轮的线速度，
Figure 38424DEST_PATH_IMAGE019
为轮式里程计中左右两驱动轮之间的距离。
进一步可以得到无人平台下一时刻t+1的位姿预测模型为：
Figure 654957DEST_PATH_IMAGE020
当
Figure 737182DEST_PATH_IMAGE021
很短时，
Figure 536511DEST_PATH_IMAGE022
，此时：
Figure 358099DEST_PATH_IMAGE023
则
Figure 584681DEST_PATH_IMAGE024
如图4所示，
Figure 888623DEST_PATH_IMAGE025
为无人平台前后时刻的转动角度，
Figure 855092DEST_PATH_IMAGE026
表示无人平台前后时刻的平移距离，
Figure 295301DEST_PATH_IMAGE027
表示无人平台前后时刻走过的距离，
Figure 958363DEST_PATH_IMAGE028
为采样时间间隔，
Figure 218443DEST_PATH_IMAGE029
和
Figure 392198DEST_PATH_IMAGE030
分别表示
Figure 421334DEST_PATH_IMAGE031
时刻和
Figure 255298DEST_PATH_IMAGE032
时刻的位姿。
基于扩展卡尔曼滤波的融合轮式里程计和IMU的位姿预测方法中，预测模型为:
Figure 268253DEST_PATH_IMAGE033
Figure 711610DEST_PATH_IMAGE034
其中，
Figure 126411DEST_PATH_IMAGE035
和
Figure 131276DEST_PATH_IMAGE036
表示预测时刻
Figure 365948DEST_PATH_IMAGE037
和前一时刻
Figure 615927DEST_PATH_IMAGE038
的状态量，
Figure 885234DEST_PATH_IMAGE039
为状态矩阵，
Figure 61000DEST_PATH_IMAGE040
为高斯噪声。
轮式里程计的观测模型为：
Figure 48548DEST_PATH_IMAGE041
Figure 863005DEST_PATH_IMAGE042
Figure 252398DEST_PATH_IMAGE043
其中，
Figure 333486DEST_PATH_IMAGE044
为轮式里程计的观测值，
Figure 808330DEST_PATH_IMAGE045
为轮式里程计的观测矩阵，
Figure 400110DEST_PATH_IMAGE046
为高斯噪声。
IMU的观测模型为：
Figure 378431DEST_PATH_IMAGE047
Figure 896000DEST_PATH_IMAGE048
Figure 592560DEST_PATH_IMAGE049
其中，
Figure 719523DEST_PATH_IMAGE050
为IMU的观测值，
Figure 83508DEST_PATH_IMAGE051
为IMU的观测矩阵，
Figure 771978DEST_PATH_IMAGE052
为高斯噪声。
将以上轮式里程计观测模型和IMU观测模型结合，总观测模型为：
Figure 50775DEST_PATH_IMAGE053
Figure 482894DEST_PATH_IMAGE054
Figure 701385DEST_PATH_IMAGE055
Figure 295178DEST_PATH_IMAGE056
Figure 205146DEST_PATH_IMAGE057
无人平台初步位姿的预测过程为：
Figure 175376DEST_PATH_IMAGE058
Figure 248374DEST_PATH_IMAGE059
为
Figure 278647DEST_PATH_IMAGE060
时刻状态向量的先验估计，
Figure 938561DEST_PATH_IMAGE061
为
Figure 571536DEST_PATH_IMAGE062
时刻状态向量的后验估计，
Figure 499041DEST_PATH_IMAGE063
为雅克比矩阵。
无人平台初步位姿的更新过程为：
Figure 198750DEST_PATH_IMAGE065
Figure 985441DEST_PATH_IMAGE066
为所求的无人平台初步位姿，
Figure 563053DEST_PATH_IMAGE067
为增益矩阵。
步骤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中将深度点云数据转换为二维伪激光点云数据的方法是：将所述深度相机所测量的深度点云数据，截取所述深度点云数据中间的一部分行数的像素点，选取这一部分行数的像素点中每列数据中深度值最小的点作为二维伪激光点云数据。
本实施例中，截取所述深度点云数据中间的一部分行数的像素点是指截取滤掉所述深度点云数据中与地面相关的部分行数数据。既避免因将深度图像中的地面作为障碍物而导致的建图失败，又在保留障碍物不同高度平面信息的基础上，减少了数据处理量，节省了计算资源。
深度相机模型为：
Figure 204118DEST_PATH_IMAGE068
其中，
Figure 77659DEST_PATH_IMAGE069
为深度点云在像素坐标系下的坐标，
Figure 210700DEST_PATH_IMAGE070
为深度点云在相机坐标系下的坐标，
Figure 326423DEST_PATH_IMAGE071
称作相机的内参矩阵，
Figure 228520DEST_PATH_IMAGE072
和
Figure 10312DEST_PATH_IMAGE073
为深度相机放大系数，
Figure 365070DEST_PATH_IMAGE074
和
Figure 18906DEST_PATH_IMAGE075
为深度相机的主点坐标。
本实施例中，深度相机所采集的深度图像由高到低对应0~475行像素点，为避免将地面深度点云作为障碍物，只截取120行附近的像素点，并保存每列深度值最小的点，作为二维伪激光点云。将深度点云转换为激光点云的模型为：
步骤3中将深度点云数据转换为二维伪激光点云数据的模型为：
Figure 775509DEST_PATH_IMAGE001
Figure 256431DEST_PATH_IMAGE076
其中，
Figure 832906DEST_PATH_IMAGE077
分别为深度点云在相机坐标系下的横坐标值及深度值，
Figure 290432DEST_PATH_IMAGE079
为深度点云与
Figure 167121DEST_PATH_IMAGE080
轴的角度，
Figure 550436DEST_PATH_IMAGE081
为深度点云到深度相机的距离。
本实施例中，截取所述深度点云数据中间的一部分行数的像素点是指截取深度点云数据中从上到下第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算法得到的无人平台的优化位姿数据，通过占用栅格地图算法构建出无人平台周围环境的含有障碍物不同高度平面信息的深度相机栅格地图；
融合地图输出模块：用于将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种融合激光雷达和深度相机的地图构建方法，其特征在于，包括以下步骤：
步骤1：根据安装在无人平台上的轮式里程计和惯性导航模块所采集的数据，基于扩展卡尔曼滤波预测所述无人平台的初步位姿；
步骤2：根据安装在所述无人平台上的二维激光雷达所采集的二维激光点云数据，将所述初步位姿及所述二维激光点云数据输入Gmapping算法，构建出所述无人平台周围环境的激光雷达栅格地图，同时也得到无人平台的优化位姿数据；
步骤3：将安装在所述无人平台上的深度相机所采集的深度点云数据转换为二维伪激光点云数据，结合经Gmapping算法得到的无人平台的优化位姿数据，通过占用栅格地图算法构建出无人平台周围环境的深度相机栅格地图；
步骤4：将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。
2.根据权利要求1所述的构建方法，其特征在于，步骤3中将深度点云数据转换为二维伪激光点云数据的方法是：
将所述深度相机所测量的深度点云数据，截取所述深度点云数据中间的一部分行数的像素点，选取这一部分行数的像素点中每列数据中深度值最小的点作为二维伪激光点云数据。
3.根据权利要求2所述的构建方法，其特征在于，步骤4中将激光雷达栅格地图和深度相机栅格进行融合的方法是：
步骤4.1：将由同一时刻所采集的二维激光点云数据所得到的激光雷达栅格地图，以及由相同时刻所采集的深度点云数据所得到的深度相机栅格地图进行原点定位对齐，所述激光雷达栅格地图中的栅格与深度相机栅格地图中的栅格大小一致，所述激光雷达栅格地图中的栅格与深度相机栅格地图中的地图原点、地图分辨率、地图尺寸参数保持一致；
步骤4.2：构建融合栅格地图，所述融合栅格地图的地图原点、地图分辨率、地图尺寸参数与激光雷达地图的参数保持一致；
步骤4.3：当激光雷达栅格地图与深度相机栅格地图对应栅格状态相同时，融合栅格地图对应的栅格状态与当前状态相同；当激光雷达栅格地图或深度相机栅格地图中对应的栅格状态为障碍物占据状态，则融合栅格地图对应的栅格状态为占据状态；当激光雷达栅格地图与深度相机栅格地图所对应的栅格状态不同且均不为占据状态时，融合地图栅格状态与激光雷达栅格状态相同。
4.根据权利要求2所述的构建方法，其特征在于，截取所述深度点云数据中间的一部分行数的像素点是指截取滤掉所述深度点云数据中与地面相关的部分行数数据。
5.根据权利要求4所述的构建方法，其特征在于，截取所述深度点云数据中间的一部分行数的像素点是指截取深度点云数据中从上到下第120行附近的像素点数据。
6.根据权利要求2所述的构建方法，其特征在于，步骤3中将深度点云数据转换为二维伪激光点云数据的模型为：
Figure 188102DEST_PATH_IMAGE001
Figure 70608DEST_PATH_IMAGE002
其中，
Figure 891933DEST_PATH_IMAGE003
分别为深度点云在相机坐标系下的横坐标值及深度值，
Figure 834481DEST_PATH_IMAGE004
为深度点云与
Figure 272416DEST_PATH_IMAGE005
轴的角度，
Figure 411142DEST_PATH_IMAGE006
为深度点云到深度相机的距离。
7.根据权利要求1所述的构建方法，其特征在于，步骤3中深度相机栅格地图的构建方法是：
步骤3.1：对齐二维伪激光点云与所述优化位姿数据的时间戳，对于每一帧优化位姿数据，提取一段时间内与二维伪激光点云时间戳最相近的一帧作为二维伪激光点云数据与之对应；
步骤3.2：将每一帧的优化位姿数据通过静态坐标转换为深度相机在地图坐标系下的位姿；
步骤3.3：将二维伪激光点云数据以及优化位姿数据转换后的位姿数据输入到占用栅格地图构建算法中，得到深度相机栅格地图。
8.一种融合激光雷达和深度相机的地图构建装置，其特征在于，包括以下模块：
初步位姿预测模块：用于根据安装在无人平台上的轮式里程计和惯性导航模块所采集的数据，基于扩展卡尔曼滤波预测所述无人平台的初步位姿；
激光雷达栅格地图构建模块：用于根据安装在所述无人平台上的二维激光雷达所采集的二维激光点云数据，将所述初步位姿及所述二维激光点云数据输入Gmapping算法，构建出所述无人平台周围环境的激光雷达栅格地图，同时也得到无人平台的优化位姿数据；
深度相机栅格地图构建模块：用于将安装在所述无人平台上的深度相机所采集的深度点云数据转换为二维伪激光点云数据，得到所述无人平台周围环境的障碍物不同高度平面信息，结合经Gmapping算法得到的无人平台的优化位姿数据，通过占用栅格地图构建算法构建出无人平台周围环境的含有障碍物不同高度平面信息的深度相机栅格地图；
融合地图输出模块：用于将激光雷达栅格地图和深度相机栅格进行融合，得到无人平台周围环境的完整栅格地图。
CN202110683274.5A 2021-06-21 2021-06-21 融合激光雷达和深度相机的地图构建方法及装置 Active CN113253297B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110683274.5A CN113253297B (zh) 2021-06-21 2021-06-21 融合激光雷达和深度相机的地图构建方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110683274.5A CN113253297B (zh) 2021-06-21 2021-06-21 融合激光雷达和深度相机的地图构建方法及装置

Publications (2)

Publication Number Publication Date
CN113253297A true CN113253297A (zh) 2021-08-13
CN113253297B CN113253297B (zh) 2021-09-17

Family

ID=77188854

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110683274.5A Active CN113253297B (zh) 2021-06-21 2021-06-21 融合激光雷达和深度相机的地图构建方法及装置

Country Status (1)

Country Link
CN (1) CN113253297B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113763548A (zh) * 2021-08-17 2021-12-07 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN114018236A (zh) * 2021-09-30 2022-02-08 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN114111791A (zh) * 2021-11-22 2022-03-01 国网江苏省电力有限公司信息通信分公司 一种智能机器人室内自主导航方法、系统及存储介质
CN114202567A (zh) * 2021-12-03 2022-03-18 江苏集萃智能制造技术研究所有限公司 一种基于视觉的点云处理避障方法
CN115307646A (zh) * 2022-10-08 2022-11-08 浙江光珀智能科技有限公司 一种多传感器融合的机器人定位方法、系统及装置
EP4250254A1 (en) * 2022-03-21 2023-09-27 Volvo Car Corporation Fast sensor fusion for free space detection

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106526605A (zh) * 2016-10-28 2017-03-22 北京康力优蓝机器人科技有限公司 激光雷达与深度相机的数据融合方法及系统
CN108073167A (zh) * 2016-11-10 2018-05-25 深圳灵喵机器人技术有限公司 一种基于深度相机与激光雷达的定位与导航方法
CN110595457A (zh) * 2019-08-29 2019-12-20 山东大学 伪激光数据生成方法、地图构建方法、导航方法及系统
US20200184718A1 (en) * 2018-12-05 2020-06-11 Sri International Multi-modal data fusion for enhanced 3d perception for platforms
CN111580130A (zh) * 2020-05-25 2020-08-25 中国计量大学 一种基于多传感器融合的建图方法
CN111679663A (zh) * 2019-02-25 2020-09-18 北京奇虎科技有限公司 三维地图构建方法、扫地机器人及电子设备
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106526605A (zh) * 2016-10-28 2017-03-22 北京康力优蓝机器人科技有限公司 激光雷达与深度相机的数据融合方法及系统
CN108073167A (zh) * 2016-11-10 2018-05-25 深圳灵喵机器人技术有限公司 一种基于深度相机与激光雷达的定位与导航方法
US20200184718A1 (en) * 2018-12-05 2020-06-11 Sri International Multi-modal data fusion for enhanced 3d perception for platforms
CN111679663A (zh) * 2019-02-25 2020-09-18 北京奇虎科技有限公司 三维地图构建方法、扫地机器人及电子设备
CN110595457A (zh) * 2019-08-29 2019-12-20 山东大学 伪激光数据生成方法、地图构建方法、导航方法及系统
CN111580130A (zh) * 2020-05-25 2020-08-25 中国计量大学 一种基于多传感器融合的建图方法
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
DONG WANG 等: "SLAM-based Cooperative Calibration for Optical Sensors Array with GPS/IMU Aided", 《2016 INTERNATIONAL CONFERENCE ON UNMANNED AIRCRAFT SYSTEMS》 *
陈文佑 等: "一种融合深度相机与激光雷达的室内移动机器人建图与导航方法", 《智能计算机与应用》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113763548A (zh) * 2021-08-17 2021-12-07 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN113763548B (zh) * 2021-08-17 2024-02-27 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN114018236A (zh) * 2021-09-30 2022-02-08 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN114018236B (zh) * 2021-09-30 2023-11-03 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN114111791A (zh) * 2021-11-22 2022-03-01 国网江苏省电力有限公司信息通信分公司 一种智能机器人室内自主导航方法、系统及存储介质
CN114202567A (zh) * 2021-12-03 2022-03-18 江苏集萃智能制造技术研究所有限公司 一种基于视觉的点云处理避障方法
EP4250254A1 (en) * 2022-03-21 2023-09-27 Volvo Car Corporation Fast sensor fusion for free space detection
CN115307646A (zh) * 2022-10-08 2022-11-08 浙江光珀智能科技有限公司 一种多传感器融合的机器人定位方法、系统及装置

Also Published As

Publication number Publication date
CN113253297B (zh) 2021-09-17

Similar Documents

Publication Publication Date Title
CN113253297B (zh) 融合激光雷达和深度相机的地图构建方法及装置
KR102292277B1 (ko) 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정
JP2022019642A (ja) マルチセンサ融合に基づく測位方法及び装置
CN102042835B (zh) 自主式水下机器人组合导航系统
CN109300143B (zh) 运动向量场的确定方法、装置、设备、存储介质和车辆
CN113819914A (zh) 一种地图构建方法及装置
Dill et al. Seamless indoor-outdoor navigation for unmanned multi-sensor aerial platforms
CN111316285A (zh) 物体检测方法、电子设备与计算机存储介质
CN112596071A (zh) 无人机自主定位方法、装置及无人机
CN112379681A (zh) 无人机避障飞行方法、装置及无人机
CN112378397A (zh) 无人机跟踪目标的方法、装置及无人机
Liu A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure
Zhang et al. Online ground multitarget geolocation based on 3-D map construction using a UAV platform
CN115585818A (zh) 一种地图构建方法、装置、电子设备及存储介质
CN115540850A (zh) 一种激光雷达与加速度传感器结合的无人车建图方法
CN111664842A (zh) 一种无人扫地机的即时定位与地图构建系统
CN108646760B (zh) 基于单目视觉移动机器人目标跟踪及平台控制系统和方法
CN112380933B (zh) 无人机识别目标的方法、装置及无人机
Hu et al. A small and lightweight autonomous laser mapping system without GPS
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
Zahran et al. Enhancement of real-time scan matching for uav indoor navigation using vehicle model
Wang et al. Micro aerial vehicle navigation with visual-inertial integration aided by structured light
CN111197986A (zh) 一种无人机三维路径实时预警避障方法
CN113403942B (zh) 一种基于标签辅助的桥梁检测无人机视觉导航方法
Serranoa et al. Seamless indoor-outdoor navigation for unmanned multi-sensor aerial platforms

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant