基于多狀態(tài)卡爾曼濾波的雙目視覺(jué)導(dǎo)航設(shè)計(jì)

趙中堂，吳慶濤

(鄭州航空工業(yè)管理學(xué)院智能工程學(xué)院， 鄭州 450015)

狀態(tài)與參數(shù)的聯(lián)合估計(jì)在軍事和民用方面均有十分廣泛的運(yùn)用?？柭悶V波器，例如擴(kuò)展卡爾曼、無(wú)跡卡爾曼、兩階段卡爾曼和魯棒卡爾曼等濾波器，經(jīng)常用于處理狀態(tài)與參數(shù)聯(lián)合估計(jì)的問(wèn)題。

最常見(jiàn)的緊耦合視覺(jué)慣性導(dǎo)航是在濾波狀態(tài)下增加三維特征位置，同時(shí)估計(jì)姿態(tài)和三維點(diǎn)，但該方法由于新的觀測(cè)特征增加導(dǎo)致計(jì)算復(fù)雜度增加。為解決該問(wèn)題，多狀態(tài)約束卡爾曼濾波器(Multi-State Constrained Kalman Filter，MSCKF)通過(guò)滑動(dòng)窗口代替估計(jì)狀態(tài)向量中標(biāo)志的位置，從而保持恒定的計(jì)算需求。與傳統(tǒng)的擴(kuò)展卡爾曼濾波(Extended KalmanFilter，EKF)視覺(jué)慣性導(dǎo)航方法相比，MSCKF方法可以在較大的導(dǎo)航范圍內(nèi)兼顧視覺(jué)慣性導(dǎo)航的效率和精度。在MSCKF的基礎(chǔ)上，文獻(xiàn)[1-2]對(duì)其進(jìn)行了改進(jìn)，提高了MSCKF的精度和在線校準(zhǔn)性能。然而，由于狀態(tài)更新滑動(dòng)窗口的最小長(zhǎng)度限制，MSCKF中的初始化問(wèn)題仍未得到解決。文獻(xiàn)[3]基于無(wú)跡卡爾曼濾波器實(shí)現(xiàn)相機(jī)與IMU(Inertial Measurement Unit)之間的自校準(zhǔn)，傳感器間耦合度低，只適用于單目視覺(jué)。通常情況下，為了實(shí)現(xiàn)高質(zhì)態(tài)初始化，傳感器模塊最好保持靜止一段時(shí)間。為提高M(jìn)SCKF的初始化性能，本文提出了一種基于三焦點(diǎn)張量約束的初始化MSCKF方法。該方法將連續(xù)三幅圖像之間的約束條件應(yīng)用于初始狀態(tài)估計(jì)，同時(shí)完成姿態(tài)計(jì)算，以滿足MSCKF的最小滑動(dòng)窗口約束。此外，為了提高系統(tǒng)的初始化精度，采用Sigma濾波器對(duì)非線性系統(tǒng)進(jìn)行逼近，可以有效改善算法魯棒性和初始化精度，進(jìn)而使得雙目視覺(jué)慣性導(dǎo)航具有更優(yōu)異的性能。改進(jìn)的MSCKF算法結(jié)合三焦點(diǎn)張量和Sigma濾波器進(jìn)行初始化并對(duì)后續(xù)狀態(tài)估計(jì)進(jìn)行無(wú)縫切換，進(jìn)而通過(guò)雙目視覺(jué)慣性導(dǎo)航輸出狀態(tài)估計(jì)，其實(shí)現(xiàn)框圖如圖1。

圖1 視覺(jué)慣性導(dǎo)航實(shí)現(xiàn)框圖

1 狀態(tài)估計(jì)和MSCKF

1.1 IMU傳播模型

從IMU測(cè)量中進(jìn)行狀態(tài)傳播，本文的IMU狀態(tài)參數(shù)取為16維矢量，即

(1)

利用狀態(tài)的連續(xù)時(shí)間動(dòng)力學(xué)，可得：

(2)

其中，Ga是全局幀的加速因子；nbω和nba分別表示偏差bω和ba的高斯游走過(guò)程；Ω(ω)表示ω的四元數(shù)乘法矩陣，即

