基于ArUco码的无地图AMR视觉辅助归航方法*

郭驿众，李 根，许佳斌，王卫军，冯 伟，王 建

(1. 广州先进技术研究所·广州·511458；2. 中国科学院深圳先进技术研究院·深圳·518055)

0 引 言

作为自主移动机器人(Autonomous Mobile Robot，AMR)的关键组成部分，导航模块已经发展出以惯性、地磁、视觉、激光雷达等传感器为基础的同时兼具效率和精度的导航方法。由于图像中包含大量空间信息，对于室内环境中工作的AMR，采用视觉传感器的导航方式在信息丰富度和环境适应性上具有较大的优势[1]。

视觉归航(Visual Homing，VH)是一种轻量级机器人的导航方式，只利用参考位置和当前位姿下拍摄的图像来进行导航，不需要当前状态之前的路径和运动。该导航方式通常采用纯方位方法实现，比如Cartwright和Collett仿照蜜蜂归巢原理提出的快照模型(Snapshot Model，SM)[2]，通过移动不断减小当前位姿下图像和参考图像的差距来达到靠近目标点的目的。Corke P使用平均地标矢量(Average Landmark Vector，ALV)原理[3]，通过检测地标并对比当前位置和目标位置关键点方向矢量和的差值求出导航矢量。Robert和 Michael等人提出考虑图像平面均匀网格上点的流向量属性[4]。流向量指示当前图像中局部区域必须移动的方向，通过对齐当前图像与参考图像提供导航控制。这些传统的视觉归航方法多依赖于环境中的全局地标及导航过程中地标的选择[5]，且对于环境纹理及光照的要求较高，对于动态环境则缺乏鲁棒性[4]，此外，还存在无法获得精确的导航位置，以及无法保证收敛性等问题[5]。

通过引入ArUco码作为地标，可以解决传统视觉归航方法的弊端[6]。相较于传统的基于视觉的方式，对ArUco码进行识别和相对定位可以直接估计位置和航向，减少了计算量，且具有较高的精度和鲁棒性[7]。另外，可以通过将ArUco码粘贴在天花板上减少可能受到的障碍物遮挡的影响[8]。ArUco码具有成本低廉，信息获取较为简单，可以使用低成本摄像头进行检测等优点[7]。然而采用ArUco码辅助视觉导航有时局限于单个标记，比如在预定位置采用ArUco码标记引导自动导航小车(Automated Guided Vehicle，AGV)转向[9]；或者至多局限于一部分标记，且这些标记的相对位姿需要已知，比如与同步定位和建图(Simultaneous Localization and Mapping，SLAM)进行结合[10]，在使用ArUco码辅助机器人导航之前，对ArUco码进行全局定位并提前构建地图，通过识别ArUco码对比全局地图，提供自主移动机器人的位置和航向信息。

使用SLAM对ArUco码进行建图，具有较高的定位精度，然而其需要预先构建精确且全局一致的地图，当地图丢失时，机器人立即失去定位和导航的能力。本文提出了一种在没有构建全局地图的情况下，通过对既往路径中记录的ArUco码信息进行反向对比和搜寻的方式，实现移动机器人无地图自主视觉返航同时实现避障功能的方法。

1 视觉导航设计

1.1 系统设计

如图1所示，相机视界内通常可以识别到多个ArUco码，机器人的运动会导致识别的ArUco码产生时间顺序上的连续变化，本文将通过识别和记录对应ArUco码构建的局部坐标系下的关键位姿，并通过不同的ArUco码构建的局部坐标系的连续变换实现机器人的反向导航并回到出发点。然而该方法只能适用于机器人经过和记录的地标物，当遇到动态障碍物或者未曾经过的地方，该方法就无效了，因此，本文引入里程计，通过对车轮转速进行积分来获得机器人既往任意时刻的位姿。然而里程计存在累积误差，且随着累计时间的增加而变大，因此机器人只采用较近期位姿实现超出原有识别区域的自主返航。同时使用激光雷达提供障碍物的距离和方位信息用以避障。

图1 相机视界与机器人轨迹Fig.1 Camera field of view and the robot trajectory

具体来说，系统有返航和非返航两种状态，非返航状态下可以通过预设轨迹或手动方式操作移动机器人进行运动，全过程中相机采用固定频率对视界中的ArUco码进行识别，由于同一时刻相机视界内存在多个二维码，解算当前时刻相机相对视界内所有ArUco码的位姿，并以当前时刻相机直线距离最近的ArUco码建立局部坐标系，如果该时刻是初次识别到ArUco码的时刻，将机器人当前相对于该ArUco码的位姿作为关键位姿进行保存，同时左右轮编码器通过计算转动速度积分并记录车辆轨迹。

