盧佳偉，許哲

(上海海洋大學(xué)工程學(xué)院，上海 201306)

0 引言

結(jié)合視覺和慣性測量進(jìn)行同步定位和地圖構(gòu)建，在智能機(jī)器人和計算機(jī)視覺領(lǐng)域已經(jīng)引起廣泛關(guān)注.通過引入慣性測量單元（IMU）的信息可以很好地彌補(bǔ)單目視覺即時定位與地圖構(gòu)建（SLAM）算法本身存在的一些缺陷.一方面將IMU和相機(jī)估計的位姿序列對齊可以得到相機(jī)軌跡的真實(shí)尺度，而且IMU可以很好地預(yù)測出圖像幀的位姿，提高特征跟蹤算法的匹配速度和應(yīng)對快速旋轉(zhuǎn)的算法魯棒性.另一方面IMU中加速度計提供的重力向量可以將估計的位姿轉(zhuǎn)化為實(shí)際導(dǎo)航需要的世界坐標(biāo)系中.

根據(jù)相機(jī)和IMU數(shù)據(jù)融合框架的不同，視覺慣性里程計（VIO）可分為緊耦合和松耦合[1].松耦合直接將VIO和慣性導(dǎo)航的輸出結(jié)果進(jìn)行融合.典型的方法是S.Weiss等[2-3]提出的基于擴(kuò)展卡爾曼濾波的SSF、MSF算法，將視覺里程計（VO）處理完的數(shù)據(jù)與IMU數(shù)據(jù)進(jìn)行融合.這兩種算法雖然能實(shí)時獲得航行器的位姿，但是無法糾正視覺測量引入的漂移，所以定位精度沒有緊耦合框架的高.而在緊耦合中是使用兩個傳感器的原始數(shù)據(jù)共同估計一組變量，傳感器的噪聲也是相互影響的.雖然緊耦合的算法比較復(fù)雜，但充分利用了傳感器數(shù)據(jù)，是目前研究的重點(diǎn).Mourikis等[4]于2007年提出了一種基于濾波的VIO算法模型多狀態(tài)約束卡爾曼濾波（MSCKF）.MSCKF利用一個特征點(diǎn)同時約束多個相機(jī)位姿，進(jìn)行卡爾曼濾波更新，但是在跟蹤情況下沒有使用IMU的先驗(yàn)數(shù)據(jù)，且無法用到全局的信息進(jìn)行全局優(yōu)化，因此系統(tǒng)極易不收斂，導(dǎo)致定位精度較差.Stefan Leutenegge等[5]于2015年提出基于雙目相機(jī)和IMU的OKVIS算法.該算法通過IMU測量值對當(dāng)前狀態(tài)做預(yù)測，根據(jù)預(yù)測進(jìn)行特征提取與匹配，并將優(yōu)化中的重投影和IMU的測量誤差放在一起做優(yōu)化.但是由于OKVIS不支持重定位，也沒有閉環(huán)檢測和校正方案，在機(jī)器人劇烈運(yùn)動時容易跟蹤失敗.香港科技大學(xué)沈劭劼團(tuán)隊(duì)[6]于2017年開源了一套Visual-Inertial融合定位算法VINS-MOMO，該算法以KTL光流跟蹤作為前端，而且具有比OKVIS更加完善和魯棒的初始化以及閉環(huán)檢測過程.但是該算法不容易構(gòu)建全局地圖，且視覺約束只靠滑動窗口里的關(guān)鍵幀.

針對上述問題，本文采用將IMU與視覺SLAM兩種定位方案在基于非線性的緊耦合結(jié)構(gòu)下相互融合，構(gòu)建具有較高定位精度的定位系統(tǒng)[7-8].采用VINS-MONO算法，使用單目相機(jī)作為視覺傳感器與IMU進(jìn)行組合定位與建圖.但是在前端中選用快速特征點(diǎn)提取和描述的算法（ORB）[9]特征，保證了特征子具有旋轉(zhuǎn)，尺度不變性的同時，提取特征點(diǎn)的速度與匹配效率明顯提升，從而能夠及時的擴(kuò)展場景，構(gòu)建全局地圖.

1 視覺里程計

