CN109374003A - 一种基于ArUco码的移动机器人视觉定位与导航方法 - Google Patents

Abstract

本发明公开了一种基于ArUco码的移动机器人视觉定位与导航方法，属于计算机视觉定位、移动机器人导航领域，包括如下步骤：创建地图；创建路径；定位与导航。本发明方法不仅克服了传统视觉导航方式对辅助物理条件依赖性强、对环境要求高、不灵活等缺陷，而且将传统仅适用于地面移动机器人的视觉导航方法拓展到了三维空间，对空中无人机的定位与导航亦非常适用；具有精度高、灵活性强、应用范围广等诸多优势，可用于室内环境下移动机器人、无人机等智能移动设备的自主定位与导航，拓宽了应用场景。

Description

一种基于ArUco码的移动机器人视觉定位与导航方法

技术领域

本发明属于计算机视觉定位、移动机器人导航领域，具体涉及一种基于ArUco码的移动机器人视觉定位与导航方法。

背景技术

目前，移动机器人技术在计算机、传感器等相关技术的推动下，得到了飞速发展，并且在医疗、军事、工业和太空等领域得到了广泛地应用。移动机器人的首要任务是根据自身的传感器实现灵活的自主定位与导航，但是，各种传感器受限于不同的空间环境，要完成自主定位与导航任务，需要在不同的环境下应用不同的方案。

由于移动机器人的应用环境不同，有些环境下需要较高的定位和导航精度，比如工厂车间、室内等情况，其定位精度通常需要达到厘米甚至毫米级别。GPS(全球卫星定位系统)在信号良好的条件下，定位精度一般在米级，不符合应用要求。利用惯性导航元件可以测量相对位移，但缺点是惯导元件测量数据存在漂移，需要配合其他传感器以提高精度，算法复杂。利用激光雷达可以进行精准测距定位，完成导航任务，但是需要布置反光板以提供导航信息，而且激光雷达价格昂贵。机器人通过电磁、寻迹的方式可以满足精度要求，但是需要布置轨道或路标，当机器人的任务发生变化时，需要重新布置路径，灵活性差，成本高。

此外，出现了利用色带和条形码、QR码等基于二维码的视觉导航方法，这种方法通常用地面铺设的彩带做引导、用二维码中包含的位置信息做定位实现机器人导航，但地面上的彩带和二维码容易污损，且当任务路径发生变化时，需要重新粘贴色带和二维码，灵活性差、对环境要求高。另外，这种方法往往要求摄像头光轴方向与二维码铺设平面成垂直关系，且要求摄像头与二维码铺设平面距离非常接近，否则机器人将不能很好地识别，有较大的局限性。在专利CN106969766A中公开了一种基于单目视觉和二维码路标的室内自主导航方法，该方法克服了不能有效识别二维码的局限性，且当机器人执行任务的内容发生改变时不需要对路径进行重新布置，但依然不够灵活。其不足之处在于，需要根据机器人的行驶路径提前布置二维码，且二维码的布局需要严格按照一定的规律(一般网格状)，必须经过精确地测量以确定二维码之间的位姿关系；机器人行进路线受二维码布局和二维码导航信息的制约，行走方式单一，不能完成复杂的曲线行驶。而且，此方法依然使用在地面铺设二维码的方式，不能获得机器人的三维空间坐标，只能局限于规划平面路径，无法规划无人机等空间中运行的机器人的三维路径，应用场景有限。

发明内容

针对现有技术中存在的上述技术问题，本发明提出了一种基于ArUco(AugmentedReality library from the University of Cordoba，科尔多瓦大学的增强现实库)码的移动机器人视觉定位与导航方法，使得机器人视觉导航更加方便灵活，设计合理，克服了现有技术的不足，具有良好的效果。

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

一种基于ArUco码的移动机器人视觉定位与导航方法，包括如下步骤：

步骤1：创建地图；

利用相机对环境中随机分布的ArUco码进行视觉建图，获得ArUco码分布地图；

步骤2：创建路径；

在计算机中生成导航路径的坐标点，获得由一系列三维坐标点组成的导航路径文件；

步骤3：定位与导航；

根据步骤1和步骤2创建的ArUco码分布地图和导航路径文件，利用相机实现移动机器人在三维空间中的视觉定位与导航。

优选地，在步骤1中，具体包括如下步骤：

步骤1.1：将打印好的ArUco码随机地粘贴在包括墙面、门面、地面、天花板或立柱在内的光照条件良好的平整位置；

