趙中堂，吳慶濤

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

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

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

圖1 視覺慣性導(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的高斯游走過程；Ω(ω)表示ω的四元數(shù)乘法矩陣，即

(3)

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

(4)

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

(5)

(6)

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

(7)

1.2 狀態(tài)傳播

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

圖2 MSCKF約束框架示意圖

在雙目視覺慣性導(dǎo)航中，MSCKF的主要目的是估計(jì)IMU的全局方位和位置，通過與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è)量值是通過對(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)通過極限幾何計(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分別表示了無障礙物情況和有障礙物情況下兩種算法的軌跡跟蹤誤差，縱坐標(biāo)為軌跡橫向吻合誤差，橫坐標(biāo)表示軌跡延伸方向。從圖5可以看出本文算法采用三焦點(diǎn)張量有效實(shí)現(xiàn)初始化和后續(xù)導(dǎo)航之間的無縫過渡，跟蹤效果良好；從圖6可以看出本文算法在MSCKF基礎(chǔ)上融合Sigma濾波器，有效提高軌跡估計(jì)的精度和跟蹤魯棒性。

圖5 無障礙物軌跡跟蹤誤差

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

由于系統(tǒng)引入了慣性元件，且通過Sigma濾波器的方法有效抑制了測(cè)量誤差，雙目視覺的初始位姿更加穩(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)，無需計(jì)算實(shí)際場(chǎng)景中的三維點(diǎn)，直接通過連續(xù)幀圖之間的約束條件實(shí)現(xiàn)初始化估計(jì)；

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

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