CN103123682A - 基于规则图形码复合标签的移动机器人定位系统及方法 - Google Patents

Abstract

本发明涉及一种基于规则图形码复合标签的移动机器人定位系统及方法，该系统包括规则图形码定位控制器、规则图形码读码器、射频标签读码器以及分布于地面上的规则图形码复合标签，该复合标签由规则图形码标签和RFID标签复合构成；该方法包括：⑴建立图像坐标系、小车坐标系、世界坐标系；⑵计算在小车坐标系下移动机器人小车相对于复合标签的偏离位置和位置角度；⑶计算复合标签相对于世界坐标系的偏离位置和角度；⑷计算移动机器人小车相对于世界坐标系的偏离位置和角度。本发明设计合理，有效地将规则图形码标签和射频标签的特性结合在一起，实现了对移动机器人小车的精确定位功能，提高了定位效率。

Description

基于规则图形码复合标签的移动机器人定位系统及方法

技术领域

本发明属于移动机器人技术领域，尤其是一种基于规则图形码复合标签的移动机器人定位系统及方法。

背景技术

目前，二维码定位方式逐渐应用于移动机器人定位系统中。其应用方法通常是：利用二维码（QR码）的硬件特性：三个顶角的位置探测图形读取、计算出二维码四个顶点坐标和中心点坐标，进而计算出小车和二维码之间的姿态偏差；通过对二维码的数据区写入数据（如二维码的绝对坐标），并进行解码，从而计算小车和二维码的绝对位置偏差。上述方法在定位被测物体的方向或姿态上比传统视觉定位的图像处理方法有着非常明显的进步，但是，由于二维码（QR码）定位仍然属于图像处理的范畴，仍然摆脱不了视觉定位的种种条件限制，没有从根本上实现突破，尽管可以利用图形探测区的数据高效率地定位移动移动机器人小车的方向，但是，对于二维码数据区的解码效果往往不尽人意。由于二维码定位精度受光源的照度、码本身的清晰度，相机视野的影响，常常出现数据区的数据解码解不出来的现象，在被测的二维码中，近三分之一的二维码虽然能够正确返回三个顶点坐标，但是，数据区解码失败。解码失败原因和二维码（QR码）的结构有关：二维码（QR码）是由N行N列个基本图形模块组成的，根据不同版本，分别从21×21到7×7一个模块，其中三个顶角的位置探测图形在二维码中占有很大的比例，最多占整个二维码面积的二分之一。位置探测区域以外，是由基本数据模块组成的数据区，用来存储数据，由于一个位置探测图形是由8个基本数据模块组成，和基本数据模块相比，比例悬殊，在图像扫描中比较容易被识别，因此，三个顶点的数据常常容易被采集和成功解码，由于数据区域的基本数据模块面积远远小于二维码三个顶角的位置探测图形面积，当光源照度弱、码的清晰度差、相机视野欠佳时，面积越小，识别率越低、清晰度越差，因此二维码数据区的解码效率远远低于三个顶角的位置探测图形的解码效率，所以才经常出现近三分之一的二维码其数据区解码解不出来的问题。

针对二维码数据区解码问题，长期以来，人们总是从围绕二维码的环境因素上去考虑解决问题的方法，想方设法通过增加光源照度、提高二维码清晰度，寻找最佳的相机视野来提高二维码数据区解码效率，但无论怎样都效果甚微，不能产生实质性的突破，其原因就是思维的局限性，不能跳出二维码的视野去想问题。

发明内容

本发明的目的在于克服现有技术的不足，提供一种设计合理、解码准确率高且定位精确的基于规则图形码复合标签的移动机器人定位系统及方法。

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

一种基于规则图形码复合标签的移动机器人定位系统，包括安装在移动机器人小车上的规则图形码定位控制器、规则图形码读码器、射频标签读码器以及分布于地面上的规则图形码复合标签,该规则图形码复合标签由可以通过计算得到其中心点和方向的规则图形码标签和RFID标签复合构成，规则图形码定位控制器与规则图形码读码器、射频标签读码器相连接，用于控制规则图形码读码器和射频标签读码器进行规则图形码图像读取、RFID位置信息采集并实现移动机器人的精确定位功能。

而且，所述的规则图形码标签和RFID标签复合方式为：上下重叠式摆放或并列式摆放。

而且，所述的规则图形码标签由一个或多个位置探测图形构成；所述的规则图形码标签可以为正方形、圆形、三角形或长方形，所述的位置探测图形可以为正方形、圆形、三角形或长方形。

而且，所述的规则图形码标签用于计算规则图形的方向和中心点，所述的RFID射频标签用于设置规则图形码标签的绝对值坐标。

