CN116718188A - 一种面向异构集群机器人的室内大范围协同定位方法 - Google Patents

Abstract

本发明提出了一种面向异构集群机器人的室内大范围协同定位方法，将集群机器人分为导航机器人与工作机器人，导航机器人具有向上投影设备，能够向任务场景空间顶部投影特定的光学信标，为工作机器人提供辅助定位服务；导航机器人和工作机器人均具有向上的视觉定位模块，工作机器人能够通过观察光学信标实现自主定位；并且考虑到室内大范围协同定位需求下，光学信标的覆盖范围有限的问题，提出了导航机器人的运动控制方法；进一步考虑屋顶投影面存在凸起，会导致光学信标畸变的问题，提出了相应的投影位置优化逻辑。

Description

一种面向异构集群机器人的室内大范围协同定位方法

技术领域

本发明涉及机器人定位技术领域，特别涉及一种面向异构集群机器人的室内大范围协同定位方法。

背景技术

实现集群机器人在建筑物内部的自主定位，是其成功执行特定室内任务的前提。

由于建筑物外墙对电磁信号会产生屏蔽，导致卫星定位在室内的定位效果十分不稳定，甚至于完全失效。因此，主流的室内定位方法一般采用WiFi定位、超声波定位、红外或视觉定位等方式，但通过红外传感器或视觉获得个体间的相对位置信息，再通过算法整合相对距离信息来实现定位时，由于红外传感器布局及墙面漫反射，此类定位方法角分辨率较低，易受环境光干扰，在大规模集群中面临红外信号相互干扰的问题；而采用超声波信号定位的方法，具有高精度和长距离的优势，但也面临着信号衰减缓慢的挑战，特别是在封闭的空间内，当应用于大规模的机器人群时，重复的超声波信号会导致该技术失效，另外，超声波传感器的尺寸和重量对于厘米级别的机器人而言也过大，从硬件安装的角度考虑，这种方案也不适合集群机器人。

为此，有研究人员提出了向上投影的方式，如公开号为CN112601060A的中国专利申请中公开了一种桌面集群机器人的主动共享投影面感知系统及方法，该方法中，每个机器人均向顶层投影板上投射一个带有内圆的等腰三角形标示，通过机器人上的CMOS视觉传感获取顶部视野范围内的所有标示图像，通过识别出图像上所有的标示的位置和所指向的方向，计算出邻居相对与自身的相对位置和相对航向。但该方法存在以下问题：该方法中，每个机器人计算的坐标和航向都是在自身局部坐标系下的坐标和相对航向，各个机器人坐标系之间没有关联，导致集群机器人在执行整体任务时，中央控制器无法整体获知集群机器人在全局坐标系下的位置和航向，进而无法进行有效的整体决策与控制。

此外，在室内大范围场景任务下，我们还发现，由于室内高度有限，导致向上投影的方式还存在投射标示的覆盖范围有限，如果采用鱼眼相机扩大图像采集范围，会出现严重的图像畸变，导致定位失败；另外，当室内屋顶面积较大时，顶部平面往往还会有各种梁、装饰物，照灯和管道等，投射标示如果投射在这些物体上也会出现图像畸变，导致定位失败。

发明内容

为解决现有技术存在的问题，本发明提出一种面向异构集群机器人的室内大范围协同定位方法，该方法中，将集群机器人分为导航机器人与工作机器人，其中导航机器人具有向上投影设备，能够向任务场景空间顶部投影特定的光学信标，为工作机器人提供辅助定位服务；导航机器人和工作机器人均具有向上的视觉定位模块，其中工作机器人能够通过观察光学信标实现自主定位；并且考虑到室内大范围协同定位需求下，光学信标的覆盖范围有限的问题，提出了导航机器人的运动控制方法；进一步考虑屋顶投影面存在凸起，会导致光学信标畸变的问题，提出了相应的投影位置优化逻辑。

本发明的技术方案为：

一种面向异构集群机器人的室内大范围协同定位方法，包括以下步骤：

步骤1：在室内任务场景下，导航机器人选择首个期望投影位置，并在首个期望投影位置向任务场景空间顶部投影定制的光学信标；所述光学信标具有至少两个关键点，并能够通过所述关键点形成信标坐标系作为参考坐标系；

