CN102944236B - 基于多个二维码读码器的移动机器人定位系统及方法 - Google Patents

Abstract

本发明涉及一种基于多个二维码读码器的移动机器人定位系统及方法，其特点是：定位系统包括安装在移动机器人小车上的二维码定位控制器、至少两个二维码读码器和分布于地面上的二维码标签，所述的二维码定位控制器由微处理器和通信接口连接构成，所述的通信接口与至少两个二维码读码器相连接；所述定位方法包括：⑴建立三个坐标系；⑵计算在小车坐标系下移动机器人小车相对于两个二维码中心点连线的偏离角度；⑶计算两个二维码中心点连线相对于世界坐标系的偏离角度；⑷计算移动机器人小车相对于世界坐标系的偏离角度，从而获得移动机器人的精确位置。本发明设计合理，实现了对移动机器人的准确定位功能，具有精度高、稳定性强且便于实施等特点。

Description

基于多个二维码读码器的移动机器人定位系统及方法

技术领域

本发明属于移动机器人小车定位领域，尤其是一种基于多个二维码读码器的移动机器人定位系统及方法。

背景技术

目前，移动机器人小车定位方法普遍采用二维码视觉定位方法。现有的移动机器人小车定位方法通常安装一个二维码读码器实现的，其具体方法是：在移动机器人小车的中心位置安装一个二维码读码器，同时在室内地面按照一定间隔敷设一定数量的二维码，机器人小车上的二维码读码器每经过一个二维码，就对该二维码进行拍摄，然后以位于小车中心点的二维码的一组对边和沿着小车前进方向的直线所构成的夹角以及夹角的两条线之间的线性距离作为小车角度偏离的判别标准。上述定位方法存在的问题是：当移动机器人小车比较长时，小车偏离夹角在车体尾部两边之间的线性距离往往几倍大于车体中心点附近的线性距离，一旦中心点偏角有略微的误差，就会造成车体尾部的大幅度误差，这就要求车体中心点的偏角必须十分精确，也就对手工操作和设备精度提出了很高的要求：其一，手工敷设二维码，其二维码的姿态必须完全正确，其四条边和车体十字中心线必须互为垂直，稍微有一点倾斜就会出现中心点的偏角；其二，要求读码设备精度非常高，读出的二维码四个顶点的数据不能有任何误差，否则，也会因计算错误而形成中心点的偏角。以上要求对于实际工作很难做到：首先，人工操作难以保证没有误差，其次，即使是精度非常高的二维码读码器在车体动态运行时，也往往会产生略微的误差。

综上所述，采用一个二维码读码器进行视觉定位存在两个问题：1、施工难度大，对人工和环境设备要求非常苛刻，形成人工成本和设备成本的高投入；2、容错性差，移动机器人运行非常不稳定：由于采用一个二维码读码器定位的方法不能容错人工和设备的误差而常常造成误判断，致使移动机器人不得不长期处于大幅度位姿调整的状态，移动机器人运行时机体晃动、摇摆不定，严重影响了稳定性。

发明内容

本发明的目的在于克服现有技术的不足，提供一种定位准确、稳定性强且便于实施的基于多个二维码读码器的移动机器人定位系统及方法。

本发明解决其技术问题是采取以下技术方案实现的：

一种基于多个二维码读码器的移动机器人定位系统，包括安装在移动机器人小车上的二维码定位控制器、至少两个二维码读码器和分布于地面上的二维码标签，所述的二维码定位控制器由微处理器和通信接口连接构成，所述的通信接口与至少两个二维码读码器相连接，微处理器通过通信接口控制二维码读码器进行二维码图像采集、接收二维码图像并实现移动机器人的精确定位功能。

而且，所述的通信接口为网络接口、USB接口或者1394接口，所述的二维码读码器为具有网络接口、USB接口或者1394接口的摄像头。

