基于视觉的无人机定点飞行控制技术研究

陈崟松，吴 芬，吴 妍

（1.南京理工大学教育实验学院，江苏南京 210094；2.南京理工大学自动化院，江苏南京 210094）

无人机（Unmanned Aerial Vehicles，UAV）是利用无线电遥控和自备的程序控制装置操纵的不载人飞机[1]。目前无人机主要可分为固定翼无人机，旋翼无人机以及扑翼式无人机[2]。而旋翼无人机因其机动性好，起落受限制少，能适应多种复杂环境[3]，而被广泛运用于军事侦察、目标跟踪、地形勘探、输电线路维护[4]等领域，而四旋翼无人机则是其中最流行的一种。

无人机的导航通常采用惯性导航系统[5]（Inertial Navigation System，INS）和全球定位系统（Global Position System，GPS），但是导航过程中惯性器件具有积累误差，对初始值过于敏感的缺点，而GPS又受环境约束，在室内和复杂环境下会难以获取信号，同时精度也往往满足不了无人机的要求。

近年来，随着计算机视觉的飞速发展，使运用视频图像处理的方法实现无人机导航成为可能。首先，视觉传感器具有价格便宜、采集信息丰富的优点。同时，视觉传感器可以与惯性导航和GPS结合，提升导航精度[6]。此外，相比于无线电与GPS，视觉导引抗干扰性能更好[7]。

在无人机视觉方面，世界很多研究机构已经做出很多尝试。卡耐基梅隆大学的研究人员使用双目视觉、模板匹配、特征跟踪的方法实现四旋翼无人机目标识别与跟踪[8]。近年来国内也出现了基于视觉的无人机飞行控制研究成果。王翔鹏[9]提出了在GPS失效情况下农用无人机视觉导航算法，主要利用田间直线等信息进行无人机被动定位。

文中从硬件选择，图像处理，飞行控制3个方面介绍基于视觉的定点飞行控制技术。

1 硬件平台搭建

基于视觉的四旋翼无人机定点飞行系统由无人机硬件平台以及PC（Personal Compute，个人计算机）上位机两部分组成，系统总体硬件组成及通信结构如图1所示。为了减小无人机负载，方便我们对算法执行过程进行监测，我们将视频图像处理工作交给PC机处理，为此将摄像机获得的视频信息通过无线图像传输设备发送给PC机，当PC机完成视觉处理后将偏差信息通过无线数据传输设备发送给无人机，作为导航指令输入飞行控制器。

图1 系统结构图

1.1 无人机硬件平台

我们选用了Pixhawk飞控板，Gopro Hero4运动摄像机，XL-MaxSonar-EZ4声呐，XBEE pro无线数传，大疆公司的5.8G图传以及无人机自稳云台等器件搭建无人机硬件平台。理由如下：

1）Pixhawk飞控板配有MPU6000三轴加速度计/陀螺仪、LSM303D磁力计和MS5611气压计，性能优越，同时具备进行二次开发便捷的优点。

2）XL-MaxSonar-EZ4声呐，其内嵌了距离读数实时噪声抑制算法，即便在电气噪声源存在的环境下依然能够正常工作，能准确采集无人机实时高度。

3）无线图传与数传用于无人机与PC之间的数据与图像传输交互，我们选用的XBEE pro无线数传与大疆公司的5.8G图传都具有实时稳定且小巧坚固的优点。

4）自稳云台则用于使得摄像机的镜头始终竖直向下，隔离了视觉信息采集与四旋翼无人机姿态抖动，方便之后的图像处理。

5）Gopro Hero4摄像机可以在2.7 K和1 080 P的画质下连续拍摄1小时40分钟左右，充分满足了我们的需求。图2为搭建完成的四旋翼无人机。

图2 四旋翼无人机

1.2 PC上位机

PC机采用Intel Core i3-2310M处理器，工作频率为2.1 GHz，内存为4 G，安装32位操作系统，使用Samsung SSD固态硬盘，处理器性能优越，图像处理速度快。

2 图像处理

为了实现对地面标识点的跟踪，我们要对摄像机采集到的图像进行处理，主要分为3个步骤：1）图像预处理；2）目标跟踪；3）上位机设计。