步骤1.2：用相机对室内随机分布的ArUco码进行视觉建图，确定一个参考世界坐标系，并获得所有ArUco码的ID信息及其对应四个角点在世界坐标系下的三维坐标。

优选地，在步骤1.2中，用相机对室内随机分布的ArUco码进行视觉建图，具体包括如下步骤：

步骤1.2.1：用棋盘格预先标定出相机的内参数；

步骤1.2.2：打开ArUco建图工具，传入相机内参，并指定环境中的一个ArUco码作为参考，程序开始运行；

步骤1.2.3：手持或机载相机缓慢移动，使相机能遍历所有ArUco码；

步骤1.2.4：建图程序运行结束，得到ArUco码的分布地图，分布地图中包含环境中所有ArUco码的ID信息及其对应四个角点的三维坐标信息，另外包括一个分布点云。

优选地，在步骤2中，具体包括如下步骤：

步骤2.1：根据周围环境和移动机器人的任务规划机器人的行走路线；

步骤2.2：将移动机器人的行走路线在世界坐标系下用数学方程标出；

步骤2.3：根据导航路径的起止点坐标，在计算机中将用数学方程表示的行走路线离散化为一系列三维坐标点，所有点的坐标按顺序保存在一个文本文档里，得到导航路径文件。

优选地，在步骤3中，具体包括如下步骤：

步骤3.1：加载ArUco码分布地图和导航路径文件，并将ArUco码的分布点云以及导航路径显示到可视化界面中；

步骤3.2：根据图像中每个ArUco码角点的像素坐标和其对应的三维世界坐标，利用PnP迭代算法估计相机的位姿，从而实时地获取机器人在世界坐标系下的三维空间坐标和姿态；

步骤3.3：判断机器人是否到达目标位置；

若：判断结果是机器人到达目标位置，则结束；

或判断结果是机器人没有到达目标位置，则计算出与机器人当前位置距离最近的路径点，并确定下一个参考路径点的位置；

步骤3.4：计算机器人当前位置与下一个参考路径点之间的方向向量V′，并计算机器人当前姿态向量V与方向向量V′之间的夹角α；

步骤3.5：机器人跟据偏航角α调整自身的姿态，以速度v沿着导航路径前进，然后执行步骤3.2。

优选地，在步骤3.2中，具体包括如下子步骤：

步骤3.2.1：相机采集一帧图像，对图像进行包括灰度转换、二值化和轮廓提取在内的预处理操作，然后对每个轮廓进行筛选、透视变换、内部编码提取；

步骤3.2.2：判断检测图像中是否出现至少一个ArUco码；

若：判断结果是检测图像中出现至少一个ArUco码，则根据识别出的所有ArUco码的ID信息，确定每个ArUco码的角点像素坐标和与之相对应的三维空间坐标，利用PnP迭代算法估计相机在世界坐标系中的位置和姿态；

或判断结果是检测图像中没有出现ArUco码，则执行步骤3.2.1；

步骤3.2.3：根据相机与机器人本体的相对位姿关系和相机在世界坐标系中的位姿进行坐标变换，计算出机器人在世界坐标系下的位置P和姿态向量V。

本发明所带来的有益技术效果：

本发明通过相机观察环境中随机分布的ArUco码实时获取机器人在世界坐标系下的三维坐标和姿态，利用精确的位姿信息进行视觉导航，方法中用到的ArUco码布置方便灵活，导航路径可以根据机器人的任务路线随时更改，且不需要对环境做任何改变，大大提高了移动机器人导航的灵活性，同时降低了对环境的要求；另外，由于本发明提供的定位方法可以实时获取机器人的三维坐标和姿态，该方法不仅可以用于地面移动机器人的导航，对于空中飞行的无人机同样适用。

本发明方法不仅克服了传统视觉导航方式对辅助物理条件依赖性强、对环境要求高、不灵活等缺陷，而且将传统仅适用于地面移动机器人的视觉导航方法拓展到了三维空间，对空中无人机的定位与导航亦非常适用；具有精度高、灵活性强、应用范围广等诸多优势，可用于室内环境下移动机器人、无人机等智能移动设备的自主定位与导航，拓宽了应用场景。

附图说明

图1是本发明利用ArUco码实现移动机器人视觉定位与导航的功能示意图；

图2是本发明方法中使用的ArUco码示意图；其中，

图2(a)为ARUCO_MIP_36h12字典中ID为0的二维码(marker 0)图；图2(b)为上述marker 0的Enclosed Marker形式图；

图3是本发明所提供的一种基于ArUco码的移动机器人视觉定位与导航方法结构框图；

