黄 辉， 邹安安， 胡 鹏， 邹媛媛， 蔡庆荣

(五邑大学 智能制造学部，广东 江门 529020)

随着科学技术和人工智能技术的蓬勃发展，人们对科技产品的追求意愿也在愈渐增大。与此同时，汇集多种技术的移动机器人的发展进入到一个高潮。需求推动科技产品的发展，在具体应用环境中，如灾难现场、地下电缆等恶劣环境需要使用移动机器人代替人工进行检测和探索。移动机器人能完成这些工作的前提是能感知并认识环境，并能确定自身处于环境的位置。SLAM(Simultaneous Localization and Mapping，同时定位与地图创建)[1]是移动机器人完成上述任务的基本前提，也是移动机器人实现自主的关键。

SLAM为移动机器人在部分已知或未知环境中利用载有的传感器感知环境并同时建图和定位，Smith和Chesseman最先于1987年提出这一概念[2],国内外研究者共同推动了它的发展。如今，主要的SLAM研究方法有基于图优化的方法和基于概率估计的方法。但在研究SLAM问题时，由于机器人具有很多不确定性，研究者更习惯把SLAM当作一个状态估计问题，产生了基于概率估计的SLAM流派。扩展卡尔曼滤波(Extended Kalman Filter,EKF)和粒子滤波(Particle Filter，PF)方法常被用于SLAM状态估计中，由此产生了EKF-SLAM和PF-SLAM算法。对于服从高斯分布噪声的系统，EKF是以高斯函数来表示系统的概率密度函数，对于非线性模型的系统，EKF将非线性模型泰勒展开近似为线性模型，因此存在线性化误差，在强非线性系统中效果很差。而PF是以一定数量的样本粒子来近似系统概率密度分布[3]。粒子集若具有足够的粒子数，就能表示任何复杂的概率分布，完美适用于非线性、非高斯系统的概率估计。

Doucet和Mur-phy等于1999年提出Rao-Blackwellized粒子滤波SLAM(RBPF-SLAM)[4]，借鉴粒子滤波的思想，通过具有一定权重的粒子来估计机器人位姿和环境地图特征。粒子估计值与真实值越接近，粒子权重就越大，在迭代中就容易被保留下来；反之，估计值与真实值相差太远的粒子，权重越小，更易在迭代中被舍去。粒子数目越多，与真实位置接近的粒子也越多，RBPF-SLAM算法定位、构图就越精确，但是粒子数量的增多会加大算法的计算量并影响实时性[5-8]。

本文首先基于kobuki底盘和RPLIDAR A2雷达搭建机器人系统，针对常规RBPF-SLAM算法建图精度差的不足，改进其建议分布，采用高精度的扫描帧匹配算法，增加自适应采样操作，在已搭建好的机器人平台中进行建图实验，验证了优化RBPF-SLAM算法的实用性。

1 移动机器人总体框架

1.1 硬件框架

基于分层控制的理念，搭建了差速型机器人平台，将机器人硬件系统架构分为3层：底层，中层和上层。底层为运动控制层，包括kobuki底盘、STM32控制板，带编码器的直流电机，执行导航规划层发送的控制指令，并将测得的机器人里程数据反馈给导航规划层，提供定位信息；中层为导航与规划层，包括装有ROS(Robot Operating System)的TX2开发板和主传感器RPLIDAR A2雷达，收集机器人本体传感器数据和激光传感器数据，并对这些数据进行融合处理，计算出机器人位姿信息并根据相应规划任务向底层发出控制指令；上层为人机交互层，包括装有ROS的PC机，实时显示机器人运动环境信息和自身状态信息。中层与底层通过串口通信，上层与中层通过无线局域网通信。

图1 移动机器人硬件框架

1.2 ROS系统

在ubutu16.04系统中，搭建ROS软件系统[9]，/Base_link是公共话题，节点在此进行信息交互。里程计节点、激光节点、机器人节点分别记录里程计信息、激光信息和键盘控制信息，并将记录的信息发布到/Base_link主题；定位节点、地图构建节点订阅/Base_link主题，获取所需的里程计信息和激光测距信息，通过/Rosaria节点控制机器人完成环境地图构建、定位等操作，通过Rviz可视化平台实时显示地图和机器人位置信息。

图2 机器人及ROS的主要节点

2 RBPF-SLAM算法原理

2.1 基于粒子滤波的RBPF-SLAM算法

RBPF-SLAM的目的是给出激光观测数据p(zt|xt,m)和里程计数据p(xt|xt-1,ut-1)，估计机器人运动轨迹x1∶t和地图m的联合后验概率p(x1∶t,m|z1∶t,u1∶t-1)。将联合后验概率分解为

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

(1)

从而将SLAM问题分解为两个后验概率的乘积。式(1)等号右侧第一部分为粒子的后验路径，通过粒子滤波来估计后验路径p(x1∶t|z1∶t,u1∶t-1)。每个粒子都有一个潜在的运动轨迹，这代表了机器人一条可能的路径，同时生成自己的地图。

