CN111964681B - 一种巡检机器人的实时定位系统 - Google Patents

一种巡检机器人的实时定位系统 Download PDF

Info

Publication number
CN111964681B
CN111964681B CN202010746225.7A CN202010746225A CN111964681B CN 111964681 B CN111964681 B CN 111964681B CN 202010746225 A CN202010746225 A CN 202010746225A CN 111964681 B CN111964681 B CN 111964681B
Authority
CN
China
Prior art keywords
dimensional code
monocular camera
uwb
positioning data
image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010746225.7A
Other languages
English (en)
Other versions
CN111964681A (zh
Inventor
史聪灵
车洪磊
赵晨
石杰红
李建
钱小东
任飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Academy of Safety Science and Technology CASST
Original Assignee
China Academy of Safety Science and Technology CASST
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Academy of Safety Science and Technology CASST filed Critical China Academy of Safety Science and Technology CASST
Priority to CN202010746225.7A priority Critical patent/CN111964681B/zh
Publication of CN111964681A publication Critical patent/CN111964681A/zh
Application granted granted Critical
Publication of CN111964681B publication Critical patent/CN111964681B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种巡检机器人的实时定位系统，包括：位UWB标签和二维码标签；各个单目摄像头，获取对二维码标签的识别结果；各个UWB锚点，获取UWB标签的UWB定位数据；主机，在各个单目摄像机可以识别二维码标签时，根据各个单目摄像机对二维码标签的识别结果确定各个单目摄像机的第一定位数据，基于第一定位数据确定巡检机器人的实时位置；在至少一个单目摄像机无法识别二维码标签时，根据巡检机器人移动后各个单目摄像机的第二定位数据，并对UWB定位数据进行补偿和矫正，基于补偿和矫正后的第三定位数据确定巡检机器人的实时位置。本发明结合UWB定位和单目视觉定位，在降低成本的同时，实现了巡检机器人的高精度定位。

Description

一种巡检机器人的实时定位系统
技术领域
本发明涉及机器人技术领域，具体而言，涉及一种巡检机器人的实时定位系统。
背景技术
近年来，巡检机器人导航定位技术不断发展，定位方法不断多样化，定位精度不断提高。早期的磁带导航和电磁导航技术被广泛使用。如世界上最繁忙的码头上海洋山港，巡检机器人采用了磁导航技术，通过埋在地下的多根磁钉来感知自己的位置，根据实时交通状况来优化机器人运动路线。随着智能制造领域的不断发展，对巡检机器人定位精度和灵活配置提出了更高要求，激光雷达、视觉导航、QR码导航和SLAM导航已经产生，并应用于不同领域。目前巡检机器人定位方法大多采用激光雷达，它可以帮助巡检机器人在完全未知的室内环境构建室内环境地图，并实现巡检机器人的自主导航，其室内定位精度为20mm左右。但是激光雷达的价格高，部分激光雷达占到了巡检机器人总成本的三分之一。另外，构建室内环境地图一般采用SLAM技术，主要包括Visual SLAM(VSLAM)和LiDAR SLAM。VSLAM是指在室内环境中使用深度相机进行导航，其工作原理是对巡检机器人周围的环境进行光学处理。深度相机收集图像信息，处理器将收集的图像信息链接到巡检机器人的实际位置，从而完成巡检机器人的自主导航和定位。VSLAM的计算量太大，对巡检机器人系统的性能要求很高，且由VSLAM生成的地图通常是点云，需经过处理，才能应用于巡检机器人的路径规划。因此，本发明结合UWB定位和单目视觉定位，在减少成本的同时实现巡检机器人的高精度定位。
发明内容
为解决上述问题，本发明的目的在于提供一种巡检机器人的实时定位系统，结合UWB定位和单目视觉定位，在减少成本的同时实现高精度的定位。
本发明提供了一种巡检机器人的实时定位系统，所述系统包括：
位于巡检机器人上的UWB标签和二维码标签，其中，所述UWB标签与各个UWB锚点匹配设置；
位于室内的各个单目摄像头，用于对所述二维码标签进行识别，获取对所述二维码标签的识别结果；
位于室内的各个UWB锚点，用于与所述UWB标签进行通信，获取所述UWB标签的UWB定位数据；
位于巡检机器人上的主机，用于在所述各个单目摄像机可以识别所述二维码标签时，根据所述各个单目摄像机对所述二维码标签的识别结果确定所述各个单目摄像机的第一定位数据，基于所述各个单目摄像机的第一定位数据确定所述巡检机器人的实时位置；在至少一个单目摄像机无法识别所述二维码标签时，根据所述巡检机器人移动后所述各个单目摄像机的第二定位数据，并对所述UWB标签的UWB定位数据进行补偿和矫正，基于补偿和矫正后的第三定位数据确定所述巡检机器人的实时位置。
作为本发明进一步的改进，所述各个单目摄像头对所述二维码标签进行识别，获取对所述二维码标签的识别结果，包括：
一个单目摄像机拍摄所述二维码标签的二维码图像；
基于所述二维码图像，获取所述单目摄像机对所述二维码标签的识别结果；
依次类推，获取所述各个单目摄像机对所述二维码标签的识别结果。
作为本发明进一步的改进，基于所述二维码图像，获取所述单目摄像机对所述二维码标签的识别结果，包括：
对所述二维码图像进行二值化处理，得到第一二值图像；
从所述第一二值图像中提取所述二维码的轮廓；
基于提取出的轮廓，对所述第一二值图像进行透视变换，得到第二二值图像；
通过OTSU二值化处理获取所述第二二值图像的白色位和黑色位；
根据所述第二二值图像的白色位和黑色位，识别所述二维码标签，得到所述单目摄像机对所述二维码标签的识别结果。
作为本发明进一步的改进，根据所述第二二值图像的白色位和黑色位，识别所述二维码标签，得到所述单目摄像机对所述二维码标签的识别结果，包括：
根据所述第二二值图像的白色位和黑色位，确定所述二维码标签的字典类型；
从字典库中查找所述二维码标签的字典类型，获取所述单目摄像机对所述二维码标签的识别结果。
作为本发明进一步的改进，所述主机根据所述各个单目摄像机对所述二维码标签的识别结果确定所述各个单目摄像机的第一定位数据，包括：
所述主机以所述二维码图像的标记点为起点，按照顺时针方向依次获取所述二维码图像的四个角点的图像坐标值，其中，所述二维码图像的标记点为所述二维码图像的四个角点中的一个角点；
所述主机通过P3P算法确定图像坐标系相对于世界坐标系的旋转矩阵R和平移矩阵T，并根据所述旋转矩阵R、所述平移矩阵T和所述二维码图像的四个角点的图像坐标值，获取所述二维码图像的四个角点的世界坐标值；
所述主机根据所述二维码图像的四个角点的世界坐标值，确定所述二维码图像的世界坐标值，作为所述单目摄像机的第一定位数据；
依次类推，所述主机分别确定所述各个单目摄像机的第一定位数据。
作为本发明进一步的改进，所述主机根据所述二维码图像的四个角点的世界坐标值，确定所述二维码图像的世界坐标值，包括：
所述主机根据所述二维码图像的四个角点的世界坐标值，确定目标世界坐标值；
将所述目标世界坐标值作为所述二维码图像的世界坐标值。
作为本发明进一步的改进，所述主机基于所述各个单目摄像机的第一定位数据确定所述巡检机器人的实时位置，包括：
所述主机根据所述各个单目摄像机对所述二维码标签的识别结果，分别确定所述各个单目摄像机的第一定位数据；
所述主机根据各个第一定位数据，确定目标定位结果；
将所述目标定位结果作为所述巡检机器人的实时位置。
作为本发明进一步的改进，所述主机在至少一个单目摄像机无法识别所述二维码标签时，获取所述巡检机器人移动后所述各个单目摄像机的第二定位数据，并对所述UWB标签的UWB定位数据进行补偿和矫正，基于补偿和矫正后的第三定位数据确定所述巡检机器人的实时位置，包括：
所述主机获取所述UWB标签的至少三组UWB定位数据，确定所述至少三组UWB定位数据的第一平均值(xu,yu)；
移动所述巡检机器人，所述主机获取所述各个单目摄像机的至少三组第二定位数据，确定所述至少三组第二定位数据的第二平均值(xc,yc)；
所述主机基于所述第一平均值(xu,yu)和所述第二平均值(xc,yc)，确定误差值(δx,δy)＝((xc-xu),(yc-yu))；
所述主机采用所述误差值(δx,δy)对所述UWB标签的另一组UWB定位数据(xm,ym)进行补偿校正，得到第三定位数据(xr,yr)＝(xm+δx,ym+δy)，作为所述巡检机器人的实时位置。
作为本发明进一步的改进，所述二维码标签包括黑色边框以及所述黑色边框内的二进制矩阵。
作为本发明进一步的改进，所述系统还用于对多个巡检机器人同时定位，其中，每个巡检机器人上设置不同的二维码标签和不同的UWB标签。
本发明的有益效果为：
本发明所述的系统利用单目摄像头对巡检机器人上的二维码标签进行识别，通过世界坐标系，图像坐标系及相机坐标系的转换，实现对巡检机器人的高精度定位，定位精度可精确到厘米级。同时，联合UWB定位数据，对单目摄像头无法覆盖的区域，进行数据融合，最终实现室内的全覆盖高精度定位，定位精度可达15mm。本发明还可以实现多个巡检机器人的同时定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一示例性实施例所述的一种巡检机器人的实时定位系统的示意图；
图2为本发明一示例性实施例所述的单目摄像机的定位示意图；
图3为本发明一示例性实施例所述的姿态转换的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明的描述中，所用术语仅用于说明目的，并非旨在限制本发明的范围。术语“包括”和/或“包含”用于指定所述元件、步骤、操作和/或组件的存在，但并不排除存在或添加一个或多个其他元件、步骤、操作和/或组件的情况。术语“第一”、“第二”等可能用于描述各种元件，不代表顺序，且不对这些元件起限定作用。此外，在本发明的描述中，除非另有说明，“多个”的含义是两个及两个以上。这些术语仅用于区分一个元素和另一个元素。结合以下附图，这些和/或其他方面变得显而易见，并且，本领域普通技术人员更容易理解关于本发明所述实施例的说明。附图仅出于说明的目的用来描绘本发明所述实施例。本领域技术人员将很容易地从以下说明中认识到，在不背离本发明所述原理的情况下，可以采用本发明所示结构和方法的替代实施例。
本发明实施例所述的一种巡检机器人的实时定位系统，如图1所示，所述系统包括：
位于巡检机器人上的UWB标签和二维码标签，其中，所述UWB标签与所述各个UWB锚点匹配设置；
位于室内的各个单目摄像头，用于对所述二维码标签进行识别，获取对所述二维码标签的识别结果；
位于室内的各个UWB锚点，用于与所述UWB标签进行通信，获取所述UWB标签的UWB定位数据；
位于巡检机器人上的主机，用于在所述各个单目摄像机可以识别所述二维码标签时，根据所述各个单目摄像机对所述二维码标签的识别结果确定所述各个单目摄像机的第一定位数据，基于所述各个单目摄像机的第一定位数据确定所述巡检机器人的实时位置；在至少一个单目摄像机无法识别所述二维码标签时，根据所述巡检机器人移动后所述各个单目摄像机的第二定位数据对所述UWB标签的UWB定位数据进行补偿和矫正，基于补偿和矫正后的第三定位数据确定所述巡检机器人的实时位置。
本发明所述的系统结合UWB(超宽带)定位和单目视觉定位，当各个单目摄像机都能拍摄到二维码标签时，根据各个单目摄像机的定位数据确定巡检机器人(图1中的AGV)的实时坐标，当至少一个单目摄像机被遮挡(例如一个单目摄像机被遮挡，例如两个单目摄像机同时被遮挡)，无法拍摄到二维码标签时，利用UWB标签的UWB定位数据对巡检机器人进行辅助定位，将UWB标签的定位数据进行补偿和校正，补偿和矫正后的定位数据作为巡检机器人的实时坐标。
其中，单目摄像机例如可以设置四个，分别设置于室内四面墙上，摄像机的视野(FOV)可以覆盖房间的大部分区域。本发明对单目摄像机的数量和位置不做具体限定，根据房间的大小和摄像机的视野可对各个单目摄像机的具体安装位置进行调整。
其中，UWB锚点例如可以设置四个，分别设置于室内的四面墙。本发明在获取UWB标签的定位数据时采用双向测距方法，测量UWB标签到UWB锚点的飞行时间(TOF)，然后将TOF乘以光速，即可获得UWB锚点与UWB标签之间的距离。本发明对锚点的数量和位置不做具体限定。
举例说明，本发明在获取UWB标签的定位数据时，设置四个UWB锚点，假设UWB锚点A的坐标为xl，y1，z1)，UWB锚点B的坐标为(x2，y2，z2)，UWB锚点C的坐标为(x3，y3，z3)，UWB锚点D的坐标为(x4，y4，z4)，需要求解的UWB标签的坐标为(x，y，z)，则有：
Figure BDA0002608449020000071
Figure BDA0002608449020000072
Figure BDA0002608449020000073
Figure BDA0002608449020000074
将这些式子展开得到：
Figure BDA0002608449020000075
Figure BDA0002608449020000076
Figure BDA0002608449020000077
Figure BDA0002608449020000078
将上述第2、3、4行的式子分别减去第1行的式子，得到：
2(x1-x2)x+2(y1-y2)y+2(z1-z2)z＝λ1
2(x1-x3)x+2(y1-y3)y+2(z1-z3)z＝λ2
2(x1-x4)x+2(y1-y4)y+2(z1-z4)z＝λ3
其中：
Figure BDA0002608449020000079
Figure BDA00026084490200000710
Figure BDA00026084490200000711
转换成矩阵相乘的形式，得到：
Figure BDA00026084490200000712
上式中，R1、R2、R3和R4分别表示UWB锚点A、UWB锚点B、UWB锚点C和UWB锚点D到UWB标签的距离，由此可以根据四个UWB锚点的坐标即可获取UWB标签的定位数据。
其中，单目摄像机例如可以采用型号为MV-CA060-10GM、MV-CA060-10GC的工业相机。UWB锚点例如可以采用型号为D-DWM-PG的UWB定位模块，可以至少达到10cm的测距精度和30cm的定位精度。例如可以在巡检机器人的背部设置一个OLED显示屏，可以直观获取定位结果。主机的USB串口采用CP2102方案，硬件串口更加稳定，且配有电源指示灯与信号指示灯。
在一种可选的实施方式中，所述各个单目摄像头对所述二维码标签进行识别，获取所述二维码标签的识别结果，包括：
一个单目摄像机拍摄所述二维码标签的二维码图像；
基于所述二维码图像，获取所述单目摄像机对所述二维码标签的识别结果；
依次类推，获取所述各个单目摄像机对所述二维码标签的识别结果。
在一种可选的实施方式中，所述二维码标签包括黑色边框以及所述黑色边框内的二进制矩阵。其中，黑色边框有利于二维码图像中的快速检测，内部的二进制矩阵有利于二维码标签的快速识别和纠错。
在一种可选的实施方式中，基于所述二维码图像，获取所述单目摄像机对所述二维码标签的识别结果，包括：
对所述二维码图像进行二值化处理，得到第一二值图像；
从所述第一二值图像中提取所述二维码的轮廓；
基于提取出的轮廓，对所述第一二值图像进行透视变换，得到第二二值图像；
通过OTSU二值化处理获取所述第二二值图像的白色位和黑色位；
根据所述第二二值图像的白色位和黑色位，识别所述二维码标签，得到所述单目摄像机对所述二维码标签的识别结果。
其中，OTSU是一种确定图像二值化分割阈值的最佳算法，该方法也称最大类间差法。OTSU方法计算简单，不受图像亮度和对比度的影响。OTSU方法是按图像的灰度特性，将图像分成背景和前景两部分。因方差是灰度分布均匀性的一种度量，背景和前景之间的类间方差越大，说明构成图像的两部分的差别越大，当部分前景错分为背景或部分背景错分为前景都会导致两部分差别变小。因此，使类间方差最大的分割意味着错分概率最小。
在一种可选的实施方式中，根据所述第二二值图像的白色位和黑色位，识别所述二维码标签，得到所述单目摄像机对所述二维码标签的识别结果，包括：
根据所述第二二值图像的白色位和黑色位，确定所述二维码标签的字典类型；
从字典库中查找所述二维码标签的字典类型，获取所述单目摄像机对所述二维码标签的识别结果。
其中，本发明在字典库中存储了多种字典类型，当第二二值图像的白色位和黑色位在字典库中查找到某一字典类型时，可以获取二维码标签的匹配结果，作为二维码标签的识别结果。
在一种可选的实施方式中，所述主机根据所述各个单目摄像机对所述二维码标签的识别结果确定所述各个单目摄像机的第一定位数据，包括：
所述主机以所述二维码图像的标记点为起点，按照顺时针方向依次获取所述二维码图像的四个角点的图像坐标值，其中，所述二维码图像的标记点为所述二维码图像的四个角点中的一个角点；
所述主机通过P3P算法确定图像坐标系相对于世界坐标系的旋转矩阵R和平移矩阵T，并根据所述旋转矩阵R、所述平移矩阵T和所述二维码图像的四个角点的图像坐标值，获取所述二维码图像的四个角点的世界坐标值；
所述主机根据所述二维码图像的四个角点的世界坐标值，确定所述二维码图像的世界坐标值，作为所述单目摄像机的第一定位数据；
依次类推，所述主机分别确定所述各个单目摄像机的第一定位数据。
如图2所示，在利用单目摄像机进行定位时，得到的是图像坐标系的定位数据，需要将图像坐标系与世界坐标系进行转换，得到世界坐标系的定位数据。
需要建立世界坐标系、相机坐标系和图像坐标系之间的转换关系，其中，Ow代表世界坐标系w的原点，Oc代表相机坐标系c的原点，O代表图像坐标系o的原点。
相机坐标系c相对于世界坐标系w的描述为
Figure BDA0002608449020000091
图像坐标系o相对于相机坐标系c的描述为
Figure BDA0002608449020000092
可以得到图像坐标系o相对于世界坐标系w的描述为
Figure BDA0002608449020000093
Figure BDA0002608449020000094
式中，
Figure BDA0002608449020000101
代表相机坐标系c相对于世界坐标系w的旋转矩阵，
Figure BDA0002608449020000102
代表图像坐标系o相对于相机坐标系c的旋转矩阵，wPo代表相机坐标系c相对于世界坐标系w的平移矩阵，cPo代表图像坐标系o相对于相机坐标系c的平移矩阵。
本发明采用P3P算法进行姿态变换获取旋转矩阵R和平移矩阵T，再根据旋转矩阵R和平移矩阵T得到单目摄像机的定位数据。P3P算法根据针孔模型原理，将3d点投影到相机坐标系再汇集到光心。如图3所示，根据余弦定理：
Figure BDA0002608449020000103
式中，p1、p2、p3分别为图像坐标系中的三个成像点。
将上式中三个表达式除以
Figure BDA0002608449020000104
三个表达式变为：
Figure BDA0002608449020000105
其中，
Figure BDA0002608449020000106
x和y表示二维码图像的角点在世界坐标系的世界坐标值；
进一步得到：
Figure BDA0002608449020000107
其中，
Figure BDA0002608449020000108
u、v和w表示二维码图像的角点在图像坐标系的图像坐标值；
进一步变换得到
Figure BDA0002608449020000109
在三个余弦值
Figure BDA0002608449020000111
和
Figure BDA0002608449020000112
已知时，可以通过二维码图像的角点在图像坐标系的图像坐标值转换得到二维码图像的角点的世界坐标值。
在一种可选的实施方式中，所述主机根据所述二维码图像的四个角点的世界坐标值，确定所述二维码图像的世界坐标值，包括：
所述主机根据所述二维码图像的四个角点的世界坐标值，确定目标世界坐标值；
将所述目标世界坐标值作为所述二维码图像的世界坐标值。
其中，二维码图像的标记点是四个角点检测的起点，也是定位的坐标点，顺时针提取四个角点。在得到四个角点的世界坐标值后，需要进一步确定目标世界坐标值作为所述二维码图像的世界坐标值。例如，可以将四个角点的世界坐标值分别进行比较，选择其中四个角点的世界坐标值的最优解作为目标世界坐标值。例如，可以将四个角点的世界坐标值的平均值作为目标世界坐标值。例如，还可以通过其他补偿、加权等算法对四个角点的世界坐标值进行处理得到目标世界坐标值。本发明对如何利用四个角点的世界坐标值确定目标世界坐标值的具体处理方式不做具体限定。
在一种可选的实施方式中，所述主机基于所述各个单目摄像机的第一定位数据确定所述巡检机器人的实时位置，包括：
所述主机根据所述各个单目摄像机对所述二维码标签的识别结果，分别确定所述各个单目摄像机的第一定位数据；
所述主机根据各个第一定位数据，确定目标定位结果；
将所述目标定位结果作为所述巡检机器人的实时位置。
本发明设置了多个单目摄像机，每个单目摄像机分别对二维码标签进行识别，得到一个第一定位数据。在得到各个第一定位数据后，需要进一步确定目标定位结果作为巡检机器人的实时位置。例如，可以将各个第一定位数据取平均值，将各个第一定位数据分别与平均值计算残差，将残差最小值对应的第一定位数据确定为最佳的定位结果，作为目标定位结果。例如，还可以对各个第一定位数据取平均值，将平均值作为目标定位结果。例如还可以通过其他补偿、加权等算法对各个第一定位数据进行计算确定目标定位结果。本发明对如何利用各个第一定位数据确定目标定位结果的具体处理方式不做具体限定。
在一种可选的实施方式中，所述主机在至少一个单目摄像机无法识别所述二维码标签时，根据所述各个单目摄像机的第二定位数据对所述UWB标签的UWB定位数据进行补偿和矫正，基于补偿和矫正后的第三定位数据确定所述巡检机器人的实时位置，包括：
所述主机获取所述UWB标签的至少三组UWB定位数据，确定所述至少三组UWB定位数据的第一平均值(xu,yu)；
移动所述巡检机器人，所述主机获取所述各个单目摄像机的至少三组第二定位数据，确定所述至少三组第二定位数据的第二平均值(xc,yc)；
所述主机基于所述第一平均值(xu,yu)和所述第二平均值(xc,yc)，确定误差值(δx,δy)＝((xc-xu),(yc-yu))；
所述主机采用所述误差值(δx,δy)对所述UWB标签的另一组UWB定位数据(xm,ym)进行补偿校正，得到第三定位数据(xr,yr)＝(xm+δx,ym+δy)，作为所述巡检机器人的实时位置。
由于障碍物很容易挡住单目摄像机的视野，导致单目摄像机无法识别巡检机器人机身上的二维码标签。在这种情况下，需要读取UWB标签的UMB定位数据作为巡检机器人的实时位置。当单目摄像机能够准确识别二维码标签时，读取单目摄像机的定位数据作为巡检机器人的实时位置，这样可以满足巡检机器人在复杂环境中的定位要求。
如前述所述，本发明利用各个UWB锚点以及双向测距法来获取对所述UWB标签的UWB定位数据。由于各个单目摄像机和UWB标签采用不同的定位方法，当至少一个单目摄像机无法识别巡检机器人机身上的二维码标签时，此时需利用UWB标签的UMB定位数据进行巡检机器人定位。
由于单目摄像机和UWB标签所采用的定位方法均为毫秒级，巡检机器人在移动时以0.2m/s的速度移动，以这种速度可以将至少三组连续的单目摄像机定位数据视为在同一位置测量的数据，并利用这些连续的单目摄像机定位数据对UMB定位数据进行补偿和矫正。
举例说明，在进行单目摄像机定位数据到UMB定位数据的数据切换时，例如，获取前三组UWB定位数据(xu1,yu1)、(xu2,yu2)、(xu3,yu3)，确定三组UWB定位数据的第一平均值(xu,yu)；获取前三个单目摄像机的三组第二定位数据(xc1,yc1)、(xc2,yc2)、(xc3,yc3)，确定三组第二定位数据的第二平均值(xc,yc)；将第二平均值(xc,yc)减去第一平均值(xu,yu)得到误差值(δx,δy)。计算公式为：
Figure BDA0002608449020000131
Figure BDA0002608449020000132
Figure BDA0002608449020000133
将(δx,δy)作补偿和矫正UWB定位数据的补偿因子。
将第四组UWB定位数据(xm,ym)通过补偿因子进行补偿和矫正，得到第三定位数据(xr,yr)＝(xm+δx,ym+δy)，作为所述巡检机器人的实时位置。
本发明利用第二定位数据的平均值和UWB定位数据的平均值来计算补偿因子，对具体采用几组第二定位数据和几组UWB定位数据来计算补偿因子不做具体限定。
在一种可选的实施方式中，所述系统还用于对多个巡检机器人同时定位，其中，每个巡检机器人上设置不同的二维码标签和不同的UWB标签。
本发明所述的系统可以将巡检机器人分别固定在室内的多个坐标测量点上(例如30个，每间隔120cm布置一个坐标测量点)，多个坐标测量点的坐标值组成坐标集。当巡检机器人沿预设路径行驶时，分别记录单目摄像机的定位数据和UWB标签的定位数据，得到例如30组静态数据。其中，在每个坐标测量点重复测量多次(例如100次)，得到每个坐标测量点的定位数据，计算每个坐标测量点的定位数据和坐标集中的该坐标测量点的坐标值之间的欧几里得距离，视为定位误差。
Figure BDA0002608449020000134
上式中，(x，y)代表坐标测量点的坐标值，(xi，yi)代表得到的坐标测量点的定位数据，i表示测量的次数。通过测定，本发明所述的系统可以实现15mm的动态定位精度。
本发明所述的系统利用单目摄像头对巡检机器人上的二维码标签进行识别，通过世界坐标系，图像坐标系及相机坐标系的转换，实现对巡检机器人的高精度定位，定位精度可精确到厘米级。同时，联合UWB定位数据，对单目摄像头无法覆盖的区域，进行数据融合，最终实现室内的全覆盖高精度定位，定位精度可达15mm。本发明还可以实现多个巡检机器人的同时定位。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
此外，本领域普通技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本领域技术人员应理解，尽管已经参考示例性实施例描述了本发明，但是在不脱离本发明的范围的情况下，可进行各种改变并可用等同物替换其元件。另外，在不脱离本发明的实质范围的情况下，可进行许多修改以使特定情况或材料适应本发明的教导。因此，本发明不限于所公开的特定实施例，而是本发明将包括落入所附权利要求范围内的所有实施例。

