傅君 劉子龍

摘 要：為使四旋翼飛行器具有更好的室內(nèi)定點(diǎn)懸停效果及定位精度，提出一種基于視覺輔助與四旋翼飛行器慣性傳感器數(shù)據(jù)融合的控制算法。利用機(jī)體下視攝像機(jī)獲得光流航速信息，與慣性傳感器姿態(tài)信息數(shù)據(jù)融合實(shí)現(xiàn)良好的室內(nèi)懸停效果。機(jī)體前視攝像機(jī)通過(guò)ORB算法將當(dāng)前幀與關(guān)鍵幀進(jìn)行特征點(diǎn)匹配，以提高四旋翼飛行器的室內(nèi)定位精度。將PARROT公司的ARDrone 2.0四旋翼飛行器作為實(shí)驗(yàn)平臺(tái)， 采用OpenCV軟件對(duì)圖像進(jìn)行處理，對(duì)控制算法進(jìn)行驗(yàn)證，結(jié)果表明：基于光流和慣性傳感器姿態(tài)數(shù)據(jù)的融合確保了四旋翼飛行器控制的安全性，提高了飛行器懸停效果和定位精度。

關(guān)鍵詞：視覺；四旋翼飛行器；室內(nèi)懸停與定位；光流；ORB特征提??；OpenCV

DOIDOI：10.11907/rjdk.181731

中圖分類號(hào)：TP319

文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1672-7800（2018）010-0144-04

英文摘要Abstract：In order to achieve a better indoor fixed-point hover effect and positioning accuracy for quadrotorcrafts，a control algorithm based on visual aided data fusion with quadrotorcraft inertial sensors was proposed.We utilize the body down camera to obtain the optical flow speed information，and the inertial sensor pose information data fusion achieves a good indoor hover effect.The ORB algorithm is employed so that the body front looking camera can match the current frame with the key frame to achieve the feature point matching of the four-rotor aircraft and indoor positioning accuracy.In this paper，the ARDrone 2.0 quadrotor from PARROT was used as the experimental platform.The above control algorithm was verified by using OpenCV to process the acquired images.The results show that the data fusion based on optical flow and inertial sensor attitude data ensures the control safety of of the quadrotor and improves the aircraft′s hover effect and positioning accuracy.

英文關(guān)鍵詞Key Words：vision；quadrotor crafts；indoor hover and position；optical flow；ORB；OpenCV

0 引言

四旋翼無(wú)人機(jī)飛行器具有可垂直起降、機(jī)動(dòng)性能好的優(yōu)點(diǎn)，定位是其發(fā)展的關(guān)鍵因素。無(wú)人機(jī)定位指利用自身傳感器確定無(wú)人機(jī)在飛行環(huán)境中相對(duì)于慣性坐標(biāo)系的位置和姿態(tài)信息準(zhǔn)確的位姿估計(jì)，是實(shí)現(xiàn)無(wú)人機(jī)避障、軌跡規(guī)劃及目標(biāo)跟蹤等復(fù)雜飛行任務(wù)的前提和基礎(chǔ)[1]。傳統(tǒng)的四旋翼飛行器定位主要借助機(jī)身自帶的慣性傳感器單元（IMU）和全球定位系統(tǒng)（GPS）的數(shù)據(jù)融合，但在一些特殊環(huán)境如室內(nèi)、樓群之間，由于GPS信號(hào)弱，很難實(shí)現(xiàn)傳統(tǒng)的飛行器定位[2]。而基于視覺的定位方法僅使用機(jī)載攝像頭作為外部傳感器，具有體積小、重量輕、價(jià)格低、精度高等優(yōu)勢(shì)，視覺導(dǎo)航逐漸成為四旋翼無(wú)人機(jī)自主控制研究的主要方向[3-5]。

