基于IINFO的粒子重组FastSLAM在SLAM中的研究*

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

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

0 引言

随着机器人技术的快速发展,机器人自主作业的需求日益增加,而瞬时定位与建图(simultaneous localization and mapping,SLAM)是机器人智能化的关键技术。目前SLAM的求解途径大致可归纳为基于最优滤波的方法和基于图优化的方法[1]。其中,基于滤波的方法主要包括基于扩展卡尔曼滤波和基于粒子滤波的方法[2]。基于粒子滤波的FastSLAM[3]算法使用蒙特卡洛方法[4],易实现且具有非线性,是常用于解决SLAM问题的方法之一。

由于粒子滤波算法基于蒙特卡洛方法,因此需要大量粒子才能很好的估计机器人路径与环境地图,然而粒子数量越多,算法时间复杂度越高;此外,重采样会不断拷贝权重较大的样本,抛弃权重较小的样本,易导致样本多样性下降,出现粒子退化的问题。而进化问题和粒子滤波器本质上都是通过评价、选择和更新的迭代过程获得最优解[5],因此很多研究者采用进化方法来解决粒子滤波器存在的问题。MORENO等[6]基于进化概念,在状态空间中随机搜索最佳粒子位姿估计,提出了一种称为进化定位滤波器(ELF)的进化定位算法。YIN等[7]引入遗传算法,避免粒子提早收敛,同时提高粒子集的质量,从而减少所需样本。

上述方法均提高了粒子滤波算法的性能,但都只是通过提高粒子集质量来提高算法性能,对缓解粒子贫乏效果并不佳。受这些方法的启发,本文引入向量加权平均算法这一智能算法来改善粒子滤波器的性能。向量加权平均算法(weighted mean of vectors algorithm,INFO)是一种新型智能优化算法[11],该算法通过向量的不同加权平均规则,来达到寻优目的。相比之前的智能算法,如粒子群算法(PSO)、灰狼优化算法(GWO)、引力搜索算法(GSA)等,该算法的搜索精度、收敛速度、稳定性更佳[11],但仍可能陷入局部最优,本文引入柯西变异算子来提高INFO跳出局部最优的能力。

本文为进一步提高FastSLAM算法的性能,在预测粒子集时加入机器人最新时刻的观测信息并通过引入柯西变异改进的INFO收敛速度快、寻优能力强的特性更新预测粒子集,提高粒子集质量;根据改进INFO获取的最优解调整预测粒子集的提议分布,使预测粒子集在权重计算前就更接近期望值,因此提高路径和路标的估计精度;在重采样阶段通过重组粒子集来增加粒子的多样性。为验证本文算法的优越性,将本文算法与传统FastSLAM、INFO-FastSLAM算法在仿真实验环境下进行对比。

1 改进向量加权平均优化的重组FastSLAM

1.1 改进向量加权平均算法

向量加权平均算法(weighted mean of vectors algorithm,INFO),是一种新型智能优化算法。该算法通过向量的不同加权平均规则,来达到寻优目的,具有寻优能力强,收敛速度快等特点。INFO分为3个阶段,分别为更新规则阶段、向量合并阶段与局部搜索阶段。

(1)更新规则阶段。INFO使用基于均值的规则(MeanRule)更新向量的位置,此外,为了提高全局搜索能力,在更新规则算子中加入了收敛加速部分。更新规则的主要表达式如下:

(1)

(2)

(3)

(3)局部搜索阶段。INFO使用局部搜索阶段来防止陷入局部最优解。根据该算子,如果rand<0.5,则可以围绕生成一个新的向量。

(4)

INFO在处理寻优问题时收敛速度较快,但仍可能陷入局部最优。为提高其跳出局部最优的能力,本文引入柯西变异算子来改进INFO算法。

柯西变异来源于连续型概率分布的柯西分布[14],主要特点为原点处的峰值较小,从峰值到零值下降缓慢,使变异范围更均匀。在求得当前最优解后,对其进行变异操作,表达式为:

xnewbs=xbs+xbs×Cauchy(0,1)

(5)

式中:Cauchy(0,1)为标准柯西分布。

1.2 粒子重组重采样

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

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

(6)

1.3 基于改进向量加权平均的重组FastSLAM算法

针对传统FastSLAM算法存在需要大量粒子来提高精度与粒子多样性缺失的问题,本文引入IINFO算法对FastSLAM进行优化。

通过适应度函数来衡量IINFO算法对机器人位姿估计的精确程度,如下:

(7)

ztPred=g(st,μnt,t-1)

(8)

算法步骤如下:

步骤1:粒子采集时将传感器观测到的最新路标信息加入粒子集;

步骤2:IINFO优化,更新粒子集:

①根据粒子集初始化向量集;

