基于互相關(guān)和旋轉(zhuǎn)約束的視覺(jué)慣性里程計(jì)在線時(shí)間校準(zhǔn)算法

摘 要：在融合相機(jī)和慣性測(cè)量單元（IMU）的數(shù)據(jù)推測(cè)機(jī)器人的運(yùn)動(dòng)軌跡時(shí)，傳感器測(cè)量記錄的時(shí)間點(diǎn)對(duì)用于估計(jì)軌跡的視覺(jué)慣性里程計(jì)（VIO）的魯棒性和準(zhǔn)確性至關(guān)重要。然而，由于傳感器數(shù)據(jù)到達(dá)接收端的延遲存在差異，圖像數(shù)據(jù)流和IMU數(shù)據(jù)流之間通常存在不可避免的時(shí)間偏置，為此提出了一種基于互相關(guān)和旋轉(zhuǎn)對(duì)齊的視覺(jué)慣性里程計(jì)在線時(shí)間校準(zhǔn)的算法。首先使用對(duì)極幾何和預(yù)積分算法分別得到相機(jī)和IMU各自的相對(duì)位姿，并計(jì)算出相機(jī)的角速度；然后根據(jù)相機(jī)與IMU的角速度進(jìn)行互相關(guān)計(jì)算，得到初步的時(shí)間偏置估計(jì)；最后利用相機(jī)和IMU相對(duì)位姿進(jìn)行旋轉(zhuǎn)約束，通過(guò)優(yōu)化誤差函數(shù)得到更精確的相對(duì)時(shí)間偏置估計(jì)，該時(shí)間偏置值隨后用于平移傳感器的時(shí)間軸以進(jìn)行校準(zhǔn)。實(shí)驗(yàn)表明，該算法能夠減緩時(shí)間偏置對(duì)里程計(jì)精度帶來(lái)的影響，并使得VIO能夠在具有更大時(shí)間偏置范圍的數(shù)據(jù)流下穩(wěn)定運(yùn)行。

關(guān)鍵詞：在線時(shí)間校準(zhǔn)；旋轉(zhuǎn)約束；視覺(jué)慣性里程計(jì)

中圖分類號(hào)：TP391"" 文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1001-3695（2025）01-040-0288-05

doi： 10.19734/j.issn.1001-3695.2024.03.0114

Online temporal calibration based on cross-correlation androtation constraints for visual-inertial odometry

Abstract：In the fusion of camera and inertial measurement unit （IMU） data for inferring the robot’s trajectory， the timing of sensor measurements plays a crucial role in the robustness and accuracy of the visual-inertial odometry （VIO） . However， due to differences in the delay of sensor data reaching the receiver， an unavoidable time offset exists between the image data stream and the IMU data stream. To address this issue， this paper proposed an algorithm based on cross-correlation and rotation constraints for online temporal calibration of VIO. Firstly， it used the epipolar geometry and preintegration algorithms to obtain the relative poses for both the camera and the IMU. Then， it evaluated cross-correlation based on the angular velocities to obtain an initial estimate of the time offset. Finally， it utilized the relative pose between the camera and IMU to impose rotation constraints and optimize an error function， leading to a more accurate estimation of the time offset. It employed this time offset va-lue to calibrate the sensors. Experimental results show that the proposed algorithm effectively mitigates the impact of time offset on the accuracy of odometry trajectories， enabling the system to reliably operate when significant time offsets exist in the data streams.

Key words：online temporal calibration; rotation constraints; visual-inertial odometry

0 引言

同步定位與建圖（SLAM）是機(jī)器人和計(jì)算機(jī)視覺(jué)領(lǐng)域的研究熱點(diǎn)，它使得機(jī)器人可以在未知環(huán)境中進(jìn)行自主探索和定位，同時(shí)構(gòu)建地圖，實(shí)現(xiàn)路徑規(guī)劃和導(dǎo)航等功能。機(jī)器人可以使用不同的傳感器組合進(jìn)行運(yùn)動(dòng)估計(jì)：?jiǎn)文肯鄼C(jī)[1～3]、雙目相機(jī)[4，5]、RGB-D[6，7]和視覺(jué)慣性測(cè)量單元（IMU）組合[8～10]。其中，相機(jī)和慣性測(cè)量單元的組合，也稱為視覺(jué)慣性里程計(jì)（VIO），近年來(lái)越來(lái)越受關(guān)注[8～12]。一方面，相機(jī)和IMU具有互補(bǔ)性，兩種傳感器的組合可以顯著提高VIO的準(zhǔn)確性和魯棒性。相機(jī)能夠提供清晰的視覺(jué)測(cè)量信息，可以用于識(shí)別和跟蹤場(chǎng)景中的特征點(diǎn)，從而提供有關(guān)相對(duì)運(yùn)動(dòng)的信息，IMU能夠提供高頻率的本體加速度和角速度測(cè)量數(shù)據(jù)，可以用于推斷姿態(tài)變化和速度信息，不會(huì)受到外部光照和天氣的影響。另一方面，隨著傳感器技術(shù)的發(fā)展，IMU的成本下降，越來(lái)越多的機(jī)器人、無(wú)人機(jī)和消費(fèi)電子產(chǎn)品都配備了低成本的基于微機(jī)電系統(tǒng)（MEMS）的IMU。然而，由于數(shù)據(jù)傳輸、傳感器延遲和操作系統(tǒng)開(kāi)銷等各種原因，傳感器數(shù)據(jù)到達(dá)接收端存在延遲，這導(dǎo)致了傳感器實(shí)際采樣和記錄上存在時(shí)間延遲，當(dāng)兩個(gè)傳感器的延遲不同時(shí)，就會(huì)出現(xiàn)相對(duì)時(shí)間偏置。現(xiàn)有研究表明，該時(shí)間偏置會(huì)影響VIO的定位精度，甚至可能導(dǎo)致系統(tǒng)無(wú)法運(yùn)行。