在經(jīng)典的視覺SLAM框架中，VO是通過相鄰圖像之間匹配的特征點(diǎn)估算出相機(jī)的運(yùn)動并構(gòu)建局部場景的地圖.VO又稱前端（Front End），它通過匹配好的特征點(diǎn)計算出相鄰圖像之間的運(yùn)動，得到機(jī)器人的運(yùn)動軌跡，從而解決了定位問題.另一方面，根據(jù)每個時刻的相機(jī)位置，計算出各像素對應(yīng)空間點(diǎn)的位置，就得到了地圖.VO的算法流程如圖1所示.

圖1 VO算法流程圖

1.1 圖像特征點(diǎn)提取與匹配

在圖像特征中，尺度不變特征轉(zhuǎn)換(SIFT)[10]是考慮得比較完善的一種.它能夠適應(yīng)機(jī)器人運(yùn)動過程中出現(xiàn)的光照及旋轉(zhuǎn)等變化，但是會增加大量的計算.另一些特征，考慮適當(dāng)?shù)亟档途群汪敯粜?，提升計算速?例如FAST關(guān)鍵點(diǎn)屬于計算特別快的一種特征點(diǎn).而ORB特征可以用來對圖像中的關(guān)鍵點(diǎn)快速創(chuàng)建特征向量，這些特征向量可以用來識別圖像中的對象.它在FAST檢測子不具有方向性的問題上做了改進(jìn)，并采用速度極快的二進(jìn)制描述子二元魯棒獨(dú)立基本特征（BRIEF），使整個圖像特征提取環(huán)節(jié)大大加速.提取ORB特征分為兩個步驟：

1）查找圖像中的關(guān)鍵點(diǎn)，并使用FAST算法對關(guān)鍵點(diǎn)進(jìn)行檢測；

2）使用BRIEF算法將關(guān)鍵點(diǎn)變成特征向量.

特征匹配是通過計算特征向量之間的距離來確定路標(biāo)點(diǎn)前后時間的對應(yīng)關(guān)系.當(dāng)特征點(diǎn)數(shù)量較小時，可使用暴力匹配算法，它計算某一個特征點(diǎn)的特征向量與其他所有特征點(diǎn)的特征向量之間的距離，然后取向量距離最小的一個作為匹配點(diǎn).但特征點(diǎn)多時，暴力匹配的運(yùn)算量變大，會影響算法的實(shí)時性要求.因此，選用快速近似最近鄰（FLANN）算法[11]更加適合于匹配點(diǎn)數(shù)量極多的情況.該算法理論已集成到OpenCV，使用時可直接從中調(diào)用.

1.2 單目相機(jī)運(yùn)動估計

通過特征提取與匹配，可從兩張圖像中得到一對匹配好的特征點(diǎn)，如圖2所示.相鄰的圖像存在很多對匹配點(diǎn)，可通過它們之間的對應(yīng)關(guān)系得到相機(jī)的運(yùn)動.

圖2 對極幾何約束

在第一幀的坐標(biāo)系下，設(shè)P點(diǎn)的空間位置為：

由針孔相機(jī)模型推出兩個像素點(diǎn)p1,p2的像素位置為：

式中：K為相機(jī)內(nèi)參矩陣；R、t為兩個坐標(biāo)系下相機(jī)的旋轉(zhuǎn)與平移運(yùn)動.

兩個像素點(diǎn)在歸一化平面上的坐標(biāo)x1,x2為：

對式（3）進(jìn)行一系列變換并帶入p1,p2得：

可將上式（4）中間部分記作兩個矩陣：基礎(chǔ)矩陣F和本質(zhì)矩陣E，帶入（4）式得：

相機(jī)的位置估計問題變?yōu)橐韵聝刹剑?/p>

1）根據(jù)配對點(diǎn)的像素位置，求出E或者F；

2）根據(jù)E或者F，求出R、t.

2 IMU傳感器模型構(gòu)建

一般而言，IMU內(nèi)部包含加速度計和陀螺儀，分別測量機(jī)器人的加速度信息和角速度信息.

忽略尺度因子的影響，只考慮白噪聲和隨機(jī)游走偏差，VIO中IMU模型為：