图4是本发明所提供的一种基于ArUco码的移动机器人视觉定位与导航方法中定位与导航部分的方法流程图。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1所示为本发明利用ArUco码实现机器人视觉定位与导航的功能示意图。本实施例通过事先创建环境中ArUco码的分布地图和机器人的导航路径，利用单目相机观察室内随机分布的ArUco码，实时获取机器人在世界坐标系下的三维坐标和姿态，并根据机器人当前位置与导航路径坐标点之间的方向向量，确定机器人在行进过程中的偏航角α。机器人可以根据偏航角α实时地调整自身的运动方向，从而沿着预设的导航路径到达目标位置。

如图3所示，本发明所提供的基于ArUco码的机器人视觉定位与导航方法主要分为三大部分，分别为创建地图、创建路径、定位与导航。其中，创建地图部分通过视觉建图方式获得ArUco码的分布地图；创建路径部分通过在计算机中生成导航路径的坐标点，获得导航路径文件；定位与导航部分通过相机利用ArUco码分布地图和导航路径文件实现机器人视觉定位与导航。下面依据这三大部分对本发明的整个实现过程进行详细说明：

步骤1：创建地图：通过视觉建图方式获得ArUco码的分布地图，具体实现步骤如下：

步骤1.1：将打印好的ArUco码随机地粘贴在墙(门)面、地面、天花板或立柱上等光照条件良好的平整位置。

ArUco码是一种应用于AR/VR领域的二维基准标识码，可以实现精准地相机位姿估计。本发明选择使用ArUco二维基准标识码，其优势在于：

(a)ArUco二维基准标识码与日常所见的QR码、Data Matrix等二维码有很大不同，每个ArUco码仅包含自身唯一确定的ID信息，该ID由阿拉伯数字表示。

(b)ArUco码编码简单，鲁棒性强，可在相机距离较远、相机移动速度较快或图像相对较模糊的情况下快速准确地识别出图像中的ArUco码。

(c)ArUco码具有旋转不变性，根据内部编码可唯一确定它四个角点的顺序并得到ArUco码自身坐标系的方向。如图2(a)所示，坐标系原点在ArUco码的中心，Z轴垂直ArUco码平面指向外，X轴与Y轴的方向由内部编码唯一确定。

如图2(b)所示，本实施例中使用ARUCO_MIP_36h12字典中的Enclosed Markers，使用这样的ArUco码可以获得更加精准的角点像素坐标，有助于进一步提高定位的精度与鲁棒性。

需要指出的是，ArUco码打印的尺寸需根据使用相机的分辨率和相机与ArUco码之间的距离而定，通常要求ArUco码在图像中至少占据5％的面积。在本实施例中，分辨率为640×480的相机在2m以内观察边长为0.135m的ArUco码可满足要求。如果实际环境中相机无法与ArUco码保持较近的距离，须适当增大ArUco码的尺寸。

在室内粘贴ArUco码时，ArUco码粘贴的位置和姿态不需要提前根据机器人的导航任务进行严格布置，即ArUco码的粘贴位置与机器人的行驶路径没有必然联系。在保证粘贴表面平整的情况下，ArUco码可以任意姿态粘贴在墙面、天花板、地面、墙角、梁柱等等一切相机能够看得到的地方。ArUco码之间的距离随机，通常每个ArUco码之间距离在1m～2m为宜。在本实施例中，整个室内空间大小约为12×7×4mm³，在离地面约1.5高的墙面四周随机粘贴了30张ArUco码，ArUco码之间的距离均保持在1～1.5m之间，姿态任意。

需要强调的是，同一字典中同一个ID的ArUco码不能在环境中重复出现，且环境中粘贴的所有ArUco码尺寸必须一致。

步骤1.2：用相机对室内随机分布的ArUco码进行视觉建图，确定一个参考世界坐标系，并获得所有ArUco码的ID信息及其对应四个角点在世界坐标系下的三维坐标。视觉建图过程包括如下步骤：

步骤1.2.1：用棋盘格预先标定出相机的内参数。

步骤1.2.2：打开ArUco建图工具，传入相机内参并指定环境中的一个ArUco码作为参考，程序开始运行。

参考ArUco码的坐标系将作为整个环境中的参考世界坐标系，应选择在合适位置的ArUco码，比如墙角、天花板中心等。

步骤1.2.3：手持或机载相机缓慢移动，保证相机能够遍历所有ArUco码。

步骤1.2.4：建图程序运行结束，环境中所有ArUco码的ID及其对应四个角点的三维坐标会保存在一个脚本文件中，且附带一个ArUco码的分布点云。

