基于非線性優化的單目視覺/慣性組合導航算法

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

（信息工程大學，鄭州 450001）

基于非線性優化的單目視覺/慣性組合導航算法

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

（信息工程大學，鄭州 450001）

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

單目視覺；慣性測量單元；預積分；初始化；緊耦合；非線性優化

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

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

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

1 基本原理

1.1 坐標系約定

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

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

1.2 相機投影模型

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

1.3 IMU預積分

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

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

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

2 IMU初始化

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

2.1 估計陀螺零偏

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

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

2.2 估計尺度及重力方向

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

2.3 估計加速度零偏，優化尺度和重力方向

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

那么重力可改寫為：

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

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

聯立所有連續三幀關系組成線性方程組，通過奇異值分解求解估計值：優化后的尺度*s，重力方向改正量及加速度計零偏

2.4 估計速度

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

3 基于非線性優化的視覺/慣導緊耦合模型

組合導航過程中，根據地圖點是否更新（插入新關鍵幀時，更新地圖點），設計了兩種優化模式。

3.1 當前幀-鄰幀優化模型

若上一幀為非關鍵幀，則地圖點不更新，此時執行當前幀-鄰幀優化。

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

目標函數為：

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

其中：IΩ為預積分測量值的權矩陣；Ωbias是零偏權矩陣；分別為姿態、速度、位置和零偏誤差項，定義如下：

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

3.2 當前幀-關鍵幀優化模型

當前幀-鄰幀優化模型中包含先驗信息項，但當地圖點更新后（如插入新關鍵幀，見3.3節），此時上一幀的先驗信息對當前優化失效，應采用當前幀-關鍵幀優化模型。

在當前幀-關鍵幀優化模型保持關鍵幀各狀態不變，待估計量只有當前幀的位置、速度、姿態及IMU零偏，記為：

目標函數為：

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

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

3.3 結合全局地圖三維點的滑動窗口優化

插入新關鍵幀后，需要在后臺進程執行局部窗口優化，同時優化前N關鍵幀狀態及被這些關鍵幀觀測到的所有地圖點。其他可觀測到這些地圖點的關鍵幀也包含進能量函數中，但這些關鍵幀狀態在優化過程中保持不變。

4 算法流程

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

Step2：視覺/慣導初始化（第2節）；

Step3：新圖像幀及 IMU測量值輸入，基于當前幀-關鍵幀優化模型解算導航狀態；

Step4：判斷是否關鍵幀[17]，若是關鍵幀，則執行地圖點更新（3.3節），跳轉Step3；若不是關鍵幀，則繼續Step5；

Step5：新圖像幀及 IMU測量值輸入，基于當前幀-鄰幀優化模型解算導航狀態，跳轉Step4；

重復以上，直至導航結束。

5 實驗與分析

5.1 實驗數據介紹

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

表1 裝置標定參數Tab.1 Calibr ation parameters

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

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

5.2 IMU初始化性能測試

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

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

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

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

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

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

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

5.3 算法導航性能測試

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

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

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

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

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

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

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

表2、表3統計了解算位置絕對誤差?？煽闯霰疚乃惴▽Ш骄葹閬喢准?，甚至厘米級，驗證了本文算法用于機器人自主導航的有效性。另外，由于全局地圖維護作為單獨的一個線程[17]在后臺運行，因而狀態估計線程運算中平均每幀處理時間約為 42 ms，可滿足導航實時性需求。

6 結 語

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

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

表3 實驗2解算位置絕對誤差統計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]馮國虎, 吳文啟, 曹聚亮, 等.單目視覺里程計/慣性組合導航算法[J].中國慣性技術學報, 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]張朝飛, 羅建軍, 龔柏春, 等.一種復雜多介質環境下的視覺/慣性自適應組合導航方法[J].中國慣性技術學報, 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）；國家自然科學基金項目（61173077）

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