對(duì)于自行組裝的傳感器套件而言，通常無(wú)法實(shí)現(xiàn)硬件同步，故而軟件同步方法受到歡迎。Liu 等人[13]將時(shí)間偏置作為擴(kuò)展卡爾曼濾波（EKF） VIO中的估計(jì)狀態(tài)變量，并提出了一種方法來(lái)獲取特征點(diǎn)在時(shí)間偏置影響下的虛擬觀測(cè)，通過(guò)修正后的投影模型在時(shí)域內(nèi)對(duì)齊相機(jī)和 IMU 測(cè)量。Kelly 等人[14]將時(shí)間校準(zhǔn)問(wèn)題轉(zhuǎn)換為一種空間曲線對(duì)齊問(wèn)題，通過(guò)類似迭代最近點(diǎn)（ICP）的方法對(duì)齊三維旋轉(zhuǎn)空間中的曲線實(shí)現(xiàn)傳感器之間的時(shí)間校準(zhǔn)，但算法為離線算法，無(wú)法在線校準(zhǔn)。Wang 等人[15]提出了一種在線初始化方法，用于初始化視覺(jué)慣性激光雷達(dá)系統(tǒng)（VILS），可以實(shí)現(xiàn)各傳感器的時(shí)間同步和自動(dòng)計(jì)算外部參數(shù)，無(wú)須事先了解環(huán)境信息或特殊運(yùn)動(dòng)。Qin 等人[16]通過(guò)在SLAM系統(tǒng)中估計(jì)相機(jī)和IMU的狀態(tài)以及特征點(diǎn)位置的同時(shí)估計(jì)時(shí)間偏置來(lái)進(jìn)行校準(zhǔn)，該方法可以應(yīng)用于多種基于特征點(diǎn)的優(yōu)化框架中，但對(duì)于目前的半稠密或者稠密建圖的SLAM系統(tǒng)難以適用，且需要提供初始估計(jì)值。Li 等人[17]將時(shí)間偏置視為EKF框架中附加的狀態(tài)變量，并與其他狀態(tài)變量（IMU姿態(tài)和速度、偏差、相機(jī)到 IMU 的轉(zhuǎn)換、特征位置）一起進(jìn)行估計(jì)，且分析了視覺(jué)和慣性傳感器之間時(shí)間偏置的可識(shí)別性。Yang等人[18]對(duì)EKF框架下的輔助慣性導(dǎo)航系統(tǒng)（INS）的空間和時(shí)間校準(zhǔn)參數(shù)進(jìn)行了詳細(xì)的能觀性分析， 發(fā)現(xiàn)四種特定的退化運(yùn)動(dòng)模式會(huì)降低校準(zhǔn)的準(zhǔn)確性，在實(shí)際應(yīng)用中應(yīng)盡量避免該類運(yùn)動(dòng)模式。

