CN101619984B - 一种基于颜色路标的移动机器人视觉导航方法 - Google Patents

Abstract

本发明请求保护一种基于颜色路标的移动机器人视觉导航方法，涉及智能控制领域。本发明设计了带有两个不同颜色环的圆柱体作为机器人视觉导航的路标，在HSI(HSI：Hue，Saturation，Intensity，色调，饱和度，亮度)颜色空间下，以H为主阈值，I为辅助阈值的阈值分割法和基于S分量边缘点数量统计的分割法相结合，有效地实现了彩色路标的分割；通过色环的图像信息，实现了机器人对路标的识别和对准技术。引入改进的灰度相关匹配法，优化了两色环中心垂直距离值。通过建立机器人坐标系和路标的成像模型，得到了路标的全局坐标，并最终引入粒子群算法对其优化。建立数学模型，完成了基于路标的机器人视觉重定位。

Description

一种基于颜色路标的移动机器人视觉导航方法

技术领域

本发明涉及智能控制领域，具体涉及移动机器人的视觉导航方法。 

背景技术

自从20世纪60年代初，第一台工业机器人发明以来，机器人的发展已有半个多世纪，机器人的应用越来越广泛，几乎渗透到所有的领域。机器人大致经过三代的演变：第一代是可编程的示教再现型机器人；第二代是具有一定感觉功能和自适应能力的离线编程机器人；第三代机器人是智能机器人。机器人正在向智能化和视觉化的趋势发展，智能化和视觉化的机器系统将从事目前传统机器系统和人类难以胜任的恶劣环境下的一些操作。 

移动机器人是机器人学中的一个重要分支，是一类能够通过传感器感知环境和自身状态，实现在非结构环境下，动态决策与规划、行为控制与执行等多项功能于一体的高智能化机器系统。它与机器人学、通讯技术、计算机视觉、多传感器信息融合、智能控制以及多智能体(Multi-Agent)、机械学等学科密切相关，体现了信息科学和人工智能技术的最新成果，具有重要的军用及民用价值，是现代机器人学中一个重要而且相当活跃的研究领域。 

众所周知，人们从外界环境获取的信息中，80％来自于视觉，其他来自于触觉、听觉、嗅觉等感觉器官。当人们的眼睛从自己周围的环境获取大量信息，并传入大脑后，由大脑根据知识或经验，对信息进行加工、推理等处理工作，最后识别、理解周围环境：包括环境内的对象物、物体间的相对位置、形状、大小、颜色及纹理；从而使人能与周围环境进行有效的信息交互，完成各种复杂的任务与行动。同样，移动机器人感知外部环境也需要“眼睛”，那就是机器视觉。现在，视觉系统已成为移动机器人重要的组成部分之一。移动机器人视觉可以定义为这样一个过程：利用视觉传感器获取三维景物的二维图像，通过视觉处理算法对一幅或多幅图像进行处理、分析和解释，得 到有关景物的符号描述，并为特定的任务提供有用的信息，用于引导机器人的动作。 

在移动机器人的研究课题中，对移动机器人的视觉导航进行研究是一个重要方面。视觉信息是人类感知环境世界的主要途径，也是移动机器人视觉导航的关键技术。如果机器人能够能像人一样通过视频处理而具有从外部环境获取信息的能力，这样就不仅大大提高了它们环境适应和生存能力，更重要的是使人机交互更加方便自然。因此这方面的研究工作具有重要的理论意义和潜在的应用价值。 

世界上第一个自治机器人Shakey(Nilsson 1984)，就使用了基于单色图像的障碍物检测方法。Shakey是在无质感的地面上行走，障碍物可以通过对单色输入图像进行边缘检测来检测。但是Shakey能行走的环境需要人工专门设置。障碍物的表面不能有反射并且表面的颜色都是均匀统一的。光源也需要特别放置，使得墙和地面不能出现反射和阴影。Becker等使用绑缚在屋顶的简单路标来实现机器人的导航。Stephen Se和David Lowe提出了利用视觉路标进行移动机器人全局定位和地图生成算法。该算法在没有任何修改的环境中利用尺度不变特征变换(Scale Invariant Feature Transform，SIFT)的来提取环境图像特征作为自然路标，实现了机器人的定位和地图构建。 

