基于单目摄像头和LED的机器人对接定位和导航方法

1.基于单目摄像头和LED的机器人对接定位和导航方法，其特征在于，具体包括如下步骤：
步骤1：机器人A原地旋转调整，随机移动，直到发现机器人Ｂ的红色顶部LED；
步骤2：机器人A根据观察到的红色顶部LED成像位置调整自身姿态，使红色顶部LED在机器人Ａ的摄像头中的成像位于图像水平方向的中央；
步骤3：计算机器人Ｂ的左右两个底部LED成像到顶部LED成像的水平距离；
步骤4：根据观察到的步骤3中的机器人Ｂ的LED成像距离关系，机器人A调整自身的位姿；
所述的机器人Ｂ的LED成像距离关系是指在摄像头的成像平面上，底部LED的成像点到顶部LED的成像点的水平间距的比值ratio，具体的位姿调整基于ratio的值来确定：
如果ratio>1，机器人A顺时针转动一个角度，然后向左平移，直到红色的顶部LED的成像再次位于图像水平方向中央位置，返回步骤3；
如果ratio<1，机器人A逆时针转动一个角度，然后向右平移，直到红色的顶部LED的成像再次位于图像水平方向中央位置，返回步骤3；
如果ratio=1，机器人A已经与机器人B的当前面实现对正，机器人A通过观察机器人Ｂ的当前面上三个LED的颜色组合状态，采取下一步动作，导航或者对接，执行步骤5；
步骤5：观察机器人Ｂ的当前面上三个LED的颜色组合状态，根据观察到的机器人Ｂ的LED颜色组合状态，获得导航信息；如果得到当前面是目标对接面则执行步骤7，否则执行步骤6；
步骤6：根据步骤5中的导航信息，机器人A导航移动到下一个对接面，然后执行步骤
1；
步骤7：执行对接过程；
所述的机器人A和机器人B均包括至少两个侧面，其中一个为主动对接面，其余为被动对接面，所述的主动对接面和被动对接面上分别设置有一组LED、两个模拟红外收发传感器和一对对接卡槽，所述的主动对接面上还设置有一对对接卡扣和摄像头；所述的一组LED包括两个底部LED和一个顶部LED，所述的摄像头位置，从主动对接面的正前方观察，摄像头恰好位于顶部LED和底部LED组成的三角形的几何中心位置；LED的位置，两个底部LED布置在机器人的主动对接面和被动对接面上，顶部LED布置在靠近机体内侧的位置，高度比两个底部LED高，并且与主动对接面或被动对接面不共面。
2.根据权利要求1所述的基于单目摄像头和LED的机器人对接定位和导航方法，其特
3
征在于：步骤5中所述的三个LED的颜色组合状态，对应的完整的状态空间有3=27个组合方式。
3.根据权利要求1所述的基于单目摄像头和LED的机器人对接定位和导航方法，其特征在于：步骤5中所述的三个LED的颜色组合状态，具体是指如下几种：
三个LED均亮红色：该机器人B为已对接机器人，当前面为目标对接面，机器人A应该和该目标对接面进行对接；
顶部LED为红色，底部LED均为绿色：该机器人B为已对接机器人，当前面不是目标对接面，目标对接面在当前面的右侧，机器人A应该向右侧导航移动；
顶部LED为红色，底部LED均为蓝色：该机器人B为已对接机器人，当前面不是目标对接面，目标对接面在当前面的左侧，机器人A应该向左侧导航移动；
三个LED均亮蓝色：该机器人B为未对接机器人，且处于漫游状态，还没有找到一个已对接机器人；
三个LED均亮绿色：该机器人B为未对接机器人，且处于导航状态，已经找到已对接机器人，但是还没有找到目标对接面。
4.根据权利要求1所述的基于单目摄像头和LED的机器人对接定位和导航方法，其特征在于：步骤7中所述的对接过程开始于机器人Ａ与机器人B的目标对接面对正之后，机器人A继续前进并不断调整保持对正状态，当机器人Ａ主动对接面和机器人Ｂ的目标对接面接近时，机器人B的LED会移出摄像头的视角，此时机器人Ａ利用自身模拟红外收发传感器测距来保证最后一段距离的对正状态，直到主动对接面上的接触开关碰到目标对接面，实现对接。
5.根据权利要求1所述的基于单目摄像头和LED的机器人对接定位和导航方法，其特征在于：所述的LED均为RGB三色LED。

