基于光流的四旋翼飞行器控制技术研究

李志祥 唐春晖

摘 要：为了解决无人机在无GPS或GPS信号较弱情况下的稳定悬停问题，并考虑到成本与板载资源限制，将通过小型激光雷达获得的距离信息分别与通过单摄像头获得的视觉信息，以及通过IMU（惯性测量单元）获得的惯性信息利用互补滤波算法进行融合，以实现对四旋翼飞行器姿态与水平速度的运动估计。采用基于串级PID的多闭环控制策略，实现对四旋翼飞行器水平与垂直方向的控制。实验结果表明，所设计的基于光流和小型激光雷达的四旋翼飞行器控制策略与传统利用光流和超声波测距传感器方案相比，控制精度提高了10%左右，能够以最大±2°的姿态角误差，以及最大2.3cm/s的水平速度误差实现定点悬停功能。

关键词：光流;四旋翼飞行器;IMU;小型激光雷达;串级PID;悬停控制

DOI：10. 11907/rjdk. 182891 开放科学（资源服务）标识码（OSID）：

中图分类号：TP319文献标识码：A 文章编号：1672-7800（2019）009-0134-05

Research on Control Technology of Four-rotor Aircraft Based on Optical Flow

LI Zhi-xiang，TANG Chun-hui

（School of Optical-Electrical and Computer Engineering， University of Shanghai for Science and Technology，Shanghai 200093，China）

Abstract： In order to solve the problem of stable hovering of drones without GPS or GPS signals， and taking into account cost and onboard resource constraints， the distance information obtained by a small lidar is fused with the visual information obtained by a single camera and the inertial information obtained by the IMU （Inertial Measurement Unit） by a complementary filtering algorithm to realize the motion estimation of the attitude and horizontal velocity of the quadrotor. The horizontal and vertical control of the quadrotor is realized by a multi-closed loop control strategy based on cascade PID. The experimental results show that the control precision of the four-rotor aircraft based on optical flow and small lidar designed in this paper is about 10% higher than that of the conventional optical flow and ultrasonic ranging sensor， and can achieve a maximum ±2° attitude angular error and a horizontal speed error of up to 2.3 cm/s to achieve the function of fixed-point hover.

Key Words： optical flow; four-rotor aircraft;IMU; small lidar; cascade PID; hover control

0 引言

如今各種视觉导航技术已被应用于使用嵌入式摄像机的无人驾驶飞行器[1-2]，从而增强其自主性与适应性以完成不同任务。由于无人机具有极高的民用与军事应用价值，在过去10年间，人们对自主无人机（UAV）的开发一直具有浓厚兴趣。无人机通常用于搜索与救援任务[3]，可进行实时监视、巡逻等操作。

无人机定位与导航通常使用全球定位系统（GPS），Jeonghoon等[4]基于GPS数据与搜索算法对无人机进行路径规划，以实现自主飞行;Zhu等[5]提出一种基于视觉与GPS的无人机自主空中加油对接控制系统;黄艳虎等[6]利用GPS，通过GPRS网络通信设计了一套可以在用户端进行软件控制的定位系统，并将该系统应用于四旋翼。但在偏僻或狭小的室内空间等GPS信号较弱的环境下，对无人机的稳定控制仍然存在很大挑战;孙瑶等[7]针对室内无GPS情况下的四旋翼定位问题，提出一种优于传统NN与KNN算法的WiFi-RSSI算法，以实现四旋翼在室内的高精度定位，但限于小型飞行器的有效载荷，无法搭载可以承担该算法的计算设备，因此该算法不适用于小型四旋翼飞行器;王清鹏[8]利用惯性传感器和超声波测距传感器，提出一种适用于无GPS环境下的通信与导航技术，但控制精度会受到超声波测距与惯性传感器数据带来积分误差的影响;邓一民[9]、Zhang等[10]针对旋翼无人机在GPS信号丢失情况下，基于光流与惯性组合实现一种自助导航方案，但该方案中光流传感器精度受飞行器高度影响较大;Denuelle等[11]利用光流与图像匹配方法实现了对小型四旋翼无人机室外飞行漂移的抑制，达到了稳定悬停的效果，但该方案是在相机的较低帧率下实现的，无法满足对无人机高速飞行情形下的要求。