但现有技术针对移动机器人视觉导航中，存在自然路标的鲁棒性不高，图像处理时间复杂度高等缺点。 

发明内容

本发明针对移动机器人视觉导航中，自然路标的鲁棒性不高，图像处理时间复杂度高等缺点，提出了基于颜色路标的移动机器人视觉导航的方法。 

本发明实现基于颜色路标的移动机器人视觉导航方法所采用的技术方案，具体包括如下步骤： 

设计带有两个不同颜色环的圆柱体作为机器人视觉导航的路标，对不同 的路标加以编号并存储；机器人根据不同路标的色带组合依次寻找路标，算法模块根据公式 实时计算移动机器人到路标的距离L；移动机器人在开始运动时以初始行进的方向为X轴建立机器人坐标系，里程计等惯性器件实时检测机器人的当前坐标(X，Y)和旋转角度TH，调用公式： 

PX＝X+LcosTH    (2) 

PY＝Y+LsinTH    (3) 

确定路标坐标(PX，PY)； 

机器人行进过程中，获取每一时刻路标坐标(px_k，py_k)，以及机器人本身的坐标和旋转角度(x_k，y_k，th_k)，机器人与路标的距离d_k；采用粒子群优化算法(Particle Swarm Optimization，PSO)确定最优坐标值。最优坐标值应满足距离适应度函数与角度适应度函数最小。 

寻找路标并识别的过程中，首先在HSI(HSI：Hue，Saturation，Intensity，色调，饱和度，亮度)颜色空间下，以H为主阈值，I为辅助阈值的阈值分割法和基于S分量边缘点数量统计的分割法相结合，分割路标上的两色块，并得到它们的面积、图像坐标等信息。通过两个颜色块的面积大小相近小于某一阈值进行初步的路标判断，采用水平方向的两色块的中心坐标之差小于某一阈值作为路标识别的进一步判断，根据两色块的颜色并与存储空间中已经编号的路标比对最终完成路标的识别。 

本发明的有益效果：本发明是一种基于颜色路标的移动机器人视觉导航的方法，与常规的移动机器人视觉导航方法相比，具有人工路标简单，图像处理复杂度低。机器人对路标的识别鲁棒性高，并通过数学建模，完成了移动机器人的视觉重定位。完成了机器人学习环境和利用环境认识自我的结合。 

附图说明

图1机器人视觉导航流程图 

图2人工路标示意图 

图3路标成像模型示意图 

图4机器人视觉重定位示意图 

具体实施方式

本发明设计了一种基于颜色路标的移动机器人视觉导航方法，移动机器人在开始运动时以初始行进的方向为X轴建立默认的坐标系，这个坐标系称作机器人坐标系，也称为局部坐标系，机器人中的里程计等惯性器件获取得到每时刻移动机器人的在局部坐标系中的位置坐标，当然也可以通过坐标旋转平移得到移动机器人的全局坐标。里程计在短时间内的坐标是较为准确的，但随着时间的增加由于移动机器人车轮的打滑，路面的不平整等因素会使坐标的误差累积不断加大，移动机器人的坐标就会出现与实际情况偏差变大。此时，移动机器人利用设置的路标，通过视觉获取并计算路标的最优坐标值来求得机器人的准确坐标进行全局定位。 

如图1所示为机器人视觉导航示意流程图，具体包括如下步骤： 

首先设计带有不同颜色环的圆柱体作为机器人视觉导航的路标，对不同的路标加以编号并存储； 

然后机器人根据不同路标的色带组合依次寻找路标，实时计算移动机器人到路标的距离，并引入粒子群算法获得路标的最优坐标。 

最后根据里程计误差，判断是否引入机器人视觉重定位模块来重置机器人位姿，以实现持续安全的导航。 

