1、引言

多年來，慣性導航系統(INS)[1,2]被廣泛用于估計傳感平臺(如自動駕駛車輛)的6自由度姿態(位置和方向)，特別是在GPS信號缺乏的環境，如水下，室內，城市峽谷或在其他星球上。大多數INS依賴于一個6軸慣性測量單元(IMU)，它測量平臺的局部線性加速度和角速度，而平臺與IMU是剛性連接的。隨著近年來硬件設計和制造的進步，低成本的輕量化微電子機械(MEMS) IMU已經普遍存在[3-5]，這使得高精度定位成為可能。移動設備[6]和微型飛行器(MAVS)[7-111]，在廣泛的新興應用領域中有著巨大的影響[12,13]，從移動增強現實(AR)到虛擬現實(VR)[14]再到自動駕駛[15,16]。不幸的是，高速率IMU測量的簡單集成被噪聲和偏置所破壞。往往導致長期導航下姿態估計的不可靠。盡管存在一種高端戰術級IMU，但對于廣泛部署來說，它的成本仍然高得令人望而卻步。另一方面，體積小、重量輕、能效高的相機可提供豐富的環境信息，并作為INS的輔助來源，產生視覺慣性導航系統(VINS)。

雖然這一問題具有挑戰性，因為缺乏能夠減少隨時間累積的運動漂移的全球信息(如果使用低成本、低質量的傳感器，這種情況甚至會加劇)。在過去的十年里，VINS吸引了大量的關注。到目前為止。許多VINS算法可用于視覺慣性SLAM[17]和視覺慣性測程(VIO)，如擴展卡爾曼濾波器(EKF)[17,18. 20 - 22]，無跡卡爾曼濾波UKF[23 - 25]，批處理或增量平滑[26,27]，以及基于滑動窗口的方法 [28-31]。在這些方法中，基于EKF的方法因其效率高而仍然很受歡迎。例如，作為移動設備上最先進的VINS解決方案，Tango[32]項目(或ARCore[12])似乎使用EKF來融合用于運動跟蹤的視覺和慣性測量。然而，近年來預積分技術的進步也使得在基于圖優化的方法高效地融合了高頻IMU測量 [29, 30, 33–35]。

顯然，VINS技術正在興起，主要是由于移動感知和導航應用的高要求，這在該領域產生了大量的文獻。然而，據我們所知，目前還沒有關于VINS的當代文獻綜述，盡管最近有關于SLAM的廣泛調研[16,36]，但并不專門研究VINS。這使得學術界和工業界的研究人員和工程師很難有效地找到和理解與他們的興趣相關的最重要的工作，而這些工作我們多年來在處理這個問題時都經歷過。因此，我們正努力填補這一缺口：(1)提供一個簡潔(由于空間限制)但完整的有關VINS進行狀態估計的關鍵方面的回顧，(2)提供了我們對最重要的相關工作的理解，（3）對尚待解決的挑戰的開放的討論。這是由于希望(至少)幫助研究者/工程師跟隨和理解最先進的VINS算法和系統，從而更有效地加速我們整個研究領域的VINS研究和開發。

2、狀態估計

A.基于濾波的vs.基于優化的估計

Mourikis和Roumeliotis [18]開發了最早成功的VINS算法之一，稱為多態約束卡爾曼濾波器（MSCKF），后來應用于航天器下降和著陸[21]以及快速無人機自主飛行[43]。這種方法使用基于四元數的慣性動力學[37]進行狀態傳播，與有效的EKF更新緊密結合。具體而言，不是將通過相機圖像檢測和跟蹤的特征添加到狀態向量，而是將它們的視覺方位測量值投影到特征雅可比矩陣的零空間（即線性邊緣化[44]），從而保留僅與狀態向量中與隨機克隆的相機位姿[45]關聯的運動約束。在通過移除共同估計可能成百上千個點特征的需要來降低計算量的同時，該操作阻止了特征在以后的非線性測量的重新線性化，從而產生性能的近似惡化。

