CN101183266A - 使用粒子滤波器估算移动机器人姿态的方法、设备和介质 - Google Patents

Abstract

公开了一种使用粒子滤波器估算移动机器人的方法、设备和介质。根据本发明实施例的使用粒子滤波器估算移动机器人的姿态的方法包括：检测移动机器人的姿态改变，并通过将检测到的姿态改变应用于先前的粒子来计算当前粒子的姿态；基于由传感器获得的范围数据和地图信息来预测所述当前粒子的姿态的概率并获取所述当前粒子的权重；基于权重对当前粒子进行重新采样；以及考虑传感器的误差调整权重。

Description

使用粒子滤波器估算移动机器人姿态的方法、设备和介质

本申请要求于2006年11月16日提交到韩国知识产权局的第10-2006-0113440号韩国专利申请的优先权，该申请的公开完全合并于此，以资参考。

技术领域

各实施例涉及一种使用粒子滤波器(particle filter)估算移动机器人的姿态的方法、设备和介质，更具体地说，涉及一种使用粒子滤波器估算移动机器人的姿态的方法、设备和介质，所述方法能够通过使用粒子滤波器以基于网格(grid)的同步定位和地图创建(SLAM)算法反映传感器的误差并调整权重来精确地估算移动机器人的姿态。

背景技术

近年来，除了工业用途的机器人之外，家用的机器人和用于办公室业务的机器人已经投入实际使用。例如，有清洁机器人、向导机器人和保安机器人。

机器人在预定的区域中移动的同时在预定的环境中自行。为了使每个机器人在预定的环境中移动并自行工作，机器人需要创建该机器人的周围空间的地图以及关于机器人的位置和朝向角的信息。可将基于网格的同步定位和地图创建(SLAM)算法用作创建地图和识别机器人位置的方法。

根据SLAM算法，创建预定位置的周围环境的地图，并基于创建的地图确定机器人曾路过的位置。重复这些过程从而估算机器人的位置和周围的地图。

在SLAM算法中，粒子滤波器技术可用于估算机器人自身的位置。粒子滤波器技术是一种用于通过提取多个预测的机器人的位置和朝向角的样本并使用每一样本具有机器人的实际位置和朝向角的概率来估算机器人的最佳姿态的方法。在本发明中术语“姿态”指示在移动机器人的平面上的二维坐标位置和移动机器人的朝向角。

在使用范围数据的基于网格的SLAM中，可不用提取特征点而通过使用所有范围数据来创建地图。然而，在现有技术中，需要使用具有高精确度的范围寻找器(range finder，以下称为“传感器”)来获取范围数据。因此，传感器的精确度对于获取高质量地图非常重要。

对于投入实际使用的家用的机器人或商用机器人(诸如清洁机器人和保安机器人)，即使传感器低廉并且具有相对较低的精确度，仍需要创建地图并精确地估算机器人的位置。

发明内容

实施例通过调整经地图一致性检查的使用粒子滤波器获得的粒子的权重来增加估算移动机器人的姿态的精确度。

实施例通过执行根据移动机器人的朝向方向的爬坡来有效地估算移动机器人的姿态。

根据本发明的一方面，提供一种使用粒子滤波器估算移动机器人的姿态的方法，所述方法包括：基于移动机器人的初始位置对当前粒子进行采样，检测移动机器人的姿态的改变并通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态，基于由传感器获得的范围数据和地图信息预测当前粒子的姿态的概率并获取当前粒子的权重，基于权重对当前粒子重新采样，以及考虑传感器的误差调整权重。

根据本发明的另一方面，提供一种使用粒子滤波器估算移动机器人的姿态的设备，所述设备包括：里程表，检测移动机器人的姿态改变；粒子姿态计算单元，通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态；传感器，获取范围数据；粒子滤波器单元，基于范围数据和地图信息预测当前粒子的姿态的概率并获取当前粒子的权重；以及权重调整单元，考虑传感器的误差调整权重。

根据本发明的另一方面，提供一种使用粒子滤波器估算移动机器人的姿态的方法，所述方法包括：检测移动机器人的姿态改变，并通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态；基于由传感器获得的范围数据和地图信息预测当前粒子的姿态的概率并获取当前粒子的权重；以及考虑传感器的误差调整权重以便估算移动机器人的姿态。