而且，所述的规则图形码定位控制器由微处理器、编码器和通信接口连接构成，该微处理器与编码器相连接用于检测移动机器人的运行速度，该微处理器通过通信接口与规则图形码读码器、射频标签读码器相连接。

而且，所述的通信接口为网络接口、USB接口或者1394接口；所述的规则图形码读码器为具有网络接口、USB接口或者1394接口的摄像头；所述的射频标签读码器为具有网络接口、USB接口或者1394接口的RFID射频标签读码器。

一种基于规则图形码复合标签的移动机器人定位方法，包括以下步骤：

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

⑵计算在小车坐标系下移动机器人小车相对于规则图形码复合标签的偏离角度和偏离位置；

⑶计算规则图形码复合标签相对于世界坐标系的偏离位置和偏离角度；

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

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

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

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

上式中：

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

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

R为旋转矩阵值；

②将图像坐标系下的1个或多个顶点转换为小车坐标系，转换后的坐标为：

P_{规则图形码复合标签}(x1,y1)=R×P_{规则图形码复合标签}(u1,v1)；

P_{规则图形码复合标签}(x2,y2)=R×P_{规则图形码复合标签}(u2,v2)；

P_{规则图形码复合标签}(x3,y3)=R×P_{规则图形码复合标签}(u3,v3)；

P_{规则图形码复合标签}(x4,y4)=R×P_{规则图形码复合标签}(u4,v4)；

上式中，P_{规则图形码复合标签}(x1,y1)、P_{规则图形码复合标签}(x2,y2)、P_{规则图形码复合标签}(x3,y3)、P_{规则图形码复合标签}(x4,y4)为规则图形码复合标签在小车坐标系中的四个顶点坐标，P_{规则图形码复合标签}(u1,v1)、P_{规则图形码复合标签}(u2,v2)、P_{规则图形码复合标签}(u3,v3)、P_{规则图形码复合标签}(u4,v4)为规则图形码复合标签在图像坐标系统中的四个顶点坐标；

③计算小车坐标系下移动机器人小车相对于规则图形码复合标签的姿态变化θ₁

θ₁=atan2(Point2.x+Point3.x-Point1.x-Point4.x,Point2.y+Point3.y-Point1.y-Point4.y)；

上式中，Point1.x、Point2.x、Point3.x、Point4.x为规则图形码复合标签在小车坐标系下的四个顶点的X轴坐标；Point1.y、Point2.y、Point3.y、Point4.y为规则图形码复合标签在小车坐标系下的四个顶点的Y轴坐标；

④计算小车坐标系下规则图形码复合标签中心点的坐标：

X_{规则图形码复合标签中心}，Y_{规则图形码复合标签中心}

其中，X_{规则图形码复合标签中心}为小车坐标系下规则图形码复合标签中心点的X轴坐标，Y_{规则图形码复合标签中心}为小车坐标系下规则图形码复合标签中心点的Y轴坐标；

⑤计算整张图像中心点在图像坐标系中的坐标P_图像中心（u，v）：

计算方法为：直接用整张图像的长、宽尺寸再各减半得到；

⑥将整张图像的中心点坐标转换为小车坐标系坐标：

P_图像中心（x，y）=R×P_图像中心（u，v），

上式中，X_图像中心为图像中心点在小车坐标系下的X轴坐标，Y_图像中心为图像中心点在小车坐标系下的Y轴坐标；

⑦计算小车坐标系下移动机器人小车相对于规则图形码复合标签的位置变化：

移动机器人小车在X方向的偏差x₁=X_{规则图形码复合标签中心}-X_图像中心；

移动机器人小车在Y方向的偏差y₁=Y_{规则图形码复合标签中心}-Y_图像中心。

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

①射频标签读码器对RFID射频标签进行解码，获得规则图形码复合标签中心点相对于世界坐标系的位置偏差（x₀，y₀）；其中，x₀为世界坐标系下X轴方向的偏差值，y₀为世界坐标系下Y轴方向的偏差值；

②获得规则图形码复合标签中心点相对于世界坐标系的角度偏差。

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

①计算移动机器人小车相对于世界坐标系的偏离角度：

θ=θ₀-θ₁

上式中，θ₀为世界坐标系下规则图形码复合标签相对于世界坐标系的角度偏差，θ₁为小车坐标系下移动机器人小车相对于规则图形码复合标签的角度偏差；

②计算移动机器人在世界坐标系下相对于规则图形码复合标签的偏差值：

af为移动机器人小车在世界坐标系下相对于规则图形码复合标签的X轴方向偏差；

