電動VTOL飛行器雙目立體視覺導航方法

張博翰 蔡志浩 王英勛

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

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

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

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

1 系統硬件構成

1.1 飛行器平臺

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

圖1 四旋翼飛行器

1.2 雙目立體視覺系統

建立了無線的雙目立體視覺系統，見圖2.左右兩個攝像頭通過圖3所示的控制云臺，固定在飛機上，獲取的兩路視頻由無線鏈路傳輸到地面站電腦進行處理，圖像經過視覺導航系統處理后生成一系列航跡點傳給飛行控制系統的外回路.

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

圖2 雙目視覺系統硬件

圖3 控制云臺

2 雙目立體視覺圖像處理

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

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

2.1 攝像機數學模型和立體標定

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

圖5 針孔攝像機模型

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

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

矩陣M稱為攝像機的內參數矩陣.

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

圖6 繪制棋盤圖角點

2.2 立體校正

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

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

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

2.3 立體匹配

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

圖7 立體校正示意圖

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

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

圖8 立體匹配示意圖

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

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

2.4 三維坐標恢復

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

圖9 立體匹配示意圖

圖10 雙目立體視覺系統坐標系

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

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

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

3 室內走廊中飛行器橫向坐標定位

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

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

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

圖11 角點匹配

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

表1 飛行器橫向坐標定位

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

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

圖12 走廊中障礙物視差圖

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

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

圖13 提取出的障礙物

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

5 室內走廊中飛行器躲避障礙物

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

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

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

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

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

4)結合危險區域的范圍、無人機的尺寸以及走廊環境內的空間，在障礙物附近生成一系列控制點，引導無人機安全飛過障礙物區域.

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

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

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

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

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

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

圖14 躲避障礙物導航點示意圖

6 結論

1)采用雙目立體視覺導航技術，結合電動VTOL飛行器的特點，可以在未知的復雜環境中實現飛行器的自主飛行.

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

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

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.學習 OpenCV(中文版)［M］.于仕琪，劉瑞禎，譯.北京:清華大學出版社，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］.杭州:浙江大學信息學院，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++數字圖像處理［M］.北京:電子工業出版社，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，Frentiu 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