一种小型四轴飞行器目标跟踪控制算法

顾兆军，陈 辉，，王家亮，高 冰，

(1.中国民航大学 信息安全测评中心，天津 300300；2.中国民航大学 计算机科学与技术学院，天津 300300)

小型无人机是一种典型的人工智能空中机器人。其具有成本低、体积小、结构简单、可对目标进行移动监控等特点，可以有效解决视觉场景“看不清”问题，所以在智慧监控、物流、农业植保、侦查、救援、编队飞行[1-3]等方面都有着广泛的应用。最常用的小型无人机是四轴飞行器。近年来，国内外许多高校以及研究机构针对四轴飞行器均开展很多研究工作，如目标跟踪[4-8]、目标检测[9-10]、路径规划[11]、稳态飞行控制[12]、避障[12-13]、姿态调整[14]、无人机网络资源优化[15]等方面，并且已经取得显著的技术成果。其中，目标检测与跟踪是小型无人机的重要任务之一，为了能够持续跟踪目标、获取目标信息，无人机执行任务的实时性和飞行稳定性是两个主要指标。

四轴飞行器的目标检测与跟踪是指接收无人机传回的图片帧，处理后标记出识别出的物体类别，并跟踪选定目标飞行。现有的目标检测算法主要包括one-stage和two-stage两种。two-stage检测算法一般分两步进行，先获取候选区域再进行分类，比如循环卷积神经网络(Region-based CNN，R-CNN)[16]系列目标检测算法；而one-stage则一步到位，不用再单独获取候选区域，常见的有SSD[17]、YOLO[18]等。现有的基于计算机视觉的目标跟踪算法一般可以分为4种：基于目标模型建模的方法、基于搜索的方法、基于相关滤波的跟踪算法和深度学习方法。前两种属于早期的目标跟踪算法，其没有将背景信息考虑在内，在目标运动或被遮挡后容易跟踪失败，而且其跟踪算法执行速度慢，无法满足实时性要求。随着深度学习的广泛应用，深度学习方法被应用到目标跟踪中，但在目标跟踪过程中，正样本只有初始帧中的目标，即难以提供大量的高质量数据去训练一个性能良好的模型，所以深度学习方法应用在实时目标有部分缺陷。而一些基于相关滤波的跟踪算法，处理速度可以达到数百帧每秒，可以广泛应用于实时跟踪系统中，如核相关滤波法(Kernel Corrolation Filter，KCF)[19]、多功能自适应核相关滤波[20]、目标跟踪评估算法[21]等。

在以前的文章中，已经探索了许多基于视觉的无人机目标跟踪系统。文献[4]等提出了一种基于颜色过滤算法和形状过滤算法的四轴飞行器目标跟踪控制系统，可以根据被跟踪目标的颜色和形状特征控制飞行器执行跟踪任务。文献[5]等提出一种基于PID和模糊控制算法结合的四轴飞行器运动控制方法，可根据opencv识别和跟踪目标颜色，并在AR.Drone.2.0四轴飞行器上验证了结果。文献[6]提出了两种基于DSST和KCF的跟踪算法，使用无模型、比例自适应的方式来估计与被跟踪目标的距离。在用于四轴飞行器跟踪任务时，可以选择人作为跟踪目标，而不再根据颜色或者形状进行区分。这些研究的共同点是，由于四轴飞行器机载的处理系统效率有限，他们都是接收单目摄像头传回的视频流，并在地面站进行图像处理。其在视频流处理过程时采用逐帧处理的方式，而在执行跟踪任务时，对跟踪结果影响不大的图片帧不必要占用处理时间，所以这种方式浪费了大量的计算资源。故文中提出一种基于注意力模型的图片帧预处理算法，针对单目摄像头采集的视频图像信息，使用注意力模型动态地过滤到部分对跟踪结果影响不大的部分帧，通过反馈控制及时根据误差估计调整四轴飞行器姿态，旨在实现四轴飞行器高实时性目标检测与跟踪。

1 四轴飞行器动力学模型

四轴飞行器机架呈交叉状，有4个轴臂，分别安装有4个螺旋桨和电机。从结构上，四轴飞行器可以分为“十”形和“×”形两种机型。从本质上讲这两种机型无明显区别，笔者主要针对“×”形进行研究。

