回到目錄

寫在前面

港科大的VINS-Mono作為目前state of the art的開源VIO專案，是研究視覺與IMU緊耦合的必讀演算法，網上的論文解讀與程式碼實現也非常豐富（感謝！）。為更好地進行學習，本人在幾篇VINS論文解讀與翻譯的參考與改進下，完成了自己對全文的閱讀與翻譯，以後將逐步更新自己對演算法的解讀，歡迎大家一起討論。
原文：VINS-Mono A Robust and Versatile Monocular Visual-Inertial State Estimator
VINS程式碼地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono

翻譯參考：
VINS-Mono翻譯 by 是皮皮攀呀，（以這篇文章為主要參考）
【泡泡圖靈智庫】VINS-Mono：一種魯棒多功能的單目視覺慣性狀態估計器 by 泡泡機器人SLAM
VINS-Mono：一種功能強大且通用的單目視覺慣性狀態估計器 by 陌上塵離

VINS-Mono：一種魯棒且通用的單目視覺慣性狀態估計器

摘要：由一個相機和一個低成本慣性測量單元(IMU)組成的單目視覺慣性系統(VINS)，構成了用於度量六自由度狀態估計的最小感測器套件。然而，由於缺乏直接距離測量，在IMU處理、估計器初始化、外部標定和非線性優化等方面提出了重大挑戰。本文提出了VINS-Mono：一種具有魯棒性和通用性的單目視覺慣性狀態估計器。該方法從用於估計器初始化和故障恢復的魯棒的程式開始。採用一種基於緊耦合、非線性優化的方法，通過融合預積分後的IMU測量值和特徵觀測值，獲得高精度的視覺慣性里程計。結合緊耦合方法，迴環檢測模組能夠以最小的計算代價實現重定位。此外，我們還進行四自由度位姿圖優化，以加強全域性一致性。我們驗證了該系統在公共資料集和真實環境實驗的效能，並與其他最先進的演算法進行了比較。我們還在MAV平臺上執行機載閉環自主飛行，並將演算法移植到基於iOS的demo中。特別強調的是，本文提出的工作是一個可靠、完整和通用的系統，適用於需要高精度定位的不同應用。我們為PC和iOS移動裝置開源了我們的實現方法。

關鍵詞：單目視覺慣性系統，狀態估計，感測器融合，SLAM

I. 引言

狀態估計無疑是機器人導航、自主駕駛、虛擬現實(VR)和增強現實(AR)等廣泛應用中最基本的模組。僅使用單目攝像機的方法由於其體積小、成本低和硬體設定簡單而獲得了社會的極大興趣[1]-[5]。然而，單目視覺系統無法恢復度量尺度，因此限制了它們在實際機器人中的應用。近年來，我們看到了一種發展趨勢，即用低成本慣性測量單元(IMU)輔助單目視覺系統。這種單目視覺-慣性系統(VINS)的主要優點是具有可觀測的度量尺度，以及翻滾角(roll)和俯仰角(pitch)。這讓需要有度量的狀態估計的導航任務成為可能。此外，對IMU測量值的積分可以顯著提高運動跟蹤效能，彌補光照變化、缺少紋理的區域或運動模糊的視覺軌跡損失的差距。事實上，單目VINS不僅廣泛應用於移動機器人、無人機和移動裝置上，還是滿足充分自我感知和環境感知的最小感測器。

然而，所有這些優點都是有代價的。對於單目VINS，眾所周知，需要加速度激勵以測量尺度。這意味著單目VIN估計器不能從靜止狀態啟動，而是從未知的移動狀態發動。同時要認識到視覺慣性系統高度非線性的事實，在估計器初始化方面還有重大挑戰。兩個感測器的存在也使得攝像機-IMU的外部校準至關重要。最後，為了消除在可接受的處理視窗內的長期漂移，提出了一個完整的系統，包括視覺慣性里程計、迴環檢測、重定位和全域性優化。

為了解決所有這些問題，我們提出了VINS-Mono，一個魯棒且通用的單目視覺慣性狀態估計器。我們的解決方案開始於即時估計初始化。這個初始化模組也用於故障恢復。我們的解決方案的核心是一個魯棒的基於緊耦合的滑動窗非線性優化的單目視覺慣性里程計(VIO)。單目VIO模組不僅提供精確的區域性姿態、速度和方位估計，而且還以線上方式執行攝像機IMU外部校準和IMU偏置校正。使用DBoW2[6]進行迴環檢測。重新定位是在對單目VIO進行特徵級別融合的緊耦合設定中完成。這使得重新定位具有魯棒性和精確性且有最小的計算代價。最後，幾何驗證的迴環被新增到位姿圖中，並且由於來自單目VIO的可觀測的翻滾角和俯仰角，生成四自由度(DOF)位姿圖以確保全域性一致性。

