雙目視覺-慣性SLAM系統的閉環標記：理解漂移和延遲對跟蹤精度的影響

論文名稱：Closed-Loop Benchmarking of Stereo Visual-Inertial SLAM Systems: Understanding the Impact of Drift and Latency on Tracking Accuracy

作者單位：Georgia Institute of Technology

論文下載鏈接：https://arxiv.org/pdf/2003.01317

摘要

在GPS拒絕的環境下，視覺慣性SLAM對於機器人導航至關重要，例如：室內，地下。一貫以來，視覺慣性SLAM通過開環分析評估其系統性能，重點關注SLAM系統的漂移級別。這篇論文中，我們提出的問題是視覺估計延遲在閉環導航任務中的重要性，例如准確軌跡追蹤任務。為了理解視覺慣性SLAM系統中漂移和延遲的影響，進行閉環基准測試模擬，使用視覺慣性估計的反饋來命令機器人遵循期望的軌跡。通過額外評估目前具有先進代表性的視覺慣性SLAM系統進行軌跡追蹤的性能，我們揭示了這些系統中，延遲減少對於視覺估計模塊的重要性。這些發現表明了視覺慣性SLAM將來發展的方向。

1. 介紹

基於視覺的狀態估計技術，例如視覺里程計（VO）和視覺同步定位和建圖（VSLAM），是機器人在未建圖場景下自主導航的關鍵技術。VO會經常忘記感知過的世界結構，但是VSLAM保留了所遍歷世界的長期地圖。由於缺失絕對的位置信號，例如來自於GPS信號，VO/VSLAM補充了傳統基於車輪/慣性的里程計。

對於僅僅依賴視覺傳感器的VO/VSLAM，視覺慣性SLAM（VI-SLAM）使用兩種互補的數據流，來獲得更好的准確性和魯棒性，以及更高的狀態估計頻率。視覺傳感器提供准確的，但是對於環境中的絕對地標是稀疏而延遲的測量。減少估計漂移，是通過在一個長期而潛在斷續測量的過程中，進行觀測和匹配地標。慣性傳感器是對機器人進行高頻率，幾乎是實時的，但是有漂移的測量。慣性測量彌補了短期的視覺特征損失（例如在紋理缺失的情境下）。對於一個VI-SLAM系統的姿態估計信息，能夠作為一個高質量的反饋信號發送給一個控制器，用來支持在某個環境中移動機器人的軌跡追蹤。

盡管VI-SLAM在機器人學中最終用途是閉環導航，但是VI-SLAM的一般基准是采用開環分析，即，SLAM的輸出不影響實際機器人的驅動和將來傳感器的輸入。雖然反映了VI-SLAM估計漂移級別，開環估計不能完全解決在閉環操作中，由導航和VI-SLAM估計所帶來的雙重影響。對於有針對性的閉環導航，很難從獲得到的開環基准得分中，分析到VI-SLAM的獨特見解。為了解決這樣的基准差距，對於閉環VI-SLAM估計，我們提出了一個開源的、可再現的基准模擬，以及用它評估幾種VI-SLAM方法的結果。可再現、閉環的基准應該服務於指導移動機器人將來的VI-SLAM研究。

盡管VI-SLAM漂移是影響閉環導航性能的關鍵因素，視覺估計的延遲同樣在閉環中產生重要影響。如圖1所示，對於視覺過程子系統的延遲減少，由於累計IMU偏差的二項性（實時），能夠提高融合的視覺慣性狀態估計的准確性。因此，這篇論文研究了在閉環基准模擬下的VI-SLAM系統的漂移和視覺估計延遲的影響，通過實現和測試具有不同運行屬性的公開VI-SLAM系統。閉環基准結果表明，VI-SLAM系統必須平衡漂移和延遲。

圖1：延遲對於誤差的影響。

2. 相關工作

這段首先回顧了視覺慣性狀態估計在閉環導航中的現狀。名稱VI-SLAM會被用來表示視覺慣性里程計（VIO）和視覺慣性SLAM。之后，本章結合對於閉環軌跡追蹤基准的討論，回顧了VI-SLAM的評估方法。

A. 閉環導航中的VI-SLAM

