基于激光雷达传感器的RBPF-SLAM系统优化设计

王依人， 邓国庆， 刘 勇， 张 文，2

(1.中国科学院 合肥物质科学研究院 应用技术研究所，安徽 合肥 230031; 2.中国科学技术大学，安徽 合肥 230026)

基于激光雷达传感器的RBPF-SLAM系统优化设计

王依人1,2， 邓国庆1， 刘 勇1， 张 文1，2

(1.中国科学院 合肥物质科学研究院 应用技术研究所，安徽 合肥 230031; 2.中国科学技术大学，安徽 合肥 230026)

针对传统Rao-Blackwellized粒子滤波(RBPF)方法的同步定位和建图(SLAM)问题，提出了一种基于激光雷达的RBPF-SLAM系统优化方法，利用高精度激光雷达数据，修正了基于里程计读数的建议分布函数，减少了滤波过程所需的粒子数目；引入了自适应重采样机制，缓解由于重采样带来的粒子消耗问题。为验证改进算法性能，在搭建的差速型移动机器人平台上，进行了验证试验，结果表明：改进后的RBPF-SLAM方法，能够实时构建栅格地图，在建图效率和精度上均有明显的提升。

激光雷达传感器; 同时定位与建图(SLAM); 机器人; Rao-Blackwellized粒子滤波(RBPF)

0 引 言

室内自主移动机器人具有广泛的应用前景，其实现自主移动需同时具备环境建图、自主定位、路径规划、自主避障、运动控制等能力，而实现同步定位和环境建图(simultaneous localization and mapping,SLAM)是核心问题，也是当前机器人领域的研究热点。

机器人SLAM，概念最早由Smith R,Self M,Cheeseman P提出[1]，指机器人在未知环境中，借助传感器感知环境，以构建环境地图，同时确定自身在环境中所处位置的方法。Hammersley J M等人提出了基于粒子滤波的SLAM方法[2]，利用状态空间中一组带权值的粒子，拟合机器人系统状态的后验概率分布，但由于计算量大，该方法发展缓慢。20世纪初，Murphy K等人提出了基于Rao-Blackwellized粒子滤波(Rao-Blackwellized particle filtering,RBPF)方法[3]，Montemerlo M等人又将RBPF应用于SLAM问题中[4，5]RBPF-SLAM，使得粒子滤波的SLAM方法计算量大大减少，促进了SLAM技术的发展。国内，哈尔滨工业大学历茂海、洪炳荣等人提出了自适应的重采样的RBPF-SLAM方法[6]；祝继华等人，改进了粒子采样过程中的建议分布函数及重采样方法[7]，缓解了SLAM过程中的粒子消耗问题。

本文在RBPF-SLAM方法的基础上，利用高精度的激光雷达传感器数据，修正基于里程计读数的建议分布函数，获得了更为有效的粒子集合。同时，引入自适应重采样机制，有效缓解粒子滤波过程中的粒子消耗问题。为验证算法的有效性，以Hands Free开源项目为模型，搭建了机器人平台并实现本文算法，并开展了相关实验。

1 室内移动机器人系统

1.1 移动机器人硬件平台与传感器系统

以Hands Free机器人为模型，搭建了差速型移动机器人平台，系统结构如图1所示。主要包括机械结构部分、传感器部分、控制部分组成，包含电机驱动器、控制器、传感器、电池等模块。系统中激光雷达、编码器与嵌入式实时控制器连接，嵌入式控制设备与上位机连接。各模块信息处理通过上位机实现，实时获取传感器数据并进行处理。

图1 移动机器人系统结构

RBPF-SLAM系统，主要使用激光雷达和里程计数据。其中，里程计通过编码器记录机器人轨迹。激光雷达属于外部传感器，测量数据包含被观测环境的结构化信息。文中选取激光雷达RPLIDAR A1，其原理如图2所示，可实现360°，6 m范围内的激光测距扫描，扫描频率最高可达10 Hz，分辨率达到1°。原始激光雷达数据用极坐标(d,θ)表示， 转换为笛卡尔坐标

(1)

图2 RPLIDAR A1工作原理

1.2 机器人操作系统与软件实现

在Ubuntu14.04操作系统下，基于机器人操作系统(ROS)环境构建软件系统。算法软件实现系统，通过节点获取传感器数据，并将数据传输到定位与地图构建模块中，通过地图节点进行定位和建图，并借助可视化软件.rivz，实时显示机器人移动过程中构建的环境地图，其软件系统如图3所示。

图3 SLAM系统软件实现框架

2 基于RBPF-SLAM方法优化

2.1RBPF-SLAM系统原理

(2)

式中 函数π(·)为建议分布函数。RBPF-SLAM算法中，为简化计算一般以基于里程计读数的运动模型为建议分布函数，计算公式如下

(3)

(4)

