基于雙目視覺的SLAM四旋翼無人機

譚凱元，朱嘉林，鄧 君，王荔薔，楊家源

（東莞理工學院機械工程學院，廣東東莞 523000）

0 引言

近年來，隨著人民生活質量的提高，機器人的應用也越來越廣泛，其在不同的領域發揮著重要的作用。而且在許多地方，機器人已經可以代替人類去完成一些工作，特別是可以幫助人類完成復雜的計算工作，或者在一些對人類來說具有安全隱患的工作環境，機器人更是取代人類去完成這些危險的工作。而在諸如此類的工作環境中，機器人需要強大的同步定位和自動建圖能力，這能夠讓機器人在未知的環境下對周邊的環境進行實時的地圖構建，以便機器人在之后的進行自主導航和路徑規劃。

為了進行實時地圖的構建，所選擇的載體機器人應該為較為靈活機動的，所以現在市面上的傳統選擇為小車。但是這種SLAM 小車最大的缺點是對地形的環境、地貌有很多要求，會使地圖的構建產生相當多的缺陷，有許多地方無法建圖，大幅度會受限于地形地貌[1]。而運用無人機搭載攝像頭進行建圖可以克服如此眾多缺點，突破地形限制，更加靈活方便，同時構建的地圖也會更加立體完全。

無人機又稱無人機駕駛飛行器，是近年來發展起來的一個集空氣動力學、材料力學、自動控制技術、軟件技術為一體的高科技產品。無人機廣泛用于空中偵察、監視、通信、反潛、電子干擾等領域。目前世界上有30余個國家和地區研制出了50多種無人機，無人機型號超過300余種，有55個國家裝備了無人機[2]。

本文所設計的雙目SLAM 無人機是一種應用于復雜地形地貌下建模的機器人[3]，其上面搭載了雙目攝像頭，進行對周邊環境的信息進行采集，并裝載了樹莓派將雙目攝像頭采集到的信息篩選處理并轉化成點云圖和輸出，同時樹莓派還進行與飛控板之間的信息交流，實時控制無人機位姿、運動位移。最終將地圖完整地構建之后，再通過不同的算法規劃和篩選出無人機從起點到終點的最優路徑解[4]，從而實現雙目SLAM 無人機的自主導航功能。

1 雙目SLAM無人機整體結構

1.1 控制部分

無人機的控制部分是一塊樹莓派4B 主控板，樹莓派4B 主控板相當于一個微處理器，具有獨立的CPU芯片，可以實現本項目所需的建圖功能。通過在樹莓派中燒錄一個Ubuntu16.04系統，并安裝ROS 系統，將ORB-SLAM 建圖，定位避障，自主導航各種算法集成在一起，無人機能夠實現自動建圖，自主導航的目標。

1.2 信息采集部分

無人機的信息采集部分主要是采用USB 雙目攝像頭，USB雙目攝像頭是一款集雙目圖像及IMU 為一體的高清圖像傳感器，該設備支持Windows、Linux、Android 等多種平臺，且支持C/C++、Python 等各種編程語言，不僅支持PC 桌面操作系統，還支持嵌入式系統，如樹莓派開發板。適合應用于ORBSLAM建圖。

1.3 外觀部分

其主體外觀如圖1所示。

圖1 雙目無人機主體外觀

2 雙目攝像頭的標定與匹配

在無人機自動導航中必需的點云圖基于深度圖，在室內，自主機器人一般使用深度RGB-D 相機獲得深度圖；而在室外，雙目相機沒有尺度問題，安裝在無人機上來計算深度圖正合適。

2.1 相機標定

在使用攝像頭之前，首先要進行相機標定[5]，在各種圖像測量的過程中以及機器視覺應用中，為了重建和識別物體，需要確定空間物體表面任意點的三維幾何位置與其在圖像中的對應點之間的相互關系，進而建立相機成像的幾何模型，而這些幾何模型參數就是攝像頭的相機參數。這些參數分別是相機的內、外參和畸變參數，攝像機內參反映的是攝像機從三維空間到二維圖像的投影關系，即坐標系到圖像坐標系之間的投影關系；攝像機外參決定了攝像機坐標系和世界坐標系之間相對位置關系，即其旋轉R和平移T關系。

