CN110095788A - 一种基于灰狼优化算法的rbpf-slam改进方法 - Google Patents

Abstract

本发明提出了一种基于灰狼优化算法的RBPF‑SLAM改进方法，用于进一步优化建议分布函数的分布预测，提高算法的定位和建图精度。灰狼算法模型简单、寻优性能强，其独特的搜索能力与探索开发平衡方式使得该算法与PSO等算法相比有着更好的全局搜索能力与更低的空间复杂度。该算法利用灰狼优化算法的局部探索和全局开发能力提高RBPF的估计性能，使得低权值粒子能够向最优值奔袭，并能够在奔袭过程中进一步优化粒子的估计值。在改进算法中，粒子退化得到了一定程度上的缓解，精确定位与建图所需的粒子数得到了有效的降低。

Description

一种基于灰狼优化算法的RBPF-SLAM改进方法

技术领域

本发明涉及移动机器人的定位和地图构建的科学研究，应用灰狼优化算 法对RBPF-SLAM算法中的粒子位姿进行进一步优化，从而进一步优化粒子集 的分布，提高了定位和建图的精度。

背景技术

移动机器人的智能化程度主要体现为它在所处环境中进行自主导航的能 力，即时定位与地图构建(SLAM)技术作为自主导航系统的核心，被认为是移 动机器人实现智能化和自主化的重要前提。移动机器人的SLAM技术是机器人 实现智能化和自主导航的关键前提，是机器人领域中的一个热点研究问题，由于 其对机器人导航的关键性和实现方法的复杂性，其研究与发展对于机器人智能化 和产业化有着极其重要的作用。

激光雷达具有测距精度高、测量范围广、采样密集、不受光线影响、抗干 扰能力强等优点，已广泛应用于工业和商业等领域，同时，基于RBPF滤波器的 方法实时性能好且能够在线处理定位与建图。然而该算法在迭代的过程中会出现 粒子退化和粒子耗散的问题，导致粒子集的分布不能很好的表示真正的后验概率 密度，造成定位和建图精度差甚至完全偏离的情况。

发明内容

本发明为了克服上述的缺陷，提出了一种基于灰狼优化算法的RBPF- SLAM改进方法，该方法优化了粒子的分布，提高了算法的建图和定位估计性能。

该发明利用灰狼优化算法的局部探索和全局开发能力提高RBPF的估计 性能，使得低权值粒子能够向最优值奔袭，并能够在奔袭过程中进一步优化粒子 的估计值。在改进算法中，粒子退化得到了一定程度上的缓解，精确定位与建图 所需的粒子数得到了有效的降低。

本发明采用以下技术方法予以实现，包括以下步骤：

第一步、采样：基于上一时刻的粒子和该时刻得到的里程计数据产生 时刻t的初步估计状态

第二步、根据得到的激光雷达传感器的数据，以及上一时刻各个粒子独立 的地图执行爬山扫描匹配算法，对上一步骤中的粒子位姿进行初步优化。

第三步、计算优化后的粒子得分，并将计算粒子得分的方式作为灰狼算法 的适应度函数计算方式求出灰狼个体的优劣。

第四步、进行灰狼算法优化处理，它对粒子的分布进行进一步优化，粒子 通过狩猎过程中的开发和探索步骤来优化位姿。

第五步、根据优化后的位姿并参考当前激光雷达扫描数据对各个粒子的 地图进行更新，将权值最高的粒子的地图作为当前估计的地图。

第一步中：

初步估计的粒子集由上一时刻的粒子集和里程计数据产生，这一步包括 从状态转移分布p(x_t|u_t,x_t-1)采样，采样后的粒子平均分配1/N的粒子权值,其中N 为粒子个数。

第二步中：

爬山扫描匹配算法从当前位姿开始，在周围栅格对位姿进行微调并作为 其比较点，如果当前位姿的匹配程度最高，则返回当前结点作为最大值；否则就 用最高的匹配点替代当前点，继续迭代参与迭代过程，如此循环达到局部最优状 态。

第三步中：

