1 引言(Introduction)

确定自身在周围环境内的位姿, 被称作移动机器人的定位问题。解决此问题对机器人执行自动作业非常重要。移动机器人的定位问题大体可分为两类, 位置跟踪与全局定位。

在位置跟踪问题中, 机器人已知自身位姿或者已经通过“全局定位”得到了较好的位姿估计, 此时机器人的姿态可以通过在已知环境中校正姿态误差和随时间跟踪机器人来获得。相反地, 由于全局定位问题假设机器人没有先验位姿, 要求机器人靠自身传感器数据确定自己在地图中的位姿, 因此全局定位问题更加困难。

本文所述移动机器人的全局定位问题主要体现在以下2个方面: (1) 机器人刚刚启动, 不知自身在环境内的位置, 需要获取初始位姿后才能执行进一步的定位与地图构建（SLAM）算法。(2) 机器人在运行时被“绑架”, 在传感器信息丢失的情况下被外界移动到另一区域, 此时需要重新获取位姿并进行校正。其中第(1) 种情况在机器人每次开机时都会发生, 解决移动机器人的全局定位问题, 是保证其后续巡检任务完成的关键。

本文的主要贡献如下:

•提出一种基于贡献度的点云降采样方法, 能过滤出原始点云中对全局定位帮助较大的点。

•使用上述方法改进了已有CSM算法的点云预处理和角度步长计算部分, 使其定位速度与成功率得到提高, 能够更好地应用在室内大场景中。

•在仿真环境与真实环境内进行对比实验, 表明相比于原CSM算法以及其他常用全局定位算法, 改进后算法的全局定位成功率与速度更佳。

2 相关工作（Related works） 2.1 常用全局定位方法

Kim等^[1]提出了SC (scan context) 描述子, 该描述子将笛卡儿坐标系的信息转换到极坐标系下计算, 具有较高的关键帧搜索速度。Wang等^[2]在SC描述子的基础上加入了强度信息, 得到了精度更高的ISC（intensity scan context）描述子。Wang等^[3]提出了LiDAR-Iris, 通过Log-Gabor滤波和阈值运算获取点云描述符, 匹配速度较SC描述子有所提高。ORB-SLAM、ORB2-SLAM与ORB3-SLAM算法^[4-6]使用基于ORB（oriented FAST and rotated BRIEF）描述子的词袋模型进行定位。文[7] 提出了SegMatch方法, 利用基于稠密点云的描述符计算方法、结合随机森林法进行定位。Pan等^[8]提出了ClusterMap方法, 其思路与SegMatch方法类似, 却不需要稠密点云。文[9] 提出了SDF（符号距离函数) 法, 该方法对栅格地图进行距离变换, 然后使用黑塞行列式法与方向梯度直方图法提取特征点并计算其描述符进行匹配。文[10] 将点云转为ODVL（optimal depth & vector length）图像来提取环境语义信息进行定位。文[11] 通过拓扑节点划分地图, 并提取子图的边界特征、空闲特征与对称性特征用以进行定位。Cao等^[12]使用非监督深度学习算法提取$3 \mathrm{D}$点云的特征进行定位。Sinha等^[13]使用基于卷积神经网络的实时架构, 将移动机器人的传感器与使用扩展卡尔曼滤波器的单目相机图像信息结合进行定位。$\mathrm{Yu}$等^[14]将$2 \mathrm{D}$激光数据转为多通道图像并交由神经网络来预测位姿。文[15] 将$3 \mathrm{D}$点云从高到低投影成3张2D图像, 使用深度卷积神经网络根据堆叠而成的图像预测位姿。文[16] 使用卷积神经网络与长短期记忆网络基于去噪后的图像进行定位。然而, 由于上述方案具有以下缺点中的一种或几种, 准备时间长、耗费算力大、成本较高, 因此不适合本文所述巡检机器人。

2.2 相关性扫描匹配算法

