基于在線時間延遲估計的雙目視覺慣性里程計

范涵奇,車金釗

(北方工業(yè)大學(xué)信息學(xué)院,北京 100144)

1 引言

視覺慣性里程計[1](Visual-inertial Odometry, VIO)是運動物體在未知環(huán)境中通過融合以相機為主的視覺傳感器與慣性測量單元(Inertial measurement unit, IMU)數(shù)據(jù)實現(xiàn)自身定位和導(dǎo)航的技術(shù),現(xiàn)主要應(yīng)用于無人機[2]、AR/VR[3]等領(lǐng)域。

在VIO前端需要在算法啟動階段執(zhí)行IMU初始化操作,期間陀螺儀的零偏往往難以收斂,出于該原因,將IMU信息與視覺信息采用聯(lián)合初始化的方案[4,5]有一定幾率會失敗。因此本文在初始化階段選擇采用靜止初始化的方式,Geneva等人[6]用靜止初始化的方式,通過計算一段時間內(nèi)加速度的方差大小來判斷系統(tǒng)是處于靜止還是運動狀態(tài),不同于他們劃分系統(tǒng)靜止與運動狀態(tài)的方式,本文選擇前n幀圖像的時間內(nèi)為系統(tǒng)靜止?fàn)顟B(tài)并使用幀間的IMU數(shù)據(jù)進(jìn)行初始化,從而快速完成初始化操作。

VIO在后端維護(hù)了一個高維的狀態(tài)向量,根據(jù)是否將視覺信息加入到狀態(tài)向量中可分為緊耦合與松耦合。由于松耦合將IMU與相機分別進(jìn)行自身的運動估計,在視覺定位困難的地方不夠魯棒[7],因此大都采用緊耦合的方式。而根據(jù)后端實現(xiàn)的方式, VIO的后端分為基于濾波和基于優(yōu)化[8]?；跒V波的緊耦合方案有MSCKF[9]、ROVIO[10]等,其主要優(yōu)勢在于降低了對計算的需求,但計算的精度卻不如基于優(yōu)化的方案。Forster等人[11]提出了IMU預(yù)積分理論,他們將兩幀間的多組IMU數(shù)據(jù)積分得到一組IMU數(shù)據(jù),隨后與視覺信息融合一起優(yōu)化,以達(dá)到減少優(yōu)化結(jié)點的目的,在保持精度的同時提升優(yōu)化的速度。

在IMU預(yù)積分理論中,要在預(yù)積分之前將圖像數(shù)據(jù)的時間軸與IMU數(shù)據(jù)的時間軸對齊,以得到兩圖像間準(zhǔn)確的IMU數(shù)據(jù)。但是在傳感器融合中由于存在著傳感器傳輸延遲、數(shù)據(jù)采樣延遲、時鐘不同步等問題而產(chǎn)生時間延遲(Temporal delay,td)[12,13],導(dǎo)致時間戳不能很好的對齊。為解決這個問題,Li和Mourikis[12]提出在線估計時間延遲的方法,他們將時間延遲作為擴展卡爾曼濾波中待估計的狀態(tài)變量。Cen等人[14]則認(rèn)為IMU數(shù)據(jù)相對于圖像數(shù)據(jù)存在著時間延遲,他們將時間延遲加入到根據(jù)IMU數(shù)據(jù)構(gòu)建的IMU誤差模型中,隨后在后端采用基于濾波的方式得到時間延遲量。但是他們計算結(jié)果的精度并不高。Qin和Shen[13]也采用在線的方式估計時間延遲,他們假設(shè)時間延遲是未知且恒定的并在優(yōu)化中估計它。

針對上述分析的問題,本文提出一種基于在線時間延遲估計的雙目視覺里程計,在IMU初始化階段采用靜止初始化的方式,在雙目間對關(guān)鍵點采用往返追蹤的策略構(gòu)造準(zhǔn)確的數(shù)據(jù)關(guān)聯(lián)?？紤]了傳感器上的時間延遲后,在后端優(yōu)化中,通過計算實際時刻圖像,將時間延遲作為待優(yōu)化變量加入到基于直接法的光度殘差中,并使用緊耦合優(yōu)化的方式進(jìn)行估計,優(yōu)化結(jié)束后使用估計的時間延遲量補償圖像時間戳,使得傳感器時間一致,以消除時間延遲對數(shù)據(jù)融合的影響,從而提高算法的定位精度。

2 基于估計時間延遲的視覺慣性融合算法