求出优化后的粒子得分，将匹配得分计算的适应度函数作为灰狼算法的 适应度函数。在对粒子进行得分计算的过程中，采用如下公式：

其中，S为匹配得分，b_m,i表示激光束i在全局坐标系下所对应的栅格， m_p,i为距b_m,i最近的栅格地图中的占用栅格，G_s表示高斯系数。

第四步中：

通过灰狼优化算法对粒子位姿进行进一步优化：将第二步得到的粒子集 作为初始狼群，将粒子个体当作灰狼个体，α狼代表最优粒子，β狼代表次优粒 子，σ狼代表第三优粒子。用适应度函数对灰狼个体的优劣进行计算，并使灰狼 群以α狼、β狼、σ狼为引导来寻找猎物即最优解，继而得到进一步优化后的粒 子集。

i)根据适应度函数计算每个灰狼个体的适应度，并找到最优个体α，次优 个体β和第三优个体σ。

ii)按照如下公式计算和的值：

其中，和为[0,1]上的随机变量，和为系数向量。在整个算法中， 参数是算法中的随机因子，能随机增加或减轻攻击猎物的难以程度，它使得 算法的开发与探索能力不仅仅依赖于迭代的次数。参数决定了算法的搜索趋势， 当时表明了灰狼正在扩大其搜索范围即侧重开发能力，而则说明灰狼 正在靠近猎物即侧重探索能力。的值由及决定，通常来说，的值随迭代 次数的增加而减小

的初始值为一标量，随着迭代次数的增加而递减，本文设计为：

其中，i为初始设定值，η为范围在[0,1]的系数，t为当前迭代次数，T为 最大迭代次数。η决定了算法的收敛程度，η越趋近于0则灰狼粒子越集中，差 异性就越小。系数η决定了粒子的聚集程度，可进行适当的调节。

iii)根据如下公式包围猎物，使灰狼围绕在猎物周围并能在下一时刻分布 在周围任意位置上：

其中，为求解当前狼与猎物之间距离的中间变量，为当前的猎物 位置，要更新的下一时刻的灰狼个体的位置。

iv)灰狼群体会感知猎物位置并跟随头狼攻击猎物。然而，在数学模型中 我们通常不知道最优解的位置，因此在处理中我们结合α狼，β狼和σ狼来确定 猎物的位置，如下式：

其中，分别为当前灰狼与α狼、β狼、σ狼之间距离的中 间变量，分别表示α狼、β狼、σ狼位置，和 分别为按照和计算公式求出的随机系数向量。

v)判断迭代次数是否大于最大迭代次数，如果是则结束灰狼优化，否则 跳转到步骤i)继续执行。

第五步中：

根据粒子上一时刻的地图、机器人位姿和激光雷达扫描信息，更新粒子的 当前地图，如果数据仍未处理完，则转到第一步继续执行，否则结束程序运行。

附图说明

图1所示是本发明的算法流程图

图2所示为传统算法和改进算法的最终建图效果。其中，图a表示传统 RBPF算法—Gmapping算法的最终构图效果，图b表示基于灰狼优化算法的 RBPF-SLAM改进方法。

具体实施方式

如图1所示，一种基于激光SLAM的室内定位方法，包括以下步骤：

第一步，采样。基于上一时刻的粒子和该时刻得到的里程计数据产生 时刻t的初步估计状态这一步包括从状态转移分布p(x_t|u_t,x_t-1)采样，采样 后的粒子平均分配1/N的粒子权值,其中N为粒子个数。

第二步，根据得到的激光雷达传感器的数据，以及上一时刻各个粒子独立 的地图执行爬山扫描匹配算法，对上一步骤中的粒子位姿进行初步优化。 爬山扫描匹配算法从当前位姿开始，在周围栅格对位姿进行微调并作为其比较点， 如果当前位姿的匹配程度最高，则返回当前结点作为最大值；否则就用最高的匹 配点替代当前点，继续迭代参与迭代过程，如此循环达到局部最优状态。

