CN101049693A - 控制系统、具有该控制系统的移动机器人装置及控制方法 - Google Patents

Abstract

提供了一种移动机器人装置及其控制系统和方法。可通过使用机器人装置的传感器部件来估计当前状态变量。初始地，确定从传感器部件输出的当前数据是否满足必要条件。在当前数据不受干扰或外部干涉影响时，认为满足必要条件。然后，可从自传感器部件获得的前一采样值提取当前采样值。前一采样值可与前一状态变量相关。可基于当前采样值计算当前数据的权重。权重的值依赖于当前数据是否满足必要条件。然后，基于提取的当前采样值和计算的权重来估计当前状态变量。可基于估计的当前状态变量来提供机器人主体的定向移动的控制。提供一种当不能确定外部干扰是否反映在从传感器部件输出的数据中时使用概率分布来准确估计机器人主体的方向的控制系统。

Description

控制系统、具有该控制系统的移动机器人装置及控制方法

本申请要求于2006年4月4日在韩国知识产权局提交的第2006-0030681号韩国专利申请的利益，该申请的公开通过引用全部包含于此。

                       技术领域

本发明总体构思涉及一种控制系统、具有该控制系统的移动机器人装置及其控制方法。更具体地讲，本发明总体构思涉及一种能够使用传感器并基于是否满足必要条件的确定来估计用于移动机器人的控制数据的控制系统。本发明总体构思还涉及具有该控制系统的移动机器人装置及其控制方法。

                       背景技术

将参照图1描述具有通过使用输出数据的传感器来估计最优机器人控制数据的控制系统的传统移动机器人装置。

图1是传统移动机器人装置的控制框图。如图1所示，移动机器人装置包括机器人主体100、传感器部件200、数据选择部件300和滤波器部件400。

传感器部件200可包括一个或多个传感器，用于感测机器人主体100的方向，例如，机器人主体相对于地磁场的绝对方向。从传感器部件200输出的数据(下面称作“感测数据”)包括在感测机器人主体的方向期间从周围环境引入的外部干扰，以及在测量感测数据时产生的测量误差。

总之，干扰具有非高斯有色分布(colored non-Gaussian distribution)，测量误差具有高斯白分布(white Gaussian distribution)。详细地，所述干扰可使得从传感器部件200输出的感测数据失真并无效。

数据选择部件300使用预定规则来确定从传感器部件200输出的感测数据中包含的干扰是否可以被忽略。如果感测数据满足该规则，则确定该干扰可以被忽略，然后将感测数据输出到滤波器部件400。

可以由Kalman滤波器来实现的滤波器部件400通过使用从数据选择部件300输入的感测数据准确地估计机器人主体100的方向。当输入的感测数据具有高斯概率分布时，Kalman滤波器可以计算(方向估计的)最优值。

然而，如果数据选择部件300使用的规则是不准确的，则具有非高斯概率分布的感测数据可被输入到滤波器部件400。换句话说，尽管实际上干扰不能被忽略，但是干扰仍然可以满足所述规则，因此可以被输入到滤波器部件400，这会导致不期望的结果，例如，机器人主体100的错误的方向。

例如，假设传感器部件200包括两个磁罗盘，并且数据选择部件300应用的规则为：如果从两个磁罗盘输出的感测数据相同，则干扰可以被忽略。如果包含干扰的感测数据从传感器部件200的两个磁罗盘输出，并且恰好相同，则数据选择部件300确定干扰可以被忽略(尽管干扰不应该被忽略)，然后将感测数据(包含干扰)输出到滤波器部件400。

结果，由滤波器部件400估计的机器人主体100的方向与机器人主体100相对于地磁场的期望的方向极大不同。这可导致移动机器人装置向着错误方向移动。

因此，找到用于数据选择部件300的准确的规则是重要的。然而，难于找到这种使数据选择部件300准确地确定感测数据中是否包含干扰的规则。

为了部分解决这个问题，在2006 IEEE国际机器人学与自动化会议的论文集中，由Woong Kwon、Kyung-Shik Roh和Hak-Kyung创作的题为“ParticlesFilter-based Heading Estimation using Magnetic Compasses for mobile RobotNavigation”的文章中，曾经提出一种间接使用在感测数据没有由于干扰而失真时满足的必要条件来估计感测数据的方法。使用必要条件的原因是：不难找到感测数据有效时要满足的必要条件。

这个方法可以减少对感测数据的有效性错误判断的可能性，但是，由于仅使用用于确定感测数据的有效性的必要条件，而不是充分条件，因此，不能完全消除错误判断的可能性。

                       发明内容

本发明总体构思提供一种控制系统，该控制系统能够在不能确定外部干扰是否反映在从传感器部件输出的数据中时使用概率方法来准确地估计机器人主体的方向。还提供了一种具有该控制系统的移动机器人装置及其控制方法。

本发明总体构思还提供一种控制系统，该控制系统即使在感测数据不具有高斯概率分布时也能够准确估计机器人主体的方向。还提供了一种具有该控制系统的移动机器人装置及其控制方法。

本发明总体构思还提供了一种控制系统，该控制系统能够通过融合具有与传感器有效性相关问题的各种传感器来获得机器人方向的最优估计值。

本发明总体构思还提供一种控制系统，所述控制系统能够通过粗略地融合包括一些输出由于过度扰动而失真的无效数据的传感器的普通传感器系统中的各种传感器，准确估计状态变量。

可通过提供一种控制系统的控制方法来实现本发明总体构思的上述和/或其它方面和效用，所述控制方法使用从控制系统中的传感器部件输出的当前数据来估计当前状态变量。所述方法包括：确定从传感器部件输出的当前数据是否满足必要条件；从前一采样值提取当前采样值，其中，前一状态变量从所述前一采样值获得；基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；基于提取的当前采样值和计算的权重来估计当前状态变量。

