刘坤坤

摘 要：为了使自主移动机器人在SLAM（同步定位和地图创建）上更加准确，分析了粒子滤波器（Particle Filter，PF）的FastSlam 算法在粒子退化和粒子早熟两方面的不足，提出了一种改进算法（IGA算法）。该算法通过替代原有的重采样过程，改善了粒子多样性，提高了预测精度。在粒子早熟方面采用模擬退火思想对遗传算子进行改进，避免了遗传算法中的遗传算子易陷入局部最优解产生“早熟”现象问题。仿真结果表明，IGA算法使粒子保持的多样性更加持久，算法精度持续时间更长。

关键词：SLAM；Fast SLAM；IGA遗传算法粒子滤波；粒子退化；粒子早熟

DOI：10.11907/rjdk.172234

中图分类号：TP312 文献标识码：A 文章编号：1672-7800（2017）009-0055-06

Abstract：In order to make the autonomous mobile robot more accurate in SLAM， an improved algorithm （IGA algorithm） is proposed based on the shortcomings of Particle Filter （PF） FastSlam algorithm in particle degradation and early maturing of particles. The algorithm is used to replace the original resampling process to improve the particle diversity and improve the prediction accuracy. The genetic algorithm is improved by using simulated annealing in the early maturing of particles， which avoids the genetic algorithm in the genetic algorithm which is easy to fall into the local optimal solution and produce "premature" phenomenon. The simulation results show that the IGA algorithm makes the diversity of particles more durable and the algorithm has longer duration.

Key Words：Synchronous location and map creation （SLAM）； genetic algorithm particle filter； particle degeneration； particle premature

0 引言

SLAM（Simultaneous Localization and Mapping） 指机器人在未知环境中通过自身传感器数据同时进行地图创建与定位导航任务。目前典型的 SLAM 算法大致分为两类，一种是概率定位模型方法，另一种是非概率定位模型方法。目前基于扩展卡尔曼滤波器（ Extended Kalman FIlter， EKF）的 EKF_SLAM 算法和基于粒子滤波器（Particle Filter， PF）的 FastSlam 算法都属于概率定位，非概率定位有数据融合、扫描匹配、SM-SLAM等。

移动机器人同时定位与地图创建是实现未知环境下机器人自主导航的关键技术，结合对SLAM问题的描述以及对FastSLAM1.0和FastSLAM2.0算法模型的分析，可以看出粒子早熟和粒子耗尽是困扰基于粒子群的SLAM算法的一个重要问题。GA-FastSLAM2.0算法利用遗传优化代替FastSLAM2.0的重采样过程，主流的重采样算法有重要性采样算法、残差重采样算法、优化组合重采样等等。优化组合重采样法主要思想是充分利用权值较小的粒子，在复制某个粒子点时，通过采样点和被废弃的采样点进行适当的线性组合，从而得到新的采样点。该算法生成的粒子没有重复，保证了粒子群的多样性。这种变化改善了“粒子耗尽”问题，提高了算法的精度。但是通过实验发现，GA-FastSLAM2.0算法虽然在移动机器人运行初期对粒子群的多样性保持得很好，但是到了算法运算后期，粒子退化问题仍然会出现，这对需要长时间运行的实验过程来说是行不通的。故本文提出改进遗传算法中变异和交叉过程，使粒子多样性保持更长时间，从而使整个SLAM算法能长时间高精度运行。

1 基于改进遗传算法的FastSLAM算法

1.1 粒子滤波器

粒子滤波算法源于蒙特卡洛思想，即用某一事件出现的频率代替该事件的概率。首先根据已知的先验概率求其状态变量的建议分布，接着利用该建议分布的状态空间生成一群粒子，并在之后的估计过程中不断更新每个粒子的权重值，利用这些权值来指代状态变量的概率分布情况。基本的粒子滤波算法过程如下：

（1）粒子初始化。从初始化分布P（x0）中选取N个粒子，定义为：{xi0，i=1，2，…，N}，每个粒子初始化权重为wi0=1/N。

（2）重要性采样。假设xik为近似后验概率分布，建议分布函数为P（x1：k|z1：k，u1：k）。

计算权值：wik=wik-1P（zk|xik）P（xik|xik-1）P（xik|xik-1，z1：k）