cf为移动机器人小车在世界坐标系下相对于规则图形码复合标签的Y轴方向偏差；

③计算移动机器人小车在世界坐标系下的坐标值：

移动机器人小车X轴方向绝对坐标值x=x₀-af；

移动机器人小车Y轴方向绝对坐标值y=y₀-cf；

上式中，当af小于x₀时为负数，反之为正数；当cf小于y₀时为负数，反之为正数；x为移动机器人小车X方向绝对坐标值；y为移动机器人小车Y方向绝对坐标值；x₀为规则图形码复合标签的X轴绝对坐标值，y₀为规则图形码复合标签的Y轴绝对坐标值。

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

1、本发明充分利用规则图形码标签和射频标签的不同特性，以及它们尺寸相同和中心点相同的共性，将移动机器人小车定位分为相对定位、绝对定位和移动机器人小车定位三个阶段，在相对定位阶段使用摄像头和规则图形码定位小车坐标系下移动机器人小车相对于规则图形码的位置和姿态，在绝对定位阶段使用射频标签读码器读取RFID的绝对坐标，进而定位规则图形码复合标签在世界坐标系下的位置和姿态，在移动机器人小车定位阶段将前两个阶段的数据附加到一起，最终获得了移动机器人小车在世界坐标系下的位置和姿态，从而实现了对移动机器人的精确定位功能。

2、本发明设计合理，所采用的规则图形码复合标签吸取了二维码（QR码）在计算方向和中心点方面的优势和特点，并在此基础上发扬光大：其外形的多样化和内部图形组合的多样化对比二维码（QR码）都更具灵活性、拓展性、简洁性；规则图形码复合标签还通过集成RFID射频标签的方法弥补了二维码（QR码）应用于AGV解码时的不足，有效提高了规则图形码复合标签的解码效率，克服了机器人视觉定位采用单一技术所不能逾越的鸿沟，从根本上跨越了视觉定位技术领域天然的技术屏障，消除了人们长期以来用单一技术解决视觉定位问题的偏见和思想禁锢，有效地解决了视觉定位系统过于复杂，难于实现精确定位的问题。

附图说明

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

图2为由3个探测图形组成的规则图形码标签示意图；

图3为由1个探测图形组成的规则图形码标签示意图；

图4为前后摆放的规则图形码复合标签示意图；

图5为本发明的定位方法处理流程图；

图6为本发明所使用的三个坐标系示意图；

图7为小车坐标系下计算小车相对于规则图形码复合标签的位置偏差和角度偏差示意图；

图8为世界坐标系下计算小车相对于规则图形码复合标签的位置偏差示意图；

图9为用a、b、c三个顶点坐标计算d点坐标示意图；

图10用x1、x2、x3、x4四个顶点坐标计算中心点坐标示意图。

具体实施方式

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

一种基于规则图形码复合标签的移动机器人定位系统，如图1所示，包括安装在移动机器人小车上的规则图形码定位控制器、规则图形码读码器、射频标签读码器以及分布于地面上的规则图形码复合标签，规则图形码定位控制器通过规则图形码读码器、射频标签读码器读取一定数量的规则图形码复合标签实现对移动机器人的精确引导功能。本移动机器人定位系统为三层系统结构，第一层为安装在移动机器人上的规则图形码定位控制器，该规则图形码定位控制器由编码器、微处理器和通信接口连接构成，该微处理器与编码器相连接用于检测移动机器人的运行速度，该微处理器通过通信接口与至少一个规则图形码读码器、至少一个射频标签读码器相连接，用于控制规则图形码读码器和射频标签读码器进行规则图形读取、RFID位置信息采集与处理并实现移动机器人的精确定位功能，所述的通信接口为网络接口、USB接口或者1394接口。第二层为安装在移动机器人上的读码器层，读码器层包括规则图形读吗器和射频标签读码器，该规则图形码读码器为具有网络接口、USB接口或者1394接口的摄像头，用于读取规则图形码复合标签中的规则图形信息并传送给规则图形码定位控制器；该射频标签读码器为具有网络接口、USB接口或者1394接口的RFID射频标签读码器，用于读取规则图形码复合标签中的RFID位置信息并传送给规则图形码定位控制器。第三层为安装在地面上的规则图形码复合标签，该规则图形码复合标签由可以通过计算得到其中心点和方向的规则图形码标签和RFID标签复合构成；所述的规则图形码可以由1个或多个位置探测图形组成；所述的规则图形码和位置探测图形被定义为：通过计算，可以用来判断中心点和角度的所有图形码和图形。

