二维码不神秘, 用手机1分钟就能制作完成, 扫描结果你说了算!

本发明涉及一种室内自主导航方法，尤其涉及一种基于单目视觉和二维码路标的室内自主导航方法，属于室内导航技术领域。

目前，很多自主导航技术或装置都基于GPS(全球卫星定位系统)技术，通过接收GPS信号进行室外导航。但是，在室内环境中，GPS信号强度衰减非常严重，甚至GPS信号会被完全阻断，此时将造成导航结果变差，甚至完全不可用。因此在室内环境中，必须采用其它的适用于室内环境的导航技术。

室内导航技术大致来说可分为两类方法，一类是相对位置测量方法，一类是绝对位置测量方法。

相对位置测量方法一般指的是测程法或基于惯性模块的估计方法，这类方法的优点是成本较低，计算简单，但缺点是往往只能在很短的移动距离或很短的时间内给出可用的位置与方向定位精度，当需要长时间观测且移动距离比较长时，往往无法解决因长时间统计误差所造成的估计距离与方向发散问题。

绝对位置测量方法中，有一类方法在巡检机器人中普遍采用。该方式需要在机器人活动区域内预先埋设电缆线或磁力线，机器人沿着预先设置的路线行进。通过在路线两侧布置一些颜色块或条形码等路标来确定机器人的位置。这种方式的不足之处在于，一旦机器人执行任务的内容发生改变，就需要对路径进行重新布置，灵活性差。且铺设电缆线与磁力线的成本比较高，对室内环境会造成永久性的破坏与改变，不能很好地满足室内自主导航的需求。

为了解决上述问题，现有技术出现了一些基于视觉技术和利用二维码作为路标的方法。但这类方法，往往要求摄像头光轴方向与二维码铺设平面成垂直关系，有的方法还要求摄像头与二维码铺设平面距离非常接近(<10cm)，如果不满足上述2个条件，将无法有效估计出机器人所在位置与二维码位置之间的关系。

要求摄像头光轴与二维码平面成垂直关系，会导致摄像头捕捉图像的视野变小，制约了机器人的最高运行速度，不利于机器人以较高速度行驶时的自主导航。要求摄像头与二维码铺设平面距离非常接近，除了也存在上述的视野小的问题外，还存在以下问题：一般二维码都是铺设在地面上的，因此，当摄像头和地面非常接近时，就对地面的清洁程度提出了较高的要求，一旦有尘土、油渍等溅起粘在镜头上将大大影响二维码路标的识别率和导航精度，不能很好地满足室内导航的需求。

针对现有技术的不足，本发明所要解决的技术问题在于提供一种基于单目视觉和二维码路标的室内自主导航方法。

为实现上述发明目的，本发明采用下述的技术方案：

一种基于单目视觉和二维码路标的室内自主导航方法，包括如下步骤：

S1，获取机器人的初始位置，根据机器人的初始位置和目标位置设置二维码路线；

S2，机器人的单目摄像头实时检测二维码，根据单目摄像头的内外参数，计算出机器人与所述二维码之间的相对位置关系，确定机器人当前导航信息；

S3，根据机器人的当前导航信息进行运动，判断到达的目的地址是否是目标位置，如果是，则导航结束；否则，按照二维码的导航方向调整角度，转向步骤S2。

其中较优地，在步骤S1中，获取机器人的初始位置包括如下步骤：

S11，启动机器人的单目摄像头，识别当前二维码；

S12，根据所述二维码包含的信息，计算出机器人与所述二维码之间的相对位置关系；

S13，根据二维码在自主导航区域的坐标位置以及机器人与所述二维码之间的相对位置关系，计算出机器人在自主导航区域内的绝对位置，即得到起始位置。

其中较优地，在步骤S12中，机器人与所述二维码之间的相对位置关系，用机器人中心坐标与所述二维码的中心点之间的夹角以及距离进行表示。