根据另一方面，提供至少一种存储用于实施实施例的方法的计算机可读指令的计算机可读存储介质。

附图说明

通过下面参照附图对示例性实施例的详细描述，本发明的这些和/或其他方面、特点和优点将会变得清楚和更容易理解，其中：

图1是示出根据示例性实施例的使用粒子滤波器估算移动机器人的姿态的方法的流程图；

图2A是示出根据现有技术的爬坡方法的坐标轴的示图，其中，基于绝对坐标系步长相同；

图2B是示出修改的爬坡方法的坐标轴的示图，其中，基于根据机器人的朝向角的相对坐标轴步长不同；

图3A和图3B是示出根据图2A和图2B中示出的方法对其执行爬坡的粒子的示图；

图4A是示出当里程表与传感器的精确度的标准差之比为30∶1时根据里程表和传感器的机器人的姿态的概率值的示图；

图4B是示出当里程表与传感器的精确度的标准差之比为10∶1时根据里程表和传感器的机器人的姿态的概率值的示图；

图5A是示出当里程表与传感器的精确度的标准差之比为30∶1时根据精确等式(等式3)的姿态的概率值、根据近似等式(等式4)的姿态的概率值以及所述精确等式和所述近似等式之间的误差比r(x)的示图；

图5B是示出当里程表与传感器的精确度的标准差之比为10∶1时根据精确等式(等式3)的姿态的概率值、根据近似等式(等式4)的姿态的概率值以及所述精确等式和所述近似等式之间的误差比r(x)的示图；

图6是示出根据示例性实施例的按照传感器的误差特性指示重新调整权重的函数P(x)的曲线图；以及

图7是根据示例性实施例的使用粒子滤波器估算移动机器人的姿态的设备的框图。

具体实施方式

现在将详细描述示例性实施例，其示例在附图中示出，其中，相同的标号始终表示相同的组件。以下，通过参照附图来描述示例性实施例。

图1是示出根据示例性实施例的使用粒子滤波器来估算移动机器人的姿态的方法的流程图。

根据一示例性实施例的使用粒子滤波器估算移动机器人的姿态的方法包括：检测移动机器人的姿态改变并通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态(S100)，通过爬坡技术更新粒子的姿态(S110)，基于传感器620获得的范围数据和地图信息预测所述姿态的概率并计算当前粒子的权重(S120)，基于所述权重对当前粒子进行重新采样(S130)，以及考虑传感器620的误差调整权重(S140)。

为了实施根据本发明实施例的使用粒子滤波器估算移动机器人的姿态的方法，将描述当移动机器人在行进的同时创建地图时的产生过程。

首先，给定移动机器人出发的位置和机器人的朝向角。或者，可将移动机器人出发的位置和移动机器人出发的朝向角设置为参考值。由于术语“姿态”指示移动机器人的二维平面上的位置和移动机器人的朝向角，因此，因此估算移动机器人的姿态表示估算移动机器人的位置和朝向角。

处于给定的出发点的移动机器人可移动预定的距离。可由安装在机器人上并能够检测机器人的移动量和机器人的旋转角度的里程表600来检测已经移动的机器人的姿态改变。例如，里程表600可包括编码器和陀螺仪。例如，可通过编码器对机器人已经移动的距离和方向进行积分，从而估算当前移动机器人的姿态。当不存在积分误差时，可仅通过编码器来估算机器人的姿态。编码器在短时间内提供相对较高的精确度，但是当编码器对数字连续执行积分时遇到问题时，造成误差的累加。此外，由于移动机器人在移动的同时可能滑行或遇到障碍，因此基于编码器获得的机器人的位置与实际位置之间可能出现差异。

因此，为了更精确地估算机器人的姿态，应用了使用粒子滤波器的同步定位和地图创建(SLAM)算法。

在初始阶段，采样单元640随机采样N个粒子。采样单元640选择移动机器人的初始位置周围的N个随机点。也就是说，根据如下的等式1选择N个粒子：

【等式1】

$S=\left\{(x_0^{\left(j\right)},w_0^{\left(j\right)})\right|j=1,...,N\}$

