基于非線性優(yōu)化的單目視覺/慣性組合導(dǎo)航算法

程傳奇，郝向陽，李建勝，劉智偉，胡 鵬

（信息工程大學(xué)，鄭州 450001）

基于非線性優(yōu)化的單目視覺/慣性組合導(dǎo)航算法

程傳奇，郝向陽，李建勝，劉智偉，胡 鵬

（信息工程大學(xué)，鄭州 450001）

針對自主移動機器人在 GPS拒止環(huán)境下準確實時定位的問題，提出一種單目視覺/慣性組合導(dǎo)航算法。為解決視覺/慣導(dǎo)工作頻率不一致問題，利用預(yù)積分技術(shù)預(yù)先處理慣性測量值；引入了一種快速高精度線性初始化方法，分步估計初始系統(tǒng)尺度、重力方向、速度及零偏；結(jié)合全局地圖三維點約束削弱累積誤差，基于是否更新地圖點，構(gòu)建了兩種基于非線性優(yōu)化的單目視覺/慣性緊耦合模型，以保證導(dǎo)航的局部精度及全局一致性。實驗結(jié)果表明：初始化方法可快速（15 s內(nèi)）實現(xiàn)高精度狀態(tài)初始化；與單目視覺導(dǎo)航算法相比，提出的算法不僅可獲取絕對尺度信息，且導(dǎo)航精度更高；與傳統(tǒng)滑動窗口非線性優(yōu)化方法相比，提出的算法在窗口優(yōu)化過程中加入全局地圖三維點約束，可有效削弱累積誤差，驗證了算法的正確性和可行性。

單目視覺；慣性測量單元；預(yù)積分；初始化；緊耦合；非線性優(yōu)化

高機動性機器人（微小型無人機等）正廣泛應(yīng)用于搜救行動、軍事偵察、邊境保護、戰(zhàn)場損傷評估等任務(wù)[1]中，這些機器人的任務(wù)載荷必須具有重量輕、功耗小、高可靠性及容易配置的特點。城市及室內(nèi)環(huán)境遮擋使得GPS系統(tǒng)不再可靠[2]，而由低成本MEMS慣性測量單元（Inertial Measurement Unit, IMU）和相機組成的單目視覺/慣性系統(tǒng)是一個不錯的選擇[3]。相機捕獲豐富的場景信息以及 IMU可提供高精度短期運動信息，可為機器人自主導(dǎo)航提供必要的運動狀態(tài)估計及足夠的環(huán)境感知能力。

因其高度自主性及可用性，視覺/慣性導(dǎo)航系統(tǒng)（Visual Inertial Navigation System, VINS）成為了機器人研究領(lǐng)域的熱點。目前VINS可分為基于濾波器的方法[4-10]和基于優(yōu)化/光束平差（Bundle Adjustment,BA）的方法[3,11-15]。基于濾波器的方法由于僅優(yōu)化當前的狀態(tài)，故計算量較小， 但由于保持之前的線性化不變，會導(dǎo)致狀態(tài)估計非最優(yōu)；基于優(yōu)化的方法則通過迭代重新線性化以優(yōu)化狀態(tài)估計，但隨著導(dǎo)航時間增加，計算復(fù)雜度太大。在實際操作中，基于優(yōu)化的方法通常采用滑動窗口優(yōu)化，以限制計算復(fù)雜度無限增加[16]。VINS系統(tǒng)另一種分類方式為：松耦合系統(tǒng)[6,8]和緊耦合系統(tǒng)[3-5,7,9-15]。松耦合系統(tǒng)包含獨立的視覺導(dǎo)航系統(tǒng)和IMU系統(tǒng)，把各自狀態(tài)估計結(jié)果作為組合模型輸入進行耦合解算；緊耦合模型把視覺和IMU的測量值作為耦合系統(tǒng)的輸入進行狀態(tài)估計，往往可取得更優(yōu)的結(jié)果。但以上所有方法都是計算增量運動，在增量運動基礎(chǔ)上進行全局運動估計，使得最終導(dǎo)航誤差會無限制增加，最終導(dǎo)致導(dǎo)航系統(tǒng)不可用[15]。