而且，所述二维码读码器为两个的时候，其安装在移动机器人小车上的位置为：两个二维码读码器安装在移动机器人小车长度方向的中心线上，一个位于移动机器人小车的中心点，另外一个位于移动机器人小车的尾部，两个二维码读码器之间的距离近似于移动机器人车体长度的二分之一。

一种基于多个二维码读码器的定位方法，包括以下步骤：

⑴建立如下三个坐标系：图像坐标系（u、v）、小车坐标系（x小车、y小车）、世界坐标系（x世界、y世界）；

⑵计算在小车坐标系下移动机器人小车相对于两个二维码中心点连线的偏离角度；

⑶计算两个二维码中心点连线相对于世界坐标系的偏离角度；

⑷计算移动机器人小车相对于世界坐标系的偏离角度，从而获得移动机器人的精确位置。

而且，所述步骤⑵包括以下处理过程：

①标定图像坐标系和小车坐标系的关系：

P1小车(x,y)=R×P1图像(u1,v1)；

P1小车(x,y)=R×P2图像(u2,v2)；

上式中：

P1图像(u1,v1)为空间某一点P1在图像坐标系中的坐标；

P2图像(u2,v2)为空间某一点P2在图像坐标系中的坐标；

P1小车(x,y)为空间某一点P1在小车坐标系下的坐标；

P2小车(x,y)为空间某一点P2在小车坐标系下的坐标；

R为旋转矩阵值；

②将图像坐标系下的四个顶点转换为小车坐标系；

第一个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标为：

P1二维码(x1,y1)=R×P1二维码(u1,v1)

P1二维码(x2,y2)=R×P1二维码(u2,v2)

P1二维码(x3,y3)=R×P1二维码(u3,v3)

P1二维码(x4,y4)=R×P1二维码(u4,v4)

第二个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标为：

P2二维码(x1,y1)=R×P2二维码(u1,v1)

P2二维码(x2,y2)=R×P2二维码(u2,v2)

P2二维码(x3,y3)=R×P2二维码(u3,v3)

P2二维码(x4,y4)=R×P2二维码(u4,v4)

③计算小车坐标系下的两个二维码的中心点坐标：

第一个二维码的中心点在小车坐标系中的坐标为：

X1二维码=（Point11.x+Point12.x+Point13.x+Point14.x）/4；

Y1二维码=（Point11.y+Point12.y+Point13.y+Point14.y）/4；

上式中，Point11.x、Point12.x、Point13.x、Point14.x为第一个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；

第二个二维码的中心点在小车坐标系中的坐标为：

X2二维码=（Point21.x+Point22.x+Point23.x+Point24.x）/4；

Y2二维码=（Point21.y+Point22.y+Point23.y+Point24.y）/4；

上式中，Point21.x、Point22.x、Point23.x、Point24.x为第二个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；

④计算小车坐标系下两个中心点位置偏差△X小车、△Y小车：

△X小车=X2二维码-X1二维码；

△Y小车=Y2二维码-Y1二维码；

⑤计算小车坐标系下的角度偏差tanθ小车_码：

tanθ小车码=△X小车∕△Y小车。

而且，所述步骤⑶包括以下处理过程：

①获得两个二维码在世界坐标系下的坐标：P1(x1世界，y1世界)；p2(x2世界，y2世界)；

②计算得到两个二维码中心点连线相对于世界坐标系的△X世界、△Y世界；

△X世界=x2世界-x1世界；

△Y世界=y2世界-y1世界；

③计算得到两个二维码中心点连线相对于世界坐标系的偏离角度；

tanθ码_世界=△X世界/△Y世界。

而且，所述步骤⑷计算移动机器人小车相对于世界坐标系的偏离角度的公式为：

θ车世界=θ小车_码+θ码_世界

上式中，θ车_世界为移动机器人小车相对于世界坐标系的偏离角度；θ小车_码为移动机器人小车相对于二维码的偏离角度为；θ码_世界为两个二维码中心点连线在世界坐标系下相对于世界坐标系的偏离角度。

本发明的优点和积极效果是：