在目标检测和识别中，颜色作为物体表面的一个特征提供目标的位置或姿态等重要信息。常用的彩色图像目标检测和识别方法大多基于固定的颜色阈值，这种方法简单，处理速度快，但由于空间光线变化的影响，故在目标识别和检测时的鲁棒性不尽人意。对于移动机器人准确快速实现路标的识别要求来说，重点在鲁棒性、实时性以及符合要求的精度要求。在视觉图像的处理程序中，一些简单的信息通过颜色的阈值分割边界联接而得到，如不同 的颜色通道(Channel)、相同颜色下一定数目的颜色区域也即色块(Blob)、色块的周边坐标信息(Top，Bottom、Left、Right)、色块的面积大小(Area)、色块中心坐标(Center of Gravity of the blob，X_CG Y_CG)等。为保证路标设定的鲁棒性，每个路标使用两种与环境差异较大的颜色Channel1和Channel2，使两色带宽度相同，两色带间距为一色带宽度，并对不同的路标加以编号，进行存储。通过色环的图像信息，实现机器人对路标的识别和对准。引入改进的灰度相关匹配法，优化了两色环中心垂直距离值。通过建立机器人坐标系和路标的成像模型，得到了路标的全局坐标，并最终引入粒子群算法对其优化。建立数学模型，完成了基于路标的机器人视觉重定位。 

具体采用如下方法来设计路标，设计带有两个不同颜色环色带的圆柱体作为机器人视觉导航的路标，两色环的颜色选取与周围环境差异较大的颜色，例如红色、绿色等，色环带的宽度和两色环带间距为一固定值。色环带的两两组合可以方便的区分不同的路标，路标设计成圆柱体可以保证机器人在各个角度都能获得无差别的路标图像，从而确保了机器人对路标识别的鲁棒性。对不同的路标加以编号并存储。 

在HSI颜色空间下，以H为主阈值，I为辅助阈值的阈值分割法和基于S分量边缘点数量统计的分割法相结合，实现对彩色路标的分割。对路标图像分割的目的是要分割出柱子上的两色块，并得到它们的面积、图像坐标等图像信息，以用于路标识别。根据我们路标设计的特点和环境的复杂性，我们选取以区域和边缘相结合的彩色图像分割方法。具体做法是在HSI颜色空间中以H分量为主阈值，S分量为辅助阈值对彩色图像进行阈值图像分割；对I分量图像首先用Canny算子进行边缘检测，然后用基于边缘点的数量统计分割出色环区域，将两幅分割图像合成，完成路标图像中对色块的最终分割。 

通过分割的色块采取以下相应的措施进行路标的识别： 

1)通过两个颜色块的面积大小相近小于某一阈值进行初步的路标判断，这样 在路标不完全进入摄像头视野时也不影响路标的识别，如根据公式|Area1-Area2|/(Area1+Area2)＜2％进行初步判定，其中，Area1和Area2分别为亮色块面积； 

2)采用水平方向的两色块的中心坐标之差(|x_CG1-x_CG2|)小于某一阈值Threshold作为路标识别的进一步判断，防止视野中相同颜色其他色块的干扰，如根据公式|x_CG1-x_CG2|＜Threshold进一步判断； 

3)根据两色块的颜色并与存储空间中已经编号的路标比对最终完成路标的识别。 

机器人可根据不同路标的色带组合依次寻找人工路标的柱子，找到后对准路标，并向其驶近直到机器人到路标的距离接近设定值。 

假设图2是路标通过摄像头在成像平面的投影，两色块的中心坐标为X_CG，摄像头中两色块垂直方向中心距离为ΔY_CG。由图3的路标成像模型可知，在移动机器人焦距f，路标的两色带中心距离W，以及摄像头中两色块垂直方向中心距离ΔY_CG已知的情况下，算法模块调用公式(1)得到移动机器人到路标的距离L： 

$L=\frac{W}{{\mathrm{\ΔY}}_{\mathrm{CG}}}f---\left(1\right)$

移动机器人在开始运动时即以初始行进的方向为x轴建立默认的坐标系，这个坐标系称为机器人坐标系。设置在机器人中的里程计等惯性器件实时检测机器人的当前坐标(X，Y)和旋转角度TH。根据距离L、当前坐标(X，Y)和旋转角度TH根据以下公式可以得到已知路标的坐标(PX，PY)： 

PX＝X+LcosTH    (2) 

PY＝Y+LsinTH    (3) 

机器人行进过程中，通过上述计算分别得到每一时刻路标的坐标(px_k，py_k)，由此确定机器人本身的坐标和旋转角度(x_k，y_k，th_k)，机器人与路标的距离d_k。 

