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

说明书

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

技术领域

本发明涉及机器人即时定位与地图构建，具体涉及一种融合最小二乘法的粒子滤波SLAM方法。

背景技术

我国为推进实施“科技兴安”战略，调动社会力量参与安全生产科技攻关，进一步强化防范遏制重特大事故科技保障能力,在重点行业领域开展“机械化换人、自动化减人”科技强安专项行动,重点是以机械化生产替换人工作业、以自动化控制减少人为操作，大力提高企业安全生产科技保障能力。

在机器人的定位与地图构建中，通常情况下使用粒子滤波的SLAM算法可以得到一个较好的估计结果，但由于采样过程对里程计数据的严重依赖，导致当机器人里程计数据误差较大时，基于里程计的提议分布与目标分布严重不符，后续进行的扫描匹配，权值计算等步骤毫无意义。且由于粒子滤波系统本身的复杂度较高，当机器人移动速度过快，t-1至t时刻的时间段内机器人位置已经有了较大变化，而此时算法正在执行t-1时刻的估计过程，这种现象会使未知环境下的扫描匹配过程精度下降。目前广泛应用研究的SLAM系统大多专注于轮式移动机器人，且要求机器人自身携带定位传感器，如编码器，陀螺仪等，并由此计算得到机器人运动里程。但此类算法严重依赖里程计的精度，当地面有坑洼或轮子发生漂移时，里程计等传感器误差较大，极限情况下机器人实际位置并没有发生改变，但里程计数据已经显示机器人移动一段距离，这种情况不仅会使SLAM算法无法得到机器人的准确位置，创建的栅格地图也会发生撕裂、扭曲等现象。

发明内容

本发明目的在于提供一种融合最小二乘法的粒子滤波SLAM方法，该方法不需要过度依赖里程计数据作为估计初值，本申请解决的技术问题为：机器人在地图中的位置数据采样严重依赖里程计数据。

本发明通过下述技术方案实现：

一种融合最小二乘法的粒子滤波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，则线程Ⅰ的一个更新时刻内线程Ⅱ会进行多次更新。