標準MSCKF [18]最近已經沿著不同的方向進行了擴展和改進。特別是，通過利用我們以前的工作[46-49]中提出的基于可觀性的方法，已經開發了不同的可觀測性約束（OC）-MSCKF算法，通過強制線性化VINS的正確可觀察性來改善濾波器的一致性[19,38-40,50-52]。MSCKF的平方根逆版本，即平方根逆滑動窗口濾波器（SR-ISWF）[6,53]，以提高計算效率和數值穩定性，使VINS能夠在計算資源有限的移動設備上運行，同時不犧牲估計精度。我們已經引入了最佳狀態約束（OSC）-EKF [54,55]，其首先在滑動窗口中最佳地提取關于相對相機位姿的視覺測量中包含的所有信息，然后在EKF更新中使用這些推斷的相對測量。最近采用（右）不變卡爾曼濾波器[56]來改善濾波器一致性[25,57-60]，以及用于機器人中心設備中VINS的（迭代）EKF [22,61-63]。另一方面，在EKF框架中，除了點之外的不同幾何特征也被用于改善VINS性能，例如[64-68]中使用的線特征和[69-72]中的平面特征。此外，基于MSCKF的VINS也擴展到使用具有不準確時間同步的卷簾門相機[64,73]，RGBD攝像機[69,74]，多個攝像機[53,75,76]和多個IMU [77]。雖然基于濾波的VINS已顯示出高精度狀態估計，但它們在理論上受到限制; 也就是說，非線性測量（12）在處理之前必須具有一次性線性化，可能將大的線性化誤差引入估計器并降低性能。

相比之下，批量優化方法解決了一組測量中的非線性最小二乘（束調整或BA[78]）問題，允許通過重新線性化[79,80]減少誤差，但計算成本高。Indelman等[27]使用因子圖來表示VINS問題，然后類似于iSAM[81,82]逐步解決它。為了在應用于VINS時實現恒定的處理時間，通常將有界滑動窗口中的近期狀態視為有效的優化變量，同時邊緣化過去的狀態和測量[28,31,83-85]。特別是Leutenegger等人[28]引入了一種基于關鍵幀的優化方法（即OKVIS），其中一組非連續的過去相機姿態和一系列近期慣性狀態，與慣性測量相關聯，被用于非線性優化以進行精確的軌跡估計。Qin，Li和Shen [31]最近提出了一種基于優化的單目VINS，它可以在非實時線程中包含閉環，而我們最近的VINS [86]能夠在一個僅有線性復雜度線程中有效地利用閉環。

B.緊耦合vs.松耦合傳感器融合

VINS融合視覺和慣導有多種方案，可以廣義分為松耦合和緊耦合兩種。具體來說，松耦合融合，無論是濾波還是基于優化的估計，分別處理視覺和慣導的運動約束，然后融合這些約束(例如 [27,87-91])。雖然該方法具有較高的計算效率，但視覺約束和慣導約束的解耦會導致信息丟失。相比之下，緊耦合的方法直接融合視覺和慣性測量在同一過程，從而實現更高的精度(例如，[18,28,34,40,85,92])。

C.VIOvs.SLAM

