Abstract

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

 

I. INTRODUCTION

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

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

為了解決所有這些問題，我們提出了VINS-MONO，一個強大的和多功能的單目視覺慣性狀態估計。我們的解決方案開始於即時估計初始化。這個初始化模組也用於故障恢復。我們的解決方案的核心是一個魯棒的基於緊耦合滑動窗非線性優化的單目視覺慣性里程計（VIO）。單目

VIO模組不僅提供精確的區域性姿態、速度和方位估計，而且還以線上方式執行攝像機IMU外部校準和IMU偏置校正。使用DBoW進行迴環檢測。重新定位是在對單目VIO進行特徵級別融合的緊耦合設定中完成。這使得重新定位具有魯棒性和精確性且有最小的計算開銷。最後，幾何驗證迴路被新增到姿態圖中，並且由於來自單目VIO的可觀察的滾轉和俯仰角，生成四自由度（DOF）姿態圖以確保全域性一致性。

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

整個系統完整且易於使用。它已經被成功應用於小規模AR場景、中型無人機導航和大規模狀態估計任務。已經針對現有技術方法的其它狀態示出了優異的效能。為此，我們總結了我們的貢獻如下所示：

*一個健壯的初始化過程，它能夠從未知的初始狀態引導系統。

*一個緊耦合，優化的單目視覺慣性里程計與相機-IMU外部校準和IMU偏置估計。

*線上迴環檢測與緊耦合重定位。

*四自由度全域性姿態圖優化。

*用於無人機導航、大規模本地化和移動AR應用的實時效能演示。

*與ros完全積分的pc版本以及在iphone 6或更高版本上執行的IOS版本的開源版本。

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

 

II. RELATED WORK

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

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

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

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

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

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

 

III.OVERVIEW

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

我們現在定義在整個論文中使用的符號和框架定義。我們認為(.)^w是世界框架。重力的方向與世界幀的z軸對齊。(·)^b是物體框架，我們把它定義為與IMU框架相同。(·)^c是相機框架。我們使用旋轉矩陣R和Hamilton四元數q來表示旋轉。我們主要在狀態向量中使用四元數，但三維向量的旋轉用旋轉矩陣來方便表示。q_w^b,P_W^b是從B幀到W幀的旋轉和平移。b_k是當取k個影象時的B幀。c_k是在獲取k個影象時的相機幀。⊗表示兩個四元數之間的乘法運算。gw=[0,0,g]^T是世界範圍內的重力向量。最後，我們將(^{^}_·)表示為某一量的噪聲測量值或估計值。

 

IV.MEASUREMENT PREPROCESSING

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

A.視覺處理前端

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

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

B.IMU預積分

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

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

IMU測量值是在B幀中測量的，它結合了對抗重力和平臺動力學的力量，並受到加速度偏置b_a、陀螺儀偏置b_w和附加噪聲的影響。假設加速度計和陀螺儀測量中的附加噪聲為高斯噪聲，n_a∼N(0,σ_a ²)，n_w∼N(0,σ_w²)。加速度計偏置和陀螺儀偏置被建模為隨機遊走，其導數為高斯性的，n_ba∼N(0,σ_ba ²)，n_bw∼N(0,σ_bw²)。

       給定對應於影象幀的兩個時刻b_k和b_k+1，位置、速度和方向狀態可以在時間間隔期[t_k,t_k+1 ]間在W幀內通過慣性測量傳播：

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

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

       在將參考幀從世界幀改為區域性幀bk後，我們能對只與線性的加速度a^和角速度相關的部分進行預積分，如下所示：

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

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

    在開始時， 是0，是恆等四元數。α,β,γ在(6)中的平均值是按以下步驟逐步傳播的。注意，加性噪聲項n_a,n_w是未知的，在實現中被視為零。這導致了預積分的估計值，標記為(^·)：

i是與[t_k,t_k+1]內的IMU測量相對應的離散時刻,δt是兩個IMU測量i和i+1之間的時間間隔。

然後討論協方差傳播問題。由於四維旋轉四元數γ_t^bk被過引數化，我們將其誤差項定義為圍繞其平均值的擾動：

其中δθ^bk_t是三維小擾動。