對於移動機器人，將過濾器使用在視覺慣性狀態估計有很久遠的歷史（例如：EKF；MSCKF）。稀疏光流（例如：KLT）和MSCKF的結合已經被認為是一種很有效的VI-SLAM方法。大部分基於過濾的方法有一個缺點是構圖質量較差，從而影響長期的重定位導航。

VI-SLAM通過運行集束優化（BA）來保持一個明確的地圖，從而提高狀態估計的准確性和長期的魯棒性。為了降低BA的立方計算耗費，基於BA的VI-SLAM一般用歷史信息（關鍵幀和地標）的一個子集進行運算，這個子集數據通過一個滑動窗口或一個可見度圖來提取。目前代表性基於BA的VI-SLAM包括基於特征的OKVIS，基於KLT的VINS-Fusion和Kimera。使用OKVIS進行閉環導航，已經在地面和空中機器人被證明有效。VINS-Fusion已經在微型飛行器上演示了完整的導航。Kimera實時估計3D網格，這有益於導航。

近來，直接VI-SLAM系統被提出，這些系統不需要明確的特征提取和匹配。直接系統通過使用原始讀取的圖像數據進行對象函數優化，從而同步完成數據關聯和狀態估計。直接VI-SLAM系統，例如SVO和ROVIO，已經被集成到閉環導航系統中。盡管KLT和直接VI-SLAM都比基於特征的VI-SLAM計算簡單，但是它們也對基於導航的條件更加敏感，例如，它們需要准確的姿態預測（來自慣性）和最小的光源條件變化。此外，KLT和直接方法都是以短基線特征匹配為特點。在另一方面，特征描述子匹配可以提供可靠的長期基線匹配，從而提高狀態估計效果。（如圖2所示）

圖2：3種常用系統特征匹配分布。

B. 閉環軌跡追蹤評估

不同VI-SLAM方法的開環估計已經在論文中被廣泛實施，例如，在多種數據集上，在多種計算設備上和多種擬合環境中。在導航任務中，不同VI-SLAM方法的閉環估計卻很好被研究。機載估計傾向於被用來作為單獨實現的報道。閉環估計的一個挑戰就是，閉環導航不僅僅是軟件層面的問題。整個系統的性能受到所選擇傳感器、計算資源、系統動力和目標環境的影響。所以這些因素都要通過實驗控制，從而達到對於使用VI-SLAM進行閉環導航的系統性能的全面評估。實現全面而重復的閉環評估的一個途徑就是通過模擬。有幾種模擬器常被機器人領域所使用。Gazebo是最流行模擬器之一，而RotorS則帶有特定飛行器的擴展內容。AirSim是另外一種選擇，通過循環引擎對於數據進行逼真的渲染。而一個更新的發展是，將硬件集成到循環中。實時捕獲機器人軌跡的方法，同時在遠處工作站上渲染虛擬視覺數據，來收集真實物理情況下的實際數據和來自易於擴展的渲染器的虛擬數據。但是，所獲取的真值依賴於MoCap設備，它很難調整到超過房間大小的環境中。為了在閉環導航中正確基准化VI-SLAM，基准框架需要被重新配置不來涵蓋不同的因素，例如，傳感器配置，計算/機器人平台，和目標環境。此外，在整個導航過程中，真值都需要被報道。這項工作的目的是為了填補一個開源、閉環基准框架的差異，用來支持上述不同配置的需要；並基於閉環評估的結果，為不同具有代表性的VI-SLAM系統提供不同的性能見解。

3. 閉環系統概覽

閉環軌跡追蹤系統包含兩個主要子系統，如圖3所示。，包括：1）一個VI-SLAM系統輸入視覺/慣性數據來生成高頻的狀態估計和低頻的地圖更新；2）一個使用來自VI-SLAM姿態最終模塊的高頻輸出信息的控制器，用以生成實際的命令。盡管這篇論文僅關注雙目慣性傳感輸入，但是該系統支持其他常見的數據傳感器，例如單目和RGB-D相機。

圖3：閉環軌跡追蹤系統概要。

