CN108827342A - 扫地机器人的摄像头与里程计坐标标定方法及系统 - Google Patents

Abstract

本发明涉及扫地机器人技术领域，特别涉及一种扫地机器人的摄像头与里程计坐标标定方法及系统。以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于所述二维码的yz轴平面上，所述方法包括：获取摄像头与所述二维码中心之间的相对位置关系，获取里程计与所述二维码中心之间的相对位置关系，根据所述摄像头与所述二维码中心之间的相对位置关系、所述里程计与所述二维码中心之间的相对位置关系，获取所述摄像头与所述里程计之间的相对位置关系。标定摄像头与里程计之间的相对位置关系，旨在解决现有的扫地机器人不考虑摄像头与里程计之间相对位置关系，导致融合得到的位置信息有较大偏差的问题。

Description

扫地机器人的摄像头与里程计坐标标定方法及系统

技术领域

本发明涉及扫地机器人技术领域，特别涉及一种扫地机器人的摄像头与里程计坐标标定方法及系统。

背景技术

随着视觉SLAM技术的成熟，基于单目的视觉定位技术因其价格低廉，定位精度高而逐渐应用于扫地机上。为了保证扫地机器人定位系统的稳定性以及定位的精确性，一般还要融合扫地机器人自身的里程计信息来与视觉做融合。由于摄像头摆放的位置与里程计中心往往不在一起，如果不考虑它们之间的相对位置关系，融合得到的位置信息会有较大偏差。

发明内容

针对现有技术不足，本发明提出一种扫地机器人的摄像头与里程计坐标标定方法及系统，根据摄像头与二维码中心之间的相对位置关系、里程计与二维码中心之间的相对位置关系，获取摄像头与里程计之间的相对位置关系，旨在解决现有的扫地机器人不考虑摄像头与里程计之间相对位置关系，导致融合得到的位置信息有较大偏差的问题。

本发明提出的技术方案是：

一种扫地机器人的摄像头与里程计坐标标定方法，以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于所述二维码的yz轴平面上，所述方法包括：

获取摄像头与所述二维码中心之间的相对位置关系；

获取里程计与所述二维码中心之间的相对位置关系；

根据所述摄像头与所述二维码中心之间的相对位置关系、所述里程计与所述二维码中心之间的相对位置关系，获取所述摄像头与所述里程计之间的相对位置关系。

进一步地，在所述获取摄像头与所述二维码中心之间的相对位置关系的步骤中，包括：

通过二维码定位获取摄像头与所述二维码中心之间的相对位置关系，所述二维码定位包括线检测、四边形检测、计算单应性矩阵和外参。

进一步地，在所述获取摄像头与所述二维码中心之间的相对位置关系的步骤之前，所述方法包括：

检测所述摄像头拍摄的图像中是否存在所述二维码；

若不存在，则原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

进一步地，在所述原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码的步骤中，包括：

按预设的旋转角度原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

进一步地，在所述获取摄像头与所述二维码中心之间的相对位置关系的步骤之前，包括：

将所述扫地机器人的坐标系进行变换，使扫地机器人的坐标系三轴方向与所述二维码中心的三轴方向相同。

本发明还提供一种扫地机器人的摄像头与里程计坐标标定系统，以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于所述二维码的yz轴平面上，所述系统包括：

第一获取模块，用于获取摄像头与所述二维码中心之间的相对位置关系；

第二获取模块，用于获取里程计与所述二维码中心之间的相对位置关系；

第三获取模块，用于根据所述摄像头与所述二维码中心之间的相对位置关系、所述里程计与所述二维码中心之间的相对位置关系，获取所述摄像头与所述里程计之间的相对位置关系。

进一步地，所述二维码定位包括线检测、四边形检测、计算单应性矩阵和外参，所述第一获取模块包括：

第一子获取模块，用于通过二维码定位获取摄像头与所述二维码中心之间的相对位置关系。

进一步地，所述系统包括：

检测模块，用于检测所述摄像头拍摄的图像中是否存在所述二维码；

旋转模块，用于若不存在，则原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

进一步地，所述旋转模块包括：