鉴于上述诸多限制, 本文最终选择CSM算法^[17]作为机器人的全局定位方案的核心。CSM算法通过穷举所有可能的位姿来篮选最佳位姿, 能够保证位姿解算的准确性。并且, CSM算法利用分支定界的思路, 构建多分辨率栅格地图(如图 1所示）来加速穷举的过程, 保证了位姿解算的速度。

给定一个位姿、一帧点云与一张栅格地图, CSM算法使用式(1) 判断位姿是否准确:

$ \operatorname{score}(\boldsymbol{T})=\sum\limits_{k=1}^{K} \frac{M_{\text {nearest }}\left(\boldsymbol{T} \boldsymbol{p}_{k}\right)}{K} $

(1)

其中$K$为输入点云中包含的点的个数。$\boldsymbol{p}_{k}$是输入点云中的第$k$个点的位置。$\boldsymbol{T}$是机器人在地图上的位姿, 可以把点$p_{k}$从机器人坐标系下变换到世界坐标系下。$M_{\text {nearest }}$是一个函数, 其功能是获取点在栅格地图$M$中对应位置的栅格值。$\operatorname{score}(\boldsymbol{T})$是评分函数, 用来衡量位姿$\boldsymbol{T}$是否准确。

此外, CSM算法与本文改进算法的目的均可以表示为

$ \underset{\boldsymbol{T}_{\xi} \in W}{\arg \max } \operatorname{score}\left(\boldsymbol{T}_{\xi}\right) $

(2)

其中, $W$为搜索窗口, $\boldsymbol{T}_{\xi}$为$W$中的一个位姿。通俗地说, 算法的目的是在搜索窗$W$内寻找最佳位姿$\boldsymbol{T}_{\xi}$, 使机器人在此位姿下得到的评分最高。

在CSM算法中, 一个位姿由3个变量决定, 即$x, y$坐标值和绕$z$轴旋转的角度。对于给定的一张栅格地图以及一帧点云, 穷举位姿时所需的距离步长等同于当前层栅格地图的栅格尺寸, 而角度步长计算方式如下:

$ d_{\max }=\max _\limits{k=1, \cdots, K}\left|\boldsymbol{p}_{k}\right| $

(3)

$ \delta_{\theta}=\arccos \left(1-\frac{r^{2}}{2 d_{\max }^{2}}\right) $

(4)

其中, $|\cdot|$表示取模运算, $d_{\max }$是点云内点与点云坐标系原点之间的最大距离, $r_{\mathrm{slt}}$为原始栅格地图的栅格尺寸，$\delta_{\theta}$为角度步长。

CSM算法首先对输入点云进行降采样处理, 根据输入地图构建多分辨率栅格地图。然后, 在顶层栅格地图中穷举位姿, 将这些位姿插入一个堆栈中。接下来, 算法进入循环: 不断取出堆栈中评分最高的位姿(注意, 该位姿的评分还需要高于人为给定的阈值, 否则该位姿会被直接丢弃不作运算), 查看该位姿对应的地图是否为原始栅格地图; 若是, 则将其记录备用; 否则, 将该位姿分解为下一层栅格地图中对应的4个位姿, 重新加入堆栈中。当堆栈为空时, 循环结束。同时, 算法会输出已记录的、分数最高的位姿作为结果。

本文对传统CSM算法中的点云降采样处理以及角度步长选取步骤进行了改进, 即使用基于贡献度的方法进行降采样, 并以此计算罗列位姿时所用的角度步长。之后, 在仿真环境与实地环境中进行若干实验, 表明改进后算法的定位成功率与速度优于原CSM算法, 并能在室内大场景下得到应用。

3 基于贡献度改进的CSM算法（The improved CSM algorithm based on contribution)

本文算法的总体流程可分为以下3步:

(1) 借鉴IMLS-SLAM算法^[18]（implicit moving least squares-SLAM）中使用的点云降采样方法, 计算点云内所有点的贡献度。

(2) 借鉴LOAM (lidar odometry and mapping) 算法^[19]中的采样方法, 均匀地提取高贡献度的点, 并根据贡献度计算步骤(3) 中所用的角度步长。

(3) 使用步骤(1) 处理得到的点云和角度步长进行相关性扫描匹配, 求解出机器人的位姿。

3.1 贡献度的设计

在使用CSM算法前, 通常需要对输入点云进行降采样。Cartographer ^[17]是最先使用CSM算法的开源框架, 该框架在此步骤中使用了其自创的自适应体素滤波。然而, 此种体素滤波简单地用1个体素格中的某随机点代替整个体素格内的点云, 由于未考虑点对全局定位中位姿解算的贡献, 因而降低了点云的质量。

