杜 晨, 杜 煜

(1.北京联合大学 北京市信息服务工程重点实验室，北京 100101；2.北京联合大学 机器人学院，北京 100101)

0 引 言

作为机器人自主定位导航技术的关键，同时定位与地图构建(simultaneous localization and mapping，SLAM)技术也正成为关注焦点[1,2]。移动机器人的SLAM技术包括对机器人的状态和环境信息的同时估计[3]。

Rao-Blackwellized粒子滤波器(Rao-Blackwellized particle filter,RBPF)是解决SLAM技术的方法之一。经过多年的发展也出现了很多改进办法，国内的王田橙[4]、国外PopovlG[5]等人都在不同程度上对RBPF-SLAM算法做出了改进。

本文在RBPF-SLAM方法的基础上，将采样集中在观测信息的可能性区域，减少了与目标分布的误差，使得采样的粒子更加符合真实环境状态，建立更准确的地图环境；并且引入分层重采样优化策略，通过控制阈值，维持尽可能多的粒子多样性，有效地解决了粒子退化问题。最后，为了验证算法的有效性，以Bulldog移动机器人平台为基础，开展了相关实验。并针对实验平台，优化了扫描处理步骤，加快了扫描处理时间，有效降低了实验运行时间。

1 基于RBPF-SLAM算法的优化

1.1 RBPF-SLAM原理

RBPF-SLAM问题可理解为:初始地图m0与初始位姿x0已知的情况下，从最初到时刻t的传感器观测信息z1:t=z1,…,zt，与移动机器人里程计的控制信息u1:t-1=u1,…,ut-1，来估计p(x1:t,m|z1:t,u1:t-1)，即机器人轨迹x1:t=x1…,xt与地图m的联合后验概率分布。通过贝叶斯滤波器可以对其进行分解

p(x1:t,m|z1:t,u1:t-1)=p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t-1)

(1)

RBPF-SLAM算法采用粒子滤波器来估算机器人的位姿，其中，每一个粒子代表一条可能的轨迹。包含4个主要步骤：采样、权重计算、粒子重采样、地图更新。在权重计算阶段可以通过限制概率密度函数来进行

(2)

式中η=1/p(zt|z1:t-1,u1:t-1)为贝叶斯定律中归一化因子，所有粒子拥有相同的η。

1.2 优化的RBPF-SLAM

1.2.1 融合观测信息的提议分布

机器人在实际移动过程中，激光雷达作为机器人的外部传感器具有较高的精度，似然函数区域比较集中；而基于里程计的运动模型，因里程计自身误差较大，似然函数区域比较分散，导致了两者之间重合的地方很少。

融合观测信息[6]，将采样集中在观测信息的可能性区域。可以得到关于粒子权重方差的最佳提议分布，减少与目标分布的误差，使得采样的粒子更加符合真实环境状态，建立更准确的地图环境。此时的提议分布

进一步更新式(2)，得到

在似然函数的峰值进行采样，可以得到化简之后的权重计算

1.2.2 重采样优化策略

引入有效粒子数Neff来估计当前粒子集代表目标函数的近似程度

为解决粒子退化问题[7，8]，本文引入了一种分层重采样优化策略。首先设置采样粒子高权重阈值ωk=2/N和低权重阈值ωL=1/(2N)，其中N代表粒子数。根据设定的阈值，把粒子权值分为3个区间，较高权值区间、较低权值区间和中等权值区间。其中，对中等权值区间的粒子不做处理，对具有较高和较低权值区间的粒子按权值大小由高到低进行排序，之后再计算每个粒子被选择的概率

p(i)={a+[rank(i)/(Nh+Nl-1)]·

(b-a)}/(Nh+Nl)

(7)

式中 rank(i)为粒子根据权重排序后的序号，a,b为系数，Nh和Nl分别为高权重粒子数和低权重粒子数。

1.2.3 优化RBPF-SLAM算法流程

1)首先，估计机器人初始位姿；2)使用迭代最近邻ICP算法进行点云匹配；3)进行采样，计算目标分布，进而求得提议分布；4)在融合观测信息的提议分布中采样求得新粒子集，计算粒子权重；5)进行优化重采样策略更新粒子权值；6)地图更新。

2 移动机器人系统

2.1 移动机器人硬件平台

本实验平台为长90 cm、宽70 cm 、高80 cm的Bulldog机器人，实物图如图1(a)所示。

本实验选取Rslidar激光雷达传感器以及里程计进行实验。Rslidar激光雷达集合了16个激光收发元器件，可以按照设定的频率进行扫描。里程计能够记录机器人走过的路程以及方向等信息。其内部存在一个用串口线连接的PC，外部PC连接Bulldog的路由，通过SSH协议实现通信。

Bulldog轮式机器人包含感知模块、决策模块和底盘控制模块等。当控制层对Bulldog轮式机器人进行控制时，感知层中的传感器会接收到自身以及周围场景的信号。之后，将信号节点发布到内部PC中，外部PC连接Bulldog的路由，实现与内部PC的通信。在订阅节点信息的同时，对这些数据进行处理，根据各自算法的需求，来完成相应的实验。