②对粒子集中所有粒子根据式(7)计算适应度函数;

③更新规则阶段,根据式(1)和式(2)更新向量位置;

④向量合并阶段,通过更新规则阶段得到的向量根据式(3)合并成新的向量;

⑤局部搜索阶段,如果rand<0.5,则根据式(4)生成一个新的向量;

⑥根据式(5)变异最优个体的位置xbs,使其偏离当前位置;

⑦计算适应度值,筛选最优个体,判断是否达到最大迭代次数,若达到则输出xnewbs,否则返回③。

步骤3:根据IINFO返回的最优解更新粒子集分布;

步骤4:计算粒子权重;

步骤5:计算Neff,当Neff小于阈值时执行步骤6,否则执行步骤7;

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

步骤7:使用扩展卡尔曼滤波进行路标更新。

基于IINFO优化的线性重组FastSLAM算法流程图如图1所示。

图1 基于IINFO的粒子重组FastSLAM算法流程图

2 仿真实验

2.1 仿真模型

(1)运动模型。本文移动机器人运动模型如下:

(9)

式中:[xtytθt]T表示t时刻机器人预测位姿,ΔT为机器人里程计采样时间间隔,vt为移动机器人运动速度,ω为角速度,ωΔt为经过Δt时间后机器人转向角,L为轴间距,ε为里程计噪声。

(2)观测模型。激光雷达观测模型描述了机器人观测值与当前位姿估计和环境路标之间的关系。本文机器人观测模型如下:

(10)

式中:dt与β分别为t时刻机器人观测到的路标距离和角度,(xLj,yLj)为机器人当前观测到的第j个环境路标位置,δ为观测噪声。

2.2 实验结果及分析

为验证本文算法有较好的估计性能,在实验平台MATLAB R2020b下将INFO-FastSLAM算法、IINFO-FastSLAM算法与传统的FastSLAM算法进行对比实验,并进行误差对比分析。

本文模拟机器人在真实场景中进行瞬时定位与建图来搭建实验环境,仿真环境如图2所示,其中星号代表路标,图中共有42个设定路标点;折线代表机器人的控制输入量,即规定路径;圆圈为机器人位姿点,共有14个。其中,路标点与位姿点均为人为设定,仿真环境的移动机器人相关参数设置如表1所示。

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

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

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

可以看出,传统FastSLAM随着误差的累积,机器人路径随着时间误差增大,路标估计也出现较大误差;INFO-FastSLAM的机器人路径与真实路径基本一致,且路标估计的误差明显减少;IINFO-FastSLAM相比INFO-FastSLAM,其预测轨迹更接近实际路径,且环境路标估计位置与实际路标位置基本一致。其原因在于:INFO-FastSLAM运用向量加权平均优化这一思想优化传统算法,能有效提高路径与路标估计性能;IINFO-FastSLAM采用柯西变异的INFO算法调整预测粒子集,能实时纠正机器人路径估计,降低路标估计误差。

2.3 实验误差对比

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

图4 x轴方向位姿估计误差 图5 y轴方向位姿估计误差

图6 路标位置估计误差

图4和图5为传统FastSLAM、INFO-FastSLAM和IINFO-FastSLAM三种算法的位姿预测误差比较图。可以看出,IINFO-FastSLAM算法的位姿预测误差最小;从图6可以看出,IINFO-FastSLAM算法的路标位置预测误差同样最小。

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

(11)

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

从表2可知,当粒子数相同时INFO-FastSLAM和IINFO-FastSLAM算法的估计误差要明显小于FastSLAM,且IINFO-FastSLAM的估计误差更小。此外,IINFO-FastSLAM算法使用10个粒子的估计精度比FastSLAM算法使用100个粒子的估计精度更高,因此,IINFO-FastSLAM算法不仅在粒子数相同时具有较高精度,还能减少粒子使用数量,以此减少计算量。

3 结论

本文提出一种基于改进INFO优化的FastSLAM算法,在预测粒子集时加入机器人最新时刻的观测信息并通过改进INFO算法计算粒子适应度值,为避免INFO陷进局部最优,引入柯西变异算子进行优化;通过IINFO算法调整粒子分布,使预测粒子集更快向移动机器人真实位置聚集,提高粒子集质量;在重采样阶段,考虑大权值粒子和部分小权值粒子的共同作用重组粒子集。仿真实验结果表明,文章提出的IINFO-FastSLAM相比于传统的FastSLAM算法在同等条件下具有更高的定位及地图构建精度,综合提高了SLAM性能。在后续工作中,将在此基础上进一步合理自适应调整迭代次数以减少算法时间复杂度,综合提高FastSLAM性能,并将其应用于解决复杂环境的移动机器人SLAM问题。