为了解决在无GPS信号或GPS信号丢失情况下无人机的稳定控制问题，本文利用光流传感器与小型激光雷达，同时融合惯性传感器估算无人机相对地面的水平速度，并将该速度信息用于无人机控制，以实现无人机的定点悬停功能。视觉和惯性感知是两种感知方式，可用来探索图像分割与图像结构恢复的解决方案[12-13]。结合惯性和视觉传感器可提高机器人的感知能力，并扩大视觉系统应用潜力。相机和惯性传感器可实现优势互补，一方面，惯性传感器在慢速运动时具有较大的测量不确定性，在高速运动时具有较低的相对不确定性，因此可以测量很高的速度与加速度;另一方面，相机在低速条件下可以非常精确地跟踪特征，在高速情况下却很难进行准确跟踪，必须降低图像分辨率以获得具有相同像素尺寸的跟踪窗口，才能获得更高的跟踪速度。在UAV应用中，受到有效载荷约束，并为了满足实时计算的要求，计算资源受到限制。因此，基于特征提取的计算密集型技术在这里并不适用，可行的方法是光流计算与随后的运动估计[14]。光流计算的主要原理是在一系列时间排序图像上计算光学流场或图像速度场，然后将投影的二维图像运动估计为瞬时图像速度或离散图像位移[15]。本文使用的光流传感器与小型激光雷达具有体积小、质量轻、测量精度高等优点，适合应用于小型四旋翼飞行器。

1 基于光流与小型激光雷达的无人机控制策略

为了控制无人机在无GPS情况下的稳定悬停，首先需要对无人机运动状态信息进行估算。采用搭载的光流传感器通过光流计算可获得无人机在水平方向上的速度信息，通过小型激光雷达融合IMU获得的惯性数据可获得无人机在竖直方向上的信息。悬停控制策略是在姿态双闭环串级PID控制基础上设计出的速度多闭环控制策略，可以有效控制四旋翼飞行器的稳定悬停。

1.1 光流法简介

光流概念最早是由Gibson在1950年提出的，其是基于空间运动物体在观察成像平面上像素运动的瞬时速度进行定义的，即利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性，找到上一帧与当前帧之间存在的对应关系，从而计算出相邻帧之间物体运动信息的一种方法[16]。一般情况下，光流的产生来源于场景中前景目标平移、相机晃动，或两者的共同运动。应用于光流计算的方法很多，大致可概括为：基于匹配的方法、基于微分的方法、基于能量的方法、基于相位的方法及基于神经动力学的方法共5大类。简单来讲，空间运动的物体通过光学透镜在成像平面上成像，其像素运动的瞬时速度即为光流。因此，通过对图像序列中像素数据时间变化与相关性的研究，获得每个像素位置的运动信息，再根据成像原理即可近似估算出空间物体运动状态[17]。

本文基于光流与小型激光雷达及IMU模块设计一种适用于四旋翼无人机的控制策略，该策略主要利用小型激光雷达传感器获取相机距地面的距离，并通过卡尔曼滤波处理滤除噪声。该距离信息主要有两个用途：一是被实时反馈到飞行控制器，融合IMU测得的角度信息计算出飞行器距离地面的高度，实现垂直方向上的高度控制;二是融合光流传感器数据计算出飞行器速度信息，从而实现对飞行器水平方向的控制。

1.2 光流数据与飞行器运动状态转换

飞行器状态信息是一个基于三维空间的数据，而光流数据是一个二维投影数据。因此，要获得飞行器三维空间速度信息，需要将二维空间数据转换为三维空间数据[18]。图1为二维光流数据与三维飞行器运动状态信息的转换示意图。

从式中可以看到，光流速度是由摄像头平移与旋转共同组成的，因此需要对得到的光流数据进行距离和姿态角补偿，从而得到最终结果。本文采用小型激光雷达获取四旋翼距离地面与目标的差值，并结合板载IMU数据对光流数据进行距离与姿态角补偿。

1.3 小型激光雷达测距

