一種融入注意力和預(yù)測(cè)的特征選擇SLAM 算法

曾毓菁，姜勇

（1.東北大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽(yáng) 110006;2.中國(guó)科學(xué)院 沈陽(yáng)自動(dòng)化研究所，遼寧 沈陽(yáng) 110016;3.中國(guó)科學(xué)院 網(wǎng)絡(luò)化控制系統(tǒng)重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽(yáng) 110016;4.中國(guó)科學(xué)院 機(jī)器人與智能制造創(chuàng)新研究院，遼寧沈陽(yáng) 110169）

人類(lèi)在日常生活中可以不間斷地處理大量獲取來(lái)自外界的信息，如在高速公路上駕駛汽車(chē)，或在擁擠的街道上行走。在認(rèn)知科學(xué)中，人們一致認(rèn)為，人類(lèi)可以高效地處理面臨的大量數(shù)據(jù)是由于我們能夠優(yōu)先考慮視覺(jué)場(chǎng)景的某些方面，而忽視其他方面。這是人類(lèi)的一種視覺(jué)注意力機(jī)制，是一種認(rèn)知過(guò)程，通過(guò)選擇相關(guān)信息，過(guò)濾掉不相關(guān)的刺激，使人類(lèi)能夠解析大量的視覺(jué)數(shù)據(jù)，從而在有限的資源下實(shí)現(xiàn)性能的最大化[1]。機(jī)器人雖然做不到人類(lèi)這樣，但這種視覺(jué)注意力機(jī)制是值得機(jī)器人自主定位導(dǎo)航借鑒的。

國(guó)內(nèi)外學(xué)者開(kāi)展了很多通過(guò)主動(dòng)特征選擇提高視覺(jué)SLAM 和視覺(jué)里程計(jì)性能的研究。Ouerhani 等[2]利用注意的路標(biāo)點(diǎn)構(gòu)建拓?fù)涞貓D，在導(dǎo)航過(guò)程中，利用相同的視覺(jué)注意模型檢測(cè)最突出的視覺(jué)特征，與注意的路標(biāo)點(diǎn)相匹配獲取特征點(diǎn)。Newman 等[3]考慮一個(gè)裝備了相機(jī)和激光測(cè)距儀的機(jī)器人，并利用基于外觀的視覺(jué)顯著性概念進(jìn)行特征選擇，提高SLAM 回環(huán)檢測(cè)的穩(wěn)定性，在室內(nèi)環(huán)境中可以很好地檢測(cè)到回環(huán)(大約100 m)。Sola 等[4]指出盡管許多特征可能在機(jī)器人環(huán)境的給定視圖中可見(jiàn)，只有少數(shù)這樣的特征是估計(jì)機(jī)器人的位置和方向所必需的。它們利用共視性作為標(biāo)準(zhǔn)，自動(dòng)選擇特征最優(yōu)集。Frintrop等[5]使用注意力框架進(jìn)行特征選擇和主動(dòng)視角控制，能夠克服使用相對(duì)較小視野的單目視覺(jué)SLAM系統(tǒng)的一些限制，提高姿態(tài)估計(jì)的準(zhǔn)確性。該特征選擇系統(tǒng)是基于VOCUS 模型的[5]，其包含一個(gè)自底向上的注意力系統(tǒng)(根據(jù)特征外觀計(jì)算顯著性)，還包含一個(gè)自頂向下的機(jī)制(考慮任務(wù)性能)。Hochdorfer 等[6]提出了一種基于區(qū)域覆蓋的特征點(diǎn)評(píng)級(jí)和選擇機(jī)制，解決了隨著特征數(shù)量不斷增加，運(yùn)算資源占用率越來(lái)越高的問(wèn)題，為SLAM算法在服務(wù)機(jī)器人整個(gè)生命周期中的應(yīng)用，甚至在不同的環(huán)境中的應(yīng)用做出了貢獻(xiàn)。Strasdat 等[7]提出了一種基于強(qiáng)化學(xué)習(xí)方法的特征選擇，減輕了機(jī)器人在SLAM 建圖中的計(jì)算負(fù)擔(dān)。Shi 等[8]提出了一種特征選擇方法來(lái)提高SLAM 中數(shù)據(jù)關(guān)聯(lián)的魯棒性，防止了不好的特征被初始化，更好地保證了SLAM 估計(jì)的一致性。Cvisic 等[9]通過(guò)穩(wěn)定的特征選擇加速了立體視覺(jué)里程計(jì)的計(jì)算，減少了SLAM 軌跡漂移，該特征選擇是基于統(tǒng)分的(將特征均勻地分布在整個(gè)圖像中)和基于外觀的排名。最近在計(jì)算機(jī)視覺(jué)領(lǐng)域的研究表明利用注意力機(jī)制可以減少神經(jīng)網(wǎng)絡(luò)的計(jì)算負(fù)擔(dān)。Mnih等[10]通過(guò)引入瞥見(jiàn)的概念，減少了使用遞歸神經(jīng)網(wǎng)絡(luò)進(jìn)行目標(biāo)檢測(cè)和跟蹤的步驟，可以在圖像感興趣的區(qū)域提供更高的分辨率。文獻(xiàn)[11]是最早使用信息理論結(jié)構(gòu)來(lái)推理視覺(jué)特征的論文之一，提出了一個(gè)基于模型的跟蹤問(wèn)題，在該問(wèn)題中，運(yùn)動(dòng)相機(jī)的狀態(tài)必須通過(guò)對(duì)已知特征的觀察來(lái)估計(jì)。

