CN111823237B - 基于rgb led动态信标的多机器人定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于RGB LED动态信标的多机器人定位方法及系统，通过采用外置的相机捕捉机器人上安装的RGB LED灯，通过RGB LED灯交替闪烁红、绿、蓝三种颜色的不同排列组合，来对RGB LED灯进行编码；再通过已经标定好外参和内参的相机来捕捉与跟踪上述LED灯，并且逐一识别其编码信息，通过相机参数可以解出机器人的位置，同时根据LED编码信息能够在多机器人工作条件下对机器人加以区分。即本发明采用动态的LED视觉信标，在无遮挡的情况下，将有编码信息的信标体积缩小到了一个点的大小，相比于其他静态的视觉信标在体积上有明显优势；在可视角度上，LED是全方位的，相比于二维码式的视觉信标也是有优势的，具有结构简单、信息识别量少、定位精准的优点。

Description

基于RGB LED动态信标的多机器人定位方法及系统

技术领域

本发明属于机器人定位技术领域，具体涉及一种基于RGB LED动态信标的多机器人定位方法及系统。

背景技术

现有的机器人定位技术主要分为两大类，第一类是相对定位：移动机器人的相对定位也称作位姿跟踪，假定机器人的初始位姿，采用相邻时刻的传感器信息对机器人的位置信息进行跟踪估计。常用的相对定位法有里程计法和惯性导航法。第二类是绝对定位：移动机器人的绝对定位又称为全局定位。完成机器人的全局定位需要预先确定好环境模型或者通过外部传感器直接向机器人提供位置信息，计算机器人在全局坐标系中的位置。全局定位的主要方法包括导航信标、图形匹配、GPS、概率定位以及结合了导航信标与GPS的思想，将信标放置在机器人上，由已知位置的外部传感器捕捉机器人上的信标，通过三角定位或者某些线性约束来得知机器人的位置坐标。列举的最后一种方法诸如基于无线的uwb定位以及基于视觉的分布式定位。基于分布式的视觉定位方法，主要是采用位置姿态已知的图像传感器，来识别捕捉机器人上的视觉信标，再经过线性约束的计算，得到机器人的世界坐标。线性约束有几种：图像传感器是深度相机，直接可以得到像素的深度信息来作为约束；机器人只在平面内移动，该平面也可以作为线性约束；图像传感器有多个，则多余的图像传感器捕捉到的相同的机器人视觉信标也可以作为线性约束。视觉信标分为带编码信息的与不带编码信息的，例如颜色空间信息与差分来探测运动目标是不带编码信息的，而QR码与ARTag码是带编码信息的。在一副二维图像中，视觉信标需要能够提供足够精确的像素坐标信息，以及足够的识别度来消除环境干扰。对于不带编码信息的视觉信标，其都有识别度差、无法定位与区分多个信标等缺点。如果要做多机器人定位，那么则无法使用这类信标。对于上述的ARTag等带编码信息的视觉信标，则要占据大量的机器人的顶部空间，且需要一个大平台，极大的影响了机器人的结构。

发明内容

鉴于此，本发明的目的在于避免现有技术中的不足而提供一种基于RGB LED动态信标的多机器人定位方法及系统，通过采用外置的相机捕捉机器人上安装的RGB LED灯，通过RGB LED灯交替闪烁红、绿、蓝三种颜色的不同排列组合，来对RGB LED灯进行编码；再通过已经标定好外参和内参的相机来捕捉与跟踪上述LED灯，并且逐一识别其编码信息，通过相机参数可以解出机器人的位置，同时根据LED编码信息能够在多机器人工作条件下对机器人加以区分，具有结构简单、定位精准的优点。

本发明的目的之一通过以下技术方案实现：提供一种基于RGB LED动态信标的多机器人定位方法，包括以下步骤：

S1、通过固定于机器人上方的相机进行标定，得到相机的外参与内参；

S2、开启所有机器人，且所有机器人上均设置有可控的RGB LED灯；

S3、每个机器人根据其自身存储的ID号，以一定频率与颜色顺序点亮RGB LED灯；

S4、通过计算机得到由相机捕获的图像序列，并经过LED识别跟踪算法处理得到所有机器人的RGB LED灯在图像中的像素位置坐标以及ID号；

S5、得到像素位置坐标后利用线性约束得到所有机器人的位置坐标。

作为进一步的改进，所述步骤S4中LED识别跟踪算法具体分为如下过程：

S41、算法初始化：初始化的同时让所有RGB LED灯都被相机捕捉到，并通过计算机读取输入的图像序列；

S42、创建TB：根据RGB LED灯的数量，针对每一个RGB LED灯的位置创建一个TB；

S42B、开启TB跟踪线程：针对每一个TB做LED跟踪，使LED能够一直处在TB中心，并且有着读取LED编码信息的能力；

S42’、开启重定位线程：针对丢失状态的TB，重新在全图中找回来，并恢复该TB。

作为进一步的改进，所述TB包括四种状态，分别为准备状态、正常状态、疑似丢失状态和丢失状态。

作为进一步的改进，所述步骤S41具体表现为：

S410、初始化开始；

S411、输入新的一帧图像；

S412、根据压缩比来改变图像；

