施佳良，宋來(lái)亮，江澤

摘 要：為了對(duì)兩種傳感器進(jìn)行更好的融合，以實(shí)現(xiàn)更高的組合導(dǎo)航定位精度，文章基于多狀態(tài)約束卡爾曼濾波器，提出了一種能夠在線標(biāo)定雙目相機(jī)與IMU相對(duì)姿態(tài)誤差的方法。

關(guān)鍵詞：視覺(jué)導(dǎo)航;在線標(biāo)定;緊耦合;慣性測(cè)量單元;多狀態(tài)約束卡爾曼濾波

傳統(tǒng)GPS/INS組合導(dǎo)航方式在一些區(qū)域無(wú)法實(shí)現(xiàn)組合導(dǎo)航定位，比如在高樓林立的樓宇間以及山區(qū)等地方，而視覺(jué)圖像技術(shù)則可以在上述區(qū)域?qū)崿F(xiàn)導(dǎo)航定位。雖然通過(guò)視覺(jué)圖像技術(shù)可以獲得較豐富的場(chǎng)景信息，但容易受到外界因素干擾;慣性導(dǎo)航技術(shù)雖然有累積誤差，但是頻率高、數(shù)據(jù)穩(wěn)定、短期精度較高。如果將兩者有效結(jié)合，則可以實(shí)現(xiàn)對(duì)載體精度較高的運(yùn)動(dòng)狀態(tài)估計(jì)[1]。

視覺(jué)圖像技術(shù)與慣性導(dǎo)航技術(shù)分別依賴于相機(jī)和慣性測(cè)量單元（Inertial Measurement Unit，IMU），如果能夠準(zhǔn)確估計(jì)相機(jī)和IMU之間的相對(duì)位姿，對(duì)于提高相機(jī)和IMU兩種傳感器之間的數(shù)據(jù)融合精度非常關(guān)鍵[2]。

通常，對(duì)于兩種傳感器相對(duì)姿態(tài)和位置的確定是依賴于標(biāo)定板等其他外部輔助設(shè)備，但對(duì)于兩種傳感器相對(duì)位姿的標(biāo)定結(jié)果會(huì)由于時(shí)間推移等因素影響變得不夠準(zhǔn)確。如果直接將兩者之前確定的坐標(biāo)關(guān)系代入視覺(jué)慣導(dǎo)融合算法中，會(huì)導(dǎo)致融合定位的效果非常不好，甚至?xí)构烙?jì)的結(jié)果發(fā)散[3]。因此，準(zhǔn)確地估計(jì)相機(jī)和IMU之間的相對(duì)位姿有重要的意義。

傳統(tǒng)的相機(jī)-IMU標(biāo)定多是離線標(biāo)定，并依賴于標(biāo)定板等其他輔助設(shè)備。通常，離線標(biāo)定可以分為基于優(yōu)化與基于濾波兩種方法，基于濾波器的方法相較于基于優(yōu)化的方法[3-4]，計(jì)算量通常比較小;基于優(yōu)化的方法為了防止計(jì)算量過(guò)大，采用了滑動(dòng)窗口優(yōu)化的方法。

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

本文基于多狀態(tài)卡爾曼濾波，實(shí)現(xiàn)了雙目視覺(jué)與慣性測(cè)量單元緊耦合在線標(biāo)定，與傳統(tǒng)的離線標(biāo)定相機(jī)-IMU的方法相比，本算法不依賴于任何標(biāo)定設(shè)備就能夠在環(huán)境未知的情況下，通過(guò)運(yùn)動(dòng)激勵(lì)在線估計(jì)相機(jī)與慣性測(cè)量單元相對(duì)位姿。

1 系統(tǒng)描述

全局坐標(biāo)系：OGXGYGZG;IMU坐標(biāo)系：OIXIYIZI;相機(jī)坐標(biāo)系：OCXCYCZC。

特征點(diǎn)在全局，IMU與相機(jī)3個(gè)坐標(biāo)系下分別表示為，，，這3個(gè)參數(shù)都是待估計(jì)量。表示IMU在全局坐標(biāo)系下的位置與姿態(tài)。表示特征點(diǎn)在全局坐標(biāo)系下的位置與姿態(tài)，待標(biāo)定的相機(jī)與IMU的相對(duì)位姿用表示，表示歸一化四元數(shù)。