現(xiàn)有的時(shí)間校準(zhǔn)算法多以增廣機(jī)器人的狀態(tài)向量的方式對(duì)時(shí)間偏置進(jìn)行后端優(yōu)化估計(jì)，這些方法僅適用于特定的系統(tǒng)，一般為EKF框架下進(jìn)行狀態(tài)估計(jì)的系統(tǒng)，而且校準(zhǔn)算法使用前往往需要設(shè)定初始的估計(jì)值，初始值會(huì)影響整個(gè)算法的校準(zhǔn)質(zhì)量，但該值現(xiàn)實(shí)中一般是不可知的。面對(duì)各種不同原理的SLAM，如預(yù)積分形式的SLAM[19]和基于因子圖優(yōu)化理論的iSAM2[20]等，當(dāng)前的校準(zhǔn)算法要么不適用，要么需要對(duì)方法框架進(jìn)行調(diào)整才能兼容。為了使更多的系統(tǒng)能夠使用對(duì)齊的相機(jī)和IMU數(shù)據(jù)，本文提出一種在線的時(shí)間偏差校準(zhǔn)算法，該算法依靠?jī)蓚€(gè)傳感器的相對(duì)旋轉(zhuǎn)，利用互相關(guān)和優(yōu)化旋轉(zhuǎn)約束對(duì)相機(jī)和IMU之間的相對(duì)時(shí)間偏置進(jìn)行估計(jì)。通過(guò)互相關(guān)進(jìn)行初步的時(shí)間偏置估計(jì)，在無(wú)須提供初始值的前提下自動(dòng)給出合理的起始估計(jì)值；通過(guò)旋轉(zhuǎn)約束構(gòu)造優(yōu)化的目標(biāo)函數(shù)，優(yōu)化過(guò)程結(jié)束后將得到更準(zhǔn)確的估計(jì)值。該算法僅依賴?yán)锍逃?jì)運(yùn)行過(guò)程中得到的相對(duì)位姿與IMU測(cè)量數(shù)據(jù)，降低了算法與具體系統(tǒng)的依賴程度，在不需修改的前提下可以推廣到更多的SLAM系統(tǒng)中使用。

1 研究問(wèn)題設(shè)定與校準(zhǔn)算法框架

圖1展示了所研究問(wèn)題的示意圖，即相機(jī)和IMU數(shù)據(jù)流之間的時(shí)間偏置，td為兩個(gè)傳感器之間的相對(duì)時(shí)間偏置。在采樣時(shí)，第k幀圖片與第n個(gè)IMU測(cè)量數(shù)據(jù)是同步進(jìn)行的，由于數(shù)據(jù)傳輸延遲等各種原因，在數(shù)據(jù)的記錄上并不同步，當(dāng)兩個(gè)傳感器的延遲不同時(shí)，就會(huì)出現(xiàn)相對(duì)時(shí)間偏置，此時(shí)IMU測(cè)量數(shù)據(jù)超前于相機(jī)數(shù)據(jù)到達(dá)接收端。本文的目標(biāo)是通過(guò)算法估計(jì)出兩個(gè)數(shù)據(jù)流之間的td，利用td的估計(jì)值將IMU數(shù)據(jù)流的記錄時(shí)間軸進(jìn)行平移得到對(duì)齊的相機(jī)和IMU數(shù)據(jù)流。

算法的框架示意圖如圖2所示。本文算法主要分為兩個(gè)部分，首先利用相鄰兩幀圖片計(jì)算出相機(jī)的相對(duì)位姿，在相鄰兩幀相機(jī)保持勻速運(yùn)動(dòng)的假設(shè)下計(jì)算出相機(jī)的角速度；在得到足夠多的角速度信息后，利用相機(jī)的角速度與IMU測(cè)量得到的角速度進(jìn)行互相關(guān)計(jì)算，得到初步的時(shí)間偏置估計(jì)值txcorrd；最后，根據(jù)相鄰圖片幀的相對(duì)旋轉(zhuǎn)，與IMU積分得到的相對(duì)旋轉(zhuǎn)進(jìn)行對(duì)齊，通過(guò)優(yōu)化目標(biāo)函數(shù)從而得到時(shí)間偏置微調(diào)量tcond，使得兩個(gè)估計(jì)出的時(shí)間偏置值接近真實(shí)的時(shí)間偏置td，即滿足式（1）。

txcorrd+tcond=td（1）

2 相對(duì)位姿和角速度計(jì)算

本章先對(duì)常用的一些符號(hào)進(jìn)行定義。（·）w表示全局坐標(biāo)系，（·）c表示相機(jī)坐標(biāo)系，（·）b表示IMU坐標(biāo)系。Rwc，pwc是相機(jī)在全局坐標(biāo)系中的位姿，可以將相機(jī)坐標(biāo)系中的三維空間點(diǎn)轉(zhuǎn)換到全局坐標(biāo)系中。

2.1 IMU積分預(yù)測(cè)位姿

IMU的積分預(yù)測(cè)有多種不同的形式，在本文使用SO（3）形式的積分方法。首先給出IMU的數(shù)學(xué)模型：

其中：Rwi+1為IMU第i+1時(shí)刻在世界坐標(biāo)系下的旋轉(zhuǎn)矩陣;vwi+1為IMU第i+1次測(cè)量時(shí)刻的線速度;pwi+1為第i+1次測(cè)量的位置;·」×為向量對(duì)應(yīng)的反對(duì)稱矩陣;Δt是第i時(shí)刻到第i+1時(shí)刻的時(shí)間間隔。本文用的是中值積分方法，故上述公式中的ω和a為

