基于和声搜索算法的改进粒子滤波定位与建图方法

1.本发明涉及定位和建图技术领域，具体涉及一种基于和声搜索算法的改进粒子滤波定位与建图方法。


背景技术：

2.移动机器人的同步定位与地图构建(simultaneous localization and mapping)是指机器人在未知的环境中运动，根据传感器的观测信息与里程计的位置信息进行位姿估计，再根据自身位置构建地图，以此达到同步定位和地图构建的目的。实时、高精度的slam算法是机器人实现自主定位、路径规划和自主导航的关键技术，其难点在于如何通过自身携带的传感器，采用滤波方法实现未知环境地图构建和精准定位。
3.由于激光雷达具有受光照条件影响小、测距范围精准的优势，因此是目前slam领域应用最广泛的技术。同时，基于粒子滤波的方法实时性能好且能够在线处理定位与建图。传统的粒子滤波算法通过一组粒子来表示后验概率，但是随着算法迭代次数的增加，会出现粒子退化问题，在粒子滤波重采样过程中，由于淘汰大量的低权重粒子，又会导致粒子贫化现象，降低粒子的多样性。


技术实现要素：

4.为了提升现有技术方案的性能和效果，本发明提出了一种基于和声搜索算法的改进粒子滤波定位与建图方法，利用和声搜索算法来优化采样后的粒子集，并加入小生境算法惩罚适应度低的粒子进行进一步优化，最后用基于偏差修正的ewa优化算法进行自适应优化组合重采样，缓解传统重采样带来的粒子贫化问题，多方面改进提高了定位与建图的效率和精度。包括初始化粒子和粒子权重；根据提议分布获取当前时刻的粒子集并计算粒子权重；利用和声搜索算法优化粒子位姿；利用小生境算法进一步优化；计算优化后的粒子权重并归一化；计算有效粒子数，基于偏差修正的ewa优化算法进行自适应优化组合重采样；基于粒子的运动轨迹，更新粒子的地图信息，建立实际地图。本发明所提供的粒子滤波方法，能够在使用更少粒子的同时获得更为准确的滤波估计；重采样改进策略在保证粒子多样性的同时解决了传统重采样引起的粒子贫化问题。多方面改进提高了定位与建图的效率和精度。
5.为实现上述目的，本发明具体采取如下技术方案：
6.一种基于和声搜索算法的改进粒子滤波定位与建图方法，其特征在于，包括以下步骤：
7.步骤s1：在t＝0时刻，随机选取n个粒子，并将其中第i个粒子的权重记为
8.步骤s2：将里程计信息作为提议分布，根据上一个时刻的粒子集和提议分布对当前时刻的粒子集进行估计，并按照重要性采样的原理，计算每个粒子的权重大小；
9.步骤s3：利用和声搜索算法优化采样粒子的位姿，粒子集中的一个粒子对应和声
搜索算法中的一个和声，基于适应度函数对和声进行调整优化，从而得到更加准确的粒子状态；
10.步骤s4：采用小生境算法进行寻优，利用小生境半径对半径内适应度低的粒子进行惩罚，进一步优化粒子；
11.步骤s5：计算优化后的粒子权重并进行归一化；
12.步骤s6：当有效粒子数n
eff
小于n/2时，基于偏差修正的ewa优化算法进行自适应优化组合重采样；
13.步骤s7：根据优化后的位姿，参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权重最高的粒子的地图作为当前估计的地图。
14.进一步地，步骤s3具体包括：
15.步骤s31：对和声搜索算法进行初始化，确定各类参数值，包括：和声记忆库的大小hms、和声记忆库取值概率的最大值hmcr
max
和最小值hmcr
min
、音调微调概率的最大值par
max
和最小值par
min
、音调微调带宽的最大值bw
max
和最小值bw
min
、创作的次数t
max
；
16.步骤s32：对于固定的和声记忆库取值概率hmcr进行调整，使其随着迭代次数ti进行动态变化，公式为：其中；
17.步骤s33：对于固定的音调微调概率par进行调整，使其随着迭代次数ti进行动态变化，公式为：
18.步骤s34：对音调微调带宽进行动态调整，根据迭代次数ti进行变化，以t
max
/2为阈值，bw变化遵循以下规则：
19.步骤s35：在新和声的生成中，取和声记忆库中最优和声的某一位置的和声变量替代新和声中同位置的和声变量，即其中，为和声记忆库中最优和声的第j个和声变量。
20.进一步地，步骤s4具体包括：
21.步骤s41：将最新观测值引入适应度函数，定义适应度公式为：
22.其中，rk为观测噪声的方差，zk为最新观测值，为预测的观测值；
23.步骤s42：计算粒子xi与xj之间的距离范数d
ij
：d
ij
＝||x
i-xj||,i,j＝1,2,
…
,n,i≠j；
24.步骤s43：如果d
ij
《σ，σ为小生境半径参数，计算粒子xi与xj的适应度f(xi)和f(xj)，对比f(xi)和f(xj)的大小，利用惩罚函数降低f(xi)和f(xj)中数值较小的粒子，即适应度较低的粒子的适应度：其中，为适应度较低的粒子，p(k)为惩罚函数；
25.步骤s44：利用高斯变异算子对进行随机扰动：其
中，μ为扰动控制因子，g(0,1)是均值为0，方差为1的高斯分布随机变量，为高斯扰动项；
26.步骤s45：重复步骤s42-步骤s44，直到达到预设的迭代次数或者粒子间的距离不小于小生境半径时，跳出小生境算法。
27.进一步地，步骤s6具体包括：
28.步骤s61：计算有效粒子数n
eff
来衡量粒子退化的程度，当有效粒子数n
eff
《n/2时，进行重采样；
29.步骤s62：对粒子权重按照升序进行排序；
30.步骤s63：利用基于偏差修正的ewa优化算法计算排序后的粒子集的平均值分布曲线，表达式为：其中，分别是前i、前i-1个粒子的平均权重，α为调超函数，是偏差修正项；
31.步骤s64：计算曲线的平均值以为基准，将所有粒子按照粒子权重分为高、中、低权重粒子，设定两个粒子权重划分阈值分别为ω
l
和ωh，其中，高、低权重粒子组成新的不稳定粒子集s：或
32.步骤s65：对稳定粒子不进行重采样操作，对不稳定粒子集s进行重采样，随机从粒子集s中选择两个粒子，重采样公式为：x
new
＝xa+kl(x
b-xa)，其中，x
new
为新产生的粒子，xa为随机被选择的粒子中权重较大的粒子，xb为随机被选择的粒子中权重较小的粒子，k为步长系数，l为(x
b-xa)方向上的步长；l的具体表达公式为其中，ns为粒子集s中粒子的数目，β为任意一个采样粒子邻域空间内采样粒子的分布概率，m为采样空间的维数；
33.步骤s66：新生成的粒子数与稳定粒子的总和达到粒子总数n时，停止重采样操作。
34.进一步地，步骤s1具体包括：当t＝0时，根据机器人运动模型先验概率p(x0)选取n个粒子，记为采样后的粒子平均分配权重为：
35.进一步地，步骤s2具体包括：基于上一个时刻的粒子集和提议分布对当前时刻的粒子集进行估计，提议分布一般采用里程计信息，所采样的粒子服从重要性密度函数：其中，x
t
为粒子在t时刻的采样位姿，为t-1时刻粒子的采样位姿，u
t
为t时刻的里程计信息；之后通过公式：对每个粒子的权重进行计算，其中，为t-1时刻第i个粒子的权重，1时刻第i个粒子的权重，分别是系统观测、状态模型和先验概率分布函数。
36.进一步地，在步骤s3当中：
37.把当前时刻的粒子集中的每个粒子当做一个和声，对和声中的每个音调进行选
取、调整，产生具有更精确位姿的粒子，具体为：
38.步骤a31：对和声搜索算法进行初始化，确定各类参数值，包括：和声记忆库的大小hms、和声记忆库取值概率的最大值hmcr
max
和最小值hmcr
min
、音调微调概率的最大值par
max
和最小值par
min
、音调微调带宽的最大值bw
max
和最小值bw
min
、创作的次数t
max
；
39.步骤a32：对固定的和声记忆库取值概率hmcr进行调整，使其随着迭代次数ti进行动态变化，公式为：
[0040][0041]
步骤a33：对固定的音调微调概率par进行调整，使其随着迭代次数ti进行动态变化，公式为：
[0042][0043]
步骤a34：对音调微调带宽进行动态调整，根据迭代次数t进行变化，以t
max
/2为阈值，bw变化遵循以下规则：
[0044][0045]
步骤a35：最新观测值引入适应度函数，定义和声搜索算法的适应度公式为步骤a35：最新观测值引入适应度函数，定义和声搜索算法的适应度公式为其中，rk为观测噪声的方差，zk为最新观测值，为预测的观测值；
[0046]
步骤a36：初始化和声记忆库，每一个粒子代表一个和声，粒子所携带的位姿参数为各个和声变量；随机生成hms个和声x1，x2，
…
，x
hms
放入和声记忆库；记忆库形式为：
[0047][0048]
步骤a37：生成新的和声对于新和声的每一个音调在[0，1]之间产生一个随机数rand1，若rand1≤hmcr，从和声记忆库中随机拿出一个和声变量作为新音调，否则，从解空间随机生成一个和声变量作为新音调，规则为：
[0049]
其中x为解空间；
[0050]
步骤a38：若和声变量是从和声记忆库中得到的，那么需要对其进行微调；在[0，1]之间产生一个随机数rand2，若rand2≤par，则根据微调带宽bw对得到的和声变量进行调整，否则，不做任何调整，规则为：
[0051][0052]
步骤a39：新和声生成后，取和声记忆库中最优和声的某一位置的和声变量替代新
和声中同位置的和声变量，即其中，为和声记忆库中最优和声的第j个和声变量；
[0053]
步骤a310：更新和声记忆库：对新和声xnew进行评估，即计算其适应度，若适应度优于和声记忆库中的最差值，则将x
new
代替记忆库中函数值最差的和声x
worst
，否则，不作修改，规则为：ifthenxworst＝xnew：
[0054]
步骤a311：重复步骤a37-步骤a310，直到迭代次数达到tmax为止。
[0055]
进一步地，步骤s5具体包括：对于步骤s3、步骤s4优化后的粒子集进行权重更新，通过公式：对每个粒子的权重进行计算，其中，为t-1时刻第i个粒子的权重，分别是系统观测、状态模型和先验概率分布函数，并进行归一化：
[0056]
进一步地，步骤s7具体包括：对t时刻，根据优化后的粒子信息，得到机器人的位姿状态结合传感器的观测信息对地图进行更新，其中，权重最大的粒子所携带的地图为当前时刻最佳的估计地图；对于t时间内的地图构建，根据粒子集中粒子的信息结合每个时刻的观测值计算环境地图数据即即进行地图的更新。
[0057]
与现有技术相比，本发明及其优选方案利用和声搜索算法来优化采样后的粒子集，并加入小生境算法惩罚适应度低的粒子进行进一步优化，最后用基于偏差修正的ewa优化算法进行自适应优化组合重采样，缓解传统重采样带来的粒子贫化问题，多方面改进提高了定位与建图的效率和精度。
附图说明
[0058]
图1为本发明方法的主要步骤流程示意图；
[0059]
图2为本发明和声搜索算法优化采样粒子位姿过程示意图。
具体实施方式
[0060]
为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：
[0061]
应该指出，以下详细说明都是例示性的，旨在对本技术提供进一步的说明。除非另有指明，本说明书使用的所有技术和科学术语具有与本技术所属技术领域的普通技术人员通常理解的相同含义。
[0062]
本实施例将和声搜索优化算法与粒子滤波结合，设计了一种和声搜索优化的粒子滤波算法，其主要思想是将和声搜索算法用于粒子滤波在粒子选取阶段的优化，对只利用
里程计运动模型作为提议分布而生成的粒子进行改进，产生满足一定性能指标要求的粒子，从而弥补普通粒子滤波算法在粒子生成阶段的不可靠性、缩短粒子滤波算法的收敛时间、提高估计算法的估计精度。并利用基于偏差修正的ewa优化算法的自适应优化组合重采样方法对传统重采样方法进行改进，缓解了传统重采样过程中导致的粒子耗尽问题。并将改进算法应用到机器人slam中，核心是提高算法的熟练能力与估计精度，优化机器人对运行轨迹与地图构建的精度与稳定性。
[0063]
参照图1和图2，本发明实施例提供了基于和声搜索算法的改进粒子滤波定位与建图方法，包括以下步骤：
[0064]
步骤s1：在t＝0时刻，随机选取n个粒子，并将其权重记为
[0065]
步骤s2：将里程计信息作为提议分布，根据上一个时刻的粒子集和提议分布对当前时刻的粒子集进行估计，并按照重要性采样的原理，计算每个粒子的权重大小；
[0066]
步骤s3：利用和声搜索算法优化采样粒子的位姿，粒子集中的一个粒子对应和声搜索算法中的一个和声，基于适应度函数对和声进行调整优化，从而得到更加准确的粒子状态；
[0067]
步骤s4：小生境算法进行寻优，利用小生境半径对半径内适应度低的粒子进行惩罚，进一步优化粒子；
[0068]
步骤s5：计算优化后的粒子权重并进行归一化；
[0069]
步骤s6：当有效粒子数n
eff
小于n/2时，基于偏差修正的ewa优化算法进行自适应优化组合重采样；
[0070]
步骤s7：根据优化后的位姿，参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权重最高的粒子的地图作为当前估计的地图。
[0071]
步骤s1具体包括：
[0072]
当t＝0时，根据机器人运动模型先验概率p(x0)选取n个粒子，记为采样后的粒子平均分配权重为：
[0073]
步骤s2具体包括：
[0074]
基于上一个时刻的粒子集和提议分布对当前时刻的粒子集进行估计，提议分布一般采用里程计信息，所采样的粒子服从重要性密度函数：其中，x
t
为粒子在t时刻的采样位姿，为t-1时刻粒子的采样位姿，u
t
为t时刻的里程计信息；之后通过公式：对每个粒子的权重进行计算，其中，为t-1时刻第i个粒子的权重，分别是系统观测、状态模型和先验概率分布函数。
[0075]
步骤s3具体包括：
[0076]
把当前时刻的粒子集中的每个粒子当做一个和声，对和声中的每个音调进行选
取、调整，产生更优美的和声，即产生具有更精确位姿的粒子；
[0077]
步骤s31：对和声搜索算法进行初始化，确定各类参数值：和声记忆库的大小hms、和声记忆库取值概率的最大值hmcr
max
和最小值hmcr
min
、音调微调概率的最大值par
max
和最小值par
min
、音调微调带宽的最大值bw
max
和最小值bw
min
、创作的次数t
mmax
；
[0078]
步骤s32：考虑在寻优的初始阶段采用较大的取值概率，有利于找到局部最优解，在后期则采用较小的取值概率，有利于增加解的多样性。故对固定的和声记忆库取值概率hmcr进行调整，使其随着迭代次数t进行动态变化，公式为：
[0079]
其中，t是当前迭代次数；
[0080]
步骤s33：在初始阶段，一个较小的微调概率有利于寻找局部最优值，而在后期，一个较大的微调概率能够使解的多样性得到增加。故对固定的音调微调概率par进行调整，使其随着迭代次数t进行动态变化，公式为：
[0081]
其中，t是当前迭代次数；
[0082]
步骤s34：在搜索初期采用较大的微调带宽有利于算法在较大范围内探测，在搜索后期，较小的微调带宽有利于进行小范围内的精细搜索。故对音调微调带宽进行动态调整，根据迭代次数t进行变化，以t
max
/2为阈值，bw变化遵循以下规则：
[0083][0084]
步骤s35：设定优化的评判标准(即适应度函数)。将最新观测值引入适应度函数，定义和声搜索算法的适应度公式为其中，rk为观测噪声的方差，zk为最新观测值，为预测的观测值；
[0085]
步骤s36：初始化和声记忆库，每一个粒子代表一个和声，粒子所携带的位姿参数为各个和声变量。随机生成hms个和声x1，x2，
…
，x
hms
放入和声记忆库。记忆库形式为：
[0086][0087]
步骤s37：生成新的和声对于新和声的每一个音调在[0，1]之间产生一个随机数rand1，若rand1≤hmcr，从和声记忆库中随机拿出一个和声变量作为新音调，否则，从解空间随机生成一个和声变量作为新音调，规则为：
[0088]
其中x为解空间；
[0089]
步骤s38：若和声变量是从和声记忆库中得到的，那么需要对其进行微调。在[0，1]
之间产生一个随机数rand2，若rand2≤par，则根据微调带宽bw对得到的和声变量进行调整，否则，不做任何调整，规则为：
[0090][0091]
步骤s39：新和声生成后，取和声记忆库中最优和声的某一位置的和声变量替代新和声中同位置的和声变量，即其中，为和声记忆库中最优和声的第j个和声变量；
[0092]
步骤s310：更新和声记忆库。对新和声x
new
进行评估，即计算其适应度，若适应度优于和声记忆库中的最差值，则将x
new
代替记忆库中函数值最差的和声x
worst
，否则，不作修改，规则为：ifthenx
worst
＝x
new
：
[0093]
步骤s311：重复步骤s37、s38、s39、s310，直到迭代次数达到tmax为止。
[0094]
步骤s4具体包括：
[0095]
利用和声搜索算法优化后的粒子位姿，继续判断各粒子之间的距离，若小于小生境半径，则利用惩罚函数进一步降低适应度差的粒子的适应度值，最后采用高斯变异算子对粒子适应度进行随机扰动，使适应度差的粒子远离适应度好的粒子；
[0096]
步骤s41：将最新观测值引入适应度函数，定义适应度公式为：
[0097]
其中，rk为观测噪声的方差，zk为最新观测值，为预测的观测值；
[0098]
步骤s42：计算粒子xi与xj之间的距离范数d
ij
：d
ij
＝||x
i-xj||，i，j＝1，2，
…
，n，i≠j；
[0099]
步骤s43：如果d
ij
＜σ，σ为小生境半径参数，计算粒子xi与xj的适应度f(xi)和f(xj)，对比f(xi)和f(xj)的大小，利用惩罚函数降低f(xi)和f(xj)中数值较小的粒子，即适应度较低的粒子的适应度：其中，为适应度较低的粒子，p(k)为惩罚函数；
[0100]
步骤s44：利用高斯变异算子对进行随机扰动：其中，μ为扰动控制因子，g(0，1)是均值为0，方差为1的高斯分布随机变量，为高斯扰动项；
[0101]
步骤s45：重复步骤s42、s43、s44，直到达到预设的迭代次数或者粒子间的距离不小于小生境半径时，跳出小生境算法。
[0102]
步骤s5包括：
[0103]
对于s3、s4步骤优化后的粒子集进行权重更新，通过公式：对每个粒子的权重进行计算，其中，为t-1时刻第i个粒子的权重，1时刻第i个粒子的权重，分别是系统观测、状态模型和先验概率分布函数，并进行归一
化：
[0104]
步骤s6包括：
[0105]
步骤s61：计算有效粒子数neff来衡量粒子退化的程度，当有效粒子数n
eff
＜n/2时，进行重采样；
[0106]
步骤s62：对粒子权重按照升序进行排序；
[0107]
步骤s63：利用基于偏差修正的ewa优化算法计算排序后的粒子集的平均值分布曲线，表达式为：其中，是前i、i-1个粒子的平均权重，α为调超函数，是第i个粒子的权重，1-αi是偏差修正项；
[0108]
步骤s64：计算曲线的平均值以为基准，将所有粒子按照粒子权重分为高、中、低权重粒子，设定粒子权重划分阈值为ω
l
和ωh，其中，高、低权重粒子组成新的不稳定粒子集s：或
[0109]
步骤s65：对稳定粒子不进行重采样操作，对不稳定粒子集s进行重采样，随机从粒子集s中选择两个粒子，重采样公式为：x
new
＝xa+kl(x
b-xa)，其中，x
new
为新产生的粒子，xa为随机选择的粒子中权重较大的粒子，xb为随机被选择的粒子中权重较小的粒子，k为步长系数，l为(x
b-xa)方向上的步长；l的具体表达公式为其中，ns为粒子集s中粒子的数目，β为任意一个采样粒子邻域空间内采样粒子的分布概率，m为采样空间的维数；
[0110]
步骤s66：新生成的粒子数与稳定粒子的总和达到粒子总数n时，停止重采样操作。
[0111]
步骤s7包括：
[0112]
对t时刻，根据优化后的粒子信息，得到机器人的位姿状态结合传感器的观测信息对地图进行更新，其中，权重最大的粒子所携带的地图为当前时刻最佳的估计地图；对于t时间内的地图构建，根据粒子集中粒子的信息结合每个时刻的观测值计算环境地图数据即进行地图的更新。
[0113]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。
[0114]
本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的基于和声搜索算法的改进粒子滤波定位与建图方法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。