结合退火优化和遗传重采样的RBPF算法

孙弋 张笑笑

摘 要：RBPF是一种有效解决同时定位和建图的算法。传统的RBPF算法使用的粒子数目多并且频繁地执行重采样，导致粒子退化且估计能力下降，从而构建的栅格地图精度不高。针对上述缺点，对RBPF提出优化，首先将机器人的运动模型与观测模型结合作为其混合提议分布，同时利用退火参数优化混合提议分布，调控两者在提议分布中的比例，使其更加精确;其次在重采样过程中根据粒子的权值对其进行分类，对高权重以及低权重粒子引入自适应遗传算法变异交叉操作，减少了重采样次数，有效维持了粒子多样性。在MATLAB上进行仿真验证，同时结合了Kobuki运动底盘在机器人操作系统（ROS）上进行实际验证。实验结果表明，与传统的RBPF算法相比，算法能够使用更少的粒子精确估计出机器人的位姿及路标，能够建立精度更高的栅格地图，并且具有更低的均方根误差和计算时间。

关键词：粒子滤波;RBPF算法;提议分布;重采样;交叉变异

中图分类号：TP 242

文献标志码：A

文章编号：1672-9315（2020）02-0349-07

DOI：10.13800/j.cnki.xakjdxxb.2020.0222开放科学（资源服务）标识码（OSID）：

RBPF algorithm based on annealing optimization

and genetic resampling

SUN Yi，ZHANG Xiao-xiao

（College of Communication and InformationEngineering，Xian University of Science and Technology，Xian 710054，China）

Abstract：RBPF is an algorithm that effectively solves simultaneous positioning and mapping.The traditional RBPF algorithm uses a large number of particles and performs resampling frequently，resulting in particle degradation and reduced estimation ability，so that the constructed raster map is not accurate.In view of the above shortcomings，the RBPF is optimized.Firstly，the motion model of the robot is combined with the observation model to be its mixed proposal distribution.At the same time，the annealing parameters are used to optimize the mixed proposal distribution，and the ratio of the two in the proposed distribution is adjusted to make it more accurate.In the process of resampling，the particles are classified according to the weight of the particles，and the adaptive genetic algorithm mutation crossover operation is introduced to the high weight and low weight particles，which reduces the number of resampling and effectively maintains the particle diversity.Simulation verification was performed on MATLAB，together with the Kobuki motion chassis to conduct actual verification on the robot operating system（ROS）.The experimental results show that compared with the traditional RBPF algorithm，the proposed algorithm can accurately estimate the pose and road signs of the robot using fewer particles，and then establish a higher precision raster map with lower root mean square error and less calculating time.

Key words：particle filter;RBPF algorithm;proposed distribution;resampling;cross mutation

0 引 言

近年來，智能移动机器人技术得到飞速发展，已经应用到矿井、安防、家庭服务等领域[1-3]，让机器人来替代人类完成那些重复的、枯燥的、危险的甚至是人类不能完成的工作成为社会发展的趋势[4-5]。机器人发展逐渐智能化和自动化[6-7]，随着人工智能技术的发展，机器人在辅助人们完成各种任务时，需要具有良好的定位、建图和路径规划的能力[8-9]。机器人定位与建图问题是相辅相成、不可分割的，即同时定位与建图（Simultaneous Localization and Mapping，SLAM） [10-12]。它将机器人定位与建图合为一体，为今后机器人的导航奠定基础。

早期SLAM技术的研究大部分都是基于概率理论的扩展卡尔曼滤波算法[13-14]。近年来，粒子滤波器已广泛用于机器人领域SLAM问题的解决，对此已经进行了许多研究[15-17]。

Murphy，Doucet等引入RBPF作为解决SLAM问题的有效手段[18-20]。Rao-Blackwellized粒子滤波器（RBPF）比扩展卡尔曼滤波器（EKF）更广泛地用于概率估计机器人的位置[21-22]和环境地图构建，与EKF SLAM相比，RBPF在多测量数据关联时具有更强的稳定性，所以当数据发生错误关联的时候RBPF SLAM会得到比EKF SLAM更好的结果[23-24]。

