一種改進(jìn)的SLAM建圖方法研究

賈雨萌，劉甜甜，李振華

（北京信息科技大學(xué) 信息與通信工程學(xué)院，北京 100085）

0 引 言

如今，智能機(jī)器人的應(yīng)用越來(lái)越普遍，機(jī)器人的使用環(huán)境、使用場(chǎng)合也變得更加豐富。機(jī)器人所處的環(huán)境往往難以預(yù)測(cè)，智能機(jī)器人的定位和導(dǎo)航大都依靠SLAM算法實(shí)現(xiàn)。SLAM（Simultaneous Localization and Mapping, SLAM），即同步定位與地圖構(gòu)建。SLAM算法的出現(xiàn)，基本解決了路徑規(guī)劃、空間感知及檢測(cè)和躲避障礙物等問(wèn)題。SLAM技術(shù)的內(nèi)容和實(shí)現(xiàn)趨于多樣化、多元化。本文分析了具有代表性的RGB-D SLAM技術(shù)，并對(duì)其提出了一些改進(jìn)。

1 RGB-D SLAM建圖

根據(jù)數(shù)據(jù)源的不同，SLAM技術(shù)主要分為基于激光的SLAM與基于視覺(jué)的SLAM。隨著圖像處理技術(shù)的深入研究，以攝像機(jī)為傳感器的SLAM系統(tǒng)得到了快速發(fā)展。視覺(jué)SLAM通過(guò)處理機(jī)器人（相機(jī)）拍攝的照片構(gòu)建環(huán)境地圖，其優(yōu)勢(shì)在于成本低、場(chǎng)景表達(dá)能力強(qiáng)，被認(rèn)為是未來(lái)SLAM的主要發(fā)展方向。

視覺(jué)SLAM的功能實(shí)現(xiàn)主要包括傳感器（攝像機(jī)）數(shù)據(jù)的搜集（Camera and IMU），前端視覺(jué)（慣性）測(cè)程，后端優(yōu)化，閉環(huán)及映射等，還包括重新定位等。其中，RGB-D SLAM技術(shù)以RGB-D相機(jī)作為傳感器，可以直接獲取圖像中物體的深度。

1.1 RGB-D深度圖像

RGB-D即RGB和Depth Map。RGB即紅、綠、藍(lán)顏色標(biāo)準(zhǔn)，紅、綠、藍(lán)三個(gè)通道的變化和它們之間的疊加可以得到人類視力所能感知的所有顏色。這個(gè)標(biāo)準(zhǔn)是目前應(yīng)用范圍最廣的顏色系統(tǒng)之一。

Depth Map（深度圖）是包含與視點(diǎn)場(chǎng)景對(duì)象表面距離有關(guān)信息的圖像或圖像通道。其中，Depth Map 類似灰度圖像，它的每個(gè)像素值是傳感器距離物體的實(shí)際距離。通常RGB圖像和Depth圖像存在配準(zhǔn)關(guān)系，因而像素點(diǎn)之間具有一對(duì)一對(duì)應(yīng)關(guān)系。

在3D計(jì)算機(jī)圖形和計(jì)算機(jī)視覺(jué)中，深度圖像是一種圖像或圖像通道，其中包含與場(chǎng)景對(duì)象表面到視點(diǎn)距離有關(guān)的信息。

1.2 RGB-D SLAM算法設(shè)計(jì)

在自主移動(dòng)小車實(shí)例中，基于RGB-D SLAM 的方法可以實(shí)現(xiàn)環(huán)境全局地圖的建立與小車實(shí)時(shí)定位功能。

RGB-D SLAM算法的具體實(shí)現(xiàn)如圖1所示。流程圖分為前端和后端兩部分。前端采集圖像信號(hào)，處理相機(jī)捕獲的圖像信息并對(duì)圖像進(jìn)行特征提取與匹配，提取關(guān)鍵幀構(gòu)建地圖的同時(shí)估計(jì)位姿并對(duì)位姿進(jìn)行優(yōu)化。此外，還可以計(jì)算相鄰圖像間的關(guān)系對(duì)運(yùn)動(dòng)進(jìn)行估計(jì)，并通過(guò)特征點(diǎn)法計(jì)算小車的運(yùn)動(dòng)。后端主要處理前端產(chǎn)生噪聲干擾的數(shù)據(jù)，盡可能減少誤差，使得到的地圖較為精確。前端出現(xiàn)的漂移現(xiàn)象可通過(guò)算法進(jìn)行后端優(yōu)化和回環(huán)檢測(cè)來(lái)消除：后端優(yōu)化主要處理采集圖像信息時(shí)產(chǎn)生的噪聲；回環(huán)檢測(cè)通過(guò)比較和處理同一位置的地圖及位姿來(lái)消除漂移量。從前端輸入數(shù)據(jù)，到后端處理數(shù)據(jù)，通過(guò)回環(huán)檢測(cè)減少誤差，實(shí)現(xiàn)地圖的構(gòu)建。