其中较优地，在步骤S2中，所述根据单目摄像头的内外参数，计算出机器人与所述二维码之间的相对位置关系，确定机器人当前导航信息，包括如下步骤：

S21，根据单目摄像头的内外参数，建立像平面坐标系中的点与世界坐标系中的点之间的变换关系；

S22，单目摄像头检测二维码，获取二维码中心点在像平面坐标系中的坐标以及二维码信息；

S23，根据二维码中心点在像平面坐标系中的坐标，以及像平面坐标系与世界坐标系之间的变换关系，确定二维码路标中心点在世界坐标系的坐标；

S24，计算机器人在当前行驶方向下与二维码路标中心点之间的夹角，以及机器人与二维码路标中心点之间的距离，确定机器人当前导航信息。

其中较优地，在步骤S2中，获取单目摄像头的内外参数包括如下步骤：

S211，制作单目摄像头标定板；

S212，分别设立像平面坐标系、相机坐标系和世界坐标系，并采用单目摄像头的内外参数确定像平面坐标系与相机坐标系之间的转换关系；

S213，通过旋转矩阵和平移矩阵得到从相机坐标系到世界坐标系的变换关系；其中，所述旋转矩阵和实时平移矩阵为单目摄像头的外参数矩阵；

S214，将像平面坐标系与相机坐标系之间的转换关系以及相机坐标系到世界坐标系的变换关系联立方程，得到像平面坐标系与世界坐标系之间的变换关系；

S215，采集标定板在不同位姿的图像，根据步骤S214的变换关系建立方程组，通过求解方程组得到单目摄像头的内外参数。

其中较优地，在步骤S2中，当机器人的单目摄像头检测的当前的二维码不能扫描时，扫描单目摄像头视野范围内的另一二维码。

其中较优地，所述的基于单目视觉和二维码路标的室内自主导航方法，还包括如下步骤：

当机器人运行轨迹不在设置二维码路线中时，计算机器人的当前位置，重新设置二维码路线。

其中较优地，所述当机器人运行轨迹不在设置二维码路线中时，计算机器人的当前位置，重新设置二维码路线，包括如下步骤：

使用单目摄像头采集二维码信息，根据二维码信息得到二维码在自主导航区域的坐标位置；

根据所述二维码包含的信息，计算出机器人与所述二维码之间的相对位置关系；

根据所述二维码在自主导航区域的坐标位置以及机器人与二维码路标的相对位置关系，计算出机器人在自主导航区域内的绝对位置，即得到当下位置信息；

根据当下位置信息和目标位置重新设置二维码路线。

其中较优地，所述二维码路标采用油墨印刷、油墨打印、紫外油墨印刷或者荧光油墨印刷中的一种；

当在导航过程中需要二维码路标肉眼不可见时，采用紫外油墨印刷或者荧光油墨印刷，并采用紫外或者荧光摄像头进行识读。

本发明所提供的基于单目视觉和二维码路标的室内自主导航方法，根据机器人的初始位置和目标位置设置二维码路线。然后，通过机器人的单目摄像头从二维码路标组成的图像中实时的检测与识别二维码，根据单目摄像头的内外参数，计算出机器人当前行驶方向和位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息；根据机器人的当前导航信息进行运动，直至到达目的地。该方法不仅可以解决双目摄像头计算复杂的问题，而且可以有效地满足室内自主导航的精确度要求。

图1为本发明所提供的基于单目视觉和二维码路标的室内自主导航方法的流程图；

图2为本发明所提供的一个实施例中，二维码路标的结构示意图；

图3为本发明所提供的一个实施例中，标定板的结构示意图；

图4为本发明所提供的一个实施例中，二维码路标导航信息所指示方向的结构示意图；

图5为本发明所提供的一个实施例中，二维码路标导航示意图。

下面结合附图和具体实施例对本发明的技术内容进行详细具体的说明。