雖然，構圖和閉合回環對於准確和魯棒的狀態估計很重要，但是這兩個模塊需要很高的計算量，並且它們的計算的頻率比姿態追蹤的低（經常相差一個數量級）。因此，需要在反饋控制中使用的高頻位姿估計信息，被位姿追蹤模塊所搜集。這篇論文，探討了被用來作為反饋控制的狀態追蹤模塊的有效性和准確性。大量的VI-SLAM系統被集成進閉環系統中，涉及代表性的涉及選項，例如松/緊耦合的視覺慣性融合，基於直接/特征數據的關聯，以及基於過濾/BA的后端。

這項研究的關注的是，被使用在閉環中的VI-SLAM系統的軌跡追蹤性能。通過計算在整個導航過程中理想和實際軌跡之間的累計姿態誤差，來反映追蹤性能。這里，我們計算了理想和實際軌跡之間平移誤差的均方根，稱為追蹤RMSE，作為性能指標。追蹤RMSE與ATE公式相匹配，一般用在開環估計，但是卻使用實際機器人軌跡。它直接測量軌跡追蹤系統的最終性能，從而捕獲姿態追蹤漂移和延遲的聯合影響。其他捕獲方向誤差的指標被在線報道。

模擬實驗中使用的移動機器人是差速駕駛TurtleBot2，安裝在機器人上的是一個30fps的雙目相機，其基線為11cm，並且在其底座安置了IMU裝置。來自於雙目相機和IMU裝置的數據流輸入到VI-SLAM系統中，從而獲得\(SE(3)\)的狀態估計輸出。軌跡追蹤控制器使用\(SE(3)\)中的子集\(SE(2)\)估計信息數據，來追蹤目標軌跡。下一章將陳述所實現的VI-SLAM系統、軌跡追蹤控制器和Gazebo/ROS中模擬配置信息。

A. 視覺慣性SLAM系統

幾個可獲得的公開雙目慣性SLAM系統，被選來集成在閉環基准系統中。下面陳述具體5個實現：

1. MSC： MSCKF-VIO + MSF。MSCKF-VIO是一個緊耦合的VIO系統，使用基於KLT的前端和MSCKF的后端。基於EKF的傳感器融合MSF在將其發送到控制器之前，先對MSCKF-VIO的低速率估計輸出進行了壓縮，並且不包含閉環部分。
2. VINS：VINS-Fusion是一個緊耦合的VI-SLAM系統，使用基於KLT的前端和基於BA的后端。VINS具有很大的延遲，是由於使用BA。它提供一個低延遲、高速率的IMU傳播信號，這個信號被發送給控制器。VINS帶有閉環功能，這樣就適用在長期重訪的場景中。執行圓周運動來初始化VINS先驗知識，從而開啟SLAM的估計過程。
3. SVO：SVO + MSF。一個有效的、松耦合的VIO系統，包含直接SVO和融合MSF，並且沒有閉合回環模塊。SVO上述方法中最低的姿態追蹤延遲。
4. ORB：ORB-SLAM + MSF。ORB-SLAM有一個基於特征的前端和一個基於BA的后端。由於需要大量計算的特征提取和匹配，ORB-SLAM具有很大的視覺估計延遲。類似於MSC和SVO，MSF被集成進ORB來生成高頻的估計信號。
5. GF：懶GF-ORB-SLAM + MSF。一個送耦合ORB的變化版本，帶有兩方面高效的調整：良好特征和懶雙目匹配。良好的特征匹配在特征匹配上限的預算下執行目標地圖到圖像幀的匹配。懶雙目調整將雙目ORB-SLAM計算划分為即時姿態估計所需要的內容和哪些組成將來姿態估計計算的內容。前者被有限運行，因此，能夠更快地輸出位姿估計數據。這兩方面的調整降低了延遲，同時對於姿態估計的准確性沒帶來顯著的影響。

如果初始化方法沒有被陳述，那么默認方法，是在開始閉環/開環之前將機器人靜置10秒鍾。

B. 反饋控制

理想軌跡\(d^*(t) \in \Bbb{R}^2\)是通過使用樣條線的一系列指定航路點構建的。希拉雷式機器人的指數穩定軌跡跟蹤控制器產生了機器人可遵循的運動學上可行的軌跡。下面討論中，對於加速度和速度的限制闡述地很清楚，盡管它們存在於實際地實現中。

