基于2D旋转激光的室内3D-SLAM

罗 磊 谈 彬 姚 剑 李 礼

1武汉大学遥感信息工程学院，湖北 武汉，430079

同步定位与构图（simultaneous location and mapping，SLAM）是机器人利用传感器感知周围环境，同时构建周围地图和确定自身位置的技术，广泛应用于智能家居、会场服务、快递物流、VR、AR、文物保护等领域［1］。激光SLAM是利用激光传感器作为数据源的SLAM系统，具有精度高、测距远、不受光照影响、直接获取坐标等特点，可快速进行场景识别定位和构建高精度环境地图［2］。对于室内激光SLAM，2D-SLAM系统相对成熟，出现了一些经典算法，如：Gmapping算法［3］、Hectormap算法［4］、Cartographer算法［5］等。但由于2D地图缺乏3D信息表达，不能满足楼梯、不同楼层等场景表达需求，需要一个3D激光SLAM解决方案。

Trevor等［6］利用2D激光传感器和RGBD相机组合采集场景点云数据，在2D-SLAM基础上结合平面约束进行3D-SLAM，但由于RGBD相机测距短，仅适用小尺度场景。Geneva等［7］利用多线激光传感器和惯性测量单元（inertial measurement unit，IMU）提出了一种无歧义平面表达方式的平面SLAM系统；文献［8，9］提出的室内背包系统，采用多线激光与惯导组合的背包式设备，可快速进行数据获取。这两种解决方案由于采用价格昂贵的多线激光传感器，在很多应用场景中存在局限性，因此一些学者开始研究使用低成本的2D激光传感器进行3D-SLAM的研究。经典激光SLAM算法LOAM（LiDAR odometry and mapping）［10］，采用IMU预积分，与旋转2D激光传感器松耦合，提取场景平面点和角点作为特征进行室内外SLAM。但在室内场景中，LOAM较依赖IMU，存在对较大变形敏感、易跟踪丢失的问题。本文探索仅使用一个2D激光传感器，不借助其他传感器，进行稳定鲁棒的室内3DSLAM。相比于3D传感器，采集的数据少，场景采集频率低，帧内易存在较大变形。

1 低成本室内3D-SLAM系统

数据采集硬件是由电机带动2D激光传感器绕前向轴旋转的低成本数据采集装置，如图1所示。2D激光传感器采用Hokuyo UTM-30LX，视场角达270°，点位分辨率0.25°，一条扫描线可获取1 081个场景点，采集频率40 Hz。电机绕前向轴以180°/s的速度带动激光传感器旋转，每秒钟40条扫描线（称为一帧）可覆盖整个3D场景。

图1 硬件设备示意图Fig.1 Hardware Equipment Diagram

基于此采集设备，本文提出了一种利用平面和面交线特征鲁棒抗形变的室内3D-SLAM算法，流程如图2所示，包括特征提取、特征匹配、位姿优化、地图表达等部分。

图2 本文算法流程图Fig.2 Flow Chart of Algorithm

1.1 特征提取与表达

在室内场景中，广泛存在大量平面结构，如墙面、地面、屋顶等，可作为特征约束。但当遇到长走廊等易退化的场景，平面约束往往不足，可通过面交线进行有效场景约束，如门框边线、柱子角线等。本文中采用有界平面和相邻平面相交线段（面交线）作为特征，具有较强的结构和语义信息，能够稳定有效表达场景特征，一定程度上克服场景约束不足、帧内变形大的情况，如图3所示。

图3 特征提取示意图Fig.3 Feature Extraction Diagram

对于一条扫描线，首先利用种子点增长算法快速提取直线段；然后计算相邻直线交点作为角点。利用扫描线间的相邻关系，直线段区域增长，快速生成场景平面，得到平面特征［11，12］。该平面提取方法依据扫描线特有邻接结构，可快速提取场景平面，满足SLAM系统实时性要求。平面特征表达为P=[p，n，E]，其中p表示平面质点，n表示指向背离激光传感器方向的平面法向量，E表示平面边界点，构建有边界的平面特征。