基于单目摄像头和LED的机器人对接定位和导航方法\n技术领域\n[0001] 本发明属于群体机器人领域，更具体地说，是涉及一种群体机器人自组装对接过程中的定位和导航策略。\n背景技术\n[0002] 随着技术的发展和社会的进步，人们对机器人的功能需求不断增加，在一些非结构化的复杂环境中往往需要机器人具有良好的环境适应性和灵活性，群体机器人自组装和自重构特点能够满足这种要求同时也是群体机器人研究的关键方面。目前，国内外一些高校研究的群体机器人在自组装和自重构方面的能力都还比较有限，尤其是针对自组装过程中机器人的定位和导航策略这一难点，主要的方案都是基于红外传感器、触控开关等局部有限传感导航和机械外形配合、磁铁等对齐引导方式，所提供的定位和导航信息不足，具有使用价值的有效方案和策略较少，限制了群体机器人自组装的效率。\n发明内容\n[0003] 现有基于红外传感器、触控开关、机械外形配合或磁铁的技术方案存在对接引导信息不足，效率较低等问题，为了解决现有技术的不足，本发明提出了一种能够提供相对丰富的定位和导航信息，引导机器人快速组装对接的基于单目摄像头和LED的对接定位和导航方案。本发明突破了现有常见技术方案的思维，采用摄像头观察RGB的LED的颜色和位置，为定位和导航提供丰富全面的信息，提供了一种更加有效的对接定位和导航策略。\n[0004] 所述的对接定位和导航策略通过如下步骤实现：\n[0005] 步骤1：机器人A原地旋转调整，随机移动，直到发现机器人B的红色顶部LED；\n[0006] 步骤2：机器人A根据观察到的红色顶部LED成像位置调整自身姿态，使红色顶部LED在机器人A的摄像头中的成像位于图像水平方向的中央；\n[0007] 步骤3：计算机器人B的左右两个底部LED成像到顶部LED成像的水平距离；\n[0008] 步骤4：根据观察到的步骤3中的机器人B的LED成像距离关系，机器人A调整自身的位姿；\n[0009] 所述的机器人B的LED成像距离关系是指在摄像头的成像平面上，底部LED的成像点到顶部LED的成像点的水平间距的比值ratio，具体的位姿调整基于ratio的值来确定：\n[0010] 如果ratio>1，机器人A顺时针转动一个角度，然后向左平移，直到红色的顶部LED的成像再次位于图像水平方向中央位置，返回步骤3；\n[0011] 如果ratio<1，机器人A逆时针转动一个角度，然后向右平移，直到红色的顶部LED的成像再次位于图像水平方向中央位置，返回步骤3；\n[0012] 如果ratio=1，机器人A已经与机器人B的当前面实现对正，机器人A通过观察机器人B的当前面上三个LED的颜色组合状态，采取下一步动作，导航或者对接，执行步骤5；\n[0013] 步骤5：观察机器人B的当前面上三个LED的颜色组合状态，根据观察到的机器人B的LED颜色组合状态，获得导航信息；如果得到当前面是目标对接面则执行步骤7，否则执行步骤6；\n[0014] 步骤6：根据步骤5中的导航信息，机器人A导航移动的到下一个对接面，然后执行步骤1；\n[0015] 步骤7：执行对接过程。\n[0016] 所述的机器人A和机器人B均包括至少两个侧面，其中一个为主动对接面，其余为被动对接面，所述的主动对接面和被动对接面上分别设置有一组LED、两个模拟红外收发传感器和一对对接卡槽，所述的主动对接面上还设置有一对对接卡扣和摄像头；所述的一组LED包括两个底部LED和一个顶部LED，所述的摄像头位置，从主动对接面的正前方观察，摄像头恰好位于顶部LED和底部LED组成的三角形的几何中心位置；LED的位置，两个底部LED布置在机器人的主动对接面和被动对接面上，顶部LED布置在靠近机体内侧的位置，高度比两个底部LED高，并且与主动对接面或被动对接面不共面。\n[0017] 本发明的对接定位和导航策略的硬件系统构成主要由摄像头部分和RGB三色一体LED部分构成，通过对摄像头中观察到的LED成像信息提取，来实现对接导航和相对定位的目的。\n[0018] 所述的摄像头部分主要是由一个以CMOS的图像传感器芯片为核心的摄像头组成，摄像头固定安装在机器人主动对接面上，采用240×320像素点通过VGA时序以RGB565格式输出图像数据。\n[0019] 所述的LED部分采用的是RGB三色一体的LED，每3个LED为一组以特定的方式布置在机器人的所有侧面上，通过不同的颜色组合来表示不同的导航信息；通过特定的布置方式使得同一面上的3个LED在摄像头中成像的位置关系和机器人之间的相对位姿有确定的函数关系，从而实现准确的相对定位。\n[0020] 所述的对接定位策略是通过计算LED在摄像头中成像的距离关系，利用同一面上底部两个LED成像到顶部LED成像的水平距离的比值结合相关的几何关系解算出两个机器人之间的相对位姿，并根据当前位姿不断调整，直到机器人之间相互对正。\n[0021] 所述的对接导航策略是通过摄像头观察当前对接面上的LED成像的颜色信息来实现的，当前对接面通过设置自身的LED颜色组合引导机器人尽快导航到目标对接面，实\n3\n现对接。每个面上LED颜色的组合有3=27种，可以充分满足信息表达需要。\n[0022] 所述的定位和导航策略的硬件系统还包括在机器人的每个侧面上都布置有两个模拟红外收发传感器，主要在两个对接面相互贴近LED移出摄像头视角之后，用来辅助机器人之间的相对定位以及障碍的规避。\n[0023] 本发明的优点是：\n[0024] (1)对接机器人之间的相对定位主要采用一个单目摄像头和LED配合实现，设计简单，成本低廉；摄像头数据输出采用RGB565格式，包含丰富的颜色空间信息，可以根据需要转换到多种颜色空间中，方便进行颜色识别。\n[0025] (2)采用LED颜色组合表示机器人的状态，每个面上LED颜色组合有33=27种，能够为机器人导航提供丰富的信息。\n[0026] (3)每个对接面上的3个LED采用了一种特殊的布置方案，机器人不需要反复的多余移动，通过一次观察LED在摄像头中的成像位置关系即可确定两个机器人之间的相对位姿及目标对接面所在的相对方位。\n[0027] (4)本发明的策略设计简单，通过摄像头和LED的特殊布置及颜色组合即可以实现相关的定位和导航，有效提高了机器人对接组装的效率。\n附图说明\n[0028] 图1是本发明基于单目摄像头和LED的机器人对接定位和导航策略的机器人结构示意图；\n[0029] 图2是主动对接面上摄像头布置示意图；\n[0030] 图3是机器人定位几何关系原理分析图；\n[0031] 图4是基于单目摄像头和LED的机器人对接定位和导航策略方法的流程图;\n[0032] 图中：\n[0033] 1.主动对接面；2.被动对接面；3.摄像头；4.顶部LED；5.底部LED；6.模拟红外收发传感器；7.对接卡槽；8.对接卡扣；9.CMOS成像传感器；10.摄像头镜头。\n具体实施方式\n[0034] 下面结合附图对本发明做进一步的详细说明。\n[0035] 请参阅图1～2所示，本发明中的定位和导航策略通过CMOS单目摄像头观察LED实现机器人之间的定位和导航。图1展示了两个机器人之间定位和导航的一个基本策略，所述的机器人包括至少两个侧面，其中一个为主动对接面1，其余为被动对接面2，所述的主动对接面1和被动对接面2上分别设置有一组LED（包括两个底部LED5和一个顶部LED4）、两个模拟红外收发传感器6和一对对接卡槽7，所述的主动对接面1上还设置有一对对接卡扣8和CMOS单目摄像头3（简称摄像头）。通过机器人Ａ的主动对接面1上的CMOS单目摄像头3观察机器人Ｂ上LED组的颜色，结合机器人A自身的主动对接面1上两个模拟红外收发传感器6的信息，使得带有主动对接面1的机器人Ａ迅速导航定位，与机器人Ｂ实现对接，多个机器人的对接形成群体机器人。将需要对接的主动对接面或者被动对接面统称为目标对接面，目标对接面具有与其余主动对接面或者被动对接面2不同的LED组合信号。\n[0036] 本发明中，机器人的对接是通过每个机器人的主动对接面1寻找目标对接面，并将主动对接面1与目标对接面连接，实现两个机器人之间连接，其中主动对接面1上的摄像头3观察目标对接面上LED颜色和成像位置信息，采取定位和导航决策。实现定位和导航的过程对硬件的要求比较简单，每个机器人上的相关的设备主要包括一个摄像头3、两个模拟红外收发传感器6、一个顶部LED4和两个底部LED5，所述的LED均为RGB三色LED。通过以上基本的硬件配置，结合本发明的策略算法即可以为机器人自组装过程提供有效的对接定位和导航功能。\n[0037] 请参阅图2～3所示，本发明的定位和导航策略所使用的摄像头3和LED的安装和位置关系都进行了特殊的设计，所述的摄像头3位置如图2所示，从主动对接面1的正前方观察，摄像头3恰好位于顶部LED4和底部LED5组成的三角形的几何中心位置；所述的LED的位置如图1～2所示，两个底部LED5以一定的间距布置在机器人的主动对接面1和被动对接面2上，顶部LED4布置在靠近机体内侧的位置，高度比两个底部LED5高，并且与主动对接面1或被动对接面2不共面；以上布置可以保证摄像头3在不同的相对位姿上观察到的LED成像关系不同且有明确的函数对应关系。\n[0038] 请参阅图3所示，为本发明的定位原理图，所述的明确的函数对应关系是由机器人本身的结构参数和相关的几何光学原理确定的，图中的角度α，L和f为机器人本身的安装和结构参数，其中，角α为底部LED的俯视图投影与顶部LED的俯视图投影形成的等腰三角形的顶角的一半，L为顶部LED的俯视图投影与底部LED的俯视图投影之间的距离，两个底部LED与顶部LED之间的距离相等，即三个LED组成等腰三角形，f为摄像头3的CMOS成像传感器9所在平面与摄像头3上镜头10的垂直距离；角度β和距离d为需要对接的两个机器人（本发明中为机器人A和机器人B）的相对位姿参数，其中，β为需要对接的两个机器人之间的偏角（主动对接面与目标对接面对正状态下此角应为0度），d为主动对接面与目标对接面之间的距离，本发明中选取的是主动对接面与目标对接面上顶部LED之间的距离；L1和L2分别为两个底部LED5在摄像头成像平面方向到顶部LED4的实际水平间距，L1'和L2'分别为两个底部LED5在摄像头成像平面上的成像点到顶部LED4的成像点的水平间距，利用摄像头图像，通过测量L1'和L2'并得到L1'和L2'的比值ratio，就可以计算出两个机器人之间的相对位姿关系参数——角度β和距离d。根据图3中的几何关系，有：\n[0039] L1＝L·sin(α-β) (1)\n[0040] L2＝L·sin(α+β) (2)\n[0041] \n[0042] \n[0043] 如图4所示，本发明的基于单目摄像头和LED的机器人对接定位和导航策略，通过如下步骤实现目标对接面的寻找和导航过程，本发明中以机器人A与机器人B之间的定位导航为例来说明，具体为：\n[0044] 步骤1：机器人A原地旋转调整，随机移动，直到发现红色顶部LED4；\n[0045] 步骤2：机器人A根据观察到的红色顶部LED4成像位置调整自身姿态，使红色顶部LED4在摄像头3中的成像位于图像水平方向的中央；所述的摄像头3是位于机器人A的主动对接面上的摄像头。\n[0046] 步骤3：计算左右两个底部LED成像到顶部LED成像的水平距离；\n[0047] 步骤4：根据观察到的LED成像距离关系（即步骤3中的水平距离关系），机器人A调整自身的位姿；\n[0048] 所述的LED成像距离关系是指在摄像头3的成像平面上，底部LED5的成像点到顶部LED4的成像点的水平间距的比值ratio，具体的位姿调整策略基于ratio的值来确定：\n[0049] 如果ratio>1，机器人A顺时针转动一个角度，然后向左平移，直到红色的顶部LED4的成像再次位于图像水平方向中央位置，返回步骤3。\n[0050] 如果ratio<1，机器人A逆时针转动一个角度，然后向右平移，直到红色的顶部LED4的成像再次位于图像水平方向中央位置，返回步骤3。\n[0051] 如果ratio=1，机器人A已经与机器人B的当前面实现对正，机器人A可以通过观察当前面上三个LED的颜色组合状态，采取下一步动作，导航或者对接，执行步骤5。\n[0052] 其中，机器人A顺时针或者逆时针转动的角度和ratio的大小有关，机器人根据当前姿态偏差程度来决定微调角度的大小，ratio偏离1越大，对应的转角也就会较大。步骤\n5：观察当前面上三个LED的颜色组合状态，根据观察到的LED颜色组合状态，获得导航信息；如果得到当前面是目标对接面则执行步骤7，否则执行步骤6；\n[0053] 所述的三个LED颜色组合状态，对应的完整的状态空间有33=27个组合方式，在本实施例中我们选用的方式有以下几种：\n[0054] 三个LED均亮红色：该机器人B为已对接机器人，当前面为目标对接面，机器人A应该和该目标对接面进行对接。\n[0055] 顶部LED为红色，底部LED均为绿色：该机器人B为已对接机器人，当前面不是目标对接面，目标对接面在当前面的右侧（左侧和右侧是相对机器人A视角），机器人A应该向右侧导航移动。\n[0056] 顶部LED为红色，底部LED均为蓝色：该机器人B为已对接机器人，当前面不是目标对接面，目标对接面在当前面的左侧（左侧和右侧是相对于机器人A视角），机器人A应该向左侧导航移动。\n[0057] 三个LED均亮蓝色：该机器人B为未对接机器人，且处于漫游状态，还没有找到一个已对接机器人。\n[0058] 三个LED均亮绿色：该机器人B为未对接机器人，且处于导航状态，已经找到已对接机器人，但是还没有找到目标对接面。\n[0059] 步骤6：根据步骤5中的导航信息，机器人A导航移动的到下一个对接面，然后执行步骤1；\n[0060] 步骤7：执行对接过程。\n[0061] 所述的对接过程开始于机器人Ａ与目标对接面对正之后，机器人A继续前进并不断调整保持对正状态，当机器人Ａ主动对接面和机器人Ｂ的目标对接面十分接近时，LED会移出摄像头3的视角，此时机器人Ａ利用自身模拟红外收发传感器6测距来保证最后一段距离的对正状态，直到主动对接面上的接触开关碰到目标对接面，触发对接卡扣动作，通过对接卡槽卡紧目标对接面实现对接。\n[0062] 上述的实施例中，以机器人A寻找已对接机器人B为例进行了说明，实际过程中，可以根据27中LED的组合状态，任意设置信号组合来导航定位，机器人A和机器人B的导航移动通过自身设置的自动驱动装置实现，所述的自动驱动装置可以是现有技术中的任何一种实现方式，本发明中只需要机器人可以根据信号指令实现移动即可。\n[0063] 本发明的对接定位和导航策略，主要基于摄像头和LED，其中尤其是设计了一种摄像头和LED的特殊的位置布置关系，实现了单目摄像头的快速准确定位方法设计，在此基础上为群体机器人自组装提供了一种快速、有效的定位和导航策略，提高了对接和组装效率。本发明适用于地面群体移动机器人对接和组装过程的定位和导航。\n[0064] 以上所述，仅为本发明策略较佳的具体实施方式之一，比如在LED的数目和布置参数以及相应的非原理性上的变化都仍然可以保证相关设计目的的实现，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可以轻易想到的变化或替代，都应涵盖在本发明的保护范围之内。