基于全局特征描述子的激光SLAM回环检测方法

doi:10.16183/j.cnki.jsjtu.2021.202

基于全局特征描述子的激光SLAM回环检测方法

韩超¹, 陈敏¹, 黄宇昊¹, 赵明辉², 杜乾坤¹, 梁庆华^,¹

1.上海交通大学机械与动力工程学院,上海 200240

2.中煤科工集团上海有限公司,上海 201100

Loop Closure Detection Method of Laser SLAM Based on Global Feature Descriptor

HAN Chao¹, CHEN Min¹, HUANG Yuhao¹, ZHAO Minghui², DU Qiankun¹, LIANG Qinhua^,¹

1. School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China

2. China Coal Technology and Engineering Group Shanghai Co., Ltd., Shanghai 201100, China

通讯作者: 梁庆华,男,副教授,电话(Tel.):021-34206354;E-mail:qhliang@sjtu.edu.cn.

责任编辑: 石易文

收稿日期: 2021-06-11

Received: 2021-06-11

作者简介 About authors

韩超(1997-),男,浙江省宁波市人,硕士生,主要从事井下无人机定位技术研究.

摘要

针对井下环境中设备巡检时定位误差随时间不断累积的问题,提出一种适用于激光同步定位和建图技术的基于点云全局特征描述子的回环检测方法.该方法利用曲率计算点云中特征点的特征向量,通过特征向量与中心点坐标系的角度分布和尺度分布关系构建点云全局特征描述子;使用部分特征点进行位姿变换的计算,提高计算效率.通过仿真实验和开源数据集实验对所提算法进行验证.实验结果表明,所提算法在定位精度和实时性上提升明显,可以有效解决定位算法在长时间运行过程中的累积误差和全局一致性差的问题.

关键词： 井下巡检; 激光定位; 回环检测; 全局特征描述子

Abstract

To solve the problem that localization error of the underground inspection system continues to accumulate over time, a loop closure detection algorithm based on point cloud global feature descriptor is proposed, which is suitable for laser simultaneous localization and mapping (SLAM). The feature vector of each point in point cloud is calculated by curvature, then the global feature descriptor of point cloud is constructed based on the angle distribution and scale distribution relationship between the feature vector and center point coordinate system. In addition, the pose transformation of two similar frames is calculated by feature point registration to improve computing efficiency. The proposed algorithm is verified by simulation experiments and open-source data experiments. The experimental results show that the proposed algorithm has a significant improvement in localization accuracy and real-time performance, which can effectively solve the problems of increased cumulative error and poor global consistency of the localization algorithm during long-term inspections.

Keywords： underground inspection; laser localization; loop closure detection; global feature descriptor

PDF (5130KB) 元数据多维度评价相关文章导出 EndNote| Ris| Bibtex 收藏本文

本文引用格式

韩超, 陈敏, 黄宇昊, 赵明辉, 杜乾坤, 梁庆华. 基于全局特征描述子的激光SLAM回环检测方法[J]. 上海交通大学学报, 2022, 56(10): 1379-1387 doi:10.16183/j.cnki.jsjtu.2021.202

HAN Chao, CHEN Min, HUANG Yuhao, ZHAO Minghui, DU Qiankun, LIANG Qinhua. Loop Closure Detection Method of Laser SLAM Based on Global Feature Descriptor[J]. Journal of Shanghai Jiaotong University, 2022, 56(10): 1379-1387 doi:10.16183/j.cnki.jsjtu.2021.202

同步定位和建图(SLAM)是实现机器人在未知环境下的定位和移动的重要技术方法^[1].定位精度是井下巡检的核心指标,高精度的定位算法是巡检过程中导航和避障的基础.但井下环境复杂,具有低照度、弱纹理、图像特征难以识别的特点^[2],给基于视觉的SLAM算法带来了极大的困难.而激光SLAM算法测量距离远、精度高,利用环境的结构特征进行定位^[3],在井下环境中更具应用前景^[4-5].

目前,SLAM算法通过估计的相邻帧之间的位姿变换不断叠加进行位姿信息计算,随着巡检时间的增加,每次位姿估计中的偏差不断累积形成较大的累积误差,制约了算法的定位精度.

利用图像语义信息丰富的特点,视觉SLAM算法如ORB-SLAM^[6]、VINS^[7]等,多使用基于词袋模型^[8]的回环检测方法,利用预先训练的视觉特征单词构建词典,将图像中的特征描述为一个单词,图像中单词种类和数量构成图像的词袋向量,通过词袋向量之间的相似度进行回环检测.

但激光雷达分辨率低,传递信息不如图像丰富,基于词袋模型的回环检测方法难以应用.在LeGO-LOAM^[9]和LIO-SAM^[10]等激光SLAM方案中使用基于里程计的回环检测方法,通过当前位置信息判断是否可能存在回环,如当前位置一定范围内,存在历史轨迹点,则与对应关键帧进行匹配, 并通过迭代最近点(ICP)配准方法进行位姿计算,实现累计误差的校正.此类方法效果受巡检距离限制,当累积误差超过一定范围时出现无法检测到回环或错误检测的问题,仍未解决累积误差不断增长的问题,受传感器精度影响大,且ICP方法对全部点云进行配准计算,计算时间较长.

文献[11-12]通过正态分布变换(NDT)或特征直方图检测闭环,通过距离信息判定当前位置阈值与历史轨迹的一致性,使用NDT方法或特征直方图方法进行位姿估计与二次回环检测,有效提升了匹配准确率,但召回率以及回环检测的实时性无法适用于井下无人机的定位需求.因此,使用回环检测方法解决井下无人机巡检保持长时间高精度定位的问题,需避免对里程计的依赖,使用场景相似特征进行回环检测,同时提高计算效率,保证实时性.