我們可以匯出連續時間線性化的誤差項(6)的動力學：

P_bk^bk+1可以通過初始協方差P_bk^bk=0的一階離散時間協方差更新遞迴計算：

其中Q是噪聲的對角線協方差矩陣(σ_a²,σ_w²,σ_ba²,σ_bw²)。

同時，δz _bk^bk+1的一階Jacobian矩陣J_bk+1相對於δz_bk^bk也可以用初始Jacobian J_bk=I遞迴計算。

利用這個遞推公式，得到協方差矩陣p _bk^bk+1和Jacobian J_bk+1。 關於偏置的一階近似可以寫為:

其中J_ba^α是J_bk+1中的子塊矩陣，其位置對應於 。 也使用同樣的含義。當偏置估計發生輕微變化時，我們使用(12)近似校正預積分結果，而不是重傳。

現在我們可以寫下IMU測量模型及其對應的協方差P_bk^bk+1：

V. ESTIMATOR INITIALIZATION

       單目緊耦合VIO是一個高度非線性的系統.由於單目相機無法直接觀測到尺度，因此，如果沒有良好的初始值，很難直接將這兩種測量結果融合在一起。人們可以假設一個固定的初始條件來啟動單目vins估計器。然而，這種假設是不合適的，因為在實際應用中經常會遇到運動下的初始化.當IMU測量結果被大偏置破壞時，情況就變得更加複雜了。事實上，初始化通常是單目vins最脆弱的步驟。需要一個健壯的初始化過程來確保系統的適用性。

       我們採用鬆耦合的感測器融合方法得到初始值.我們發現，只有視覺的SLAM，或SfM，具有良好的初始化性質。在大多數情況下，視覺系統可以通過從相對運動方法(如八點或五點演算法或估計齊次矩陣)中匯出初始值來引導自己。通過將度量IMU預積分與目視SfM結果相匹配，我們可以粗略地恢復尺度、重力、速度，甚至偏置。這足以引導非線性單目vins估計器，如圖4所示。

       與在初始階段同時估計陀螺儀和加速度計偏置的[17]相比，我們在初始階段選擇忽略加速度計偏置項。加速度計偏置與重力耦合，由於重力向量相對於平臺動力學的大量級，以及初始階段相對較短，這些偏置項很難被觀測到。我們以前的工作對加速度計偏置標定進行了詳細的分析。

A. Sliding Window Vision-Only SfM

       初始化過程開始於一個只有視覺的SfM來估計一個高比例相機的姿勢和特徵位置的圖表。

       我們保持了一個幀的滑動視窗來限制計算複雜度。首先，我們檢查了最新幀與之前所有幀之間的特徵對應。如果我們能找到穩定的特徵跟蹤(超過30個跟蹤特徵)和足夠的視差(20個以上的旋轉補償畫素)在滑動視窗中的最新幀和任何其他幀之間，我們恢復相對旋轉和這兩個幀之間的上尺度平移使用五點演算法。否則，我們會將最新的幀儲存在視窗中，並等待新的幀。如果五點演算法成功的話，我們可以任意設定標度，並對這兩個幀中觀察到的所有特徵進行三角化。基於這些三角特徵，採用透視n點(Pnp)方法來估計視窗中所有其他幀的姿態。最後，應用全域性整束平差[36]將所有特徵觀測的總重投影誤差降到最小。由於我們還沒有任何世界幀的知識，我們設定了第一個相機幀(·)^c0作為參考幀的SfM。所有幀的姿態(p_ck^c0，q_ck^c0)和特徵位置表示相對於(·)^c0。假設攝像機和IMU之間有一個粗糙測度的外部引數(P_c^b,q_c^b)，我們可以將姿態從C幀轉換到B幀(IMU)。

其中s是匹配視覺結構與米制尺度的尺度引數，解決這個縮放參數是實現成功初始化的關鍵。

B. Visual-Inertial Alignment

       1)陀螺儀偏置標定：考慮視窗連續兩幀b_k和b_k+1，我們從視覺sfM中得到旋轉q_bk^c0和q_bk+1^c0，以及通過IMU預積分得到的相對約束γ_bk+1^bk。我們將IMU預積分項線性化，使陀螺儀偏置最小，並將下列成本函式降到最小：

