基于高效回环检测的大场景下的SLAM 算法

2024-03-11娜程

沈阳理工大学学报 2024年2期

杨娜程磊

(沈阳理工大学信息科学与工程学院，沈阳 110159)

同步定位与地图构建是智能机器人在未知环境中进行避障和导航等任务的先决条件之一[1]。激光雷达同步定位与地图构建(SLAM)是利用激光雷达传感器进行自身状态估计和构建周围环境地图的一项技术。相较于视觉[2]，激光雷达受光照因素影响较小，夜间依然可以正常工作，凭借其测距精度高、视角范围广等优势[3－4]，激光SLAM 在无人自主导航、自主驾驶等领域取得很大进展。因此，本文选用基于激光雷达SLAM 方案。

为降低雷达里程计和建图的累计误差，引入回环检测技术，即通过计算当前帧位姿与历史帧位姿间的变换，判断移动机器人是否经过同一位置，将其结果作为约束加入到全局优化中去，以达到提升全局位姿图整体精度的目的。雷达里程计位姿误差会随着时间增加而积累，在室外大场景情况下误差积累更甚，从而影响SLAM 后端建图精度和效果。

作为激光SLAM 的经典之作，LOAM 算法[5]在kitti odometry[6]门类长期霸榜，包括其衍生算法LeGO-LOAM[7]等，其主要思想是通过一个高频激光里程计进行低精度的运动估计和一个低频高精度的建图与校正里程计的估计结果相融合，生成一个高频高精度的单一运动估计。 LOAM 算法不带有回环检测，因此不可避免会引起漂移。陈文浩等[8]提出一种基于紧耦合(IMU)的LeGOLOAM 算法，改善了在低分辨率的激光雷达设备上运动畸变和点云数据稀疏的问题，能够有效抑制重力矢量漂移问题。庞帆等[9]给出了一种利用激光雷达惯导耦合的里程计与建图方法，算法对数据预处理模块、特征提取模块和里程计建图模块的改进，改善了场景中动态物体移动造成里程计精度降低、算法鲁棒性降低的问题。郑川川等[10]提出一种基于图优化的激光管道紧耦合的SLAM 算法，后端采用图优化的方法融合IMU 预积分、激光里程计和回环检测信息完成地图构建，可以一定程度抑制运动估计漂移。上述所提方法都没有改变算法中的回环检测策略，若偏移较大，便无法修正，且没有重定位功能。

Kim 等[11]提出Scan Context 算法，将点云信息压缩，只利用点云最大高度信息。周治国等[12]提出一种基于强度扫描上下文回环检测的算法，采用强度扫描上下文作为全局描述符进行回环检测以减少漂移。耿丽杰等[13]提出一种建图方法，将Sacn Context 回环检测策略与点云配准策略NDT-ICP 融合以减少地图中的误匹配。基于扫描上下文的回环策略均需要进行暴力匹配实现旋转不变性。 Wang 等[14]提出Iris 回环检测算法，充分利用点云大部分信息，具有旋转不变性，避免暴力搜索，节省计算资源。

本文在LeGO-LOAM 基础上改进回环策略，选用关键特征(Ring-key) 向量构建KD 树查找相似候选帧，计算两个二值特征图像的汉明距离，得到两帧点云的相似度来判断回环帧是否完成回环检测，避免暴力匹配检测点云旋转问题，有效解决回环帧漏检和错检情况，减小里程计的累计误差，提高建图精度。

1 高效回环检测的激光SLAM 算法

1.1 系统概况

系统整体框图如图1 所示，首先对输入点云进行处理，分离出地面点云，得到有效点云，将有效点云进行特征提取，得到面特征和边缘特征，将特征点云传入激光建图模块建立全局点云地图，同时传入回环检测模块检测回环，最后通过变换集成模块融合计算结果，输出最终姿态估计。

图1 系统整体框图Fig.1 System overview

1.2 LeGO-LOAM 算法

整个算法可以分为五个模块，其中点云分割模块对获取到的点云实行聚类分割操作，将地面点云和异常点云分离开。然后将分割后的点云传到特征提取模块，得到面点点云和边缘点点云。将分割后的点云传入激光里程计模块进行位姿变换。将变换后点云传入激光建图模块建立全局点云地图，最后通过变换集成模块融合激光里程计模块和激光建图模块的计算结果，输出最终的姿态估计。