机器人从开始驶近路标至停止会得到路标的一系列坐标值，从这一系列坐标中如何求出最恰当的是研究的关键。一般的方法是求这一系列坐标值的算术平均，作为路标的最优坐标值。本发明采用PSO(Particle SwarmOptimizer，粒子群最优值)算法确定最恰当的坐标值。 

PSO算法。PSO模拟鸟群的捕食行为，建立速度-位置(v-x)搜索模型，把优化问题空间转化为随机粒子空间，通过反复迭代寻找最优解。每一个备选解(机器人坐标和旋转角度)作为一个粒子，粒子的优劣程度由适应度函数F(x)决定。每一个粒子都由一个速度决定其更新的方向和大小，粒子们追随当前最优粒子通过迭代在解空间中搜索。每一次迭代，粒子通过跟踪两个极值更新自己的速度和位置：粒子本身所找到的最优解pBest和整个种群目前找到的最优解gBest。定义种群中存在n个粒子，每个粒子m维，其速度和位置的更新方法为： 

v_id(k+1)＝wv_id(k)+c₁r₁[p_id(k)-x_id(k)]+c₂r₂[g_d(k)-x_id(k)]    (4) 

x_id(k+1)＝x_id(k)+v_id(k+1)                                    (5) 

其中v_id(k)，x_id(k)分别表示粒子i(i＝1，2，...，n)第d(d＝1，2，...，m)维分量在k时刻的速度和位置；p_id(k)表示粒子i第d维分量到k时刻为止搜索到的最优位置；g_d(k)表示种群中所有粒子第d维分量到k时刻为止搜索到的最优位置；r₁，r₂为(0～1)之间的随机数；c₁，c₂为加速常数，表示每个粒子受pBest和gBest位置吸引的加速项的权重，一般取c₁＝c₂＝2；w为惯性权重，较大则算法具有较强的全局搜索能力，较小则算法倾向于局部搜索，一般是将随迭代次数线性减小，即 

$w=w_{\max }-\mathrm{iter}*\frac{w_{\max }-w_{\min }}{{\mathrm{iter}}_{\max }}---\left(6\right)$

其中：iter为当前迭代次数，iter_max为总的迭代次数，w_max＝0.9，w_max＝0.4。 

利用PSO算法确定路标的最优坐标值。应用PSO算法解决优化问题的两个重要步骤是：问题解的编码和适应度函数的选择。路标的最优坐标值(px，py)应 满足与机器人每时刻的距离和或角度和最接近已测量值，即位置坐标的距离适应度函数与旋转角度适应度函数最小， 

$\underset{k=1}{\overset{n}{\mathrm{\Σ}}}[{(\mathrm{py}-y_k)}^2+{(\mathrm{px}-x_k)}^2-d_k^2]---\left(7\right)$

或 $\underset{k=1}{\overset{n}{\mathrm{\Σ}}}[\frac{\mathrm{py}-y_k}{\mathrm{px}-x_k}-\tan {\mathrm{th}}_k]---\left(8\right)$

最小，其中，n为总的粒子数，k表示不同的时刻。 

以下以(7)式的距离适应度函数为例作为PSO算法的适应度函数确定路标的最优坐标值。具体包括如下步骤： 

Step1：根据设定值，依次寻找路标，并对准。 

Step2：驶近路标，记录每时刻路标的坐标(px_k，py_k)，机器人本身的坐标和旋转角度(x_k，y_k，th_k)，机器人与路标的距离d_k。 

Step3：将Step2中获取的上述信息作为粒子群优化算法的粒子。粒子的位置即为(px_k，py_k)，速度是位置的10％。粒子是2维的。从速度分量中分别找到每维的最大值vx_max和vy_max，作为边界约束值。 

Step4：算法模块调用公式(4)更新粒子的各维速度，并进行边界约束(即超过[-v_max，v_max]的值以边界值代替)。算法模块调用公式(5)更新粒子的各维位置。 

Step5：算法模块调用公式(7)求每一个粒子的距离适应度函数，更新p_ij(k)和g_j(k)。 