步骤2：工作机器人通过自身顶部装配的光轴垂直于任务场景空间顶部，且向上观察的视觉定位模块，观察步骤1所述光学信标，根据光学信标图像信息，解算自身在参考坐标系中的位置与方向，实现自主定位；

步骤3：导航机器人收集所有工作机器人广播的定位信息，在工作机器人在执行任务运动过程中，导航机器人解算自身与工作机器人的最远距离则工作机器人停止运动，其中/>为预设服务半径阈值；

步骤4：导航机器人根据工作机器人的运动态势，选择下一个期望投影位置，并移动到下一个期望投影位置，向任务场景空间顶部投影定制的光学信标；然后工作机器人通过自身顶部装配的视觉定位模块观察光学信标，根据光学信标图像信息，解算自身在参考坐标系中的位置与方向，实现自主定位；

步骤5：重复步骤3和步骤4，直至工作机器人任务完成。

进一步的，步骤4中，导航机器人选择第n个期望投影位置的步骤为：/>由导航机器人的当前位置/>工作机器人群体的当前质心/>和工作机器人群体的当前平均朝向/>确定：

进一步的，步骤4中，导航机器人向第n个期望投影位置移动过程中，通过以下过程优化所述第n个期望投影位置，n≥2：

步骤4.1：导航机器人在向第n个期望投影位置移动过程中，导航机器人解算自身位置与所述第n个期望投影位置的距离d_t，若d_t＜d_{set 2}，则认为导航机器人已移动到第n个期望投影位置附近；d_set2为预设距离阈值；

步骤4.2：当导航机器人已移动到第n个期望投影位置附近，则导航机器人停止移动，然后采用动静结合的方式进行第n个期望投影位置优化：

在导航机器人静止时，向任务场景空间顶部投影检测图案，并通过自身顶部装配的光轴垂直于任务场景空间顶部，且向上观察的视觉定位模块观察检测投影图案，导航机器人通过判断观察到的检测投影图案是否出现畸变，判断顶部区域是否有凸起，并根据上一时刻的凸起检测结果Flag1，当前时刻的凸起检测结果Flag2以及凸起累计量m共同进行判断，其中Flag1＝0表示无凸起、Flag1＝1表示有凸起，Flag1初始值为1，Flag2＝0表示无凸起、Flag2＝1表示有凸起，m初始值为0；

当Flag2＝1且Flag1＝1时，则m＝m+1，当m是奇数时，导航机器人选择一个随机方向作为当前时刻的运动方向进行运动，当m是偶数时，导航机器人沿靠近第n个期望投影位置的方向进行运动；

若Flag2＝0，则m＝0，导航机器人则沿靠近第n个期望投影位置的方向进行运动；

若Flag2＝1且Flag1＝0，则m＝0，导航机器人返回至上一时刻所在位置，以导航机器人上一时刻所在位置为优化后的第n个期望投影位置，位置优化结束；

若导航机器人运动到第n个期望投影位置时，位置优化也结束。

进一步的，步骤4中，所述检测图案采用矩形图案，且矩形图案大小能够覆盖所述光学信标大小。

进一步的，所述光学信标由3个排列为等腰三角形的圆形构成，以三角形信标底边中点M为原点，建立信标坐标系X_BO_BY_B，信标坐标系X轴与三角形信标底边垂直，其X轴正方向指向三角形信标顶点N；点M和点N为关键点；信标坐标系即为参考坐标系。

进一步的，步骤2中根据光学信标图像信息，解算自身在参考坐标系中的位置与方向的过程为：

步骤2.1：首先建立相机坐标系：以工作机器人自身顶部装配的视觉定位模块中的相机光心为原点，建立相机坐标系X_CY_CZ_C，该坐标系Y轴正方向与机器人运动方向一致，Z轴与相机光轴重合；

步骤2.2：根据光学信标图像信息，识别图像中的光学信标，获取光学信标关键点在像素坐标系下的坐标；

步骤2.3：基于相机针孔成像模型，将光学信标关键点在像素坐标系下的坐标转换到相机坐标系下；

步骤2.4：将相机坐标系投影到光学信标所在平面得到的二维坐标系X_CO_CY_C，相机光心在坐标系X_CO_CY_C中的投影位置^CR为二维坐标系X_CO_CY_C原点，得到光学信标关键点在二维坐标系X_CO_CY_C中的坐标；