这里，x₀ ^(j)和w₀ ^(j)分别指示关于与预定的指数j相应的粒子的移动机器人的初始位置和初始权重。与每个指数相应的w₀ ^(j)可被设为与指数j无关的1/N。权重指示当前粒子的位置成为移动机器人的位置的概率。

当机器人移动预定的距离时，用去预定的时间。因此，可随时间更新每个粒子的姿态。

粒子姿态计算单元610通过将移动机器人中设置的上述里程表600检测到的姿态改变应用于粒子来计算每个粒子的姿态(S100)。

此时，为了增加估算粒子位置的概率的精确度，粒子姿态计算单元610可通过应用爬坡方法来更新每个粒子的姿态(S110)。

图2A是示出根据现有技术的爬坡方法的坐标轴的示图，其中，基于绝对坐标系步长相同。图2B是示出修改的爬坡方法的坐标轴，其中，基于根据机器人的朝向角的相对坐标系步长不相同。图3是示出对其执行根据图2A和图2B中示出的方法的粒子的示图。

如图2A所示，在根据现有技术的爬坡方法中，由基于绝对坐标系的四个方向上的线性运动引起的改变(Δx、-Δx、Δy和-Δy)和两个方向上的旋转运动引起的改变(Δθ和-Δθ)被加入到获得的每个粒子的姿态。也就是说，根据现有技术的爬坡方法，六个改变被加入到每个粒子具有的机器人的姿态以轻微改变粒子的姿态，从而在位置概率增加的方向上改变机器人的位置。在G.Grisetti、C.Stachniss和W.Burgard的“Improving grid-based slam withRao-Blackwellized particle filters by adaptive proposals and selectiveresampling，”(IEEE机器人和自动化(ICRA)国际会议会刊，第2443-2448页，巴塞罗那，西班牙，2005)以及D.Hhnel、D.Schulz和W.Burgard的“Mapbuilding with mobile robots in populated environments，”(IEEE/RSJ智能机器人和系统(IROS)国际会议，2002年)中详细描述了根据现有技术的爬坡方法。因此，将省略其详细描述。将在下面描述一种计算机器人的位置的概率的方法。

由本发明提出的修改的爬坡方法可基于机器人的朝向角更新每个粒子的姿态。因此，如图2B所示，不是基于绝对坐标系执行爬坡，而是基于两个轴(即，根据机器人的朝向角的相对坐标系中的x’轴以及与x’轴垂直的y’轴)执行爬坡。

此外，爬坡步长(与现有技术中的Δx和Δy对应)可不设为常数，而是可适应于里程表600的模型的误差协方差椭圆而改变。这反映了这样的事实：里程表600可能在机器人移动的方向上产生较大的误差，这减少了执行爬坡所需的时间和粒子的位置的误差。将通过等式2对此进行定义。

【等式2】

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{\σ}}_x\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ -\sin \mathrm{\θ}& {\mathrm{\σ}}_y\cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]$

这里，x和y指示根据基于绝对坐标系的爬坡方法的轴。x’和y’指示根据基于机器人的朝向角的相对坐标系的爬坡方法的轴。

图3A和图3B是示出根据上述两种爬坡方法更新的粒子210的每一个的姿态的示图。在图3A中，粒子210的每一个不考虑机器人200的朝向角基于绝对坐标系以±Δx、±Δy和±Δθ移动其位置，从而每个粒子在位置概率增加的方向上移动。另一方面，在图3B中，以这样的方式执行爬坡技术：粒子210的每一个基于根据与机器人200移动的方向对应的朝向角的相对坐标系移动±Δx’、±Δy’和±Δθ的位置。这里，可以看出在±Δx’方向(即，机器人200的前进方向)上的步长距离间隔大于在±Δy’方向上的步长距离间隔。

因此，爬坡被应用于x轴运动(±Δx)、y轴运动(±Δy)和旋转运动(±Δθ)(优选地，根据相对坐标系的x’轴运动±Δx’、y’轴运动±Δy’和旋转运动±Δθ的六种情况)，并且可通过将在下面描述的等式5来计算关于每种情况的得分。所述得分指示通过爬坡来计算的权重。在爬坡结束之后，具有最高得分的值变成相应粒子的权重。具有最高得分的值的位置和朝向角变成相应粒子的姿态。