2.1 图像预处理

在无人机飞行过程中由于机体的机械振动和无线传输模块导致的噪声会对摄像头采集的图像造成干扰，因此对图像进行预处理是很重要的。本文中我们采取了中值滤波[10]的方法对原始图像进行处理，将每一像素点的灰度值设置为该点某邻域窗口内的所有像素点灰度值的中值，从而消除孤立噪声点，同时又可以保持图像的边缘特性[11]。

2.2 目标跟踪算法

本文采用基于卡尔曼滤波的Camshift（Continuously Adaptively Mean Shift，连续自适应均值偏移）算法[12]，即对于当前一帧而言，已知当前目标物重心的位置，利用卡尔曼滤波预测下一帧目标物的位置，并与Camshift算法解算出的位置进行最优估计，得到最优的目标位置实现最优跟踪。

2.2.1 Camshift算法组成

Camshift算法[13]可以分为3个部分：反向投影计算、Mean Shift算法和Camshift算法。算法流程图如图3所示，虚线框内为Mean shift算法。

图3 Camshift算法流程图

2.2.2 反向投影

RGB颜色空间对亮度变化较为敏感，为了降低此变化对目标跟踪效果的影响，首先将图像从RGB空间转换到HSV空间（Hue，Saturation，Value色调，饱和度，亮度）。然后对其中的H分量进行直方图统计得到H分量的直方图，再对图像中每个像素的值用其颜色出现的概率对替换，就获得了颜色概率分布图，即反向投影图。原图像中每一个像素值反映的是在这一点的颜色信息，在反向投影后，图像中每一个像素值转换成了该颜色信息出现在此处的一种离散化概率度量，为之后的跟踪提供了线索。

RGB颜色空间模型与HSV颜色空间模型转化公式如下：

1）计算max=max（R，G，B），min=min（R，G，B）；

2）如果R=max；H=（G-B）/（max-min）；如果G=max；H=2+（B+R）/（max-min）；如果B=max，H=4+（R-G）/（max-min）；

3）将上一步中计算得到的H乘以60得到角度，若此时H小于0，则H=H+360；

4）V=max（R，G，B），S=（max-min）/max。

2.2.3 Mean Shift算法

Mean Shift算法（图3中虚线框部分）在已知目标图像的反向投影和初始搜索窗口位置的情况下，用迭代法寻找跟踪目标的质心，当搜索窗口中心的移动距离小于固定阈值或者循环达到某一个固定次数，停止迭代。

设I（x，y）为反向投影图中（x，y）处的像素值，x与y的变化范围即搜索窗的范围。

零阶距：

一阶距：

搜索窗口质心：

2.2.4 Camshift算法实现

Camshift算法的基本原理是在Mean Shift上实现了搜索窗口的自适应，即在每次进行搜索之前，根据当前锁定目标的形状与尺寸确定下一帧窗口的尺寸。

对于当前图像，若以椭圆形状锁定此时跟踪的目标，则计算锁定窗口的二阶距：

，经过质心且相互垂直的长轴l与短轴w如下所示：

根据上式，在每次进行搜索前都自适应调整搜索窗口的窗口，从而解决由于目标物发生外形变化而导致跟不上的情况。

2.2.5 卡尔曼滤波

卡尔曼滤波是一种以均方误差最小作为估计时的准则从而得到一套递推估计的滤波方法。其基本思想是：建立信号与噪声的状态空间模型，通过前一时刻的估计值和当前时刻的观测值实现对状态变量的更新与估计，从而得到当前时刻的估计值。对于满足状态和测量都是高斯噪声且系统可描述为线性随机微分方程（如式（8）所示），我们可以采取卡尔曼滤波，系统可表述如下：

状态方程：

观测方程：

其中，k∈N是时间指标，Xk表示k时刻的状态向量，Zk为k时刻系统状态的观测向量，Ak与Hk分别表示系统状态转移矩阵与观测矩阵，Uk-1为k-1时刻的外部控制量，在本文中取0。Wk与Vk表示k时刻的状态与观测噪声，通常看作正态分布。矩阵Q表示状态噪声的协方差矩阵，R表示测量时噪声的协方差矩阵，即Wk～N(0,Q)，Vk～N(0,R)。