本文算法框架如圖1所示。本文在前端分別處理圖像數(shù)據(jù)和IMU數(shù)據(jù),在后端維護(hù)了一個擁有固定數(shù)量關(guān)鍵幀的滑動窗口,并采用基于優(yōu)化的方式融合視覺信息與慣性信息共同估計相機位姿,同時估計時間延遲,提升視覺慣性融合的精度,優(yōu)化結(jié)束后得到相機位姿。

圖1 本文算法框架

2.1 IMU初始化

(1)

在靜止時刻,陀螺儀的理想測量值應(yīng)該為0,因此陀螺儀的零偏應(yīng)該為前n幀間IMU數(shù)據(jù)中陀螺儀測量值的平均值。Geneva等人[6]通過加速度計測量值的平均值初始化了加速度計的零偏,但是Qin和Shen在文章[15]中通過實驗指出在初始化中如果忽略加速度計的零偏則不會明顯影響初始化結(jié)果,因此本文沒有計算加速度計的零偏而是設(shè)初始值為0。在IMU初始化結(jié)束后將為后續(xù)的優(yōu)化提供一個更好的初值從而減少IMU的噪聲積累。

2.2 前端相機追蹤

2.2.1 構(gòu)建雙目間準(zhǔn)確的數(shù)據(jù)關(guān)聯(lián)

在前端相機追蹤部分中,讀取相機傳來的雙目圖像,并對圖像進(jìn)行處理。在左目圖像中提取關(guān)鍵點,在關(guān)鍵點的選擇方面,本文在圖像中盡可能提取分布均勻且具有足夠像素梯度的點,根據(jù)當(dāng)前幀的左右圖像恢復(fù)點的深度信息,并采用逆深度參數(shù)化的方式表示關(guān)鍵點。在基于直接法的方案中需要通過當(dāng)前幀的雙目圖像上關(guān)鍵點的光度誤差建立雙目間的數(shù)據(jù)關(guān)聯(lián),恢復(fù)圖像上關(guān)鍵點的逆深度。具體步驟需要將相機左側(cè)圖像提取的關(guān)鍵點變換到右側(cè)圖像,尋找右側(cè)圖像上與之關(guān)聯(lián)的關(guān)鍵點。Rui Wang等人[16]使用極線搜索的方式,通過建立兩點間的光度殘差使用高斯牛頓優(yōu)化的方式得到相關(guān)聯(lián)的關(guān)鍵點,最后恢復(fù)深度信息。本文采用該方法,但是該方法難免會存在構(gòu)建錯誤數(shù)據(jù)關(guān)聯(lián)的情況。為此本文認(rèn)為如果左目圖像上一點pL能夠在右目圖像中找到與之關(guān)聯(lián)的點pR,則將pR以同樣的方式再變換回左目圖像中,如果依然能夠找到與之關(guān)聯(lián)的點,則認(rèn)為pL和pR是一對正確的關(guān)聯(lián)點否則就是一對錯誤的關(guān)聯(lián)點,本文使用往返追蹤描述這種將關(guān)鍵點從左側(cè)圖像變換到右側(cè)圖像再變換回左側(cè)圖像的操作。如圖2圖3所示,為了便于觀察,此處降低了所挑選關(guān)鍵點數(shù)量的閾值,采用往返追蹤可以有效的減少雙目間錯誤的數(shù)據(jù)關(guān)聯(lián),進(jìn)而提高雙目間數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性。

圖2 使用本文方法剔除的錯誤數(shù)據(jù)關(guān)聯(lián)

圖3 使用本文方法得到的最終結(jié)果

2.2.2 幀間追蹤

在追蹤模塊中,利用滑動窗口中的最新關(guān)鍵幀作為參考幀,對新的幀進(jìn)行追蹤。一般來說短暫的幀間運動可以看作是勻速的、平滑的運動,本文采用勻速運動模型來表示幀間運動,追蹤最新的關(guān)鍵幀。由于上一幀的運動與當(dāng)前幀的運動是相似的,因此可以用上一幀的運動代替當(dāng)前幀的運動,求得當(dāng)前幀到最新關(guān)鍵幀的位姿變換,這樣可以將最新關(guān)鍵幀上的點投影到當(dāng)前幀上:

(2)

本文將投影到當(dāng)前幀上的點與參考幀上的點建立光度殘差, 隨后使用高斯牛頓法迭代優(yōu)化位姿。 最后決定當(dāng)前幀是否為新的關(guān)鍵幀, 新的關(guān)鍵幀將加入到滑動窗口中并更新參考幀, 而優(yōu)化的位姿將用于關(guān)鍵點的逆深度值更新。

2.3 時間延遲對圖像信息的影響

