CN102359783A - 基于视觉的移动机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于视觉的移动机器人定位方法，包括1)在机器人工作环境中的任意三个位置上分别设置三个地标；2)对测量出的每个方向角数据统计其方差σ²，得到测量角度呈现的分布特性；3)统计分析得到测量角度的后验概率分布p(Z_ij|α_ij)服从以上分布；4)由两个地标L_i和L_j的坐标，以及随机变量Z_ij生成圆弧簇；5)以半径R_ij表示圆弧簇的概率值，获得概率图M₁和M₂；6)将M₁和M₂进行融合，得到一个概率分布图M_t；7)寻找概率分布图M_t中具有最大权值的坐标位置作为机器人的位置。本发明对基于三角定位系统的移动机器人的自定位精度具有一定提高。在定位精确度上本发明方法比原始方法平均提高21.9％。当机器人处于“三角定位困难区域”时，机器人的定位精度有显著提高。

Description

基于视觉的移动机器人定位方法

技术领域

本发明属于移动机器人的自定位技术，具体涉及一种基于视觉的移动机器人定位方法。

背景技术

移动机器人自定位是指移动机器人通过对内部状态的检测或对外部环境的感知估算自身位置和姿态的过程。机器人自定位系统的研究对机器人的研究有着深远的意义，定位系统提供给机器人定位结果精度的高低直接影响到机器人导航及运动过程控制的可靠性，也会影响到人机交互的安全性问题。因此，机器人自定位系统亦成为国际机器人领域的研究热点。

由于，以“三角定位”、“三边定位”等为代表的几何计算方法能够为机器人提供相对精确的二维坐标形式的定位结果，这些几何定位方法受到了各国科学研究者的普遍关注，并被广泛应用于航海、航天、工厂环境中移动平台以及机器人足球比赛的自定位系统中。

通常使用的“三角定位”方法，其原理如下：

1.圆的求解。三个不在同一条直线上的点确定一个圆(如图1所示)。该圆可以由平面中两个地标(如P1、P2)的坐标和另外一个点(Robot，机器人的位置，图中用方块来表示)与此两地标的夹角角度计算得来。

几何公式推导结果如下：

$l=L_1{L}_2=\sqrt{{(x_2-x_1)}^2+{(y_2-y_1)}^2}$

$[x_Q,y_Q]=[\frac{x_1+x_2}{2},\frac{y_1+y_2}{2}]$

$R=\frac{L_{1}Q}{\sin {\widetilde{\mathrm{\α}}}_{12}}=\frac{0.5l}{\sin {\widetilde{\mathrm{\α}}}_{12}}$

$d=\mathrm{OQ}=\sqrt{R^2-0.25l^2}$

$\mathrm{\β}=\arctan \left|\frac{y_2-y_1}{x_2-x_1}\right|$

而圆心坐标则可以根据三点位置拓扑关系的不同而分八种情况予以确定，如图2所示。

2.已知二维平面中任意三个地标的坐标，以及平面中机器人与这三个点的夹角的度数，可以根据1中的几何计算方法得出三个圆的方程如下：

(x-X₀)²+(y-Y₀)²＝R₀ ²

(x-X₁)²+(y-Y₁)²＝R₁ ²

(x-X₂)²+(y-Y₂)²＝R₂ ²

此三个圆相交于共同的一点，求出此交点坐标，该坐标就是机器人的位置坐标。(参见图3)，三个不同的两两交叉的圆，共同相交在同一个点(方块指示的位置)，圆点指示三个地标。

在实际机器人自定位的系统中，图中圆点就是机器人所识别的地标的位置，而方块的点就是机器人本身的位置。运用传感器分别检测三个地标在环境中相对于机器人的方向，以及机器人和这三个地标所呈夹角

就可以得到机器人本身的位置。该夹角即为机器人相对地标的方向角。

但是传感器对各地标方向角测量具有误差，所以定位系统的输入是具有误差的，相应也就影响了定位系统对机器人本身定位结果的准确度。当机器人处于三个地标围成的三角形区域中时，如今国际普遍应用的三角定位系统均能够提供较为精确的定位结果；而当机器人进入某些特殊区域(我们称之为“三角定位困难区域”)时，如贯穿三个地标的圆环状区域(如图4所示，为三角定位困难区域I，图中L₁，L₂，L₃表示三个地标)以及距离三个地标围成的三角形区域较远的地带(图5所示，为三角定位困难区域II，图中L₁，L₂，L₃表示三个地标)，三角定位系统的定位误差变大，甚至会出现定位错误的情况。尽管国际上有很多学者在这方面做了很多努力，但在困难区域内机器人定位精度的提高上依旧收效甚微。