返航状态的流程如图2所示，首先相机对视界内所有ArUco码进行识别；再检索数据库中是否有保存对应ArUco码ID号，并以此判定是否经过该ArUco码对应的可识别区域。如果保存有对应的ID号，则判定经过，并进行基于ArUco码时序最小的导航偏差估计。由于ArUco码存取是按照识别的时间顺序保存，如果存有甚至有多个ArUco码被识别和保存，则选取时序更小的ArUco码构建局部坐标系，获得机器人在当前ArUco码坐标系下的位姿作为当前点，并读取该ArUco码被初次识别时的机器人的位姿作为目标点。如果没有保存对应的ArUco码，说明机器人移动到一个未曾到过的区域，则判定未经过，则以当前视界范围内直线距离最近的ArUco码构建局部坐标系，并获取机器人在当前坐标系下的位姿作为当前点。通过读取编码器积分轨迹选取前瞻点作为目标点。

图2 无地图视觉导航系统返航流程图Fig.2 Flow chart of the visual homing navigation system with no map building

随后，通过激光雷达判定是否有障碍物及与障碍物之间的距离。当判定路径上没有障碍物时，利用纯跟踪(Pure Pursuit，PP)导航算法计算线速度和角速度并驱动机器人移动，直至返回出发位置。当判定路径上存在障碍物时，采用动态窗口(Dynamic Window Approach，DWA)算法对机器人进行轨迹预测，同时通过读取编码器积分轨迹选取前瞻目标点，实现避障功能并返回既往轨迹。

1.2 导航原理

基于人工标记物位置固定的前提下，如ArUco码粘贴在天花板上(图3)，相机在任何可以拍摄到ArUco码的位置都可以通过识别视界内的ArUco码获得该码坐标系下的相对位姿，即该位姿下相机坐标系相对于ArUco码坐标系的空间转换矩阵。如图3所示，M1CT为相机相对1号ArUco码的空间转换矩阵，M2CT为相机相对2号ArUco码的空间转换矩阵。同时，雷达和相机固定在机器人上，且两个传感器都有其特定的测定坐标系。从图3中可知，雷达坐标系、相机坐标系及机器人坐标系的Z轴重合，且三者原点都位于机器人本体中轴线上，通过测量固定位置的距离，可以很轻易地实现从雷达到相机及从机器人到相机的坐标转换，为了简化说明，后文以相机坐标系替代机器人本体坐标系进行讨论。

图3 局部坐标系的连续变换Fig.3 Continuous transform of the local coordinate system

M2rR=M2CR×CrR+M2CS

(1)

CrRQ=M2CRR-1×(M2rQ-M2rR)

(2)

通过将方向矢量带入运动学模型中即可解出机器人线速度及角速度[v，ω]。为了实现连续的导航，机器人需要在上一个ArUco码离开相机视界范围之前识别到下一个ArUco码并重复构建局部坐标系、坐标转换及计算控制量的步骤，即要求任意ArUco码间距小于相机的视界直径。

在非返航过程中机器人将所有识别的ArUco码以时间顺序保存，在返航过程中通过对视界内ArUco码的识别和对比，始终选取时序最小的，可以确保在连续返航的过程中回到出发点的收敛性，同时在偏离原有航迹且非连续倒序识别ArUco码时，对于重新识别的ArUco码仍然可以通过时序对比返回出发点，具有一定的自适应性。

1.3 ArUco码辅助定位

ArUco码是一种由黑色边框及内部白色二进制编码组成的正方形基准标记(Squared Fiducial Marker，SFM)，相较其他人工路标具有检测精度高及检测速度快等优点[6]。通过基于OpenCV的图像识别和处理，将识别出的ArUco码解码为二进制与字典库进行匹配可以获得对应的唯一ID号，并获得ArUco码中心坐标系的四个角点的坐标(见图4)。

图4 ArUco码识别及中心坐标系下角点坐标Fig.4 Recognition of the ArUco marker and the endpoint coordinates at the corresponding ArUco marker coordinate system

(3)

(4)

(5)

将n个匹配点对全部展开，可以写成一个2n×12的矩阵M和一个12×1的矩阵x，所有使得Mx=0的解可以表示为

(6)

其中，vi是M的零奇异值对应的右奇异矢量，通过构建MTM组成的方阵，求解特征值为0的特征矢量，求得为vi。根据刚体结构不变性，不同坐标系下的两个点的相对距离是不变的，原式可优化为