機器人姿態作為一個時間地函數\(g(t) \in SE(2)\)服從下面地控制等式：

\(\dot{g} = g \cdot \left[ \begin{matrix} v \\ 0 \\ w \end{matrix} \right]\) and \(\dot{v}=u^1 \\ \dot{w}=u^2\) (1)

公式1中，\(v\)是前向速度，\(w\)是角速度，這兩者都存在實體坐標系中。信號\(u=(u^1,u^2)^T\)坐標是前向加速度和角加速度（在實體坐標系中）。

所使用地控制器依賴於機器人移動地差分平整度，從而獲得一個機器人前面虛擬點地指數穩定化數據（通過一個距離\(\lambda\)）。下面定義\(\lambda\)調整選擇矩陣和角速度矩陣：

\(R_{\lambda}=R \cdot diag(1, \lambda)\) and \(\hat{w}(\lambda, \dot{\lambda})=\left[ \begin{matrix} 0 & -\lambda w \\ \frac{1}{\lambda}w & \frac{\dot{\lambda}}{\lambda} \end{matrix} \right]\) (2)

公式2中，\(R\)是\(g\)中方向所提供的選擇矩陣。對於世界坐標系下\(\hat{x}\)向量的單位向量\(e_1\)，軌跡追蹤控制律如下：

\(u=c_pR_{\lambda}^{-1}(d^*-d-\lambda * Re_1)+c_d(R_{\lambda}^{-1}\dot{d^*}-V)-c_d\dot{\lambda}e_1-\hat{w}(\lambda,\dot{\lambda})V-(\hat{w}(\lambda,\dot{\lambda})-c_{\lambda}I)\dot{\lambda}e_1\) (3)

公式3中，\(c_p\)，\(c_d\)，\(c_{\lambda}\)是反饋增益，並且\(V=[v|w]^T\)。額外的偏移動力學是：

\(\dot{\lambda}=-c_{\lambda}(\lambda-\epsilon)\)，其中，\(\lambda(0)>\epsilon > 0, c_{\lambda} > 0\). (4)

動態系統由公式1-4所表示，獲得了一個機器人姿態姿態\(g*(t)\)的參考軌跡和主要的速度成分\(V*(t)\)，用來追蹤理想的軌跡\(d*(t)\)。偏移變量\(\lambda*(t)\)可以被忽略。

實時軌跡控制器驅使機器人去追蹤基於機器人所估計狀態（也就是\(SE(3)\)的一個狀態子集\(SE(2)\)）的反饋的參考軌跡。這些控制命令是：

\(v_{cmd}=k_x*\widetilde{x}+v^*\) (5)

\(w_{cmd}=k_{\theta}*\widetilde{\theta}+k_{y}*\widetilde{y}+w^*\)

其中，\([\widetilde{x}, \widetilde{y}, \widetilde{\theta}]^T \approx g^{-1}g^*\)是在物體坐標系下目前狀態\(g\)和理想狀態\(g^*\)之間的相對位姿誤差。在誤差缺失的情況下，控制信號是\(V^*(t)\)。

C. 模擬配置

這一章描述模擬Gazebo環境，用來測試使用VI-SLAM系統的閉環軌跡追蹤。所創建測的場景用來機器人導航的是一個虛擬辦公室世界（如圖4）。這個虛擬世界是基於一個實際辦公室的平面圖，同時帶有紋理地圖的表面。由於碰撞檢查和路徑規划不在本文討論范圍之內，因此牆要放置在離地面1米的位置。引入碰撞避免會增加另一個耦合因素到閉合系統中，這也就會在確定追蹤錯誤源（即，是否用來避免碰撞或由於錯誤的估計）中引入不需要的復雜度。

圖4：模擬地圖。

六個測試軌跡被創建用來閉環導航實驗，每個都有不同的特定（如圖5所示）。前兩個軌跡相對短（不超過50米），並且具有很少的重訪區域。第三和第四條軌跡都是中等長度（不超過120米），其中第三條有很多的重訪區域，因為它回溯了上次軌跡；相比，第四條軌跡交叉得較早軌跡端，並且是反向或垂直於之前軌跡。最后兩條是長軌跡（不超過240米），其中第五條回溯軌跡段，相比第六條沒有沿着相反方向回溯。所有的軌跡對於機器人來說都是相同的出發點，即都是世界坐標下的原點。測試了三種理想的線性速度：0.5m/s，1.0m/s，和1.5m/s。基於這些速度，模擬中的導航線路從30秒持續的480秒。

