CN102138768B - 清洁机器人的定位方法和设备 - Google Patents

Abstract

提供一种清洁机器人的定位方法和设备，其方法包括：根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；从电子罗盘获取当前时刻的绝对方向和磁场强度；若所述磁场强度在预设的强度范围内，将所述绝对方向和所述相对行走距离作为定位数据；否则，将所述相对角度值和所述相对行走距离作为定位数据。上述清洁机器人的定位方法采集电子罗盘的磁场强度作为电子罗盘当前时刻的绝对方向的置信度，可以在一定程度上避免编码器定位造成的累积误差，又可以防止外界磁场干扰对机器人方位数据进行影响，在较低的硬件成本下，大幅提高了定位精度。

Description

清洁机器人的定位方法和设备

【技术领域】

本发明涉及定位方法和设备，尤其涉及清洁机器人的定位方法和设备。

【背景技术】

清洁机器人是具有智能扫地功能的自主移动机器人，清洁机器人的定位问题就是自主移动机器人的室内定位问题，即对机器人在室内环境中的位置、方向和环境信息的精确检测。

目前的清洁机器人由于成本限制，大部分只采用了驱动轮轴上安装的编码器来定位，但是由于打滑、两驱动轮和地面接触点之间距离的不确定等原因，使得编码器的测算过程存在累积误差，机器人无法进行长时间的可靠定位，进而导致机器人在工作过程中出现方位迷失、工作区域漏扫等问题或采用无规则的路径清扫，通过延长工作时间来增大覆盖率，降低了清扫效率。如图1所示的室内平面图，左上角为柱子，如果采用上述用编码器定位机器人，其实验结果如图2所示，由图2可知，编码器测量的数据缺点就是角度误差累积，如果测量中间一旦有角度出错，之后的角度位姿就可能是错误数据，由分析可知，机器人可能是在图1所示的柱子附近由于多次旋转碰撞迷失了方向，从而造成之后方位坐标的错乱。

为了提高定位精度，韩国三星公司研制一种采用陀螺传感器补偿定位的清洁机器人(专利号200510062606.9)，如果清洁机器人行进得大于补偿基准时则变化到补偿模式，并且通过使用机器人上部摄像机补偿陀螺传感器的输出量，以补偿陀螺传感器的角度误差来进行定位。该技术的定位误差小，但该清洁机器人的定位成本却大幅提高了。

【发明内容】

提供一种兼顾成本和精度的清洁机器人的定位方法和设备。

技术方案如下：

一种清洁机器人的定位方法，包括：根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；从电子罗盘获取当前时刻的绝对方向和磁场强度；若所述磁场强度在预设的强度范围内，将所述绝对方向和所述相对行走距离作为定位数据；否则，将所述相对角度值和所述相对行走距离作为定位数据。

一种清洁机器人的定位设备，包括：编码器，与该编码器连接的相对数据计算单元，根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；电子罗盘，输出当前时刻的绝对方向和磁场强度；与该电子罗盘和所述相对数据计算单元连接的比较输出单元，当所述磁场强度在预设的强度范围内，将所述绝对方向和所述相对行走距离作为定位数据，否则，将所述相对角度值和所述相对行走距离作为定位数据。

上述清洁机器人的定位方法和设备，采集电子罗盘的磁场强度作为电子罗盘当前时刻的绝对方向的置信度，可以在一定程度上避免编码器定位造成的累积误差，又可以防止外界磁场干扰对机器人方位数据进行影响，在较低的硬件成本下，大幅提高了定位精度。

【附图说明】

图1是室内平面图；

图2是目前的定位方法的实验轨迹图；

图3是清洁机器人的定位方法的流程框图；

图4是清洁机器人的定位方法的实验轨迹图；

图5是清洁机器人的定位方法中卡尔曼滤波的流程框图；

图6是清洁机器人的定位设备的结构框图。

【具体实施方式】

以下结合具体实施方式和附图做出详细描述。

本实施方式的清洁机器人的定位方法，见图3，包括以下步骤：

