基于复合标志的四旋翼无人机自主降落系统*

袁泽慧，陈国栋，张 亚，王忠言

（1.中北大学机电工程学院，山西 太原 030051；2.江苏省机器人技术及智能制造装备工程实验室（苏州大学），江苏 苏州 215021）

0 引 言

无人机能否在规定空间内快速、精确降落在地面充电装置或回收装置上，是实现空地机器人协同的关键，同时也是无人机最具挑战性的任务之一。目前最常用的方法是利用高精度的全球定位系统（global positioning system，GPS）和惯性传感器［1］、运动捕捉系统［2，3］、计算机视觉［4］来估计无人机和降落目标之间的相对位姿。但GPS 在室内、森林、城市等环境中信号较弱甚至失效［5］，这成为GPS 的致命缺陷。而运动捕捉系统一般仅用于室内环境中。相反，视觉传感器可以获得丰富的环境信息，有效避免了这些问题。同时，视觉传感器具有轻便、低功耗、低成本等优点，而且在近距离测量精度更高。因此视觉传感器成为无人机的必备传感器之一。

Yang S等人利用单目摄像头，结合比例校正并行追踪与绘制（parallel tracking and mapping，PTAM）技术，通过ORB（oriented FAST and rotated BRIEF）匹配算法，实现了无人机在任意纹理丰富的平台上降落［6］。Foster C等人则利用下视单目摄像头，结合惯性测量单元，建立以无人机为中心点的下方区域的概率二维高程地图，进而实现对安全降落点的检测［7］。这2种方法都实现了在非标志平台上的降落，但相较于标志平台的检测，鲁棒性较低，而且所需的降落时间更长。

H形图像常作为着陆标志在无人机降落中使用［8，9］，Ling K［10］、Borowczyk A 等人［11］在他们的工作中则采用了AprilTag二维码［12］作为着陆标志。但由于无人机高度的变化，单个的视觉标签经常会面临无法检测到的情况，如当无人机飞行高度较高时，标签无法被检测到，或者当无人机逐渐接近地面时，标签逐渐从无人机下方摄像头的视野内移出，从而导致无人机的降落精度降低。为了应对着陆目标“丢失”的情况，西北工业大学张咪等人提出了一种多层嵌套二维编码的阶层降落标识［13］，索文凯等人设计了一种多图形组合、多色彩导引、多方法识别的地面合作目标［14］。

本文基于大疆M210V2 四旋翼无人机，搭建了一种基于视觉的无人机在静止目标上的自主降落系统，其中摄像头采用低成本的单目摄像头。为了应对着陆目标丢失的情况，以及抵消单目摄像头导致的卷帘效应［15］和成像噪声等不利因素，基于AprilTag 视觉基准库设计了一复合着陆标志，使得无人机在着陆过程中的不同高度都能采集到有效的视觉信息。同时结合卡尔曼滤波器，对着陆目标进行实时跟踪。最后，在大疆专用模拟器中对所设计的自主降落系统进行了仿真，验证了所设计的自主降落系统的有效性和精确性。

1 无人机自主降落系统概述

本文设计的四旋翼无人机自主降落系统的总体框架如图1 所示。四旋翼无人机采用大疆M210V2 无人机，该无人机配备有大疆FlightAutonomy系统，以不同频率向外发布无人机的位置、姿态、速度以及加速度等信息；无人机底部安装有下视摄像头，其光轴垂直向下，主要用于对着陆标志的识别和检测；这两部分构成了系统的硬件部分，如图2所示。

图1 四旋翼无人机自主降落系统总体框架

图2 大疆M210V2 无人机及安装在底部的下视摄像头（框标注）

基于卡尔曼滤波器的状态估计模块主要对无人机flightAutonomy系统发布的状态信息，以及基于着陆标志检测算法测量得到的无人机和着陆标志间的相对位姿进行融合，实时对无人机和着陆目标间的运动状态信息进行估计。在此基础上，PID 控制器根据无人机和着陆目标间的相对位置，计算下一时刻无人机的期望加速度；根据文献［11］中建立无人机动力学模型的方法，将PID 控制器模块输出的期望加速度转换为无人机的姿态控制指令，通过无人机姿态的改变，来控制无人机飞行到期望位置。这4 个模块均由机载计算机来完成。各模块之间利用机器人操作系统（robot operation system，ROS）接口进行通信［16］。模块化的结构便于轻松修改或替换各模块内的算法，而无需对其他模块进行更改。因此，本文提出的是一种无人机自主降落的通用框架，可通过更改对应的模块来适应不同的算法或场景。