受IMLS-SLAM算法的启发, 本文计算点对位姿解算的贡献度, 并以贡献度为基准进行降采样。该方法最先出自文[20], 是为了提高点到面ICP (迭代最近点) 方法的准确性与速度而提出的。点到面ICP方法可以这样描述:

$ E=\sum\limits_{i=1}^{k}\left(\left(\boldsymbol{R} \boldsymbol{p}_{i}+\boldsymbol{t}-\boldsymbol{q}_{i}\right) \cdot \boldsymbol{n}_{i}\right)^{2} $

(5)

其中, $E$是误差, $\boldsymbol{p}_{i}$与$\boldsymbol{q}_{i}$分别是属于点云$P, Q$的匹配点对所对应的位置, $\boldsymbol n_{i}$是点$q_{i}$的法向量（该法向量通过对点$q_{i}$附近的点云簇进行平面拟合得出), $\boldsymbol{R}, \boldsymbol{t}$分别代表点云$P$到点云$Q$的旋转与平移变换。

当旋转矩阵$\boldsymbol{R}$非常小时, 可以用旋转向量$\boldsymbol{r}$代替$\boldsymbol{R}$, 并令$\boldsymbol{r} \times \boldsymbol{p}_{i}+\boldsymbol{t}=\boldsymbol{R} \boldsymbol{p}_{i}+\boldsymbol{t}$, 从而将式(5) 写为

$ E=\sum\limits_{i=1}^{k}\left(\left(\boldsymbol{p}_{i}-\boldsymbol{q}_{i}\right) \cdot \boldsymbol{n}_{i}+\boldsymbol{r} \cdot\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right)+\boldsymbol{t} \cdot \boldsymbol{n}_{i}\right)^{2} $

(6)

由式(6) 可得, 当旋转变换的旋转轴垂直于向量$\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}$, 或者平移变换的方向垂直于$\boldsymbol{n}_{i}$时, 对误差$E$的改变为0。这些变换被称作“无约束变换”, 在ICP方法的迭代过程中, 这些变换会使已经配准好的点云滑动, 造成配准失败。此外, 若$\left|\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right|$过小, 或者$\left|\boldsymbol{n}_{i}\right|$过小, 也会导致优化时误差$E$的改变量过小，不利于点云匹配。

因此, 文[20] 中的研究思路是篮选出$\left|\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right|$与$\left|\boldsymbol n_{i}\right|$较大的点。同时, 该算法的作者希望使全局最小值周围的误差梯度陡峭, 并使局部最小值周围的梯度较平缓, 以允许算法逃逸。当到达全局最小值时, 变换后点云$P$和点云$Q$应当重合, 此时点$p_{i}$的法向量与点$q_{i}$的法向量一致。因此在计算$\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}$与$\boldsymbol{n}_{i}$时, 可以使用点$p_{i}$的法向量$\boldsymbol{n}_{i}^{P}$替换$\boldsymbol{n}_{i}$。

IMLS-SLAM算法中使用的降采样方法是基于上述思路改进而来, 该方法引入了平面几何特征, 将以下9个数值作为点云降采样的依据:

$ \begin{array}{*{20}{l}} {a_{2{\text{D}}}^2\left( {{{\boldsymbol{p}}_i} \times {{\boldsymbol{n}}_i}} \right) \cdot {{\boldsymbol{X}}_{\text{v}}},}&{ - a_{2{\text{D}}}^2\left( {{{\boldsymbol{p}}_i} \times {{\boldsymbol{n}}_i}} \right) \cdot {{\boldsymbol{X}}_{\text{v}}},} \\ {a_{2{\text{D}}}^2\left( {{{\boldsymbol{p}}_i} \times {{\boldsymbol{n}}_i}} \right) \cdot {{\boldsymbol{Y}}_{\text{v}}},}&{ - a_{2{\text{D}}}^2\left( {{{\boldsymbol{p}}_i} \times {{\boldsymbol{n}}_i}} \right) \cdot {{\boldsymbol{Y}}_{\text{v}}},} \\ {a_{2{\text{D}}}^2\left( {{{\boldsymbol{p}}_i} \times {{\boldsymbol{n}}_i}} \right) \cdot {{\boldsymbol{Z}}_{\text{v}}},}&{ - a_{2{\text{D}}}^2\left( {{{\boldsymbol{p}}_i} \times {{\boldsymbol{n}}_i}} \right) \cdot {{\boldsymbol{Z}}_{\text{v}}},} \\ {a_{2{\text{D}}}^2\left| {{{\boldsymbol{n}}_i} \cdot {{\boldsymbol{X}}_{\text{v}}}} \right|,\quad }&{a_{2{\text{D}}}^2\left| {{{\boldsymbol{n}}_i} \cdot {{\boldsymbol{Y}}_{\text{v}}}} \right|,\;\;\;\;\;\;a_{2{\text{D}}}^2\left| {{{\boldsymbol{n}}_i} \cdot {{\boldsymbol{Z}}_{\text{v}}}} \right|} \end{array} $

