基于多策略人工蜂鸟优化PF的SLAM研究*

蔡 艳,杨光永,樊康生,徐天奇

(云南民族大学电气信息工程学院,昆明 650000)

0 引言

瞬时定位与地图构建(simultaneous localization and mapping,SLAM)在移动机器人自主工作领域扮演重要角色。目前,SLAM的求解方法可大致分为基于卡尔曼滤波器的方法、基于粒子滤波器的方法、基于图优化的方法[1]。基于蒙特卡洛方法的粒子滤波算法(particle filter,PF)不受系统非线性和非高斯噪声的限制,是常用于SLAM工程领域的方法之一。

由于粒子滤波算法基于蒙特卡洛方法,因此要达到所需估计精度需要大量的粒子,而粒子数量越多,算法时间复杂度越高;此外,重采样易导致样本多样性下降,出现粒子退化的问题。而进化问题和粒子滤波器本质上都是通过评价、选择和更新的迭代过程获得最优解[4],因此很多研究者采用进化方法来解决粒子滤波器存在的问题。张翔等[5]引入遗传算法改善标准粒子滤波中的粒子退化与粒子衰退问题。刘海涛等[6]在基于遗传算法的智能粒子滤波基础上,提出对低权值粒子的改进的智能粒子滤波(IIPF)处理策略,提高了滤波精度。韩锟等[7]提出一种基于果蝇优化思想的粒子滤波算法,将遗传算法中的交叉、变异操作自适应地应用到果蝇优化算法寻优过程当中,有效提高了估计精度。陈志敏等[8]提出了一种基于蝙蝠算法的新型粒子滤波算法。该算法用粒子表征蝙蝠个体,模拟蝙蝠群体搜索猎物的过程,进而提髙粒子整体的质量和分布的合理性。李冀等[9]结合融入围猎策略的哈里斯鹰优化算法设计一种群智能优化粒子滤波方法(EHHOPF),有效提升了系统状态估计精度及滤波稳定性等。上述方法均提高了粒子滤波算法的性能,但大多都是通过经验值控制智能算法迭代次数,易造成优化不足或过度优化而引起估计精度降低的问题。

本文为进一步提高PF算法的性能,引入多策略的人工蜂鸟算法(artificial hummingbird algorithm,AHA)[10]寻优能力强的特性更新预测粒子集,提高粒子集质量;当粒子密集度过高时,自适应调整优化程度;根据改进的人工蜂鸟算法获取的最优解调整预测粒子集,使粒子集在权重计算前就更接近期望值,以此提高路径和路标的估计精度;在重采样阶段通过重组粒子集来增加粒子多样性。

1 多策略人工蜂鸟算法优化的粒子重组PF

1.1 标准人工蜂鸟算法

人工蜂鸟算法模拟了自然界中蜂鸟的特殊飞行技能和智能觅食策略,该算法包括3种觅食行为方式。

(1)引导觅食。在觅食过程中,AHA算法建模了3种飞行技能,通过引入方向切换向量控制D维空间中的一个或多个方向是否为可用,其中轴向飞行定义如下:

(1)

对角飞行定义为:

(2)

全向飞行定义为:

D(i)=1i=1,…,d

(3)

式中:D(i)表示飞行技能,randi([1,d])表示生成一个1～d的随机整数,randperm(k)表示创建一个1～k的随机整数排列,r1是(0,1]的随机数。凭借这些飞行技能,蜂鸟可以访问目标食物源,从而获得候选食物源。候选食物源位置更新为:

vi(t+1)=xi,tar(t)+a·D·(xi(t)-xi,tar(t))

(4)

式中:vi(t+1)为第t+1次迭代第i个候选食物源位置,xi(t)为第t次迭代第i个食物源位置,xi,tar(t)为第i只蜂鸟计划访问的目标食物源位置,a为服从标准正态分布的引导因子。第i个食物源的位置更新如下:

(5)

式中:f(·)表示适应度值。

(2)区域性觅食。蜂鸟在访问了目标食物源后,可能会移动到邻近区域寻找新的食物源,而不是访问其他现有的食物源。邻近区域候选食物源位置更新为:

vi(t+1)=xi(t)+b·D·xt(t)

(6)

式中:b为服从标准正态分布的引导因子。

(3)迁徙觅食。当蜂鸟经常访问的地区缺乏食物时,通常会迁移到较远的食物源觅食。位于花蜜补充率最差食物源的蜂鸟将迁徙到搜索空间中随机产生的新食物源处,为:

xwor(t+1)=Low+r·(Up-Low)

(7)

式中:xwor为种群中花蜜补充率最差的食物来源。

1.2 中垂线收敛策略