平面特征提取后，可利用角点快速提取面交线特征。在一条扫描线中，利用构成角点的两条相邻直线所属平面描述角点属性，即该角点与那些平面相连接。将具有相同属性的角点进行直线拟合即可得到面交线特征，表示为：L=[pstart，pend，iP1，iP2]，其中pstart，pend表示线段两端点，iP1，iP2索引面交线连接的平面。该角点提取方式相较于LOAM，具有更强的结构和语义信息，可有效表征场景结构骨架信息，有利于特征匹配和单帧形变去除。

1.2 特征匹配

平面特征与面交线特征匹配采用先剔除错误匹配，再挑选最优匹配的策略。对于平面特征匹配，记当前平面为Pi，地图平面为质心到的距离设为；Pi与的法向量夹角为；Pi与的面积和Pi投影到的重叠面积分别表示为Si、Sj和Sij，则两平面的重叠率。则平面匹配策略可表示为：

在满足后3个条件的前提下，选择代价最小的匹配作为最终匹配。其中，依据经验设为0.5m，设为π/6，设为0.4。

面交线特征匹配策略与平面匹配类似。记当前面交线为L i，地图面交线特征为质心到的距离设为；L i与的法向量夹角；设面交线长度为M，则两面交线重叠率为；面交线对应平面的法向量之和单位化后表示为N LP，则用表征面交线的面朝向相似性。则面交线匹配策略可表示为：

式中，依据经验设为0.8 m；和设为π/5；设为0.1。

本文中的特征表达具有较强的结构信息，不同结构间差异大，可有效减少错误匹配，即使单帧内有较大变形，仍可以进行稳健的特征匹配。

1.3 分级位姿优化

本文位姿优化采用由粗到精，逐步优化的分级位姿优化策略，可在保证精度的前提下，减少计算量，保证算法的实时性。分级位姿优化策略分为3部分，分别为单帧粗略估计运动变形、帧内线性位姿优化、地图B样条精细优化。

由于数据采集频率为1 Hz，在采集数据时，平移速度不超过0.7 m/s、旋转不超过40°/s，因此单帧内可能具有较大变形。首先将局部地图转换到当前帧坐标系下，通过提取当前帧后0.5 s的角点特征（不超过100个）与局部地图角点快速ICP（iterative closest point）配准［13］，即可获得当前帧的粗略变形，纠正帧内较大形变。

粗略纠正运动变形后，利用当前帧特征与局部地图的匹配关系，在当前帧内部采用匀速运动模型进行位姿优化。每条扫描线位姿可表示为：

式中，Tstart、Tend是李代数表达的帧内首尾位姿；⊕表示位姿变换操作；t∈[0，1)表示当前扫描线在帧内的时间。

当局部地图积累一些线性优化帧后，将局部地图内部采用3次B样条进行光滑精确位姿估计。为平衡精度和优化效率，间隔0.75 s设置B样条控制点，以0.2 Hz优化局部地图，可获得光滑连续轨迹，优化局部地图精度，进而减少漂移，提升算法性能。

匀速假设和三次B样条位姿优化能量方程同为：

式中，d(a，b)表示a到b的距离；p s表示平面抽稀点；p m表示面交线两端点。

1.4 地图表达与构建

激光SLAM常规地图表达形式有格网地图、点云地图等，存在表达冗余、无结构邻接信息等问题。本文结合自身特征描述特点，提出了一种结构化场景地图表达形式。结构化场景地图是指由平面和面交线特征及其邻接关系构建的场景骨架地图，可以简洁有效地表达场景结构，具有很强的结构和语义信息，可方便快速进行特征关联和地图更新，适用于室内场景。

局部地图采用宽度为8帧的滑动窗口保存环境地图，通过结构化特征地图表达，可有效简洁表达局部环境［13］。并且平面和面交线等具有全局特性，可有效降低位姿漂移现象。