（1） 计算归一化权值：ik=wik∑Nj=1wjk

（2） （3）分析粒子退化情况。粒子退化现象是粒子滤波中普遍存在的问题也是研究难点，这主要是由于算法在多次迭代后，只有少数粒子的权重值得分较高，其余大多数粒子权重值得分过低，导致算法在运算后期大量的运算消耗在对结果影响较小的粒子上，降低了算法的运算效率。endprint

本文定义有效粒子数比例参数为Neff，Neff越小表示粒子退化现象越严重。本文将Neff参数定义为：Neff=1∑Ni=1（ik）2×100%

（3） （4）重采样。重采样过程是缓解粒子退化问题的常用途径，即当Neff小于某一给定阈值Nmin时，算法通过增加权重值较大的粒子和减少权重值较小的粒子来更新粒子群。

主流的重采样算法有重要性采样算法、残差重采样算法、优化组合重采样等。优化组合重采样法的主要思想是：充分利用权值较小的粒子，在复制某个粒子点时，通过采样点和被废弃的采样点进行适当的线性组合，从而得到新的采样点。该算法生成的粒子没有重复，保证了粒子群的多样性。

（5）输出。根据重采样后更新的粒子群，计算出后验概率估计公式为：P（xk|z1：k）=∑Ni=1ikδ（xk-xik）

（4） 状态估计公式为：k=∑Ni=1ikxik

（5） 方差估计公式为：Pk=∑Ni=1ik（xik-k）（xik-k）T

（6） （6）判断移动机器人运动过程是否完成任务，如果完成则结束程序，否则，令k=k+1，返回步骤（2）。

1.2 基于改进遗传算法的粒子优化

对粒子群存在的粒子耗尽问题，常用方法是对预测的粒子群进行重采样操作，该方法可有效降低粒子早熟现象的发生，但算法运行时间长，算法效率低。本文提出用一种改进遗传算法（improved genetic algorithm， IGA）替代原有的重采样过程，改善了粒子多样性、提高了预测精度。遗传算法是模仿自然界生物进化机制的随机全局搜索和优化方法，是一种高效、并行、全局搜索的方法，能在搜索过程中自动获取和累积所需的数据，并自适应地控制搜索过程以求最佳解。但遗传算法中的遗传算子易陷入局部最优解，产生“早熟”现象，为解决这一问题，本文采用模拟退火思想对遗传算子进行改进。

1.2.1 适应度与多样性计算

在FastSLAM算法中，本文利用粒子权重（wk）表示粒子的适应度大小，因为粒子的权重大小直接反映了路径估计的优劣，即粒子适应度函数为：f（xk）=wk

（7） 本文采用k时刻粒子群的平均海明距离和粒子群最优平均海明距离的比值表示粒子群的多样性。粒子群多样性函数为：Vdiv=ko

（8） 其中，Hk表示k时刻粒子群的平均海明距离，Ho表示k时刻粒子群最优平均海明距离。

根据公式（7）和公式（8）计算出所有粒子的适应度和整个粒子群的多样性。

1.2.2 粒子评估

根据每个粒子的适应度大小从低到高进行排序，并平均分为十档，第一档设为0分，之后每档评分比前一档多一分，将得分最好的一半粒子复制到优化粒子群中。

1.2.3 交叉与变异

交叉和变异操作的概率为Pc和Pm，其值直接影响算法的收敛性。Pc和Pm的取值由粒子群的多样性决定。多样性较好时，Pc取较大值，Pm取较小值，而多样性较差时，Pc取较小值，Pm取较大值。本文引入模拟退火思想，自适应地给出概率Pc和Pm的值，函数如下：

定义交叉操作的概率Pc为：Pc=sin（1T×π2）+0.4

（9） 定义变异操作的概率Pm为：Pm=sin（T×π2）+0.4

（10） 其中，T为模拟退火思想中的温度概念，在算法初期，粒子种群多、多样性好，需要粒子间更多的交叉操作来提高Pc值，保持粒子群多样性。而到了算法后期，粒子多样性下降，需要更多的变异操作来提高Pm值，缓解粒子多样性下降速度。

（1）交叉操作。在优化粒子群中，根据得分产生粒子群{xi}i=1，…，Pc*N/2，并从排序得分低的粒子群中随机生成粒子群{xj}j=1，…，Pc*N/2，新粒子的位姿和地图估计定义为：xcross=0.5（xi+xj）

