[发明专利]一种融合最小二乘法的粒子滤波SLAM方法

摘要

本发明公开了一种融合最小二乘法的粒子滤波SLAM方法，涉及机器人即时定位与地图构建，解决了机器人在地图中的位置数据采样严重依赖里程计数据的问题。本发明包括线程Ⅰ和线程Ⅱ双线程执行以获取机器人在地图中的位置，由于线程Ⅱ所使用的粒子滤波算法耗时长，故在t‑1～t时间间隔内线程Ⅱ已经进行多次更新。线程Ⅱ内的最小二乘匹配算法的目的就是计算t‑1～t时间内的机器人相对移动位置，同时本申请使用了局部地图的方式，当线程Ⅰ内粒子滤波系统更新完成后局部地图重置。其核心在于每次迭代过程中，使用最小二乘匹配计算得到的机器人位置对粒子集进行扩充，当一次更新间隔内里程计误差较大时可以有效将部分粒子限制在真实分布附近。

主权项

1.一种融合最小二乘法的粒子滤波SLAM方法，其特征在于，包括线程Ⅰ和线程Ⅱ双线程执行以获取机器人在地图中的位置，所述线程Ⅰ为采用粒子滤波获取机器人在地图中的位置，线程Ⅱ为采用最小二乘匹配获取机器人在地图中的位置，具体步骤包括：步骤1，t＝0时，线程Ⅰ和线程Ⅱ同时获取雷达数据得到机器人在地图中的初始位置，此后，机器人开始移动；步骤2，当机器人移动距离超过设定阈值时，进入t＝1时刻，线程Ⅰ中粒子滤波算法使用里程计运动模型对粒子进行采样得到的样本代替地图内机器人位置，并根据权值对粒子进行排序；步骤3，在t＝0～t‑1时间段内，线程Ⅱ同步获取雷达数据得到机器人位置x′₁，则可得到粒子集内权值最大的N个粒子，使用最小二乘法对x′₁采样得到的样本代替t＝0～t‑1时间段内机器人位置即对局部地图重置，得到扩充后的粒子集，粒子集内粒子的总数变为M+N；步骤4，线程Ⅱ对粒子集进行扫描匹配、权值计算、重采样完成更新，其中权值最大的粒子代表的机器人在地图中的位置，即代表机器人此时的位置与环境地图数据；步骤5，线程Ⅱ对粒子集进行排序，舍弃权值较小的N个粒子，粒子总数变为M，由于线程Ⅰ在t＝1时刻地图内机器人位置更新已经完成，线程Ⅱ内对局部地图重置，待下一个t＝2时刻到来时继续执行步骤2～5，则线程Ⅰ的一个更新时刻内线程Ⅱ会进行多次更新。