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

李志祥 唐春暉

摘 要：為了解決無人機(jī)在無GPS或GPS信號(hào)較弱情況下的穩(wěn)定懸停問題，并考慮到成本與板載資源限制，將通過小型激光雷達(dá)獲得的距離信息分別與通過單攝像頭獲得的視覺信息，以及通過IMU（慣性測(cè)量單元）獲得的慣性信息利用互補(bǔ)濾波算法進(jìn)行融合，以實(shí)現(xiàn)對(duì)四旋翼飛行器姿態(tài)與水平速度的運(yùn)動(dòng)估計(jì)。采用基于串級(jí)PID的多閉環(huán)控制策略，實(shí)現(xiàn)對(duì)四旋翼飛行器水平與垂直方向的控制。實(shí)驗(yàn)結(jié)果表明，所設(shè)計(jì)的基于光流和小型激光雷達(dá)的四旋翼飛行器控制策略與傳統(tǒng)利用光流和超聲波測(cè)距傳感器方案相比，控制精度提高了10%左右，能夠以最大±2°的姿態(tài)角誤差，以及最大2.3cm/s的水平速度誤差實(shí)現(xiàn)定點(diǎn)懸停功能。

關(guān)鍵詞：光流;四旋翼飛行器;IMU;小型激光雷達(dá);串級(jí)PID;懸?？刂?/p>

DOI：10. 11907/rjdk. 182891 開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

中圖分類號(hào)：TP319文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：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 引言

如今各種視覺導(dǎo)航技術(shù)已被應(yīng)用于使用嵌入式攝像機(jī)的無人駕駛飛行器[1-2]，從而增強(qiáng)其自主性與適應(yīng)性以完成不同任務(wù)。由于無人機(jī)具有極高的民用與軍事應(yīng)用價(jià)值，在過去10年間，人們對(duì)自主無人機(jī)（UAV）的開發(fā)一直具有濃厚興趣。無人機(jī)通常用于搜索與救援任務(wù)[3]，可進(jìn)行實(shí)時(shí)監(jiān)視、巡邏等操作。

無人機(jī)定位與導(dǎo)航通常使用全球定位系統(tǒng)（GPS），Jeonghoon等[4]基于GPS數(shù)據(jù)與搜索算法對(duì)無人機(jī)進(jìn)行路徑規(guī)劃，以實(shí)現(xiàn)自主飛行;Zhu等[5]提出一種基于視覺與GPS的無人機(jī)自主空中加油對(duì)接控制系統(tǒng);黃艷虎等[6]利用GPS，通過GPRS網(wǎng)絡(luò)通信設(shè)計(jì)了一套可以在用戶端進(jìn)行軟件控制的定位系統(tǒng)，并將該系統(tǒng)應(yīng)用于四旋翼。但在偏僻或狹小的室內(nèi)空間等GPS信號(hào)較弱的環(huán)境下，對(duì)無人機(jī)的穩(wěn)定控制仍然存在很大挑戰(zhàn);孫瑤等[7]針對(duì)室內(nèi)無GPS情況下的四旋翼定位問題，提出一種優(yōu)于傳統(tǒng)NN與KNN算法的WiFi-RSSI算法，以實(shí)現(xiàn)四旋翼在室內(nèi)的高精度定位，但限于小型飛行器的有效載荷，無法搭載可以承擔(dān)該算法的計(jì)算設(shè)備，因此該算法不適用于小型四旋翼飛行器;王清鵬[8]利用慣性傳感器和超聲波測(cè)距傳感器，提出一種適用于無GPS環(huán)境下的通信與導(dǎo)航技術(shù)，但控制精度會(huì)受到超聲波測(cè)距與慣性傳感器數(shù)據(jù)帶來積分誤差的影響;鄧一民[9]、Zhang等[10]針對(duì)旋翼無人機(jī)在GPS信號(hào)丟失情況下，基于光流與慣性組合實(shí)現(xiàn)一種自助導(dǎo)航方案，但該方案中光流傳感器精度受飛行器高度影響較大;Denuelle等[11]利用光流與圖像匹配方法實(shí)現(xiàn)了對(duì)小型四旋翼無人機(jī)室外飛行漂移的抑制，達(dá)到了穩(wěn)定懸停的效果，但該方案是在相機(jī)的較低幀率下實(shí)現(xiàn)的，無法滿足對(duì)無人機(jī)高速飛行情形下的要求。