在計(jì)算IMU積分的時(shí)間間隔內(nèi)通常將bg和ba視為不變，通過(guò)預(yù)積分，系統(tǒng)得到IMU的相對(duì)位姿。本文僅需要相對(duì)旋轉(zhuǎn)部分，故只需要式（3）中的第一項(xiàng)即可。

2.2 相機(jī)相對(duì)旋轉(zhuǎn)和角速度

假設(shè)系統(tǒng)從相鄰兩幀圖片得到一些配對(duì)好的特征點(diǎn)。利用配對(duì)點(diǎn)集，系統(tǒng)可以恢復(fù)出在兩幀相機(jī)的相對(duì)運(yùn)動(dòng)。設(shè)某個(gè)特征點(diǎn)P在上一幀相機(jī)坐標(biāo)系下的空間位置為P=［X，Y，Z］T，則該特征點(diǎn)在兩幀圖片中對(duì)應(yīng)的像素點(diǎn)的齊次坐標(biāo)p1、p2之間有位置關(guān)系：

其中：R和t為相對(duì)位姿對(duì)應(yīng)的旋轉(zhuǎn)和平移。利用外積的特性，即t」×t=0和xTt」×x=0，·」×可將兩個(gè)向量的外積轉(zhuǎn)換為反對(duì)稱矩陣和向量相乘，對(duì)式（5）左右兩邊乘上t」×和pT2有

對(duì)應(yīng)的旋轉(zhuǎn)軸n可以根據(jù)旋轉(zhuǎn)矩陣的特性Rn=n得到，此時(shí)n為旋轉(zhuǎn)矩陣特征值為1所對(duì)應(yīng)的特征向量。獲得旋轉(zhuǎn)軸n和旋轉(zhuǎn)角度θ后，假設(shè)相機(jī)在相鄰兩幀之間做勻速運(yùn)動(dòng)，根據(jù)相機(jī)的采樣頻率fc得到相機(jī)當(dāng)前時(shí)刻的角速度ωc：

ωc=fcnθ（8）

3 時(shí)間偏置估計(jì)方法

3.1 互相關(guān)估計(jì)方法

相機(jī)和IMU之間的時(shí)間偏置可以通過(guò)對(duì)齊IMU測(cè)量的角速度和相機(jī)對(duì)極幾何估算出的角速度解決，本文使用互相關(guān)對(duì)兩個(gè)傳感器的角速度進(jìn)行對(duì)齊。經(jīng)典的互相關(guān)估計(jì)精度取決于兩個(gè)序列的采樣頻率，為了解決這個(gè)問(wèn)題，可以使用更高的頻率對(duì)IMU和相機(jī)的角速度進(jìn)行插值，本文實(shí)驗(yàn)中的插值頻率比IMU采樣頻率高。由于相機(jī)的角速度通過(guò)相對(duì)位姿的微分得到，受噪聲影響較大，在插值之前先使用中值濾波對(duì)相機(jī)的角速度進(jìn)行濾波。將IMU和相機(jī)角速度插值到同樣的頻率后，要解決的問(wèn)題轉(zhuǎn)換為

其中：Δt為插值后相鄰數(shù)據(jù)點(diǎn)的時(shí)間間隔；N為時(shí)間偏差的最大估計(jì)間隔，它對(duì)應(yīng)著最大的估計(jì)偏移量NΔt；{·}Nm=－N是包含了m∈［－N，N］Z時(shí)對(duì)應(yīng)的集合?；ハ嚓P(guān)函數(shù)xcorr（m）定義如下：

當(dāng)i+mgt;L或i+mlt;1時(shí)，ωc（i+m）=0，ωI同理。L為插值后的序列長(zhǎng)度，RIC為相機(jī)和IMU之間的外參旋轉(zhuǎn)矩陣。由于角速度有三個(gè)方向的分量，本文對(duì)角速度的三個(gè)分量分別進(jìn)行互相關(guān)計(jì)算，取三個(gè)方向估計(jì)出的時(shí)間偏置的中值作為初步估計(jì)的結(jié)果。當(dāng)插值頻率較高時(shí)可能會(huì)占用一定的計(jì)算資源，將數(shù)據(jù)序列通過(guò)傅里葉變化，在頻域上進(jìn)行計(jì)算可使速度加快。

3.2 旋轉(zhuǎn)約束估計(jì)方法

在3.1節(jié)中，使用互相關(guān)的方式獲得了初步的時(shí)間偏差估計(jì)，這個(gè)估計(jì)的精度取決于角速度序列的插值間隔，即最好的結(jié)果只能獲得插值間隔步長(zhǎng)的準(zhǔn)確度，而縮小插值間隔又會(huì)顯著降低運(yùn)算的速度，所以在有實(shí)時(shí)性要求的系統(tǒng)中只能適當(dāng)?shù)厥褂没ハ嚓P(guān)進(jìn)行時(shí)間偏差估計(jì)。本節(jié)將介紹通過(guò)優(yōu)化視覺(jué)測(cè)量與IMU測(cè)量之間的目標(biāo)函數(shù)得到更細(xì)致的估計(jì)。