子旋转模块，用于按预设的旋转角度原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

进一步地，所述系统包括：

坐标变换模块，用于将所述扫地机器人的坐标系进行变换，使扫地机器人的坐标系三轴方向与所述二维码中心的三轴方向相同。

根据上述的技术方案，本发明有益效果：根据摄像头与二维码中心之间的相对位置关系、里程计与二维码中心之间的相对位置关系，获取摄像头与里程计之间的相对位置关系，标定摄像头与里程计之间的相对位置关系，旨在解决现有的扫地机器人不考虑摄像头与里程计之间相对位置关系，导致融合得到的位置信息有较大偏差的问题。

附图说明

图1是应用本发明实施例提供的扫地机器人的摄像头与里程计坐标标定方法的流程图；

图2是应用本发明实施例提供的扫地机器人的摄像头与里程计坐标标定系统的功能模块图；

图3是应用本发明实施例提供的扫地机器人的摄像头与二维码的坐标系示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图1和图3所示，本发明实施例提出一种扫地机器人的摄像头与里程计坐标标定方法，以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于二维码的yz轴平面上，所述方法包括以下步骤：

步骤S101、获取摄像头与二维码中心之间的相对位置关系。

将二维码垂直于地面放置，以二维码中心为坐标原点建立坐标系，扫地机器人放置在地面上，并正对二维码，扫地机器人的中心轴线位于二维码的yz轴平面上，摄像头的拍摄口朝向二维码，以二维码中心为坐标原点为坐标系，获取摄像头与二维码中心之间的相对位置关系。

在一些实施例中，在步骤S101之前，所述方法包括：

检测摄像头拍摄的图像中是否存在二维码；

若不存在，则原地旋转扫地机器人，直至摄像头拍摄的图像中存在二维码。

开启摄像头，并使用摄像头拍摄图像，在摄像头拍摄到图像之后，检测摄像头拍摄的图像中是否存在二维码，若摄像头拍摄的图像中不存在二维码，说明摄像头的拍摄口没有朝向二维码，则原地旋转扫地机器人，改变摄像头的拍摄口朝向，直至摄像头拍摄的图像中存在二维码，也就是，原地旋转扫地机器人，原地旋转一定角度之后，停止原地旋转并进行拍摄，检测摄像头拍摄的图像中是否存在二维码，若不存在，再次原地旋转扫地机器人，直到检测摄像头拍摄的图像中存在二维码，不再原地旋转。

在原地旋转扫地机器人，直至摄像头拍摄的图像中存在二维码的步骤中，包括：

按预设的旋转角度原地旋转扫地机器人，直至摄像头拍摄的图像中存在二维码。

每次原地旋转扫地机器人的旋转角度为预设旋转角度，具体地，预设的旋转角度为15°。

在本实施例中，在步骤S101中，包括：

通过二维码定位获取摄像头与二维码中心之间的相对位置关系。

摄像头拍摄二维码，通过二维码定位获取摄像头与二维码中心之间的相对位置关系。

二维码定位包括线检测、四边形检测、计算单应性矩阵和外参。

具体地，线检测：计算图像中每个像素的梯度方向和梯度大小，然后利用像素点梯度的相似性度量，具有相似的梯度信息的相邻的像素点被合并成一个整体。采用类似图割的方法，图的节点为一个像素点，边的权重为两个像素点(区域)的梯度相似性。对于像素(区域)n，利用D(n)代表其梯度方向，M(n)代表梯度值大小，像素(区域)n与像素(区域)m合并的条件是：

D(n∪m)≤min(D(n),D(m))+KD/|n∪m|

M(n∪m)≤min(M(n),M(m))+KM/|n∪m|

四边形检测：将检测出的线通过空间相邻准则连接构成多边形，通过对多边形边长的限制和对多边形所构成的角点的个数对多边形的数量进行限制，得到四边形，空间相邻的四边形则合并成新的四边形，最终得到一个包含很多0,1编码(0,1代表小的四边形)的大四边形。在检测到四边形之后，通过对大四边形的编码和预先设定的编码类型对比计算距离，得到更加准确的检测目标。