其中B代表視窗中的所有幀。利用第四部分匯出的偏置雅可比，給出了γ_bk+1^bk對陀螺儀偏置的一階近似。這樣，我們得到了陀螺儀偏置b_w的初始校準。然後我們用新的陀螺儀偏置重新傳播所有的imu預積分項 。

2)速度、重力向量和米制尺度初始化：在陀螺儀偏置初始化後，我們繼續初始化導航的其他基本狀態，即速度、重力向量和公制標度：

其中，當取第k幀影象時，v_bk^bk是B幀中的速度，g^c0是c0幀中的重力向量，s將單目SfM縮放為公制單位。

考慮視窗中兩個連續的幀b_k和b_k+1，那麼(5)可以寫成：

我們可以將(14)和(17)合併成以下線性測量模型：

可以看出，R_bk^c0 ,R_bk+1^c0,P_ck^c0 ,P_ck+1^c0是從上尺度單目視覺∆t_k中得到的，∆t_k是兩個連續幀之間的時間間隔。通過求解這個線性最小二乘問題：

我們可以得到視窗中每一幀的物體幀速度，視覺參照系(·)^c0中的重力向量，以及尺度引數。

3)重力精化：通過約束量值，可以對原線性初始化步驟得到的重力向量進行細化。在大多數情況下，重力向量的大小是已知的。這導致重力向量只剩2自由度。因此，我們在其切線空間上用兩個變數重新引數化重力。引數化將重力向量表示為 ，其中g是已知的重力大小， 是表示重力方向的單位向量，b1和b2是跨越切平面的兩個正交基，如圖5所示，w1和w2分別是b1和b2的對應位移。通過演算法1的交叉乘積運算，可以找到一組b1，b2。然後用 代替(17)中的g，並與其它狀態變數一起求解w1和w2。此過程迭代到g^收斂為止。

       4)完成初始化：經過對重力向量的細化，通過將重力旋轉到z軸上，得到世界幀與攝像機幀c0之間的旋轉q_c0^W。然後我們將所有變數從參考框架(·)^c0旋轉到世界框架(·)^w。B幀的速度也將被旋轉到世界框架。視覺SfM的轉換元件將被縮放為公制單位。此時，初始化過程已經完成，所有這些度量值都將被輸入到一個緊耦合的單目VIO中。

 

VI. TIGHTLY-COUPLED MONOCULAR VIO

       在估計器初始化後，我們採用基於滑動視窗的緊耦合單目VIO進行高精度和魯棒狀態估計。圖3顯示了滑動視窗的圖示。

A. Formulation

滑動視窗中的完整狀態向量定義為：

       其中x_K是捕獲k^th影象時的IMU狀態。它包含了IMU在世界幀中的位置、速度和方向，以及在IMU Body幀中的加速度計偏置和陀螺儀偏置，n是關鍵幀的總數，m是滑動視窗中的特徵總數，λ_l是第一次觀測到的lth特徵的逆深度。

我們使用視覺慣性束調整公式（BA）.我們最小化所有測量殘差的先驗和Mahalanobis範數之和，得到最大後驗估計：

r_B(z_bk+1^bk,X)和r_C(z_l^cj,X)分別是IMU和視覺測量的殘差。剩餘條件的詳細定義將在第六節的B和C中提出。B是所有IMU測量的集合，C是在當前滑動視窗中至少觀察到兩次的一組特徵。{r_p,H_p}是來自邊緣化的先驗資訊。CeresSolver被用來解決這個非線性問題。

B. IMU Measurement Residual

考慮滑動視窗中連續兩個幀(b_k和b_k+1)內的IMU測量，根據(13)中定義的IMU測量模型，預積分IMU測量的殘差可以定義為：

其中，[·]_xyz是提取四元數q的向量部分，以進行誤差狀態表示。δθ_bk+1^bk是四元數的三維誤差狀態表示。[ ]^T是在兩個連續影象幀之間的間隔時間內僅使用噪聲加速度計和陀螺儀測量值預積分的IMU測量項。加速度計和陀螺儀偏置也包括線上校正的剩餘項中。

C. Visual Measurement Residual