具体在涉及两种特殊情况时，现有技术的定位方法，定位精度较低：

1.当机器人的位置处于三个地标所确定的公共圆附近的区域时，如图4中三角定位困难区域I，此时图3中的三个相交叉的圆重合，三个圆的交点为无穷多个，从而定位系统完全失效。

2.而由于对三个地标的检测带有一定噪声干扰，即定位系统的输入量——三个角度的度数的测量值会出现不稳定的情况，进而影响到定位结果的精确性。当机器人的位置处于远离三个地标的区域时，定位结果的误差较大，定位精确度降低。

以上两种特殊情况时，传统三角定位系统的定位结果很不稳定，或者失效。

发明内容

针对以上现有技术的不足。本发明提出一种在仅具有三个地标的工作空间中，基于三角定位法的一种准确定位机器人坐标位置的方法。传感器测量的各地标的方向夹角存在误差，且本误差经统计后的概率分布呈现高斯分布，因此可以将输入角度误差所形成的分布作为先验知识，获得一个二维概率映射图，此概率映射图的大小等比例地描述了工作区域的大小，图中每点的概率值反映了机器人在该位置出现的可能性的大小。此概率映射图中概率值最大点的坐标就是机器人当前最有可能所在的位置。

将本概率图结合机器人里程计航位测量信息，运用于蒙特卡洛粒子滤波定位技术进行信息融合，能够进一步提高定位精度。

本发明的方法为：

1)在机器人工作环境中的任意三个位置上分别设置三个地标；

2)多次测量机器人相对地标的方向角，对测量出的每个方向角数据进行统计分析，拟合出其所服从高斯分布模型的方差参数；

3)从高斯分布模型引出方向角的后验概率分布；

4)由地标的坐标和方向角的后验概率分布生成圆弧簇概率图M₁和M₂；

5)融合圆弧簇概率图M₁和M₂，得到一个概率分布图M_t；

6)寻找概率分布图M_t中具有最大权值的坐标位置作为机器人的位置。

由地标的坐标和方向角的后验概率分布生成圆弧簇概率图M₁和M₂的方法为：

A、由任意两地标L_i和L_j的坐标和方向角的后验概率分布生成圆弧簇C_ij＝[X_ij，Y_ij，R_ij]，其中，X_ij和Y_ij是地标L_i和L_j及Z_ij确定的圆弧的圆心的坐标，R_ij为圆弧的半径；

B、采用R_ij的后验概率密度函数遍历所有方向角的测量值，获得概率图M₁和M₂。

所述采用R_ij的后验概率密度函数遍历所有方向角的测量值的方法为：

a)获得R_ij的后验概率密度函数：

其中，

$d_{\mathrm{ij}}=\sqrt{{(x_i-x_j)}^2+{(y_i-y_j)}^2},$ r_ij为随机变量R_ij的一个样本，(x_i，y_i)和(x_j，y_j)分别为地标L_i和L_j的坐标。

b)将此式子求出的概率值反映到C_ij＝[X_ij，Y_ij，R_ij]上，形成概率图M₁和M₂。

其具体实施计算过程如下：

1.在机器人工作环境中的任意三个位置上分别设置三个地标；

2.多次测量机器人相对地标的方向角，对测量出的每个方向角数据统计其方差σ²，从而得到传感器测量角度呈现的分布特性：

$A_{\mathrm{ij}}~N({\widetilde{\mathrm{\α}}}_{\mathrm{ij}},{\mathrm{\σ}}^2)$

其中，

为机器人与任意两地标L_i和L_j的连线形成的夹角的真实大小，i和j为地标的标号，为正整数；A_ij是夹角的测量值所服从的随机分布的随机变量；N表示高斯分布；

3.统计分析可以得出测量角度的后验概率分布，p(Z_ij|α_ij)，就服从以上分布。该后验概率分布表示的是，当测量值为α_ij时，其真实的角度是Z_ij的可能性的大小；

