CN101660908A - 基于单个路标的视觉定位与导航方法 - Google Patents

Abstract

基于单个路标的视觉定位与导航方法。解决现有定位与导航方法实时性不高、抗干扰能力差、路径不易改变等问题。本发明提供的路标包括圆盘式路标主体和圆盘上用于对机器人进行定位的第一辅助部分和指示下一路标所在方向的第二辅助部分。该方法利用单个路标即可实现机器人的定位与导航，包括：将路标间隔铺设在机器人上方；通过镜头获取场景图像并对路标及其第一、第二辅助部分进行识别定位，然后利用图像坐标系、路标坐标系和世界坐标系的关系对机器人进行定位以及导航。基于该路标的定位与导航算法有效的缩小了处理区域，实时性好，抗干扰能力强，路径可编程设定。

Description

基于单个路标的视觉定位与导航方法

【技术领域】：

本发明属于机器人视觉定位与导航技术领域。

【背景技术】：

机器人工作过程中经常需要确定自身位置(定位)并进行导航，因此定位和导航一直是机器人领域中的一个重要研究方向，具有很好的应用前景。

常用的定位方法有利用超声或红外传感器等感知机器人与周围物体的相对位置，从而对机器人进行定位，这种方法可以较准确的判断出机器人与周围物体的相对位置，但是要想对机器人进行绝对定位却很困难，因此主要用于导航。另一种方法是利用视觉获取环境信息与数据库中的数据进行匹配从而对机器人进行定位，该方法可以较准确的获取机器人的绝对位置，但是定位算法比较复杂。

常用的导航方法有利用导轨进行导航，利用超声或红外传感器等进行导航和利用视觉进行导航。其中利用导轨进行导航精度很高，实时性好，但是导轨铺设困难，导航路径固定；利用超声或红外传感器等进行导航精度较高，但信息量小，不利于机器人的综合运用；利用视觉进行导航由于信息量大，设备简单，使用方便，受到了广大研究者的青睐。

目前，视觉定位与导航中使用最多的是利用路标进行定位与导航。路标选取可分为两大类：一是利用自然信息作为路标，二是利用人造路标。利用自然信息作为路标可以提取同一场景中多种自然信息，定位与导航精度高，但该方法计算复杂，并且需要事先建模，系统实时性不高；利用人造路标进行定位与导航算法相对简单，系统实时性较高，但是抗干扰能力差，可靠性不高。

【发明内容】：

本发明目的是解决现有机器人定位与导航方法实时性不够高、抗干扰能力差、路标信息量小、路径不易改变等问题，提供一种简单、经济，路径可随意改变且实时性更高的机器人视觉定位与导航方法，以及适合该方法的路标。

该方法根据路标和机器人在图像坐标系的位姿关系计算机器人相对于路标的相对位姿，依据路标坐标系和世界坐标系的关系计算机器人在世界坐标系的绝对位姿，即对机器人进行定位。

根据路标、目标点和机器人在图像中的相互关系对机器人进行视觉导航。

1、本发明提供的基于单个路标的机器人视觉定位方法，包括如下步骤：

第一、路标的设计

该路标为表盘式组合结构，路标主体是半径为R的圆盘，圆盘圆心为路标中心，圆盘上设有用于机器人定位的第一辅助部分和指示下一路标所在方向的第二辅助部分；路标的主体采用黄色，第一辅助部分采用蓝色，第二辅助部分采用紫色；

第二、路标铺设

首先确定机器人的移动路径，然后将路标间隔铺设在移动路径上方，铺设时，使当前路标的中心点及其第二辅助部分的中心点和下一路标的中心点及其第一辅助部分的中心点位于同一条直线上；

第三、路标的识别与定位

令机器人起始时位于第一个路标下方，由机器人对路标进行图像采集，并对图像进行颜色识别，利用模板匹配确定在图像坐标系中路标的中心坐标(x，y)，利用统计平均坐标确定在图像坐标系中路标第一辅助部分的中心坐标(x₁，y₁)和第二辅助部分的中心坐标(x₂，y₂)；

第四、机器人的定位

1)、机器人的相对定位

令MO₁N为路标坐标系，M’O₁’N’为路标坐标系在图像中的投影，则机器人在图像中相对于路标坐标系的坐标为