圖1 RGB-D SLAM算法流程

1.2.1 相機(jī)建模及特征提取

相機(jī)建模依據(jù)小孔成像原理。通過(guò)投影得到相機(jī)的內(nèi)參矩陣與外參矩陣在世界坐標(biāo)系、相機(jī)坐標(biāo)系及圖像坐標(biāo)系之間的轉(zhuǎn)換關(guān)系：

式中：、、為特征點(diǎn)在世界坐標(biāo)系中的坐標(biāo)；、為該點(diǎn)在圖像坐標(biāo)系中的投影坐標(biāo)；()為映射函數(shù)。

RGB-D SLAM算法的實(shí)現(xiàn)過(guò)程中，對(duì)前端進(jìn)行特征提取時(shí)使用ORB（Oriented FAST and Rotated BRIEF, ORB）特征點(diǎn)進(jìn)行計(jì)算。ORB 特征點(diǎn)算子結(jié)合加速分割測(cè)試特征（Featuresfrom Accelerated Segment Test, FAST）角點(diǎn)檢測(cè)算法與二進(jìn)制魯棒獨(dú)立基本特征（Binary Robust Independent Elementary Features, BRIEF）描述子算法。其中，ORB 算子使圖像具有尺度不變性；FAST角點(diǎn)檢測(cè)算法對(duì)每個(gè)像素點(diǎn)周圍各點(diǎn)與該點(diǎn)灰度值之差的絕對(duì)值與閾值ε進(jìn)行比較，并統(tǒng)計(jì)前者大于后者的數(shù)量，當(dāng)>12時(shí)，則標(biāo)記為角點(diǎn)：

式中：()、()分別為第點(diǎn)、臨近點(diǎn)的灰度值；BRIEF特征點(diǎn)描述子通過(guò)比較測(cè)試點(diǎn)對(duì)的灰度值來(lái)描述圖像。

1.2.2 后端工作及地圖構(gòu)建

后端主要對(duì)前后幀的圖像，選擇漢明距離作為特征之間的相似性度量，提高特征匹配的準(zhǔn)確率。經(jīng)前后幀的特征點(diǎn)匹配后，采用迭代最近點(diǎn)（Iterative Closest Point, ICP）算法進(jìn)行相機(jī)的位姿估計(jì)。

關(guān)鍵幀的提取可以通過(guò)奇異值分解（Singular Value Decomposition, SVD）算法實(shí)現(xiàn)。首先得到2幀圖像對(duì)應(yīng)的位姿變化，然后根據(jù)位姿前后的變化和2幀圖像間的時(shí)間差來(lái)選取關(guān)鍵幀，減少不必要的冗余數(shù)據(jù)。

將關(guān)鍵幀對(duì)應(yīng)的三維點(diǎn)云地圖加入局部地圖中，并對(duì)局部地圖的相機(jī)位姿與路標(biāo)地圖點(diǎn)進(jìn)行優(yōu)化。通過(guò)對(duì)關(guān)鍵幀的檢測(cè)，識(shí)別之前訪問(wèn)的場(chǎng)景，增加圖模型的回環(huán)約束，消除累積誤差，得到全局一致的地圖。

1.3 經(jīng)典回環(huán)檢測(cè)分析

在上述地圖構(gòu)建過(guò)程中，主要通過(guò)對(duì)關(guān)鍵幀的檢測(cè)來(lái)進(jìn)行。但這需要每一幀圖像與前面的整個(gè)圖像做一遍特征匹配，計(jì)算量極大。根據(jù)時(shí)間差選取關(guān)鍵幀，對(duì)關(guān)鍵幀進(jìn)行匹配的做法雖然能夠減少一部分運(yùn)算量，但回環(huán)檢測(cè)效果不佳。因此，提出使用特征點(diǎn)做回環(huán)檢測(cè)，即詞袋模型的方法。