中垂线收敛[12]策略二维示意图如图1所示,图中best表示矩形区域内的最优解,本文用bs与bt分别表示区域内当前最优和次优的值,若best到bs的距离小于best到bt的距离,则best点必位于bs与bt的中垂线靠bs侧区域(即图1灰色区域)。此时,会重新在灰色区域找到bt点,再按照上述规则再次判断最优点的区域,直至找到最优点。中垂线收敛策略利用该思想,以点的适应度代替“距离”。

图1 中垂线算法原理图

设目标种群当前迭代过程中适应度最优的个体为bs。适应度次优的个体为bt,定义:

(8)

若某一点xi位于中垂线靠bt侧区域,则xi的第j维需满足下列条件:

(9)

式中:

(10)

若判定该粒子位于中垂线靠bt侧区域,则更新xi,为:

(11)

(12)

式中:D为维度。

1.3 自适应调整策略

为避免AHA仅依赖经验值设置迭代次数造成优化不足或过度优化的问题,引入种群密度监测阶段实现自适应调整迭代次数,实时监测最优个体附近的种群密度,当密度达到阈值时,停止迭代,相应的表达式为:

(13)

式中:Nb表示最优个体周围的蜂鸟数,rb表示密度半径,find操作表示查找密度半径内的个体数量。

(14)

式中:ρb表示最优个体周围的种群密度,N表示粒子总数。

1.4 Levy飞行策略

当种群进行区域性觅食时,引入Levy飞行更新个体位置,通过步长变化扩大搜索空间,以提高算法全局搜索能力。当种群密度大于设置的区域搜索阈值时采用Levy飞行策略更新个体位置,公式为:

vi(t+1)=xi(t)+b·D·xt(t)+α⊕Levy(s,β)

(15)

式中:Levy(·)为Levy分布函数,α为步长控制因子,β为概率系数,s为Levy飞行步长,公式为:

(16)

(17)

式中:Γ(·)为伽马函数。

1.5 粒子重组重采样

针对粒子滤波算法在重采样阶段去除小权值粒子而降低估计性能的问题,在重采样阶段设计如下粒子筛选策略:

(1)计算有效粒子数Neff以及粒子权重,并将权重按降序排列;

(18)

1.6 基于多策略人工蜂鸟的粒子重组粒子滤波

针对传统粒子滤波算法精度较低以及粒子贫乏的问题,本文引入AHA算法对其进行优化以提高粒子集质量;为避免优化不足或过度优化且减少时间复杂度,采用自适应调整迭代次数策略;为提高算法收敛速度在引导觅食阶段引入中垂线收敛策略;在区域性觅食阶段引入Levy飞行扩大搜索空间自适应调整粒子集分布,多样化粒子;在重采样阶段采用粒子重组策略增加粒子多样性。IAHA-PF算法步骤为:

步骤1:初始化粒子集,每个粒子初始权重为w0=1/M;

步骤2:从proposal分布中采样粒子集,t时刻的采样粒子集Xt为:

Xt～p(xt|xt-1,ut)

(19)

步骤3:IAHA优化,更新粒子集;

步骤4:根据IAHA的输出更新粒子集分布;

步骤5:计算粒子权重;

步骤6:根据式(18)重组粒子集;

步骤7:迭代更新预测值,根据式(20)输出预测值。

(20)

多策略人工蜂鸟算法步骤如表1所示。

表1 多策略人工蜂鸟算法

1.7 IAHA-PF算法时间复杂度分析

假设粒子数为N,最大迭代次数为M,跟踪时长为T,则粒子集采样时间复杂度为O(TN),IAHA算法时间复杂度为O(TMN),重采样阶段时间复杂度为O(TN+TNN),所以总的时间复杂度为O(TN+TMN+TNN),由于本文算法采用自适应调整迭代次数策略,因此实际迭代次数是小于最大迭代次数M的,并且随着粒子数量的增加,实际迭代次数会逐渐减少,此外,实际迭代次数一般是小于N的,这与仿真实验自适应调整迭代次数部分相符,忽略低阶项,记K=max(M,N),最后IAHA-PF算法的时间复杂度为O(TKN),与PF的时间复杂度同量级,说明相比于标准PF算法,本文的改进算法总体时间复杂度略高,但增加幅度不大。

2 仿真实验

为验证本文算法具有较好的滤波性能以及应用在SLAM中有较好的定位与建图性能,进行仿真对比实验。

2.1 密度半径设定

