雙目視覺與慣性測量單元相對姿態在線標定

施佳良，宋來亮，江澤

摘 要：為了對兩種傳感器進行更好的融合，以實現更高的組合導航定位精度，文章基于多狀態約束卡爾曼濾波器，提出了一種能夠在線標定雙目相機與IMU相對姿態誤差的方法。

關鍵詞：視覺導航;在線標定;緊耦合;慣性測量單元;多狀態約束卡爾曼濾波

傳統GPS/INS組合導航方式在一些區域無法實現組合導航定位，比如在高樓林立的樓宇間以及山區等地方，而視覺圖像技術則可以在上述區域實現導航定位。雖然通過視覺圖像技術可以獲得較豐富的場景信息，但容易受到外界因素干擾;慣性導航技術雖然有累積誤差，但是頻率高、數據穩定、短期精度較高。如果將兩者有效結合，則可以實現對載體精度較高的運動狀態估計[1]。

視覺圖像技術與慣性導航技術分別依賴于相機和慣性測量單元（Inertial Measurement Unit，IMU），如果能夠準確估計相機和IMU之間的相對位姿，對于提高相機和IMU兩種傳感器之間的數據融合精度非常關鍵[2]。

通常，對于兩種傳感器相對姿態和位置的確定是依賴于標定板等其他外部輔助設備，但對于兩種傳感器相對位姿的標定結果會由于時間推移等因素影響變得不夠準確。如果直接將兩者之前確定的坐標關系代入視覺慣導融合算法中，會導致融合定位的效果非常不好，甚至會使估計的結果發散[3]。因此，準確地估計相機和IMU之間的相對位姿有重要的意義。

傳統的相機-IMU標定多是離線標定，并依賴于標定板等其他輔助設備。通常，離線標定可以分為基于優化與基于濾波兩種方法，基于濾波器的方法相較于基于優化的方法[3-4]，計算量通常比較小;基于優化的方法為了防止計算量過大，采用了滑動窗口優化的方法。

在線標定相比于離線標定，不需要標定板等其他輔助設備，成本更低、操作更容易，不過算法難度較大。

本文基于多狀態卡爾曼濾波，實現了雙目視覺與慣性測量單元緊耦合在線標定，與傳統的離線標定相機-IMU的方法相比，本算法不依賴于任何標定設備就能夠在環境未知的情況下，通過運動激勵在線估計相機與慣性測量單元相對位姿。

1 系統描述

全局坐標系：OGXGYGZG;IMU坐標系：OIXIYIZI;相機坐標系：OCXCYCZC。

特征點在全局，IMU與相機3個坐標系下分別表示為，，，這3個參數都是待估計量。表示IMU在全局坐標系下的位置與姿態。表示特征點在全局坐標系下的位置與姿態，待標定的相機與IMU的相對位姿用表示，表示歸一化四元數。

2 MSCKF算法

多狀態約束下的卡爾曼濾波器（Multi-State Constraint Kalman Filter，MSCKF）算法的流程如下：

（1）IMU積分，先利用IMU的加速度計和陀螺儀測得的加速度和角速度的信息對狀態向量中的IMU狀態進行預測，一般會處理多幀的IMU觀測數據。

（2）相機狀態擴增，每收到一張圖片后，計算當前相機狀態并加入到狀態向量中，同時擴充狀態協方差矩陣。

（3）特征點三角化，根據歷史相機狀態三角化估計3D特征點在全局坐標系下的坐標。

（4）特征更新，再利用特征點對多個歷史相機狀態的約束，來更新狀態向量。

（5）歷史相機狀態移除，如果相機的狀態個數超過N，則剔除最老或最近的相機狀態以及對應的協方差。

關于初始化，保持載體（IMU）靜止將前200幀加速度和角速度求平均，平均加速度的模值g作為重力加速度，平均角速度作為陀螺儀的零偏，計算重力向量（0，0，﹣g）和平均加速度之間的夾角（通過旋轉四元數），標定初始時刻IMU系與全局坐標系之間的夾角。

3 視覺/慣性緊耦合模型

IMU的狀態向量如下：

（1）

其中，bg和ba分別是慣性測量單元在全局坐標系下的陀螺儀和加速度計的零偏。IMU的誤差狀態向量表示如下：

（2）

由于最后對應的N個相機狀態也被增廣到狀態向量中，得到整個誤差狀態向量，包含一個IMU狀態向量以及N個相機狀態：

（3）

， （4）

3.1 過程模型

式（5）和式（6）分別表示IMU的加速度計和陀螺儀的量測模型，沒有考慮地球自轉角速度：

（5）

（6）

wm和am分別為陀螺和加表在IMU坐標系下的量測值，ng和na是陀螺儀和加速度計的零均值高斯白噪聲。

由于IMU的更新頻率是相機的十幾倍，所以兩幀圖像之間的IMU更新離散化如式。其中，帶上三角表示估計值。表示四元數轉旋轉矩陣。

（7）

（8）

（9）

其中：

（10）

（11）

（12）

（13）

線性連續IMU誤差狀態模型：

（14）

狀態轉移矩陣以及噪聲方差矩陣的離散化處理：

（15）

（16）

其中，是系統的連續時間噪聲方差矩陣。表示離散時間系統IMU狀態轉移矩陣。表示離散時間系統中IMU的噪聲方差陣。誤差狀態模型離散化得：

（17）

是IMU中傳感器的噪聲。

計算IMU狀態的一步預測方差陣公式表示如式（18）：

（18）

整體狀態的一步預測方差陣表示如式（19）：

（19）

3.2 觀測模型

