基于光流的四旋翼飛行器控制技術研究

李志祥 唐春暉

摘 要：為了解決無人機在無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.

（責任編輯：黃 健）