步骤2.5：根据光学信标关键点在二维坐标系X_CO_CY_C中的坐标，反推^CR在信标坐标系下的坐标，为机器人在参考坐标系下的位置；二维坐标系X_CO_CY_C相对于信标坐标系的旋转角为机器人在参考坐标系下航向角。

进一步的，步骤2.2中，识别图像中光学信标的过程包括信标顶点检测和信标信息提取环节：

信标顶点检测：对视觉定位模块采集的图像做灰度处理，基于Canny算子边缘检测与霍夫变换圆形识别进行信标顶点检测，得到工作机器人视野中所有可能的信标顶点；

信标信息提取：采用模板匹配方法对信标顶点检测环节得到的所有可能的信标顶点进行处理，得到光学信标的关键点，以及关键点在像素坐标系下的坐标。

进一步的，在信标顶点检测环节，还根据导航机器人投影部件的尺寸，设定光学信标圆形顶点的像素阈值，若基于Canny算子边缘检测与霍夫变换圆形识别中得到的圆形半径小于像素阈值，则将该圆形视为噪点，将其删除。

进一步的，在信标信息提取环节，采用模板匹配方法对信标顶点检测环节得到的所有可能的信标顶点进行处理，得到光学信标的3个顶点，通过计算三个顶点之间的两两距离确定该信标的底边、腰以及三个顶点N，J和Q在像素坐标系下的坐标；根据光学信标的设计方案，信标坐标系原点M为三角形信标底边中点，M＝(J+Q)/2，指向X轴正方向,/>指向Y轴正方向，根据三个顶点在像素坐标系下的坐标，得到光学信标关键点M和N在像素坐标系下的坐标分别为^PM(u_m,v_m)和^PN(u_n,v_n)。

进一步的，步骤2.3中，基于相机针孔成像模型，将光学信标关键点M和N点在像素坐标系下的坐标^PM(u_m,v_m)和^PN(u_n,v_n)转换到相机坐标系下^CM(x_m,y_m,h)和^CN(x_n,y_n,h)，其中h为相机光心到光学信标所在平面的垂直距离；步骤2.4中，M和N点在二维坐标系X_CO_CY_C中的坐标分为^CM(x_m,y_m)和^CN(x_n,y_n)；步骤2.5中，在信标坐标系下，机器人的位置为^BR(x_r,x_r)为：

机器人航向角heading为：

heading＝arctan2(y_n-y_m,x_n-x_m)。

有益效果

本发明提出了一种面向异构集群机器人的室内大范围协同定位方法，使用向上投影的方式有效解决了机器人相互遮挡问题；并且将集群机器人分为导航机器人和工作机器人，使用导航机器人主动向天花板投影特殊设计的光学信标图案(包括参考坐标系原点和方向基准)，辅助集群机器人实现自主定位，有效解决了较大规模集群机器人在室内定位中存在的不能获得准确的群体全局态势信息问题；导航机器人获得群体全局态势信息后，根据集群群体下一阶段任务需求向群体期望工作区域进行位置转移并重新投影光学信标，进一步实现对大范围工作区域的全覆盖；而且还能针对顶部存在凸起等情况的特殊投影位置优化。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为方法流程图。

图2为信标坐标系X_BO_BY_B。

图3为相机坐标系X_CY_CZ_C。

图4为信标坐标系与相机坐标系对应关系。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本实施例中的集群机器人分为导航机器人与工作机器人，其中导航机器人具有向上投影设备，能够向任务场景空间顶部垂直投影特定的光学信标，为工作机器人提供辅助定位服务，导航机器人和工作机器人均具有光轴垂直于任务场景空间顶部的向上观察的视觉定位模块，其中工作机器人能够通过观察光学信标实现自主定位。

基于上述功能分类，面向异构集群机器人的室内大范围协同定位方法具体包括以下步骤：

步骤1：在室内任务场景下，导航机器人选择首个期望投影位置，并在首个期望投影位置向任务场景空间顶部投影定制的光学信标；所述光学信标具有至少两个关键点，并能够通过所述关键点形成信标坐标系作为参考坐标系。

首个期望投影位置由具体任务决定。例如，普通集群编队任务中可以随机选取首个期望投影位置/>集群协同运输任务则会指定首个期望投影位置/>

