利用慣導測量單元確定關鍵幀的實時SLAM算法

衛(wèi)文樂，金國棟，譚力寧，蘆利斌，陳丹琪

（火箭軍工程大學核爆偵測工程系，西安710025）

（?通信作者電子郵箱xxlwei@qq.com）

0 引言

高機動性機器人（微小型無人機等）正廣泛應用于情報偵察、電子對抗、目標監(jiān)視和災害救援等任務，這些機器人對載荷的要求是重量輕、尺寸小和功耗低等［1］。實時高效的狀態(tài)估計是機器人完成這些任務的關鍵，特別是無人機，獲得正確的位姿是空中機器人在空中保持穩(wěn)定的前提。視覺同時定位與 建 圖（Visual Simultaneous Localization and Mapping，V-SLAM）是機器人將機載相機作為唯一傳感器在未知環(huán)境中實時定位，同時構建周邊環(huán)境地圖的一種方法，被普遍應用于視覺導航系統(tǒng)［2-3］。

單純的V-SLAM 在發(fā)生純旋轉或者快速運動時會失效，結合來自攝像機的視覺信息和慣導測量單元（Inertial Measurement Unit，IMU）的視覺慣導同時定位與建圖（Visual Inertial-SLAM，VI-SLAM）方法很受歡迎，因為它在全球定位系統(tǒng)（Global Positioning System，GPS）拒絕的環(huán)境中表現(xiàn)良好，并且與基于激光雷達的方法相比，只需要一個小巧輕便的傳感器封裝，使其成為無人機平臺的首選技術［4］。

V-SLAM 中的視覺里程計（Visual Odometry，VO）是利用兩幀圖像信息及其之間的關聯(lián)性恢復相機的三維運動并獲得局部地圖，主要方法有直接法和特征點法。直接法通過最小化光度誤差來獲得相機位姿，如LSD-SLAM（Large-Scale Direct Monocular SLAM）［5］中的VO；特征點法在檢測、提取以及匹配圖像特征后，最小化重投影誤差，得到相機間的位姿變換，例如ORB-SLAM2［6］中的VO；相對于直接法，特征點法對光照變化更加魯棒，沒有小基線運動假設，精度較高，但是需要提取和匹配特征，計算量較大。

VI-SLAM 從融合方式上看分為松耦合和緊耦合。松耦合是分別使用IMU 信息和視覺信息來得到各自位姿變化，再將得到的兩個位姿變化進行融合。例如Lynen 等［7］將IMU 輸出的位姿信息與通過擴展卡爾曼濾波器進行多傳感器融合以得到一個更加魯棒的位姿。緊耦合需要先把圖像特征添加到特征向量中去，而后通過濾波或者優(yōu)化的方法得到系統(tǒng)的姿態(tài)。例如VINS-Mono［8］通過聯(lián)合優(yōu)化所有的視覺、慣性信息求解滑窗內(nèi)的狀態(tài)來求解相機的位姿。松耦合的融合方式計算精簡、易擴展，但是沒有考慮傳感器間的內(nèi)在聯(lián)系，因此精度也不高；緊耦合則由于把圖像特征加入到狀態(tài)向量，整個系統(tǒng)狀態(tài)向量的維數(shù)會非常高，因此也就需要較高的計算量。

VI-SLAM 中IMU 的作用主要有兩方面:為幀間的特征匹配提供一個更好的初始值［9-10］；在后端優(yōu)化中提供更多的有效約束，使得結果更加精確［4，7］。

在實際應用中，從低級別傳感器驅動程序到高級規(guī)劃算法的所有軟件組件必須在嵌入式計算機上實時運行。與其他部件共享計算資源，這就給VI-SLAM 算法效率提出了更高的要求，重要的是，不會產(chǎn)生過多的CPU 消耗間歇性峰值。由于基于特征點法的VI-SLAM 在前端對特征點的提取、描述和匹配耗費大量時間，這是其實時性差一個重要原因，因此在保證不丟失的情況下減少前端對該部分處理的頻次，對提高系統(tǒng)的實時性、降低嵌入式計算機的處理壓力很有意義。

綜上所述，本文提出一種利用IMU 確定關鍵幀的實時SLAM 算法，通過IMU 預積分自適應地確定關鍵幀，自適應閾值來自視覺慣性緊耦合優(yōu)化的結果，而后僅對關鍵幀進行跟蹤，以避免對所有幀進行特征處理。

1 IMU和相機模型

1.1 相機針孔模型

相機將三維空間信息投影到二維成像平面的過程可以用一個針孔模型來表示，針孔模型表示了三維空間點在光線的映射下透過針孔之后，在相機成像平面成像的關系。

其中:k1、k2、p1、p2均是相機的畸變參數(shù)。則該點在像素平面的投影為:

1.2 IMU預積分模型

如圖1 所示，一般情況下IMU 是采樣頻率要遠高于相機的幀率，因此采用預積分模型。該方法對相對測量進行處理，不但能大大提高優(yōu)化速度，還可通過重力對齊得到系統(tǒng)的絕對姿態(tài)。

圖1 IMU預積分示意圖Fig.1 Schematic diagram of IMU pre-integration

在進行后端的非線性優(yōu)化時，為了避免姿態(tài)估計的初始幀估計導致積分重新計算的情況，本文采用增量式。ΔRijB、ΔvijB和ΔpijB為i時刻到j時刻的IMU 的旋轉矩陣、速度和位置的變化量，計算公式如下:

其中:

2 慣導信息確定關鍵幀的SLAM算法

2.1 系統(tǒng)概述

本文方法是在文獻［4］方法的基礎上，采用緊耦合的非線性優(yōu)化的融合方式，同時通過基于詞袋的閉環(huán)優(yōu)化來減少累計漂移，使得算法能在大場景中更好地工作。算法的主要改進在于在特征跟蹤之前通過IMU 預積分自適應地確定關鍵幀，而后僅對關鍵幀進行跟蹤，避免了對所有幀進行特征處理。圖2 為本文算法的系統(tǒng)結構，同時運行跟蹤線程、局部建圖線程和閉環(huán)線程。

1）跟蹤線程:使用IMU預積分的結果來確定關鍵幀，而后只對關鍵幀進行特征跟蹤。在特征跟蹤時，使用關鍵幀間的IMU 信息提供一個較好的初值完成特征匹配。若未IMU 初始化，則進行視覺慣導對齊。最后進行相鄰關鍵幀間的視覺慣導緊耦合優(yōu)化，同時為關鍵幀的確定提供自適應閾值。

2）局部建圖線程:利用跟蹤線程所提供的關鍵幀更新共視圖和地圖點，并剔除掉質量不好地圖點，再通過共視程度較高地利用三角化恢復更多的地圖點。然后在一個本地窗口中優(yōu)化最近的N個關鍵幀及所有被觀測到的地圖點。最后剔除掉冗余的關鍵幀。

3）閉環(huán)線程:通過詞袋模型檢測閉環(huán)幀，利用剛體變換來檢驗此匹配的正確性。若閉環(huán)檢測成功最后執(zhí)行圖優(yōu)化來減小軌跡中的累積誤差。最后將所有的狀態(tài)作為優(yōu)化變量來執(zhí)行全局BA 優(yōu)化，最小化重投影誤差和IMU 誤差，來得到一個全局一致的軌跡和地圖。

2.2 跟蹤線程

跟蹤線程負責實時地跟蹤傳感器的位姿、速度和IMU零偏。

2.2.1 關鍵幀確定

現(xiàn)有的確定關鍵幀的方法一般是通過跟蹤地圖點云的數(shù)量和距離上一關鍵幀的幀數(shù)來判斷，這種方法不但要對每個當前幀進行特征的提取、描述和匹配，而且需要固定的閾值。本文利用IMU 所提供的信息來確定關鍵幀，而后只對關鍵幀進行特征處理，避免了對所有幀進行特征處理。

將第一幀設定為關鍵幀，第二個關鍵幀的判斷閾值為給定τ1。從第三個關鍵幀開始判斷閾值來自適應閾值τm(m ≥2)，它來自于視覺慣性緊耦合優(yōu)化的結果，利用前兩個關鍵幀的位姿差來作為選取關鍵幀的閾值。對于時間戳為tn的n 幀圖像，判斷關鍵幀算法如下:

其中:kfm中存放著關鍵幀的時間戳；imun為tn時刻的IMU 的積分位姿；pn為位置。轉角θn可由式（5）推導而來:

關鍵幀的判斷發(fā)生在第8）行，位置差和轉角差只要有一個達到閾值，則確定為關鍵幀。而后本文只對關鍵幀進行特征跟蹤，大大減輕了平臺的計算壓力。

在上述方法所確定的關鍵幀發(fā)生丟失時，則退回到上一關鍵幀采用ORB-SLAM2 中單純采用視覺的方法來確定下一關鍵幀，而后繼續(xù)采用上述利用IMU信息確定關鍵幀的方法。

2.2.2 特征跟蹤

利用IMU 預積分結果可以得到一個可靠的當前幀位姿，摒棄了ORB-SLAM2的勻速運動模型。一旦預測到相機姿勢，就能更快地搜索到上一關鍵幀中的特征點在地圖中對應的點云與當前關鍵幀圖像的匹配點，最后利用匹配點對當前相機的位姿進一步優(yōu)化。

如果搜索不到特征點，則計算當前關鍵幀的詞袋向量進行全局重定位的初始化位姿。一旦獲得了初始相機位姿和一組初始特征匹配點，就可以將更多的地圖點投影到圖像上以尋找更多匹配點。

2.2.3 視覺慣導對齊

如果整個系統(tǒng)沒有初始化，還需要進行視覺慣導對齊，其過程如圖3 所示。本文采用松耦合的傳感器融合方法來獲得初始值，在大多數(shù)情況下，純視覺SLAM 系統(tǒng)可以從相對運動中來估計相機的位姿信息SFM（Structure From Motion）。通過將IMU 預積分與純視覺SFM 結果進行對齊，可以粗略地恢復出偏置、速度、尺度和重力。如圖3 所示，框狀模型是相機，五角星為相機觀測到的三維路標，最上方的實線是IMU 預積分算出的相機位姿信息。

圖3 初始化的視覺慣性對齊過程Fig.3 Initialized visual inertia alignment process

2.2.4 視覺慣導緊耦合優(yōu)化

ΣI為預積分的信息矩陣；ΣR為零偏的隨機游走［11］。本文通過高斯牛頓法求解該優(yōu)化問題，將優(yōu)化的結果和海塞矩陣用于下次優(yōu)化。

如果地圖點沒有更新，利用當前關鍵幀k 去優(yōu)化下一關鍵幀k + 1，并使用先前優(yōu)化結束時計算的先驗:

其中Eprior( k )為先驗項:

其中:

2.3 局部建圖線程

局部建圖線程在插入來自跟蹤線程的關鍵幀之后執(zhí)行視覺慣導局部BA。這里優(yōu)化滑動窗口中N 關鍵幀和被這N 幀所能觀測到的地圖點。與ORB-SLAM2不同的是，所有其余可以觀測到這些地圖點的關鍵幀不僅通過共視圖與滑動窗口連接，還通過IMU信息連接。為了保證局部后端優(yōu)化的可靠性，滑動窗口的長度N 取10，同時這個長度也兼顧了后端的實時表現(xiàn)。

純視覺的ORB-SLAM2 后端局部優(yōu)化是通過優(yōu)化殘差式（11）來得到最優(yōu)局部位姿和地圖點:

3 實驗驗證與分析

為了驗證本文算法的實時性和可行性，對算法的每個部分的耗時進行統(tǒng)計。有效性的驗證分為兩部分:在公開數(shù)據(jù)集進行仿真并與優(yōu)秀算法進行對比；在實驗室搭建的無人機平臺上進行實驗驗證。

3.1 算法耗時

PC 平臺配置為Intel Core i7-7700HQ CPU @ 2.8 GHz，內(nèi)存為16 GB，系統(tǒng)為Ubuntu16.04。本文采用了EuRoC 數(shù)據(jù)集［12］，它采用SkybotixVI 傳感器提供一系列的單目及雙目相機圖片，以及與其對應對齊的IMU 加速度及角速度測量值。使用其中MH-01 進行算法的跟蹤實時性檢測，統(tǒng)計每幀跟蹤所需要的時間，本文算法和VIORB-SLAM 跟蹤耗時如圖4所示。

從圖4 中可以看出，本文算法的跟蹤耗時要少于VIORBSLAM 耗時，同時沒有隨著時間的推移而增加。這主要是本文在2.2.1 節(jié)提出的采用了利用IMU 預積分信息確定了關鍵幀的方法，在沒有丟失的情況下對中間幀不進行處理，減少了跟蹤的計算量，提高了實時性。同時本文算法在進行實驗時會出現(xiàn)了兩次跟蹤丟失的情況，如圖4 中矩形框所示，其發(fā)生很大程度是由于初始閾值τ1的選取是個經(jīng)驗值。

同時局部建圖和閉環(huán)線程的耗時如圖5 所示，也能保持在較低的水平。

圖4 兩種算法的跟蹤耗時Fig.4 Tracking time cosumption of two algorithms

圖5 本文算法的局部建圖和閉環(huán)線程耗時Fig.5 Local mapping and loop closing thread time consumption of the proposed algorithm

3.2 公開數(shù)據(jù)集仿真

為進一步驗證本文算法的性能，將本文算法與ORBSLAM、VIORB-SLAM 分別進行了姿態(tài)/定位精度的對比評估。本文分別在EuRoC 中3 個難度不同的數(shù)據(jù)集MH-01、MH-03、MH-05上進行實驗。

確定關鍵幀時初始閾值τ1=［0.4，0.2］為經(jīng)驗值，在較為復雜的場景下可適當減小該閾值，不過隨著閾值的減小跟蹤線程的計算量會相應增加。圖6顯示了算法在MH-05數(shù)據(jù)集上運行的效果圖，圖像上的點為地圖點云，線條為共視圖，線上的框形為關鍵幀的位姿?？梢钥闯?.2.1 節(jié)所提算法能自適應地選取關鍵幀，在MH-05 較復雜的場景沒有發(fā)生丟失情況。

圖6 MH-05數(shù)據(jù)集仿真界面圖Fig.6 Simulation interface of MH-05 dataset

實驗結果通過文獻［13］算法進行評估，其代碼已經(jīng)開源（https://github. com/uzh-rpg/rpg_trajectory_evaluation）。實驗的軌跡對比如圖7 所示，可以看出在三種不同難度的場景下本文算法表現(xiàn)較好，實際軌跡與真值基本重合，而單純基于視覺的ORB-SLAM 絕對軌跡誤差較大，尤其是在MH-03 數(shù)據(jù)集上，而且缺失絕對尺度。

三種方法的均方根誤差（Root Mean Square Error，RMSE）對比如表1 所示。與單純視覺的ORB-SLAM 算法相比，本文算法引入IMU預積分預測當前幀的位姿并在優(yōu)化中增加了約束，因此定位精度得到了有效提高；在定位精度方面，本文算法與融合IMU信息的VIORB-SLAM算法的表現(xiàn)基本持平。

圖7 不同算法的估計軌跡對比Fig.7 Estimated trajectory comparison of different algorithms

圖8～9 是本文算法在三個數(shù)據(jù)集上的結果。從曲線中可以看出:位置漂移不超過±300 mm，且隨著場景難度的增加而增大；旋轉角誤差不超過±2.5°，算法的精度也有明顯的改善。

圖8 本文算法的旋轉角誤差Fig.8 Rotation angle error of the proposed algorithm

圖9 本文算法的位置漂移Fig.9 Position drift of the proposed algorithm

3.3 實際飛行測試

本文采用Jetson-TX2作為嵌入式處理器，以小覓S1030視覺慣導相機作為傳感器，搭載于大疆經(jīng)緯M100開發(fā)平臺上進行實際飛行測試。NVIDIA Jetson-TX2 配置為ARM A57 +HMP Dual Denver 雙 核 心CPU，內(nèi) 存 為8 GB，系 統(tǒng) 為Ubuntu16.04。 通 過 機 器 人 操 作 系 統(tǒng)（Robot Operating System，ROS）將PC 作為從機來接收TX2 傳過來的位姿和地圖數(shù)據(jù)，并在界面上顯示。在PC 上所接收到的數(shù)據(jù)如圖10所示。

表1 各算法的RMSE對比Tab.1 RMSE comparison of different algorithms

圖10 從機的ROS界面Fig.10 ROS interface of slave

為了對比由單目SLAM 系統(tǒng)、本文融合系統(tǒng)和IMU 信息估計出的無人機飛行軌跡，操作無人機在室內(nèi)按照6.5 m×10.5 m 矩形路線飛行一圈。圖11 中點劃線為單目SLAM 估計出的軌跡，虛線為本文融合系統(tǒng)估計的軌跡，實線為IMU 信息得到得軌跡，雙劃線為6.5 m×10.5 m 矩形?？梢钥闯隼肐MU 信息估計無人機運動時，其導航誤差就隨時間而快速增長，難以估計出無人機的準確飛行軌跡；利用單目SLAM 系統(tǒng)估計無人機運動時，雖然可以得到無人機在空中飛行的大致軌跡，但是由于單目相機無法得到環(huán)境的絕對尺度，所以無法準確地表達無人機在空間中的飛行軌跡；利用本文算法進行視覺慣導組合導航時，能夠較準確地估計出無人機飛行的真實軌跡，且能得具有尺度信息的點云地圖。

圖11 單目SLAM、融合IMU的單目SLAM和IMU信息估計的無人機飛行軌跡Fig.11 UAV trajectories estimated by monocular SLAM，monocular SLAM merging IMU and IMU information

4 結語

本文提出一種利用IMU 確定關鍵幀的實時SLAM 算法。該算法在跟蹤線程時不需要對來自視覺傳感器的每幀圖像進行特征的提取、描述和匹配以確定關鍵幀，而是通過IMU預積分自適應地確定關鍵幀，自適應閾值來自視覺慣性緊耦合優(yōu)化的結果，而后僅對關鍵幀進行跟蹤，減少了前端對該部分處理的頻次，提高了實時性。使用關鍵幀間的IMU 信息提供一個較好的初值來完成特征匹配，加快了算法收斂的速度。局部建圖線程中，將滑動窗口中的視覺和IMU 約束通過視覺慣導光束平差法進行優(yōu)化，能得到更加準確的位姿狀態(tài)和環(huán)境信息。EuRoC 數(shù)據(jù)集仿真實驗實驗表明，算法能在不失精度和魯棒性的情況下顯著減少跟蹤線程耗時，降低了VI-SLAM對計算資源的依賴。在實際飛行測試中，算法能夠較準確地估計出具有尺度信息的無人機飛行真實軌跡，對于無人機的自主化研究有一定意義。