1.2.1 点云分割

本模块主要流程为:先进行地面提取，再对剩下点云进行聚类分割，为提高效率和可靠性，分割时会忽略少于30 个点的集群。去除地面过程如下:对激光雷达线束序号中小于31 的进行地面检测，通过图2 中A点和相邻点B与AC的夹角α的值判断是否为地面，其中AC平行于激光雷达坐标系x轴，若α小于10°，则认为是地面点，否则不是地面点。去除地面之后，通过广度优先搜索递归进行查找，从[0，0]点开始，遍历其前后左右四个点，并分别与四个点对比，若其相对角度超过60°，那么便划分为同一个点云簇。

图2 地面点判断Fig.2 Ground point judgment

1.2.2 点云特征提取

点云特征提取的目的是进行点云配准，进而得出当前位姿。点云首先进行运动补偿，然后计算每个点曲率，再按照曲率排序，若该点曲率大于给定阈值，则选为边缘点，否则选为面点，曲率计算公式为

式中:S是同一线激光连续点的集合；i和j分别表示集合S中的点；rj表示S中不等于i的点j的深度；ri表示点i的深度。本文取｜S｜等于10。

1.2.3 点云特征匹配

特征匹配主要根据特征提取出的边缘点集合和面点集合，得到点云的线特征和面特征，根据最小二乘法，得出前后两帧的坐标转换。空间中一个点的坐标可以通过平移和旋转来转换到另一个坐标，则前一帧中的点可以通过公式(2)转换到当前帧，假设以空间中点的欧氏距离表示匹配好坏，则可以把问题转换为公式(3)。

式中:k表示一个扫描周期；表示点i在k＋1周期的雷达坐标系L中坐标；表示在世界坐标系W中的坐标；表示特征点的旋转平移变换；R表示旋转矩阵；t表示平移向量；dε为线到线距离；fε表示线特征函数。

面特征的匹配与线特征匹配相似，面特征匹配就是找到一个转换，使得当前帧所有点到前一帧面的距离平方差最小，如公式(5)所示。根据公式(6)计算点到面的距离dϑ，其中为当前帧中的三个面上的m点的位置，fϑ表示面特征函数。

1.2.4 激光建图

LeGO-LOAM 以一个较低的频率对当前帧扫描到的点云进行线特征提取和面特征提取，将其与地图中的点云进行匹配，并优化位姿变换。

1.3 回环检测算法

1.3.1 构建描述子

首先将点云转换为鸟瞰图，如图3 所示，保留一个Q×Q大小的正方形感应区，感应区中心为激光雷达位置。以感应区为单位，将点云离散为一个径向80、角向360 的网格，使用八位二进制代码对同一个网格内的所有点进行编码。将每个网格中的点按照高度排序再线性离散化为8 个网格，有点的网格取值为1，无点的网格取值为0，最后获得一个八位二进制编码(code)，再仿照虹膜识别算法[14]将鸟瞰图扩展为图像条，如图4所示。

图3 点云离散化形成鸟瞰图Fig.3 An aerial view of discretization of point clouds

图4 雷达虹膜特征提取图Fig.4 Iris feature extraction view

利用四个一维LoG-Garbo 滤波器对图像每一行进行卷积操作，得到每个滤波器的实部和虚部，其中一维LoG-Garbo 滤波器的频率响应G(f)计算式为

式中:f0表示滤波器中心频率；f指的是传感器接收到的激光脉冲信号的频率；σ表示滤波器带宽，σ/f0需要保持常数值。

最后进行阈值运算，把四个一维LoG-Garbo滤波器的卷积响应二值化，再叠加到每个雷达虹膜图像的二值特征图中，得到点云的二值特征图。

1.3.2 构建KD-tree 加速匹配

通过激光雷达扫描数据生成的环路闭合检测的Ring-Key 构建KD-tree 查找相似帧，根据距离原点由近及远对矩阵的每一行向量pe通过φ(pe)函数使用L0范数编码得到一个Np维向量表示环的占用率，再进行匹配计算，以起到数据降维和搜索加速的作用。