根據(jù)IMU預(yù)積分模型[11]可以看出在做預(yù)積分時,將兩幀圖像的時間戳與IMU時間戳作對比找到兩幀之間的IMU數(shù)據(jù)是非常關(guān)鍵的,對于異構(gòu)傳感器中存在通信延遲、傳輸延遲、時鐘不同步等問題,使得相機實際捕獲圖像的時間戳與數(shù)據(jù)集給出的對應(yīng)圖像時間戳并非嚴(yán)格相等,二者之間存在著時間延遲。

時間延遲是一個恒定且未知的值,它的存在使得在做IMU預(yù)積分前,即便使用了插值的方式仍然不能準(zhǔn)確的根據(jù)圖像時間戳找到兩幀間IMU數(shù)據(jù)。如果不能很好的解決這個問題,將影響到IMU預(yù)積分、多傳感器數(shù)據(jù)融合和算法優(yōu)化的結(jié)果,進(jìn)而使得算法精度下降。

基于上述分析,準(zhǔn)確的估計時間延遲并且矯正圖像時間戳成為了非常重要的一步。在多傳感器數(shù)據(jù)融合時,由于數(shù)據(jù)集給出的圖像時間戳并不等于相機在實際時刻捕獲圖像的時間,因此圖像也不是相機在實際時刻捕獲的圖像。 時間延遲的存在導(dǎo)致無法獲取相機在實際時刻捕捉的圖像以及對應(yīng)的圖像時間戳,因此需要預(yù)測相機在實際時刻捕獲的圖像信息以及對應(yīng)的時間戳。接下來本文將相機在實際時刻捕獲的圖像稱為實際時刻圖像, 記為I(treal),該圖像對應(yīng)的時間戳為treal,如圖4所示,圖4描述了時間軸上不同傳感器數(shù)據(jù)的時間戳之間的關(guān)系以及圖像時間戳對應(yīng)的圖像表示,其中td表示相機實際時刻捕獲圖像的時間與數(shù)據(jù)集給出圖像的時間戳之間的時間延遲,假設(shè)在第tk時刻有圖像Ik,根據(jù)本文的方法可以求出td,同時計算第k個相機實際時刻捕獲的圖像Ik(tk＿real),以及對應(yīng)的時間tk＿real。一般來說,td的值可能是正的也可能是負(fù)的,如果實際時刻的圖像時間戳相對于數(shù)據(jù)集提供的圖像時間戳有著更高的時間延遲,那么td的值就是正的,否則就是負(fù)的。

圖4 不同傳感器上時間戳關(guān)系圖示

綜上所述為了能夠估計,Qin和Shen[13]通過計算特征點的移動速度求解特征點在時間延遲前的坐標(biāo),從而將帶入到優(yōu)化方程中。由于本文使用的是直接法,根據(jù)特征匹配求特征點速度是不可行的,且直接法在后端優(yōu)化中根據(jù)特征點之間的灰度值差異構(gòu)造光度殘差而非通過幾何位置的差異構(gòu)造的重投影誤差,因此迫切需要找到一種新的方法將時間延遲帶入到優(yōu)化中去。與Qin和Shen的做法不同的是,本文假設(shè)相機在極短的時間內(nèi)(幾毫秒)捕獲的連續(xù)兩幀圖像上每一個像素點的灰度值隨時間發(fā)生著均勻變化,因此當(dāng)滑動窗口中加入新的關(guān)鍵幀時,可以通過時間延遲計算出該幀圖像上的像素點在實際時刻的灰度值,通過實際時刻的灰度值構(gòu)造光度殘差,從而將時間延遲帶入到優(yōu)化中去。

2.4 帶有時間延遲的視覺慣性聯(lián)合優(yōu)化

在考慮時間延遲后采用緊耦合的方式將視覺部分與IMU測量值數(shù)據(jù)進(jìn)行融合,使用兩個模型共同估計一組狀態(tài)向量, 將時間延遲考慮進(jìn)狀態(tài)向量S

S=[c,td,s0,s1,…,sn,d0,d1,…,dm]

(3)

其中c為相機內(nèi)參矩陣,d為關(guān)鍵點的逆深度,m為滑動窗口中參與優(yōu)化的關(guān)鍵點總數(shù),n為滑動窗口中關(guān)鍵幀的總數(shù),p為位置,v為速度,q為旋轉(zhuǎn)的四元數(shù)表示。 滑動窗口內(nèi)的目標(biāo)函數(shù)為最小化IMU項、視覺項與來自邊緣化之后先驗項之和

(4)

