CN105004336A - 一种机器人的定位方法 - Google Patents

Abstract

本发明涉及一种机器人的定位方法，包括：机器人移动时通过光电图像传感器、位置敏感传感器、里程计和陀螺仪采集方位数据；光电图像传感器采集的方位数据输入至所有局部滤波器和主滤波器；位置敏感传感器采集的方位数据输入至第一局部滤波器；第一局部滤波器对输入的方位数据进行处理，将处理结果输入至主滤波器；里程计采集的方位数据输入至第二局部滤波器；第二局部滤波器对输入的方位数据进行处理，将处理结果输入至主滤波器；陀螺仪采集的方位数据输入至第三局部滤波器；第三局部滤波器对输入的方位数据进行处理，将处理结果输入至主滤波器；主滤波器对光电图像传感器输出的数据、所有局部滤波器输出的数据进行处理；获得机器人的定位信息。

Description

一种机器人的定位方法

技术领域

本发明涉及机器人技术领域，特别涉及一种机器人的定位方法。

背景技术

室内清洁机器人的定位导航技术是服务机器人研究领域的一个重点研究热点。在相关技术研究中，定位与导航技术是移动机器人实现智能化和完全自主移动的关键。对于导航来说，就是让机器人在没有外界人为干预的情况下，自主识别陌生环境，识别自身的相对位置，从而准确运动到达指定的目标位置，完成指定工作任务。对于扫地机器人来说，就是准确地按照制定的扫地路径、策略完成清扫任务。目前，扫地机器人常用的定位技术如下：

专利(1)一种扫地机器人智能导航系统，申请号：201310087158.2

它是采用红外技术设计的一种室内定位方案，在室内安放一个基站，基站上面安装了红外发射头和噪声波发射头，然后在扫地机器人上面安装红外接收头和噪声波接收头。机器人通过接受基站发射的红外和声波信号来计算距离从而达到定位的目的，要求是机器人运动的轨迹必须始终垂直基站。

本技术方案基于红外的定位技术由于红外信号容易受障碍物遮挡影响，并且距离远近影响其信号强度分布，使得其定位并不可靠。

专利(2)基于万向轮的扫地机器人测速、测距系统及方法，申请号：201410266593.6

该技术主要是设计了一种编码盘，通过在车轮上安装霍尔传感器和多极磁环来构成电磁感应系统，车轮旋转时可以使霍尔传感器产生电流，处理器通过电流的大小来判定车轮的转速和转数。然后也是通过累计转速来达到机器人定位的目的。

本技术方案基于里程计的定位技术虽然比较容易实现，但其存在累积误差的问题，长时间工作后会影响机器人的定位准确性。

专利(3)一种基于视觉的机器人室内定位导航方法。申请号：201010611473.7

它是一种基于视觉的机器人室内定位导航方法，根据二维码的思路，设计一种简单方便、易于识别、内含绝对位置坐标且具有一定纠错能力的人工路标，将路标设置于天花板上，由安装在机器人上且光轴与天花板垂直的摄像机进行拍摄，再通过图像的阈值分割、连通域提取、轮廓曲线匹配以及路标特征识别一系列步骤定位路标，解析路标中所包含的坐标信息，最终通过机器人的位置估计算法获得机器人当前的绝对位置和航向角。

本技术方案基于视觉的定位方式一方面可能需要改变原来室内环境的外貌，另一方面算法可靠性较低，抗干扰能力差，且对处理器的性能要求较高，成本较高。

专利(4)扫地机器人避障、定位系统及方法。申请号：201410266597.4

介绍了一种九段碰撞检测器，由于机器人周身的碰撞检测器较多，因此可以检测到多个角度的碰撞，对避障和定位有一定的帮助。然后距离计算方面，采用霍尔传感器对车轮转速进行编码，累加转速构成里程计。

本技术方案基于碰撞的避障方式显得非常生硬不够智能，并且碰撞检测器长期使用可能存在机械损坏的问题，影响可靠性。

此外，基于激光传感器的定位技术由于其精度高、数据可靠性强等优点使得其在导航领域很受关注，但是激光传感器体积较大不方便安装在室内小型机器人上，数据量大不方便处理，重点是其价格昂贵，目前尚未在家庭服务机器人应用中推广。GPS技术在导航领域中应用较广，但是其在室内没有信号，并不适用于室内机器人的定位。

发明内容

为解决现有技术的问题，本发明提出一种机器人的定位方法，该方法能够使得机器人移动时定位准确度高，且成本低。