本发明的规则图形码复合标签结构示意图，如图2、图3所示，规则图形码标签不限于方形，也可以是圆形、三角形或长方形或其它通过计算可以得到中心点和方向的规则图形码标签；位置探测图形也不限于方形：也可以是圆形、三角形或长方形或其它通过计算可以得到中心点和方向的规则图形；如图2中的长方方形码（标签2-1）以及方形码中的长方形位置探测图形，圆形码（标签2-2）以及圆形码中的三角形位置探测图形、三角形码（标签2-1）以及三角形码中的圆形位置探测图形，正方形码以及正方形码中的正方形位置探测图形。本实施例优选方形码。所述的规则图形码可以由1个或多个位置探测图形组成：如图3中的标签3-1是由1个非等边直角三角形探测图形组成的标签；标签3-2是由1个等腰直角三角形探测图形组成的规则图形标签；标签3-3是由1个由非等腰直角梯形探测图形组成的规则图形标签；图2中的标签2-1为由3个长方形探测图形组成的规则图形标签，图2中的标签2-2为由3个三角形形探测图形组成的规则图形标签，图2中的标签2-3为由3个圆形探测图形组成的规则图形标签；图2中的标签2-4为由3个正方形探测图形组成的规则图形标签。如图4所示，规则图形码复合标签摆放方式可以是上下重叠式的摆放，也可以是并列式的摆放，本实施例优选为上下重叠式的摆放：即规则图形复合标签的尺寸和中心点相同，上层为规则图形码标签，下层为RFID标签。

如图5所示：由规则图形码定位控制器内置的定位软件来实现的，该定位软件的处理流程如图5所示，规则图形码定位控制器同时向规则图形码读码器、射频标签读码器发送采集命令，规则图形码读码器、射频标签读码器分别采集规则图形信息和RFID位置信息信息并返回为规则图形码定位控制器，然后由规则图形码定位控制器进行定位处理，具体包括以下步骤：

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

如图6所示，本定位方法采用三个坐标系来实现定位功能，其原理为：实时获取小车坐标系和世界坐标系的位姿关系，需要经过两个步骤，其一，先计算规则图形码复合标签和移动机器人小车的相对位姿关系(x₁、y₁，θ₁)，因此需要建立小车坐标系和图像坐标系；其二，计算规则图形码复合标签和世界坐标系的关系(x₀，y₀，θ₀)，因此需要建立世界坐标系，最后将两个坐标系下的位置偏差和角度偏差进行叠加即可实时获得移动移动机器人小车和世界坐标系的位姿关系，从而实现移动机器人的准确定位。三个坐标系变换公式如下：

设：

规则图形码复合标签在世界坐标系下的坐标和偏转角度：(x₀，y₀，θ₀)；

规则图形码复合标签在小车坐标系下的坐标和偏转角度：(x₁，y₁，θ₁)；

小车在世界坐标系下的坐标和偏转角度：(x，y，θ)；

θ=θ₀-θ₁；

x=x₀-x₁*cosθ+y₁*sinθ；

y=y₀-y₁*sinθ-y₁*cosθ；

步骤2：计算在小车坐标系下移动机器人小车相对于规则图形码复合标签的偏离角度和偏离位置(x₁，y₁，θ₁)；

如图7所示，∠uoq为移动机器人小车和规则图形码复合标签的角度偏差，x₁为小车坐标系下沿X轴方向的位置偏差，y₁为小车坐标系下沿Y轴方向的位置偏差，具体步骤如下：

1、标定图像坐标系（摄像头）和小车坐标系的关系：

由于在本定位系统中，存在着二个坐标系：小车坐标系和图像坐标系，因此，在进行坐标提取前必须对摄像头进行标定以建立摄像头和小车对应关系。如图6所示，其中图像坐标系是以图像左上角为原点，以像素为单位的直角坐标系，u、v轴分别表示该像素在数字图像中的行数与列数。为了在视觉系统中建立统一的坐标，就需要建立图像坐标系与小车坐标系两者之间坐标转换关系，最终将图像坐标统一在小车坐标系下。标定方法可以采用传统的摄像头标定方法进行，如棋盘格标定方法，通过标定可以得到旋转矩阵R值，通过R值可将图像上某一点P_图像(u,v)转换为小车坐标系上某一点P_小车(x,y)，空间某一点在图像坐标系和小车坐标系的转换关系式如下：

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

上式中：

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

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

R为旋转矩阵值；

2、将规则图形码复合标签在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标；

