基于AprilTag与激光雷达的移动机器人自动跟随避障方法

摘要

本发明公开了一种基于AprilTag与激光雷达的移动机器人自动跟随避障方法。在通过AprilTag码定位跟随目标的三维距离的同时进行激光雷达的辅助定位，提高对跟随目标定位的精准度、可信度和鲁棒性。在激光雷达实现辅助定位的同时实现局部避障的功能，从而降低建图与定位对机器人计算资源的消耗，实现结构相对简单和硬件要求相对较低的移动机器人自动跟随和避障功能，解决在机器人在自动跟随过程中出现的目标遮挡及丢失的问题。

说明书

技术领域

本发明属于移动机器人自动跟随避障方法技术领域，特别是涉及一种基于AprilTag与激光雷达的移动机器人自动跟随避障方法。

背景技术

AprilTag是Edwin Olson于2011年提出的一种以二维图形作为视觉基准定位的方法，特点是信息量少，可识别距离大，鲁棒性高，部署环境对硬件要求限制较少，特别适合于移动机器人的目标识别和定位领域。在机器人跟随的应用场景下，可通过用常见的普通CMOS摄像头识别出AprilTag，并计算出Apriltag与摄像头之间的三维距离关系，缺点是并不能感知到周围环境的三维距离关系。

激光雷达作为可以通过激光反射时间精准判断激光发射点到激光反射点的距离，可以通过环线扫描获知机器人与周围环境的三维关系。在移动机器人自动跟随领域中通过对周围环境的感知建立环境地图，同时定位机器人自身在环境中的位置，所谓SLAM过程(Simultaneous Localization And Mapping)。在此基础上将环境信息以地图的形式存储到设备存储空间中，并同时根据环境信息识别出障碍物，进行避障动作。激光雷达环状扫描的过程由于自身基于构件向外围放射激光的物理特性，点云特征呈现近密远疏的特性，对于远处目标的点云数据较为稀疏，给识别目标带来难度。高精度高实时性大范围的SLAM方法在移动机器人的部署实现对机器人的计算能力和建图所需设备的存储能力有一定的要求，资源所需成本较高。

多传感器融合是提高移动机器人感知周围环境可靠性、鲁棒性和精度的有效手段。移动机器人领域最为常见的是相机和激光雷达的多模态融合(即RGB图像和点云数据)，但两种不同的数据进行融合可以通过融合的时间阶段的不同划分为前端数据融合、特征融合和决策融合，即前、中、后三个融合阶段。在前融合阶段，RGB数据和点云数据在采集端直接进行融合，特点是两种传感器数据融合后不会丢失，环境感知信息最为完整，但往往融合数据量较大，对后续数据处理的算法和计算能力带来较大压力；特征融合是各自分别将RGB数据和点云数据进行处理，提取相关特征后将对应特征进行融合，数据压力和计算要求相比前端数据融合较小，同时也会丢失一些前端数据特征；决策融合往往是各自将RGB数据和点云数据进行处理得出决策，在决策阶段将多源传感器的各自决策进行汇总，综合决策出最佳方案，决策融合相比前两个阶段，数据量更小，丢失信息更多，计算压力更小。

自动跟随技术作为移动机器人领域的基本功能需求之一，在当今人口老龄化和产业结构调整的环境下其应用场景的广度和重要性显著提升。移动机器人自动跟随技术在实际应用的过程中对避障功能的需求是不可避免的。高精度的自动跟随避障方法实现往往需要多传感器和复杂融合算法，需要成本较高的计算单元。目前移动机器人的普及受到成本高昂，结构复杂维护难度大等因素的制约。

发明内容

本发明目的在于解决目前移动机器人成本高昂，结构复杂维护难度大的问题。

为了实现本发明目的，本发明提供了一种基于AprilTag与激光雷达的移动机器人自动跟随避障方法，包括以下步骤：

步骤1、构建移动机器人平台，使激光雷达和相机同在移动机器人的前进方向中轴线上，并无横向上的偏移量；

步骤2、对相机进行标定，构建相机像素坐标系和世界坐标系的转换矩阵，即像素值与真实世界尺度的映射关系；

步骤3、确定移动机器人真实速度与代码控制速度的比例参数；

步骤4、在跟随目标的背后显著处粘贴AprilTag码；

步骤5、将单目相机中呈现的Apriltag码中心所占取的像素位置和距离相机中心的像素距离根据像素坐标系与真实世界坐标系的转换矩阵计算出AprilTag码在移动机器人视野中心的X方向、Y方向投影距离和纵向距离Z；根据纵向距离Z和X方向的偏移距离进行机器人自身速度计算(Y方向为地面高度方向，无需考虑)，并测算跟随目标相对于机器人的移动速度，根据机器人与跟随目标之间的真实距离和跟随目标以机器人为参考系的相对速度，综合得出移动机器人的最终速度；同时用激光雷达作为辅助定位方式，确保精度、可信度和鲁棒性的提高；