本文提出一种适用于激光SLAM的点云全局特征描述子回环检测方案,不依赖于位姿信息,利用矿井通道的几何结构特征进行匹配,通过计算与误差较小的历史场景之间的位姿变换,消除累积误差,并完成了完整SLAM算法框架的搭建,实时性好.该框架里程计部分基于激光里程计和建图(LOAM)^[13]算法计算不带回环的位姿信息,回环检测部分基于曲率划分的特征点与点云质心之间的角度关系和尺度关系进行相似度检测,使用平面点和边角点配准方法优化计算速度,并由因子图优化方法保证全局一致性.最终, 通过仿真和开源数据集实验验证了所提算法的有效性,本文研究可为激光SLAM的全局特征提取及回环算法提供理论基础.

1 基于全局特征描述子的回环检测

设计的定位系统整体分为2个模块:里程计模块和回环检测模块.

里程计模块参考LOAM算法^[13]实现, 输出里程计位姿信息,并根据点云三维曲率完成了特征点云的筛选,将边角特征点云P_edg和平面特征点云P_pla传递给回环检测模块.

回环检测模块中进行回环识别和位姿的更新.为实现对井下通道的几何描述,在激光雷达相对坐标系下,进行特征向量提取和中心点计算,基于点云的角度分布和尺度分布关系,构建点云的全局特征描述子;以多维描述子之间的相关性作为点云相似度的评价指标;使用特征点配准方法以改进全局配准计算耗时过大的问题;利用检测到的相似场景建立回环约束,通过因子图优化消除累积误差并保证轨迹和地图的全局一致性.回环检测模块是本文研究的重点,算法流程框图如图1所示.其中:n_edg和n_pla分别为边角特征点云和平面特征点云对应的特征向量;H_k为关键帧k的全局描述子向量;p_edg,_k和p_pla,_k分别为关键帧k的边角特征和平面特征.

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1 基于全局特征描述子的回环检测算法流程

Fig.1 Flow chart of loop closure detection algorithm flow based on global feature descriptor

1.1 特征向量提取

使用全局点云中具有代表性信息的边角特征和平面特征进行数据处理,对里程计模块中提取的边角特征点云P_edg={p_edg,1, p_edg,2, …, p_edg,_k, …}和平面特征点云P_pla={p_pla,1, p_pla,2, …, p_pla,_k, …}分别提取特征向量.取p_edg,_k或p_pla,_k中的任意一点p_i,使用k-维(KD)树搜索其所在点云中邻近的k个点,计算该部分点云的协方差矩阵,具体步骤如下.

步骤1 计算邻近k点的质心:

（1）

\begin{matrix} o_{c} = \frac{1}{k} \overset{k}{\sum_{i = 1}} p_{i} \end{matrix}

步骤2 计算协方差矩阵C,即质心o_c到邻近点云中所有点p_i的向量与其转置的内积:

（2）

\begin{matrix} C = \frac{1}{k} \overset{i \leq k}{\sum_{i = 1}} [p_{i} - o_{c}] \cdot [p_{i} - o_{c}]^{T} \end{matrix}

步骤3 对协方差矩阵进行奇异值分解(SVD),求特征值:

（3）

C = U \cdot Λ \cdot V^{T} = \sum λ_{j} n_{j} {n^{T}}_{j}

（4）

\begin{array}{l} Λ = d i a g (λ_{1}, λ_{2}, λ_{3}) = |\begin{array}{l} λ_{1} & 0 & 0 \\ 0 & λ_{2} & 0 \\ 0 & 0 & λ_{3} \end{array}|, \\ 且 λ_{1} < λ_{2} < λ_{3} \end{array}

式中: $λ_{j} (j = 1,2, 3)$ 为矩阵C对应的特征值;为矩阵C对应的特征向量;Λ为对角线 $λ_{i}$ 组成的对角矩阵;U,V为奇异值分解后的实正交矩阵.

步骤4 由 $|λ_{j} E - C| n = 0$ 解得对应的特征向量 $n_{1}$ , $n_{2}$ , $n_{3}$ .

其中,边角特征提取线向量为特征向量n_edg,邻近点投影至该方向上的向量方差最最小,为最大特征值λ₃对应的特征向量n₃;平面特征提取法向量为特征向量n_pla,邻近点投影至该方向上的向量方差最最大,为最小特征值λ₁对应的特征向量 ${n_{1}}^{[14]}$ .

1.2 特征描述和相似度计算

设计全局特征描述子的计算方法.通过计算全局点云P中所有点的三维坐标平均值得到当前帧的点云质心o_c,以点云P_edg和P_pla中所有点对应的特征向量的均值作为质心o_c处对应的特征向量n_c,进而计算点云的全局特征描述子,边角点云P_edg和平面点云P_pla分开计算.

遍历点云P中的所有点,确定以质心o_c为原点o的参考坐标系uvw:

（5）

\begin{matrix} u = n_{c} \end{matrix}

（6）

\begin{array}{l} v = u \times \frac{p_{t} - o_{c}}{‖ p_{t} - o_{c} ‖_{2}} \end{array}

（7）

\begin{matrix} w = u \times v \end{matrix}

式中:p_t为点云P_edg或P_pla中任意选取的一点;n_c为点云质心处的归一化特征向量;‖p_t-o_c‖₂为点p_t到质心o_c之间的欧氏距离.以p_t, o_c确定的坐标系如图2所示,其中:n_t为点p_t处归一化之后的特征向量.

图2

新窗口打开| 下载原图ZIP| 生成PPT

图2 全局点云下的uvw坐标系

Fig.2 uvw coordinate system of global point cloud

以上述计算得到的uvw坐标系为基准,计算点云特征向量相对于基准坐标系3个坐标轴的角度分布,则有:

（8）

\begin{matrix} α = v \cdot n_{t} \end{matrix}

（9）

ϕ = u \cdot \frac{p_{t} - o_{c}}{d}

（10）

