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

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

0 引 言

隨著機(jī)器人在生活方面的應(yīng)用越來越廣泛，社會(huì)生產(chǎn)對機(jī)器人行動(dòng)精準(zhǔn)度的要求也日益提高，傳統(tǒng)機(jī)器人定位系統(tǒng)已經(jīng)無法滿足，因此，SLAM（同步定位與地圖構(gòu)建）方法應(yīng)運(yùn)而生?？紤]到成本和研發(fā)精力等各方面問題，視覺SLAM 理所應(yīng)當(dāng)?shù)某蔀楫?dāng)下研究的主流之一。視覺里程表、非線性優(yōu)化和環(huán)回檢測這三個(gè)主要部分共同構(gòu)成了視覺SLAM[1-2]實(shí)現(xiàn)中公認(rèn)的經(jīng)典框架，如圖1 所示。

圖1 SLAM 框架

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

1 特征點(diǎn)的提取與匹配

1.1 特征點(diǎn)提取

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

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

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

1.2 特征點(diǎn)匹配

視覺SLAM 中視覺圖像分析的一個(gè)重要部分是特征匹配，它在解決數(shù)據(jù)關(guān)聯(lián)方面起著關(guān)鍵作用。

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

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

1.3 相機(jī)位姿估計(jì)

對于相機(jī)位置和姿態(tài)信息的計(jì)算，本文使用EPnP 方法來計(jì)算濾波點(diǎn)，然后通過構(gòu)建來求解，以優(yōu)化結(jié)果[7]。

通常有三種情況通過匹配點(diǎn)來計(jì)算相機(jī)姿態(tài)，這三種情況也有三種相應(yīng)的解決方案。在處理二維點(diǎn)位信息時(shí)，通常采用極坐標(biāo)幾何方法求解，而在三維點(diǎn)位信息中，通常采用ICP 算法求解。從3D 到2D 的第三種情況是本文描述的PnP解決方案。

2 閉環(huán)檢測

如果只考慮相鄰幀之間的運(yùn)動(dòng)交換，則它們之間的誤差將一直傳輸。從長遠(yuǎn)來看，將產(chǎn)生非常大的累積誤差，這是計(jì)算結(jié)果中的嚴(yán)重漂移。此時(shí)，需要閉環(huán)檢測。

2.1 閉環(huán)檢測方法

閉環(huán)檢測使機(jī)器人能夠識別當(dāng)前場景是否已出現(xiàn)。閉環(huán)檢測可以在不考慮相鄰幀的情況下提供更長的間隔約束，并為后端優(yōu)化部分形成的姿態(tài)圖提供更有效的數(shù)據(jù)，因此可以提高姿態(tài)估計(jì)和地圖構(gòu)建的精度。這對于時(shí)間長、跨度大的SLAM 系統(tǒng)非常重要。

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

2.2 詞袋模型

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

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

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

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

（1）從數(shù)據(jù)點(diǎn)集中隨機(jī)選擇一個(gè)對象作為算法的第一聚類中心；

（2）計(jì)算集合中每個(gè)點(diǎn)與初始簇中心之間的距離L；

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

（4）重復(fù)步驟（2）和（3），直到選擇第K個(gè)集群中心。

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

3 實(shí)驗(yàn)及結(jié)果分析

3.1 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)平臺(tái)參數(shù)見表1 所列。

表1 實(shí)驗(yàn)平臺(tái)參數(shù)

3.2 數(shù)據(jù)集

驗(yàn)證的數(shù)據(jù)集采用TUM RGBD fr1 系列數(shù)據(jù)集。TUM RGBD fr1 系列數(shù)據(jù)集是通過手持?jǐn)z像機(jī)在辦公室環(huán)境中拍攝收集，有平移、旋轉(zhuǎn)、360°環(huán)繞和重復(fù)拍攝等功能，本文選用的fr1/desk 序列。

3.3 實(shí)驗(yàn)結(jié)果與分析

實(shí)際上，從宏觀來看，圖像處理的整體流程相當(dāng)簡單。環(huán)境實(shí)物圖如圖2 所示。經(jīng)過系統(tǒng)處理后可得特征點(diǎn)檢測結(jié)果，如圖3 所示。對圖像進(jìn)行特征點(diǎn)提取后，圖中各個(gè)物體均可識別出對應(yīng)的特征點(diǎn)。再經(jīng)算法處理得到對應(yīng)的關(guān)鍵幀，如圖4 所示。最后，進(jìn)行機(jī)器人的運(yùn)動(dòng)控制，得到軌跡圖如圖5 所示。顯而易見，在整個(gè)過程中圖像的處理效果都達(dá)到了系統(tǒng)設(shè)計(jì)的預(yù)期。

圖2 數(shù)據(jù)集環(huán)境圖

圖3 特征點(diǎn)檢測結(jié)果圖

圖4 關(guān)鍵幀軌跡圖

圖5 本文算法估計(jì)軌跡圖

4 結(jié) 語

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