VIO-SLAM

Visual-Inertial Odometry（VIO）即視覺慣性里程計，有時也叫視覺慣性系統(tǒng)（VINS，visual-inertial system），是融合相機和IMU數(shù)據(jù)實現(xiàn)SLAM的算法，根據(jù)融合框架的不同又分為松耦合和緊耦合。

其中VO（visual odometry）指僅視覺的里程計，T表示位置和姿態(tài)。松耦合中視覺運動估計和慣導(dǎo)運動估計系統(tǒng)是兩個獨立的模塊，將每個模塊的輸出結(jié)果進(jìn)行融合。

緊耦合則是使用兩個傳感器的原始數(shù)據(jù)共同估計一組變量，傳感器噪聲也是相互影響的。緊耦合算法比較復(fù)雜，但充分利用了傳感器數(shù)據(jù)，可以實現(xiàn)更好的效果，是目前研究的重點。

相機和IMU的缺點及互補性

相機和IMU融合有很好的互補性。首先通過將IMU 估計的位姿序列和相機估計的位姿序列對齊可以估計出相機軌跡的真實尺度，而且IMU 可以很好地預(yù)測出圖像幀的位姿以及上一時刻特征點在下幀圖像的位置，提高特征跟蹤算法匹配速度和應(yīng)對快速旋轉(zhuǎn)的算法魯棒性，最后IMU 中加速度計提供的重力向量可以將估計的位置轉(zhuǎn)為實際導(dǎo)航需要的世界坐標(biāo)系中。

隨著MEMS器件的快速發(fā)展，智能手機等移動終端可以便捷地獲取IMU數(shù)據(jù)和攝像頭拍攝數(shù)據(jù)，融合IMU 和視覺信息的VINS 算法可以很大程度地提高單目SLAM 算法性能，是一種低成本高性能的導(dǎo)航方案，在機器人、AR/VR 領(lǐng)域得到了很大的關(guān)注。

算法流程

整個流程圖可以分解為五部分：數(shù)據(jù)預(yù)處理、初始化、局部非線性優(yōu)化、回環(huán)檢測和全局優(yōu)化。

各個模塊的主要作用是：

圖像和IMU數(shù)據(jù)預(yù)處理：對于圖像，提取特征點，利用KLT金字塔進(jìn)行光流跟蹤，為后面僅視覺初始化求解相機位姿做準(zhǔn)備。對于IMU，將IMU數(shù)據(jù)進(jìn)行預(yù)積分，得到當(dāng)前時刻的位姿、速度、旋轉(zhuǎn)角，同時計算在后端優(yōu)化中將要用到的相鄰幀間的預(yù)積分增量，及預(yù)積分的協(xié)方差矩陣和雅可比矩陣。

初始化：初始化中，首先進(jìn)行僅視覺的初始化，解算出相機的相對位姿；然后再與IMU預(yù)積分進(jìn)行對齊求解初始化參數(shù)。

局部非線性優(yōu)化：對應(yīng)流程圖中滑動窗口的視覺慣導(dǎo)非線性優(yōu)化，即將視覺約束、IMU約束放在一個大目標(biāo)函數(shù)中進(jìn)行優(yōu)化，這里的局部優(yōu)化也就是只優(yōu)化當(dāng)前幀及之前的n幀的窗口中的變量，局部非線性優(yōu)化輸出較為精確的位姿。

回環(huán)檢測：回環(huán)檢測是將前面檢測的圖像關(guān)鍵幀保存起來，當(dāng)再回到原來經(jīng)過的同一個地方，通過特征點的匹配關(guān)系，判斷是否已經(jīng)來過這里。前面提到的關(guān)鍵幀就是篩選出來的能夠記下但又避免冗余的相機幀（關(guān)鍵幀的選擇標(biāo)準(zhǔn)是當(dāng)前幀和上一幀之間的位移超過一定閾值或匹配的特征點數(shù)小于一定閾值）。