麻省理工學(xué)院和賓夕法尼亞大學(xué)是基于視覺圖像的自主定位導(dǎo)航四旋翼無(wú)人飛行器研究的典型代表。麻省理工學(xué)院的實(shí)時(shí)室內(nèi)自主飛行器試驗(yàn)環(huán)境主要用于多無(wú)人飛行器的協(xié)同控制與定位。賓夕法尼亞大學(xué)的HMX4通過(guò)安裝在地面的攝像頭測(cè)量四旋翼無(wú)人飛行器底部標(biāo)記的位置和，得到其姿態(tài)角與位置信息，進(jìn)而對(duì)其進(jìn)行控制與定位[6-8]。清華大學(xué)通過(guò)光流傳感器獲取無(wú)人機(jī)的水平速度信息，實(shí)現(xiàn)無(wú)人機(jī)的定點(diǎn)懸停功能。而光流法測(cè)量相對(duì)運(yùn)動(dòng)的積分不是絕對(duì)運(yùn)動(dòng)的積分，不可避免地具有位置漂移現(xiàn)象，不適合長(zhǎng)時(shí)間導(dǎo)航。除了基于視覺的飛行器控制，還包括視覺同步定位與建圖（SLAM）[9]。SLAM主要依靠并行跟蹤與建圖，其基本原理是通過(guò)跟蹤地圖中的關(guān)鍵幀及關(guān)鍵幀中的特征點(diǎn)進(jìn)行位置和姿態(tài)估計(jì)，建立的地圖可隨定位過(guò)程逐步擴(kuò)充。SLAM算法對(duì)四旋翼飛行器系統(tǒng)要求較高，需同時(shí)測(cè)量飛行過(guò)程中的圖像數(shù)據(jù)與距離信息，這對(duì)機(jī)載系統(tǒng)與激光測(cè)距儀要求較高，會(huì)提高飛行器成本與算法難度[10]。

結(jié)合以上兩種方法的優(yōu)缺點(diǎn)，本文提出基于光流傳感器與慣性測(cè)量傳感器數(shù)據(jù)融合，結(jié)合飛行器動(dòng)力學(xué)模型實(shí)現(xiàn)四旋翼飛行器的懸?？刂葡到y(tǒng)。機(jī)載前視攝像頭對(duì)關(guān)鍵幀的特征進(jìn)行室內(nèi)定位，實(shí)現(xiàn)對(duì)四旋翼飛行器的室內(nèi)控制。

1 四旋翼飛行器平臺(tái)

本文采用法國(guó)PARROT公司的ARDrone 2.0四旋翼飛行器為實(shí)驗(yàn)平臺(tái)，該飛行器原本設(shè)計(jì)用于增強(qiáng)現(xiàn)實(shí)（VR）游戲，但設(shè)計(jì)人員并不想將該飛行器局限于此。PARROT公司開源了ARDrone的SDK，希望更多的人對(duì)ARDrone進(jìn)行開發(fā)。ARDrone 2.0機(jī)身如圖1所示，該飛行器前部有帶90°角鏡頭的攝像機(jī)，可拍攝最高720P（1 280×720）視頻，采樣頻率為15幀/s；底部有一個(gè)微型攝像頭，可捕捉地面360P視頻，采樣頻率為30幀/s，底部的超聲波測(cè)距儀可獲得高度信息。飛行器本身為一個(gè)LINUX嵌入式系統(tǒng)，通過(guò)連接WiFi，將圖像傳至iPhone、iPad或電腦等上位機(jī)進(jìn)行處理[11]。

雖然該飛行器是一款商業(yè)級(jí)無(wú)人機(jī)，但PARROT公司提供了一套成熟的開發(fā)包（SDK）供二次開發(fā)。本文根

據(jù)其SDK中提供的API接口及控制飛行器的各種上位機(jī)AT COMMAND，通過(guò)C++在Visual Studio 2017構(gòu)建項(xiàng)目，并加載圖像處理OpenCV庫(kù)，對(duì)四旋翼飛行器采集的圖像信息進(jìn)行處理，從而對(duì)相關(guān)算法進(jìn)行可行性驗(yàn)證。

2 懸停及定位算法

2.1 光流法航速估計(jì)

人眼觀察物體時(shí)，相對(duì)運(yùn)動(dòng)物體在視網(wǎng)膜上會(huì)形成一系列連續(xù)變化的影像，即變化的信息不斷“流過(guò)”視網(wǎng)膜，這樣一種光的“流”被稱作光流。光流法就是利用視頻中像素點(diǎn)的灰度相對(duì)變化確定各個(gè)位置的運(yùn)動(dòng)情況，即研究圖像灰度的時(shí)域變化與實(shí)際物體運(yùn)動(dòng)的關(guān)系。光流反映了在時(shí)間間隔dt內(nèi)由于運(yùn)動(dòng)所造成的圖像變化[12]。光流場(chǎng)是通過(guò)二維圖像表示物體點(diǎn)的三維運(yùn)動(dòng)速度場(chǎng)，即表示圖像中各位置的運(yùn)動(dòng)情況。由Lucas和Kanade[13]提出的光流法應(yīng)用較廣，在OpenCV中也有相關(guān)函數(shù)，逐漸應(yīng)用在無(wú)人機(jī)懸?？刂浦小?/p>

