一种基于狮群算法优化的SLAM算法

一种基于狮群算法优化的slam算法
技术领域
1.本发明涉及slam算法，具体涉及一种基于狮群算法优化的slam算法。


背景技术：

2.fastslam算法简述：slam问题表述的是：移动机器人在未知环境下，通过此环境的相关特征（路标），构建一致地图，同时通过该地图获取自身位置。而机器人定位与建图问题是相互耦合的，机器人定位精度影响其建图准确性，同时，机器人建立地图的精度会影响机器人对自身位置的确定。在slam算法研究过程中，根据机器人自身携带传感器的差异可分为激光slam与视觉slam。本发明主要激光slam算法，在激光slam算法研究过程中，由于算法采用的滤波理论不同，可分为基于扩展卡尔曼滤波器的ekf
‑
slam算法，与基于粒子滤波器的fastslam算法，其中基于扩展卡尔曼滤波器的ekf
‑
slam算法在进行状态估计时，一般都假设测量嗓声服从高斯分布，然而在实际应用中，在复杂环境下，噪声分布处于非线性，非高斯状态，因而机器人定位建图精度会存在较大的误差，因此本发明针对fastslam算法进行优化。
3.fastslam算法的系统状态估计通过rao
‑
blackwellised粒子滤波器分解为对机器人运动轨迹的递归估计和基于机器人轨迹的路标位置估计这两部分，用公式表示如下： 其中，表示地图路标的个数。在fastslam算法中，粒子滤波算法用来估计机器人运动轨迹，算法中的每个粒子都保存有一份完整的地图信息；地图估计可分解成个相互独立的路标位置估计，路标的位置用扩展卡尔曼滤波器进行估计。
4.因此在fastslam中，若选取个粒子，则总共有个扩展卡尔曼滤波器。时刻的第个粒子的数据结构表达式如下： 其中，表示第个粒子对机器人当前时刻的位姿估计值，表示机器人当前状态的协方差矩阵，表示第个粒子的权重，和分别表在第个粒子维护的地图中，第个环境特征的全局坐标和协方差。fastslam算法的结构如图1所示：归纳得到fastslam算法步骤如下所示：步骤1：初始化
对移动机器人的初始状态位姿、协方差矩阵、测量协方差矩阵等参数进行初始化设置，并从分布函数中采样个粒子样本，粒子权值设定为。
5.步骤2：预测根据时刻的后验概率运动模型，预测机器人时刻位姿状态。
6.步骤3：数据关联与获得提议分布将时刻观测信息依次关联时刻各个粒子的地图估计，更新粒子的先验分布，得到粒子的提议分布。
7.步骤4：重采样首先判断是否需要重采样，计算当前时刻粒子集的有效粒子数，若小于给定阈值，则进行重采样，得到新的粒子集，否则不需要进行重采样。
8.步骤5：路径估计与地图更新用sir粒子滤波器去估计运动路径，可以得到时刻后验概率的分布的粒子集，将没关联上的观测特征加入地图，更新地图并删除伪特征。
9.经典的fastslam算法存在以下缺点：（1）粒子贫化问题：fastslam算法系统状态估计通过rao
‑
blackwellised滤波器分解为路径轨迹估计和地图估计这两个两部分，用带有不同权重的粒子表示机器人运动状态以及地图特征。但由于粒子不断进行重采样，以保证粒子数目，降低了粒子的多样性，导致粒子贫化，不能正确的表示出状态后验概率分布，导致算法估计精度下降。同时算法通过重采样使的粒子向高概率区域移动，有可能会使算法陷入局部最优值，从而影响算法的最优估计。
10.（2）计算效率问题：对于相对复杂的机器人系统，粒子滤波slam算法需要大量粒子才能获得较高的估计精度。
11.（3）外部环境的不确实干扰问题：一般地，在外部环境有干扰的情况下，传感器的噪声特性会突然发生变化，得到建议分布函数的后验分布也具有很大的差距，滤波性能受到干扰，会加剧粒子的退化，进而导致粒子的多样性的丢失。
12.针对fastslam存在由粒子权值退化与粒子多样性丧失导致机器人定位精度下降的问题，国内外研究学者提出了不同的改进思路，思路一为通过对fastslam算法重要性采样过程的优化，使重采样后的粒子结合集中分布在高似然区域，从而解决粒子权值退化与粒子多样性丧失的问题，提升机器人定位建图精度。如采用萤火虫算法，粒子群算法等实现对fastslam算法的优化。思路二为优化fastslam算法重采样过程，从而解决fastslam算法由于重采样过程导致的粒子多样性丧失问题，同时解决粒子权值退化，最终实现机器人定位建图精度的提升，如通过遗传算法替代fastslam算法重采样过程，通过遗传算法来兼顾粒子权值和粒子集的多样性，解决粒子权值退化与粒子多样性丧失的问题，实现对机器人定位建图精度的提升。