无人机自主降落系统中所定义的坐标系如图3 所示。世界｛W｝坐标系，采用东-北-天（east-north-up，ENU）坐标系表示，其坐标原点定义为无人机起飞时的位置；机体坐标系｛B｝，其原点固连在四旋翼无人机的重心上，X轴指向机头方向，Y轴指向左侧，Z轴垂直于XY轴向上。摄像头｛C｝坐标系的原点为相机光心，Z轴垂直于相机平面，沿着光轴方向，X轴和Y轴分别平行于图像平面，其中，X轴指向右侧，而Y轴由右手定则所确定。

图3 定义的坐标系

2 无人机着陆标志设计与检测

当无人机与降落目标距离较近时，利用视觉估计的无人机和降落目标间的相对位姿远比GPS精确。为了减小由于使用低成本单目摄像头带来的不利因素，设计了基于Apriltag视觉基准库的复合标签作为着陆标志，如图4所示。

图4 着陆标志

结合本项目中地面机器人的尺寸，着陆标志的总体大小为90 cm×90 cm，由5个不同大小的Apriltag标签组成，其中中间较小的标签为16 cm×16 cm，而周围4个标签的大小为28 cm×28 cm。复合标签的使用可以使得摄像机无论是在高空和低空都能探测到着陆标志，从而降低了着陆目标丢失的可能性。当无人机离着陆目标较远时，可以检测到较大的标签，当无人机在降落过程中不断接近着陆目标时，较大的标签逐渐移出摄像头的视野范围，但中间较小的标签仍然在其视野范围内，这样就保证了无人机无论是在较远处还是近处，着陆目标都位于摄像头的视角范围之内。

着陆目标检测算法设计中，本文使用了基于Apriltag2的ROS封装器［17］，与主体算法相接。下视摄像头将采集到的640 ×480的原图像Iraw输入到AprilTag2，并利用AprilTag2算法对着陆标志中的标签进行识别和标记。对于识别到的标签，将其ID 标记为i，假设至少有一个标签被检测到，利用OpenCV 中的PnP（perspective n point）算法估计得到着陆标志在下视摄像头坐标系中的位置和姿态。由于多个标签之间的相对位姿已知或可事先进行标定，在对着陆目标进行检测时，可提取所有标签的信息，以及标签间的位姿约束，来估计无人机和着陆目标时的相对位置和姿态。因此相对于单个标签，利用复合标签估计得到的相对位姿更加精确，从而有利于提高无人机的自主降落精度。

3 基于卡尔曼滤波器的状态估计

无人机在接近着陆目标的过程中，容易发生着陆目标“丢失”的现象［10］。因此设计卡尔曼滤波器来估计和跟踪无人机和着陆目标的运动状态。

3.1 运动方程

为方便将该模块日后移植到移动目标上的降落中，滤波器状态量包含了无人机和着陆目标的运动状态，状态量为其中分别为无人机在世界坐标系中的位置、速度和加速度，而则为着陆目标在世界坐标系中的位置、速度和加速度。显然，在本文中着陆目标的速度和加速度都为0。

运动方程定义如下

式中vt，at分别为速度和加速度。设无人机加速度（）T的一次微分为高斯白噪声，其功率谱密度矩阵为qwI3，其中qw为功率谱密度参数，I3为3 ×3的单位阵。对运动方程进行离散化，得到其离散形式xk＋1＝Fxk＋wk，其中

式中 Δt为k＋1时刻和k时刻之间的时间间隔，I3为3 ×3的单位矩阵，03为3 ×3 的零矩阵，⊗为两矩阵间的克罗内克积，而wk为高斯白噪声，其协方差矩阵定义为

3.2 测量方程

如前所述，本文中的测量数据主要来自于：1）无人机的FlightAutonomy系统；2）机载摄像头估计得到的着陆目标和无人机间的相对位置和姿态。当滤波器接收到任意一组数据时，滤波器对无人机的状态进行预测和更新。虽然测量数据来源不同，但本文中采用了统一的测量模型，即

式中vk为高斯白噪声，其协方差矩阵为Vk。对于不同的测量数据，其Hk和vk不同。

大疆M210V2 FlightAutonomy 系统融合了GPS、惯性测量单元等多传感器信息，可提供无人机在世界坐标系｛W｝下的位置、速度和加速度。所以其测量矩阵Hk＝［I909×9］。

