CN110986956B - 一种基于改进的蒙特卡洛算法的自主学习全局定位方法 - Google Patents

Abstract

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

Description

一种基于改进的蒙特卡洛算法的自主学习全局定位方法

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

步骤五：滤波器更新。

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

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

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

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

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

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

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

a.读取当前时刻的激光数据，将激光数据转换到机器人坐标系下；接着，将转换后的坐标投影到地图上，并计算出最近邻占据点的距离；进一步，计算激光粒子击中占据格子的概率。

b.为了保证定位的实时性，采用并行计算，开辟一个扫描匹配的线程，利用上一时刻最优位姿对应的激光数据和当前的地图进行扫描匹配，获取当前时刻最优的位姿。

本发明提供的一种基于蒙特卡洛算法的自主学习全局定位方法具有以下优势：

1、在环境良好的情况下，加入扫描匹配后提高了整体粒子群的精度，由原来的5cm-20cm，提高至1cm-5cm。

2、利用扫描匹配，添加了自动给初始位姿的功能，取代了人工通过上位机给位姿，简化了操作。

3、加入了粒子群的预收敛，规避了机器人在起步行走时不必要的调整。

利用自主学习，在环境变化和地图差异较大的情况下，也能继续导航，提高了定位的鲁棒性。

附图说明

图1所示为本发明的一种实施例中基于改进的蒙特卡洛算法的自主学习全局定位方法的简略图；

图2所示为图1左侧部分的示意图；

图3所示为图1右侧部分的示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整的说明。

需要说明的是，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例，以下对至少一个示例性实施例的描述实际上仅仅是说明性的，而不应当作为本申请及其应用的限制。

在本发明中，术语“第一”、“第二”仅用于描述的目的，而不能理解为指示或者暗示相对重要性；属于“多个”则指两个或两个以上，除非另有明确的限定。术语“安装”、“相连”、“连接”、“固定”等术语均应作广义理解，例如，“连接”可以是固定连接，也可以是可拆卸连接，或一体地连接；“相连”可以是直接连接，也可以是通过中间媒介间接相连。术语“上”、“下”等指示的方位或者位置关系为基于附图所示的方式或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或者暗示所指的装置或单元必须具有特定的方向、以特定的方位构造和操作。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

为解决背景技术中的问题，本发明提供一种改进蒙特卡洛算法的自主学习全局定位方法。其中，所述移动机器人包括轮式编码器和激光雷达。进一步地，本发明还提供一种激光扫描匹配算法，获取最优位姿并送入粒子群中更新来提高定位精度。还提供一种自主学习策略，在机器人导航时，记录下最优的位姿和对应的激光帧，当机器人再次来到同样的环境下，如果发现环境变化较大时，首先过滤掉动态环境，只用和原来环境一致的激光点来更新粒子群，若发现环境变化达到90％以上，则使用数据库中的和当前位置最近的激光帧来替换当前的激光数据，让粒子群收敛，保证机器人不丢位置。

如图1至图3所示，本发明提供的一种改进蒙特卡洛算法的自主学习全局定位方法的步骤包括：

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

步骤二：读取当前激光和已知地图，利用公式(1)进行扫描匹配，在地图上获得最优初始位姿。其中p是机器人的位姿坐标，L_i(p)是在机器人p位姿下，对应第i个激光点的坐标，W(L_i(p))表示这些激光点落在栅格地图中占据格子(Occupied)里的概率。

步骤三：利用步骤二中的位姿初始化粒子群，更新粒子群至收敛或超时。此时已经完成自动给初始位姿。

步骤四：判断是否有激光输入，如果没有，则等待；若有，判断机器人是否移动，当里程计大于阈值时或角度大于阈值时，利用公式(2-10)里程采样运动模型算法进行滤波器预测，在机器人运动学约束下的可行区域内高斯采样。其中α₁α₂α₃α₄是里程模型误差参数。[x′,y′,θ′]为预测样本的位姿。

步骤五：滤波器更新。a.读取当前时刻的激光数据，用公式(11)将激光数据转换到机器人坐标系下。其中θ是机器人的偏航角，l_i是指在机器人位姿p下，第i个激光点的原始数据(即激光雷达坐标系下的坐标)，s_x和s_y分别表示在机器人坐标系下，激光雷达位置的坐标。

接着，将转换后的坐标投影到地图上，并通过似然模型算法中公式(12)计算出最近邻占据点的距离d。其中，