传统的RBPF-SLAM算法的基本思想是[8,9]：根据观测信息z0∶k={zk|k=1,…,k},控制输入信息u1∶k={uk|k=1,…,k},估计机器人的运动轨迹的后验概率分布p(x0∶k|z0∶k,u1∶k,n1∶k)，然后推算机器人系统状态的后验概率分布p(x0∶k,m|z0∶k,u1∶k,n1∶k)，表示为

p(x0∶k,m|z0∶k,u1∶k,n1∶k)=p(x0∶k|z0∶k,u1∶k,n1∶k)

(5)

式中 右边第一部分采用粒子滤波估计机器人运动路径；第二部分采用扩展Kalman滤波估计路标位置。

2.2 RBPF-SLAM系统优化

机器人在实际运动过程中，机器人运动模型误差相对于观测模型误差较大，似然概率与先验概率分布之间重叠较小，如图4所示。此时，根据基于运动模型的建议分布函数重采样的RBPF-SLAM方法，存在粒子多样性退化和重要粒子丢失的问题。针对上述情况，提出了利用高精度的激光雷达数据修正机器人运动模型，优化建议分布函数。同时引入自适应重采样机制，缓解了由于重采样产生的粒子消耗问题。

图4 观测方程和运动方程的似然函数分布

2.2.1 建议分布函数优化

由于里程计易受外部环境干扰，将鲁棒性较强的激光雷达数据引入到建议分布函数中，修正基于里程计读数的机器人运动方程，获得改进的建议分布函数。改进后的建议分布函数为

(6)

代入粒子权重计算公式(3)可得

(7)

将激光雷达数据加入建议分布函数中，主要根据激光雷达的前、后2帧扫描数据进行匹配，获得机器人的运动参数，修正依靠里程计数据估计的机器人运动模型。本文通过迭代最近点(iterative closed point，ICP)算法[10]进行扫描匹配，表示为

(8)

2.2.2 自适应重采样

滤波过程中，引入有效粒子数Neff[11,12]估计当前粒子集对目标函数的近似程度，有效粒子数Neff的大小反映粒子集权值的方差大小，表示为

(9)

为保持粒子多样性，减少粒子退化，引入了一种自适应的重采样方法，该方法的思想为：重采样过程中，对集中具有较高权值和较低权值的粒子进行采样，保留具有中等权值的粒子。具体步骤为：

1)根据文献[13，14]中提出的相关方法，设置采样粒子高权重阈值wh=2/N和低权重阈值wl=1/(2N)。根据粒子的重要性权值与设置的阈值大小关系，将样本粒子分为高、中、低三部分。

2)计算重采样过程中各样本粒子被复制的次数ni。中等权重粒子保留，高权重粒子和低权重粒子按权重由高到低排序。计算每个粒子被选择的概率为

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

(Nh+Nl)

(10)

式中 rank(i)为粒子根据权重排序后的序号；a,b为系数，当粒子数确定时，a=2-b,1≤b≤2，取值为

(11)

如果第i个粒子被选择的概率p(i)*=1/(Nh+Nl)≥p(i)，则该粒子被复制，复制次数ni为

(12)

根据该算法执行重采样，如果粒子被复制的总次数∑n(i)<(Nh+Nl)，则权重由高到低的增加粒子被复制次数，最终使得∑ni=Nh+Nl；如果粒子被复制的总次数∑n(i)>(Nh+Nl)，则依次减少权重由低到高的粒子被复制次数，保证∑ni=Nh+Nl的总数不变。

2.3 改进的RBPF-SLAM算法流程

4)根据改进的建议分布，采样获取新的位姿粒子集。

5)更新粒子的重要性权值。

6)根据设定的粒子重采样条件判断，是否执行重采样计算。

3 实验分析

为探究改进后的RBPF-SLAM方法性能，开展算法验证实验。本文自主搭建的机器人平台如图5所示，所述算法在计算机(处理器：IntelCore i5—4200U;RAM:4.00 GB)Ubuntu14.04操作系统环境下，基于ROS框架环境编程实现，其中激光雷达RPLIDAR A1安装于机器人平台顶部，里程计位于平台内部，分别开展基本建图实验和对比试验。

图5 机器人平台

1)基本建图

首先在图6(a)所示的模拟环境中，开展建图实验。实验构建栅格地图初始设置为200 m×200 m，算法中滤波定位部分，样本粒子数目经过多次试验后设置为30，效率和建图效果最佳。

实验结果表明：机器人在地图中移动，可以根据激光雷达和里程计数据，构建满足自主机器人导航的栅格地图，图6(b)所示。

图6 搭建地图及构建的栅格地图

2)算法对比试验

实际情况下，机器人工作环境较搭建地图复杂，环境特征更为丰富，且存在较多不确定因素干扰，例如移动的物体、行人等。为了验证此类情况下改进算法的有效性，分别采用传统RBPF-SLAM方法和本文改进算法进行定位和建图试验。