S413、通过R-G图像差分，得到二值图像；

S414、对得到的二值图像进行轮廓检测，并找出对应轮廓的面积、中心点；

S415、判断是否检测到轮廓，若是，则进入步骤S416；反之，则进入步骤S417；

S416、判断检测是否超过20次，若是，则对历史存储的所有轮廓中心点进行聚类，并根据聚类结创建TB；反之，则存储当前轮廓中心点与面积，并将返回值数为false；

S417、更新帧缓存；

S418、初始化结束。

作为进一步的改进，所述步骤S416中根据聚类结创建TB的具体过程如下：

1)聚类创建BT开始；

2)整合所有存储的轮廓点与其面积；

3)计算轮廓平均面积Sav；

4)设计TB的跟踪框大小，其表达式为：

其中，Ks表示系数；

5)计算轮廓中心点聚类邻域距离，其表达式为：

6)聚类结果过滤；

7)创建TB，并将返回值设为ture；

8)聚类创建TB结束。

作为进一步的改进，所述步骤S42B具体表现为：

S42B0、TB跟踪程序开始后根据TB框的位置大小，在输入的图像序列中截取一个方框作为跟踪的处理图片；

S42B1、判断TB状态是否为丢失状态，若是，则设置读编码状态为FINISH，并记录当前帧读取字符为N,然后进入步骤S42B9；反之，则进入步骤S42B2；

S42B2、进行HSV识别与轮廓点检测，分别检测出R、G、B颜色的轮廓点；

S42B3、找出所有轮廓点中离TB图像中心最近的点，并通过运动学滤波，得到移动向量与颜色字符；

S42B4、根据移动向量更新TB在中心点；

S42B5、查看TB框是否会越界，若是，则更新TB状态为丢失状态，然后进入步骤S42B9；反之，则进入步骤S42B6；

S42B6、判断TB状态是否为准备状态，若是，则准备状态处理，然后进入步骤S42B9；反之，则进入步骤S42B7；

S42B7、判断TB状态是否为正常状态，若是，则正常状态处理，然后进入步骤S42B9；反之，则进入步骤S42B8；

S42B8、判断TB状态是否为疑似丢失状态，若是，则疑似丢失处理，然后进入步骤S42B9；反之，则直接进入步骤S42B9；

S42B9、更新缓存记录当前帧的各种状态，之后，TB跟踪程序结束。

作为进一步的改进，所述TB为准备状态时，准备状态处理过程具体如下：

A0)准备状态处理开始；

A1)判断是否到了读取时间，若是则进入步骤A2)；反之，则读取加速器+1，然后，进入步骤A8)；

A2)判断读取字符是否为G或B，若是，则进入步骤A3)；反之，则读取错误，然后，进入步骤A8)；

A3)读取写入1或0，其中，G为0，B为1；

A4)判断是否完成读取所有字符，若是，则进入步骤A5)；反之，则进入步骤A8)；

A5)设置读编码状态为FINISH；

A6)TB状态设为正常状态，记录当前帧字符为N，为下一帧做准备；

A7)计算ID，并且记录TB的ID号；

A8)准备状态处理结束。

作为进一步的改进，所述TB为正常状态时，正常状态处理过程具体如下：

B0)准备状态开始处理；

B1)判断当前TB的读编码状态是否为FINISH，若是，则进入步骤B2)；反之则进入步骤B2’)；

B2)判断上一帧字符是否为R，当前帧字符是否为G，若是，则设置读编码状态为READING，然后进入步骤B4)；反之，则进入步骤B3)；

B3)判断当前帧字符是否为N，若是，则TB状态设为疑似丢失状态，然后，进入步骤B4)；反之，则直接进入步骤B4)；

B2’)判断读码编码状态是否为READING，若是，则进入步骤B3’)；反之，则进入步骤B4)；

B3’)判断是否到了读取时间，若是则进入步骤B4’)；反之，则读取加速器+1，然后，进入步骤B4)；

B4’)判断读取字符是否为G或B，若是，则进入步骤B5’)；反之，则TB状态设为疑似丢失状态，然后，进入步骤B4)；

B5’)读取当前帧字符为1或0，并写入编码队列，其中，G为0，B为1；

B6’)判断是否完成读取所有字符，若是，则进入步骤B7’)，反之，则进入步骤B4)；

B7’)根据编码队列计算ID；

B8’)判断是否与原ID一样，若是，则进入步骤B9’)；反之，则判断TB状态为丢失状态，之后，进入步骤B10’)；

B9’)TB状态设为正常状态；

B10’)读编码状态设为FINISH，并记录这一帧的字符为N，之后，进入步骤B4)；

B4)正常状态处理结束。

作为进一步的改进，所述TB为疑似丢失状态时，疑似丢失状态处理过程具体如下：

C0)疑似丢失处理开始；

C1)判断当前TB的读编码状态是否为FINISH，若是，则进入步骤C2)，反之则进入步骤C2’)；

C2)判断上一帧字符是否为N，当前帧字符是否为R，若是，则TB状态转为正常状态，然后进入步骤C5)，反之，则进入步骤C3)；

C3)判断上一帧字符是否为R，当前帧字符是否为G，若是，则设置读码编码状态是否为READING，TB状态转为正常状态，然后，进入步骤C5)；反之，则进入步骤C4)；