Murphy等将RBPF算法作为一种新的方法来处理SLAM问题[25]。

SLAM问题可分解为地图估计以及位姿估计2部分。使用记录的测距和激光扫描数据估计机器人的位置，然后再次使用该位置和激光数据来更新地图。但是仍然存在由于粒子分集损失而造成的估计性能下降、频繁重采样导致粒子多样性下降等缺点。此后，有许多改进算法被提出：郑兵等提出一种融合萤火虫算法的Rao-Blackwellized粒子滤波器RBPF同步定位与地图构建优化算法[26]。利用萤火虫算法改善粒子采样过程，一定程度上保证了粒子的多样性。但对于精度的提高效果不佳。张毅等采用了一种基于高斯分布重采样的RBPF-SLAM算法[27]。根据粒子权重对粒子进行分类，利用高斯分布分散高权重粒子得到新粒子，虽然在较少粒子下得到可靠估计，但对于低权重粒子未作处理，对于缓解粒子退化还存在不足。王田橙等通过区域粒子群方法优化提议分布，让各个区域中的离散粒子都向中心的高似然的位置进行移动[28]。对于密集粒子部分保持不变，使其精度得到提高，但对于增加粒子多样性效果不佳。

针对上述存在的问题，对于RBPF-SLAM算法存在提议分布精度低以及重采样次数多导致粒子多样性减少的问题，对RBPF提出改进。一方面结合机器人的运动模型以及观测模型作为混合提议分布，同时使用退火参数来调控2种模型在混合提议分布中的比例，以提高提议分布的精度。另一方面，对于重采样次数太多造成粒子退化问题根据粒子的权重对其进行分类，对部分粒子，引入自适应遗传算法交叉变异操作，产生新粒子，减少重采样次数，并维持了粒子的多样性。提出的改进算法能够在较少的时间内利用更少的粒子获得更加可靠的位姿估计，构建高精度的栅格地图，从而更有效地进行路径规划。

1 RBPF-SLAM的基本原理

SLAM主要是依据传感器的观测数据

Z1∶t以及机器人里程计数据

u1∶t

去估计联合后验概率密度函数

p（X1∶t，m|Z1∶t，u1∶t-1）.使用贝叶斯过滤将公式分为2个过程：预测和观察，分别对应2个模型：运动模型p（xt|xt-1，ut-1和观测模型p（zt|xt，m）.根据从机器人获得的输入数据控制移动机器人的运动模型，或者计算机器人编码器的当前姿势和最后时刻的相对值，陀螺仪运动检测传感器数据，计算机器人的最后时刻定位结果作为模型输入，获得机器人定位的先验概率分布。观测模型基于由激光雷达等传感器和移动机器人上的其他传感器获得的测量数据，并且与现有地图相比计算观测到的可能性。

SLAM使用马尔科夫的假设，即移动机器人的连续运动被时间分离成离散系统状态，构成马尔可夫链。此时机器人的定位结果被作为下一次定位算法的输入，传感器是用于实时定位移动和环境测距信息，定位结果用于实时地图构建。与传统的位置图不同，SLAM可用于实时定位和地图构建，无需任何需要提前完成的地图输入，在机器人的移动和定位期间将生成环境地图。然而，在实施SLAM时存在某些困难，主要是为了使机器人定位，必须在之前建立非常精确的地图。但是，对于要构建的非常精确的地图，机器人必须能够准确获取当前时刻自身位置。

RBPF-SLAM是一种基于粒子濾波器的SLAM算法，它使用粒子来表示机器人的位置和姿态。广泛应用在机器人的同步定位和地图构建。RBPF（Rao-Blackwellized Particle Filter）算法利用公式（1）对联合概率密度函数进行因式分解

RBPF允许使用记录的测距和激光扫描数据估计机器人的位置，然后根据该位置和激光数据来更新地图。将位姿估计与建图2部分分开。首先根据运动模型进行位姿估计，RBPF算法使用粒子样本来表示定位结果的概率分布，并且每一个粒子代表机器人的可能位姿。再根据得到的位姿结合观测模型更新地图。RBPF粒子滤波器的步骤如下