對于加入到滑動窗口中的關(guān)鍵幀來說,往往根據(jù)兩關(guān)鍵幀上關(guān)鍵點的灰度值差異構(gòu)造光度殘差以優(yōu)化相機位姿

rij=Ij[p′]-Ii[p]

(5)

其中p′表示第i幀上關(guān)鍵點p變換到第j幀上的點的坐標(biāo)。由于時間延遲的存在,關(guān)鍵點p與p′的灰度值不再是該點所在圖像的灰度值, 而是該點在實際時刻的灰度值。因此首先求出該點在實際時刻的灰度值。

基于上節(jié)的假設(shè),首先獲取實際時刻圖像。記滑動窗口中第i個關(guān)鍵幀對應(yīng)的圖像為第k個相機的左側(cè)圖像,通過與其相鄰的上一幀圖像,可以求得幀上每一個像素點的灰度值隨時間的變化量,用如下公式表示

(6)

(7)

獲得關(guān)鍵幀的實際時刻圖像后, 對于之前在原圖像上提取的關(guān)鍵點, 由于時間延遲的存在, 即可在實際時刻圖像中根據(jù)之前提取關(guān)鍵點的坐標(biāo)信息得到關(guān)鍵點在實際時刻的灰度值。 從而將時間延遲代入到殘差函數(shù)式(5), 由于第i幀中左目圖像與右目圖像構(gòu)建的殘差函數(shù)與IMU信息無關(guān),因此僅將時間延遲考慮進(jìn)第i幀中左目圖像與第j幀中左目圖像上的關(guān)鍵點構(gòu)造的殘差函數(shù)。 在考慮了時間延遲后殘差函數(shù)式(5)改為如下所示

(8)

接下來使用非線性最小二乘求解,通過增量更新狀態(tài),在每次迭代后估計的時間延遲在下次迭代優(yōu)化前使用式(9)對圖像時間戳tcam進(jìn)行補償?shù)玫綄嶋H時刻treal

treal=tcam+td

(9)

經(jīng)過補償后在IMU預(yù)積分時根據(jù)圖像時間戳將挑選更準(zhǔn)確的IMU數(shù)據(jù)。本文算法為了能夠控制滑動窗口中優(yōu)化規(guī)模,以保持系統(tǒng)的實時性,使用舒爾補通過邊緣化刪除舊變量[17], 包括幀的位姿、速度以及該幀上提取的點等, 同時將舊變量的信息保留在滑動窗口中。在初始階段,經(jīng)過優(yōu)化后估計的時間延遲會不穩(wěn)定, 但在后續(xù)優(yōu)化中會逐漸收斂,從而證明文中的假設(shè),這一點將在實驗中說明。

3 實驗與分析

本文在EuRoC數(shù)據(jù)集[18]上測試提出的算法。實驗環(huán)境為裝有Ubuntu 18.04, 配置為Intel?CoreTMi7-4770CPU, 8G RAM的計算機。EuRoC數(shù)據(jù)集提供11個雙目圖像序列以及與其對應(yīng)對齊的 IMU加速度、角速度測量值,同時還有11個圖像序列的真實軌跡。數(shù)據(jù)集總共包含兩個場景, 一個是蘇黎世聯(lián)邦理工學(xué)院的機器房, 另一個是普通房間。 本文算法在基于優(yōu)化的直接法視覺里程計的基礎(chǔ)上融合IMU數(shù)據(jù), 同時考慮了數(shù)據(jù)集的圖像時間戳與捕獲該圖像的實際時刻之間時間延遲對算法精度的影響, 傳感器上的時間延遲是未知的, 本文在假設(shè)時間延遲存在且初值為一定值情況下通過計算實際時刻圖像將時間延遲帶入到優(yōu)化中估計時間延遲。本文使用均方根誤差(RMSE)的大小來表示算法的準(zhǔn)確性, 均方根誤差越小代表算法結(jié)果越精確。

表1展示了在EuRoC數(shù)據(jù)集的部分圖像序列中, 假設(shè)當(dāng)前系統(tǒng)存在的時間延遲為5ms時,是否使用本文的方法估計時間延遲對本文算法的影響, 其中前兩行分別為不估計與估計時間延遲時算法的RMSE, 可以看出估計時間延遲的RMSE更小,這是由于估計時間延遲提升了算法中數(shù)據(jù)融合的效果, 進(jìn)而提高了本文算法的精度。 另外選擇估計時間延遲會提高算法的計算量, 本文統(tǒng)計了在后端優(yōu)化執(zhí)行次數(shù)和執(zhí)行總時長, 別得到了估計與不估計時間延遲時優(yōu)化的平均時長, 在第三行和第四行中分別展示了當(dāng)估計時間延遲時, 算法在滑動窗口中執(zhí)行優(yōu)化的平均時長增加量和增加時間占原優(yōu)化時間百分比, 增長維持在20%～30%, 總的來說估計時間延遲在不增加太多的計算量情況下可以提高算法的精度。

