CN116718187A - 一种基于主动信标的集群机器人室内定位方法 - Google Patents

Abstract

本发明提出一种基于主动信标的集群机器人室内定位方法，将集群机器人分为导航机器人与工作机器人，导航机器人向工作空间顶部投影定制的光学信标；光学信标具有至少两个关键点，并能够通过关键点形成信标坐标系作为全局参考坐标系；工作机器人通过自身顶部装配的光轴垂直于工作空间顶部，向上观察的视觉定位模块，观察光学信标，根据光学信标图像信息，解算自身在全局参考坐标系中的位置与方向，实现自主定位。本发明使用向上投影的方式有效解决了机器人相互遮挡问题；使用导航机器人主动向天花板投影特殊设计的光学信标图案，辅助集群机器人实现自主定位，有效解决了较大规模集群机器人在室内定位中存在的不能获得准确的群体全局态势信息问题。

Description

一种基于主动信标的集群机器人室内定位方法

技术领域

本发明涉及集群机器人定位技术领域，具体为一种基于主动信标的集群机器人室内定位方法。

背景技术

实现集群机器人在室内环境中的自主定位，是其成功执行特定室内任务的前提。现有的集群机器人自主定位主要通过红外传感器或视觉获得个体间的相对位置信息，再通过算法整合相对距离信息来实现。由于红外传感器布局及墙面漫反射，此类定位方法角分辨率较低，易受环境光干扰，在大规模集群中面临红外信号相互干扰的问题。此外，上述定位方法还会因机器人间相互遮挡而失效，该问题在大规模密集集群中尤为凸显。

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

发明内容

为解决现有技术存在的问题，本发明提出一种基于主动信标的集群机器人室内定位方法，该方法中，将集群机器人分为导航机器人与工作机器人，其中导航机器人具有向上投影设备，能够向工作空间顶部投影特定的光学信标，为工作机器人提供辅助定位服务，工作机器人具有向上的视觉定位模块，能够通过观察光学信标实现自主定位。

本发明的技术方案为：

一种基于主动信标的集群机器人室内定位方法，包括以下步骤：

步骤1：导航机器人向工作空间顶部投影定制的光学信标；所述光学信标具有至少两个关键点，并能够通过所述关键点形成信标坐标系作为全局参考坐标系；

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

进一步的，所述光学信标由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.1中，相机光心与机器人运动中心重合。

进一步的，步骤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,yk,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表示四象限反正切运算。

这样，就完成了工作机器人在全局参考坐标系下的位置和航向解算；所有工作机器人均在统一的全局参考坐标系进行解算，并汇总到中央控制器后，中央控制器能够准确快速获取群体态势信息。

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

Claims (8)

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

所述方法包括以下步骤：

步骤1：导航机器人向工作空间顶部投影定制的光学信标；所述光学信标具有至少两个关键点，并能够通过所述关键点形成信标坐标系作为全局参考坐标系；

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

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

3.根据权利要求1或2所述一种基于主动信标的集群机器人室内定位方法，其特征在于：步骤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相对于信标坐标系的旋转角为机器人在全局参考坐标系下航向角。

4.根据权利要求3所述一种基于主动信标的集群机器人室内定位方法，其特征在于：步骤2.1中，相机光心与机器人运动中心重合。

5.根据权利要求3所述一种基于主动信标的集群机器人室内定位方法，其特征在于：步骤2.2中，识别图像中光学信标的过程包括信标顶点检测和信标信息提取环节：

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

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

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

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

8.根据权利要求7所述一种基于主动信标的集群机器人室内定位方法，其特征在于：步骤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)。