Abstract

由攝像機(jī)和低成本慣性測量單元(IMU)組成的單目視覺慣性系統(tǒng)(VINS)構(gòu)成了用于度量六自由度狀態(tài)估計(jì)的最小傳感器套件。然而，由于缺乏直接距離測量，在IMU處理、估計(jì)器初始化、外部標(biāo)定和非線性優(yōu)化等方面提出了重大挑戰(zhàn)。在本文中，我們提出了一種魯棒的、通用的單目視覺慣性狀態(tài)估計(jì)器VINSMono.我們的方法從一個(gè)穩(wěn)健的程序開始，用于估計(jì)器初始化和故障恢復(fù)。采用一種基于緊耦合、非線性優(yōu)化的方法，通過融合預(yù)計(jì)分的IMU測量數(shù)據(jù)和特征觀測數(shù)據(jù)，獲得高精度的視覺慣性里程計(jì)。結(jié)合我們緊耦合的公式，一個(gè)循環(huán)檢測模塊能夠以最小的計(jì)算開銷重新定位.此外，我們還執(zhí)行四自由度姿態(tài)圖優(yōu)化，以加強(qiáng)全局一致性.我們驗(yàn)證了我們的系統(tǒng)在公共數(shù)據(jù)集和真實(shí)世界實(shí)驗(yàn)上的性能，并與其他最先進(jìn)的算法進(jìn)行了比較。我們還在MAV平臺(tái)上執(zhí)行星載閉環(huán)自主飛行，并將算法移植到基于IOS的演示中。我們強(qiáng)調(diào)，所提議的工作是一個(gè)可靠、完整和多功能的系統(tǒng)，適用于需要高精度定位的不同應(yīng)用程序。我們?yōu)閭€(gè)人電腦和iOS移動(dòng)設(shè)備開放了我們的實(shí)現(xiàn)方法。

?

I. INTRODUCTION

狀態(tài)估計(jì)無疑是機(jī)器人導(dǎo)航、自主駕駛、虛擬現(xiàn)實(shí)(VR)和增強(qiáng)現(xiàn)實(shí)(AR)等廣泛應(yīng)用中最基本的模塊。僅使用單目攝像機(jī)的方法由于其體積小、成本低和硬件設(shè)置簡單而獲得了社區(qū)的極大興趣。然而，單目視覺系統(tǒng)無法恢復(fù)公制尺度，因此限制了它們?cè)趯?shí)際機(jī)器人應(yīng)用中的應(yīng)用。近年來，我們看到了一種發(fā)展趨勢，即用低成本慣性測量單元(IMU)輔助單目視覺系統(tǒng)。這種單目視覺-慣性系統(tǒng)(VINS)的主要優(yōu)點(diǎn)是具有公制尺度，以及滾動(dòng)角和俯仰角，所有這些都是可觀測的。這讓需要度量狀態(tài)估計(jì)的導(dǎo)航任務(wù)成為可能。此外，積分IMU測量可以顯著地改善運(yùn)動(dòng)跟蹤性能，彌補(bǔ)由于光照變化，紋理無區(qū)域，或運(yùn)動(dòng)模糊的視覺軌跡損失之間的差距。事實(shí)上，單目VINS不僅廣泛應(yīng)用于移動(dòng)機(jī)器人、無人機(jī)和移動(dòng)設(shè)備上，而且是滿足充分自我感知和環(huán)境感知的最小傳感器設(shè)置。

然而，所有這些優(yōu)勢都是有代價(jià)的。對(duì)于單目VIN，眾所周知，加速度激勵(lì)需要測量到尺度。這意味著單目VIN估計(jì)器不能從靜止?fàn)顟B(tài)開始，而是從未知的移動(dòng)狀態(tài)發(fā)射。也認(rèn)識(shí)到視覺慣性系統(tǒng)高度非線性的事實(shí)，在初始化估計(jì)方面還有重大挑戰(zhàn)。兩個(gè)傳感器的存在也使得攝像機(jī)-IMU的外部校準(zhǔn)至關(guān)重要。最后，為了消除在可接受的處理窗口內(nèi)的長期漂移，提出了一個(gè)完整的系統(tǒng)，包括視覺慣性里程計(jì)，回環(huán)檢測、重定位和全局優(yōu)化。

為了解決所有這些問題，我們提出了VINS-MONO，一個(gè)強(qiáng)大的和多功能的單目視覺慣性狀態(tài)估計(jì)。我們的解決方案開始于即時(shí)估計(jì)初始化。這個(gè)初始化模塊也用于故障恢復(fù)。我們的解決方案的核心是一個(gè)魯棒的基于緊耦合滑動(dòng)窗非線性優(yōu)化的單目視覺慣性里程計(jì)（VIO）。單目VIO模塊不僅提供精確的局部姿態(tài)、速度和方位估計(jì)，而且還以在線方式執(zhí)行攝像機(jī)IMU外部校準(zhǔn)和IMU偏置校正。使用DBoW進(jìn)行回環(huán)檢測。重新定位是在對(duì)單目VIO進(jìn)行特征級(jí)別融合的緊耦合設(shè)置中完成。這使得重新定位具有魯棒性和精確性且有最小的計(jì)算開銷。最后，幾何驗(yàn)證回路被添加到姿態(tài)圖中，并且由于來自單目VIO的可觀察的滾轉(zhuǎn)和俯仰角，生成四自由度（DOF）姿態(tài)圖以確保全局一致性。

VINS-Mono結(jié)合并改進(jìn)了我們以前在單目視覺-慣性融合方面的工作。它建立在我們的緊耦合，基于優(yōu)化的單目VIO的公式之上，并結(jié)合了[9]中引入的改進(jìn)的初始化過程。第一次嘗試移植到移動(dòng)設(shè)備是在[10]。與我們以前的工作相比，VINS-Mono的進(jìn)一步改進(jìn)包括改進(jìn)的含偏置校正的IMU預(yù)積分、緊耦合重定位、全局姿態(tài)圖優(yōu)化、廣泛的實(shí)驗(yàn)評(píng)估以及健壯和通用的開源實(shí)現(xiàn)。

整個(gè)系統(tǒng)完整且易于使用。它已經(jīng)被成功應(yīng)用于小規(guī)模AR場景、中型無人機(jī)導(dǎo)航和大規(guī)模狀態(tài)估計(jì)任務(wù)。已經(jīng)針對(duì)現(xiàn)有技術(shù)方法的其它狀態(tài)示出了優(yōu)異的性能。為此，我們總結(jié)了我們的貢獻(xiàn)如下所示：

*一個(gè)健壯的初始化過程，它能夠從未知的初始狀態(tài)引導(dǎo)系統(tǒng)。

*一個(gè)緊耦合，優(yōu)化的單目視覺慣性里程計(jì)與相機(jī)-IMU外部校準(zhǔn)和IMU偏置估計(jì)。

*在線回環(huán)檢測與緊耦合重定位。

*四自由度全局姿態(tài)圖優(yōu)化。

*用于無人機(jī)導(dǎo)航、大規(guī)模本地化和移動(dòng)AR應(yīng)用的實(shí)時(shí)性能演示。

*與ros完全積分的pc版本以及在iphone 6或更高版本上運(yùn)行的IOS版本的開源版本。