（11） 其中，xcross为交叉操作后的子代，xi与xj为xcross的父代，而xi为xcross的直系父代。

计算完所有xcross后，分别计算每个进行交叉操作的粒子直系父代与子代适应度。如果子代适应度低于直系父代适应度，则用子代代替直系父代。否则，粒子群以概率P接受子代。

（2）变异操作。在优化粒子群中，根据得分产生粒子群{xi}i=1，…，Pm*N/2，新粒子的地图估计不变，位姿估计为：xvariance=（1+n*Nrand）xi

（12） 其中，xvariance为变异操作后的子代，xi为xvaricance的直系父代，Nrand是均值为0、方差为1的随机数，n为较小正数，表示变异程度。

计算完所有xvariance后，分别计算每个进行交叉操作的粒子直系父代与子代适应度，如果子代适应度低于直系父代适应度，则用子代代替直系父代。否则，粒子群以概率P接受子代。

1.2.4 获取最终优化粒子群

将退火交叉和变异过程得到的新粒子与粒子估计得到的一半优化粒子组合成最终的粒子群，并将所有粒子的权重设为1/N。

2 FastSLAM算法流程

算法伪码如下：Start

设定0、P0、Q0、 R0值；

（1）基于差動移动机器人运动模型，利用控制输入计算出移动机器人在k+1时刻的位姿状态，即计算出每个粒子在k+1时刻对机器人位姿的预测均值和方差。

（2）将k时刻每个粒子估计观测值与获取的k+1时刻观测值进行数据关联，这些数据关联是相互独立的。

（3）根据每个粒子获得的观测值，计算出粒子位姿估计的均值和方差。利用这些均值和方差构建一个高斯分布函数作为重要性概率密度函数。

（4）利用IGA算法步骤对移动机器人进行路径估计，并更新粒子群。endprint

（5）用公式（11）对SLAM问题进行Rao-Blackwellised分解架构，环境地图的估计被分解成独立的路标进行估计，而对每个独立的路标估计，EKF既可满足精度要求，也可满足运算效率要求，所以地图路标的后验概率分布P（miX1∶k+1，Z1∶k+1，U1∶k+1）采用EKF算法进行估计。地图估计表示为：{μ′，P′，…，μM，PM}，其中，μi表示地图中第i个路标的高斯均值，Pi表示地图中第i个路标的方差。因为本文采用静态仿真地图作为研究对象，所以可直接用k时刻路标的估计均值和方差作为μi和Pi。如果路标被观察到，则使用KEF算法对地图进行更新，否则此时刻的均值和方差等于上一时刻的均值和方差。

（6）检测是否达到停止条件，如果未完成，则返回步骤（2）继续执行。

End

IGA-FastSLAM算法流程如图1所示。

3 实验结果分析

本文通过移动机器人传感器感知周边环境，并依靠周边环境预测位置信息。利用IGA算法改进粒子滤波器，有效抑制了粒子退化，提高了移动机器人运动精度。

3.1 系统结构

本文利用MATLAB环境开发移动机器人运动仿真模型，为了更好地比较各算法的优劣，本文对实验环境进行统一设定。设置移动机器人每个轮子的运动速度为3m/s，两差动轮之间的距离为1m，机器人可以观测到30m以内的路标信息，系统采样时间为0.025s，速度误差为0.3m/s，仿真环境大小为150m×150m，此环境中航标信息17个，路标信息35个，如图2所示。

设置粒子重采样阈值Neff<75%时进行重采样。绿色*表示路标信息，红色o表示航标信息，绿色三角表示真实移动机器人位置，地图中的红色点以及红色三角表示预测的路标位置以及预测的移动机器人位置。可以看出，由于移动机器人观测距离的限制，部分位于中间的路标未被观测到。

3.2 算法比较和分析