因此，本文提出一種基于非線性優(yōu)化的視覺/慣性組合導(dǎo)航算法，構(gòu)建兩種模式的視覺/慣性緊耦合模型進行實時運動狀態(tài)估計，利用全局地圖三維點[17]約束漂移限制誤差累積，以保證單目VINS導(dǎo)航的局部精度和全局一致性。

1 基本原理

1.1 坐標系約定

如圖1所示，系統(tǒng)默認IMU坐標系與載體坐標系一致，記為B系，文中用下標(·)B表示；世界坐標系記為W系，文中用下標(·)W表示；相機坐標系記為C系，文中用下標(·)C表示。表示載體相對于世界坐標系的位姿，為VINS待估計量。

圖1 視覺/慣性組合導(dǎo)航中的坐標系Fig.1 Coordinate systems used in visual inertial system

1.2 相機投影模型

如圖1所示，針孔相機模型通過投影函數(shù)π(·)把相機坐標系中的三維點投影為像平面的二維像點：

1.3 IMU預(yù)積分

IMU測量在固定時間間隔Δt傳感器的三軸加速度aB和角速度ωB，測量值包含加速度計和陀螺儀零偏ba和bg以及隨機噪聲。IMU姿態(tài)RWB、速度WvB、位置WpB更新方程為

一般情況下，IMU采樣頻率遠大于相機采樣頻率，因此這里采用預(yù)積分技術(shù)[19]計算兩幀影像之間的相對運動作為IMU測量值。

兩幀影像之間的IMU更新可寫為：

2 IMU初始化

借鑒文獻[15]中的方法實現(xiàn)IMU分步初始化：首先通過單目視覺進行初始化關(guān)鍵幀及三維點；然后分步初始化尺度、重力方向、速度及加速度計和陀螺儀零偏。具體實施步驟如下：

2.1 估計陀螺零偏

通過已知的相鄰關(guān)鍵幀姿態(tài)估計陀螺零偏。假設(shè)陀螺隨機噪聲忽略不計，對所有相鄰關(guān)鍵幀，通過最小化陀螺積分獲取的相對姿態(tài)及單目視覺獲取的相對姿態(tài)之間的誤差，可求解陀螺零偏。令bg初始值為0，則可通過Gauss-Newton迭代更新δbg求解。相關(guān)的雅各比解析式可參考文獻[19]。

其中：N是關(guān)鍵幀的數(shù)目；由視覺初始化獲取，為已知量（事先標定）；由式(3)中姿態(tài)積分獲取。

2.2 估計尺度及重力方向

估計出陀螺零偏后，忽略加速度計零偏，可通過預(yù)積分(3)等式更新姿態(tài)、速度和位置測量值。單目視覺無法獲取絕對尺度，因此相機坐標系與IMU坐標系的坐標轉(zhuǎn)換存在尺度因子s。考慮連續(xù)三幀關(guān)系并消去速度分量，組成線性方程組，通過奇異值分解求解估計的尺度和重力

2.3 估計加速度零偏，優(yōu)化尺度和重力方向

令慣性參考系中重力大小為G，重力方向為而 2.2節(jié)估計出的重力方向為則二者之間的旋轉(zhuǎn)RWI可以通過式(7)計算：

那么重力可改寫為：

因航向角（繞z軸旋轉(zhuǎn)）對gW無影響，故把RWI參數(shù)化為繞x、y軸的旋轉(zhuǎn)角，使用擾動模型優(yōu)化：

此時考慮加入加速度計零偏分量的連續(xù)三幀關(guān)系，并消去速度分量，得線性方程。

聯(lián)立所有連續(xù)三幀關(guān)系組成線性方程組，通過奇異值分解求解估計值：優(yōu)化后的尺度*s，重力方向改正量及加速度計零偏

2.4 估計速度

估計出尺度、重力和零偏后，通過積分方程可求解所有幀的速度

3 基于非線性優(yōu)化的視覺/慣導(dǎo)緊耦合模型

