基于3D 点云地图和描述子的蒙特卡罗定位方法∗

吴官东 宋勇磊

（南京理工大学 南京 210094）

1 引言

位姿跟踪是机器人领域中最为重要的技术之一。移动机器人在自主导航过程中需要实时感知自身位姿。在未装配GPS 的机器人上，或处于GSP无法有效工作的环境中，位姿跟踪是实现机器人实时定位的主要方式。它通常是指在已知机器人上一时刻位姿的条件下，借助控制指令及机器人自身携带的传感器感知信息推断当前时刻机器人所处的位置及朝向。

粒子滤波（Particle Filter，PF）［1］是卡尔曼滤波的非参数实现，它使用大量样本实现对概率密度的离散近似。蒙特卡罗定位（Monte Carlo Localization，MCL）［2］，作为粒子滤波在机器人定位领域的应用，是目前最为流行的位姿跟踪算法。蒙特卡罗定位算法是一个循环迭代过程，每次迭代过程中以旧的粒子群、控制信息、观测信息和环境地图作为输入，经过位置预测、观测更新以及重采样三个步骤，最终输出更新后的粒子群。

传统的蒙特卡罗定位［3～6］需要机器人提供里程计和单线激光雷达的实时感知信息，同时需要提供机器人所处环境的二维占据栅格地图（Occupancy Grid Map，OGM）［7］，该地图通常使用Gmapping［8］、Cartographer［9］等2D 同时定位与地图创建（Simultaneous Localization And Mapping，SLAM）算法建立得到。随着光电行业的发展，多线激光雷达的价格降至民用标准，在自主导航机器人上得以普及，通常作为必备传感器之一。在装配有多线激光雷达的机器人上使用传统蒙特卡罗定位，通常仅使用多线中水平射出的单线作为算法的输入，这种方式浪费了n线激光雷达的(n-1)/n的信息，因此在室外环境中经常出现位姿跟踪丢失的现象。

与此同时，以多线激光雷达传感器作为数据来源的3D SLAM 技术也得到了快速的发展。Zhang等［10］提出了以点云的角点特征、平面特征的匹配作为帧间配准技术的LOAM 算法，该算法能够用于激光里程计或建立环境的3D 点云地图。在此基础上，又产生了如Lego-LOAM［11］、LIO-SAM［12］等改进算法，进一步提升了算法的实时性和生成地图的一致性。3D SLAM 生成的全局点云地图正逐渐应用于机器人定位和导航技术中。

点云描述子是对空间中某一点周围一定半径邻域内所有点云结构信息的描述和概括，其在保持差异性特征的前提下尽可能地压缩信息的维度。全局描述子用一个低维的数据结构描述整帧激光雷达点云。常见的描述子的设计方式为，首先将扫描点云所在的空间以一定的形式划分为众多子空间，然后使用特征统计或直方图的方式对子空间内点的结构信息进行描述，最后合并所有子空间的描述形成描述子。Cop 等［13］提出的DELIGHT 全局描述子首先使用三个相互正交的平面将三维空间等分为8 个子空间，使用两个同心球面再次划分，形成16 个子空间，然后利用直方图对每个子空间内点云的强度信息进行统计，最后拼接所有直方图形成描述子。由于直方图统计过程较为耗时，该描述子在实时性方面表现较差。Kim 等［14～15］提出了2.5D 的Scan Context 描述子，对空间进行径向和朝向上的划分，使用子空间点云的最大高度作为特征，形成矩阵形式的描述结构。该描述子具有较好的实时性，但仅适用于扫描与扫描之间（Scan-to-Scan）的匹配。

为了充分利用多线激光雷达的感知信息，提高机器人导航过程中位姿跟踪的鲁棒性，本文对传统的蒙特卡罗定位方法进行了改进，提出了新的观测模型。本文首先提出了全新的适用于扫描与局部点云地图之间（Scan-to-Submap）相似度匹配的周围环境二值占据状态（Binary Occupancy Status of Surroundings，BOSS）描述子，并设计了基于快速位运算的描述子相似度计算方法。通过引入基于BOSS 描述子匹配的观测模型，提出了更加鲁棒的BOSS-MCL位姿跟踪方法。

2 方法介绍

2.1 BOSS描述子