VINS-Mono結合並改進了我們先前在單目視覺-慣性融合方面的工作[7]-[10]。它建立在我們緊耦合、基於優化的單目VIO的公式之上[7][8]，並結合了[9]中引入的改進初始化過程。[10]中給出了移植到移動裝置的第一次嘗試。與我們以前的工作相比，VINS-Mono的進一步改進包括改進的含偏置校正的IMU預積分、緊耦合重定位、全域性位姿圖優化、廣泛的實驗評估以及魯棒和通用的開源實現。

整個系統完整且易於使用。它已經被成功應用於小規模AR場景、中型無人機導航和大規模狀態估計任務。與其他最先進的方法相比具有優異的效能。為此，我們總結了我們的貢獻，如下所示：
1、一個魯棒的初始化過程，它能夠從未知的初始狀態引導系統。
2、一個緊耦合、基於優化的單目視覺慣性里程計，具有相機-IMU外部校準和IMU偏置估計。
3、線上迴環檢測與緊耦合重定位。
4、四自由度全域性位姿圖優化。
5、用於無人機導航、大規模定位和移動AR應用的實時效能演示。
6、完全集成於ros的pc版本以及可在iphone 6或更高版本上執行的IOS版本的開原始碼。

論文的其餘部分如下：在第二節中，我們討論了相關的文獻。在第三節中，我們對完整的系統框架進行了概述。在第四節中，給出了視覺的預處理和IMU測量值的預積分步驟。在第五節中，我們討論了估計器的初始化過程。在第六節中提出了一種緊耦合、自標定、非線性優化的單目VIO。第七節和第八節分別給出了緊耦合重定位和全域性位姿圖優化。實施細節和實驗結果見第九節。最後，第十節本文對研究方向進行了探討和展望。

II. 相關工作

關於基於單目視覺的狀態估計/里程計SLAM的學術工作非常廣泛。值得注意的方法包括PTAM[1]、SVO[2]、LSD-SLAM[3]、DSO[5]和ORB-SLAM[4]。顯然，嘗試對任何方法進行全面回顧都無法完整。然而，在這一節中，我們跳過了關於只使用視覺的方法的討論，而只專注於關於單目視覺慣性狀態估計的最相關的結果。

處理視覺和慣性測量的最簡單的方法是鬆耦合的感測器融合[11][12]，其中IMU被視為一個獨立的模組，用於輔助運動的視覺結構(sfm)獲得的純視覺位姿估計。融合通常由擴充套件卡爾曼濾波(EKF)完成，其中IMU用於狀態傳播，而視覺位姿用於更新。進一步說，緊耦合視覺慣性演算法要麼基於EKF[13]-[15]，要麼基於圖優化[7][8][16][17]，其中相機和IMU測量值是從原始測量水平聯合優化的。一種流行的基於EKF的VIO方法是MSCKF[13][14]。MSCKF在狀態向量中維護以前的幾個攝像機位姿，並使用多個攝像機檢視中相同特徵的視覺測量來形成多約束更新。SR-ISWF[18][19]是MSCKF的擴充套件。它採用squareroot形式[20]實現單精度表示，避免了較差的數值性質。該方法採用逆濾波器進行迭代再線性化，使其與基於優化的演算法相當。批量圖優化或集束調整技術（BA）維護和優化所有測量值以獲得最優狀態估計。為了達到恆定的處理時間，流行的基於圖的VIO方法[8][16][17]通常採用邊緣化過去的狀態和測量來優化最近狀態的有界滑動視窗。由於對非線性系統迭代求解的計算要求很高，很少有基於圖的非線性系統能夠在資源受限的平臺（如手機上）實現實時效能。

對於視覺測量處理，根據視差模型的定義，演算法可分為直接法和間接法。直接法[2][3][21]最小化光度誤差，而間接法最小化幾何位移。直接法因其吸引區域小，需要很好的初始估計，而間接法在提取和匹配特徵時需要額外的計算資源。間接法由於其成熟性和魯棒性，在實際工程部署中得到了廣泛的應用。然而，直接法更容易擴充套件到稠密建圖，因為它們是直接在畫素級別上操作的。