式(1)等号右侧第二部分为粒子生成的后验地图，采用EKF估计地图特征，每个粒子都有自己单独的地图。使用占据栅格地图算法来生成二维平面占用栅格图，地图m被划分为有限多个栅格单元mi，每一栅格单元有占据和空闲两种状态。栅格有障碍物用占据(Occupied状态)表示；栅格无障碍物用空闲(Free状态)表示。在占据栅格地图中，用p(mi)=0表示栅格空闲状态概率，p(mi)=1表示栅格占据状态概率，两者概率和为1。假设栅格地图中每个矩形栅格相互独立，因此全局地图m的占用概率为各单元格占用概率之积。后验地图近似为栅格单元边缘的乘积。

(2)

2.2 RBPF-SLAM算法流程

RBPF-SLAM算法步骤如下。

② 权重估计。为了更好地估计建议分布，减少与目标分布的误差，需要计算每个粒子的重要性权重。

(3)

③ 重采样。根据设定的阈值对系统粒子进行重采样。重采样后，所有粒子具有相同的权重。

3 RBPF-SLAM改进

3.1 改进提议分布

获得下一时刻的粒子集，需要从建议分布π抽取样本粒子。建议分布和目标分布越接近，算法估计的最终结果越好。传统RBPF-SLAM算法采用里程计运动模型作为建议分布，该运动模型计算简单，根据观测模型计算重要性权重

(4)

因为地面不平、轮子打滑等影响，里程计数据的准确性较低，建议分布采用里程计模型采样时样本只能覆盖状态空间区域的一小部分，导致粒子之间权重差异大，得到的有效粒子数少，如图3所示。传统RBPF-SLAM算法执行时，由于有效粒子数少需多次重新采样，导致粒子耗散且多样性降低，影响算法性能。针对这些问题，考虑精度更高的激光传感器数据，通过匹配前后两帧激光数据获取机器人的运动参数，修正机器人里程计运动模型。将其定位在一个更精确的似然区域，从而采样更多有效粒子。

图3 粒子似然区域

因此，将激光数据zt融合到建议分布中，采样集中在观测可能性更高的区域，能够得到采样粒子权重方差的最佳建议分布。由此产生的建议分布为

(5)

使用该建议分布时，权重计算公式为

(6)

选用迭代最近点算法(Iterative Closest Point,ICP)对激光点云数据进行扫描匹配。设当前帧点集为P={p1,p2,…,pm}，对应参考点集为Q={q1,q2,…,qn}，分别包含m个点和n个点，求解两组点云之间的变换T(T={R,t})，使式(7)得到最优解。

(7)

式中，Qi、Pi分别为参考点集和当前点集在笛卡尔坐标系下第i对邻近点的二维坐标；N为成功配对的点对数。

3.2 自适应重采样

重采样是RBPF-SLAM算法的重要步骤。在进行粒子重采样时，用高权值的粒子替代低权值的粒子。但是，在重采样过程中有可能会滤除效果不错但权值不高的粒子，这会导致有效粒子数减少、粒子退化。因此，引入有效粒子数Neff[10]来决定是否有必要执行重采样操作，它表示粒子集的退化程度，其值越大说明粒子集多样性越好，越小则粒子退化越严重。有效粒子数计算公式为

(8)

图4 优化的RBPF-SLAM算法流程框图

4 仿真和实验

4.1 建图环境与结果

选取实验环境为两个不同大小区域，如图5、图6所示，在开源的ROS中进行RBPF-SLAM建图实验。首先PC机远程连接处于同一局域网中的移动机器人机载工控机，启动底盘和激光雷达，同时启动手动控制，以恒定速度控制机器人扫描环境一周，扫描过程中利用激光数据和里程计数据更新环境地图和机器人位姿信息。常规RBPF-SLAM算法和优化的RBPF-SLAM算法构建的地图如图7、图8所示。

图5 环境1

图6 环境2

图7 环境1地图

图8 环境2地图

实验环境1为10 m×5 m的办公区域，常规的RBPF-SLAM算法构建的栅格地图出现扭曲、失真的现象，而优化后的RBPF-SLAM算法构建的栅格地图精度更高，地图中障碍物轮廓更清晰，且地图无扭曲失真现象。

实验环境2为3 m×4 m的小房间，常规的RBPF-SLAM算法由于回环错误，构建的栅格地图出现重叠的现象，而优化的RBPF-SLAM算法，则回环成功，地图轮廓分明，能更好地表示环境。

4.2 结果分析

对比2种方法的运行时间和机器人在地图中的定位误差，分别在环境1、环境2各进行10次实验，记录运行时间并在Rviz中查看机器人在地图中的定位误差，如表1所示。改进的RBPF-SLAM算法的运行效率以及地图平均误差、最大误差都要优于常规RBPF-SLAM算法，创建的地图也更加清晰精确，轮廓分明，表明改进的RBPF-SLAM算法构建的地图精度更优。

表1 不同算法定位误差比较

5 结束语

利用激光雷达、kobuki底盘搭建了差速型机器人平台，为机器人搭建了ROS软件操作系统。从RBPF-SLAM的基本定义入手，研究了基于栅格地图的RBPF-SLAM算法，在常规RBPF-SLAM算法的基础上，改进其建议分布函数，对点云采用迭代最近点匹配，并增加自适应重采样操作，得到最优粒子，构建了更加逼近环境的真实地图。在ROS系统中基于现场环境实时建图，对比运行时间和机器人定位误差，结果表明优化后的RBPF-SLAM算法能以更高的运行速度构建精度更高的栅格地图。移动机器人的分层设计理念能提高机器人各功能模块的复用性，改进的RBPF-SLAM算法为移动机器人导航提供了条件。