為了解決在無GPS信號(hào)或GPS信號(hào)丟失情況下無人機(jī)的穩(wěn)定控制問題，本文利用光流傳感器與小型激光雷達(dá)，同時(shí)融合慣性傳感器估算無人機(jī)相對(duì)地面的水平速度，并將該速度信息用于無人機(jī)控制，以實(shí)現(xiàn)無人機(jī)的定點(diǎn)懸停功能。視覺和慣性感知是兩種感知方式，可用來探索圖像分割與圖像結(jié)構(gòu)恢復(fù)的解決方案[12-13]。結(jié)合慣性和視覺傳感器可提高機(jī)器人的感知能力，并擴(kuò)大視覺系統(tǒng)應(yīng)用潛力。相機(jī)和慣性傳感器可實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)，一方面，慣性傳感器在慢速運(yùn)動(dòng)時(shí)具有較大的測(cè)量不確定性，在高速運(yùn)動(dòng)時(shí)具有較低的相對(duì)不確定性，因此可以測(cè)量很高的速度與加速度;另一方面，相機(jī)在低速條件下可以非常精確地跟蹤特征，在高速情況下卻很難進(jìn)行準(zhǔn)確跟蹤，必須降低圖像分辨率以獲得具有相同像素尺寸的跟蹤窗口，才能獲得更高的跟蹤速度。在UAV應(yīng)用中，受到有效載荷約束，并為了滿足實(shí)時(shí)計(jì)算的要求，計(jì)算資源受到限制。因此，基于特征提取的計(jì)算密集型技術(shù)在這里并不適用，可行的方法是光流計(jì)算與隨后的運(yùn)動(dòng)估計(jì)[14]。光流計(jì)算的主要原理是在一系列時(shí)間排序圖像上計(jì)算光學(xué)流場(chǎng)或圖像速度場(chǎng)，然后將投影的二維圖像運(yùn)動(dòng)估計(jì)為瞬時(shí)圖像速度或離散圖像位移[15]。本文使用的光流傳感器與小型激光雷達(dá)具有體積小、質(zhì)量輕、測(cè)量精度高等優(yōu)點(diǎn)，適合應(yīng)用于小型四旋翼飛行器。

1 基于光流與小型激光雷達(dá)的無人機(jī)控制策略

為了控制無人機(jī)在無GPS情況下的穩(wěn)定懸停，首先需要對(duì)無人機(jī)運(yùn)動(dòng)狀態(tài)信息進(jìn)行估算。采用搭載的光流傳感器通過光流計(jì)算可獲得無人機(jī)在水平方向上的速度信息，通過小型激光雷達(dá)融合IMU獲得的慣性數(shù)據(jù)可獲得無人機(jī)在豎直方向上的信息。懸?？刂撇呗允窃谧藨B(tài)雙閉環(huán)串級(jí)PID控制基礎(chǔ)上設(shè)計(jì)出的速度多閉環(huán)控制策略，可以有效控制四旋翼飛行器的穩(wěn)定懸停。

1.1 光流法簡(jiǎn)介

