基于Apriltag算法的在线恢复深度的无人机单目SLAM系统

安晨亮，高喜天，冀 东

（天津工业大学 电气工程与自动化学院，天津300387）

近年来，无人驾驶飞机在视觉伺服[1-2]、非线性控制[3]、SLAM 等许多研究领域受到了广泛的关注。大疆Matrice100 由于其出色的稳定性，对开发的再次支持[4]和强大的耐力而被广泛用于无人机研究领域。例如轨迹跟踪控制[5]，目标检测和跟踪[6]，路径规划[7]，基于视觉的导航[8-9]等。在室外环境中，大疆无人机可以稳定多旋翼的位置，并通过全球定位系统（GPS）提供正确的姿态和速度信号估计。但是，在室内环境下使用大疆Guidance[10]时，它无法直接融合来自外部传感器的任何数据，因为缺少GPS 信号。因此，设计无人机的框架以提供状态和控制信息非常重要，该信息可以获取无法访问GPS 的速度和姿态。

单目相机具有结构简单、成本低廉、采样数据充足等优点，在SLAM 研究领域引起了广泛的关注。利用单目视觉，已经有许多关于状态估计，里程表和SLAM 的完整著作，包括PTAM[11]、SVO[12]、LSDSLAM[13]和VINS-Mono[14]。对于单目视觉和无人机上的SLAM 应用而言，快速完成初始化并准确恢复真实尺度信息至关重要。为了解决初始化问题，已经提出了许多方法，例如文献[15]和文献[16]。在文献[15]中设计了线性估计器初始化模式，它利用了来自短期惯性测量单元（IMU）预积分的相对旋转，文献[16]中针对单目视觉惯性初始化问题引入了封闭形式的解决方案。

使用ORB-SLAM2[17]，仅通过使用单目视觉就可以快速完成初始化过程，这意味着可以满足无人机的状态估计和建图需求。但是，由于无法从单目相机恢复深度信息，因此，地图的比例和预测的轨迹是未知的。对于无人机上的单目SLAM 应用，应该完成确定有关单目相机的比例尺信息的过程。此外，Apriltag2[18]最近因其稳定性和准确性而备受关注，可用于单目SLAM 的尺度恢复。在文献[19]中，基于具有饱和位置的四旋翼无人机的线性化动力学，开发了一种非线性控制器。文献[20]中将反推和滑模技术都应用于无人机轨迹跟踪。在文献[21]中开发了一种用于轨迹跟踪任务的四旋翼无人机的几何控制器，它可以控制欠驱动的四旋翼无人机使用4 个推力输入来稳定6 个平移和旋转自由度，同时渐近地跟踪4 个由其位置和航向组成的输出。

1 系统概述

框架中5 个模块的关系如图1所示。深度求解模块首先被设计为利用Apriltag2 算法来计算比例尺信息。初始化单目模式的ORB-SLAM2，然后将其与相机深度融合以提供状态估计结果。引入了轨迹计划模块以生成所需轨迹，并且控制器模块通过融合实时姿态信息来计算用于安全、准确地为无人机提供运动的输入命令。最后，大疆自动驾驶仪通过硬件接口接收控制器发送的输入命令。框架中所有模块之间的通信都是通过ROS 接口进行的。

图1 单目视觉无人机控制框架概述Fig.1 Overview of monocular vision UAV control framework

2 自主式单目无人机控制系统

2.1 框架转换和深度求解

该框架的第一个模块是通过合并Apriltag2 和ORB-SLAM2 获得有关单目相机的深度信息。位置、姿态的发布和转换模块是在ROS 环境设计的。无人机上安装了英特尔实感D435i 传感器，并且所提的框架中仅使用单目相机，相机坐标系表示为Fc，Fc在初始时刻时表示为Fc0，Fb表示无人机机体坐标系，Fb在初始时刻t=0 时表示为Fb0。

参考坐标系定义在Apriltag2 的标签上，用Fw表示。利用标签检测算法，在Fc下表示的坐标系Fw的旋转矩阵和平移向量都是通过Apriltag2姿态计算算法获得的。表示坐标系Fb在Fw下的转换关系，同样的表示坐标系Fc在Fw下的转换关系，表示预校准的相机和无人机之间的转换关系。根据坐标系转换原理，可以通过计算和得到，计算过程如下所示：

