多策略鲸鱼算法优化粒子滤波的SLAM精度研究

蔡 艳,杨光永,黄训爱,徐天奇

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

0 引言

瞬时定位与地图构建(simultaneous localization and mapping,SLAM)是实现移动机器人自主工作的关键技术之一。目前,SLAM的求解方法可大致分为基于卡尔曼滤波器的方法、基于粒子滤波器的方法、基于图优化的方法[1]。基于蒙特卡洛方法的粒子滤波算法(particle filter,PF)不受非高斯噪声的影响,是常用于解决SLAM问题的方法之一。

由于粒子滤波算法基于蒙特卡洛方法,因此要达到所需估计精度需要大量的粒子,而粒子数量越多,算法时间复杂度越高;此外,重采样易导致样本多样性下降,出现粒子退化的问题。而进化问题和粒子滤波器本质上都是通过评价、选择和更新的迭代过程获得最优解[5],因此,很多研究者采用进化方法解决粒子滤波器存在的问题。如曹洁等[6]提出了权值抖动萤火虫算法和不完全重采样结合的方法改进粒子滤波,缓解了粒子退化粒子多样性贫化问题。刘海涛等[7]在基于遗传算法的智能粒子滤波基础上,提出对低权值粒子改进的智能粒子滤波(IIPF)处理策略,提高了滤波精度。李维刚等[8]提出了基于改进灰狼算法的新型粒子滤波方法,提高了粒子滤波的估计精度。韩锟等[9]提出一种基于果蝇优化思想的粒子滤波算法,将遗传算法中的交叉、变异操作自适应地应用到果蝇优化算法寻优过程中,有效提高了估计精度。陈志敏等[10]在粒子滤波中引入蝙蝠算法,设计了自适应闭环控制策略,对算法的全局搜索能力和局部搜索能力进行全程动态控制,进一步提高了粒子滤波的精度。李冀等[11]结合融入围猎策略的哈里斯鹰优化算法,设计一种群智能优化粒子滤波方法(EHHOPF),有效提升了系统状态估计精度及滤波稳定性。上述方法均提高了粒子滤波算法的性能,但大多都是通过经验值控制智能算法迭代次数,易造成优化不足或过度优化,从而引起估计精度降低。

受这些方法的启发,本文引入鲸鱼算法改善粒子滤波器的性能。鲸鱼优化算法(whale optimization algorithm,WOA)是2016年由Mirjalili等[12]提出的一种新的群体智能优化算法,该算法操作简单,设置的参数少及跳出局部最优的能力强,目前已被应用于三维路径规划[13]、制造工艺[14]、变压器故障诊断方法[15]、云上资源调度[16]等问题的求解之中。为避免仅依靠经验值设置迭代次数,本文引入自适应调整迭代次数策略,为增强其全局探索能力引入Levy飞行策略。

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

1 多策略鲸鱼算法优化的粒子重组PF

1.1 标准鲸鱼算法

鲸鱼优化算法(whale optimization algorithm,WOA)是参照座头鲸的气泡网捕食机制提出的一种新型启发式优化算法,该算法包括以下3种行为方式。

1) 包围狩猎

(1)

(2)

A=2ar1-a

(3)

C=2r2

(4)

式中：r1、r2为[0,1]之间的随机数;a从2到0线性下降,此阶段|A|≤1。

2) 螺旋包围

鲸鱼围捕猎物时,选择螺旋包围的概率是P,选择包围狩猎或随机搜索的概率是1-P,螺旋包围公式如下：

(5)

(6)

式中：l为(-1,1)内的随机数;β为定义螺线形状的常数参数。

3) 随机搜索

鲸鱼种群会根据彼此的位置进行随机搜索,借此找到更好的猎物,这样可以加强算法的勘察能力,从而进行全局搜索。随机搜索公式如下：

(7)

(8)

式中：Xrand为随机选择的鲸鱼位置向量。此阶段,A>1。

1.2 自适应调整策略

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

(9)

式中：Nb表示最优个体周围的鲸鱼数;rb表示密度半径;find操作表示查找密度半径内的鲸鱼数量。

ρb=Nb/N

(10)

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

1.3 Levy飞行策略

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

(11)

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

(12)

(13)

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

1.4 粒子重组重采样