在2.1節(jié)和2.2節(jié)通過(guò)IMU的測(cè)量數(shù)據(jù)和相鄰兩幀圖片計(jì)算出對(duì)應(yīng)的相對(duì)位姿，這兩個(gè)傳感器計(jì)算出來(lái)的位姿是相關(guān)的，系統(tǒng)通過(guò)優(yōu)化目標(biāo)函數(shù)的方法，對(duì)齊兩個(gè)傳感器的相對(duì)旋轉(zhuǎn)后可以估算出更準(zhǔn)確的時(shí)間偏差，其原理如圖3所示。假設(shè)在某條機(jī)器人的運(yùn)動(dòng)軌跡中，根據(jù)互相關(guān)估計(jì)的時(shí)間偏置值平移IMU傳感器的記錄時(shí)間后，相機(jī)和IMU數(shù)據(jù)之間仍有較小的時(shí)間偏置存在。

圖3中的藍(lán)點(diǎn)和紅點(diǎn)代表IMU和相機(jī)同步采樣的測(cè)量數(shù)據(jù)（參見(jiàn)電子版），它們受到傳輸延遲等影響導(dǎo)致在數(shù)據(jù)的記錄上并不同步，此時(shí)IMU的測(cè)量數(shù)據(jù)比相機(jī)晚到達(dá)接收端。通過(guò)對(duì)極幾何和IMU積分計(jì)算后，分別得到了相機(jī)在第一幀到第二幀時(shí)兩個(gè)傳感器各自的相對(duì)旋轉(zhuǎn)。在IMU積分預(yù)測(cè)時(shí)，起點(diǎn)多計(jì)算了exp（ω1」×td）的旋轉(zhuǎn)量，而在積分終點(diǎn)處少了exp（ω2」×td），在積分的起點(diǎn)和終點(diǎn)添加反方向的旋轉(zhuǎn)，從而使得兩個(gè)相對(duì)旋轉(zhuǎn)能夠?qū)R。兩個(gè)相對(duì)旋轉(zhuǎn)構(gòu)成匹配對(duì)，當(dāng)N對(duì)計(jì)算出的相對(duì)旋轉(zhuǎn)進(jìn)行對(duì)齊后，理想狀態(tài)下，積分估計(jì)應(yīng)與相機(jī)估計(jì)一致。根據(jù)式（7）的Rodrigues公式，以匹配的相對(duì)旋轉(zhuǎn)的誤差作為目標(biāo)函數(shù)，具體公式如下：

關(guān)估計(jì)結(jié)果可以極大地避免需要限定結(jié)果的情況發(fā)生。當(dāng)獲得tcond后，根據(jù)式（14）進(jìn)行整體的時(shí)間軸平移，從而完成校準(zhǔn)。

tcam=tIMU+txcorrd+tcond（14）

4 實(shí)驗(yàn)結(jié)果分析

4.1 數(shù)據(jù)集和實(shí)驗(yàn)系統(tǒng)

實(shí)驗(yàn)是在Blackbird[21]數(shù)據(jù)集上進(jìn)行的，該數(shù)據(jù)集是一個(gè)室內(nèi)飛行數(shù)據(jù)集，使用自制的四旋翼平臺(tái)進(jìn)行收集。它的突出特點(diǎn)是運(yùn)動(dòng)較快速劇烈，速度可達(dá)到7 m/s。實(shí)驗(yàn)系統(tǒng)如圖4所示，系統(tǒng)是單目相機(jī)的VIO，仿照VINS-Mono[8]，去掉了回環(huán)檢測(cè)模塊和重定位模塊，使用因子圖優(yōu)化框架作為后端。在融合相機(jī)和IMU數(shù)據(jù)前，系統(tǒng)會(huì)進(jìn)行相機(jī)與IMU對(duì)齊操作，即2.2節(jié)中提到的初始化過(guò)程，此時(shí)將估算相機(jī)位姿的尺度因子、第一幀相機(jī)的重力方向以及IMU的初始速度和偏置，具體可參考文獻(xiàn)[8]。系統(tǒng)的前端負(fù)責(zé)在多幀之間進(jìn)行SfM算法，檢測(cè)跟蹤特征點(diǎn)進(jìn)行相機(jī)的姿態(tài)估計(jì)，并使用三角測(cè)量算法計(jì)算新檢測(cè)到的特征點(diǎn)的空間位置。系統(tǒng)后端使用因子圖優(yōu)化框架對(duì)相機(jī)姿態(tài)、3D點(diǎn)、IMU初始速度和偏置進(jìn)行優(yōu)化。此對(duì)齊過(guò)程確保了相機(jī)和IMU測(cè)量之間的一致性，是單目相機(jī)VIO開(kāi)始前的必要準(zhǔn)備。在使用本文提出的時(shí)間校準(zhǔn)算法前，系統(tǒng)需要獲取足夠多的相機(jī)位姿信息，實(shí)驗(yàn)設(shè)定包含50幀位姿信息的窗口，當(dāng)窗口填滿或者有新的10幀位姿信息輸入時(shí)，會(huì)觸發(fā)校準(zhǔn)算法進(jìn)行時(shí)間偏置估計(jì)。每當(dāng)算法估出時(shí)間偏置后，利用它對(duì)IMU測(cè)量數(shù)據(jù)的時(shí)間軸進(jìn)行平移。