圖2所示為經(jīng)典回環(huán)檢測(cè)，圖3所示為詞袋模型。

圖2 經(jīng)典回環(huán)檢測(cè)

圖3 詞袋模型

2 回環(huán)檢測(cè)的改進(jìn)

2.1 詞袋模型

詞袋模型被廣泛應(yīng)用在文件分類領(lǐng)域，通過(guò)統(tǒng)計(jì)單詞出現(xiàn)的頻率，比較它們的權(quán)重后進(jìn)行分類。詞袋模型應(yīng)用在計(jì)算機(jī)視覺(jué)領(lǐng)域，就是把特征看成是單詞，通過(guò)比較兩張圖片中單詞的一致性，來(lái)判斷兩張圖片是否屬于同一場(chǎng)景。為了能夠把特征歸類為單詞，需要對(duì)機(jī)器人進(jìn)行海量訓(xùn)練才能達(dá)到效果。

2.2 回環(huán)檢測(cè)算法優(yōu)化設(shè)計(jì)

在ORB特征提取的基礎(chǔ)上，使用詞袋模型方法進(jìn)行回環(huán)檢測(cè)。首先，找到根節(jié)點(diǎn)并設(shè)置權(quán)重，具體算法流程如圖4所示。

圖4 詞袋模型節(jié)點(diǎn)算法流程

確定個(gè)中心點(diǎn)和1個(gè)閾值，之后將提取得到的特征與各中心點(diǎn)的距離最小值作為特征的類，然后計(jì)算每個(gè)類新的中心點(diǎn)，重復(fù)此步驟直至每個(gè)特征與它的類中心點(diǎn)的距離小于閾值。結(jié)束上述步驟后設(shè)置節(jié)點(diǎn)權(quán)重，并計(jì)算。

根據(jù)上述步驟生成一個(gè)詞典，并用叉樹(shù)表示該詞典，如圖5所示。其中，第一層是根節(jié)點(diǎn)，由上述方法得到；每一層的個(gè)節(jié)點(diǎn)也聚類成類，得到下一層；最后一層為葉子節(jié)點(diǎn)，即words。

圖5 K叉樹(shù)

使用Term Frequency-Inverse Document Frequency（TFIDF）計(jì)算節(jié)點(diǎn)權(quán)重，即

式中：η為某葉子節(jié)點(diǎn)w的權(quán)重；n為w中的特征數(shù)量；為所有特征數(shù)量。

在圖像中建立坐標(biāo)軸，權(quán)重最大的2個(gè)節(jié)點(diǎn)為a(x,y)和b(x,y),計(jì)算它們之間的距離：

節(jié)點(diǎn)權(quán)重與節(jié)點(diǎn)距離組合，形成詞袋模型：

詞袋模型構(gòu)建完成后，通過(guò)對(duì)兩幀圖像的詞袋模型中的節(jié)點(diǎn)計(jì)算相似度，減少算法累積的誤差。

需要指出的是，只有當(dāng)詞典的容量足夠大時(shí)，詞庫(kù)提取的準(zhǔn)確率才會(huì)高。在實(shí)際操作中，如果詞典容量不夠大，那么相似圖像與無(wú)關(guān)圖像的相似度對(duì)比就不明顯，回環(huán)檢測(cè)的效果不佳。

3 結(jié) 語(yǔ)

本文介紹了一種使用詞袋模型改進(jìn)的RGB-D SLAM技術(shù)。在保持RGB-D SLAM算法優(yōu)點(diǎn)的同時(shí)，對(duì)回環(huán)檢測(cè)的過(guò)程進(jìn)行了優(yōu)化，加速了特征匹配，有效減少了運(yùn)算量，提高了回環(huán)檢測(cè)的效率。

SLAM技術(shù)持續(xù)更新，計(jì)算機(jī)視覺(jué)在智能機(jī)器人領(lǐng)域的應(yīng)用也日益廣泛，越來(lái)越多的機(jī)器人進(jìn)入人們的生活，為生活帶來(lái)便捷。