为解决粒子滤波算法在重采样阶段去除小权值粒子导致估计性能降低的问题,在重采样阶段设计如下粒子筛选策略：

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

(14)

1.5 基于多策略鲸鱼算法的粒子重组粒子滤波

针对传统粒子滤波算法存在的问题,本文引入WOA算法对其进行优化以提高粒子集质量;为避免优化不足或过度优化,同时减少时间复杂度,采用自适应调整迭代次数策略;在随机搜索阶段,引入Levy飞行策略,扩大搜索空间自适应调整粒子集分布;在重采样阶段,采用粒子重组策略增加粒子多样性。IWOA-PF算法步骤如下：

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

步骤2从proposal分布中采样粒子集,t时刻的采样粒子集Xt如下：

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

(15)

步骤3IWOA优化,更新粒子集,如表1所示;

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

步骤5计算粒子权重;

步骤6粒子重组重采样,根据式(14)重组粒子集;

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

(16)

多策略鲸鱼算法伪代码如下：

IWOA算法：

1.初始化鲸鱼数量N、最大迭代次数T、个体维度d

2.初始化种群：Xi(i=1,2,…,N)

3.Whilet

4.检查是否有鲸鱼超出了搜索空间并进行修改

5.计算每条鲸鱼的适应度值,找到位置最佳的鲸鱼Xb

6.Fori=1 toNdo

7.根据式(3)和式(4)更新a,A,C,l

8.Ifp<0.5 then

9.If |A|≤1 then

10.使用式(1)—式(4)进行收缩包围策略

11.Else if |A|>1&&ρ≥pth

12.使用式(11)—式(13)执行Levy飞行策略

13.End If

14.Else

15.使用式(5)和式(6)执行螺旋包围策略

16.End If

17.使用式(9)和式(10)计算最优个体的粒子密度

18.Ifρ≥pmax

19.停止迭代

20.End if

21.End For

22.t=t+1

19.End While

20.返回最优解Xb、优化后的种群Xi

1.6 IWOA-PF算法时间复杂度分析

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

2 仿真实验

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

2.1 密度半径设定

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

表1 不同时刻不同密度半径下的粒子密度占比

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

2.2 粒子滤波估计精度测试

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

8cos[1.2(t-1)]+w(t)

(17)

(18)

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

图1 N=50时,3种算法对比实验

图2 N=80时,3种算法对比实验

图3 N=100时,3种算法对比实验

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

2.3 粒子分布多样性测试

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

图4 不同时刻粒子分布多样性实验结果示意图

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

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

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

表2 不同粒子数的IWOA-PF平均迭代次数

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

2.5 定位与建图实验及分析

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

1) 运动模型,本文仿真运动模型如下：

(19)

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

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

(20)

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

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

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

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

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

图5 3种算法预测结果示意图

为进一步验证本文算法应用到实际场合中的有效性,在该仿真环境中,分别对传统FastSLAM、WOA-FastSLAM以及IWOA-FastSLAM 3种算法,进行位姿预测误差对比与路标位置预测误差对比,如图6—图8所示。

图6、图7为3种算法的位姿预测误差曲线。可以看出,IWOA-FastSLAM的位姿预测误差稳定且最小;从图8可以看出,IWOA-FastSLAM算法的路标位置预测误差相比WOA-FastSLAM算法更小。

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

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

图8 路标位置估计误差

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

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

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

3 结论

1) 提出一种基于多策略WOA优化的粒子重组粒子滤波算法,引入混合策略的WOA更新预测粒子集,在随机搜索阶段,通过Levy飞行策略扩大搜索空间,自适应调整粒子集分布,当粒子密集度过高时,自适应调整优化程度,使预测粒子集更快向移动机器人真实位置聚集,提高粒子集质量;在重采样阶段,考虑大权值粒子和部分小权值粒子的共同作用重组粒子集。

2) 通过仿真验证了IWOA-PF较好的估计性能,将该算法应用到SLAM中,结果表明,当粒子数为20时,IWOA-FastSLAM相比传统FastSLAM,在x轴位姿估计精度提高了83.8%,在y轴位姿估计精度提高了77.6%,路标估计精度提高了81.4%。在同等条件下,该算法定位及地图构建精度更高,综合性能更强,为智能移动设备的自动化作业提供了一种精确定位与建图的方法,可为工业机器人智能化研究提供参考。