基于非对称激光雷达的自适应蒙特卡洛定位算法

2020-10-09王志红

自动化与仪表 2020年9期

王志红，王奔

（1.武汉理工大学现代汽车零部件技术湖北省重点实验室，武汉430070；2.汽车零部件技术湖北省协同创新中心，武汉430070）

定位是机器人要解决的核心问题。机器人的定位问题主要涉及到三点，即位置跟踪、全局定位以及绑架问题。常用的算法是MCL 的各种衍生算法，主要分为两大类：（1）对地图进行离线处理，如文献[1]为了加强初始时匹配的成功率，对地图进行了膨胀处理；（2）提高采样效率，如文献[2]使用自适应KLD 采样提高重采样效率。

自适应蒙特卡洛定位算法（SAMCL）[3]结合了以上两种思路，使用了一种预处理（Pre-caching）的离线处理方法，降低实时计算的负担。此外，SAMCL 限定粒子重采样的范围以提升采样效率，并将粒子分配为2 组来处理定位失效的问题。

SAMCL 的缺点是要求距离传感器必须按照中心对称排布，影响了该算法的实用性。文献[4-5]提出了基于椭圆能量栅格的SAMCL，解决了多个非对称传感器应用SAMCL 的问题，并基于ROS 进行了测试。

本文结合二维激光雷达提出一种SAMCL 改进算法（下称iSAMCL），使其能够应用在非对称分布的单个传感器，以视角为270°的激光雷达为例进行说明，并基于ROS 进行仿真与实验。

1 自适应蒙特卡洛定位算法（SAMCL）

算法总体架构如图1所示。SAMCL 算法包含预处理、计算SER、定位3 个过程。其中预处理离线进行，另外两个步骤实时进行。SAMCL 针对传统的MCL 算法主要提出了3 点改进：

（1）采用了离线的地图预处理方法，减少实时计算量。预处理的主要思想是：预测地图上每个栅格处的一组距离测量值｛d1，…，dN｝，根据式（1）计算出预测能量值E，保存在栅格中。保存了能量信息的地图称为能量地图，记作GE。在需要获取粒子的权重时，每个粒子从它所在的栅格中获取能量值，避免了实时计算能量。

（2）提出等能量区域（similar energy region，SER）的概念，限定重采样区域，提高采样效率。运行时机器人上安装的激光雷达得到测量值，按照式（1）计算得到，称为真实能量值。SER 是GE的子集，表示E 与接近的栅格的集合，因此在SER中进行重采样能够提升效率。

图1 自适应蒙特卡洛定位算法架构Fig.1 Architecture of SAMCL algorithm

图2 自适应蒙特卡洛定位算法Fig.2 Flow chart of SAMCL algorithm

2 改良的自适应蒙特卡洛定位算法（iSAMCL）

算法流程上iSAMCL 与SAMCL 相同，包含在线与离线两个阶段。离线部分对地图进行预处理，在线部分通过粒子滤波实时计算更新机器人位姿，两个算法的主要区别在于预处理及SER 的计算方法。

2.1 预处理

SAMCL 规定了只能使用中心对称的传感器，假设地图如图3（a）所示，使用视角360°的激光雷达。当机器人在图中黑色圆点所示的位置时，预测激光雷达的测量数据如图3（b）所示。因此在预处理时，每个栅格根据式（1）计算得到一个能量值E。

若使用视角270°的激光雷达，激光雷达可能测量到的数据如图3（c）所示。此时如果机器人原地转向，雷达数据改变，计算得到的能量值E 也会变化，因此同一个栅格对应了多个E 值。

图3 地图与预处理Fig.3 Pre-caching of map

iSAMCL 在预处理过程中，将方向维度划分为离散的空间，算法将同一位置不同方向的能量值计算出来，则生成三维能量栅格地图，记作G3D。这一步离线进行。

地图预处理算法如算法1所示，算法的输入是二维地图中的某个栅格的坐标（x，y）。第2 行中I 表示将方向维度划分为I 等份，激光雷达的光束数量用N 表示，可视角度为β。例如激光雷达的视角为270°，且每帧包含540 个距离数据，则N=540，I=540×360°/270°＝720，将方向维度720 等分。第3～4行表示，如果当前单元格未知或者被占用，则能量为1。第5～6 行表示该栅格与障碍物的距离小于机器人半径R，能量为1。第8 行根据式（1）计算能量，得到的结果是区间（0，1］之内的数。

算法1：地图预处理算法

2.2 等能量区域的计算

SER 是G3D的一个子集。SER 的计算方法如算法2所示，前4 行根据真实光束的数值计算机器人当前的真实能量值Eˆ，此后，与每个栅格（x，y，θ）中能量对比，若Eˆ与G3D（x，y，θ）相差在δ 范围内，则将栅格标记为SER。