式中：上標(biāo)b為IMU坐標(biāo)系，w為世界坐標(biāo)系world；上標(biāo)a為加速度計，g為陀螺儀；ωb和ab分別為IMU輸出的角速度和加速度；為IMU真實(shí)的角速度和加速度數(shù)據(jù)；mg和ma為陀螺儀和加速度計的零偏；ng和na為陀螺儀和加速度計的高斯白噪聲；gw為世界坐標(biāo)系下的重力加速度；qbw為世界坐標(biāo)系向IMU坐標(biāo)系下的旋轉(zhuǎn)矩陣.

IMU的位移,速度,旋轉(zhuǎn)對時間的導(dǎo)數(shù)為：

根據(jù)式（10）、（11）、（12）導(dǎo)數(shù)關(guān)系，使用中值法得到運(yùn)動模型的離散積分，即兩個相鄰時刻k到k+1的位姿用兩個時刻的測量值ω，a的平均值來計算：

其中：

3 VIO緊耦合優(yōu)化器設(shè)計

融合IMU是為了克服純視覺SLAM的一些缺點(diǎn).視覺受光線條件影響和限制，且在紋理豐富的場景中可以正常工作，然而遇到玻璃、白墻等特征稀少的場景時就無法正常工作.純視覺還難以處理動態(tài)場景，環(huán)境變化時，會誤認(rèn)為自己在運(yùn)動，而IMU能夠感受到自己的運(yùn)動，避免運(yùn)動誤判.純視覺也難以處理運(yùn)動過快的場景，兩幀圖像之間的重疊區(qū)域太少甚至沒有，造成無法基于兩幀圖像中相同點(diǎn)和不同點(diǎn)測算運(yùn)動，而IMU可以補(bǔ)充發(fā)揮作用，提供可靠的位姿估算.

VINS系統(tǒng)主要包括3部分.如圖3所示：前端進(jìn)行數(shù)據(jù)預(yù)處理，包括特征提取匹配和IMU預(yù)積分；初始化是對系統(tǒng)初始狀態(tài)變量（重力方向、速度、尺度等等）進(jìn)行處理；后端通過滑動窗口算法得到最優(yōu)狀態(tài)估計.

圖3 VINS系統(tǒng)框架

3.1 測量數(shù)據(jù)預(yù)處理

特征提取與跟蹤在視覺里程計部分已進(jìn)行詳細(xì)討論.接下來主要是對IMU數(shù)據(jù)進(jìn)行預(yù)積分.可對IMU數(shù)據(jù)進(jìn)行積分得到從i時刻到j(luò)時刻的位移、旋轉(zhuǎn)和速度.但是每次優(yōu)化更新后，都需要重新進(jìn)行積分，導(dǎo)致運(yùn)算量較大.此時，可用一個簡單的公式轉(zhuǎn)換，將積分模型轉(zhuǎn)化為預(yù)積分模型：

通過式（18），位移、速度、旋轉(zhuǎn)積分公式中的積分項(xiàng)則變成相對于第i時刻的姿態(tài)，而不是相對于世界坐標(biāo)系的姿態(tài).此時，位移、速度、旋轉(zhuǎn)積分公式為：

式(19)、(20)、(21)中的積分項(xiàng)僅跟IMU測量值有關(guān)，它將一段時間內(nèi)的IMU數(shù)據(jù)直接積分起來就得到了預(yù)積分量：

3.2 VINS系統(tǒng)初始化

單目緊耦合VIO是一個非線性的系統(tǒng).VIO無法直接觀測到尺度，如果沒有良好的初始值，很難直接將相機(jī)和IMU的數(shù)據(jù)融合在一起.可以假設(shè)一個靜止的初始條件來啟動單目VINS估計器.然而，這種假設(shè)是不合適的，因?yàn)樵趯?shí)際應(yīng)用中經(jīng)常會遇到運(yùn)動下的初始化.當(dāng)IMU測量結(jié)果被大偏置破壞時，情況就變得更加復(fù)雜了.事實(shí)上，初始化通常是單目VINS最脆弱的步驟.需要一個魯棒的初始化過程以確保系統(tǒng)的適用性.