在对飞行器进行姿态估计时，一般会用到机体坐标系和本地坐标系，如图1所示。机体坐标系原点在四轴飞行器重心位置，x轴为四轴飞行器前进方向，z轴垂直x轴向下，y轴右手定则确定。本地坐标系能够确定无人机相对于地面的位置，是以飞行器的初始位置为原点，在水平面正北方向为x轴正方向，正东方向为y轴正方向，垂直地面向下为z轴正方向。规定四轴飞行器绕z、y、x轴旋转分别得到偏航角y(ψ)、俯仰角p(θ)、滚动角r(φ)。通过机体坐标系向本地坐标系的坐标变换，可以将数据运用到各项计算，其转换矩阵如下：

图1 四轴飞行器坐标系示意图

(1)

如图2所示，根据四轴飞行器结构的几何关系对螺旋桨的拉力进行分析。设每个螺旋桨的拉力均为f，飞行器俯仰时螺旋桨拉力的变化量为Δfp，飞行器滚动时螺旋桨拉力的变化量为Δfr，飞行器偏航时螺旋桨拉力的变化量为Δfy。则可以得出每一个螺旋桨的拉力：

图2 四轴飞行器拉力分解示意图

(2)

(3)

(4)

(5)

四轴飞行器螺旋桨的转速的变化可以改变螺旋桨的拉力，从而控制飞行器执行前进、后退、向左、向右等飞行动作。

2 基于特洛平台的目标检测与跟踪系统设计与实现

在进行目标检测与跟踪时，系统终端处理图片帧的实时性和飞行器控制的稳定性是需要重点考虑的两个方面。在图片帧预处理阶段，传统方法对飞行器传回的图片帧逐一处理，这样在准确性上确实效果较佳，但消耗大量时间。针对飞行器实时性要远大于准确性的要求，文中提出一种基于注意力模型的图片帧预处理方法，选出对目标跟踪处理贡献度较高的部分帧，配合基于像素点的反馈控制算法，旨在提高飞行器目标检测与跟踪系统的实时性和飞行器控制的稳定性。

目标检测与跟踪系统结构如图3所示，系统运行流程设计如下：

图3 系统结构图

(1)系统初始化。系统终端与四轴飞行器建立连接，并通过用户数据报协议协议(User Datagram Protocol，UDP)控制飞行器起飞，同时系统终端接收实时传回的视频流并按视频帧的方式读取。

(2)计算选框位置偏差。将图片帧送入目标检测与跟踪模型，得到被跟踪目标选框在视频帧中位置偏差。

(3)注意力模型。对后续帧运用注意力模型挑选贡献度较高的部分帧进行处理。

(4)飞行器反馈控制。根据反馈控制将选框位置偏差值转换为相应的四轴飞行器电机转速变化。

(5)飞行器姿态调整。将变化后的电机转速值发送给无人机，使其及时作出姿态调整。

2.1 基于注意力模型的图片帧处理

2.1.1 注意力模型

对四轴飞行器传回的视频流，如果每一帧都送入目标检测和跟踪算法，则会消耗大量的资源，进而降低四轴飞行器跟踪的实时性。若先对图片帧进行判别，则选择对跟踪结果贡献度较大，即对结果的影响较高的某些帧，这样动态地选取部分图片帧进行处理，可有效缩短四轴飞行器采集图片帧后预处理时间。

注意力模型又称为注意力机制。在计算能力有限的情况下，注意力模型作为一种资源分配方案，将有限的计算资源用来处理更重要的信息，因此将注意力模型的思想运用到对视频帧的预处理中。首先，在所有的输入信息上计算注意力分布；然后，再根据注意力分布来计算输入信息的加权平均。注意力模型的核心在于计算注意力分布，用打分函数来计算每个输入向量和查询向量之间的相关性，其中查询向量是一个和任务相关的表示。给定一个和任务相关的查询向量q，注意力变量z∈[1，N]表示被选择信息的索引位置，X=[x1，…，xN]∈RD×N表示N组输入信息。则计算在给定q和X下，选择第i个输入向量的概率αn：

(6)

其中，αn称为注意力分布，s(x，q)为注意力打分函数。

2.1.2 运用注意力模型的图片帧预处理

如图4所示，基于注意力机制的原理，当开始目标跟踪任务后，得到第1帧图片的分析结果时，则会对该图片帧进行打分，从而决策在后续的第i+1帧继续处理(i为s对应结果)。

图4 注意力模型的图片帧预处理

2.2 基于反馈控制算法的四轴飞行器控制