與傳統的在廣義影象平面上定義再投影誤差的針孔相機模型相比，我們定義了單位球面上攝像機的測量殘差。幾乎所有型別相機的光學，包括廣角、魚眼或全向相機，都可以模擬為連線單位球體表面的單位射線。考慮第一次在第i幅影象中觀察到的第l個特徵，第j幅影象中的特徵觀測的殘差定義為：

其中[u_l^ci,v_l^ci]是第一次觀測第一次出現在i影象中的第l個特徵。[u^_l^ci,v^_l^ci]是對jth影象中相同特徵的觀察。π_c⁻¹是利用攝像機內參數將畫素位置轉換成單位向量的反投影函式。由於視覺殘差的自由度是2，所以我們將殘差向量投影到切平面 上，如圖6所示，b1,b2是兩個任意選擇的正交基。我們可以很容易地找到一組b1,b2，如演算法1所示。在(22)中使用的p_l^cj是正切空間中固定長度的標準協方差。

D. Marginalization

為了限制基於優化的VIO的計算複雜度，本文引入了邊緣化.我們有選擇地將IMU狀態X_K和特徵λ1從滑動視窗邊緣化，同時將對應於邊緣狀態的度量轉換為先驗。

如圖7所示，當第二個最新的幀是關鍵幀時，它將停留在視窗中，而最老的幀與其相應的測量值被邊緣化（邊緣化的測量值作為先驗資訊）。否則，如果第二個最新的幀是非關鍵幀，我們丟掉視覺測量值和保留連線到這個非關鍵幀IMU測量值。為了保持系統的稀疏性，我們不丟棄非關鍵幀的所有測量值（留下IMU預積分值）。我們的邊緣化方案的目的是在視窗中保持空間分離的關鍵幀。這確保了特徵三角化有足夠的視差，並且最大限度地提高了在大激勵下獲得加速度計測量值的概率。

邊緣化是利用Schur補充[39]進行的。我們基於與移除狀態相關的所有邊緣化度量來構造一個新的先驗。新的先驗項被新增到現有的先驗項中。

       我們確實注意到，邊緣化導致了線性化點的早期固定，這可能導致次優估計結果。然而，由於小型漂移對於VIO來說是可以接受的，我們認為邊緣化所造成的負面影響並不重要。

E. Motion-only Visual-Inertial Bundle Adjustment for Camera-RateState Estimation

       對於計算能力較低的裝置，如手機，由於對非線性優化的計算要求很高，使得緊耦合單目VIO無法實現攝像機速率輸出。為此，我們採用了一種輕量級的只運動視覺慣性束調整（只運動的VI BA），以提高狀態估計到相機的速率(30赫茲)。

       單目視覺慣性集束調整（BA）的成本函式與(22)中的單目VIO的代價函式相同。然而，我們沒有對滑動視窗中的所有狀態進行優化，而是只對固定數量的最新IMU狀態的姿態和速度進行了優化。我們將特徵深度、外部引數、偏置和舊的IMU狀態作為常量來處理，我們不希望優化這些狀態。我們使用所有的視覺和慣性測量來進行僅運動的BA.這導致了比單幀PNP方法更平滑的狀態估計。圖8顯示了提出方法的插圖。與在最先進的嵌入式計算機上可能導致超過50 ms的完全緊耦合的單目VIO不同，這種只需運動的視覺慣性束調整只需大約5ms即可計算。這使得低延遲相機的姿態估計對無人機和AR應用特別有利.

F. IMU Forward Propagation for IMU-Rate State Estimation

       IMU測量的速度比視覺測量高得多。雖然我們的VIO的頻率受到影象捕獲頻率的限制，但是我們仍然可以通過最近的IMU測量來直接傳播最新的VIO估計，以達到IMU速率的效能。高頻狀態估計可以作為迴環檢測的狀態反饋。利用這種IMU速率狀態估計進行的自主飛行實驗在第九節的D中進行。

G. Failure Detection and Recovery

雖然我們緊耦合的單目視覺對各種具有挑戰性的環境和運動是健壯的。由於強烈的光照變化或劇烈的運動，失敗仍然是不可避免的。主動故障檢測和恢復策略可以提高系統的實用性。故障檢測是一個獨立的模組，它檢測估計器的異常輸出。我們目前使用以下標準進行故障檢測：