Claims (9)

1.一种巡检机器人的实时定位系统，其特征在于，所述系统包括：
位于巡检机器人上的UWB标签和二维码标签，其中，所述UWB标签与各个UWB锚点匹配设置；
位于室内的各个单目摄像机，用于对所述二维码标签进行识别，获取对所述二维码标签的识别结果；
位于室内的各个UWB锚点，用于与所述UWB标签进行通信，获取所述UWB标签的UWB定位数据；
位于巡检机器人上的主机，用于在所述各个单目摄像机可以识别所述二维码标签时，根据所述各个单目摄像机对所述二维码标签的识别结果确定所述各个单目摄像机的第一定位数据，基于所述各个单目摄像机的第一定位数据确定所述巡检机器人的实时位置；在至少一个单目摄像机无法识别所述二维码标签时，根据所述巡检机器人移动后所述各个单目摄像机的第二定位数据，并对所述UWB标签的UWB定位数据进行补偿和矫正，基于补偿和矫正后的第三定位数据确定所述巡检机器人的实时位置，包括：
所述主机获取所述UWB标签的至少三组UWB定位数据，确定所述至少三组UWB定位数据的第一平均值
Figure DEST_PATH_IMAGE002A
；
移动所述巡检机器人，所述主机获取所述各个单目摄像机的至少三组第二定位数据，确定所述至少三组第二定位数据的第二平均值
Figure DEST_PATH_IMAGE003
；
所述主机基于所述第一平均值
Figure 999539DEST_PATH_IMAGE004
和所述第二平均值
Figure DEST_PATH_IMAGE005
，确定误差值
Figure 57624DEST_PATH_IMAGE006
；
所述主机采用所述误差值
Figure DEST_PATH_IMAGE007
对所述UWB标签的另一组UWB定位数据
Figure 791356DEST_PATH_IMAGE008
进行补偿校正，得到第三定位数据
Figure DEST_PATH_IMAGE009
，作为所述巡检机器人的实时位置。
2.如权利要求1所述的系统，其中，所述各个单目摄像机对所述二维码标签进行识别，获取所述二维码标签的识别结果，包括：
一个单目摄像机拍摄所述二维码标签的二维码图像；
基于所述二维码图像，获取所述单目摄像机对所述二维码标签的识别结果；
依次类推，获取所述各个单目摄像机对所述二维码标签的识别结果。
3.如权利要求2所述的系统，其中，基于所述二维码图像，获取所述单目摄像机对所述二维码标签的识别结果，包括：
对所述二维码图像进行二值化处理，得到第一二值图像；
从所述第一二值图像中提取所述二维码的轮廓；
基于提取出的轮廓，对所述第一二值图像进行透视变换，得到第二二值图像；
通过OTSU二值化处理获取所述第二二值图像的白色位和黑色位；
根据所述第二二值图像的白色位和黑色位，识别所述二维码标签，得到所述单目摄像机对所述二维码标签的识别结果。
4.如权利要求3所述的系统，其中，根据所述第二二值图像的白色位和黑色位，识别所述二维码标签，得到所述单目摄像机对所述二维码标签的识别结果，包括：
根据所述第二二值图像的白色位和黑色位，确定所述二维码标签的字典类型；
从字典库中查找所述二维码标签的字典类型，获取所述单目摄像机对所述二维码标签的识别结果。
5.如权利要求1所述的系统，其中，所述主机根据所述各个单目摄像机对所述二维码标签的识别结果确定所述各个单目摄像机的第一定位数据，包括：
所述主机以所述二维码图像的标记点为起点，按照顺时针方向依次获取所述二维码图像的四个角点的图像坐标值，其中，所述二维码图像的标记点为所述二维码图像的四个角点中的一个角点；
所述主机通过P3P算法确定图像坐标系相对于世界坐标系的旋转矩阵R和平移矩阵T，并根据所述旋转矩阵R、所述平移矩阵T和所述二维码图像的四个角点的图像坐标值，获取所述二维码图像的四个角点的世界坐标值；
所述主机根据所述二维码图像的四个角点的世界坐标值，确定所述二维码图像的世界坐标值，作为所述单目摄像机的第一定位数据；
依次类推，所述主机分别确定所述各个单目摄像机的第一定位数据。
6.如权利要求5所述的系统，其中，所述主机根据所述二维码图像的四个角点的世界坐标值，确定所述二维码图像的世界坐标值，包括：
所述主机根据所述二维码图像的四个角点的世界坐标值，确定目标世界坐标值；
将所述目标世界坐标值作为所述二维码图像的世界坐标值。
7.如权利要求1所述的系统，其中，所述主机基于所述各个单目摄像机的第一定位数据确定所述巡检机器人的实时位置，包括：
所述主机根据所述各个单目摄像机对所述二维码标签的识别结果，分别确定所述各个单目摄像机的第一定位数据；
所述主机根据各个第一定位数据，确定目标定位结果 ；
将所述目标定位结果作为所述巡检机器人的实时位置。
8.如权利要求1所述的系统，其中，所述二维码标签包括黑色边框以及所述黑色边框内的二进制矩阵。
9.如权利要求1所述的系统，其中，所述系统还用于对多个巡检机器人同时定位，其中，每个巡检机器人上设置不同的二维码标签和不同的UWB标签。
CN202010746225.7A 2020-07-29 2020-07-29 一种巡检机器人的实时定位系统 Active CN111964681B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010746225.7A CN111964681B (zh) 2020-07-29 2020-07-29 一种巡检机器人的实时定位系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010746225.7A CN111964681B (zh) 2020-07-29 2020-07-29 一种巡检机器人的实时定位系统

