基于粒子自收敛的移动机器人重定位方法与流程

本发明属于机器人定位技术领域，提供了一种基于粒子自收敛的移动机器人重定位方法。

背景技术：

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置，移动机器人定位导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的导航路径单一，不利于扩展和更改，使用的局限性较大。基于视觉的定位导航方法受光源条件影响大，稳定性和精度较差，无法让移动机器人进行稳定、精密的操作。基于激光雷达的定位导航方法可以分为基于反光板的定位导航方法和基于轮廓的定位导航方法，其中基于反光板的定位导航方法需要在环境中布设大量的路标，应用场景受限制较多。基于轮廓的定位导航方法由于路径灵活、不需要布置人工标志，越来越广泛的应用在移动机器人定位导航中。而现有基于轮廓的定位导航方法，在移动机器人出现打滑漂移、人为移动、重启或关机等情况时，会导致机器人定位失败，需要进行重定位，因此移动机器人的重定位问题在基于轮廓的定位导航应用中至关重要。

现有的重定位问题解决方案主要包含如下三种：1)让机器人回到初始位置，定位失败后，人为的将机器人推到初始位置重新开机；2)对比机器人定位导航的地图和真实环境，人工标记机器人位姿；然而这些方案在实际应用过程中都存在一定的问题：上述方案都需要人工干预，无法保证重定位的效率和准确性。

技术实现要素：

本发明实施例提供一种基于粒子自收敛的移动机器人重定位方法，在移动机器人在重新开机、打滑或其他原因导致位姿丢失的状态下，自动快速且准确的恢复机器人的位姿。

本发明是这样实现的，一种基于粒子自收敛的移动机器人重定位方法，所述方法具体包括如下步骤：

s1、基于栅格地图生成格网粒子集合set_a，格网粒子集合set_a由格网粒子组成，格网粒子即为各格网点pi(xi,yi)的最优位姿；

s2、更新格网粒子集合set_a中的粒子及对应的打分；

s3、对格网粒子集合set_a中的粒子进行重采样，将重采样的粒子复制至新的粒子集合set_b，粒子集合set_b中的粒子权重相等，

s4、计算粒子集合set_b中所有粒子的位姿标准差，检测是否所有粒子的位姿标准差都小于预设的阈值，若检测结果位是，将粒子集合中set_b所有粒子的平均位姿作为机器人的当前重定位的位姿，若检测结果为否，则执行步骤s2。

进一步的，格网粒子集合set_a的生成方法具体包括如下步骤：

s11、在栅格地图上每隔设定距离δd取一个格网点pi(xi,yi)；

s12、检测取格网点所在的栅格是否被障碍物占据；

s13、若检测结果为否，则获取格网点pi(xi,yi)的最优位姿，若检测结果为是，则基于设定距离δd在栅格地图上取下一个格网点，并执行步骤s12；

s14、遍历栅格地图上的所有格网点，输出格网粒子集合set_a。

进一步的，格网点pi(xi,yi)最优位姿的获取方法具体如下：

从0度到360度，每个设定的角度步进δθ取一个角度θj，将角度θj与格网点pi(xi,yi)组成一个位姿pi,j(xi,yi,θj)；

利用似然域模型对位姿pi,j(xi,yi,θj)进行打分，遍历格网点pi(xi,yi)所有的角度，最高打分位姿即为格网点pi(xi,yi)的最优位姿。

进一步的，格网粒子集合set_a中的粒子更新方法具体包括如下步骤：

s21、格网粒子集合set_a中顺序选择一个粒子pi(xi,yi,θi)，并在粒子pi(xi,yi,θi)周围生成k个高斯随机位姿；

s22、利用似然域模型计算粒子pi(xi,yi,θi)及k个随机位姿集合的得分，得到最高得分wmax和对应的位姿pmax，令当前位姿等于得分最高的位姿pi＝pmax，当前位姿的权重等于最高得分，即wi＝wmax；

s23、遍历粒子格网粒子集合set_a中所有的粒子，完成格网粒子集合set_a中的更新与打分。

进一步的，粒子重采样过程具体如下：

s31、对格网粒子集合中的粒子权重进行降序排列；

s32、基于粒子复制比率计算待复制粒子的数量copy_count，设定粒子复制比率为r，当前格网粒子集合中的粒子数set_count，计算得到待复制粒子的数copy_count＝r*set_count；

s33、构建空的粒子集合set_b，将排名靠前的copy_count个粒子复制到粒子集合set_b，并将粒子集合set_b中的所有粒子权重设为1/copy_count；

s34、将粒子集合set_b替代格网粒子集合set_a。

本发明提供的基于粒子自收敛的移动机器人重定位方法具有如下有益效果：在机器人丢失位姿后，无需人工干预下，能快速、准确的恢复机器人的位姿，即重定位。