在飞行器控制领域，常用的测距传感器为超声波传感器和TOF传感器[19-20]。超声波传感器因具有成本低、使用方便等特点，得到了广泛应用，但受到飞行器飞行中产生的震动和气流等影响，其测量数据往往具有很大的噪声，工作频率也很难满足快速飞行的要求。本文选取的采用850nm红外光源、基于TOF测距原理的小型激光雷达传感器具有抗干扰性强，以及测量准确、稳定、高速等优点，可实现快速飞行下的距离测量，其测距原理如图3所示。其距离计算公式可表示为：

1.4 传感器数据融合

受图像噪声影响，光流传感器的直接数据输出有着明显噪声，这些噪声主要是高频噪声，因此使用一阶低通滤波器对其进行滤波。采用软件编程实现普通硬件RC低通滤波器功能。其公式为：

1.5 基于速度与高度信息的悬停控制

在本文中，无人机姿态及高度控制采用双闭环PID控制算法，其控制流程分别如图2、图3所示。

悬停控制属于位置控制环节的任务，悬停控制的根本目标是保持无人机位置的稳定。光流传感器通过光流计算可得到无人机的实时速度信息，因此可以在姿态控制回路基础上增加一个速度环PID控制，将无人机各级控制耦合起来，实现一个完整的多闭环控制回路，如图4所示。

在无人机的高度双闭环控制回路中，以传感器测得的当前高度值作为控制系统的期望高度值，即可实现对飞行器竖直方向的控制;在速度多闭环控制回路中，将期望速度设为零，即可实现飞行器水平方向上的定点控制。结合两个控制回路可最终实现飞行器在当前位置的穩定悬停。

2 实验设计及结果分析

为了验证本文提出控制策略的有效性，笔者搭建了一个实验平台，通过实际飞行实验观察飞行器飞行状态，并采集飞行器水平速度与姿态信息等数据，通过数据分析验证飞行器的飞行效果。

2.1 平台搭建

本文搭建的四旋翼试验平台基于380mm轴距的机架，以STM32F407作为核心飞行控制器，搭载使用SPI通信的ICM20602惯性传感器（包括3轴陀螺仪和3轴加速度计），板载具有高精度与高灵敏度特点的SPL06气压计以及磁场传感器AK8975。光流传感器模块采用分辨率为[640×480]像素、焦距为16mm的CMOS摄像头和1块集成型号为STM32F411的MCU组成。小型激光雷达采用型号为TFmini的距离传感器，其有效测距范围为0～12m。如图5所示为笔者搭建的四旋翼实验平台实物图。四旋翼通过光流传感器采集图像信息，经过低通滤波滤除高频噪声，然后通过光流算法计算并输出像素速度信息，通过与距离信息及IMU数据进行姿态融合解算，实现对四旋翼水平与垂直方向的控制，最终达到稳定悬停的效果。控制系统架构如图6所示。

2.2 試验结果分析

飞行试验数据通过无线数传模块实时传送到地面计算机进行保存，计算机对保存的数据进行分析以评估控制策略的优劣。图7（a）-图7（e）分别展示了飞行过程中水平速度与姿态角的状态。实验中设置期望速度和角度均为0，从图7（a）与图7（b）可以看出，四旋翼从起飞到进入悬停状态，X方向和Y方向的水平速度在x轴上下波动，经过精确计算得到水平速度误差维持在±2.3cm/s左右。图7（c）-7（d）为飞行过程中四旋翼姿态角变化曲线，可以看出在悬停过程中，俯仰角和横滚角均在0°附近波动，偏航角也几乎维持不变，其中俯仰角和横滚角误差在±2°左右，偏航角误差在±1°左右，飞行器表现出令人满意的悬停效果。

3 结语

本文基于光流传感器与小型激光雷达设计了一种四旋翼飞行器控制策略，用于解决四旋翼飞行器在无GPS或GPS信号较弱情形下的稳定悬停问题。将通过小型激光雷达获得的距离信息分别与光流计算及惯性测量值相融合，最终估计出飞行器的姿态与水平速度。实验结果表明，本文设计的基于光流与小型激光雷达的四旋翼飞行器控制策略与传统利用光流及超声波测距传感器方案相比，控制精度提高了10%左右，能够以最大±2°的姿态角误差，以及最大2.3cm/s的水平速度误差实现定点悬停功能。然而，如实验结果所示，飞行器的俯仰和横滚振荡仍然存在。目前笔者正在对光流测量方法作进一步改进，以便获得更好的飞行性能。

