基于萤火虫优化粒子滤波的机器人定位与建图方法及装置与流程

本发明涉及智能移动设备环境感知与自动化作业领域，尤其是涉及一种基于 萤火虫优化粒子滤波的机器人定位与建图方法及装置。

背景技术：

智能移动巡检设备中的同时定位和地图创建(Simultaneous Localization And Mapping,SLAM)技术是实现电力设备状态信息智能感知与自动化作业的核心问题， SLAM既是移动机器人自主移动的前提，又是后续路径规划与运动控制的基础，其 难点在于如何通过自身携带的传感器，采用滤波方法实现未知环境地图构建和精准 定位。

标准粒子滤波(Particle Filter,PF)在重要性采样时遵循次优选择原则，会导致样 本有效性的遗失，引起样本贫化现象；同时标准PF是以牺牲小权值粒子为代价来 进行重采样，经过多次循环迭代会导致粒子退化，降低了采样粒子的多样性，导致 定位预测精度难以满足高性能机器人精确定位的要求。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于萤火虫 优化粒子滤波的机器人定位与建图方法及装置。

本发明的目的可以通过以下技术方案来实现：

一种基于萤火虫优化粒子滤波的机器人定位与建图方法，包括：

步骤S1：获得采样初始时刻粒子；

步骤S2：通过机器人运动模型和观测模型得到t时刻的目标值，并通过重要 性采样得到t时刻的粒子集合；

步骤S3：利用改进后的萤火虫算法优化采样粒子的位置，分别得到所有粒子 t+1时刻优化后的位置；

步骤S4：计算优化后的粒子权重并进行归一化，筛选有效粒子，并基于有效 粒子进行重采样，直至粒子总数达到设定数目；

步骤S5：根据步骤S4得到的粒子群进行机器人位姿与环境估计。

所述步骤S3具体包括：

步骤S31：根据重定义后的相对亮度公式计算t+1时刻粒子i、j的相对亮度：

其中：I为相对亮度，pdist(·)euc为欧氏距离，为第i粒子的预测观测值，为第i粒子的最新观测值，为第i粒子的预测状态值，为第i粒子的最新状 态值；

步骤S32：计算粒子t+1时刻粒子i、j的欧式距离；

步骤S33：根据改进后的位置方程来更新粒子i的位置：

其中：xi'为更新后的粒子i所在的空间位置，xi为粒子i所在的空间位置，β0为 萤火虫最大吸引度，γ为光强吸收系数，rij为粒子i、j的空间距离，xj为粒子j所 在的空间位置，Rt为观测过程的噪声值，I(i)为粒子i相对亮度，I(j)为粒子j相对 亮度，rand为服从均值为0、方差为1的均匀分布随机数。

所述步骤S4具体包括：

步骤S41：计算粒子权重并按升序排列，采用基于偏差修正的指数加权平均算 法计算排序后粒子集的平均值δ，并将权重小于该平均值δ的粒子舍弃，并将剩余 的粒子作为有效粒子；

步骤S42：按照有效粒子的权重大小，对各有效粒子进行复制；

步骤S43：对各粒子权值降序排列，并利用高斯变异算子依次对粒子i位姿xi进行随机扰动从而产生新粒子x′i，直至满足总数N。

所述步骤S42中，各粒子的复制次数为：

其中：ni为粒子i的复制次数，g(·)为向上取整函数，为粒子i的权重，δ为 粒子权重的平均值。

所述步骤S43中新添粒子公式如下：

xi'＝xi+xi×μ×M(0,1)

其中：xi'为加入随机扰动后的粒子i的位置，xi为加入随机扰动前的粒子i的 位置，μ为扰动控制因子，M(0,1)为均值为0、方差为1的高斯分布随机变量。

一种基于萤火虫优化粒子滤波的机器人定位与建图装置，包括处理器、存储器， 以及存储于存储器中并由所述处理器执行的程序，所述处理器执行所述程序时实现 以下步骤：

步骤S1：获得采样初始时刻粒子；

步骤S2：通过机器人运动模型和观测模型得到t时刻的目标值，并通过重要 性采样得到t时刻的粒子集合；

步骤S3：利用改进后的萤火虫算法优化采样粒子的位置，分别得到所有粒子 t+1时刻优化后的位置；

步骤S4：计算优化后的粒子权重并进行归一化，筛选有效粒子，并基于有效 粒子进行重采样，直至粒子总数达到设定数目；

步骤S5：根据步骤S4得到的粒子群进行机器人位姿与环境估计。

与现有技术相比，本发明具有以下有益效果：

1)重定义后的荧光亮度公式中加入了最新时刻观测与状态值，重定义后的步 长因子公式中加入了观测过程的噪声值和重定义后的荧光亮度，分步重采样环节采 用基于偏差修正的指数加权平均算法筛选粒子，分步重采样环节利用高斯变异算子 依次对筛选后的大权值粒子进行随机扰动从而产生新粒子。通过重定义萤火虫算法 中的亮度公式提高了融合算法的寻优精度；同时改进了步长因子调整机制，解决了 固定步长因子导致的收敛速度慢、在极值点附近易“振荡”等问题，提高了粒子全 局寻优和局部寻优的精度与效率。