为实现上述目的，本发明提供了一种机器人的定位方法，该方法包括：

所述机器人移动时通过光电图像传感器、位置敏感传感器、里程计和陀螺仪采集相应的方位数据；

所述光电图像传感器采集的方位数据均输入至第一局部滤波器、第二局部滤波器、第三局部滤波器和主滤波器；所述位置敏感传感器采集的方位数据输入至第一局部滤波器；所述第一局部滤波器根据主滤波器反馈的最新信息对光电图像传感器采集的方位数据和位置敏感传感器采集的方位数据进行处理，将处理结果输入至主滤波器；所述里程计采集的方位数据输入至第二局部滤波器；所述第二局部滤波器根据主滤波器反馈的最新定位信息对光电图像传感器采集的方位数据和里程计采集的方位数据进行处理，将处理结果输入至主滤波器；所述陀螺仪采集的方位数据输入至第三局部滤波器；所述第三局部滤波器根据主滤波器反馈的最新信息对光电图像传感器采集的方位数据和陀螺仪采集的方位数据进行处理，将处理结果输入至主滤波器；

所述主滤波器对光电图像传感器输出的数据、第一局部滤波器输出的数据、第二局部滤波器输出的数据和第三局部滤波器输出的数据进行处理，获得机器人的定位信息；同时，向第一局部滤波器、第二局部滤波器和第三局部滤波器反馈信息。

优选地，所述机器人的定位信息为机器人在经度方向上的位置信息e、机器人在经度方向上的速度信息v_e、机器人在经度方向上的加速度信息a_e、机器人在纬度方向上的位置信息n、机器人在纬度方向上的速度信息v_n、机器人在纬度方向上的加速度信息a_n、机器人的姿态信息θ和/或机器人的转速信息ω。

优选地，还包括：

对所述位置敏感传感器采集到的方位信息运用三角形相似原理获得机器人正前方与障碍物之间的距离，并同时获取机器人的移动速度；

利用机器人正前方与障碍物之间的距离、机器人的移动速度获取避障策略。

优选地，还包括：

所述机器人利用机器人的定位信息和避障策略移动至目标位置。

优选地，所述光电图像传感器设置于机器人机身下方。

优选地，所述机器人的机身上设置两个位置敏感传感器。

上述技术方案具有如下有益效果：本技术方案采用光电图像传感器，成本低，数据可靠，对处理器性能要求低。进一步地，本技术方案对光电图像传感器、位置敏感传感器、里程计和陀螺仪这四种传感器的信息进行融合，使得定位误差不积累，定位精度较高；且无需在室内布置任何路标，对机器人的运动路径没有限制。

另外，本技术方案采用位置敏感传感器，能同时实现避障和返回充电站的功能。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提出的一种机器人的定位方法流程图；

图2为光电图像传感器位置俯视图；

图3为机器人的上位机软件获取各传感器采集的数据流程图；

图4为本发明的联合卡尔曼滤波算法结构示意图；

图5为本发明的位置敏感传感器的测距示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本技术方案的工作原理为：大多数机器人都面临着各种各样的大小问题，比如：价格、定位误差等问题；另外，室内的运动物体影响机器人原有算法的发挥；还有一些机器人需要部署许多传感器和附件在房间，这样改变了房间原貌，而且也不是我们希望的。考虑到这些缺陷，我们提出了更好的方法。本技术方案采用成本较低的传感器，融合各传感器的数据，克服各自的不足，使定位更加准确。还有一个是它的能源效率，由于高质量的定位导航，他不会去同一个地方来回重复移动，使得它长时间工作不需要充电。

本技术方案采用光电图像传感器作为主要的运动参数测量单元，将其与里程计、陀螺仪、位置敏感传感器的数据进行融合，增加其可靠性。由于光电图像传感器数据不受机器人漂移影响、不容易产生累积误差，并且其安装在机器人下方，不受运动物体的干扰，能最大限度地提供准确的运动数据去进行定位运算。为了实现机器人的智能避障，本技术方案基于位置敏感传感器的距离测量系统，能让机器人在较大的范围内感知障碍物，做出避障策略。

基于上述工作原理，本发明提出一种机器人的定位方法。如图1所示。该方法包括：

步骤101)：所述机器人移动时通过光电图像传感器、位置敏感传感器、里程计和陀螺仪采集相应的方位数据；