四轴飞行器传回的图片帧，经过注意力模型的预处理后再次送入YOLO+KCF模型；如果跟踪目标发生位移，则在图片帧上会显示为选框位置和面积的变化，因此可将飞行器飞行控制简化为采用基于PID控制的图片中心位置像素点偏差自适应反馈控制和基于像素面积偏差反馈的四轴飞行器位置控制，分别控制四轴飞行器的左右偏航和前后位移。该四轴飞行器控制算法情景设计如下。

情景1当选框中心点相对图片帧中心点左偏移时，超过设定阈值，表明目标即将从左侧逃离跟踪。此时，飞行器应向左偏航。

情景2当选框中心点相对图片帧中心点右偏移时，超过设定阈值，表明目标即将从右侧逃离跟踪。此时，飞行器应向右偏航。

情景3当选框像素面积变大时，超过设定阈值，表明目标与飞行器距离较近。此时，飞行器应向后退。

情景4当选框像素面积变小时，超过设定阈值，表明目标与飞行器距离较远。此时，飞行器应向前进。

2.2.1 基于PID控制的图片中心位置像素点偏差自适应反馈控制

PID[22]控制目前在工业控制系统中应用较广泛，它是一个典型的单位负反馈系统，系统由PID控制器和被控对象组成。其中PID分别为比例、积分、微分(Proportional、Integral、Differential)。PID控制的核心就是计算出确定的给定值与实际的输出值之间的偏差e(t)，然后根据比例—积分—微分的线性组合关系计算出控制量u(t)，再通过改变控制量来控制被控对象。PID控制可连续地表示为

(7)

但在实际应用中，计算机采样无法做到真正的连续，所以将PID进行离散化处理，分为增量式PID和位置式PID。增量式PID控制用数学表示为

u(t)=u(t)+Kp[e(k)-e(k-1)]+Kie(k)+Kd[e(k)-2e(k-1)+e(k-2)] 。

(8)

采用增量式PID对飞行器电机转速进行闭环控制，通过计算被跟踪目标的选框像素中心点与当前图片帧像素中心点的水平偏差e(k)，并将该偏差作为误差参数，通过PID控制计算公式得到四轴飞行器此刻的偏航转速u(t)为speed；使用send_rc_control(0，fb，0，speed)函数向四轴飞行器发送指令，控制飞行器左右偏航。

在四轴飞行器执行目标跟踪任务时，PID参数初始化为[0.4，0.4，0.1]；e(k)、e(k-1)和e(k-2)分别代表本次误差、上次误差和上上次误差，误差初始值默认为0，偏航控制系统工作流程如图5所示。如图5(a)所示，目标的中心与图片帧中心有偏差，计算此数值e(k)作为输入参数，通过增量PID公式计算出speed，使无人机向右偏转，同时更新误差e(k-1)=e(k)。

图5 偏航控制系统工作流程

2.2.2 基于像素面积偏差反馈的四轴飞行器位置控制算法

在执行目标跟踪任务时，需要使四轴飞行器和被跟踪目标之间保持安全距离。对距离的判断采用一种基于选框内像素点所占面积的方法。首先设定一个面积区间A[a，b]。当被跟踪目标远离四轴飞行器并且选框面积大于b时，飞行器会向前移动；当被跟踪目标靠近四轴飞行器并且面积小于a时，飞行器会向后移动，文中使用的面积区间A[60 000，68 000]，从四轴飞行器传回的视频流经cv2.resize()函数处理，统一分辨率为960×720在终端显示，前后位移控制如图6所示。计算选框面积时，使用数组boundingbox[x，y，w，h]接收KCF跟踪器在当前图片帧的跟踪结果。(x，y)表示选框的左上角坐标，w、h分别代表选框的长、宽，则选框的面积为A=w×h。

图6 位移控制

3 实验结果与分析

3.1 软硬件平台介绍

四轴飞行器目标检测与跟踪研究在笔者电脑中进行，电脑配置Intel(R)Core(TM)i5-7200U处理器，NVIDIA GeForce 940MX显卡。开发环境采用python 3.7、CUDA 10.0、cuDNN 7.6、OpenCV 3.4。使用的硬件平台是四轴飞行器Tello，如图7所示。Tello是深圳睿炽科技生产的小型航拍无人机，结合了DJI大疆飞控技术和英特尔芯片，具备红外定高、气压计定高、下视视觉等功能，支持一键抛飞、全向翻滚，并且携带500万像素摄像头，可以进行720P高清图传。

图7 四轴飞行器Tello

3.2 系统框架(YOLOV4-tiny + KCF)介绍