如图1所示，本发明所提供的基于单目视觉和二维码路标的室内自主导航方法，包括如下步骤：首先，获取机器人的初始位置，根据机器人的初始位置和目标位置设置二维码路线。其次，机器人的单目摄像头从图像中实时的检测与识别二维码，根据单目摄像头的内外参数，计算出机器人当前行驶方向和位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息；然后，根据机器人的导航方向行进，判断到达的目的地址是否是目标位置，如果是，则导航结束；否则，按照二维码的导航方向调整角度，继续从图像中实时的检测与识别二维码，根据单目摄像头的内外参数，计算出机器人当前行驶方向与位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息，直至到达目标位置为止。下面对这一处理过程进行详细说明。

S1，获取机器人的初始位置，根据机器人的初始位置和目标位置设置二维码路线。

在室内环境预先铺设二维码(QR code)作为路标，并将此路标作为室内导航定位定向的路标。在本发明所提供的实施例中，二维码路标可以采用常见的油墨印刷、油墨打印的方式铺设在室内地面、墙面上，供摄像头扫描识读；也可以根据技术水平和铺设需要采用紫外油墨印刷或者荧光油墨印刷，供紫外或者荧光摄像头进行识读。当实际使用过程中，导航路线需要隐藏或者出于视觉效果，需要二维码标识肉眼不可见时，可以采用紫外油墨印刷或者荧光油墨印刷，这样肉眼看是无色的，但紫外或者荧光摄像头可以进行识读。二维码具有信息量大、易于识别、且即便有部分污损或遮挡仍能准确快速识别的优点，而且二维码生成简单，成本较低，支持用户的自行设计与布置，对室内环境不会造成永久性的破坏与改变。在本发明所提供的一个实施例中，可以采用比较通用的一种二维码：QR code，如图2所示。在实际应用中，也可以根据需要设置其他形式的二维码或者数字码等。

假设室内地面需要进行自主导航的区域是一个宽度为a米，长度为b米的矩形(a可以等于b)，对这块区域根据导航定位的需求进行网格划分，将这块区域划分为m行n列，形成m*n个网格交叉点，每个交叉点的坐标为(Xi,Yj)，1<＝i<＝m，1<＝j<＝n。生成m*n个不同的二维码路标，每个二维码路标包含如下信息：1)编号，由此编号可以唯一的确认该二维码在网格交叉点的坐标位置；2)导航信息，如直行、左转、右转等。

二维码铺设好之后，每次进行室内自主的导航前需要获取机器人的初始位置，根据机器人的初始位置和目标位置设置二维码路线。在本发明中，机器人的初始位置一般为零点位置，即为机器人的起点位置。每天的自主导航完成，机器人结束工作会回到起点位置，等待第二天的自主导航任务。当然，为了防止特殊情况下机器人的起始位置并不在起点位置，在进行路线规划之前，需要先获取机器人的起始位置，具体包括如下步骤：

S11，启动机器人的单目摄像头，识别当前二维码。

S12，根据该二维码包含的信息，计算出机器人与该二维码路标之间的相对位置关系。

根据该二维码包含的信息，计算出机器人与该二维码路标之间的相对位置关系。在本发明中，计算出机器人与该二维码路标之间的相对位置关系用机器人中心坐标与该二维码路标中心点之间的夹角以及距离来表示。其中，计算出机器人与该二维码路标之间的相对位置关系的具体过程在后续内容中进行详细描述。

S13，根据二维码在自主导航区域的坐标位置以及机器人与该二维码路标之间的相对位置关系计算出机器人在自主导航区域内的绝对位置，即得到起始位置。

二维码的坐标位置为二维码在自主导航区域内的坐标，可以根据二维码内的标号信息获取。根据机器人与该二维码路标之间的相对位置关系可以得到机器人坐标位置与二维码坐标位置的距离以及角度，将该相对位置关系应用到二维码所在的自主导航区域，根据二维码在自主导航区域的坐标位置，即可以得到机器人的当下坐标信息，即起始位置。

S2，机器人的单目摄像头实时的检测与识别二维码(二维码路标)，根据单目摄像头的内外参数，计算出机器人当前行驶方向与位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息。