視覺(jué)SLAM 在面對(duì)快速變動(dòng)場(chǎng)景時(shí)，由于缺乏可跟蹤特征，基于視覺(jué)的感知技術(shù)往往會(huì)失敗。這是一個(gè)眾所周知的問(wèn)題，在文獻(xiàn)中經(jīng)常被討論(例如，見(jiàn)文獻(xiàn)[12-13])。本文基于環(huán)境和相機(jī)運(yùn)動(dòng)狀態(tài)的相互關(guān)聯(lián)性而產(chǎn)生的特征效用，引入預(yù)測(cè)機(jī)制，利用注意力和預(yù)測(cè)選擇特征來(lái)減少跟蹤特征的丟失，通過(guò)濾去那些將丟失在視野中的特征點(diǎn)，增加SLAM 系統(tǒng)的穩(wěn)定性，特征選擇器知道機(jī)器人的意圖(未來(lái)的運(yùn)動(dòng))，并相應(yīng)地選擇更有可能出現(xiàn)在未來(lái)視野中的特征，可以用更少的含信息多的特征點(diǎn)來(lái)提高優(yōu)化效率。

1 注意力和預(yù)測(cè)機(jī)制

本文提出的注意力和預(yù)測(cè)機(jī)制是從當(dāng)前幀檢測(cè)到的特征F中選擇子集S傳遞給SLAM 后端進(jìn)行非線性?xún)?yōu)化，整體流程如圖1 所示。

圖1 整體流程Fig.1 Whole process

為了最大限度地提高運(yùn)動(dòng)估計(jì)的性能，特征子集應(yīng)該最多具有 κ 個(gè)特征，這些特征對(duì)于減少基于視覺(jué)狀態(tài)估計(jì)的不確定性是最有效的。可以把這個(gè)問(wèn)題先簡(jiǎn)單地表述為

這個(gè)問(wèn)題的解決依賴(lài)于度量方程f的選擇，它將量化特征子集的可行性。特征選擇與控制理論中的傳感器布置問(wèn)題有著密切的聯(lián)系，盡管由于二元選擇變量的引入，傳感器選擇問(wèn)題已被證明是NP-hard 問(wèn)題[14]，如著名的旅行商問(wèn)題，但最近的研究結(jié)果表明利用子模塊代價(jià)函數(shù)、貪婪算法能夠近似地找到式(1)的解，同時(shí)保證其次優(yōu)性[15]。本文確定了度量方程，該度量可以表示子模塊性，可以量化狀態(tài)估計(jì)的不確定性。更重要的是，通過(guò)它應(yīng)該能得到最相關(guān)的特征點(diǎn)。1)該度量必須鼓勵(lì)選擇最顯著的特征，因?yàn)檫@些特征更有可能在連續(xù)幀中被重新觀察到。2)度量必須獎(jiǎng)勵(lì)那些在視場(chǎng)中停留較長(zhǎng)時(shí)間的特征。因此，預(yù)測(cè)是一個(gè)關(guān)鍵的方面:利用度量必須知道，在一定的運(yùn)動(dòng)下，哪些特征更有可能留在相機(jī)的視場(chǎng)中。3)度量必須獎(jiǎng)勵(lì)提供更多信息的特征，以減少不確定性。