YOLO[15](You Only Look Once)是一种基于深度神经网络的对象识别和定位算法。其采用端到端的设计思想，将目标定位和目标识别这两个阶段合二为一，对同一图片只需扫描一次。该算法目标识别的速度可以达到实时检测的要求。基于对四旋翼无人机目标检测的实时性考虑，飞行器回传视频图像的目标识别处理对速度的需求大于识别精度的需求。文中使用的是YOLOV4-tiny版本，其于2020年6月正式发布，是YOLOV4的轻量型版本，具有更快的识别速度。

KCF[16]是一类基于模板匹配的可视物体跟踪方法，它把标识目标的矩形看作一个模板，用于搜寻物体在下一个视图中的位置，其在计算模板时采用基于傅里叶变换的计算方法，显著提高了在下一帧中识别匹配窗口的速度。

采用YOLOV4-tiny和KCF结合的方式，实现四轴飞行器目标检测与跟踪，基于KCF的跟踪算法可以在传回的图片帧中框选跟踪目标(即使该目标没有被YOLOV4-tiny目标检测算法精准识别)。此处理方式，极大可能地弥补了YOLOV4-tiny目标检测算法因轻量化导致的识别度略低的缺陷。结合飞行器的YOLO+KCF算法实现如图8所示，通过加载数据文件、权重文件以及配置文件利用GPU对传入的图片帧进行多次卷积和池化处理，随后经过全连接层和输出层。最后将经过目标检测处理的图片帧加载KCF跟踪器，并将最终结果显示在系统终端。

图8 图片帧通过YOLO+KCF算法

3.3 实验结果与分析

为了验证笔者提出的基于注意力模型的图像预处理，以及基于反馈控制的无人机姿态调整，布置如下实验环境。

3.3.1 目标检测测试

为了减少因无人机执行目标检测任务时飞行速度带来的精度下降，设计如下对比实验。因为不同的打杆杆量对应无人机在6个自由度的不同速度，分别选取40%、60%、80%的打杆杆量(对应3组不同飞行速度实验)，因此可以根据检测模型在无人机不同打杆杆量下进行实验，确定小型四轴飞行器对目标检测的最佳速度。

四轴飞行器起飞后进入目标检测阶段，其飞行实景以及摄像头采集到的图片经过处理后的结果如图9、10、11所示，处理后的结果会在系统终端显示，其中多个目标已经被矩形框框住，并在矩形框的上方标记类别。图片帧左上角显示检测出的类别及其概率，左下角为此时四轴飞行器电池电量和姿态角。分析实验结果：当杆量设置为40%时，飞行器在偏航测试中目标识别率约为97%，在位移测试中目标识别率约为95%，都具有较好的识别精度，但该状态下飞行器姿态调整速度较慢，会影响其执行任务的实时性；当杆量设置为60%时，飞行器在偏航测试中目标识别率约为94%，在位移测试中目标识别率约为95%，具有较好的识别精度，同时飞行器也能及时完成姿态调整；当杆量设置为80%时，飞行器在位移测试中目标识别率约为90%，但在偏航时因飞行器偏航速度较快，目标识别率仅约为80%。综上实验结果，在杆量设置为60%时，对目标识别的精度和飞行器姿态调整的速度都有较好的效果，因此设置杆量60%进行后续目标检测与跟踪控制实验。

图9 打杆杆量设置为40%(实验1)

图10 打杆杆量设置为60%(实验2)

3.3.2 目标跟踪与控制测试

为了验证目标跟踪的实时性和稳定性，设计实验场景：工作人员从办公室内的办公桌旁出发，打开门，途经走廊到达会议室，飞行器实时调整姿态角和前后距离跟踪工作人员飞行。

四轴飞行器起飞后，先进入目标检测阶段，其拍摄到的图片以及经过处理后的图片如图12所示。其中图12(a)为飞行器在执行飞行实验时的实景；图12(b)为系统终端显示的目标检测结果，图片帧中多个目标已经被矩形框框住，并在矩形框的上方标记类别。

在四轴飞行器飞行状态下，自定义选择目标，如图13所示，图13(a)为飞行器与被跟踪目标的初始位置，图13(b)为在系统终端选中被跟踪目标后的显示结果。可以看出，与前面目标检测不同的是：目标跟踪时，图片帧右上角实时显示此刻飞行器应做出的飞行调整；左下角显示了此刻选框的中心点坐标和选框的面积，便于调整反馈控制时的参数。随后，被跟踪目标发生位移，发生位移时飞行器飞行实景和系统终端的处理结果，如图14所示。

图13 目标跟踪初始状态

