基于狮群优化的FastSLAM算法

周宁亚 黄友锐 韩 涛

(安徽理工大学电气与信息工程学院 安徽 淮南 232001)

0 引 言

同时定位与建图(SLAM)是机器人在未知环境下进行自主导航等任务的基本问题[1]，FastSLAM是一种成功求解SLAM问题的算法。FastSLAM中使用的RaoBlackwellized Particle Filter(RBPF)[2]算法与其他算法相比，具有线性时间复杂度和多假设数据关联两大优点。然而，FastSLAM在精度方面会随时间发生退化[3-5]，当机器人估计姿态的粒子集失去多样性时，就会发生这种退化。FastSLAM中粒子多样性的丧失主要有两个原因：(1) 由于目标分布与建议分布不匹配导致样本贫化，产生不可靠粒子，导致对机器人姿态估计不准确。(2) 在FastSLAM重采样过程中，不可靠的粒子被丢弃，只有权重大的粒子存活。但是，如果准确估计机器人姿态的粒子权重较低，则会被误丢弃，那么被丢弃的粒子中存储的机器人信息将无法恢复。换句话说，对粒子权重的错误计算导致了粒子损耗问题。

近年来，许多算法被提出来克服粒子耗尽问题。Kwak等[6]对FastSLAM中使用的几种粒子滤波器进行了分析，提出了一种针对损耗问题的补偿技术。Kim等[7]提出了一种Unscented FastSLAM算法，该算法利用无迹变换进行粒子滤波、特征初始化和特征估计。Cugliari等[8]利用无迹变换提高了粒子滤波和特征估计器的精度，并提出了一种自适应选择性重采样技术。到目前为止，这些技术已经显示出比传统的FastSLAM更好的性能，但其对缓解粒子退化问题效果并不佳，只是试图保持多样性来改进FastSLAM算法。

为了提高FastSLAM的性能，本文引入了群体智能算法——狮群优化算法。狮群算法[9-10]比遗传算法、粒子群算法和万有引力算法等具有更好的全局收敛性，收敛速度更快，精度更高，能更好地获得全局最优解。首先通过狮群优化算法对FastSLAM中预测粒子进行更新，调整粒子的建议分布。其次对粒子群进行分工合作扩大搜索范围，增加粒子多样性。最后通过“适者生存”的竞争法则促使粒子更快地朝着真实的机器人位姿状态逼近，减缓粒子退化。

1 基本SLAM问题

从概率的角度来看，SLAM问题涉及对机器人路径后验值的估计以及对地图的估计。

p(xt,m|zt,ut,nt)

(1)

式中：xt={x1,x2,…,xt}为机器人的路径，xt表示t时刻机器人的位姿；m为地图，由一系列环境特征组成，每个特征用mn表示，静止特征的总数设为N，为了计算方便，通常认为它们的坐标和数量是已知的且坐标是唯一确定的，相互是独立的；zt={z1,z2,…,zt}、ut={u1,u2,…,ut}分别为t时刻之前的机器人的观测量和控制量。

将SLAM问题画成一个动态贝叶斯网络[11]可以更方便地理解这一点。如图1所示，机器人在t时刻机器人的位姿记为xt，该位姿是机器人位姿xt-1和机器人执行的控制量ut的概率函数，传感器在时间t的观测值，记作zt，由位姿xt和观测到的路标mn决定。机器人在t-1时刻和t+1时刻观察到的路标均是mi，在t时刻观察到的路标是mj，从xt-1到xt为机器人的完整路径。

图1 SLAM动态贝叶斯网络

2 算法设计

2.1 传统FastSLAM算法

FastSLAM算法是由Montemerlo等[14]首先提出来的，利用一组随机地从分布中抽取出的粒子(样本)来表示机器人的位姿。Montemerlo等利用贝叶斯理论将式(1)的条件联合概率分布分解为条件概率：

p(xt,m|zt,ut,nt)=p(xt|zt,ut,nt)·

(2)

式中：nt={n1,n2,…,nt}为相关性变量，每个nt指定t时刻观测到的路标的标识。这一因子分解说明SLAM问题可以分解为N+1个递归估计量的乘积：机器人路径上的一个估计量，路标位置上的N个独立估计量，且每个独立估计量都以路径估计为条件(每个路标是相互独立的)。等式右边前一项被称为路径估计，FastSLAM算法利用粒子滤波器来实现此估计，粒子滤波器中的每个粒子都代表一条机器人可能的路径，利用观测信息计算每个粒子的权重大小来评价每条路径的好坏；后一项是基于该路径估计下的路标估计，采用N个扩展卡尔曼滤波器分别估计N个路标。

所以，在FastSLAM中每个粒子k可以表示为：

(3)

FastSLAM算法步骤如下：