式中:M为一维数组Ring-Key；φ(pe)为数组中每个元素第e个Ring-Key 的编码值；pe排布为距离原点由近及远，‖pe‖0表示pe中的非零向量；No表示在构建环形描述子时使用的尺度个数。

1.3.3 闭环检测修正

每个点云生成一个二值特征图，并将所有关键帧获取的二值特征图存于数据库，比较当前关键帧和历史关键帧的二进制特征图之间不同值的个数，即计算两个二进制图之间的汉明距离，如果计算结果小于阈值，则认为检测到回环，完成算法的粗配准部分。

检测到回环后，会以当前帧点云为源点云，KD-tree 中的回环帧点云与目标点云进行匹配，得到配准后的旋转矩阵R∗和平移向量t∗，即位姿变换矩阵，完成算法的精配准部分，如公式(10)所示。

式中:P1为源点云集合；P2为目标点云集合；表示目标点云中的第i个点；表示源点云中的第i个点；｜P1｜为源点云中点的个数。

回环检测完成后，将得到的位姿变换矩阵作为新的约束传入因子图优化框架中，进一步更新传感器的位姿估计，消除里程计累计误差和漂移。

2 仿真实验结果与分析

本文使用轨迹评估工具evo 进行轨迹评估。evo 是一款用于视觉里程计和SLAM 问题的轨迹评估工具，其核心功能是绘制相机的轨迹，评估估计轨迹和真实值之间的误差，支持多种数据集的轨迹格式(TUM、KITTI、EuRoC MAV)，本文使用KITTI 轨迹格式进行评估。

2.1 使用KITTI 数据集验证分析

本文使用KITTI 数据集验证算法在大场景下的定位精度、回环检测精度及效率和建图效果。KITTI 数据集是由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办的用于自动驾驶领域研究的数据集。该信息收集系统配有两个灰度相机、两个彩色摄像机、四个光学相机、一个Velodyne 64 线激光雷达，另外还有一套GPS 导航设备。本次实验平台为具有16 GB 运行内存、AMD Ryzen7-5800H CPU 的个人电脑，操作系统为Ubuntu18.04LTS，ROS 版本为melodic。本文采用带有环路闭合的KITTI 里程计数据，即序列00、05、08 验证算法定位精度，其中08 序列带有反向环路闭合帧。数据序列的环境、位姿数量和路程如表1 所示。

表1 数据集序列信息Table 1 Data set sequence information

图5 为三个序列A-LOAM、LeGO-LOAM、本文算法(MY-SYSTEM)与KITTI 轨迹真值(GT)俯视图轨迹对比，可以看到本文算法相对更加贴合真值轨迹。图6 为本文算法在三个数据集序列上与轨迹真值的绝对位姿误差图，图7 为本文算法在三个数据集序列上与轨迹真值的相对平移误差图。

图5 KITTI 00、05、08 序列俯视图Fig.5 Top view of KITTI sequence 00，05 and 08

图6 KITTI 00、05、08 绝对位姿误差图Fig.6 Absolute pose error diagram of KITTI 00，05 and 08

图7 KITTI 00、05、08 相对平移误差图Fig.7 Relative translation error diagram of KITTI 00，05 and 08

表2 列出各个方案在00、05 和08 三个数据集序列上的轨迹绝对位姿误差，同时考虑旋转和平移误差[15](无单位)，表中数据采用五次实验结果平均值。可见，本文算法与A-LOAM 和LeGOLOAM 算法相比，绝对位姿误差在均值、均方根和标准差都有所降低，相比较于LeGO-LOAM 算法，绝对位姿误差均值平均降低14.84%，均方根平均降低19.27%，标准差平均降低51.54%。

表2 不同方案各个序列绝对位姿误差Table 2 Absolute trajectory error of each sequence in different schemes

表3 列出各个方案在00、05 和08 三个数据集序列上的轨迹相对位姿平移误差(单位:m)。表中数据采用五次实验结果平均值。本文算法与A-LOAM 和LeGO-LOAM 算法相比，相对位姿平移误差在均值、均方根和标准差都有所降低，相比较LeGO-LOAM 算法，相对位姿平移误差均值平均降低11.12%，均方误差平均降低19.39%，标准差平均降低14.42%。