算法2：SER 计算算法

借助ROS 使用图3（a）所示的地图进行测试，SER 的计算结果如图4所示。由于SER 的栅格含有x，y，θ 三个维度的坐标，在二维地图中，使用箭头来表示，箭头末端表示x，y，箭头指向表示θ。图4（a）是能量值为0.95 时的SER，可以看到其覆盖区域略小于地图。图4（b）是能量为1.0 时的SER，可以看到与图4（a）相比，面积大幅缩小了，若在该区域中进行采样，效率将会大大提升。对应地，图4（c）与图4（d）分别为能量为0.95 与1.0 时，在SER 中进行采样得到的粒子。

图4 等能量区域以及等能量区域中的采样Fig.4 SER & sampling in SER

2.3 定位

iSAMCL 实时定位算法如算法3所示，主要分为5 个部分。

第二部分（第6～10 行）：确定用于全局定位与用于局部定位的粒子数量。这一步根据判定是否需要划分粒子。α 为分配系数，可以根据需要进行调整。根据α 计算出用于全局定位的粒子数量NG与用于局部定位的粒子数量NL。

第三部分（第11～14 行）：局部定位的粒子进行重采样。这一步是对局部粒子的操作，与传统的MCL 算法相同。开始时，对权重wt进行归一化，重采样基于wt进行。

第四部分（第15～18 行）：计算SER 与全局粒子采样。从SER 中按照均匀分布采样，获得全局定位的粒子。

算法3：iSAMCL 算法

3 实验

3.1 实验平台

实验基于EAIBOT Dashgo D1 机器人搭建平台，传感器采用了Hokuyo UTM-30LX-EW 激光雷达，其可视角度为270°，可测量的最大距离为30 m。计算平台为HP OMEN-15 笔记本电脑，其搭载6 核心12 线程Intel Core i7 8750H 处理器，操作系统为Ubuntu 16.04 LTS，整个系统外观如图5（a）所示。

算法使用Python 在ROS 中实现。实验室真实环境如图5（b）所示，使用的地图如图5（c）所示。三维栅格地图的分辨率是0.05 m×0.05 m×0.5°。

图5 实验室真实环境、地图Fig.5 Environment of lab & map

本文进行了两项实验。第一项实验的目的是测试位置跟踪能力。第二项实验测试的是机器人从绑架问题中恢复的能力。

由于SAMCL 不支持270°的激光雷达，无法在完全相同的条件下进行iSAMCL 与SAMCL 的对比，因此本实验无法得知iSAMCL 与SAMCL 在定位性能上的区别，实验结果将与里程计以及文献[3]中给出的数据进行对比，以说明iSAMCL 在270°视角激光雷达上的可行性。

3.2 位置跟踪实验

实验过程中，使用键盘控制小车在实验室中运行并分别记录定位算法给出的位姿以及里程计给出的位姿，并使用ROS 中的可视化工具rviz 实时观察，过程如图6所示。

实验中记录的轨迹如图7所示，其中实线是由iSAMCL 产生的轨迹，而虚线是由里程计产生的轨迹。机器人到达终点后，使用激光测距仪测出机器人在环境中的实际位置，与算法给出的位置对比，得出x，y，θ，三个维度的误差并记录。进行20 次实验，得到的平均误差如表1所示。文献[3]中给出的位置跟踪实验平均误差如表2所示。对比可以看出，定位算法的误差基本处于相同的数量级内，而本实验由于实验室环境较小，因此显得里程计漂移较小。这一结果能够说明iSAMCL 在位置跟踪问题上的可行性。

图6 位置跟踪实验过程Fig.6 Position tracking experiment

图7 里程计与iSAMCL 轨迹对比Fig.7 Comparison between tracks of odometer and iSAMCL

表1 SAMCL 绑架恢复最终位置平均误差Tab.1 Average error of final position for iSAMCL

表2 SAMCL 绑架恢复最终位置平均误差Tab.2 Average error of final position for SAMCL

3.3 绑架问题实验

绑架问题的测试方法为当机器人准确获取自身的定位后，人为地将机器人搬离原有位置一段距离，造成里程计的失效，测试定位算法能否恢复正确位置。

图8 展示了机器人从绑架问题中恢复的过程。2 s 时刻发生绑架，此时发生绑架，粒子的权重分散，导致了的减小，算法分出粒子在SER 进行采样，粒子显得较为分散。如图8（a）所示。通过几轮迭代，粒子逐渐确定了机器人的正确位姿，如图8（b）（c）所示。使用激光测距仪测量机器人最终位置，并与定位算法给出的位置进行比较，记录误差。