(1) 采样新位姿，扩展对机器人路径的后验估计：

(4)

(2) 利用扩展卡尔曼滤波器更新观察到路标估计。

(3) 计算重要性权重：

(4) 根据权重判断是否进行重要性重采样：

(5)

式中：Neff为有效粒子数；ω[k]表示第k个粒子的权重。当Neff小于设置的阈值时进行重采样，否则不进行。

2.2 狮群算法

狮群算法[10]是一种新型群体智能算法，其根据自然界狮子真实的生存状态将狮群按照一定比例分为雄狮、母狮和幼狮三类。

狮王是狮群中最强壮和凶猛的公狮，是在“弱肉强食，胜者为王”的自然界生存法则产生的首领，有守护领土和保护幼狮以及分配食物的职责。

母狮在狮群中的任务主要是捕猎和照顾幼狮。它们首先探寻猎物的踪迹，再通过协作来进行围捕。在追捕猎物时先进行大范围的搜索，当靠近猎物时，缩小包围圈，然后围捕猎物。

幼狮在狮群中属于跟随狮，主要有三种活动，一是饥饿时向狮王附近位置移动寻找食物；二是食饱后跟随母狮学习捕猎；三是长大后将被驱赶出狮群进行磨练，在经过磨练后可以对狮王发起挑战。

狮群优化算法的主要思想如下:从待寻优空间中的某一初始位置开始，其中具有最佳适应度值的就是狮王，然后选取一定比例的捕猎狮，捕猎狮相互配合捕猎，一旦发现比当前狮王占有的猎物更优质的猎物，该猎物的位置会被狮王拥有。幼狮跟随母狮学习打猎或在狮王附近进食，成年后会被驱赶出狮群，为了生存，被驱赶的狮子会努力朝记忆中的最佳位置靠近。狮群内部分工合作，不断重复搜寻，最终得出目标函数最优值。

2.2.1狮群初始化

设狮群中的狮子数量为Q，维度空间为D，其中成年狮子的数量为qLeader：

(6)

式中：β为成年狮所占比例因子，为(0,1)内的一个随机数。令待寻优的阈值向量为yi=(yi1,yi2,…,yiD),1≤i≤Q。捕猎过程中不同类型的狮子都会按照自己的方式来移动自身的位置。

2.2.2狮王守护

从待寻优空间的初始化位置开始，计算适应度值，其中具有最佳适应度值的为狮王，记为Llion。狮王在最佳食物处小范围移动，以确保自己的特权，按照式(7)更新自己的位置:

(7)

狮王只负责照顾幼狮和保护领土和给幼狮分配食物，直到进入下一次迭代，被更为强壮和凶猛的成年狮替代。

2.2.3母狮捕猎

确定狮王后，选取一定比例的捕猎狮，开始时目标搜索空间只有一头公狮，所以母狮的数量为nLeader-1。母狮在捕食过程中需要与另一头母狮合作，按式(8)更新自己的位置：

(8)

(9)

式中：αf为母狮移动范围扰动因子，这是为了加强局部开发能力，让母狮在捕猎过程中先在较大的范围内勘探食物，确定大致范围后，勘探范围慢慢缩小，后期保持活动范围趋于零的微小值；step为狮子的最大步长；T为种群最大迭代次数；t为当前迭代次数。

2.2.4幼狮跟随

幼狮是跟随狮，主要围绕狮王和母狮进行活动，按式(10)计算和调整自己的位置。

(10)

狮群优化步骤如下：

(1) 初始化相关参数。

(2) 根据式(6)计算三类狮子数量，并确定各个狮子此刻的位置，占据最优位置的为狮王。

(3) 根据式(7)更新狮王位置，并计算适应度值。

(4) 通过式(8)更新母狮的位置。

(5) 产生(0,1)内的均匀随机数q，通过式(10)更新幼狮的位置。

(6) 计算适应度值，更新自身历史最优位置和狮群历史最优位置，判断算法是否满足终止条件，若满足跳转到步骤8，否则跳转步骤7。

(7) 间隔一定的迭代次数(约10次)对狮群进行重新排序，重新确定三类狮子的位置，跳转到步骤3。

(8) 输出狮王的位置，即所求问题的最优解，算法结束。

2.3 基于狮群优化的FastSALM算法

针对传统FastSLAM出现的粒子退化和多样性缺失问题，本文引入狮群这一新型群智能算法优化FastSLAM。狮群内部分工合作，扩大搜索范围，减缓粒子多样性缺失，加快收敛，通过不断重复搜寻最优值的方法，来优化调整机器人的建议分布情况，促使粒子集朝最优的真实位置移动，同时改善FastSLAM算法中出现的粒子退化和多样性缺失问题。

算法步骤如下：