(3)

在慣性傳感器中給定陀螺儀參數(shù)，其輸出包含給定的偏差bω和ba以及高斯白點(diǎn)噪聲nω和na。通過(guò)從陀螺儀中測(cè)得的參數(shù)ωm和am，可以由下式計(jì)算得到實(shí)際的角速度ω和實(shí)際的加速度a，即

(4)

根據(jù)式(2)，可得連續(xù)時(shí)間模型為

(5)

(6)

GδθI是誤差四元數(shù)δq的角度偏差，15維IMU誤差通過(guò)下式計(jì)算獲得。

(7)

1.2 狀態(tài)傳播

與視覺(jué)輔助慣性EKF融合方法不同[5]，MSCKF算法不向狀態(tài)向量添加新特征，當(dāng)特征值失效或者附加位姿達(dá)到最大閾值時(shí)，執(zhí)行更新程序。換句話說(shuō)，MSCKF的狀態(tài)向量保持了相機(jī)姿態(tài)的滑動(dòng)窗口，如圖2所示，n個(gè)攝像頭姿態(tài)和m個(gè)特征彼此可見(jiàn)。因此，滑動(dòng)窗口中的所有相機(jī)位姿受特征fi(i=1,2,…,m)約束。

圖2 MSCKF約束框架示意圖

在雙目視覺(jué)慣性導(dǎo)航中，MSCKF的主要目的是估計(jì)IMU的全局方位和位置，通過(guò)與IMU的剛性連接可以得到相機(jī)的狀態(tài)。在k時(shí)刻，全狀態(tài)向量包括當(dāng)前IMU狀態(tài)估計(jì)，全狀態(tài)向量表示為

(8)

(9)

(10)

在濾波器預(yù)測(cè)中，因?yàn)镮MU的狀態(tài)傳播測(cè)量值是通過(guò)對(duì)其進(jìn)行離散化獲得，所以可以認(rèn)為陀螺儀和加速度計(jì)的信號(hào)為一定時(shí)間間隔的采樣值[6]。結(jié)合式(2)和式(5)，IMU的運(yùn)動(dòng)學(xué)誤差可表示為

(11)

因此，IMU狀態(tài)誤差的線性化連續(xù)時(shí)間模型表示為

(12)

(13)

(14)

其中，I3表示維度為3×3的單位矩陣。

1.3 狀態(tài)增強(qiáng)

(15)

假設(shè)MSCKF的狀態(tài)已經(jīng)包含相機(jī)的n個(gè)位姿，則第n+1個(gè)狀態(tài)的協(xié)方差矩陣pk|k為

(16)

其中，I6n+15是(6n+15)×(6n+15)為單位矩陣，根據(jù)式(15)可得雅各布式Jk為：

(17)

1.4 協(xié)方差傳播

(18)

考慮到計(jì)算的復(fù)雜性，采用歐拉積分法對(duì)采樣時(shí)間Δt進(jìn)行積分。因此，離散誤差狀態(tài)變換矩陣Φ(tk+Δt,tk)為

Φ(tk+Δt,tk)=I15+FΔt

(19)

(20)

1.5 狀態(tài)修正

(21)

(22)

根據(jù)狀態(tài)中的各個(gè)位姿的觀測(cè)特征，計(jì)算出的測(cè)量誤差為

(23)

2 三焦點(diǎn)張量約束

2.1 對(duì)極幾何約束

圖3 兩視圖對(duì)極幾何約束基本模型示意圖

2.2 三焦點(diǎn)張量

在對(duì)極幾何約束下，三焦點(diǎn)張量涵蓋三種不同視圖之間的幾何關(guān)系，如圖4所示。它可以將兩個(gè)視圖中的對(duì)應(yīng)點(diǎn)轉(zhuǎn)換為第三個(gè)視圖中的對(duì)應(yīng)點(diǎn)[9]。分析兩個(gè)視圖中的極線約束，三焦點(diǎn)張量的本質(zhì)是三維空間中點(diǎn)-線-點(diǎn)對(duì)應(yīng)的幾何約束，是一種對(duì)位姿的強(qiáng)約束。