为了实现点云信息压缩，本文设计了全新的BOSS 点云全局描述子。BOSS 描述子在不丢失障碍物位置及体积信息的前提下，尽可能地压缩原始点云的数据量，同时提供了Scan-to-Submap 匹配的能力。BOSS描述子既可以用于描述单帧扫描点云，也可以用于描述局部点云地图（在全局点云地图中截取的半径等于雷达最大感知距离的球形子点云）。该描述子的设计思想是对点云空间进行三个维度上的划分，统计每个子空间的二值占据状态，形成三维的布尔数组。

为了使得生成的BOSS描述子不受地面及传感器倾斜的影响，同时降低计算量，需要对输入点云进行预处理。预处理过程包含4 个步骤：首先使用RANSAC［16］算法拟合地面，得到地面的高度和法向量；然后对点云中所有点执行高度校正和倾斜校正，使得地面点接近水平且高度为0；接着使用直通滤波去除地面点；最后对点云执行体素网格滤波降采样。

设P为经过预处理后的点云，其所覆盖球形范围的半径为Rlidar。对点云所在的空间进行三个维度的划分：在方位维度上将空间划分为S个扇形区域，每个扇形的圆心角为Asector=2π/S；在径向维度上将空间划分为C个同心圆环，每个圆环的环宽为Wcircle=Rlidar/C；在高度维度上将空间划分为F层，Hmin和Hmax分表表示最小、最大高度的参数，则每层的高度为Hfloor=(Hmax-Hmin)/F。将三个划分维度相交的子空间称为箱，最终点云空间被不等分为N=S×C×F个箱。式（1）中Pijk表示第i环、第j个扇区和第k层相交区域对应的箱中的所有点：

箱与箱之间紧密无缝毗邻且不相交，点云中的每个点属于且仅属于一个箱，因此如式（2）所示所有箱中的点的并集即为原点云。

式（3）中的函数Ψ(∙)用于描述箱所覆盖的子空间的信息。本文选用二值化的方式，设定阈值t，当箱内点数小于t时，将箱标记为空闲状态，当箱内点数大于等于t时，将箱标记为占据状态。此特征提取方式能够使得描述子生成和匹配在O(n)时间复杂度内完成，从而保证了定位方法的实时性。

对所有箱进行特征提取，按照箱与箱在空间中的位置关系排列所有二值占据状态值，形成布尔类型的三维数组形式的BOSS 描述子（如式（4）所示）。其中，数组中的每行对应一个环，每列对应一个扇区，每个二维切片对应一层。

2.2 快速描述子相似度计算

本文设计的BOSS描述子相似度计算方法主要用于Scan-to-Submap，即用于估计实际扫描点云与局部点云地图之间的匹配度。局部点云地图，作为全局点云地图的区域截取，包含更丰富的环境信息。即使从相同的位姿采集，实际扫描和局部点云地图之间也存在很大的差异。图1（a）、（b）展示了同一场景下两者之间的区别。因此，如图1（c）、（d）所示，实际扫描和局部点云地图生成的描述子也存在很大差异。

图1 点云和BOSS描述子可视化

BOSS描述子设计之初就是用来解决现有描述子不具有Scan-to-Submap 能力的问题。由于保留了原点云中障碍物的相对位置及体积信息，BOSS描述子可以做到实际扫描到局部点云地图的单向匹配。具体思想是，对于实际扫描描述子中某个占据状态的箱，判断局部点云地图中对应位置的箱是否也为占据状态。

由于箱只有空间和占据两种状态，因此可以使用计算机中的一位（bit）进行表示，以减小描述子数据的内存占用。同时，使用计算机中的位运算完成相似度计算，以缩短耗时。

正式计算前，如式（5）所示，将扫描帧BOSS描述子和局部点云BOSS 描述子均转换为32 位无符号整型（uint32）数组表示方式。

函数ℎw(x) 用来计算十进制数x的汉明重量（Hamming Weight），即统计x的二进制形式中1 出现的次数。对x执行(x-1)x操作，能使x的二进制形式最后一处的1变为0。使用循环统计x不断执行该操作直至变为0 的操作次数，即可得到的x汉明重量。

通过式（6）可以快速计算出Scan-to-Submap的相似度。该式分子统计了实际扫描描述子所有占据状态的箱中，对应的局部地图描述子的箱也为占据状态的个数；通过除以用于统计实际描述子占据状态箱数量的分母，将相似度标准化至[0,1] 。