卡尔曼滤波公式如下：

预测值和预测误差的协方差矩阵分别为：

获取新的观测Zk后，最优估算值、卡尔曼增益和更新的滤波误差协方差矩阵分别为：

卡尔曼滤波公式可以利用正交定理推导，推导过程可参考文献[14]。

2.2.6 基于卡尔曼滤波的Camshift算法

在目标跟踪过程中，由于相邻两帧图像间隔时间较短，目标运动状态变化较小，可以假设目标在两帧时间间隔内做匀速运动。定义目标运动状态向量Xk=(xk,yk,vxk,vyk)T，观测状态向量Zk=(xk,yk)。xk,yk表示目标质心在图像中位置，vxk,vyk表示目标运动的速度。故由2.2.2可知：

主要流程可以概括为：1）初始化。选择0时刻的窗口大小位置作为Camshift算法的输入，同时对0时刻窗口状态向量X0进行初始化。2）迭代。在时刻k，将上一时刻最优估计k-1|k-1代入式（11）得到|k-1，将Camshift算法给出的目标质心位置作为观测值Zk，把Zk代入式（13）对k|k-1进行修正得到Xk的最优估计|k，再将|k中的目标质心的位置分量作为下一时刻Camshift算法的输入。

在跟踪过程中，Camshift算法可依据目标大小自主调节搜索窗口大小，在目标突然跟踪失败无法继续跟踪的情况下，基于卡尔曼滤波的Camshift算法会产生一个可扩大窗口进一步进行目标跟踪。此外，搜索窗口的位置精度也有所提高。

2.3 上位机设计

本文中上位机主要完成以下事件：1）对传输过来的图像进行处理，包括目标跟踪与求取位置偏差两个部分，又由于本文中摄像头的位置相对于无人机是固定不变的，我们可以利用图像中目标物位置与图像中心的偏差来比拟实际无人机与目标物的位置偏差；2）将位置偏差实时返回。

对事件1我们采用Visual Studio编写上位机人机交互界面同时加载Open CV的库文件辅助图像处理实现。对事件2我们采用串口通信，主要包括了串口通信的初始化设置（包括串口号的选择，波特率的设定，数据位的选择以及奇偶校验位的设置），串口通信的发送（由于在本文中，只需要上位机界面给下位机飞行控制板发送位置偏差，因此不需要设计串口接收的部分）。

通信时采用数据发送格式如表1所示。

表1 数据发送格式

上表中，0xFA与0xAF均为设计的校验位。在下位机飞控板端，通过不断接收6个字符，判断第一位与最后一位是否为设计的校验位，从而保证发送正确。最终设计的上位机界面如图4所示。

图4 上位机界面

3 飞行控制

3.1 控制系统建立与仿真模型搭建

PID算法是一种不依赖被控对象模型，只和系统输入输出数据有关的控制方法，又因为四旋翼无人机有4个控制输入量与6个状态输出量，是一个欠驱动系统，很难准确建模，所以我们选用PID控制方法。

文献[15]向我们展示了关于四旋翼无人机比较成熟的动力学建模方法，本文基于该方法结合图像处理返回的x与y方向的偏差利用simulink仿真工具搭建控制模型。又由于无人机在根据偏差进行横滚与俯仰校准，将控制量输送给电机时，会导致无人机高度上的变化。因此，在飞行过程中需要考虑保持无人机的飞行高度，而且飞行高度的稳定也有利于摄像头返回稳定的图像。最终得到控制框图如图5所示。

图5 控制模型框图

根据本文所搭建的无人机，结合相关文献，得到相关变量的仿真参数如表2。

表2 simulink仿真参数表

3.2 PID参数整定与仿真结果

PID控制器参数整定，是指在控制器规律已经确定为PID形式的情况下，通过调整控制器的参数，使得由控制对象、控制器等组成的控制回路的动态特性满足期望的指标要求，达到理想的控制目标。

图6 X，Y，Z 方向跟踪仿真图（纵轴单位：m，横轴单位：s）