还可通过提供一种控制移动机器人装置的方法来实现本发明总体构思的上述和/或其它方面和效用，所述移动机器人装置具有机器人主体和传感器部件，所述传感器部件输出与机器人主体相对于地磁场的当前方向相关的当前数据。所述方法包括：确定从传感器部件输出的当前数据是否满足必要条件，其中，当从传感器部件输出的当前数据不受地磁场影响时，认为满足必要条件；从自传感器部件获得的前一采样值提取当前采样值，其中，前一采样值与机器人主体的前一方向相关；基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；基于提取的当前采样值和计算的权重估计机器人主体的当前方向。

估计机器人主体的当前方向的步骤包括：使用提取的当前采样值和计算的权重，并将提取当前采样值和计算权重的步骤重复预定次数来计算概率分布。可计算概率分布的平均值来估计机器人主体的当前方向。

估计机器人主体的方向的步骤包括：基于计算的概率分布的平均值来估计机器人主体的方向。基于当前采样值计算当前数据的权重的步骤包括：在当前数据满足必要条件时，根据下述式1来计算当前数据的权重，在当前数据不满足必要条件时，根据下述式2来计算当前数据的权重：

<式1>p(Z_t|X_t，C，V)p(C|V)+p(Z_t|X_t，C^c，V)p(C^c|V)；

<式2>p(Z_t|X_t，C，V^c)p(C|V^c)+p(Z_t|X_t，C^c，V^c)p(C^c|V^c)

其中，Z_t是当前时间t时的当前数据，X_t是当前时间t时的机器人方向的估计值，C是当前数据不具有由于地磁场干扰引起的数据失真的事件，C^c是当前数据受到地磁场干扰的影响的事件，V是满足当传感器部件不具有由于地磁场干扰引起的数据失真时的必要条件的事件，V^c是不满足当传感器部件不具有由于地磁场干扰引起的数据失真时的必要条件的事件。

传感器部件可包括磁罗盘。此外，可使用粒子滤波器来估计机器人主体的方向。

因此，可基于估计的当前方向提供关于机器人主体的定位或定向控制。

还可通过提供一种控制系统来实现本发明总体构思的上述和/或其它方面和效用，所述控制系统包括：传感器部件、必要条件确定部件和估计器，用于执行上述各种方法操作，以估计当前状态变量(例如，机器人主体的方向)。

还可通过提供一种移动机器人装置来实现本发明总体构思的上述和/或其它方面和效用，所述移动机器人装置包括：机器人主体；传感器部件，输出与机器人主体相对于地磁场的方向相关的数据；必要条件确定部件，设置当输出数据不受外部磁场影响时满足的必要条件，并确定从传感器部件输出的当前数据是否满足必要条件；数据估计器，从前一采样值提取当前采样值，根据所述采样值，从传感器部件获得机器人主体的方向；基于当前采样值计算当前数据的权重，所述权重依赖于是否满足必要条件；基于提取的当前采样值和计算的权重来估计机器人主体的方向。

数据估计器通过将提取当前采样值并计算权重的操作重复预定次数来计算提取的当前采样值和计算的权重，作为概率分布。

数据估计器可基于计算的概率分布的平均值估计机器人主体的方向。

数据估计器可包括基于当前采样值计算当前数据的权重的权重计算器，其中，当必要条件确定部件确定当前数据满足必要条件时，根据下述式1计算当前数据的权重；当必要条件确定部件确定当前数据不满足必要条件时，根据下述式2来计算当前数据的权重：

<式1>

p(Z_t|X_t，C，V)p(C|V)+p(Z_t|X_t，C^c，V)p(C^c|V)

<式2>

p(Z_t|X_t，C，V^c)p(C|V^c)+p(Z_t|X_t，C^c，V^c)p(C^c|V^c)

其中，Z_t是当前时间t时的当前数据，X_t是当前时间t时的机器人方向的估计值，C是当前数据不具有由于地磁场干扰引起的数据失真的事件，C^c是当前数据受到地磁场干扰的影响的时间，V是满足当传感器部件不具有由于地磁场的干扰引起的数据失真时的必要条件的事件，V^c是不满足当传感器部件不具有地磁场干扰引起的数据失真时的必要条件的事件。

数据估计器可以被配置为基于估计的当前方向来为机器人主体提供信号，以将机器人主体定位在下一方向上。

还可通过提供一种其中包含控制机器人装置的定向移动的方法的处理器来实现本发明总体构思的上述和/或其它方面和效用，所述方法包括：接收由机器人装置的传感器部件输出并且与机器人装置相对于外部干扰的主体的当前方向相关的当前数据；确定从传感器部件输出的当前数据是否满足必要条件，其中，当从传感器部件输出的当前数据不受外部干扰的影响时，认为满足外部条件；从自传感器获得的前一采样值提取当前采样值，其中，前一采样值与主体的前一方向相关；基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；基于提取的当前采样值和计算的权重来估计机器人装置的主体的当前方向。

通过使用提取的当前采样值和计算的权重计算概率分布的平均值来执行估计主体的当前方向的步骤。

所述方法还包括：基于估计的当前方向为机器人装置的主体提供信号，以控制主体的定向移动。

还可通过提供一种含有程序指令的数据存储介质来实现本发明总体构思的上述和/或其它方面和效用，当由机器人装置中的处理器执行所述程序指令时，所述程序指令使得处理器执行下述操作：接收由机器人装置的传感器部件输出并且与机器人装置的主体相对于外部干扰的当前方向相关的当前数据；确定从传感器部件输出的当前数据是否满足必要条件，其中，当从传感器部件输出的当前数据不受外部干扰的影响时，认为满足外部条件；从自传感器获得的前一采样值提取当前采样值，其中，前一采样值与主体的前一方向相关；基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；基于提取的当前采样值和计算的权重来估计机器人装置的主体的当前方向。

                       附图说明

通过下面结合附图对实施例的描述，本发明总体构思的这些和/或其它方面和优点，将会变得更加清楚和易于理解，其中：

图1是传统移动机器人装置的控制框图；

图2是具有根据本发明总体构思的实施例的控制系统的移动机器人装置的控制框图；

图3是示出根据本发明总体构思的实施例估计状态变量的示例性控制过程的流程图；

图4是根具有根据本发明总体构思的实施例的控制系统的移动机器人装置的立体图；

图5是示出具有根据本发明总体构思的实施例的控制系统的图4中的移动机器人装置的操作的流程图。

                       具体实施方式