論文的其余部分如下：在第二節(jié)中，我們討論了相關(guān)的文獻(xiàn)。我們?cè)诘谌?jié)中對(duì)完整的系統(tǒng)框架進(jìn)行了概述。在第四節(jié)中給出了視覺和預(yù)處理IMU測量的預(yù)處理步驟。在第五節(jié)中，我們討論了估計(jì)器的初始化過程。在第六節(jié)中提出了一種緊耦合、自標(biāo)定、非線性優(yōu)化的單目VIO。第七節(jié)和第八節(jié)分別給出了緊耦合重定位和全局姿態(tài)圖優(yōu)化。實(shí)施細(xì)節(jié)和實(shí)驗(yàn)結(jié)果見第九節(jié)。最后，第十節(jié)本文對(duì)研究方向進(jìn)行了探討和展望。

?

II. RELATED WORK

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

處理視覺和慣性測量的最簡單的方法是松耦合傳感器融合，其中IMU被視為一個(gè)獨(dú)立的模塊，用于輔助從運(yùn)動(dòng)中獲得的視覺結(jié)構(gòu)的視覺姿態(tài)估計(jì)。融合通常由擴(kuò)展卡爾曼濾波(EKF)完成，其中IMU用于狀態(tài)傳播，而視覺姿態(tài)用于更新。進(jìn)一步說，緊耦合視覺慣性算法要么基于EKF，要么基于圖優(yōu)化，其中攝像機(jī)和IMU測量是從原始測量水平聯(lián)合優(yōu)化的。一種流行的基于EKF的VIO方法是MSCKF。MSCKF在狀態(tài)向量中維護(hù)以前的幾個(gè)攝像機(jī)姿態(tài)，并使用多個(gè)攝像機(jī)視圖中相同特征的視覺測量來形成多約束更新。SR-ISWF是MSCKF的擴(kuò)展。它采用SQuareroot格式實(shí)現(xiàn)單精度表示，避免了差的數(shù)值性質(zhì)。該方法采用逆濾波器進(jìn)行迭代再線性化，使其與基于優(yōu)化的算法相當(dāng).批量圖優(yōu)化或捆綁調(diào)整（BA）技術(shù)維護(hù)和優(yōu)化所有的測量，以獲得最優(yōu)狀態(tài)估計(jì)。為了達(dá)到恒定的處理時(shí)間，流行的基于圖的VIO方法通常通過邊緣化有過去的狀態(tài)和測量的有界滑動(dòng)窗口來優(yōu)化最近狀態(tài)。由于對(duì)非線性系統(tǒng)迭代求解的計(jì)算要求很高，很少有基于圖的非線性系統(tǒng)能夠在資源受限的平臺(tái)(如手機(jī))上實(shí)現(xiàn)實(shí)時(shí)性能。

對(duì)于視覺測量處理，根據(jù)視覺殘差模型的定義，算法可分為直接法和間接法。直接法最小光度誤差，而間接法最小幾何位移。直接方法由于其吸引區(qū)域小，需要很好的初始估計(jì)，而間接方法在提取和匹配特征時(shí)需要額外的計(jì)算資源。間接方法由于其成熟性和魯棒性，在實(shí)際工程部署中得到了廣泛的應(yīng)用。然而，直接方法更容易擴(kuò)展到稠密建圖，因?yàn)樗鼈兪侵苯釉谙袼丶?jí)別上操作的。

在實(shí)踐中，IMU通常以比攝像機(jī)更高的速度獲取數(shù)據(jù)。不同的方法被提出來處理高速率的IMU測量。最簡單的方法是在基于EKF的方法中使用IMU進(jìn)行狀態(tài)傳播。在圖優(yōu)化公式中，為了避免重復(fù)的IMU重復(fù)積分，提出了一種有效的方法，即IMU預(yù)積分，這種方法是在[22]中首次提出的，它用歐拉角來參數(shù)化旋轉(zhuǎn)誤差。在我們先前的工作中，我們提出了一種流形上的IMU-preIntegration旋轉(zhuǎn)公式，該文利用連續(xù)IMU誤差狀態(tài)動(dòng)力學(xué)推導(dǎo)了協(xié)方差傳播。然而，IMU偏置被忽略了。文[23]通過增加后驗(yàn)IMU偏置校正，進(jìn)一步改進(jìn)了預(yù)積分理論。

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

VIO方法，不管它們所依賴的基本數(shù)學(xué)公式，在全局的平移和旋轉(zhuǎn)中長期受到漂移的影響。為此，回環(huán)檢測在長期操作中起著重要的作用.ORBSLAM能夠關(guān)閉循環(huán)并重新使用地圖，它利用了詞袋模型。一個(gè)7自由度（位置、方向和尺度)的姿態(tài)圖優(yōu)化遵循回環(huán)檢測。相對(duì)于單目Vins，由于IMU的加入，漂移只發(fā)生在4自由度，即三維平移，并圍繞重力方向(偏航角)旋轉(zhuǎn)。因此，本文選擇在最小四自由度設(shè)定下，優(yōu)化具有回路約束的姿態(tài)圖。

?

III.OVERVIEW

提出的單目視覺慣性狀態(tài)估計(jì)器的結(jié)構(gòu)如圖2所示.該系統(tǒng)從測量預(yù)處理(SectIV)開始，在其中提取和跟蹤特征，對(duì)兩個(gè)連續(xù)幀間的IMU測量進(jìn)行預(yù)處理。初始化過程(SectV)提供了所有必要的值，包括姿態(tài)、速度、重力向量、陀螺儀偏置和三維特征位置，用于引導(dǎo)隨后的基于非線性優(yōu)化的VIO。VIO(SectVI)與重新定位(SectVII)模塊緊密地融合了預(yù)先積分的IMU測量、特征觀測和回環(huán)重新檢測到的特征。最后，位姿圖優(yōu)化模塊(SectVIII)接受幾何驗(yàn)證的重定位結(jié)果，并進(jìn)行全局優(yōu)化以消除漂移。VIO、重新定位和姿態(tài)圖優(yōu)化模塊在多線程設(shè)置中同時(shí)運(yùn)行.每個(gè)模塊有不同的運(yùn)行速度和實(shí)時(shí)保證，以確保在任何時(shí)候的可靠運(yùn)行。

我們現(xiàn)在定義在整個(gè)論文中使用的符號(hào)和框架定義。我們認(rèn)為(.)^w是世界框架。重力的方向與世界幀的z軸對(duì)齊。(·)^b是物體框架，我們把它定義為與IMU框架相同。(·)^c是相機(jī)框架。我們使用旋轉(zhuǎn)矩陣R和Hamilton四元數(shù)q來表示旋轉(zhuǎn)。我們主要在狀態(tài)向量中使用四元數(shù)，但三維向量的旋轉(zhuǎn)用旋轉(zhuǎn)矩陣來方便表示。q_w^b,P_W^b是從B幀到W幀的旋轉(zhuǎn)和平移。b_k是當(dāng)取k個(gè)圖像時(shí)的B幀。c_k是在獲取k個(gè)圖像時(shí)的相機(jī)幀。?表示兩個(gè)四元數(shù)之間的乘法運(yùn)算。gw=[0,0,g]^T是世界范圍內(nèi)的重力向量。最后，我們將(^{^}_·)表示為某一量的噪聲測量值或估計(jì)值。

