基于多狀態卡爾曼濾波的雙目視覺導航設計

趙中堂，吳慶濤

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

狀態與參數的聯合估計在軍事和民用方面均有十分廣泛的運用。卡爾曼類濾波器，例如擴展卡爾曼、無跡卡爾曼、兩階段卡爾曼和魯棒卡爾曼等濾波器，經常用于處理狀態與參數聯合估計的問題。

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

圖1 視覺慣性導航實現框圖

1 狀態估計和MSCKF

1.1 IMU傳播模型

從IMU測量中進行狀態傳播，本文的IMU狀態參數取為16維矢量，即

(1)

利用狀態的連續時間動力學，可得：

(2)

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

(3)

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

(4)

根據式(2)，可得連續時間模型為

(5)

(6)

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

(7)

1.2 狀態傳播

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

圖2 MSCKF約束框架示意圖

在雙目視覺慣性導航中，MSCKF的主要目的是估計IMU的全局方位和位置，通過與IMU的剛性連接可以得到相機的狀態。在k時刻，全狀態向量包括當前IMU狀態估計，全狀態向量表示為

(8)

(9)

(10)

在濾波器預測中，因為IMU的狀態傳播測量值是通過對其進行離散化獲得，所以可以認為陀螺儀和加速度計的信號為一定時間間隔的采樣值[6]。結合式(2)和式(5)，IMU的運動學誤差可表示為

(11)

因此，IMU狀態誤差的線性化連續時間模型表示為

(12)

(13)

(14)

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

1.3 狀態增強

(15)

假設MSCKF的狀態已經包含相機的n個位姿，則第n+1個狀態的協方差矩陣pk|k為

(16)

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

(17)

1.4 協方差傳播

(18)

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

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

(19)

(20)

1.5 狀態修正

(21)

(22)

根據狀態中的各個位姿的觀測特征，計算出的測量誤差為

(23)

2 三焦點張量約束

2.1 對極幾何約束

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

2.2 三焦點張量

在對極幾何約束下，三焦點張量涵蓋三種不同視圖之間的幾何關系，如圖4所示。它可以將兩個視圖中的對應點轉換為第三個視圖中的對應點[9]。分析兩個視圖中的極線約束，三焦點張量的本質是三維空間中點-線-點對應的幾何約束，是一種對位姿的強約束。

圖4 三焦點張量之間的幾何關系示意圖

因此，為了提高MSCKF的初始魯棒性，基于三焦點張量的狀態矢量表示為

(24)

(25)

2.3 初始狀態更新

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

(26)

(27)

由于三焦點張量主要集中在三個連續幀圖像上，假設三幀圖像的對應特征點為{m1,m2,m3}，根據極限約束和三焦點張量約束可以推導出測量模型。因此，三幀圖像之間的兩個測量模型為

(28)

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

綜上所述，總測量值zin表示為

(29)

3 Sigma濾波器

(30)

(31)

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

4 實驗結果與分析

算法運行平臺為配備Intel Core i7處理器，內存為8G的筆記本電腦，實驗數據為MH_05_difficult數據文件。

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

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

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

由于系統引入了慣性元件，且通過Sigma濾波器的方法有效抑制了測量誤差，雙目視覺的初始位姿更加穩定，可以有效估計目標軌跡與障礙物的相對位置。預設100個停車點位，圖7和圖8分別位置和角度的跟蹤估計誤差，位置誤差維持在±1 mm，角度誤差維持在±1°，糾偏響應很快。

圖7 位置跟蹤估計誤差

圖8 角度跟蹤估計誤差

5 結論

1) 針對MSCKF的初始化階段的缺陷，采用三焦點張量和Sigma濾波器優化初始化狀態，無需計算實際場景中的三維點，直接通過連續幀圖之間的約束條件實現初始化估計；

2) 利用Sigma濾波器對非線性系統進行逼近，既實現和后續導航之間的無縫過渡，又提高了系統狀態估計的精度和魯棒性；

3) 在工業雙目視覺慣性導航領域具有應用前景。