表1 估計與不估計時間延遲的軌跡與地面真值之間的對比,以及進(jìn)行時間延遲估計后對系統(tǒng)平均優(yōu)化時間的影響

圖5顯示了在V1＿01＿easy序列中本文算法選擇對時間延遲是否進(jìn)行估計的軌跡同真實軌跡對比的結(jié)果, 其中虛線為軌跡真實值, 藍(lán)色線條為不估計時間延遲后的軌跡, 橙色線條為估計時間延遲后的軌跡。同樣可以看出時間延遲存在時, 使用本文的方法估計時間延遲對軌跡精度有所提升。

圖5 是否估計時間延遲的軌跡對比

圖6顯示了時間延遲存在下, 使用本文方法在V1＿01＿easy序列上估計的時間延遲量的過程, 可以看出隨著程序運行, 待估計的時間延遲在第500幀左右開始穩(wěn)定的收斂到一定值,隨后趨于穩(wěn)定, 最后估計的時間延遲量為4.35ms。

圖6 本文方法估計時間延遲收斂情況

VINS-Mono[4]是香港科技大學(xué)的Shen 等人開源的一套基于特征點法的單目視覺慣性融合定位算法, 而VINS-Fusion[19]則是Shen 等人在VINS-Mono的基礎(chǔ)上擴展了雙目視覺慣性融合等功能,本文選擇使用VINS-Fusion中的雙目視覺慣性里程計同本文算法進(jìn)行比較, 同時開啟二者的回環(huán)檢測功能。 本文算法與VINS-Mono、VINS-Fusion在EuRoC數(shù)據(jù)集的不同序列的均方根誤差在表2中所示, 由表2可以看出假設(shè)當(dāng)前系統(tǒng)存在時間延遲時(=5ms), 本文算法在圖像序列中的均方根誤差要小于VINS-Mono和VINS-Fusion, 取得的定位精度更高。

表2 本文算法同其它算法的RMSE/m對比

在表2中, V1＿02＿medium和V1＿03＿difficult序列中存在大量的回環(huán), 本文算法由于沒有回環(huán)檢測, 難以消除累積誤差, 因此軌跡誤差較大, 此外序列中還存在一些復(fù)雜的情況,比如光照劇烈變化, 這些情況對直接法非常不友好,本文通過融合IMU以彌補視覺方面的不足, 同時本文通過計算實際時刻圖像將時間延遲帶入到優(yōu)化中求解, 對圖像時間戳進(jìn)行補償, 提升了算法融合的效果。 最后序列中存在著特征丟失等特征點法難以應(yīng)對的問題, 而這在直接法中得到了很好的解決。 因此本文算法在V1＿02＿medium序列中的均方根誤差盡管與VINS-Mono、VINS-Fusion的均方根誤差相近但仍然小于二者的均方根誤差。

圖7和圖8展示了當(dāng)假設(shè)系統(tǒng)存在5ms的時間延遲時,本文算法與VINS-Mono以及VINS-Fusion在EuRoC數(shù)據(jù)集中部分圖像序列上的軌跡和地面真值對比, 進(jìn)一步顯示了本文算法可以在場景中取得較好的定位。 其中虛線為軌跡真實值, 藍(lán)色線條為本文算法軌跡, 橙色線條為VINS-Mono, 綠色線條為VINS-Fusion。

圖7 在MH＿03＿M(jìn)edium序列上同其它算法的軌跡對比

圖8 在V2＿01＿easy序列上同其它算法的軌跡對比

4 結(jié)語

本文基于在線估計時間延遲, 提出了一種精確的雙目視覺慣性里程計算法, 為了消除時間延遲帶來的影響, 在緊耦合優(yōu)化的視覺慣性里程計中將時間延遲作為待優(yōu)化變量, 通過優(yōu)化求得正確的時間延遲, 補償數(shù)據(jù)集中的圖像時間戳, 使得兩關(guān)鍵幀之間的IMU預(yù)積分項更加準(zhǔn)確, 進(jìn)而提升數(shù)據(jù)融合效果以及定位精度。 在EuRoC數(shù)據(jù)集上的實驗結(jié)果表明本文提出的融合算法能夠很好的估計時間延遲同時消除時間延遲帶來的影響, 與特征點法相比本文算法有著較好的定位精度。