如图5所示，微处理器同时发送数据采集的命令给规则图形码读码器和射频标签读码器，规则图形码读码器接收到命令后开始采集图像信息，然后进行解码，获取规则图形码的三个顶点的位置信息，并将三个顶点的位置信息返回给微处理器，微处理器的坐标变换模块根据返回的三个顶点信息，计算出第四个顶点，并将规则图形码在图像坐标系下四个顶点坐标转换为小车坐标系下的坐标：

P_{规则图形码复合标签}(x1,y1)=R×P_{规则图形码复合标签}(u1,v1)；

P_{规则图形码复合标签}(x2,y2)=R×P_{规则图形码复合标签}(u2,v2)；

P_{规则图形码复合标签}(x3,y3)=R×P_{规则图形码复合标签}(u3,v3)；

P_{规则图形码复合标签}(x4,y4)=R×P_{规则图形码复合标签}(u4,v4)；

上式中，P_{规则图形码复合标签}(x1,y1)、P_{规则图形码复合标签}(x2,y2)、P_{规则图形码复合标签}(x3,y3)、P_{规则图形码复合标签}(x4,y4)为规则图形码复合标签在小车坐标系中的四个顶点坐标，P_{规则图形码复合标签}(u1,v1)、P_{规则图形码复合标签}(u2,v2)、P_{规则图形码复合标签}(u3,v3)、P_{规则图形码复合标签}(u4,v4)为规则图形码复合标签在图像坐标系统中的四个顶点坐标；

3、计算小车坐标系下移动机器人小车相对于规则图形码复合标签的姿态变化：

θ₁=atan2(Point2.x+Point3.x-Point1.x-Point4.x,Point2.y+Point3.y-Point1.y-Point4.y)；

上式中，Point1.x、Point2.x、Point3.x、Point4.x分别为规则图形码复合标签在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；Point1.y、Point2.y、Point3.y、Point4.y为规则图形码复合标签在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的Y轴的坐标；

4、计算小车坐标系下规则图形码复合标签中心点的坐标（X_{规则图形码复合标签中心}，Y_{规则图形码复合标签中心}）。

计算方法可以直接通过其四个顶点的小车坐标系坐标计算后得到：

X_{规则图形码复合标签中心}=（Point1.x+Point2.x+Point3.x+Point4.x）/4;

Y_{规则图形码复合标签中心}=（Point1.y+Point2.y+Point3.y+Point4.y）/4;

5、计算整张图像中心点在图像坐标系中的坐标：

由于整张图像尺寸是固定的，其像素中心点直接用整张图像的长、宽尺寸再各减半得到：P_图像中心（u，v）；

6、将整张图像的中心点坐标转换为小车坐标系坐标：

P_图像中心（x，y）=R×P_图像中心（u，v），

X_图像中心为图像中心点在小车坐标系下的X轴坐标，Y_图像中心为图像中心点在小车坐标系下的Y轴坐标；

7、计算小车坐标系下移动机器人小车相对于规则图形码复合标签的位置变化：

小车坐标系下移动机器人小车相对于规则图形码复合标签在小车X轴、Y轴方向的偏差值必须在统一坐标系的条件下计算。通过计算将得到小车坐标系下的规则图形码复合标签图像中心点（X_{规则图形码复合标签中心}，Y_{规则图形码复合标签中心}）和小车坐标系下的整张图像中心点（X_图像中心，Y_图像中心）都转换为小车坐标系下的中心点后，即可计算移动机器人小车在X方向的偏差Delta_X和在Y方向的偏差Delta_y。

移动机器人小车在X方向的偏差：x₁=X_{规则图形码复合标签中心}-X_图像中心；

移动机器人小车在Y方向的偏差：y₁=Y_{规则图形码复合标签中心}-Y_图像中心。

步骤3：计算规则图形码复合标签相对于世界坐标系的偏离位置和角度(x₀,y₀，θ₀)；

由于射频标签内已经预先写入了该站点的绝对值坐标，或者写入序列号，因此射频标签的绝对位置信息即可作为规则图形码的绝对位置信息；当射频标签读码器接收到规则图形码定位控制器的命令后读取RFID数据，如果读取射频标签的数据是绝对值坐标，则直接返回数据给微处理器，如果读取射频标签的数据是序列号，则将序列号返回给微处理器，微处理器将标签序列号和所对应的绝对坐标进行一一对应，从而获得绝对值坐标；

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

1、获得规则图形码复合标签中心点相对于世界坐标系的位置偏差(x₀,y₀)。

通过射频标签读码器对RFID射频标签进行解码，获得（x₀,y₀），其中，x₀为世界坐标系下X轴方向的偏差值，y₀为世界坐标系下Y轴方向的偏差值；