?

IV.MEASUREMENT PREPROCESSING

本節(jié)介紹VIO的預(yù)處理步驟。對(duì)于視覺測量，我們跟蹤連續(xù)幀之間的特征，并檢測最新幀中的新特征。對(duì)于IMU測量，我們將它們預(yù)先積分在兩個(gè)連續(xù)的幀之間。請(qǐng)注意，我們使用的低成本IMU的測量值受到偏置和噪聲的影響。因此，我們?cè)贗MU的預(yù)積分過程中特別考慮到偏置。

A.視覺處理前端

?????? 對(duì)于每一幅新圖像，KLT稀疏光流算法對(duì)現(xiàn)有特征進(jìn)行跟蹤。同時(shí)，檢測新的角點(diǎn)特征以保持每個(gè)圖像中特征的最小數(shù)目(100-300)。該檢測器通過設(shè)置兩個(gè)相鄰特征之間像素的最小間隔來執(zhí)行均勻的特征分布。二維特征首先是不失真的，然后在通過異常點(diǎn)剔除后投射到一個(gè)單位球面上。利用基本矩陣模型的RANSAC進(jìn)行外點(diǎn)剔除。（外點(diǎn)、異常點(diǎn)）

?????? 在此步驟中還選擇了關(guān)鍵幀。我們有兩個(gè)關(guān)鍵幀選擇的標(biāo)準(zhǔn)。第一個(gè)是與前一個(gè)關(guān)鍵幀不同的平均視差。如果跟蹤特征的平均視差介于當(dāng)前幀和最新的關(guān)鍵幀之間，則將幀視為新的關(guān)鍵幀。請(qǐng)注意，不僅平移，旋轉(zhuǎn)也會(huì)產(chǎn)生視差。然而，特征不能在旋轉(zhuǎn)-純運(yùn)動(dòng)中三角化.為了避免這種情況，在計(jì)算視差時(shí)，我們使用陀螺儀測量的短期積分來補(bǔ)償旋轉(zhuǎn)。請(qǐng)注意，此旋轉(zhuǎn)補(bǔ)償僅用于關(guān)鍵幀選擇，而不涉及VINS公式中的旋轉(zhuǎn)計(jì)算。為此，即使陀螺儀含有較大的噪聲或存在偏置，也只會(huì)導(dǎo)致次優(yōu)的關(guān)鍵幀選擇結(jié)果，不會(huì)直接影響估計(jì)質(zhì)量。另一個(gè)標(biāo)準(zhǔn)是跟蹤質(zhì)量。如果跟蹤的特征數(shù)量低于某一閾值，我們將此幀視為新的關(guān)鍵幀。這個(gè)標(biāo)準(zhǔn)是為了避免完全丟失特征軌跡。

B.IMU預(yù)積分

?????? IMU預(yù)積分是在[22]中首次提出的，它將歐拉角的旋轉(zhuǎn)誤差參數(shù)化.在我們先前的工作中，我們提出了一個(gè)流形上的IMU預(yù)積分旋轉(zhuǎn)公式，該文利用連續(xù)時(shí)間的IMU誤差狀態(tài)動(dòng)力學(xué)推導(dǎo)了協(xié)方差傳播，但忽略了IMU偏置.文[23]通過增加后驗(yàn)IMU偏置校正，進(jìn)一步改進(jìn)了預(yù)積分理論。本文通過引入IMU偏置校正，擴(kuò)展了我們?cè)谇懊婀ぷ鱗7]中提出的IMU預(yù)積分。

?????? IMU的的原始陀螺儀和加速度計(jì)測量結(jié)果w^和a^如下：

IMU測量值是在B幀中測量的，它結(jié)合了對(duì)抗重力和平臺(tái)動(dòng)力學(xué)的力量，并受到加速度偏置b_a、陀螺儀偏置b_w和附加噪聲的影響。假設(shè)加速度計(jì)和陀螺儀測量中的附加噪聲為高斯噪聲，n_a～N(0,σ_a ²)，n_w～N(0,σ_w²)。加速度計(jì)偏置和陀螺儀偏置被建模為隨機(jī)游走，其導(dǎo)數(shù)為高斯性的，n_ba～N(0,σ_ba ²)，n_bw～N(0,σ_bw²)。

?????? 給定對(duì)應(yīng)于圖像幀的兩個(gè)時(shí)刻b_k和b_k+1，位置、速度和方向狀態(tài)可以在時(shí)間間隔期[t_k,t_k+1 ]間在W幀內(nèi)通過慣性測量傳播：

?t_k是時(shí)間間隔[t_k,t_k+1]之間的持續(xù)時(shí)間。

?????? 可見，IMU狀態(tài)傳播需要幀b_k的旋轉(zhuǎn)、位置和速度。當(dāng)這些起始狀態(tài)改變時(shí)，我們需要重新傳播IMU測量值.特別是在基于優(yōu)化的算法中，每次調(diào)整姿態(tài)時(shí)，都需要在它們之間重傳IMU測量值。這種傳播策略在計(jì)算上要求很高。為了避免再傳播，我們采用了預(yù)積分算法。

?????? 在將參考幀從世界幀改為局部幀bk后，我們能對(duì)只與線性的加速度a^和角速度相關(guān)的部分進(jìn)行預(yù)積分，如下所示：

? ? ? ?可以看出預(yù)積分項(xiàng)能通過IMU測量值單獨(dú)得到，其中將b_k視為參考幀。 只與b_k和b_k+1中的IMU偏置有關(guān)與其他狀態(tài)無關(guān)。當(dāng)偏置估計(jì)發(fā)生變化時(shí)，當(dāng)偏置變化很小時(shí)，我們將按其對(duì)偏置的一階近似來調(diào)整，否則就進(jìn)行重傳。這種策略為基于優(yōu)化的算法節(jié)省了大量的計(jì)算資源，因?yàn)槲覀儾恍枰貜?fù)傳播IMU測量。

??? 對(duì)于離散時(shí)間的實(shí)現(xiàn)，可以采用不同的數(shù)值積分方法，如歐拉積分、中點(diǎn)積分、RK4積分等.這里選擇了Euler積分來演示易于理解的過程(我們?cè)趯?shí)現(xiàn)代碼中使用了中點(diǎn)積分)。

??? 在開始時(shí)， 是0，是恒等四元數(shù)。α,β,γ在(6)中的平均值是按以下步驟逐步傳播的。注意，加性噪聲項(xiàng)n_a,n_w是未知的，在實(shí)現(xiàn)中被視為零。這導(dǎo)致了預(yù)積分的估計(jì)值，標(biāo)記為(^·)：

i是與[t_k,t_k+1]內(nèi)的IMU測量相對(duì)應(yīng)的離散時(shí)刻,δt是兩個(gè)IMU測量i和i+1之間的時(shí)間間隔。

然后討論協(xié)方差傳播問題。由于四維旋轉(zhuǎn)四元數(shù)γ_t^bk被過參數(shù)化，我們將其誤差項(xiàng)定義為圍繞其平均值的擾動(dòng)：

其中δθ^bk_t是三維小擾動(dòng)。

我們可以導(dǎo)出連續(xù)時(shí)間線性化的誤差項(xiàng)(6)的動(dòng)力學(xué)：

