基于果蝇算法优化的移动机器人RBPF-SLAM研究

韩锟，章京涛，杨穷千

(中南大学 交通运输工程学院，湖南 长沙 410075)

21世纪以来，机器人的功能越来越强大，已经能够实现目标跟踪[1]、导航、路径规划等复杂功能，被广泛应用于工业、军事、家庭服务等领域。SLAM[2]作为移动机器人的基础问题，是机器人实现其他功能的前提，SLAM精度直接影响其他功能实现的效果，因此，如何提高SLAM精度一直是国内外学者的研究热点。SLAM问题是指移动机器人在无地图信息的环境中运动时通过实时获取周边特征信息以实现对自身的定位，并在此基础上创建周围环境的增量式地图。目前，解决SLAM问题较为成熟的方法主要有基于滤波的方法和基于图优化的方法。其中，Rao-Blackwellized粒子滤波 器(Rao-Blackwellized Particle Filter，RBPF)[3]将SLAM问题视为机器人轨迹估计和环境地图估计这2个后验概率的乘积，降低了状态估计问题的空间维数，大幅度减少了算法的计算量，得到了广泛应用。但是由于RBPF-SLAM采用的建议分布精度有限，导致粒子退化严重，并且频繁的重采样会引发样本贫化问题。近年来涌现了大量改进RBPF-SLAM的研究：王田橙等[4]提出了一种基于区域粒子群算法的SLAM方法，通过粒子群优化驱使粒子向其所在区域的中心移动，提高粒子种群的质量，但在保证种群多样性方面还存在不足。郑兵等[5]将萤火虫算法引入到粒子滤波器中，调整粒子的分布状态，提高了滤波器的估计精度，但算法的全局寻优能力不佳，当观测存在多个峰值时，易陷入局部最优解而导致粒子估计出现偏差。孙弋等[6]利用退火参数对建议分布中运动模型和观测模型的比例进行调控，并对粒子集进行交叉变异操作，增加了种群多样性，但对建议分布的优化还存在改进的空间。针对当前RBPF-SLAM改进算法仍存在的粒子退化、种群多样性匮乏导致移动机器人位姿估计精度不足问题，本文提出一种基于果蝇优化算法(Fruit Fly Optimization Algorithm，FOA)[7]改进的RBPF算法：第一，引入FOA算法，利用其高寻优能力使粒子聚集在高似然区域，从而使粒子分布更逼近机器人的真实位置；第二，引入遗传算法的自适应交叉和变异操作，维持粒子多样性，对最优粒子进行柯西扰动变异，防止算法陷入局部最优解；第三，改进了FOA算法的寻优步长，增大迭代时粒子的移动距离，提高算法的收敛效率。改进后的RBPF算法能够有效减少SLAM所需的粒子数，并能得到更加精确的栅格地图。

1 RBPF算法简介

RBPF算法的目的是估计环境地图m和机器人轨迹x1:t的联合后验概率，表达式为p(x1:t,m|z1:t,u1:t-1)，z1:t=z1,z2,…,zt表示激光雷达获取的观测信息，u1:t=u1,u2,…,ut-1表示里程计信息。通过贝叶斯公式可将RBPF粒子滤波器分解如下：

RBPF-SLAM由于只采用运动模型p(xt|xt-1,ut-1)作为建议分布π，随着时间的推移，里程计的累积误差越来越大，使其估计性能下降。文献[8]在RBPF算法的基础上做了2方面的改进，后被封装为开源功能包Gmapping：第一，使用运动模型计算建议分布时加入了激光观测信息，得到更优的建议分布π′=p(xt|mt-1,zt,xt-1,ut-1)。第二，对重采样步骤进行了自适应改进，提出衡量粒子的退化严重性的变量Neff，当Neff＜N/2时进行重采样，从而减少了不必要的重采样，缓解了粒子耗尽的问题。改进后算法的流程图如图1所示。

图1 Gmapping的基本流程图Fig.1 Basic flow chart of Gmapping

虽然Gmapping一定程度上解决了RBPF算法存在的问题，但在规模较大、相似度高的场合仍存在精度不佳的情况。因为该类场景需要大量的采样粒子才能较好地描述后验概率密度分布，导致算法复杂度大幅提高，引发粒子退化、种群多样性降低、算法实时性差等问题[9]。针对上述问题，本文在Gmapping的基础上利用FOA算法做出进一步改进。

2 基于FOA的改进RBPF算法

2.1 标准FOA算法