2、获得规则图形码复合标签中心点相对于世界坐标系的角度偏差θ₀：

规则图形码相对于世界坐标系的角度偏差θ₀为：被敷设于地面上的规则图形码一组对边和世界坐标系X轴的夹角。所谓世界坐标系是随意而定的，一般情况下以当前房间为坐标系，房间的某个角为世界坐标系的中心点。通常情况下施工时尽量将规则图形码复合标签的敷设角度和世界坐标系保持一致。本实施例中假设一个特例：规则图形码复合标签和世界坐标系的角度偏差为45度。

步骤4：计算移动机器人小车相对于世界坐标系的偏离角度和偏离位置(x,y,θ)，从而获得移动机器人的精确位置。具体包括以下处理过程：

1、移动机器人小车相对于世界坐标系的偏离角度θ：

θ=θ₀-θ₁；

θ₁=atan2(Point2.x+Point3.x-Point1.x-Point4.x,Point2.y+Point3.y-Point1.y-Point4.y)；

上式中，Point1.x、Point2.x、Point3.x、Point4.x分别为规则图形码复合标签在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的X轴的坐标；Point1.y、Point2.y、Point3.y、Point4.y为规则图形码复合标签在图像坐标系下的四个顶点坐标转换为小车坐标系下的四个顶点的Y轴的坐标；

θ₀为规则图形码复合标签相对于世界坐标系的角度偏差：规则图形码复合标签一组对边和世界坐标系X轴的夹角即为θ₀。所谓世界坐标系是随意而定的，一般情况下以当前房间为坐标系，房间的某个角为世界坐标系的中心点。通常情况下施工时尽量将规则图形码复合标签的敷设角度和世界坐标系保持一致。

如图8中的θ₀=45°夹角，这是本实施例中假设一个特例。

2、计算移动机器人小车在世界坐标系下相对于规则图形码复合标签的位置偏差值（af,cf）：

由于移动机器人小车的姿态是动态变化的，不能保持其X方向或Y方向始终和世界坐标系一致，因此，需要将小车坐标系下移动机器人小车相对于规则图形码复合标签X、Y方向的位置偏差转换为世界坐标系下小车相对于规则图形码复合标签的X、Y方向的位置偏差。如图8所示，计算世界坐标系下小车相对于规则图形码复合标签的位置偏差。步骤如下：

⑴计算世界坐标系下小车相对于规则图形码复合标签的X轴方向的位置偏差af。

方法一：根据公式推导af和cf：

θ=θ₀-θ₁；

x=x₀-（x₁*cos60°-y₁*sinθ）；

y=y₀-y₁*sin600°y₁*cos60°；

由于：

af=x₁*cos60°-y₁*sin60°;

cf=x₁*sin60°+y₁*cos60°;

所以：

$\mathrm{af}=\left|x_1\right|*1/2-\left|y_1\right|*\sqrt{3}/2;$

$\mathrm{cf}=\left|x_1\right|*\sqrt{3}/2+\left|y_1\right|*1/2;$

方法二：用图形方法验证公式：

如图8：af=ae+ef;

因为：

ae=|x₁|*1/2;

$\mathrm{ef}=1/2\left(\right|x_1|-|y_1|*\sqrt{3}/3)=1/2\left(\right|x_1|-|y_1|*/\sqrt{3});$

所以：

$\mathrm{af}=\left|x_1\right|*1/2-\left|y_1\right|*\sqrt{3}/2;$

⑵计算世界坐标系下小车相对于规则图形码复合标签的Y轴方向的位置偏差cf。

因为：

$\mathrm{cf}=\left|\mathrm{ce}\right|*\sqrt{3}/2,$

$\left|\mathrm{ec}\right|=\left|x_1\right|-\left|y_1\right|*\sqrt{3}*\sqrt{3}/3*\sqrt{3};$

所以：

$\mathrm{cf}=\left|x_1\right|*\sqrt{3}/2+\left|y_1\right|*1/2;$

3、计算移动机器人在世界坐标系下的位置偏差（x,y)

用射频标签读码器对RFID进行解码，获得规则图形码复合标签的绝对值坐标：x0，y0。由于RFID的中心点和规则图形码复合标签重合，因此，RFID的坐标值也就是规则图形码复合标签的绝对值坐标。

小车在X轴方向的绝对坐标值：x=x₀-af;

小车在Y轴方向的绝对坐标值：y=y₀-cf;

所以：

$x=x_0-\mathrm{af}=x_0-\left(\right|x_1|*1/2-|y_1|*\sqrt{3}/2);$