*在最新幀中跟蹤的特徵數小於某一閾值；

*最後兩個估計器輸出之間的位置或旋轉有較大的不連續性；

*偏置或外部引數估計有較大的變化；

一旦檢測到故障，系統將切換回初始化階段。一旦單目VIO被成功初始化，將新建一個獨立的位姿圖段。

 

VII. RELOCALIZATION

我們的滑動視窗和邊緣化方案限制了計算的複雜性，但也給系統帶來了累積漂移。更確切地說，漂移發生在全域性三維位置(x，y，z)和圍繞重力方向的旋轉(偏航)。為了消除漂移，提出了一種與單目VIO無縫整合的緊耦合重定位模組.重定位過程從一個迴圈檢測模組開始，該模組標識已經訪問過的地方。然後建立迴環檢測候選幀和當前幀之間的特徵級連線。這些特徵被對應緊密地整合到單目VIO模組中，從而得到無漂移狀態估計，並且計算開銷最小。多個特徵的多觀測直接用於重定位，從而提高了定位的精度和狀態估計的平滑性。圖9(a)示出了重新定位過程的圖形說明。

A. Loop Detection

我們利用DBoW 2，一種最先進的詞袋位置識別方法來進行迴圈檢測.除了用於單目VIO的角特徵外，另外500個拐角被檢測並由BRIEF描述符描述。額外的角落特徵用於在迴路檢測中實現更好的召回率。描述符被視為用於查詢視覺化資料庫的可視單詞。DBoW 2在時間和幾何一致性檢查後返回迴環檢測候選項。我們保留所有用於特徵檢索的BRIEF描述符，丟棄原始影象以減少記憶體消耗。

我們注意到，我們的單目VIO能夠使滾動和俯仰角可以被觀察到。因此，我們不需要依賴旋轉不變的特性，例如ORB SLAM中使用的ORB特性。

B. Feature Retrieval

當檢測到環路時，通過檢索特徵對應性建立本地滑動視窗與迴環候選點之間的連線。通過BRIEF描述符匹配找到對應關係。直接描述符匹配可能會導致大量異常值。為此，我們使用兩步幾何離群點剔除，如圖10所示。

*2D-2D：RANSAC的基本矩陣檢驗.我們利用當前影象中檢索到的特徵的二維觀測和迴環候選影象進行基本矩陣檢驗。

*3D-2D:RANSAC的PNP檢驗。基於已知的特徵在區域性滑動視窗中的三維位置，以及在環路閉合候選影象中的二維觀測，進行PNP檢驗。

當超過一定閾值的不動點數時，我們將該候選點視為正確的迴圈檢測並執行重新定位。

C. Tightly-Coupled Relocalization

       重定位過程有效地使單目VIO保持的當前滑動視窗與過去的姿態圖保持一致。在重新定位過程中，我們將所有閉環幀的姿態作為常量，利用所有IMU測量值、區域性視覺測量和從迴環中提取特徵對應值，共同優化滑動視窗。我們可以輕鬆地為迴環幀v所觀察到的檢索到的特徵編寫視覺測量模型，使其與VIO中的視覺測量相同，如(25)所示。唯一的區別是，從位姿圖或直接從上一個里程計的輸出(如果這是第一次重定位)獲得的迴環幀的姿態(q_v^w,p_v^w)被視為常數。為此，我們可以在(22)中稍微修改非線性代價函式，增加迴圈項：

其中L是迴環幀中檢索到的特徵的觀測集。(l,v)是指在迴環幀v中觀察到的第l個特徵。注意，雖然代價函式與(22)略有不同，但待解狀態的維數保持不變，因為迴環幀的構成被視為常數。當用當前滑動視窗建立多個迴環時，我們使用來自所有幀的所有環路閉合特徵對應進行優化。這就為重新定位提供了多視角的約束，從而提高了定位的精度和平滑性。請注意，過去的姿態和迴圈關閉幀的全域性優化發生在重新定位之後，將在第八節中討論。

 

 