计算单应性矩阵和外参：单应性矩阵代表在二维码坐标系上2D点投影到摄像头坐标系所进行的其次变换，可以通过直接线性变化法(Direct Linear Transformalgorithm)来求得。相机内参用P表示，包括相机焦距，中心偏差。外参用E表示。则单应性矩阵可以写为如下形式：

其中，Rij(i,j＝0,1,2)代表旋转参数，Tk(k＝x,y,z)代表平移参数。

由于旋转矩阵的列必须是单位大小，再根据二维码与摄像头的对应的方向信息，其中，二维码出现在摄像头的前面，可以获得s的大小与方向。旋转矩阵的第三列可以通过计算两个已知列的交叉乘积来恢复，因为旋转的列矩阵必须是正交的。由此可以得到二维码相对于摄像头的相对位置关系。

通过二维码定位获取摄像头与二维码中心之间的相对位置关系T，记为(x,0,z),其中，扫地机器人只在水平面上移动，y轴不用标定，y轴的坐标信息设为0。

在通过二维码定位获取摄像头与二维码中心之间的相对位置关系的步骤之后，方法包括：

获取摄像头与扫地机器人中心之间相对旋转矩阵。

由于扫地机器人在xz轴移动，y轴是不变的，根据二维码定位获取摄像头与二维码中心之间的相对位置关系，可以得到摄像头与扫地机器人中心之间的相对旋转矩阵即为R。

在步骤S101之前，所述方法包括：

将扫地机器人的坐标系进行变换，使扫地机器人的坐标系三轴方向与二维码中心的三轴方向相同。

在本实施例中，将扫地机器人的坐标系统进行变换，使扫地机器人的坐标系xyz三轴与二维码中心的xyz三轴方向相同，方便后续扫地机器人的位置信息计算。

步骤S102、获取里程计与二维码中心之间的相对位置关系。

在本实施例中，以二维码中心为坐标原点，里程计在二维码的yz轴平面上，扫地机器人只在水平面上移动，y轴不用标定，y轴的坐标信息设为0。为此，里程计坐标为(0，0，s)。

步骤S103、根据摄像头与二维码中心之间的相对位置关系、里程计与二维码中心之间的相对位置关系，获取摄像头与里程计之间的相对位置关系。

根据摄像头与二维码中心之间的相对位置关系T(x,0,z)，里程计与二维码中心之间的相对位置关系(0，0，s)，为此，通过计算可以得到摄像头与里程计之间的相对位置关系(x,0,z-s)。

综上所述,根据摄像头与二维码中心之间的相对位置关系、里程计与二维码中心之间的相对位置关系，获取摄像头与里程计之间的相对位置关系，标定摄像头与里程计之间的相对位置关系，旨在解决现有的扫地机器人不考虑摄像头与里程计之间相对位置关系，导致融合得到的位置信息有较大偏差的问题。

如图2和图3所示，本发明实施例还提出一种扫地机器人的摄像头与里程计坐标标定系统1，以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于二维码的yz轴平面上，系统1包括第一获取模块11、第二获取模块12和第三获取模块13。

第一获取模块11，用于获取摄像头与二维码中心之间的相对位置关系。

将二维码垂直于地面放置，以二维码中心为坐标原点建立坐标系，扫地机器人放置在地面上，并正对二维码，扫地机器人的中心轴线位于二维码的yz轴平面上，摄像头的拍摄口朝向二维码，以二维码中心为坐标原点为坐标系，获取摄像头与二维码中心之间的相对位置关系。

在一些实施例中，系统1包括：

检测模块，用于检测摄像头拍摄的图像中是否存在二维码；

旋转模块，用于若不存在，则原地旋转扫地机器人，直至摄像头拍摄的图像中存在二维码。