(1) 根据式(4)进行采样，获取新粒子集。

(2) 对新粒子集进行狮群优化，调整粒子分布情况：

① 对新粒子集中的所有粒子计算适应度函数，确定狮群构成。

② 位置更新，狮王根据式(7)进行位置更新；母狮根据式(8)进行位置更新；幼狮根据式(10)进行位置更新。

③ 更新适应度函数，进而更新自身历史最优位置。

④ 判断是否达到迭代次数，若达到输出最优解，否则返回步骤②。

(3) 根据式(5)计算粒子权值。

(4) 重要性重采样：计算Neff判断是否进行重采样。

(5) 更新地图：利用扩展卡尔曼进行更新。

对传统FastSLAM算法引入狮群优化，调整粒子的建议分布，使得粒子集更快地朝着真实的机器人位姿状态逼近，为下一刻采样提供更好的输入值。

3 仿真实验

3.1 仿真模型

(1) 运动模型。本文仿真运动模型为：

(11)

(2) 观测模型。观测模型zt使用传感器与某一路标的距离值和角度值表示，在t时刻，机器人的观测模型为：

(12)

式中：rt和φt分别表示机器人在t时刻离观测到的路标的距离和角度值；(xi,yi)表示观测到的第i个路标的坐标。

(3) 噪声模型。移动机器人在运动过程中，会受到机器人轮子打滑、外界干扰以及传感器等误差的影响，这种影响会给实际测量值带来很多不确定性，为了简单起见，我们通常用高斯白噪声近似表示噪声模型。

3.2 实验结果与分析

为验证本文算法的正确性和有效性，对本文提出的LSO-FastSLAM算法进行仿真实验，并与基础FastSLAM算法进行对比，仿真平台是在MATLAB R2012a版本下，使用悉尼大学发布的模拟器[13]进行测试的。

仿真模拟器环境界面如图2所示，界面尺寸为200 m×160 m。仿真环境采用黑色星星为路标点，数量设为35个，黑色空心圆为位姿点，数量设为17个，黑色细曲线为机器人预设路径轨迹，仿真模拟机器人在环境中运行的轨迹。

图2 MATLAB模拟器仿真界面

实验环境配置为：Intel酷睿i3，4 GB内存，300 GB固态硬盘，Windows 7系统。软件开发环境为MATLAB 2010a。仿真程序中设置狮群的群体规模，向量空间维数D=2，最大迭代次数T=100，根据文献[10]并经过多次仿真实验发现比例因子β=2时收敛效果最好。

移动机器人的相关参数如表1所示。

表1 机器人仿真参数设置

续表1

对机器人相关参数设置后，机器人从坐标为(0,0)位置开始沿着位姿点逆时针运动行走一周。分别对两种算法进行仿真实验，实验结果如图3和图4所示。

图3 传统FastSLAM算法估计结果

图4 基于狮群优化的FastSLAM估计结果

对比图3、图4可以看出：传统FastSLAM算法随着累积误差的影响，后面机器人的路径严重偏离了真实路径，路标估计也出现较大误差；加入狮群优化的FastSLAM算法估计的路径与真实路径基本一致，估计的路标位置也基本一致，精度得到大幅提高。

图5为传统FastSLAM和LSO-FastSLAM两种算法的误差对比仿真图。由图5(a)、(b)可以看出，LSO-FastSLAM算法的位姿估计误差均明显低于传统FastSLAM算法，且误差幅度变化不大；由图5(c)可以看出，LSO-FastSLAM算法的路标估计误差也明显低于传统FastSLAM算法。

(a) x轴方向上的位姿估计误差

(b) y轴方向上的位姿估计误差

(c) 路标位置估计误差图5 算法仿真结果对比图

为了进一步说明本文算法的优越性能，引入均方根误差RMSE[14]，本文算法性能用机器人路径和路标位置的均方根误差(RMSE)进行评价，计算公式如下：

(13)

考虑到实验具有偶然性，对此本文分别对两种算法在图1环境下运行30次仿真实验，然后分别计算机器人位姿估计和路标估计的RMSE值，如表2所示。

表2 算法性能比较

可以看出，本文提出的LSO-FastSLAM算法的RMSE不仅在路标和路径上优于传统FastSLAM算法，而且在运行时间上也明显低于传统算法，提高了FastSLAM的实时性。

4 结 语

本文提出了一种基于狮群优化的FastSLAM算法，该算法创新性地将狮群优化思想引入到传统FastSLAM算法中，通过狮群优化算法内部分工合作，扩大搜索范围，增加粒子多样性，利用其优越的全局收敛性，不断迭代搜寻最优值，促使粒子更快地朝着真实的机器人位姿状态逼近，减缓粒子退化。仿真实验验证了在等同条件下，本文算法的可行性和优越性。