可执行若干次爬坡。也就是说，将爬坡应用于所述六种情况，并且与最高得分相应的值成为相应粒子的姿态。重复该处理以获得与最高得分相应的值。结果，在重复所述处理之后，具有最高得分的值被更新为相应粒子的权重，机器人的位置和朝向角被更新为相应粒子的姿态。

在移动机器人移动预定的距离之后，由传感器620获得作为新的观察信息的范围数据。基于所述范围数据和现有地图信息，粒子过滤单元630预测每个粒子的姿态的概率，并计算每个粒子的权重(S120)。

粒子的姿态与移动机器人的当前姿态越相似，粒子的权重越高。因此，可以看出当权重较高时，移动机器人具有拥有所述权重的粒子的姿态的概率很高。此外，可通过设置在移动机器人中并能够获取周围信息(具体地说，识别移动机器人和移动机器人周围的障碍之间的距离)的传感器620获得观察信息。传感器620可通过使用超声波或紫外线传感器620或构建的光模块来检测机器人和机器人周围的障碍之间的距离。

将描述使用粒子滤波器来预测粒子的姿态的概率并计算权重的方法。

移动机器人的位置可被表示为概率分布。可通过使用先前步骤(时间t-1)中的数据和当前步骤(时间t)中的数据的等式3来表示概率分布p。

【等式3】

$p\left(x_t^{\left(i\right)}\right|m_{t-1}^{\left(i\right)},x_{t-1}^{\left(i\right)},z_t,u_t)=\frac{p\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})p\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_t)}{\∫p\left(z_t\right|m_{t-1}^{\left(i\right)},x^{\′})p\left(x^{\′}\right|x_{t-1}^{\left(i\right)},u_t){\mathrm{dx}}^{\′}}$

这里，x_t-1指示时间t-1的姿态，u_t指示在时间t(当前)里程表600检测的数据，x_t指示在时间t的姿态。m_t-1指示关于地图的在时间t-1的数据，z_t指示传感器620在时间t获得的数据。概率分布指示当机器人在其中包括动态噪声或系统中存在的另外的不确定因素的情况下从x_t-1移动到x_t时在时间t的关于机器人姿态的概率分布。根据现有技术的基于网格的SLAM算法根据近似等式3执行计算。当传感器620的精确度远高于里程表600的精确度时(例如，如图4A所示，当里程表600和传感器620之间的精确度的标准差之比为30∶1时)，在观察区 $L^{\left(i\right)}=\left\{x\right|p\left(z_t\right|m_{t-1}^{\left(i\right)},x)>\mathrm{\ϵ}\}$ 中，根据传感器620的机器人姿态的概率值p(z_t|m_t-1 ⁽ⁱ⁾，x_t ⁽ⁱ⁾)迅速改变，而根据里程表600的机器人姿态的概率值p(x_t ⁽ⁱ⁾|x_t-1 ⁽ⁱ⁾，u_t)可近似为常数k。因此，等式3可近似地由如下的等式4表示：

【等式4】

$p\left(x_t^{\left(i\right)}\right|m_{t-1}^{\left(i\right)},x_{t-1}^{\left(i\right)},z_t,u_t)\≈\frac{p\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}{{\∫}_{x^{\′}\∈L^{\left(i\right)}}p\left(z_t\right|m_{t-1}^{\left(i\right)},x^{\′}){\mathrm{dx}}^{\′}}$

可通过如下的等式5来近似计算当前粒子的权重：

【等式5】

$w_t^{\left(i\right)}=w_{t-1}^{\left(i\right)}p\left(z_t\right|m_{t-1}^{\left(i\right)},x_{t-1}^{\left(i\right)})$

$=w_{t-1}^{\left(i\right)}\∫p\left(z_t\right|m_{t-1}^{\left(i\right)},x^{\′})p\left(x^{\′}\right|x_{t-1}^{\left(i\right)},u_t){\mathrm{dx}}^{\′}$