4. 實驗結果

這章描述了兩個主要實驗的結果。前一個涉及雙目VI-SLAM方法的開環估計，其中控制器輸入正確的姿態而不是VI-SLAM的估計。開環估計主要有兩個目標：1）它表明，與視頻錄制的開環基准測試（例如EuRoC）相比，該方法在模擬世界中的排名大致得以保留；2）它描述了相對基准的仿真環境的功能范圍。第二個實驗實施閉環追蹤測試，其中控制器輸入VI-SLAM的估計數據。閉環基准的目標是確定VI-SLAM的哪些屬性（即，漂移，延遲），對軌跡追蹤性能由影響。

所有5個VI-SLAM系統的參數配置都是通過參數掃描發現的。對於每個測試配置（理想軌跡，理想線性速度，VI-SLAM方法和IMU），基准的運行被重復了五次，以至於隨機參數，例如多線程數和隨機傳感器噪聲，都能被正確反映。模擬了兩個常被使用的IMU：一個高端ADIS16448和一個低端的MPU6000。開環基准運行在i7-4770（單線程通過得分：2228）。閉環基准運行在一個雙Intel Xeon E5-2680內核的工作站（單線程通過得分：1661）。作為參考，大部分開源閉環導航系統使用一個Intel NUC，它們的CPUs得分在1900-2300之間（單線程）。整個方法模塊，包括模擬器，集成VI-SLAM系統和軌跡追蹤控制器都被公開了。

A. 開環結果與分析

鑒於模擬和記錄的開環基准數據可能不一致，這章實施一次對比來驗證模擬的應用領域。比較表明，模擬場景與現有基准有一些重疊，盡管它們並不跨越整個域。基於相似性，當在等效的實際環境中部署閉環系統時，閉環實現應具有預測能力。

一個EuRoC的子集和模擬的開環序列特征如表1所示。持續時間配置項以描述[2, 10]分鍾間隔的媒介定義；Duration屬性分類為低（0.5m/s）到中等（1.0m/s）再到高（1.5m/s）；Revisit頻率是跟隨的軌跡以及在時間上相距遙遠的軌跡段上出現特征的共同可見性的函數；捕獲的另一個統計數據是使用ORB（最后一列）每幀跟蹤的平均特征數（Feats）。模擬序列比EuRoC序列具有較少的紋理信息。這兩個基准的特征有足夠的重疊，模擬反映了稍微不同的情況。 定性地，相對於EuRoC序列，模擬序列是可比較的或更難的基准測試案例。

表1：模擬數據序列特征。

為了進一步比較，我們針對地面真實性進行了開環基准測試，以了解VI-SLAM算法的姿態估計屬性以及兩個基准測試集在相對順序方面是否一致。 表2和表3匯總了中等運動曲線的5次重復的平均結果，其中省略了磁跡丟失的情況（破折號）。 根據這些表，對於EuRoC序列，VI-SLAM估計和地面真實性之間的ATE通常較低。 相對於所有方法中的主流值，SVO和VINS在EuRoC中均表現出異常，其中SVO為1，而VINS為3。 它們出現在MH序列中，這表明這些通常對於VINS和SVO而言更成問題。 但是，對於模擬情況，SVO和VINS也有一個磁道故障。 總的來說，結果與先前的說法一致，即模擬序列與EuRoC相當或更難。 如果將軌道故障作為對模擬性能結果的懲罰而添加，那么算法的等級排序在EuRoC和模擬之間是一致的。 GF通常具有最低的ATE，而VINS具有最高的ATE。 此外，不同VI-SLAM的相對延遲時間順序也一致：SVO最低，而VINS最高（MSC是唯一的，因為不匹配）。這些比較支持使用仿真對VISLAM進行基准測試，並具有針對特定部署條件的有效性。

表2和3：運動曲線5次重復的平均結果。

B. 閉環結果與分析