文中在PID参数整定时参照了ZN临界比度法[16]，即在闭环的情况下，将控制器的积分和微分作用先去掉，仅留下比例作用，然后在系统中加入一个扰动，反复调试得出临界增益Ku和临界振荡周期Tu，最后利用Ku和Tu通过经验公式得到PID参数整定值。最终位置PID参数：kpx=kpy=0.8，kdx=kdy=1.3，kix=10-4，kiy=3×10-4，高度PID参数：kpz=1.2，kdz=2，kiz=10-6。进行仿真实验得到如图5所示的位置输出与高度输出曲线图。

图6给出了系统仿真模型对设定点（3，4，5）（单位：m）的跟踪性能。通过不停的反馈位置偏差调整无人机的姿态，最终无人机稳定在（3，4，5）这个点。根据上述仿真给出的结果图，可以判断出这种控制算法是可行的。

3.3 定点实验

为了测试定点装置的实际应用效果，通过一定量的实验测试其性能具体测试内容如下：当无人机定点装置，处于目标点位置飞行较为稳定时，通过将遥控器拨到手动通道，人为的将无人机拉到远离目标点的位置，再将遥控器迅速拨回自动通道，观察无人机定点装置的运动，并利用Vicon室内目标定位系统记录下无人机XY通道的运动数据，如图7所示。

通过实验数据可以看到，在开始位置将无人机拉到距离目标为（210，-200）位置处，通过控制律的调节，在误差允许的范围内，可以视为将无人机拉回原点，即目标位置，经测试能较好的实现定点功能。

图7 XY通道跟踪误差曲线（纵轴单位：cm，横轴单位：ms）

4 结束语

本文通过在simulink平台上搭建四旋翼无人机模型，和实际中搭建无人机硬件平台和地面站，分别从仿真与实验的角度证明了基于视觉的无人机在定点飞行时的优良性能。

参考文献：

[1]李雷，陈桂琴，田园.无人机的发展历程与展望[J].科技论坛，2011（32）：6.

[2]吴瑶瑶，白云飞，曹科才.扑翼飞行器硬件平台设计与实现[J].信息化研究，2016，42（3）：38-42.

[3]吴显亮，石宗英，钟宜生.无人机视觉导航研究综述[J].系统仿真学报，2010，22（1）：62-65.

[4]郭佳琪，吴杰，蒋一琦.高压线巡线飞行器的研究与设计[J].南京工程学院学报：自然科学版，2012，10（1）：39-42.

[5]徐东升.惯性导航中加速度计和陀螺仪性能研究[J].佳木斯大学学报：自然科学版，2012，30（3）：334-337.

[6]刘畅，杨良军，刘峰.一种基于惯性/视觉信息融合的无人机自主着陆导航算法[J].导航定位与授时，2016，3（6）：7-11.

[7]盛哲.基于视觉导航的无人机位姿控制与自动返航技术[D].上海：上海交通大学，2015.

[8]Zhang X，Xian B.Autonomous flight control of a nano quadrotor helicopter in a GPS-Denied envi⁃ronment using on-Board vision[J].IEEE Transac⁃tions on Industrial Electronics，2015：1-11.

[9]王翔鹏.GPS失效情况下农用无人机视觉导航关键算法研究[D].青岛：青岛科技大学，2014.

[10]刘国宏，郭文明.改进的中值滤波去噪算法应用分析[J].计算机工程与应用，2010，46（10）：187-189.

[11]赵高长，张磊，武风波.改进的中值滤波算法在图像去噪中的应用[J].应用光学，2011，32（4）：678-682.

[12]黄忠主.面向监视视频的运动轨迹提取方法研究[D].长沙：国防科技大，2011.

[13]闫钧华，陈少华，艾淑芳.基于Kalman预测器的改进的CAMShift目标跟踪[J].中国惯性技术学报，2014，22（4）：536-542.

[14]韩崇昭，朱洪艳，段战胜.多源信息融合[M].北京：清华大学出版社，2006.

[15]李俊，李运堂.四旋翼飞行器的动力学建模及PID控制[J].辽宁工程技术大学学报：自然科学版，2012，31（1）：114-117.

[16]付瑞玲，乐丽琴.基于MATLAB/Simulink的PID参数整定[J].工业控制计算机，2013，26（8）：75-76.