$\≈w_{t-1}^{\left(i\right)}k{\∫}_{x^{\′}\∈L^{\left(i\right)}}p\left(z_t\right|m_{t-1}^{\left(i\right)},x^{\′}){\mathrm{dx}}^{\′}$

$\≈w_{t-1}^{\left(i\right)}k\underset{j=1}{\overset{l}{\mathrm{\Σ}}}p\left(z_t\right|m_{t-1}^{\left(i\right)},x_j)$

$=w_{t-1}^{\left(i\right)}\mathrm{k\η}$

(其中， $\mathrm{\η}=\underset{j=1}{\overset{l}{\mathrm{\Σ}}}p\left(z_t\right|m_{t-1}^{\left(i\right)},x_j)$ )

在G.Grisetti、C.Stachniss和W.Burgard的“Improving grid-based slam withRao-Blackwellized particle filters by adaptive proposals and selectiveresampling，”(IEEE机器人和自动化(ICRA)国际会议会刊，第2443-2448页，巴塞罗那，西班牙，2005)中详细描述了通过上述的等式3、等式4和等式5测量姿态并计算每个粒子的权重的处理。

在粒子过滤单元630通过上述的等式3、等式4和等式5测量了每个粒子的姿态的概率并计算了每个粒子的权重之后，采样单元640基于所述权重对当前粒子重新采样(S130)。当从在初始阶段的N个粒子开始并且时间计数t增加1时，从所述N个粒子中移除具有较低权重的粒子。然后，基于具有较高权重的粒子再次对粒子进行采样，从而使N个粒子保持为常数。这称为重新采样。

然后，当通过地图估算更新网格地图时，权重调整单元650考虑传感器620的误差来调整权重(S140)。

由于传感器620的精确度不高，因此本发明的实施例提供一种在根据里程表600的机器人姿态的概率值p(x_t ⁽ⁱ⁾|x_t-1 ⁽ⁱ⁾，u_t)不被应用为常数的情况下计算权重的方法。也就是说，通过上述方法获得的权重被重新采样，并随后通过将在下面描述的方法进行调整。

