張咪,趙勇,布樹輝,張臻煒,楊君
西北工業(yè)大學(xué) 航空學(xué)院,西安 710072
近年來,隨著具有體積小、重量輕、隱蔽性好等特點的無人飛行器被廣泛應(yīng)用,迎來了無人機(jī)(UAV)技術(shù)大爆炸時代。其中多旋翼無人機(jī)系統(tǒng)更是因為結(jié)構(gòu)簡單、穩(wěn)定性好、起飛降落方式靈活、自身占用空間小、轉(zhuǎn)向性能良好等優(yōu)點,成為民/商用及研究領(lǐng)域的熱門。面對越來越多的無人機(jī)民用新需求,比如動態(tài)追蹤拍攝[1]、快遞行業(yè)定點投送物品[2]等,對微型無人機(jī)的自主精準(zhǔn)降落技術(shù)[3]提出了新的挑戰(zhàn)。
基于全球定位系統(tǒng)(GPS)導(dǎo)航[4]的自動降落是目前最常用的方法,但GPS引導(dǎo)自動降落有一定的不足。首先,不做特殊處理的GPS定位精度僅能達(dá)到10 m,對于地形復(fù)雜的區(qū)域,比如建筑群密集的城市,無人機(jī)很可能因為導(dǎo)航誤差在低空進(jìn)近區(qū)域墜毀;其次,GPS信號受非空氣介質(zhì)干擾大,在遮擋物較多的區(qū)域,比如森林,會造成誤差增大甚至信號丟失;專業(yè)級高精度GPS設(shè)備成本昂貴,不具備經(jīng)濟(jì)實用性。
鑒于GPS的精度不高,有學(xué)者陸續(xù)提出了基于視覺的自動降落技術(shù)。其中,2014年上海理工大學(xué)的夏云龍等提出基于聲源時延和氣壓高度傳感器的無人機(jī)精確降落技術(shù)[5],該項技術(shù)在降落點安置聲源,在無人機(jī)上構(gòu)造基于全向性駐極體麥克風(fēng)的正四面體麥克風(fēng)接收陣列[6],利用聲源時延技術(shù)計算無人機(jī)的俯仰角和偏航角,再通過無人機(jī)上安裝的氣壓高度傳感器感知無人機(jī)的高度,從而精確引導(dǎo)無人機(jī)降落。2016年南京航空航天大學(xué)吳賽飛等提出基于紅外視覺的無人機(jī)自動著陸引導(dǎo)系統(tǒng)[7],通過在無人機(jī)上設(shè)計紅外視覺識別標(biāo)志,利用停機(jī)坪上的紅外相機(jī)對無人機(jī)目標(biāo)進(jìn)行識別和定位并完成自主降落。同年,哈爾濱工業(yè)大學(xué)樊瓏提出一種在無人機(jī)上完成所有計算的視覺定位方法[8],但此方法也需要借助超聲波測距儀測量高度,再通過機(jī)載相機(jī)尋找停機(jī)坪上黑白相間的圓環(huán)降落標(biāo)志的圓心計算出無人機(jī)的位置,完成自動降落控制。
然而以上方法存在一些共同的不足:
1) 僅支持無人機(jī)和停機(jī)坪一對一關(guān)系,無法支持多無人機(jī)同時降落。但在快遞運輸定點投送物品等實際應(yīng)用中,需要多架無人機(jī)同時進(jìn)行配送任務(wù),以提高工作效率。
2) 易受降落環(huán)境干擾,對降落環(huán)境要求較高,很難完成精準(zhǔn)降落。比如紅外視覺識別標(biāo)志在強(qiáng)光下很容易被干擾,或產(chǎn)生錯誤的識別。氣壓高度計極易受到溫度影響,而超聲波測距對氣溫變化和噪聲很敏感,環(huán)境因素的影響都可能導(dǎo)致精度不高。
3) 在完成整個自主降落的過程中,需要其他設(shè)備(比如紅外激光探測儀、氣壓高度傳感器和超聲波測距儀等[9-10])輔助完成定位、測高等任務(wù)。
為了解決上述問題,我們設(shè)計了一種多層嵌套Marker的階層降落標(biāo)識,并針對該標(biāo)識提出了有針對性的快速檢測識別及定位算法,實現(xiàn)了一套自主精準(zhǔn)降落系統(tǒng)。經(jīng)電腦仿真試驗和無人機(jī)自主降落試驗驗證了系統(tǒng)的可行性和實效性。與現(xiàn)有系統(tǒng)相比,此系統(tǒng)具備以下特點:
1) 無須依賴其他設(shè)備就可以完成精確的相對定位、定高、定姿態(tài)等工作。
2) 抗干擾能力強(qiáng)、計算效率高,可在低成本計算平臺上快速實現(xiàn)圖像處理、無人機(jī)位姿計算等任務(wù)。
3) 所設(shè)計的算法可以為多無人機(jī)提供定位和降落信息。
本系統(tǒng)由硬件和軟件兩部分構(gòu)成,系統(tǒng)的整體框架與流程如圖1所示。硬件部分主要包括機(jī)載攝像機(jī)、機(jī)載CPU處理器和機(jī)載飛行控制器。
對于本系統(tǒng),在降落過程中將機(jī)載攝像機(jī)拍攝的實時圖像傳給機(jī)載CPU處理器進(jìn)行處理,檢測并校驗停機(jī)坪上的降落標(biāo)識估計無人機(jī)的位置和姿態(tài);將相對位置、姿態(tài)信息發(fā)送給飛行控制器,從而控制無人機(jī)逐步逼近停機(jī)坪,最終實現(xiàn)無人機(jī)全自主降落。
圖1 系統(tǒng)總體框架Fig.1 Overall framework of system
無人機(jī)降落標(biāo)識為用于輔助無人機(jī)視覺定位的基礎(chǔ),其設(shè)計需要滿足以下要求:
1) 特征鮮明突出,容易被識別且識別應(yīng)該盡可能魯棒準(zhǔn)確。
2) 為了能夠在不同高度,或者多尺度成功定位,其應(yīng)具有多尺度識別能力。
3) 該特征能夠較好地輔助無人機(jī)進(jìn)行相對定位。
為了使降落標(biāo)識符合以上3點要求,提出了多層嵌套Marker的階層降落標(biāo)識設(shè)計,能夠很好地解決一般情況下無人機(jī)對降落標(biāo)識的需求。
Marker設(shè)計規(guī)則:每個Marker由N×N的黑白方格組成(根據(jù)編碼的容量來選擇N的大小,一般情況下N為5~7);每個Marker最外側(cè)方格顏色一致,并根據(jù)內(nèi)側(cè)(N-2)×(N-2)的區(qū)域進(jìn)行編碼,組成獨一無二的無符號整型編碼值(ID)。根據(jù)最外側(cè)顏色可將Marker分為白邊和黑邊兩種,對于黑邊Marker中的每個方格,白色多的是1,黑色多的是0,對于白邊Marker則相反。無人機(jī)降落標(biāo)識示例如圖2所示。
如圖2(a)所示的單個Marker設(shè)計,其ID對應(yīng)二進(jìn)制011 101 111,即239。由于識別此Marker在4個方向上觀察不一致,為保證其唯一性,一般不允許Marker存在對稱性,同時選擇4個解析結(jié)果中的最小值作為Marker的ID,并記錄獲得最小值所在的方向,從而確定唯一的頂點順序。雖然為了滿足通用性,Marker采用了最簡單的編碼規(guī)則,但這樣的設(shè)計同樣可以滿足利用冗余信息來使得ID檢測更魯棒的需求。
為了能在多尺度上提供定位信息,停機(jī)坪上設(shè)計的降落標(biāo)識由單個或多個Marker鑲嵌而成。在降落標(biāo)識(本身為Marker)內(nèi)嵌套設(shè)計規(guī)則相同但規(guī)格較小的Marker,所有的Marker采用統(tǒng)一的方格數(shù)目。嵌套Marker有很多規(guī)格,分布在階層降落標(biāo)識的內(nèi)部。這里的多種規(guī)格是指嵌套Marker的大小不一樣,即邊長不一樣,并且每個嵌套Marker內(nèi)部的黑色、白色區(qū)域分布也是不同的,這樣每個嵌套Marker的ID互不相同。雖然多個Marker之間可以相互嵌套,但嵌套不能影響對父Marker的ID判斷,為滿足這個要求,對于父Marker的每個單元格改動不能超過三分之一。針對停機(jī)坪上設(shè)計的降落標(biāo)識的中心GPS位置、各停機(jī)位Marker的ID和頂點位置已知。
圖2 無人機(jī)降落標(biāo)識示例Fig.2 Landing markers of UAVs
如圖2(b)和圖2(c)分別是為單個無人機(jī)和多個無人機(jī)泊位停機(jī)坪上設(shè)計的階層降落標(biāo)識示例。圖2(b)的單泊位停機(jī)坪中,停機(jī)位采用5×5的Marker作為階層降落標(biāo)識,編碼為9位。其外側(cè)大Marker的ID對應(yīng)數(shù)字為239,內(nèi)側(cè)Marker對應(yīng)二進(jìn)制000 011 110,即30。對于圖2(c) 的多泊位停機(jī)坪,每個停機(jī)位采用7×7的Marker作為階層降落標(biāo)識,編碼為25位。多泊位停機(jī)坪采用漢明編碼[11],可以保證在每行出現(xiàn)一個二進(jìn)制錯誤時給予糾正,同時也保證ID之間至少存在每行一個以上的二進(jìn)制不同,相當(dāng)于文本所用編碼中的多個編碼可被看做同一個編碼,并映射到統(tǒng)一的頂點。
如圖3所示,當(dāng)無人機(jī)接收到飛行控制的降落指令(停機(jī)坪GPS位置信息、無人機(jī)待降落停機(jī)位Marker ID),到達(dá)設(shè)定的停機(jī)坪發(fā)現(xiàn)域(停機(jī)坪上方半徑3~10 m的空域內(nèi))準(zhǔn)備降落時,系統(tǒng)啟動自主降落程序,通過邊緣檢測和輪廓提取、確定候選停機(jī)位Marker、檢測并校驗Marker ID、實時位姿估計等步驟完成無人機(jī)精確降落。
步驟1邊緣檢測和輪廓提取
無人機(jī)到達(dá)停機(jī)坪發(fā)現(xiàn)域后,以固定速率邊下降邊利用機(jī)載攝像機(jī)不斷拍攝采集實時圖像,并將其轉(zhuǎn)化為灰度圖,利用Canny算子或自適應(yīng)閾值方法[12-13]得到二值化的邊緣分布圖,利用腐蝕膨脹等算法[14-15]進(jìn)行濾噪,然后提取圖像的輪廓信息(包含停機(jī)坪以及停機(jī)坪內(nèi)所有停機(jī)位Marker的輪廓)。
步驟2確定候選停機(jī)位Marker
對于得到的輪廓信息,濾除兩類輪廓:① 停機(jī)坪中停機(jī)位Marker內(nèi)部必定會有其他輪廓,因此將所有內(nèi)部無子輪廓的輪廓去除;② 構(gòu)成輪廓的像素點個數(shù)應(yīng)超過一定數(shù)量,以去除噪點的干擾。對于剩下的輪廓,選取外部為凸四邊形的輪廓作為候選停機(jī)位Marker,并求取其4個頂點在圖像中的坐標(biāo)。
步驟3檢測并校驗Marker ID
圖3 自主精準(zhǔn)降落算法流程圖Fig.3 Flow chart of algorithm of autonomous precision landing
對每一個候選停機(jī)位Marker,將其通過射影變換映射到固定大小的方形區(qū)域,其中方形區(qū)域的大小應(yīng)為N的整數(shù)倍,一般寬高都為4×N個像素,這樣對于每個方格都存在16個像素點用于二進(jìn)制統(tǒng)計。將候選停機(jī)位Marker映射到方形區(qū)域后,利用計算復(fù)雜度低、效率高的K-Means算法[16]將區(qū)域內(nèi)的像素進(jìn)行二值化并進(jìn)一步利用二值化后的圖像進(jìn)行解碼(白色多為1,黑色多為0),得到一個N×N的二值矩陣。對于二值矩陣,首先判斷其最外圈是否為全0或全1,不滿足則認(rèn)為此候選停機(jī)位Marker不正確,直接丟棄。然后根據(jù)內(nèi)側(cè)(N-2)×(N-2)區(qū)域可以解碼得到候選停機(jī)位Marker的ID及朝向信息。將候選停機(jī)位Marker的ID與無人機(jī)接收到的降落指令中待降落停機(jī)位Marker的ID進(jìn)行匹配,選擇匹配成功的候選停機(jī)位Marker作為待降落停機(jī)位Marker,否則停機(jī)位Marker定位失敗。
步驟4實時位姿估計
實際應(yīng)用中在地面上選定并繪制停機(jī)坪時,停機(jī)位Marker的空間位置及大小是已知信息,即已知待降落停機(jī)位Marker在三維空間的頂點坐標(biāo),根據(jù)一系列三維點和圖像二維點的對應(yīng)關(guān)系,結(jié)合相機(jī)參數(shù)對無人機(jī)相對于待停機(jī)位Marker坐標(biāo)系的位置和姿態(tài)進(jìn)行求解。
圖4為相機(jī)投影模型,對于世界坐標(biāo)系中某個坐標(biāo)為(XW,YW,ZW)的三維空間點P,利用相機(jī)位姿R、t(R為旋轉(zhuǎn)矩陣,t為平移向量)將其轉(zhuǎn)化為相機(jī)坐標(biāo)系下的點P′(XC,YC,ZC):
圖4 相機(jī)投影模型Fig.4 Projection model of camera
(1)
已知相機(jī)內(nèi)參K,將點P′投影到圖像I1上,得到圖像特征點坐標(biāo)向量[u1,v1,1]T(以歸一化平面齊次坐標(biāo)表示)為
(2)
式中: (u0,v0)為主點的坐標(biāo);fx和fy分別為x和y方向的焦距。
假設(shè)某個Marker頂點的三維空間點坐標(biāo)向量為Pi=[Xi,Yi,Zi]T,其投影到圖像上的像素坐標(biāo)向量為pi=[ui,vi]T,根據(jù)圖4的相機(jī)投影模型可得到像素位置與空間點位置關(guān)系為
(3)
式中:si為圖像上二維像素點向量pi在相機(jī)坐標(biāo)系下的深度,矩陣形式為
sipi=K(RPi+t)
(4)
由于相機(jī)位姿R、t未知以及觀測點pi的噪聲,式(4)的等式存在誤差。因此,為了尋找最好的相機(jī)位姿,構(gòu)建最小二乘問題,使誤差最小化為
(5)
此最小二乘問題的誤差項,是將Marker頂點的像素坐標(biāo)(觀測到的圖像投影位置)與其三維空間點按照相機(jī)模型得到的投影位置相比較得到的誤差,稱之為重投影誤差。通過Levenberg-Marquardt(LM)優(yōu)化算法[17]迭代優(yōu)化最小二乘問題求得無人機(jī)此時最優(yōu)相機(jī)位姿R、t。
步驟5無人機(jī)飛行控制
根據(jù)無人機(jī)與待停機(jī)位Marker坐標(biāo)系相對位置關(guān)系調(diào)整無人機(jī)的Yaw方向(繞Y方向旋轉(zhuǎn),即偏航角),并不斷下降。
無人機(jī)降落到一定位置時,停機(jī)位Marker已經(jīng)不能完全呈現(xiàn)在相機(jī)平面內(nèi),此時嵌套Marker起作用,無人機(jī)只需要按照步驟2和步驟3的Marker ID解析算法,進(jìn)行嵌套Marker的ID匹配即可(該停機(jī)位Marker內(nèi)的嵌套Marker)。
為了讓無人機(jī)保持較好的控制效果,不同階段采用不同的控制方式。當(dāng)無人機(jī)降落到一定高度時,圖像定位效果較好,即使進(jìn)行一些運動調(diào)整也不容易發(fā)生定位失敗,因此采用比例-積分-微分(PID)控制器[18-19]進(jìn)行速度環(huán)控制,使得無人機(jī)始終跟隨平臺進(jìn)行速度調(diào)整運動。而當(dāng)無人機(jī)降落高度過低,來不及進(jìn)行位置調(diào)整時,為了保證無人機(jī)能較平穩(wěn)精準(zhǔn)地降落在停機(jī)坪上,特別對于移動降落平臺,采用PID控制器對加速度環(huán)進(jìn)行控制,從而完成精確降落。
停機(jī)位Marker定位失敗有兩種情況:① 確定候選停機(jī)位Marker時未找到符合篩選條件的凸四邊形輪廓;② 找到候選停機(jī)位后,未檢測或識別到待降落停機(jī)位Marker ID。
定位失敗后,無人機(jī)盤旋數(shù)秒捕獲實時圖像,若仍無法完成準(zhǔn)確定位,無人機(jī)攀升并不斷嘗試重新對停機(jī)位Marker定位以完成降落。
為了測試本文的無人機(jī)自主精準(zhǔn)降落系統(tǒng),首先在PC機(jī)上對從圖像數(shù)據(jù)獲取、算法實現(xiàn)到閉環(huán)控制整個過程進(jìn)行了完整仿真測試。其中整個仿真系統(tǒng)主要包含無人機(jī)動力學(xué)仿真、視景仿真、算法模塊封裝和數(shù)據(jù)可視化4部分。
1) 無人機(jī)動力學(xué)仿真
采用無人機(jī)剛體動力學(xué)仿真模擬無人機(jī)實體及飛控部分。無人機(jī)的受力主要分為重力、空氣作用力和無人機(jī)推力。重力在世界坐標(biāo)系下始終不變,空氣作用力由無人機(jī)速度和風(fēng)速決定,設(shè)定一個高斯過程,絕對值不超過1.5 m/s的風(fēng)擾動,算法給出的控制決定無人機(jī)推力。無人機(jī)動力學(xué)仿真模塊以100 Hz的固定頻率進(jìn)行加速度、速度和位置、姿態(tài)等關(guān)鍵參數(shù)的更新。
2) 視景仿真
模擬攝像頭采集過程。假設(shè)攝像頭安裝在無人機(jī)機(jī)體正下方視角向下,根據(jù)無人機(jī)動力學(xué)仿真的實時位姿及攝像機(jī)內(nèi)參實時渲染得到虛擬的攝像頭曝光圖片。
3) 算法模塊封裝
將信息輸入接口、控制輸出接口和核心算法部分封裝成統(tǒng)一模塊,代碼復(fù)用方便開發(fā),并避免移植中的錯誤。
4) 數(shù)據(jù)可視化
使用Qt和OpenGL完成整個自主降落系統(tǒng)的三維可視化分析[20],實現(xiàn)無人機(jī)位置、實時渲染圖片、風(fēng)速方向和算法位姿估計與控制輸出等部分的可視化,方便觀察整個系統(tǒng)流程。
圖5是針對圖2(b)設(shè)計的降落標(biāo)識,無人機(jī)模擬自主降落仿真系統(tǒng)的界面顯示,其中上側(cè)四旋翼無人機(jī)是無人機(jī)當(dāng)前真實位姿,左下角是仿真渲染得到的圖像及算法處理檢測到的Marker標(biāo)記,中間朝向下的視角框表示算法計算得到的無人機(jī)位姿,正下方是世界坐標(biāo)系下擺放的降落板及模擬的當(dāng)前風(fēng)速表征線。從圖5(a)側(cè)視圖和圖5(b)俯視圖可以看出此時降落高度已完全檢測和識別停機(jī)位的所有Marker(包括嵌套Marker),無人機(jī)的位姿估計比較準(zhǔn)確。
圖5 自主降落系統(tǒng)仿真驗證Fig.5 Simulation verification of autonomous landing system
為測試算法在真實環(huán)境下的性能特性,搭建了試驗環(huán)境,并進(jìn)行多次測試。所采用的無人機(jī)是自己組裝的四旋翼,飛控硬件采用Pixhawk,飛控軟件采用APM,飛機(jī)上裝載一個NanoPi2(1.4 GHz四核A9處理器搭配1 G內(nèi)存)作為視覺計算平臺,飛控和計算機(jī)之間通過UART接口通信,相機(jī)為VGA(Video Graphics Array)分辨率的USB相機(jī)。
手動控制無人機(jī)起飛,到達(dá)指定空域(相機(jī)視野內(nèi)出現(xiàn)停機(jī)位),開始進(jìn)入自主精準(zhǔn)降落程序,相機(jī)將得到的含有降落標(biāo)識(Marker)的圖像信息傳送給視覺計算平臺,實時計算出無人機(jī)與Marker的相對位置關(guān)系,將相對位置坐標(biāo)傳入飛控系統(tǒng),飛控控制無人機(jī)飛行完成整個自主精準(zhǔn)降落過程。在下降過程中,圖像采集頻率是30 幀/s,分辨率是640×480。
圖6分別展示了本系統(tǒng)無人機(jī)在地面固定位置和在移動車頂降落的試驗。地面和車頂上放置的降落板上的降落標(biāo)識如圖2(b)所示。試驗部分具體采用如下硬件配置:
圖6 四旋翼無人機(jī)自主降落試驗驗證Fig.6 Autonomous landing test verification of quadrotor UAV
1) 機(jī)架與飛控
采用如圖6(a)和圖6(b)中的設(shè)計,在機(jī)架最外側(cè)4個機(jī)臂下安置腳架,盡可能讓無人機(jī)在各種降落平臺保持穩(wěn)定。而為了讓算法直接省略對姿態(tài)中Pitch(繞X方向旋轉(zhuǎn),俯仰角)和Roll(繞Z方向旋轉(zhuǎn),滾轉(zhuǎn)角)的計算,飛控將通過串口實時發(fā)布最新姿態(tài)信息,并接受算法輸出的控制指令完成相應(yīng)動作。
2) 攝像頭
對于自主降落系統(tǒng),對圖像分辨率要求不高,但幀率越高越好,并要求適應(yīng)各種光線情形。圖像采集部分使用OV2710芯片為核心的廣角USB攝像頭模塊,其特點是在低照度的環(huán)境下成像效果較好,并且分辨率為640×480時,幀率最高達(dá)到100 Hz。
從圖6(a)和圖6(b)中可以看出無人機(jī)成功并準(zhǔn)確地降落到降落板上。圖6(c)和圖6(d)是在車頂上搭建了移動降落平臺,并進(jìn)行了多次降落試驗,無人機(jī)都能很好地降落到車頂。
圖7 無人機(jī)相對位置變化圖Fig.7 Variation of relative positions of UAVs
靜止降落試驗進(jìn)行了20次,圖7是某次靜止降落試驗中,無人機(jī)在位于7 m高度下降時20 s內(nèi)X、Y、Z方向相對于起飛點的偏移量,可以看出自主精準(zhǔn)降落算法得到的相對位移量隨著高度的下降呈變小趨勢,當(dāng)無人機(jī)到達(dá)地面時,水平和垂直方向偏移量能達(dá)到10 cm以內(nèi)。經(jīng)過統(tǒng)計20次試驗的偏移量,發(fā)現(xiàn)總體偏離誤差不超過 10 cm,相比紅外等降落標(biāo)志易受強(qiáng)光、溫度或灰塵等影響,本系統(tǒng)不易受光照、溫度、噪聲等環(huán)境因素干擾。即使受風(fēng)影響,也只有在少數(shù)情況下偏離誤差會超過10 cm,如圖8所示。
圖8 靜止降落試驗總體偏離誤差散點圖Fig.8 Total deviation error scatter of static landing test
為了評估本文自主精準(zhǔn)降落系統(tǒng)的性能,在NanoPi2機(jī)載運算平臺上對系統(tǒng)各部分的時間消耗進(jìn)行統(tǒng)計,如表1所示。從表中可以看出系統(tǒng)的效率非常高,邊緣輪廓提取部分因為其處理了大量像素點,耗時最長,其他的部分都只是對少量數(shù)據(jù)進(jìn)行處理,故耗時相對較短。
表1 系統(tǒng)工作時各部分在NanoPi2的耗時統(tǒng)計Table 1 Statistics of time duration of each part of system in NanoPi2
1) 本文提出的階層降落標(biāo)識采用多層嵌套的Marker,很容易被檢測識別,并能夠在多尺度上提供其定位信息,且可以達(dá)到更魯棒的需求。
2) 在進(jìn)行充分的仿真試驗和實飛測試后,證明了本文提出的無人機(jī)自主精準(zhǔn)降落系統(tǒng)不易受到環(huán)境干擾,其魯棒性和效率較高,且對計算性能要求極低,可為多個不同無人機(jī)同時提供精準(zhǔn)降落服務(wù)。
3) 本文提出的自主精準(zhǔn)降落系統(tǒng)依托于階層降落標(biāo)識進(jìn)行相對定位,忽略了其他環(huán)境信息。后期可綜合考慮其他環(huán)境紋理信息,更魯棒地提供更準(zhǔn)確的定位信息。