在實踐中，IMU通常以比攝像機更高的速率獲取資料。不同的方法被提出來處理高速率的IMU測量值。最簡單的方法是在基於EKF的方法中使用IMU進行狀態傳播[11][13]。在圖優化公式中，為了避免重複的IMU重複積分，提出了一種有效的方法，即IMU預積分(IMU pre-integration)。這種方法在[22]中首次提出的，它用尤拉角來引數化旋轉誤差。在我們先前的工作中[7]，我們提出了一種流形上的IMU預積分旋轉公式，利用連續IMU誤差狀態動力學推導了協方差傳遞方程。然而IMU偏置被忽略了。在[23]中通過增加後驗IMU偏置校正，進一步改進了預積分理論。

精確的初始值對於引導任何單目VINS是至關重要的。在[8][24]中提出了一種利用短期IMU預積分相對旋轉的線性估計器初始化方法。但是，該方法不對陀螺儀偏置進行建模，無法在原始投影方程中對現代感測器噪聲進行建模。在實際應用中，當視覺特性遠離感測器套件時，這會導致不可靠的初始化。在[25]中給出了單目視覺慣性初始化問題的一種封閉解。隨後，在[26]中提出了對這種封閉形式的解決方案的擴充套件，增加了陀螺儀的偏置校準。這些方法依賴於長時間內IMU測量的雙重積分，無法模擬慣性積分的不確定性。在[27]中，提出了一種基於SVO的重初始化和故障恢復演算法。這是一種基於鬆耦合融合框架的實用方法。然而，需要額外的朝下的距離感測器來恢復度量尺度。在[17]中引入了一種建立在ORB-SLAM[4]上的初始化演算法。給出了一組ORB-SLAM的關鍵幀，計算了視覺慣性全域性BA的尺度、重力方向、速度和IMU偏置的初步估計。然而，尺度收斂所需的時間可能超過10秒。這可能會給需要在一開始就進行尺度估計的機器人導航任務帶來問題。

VIO方法，不管它們所依賴的基本數學公式，在全域性的平移和旋轉中長期受到漂移的影響。為此，迴環檢測在長期操作中起著重要的作用。ORB-SLAM[4] 利用了詞袋模型能夠閉合迴環並重用地圖。迴環檢測之後進行7自由度（位置、方向和尺度）的位姿圖優化。相對於單目VINS，由於IMU的加入，漂移只發生在4自由度，即三維平移，和圍繞重力方向的旋轉（偏航角）。因此，本文選擇在最小四自由度設定下，優化具有迴環約束的位姿圖。

III.概述

提出的單目視覺慣性狀態估計器的結構如圖2所示。該系統從測量預處理(IV)開始，在其中提取和跟蹤特徵，對兩個連續幀間的IMU測量值進行預積分。初始化過程(V)提供了所有必要的值，包括姿態、速度、重力向量、陀螺儀偏置和三維特徵位置，用於引導隨後的基於非線性優化的VIO。VIO(VI)與重定位(VII)模組緊密地融合了預先積分的IMU測量、特徵觀測和迴環重新檢測到的特徵。最後，位姿圖優化模組(VIII)接受幾何驗證的重定位結果，並進行全域性優化以消除漂移。VIO、重新定位和位姿圖優化模組在多執行緒設定中同時執行。每個模組有不同的執行速度和實時保證，以確保在任何時候可靠執行。

我們現在對整篇論文中使用的符號和座標系進行定義。我們認為(.)^w是世界座標系(world frame)。重力方向與世界座標系z軸對齊。(·)^b是本體座標系(body frame)，我們把它定義為與IMU座標系相同。(·)^c是相機座標系(camera frame)。我們同時使用旋轉矩陣R和Hamilton四元數q來表示旋轉。我們主要在狀態向量中使用四元數，也用旋轉矩陣來表示三維向量的旋轉。 $q_b^w，p_b^w$表示從本體座標系到世界座標系的旋轉和平移。b_k表示獲取第k個影象時的本體座標系。c_k表示獲取第k個影象時的相機座標系。⊗表示兩個四元數之間的乘法運算。g_w=[0,0,g]^T是世界座標系上的重力向量。最後，我們將(^)表示為某一具體量的噪聲測量值或估計值。

IV.測量預處理

本節介紹VIO的預處理步驟。對於視覺測量，我們跟蹤連續幀之間的特徵，並在最新幀中檢測新特徵。對於IMU測量，我們在兩個連續幀之間做預積分。請注意，我們使用的低成本IMU的測量值受到偏置和噪聲的影響。因此，我們在IMU預積分過程中特別考慮偏置。

A.視覺處理前端

對於每一幅新影象，KLT稀疏光流演算法對現有特徵進行跟蹤[29]。同時，檢測新的角點特徵[30]以保證每個影象特徵的最小數目(100-300)。該檢測器通過設定兩個相鄰特徵之間畫素的最小間隔來執行均勻的特徵分佈。二維特徵首先是不失真的，然後在通過外點剔除後投影到一個單位球面上。利用基本矩陣模型的RANSAC演算法進行外點剔除。