軌跡跟蹤性能在表4和表5中進行了量化。所需軌跡和實際軌跡之間的跟蹤RMSE反映了5次重復的平均值。 平均RMSE超過10m的情況被認為是導航失敗，因此省略（破折號）。 每個VI-SLAM中視覺估計的平均等待時間在每個表的底部行中報告（算法按升序排序）。

表4和5：量化后的軌跡追蹤性能。

根據表4和表5，在多種配置下，VINS和ORB均失敗。 與ORB相比，GF的成功率和RMS顯着提高。視覺估計等待時間的減少有助於改善，因為ORB和GF的開環結果在漂移方面相似，但在等待時間方面卻大不相同。 結果表明，滿足標准幀速率潛伏期水平（約30ms）是最好的，而且對於良好的閉環軌跡跟蹤性能來說，可能是必不可少的。基於過濾器的MSC受IMU數據質量的影響很大，因為它無法導航多個低端IMU情況和較高的速度。 結果表明，姿態估計過度依賴IMU，圖2支持了MSC，其中MSC對於檢測到的特征的長期數據關聯性較差。 通過與世界上已知的靜態點鏈接並改善絕對位置估計，能夠重新關聯到丟失的軌道可以改善性能。否則，諸如MSC之類的系統將依賴於可觀察性較差的綜合估計。

最后兩種檢查方法是SVO和GF，它們都成功地跟蹤了除一個序列以外的所有序列的攝像機姿態。 這是兩種性能最強的方法。 有趣的是，它們具有不同的運行時屬性。 對於開環和閉環評估，SVO的等待時間最低，但漂移最高，而GF相反。 從開環到閉環，它們的相對性能保持不變，只是對一小部分序列取模。 看起來低延遲可以容忍較高的漂移，而較低的漂移則允許較高的延遲。 但是，總的來說，似乎等待時間足夠低，相對於閉環軌跡跟蹤的等待時間增強，針對精度增強更好（對於大多數靜態的，具有足夠特征的地面車輛）。比較兩種IMU類型的SVO和GF可以看出，高端IMU提供了最佳的地面速度誤差縮放比例，而GF隨着速度的增加而更加一致。

這些定量結果可以在圖6和7中定性地看到，它們跟蹤了不同VI-SLAM方法的閉環機器人軌跡。 VINS超出了許多運行范圍。 着眼於SVO和GF的痕跡，很明顯，對於給定序列，SVO在運行中具有更高的估計方差，而GF軌跡更緊密地聚集。 該屬性與IMU類型無關。 總體而言，GF似乎是表現最好的。 作為ORB的一種修改，它試圖減少姿勢估計的等待時間，同時保留ORB的有益特性。 本文的研究結果表明，在爭取實現足夠小的延遲的同時優先考慮精度是一種有效的手段，可為自主移動機器人應用識別出高性能的VISLAM。 解決GF的異常情況仍然需要做一些工作。

圖6和7：定性結果。

5. 總結

本文研究了幾種立體VI-SLAM方法，以了解其閉環軌跡跟蹤特性。 這項研究得到了模擬Gazebo環境的支持，該環境表明可以代表一組特定的基准條件。 對結果的分析表明，潛伏期和漂移都在實現精確的軌跡跟蹤中起着重要作用。 建立在ORB-SLAM上的VI-SLAM系統（用GF表示）提供了最准確的軌跡跟蹤結果，這與其開環性能一致。 其他方法不一致。 SVO在閉環中具有較高的性能，但在開環中具有較差的性能，而ORB則相反。 未來的工作將通過其他移動機器人，渲染選項以及視覺環境或設置來擴展基准測試環境。 重要的是，與實際的防撞系統集成以及環境障礙對SLAM的影響將改善基准的任務真實性。

歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、檢測分割識別、三維視覺、醫學影像、GAN、自動駕駛、計算攝影、算法競賽等微信群（以后會逐漸細分），請掃描下面微信號加群，備注：”昵稱+學校/公司+研究方向“，例如：”張三 + 上海交大 + 視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據研究方向邀請進入相關微信群。請勿在群內發送廣告，否則會請出群，謝謝理解~
投稿、合作也歡迎聯系：[e-mail]

長按關注計算機視覺life