C4)判断当前帧字符是否为N，若是，则TB状态设为丢失状态，然后，进入步骤C5)；反之，则直接进入步骤C5)；

C2’)读码编码状态是否为READING，若是，则进入步骤C3’)；反之，则进入步骤C5)；

C3’)判断是否到了读取时间，若是则进入步骤C4’)；反之，则读取加速器+1，然后，进入步骤C4″)；

C4’)判断读取字符是否为G或B，若是，则进入步骤C5’)；反之，则TB状态设为疑似丢失状态，然后，进入步骤C5)；

C5’)读取当前帧字符为1或0，并写入编码队列，其中，G为0，B为1,且TB状态设为正常状态；

C6’)判断是否完成读取所有字符，若是，则进入步骤C7’)，反之，则进入步骤C5)；

C7’)根据编码队列计算ID；

C8’)判断是否与原ID一样，若是，则进入步骤C9’)；反之，则判断TB状态为丢失状态，之后，进入步骤C10’)；

C9’)TB状态设为正常状态；

C10’)读编码状态设为FINISH，并记录这一帧的字符为N，之后，进入步骤C5)；

C4″)判断读取字符是否为B或G，若是，则TB状态转为正常状态，之后，进入步骤C5)；反之，则直接进入步骤C5)；

C5)正常状态处理结束。

本发明的目的之二通过以下技术方案实现：提供一种基于RGB LED动态信标的多机器人定位系统，采用其上任一项所述的基于RGB LED动态信标的多机器人定位方法进行定位，所述定位系统包括计算机、机器人和相机：相机设置于机器人的上方，用于捕捉机器人；所述计算机分别与相机和机器人无线连接，所述计算机用于图像处理与控制机器人；所述机器人上设置有RGB LED灯，且RGB LED灯的亮度与颜色通过机器人进行调节与控制；所述机器人和相机的数量均为多个。

综上所述，本发明通过采用外置的相机捕捉机器人上安装的RGB LED灯，通过RGBLED灯交替闪烁红、绿、蓝三种颜色的不同排列组合，来对RGB LED灯进行编码；再通过已经标定好外参和内参的相机来捕捉与跟踪上述LED灯，并且逐一识别其编码信息，通过相机参数可以解出机器人的位置，同时根据LED编码信息能够在多机器人工作条件下对机器人加以区分。即本发明采用动态的LED视觉信标，在无遮挡的情况下，将有编码信息的信标体积缩小到了一个点的大小，相比于其他静态的视觉信标：二维码式、标记球式，在体积上有明显优势；在可视角度上，LED是全方位的，相比于二维码式的视觉信标也是有优势的，具有结构简单、信息识别量少、定位精准的优点。

附图说明

利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。

图1是基于RGB LED动态信标的多机器人定位方法的流程图。

图2是图1的步骤S4的分解流程图。

图3是LED识别跟踪算法的原理图。

图4是LED识别跟踪算法初始化流程图。

图5是聚类结创建TB的流程图。

图6是TB跟踪程序流程图。

图7是TB准备状态处理过程的流程图。

图8是TB正常状态处理过程的流程图。

图9是TB疑似丢失状态处理过程的流程图。

图10重定位线程的流程图。

图11是基于RGB LED动态信标的多机器人定位系统的结构示意图。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步详细的描述，需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

需要说明的是，为更深入地理解本发明特对以下词语做出解释：

RGB LED:红绿蓝发光二极管(Red Green Blue Light-Emitting Diode)，其中，R表示红色，G表示绿色，B表示蓝色；

TB:跟踪框(Tracking Block)；

准备状态(prepare)：刚创建TB时候的状态；

正常状态(OK)：正常跟踪的状态，其主要任务为LED跟踪和检查编码信息；

疑似丢失状态(suspected_lost)：怀疑TB跟丢了LED，但未完全确定，需再次检测，如果再次检测不到，那么则认为真丢失了；

丢失状态(losing)：TB彻底的跟丢了LED；

字符：TB是一个罩着LED的方框，下一帧的LED一般也会出现在方框内，读取当前帧该TB中的LED的颜色作为一个字符，字符可以的取值为R、G、B、N，其中，R、G、B分别代表红、绿、蓝颜色，N代表没有检测到RGB中的任何一个颜色；

读编码状态：读编码状态的值设立为READING和FINISH，读编码的时间比较长、不是几帧就能完成的，所以设立该值为READING的时候就是在读取编码的状态，FINISH的时候就是已经完成了一轮的读取编码，这时候应该检测到的颜色全是红色，如果检测到红到绿的跳变，那么又进入到了READING的状态；

编码或编码信息：检测到R-G跳变后，以G为0，R为1得到一串由01组成的串；

如图1所示，本发明实施例提供的基于RGB LED动态信标的多机器人定位方法，包括以下步骤：

S1、通过固定于机器人上方的相机进行标定，得到相机的外参与内参；需要说明的是，相机标定要选采用张正友标定法，张正友标定法是指张正友教授1998年提出的单平面棋盘格的摄像机标定方法，张正友标定法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以，同时也相对于自标定而言，提高了精度，便于操作，因此张正友标定法被广泛应用于计算机视觉方面。