引入智能算法进行优化时,一般设置最大迭代次数,而智能算法迭代过程中具有随机性,仅通过最大迭代次数可能会造成过度优化或优化不足问题。本文引入自适应调整策略,设置粒子密度监测阶段,自适应调整迭代次数。其中粒子密度的衡量标准取决于密度半径的设定,表2统计了当粒子数为100时,不同时刻不同密度半径下的粒子密度占比值。

表2 不同时刻不同密度半径下的粒子密度占比 (%)

从表2可知,当密度半径为0.001时,粒子密度总体随着迭代次数的增加而增大,但局部存在波动且随着迭代次数增加粒子密度增长较缓慢,达到期望优化程度时间复杂度较高;当密度半径为0.005与0.05时,随着迭代次数的增加粒子密度的增长速度较快,但后者粒子密度在迭代次数为20左右就达到90%以上,迭代次数过少会导致精度下降问题。当密度半径为0.005时,随着迭代次数的增加,粒子密度增长趋势相对平缓,能较好反映粒子分布情况,因此,本文选取密度半径为0.005。

2.2 粒子滤波估计精度分析

为验证本文算法有较好的估计性能,将BA-PF[8]、AHA-PF、IAHA-PF算法与传统PF算法进行对比实验,并进行误差对比分析。基于单变量动态变化滤波模型[15]进行粒子数为50、100和150时4种算法的仿真实验,并给出滤波精度的对比和分析。本文选取的状态方程和观测方程为:

(21)

(22)

式中:x(t)、y(t)分别为t时刻系统的状态量和观测量,w(t)、v(t)为标准分布的高斯噪声,系统初始状态x为0,采样时间步长为50,粒子数N为50、100、150时的状态估计结果和误差对比结果如图2～图4所示。

(a) 状态估计值 (b) 估计误差

(a) 状态估计值 (b) 估计误差

(a) 状态估计值 (b) 估计误差

可以看到,随着粒子数目的增加,4种算法的估计精度均有提升,在同等情况下,BA-PF、AHA-PF与IAHA-PF的整体估计效果都比PF好,其中,IAHA-PF的估计误差最小。其原因在于:AHA-PF运用智能优化这一思想优化传统算法,能有效提高状态估计性能,但仅依靠经验值会存在过度优化导致精度下降或优化不够导致误差较大的问题;IAHA-PF采用自适应调整策略的AHA算法调整预测粒子集,避免了过度优化或优化不足的问题且减少时间复杂度,并且引入中垂线收敛策略提高算法收敛速度,此外,引入Levy飞行策略以提升全局搜索能力,能实时纠正估计误差。

2.3 粒子分布多样性测试

为验证IAHA-PF算法进行状态估计时粒子分布的多样性,进行仿真实验对比PF与IAHA-PF算法的粒子分布情况。粒子总数为100,分别在t=10、t=25和t=45时刻测试了粒子分布情况,如图5所示。

(a) t=10时刻粒子分布对比图 (b) t=25时刻粒子分布对比图

可以看出,在不同采样时刻,粒子滤波算法在重采样阶段去除小权值粒子而保留大权值粒子,引起粒子多样性降低的现象,可以看到,几乎大部分粒子都集中在一个区域,易导致滤波精度降低的问题。相比之下,IAHA-PF算法中大部分粒子都集中在期望状态附近,同时有少部分粒子分布较分散,使得探索区域更广,增加了粒子多样性,有利于提高整体滤波精度。

2.4 自适应调整迭代次数分析

为进一步验证本文算法自适应调整迭代次数的准确性,分别在粒子数为50、100和150的情况下进行20次仿真实验,计算该算法的平均迭代次数,最后计算20次实验的平均迭代次数,结果如表3所示。

表3 不同粒子数的IAHA-PF平均迭代次数

可以看出,随着粒子数目的增加,IAHA-PF算法的平均迭代次数会相应减少,这是因为,粒子数的增加会提高粒子滤波的估计精度,达到期望优化程度的迭代次数会相应减少,由此可见,本文算法可根据当前优化程度自适应调整迭代次数,避免过度优化或优化不足导致的估计精度降低问题,且可自适应减少迭代次数,降低计算复杂度。

2.5 定位与建图实验及分析

进行仿真实验验证本文算法在SLAM中的估计性能,由于基于粒子滤波的FastSLAM因定位精度较高、鲁棒性较好等特性已发展成SLAM技术的主流算法[15],所以将传统FastSLAM与BA-FastSLAM、AHA-FastSLAM、IAHA-FastSLAM算法进行仿真对比实验,其中机器人的运动模型与观测模型如下:

(1)运动模型,本文移动机器人运动模型为:

(23)

式中:[xtytθt]T为t时刻机器人预测位姿,ΔT为机器人里程计采样时间间隔。