100、根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；

200、从电子罗盘获取当前时刻的绝对方向和磁场强度；

为了提高电子罗盘的精度，本步骤中还包括电子罗盘的补偿步骤：当所述磁场强度与评估基数之间的差值超过预设的补偿阈值，所述电子罗盘进行补偿，并将补偿后首次采样的磁场强度设定为下次补偿的评估基数。该补偿步骤能在磁场强度发生骤变的情况下及时进行补偿，骤变的幅度由补偿阈值来控制。作为最优，与补偿阈值比较的是磁场强度与评估基数之间的差值的绝对值。

300、判断所述磁场强度是否在预设的强度范围内，是则执行步骤400；否则执行步骤500；

400、将所述绝对方向和所述相对行走距离作为定位数据；

500、将所述相对角度值和所述相对行走距离作为定位数据。

因为电子罗盘是测量地磁方向的传感器，其特点是没有误差累积，但是却易受外界磁场干扰，尤其是在遇到强磁干扰的地方，电子罗盘就会迷失方向，例如图1中左上角的梁柱内的钢筋较多，就存在影响罗盘读数的可能；

本定位方法中若当前磁场强度超出预设的范围，则采用编码器获取的数据，若当前磁场强度落入预设的范围，则说明目前处于在外界干扰磁场较小、磁场比较均匀的区域，采用电子罗盘的获取的绝对方向，算法即时纠正了由编码器产生的角位移误差；该定位方法基于编码器和电子罗盘的特点取利舍弊，由图4可以看出，本定位方法的轨迹更接近于真实轨迹，使清洁机器人达到了较理想的定位精度。

在上述实施方式的基础上增加步骤600作为另一种实施方式：

600、通过概率方法利用感知模型和所述定位数据估计当前位姿的预测结果以及计算当前位姿的最优化结果。

概率方法进行定位，即在机器人所有的可能位置上保持概率分布，随着机器人的运动，感知到新的环境数据，概率分布被更新，从而减小机器人位置的不确定性。具体的概率定位方法包括卡尔曼滤波、马尔可夫定位、多假设跟踪和蒙特卡罗定位等。

以下以卡尔曼滤波为例介绍根据所述定位数据获取当前位姿的最优化结果的过程，见图5：

首先，引入一可用一个线性随机微分方程来描述的离散控制过程系统，因为卡尔曼滤波的应用前提为移动机器人的运动模型和感知模型假设为带高斯噪声的线性模型，系统状态服从高斯分布。

610、利用前一时刻位姿的最优化结果和系统模型估计当前位姿的预测结果；

假设当前时刻是k，k时刻的系统状态是X(k)，

X(k)＝AX(k-1)+BU(k)+W(k)                     (1)

式(1)中，

A和B是系统参数，对于多模型系统，它们为矩阵；

X(k-1)是k-1时刻的系统状态；

U(k)是k时刻对系统的控制量；

W(k)表示系统过程(即运动模型)，W(k)被假设成高斯白噪声，其协方差是Q。

k时刻的系统测量值(即所述定位数据)Z(k)，

Z(k)＝HX(k)+V(k)                            (2)

式(2)中，

H是测量系统的参数，对于多测量系统，H为矩阵；

V(k)表示测量噪声(即感知模型)，V(k)被假设成高斯白噪声，其协方差是R。

k时刻位姿的预测结果X(k|k-1)的公式：

X(k|k-1)＝AX(k-1|k-1)+BU(k)                    (3)

式(3)中，

X(k-1|k-1)是k-1时刻位姿的最优化结果；

U(k)是k时刻对系统的控制量；

A和B是系统参数，对于多模型系统，它们为矩阵。

620、利用该当前位姿的预测结果的协方差和所述感知模型的协方差，计算当前卡尔曼增益；本步骤又包括：

621、更新对应于k时刻位姿的预测结果X(k|k-1)的协方差P(k|k-1)。

P(k|k-1)＝AP(k-1|k-1)A′+Q                      (4)