通過聯合估計傳感器平臺的位置和周圍環境特征，SLAM估計容易實現加入閉環約束實現有界定位誤差，這在過去三十年中也出現了重大研究成果[16,36,93,94]。VINS可以被視為SLAM的一個實例(使用特定的視覺和慣性傳感器)，在廣義上包括視覺慣性(VI)-SLAM[28,33,85]和視覺-慣導里程計(VIO)[18,22,39,40,52,95]。前者聯合估計由環境特征位置和相機/IMU姿態共同構成狀態向量，而后者狀態向量不包含環境特征，但仍然利用視覺測量在相機/IMU姿態之間施加運動約束。一般來說，加入建圖(從而形成閉環)，VI-SLAM從特征地圖和可能回環檢測中獲得了更好的精度，同時比VIO帶來更高的計算復雜度。盡管已經提出了不同的方法來解決這個問題[18、21、28、85、96、97]。然而，VIO估計器本質上是里程計(航跡推算)方法，除非某些全局信息(例如GPS或先驗地圖)或對以前位置的約束(例如，使用閉環檢測)。許多方法利用不同關鍵幀的特征觀測來限制在軌跡上的漂移[28,98]。大多數都有一個雙線程系統，該系統優化小窗口的局部關鍵幀和限制短時漂移，而后臺進程優化一個長期稀疏位圖，其中包含強制實現長期一致性的閉環約束[31,83,99,100]。例如，VINS-Mono[31,100]在局部滑動窗口和全局批處理優化中都使用了閉環約束。具體地說，在局部優化過程中，關鍵幀的特征觀測提供了隱式的環閉約束，其中通過假設關鍵幀的姿態是理想的(從而將它們從優化中去處)，問題變得很小。

特別是，在VINS中是否通過地圖[83、101、102]和/或位置識別[103 108]實行閉環檢測是VIO和SLAM之間的關鍵區別之一。雖然利用閉環來實現有界誤差的VINS是必要的，但也面臨著由于無法在不做出不一致假設的情況下保持計算效率的挑戰，例如將關鍵幀的姿勢視為正確，或重用信息。為此，[109]提出了一種混合估計器，利用MSCKF進行實時局部估計，并在閉環檢測時觸發全局BA。在濾波器等待BA完成的額外計算期間內，允許一致性重新線性化和閉環約束。最近，Lynen等人[110]開發了一種基于大規模地圖的VINS，該VINS使用包含特征位置及其不確定性的壓縮先驗地圖，并使用先驗地圖中的特征匹配來約束全局估計。DuToit等[102]利用Schmidt KF[111]的思想，開發了一個Cholesky-Schmidt EKF，該EKF使用了一個具有完全不確定性的先驗地圖，并放松了地圖特征與當前狀態變量之間的所有關聯;而我們最新的SchmidtMSCKF[86]將閉環集成在一個線程中。此外，最近的point-line VIO[67]將邊緣關鍵點的三維位置作為閉環的真值，但這可能會引發不一致性。

D.直接vs.間接的視覺處理

視覺處理過程是任何VINS的關鍵部分之一，負責將密集的圖像數據轉換為運動約束，并將其納入估計問題中。其算法根據使用的視覺殘差模型可分為直接算法和間接算法。間接方法[18,28,30,40,51]被視為經典技術，它提取和跟蹤環境中的點特征，同時在估計過程中使用幾何重投影約束。當前最先進的間接視覺SLAM的一個例子是ORB-SLAM2[83,112]，它使用來自3D特征點對應的信息來執行基于圖形的相機姿態優化。

相比之下，直接方法[96,113,115]在公式中利用原始像素強度，并允許包含更大比例的可用圖像信息。LSDSLAM[114]是一個最先進的直接視覺SLAM的例子，它基于最小化相機關鍵幀之間的強度誤差來優化它們之間的轉換。注意，這種方法還優化了一個包含關鍵幀約束的分離的圖，以允許合并高信息量的閉環來糾正長軌跡上的漂移。這項工作后來從單目相機擴展到立體和全方位相機，以提高精度[116,117]。其他流行的直接方法包括[118]和[119]，它們以緊耦合的方式估計關鍵幀深度，并提供低的漂移結果。最近直接方法在VINS上的應用受到了關注，因為即使在低紋理環境中，它們也能夠魯棒跟蹤動態運動。例如，Bloesch等人[61,62]使用基于patch的直接方法提供迭代EKF更新;Usenko等[96]提出了一種基于離散預積分和直接圖像對齊的滑動窗口VINS; Ling, Liu, and Shen [9] 和 Eckenhoff, Geneva, and Huang[115]將不同IMU預積分的直接圖像對齊集成到動態運動估計中[34,35,85]。