S2、开启所有机器人，且所有机器人上均设置有可控的RGB LED灯。

S3、每个机器人根据其自身存储的ID号，以一定频率与颜色顺序点亮RGB LED灯；本实施例中，设定以红色(R)到绿色(G)的跳变作为起始颜色，以绿色作为信号0，以蓝色作为信号1，若相机帧率为60，则每100毫秒改变一次LED颜色。例如，若机器人ID号是一个8bit数为57，写作二进制就是00111001。机器人就控制RGB LED灯每100ms改变一次颜色：红绿绿 绿蓝蓝蓝绿绿蓝红绿绿绿蓝蓝蓝绿绿蓝红绿绿绿蓝蓝………，下划线的地方代表着机器人的ID号00111001，红绿则是起始信号。

S4、通过计算机得到由相机捕获的图像序列，并经过LED识别跟踪算法处理得到所有机器人的RGB LED灯在图像中的像素位置坐标以及ID号；优选地，参照图2和图3，本步骤中LED识别跟踪算法具体分为如下过程：

S41、算法初始化：初始化的同时让所有RGB LED灯都被相机捕捉到(即出现在相机视野范围内)，并通过计算机读取输入的图像序列；其中，该图片序列是按照指定时间周期输入的，且初始化期间机器人与场景都不能运；

具体地，参见图4，算法初始化包括如下步骤：

S410、初始化开始；

S411、输入新的一帧图像；

S412、根据压缩比来改变图像；

S413、通过R-G图像差分，得到二值图像；需要说明的是，R-G图像差分是指算法算法初始化开始后，扫描全图，将上一帧图像进行HSV(Hue,Saturation,Value，色相、饱和度、明度)红色分割并且进行膨胀操作，将当前帧图像进行HSV绿颜色分割，将两帧图像进行‘与’操作，得到了二值图像，即找出那些上一帧还是红色下一帧就变成了绿色的地方。

S414、对得到的二值图像进行轮廓检测，并找出对应轮廓的面积、中心点；

S415、判断是否检测到轮廓，若是，则进入步骤S416；反之，则进入步骤S417；

S416、判断检测是否超过20次，若是，则对历史存储的所有轮廓中心点进行聚类，并根据聚类结创建TB；反之，则存储当前轮廓中心点与面积，并将返回值数为false；

S417、更新帧缓存；

S418、初始化结束。

S42、创建TB：根据RGB LED灯的数量，针对每一个RGB LED灯的位置创建一个TB；需要说明的是，TB是一个框住LED的方框，其目的就是跟踪LED，使其一直在TB的中心；

S42B、开启TB跟踪线程：针对每一个TB做LED跟踪，使LED能够一直处在TB中心，并且有着读取LED编码信息的能力；

S42’、开启重定位线程：针对丢失状态的TB，重新在全图中找回来，并恢复该TB。具体地，本步骤中若某些TB因为读取的编码信息与之前的不符，或者未检测到亮灯等情况，TB就会处于丢失状态，重定位线程就会开始处理处于丢失状态的TB，重新在全图中找回来，并恢复该TB。

S5、得到像素位置坐标后利用线性约束得到所有机器人的位置坐标。优选地，本实施例中线性约束限定为机器人只在地平面上运动或设置多个相机。

作为进一步优选的实施方式，步骤S416的聚类过程中聚类的邻域距离以2倍轮廓平均面积的平方根为准，聚类后过滤掉那些轮廓面积过于小的点(有些只有一个像素的点大概率是噪声)，并且那些相对孤立的点也会被过滤掉(这个取决于机器人的多少，如果机器人少，那么同一个LED就会被检测很多次以上)，最后对于剩下的聚类点，聚类多少类就说明有多少TB，根据他们的位置创建TB，具体地，参见图5根据聚类结创建TB的过程如下：

1)聚类创建BT开始；

2)整合所有存储的轮廓点与其面积；

3)计算轮廓平均面积Sav；

4)设计TB的跟踪框大小，其表达式为：

其中，Ks表示系数；需要说明的是，Ks可以由用户自行调整，默认值为11，若值调大点，跟踪框TB就大一点；

5)计算轮廓中心点聚类邻域距离，其表达式为：

6)聚类结果过滤；

7)创建TB，并将返回值设为ture；

8)聚类创建TB结束。