(7)

2 运动学模型的建立

2.1 两轮差速模型

自主移动机器人采用两轮差速模型[14]，通过控制左右两个驱动轮的转动速度可以实现机器人的直行和转向。仅考虑机器人在二维水平面的运动情况，运动学模型如图5。

图5 差速自主移动机器人运动学模型Fig.5 Dynamic model of differential autonomous mobile robot

通过驱动轮上的编码器可以得到左右轮的转动速度vr，vl，已知驱动轮轮距为b，从而解算出机器人中心的线速度及角速度v，ω。

(8)

在驱动轮纯滚动，无侧向滑动的情况下，机器人受到非完整约束[15]，那么n时刻机器人在当前ArUco码坐标系下的位姿[x，y，θ]T存在以下关系

(9)

同时，机器人轨迹可以用圆弧进行表示，且n时刻圆弧半径为Ln=vn/ωn，其中vn，ωn为n时刻机器人的线速度和角速度。通过对左右轮转速记录并进行积分，可以得到小车从开始到结束任意时刻相对于出发点位置的坐标朝向和速度，尽管使用速度积分的方式会引入累积误差。

2.2 局部导航设计

DWA动态窗口法是基于运动学模型，在有限速度和加速度及短暂时间间隔的约束下的一种速度控制方法[15]，多用于障碍物躲避的局部路径规划。DWA可以概括为三步[16]：一是根据机器人自身限制和环境制约将速度的采样空间约束在一定范围内；二是根据机器人运动学对采样后的速度进行模拟得到预轨迹；三是设定评价函数对预轨迹进行评分以获取最优轨迹对应的执行速度。

将机器人当前速度设为[vc，ωc]，根据机器人自身速度及加速度的限制，速度采样空间应满足

(10)

通过设定的采样分辨率获得所有[v，ω]的速度组合，并设定预测时间窗口的长度，根据运动学模型进行模拟获得预测的轨迹和终点位置。为了获取最优的执行速度组合，采用代价函数对每条轨迹进行评价。代价函数定义为

C(v,ω)=αCdist(v,ω)+βCtarg(v,ω)

(11)

式中，使得C(v，ω)值最小的速度组合为最优速度；函数Cdist(v，ω)用来评价机器人在当前轨迹上与最近的障碍物之间的距离，距离越远，Cdist(v，ω)的值越小；Ctarg(v，ω)为当前设定的采样速度下，达到模拟轨迹终点的机器人与当前ArUco码记录的机器人起始点的欧几里得距离，即返航过程始终将当前相机视野范围内索引值最小的ArUco码对应的初次识别到该ArUco码时刻的位置作为返航目标点；α，β为每一个函数的权重。

3 实验验证与分析

3.1 实验平台搭建

本文搭建了如图6所示的两轮差速机器人实验平台进行测试，主要由显示器、工控机和运动底盘组成。摄像头使用的是索尼IMX317，分辨率为1920×1080，帧率为30Hz。实验环境为如图7的6m×12m的实验室，天花板上均匀等距粘贴ArUco码，摄像头与天花板的距离为2.5m；地面采用纸箱作为障碍物。采用激光雷达构建了全局地图及Marker的特征地图为机器人在测试过程中提供定位。

图6 机器人实验平台Fig.6 Robot platform

图7 实验室环境及地图Fig.7 Testing site and the map

3.2 导航原理验证

将相机视界以时间顺序进行拼接。由于相机视角是从下向上，而Rviz建图视角是从上向下，将相机视界翻转后可以得到如图8的视界图和实际轨迹的对比，两者具有高度的相似度，同时相机对ArUco码的识别时序与导航路径点具有同步性，每一个ArUco码的关键位姿与实际机器人轨迹之间存在对应关系，因此，找寻时序更小的ArUco码意味着找寻时序更小的路径点，即更加靠近出发点的位置，直至返回出发点。

图8 相机视界(翻转)与导航轨迹Fig.8 Camera view (reversed)and the corresponding trajectory

3.3 重复定位测试

通过对同一条长度为3m的直线路径进行20次的重复测试，以测定仅靠相机定位及导航的重复定位精度。在机器人停止时，通过读取最小时序ArUco码(即初始ArUco码)获得机器人当前位置的坐标，同时读取出发时刻机器人起始位置坐标，并将这些坐标显示在图9中。停止点距离目标点的平均距离误差为0.0169m(即1.69cm)，方差为0.000887m2(即8.87cm2)，20个停止点的平均停止点坐标为(-0.099788，0.79)cm，距目标点的误差为0.8cm，该距离小于平均距离误差的原因是平均停止点反映了空间上的分布，减小了极端数据对于均值的影响，具有更高的可信度。