組合導(dǎo)航過程中，根據(jù)地圖點是否更新（插入新關(guān)鍵幀時，更新地圖點），設(shè)計了兩種優(yōu)化模式。

3.1 當前幀-鄰幀優(yōu)化模型

若上一幀為非關(guān)鍵幀，則地圖點不更新，此時執(zhí)行當前幀-鄰幀優(yōu)化。

待估計量為當前幀及上一幀的位置、速度、姿態(tài)及IMU零偏，記為：

目標函數(shù)為：

其中，Eproj是視覺重投影誤差，定義如下：

其中：IΩ為預(yù)積分測量值的權(quán)矩陣；Ωbias是零偏權(quán)矩陣；分別為姿態(tài)、速度、位置和零偏誤差項，定義如下：

當前幀-鄰幀優(yōu)化模型中，經(jīng)過本次優(yōu)化后，第j幀的所有狀態(tài)被剔除，而第j+1幀的狀態(tài)優(yōu)化結(jié)果作為下次優(yōu)化的先驗信息進行優(yōu)化。這種模式使得優(yōu)化模型中待優(yōu)化的參數(shù)不會增加，從而保證實時運行。

3.2 當前幀-關(guān)鍵幀優(yōu)化模型

當前幀-鄰幀優(yōu)化模型中包含先驗信息項，但當?shù)貓D點更新后（如插入新關(guān)鍵幀，見3.3節(jié)），此時上一幀的先驗信息對當前優(yōu)化失效，應(yīng)采用當前幀-關(guān)鍵幀優(yōu)化模型。

在當前幀-關(guān)鍵幀優(yōu)化模型保持關(guān)鍵幀各狀態(tài)不變，待估計量只有當前幀的位置、速度、姿態(tài)及IMU零偏，記為：

目標函數(shù)為：

式中：i表示離當前幀j最近的上一個關(guān)鍵幀，其余符號說明與3.1節(jié)一致。

上述兩種緊耦合優(yōu)化模型可通過 Gauss-Newton優(yōu)化方法迭代求解。

3.3 結(jié)合全局地圖三維點的滑動窗口優(yōu)化

插入新關(guān)鍵幀后，需要在后臺進程執(zhí)行局部窗口優(yōu)化，同時優(yōu)化前N關(guān)鍵幀狀態(tài)及被這些關(guān)鍵幀觀測到的所有地圖點。其他可觀測到這些地圖點的關(guān)鍵幀也包含進能量函數(shù)中，但這些關(guān)鍵幀狀態(tài)在優(yōu)化過程中保持不變。

4 算法流程

Step1：單目視覺初始化關(guān)鍵幀及三維地圖點（尺度不確定）；

Step2：視覺/慣導(dǎo)初始化（第2節(jié)）；

Step3：新圖像幀及 IMU測量值輸入，基于當前幀-關(guān)鍵幀優(yōu)化模型解算導(dǎo)航狀態(tài)；

Step4：判斷是否關(guān)鍵幀[17]，若是關(guān)鍵幀，則執(zhí)行地圖點更新（3.3節(jié)），跳轉(zhuǎn)Step3；若不是關(guān)鍵幀，則繼續(xù)Step5；

Step5：新圖像幀及 IMU測量值輸入，基于當前幀-鄰幀優(yōu)化模型解算導(dǎo)航狀態(tài)，跳轉(zhuǎn)Step4；

重復(fù)以上，直至導(dǎo)航結(jié)束。

5 實驗與分析

5.1 實驗數(shù)據(jù)介紹

實驗平臺為搭載Aptina MT9M034相機和ADIS 16448 IMU的小型旋翼無人機，真值由Vicon運動捕捉系統(tǒng)獲取。采集的數(shù)據(jù)包括相機采集的圖像序列（20Hz），IMU測量值（200Hz）及Vicon運動捕捉系統(tǒng)獲取的6D位姿。使用文獻[18]提供的工具箱進行裝置標定，具體標定參數(shù)見表1。

表1 裝置標定參數(shù)Tab.1 Calibr ation parameters