现在，将详细描述本发明总体构思的实施例，其例子列举在附图中，其中，相同的标号始终表示相同的部件。下面，参照附图描述实施例，以解释本发明的总体构思。

图2是具有根据本发明总体构思的实施例的控制系统的移动机器人装置的控制框图。图3是示出根据本发明总体构思的实施例估计状态变量的示例性控制过程的流程图。如下面所讨论的，状态变量可包括，例如，关于移动机器人装置中的机器人主体的方向或方位而被将确定的值。尽管下面术语“值”可能不与术语“方向”、“方位”等一起使用，但是，本领域的技术人员显然可知，这里讨论的“方向”或“方位”的确定或估计隐含地包括对于“方向”的“值”(例如，离开参考方位30°等)的确定，从而以自动的方式完成机器人主体的正确的位置放置。

如图2中所示，移动机器人装置，例如清洁机器人、服务机器人等，可包括机器人主体10、传感器部件20、必要条件确定部件30和数据估计器40。

机器人主体10可具有移动轮(未示出)。机器人主体10可根据由数据估计器40提供的与方向相关的数据移动，这将在后面描述，机器人主体10可包括CPU或微型计算机(未示出)，以检测和控制机器人主体的当前移动位置、当前移动方向等。

在实施例中，传感器部件20可以附着于机器人主体10的一侧，并且可输出机器人主体10(相对于地磁场)的与方向相关的数据。传感器部件20可包括一个或多个传感器。从传感器部件20输出的与方向相关的数据可包括例如由于外部磁场干扰(例如，电子装置(例如，扬声器)的干扰)、或者是铁框或壁引起的失真。

必要条件确定部件30设置当机器人主体10不受外部磁场干扰影响时认为应该被满足的必要条件，还确定当前数据是否满足设置的必要条件。

如果当前数据不满足必要条件，则必要条件确定部件30确定当前数据受外部磁场干扰的影响。即使在当前数据满足必要条件时，当前数据仍然受外部磁场干扰的影响。

下面，将描述寻找当不受外部磁场干扰时满足的必要条件的过程。对于该过程中的详细的式，请参考上面提到的在2006 IEEE国际机器人学与自动化会议的论文集中，由Woong Kwon、Kyung-Shik Roh和Hak-Kyung创作的题为“Particles Filter-based Heading Estimation using Magnetic Compasses formobile Robot Navigation”的文章。

总之，按照下面的式1，从由传感器部件20感测的原始信息r_x和r_y计算分别在x和y方向上感测的地磁场(EMF)分量H_x和H_y。

<式1>

H_x＝|H_E|(r_x-o_x)/(r_xmax-r_xmin)

H_y＝|H_E|(r_y-o_y)/(r_ymax-r_ymin)

这里，|H_E|表示地磁场的大小，r_xmax和r_ymax分别表示r_x和r_y的最大值，r_xmin表示r_x的最小值，r_ymin表示r_y的最小值，o_x和o_y分别表示0.5(r_x，r_ymax+r_x，r_ymin)和0.5(r_xmax，r_y+r_xmin，r_y)的偏移值。

传感器部件20执行对计算的H_x和H_y的反正切(arctangent)计算，并输出H_x和H_y的反正切值作为当前数据。从传感器部件20输出原始信息作为具有比例因子和偏移的当前数据的过程被称作“校准”。

尽管这个校准过程可以补偿移动机器人装置的内部产生的内部磁场干扰，但是，它不能补偿从外部(例如，电子设备、壁、支柱、金属等)引入的外部磁场干扰。因此，通过使用比例因子的幅值的上限和下限以及偏移的上限和下限获得必要条件，所述比例因子的幅值的上限和下限以及偏移的上限和下限是在没有或几乎没有外部磁场干扰的地方，例如，远离壁、支柱、金属等地方通过校准过程计算的。

使用从传感器部件20输出的当前数据，根据下面的式2可获得地磁场的大小：

<式2>

${\left|H\right|}^2={(\frac{r_x-O_x}{{\hat{A}}_x}\&times;|H_E\left|\right)}^2+{(\frac{r_y-O_y}{{\hat{A}}_y}\&times;|H_E\left|\right)}^2,$

其中，O_x和O_y表示基本为常量值的偏移，因此，可以被假定为常量，

和

表示通过校准过程计算的幅值。

使用幅值

和 的上限和下限，建立的下述式3可具有95％的可靠性：

<式3>

$A_{\mathrm{yl}}\&le;A_y^{\mathrm{true}}\&le;A_{\mathrm{yu}}$

$A_{\mathrm{xl}}\&le;A_x^{\mathrm{true}}\&le;A_{\mathrm{xu}},$

其中，A_yl是A_y的下限，A_yu是A_y的上限，A_xl是A_x的下限，A_xu是A_x的上限。

在式3中，A_y ^true和A_x ^true表示地磁场区域中的幅值，并且具有未知值。A_y ^true和A_x ^true具有未知值的原因是这些幅值由于弱的外部磁场干扰、测量误差、地板的不平坦(例如，移动机器人装置放置的地方)等原因而具有不确定性。然而，由于这些不确定性可以被等效地施加到x和y轴，因此，可以假定建立下面的式4：

<式4>

$\frac{A_x^i-A_{\mathrm{xl}}}{A_{\mathrm{xu}}-A_{\mathrm{xl}}}=\frac{A_y^i-A_{\mathrm{yl}}}{A_{\mathrm{yu}}-A_{\mathrm{yl}}}.$

由于在式4中获得的幅值被期望符合统计分布的特性，因此也可以建立下面的式5：

<式5>

$A_y^{\mathrm{true}}\&cong;a{A}_x^{\mathrm{true}}+b,$

其中，

a≡(A_yu-A_yl)/(A_xu-A_xl)

b≡(A_ylA_xu-A_xlA_yu)/(A_xu-A_xl)。

此时，当新的未知变量定义为如下：

并且引入安全裕度γ_x和γ_y时，可从上述式3导出下述式6：

<式6>

或者ψ_xl≤ψ_x≤ψ_xu