2.3 基于描述子匹配的观测模型

观测模型在蒙特卡罗定位方法中用于计算粒子的重要性权重。从输入输出的角度来看，观测模型输入位姿粒子、观测数据以及地图，输出该粒子的概率值。

本文设计的基于描述子匹配的观测模型的基本思想是将观测数据即多线激光雷达扫描帧转换为BOSS描述子，然后以粒子位置为中心，从全局点云地图中截取与雷达感知距离等半径的局部点云地图，转换为BOSS描述子，计算两个描述子之间的相似度作为粒子的重要性权重。

为了降低观测模型的计算耗时，提升单位时间内处理粒子的数量，需要对全局点云地图预采样，生成局部点云地图描述子采样集。

在全局点云地图M覆盖的平面区域，每隔w米取一个采样点，得到采样点集S={si} 。

接着对局部点云地图进行预处理，生成BOSS描述子，并使用cvt(∙)函数转换为uint32数组形式：

最后，将“描述子· 位置”二元组加入采样集中。

对所有采样点执行上述处理后，得到的描述子采样集如式（9）所示。

在观测模型运行期间，设当前时刻的观测为zt，一个表示机器人位姿的粒子为

首先如式（10）生成激光雷达观测点云对应的BOSS描述子。为了保证实际扫描和全局地图之间的方位一致性，使用rot(∙,col)函数将描述子循环右移col列。

最后，如式（11）计算两个描述子之间的相似度，作为观测模型最终输出的概率值。

2.4 BOSS-MCL定位方法

将基于BOSS描述子匹配的观测模型应用到传统蒙特卡罗定位方法中，从而形成了基于已知全局3D 点云地图和多线激光雷达传感器的BOSS-MCL定位方法。BOSS-MCL 定位方法分为部署阶段和运行阶段。

在部署阶段中，首先需要操纵机器人运行3D SLAM 算法，建立工作环境的全局点云地图；然后在全局点云地图覆盖的平面范围内进行等距离采样，生成采样点集；最后以所有采样点为中心截取局部点云并生成BOSS描述子，得到描述子采样集，并以文件的形式存放于磁盘中。

机器人开机后，首先将描述子采样集加载至内存，用户手动指定机器人的初始位姿，初始粒子群由以初始位姿为中心的高斯分布随机生成。本文选用里程计采样模型作为BOSS-MCL 的运动模型。在运行阶段，运动模型获取里程计的增量信息，对粒子的位姿进行更新；感知模型以激光雷达的实时感知数据为输入，完成粒子重要性权重的计算；重采样模块从旧粒子群中根据权值随机采样，并在粒子群发散时根据KLD 规则增加一定数量的随机粒子。此三个步骤不断循环迭代，实现机器人自主导航过程中的实时位姿跟踪。

3 实验与分析

3.1 实验平台和场地

本文使用的实验平台为一辆小型轮式无人车，如图2 所示。该无人车采用四轮阿克曼结构的底盘，能够提供实时的前轮转向角度和后轮速度信息，通过积分能够转换为里程计。在底盘上方安装了以下设备：Intel Celeron J1900（1.99GHz× 4）CPU和4G内存的工控机，为无人车提供计算能力；Velodyne HDL-32E 多线激光雷达，用于环境感知；Trimble BD982 卫星-惯导组合定位系统（GNSS/INS），用于提供真实位姿信息。

图2 无人车实验平台

如图3 所示，本文的实验场地为校园内的一个环形道路（图中白色虚线）。选择环形道路有助于SLAM 建图时完成回环检测，从而生成一致性地图。

3.2 实验方法

本文使用C++语言和点云库（Point Cloud Library，PCL）实现了BOSS-MCL 定位方法，并集成至机器人操作系统（Robot Operating System，ROS）中。通过和ROS官方的AMCL节点对比，验证本文提出的BOSS-MCL方法的有效性和改进性。

在实验过程中，首先，分别使用2D Cartographer 和Lego-LOAM 算法建立环境的占据栅格地图（如图4所示）和全局点云地图（如图5所示）；然后，使用rosbag 命令，手动遥控车辆在实验环境行驶一圈，录制包含单线激光（多线激光雷达的水平线束）扫描话题、激光点云话题消息和里程计话题消息的数据序列；最后，以栅格地图和单线数据为输入测试AMCL 定位效果，以点云地图和多线数据为输入测试BOSS-MCL定位效果。

