随着人工智能和自动化技术的不断发展,机器人导航与自动驾驶等技术在物流配送、交通出行、矿山开采、医疗诊断、野外救援等领域发挥着重要作用^[1-3],其基础核心技术同时定位与建图（simultaneous localization and mapping, SLAM）也得到了广泛的关注。

高精度定位是SLAM中的一项重要研究。传统定位方法常常依赖于GPS^[4]或惯性导航系统（inertial navigation system, INS）^[5]。然而,GPS定位方案容易受多路径效应影响而产生较大定位误差。INS定位方案在长时间运行时容易产生姿态漂移^[6]。目前,一种主流的定位方法是在预先构建的地图中定位自动驾驶车辆或机器人。根据实时传感器采集的数据与先验地图之间的关联来计算全局位姿,这种方法一般称为全局定位或地图定位。若在后续自主导航定位中,由于某些不可抗力因素导致定位失效即机器人绑架^[7],可利用全局定位恢复机器人当前位姿。适时的重启全局定位,可有效减少位姿漂移。然而,密集的点云地图通常包含数百万或数十亿个三维点,需要大量存储空间。为了更好地进行机器人定位,也迫切需要提高全局地图的存储与维护效率^[8]。

近年来,众多学者针对全局定位开展了研究。在室外环境下,可利用全球导航卫星系统（global navigation satellite system,GNSS）^[9]、开源地图^[10]、GPS信号^[11]、实时动态差分定位^[12]技术等外源信号方案实现初始定位,但在高架、城市小巷、农村环境等环境下效果欠佳。由于缺少稳定的卫星信号,传统SLAM方案是室内环境常用的导航方式,目前主要有视觉SLAM^[13-14]、激光SLAM^[14-16],以及多传感器融合SLAM方案^[17-18]。在基于场景特征的定位方案中,文献[19]和文献[20]分别将平面特征融入到视觉和激光方案中,根据实时数据与地图中平面特征关联寻找出最优全局位姿。文献[21]使用距离函数描述已占用和未占用空间几何形状特性,根据几何拓扑关系实现定位。文献[22]采用平面图作为全局地图,选取实时数据与地图中均较为稳定的特征作为标记。文献[23]选取传感器捕获的天花板特征的位置和方向作为定位所需的特征。基于特征的定位方法通常受限于特征的准确性,在纹理信息不足的空旷场景容易失效。基于深度学习的定位方法中,文献[24]将单发检测网络与最大似然估计相结合实现全局定位。文献[25]基于单线激光雷达（light detection and ranging,LiDAR）提出可抽样深度多模态观测模型改进了传统蒙特卡洛方法中采样计算的局限性。但是作为数据驱动的方法,基于深度学习的方法严重依赖训练数据集,需要更高的计算资源,同时还需要复杂数据清洗工作。在基于辅助信息的定位方案中,利用射频识别标志^[26]的定位方法易受场景中移动目标的影响。基于Wi-Fi信号^[27]的定位方法无法解决传感器的朝向等问题。利用室内墙体^[28]进行定位的成功率仍然有待提高。

在使用各类传感器的定位方案中,视觉传感器重量轻、成本低、便于携带,但视场角有限且易受光照变化影响。多传感器融合方案虽然能增加数据丰富度,但数据融合问题增加了算法挑战性。激光雷达量测范围广、精度高,抗干扰能力强,激光定位方案也是目前较为稳定和有效的自动驾驶解决方案。此外,室内大厅、封闭走廊和停车场等场景占地面积通常在500 ${\mathrm{m}}^{\mathrm{2}}$以上,室内较空旷,特征信息匮乏,常见方法容易失效。

一些常见的人工构筑物可以用作辅助定位的天然地标,例如室外车道线、路灯、地面,以及室内墙壁、窗户、楼梯和柱子。本文发现如图1中所示的圆形支柱是室内环境中普遍存在的人造结构。它具备如下3个重要特性：（1）离群性,室内圆柱通常远离其他地物。（2）对称性,视点与圆心连线总能均匀平分视野内的圆弧。（3）显著性,作为室内支撑结构,其高度常高于其他目标。此外,可利用少量参数还原其形状信息。空旷室内场景中常见的柱子结构多为圆形或方形。