此外，在进一步的技术方案中，本发明中TB具体包括四种状态，分别为准备状态、正常状态、疑似丢失状态和丢失状态。具体地：针对准备状态，初始化只完成了检测起始信号----红到绿的跳变，目的是得到LED的位置，但是还没有读取其编码信息(即机器人ID)。处于准备状态的TB需要通过TB跟踪线程来完成两个操作，一是跟踪，读取编码信息的时间较长需要800ms，在此期间难保机器人不会运动，因此需要做到跟踪LED；二是读取编码，由于相机帧数和LED变化频率均已知，所以可以直接隔6帧采样一次，采样8次后完成读取编码的操作。得到的编码应检查一下是否在机器人群的ID范围内，若在，则确定这个8bit数字就是这个LED的编码信息，同时状态应该转变为正常状态(OK状态)。针对正常状态，其主要任务为LED跟踪，与准备状态的LED跟踪方法一样；还有一个任务就是检查编码信息，和准备状态读取编码的方法一样，不过这次得到8bit数字后是与该TB对应的LED编码检查是否一样，若一样说明跟踪是成功的那么保持不变继续跟踪；若不一样，则转换为丢失状态。(需要说明的是，若是上面跟踪失败，即在当前帧找不到合适的LED点，那么状态是转为疑似丢失状态，但是由于编码信息不一样造成的丢失，则说明在之前的800ms中不知哪里出错，所以直接变为丢失状态更靠谱)。针对疑似丢失状态，鉴于跟踪的过程中存在一些瞬间的干扰，导致LED在变换颜色的时候刚好被捕捉到，这时候出现的颜色有可能是黄色(红变绿的时候)，也有可能是青色(绿变蓝的时候)，此时，出现在TB图像中的颜色就不是红绿蓝任何一个颜色，也有可能导致当前帧跟踪失效，这时候如果直接丢失，要求重定位得不偿失，故采取了一个疑似丢失状态的缓存区域，如果再次检测不到，那么就会真丢失了。针对丢失状态，TB彻底的跟丢了LED，这时候重定位线程如果检测到存在丢失的TB，就是开始在全图扫描，方法和初始化的时候的方法一样。

本发明中跟踪线程会对每一个TB都执行跟踪，TB跟踪线程的流程图如图6所示，具体分为以下步骤：

S42B0、TB跟踪程序开始后根据TB框的位置大小，在输入的图像序列中截取一个方框作为跟踪的处理图片；需要说明的是，跟踪主要在这个图像区域内完成，计算量得以减小；

S42B1、判断TB状态是否为丢失状态，若是，则设置读编码状态为FINISH，并记录当前帧读取字符为N，然后进入步骤S42B9；反之，则进入步骤S42B2，即表明可以开始跟踪LED了；

需要说明的是，上一帧跟踪后的LED一定是处于TB框中间的，如果移动速度不快的话，这一帧的LED位置应该是挨着TB框的中心点的；

S42B2、进行HSV识别与轮廓点检测，分别检测出R、G、B颜色的轮廓点；

S42B3、找出所有轮廓点中离TB图像中心最近的点(这个点应该是所跟踪的LED在这一帧的位置)，并通过运动学滤波，得到移动向量与颜色字符；

S42B4、根据移动向量更新TB在中心点；

S42B5、查看TB框是否会越界，若是，则更新TB状态为丢失状态，然后进入步骤S42B9；反之，则进入步骤S42B6；

S42B6、判断TB状态是否为准备状态，若是，则准备状态处理，然后进入步骤S42B9；反之，则进入步骤S42B7；

S42B7、判断TB状态是否为正常状态，若是，则正常状态处理，然后进入步骤S42B9；反之，则进入步骤S42B8；

S42B8、判断TB状态是否为疑似丢失状态，若是，则疑似丢失处理，然后进入步骤S42B9；反之，则直接进入步骤S42B9；

S42B9、更新缓存记录当前帧的各种状态，比如保存这一帧中的各种状态以及信息、图片，作为以后的历史帧参考，之后，TB跟踪程序结束。

前文对TB是否是准备状态、正常状态和疑似丢失状态就行了判断，此处，对准备状态处理的过程、正常状态处理的过程和疑似丢失状态处理的过程具体展开论述(主要工作基本都是编码信息的读取)：

参见图7，若TB为准备状态时，准备状态处理过程具体如下：

A0)准备状态处理开始；

A1)判断是否到了读取时间，若是则进入步骤A2)；反之，则读取加速器+1，然后，进入步骤A8)；例如，可以设定每隔6帧读取一次字符(在60帧，led变化周期为100ms下)；

A2)判断读取字符是否为G或B，若是，则进入步骤A3)；反之，则读取错误，然后，进入步骤A8)；需要说明的是，尽管字符有4种取值可能R、G、B、N，但只有字符为B或G时才有效，否则会提示读取错误；

A3)读取写入1或0，其中，G为0，B为1；

A4)判断是否完成读取所有字符，若是，则进入步骤A5)；反之，则进入步骤A8)；例如，8bit的编码只有8位(可以随意定义，不一定非得是8位，越长的字串需要的读取时间越久)；

A5)设置读编码状态为FINISH；

A6)TB状态设为正常状态，记录当前帧字符为N，为下一帧做准备；

A7)计算ID，并且记录TB的ID号，即将编码换算成数字ID写入该TB的ID信息；

A8)准备状态处理结束。

参见图8，若TB为正常状态时，正常状态处理过程具体如下：

B0)准备状态开始处理；

B1)判断当前TB的读编码状态是否为FINISH，若是，则进入步骤B2)；反之则进入步骤B2’)；需要说明的是，FINISH阶段检测到的字符应该全是红色，若检测到了上一帧是红色这一帧变成了绿色，则设置读编码状态为READING；

B2)判断上一帧字符是否为R，当前帧字符是否为G，若是，则设置读编码状态为READING，然后进入步骤B4)；反之，则进入步骤B3)；

B3)判断当前帧字符是否为N，若是，则TB状态设为疑似丢失状态，然后，进入步骤B4)；反之，则直接进入步骤B4)；