当无人机离着陆目标较近时，机载下视摄像头可以测量着陆目标在相机坐标系下的位置。在本文中，下视摄像头固定安装在无人机底部，所以相机｛C｝坐标系和无人机｛B｝坐标系之间的变换关系可提前进行标定，进而可以得到。利用无人机FlightAutonomy 系统提供的当前无人机的姿态信息，结合矩阵变换关系得到其在世界坐标系下的表示，即，所以其测量矩阵Hk＝［I303×6-I303×6］。

4 控制系统设计

本文的控制目标是使无人机的位置无限接近着陆目标中心的位置，即使相对位置u＝-尽可能小。将无人机降落分为2 个阶段，首先调节水平方向上的误差u‖，当无人机与着陆目标中心点间距离小于一定阈值时，即｜u‖｜＜Δd时，控制无人机以一定的速度垂直且匀速下落，当无人机接触到地面后，关闭无人机，降落成功。

在水平方向上，采用标准的PID 控制器来计算目标加速度，，其中，kp，kd，ki为控制器的固定增益。因为只对水平方向无人机的位置进行控制，因此加速度a在z轴方向上分量为0。根据文献［11］中建立无人机动力学模型的方法，将期望加速度转换为无人机的姿态输入角度指令，从而使得无人机到达目标位置。为保证无人机的平稳飞行，根据无人机的飞行参数限制，在大疆无人机专用模拟器上对kp，kd，ki进行调节。

5 自主降落系统仿真

为了验证提出的四旋翼无人机自主降落控制系统，使用大疆专用的硬件在环（hardware in the loop，HITL）模拟器对其进行仿真验证，如图5 所示，其中图5（a）为模拟器控制面板，可以设置无人机起飞初始位置、风速等环境信息，图5（b）为无人机飞行界面。

图5 大疆无人机模拟器

四旋翼无人机自主降落系统的飞行过程如图6 所示。为飞行安全起见，在无人机起飞前确定电池电量充足，以及GPS信号是否能正常接收，如满足条件，将当前无人机所在位置设置为世界坐标系｛W｝的原点，无人机执行起飞动作，并飞行到预设的目标点。当到达目标点后，启动基于卡尔曼滤波的状态估计器和AprilTag 算法对着陆目标进行检测。当检测到着陆目标标志后，无人机定高飞行，在水平方向上接近着陆目标。反之如果没有检测到着陆目标标志，对着陆目标进行网格化搜索直到检测到着陆目标。当无人机与着陆目标中心点水平误差小于预设的阈值后，本文中为0.08 m，无人机在垂直方向上以一定的速度匀速降落，无人机到达地面后，关掉无人机，自主降落完成。

图6 无人机自主降落过程

在模拟实验中，为最大限度接近真实飞行环境，将模拟环境设置为有风环境，其中东风6 m／s，北风6 m／s，对应4级风速。无人机从不同高度、不同位置接近着陆目标，共进行了100次硬件在环模拟仿真实验。结果如图7 所示。从图7（a）中可以看出，无人机最终都降落在了着陆目标中心点附近，进一步根据图7（b）可知，降落误差均位于以着陆目标为圆心，0.08 m为半径的圆内，该误差值与预设的水平方向误差阈值相同。同时在仿真中发现无人机在垂直下落过程中水平误差并没有超过阈值，说明无人机在垂直下落过程中，得益于大疆无人机控制系统的稳定性能，其在水平方向的误差并没有增大。降落误差进一步统计如表1 所示。从表1中可以看出，无人机在X和Y方向的平均降落误差均小于5 cm，且最大误差不超过8 cm，实现了高精度的自主降落。

表1 无人机降落误差统计cm

图7 无人机自主降落和XY平面着陆点分布

6 结束语

针对小型无人机的高精度自主降落问题，在大疆M210V2基础上，搭建了一种基于视觉的无人机自主降落系统，其中机载摄像头采用了低成本的单目摄像头。为应对使用低成本摄像头带来的不利因素，设计了基于AprilTag的多标签着陆标志，并建立基于卡尔曼滤波器的状态估计模型对无人机和着陆目标的运动状态进行实时预测和跟踪。在此基础上，通过PID 控制器计算无人机下一时刻的期望加速度，并将该加速度转换为无人机的姿态指令。在大疆专用模拟器下，对该系统进行了100 次硬件在环模拟仿真，仿真结果表明在4级风速的模拟环境中，无人机在X和Y方向的平均降落误差均小于5 cm，最大误差不超过8 cm。在下一步工作中，将进行无人机在户外自主降落的实验，进一步验证该系统的可行性和精确性，从而为空地协同系统的实现提供基础。