3 算法實(shí)現(xiàn)

3.1 光流航速估計(jì)與姿態(tài)信息數(shù)據(jù)融合

ARDrone 2.0機(jī)體下方的攝像頭和超聲波傳感器組成的光流計(jì)，可通過(guò)光流法求得四旋翼飛行器的水平速度，但當(dāng)水平地面上地標(biāo)物較少或光線較暗時(shí)，由于圖像處理存在延遲，可能在姿態(tài)角發(fā)生突變時(shí)光流法求得的速度不準(zhǔn)確。機(jī)體自身搭載的慣性傳感器可以很好地估算姿態(tài)角，也可粗略估計(jì)四旋翼飛行器懸停過(guò)程中的速度，但該速度實(shí)際存在較大的誤差和積分漂移[18]。

本文將光流航速與慣性傳感器測(cè)量的姿態(tài)角及速度信息進(jìn)行融合，算法思路如下：若姿態(tài)角沒有發(fā)生突變，則機(jī)體速度信息以光流航速估計(jì)值為準(zhǔn)。若慣性傳感器測(cè)得的機(jī)體姿態(tài)角發(fā)生突變，則飛行器速度信息以慣性傳感器測(cè)量估計(jì)的速度平滑濾波值為準(zhǔn)，在姿態(tài)角變化穩(wěn)定時(shí)則恢復(fù)光流航速估計(jì)。圖3表示光流航速估計(jì)和慣性測(cè)量單元數(shù)據(jù)融合的結(jié)果。

3.2 無(wú)人機(jī)定位控制算法

ARDrone機(jī)底攝像頭可以測(cè)得航速與慣性單元數(shù)據(jù)，機(jī)體內(nèi)部搭載了內(nèi)環(huán)控制環(huán)節(jié)。本文設(shè)計(jì)了串級(jí)PID控制器，結(jié)合光流計(jì)航速及機(jī)體底部超聲波傳感器，實(shí)現(xiàn)對(duì)四旋翼飛行器懸停時(shí)的穩(wěn)定控制，控制系統(tǒng)如圖4所示。

從圖4可以看出，在機(jī)載內(nèi)置的內(nèi)環(huán)姿態(tài)控制基礎(chǔ)上，本文采取的串級(jí)PID控制系統(tǒng)加入了基于光流計(jì)和慣性測(cè)量單元的速度環(huán)以及超聲波測(cè)量?jī)x、前視攝像頭?；贠RB特征檢測(cè)算法的高度控制環(huán)，通過(guò)串級(jí)PID控制器實(shí)現(xiàn)更加穩(wěn)定及準(zhǔn)確的四旋翼飛行器懸?？刂?，提高了四旋翼飛行器室內(nèi)定位精度[19-21]。前部攝像頭提取關(guān)鍵幀程序流程見圖5。

通過(guò)視覺圖像處理輔助分析，懸停狀態(tài)下四旋翼飛行器高度數(shù)據(jù)如圖6所示。

從圖6可以看出，四旋翼飛行器懸停到相應(yīng)高度時(shí)，在起飛過(guò)程中圖像的突變會(huì)導(dǎo)致高度的判斷不準(zhǔn)確。但當(dāng)飛行器接近懸停位置時(shí)，借助ORB算法的特征點(diǎn)提取和匹配算法，四旋翼飛行器的懸停高度逐漸趨于平穩(wěn)，該實(shí)驗(yàn)結(jié)果表明，基于視覺的四旋翼懸?？刂扑惴尚?。

4 結(jié)語(yǔ)

基于視覺的四旋翼飛行器控制方案解決了四旋翼飛行器在室內(nèi)的懸停及定位控制問題，可應(yīng)用于室內(nèi)自主巡航、跟蹤等實(shí)驗(yàn)，但還存在以下問題：如圖像處理雖然實(shí)現(xiàn)了一些基礎(chǔ)的位置檢測(cè)，垂直方向上有超聲波測(cè)距儀輔助更好地實(shí)現(xiàn)z軸的位置控制，但在水平方向上的位置控制存在較大誤差。由于水平距離測(cè)量的缺失導(dǎo)致無(wú)法精確控制水平位置。ORB特征點(diǎn)提取與匹配在飛行器控制算法中存在一定延時(shí)，今后在實(shí)驗(yàn)中要降低特征點(diǎn)提取算法的圖像處理時(shí)間，更好地滿足四旋翼飛行器實(shí)時(shí)性要求。

參考文獻(xiàn)：