1、本定位系统在移动机器人小车上安装至少两个二维码读码器实现对移动机器人的精确定位功能，克服了长期以来视觉定位方法中使用一个二维码读码器在车体中心的四个顶点来计算角度的技术偏见，从而使误差范围比原来缩小了几十倍，提高了定位的准确性。

2、本定位方法解决了长期以来二维码定位时过分依赖二维码的姿态和读码设备精度的难题，通过两个二维码中心点连线的方法不仅大幅度提高测量精度还使容错范围得到扩大：即使贴码角度稍有误差或二维码读码器输出略有误差也都不会影响测量的精度，增强了二维码的普及性、易用性和使用效果。

3、本发明设计合理，采用两个二维码读码器测量方法后，车体运行平稳、滑爽，不再出现以前运行过程中的大幅度抖动现象，达到了预期效果，取得了机器人视觉定位的突出效果和显著进步。

附图说明

图1为本发明的系统连接示意图；

图2为两个二维码读码器安装在一个移动机器人小车上的位置示意图；

图3为采用一个二维码读码器测量时的位姿线性误差示意图；

图4为采用二个二维码读码器测量时的偏离角度示意图；

图5为采用二个二维码读码器测量时的中心点位置偏差示意图；

图6为分时获得两个二维码的误差示意图；

图7为移动机器人小车在世界坐标系的偏离角度示意图。

具体实施方式

以下结合附图对本发明实施例做进一步详述：

一种基于多个读码器的移动机器人定位系统和方法，基于在先专利申请（专利名称：基于二维码的移动机器人定位系统和方法，申请号：201210186563.5），关于小车位置的偏差计算已在上个专利中详细说明，本发明中对此不作重复性说明，而只是针对本发明用两个读码器解决小车姿态偏差的内容进行保护，因此，本实施例中也只是围绕本次发明内容进行说明。

一种基于多个二维码读码器的移动机器人定位系统，如图1所示，包括安装在移动机器人小车上的二维码定位控制器、至少两个二维码读码器和分布于地面上的二维码标签，该二维码定位控制器由微处理器和通信接口连接构成，微处理器通过通信接口与至少两个二维码读码器相连接，用于控制二维码读码器进行二维码图像采集、接收二维码读码器采集的二维码图像并实现精确定位功能。如图2所示，两个二维码读码器安装在移动机器人小车上,其安装位置为：在移动机器人小车长度方向的中心线上安装两个二维码读码器，一个位于移动机器人小车的中心点，另外一个位于移动机器人小车的尾部，两个二维码读码器之间的距离近似于移动机器人小车长度的二分之一。所述的通信接口为网络接口、USB接口或者1394接口，所述的二维码读码器为具有网络接口、USB接口或者1394接口的摄像头。

该移动机器人定位系统是通过内置于微处理器内的二维码定位控制软件来实现的，该二维码定位控制软件包括摄像头标定模块、图像处理模块、坐标变换模块和编码值映射模块，所述的摄像头标定模块用于对摄像头进行标定，以建立摄像头和移动机器人小车之间的对应关系；所述的图像处理模块输出二维码四个顶点在图像坐标系中的坐标；所述的编码值映射模块将每个唯一编码值一一对应对应其在室内的实际地理位置；所述的坐标变换模块输出小车位置偏差值、位姿变化值和小车当前的位置和方向。