开启摄像头，并使用摄像头拍摄图像，在摄像头拍摄到图像之后，检测摄像头拍摄的图像中是否存在二维码，若摄像头拍摄的图像中不存在二维码，说明摄像头的拍摄口没有朝向二维码，则原地旋转扫地机器人，改变摄像头的拍摄口朝向，直至摄像头拍摄的图像中存在二维码，也就是，原地旋转扫地机器人，原地旋转一定角度之后，停止原地旋转并进行拍摄，检测摄像头拍摄的图像中是否存在二维码，若不存在，再次原地旋转扫地机器人，直到检测摄像头拍摄的图像中存在二维码，不再原地旋转。

旋转模块包括：

子旋转模块，用于按预设的旋转角度原地旋转扫地机器人，直至摄像头拍摄的图像中存在二维码。

每次原地旋转扫地机器人的旋转角度为预设旋转角度，具体地，预设的旋转角度为15°。

在本实施例中，第一获取模块11包括：

第一子获取模块，用于通过二维码定位获取摄像头与二维码中心之间的相对位置关系。

摄像头拍摄二维码，通过二维码定位获取摄像头与二维码中心之间的相对位置关系。

二维码定位包括线检测、四边形检测、计算单应性矩阵和外参。

具体地，线检测：计算图像中每个像素的梯度方向和梯度大小，然后利用像素点梯度的相似性度量，具有相似的梯度信息的相邻的像素点被合并成一个整体。采用类似图割的方法，图的节点为一个像素点，边的权重为两个像素点(区域)的梯度相似性。对于像素(区域)n，利用D(n)代表其梯度方向，M(n)代表梯度值大小，像素(区域)n与像素(区域)m合并的条件是：

D(n∪m)≤min(D(n),D(m))+KD/|n∪m|

M(n∪m)≤min(M(n),M(m))+KM/|n∪m|

四边形检测：将检测出的线通过空间相邻准则连接构成多边形，通过对多边形边长的限制和对多边形所构成的角点的个数对多边形的数量进行限制，得到四边形，空间相邻的四边形则合并成新的四边形，最终得到一个包含很多0,1编码(0,1代表小的四边形)的大四边形。在检测到四边形之后，通过对大四边形的编码和预先设定的编码类型对比计算距离，得到更加准确的检测目标。

计算单应性矩阵和外参：单应性矩阵代表在二维码坐标系上2D点投影到摄像头坐标系所进行的其次变换，可以通过直接线性变化法(Direct Linear Transformalgorithm)来求得。相机内参用P表示，包括相机焦距，中心偏差。外参用E表示。则单应性矩阵可以写为如下形式：

其中，Rij(i,j＝0,1,2)代表旋转参数，Tk(k＝x,y,z)代表平移参数。

由于旋转矩阵的列必须是单位大小，再根据二维码与摄像头的对应的方向信息，其中，二维码出现在摄像头的前面，可以获得s的大小与方向。旋转矩阵的第三列可以通过计算两个已知列的交叉乘积来恢复，因为旋转的列矩阵必须是正交的。由此可以得到二维码相对于摄像头的相对位置关系。

通过二维码定位获取摄像头与二维码中心之间的相对位置关系T，记为(x,0,z),其中，扫地机器人只在水平面上移动，y轴不用标定，y轴的坐标信息设为0。

系统1包括：

第四获取模块，用于获取摄像头与扫地机器人中心之间相对旋转矩阵。

由于扫地机器人在xz轴移动，y轴是不变的，根据二维码定位获取摄像头与二维码中心之间的相对位置关系，可以得到摄像头与扫地机器人中心之间的相对旋转矩阵即为R。

系统1包括：

坐标变换模块，用于将扫地机器人的坐标系进行变换，使扫地机器人的坐标系三轴方向与二维码中心的三轴方向相同。

在本实施例中，将扫地机器人的坐标系统进行变换，使扫地机器人的坐标系xyz三轴与二维码中心的xyz三轴方向相同，方便后续扫地机器人的位置信息计算。

第二获取模块12，用于获取里程计与二维码中心之间的相对位置关系。

在本实施例中，以二维码中心为坐标原点，里程计在二维码的yz轴平面上，扫地机器人只在水平面上移动，y轴不用标定，y轴的坐标信息设为0。为此，里程计坐标为(0，0，s)。

