基于RGB-D攝像頭的SLAM導航算法研究

王增喜，張慶余，賈 通，張蘇林

（中汽數據（天津）有限公司，天津300393）

經過近30年的理論研究與技術沉淀，SLAM 技術取得了豐富的成果，特別是近年來視覺SLAM 取得的一系列突破性進展，正在讓移動機器人實現自主導航與人機共融的愿望逐漸變成現實。目前很多移動機器人利用激光雷達進行SLAM（同時定位與地圖創建），但由于激光雷達價格昂貴，為降低成本，本文采用視覺傳感器替代激光雷達，以到達SLAM的效果。

本方案的目標為用視覺傳感器代替激光雷達，利用RGB-D 攝像頭進行視覺SLAM，利用RGB 攝像頭提供場景數據，深度攝像頭提供距離數據。利用RGB-D 攝像頭提供的彩色數據，通過彩色數據提取特征，并根據特征建立視覺里程計，完成地圖的創建，然后對地圖進行優化，形成最終的三維點云數據，將三維點云數據轉換成可以用于導航避障的地圖[1-3]。

1 方案流程

本方案的目的是將深度數據轉換為激光雷達數據并封裝成消息進行廣播。主要功能分為4 個模塊：特征提取與配準、位姿估計、位姿優化、地圖生成與轉化。其框架以及流程如圖1 所示。

圖1 RGB-D 攝像頭導航流程Fig.1 RGB-D camera navigation process

2 功能模塊

為利用RGB-D 攝像頭SLAM 并導航的主要架構，主要功能模塊包括：圖像與參數獲取、特征提取與匹配、相機位姿估計、閉環檢測、位姿圖優化、地圖構建與轉換等。輸入為RGB-D 攝像機提供的彩色數據以及深度數據，輸出為可供導航和避障的三維地圖，系統架構如圖2 所示。

圖2 系統架構圖Fig.2 System architecture diagram

下面對整個系統的各個功能模塊進行簡單描述。

2.1 圖像與參數獲取

RGB-D 攝像頭可以提供RGB 攝像頭輸出的彩色圖像數據，深度攝像頭輸出的對應的深度數據，配準后即可根據兩幅圖像獲得每個點對應的三維坐標。同時根據相關API 獲得對應的相機內參數，包括焦距、分辨率等。

2.2 特征提取與匹配

由于ORB 特征的計算速度明顯優于SURF 和SIFT，且計算量較小，因此本方法利用ORB 特征進行特征提取。ORB 采用FAST 算法來檢測特征點。并利用BRIEF 算法計算描述子。其中，FAST 核心思想就是拿一個點跟它周圍的點比較，如果它和其中大部分的點都不一樣就可以認為它是一個特征點，FAST 特征檢測的速度快，如圖3 所示。

圖3 FAST 特征提取Fig.3 FAST feature extraction

ORB 采用BRIEF 算法來計算一個特征點的描述子。BRIEF 算法的核心思想是在關鍵點P 的周圍以一定模式選取N 個點對，把這N 個點對的比較結果組合起來作為描述子。該描述子特有的二進制串的表現形式不僅節約了存儲空間，而且大大縮短了匹配的時間[4-5]。

2.3 相機位姿估計

2.3.1 改進RANSAC 算法

由于經過ORB 特征提取后進行的特征匹配只是粗匹配，因此可能存在誤匹配，首先采用隨機抽樣一致性算法（random sample consensus，RANSAC）去除誤匹配。對于ORB 匹配后的兩幀圖像，必然存在誤匹配的情況，同時兩幀圖像必然只存在一個模型，因此需要進行RANSAC 進行計算其中的模型，將除局內點外的點作為離群點進行刪除，即刪除了誤匹配。

針對傳統的RANSAC 算法在精確匹配中計算量大、效率低等問題，本文采用改進的一種結合改進粒子群算法的RANSAC 精確匹配算法。首先，利用微分流形中單位分解的知識將圖像分成幾個部分；其次，利用改進粒子群算法選擇最佳葉節點進行模型參數估算；最后，保留N 個最佳葉節點，返回最優模型，統計幾個局部的精確匹配點[6-7]，如圖4所示。

圖4 RANSAC 效果圖Fig.4 RANSAC renderings