VIII. GLOBAL POSE GRAPH OPTIMIZATION

       重新定位後，區域性滑動視窗移動並與過去的姿態保持一致。利用重定位結果，此附加姿態圖優化步驟被開發以確保過去姿勢的集合被登記為全域性一致配置

       由於我們的視覺慣性設定使滾動和俯仰角完全可觀測，累積漂移只發生在四個自由度(x，y，z和偏航角)。為此，我們忽略了對無漂移滾轉和俯仰狀態的估計，只進行了四自由度姿態圖的優化。

A. Adding Keyframes into the Pose Graph

當關鍵幀從滑動視窗中邊緣化時，它將被新增到位姿圖中。這個關鍵幀在位姿圖中充當頂點，它通過兩種型別的邊與其他頂點連線：

1）序列邊緣：關鍵幀將建立與其先前關鍵幀的若干順序邊。序列邊緣表示區域性滑動視窗中兩個關鍵幀之間的相對轉換，其值直接從VIO中獲取。考慮到新邊緣化的關鍵幀i及其先前的一個關鍵幀j，序列邊緣只包含相對位置p_ijⁱ和偏航角 。

2）迴圈閉合邊緣：如果新邊緣化的關鍵幀有一個迴圈連線，它將與迴環幀通過一個環閉合邊在姿態圖中連線。同樣，閉環邊緣只包含與(27)相同定義的四自由度相對位姿變換。利用重新定位的結果，得到了環路閉合邊的值。

B.4-DOF Pose Graph Optimization

我們將幀i和j之間的邊緣的殘差定義為：

其中，φⁱ,θi是直接從單目VIO中得到的滾動角和俯仰角的估計。通過最小化以下代價函式，對順序邊和迴環邊的整個圖進行了優化：

其中S是所有序列邊的集合，L是全環閉包邊的集合。儘管緊耦合的重新定位已經有助於消除錯誤的閉環，但我們添加了另一個Huber規範ρ(·)，以進一步減少任何可能的錯誤迴圈的影響。相反，我們不對序列邊緣使用任何魯棒範數，因為這些邊緣是從VIO中提取出來的，VIO已經包含了足夠多的孤立點排除機制。

       位姿圖優化和重新定位非同步執行在兩個獨立的執行緒中。以便在可用重定位時，能立即使用最優化的位姿圖。同樣，即使當前的姿態圖優化尚未完成，仍然可以使用現有的姿態圖配置進行重新定位。這一過程如圖9(b)所示。

C. Pose Graph Management

隨著行程距離的增加，姿態圖的大小可能會無限增長，從而限制了系統的實時性。為此，我們實現了一個下采樣過程，以將姿態圖資料庫保持在有限的大小。所有具有迴環約束的關鍵幀都將被保留，而其他與其鄰居方向過近或方向非常相似的關鍵幀可能會被刪除。關鍵幀被移除的概率和其與鄰居空間密度成正比。

 

IX. EXPERIMENTAL RESULTS

我們進行了三個實驗和兩個應用，以評估所提出的VINS-Mono系統.在第一個實驗中，我們將所提出的演算法與另一種最新的公共資料集演算法進行了比較.通過數值分析，驗證了系統的精度。然後在室內環境中測試我們的系統，以評估在重複場景中的效能。通過大量的實驗驗證了系統的長期實用性.此外，我們還將所提出的系統應用於兩個應用程式。對於空中機器人的應用，我們使用vins-Mono作為位置反饋來控制無人機跟蹤預定的軌跡。然後我們將我們的方法移植到iOS移動裝置上，並與GoogleTango進行比較。

A. Dataset Comparison

我們使用歐洲MAV視覺-慣性資料集評估我們提出的VINS-Mono。這些資料集是在一架微型飛行器上收集的，它包含立體影象(Aptina MT9V034全球快門、WVGA單色、20 FPS)、同步IMU測量(ADIS 16448、200 Hz)和地面真實狀態(Vicon和Leica MS 50)。我們只使用左邊相機的影象。在這些資料集中觀察到了較大的IMU偏置和光照變化。