本文的方法受Carlone 等[16]的啟發(fā)，文中提出了兩種量化特征子集可行性的度量，最小特征值 λmin度量和行列式logdet 度量，并且通過(guò)嚴(yán)謹(jǐn)?shù)臄?shù)學(xué)證明，證明了logdet 度量的可行性。在式(1)之后，使用logdet 度量[16]來(lái)計(jì)算狀態(tài)估計(jì)不確定性。設(shè)k+1 為獲得一個(gè)新選擇特征集的時(shí)刻，則xk中包含前一幀的優(yōu)化位姿，這是優(yōu)化后端的最新位姿估計(jì)(見(jiàn)圖2)，注意到在前一幀k和當(dāng)前幀k+1 之間將進(jìn)行特征選擇。用表示在H時(shí)間段上的狀態(tài)向量，其中是待優(yōu)化的預(yù)測(cè)的狀態(tài)向量。此外，設(shè) 為與關(guān)聯(lián)的估計(jì)誤差的協(xié)方差矩陣，它的逆矩陣稱(chēng)為信息矩陣，表示為。有了這些定義后，logdet 度量可以寫(xiě)成

圖2 時(shí)間軸Fig.2 Time line

式中：Δl是第l個(gè)特征的信息矩陣；pl是第l個(gè)特征被跟蹤到的概率，其可以通過(guò)對(duì)特征檢測(cè)得分進(jìn)行歸一化得到[8]，保證信息較多的特征更容易保留，這是一種期望的行為。

1.1 貪婪算法

本文使用帶有延遲求值[17]的貪婪算法[18]選擇κ 個(gè)特征(近似的)最大化logdet 度量。作為輸入，需要一定數(shù)量的特征F供系統(tǒng)選擇，每個(gè)新特征點(diǎn)的預(yù)測(cè)信息矩陣{Δl}l∈F，以及未來(lái)H時(shí)間段的信息矩陣。為了保持對(duì)特征跟蹤的一致性，需要對(duì)之前確定的有用的特征進(jìn)行跟蹤。當(dāng)在未來(lái)幀中檢測(cè)到這些特征時(shí)，它們將自動(dòng)傳遞給優(yōu)化后端。

算法1帶有延遲求值的貪婪算法

1.2 視覺(jué)特征信息矩陣

本文使用線性測(cè)量模型，它簡(jiǎn)化了非線性透視投影模型，并且可以進(jìn)行有效的計(jì)算。為此，必須將像素測(cè)量表示為想要估計(jì)的未知狀態(tài)的線性函數(shù)。假設(shè)uhl是h時(shí)刻第l特征在相機(jī)坐標(biāo)系下的像素觀測(cè)，它滿(mǎn)足：

圖3 向量共線Fig.3 Vector collinear

將時(shí)間段內(nèi)每個(gè)時(shí)刻的狀態(tài)定義為xh=(機(jī)器人的預(yù)測(cè)姿態(tài))，其中ΔR=exp(φ∧)，，為了之后公式的表達(dá)方便，將表示為R和t，然后用這些量參數(shù)化線性系統(tǒng)。將式(3)的左邊表示為f(φ,t)，并用全微分公式[21]將其線性化：

第l特征在時(shí)間段H內(nèi)的貢獻(xiàn)為 Δl可以表示為

最后將其用在貪婪算法中。

2 實(shí)驗(yàn)分析