4.2 實(shí)驗(yàn)結(jié)果和分析

Blackbird數(shù)據(jù)集確保了傳感器之間的時(shí)間偏置限制在±5 ms內(nèi)，這使得實(shí)驗(yàn)可以定量分析時(shí)間偏置對(duì)系統(tǒng)的影響。為了展示提出算法的時(shí)間校準(zhǔn)能力，本實(shí)驗(yàn)通過(guò)人為地調(diào)整IMU時(shí)間戳來(lái)設(shè)置時(shí)間偏移量，在IMU時(shí)間戳上添加時(shí)間偏移量，使得IMU和相機(jī)之間存在固定的時(shí)間偏置，從而得到帶有時(shí)間偏置的數(shù)據(jù)樣本。本文設(shè)定了-50～+50 ms的時(shí)間偏移量，以系統(tǒng)的運(yùn)行軌跡與真實(shí)軌跡之間的均方根誤差（RMSE）作為評(píng)價(jià)系統(tǒng)準(zhǔn)確性的指標(biāo)?；诖?，實(shí)驗(yàn)得到了不同時(shí)間偏置下系統(tǒng)運(yùn)行的準(zhǔn)確度，具體如圖5所示。

從圖5可以看出，本文算法能夠有效減緩時(shí)間偏置對(duì)系統(tǒng)帶來(lái)的影響，整體的運(yùn)行效果基本與沒(méi)有時(shí)間偏置影響下的運(yùn)行效果持平。當(dāng)對(duì)數(shù)據(jù)集添加大于30 ms或小于-10 ms的偏移量時(shí)，實(shí)驗(yàn)中的VIO系統(tǒng)將無(wú)法運(yùn)行，原因是在對(duì)特征點(diǎn)進(jìn)行追蹤時(shí)，使用錯(cuò)誤的IMU數(shù)據(jù)進(jìn)行預(yù)測(cè)而導(dǎo)致無(wú)法正確地對(duì)特征點(diǎn)進(jìn)行追蹤。當(dāng)飛行器運(yùn)動(dòng)的某個(gè)時(shí)刻，特征點(diǎn)跟蹤失敗而導(dǎo)致系統(tǒng)提前終止運(yùn)行，這說(shuō)明進(jìn)行時(shí)間校準(zhǔn)的必要性。經(jīng)過(guò)本文的時(shí)間校準(zhǔn)算法的校正后，系統(tǒng)成功在更大的時(shí)間偏移范圍內(nèi)運(yùn)行。圖6展示了對(duì)數(shù)據(jù)添加30 ms的時(shí)間偏置后各種系統(tǒng)運(yùn)行狀況的透視圖和俯視圖。藍(lán)色線代表系統(tǒng)的真實(shí)軌跡，紅色線為數(shù)據(jù)無(wú)時(shí)間偏置時(shí)系統(tǒng)的運(yùn)行軌跡，綠色線為無(wú)校準(zhǔn)算法系統(tǒng)運(yùn)行帶偏置的數(shù)據(jù)得到的軌跡，橙色線為使用時(shí)間校準(zhǔn)算法后的系統(tǒng)的運(yùn)行同樣的數(shù)據(jù)得到軌跡（參見(jiàn)電子版）?？梢钥吹?，使用校準(zhǔn)算法的系統(tǒng)其軌跡比原系統(tǒng)更加貼合真實(shí)運(yùn)行的軌跡，校準(zhǔn)后的系統(tǒng)貼合沒(méi)有偏差的系統(tǒng)的運(yùn)行軌跡，而無(wú)校準(zhǔn)的系統(tǒng)由于錯(cuò)誤的初始化導(dǎo)致后面的軌跡尺度無(wú)法保持一致。