4.由其中两个地标L_i和L_j的坐标，以及随机变量Z_ij生成圆弧簇C_ij＝[X_ij，Y_ij，R_ij]，其中，X_ij和Y_ij是地标L_i和L_j及Z_ij确定的圆弧的圆心的坐标，R_ij为圆弧的半径；

5.由于Z_ij是随机变量，所以这簇圆弧中每一个圆弧出现的可能性都对应着相应Z_ij每个样本值的概率。这簇圆弧以向量形式的随机变量C_ij＝[X_ij，Y_ij，R_ij]表示，每条圆弧相应的概率值可以由向量中半径分量R_ij的概率表示。以半径R_ij表示圆弧簇的概率值：

A、随机变量R_ij的后验概率密度函数为：

其中，

$d_{\mathrm{ij}}=\sqrt{{(x_i-x_j)}^2+{(y_i-y_j)}^2},$ r_ij为随机变量R_ij的一个样本，(x_i，y_i)和(x_j，y_i)分别为地标L_i和L_j的坐标。

B、将此式子求出的概率值反映到C_ij＝[X_ij，Y_ij，R_ij]上，形成概率图M₁和M₂。

6.将这两幅图使用下面公式进行融合，得到一个概率分布图M_t。

$M_t(x,y)=\frac{M_1(x,y)\·M_2(x,y)}{\∫\underset{\mathrm{\Ω}}{\∫}{M}_1(x,y)\·M_2(x,y)\mathrm{dxdy}}$

7.寻找概率分布图M_t中具有最大权值的坐标位置作为机器人的位置：

$[x,y]=\underset{[u,v]}{\arg \max }\left\{M_t(u,v)\right\};[u,v]\∈\mathrm{\Ω}$

[u，v]表示为任意坐标的变量；Ω表示全空间，即所有[u，v]可能的坐标位置。符号

表示返回当函数f(u.v)取得最大值时的[u，v]。

更进一步，对以上概率分布图做进一步的精确，在Monte Carlo粒子滤波定位系统中，与机器人运动信息融合得到新的概率分布图后再计算机器人的位置。Monte Carlo粒子滤波定位方法是已知方法，在此不再赘述。

采用最大似然估计法统计方向角的方差σ²。

所述概率图M₁和M₂为新月形，如图6所示。

所述概率图M₁和M₂为用R_ij的后验概率密度函数遍历所有方向角的测量值获得。

所述机器人运动信息为机器人移动距离和旋转角度。

所述机器人运动信息由机器人里程计计量。

所述机器人采用全景摄像机测量机器人相对地标的方向角。

本发明的有益效果效果：

本发明对基于三角定位系统的移动机器人的自定位精度具有一定提高。针对三角定位困难区域中定位精度较低的问题，发明人设计了10个不同的场景，进行了上33170次的实验。实验结果显示，在定位精确度上我们的方法比原始方法平均提高21.9％。所以，当机器人在处于“三角定位困难区域”时，较之现在普遍运用的三角定位系统，机器人的定位精度有显著提高。

附图说明

图1是三点定位一个圆的原理图；

图2是根据三点位置拓扑关系不同确定圆心坐标的原理图；

图3是采用三个地标确定机器人位置的方法的原理图；

图4是三角定位方法中的定位困难区域I；

图5是三角定位方法中的定位困难区域II；

图6是本发明的方法生成的圆弧簇的概率图；

图7是图6的概率图融合过程的示意图；

具体实施方法

下面结合实施例和附图详细说明本发明的方法。

1.在机器人运行的工作环境中，随意放置三个地标；在此环境中的任意位置处，使用机器人全景摄像机多次大量测量机器人与各个地标的方向角，将这些测量结果统计分析，拟合出其所服从高斯分布模型的参数，主要是方差参数：

$A_{\mathrm{ij}}~N({\widetilde{\mathrm{\α}}}_{\mathrm{ij}},{\mathrm{\σ}}^2)$

其中，

为机器人与地标L_i和L_j的连线形成的夹角的真实大小，i和j为地标的标号，为正整数；A_ij是方向角的测量值所服从的随机分布的随机变量；N表示高斯分布；

2.根据以上分布引出后验概率分布。表示，当某次测量角度为

时，其真实角度值为Z_ij的概率：p(Z_ij|α_ij)

3.计算单个圆时形成圆簇的概率分布图。从地标L_i和L_j的坐标和方向角Z_ij，可以生成圆弧簇，由于圆弧簇中各个圆的半径并不相同，可以用半径出现的概率值来体现圆簇中各个圆的概率值。圆弧簇概率分布函数为