θ = a r c t a n (w \cdot n_{t}, u \cdot n_{t})

（11）

d = ‖ p_{t} - o_{c} ‖_{2}

式中: d为点 $p_{t}$ , $o_{c}$ 之间的欧氏距离;α为特征向量 $n_{t}$ 与坐标轴v之间的夹角,值域为[-1,1];ϕ为 $p_{t}$ , $o_{c}$ 之间连线方向与坐标轴u之间的夹角,值域为[-1,1];θ为特征向量 $n_{t}$ 投影至wou平面时与坐标轴w之间的夹角,值域为 $[- π / 2, π / 2]$ .

利用上述公式进行计算后,α,ϕ,θ,d为非线性相关的,可以替代两点的(x,y,z,n_x,n_y,n_z) $α, ϕ, θ, d 为非线性相关的, 可以替代两点的 (x, y, z, n_{x}, n_{y}, n_{z})$ 共12维数据对点云的分布进行几何关系描述.

遍历点云P_edg和P_pla中所有点,与质心点o_c构成点对,计算对应的 $α, ϕ, θ, d 四要素 . 将 α, ϕ, θ$ 的值域区间进行等分,统计全局点云三要素在各区间内的分布情况,构成特征描述子的角度分量.

计算全局点云P中距离质心点o_c最远的位置,将最大距离均分为多个区间,统计各个点相对于质心距离在区间中的分布,以此构成特征描述子的尺度分量.

最终设计的点云全局描述特征描述子为230维向量,其中前105维为边角特征点云P_edg计算得到的角度分量α_e, ϕ_e, θ_e,中间105维数据为平面特征点云P_pla计算得到的角度分量α_p, ϕ_p, θ_p,均以35维的向量表示,最后20维数据为当前帧点云P全部点的尺度分量β_d.

本文提出的全局描述子计算方法以点云中心建立相对坐标系进行计算,具有旋转不变性和平移不变性,如图3所示.图3(a)为原始点云P及其旋转45°、平移10 m后的点云,经旋转和平移变换后,如图3(b)和3(c)所示,点云的全局特征描述子不发生变化.其中:D_H为描述子H的维度信息;V_H为对应维度的数值信息.旋转不变性和平移不变性说明,对于同一场景下,激光雷达传感器处于不同角度和位置时,仍能进行准确地进行回环检测.

图3

新窗口打开| 下载原图ZIP| 生成PPT

图3 旋转和平移不变性实验

Fig.3 Experiments of rotation and translation invariance

使用点云全局特征描述子的皮尔逊相关系数^[15]作为衡量点云相似度的标准,其计算公式如下:

（12）

\begin{array}{l} S_{H_{1}, H_{2}} = \frac{c o v (H_{1}, H_{2})}{σ_{H_{1}} σ_{H_{2}}} = \\ \frac{E [(H_{1} - μ_{H_{1}}) (H_{2} - μ_{H_{2}})]}{σ_{H_{1}} σ_{H_{2}}} = \\ \frac{E (H_{1} H_{2}) - E (H_{1}) E (H_{2})}{\sqrt{E (H_{1}^{2}) - E^{2} (H_{1})} \sqrt{E (H_{2}^{2}) - E^{2} (H_{2})}} \end{array}

式中:H₁=[h₁₁h₁₂ … h₁_i …],H₂=[h₂₁h₂₂ … h₂_i …]为归一化后的点云全局特征描述子; $μ_{H_{1}}$ 和 $μ_{H_{2}}$ 分别为H₁和H₂元素的平均值向量;E为数学期望; σ为方差; $S_{H_{1}, H_{2}}$ 为相关系数,其取值范围为[0, 1].当 $S_{H_{1}, H_{2}}$ 接近于1时,说明特征描述子为正相关关系,点云的相似度越高^[16].

1.3 位姿变换

对筛选后相似度大于阈值的当前帧点云P_cur和历史相似帧点云P_sim进行相对位姿变换的计算.相对位姿的计算使用点云的边角特征和平面特征配准方法,即使用三维点的曲率计算公式^[13]计算点云不同位置处的光滑度,以光滑度为依据提取点云的边角特征和平面特征,只对边角特征和平面特征进行点云配准,提高了计算速度.

为减少点云的误匹配概率,使用曲率估计方法对点云配准效果进行验证.当前帧边角点和平面点分别在相似帧边角特征和平面特征中搜索匹配点,对相似帧的特征进行主成分分析,计算其特征值 $λ_{1}, λ_{2}, λ_{3} (λ_{1} > λ_{2} > λ_{3}) .$ 其中,当点云为边角特征时,扫描点按照线方向分布;为平面特征时,扫描点呈现平面式分布,在垂直平面方向上的方差最小.边角特征和平面特征应分别满足如下关系式:

（13）$\left.\begin{array}{ll}\lambda_{1}>3 \lambda_{2}, & \text { 边角特征 } \\\lambda_{2}>3 \lambda_{3}, & \text { 平面特征 }\end{array}\right\}$

对于配准后的相似帧点云,构建需要求解的目标函数,使用Levenberg-Marquardt法迭代求解使目标函数最小的位姿变换,其目标函数如下:

（14）

\begin{array}{l} F = φ_{e} + φ_{p} = \\ \sum \frac{|({\tilde{r}}_{e d g (c u r, i)}^{W} - {\bar{r}}_{e d g (s i m, j)}^{W}) \times ({\tilde{r}}_{e d g (c u r, i)}^{W} - {\bar{r}}_{e d g (s i m, k)}^{W})|}{|{\bar{r}}_{e d g (s i m, j)}^{W} - {\bar{r}}_{e d g (s i m, k)}^{W}|} + \\ \sum \frac{|\begin{array}{l} ({\tilde{r}}_{p l a (c u r, i)}^{W} - {\bar{r}}_{p l a (s i m, j)}^{W}) \\ ({\bar{r}}_{p l a (s i m, j)}^{W} - {\bar{r}}_{p l a (s i m, k)}^{W}) \times ({\bar{r}}_{p l a (s i m, j)}^{W} - {\bar{r}}_{p l a (s i m, l)}^{W}) \end{array}|}{|({\bar{r}}_{p l a (s i m, j)}^{W} - {\bar{r}}_{p l a (s i m, k)}^{W}) \times ({\bar{r}}_{p l a (s i m, j)}^{W} - {\bar{r}}_{p l a (s i m, l)}^{W})|} \end{array}

