VIO系統的IMU與相機時間偏差標定

  視覺里程計（VIO）作為一種空間定位方法，廣泛應用於VR/AR、無人駕駛和移動機器人，比如近年火熱的蘋果 AR-Kit和谷歌AR-Core都使用了VIO技術進行空間定位。通常，VIO系統忽略IMU與相機時間偏差，認為IMU和相機時間是同步和對齊的，然而由於硬件系統的觸發延時、傳輸延時和沒有准確同步時鍾等問題，IMU和相機之間通常存在時間偏差，估計並糾正這個偏差將有效提升VIO系統的性能。本文將介紹一種簡單有效方法，該方法發表與IROS2018，是港科大沈劭劼老師團隊的成果，並且該方法對應的文章獲得了IROS2018 Best Student Paper，特別值得我們一起看一看。

 圖1-1 VIO系統 IMU和Camera # 1. 介紹   一般的視覺里程計(VIO)系統包含兩個傳感器，一個視覺傳感器（相機）和一個慣性傳感器(IMU),如圖1-1。系統對兩個傳感器進行采樣，獲得相應的數據（圖像、IMU數據）與對應的采樣時間戳。通常，我們假設獲得的采樣時間戳即為傳感器采樣時刻對應的時間，比如相機曝光時刻（通常曝光持續幾毫秒到幾十毫秒，認為曝光時刻為曝光持續的中間時刻），然而，系統存在的觸發延時、傳輸延時和沒有准確同步時鍾等問題，使得相機和IMU之間的時間不對齊。  圖1-2 - 相機和IMU數據流時間不對齊   如圖1-2，上方為相機真實的采樣時間點，下方為對應采樣記錄的時間戳，相機時間戳與真實采樣時間存在一個$t_d$的時間偏差。相機與IMU數據流時間戳上的偏差將影響VIO系統的工作，比如在[VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono)系統中，兩個KeyFrame圖像之間的IMU預積分項將因為時間不對齊而不准確，從而給狀態估計帶來偏差。因此，IMU與相機時間對齊標定是一個重要的問題，筆者准備對幾種IMU與相機時間對齊標定方法進行整理和總結。

IMU和相機時間偏差標定的幾種方法包括：

• 港科大沈劭劼團隊 - 基於VINS-Mono的在線時間標定方法
• 李名揚 - 基於MSCKF的在線時間標定方法
• Kalibr 工具箱中離線的時間標定方法

  本文先對其中第一種方法，港科大沈劭劼老師團隊的 基於VINS-Mono的在線時間標定方法[1]進行梳理。這篇文章獲得了機器人領域頂級會議 IROS2018 的 Best Student Paper 獎項，是一篇很值得了解的文章。

2. 基於VINS-Mono的在線時間標定方法

• 文獻：Online Temporal Calibration for Visual Intertial System
• 作者：Tong Qin and Shaojie Shen, HKUST
• 會議：IROS2018
• 榮譽：Best Student Paper

2.1 基本思想

  該方法的基本思想是將時間偏差作為狀態變量的一部分，關鍵問題在於如何構建帶時間偏差的VIO數學模型。設想，在IMU和相機存在時間偏差\(t_d\)的情形下,令\(t_{IMU} = t_{cam} + t_d\)，如圖1-2，相機數據的時間戳比對應數據真實的采樣時間戳大\(t_d\)。假設第\(k\)幀圖像\(I_k\)對應的時間戳為\(t_k\)，那么其真實的采樣時間為\(t_k - t_d\)。對於\(I_k\)上的一個特征點\(\mathbf{z}_l^k\),其圖像坐標\([u_l^k, v_l^k]^T\),在\(t_k\)時間的位置已經發生偏移，如圖2-1。

 圖2-1 $t_k$時刻特征點位置發生偏移

作者希望估計\(t_k\)時(采樣時間)特征點的位置，假設

特征點在短時間（比如\(t_d\)）內在圖像平面內的運動為勻速運動。

那么\(t_k\)時刻估計的特征點坐標（時間偏差校正后的坐標）\(\mathbf{z}_l^k(t_d)\)為：