光流概念最早是由Gibson在1950年提出的，其是基于空間運(yùn)動(dòng)物體在觀察成像平面上像素運(yùn)動(dòng)的瞬時(shí)速度進(jìn)行定義的，即利用圖像序列中像素在時(shí)間域上的變化以及相鄰幀之間的相關(guān)性，找到上一幀與當(dāng)前幀之間存在的對(duì)應(yīng)關(guān)系，從而計(jì)算出相鄰幀之間物體運(yùn)動(dòng)信息的一種方法[16]。一般情況下，光流的產(chǎn)生來源于場(chǎng)景中前景目標(biāo)平移、相機(jī)晃動(dòng)，或兩者的共同運(yùn)動(dòng)。應(yīng)用于光流計(jì)算的方法很多，大致可概括為：基于匹配的方法、基于微分的方法、基于能量的方法、基于相位的方法及基于神經(jīng)動(dòng)力學(xué)的方法共5大類。簡(jiǎn)單來講，空間運(yùn)動(dòng)的物體通過光學(xué)透鏡在成像平面上成像，其像素運(yùn)動(dòng)的瞬時(shí)速度即為光流。因此，通過對(duì)圖像序列中像素?cái)?shù)據(jù)時(shí)間變化與相關(guān)性的研究，獲得每個(gè)像素位置的運(yùn)動(dòng)信息，再根據(jù)成像原理即可近似估算出空間物體運(yùn)動(dòng)狀態(tài)[17]。

本文基于光流與小型激光雷達(dá)及IMU模塊設(shè)計(jì)一種適用于四旋翼無人機(jī)的控制策略，該策略主要利用小型激光雷達(dá)傳感器獲取相機(jī)距地面的距離，并通過卡爾曼濾波處理濾除噪聲。該距離信息主要有兩個(gè)用途：一是被實(shí)時(shí)反饋到飛行控制器，融合IMU測(cè)得的角度信息計(jì)算出飛行器距離地面的高度，實(shí)現(xiàn)垂直方向上的高度控制;二是融合光流傳感器數(shù)據(jù)計(jì)算出飛行器速度信息，從而實(shí)現(xiàn)對(duì)飛行器水平方向的控制。

1.2 光流數(shù)據(jù)與飛行器運(yùn)動(dòng)狀態(tài)轉(zhuǎn)換

飛行器狀態(tài)信息是一個(gè)基于三維空間的數(shù)據(jù)，而光流數(shù)據(jù)是一個(gè)二維投影數(shù)據(jù)。因此，要獲得飛行器三維空間速度信息，需要將二維空間數(shù)據(jù)轉(zhuǎn)換為三維空間數(shù)據(jù)[18]。圖1為二維光流數(shù)據(jù)與三維飛行器運(yùn)動(dòng)狀態(tài)信息的轉(zhuǎn)換示意圖。

從式中可以看到，光流速度是由攝像頭平移與旋轉(zhuǎn)共同組成的，因此需要對(duì)得到的光流數(shù)據(jù)進(jìn)行距離和姿態(tài)角補(bǔ)償，從而得到最終結(jié)果。本文采用小型激光雷達(dá)獲取四旋翼距離地面與目標(biāo)的差值，并結(jié)合板載IMU數(shù)據(jù)對(duì)光流數(shù)據(jù)進(jìn)行距離與姿態(tài)角補(bǔ)償。

1.3 小型激光雷達(dá)測(cè)距

在飛行器控制領(lǐng)域，常用的測(cè)距傳感器為超聲波傳感器和TOF傳感器[19-20]。超聲波傳感器因具有成本低、使用方便等特點(diǎn)，得到了廣泛應(yīng)用，但受到飛行器飛行中產(chǎn)生的震動(dòng)和氣流等影響，其測(cè)量數(shù)據(jù)往往具有很大的噪聲，工作頻率也很難滿足快速飛行的要求。本文選取的采用850nm紅外光源、基于TOF測(cè)距原理的小型激光雷達(dá)傳感器具有抗干擾性強(qiáng)，以及測(cè)量準(zhǔn)確、穩(wěn)定、高速等優(yōu)點(diǎn)，可實(shí)現(xiàn)快速飛行下的距離測(cè)量，其測(cè)距原理如圖3所示。其距離計(jì)算公式可表示為：

1.4 傳感器數(shù)據(jù)融合

受圖像噪聲影響，光流傳感器的直接數(shù)據(jù)輸出有著明顯噪聲，這些噪聲主要是高頻噪聲，因此使用一階低通濾波器對(duì)其進(jìn)行濾波。采用軟件編程實(shí)現(xiàn)普通硬件RC低通濾波器功能。其公式為：

1.5 基于速度與高度信息的懸?？刂?/p>

