[发明专利]一种基于改进的蒙特卡洛算法的自主学习全局定位方法

说明书

本发明公开了一种基于改进的蒙特卡洛算法的自主学习全局定位方法，其包括有获取地图，创建粒子滤波器；获得最优初始位姿；更新粒子群至收敛或超时；判断是否有激光输入；滤波器更新；计算出最优位姿的得分并添加到粒子群中；对粒子群重采样和归一化；获取到当前时刻最优的位姿和方差；判断是否正在进行导航任务，如果是，开启自主学习进程；判断当前环境变化和地图差异是否超过阈值；判断当前环境变化和地图差异是否超过阈值。

技术领域

本发明涉及一种室内移动机器人定位方法，尤其涉及一种基于改进的蒙特卡洛算法的自主学习全局定位方法。

背景技术

随着移动机器人的快速发展，其三大核心技术(建图、定位和导航)的研究也日渐深入。其中机器人定位作为自主导航中的一项核心技术，是机器人具有行走能力的关件。机器人定位可分为全局定位和局部定位，常用的局部定位有里程计推算、惯导定位等。中国专利“CN108931245移动机器人的局部自定位方法及设备”利用里程计和加速度信息来检测机器人运动状态，有效避免轮子大话而导致机器人丢失位置；全局定位是指机器人在已知地图下的位姿(坐标+偏航角)，常见的方法有GPS定位、视觉特征匹配、反光板三角定位和激光扫描匹配等。中国专利“CN110082776一种基于2D激光数据的机器人实时定位方法”公开了一种激光匹配实时定位算法。

在很多应用场景中，尤其是工业和仓储物流等领域，移动机器人都需要实时精确地估算出自身的位置。目前，室内移动机器人的全局定位，主要使用蒙特卡算法，它是基于2D栅格地图的概率定位算法，使用粒子滤波器来估计机器人的位置。传统的蒙特卡洛定位算法由F.Dellaert,D.Fox等人于1999年提出，在不加任何人工路标的情况下，实现了0.05米-0.2米的精度。但是这种算法定位精度低，很多情况下，大于5厘米的精度均无法满足工业需求；收敛速度慢，在初始化位姿后，需要更新一端距离才能收敛，且收敛的过程中可能会出现位置的漂移，导致机器人出现急转的现象；对环境敏感，鲁棒性差，当环境和地图差异较大时，粒子会慢慢发散，直到丢失自身的位置。

发明内容

有鉴于此，本发明提供一种基于改进的蒙特卡洛算法的自主学习全局定位方法，其包括以下步骤：

步骤一：获取地图，创建粒子滤波器。

步骤二：读取当前激光和已知地图，并进行扫描匹配，在地图上获得最优初始位姿。

步骤三：利用步骤二中的位姿初始化粒子群，更新粒子群至收敛或超时。

步骤四：判断是否有激光输入，如果没有，则等待；若有，判断机器人是否移动，当里程计大于阈值时或角度大于阈值时，利用公式里程采样运动模型算法进行滤波器预测，在机器人运动学约束下的可行区域内高斯采样。

步骤五：滤波器更新。

步骤六：计算出最优位姿的得分并添加到粒子群中。

步骤七：对粒子群重采样和归一化，为了实现粒子数的自适应，使用KLD算法，计算出粒子群的总数。

步骤八：经过上述步骤，粒子滤波器完成了预测和更新，此时获取到当前时刻最优的位姿和方差。

步骤九：判断是否正在进行导航任务，如果是，开启自主学习进程。

步骤十：判断当前环境变化和地图差异是否超过阈值，当超过阈值时，阻塞自主学习线程，进入第一阶段，过滤掉不匹配的激光点，用剩下的激光点执行上述步骤一至七；当超过阈值时，阻塞自主学习线程，进入第二阶段，在数据库中查找与上一时刻机器人位姿最近邻的一个位姿，获取对应的激光帧。

步骤十一：判断当前环境变化和地图差异是否超过阈值，若没有超过，则通知自主学习线程继续运行，读取激光的数据执行步骤一至七。

进一步地，步骤五中滤波器的更新有以下两个步骤：