其中, $p_{i}$是点云中一点, $\boldsymbol{n}_{i}$为其法向量, $a_{2 \mathrm{D}}$是$p_{i}$的平面几何特征数值（关于$a_{2 \mathrm{D}}$的获取方法将在3.2节讨论), $\boldsymbol{X}_{\mathrm{v}}, \boldsymbol{Y}_{\mathrm{v}}, \boldsymbol{Z}_{\mathrm{v}}$是点云坐标系的基向量。这9个量中, 前6个可以帮助提取输入点云中分布于$x$, $y, z$轴两侧的、平行于$x, y, z$轴的平面点, 也可以帮助提取距离点云坐标系中心较远的点, 这些点对求解位姿的旋转部分贡献较高; 后3个量可以帮助提取点云中垂直于$x, y, z$轴的平面点, 这些点对求解位姿的平移部分贡献较高。由于室内环境中有众多墙面, 平面特征较多, 因而利用此方法进行降采样可以提取到较丰富的环境信息。

虽然在降采样步骤中处理的点云为$3 \mathrm{D}$点云, 但后续算法所需点云为$2 \mathrm{D}$点云, 故在降采样步骤结束后, 需要抹除点云的高度信息, 将其转换为$2 \mathrm{D}$点云（可以将此过程想像成把一团3D点云“拍扁”)。若此时输入点云中存在垂直于$z$轴的平面点(例如地面或者天花板上的点), 则这些点将成为转换后$2 \mathrm{D}$点云中的噪点。由于没有高度信息, 因而无法分辨这些点属于墙面、地面或者天花板, 它们会严重影响后续算法的运行。因此本文对这9个贡献度进行了改进, 改进后的贡献度不再把垂直于$z$轴的平面点视作贡献较高的点。改进后的7个贡献度具体如下:

$ \begin{array}{ll} a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{X}_{\mathrm{v}}, & -a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{X}_{\mathrm{v}}, \\ a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{Y}_{\mathrm{v}}, & -a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{Y}_{\mathrm{v}}, \\ a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{Z}_{\mathrm{v}}, & -a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{Z}_{\mathrm{v}}, \\ a_{2 \mathrm{D}}^{2}\left|\boldsymbol{n}_{i} \cdot\left(\boldsymbol{X}_{\mathrm{v}}+\boldsymbol{Y}_{\mathrm{v}}\right)\right| & \end{array} $

3.2 点云贡献度计算的方法

从3.1节的末尾可以看出, 若想计算点云中一点$p_{i}$的贡献度, 需要获取$p_{i}$的法向量$\boldsymbol{n}_{i}$和$p_{i}$的平面几何特征数值$a_{2 \mathrm{D}}$。目前界内已有成熟的方法（例如主成分分析法）可计算$\boldsymbol{n}_{i}$, 因此, 本节主要介绍如何计算$a_{2 \mathrm{D}}$进而计算贡献度, 方法如下:

(1) 定义$V_{p_{i}}^{r}$为原始点云中距离点$p_{i}$不超过$r$的点组成的点云簇。

$ p_{k} \in V_{p_{i}}^{r} \Leftrightarrow\left|\boldsymbol{p}_{i}-\boldsymbol{p}_{k}\right| \leqslant r, \quad k=1, 2, 3, \cdots, K $

(7)

(2) 对原始点云中任意点$p_{i}$, 计算其法向量$\boldsymbol{n}_{i}$。