如图2所示，本实施例中，导航机器人顶部安装了RGB射灯模块，用于向工作空间顶部垂直投影特定的光学信标，本实施例中形成的所述光学信标由3个排列为等腰三角形的圆形构成，以三角形信标底边中点M为原点，建立信标坐标系X_BO_BY_B，信标坐标系X轴与三角形信标底边垂直，其X轴正方向指向三角形信标顶点N，该坐标系作为集群机器人自主定位的参考坐标系，该坐标系原点在地面的投影就是导航机器人位置。这里的点M和点N即为光学信标中的关键点。

当然，光学信标形成图形也可以采用其他形状，只需信标中的关键点能够通过图像被唯一识别，且通过关键点能够建立全局参考坐标系即可。

步骤2：工作机器人通过自身顶部装配的光轴垂直于任务场景空间顶部，且向上观察的视觉定位模块，观察步骤1所述光学信标，根据光学信标图像信息，解算自身在参考坐标系中的位置与方向，实现自主定位。

步骤2.1：工作机器人首先建立相机坐标系：

以工作机器人自身顶部装配的视觉定位模块中的相机光心为原点，建立相机坐标系X_CY_CZ_C，该坐标系Y轴正方向与机器人运动方向一致，Z轴与相机光轴重合，为了便于计算，这里使相机光心与机器人运动中心重合。

步骤2.2：其次，所述视觉定位模块获取工作空间顶部天花板图像，识别图像中的光学信标：

这里识别光学信标的过程包括信标顶点检测和信标信息提取环节，并且为了提高可靠性，还可以增加信息校验环节。

信标顶点检测：对视觉定位模块采集的图像做灰度处理，基于Canny算子边缘检测与霍夫变换圆形识别进行信标顶点检测，以此得到工作机器人视野中所有可能的信标顶点。为了减少后面模板匹配方法的计算量，这里根据导航机器人顶部RGB射灯模块的尺寸，设定光学信标圆形顶点的像素阈值，这里我们设定像素阈值为3个像素点，即基于Canny算子边缘检测与霍夫变换圆形识别中得到的圆形半径小于3个像素点，则将该圆形视为噪点，将其删除。

信标信息提取：对经过信标顶点检测处理后的信标顶点采用模板匹配方法，得到光学信标的3个顶点，通过计算三个顶点之间的两两距离确定该信标的底边、腰以及三个顶点N，J和Q在像素坐标系下的坐标，所述像素坐标系以视觉定位模块采集的图像左上角为原点，以图像长和宽方向为坐标系X和Y轴方向。根据光学信标的设计方案，信标坐标系原点M为三角形信标底边中点，即M＝(J+Q)/2，指向X轴正方向,/>指向Y轴正方向，那么根据三个顶点在像素坐标系下的坐标，能够得到光学信标关键点M和N在像素坐标系下的坐标分别为^PM(u_m,v_m)和^PN(u_n,v_n)。

信息校验：为了防止识别错误，这里还利用等腰三角形中线和底边相互垂直的几何关系，对识别出的信标进行校验。理论上若和/>垂直，则二者内积为零，但实际信标识别过程中由于相机畸变、杂光干扰、轮廓模糊等因素影响，会产生信标顶点识别错误的问题。本实施例中，对计算的/>和/>的夹角适当放宽，即若/>和/>的夹角在(80°,100°)内，则认为二者垂直，校验成功；否则，认为本次识别有误，重新采集图像进行信标识别和提取。

步骤2.3：在识别得到导航机器人发出的光学信标关键点在像素坐标系下的坐标后，基于相机针孔成像模型，将光学信标关键点在像素坐标系下的坐标转换到相机坐标系下，得到信标底边中点M和信标顶点N在相机坐标系下的坐标^CM(x_m,y_m,h)和^CN(x_n,y_n,h)。具体过程如下：

根据相机针孔成像模型，相机坐标系下某一点与其在像素坐标系下位置的对应关系如下：

式中Z_C为相机光心到成像物体所在平面的距离；(u,v)表示像素坐标系中的点的坐标；(X_C,Y_C,Z_C)为相机坐标系下的对应点的坐标；T为相机3×3内参矩阵，可利用相机标定方法得出；(u₀,v₀)为相机光心在像素坐标系中的表示；dx,dy分别代表像素坐标系中X和Y方向上单位像素实际对应的物理尺寸；f为相机镜头焦距。

当集群机器人在室内作业时，地面与天花板平整且基本保持平行。设定h为机器人相机光心到信标所在天花板平面的处置距离，此时若忽略地面起伏对机器人运动的影响，则Z_C＝h为定值。于是式(1)可简化为：