Δm＝-l×sin(θ-β)

Δn＝-l×cos(θ-β)

$\mathrm{\β}=\arctan \frac{x_1-x}{y_1-y}$

$\mathrm{\θ}=\arctan \frac{x_0-x}{y_0-y}$

其中，l为图像中路标距图像中心的距离；β为图像中路标坐标系N’轴与机器人前进方向的夹角，θ为图像中心和路标中心连线与机器人前进方向的夹角，(x₀，y₀)代表机器人在图像中的坐标；

2)、机器人的绝对定位

假设路标在世界坐标系X_wO_wY_w中的坐标为(X，Y)，则机器人在世界坐标系中的绝对坐标(X′，Y′)为

X′＝X+Lsin(α-λ)

Y′＝Y-Lcos(α-λ)

其中L＝l/k；λ＝θ-β；α为世界坐标系与路标坐标系的夹角，若路标坐标系相对于世界坐标系发生逆时针旋转则α为正，反之为负，k为图像坐标系与世界坐标系的比例系数。

其中，图像坐标系与世界坐标系的比例系数k的计算方法如下：

统计与路标同高度的长度为Lcm的线段在图像中的像素个数n，则比例系数为： $k=\frac{n}{\mathrm{Lcm}}.$

上述第三步路标的识别与定位中对图像进行颜色识别的处理区域为边长4kR～6kR的方形区域，路标主体半径R的取值范围一般为10～30cm。

路标的识别与定位具体方法是：

1)、设定黄色的颜色阈值为123＜U＜143，32＜H＜52，紫色的阈值范围为174＜U＜194，322＜H＜342，蓝色的阈值范围为176＜U＜196，188＜H＜212；其中U为YUV颜色空间的色调分量U，H为HSI颜色空间的色调分量H；

2)、计算处理区域的起止坐标(x-pkR，y-pkR)，(x+pkR，y+pkR)，2≤p≤3；

3)、利用设置的颜色阈值对处理区域内的每一像素点进行颜色识别，并分别将黄色、紫色、蓝色和其他颜色的像素点灰度值设置为255、150、100和0，然后进行腐蚀滤波和膨胀滤波去除干扰点；

4)、统计灰度值非0的像素点的平均坐标(x_temp，y_temp)，采用边长为1.5kR的方形为模板，在(x_temp，y_temp)附近进行模板匹配精确定位当前路标中心在图像坐标系的坐标(x，y)；

5)、分别统计以(x，y)为中心，2kR为边长的方形区域内灰度值为150和100的像素点的平均坐标，即为当前路标第一辅助部分的中心在图像坐标系的坐标(x₁，y₁)和第二辅助部分的中心在图像坐标系的坐标(xX，y₂)。

本发明同时提供了一种基于以上所述机器人视觉定位方法的机器人视觉导航方法，该方法在以上定位的基础上还包括如下步骤：

第五、导航方法：

1)、固定路径时的导航方法：

令机器人按权利要求1中第二步铺设的路标沿固定路径移动导航，则首先依第四步中的方法计算出θ，机器人车体偏离角γ＝θ，如果-T＜γ＜T，机器人前行，如果γ＞T，机器人需顺时针旋转，如果γ＜-T，机器人需逆时针旋转，其中T为预先设定的是否对机器人进行纠偏的阈值；

2)、按编程设定路径时的导航方法：

如果路标不变，导航路径依导航目标点编程设定，其中的导航目标点可以是路径中的某个路标，也可以是路标以外的某个点，则机器人车体偏离角γ的计算公式为：

$\mathrm{\γ}=\arctan \frac{{x_0}^{\′}-x}{y_0-y}$

其中x₀′＝x₀+kΔd，Δd为当前目标点到前一目标点和当前路标连线的垂直距离并规定目标点在连线右侧时Δd为正，目标点在连线左侧时Δd为负，如果-T＜γ＜T，机器人前行，如果γ＞T，机器人需顺时针旋转，如果γ＜-T，机器人需逆时针旋转，T为预先设定的是否对机器人进行纠偏的阈值；

第六、预测路标位置

如果y＜Tf，则认为当前路标有效，其在下一帧图像中的坐标仍预测为(x，y)；

如果y＞Tf，则应寻找新的路标，预测新路标在下一帧图像中的坐标(x′，y′)为：