2)采用分步重采样环节考虑到了大权值粒子是影响估计精度的决定性因素， 提高了粒子筛选的准确性，也保证了复制与新添粒子的有效性，解决重采样造成的 样本退化和预估精度降低等问题。

附图说明

图1为本发明方法的主要步骤流程示意图；

图2为本发明萤火虫算法优化采样粒子位姿过程示意图；

图3为本发明分步重采样的粒子位姿优化过程示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方 案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范 围不限于下述的实施例。

针对标准粒子滤波运行机制存在的缺陷，本申请提出了一种机器人定位与建图 算法中的仿生滤波方法。仿生滤波方法将每个粒子当成群体中的个体，通过完成个 体间的消息交替与更新，使粒子快速且智能得集中至高似然区域实现粒子位置优化。

本申请将萤火虫优化算法(Firefly Algorithm,FA)与粒子滤波算法相结合，提出 一种改进的萤火虫优化粒子滤波算法，并将改进算法应用到机器人SLAM中，核 心是提高算法的熟练能力与预测精度，优化机器人对运行轨迹与环境路标的预测精 度与稳定性。

一种基于萤火虫优化粒子滤波的机器人定位与建图方法，在采用萤火虫优化粒 子滤波算法进行移动机器人定位与建图时，通过实时反馈最新时刻观测与状态值重 定义亮度公式、构建加入观测过程噪声值和相对亮度信息的非线性衰减函数；同时 采用基于偏差修正的指数加权平均算法和高斯扰动系数的分步重采样策略进一步 优化粒子分布，具体如下：相比邻近粒子，若当前粒子预测值和真实结果偏差较大， 说明邻近粒子位置更优，即当前粒子移动至邻近粒子位置，通过循环迭代得到全部 采样粒子的优化结果；进而再采用基于偏差修正的指数加权平均算法得到优化粒子 的筛选结果，由于筛选后已不满足粒子总数N，因此设计复制和利用高斯变异算子 的新添过程使粒子增加至N。具体的，该方法以计算机程序的形式由计算机系统实 现，对应的装置，包括处理器、存储器，以及存储于存储器中并由处理器执行的程 序，如图1所示，处理器执行程序时实现以下步骤：

步骤S1：获得采样初始时刻粒子；

步骤S2：通过机器人运动模型和观测模型得到t时刻的目标值，并通过重要 性采样得到t时刻的粒子集合；

步骤S3：利用改进后的萤火虫算法优化采样粒子的位置，分别得到所有粒子 t+1时刻优化后的位置，具体包括：

步骤S31：根据重定义后的相对亮度公式计算t+1时刻粒子i、j的相对亮度：

其中：I为相对亮度，pdist(·)euc为欧氏距离，为第i粒子的预测观测值，为第i粒子的最新观测值，为第i粒子的预测状态值，为第i粒子的最新状 态值，在机器人定位与建图时代表第i粒子的预测路标值、 最新时刻路标值、预测位姿值和最新时刻位姿值；

步骤S32：计算粒子t+1时刻粒子i、j的欧式距离；

步骤S33：根据改进后的位置方程来更新粒子i的位置：

其中：xi'为更新后的粒子i所在的空间位置，xi为粒子i所在的空间位置，β0为 萤火虫最大吸引度，γ为光强吸收系数，rij为粒子i、j的空间距离，xj为粒子j所 在的空间位置，Rt为观测过程的噪声值，I(i)为粒子i相对亮度，I(j)为粒子j相对 亮度，rand为服从均值为0、方差为1的均匀分布随机数。

步骤S4：计算优化后的粒子权重并进行归一化，筛选有效粒子，并基于有效 粒子进行重采样，直至粒子总数达到设定数目，具体包括：

步骤S41：计算粒子权重并按升序排列，采用基于偏差修正的指数加权平均算 法计算排序后粒子集的平均值δ，并将权重小于该平均值δ的粒子舍弃，并将剩余 的粒子作为有效粒子；

步骤S42：按照有效粒子的权重大小，对各有效粒子进行复制，各粒子的复制 次数为：

其中：ni为粒子i的复制次数，g(·)为向上取整函数，为粒子i的权重，δ为 粒子权重的平均值。

步骤S43：对各粒子权值降序排列，并利用高斯变异算子依次对粒子i位姿xi进行随机扰动从而产生新粒子x′i，直至满足总数N。新添粒子公式如下：

xi'＝xi+xi×μ×M(0,1)

其中：xi'为加入随机扰动后的粒子i的位置，xi为加入随机扰动前的粒子i的 位置，μ为扰动控制因子，M(0,1)为均值为0、方差为1的高斯分布随机变量。

步骤S5：根据步骤S4得到的粒子群进行机器人位姿与环境估计。