回环检测是什么？ 解读SLAM算法中回环检测的意义

隨著路徑的不斷延伸，機(jī)器人在建圖過程中會(huì)存在一些累計(jì)誤差，除了利用局部?jī)?yōu)化、全局優(yōu)化等來調(diào)整之外，還可以利用回環(huán)檢測(cè)來優(yōu)化位姿。

回環(huán)檢測(cè)是什么？

回環(huán)檢測(cè)，又稱閉環(huán)檢測(cè)，是指機(jī)器人識(shí)別曾到達(dá)某場(chǎng)景，使得地圖閉環(huán)的能力。說的簡(jiǎn)單點(diǎn)，就是機(jī)器人在左轉(zhuǎn)一下，右轉(zhuǎn)一下建圖的時(shí)候能意識(shí)到某個(gè)地方是“我”曾經(jīng)來過的，然后把此刻生成的地圖與剛剛生成的地圖做匹配。

回環(huán)檢測(cè)成功

回環(huán)檢測(cè)之所以能成為一個(gè)難點(diǎn)，是因?yàn)椋喝绻丨h(huán)檢測(cè)成功，可以顯著地減小累積誤差，幫助機(jī)器人更精準(zhǔn)、快速的進(jìn)行避障導(dǎo)航工作。而錯(cuò)誤的檢測(cè)結(jié)果可能使地圖變得很糟糕。因此，回環(huán)檢測(cè)在大面積、大場(chǎng)景地圖構(gòu)建上是非常有必要的 。

回環(huán)檢測(cè)失敗

機(jī)器人回環(huán)檢測(cè)能力如何提升？

那么，怎么才能讓機(jī)器人的回環(huán)檢測(cè)能力得到一個(gè)質(zhì)的提升呢？首先要有一個(gè)算法上的優(yōu)化。

1、基于圖優(yōu)化的SLAM算法

基于圖優(yōu)化的SLAM 3.0 算是提升機(jī)器人回環(huán)檢測(cè)能力的一大突破。

SLAM 3.0采用圖優(yōu)化的方式進(jìn)行建圖，進(jìn)行了圖片集成與優(yōu)化處理，當(dāng)機(jī)器人運(yùn)動(dòng)到已經(jīng)探索過的原環(huán)境時(shí)， SLAM 3.0可依賴內(nèi)部的拓?fù)鋱D進(jìn)行主動(dòng)式的閉環(huán)檢測(cè)。當(dāng)發(fā)現(xiàn)了新的閉環(huán)信息后，SLAM 3.0使用Bundle Adjuestment(BA)等算法對(duì)原先的位姿拓?fù)涞貓D進(jìn)行修正（即進(jìn)行圖優(yōu)化），從而能有效的進(jìn)行閉環(huán)后地圖的修正，實(shí)現(xiàn)更加可靠的環(huán)境建圖。

SLAM 3.0閉環(huán)檢測(cè)

SLAM 3.0環(huán)路閉合邏輯：先小閉環(huán)，后大閉環(huán) ；選擇特征豐富的點(diǎn)作為閉環(huán)點(diǎn)；多走重合之路，完善閉環(huán)細(xì)節(jié)。即使在超大場(chǎng)景下建圖，也不慌。

超大場(chǎng)景下建圖完整閉合過程

2、詞袋模型

除了SLAM算法的升級(jí)和優(yōu)化之外，現(xiàn)在還有很多系統(tǒng)采用成熟的詞袋模型方法來幫助機(jī)器人完成閉環(huán)，說的簡(jiǎn)單點(diǎn)就是把幀與幀之間進(jìn)行特征比配。

從每幅圖像中提取特征點(diǎn)和特征描述，特征描述一般是一個(gè)多維向量，因此可以計(jì)算兩個(gè)特征描述之間的距離；

將這些特征描述進(jìn)行聚類（比如k－means），類別的個(gè)數(shù)就是詞典的單詞數(shù)，比如1000；也可以用Beyes、SVM等；

將這些詞典組織成樹的形式，方便搜索。

利用這個(gè)樹，就可以將時(shí)間復(fù)雜度降低到對(duì)數(shù)級(jí)別，大大加速了特征匹配。

3、相似度計(jì)算

這種做法是從外觀上根據(jù)兩幅圖像的相似性確定回環(huán)檢測(cè)關(guān)系，那么，如何確定兩個(gè)地圖之間的相關(guān)性呢？

比如對(duì)于圖像A和圖像B，我們要計(jì)算它們之間的相似性評(píng)分：s(A,B)。如果單單用兩幅圖像相減然后取范數(shù)，即為： s(A,B)=||A−B||s(A,B)=||A−B||。但是由于一幅圖像在不同角度或者不同光線下其結(jié)果會(huì)相差很多，所以不使用這個(gè)函數(shù)。而是使用相似度計(jì)算公式。

這里，我們提供一種方法叫TF-IDF。

TF的意思是：某特征在一幅圖像中經(jīng)常出現(xiàn)，它的區(qū)分度就越高。另一方面，IDF的思想是，某特征在字典中出現(xiàn)的頻率越低，則分類圖像時(shí)的區(qū)分度越高。

對(duì)于IDF部分，假設(shè)所有特征數(shù)量為n，某個(gè)節(jié)點(diǎn)的Wi所含的數(shù)量特征為Ni，那么該單詞的IDF為：

TF是指某個(gè)特征在單副圖像中出現(xiàn)的頻率。假設(shè)圖像A中單詞Wi出現(xiàn)了N次，而一共出現(xiàn)的單詞次數(shù)是n，那么TF為： 

于是Wi的權(quán)重等于TF乘IDF之積，即 

考慮權(quán)重以后，對(duì)于某副圖像，我們可以得到許多個(gè)單詞，得到BOW：

（A表示某幅地圖）

如何計(jì)算倆副圖像相似度，這里使用了L1范數(shù)形式：

4、深度學(xué)習(xí)及其他

除了上面的幾種方式之外，回環(huán)檢測(cè)也可以建成一個(gè)模型識(shí)別問題，利用深度學(xué)習(xí)的方法幫助機(jī)器人完成回環(huán)檢測(cè)。比如：決策樹、SVM等。

……

最后，當(dāng)回環(huán)出現(xiàn)以后，也不要急著就讓機(jī)器人停止運(yùn)動(dòng)，要繼續(xù)保持運(yùn)動(dòng)，多走重合的路，在已經(jīng)完成閉合的路徑上，進(jìn)一步掃圖完善細(xì)節(jié)。

繼續(xù)走重合之路，完善閉環(huán)細(xì)節(jié)