實(shí)驗(yàn)還研究了僅使用互相關(guān)進(jìn)行時(shí)間校準(zhǔn)，與互相關(guān)和旋轉(zhuǎn)約束結(jié)合的時(shí)間校準(zhǔn)算法進(jìn)行了對(duì)比，與VINS-Mono及文獻(xiàn)[16]的時(shí)間校準(zhǔn)算法也進(jìn)行了對(duì)比，具體結(jié)果如表1和2所示。表格中td為所添加的時(shí)間偏移量，origin為未經(jīng)修改的原VIO系統(tǒng)得到的誤差，xcorr為僅使用互相關(guān)算法得到的軌跡誤差， xcorr+con為互相關(guān)和旋轉(zhuǎn)約束結(jié)合的校準(zhǔn)后系統(tǒng)得到的誤差。

可以看到兩步結(jié)合的算法在精度上優(yōu)于僅使用互相關(guān)進(jìn)行估計(jì)的方法。VINS-Mono與本文實(shí)驗(yàn)系統(tǒng)使用不同的后端優(yōu)化方法，且加入了回環(huán)檢測(cè)和重定位功能，本文將直接比較其在提升精度能力的百分比。在與VINS-Mono的對(duì)比中，由于文獻(xiàn)[16]的校準(zhǔn)算法需要提供初始值，表中VINS（ori）為無(wú)校準(zhǔn)算法的VINS-Mono系統(tǒng)的軌跡精度，VINS（calib）為加入文獻(xiàn)[16]的校準(zhǔn)算法，但初始值設(shè)置為0 ms（除td=0 ms）的軌跡精度，VINS（calib*）為初始值設(shè)置為對(duì)應(yīng)時(shí)間偏置，如在-30 ms的實(shí)驗(yàn)中初始值設(shè)置為-30 ms。實(shí)驗(yàn)對(duì)比了VINS（ori）和加入校準(zhǔn)算法后精度提升的百分比數(shù)，即表格中括號(hào)內(nèi)的數(shù)據(jù)，數(shù)據(jù)表明文獻(xiàn)[16]初始值的設(shè)定對(duì)于軌跡精度的提升能力有明顯影響，當(dāng)不提供準(zhǔn)確的初始值可能會(huì)對(duì)精度有負(fù)優(yōu)化影響，兩個(gè)表格對(duì)比可以得出本文算法對(duì)軌跡精度的提升能力強(qiáng)于文獻(xiàn)[16]。校準(zhǔn)算法對(duì)系統(tǒng)在同步數(shù)據(jù)樣本上的影響（即td=0 ms時(shí)）實(shí)驗(yàn)中，VINS（calib）初始值設(shè)置為-10 ms，VINS（calib*）初始值設(shè)置為0 ms，兩者對(duì)系統(tǒng)的影響均大于本文所提算法，而實(shí)際場(chǎng)景中是無(wú)法預(yù)知時(shí)間偏置的，初始值的誤判會(huì)嚴(yán)重影響原系統(tǒng)的軌跡精度。

本文算法依賴角速度，并通過(guò)緩存50幀相機(jī)角速度進(jìn)行估算。為了進(jìn)一步說(shuō)明算法的有效性，圖7展示了當(dāng)飛行器剛開(kāi)始運(yùn)動(dòng)時(shí)，窗口內(nèi)各傳感器的角速度在三個(gè)軸的分量。藍(lán)色線段代表通過(guò)2.2節(jié)的方法根據(jù)相機(jī)測(cè)量計(jì)算出來(lái)的角速度，紅色線則是在時(shí)間偏置影響下IMU得到的角速度測(cè)量值，墨綠色粗線則是經(jīng)過(guò)時(shí)間校準(zhǔn)算法平移IMU時(shí)間戳得到的角速度曲線（參見(jiàn)電子版）?？梢钥吹奖疚男?zhǔn)算法能夠契合地將IMU三個(gè)軸的角速度分量對(duì)齊到相機(jī)的角速度上。

5 結(jié)束語(yǔ)

本文提出了一種時(shí)間校準(zhǔn)算法，用于校準(zhǔn)慣性和視覺(jué)傳感器數(shù)據(jù)流之間的相對(duì)時(shí)間偏置。通過(guò)引入互相關(guān)和相機(jī)與IMU之間的旋轉(zhuǎn)約束的方法，校準(zhǔn)了兩個(gè)傳感器數(shù)據(jù)流，提高了VIO的魯棒性和準(zhǔn)確性。本文的結(jié)果表明，互相關(guān)和旋轉(zhuǎn)對(duì)齊之間能夠互相補(bǔ)充。具體而言，互相關(guān)提供了一個(gè)大致的時(shí)間偏置估計(jì)值，此后通過(guò)旋轉(zhuǎn)約束進(jìn)一步微調(diào)互相關(guān)的結(jié)果，微調(diào)后的估計(jì)值用于系統(tǒng)的時(shí)間校準(zhǔn)，校準(zhǔn)后的系統(tǒng)表現(xiàn)出更好的魯棒性。

參考文獻(xiàn)：

