一種融入注意力和預測的特征選擇SLAM 算法

曾毓菁，姜勇

（1.東北大學 信息科學與工程學院，遼寧 沈陽 110006;2.中國科學院 沈陽自動化研究所，遼寧 沈陽 110016;3.中國科學院 網絡化控制系統重點實驗室，遼寧 沈陽 110016;4.中國科學院 機器人與智能制造創新研究院，遼寧沈陽 110169）

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

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

視覺SLAM 在面對快速變動場景時，由于缺乏可跟蹤特征，基于視覺的感知技術往往會失敗。這是一個眾所周知的問題，在文獻中經常被討論(例如，見文獻[12-13])。本文基于環境和相機運動狀態的相互關聯性而產生的特征效用，引入預測機制，利用注意力和預測選擇特征來減少跟蹤特征的丟失，通過濾去那些將丟失在視野中的特征點，增加SLAM 系統的穩定性，特征選擇器知道機器人的意圖(未來的運動)，并相應地選擇更有可能出現在未來視野中的特征，可以用更少的含信息多的特征點來提高優化效率。

1 注意力和預測機制

本文提出的注意力和預測機制是從當前幀檢測到的特征F中選擇子集S傳遞給SLAM 后端進行非線性優化，整體流程如圖1 所示。

圖1 整體流程Fig.1 Whole process

為了最大限度地提高運動估計的性能，特征子集應該最多具有 κ 個特征，這些特征對于減少基于視覺狀態估計的不確定性是最有效的。可以把這個問題先簡單地表述為

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

本文的方法受Carlone 等[16]的啟發，文中提出了兩種量化特征子集可行性的度量，最小特征值 λmin度量和行列式logdet 度量，并且通過嚴謹的數學證明，證明了logdet 度量的可行性。在式(1)之后，使用logdet 度量[16]來計算狀態估計不確定性。設k+1 為獲得一個新選擇特征集的時刻，則xk中包含前一幀的優化位姿，這是優化后端的最新位姿估計(見圖2)，注意到在前一幀k和當前幀k+1 之間將進行特征選擇。用表示在H時間段上的狀態向量，其中是待優化的預測的狀態向量。此外，設 為與關聯的估計誤差的協方差矩陣，它的逆矩陣稱為信息矩陣，表示為。有了這些定義后，logdet 度量可以寫成

圖2 時間軸Fig.2 Time line

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

1.1 貪婪算法

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

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

1.2 視覺特征信息矩陣

本文使用線性測量模型，它簡化了非線性透視投影模型，并且可以進行有效的計算。為此，必須將像素測量表示為想要估計的未知狀態的線性函數。假設uhl是h時刻第l特征在相機坐標系下的像素觀測，它滿足：

圖3 向量共線Fig.3 Vector collinear

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

第l特征在時間段H內的貢獻為 Δl可以表示為

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

2 實驗分析

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

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

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

圖5 本文算法向左急轉彎時的快照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 結束語

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

以后的研究中，將進一步研究如何聯合優化視覺和IMU，以提高注意力和預測機制的穩定性和適應性。