其中，

$d_{\mathrm{ij}}=\sqrt{{(x_i-x_j)}^2+{(y_i-y_j)}^2},$ (x_i，y_i)和(x_j，y_j)是地标L_i和L_j的坐标。

根据此公式遍历所有方向角的测量值，形成新月形圆弧簇概率图M₁和M₂。如图6所示，方点代表三个地标。图6(a)是M₁和图6(b)是M₂。

4.参见图7，将M₁和M₂两图以如下方法进行融合，形成融合后的概率分布图M_t：

$M_t(x,y)=\frac{M_1(x,y)\·M_2(x,y)}{\∫\underset{\mathrm{\Ω}}{\∫}{M}_1(x,y)\·M_2(x,y)\mathrm{dxdy}}$

5.为了使定位精度更高，将此概率分布图作为观测模型，运用到Monte Carlo粒子滤波定位系统中，与里程计中计量的机器人运动信息，即机器人移动距离和旋转角度相融合得到融合后新的的概率分布图。

6.搜索机器人最终的坐标位置为：概率分布图M_t中具有最大权值的坐标位置，即

$[x,y]=\underset{[u,v]}{\arg \max }\left\{M_t(u,v)\right\};[u,v]\∈\mathrm{\Ω}$

[u，v]表示为任意坐标的变量；Ω表示全空间，即所有[u，v]可能的坐标位置。符号

表示返回当函数f(u.v)取得最大值时的[u，v]。

Claims (9)

1.一种基于视觉的移动机器人定位方法，包括如下步骤：

1)在机器人工作环境中的任意三个位置上分别设置三个地标；

2)多次测量机器人相对地标的方向角，对测量出的每个方向角数据进行统计分析，拟合出其所服从高斯分布模型的方差参数；

3)从高斯分布模型引出方向角的后验概率分布；

4)由地标的坐标和方向角的后验概率分布生成圆弧簇概率图M₁和M₂；

5)融合圆弧簇概率图M₁和M₂，得到一个概率分布图M_t；

6)寻找概率分布图M_t中具有最大权值的坐标位置作为机器人的位置。

2.根据权利要求1所述的基于视觉的移动机器人定位方法，其特征在于，在Monte Carlo粒子滤波定位系统中，将概率分布图M_t与机器人运动信息融合得到新的概率分布图后再计算机器人的位置。

3.根据权利要求1或2所述的基于视觉的移动机器人定位方法，其特征在于，采用最大似然估计法统计方向角的方差参数。

4.根据权利要求1或2所述的基于视觉的移动机器人定位方法，其特征在于，所述由地标的坐标和方向角的后验概率分布生成圆弧簇概率图M₁和M₂的方法为：

A、由任意两地标L_i和L_j的坐标和方向角的后验概率分布生成圆弧簇C_ij＝[X_ij，Y_ij，R_ij]，其中，X_ij和Y_ij是地标L_i和L_j及Z_ij确定的圆弧的圆心的坐标，R_ij为圆弧的半径；

B、采用R_ij的后验概率密度函数遍历所有方向角的测量值，获得概率图M₁和M₂。

5.根据权利要求4所述的基于视觉的移动机器人定位方法，其特征在于，采用R_ij的后验概率密度函数遍历所有方向角的测量值的方法为：

a)获得R_ij的后验概率密度函数：

其中，

$d_{\mathrm{ij}}=\sqrt{{(x_i-x_j)}^2+{(y_i-y_j)}^2},$ r_ij为随机变量R_ij的一个样本，(x_i，y_i)和(x_j，y_j)分别为地标L_i和L_j的坐标；

b)将此式子求出的概率值反映到C_ij＝[X_ij，Y_ij，R_ij]上。

6.根据权利要求1或2所述的基于视觉的移动机器人定位方法，其特征在于，所述概率图M1和M2为新月形。

7.根据权利要求2所述的基于视觉的移动机器人定位方法，其特征在于，所述机器人运动信息为机器人移动距离和旋转角度。

8.权利要求7所述的基于视觉的移动机器人定位方法，其特征在于，所述机器人运动信息由机器人里程计计量。

9.根据权利要求1或2所述的基于视觉的移动机器人定位方法，其特征在于，机器人采用全景视觉摄像机测量机器人相对地标的方向角。

Images