或者ψ_yl≤ψ_y≤ψ_yu。

如果机器人主体10不受外部磁场干扰的影响，则可从上述式1得到下述式7：

<式7>

${\left(\frac{r_x-O_x}{A_x^{\mathrm{true}}}\right)}^2+{\left(\frac{r_y-O_y}{A_y^{\mathrm{true}}}\right)}^2=1.$

使用上述式2和6，可以获得与从传感器部件20输出的当前数据相关的下述式8，并且也可在假设当前数据不受外部磁场干扰的影响的条件下获得下述式9：

<式8>

|H|²/|H_E|²＝(ψ_xsin²θ+ψ_ycos²θ)，

其中，θ是当前数据。

<式9>

${\mathrm{\&theta;}}^{\mathrm{true}}(=\arctan \frac{A_y^{\mathrm{true}}(r_x-O_x)}{A_x^{\mathrm{true}}(r_y-O_y)}).$

整理上述式8，可得下述式10：

<式10>

${\left|H\right|}^2/{\left|H_E\right|}^2=0.5({\mathrm{\&psi;}}_x^2+{\mathrm{\&psi;}}_y^2+({\mathrm{\&psi;}}_y^2\&CenterDot;{\mathrm{\&psi;}}_x^2)\cos 2\mathrm{\&theta;})$

其中，将上述式5和6代入获得的式10中，获得下述式11：

<式11>

其中， ${\mathrm{\&psi;}}_1^{+}={\mathrm{\&psi;}}_{x1}^2+{\mathrm{\&psi;}}_{y1}^2,$ ${\mathrm{\&phi;}}_1=\min ({\mathrm{\&psi;}}_1^{-}\cos 2\mathrm{\&theta;},{\mathrm{\&psi;}}_u^{-}\cos 2\mathrm{\&theta;}),$ ψ_u ⁺＝ψ_xu ²+ψ_yu ²，φ_u＝max(ψ_l ^-cos2θ，ψ_u ^-cos2θ)， ${{\mathrm{\&psi;}}_1}^{-}={\min }_{{\mathrm{\&psi;}}_y=a^{\&prime;}{\mathrm{\&psi;}}_x+b^{\&prime;}}({{\mathrm{\&psi;}}_y}^2-{{\mathrm{\&psi;}}_x}^2),{{\mathrm{\&psi;}}_u}^{-}={\min }_{{\mathrm{\&psi;}}_y=a^{\&prime;}{\mathrm{\&psi;}}_x+b^{\&prime;}}({{\mathrm{\&psi;}}_y}^2-{{\mathrm{\&psi;}}_x}^2),$ θ是当前数据。

如果传感器部件20包括两个传感器，则可在两个传感器感测的地磁场的大小之间出现不同。在这种情况下，可获得下述式子：

<式12>

$f_{\mathrm{xl}}+f_{\mathrm{yl}}+f_y^{-}\&le;2({|}^{1}H{|}^2\&CenterDot;{|}^{2}H{|}^2)/{\left|H_E\right|}^2\&le;f_{\mathrm{xu}}+f_{\mathrm{yu}}+f_u^{-}$

其中，f_x1＝min f_x(¹ψ_x)，f_y1＝min f_y(¹ψ_y)，f_xu＝max f_x(¹ψ_x)，f_yu＝max f_y(¹ψ_y)，其中，¹ψ_x1≤¹ψ_x≤¹ψ_xu，¹ψ_y1≤¹ψ_y≤¹ψ_yu， $f_x\left(\mathrm{\&psi;}_y^1\right)=\mathrm{\&psi;}_x^2{}_1{({{\mathrm{\&alpha;}}^{\&prime;}}_x\mathrm{\&psi;}_x^1+{{\mathrm{\&beta;}}^{\&prime;}}_x)}^2,$ $f_y\left(\mathrm{\&psi;}_y^1\right)=\mathrm{\&psi;}_y^2{}_1{({{{\mathrm{\&alpha;}}^{\&prime;}}_y}^1{\mathrm{\&psi;}}_y+{{\mathrm{\&beta;}}^{\&prime;}}_y)}^2,$ $f_1^{-}=(g_1\cos 2\mathrm{\&theta;},g_u\cos 2\mathrm{\&theta;}),$ $f_u^{-}=\max \left(g_1\cos {2\mathrm{\&theta;},g}_u\cos 2\mathrm{\&theta;}\right),$ $g_u={\max }_{\mathrm{\&psi;}_y^1=a^{\&prime;1}{\mathrm{\&psi;}}_x+b^{\&prime;}}(f_y\left(\mathrm{\&psi;}_y^1\right)\&CenterDot;f_x\left(\mathrm{\&psi;}_x^1\right)).$ $g_u={\max }_{\mathrm{\&psi;}_y^1=a^{\&prime;1}{\mathrm{\&psi;}}_x+b^{\&prime;}}(f_y\left(\mathrm{\&psi;}_y^1\right)\&CenterDot;f_x\left(\mathrm{\&psi;}_x^1\right)).$ 上述式11和12表示当机器人主体10不受外部磁场干扰的影响时满足的必要条件。因此，必要条件确定部件30确定当前数据是否满足式11，然后，如果满足式11，则确定当前数据是否满足式12。

如果当前数据不满足式11，或者尽管当前数据式11，但是不满足式12，则必要条件确定部件30确定当前数据受外部磁场干扰的影响。

尽管在上述讨论的实施例中，必要条件是两个，但是应该理解的是，必要条件也可以是一个或三个或更多。

数据估计器40将从传感器部件20输出的当前数据和将从传感器部件20输出的采样值进行比较，并基于比较的结果，估计当前状态变量(例如，如上面所提到的机器人主体10的方向的值)。在本实施例中，数据估计器40可以由在即使输入数据具有非高斯概率分布时也能够计算准确值的粒子滤波器来实现。

在本实施例中，数据估计器40还可包括权重计算器45，用于基于采样值(从传感器20输出)计算当前数据(也从传感器20输出)的权重，这将在后面更详细地描述。