P_bk^bk+1可以通過初始協(xié)方差P_bk^bk=0的一階離散時(shí)間協(xié)方差更新遞歸計(jì)算：

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

同時(shí)，δz _bk^bk+1的一階Jacobian矩陣J_bk+1相對(duì)于δz_bk^bk也可以用初始Jacobian J_bk=I遞歸計(jì)算。

利用這個(gè)遞推公式，得到協(xié)方差矩陣p _bk^bk+1和Jacobian J_bk+1。 關(guān)于偏置的一階近似可以寫為:

其中J_ba^α是J_bk+1中的子塊矩陣，其位置對(duì)應(yīng)于 。 也使用同樣的含義。當(dāng)偏置估計(jì)發(fā)生輕微變化時(shí)，我們使用(12)近似校正預(yù)積分結(jié)果，而不是重傳。

現(xiàn)在我們可以寫下IMU測量模型及其對(duì)應(yīng)的協(xié)方差P_bk^bk+1：

V. ESTIMATOR INITIALIZATION

?????? 單目緊耦合VIO是一個(gè)高度非線性的系統(tǒng).由于單目相機(jī)無法直接觀測到尺度，因此，如果沒有良好的初始值，很難直接將這兩種測量結(jié)果融合在一起。人們可以假設(shè)一個(gè)固定的初始條件來啟動(dòng)單目vins估計(jì)器。然而，這種假設(shè)是不合適的，因?yàn)樵趯?shí)際應(yīng)用中經(jīng)常會(huì)遇到運(yùn)動(dòng)下的初始化.當(dāng)IMU測量結(jié)果被大偏置破壞時(shí)，情況就變得更加復(fù)雜了。事實(shí)上，初始化通常是單目vins最脆弱的步驟。需要一個(gè)健壯的初始化過程來確保系統(tǒng)的適用性。

?????? 我們采用松耦合的傳感器融合方法得到初始值.我們發(fā)現(xiàn)，只有視覺的SLAM，或SfM，具有良好的初始化性質(zhì)。在大多數(shù)情況下，視覺系統(tǒng)可以通過從相對(duì)運(yùn)動(dòng)方法(如八點(diǎn)或五點(diǎn)算法或估計(jì)齊次矩陣)中導(dǎo)出初始值來引導(dǎo)自己。通過將度量IMU預(yù)積分與目視SfM結(jié)果相匹配，我們可以粗略地恢復(fù)尺度、重力、速度，甚至偏置。這足以引導(dǎo)非線性單目vins估計(jì)器，如圖4所示。

?????? 與在初始階段同時(shí)估計(jì)陀螺儀和加速度計(jì)偏置的[17]相比，我們在初始階段選擇忽略加速度計(jì)偏置項(xiàng)。加速度計(jì)偏置與重力耦合，由于重力向量相對(duì)于平臺(tái)動(dòng)力學(xué)的大量級(jí)，以及初始階段相對(duì)較短，這些偏置項(xiàng)很難被觀測到。我們以前的工作對(duì)加速度計(jì)偏置標(biāo)定進(jìn)行了詳細(xì)的分析。

A. Sliding Window Vision-Only SfM

?????? 初始化過程開始于一個(gè)只有視覺的SfM來估計(jì)一個(gè)高比例相機(jī)的姿勢和特征位置的圖表。

?????? 我們保持了一個(gè)幀的滑動(dòng)窗口來限制計(jì)算復(fù)雜度。首先，我們檢查了最新幀與之前所有幀之間的特征對(duì)應(yīng)。如果我們能找到穩(wěn)定的特征跟蹤(超過30個(gè)跟蹤特征)和足夠的視差(20個(gè)以上的旋轉(zhuǎn)補(bǔ)償像素)在滑動(dòng)窗口中的最新幀和任何其他幀之間，我們恢復(fù)相對(duì)旋轉(zhuǎn)和這兩個(gè)幀之間的上尺度平移使用五點(diǎn)算法。否則，我們會(huì)將最新的幀保存在窗口中，并等待新的幀。如果五點(diǎn)算法成功的話，我們可以任意設(shè)置標(biāo)度，并對(duì)這兩個(gè)幀中觀察到的所有特征進(jìn)行三角化。基于這些三角特征，采用透視n點(diǎn)(Pnp)方法來估計(jì)窗口中所有其他幀的姿態(tài)。最后，應(yīng)用全局整束平差[36]將所有特征觀測的總重投影誤差降到最小。由于我們還沒有任何世界幀的知識(shí)，我們?cè)O(shè)置了第一個(gè)相機(jī)幀(·)^c0作為參考幀的SfM。所有幀的姿態(tài)(p_ck^c0，q_ck^c0)和特征位置表示相對(duì)于(·)^c0。假設(shè)攝像機(jī)和IMU之間有一個(gè)粗糙測度的外部參數(shù)(P_c^b,q_c^b)，我們可以將姿態(tài)從C幀轉(zhuǎn)換到B幀(IMU)。

其中s是匹配視覺結(jié)構(gòu)與米制尺度的尺度參數(shù)，解決這個(gè)縮放參數(shù)是實(shí)現(xiàn)成功初始化的關(guān)鍵。

B. Visual-Inertial Alignment

?????? 1)陀螺儀偏置標(biāo)定：考慮窗口連續(xù)兩幀b_k和b_k+1，我們從視覺sfM中得到旋轉(zhuǎn)q_bk^c0和q_bk+1^c0，以及通過IMU預(yù)積分得到的相對(duì)約束γ_bk+1^bk。我們將IMU預(yù)積分項(xiàng)線性化，使陀螺儀偏置最小，并將下列成本函數(shù)降到最小：

其中B代表窗口中的所有幀。利用第四部分導(dǎo)出的偏置雅可比，給出了γ_bk+1^bk對(duì)陀螺儀偏置的一階近似。這樣，我們得到了陀螺儀偏置b_w的初始校準(zhǔn)。然后我們用新的陀螺儀偏置重新傳播所有的imu預(yù)積分項(xiàng) 。

2)速度、重力向量和米制尺度初始化：在陀螺儀偏置初始化后，我們繼續(xù)初始化導(dǎo)航的其他基本狀態(tài)，即速度、重力向量和公制標(biāo)度：

其中，當(dāng)取第k幀圖像時(shí)，v_bk^bk是B幀中的速度，g^c0是c0幀中的重力向量，s將單目SfM縮放為公制單位。

考慮窗口中兩個(gè)連續(xù)的幀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是兩個(gè)連續(xù)幀之間的時(shí)間間隔。通過求解這個(gè)線性最小二乘問題：

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

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

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

?

VI. TIGHTLY-COUPLED MONOCULAR VIO

?????? 在估計(jì)器初始化后，我們采用基于滑動(dòng)窗口的緊耦合單目VIO進(jìn)行高精度和魯棒狀態(tài)估計(jì)。圖3顯示了滑動(dòng)窗口的圖示。

A. Formulation

滑動(dòng)窗口中的完整狀態(tài)向量定義為：

?????? 其中x_K是捕獲k^th圖像時(shí)的IMU狀態(tài)。它包含了IMU在世界幀中的位置、速度和方向，以及在IMU Body幀中的加速度計(jì)偏置和陀螺儀偏置，n是關(guān)鍵幀的總數(shù)，m是滑動(dòng)窗口中的特征總數(shù)，λ_l是第一次觀測到的lth特征的逆深度。