全局優(yōu)化：全局優(yōu)化是在發(fā)生回環(huán)檢測時，利用相機約束和IMU約束，再加上回環(huán)檢測的約束，進(jìn)行非線性優(yōu)化。全局優(yōu)化在局部優(yōu)化的基礎(chǔ)上進(jìn)行，輸出更為精確的位姿。

算法核心

局部優(yōu)化會用到邊緣化，僅用局部優(yōu)化精度低，全局一致性差，但是速度快，IMU利用率高；僅用全局優(yōu)化精度高，全局一致性好，但是速度慢，IMU利用率低；兩者側(cè)重點不同，所以將兩者結(jié)合，可以優(yōu)勢互補。

因此小編設(shè)計實驗采用局部優(yōu)化和全局優(yōu)化融合的方法。

局部優(yōu)化是滑動窗口內(nèi)相機幀的優(yōu)化，全局優(yōu)化是所有關(guān)鍵幀的優(yōu)化，兩者結(jié)合會產(chǎn)生邊緣幀沖突的問題，因為局部優(yōu)化會固定滑動窗口邊緣幀，而全局優(yōu)化發(fā)生回環(huán)檢測的時候則會固定回環(huán)起點的幀。這里的改進(jìn)就是采用相對的位姿邊緣化，即邊緣化以后的點是相對于它上一時刻關(guān)鍵幀的位姿而不是全局的位姿，這樣局部優(yōu)化邊緣化相對位姿（關(guān)鍵幀），扔給全局優(yōu)化整體優(yōu)化。局部邊緣化和全局邊緣化的結(jié)合部分是關(guān)鍵幀。

相對邊緣化可以具體解釋為，相對邊緣化的參考坐標(biāo)系不再是世界坐標(biāo)系，而是與當(dāng)前幀共視且距離最近的一個關(guān)鍵幀的相機系（設(shè)為第k0幀）。視覺約束可以表示為：

區(qū)別于絕對邊緣化的視覺約束

實驗結(jié)果與總結(jié)

實驗一：無人機數(shù)據(jù)集上的實驗

數(shù)據(jù)集采用了歐盟機器人挑戰(zhàn)數(shù)據(jù)集（EuRoC）。EuRoC 數(shù)據(jù)集使用 Asctec Firefly 六旋翼飛行器在倉庫和房間采集數(shù)據(jù)，數(shù)據(jù)集中包括以20Hz采集的相機圖像和200Hz的IMU數(shù)據(jù)，以及運動真值。

實驗結(jié)果如下：

實驗結(jié)果可見，融合優(yōu)化的軌跡和真實軌跡很接近，而僅使用局部優(yōu)化的定位結(jié)果誤差不斷累積。

實驗二：車載數(shù)據(jù)上的實驗

該車載數(shù)據(jù)是在北京市朝陽區(qū)某小區(qū)采集的，在數(shù)據(jù)采集階段，車輛以5km/h 到 30km/h 的速度行駛，一共行駛2271m。

實驗結(jié)果如下：

實驗結(jié)果可見，融合優(yōu)化后的定位結(jié)果明顯優(yōu)于僅使用局部優(yōu)化的定位結(jié)果，融合優(yōu)化中誤差得到及時修正。

參考文獻(xiàn)

J. Delmerico. A Benchmark Comparison of Monocular Visual-Inertial OdometryAlgorithms for Flying Robots. 2018. ICRA

T. Qin. VINS-Mono： A robust and versatile monocular visual-inertial state estimator. arXivpreprint arXiv： 1708.03852， 2017.

N. Trawny. Indirect KalmanFilter for 3D Attitude Estimation. 2005.

Sola. Quaternion kinematics for error-state kalmanfilter. 2017.

K. Eckenhoff. Decoupled， Consistent Node Removal and Edge sparsificationfor graph-based SLAM. 2016.

G. Sibley. Sliding window filter with application to planetary landing. 2010.

S. Leutenegger. Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization. 2015.

H. Liu. ICE-BA： Incremental， consistenand efficient bundle adjustment for visual-inertial slam. 2018. CVPR.

H. Liu. Robust keyframe-based dense SLAM with an RGB-D camera. 2017.

編輯：jq