式(4)中，

P(k|k-1)是X(k|k-1)对应的协方差，

P(k-1|k-1)是X(k-1|k-1)对应的协方差

A′表示A的转置矩阵，

Q是系统过程W(k)的协方差，当轮子打滑或撞击堵转时，W(k)中的变量涉及碰撞传感器，万向轮编码器的检测脉冲和驱动轮检测脉冲。

622、利用k时刻位姿的预测结果X(k|k-1)的协方差P(k|k-1)、测量系统的测量噪声V(k)的协方差R，计算k时刻的卡尔曼增益Kg：

Kg(k)＝P(k|k-1)H′/(HP(k|k-1)H′+R)                (5)

式(5)中，

Kg(k)是k时刻卡尔曼增益；

H’是测量系统的参数H的转置矩阵，R、H见公式1。

由式(5)可知，当测量噪声的协方差R越小，k时刻的卡尔曼增益Kg(k)越大；当k时刻位姿的预测结果的协方差P(k|k-1)越小，k时刻的卡尔曼增益Kg(k)越大。由此可以看出，k时刻位姿的预测结果增益Kg(k)表示在更新阶段测量信息的权值。如果在预估阶段误差越小，则测量值Z(k)的权值越小，而Z(k)的预测值HX(k|k-1)的权值越大。相反，如果测量噪声越小，则测量变量Z(k)的权值越大，而Z(k)的预测HX(k|k-1)的权值越小。

630、利用该当前卡尔曼增益Kg(k)、所述当前位姿的预测结果X(k|k-1)、当前时刻的所述定位数据Z(k)(即所述当前定位数据)，计算得到所述当前位姿的最优化结果X(k|k)。

X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))                    (6)

式(6)中，

H是测量系统的参数，对于多测量系统，H为矩阵。

为了卡尔曼滤波器不断的运行下去直到系统过程结束，还要更新k状态下X(k|k)的协方差：

P(k|k)＝(I-Kg(k)H)P(k|k-1)                                (7)

其中I为1的矩阵，对于单模型单测量，I＝1。当系统进入k+1状态时，P(k|k)就是式子(4)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

对于满足前面提到的条件，即，线性随机微分系统、过程和测量都是高斯白噪声，卡尔曼滤波器是最优的信息处理器。

本实施方式的清洁机器人的定位设备，如图6所示，包括：编码器，相对数据计算单元，电子罗盘和比较输出单元，

电子罗盘用于输出当前时刻的绝对方向和磁场强度；

相对数据计算单元与编码器连接，用于根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；

比较输出单元设置于电子罗盘和相对数据计算单元之间，用于当所述磁场强度在预设的强度范围内，利用所述当前时刻的绝对方向获得所述当前时刻较之前一时刻的相对角度值，并发送至所述相对数据计算单元。

通过比较输出单元在当前磁场强度符合预设的范围时，利用当前时刻的绝对方向获得所述当前时刻较之前一时刻的相对角度值，即时纠正了由编码器产生的角位移误差造成定位数据的误差积累，提高了定位精度。

在上述实施方式的基础上，增加了位姿确定模块，通过概率方法利用感知模型和所述定位数据估计当前位姿的预测结果以及计算当前位姿的最优化结果；位姿确定模块包括依次连接的预测单元、权值单元和优化单元，

预测单元用于利用前一时刻位姿的最优化结果和系统模型估计当前位姿的预测结果。

权值单元用于利用该当前位姿的预测结果的协方差和感知模型的协方差，计算当前卡尔曼增益。

优化单元用于利用该当前卡尔曼增益、所述当前位姿的预测结果、当前时刻的所述定位数据，计算所述当前位姿的最优化结果。

对于满足以下两个条件：线性随机微分系统、过程和测量都是高斯白噪声的系统，卡尔曼滤波器是最优的信息处理器。

以上仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (9)

1.一种清洁机器人的定位方法，包括：

根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；

其特征在于，

从电子罗盘获取当前时刻的绝对方向和磁场强度；