Publications (2)

Publication Number Publication Date
CN111964681A CN111964681A (zh) 2020-11-20
CN111964681B true CN111964681B (zh) 2021-05-18

Family

ID=73363495

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010746225.7A Active CN111964681B (zh) 2020-07-29 2020-07-29 一种巡检机器人的实时定位系统

Country Status (1)

Country Link
CN (1) CN111964681B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113645581A (zh) * 2021-09-24 2021-11-12 国网上海市电力公司 一种室内巡检机器人超宽带时差法移动定位系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11126198B2 (en) * 2018-12-30 2021-09-21 Ubtech Robotics Corp Robot movement control method, apparatus and robot using the same
CN110163912B (zh) * 2019-04-29 2022-01-11 广州达泊智能科技有限公司 二维码位姿标定方法、装置及系统
CN110262507B (zh) * 2019-07-04 2022-07-29 杭州蓝芯科技有限公司 一种基于5g通信的相机阵列机器人定位方法及装置
CN110345937A (zh) * 2019-08-09 2019-10-18 东莞市普灵思智能电子有限公司 一种基于二维码的导航定姿定位方法和系统
CN110879071B (zh) * 2019-12-06 2021-05-25 成都云科新能汽车技术有限公司 基于车路协同的高精度定位系统及定位方法