为了比较传统遗传算法与本文改进遗传算法对粒子退化问题的抑制效果，设计将两种改进算法运用到同一个粒子滤波过程中，比较两种情况下有效粒子数比例的变化。如图3所示，X轴表示算法运行的迭代次数，Y轴表示有效粒子数比例。从图中可以看出，算法刚运行时，有效粒子数比例很高，两种算法粒子群多样性保持很好。随着算法迭代次数的增加，有效粒子数比例不断下降，从图3可以清楚地看出，基于传统遗传算法改进的粒子滤波器有效粒子数比例下降速率比改进的粒子滤波器下降速率要快。传统算法在迭代20次左右时，有效粒子已经降低到10%，而改进遗传算法在迭代30次左右时才降低到10%。这是由于在遗传算法的变异和交叉过程中，两种运算过程的比例由基于模拟退火思想的公式（9）和公式（10）自适应取值，比传统遗传算法人为给定比例值更具客观性。所以，本文算法能更有效地抑制粒子濾波器中粒子退化现象的产生。运用到SLAM问题中，则有效提高了算法预测精度。为了比较IGA-FastSLAM与FastSLAM2.0算法在预测移动机器人自身位姿信息时的准确性，本文分别让移动机器人利用两种SLAM算法在实验环境中运行10次，每次测试在仿真环境中运行两圈，计算移动机器人位姿x轴、y轴和整体的平均估计偏差值，如图4、图5、图6所示。红色表示IGA-FastSLAM算法估计误差值，绿色表示GA-FastSLAM2.0算法估计误差值，蓝色表示FastSLAM2.0算法估计误差值。

对比各算法X轴偏差值可以清楚地看出，在9 000步以前，机器人在未知环境中行走一圈，FastSLAM2.0算法产生的最大估计误差为21.57m，GA-FastSLAM2.0算法产生的最大估计误差为12.59m，而本文算法产生最大估计误差为8.08m，比FastSLAM2.0算法估计误差降低了62.54%，比GA-FastSLAM2.0算法估计误差降低了35.82%。9 000步以后，3个算法估计误差趋于平稳，这是由于机器人运行到了第2圈，并不是面对完全未知的环境，所以对环境有了一定的预知性，降低了估计时的不确定性。9 000步以后IGA-FastSLAM算法分别比Fast SLAM2.0算法和GA-FastSLAM2.0算法估计误差平均降低了48.98%和47.92%。

对比各算法Y轴估计偏差值：在第1圈测试中，IGA-FastSLAM算法分别比FastSLAM2.0算法和GA-FastSLAM2.0算法最大估计误差降低了75.06%和69.73%，而在第2圈测试中，IGA-FastSLAM算法分别比FastSLAM2.0算法和GA-FastSLAM2.0算法估计误差平均降低了48.98%和47.92%。

对比各算法整体估计误差值，如图6可以清楚看出，IGA-FastSLAM算法在整个测试过程中保持着良好的估计精度。在第1圈测试中，本文算法比FastSLAM2.0算法估计误差降低了72.21%，比GA-FastSLAM2.0算法估计误差降低了46.10%。而在第2圈趋于稳定的预测过程中，本文算法比FastSLAM2.0算法估计误差降低了59.04%，比GA-FastSLAM2.0算法估计误差降低了24.30%。

比较各种算法的实时性，如表1所示。从表中可以清楚地看出IGA-FastSLAM比FastSLAM2.0算法和GA-FastSLAM2.0算法运算时间分别增加了约36.93%和6.91%。这是由于本文算法利用改进遗传算法代替FastSLAM2.0算法的重采样过程，算法复杂度要高于其它算法，导致算法运算时间增加。但考虑到本文算法对误差的优化效果大于运算时间的消耗，所以本文算法更适合解决SLAM问题。

为了对比不同粒子数对IGA-FastSLAM算法的影响，本文分别对初始化为10、50、100和200个的粒子数进行实验。如图7、图8、图9所示，红色、黄色、蓝色、绿色线条分别表示100个、10个、50个、200个粒子的IGA-FastSLAM实验结果。从实验结果可以清楚看出，随着粒子数的增加，算法估计误差不断降低，但是降低的趋势下降。为了更方便地比较各个算法精度，只观察测试第2圈时的数据，如表2所示。从表中可以清楚看出，随着粒子数的增多，X轴、Y轴和整体误差不断减小，但是减小的幅度越来越小，带来的负面影响是算法计算时间的增加。从图10可以看出，50个粒子比100粒子运算时间增加了约8.83%，误差缩减了47.41%，但200粒子比100粒子运算时间增加了约10.09%，而误差仅仅降低了18.65%。从经济学角度考虑，为增加算法精度而一味提高粒子个数，从而导致运算时间的大幅度消耗，这种优化策略并不可取。endprint