我們使用視覺慣性束調(diào)整公式（BA）.我們最小化所有測量殘差的先驗(yàn)和Mahalanobis范數(shù)之和，得到最大后驗(yàn)估計(jì)：

r_B(z_bk+1^bk,X)和r_C(z_l^cj,X)分別是IMU和視覺測量的殘差。剩余條件的詳細(xì)定義將在第六節(jié)的B和C中提出。B是所有IMU測量的集合，C是在當(dāng)前滑動(dòng)窗口中至少觀察到兩次的一組特征。{r_p,H_p}是來自邊緣化的先驗(yàn)信息。CeresSolver被用來解決這個(gè)非線性問題。

B. IMU Measurement Residual

考慮滑動(dòng)窗口中連續(xù)兩個(gè)幀(b_k和b_k+1)內(nèi)的IMU測量，根據(jù)(13)中定義的IMU測量模型，預(yù)積分IMU測量的殘差可以定義為：

其中，[·]_xyz是提取四元數(shù)q的向量部分，以進(jìn)行誤差狀態(tài)表示。δθ_bk+1^bk是四元數(shù)的三維誤差狀態(tài)表示。[ ]^T是在兩個(gè)連續(xù)圖像幀之間的間隔時(shí)間內(nèi)僅使用噪聲加速度計(jì)和陀螺儀測量值預(yù)積分的IMU測量項(xiàng)。加速度計(jì)和陀螺儀偏置也包括在線校正的剩余項(xiàng)中。

C. Visual Measurement Residual

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

其中[u_l^ci,v_l^ci]是第一次觀測第一次出現(xiàn)在i圖像中的第l個(gè)特征。[u^_l^ci,v^_l^ci]是對(duì)jth圖像中相同特征的觀察。π_c^?1是利用攝像機(jī)內(nèi)參數(shù)將像素位置轉(zhuǎn)換成單位向量的反投影函數(shù)。由于視覺殘差的自由度是2，所以我們將殘差向量投影到切平面 上，如圖6所示，b1,b2是兩個(gè)任意選擇的正交基。我們可以很容易地找到一組b1,b2，如算法1所示。在(22)中使用的p_l^cj是正切空間中固定長度的標(biāo)準(zhǔn)協(xié)方差。

D. Marginalization

為了限制基于優(yōu)化的VIO的計(jì)算復(fù)雜度，本文引入了邊緣化.我們有選擇地將IMU狀態(tài)X_K和特征λ1從滑動(dòng)窗口邊緣化，同時(shí)將對(duì)應(yīng)于邊緣狀態(tài)的度量轉(zhuǎn)換為先驗(yàn)。

如圖7所示，當(dāng)?shù)诙€(gè)最新的幀是關(guān)鍵幀時(shí)，它將停留在窗口中，而最老的幀與其相應(yīng)的測量值被邊緣化（邊緣化的測量值作為先驗(yàn)信息）。否則，如果第二個(gè)最新的幀是非關(guān)鍵幀，我們丟掉視覺測量值和保留連接到這個(gè)非關(guān)鍵幀IMU測量值。為了保持系統(tǒng)的稀疏性，我們不丟棄非關(guān)鍵幀的所有測量值（留下IMU預(yù)積分值）。我們的邊緣化方案的目的是在窗口中保持空間分離的關(guān)鍵幀。這確保了特征三角化有足夠的視差，并且最大限度地提高了在大激勵(lì)下獲得加速度計(jì)測量值的概率。

邊緣化是利用Schur補(bǔ)充[39]進(jìn)行的。我們基于與移除狀態(tài)相關(guān)的所有邊緣化度量來構(gòu)造一個(gè)新的先驗(yàn)。新的先驗(yàn)項(xiàng)被添加到現(xiàn)有的先驗(yàn)項(xiàng)中。

?????? 我們確實(shí)注意到，邊緣化導(dǎo)致了線性化點(diǎn)的早期固定，這可能導(dǎo)致次優(yōu)估計(jì)結(jié)果。然而，由于小型漂移對(duì)于VIO來說是可以接受的，我們認(rèn)為邊緣化所造成的負(fù)面影響并不重要。

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

?????? 對(duì)于計(jì)算能力較低的設(shè)備，如手機(jī)，由于對(duì)非線性優(yōu)化的計(jì)算要求很高，使得緊耦合單目VIO無法實(shí)現(xiàn)攝像機(jī)速率輸出。為此，我們采用了一種輕量級(jí)的只運(yùn)動(dòng)視覺慣性束調(diào)整（只運(yùn)動(dòng)的VI BA），以提高狀態(tài)估計(jì)到相機(jī)的速率(30赫茲)。

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

F. IMU Forward Propagation for IMU-Rate State Estimation

?????? IMU測量的速度比視覺測量高得多。雖然我們的VIO的頻率受到圖像捕獲頻率的限制，但是我們?nèi)匀豢梢?span style="color:#FF0000;">通過最近的IMU測量來直接傳播最新的VIO估計(jì)，以達(dá)到IMU速率的性能。高頻狀態(tài)估計(jì)可以作為回環(huán)檢測的狀態(tài)反饋。利用這種IMU速率狀態(tài)估計(jì)進(jìn)行的自主飛行實(shí)驗(yàn)在第九節(jié)的D中進(jìn)行。

G. Failure Detection and Recovery

雖然我們緊耦合的單目視覺對(duì)各種具有挑戰(zhàn)性的環(huán)境和運(yùn)動(dòng)是健壯的。由于強(qiáng)烈的光照變化或劇烈的運(yùn)動(dòng)，失敗仍然是不可避免的。主動(dòng)故障檢測和恢復(fù)策略可以提高系統(tǒng)的實(shí)用性。故障檢測是一個(gè)獨(dú)立的模塊，它檢測估計(jì)器的異常輸出。我們目前使用以下標(biāo)準(zhǔn)進(jìn)行故障檢測：

*在最新幀中跟蹤的特征數(shù)小于某一閾值；

*最后兩個(gè)估計(jì)器輸出之間的位置或旋轉(zhuǎn)有較大的不連續(xù)性；

*偏置或外部參數(shù)估計(jì)有較大的變化；

一旦檢測到故障，系統(tǒng)將切換回初始化階段。一旦單目VIO被成功初始化，將新建一個(gè)獨(dú)立的位姿圖段。

?

VII. RELOCALIZATION

我們的滑動(dòng)窗口和邊緣化方案限制了計(jì)算的復(fù)雜性，但也給系統(tǒng)帶來了累積漂移。更確切地說，漂移發(fā)生在全局三維位置(x，y，z)和圍繞重力方向的旋轉(zhuǎn)(偏航)。為了消除漂移，提出了一種與單目VIO無縫集成的緊耦合重定位模塊.重定位過程從一個(gè)循環(huán)檢測模塊開始，該模塊標(biāo)識(shí)已經(jīng)訪問過的地方。然后建立回環(huán)檢測候選幀和當(dāng)前幀之間的特征級(jí)連接。這些特征被對(duì)應(yīng)緊密地集成到單目VIO模塊中，從而得到無漂移狀態(tài)估計(jì)，并且計(jì)算開銷最小。多個(gè)特征的多觀測直接用于重定位，從而提高了定位的精度和狀態(tài)估計(jì)的平滑性。圖9(a)示出了重新定位過程的圖形說明。