$y=y_0-\mathrm{cf}=y_0-\left(\right|x_1|*\sqrt{3}/2+|y_1|*1/2);$

以上x、y为小车在世界坐标系下的坐标；x₀、y₀为规则图形码复合标签在世界坐标系下的坐标；af、cf为小车在世界坐标系下相对于规则图形码复合标签的位置偏差。

本步骤得到的移动机器人在世界坐标系下的坐标值，也就是移动机器人的实际地理位置。

下面对本发明的规则图形码原理进行说明：

1、规则图形码能够满足唯一确定方向的条件：图2中标签2-1、标签2-2、标签2-3、标签2-4具有当前姿态的唯一性，不论怎样调换标签的方向，标签中的三个探测图形的相互位置都不会重复出现。同理，图3中的标签3-1、标签3-2、标签3-3也具有当前姿态的唯一性，也就是不论怎样调换图形的方向，当前图形的姿态都不会重复出现。

2、规则图形码能够满足计算中心点的条件：如图2和图3的规则图形码标签都具有3个以上的顶点，读码器能够检测到3个顶点的坐标位置，再根据用3个顶点计算第4个顶点的公式计算出第4个顶点的坐标位置，再根据用长方形或正方形4个顶点计算中心点的公式计算出中心点坐标。

3、如图9所示，用a、b、c三个顶点坐标位置计算d点坐标的示意图：

由于:三角型anb等于三角形cdm;所以：

dx=cx+(bx-ax);dy=cy+(by-ay);

4、如图10所示，计算长方形4个点计算中心点示意图：

设长方形中心X坐标为X_中，Y轴坐标为Y_中，

由于长方形两条对角线的交点到四个顶点的距离相等：

X1-X_中=X_中-X3；X4-X_中=X_中-X2；

X1+X3=2X_中；X4+X2=2X_中；

因此，X1+X3+X4+X2=4X_中；

X_中=（X1+X3+X4+X2）/4；

同理:Y_中=（Y1+Y3+Y4+Y2）/4。

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

Claims (10)

1.一种基于规则图形码复合标签的移动机器人定位系统，其特征在于：包括安装在移动机器人小车上的规则图形码定位控制器、规则图形码读码器、射频标签读码器以及分布于地面上的规则图形码复合标签,该规则图形码复合标签由通过计算得到其中心点和方向的规则图形码标签和RFID标签复合构成，规则图形码定位控制器与规则图形码读码器、射频标签读码器相连接，用于控制规则图形码读码器和射频标签读码器进行规则图形码图像读取、RFID位置信息采集并实现移动机器人的精确定位功能。

2.根据权利要求1所述的基于规则图形码复合标签的移动机器人定位系统，其特征在于：所述的规则图形码标签和RFID标签复合方式为：上下重叠式摆放或并列式摆放。

3.根据权利要求2所述的基于规则图形码复合标签的移动机器人定位系统，其特征在于：所述的规则图形码标签由一个或多个位置探测图形构成；所述的规则图形码标签为正方形、圆形、三角形或长方形，所述的位置探测图形为正方形、圆形、三角形或长方形。

4.根据权利要求1或2或3所述的基于规则图形码复合标签的移动机器人定位系统，其特征在于：所述的规则图形码标签用于计算规则图形的方向和中心点，所述的RFID射频标签用于设置规则图形码标签的绝对值坐标。

5.根据权利要求1所述的基于规则图形码复合标签的移动机器人定位系统，其特征在于：所述的规则图形码定位控制器由微处理器、编码器和通信接口连接构成，该微处理器与编码器相连接用于检测移动机器人的运行速度，该微处理器通过通信接口与规则图形码读码器、射频标签读码器相连接。

6.根据权利要求1所述的基于规则图形码复合标签的移动机器人定位系统，其特征在于：所述的通信接口为网络接口、USB接口或者1394接口；所述的规则图形码读码器为具有网络接口、USB接口或者1394接口的摄像头；所述的射频标签读码器为具有网络接口、USB接口或者1394接口的RFID射频标签读码器。

7.一种实现权利要求1至6任一项所述定位系统的定位方法，其特征在于：包括以下步骤：

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

⑵计算在小车坐标系下移动机器人小车相对于规则图形码复合标签的偏离角度和偏离位置；

⑶计算规则图形码复合标签相对于世界坐标系的偏离位置和偏离角度；

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

8.根据权利要求7所述的基于规则图形码复合标签的移动机器人的定位方法，其特征在于：所述步骤⑵包括以下处理过程：

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

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

上式中：

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

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

R为旋转矩阵值；