式(2)中(u_k,v_k)和(x_k,y_k,h)分别表示任意点K在像素坐标系中的位置以及在相机坐标系中的空间坐标；a_x＝f/dx和a_y＝f/dy分别表示像素坐标系水平轴和垂直轴的尺度因子。对式(2)进行逆变换，可得像素坐标系下任意点K在相机坐标系下的对应坐标：

利用式(3)，视觉定位模块所识别的像素位置^PM(u_m,v_m)与^PN(u_n,v_n)可转换至相机坐标系下，其对应坐标分别为^CM(x_m,y_m,h)和^CN(x_n,y_n,h)。

步骤2.4：由于视觉定位模块的光轴垂直于天花板平面，且机器人运动中心与相机坐标系原点重合，可将机器人的运动映射至信标所在的天花板平面，从而简化为二维平面运动。将相机坐标系投影到天花板平面得到的二维坐标系X_CO_CY_C，机器人运动中心在坐标系X_CO_CY_C中的投影位置^CR就是二维坐标系X_CO_CY_C原点，M和N点在二维坐标系X_CO_CY_C中的坐标分为^CM(x_m,y_m)和^CN(x_n,y_n)。

步骤2.5：在二维坐标系X_CO_CY_C中，得到了信标关键点M和N的坐标，而机器人运动中心在二维坐标系X_CO_CY_C中的投影位置^CR就是二维坐标系X_CO_CY_C原点，因此就能够利用信标坐标系与二维坐标系X_CO_CY_C之间的几何关系，计算出工作机器人在信标坐标系(即全局参考坐标系)下的位置与方向。

其中在信标坐标系下，机器人的位置为^BR(x_r,x_r)为：

式(4)中·表示向量之间的内积。

由于规定机器人航向与相机坐标系Y轴正方向一致，即与二维坐标系X_CO_CY_C的Y轴正方向平行，则机器人航向角heading为二维坐标系X_CO_CY_C相对于信标坐标系的旋转角，即：

heading＝arctan2(y_n-y_m,x_n-x_m) (5)

式(5)中arctan2表示四象限反正切运算。

步骤3：导航机器人收集所有工作机器人广播的定位信息，在工作机器人在执行任务运动过程中，导航机器人解算自身与工作机器人的最远距离则工作机器人停止运动，其中/>为预设服务半径阈值。

机器人视觉定位模块的视野范围有限，若光学信标大小及高度确定，其有效定位范围是有限的，并且可以通过实际测试得到。但在视野边缘，信标成像会出现明显畸变，导致信标识别失误，除此之外，考虑到工作机器人群体大小，实际有效定位面积(即预设服务半径阈值)会小于理论有效定位距离。

步骤4：导航机器人根据工作机器人的运动态势，选择下一个期望投影位置，并移动到下一个期望投影位置，向任务场景空间顶部投影定制的光学信标；然后工作机器人通过自身顶部装配的视觉定位模块观察光学信标，根据光学信标图像信息，解算自身在参考坐标系中的位置与方向，实现自主定位。

导航机器人选择第n个期望投影位置的步骤为：/>由导航机器人的当前位置工作机器人群体的当前质心/>和工作机器人群体的当前平均朝向/>确定：

由于天花板不可避免会存在一些凸起，例如房梁、照灯和管道，这对光学信标投影图案的质量会带来不利影响。因此，导航机器人需要判断期望投影点是否满足投影要求(默认首个期望投影位置满足投影要求)。

导航机器人向第n个期望投影位置移动过程中，通过以下过程优化所述第n个期望投影位置，n≥2：

步骤4.1：导航机器人在向第n个期望投影位置移动过程中，导航机器人解算自身位置与所述第n个期望投影位置的距离d_t，若d_t＜d_{set 2}，则认为导航机器人已移动到第n个期望投影位置附近；d_set2为预设距离阈值；这里取

步骤4.2：当导航机器人已移动到第n个期望投影位置附近，则导航机器人停止移动，然后采用动静结合的方式进行第n个期望投影位置优化：

在导航机器人静止时，向任务场景空间顶部投影检测图案，并通过自身顶部装配的光轴垂直于任务场景空间顶部，且向上观察的视觉定位模块观察检测投影图案，导航机器人通过判断观察到的检测投影图案是否出现畸变，判断顶部区域是否有凸起。这里所述检测图案采用矩形图案，且矩形图案大小能够覆盖所述光学信标大小。