系统启动时，Apriltag2 和ORB-SLAM2 都会启动，t′表示ORB-SLAM2 初始化完成的瞬间。我们假设启动系统时标签位于相机视场中。通过对Apriltag2进行姿态测量，可以得到从时间ti到Fc0的摄像机帧的变化量，如下所示：

此外，在时间t′处，可以在ORB-SLAM2 的输出下获得从Fc0到时间t′的摄像机坐标系的缩放平移矢量。因此，通过计算相同相机平移矢量的上述两个不同表达式之间的比率，可以恢复晦涩难懂的ORB-SLAM2 的比例因子s：

2.2 状态估计模块

结合已建立的单目SLAM（例如ORB-SLAM2），将其用于UAV 平台，它可以提供可靠、准确的UAV状态估计和工作区映射结果。

在ORB-SLAM2 中，仅运动包调整（BA）可以将摄像机帧的方向和位置从Fc0最佳化为当前时间t，从而使匹配的3-D 点Xi∈R3在Fc0以下和图像关键点与i∈c 之间的重投影误差最小。所有匹配项的集合为

式中：r（·）是鲁棒的Huber 成本函数；∑是与关键点规模相关的协方差矩阵。pm（·）的定义如下：

式中：校准的相机参数（fx，fy）是缩放的焦距；（cx，cy）是图像上的主要特征点。

原始的ORB-SLAM2 主要具有3 个并行线程：跟踪，局部映射和循环关闭。但是，生成的姿态信息不会实时发布。为了解决无人机无法直接获得深度融合ORB-SLAM2 提供的状态估计的问题，设计了一种姿态发布转换模块，如图2所示。

图2 发布和转换模块Fig.2 Release and conversion module

在图2 中，姿态信息是通过利用ROS 发布者/订阅者工具进行传输的。利用转换节点，可以根据摄像机/UAV 坐标系的关系从深度融合摄像机的姿态中获得UAV 姿态，如图3所示。

图3 无人机和相机之间的坐标系关系Fig.3 Frames relationship of the drone and onboard camera

2.3 轨迹规划模块

为了通过无人机完成复杂的任务，通常会结合平台大小、目标跟踪、能耗等因素，组合适当的运动轨迹计划。

在本框架中，和文献[3]类似，以伯努利曲线为例，将伯努利曲线作为期望轨迹，因为其难以被跟踪的特性，可以很好地评估后续跟踪控制模块的性能。伯努利曲线的参数方程如下所示：

在式（12）中，引入参数w 来设置所需的线性和角速度，r 与曲线的大小有关，并且参数p/2 涉及与无人机起降过程的合理连接。

2.4 运动控制器

对于大疆无人机而言，不支持期望总时刻的输入，推力fu（t）、俯仰角qu（t）、横摆角fu（t）、偏航率是唯一可用的输入。为了应对这种情况，在框架中采用了基于SE（3）控制方法的几何控制器[21]来实现复杂的轨迹跟踪任务。

控制过程如图4所示。位置和速度误差都可以通过计划模块和当前状态估计模块中的所需轨迹来计算。

图4 控制流程Fig.4 Control flow chart

ep（t）表示期望位置和当前位置之间的误差向量；ev（t）表示期望速度和当前速度之间的误差向量；m 是整个系统的总重量。对角增益矩阵表示为Kp和Kv。然后，所需推力fu（t）计算公式为

如下提供了计算控制器的期望姿态输入Rd（t）的方法：

然后分别计算输入的俯仰角和侧倾角为

为了稳定偏航角跟踪误差的零平衡，应用比例控制器来计算所需的偏航率输入：

2.5 硬件接口

框架中的最后一个模块是硬件接口，如图5所示。在收到控制输入后，接口模块将通过硬件连接将控制命令发送到大疆无人机。

图5 框架中的硬件接口Fig.5 Hardware interface of the framework