為了測(cè)試本文的方法，使用EuRoC 數(shù)據(jù)集[22]，該數(shù)據(jù)集是使用一個(gè)配備了視覺(jué)慣性[23]傳感器的AscTech Firefly 六旋翼飛行器采集的，其包含簡(jiǎn)單場(chǎng)景和復(fù)雜場(chǎng)景。在本文中，使用收集自一個(gè)工廠大廳環(huán)境的數(shù)據(jù)集MH01-easy、MH02-easy、MH03-medium、MH04-difficult、MH05-difficult，隨著場(chǎng)景復(fù)雜度提高，飛行器速度依次遞增且有更多的急轉(zhuǎn)彎。為了更好地說(shuō)明算法的效果，對(duì)比了ORB-SLAM2[24]和Random 兩種情況，ORB-SLAM2 是不添加本文算法的SLAM 系統(tǒng)，Random 是一種在ORB-SLAM2 選擇好特征點(diǎn)的幀上隨機(jī)地選擇跟本文算法同樣數(shù)量特征點(diǎn)的情況。使用ATE(絕對(duì)軌跡誤差)中的RMSE(均方根誤差)，即為位姿誤差)作為比較標(biāo)準(zhǔn)[25]，結(jié)果如圖4 所示。雖然本文算法的絕對(duì)軌跡誤差平均比ORB-SLAM2大0.025 m，但相對(duì)這5 個(gè)工廠數(shù)據(jù)集真值軌跡長(zhǎng)度來(lái)說(shuō)是很小的，值得關(guān)注的是我們只傳遞了20 個(gè)特征點(diǎn)去SLAM 后端優(yōu)化。而隨機(jī)選擇的Random 隨著場(chǎng)景復(fù)雜度的不斷提高誤差也在不斷增大，在MH05-hard 中更是達(dá)到了0.259 m 的誤差。

圖4 ORB-SLAM2、Random 和本文算法的表現(xiàn)Fig.4 Performance of ORB-SLAM2、Random and our algorithm

為了更好地理解所提出方法所帶來(lái)的性能提升，在圖5 按時(shí)間順序給出了生成的一些快照。每個(gè)子圖顯示了關(guān)鍵幀中待選擇特征(綠色方塊)和選擇的特征(加粗黑色方塊)。這些關(guān)鍵幀是MH03-medium 數(shù)據(jù)集向左急轉(zhuǎn)彎時(shí)獲取的。注意到兩幅圖選擇的特征大部分都集中在畫(huà)面左側(cè)，也就是相機(jī)急轉(zhuǎn)彎的方向，隨著相機(jī)的運(yùn)動(dòng)這些特征更有可能保持在視野中。這是因?yàn)?，本文所提出的注意力和預(yù)測(cè)機(jī)制充分利用了平臺(tái)的即時(shí)運(yùn)動(dòng)信息。在圖6 中給出了ORB-SLAM2、Random 和本文算法運(yùn)行在MH05-difficult 的軌跡圖。從圖6 中都可以看出，本文的算法雖然沒(méi)有ORB-SLAM2 跟地面真值貼合度那么高，但卻明顯優(yōu)于Random。

圖5 本文算法向左急轉(zhuǎn)彎時(shí)的快照Fig.5 Snapshot of our algorithm during a sharp left turn

圖6 ORB-SLAM2、Random 和本文算法的軌跡Fig.6 Trajectory of ORB-SLAM2,Random,and our algorithm

3 結(jié)束語(yǔ)

本文提出的注意力和預(yù)測(cè)機(jī)制保留了前端檢測(cè)到的含信息最豐富的視覺(jué)特征，并將其反饋給估計(jì)后端。該機(jī)制的特點(diǎn)：1) 本質(zhì)上具有預(yù)測(cè)性，能夠感知機(jī)器人未來(lái)的運(yùn)動(dòng)，并據(jù)此選擇特征。正如我們?cè)趯?shí)驗(yàn)中看到的，在急轉(zhuǎn)彎和快速變動(dòng)的情況下能確保定位的準(zhǔn)確性。2)它具有貪婪性，利用帶有延遲求值的貪婪算法保證了logdet 度量的次優(yōu)性。本文在EuRoC 工廠大廳環(huán)境數(shù)據(jù)集中進(jìn)行了大量特征選擇實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明本文加入的注意力和預(yù)測(cè)的特征選擇算法對(duì)SLAM 系統(tǒng)性能影響很大，使用很少的特征點(diǎn)也能達(dá)到定位的精度要求。

以后的研究中，將進(jìn)一步研究如何聯(lián)合優(yōu)化視覺(jué)和IMU，以提高注意力和預(yù)測(cè)機(jī)制的穩(wěn)定性和適應(yīng)性。