視覺慣性導(dǎo)航融合算法研究進(jìn)展

0   引言

近些年來，伴隨著圖形處理技術(shù)、計算機視覺和元宇宙等相關(guān)技術(shù)的進(jìn)步，實現(xiàn)無人設(shè)備高精度、強魯棒性的定位導(dǎo)航始終為無人系統(tǒng)中的關(guān)鍵環(huán)節(jié)和待突破環(huán)節(jié)。美國國防研究與工程署連續(xù)數(shù)年組織機器人生存挑戰(zhàn)賽，以求在復(fù)雜環(huán)境下探索提升導(dǎo)航性能，確保美軍在無人領(lǐng)域的領(lǐng)先地位。中國科協(xié)也于2020 年將無人車的高精度智能導(dǎo)航問題列為十大工程技術(shù)難題之一^[1]。

無人系統(tǒng)導(dǎo)航領(lǐng)域一直以即時定位與地圖構(gòu)建(Simultaneous Localization And Mapping，SLAM) 技術(shù)為核心。純視覺導(dǎo)航由于缺乏深度信息導(dǎo)致對運動敏感薄弱，需要依賴回環(huán)檢測來校正誤差，這導(dǎo)致其魯棒性不足。視覺導(dǎo)航與衛(wèi)星導(dǎo)航系統(tǒng)的組合導(dǎo)航在復(fù)雜的城市場景下，光影變幻導(dǎo)致有效特征點選取難度變大，諸如地下車庫等場景對衛(wèi)星信號干擾強烈，而本文介紹的視覺慣性導(dǎo)航有著顯著的優(yōu)勢。

一是誤差的互補：視覺導(dǎo)航的穩(wěn)定位姿估計可以彌補慣性導(dǎo)航自身固存的累積誤差， 慣性導(dǎo)航測量單元（Inertial Measurement Unit，IMU）高頻率的動態(tài)信號可以補足相機的深度信息；二是適用速度領(lǐng)域的 互補：視覺導(dǎo)航在低速靜態(tài)領(lǐng)域的優(yōu)秀表現(xiàn)，可以抑制IMU的零漂。而在高速運動場景領(lǐng)域，慣性導(dǎo)航可以解決視覺導(dǎo)航由于圖像幀之間缺乏重疊區(qū)域，導(dǎo)致特征提取算法失效的問題。

1   經(jīng)典融合框架

經(jīng)典融合框架中，松耦合算法是視覺慣性領(lǐng)域早期重點研究內(nèi)容。松耦合算法主要依靠卡爾曼濾波器及其后續(xù)改進(jìn)版本，諸如應(yīng)用廣泛的擴展卡爾曼濾波器（Extended Kalman Filter，EKF）、基于蒙特卡洛原理的無跡卡爾曼濾波器（Unscented Kalman Filter，UKF）、非線性非高斯領(lǐng)域表現(xiàn)不俗的粒子濾波器（Particle Filter，PF）等的松耦合方 案陸續(xù)提出。Kelly提出的無人機視覺慣性導(dǎo)航^[2]缺少對傳感器信息缺乏交叉項誤差，無法全面利用傳感器信息，故精度有限。自2010 年起，慣性輔助視覺的緊耦合算法大量出現(xiàn)，由Davison等人提出的單目SLAM^[3]開辟了緊耦合領(lǐng)域的先河。SLAM算法框架如圖1 所示。

同一時間，Klein^[4]等提出了平行跟蹤與建圖（Parallel Tracking And Mapping，PTAM）該文章也是視覺SLAM中里程碑式的文章，為視覺建圖開辟了新的路徑。由于早期文章均針對簡單場景建模，且傳感器采樣率低，特征點提取成功率有限^[5]。之后相關(guān)研究跟進(jìn)，經(jīng)典融合框架發(fā)展迅速。自此以視覺傳感器為核心，慣導(dǎo)信息輔助優(yōu)化后端環(huán)節(jié)成為了主流研究方案。

H.Ranch、F.Tung 與C.Striebel 合作提出了固定區(qū)間最優(yōu)平滑算法^[6]。該算法在數(shù)據(jù)后端處理中具有重要的應(yīng)用價值，在前向估計的基礎(chǔ)上加上反向濾波，充分利用了量測區(qū)間內(nèi)的所有量測值，具有更高的估計精度，所以主流的算法大都使用該理論作為航跡推算框架。

其中以擴展卡爾曼濾波器為基礎(chǔ)的多態(tài)融合卡爾曼濾波器(Multi-State Constraint Kalman Filter，MSCKF)最具有代表性^[7]，此算法最先由Mouriki 和Roumeliotis于2007 年提出，MSCKF 最重要的突破點在于，解決了傳統(tǒng)松耦合算法在圖像信息豐富情景下，視覺算法選取特征點超出系統(tǒng)處理能力的問題。