步骤102)：所述光电图像传感器采集的方位数据均输入至第一局部滤波器、第二局部滤波器、第三局部滤波器和主滤波器；所述位置敏感传感器采集的方位数据输入至第一局部滤波器；所述第一局部滤波器根据主滤波器反馈的最新信息对光电图像传感器采集的方位数据和位置敏感传感器采集的方位数据进行处理，将处理结果输入至主滤波器；所述里程计采集的方位数据输入至第二局部滤波器；所述第二局部滤波器根据主滤波器反馈的最新定位信息对光电图像传感器采集的方位数据和里程计采集的方位数据进行处理，将处理结果输入至主滤波器；所述陀螺仪采集的方位数据输入至第三局部滤波器；所述第三局部滤波器根据主滤波器反馈的最新信息对光电图像传感器采集的方位数据和陀螺仪采集的方位数据进行处理，将处理结果输入至主滤波器；

步骤103)：所述主滤波器对光电图像传感器输出的数据、第一局部滤波器输出的数据、第二局部滤波器输出的数据和第三局部滤波器输出的数据进行处理，获得机器人的定位信息；同时，向第一局部滤波器、第二局部滤波器和第三局部滤波器反馈信息。

如图2所示，为光电图像传感器位置俯视图。采用光电图像传感器作为主要的机器人运动参数感知单元。把光电图像传感器1安装在机器人机身2的底部，并且位于机器人轮子3之间。使其靠近地面。将传感器接受到的信息通过串口传输给机器人的上位机软件，上位机软件提取机器人运动的位移和方向数据，然后应用于机器人的室内定位。光电图像传感器通过比较两幅图像的差别来得到机器人移动的距离和方向。

如图3所示，为机器人的上位机软件获取各传感器采集的数据流程图。传感器检测到的位移信息放在对应的X、Y寄存器里，每移动一段距离读取一X、Y寄存器，防止寄存器溢出。内部运动状态寄存器标记传感器是否产生位移，如果没有位移，则一直循环查询运动状态寄存器的值；如果产生了位移，则读取X、Y寄存器的值，分别累加X、Y方向上的值并保存。然后对X、Y寄存器清零，继续读取运动状态寄存器，如此循环。

本技术方案使用了光电图像传感器、位置敏感传感器、里程计、陀螺仪等多种传感器，各种传感器的输出数据的形式不同，必须将这些数据转换成统一的标准，利用多传感器的数据进行目标的状态估计，寻找到与观测数据最佳拟合的状态向量。本技术方案采用联合卡尔曼滤波器的结构对各种传感器数据进行融合。

局部滤波器根据状态方程和测量方程进行滤波,并将每步的滤波结果传递给主滤波器，主滤波器完成信息的最优综合，形成全局系统的综合信息P_g。每个滤波阶段完成后,由主滤波器将合成的全局估计和P_g。同时，主滤波器按照信息分配原则形成的信息分配量，反馈给各个局部滤波器。

记局部滤波器i的状态估计矢量为系统协方差阵为Q_i，状态矢量协方差阵为P_i,其中i＝1……N。主滤波器的状态估计矢量、系统协方差阵和状态矢量协方差阵相应为:P_g、Q_g。联合卡尔曼滤波器的计算过程如下式(1)～式(11)：

a.给定初始值：

假设起始时刻全局状态的初始值为X₀,系统协方差阵为Q₀，状态矢量协方差阵为P₀将这一信息通过信息分配因子按规则分配到各局部滤波器和全局滤波器。

$Q^{-1}=Q_1^{-1}+Q_2^{-1}+......+Q_N^{-1}+Q_m^{-1}---\left(1\right)$

$Q_i^{-1}=\begin{array}{cc}{\mathrm{\β}}_i& Q^{-1}\end{array}---\left(2\right)$

$P^{-1}=P_1^{-1}+P_2^{-1}+......+P_N^{-1}+P_m^{-1}---\left(3\right)$

$P_i^{-1}=\begin{array}{cc}{\mathrm{\β}}_i& P^{-1}\end{array}---\left(4\right)$

$P^{-1}\hat{X}=P_1^{-1}{\hat{X}}_i+P_1^{-2}{\hat{X}}_2+......+P_N^{-1}{\hat{X}}_N+P_m^{-1}{\hat{X}}_m---\left(5\right)$

其中,β_i满足信息守恒原则β₁+β₂+……+β_N+β_m＝1，0≤β_i≤1。

b.信息的时间更新情况为：

X_i,(k|k-1)＝Φ_k|k-1X_i,(k-1)          (6)

$P_{i,\left(k\right|k-1)}={\mathrm{\Φ}}_{k|k-1}{P}_{i,(k-1|k-1)}{\mathrm{\Φ}}_{k|k-1}^T+G_{k|k-1}{Q}_{i,k-1}{G}_{k|k-1}^T---\left(7\right)$