IMU與GPS組合過程中，GPS可以直接給出載體位置GpI的觀測量，而視覺提供的不是位置信息。在視覺當中，約束通常是特征點到相機的重投影誤差（空間中一個3D特征點根據相機的姿態和位置投影到相機平面，與實際觀測的特征點之間像素的誤差），通過重投影誤差約束等來構建觀測模型。

整個系統的量測用zm，k表示，由k時刻對應的m個特征點的像素坐標構成，系統的量測模型如式（20）所示：

（20）

式（21）構建的是簡化后的雙目相機視覺測量模型：

（21）

其中，和分別表示特征點fj在左相機和右相機下的像素坐標，為了方便表達，這里的像素坐標是沒有考慮內參矩陣以及畸變系數的坐標值。表示特征點fj對應的相機第i次觀測，j對應一幀圖像里特征點的個數。測量殘差可以表示如式（22）：

（22）

式（23）表示利用IMU的狀態估計以及相機和IMU之間的外參估計，最終得到點在相機坐標系下的坐標， 是上述MSCKF算法中通過第3個步驟估計的。

（23）

上述變量線性化后可得殘差表示如式（24）：

（24）

上述公式（24）是單個相機對單個特征點的觀測模型，，分別是殘差對狀態向量和特征點的雅可比矩陣，其中：

（25）

式（25）中是相機狀態對應的雅可比矩陣。

對單個特征點對應的所有相機觀測模型進行合并，可以得到單個特征點的觀測模型：

（26）

根據MSCKF算法并未包含在狀態向量中，所以希望將特征點從觀測模型中移除，移除方式是觀測模型兩邊乘以的左零空間，左零空間定義為：

（27）

對所有的特征點殘差模型進行合并，得到整體的殘差模型：

（28）

最后得到MSCKF的最優增益為：

（29）

狀態方程的修正量：

ΔX=Krn （30）

狀態協方差矩陣的更新量：

（31）

式（31）中，ξ=6N+21是協方差矩陣的維數，6N對應的是相機在全局坐標系下的姿態和位置狀態[4]。

4 實驗與分析

數據集中，左右相機與IMU之間的相對位置和姿態的真實值是已知的（提前離線標定過），如表1所示。

采集設備的坐標關系如圖1所示。左相機繞先沿X軸正向移動5 cm，使得相機坐標系中心與IMU坐標系中心重合，最后繞Z軸逆時針旋轉90°（真實值），使兩者坐標系完全重合，左相機并不繞X軸或Y軸旋轉。

同理，右相機先沿X軸負向移動5 cm，使得相機坐標系中心與IMU坐標系中心重合。最后繞Z軸逆時針旋轉90°（真實值），使兩者坐標系完全重合，同理其他兩個自由度姿態沒有變化。這里，繞Z軸變換表示橫滾角發生變化[5]。

首先，設置左相機和IMU之間的初始相對姿態相對于相機與IMU真實的相對姿態之間的歐拉角偏差，其中，只在繞Z軸的姿態變化上疊加了1°誤差，橫滾、俯仰以及航向角誤差表示如下：[-1°，0°，0°]。

同理，設置的右相機和IMU之間的初始相對姿態相對于相機與IMU真實的相對姿態之間的橫滾角，俯仰角以及航向角偏差為[-2.25°，0°，0°]。同時，圖像噪聲設為噪聲水平為1個像素的高斯白噪聲。

如圖2—3所示，左相機與IMU的相對姿態和真實兩者的相對姿態之間的誤差最終收斂于0，右相機與IMU的相對姿態和真實兩者的相對姿態誤差也最終收斂于0，且誤差收斂速度快。

5 結語

本文提出了在初始誤差較大的情況下，雙目相機與IMU相對姿態的在線標定方法。通過實驗表明，該方法有效且準確。不同傳感器之間的數據同步對于視覺慣性組合系統數據融合的影響也很大，除了使用硬件同步的方式對數據進行同步處理外，也可以考慮把時間同步誤差也加入到狀態量里面，進行實時估計。

作者簡介：施佳良（1994— ），男，陜西西安人，碩士研究生;研究方向：慣性導航，組合導航。

[參考文獻]

[1]SUN K，MOHTA K，PFROMMER B，et al.Robust stereo visual inertial odometry for fast autonomous flight[J].IEEE Robotics and Automation Letters，2018（2）：965-972.

[2]楊浩，張峰，葉軍濤.攝像機和慣性測量單元的相對位姿標定方法[J].機器人，2011（4）：419-426.

[3]YANG H，ZHANG F，YE J.A Camera-IMU relative pose calibration method[J].Jiqiren/Robot，2011（4）：419-426.

[4]BRINK K，SOLOVIEV A.Filter-based calibration for an IMU and multi-camera system[C].Washington：Proceedings of the 2012 IEEE/ION Position，Location and Navigation Symposium，2012.

[5]MIRZAEI F M，ROUMELIOTIS S I.A kalman filter-based algorithm for IMU-Camera calibration：observability analysis and performance evaluation[J].IEEE Transactions on Robotics，2008（5）：1143-1156.

Online calibration method for stereo cameras-IMU relative pose

Shi Jialiang1， Song Lailiang2， Jiang Ze3

（1.Xian Institute of Aerospace Precision Mechatronics， Xian 710100， China; 2.Beihang University， School of Instrumentation and Optoelectronic Engineering， Beijing 100083， China）

Abstract：In order to combine these two kinds of sensors and achieve better fusion results， this paper puts forward a method which can precisely calibrate the relative pose between the stereo cameras and IMU online.

Key words：visual navigation; online calibration; tightly-coupling; inertial measurement unit; multi states constraint kalman filter