2 MSCKF算法

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

（1）IMU積分，先利用IMU的加速度計(jì)和陀螺儀測(cè)得的加速度和角速度的信息對(duì)狀態(tài)向量中的IMU狀態(tài)進(jìn)行預(yù)測(cè)，一般會(huì)處理多幀的IMU觀測(cè)數(shù)據(jù)。

（2）相機(jī)狀態(tài)擴(kuò)增，每收到一張圖片后，計(jì)算當(dāng)前相機(jī)狀態(tài)并加入到狀態(tài)向量中，同時(shí)擴(kuò)充狀態(tài)協(xié)方差矩陣。

（3）特征點(diǎn)三角化，根據(jù)歷史相機(jī)狀態(tài)三角化估計(jì)3D特征點(diǎn)在全局坐標(biāo)系下的坐標(biāo)。

（4）特征更新，再利用特征點(diǎn)對(duì)多個(gè)歷史相機(jī)狀態(tài)的約束，來(lái)更新狀態(tài)向量。

（5）歷史相機(jī)狀態(tài)移除，如果相機(jī)的狀態(tài)個(gè)數(shù)超過(guò)N，則剔除最老或最近的相機(jī)狀態(tài)以及對(duì)應(yīng)的協(xié)方差。

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

3 視覺(jué)/慣性緊耦合模型

IMU的狀態(tài)向量如下：

（1）

其中，bg和ba分別是慣性測(cè)量單元在全局坐標(biāo)系下的陀螺儀和加速度計(jì)的零偏。IMU的誤差狀態(tài)向量表示如下：

（2）

由于最后對(duì)應(yīng)的N個(gè)相機(jī)狀態(tài)也被增廣到狀態(tài)向量中，得到整個(gè)誤差狀態(tài)向量，包含一個(gè)IMU狀態(tài)向量以及N個(gè)相機(jī)狀態(tài)：

（3）

， （4）

3.1 過(guò)程模型

式（5）和式（6）分別表示IMU的加速度計(jì)和陀螺儀的量測(cè)模型，沒(méi)有考慮地球自轉(zhuǎn)角速度：

（5）

（6）

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

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

（7）

（8）

（9）

其中：

（10）

（11）

（12）

（13）

線性連續(xù)IMU誤差狀態(tài)模型：

（14）

狀態(tài)轉(zhuǎn)移矩陣以及噪聲方差矩陣的離散化處理：

（15）

（16）

其中，是系統(tǒng)的連續(xù)時(shí)間噪聲方差矩陣。表示離散時(shí)間系統(tǒng)IMU狀態(tài)轉(zhuǎn)移矩陣。表示離散時(shí)間系統(tǒng)中IMU的噪聲方差陣。誤差狀態(tài)模型離散化得：

（17）

是IMU中傳感器的噪聲。

計(jì)算IMU狀態(tài)的一步預(yù)測(cè)方差陣公式表示如式（18）：

（18）

整體狀態(tài)的一步預(yù)測(cè)方差陣表示如式（19）：

（19）

3.2 觀測(cè)模型

IMU與GPS組合過(guò)程中，GPS可以直接給出載體位置GpI的觀測(cè)量，而視覺(jué)提供的不是位置信息。在視覺(jué)當(dāng)中，約束通常是特征點(diǎn)到相機(jī)的重投影誤差（空間中一個(gè)3D特征點(diǎn)根據(jù)相機(jī)的姿態(tài)和位置投影到相機(jī)平面，與實(shí)際觀測(cè)的特征點(diǎn)之間像素的誤差），通過(guò)重投影誤差約束等來(lái)構(gòu)建觀測(cè)模型。

整個(gè)系統(tǒng)的量測(cè)用zm，k表示，由k時(shí)刻對(duì)應(yīng)的m個(gè)特征點(diǎn)的像素坐標(biāo)構(gòu)成，系統(tǒng)的量測(cè)模型如式（20）所示：

（20）

式（21）構(gòu)建的是簡(jiǎn)化后的雙目相機(jī)視覺(jué)測(cè)量模型：

（21）