当传感器620的精确度不高时，根据里程表600的姿态概率值不能近似为常数。也就是说，如图4B所示，里程表600和传感器620的精确度的标准差之比为10∶1，在观察区 $L^{\left(i\right)}=\left\{x\right|p\left(z_t\right|m_{t-1}^{\left(i\right)},x)>\mathrm{\ϵ}\}$ 中根据里程表600的机器人的姿态概率值p(z_t|m_t-1 ⁽ⁱ⁾，x_t ⁽ⁱ⁾不能近似为常数k。

图5A是示出当里程表600和传感器620的精确度的标准差之比为30∶1时，根据精确等式(等式3)的姿态概率值、根据近似等式(等式4)的姿态概率值以及所述精确等式和所述近似等式之间的误差比r(x)的示图。图5B是示出根据当里程表600和传感器620的精确度的标准差之比为10∶1时，根据精确等式(等式3)的姿态概率值、根据近似等式(等式4)的姿态概率值以及所述精确等式和所述近似等式之间的误差比r(x)的示图。

在30∶1的情况下，即使根据里程表600的机器人的姿态概率值被设为常数，在估算的机器人的位置附近(图5A的部分A)通常不产生误差，在此期间实际执行积分，并且可以看出误差比r(x)接近1。相反，在10∶1的情况下，由于传感器620的精确度相对不高，因此误差变得较大。这可以从以下事实中看出：即，图5B中分布的误差比r(x)在该区域(被估算为机器人位置的图5B的区域)中被相对较大地改变。

因此，在图5B中示出，可考虑误差比r(x)计算概率，通过将根据里程表600的机器人姿态的概率值设置为变量而不是常数来计算概率。

考虑误差比r(x)可获得下面的权重调整等式。

【等式6】

$w_t^{\left(i\right)}=w_{t-1}^{\left(i\right)}p\left(z_t\right|m_{t-1}^{\left(i\right)},x_{t-1}^{\left(i\right)})$

$=w_{t-1}^{\left(i\right)}\∫p\left(z_t\right|m_{t-1}^{\left(i\right)},x^{\′})p\left(x^{\′}\right|x_{t-1}^{\left(i\right)},u_t){\mathrm{dx}}^{\′}$

$=w_{t-1}^{\left(i\right)}k{\∫}_{x^{\′}\∈L^{\left(i\right)}}p\left(z_t\right|m_{t-1}^{\left(i\right)},x^{\′}){\mathrm{dx}}^{\′}\·r\left(x\right)$

$\≈w_{t-1}^{\left(i\right)}[k\underset{j=1}{\overset{l}{\mathrm{\Σ}}}p\left(z_t\right|m_{t-1}^{\left(i\right)},x_j)\·r\left(x_j\right)]$

$=w_{t-1}^{\left(i\right)}(\mathrm{k\η}\·r\left(x\right))$

为了有效执行权重调整，将等式6转换为如等式7所示的对数等式。

【等式7】

$\log w_t^{\left(i\right)}=\log w_{t-1}^{\left(i\right)}+\log \left(\mathrm{k\η}\right)+\log r\left(x\right)$

$=\log w_{t-1}^{\left(i\right)}+\log \left(\mathrm{k\η}\right)-R\left(x\right)$

当从等式5转换的对数等式与等式7比较时，在等式7减去R(x)大小。这是当传感器620的误差相对较大时产生的量，并且R(x)的形状和大小可根据每个传感器620的误差特性而改变。

这里，等式7中的R(x)可由P(x)和网格单元占用率c_j目乘来表达。将在下面描述的等式8中示出的P(x)的形式描述了沿着网格线的抛物线，如图6所示，所述抛物线连接传感器620和由传感器620检测的区域。当然，示出等式8中的P(x)的图6的曲线图的形状和值可根据传感器620进行各种改变。

【等式8】

$R\left(x\right)=\underset{j=0}{\overset{m}{\mathrm{\Σ}}}{c}_j\·P\left(x\right)$

在等式8中，m是取决于传感器620的精确度的值，c_j是0至1之间的网格单元占用率。此外，x指示关于所述姿态的每个粒子具有的地图上的网格点的距离，D指示由当前移动机器人的传感器620测量的到网格点的距离。因此，当由传感器620测量的当前移动机器人的网格点数据是正确的并且每个粒子的姿态都接近移动机器人的实际姿态时，值x和值D之间几乎没有差异。结果，值P(x)和值R(x)变得较小。相反，当每个粒子所具有的姿态与当前机器人的实际姿态不相似时，值x和值D之间存在巨大差异。结果，值P(x)和值R(x)变得较大。这被称为地图一致性检查。

通过执行关于每个粒子的地图一致性检查来检查值R(x)，从而可调整每个粒子的权重。在更新网格地图时对每个粒子执行权重调整。因此，计算时间未增加。

重复上述的处理从而估算正在移动的机器人的姿态。

图7是根据本发明实施例的使用粒子滤波器估算移动机器人的姿态的设备的框图。

根据本发明实施例的使用粒子滤波器估算移动机器人的设备包括：里程表600、粒子姿态计算单元610、传感器620、粒子滤波器单元630、权重调整单元650、采样单元640、地图产生单元660等。

里程表660检测移动机器人的姿态改变。例如，里程表600可包括编码器或陀螺仪。

粒子姿态计算单元610基于由里程表600检测到的姿态改变来获取每个粒子的姿态。此时，优选的是，按照根据现有技术的爬坡技术或者沿着坐标轴的方向和步长根据机器人的朝向角而改变的爬坡技术，在每个粒子的位置概率增加的方向上更新每个粒子的姿态。

传感器620设置在移动机器人的外部，并通过使用观察信息获得范围数据，具体地说，通过识别机器人和机器人周围的障碍之间的距离。在本发明的实施例中，可将具有相对较低精确度的结构光传感器用作传感器620。

粒子滤波器单元630预测当前粒子的姿态的概率，并基于由传感器620获得的范围数据和地图信息来获取当前粒子的权重。

权重调整单元650考虑传感器620的误差，调整由粒子过滤单元630获得的权重。此时，可基于当前粒子所在的位置在地图上的距离和由传感器620测量的距离，通过使用根据传感器620先前定义的误差特性值来调整权重。

采样单元640基于移动机器人的初始位置对粒子进行采样。在机器人移动之后，采样单元640基于由粒子滤波器单元630获得的权重对粒子进行重新采样。此时，当粒子被重新采样时，从所述粒子中移除具有较低权重的粒子，并基于具有较高权重的粒子对粒子进行采样。

在机器人移动的同时，地图产生单元660基于由传感器620获得的范围数据产生地图，并更新地图。

除了上述的示例性实施例，还可通过执行介质/媒体(例如，计算机可读介质/媒体)中/上的计算机可读代码/指令来实施示例性实施例。所述介质/媒体可对应于允许存储和/或传输计算机可读代码/指令的任何介质/媒体。所述介质/媒体还可包括分开或组合的计算机可读代码/指令、数据文件、数据结构等。所述代码/指令的示例包括：诸如由编译器产生的机器代码和包含可被计算装置等使用解释器执行的较高级代码的文件。另外，所述代码/指令可包括功能性程序和代码段。

可通过各种方式在介质/媒体中/上记录/传输计算机可读代码/指令，介质/媒体(可包括计算机可读代码/指令、数据文件、数据结构等)的示例包括磁性存储介质(例如，软盘、硬盘、磁带等)、光学介质(例如，CD-ROM、DVD等)、磁光媒体(例如，可光读碟)、硬件存储装置(例如，只读存储器媒体、随机访问存储器媒体、闪速存储器等)以及诸如发送信号的载波的存储/传输媒体，存储/传输媒体的例子可包括有线和/或无线传输媒体。例如，存储/传输媒体可包括光缆(optical wire)/光纤、波导和金属丝/线缆等(以上各项包括发送指定指令、数据结构、数据文件等的信号的载波)。介质/媒体还可以是分布式网络，从而能够以分布式存储/传输以及执行计算机可读代码/指令。可由一个或者多个处理器执行计算机可读代码/指令。还可在专用集成电路(ASIC)或者现场可编程门阵列(FPGA)中的至少一个中实施计算机可读代码/指令。

另外，可配置一个或多个软件模块或者一个或多个硬件模块以便执行上述示例性实施例的操作。

在这里使用的与代码/指令的执行有关的术语“模块”代表但不限于执行特定任务的软件组件、硬件组件、多个软件组件、多个硬件组件、软件组件和硬件组件的组合、硬件组件和多个软件组件的组合、软件组件和多个硬件组件的组合或者多个软件组件和多个硬件组件的组合。有利地，模块可被配置驻留在可寻址存储介质/媒体内也可被配置为在一个或多个处理器上执行。因此，模块可例如包括：组件(诸如软件组件、专用软件组件、面向对象软件组件、类组件和任务组件)、处理、功能、操作、执行线程、属性、过程、子程序、程序代码段、驱动程序、固件、微码、电路、数据、数据库、数据结构、表、数组和变量。在组件或模块中提供的功能组合成更少的组件或模块，或者进一步分离成附加的组件或模块。此外，可在装置中提供的至少一个处理器(例如，中央处理单元(CPU))操作组件或模块。另外，硬件组件的示例包括专用集成电路(ASIC)和现场可编程门阵列(FPGA)。如所示，模块还可代表软件组件和硬件组件的组合。这些硬件组件也可以是一个或多个处理器。

计算机可读代码/指令和计算机可读介质/媒体可被专门设计并为示例性实施例的目的而构造，或者可以是对于计算机硬件和/或计算机软件领域的技术人员的已知类型。

根据使用粒子滤波器估算移动机器人的姿态的方法、设备和介质，可获得以下的效果。

首先，即使使用具有相对较低精确度的低廉的传感器，仍可使用粒子滤波器根据基于网格的SLAM算法以高精确度估算移动机器人的姿态。

其次，可考虑机器人的朝向角通过执行爬坡技术来减少计算时间和位置误差。

虽然已经示出和描述了若干示例性实施例，但是本领域的技术人员将理解，可以对这些示例性实施例做出改变，实施例的范围由权利要求及其等同物限定。

Claims (23)

1.一种使用粒子滤波器估算移动机器人的姿态的方法，所述方法包括：

基于移动机器人的初始位置对当前粒子进行采样；

检测移动机器人的姿态的改变，并通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态；

基于由传感器获得的范围数据和地图信息预测所述当前粒子的姿态的概率并获取所述当前粒子的权重；

基于权重对所述当前粒子重新采样；以及

考虑传感器的误差调整权重。

2.如权利要求1所述的方法，其中，检测移动机器人的姿态改变并通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态的步骤包括：通过使用爬坡方法在位置概率增加的方向上更新当前粒子的姿态。

3.如权利要求1所述的方法，其中，检测移动机器人的姿态改变并通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态的步骤包括：通过使用基于根据移动机器人的朝向角的相对坐标系的爬坡方法在位置概率增加的方向上更新当前粒子的姿态。

4.如权利要求3所述的方法，其中，所述爬坡方法使步长适应于检测姿态改变的里程表模型的误差协方差椭圆而改变，从而使所述步长在机器人前进的方向上增加。

5.如权利要求1所述的方法，其中，考虑传感器的误差调整权重的步骤中，根据基于当前粒子所在的位置在地图上的距离和由传感器测量的距离而定义的误差特性值来调整所述权重。

6.如权利要求1所述的方法，其中，由编码器或陀螺仪检测所述姿态改变。

7.如权利要求1所述的方法，其中，所述姿态包括在平面上的二维坐标系中的坐标值和朝向角。

8.如权利要求1所述的方法，其中，所述传感器包括结构光传感器。

9.如权利要求1所述的方法，其中，重新采样步骤移除具有低权重的粒子，并基于具有高权重的粒子执行采样操作。

10.一种使用粒子滤波器估算移动机器人的姿态的设备，所述设备包括：

里程表，检测移动机器人的姿态改变；

粒子姿态计算单元，通过将检测到的姿态改变应用于先前粒子来计算当前粒子的姿态；

传感器，获取范围数据；

粒子滤波器单元，基于范围数据和地图信息预测所述当前粒子的姿态的概率并获取所述当前粒子的权重；以及

权重调整单元，考虑传感器的误差调整权重。

11.如权利要求10所述的设备，其中，粒子姿态计算单元通过使用爬坡方法在位置概率增加的方向上更新当前粒子的姿态。

12.如权利要求10所述的设备，其中，粒子姿态计算单元通过使用基于根据移动机器人的朝向角的相对坐标系的爬坡方法来在位置概率增加的方向上更新当前粒子的姿态。

13.如权利要求12所述的设备，其中，爬坡方法使得步长适应于检测姿态改变的里程表模型的误差协方差椭圆而改变，从而使步长在机器人前进的方向上增加。

14.如权利要求10所述的设备，其中，传感器包括结构光传感器。

15.如权利要求10所述的设备，其中，权重调整单元根据基于当前粒子所在的位置在地图上的距离和由传感器测量的距离而定义的误差特性值来调整所述权重。

16.如权利要求10所述的设备，其中，里程表包括编码器或陀螺仪。

17.如权利要求10所述的设备，其中，所述姿态包括在平面上的二维坐标系中的坐标值和朝向角。

18.如权利要求10所述的设备，还包括：

地图产生单元，基于范围数据产生地图。

19.如权利要求10所述的设备，还包括：

采样单元，基于移动机器人的初始位置对粒子进行采样，并基于由粒子滤波器单元获得的权重对所述粒子进行重新采样。

20.如权利要求19所述的设备，其中，重新采样步骤移除具有低权重的粒子，并基于具有高权重的粒子执行采样操作。

21.存储控制实施权利要求1所述的方法的至少一个处理器的计算机可读指令的至少一种计算机可读介质。

22.一种使用粒子滤波器估算移动机器人的姿态的方法，所述方法包括：

检测移动机器人的姿态改变，并通过将检测到的姿态改变应用于先前的粒子来计算当前粒子的姿态；

基于由传感器获得的范围数据和地图信息来预测所述当前粒子的姿态的概率并获取所述当前粒子的权重；以及

考虑传感器的误差调整权重以估算移动机器人的姿态。

23.存储控制实施权利要求22所述的方法的至少一个处理器的计算机可读指令的至少一种计算机可读介质。

Images