CN104180799A

CN104180799A - 一种基于自适应蒙特卡罗定位的机器人定位方法

Info

Publication number: CN104180799A
Application number: CN201410336894.1A
Authority: CN
Inventors: 张磊; 张华希; 方正; 徐泉; 刘腾飞; 于合强
Original assignee: Northeastern University China
Current assignee: Northeastern University China
Priority date: 2014-07-15
Filing date: 2014-07-15
Publication date: 2014-12-03

Abstract

本发明提供一种基于自适应蒙特卡罗定位的机器人定位方法，包括：采用预抓取技术将机器人所在环境的地图信息进行预抓取；在待定位的机器人运动过程中，计算相似位能区；根据三维栅格和相似位能区，采用自适应样本的蒙特卡罗定位方法对机器人进行定位。本发明方法不仅具备常规蒙特卡罗定位方法的所有优点，而且能够在保证样本的多样性、计算的高效性的同时，解决位置跟踪、全局定位和机器人“绑架”三个定位问题。计算所有传感器位能和值的好处就是不用考虑机器人的朝向，这样就减少了一个维度，使计算量大大减小。

Description

一种基于自适应蒙特卡罗定位的机器人定位方法

技术领域

本发明涉及机器人定位技术领域，具体涉及一种基于自适应蒙特卡罗定位的机器人定位方法。

背景技术

机器人在感知—执行的过程中，总是存在着很大的不确定性。机器人本身的观测必然是不完整的、充满噪声的，因此有效的定位成为实现机器人能够自主移动探索环境的先决条件。目前机器人定位问题主要有：位置跟踪、全局定位和机器人“绑架”问题。目前大部分研究集中于解决其中一个问题，很少有人能够同时解决这三个问题，特别是“绑架”问题(定位领域最难的问题之一)。为了解决这些问题，越来越多的学者将概率理论应用到机器人的定位导航中，因为概率理论对于解决充满不确定性的问题具有先天优势。目前比较有代表性是基于贝叶斯滤波和马尔可夫模型的定位方法，例如：扩展卡尔曼滤波定位方法、网格定位方法和蒙特卡罗定位方法等。

贝叶斯滤波定位方法的基本思想是将机器人定位问题转化为状态空间中机器人的状态估计问题，其实质是试图用所有已知信息来构造系统状态变量的后验概率密度，即用系统模型预测状态的先验概率密度，再使用最近的测量值进行修正，得到后验概率密度。扩展卡尔曼滤波定位方法的主要缺点是只能够维持一个信度，因此它只能用来解决位置跟踪问题。网格定位方法可以维持多个信度，但是它在计算量和存储量上需求巨大。蒙特卡罗定位方法与网格定位方法相比，在计算量和存储量上大大减少。常规蒙特卡罗定位方法基于粒子滤波，用一系列加权的样本来代表后验信度。其核心思想是对样本进行评价，当样本与当前观测值位置越接近时，样本的权值越高，下次采样时它被采样几率就越大。但是常规蒙特卡罗定位方法的问题在于：(1)权重较大的样本存在多次采样问题，破坏了样本的多样性；(2)采样的数据过多会占据较大的计算资源，采样的数据过少时又很难保证对信度的有效描述；(3)不能解决机器人被“绑架”问题。

发明内容

针对现有技术存在的问题，本发明提供一种基于自适应蒙特卡罗定位的机器人定位方法。

本发明的技术方案是：

一种基于自适应蒙特卡罗定位的机器人定位方法，包括以下步骤：

步骤1：采用预抓取技术将机器人所在环境的地图信息进行预抓取；

步骤1.1：对获取的机器人所在环境的地图信息进行栅格化处理；

步骤1.2：对机器人所在环境的地图分解成多个三维栅格；

步骤1.3：预计算机器人的各传感器在每个三维栅格获得的位能值，确定每一个三维栅格的位能栅格形式，即预计算机器人的各传感器在每个三维栅格获得的位能和值为预计算的机器人的第i个传感器在其所在三维栅格中获得的位能值，i＝1，...，I；

步骤2：在待定位的机器人运动过程中，计算相似位能区；

步骤2.1：在待定位的机器人运动过程中，机器人的各传感器实时获得机器人在各三维栅格的位能值，并实时计算机器人的各传感器在其所在的三维栅格中获取的位能和值e_i为实时计算的机器人的第i个传感器在其所在三维栅格中获得的位能值；