本发明采用单目摄像头，基于计算机视觉与图像处理的方法，从二维码路标组成的图像中实时的检测与识别二维码，然后根据该二维码包含的信息以及单目摄像头的内外参数，计算出机器人当前行驶方向与位置与检测到的二维码路标位置之间的角度与距离关系，并根据二维码路标指示的方向给出机器人当前导航信息，采用本发明可以降低使用双目摄像头进行计算的复杂度，同时可以达到很高的导航精度(测量精度<2cm),从而实现机器人的自主室内导航。

当从单目摄像头采集到的图像中检测并识别到该二维码后，将可以根据该二维码包含的信息以及单目摄像头的内外参数，计算出机器人与该二维码路标之间的相对位置关系，进一步推算出机器人本身在自主导航区域内的绝对位置信息，便于在导航丢失时重新进行二维码路线的设置。同时还可以根据读取到的二维码路标导航信息，决定机器人下一步的运行方向。比如图2所示的二维码，就可以读取到以下信息，编号：001，导航信息：直行。

在本发明所提供的实施例中，单目摄像头根据使用需求可以安装在机器人的任意位置上，由该单目摄像头拍摄地面图像，并根据此图像进行二维码路标的检测、识别。本发明中，对单目摄像头在机器人上的安装位置不做严格要求，不要求摄像头光轴方向必须与二维码铺设平面成垂直关系，也不要求摄像头与二维码铺设平面之间的距离比较非常小，即对摄像头光轴与二维码铺设平面之间的夹角和距离不做严格要求。可以容许摄像头光轴与地面有一定的倾角，以获得较大的视野范围。本发明可支持摄像头光轴与二维码铺设平面夹角达到45度，且摄像头与二维码铺设平面距离达到2m。当摄像头与二维码路标之间的距离加大后，本发明中所提出的导航技术将可适用于更广泛的室内环境和应用场景，比如工厂、仓库等地面洁净程度不高的环境，而不用担心摄像头被污损的问题，大大降低维护成本。单目摄像头与二维码路标之间的夹角和距离加大之后，摄像头的视野范围扩大，可以同时看到近处与稍远处的多个二维码，利用摄像头视野范围内存在多个二维码提供的冗余信息，当二维码出现部分污损或遮挡时，可利用冗余的二维码提供定位信息，将大大提高二维码的识别率，提高导航准确度。

其中，根据单目摄像头的内外参数，计算出机器人与检测到的二维码之间的相对位置关系，确定机器人当前导航信息，具体包括如下步骤：

S21，根据单目摄像头的内外参数，建立摄像头检测到的二维图像像素点(u,v)与以机器人底盘中心为原点的世界坐标系中(Xw,Yw,Zw)之间的变换关系。从单目摄像头采集到的图像中检测与识别二维码路标，这部分属于图像处理的范畴，在本发明中采用常规技术方法实现，具体检测与识别步骤不再赘述。

其中，获取单目摄像头的内外参数包括如下步骤：

S211，制作单目摄像头标定板。

在本发明所提供的实施例中，标定板尺寸为324mm*252mm，标定板由7行9列共63个边长为36mm的正方形方格组成，如图3所示。

S212，分别设立像平面坐标系、相机坐标系和世界坐标系，并采用单目摄像头的内外参数确定像平面坐标系与相机坐标系之间的转换关系。

假设像平面坐标系为P，以图像左上角为坐标系原点；假设相机坐标系(单目摄像头坐标系)为U，以光轴中心点为坐标原点；假设世界坐标系为W，以机器人底盘中心点为坐标原点；像平面坐标系与相机坐标系之间的转换关系，如式1所示：