Step6：返回Step4进行迭代，直到达到最大迭代次数iter_max或满足精度要求(收敛)。 

下面以一具体实例，说明随着里程计误差的增大，机器人根据已获得的路标坐标来实现自身的视觉重定位过程。如图4所示，移动机器人从起始点A(x1，y1)开始寻找路标P₁(X1，Y1)，当它找到后，对准并驶向P₁，直到距离P₁ 到指定的长度。接着机器人寻找目标P₂(X2，Y2)，并重复以上过程。如果机器人的行驶路径是由A(x₁，y₁)到B(x₂，y₂)再到C(x₃，y₃)。由里程计测量得到线段BC的长度。由图像处理可以得到机器人从B和C到相应坐标的距离。 

这时机器人在C点的坐标可由公式(9)得到： 

X₃＝X₂-L₂ cos(β+α) 

Y₃＝T₂-L₂ sin(β+α)    (9) 

θ_C＝β+α 

其中α和β为中间变量，具体含义如图4所示。算法模块调用公式(10) 

$\mathrm{\α}={\cos }^{-1}\frac{L^2+{(D+L_2)}^2-L_1^2}{2L(D+L_2)}$

$\mathrm{\β}={\tan }^{-1}\frac{Y_2-Y_1}{X_2-X_1}$

计算得到： $D=\sqrt{{(x_3-x_2)}^2+{(y_3-y_2)}^2}---\left(10\right)$

$L=\sqrt{{(X_1-X_2)}^2+{(Y_1-Y_2)}^2}$

其中D为B点到C点的距离，L为路标P₁到路标P₂的距离。 

Claims (2)

1.一种基于颜色路标的移动机器人视觉导航方法，其特征在于，设计带有两个不同颜色环的圆柱体作为机器人视觉导航的路标，对不同的路标加以编号并存储；机器人根据不同路标的颜色环组合依次寻找并识别路标，根据摄像头中两颜色环垂直方向中心距离ΔY_CG、机器人焦距f、路标的两不同颜色环中心距离W，算法模块根据公式

实时计算机器人到路标的距离L；以机器人初始行进方向为X轴建立坐标系为机器人坐标系，里程计实时检测机器人的当前坐标和旋转角度（X，Y，TH），机器人行进过程中，通过算法模块调用公式：PX＝X+LcosTH、PY＝Y+LsinTH，分别得到在机器人坐标系中每一时刻路标的坐标；根据每时刻路标坐标、机器人坐标和旋转角度、机器人与路标的距离，算法模块建立速度-位置搜索模型，机器人坐标和旋转角度作为一个粒子，粒子的优劣程度由适应度函数F(x)决定，每一个粒子都由一个速度决定其更新的方向和大小，粒子们追随当前最优粒子通过迭代在解空间中搜索，每一次迭代，粒子通过跟踪两个极值更新自己的速度和位置：粒子本身所找到的最优解pBest和整个种群目前找到的最优解gBest，定义种群中存在n个粒子，每个粒子m维，根据公式：v_id(k+1)=wv_id(k)+c₁r₁[p_id(k)-x_id(k)]+c₂r₂[g_d(k)-x_id(k)]，x_id(k+1)=x_id(k)+v_id(k+1)更新，通过反复迭代寻找最优坐标值，其中v_id(k)，x_id(k)分别表示粒子i(i＝1,2,...,n)第d(d=1,2,...,m)维分量在k时刻的速度和位置，p_id(k)表示粒子i第d维分量到k时刻为止搜索到的最优位置，g_d(k)表示种群中所有粒子第d维分量到k时刻为止搜索到的最优位置，r₁,r₂为(0～1)之间的随机数；c₁,c₂为加速常数，w为惯性权重，w随迭代次数线性减小，即

其中：iter为当前迭代次数，iter_max为总的迭代次数，w_max=0.9，w_min=0.4。

2.根据权利要求1所述的移动机器人视觉导航方法，其特征在于，寻找并识别路标的步骤进一步包括，在HSI颜色空间下，采用以H为主阈值，I为辅助阈值的阈值分割法和基于S分量边缘点数量统计的分割法相结合，分割路标上的两个不同颜色环，并获取它们的面积、图像坐标。

Images