(3) 对原始点云中任意点$p_{i}$, 令$r=0.5 \mathrm{~m}$, 获取点$p_{i}$的邻近点云簇$V_{p_{i}}^{r}$, 记点$\bar{p}$为$V_{p_{i}}^{r}$的质心, 令$\boldsymbol{M}=\left(\boldsymbol{p}_{1}-\overline{\boldsymbol{p}}, \boldsymbol{p}_{2}-\overline{\boldsymbol{p}}, \cdots, \boldsymbol{p}_{K}-\overline{\boldsymbol{p}}\right)^{\mathrm{T}}$, 计算$\boldsymbol{C}=(1 / K)$. $\boldsymbol{M}^{\mathrm{T}} \boldsymbol{M}$, 其中$\boldsymbol{C}$是$3 \times 3$矩阵。再计算$\boldsymbol{C}$的3个特征值$\lambda_{1}, \lambda_{2}, \lambda_{3} \quad\left(\lambda_{1} \geqslant \lambda_{2} \geqslant \lambda_{3}\right)$, 并令$\sigma_{i}=\sqrt{\lambda_{i}}$, $i=1, 2, 3$, 计算$a_{2 \mathrm{D}}=\left(\sigma_{2}-\sigma_{3}\right) / \sigma_{1}$并记录。$a_{2 \mathrm{D}}$可以表示$p_{i}$的平面几何信息, $a_{2 \mathrm{D}}$的数值越大, $p_{i}$所在的点云簇$V_{p_{i}}^{r}$越接近平面, 并且$0<a_{2 \mathrm{D}}<1$。

(4) 根据所得的$a_{2 \mathrm{D}}, \boldsymbol{p}_{i}, \boldsymbol{n}_{i}$等参数计算改进后的7个贡献度(3.1节)。

此方法可以计算点云中所有点的贡献度。

3.3 基于贡献度的均匀降采样

获取贡献度后, IMLS-SLAM算法将原始点云中所有点分别按照贡献度的大小进行降序排序, 排序后获得列表$L_{1}, \cdots, L_{9}$（注意IMLS-SLAM算法提供了9个贡献度, 故得到9个列表), 选取列表中靠前的点及其邻近点, 一共获得900个点组成降采样后的点云。但是在实际使用时笔者发现, 此种选取方法会使降采样后的点云中的一部分点过于密集, 也可能造成部分贡献度较高但排序靠后的点被滤除。为了使降采样后的点云的分布更加均匀, 本文借鉴LOAM中的方法来选取点, 具体步骤如下:

(1) 如同IMLS-SLAM算法, 分别按照本文提出的7个贡献度, 对原始点云中的每个点进行降序排序, 得到7个列表$L_{1}, \cdots, L_{7}$。

(2) 对其中一个列表$L_{k}$, 设置集合$S_{k}$。从列表的开始位置取出点$p_{i}$, 若$p_{i}$的第$k$个贡献度大于0.4, 且列表中已选中的点个数小于$100, p_{i} \notin S_{k}$, 则选中$p_{i}$, 并把$p_{i}$加入降采样后的点云$P_{\mathrm{f}}$中; 将$p_{i}$与原始点云中距离$p_{i}$小于等于$0.5 \mathrm{~m}$的点加入集合$S_{k}$中; 继续从列表的开头取出点, 重复此步骤直到$L_{k}$为空。

(3) 对所有其他列表执行步骤(2)。

由图 2可得, 在借鉴LOAM算法中的方法进行选点后, 得到的点云更加均匀。图中红色点云是原始点云, 绿色点云是使用IMLS-SLAM算法中的降采样方法处理得到的点云, 黄色点云是本文降采样方法处理得到的点云。

3.4 角度步长选取

原CSM算法使用式(4) 计算角度步长, 该方法保证在搜索过程中点云旋转$\delta_{\theta}$ (角度步长) 后, 其距离雷达坐标系原点最远的点移动的距离不超过$r_{\text {slt }}$ (栅格尺寸), 确保了CSM算法的准确性。但是实际实验发现, 此种方法所得的$\delta_{\theta}$往往过小（通常小于$0.1^{\circ}$), 增大了匹配所需时间。本文对此作出改进, 即应当以对解算偏航角贡献最大的点为基准。与偏航角有关的贡献度为