图  1  存在柱子的常见场景

Figure  1.  Common Scenarios with Cylinders

下载: 全尺寸图片 幻灯片

本文提出在大范围室内环境利用圆柱实现机器人的全局定位与回环检测,分析室内建筑布局特点及圆柱所具备的几何特性,将圆柱作为地标应用于定位。传统的定位方法在结构复杂的环境下,能够提取更多的点、线、面等几何级特征,定位性能更优。本文选取圆柱这种对象级的特征进行定位,能够解决常规算法在特征匮乏的空旷场景中的定位难题,与其他定位方法相结合,可提高定位算法的适用性。虽然本文专注于使用圆柱进行定位,但该方法可以扩展到基于方柱或基于融合的定位。

本文主要贡献如下：

1） 设计了一种参数化地图生成方法和地图数据管理方案,实现地图的参数化表达,并离线存储地图圆柱和环境信息,有效降低全局地图维护成本。

2） 提出了一种多线激光雷达数据中柱体结构的分割方法,可有效克服因车距和点云密度造成的分割性能差的问题,采用简单的几何属性表达三维圆柱,提高了特征表达效率。

3） 提出两种基于圆柱结构的位姿求解策略。第一种启发式搜索策略可以有效提高全局定位的成功率。第二种优化求解式策略显著提高了定位精度和效率。

1 研究方法

图2是本文全局定位流程图,主要包括参数化地图生成与实时全局定位。

图  2  全局定位流程图

Figure  2.  Flowchart of Global Localization

下载: 全尺寸图片 幻灯片

1.1   参数化地图生成

1）点云地图拼接。在实时定位开启前,在特定的场景预先采集一组或多组数据来构建先验地图。在室外环境中可采用GPS和惯性测量单元等组合导航来构建高精度地图。在室内环境下由于缺少稳定的外源信号,采用激光SLAM算法LOAM^[29]对离线采集的LiDAR点云进行帧间匹配,计算出相对位姿并构建稠密的点云地图。LOAM在公开评测的KITTI自动驾驶数据集上依旧取得良好的定位性能,其位置误差小于0.55%,旋转误差约为0.001 6 º/m。

2）地图预处理。拼接得到的点云地图数据海量,采用0.1 m的体素对地图滤波降采样。图3是激光雷达坐标系示意图,其中x轴向前,y轴向左,z轴竖直向上。室内环境下本文假定车辆在水平面内运动,地面和天花板对后续栅格地图的构建造成干扰,采用直通滤波经验性地设置阈值$z_{\mathrm{1}}$和$z_{\mathrm{2}}$来滤除天花板和地面点云。

图  3  激光雷达坐标系

Figure  3.  Coordinate System of LiDAR

下载: 全尺寸图片 幻灯片

3）地图圆柱分割。如图4所示,由于视野遮挡,实时LiDAR点云无法捕获完整柱面,在水平面上呈现为圆的部分圆弧。先验地图将多帧点云拼接在一起,使得地图中的柱子具有完整的表面点云,在水平面上呈现为一个完整的圆。在地图数据中,采用随机采样一致性（random sample consensus,RANSAC）算法迭代循环地分割出圆柱表面点,其中每次分割的圆柱点满足如下条件：

式中,$k$是迭代分割次数索引；${\mathit{L}}_{q_k{p}_i}$是由圆柱中轴线上一点$q_k$与柱面点$p_i$构成的向量；$p_i$是分割的柱面点；$\times$表示叉积运算；${\mathit{n}}_k$为圆柱中心轴的方向向量；$r_k$是柱子的半径。

图  4  地图与实时数据中的圆柱差异

Figure  4.  Differences Between Map and Online Cylinders

下载: 全尺寸图片 幻灯片