在本文中，無人機(jī)姿態(tài)及高度控制采用雙閉環(huán)PID控制算法，其控制流程分別如圖2、圖3所示。

懸?？刂茖儆谖恢每刂骗h(huán)節(jié)的任務(wù)，懸?？刂频母灸繕?biāo)是保持無人機(jī)位置的穩(wěn)定。光流傳感器通過光流計(jì)算可得到無人機(jī)的實(shí)時(shí)速度信息，因此可以在姿態(tài)控制回路基礎(chǔ)上增加一個(gè)速度環(huán)PID控制，將無人機(jī)各級(jí)控制耦合起來，實(shí)現(xiàn)一個(gè)完整的多閉環(huán)控制回路，如圖4所示。

在無人機(jī)的高度雙閉環(huán)控制回路中，以傳感器測(cè)得的當(dāng)前高度值作為控制系統(tǒng)的期望高度值，即可實(shí)現(xiàn)對(duì)飛行器豎直方向的控制;在速度多閉環(huán)控制回路中，將期望速度設(shè)為零，即可實(shí)現(xiàn)飛行器水平方向上的定點(diǎn)控制。結(jié)合兩個(gè)控制回路可最終實(shí)現(xiàn)飛行器在當(dāng)前位置的穩(wěn)定懸停。

2 實(shí)驗(yàn)設(shè)計(jì)及結(jié)果分析

為了驗(yàn)證本文提出控制策略的有效性，筆者搭建了一個(gè)實(shí)驗(yàn)平臺(tái)，通過實(shí)際飛行實(shí)驗(yàn)觀察飛行器飛行狀態(tài)，并采集飛行器水平速度與姿態(tài)信息等數(shù)據(jù)，通過數(shù)據(jù)分析驗(yàn)證飛行器的飛行效果。

2.1 平臺(tái)搭建

本文搭建的四旋翼試驗(yàn)平臺(tái)基于380mm軸距的機(jī)架，以STM32F407作為核心飛行控制器，搭載使用SPI通信的ICM20602慣性傳感器（包括3軸陀螺儀和3軸加速度計(jì)），板載具有高精度與高靈敏度特點(diǎn)的SPL06氣壓計(jì)以及磁場(chǎng)傳感器AK8975。光流傳感器模塊采用分辨率為[640×480]像素、焦距為16mm的CMOS攝像頭和1塊集成型號(hào)為STM32F411的MCU組成。小型激光雷達(dá)采用型號(hào)為TFmini的距離傳感器，其有效測(cè)距范圍為0～12m。如圖5所示為筆者搭建的四旋翼實(shí)驗(yàn)平臺(tái)實(shí)物圖。四旋翼通過光流傳感器采集圖像信息，經(jīng)過低通濾波濾除高頻噪聲，然后通過光流算法計(jì)算并輸出像素速度信息，通過與距離信息及IMU數(shù)據(jù)進(jìn)行姿態(tài)融合解算，實(shí)現(xiàn)對(duì)四旋翼水平與垂直方向的控制，最終達(dá)到穩(wěn)定懸停的效果?？刂葡到y(tǒng)架構(gòu)如圖6所示。

2.2 試驗(yàn)結(jié)果分析

飛行試驗(yàn)數(shù)據(jù)通過無線數(shù)傳模塊實(shí)時(shí)傳送到地面計(jì)算機(jī)進(jìn)行保存，計(jì)算機(jī)對(duì)保存的數(shù)據(jù)進(jìn)行分析以評(píng)估控制策略的優(yōu)劣。圖7（a）-圖7（e）分別展示了飛行過程中水平速度與姿態(tài)角的狀態(tài)。實(shí)驗(yàn)中設(shè)置期望速度和角度均為0，從圖7（a）與圖7（b）可以看出，四旋翼從起飛到進(jìn)入懸停狀態(tài)，X方向和Y方向的水平速度在x軸上下波動(dòng)，經(jīng)過精確計(jì)算得到水平速度誤差維持在±2.3cm/s左右。圖7（c）-7（d）為飛行過程中四旋翼姿態(tài)角變化曲線，可以看出在懸停過程中，俯仰角和橫滾角均在0°附近波動(dòng)，偏航角也幾乎維持不變，其中俯仰角和橫滾角誤差在±2°左右，偏航角誤差在±1°左右，飛行器表現(xiàn)出令人滿意的懸停效果。