图14 四轴飞行器目标跟踪

分析四轴飞行器实验中的图片资料，位移1、位移2和位移8被跟踪目标正面对着飞行器，其他位移状态被跟踪目标背面对着飞行器，并且伴随着被跟踪目标的不断移动。目标跟踪算法能够在跟踪过程中及时调整选框位置和选框大小，飞行器能够及时作出姿态调整。飞行过程中部分关键点姿态角变化如图15所示，图中标记出几处拐点，表示此时飞行器正在调整偏航角使得飞行器正对被跟踪目标，其偏航角调整角度在[-60°，70°]区间内并未发生反复左右震荡偏航现象。

图15 姿态角变化

图16中圆形连接的折线为被跟踪目标运动轨迹，三角形连接的折线为四轴飞行器飞行轨迹。根据轨迹点所示，确定被跟踪目标后，四轴飞行器向目标飞行，调整与目标之间的距离。在目标移动时，其在经过沙发和左侧门两处时距离偏差较大。其中，在沙发处飞行器和被跟踪目标距离较远，导致飞行器跟踪目标过门时出现飞行器未飞出办公室的情况，经目标缓慢移动调整与飞行器距离后，飞行器才成功过门。出现这种情况，一方面是因为飞行器目标跟踪算法的精度问题，在处理过大量视频帧后，选框的大小判断出现偏差，导致飞行器与目标距离误差较大；另一方面是由于像素面积偏差反馈的四轴飞行器位置控制算法中面积参数范围较大。

为了量化图16中飞行器轨迹与被跟踪目标运动轨迹之间的偏离，考虑到飞行器在执行跟踪任务时，被跟踪目标移动距离未超过设定的飞行器有效跟踪面积区间，此时不必调整飞行器距离只需适当调整偏航角。因此，采用距离偏差来评价各组采样点(t时刻飞行器位置与被跟踪目标位置)的偏离程度，用方差来评价所有采样点的整体偏离情况。通过测量实验中各组采样点的距离，可得飞行器与被跟踪目标距离偏差最大值约为35.3 cm，最小值近似为0 cm(由于采用的位移跟踪控制是一个面积区间，因此实验时大部分样本点都在此区间内)，平均值约为3.33 cm。通过测量的距离偏差，计算得所有采样点的方差值为69.15。实验结果表明，飞行器飞行轨迹与被跟踪目标运动轨迹之间偏差较小，执行跟踪任务时大部分轨迹点处于设定的安全跟踪距离范围内。因此，笔者提出的算法在目标跟踪的实时性、稳定性上具有较好的效果。

图16 飞行轨迹与被跟踪目标轨迹

为了进一步验证算法实时性，在实验过程中，随机抽取20 s的视频流。因飞行器每秒传输30帧图片，则20 s共传输图片600帧，利用文中提出的注意力模型进行图片预处理后，随机抽取的600帧中只处理了23帧，处理时间约为0.327 5 s。若未使用注意力模型，每1帧都处理，共需要约8.772 s。在实际飞行过程中，随着图片帧的增多，预处理阶段带来的效果是很明显的，图17为系统处理一帧图片所需要的时间。表1为在实验环境下，基于注意力模型的图片帧预处理算法与其他目标检测跟踪算法在接收到相同图片帧时，处理图片的时间对比。表2为文中模型随着接收帧数的增加，所处理的图片帧数量的变化。

表1 接收50个图片帧处理效率对比

表2 文中模型处理图片帧效率

图17 图片帧处理时间

可见，基于注意力模型的图像预处理可有效节省图像帧处理时间，节约大量计算资源，从而提升了四轴飞行器的目标跟踪实时性，为四轴飞行器的实时跟踪飞行应用提供了保障。

4 结束语

基于四轴飞行器单目摄像头采集到的视觉信息，将视频流分解为图片帧。针对图片帧处理速度和四轴飞行器姿态控制两方面，分别提出基于注意力模型的图片预处理和基于反馈控制的四轴飞行器姿态调整。实验结果表明，文中提出的方法在执行目标监测与跟踪任务时，具有较好的实时性和稳定性效果。利用注意力模型的图片预处理，可以合理地舍弃部分对跟踪结果影响不大的图片帧，节省大量计算资源；基于反馈控制的四轴飞行器姿态调整，使四轴飞行器能够及时作出偏航和进退调整。下一步将考虑进一步结合深度学习框架，优化目标跟踪功能，使四轴飞行器能自主选择跟踪目标，并完成跟踪任务。