由于光度一致性假設，直接圖像對齊需要良好的初始猜測和較高的幀率，而間接視覺跟蹤在提取和匹配特征時需要額外的計算資源。然而，由于間接方法的成熟和魯棒性，它在實際應用中得到了更廣泛的應用，但是直接方法在非結構場景中具有潛力。

E.慣導預積分

Lupton和Sukkarieh[33]首先提出了IMU預積分，這是標準慣性測量積分的一種計算上有效的替代方案，它將慣性測量動力學的離散積分與局部參考系統相結合，從而避免了在每個優化步驟里重新積分狀態動力學的需要。雖然這解決了計算復雜性問題，但由于在方向表示中使用歐拉角，該方法存在奇點。為了提高這種預積分的穩定性，在[29,34]中引入了流形上的表示，其在SO（3）流形上呈現了無奇點定向表示，將IMU預積分結合到基于圖優化的VINS中。

雖然Shen，Michael和Kumar [85]引入了連續狀態下的預積分。但他們在不提供封閉形式解決方案的情況下對測量動態進行離散采樣，這與從連續時間的角度預積分的理論完整性方面存在顯著的區別。與先前方法中使用的預積分測量和協方差計算的離散近似相比，在我們先前的工作[30,35]中，我們得出了測量和協方差預積分方程的閉合形式解，并且這些解決方案提高了在高動態運動的情況下離散方法的精度。

F.狀態初始化

提供準確的初始狀態估計的魯棒、快速初始化對于手持實時VINS估計器至關重要，其通常以線性閉合形式求解[7,84,120-124]。特別是，Martinelli [123]引入了單眼視覺慣性初始化問題的閉合解決方案，后來擴展到陀螺儀偏差校準。這些方法無法模擬慣性積分中的不確定性，因為它們依賴于長時間內IMU測量的雙重積分。費斯勒等人[127]在松散耦合的估計框架內開發了基于SVO [113]的重新初始化和故障恢復算法，但需要額外的向下距離傳感器來恢復尺度。Mur-Artal和Tard'os [83]在他們的ORBSLAM [112]上引入了一個高度（約10秒）的初始化器，它利用已知ORB-SLAM的關鍵幀的視覺慣導全BA計算初始尺度，重力方向，速度和IMU偏差。在[7,84]中，最近提出了一種線性方法用于無噪聲情況，通過利用短期IMU（陀螺儀）預積分獲得的相對旋轉但不對陀螺儀偏差建模。當觀察的是遠處的視覺特征時，這在現實世界中并不可靠。

3、傳感器標定

在融合來自不同傳感器的測量結果時，必須高精度地確定空間和時間傳感器校準參數。特別是，我們應該準確地知道相機和IMU之間的剛體轉換，以便正確地融合從它們的測量中提取的運動信息。此外，由于不正確的硬件觸發，傳輸延遲和時鐘同步誤差，每個傳感器的帶時間戳的傳感數據可能不一致，因此，視覺和慣性測量之間的時間線錯位（時間偏移）可能會發生。因此，校準這些時間偏移很關鍵。傳感器校準空間和時間參數的問題已成為許多最近VINS研究工作的主題[42,128-132]。例如，Mirzaei和Roumeliotis [42]在相機和IMU之間開發了基于EKF的空間校準。進行校準參數的非線性可觀測性分析[133]以表明這些參數在隨機運動下是可觀察的。同樣，Jones和Soatto [128]基于無法區分的軌跡分析檢查了相機和IMU空間校準的識別能力，并在嵌入式平臺上開發了基于濾波器的在線校準。Kelly和Sukhatme[129]通過類似ICP的匹配方法對齊這兩個傳感器的旋轉曲線，解決了相機和IMU之間的剛體轉換問題。

