電動VTOL飛行器雙目立體視覺導(dǎo)航方法

張博翰 蔡志浩 王英勛

(北京航空航天大學(xué) 無人駕駛飛行器設(shè)計研究所，北京100191)

電動VTOL(Vertical Take-Off and Landing)飛行器在建筑物內(nèi)部、城區(qū)、叢林等復(fù)雜環(huán)境中執(zhí)行偵查、監(jiān)視等任務(wù)時，其飛行控制面臨著一些新的問題.首先，這類無人機的導(dǎo)航要更加智能化，無人機需要對周圍環(huán)境一定程度的認知，當障礙物不存在時應(yīng)能夠預(yù)先形成比較理想的飛行軌跡控制飛行.其次，這類無人機必須具備檢測和躲避未知障礙物的能力［1］.傳統(tǒng)的由慣性測量單元和GPS(Global Position System)組成的導(dǎo)航系統(tǒng)顯然無法滿足要求.

美國麻省理工學(xué)院的Abraham Bachrach等［2］采用激光測距儀和雙目視覺相結(jié)合，選取常規(guī)布局的四旋翼飛行器為驗證平臺，在簡單的室內(nèi)環(huán)境下進行了無人機自主飛行的驗證.此外，美國愛荷華州立大學(xué)航空機器人實驗室［3］采用單個攝像頭，以普通旋翼直升機為飛行平臺，實現(xiàn)了室內(nèi)的視覺導(dǎo)航、測距以及飛行姿態(tài)估計.

本文在雙目立體視覺的基礎(chǔ)上研究了無人機在走廊中的橫向位置坐標定位、檢測未知環(huán)境中的障礙物并生成避障導(dǎo)航點的方法.

1 系統(tǒng)硬件構(gòu)成

1.1 飛行器平臺

研究基于圖1所示的飛行器平臺，其寬度為99 cm，縱向槳距為 46.9 cm，全機長度 65.7 cm.它將飛翼機身與4個可傾轉(zhuǎn)的旋翼相結(jié)合，是一種靠電力驅(qū)動的、能垂直起降和高速前飛的、多旋翼式飛行器.與常規(guī)布局的旋翼式飛行器相比，其結(jié)構(gòu)更為緊湊，且可以產(chǎn)生更大的升力，特別適合在建筑物內(nèi)部、城區(qū)以及叢林中執(zhí)行監(jiān)視、偵查、損傷評估、態(tài)勢感知、通信中繼等任務(wù)，具有廣闊的應(yīng)用前景.

圖1 四旋翼飛行器

1.2 雙目立體視覺系統(tǒng)

建立了無線的雙目立體視覺系統(tǒng)，見圖2.左右兩個攝像頭通過圖3所示的控制云臺，固定在飛機上，獲取的兩路視頻由無線鏈路傳輸?shù)降孛嬲倦娔X進行處理，圖像經(jīng)過視覺導(dǎo)航系統(tǒng)處理后生成一系列航跡點傳給飛行控制系統(tǒng)的外回路.

選用的攝像頭為480線的微型攝像頭，單個重20 g，工作電壓12 V，分辨率640×480.兩個攝像頭鏡頭中心橫向距離11 cm，前向平行放置，光軸與地面平行.

圖2 雙目視覺系統(tǒng)硬件

圖3 控制云臺

2 雙目立體視覺圖像處理

雙目立體視覺系統(tǒng)算法流程如圖4所示，左右圖像經(jīng)過圖像預(yù)處理轉(zhuǎn)換為灰度圖，由攝像機標定、校正、立體匹配輸出視差圖，通過提取特征點的視差值算出其相對與攝像頭位置的三維坐標，再經(jīng)過信息融合生成無人機的航跡點.

圖4 雙目立體視覺算法流程

2.1 攝像機數(shù)學(xué)模型和立體標定

攝像機標定的目的是建立有效的攝像機成像模型，確定攝像機的內(nèi)外屬性參數(shù)，以便正確建立空間坐標系中物體的空間點與像點之間的對應(yīng)關(guān)系.雙目立體視覺系統(tǒng)中的單個攝像機的成像采用針孔攝像機數(shù)學(xué)模型來描述，即任何點Q在圖像中的投影位置q，為光心與Q點的連線與圖像平面的交點.如圖5所示.

圖5 針孔攝像機模型

物理世界中的點Q，其坐標為(X，Y，Z)，投影為點(x，y，f)，如下式所示:

式中，cx和cy為成像芯片的中心與光軸的偏移;fx和fy為透鏡的物理焦距長度與成像儀每個單元尺寸sx和sy的乘積.式(1)寫成矩陣形式為