A. Loop Detection

我們利用DBoW 2，一種最先進(jìn)的詞袋位置識(shí)別方法來進(jìn)行循環(huán)檢測.除了用于單目VIO的角特征外，另外500個(gè)拐角被檢測并由BRIEF描述符描述。額外的角落特征用于在回路檢測中實(shí)現(xiàn)更好的召回率。描述符被視為用于查詢可視化數(shù)據(jù)庫的可視單詞。DBoW 2在時(shí)間和幾何一致性檢查后返回回環(huán)檢測候選項(xiàng)。我們保留所有用于特征檢索的BRIEF描述符，丟棄原始圖像以減少內(nèi)存消耗。

我們注意到，我們的單目VIO能夠使?jié)L動(dòng)和俯仰角可以被觀察到。因此，我們不需要依賴旋轉(zhuǎn)不變的特性，例如ORB SLAM中使用的ORB特性。

B. Feature Retrieval

當(dāng)檢測到環(huán)路時(shí)，通過檢索特征對(duì)應(yīng)性建立本地滑動(dòng)窗口與回環(huán)候選點(diǎn)之間的連接。通過BRIEF描述符匹配找到對(duì)應(yīng)關(guān)系。直接描述符匹配可能會(huì)導(dǎo)致大量異常值。為此，我們使用兩步幾何離群點(diǎn)剔除，如圖10所示。

*2D-2D：RANSAC的基本矩陣檢驗(yàn).我們利用當(dāng)前圖像中檢索到的特征的二維觀測和回環(huán)候選圖像進(jìn)行基本矩陣檢驗(yàn)。

*3D-2D:RANSAC的PNP檢驗(yàn)。基于已知的特征在局部滑動(dòng)窗口中的三維位置，以及在環(huán)路閉合候選圖像中的二維觀測，進(jìn)行PNP檢驗(yàn)。

當(dāng)超過一定閾值的不動(dòng)點(diǎn)數(shù)時(shí)，我們將該候選點(diǎn)視為正確的循環(huán)檢測并執(zhí)行重新定位。

C. Tightly-Coupled Relocalization

?????? 重定位過程有效地使單目VIO保持的當(dāng)前滑動(dòng)窗口與過去的姿態(tài)圖保持一致。在重新定位過程中，我們將所有閉環(huán)幀的姿態(tài)作為常量，利用所有IMU測量值、局部視覺測量和從回環(huán)中提取特征對(duì)應(yīng)值，共同優(yōu)化滑動(dòng)窗口。我們可以輕松地為回環(huán)幀v所觀察到的檢索到的特征編寫視覺測量模型，使其與VIO中的視覺測量相同，如(25)所示。唯一的區(qū)別是，從位姿圖或直接從上一個(gè)里程計(jì)的輸出(如果這是第一次重定位)獲得的回環(huán)幀的姿態(tài)(q_v^w,p_v^w)被視為常數(shù)。為此，我們可以在(22)中稍微修改非線性代價(jià)函數(shù)，增加循環(huán)項(xiàng)：

其中L是回環(huán)幀中檢索到的特征的觀測集。(l,v)是指在回環(huán)幀v中觀察到的第l個(gè)特征。注意，雖然代價(jià)函數(shù)與(22)略有不同，但待解狀態(tài)的維數(shù)保持不變，因?yàn)榛丨h(huán)幀的構(gòu)成被視為常數(shù)。當(dāng)用當(dāng)前滑動(dòng)窗口建立多個(gè)回環(huán)時(shí)，我們使用來自所有幀的所有環(huán)路閉合特征對(duì)應(yīng)進(jìn)行優(yōu)化。這就為重新定位提供了多視角的約束，從而提高了定位的精度和平滑性。請(qǐng)注意，過去的姿態(tài)和循環(huán)關(guān)閉幀的全局優(yōu)化發(fā)生在重新定位之后，將在第八節(jié)中討論。

?

?

VIII. GLOBAL POSE GRAPH OPTIMIZATION

?????? 重新定位后，局部滑動(dòng)窗口移動(dòng)并與過去的姿態(tài)保持一致。利用重定位結(jié)果，此附加姿態(tài)圖優(yōu)化步驟被開發(fā)以確保過去姿勢的集合被登記為全局一致配置

?????? 由于我們的視覺慣性設(shè)置使?jié)L動(dòng)和俯仰角完全可觀測，累積漂移只發(fā)生在四個(gè)自由度(x，y，z和偏航角)。為此，我們忽略了對(duì)無漂移滾轉(zhuǎn)和俯仰狀態(tài)的估計(jì)，只進(jìn)行了四自由度姿態(tài)圖的優(yōu)化。

A. Adding Keyframes into the Pose Graph

當(dāng)關(guān)鍵幀從滑動(dòng)窗口中邊緣化時(shí)，它將被添加到位姿圖中。這個(gè)關(guān)鍵幀在位姿圖中充當(dāng)頂點(diǎn)，它通過兩種類型的邊與其他頂點(diǎn)連接：

1）序列邊緣：關(guān)鍵幀將建立與其先前關(guān)鍵幀的若干順序邊。序列邊緣表示局部滑動(dòng)窗口中兩個(gè)關(guān)鍵幀之間的相對(duì)轉(zhuǎn)換，其值直接從VIO中獲取。考慮到新邊緣化的關(guān)鍵幀i及其先前的一個(gè)關(guān)鍵幀j，序列邊緣只包含相對(duì)位置p_ijⁱ和偏航角 。

2）循環(huán)閉合邊緣：如果新邊緣化的關(guān)鍵幀有一個(gè)循環(huán)連接，它將與回環(huán)幀通過一個(gè)環(huán)閉合邊在姿態(tài)圖中連接。同樣，閉環(huán)邊緣只包含與(27)相同定義的四自由度相對(duì)位姿變換。利用重新定位的結(jié)果，得到了環(huán)路閉合邊的值。

B.4-DOF Pose Graph Optimization

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

其中，φⁱ,θi是直接從單目VIO中得到的滾動(dòng)角和俯仰角的估計(jì)。通過最小化以下代價(jià)函數(shù)，對(duì)順序邊和回環(huán)邊的整個(gè)圖進(jìn)行了優(yōu)化：

其中S是所有序列邊的集合，L是全環(huán)閉包邊的集合。盡管緊耦合的重新定位已經(jīng)有助于消除錯(cuò)誤的閉環(huán)，但我們添加了另一個(gè)Huber規(guī)范ρ(·)，以進(jìn)一步減少任何可能的錯(cuò)誤循環(huán)的影響。相反，我們不對(duì)序列邊緣使用任何魯棒范數(shù)，因?yàn)檫@些邊緣是從VIO中提取出來的，VIO已經(jīng)包含了足夠多的孤立點(diǎn)排除機(jī)制。

?????? 位姿圖優(yōu)化和重新定位異步運(yùn)行在兩個(gè)獨(dú)立的線程中。以便在可用重定位時(shí)，能立即使用最優(yōu)化的位姿圖。同樣，即使當(dāng)前的姿態(tài)圖優(yōu)化尚未完成，仍然可以使用現(xiàn)有的姿態(tài)圖配置進(jìn)行重新定位。這一過程如圖9(b)所示。