步骤6、在移动机器人跟随目标的过程中，用激光雷达扫描周围环境得到的点云信息进行处理，过滤地面点云信息；将处理后的点云信息以鸟瞰图的形式作为局部避障的环境信息，得出可通行区域，进行避障动作，并在到达安全点后进行搜寻动作；

步骤7、在跟随过程中出现目标丢失或目标遮挡的情况下，根据丢失点的目标信息并记录转向指标，估算出固定时间内移动机器人到达丢失点的速度，并保持速度到固定时间结束；此时机器人到达目标丢失点，根据转向指标进行车辆转向，在可能的方向上寻找丢失的跟随目标。转向指标所指向的范围内，如转向指向右侧(正)即小车转向90°，即在0-90°的转向过程中，均有可能找到跟随的目标。

进一步地，步骤1中，移动机器人平台，包括机器人底盘、微型电脑、锂电池、相机、激光雷达；移动机器人不需要配置IMU、轮式编码器；机器人底盘设置有实现转向功能的驱动轮组和串口接收器，锂电池用于为机器人底盘提供电源，激光雷达和相机连接于微型电脑，微型电脑连接于机器人底盘的串口接收器，包括但不仅限于USB、WIFI、移动蜂窝信号等形式。

进一步地，步骤2中，对相机参数进行标定，根据Apriltag码在像素坐标系的像素位置和大小对应计算出真实世界坐标尺度中AprilTag码的实际大小和位置，对应关系如下所示：

式中，(u

进一步地，步骤5中，确定跟随目标位置时，通过相机AprilTag码定位的技术方案得出的跟随目标的三维距离，通过ROS系统将AprilTag识别定位模块打包为ROS功能包，并利用ROS中的订阅发布机制，以话题的形式将机器人和跟随目标的角度发送给激光雷达的数据处理模块，由激光雷达通过对应角度的点云数据进行定位并加以确认。

进一步地，步骤6中，在激光雷达进行点云处理时，如果是多线雷达产生的点云数据，通过点云数据处理模块进行合并简化，即鸟瞰的视角将多线雷达的点云数据合并，同一激光射线方向过滤掉较远处的点云数据，保留近处的点云数据，并以过滤后的鸟瞰形式点云作为局部避障的根据。

进一步地，步骤6中，在避障完成到达安全点后，若暂时未检测到跟随目标，机器人根据避障过程中最后检测到跟随目标的姿态进行预测性转向搜寻。

进一步地，步骤7中，丢失跟随目标时，机器人通过程序计算，以预设时间(人为设定的丢失追踪时间)到达目标丢失点，并根据目标丢失时的姿态进行预测性转向搜寻。

与现有技术相比，本发明的显著进步在于：1)结合了激光雷达作为环境感知模块，无需大量的计算能力的同时可以高精度实时地感知周围障碍物信息；2)在数据融合中采取了决策融合的方式，以距离和方向作为数据桥梁，沟通两种传感器的处理流程，减少了数据融合过程中数据量增加、算力要求增高的现象；3)现有技术方案中，往往不涉及跟随目标丢失后的处理机制，多是原地旋转或等待等方式，过于简单且较难适应以跟随目的为背景下的应用场景。本发明以丢失目标后可以自动前进到目标丢失点或者障碍物通过后的安全点为丢失后的第一阶段，以完成第一阶段后通过转向指标确定搜寻方向开始搜索丢失目标的过程为第二阶段，两个阶段共同构成了目标丢失后的处理机制，使机器人的行为方式更加智能且拟人化，解决了跟随目标丢失和遮挡的问题，提高了机器人跟随目标的成功率。

为更清楚说明本发明的功能特性以及结构参数，下面结合附图及具体实施方式进一步说明。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为Gazebo仿真环境下机器人结构示意图；

图2为真实世界坐标系和相机坐标系的关系图；

图3为相机标定板示意图；

图4为机器人真实世界坐标系下跟随关系示意图；

图5为Gazebo仿真环境下机器人跟随场景的环境示意图；

图6为16线激光雷达点云数据图；

图7为点云数据处理结果图；

图8为16线点云转换为单线点云鸟瞰图；

图9为点云数据融合相机数据的点云鸟瞰图；

图10位本发明总体流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图10所示，配置机器人平台后，启动跟随程序，相机和激光雷达作为传感器模块启动，由相机传感器模块发送图像数据到跟随模块，通过AprilTag技术方案识别出跟随目标，记为第一次目标出现，此时激光雷达模块将环境数据处理并判断是否存在障碍物；若无障碍物，将AprilTag技术方案计算得出的目标距离通过激光雷达模块进行再次确认并进而计算出跟随的速度和方向。若存在障碍物，进入避障行为模式，机器人将选择前进方向180°范围内的最大空隙的中线方向作为前进方向，到达安全点后进入搜寻模式。以上过程中，时刻通过相机模块和AprilTag处理模块实时更新目标的姿态信息作为转向指标，在机器人到达避障安全点后或者到达丢失点后，开启搜寻模式，进行当前机器人方向为方向起点，转向指标指向的方向(丢失点转向指标为-90°或+90°，避障安全点转向指标为-180°或+180°)为终点，搜索跟随目标。跟随过程中，若丢失目标，进入丢失模式，机器人移动到丢失点后同样开启搜寻模式，流程以搜寻模式无法搜索到跟随目标为结束标志。