和/>

分别表示激光投影到地图上的坐标，〈x′,y′〉表示占据格子的坐标，m表示地图。

进一步，利用公式(13)计算激光粒子击中占据格子的概率。其中，prob(d,σ_hit)是零均值下，σ_hit为标准差的高斯函数。z_hit、z_random和z_max分别为测量误差的权重参数。

用公式(11-13)，遍历所有的激光点，为每个粒子计算出一个得分。

b.为了保证定位的实时性，采用并行计算，开辟一个扫描匹配的线程，利用上一时刻最优位姿对应的激光数据和当前的地图进行扫描匹配，获取当前时刻最优的位姿。

步骤六：利用公式(14)计算步骤4b中的最优位姿的得分并添加到粒子群中。这样保证了在粒子群中，最优的位姿有较高的权重。其中，s_max为当前粒子群中最高的得分，τ表示扫描匹配的误差。

s＝s_max·e^-τ·τ (14)

步骤七：对粒子群重采样和归一化，为了实现粒子数的自适应，使用KLD算法，该算法用公式(15)根据当前粒子得分的分布每次计算出粒子群的总数N。其中，k为粒子状态空间中非空的数量。α为真是后验分布和估计分布之间的最大误差，β为以概率(1-σ)来确定的样本数。

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

步骤九：判断是否正在进行导航任务，如果是，开启自主学习进程。在导航过程中，对粒子得分和匹配度大于阈值的位姿和对应的激光帧保存至数据库中，可以设定一些规则来维护数据库，如：(1)设定每隔一段距离记录数据对(指位姿+激光帧)，(2)当粒子得分和扫描匹配结果超过阈值时保存，(3)实时刷新数据库，发现有更优的数据对时取代原来的。(4)设定数据库数据对的上限。

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

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

本发明提供的一种基于蒙特卡洛算法的自主学习全局定位方法具有以下优势：

4、在环境良好的情况下，加入扫描匹配后提高了整体粒子群的精度，由原来的5cm-20cm，提高至1cm-5cm。

5、利用扫描匹配，添加了自动给初始位姿的功能，取代了人工通过上位机给位姿，简化了操作。

6、加入了粒子群的预收敛，规避了机器人在起步行走时不必要的调整。

利用自主学习，在环境变化和地图差异较大的情况下，也能继续导航，提高了定位的鲁棒性。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (1)

1.一种基于改进的蒙特卡洛算法的自主学习全局定位方法，其包括以下步骤：

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

步骤二：读取当前激光和已知地图，利用公式(1)进行扫描匹配，在地图上获得最优初始位姿；其中，p是机器人的位姿坐标，L_i(p)是在机器人p位姿下，对应第i个激光点的坐标，W(L_i(p))表示这些激光点落在栅格地图中占据格子(Occupied)里的概率：

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

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

其中α₁、α₂、α₃、α₄是里程模型误差参数，[x′,y′,θ′]为预测样本的位姿；

步骤五：滤波器更新；包括以下步骤：

a.读取当前时刻的激光数据，用公式11将激光数据转换到机器人坐标系下；

其中，θ是机器人的偏航角，l_i是指在机器人位姿p下，第i个激光点的原始数据，s_x和s_y分别表示在机器人坐标系下，激光雷达位置的坐标；

接着，将转换后的坐标投影到地图上，并通过似然模型算法中公式12计算出最近邻占据点的距离d；

其中，

和/>

分别表示激光投影到地图上的坐标，x′,y′表示占据格子的坐标，m表示地图；

利用公式13计算激光粒子击中占据格子的概率；

其中，prob(d,σ_hit)是零均值下，σ_hit为标准差的高斯函数，z_hit、z_random和z_max分别为测量误差的权重参数；

用公式11-公式13，遍历所有的激光点，为每个粒子计算出一个得分；

b.为了保证定位的实时性，采用并行计算，开辟一个扫描匹配的线程，利用上一时刻最优位姿对应的激光数据和当前的地图进行扫描匹配，获取当前时刻最优的位姿；

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

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

所述KLD算法用公式15根据当前粒子得分的分布每次计算出粒子群的总数N；

其中，k为粒子状态空间中非空的数量，α为真是后验分布和估计分布之间的最大误差，β为以概率(1-σ)来确定的样本数；

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

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

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

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

Images