使用標定好的視覺/慣導(dǎo)裝置采集兩組數(shù)據(jù)進行實驗，第一組3682幀圖像，第二組2700幀圖像，同時有對應(yīng)的IMU測量及Vicon運動捕捉系統(tǒng)真值。值得注意的是，運動捕捉系統(tǒng)獲取的真值頻率與視覺/慣導(dǎo)解算頻率不一致，因而進行精度評價時，根據(jù)時間戳進行對齊比較。

實驗平臺為 Lenovo W520筆記本電腦（Inteli7-2760QM, 2.4GHz CPU, 8GB RAM），所有實驗都在Ubuntu14.04系統(tǒng)下使用C++11實現(xiàn)。

5.2 IMU初始化性能測試

為驗證IMU初始化算法的有效性，使用兩組數(shù)據(jù)進行25s的初始化，初始化結(jié)果如圖2～5所示。

圖2、圖4為初始化過程估計的尺度及IMU陀螺、加速度零偏。兩組實驗結(jié)果都可看出，在15s左右時，所有的變量都已經(jīng)收斂，驗證了IMU初始化的有效性。

圖3、圖5為初始化過程中的運算時間及條件數(shù)。條件數(shù)的變化表明：該優(yōu)化問題逐漸由不適定問題變?yōu)檫m定問題，這是因為初始化需要一段時間的運動才能使得所有變量具有可觀測性[3]；初始化運算時間的變化，表明隨著優(yōu)化關(guān)鍵幀數(shù)量的增加，初始化運算時間基本呈線性增加，這是由于該初始化方法為在解算過程中不包含速度分量，為線性模型，計算復(fù)雜度為O(n)。

圖2 IMU初始化結(jié)果（數(shù)據(jù)集1）Fig.2 Results of IMU initialization ( Dataset 1)

圖3 IMU初始化運行時間及條件數(shù)（數(shù)據(jù)集1）Fig.3 Processing time and condition numbers in IMU initialization (Dataset 1)

圖4 IMU初始化結(jié)果（數(shù)據(jù)集2）Fig.4 Results of IMU initialization (Dataset 2)

圖5 IMU初始化過程運行時間及條件數(shù)（數(shù)據(jù)集2）Fig.5 Processing time and condition numbers in IMU initialization (Dataset 2)

5.3 算法導(dǎo)航性能測試

為驗證本文算法的導(dǎo)航性能，把本文算法與純視覺導(dǎo)航及文獻[12]所提非線性優(yōu)化算法解算進行對比分析，具體解算結(jié)果如圖6～9及表2、表3所示。值得注意的是，純單目視覺導(dǎo)航無法獲取絕對尺度，故該對比試驗中，通過 Horn方法[20]求解得到純視覺解算軌跡與真實軌跡之間的相似變換，利用該相似變換把解算軌跡與真實軌跡對齊。

圖6、圖7為純視覺導(dǎo)航、文獻[12]方法與本文提出的視覺/慣導(dǎo)組合算法解算軌跡與真值對比。可看出，三種方法導(dǎo)航解算軌跡都與真值相吻合，驗證了本文算法的解算尺度的正確性；與純視覺導(dǎo)航和文獻[12]方法相比，本文算法與真值偏差更小，驗證了本文算法的有效性。

圖6解算軌跡（數(shù)據(jù)集1）Fig.6 Trajectories of MAV computing by three methods(Dataset 1)

圖7 解算軌跡（數(shù)據(jù)集2）Fig.7 Trajectories of MAV computing by three methods(Dataset 2)

圖8、圖9為三種方法解算誤差對比。可看出，本文提出的視覺/慣性組合導(dǎo)航算法整體上優(yōu)于純視覺導(dǎo)航和文獻[12]方法；且單純視覺導(dǎo)航精度優(yōu)于文獻[12]視覺/慣性組合方法，這里值得注意的是，單目視覺無法獲取絕對尺度，此時的尺度是基于解析方法利用真值解算出的，故單目視覺無法直接應(yīng)用于機器人自主導(dǎo)航；而單目視覺/慣性系統(tǒng)（本文算法與文獻[12]）可直接求解絕對尺度，更加適用于自主導(dǎo)航。與文獻[12]相比，本文方法由于結(jié)合全局地圖點進行滑動窗口優(yōu)化，可有效減小累積誤差。