其中，(u,v)是像平面坐标系P中的坐标，(Xu,Yu,Zu)是相机坐标系U中的坐标，(1/s_x,1/s_y,c_x,c_y,f,θ)为相机的6个内参数，其组成的矩阵为内参数矩阵。其中，θ是相机坐标系Z轴与像平面夹角，一般情况下Z轴与像平面垂直，θ值为90。f为摄像头的焦距。摄像头光轴中心z轴方向上与像平面的交点称为投影中心，坐标为(c_x，c_y)，是像素单位，而每个像素在Xf和Yf的物理尺寸为Sx＝1/dx和Sy＝1/dy，单位是像素/毫米。这6个参数都是和摄像头的具体成像原理和设计有关，因此被称为摄像头的内参数矩阵。

S213，通过旋转矩阵R和平移矩阵T得到从相机坐标系到世界坐标系的变换关系。其中，R和T统称为摄像头的外参数矩阵。

对于从相机坐标系到世界坐标系的变换，是通过旋转矩阵R和平移矩阵T完成的。R和T统称为摄像头的外参数矩阵。

其中，(Xu,Yu,Zu)是相机坐标系U中的坐标；(Xw,Yw,Zw)是世界坐标系中的坐标。

S214，像平面坐标系与相机坐标系之间的转换关系以及相机坐标系到世界坐标系的变换关系，得到像平面坐标系与世界坐标系之间的变换关系。

通过联立式(1)和式(2)求得像平面坐标系与世界坐标系之间的变换关系，如式(3)：

S215，采集标定板在不同位姿的图像，根据步骤S214的变换关系建立方程组，通过求解方程组得到单目摄像头的内外参数。

安装好单目摄像头后，采集标定板在不同位姿的图像，根据步骤S214获取的像平面坐标系与世界坐标系之间的变换关系建立方程组，通过求解方程组可以求取出单目摄像头的内外参数，从而建立起摄像头采集的二维图像像素点(u,v)与以机器人底盘中心为原点的世界坐标系中(Xw,Yw,Zw)之间的一一对应关系。

S22，单目摄像头检测二维码，获取二维码路标中心点在像平面坐标系中的坐标(u,v)以及二维码信息。

如图5所示，以二维码路标指示方向为直行举例说明如何估计机器人与二维码路标之间的角度与距离关系。假定世界坐标系原点为O，世界坐标系原点是机器人底盘中心点，y轴正方向为机器人行进方向。单目摄像头检测二维码，获取二维码路标中心点在图像中的像素点(像平面坐标系中点的坐标)(u,v)以及二维码信息。其中，二维码信息包括二维码编号和二维码的导航方向。二维码的导航方向为二维码路标指示的行进方向，本发明中使用QR code，QR code的特点是在二维码的3个角上，有3个像“回”字的正方图案，由这3个方块的位置可以唯一的确定二维码的方向。同时二维码包含的导航方向，直行、左转、右转3个方向的二维码行进方向如图4所示。

S23，根据二维码中心点在像平面坐标系中的坐标(u,v)，以及像平面坐标系与世界坐标系之间的变换关系，确定二维码路标中心点在世界坐标系的坐标(Xw，Yw)。

在前述步骤S214中，已经得到像平面坐标系与世界坐标系之间的变换关系。根据采集的二维码路标中心点在像平面坐标系的坐标(u,v)可以得到其在世界坐标系的坐标(Xw，Yw)。

S24，计算机器人在当前行驶方向下与二维码路标中心点之间的夹角，以及机器人与二维码路标中心点之间的距离，确定机器人当前导航信息。

在本发明所提供的实施例中，机器人的中心点为世界坐标系的原点，根据二维码路标中心点在像平面坐标系中的坐标(u,v)，以及像平面坐标系与世界坐标系之间的变换关系，确定二维码路标中心点在世界坐标系的坐标(Xw，Yw)。得到两个点的坐标之后，通过计算可以得到机器人在当前行驶方向下与二维码路标之间的夹角，以及机器人与二维码路标中心点中间的距离。

其中，计算机器人在当前行驶方向下与二维码路标之间的夹角，采用如下公式：

计算机器人与二维码路标中心点中间的距离D，采用如下公式：