攝像機標定就是從世界坐標系轉換為攝像機坐標系，再由攝像機坐標系轉換為圖像坐標系的過程，也就是求得最終投影矩陣的過程。

（1）從世界坐標系到攝像機坐標系：是三維點到三維點的轉換，包括旋轉R和平移T（相機外參）等參數。如圖2所示。

圖2 世界坐標系到攝像機坐標系的變換

（2）相機坐標系轉換為圖像坐標系：這是三維點到二維點的轉換，包括焦距f（相機內參）等參數。如圖3所示。

圖3 相機坐標系到圖像坐標系的變換

2.2 相機匹配

為了得到深度圖需要使用立體匹配算法，利用了雙目立體匹配原理。圖4所示為雙目圖像立體匹配的具體流程。

圖4 雙目立體匹配原理流程

利用雙目相機計算稠密的深度需要經過兩個步驟，為圖像矯正和視差計算，具體如下。

步驟1：圖像矯正分為圖像去畸變矯正和圖像立體匹配[6]。圖像畸變指的是圖像邊緣的直線由于相機的畸變導致在成像的時候變成了曲線。圖像去畸變則是利用圖像的畸變參數對圖像畸變進行校正，檢測方法是將原本畸變的圖像在通過畸變校正后，變回直線，如圖5 所示。這個也稱作相機的內參數校準（焦距為fx、fy；相機中心為cx、cy），通常調用ROS 里面的包就可以實現了。

圖5 圖像去畸變

步驟2：立體畸變校正。立體匹配是指把左右圖像旋轉到同一個平面（這里說同一個平面不太準確，應該是兩個平行的平面），使得左右兩幅圖片的光軸平行，檢測方法是判斷左右目圖像中的同一個像素點是否在同一水平線上面，是立體視覺研究中的關鍵部分，其目標是在兩個或多個視點中匹配相應像素點，計算視差。通過建立一個能量代價函數，對其最小化來估計像素點的視差，求得深度。

立體矯正是校準相機的外參數實現的，這個也可以通過ROS 下的相機校準包實現，只需要輸入棋盤格的size 即可。校準的參數包括基線fx、相機的R矩陣、投影矩陣P矩陣。

一般來說epi＜0.2就已經可以進行建圖了，epi＜0.15算是很好了，如圖6所示。

圖6 立體矯正

在獲得相機的內參數和外參數以后，可以調用opencv的函數對雙目的圖像進行矯正，如下所示[7]：

本項目采用（9×7）的黑白格棋盤，如圖8 所示。未標定前雙目相機的左右視角畫面如圖7 所示，黑白格以及環境有明顯的扭曲變形，且從棋盤可以看出左右視角畫面不在同一平面上。標定校準之后的黑白格如圖8所示。

圖7 未標定前雙目相機

圖8 標定校準后雙目攝像機

3 ORB-SLAM算法

ORB-SLAM 算法是一種基于ORB 特征的三維定位與地圖構建算法（SLAM），具有地圖初始化和回環檢測的功能，并優化了關鍵幀選取和地圖構建的方法[8]。

ORB-SLAM 共有3 個線程，分別是跟蹤線程（Tracking）、地圖構建線程（Local Mapping）和回環檢測線程（Loop Closing）。

3.1 跟蹤線程（Tracking）

跟蹤線程運用ORB（Oriented Fast and Rotated Brief）算法對圖像中的關鍵點快速創建特征向量。第一步是使用的是FAST 算法查找圖像中的關鍵點，提取出ORB 特征；第二步是初始姿態估計，根據相鄰幀之間的信息估計出相機的初始位姿，并通過全局重定位來初始化位姿；第三步是姿態優化，利用相鄰幀尋找更多的特征匹配，優化當前幀的位姿；第四步是選取關鍵幀，通過篩選冗余關鍵幀來決定當前幀是否可以作為關鍵幀。

3.2 地圖構建線程（Local Mapping）

