汪志剛，郭 杭，敖龍輝

(南昌大學(xué)信息工程學(xué)院，南昌 330031)

室內(nèi)機(jī)器人的導(dǎo)航定位問題是機(jī)器人研究的一大核心課題。室內(nèi)環(huán)境下，衛(wèi)星信號弱，衛(wèi)星導(dǎo)航定位方法并不適用。一般利用各種傳感器得到機(jī)器人初始位姿和各個(gè)時(shí)刻的運(yùn)動狀態(tài)，來推算機(jī)器人位姿，實(shí)現(xiàn)室內(nèi)機(jī)器人定位。

視覺傳感器能獲得環(huán)境圖像信息或深度信息，被廣泛適用于各種定位算法中，如半直接單目視覺里程計(jì)(semi-direct monocular visual odometry，SVO)[1]、多功能、高精度的單目同步定位與地圖構(gòu)建系統(tǒng)(oriented FAST and rotated BRIEF-simultaneous localization and mapping,ORB-SLAM)[2]、大尺度直接單目同步定位與地圖構(gòu)建系統(tǒng)(large-scale direct monocular-simultaneous localization and mapping,LSD-SLAM)[3]、直接稀疏里程計(jì)(direct sparse odometry,DSO)[4]。除此之外，慣性測量單元(inertial measurement unit, IMU)由于抗干擾能力強(qiáng)、短期定位結(jié)果精確等優(yōu)點(diǎn)，成為慣性導(dǎo)航系統(tǒng)[5-6]的關(guān)鍵傳感器。在室內(nèi)機(jī)器人的導(dǎo)航定位研究中，結(jié)合視覺信息和慣性數(shù)據(jù)的視覺慣性組合導(dǎo)航方法有著很大的發(fā)展前景[7-9]。一方面，相機(jī)能夠捕捉場景中豐富的信息，而IMU能夠在短時(shí)間內(nèi)高頻地獲得精準(zhǔn)的估計(jì)，減輕動態(tài)物體對相機(jī)的影響；另一方面，視覺信息能有效地修正IMU的零漂。一定程度上，兩種傳感器互補(bǔ)，搭配使用能得到更好的定位結(jié)果。

針對室內(nèi)機(jī)器人的定位問題，提出一種緊耦合的視覺慣性組合定位方法。本文方法將IMU的位姿等狀態(tài)量與雙目相機(jī)的位姿等狀態(tài)量合并在一起，共同構(gòu)建優(yōu)化函數(shù)，然后進(jìn)行狀態(tài)估計(jì)。并提出了一種新的基于Vicon[10-12]的IMU內(nèi)參標(biāo)定方法。Vicon是高精度的光學(xué)動作捕捉系統(tǒng)，常用Vicon數(shù)據(jù)作為數(shù)據(jù)集的真值，并以此評估其他定位系統(tǒng)的定位精度。

1 慣性測量單元

1.1 參考坐標(biāo)系

如圖1所示，本文方法主要涉及4個(gè)坐標(biāo)系，分別為地球慣性坐標(biāo)系{E}、Vicon參考坐標(biāo)系{W}、Vicon體坐標(biāo)系{V}以及IMU體坐標(biāo)系{I}，不同坐標(biāo)系的變換為

圖1 參考坐標(biāo)系

1.2 預(yù)積分

采用預(yù)積分技術(shù)[13]，如圖2所示，將Vicon和IMU的數(shù)據(jù)融合，計(jì)算兩幀之間的相對位置、速度、姿態(tài)之間的變化。由于Vicon跟IMU采集頻率相同，于是對Vicon數(shù)據(jù)進(jìn)行降采樣，降至30 Hz。

圖2 Vicon和IMU的采集頻率對比

對于連續(xù)兩個(gè)關(guān)鍵幀ik和ik+1,它們的對應(yīng)時(shí)刻分別為k和k+1,從k時(shí)刻的狀態(tài)對IMU測量值進(jìn)行積分得到k+1時(shí)刻的狀態(tài)，即

(1)

(2)

(3)

式中：

可由Vicon數(shù)據(jù)直接得到，表示載體在Vicon參考坐標(biāo)系下的速度、位置和姿態(tài)；w、a為IMU的陀螺儀和加速度計(jì)的測量值；gw為Vicon參考坐標(biāo)系下的重力矢量；Δtk為k～k+1的時(shí)間間隔；bg為陀螺儀的零偏估計(jì)參數(shù)；ba為加速度計(jì)的零偏估計(jì)參數(shù)。

為

(4)

(5)

(6)

式中：

為預(yù)積分的值，表示載體在IMU體坐標(biāo)系下的位置、速度、姿態(tài)變化量。它們只和IMU的bias有關(guān)，則當(dāng)bias變化很小的時(shí)候，使用一階線性展開公式，求得

