一种异构传感器的地图融合构建方法

文章编号： 1006-9798（2024）03-0001-06； DOI： 10.13306/j.1006-9798.2024.03.001

摘要： 针对单一传感器建图时还原度较低，易受环境干扰及可靠性不足等问题，提出一种任务层面融合激光雷达与深度相机的建图方法。采用一种针对走廊环境的组合式对齐算法对地图进行对齐，利用基于贝叶斯推理的方法进行信息融合。为验证融合建图效果，通过搭建多样化虚拟场景进行仿真和对真实走廊环境进行地图融合实验。结果表明，经过融合的栅格地图比单一传感器创建的栅格地图还原度更高，有效提高了地图的精度，得到了更能反映真实物理环境信息的栅格地图，为后续的路径规划提供了准确可靠的环境地图。

关键词： 融合建图； 栅格地图建立； 激光雷达； 深度相机

中图分类号： TP391文献标识码： A

同步定位与地图构建技术（Simultaneous Localization and Mapping，SLAM）[1]是利用移动机器人自身携带的传感器在陌生环境中同时定位与构建周围环境地图[2-3]。目前，激光SLAM[4]与视觉SLAM[5]研究均取得较大进展，然而2种方法仍存在各自的局限性和缺点，因此多传感器融合的SLAM系统成为移动机器人的研究热点。激光与视觉融合建图按照融合层面可分为数据层面融合和任务层面融合。数据层面融合主要是将激光雷达扫描点云与相机点云通过数据处理、特征提取等方式进行匹配和对齐，达到充分利用两种传感器信息。任务层面融合过程为激光雷达和相机分别构建出场景的点云地图或栅格地图，匹配对齐后对两者构建的地图进行融合。基于自适应蒙特卡罗定位（Adaptive Monte Carlo Localization，AMCL）的方法对齐由异构传感器构建的占用栅格地图[6]；激光雷达与深度相机融合建图方法，通过贝叶斯公式更新融合地图中栅格的概率融合两者地图信息[7]。任务层面的融合在决策过程中有效的利用来自不同传感器的信息，可以在导航任务中更好的考虑环境的复杂性，并可减少算法对重复信息的处理，因此本文在以上学者的基础上主要研究任务层面上的激光与视觉融合建图。在地图对齐中使用改进的BRIEF特征作为内角点描述子，并为其添加旋转不变性。在信息融合中充分考虑两种传感器的建图特点制定新的融合规律进行地图融合，从而生成最终的真实场景地图。利用仿真和实验验证多传感器融合建图方法的可靠性和准确性。

1地图融合研究

地图融合的关键步骤是地图对齐和信息融合[8]。地图对齐的目的是使2幅地图具有相同的地图格式、尺度以及分辨率[9]，通过一些对齐算法[10-11]得出子地图之间的坐标转换，为之后的信息关联奠定基础。信息关联是在子地图对齐后，将地图之间的信息进行合并的过程，生成最终的场景地图。地图融合流程如图1所示。

1.1地图对齐

本文提出一种针对走廊环境的组合式对齐算法，输入子地图后，统一尺度及分辨率，提取Harris角点并筛选出内角点，为角点添加主方向使之具有旋转不变性，并添加描述子，通过描述子以及点对距离匹配子地图之间的角点，匹配成功后计算出子地图之间的变换矩阵。

Harris角点检测是一种点特征提取算法，核心思想是利用局部窗口在图像上移动来观察灰度值的变化，如果梯度图上的窗口内灰度变化较大，则窗口区域就存在角点。对于每个窗口，根据式（1）计算其对应的角点响应函数R，如果R大于设定阈值，则表示该窗口对应一个角点特征。

R=det（M）-k（trace（M））2（1）

其中，det（M）为矩阵M的行列式的值；k是调节响应函数敏感度的常数；trace（dyWgZILhp4zT6UhKmJgl0w==M）为矩阵对角线的和。

当地图分辨率不同时，角点响应值会发生变化，因此需要为Harris角点添加尺度不变性。通过建立多尺度空间，将每个位置特征赋予不同尺度的表示，找到能够使角点响应最显著的尺度。添加尺度不变性后的Harris二阶矩为

M=μ（x，y，σD，σI）=σ2Dg（σI）I2x（x，y，σD）IxIy（x，y，σD）IxIy（x，y，σD）I2y（x，y，σD）（2）