果蝇觅食时，首先根据食物散发在空气中的浓度，依靠自身嗅觉向食物的大致方向移动，当食物进入视野后则依靠视觉精准飞去。受到果蝇觅食行为的启发，FOA算法将每个果蝇个体看作一个可行解，不断向最佳位置移动，寻求模型的全局最优解。FOA算法的过程如下。

步骤1：初始化。设置果蝇初始位置、种群规模N，最大迭代次数Maxgen等参数。

步骤2：果蝇从当前位置飞出，在周围随机搜寻浓度更高的位置，移动公式如下：

步骤3：由果蝇的位置浓度得到个体的适应度值，并找到种群的最优个体。

步骤4：若当前最优适应度值大于前一迭代最大适应度值，则所有果蝇向该最优个体飞去。重复步骤2～4，直到满足终止条件。

相比于萤火虫算法、蝴蝶算法等群智能算法[10]，FOA算法的优点在于其算法复杂度低、机制简单，且参数相对较少，只需对种群规模、迭代步进值和最大迭代次数等几个参数进行设置调整[11-12]。

2.2 果蝇优化策略

文献[13]将FOA算法融合进粒子滤波算法并将其应用于目标跟踪领域，取得了较好的效果，证明了FOA算法优化粒子滤波算法的可行性，且目前将FOA算法引入SLAM问题的研究工作极少，因此本文尝试将FOA算法与RBPF算法进行融合。

融合的基本思路为：在采样过程得到优化后提议分布的采样粒子后，将粒子集看作果蝇种群，扫描匹配的得分作为个体的适应度值，适应度值反映了个体与机器人真实状态的符合程度。通过FOA算法优化粒子分布，驱使果蝇个体向最优位置飞去，不断向周边范围随机搜寻更优位置，使果蝇不断向移动机器人的真实状态逼近，缓解粒子退化。

为克服标准FOA算法在收敛过程中种群多样性降低、易陷入局部最优而导致粒子偏离真实状态的局限，本文引入交叉变异操作对种群进行适应性改进，将粒子随机配对后根据自适应概率进行交叉操作，然后复制一定数量的最优粒子并对其进行变异操作，以保持种群的多样性，防止陷入局部最优解。最后采用指数函数步长的状态更新公式移动果蝇个体，以增大寻优步长，提高算法的收敛速度，同时有利于算法跳出局部最优。

通过上述改进提高滤波器的估计精度，使得所需的粒子数减少，从而减小了算法计算量，在大规模、相似度高等环境同样适用，有效解决了Gmapping存在的粒子退化、种群多样性不足、实时性差等问题。

2.3 算法实现

FOA算法描述为：每个粒子状态xi被视为一个可行解，根据适应度值fi的大小，获取种群迭代的最优状态x′，即利用N个粒子组成的集合在目标搜索空间中搜索最优解。

针对FOA算法存在迭代速度慢、搜索精度低等问题，本文在果蝇寻优过程中采用了基于指数函数步长的移动公式[14]。由于指数函数的变化速率快，因而能够增加粒子的移动步长，不但算法的收敛效率得到了提高，而且移动后的新个体更有可能跳出局部最优。改进后的果蝇个体位置更新公式如下：

确定交叉概率后，将果蝇随机配对，根据式(4)和式(5)进行交叉操作。自适应交叉概率使得分布较优的粒子也能有一定的交叉概率，这些粒子交叉后，更有可能将优势基因遗传到子代，提升种群个体的质量。

式中：p1，p2为概率参数；fb为选择算子中更优个体的适应度值；A为常数。

为避免早熟，复制K个当前最优个体，根据变异概率Pmu对其进行柯西扰动变异操作。若变异个体的适应度值增加，则替换掉原来的最优个体。采用的柯西变异公式如下[13]：

式中：xa(j)表示变异个体的状态。

2.4 算法步骤

步骤1：根据运动模型进行状态估计。

步骤2：进行扫描匹配。在混合提议分布π′中采样N个粒子xi(i=1,…,N)，并将粒子的匹配得分作为适应度值fi。

步骤3：调整粒子分布。

1)找到适应度最大fmax的个体，所有粒子向该个体飞去，根据式(3)给出果蝇个体搜寻食物的步长。

2)更新种群的适应度值，根据式(4)和式(5)进行自适应交叉操作。

3)复制K个最优个体，按式(6)进行变异，若其适应度增加，则替换为最优个体。

4)进行迭代寻优，直到满足适应度条件或达到最大迭代次数Maxgen。

步骤4：计算优化后粒子的权重并进行归一化，若Neff＜N/2，则进行重采样。

步骤6：进入下一时刻，转到步骤1。

3 实验结果及分析

3.1 仿真实验