根据上一时刻的凸起检测结果Flag1，当前时刻的凸起检测结果Flag2以及凸起累计量m共同进行判断，其中Flag1＝0表示无凸起、Flag1＝1表示有凸起，Flag1初始值为1，Flag2＝0表示无凸起、Flag2＝1表示有凸起，m初始值为0；

当Flag2＝1且Flag1＝1时，则m＝m+1，当m是奇数时，导航机器人选择一个随机方向作为当前时刻的运动方向进行运动，当m是偶数时，导航机器人沿靠近第n个期望投影位置的方向进行运动；

若Flag2＝0，则m＝0，导航机器人则沿靠近第n个期望投影位置的方向进行运动；

若Flag2＝1且Flag1＝0，则m＝0，导航机器人返回至上一时刻所在位置，以导航机器人上一时刻所在位置为优化后的第n个期望投影位置，位置优化结束；

若导航机器人运动到第n个期望投影位置时，位置优化也结束。

步骤5：重复步骤3和步骤4，直至工作机器人任务完成。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：所述集群机器人分为导航机器人与工作机器人，其中导航机器人具有向上投影设备，能够向任务场景空间顶部垂直投影特定的光学信标，工作机器人具有光轴垂直于任务场景空间顶部的向上观察的视觉定位模块，能够拍摄工作空间顶部图像；

所述方法包括以下步骤：

步骤1：在室内任务场景下，导航机器人选择首个期望投影位置，并在首个期望投影位置向任务场景空间顶部投影定制的光学信标；所述光学信标具有至少两个关键点，并能够通过所述关键点形成信标坐标系作为参考坐标系；

步骤2：工作机器人通过自身顶部装配的光轴垂直于任务场景空间顶部，且向上观察的视觉定位模块，观察步骤1所述光学信标，根据光学信标图像信息，解算自身在参考坐标系中的位置与方向，实现自主定位；

步骤3：导航机器人收集所有工作机器人广播的定位信息，在工作机器人在执行任务运动过程中，导航机器人解算自身与工作机器人的最远距离 则工作机器人停止运动，其中/>为预设服务半径阈值；

步骤4：导航机器人根据工作机器人的运动态势，选择下一个期望投影位置，并移动到下一个期望投影位置，向任务场景空间顶部投影定制的光学信标；然后工作机器人通过自身顶部装配的视觉定位模块观察光学信标，根据光学信标图像信息，解算自身在参考坐标系中的位置与方向，实现自主定位；

步骤5：重复步骤3和步骤4，直至工作机器人任务完成。

2.根据权利要求1所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：步骤4中，导航机器人选择第n个期望投影位置的步骤为：/>由导航机器人的当前位置/>工作机器人群体的当前质心/>和工作机器人群体的当前平均朝向确定：

3.根据权利要求1或2所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：步骤4中，导航机器人向第n个期望投影位置移动过程中，通过以下过程优化所述第n个期望投影位置，n≥2：

步骤4.1：导航机器人在向第n个期望投影位置移动过程中，导航机器人解算自身位置与所述第n个期望投影位置的距离d_t，若d_t＜d_set2，则认为导航机器人已移动到第n个期望投影位置附近；d_set2为预设距离阈值；

步骤4.2：当导航机器人已移动到第n个期望投影位置附近，则导航机器人停止移动，然后采用动静结合的方式进行第n个期望投影位置优化：

在导航机器人静止时，向任务场景空间顶部投影检测图案，并通过自身顶部装配的光轴垂直于任务场景空间顶部，且向上观察的视觉定位模块观察检测投影图案，导航机器人通过判断观察到的检测投影图案是否出现畸变，判断顶部区域是否有凸起，并根据上一时刻的凸起检测结果Flag1，当前时刻的凸起检测结果Flag2以及凸起累计量m共同进行判断，其中Flag1＝0表示无凸起、Flag1＝1表示有凸起，Flag1初始值为1，Flag2＝0表示无凸起、Flag2＝1表示有凸起，m初始值为0；

当Flag2＝1且Flag1＝1时，则m＝m+1，当m是奇数时，导航机器人选择一个随机方向作为当前时刻的运动方向进行运动，当m是偶数时，导航机器人沿靠近第n个期望投影位置的方向进行运动；