第三获取模块13，用于根据摄像头与二维码中心之间的相对位置关系、里程计与二维码中心之间的相对位置关系，获取摄像头与里程计之间的相对位置关系。

根据摄像头与二维码中心之间的相对位置关系T(x,0,z)，里程计与二维码中心之间的相对位置关系(0，0，s)，为此，通过计算可以得到摄像头与里程计之间的相对位置关系(x,0,z-s)。

综上所述,根据摄像头与二维码中心之间的相对位置关系、里程计与二维码中心之间的相对位置关系，获取摄像头与里程计之间的相对位置关系，标定摄像头与里程计之间的相对位置关系，旨在解决现有的扫地机器人不考虑摄像头与里程计之间相对位置关系，导致融合得到的位置信息有较大偏差的问题。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种扫地机器人的摄像头与里程计坐标标定方法，以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于所述二维码的yz轴平面上，其特征在于，所述方法包括：

获取摄像头与所述二维码中心之间的相对位置关系；

获取里程计与所述二维码中心之间的相对位置关系；

根据所述摄像头与所述二维码中心之间的相对位置关系、所述里程计与所述二维码中心之间的相对位置关系，获取所述摄像头与所述里程计之间的相对位置关系。

2.根据权利要求1所述的扫地机器人的摄像头与里程计坐标标定方法，其特征在于，在所述获取摄像头与所述二维码中心之间的相对位置关系的步骤中，包括：

通过二维码定位获取摄像头与所述二维码中心之间的相对位置关系，所述二维码定位包括线检测、四边形检测、计算单应性矩阵和外参。

3.根据权利要求1所述的扫地机器人的摄像头与里程计坐标标定方法，其特征在于，在所述获取摄像头与所述二维码中心之间的相对位置关系的步骤之前，所述方法包括：

检测所述摄像头拍摄的图像中是否存在所述二维码；

若不存在，则原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

4.根据权利要求3所述的扫地机器人的摄像头与里程计坐标标定方法，其特征在于，在所述原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码的步骤中，包括：

按预设的旋转角度原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

5.根据权利要求1所述的扫地机器人的摄像头与里程计坐标标定方法，其特征在于，在所述获取摄像头与所述二维码中心之间的相对位置关系的步骤之前，包括：

将所述扫地机器人的坐标系进行变换，使扫地机器人的坐标系三轴方向与所述二维码中心的三轴方向相同。

6.一种扫地机器人的摄像头与里程计坐标标定系统，以二维码中心为坐标原点建立坐标系，扫地机器人的中心轴线位于所述二维码的yz轴平面上，其特征在于，所述系统包括：

第一获取模块，用于获取摄像头与所述二维码中心之间的相对位置关系；

第二获取模块，用于获取里程计与所述二维码中心之间的相对位置关系；

第三获取模块，用于根据所述摄像头与所述二维码中心之间的相对位置关系、所述里程计与所述二维码中心之间的相对位置关系，获取所述摄像头与所述里程计之间的相对位置关系。

7.根据权利要求6所述的扫地机器人的摄像头与里程计坐标标定系统，其特征在于，所述二维码定位包括线检测、四边形检测、计算单应性矩阵和外参，所述第一获取模块包括：

第一子获取模块，用于通过二维码定位获取摄像头与所述二维码中心之间的相对位置关系。

8.根据权利要求6所述的扫地机器人的摄像头与里程计坐标标定系统，其特征在于，所述系统包括：

检测模块，用于检测所述摄像头拍摄的图像中是否存在所述二维码；

旋转模块，用于若不存在，则原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

9.根据权利要求8所述的扫地机器人的摄像头与里程计坐标标定系统，其特征在于，所述旋转模块包括：

子旋转模块，用于按预设的旋转角度原地旋转所述扫地机器人，直至所述摄像头拍摄的图像中存在所述二维码。

10.根据权利要求6所述的扫地机器人的摄像头与里程计坐标标定系统，其特征在于，所述系统包括：

坐标变换模块，用于将所述扫地机器人的坐标系进行变换，使扫地机器人的坐标系三轴方向与所述二维码中心的三轴方向相同。