\[\begin{equation} \mathbf{z}_l^k(t_d) = [u_l^k, v_l^k]^T + t_d\mathbf{V}_l^k \tag{2-2} \end{equation} \]

其中，\(\mathbf{V}_l^k\)為特征點在像素平面內的速度，可通過前后兩幀特征點的位置進行估計，即

\[\begin{equation} \mathbf{V}_l^k=(\begin{bmatrix}u_l^{k+1} \\ v_l^{k+1}\end{bmatrix} - \begin{bmatrix}u_l^k \\ v_l^k\end{bmatrix}) / (t_{k+1} - t_k) \tag{2-1} \end{equation} \]

  基於上述簡單的假設，在VIO系統中將經時間偏差校正后的特征點坐標\(\mathbf{z}_l^k(t_d)\)（帶參數\(t_d\)）替換之前的特征點坐標\(\mathbf{z}_l^k\)，從而將時間偏差參數\(t_d\)引入了優化方程相機殘差項，即重投影誤差可以寫成：

\[\begin{equation} \mathbf{e}_l^k = \mathbf{z}_l^k(t_d) - \pi(\mathbf{R}_{wc_k}^T(\mathbf{P}_l-\mathbf{p}_{c_k})) \tag{2-3} \end{equation} \]

狀態變量增加時間偏差\(t_d\)為

\[\begin{equation} \chi = [\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_n, \mathbf{P}_0, \mathbf{P}_1,\dots, \mathbf{P}_l, \color{red}{t_d}] \\ \mathbf{x}_k = [\mathbf{p}_k, \mathbf{v}_k, \mathbf{R}_k, \mathbf{b}_a, \mathbf{b}_g], k \in [0, n] \tag{2-4} \end{equation} \]

對於優化目標函數，將特征點重投影誤差項替換為式(2-3)，形式與VINS-Mono原來保持一致，即：

\[\begin{equation} min\bigg\{ \Big\|e_p - \mathbf{H}_p\chi \Big\|^2 + \sum_{k\in B}{\Big\|\mathbf{e}_B(\mathbf{z}_{k+1},\chi)\Big\|_{\mathbf{P}_{k+1}^k}^2} + \sum_{ (l, j) \in C}{\Big\| \mathbf{e}_C(\color{red}{\mathbf{z}_l^j(t_d)}, \chi) \Big\|_{\mathbf{P}_l^j}^2} \bigg\} \tag{2-5} \end{equation} \]