技术实现要素：

13.本发明的主要目的在于提供一种基于狮群算法优化的slam算法，解决fastslam2.0算法存在粒子权值退化与粒子多样性丧失的缺点。
14.本发明采用的技术方案是：一种基于狮群算法优化的slam算法，包括：改进狮王位置更新策略：将全局最优值做为狮王，并通过改进的狮王位置更新策略，实现粒子权值更新；改进母狮位置更新策略：通过改进的母狮狩猎法则，实现粒子集合的有效调整；改进幼狮更新策略：幼师向狮王位置移动，通过上述调整，实现粒子集合集中分布在高斯然区域，解决粒子权值退化与粒子多样性丧失的问题，实现对机器人定位建图精度的提升。
15.进一步地，所述改进狮王位置更新策略包括：首先构造以当前狮王（全局最优值）所在位置为中心，数目为的集合；表示粒子数目，代表移动步长，代表具有与全局最优值最远的粒子的距离；其次，判断移动步长与设定步长阀值对比，当，取移动步长为；反之将当前移动步长作为狮王的移动步长；通过此步骤实现步长的自动适应，在算法的迭代初期，各个粒子距离较大，通过计算获取的移动步长势必大于阀值，采用阀值作为当前的移动步长，保证狮王在较小范围内进行精确的更新，而当算法进入后期，粒子间距缩小，移动步长小于阀值，采用当前移动步长作为狮王移动步长，保障狮王更新；最后计算产生新的粒子集合中各个个体所对应的权值，取权值最大的个体作为当前新的狮王。
16.更进一步地，所述改进母狮位置更新策略包括：采用遗传算法中交叉思想，改进后的母狮位置更新公式如下：
ꢀꢀꢀ
上式中代表变异概率，取值为。
17.更进一步地，所述改进幼狮更新策略包括：幼狮主要采取两种位置更新策略，包括：向母狮运动，并与母狮协同狩猎；离开狮群；将上述两种策略应用到fastslam中。
18.本发明的优点：本发明针对fastslam2.0算法存在粒子权值退化与粒子多样性丧失的缺点，从而造成机器人定位下降的问题，通过狮群算法优化fastslam2.0算法，通过狮群算法的强寻优能力，优化重要性采样后的粒子分布，使粒子集中分布在高斯然区域，从而解决粒子权值退化与粒子多样性丧失问题，最终完成对fastslam2.0算法的改进，得到基于狮群算法优化的slam算法。
19.本发明的狮群算法优化slam算法，即通过狮群算法优化slam算法重要性采样后的粒子分布，其详细过程为，首先，将全局最优值做为狮王，并通过改进的狮王位置更新策略，实现粒子权值更新，然后，通过改进的母狮狩猎法则，实现粒子集合的有效调整，最后，幼师向狮王位置移动，通过上述调整，实现粒子集合集中分布在高斯然区域，解决粒子权值退化与粒子多样性丧失的问题，实现对机器人定位建图精度的提升。
20.除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。
附图说明
21.构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
22.图1是 fastslam算法结构图；图2是本发明的基于狮群算法优化的slam算法流程图；图3是本发明的仿真环境图；图4 是本发明的fastslam2.0仿真结果图；图5 是本发明的gfa
‑
fastslam2.0仿真结果图；图6 是本发明的lso
‑
fastslam2.0仿真结果图；图7是本发明的 机器人定位精度误差对比图。
具体实施方式
23.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
24.参考图2，狮群算法优化fastslam：狮群算法通过模拟狮群群体活动，将复杂函数问题的求解，通过狮王，母狮，幼狮等个体活动，进而求取问题最优解。通过上述分析，可以考虑将狮群算法应用到优化fastslam算法重要性采样后的粒子分布中，但如果直接引入狮群算法，必定会出现许多问题，因此需要在原有的狮群算法上进行改善，以便完成算法的高效结合。
25.将粒子权值作为狮群算法的个体评价准则：fastslam算法在重要性采样过程中，由于采用次优的概率密度函数，因此会造成粒子权值退化与算法滤波精度丧失的问题。本发明提出的改进思路的核心是通过狮群算法的较强的寻优能力改善重要性采样后的粒子分布，使粒子集合集中分布在高似然区域，因
此提出将重要性采样后的粒子权值作为狮群算法的个体评价准则，这也是将狮群算法与粒子滤波结合的前提。
26.改进狮王位置更新策略：在原始的狮王算法中，在根据适度值确定种群个体后，狮王在食物范围内维护自己的特权。如果直接将原始的位置更新公式引入改进算法中，势必会造成新产生的狮王位置所对应的适度值（权值）低于原始狮王位置所对应的适度值（权值），这将造成计算资源的浪费。本发明通过广义神经网路改进粒子滤波过程的启发，设定新的狮王位置更新策略：首先构造以当前狮王（全局最优值）所在位置为中心，数目为的集合。表示粒子数目，代表移动步长，代表具有与全局最优值最远的粒子的距离；其次，判断移动步长与设定步长阀值对比，当，取移动步长为；反之将当前移动步长作为狮王的移动步长。通过此步骤实现步长的自动适应，在算法的迭代初期，各个粒子距离较大，通过计算获取的移动步长势必大于阀值，采用阀值作为当前的移动步长，保证狮王（全剧最优值）在较小范围内进行精确的更新，而当算法进入后期，粒子间距缩小，移动步长小于阀值，采用当前移动步长作为狮王移动步长，保障狮王（全局最优值）更新。
27.最后计算产生新的粒子集合中各个个体所对应的权值，取权值最大的个体作为当前新的狮王。
28.通过新的狮王更新策略，保证在每次狮王觅食时，都会产生优于当前的狮王（全局最优值）的个体，而后使粒子集合整体移动到高似然区域，进而提升算法的滤波精度。
29.改进母狮位置更新策略：在原始狮群算法中，母狮狩猎，采取两只母狮协同狩猎的方式，狩猎后两只母狮处于同一位置。将其直接应用到粒子滤波改进中，在算法的运行后期，粒子之间距离较近，粒子多样性降低，应用此方法改进fastslam算法中，将再次降低粒子的多样性，本发明采用遗传算法中交叉思想，改进后的母狮位置更新公式如下：改进后的母狮位置更新公式如下：改进后的母狮位置更新公式如下： 上式中代表变异概率，本发明中取值为。
30.改进幼狮更新策略：在原始狮群算法中，幼狮主要采取三种位置更新策略，分别为1）向狮王（全局最优值）移动；2）向母狮运动，并与母狮协同狩猎；3）离开狮群。将2）、3）应用到fastslam进中，会导致粒子结合无法全部集中在高斯然区域，一定程度上产生粒子权值退化现象，势造成算
法定位精度下降，这与本发明改进思路使粒子集合整体向高似然区域移动相违背，因此省略2）、3）。
31.本发明的狮群算法优化slam算法，即通过狮群算法优化slam算法重要性采样后的粒子分布，其详细过程为，首先，将全局最优值做为狮王，并通过改进的狮王位置更新策略，实现粒子权值更新，然后，通过改进的母狮狩猎法则，实现粒子集合的有效调整，最后，幼师向狮王位置移动，通过上述调整，实现粒子集合集中分布在高斯然区域，解决粒子权值退化与粒子多样性丧失的问题，实现对机器人定位建图精度的提升。
32.实验验证：为了验证本发明所提出的算法的有效性，将经典的fastslam2.0算法，基于引力场算法改进的fastslam2.0算法（gfa
‑
fastslam2.0），与本发明所提出的算法（lso
‑
fastslam2.0）进行对比，通过狮群算法优化fastslam2.0算法，为了提升改进算法的时间效率，在狮群算法优化粒子滤波过程中，取消了狮王移动过程。
33.首先建立移动机器人仿真模型，模型如下：移动机器人运动模型： 其中，代表机器人时刻在二维平面环境中的位姿状态；代表航向角，其取值范围为，代表机器人的运动速度，代表其转向角度，为机器人里程计采样时间，为机器人运动过程中的噪声，为驱动轴之间的间距。
34.移动机器人观测模型： 其中，分别表示检测到的环境特征与移动机器人的距离、运动方向夹角；为观测噪声。
35.首先模拟机器人定位与建图，建立移动机器人工作环境，如图3所示，设置移动机器人运动范围100m80m，并设定17个航向点，35个路标点，移动机器人出坐标原点（图3中圆点）出发，逆时针运动，图中代表路标点，圆点代表航向点，折线代表机器人的规定路径。
36.为了验证改进算法的对机器人定位精度的提升，首先对上文中提到的三种算法在同样的机器人运动参数以及噪声参数进行对比，其中实验相关参数如下表1所示。
37.由图4~图6可知，本发明所提出的改进算法其预测轨迹与真实轨迹重合度最高，而基于引力场算法优化fastslam2.0算法（gfa
‑
fastslam2.0）次之，fastslam2.0算法重合度最低。这表明本发明提出的算法定位精度最高，产生此现象的原因是，fastslam2.0算法在算法运行后期，粒子退化严重，并且粒子多样性丧失。而gfa
‑
fastslam2.0算法滤波精度较高，因为其通过引力场作用粒子，使粒子分布在高似然区域，有效的缓解了粒子退化的问题，因此提升了机器人的滤波精度，对于本发明提出的算法，其算法定位精度最高，这是因为相对于引力场算法对粒子的优化，狮群算法对于粒子的优化作用更加高效，因此可以有效地解决粒子权值退化问题，提升机器人的滤波精度。
38.为了进一步验证改进算法对机器人定位与建图的优化效果，在8000个采样时刻，分别对比三种算法预测值与估计值的欧氏距离，即定位误差，其公式如下： 上式中，分别代表预测位置，与实际位置的坐标。
39.三种算法机器人定位精度误差对比如图7所示：由图7可知，本发明所提出的算法误差最小，并且较为稳定，而经典的fastslam2.0算法其定位精度误差随着算法运行时间的逐渐增加，其定位精度误差也逐渐增加，这是因为经典算法中在迭代后期，粒子退化严重，并且粒子的多样性丧失，因此导致机器人定位精度较低，而本发明提出的改进思路，有效的解决了此问题，进而增加了机器人的定位精度。对于gfa
‑
fastslam2.0算法，其定位精度低于本发明提出的改进算法，但是优于经典fastslam2.0算法，产生此现象的原因是引力场算法对重要性采样后的粒子具有一定程度的优化作用，并且通过引力场算法改进粒子滤波在一定程度上提升了粒子多样性。
40.为了验证验证改进算法对机器人定位精度的提升效果，对比三种算法的定位精度误差均值与方差，见下表2：
在上表中，本发明提出的lso
‑
fastslam2.0算法的定位精度误差均值最低，方差最小，这表明改进算法在提升fastslam2.0算法的定位精度的同时，算法的稳定性也得到了提升；而gfa
‑
fastslam2.0由于其算法对采样后的粒子进行调整的策略，使重要性采样后的粒子集中分布在全局最优值附近，由于算法特有的通过模拟引力场策略，即中心灰尘吸引、排斥周围灰尘运动，调整粒子分布，这是gfa
‑
fastslam2.0算法精度提升的原因，但同时这也一定程度上造成了算法的稳定性下降。
41.为了验证改进算法对机器人定位建图精度提升程度，分别对比x轴，y轴，路标的均方根误差，见表3：由表3可知，本发明提出的改进算法在x轴，y轴，以及路标估计均优于fastslam2.0以及gfa
‑
fastslam2.0算法，这正是因为改进算法对重要性采样后的粒子的高效优化方式，使得改进算法效果最好，并且这也表明，本发明提出的改进方式对于粒子的优化优于基于引力场算法改进fastslam2.0算法，产生此现象的主要原因是本发明提出的算法对粒子独特的优化方式，即通过幼狮在成长过程中向狮王移动，并且母狮之间合作捕猎的方式，实现了对重要性采样后的粒子的优化，因此本发明提出的算法效果最好。
42.以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。