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

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

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

0 引言

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

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

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

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

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

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

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

1 IMU和相機(jī)模型

1.1 相機(jī)針孔模型

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

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

1.2 IMU預(yù)積分模型

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

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

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

其中:

2 慣導(dǎo)信息確定關(guān)鍵幀的SLAM算法

2.1 系統(tǒng)概述

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

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

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

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

2.2 跟蹤線程

跟蹤線程負(fù)責(zé)實(shí)時(shí)地跟蹤傳感器的位姿、速度和IMU零偏。

2.2.1 關(guān)鍵幀確定

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

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

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

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

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

2.2.2 特征跟蹤

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

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

2.2.3 視覺慣導(dǎo)對(duì)齊

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

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

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

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

如果地圖點(diǎn)沒有更新，利用當(dāng)前關(guān)鍵幀k 去優(yōu)化下一關(guān)鍵幀k + 1，并使用先前優(yōu)化結(jié)束時(shí)計(jì)算的先驗(yàn):

其中Eprior( k )為先驗(yàn)項(xiàng):

其中:

2.3 局部建圖線程

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

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

3 實(shí)驗(yàn)驗(yàn)證與分析

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

3.1 算法耗時(shí)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 實(shí)際飛行測試

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

表1 各算法的RMSE對(duì)比Tab.1 RMSE comparison of different algorithms

圖10 從機(jī)的ROS界面Fig.10 ROS interface of slave

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

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

4 結(jié)語

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