第三步，计算优化后的粒子得分，并将计算粒子得分的方式作为灰狼算法 的适应度函数计算方式求出灰狼个体的优劣。在对粒子进行得分计算的过程中， 采用如下公式：

其中，S为匹配得分，b_m,i表示激光束i在全局坐标系下所对应的栅格， m_p,i为距b_m,i最近的栅格地图中的占用栅格，G_s表示高斯系数。

第四步、进行灰狼算法优化处理，它对粒子的分布进行进一步优化，粒子 通过狩猎过程中的开发和探索步骤来优化位姿。将第二步得到的粒子集作为初始 狼群，将粒子个体当作灰狼个体，α狼代表最优粒子，β狼代表次优粒子，σ狼 代表第三优粒子。用适应度函数对灰狼个体的优劣进行计算，并使灰狼群以α狼、 β狼、σ狼为引导来寻找猎物即最优解，继而得到进一步优化后的粒子集。

i)根据适应度函数计算每个灰狼个体的适应度，并找到最优个体α，次优 个体β和第三优个体σ。

ii)按照如下公式计算和的值：

其中，和为[0,1]上的随机变量，和为系数向量。在整个算法中， 参数是算法中的随机因子，能随机增加或减轻攻击猎物的难以程度，它使得 算法的开发与探索能力不仅仅依赖于迭代的次数。参数决定了算法的搜索趋势， 当时表明了灰狼正在扩大其搜索范围即侧重开发能力，而则说明灰狼 正在靠近猎物即侧重探索能力。的值由及决定，通常来说，的值随迭代 次数的增加而减小

的初始值为一标量，随着迭代次数的增加而递减，本文设计为：

其中，i为初始设定值，η为范围在[0,1]的系数，t为当前迭代次数，T 为最大迭代次数。η决定了算法的收敛程度，η越趋近于0则灰狼粒子越集中， 差异性就越小。系数η决定了粒子的聚集程度，可进行适当的调节。

iii)根据如下公式包围猎物，使灰狼围绕在猎物周围并能在下一时刻分布 在周围任意位置上：

其中，为求解当前狼与猎物之间距离的中间变量，为当前的猎 物位置，要更新的下一时刻的灰狼个体的位置。

iv)灰狼群体会感知猎物位置并跟随头狼攻击猎物。然而，在数学模型中 我们通常不知道最优解的位置，因此在处理中我们结合α狼，β狼和σ狼来确定 猎物的位置，如下式：

其中，分别为当前灰狼与α狼、β狼、σ狼之间距离的中 间变量，分别表示α狼、β狼、σ狼位置，和 分别为按照和计算公式求出的随机系数向量。

v)判断迭代次数是否大于最大迭代次数，如果是则结束灰狼优化，否则跳 转到步骤i)继续执行。

第五步、根据优化后的位姿，参考当前激光雷达扫描数据对各个粒子的地 图进行更新，将权值最高的粒子的地图作为当前估计的地图，如果数据仍未处理 完，则转到第一步继续执行，否则结束程序运行。

为了验证本发明的有效性和优越性，在实际环境中进行了传统RBPF- SLAM算法(Gmapping)与基于灰狼优化算法的RBPF-SLAM改进算法进行了 对比实验，实验中采用5个粒子，并设置地图分辨率为5cm。两种算法的建图效 果如图2所示，改进算法要明显优于Gmapping算法。

Claims (6)

1.一种基于灰狼优化算法的RBPF-SLAM改进方法，其特征在于，包括以下步骤：

第一步、采样：基于上一时刻的粒子和该时刻得到的里程计数据产生时刻t的初步估计状态

第二步、根据得到的激光雷达传感器的数据，以及上一时刻各个粒子独立的地图执行爬山扫描匹配算法，对上一步骤中的粒子位姿进行初步优化；

第三步、计算优化后的粒子得分，并将计算粒子得分的方式作为灰狼算法的适应度函数计算方式求出灰狼个体的优劣；

第四步、进行灰狼算法优化处理，它对粒子的分布进行进一步优化，粒子通过狩猎过程中的开发和探索步骤来优化位姿；