其中Tf为是否寻找下一路标的阈值，D为两路标在世界坐标系的距离。Tf取值范围一般为((y₀-20k)～y₀)，机器人延迟越大，Tf越小，机器人延迟越小，Tf应越接近图像中心纵坐标y₀。

本发明的优点和积极效果：

1、本发明方法利用单个路标即可实现机器人的定位和导航，算法简单，并且避免了多个路标进行定位时的匹配失误，提高了实时性和可靠性。2、本发明所设计的路标含有方向信息，即使机器人发生很大偏转仍能找到正确的方向，有很强的抗干扰能力。3、本发明方法涉及的导航路径即可以为固定路径，也可以编程设定，不仅提高了导航路径的灵活性，也提高了路标铺设的灵活性。4、本发明所设计的路标可以采用纸张、塑料等材质制成，非常经济，并且该路标结构简单，铺设方便。

【附图说明】：

图1(a)为路标外形示意图、图1(b)为路标铺设方式示意图；其中，1为路标第一辅助部分，2为路标第二辅助部分，3为路标主体。

图2(a)为机器人相对定位示意图，图2(b)为机器人绝对定位示意图；图中，XOY，MO₁N，X_wO_wY_w分别为图像坐标系、路标坐标系和世界坐标系；M’O₁’N’为路标坐标系在图像中的投影；O₀为图像中心点(即机器人在图像中的投影)，O₀Q为机器人当前的前进方向。

图3(a)为世界坐标系及其中的路标和目标点示意图；3(b)为某一时刻的一帧图像示意图，其中a为前一目标A在图像中的投影，b为当前路标B在图像中的投影，e为当前目标点E在图像中的投影。

图4为预测下一路标位置示意图。

图5为路标铺设实例图。

图6(a)为机器人定位实例图，图6(b)为图6(a)的局部放大图，其中的十字交点为找到的路标及其两个辅助部分的中心。

图7为导航实例图，图中，4为当前路标，5为当前目标点所在位置。

图8(a)为预测下一路标位置实例图，图8(b)为图8(a)的局部放大图，其中的白色十字交点为预测的新路标的中心。

图9为算法流程图。

【具体实施方式】：

步骤1：路标设计

如图1(a)所示，本发明所设计的路标可选用纸张、塑料等材质制成，结构为表盘式组合结构。路标主体3是半径为R的圆盘，圆盘圆心为路标中心，R的取值范围一般为10～30cm，相机分辨率越高，R的取值可以越小；路标辅助部分形状设计为梯型结构，其宽度为R，辅助部分内侧距离圆心为R/2，其中路标的第二辅助部分能够以圆盘的圆心为中心正负旋转90度。

路标的主体采用黄色，第一辅助部分采用蓝色，第二辅助部分采用紫色。

步骤2：路标铺设

首先确定机器人的移动路径，然后将路标间隔铺设在移动路径上方(尽量让所有路标在一个水平面内，以提高定位精度)，如图1(b)所示；铺设时，使当前路标的中心(x，y)及其第二辅助部分的中心(x₂，y₂)和下一路标的中心(x′，y′)及其第一辅助部分的中心(x′₁，y₁′)在一条直线上，如图4所示。

步骤3：初始化

1、(x，y)代表当前路标中心在图像中的坐标，(x₀，y₀)代表机器人在图像中的坐标，本实施例中机器人在图像中的坐标为图像中心坐标(568，426)，将机器人放到第一个路标下方，并设定起始时当前路标中心在图像中的坐标为图像中心坐标(x₀，y₀)；

2、设定黄色的颜色阈值为123＜U＜143，32＜H＜52，紫色的阈值范围为174＜U＜194，322＜H＜342，蓝色的阈值范围为176＜U＜196，188＜H＜212；其中U为YUV颜色空间的色调分量U，H为HSI颜色空间的色调分量H。

3、统计与路标同高度的100cm长度的线段在图像中的像素个数n，比例系数

本实施例中k＝5.1；

4、根据摄像机的分辨率设定颜色识别的处理区域为边长4kR～6kR的方形区域，本实施例中选用的边长为4kR；