下面首先对本发明的定位原理进行说明。当采用一个二维码读码器时，如图3所示，小车角度测量是以小车的中心线（沿长度方向）和二维码一组对边的夹角作为测量依据，当二维码的姿态未发生倾斜时，其夹角为0度，当二维码发生了倾斜时，中心点附近2MM的线性偏差将产生小车尾部的几倍于它的偏差，也就是说采用一个二维码编码器会产生很大的偏差。当采用两个二维码读码器时，如图4所示，两个二维码读码器同时拍摄两张二维码图片时，尽管第一个和第二个二维码的姿态都明显发生了倾斜，但此时不再以二维码的倾斜边作为测量依据，而只是和两个二维码的中心点有关系，从图5中看到，虽然两个二维码各自左右倾斜，但中心点是准确的，两个中心点都在小车的中心线上而未发生丝毫的偏离，因此，用两个中心点测量的方法只要求中心点准确，和二维码的姿态没有任何关系；虽然采用两个中心点的方法也会带来一些副作用：如图6所示，两个中心点的位置可能会有偏差，如图7所示，同一时刻会出现只能拍摄到其中一个二维码，而不能同时获取两个码的情况，但是以上两种情况所造成的误差都是非常小，可以忽略不计，尽管两个中心点之间有2MM的误差，但由于一个中心点位于车体的中心，另外一个中心点位于车体的尾部，因此可以看成是整个车体的误差，这与图3采用一个二维码的测量结果相比，误差仅是原来的几分之一；如图7所示，虽然在t1和t2分时获取这两个二维码图像，但由于相隔时间非常短，远远小于车体姿态发生变化的速度，因此，可以假设在这样非常短暂的时间内，小车的姿态未发生任何变化，也就是说t1和t2分时获取的图像采集精度可以视同t1时刻同时获取的图像采集精度。

如图7所示，本定位方法的基本思想和最终目标是实时获得移动机器人小车在世界坐标系下的偏离角度。虽然在小车坐标系下可以测量移动机器人小车相对于两个二维码中心点的偏离角度，但由于不能保证二维码的两个中心点连线同世界坐标系的X轴方向始终保持一致，因此，所计算出的小车相对于二维码的偏离角度并不能代表小车相对于世界坐标系的偏离角度，而调整小车位姿唯有参照世界坐标系才准确，因此必须在计算小车相对于两个二维码中心点连线的角度的同时，还要计算两个二维码中心点连线和世界坐标系的偏离角度。具体为：获取射线OA1相对于射线OX_世界的偏离角度。首先获取OA1相对于OB1的偏离角度，也就是小车相对于两个二维码中心点连线OB1的偏离角度；其次获得OB1相对于OX_世界的偏离角度，也就是两个二维码中心点连线相对于世界坐标系的偏离角度；当以上两个角求出以后，即可获得OA1相对于OX_世界的偏离角度，也就是小车相对于世界坐标系的偏离角度。

一种基于多个二维码读码器的移动机器人小车定位方法，包括以下步骤：

步骤1、建立如下三个坐标系：图像坐标系（u、v）、小车坐标系（x小车、y小车）、世界坐标系（x世界、y世界）。

步骤2：计算在小车坐标系下移动机器人小车相对于两个二维码中心点连线的偏离角度（如图7中的30度角）；

本步骤包括以下处理过程：

①标定图像坐标系和小车坐标系的关系：二维码定位控制器对至少两个二维码读码器进行标定获得旋转矩阵R值，并建立空间某个点在图像坐标系和小车坐标系的转换关系：

P1小车(x,y)=R×P1图像(u1,v1)；

P1小车(x,y)=R×P2图像(u2,v2)；

上式中：

P1图像(u1,v1)为空间某一点P1在图像坐标系中的坐标；

P2图像(u2,v2)为空间某一点P2在图像坐标系中的坐标；

P1小车(x,y)为空间某一点P1在小车坐标系下的坐标

P2小车(x,y)为空间某一点P2在小车坐标系下的坐标；

②将图像坐标系下的四个顶点转换为小车坐标系。二维码定位控制器向至少两个二维码读码器发出二维码图像采集命令，至少两个二维码读码器通过照相方式采集室内二维码标签的二维码图像，并发送给二维码定位控制器；

获取机器人在某时刻t,两个二维码读码器在图像坐标系下分别采集的四个顶点坐标并将其转换为小车坐标系下的坐标；

将第一个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标：

P1二维码(x1,y1)=R×P1二维码(u1,v1)

P1二维码(x2,y2)=R×P1二维码(u2,v2)