其中，Ix，Iy是图像在x，y方向的梯度图；σD和σI分别为微分尺度窗口与积分尺度窗口。

通过建立多尺度空间σn=σI，kσI，…knσI，在每个尺度空间下计算角点响应值，并在大于阈值的候选角点8领域内进行非极大值抑制，在不同尺度的该候选角点对应位置下进行拉普拉斯响应LoG的计算

|LoG（x，σn）|=σ2n|Ixx（x，σn）+Iyy（x，σn）|（3）

在相邻尺度空间内对比LoG值[12]

|LoG（x，σn）|>|LoG（x，σi）|， i∈｛n-1，n+1｝（4）

当满足式（4）时，保留此候选角点，直到找到不同尺度下该位置的最大LoG值，作为该角点最佳尺度。通过Harris角点检测提取图像角点，并不是所有角点都可以作为有效信息使用，只需要在各子地图中都能检测出来且相似度较高的角点，角点太多影响匹配的效率以及精度，因此需要对角点进行筛选。将处于墙角位置的角点定义为内角点，将以内角点为中心，半径为r的圆域内各栅格状态区域占比作为判断条件，对角点进行筛选。使用灰度质心法为内角点添加主方向，使之具有旋转不变性[13]。

1）以角点位置为几何中心O的图像块B中，定义图像块的矩

mpq=∑x，y∈BxpyqI（x，y）p，q={0，1}（5）

2）通过图像矩找到图像块B的质心

C=（cx，cy）=m10m00，m01m00（6）

3）连接角点位置O与质心C，得到方向向量OC，角点主方向定义为

θ=arctan 2（cy，cx）=arctan 2（m01，m10）（7）

通过以上步骤，内角点便具有了旋转的描述。对每个点计算描述子，本文使用改进的BRIEF特征作为内角点描述子。改进的BRIEF利用特征点主方向信息使描述子具有较好的旋转不变性。

在角点对匹配阶段，对子图内部的各内角点进行两两组合并计算之间的欧氏距离，然后取子图间组合点对距离差距最小的两个点对，计算其点对间的描述子距离，本文使用二进制描述子，因此采用汉明距离（Hamming Distance）作为度量，由于计算的是点对与点对的描述子距离，两点之间的先后顺序可能导致距离变化，因此对两点之间交换顺序后再计算一次汉明距离，即对每两个点对计算2次汉明距离，取其中最小值为最终描述子距离。如果点对欧式距离以及汉明距离差值在一定范围内，即视为点对匹配成功；否则将重新筛选内角点。

内角点对成功匹配后，设子图a和子图b中匹配内角点位置分别为ma=[xa，ya，1]T，mb=[xa，ya，1]T，则两子图之间的映射关系为[14]

ma=λRmb+l（8）

其中，λ为尺度变化，可由匹配点对间的距离比求出；R为旋转矩阵；l为平移矩阵。

1.2信息融合

地图对齐后得出子图之间的映射关系，需要依靠信息融合部分将子图间的数据进行结合。本文提出一种基于贝叶斯推理来融合栅格信息的方法，使融合后的地图更反映真实情况。

贝叶斯推理用于根据已知的观察结果和先验知识，计算出未知变量的后验概率分布。假设在状态空间中，t时刻的概率为p（xt），观测数据为Zt={z1，z2，…，zt}，则后验概率为

p（xt|Zt）=p（zt|xt）p（xt|Zt-1）p（Zt|Zt-1）（9）

其中，p（Zt|Zt-1）是一种规格化术语，保证概率密度函数归一化；p（zt|xt）表示测量模型的似然函数；p（xt|Zt-1）表示在t-1时刻的先验概率。设O表示激光雷达测得栅格被占据，表示栅格未被占据，E为相机观测栅格被占据，表示未被占据。根据贝叶斯定理得出后验概率为

p（E|O）=p（O|E）p（E）p（O|E）p（E）+p（O|）p（）（10）

p（E|）=p（|E）p（E）p（|E）p（E）+p（|）p（）（11）

其中，P（E）表示先验概率；p（O|E）表示观测模型，p（）=1-p（E），融合后的栅格概率为

33FsvpDhazyNM7zMLnWu8g==

p=papbpapb+（1-pa）（1-pb）（12）