C. Pose Graph Management

隨著行程距離的增加，姿態(tài)圖的大小可能會(huì)無限增長，從而限制了系統(tǒng)的實(shí)時(shí)性。為此，我們實(shí)現(xiàn)了一個(gè)下采樣過程，以將姿態(tài)圖數(shù)據(jù)庫保持在有限的大小。所有具有回環(huán)約束的關(guān)鍵幀都將被保留，而其他與其鄰居方向過近或方向非常相似的關(guān)鍵幀可能會(huì)被刪除。關(guān)鍵幀被移除的概率和其與鄰居空間密度成正比。

?

IX. EXPERIMENTAL RESULTS

我們進(jìn)行了三個(gè)實(shí)驗(yàn)和兩個(gè)應(yīng)用，以評(píng)估所提出的VINS-Mono系統(tǒng).在第一個(gè)實(shí)驗(yàn)中，我們將所提出的算法與另一種最新的公共數(shù)據(jù)集算法進(jìn)行了比較.通過數(shù)值分析，驗(yàn)證了系統(tǒng)的精度。然后在室內(nèi)環(huán)境中測試我們的系統(tǒng)，以評(píng)估在重復(fù)場景中的性能。通過大量的實(shí)驗(yàn)驗(yàn)證了系統(tǒng)的長期實(shí)用性.此外，我們還將所提出的系統(tǒng)應(yīng)用于兩個(gè)應(yīng)用程序。對(duì)于空中機(jī)器人的應(yīng)用，我們使用vins-Mono作為位置反饋來控制無人機(jī)跟蹤預(yù)定的軌跡。然后我們將我們的方法移植到iOS移動(dòng)設(shè)備上，并與GoogleTango進(jìn)行比較。

A. Dataset Comparison

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

在這些實(shí)驗(yàn)中，我們將vins-mono和OKVIS進(jìn)行了比較，這是一種最先進(jìn)的VIO，可以用單目和立體相機(jī)工作。OKVIS是另一種基于優(yōu)化的滑動(dòng)窗口算法.我們的算法與OKVIS在許多細(xì)節(jié)上是不同的，如技術(shù)部分所示。該系統(tǒng)具有良好的初始化和閉環(huán)控制功能。我們使用MH-03和MH-05兩種序列來證明該方法的性能。為了簡化表示法，我們使用vins來表示我們只使用單目VIO的方法，而vins_loop表示含重新定位和姿態(tài)圖優(yōu)化的完全版本。我們分別用OKVISMono和OKVIS_stereo表示OKVIS的結(jié)果。為了進(jìn)行公平的比較，我們丟棄前100個(gè)輸出，并使用接下來的150個(gè)輸出對(duì)齊地面真值，并比較其余的估計(jì)器輸出。

MH-03序列軌跡如圖11所示。我們只比較平移誤差，因?yàn)樾D(zhuǎn)運(yùn)動(dòng)在這個(gè)序列中是可以忽略的。圖12顯示了x,y,z誤差與時(shí)間的關(guān)系，以及平移誤差與距離的關(guān)系。在誤差圖中，具有回環(huán)的vins-mono具有最小的平移誤差.我們?cè)贛H 05上觀察到類似的結(jié)果。該方法具有最小的平移誤差。平移和旋轉(zhuǎn)誤差如圖14所示。由于該序列運(yùn)動(dòng)平穩(wěn)，偏角變化不大，只發(fā)生位置漂移。顯然，循環(huán)閉合能力有效地約束了累積漂移。OKVIS在滾動(dòng)和俯仰角度估計(jì)方面表現(xiàn)更好。一個(gè)可能的原因是VINSMono采用了預(yù)積分技術(shù)，即IMU傳播的一階近似，以節(jié)省計(jì)算資源。

?????? vins-Mono在所有Euroc數(shù)據(jù)集中表現(xiàn)良好，即使在最具挑戰(zhàn)性的序列中，V1-03包括劇烈性的運(yùn)動(dòng)、紋理減少的區(qū)域和顯著的光照變化。由于采用了專用的初始化過程，該方法在V1-03中難以快速初始化。

?????? 對(duì)于純VIO，vin-Mono和OKVIS具有相似的精度，很難區(qū)分哪個(gè)比較好。然而，vins-Mono在系統(tǒng)級(jí)別上優(yōu)于OKVIS。它是一個(gè)完整的系統(tǒng)，具有魯棒的初始化和閉環(huán)功能來輔助單目視覺。

B. Indoor Experiment

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

我們將我們的結(jié)果與OKVIS進(jìn)行了比較，如圖17所示。圖17(a)是OKVIS的VIO輸出。圖17(b)是所提出的無回環(huán)方法的VIO結(jié)果。圖17(c)是所提出的具有重新定位和環(huán)路閉合的方法的結(jié)果。當(dāng)我們?cè)谑覂?nèi)循環(huán)時(shí)，會(huì)出現(xiàn)明顯的漂移。OKVIS和只有VIO版本的vins-Mono在x,y,z和偏航角上積累了大量漂移.我們的重新定位和循環(huán)關(guān)閉模塊有效地消除了所有這些漂移。

C. Large-scale Environment

?????? 1)走出實(shí)驗(yàn)室：我們?cè)谑覂?nèi)和室外混合的環(huán)境中測試vins-mono。傳感器套件與圖15所示的相同。我們從實(shí)驗(yàn)室的一個(gè)座位上開始，在室內(nèi)空間里走來走去。然后我們下了樓梯，在大樓外的操場上走來走去。接下來，我們回到樓里上樓。最后，我們回到了實(shí)驗(yàn)室的同一個(gè)座位。整個(gè)軌道超過700米，持續(xù)約10分鐘。在多媒體附件中可以找到實(shí)驗(yàn)的視頻。

?????? 軌跡如圖19所示。圖19(a)是OKVIS的軌跡。當(dāng)我們上樓時(shí)，OKVIS顯示出不穩(wěn)定的特征跟蹤，導(dǎo)致估計(jì)錯(cuò)誤。我們看不到紅色街區(qū)樓梯的形狀。VINS-Mono的唯一結(jié)果如圖19(b)所示.有閉環(huán)的軌跡如圖19(c)所示。該方法的樓梯形狀清晰。閉環(huán)軌跡與谷歌地圖對(duì)齊，以驗(yàn)證其準(zhǔn)確性，如圖18所示。

?????? OKVIS x,y和z軸的最終漂移為[13.80,-5.26,7.23]米。VINS-Mono無環(huán)閉路的最終方向?yàn)閇-5.47,2.76,-0.29]m，占總彈道長度的0.88%，小于OKVIS的2.36%。經(jīng)回環(huán)修正，最終漂移有界于[-0.032,0.09,-0.07]m，與總彈道長度相比，這是微不足道的。雖然我們沒有地面真值，但我們?nèi)匀豢梢灾庇^地檢查優(yōu)化后的軌道是否平滑，并能精確地與衛(wèi)星地圖對(duì)齊。

2)環(huán)游校園：這張環(huán)繞整個(gè)科大校園的非常大規(guī)模的數(shù)據(jù)集是用一個(gè)手持的VI-Sensor 4記錄下來的。該數(shù)據(jù)集覆蓋的地面長度約為710米，寬度為240米，高度變化為60米。總路徑長度為5.62km。數(shù)據(jù)包含25赫茲圖像和200赫茲IMU，持續(xù)1小時(shí)34分鐘。對(duì)VINS-Mono的穩(wěn)定性和耐久性進(jìn)行測試是一個(gè)非常有意義的實(shí)驗(yàn)。