(7)

(8)

(9)

在初始時(shí)刻

(10)

(11)

(12)

式中：i為在[k,k+1]中測量值對應(yīng)的離散時(shí)刻,δt為測量值在[k,k+1]之間的時(shí)間間隔。

1.3 協(xié)方差矩陣與雅可比矩陣

離散形式下有誤差傳遞過程，k+1時(shí)刻下的狀態(tài)量誤差來源主要是前一時(shí)刻狀態(tài)量的誤差傳播以及各種噪聲。采用誤差隨時(shí)間變化的遞推方程，則t時(shí)刻誤差項(xiàng)的導(dǎo)數(shù)為

(13)

式(13)中：Ft、Gt為兩個(gè)時(shí)刻間的協(xié)方差矩陣；

為加速度計(jì)和陀螺儀的高斯白噪聲；

為加速度計(jì)和陀螺儀隨機(jī)游走bias的高斯白噪聲。

根據(jù)導(dǎo)數(shù)的定義，由式(13)得出IMU誤差運(yùn)動方程為

(14)

式(14)表示了下一時(shí)刻IMU測量誤差與上一時(shí)刻的測量誤差成線性關(guān)系，那么根據(jù)當(dāng)前時(shí)刻的狀態(tài)，可以預(yù)測下一時(shí)刻的IMU均值和協(xié)方差，協(xié)方差預(yù)測公式為

(Gtδt)Q(Gtδt)ηt

(15)

通過式(14)，使用迭代的方式求得誤差項(xiàng)的雅可比矩陣為

Jt+δt=(I+Ftδt)Jt

(16)

式(16)中：雅可比矩陣的初值Jik=I。

1.4 殘差

預(yù)積分殘差表示一段時(shí)間內(nèi)預(yù)積分量作為測量值，對兩時(shí)刻之間的狀態(tài)量進(jìn)行約束，如式(17)所示。其中，等式右邊第3項(xiàng)是關(guān)于四元數(shù)的旋轉(zhuǎn)誤差，[·]xyz表示只取四元數(shù)的虛部(x,y,z)組成的三維向量。

(17)

1.5 IMU內(nèi)參標(biāo)定

首先最小化相對姿態(tài)殘差，得到陀螺儀的零偏估計(jì)參數(shù)bg,即

(18)

然后固定bg,最小化相對速度殘差和相對位置殘差，得到加速度計(jì)的零偏估計(jì)參數(shù)ba,即

(19)

2 視覺數(shù)據(jù)處理

2.1 視覺觀測值殘差

視覺觀測值殘差是特征點(diǎn)在當(dāng)前幀的重投影誤差，這里選用切平面重投影誤差。

(20)

式(20)中：π-1為利用攝像機(jī)內(nèi)參將像素位置轉(zhuǎn)換成單位向量的反投影函數(shù)。

2.2 視覺慣性聯(lián)合初始化

視覺慣性組合定位方法是個(gè)非線性程度很高的系統(tǒng)，因此初始化直接影響整個(gè)緊耦合系統(tǒng)的魯棒性和定位精度。由于IMU的內(nèi)參是基于Vicon數(shù)據(jù)求得，比較精確，故只對速度和重力加速度初始化，即

(21)

3 視覺慣性組合方法

3.1 優(yōu)化函數(shù)

視覺慣性組合定位方法是將視覺和慣性的原始觀測量進(jìn)行有效組合，將滑動窗口內(nèi)的狀態(tài)量(包括位置、姿態(tài)、速度、IMU 零偏、相機(jī)-IMU 外參等)放到一個(gè)狀態(tài)向量中，即

(22)

式(22)中：第1個(gè)式子為滑動窗口內(nèi)整個(gè)狀態(tài)向量，n為幀數(shù)，m為滑動窗口中的特征點(diǎn)總數(shù)；第2個(gè)式子表示第k幀圖像捕捉到的IMU狀態(tài)，包含位姿、速度、旋轉(zhuǎn)等；第3個(gè)式子為相機(jī)外參；λ為特征點(diǎn)的逆深度。

針對狀態(tài)向量，引入邊緣化技術(shù)，建立非線性優(yōu)化函數(shù)為

(23)

式(23)中：第1部分為IMU在相鄰兩幀之間產(chǎn)生的殘差；第2部分為特征點(diǎn)在相機(jī)下的投影產(chǎn)生的視覺殘差；第3部分為邊緣化殘差，表示從滑動窗口中去掉的位姿和特征點(diǎn)的約束。

3.2 邊緣化和舒爾補(bǔ)理論