4） 栅格地图构建。室内环境中存在大量稳定的人工构筑物如墙壁、柱子和楼梯等,其位置不会轻易发生变动。本文采用栅格地图来表示这些稳固结构在地图中位置。由于配准和车辆运动仍然会导致地图略微倾斜,本文使用主成分分析（principal component analysis, PCA）计算校正矩阵${\mathit{T}}_m$,从而消除这种倾斜误差：

式中,${\mathit{t}}_m=-{\mathit{R}}_m{\mathit{t}}_c$,${\mathit{t}}_c=[x_c^m{y}_c^m{z}_c^m{]}^{\mathbf{T}}$是滤波后点云地图重心向量；${\mathit{R}}_m=\left[{\mathit{\eta }}_{\mathrm{1}}{\mathit{\eta }}_{\mathrm{2}}{\mathit{\eta }}_{\mathrm{3}}\right]$是3×3矩阵,${\mathit{\eta }}_{\mathrm{1}}$、${\mathit{\eta }}_{\mathrm{2}}$和${\mathit{\eta }}_{\mathrm{3}}$是对滤波地图构建协方差矩阵后,特征值分解得到的3个特征向量。在对滤波地图的主成分分析中,第一、二方向向量为水平方向。进一步采用仿射变换矩阵${\mathit{T}}_f$生成栅格地图$M^G$,计算公式为：

式中,${\mathit{t}}_{\mathrm{3}\times \mathrm{1}}={\left[\begin{array}{ccc}-\underset{x}{\mathrm{m}\mathrm{i}\mathrm{n}}{x}^p& -\underset{y}{\mathrm{m}\mathrm{i}\mathrm{n}}{y}^p& \mathrm{0}\end{array}\right]}^{\mathbf{T}}$,（$x^p$,$y^p$,$z^p$）为滤波地图中三维点的坐标；$g$为栅格地图分辨率。

5）地图数据管理。为提高实时定位效率,降低地图存储空间,本文提出了一种离线的地图数据管理方法,采用几何属性（圆心坐标（$c_x$,$c_y$）和半径长度$r$）来表示地图中圆柱,采用行、列索引记录栅格地图中被结构环境占据的位置。将以上数字以二进制文件的形式离线保存,这些文件称为参数化地图。一部分用于说明圆柱的位置和半径,另一部分用来表达整个场景中稳定构筑物的位置。在开启自动驾驶模式时,只需加载一次参数化地图即可恢复地图数据,显著降低了地图存储开销。

1.2   实时全局定位

本文假定室内环境中车辆是在平面上行驶的,因此6自由度的定位问题转为3自由度的位姿求解问题。如图4所示,实时扫描的柱面点云在水平面的投影总是呈圆弧状。当车辆驶离圆柱时,柱面点开始变稀疏,但在水平面上投影得到的圆弧依旧属于同一个圆的部分圆弧,唯一的区别仅是长度更短。

1）实时柱子分割。多线激光雷达点云在垂直方向上存在稀疏性,直接使用RANSAC提取柱子效果不佳。本文提出了一种在实时点云中提取圆柱的方法。首先,在单帧LiDAR点云$S$中使用高度直接滤除地面和天花板,采用式（2）中PCA来校正点云数据。其次,将其投影至水平面内,可使得三维空间中稀疏的表面点云变为稠密的平面聚类点云。然后,采用欧氏聚类将投影点云划分为多个聚类,其中包含柱体的点云聚类与其他聚类不同,在水平面内呈圆弧状。在校正和投影之前预先记录每个点的高度信息,进一步计算每个聚类的高度为：

式中,$h_i$是原始3D空间中第$i$个聚类$C_i$的高度；$z_j$是点云$S^h$中三维点$p_j$的z坐标。如图5所示,室内环境中存在一些非圆柱的聚类干扰物,如行人、盆栽和桌椅等,其在水平面的投影也近似圆弧状。基于圆柱结构的显著性,作为室内重要的承重结构,其高度一般高于动态目标。利用高度阈值$z_{\mathrm{3}}$删除一些明显非圆柱的聚类干扰物。

图  5  非圆柱相似聚类

Figure  5.  Non-cylinder Similar Clusters

下载: 全尺寸图片 幻灯片