1）初始化：当t=0的时候根据机器人运动模型先验概率p（x0）选取N个粒子，记为X（i）0（i=1，2，…，N）每个粒子对应的权值为

w（i）0=1/N.

2）采样：根据提议分布π采样，从粒子集合

{X（i）t-1}中产生下一代粒子集合

{X（i）t} 。通常将里程计运动模型

p{xt|x（i）t-1，ut-1}

，作为提议分布π.

3）计算粒子权重：根据重要性重采样原则，由式（2）计算每个粒子的权重

4）重采样：根据式（3）计算有效粒子数，并设定一个阈值

Nth.当Neff

5）更新地图：根据粒子的位姿

x（i）1∶t和历史观测信息

z1∶t，来更新相应的地图：

p（m（i）|x（i）1∶t，Z1∶t）.

2 RBPF-SLAM算法改进

2.1 自适应优化混合分布

对于重采样过程，需要根据提议分布来对下一代粒子进行采样，基本的RBPF中把机器人运动模型作为提议分布，导致仅仅具有较高观测后验似然值的粒子权值才较高，会使粒子间的权重差异变大，粒子退化严重。从而使构建的环境地图精度不高。为了解决上述问题，在运动模型的基础上加上观测模型，作为其混合提议分布，如式（4）所示。

与运动模型不同，观测模型呈现一个相对集中的峰值分布，针对上述混合提议分布无法直接进行采样，采用高斯函数来构建提议分布。首先根据运动模型得到预测，然后把该预测值当作初值进行一次扫描匹配，得到概率大的区域，在该区域内随机选取K个数据，利用其观测模型以及运动模型计算方差和均值，因此可以从模拟出的高斯函数中得到新粒子

u（i）t=1η（i）

·kj=1xj·p（zt|m（i）t-1，xj）·p（xj|x（i）t-1，

ut-1）（i）t

=

1η（i）

·kj=1p（zt|m（i）t-1，xj）·p（xj|x（i）t-1，ut-1）·（xj-u（i）t）（xj-u（i）t）T

的混合提议分布之后就能进行下一时刻机器人位姿信息的采样。此时对于粒子权重的计算公式为

方差变小，但是积分比较困难，而且当观测模型呈现峰态分布时，采样的效率降低，会造成滤波器发散，因此引入退火参数α来调控混合分布中2种模型的比例，如下公式（6）所示

（7）

通过不断地实验以及对观测数据与真实分布之间的关系对比得出，一般情况下，当运动模型起主导作用时，取α为0.6;反之，当传感器的观测模型更加接近真实分布时，取α为0.02，以增加观测模型的比例。

2.2 改进重采样

传统的RBPF算法由于重采样次数多，而导致粒子多样性减少甚至粒子耗尽。为了保持粒子的多样性，优化所得到的粒子集，引入自适应遗传算法，对部分粒子进行交叉变异操作。其基本思想为：根据计算得到的粒子权重，对粒子进行分类，高权重粒子、中权重粒子以及低权重粒子，由式（8）设置合适的高权重以及低权重阈值，两者之间的为中权重粒子。

引入自适应遗传算法，选择权重

F（Xi）=w（i）t作为粒子的适应度函数，则在t时刻交叉变异操作如下。

交叉操作：从得到的高权重以及低权重粒子群中随机选择2个粒子个体作为父辈按照式（9）所示的自适应交叉率pc对粒子进行交叉得到新的个体。

变异操作：从按照上述交叉率得到的新粒子集合中，随机选取一个作为父辈个体按照式（10）自适应变异率pm操作得到新的粒子。

式中 Fmax为集合中粒子最大的适应度值;Favg为每一代群体中粒子的平均适应度值;F′为交叉操作中2个个体中较大的适应度值;F为进行变异操作的粒子的适应度值。

改进RBPF算法流程

1）当 t=0时，选取N个粒子，计算粒子权重为w（i）0=1/N;设置

pc以及pm的值。