（15）

\begin{matrix} {\tilde{r}}_{e d g (c u r, i)}^{W} = {\tilde{q}}_{w_{c}} \cdot r_{e d g (c u r, i)}^{L} + {\tilde{t}}_{w_{c}} \end{matrix}

（16）

\begin{matrix} {\tilde{r}}_{p l a (c u r, i)}^{W} = {\tilde{q}}_{w_{c}} \cdot r_{e d g (c u r, i)}^{L} + {\tilde{t}}_{w_{c}} \end{matrix}

式中:上标^L和^W分别为局部和世界坐标系;上划线-为测量值,~为估计值; $r_{e d g (c u r, i)}^{L}$ 为当前边角特征点云P_edg,cur中一点p_edg(cur,_i₎在激光雷达局部坐标系下的坐标; $r_{p l a (c u r, i)}^{L}$ 为当前平面特征点云P_pla,cur中一点p_pla(cur,_i₎在激光雷达局部坐标系下的坐标; ${\bar{r}}_{e d g (s i m, j)}^{W}$ , ${\bar{r}}_{e d g (s i m, k)}^{W}$ 分别为相似帧边角特征点云在世界坐标系下距离p_edg(cur,_i₎最近的两点坐标; ${\bar{r}}_{p l a (s i m, j)}^{W}$ , ${\bar{r}}_{p l a (s i m, k)}^{W}$ , ${\bar{r}}_{p l a (s i m, l)}^{W}$ 分别为相似帧平面特征在世界坐标系下距离p_pla(cur,_i₎最近的三点坐标;φ_e, φ_p分别为边角和平面特征的目标函数,其含义为配准点到线或平面的距离; ${\tilde{q}}_{w_{c}}$ 和 ${\tilde{t}}_{w_{c}}$ 为待求解变量,分别为当前坐标系到世界坐标系下的旋转四元数估计值和平移向量估计值.

边角特征和平面特征的配准如图4所示,其中:黑色点云为当前帧;绿色点云为历史相似帧;黑色加粗点为当前帧边角特征点坐标 ${\tilde{r}}_{e d g (c u r, i)}^{W}$ 和平面特征点坐标 ${\tilde{r}}_{p l a (c u r, i)}^{W}$ ;绿色加粗点分别为相似帧的边角特征点 ${\tilde{r}}_{e d g (s i m, j)}^{W}$ , ${\tilde{r}}_{e d g (s i m, k)}^{W}$ 构成直线,以及平面特征点 ${\tilde{r}}_{p l a (s i m, j)}^{W}$ , ${\tilde{r}}_{p l a (s i m, k)}^{W}$ , ${\tilde{r}}_{p l a (s i m, l)}^{W}$ 构成平面.

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4 边角特征和平面特征点云配准

Fig.4 Registration of edge features and plane features

1.4 因子图优化

通过位姿变换方法可以获得当前关键帧与历史相似帧之间的相对位姿变换,以此建立新的回环约束,构建因子图如图5所示.其中:g为测量函数;R为测量噪声协方差矩阵;因子图由一系列不同时刻位姿测量值s_a (a=0,1,2,…)和地图路标点m_a (a=0,1,2,…)的观测量构成.每条实线为1次前后帧之间的位姿累积测量事件,每条虚线为1次检测到的回环约束测量事件,将因子图表达转换为线性化的约束的线性矩阵Ω的形式,每一个新的测量值都会致使信息矩阵发生更新.

图5

新窗口打开| 下载原图ZIP| 生成PPT

图5 因子图优化模型

Fig.5 Model of factor graph optimization

对于1次测量事件, $t_{1}$ 和 $t_{2}$ 时刻位姿测量值分别为 $s_{t_{1}}$ , $s_{t_{2}}$ ,对应的路标点集合为 $m_{t_{2}}$ 测量事件构成的约束函数如下:

（17）

\begin{array}{l} J' = \\ [s_{t_{2}} - g (m_{t_{2}}, s_{t_{1}} {)]}^{T} R^{- 1} [s_{t_{2}} - g (m_{t_{2}}, s_{t_{1}})] \end{array}

因子图优化的过程为求解目标函数得到概率最大的地图和位姿信息路径,其目标函数为测量约束的集合,目标函数公式如下:

（18）

\begin{array}{l} J = s_{0}^{T} Ω s_{0} + \\ \sum [s_{t_{2}} - g (m_{t_{2}}, s_{t_{1}} {)]}^{T} R^{- 1} [s_{t_{2}} - g (m_{t_{2}}, s_{t_{1}})] \end{array}

因子图优化的方法将图映射为信息矩阵的表达方式进行求解,利用图的稀疏性特点,在求解过程中保持较低的计算量和错误的数据关联,优于一般的滤波方法^[17].

2 实验结果与讨论

2.1 准确率和召回率实验

准确率和召回率是衡量回环检测模块中相似度检测算法性能的重要指标,在回环检测算法设计中需首先保证较高的准确率,其次在满足高准确率的同时尽可能提高相似度检测模块的召回率.

准确率和召回率的计算方法如下:

（19）

\begin{matrix} η = \frac{T P}{T P + F P} \end{matrix}

（20）

\begin{array}{l} ε = \frac{T P}{T P + F N} \end{array}

式中:η为准确率;ε为召回率;TP为正确识别出同一场景的数量;FP为将不同场景错误识别为同一场景的数量;FN为同一场景被错误识别为不同场景的数量.

实验采用苏黎世联邦理工学院公开数据集^[18]进行测试,该数据集为手持激光雷达在真实矿井中扫描得到的数据.改变相似度检测算法的阈值ω(0<ω<1),以此测试算法在不同阈值情况下的准确率和召回率,结果如图6所示.

图6

新窗口打开| 下载原图ZIP| 生成PPT

图6 准确率和召回率曲线

Fig.6 Curves of precision and recall rates

由图6可知,在保证准确率为100%的条件下,召回率最高可以达到50.76%,超过一半的相似场景会被召回,此时阈值ω 为0.95,随着阈值ω进一步降低,召回率增大,准确率降低,回环检测可能出现错误匹配,得到错误的位姿信息,对定位和建图产生严重的影响、因此应保证高准确率,在后续算法实验中均将阈值ω设置为0.95,此时准确率为100%.

2.2 定位精度实验

本文进行了多组仿真和开源数据集下的定位精度实验.实验平台CPU为AMD 3600,内存16 GB,算法采用C++语言编写,均在Ubuntu 18.0的机器人操作系统(ROS)进行测试.仿真实验中,使用文献[19]提供的开源数据进行测试,该数据来自于美国国防部高级研究计划局(DARPA)在匹斯堡举行的“DARPA地下煤矿挑战赛”.使用该数据集进行了4组实验,其中Sim_mine_1和Sim_mine_2为仿真实验,将井下三维模型导入Gazebo仿真系统,添加传感器进行数据采集,模拟巡检工作.搭建的仿真实验环境如图7所示,仿真实验中以无人机作为载体,上方搭载Velodyne-16激光雷达进行数据采集.

图7

新窗口打开| 下载原图ZIP| 生成PPT

图7 井下仿真实验环境

Fig.7 Environment of underground simulation experiments

M100_Underground和M100_Tunnel分别为在真实矿井中大疆M100无人机搭载激光雷达在井下巷道和矿山隧道巡检时录制的数据包文件,其中包含了雷达运动过程中的里程计信息.共进行了4组井下环境的实验,实验中每次巡检结束时,传感器回到起点附近.实验对照算法分别为LOAM^[13],以及复现文献[9]的回环检测算法,以LOAM作为前端里程计,使用近邻点搜索和ICP配准进行位姿变换计算的LOAM+ICP算法.将3种定位算法结果分别与仿真实验中记录的真实值或开源数据集中记录的里程计值进行对比.

实验的4个场景分别使用轨迹真实值进行建图, 地图情况如图8所示.图8(a)和8(b)分别为仿真环境的地图,为开采矿井的巷道场景,路段分叉多,通道为非结构化场景,缺乏明显特征;图8(c)和8(d)为M100无人机搭载激光雷达扫描得到的地图,图8(c)环境与图8(a)和8(b)类似,分叉路段多,通道为挖掘面,而图8(d)为矿井入口隧道,采用水泥浇筑,具有支撑柱等结构化特征.

图8

新窗口打开| 下载原图ZIP| 生成PPT

图8 实验场景点云地图

Fig.8 Point cloud maps of experiments

算法定位精度实验中,进行初始坐标系对齐,以均方根误差,全局最大误差值以及终点处的绝对误差作为评价定位精度的指标,实验结果如表1所示, 其中:序号1,2,3,4分别为实验Sim_mine_1、实验Sim_mine_2、实验M100_Underground和实验M100_Tunnel.

表1 定位精度实验结果

Tab.1 Results of localization accuracy experiments

序号	实验算法	最大误差/m	终点误差/m	均方根误差/m	巡检距离/m
1	LOAM	3.48863	3.45962	1.52933	618.007
	LOAM+ICP	2.50563	0.14402	1.03654	618.007
	本文算法	1.46447	0.06376	0.58827	618.007
2	LOAM	1.69615	1.53225	0.83298	638.446
	LOAM+ICP	0.54603	0.02760	0.32963	638.446
	本文算法	0.40609	0.02806	0.17783	638.446
3	LOAM	1.16494	1.15254	0.71497	264.852
	LOAM+ICP	0.82921	0.08813	0.43781	264.852
	本文算法	0.47062	0.09923	0.21525	264.852
4	LOAM	1.92765	0.16456	1.06969	197.782
	LOAM+ICP	0.45537	0.04628	0.22482	197.782
	本文算法	0.39201	0.05806	0.15977	197.782

新窗口打开| 下载CSV

4次实验中,与没有回环检测模块的LOAM算法相比,LOAM+ICP算法和本文算法在最大误差,终点绝对误差以及均方根绝对误差3个指标上均得到明显提高.没有回环检测模块的定位算法随运行时间增长,累积误差逐步增大,体现为LOAM算法的终点绝对误差明显大于其他两种算法,这是造成LOAM算法全局轨迹的均方根误差较大的主要原因.

使用定位评价和可视化工具(EVO)集绘制 Sim_mine_1实验中的定位轨迹,初始坐标轴x,y,z对齐后的轨迹曲线如图9所示,其中:t为时间.3种算法在z坐标下的差值明显,LOAM算法没有回环检测模块,累积误差未得到校正,最终偏移误差绝对值最大;LOAM+ICP和本文算法在加入回环检测后,在巡检轨迹中间段产生一定偏移,但通过回环检测校正了累积误差,全局轨迹的均方根误差较小.

图9

新窗口打开| 下载原图ZIP| 生成PPT

图9 Sim_mine_1定位实验轨迹

Fig.9 Trajectory of localization experiment Sim_mine_1

与LOAM+ICP算法相比,本文算法的定位精度更高,均方根误差较小,但两者终点绝对误差较为接近.其原因在于LOAM+ICP算法计算速度慢,影响了里程计算法的精度,中期误差大,但当检测到回环时仍能计算出准确的位姿,消除累积误差.实验中LOAM+ICP算法和本文算法回环检测部分各模块的计算耗时如表2所示.

表2 算法各模块运行时间

Tab.2 Running time of each module of proposed algorithm

算法	相似度检测时间/ms	位姿计算时间/ms	回环时间/ms
LOAM+ICP	12.6676	122.3668	147.0491
本文算法	16.2595	67.4053	88.7274

新窗口打开| 下载CSV

上述实验数据均为实验过程中记录的单次完整执行对应模块需要的平均耗时.实验结果显示,相似度检测模块中,LOAM+ICP基于位置进行判断,而本文算法需要计算点云描述子,算法步骤较多,在该模块中耗时增加 3.5919 ms;而在位姿计算模块,LOAM+ICP通过ICP方法计算相似帧之间的位姿变换,对点云中的所有点进行配准、目标函数的构建和迭代求解,而本文算法基于曲率计算方法,仅对边角和平面特征进行处理和计算,优化了计算效率,平均耗时由 122.3668 ms降至 67.4053 ms,降低了44.92%;回环时间反映了完整执行一次回环检测模块需要的平均时间,得益于位姿计算时间的明显下降,回环检测模块的整体耗时由 147.0491 ms降低为 88.7274 ms.

在LOAM+ICP算法中由于回环检测模块平均耗时超过100 ms,低于点云数据采集频率10 Hz.且基于全局描述子的回环检测方法解决了 LOAM+ICP算法召回率随累积误差增大不断减小的问题,在长时间运行中更加具有优势.

因子图优化模块对于保证轨迹的全局一致性和平滑性有重要作用.Sim_mine_1实验使用本文算法进行因子图优化前后的轨迹如图10所示.其中:绿色轨迹为因子图优化前轨迹;红色轨迹为因子图优化后轨迹.由局部放大图可以看出,优化后点云地图无重影,全局一致性良好,因子图优化前轨迹的均方根误差为 0.888733 m,大于优化后的 0.588270 m.

图10

新窗口打开| 下载原图ZIP| 生成PPT

图10 因子图优化前后轨迹

Fig.10 Localization trajectory before and after graph optimization

3 结语

针对井下巡检时定位误差随时间不断累积的问题,提出一种适用于激光SLAM的基于点云全局特征描述子的回环检测方法.该方法具有以下优势:① 充分利用了井下巷道的结构特征,相似度检测不受位姿信息约束,通过曲率分别提取通道四周墙壁边角处和平面处的特征向量,利用特征向量和点云中心点之间的角度关系和点云通道和中心点之间的距离关系构建了全局特征描述子;② 提高了计算速度,基于边角点和平面点配准策略,进行高效率的位姿变换算法研究;③全局一致性优,采用因子图优化获得了使全局误差最小的位姿,校正了累积误差.

实验结果表明,本文算法定位精度相较于不加回环检测的LOAM算法和使用距离信息进行回环检测的LOAM+ICP算法得到较大提升,在长时间长距离的实验中提升效果显著.实验结果对于井下执行长时间的巡检任务具有指导意义,未来工作中考虑进一步研究,将算法应用于井下巡检无人机平台上,探究在实际应用场景中的可行性.

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

裴福俊, 李昊洋, 武玫.

基于分布式无味边缘粒子滤波的同步定位与地图构建

[J]. 上海交通大学学报, 2014, 48(7): 987-992.

[本文引用: 1]

PEI

Fujun

, LI

Haoyang

, WU

Mei

Distributed unscented marginalized particle filter for simultaneous localization and mapping

[J]. Journal of Shanghai Jiao Tong University, 2014, 48(7): 987-992.

[本文引用: 1]

[2]

AZHARI

, KIELY

, SENNERSTEN

et al.

A comparison of sensors for underground void mapping by unmanned aerial vehicles

[C]//Proceedings of the First International Conference on Underground Mining Technology. Perth, Australia: ACG, 2017: 419-430.

[本文引用: 1]

[3]

杨晶东, 彭坤, 顾浩楠,

等.

基于直线特征的机器人自主定位方法

[J]. 上海交通大学学报, 2018, 52(9): 1120-1124.

[本文引用: 1]

YANG

Jingdong

, PENG

Kun

, GU

Haonan

et al.

Autonomous localisation method based on linear feature for robots

[J]. Journal of Shanghai Jiao Tong University, 2018, 52(9): 1120-1124.

[本文引用: 1]

[4]

REN

Z L

, WANG

L G

, BI

Robust GICP-based 3D LiDAR SLAM for underground mining environment

[J]. Sensors, 2019, 19(13): 2915.

DOI:10.3390/s19132915 URL [本文引用: 1]

[5]

M G

, ZHU

, YOU

S Z

et al.

Efficient laser-based 3D SLAM for coal mine rescue robots

[J]. IEEE Access, 2019, 7: 14124-14138.

DOI:10.1109/ACCESS.2018.2889304 [本文引用: 1]

An accurate description of laneway space with self-localization is a key issue when coal mine rescue robots (CMRRs) perform post-disaster exploration and rescue missions. The 3D simultaneous localization and mapping (SLAM) is an effective but time-critical and highly challenging task in complex laneway scenarios, especially after disasters. In this paper, we propose a novel real-time 3D SLAM based on normally distributed transform (NDT) that employs pose graph optimization and loop closure to further improve mapping consistency. We innovatively extract floors and walls in the laneway as plane nodes to construct landmark constraints, in addition to applying pose nodes from the lidar odometry via NDT. A lightweight and effective loop detection method is conducted using odometry with an appearance-based approach to building a globally consistent map. The proposed method was evaluated on a public dataset, and field tests in an underground coal mine were performed. Results indicate that our algorithm can achieve lower computational complexity and drift, which can provide pose estimation and environment description for CMRRs to realize remote control assistance and automatic navigation in coal mine rescue missions.

[6]

MUR-ARTAL

, TARDÓS

J D

ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras

[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

DOI:10.1109/TRO.2017.2705103 URL [本文引用: 1]

[7]

QIN

, LI

P L

, SHEN

S J

VINS-mono: A robust and versatile monocular visual-inertial state estimator

[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

DOI:10.1109/TRO.2018.2853729 URL [本文引用: 1]

[8]

GALVEZ-LÓPEZ

, TARDOS

J D

Bags of binary words for fast place recognition in image sequences

[J]. IEEE Transactions on Robotics, 2012, 28(5): 1188-1197.

DOI:10.1109/TRO.2012.2197158 URL [本文引用: 1]

[9]

SHAN

T X

, ENGLOT

LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain

[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid, Spain:IEEE, 2018: 4758-4765.

[本文引用: 2]

[10]

SHAN

T X

, ENGLOT

, MEYERS

et al.

LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping

[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Las Vegas, NV, USA: IEEE, 2021: 5135-5142.

[本文引用: 1]

[11]

柴梦娜, 刘元盛, 任丽军.

基于激光点云NDT特征的两步回环检测

[J]. 激光与红外, 2020, 50(1): 17-24.

[本文引用: 1]

CHAI

Mengna

, LIU

Yuansheng

, REN

Lijun

Two-step loop closure detection based on laser point cloud NDT features

[J]. Laser & Infrared, 2020, 50(1): 17-24.

[本文引用: 1]

[12]

李旭.

基于多线激光雷达建图的里程计优化及回环检测[D]. 哈尔滨: 哈尔滨工业大学, 2019.

[本文引用: 1]

Odometry optimization and loop detection based on multi-beam LiDAR mapping[D]. Harbin:Harbin Institute of Technology, 2019.

[本文引用: 1]

[13]

ZHANG

, SINGH

LOAM: Lidar odometry and mapping in real-time

[EB/OL]. (2014-07-16) [2021-05-18]. http://www.roboticsproceedings.org/rss10/p07.pdf.

URL [本文引用: 4]

[14]

LIU

, POMERLEAU

, COLAS

et al.

Normal estimation for pointcloud using GPU based sparse tensor voting

[C]//2012 IEEE International Conference on Robotics and Biomimetics. Guangzhou, China: IEEE, 2012: 91-96.

[15]

STIGLER

S M

Francis galton’s account of the invention of correlation

[J]. Statistical Science, 1989, 4(2): 73-79.

[本文引用: 1]

[16]

张小利, 李雄飞, 李军.

融合图像质量评价指标的相关性分析及性能评估

[J]. 自动化学报, 2014, 40(2): 306-315.

[本文引用: 1]

ZHANG

Xiaoli

, LI

Xiongfei

, LI

Jun

Validation and correlation analysis of metrics for evaluating performance of image fusion

[J]. Acta Automatica Sinica, 2014, 40(2): 306-315.

[本文引用: 1]

[17]

WEN

W S

, KAN

Y C

, HSU

L T

Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization

[C]//Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation. Miami, Florida, USA: Institute of Navigation, 2019: 3019-3032.

[本文引用: 1]

[18]

RATZ

, DYMCZYK

, SIEGWART

et al.

OneShot global localization: Instant LiDAR-visual pose estimation

[C]//2020 IEEE International Conference on Robotics and Automation. Paris, France: IEEE, 2020: 5415-5421.

[本文引用: 1]

[19]

DANG

, KHATTAK

, MASCARICH

et al.

Explore locally, plan globally: A path planning framework for autonomous robotic exploration in subterranean environments

[C]//2019 19th International Conference on Advanced Robotics. Belo Horizonte, Brazil: IEEE, 2019: 9-16.

[本文引用: 1]

基于分布式无味边缘粒子滤波的同步定位与地图构建

2014

... 同步定位和建图(SLAM)是实现机器人在未知环境下的定位和移动的重要技术方法^[1].定位精度是井下巡检的核心指标,高精度的定位算法是巡检过程中导航和避障的基础.但井下环境复杂,具有低照度、弱纹理、图像特征难以识别的特点^[2],给基于视觉的SLAM算法带来了极大的困难.而激光SLAM算法测量距离远、精度高,利用环境的结构特征进行定位^[3],在井下环境中更具应用前景^[4-5]. ...

基于分布式无味边缘粒子滤波的同步定位与地图构建

2014

A comparison of sensors for underground void mapping by unmanned aerial vehicles

2017

基于直线特征的机器人自主定位方法

2018

基于直线特征的机器人自主定位方法

2018

Robust GICP-based 3D LiDAR SLAM for underground mining environment

2019

Efficient laser-based 3D SLAM for coal mine rescue robots

2019

ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras

2017

... 利用图像语义信息丰富的特点,视觉SLAM算法如ORB-SLAM^[6]、VINS^[7]等,多使用基于词袋模型^[8]的回环检测方法,利用预先训练的视觉特征单词构建词典,将图像中的特征描述为一个单词,图像中单词种类和数量构成图像的词袋向量,通过词袋向量之间的相似度进行回环检测. ...

VINS-mono: A robust and versatile monocular visual-inertial state estimator

2018

Bags of binary words for fast place recognition in image sequences

2012

LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain

2018

... 但激光雷达分辨率低,传递信息不如图像丰富,基于词袋模型的回环检测方法难以应用.在LeGO-LOAM^[9]和LIO-SAM^[10]等激光SLAM方案中使用基于里程计的回环检测方法,通过当前位置信息判断是否可能存在回环,如当前位置一定范围内,存在历史轨迹点,则与对应关键帧进行匹配, 并通过迭代最近点(ICP)配准方法进行位姿计算,实现累计误差的校正.此类方法效果受巡检距离限制,当累积误差超过一定范围时出现无法检测到回环或错误检测的问题,仍未解决累积误差不断增长的问题,受传感器精度影响大,且ICP方法对全部点云进行配准计算,计算时间较长. ...

... M100_Underground和M100_Tunnel分别为在真实矿井中大疆M100无人机搭载激光雷达在井下巷道和矿山隧道巡检时录制的数据包文件,其中包含了雷达运动过程中的里程计信息.共进行了4组井下环境的实验,实验中每次巡检结束时,传感器回到起点附近.实验对照算法分别为LOAM^[13],以及复现文献[9]的回环检测算法,以LOAM作为前端里程计,使用近邻点搜索和ICP配准进行位姿变换计算的LOAM+ICP算法.将3种定位算法结果分别与仿真实验中记录的真实值或开源数据集中记录的里程计值进行对比. ...

LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping

2021

基于激光点云NDT特征的两步回环检测

2020

... 文献[11-12]通过正态分布变换(NDT)或特征直方图检测闭环,通过距离信息判定当前位置阈值与历史轨迹的一致性,使用NDT方法或特征直方图方法进行位姿估计与二次回环检测,有效提升了匹配准确率,但召回率以及回环检测的实时性无法适用于井下无人机的定位需求.因此,使用回环检测方法解决井下无人机巡检保持长时间高精度定位的问题,需避免对里程计的依赖,使用场景相似特征进行回环检测,同时提高计算效率,保证实时性. ...

基于激光点云NDT特征的两步回环检测

2020

2019

LOAM: Lidar odometry and mapping in real-time

... 本文提出一种适用于激光SLAM的点云全局特征描述子回环检测方案,不依赖于位姿信息,利用矿井通道的几何结构特征进行匹配,通过计算与误差较小的历史场景之间的位姿变换,消除累积误差,并完成了完整SLAM算法框架的搭建,实时性好.该框架里程计部分基于激光里程计和建图(LOAM)^[13]算法计算不带回环的位姿信息,回环检测部分基于曲率划分的特征点与点云质心之间的角度关系和尺度关系进行相似度检测,使用平面点和边角点配准方法优化计算速度,并由因子图优化方法保证全局一致性.最终, 通过仿真和开源数据集实验验证了所提算法的有效性,本文研究可为激光SLAM的全局特征提取及回环算法提供理论基础. ...

... 里程计模块参考LOAM算法^[13]实现, 输出里程计位姿信息,并根据点云三维曲率完成了特征点云的筛选,将边角特征点云P_edg和平面特征点云P_pla传递给回环检测模块. ...

... 对筛选后相似度大于阈值的当前帧点云P_cur和历史相似帧点云P_sim进行相对位姿变换的计算.相对位姿的计算使用点云的边角特征和平面特征配准方法,即使用三维点的曲率计算公式^[13]计算点云不同位置处的光滑度,以光滑度为依据提取点云的边角特征和平面特征,只对边角特征和平面特征进行点云配准,提高了计算速度. ...

Normal estimation for pointcloud using GPU based sparse tensor voting

2012

Francis galton’s account of the invention of correlation

1989

... 使用点云全局特征描述子的皮尔逊相关系数^[15]作为衡量点云相似度的标准,其计算公式如下: ...

融合图像质量评价指标的相关性分析及性能评估

2014

... 式中:H₁=[h₁₁ h₁₂ … h₁_i …],H₂=[h₂₁ h₂₂ … h₂_i …]为归一化后的点云全局特征描述子;

μ_{H_{1}}

和

μ_{H_{2}}

分别为H₁和H₂元素的平均值向量;E为数学期望; σ为方差;

S_{H_{1}, H_{2}}

为相关系数,其取值范围为[0, 1].当

S_{H_{1}, H_{2}}

接近于1时,说明特征描述子为正相关关系,点云的相似度越高^[16]. ...

融合图像质量评价指标的相关性分析及性能评估

2014

... 式中:H₁=[h₁₁ h₁₂ … h₁_i …],H₂=[h₂₁ h₂₂ … h₂_i …]为归一化后的点云全局特征描述子;

μ_{H_{1}}

和

μ_{H_{2}}

分别为H₁和H₂元素的平均值向量;E为数学期望; σ为方差;

S_{H_{1}, H_{2}}

为相关系数,其取值范围为[0, 1].当

S_{H_{1}, H_{2}}

接近于1时,说明特征描述子为正相关关系,点云的相似度越高^[16]. ...

Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization

2019

... 因子图优化的方法将图映射为信息矩阵的表达方式进行求解,利用图的稀疏性特点,在求解过程中保持较低的计算量和错误的数据关联,优于一般的滤波方法^[17]. ...

OneShot global localization: Instant LiDAR-visual pose estimation

2020

... 实验采用苏黎世联邦理工学院公开数据集^[18]进行测试,该数据集为手持激光雷达在真实矿井中扫描得到的数据.改变相似度检测算法的阈值ω(0<ω<1),以此测试算法在不同阈值情况下的准确率和召回率,结果如图6所示. ...

Explore locally, plan globally: A path planning framework for autonomous robotic exploration in subterranean environments

2019

... 本文进行了多组仿真和开源数据集下的定位精度实验.实验平台CPU为AMD 3600,内存16 GB,算法采用C++语言编写,均在Ubuntu 18.0的机器人操作系统(ROS)进行测试.仿真实验中,使用文献[19]提供的开源数据进行测试,该数据来自于美国国防部高级研究计划局(DARPA)在匹斯堡举行的“DARPA地下煤矿挑战赛”.使用该数据集进行了4组实验,其中Sim_mine_1和Sim_mine_2为仿真实验,将井下三维模型导入Gazebo仿真系统,添加传感器进行数据采集,模拟巡检工作.搭建的仿真实验环境如图7所示,仿真实验中以无人机作为载体,上方搭载Velodyne-16激光雷达进行数据采集. ...

〈

〉