为了充分证明IGA-FastSLAM算法可以适应各种环境，本文改变原有仿真环境，将航标设为9个、路标设为14个，再次比较FastSLAM2.0、GA-FastSLAM2.0以及IGA-FastSLAM算法的估计误差值，如图11所示。降低环境复杂度是为了方便比较各种算法之间的表现效果。

图12、13、14分别表示FastSLAM2.0、GA-FastSLAM 2.0以及IGA-FastSLAM算法在新建仿真环境中移动机

器人的估计误差值。从图中可以清楚地看出，FastSLAM2.0算法无论在X轴、Y轴估计误差值比较，还是在整体估计误差值比较中，都表现出算法估计误差远高于其它两种算法。通过计算，可得出FastSLAM2.0算法估计误差比GA-FastSLAM2.0算法高56.21%，比IGA-FastSLAM算法高57.38%。比较本文算法和GA-FastSLAM2.0算法可以看出，在仿真程序开始时，两种算法精度相差不大，这是由于GA-FastSLAM2.0已经就重采样问题进行了改进，有效提高了粒子群滤波器的多样性。但从整体误差图不难看出，在仿真程序运行后期，本文算法的误差值明显小于GA-FastSLAM2.0算法。在程序的前半段，两种算法的平均整体估计均为0.48m，但在程序的后半段，IGA-FastSLAM算法整体估计误差比GA-FastSLAM2.0算法降低了约43.25%。这是因为本文算法利用改进的遗传算法改进了粒子滤波器，比仅利用遗传算法进行改进，粒子保持的多样性更加持久，算法精度持续时间也就更长。

4 结语

本文从传统粒子群算法出现的粒子早熟和粒子耗尽问题出发，利用改进遗传算法对粒子滤波器重采样过程进行优化，使粒子多样性保持更加持久。实验结果表明，合理设置粒子个数能在保持高精度的前提下，降低计算机消耗，有效提高算法的运算效率。

参考文献：

[1] GUPTA S K， GARG S. Chapter four-multiobjective optimization using genetic algorithm[J]. Advances in Chemical Engineering， 2013（43）：205-245.

[2] LIU J S， CHEN R. Sequential monte carlo methods for dynamic systems[J]. Journal of the American Statistical Association， 1998，93（443）：1032-1044.

[3] 周武，赵春霞.一种基于遗传算法的FastSLAM 2.0算法[J]. 机器人， 2009，31（1）：25-32.

[4] WANZHOU YE. Optimality of the median filtering operator [J]. Circuits， Systems， and Signal Processing， 2011，30（6）：1329-1340.

[5] SB WILLIAMS， G DISSANAYAKE， H DURRANT WHYTE. An efficient approach to the simultaneous localisation and mapping problem[C]. IEEE International Conference on Robotics and Automation， 2002：406-411.

[6] 董海巍，陈卫东.基于稀疏化的快速扩展信息滤波SLAM算法[J].机器人， 2008，30（3）：193-200.

[7] THRUN S， LIU Y， KOLLER D， et al. Simultaneous localization and mapping with sparse extended information filters[J]. Int.j.robot.res， 2004，23（7-8）：693-716.

[8] MOREFIELD C L. Application of 0-1 integer programming to multitarget tracking problems[J]. Automatic Control IEEE Transactions on， 1977，22（3）：302-312.

[9] COOPER A J. A comparison of data association techniques for simultaneous localization and mapping[D]. Aerospace Engineering and Mechanics， University of Minnesota， 2005.

[10] MONTEMERLO M， THRUN S. Simultaneous localization and mapping with unknown data association using fastSLAM[C].Proc of IEEE International Conference on Robotics and Automation， Taibei， 2003：1985-1991.

[11] OHY A， NAGASHIMA Y， YUTA S. Exploring unknown environment and map construction using ultrasonic sensing of normal direction of walls[J]. Proceedings of the IEEE International Conferences on Robotics and Automation， 1994（1）：485-492.

[12] 鄒国辉，敬忠良，胡洪涛.基于优化组合重采样的粒子滤波算法[J].上海交通大学学报，2006，40（7）：1135-1139.

（责任编辑：杜能钢）endprint