3 結(jié)語

本文基于光流傳感器與小型激光雷達(dá)設(shè)計(jì)了一種四旋翼飛行器控制策略，用于解決四旋翼飛行器在無GPS或GPS信號(hào)較弱情形下的穩(wěn)定懸停問題。將通過小型激光雷達(dá)獲得的距離信息分別與光流計(jì)算及慣性測(cè)量值相融合，最終估計(jì)出飛行器的姿態(tài)與水平速度。實(shí)驗(yàn)結(jié)果表明，本文設(shè)計(jì)的基于光流與小型激光雷達(dá)的四旋翼飛行器控制策略與傳統(tǒng)利用光流及超聲波測(cè)距傳感器方案相比，控制精度提高了10%左右，能夠以最大±2°的姿態(tài)角誤差，以及最大2.3cm/s的水平速度誤差實(shí)現(xiàn)定點(diǎn)懸停功能。然而，如實(shí)驗(yàn)結(jié)果所示，飛行器的俯仰和橫滾振蕩仍然存在。目前筆者正在對(duì)光流測(cè)量方法作進(jìn)一步改進(jìn)，以便獲得更好的飛行性能。

參考文獻(xiàn)：

[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的四旋翼飛行器定位系統(tǒng)的設(shè)計(jì)[J]. ?電腦知識(shí)與技術(shù)， 2016， 12（8）：205-206.

[7] 孫瑤，王磊，王延召，等. 基于PSO-KNN的WiFi-RSSI指紋算法的四旋翼室內(nèi)定位[J]. 無線互聯(lián)科技， 2018（3）： 117-118.

[8] 王清鵬. 基于MEMS的四旋翼飛行器通信與導(dǎo)航技術(shù)研究[D]. 沈陽：沈陽理工大學(xué)，2016.

[9] 鄧一民. 多旋翼無人機(jī)光流/慣性組合導(dǎo)航技術(shù)研究[D]. 南京：南京航空航天大學(xué)，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] 劉夢(mèng)瑤，王巖，郭雷. 6-DOF motion estimation using optical flow based on dual cameras[J]. 中南大學(xué)學(xué)報(bào)：英文版，2017，24（2）：459-466.

[17] 楊亞東. 光流法在運(yùn)動(dòng)目標(biāo)識(shí)別領(lǐng)域的理論與應(yīng)用[J]. 電子設(shè)計(jì)工程，2013，21（5）：24-26.

[18] 和世熙，張聰炫. 基于直線與端點(diǎn)光流對(duì)應(yīng)的剛體三維運(yùn)動(dòng)測(cè)量與輪廓重建[J]. 南昌航空大學(xué)學(xué)報(bào)：自然科學(xué)版，2016，30（3）：23-30.

[19] 潘日萍，黃文君. 基于3DTOF的景深測(cè)量系統(tǒng)的原理研究與系統(tǒng)設(shè)計(jì)[J]. 太原師范學(xué)院學(xué)報(bào)：自然科學(xué)版，2017（4）：44-48.

[20] 牛群峰，曹一帆，王莉，等. 基于TW-TOF的UWB室內(nèi)定位技術(shù)與優(yōu)化算法研究[J]. 自動(dòng)化與儀表， 2018（1）：5-9.

[21] 張棟，焦嵩鳴，劉延泉. 互補(bǔ)濾波和卡爾曼濾波的融合姿態(tài)解算方法[J]. 傳感器與微系統(tǒng)，2017， 36（3）：62-65.

[22] 許晶晶，郭培源，董小棟，等. 改進(jìn)互補(bǔ)濾波在六旋翼飛行器中的應(yīng)用[J]. 傳感器與微系統(tǒng)，2018，37（1）：157-160.

（責(zé)任編輯：黃 ?。?/p>