范涵奇,車金釗

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

1 引言

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

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

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

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

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

2 基于估計(jì)時(shí)間延遲的視覺慣性融合算法

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

圖1 本文算法框架

2.1 IMU初始化

(1)

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

2.2 前端相機(jī)追蹤

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

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

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

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

2.2.2 幀間追蹤

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

(2)

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

2.3 時(shí)間延遲對(duì)圖像信息的影響

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

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

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

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

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

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

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

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

(3)

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

(4)

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

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

(5)

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

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

(6)

(7)

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

(8)

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

treal=tcam+td

(9)

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

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

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

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

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

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

圖5 是否估計(jì)時(shí)間延遲的軌跡對(duì)比

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

圖6 本文方法估計(jì)時(shí)間延遲收斂情況

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

表2 本文算法同其它算法的RMSE/m對(duì)比

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

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

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

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

4 結(jié)語

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