图4 占据栅格地图

图5 全局点云地图

本文实验以差分（RTK）GNSS/INS 设备的位姿输出作为机器人的真实位姿（Ground Truth），精度为厘米级。使用相同数据序列，分别对比AMCL 和BOSS-MCL在低粒子数量、高粒子数量下位姿跟踪的跟踪轨迹、位置误差、航向误差。

为了保证对比实验的公平性，粒子滤波器、里程计模型、激光观测模型均使用ROS 默认的参数。全局点云地图描述子采样的步长为w=0.2；BOSS描述子相关参数如表1所示。

3.3 实验结果分析

图6 展示了实验过程中机器人的真实轨迹、纯里程计积分轨迹，以及不同粒子数量范围配置下的AMCL 位姿跟踪轨迹、BOSS-AMCL 位姿跟踪轨迹。对比图6（a）和 图6（b）可见，仅依靠里程计进行位姿推算会产生极大的累积误差；图6（c）和图6（d）展示了将粒子数量动态范围设置为200～500 时两种方法的跟踪轨迹，该实验结果表示在粒子数量较多的情况下，AMCL 和BOSS-MCL 均能够顺利完成整个行驶过程的实时定位，均未出现粒子发散及定位飘移的现象；为了验证粒子数量对定位效果的影响，将粒子数量缩减至20～50，在较少粒子的情况下，如图6（e）所示，AMCL 方法在第二个转弯处开始出现定位偏移，行驶至坐标（-16，-15）时，粒子群发散导致位姿跟踪失败，而BOSS-MCL在粒子较少时依然顺利完成了全程的位姿跟踪，该实验结果表示，在粒子数量较少的情况下，BOSS-MCL 凭借信息更加丰富的3D点云地图和多线激光雷达感知数据，依然能够保持稳定的位姿跟踪表现。

图8 绘制了两种方法在不同粒子数量配置下，定位位置误差和定向航向误差随着时间的变化曲线。图8（b）中虚线的两个波峰（时间220s、260s处），表示AMCL 方法在粒子数量设置为200～500，无人车行驶至第2 个和第3 个转弯处时，出现较大的定向误差。如图7（a）、（b）所示，由于转弯位置较为空旷，AMCL 单线激光数据与地图匹配出现了较大误差，对比图7（c）、（d）展示的BOSS-MCL 在转弯位置的匹配表现，可以得到的结论是，本文提出的BOSS-MCL方法在相对空旷的环境下，能够取得比AMCL更好的匹配效果。

图7 转弯处匹配情况图

图8 误差-时间曲线图

对比图8 中两种方法位置误差和航向误差随时间变化的趋势，可以明显看出，AMCL 方法的误差会随着定位时间的推移逐渐增加，这体现了在环境感知信息量不足的情况下，匹配程度适中的粒子取得更高的权值，最贴近真实位姿的粒子逐渐被淘汰，出现了“劣粒子驱逐良粒子”的现象。BOSSMCL的定位误差随着时间的推移，一直处于一定的范围内，体现了其粒子重采样后的平均权值的稳定性。

表2 展示了基于二维信息的AMCL 和基于三维信息的BOSS-MCL，完成单个粒子重要性权重计算所需要的时间，以及BOSS-MCL方法单帧点云数据预处理平均耗时。数据表明，BOSS-MCL通过快速位运算，单个粒子权重计算耗时相比AMCL 减少了65%。BOSS-MCL 在增加粒子匹配信息量的同时，降低了粒子权重更新耗时。BOSS-MCL需要对每帧点云做预处理，考虑综合可得，当粒子数量大于300时，BOSS-MCL具有更好的实时性。

表2 计算耗时统计表

4 结语

本文通过设计BOSS激光点云全局描述子和基于描述子匹配的观测模型，对传统蒙特卡罗方法进行了改进。该观测模型充分利用了多线激光雷达的感知信息完成粒子重要性权重的计算，从而减少了因误匹配导致粒子群发散的可能性，提高了定位的鲁棒性；信息量的增加使得维护较少数量的粒子依然能够保持稳定的位姿跟踪；借助快速位运算降低了粒子权重更新耗时，提高了实时性。综上，本文提出的BOSS-MCL 位姿跟踪方法具有较高的实用价值。