在此步驟中還選擇了關鍵幀。我們有兩個關鍵幀選擇標準。第一是與上一個關鍵幀的平均視差。如果在當前幀和最新關鍵幀之間跟蹤的特徵點的平均視差超出某個特定閾值，則將該幀視為新的關鍵幀。請注意，不僅平移，旋轉也會產生視差。然而，特徵點無法在純旋轉運動中三角化。為了避免這種情況，在計算視差時我們使用陀螺儀測量值的短時積分來補償旋轉。請注意，此旋轉補償僅用於關鍵幀選擇，而不涉及VINS公式中的旋轉計算。為此，即使陀螺儀含有較大的噪聲或存在偏置，也只會導致次優的關鍵幀選擇結果，不會直接影響估計質量。另一個標準是跟蹤質量。如果跟蹤的特徵數量低於某一閾值，我們將此幀視為新的關鍵幀。這個標準是為了避免跟蹤特徵完全丟失。

B.IMU預積分

IMU預積分是在[22]中首次提出的，它將尤拉角的旋轉誤差引數化。在我們先前的工作中[7]，我們提出了一個流形上的IMU預積分旋轉公式。該文利用連續時間的IMU誤差狀態動力學推導協方差傳遞函式，但忽略了IMU偏置。文[23]通過增加後驗IMU偏置校正，進一步改進了預積分理論。本文通過引入IMU偏置校正，擴充套件了我們在前面工作[7]中提出的IMU預積分。

IMU的原始陀螺儀和加速度計測量結果 $\hat w$和 $\hat a$如下：

IMU測量值是在本體座標系中測量的，它是平衡重力和平臺動力的合力，並受到加速度偏置b_a、陀螺儀偏置b_w和附加噪聲的影響。假設加速度計和陀螺儀測量值中的附加噪聲為高斯噪聲， $n_a∼N(0,σ_a^2)$， $n_w∼N(0,σ_w^2)$。加速度計偏置和陀螺儀偏置被建模為隨機遊走，其導數為高斯性的， $n_{b_a}∼N(0,σ_{b_a}^2)$， $n_{b_w}∼N(0,σ_{b_w}^2)$。

給定對應於體座標系b_k和b_k+1的兩個時刻，位置、速度和方向狀態可以在時間間隔[t_k,t_k+1]間，在世界座標系下中通過慣性測量值傳遞：

∆t_k是時間間隔[t_k,t_k+1]之間的持續時間。

可見，IMU狀態傳遞需要座標系b_k的旋轉、位置和速度。當這些起始狀態改變時，我們需要重新傳遞IMU測量值。特別是在基於優化的演算法中，每次調整位姿時，都需要在它們之間重新傳遞IMU測量值。這種傳遞策略在計算上要求很高。為了避免重新傳遞，我們採用了預積分演算法。

將參考座標系從世界座標系轉變為區域性座標系b_k後，我們只能對線性的加速度 $\hat a$和角速度 $\hat w$相關的部分進行預積分，如下所示：

可以看出預積分項(6)能通過將bk視為參考幀的IMU測量值單獨得到。 $α_{b_{k+1}}^{bk}、β_{b_{k+1}}^{bk}、γ_{b_{k+1}}^{bk}$只與b_k和b_k+1中的IMU偏置有關，與其他狀態無關。當偏置估計發生變化時，若偏置變化很小，我們將 $α_{b_{k+1}}^{bk}、β_{b_{k+1}}^{bk}、γ_{b_{k+1}}^{bk}$按其對偏置的一階近似來調整，否則就進行重新傳遞。這種策略為基於優化的演算法節省了大量的計算資源，因為我們不需要重複傳遞IMU測量值。

對於離散時間的實現，可以採用不同的數值積分方法，如尤拉積分、中點積分、RK4積分等。這裡選擇了尤拉積分來演示易於理解的過程(我們在程式碼中使用了中點積分)。

在開始時， $α_{b_k}^{b_k}$、 $β_{b_k}^{b_k}$是0， $γ_{b_k}^{b_k}$是單位四元數。α，β，γ在(6)中的平均值是如下逐步傳遞的。注意，增加的噪聲項na，nw是未知的，在實現中被視為零。這得到了預積分的估計值，標記為 $(\hat ·)$：

i是在[t_k,t_k+1]中IMU測量值對應的離散時刻，δt是IMU測量值i和i+1之間的時間間隔。

然後討論協方差傳遞問題。由於四維旋轉四元數 $γ_t^{b_k}$