P1二维码(x3,y3)=R×P1二维码(u3,v3)

P1二维码(x4,y4)=R×P1二维码(u4,v4)

将第二个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标：

P2二维码(x1,y1)=R×P2二维码(u1,v1)

P2二维码(x2,y2)=R×P2二维码(u2,v2)

P2二维码(x3,y3)=R×P2二维码(u3,v3)

P2二维码(x4,y4)=R×P2二维码(u4,v4)

③计算小车坐标系下的两个二维码的中心点坐标。小车坐标系中两个二维码的中心点坐标分别为X1二维码,Y1二维码、X2二维码,Y2二维码：

第一个二维码的中心点在小车坐标系中的坐标（X1二维码,Y1二维码）：

X1二维码=（Point11.x+Point12.x+Point13.x+Point14.x）/4；

Y1二维码=（Point11.y+Point12.y+Point13.y+Point14.y）/4；

上式中，Point11.x、Point12.x、Point13.x、Point14.x为第一个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；

第二个二维码的中心点在小车坐标系中的坐标（X1二维码,Y1二维码）：

X2二维码=（Point21.x+Point22.x+Point23.x+Point24.x）/4；

Y2二维码=（Point21.y+Point22.y+Point23.y+Point24.y）/4；

上式中，Point21.x、Point22.x、Point23.x、Point24.x为第二个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；

④算小车坐标下两个中心点位置偏差。两个二维码中心点在小车坐标系下的位置偏差分别为△X小车、△Y小车；

△X小车=X2二维码-X1二维码；

△Y小车=Y2二维码-Y1二维码；

⑤计算小车坐标系下的角度偏差。角度偏差用正切函数计算：

tanθ小车_码=△X小车∕△Y小车。

步骤3：计算两个二维码中心点连线相对于世界坐标系的偏离角度（如图7中的15度角）。

本步骤包括以下处理过程：

①二维码读码器读取二维码数据并进行解码，解码后获得两个二维码在世界坐标系下的坐标：P1(x1世界，y1世界)；p2(x2世界，y2世界)；

②计算得到两个二维码中心点连线相对于世界坐标系的△X世界、△Y世界；

△X世界=x2世界-x1世界；

△Y世界=y2世界-y1世界；

③计算得到两个二维码中心点连线相对于世界坐标系的偏离角度；

tanθ码_世界=△X世界/△Y世界；

步骤4：计算移动机器人小车相对于世界坐标系的偏离角度（如图7中的30度+15度=45度）：

θ车_世界=θ小车_码+θ码_世界；

其中θ车_世界为：30度+15度=45度，是移动机器人小车相对于世界坐标系的偏离角度；θ小车_码为：30度，是移动机器人小车相对于二维码的偏离角度为；θ码_世界为：15度，是两个二维码中心点连线在世界坐标系下相对于世界坐标系的偏离角度。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (3)

1.一种基于多个二维码读码器的移动机器人定位方法，其特征在于：包括以下步骤：

⑴建立如下三个坐标系：图像坐标系(u、v)、小车坐标系(x小车、y小车)、世界坐标系(x世界、y世界)；

⑵计算在小车坐标系下移动机器人小车相对于两个二维码中心点连线的偏离角度；

⑶计算两个二维码中心点连线相对于世界坐标系的偏离角度；

⑷计算移动机器人小车相对于世界坐标系的偏离角度，从而获得移动机器人的精确位置；

所述步骤⑵包括以下处理过程：

①标定图像坐标系和小车坐标系的关系：

P1小车(x,y)＝R×P1图像(u1,v1)；

P1小车(x,y)＝R×P2图像(u2,v2)；

上式中：

P1图像(u1,v1)为空间某一点P1在图像坐标系中的坐标；

P2图像(u2,v2)为空间某一点P2在图像坐标系中的坐标；

P1小车(x,y)为空间某一点P1在小车坐标系下的坐标；

P2小车(x,y)为空间某一点P2在小车坐标系下的坐标；