初始化過程中，首先要建立視覺坐標(biāo)系和IMU坐標(biāo)系的聯(lián)系.假設(shè)相機(jī)和IMU之間有粗略測量的外部參數(shù)：旋轉(zhuǎn)qbc、位移pbc，我們可以將姿態(tài)從IMU坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系：

由于IMU采集數(shù)據(jù)的頻率大于相機(jī)的，所以初始化過程中需將IMU預(yù)積分?jǐn)?shù)據(jù)與相機(jī)的位姿數(shù)據(jù)對齊.具體過程如下：

1)旋轉(zhuǎn)外參數(shù)qbc未知,則先估計旋轉(zhuǎn)外參數(shù).

2)利用旋轉(zhuǎn)約束估計陀螺儀偏離.

3)利用平移約束估計重力方向、速度、以及尺度初始值.

5)求解相機(jī)坐標(biāo)系與世界坐標(biāo)系之間的旋轉(zhuǎn)矩陣，并將軌跡對齊到世界坐標(biāo)系.

3.3 滑動窗口優(yōu)化

初始化完成后，VINS系統(tǒng)采用基于滑動窗口的緊耦合單目VIO進(jìn)行高精度和魯棒的狀態(tài)估計.

VINS系統(tǒng)優(yōu)化的狀態(tài)變量為：

式中：xk表示第k幀圖像時的IMU的位姿；n是關(guān)鍵幀的總數(shù)；m是滑動窗口中的特征總數(shù)；λi為第一次觀測到第i個特征的逆深度.

通過最小化滑動窗口中的殘差項(xiàng)來估計系統(tǒng)的狀態(tài)變量：

式中：rB和rC分別是IMU和視覺測量的殘差；B是所有IMU數(shù)據(jù)的集合；C是在當(dāng)前滑動窗口中至少觀測到兩次的一組特征；（rP，JP）是來自邊緣化的先驗(yàn)信息；ρ為Huber范數(shù).

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

本文中實(shí)驗(yàn)所用電腦為Inter Core i5-4210記本，內(nèi)存12 GB，系統(tǒng)為Ubuntu16.04，ROS版本為kinetic.

實(shí)驗(yàn)1的算法驗(yàn)證是基于圖3所示的VINS系統(tǒng)框架（無回環(huán)檢測）.通過數(shù)據(jù)文本構(gòu)建仿真場景，如圖4所示該場景是由三個長方體堆疊組成，長、寬、高分別為：10×10×3、7×7×3、4×4×4.然后由仿真場景生成視覺特征的仿真數(shù)據(jù)集，采樣頻率為30 Hz，其中特征點(diǎn)的3D坐標(biāo)如圖4所示為長方體的頂點(diǎn)和附近的點(diǎn)，共48個.IMU的仿真數(shù)據(jù)集是由在x、y平面內(nèi)做橢圓運(yùn)動，z軸做正弦運(yùn)動的曲線生成，采樣頻率為180Hz.最后通過配置不同陀螺儀和加速度計噪聲的大小，驗(yàn)證該算法的有效性，其中圖像的像素誤差給定為1.5.

圖4 仿真場景

將已經(jīng)準(zhǔn)備好的仿真數(shù)據(jù)集（視覺特征，IMU數(shù)據(jù)）接入VINS代碼，并運(yùn)行出軌跡結(jié)果.具體實(shí)驗(yàn)步驟為：

1)IMU仿真數(shù)據(jù)集無噪聲；

2)IMU仿真數(shù)據(jù)集有噪聲,具體噪聲參數(shù)的設(shè)置如表1所示.表中g(shù)_bias、a_bias分別表示陀螺儀和加速度計的隨機(jī)游走偏差，g_noise、a_noise分別表示陀螺儀和加速度計的高斯白噪聲.

表1 仿真數(shù)據(jù)集噪聲參數(shù)

當(dāng)IMU仿真數(shù)據(jù)集無噪聲，如圖5所示，優(yōu)化軌跡imu_int和帶有噪聲的軌跡imu_int_noise重合，但由于存在像素誤差，它們與真實(shí)軌跡ground_truth并未重合.當(dāng)IMU仿真數(shù)據(jù)集有噪聲時，由表1所示，第三組數(shù)據(jù)各項(xiàng)參數(shù)大概是第二組的10倍，但如圖7所示，盡管imu_int_noise軌跡變化很大，最終優(yōu)化軌跡imu_int都能像圖6有有效的跟蹤真實(shí)軌跡ground_truth.