构建的办公区域栅格地图如图7所示，其中图7(a)和7(b)分别为改进算法构建的部分走廊、办公区域的栅格地图，图7(c)和7(d)为传统RBPF-SLAM方法构建的栅格地图。从试验结果可知：传统RBPF-SLAM方法，构建的走廊及办公区域栅格地图出现失真。而改进的RBPF-SLAM方法在障碍物较多、结构化特征不明显的复杂环境中时，也能够对运动中的机器人位姿进行准确定位，构建出准确的栅格地图，同时地图精度明显高于传统方法。

图7 栅格地图

4 结 论

开展了室内移动机器人同时定位和环境建图方法研究，基于激光雷达进行RBPF-SLAM方法改进，根据高精度激光雷达数据，修正基于里程计读数的建议分布函数，减少了滤波过程所需的粒子数目；同时引入了自适应重采样机制，缓解了由于重采样带来的粒子消耗问题。为验证改进算法性能，进行建图和与传统方法对比实验，结果表明：改进后的RBPF-SLAM系统，能够构建更加准确的环境地图。

[1] Smith R,Self M,Cheeseman P.Estimating uncertain spatial relationships in robotics[C]∥Proceedings of 1987 IEEE Interna-tional Conference on Robotics and Automation,North-Holland:1987:850.

[2] Hammersley J M,Morton K W.Poorman's Monte Carlo[J].J of the Royal Statistical Society B,1954,16(1):23-38.

[3] Murphy K,Russell S.Rao-Blackwellized particle filtering for dynamic Bayesian networks,sequential MonteCarlo methods[M].Lorne,Australia:Springer-Verlag,2001:499-515.

[4] Montemerlo M,Thrun S,Koller D,et al.FastSLAM:A factored solutionto the simultaneous localization and mapping pro-blem[C]∥Proceedings of the AAAI National Conference on Artificial Intelligence,Menlo Park,CA,USA:AAA I,2002:593-598.

[5] Montemerlo M,Thrun S,Koller D,et al.FastSLAM 2.0:An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]∥Proceedigns of the Sixteenth International Joint Conference on Artificial Intelligence(IJCAI),Menlo Park,CA,USA:AAAI,2003:1151-1156.

[6] 历茂海，洪炳荣，罗荣华.用改进的Rao-blackwellized粒子滤波器实现移动机器人同时定位和地图创建[J].吉林大学学报，2007,37(2):402-406.

[7] 祝继华,郑南宁,袁泽剑,等.基于ICP算法和粒子滤波的未知环境地图创建[J].自动化学报,2009,35(8):1107-1113.

[8] Giorgio Grisetti,Cyrill Stachniss,Wolfram Burgard.Improving grid-based SLAM with Rao-Blackwellized particle filters by adaptive proposals and selective resampling[C]∥IEEE International Conference on robotics and Automation(ICRA),2005.

[9] Giorgio Grisetti,Cyrill Stachniss,Wolfram Burgard.Improved techniques for grid mapping with Rao-Blackwellized particle filters[J].IEEE Transactions on Robotics,2007,23:34-46.

[10] Besl P J,Mckay N D.Method for registration of 3D shapes[J].IEEE Transactions on Pattern Analysis & Machine Intelligence,1992,14(2):239-256.

[11] 沈林成，徐 昕，朱华勇,等.移动机器人自主控制技术[M].北京：科学出版社，2011:248-269.

[12] 张建伟，张立伟，胡 俊,等.开源机器人操作系统ROS[M].北京：科学出版社，2012.

[13] Bolic M,Djuric P M,Hong S.New resampling algorithms for particle filters[C]∥IEEE International Conference on Acoustics,Speech,and Signal Processing,HongKong: IEEE，2003:589-592.

[14] 罗 元,苏 琴,张 毅,等.基于优化RBPF的同时定位与地图构建[J].华中科技大学学报:自然科学版,2016,44(5):30-34.

Optimal design of RBPF-SLAM system based on LIDAR sensor

WANG Yi-ren1,2, DENG Guo-qing1, LIU Yong1, ZHANG Wen1,2

(1.Institute of Applied Technology,Hefei Institutes of Physical Science,Chinese Academy of Sciences, Hefei 230031,China; 2.University of Science and Technology of China,Hefei 230026,China)

Aiming at problem of simultaneous localization and mapping(SLAM)of traditional Rao-Blackwellized particle filtering(RBPF)method,an optimization method of RBPF-SLAM system based on laser radar(LIDAR)is proposed.This method uses high precision LIDAR data to correct the proposed distribution function based on the odometer readings and reduce number of particles required for filtering process.At the same time, the adaptive resampling mechanism is introduced to alleviate the problem of particle consumption due to resampling.In order to verify the performance of the improved algorithm,a method validation test is carried out on the platform of the differential mobile robot.The results show that the improved RBPF-SLAM method can build the gridding map in real time,and it can improve the efficiency and precision of mapping.

laser radar(LIDAR)sensor; simultaneous localization and mapping(SLAM); robot; Rao-Blackwellized particle filtering(RBPF)

10.13873/J.1000—9787(2017)09—0077—04

2017—07—13

TP 242.6

A

1000—9787(2017)09—0077—04

王依人(1990-)，女，硕士研究生,研究方向为计算机视觉。