2.2 移动机器人软件系统

机器人操作系统(robot operating system,ROS)是移动机器人研究的基础，ROS的提出推动了机器人产业的发展。系统框架分为:节点(node)、节点管理器(master)、消息(message)、话题(topic)。其中，节点是核心，可以执行简单的程序语句[9]。

内部PC为Linux(Ubuntu14.04)上运行的ROS，主要节点简化图如图1(b)所示。

图1 移动机器人及ROS的主要节点

在ROS框架下，激光雷达传感器用激光雷达节点来实现激光雷达数据帧的读取，由/scan话题完成激光数据的发布工作。里程计节点得到里程计信息，由/odom完成里程计信息的发布工作。所有话题都会发布到公共主题当中，等待其它节点订阅。ROS中还提供了一个可视化工具，可以实现实时在线地图构建的显示。

由于实验平台实现的功能较简单，仅在移动机器人已经行进了给定距离后才开始处理扫描数据，随着扫描数据的增多，运行时间会加大。因而，在扫描处理上也做出了优化改进，增加了限定时间的扫描处理条件，以便更加快速的处理扫描。

3 实验环境与分析

3.1 实验环境与结果

选取由简单到复杂的两个实验场景，分别用传统RBPF-SLAM算法即Gmapping算法、其它改进RBPF-SLAM算法[7]以及本文优化的RBPF-SLAM算法进行实验。

实验场景一：近似地图为80 m×2 m，环境比较简单，包括长走廊，电梯口等，真实场景如图2(a)所示。三种算法使用相同的粒子数，构建地图结果如图2(b)、(c)、(d)所示。

图2 三种算法分别构图结果

图2(b)、(c)、(d)是移动机器人从A点到B点再到C点使用不同算法在RVIZ上显示的构图情况。选取的栅格大小同样为5 cm×5 cm。从构建地图的最终结果来看，随着时间增加，图2(b)中会出现边缘地方不一致性现象，如图中标出的地方；图2(c)和图2(d)构图效果较好，可以明显地看出D点凹凸的轮廓。

在场景一中分别进行5次实验，计算这两种算法创建相同一致性地图时，需要的平均运行时间。实验时，机器人平均速度为0.4 m/s，改进算法需要258 s，本文优化的RBPF-SLAM算法需要232 s，比改进算法更快。

实验场景二：近似地图为6 m×8 m，存在一些障碍物，如办公桌椅、书柜、长桌等，如图3(a)所示。同样使用三种算法进行对比实验。查看构建一致性地图时，三种算法使用粒子数的情况。图3(b)是本文优化的RBPF-SLAM算法构建的地图结果。

图3 实验场景及本文算法构图结果

当构建的地图结果中A点、B点和C点都能清晰看出轮廓时，记录三种算法使用粒子数的情况，传统RBPF-SLAM算法使用38个粒子，改进RBPF-SLAM算法使用20个粒子，本文优化的RBPF-SLAM算法只需要18个粒子。

3.2 实验结果分析

从构建地图结果中可以看出，当实验环境较简单(实验场景一)，使用相同粒子数时，其它研究者的改进算法以及本文优化的RBPF-SLAM算法构图结果更加可靠，地图出现不一致性现象的情况较少。说明将采样更集中在观测信息的可行性区域，使得采样的粒子更加符合真实环境的状态，可以改善地图构建不一致性。而构建相同精度的地图时，与改进RBPF-SLAM算法相比较，本文优化的RBPF-SLAM算法所需要的平均运行时间更短，算法的效率更高。

当实验环境复杂一些(实验场景二)，构建相同清晰度的地图时，本文优化的RBPF-SLAM算法使用的粒子数更少，有效地降低了算法的计算量。

为了对算法的粒子多样性进行验证，同样用三种算法在使用相同30个粒子时，查看重采样过程中粒子多样性变化情况，如图4所示。

图4 重采样过程中粒子多样性变化

由图4可以看出：本文优化的RBPF-SLAM算法，在重采样过程中，不同粒子数明显多于传统的RBPF-SLAM算法；与改进的RBPF-SLAM算法相比较，粒子多样性的波动范围会更大一些。也说明了优化算法不仅可以缓解粒子退化现象，而且粒子的多样性更好一些。

4 结 论

本文优化的Rao-Blackwellized粒子滤波的同时定位与地图构建算法，在提议分布中融合了激光雷达的观测信息，将采样集中在观测信息的可能性区域，减少了与目标分布的误差，提高了准确度，有效地减少了地图不一致性。而通过控制权值的阈值，维持尽可能多的粒子多样性的分层重采样优化策略，有效缓解了粒子退化现象。而进一步优化的扫描处理，增加了限定时间的扫描处理条件，加快了处理速度，降低了运行时间。下一步工作，尝试在更大的实验环境下验证优化RBPF-SLAM算法的鲁棒性。