下面，将参照图3来详细描述校正从传感器部件20输出的当前数据的过程，图3是示出根据本发明总体构思的实施例的估计状态变量(例如，机器人主体10的方向和方位的值)的示例性控制过程的流程图。

参照图3，在操作S11中，在当前时间“t”从传感器部件20输出当前数据Z_t时，必要条件确定部件30(在操作S13中)确定当前数据Z_t是否满足必要条件。

数据估计器40在操作S15中从概率分布 $p\left(x_{t-1}\right)={\{x_{t-1}^i,w_{t-1}^i)}_{i=1,...,N}$ 中提取多个(N)采样值X_t-1，其中，所述多个采样值X_t-1在前一时间“t-1”被提取，并具有在前一时间“t-1”分配给它们的相应权重W_t-1。在这种情况下，具有更高权重的采样值更有可能被提取。下面将描述概率分布的细节。

此外，数据估计器40在考虑在前一时间“t-1”预先提取的第i个采样值Xⁱ _t-1的情况下，提取将在当前时间“t”输出的第i个采样值Xⁱ _t-1。在这种情况下，使用测量误差概率分布P(X_t|X_t-1，V_t-1)(其中，V_t-1是在前一时间“t-1”时的输入控制)来提取当前时间“t”的采样值Xⁱ _t。

例如，假设前一时间“t-1”的采样值X_t-1是30°，并且从机器人主体10输入移动2°的控制指令。然后，数据估计器40计算平均值为32°(这是30°的前一时间t-1的采样值X_t-1和2°的控制输入的相加的和)的高斯概率分布，从计算的高斯概率分布提取在当前时间“t”的采样值Xⁱ _t。

在操作S17，数据估计器40的权重估计器45从当前数据Z_t落入提取的采样值Xⁱ _t的预定范围内的概率来计算权重Wⁱ _t。这里，权重W_t是指似然性p(z|x)，即，从当前时间“t”的采样值Xⁱ _t获得当前数据Z_t的概率。

下面，将描述权重计算器45如何计算权重Wⁱ _t。

开始，定义当前数据Z_t不受外部磁场干扰影响的事件C和当前数据Z_t满足必要条件的事件V来计算权重Wⁱ _t。详细地，为了防止当前数据Z_t受到外部磁场干扰的影响，必须产生事件C。然而，通常不可能仅通过从传感器部件20输出的当前数据Z_t确认事件C是否产生。然而，可以从必要条件确定部件30的确定结果确认事件V是否产生。因此，当产生事件V时，可根据贝叶斯定理和全概率公式(Total probability theorem)来获得下述式13：

<式13>

$w_t^i{}_k=p\left(z_t^i\right|x_t^i,C,V)p\left(C\right|V)+p\left(z_t^k\right|x_t^i,C^c,V)p\left(C^c\right|V)$

其中，Z_t是当前时间“t”时的当前数据，X_t是当前时间“t”时的机器人方向的估计值，C是当前数据不具有由于外部磁场干扰引起的失真的事件，C^c是当前数据受到外部磁场干扰的事件，V是满足当传感器部件不具有由于干扰引起的数据失真时的必要条件的事件。V^c是不满足当传感器部件不具有干扰引起的数据失真时的必要条件的事件，上标“k”表示第k个传感器。

同样地，当未产生事件V时，可获得下述式子：

<式14>

$w_t^i{}_k=p\left(z_t^k\right|x_t^i,C,V^c)p\left(C\right|V^c)+p\left(z_t^k\right|x_t^i,C^c,V^c)p\left(C^c\right|V^c)$

其中，Z_t是当前时间“t”时的当前数据，X_t是当前时间“t”时机器人方向的估计值，C是当前数据不具有由于外部磁场干扰引起的数据失真的事件，C^c是当前数据受外部磁场干扰的事件，V是满足传感器部件不具有由于干扰引起的数据失真时的必要条件的事件，V^c是不满足在传感器部件不具有由于干扰引起的失真时的必要条件的事件，上标“k”表示第k个传感器。

可以假设从传感器20输出的当前数据Z_t的分布通常独立于事件V。因此，可建立下述式15和16：

<式15>

$p\left(z_t^k\right|x_t^i,C)=p\left(z_t^k\right|x_t^i,C,V)=p\left(z_t^k\right|x_t^i,C,V^c)$

<式16>

$p\left(z_t^k\right|x_t^i,C^c)=p\left(z_t^k\right|x_t^i,C^c,V)=p\left(z_t^k\right|x_t^i,C^c,V^c)$

上述式15的概率分布表示产生了事件C，即当前数据Z_t不受外部磁场干扰的影响。因此，这个概率分布可被表达为下述式17的高斯高率分布：

<式17>

$p\left(z_t^k\right|x_t^i,C)~N(h\left(x_t^i\right),\mathrm{\&sigma;}_z^k)$

其中，σ_z表示传感器部件20的测量误差的标准偏差，包括真实测量误差、校准的不确定性引起的误差、传感器的制造误差等，这可以通过试验来确定。

上述式16的概率分布表示当前数据Z_t受到外部磁场干扰的影响。该概率分布可以是用于提取采样值X_t而不是当前数据Z_t的任何概率分布，因此，该概率分布可以被表达为下述式18的高斯概率分布：

<式18>

$p\left(z_t^k\right|x_t^i,C^c)~N(h\left(x_t^i\right),\mathrm{\&sigma;}_{h\left(x\right)}^k)$

其中，σ_h(x)作为当前数据Z_t的标准偏差，是独立于测量误差的系统误差。

当从传感器部件20输出的当前数据Z_t满足必要条件时，上述式13中的P(C|V)表示当前数据Z_t不受外部磁场干扰影响的概率，并且可通过试验获得。

可从1-P(C|V)来计算上述式中的P(C^c|V)，并且可根据下述式19来获得上述式14中的P(C^c|V^c)。

<式19>

$p\left(C^c\right|V^c)=\frac{(1-p\left(C\right)+p\left(C\right)p\left(V\right|C))p\left(C\right|V)-p\left(C\right)p\left(V\right|C)}{p\left(C\right|V)-p\left(C\right)p\left(V\right|C)}$