大疆经纬100（M100）无人机用作运动平台，因特尔D435i 设备用作单目相机以捕获图像信息，并且在飞机上安装了具有Ubuntu 16.04 的因特尔NUC（8i7BEH）以发送控制指令。作为地面站，个人计算机还配备有Ubuntu 16.04，可通过安全外壳协议（SSH）远程终端实时掌握机载计算机的状态。USBTTL 和UART 电缆都是M100 和因特尔NUC 之间的通信通道，但是远程终端相对独立于板载控制。在基于ROS（Kinetic）的Ubuntu 16.04 上运行的大疆SDK（OSDK 3.9）是用于M100 开发的必不可少的工具包。

框架中的所有模块都以ROS 节点的形式执行通信，这些节点是Apriltag 节点、ORB-SLAM2 节点、姿态转换节点、深度节点和大疆SDK 节点。节点关系如图6所示。

启动系统后，Apriltag 节点和ORB-SLAM 节点均立即开始工作。通过深度节点，利用接收到的上述2 个节点的信息输入，获得单目相机的深度信息。对于比例恢复的姿态，UAV 的状态信息是通过姿态转换节点获得的。大疆SDK 节点一旦接收到相应的姿态信息，便向UAV 发送控制信号。

在大疆SDK 节点中，操作流程如图7所示。在初始化过程中，需要启用4 个电机并掌握控制权限，以便机载计算机可以操纵四旋翼。电动机启动后，需要以50 Hz 的频率向无人机发送控制命令，以进行稳定的飞行控制。在无人机着陆后释放控制权之前，应先禁用电动机。

图6 ROS 中的节点关系Fig.6 Node diagram in ROS

图7 大疆SDK 节点中的运行流程Fig.7 Operating flow chart of DJI SDK node

3 实验结果

在本节中，将提供实验结果，以基于具有深度恢复能力的单目SLAM 来验证所提出框架的有效性。实验场景如图8（a）所示，实验平台为大疆 M100无人机，配备了因特尔NUC 机载计算机和因特尔实感单目相机，如图8（b）所示，图8（c）为在执行轨迹跟踪任务期间捕获图像的特征点信息。此外，在https：//github.com/TGUMobileVision/MonocularUAV上提供了有关框架的代码。

图8 实验设备Fig.8 Experimental setup

D435i 摄像机的校准参数为fx：615.9；fy：616.1；cx：322.4；cy：240.4。外在参数测量为

此外，Kp=diag（［0.09，0.09，0.1］），ky=2.8，r=1.0，w=0.0042，Kv=diag（［0.09，0.09，0.12］）。标签的规格和位置为｛id：1，size：0.143，x：1.8100，y：0.0000，z：0.0，qw：0.707，qx：0.0，qy：0.0，qz：-0.707｝。

在实验中，无人机分别在执行轨迹跟踪任务之前和之后起飞和降落。在轨迹跟踪任务中，无人机的运动轨迹如图9所示，姿态演变如图10所示，可以看出当前位置和所需位置具有很好的一致性。

在Fc0坐标系下表示的横摆角、俯仰角和偏航角的演变如图11所示，而推力f 的输出如图12所示。

实验结果表明，无人机在深度恢复后可以通过单目ORB-SLAM2 算法成功完成轨迹跟踪任务，所提出的框架可以实时执行高层任务。

图9 无人机在跟踪过程中的运动轨迹Fig.9 Motion trajectories of the UAV during the tracking process

图10 无人机在跟踪过程中的位置演变Fig.10 Position evolution of the UAV during the tracking process

图11 无人机在整个过程中的姿态演变Fig.11 Attitude evolution of the UAV during the entire process

图12 整个过程中无人机推力的演变Fig.12 Evolution of UAV thrust during the entire process

4 结语

本文通过利用具有深度恢复的单目状态估计，为开源无人机设计了可扩展框架。在计算单目SLAM 的深度信息后，无人机使用状态估计提供的姿态信息成功完成轨迹跟踪和自主着陆任务。在提出的框架中，深度恢复和轨迹跟踪任务在无人机飞行过程中自动完成。将来我们会努力将附加传感器的优势（例如IMU 和LiDAR）带入该框架，以减少单目SLAM 中的比例尺漂移并提高姿态估计精度。