2012 年，預(yù)積分理論^[8]的提出，為本領(lǐng)域研究帶來了深遠(yuǎn)影響。系統(tǒng)中為了減小優(yōu)化求解器的負(fù)擔(dān)，采用了關(guān)鍵幀策略，由于IMU 的速率顯然要快于關(guān)鍵幀的插入, 預(yù)積分干通過重新參數(shù)化，將關(guān)鍵幀間的IMU測量值積分成相對運動的形式，避免了因為初始條件變化造成的無用信息。隨后Foster也將其拓展到李代數(shù)上，當(dāng)然也增大了預(yù)積分的計算量^[9]。

在預(yù)積分理論基礎(chǔ)上， Raul Mur-Artal 等人提出了ORB-SLAM算法。在此基礎(chǔ)上結(jié)合慣性技術(shù)，構(gòu)想出了一種視覺慣性快速旋轉(zhuǎn)二進(jìn)制描述子算法^[10]（Visual Inertial-Oriented fast and Rotated Brief SLAM，VI-ORB SLAM）。VI-ORB 算法對特征提取也做出了改進(jìn)，在BRIEF采點的基礎(chǔ)上提出了Fast 采點方案，提出定義特征點方向，來實現(xiàn)特征點的旋轉(zhuǎn)不變形。

如果說ORB-SLAM 是導(dǎo)航領(lǐng)域的一顆明珠，那么香港科技大學(xué)的沈劭劼的單目視覺慣性系統(tǒng)算法^[12]（Monocular Visual-Inertial System，VINS-Mono） 無疑是另一個代表性成果。VINS-Mono 提出了一種健壯的基于緊耦合滑動窗口非線性優(yōu)化的單目視覺慣性里程計，初始化策略，包括對相機與IMU 間的外參、數(shù)據(jù)間時間戳進(jìn)行在線校準(zhǔn)。該算法在前端的處理上使用光流追蹤特征點，當(dāng)讀取到新圖像后提取Harris 角點^[13]，然后使用光流法在更新環(huán)節(jié)迭代計算，最后利用RANSAC^[14]算法對跟蹤錯誤的特征點進(jìn)行剔除, 其框架如如圖2 所示。

總的來說，視覺慣性導(dǎo)航算法已經(jīng)經(jīng)過了長足的發(fā)展，在前端和后端上的大量改進(jìn)使得導(dǎo)航精度不斷提升，伴隨著性能的提升，其計算量也在大幅提升，對于實際應(yīng)用仍是一個考驗。

圖2 VINS框架

2   新型融合框架

為提高圖像 識別準(zhǔn)確度，斯坦福大學(xué)李飛飛團隊舉辦了圖像識別挑戰(zhàn)賽，Alex 等人設(shè)計的Alex Net 模型^[15]以領(lǐng)先第2 名近10% 的準(zhǔn)確度奪冠，迅速席卷了深度學(xué)習(xí)領(lǐng)域，使得卷積神經(jīng)網(wǎng)絡(luò)聲名大噪，隨后的卷積神經(jīng)網(wǎng)路的快速算法也廣泛應(yīng)用于本領(lǐng)域^[16]。

Huang 提出了使用循環(huán)神經(jīng)網(wǎng)絡(luò)（Recurrent Neural Network，RNN）網(wǎng)絡(luò)學(xué)習(xí)光流法的視覺里程計^[17]，該算法就是一種典型的用深度學(xué)習(xí)替代前端視覺里程計的計算過程的方法。作者立足于單目視覺里 程計，提出了用于估計相機的自我運動的序列到序列算法。該網(wǎng)絡(luò)學(xué)習(xí)光流的潛在空間并建立圖像序列動力學(xué)模型，并且計算連續(xù)圖像的光流場，最終以自編碼方式提取潛在的光流表示。

Li 提出了端到端EKF 的深度視覺- 慣導(dǎo)里程計^[18]（Towards End-to-end Learning of Visual Inertial Odometry with an EKF）。在耦合思路上仿照專家系統(tǒng)，將傳統(tǒng)算法中表現(xiàn)良好的環(huán)節(jié)予以保留。采用深度學(xué)習(xí)得到的姿態(tài)構(gòu)建量測方程，整體上采用RTS 算法模型，在正反雙方向的計算上優(yōu)化結(jié)果。在數(shù)據(jù)集上測試并證明了系統(tǒng)的有效性。