地圖構建線程ORB-SLAM 關鍵步驟，即建立點云地圖。第一步是加入跟蹤線程中篩選出的關鍵幀；第二步是驗證最近加入的地圖點（去除異常）；第三步是生成新的地圖點，利用三角法生成新的地圖云點，三角化的云點必須經過地圖點云篩選測試，保證留下的點云都是能被跟蹤的；第四步是局部束調整（Local Bundle Adjustment），添加待優化的位姿頂點，進行優化迭代，檢測和排除誤差較大的異常；第五步是驗證關鍵幀，構建局部地圖時檢測并刪除重復幀。

3.3 回環檢測線程（Loop Closing）

回環檢測可以解決累計漂移，校正整個軌跡的形狀[9]。第一步是選取相似幀，在相似幀提取關鍵點和局部特征，判斷是否先前已經構建過地圖的位置；第二步是檢測閉環，獲得回環的累積誤差；第三步是融合三維點，融合重復的點云，并且在Covisibility Graph 中插入新的邊以連接閉環；第四步是圖優化（傳導變換矩陣），用本質圖（Essential Graph）去優化位姿圖，將閉環的誤差分散到整個圖中。

4 ORB-SLAM建圖

4.1 稀疏點云地圖

首先進行位姿的初始化和優化，通過提取圖像中的特征點，根據相鄰幀之間的信息，將其各自的特征點進行匹配，從而估計出相機的初始位姿，并通過全局重定位來初始化位姿。

得出相機的位姿后，可以通過計算得出視覺里程計。圖10 中相機圖像上的綠色小方塊為當前提取的圖像特征點，即ORB特征。圖中的黑點和紅點代表環境的稀疏地圖（紅色為局部地圖點，表示當前路標；黑色為全局地圖點，代表歷史路標）。圖中的綠線為相機的運動軌跡，綠色方框代表相機的實時位姿；藍色方框為相機的歷史位姿，代表相機運動過程中的空間位置（即關鍵幀）[10]。最后將生成的ORB-SLAM2 文件修改，基于ROS系統，將相機的位姿和視覺里程計生成為節點并發布，用于后面的稠密點云地圖、八叉樹地圖及代價地圖建圖。

圖10 實驗室稀疏點云圖

4.2 稠密點云地圖

稠密點云地圖的建圖系統分為了3 個節點，如圖11 所示。第一個節點作為驅動節點，負責采集圖像的數據；第二個節點為姿態節點，主要運行ORB-SLAM 做姿態估計；第三個節點作為建圖節點，是系統核心，收集第一和第二節點的建圖節點并接收攝像頭傳回的數據和實時位姿，最后進行點云拼接。

圖11 稠密點云地圖建圖步驟

4.3 八叉樹地圖

相較于各種點云圖，八叉樹地圖更加靈活且易于壓縮，并且還可以隨時更新[11]。如圖12 所示，每個立方體都會不斷地均勻分成8 塊，直到變成最小的方塊為止，而整個最大的方塊，可以看成是一個根節點，而最小的方塊就是末端的“葉子節點”。于是在八叉樹地圖中，當從下一層的節點往上走時，這個地圖的體積每經過一層就可以擴大8倍。如圖12所示。基于八叉樹地圖的特性，八叉樹地圖有以下4個優點。

圖12 八叉樹地圖

（1）八叉樹地圖可以描述所建地圖中的任意位置的狀態，可以用樹中的節點來表示空間中的任意位置。

（2）同時，八叉樹地圖更易于更新和維護，他將各個位置的狀態信息保存于節點，當需要修改這些位置的狀態時，則只需要修改節點中的變量即可。

（3）八叉樹地圖還可以根據所需要得到地圖的精度而隨時調整。

（4）八叉樹地圖需要的內存相較于點云地圖而言小很多，當對地圖的精度要求不高時，可以用低分辨率的構建方式來節省空間。這更加有利于用在機器人的導航上。

5 路徑規劃

路徑規劃是雙目SLAM 無人機能夠實現自主導航的重要過程，主要功能是讓無人機在地圖內自主規劃出一條從起點到終點的無碰撞的最優路徑解。路徑規劃由兩部分組成，分別是全局路徑規劃和局部路徑規劃。全局路徑規劃是當已經知道了起點、終點的位置和無人機所處的地圖時，通過一系列算法計算出一條從起點到終點的最優路徑解，更加適用于靜態的環境；而局部路徑規劃則是依靠攝像頭等傳感器模塊感知實時的信息，通過一系列算法實現對不斷變化的障礙物和突發事件進行及時的避險，更具實時性和實用性，更加適用于動態的環境[12]。全局路徑規劃和局部路徑規劃這兩種路徑規劃方法的組成了無人機完備的路徑規劃系統。