[1] 肖支才.室內(nèi)四旋翼無(wú)人飛行器定位導(dǎo)航的研究現(xiàn)狀與關(guān)鍵技術(shù)[J].飛航導(dǎo)彈，2014（8）：56-75.

[2] 李力，王耀南.旋翼飛行機(jī)器人視覺定位方法及系統(tǒng)[J].機(jī)器人，2016（38）：6-11.

[3] PLINVAL H，MORIN P.Visual servoing for underactuated VTOL UAVs：a linear ，homography-based framework[C].Proceedings of IEEE International conference on Robotics and Automation.Shanghai：IEEE，2011：3004-3010.

[4] ABEYWARDENA D，WANG Z.Visual-inertial fusion for quadrotor micro air vechicles with improved scale obsercability[C].Proceedings of IEEE International conference on Robotics and Automation.Karlsruhe：IEEE，2013：3133-3138.

[5] EHASAN A，MOHAMMAD A.Attitude Control using an extended calssifier system algorithm for offline autotuning of a PID controller[C].IEEE RSI/ISM International Conference on Robotics and Mechatronics，2014：925-930.

[6] 鄭偉.基于視覺的微小型四旋翼飛行機(jī)器人位姿估計(jì)與導(dǎo)航研究[D].合肥：中國(guó)科學(xué)技術(shù)大學(xué)，2014.

[7] 呂強(qiáng)，馬建業(yè).基于視覺伺服的小型四旋翼無(wú)人機(jī)自主飛行控制研究進(jìn)展[J].科技導(dǎo)報(bào)，2016（34）：19-26.

[8] HUANG R，TAN P，CHEN B M.Monocular vision-based autonomous navigation system on a toy quadcopter in unknown environments[C].2015 International Conference on Unmanned Aircraft Systems ，2015：1260-1269.

[9] 曹美會(huì)，鮮斌.基于視覺的無(wú)人機(jī)自主定位與控制系統(tǒng)[J].信息與控制，2015（33）：11-30.

[10] 姚西，亢巖.圖像透視特征提取方法及其無(wú)人機(jī)視覺導(dǎo)航中的應(yīng)用[J].現(xiàn)代電子技術(shù)，2014，37（2）：16-20.

[11] PISKORSKI S.Drone Developer Guide[EB/OL].2010-10-05.http：//ardrone.parrot.com.

[12] 鄭馳，項(xiàng)志宇，劉濟(jì)林.融合光流和特征點(diǎn)匹配的單目視覺里程計(jì)[J].浙江大學(xué)學(xué)報(bào)，2014，48（2）：25-50.

[13] 魏偉波，潘振寬，李媛媛，等.小位移光流計(jì)算的中值公式[J].儀器儀表學(xué)報(bào)，2011，32（10）：2256-2260.

[14] 劉冠延，郭明，岳曉奎.特征點(diǎn)提取算法比較及四旋翼平臺(tái)驗(yàn)證[J].哈爾濱商業(yè)大學(xué)學(xué)報(bào)，2015，31（1）：155-170.

[15] 呂科，施澤南，李一鵬.微型無(wú)人機(jī)視覺定位與環(huán)境建模研究[J].電子科技大學(xué)學(xué)報(bào)，2017，46（3）： 544-550.

[16] 林志林，張國(guó)良，姚二亮，等.動(dòng)態(tài)場(chǎng)景下基于運(yùn)動(dòng)物體檢測(cè)的立體視覺里程計(jì)[J].光學(xué)學(xué)報(bào)，2017，37（11）： 111-115

[17] 毛星云，冷雪飛.OpenCV3編程入門[M].北京：電子工業(yè)出版社，2015.

[18] （澳）科克.機(jī)器人學(xué)、機(jī)器視覺與控制：MATLAB算法基礎(chǔ)[M].劉榮，譯.北京：電子工業(yè)出版社，2015.

[19] 劉剛，王彪，曹云峰.PID/ADRC控制器在四旋翼無(wú)人飛行控制中的應(yīng)用[J].云南民族大學(xué)學(xué)報(bào)，2014，23（2）：152-154.

[20] 時(shí)夢(mèng)瑤，李健奇.基于視覺引導(dǎo)的室內(nèi)四旋翼飛行器控制系統(tǒng)研究與設(shè)計(jì)[J].自動(dòng)化與儀表，2018，33（2）：50-62.

[21] 季元揚(yáng)，陳躍東.基于 PID 改進(jìn)型互補(bǔ)濾波[J].控制工程，2017，24（5）：61-75.

（責(zé)任編輯：杜能鋼）