圖8 解算位置誤差（數(shù)據(jù)集1）Fig.8 Position errors of three methods (Dataset 1)

圖9 解算位置誤差（數(shù)據(jù)集2）Fig.9 Position errors of three methods (Dataset 2)

表2、表3統(tǒng)計了解算位置絕對誤差。可看出本文算法導(dǎo)航精度為亞米級，甚至厘米級，驗證了本文算法用于機器人自主導(dǎo)航的有效性。另外，由于全局地圖維護作為單獨的一個線程[17]在后臺運行，因而狀態(tài)估計線程運算中平均每幀處理時間約為 42 ms，可滿足導(dǎo)航實時性需求。

6 結(jié) 語

本文提出了一種基于非線性優(yōu)化的視覺/慣性組合導(dǎo)航算法用于機器人自主導(dǎo)航，引入分步初始化方法進行系統(tǒng)初始化，結(jié)合IMU預(yù)積分技術(shù)，推導(dǎo)了基于非線性優(yōu)化的視覺/慣導(dǎo)緊耦合模型，利用全局地圖三維點約束限制了誤差累積，并給出了具體的實施步驟，實驗驗證了算法的優(yōu)良性能。但長航時自主導(dǎo)航中環(huán)境中可能會存在動態(tài)目標，動態(tài)場景不再滿足計算機視覺多視幾何原理，因此如何利用IMU特性進一步提高視覺/慣性組合導(dǎo)航在動態(tài)場景下的穩(wěn)健性是下一步要開展的研究。

表2 實驗1解算位置絕對誤差統(tǒng)計Tab.2 S tatistics of absolute position errors of experiment 1

表3 實驗2解算位置絕對誤差統(tǒng)計Tab.3 S tatistics of absolute position errors of experiment 2

（References）：

[1]Santoso F, Garratt M A, Anavatti S G.Visual-inertial navigation systems for aerial robotics: Sensor fusion and technology[J].IEEE Transactions on Automation Science and Engineering, 2016, 99: 1-16.

[2]Bachrach A, Prentice S, He R, et al.RANGE-Robust autonomous navigation in GPS-denied environments[J].Journal of Field Robotics, 2010, 28(5): 644-666.

[3]Yang Z, Shen S.Monocular visual-inertial fusion with online initialization and camera-IMU calibration[C]//IEEE International Symposium on Safety, Security, and Rescue Robotics.IEEE, 2016: 1-8.

[4]Hesch J A, Kottas D G, Bowman S L, et al.Consistency Analysis and Improvement of Vision-aided Inertial Navigation[J].IEEE Transactions on Robotics, 2016,30(1): 158-176.

[5]Li Ming-yang, Mourikis A I.High-precision, consistent EKF-based visual–inertial odometry[J].International Journal of Robotics Research, 2013, 32(32): 690-711.

[6]馮國虎, 吳文啟, 曹聚亮, 等.單目視覺里程計/慣性組合導(dǎo)航算法[J].中國慣性技術(shù)學(xué)報, 2011, 19(3): 302-306.Feng G H, Wu W Q, Cao J L, et al.Algorithm for monocular visual Odometry/SINS integrated navigation[J].Journal of Chinese Inertial Technology, 2011, 19(3):302-306.

[7]Jones E S, Soatto S.Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[J].International Journal of Robotics Research, 2011, 30(4):407-430.

[8]Scaramuzza D, Achtelik M C, Doitsidis L, et al.Visioncontrolled micro flying robots: from system design to autonomous navigation and mapping in GPS-denied Environments[J].IEEE Robotics & Automation Magazine, 2014, 21(3): 26-40.