B2’)判断读码编码状态是否为READING，若是，则进入步骤B3’)；反之，则进入步骤B4)；

B3’)判断是否到了读取时间，若是则进入步骤B4’)；反之，则读取加速器+1，然后，进入步骤B4)；

B4’)判断读取字符是否为G或B，若是，则进入步骤B5’)；反之，则TB状态设为疑似丢失状态，然后，进入步骤B4)；

B5’)读取当前帧字符为1或0，并写入编码队列，其中，G为0，B为1；

B6’)判断是否完成读取所有字符，若是，则进入步骤B7’)，反之，则进入步骤B4)；

B7’)根据编码队列计算ID；

B8’)判断是否与原ID一样，若是，则进入步骤B9’)；反之，则判断TB状态为丢失状态，之后，进入步骤B10’)；

B9’)TB状态设为正常状态；

B10’)读编码状态设为FINISH，并记录这一帧的字符为N，之后，进入步骤B4)；

B4)正常状态处理结束。

从其上过程可知，READING阶段与准备状态的流程差不多，只是如果当前字符不是B或G的话，那么状态会转为疑似丢失。读取到的ID会与原来的ID做比较，如果一样状态设为OK继续。如果不一样直接丢失,最后设置读编码状态为FINISH。

参见图9，若TB为疑似丢失状态时，疑似丢失状态处理过程具体如下：

C0)疑似丢失处理开始；

C1)判断当前TB的读编码状态是否为FINISH，若是，则进入步骤C2)，反之则进入步骤C2’)；

C2)判断上一帧字符是否为N，当前帧字符是否为R，若是，则TB状态转为正常状态，然后进入步骤C5)，反之，则进入步骤C3)；

C3)判断上一帧字符是否为R，当前帧字符是否为G，若是，则设置读码编码状态是否为READING，TB状态转为正常状态，然后，进入步骤C5)；反之，则进入步骤C4)；

C4)判断当前帧字符是否为N，若是，则TB状态设为丢失状态，然后，进入步骤C5)；反之，则直接进入步骤C5)；

C2’)读码编码状态是否为READING，若是，则进入步骤C3’)；反之，则进入步骤C5)；

C3’)判断是否到了读取时间，若是则进入步骤C4’)；反之，则读取加速器+1，然后，进入步骤C4″)；

C4’)判断读取字符是否为G或B，若是，则进入步骤C5’)；反之，则TB状态设为疑似丢失状态，然后，进入步骤C5)；

C5’)读取当前帧字符为1或0，并写入编码队列，其中，G为0，B为1,且TB状态设为正常状态；

C6’)判断是否完成读取所有字符，若是，则进入步骤C7’)，反之，则进入步骤C5)；

C7’)根据编码队列计算ID；

C8’)判断是否与原ID一样，若是，则进入步骤C9’)；反之，则判断TB状态为丢失状态，之后，进入步骤C10’)；

C9’)TB状态设为正常状态；

C10’)读编码状态设为FINISH，并记录这一帧的字符为N，之后，进入步骤C5)；

C4″)判断读取字符是否为B或G，若是，则TB状态转为正常状态，之后，进入步骤C5)；反之，则直接进入步骤C5)；

C5)正常状态处理结束。

从其上过程可以看出，疑似丢失状态的流程与正常状态的流程一样，只不过会在一些地方加上“TB状态转为OK”，以保证能够有马上恢复正常状态的能力，原来的“状态转为疑似丢失”会变为“状态转为丢失”。

此外，值得提及的是，本发明中提供重定位线程是系统初始化另外开出的线程。线程开始会每隔1ms检测一次是否有新的一帧输入系统，如果有那么从主线程那边取得系统信息，包括图像以及所有TB的信息。然后会检查里面是否有处于丢失状态的TB，如果存在那么就开始进行全图扫描，全图扫描流程与初始化的图4类似，返回状态给status变量。不同的是重定位的全图扫描在原图的基础上要除去正常的TB框，以免把正常的TB框检测进去浪费资源。同时由于丢失的机器人不一定是静止的，所以不能检测20次了，检测2次就足够了。还有一点不同的是创建的TB框应当另外算，定义为sTB，sTB是重定位线程的变量，与主线程的TB要区分开来，sTB作用只有读取编码信息，以及在恢复TB之前短暂的跟踪。在完成了全盘扫描后，下一帧图片就要开始用作与sTB了，与主TB功能一样sTB创建开始就处于准备状态，并开始一帧一帧的跟踪分析，等到完成编码信息读取，进入到OK状态的时候，就可以按照ID来恢复主线程丢失的TB了，具体表现为：根据得到的sTB的ID来恢复相应的主流程中的TB。恢复了之后应当删除该sTB。当所有TB都恢复了之后status重新置为0。具体流程参见图10。

综上所述，本发明通过采用外置的相机捕捉机器人上安装的RGB LED灯，通过RGBLED灯交替闪烁红、绿、蓝三种颜色的不同排列组合，来对RGB LED灯进行编码；再通过已经标定好外参和内参的相机来捕捉与跟踪上述LED灯，并且逐一识别其编码信息，通过相机参数可以解出机器人的位置，同时根据LED编码信息能够在多机器人工作条件下对机器人加以区分。即本发明采用动态的LED视觉信标，在无遮挡的情况下，将有编码信息的信标体积缩小到了一个点的大小，相比于其他静态的视觉信标：二维码式、标记球式，在体积上有明显优势；在可视角度上，LED是全方位的，相比于二维码式的视觉信标也是有优势的，具有结构简单、信息识别量少、定位精准的优点。