在這些實驗中，我們將vins-mono和OKVIS進行了比較，這是一種最先進的VIO，可以用單目和立體相機工作。OKVIS是另一種基於優化的滑動視窗演算法.我們的演算法與OKVIS在許多細節上是不同的，如技術部分所示。該系統具有良好的初始化和閉環控制功能。我們使用MH-03和MH-05兩種序列來證明該方法的效能。為了簡化表示法，我們使用vins來表示我們只使用單目VIO的方法，而vins_loop表示含重新定位和姿態圖優化的完全版本。我們分別用OKVISMono和OKVIS_stereo表示OKVIS的結果。為了進行公平的比較，我們丟棄前100個輸出，並使用接下來的150個輸出對齊地面真值，並比較其餘的估計器輸出。

MH-03序列軌跡如圖11所示。我們只比較平移誤差，因為旋轉運動在這個序列中是可以忽略的。圖12顯示了x,y,z誤差與時間的關係，以及平移誤差與距離的關係。在誤差圖中，具有迴環的vins-mono具有最小的平移誤差.我們在MH 05上觀察到類似的結果。該方法具有最小的平移誤差。平移和旋轉誤差如圖14所示。由於該序列運動平穩，偏角變化不大，只發生位置漂移。顯然，迴圈閉合能力有效地約束了累積漂移。OKVIS在滾動和俯仰角度估計方面表現更好。一個可能的原因是VINSMono採用了預積分技術，即IMU傳播的一階近似，以節省計算資源。

       vins-Mono在所有Euroc資料集中表現良好，即使在最具挑戰性的序列中，V1-03包括劇烈性的運動、紋理減少的區域和顯著的光照變化。由於採用了專用的初始化過程，該方法在V1-03中難以快速初始化。

       對於純VIO，vin-Mono和OKVIS具有相似的精度，很難區分哪個比較好。然而，vins-Mono在系統級別上優於OKVIS。它是一個完整的系統，具有魯棒的初始化和閉環功能來輔助單目視覺。

B. Indoor Experiment

在室內實驗中，我們選擇實驗室環境作為實驗區域。我們使用的感測器套件如圖15所示。它在DJIA3控制器中包含一個單目照相機(20Hz)和一個IMU(100 Hz)。我們手握感測器套件，在實驗室以正常的速度行走。如圖16所示，我們遇到行人，光線較弱，紋理較少，玻璃和反射。視訊可以在多媒體附件中找到。

我們將我們的結果與OKVIS進行了比較，如圖17所示。圖17(a)是OKVIS的VIO輸出。圖17(b)是所提出的無迴環方法的VIO結果。圖17(c)是所提出的具有重新定位和環路閉合的方法的結果。當我們在室內迴圈時，會出現明顯的漂移。OKVIS和只有VIO版本的vins-Mono在x,y,z和偏航角上積累了大量漂移.我們的重新定位和迴圈關閉模組有效地消除了所有這些漂移。

C. Large-scale Environment

       1)走出實驗室：我們在室內和室外混合的環境中測試vins-mono。感測器套件與圖15所示的相同。我們從實驗室的一個座位上開始，在室內空間裡走來走去。然後我們下了樓梯，在大樓外的操場上走來走去。接下來，我們回到樓裡上樓。最後，我們回到了實驗室的同一個座位。整個軌道超過700米，持續約10分鐘。在多媒體附件中可以找到實驗的視訊。

       軌跡如圖19所示。圖19(a)是OKVIS的軌跡。當我們上樓時，OKVIS顯示出不穩定的特徵跟蹤，導致估計錯誤。我們看不到紅色街區樓梯的形狀。VINS-Mono的唯一結果如圖19(b)所示.有閉環的軌跡如圖19(c)所示。該方法的樓梯形狀清晰。閉環軌跡與谷歌地圖對齊，以驗證其準確性，如圖18所示。

       OKVIS x,y和z軸的最終漂移為[13.80,-5.26,7.23]米。VINS-Mono無環閉路的最終方向為[-5.47,2.76,-0.29]m，佔總彈道長度的0.88%，小於OKVIS的2.36%。經迴環修正，最終漂移有界於[-0.032,0.09,-0.07]m，與總彈道長度相比，這是微不足道的。雖然我們沒有地面真值，但我們仍然可以直觀地檢查優化後的軌道是否平滑，並能精確地與衛星地圖對齊。

