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

（東莞理工學(xué)院機械工程學(xué)院，廣東東莞 523000）

0 引言

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

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

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

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

1 雙目SLAM無人機整體結(jié)構(gòu)

1.1 控制部分

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

1.2 信息采集部分

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

1.3 外觀部分

其主體外觀如圖1所示。

圖1 雙目無人機主體外觀

2 雙目攝像頭的標(biāo)定與匹配

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

2.1 相機標(biāo)定

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

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

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

圖2 世界坐標(biāo)系到攝像機坐標(biāo)系的變換

（2）相機坐標(biāo)系轉(zhuǎn)換為圖像坐標(biāo)系：這是三維點到二維點的轉(zhuǎn)換，包括焦距f（相機內(nèi)參）等參數(shù)。如圖3所示。

圖3 相機坐標(biāo)系到圖像坐標(biāo)系的變換

2.2 相機匹配

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

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

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

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

圖5 圖像去畸變

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

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

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

圖6 立體矯正

在獲得相機的內(nèi)參數(shù)和外參數(shù)以后，可以調(diào)用opencv的函數(shù)對雙目的圖像進行矯正，如下所示[7]：

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

圖7 未標(biāo)定前雙目相機

圖8 標(biāo)定校準(zhǔn)后雙目攝像機

3 ORB-SLAM算法

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

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

3.1 跟蹤線程（Tracking）

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

3.2 地圖構(gòu)建線程（Local Mapping）

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

3.3 回環(huán)檢測線程（Loop Closing）

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

4 ORB-SLAM建圖

4.1 稀疏點云地圖

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

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

圖10 實驗室稀疏點云圖

4.2 稠密點云地圖

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

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

4.3 八叉樹地圖

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

圖12 八叉樹地圖

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

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

（3）八叉樹地圖還可以根據(jù)所需要得到地圖的精度而隨時調(diào)整。

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

5 路徑規(guī)劃

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

5.1 全局路徑規(guī)劃

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

5.2 局部路徑規(guī)劃

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

6 雙目SLAM無人機優(yōu)化與測試

6.1 系統(tǒng)測試環(huán)境

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

圖13 雙目SLAM無人機

6.2 雙目攝像頭的標(biāo)定與使用

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

圖14 攝像頭標(biāo)定后的拍攝效果

6.3 ORB-SLAM 地圖創(chuàng)建

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

圖15 實驗室稀疏點云地圖

7 結(jié)束語

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