c.信息的量测更新时，第i个局部滤波器的量测更新为：

$P_{i,\left(k\right|k-1)}={\hat{X}}_{i,\left(k\right|k)}=P_{i,\left(k\right|k-1)}{\hat{X}}_{i,\left(k\right|k-1)}+H_{i,\left(k\right)}^T{R}_{i,\left(k\right)}^{-1}{Z}_{i,\left(k\right)}---\left(8\right)$

$P_{i,\left(k\right|k)}^{-1}=P_{i,\left(k\right|k-1)}^{-1}+H_{i,\left(k\right)}^T{R}_{i,\left(k\right)}^{-1}{H}_{i,\left(k\right)}---\left(9\right)$

d.最优信息融合符合下式：

${\hat{X}}_{g,k}=P_{g,k}\underset{i=1}{\overset{N}{\Σ}}{P}_{i,k}^{-1}{\hat{X}}_{i,k}---\left(10\right)$

$P_{g,k}=(\underset{i=1}{\overset{N}{\Σ}}{P}_{i,k}^{-1}{)}^{-1}---\left(11\right)$

联合卡尔曼滤波器是一种理想的方法。联合卡尔曼滤波器设计的基本思想是：先分散处理，再全局融合，即在诸多非相似子系统中选择一个信息全面、输出速率高、可靠性有保证的子系统作为公共参考系统，与其他子系统两两结合，形成若干子滤波器。

对于本技术方案来说，公共参考系统为光电图像传感器。为了兼顾系统的精确度和容错性，本技术方案采用融合复位结构。此种结构中，各个子系统分别经过卡尔曼滤波后送入主滤波器，主滤波器只完成对局部滤波信息的综合，而不进行滤波处理，此时主滤波器状态方程无信息分配，所以主滤波器的估计值就取全局估计。即：

${\hat{X}}_m={\hat{X}}_g=P_g\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i^{-1}{\hat{X}}_i$

如图4所示，为本发明的联合卡尔曼滤波算法结构示意图。设联合卡尔曼滤波器的状态向量为X_g，方差阵P_g，局部滤波器的状态向量为X_i,其方差阵为P_i,主滤波器的状态向量为X_m，方差阵为P_m，测量信息用量测噪声方差阵的逆R^-1来表示,系统信息用系统噪声方差阵的逆Q^-1来表示,滤波估计误差信息用估计误差方差阵的逆P^-1来表示。本技术方案的融合算法包括4个滤波器，即主滤波器、局部滤波器1，局部滤波器2，局部滤波器3。其中，局部滤波器1负责位置敏感传感器采集的信息与光电图像传感器采集的信息融合；局部滤波器2负责光电图像传感器采集的信息与里程计采集的信息融合；局部滤波器3负责光电图像传感器采集的信息和陀螺仪采集的信息融合。主滤波器一方面对各局部滤波器进行信息综合与分配,另一方面将系统状态误差的估计值反馈给各个局部滤波器,以校正其累积误差。

在图4中，采用的是融合复位结构，于是有：

β₁＝β₂＝β₃＝1/3

那么，在联合滤波结构中，系统整体信息按以下的规则在各滤波器间分配：

各滤波模型的过程噪声方差也按同样规则分配：

X_i＝X_m＝X_g

$P_i^{-1}=P_g^{-1}{\mathrm{\β}}_i,(i=1,2,3)$

$Q^{-1}=Q_m^{-1}+\underset{i=1}{\overset{n}{\Σ}}{Q}_i=Q^{-1}{\mathrm{\β}}_m+Q^{-1}\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\β}}_i=Q^{-1}\[{\mathrm{\β}}_m+\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\β}}_i\]$

$Q_i^{-1}=Q^{-1}{\mathrm{\β}}_i$

式中，β_i表示第i个滤波器的信息分配系数；

当各个局部滤波器和主滤波器的解是统计独立时，它们可以按照下面的算法进行最优合成：

$\hat{X}=P_g\[P_m^{-1}{\hat{X}}_m+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i^{-1}{\hat{X}}_i\]$

$P_g=\[P_m^{-1}+\underset{i=1}{\overset{n}{\Σ}}{P}_i^{-1}{\]}^{-1}$

对于本实施来说，所述机器人的定位信息为机器人在经度方向上的位置信息e、机器人在经度方向上的速度信息v_e、机器人在经度方向上的加速度信息a_e、机器人在纬度方向上的位置信息n、机器人在纬度方向上的速度信息v_n、机器人在纬度方向上的加速度信息a_n、机器人的姿态信息θ和/或机器人的转速信息ω。