$ a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{Z}_{\mathrm{v}}, \quad-a_{2 \mathrm{D}}^{2}\left(\boldsymbol{p}_{i} \times \boldsymbol{n}_{i}\right) \cdot \boldsymbol{Z}_{\mathrm{v}} $

在点云降采样步骤结束后, 选出点云中上述2个贡献度最大的点(此点应是对解算偏航角贡献最大的点), 将其距离雷达坐标系原点的距离记为$d_{\mathrm{c} \text { max }}$, 则获得的角度步长为

$ \delta_{\theta}=\arccos \left(1-\frac{r^{2}}{2 d_{\mathrm{c} \max }^{2}}\right) $

(8)

结合上述改进方案及原CSM算法, 可以得出本文改进CSM算法的总体流程, 如图 3所示。其中改进的部分已用绿色标出。

4 仿真与真实环境实验(Experiments in simulation and real environments)

对本文改进后的CSM算法、原CSM算法、Scan Context算法以及SDF法进行了实验对比。

实验着重于定位算法的定位成功率与耗时。根据项目甲方的要求, 若定位结果与位姿真值之间的位移误差小于$0.5 \mathrm{~m}$, 且旋转误差小于$10^{\circ}$, 则将这次定位称作是成功的。此外, 上述定位算法都存在无法给出位姿的情况, 这种情况也会被视作定位失败, 但是无法显示在实验示意图中。

实验数据统计表中的位移误差与旋转误差均为定位成功情况下的误差平均值, 计算公式如下:

$ e_{d}=\frac{1}{n} \sum\limits_{i=1}^{n}\left[\left(x_{\mathrm{GT}}^{i}-x_{\mathrm{m}}^{i}\right)^{2}+\left(y_{\mathrm{GT}}^{i}-y_{\mathrm{m}}^{i}\right)^{2}\right] $

(9)

$ e_{\theta}=\frac{1}{n} \sum\limits_{i=1}^{n}\left|\theta_{\mathrm{GT}}^{i}-\theta_{\mathrm{m}}^{i}\right| $

(10)

$n$为样本个数, $x_{\mathrm{m}}^{i}, y_{\mathrm{m}}^{i}, \theta_{\mathrm{m}}^{i}$是第$i$个位姿测量值的$x$坐标、$y$坐标和绕$z$轴旋转的角度, $x_{\mathrm{GT}}^{i}, y_{\mathrm{GT}}^{i}, \theta_{\mathrm{GT}}^{i}$是第$i$个位姿真实值的$x$坐标、$y$坐标和绕$z$轴旋转的角度, $e_{d}, e_{\theta}$为平均位移误差与平均旋转误差。

实验中的激光雷达数据均使用rosbag工具录制以保证每次算法运行都是相同的输入信息。

实验中全局定位算法所需的栅格地图是预先使用Cartographer算法构建的。Cartographer是Google开源的ROS系统支持的SLAM算法, 可以结合来自多个传感器(例如雷达、轮式里程计、IMU等) 的数据, 同步计算机器人的位置并绘制周围环境的地图。实际操作时, 只需要开启机器人传感器与Google提供的Cartographer程序节点, 让机器人在实验场景游历1遍即可生成地图。Cartographer算法也可以使用仿真数据集中的传感器数据构建地图。由于Cartographer已是成熟且应用广泛的SLAM算法, 其具体使用细节便不再赘述。

4.1 仿真数据集实验

仿真数据集取自北京科源社区18号楼内部, 该区域长约$100 \mathrm{~m}$、宽约$30 \mathrm{~m}$, 属于室内大场景, 并且环境特征稀疏, 适合用来测试算法的可行性。在实验时, 每隔$1 \mathrm{~s}$提取数据集中的1帧点云数据, 一共提取178帧数据, 分别传入各个定位算法中进行定位。原CSM算法的具体定位流程可以参考文[17], 本文改进算法的具体定位流程见图 3。SDF法则将每帧点云数据转为子图并提取其特征点, 再将这些特征点与整张地图的特征点进行匹配, 以找出合适的位姿。Scan Context算法需要将每帧点云转换成描述符, 然后与预先构建的关键帧数据库中的描述符作比较, 来搜索合适的位姿。

实验结果如图 4所示, 其中绿色轨迹代表小车运动的真实轨迹, 蓝色点代表算法解算成功的坐标, 红色点代表算法解算错误的坐标。实验数据统计如表 1所示。