邊緣化[14]的目的是防止位姿和特征點(diǎn)的個(gè)數(shù)隨著時(shí)間不斷增加，使得優(yōu)化函數(shù)始終在一個(gè)有限的復(fù)雜度內(nèi)。邊緣化在移除位姿或特征點(diǎn)的時(shí)候，將相關(guān)聯(lián)的約束轉(zhuǎn)變成一個(gè)約束項(xiàng)作為先驗(yàn)信息放到優(yōu)化函數(shù)中。

邊緣化來源于概率論中的邊際分布，在非線性優(yōu)化中，邊緣化將概率問題變成最小二乘法問題，即

(24)

式(24)中：δx為滑動窗口內(nèi)的變量；b表示邊緣化殘差。假設(shè)邊緣化δxa,保留δxb,通過舒爾補(bǔ)理論直接得到：

(25)

通過舒爾補(bǔ)理論[14]完成了對δxa邊緣化的操作，同時(shí)不會丟失變量間的約束信息。

4 實(shí)驗(yàn)

4.1 實(shí)驗(yàn)環(huán)境

在實(shí)驗(yàn)室中搭建模擬室內(nèi)環(huán)境(圖3)，在HUSKY A200平臺上搭載領(lǐng)晰雙目相機(jī)和Vicon，操作系統(tǒng)選用ubuntu18.04版本。其中，領(lǐng)晰雙目攝像頭設(shè)備包含兩個(gè)快門攝像機(jī)以及一個(gè)九軸IMU傳感，相機(jī)頻率為60幀，IMU頻率為200幀。

圖3 實(shí)驗(yàn)環(huán)境和模擬機(jī)器人

首先通過EuRoc[15]數(shù)據(jù)集實(shí)驗(yàn)對IMU內(nèi)參標(biāo)定方法進(jìn)行驗(yàn)證，EuRoc數(shù)據(jù)集本身提供了基于多狀態(tài)約束下的卡爾曼濾波器(multi-state constraint Kalman filter，MSCKF)融合濾波方法得到的IMU內(nèi)參真值，可以看作真值；然后對視覺慣性組合定位方法的準(zhǔn)確性和穩(wěn)定性進(jìn)行驗(yàn)證，在模擬環(huán)境中分別隨機(jī)驅(qū)動機(jī)器人行走5圈、1圈以及半圈。

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

表1和表2分別是加速度計(jì)和陀螺儀的零偏估計(jì)參數(shù)的標(biāo)定結(jié)果。結(jié)果顯示，本方法對陀螺儀的內(nèi)參標(biāo)定相當(dāng)準(zhǔn)確，大部分結(jié)果都與EuRoc提供的真值很接近。而加速度計(jì)的標(biāo)定結(jié)果則有不小的偏差，但也處于同一范圍內(nèi)，這是因?yàn)榧铀俣扔?jì)測量的加速度內(nèi)包含重力，采用運(yùn)動標(biāo)定的方式無法忽略這種影響?？傊?，基于Vicon的IMU內(nèi)參標(biāo)定方法是有效的，可以應(yīng)用于視覺慣性組合定位系統(tǒng)。

表1 加速度計(jì)的標(biāo)定結(jié)果

表2 陀螺儀的標(biāo)定結(jié)果

圖4為機(jī)器人在模擬環(huán)境下行走5圈、1圈以及半圈的實(shí)驗(yàn)結(jié)果，分別為瑞立視跟蹤系統(tǒng)(Realis tracking system，RTS)高精度光學(xué)動作捕捉設(shè)備采集的真值數(shù)據(jù)軌跡和本文方法的實(shí)驗(yàn)結(jié)果軌跡。可以看出，本文提出的視覺慣性組合系統(tǒng)與真實(shí)軌跡幾乎貼合，具有極高的穩(wěn)定性，平均誤差在0.1 m以內(nèi)，完全可以滿足室內(nèi)移動機(jī)器人的定位要求。

圖4 真值軌跡與實(shí)驗(yàn)軌跡對比

5 結(jié)論

提出一種緊耦合的視覺慣性組合定位方法，在搭建的模擬環(huán)境中，分別對IMU內(nèi)參標(biāo)定效果、定位精度及系統(tǒng)穩(wěn)定性進(jìn)行驗(yàn)證，IMU內(nèi)參標(biāo)定能得到比較準(zhǔn)確的真值，定位的平均誤差能控制在 0.1 m以內(nèi)，為室內(nèi)移動機(jī)器人的定位提供了一種新的可借鑒方案。當(dāng)然也存在可以改進(jìn)的地方，如對加速度計(jì)的零偏估計(jì)參數(shù)時(shí)需要考慮重力的影響以及需要提高IMU數(shù)據(jù)與相機(jī)數(shù)據(jù)時(shí)間同步精確度。