5.1 全局路徑規劃

傳統使用的全局路徑規劃算法有4 種，分別是Dijkstra 算法、RRT 算法、BFS 算法和A*算法。Dijkstra 算法的特點是其支持的搜索地圖面積較大，該算法從起點向外擴散，生成最優路徑解，但是其缺點明顯，開始建圖之前會將整張地圖全部搜索完，所以效率相較于其他3 種算法比較低，但仍然可以生成最優路徑；RRT算法是一種基于樹狀結構的路徑規劃算法，從起點開始向外擴散出一個個的樹狀結構，但這種算法最大的缺點是無法對規劃的結果進行預判，并且每次規劃的結果可能都不一樣；BFS 算法的特點是搜索空間小，但是效率高，不能保證找到一條最優路徑解；A*算法在Dijkstra 算法的基礎上加入了啟發函數，用于引導其搜索方向；并且結合了BFS算法的優點，相較于其他路徑規劃算法，使用更加普遍，因此本項目中使用A*算法作為無人機的全局路徑規劃。

5.2 局部路徑規劃

當無人機在建立好了所需地圖后，通過A*算法可以規劃出一條大致可行的路線，但面對實際環境中各種突發的狀況，例如突然出現了動態障礙或者建圖時未被檢測到的障礙，就需要通過局部的路徑規劃來進行路徑的調整。于是本項目采用DWA 算法進行局部路徑規劃。DWA 算法會先建立機器人的速度運動模型，建立之后對機器人的速度進行采樣，最后使用評估函數完成局部規劃。

6 雙目SLAM無人機優化與測試

6.1 系統測試環境

無人機以ROS 系統為載體，將ORB-SLAM 建圖，定位避障，自主導航各種算法集成在一起。在ROS系統中，最小的進程單元就是節點，一個軟件包里可以有多個可執行文件，可執行文件在運行之后就成了一個進程，這個進程在ROS中就叫做節點[13]。在無人機中建圖、定位、循跡等就是不同的節點。ROS系統將節點聲明為話題，然后以發布話題的形式將數據傳輸，最后使用不同的ROS通信機制實現數據的交互。本項目采用雙目攝像頭，以雙目坐標系作為點云地圖的基礎坐標系，圖13所示為本項目的雙目SLAM無人機。

圖13 雙目SLAM無人機

6.2 雙目攝像頭的標定與使用

雙目攝像頭的標定與使用在第2節已敘述，圖14所示為標定后的拍攝效果。

圖14 攝像頭標定后的拍攝效果

6.3 ORB-SLAM 地圖創建

實驗室稀疏點云地圖構建如圖15 所示。圖中相機圖像上的綠色小方塊為當前提取的圖像特征點，即ORB 特征。圖中的黑點和紅點代表環境的稀疏地圖（紅色為局部地圖點，表示當前路標；黑色為全局地圖點，代表歷史路標）。綠色方框代表相機的實時位姿；藍色方框為相機的歷史位姿，代表相機運動過程中的空間位置（即關鍵幀）[10]。

圖15 實驗室稀疏點云地圖

7 結束語

本文設計了一種基于雙目視覺的SLAM 四旋翼無人機，可以在未知的環境下對周邊的環境進行實時的地圖構建，并且完成自主定位導航。并且由于無人機靈活機動的特性，可以不受限于地形，在許多傳統SLAM 小車去不了的地方，也能進行建圖。在設計的過程中通過各種渠道查閱關于ORB-SLAM、無人機的知識；對雙目攝像頭的標定匹配，以及矯正；通過使用樹莓派將雙目攝像頭采集到的信息進行篩選處理后，轉化成點云圖再輸出，同時樹莓派還進行與飛控板之間的信息交流，實時控制無人機位移；將ORB-SLAM 算法融入到ROS 系統與建圖當中，最終實現實時點云地圖的建立。