根据上述计算出的机器人在当前行驶方向下与二维码路标之间的夹角和机器人与二维码路标中心点中间的距离确定机器人当前导航信息。

S3，根据机器人的当前导航信息进行运动，判断到达的目的地址是否是目标位置，如果是则导航结束，否则，按照二维码的导航方向调整角度，转向步骤S2。

根据机器人的当前导航信息进行运动，判断到达的目的地址是否是目标位置，如果是则导航结束，否则，按照二维码的导航方向调整角度，如图5中箭头所示(该二维码的导航方向为直行)。当机器人到达二维码所在的目的地址时，按照二维码导航方向旋转角度。由此确定了机器人在导航区域的位置信息，以及机器人下一步的运动方向，完成了对机器人的当前的自主导航。之后，继续从图像中实时的检测与识别二维码，根据单目摄像头的内外参数，计算出机器人当前行驶方向和位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息，直至到达目标位置为止。

在本发明所提供的实施例中，由于单目摄像头与二维码铺设平面之间的夹角和距离没有受到很大限制，单目摄像头根据使用需求可以安装在机器人的任意位置上，不要求摄像头光轴方向必须与二维码铺设平面成垂直关系，也不要求摄像头与二维码铺设平面之间的距离比较非常小，可以容许摄像头光轴与地面有一定的倾角，以获得较大的视野范围。摄像头的视野范围扩大，可以同时看到近处与稍远处的多个二维码，利用摄像头视野范围内存在多个二维码提供的冗余信息，当二维码出现部分污损或遮挡，以致于单目摄像头无法采集当前二维码信息时，可利用冗余的二维码提供定位信息。这就导致机器人出现智能性行驶，可能并没有按照之前既定的路线行驶，当由于二维码出现部分污损或遮挡致使机器人没有按照既定路线行驶，当扫描的二维码不包含在二维码路线中，出现走丢情况时，可以重新获取机器人的当下位置信息，根据当下位置信息和目标位置重新设置二维码路线，具体包括如下步骤：

使用单目摄像头采集二维码信息，根据二维码信息得到二维码在自主导航区域的坐标位置。

根据该二维码包含的信息，计算出机器人与该二维码路标之间的相对位置关系。在本发明中，机器人与该二维码路标之间的相对位置关系用机器人中心坐标与该二维码路标中心点之间的夹角以及距离来表示。其中，计算出机器人与该二维码路标之间的相对位置关系采用上述步骤S21～S24即可得到，在此便不再赘述了。

根据二维码在自主导航区域的坐标位置以及机器人与该二维码路标之间的相对位置关系，计算出机器人在自主导航区域内的绝对位置，即得到当下位置信息；

根据当下位置信息和目标位置重新设置二维码路线。

综上所述，本发明所提供的基于单目视觉和二维码路标的室内自主导航方法，获取机器人的初始位置，根据机器人的初始位置和目标位置设置二维码路线。然后，机器人的单目摄像头从二维码路标组成的图像中实时的检测与识别二维码，根据单目摄像头的内外参数，计算出机器人当前行驶方向和位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息；根据机器人的当前导航信息进行运动，判断到达的目的地址是否是目标位置，如果是，则导航结束；否则，按照二维码的导航方向调整角度，继续从二维码路标组成的图像中实时的检测与识别二维码，根据单目摄像头的内外参数，计算出机器人当前行驶方向与位置与检测到的二维码路标位置之间的角度与距离关系，确定机器人当前导航信息，直至到达目标位置为止。该方法不仅可以提高室内导航的准确度，可利用性高，而且可适用于更广泛的室内环境和应用场景，比如工厂、仓库等地面洁净程度不高的环境，而不用担心摄像头被污损的问题，大大降低维护成本。

上面对本发明所提供的基于单目视觉和二维码路标的室内自主导航方法。进行了详细的说明。对本领域的一般技术人员而言，在不背离本发明实质精神的前提下对它所做的任何显而易见的改动，都将构成对本发明专利权的侵犯，将承担相应的法律责任。