三維視覺慣性SLAM的有效Schmidt-EKF
An Efficient Schmidt-EKF for 3D Visual-Inertial SLAM
論文地址：
摘要
為移動和可穿戴傳感器系統實現厘米級精確定位具有重要的實際應用意義。本文提出了一種新的、高精度、高效的視覺慣性（VI）SLAM算法，稱為Schmidt-EKF-VI-SLAM（SEVIS），該算法以緊密耦合的方式將IMU測量和單目圖像最佳融合，以提供具有有界誤差的三維運動跟蹤。采用Schmidt-Kalman濾波公式，在狀態向量中選擇性地包含信息特征，同時在它們成熟后將它們作為干擾參數（或Schmidt狀態）處理。建模的這種變化使得不再需要不斷更新施密特狀態（或它們的協方差），允許EKF正確地解釋它們與活動狀態的相互關系，節省了大量的計算量。在地圖大小方面獲得線性計算復雜度，而不是標准SLAM系統中的二次型。為了充分利用地圖信息來約束導航漂移，提倡有效的關鍵幀輔助的二維到二維特征匹配，以找到當前二維視覺測量和三維地圖特征之間的可靠對應。所提出的SEVIS在仿真和實驗中都得到了廣泛的驗證。
本文創新點
該文件的主要貢獻包括：
•設計了一種高精度、高效的基於Schmidt-e KF的VI-SLAM（即SEVIS）算法，該算法利用Schmidt-KF公式，允許對用於長期環路閉合的環境地圖進行並行估計，以線性計算復雜度將導航漂移綁定。
•提出了一種關鍵幀輔助的2D-to-2D匹配方案，用於在不執行3D-to-2D匹配（這可能不適用於稀疏3D環境地圖）的情況下，將2D視覺測量與3D地圖特征匹配的具有挑戰性的數據關聯問題。這種二維到二維匹配不受估計性能的影響，允許長期的環路閉合和從極端漂移中恢復。
•在蒙特卡羅模擬和實際實驗中廣泛驗證了所提出的SEVIS算法，顯示了的系統提供的適用性和性能增益。對計算要求的實驗研究進一步表明，所提出的SEVIS在建立和維護基於三維特征的地圖時仍然是實時的。
Visual-Inertial SLAM
VI-SLAM過程將攝像機圖像和IMU（陀螺儀和加速度計）測量數據進行最佳融合，提供傳感器平台的6自由度姿態估計以及環境特征（map）的三維位置重建。將簡要介紹EKF框架中的VI-SLAM，是SEVIS算法的基礎。VI-SLAM的狀態向量包含IMU導航狀態xI和克隆的過去IMU（或相機）姿勢xC的滑動窗口，如MSCKF[30]中所示，以及在全局幀中表示的地圖特征的位置xS：
Schmidt-EKF based VI-SLAM
狀態估計和協方差的EKF更新在map特征的數量方面具有二次復雜性[38]，這使得VI-SLAM的天真實現太昂貴而無法實時運行。利用SKF[44]，提出了一種新的用於VI-SLAM（SEVIS）的Schmidt-EKF算法，降低了這種二次復雜度。關鍵思想是有選擇地將map特征視為狀態向量（即Schmidt state xS（1））中的干擾參數，均值和協方差將不再被更新，與激活狀態xA的互相關仍被利用和更新。提議的sevi的IMU傳播與標准EKF相同。在接下來的內容中，主要關注單目圖像的更新，這是SEVIS的核心，但是這種方法很容易擴展到立體系統。當相機IMU傳感器對在環境中移動時，使用基於描述符的跟蹤來跟蹤特征。首先檢測到快速特征[41]，並為每個特征提取ORB描述符[43]。OpenCV[36]“BruteForceHamming”KNN描述符匹配器用於查找對應關系，在前兩個返回之間執行比率測試以確保有效匹配，並使用8點RANSAC來拒絕任何額外的異常值。一旦發現視覺軌跡，將使用三種類型的跟蹤特征來有效地更新狀態估計和協方差：
（i）具有機會性且只能在短時間內跟蹤的VIO特征。
（ii）比上述特征更穩定且可以在當前滑動窗口之外跟蹤的SLAM特征。
（iii）地圖特征，是成熟和信息量大的SLAM特征，在一段獨立的時間內保持在Schmidt狀態。
為了執行SLAM和利用地圖約束來限制導航錯誤的同時獲得顯著的計算節省，采用SKF方法[44]並將map特征視為干擾參數，跟蹤它們與活動狀態的相互關系，同時在更新期間仍允許它們的概率包含。特別是，計算施密特科夫的增益矩陣如下：
已經充分利用當前相機測量信息來更新SEVIS狀態估計和協方差。算法1概述了該算法的主要步驟。
為了驗證所提出的SEVIS的后端估計引擎，首先對具有已知測量特征對應關系的視覺慣性SLAM進行Monte Carlo模擬，其中單目視覺慣性傳感器平台在圓柱競技場內的圓形軌道上移動，觀察一系列環境特征。傳感器和彈道的仿真參數見表1。
特別是，比較了三種VIN算法，以揭示所提出的SEVIS的優點：
（i）基線VIO方法，包括增加6個SLAM特征的MSCKF（見[23]）。這些大滿貫的特點是明確邊緣化時，他們離開視野。
（ii）基線SLAM方法，使用相同的MSCKF窗口，但增加了90個SLAM特征。與上面的VIO不同，在這種情況下SLAM特性從未被邊緣化，因此它們可以用於（隱式）循環閉包。
（iii）提出的SEVIS算法，由與基線VIO相同的MSCKF窗口和6個SLAM特征組成，同時增加了一組90個地圖特征，這些特征被建模為干擾參數。當SLAM特征離開視場時，它們被移動到Schmidt狀態，成為算法1中描述的地圖特征。50次蒙特卡羅模擬運行的平均平方根誤差（RSSE）性能如圖2所示。
正如預期的那樣，基線VIO隨着時間的推移在方向和位置上累積漂移，而基線SLAM在沒有長期漂移的情況下提供有界誤差性能。有趣的是指出，位置RSSE根據相對於初始循環閉包的位置略有振盪。這是因為EKF與基於優化的方法不同，它不能重新線性化過去的測量值，因此修正這些誤差的能力有限[48]。更重要的是，很明顯，所提出的SEVIS算法也不會累積長期漂移，盡管其精度略低於基線SLAM。然而，考慮到SEVIS相對於map特征的數量具有線性計算復雜性，而基線SLAM具有二次復雜性，這種精度的降低是一個很小的代價。
Real-World Experimental Results
使用兩種不同的誤差度量：絕對軌跡誤差（ATE）和相對誤差（RE）來評估所提出的方法。將讀者引向[54]了解這些錯誤度量的詳細定義。除了的基線和建議的方法之外，還對VINSMono[40，39]進行了額外的評估，以提供與利用循環閉合信息的當前最先進方法的比較。如表2和圖4所示，所提出的SEVIS能夠高精度定位並在全基線VI-SLAM系統的水平上執行。從RE來看，很明顯，包含map特征可以防止長期漂移，並且隨着軌跡段長度的增長，RE幾乎恆定，從而提供更高的精度。提議的SEVIS提供了一個計算上可行的濾波器，其精度與全基線VI-SLAM相似，具有與VINS Mono類似的競爭性能（盡管VINS Mono利用了批處理優化）。與全協方差SLAM相比，所提出的SEVIS算法的主要優點是降低了計算復雜度。這種方法的實用性在不同算法的運行時間中是顯而易見的。如圖5所示，評估了這三個系統並收集了實現的時序統計信息。
進一步挑戰了在困難的室內夜間多樓層數據集上提出的系統，該數據集具有多個挑戰，包括低光環境、長曝光時間和運動模糊的低對比度圖像不適合正確的特征提取（見圖6）。如果能夠提取特征，由於噪聲大、梯度小，得到的描述符匹配較差，與Vicon循環數據集相比，更新過程中使用的異常值較多，導致估計量跳變大，校正不正確。由於關鍵幀輔助的2D-to-2D匹配對估計性能差或漂移和地圖特征更新具有不變性，因此所提出的SEVIS可以在這些場景中恢復，並防止錯誤的漂移。
Realsense ZR300傳感器3用於采集20分鍾的灰度單目菲舍耶圖像，具有慣性讀數，1.5公里的軌跡跨越兩層樓。執行了攝像頭到IMU外部的在線校准，以進一步確定制造商驅動程序提供的轉換。最多允許700個地圖點，以便充分覆蓋地圖區域，在4Hz時插入查詢數據庫以確保所有地圖要素的充分覆蓋的關鍵幀，以及一次處於活動狀態的兩個SLAM要素。由基線VIO和提議的sevi生成的軌跡如圖7所示。顯然，地圖特征的加入防止了基線VIO所經歷的長期漂移，這種漂移在偏航和z軸方向上都顯示出很大的誤差。由於此數據集沒有可用的基本事實，作為一種常見的做法，計算了當傳感器平台返回到起始位置時理想情況下應等於零的軌跡起點-終點誤差。基線VIO的誤差為4.67m（彈道距離的0.31%），而建議的SEVIS的誤差僅為0.37m（彈道距離的0.02%）。