其中，和分別表示特征點(diǎn)fj在左相機(jī)和右相機(jī)下的像素坐標(biāo)，為了方便表達(dá)，這里的像素坐標(biāo)是沒(méi)有考慮內(nèi)參矩陣以及畸變系數(shù)的坐標(biāo)值。表示特征點(diǎn)fj對(duì)應(yīng)的相機(jī)第i次觀測(cè)，j對(duì)應(yīng)一幀圖像里特征點(diǎn)的個(gè)數(shù)。測(cè)量殘差可以表示如式（22）：

（22）

式（23）表示利用IMU的狀態(tài)估計(jì)以及相機(jī)和IMU之間的外參估計(jì)，最終得到點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)， 是上述MSCKF算法中通過(guò)第3個(gè)步驟估計(jì)的。

（23）

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

（24）

上述公式（24）是單個(gè)相機(jī)對(duì)單個(gè)特征點(diǎn)的觀測(cè)模型，，分別是殘差對(duì)狀態(tài)向量和特征點(diǎn)的雅可比矩陣，其中：

（25）

式（25）中是相機(jī)狀態(tài)對(duì)應(yīng)的雅可比矩陣。

對(duì)單個(gè)特征點(diǎn)對(duì)應(yīng)的所有相機(jī)觀測(cè)模型進(jìn)行合并，可以得到單個(gè)特征點(diǎn)的觀測(cè)模型：

（26）

根據(jù)MSCKF算法并未包含在狀態(tài)向量中，所以希望將特征點(diǎn)從觀測(cè)模型中移除，移除方式是觀測(cè)模型兩邊乘以的左零空間，左零空間定義為：

（27）

對(duì)所有的特征點(diǎn)殘差模型進(jìn)行合并，得到整體的殘差模型：

（28）

最后得到MSCKF的最優(yōu)增益為：

（29）

狀態(tài)方程的修正量：

ΔX=Krn （30）

狀態(tài)協(xié)方差矩陣的更新量：

（31）

式（31）中，ξ=6N+21是協(xié)方差矩陣的維數(shù)，6N對(duì)應(yīng)的是相機(jī)在全局坐標(biāo)系下的姿態(tài)和位置狀態(tài)[4]。

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

數(shù)據(jù)集中，左右相機(jī)與IMU之間的相對(duì)位置和姿態(tài)的真實(shí)值是已知的（提前離線標(biāo)定過(guò)），如表1所示。

采集設(shè)備的坐標(biāo)關(guān)系如圖1所示。左相機(jī)繞先沿X軸正向移動(dòng)5 cm，使得相機(jī)坐標(biāo)系中心與IMU坐標(biāo)系中心重合，最后繞Z軸逆時(shí)針旋轉(zhuǎn)90°（真實(shí)值），使兩者坐標(biāo)系完全重合，左相機(jī)并不繞X軸或Y軸旋轉(zhuǎn)。

同理，右相機(jī)先沿X軸負(fù)向移動(dòng)5 cm，使得相機(jī)坐標(biāo)系中心與IMU坐標(biāo)系中心重合。最后繞Z軸逆時(shí)針旋轉(zhuǎn)90°（真實(shí)值），使兩者坐標(biāo)系完全重合，同理其他兩個(gè)自由度姿態(tài)沒(méi)有變化。這里，繞Z軸變換表示橫滾角發(fā)生變化[5]。

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

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

如圖2—3所示，左相機(jī)與IMU的相對(duì)姿態(tài)和真實(shí)兩者的相對(duì)姿態(tài)之間的誤差最終收斂于0，右相機(jī)與IMU的相對(duì)姿態(tài)和真實(shí)兩者的相對(duì)姿態(tài)誤差也最終收斂于0，且誤差收斂速度快。

5 結(jié)語(yǔ)

本文提出了在初始誤差較大的情況下，雙目相機(jī)與IMU相對(duì)姿態(tài)的在線標(biāo)定方法。通過(guò)實(shí)驗(yàn)表明，該方法有效且準(zhǔn)確。不同傳感器之間的數(shù)據(jù)同步對(duì)于視覺(jué)慣性組合系統(tǒng)數(shù)據(jù)融合的影響也很大，除了使用硬件同步的方式對(duì)數(shù)據(jù)進(jìn)行同步處理外，也可以考慮把時(shí)間同步誤差也加入到狀態(tài)量里面，進(jìn)行實(shí)時(shí)估計(jì)。

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

[參考文獻(xiàn)]

[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ī)和慣性測(cè)量單元的相對(duì)位姿標(biāo)定方法[J].機(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