5、依据机器人的性能设定寻找下一路标的阈值Tf，其取值范围一般为((y₀-20k)～y₀)，机器人延迟越大，Tf越小，机器人延迟越小，Tf应越接近图像中心纵坐标y₀；本实施例中Tf＝y₀-10k＝426-10×5.1＝375；

6、设定机器人的纠偏阈值T为10°，即当机器人的偏离角度在(-10°～10°)之间时，机器人仍然正常前行，当超过这个范围之后，对机器人进行纠偏。

步骤4：定位方法

1、路标在图像中的识别定位：

1)计算处理区域的起止坐标(x-2kR，y-2kR)，(x+2kR，y+2kR)

2)利用设置的颜色阈值对处理区域内的每一像素点进行颜色识别，并分别将黄色、紫色、蓝色和其他颜色的像素点灰度值设置为255、150、100和0，然后进行腐蚀滤波和膨胀滤波去除干扰点；

3)统计灰度值非0的像素点的平均坐标(x_temp，y_temp)，采用边长为1.5kR的方形为模板，在(x_temp，y_temp)附近进行模板匹配精确定位当前路标中心在图像坐标系的坐标(x，y)；

4)分别统计以(x，y)为中心，2kR为边长的方形区域内灰度值为150和100的像素点的平均坐标，即为当前路标第一辅助部分的中心在图像坐标系的坐标(x₁，y₁)和第二辅助部分的中心在图像坐标系的坐标(x₂，y₂)，如图6所示。

2、机器人的相对定位

如图2(a)所示，l为图像中路标距图像中心的距离；β为图像中路标坐标系N’轴与机器人前进方向的夹角，θ为图像中心和路标中心连线与机器人前进方向的夹角，则图像中机器人在路标坐标系的相对位置为

Δm＝-l×sin(θ-β)

Δn＝-l×cos(θ-β)

其中：

$\mathrm{\β}=\arctan \frac{x_1-x}{y_1-y}$

$\mathrm{\θ}=\arctan \frac{x_0-x}{y_0-y}$

3、机器人的绝对定位

如图2(b)所示，假设路标在世界坐标系的坐标为(X，Y)，则机器人在世界坐标系的绝对坐标(X′，Y′)为

X′＝X+Lsin(α-λ)

Y′＝Y-Lcos(α-λ)

其中L＝l/k；λ＝θ-β；α为世界坐标系与路标坐标系的夹角，若路标坐标系相对于世界坐标系发生逆时针旋转则α为正，反之为负；

步骤5：导航方法

1、固定路径时的导航方法：

如图3所示，如果按路标所示的固定路径A->B->C->D进行导航，则按步骤4中2的方法计算出θ，车体偏离角γ＝θ，如果-T＜γ＜T(T为10°)，机器人前行；如果γ＞T，机器人需顺时针旋转。如果γ＜-T，机器人需逆时针旋转。其中T为是否对机器人进行纠偏的阈值。

2、编程设定路径时的导航方法：

如图3所示，如果路标不变，导航路径设定为A->E->F->D，(其中A、E、F、D为路径中的不同目标点)则γ的计算公式为：

$\mathrm{\γ}=\arctan \frac{{x_0}^{\′}-x}{y_0-y}$

其中x₀′＝x₀+kΔd，Δd为当前目标点到前一目标点与当前路标连线的垂直距离(规定目标点在连线右侧时Δd为正，目标点在连线左侧时Δd为负，如图3所示Δd₁为正，Δd₂为负)。图3(a)为世界坐标系及其中的路标和目标点；3(b)为某一时刻的一帧图像，其中a为前一目标A在图像中的投影，b为当前路标B在图像中的投影，e为当前目标点E在图像中的投影。

如果-T＜γ＜T(T为10°)，机器人前行；如果γ＞T，机器人需顺时针旋转。如果γ＜-T，机器人需逆时针旋转。

步骤6：预测路标位置

如果y＜Tf，则认为当前路标有效，其在下一帧图像中的坐标仍预测为(x，y)。

如果y＞Tf，则应寻找新的路标，如图4所示，预测新路标在下一帧图像中的坐标(x′，y′)为

其中Tf为是否寻找下一路标的阈值，D为两路标在世界坐标系的距离。

应用实例1：

路标铺设实例：如图5所示，根据机器人的行走路径将路标水平间隔铺设在路径上方的房顶上，其中各相邻路标之间的铺设方法参见步骤2。.

