基于ORB-SLAM2 的移動機器人定位與建圖研究

詹文杰，宮 振，雷 震，鮑曉宇

（安徽理工大學 電氣與信息工程學院，安徽 淮南 232001）

0 引 言

隨著機器人在生活方面的應用越來越廣泛，社會生產對機器人行動精準度的要求也日益提高，傳統機器人定位系統已經無法滿足，因此，SLAM（同步定位與地圖構建）方法應運而生。考慮到成本和研發精力等各方面問題，視覺SLAM 理所應當的成為當下研究的主流之一。視覺里程表、非線性優化和環回檢測這三個主要部分共同構成了視覺SLAM[1-2]實現中公認的經典框架，如圖1 所示。

圖1 SLAM 框架

在該框架中，視覺里程表作為前端部分，負責使用過濾和預處理的視覺傳感器信息數據構建地圖輪廓；該框架的另一個主體，即后端優化部分的非線性優化和環回檢測，優化了來自視覺里程表的位置和姿態信息的重復信息處理，以最小化其累積誤差。最后，完成路線的地圖規劃。由上述可知，視覺SLAM 的精度主要受到傳感器數據和算法方式的影響，因此基于算法融合并使用特殊相機的研究，仍具有較高的研究價值和實際意義。本文基于ORB-SLAM2 算法[3-4]，進行視覺同步定位和地圖構建。

1 特征點的提取與匹配

1.1 特征點提取

在圖像處理中，特征點的形成來自于圖像灰度值的急劇變化或圖像邊緣上的大曲率（即邊緣的相交）。特征點由關鍵點和描述點組成。關鍵點直接指特征點的圖像位置，而描述點被設計為以向量的形式描述關鍵點周圍的像素信息。

SIFT、ORB 和SURF 是三種常用的特征提取算法[5-6]。SIFT（尺度不變特征變換）是一種尺度不變的特征變換，其穩定性主要體現在透視變換、仿射變換和噪聲等方面。它具有良好的差異性、多樣性、高速性、可擴展性等優點；同時其缺點也非常明顯，由于大量的計算，其實用性大大降低。

SURF（加速強健功能）與SIFT 算法相比，仍具有特征穩定性、旋轉不變性、尺度變換、亮度等優點，視角變換具有一定的穩定性，但缺點是不實時，提取邊緣平滑對象特征點的能力較弱。ORB（Oriented Fast and Rotated Brief）可以以最快的速度為圖像的關鍵點創建特征向量。其中，Fast 和Brief 分別是特征檢測算法和矢量創建算法。ORB 將選擇圖像中的特定區域作為關鍵點，然后為每個關鍵點計算相應的特征向量。該向量用于表示關鍵點周圍的強度模式，因此多個向量的疊加和比較可以識別更大的區域，甚至可以識別圖形中的特定對象。ORB 的特點是保持了前兩種算法的優點，大大提高了計算速度，具有極高的實時性。

1.2 特征點匹配

視覺SLAM 中視覺圖像分析的一個重要部分是特征匹配，它在解決數據關聯方面起著關鍵作用。

在不同時間比較兩個圖像，以便從圖像中提取特征點（n=1, 2, ...,N），并且從圖像提取的特征點（m=1, 2, ...,M）匹配兩者之間的每個元素。

特征點匹配最簡單和最有效的方法是暴力匹配，即逐一測量和比較每個特征子的描述符之間的距離，并成對匹配最近的描述符。特征之間的相似性取決于描述符之間的距離。當描述符之間的距離足夠接近時，可以假設這兩個特征近似相等。但事實上，暴力匹配的結果往往令人不滿意，存在很強的不確定性。PnP問題是當n個3D空間點及其位置已知時，如何估計相機的姿態。如果兩個圖像之一的特征點的3D 位置是已知的，則需要至少3 個點對（以及至少一個額外的驗證點驗證結果）來計算相機運動。在PnP 算法中，EPnP 算法比其他算法復雜度較低，并且對于具有更多特征點的PnP問題非常有效。其次，在解中最多考慮4 個奇異向量，因此精度也很高。

1.3 相機位姿估計

對于相機位置和姿態信息的計算，本文使用EPnP 方法來計算濾波點，然后通過構建來求解，以優化結果[7]。