R为旋转矩阵值；

②将图像坐标系下的四个顶点转换为小车坐标系；

第一个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标为：

P1二维码(x1,y1)＝R×P1二维码(u1,v1)

P1二维码(x2,y2)＝R×P1二维码(u2,v2)

P1二维码(x3,y3)＝R×P1二维码(u3,v3)

P1二维码(x4,y4)＝R×P1二维码(u4,v4)

第二个二维码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标为：

P2二维码(x1,y1)＝R×P2二维码(u1,v1)

P2二维码(x2,y2)＝R×P2二维码(u2,v2)

P2二维码(x3,y3)＝R×P2二维码(u3,v3)

P2二维码(x4,y4)＝R×P2二维码(u4,v4)

③计算小车坐标系下的两个二维码的中心点坐标：

第一个二维码的中心点在小车坐标系中的坐标为：

X1二维码＝(Point11.x+Point12.x+Point13.x+Point14.x)/4；

Y1二维码＝(Point11.y+Point12.y+Point13.y+Point14.y)/4；

上式中，Point11.x、Point12.x、Point13.x、Point14.x为第一个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；Point11.y、Point12.y、Point13.y、Point14.y为第一个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的Y轴的坐标；

第二个二维码的中心点在小车坐标系中的坐标为：

X2二维码＝(Point21.x+Point22.x+Point23.x+Point24.x)/4；

Y2二维码＝(Point21.y+Point22.y+Point23.y+Point24.y)/4；

上式中，Point21.x、Point22.x、Point23.x、Point24.x为第二个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；Point21.y、Point22.y、Point23.y、Point24.y为第二个二维码在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的Y轴的坐标；

④计算小车坐标系下两个中心点位置偏差△X小车、△Y小车：

△X小车＝X2二维码-X1二维码；

△Y小车＝Y2二维码-Y1二维码；

⑤计算小车坐标系下的角度偏差tanθ小车_码：

tanθ小车_码＝△X小车/△Y小车；

所述步骤⑶包括以下处理过程：

①获得两个二维码在世界坐标系下的坐标：P1(x1世界，y1世界)；p2(x2世界，y2世界)；

②计算得到两个二维码中心点连线相对于世界坐标系的△X世界、△Y世界；

△X世界＝x2世界-x1世界；

△Y世界＝y2世界-y1世界；

③计算得到两个二维码中心点连线相对于世界坐标系的偏离角度；

tanθ码_世界＝△X世界/△Y世界；

所述步骤⑷计算移动机器人小车相对于世界坐标系的偏离角度的公式为：

θ车_世界＝θ小车_码+θ码_世界

上式中，θ车_世界为移动机器人小车相对于世界坐标系的偏离角度；θ小车_码为移动机器人小车相对于二维码的偏离角度；θ码_世界为两个二维码中心点连线相对于世界坐标系的偏离角度。

2.一种应用如权利要求1所述的基于多个二维码读码器的移动机器人定位方法的基于多个二维码读码器的移动机器人定位系统，其特征在于：所述定位系统包括安装在移动机器人小车上的二维码定位控制器、至少两个二维码读码器和分布于地面上的二维码标签，所述的二维码定位控制器由微处理器和通信接口连接构成，所述的通信接口与至少两个二维码读码器相连接，微处理器通过通信接口控制二维码读码器进行二维码图像采集、接收二维码图像并实现移动机器人的精确定位功能；

所述二维码读码器为两个的时候，其安装在移动机器人小车上的位置为：两个二维码读码器安装在移动机器人小车长度方向的中心线上，一个位于移动机器人小车的中心点，另外一个位于移动机器人小车的尾部，两个二维码读码器之间的距离近似于移动机器人车体长度的二分之一。

3.根据权利要求2所述的基于多个二维码读码器的移动机器人定位系统，其特征在于：所述的通信接口为网络接口、USB接口或者1394接口，所述的二维码读码器为具有网络接口、USB接口或者1394接口的摄像头。