2.3.2 ICP 計算相機位姿

ICP 算法是從源點集P 上的每個點先計算出目標點集U 的每個點的距離，配準過程就是求取2 個坐標系間的旋轉和平移變換矩陣，使得來自U 與P的同源點間距離最小。ICP 的算法流程如圖5 所示。

2.4 閉環檢測

圖5 ICP 算法流程Fig.5 ICP algorithm flow chart

ICP 算法可以輸出相鄰兩幀圖像之間的旋轉矩陣以及平移矩陣，若該相鄰兩幀計算獲得的角度偏差為1°，但隨著時間的推移，若每幀都有或多或少的誤差，則一段時間后整個地圖可能會出現較大的誤差，且相鄰幀的位姿偏移較小，因此為了加快運算速度，選取關鍵幀并利用關鍵幀進行閉環檢測。

閉環檢測是指當移動機器人到達一個先前已經構建過地圖的位置時，能夠判斷出這個位置已經構建過地圖，然后對原來構建的地圖進行更新、修正。

2.4.1 關鍵幀提取

本方法首先提取ORB 特征，然后利用詞袋算法（bag of words，BoWs）建立視覺詞典。

BoWs 模型是信息檢索領域常用的文檔表示方法，現也常用于視覺分類。首先，對于一幅圖像將其僅僅看作是若干個視覺詞匯的集合，每個詞匯的出現都是相對獨立的，不依賴于其他單詞是否出現，且單詞之間沒有順序。其中，相似的視覺詞匯都用一個單詞表示，轉化為聚類問題，對于常見的、沒有區分度的詞匯將被舍棄。BoWs 模型通常包含3 個步驟：特征檢測、特征表示、詞匯表生成。

對于一幅新的圖像，首先提取ORB 特征。根據聚類結果將圖像用視覺詞匯進行表示，建立視覺詞匯直方圖，統計每個視覺詞匯在圖像中出現的頻率，生成視覺詞匯直方圖，利用直方圖進行圖像的相似度的計算。

（1）利用ORB 特征從采集到的圖像中提取視覺詞匯向量，這些向量代表的是圖像中局部不變的特征點；

（2）將所有特征點向量集合到一塊，利用K-Means算法進行聚類。合并詞義相近的視覺詞匯，構造一個包含K 個詞匯的單詞表；

（3）統計單詞表中每個單詞在某一幅圖像中出現的次數，從而將圖像表示成為一個K 維數值向量[8-11]。

其計算過程如圖6 所示。

圖6 BoWs 模型建立過程Fig.6 BoWs model building process

其中，K-Means 聚類算法為離線訓練，算法基于樣本間的相似度，輸入參數為K，即把所有樣本分為K 個簇，目的是最大化類內相似度，最小化類間相似度，可以根據距離的遠近，對詞義相近的詞匯進行合并。對于兩幀圖像，可根據兩幀圖像依據事先建立的視覺詞典分別計算無權詞匯直方圖，通過TFIDF 對詞匯表加上權重，生成最終的直方圖。利用兩個直方圖的相似度代表兩幀圖像的相似度。

對于TF-IDF，訓練中只計算和保存單詞的IDF值，即單詞在眾多圖像中的區分度。TF 則是從實際圖像中計算得到各個單詞的頻率。單詞的TF 越高，說明單詞在這幅圖像中出現的越多；單詞的IDF 越高，說明單詞本身具有高區分度。二者結合起來，即可得到這幅圖像的BoWs 描述。

對于兩幀相鄰圖像幀，需要利用ICP 算法對求兩幀圖像間的旋轉和平移矩陣，但因為需要做閉環，因此若每幀圖像的特征都進行存儲來做閉環，則數據量很龐大，同時受到幀頻以及移動速度的影響，相鄰若干幀的特征變化相對較小。因此利用一些有代表性的幀來作為關鍵幀以提升計算速度。

2.4.2 位姿圖建立

通過將檢測到的ORB 特征點，找到Covisibility Graph 中與之相連的關鍵幀Kc，進行特征匹配，然后將匹配到的特征點進行三角化。對于沒有匹配上的點，又與其它關鍵幀中未被匹配的特征點進行匹配。ORB 特征點對三角化后，檢查正向景深、視差、反投影誤差和尺度一致性，這時才得到地圖點。最后，在所有步驟結束后，會將關鍵幀記錄到數據庫列表中。