其中，P(C)是当前数据Z_t不受外部磁场干扰影响的概率，P(V|C)是当前数据Z_t满足在当前数据Z_t不受外部磁场干扰影响时的必要条件的概率。可在没有外部磁场干扰的条件下通过试验获得这些概率。

因此，在当前数据Z_t满足必要条件时，权重计算器45可以使用上述式13来计算权重Wⁱ _t，在当前数据Z_t不满足必要条件时，权重计算器45可以使用上述式14来计算权重Wⁱ _t。这里，由于上述式13和14不具有高斯概率分布，因此，可以使用粒子滤波器(未示出)来获得权重Wⁱ _t。

数据估计器40根据计算的权重Wⁱ _t估计机器人主体10的方向。在本实施例中，权重计算器45可将计算权重的过程(操作S13和操作S15)重复N次。重复的次数越多，校正的当前估计值Xt[Zt]的可靠性越高。

例如，如果重复的次数是100(即，N＝100)，则可获得具有当前采样值Xⁱ _t和计算的权重Wⁱ _t的概率分布(P＝{X_t ⁱ，W_t ⁱ}_i＝1...100)，其平均值(∑X_t*W)作为当前时间t时机器人主体10的方向。

然后，数据估计器40为机器人主体10提供关于当前时间“t”时的采样值X_t和权重W_t的概率分布(P＝{X_t ⁱ，W_t ⁱ}_i＝1...100)，这将用于在下一时间“t+1”移动机器人主体10。

然后，在下一时间“t+1”重复操作S11至S17，并且在下一时间“t+1”，在操作S11中使用在当前时间“t”从机器人主体10提供的概率分布(P＝{X_t ⁱ，W_t ⁱ}_i＝1...100)。

通过上述过程，在操作S19中，可以在从传感器部件20输出的当前数据可能受到外部磁场干扰的影响的情况下，根据是否满足必要条件，来准确地计算机器人主体10的实际方向(即，当前状态变量)。

此外，由于数据估计器40可以由粒子滤波器(未示出)来实现，因此，即使当从传感器部件20输出的当前数据具有非高斯分布时，也能在操作S19中准确计算机器人主体10的实际方向。

图4是具有根据本发明总体构思的实施例的控制系统的移动机器人装置的立体图。参照图4，将更详细地描述移动机器人装置，所述移动机器人装置具有由磁罗盘实现的传感器部件20(例如，传感器20a和20b)以及由粒子滤波器(未示出)实现的数据估计器40。这里应该注意，必要条件确定部件30也可被包含作为机器人装置的部件(例如，位于机器人主体10的内部)，即使这种包含在图4中是不可见的。

如图4中所示，传感器部件20包括多个磁罗盘，例如，包括第一磁罗盘(下面称为“第一传感器”)20a和第二磁罗盘(下面称为“第二传感器”)20b。

第一传感器20a和第二传感器20b附着于机器人主体10上，并输出与机器人主体10相对于地磁场的方向相关的数据，即，机器人主体10偏离地磁场北极的角度。此时，第一传感器20a和第二传感器20b可同时感测机器人主体10的方向，并分别输出第一当前数据和第二当前数据。

必要条件确定器30确定从第一传感器20a输出的第一当前数据和从第二传感器20b输出的第二当前数据是否满足必要条件。

当粒子滤波器(例如，用于实施数据估计器40的粒子滤波器)从前一时间的采样值X_t-1提取当前时间的采样值X_t时，权重计算器45计算使从第一传感器20a输出的第一当前数据落入提取的采样值X_t的预定范围内的第一权重以及使从第二传感器20b输出的第二当前数据落入提取的采样值X_t的预定范围内的第二权重。

由于第一传感器20a和第二传感器20b相互独立，因此粒子滤波器将第一权重乘以第二权重来计算权重Wt，并使用计算的权重Wt来估计当前时间“t”时的机器人主体10的方向。当前时间“t”时的方向因此被当作当前状态变量，从提取的采样值和相应计算的权重来估计当前状态变量的值。

由于可使用当传感器部件20包括多个磁罗盘时的概率，通过上述过程计算当前估计值，因此，可以准确地计算机器人主体10的移动方向。

此外，即使当不能确定从磁罗盘20a和20b输出的当前数据是否受外部磁场干扰的影响时，也可使用粒子滤波器来准确地计算机器人主体10的移动方向。

下面，将参照图5来描述控制具有根据本发明总体构思的实施例的控制系统的移动机器人装置的方法。图5是示出具有根据本发明总体构思的实施例的控制系统的图4中的移动机器人装置的操作的流程图。

如图5中所示，在操作S100中，当从传感器部件20输出的数据不受外部磁场干扰影响时，必要条件确定部件30将必要条件设置为“满足”。因此，当发现从传感器部件20输出的数据不受外部磁场干扰的影响时，认为“满足”必要条件。

在操作S101中，传感器部件20输出与机器人主体10相对于地磁场的方向相关的当前数据Z_t。

在操作S103中，必要条件确定部件30确定从传感器部件20输出的当前数据Z_t是否满足必要条件。

在操作S105中，计数变量“i”被初始化为“1”，以将后续操作重复N次。

在操作S107中，数据估计器40从采样值X_t-1提取当前时间的采样值X_t，所述采样值X_t-1是在前一时间从传感器部件20提取的。此时，可以通过考虑前一数据Z_t-1和在前一时间t-1与机器人主体10的运动相关的控制输入来提取采样值X_t。

在操作S109，权重计算器45根据是否满足来自必要条件确定部件30的必要条件，基于提取的采样值X_t，计算当前数据Z_t的权重W_t。

具体地，在当前数据Z_t满足必要条件时，根据上述式13来计算权重W_t，在当前数据Z_t不满足必要条件时，根据上述式14来计算权重W_t。为了便于参考，下面再给出这两个式：

<式13>

$w_t^i{}_k=p\left(z_t^k\right|x_t^i,C,V)p\left(C\right|V)+p\left(z_t^k\right|x_t^i,C^c,V)p\left(C^c\right|V)$

<式14>