通常有三種情況通過匹配點來計算相機姿態，這三種情況也有三種相應的解決方案。在處理二維點位信息時，通常采用極坐標幾何方法求解，而在三維點位信息中，通常采用ICP 算法求解。從3D 到2D 的第三種情況是本文描述的PnP解決方案。

2 閉環檢測

如果只考慮相鄰幀之間的運動交換，則它們之間的誤差將一直傳輸。從長遠來看，將產生非常大的累積誤差，這是計算結果中的嚴重漂移。此時，需要閉環檢測。

2.1 閉環檢測方法

閉環檢測使機器人能夠識別當前場景是否已出現。閉環檢測可以在不考慮相鄰幀的情況下提供更長的間隔約束，并為后端優化部分形成的姿態圖提供更有效的數據，因此可以提高姿態估計和地圖構建的精度。這對于時間長、跨度大的SLAM 系統非常重要。

簡單的閉環檢測是一種基于反饋系統的算法。一般關鍵幀檢測過程為：（1）取出候選關鍵幀和子候選關鍵幀組；（2）如果沒有觀察到連續關鍵幀，則連續關鍵幀組為0；（3）如果觀察到連續的關鍵幀組，則判斷最后觀察到的關鍵幀群組是否與當前子候選群組重疊。如果同一關鍵幀組為1，且滿足三個連續關鍵幀組，則添加后續關鍵幀作為閉環檢測的關鍵幀。

2.2 詞袋模型

通過自然語言處理和信息檢索，可以將普通的表達模型簡化為一袋單詞模型。在該模型中，根據每個圖像的特征對特征向量進行分類，并建立類別數據庫。

偏好圖像優化的SLAM 系統通常使用Kd 樹或K-means算法[8]構建數據庫。考慮到K-means 算法易受噪聲、邊緣點、孤立點、可處理的有限數據類型以及K值的選擇不易掌握的影響，本文使用相對高效的K-means++算法來構建數據庫。

K-means++是對K-means算法的改進。根據K-means聚類算法的原理，在正式聚類之前首先要做的是初始化K個聚類中心。同時，正是由于這個原因，K-means 聚類算法存在一個巨大的缺陷，即收斂嚴重依賴于聚類中心的初始化。

相比之下，K-means++算法與K-means 算法非常相似，但兩種算法之間的最大區別在于選擇初始和后續聚類中心的方法不同。同時，K-means++還要求初始集群中心之間的距離應盡可能遠，步驟如下：

（1）從數據點集中隨機選擇一個對象作為算法的第一聚類中心；

（2）計算集合中每個點與初始簇中心之間的距離L；

（3）在剩余點中選擇新的簇中心以獲得距離L(Y)。被選為聚類中心的概率取決于集合中的點之間的距離是否足夠遠；

（4）重復步驟（2）和（3），直到選擇第K個集群中心。

通過上述算法，從圖像中提取的大量特征點被聚類到包含K個對象的數據庫中。接下來，為了解決數據庫內容查詢的問題，本文使用K樹來提取數據，即從H個特征點中建立深度為d且每個分叉為k的樹。

3 實驗及結果分析

3.1 實驗平臺

實驗平臺參數見表1 所列。

表1 實驗平臺參數

3.2 數據集

驗證的數據集采用TUM RGBD fr1 系列數據集。TUM RGBD fr1 系列數據集是通過手持攝像機在辦公室環境中拍攝收集，有平移、旋轉、360°環繞和重復拍攝等功能，本文選用的fr1/desk 序列。

3.3 實驗結果與分析

實際上，從宏觀來看，圖像處理的整體流程相當簡單。環境實物圖如圖2 所示。經過系統處理后可得特征點檢測結果，如圖3 所示。對圖像進行特征點提取后，圖中各個物體均可識別出對應的特征點。再經算法處理得到對應的關鍵幀，如圖4 所示。最后，進行機器人的運動控制，得到軌跡圖如圖5 所示。顯而易見，在整個過程中圖像的處理效果都達到了系統設計的預期。

圖2 數據集環境圖

圖3 特征點檢測結果圖

圖4 關鍵幀軌跡圖

圖5 本文算法估計軌跡圖

4 結 語

本文基于ORB-SLAM2 算法，使用EPnP 等算法進行特征點的提取與匹配，并通過閉環檢測以及詞袋模型完成數據在傳遞過程中的優化，有利于提高位姿估計精度。最后進行實驗對比研究，結果驗證了該系列方法可以提高本SLAM 系統的魯棒性，具有一定價值。