2）根據式（6）求取混合提议分布并采样粒子。

3）根据式（7）计算并更新粒子权重。

4）根据粒子权重，对粒子进行划分，对高权重以及低权重粒子进行自适应遗传算法式（9）和式（10）交叉变异操作。

5）计算得到的新粒子集中有效粒子数，根据式（3）判断是否进行重采样，Neff

6）根据机器人的位姿x（i）t以及传感器的观测信息zt计算并更新地图m.

3 实验结果及分析

3.1 仿真

为验证算法的有效性，在MATLAB上对机器人先进行自身位姿的估计对比，设置其实际运行轨迹中真实的位姿状态，利用基本的RBPF-SLAM，文献[27]算法以及改进的算法在粒子数N分别取50以及100时对机器人真实位姿进行估计。如图1所示：其中Pc1=0.7，Pc2=0.5，Pm1=0.1，Pm2=0.01.

从图1和表1的数据可知，在粒子数相同的情况下，提出的改进RBPF算法均方根误差比基本RBPF与文献[27]算法小，更接近真实状态，随着粒子数增加，虽然改进的算法运行时间较长，但是均方根误差更小，与真实状态更加符合。同时由数据可以看出，文中算法采取50个粒子的均方根误差小于RBPF采用100个粒子的均方根误差，说明算法采取50个粒子就能达到RBPF采取100个粒子的效果，因此改进的算法能够用更少的粒子获取更加精确地估计，有效抑制了粒子退化。

其次对机器人真实轨迹以及路标进行估计，如图2所示以及见表2.

从图2和表2数据可知，在轨迹估计方面，改进的算法比基本RBPF以及文献[27]估计的误差小，更加接近真实轨迹;在路标估计方面改进算法也更加接近真实路标位置，估计的误差更小，而基本的RBPF以及文献[27]算法估计的路标则与实际路标位置差异较大，而且改进的RBPF进行估计时所用的粒子数更少、时间更短。因此，改进的算法在机器人轨迹估计以及路标估计方面能取得更准确的结果，能更有效地建立精度较高的栅格地图。

3.2 实际验证

ROS（机器人操作系统）是一个机器人软件平台，提供库以及工具来帮助软件开发人员创建机器人应用程序。在ROS系统中，RBPF-SLAM算法被封装为Gmapping建图功能包，使用激光数据能够建立精度比较高的二维环境栅格地图。

实验平台是Kobuki运动底盘，内部含有里程计且携带激光雷达，在装有ROS的linux（Ubuntu 16.04）移动平台上分别对RBPF，文献[27]以及文中算法在相同的环境下完成同时定位与建图。

选取实验室部分区域作为本次实验的实验环境，如图3所示，选取的区域为6 m×3.2 m，机器人利用里程计数据和激光观测数据分别基于RBPF，文献[27]以及改进RBPF-SLAM算法进行地图构建。如图4所示，分别为3种算法所构建的环境栅格地图。

从图4以及表3数据可知，对于构建相同复杂度环境的栅格地图，传统RBPF构建的地图精度不够准确，文献[27]改进的算法以30个粒子使构建的地图精度有所提高，但是效果不是特别明显，改进的算法只使用8个粒子在较短的时间内构建了更加精确的地图，所以改进算法能够以更少的粒子构建更精确的地图。

4 结 论

1）算法能够使用较少的粒子数得到比传统RBPF算法更精确的位姿，更加接近真实状态，能有效抑制粒子退化。而且具有较小的均方根误差和更短的运算时间。

2）在相同环境下，算法在较短时间内使用少数粒子构建了高精度的栅格地图。下一步，能够使机器人更好地进行路径规划。

参考文献（References）：

[1]

陈 卓，苏卫华，安慰宁，等.移动机器人SLAM与路径规划在ROS框架下的实现[J].医疗卫生装备，2017，38（2）：109-113.

CHEN Zhuo，SU Wei-hua，AN Wei-ning，et al.SLAM and path planning of mobile robot in ROS framework[J].Chinese Medical Equipment Journal，2017，38（2）：109-113.