若Flag2＝0，则m＝0，导航机器人则沿靠近第n个期望投影位置的方向进行运动；

若Flag2＝1且Flag1＝0，则m＝0，导航机器人返回至上一时刻所在位置，以导航机器人上一时刻所在位置为优化后的第n个期望投影位置，位置优化结束；

若导航机器人运动到第n个期望投影位置时，位置优化也结束。

4.根据权利要求3所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：步骤4中，所述检测图案采用矩形图案，且矩形图案大小能够覆盖所述光学信标大小。

5.根据权利要求1所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：所述光学信标由3个排列为等腰三角形的圆形构成，以三角形信标底边中点M为原点，建立信标坐标系X_BO_BY_B，信标坐标系X轴与三角形信标底边垂直，其X轴正方向指向三角形信标顶点N；点M和点N为关键点；信标坐标系即为参考坐标系。

6.根据权利要求1或5所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：步骤2中根据光学信标图像信息，解算自身在参考坐标系中的位置与方向的过程为：

步骤2.1：首先建立相机坐标系：以工作机器人自身顶部装配的视觉定位模块中的相机光心为原点，建立相机坐标系X_CY_CZ_C，该坐标系Y轴正方向与机器人运动方向一致，Z轴与相机光轴重合；

步骤2.2：根据光学信标图像信息，识别图像中的光学信标，获取光学信标关键点在像素坐标系下的坐标；

步骤2.3：基于相机针孔成像模型，将光学信标关键点在像素坐标系下的坐标转换到相机坐标系下；

步骤2.4：将相机坐标系投影到光学信标所在平面得到的二维坐标系X_CO_CY_C，相机光心在坐标系X_CO_CY_C中的投影位置^CR为二维坐标系X_CO_CY_C原点，得到光学信标关键点在二维坐标系X_CO_CY_C中的坐标；

步骤2.5：根据光学信标关键点在二维坐标系X_CO_CY_C中的坐标，反推^CR在信标坐标系下的坐标，为机器人在参考坐标系下的位置；二维坐标系X_CO_CY_C相对于信标坐标系的旋转角为机器人在参考坐标系下航向角。

7.根据权利要求6所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：步骤2.2中，识别图像中光学信标的过程包括信标顶点检测和信标信息提取环节：

信标顶点检测：对视觉定位模块采集的图像做灰度处理，基于Canny算子边缘检测与霍夫变换圆形识别进行信标顶点检测，得到工作机器人视野中所有可能的信标顶点；

信标信息提取：采用模板匹配方法对信标顶点检测环节得到的所有可能的信标顶点进行处理，得到光学信标的关键点，以及关键点在像素坐标系下的坐标。

8.根据权利要求7所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：在信标顶点检测环节，还根据导航机器人投影部件的尺寸，设定光学信标圆形顶点的像素阈值，若基于Canny算子边缘检测与霍夫变换圆形识别中得到的圆形半径小于像素阈值，则将该圆形视为噪点，将其删除。

9.根据权利要求7所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：在信标信息提取环节，采用模板匹配方法对信标顶点检测环节得到的所有可能的信标顶点进行处理，得到光学信标的3个顶点，通过计算三个顶点之间的两两距离确定该信标的底边、腰以及三个顶点N，J和Q在像素坐标系下的坐标；根据光学信标的设计方案，信标坐标系原点M为三角形信标底边中点，M＝(J+Q)/2，指向X轴正方向,/>指向Y轴正方向，根据三个顶点在像素坐标系下的坐标，得到光学信标关键点M和N在像素坐标系下的坐标分别为^PM(u_m,v_m)和^PN(u_n,v_n)。

10.根据权利要求9所述一种面向异构集群机器人的室内大范围协同定位方法，其特征在于：步骤2.3中，基于相机针孔成像模型，将光学信标关键点M和N点在像素坐标系下的坐标^PM(u_m,v_m)和^PN(u_n,v_n)转换到相机坐标系下^CM(x_m,y_m,h)和^CN(x_n,y_n,h)，其中h为相机光心到光学信标所在平面的垂直距离；步骤2.4中，M和N点在二维坐标系X_CO_CY_C中的坐标分为^CM(x_m,y_m)和^CN(x_n,y_n)；步骤2.5中，在信标坐标系下，机器人的位置为^BR(x_r,x_r)为：

机器人航向角heading为：

heading＝arctan2(y_n-y_m,x_n-x_m)。