实施例

本实施例基于以下硬件平台：CPU配置为

本实施例基于以下软件环境：计算机操作系统：Ubuntu20.04，搭配ROS(RobotOperation System)通用的机器人软件框架封装各个功能包同时使各功能包之间可以通信。

在本实例中，图1为仿真环境下机器人结构示意图。

本方法具体实施步骤如下：

(一)参数确定

(1)相机参数确定

本方法需要对CMOS相机参数进行标定，其目的是根据Apriltag码在像素坐标系的像素位置和大小对应计算出真实世界坐标尺度中AprilTag码的实际大小和位置。

其对应关系如图2用矩阵来表示：

(u

本实例采用外标定的方法，即通过外在参照物进行相机标定。本实例可采用张正友标定法。张正友标定法是通过已知真实尺寸的黑白相间的棋格图(图3)作为外界参照物进行一系列的相机参数标定，将标定得到的一系列矩阵信息作为camera.info文件注入到.ros文件夹中(ros系统运行时可将.ros文件夹中的文件信息读取供ros功能包使用)。

(2)机器人移动速度参数确定

可根据不同型号确定机器人线速度控制代码和实际代码的比例关系。本实例根据机器人底盘说明书和现场实测，可获得对应关系为T＝0.024m/s(米/秒)。

(二)计算目标的三维距离

在参数确定的基础上，启动单目相机并用ROS打包编译好的AprilTag节点，该节点将识别机器人视野中的AprilTag码并发布AprilTag码所在位置与机器人坐标系的话题名为“/TF”，TF是ROS系统中一种树形数据结构，可以反映两个坐标系的位置关系。

在本实例中，两个坐标系的关系指的是移动机器人的中心点为原点的坐标系和AprilTag码图像中心为原点的坐标系之间的关系。

通过TF话题得到消息可以知道AprilTag码到移动机器人的X方向、Y方向、和Z方向的距离，以及AprilTag码的姿态，即绕X、Y、Z方向的旋转角。

得到AprilTag码和移动机器人的三维距离即得到了机器人和跟随目标之间的三维距离。

(三)跟随目标的速度控制策略

移动机器人与跟随目标距离关系如图4所示。

速度控制策略的目的有三个方面：

移动机器人和跟随目标保持速度v同步；

移动机器人和跟随目标保持方向α同步；

移动机器人和跟随目标保持设定距离L

距离L

同时启动激光雷达感知周围环境(图5)产生点云信息(图6)，将地面点云信息去除(图7)，并将16线点云数据合并为单线数据(图8)，将相同射线角度的点云数据去最小值，并根据α角度锁定点云数据中感兴趣方向，探知同为α角度为中线左右扩展5°(共计10°范围)范围的激光束中激光点的距离(图9)，作为L

角速度始终渐近于α，直到α变为0。

通过跟随目标三维距离的变化可计算出基于移动机器人为参照系的跟随目标速度变化v，移动机器人相对于真实世界参照系初始速度为0，此时将移动机器人速度渐近调整为和跟随目标速度保持一致。

如果移动机器人和跟随目标之间速度一致，但距离L小于设定的保持距离L

上述所有命令通过串口的形式发送给移动机器人底盘。

(四)跟随目标丢失的策略控制

在目标丢失后记录目标丢失时刻的三维距离和姿态，根据设定时间t和丢失时刻的距离L，得出所应保持的速度v

v

移动机器人前进方向由渐近趋向α转换为立刻旋转α角度，以保持丢失时刻移动机器人前进方向与目标丢失点无偏差。

达到时间t之后，移动机器人即达到了目标丢失点。

此时根据丢失时刻的目标姿态，目标姿态分为绕X方向、Y方向、Z方向旋转角度

若旋转至搜寻方向90°后依然无目标出现，本次跟随任务结束。

若期间雷达监测到前方有障碍物出现，则切换为避障丢失模式，扫描周围点云最大空隙，并记录检测到障碍物时，跟随目标与最大空隙中线的方向侧F(在最大空隙中线的右侧或左侧)，并以丢失时刻移动机器人的速度v、前进方向为最大空隙中线方向，并在通过最大空隙后向方向侧F旋转180°(F为前进方向左/右)作为搜寻方向，寻找跟随目标。若在避障过程中检测到跟随目标，则随时更新方向侧F作为到达安全点后的搜寻方向。

若旋转至搜寻方向180°后依然无目标出现，本次跟随任务结束。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。