[2]Endres F，Jürgen Hess，Engelhard N，et al.An evaluation of the RGB-D SLAM system[C]//2012 IEEE International Conference on Robotics and Automation IEEE，2012：1691-1696.

[3]Cheein，Toibero，Sciascio D，et al.Monte carlo uncertainty maps-based for mobile robot autonomous SLAM navigation[C]//IEEE International Conference on Industrial Technology IEEE，2010：1433-1438.

[4]刘 畅.基于扩展卡尔曼滤波的同步定位与地图构建（SLAM）算法研究进展[J].装备制造技术，2017（12）：41-43.

LIU Chang.Research progress of synchronous positioning and map construction（SLAM）algorithm based on extended Kalman filter[J].Equipment Manufacturing Technology，2017（12）：41-43.

[5]Grisetti G，Tipaldi G D，Stachniss C，et al.Fast and accurate SLAM with Rao–Blackwellized particle filters[J].Robotics and Autonomous Systems，2007，55（1）：30-38.

[6]王志远，程 兰，谢 刚.一种改进粒子滤波算法及其在多径估计中的应用[J].计算机工程，2017，43（6）：289-295.

WANG Zhi-yuan，CHENG Lan，XIE Gang.An improved particle filter algorithm and its application in multipath estimation[J].Computer Engineering，2017，43（6）：289-295.

[7]张宏伟.一种约束扩展卡尔曼粒子滤波器[J].东莞理工学院学报，2018，25（5）：10-16.

ZHANG Hong-wei.Constrained extended Kalman particle filter[J].Journal of Dongguan Institute of Technology，2018，25（5）：10-16.

[8]Yuvapoositanon P.Fast computation of look-ahead rao blackwellised particle filter in SLAM[C]//2014 International Electrical Engineering Congress（iEECON）.IEEE，2014：1-4.

[9]De-Yun M O Y W X.Hybrid system monitoring and diagnosing based on particle filter algorithm[J].Acta Automatica Sinica，2003，29（5）：641-648.

[10]Bailey T，Durrant-Whyte H.Simultaneous localization and mapping （SLAM）：Part II[J].IEEE Robotics & Automation Magazine，2006，13（3）：108-117.

[11]Agarwal S，Shree V，Chakravorty S.RFM-SLAM：Exploiting relative feature measurements to separate orientation and position estimation in SLAM[C]//2017 IEEE International Conference on Robotics and Automation（ICRA）.IEEE，2017：6307-6314.

[12]禹鑫燚，朱熠琛，詹益安，等.SLAM過程中的机器人位姿估计优化算法研究[J].高技术通讯，2018（8）：712-718.

YU Xin-yi，ZHU Yi-chen，ZHAN Yi-an，et al.Research on optimization algorithm of robot pose estimation in slam process[J].High Technology Letters，2018（8）：712-718.

[13]强 锋.移动机器人室内地图构建及定位方法的相关分析[J].文化创新比较研究，2018（35）：161-162.

QIANG Feng.Correlation analysis of interior map construction and location method of Mobile robot[J].Comparative Study of Cultural Innovation，2018（35）：161-162.

[14]庄 严，王 伟，王 珂，等.移动机器人基于激光测距和单目视觉的室内同时定位和地图构建[J].自动化学报，2005，31（6）：113-121.

ZHUANG Yan，WANG Wei，WANG Ke，et al.Indoor simultaneous positioning and map construction of mobile robot based on laser ranging and monocular vision[J].ACTA Automatica Sinica，2005，31（6）：113-121.

[15]王法胜，鲁明羽，赵清杰，等.粒子滤波算法[J].计算机学报，2014，37（8）：1679-1694.

WANG Fa-sheng，LU Ming-yu，ZHAO Qing-jie，et al.Particle filter algorithm[J].Chinese Journal of Computers，2014，37（8）：1679-1694.

[16]王卫华，陈卫东，席裕庚.基于不确定信息的移动机器人地图创建研究进展[J].机器人，2001（6）：563-568.

WANG Wei-hua，CHEN Wei-dong，XI Yu-geng.Research progress on mobile robot map creation based on uncertain information[J].Robot，2001（6）：563-568.