圖5 第1組實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)2采用的是EuRoC提供的V1_01_easy數(shù)據(jù)集，將本文方案與ORB-SLAM 2單目作比較.圖8和圖9分別為兩種算法的V1_01_easy軌跡平面圖，圖中的虛線表示真實(shí)軌跡，彩色線表示算法的定位軌跡，從這兩幅幅圖中，可以看出VINS算法定位軌跡在V1_01_easy數(shù)據(jù)集下與真實(shí)軌跡更為穩(wěn)合.圖10表示兩種算法在V1_01_easy數(shù)據(jù)集下的軌跡誤差，圖中可以明顯看出VINS_result_no_loop_V1.csv的定位誤差在整體上要優(yōu)于ORB-SLAM 2_1.txt單目的定位誤差.表2顯示了定位誤差的狀態(tài)對比，VINS-MONO定位誤差的最大值、最小值、標(biāo)準(zhǔn)差、均方根誤差均小于ORB-SLAM 2單目的定位誤差.圖11是定位誤差的小提琴圖，主要統(tǒng)計和分析數(shù)據(jù)的離散程度和分布密度，兩圖中左邊表示VINS_result_no_loop_V1.csv，右邊表示ORB-SLAM 2_1.txt.圖中可以看出VINS_result_no_loop_V1.csv誤差的分布集中在0.2m附近，而ORB-SLAM 2_1.txt分布集中在1m附近且分布范圍較廣；圖中VINS_result_no_loop_V1.csv誤差的中位數(shù)和四分位數(shù)都優(yōu)于ORBSLAM 2_1.txt單目；VINS_result_no_loop_V1.csv誤差的上側(cè)和下側(cè)的離散情況也要優(yōu)于ORBSLAM 2_1.txt單目.

圖6 第2組實(shí)驗(yàn)結(jié)果

圖7 第3組實(shí)驗(yàn)結(jié)果

圖8 VINS算法軌跡

圖9 單目ORB-SLAM算法軌跡

圖10 兩種算法軌跡誤差對比

表2 兩種算法在V1_01_easy數(shù)據(jù)集下誤差狀態(tài)對比 m

圖11 兩種算法定位誤差對比的小提琴圖

5 結(jié)束語

本文針對慣性傳感器積分時位姿容易發(fā)散，且機(jī)器人劇烈運(yùn)動時，VO定位容易失效等問題，提出了一種基于單目相機(jī)與IMU的機(jī)器人定位算法.該算法主要是利用VINS-MONO框架，但在前端進(jìn)行特征提取與匹配的時候，采用ORB特征，提高了新增關(guān)鍵點(diǎn)的特征匹配效率，更好地構(gòu)建場景地圖.在處理IMU數(shù)據(jù)時，利用中值法得到運(yùn)動模型的離散積分，能夠更加精確地定位機(jī)器人的位置.后端仍采用滑動窗口的非線性優(yōu)化得到機(jī)器人運(yùn)動的最優(yōu)狀態(tài)估計.

實(shí)驗(yàn)一在IMU仿真數(shù)據(jù)中添加噪聲進(jìn)行試驗(yàn)驗(yàn)證，該算法能夠有效地減小慣性傳感器的累積誤差.實(shí)驗(yàn)二利用V1_01_easy數(shù)據(jù)集驗(yàn)證了該算法優(yōu)于單目ORB-SLAM算法.但本文算法沒有閉環(huán)檢測模塊，在機(jī)器人長時間、大范圍的導(dǎo)航和定位中，單純依靠VINS系統(tǒng)的狀態(tài)估計會出現(xiàn)嚴(yán)重偏差.且該算法處理的數(shù)據(jù)量很大，實(shí)時性問題還存在問題，現(xiàn)只適用于室內(nèi)小范圍場景.因此下一步會將閉環(huán)檢測添加到該算法框架，考慮進(jìn)一度簡化該算法框架，達(dá)到實(shí)時性的要求.