②将图像坐标系下的1个或多个顶点转换为小车坐标系，转换后的坐标为：

P_{规则图形码复合标签}(x1,y1)=R×P_{规则图形码复合标签}(u1,v1)；

P_{规则图形码复合标签}(x2,y2)=R×P_{规则图形码复合标签}(u2,v2)；

P_{规则图形码复合标签}(x3,y3)=R×P_{规则图形码复合标签}(u3,v3)；

P_{规则图形码复合标签}(x4,y4)=R×P_{规则图形码复合标签}(u4,v4)；

上式中，P_{规则图形码复合标签}(x1,y1)、P_{规则图形码复合标签}(x2,y2)、P_{规则图形码复合标签}(x3,y3)、P_{规则图形码复合标签}(x4,y4)为规则图形码复合标签在小车坐标系中的四个顶点坐标，P_{规则图形码复合标签}(u1,v1)、P_{规则图形码复合标签}(u2,v2)、P_{规则图形码复合标签}(u3,v3)、P_{规则图形码复合标签}(u4,v4)为规则图形码复合标签在图像坐标系统中的四个顶点坐标；

③计算小车坐标系下移动机器人小车相对于规则图形码复合标签的姿态变化θ₁

θ₁=atan2(Point2.x+Point3.x-Point1.x-Point4.x,Point2.y+Point3.y-Point1.y-Point4.y)；

上式中，Point1.x、Point2.x、Point3.x、Point4.x为规则图形码复合标签在小车坐标系下的四个顶点的X轴坐标；Point1.y、Point2.y、Point3.y、Point4.y为规则图形码复合标签在小车坐标系下的四个顶点的Y轴坐标；

④计算小车坐标系下规则图形码复合标签中心点的坐标：

X_{规则图形码复合标签中心}，Y_{规则图形码复合标签中心}

其中，X_{规则图形码复合标签中心}为小车坐标系下规则图形码复合标签中心点的X轴坐标，Y_{规则图形码复合标签中心}为小车坐标系下规则图形码复合标签中心点的Y轴坐标；

⑤计算整张图像中心点在图像坐标系中的坐标P_图像中心（u，v）：

计算方法为：直接用整张图像的长、宽尺寸再各减半得到；

⑥将整张图像的中心点坐标转换为小车坐标系坐标：

P_图像中心（x，y）=R×P_图像中心（u，v），

上式中，X_图像中心为图像中心点在小车坐标系下的X轴坐标，Y_图像中心为图像中心点在小车坐标系下的Y轴坐标；

⑦计算小车坐标系下移动机器人小车相对于规则图形码复合标签的位置变化：

移动机器人小车在X方向的偏差x₁=X_{规则图形码复合标签中心}-X_图像中心；

移动机器人小车在Y方向的偏差y₁=Y_{规则图形码复合标签中心}-Y_图像中心。

9.根据权利要求7所述的基于规则图形码复合标签的移动机器人的定位方法，其特征在于：所述步骤⑶包括以下处理过程：

①射频标签读码器对RFID射频标签进行解码，获得规则图形码复合标签中心点相对于世界坐标系的位置偏差（x₀，y₀）；其中，x₀为世界坐标系下X轴方向的偏差值，y₀为世界坐标系下Y轴方向的偏差值；

②获得规则图形码复合标签中心点相对于世界坐标系的角度偏差。

10.根据权利要求7所述的基于规则图形码复合标签的移动机器人的定位方法，其特征在于：所述步骤⑷包括以下处理过程：

①计算移动机器人小车相对于世界坐标系的偏离角度：

θ=θ₀-θ₁

上式中，θ₀为世界坐标系下规则图形码复合标签相对于世界坐标系的角度偏差，θ₁为小车坐标系下移动机器人小车相对于规则图形码复合标签的角度偏差；

②计算移动机器人在世界坐标系下相对于规则图形码复合标签的偏差值：

af为移动机器人小车在世界坐标系下相对于规则图形码复合标签的X轴方向偏差；

cf为移动机器人小车在世界坐标系下相对于规则图形码复合标签的Y轴方向偏差；

③计算移动机器人小车在世界坐标系下的坐标值：

移动机器人小车X轴方向绝对坐标值x=x₀-af；

移动机器人小车Y轴方向绝对坐标值y=y₀-cf；

上式中，当af小于x₀时为负数，反之为正数；当cf小于y₀时为负数，反之为正数；x为移动机器人小车X方向绝对坐标值；y为移动机器人小车Y方向绝对坐标值；x₀为规则图形码复合标签的X轴绝对坐标值，y₀为规则图形码复合标签的Y轴绝对坐标值。

Images