2 实验分析

2.1 定性对比实验

选取LOAM作为对比试验，去除LOAM中的IMU预积分部分，便可与本文算法形成对比。本文采集的一栋写字楼1～5楼的楼梯数据，该场景数据具有较大旋转变形。具体实验结果如图4所示。

图4 定性对比实验结果Fig.4 Results of Qualitative Comparison Experiment

图4（a）所示为LOAM重建结果，在楼梯转角平台处没有有效去除旋转变形，地图变形明显。由于LOAM角点提取在室内环境中不鲁棒，且较依赖IMU提供初始位姿，对较大的旋转变形不能有效去除。图4（b）所示为本文算法结果，地图重建完好，结构清晰，表明算法具有较强的抗形变能力，可以稳定进行定位与构图。

2.2 定量消融实验

本部分验证面交线结构约束和多级位姿优化的有效性。实验数据仍采用楼梯数据。由于没有绝对位姿真值，本文对比指标参考Razlaw等［14］提出的评价点云地图的平均地图熵（mean map entropy，MME）和 平 均 平 面 变 量（mean plane variance，MPV），以及采用平面点云到对应平面特征距离的均方根误差（root mean square error，RMSE）作为定量指标，这3项指标越小，地图质量越好，定位精度越高。MME和MPV计算公式为：

式中，Q表示点云总数；qk表示计算点；Σ(qk)表示qk邻域内点云的协方差；v(qk)表示在qk邻域内点到拟合平面的上四分位距离。

实验结果如表1所示，由表1可知，面交线作为额外约束，与平面约束相互提升，可提升地图质量。尤其在平面易退化场景，面交线约束可有效减弱退化现象，保证定位精度。通过B样条优化，可有效提升定位精度，优化地图质量，验证了多级位姿优化的有效性。

表1 定量消融对比实验结果Tab.1 Results of Quantitative Ablation Comparison Experimental

2.3 多场景数据集验证

为验证本文方法在不同室内场景进行SLAM的有效性和鲁棒性，本文采集了9个不同尺度功能的数据构建室内场景数据集，包括多层楼梯、长厅、回环走廊、教学楼、图书馆、写字楼、办公楼、地下车库、居民住宅等多种场景，算法构建场景地图如图5所示。每个数据的轨迹长度、帧数及评价结果如表2所示。评价指标增加平移误差和旋转误差，用于评价算法的抗漂移能力，误差越小抗漂移能力越强。数据集中部分有闭环结构，但由于无绝对真值位姿，采用起始3帧与最后3帧的点云地图进行ICP配准，获取闭环漂移量。

图5 室内场景数据集Fig.5 Indoor Scene Dataset

由表2和图5可知，本文算法对9组室内场景均可鲁棒地实现位姿估计和构建精确环境地图。通过双线程运行（局部地图B样条优化单独线程），可满足实时性需求。其中6组数据有闭环结构，平均相对平移误差为1.095%，平均相对旋转误差为0.086°/m，本算法具有全局性的特征表达和由粗到精的分级位姿优化策略，因而有较强的抗漂移能力。而对比算法LOAM，在数据集场景极易跟踪丢失，不能构建地图，无法通过评价指标衡量，本文算法明显优于LOAM。通过多场景证明了即使采用低成本采集硬件，本文算法仍可取得不错的效果。

表2 多场景数据集验证结果Tab.2 Results of Multi-scene Dataset Validation

3 结束语

针对当下室内激光SLAM系统存在的硬件成本高、室内场景定位构图不鲁棒的情况，本文设计实现了一种仅依靠低成本2D激光传感器的数据采集设备，基于该设备提出了一种基于平面和面交线特征的室内3D-SLAM算法，通过鲁棒的特征设计、匹配和分级位姿优化策略，可以稳定有效地进行室内定位与构图。通过对比实验和多场景数据集验证，证明本文算法的有效性、鲁棒性，说明低成本采集硬件的可行性。