应用例2：

机器人定位实例：如图6所示，通过模板匹配和统计平均坐标可分别得到在图像坐标系中路标的中心坐标为(501，260)，辅助部分1的中心坐标为(447，310)，已知图像中心坐标为(568，426)，k＝5.1，α＝0°，X＝0，Y＝140。则机器人在图像中相对于路标坐标系的位置为：

$\mathrm{\Δm}=-\sqrt{{(501-568)}^2+{(260-426)}^2}\×\sin (\arctan \frac{568-501}{426-260}-\arctan \frac{447-501}{310-260})\≈-167$

$\mathrm{\Δn}=-\sqrt{{(501-568)}^2+{(260-426)}^2}\×\cos (\arctan \frac{568-501}{426-260}-\arctan \frac{447-501}{310-260})\≈-64$

机器人在世界坐标系的绝对坐标为

$X^{\′}=0+\sqrt{{(501-568)}^2+{(260-426)}^2}\×\sin [0-(\arctan \frac{568-501}{426-260}-\arctan \frac{447-501}{310-260})]/5.1\≈-33\mathrm{cm}$

$Y^{\′}=140-\sqrt{{(501-568)}^2+{(260-426)}^2}\×\cos [0-(\arctan \frac{568-501}{426-260}-\arctan \frac{447-501}{310-260})]/5.1\≈128\mathrm{cm}$

实际测量的A′＝-34cm，B′＝129cm，误差小于2cm

应用例3：

机器人导航实例：如图7所示，图像上方的路标4为当前路标，并且已知x＝399，y＝101，x₀＝568，y₀＝426。

1、若机器人按路标指示的固定路径行走，则

27.5°＞10°，小车需顺时针旋转才能到达当前路标4。

2、若编程设定行走路线，设当前目标点5在当前路标(路标4)左侧，且Δd＝-60，已知k＝5.1，则

x₀′＝x₀+kΔd＝568-5.1×60＝262

因为-22.9°＜-10°，小车需逆时针旋转才能到达当前目标点5。

应用例4：

预测路标位置实例：如图8所示，已知在图像坐标系中路标中心坐标为(396，378)，辅助部分2的中心坐标为(458，342)，寻找下一路标的阈值Tf为375，k＝5.1，D＝140。因为y＞Tf，需寻找新路标，新路标在下一帧图像中的坐标计算如下：

x′＝396+5.1×140×cos30.1°≈1013

y′＝378-5.1×140×sin30.1°≈19

Claims (6)

1、一种基于单个路标的机器人视觉定位方法，其特征在于该定位方法包括如下步骤：

第一、路标的设计

该路标为表盘式组合结构，路标主体是半径为R的圆盘，圆盘圆心为路标中心，圆盘上设有用于机器人定位的第一辅助部分和指示下一路标所在方向的第二辅助部分；路标的主体采用黄色，第一辅助部分采用蓝色，第二辅助部分采用紫色；

第二、路标铺设

首先确定机器人的移动路径，然后将路标间隔铺设在移动路径上方，铺设时，使当前路标的中心点及其第二辅助部分的中心点和下一路标的中心点及其第一辅助部分的中心点位于同一条直线上；

第三、路标的识别与定位

令机器人起始时位于第一个路标下方，由机器人对路标进行图像采集，并对图像进行颜色识别，利用模板匹配确定在图像坐标系中路标的中心坐标(x，y)，利用统计平均坐标确定在图像坐标系中路标第一辅助部分的中心坐标(x₁，y₁)和第二辅助部分的中心坐标(x₂，y₂)；

第四、机器人的定位

1)、机器人的相对定位

令MO₁N为路标坐标系，M’O₁’N’为路标坐标系在图像中的投影，则机器人在图像中相对于路标坐标系的坐标为

Δm＝-l×sin(θ-β)

Δn＝-l×cos(θ-β)

$\mathrm{\β}=\arctan \frac{x_1-x}{y_1-y}$

$\mathrm{\θ}=\arctan \frac{x_0-x}{y_0-y}$

其中，l为图像中路标距图像中心的距离；β为图像中路标坐标系N’轴与机器人前进方向的夹角，θ为图像中心和路标中心连线与机器人前进方向的夹角，(x₀，y₀)代表机器人在图像中的坐标；