$w_t^i{}_k=p\left(z_t^k\right|x_t^i,C,V^c)p\left(C\right|V^c)+p\left(z_t^k\right|x_t^i,C^c,V^c)p\left(C^c\right|V^c)$

其中，Z_t是当前数据，X_t是采样值，C是当前数据不受外部磁场干扰影响的事件，C^C是当前数据受外部磁场干扰影响的事件，V是满足必要条件的事件，V^C是不满足必要条件的事件。

在操作S111，数据估计器40比较“i”和N，以确定使用提取的采样值X_t计算当前数据Z_t的权重W_t的操作是否被重复N次。

如果确定“i”不等于N，则在操作S113中，将“i”增加“1”，然后，重复上述操作S107和S109。

如果确定“i”等于N，则数据估计器40计算当前数据Z_t没有外部磁场干扰的权重W_t的概率分布(P＝{X_t ⁱ，W_t ⁱ}_i＝1...N))，并在操作S115中，基于计算的概率分布(P＝{X_t ⁱ，W_t ⁱ}_i＝1...N))估计机器人主体10的方向。这里，估计的机器人主体10的方向是概率分布(P＝{X_t ⁱ，W_t ⁱ}_i＝1...N))的平均值(∑Xt*W)。

通过上述操作，即使在存在由于障碍引起的外部磁场干扰的情况下，也能计算与机器人主体10的方向相关的数据。

这里应该注意的是，在本发明总体构思的实施例中，可以使用可作为移动机器人装置的一部分的合适的编程处理器、CPU和控制单元(未示出)，来实施参照图2中的必要条件确定部件30和数据估计器40讨论的各种功能。此外，在本发明总体构思的另一实施例中，数据存储介质(未示出)可设置有存储在其上的合适的软件或程序指令，所述数据存储介质可包括：例如，在机器人主体10中或与机器人主体10关联的存储器单元(未示出)、光学数据存储单元(未示出)(例如压缩盘(CD))或电磁数据存储单元(未示出)(例如，数据盒式磁带)。当由处理器或CPU(未示出)执行时，程序指令可配置处理器或CPU执行必要条件确定部件30和/或数据估计器40的功能(上面讨论的)。

此外，由于感测绝对方向的磁罗盘可应用于移动机器人，例如，用于室内的清洗机器人、服务机器人等，可以使用根据本发明总体构思的实施例的位置确定方法来解决由于移动机器人自身位置识别并自动运行而发生的问题。

如上所述，根据本发明总体构思，必要条件确定部件确定当前数据是否满足必要条件。必要条件确定部件不使用规则来确定传感器的有效性，从而避免规则的错误判断的可能性。然后，通过使用传感器的有效性和无效性的条件概率应用粒子滤波器来获得数据的估计值，基于当前数据是否满足必要条件的确定结果来计算所述条件概率。

条件概率可以具有非高斯概率分布。因此，当具有非高斯高率分布的数据被输入粒子滤波器时，粒子滤波器可获得最优估计值。

此外，如上所述，通过使用多个磁罗盘可找到移动机器人的准确的方向，例如，航向角(heading angle)。

尽管上面讨论了粒子滤波器使用权重(作为加权估计方式的一部分而被计算)来估计机器人主体的方向，但是应该理解的是，粒子滤波器可使用除了权重之外的因子来估计机器人主体的方向。

从上面的描述清楚可知，本发明总体构思提供的移动机器人装置及其控制方法即使在从传感器部件输出的数据受到外部磁场干扰的影响的情况下下，也能够非常准确地估计机器人主体的移动方向。

此外，本发明总体构思提供的移动机器人装置及其控制方法，即使在从传感器部件输出的数据会受到过度的外部磁场干扰的影响的情况下，外部磁场的干扰未知时，也能够通过使用粒子滤波器，准确地估计机器人主体的移动方向。

此外，本发明总体构思提供的移动机器人装置及其控制方法，通过使用满足必要条件的概率确定从传感器输出的数据是否受到外部磁场干扰的影响，然后通过使用根据是否满足必要条件的条件概率，能够准确地估计机器人主体的方向。

此外，本发明总体构思提供的移动机器人装置及其控制方法通过使用磁罗盘测量施加给移动机器人装置的绝对方向，能够克服与室内移动机器人(例如，清洗机器人、服务机器人等)的自身位置识别和自动运转相关的方向或位置定位相关的问题。

尽管已示出和描述了本发明总体构思的一些实施例，但是本领域的技术人员应该理解，在不脱离本发明总体构思的原理和精神的情况下，可以在这些实施例中作出改变，本发明总体构思的范围由权利要求及其等同物限定。

Claims (24)

1.一种使用从控制系统中的传感器部件输出的当前数据来估计当前状态变量的方法，所述方法包括：

确定从传感器部件输出的当前数据是否满足必要条件；

从前一采样值提取当前采样值，前一状态变量从所述前一采样值获得；

基于当前采样值计算当前数据的权重，其中，所述权重的值依赖于当前数据是否满足必要条件；

基于提取的当前采样值和计算的权重来估计当前状态变量。

2.如权利要求1所述的方法，还包括：

在当前数据输出不受干扰影响时，认为满足必要条件。

3.一种控制移动机器人装置的方法，所述移动机器人装置具有机器人主体和传感器部件，所述传感器部件输出与机器人主体相对于地磁场的当前方向相关的当前数据，所述方法包括：

确定从传感器部件输出的当前数据是否满足必要条件，其中，当从传感器部件输出的当前数据不受地磁场影响时，认为满足必要条件；

从自传感器部件获得的前一采样值提取当前采样值，其中，前一采样值与机器人主体的前一方向相关；

基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；

基于提取的当前采样值和计算的权重估计机器人主体的当前方向。

4.如权利要求3所述的控制方法，其中，估计机器人主体的当前方向的步骤包括：使用提取的当前采样值和计算的权重，并将提取当前采样值和计算权重的步骤重复预定次数来计算概率分布。

5.如权利要求4所述的控制方法，其中，估计机器人主体的当前方向的步骤还包括：

基于计算的概率分布的平均值来估计机器人主体的当前方向。