步骤2.2：分别对实时计算的机器人的各传感器在其所在的三维栅格中获取的位能和值和预计算的机器人的各传感器在每个三维栅格获得的位能和值进行规范化处理：

\overset{&OverBar;}{E} = \frac{1}{I} E

E^{''} = \frac{1}{I} E^{'}

步骤2.3：对规范化处理后的实时计算的机器人的各传感器在其所在的三维栅格中获取的位能和值和预计算的机器人的各传感器在每个三维栅格获得的位能和值进行能量匹配：计算两位能和值的差值的绝对值，若该绝对值小于设定的能量临界值，则对应的三维栅格判定为属于相似位能区；

步骤3：根据三维栅格和相似位能区，采用自适应样本的蒙特卡罗定位方法对机器人进行定位；

步骤3.1：以三维栅格作为最小单位，采用重要性采样方法获得加权的随机采样的样本集合，该集合中的每一个样本均由机器人的姿态及权重构成，该集合中的样本的个数为M_Z；

步骤3.2：根据样本的权重判断机器人是否被“绑架”：如果权重的最大值低于给定的临界值，则机器人被“绑架”，此时，将加权的随机采样的样本集合中的一部分样本转化为全局样本，则局部样本数为M_J＝α·M_Z，参数α为决定全局样本和局部样本的比例的系数，相应的全局样本数为M_Q＝M_Z-M_J；如果权重的最大值不低于给定的临界值，则机器人未被“绑架”，加权的随机采样的样本集合中的所有样本全部为局部样本；

步骤3.3：根据权重重新采样局部样本，得到局部样本集；

步骤3.4：若当前机器人处于被“绑架”状态，在相似能量区中重新随机采样全局样本，得到全局样本集，若当前机器人处于未被“绑架”状态，直接执行步骤3.5；

步骤3.5：对局部样本集和全局样本集取并集，得到最终的机器人姿态集合，即机器人定位结果。

所述传感器为距离传感器或视觉传感器。

有益效果：

本发明方法不仅具备常规蒙特卡罗定位方法的所有优点，而且能够在保证样本的多样性、计算的高效性的同时，解决位置跟踪、全局定位和机器人“绑架”三个定位问题。为了减轻线上计算的负担，本方法利用了预抓取技术，使定位方法运行的更快，效率更高，也使得这个方法能够被实时在线上得到应用。计算所有传感器位能和值的好处就是不用考虑机器人的朝向，这样就减少了一个维度，使计算量大大减小。另外，本方法所涉及的传感器不仅仅局限于距离传感器，视觉传感器等其它类型传感器亦可适用。本发明可以实现机器人的预先定位，相似位能区只可能在机器人出现概率很大的区域形成，这些区域就包含机器人的精确位置，找到这些区域使得搜索范围进一步减小。当使用本发明的定位方法进行精确定位的时候，样本只需要投放到相似位能区中，这样不仅使用相对较少的样本就可以实现定位(相对较少的样本意味着减少了计算量)，而且使得样本的使用效率更高，定位更加快速。传统的蒙特卡罗定位方法，一旦机器人被定位，所有的样本都集中到机器人周围，无法再形成全局样本，所以传统的蒙特卡罗定位方法不适用于机器人被多次“绑架”的情况。

附图说明

图1是本发明具体实施方式的基于自适应蒙特卡罗定位的机器人定位方法原理示意图；

图2是本发明具体实施方式的相似位能区示意图，(a)为机器人位于走廊时形成的相似位能区，(b)为机器人位于角落时形成的相似位能区；

图3是本发明具体实施方式的基于自适应蒙特卡罗定位的机器人定位方法流程图。

具体实施方式

下面结合附图对本发明的具体实施方式做详细说明。

蒙特卡罗定位是基于粒子滤波，通过传感器获得的信息来递归的位姿状态空间的概率分布，其概率分布是以加权的形式来描述的。原理是用含有M个加权的样本概率的集合X_t来表示分布函数，因此，哪个区域分布的样本越集中，机器人就越可能位于哪个区域。其分布函数表示如：其中，样本粒子表示在t时刻机器人的姿态，代表权重是一个非负数，而且每个样本的概率值与他所对应的权值成比例。

本实施方式的基于自适应蒙特卡罗定位的机器人定位方法，原理如图1所示，流程如图3所示，包括以下步骤：

步骤1.2：对机器人所在环境的地图分解成多个三维栅格S_3D，每一个三维栅格均包含该栅格所在平面坐标和机器人的朝向；本实施方式中，三维栅格的形式为m×n×θ，其中m和n分别为相对于平面坐标原点的横向距离和纵向距离，θ为机器人的朝向；

预抓取阶段，假设得到的每一个三维栅格位置都有一个机器人，机器人的传感器会返回对环境的测量信息。对于本实施方式来说，机器人自身携带的是距离传感器，机器人的距离传感器返回的数据就是距离d，则定义此距离传感器在当前三维栅格获得的位能值：

e_{i}^{'} = 1 - \frac{d_{i}}{d_{m}}

式中，第i(i∈(1,2,...,I))个传感器获得的距离值为d_i，d_m代表各距离传感器所能获得的距离的最大值，定义一个机器人的位能或者一个三维栅格的位能为此机器人所装载的所有距离传感器的位能和值，即：

E^{'} = Σ_{i = 1}^{I} e_{i}^{'}

计算所有距离传感器的位能和值的好处就是不用考虑机器人的朝向，这样就减少了一个维度，使计算量大大减小。另外，本实施方式不仅仅局限于距离传感器，视觉传感器等其它类型传感器亦可适用。

步骤2：在待定位的机器人运动过程中，计算相似位能区，该相似位能区位于机器人周围，代表机器人可能所在位置。

相似位能区为一系列可能具有和机器人位置相似位能的三维栅格。通过相似位能区，可以实现机器人的预先定位，相似位能区只可能在机器人出现概率很大的区域形成，这些区域就包含机器人的精确位置，找到这些区域使得搜索范围进一步减小。当使用本方法进行精确定位的时候，样本只需要投放到相似位能区中，这样不仅使用相对较少的样本就可以实现定位(相对较少的样本意味着减少了计算量)，而且使得样本的使用效率更高，定位更加快速。传统的蒙特卡罗定位方法，一旦机器人被定位，所有的样本都集中到机器人周围，无法再形成全局样本，所以传统的蒙特卡罗定位方法不适用于机器人被多次“绑架”的情况。图2展示机器人位于走廊和角落时形成的相似位能区，其中，(a)为机器人位于走廊时形成的相似位能区，(b)为机器人位于角落时形成的相似位能区，显然图(b)中在相似位能区中取样的高效性更加明显，在这种情况下只需将有效样本投放在四个角落中就能很快的定位机器人的位置。

步骤2.1.1：机器人自身带有I个距离传感器，其中距离传感器i(i∈{1,...,I})获取的距离值为d_i，计算距离传感器i获取的位能值：e_i＝1-d_i/d_m，其中d_m为距离传感器i所能探测的最大距离；

步骤2.1.2：重复步骤2.1.1直至计算出机器人所带的I个距离传感器的位能值集合{e₁，e₂，e₃，...，e_I}；

步骤2.1.3：计算I个距离传感器获取的位能和值：

步骤2.2：分别对机器人所带的I个距离传感器获取的位能和值和预计算得到的机器人的各距离传感器在每个三维栅格获得的位能和值进行规范化处理：

对机器人所带的I个距离传感器获取的位能和值进行规范化处理：

对预计算得到的机器人的各距离传感器在每个三维栅格获得的位能和值进行规范化处理：

\overset{&OverBar;}{E} = \frac{1}{I} E;

设置能量临界值为ψ，针对每一个三维栅格计算如果则当前三维栅格属于相似位能区；直至对所有三维栅格对应的规范化处理后的两个位能和值进行能量匹配后，所有属于相似位能区的三维栅格形成相似位能区；

自适应蒙特卡罗定位方法采用自适应样本的方法来解决轨迹追踪、全局定位和机器人被“绑架”问题，自适应样本方法可以自动的将其分为局部样本集和全局样本集并根据不同情况在两种样本集之间变换。

当机器人被准确定位时本方法仅仅在机器人周围获取局部样本，一旦机器人被“绑架”一部分局部样本转化为全局样本，在机器人被重新定位之后，全局样本被转化为局部样本的一部分。全局样本被用来解决机器人在“绑架”时恢复定位，但是也有可能产生错误，例如，在对称的环境中时与机器人对称的区域同样具有很高的可能性，还有机器人的姿态也将是不确定的。因此，全局样本仅仅在机器人被判定确实被“绑架”时产生。本发明通过检测样本粒子的可能性(权重)判定机器人是否被“绑架”，当样本粒子可能性的最大值低于给定的临界值时机器人机会判定自己被“绑架”。

基于样本上一个时刻机器人的姿态和控制输入u_t，计算当前时刻的样本的机器人的姿态判定样本的权重，特别的是，样本的数据取自三维栅格S_3D。

计算当前时刻的样本的机器人的姿态：

当前时刻的样本的机器人的姿态相应的权值：

其中，x_t为t时刻机器人自身认定的姿态(存在误差)，z_t为t时刻机器人的姿态测量值；当前时刻的样本的机器人的姿态代表由概率经过采样得到对应的姿态(含有修正)，计算出对应的权值。将n的取值从1到M_Z，样本化所有三维栅格，所述的机器人的姿态包括位置坐标和朝向；

步骤3.2：根据样本的权重ω_t判断机器人是否被“绑架”：如果权重的最大值低于给定的临界值λ，则机器人被“绑架”，此时，将加权的随机采样的样本集合中的一部分样本转化为全局样本，则局部样本数为M_J＝α·M_Z，参数α决定全局样本和局部样本的比例的系数，相应的全局样本数为M_Q＝M_Z-M_J；如果权重的最大值不低于给定的临界值，即则加权的随机采样的样本集合中的所有样本全部为局部样本，即M_J＝M_Z；

没有将所有样本作为全局样本的原因是机器人可能会错误的认为自己被“绑架”，保留一部分局部样本可降低这种失误。给定的临界值λ是一种敏感系数，决定着对认定“绑架”的灵敏性，λ取值越大，机器人对自己被“绑架”越敏感，然而机器人也可能频繁出错。

步骤3.3：根据权重重新采样局部样本，得到局部样本集；

重新采样局部样本的操作和常规的蒙特卡罗是相同的，首选规范化权重ω_t，然后根据权重分布样本。将n取值从1到M_J，根据权值采集局部样本并将加入到局部样本集

全局样本在相似位能区中均匀分布，只有当机器人判定自己被绑架时才会在相似能量区中重新随机采样全局样本。将n取值从1到M_Q，在相似位能区中采集均匀分布的全局样本并将加入到全局样本集

步骤3.5：对局部样本集和全局样本集取并集，得到最终的机器人的姿态集合X_t，即机器人定位结果。

机器人定位结果包括机器人经过的所有点的坐标和朝向，根据这些点可以拟合出机器人运动轨迹曲线。

Claims

1.一种基于自适应蒙特卡罗定位的机器人定位方法，其特征在于：包括以下步骤：

步骤1.2：对机器人所在环境的地图分解成多个三维栅格；

步骤1.3：预计算机器人的各传感器在每个三维栅格获得的位能值，确定每一个三维栅格的位能栅格形式，即预计算机器人的各传感器在每个三维栅格获得的位能和值e′_i为预计算的机器人的第i个传感器在其所在三维栅格中获得的位能值，i＝1，…，I；

步骤2：在待定位的机器人运动过程中，计算相似位能区；

\overset{&OverBar;}{E} = \frac{1}{I} E

E^{''} = \frac{1}{I} E^{'}

步骤3.2：根据样本的权重判断机器人是否被“绑架”：如果权重的最大值低于给定的临界值，则机器人被“绑架”，此时，将加权的随机采样的样本集合中的一部分样本转化为全局样本，则局部样本数为M_J＝α·M_Z，参数α为决定全局样本和局部样本的比例的系数，相应的全局样本数为M_Q＝M_Z-M_J；如果权重的最大值不低于给定的临界值，则加权的随机采样的样本集合中的所有样本全部为局部样本；

步骤3.3：根据权重重新采样局部样本，得到局部样本集；

2.根据权利要求1所述的基于自适应蒙特卡罗定位的机器人定位方法，其特征在于：所述传感器为距离传感器或视觉传感器。