最后,在剩余的每一个聚类中采用RANSAC拟合圆弧。如图6所示,鉴于圆柱的对称特性,圆柱与激光雷达之间的位置关系应满足对称性约束,即激光雷达$p_{\mathrm{0}}$与圆柱中心$c$的连线应该近似平分圆弧$\stackrel{̑}{p_{\mathrm{1}}{p}_{\mathrm{3}}{p}_{\mathrm{2}}}$。进一步地,采用对称性约束条件来判断当前拟合圆弧是否有效,计算公式为：

式中,$\omega$是一种对称度量；$\cdot$是点积运算；${\lambda }_{\omega }$为一个阈值参数；$p_{\mathrm{1}}$和$p_{\mathrm{2}}$是圆弧的两个端点；$p_{\mathrm{3}}$是圆弧中点；${\mathit{L}}_{p_{\mathrm{1}}{p}_{\mathrm{2}}}$和${\mathit{L}}_{{\mathrm{p}}_{\mathrm{1}}{\mathrm{p}}_{\mathrm{3}}}$是由两个端点构成的向量。

图  6  对称性约束

Figure  6.  Symmetry Constraints

下载: 全尺寸图片 幻灯片

此外,圆柱体积和半径越大,与行人、盆栽等干扰目标的差异性越明显,柱面点更加稠密,柱体（半径和圆心）的拟合精度也会随之提高,进而提高后续定位精度。

2）单圆柱定位。当车辆与圆柱之间视线受阻时,不可避免地造成圆柱检测效果不佳。本文方法在未检测到柱子时无法定位车辆。为提高算法的通用性,首先提出一种基于启发式搜索的单圆柱定位（single pillar-based localization, SPL）。

首先,使用半径作为几何约束在地图圆柱集$A^m$中挑选出与实时圆柱相似的候选匹配对。然后,在所有圆柱匹配对中使用如下模型计算位姿${\mathit{T}}_{\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{a}\mathrm{l}}$：