将脚本文件与分布点云合称为ArUco码的分布地图，简称地图。

上述视觉方法建图的基本原理是，根据每个ArUco码各自与相机之间的位姿关系，经过ArUco码与相机之间的坐标变换，从而推算出每个ArUco码之间的相对位姿关系。然后根据指定的参考ArUco码，将所有ArUco码的角点坐标都转换到此ArUco码坐标系下，最后便获得了所有ArUco码在世界坐标系下的位姿分布，世界坐标系就是指定ArUco码的坐标系。

在步骤1.2.3中，为了提高建图的精度，相机应该缓慢平稳地移动，尽量保证相机能在整个过程中同时看到多个ArUco码，可在环境中多行走几遍，以使全局数据得到充分优化。

步骤2：创建路径：在计算机中生成导航路径的坐标点，获得导航路径文件，具体实现步骤如下：

步骤2.1：根据室内环境和机器人的任务合理规划机器人的行走路线。

步骤2.2：将机器人的行走路线在世界坐标系下用数学方程表出，复杂的路线可分段用方程表出。

步骤2.3：根据导航路径的起止点坐标，在计算机中将用数学方程表示的行走路线离散化为一系列三维坐标点，所有点的坐标按顺序保存在一个文本文档里，称为导航路径文件。

导航路径是由坐标点组成的虚拟路径，在真实物理环境中不可见。导航路径每单位长度内包含的点数称为路径点的密度，路径点之间的距离称为导航路径的分辨率。在本实施例中，平均每1m的导航路径中包含1000个路径点，此时导航路径的分辨率为1mm，导航效果良好。导航路径的分辨率不宜过高，分辨率通常在1cm～1mm为佳。

步骤3：定位与导航：根据步骤1和步骤2创建的地图和路径，利用相机实现机器人在三维空间中的视觉定位与导航，这一部分的方法流程如图4所示，具体实施步骤如下：

步骤3.1：加载ArUco码分布地图和导航路径文件，将ArUco码的分布点云和导航路径显示到图形界面中。

加载ArUco码分布文件以获取所有ArUco码的ID以及对应角点在世界坐标系下的三维坐标和姿态，加载导航路径文件用以获取导航路径中所有路径点的三维坐标。

步骤3.2：根据图像中每个ArUco码角点的像素坐标和其对应的三维世界坐标，利用PnP迭代算法估计相机的位姿，从而实时地获取机器人在世界坐标系下的三维空间坐标和姿态。具体包括如下步骤：

步骤3.2.1：相机采集一帧图像，对图像进行灰度转换、二值化、轮廓提取等预处理操作，然后对每个轮廓进行筛选、透视变换、内部编码提取。

步骤3.2.2：检测图像中是否出现至少一个ArUco码，如果是，根据识别出的所有ArUco码的ID信息，确定每个ArUco码的角点像素坐标和与之相对应的三维空间坐标，利用PnP迭代算法估计相机在世界坐标系中的位置和姿态；如果否，则跳转到步骤3.2.1。

步骤3.2.3：根据相机与机器人本体的相对位姿关系和相机在世界坐标系中的位姿进行坐标变换，计算出机器人在世界坐标系下的位置P和姿态向量V。

在步骤3.2.2中，当图像中检测不到ArUco码时，可考虑使用多目相机或鱼眼相机等增大相机的视野。如果不可避免地出现在较长一段时间内图像中持续检测不到ArUco码的情况，可考虑加入惯导装置等进行辅助定位。

在步骤3.2.3中，如果假定相机坐标系与机器人本体坐标系一致，则可直接用相机位姿代表机器人位姿，否则，需要进行坐标变换。

步骤3.3：判断机器人是否到达目标位置，如果是，则结束；如果否，则计算出与机器人当前位置距离最近的路径点并确定下一个参考路径点的位置。

判断机器人是否到达目标位置可以根据机器人当前位置与目标位置之间的欧式距离d确定。设机器人当前位置坐标为P(x,y,z)，目标位置坐标为P′(x′,y′,z′)，则

如果距离d足够小，则认为机器人已到达目标位置，这个限度可根据实际情况而定。

机器人在行进过程中要明确自身当前位置与导航路径之间的关系，即找到机器人当前位置与导航路径中的哪个点最近，然后根据距离最近的路径点确定下一个参考的导航路径点。下一个路径点的选择需要考虑到一个前瞻性的需求，即机器人需要事先了解前方的路况，以及时地调整自身的姿态，所以下一个路径点应该是与机器人当前位置最近的路径点前方的第n个路径点。当机器人前进速度较快时，则n较大；当机器人前进速度较慢时，则n较小。n的值亦与导航路径点的密度有关，密度越大则n越大，密度越小则n越小。当机器人通过前瞻参数n看到前方即将到达终点时，此时机器人应适当减速，并逐渐减小n的值。