許多這些研究工作都集中在需要額外校準輔助工具的離線校準流程上[42,130,134-136]。特別是，作為最先進的方法之一，Kalibr校準工具箱[130,134]使用傳感器軌跡的連續時間基函數表示[137]來校準多個傳感器的內參和外參。由于該B樣條表示允許直接計算預期的局部角速度和局部線性加速度，因此預期慣性讀數和測量慣性讀數之間的差異用作批量優化公式中的誤差。離線校準的缺點是每次重新配置傳感器套件時都必須執行此操作。例如，如果移除傳感器進行維護并返回，則放置誤差可能導致性能不佳，需要進行時間的重新校準。

相反，在線校準方法在傳感器套件的每個操作期間估計校準參數，從而使得它們在這種情況下更加魯棒并且更容易使用。Kim，Shin和Kweon [138]通過將慣性讀數從IMU幀轉換為第二幀，重新制定了IMU預積分[33-35]。這允許IMU和其他傳感器（包括其他IMU）之間的校準，但不包括時間校準，并且依賴于計算陀螺儀測量的角加速度。Li和Mourikis[131]在移動設備上使用的濾波器框架中，在單個IMU相機對之間同時校準空間和時間外參，后來擴展到包括相機和IMU的內參[139]。Qin和Shen [132]擴展了他們之前關于基于批量的單目VINS [31]的工作，通過插入圖像平面上的特征位置來包括相機和IMU之間的時間偏移。施奈德等人[140]提出了利用信息量最大的運動的可觀察性在線校準。雖然我們最近還分析了時空校準的簡并運動[141]，但尚未完全理解如何對內參進行最優建模并同時校準它們與外參[142,143]。

4、可觀性分析

系統可觀測性在一致狀態估計的設計中起著重要作用[49]，它檢查可用測量提供的信息是否足以估計狀態/參數而沒有模糊[133,144,145]。當系統可觀測時，能觀性矩陣是可逆的，這也與Fisher信息（或協方差）矩陣密切相關[146,147]。由于該矩陣描述了在測量中可用的信息，通過研究其零空間，我們可以獲得有關估計器應獲取信息的狀態空間方向的見解。。在我們之前的工作[46-48,52,146,148-150]中，我們是第一個為機器人定位問題設計可觀察性約束（OC）一致估計器的團隊。從那時起，VINS的可觀察性分析（例如，[19,38,151,152]）成為了一個重要的研究方向。

特別是，VINS非線性可觀測性分析被研究者使用不同的非線性系統分析技術進行了研究。例如，Jones和Soatto [128]以及Hernandez，Tsotsos和Soatto [153]對系統的難以區分的軌跡[154]從可觀察性的角度進行了檢驗。通過采用[155]中的連續對稱概念，Martinelli [122]分析得出了VINS的閉合形式解，并確定了IMU偏差，3D速度，全局滾動和俯仰角是可觀測的。他還研究了退化運動[156]，最小可用傳感器[157]，協同VIO [126]和未知輸入[158,159]對系統可觀測性的影響。基于李導數和可觀測矩陣秩檢驗[133]，Hesch等。[51]分析表明，單目VINS有4個不可觀察的方向，對應于全球偏航和外感受傳感器的全球位置。Guo和Roumeliotis[69]將這種方法擴展到RGBD相機輔助INS，如果點和平面測量都可用，則保留相同的不可觀測方向。根據類似的想法，在[74,129,160]中，分析研究了IMU相機（單目，RGBD）校準的可觀測性，這表明在給定通用運動的情況下IMU和相機之間的外參平移分量是可觀測的。另外，在[161,162]中，具有來自水平平面的向下看的相機測量點特征的系統被示出具有傳感器的可觀測的全局z位置。