Also Published As

Publication number Publication date
CN111964681A (zh) 2020-11-20

Similar Documents

Publication Publication Date Title
CN111462200B (zh) 一种跨视频行人定位追踪方法、系统及设备
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
CN111964680B (zh) 一种巡检机器人的实时定位方法
US11625851B2 (en) Geographic object detection apparatus and geographic object detection method
CN110766758B (zh) 标定方法、装置、系统及存储装置
CN108332752B (zh) 机器人室内定位的方法及装置
WO2012023593A1 (en) Position and orientation measurement apparatus, position and orientation measurement method, and storage medium
WO2019136613A1 (zh) 机器人室内定位的方法及装置
CN112184765B (zh) 一种用于水下航行器的自主跟踪方法
Werman et al. Robot localization using uncalibrated camera invariants
CN106651957B (zh) 基于模板的单目视觉目标空间定位方法
CN112115913B (zh) 图像处理方法、装置及设备、存储介质
CN110827361A (zh) 基于全局标定架的相机组标定方法及装置
Hochdorfer et al. 6 DoF SLAM using a ToF camera: The challenge of a continuously growing number of landmarks
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN113643380A (zh) 一种基于单目相机视觉标靶定位的机械臂引导方法
CN110675341A (zh) 一种单目光视觉引导的水下机器人与海底平台对接方法
CN112197705A (zh) 一种基于视觉和激光测距的果实定位方法
Avgeris et al. Single vision-based self-localization for autonomous robotic agents
Wang et al. Monocular visual SLAM algorithm for autonomous vessel sailing in harbor area
CN111964681B (zh) 一种巡检机器人的实时定位系统
Jiang et al. A ball-shaped target development and pose estimation strategy for a tracking-based scanning system
CN113971697A (zh) 一种空地协同车辆定位定向方法
CN110703773B (zh) 一种使用圆和编码光源作为标识物进行agv定位的方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant