張玉龍，張國(guó)山

天津大學(xué) 電氣自動(dòng)化與信息工程學(xué)院，天津 300072

1 引言

即時(shí)定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）[1]，指機(jī)器人在自身位置不確定的條件下，在未知環(huán)境中創(chuàng)建地圖，同時(shí)利用地圖進(jìn)行自主定位與導(dǎo)航。由于傳統(tǒng)視覺SLAM算法在準(zhǔn)確性和魯棒性方面存在問題，因此視覺慣性SLAM算法在計(jì)算機(jī)視覺和增強(qiáng)現(xiàn)實(shí)領(lǐng)域越來越多地引起人們的關(guān)注[2]。視覺慣性SLAM算法即為在傳統(tǒng)視覺SLAM算法中加入慣性測(cè)量單元（inertial measurement unit，IMU），算法中的相機(jī)可以提供豐富的環(huán)境信息，用于構(gòu)建3D模型；IMU提供自主運(yùn)動(dòng)信息，可以在短時(shí)間內(nèi)提供陀螺儀及加速計(jì)的估計(jì)值。通過相機(jī)信息與IMU信息的融合，可以增強(qiáng)算法的準(zhǔn)確性及魯棒性[3]。

視覺慣性SLAM算法可以分為兩類：松耦合（loosely-coupled）[4-6]和緊耦合（tightly-coupled）[7-12]。其中松耦合系統(tǒng)為先通過視覺算法估計(jì)位姿，將得到的位姿與IMU數(shù)據(jù)進(jìn)行融合，這種融合方法可以減小算法的計(jì)算復(fù)雜度，但是算法的準(zhǔn)確度較低；緊耦合系統(tǒng)為相機(jī)和IMU測(cè)量數(shù)據(jù)直接進(jìn)行位姿估計(jì)。由于算法的準(zhǔn)確性較高，緊耦合算法逐漸成為視覺慣性SLAM算法研究中的一個(gè)熱門方向。

緊耦合算法又可以分為兩類：基于濾波和基于關(guān)鍵幀。2007年Mourikis和Roumeliotis[7]提出了一種基于擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）的實(shí)時(shí)單目視覺融合算法，被稱為多狀態(tài)約束卡爾曼濾波器（multi-state constraint Kalman filter，MSCKF），提出按照時(shí)間順序維護(hù)一個(gè)位姿的滑動(dòng)窗口，如果特征點(diǎn)在滑動(dòng)窗口的幾個(gè)位姿都可被觀測(cè)，就會(huì)在這幾個(gè)位姿間建立約束，從而進(jìn)行EKF的更新。2015年Bl?sch等人[8]提出了基于直接EKF的方法，通過興趣點(diǎn)測(cè)量值直接使用EKF來估計(jì)相機(jī)位姿和IMU偏差。

2010年Strasdat等人[13]提出了在使用相同計(jì)算資源的情況下，基于優(yōu)化的算法比基于濾波的算法可以得到更準(zhǔn)確的效果，使得基于優(yōu)化的算法逐漸引起人們的關(guān)注。2013年Leutenegger等人[9]驗(yàn)證了在使用稀疏關(guān)鍵幀的情況下，基于非線性優(yōu)化的視覺SLAM算法比基于濾波的算法更為準(zhǔn)確。2015年Leutenegger等人[10]提出了一種基于關(guān)鍵幀的算法，在局部?jī)?yōu)化中優(yōu)化相機(jī)位姿和IMU誤差，跟蹤和重建圖像中興趣點(diǎn)（interest-point）的3D位置。2016年Usenko等人[11]提出了一種類似于非線性優(yōu)化的方法，通過在局部?jī)?yōu)化中使用直接圖像對(duì)齊而不是間接使用興趣點(diǎn)測(cè)量值來得到全局的連續(xù)地圖。2017年Forster等人[12]提出了一種基于預(yù)積分（preintegration）理論的視覺慣性里程計(jì)，將預(yù)積分理論應(yīng)用于SO(3)模型中，將IMU測(cè)量數(shù)據(jù)與圖像中稀疏子集的直接跟蹤相結(jié)合，從而加速了優(yōu)化計(jì)算。

基于緊耦合方法的視覺慣性SLAM算法大多都不具備檢測(cè)閉環(huán)和地圖重利用的能力，即使相機(jī)不斷重新訪問相同的位置，得到的軌跡也會(huì)出現(xiàn)累積漂移現(xiàn)象。2011年Jones等人[14]提出的基于濾波的視覺慣性SLAM算法可以檢測(cè)閉環(huán)，但是不能實(shí)時(shí)地建立全局地圖。2017年Mur-Artal等人[3]提出的基于關(guān)鍵幀非線性優(yōu)化的方法可以檢測(cè)閉環(huán)，但是由于算法在跟蹤和局部?jī)?yōu)化過程中會(huì)固定狀態(tài)量，因此對(duì)初值的要求很高，需要一個(gè)復(fù)雜繁瑣的初始化過程。

視覺SLAM算法另一重要環(huán)節(jié)為視覺定位，即為在離線環(huán)境下構(gòu)建地圖，然后在已構(gòu)建地圖內(nèi)進(jìn)行定位的算法[15-17]。Mur-Artal等人[17]在2014年首次提出了基于關(guān)鍵幀技術(shù)的SLAM算法的重定位，可以以圖像幀速率處理包含數(shù)千個(gè)關(guān)鍵幀的地圖的重定位問題。Lynen等人[16]在2015年提出一種在移動(dòng)設(shè)備上運(yùn)行的視覺慣性定位算法，使用地圖壓縮和圖像匹配技術(shù)在預(yù)先建立的基于興趣點(diǎn)的地圖上來定位相機(jī)。但是，這些算法僅實(shí)現(xiàn)了設(shè)備的重定位，并不具有繼續(xù)建圖的能力。

本文算法在邊緣化圖像幀之后，通過非線性優(yōu)化技術(shù)和閉環(huán)檢測(cè)技術(shù)來得到并行的全局連續(xù)軌跡，從而可以有效地檢測(cè)閉環(huán)和全局優(yōu)化，建立連續(xù)的全局地圖，此外算法可以在已獲得的地圖進(jìn)行重定位和具有繼續(xù)建圖的能力。

本文算法分為五部分：（1）提取特征點(diǎn)進(jìn)行特征點(diǎn)匹配，運(yùn)用RANSAC[18]（random sample consensus）算法排除誤匹配得到位姿，并通過匹配特征點(diǎn)所占的圖像區(qū)域與所有檢測(cè)到特征點(diǎn)的圖像區(qū)域的比率選取關(guān)鍵幀。（2）進(jìn)行局部?jī)?yōu)化，最小化圖像位姿信息的二次投影誤差和IMU誤差，得到局部連續(xù)軌跡估計(jì)。（3）邊緣化圖像幀和關(guān)鍵幀，保持局部?jī)?yōu)化計(jì)算量在一個(gè)恒定的范圍。（4）在全局地圖中通過圖像檢索策略找到與當(dāng)前關(guān)鍵幀匹配的關(guān)鍵幀，從而形成閉環(huán)，然后利用RANSAC算法驗(yàn)證匹配并得到位姿，最后進(jìn)行全局優(yōu)化得到較準(zhǔn)確的地圖。（5）重定位和繼續(xù)建圖。使用和閉環(huán)檢測(cè)類似的圖像檢索算法找到一系列連續(xù)的匹配關(guān)鍵幀對(duì)作為重定位假設(shè)，并根據(jù)新的關(guān)鍵幀與已構(gòu)建的地圖關(guān)鍵幀之間的約束關(guān)系繼續(xù)進(jìn)行SLAM地圖的構(gòu)建。算法框圖如圖1所示。

2 狀態(tài)參數(shù)表示及IMU動(dòng)力學(xué)

本文提到了三種坐標(biāo)系，世界坐標(biāo)系w，相機(jī)坐標(biāo)系c和IMU坐標(biāo)系s。在這里設(shè)位置為pws，姿態(tài)四元數(shù)為qws，速度為vws，陀螺儀誤差為bg，加速計(jì)誤差為ba，則狀態(tài)量x表示為：

Shin和El-Sheimy[19]在2004年提出了IMU的動(dòng)力學(xué)和偏差的表示方法，本文是對(duì)其表示方法的改進(jìn)，假設(shè)由地球自轉(zhuǎn)引起的偏差對(duì)陀螺儀測(cè)量數(shù)據(jù)的影響忽略不計(jì)，則IMU動(dòng)力學(xué)方程與動(dòng)態(tài)誤差表示如下：

設(shè)狀態(tài)量x的誤差向量為δx，動(dòng)態(tài)誤差方程表達(dá)式為：

Fig.1 Block diagram of algorithm圖1 算法程序框圖

其中，G可以直接得到，F(xiàn)c表示如下：

其中，[?]×表示向量的叉乘反對(duì)稱矩陣。

Fig.2 Graphs of state variables and measurements圖2 狀態(tài)量和測(cè)量值的圖表示

3 視覺慣性SLAM算法

3.1 特征點(diǎn)匹配與關(guān)鍵幀的選擇

本文視覺慣性SLAM算法采用的是ORB[20]特征。ORB特征可以自主確定描述子方向，但是通過提取沿著重力方向定向的描述子可以得到更好的匹配結(jié)果[9]，而本文算法由于融合了IMU的信息，重力方向可以直接得到。

通過當(dāng)前IMU測(cè)量值和上一幀圖像幀的狀態(tài)預(yù)測(cè)當(dāng)前幀的狀態(tài)，根據(jù)預(yù)測(cè)的當(dāng)前幀位姿篩選局部地圖中當(dāng)前幀可見的特征點(diǎn)，這些可見特征點(diǎn)與當(dāng)前幀的特征點(diǎn)進(jìn)行暴力匹配（brute-force matching）[20]。對(duì)于當(dāng)前幀的每個(gè)特征點(diǎn)，計(jì)算和當(dāng)前幀下所有可見的特征點(diǎn)之間的距離，本文在最近鄰點(diǎn)和第二近鄰點(diǎn)引入一個(gè)比率，因?yàn)榈诙忺c(diǎn)的距離肯定大于最近鄰點(diǎn)，因此一個(gè)適當(dāng)?shù)拈撝悼梢杂行У乜刂普_與錯(cuò)誤匹配之間的比例。隨后排除誤匹配，排除誤匹配分為兩步：首先，根據(jù)位姿預(yù)測(cè)計(jì)算馬氏距離，選取合適的閾值判斷匹配是否有效；然后根據(jù)RANSAC算法排除誤匹配，從而得到相機(jī)位姿。

為了接下來的優(yōu)化處理過程，選取某一時(shí)刻拍攝的圖像幀，這些圖像幀共享的特征點(diǎn)保存到局部地圖里，把這些圖像分為兩類：把當(dāng)前S幀圖像作為臨時(shí)優(yōu)化窗口，S幀之前選取N幀關(guān)鍵幀。關(guān)鍵幀的選取采用較簡(jiǎn)單的方法，即如果匹配特征點(diǎn)所占的圖像區(qū)域與所有檢測(cè)到特征點(diǎn)的圖像區(qū)域的比率低于50%，就稱其為關(guān)鍵幀。

3.2 跟蹤與局部?jī)?yōu)化

在視覺SLAM算法中，最小化二次投影誤差得到相機(jī)位姿和特征點(diǎn)的坐標(biāo)。圖2是狀態(tài)量和測(cè)量值的圖表示。本文引入IMU數(shù)據(jù)，不僅在連續(xù)位姿之間產(chǎn)生約束，而且對(duì)加速計(jì)和陀螺儀的速度和偏差產(chǎn)生影響。對(duì)于視覺慣性SLAM，最小化二次投影誤差的同時(shí)加入IMU誤差項(xiàng)得到的誤差函數(shù)E(x)[21]表示如下：

其中，i為立體相機(jī)檢索號(hào)；k為相機(jī)圖像幀；j為圖像中的特征點(diǎn)；J(i,k)表示相機(jī)i中圖像k的特征點(diǎn)；Wri,j,k表示特征點(diǎn)的信息矩陣；Wsk表示第k幀圖像的IMU誤差信息矩陣。

Furgale[22]于2011年提出了基于星體運(yùn)動(dòng)的投影誤差方程，本文將其應(yīng)用到視覺SLAM算法之中，表示如下：

其中，hi(?)表示相機(jī)投影模型；為相機(jī)坐標(biāo)的測(cè)量值。

對(duì)于在tk和tk+1兩個(gè)時(shí)刻的連續(xù)幀狀態(tài)xk和，是通過IMU的測(cè)量數(shù)據(jù)zks聯(lián)系的，IMU誤差表示如下：

3.3 邊緣化

隨著時(shí)間的推移，特征點(diǎn)和相機(jī)位姿越來越多，優(yōu)化的計(jì)算量隨著變量的增加而增加，圖像的邊緣化就是為了限制計(jì)算量的增加，保持優(yōu)化計(jì)算量在一個(gè)恒定的范圍。本文中在圖優(yōu)化的同時(shí)進(jìn)行圖像幀的邊緣化處理。SLAM算法中的圖優(yōu)化[23]為最小二乘問題，最小二乘問題可以通過高斯牛頓迭代求解，表示如下：

其中，H為信息矩陣。設(shè)xμ為需要邊緣化的狀態(tài)量，xλ為需要保留的狀態(tài)量，則上述方程可以轉(zhuǎn)化為：

由舒爾補(bǔ)碼[24]（Schur complement）得到：

將邊緣化引入到基于視覺慣性的SLAM算法中，最初的邊緣化誤差項(xiàng)是由S個(gè)圖像幀組成的xTk，k=1,2,…,S，S幀圖像臨時(shí)優(yōu)化窗口之前為N幀關(guān)鍵幀。當(dāng)一個(gè)新的圖像幀xTc插入到臨時(shí)優(yōu)化窗口中時(shí)，算法進(jìn)行邊緣化環(huán)節(jié)，如果臨時(shí)優(yōu)化窗口中最后一個(gè)圖像幀(xTc-s)為非關(guān)鍵幀，則拋棄其所有特征點(diǎn)測(cè)量數(shù)據(jù)并將其邊緣化；如果xTc-s為關(guān)鍵幀，直接丟棄所有特征點(diǎn)的測(cè)量信息會(huì)造成不利影響，導(dǎo)致基于兩個(gè)關(guān)鍵幀之間的相關(guān)位姿信息丟失。因此，在這種情況下，僅邊緣化N幀關(guān)鍵幀中最后關(guān)鍵幀可見而其他關(guān)鍵幀中不可見的特征點(diǎn)信息，而屬于不需要邊緣化的關(guān)鍵幀的特征點(diǎn)信息則保留。

3.4 閉環(huán)檢測(cè)

上述部分即為視覺慣性里程計(jì)，雖然可以得到地圖估計(jì)，但是由于邊緣化和局部?jī)?yōu)化，算法無法檢測(cè)閉環(huán)和地圖重利用，系統(tǒng)不可避免地出現(xiàn)誤差累積和漂移現(xiàn)象。采用添加并行的全局地圖來達(dá)到檢測(cè)閉環(huán)和減少漂移的目的。并行的全局地圖為Essential Graph[25]形式，Essential Graph即為關(guān)鍵幀位姿以及關(guān)鍵幀之間的轉(zhuǎn)換關(guān)系組成的無向圖。

在本文中，算法實(shí)時(shí)跟蹤相機(jī)訪問的位置，并使用圖像檢索技術(shù)檢測(cè)是否重新訪問相同的位置。當(dāng)關(guān)鍵幀從視覺慣性SLAM局部?jī)?yōu)化地圖中離開時(shí)，在全局地圖中與之前臨近關(guān)鍵幀通過已估計(jì)過的位姿進(jìn)行連接，通過DBOW2[26]檢索策略在全局地圖中檢索與其形成匹配的關(guān)鍵幀，然后通過2D-to-3D RANSAC[27]算法估計(jì)和驗(yàn)證關(guān)鍵幀之間正確的位姿關(guān)系，并增加空間約束，最后通過圖優(yōu)化來優(yōu)化處理，以此來檢測(cè)閉環(huán)和減少累計(jì)漂移。下面詳細(xì)敘述閉環(huán)檢測(cè)和圖優(yōu)化。

（1）閉環(huán)檢測(cè)

本文閉環(huán)檢測(cè)分為兩步：首先，當(dāng)關(guān)鍵幀從局部?jī)?yōu)化地圖中邊緣化后作為查詢關(guān)鍵幀進(jìn)入全局地圖，從全局地圖中的關(guān)鍵幀檢索與查詢關(guān)鍵幀相匹配的關(guān)鍵幀，這種檢索匹配是通過DBOW2完成的，形成匹配之后，在全局地圖中增加一條邊的約束；然后在相互匹配的關(guān)鍵幀之間通過2D-to-3D匹配運(yùn)用RANSAC算法來得到匹配關(guān)鍵幀之間的轉(zhuǎn)換位姿，這也可以從空間角度驗(yàn)證由DBOW2得到的匹配關(guān)鍵幀是否正確。

（2）圖優(yōu)化

圖優(yōu)化主要優(yōu)化關(guān)鍵幀之間的兩種空間約束：由局部?jī)?yōu)化得到的相鄰關(guān)鍵幀之間的位姿轉(zhuǎn)換和由閉環(huán)檢測(cè)找到的全局地圖中相匹配的關(guān)鍵幀之間的位姿轉(zhuǎn)換。其中后者是由RANSAC算法2D-to-3D匹配得到的，由此可以通過優(yōu)化以下的目標(biāo)函數(shù)來得到狀態(tài)量：

其中，式（13）右端包含兩個(gè)誤差項(xiàng)，第一項(xiàng)為視覺慣性里程計(jì)殘差，是由視覺慣性里程計(jì)各個(gè)關(guān)鍵幀之間的相對(duì)位姿得到的；第二項(xiàng)為閉環(huán)殘差，是由閉環(huán)檢測(cè)各關(guān)鍵幀之間的閉環(huán)約束得到的。ξk,k+1為視覺慣性里程計(jì)得到的第k和第k+1關(guān)鍵幀之間的相對(duì)轉(zhuǎn)換位姿，ξk,k′為閉環(huán)檢測(cè)里k和k′幀之間的相對(duì)轉(zhuǎn)換位姿。ek,k+1和ek,k′表示如下:

其中，L為閉環(huán)檢測(cè)關(guān)鍵幀的匹配，即為形成的閉環(huán)約束。ek,k+1和ek,k′分別為位姿跟蹤和閉環(huán)檢測(cè)得到的位姿的殘差。運(yùn)算符號(hào)⊕表示如下：

其中，ξ(T)和T(ξ)為位姿參數(shù)ξ和T之間的轉(zhuǎn)換函數(shù)，T∈SE(3)，SE(3)為三維歐氏群，物理意義為位姿。

信息矩陣Wk,k+1和Wk,k′為位姿空間約束的逆協(xié)方差矩陣。這個(gè)非線性最小二乘問題通過萊文貝格-馬夸特（Levenberg-Marquardt，LM）算法[28]進(jìn)行優(yōu)化求解。

通過并行全局地圖構(gòu)建，可以得到形成閉環(huán)的關(guān)鍵幀，進(jìn)而可以得到地圖中匹配關(guān)鍵幀之間的位姿。全局地圖中得到的位姿還可以優(yōu)化和改善局部地圖中得到的位姿。同時(shí)，通過全局優(yōu)化，可以減小全局誤差，從而得到更加完善的地圖。

3.5 重定位

重定位，即為機(jī)器人在自身位置未知的情況下在已有的地圖上進(jìn)行定位的算法。在視覺SLAM中，因?yàn)檎趽?、非紋理區(qū)域或相機(jī)快速移動(dòng)所造成的跟蹤失敗非常常見，重定位可以在跟蹤失敗后進(jìn)行位姿恢復(fù)。在機(jī)器人和增強(qiáng)現(xiàn)實(shí)等實(shí)際的領(lǐng)域內(nèi)，SLAM系統(tǒng)要允許在之前構(gòu)建的地圖中進(jìn)行重定位，并且可以無縫地在新增加的環(huán)境部分中繼續(xù)構(gòu)建地圖。重定位和繼續(xù)SLAM算法的難點(diǎn)在于檢測(cè)重定位和新地圖與已構(gòu)建的地圖的結(jié)合，從而實(shí)現(xiàn)全局地圖框架內(nèi)的位姿跟蹤，見圖3，藍(lán)色為已構(gòu)建的地圖，紅色為新的SLAM地圖，綠色點(diǎn)和線段表示兩地圖之間關(guān)鍵幀的對(duì)應(yīng)。

本文在新的軌跡ξnew和已構(gòu)建軌跡ξprev之間找到重疊部分，對(duì)于新的SLAM地圖中的每一個(gè)新的關(guān)鍵幀，通過上一部分提到的閉環(huán)檢測(cè)算法檢測(cè)并驗(yàn)證與已構(gòu)建SLAM地圖中關(guān)鍵幀的對(duì)齊情況。建立一個(gè)閉環(huán)假設(shè)集H=(h1,h2,…,hn)，其中h表示新舊地圖關(guān)鍵幀之間的聯(lián)系。

Fig.3 Relocalization stages in previous SLAM keyframe map圖3 在已構(gòu)建地圖中的重定位階段

如果新的SLAM地圖中的關(guān)鍵幀knew和之前構(gòu)建的地圖中的關(guān)鍵幀kprev形成匹配，則在假設(shè)集里添加一個(gè)新的假設(shè)h=(knew,kprev,ξkprev,knew)。如果關(guān)鍵幀對(duì)(knew,kprev) 和 (k′new,k′prev) 具有相同的時(shí)間序列，即||k′new-knew||=1并且k′new-knew=k′prev-kprev，通過將匹配對(duì)組合在單個(gè)假設(shè)集合內(nèi)來合并假設(shè)。

一旦某一假設(shè)擁有足夠數(shù)目的連續(xù)匹配對(duì)（本文選取4對(duì)），根據(jù)最近的匹配關(guān)鍵幀定義位姿轉(zhuǎn)換，通過將新的SLAM地圖中的所有關(guān)鍵幀與之前構(gòu)建的地圖中的圖像幀建立位姿轉(zhuǎn)換關(guān)系，從而把新的SLAM地圖和已構(gòu)建的SLAM地圖結(jié)合起來。優(yōu)化的目標(biāo)函數(shù)如下所示：

其中，Es(ξprev)為已構(gòu)建地圖關(guān)鍵幀之間的誤差項(xiàng)；Es(ξnew)為新的地圖關(guān)鍵幀之間的誤差項(xiàng)；最后一項(xiàng)為新舊地圖之間約束的誤差項(xiàng)。ξ為已建地圖關(guān)鍵幀和新的地圖關(guān)鍵幀之間的位姿轉(zhuǎn)換，Cmaps為已建地圖和新的地圖之間關(guān)鍵幀的匹配，這種新舊關(guān)鍵幀之間的匹配約束類似于閉環(huán)機(jī)制，可以提供軌跡之間的約束，從而改善新舊軌跡之間的對(duì)齊情況，減小新構(gòu)建地圖的誤差。

4 實(shí)驗(yàn)

為了驗(yàn)證本文算法的效果，選取了公用數(shù)據(jù)集EuRoC[29]。EuRoC 數(shù)據(jù)集包含“Machine Hall 01”“Vicon Room 1 01”“Vicon Room 1 02”等11個(gè)數(shù)據(jù)集序列，是由微型飛行器拍攝的視覺慣性數(shù)據(jù)集序列。該數(shù)據(jù)集在制作時(shí)包含一個(gè)視覺慣性傳感器單元，可以實(shí)時(shí)記錄陀螺儀和加速計(jì)數(shù)據(jù)，其中立體相機(jī)頻率為20 Hz，慣性傳感器頻率為200 Hz。數(shù)據(jù)集中還包括相機(jī)內(nèi)參、外參以及實(shí)際運(yùn)動(dòng)軌跡等數(shù)據(jù)。實(shí)驗(yàn)中運(yùn)行算法的處理器為Intel 4核處理器i7-3700 3.4 GHz。

4.1 視覺慣性SLAM算法

本文首先在“Machine Hall 01”數(shù)據(jù)集序列上與Okvis[10]算法進(jìn)行比較，數(shù)據(jù)集圖片大小為752×480，ORB特征提取數(shù)目設(shè)置為1 000個(gè)，DBOW2詞袋樹模型為6層。為了便于觀察，驗(yàn)證實(shí)驗(yàn)結(jié)果，采用Python工具將軌跡投影到x-y平面后進(jìn)行對(duì)比，如圖4（a）（b）所示，與Okvis算法相比，本文的算法與真實(shí)軌跡更加切合，特別是在形成閉環(huán)的軌跡處，可以通過檢測(cè)閉環(huán)，增加閉環(huán)約束以減小誤差。此外全局地圖的存在使得算法可以進(jìn)行全局優(yōu)化，從而可以從全局方面減小算法的誤差。使算法軌跡與真實(shí)軌跡更加吻合，得到更好的定位效果。誤差統(tǒng)計(jì)如表1所示，可以看出，本文算法較Okvis算法軌跡的各項(xiàng)誤差數(shù)據(jù)均有所降低。實(shí)驗(yàn)表明本文算法通過閉環(huán)檢測(cè)和全局優(yōu)化可以減小誤差累積和漂移。本文算法可以得到通過稀疏點(diǎn)構(gòu)建的場(chǎng)景地圖，圖5為“Vicon Room 1 02”序列的地圖估計(jì)，圖中黑色點(diǎn)為正確匹配的特征點(diǎn)，紅色點(diǎn)為誤匹配特征點(diǎn)，藍(lán)色為相機(jī)位姿，綠色線段為關(guān)鍵幀之間的連接。

Fig.4 Trajectory comparison圖4 軌跡對(duì)比圖

Table 1 Comparison of trajectory errors表1 軌跡誤差對(duì)比圖 m

Fig.5 Estimated map圖5 地圖估計(jì)

為了觀察誤差隨軌跡變化的趨勢(shì)，本文在“Vicon Room 1 01”“Vicon Room 1 02”這兩個(gè)數(shù)據(jù)集序列上分別驗(yàn)證本文算法與Okvis[10]算法以及VIORB[3]算法的效果。如圖6、圖7所示，其中Translation error為算法平移誤差，即為算法估計(jì)值與真實(shí)軌跡在平移量的差值，Orientation error為方向誤差，為在旋轉(zhuǎn)量之間的差值。通過對(duì)比可以看出本文算法由于采用了閉環(huán)檢測(cè)和全局優(yōu)化，使得軌跡的誤差較小，并且由于閉環(huán)約束的存在，使得誤差不會(huì)隨著路徑的增加而明顯增加，從而達(dá)到減少誤差累積和漂移的目的。

Fig.6 Vicon Room 1 01 dataset圖6 Vicon Room 1 01數(shù)據(jù)集

4.2 重定位

本節(jié)檢測(cè)算法的重定位能力。圖8中的直方圖顯示的是重定位所需要的關(guān)鍵幀的數(shù)目，這是在單一假設(shè)所含連續(xù)匹配對(duì)數(shù)為4時(shí)的實(shí)驗(yàn)結(jié)果?？梢钥闯觯?dāng)使用長(zhǎng)度為4的匹配對(duì)集合作為重定位假設(shè)時(shí)，算法從初始化開始可以在8個(gè)關(guān)鍵幀之內(nèi)完成重定位。此外，根據(jù)重定位結(jié)果與真實(shí)軌跡的比較，可以得到重定位的定位誤差，圖9顯示在大約20幀圖像之后，誤差變得較小且比較穩(wěn)定，即為找到了比較精確的重定位。重定位之后，本文的SLAM算法包括新軌跡和已構(gòu)建的軌跡之間的閉環(huán)，從而可以改善SLAM地圖的準(zhǔn)確性。

Fig.7 Vicon Room 1 02 dataset圖7 Vicon Room 1 02數(shù)據(jù)集

Fig.8 Number of keyframes needed to be relocalized within same sequence圖8 同一序列重定位所需的關(guān)鍵幀

Fig.9 Progression of error after relocalization圖9 重定位后誤差的變化

本文選取了不同的“Vicon Room”序列，驗(yàn)證了算法。首先運(yùn)行一個(gè)數(shù)據(jù)集序列得到SLAM地圖，然后在已得到的地圖基礎(chǔ)上再運(yùn)行另一序列進(jìn)行重定位與繼續(xù)建圖，由此根據(jù)聯(lián)合序列“Vicon Room”得到算法的誤差。如表2和表3顯示，由聯(lián)合序列得到的誤差和由單一序列得到的誤差非常接近，后兩組實(shí)驗(yàn)誤差還有所改善。

Table 2 Error of algorithm in different sequences表2 不同序列下算法的誤差

Table 3 Error of algorithm in one sequence表3 單一序列算法的誤差

4.3 運(yùn)行時(shí)間

本文算法實(shí)現(xiàn)了數(shù)據(jù)集上相機(jī)的實(shí)時(shí)跟蹤，表4顯示的是算法在EuRoC數(shù)據(jù)集的平均運(yùn)行時(shí)間，算法運(yùn)行時(shí)間包含里程計(jì)運(yùn)行時(shí)間、閉環(huán)檢測(cè)時(shí)間以及圖優(yōu)化時(shí)間。可以看出，盡管SLAM算法的并行處理模塊閉環(huán)檢測(cè)和圖優(yōu)化以較慢的速度更新，但是視覺里程計(jì)可以以13 ms左右的時(shí)間處理圖像幀，而數(shù)據(jù)集相機(jī)的采集頻率為20 Hz，因此視覺里程計(jì)可以隨著圖像幀的更新實(shí)時(shí)地處理圖像，從而使得算法可以實(shí)現(xiàn)數(shù)據(jù)集上相機(jī)的實(shí)時(shí)跟蹤。

Table 4 Run-time of proposed method表4 算法運(yùn)行時(shí)間

5 結(jié)束語

針對(duì)目前視覺慣性SLAM算法因缺少閉環(huán)檢測(cè)，導(dǎo)致算法累積誤差和漂移比較嚴(yán)重的問題，本文提出了一種基于關(guān)鍵幀技術(shù)的視覺慣性SLAM算法。在邊緣化圖像幀和關(guān)鍵幀之后引入了閉環(huán)檢測(cè)和非線性優(yōu)化，建立了一個(gè)并行的全局地圖，算法對(duì)局部連續(xù)軌跡和全局連續(xù)軌跡并行進(jìn)行估計(jì)，并可在已構(gòu)建地圖進(jìn)行重定位和在保持地圖連續(xù)性的情況下繼續(xù)建圖。實(shí)驗(yàn)表明，本文提出的方法可以使得算法在減小誤差和漂移方面取得很大的改善，并可以重定位相機(jī)位置和繼續(xù)構(gòu)建未知環(huán)境地圖。