基于視覺的四旋翼室內(nèi)懸停與定位控制

傅君 劉子龍

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

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

DOIDOI：10.11907/rjdk.181731

中圖分類號：TP319

文獻(xiàn)標(biāo)識碼：A 文章編號：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 引言

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

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

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

1 四旋翼飛行器平臺

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

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

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

2 懸停及定位算法

2.1 光流法航速估計

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

3 算法實現(xiàn)

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

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

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

3.2 無人機定位控制算法

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

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

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

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

4 結(jié)語

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

參考文獻(xiàn)：

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

[2] 李力，王耀南.旋翼飛行機器人視覺定位方法及系統(tǒng)[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] 鄭偉.基于視覺的微小型四旋翼飛行機器人位姿估計與導(dǎo)航研究[D].合肥：中國科學(xué)技術(shù)大學(xué)，2014.

[7] 呂強，馬建業(yè).基于視覺伺服的小型四旋翼無人機自主飛行控制研究進(jìn)展[J].科技導(dǎ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] 曹美會，鮮斌.基于視覺的無人機自主定位與控制系統(tǒng)[J].信息與控制，2015（33）：11-30.

[10] 姚西，亢巖.圖像透視特征提取方法及其無人機視覺導(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] 鄭馳，項志宇，劉濟林.融合光流和特征點匹配的單目視覺里程計[J].浙江大學(xué)學(xué)報，2014，48（2）：25-50.

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

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

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

[16] 林志林，張國良，姚二亮，等.動態(tài)場景下基于運動物體檢測的立體視覺里程計[J].光學(xué)學(xué)報，2017，37（11）： 111-115

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

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

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

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

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

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