式中,$\mathrm{*}$表示求得的最佳解；$i$是地图柱子索引；${\theta }_j=j\epsilon$,是第$j$次搜索的旋转角度,$\epsilon$是角度分辨率；$c_i$是第$i$个圆柱中心的$x$和$y$坐标,即圆心坐标；$K_{\mathrm{1}}$和$K_{\mathrm{2}}$是两个微小的正常数,用来保证分子和分母大于0；$S^h$是校正之前的实时单帧LiDAR点云；$N_{S^h}$表示点云$S^h$的点数；$M^G$是栅格地图；$\oplus$是一个计数操作,A$\oplus$B表示统计点云B击中栅格地图A的点数；${\mathit{T}}_{c_i}^{\mathrm{\text{'}}}$为根据圆心$c_i$计算的平移变换矩阵；${\mathit{T}}_{{\theta }_j,c_i}^{\mathrm{\text{'}}\mathrm{\text{'}}}$为根据圆心$c_i$和旋转角度${\theta }_j$计算的旋转变换矩阵。此处添加${\mathit{T}}_m$是为了保持坐标的一致性,因为栅格地图是根据校正后点云地图构建的,而地图圆柱是利用未校正的点云地图检测得到的。设定阈值$\Upsilon$,如果式（6）中位姿惩罚函数小于$\Upsilon$,则返回当前位姿。

3）多圆柱定位。当实时数据中检测到至少2个圆柱时,本文提出基于优化求解的多圆柱定位（multiple pillars-based localization, MPL）。首先,根据半径长度剔除明显错误的圆柱匹配对。其次,如图7所示,进一步根据圆柱之间拓扑关系剔除实时圆柱集$A^s$中的伪圆柱。伪圆柱数量越多,将会产生更多错误的拓扑关系。本文中只采用圆柱之间的距离和角度作为拓扑关系描述。

图  7  圆柱间拓扑关系

Figure  7.  Topological Relationships Among Cylinders

下载: 全尺寸图片 幻灯片

然后,类似四点一致集思想,挑选出所有与实时圆柱子集$\{A_t^s\}$近似全等的地图圆柱子集$\{A_{k^{\mathrm{\text{'}}}}^s\}$,其中$\{A_t^s\}\in A^s$,$\{A_{k^{\mathrm{\text{'}}}}^m\}\in A^m$。基于每一组近似全等的圆柱子集,构建如下优化模型计算位姿${\mathit{T}}_{i^{\mathrm{\text{'}}\mathrm{*}},j^{\mathrm{\text{'}}\mathrm{*}}}^{k^{\mathrm{\text{'}}\mathrm{*}}}$：

式中,$M$角标表示地图相关数据；$S$角标表示实时点云相关数据；$f^a$和$f^b$是两类误差函数；${‖\cdot ‖}_{\mathrm{2}}$表示计算向量的模长；$i^{\mathrm{\text{'}}}$和$i^{\mathrm{\text{'}}\mathrm{\text{'}}}$是两个地图圆柱索引；$j^{\mathrm{\text{'}}}$是实时柱子索引；$u$和$v$为误差函数索引；$c$是2D圆柱中心；$k^{\mathrm{\text{'}}}$是地图圆柱子集索引。进一步,计算惩罚函数来评估当前位姿$L(k^{\mathrm{\text{'}}})$：

式中,${\mathit{T}}_{i^{\mathrm{\text{'}}\mathrm{*}},j^{\mathrm{\text{'}}\mathrm{*}}}^{k^{\mathrm{\text{'}}\mathrm{*}}}$替换式（6）中的变换矩阵${\mathit{T}}_{{\theta }_j,c_i}^{\mathrm{\text{'}}\mathrm{\text{'}}}{\mathit{T}}_{c_i}^{\mathrm{\text{'}}}$。最后,如果惩罚函数小于$\Upsilon$,则返回当前位姿。

2 全局定位与回环检测实验分析

2.1   实验数据与平台

如表1所示,选取3个典型室内场景测试本文方法。3个场景占地面积最小为825 ${\mathrm{m}}^{\mathrm{2}}$,最大为1 568 ${\mathrm{m}}^{\mathrm{2}}$,分别包含7、8和9个圆柱。图8是3个场景示意图和机器人轨迹路线。图8（e）为提高可视化效果,$z$坐标依次增加显示。数据采集平台为搭载一个16线RoboSense激光雷达的TurtleBot机器人。在走廊场景中机器人环绕场地两周,十分适用于全局定位和回环检测相关算法的评估。实验环境为Intel 1.80 GHz i7-10510U CPU,32 GB内存,采用Ubuntu18.04系统,C++编程语言。

表  1  实验数据

Table  1.  Experimental Data

场景	场景类型	面积/m2	扫描数/个	圆柱数/个
1	大厅	56×28	548	7
2	走廊	55×15	4 529	8
3	大厅+走廊	60×21	1 103	9

下载: 导出CSV 

| 显示表格

图  8  实验数据

Figure  8.  Experimental Data

下载: 全尺寸图片 幻灯片

2.2   实验参数设置

表2为本文实验主要参数设置。图9为场景1中某实时LiDAR点云$z$坐标示意图。基于移动平台高度并多次实验,分别设置高度阈值$z_{\mathrm{1}}$、$z_{\mathrm{2}}$和$z_{\mathrm{3}}$为－0.3 m、2.5 m和2.0 m。在分割圆柱时,为避免过于严格的对称性造成柱体漏检,${\lambda }_{\omega }$设置为0.2。采用RANSAC拟合圆弧时,设置模型内点误差阈值$d$为$\mathrm{0.02}$ m。为保证栅格地图有效性,分辨率$g$应该满足$g<\mathrm{2}d$,在实验中设置为0.03 m。如果在其他实验中对拟合模型误差进行调整时,相应地调整栅格地图分辨率即可。

表  2  参数设置

Table  2.  Parameter Setting

参数
$z_1$/m	$z_2$/m	$z_3$/m	${\lambda }_{\omega }$	$g$/m	$\Upsilon$	$\epsilon$
－0.3	2.5	2.0	0.2	0.03	5.0	3

下载: 导出CSV 

| 显示表格

图  9  实时LiDAR点云z坐标

Figure  9.  Online LiDAR Point Cloud’s z Coordinates

下载: 全尺寸图片 幻灯片

为避免角度增量$\epsilon$过大而导致位姿精度差,应满足以下条件：

式中,$D$为车辆到圆柱表面的距离；$r$为圆柱半径长度；$v$为LiDAR水平分辨率。几何示意图如图10所示。根据室内占地面积、常见圆柱大小和LiDAR的有效量测距离,经验性地选定距移动平台40 m处的柱体为有效特征。LiDAR水平分辨率$v$为0.1º,室内柱体半径$r$通常不超过1 m,为保证计算效率及位姿准确度,本文设置$\epsilon$为3º。本文参数是在16线LiDAR数据下多次实验选取的最佳参数,但仍可用于在其他类似场景或其他线数LiDAR点云进行实验。

图  10  角度增量设置示意图

Figure  10.  Explanations of Angle Resolutions

下载: 全尺寸图片 幻灯片

2.3   圆柱分割性能评估

1）对称性约束阈值。本文将实时数据中人工标记的圆柱点云作为真值。图11记录了场景1中在参数${\lambda }_{\omega }$不同取值下的圆柱分割性能。${\lambda }_{\omega }$取值越小,圆柱分割精度越高,但所能提取到的圆柱数量会显著降低。16线LiDAR点云的稀疏性增加了柱子分割挑战性。图11表明,本文圆柱分割精度可达到90%以上。综合考虑圆柱分割的数量和精度,本文设置${\lambda }_{\omega }$为0.2。

图  11  圆柱分割评估

Figure  11.  Evaluations of Pillar Segmentation

下载: 全尺寸图片 幻灯片

2）车辆与圆柱距离。如图12记录了场景3中车辆与圆柱之间不同距离下的分割误差。随机选取场景中某一个圆柱作为参照,计算本文方法所拟合的圆心与真实圆心的距离近似代替拟合误差。实验中两圆心距离差小于1 m时,认为是正确分割。图12表明,车辆与圆柱的距离较近时（小于8 m）,分割精度可达到2 cm。当距离增大时,出现部分误差较大的圆柱分割结果,同时高精度的分割结果比重也有所下降。

图  12  分割误差与距离的关系

Figure  12.  Relationship Between Segmentation Error and Distance

下载: 全尺寸图片 幻灯片

2.4   全局定位性能评估

在全局定位方面,本文选取文献[28]作为对比方法,该方法利用室内墙壁进行全局定位。在对比方法中,基于不同的室内墙壁组合制定了3种定位策略。为简化表达,分别将其简写为平行墙壁（parallel wall localization,PWL）、对称墙壁（symmetrical wall localization,SWL）和垂直墙壁（vertical wall localization,VWL）。实验中分别评估了SPL、MPL和MPL+SPL。在MPL+SPL中,当MPL定位失败时才会启动SPL。

1）定位成功率。本文使用LOAM计算的相对位姿作为真值,在定位误差小于1 m时认为定位成功。每次的定位结果分为4类：（1）$C_{\mathrm{1}}$,实时数据中检测到的圆柱数少于定位方法所需最少圆柱数。SPL、MPL和MPL+SPL分别至少需要1个、2个和1个圆柱。（2）$C_{\mathrm{2}}$,认为当前定位结果不可靠。（3）$C_{\mathrm{3}}$,错误定位结果。（4）$C_{\mathrm{4}}$,正确定位结果。定位成功率${\phi }_s$的计算公式为：

式中,$n$表示LiDAR扫描数；$n_{tp}$为成功定位的扫描数。表3记录了各方法的定位成功率。由表3可知,本文方法优于3种对比方法,SPL能够成功定位大厅中90%的数据。图13描绘了场景3中的定位结果和成功检测的圆柱数量百分比。

表  3  定位成功率

Table  3.  Localization Success Rate

场景	对比方法			本文方法
	PWL	SWL	VWL	SPL	MPL	MPL+SPL
1	0.106	0.290	0.418	0.902	0.686	0.916
2	0.116	0.460	0.420	0.617	0.415	0.589
3	0.112	0.366	0.523	0.794	0.552	0.771
均值	0.129	0.372	0.454	0.771	0.551	0.759

下载: 导出CSV 

| 显示表格

图  13  成功率分布和圆柱数量比例

Figure  13.  Distribution of Success Rate and Ratio of Pillar Number

下载: 全尺寸图片 幻灯片

2）定位精度。表4总结了全局平均定位误差。本文方法分别取得了0.073 m（场景1）的最佳精度和0.095 m（场景2）、0.092 m（场景3）的次佳精度。图14描绘了使用SPL和MPL的定位结果,地图点根据高度进行渲染。场景2、场景3中具有大量墙体结构,使得VWL和SWL定位精度优于本文方法。MPL在3个场景的定位误差分别为0.073 m、0.095 m和0.092 m,均优于SPL。柱子之间的拓扑关系在补偿圆柱分割误差的同时提高了定位精度。

表  4  平均定位误差/m

Table  4.  Average Localization Errors/m

场景	对比方法			本文方法
	PWL	SWL	VWL	SPL	MPL	MPL+SPL
1	0.107	0.141	0.153	0.109	0.073	0.087
2	0.135	0.104	0.064	0.110	0.095	0.096
3	0112	0.074	0.113	0.125	0.092	0.111
均值	0.118	0.106	0.110	0.115	0.087	0.098

下载: 导出CSV 

| 显示表格

图  14  全局定位结果

Figure  14.  Results of Global Localization

下载: 全尺寸图片 幻灯片

3）运行速度。本文方法离线构建参数化地图,实时定位的运行时间仅包括圆柱分割和位姿求解。图15描绘了走廊场景随机选取600帧数据的运行时间。表5记录了各场景平均运行时间对比。MPL和MPL+SPL在场景2和3中实现了最优和次优的定位效率。在大厅场景中,SWL（0.072 s）定位速度略优于MPL（0.122 s）。

图  15  走廊定位效率

Figure  15.  Localization Efficiency of Corridor

下载: 全尺寸图片 幻灯片

表  5  定位时间/s

Table  5.  Localization Time/s

场景	对比方法			本文方法
	PWL	SWL	VWL	SPL	MPL	MPL+SPL
1	0.224	0.072	2.700	1.182	0.122	0.132
2	0.676	0.232	1.063	0.976	0.100	0.129
3	0.633	0.222	1.371	1.634	0.198	0.213
均值	0.511	0.175	1.711	1.264	0.140	0.158

下载: 导出CSV 

| 显示表格

4）错误案例。图16描述了本文方法所产生的两种定位失败情况。机器人在图16（a）中运行到电梯附近,在图16（b）中驶入一段封闭的走廊。以上两种情况由于视野遮挡,均未检测到圆柱,导致定位失败。

图  16  定位失败案例

Figure  16.  Localization Failure Cases

下载: 全尺寸图片 幻灯片

2.5   位置识别性能评估

回环检测或位置识别是机器人SLAM中的一个重要模块,判断机器人是否经过当前位置,将约束信息传递给后端优化模块,以减少位姿误差累积。本文方法能够计算出机器人的全局位姿,可进一步完成回环检测任务。为此本文选取Scan Context^[30]、M2DP^[31]、ESF^[32]作为对比方法,使用走廊场景（场景2）来评估位置识别性能,当两帧数据的位置在4 m以内时,认为是一对回环数据。

1）$F\mathrm{1}$分数^[33-34]。由于位置识别本质为一个二分类模型,采用准确率P、召回率R、$F\mathrm{1}$分数来评估各方法判断结果的精确度,计算公式为：

式中,$n_{\mathrm{F}\mathrm{P}}$为正确识别回环数量；$n_{\mathrm{F}\mathrm{P}}$为错误识别数量；$n_{\mathrm{F}\mathrm{N}}$为漏检数量。表6记录了各方法的最大$F\mathrm{1}$分数结果。实验中走廊场景内部通视,两侧墙壁、房间布局几乎一致,全局位置识别的挑战性较大。Scan Context取得了最优结果（0.863）,本文方法MPL得到次优结果（0.835）。本文3种方

表  6  最大$F\mathrm{1}$分数

Table  6.  Maximum $F\mathrm{1}$ Score

场景	M2DP	ESF	Scan Context	SPL	MPL	MPL+SPL
2	0.794	0.686	0.863	0.813	0.835	0.831

下载: 导出CSV 

| 显示表格

法的最大$F\mathrm{1}$分数均达到0.8以上。

2）识别成功率。在全局位置识别中,通常会给出一定候选数量的匹配结果来识别性能。如表7所示,实验中计算了最大50个候选匹配下的识别成功率。当候选数量较少时,Scan Context的正确率明显高于本文方法,但是当候选数量增加时,本文方法取得了更高的成功率。

表  7  不同候选数量下的成功率

Table  7.  Success Rate Under Different Candidate Numbers

方法	候选数量
	1	2	5	8	10	15	20	25	30	35	40	42	45	48	50
M2DP	0.783	0.786	0.793	0.797	0.802	0.812	0.814	0.819	0.820	0.823	0.836	0.838	0.842	0.848	0.850
ESF	0.862	0.880	0.905	0.917	0.925	0.935	0.940	0.943	0.945	0.946	0.950	0.951	0.952	0.952	0.952
Scan Context	0.900	0.911	0.922	0.929	0.931	0.937	0.938	0.939	0.939	0.939	0.940	0.940	0.941	0.941	0.941
SPL	0.573	0.656	0.768	0.817	0.855	0.902	0.930	0.939	0.945	0.949	0.952	0.952	0.952	0.953	0.954
MPL	0.615	0.691	0.796	0.846	0.870	0.913	0.935	0.940	0.942	0.948	0.951	0.953	0.953	0.954	0.955
MPL+SPL	0.620	0.697	0.794	0.845	0.867	0.914	0.934	0.939	0.942	0.947	0.953	0.954	0.955	0.955	0.958

下载: 导出CSV 

| 显示表格

3）匹配分析。图17描绘了各方法在总体识别精度最大时各数据的匹配结果。其中Scan Context取得最佳精度,仅有2个错误匹配。然后依次是MPL（8个）、MPL+SPL（11个）、SPL（11个）、ESF（12个）和M2DP（24个）。在实验中,鉴于本文方法使用圆柱定位的特性,在利用位姿判断是否出现回环之前,首先利用实时数据所提取的圆柱数量进行初筛,具体如下：在电梯附近未检测到圆柱,而在走廊中间可检测到多个圆柱,利用圆柱的数量差异可直接判断当前两帧数据非回环,实现快速筛选。因此,扩展使用鲁棒的圆柱特征可在一定程度上改善位置识别性能。

图  17  位置识别结果

Figure  17.  Results of Place Recognition

下载: 全尺寸图片 幻灯片

3 结 语

本文提出了一种基于室内圆柱结构的全局定位与回环检测方法,通过3D圆柱检测和栅格地图提取,并采用二进制表达场景中的鲁棒特征和稳定的人工构筑物分布,可将稠密、海量的点云地图压缩为高效的参数化地图,有效地节省了大量的地图存储空间。利用圆柱独特的几何特性从实时LiDAR点云中分割圆柱,在克服低线数LiDAR数据稀疏性的同时,还可以保证远距离目标分割的准确性。利用实时圆柱与参数化地图特征关联能够有效计算出机器人的全局位姿。在3个真实室内场景中验证该方法有效性,相比于对比方法,本文方法取得了更好的定位性能。在全局位置识别方面,本文方法引入了圆柱特征能够通过圆柱数量快速、有效地筛选部分非回环数据,整体性能达到主流方法水平。但是本文方法在无柱体环境下无法使用,在使用该方法时应结合场景的实际情况加以利用。在后续研究中,一方面将在全局定位框架中使用更多典型地物信息,以此提高定位算法的通用性；另一方面,将研究滤波算法与定位特征相结合来提高整体的定位成功率。