在這個(gè)大規(guī)模的測試中，我們將關(guān)鍵幀數(shù)據(jù)庫的大小設(shè)置為2000，以提供足夠的循環(huán)信息并實(shí)現(xiàn)實(shí)時(shí)性能。我們運(yùn)行此數(shù)據(jù)集時(shí)，英特爾i7-4790 CPU運(yùn)行在3.60GHz。時(shí)間統(tǒng)計(jì)數(shù)據(jù)顯示在表中。估計(jì)的軌跡與圖20中的谷歌地圖一致。與谷歌地圖相比，我們的結(jié)果在這個(gè)非常長時(shí)間的測試中幾乎沒有漂移。

D. Application I: Feedback Control of an Aerial Robot

如圖21(a)所示，我們將vins-Mono應(yīng)用于航空機(jī)器人的自主反饋控制。我們使用了一個(gè)具有752×480分辨率的前瞻性全球快門相機(jī)(MatrixVisionMvBlueFOXMLC200w)，并配備了190度魚眼鏡頭。DJIA3飛行控制器用于IMU測量和姿態(tài)穩(wěn)定控制。星載計(jì)算資源是Intel i7-5500 U CPU，運(yùn)行在3.00GHz。傳統(tǒng)的針孔攝像機(jī)模型不適用于大視場攝像機(jī)。我們使用MEI模型對(duì)此相機(jī)進(jìn)行校準(zhǔn)，由[43]中引入的工具包進(jìn)行校準(zhǔn)。

在本實(shí)驗(yàn)中，我們使用VINS_Mono的狀態(tài)估計(jì)來測試自主軌跡跟蹤的性能。在這個(gè)實(shí)驗(yàn)中，回環(huán)檢測被禁止。四轉(zhuǎn)子被命令跟蹤一個(gè)圖8模式，每個(gè)圓圈半徑為1.0米，如圖21(b)所示。在彈道周圍設(shè)置了四個(gè)障礙物，以驗(yàn)證VINS-Mono無閉環(huán)的準(zhǔn)確性。在實(shí)驗(yàn)過程中，四轉(zhuǎn)子連續(xù)四次跟蹤這一軌跡。100 Hz星載狀態(tài)估計(jì)支持對(duì)四轉(zhuǎn)子的實(shí)時(shí)反饋控制.

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

E. Application II: Mobile Device

我們將vins-Mono移植到移動(dòng)設(shè)備上，并提供一個(gè)簡單的AR應(yīng)用程序來展示其準(zhǔn)確性和健壯性。我們將我們的移動(dòng)實(shí)現(xiàn)命名為vins-Mobile6，并將其與GoogleTangoDevice 7進(jìn)行了比較，后者是移動(dòng)平臺(tái)上商業(yè)上最好的增強(qiáng)現(xiàn)實(shí)解決方案之一。

VINS-Mono運(yùn)行在iPhone7Plus上。我們使用iphone采集的30 Hz圖像，分辨率640×480，以及內(nèi)置的InvenSenseMP67B 6軸陀螺儀和加速度計(jì)獲得的100 Hz IMU數(shù)據(jù)。如圖24所示，我們將iphone與一個(gè)啟用Tango功能的聯(lián)想phab 2 Pro一起安裝。探戈裝置使用全局快門、魚眼相機(jī)和同步IMU進(jìn)行狀態(tài)估計(jì)。首先，我們?cè)谄矫嫔喜迦胍粋€(gè)虛擬立方體，該虛擬立方體是從估計(jì)的視覺特征中提取出來的，如圖25(a)所示。然后，我們拿著這兩個(gè)裝置，以正常的速度在房間內(nèi)外行走。當(dāng)檢測到回路時(shí)，我們使用四自由度姿態(tài)圖優(yōu)化.消除x,y,z和偏航漂移。

有趣的是，當(dāng)我們打開一扇門時(shí)，探戈的偏航估計(jì)會(huì)跳到一個(gè)很大的角度，如圖25(b)所示。其原因可能是由于不穩(wěn)定的特征跟蹤或主動(dòng)故障檢測和恢復(fù)而導(dǎo)致的估計(jì)器崩潰。然而，vins-Mono在這個(gè)具有挑戰(zhàn)性的案例中仍然工作得很好。旅行了大約264米后，我們回到起點(diǎn)。最后的結(jié)果可以在圖25(c)中看到，探戈的軌跡在最后一圈會(huì)漂移，而我們的vins會(huì)回到起點(diǎn)。通過對(duì)四自由度姿態(tài)圖的優(yōu)化，消除了總軌跡的漂移.這也證明了，與開始相比，立方體被標(biāo)記到圖像上的同一位置。

誠然，探戈比我們的實(shí)施更準(zhǔn)確，尤其是對(duì)于局部狀態(tài)的估計(jì)。實(shí)驗(yàn)結(jié)果表明，該方法可以在通用移動(dòng)設(shè)備上運(yùn)行，并且具有比較特殊工程設(shè)備的潛力。實(shí)驗(yàn)還證明了該方法的魯棒性。視頻可以在多媒體附件中找到。

?

X. CONCLUSION AND FUTURE WORK

本文提出了一種魯棒、通用的單目視覺慣性估計(jì)器.我們的方法既具有先進(jìn)的和新的解決方案的IMU預(yù)積分，估計(jì)器初始化和故障恢復(fù)，在線外部校準(zhǔn)，緊耦合視覺慣性校正，重新定位，和有效的全局優(yōu)化。我們通過與最先進(jìn)的開源實(shí)現(xiàn)和高度優(yōu)化的行業(yè)解決方案進(jìn)行比較，顯示出更好的性能。我們開放個(gè)人電腦和iOS的實(shí)現(xiàn)，以造福社會(huì)。

??? 雖然基于特征的VINS估計(jì)器已經(jīng)達(dá)到了現(xiàn)實(shí)部署的成熟程度，但我們?nèi)匀豢吹搅宋磥硌芯康脑S多方向。單目VINS可能會(huì)根據(jù)運(yùn)動(dòng)和環(huán)境而達(dá)到弱可觀測甚至退化的狀態(tài)。我們最感興趣的是在線方法來評(píng)估單目vins的可觀測性，以及在線生成運(yùn)動(dòng)計(jì)劃來恢復(fù)可觀測性。另一個(gè)研究方向是在大量消費(fèi)設(shè)備上大規(guī)模部署單目VINS，例如移動(dòng)電話。這一應(yīng)用要求在線校準(zhǔn)幾乎所有傳感器的內(nèi)在和外部參數(shù)，以及在線識(shí)別校準(zhǔn)質(zhì)量。最后，我們感興趣的是制作由單目vins給出的稠密地圖。我們?cè)赱44]中首次給出了用于無人機(jī)導(dǎo)航的單目視覺-慣性稠密地圖的結(jié)果。然而，為了進(jìn)一步提高系統(tǒng)的精度和魯棒性，還需要進(jìn)行廣泛的研究。

總結(jié)

以上是生活随笔為你收集整理的VINS-Mono翻译的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。