2.4.3 閉環檢測

由于幀間的位姿估計極有可能存在誤差，所以程序跑上一段時間后就不知道跑到哪里去了，隨著時間推移，累積誤差可能越來越大，因此需要利用閉環檢測來降低累積誤差，其主要過程為：當前幀不僅和前一幀比較，而是和其余的多個關鍵幀比較，找到相似之處，以此判斷是否回到了之前來到的地方[12-13]。

2.5 三維點云

閉環檢測以及觀測到的路標（landmark）來構建地圖的邊，完成Covisibility Graph 的構建后，就要調整機器人的位姿去盡量滿足這些邊構成的約束。本方法采用g2o（general graph optimization）位姿圖優化的方法進行地圖的優化，降低累積誤差，提升地圖的準確性。生成的地圖為三維點云，三維點云很稠密，存儲占用了大量的空間，但其實其中的眾多數據點是不必要的，即不重要的信息。并且三維點云中由于位姿的偏差可能包含了部分重疊的場景，因此不僅浪費空間，而且很難用于導航與避障。

octoMap 是根據三維點云坐標（x，y，z）進行獲取位置信息，通過三維體素進行以點云為依據構建真實地圖，將三維點云分為occupy/free/unknown 三個區域進行呈現。octoMap 有自己的空間，從-32768～+32768 空間域。點云的坐標空間是正常的空間，這兩個空間是需要進行轉換才能對應。

2.6 八叉樹

每個小方塊都有一個數描述它是否被占據。每個方塊都可以被分為8 個子塊，直至被細分到最小塊，最小塊的大小決定了地圖的分辨率。在最簡單的情況下，可以用0 或1 兩個數表示。通常還是用0～1 之間的浮點數表示它被占據的概率。0.5 表示未確定，越大則表示被占據的可能性越高，反之亦然。由于它是八叉樹，那么1 個節點的8 個孩子都有一定的概率被占據或不被占據，如圖7 所示。

圖7 八叉樹示意圖Fig.7 Schematic diagram of octree

用樹形結構的優點：當某個節點的子結點都“占據”或“不占據”或“未確定”時，就可以把它給剪掉。換句話說，如果沒必要進一步描述更精細的結構（孩子節點）時，只要一個粗方塊（父親節點）的信息就夠了，這可以省去很多的存儲空間，因為不用存一個“全八叉樹”，八叉樹的內部結構如圖8所示。

圖8 八叉樹內部結構Fig.8 Internal structure of octree

八叉樹中的父親節點占據概率，可以根據孩子節點的數值進行計算。比較簡單的是取平均值或最大值。如果把八叉樹按照占據概率進行渲染，不確定的方塊渲染成透明的，確定占據的渲染成不透明的，然后按照高度區別不同的顏色，可以發現障礙物，并且對于障礙物有一個占據比例。

3 實驗

3.1 硬件平臺

搭建基于LeTMC-520 RGB-D 深度攝像頭的硬件平臺，在Ubuntu16.04 LTS 環境下對算法進行實際測試，如圖9 所示。

圖9 RGB-D 深度攝像頭Fig.9 RGB-D depth camera

3.2 實驗結果

從圖10 運行效果來看，RGB-D SLAM 算法能夠很好地進行初始化，且高效實時運行，RGB-D 相機在特征點跟蹤階段比較穩定，無丟幀或者失幀的情況。因此實驗證明本文采用方案效率高，魯棒性好。

圖10 運行效果圖Fig.10 Running effect diagram

4 結語

本文基于RGB-D 攝像頭，根據視覺SLAM 導航的相關算法和技術，建立了一套便于機器人和自動駕駛車導航的八叉樹地圖。本文重點是在獲取圖像與參數獲取、特征提取與匹配、相機位姿估計、閉環檢測、位姿圖優化和地圖構建與轉換等過程進行詳細敘述，根據RGB-D 姿態和閉環檢測，得到稠密三維點云，并對點云地圖利用八叉樹方法得到一種用于機器人導航的三維地圖。根據實現對比，結果表明所構建的RGB-D SLAM 系統在精確性和高效性有良好效果。