优选地，还包括：

对所述位置敏感传感器采集到的方位信息运用三角形相似原理获得机器人正前方与障碍物之间的距离，并同时获取机器人的移动速度；

利用机器人正前方与障碍物之间的距离、机器人的移动速度获取避障策略。

如图5所示，为本发明的位置敏感传感器的测距示意图。运用三角形原理测量机器人前部与障碍物之间的距离。将两个等焦距的会聚透镜13安装于机器人前方，两个位置敏感传感器14分别位于两个会聚透镜13的焦点上，两透镜与光源12在同一直线上。x₁、x₂为障碍物11反射光经过会聚透镜后落在位置敏感传感器14的光敏区域位置；l为两个位置敏感传感器14之间的距离；y为障碍物到机器人前部的距离。

根据图中相似三角形关系可得：

$\frac{x_1}{f}=\frac{l_1}{y}---\left(1\right)$

$\frac{x_2}{f}=\frac{l_2}{y}---\left(2\right)$

式(1)+(2)得

$\frac{x_1+x_2}{f}=\frac{l_1+l_2}{y}$

因为l₁+l₂＝l，所以

又可得到相对速度：

$v=\frac{y_n-y_{n-1}}{t_n-t_{n-1}}=\frac{dy}{dt}$

上式中，y_n为n时刻本车与障碍物间距离，y_n-1为n-1时刻本车与障碍物间距离，t_n-t_n-1为两次测距的时间差。

对于本实施例来说，机器人利用机器人的定位信息和避障策略移动至目标位置。本技术方案实现机器人在室内的准确自主导航，高效的完成清扫任务。一般来说,现在家用清洁机器人还没有能力不重复的移动到屋内空间的各个角落，很有可能会一遍又一遍地到相同区域来回移动清扫。本技术方案能够让机器人准确定位，使得机器人能够避免重复路径,从而节约能源、节省清扫时间。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种机器人的定位方法，其特征在于，该方法包括：

所述机器人移动时通过光电图像传感器、位置敏感传感器、里程计和陀螺仪采集相应的方位数据；

所述光电图像传感器采集的方位数据均输入至第一局部滤波器、第二局部滤波器、第三局部滤波器和主滤波器；所述位置敏感传感器采集的方位数据输入至第一局部滤波器；所述第一局部滤波器根据主滤波器反馈的最新信息对光电图像传感器采集的方位数据和位置敏感传感器采集的方位数据进行处理，将处理结果输入至主滤波器；所述里程计采集的方位数据输入至第二局部滤波器；所述第二局部滤波器根据主滤波器反馈的最新定位信息对光电图像传感器采集的方位数据和里程计采集的方位数据进行处理，将处理结果输入至主滤波器；所述陀螺仪采集的方位数据输入至第三局部滤波器；所述第三局部滤波器根据主滤波器反馈的最新信息对光电图像传感器采集的方位数据和陀螺仪采集的方位数据进行处理，将处理结果输入至主滤波器；

所述主滤波器对光电图像传感器输出的数据、第一局部滤波器输出的数据、第二局部滤波器输出的数据和第三局部滤波器输出的数据进行融合，获得机器人的定位信息；同时，向第一局部滤波器、第二局部滤波器和第三局部滤波器反馈信息。

2.如权利要求1所述的方法，其特征在于，所述机器人的定位信息为机器人在经度方向上的位置信息e、机器人在经度方向上的速度信息v_e、机器人在经度方向上的加速度信息a_e、机器人在纬度方向上的位置信息n、机器人在纬度方向上的速度信息v_n、机器人在纬度方向上的加速度信息a_n、机器人的姿态信息θ和/或机器人的转速信息ω。

3.如权利要求1或2所述的方法，其特征在于，还包括：

对所述位置敏感传感器采集到的方位信息运用三角形相似原理获得机器人正前方与障碍物之间的距离，并同时获取机器人的移动速度；

利用机器人正前方与障碍物之间的距离、机器人的移动速度获取避障策略。

4.如权利要求3所述的方法，其特征在于，还包括：

所述机器人利用机器人的定位信息和避障策略移动至目标位置。

5.如权利要求1或2所述的方法，其特征在于，所述光电图像传感器设置于机器人机身下方。

6.如权利要求1或2所述的方法，其特征在于，所述机器人的机身上设置两个位置敏感传感器。