牛津大學(xué)計算機科學(xué)系Clark 團隊提出了一種基于流形的序列訓(xùn)練方法VINet[19]。其改進(jìn)之處是在中間特征表示水平上進(jìn)行數(shù)據(jù)融合。作者提出使用長短期記憶（Long Short-Term Memory，LSTM） 來彌補RNN 網(wǎng)絡(luò)對全局信息的把握不足。VINet 整體框圖如圖3 所示。

圖3 VINet框架

除此以外，Chen 提出了一種新穎的端對端選擇性傳感器融合框架^[20]，主要應(yīng)用于復(fù)雜環(huán)境下數(shù)據(jù)丟失問題，以及處理野值的框架。由于單目相機圖像信息較少，且缺乏深度信息，在預(yù)測中常出現(xiàn)誤差較大的問題。故構(gòu)架了不同掩蔽策略的方案。將丟失信息的程度劃分為兩類工作環(huán)境，對應(yīng)不同環(huán)境采用不同的策略。結(jié)合故障診斷思想，分為軟、硬兩種融合方案。后驗型視覺慣性導(dǎo)航算法對比表如表1所示。

綜合來看，深度學(xué)習(xí)在純視覺SLAM方向廣泛使用，在視覺慣性領(lǐng)域仍有較大可發(fā)展空間。對于深度學(xué)習(xí)在整體框架中使用的比例不盡相同，整體使用框架雖然性能更好但是計算量過大，局部使用計算量更小但是性能表現(xiàn)稍弱。

表1 后驗型視覺慣性導(dǎo)航算法對比

3   展望未來

3.1 多傳感器融合

視覺與慣性耦合算法在機器人領(lǐng)域取得了顯著的成果，在自動駕駛領(lǐng)域，多傳感器融合算法已經(jīng)得到了廣泛應(yīng)用，諸如全球定位系統(tǒng)(Global Positioning System，GPS) 等等。

Lee 提出一種有效而且魯棒的GPS 輔助的視覺慣導(dǎo)里程計系統(tǒng)（GPS-VIO）[21]，該系統(tǒng)使用低頻的GPS測量數(shù)據(jù)融合相機IMU 數(shù)據(jù)，作者提出一種在線標(biāo)定的方法對于外參、時間偏移以及對傳感器噪聲具有魯棒性的參考幀初始化步驟。香港大學(xué)張富團隊提出了一種快速、穩(wěn)健且通用的激光慣性里程計框架[22]。創(chuàng)新之處是不提取特征直接將原始點配準(zhǔn)到地圖，而這使得環(huán)境中的細(xì)微特征能夠被使用.

3.2 仿生導(dǎo)航

仿生導(dǎo)航作為一種多學(xué)科交叉的新興學(xué)科。動物行為學(xué)和生理學(xué)的研究成果對新型科技領(lǐng)域如計算機、通信領(lǐng)域一直有促進(jìn)推動作用。Hao 結(jié)合了昆蟲復(fù)眼結(jié)構(gòu)和人類視覺器官結(jié)構(gòu)，研制了一種混合型的仿生復(fù)眼系統(tǒng)^[23]。國防科技大學(xué)胡小平教授團隊研制了六通道點測量式仿生光羅盤^[24]。Milford等基于吸引子網(wǎng)絡(luò)模型，構(gòu)建了包含位置細(xì)胞和方向細(xì)胞模型的RatSLAM算法^[25]。

4   結(jié)束語

將視覺導(dǎo)航系統(tǒng)與慣性導(dǎo)航系統(tǒng)融合，互相取長補短，加以耦合的算法使低成本傳感器獲得了高精度和高穩(wěn)定性，讓整個系統(tǒng)邁向小型化、輕量化。視覺慣性導(dǎo)航是一個多學(xué)科交叉，對算法和硬件要求都很高的綜合性平臺。其進(jìn)步必然得益于算法效率的提升與計算機算力的進(jìn)步。所以仍需要后繼者開闊思路，進(jìn)一步探索與挖掘。

參考文獻(xiàn)：

[1] 中國科學(xué)技術(shù)協(xié)會.面向未來的科技—2020重大科學(xué)問題和工程技術(shù)難題解讀[M]. 北京:中國科學(xué)技術(shù)出版社,2020.

[2] KELLY J, SUKHATME G S. Visual-inertial sensor fusion: localization, mapping and sensor-to-sensor selfcalibration[J].International Journal of Robotics Research,2011, 30(1): 56-79.

[3] A. J. DAVISON, I. D. Reid, N. D. MOLTON. Mono S L A M : R e a l - t i m e s i n g l e c a m e r a S L A M [ J ] . I E E E Transactions on Pattern Analysis and Machine Intelligence, 2007,29(6):1052-1067.