以下是对4种算法在仿真实验中表现的分析:

原CSM算法: 虽然地图场景较大, 但是借由分支定界的思路, CSM算法可以在多分辨率地图的高层就将大多数错误位姿排除在外, 在取得较高定位成功率的同时又兼顾速度。

Scan Context算法：Scan Context是一种基于关键帧的方法。正如2.1节所说, 此类算法在正式使用前, 需要令机器人在使用场景各处游历以采集关键帧, 构建关键帧数据库。在正式使用时, 若机器人位姿与数据库中的某个关键帧所在的位姿相近, 则能很精确地完成全局定位; 反之, 则定位失败。受实验条件限制, 仿真数据集只录制了1条路径, 意味着关键帧数据库的构建与全局定位使用了同一条路径, 故在全局定位时, 算法总能找到相近的关键帧并进行定位, 因此此次试验Scan Context算法成功率较高。且Scan Context算法将当前帧与历史帧进行比较的速度很快, 算法耗时也很短。

SDF法：SDF法属于基于局部与全局环境特征匹配的方法。由于地图场景较大, 且环境特征稀疏, SDF提取的特征点较少, 匹配成功率不佳。而提取特征点的过程本身非常耗时, 算法花费时间也较长。

本文改进算法: 与原CSM算法的点云降采样步骤相比, 改进后算法的点云降采样步骤所得的点云更加稀疏，进行位姿评分时耗时更短。改进后算法所用的角度步长也长于原CSM算法中的角度步长, 可以减少位姿的总个数, 因此总体耗时比原CSM算法更短。本次实验未能体现改进算法在成功率上的优势, 可能是因为地图构建与全局定位使用了同一条路径, 导致定位难度降低。

4.2 轨迹真实值的替代

在进行真实环境实验前, 需要明确机器人位姿真实值的获取方式, 以在真实环境实验中与算法定位结果作比较。SLAM领域内获取机器人位姿真实值的常用方法有2种: RTK (real-time kinematic) 定位法或者激光全站仪定位法。但是实验场景均在地下, GPS信号微弱, 无法使用RTK定位法获取小车轨迹真实值。且场地内有较多的障碍物（如柱子、墙壁等), 当激光全站仪的测量光束被障碍物阻挡时, 便无法获取小车坐标。故本文使用Cartographer算法解算机器人轨迹, 并将其作为机器人轨迹真实值的代替。为保证解算所得轨迹的准确性, 额外设计实验对Cartographer算法解算所得位姿的位移与旋转误差分别进行评估。

4.2.1 位移误差实验

本节使用莱卡激光跟踪仪对Cartographer算法所求位移的误差进行评估。将激光跟踪仪的反射器(靶球) 放置在巡检机器人顶部。机器人在空间中移动时, 激光跟踪仪会自动追踪靶球的位置, 获取机器人的空间坐标。激光跟踪仪的绝对测距精度可达$0.01 \mathrm{~mm}$。由于巡检机器人只在2维平面上移动, 所以只考虑$x$轴和$y$轴的坐标。

实验场地选择中国计量大学现代科技学院的地下停车场。于入口处架设激光全站仪, 在激光全站仪的可视范围选取20个点, 于每一点处使用Cartographer算法解算小车坐标, 同时使用激光全站仪获取小车真实坐标。将两坐标之间的欧氏距离视作该点的位移误差。实验所用的机器人和设备如图 5所示。实验结果如图 6所示。该图分别描述了激光全站仪与Cartographer算法各自得到的坐标, 以及坐标之间的两两误差。

实验结果表明, Cartographer算法解算的坐标误差的最小值约为$0.075 \mathrm{~m}$, 最大值约为$0.248 \mathrm{~m}$, 平均值约为$0.137 \mathrm{~m}$, 能够作为真实坐标的代替。

4.2.2 旋转误差实验

本节使用北微电子罗盘对Cartographer算法所求的旋转误差进行评估。为了防止机器人铁质机身干扰电子罗盘, 将电子罗盘置于巡检机器人前方的木条上, 且电子罗盘坐标系的$x$轴与机器人坐标系的$x$轴同向。电子罗盘可以获取机器人的绝对旋转信息, 精度可以达到$0.2^{\circ}$。