圖4 三焦點(diǎn)張量之間的幾何關(guān)系示意圖

因此，為了提高M(jìn)SCKF的初始魯棒性，基于三焦點(diǎn)張量的狀態(tài)矢量表示為

(24)

(25)

2.3 初始狀態(tài)更新

相機(jī)三視圖投影矩陣記為：p1=[I|0Z,p2=[A′|a4],p3=[B′|b4]，其中，A′和B′分別為3×3矩陣，向量a4和b4分別表示各自相機(jī)的投影矩陣[10]。因此，根據(jù)投影矩陣和三維空間中的直線，三焦點(diǎn)張量標(biāo)表示為

(26)

(27)

由于三焦點(diǎn)張量主要集中在三個(gè)連續(xù)幀圖像上，假設(shè)三幀圖像的對(duì)應(yīng)特征點(diǎn)為{m1,m2,m3}，根據(jù)極限約束和三焦點(diǎn)張量約束可以推導(dǎo)出測(cè)量模型。因此，三幀圖像之間的兩個(gè)測(cè)量模型為

(28)

其中，(h1,h2)通過(guò)極限幾何計(jì)算獲得，然后根據(jù)IMU獲取觀測(cè)位姿(GRC,GpC)。

綜上所述，總測(cè)量值z(mì)in表示為

(29)

3 Sigma濾波器

(30)

(31)

其中，λ=?2(n+k)-n；?為大于0的小常量[13]，本文取值0.2；k取值為3-n；高斯分布ρ=2。

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

算法運(yùn)行平臺(tái)為配備Intel Core i7處理器，內(nèi)存為8G的筆記本電腦，實(shí)驗(yàn)數(shù)據(jù)為MH_05_difficult數(shù)據(jù)文件。

圖5、圖6分別表示了無(wú)障礙物情況和有障礙物情況下兩種算法的軌跡跟蹤誤差，縱坐標(biāo)為軌跡橫向吻合誤差，橫坐標(biāo)表示軌跡延伸方向。從圖5可以看出本文算法采用三焦點(diǎn)張量有效實(shí)現(xiàn)初始化和后續(xù)導(dǎo)航之間的無(wú)縫過(guò)渡，跟蹤效果良好；從圖6可以看出本文算法在MSCKF基礎(chǔ)上融合Sigma濾波器，有效提高軌跡估計(jì)的精度和跟蹤魯棒性。

圖5 無(wú)障礙物軌跡跟蹤誤差

圖6 有障礙物軌跡跟蹤誤差

由于系統(tǒng)引入了慣性元件，且通過(guò)Sigma濾波器的方法有效抑制了測(cè)量誤差，雙目視覺(jué)的初始位姿更加穩(wěn)定，可以有效估計(jì)目標(biāo)軌跡與障礙物的相對(duì)位置。預(yù)設(shè)100個(gè)停車點(diǎn)位，圖7和圖8分別位置和角度的跟蹤估計(jì)誤差，位置誤差維持在±1 mm，角度誤差維持在±1°，糾偏響應(yīng)很快。

圖7 位置跟蹤估計(jì)誤差

圖8 角度跟蹤估計(jì)誤差

5 結(jié)論

1) 針對(duì)MSCKF的初始化階段的缺陷，采用三焦點(diǎn)張量和Sigma濾波器優(yōu)化初始化狀態(tài)，無(wú)需計(jì)算實(shí)際場(chǎng)景中的三維點(diǎn)，直接通過(guò)連續(xù)幀圖之間的約束條件實(shí)現(xiàn)初始化估計(jì)；

2) 利用Sigma濾波器對(duì)非線性系統(tǒng)進(jìn)行逼近，既實(shí)現(xiàn)和后續(xù)導(dǎo)航之間的無(wú)縫過(guò)渡，又提高了系統(tǒng)狀態(tài)估計(jì)的精度和魯棒性；

3) 在工業(yè)雙目視覺(jué)慣性導(dǎo)航領(lǐng)域具有應(yīng)用前景。