(2)观测模型,描述了机器人观测值与当前位姿估计和环境路标之间的关系。

(24)

式中:dt和βt分别为t时刻机器人观测到的路标距离和角度。

根据Bailey开发的SLAM算法通用模拟器[1]设计了尺寸为100 m×82 m,有42个路标的环境地图及机器人运行参照路径,模拟机器人在真实场景中进行瞬时定位与建图,仿真环境如图6所示,其中星号代表路标,细实线折线代表机器人的控制输入量,即规定路径,圆圈为机器人位姿点,共有14个。仿真环境的移动机器人相关参数设置如表4所示。

表4 仿真实验机器人参数设置

(a) 传统FastSLAM算法预测结果 (b) BA-FastSLAM算法预测结果

设置机器人相关参数后,机器人从坐标原点出发沿预设路径逆时针运动一圈,通过运动模型与观测信息实现实时定位与建图。仿真实验结果对比如图6所示。

可以看出,传统FastSLAM随着误差的累积,机器人路径随着时间推移误差增大,路标估计也出现较大误差;相比之下BA-FastSLAM的路径与路标估计误差都有所减小,但存在估计不稳定现象,AHA-FastSLAM与IAHA-FastSLAM的机器人路径与真实路径基本一致,且路标估计的误差明显减少;相比BA-FastSLAM与AHA-FastSLAM,IAHA-FastSLAM路径估计更稳定且误差更小,同时环境路标估计位置与实际路标位置基本一致。其原因在于:AHA-FastSLAM运用人工蜂鸟算法的智能觅食策略这一思想优化传统算法,能有效提高路径与路标估计性能,但仅依靠经验值会存在过度优化导致精度下降或优化不够导致误差较大的问题;IAHA-FastSLAM采用改进的AHA算法调整预测粒子集,避免了过度优化或优化不足的问题且减少时间复杂度,同时提升了AHA的收敛速度及全局搜索能力,能实时纠正机器人路径估计,降低路标估计误差。

为进一步验证本文算法应用到实际场合中的有效性,在该仿真环境中,分别对传统FastSLAM、BA-FastSLAM、AHA-FastSLAM、IAHA-FastSLAM4种算法进行位姿预测误差对比与路标位置预测误差对比,如图7～图9所示。

图7 x轴方向位姿估计误差

图9 路标位置估计误差

图7、图8为4种算法的位姿预测误差比较图。可以看出,IAHA-FastSLAM的位姿预测误差稳定且最小;从图9可以看出,IAHA-FastSLAM算法的路标位置预测误差相比BA-FastSLAM与AHA-FastSLAM算法更小。

考虑到实验结果的偶然性,为进一步证明本文提出的算法性能的优越性,引入均方根误差(RMSE)对机器人路径与路标位置估计进行评价[17],将4种算法分别进行30次仿真实验并取平均值,采用RMSE作为衡量指标,表达式为:

(25)

表5 4种算法x轴、y轴、路标位置均方根误差对比 (m)

从表5可知,当粒子数相同时BA-FastSLAM、AHA-FastSLAM和IAHA-FastSLAM算法的估计误差要明显小于FastSLAM,其中IAHA-FastSLAM的路径估计与路标估计误差最小,且IAHA-FastSLAM采用自适应调整迭代次数策略,时间复杂度更小。此外,IAHA-FastSLAM算法使用20个粒子的估计精度比FastSLAM算法使用100个粒子的估计精度更高,这是因为:本文引入混合策略的AHA更新预测粒子集,使预测粒子集在权重计算前就更靠近移动机器人真实位置,整体提高了粒子集质量,此外,在重采样阶段采用粒子重组策略提高了粒子多样性,有利于提高估计精度。因此,IAHA-FastSLAM算法不仅在粒子数相同时具有较高精度,还能减少粒子使用数量,以此减少计算量。

3 结论

本文提出一种基于多策略AHA优化的粒子重组粒子滤波算法,引入混合策略的AHA更新预测粒子集,在引导觅食阶段引入中垂线收敛策略提高算法收敛速度,区域性觅食阶段通过Levy飞行策略扩大搜索空间自适应调整粒子集分布,当粒子密集度过高时,自适应调整优化程度,使预测粒子集更快向移动机器人真实位置聚集,提高粒子集质量;在重采样阶段,考虑大权值粒子和部分小权值粒子的共同作用重组粒子集。通过仿真实验验证了IAHA-PF较好的估计性能,将该算法应用到SLAM中,仿真实验结果表明,在同等条件下,该算法定位及地图构建精度更高,综合性能更强。在后续工作中,将进一步合理减少算法时间复杂度,提高算法实时性,综合提升SLAM的估计性能。