另一方面，如图11所示，本发明还提供基于RGB LED动态信标的多机器人定位系统，采用其上所述的基于RGB LED动态信标的多机器人定位方法进行定位，所述定位系统包括计算机、机器人1和相机2：相机2设置于机器人1的上方，用于捕捉机器人1；计算机分别与相机2和机器人1无线连接，计算机用于图像处理与控制机器人，优选地，计算机与相机2通过本地局域网连接；机器人1上设置有RGB LED灯3，且RGB LED灯3的亮度与颜色通过机器人1进行调节与控制；所述机器人1和相机2的数量均为多个。需要说明的是，每个机器人1上可设置有一个、两个或更多个RGB LED灯3，当只需要知道机器人1位置不需要知道机器人1姿态时只需要一个RGB LED灯3即可，当既需要知道机器人1位置，又需要知道机器人1姿态时，每个机器人1上至少需要安装两个RGB LED灯3。该基于RGB LED动态信标的多机器人定位系统显然具有结构简单、信息识别量少、定位精准的优点。

上面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，不能理解为对本发明保护范围的限制。

总之，本发明虽然列举了上述优选实施方式，但是应该说明，虽然本领域的技术人员可以进行各种变化和改型，除非这样的变化和改型偏离了本发明的范围，否则都应该包括在本发明的保护范围内。

Claims (9)

1.基于RGB LED动态信标的多机器人定位方法，其特征在于，包括以下步骤：

S1、通过固定于机器人上方的相机进行标定，得到相机的外参与内参；

S2、开启所有机器人，且所有机器人上均设置有可控的RGB LED灯；

S3、每个机器人根据其自身存储的ID号，以一定频率与颜色顺序点亮RGB LED灯；

S4、通过计算机得到由相机捕获的图像序列，并经过LED识别跟踪算法处理得到所有机器人的RGB LED灯在图像中的像素位置坐标以及ID号，所述LED识别跟踪算法具体分为如下过程：

S41、算法初始化：初始化的同时让所有RGB LED灯都被相机捕捉到，并通过计算机读取输入的图像序列；

S42A、创建TB：根据RGB LED灯的数量，针对每一个RGB LED灯的位置创建一个TB；

S42B、开启TB跟踪线程：针对每一个TB做LED跟踪，使LED能够一直处在TB中心，并且有着读取LED编码信息的能力；

S42’、开启重定位线程：针对丢失状态的TB，重新在全图中找回来，并恢复该TB；

S5、得到像素位置坐标后利用线性约束得到所有机器人的位置坐标。

2.根据权利要求1所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述TB包括四种状态，分别为准备状态、正常状态、疑似丢失状态和丢失状态。

3.根据权利要求2所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述步骤S41具体表现为：

S410、初始化开始；

S411、输入新的一帧图像；

S412、根据压缩比来改变图像；

S413、通过R-G图像差分，得到二值图像；

S414、对得到的二值图像进行轮廓检测，并找出对应轮廓的面积、中心点；

S415、判断是否检测到轮廓，若是，则进入步骤S416；反之，则进入步骤S417；

S416、判断检测是否超过20次，若是，则对历史存储的所有轮廓中心点进行聚类，并根据聚类结创建TB；反之，则存储当前轮廓中心点与面积，并将返回值数为false；

S417、更新帧缓存；

S418、初始化结束。

4.根据权利要求3所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述步骤S416中根据聚类结创建TB的具体过程如下：

1)聚类创建BT开始；

2)整合所有存储的轮廓点与其面积；

3)计算轮廓平均面积Sav；

4)设计TB的跟踪框大小，其表达式为：

其中，Ks表示系数；

5)计算轮廓中心点聚类邻域距离，其表达式为：

6)聚类结果过滤；

7)创建TB，并将返回值设为ture；

8)聚类创建TB结束。

5.根据权利要求4所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述步骤S42B具体表现为：

S42B0、TB跟踪程序开始后根据TB框的位置大小，在输入的图像序列中截取一个方框作为跟踪的处理图片；

S42B1、判断TB状态是否为丢失状态，若是，则设置读编码状态为FINISH，并记录当前帧读取字符为N，然后进入步骤S42B9；反之，则进入步骤S42B2；

S42B2、进行HSV识别与轮廓点检测，分别检测出R、G、B颜色的轮廓点；

S42B3、找出所有轮廓点中离TB图像中心最近的点，并通过运动学滤波，得到移动向量与颜色字符；

S42B4、根据移动向量更新TB在中心点；

S42B5、查看TB框是否会越界，若是，则更新TB状态为丢失状态，然后进入步骤S42B9；反之，则进入步骤S42B6；

S42B6、判断TB状态是否为准备状态，若是，则准备状态处理，然后进入步骤S42B9；反之，则进入步骤S42B7；