此外，在每次優化之后，利用估計得到的\(t_d\)對相機數據流的時間戳進行修正，即新時間戳\(t_{cam}'\)

\[\begin{equation} t_{cam}'=t_{cam}\color{red}-t_d \tag{2-6} \end{equation} \]

  原文獻中為"\(t_{cam}'=t_{cam}\color{red}+t_d\)"，我認為存在錯誤，比如假設\(t_d\)為正數，式(2-2)表示在\(t_k\)時刻，特征點\(l\)的位置向未來運動了\(t_d\)時間，即認為采樣時刻為\(t_k - t_d\)，所以，對所有的時間戳糾正，使其更接近采樣時間的方式應為\(t_{cam}'=t_{cam}\color{red}-t_d\)。
  下一次優化中，\(t_d\)表示修正后的相機時間戳和IMU時間戳之間的偏差，這個偏差在逐次優化中將不斷收斂到零，從而使得“特征點在短時間（比如\(t_d\)）內在圖像平面內的運動為勻速運動”的假設更加合理，這也可以看成是一種由粗到細corse-to-fine的模式。

2.2 實驗

  文獻作者已經將此方法開源集成在VINS-Mono系統中，其實驗也是基於VINS-Mono系統的一些實驗。作者從兩個角度設計實驗：

• 評估本方法時間偏差標定效果
• 評估本方法對VIO系統性能提升效果

2.2.1 時間偏差標定實驗

  首先，作者設計了仿真實驗。在仿真實驗環境下，相機與IMU的時間戳偏差可調，並且GroudTruth值可知。作者的結果也顯示出其能夠有效估計時間偏差，在30ms的時間偏差下，時間偏差\(t_d\)估計的RMSE為0.68ms，具體數據參考原文，不在此贅述。
  其次，設計實驗估計真實傳感器的時間偏差。使用的傳感器為RealSense ZR300系列相機（筆者剛好也使用過這款傳感器），這是一款相對廉價（幾千元級別）的消費級傳感器。作者分別使用本文方法和Kalibr工具箱進行相機和IMU時間偏差標定。Kalibr標定方式相對本文方法有兩個缺點，一是需要棋盤格Pattern，二是離線標定。也就是說Kalibr依賴外部環境和離線整體優化，雖然這樣，本方法得到了和Kalibr接近的標定結果，並且多次估計的一致性更好，見圖2-2。

 圖2-2 本方法和Kalibr對比

2.2.2 VIO系統性能實驗

作者利用3個實驗，評估使用本方法進行時間偏差修正后VIO系統性能的改善：

1. 在時間嚴格對齊的數據集（Euroc數據集）上，人為添加時間戳偏差，對比本方法和VINS-Mono
2. 在時間嚴格對齊的數據集（Euroc數據集）上，人為添加時間戳偏差，對比本方法和OKVIS
3. 使用真實傳感器，在線標定並修正時間偏差，對比本方法和OKVIS

  實驗1結果顯示，人為添加的偏差越大，VINS-Mono表現越差，而本方法可以准確估計時間偏差，因此性能不受影響，保持VINS-Mono系統最高的精度，見圖2-3。實驗2結果顯示OKVIS也明顯受時間不對齊的影響，當人為設置的時間偏差較大時，OKVIS的表現明顯變差，具體數據分析參看原文，基於實驗2，作者還對\(t_d\)的收斂時間進行分析，表明大約在幾秒（3秒左右）的時間內，\(t_d\)就已經收斂。實驗3表明本方法適用於廉價傳感器，此類傳感器中的IMU一般都是幾美元價格的消費級IMU，此類IMU的測量數據噪聲較大給VIO系統和時間偏差標定帶來挑戰，但是本方法依然能夠有效提升VIO系統性能，在長時間長距離運行的情形下，優勢更加明顯。

 圖2-3 對比本方法和VINS-Mono在不同時間偏差下的表現

2.3 筆者評價

  看完整個方法，第一感覺是這個方法也沒什么高級，很簡單呀，然而，相比Kalibr時間標定方法而言，本方法在線實時標定和無需標定pattern的優勢特別明顯，並且，實驗結果也表明效果也非常好。不得不對這樣一個既簡單又有明顯成效的方法表示贊嘆，作者當之無愧獲得Best Student Paper的獎項。
  斗膽猜測一下作者提出此方法的思路，關於時間偏差，我們估計的偏差為IMU和相機數據之間的時間戳偏差，只需要估計兩者之間的相對偏差，即比如作者假設IMU的時間戳是對的，那么圖像數據對應的時間戳是有偏差的。基於這樣的思考，那就想如何糾正圖像數據的時間戳偏差呢？兩條路徑：

1. 能否得到圖像對應的准確時間戳呢？

看起來不能直接得到，這和我們估計時間偏差是一個問題

2. 能不能得到對應時間戳的圖像呢？

看起來也不容易，但是圖像上不是每個像素都是我們關心的，我們只關心特征點像素位置，只要能夠估計特征點像素在對應時間戳的位置，問題就解決了。那么正如作者，假設像素的運動時勻速的（每個像素各自的運動時勻速的），就可以估計對應時間戳的所有特征點位置。

  那么我們可否從另一個角度假設圖像的時間戳是對的，即認為IMU的時間戳是存在偏差的呢？那么需要在IMU的預積分項中引入\(t_d\)，比如，在預積分起始時間\(t_i\)處向后多做\(t_d\)時長的積分，在預積分結束的地方\(t_j\)處向后砍掉\(t_d\)時長積分，想想公式會比假設特征點勻速運動復雜很多。
  最后，美中不足，作者沒有對\(t_d\)的可觀測性作出一些數學的分析，感覺是比較遺憾的。聯想起李名揚的文章一般會有一些變量可觀測的分析，這樣顯得更加嚴謹，更有數學之美。

參考文獻

[1] Qin T, Shen S. Online Temporal Calibration for Monocular Visual-Inertial Systems[J]. arXiv preprint arXiv:1808.00692, 2018.