参考文献：

[1] SABO C M，COPE A，GURNY K，et al. Bio-inspired visual navigation for a quadcopter using optic flow[C]. San Diego：AIAA SciTech， 2016.

[2] SHREE A S， SHARMA R S， BEHERA L， et al. Position based visual control of the hovering quadcopter[C]. Toronto：International Conference on Intelligent Human Computer Interaction，2016.

[3] MOISéS L， IVáN S， CABALLEROGIL P. UAV-based rescue system for emergency situations[M]. New York：Ubiquitous Computing and Ambient Intelligence，Springer International Publishing， 2016.

[4] Jeonghoon K，Yunsick S. Autonomous UAV flight control for GPS-based navigation[J].  IEEE Access， 2018，6：37947-37955.

[5] ZHU H，YUAN S，SHEN Q. Vision/GPS-based docking control for the UAV autonomous aerial refueling[C]. Nanjing：IEEE Conference on Guidance， Navigation & Control Conference，2017.

[6] 黄艳虎，陈家裕，陈倍昌，等. 基于GPS的四旋翼飞行器定位系统的设计[J].  电脑知识与技术， 2016， 12（8）：205-206.

[7] 孙瑶，王磊，王延召，等. 基于PSO-KNN的WiFi-RSSI指纹算法的四旋翼室内定位[J]. 无线互联科技， 2018（3）： 117-118.

[8] 王清鹏. 基于MEMS的四旋翼飞行器通信与导航技术研究[D]. 沈阳：沈阳理工大学，2016.

[9] 邓一民. 多旋翼无人机光流/惯性组合导航技术研究[D]. 南京：南京航空航天大学，2016

[10] ZHANG L，XIONG Z，LAI J， et al. Optical flow-aided navigation for UAV： a novel information fusion of integrated MEMS navigation system[J].  Optik-International Journal for Light and Electron Optics， 2016， 127（1）：447-451.

[11] DENUELLE A，STRYDOM R， SRINIVASAN M V. Snapshot-based control of UAS hover in outdoor environments[C]. Zhuhai：IEEE International Conference on Robotics & Biomimetics， 2015.

[12] MIAO J，HUANG T Z，ZHOU X，et al. Image segmentation based on an active contour model of partial image restoration with local cosine fitting energy[J].  Information Sciences， 2018：52-71.

[13] THOMAS J，WELDE J，LOIANNO G，et al. Autonomous flight for detection， localization， and tracking of moving targets with a small quadrotor[J].  IEEE Robotics & Automation Letters， 2017， 99：1.

[14] ZHOU J，ZHAO X，CHENG X，et al. Optical flow-based optimization method of vehicle motion estimation[J]. Journal of Harbin Institute of Technology， 2016：65-69.

[15] KUMAR P，KUMAR S，RAMAN B. A fractional order variational model for the robust estimation of optical flow from image sequences[J]. Optik-International Journal for Light and Electron Optics， 2016，127（20）：8710-8727.

[16] 刘梦瑶，王岩，郭雷. 6-DOF motion estimation using optical flow based on dual cameras[J]. 中南大學学报：英文版，2017，24（2）：459-466.

[17] 杨亚东. 光流法在运动目标识别领域的理论与应用[J]. 电子设计工程，2013，21（5）：24-26.

[18] 和世熙，张聪炫. 基于直线与端点光流对应的刚体三维运动测量与轮廓重建[J]. 南昌航空大学学报：自然科学版，2016，30（3）：23-30.

[19] 潘日萍，黄文君. 基于3DTOF的景深测量系统的原理研究与系统设计[J]. 太原师范学院学报：自然科学版，2017（4）：44-48.

[20] 牛群峰，曹一帆，王莉，等. 基于TW-TOF的UWB室内定位技术与优化算法研究[J]. 自动化与仪表， 2018（1）：5-9.

[21] 张栋，焦嵩鸣，刘延泉. 互补滤波和卡尔曼滤波的融合姿态解算方法[J]. 传感器与微系统，2017， 36（3）：62-65.

[22] 许晶晶，郭培源，董小栋，等. 改进互补滤波在六旋翼飞行器中的应用[J]. 传感器与微系统，2018，37（1）：157-160.

（责任编辑：黄 健）