S42B7、判断TB状态是否为正常状态，若是，则正常状态处理，然后进入步骤S42B9；反之，则进入步骤S42B8；

S42B8、判断TB状态是否为疑似丢失状态，若是，则疑似丢失处理，然后进入步骤S42B9；反之，则直接进入步骤S42B9；

S42B9、更新缓存记录当前帧的各种状态，之后，TB跟踪程序结束。

6.根据权利要求5所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述TB为准备状态时，准备状态处理过程具体如下：

A0)准备状态处理开始；

A1)判断是否到了读取时间，若是则进入步骤A2)；反之，则读取加速器+1，然后，进入步骤A8)；

A2)判断读取字符是否为G或B，若是，则进入步骤A3)；反之，则读取错误，然后，进入步骤A8)；

A3)读取写入1或0，其中，G为0，B为1；

A4)判断是否完成读取所有字符，若是，则进入步骤A5)；反之，则进入步骤A8)；

A5)设置读编码状态为FINISH；

A6)TB状态设为正常状态，记录当前帧字符为N，为下一帧做准备；

A7)计算ID，并且记录TB的ID号；

A8)准备状态处理结束。

7.根据权利要求6所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述TB为正常状态时，正常状态处理过程具体如下：

B0)准备状态开始处理；

B1)判断当前TB的读编码状态是否为FINISH，若是，则进入步骤B2)；反之则进入步骤B2’)；

B2)判断上一帧字符是否为R，当前帧字符是否为G，若是，则设置读编码状态为READING，然后进入步骤B4)；反之，则进入步骤B3)；

B3)判断当前帧字符是否为N，若是，则TB状态设为疑似丢失状态，然后，进入步骤B4)；反之，则直接进入步骤B4)；

B2’)判断读码编码状态是否为READING，若是，则进入步骤B3’)；反之，则进入步骤B4)；

B3’)判断是否到了读取时间，若是则进入步骤B4’)；反之，则读取加速器+1，然后，进入步骤B4)；

B4’)判断读取字符是否为G或B，若是，则进入步骤B5’)；反之，则TB状态设为疑似丢失状态，然后，进入步骤B4)；

B5’)读取当前帧字符为1或0，并写入编码队列，其中，G为0，B为1；

B6’)判断是否完成读取所有字符，若是，则进入步骤B7’)，反之，则进入步骤B4)；

B7’)根据编码队列计算ID；

B8’)判断是否与原ID一样，若是，则进入步骤B9’)；反之，则判断TB状态为丢失状态，之后，进入步骤B10’)；

B9’)TB状态设为正常状态；

B10’)读编码状态设为FINISH，并记录这一帧的字符为N，之后，进入步骤B4)；

B4)正常状态处理结束。

8.根据权利要求7所述的基于RGB LED动态信标的多机器人定位方法，其特征在于，所述TB为疑似丢失状态时，疑似丢失状态处理过程具体如下：

C0)疑似丢失处理开始；

C1)判断当前TB的读编码状态是否为FINISH，若是，则进入步骤C2)，反之则进入步骤C2’)；

C2)判断上一帧字符是否为N，当前帧字符是否为R，若是，则TB状态转为正常状态，然后进入步骤C5)，反之，则进入步骤C3)；

C3)判断上一帧字符是否为R，当前帧字符是否为G，若是，则设置读码编码状态是否为READING，TB状态转为正常状态，然后，进入步骤C5)；反之，则进入步骤C4)；

C4)判断当前帧字符是否为N，若是，则TB状态设为丢失状态，然后，进入步骤C5)；反之，则直接进入步骤C5)；

C2’)读码编码状态是否为READING，若是，则进入步骤C3’)；反之，则进入步骤C5)；

C3’)判断是否到了读取时间，若是则进入步骤C4’)；反之，则读取加速器+1，然后，进入步骤C4″)；

C4’)判断读取字符是否为G或B，若是，则进入步骤C5’)；反之，则TB状态设为疑似丢失状态，然后，进入步骤C5)；

C5’)读取当前帧字符为1或0，并写入编码队列，其中，G为0，B为1,且TB状态设为正常状态；

C6’)判断是否完成读取所有字符，若是，则进入步骤C7’)，反之，则进入步骤C5)；

C7’)根据编码队列计算ID；

C8’)判断是否与原ID一样，若是，则进入步骤C9’)；反之，则判断TB状态为丢失状态，之后，进入步骤C10’)；

C9’)TB状态设为正常状态；

C10’)读编码状态设为FINISH，并记录这一帧的字符为N，之后，进入步骤C5)；

C4″)判断读取字符是否为B或G，若是，则TB状态转为正常状态，之后，进入步骤C5)；反之，则直接进入步骤C5)；

C5)正常状态处理结束。

9.基于RGB LED动态信标的多机器人定位系统，其特征在于，采用权利要求1至8中任一项所述的基于RGB LED动态信标的多机器人定位方法进行定位，所述定位系统包括计算机、机器人和相机：相机设置于机器人的上方，用于捕捉机器人；所述计算机分别与相机和机器人无线连接，所述计算机用于图像处理与控制机器人；所述机器人上设置有RGB LED灯，且RGB LED灯的亮度与颜色通过机器人进行调节与控制；所述机器人和相机的数量均为多个。

Images