2)環遊校園：這張環繞整個科大校園的非常大規模的資料集是用一個手持的VI-Sensor 4記錄下來的。該資料集覆蓋的地面長度約為710米，寬度為240米，高度變化為60米。總路徑長度為5.62km。資料包含25赫茲影象和200赫茲IMU，持續1小時34分鐘。對VINS-Mono的穩定性和耐久性進行測試是一個非常有意義的實驗。

在這個大規模的測試中，我們將關鍵幀資料庫的大小設定為2000，以提供足夠的迴圈資訊並實現實時效能。我們執行此資料集時，英特爾i7-4790 CPU執行在3.60GHz。時間統計資料顯示在表中。估計的軌跡與圖20中的谷歌地圖一致。與谷歌地圖相比，我們的結果在這個非常長時間的測試中幾乎沒有漂移。

D. Application I: Feedback Control of an Aerial Robot

如圖21(a)所示，我們將vins-Mono應用於航空機器人的自主反饋控制。我們使用了一個具有752×480解析度的前瞻性全球快門相機(MatrixVisionMvBlueFOXMLC200w)，並配備了190度魚眼鏡頭。DJIA3飛行控制器用於IMU測量和姿態穩定控制。星載計算資源是Intel i7-5500 U CPU，執行在3.00GHz。傳統的針孔攝像機模型不適用於大視場攝像機。我們使用MEI模型對此相機進行校準，由[43]中引入的工具包進行校準。

在本實驗中，我們使用VINS_Mono的狀態估計來測試自主軌跡跟蹤的效能。在這個實驗中，迴環檢測被禁止。四轉子被命令跟蹤一個圖8模式，每個圓圈半徑為1.0米，如圖21(b)所示。在彈道周圍設定了四個障礙物，以驗證VINS-Mono無閉環的準確性。在實驗過程中，四轉子連續四次跟蹤這一軌跡。100 Hz星載狀態估計支援對四轉子的實時反饋控制.

地面真相是用OptiTrack 5獲得的。總彈道長度為61.97 m。最終漂移為[0.08，0.09，0.13]m，結果為0.29%的位置漂移。平移和旋轉的細節以及它們相應的誤差如圖23所示。

E. Application II: Mobile Device

我們將vins-Mono移植到移動裝置上，並提供一個簡單的AR應用程式來展示其準確性和健壯性。我們將我們的移動實現命名為vins-Mobile6，並將其與GoogleTangoDevice 7進行了比較，後者是移動平臺上商業上最好的增強現實解決方案之一。

VINS-Mono執行在iPhone7Plus上。我們使用iphone採集的30 Hz影象，解析度640×480，以及內建的InvenSenseMP67B 6軸陀螺儀和加速度計獲得的100 Hz IMU資料。如圖24所示，我們將iphone與一個啟用Tango功能的聯想phab 2 Pro一起安裝。探戈裝置使用全域性快門、魚眼相機和同步IMU進行狀態估計。首先，我們在平面上插入一個虛擬立方體，該虛擬立方體是從估計的視覺特徵中提取出來的，如圖25(a)所示。然後，我們拿著這兩個裝置，以正常的速度在房間內外行走。當檢測到迴路時，我們使用四自由度姿態圖優化.消除x,y,z和偏航漂移。

有趣的是，當我們開啟一扇門時，探戈的偏航估計會跳到一個很大的角度，如圖25(b)所示。其原因可能是由於不穩定的特徵跟蹤或主動故障檢測和恢復而導致的估計器崩潰。然而，vins-Mono在這個具有挑戰性的案例中仍然工作得很好。旅行了大約264米後，我們回到起點。最後的結果可以在圖25(c)中看到，探戈的軌跡在最後一圈會漂移，而我們的vins會回到起點。通過對四自由度姿態圖的優化，消除了總軌跡的漂移.這也證明了，與開始相比，立方體被標記到影象上的同一位置。

誠然，探戈比我們的實施更準確，尤其是對於區域性狀態的估計。實驗結果表明，該方法可以在通用移動裝置上執行，並且具有比較特殊工程裝置的潛力。實驗還證明了該方法的魯棒性。視訊可以在多媒體附件中找到。

 

X. CONCLUSION AND FUTURE WORK

本文提出了一種魯棒、通用的單目視覺慣性估計器.我們