其中，pa，pb分别表示激光雷达与深度相机所建地图中栅格处于当前状态的先验概率；p表示融合后的栅格概率。通过贝叶斯推理融合子图数据，会产生除pfree、pocc、pun之外其它的概率栅格，这些额外的概率栅格可能会造成地图信息表达不准确，为了消除这种影响，本文将额外的概率栅格以最接近的状态表示出来，保证融合后的栅格种类与融合前的相同，使融合后的地图表达的更为清晰准确

pf=argp′ minp′-pp′，p′∈{pfree，pocc，pun}（13）

其中，pf为最终的栅格概率;p为通过贝叶斯推理融合后的栅格概率。

考虑到2种传感器的建图特点，本文制定了一种融合规则，见表1，其中大部分规则与改进后的贝叶斯推理融合结果相同，个别规则（如激光雷达栅格概率为未知时，融合结果为未知）通过融合结果与真实场景对比后进行的修正，使其符合实际情况。

2地图融合仿真与实验验证

2.1仿真验证

为了检验地图融合方法的有效性和准确性，针对室内走廊环境，分别对Y型和Z型走廊做了仿真场景，如图2所示，每种场景分别构建了激光栅格地图与视觉栅格地图，仿真结果如图3和图4所示，其中图3和图4中a中左侧为视觉栅格地图，右侧为激光栅格地图。

由如图3可知，仿真环境中，右下部分设置了一个激光雷达扫描平面范围外的障碍物，从图3 a中可以看到单线激光雷达无法观测到扫描平面外的障碍物信息，而在视觉栅格地图，相机视野范围内不同平面的障碍物都可以通过八叉树投影成栅格地图信息，因此垂直地面方向的障碍物建立比较完善。在匹配融合后的地图中，如图3 b，场景中不同高度的障碍物都被完善起来，更好提高了地图还原度。

Z型走廊仿真如图4a所示，当激光栅格地图与视觉栅格地图存在着角度差时，通过为BRIEF描述子添加旋转不变性之后，使内角点仍有较好的匹配效果，融合后的地图完善了各处细节。

2.2实验验证

为验证本文的融合建图效果，搭建移动机器实验平台并部署相关软件，选用办公楼一处走廊为实验场景，构建出激光栅格地图与视觉栅格地图进行地图融合验证。

2.2.1实验平台及环境

实验选用一维弦的Tyran移动机器人作为硬件平台，选用RPLIDAR A2作为使用的激光雷达传感器、RealSense D435深度相机作为移动机器人的视觉传感器。上位机操作系统为Ubuntu 1604 LTS，机器人操作系统ROS为Kinect版本。

2.2.2地图构建实验

为验证激光与视觉融合建图在实际场景中的效果，选择楼宇内一段走廊作为实验场景，实验走廊环境如图5，图5b在走廊起始的拐角处放置学习桌作为障碍物，移动机器人无法从中间穿过。

VSLAM系统在走廊环境中所构建的稠密地图和八叉树地图如图6所示。从图6a可以看出，稠密点云构建了走廊两侧的门、地面环境等周围的物体信息，图6b为构建的实验场景八叉树地图，反映了三维空间中的占据状态。

将八叉树地图投影得到二维栅格地图如图7所示。由图7可以看出，视觉成功提取到学习桌的特征，在地图中的位置与在真实场景中的位置基本吻合，回环效果较好。由于相机视角范围限制，移动机器人在起始位置，即图中走廊末端部分的点云信息没有充分获取，导致地图部分缺失。激光栅格地图构建效果如图8所示。由于两桌腿之间的连接梁低于雷达扫描平面，无法获取桌腿连接梁的点云信息，导致学习桌的占据信息未在地图中体现。

2.2.3场景地图融合实验

单独传感器所构建的栅格地图虽然能大致反映出真实场景中障碍物的占据关系，但仍然存在一些细节缺失。将2张栅格地图信息通过本文融合方法进行融合，结果如图9所示。

由图9可以看出，学习桌及右侧走廊尽头都体现在地图中，通过融合弥补了各自的不足，使栅格地图更符合场景的实际情况。

3结束语

本文提出一种在任务层面的激光与视觉融合建图方法。在统一地图的尺度及分辨率后，采用Harris角点检测并进行筛选，对筛选出的内角点添加旋转不变性匹配和转换，最后通过概率栅格融合的方法得到更符合环境的二维栅格地图。相比于采用激光和视觉进行实时建图的数据层面融合的方法，本文采用的任务层面融合的建图方法具有更高的地图还原度和精度，验证了融合构建地图方法的有效性。但此算法是针对于类似走廊的环境，在场景应用中有一定的局限性，后续将对算法应用场景和融合后栅格地图精度展开进一步优化。