实验场地依旧选择中国计量大学现代科技学院的地下停车场。于车库内均匀选取42个点。于每一点处使用Cartographer算法解算小车的旋转信息, 同时使用电子罗盘获取小车真实的旋转信息, 将两者旋转度数之差视作该点的旋转误差。实验机器人及设备如图 7所示。

实验结果如图 8所示。该图分别描述了电子罗盘与Cartographer算法各自得到的旋转信息, 以及旋转角度之间的两两误差。

实验结果表明, Cartographer算法解算的旋转误差的最小值为$0.035^{\circ}$, 最大值为$7.394^{\circ}$, 平均值为$2.680^{\circ}$, 能够作为真实坐标的代替。

综上, 实验证明Cartographer算法解算所得的位姿信息能作为小车位姿真实值的代替。

4.3 真实环境实验

本节使用机器人底盘、工控机与$3 \mathrm{D}$激光雷达等设备搭建机器人作为实验平台。选取中国计量大学地下车库作为实验场地。使用Cartographer算法所得的机器人轨迹近似替代机器人的轨迹真实值。

4.3.1 实验平台与环境介绍

实验所用机器人以深圳重器公司MR1000移动底盘为基础, 使用北京纵横科技公司BPC-4501L工控机作为车载运算单元, 并搭配VLP-16激光雷达。以上设备的参数分别如表 2~4所示。

作为实验场地的中国计量大学地下车库长约$100 \mathrm{~m}$, 宽约$40 \mathrm{~m}$, 属于室内大场景, 且环境特征稀疏，适合用来测试算法的可行性。

4.3.2 定位成功率与耗时对比实验

真实环境的实验过程与仿真实验时类似，令机器人在实验场景中运行, 每隔$1 \mathrm{~s}$获取1帧雷达点云数据, 共获取464帧数据, 传入定位算法中进行定位, 将计算所得位姿与真实位姿作比较。实验结果如图 9所示, 其中绿色轨迹代表小车运动的真实轨迹, 蓝色点代表算法解算成功的坐标, 红色点代表算法解算错误的坐标。正如第4节开头所描述的, 改进前后的CSM算法都存在无法给出位姿的情况, 这种情况虽然会被视作定位失败, 但是不会显示在此实验结果示意图中。实验数据统计如表 5所示。

以下是对4种算法在真实环境实验中表现的分析。

原CSM算法：与仿真实验类似，原CSM算法的成功率较高, 但是真实环境场景比仿真实验时的场景大，算法耗时相对增加。定位失败的地点均处于拐角处, 可能是由于机器人在此处快速转弯, 致使记录的点云畸变严重, 从而影响定位。

Scan Context算法: 与仿真实验中不同，真实环境实验中，关键帧数据库构建与全局定位使用的是2条不同的路径。因此全局定位时, 机器人通常不在关键帧附近, 定位成功率较低。

SDF法: 定位成功的地点多处于场景左侧, 这是因为场景左侧存在大量车辆, 算法在此处能提取到更多特征点进行匹配, 提高了算法在此处的定位成功率。但是整体来看, $\mathrm{SDF}$法的定位成功率与耗时均不理想。

本文改进算法: 相较原CSM算法的自适应体素滤波, 本文算法采用的基于贡献度的点云降采样方案能处理得到更稀疏的点云, 并且基于贡献度计算的角度步长大于原$\mathrm{CSM}$算法使用的角度步长, 因此改进后算法的速度更快。在保证平均距离误差与平均旋转误差相近的情况下, 本文改进算法相较于原CSM算法, 成功率提高了约1.5%, 平均耗时缩短了约$1.1 \mathrm{~s}$, 更加适合室内大场景的全局定位。

5 结论(Conclusion)

改进了CSM算法, 并使用改进后的算法解决机器人在环境特征稀疏的室内大场景下的全局定位问题。研究中, 首先通过计算点云内点对位姿解算的贡献度, 以贡献度为基准对输入点云进行均匀降采样, 从而改进了传统CSM算法中的点云降采样部分; 同时, 选取对偏航角解算贡献最大的点来指定CSM算法中所用的角度步长。在真实物理环境实验中, 模拟了室内大场景的环境, 实验表明本文算法在定位成功率和定位耗时上的表现优于使用传统CSM算法、Scan Context算法和SDF法的定位方案。