附图说明

图1为本发明实施例提供的基于粒子自收敛的移动机器人重定位方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1为本发明实施例提供的基于粒子自收敛的移动机器人重定位方法流程图，该方法具体包括如下步骤：

s1、基于栅格地图生成格网粒子集合set_a，格网粒子集合set_a由格网粒子组成，格网粒子即为各格网点pi(xi,yi)的最优位姿；

在生成格网粒子集合之前，需要构建好占据栅格地图，格网粒子集合set_a的生成方法具体包括如下步骤：

s11、在栅格地图上每隔设定距离δd取一个格网点pi(xi,yi)；

s12、检测取格网点所在的栅格是否被障碍物占据；

s13、若检测结果为否，则获取格网点pi(xi,yi)的最优位姿，若检测结果为是，则基于设定距离δd在栅格地图上取下一个格网点，并执行步骤s12，遍历栅格地图上的所有格网点，输出格网粒子集合set_a；

在本发明实施例中，格网点pi(xi,yi)最优位姿的获取方法具体如下：

从0度到360度，每个设定的角度步进δθ取一个角度θj，将角度θj与格网点pi(xi,yi)组成一个位姿pi,j(xi,yi,θj)；

利用似然域模型对位姿pi,j(xi,yi,θj)进行打分，遍历格网点pi(xi,yi)所有的角度，最高打分位姿即为格网点pi(xi,yi)的最优位姿。

s2、更新格网粒子集合set_a中的粒子及对应的打分；

在本发明实施例中，格网粒子集合set_a中的粒子更新方法具体包括如下步骤：

s21、格网粒子集合set_a中顺序选择一个粒子pi(xi,yi,θi)，并在粒子pi(xi,yi,θi)周围生成k个高斯随机位姿；

s22、利用似然域模型计算粒子pi(xi,yi,θi)及k个随机位姿集合的得分，得到最高得分wmax和对应的位姿pmax，令当前位姿等于得分最高的位姿pi＝pmax，当前位姿的权重等于最高得分，即wi＝wmax；

s23、遍历粒子格网粒子集合set_a中所有的粒子，完成格网粒子集合set_a中的更新与打分。

s3、对格网粒子集合set_a中的粒子进行重采样，将重采样的粒子复制至新的粒子集合set_b，粒子集合set_b中的粒子权重相等，粒子重采样过程具体如下：

s31、对格网粒子集合中的粒子权重进行降序排列；

s32、基于粒子复制比率计算待复制粒子的数量copy_count，设定粒子复制比率为r，当前格网粒子集合中的粒子数set_count，计算得到待复制粒子的数copy_count＝r*set_count；

s33、构建空的粒子集合set_b，将排名靠前的copy_count个粒子复制到粒子集合set_b，并将粒子集合set_b中的所有粒子权重设为1/copy_count；

s34、将粒子集合set_b替代格网粒子集合set_a。

s4、计算粒子集合set_b中所有粒子的位姿标准差，检测是否所有粒子的位姿标准差都小于预设的阈值，若检测结果位是，将粒子集合set_b中所有粒子的平均位姿作为机器人的当前重定位的位姿，若检测结果为否，则执行步骤s2。

计算新粒子集合中所有粒子位姿的标准差，计算公式如下：

其中n为粒子集合set_b中的粒子数目，分别为粒子集合set_b中所有粒子位姿的三个分量的均值。若粒子集合set_b中所有粒子位姿的标准差满足设定的条件，即：其中ξ(x)、ξ(y)、ξ(θ)为提前设定的阈值，则输出当前的平均位姿作为移动机器人重定位的位姿，否则，返回步骤s2继续计算。

在本发明实施例中，似然域模型计算步骤如下：

从激光雷达扫描帧中顺序选择一个扫描点zt(θt,dt)，(θt,dt)为扫描点的角度及距离，将扫描点映射到全局坐标系(即栅格地图系)：

其中(x,y,θ)为粒子当前的位姿，(xt,yt)为扫描点的全局坐标，即扫描到在栅格地图中的坐标。

计算(xt,yt)与距离最近的障碍物的距离dist，计算粒子当前位姿上的得分p：

其中zhit、zranddom及zmax分别代表测距误差混合权重的不同部分，分别代表测量噪声、无法解释的随机测量及测量失败，σhit为测量噪声的标准差，遍历激光雷达扫描帧上所有的扫描点，累加得到当前位姿的得分p。

本发明提供的基于粒子自收敛的移动机器人重定位方法具有如下有益效果：在机器人丢失位姿后，无需人工干预下，能快速、准确的恢复机器人的位姿，即重定位。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。