實際上，VINS估計器通常建立在線性化系統上，實際上更重要的是對線性化VINS進行可觀測性分析。特別地，線性化VINS系統的可觀測性矩陣[41,163]具有零空間（即，不可觀察的子空間），其理想地跨越全局平移和繞重力矢量的全局旋轉四個方向[19,38,40,51] ]。在為VINS設計非線性估計器時，我們希望估計器使用的系統模型具有由這些方向跨越的不可觀測子空間。然而，如[19,38-40,51]所示，標準EKF不是這種情況。特別是，標準EKF線性化系統在當前狀態估計時使系統和測量函數線性化，具有三個不可觀測的子空間，而不是四個維度。這意味著濾波器從可用測量中獲得不存在的信息，從而導致不一致。為了解決這個問題，采用了第一個估計雅可比（FEJ）思想[47]來提高MSCKF一致性[19,40]，OC方法[48]用于開發OC-VINS[38,39,50] ]。我們最近還開發了robocentricVIO（R-VIO）[22,63]，它保留了與線性化點無關的適當的可觀察性。

隨著慣性和視覺傳感器變得無處不在，視覺慣性導航系統（VINS）已經進行了大量的研究工作，并在過去十年中取得了巨大的進步，在實踐中促進了越來越多的創新應用。作為SLAM問題的一個特例，VINS研究人員在SLAM之上迅速建立了豐富的文獻資料[36]。鑒于該領域發表的論文越來越多，了解最新技術水平變得越來越難（尤其是從業者）。此外，由于特定的傳感器特性，在不了解文獻中現有方法的優缺點的情況下從頭開發VINS算法并非易事（注意每種方法都有其特定的關注點，并不一定解決VINS估計的所有方面）。所有這些都激勵我們對VINS進行研究，據我們所知，遺憾的是，這些研究在現有文獻中是缺乏，因此對于正在研究這個問題的研究人員/工程師來說應該是一個有用的參考。基于我們在該領域的重要工作，我們通過關注構建VINS算法的關鍵方面，包括狀態估計，傳感器校準和可觀察性分析，盡量使這篇論文簡潔而完整。

雖然過去十年中VINS取得了重大進展，但仍有許多挑戰需要應對，下面我們列舉一些可供討論的問題：

?持久定位：雖然目前的VINS能夠提供準確的3D運動跟蹤，但是僅限于在小規模友好的環境中，它們不夠魯棒，不適合長期，大規模，安全關鍵的部署，例如自動駕駛，部分由于資源限制[95,97,164]。因此，要求即使在具有挑戰性的條件（例如不良照明和運動）中也能夠實現持久性VINS，例如通過有效地集成閉環或構建和利用新的地圖。

?語義定位和建圖：雖然點，線和平面[151,165]等幾何特征主要用于當前的VINS進行定位，但這些手動提取的特征可能不適用于導航，并且能夠通過深度學習最新優勢，了解VINS的最佳功能[166]。此外，最近的一些研究努力試圖賦予VINS對環境的語義理解[167-170]，這只是零星地探索但具有很大的潛力。

?高維對象跟蹤：在動態復雜環境中導航時，除了高精度定位外，通常還需要檢測，表示和跟蹤在同一空間中實時共存的移動對象，例如3D對象跟蹤自主導航[92,171,172]。

?分布式協作VINS：雖然已在[126,173]中初步研究了合作VINS，但開發實時分布式VINS仍然具有挑戰性，例如，用于眾包操作。最近關于合作繪圖的工作[174,175]可能會對如何解決這個問題提供一些啟示。

?對不同輔助傳感器的擴展：雖然在許多應用中，光學相機被視為INS的理想輔助源，但其他輔助傳感器可能更適合某些環境和運動，例如，聲學聲納可能在水下使用[176]；低成本的輕量級激光雷達可以在環境中更好地工作，例如在光照條件差的情況下[71,177]；以及事件相機[178,179]可以更好地捕捉動態運動[180,181]。在這個方向上，我們應該研究使用不同輔助源的深入VINS擴展，以便應用于我們手上的工作。

審核編輯：郭婷