若所述磁场强度在预设的强度范围内，将所述绝对方向和所述相对行走距离作为定位数据；否则，将所述相对角度值和所述相对行走距离作为定位数据；

当所述磁场强度与评估基数之间的差值的绝对值超过预设的补偿阈值，所述电子罗盘进行补偿，并将补偿后首次采样的磁场强度设定为下次补偿的评估基数；

其中，与补偿阈值比较的是磁场强度与评估基数之间的差值的绝对值。

2.根据权利要求1所述的清洁机器人的定位方法，其特征在于，还包括通过概率方法利用感知模型和所述定位数据估计当前位姿的预测结果以及计算当前位姿的最优化结果。

3.根据权利要求2所述的清洁机器人的定位方法，其特征在于，所述当前位姿的最优化结果采用卡尔曼滤波按照以下步骤获得：

利用前一时刻位姿的最优化结果和系统模型估计当前位姿的预测结果；

利用该当前位姿的预测结果的协方差和所述感知模型的协方差，计算当前卡尔曼增益；

利用该当前卡尔曼增益、所述当前位姿的预测结果、当前时刻的所述定位数据，计算得到所述当前位姿的最优化结果。

4.根据权利要求3所述的清洁机器人的定位方法，其特征在于，所述当前位姿的最优化结果按照下面公式得到：

X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))

式中，

X(k|k)是所述当前位姿的最优化结果；

X(k|k-1)是所述当前位姿的预测结果；

Kg(k)是所述当前卡尔曼增益；

Z(k)是当前时刻的所述定位数据；

H是测量系统的参数，对于多测量系统，H为矩阵。

5.根据权利要求4所述的清洁机器人的定位方法，其特征在于，所述当前位姿的预测结果X(k|k-1)是按照下面公式得到：

X(k|k-1)＝A X(k-1|k-1)+B U(k)

式中，

A和B是系统参数，对于多模型系统，它们为矩阵；

X(k-1/k-1)是前一时刻最优化结果；

U(k)是当前时刻对系统的控制量。

6.根据权利要求5所述的清洁机器人的定位方法，其特征在于，所述当前卡尔曼增益Kg(k)按照下面公式得到：

Kg(k)＝P(k|k-1)H'/(H P(k|k-1)H'+R)

式中，

P(k/k-1)是所述当前位姿的预测结果X(k|k-1)的协方差；

H^，是H的转置矩阵，

R是所述感知模型的协方差。

7.一种清洁机器人的定位设备，包括：

编码器，

与该编码器连接的相对数据计算单元，根据从编码器获取的数据计算出当前时刻较之前一时刻的相对角度值和相对行走距离；

其特征在于，

电子罗盘，输出当前时刻的绝对方向和磁场强度；

与该电子罗盘和所述相对数据计算单元连接的比较输出单元，当所述磁场强度在预设的强度范围内，将所述绝对方向和所述相对行走距离作为定位数据，否则，将所述相对角度值和所述相对行走距离作为定位数据；

所述电子罗盘还在所述磁场强度与评估基数之间的差值的绝对值超过预设的补偿阈值时进行补偿，并将补偿后首次采样的磁场强度设定为下次补偿的评估基数；

其中，与补偿阈值比较的是磁场强度与评估基数之间的差值的绝对值。

8.根据权利要求7所述的清洁机器人的定位设备，其特征在于，还包括与所述比较输出单元连接的位姿确定模块，通过概率方法利用感知模型和所述定位数据估计当前位姿的预测结果以及计算当前位姿的最优化结果。

9.根据权利要求8所述的清洁机器人的定位设备，其特征在于，所述位姿确定模块包括：

预测单元，利用前一时刻位姿的最优化结果和系统模型估计当前位姿的预测结果；

权值单元，利用该当前位姿的预测结果的协方差和感知模型的协方差，计算当前卡尔曼增益；

优化单元，利用该当前卡尔曼增益、所述当前位姿的预测结果、当前时刻的所述定位数据，计算所述当前位姿的最优化结果。