参考文献：

［1］LI J， ZHANG X， LI J， et al. Building and optimization of 3D semantic map based on Lidar and camera fusion[J]. Neurocomputing， 2020， 409： 394-407.

［2］ZHANG T， ZHANG H， LI Y， et al. Flowfusion： Dynamic dense rgbd slam based on optical flow[C]∥2020 IEEE international conference on robotics and automation （ICRA）. Paris： IEEE， 2020： 7322-7328.

［3］TAKETOMI T， UCHIYAMA H， IKEDA S. Visual SLAM algorithms： A survey from 2010 to 2016[J]. IPSJ transactions on computer vision and applications， 2017， 9： 1-11.

［4］LI H， ZHOU Y， DONG Y， et al. Research on navigation algorithm of ros robot based on laser SLAM[J]. World Scientific Research Journal， 2022， 8（5）： 581-584.

［5］张大伟， 苏帅. 自主移动机器人视觉 SLAM 技术研究[J]. 郑州大学学报（理学版）， 2021， 53（1）： 1-8.

［6］ZHANG B， LIU J， CHEN H. Amcl based map fusion for multirobot slam with heterogenous sensors[C]∥2013 IEEE International Conference on Information and Automation （ICIA）. Yinchuan： IEEE， 2013： 822-827.

［7］曾键， 夏益民， 蔡锦炜， 等. 一种融合激光与视觉传感器的栅格地图构建方法[J]. 工业控制计算机， 2020， 33（9）： 92-94，111.

［8］BOSSE M， ZLOT R. Map matching and data association for largescale twodimensional laser scanbased slam[J]. The International Journal of Robotics Research， 2008， 27（6）： 667-691.

［9］SUNIL S， MOZAFFARI S， SINGH R， et al. FeatureBased Occupancy MapMerging for Collaborative SLAM[J]. Sensors， 2023， 23（6）： 3114-3135.

［10］KAKUMA D， TSUICHIHARA S， RICARDEZ G A G， et al. Alignment of occupancy grid and floor maps using graph matching[C]∥2017 IEEE 11th international conference on semantic computing （ICSC）. San Diego： IEEE， 2017： 57-60.

［11］MA L， ZHU J， ZHU L， et al. Merging grid maps of different resolutions by scaling registration[J]. Robotica， 2016， 34（11）： 2516-2531.

［12］MIKOLAJCZYK K， SCHMID C. Scale & affine invariant interest point detectors[J]. International journal of computer vision， 2004， 60： 63-86.

［13］ROSIN P L. Measuring corner properties[J]. Computer Vision and Image Understanding， 1999， 73（2）： 291-307.

［14］YU S， FU C， GOSTAR A K， et al. A review on mapmerging methods for typical map types in multiplegroundrobot SLAM solutions[J]. Sensors， 2020， 20（23）： 6988-7008.

A Map Fusion Construction Method for Heterogeneous Sensors

MENG Xiaocheng， WANG Xianlun， CAO Tongkun， ZHOU Yusong

（College of Mechanical and Electrical Engineering， Qingdao University of Science and Technology， Qingdao 266061， China）

Abstract：

A task level mapping method that integrates Laser radar and depth camera is proposed to address the issues of low fidelity， susceptibility to environmental interference， and insufficient reliability when using a single sensor for mapping. Firstly， a combination based algorithm for corridor environment is adopted to align the map， and then a Bayesian inference based method is used for information fusion. To verify the effectiveness of fusion mapping， simulations are conducted by building diverse virtual scenes and map fusion experiments are carried out on real corridor environments. The results show that the fused grid map has a higher degree of restoration than the grid map created by a single sensor， effectively improving the accuracy of the map and obtaining a grid map that better reflects real physical environment information. This provides accurate and reliable environmental maps for subsequent path planning.

Keywords：integrated mapping; raster map establishment; laser radar; depth camera

收稿日期： 2024-06-27； 修回日期： 2024-08-29

基金项目： 国家自然科学基金资助项目（51105213）

第一作者： 孟小程（2000-），男，硕士，主要研究方向为机器人及智能制造技术。

通信作者： 王宪伦（1978-），男，博士，副教授，主要研究方向为机器人及智能制造技术、机械系统智能化设计及虚拟设计、机械加工过程的计算机控制。Email： xlwang@126.com