矩陣M稱為攝像機的內(nèi)參數(shù)矩陣.

在攝像機標定過程中可以同時求出鏡頭畸變向量，對鏡頭畸變進行校正.而立體標定是計算空間上兩臺攝像機幾何關(guān)系的過程，即尋找兩臺攝像機之間的旋轉(zhuǎn)矩陣R和平移矩陣T.標定圖像選取9行6列的黑白棋盤圖，標定過程中在攝像頭前平移和旋轉(zhuǎn)棋盤圖，在不同角度獲取棋盤圖上的角點位置.假設(shè)定標板放在世界坐標系中z=0的平面上.使用zhang的方法［4］求解攝像頭的焦距和偏移，使用另外的基于Brown的方法［4］求解畸變參數(shù).圖6為由OpenCV(Open Source Computer Vision Library)繪制棋盤圖角點的圖像［4］.

圖6 繪制棋盤圖角點

2.2 立體校正

給定立體圖像間的旋轉(zhuǎn)和平移矩陣(R，T)，使用立體校正的Bouguet算法進行立體校正［4］.立體校正的目的是使兩攝像機拍攝的圖像中的對應(yīng)匹配點分別在兩圖像的同名像素行中，從而將匹配搜索范圍限制在一個像素行內(nèi).Bouguet算法將右攝像機圖像平面旋轉(zhuǎn)到左攝像機圖像平面的旋轉(zhuǎn)矩陣R分離成兩部分，即兩個合成旋轉(zhuǎn)矩陣rl和rr.每個攝像機都旋轉(zhuǎn)一半，這樣的旋轉(zhuǎn)可以讓攝像機共面但是行不對準.為了計算將左攝像機極點變換到無窮遠，并使極線水平對準的矩陣Rrect創(chuàng)建一個由極點 e1方向開始的旋轉(zhuǎn)矩陣

這個矩陣將左圖像繞著投影中心旋轉(zhuǎn)，使得極線變成水平，并且極點在無窮遠處.這樣，兩臺攝像機的行對準即可以通過以下兩個旋轉(zhuǎn)矩陣實現(xiàn):

由圖7可以看出經(jīng)過立體校正后左右兩幅圖像中的對應(yīng)匹配點已經(jīng)處于在同名像素行中了.

2.3 立體匹配

選擇區(qū)域灰度相關(guān)法進行立體匹配.其計算簡單、匹配速度快.對于無人機視覺導(dǎo)航來說，如果計算能夠以很快的速度完成，則系統(tǒng)就可以提早探測到障礙物而及時采取有利的行動.這點對于時刻處于不斷變換的環(huán)境中的無人機導(dǎo)航系統(tǒng)的穩(wěn)定性來說是十分重要的.

圖7 立體校正示意圖

選用的相似性測度因子為:像素灰度差的絕對值和［5］，簡稱 SAD.

其中Il(x，y)和Ir(x，y)分別為左圖和右圖的像素灰度值，假設(shè)匹配以左圖為參考圖，則wp為左圖中以Pl點為中心的鄰域窗口.如圖8所示，對于左圖像中的點Pl在右圖像中沿著其對應(yīng)的極線搜索匹配像素，當區(qū)域中的像素使相似性準則最小時，則認為是匹配的.

圖8 立體匹配示意圖

對于室內(nèi)走廊環(huán)境這類弱紋理場景使用灰度相關(guān)匹配算法非常容易出現(xiàn)誤匹配的情況.所以，如何最大程度提高圖像的紋理同時減少左右攝像頭獲取的圖像對之間的亮度差異是立體匹配的關(guān)鍵.為此，引入了高斯拉普拉斯濾波對圖像進行預(yù)處理［6］.

由圖9可以看出經(jīng)過高斯拉普拉斯濾波后其紋理明顯增強.視差圖上每一個值代表位于攝像頭前的某一距離值.視差越大表示距離越近.其中灰度值越大的區(qū)域亮度越高，表示與攝像頭的相對距離越近.

2.4 三維坐標恢復(fù)

圖10為經(jīng)過校正后的理想的雙目立體視覺坐標系，其坐標原點為左攝像頭投影中心，X軸由原點指向右攝像頭投影中心，Z軸垂直于攝像機成像平面指向前方，Y軸垂直于X-Z平面箭頭向下.

圖9 立體匹配示意圖

圖10 雙目立體視覺系統(tǒng)坐標系

由立體成像得到視差圖后，如果給定屏幕坐標和攝像機內(nèi)參數(shù)矩陣，二維點即可以重投影到三維中，重投影矩陣Q如下式:

其中，除c'x外的所有參數(shù)都來自于左圖像，c'x是主點在右圖像上的x坐標.如果主光線在無窮遠處相交，那么cx=c'x，并且右下角的項為0.給定一個二維齊次點和其關(guān)聯(lián)的視差d，可以將此點投影到三維:

三維坐標是(X/W，Y/W，Z/W).

3 室內(nèi)走廊中飛行器橫向坐標定位

電動VTOL飛行器在走廊中橫向位置定位實際上是判斷走廊中左右兩側(cè)墻壁與飛行器的相對距離.實現(xiàn)方法為通過角點提取獲得位于墻壁上的角點，再對左右攝像機獲取的圖像進行角點匹配.記錄下匹配角點的橫坐標值分別為xl，xr，則其視差值d=xl－xr.再由立體視覺算出其相對飛行器的三維位置坐標，這樣就可以得到飛行器在走廊中的橫向位置.

角點是像素點在其鄰域內(nèi)的各個方向上灰度變換值足夠高的點.對于Harris角點［7］，使用每點周圍小窗口的二階導(dǎo)數(shù)圖像的自相關(guān)矩陣.其定義如下式:

角點位于自相關(guān)矩陣有兩個最大特征值的地方.采用基于奇異值分解的匹配算法對檢測到的角點進行匹配［8］.圖11為對圖像左右兩側(cè)的走廊墻壁區(qū)域進行角點匹配的示意圖，匹配角點之間用白色直線相連，從圖上可以看出該方法匹配的精度較高.

圖11 角點匹配

表1為使用立體視覺方法恢復(fù)圖中匹配角點三維坐標的結(jié)果.

表1 飛行器橫向坐標定位

4 雙目立體視覺障礙物檢測

在障礙物存在的前提下無人飛行器的實時導(dǎo)航需要一個復(fù)雜的障礙物檢測系統(tǒng)，能夠及時更新障礙物的相對位置信息，供相應(yīng)的路徑規(guī)劃算法使用.這不僅關(guān)系到預(yù)定任務(wù)的順利完成還直接關(guān)系到飛行器生存能力的提高.本文利用雙目視覺系統(tǒng)構(gòu)造觀測場景的視差圖，然后從視差圖上分離出障礙物(見圖12).

圖12 走廊中障礙物視差圖

從視差圖中分離出障礙物的方法有很多種.對于位于地面上的障礙物，文獻［9－10］通過像素點的高度判斷從視差圖中去除地面像素點，剩下的點被認為是障礙物.文獻［11］使用區(qū)域生長算法分離視差圖上相近視差區(qū)域，并且判斷該區(qū)域中像素的多少決定是否是障礙物.文獻［12］通過引入一系列濾波器對視差圖進行處理，檢測細長型的障礙物效果比較理想，依次包括:地面匹配刪除、取出虛匹配和距離過濾、列像素過濾、像素密度過濾等.

本文的方法是:首先將視差圖上所有像素點的值歸一化到0～255范圍內(nèi)，然后遍歷整個視差圖，統(tǒng)計每個視差值出現(xiàn)的次數(shù)，如視差在某一范圍內(nèi)的像素點總個數(shù)超過某一閾值時則認為可能是障礙物.再由立體視覺系統(tǒng)通過恢復(fù)特征點三維坐標去除地面和墻壁像素點.圖13為使用該方法提取出的障礙物.

圖13 提取出的障礙物

通過雙目立體視覺系統(tǒng)計算得到障礙物的位置信息為:中心坐標(－49，－3，110)，單位為cm，最大寬度50 cm，高度72 cm.

5 室內(nèi)走廊中飛行器躲避障礙物

電動VTOL飛行器障礙物躲避的實現(xiàn)主要通過在存在碰撞危險的障礙物的周圍生成一系列處于安全區(qū)域的軌跡控制點，控制飛行器通過這些航跡點.

躲避障礙物的方法分如下幾個步驟:

1)因為研究對象是三維避障問題，所以首先判斷是否可以直接越過障礙物.

2)對于不能越過的障礙物通過雙目立體視覺系統(tǒng)獲取其相對距離以及大小，判斷其危險等級，危險等級應(yīng)與可通過區(qū)域的大小成反比，相對距離成反比.

3)針對危險等級最高的障礙物，考慮加入立體視覺系統(tǒng)的位置不確定性和輪廓不確定性，生成危險區(qū)域.

4)結(jié)合危險區(qū)域的范圍、無人機的尺寸以及走廊環(huán)境內(nèi)的空間，在障礙物附近生成一系列控制點，引導(dǎo)無人機安全飛過障礙物區(qū)域.

軌跡控制點的生成原則是:

1)檢測到存在碰撞危險的障礙物時，首先判斷其左右兩側(cè)存在的安全空間.如果該安全空間小于飛行器的寬度，則判定此障礙物無法躲避.