第五步、根据优化后的位姿，参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权值最高的粒子的地图作为当前估计的地图。

2.根据权利要求1所述的一种基于灰狼优化算法的RBPF-SLAM改进方法，其特征在于，第一步中：

初步估计的粒子集由上一时刻的粒子集和里程计数据产生，这一步包括从状态转移分布p(x_t|u_t,x_t-1)采样，采样后的粒子平均分配1/N的粒子权值,其中N为粒子个数。

3.根据权利要求1所述的一种基于灰狼优化算法的RBPF-SLAM改进方法，其特征在于，第二步中：

爬山扫描匹配算法从当前位姿开始，在周围栅格对位姿进行微调并作为其比较点，如果当前位姿的匹配程度最高，则返回当前结点作为最大值；否则就用最高的匹配点替代当前点，继续迭代参与迭代过程，如此循环达到局部最优状态。

4.根据权利要求1所述的一种基于灰狼优化算法的RBPF-SLAM改进方法，其特征在于，第三步中：

求出优化后的粒子得分，将匹配得分计算的适应度函数作为灰狼算法的适应度函数；在对粒子进行得分计算的过程中，采用如下公式：

其中，S为匹配得分，b_m,i表示激光束i在全局坐标系下所对应的栅格，m_p,i为距b_m,i最近的栅格地图中的占用栅格，G_s表示高斯系数。

5.根据权利要求1所述的一种基于灰狼优化算法的RBPF-SLAM改进方法，其特征在于，第四步中：

通过灰狼优化算法对粒子位姿进行进一步优化：将第二步得到的粒子集作为初始狼群，将粒子个体当作灰狼个体，α狼代表最优粒子，β狼代表次优粒子，σ狼代表第三优粒子，用适应度函数对灰狼个体的优劣进行计算，并使灰狼群以α狼、β狼、σ狼为引导来寻找猎物即最优解，继而得到进一步优化后的粒子集；

i)根据适应度函数计算每个灰狼个体的适应度，并找到最优个体α，次优个体β和第三优个体σ；

ii)按照如下公式计算和的值：

其中，和为[0,1]上的随机变量，和为系数向量；在整个算法中，参数是算法中的随机因子，能随机增加或减轻攻击猎物的难以程度，它使得算法的开发与探索能力不仅仅依赖于迭代的次数；参数决定了算法的搜索趋势，当时表明了灰狼正在扩大其搜索范围即侧重开发能力，而则说明灰狼正在靠近猎物即侧重探索能力，的值由及决定，通常来说，的值随迭代次数的增加而减小；

的初始值为一标量，随着迭代次数的增加而递减，本文设计为：

其中，i为初始设定值，η为范围在[0,1]的系数，t为当前迭代次数，T为最大迭代次数；η决定了算法的收敛程度，η越趋近于0则灰狼粒子越集中，差异性就越小，系数η决定了粒子的聚集程度，可进行适当的调节；

iii)根据如下公式包围猎物，使灰狼围绕在猎物周围并能在下一时刻分布在周围任意位置上：

其中，为求解当前狼与猎物之间距离的中间变量，为当前的猎物位置，要更新的下一时刻的灰狼个体的位置；

iv)灰狼群体会感知猎物位置并跟随头狼攻击猎物，然而，在数学模型中我们通常不知道最优解的位置，因此在处理中我们结合α狼，β狼和σ狼来确定猎物的位置，如下式：

其中，分别为当前灰狼与α狼、β狼、σ狼之间距离的中间变量，分别表示α狼、β狼、σ狼位置，和 分别为按照和计算公式求出的随机系数向量；

v)判断迭代次数是否大于最大迭代次数，如果是则结束灰狼优化，否则跳转到步骤i)继续执行。

6.根据权利要求1所述的一种基于灰狼优化算法的RBPF-SLAM改进方法，其特征在于，第五步中：

根据粒子上一时刻的地图、机器人位姿和激光雷达扫描信息，更新粒子的当前地图，如果数据仍未处理完，则转到第一步继续执行，否则结束程序运行。