6.如权利要求3、4或5所述的控制方法，其中，基于当前采样值计算当前数据的权重的步骤包括：在当前数据满足必要条件时，根据下述式1来计算当前数据的权重，在当前数据不满足必要条件时，根据下述式2来计算当前数据的权重：

<式1>

p(Z_t|X_t，C，V)p(C|V)+p(Z_t|X_t，C^c，V)p(C^c|V)

<式2>

p(Z_t|X_t，C，V^c)p(C|V^c)+p(Z_t|X_t，C^c，V^c)p(C^c|V^c)

其中，Z_t是当前时间t时的当前数据，X_t是当前时间t时的机器人方向的估计值，C是当前数据不具有由于地磁场干扰引起的数据失真的事件，C^c是当前数据受到地磁场干扰的影响的事件，V是满足当传感器部件不具有由于地磁场干扰引起的数据失真时的必要条件的事件，V^c是不满足当传感器部件不具有由于地磁场干扰引起的数据失真时的必要条件的事件。

7.如权利要求6所述的控制方法，其中，传感器部件包括磁罗盘。

8.如权利要求6所述的控制方法，其中，估计机器人主体的当前方向的步骤还包括：

使用粒子滤波器来估计机器人主体的当前方向。

9.如权利要求3所述的控制方法，还包括基于估计的当前方向，将机器人主体定位在下一方向上。

10.一种包括传感器部件并使用从传感器部件输出的当前数据估计当前状态变量的控制系统，所述控制系统包括：

必要条件确定部件，结合到传感器部件上，被配置为设置将要满足的必要条件设置，并确定从传感器部件输出的当前数据是否满足必要条件；

数据估计器，结合到必要条件确定部件上，并被配置为：从前一采样值提取当前采样值，其中，前一状态变量从所述前一采样值获得；基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；基于提前的当前采样值和计算的权重来估计当前状态变量。

11.如权利要求10所述的控制系统，其中，在当前数据输出不受干扰影响时，认为满足必要条件。

12.一种移动机器人装置，包括：

机器人主体；

传感器部件，结合到机器人主体上，用于输出与机器人主体相对于干扰的当前方向相关的数据；

必要条件确定部件，结合到传感器部件上，被配置为设置将要满足的必要条件，并确定从传感器部件输出的当前数据是否满足必要条件；

数据估计器，结合到必要条件确定部件和机器人主体上，并被配置为：从自传感器获得的前一采样值提取当前采样值，其中，前一采样值与机器人主体的前一方向相关；基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；基于提取的当前采样值和计算的权重估计机器人主体的当前方向。

13.如权利要求12所述的移动机器人装置，其中，在当前数据输出不受干扰影响时，认为满足必要条件。

14.如权利要求12所述的移动机器人装置，其中，所述干扰包括地磁场。

15.如权利要求14所述的移动机器人装置，其中，传感器部件包括磁罗盘。

16.如权利要求12所述的移动机器人装置，其中，数据估计器被配置为使用提取的当前采样值和计算的权重，并将提取当前采样值和计算权重的操作重复预定次数来计算概率分布。

17.如权利要求16所述的移动机器人装置，其中，数据估计器被配置为基于计算的概率分布的平均值来估计机器人主体的当前方向。

18.如权利要求12所述的移动机器人装置，其中，数据估计器包括权重计算器，所述权重计算器被配置为：当必要条件确定部件确定当前数据满足必要条件时，根据下述式1基于当前采样值来计算当前数据的权重；当必要条件确定部件确定当前数据不满足必要条件时，根据下述式2基于当前采样值来计算当前数据的权重：

<式1>

p(Z_t|X_t，C，V)p(C|V)+p(Z_t|X_t，C^c，V)p(C^c|V)

<式2>

p(Z_t|X_t，C，V^c)p(C|V^c)+p(Z_t|X_t，C^c，V^c)p(C^c|V^c)

其中，Z_t是当前时间t时的当前数据，X_t是当前时间t时的机器人方向的估计值，C是当前数据不具有由于地磁场干扰引起的数据失真的事件，C^c是当前数据受到地磁场干扰的影响的事件，V是满足当传感器部件不具有由于地磁场干扰引起的数据失真时的必要条件的事件，V^c是不满足当传感器部件不具有地磁场干扰引起的数据失真时的必要条件的事件。

19.如权利要求18所述的移动机器人装置，其中，数据估计器包括粒子滤波器。

20.如权利要求12所述的移动机器人装置，其中，数据估计器被配置为基于估计的当前方向来为机器人主体提供信号，以将机器人主体定位于下一方向。

21.一种处理器，其中包含控制机器人装置的定向移动的方法，所述方法包括：

接收由机器人装置的传感器部件输出并且与机器人装置的主体相对于外部干扰的当前方向相关的当前数据；

确定从传感器部件输出的当前数据是否满足必要条件，其中，当从传感器部件输出的当前数据不受外部干扰的影响时，认为满足必要条件；

从自传感器部件获得的前一采样值提取当前采样值，其中，前一采样值与主体的前一方向相关；

基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；

基于提取的当前采样值和计算的权重来估计机器人装置的主体的当前方向。

22.如权利要求21所述的包含所述方法的处理器，其中，通过使用提取的当前采样值和计算的权重计算概率分布的平均值来执行估计所述主体的当前方向的步骤。

23.如权利要求21所述的包含所述方法的处理器，其中，所述方法还包括：

基于估计的当前方向为机器人装置的主体提供信号，以控制主体的定向移动。

24.一种含有程序指令的数据存储介质，当由机器人装置中的处理器执行所述程序指令时，所述程序指令使得处理器执行下述操作：

接收由机器人装置的传感器部件输出并且与机器人装置的主体相对于外部干扰的当前方向相关的当前数据；

确定从传感器部件输出的当前数据是否满足必要条件，其中，当从传感器部件输出的当前数据不受外部干扰的影响时，认为满足外部条件；

从自传感器部件获得的前一采样值提取当前采样值，其中，前一采样值与主体的前一方向相关；

基于当前采样值计算当前数据的权重，其中，权重的值依赖于当前数据是否满足必要条件；

基于提取的当前采样值和计算的权重来估计机器人装置的主体的当前方向。

Images