[9]張朝飛, 羅建軍, 龔柏春, 等.一種復(fù)雜多介質(zhì)環(huán)境下的視覺/慣性自適應(yīng)組合導(dǎo)航方法[J].中國慣性技術(shù)學(xué)報, 2016, 24(2):190-195.Zhang C F, Luo J J, Gong B C, et al.Adaptive integrated navigation method of visual positioning/INS in complex multi-medinm environment[J].Journal of Chinese Inertial Technology, 2016, 24(2): 190-195.

[10]Mourikis A I, Roumeliotis S I.A multi-state constraint Kalman filter for vision-aided inertial navigation[C]//IEEE International Conference on Robotics and Automation.2007: 3565-3572.

[11]Kelly J, Sukhatme G S.Visual-inertial sensor fusion: localization, mapping and sensor-to-sensor self-calibration[J].International Journal of Robotics Research, 2011, 30(1): 56-79.

[12]Leutenegger S, Lynen S, Bosse M, et al.Keyframe-based visual-inertial odometry using nonlinear optimization[J].International Journal of Robotics Research, 2015, 34(3):314-334.

[13]Shen S, Michael N, Kumar V.Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C]//IEEE International Conference on Robotics and Automation.2015: 5303-5310.

[14]Indelman V, Williams S, Kaess M, et al.Information fusion in navigation systems via factor graph based incremental smoothing[J].Robotics & Autonomous Systems, 2013, 61(8): 721-738.

[15]Mur-Artal R, Tardós J D.Visual-inertial monocular SLAM with map reuse[J].IEEE Robotics & Automation Letters, 2017, 2(2): 796-803.

[16]Sibley G, Matthies L, Sukhatme G.Sliding window filter with application to planetary landing[J].Journal of Field Robotics, 2010, 27(5): 587-608.

[17]Mur-Artal R, Montiel J M M, Tardós J D.ORB-SLAM: A versatile and accurate monocular SLAM system[J].IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[18]Furgale P, Rehder J, Siegwart R.Unified temporal and spatial calibration for multi-sensor systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.2014: 1280-1286.

[19]Forster C, Carlone L, Dellaert F, et al.On-manifold preintegration for real-time visual--inertial odometry[J].IEEE Transactions on Robotics, 2015, 33(1): 1-21.

[20]Horn B K P.Closed-form solution of absolute orientation using unit quaternions[J].Journal of the Optical Society of America A, 1987, 4(4): 629-642.

Monocular visual inertial navigation based on nonlinear optimization

CHENG Chuan-qi, HAO Xiang-yang, LI Jian-sheng, LIU Zhi-wei, HU Peng
(Information Engineering University, Zhengzhou 450001, China)

For the problem of autonomous mobile robots’ real-time precise localization in GPS-denied environments, a monocular visual inertial navigation algorithm based on nonlinear optimization is proposed.To handle multi-rate measurements in visual inertial navigation system, the IMU pre-integration technique is utilized to process inertial measurements.A fast accurate linear initialization method is introduced to estimate the initial scale, gravity direction, velocity, and gyroscope and acceleration biases.To guarantee the local accuracy and global consistency of visual inertial navigation system, the global map point constraint is utilized to decrease the influence of accumulated errors, and two kinds of nonlinear optimization based visual inertial tightly coupled model are built according to whether the map points are updated.Experimental results indicate that the initialization method can obtain accurate initial states in 15 s.Compared with the visual navigation only, the proposed algorithm can get the absolute scale and outperforms the visual navigation in accuracy.Compared with classical sliding window methods, the proposed algorithm can effectively decrease the influence of accumulated errors, which validates the correctness and feasibility of the proposed method.

monocular vision; inertial measurement unit; pre-integration; initialization; tightly coupled;nonlinear optimization

V249.3

A

1005-6734(2017)05-0643-07

10.13695/j.cnki.12-1222/o3.2017.05.015

2017-05-02；

2017-08-23

國家863計劃資助項目（2015AA7034057A）；國家自然科學(xué)基金項目（61173077）

程傳奇（1989—），男，博士生，主要研究導(dǎo)航定位與位置服務(wù)。E-mail: legend3q@163.com