2)如果左右兩側(cè)的安全空間相同且大于飛行器寬度，則判斷飛行器與障礙物左右邊緣的角度，選擇角度小的方向飛行.其第一個導(dǎo)航點高度與飛行器當前高度相同.

3)在穿越障礙物的過程中，實時判斷飛行器前方的橫向安全空間是否明顯變大以決定是否已經(jīng)成功避障.

4)避障成功后，飛行器航向偏轉(zhuǎn)回到新的安全空間的橫向中心位置.至此，完成躲避障礙物的任務(wù).

生成的躲避障礙物導(dǎo)航點如圖14所示，圖中黑色粗線為障礙物，其右側(cè)線段分別為加入的輪廓和位置不確定，構(gòu)成碰撞危險區(qū)域.以危險區(qū)域最右側(cè)點為圓心飛行器寬度的一半為半徑畫圓，如圖所示，則虛線為生成的避障軌跡.可以看出，當電動VTOL飛行器的飛行軌跡位于圓內(nèi)時存在碰撞危險.

圖14 躲避障礙物導(dǎo)航點示意圖

6 結(jié)論

1)采用雙目立體視覺導(dǎo)航技術(shù)，結(jié)合電動VTOL飛行器的特點，可以在未知的復(fù)雜環(huán)境中實現(xiàn)飛行器的自主飛行.

2)針對室內(nèi)走廊的環(huán)境采用角點匹配的方法實現(xiàn)飛行器橫向坐標定位，為飛行器的安全飛行提供保障.

3)提出的從視差圖上提取障礙物的算法簡單，提取的障礙物區(qū)域準確.再進一步由雙目立體視覺恢復(fù)障礙物的三維坐標，可以實現(xiàn)躲避障礙物的目的.

References)

［1］Yoko Watanabe，Anthony J Calise，Eric N Johnson.Vision-based obstacle avoidance for UAVs［C］//AIAA Guidance，Navigation and Control Conference and Exhibit.South Carolina:American Institute of Aeronautics and Astronautics Press，2007:20 －23

［2］Bachrach A G.Autonomous flight in unstructured and unknown indoor enviornments［D］.Dept of Electrical Engineering and Computer Science，Massachusetts Institute of Technology.2009:119－126

［3］Koray C，Chung S J，Matthew C，et al.Monocular vision SLAM for indoor aerial vehicles［C］//The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.USA St Louis:IEEE Computer Society Press，2009:1566 －1573

［4］Gary Bradski，Adrian Kaebler.學(xué)習 OpenCV(中文版)［M］.于仕琪，劉瑞禎，譯.北京:清華大學(xué)出版社，2009:178－254 Gary Bradski，Adrian Kaebler.Learning OpenCV(Chinese version)［M］.Translated by Yu Shiqi，Liu Ruizhen.Beijng:Tsinghua University Perss，2009:178 －254(in Chinese)

［5］Kanade T，Okutomi M.A stereo matching algorithm with an adaptive window:theory and experiment［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，1994，6(9):920－932

［6］杜歆.用于導(dǎo)航的立體視覺系統(tǒng)［D］.杭州:浙江大學(xué)信息學(xué)院，2003 Du Xin.Stereovision used for navigation［D］.Hangzhou:School of Information，Zhejiang University ，2003(in Chinese)

［7］Harris C，Stephens M.A combined corner and edge detector［C］//Proceedings of the 4th Alvey Vision Conference.Manchester:Manchester University Press，1988:147 －151

［8］謝鳳英，趙丹培.Visual C++數(shù)字圖像處理［M］.北京:電子工業(yè)出版社，2008:278－367 Xie Fengying，Zhao Danpei.Visual C++digital image processing［M］.Beijing:Electronic Industry Press，2008:278 － 367(in Chinese)

［9］Nedevschi S，Danescu R，F(xiàn)rentiu D，et al.High accuracy stereovision approach for obstacle detection on non-planar roads［C］//IEEE INES.Romania:IEEE Computer Society Press，2004:211－216

［10］Lemonde T，Devy N.Obstacle detection with stereovision［C］//Proc MECHROB.Germany:IEEE Computer Society Press，2004(3):919－924

［11］Dang T，Hoffmann C.Fast object hypotheses generation using 3D position and 3D motion［C］//Int IEEE Workshop Mach Vis Intell Vehicles.San Diego:IEEE Computer Society Press，2005:56

［12］Claudio Caraffi，Stefano Cattani，Paolo Grisleri.Off-road path and obstacle detection using decision networks and stereo vision［J］.IEEE Transactions on Intelligent Transportion Systems，2007(8):607－618