步骤3.4：计算机器人当前位置与下一个参考路径点之间的方向向量V′，并计算机器人当前姿态向量V与方向向量V′之间的夹角α。

步骤3.5：机器人跟据偏航角α调整自身的姿态以速度v沿着导航路径前进，程序跳转到步骤3.2。

以上为本实施例的完整实现过程。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (6)

1.一种基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：包括如下步骤：

步骤1：创建地图；

利用相机对环境中随机分布的ArUco码进行视觉建图，获得ArUco码分布地图；

步骤2：创建路径；

在计算机中生成导航路径的坐标点，获得由一系列三维坐标点组成的导航路径文件；

步骤3：定位与导航；

根据步骤1和步骤2创建的ArUco码分布地图和导航路径文件，利用相机实现移动机器人在三维空间中的视觉定位与导航。

2.根据权利要求1所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤1中，具体包括如下步骤：

步骤1.1：将打印好的ArUco码随机地粘贴在包括墙面、门面、地面、天花板或立柱在内的光照条件良好的平整位置；

步骤1.2：用相机对室内随机分布的ArUco码进行视觉建图，确定一个参考世界坐标系，并获得所有ArUco码的ID信息及其对应四个角点在世界坐标系下的三维坐标。

3.根据权利要求2所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤1.2中，用相机对室内随机分布的ArUco码进行视觉建图，具体包括如下步骤：

步骤1.2.1：用棋盘格预先标定出相机的内参数；

步骤1.2.2：打开ArUco建图工具，传入相机内参，并指定环境中的一个ArUco码作为参考，程序开始运行；

步骤1.2.3：手持或机载相机缓慢移动，使相机能遍历所有ArUco码；

步骤1.2.4：建图程序运行结束，得到ArUco码的分布地图，分布地图中包含环境中所有ArUco码的ID信息及其对应四个角点的三维坐标信息，另外包括一个分布点云。

4.根据权利要求1所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤2中，具体包括如下步骤：

步骤2.1：根据周围环境和移动机器人的任务规划机器人的行走路线；

步骤2.2：将移动机器人的行走路线在世界坐标系下用数学方程标出；

步骤2.3：根据导航路径的起止点坐标，在计算机中将用数学方程表示的行走路线离散化为一系列三维坐标点，所有点的坐标按顺序保存在一个文本文档里，得到导航路径文件。

5.根据权利要求1所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤3中，具体包括如下步骤：

步骤3.1：加载ArUco码分布地图和导航路径文件，并将ArUco码的分布点云以及导航路径显示到可视化界面中；

步骤3.2：根据图像中每个ArUco码角点的像素坐标和其对应的三维世界坐标，利用PnP迭代算法估计相机的位姿，从而实时地获取机器人在世界坐标系下的三维空间坐标和姿态；

步骤3.3：判断机器人是否到达目标位置；

若：判断结果是机器人到达目标位置，则结束；

或判断结果是机器人没有到达目标位置，则计算出与机器人当前位置距离最近的路径点，并确定下一个参考路径点的位置；

步骤3.4：计算机器人当前位置与下一个参考路径点之间的方向向量V′，并计算机器人当前姿态向量V与方向向量V′之间的夹角α；

步骤3.5：机器人跟据偏航角α调整自身的姿态，以速度v沿着导航路径前进，然后执行步骤3.2。

6.根据权利要求5所述的基于ArUco码的移动机器人视觉定位与导航方法，其特征在于：在步骤3.2中，具体包括如下子步骤：

步骤3.2.1：相机采集一帧图像，对图像进行包括灰度转换、二值化和轮廓提取在内的预处理操作，然后对每个轮廓进行筛选、透视变换、内部编码提取；

步骤3.2.2：判断检测图像中是否出现至少一个ArUco码；

若：判断结果是检测图像中出现至少一个ArUco码，则根据识别出的所有ArUco码的ID信息，确定每个ArUco码的角点像素坐标和与之相对应的三维空间坐标，利用PnP迭代算法估计相机在世界坐标系中的位置和姿态；

或判断结果是检测图像中没有出现ArUco码，则执行步骤3.2.1；

步骤3.2.3：根据相机与机器人本体的相对位姿关系和相机在世界坐标系中的位姿进行坐标变换，计算出机器人在世界坐标系下的位置P和姿态向量V。