[1]Mur-Artal R， Montiel J MM， Tardos J D. ORB-SLAM： a versatile and accurate monocular SLAM system [J]. IEEE Trans on Robo-tics， 2015， 31 （5）： 1147-1163.

[2]Forster C，Pizzoli M， Scaramuzza D. SVO： fast semi-direct monocular visual odometry [C]// Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2014： 15-22.

[3]李用杰， 秦廣健， 武利明， 等. 基于點(diǎn)線面特征的無(wú)漂移旋轉(zhuǎn)視覺(jué)里程計(jì) [J]. 計(jì)算機(jī)應(yīng)用研究， 2023， 40 （12）： 3805-3809. （Li Yongjie， Qin Guangjian， Wu Liming， et al.Drift-free rotation visual odometry based on point-line-plane feature fusion [J]. Application Research of Computers， 2023， 40 （12）： 3805-3809.）

[4]Howard A. Real-time stereo visual odometry for autonomous ground vehicles [C]// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway， NJ： IEEE Press， 2008： 3946-3952.

[5]Wang Rui， Schworer M， Cremers D. Stereo DSO：large-scale direct sparse visual odometry with stereo cameras [C]// Proc of IEEE International Conference on Computer Vision. Piscataway， NJ： IEEE Press， 2017： 3903-3911.

[6]Endres F， Hess J， Sturm J，et al.3-D mapping with an RGB-D camera [J]. IEEE Trans on Robotics， 2013， 30 （1）： 177-187.

[7]Kerl C， Sturm J， Cremers D. Robust odometry estimation for RGB-D cameras [C]// Proc of IEEE International Conference on Robotics and Automation.Piscataway， NJ： IEEE Press， 2013： 3748-3754.

[8]Qin Tong， LiPeiliang， Shen Shaojie. VINS-Mono： a robust and versatile monocular visual-inertial state estimator [J]. IEEE Trans on Robotics， 2018， 34 （4）： 1004-1020.

[9]Sun Ke，Mohta K， Pfrommer B， et al.Robust stereo visual inertial odometry for fast autonomous flight [J]. IEEE Robotics and Automation Letters， 2018， 3 （2）： 965-972.

[10]付煜， 鄭爽， 別桐， 等. 融合點(diǎn)線特征的視覺(jué)慣性SLAM算法 [J]. 計(jì)算機(jī)應(yīng)用研究， 2022， 39 （2）： 349-355. （Fu Yu， Zheng Shuang， Bie Tong， et al.Visual inertial SLAM algorithm fusing point and line feature [J]. Application Research of Computers， 2022， 39 （2）： 349-355.）

[11]Li Mingyang， Mourikis A I. High-precision， consistent EKF-based visual-inertial odometry [J]. The International Journal of Robo-tics Research， 2013， 32 （6）： 690-711.

[12]Mourikis A I， Roumeliotis S I. A multi-state constraint Kalman filter for vision-aided inertial navigation [C]// Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2007： 3565-3572.

[13]Liu Yuzhen， Meng Ziyang. Online temporal calibration based on modified projection model for visual-inertial odometry [J]. IEEE Trans on Instrumentation and Measurement， 2019， 69（7）： 5197-5207.

[14]Kelly J， Roy N，Sukhatme G S. Determining the time delay between inertial and visual sensor measurements [J]. IEEE Trans on Robotics， 2014， 30 （6）： 1514-1523.

[15]Wang Yan， Ma Hongwei. Online spatial and temporal initialization for a monocular visual-inertial-LiDAR system [J]. IEEE Sensors Journal， 2021， 22（2）： 1609-1620.

[16]Qin Tong， Shen Shaojie. Online temporal calibration for monocular visual-inertial systems [C]// Proc of IEEE/RSJ International Confe-rence on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2018： 3662-3669.

[17]Li Mingyang， Mourikis A I. Online temporal calibration for camera-IMU systems： theory and algorithms [J]. The International Journal of Robotics Research， 2014， 33 （7）： 947-964.

[18]Yang Yulin， Geneva P，Eckenhoff K， et al.Degenerate motion analysis for aided INS with online spatial and temporal sensor calibration [J]. IEEE Robotics and Automation Letters， 2019， 4 （2）： 2070-2077.

[19]Forster C， Carlone L，Dellaert F， et al.On-manifold preintegration for real-time visual-inertial odometry [J]. IEEE Trans on Robotics， 2016， 33 （1）： 1-21.

[20]Kaess M， Johannsson H， Roberts R，et al.iSAM2： incremental smoothing and mapping with fluid relinearization and incremental variable reordering [C]// Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2011： 3281-3288.

[21]Antonini A， Guerra W， Murali V，et al.The blackbird UAV dataset [J]. The International Journal of Robotics Research， 2020， 39 （10-11）： 1346-1364.