图9 20次重复定位测试Fig.9 Result of 20-times repeat positioning test

3.4 绕障轨迹归航测试

通过使用键盘控制的方式控制移动机器人出发，形成一条特定的轨迹，比如实验中采用绕障轨迹，分别对比了绕开单个(图10(a))、两个(图10(b))、三个障碍物(图10(c))及全覆盖路径(图11)的轨迹。

(a)

(b)

(c)图10 移动机器人绕障返航轨迹Fig.10 Homing trajectory of the AMR with obstacle avoidance

图11 全覆盖路径及返航轨迹Fig.11 Full coverage path and the corresponding homing trajectory

在机器人移动的同时，相机对视界范围内的ArUco码标记物进行识别，仅将5次以上关键帧识别的ArUco码ID进行保存，以避免运动过程中相机视界短时间扫过的ArUco码造成的干扰。仅保留长时间稳定识别的ArUco码，同时以此时距离最近的ArUco码的位姿信息作为该ArUco码对应的关键位姿信息，在到达预定目标点后触发机器人返航功能。机器人识别视界内的ArUco码标记物并查找存储的ID号，通过存储的时序选择时序更小的ArUco码生成局部坐标系，提取该ArUco码的关键位姿作为当前坐标系下目标点，并生成控制量以控制机器人移动。在手动操作和返航的同时，采用激光雷达对移动机器进行定位并记录轨迹，采用Rviz可视化工具显示机器人运动轨迹信息(图10)，可以清晰地对比返航前和返航后的轨迹。

在该实验中，机器人对不同的绕障轨迹的返航展示了返航的能力，在仅依靠视觉信息及激光雷达信息进行返航时实现了对静态障碍物的避让，同时返回了出发位置。对于单个障碍物的轨迹(图10(a))，返航轨迹的路程与出发轨迹的路程几乎接近，同时由于仅保存对应ArUco码的关键位姿，稀疏的导航点使得机器人在移动过程中产生一定的震荡；对于两个障碍物的轨迹(图10(b))，返航轨迹相比出发轨迹节约了9.76%的路程；对于三个障碍物的轨迹(图10(c))，返航轨迹相比出发轨迹节约了57.33%的路程。

对于出发轨迹，尤其是在复杂轨迹的情况下，本方法所具有的抄近路的能力更加显著，如图11所示全覆盖路径，返航路程相比出发路程节约了79.16%。

在复杂轨迹相交部或者颈部，由于相机视界范围的覆盖使得有重复ArUco码出现，基于时序最小的导航偏差估计，此时机器人展现出跳过部分路径抄近路的能力。当轨迹越接近直线时，返航路程则接近出发路程。由于机器人导航仅依靠视觉信息，返航的稳定性受到光照的严重影响，当无法识别ArUco码或者严重偏离已识别区域时，机器人会失去导航能力。

4 结 论

提出了一种简单的基于视觉的实现自主移动机器人无地图返航及避障的方法，该方法采用ArUco码作为视觉标定物。机器人在出发状态中识别并以时序记录相对于视界内的ArUco码的关键位姿，使得在返航状态中通过选择视界内时序更小的ArUco码并构建局部坐标系从而实现局部定位与导航，并通过局部坐标系的连续变化，实现无全局地图情况下返回出发点的目的。基于不同出发轨迹，该方法展现出一定的跳过既往路径，以更近的路线返回出发点的能力。当出发轨迹接近直线，返航轨迹接近出发轨迹；对于复杂轨迹的相交部或颈部，由于相机视界识别到时序更早的ArUco码，使得机器人可以以更短的路径返回出发点。实验结果表明：

1)基于相机和ArUco码标记物的情况下，20次重复定位测试中实现8mm的平均定位误差；

2)基于激光雷达和里程计，在无地图的情况下，机器人通过动态窗口算法实现自主返航过程中对于静态障碍物的避障；

3)在全覆盖路径测试中，返航路程相比出发路程节约了79.16%。

本次结果只是初步的，未能实现设计的全部功能，如超出识别范围后基于里程计的偏航矫正等，因此还需要进一步的测试和改进。这项工作为后续工作提供了一些有趣的方向，如基于编码器的航迹推算、优化激光雷达信息实现动态避障返航，以及使用自然特征而非人工标记等。