2)、机器人的绝对定位

假设路标在世界坐标系X_wO_wY_w中的坐标为(X，Y)，则机器人在世界坐标系中的绝对坐标(X′，Y′)为

X′＝X+Lsin(α-λ)

Y′＝Y-Lcos(α-λ)

其中L＝l/k；λ＝θ-β；α为世界坐标系与路标坐标系的夹角，若路标坐标系相对于世界坐标系发生逆时针旋转则α为正，反之为负，k为图像坐标系与世界坐标系的比例系数。

2、根据权利要求1所述的方法，其特征在于图像坐标系与世界坐标系的比例系数k的计算方法如下：

统计与路标同高度的长度为Lcm的线段在图像中的像素个数n，则比例系数为：

$k=\frac{n}{\mathrm{Lcm}}.$

3、根据权利要求1或2所述的方法，其特征在于第三步路标的识别与定位中对图像进行颜色识别的处理区域为边长4kR～6kR的方形区域，路标主体半径R的取值范围一般为10～30cm。

4、根据权利要求3所述的方法，其特征在于第三步路标的识别与定位方法是：

1)、设定黄色的颜色阈值为123＜U＜143，32＜H＜52，紫色的阈值范围为174＜U＜194，322＜H＜342，蓝色的阈值范围为176＜U＜196，188＜H＜212；其中U为YUV颜色空间的色调分量U，H为HSI颜色空间的色调分量H；

2)、计算处理区域的起止坐标(x-pkR，y-pkR)，(x+pkR，y+pkR)，2≤p≤3；

3)、利用设置的颜色阈值对处理区域内的每一像素点进行颜色识别，并分别将黄色、紫色、蓝色和其他颜色的像素点灰度值设置为255、150、100和0，然后进行腐蚀滤波和膨胀滤波去除干扰点；

4)、统计灰度值非0的像素点的平均坐标(x_temp，y_temp)，采用边长为1.5kR的方形为模板，在(x_temp，y_temp)附近进行模板匹配精确定位当前路标中心在图像坐标系的坐标(x，y)；

5)、分别统计以(x，y)为中心，2kR为边长的方形区域内灰度值为150和100的像素点的平均坐标，即为当前路标第一辅助部分的中心在图像坐标系的坐标(x₁，y₁)和第二辅助部分的中心在图像坐标系的坐标(x₂，y₂)。

5、一种基于权利要求1所述机器人视觉定位方法的机器人视觉导航方法，其特征在于该方法还包括如下步骤：

第五、导航方法：

1)、固定路径时的导航方法：

令机器人按权利要求1中第二步铺设的路标沿固定路径移动导航，则首先依第四步中的方法计算出θ，机器人偏离角γ＝θ，如果-T＜γ＜T，机器人前行，如果γ＞T，机器人需顺时针旋转，如果γ＜-T，机器人需逆时针旋转，其中T为预先设定的是否对机器人进行纠偏的阈值；

2)、按编程设定路径时的导航方法：

如果路标不变，导航路径依导航目标点编程设定，其中的导航目标点可以是路径中的某个路标，也可以是路标以外的某个点，则机器人偏离角γ的计算公式为：

$\mathrm{\γ}=\arctan \frac{{x_0}^{\′}-x}{y_0-y}$

其中x₀′＝x₀+kΔd，Δd为当前目标点到前一目标点和当前路标连线的垂直距离并规定目标点在连线右侧时Δd为正，目标点在连线左侧时Δd为负，如果-T＜γ＜T，机器人前行，如果γ＞T，机器人需顺时针旋转，如果γ＜-T，机器人需逆时针旋转，T为预先设定的是否对机器人进行纠偏的阈值；

第六、预测路标位置

如果y＜Tf，则认为当前路标有效，其在下一帧图像中的坐标仍预测为(x，y)；

如果y＞Tf，则应寻找新的路标，预测新路标在下一帧图像中的坐标(x′，y′)为：

其中Tf为是否寻找下一路标的阈值，D为两路标在世界坐标系的距离。

6、根据权利要求5所述的方法，其特征在于是否寻找下一路标的阈值Tf取值范围一般为((y₀-20k)～y₀)，机器人延迟越大，Tf越小，机器人延迟越小，Tf应越接近图像中心纵坐标y₀。

Images