[4] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR work-spaces [C].// Proc of the 6th IEEE/ACM International Symposium on Mixed and Augmented Reality.Washington DC: IEEE Computer Society, 2007.

[5] CARRILLO L R. LOPEZ A E. Lozano R. et al. Combining stereo vision and inertial navigation system for a quad-rotor UAV [J].Journal of Intelligent and Robotic Systems, 2012,65 (1) : 373-387.

[6] 嚴(yán)恭敏,鄧瑀.傳統(tǒng)組合導(dǎo)航中的實用Kalman濾波技術(shù)評述[J].導(dǎo)航定位與授時,2020,7(2): 19-24.

[ 7 ] M O U R I K I S A I , R O U M E L I O T I S S I . A m u l t i - state constraint Kalman filter for vision-aided inertial navigation[C].// Proceedings of IEEE International Conference on Robotics and Automation. Piscataway,USA:IEEE, 2007.

[8] LUPTON T, SUKKARIEH S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics,2012, 28(1) :61-76.

[9] FORSTER C, CARLONE L, DELLAERT F, et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation[C]. Proceedings of Robotics Science and Systems,2015.

[10] MUR-ARTAL R, TARDOS J D. Visual-inertial monocular SLAM with map reuse[J]. IEEE Robotics and Automation Letters, 2017,2(2):796-803.

[11] CARLOS CAMPOS, RICHARD ELVIRA. An accurate open-source library for visual, visual-inertial and multi-map SLAM[C].IEEE Transactions on Robotics 37(6):1874-1890, Dec.2021.

[12] SHEN S J, MICHAEL N, KUMAR V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C].IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE2015:5303-5310. DOI: 10.1145/3065386.

[13] SEMMA ABDELILLAH, HANNAD YAACOUB et al. Writer identification using deep learning with FAST key points and Harris corner detector[J].Expert Systems With Applications,2021,184.

[14] PAN Z T, WEN Z W. Analysis of image registration a l g o r i t h m RANSA C a n d i t s impr o v e d a l g o r i t h m [ J ] .I OP C o n f e r e n c e Se r i e s : Ea r t h a n d En v i r o n m e n t a l Science,2021(1):783.

[15] ALEX K, ILYA S, GEOFFREY E H. Image net classification with deep convolutional neural networks[J]. Communications of The ACM. 2017.

[16] A.LAVIN, S.GRAY. Fast algorithms for convolutional neural networks[C].// Proceedings of Computer Vision and Pattern Recognition (CVPR), 2016.

[17] ZHAN H, C.S.WEERASEKERA, J.W.BIAN. Visual odometry revisited: what should be learnt?[C].International Conference on Robotics and Automation (ICRA), 2020.

[18] LI C, S.L.WASLANDER. Towards end-to-end learning of visual inertial odometry with an EKF[C].Conference on Computer and Robot Vision (CRV), 2020:190-197.

[19] CLARK R, WANG S, WEN H, et al. VINet: visual inertial odometry as a sequence to sequence learning problem[C].Proceedings of International Conference on Artificial Intelligence, 2017.

[20] CHEN C, ROSA S, MIAO Y, et al. Selective sensor fusion for neural visual inertial odometry[C].Proceedings of Computer Vision and Pattern Recognition (CVPR), 2019.

[21] LEE W, K. ECKENHOFF. Intermittent GPS-aided VIO: online initialization and calibration[C].International Conference on Robotics and Automation (ICRA), 2020.

[22] XU W, CAI Y. FAST-LIO2: fast direct Li DAR-inertial odometry[C].Transactions on Robotics. DOI: 10.1109.

[23] HAO Q, WANG Z, CAO J, et al. A hybrid bionic image sensor achieving FOV extension and foveated imaging[J]. Sensors, 2018, 18(4):1042.

[24] XIAN Z, HU X, LIAN J, et al. A novel angle computation and calibration algorithm of bio-inspired sky-light polarization navigation sensor[J].Sensors,2014,14(9):17068-17088.

[25] MILFORD M J, WYETH G F. Mapping a suburb with a single camera using a biologically inspired SLAM system[J].IEEE Transactions on Robotics, 2008, 24(5):1038-1053.

[26] S.W.CHEN et al. SLOAM: semantic Lidar odometry and Mapping for Forest Inventory[J]. Robotics and Automation Letters, 2020.

[27] WANG H, XUE C, ZHOU Y et al. Visual semantic localization based on HD map for autonomous vehicles in urban scenarios. International Conference on Robotics and Automation (ICRA), 2021.

（本文來源于《電子產(chǎn)品世界》雜志2023年2月期）