[发明专利]基于自适应遗传算法的粒子滤波定位与地图构建方法

说明书

本发明公开了基于自适应遗传算法的粒子滤波定位与地图构建方法，包括初始化、判断机器人当前位置是否到达终止位置、状态估计、特征匹配、状态更新、路径估计、自适应遗传算法优化k时刻机器人路径分布。本发明用自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。

技术领域

本发明涉及一种基于粒子滤波的同时定位与地图构建方法，尤其是涉及基于自适应遗传算法的粒子滤波定位与地图构建方法。

背景技术

伴随着机器人技术的蓬勃发展，机器人对未知环境的自主认知能力已经成为机器人学中研究的一个热点。构建未知环境的地图，也就是移动机器人的定位与导航问题是机器人自主认知的其中一项重点研究内容和研究热点。其中，同时定位与地图构建(SLAM)则是移动机器人实现定位和导航的一种有效手段，就是机器人根据获取到的机器人相关数据，提取特征，经过特征匹配，最终自主构建机器人所处未知环境的地图并同时获取自身位置，对自身进行定位。进而，可以利用构建好的环境地图，继续进行其他的相关研究。

目前现有的常用于同时定位与地图构建的方法是用粒子滤波器估计机器人的运动路径，即用粒子集近似表示机器人路径的后验概率分布，粒子集的粒子权值方差会随时间是递增变化的。经过多次的迭代之后，少数的样本将会占去总体样本的绝大部分权重，除了这几个粒子外的其他粒子就具有很小的权值，这样一来样本就不能有效表达真是的后验概率密度。由此产生粒子滤波器中普遍存在的“粒子退化”问题，粒子的多样性受到影响。对路径的估计精度会下降。重采样是减弱“粒子退化”现象的一种常用方法，但是，重采样会引起“粒子耗尽”的问题。重采样的过程也会使整个SLAM方法的效率变低。

发明内容

本发明的目的在于基于自适应遗传算法的粒子滤波定位与地图构建方法，本发明用自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。

本发明解决上述技术问题采用的技术方案是：基于自适应遗传算法的粒子滤波定位与地图构建方法，包括以下步骤：

步骤一：初始化：设置机器人运动的初始位置和终止位置；

步骤二：判断：机器人从初始位置向终止位置移动过程中，判断机器人当前位置是否到达终止位置，若到达终止位置，则机器人停止移动，移动过程中的位置判断包括机器人在初始位置和终止位置时进行判断；

步骤三：状态估计：若未到达终止位置，则采用无迹卡尔曼滤波算法计算k-1时刻各个粒子的位姿建议分布；

步骤三：特征匹配：获取k时刻机器人对环境特征的实际观测值，并通过数据关联方法将实际观测值与观测估计值进行特征匹配；

步骤四：状态更新：用无迹卡尔曼滤波算法计算k时刻各个粒子的位姿建议分布；

步骤五：路径估计：从粒子的k时刻位姿建议分布中进行采样得到粒子在k时刻的位姿向量，计算每个粒子的非正则权值，得到k时刻机器人路径分布；

步骤六：采用自适应遗传算法优化k时刻机器人路径分布，返回步骤二。

利用自适应遗传算法，使粒子集能够根据自身状态，自动调整变异率和交叉率，保护粒子的多样性，得到更好的优化粒子集，提高了SLAM问题中路径估计的精度，同时自适应遗传算法来优化表示路径后验分布的粒子集，去掉了一般粒子滤波SLAM方法中的重采样，解决了重采样带来的“粒子耗尽问题”，并使整个SLAM系统的运行效率得到提升。

作为优选，步骤六中还包括自适应遗传算法优化方法，自适应遗传算法优化方法包括以下步骤：

步骤A：对k时刻粒子集的各个粒子的适应度值和粒子的相似度值进行计算；