[17]Yuen D C K，Macdonald B A.A comparison between extended Kalman filtering and sequential Monte Carlo techniques for simultaneous localisation and map-building[C]//Proceedings of the 2002 Australasian Conference on Robotics and Automation.ARAA，Auckland，New Zealand，2002：111-116.

[18]骆燕燕，陈 龙.融合视觉信息的激光定位与建图[J].工业控制计算机，2017（12）：21-23.

LUO Yan-yan，CHEN Long.Laser slam based on fusion of visual information[J].Industrial Control Computer，2017（12）：21-23.

[19]朱 磊，樊继壮，赵 杰，等.改进粒子滤波器的移动机器人同步定位与地图构建方法[J].重庆大学学报，2014，37（4）：39-45.

ZHU Lei，FAN Ji-zhuang，ZHAO Jie，et al.Mobile robot synchronous positioning and map construction method based on improved particle filter[J].Journal of Chongqing University，2014，37（4）：39-45.

[20]厉茂海，洪炳熔，罗荣华.用改进的Rao-Blackwellized粒子滤波器实现移动机器人同时定位和地图创建[J].吉林大学学报（工学版），2007，32（2）：401-406.

LI Mao-hai，HONG Bing-rong，LUO Rong-hua.Mobile robot simultaneous positioning and map creation with improved Rao-Blackwellized particle filter[J].Journal of Jilin University（Engineering Edition），2007，32（2）：401-406.

[21]余洪山，王耀南.基于粒子滤波器的移动机器人定位和地图创建研究进展[J].机器人，2007（3）：281-289，29.

YU Hong-shan，WANG Yao-nan.Research progress of mobile robot positioning and map creation based on particle filter[J].Robot，2007（3）：281-289，29.

[22]胡士強，敬忠良.粒子滤波算法综述[J].控制与决策，2005（4）：361-365，37.

HU Shi-qiang，JING Zhong-liang.Survey of particle filter algorithms[J].Control and Decision，2005（4）：361-365，37.

[23]马 成，朱 奕，伞 冶.一种基于区间估计的粒子滤波算法[J].哈尔滨工业大学学报，2013（11）：8-12.

MA Cheng，ZHU Yi，SAN Ye.A particle filter based on interval estimation[J].Journal of Harbin Institute of Technology，2013（11）：8-12.

[24]张瑞成，何丹阳.基于模拟退火粒子群算法的最大风能跟踪方法[J].工业控制计算机，2018（10）：40-41，49.

ZHANG Rui-cheng，HE Dan-yang.Maximum wind energy tracking method based on simulated annealing particle swarm optimization[J].Industrial Control Computer，2018（10）：40-41，49.

[25]

Doucet A，De Freitas N，Murphy K，et al.Rao-blackwellised particle filtering for dynamic Bayesian networks[C]//Proceedings of the Sixteenth Conference on Uncertainty in Artificial Intelligence，2000：176-183.

[26]郑 兵，陈世利，刘 蓉.基于萤火虫算法优化的Gmapping研究[J].计算机工程，2018，44（9）：22-27.

ZHENG Bing，CHEN Shi-li，LIU Rong.Gmapping research based on firefly algorithm optimization[J].Computer Engineering，2018，44（9）：22-27.

[27]张 毅，郑潇峰，罗 元，等.基于高斯分布重采样的Rao-Blackwellized粒子滤波SLAM算法[J].控制与决策，2016，31（12）：2299-2304.

ZHANG Yi，ZHENG Xiao-feng，LUO Yuan，et al.An improved particle filter algorithm and its application in multipath estimation[J].Control and Decision，2016，31（12）：2299-2304.

[28]王田橙，蔡云飛，唐振民.基于区域粒子群优化和部分高斯重采样的SLAM方法[J].计算机工程，2017，43（11）：310-316.

WANG Tian-cheng，CAI Yun-fei，TANG Zhen-min.SLAM method based on regional particle swarm optimization and partial Gaussian resampling[J].Computer Engineering，2017，43（11）：310-316.