采用激光SLAM中典型的数据集：ACES building和MΙT Killian Court，对比不同算法的地图拓扑结构的正确性。ACES数据集环境规模较小、结构较为简单，MΙT数据集地图规模较大且相似度高，包含较多的闭环，调整参数linearUpdate=1,angularUpdate=0.5以取得更好的效果[15]。进行实验的计算机的操作系统为Ubuntu16.04，主频为2.70 GHz。本文算法中，取Maxgen=10，p1=0.3，p2=0.7，Pmu=0.05，α=0.7。

3.1.1 本文算法的有效性验证

对RBPF算法和本文算法进行仿真对比实验，验证本文算法的有效性。在ACES数据集上的仿真结果如图2所示，可以看出，RBPF算法得到的地图在标记1，2和3处存在错乱、分层现象，效果较差。而本文算法得到的地图边缘清晰，无重叠等现象。图3为MΙT数据集的仿真结果。可以看出，RBPF算法的构图一致性较差，未能正确完成闭环。相比之下，本文算法在粒子数为60时，虽然地图在标记1，2和3处出现了一些重叠，但整体效果较好，粒子数为100时得到的地图能够很好地反映真实环境。

图2 10个粒子时ACESbuilding栅格地图Fig.2 ACESbuilding raster map of ten particles

图3 MΙT Killian Court栅格地图Fig.3 MΙT Killian Court raster map

此外，本文采用CPU占用率来衡量算法的性能，具体方法为：等时间采集3组CPU的占用率数据，每一组包含50个数值，样本数量为150，最后求取平均值。2种算法在各数据集下的粒子数和CPU占用率对比如表1所示。由表1可以看出，本文算法构建一致性地图所需的粒子数大幅度减少，减少了算法复杂度，CPU占用率较RBPF算法降低了约20%。

表1 各数据集粒子数和CPU使用率Table 1 Particle count and CPU usage per dataset

3.1.2 改进FOA算法的必要性验证

为验证FOA算法引入RBPF算法所作出改进的必要性，对标准FOA优化的RBPF算法和本文算法进行对比实验。标准FOA优化的RBPF算法在ACES数据集下的仿真结果如图4所示。可以看出，该算法相较于RBPF算法无明显改进，地图一致性较差。标准FOA优化的RBPF算法在MΙT数据集下的仿真结果如图5所示，可以看出，粒子数为60时该算法的建图效果并不理想，地图在标记1，2，3和4处未完成闭环，粒子数为100时，地图在标记2和4的对应位置仍存在较大的偏差。

图4 10个粒子时ACESbuilding栅格地图Fig.4 ACESbuilding raster map of ten particles

图5 MΙT Killian Court栅格地图Fig.5 MΙT Killian Court raster map

综上，只使用FOA算法进行优化不能取得理想的效果，而作出适应性改进后的算法能有效改善地图效果，验证了本文对FOA算法作出的步长改进和遗传改进的必要性。

3.2 实机测试

实验平台采用如图6所示的移动机器人，该机器人采用双轮差分驱动方式，装载RPLΙDARA2激光雷达，配有ΙntelΙ5-2430M处理器，CPU 2.4 GHz，板载2 GB内存的工控机，系统安装Ubuntu16.04操作系统，激光雷达安装在高度约为0.2 m处。实验环境选择室内，大小为6.6 m×7.2 m，如图7所示。

图6 机器人模型Fig.6 Robot model

图7 实验场景Fig.7 Laboratory environment

RBPF算法和本文算法对实验场景的建图结果如图8所示。由图8可知，RBPF算法取粒子数为10时，地图产生了严重的不一致现象；粒子数为20时，地图在标记1，2和3处仍出现“假墙”、未闭合现象。而本文算法在粒子数为10时就能够取得很好的建图效果。

图8 2种算法的真实场景建图结果Fig.8 Realistic scenario building results of two algorithms

4 结论

1)利用FOA算法改善粒子分布，结合自适应交叉变异操作，对RBPF算法进行优化，提高了滤波器的估计精度。基于仿真实验和实机测试结果表明，对相同复杂度的环境地图，本文算法能在粒子数减少50%甚至更多的情况下，取得比RBPF算法更好的建图结果。

2)本文算法通过改善粒子分布，减少了算法所需粒子数，在ACES和MΙT数据集的仿真实验结果表明，本文算法CPU的占用率较RBPF算法降低了约20%，提升了SLAM的效率和实时性。

3)通过仿真实验结果可以看出，直接将FOA算法引入RBPF算法效果不明显，而作出适应性改进后算法得到的地图效果明显改善，表明利用标准FOA算法优化RBPF算法存在局限性，应用时需要作出适当改进。