一種基于改進Kalman濾波的視覺/慣性組合導航算法

屈楨深,楚翔宇,趙霄洋,李葆華

(1.哈爾濱工業大學空間控制與慣性技術研究中心,哈爾濱 150001； 2. 北京空間飛行器總體設計部, 北京 100094)

一種基于改進Kalman濾波的視覺/慣性組合導航算法

屈楨深1,楚翔宇1,趙霄洋2,李葆華1

(1.哈爾濱工業大學空間控制與慣性技術研究中心,哈爾濱 150001； 2. 北京空間飛行器總體設計部, 北京 100094)

提出了一種基于改進Kalman濾波的視覺/慣性直接組合導航算法,首先分析了相關坐標系的定義和敏感器的誤差模型,然后分別構建了姿態濾波方程和位置濾波方程,接著對濾波方程進行離散化處理以便在計算機中實現運算,最后采用該方法進行了仿真驗證,結果表明系統在不同情況下進行組合導航都能夠滿足任務要求。

改進Kalman濾波；慣性導航；視覺導航

0 引言

視覺/慣性組合導航技術在進入21世紀后逐漸成為研究熱點,在車載導航、無人機導航等方面獲得了不少成果[1-3]。2007年,為了提高車載雷達的導航精度,陳林等提出了一種基于地標定位的INS/Vision組合導航方法,利用相機對地標識別和動態定位來確定慣性導航系統(INS)的位置,最后采用Kalman濾波將視覺數據和慣導數據進行融合,來對慣導系統誤差進行修正[4]。2009年,針對無人機編隊leader-follower(領航-跟隨)模式,崔乃剛等提出了一種解決視覺解算系統輸出存在滯后的INS/Vision相對導航法,采用無跡Kalman濾波估計leader與follower之間的相對位置、速度和姿態,結合INS姿態角信息來檢測跑道特征點,求解飛機相對位置矢量[5]。2010年,為了減小無人機INS漂移誤差累積的影響,李耀軍等提出了基于INS/SMNS緊耦合的無人機導航模式,無人機的粗定位靠IMU的位姿信息完成,航拍圖像可以實時校正IMU的積累誤差,提高了景像匹配導航系統的實時性和適配區景象匹配的精確度[6]。2011年,為了解決相機低精度造成的遠距離導航誤差,馮國虎等提出了一種基于單目視覺/慣性組合導航定位算法,觀測量采用的是INS解算的速度和視覺計算的速度之差,利用Kalman濾波修正導航信息[7]。

近幾年來,國外的學者利用Kalman濾波的各種衍生技術,在對應用視覺/慣性器件進行導航的系統進行濾波研究時,獲得了較多的研究成果。針對視覺/慣性組合導航方式的不同采樣率問題,國內外許多學者提出了不同的信息融合算法。

Strelow在進行濾波研究時提出,可以使用改進的EKF算法對視覺/慣性組合導航系統進行濾波,在使用算法對狀態進行估計時,判斷量測內容是否更新了,如果這一時刻沒有量測內容更新,那么只利用上一時刻的量測值進行時間上的更新；如果這一時刻量測內容有更新,那么既進行時間更新又進行量測更新[8]。Wu和Smyth提出在進行慣性導航濾波的過程中,位置和加速度信號應該采取不同的數據更新頻率,通常在進行位置信息更新時采用低頻采樣防止高頻噪聲,在進行加速度信息更新時采用高頻采樣以達到高精度測量,在進行濾波更新時經常采用較高的采樣頻率作為Kalman濾波的濾波頻率[9]。Rehbinder和Ghosh在解決快速移動中的視覺導航問題時,利用高頻陀螺測量信息和延遲的低頻視覺量測信息估計了相機在慣性坐標系中的位置和姿態[10]。

1 導航原理

組合導航一般分為直接法和間接法,根據系統設計的特點,本文采用直接法。直接組合導航算法是直接將慣性和相機信息進行融合,在濾波中Kalman濾波器接收各敏感器的輸出量,經過濾波計算可以得到導航參數的最優估計。濾波過程主要由2個子濾波器構成,將加速度計的加速度信息和相機的相對位置信息作為位置濾波器的輸入,陀螺儀的角速度信息和相機的相對姿態信息作為姿態濾波器的輸入,通過Kalman濾波器進行最優濾波估計,估計出最優的相對位置、相對姿態參數,基本原理圖如圖1所示。

圖1 直接組合導航法的基本原理圖Fig.1 The basic principle of direct combined navigation method

2 數學模型

2.1 陀螺誤差模型

陀螺能輸出追蹤航天器的本體坐標系相對于慣性坐標系的姿態角速度在追蹤航天器本體坐標系中的分量,其優點是可靠性高,依靠自主導航,短時間內精度較好,能夠提供較高頻率的姿態信息。根據大量的統計規律,陀螺儀的誤差主要由以下三部分構成：

ε=εc+ωg+εr

(1)

式中,εc為陀螺儀隨機常值漂移；ωg為陀螺儀的白噪聲漂移,均方差為σωg；εr為陀螺儀的隨機游走,漂移斜率白噪聲的均方差為σεr。

2.2 加速度誤差模型

加速度計能輸出加速度在追蹤航天器本體坐標系中的比力投影,其優點與陀螺儀相同,兩者一起組成的慣組單元是航天器導航的首選配置。在大量數據的統計下,加速度計的誤差由以下三部分構成

(2)

2.3 相機誤差模型

基于相機的視覺導航系統誤差主要由測量系統的標定誤差、光學鏡頭的幾何畸變誤差、參數求解的舍入誤差等組成,在進行數學仿真時,把各種隨機誤差都以白噪聲的形式表示,這種等價對位置測量和姿態測量都適合。誤差形式如下

ζ=ωc

(3)

式中,ωc為相機的白噪聲誤差,均方差為σωc。

2.4 姿態濾波方程

2.4.1 狀態方程的建立

對于航天器運動來說,其姿態方程的描述不僅可以使用歐拉角方法,還可以使用方向余弦陣方法、羅德里格斯參數方法、等效旋轉矢量法和四元數方法來進行描述,雖然使用歐拉角所描述的姿態方程其物理意義較為明確,但存在奇異性問題,并且三角函數運算比較麻煩。使用四元數表示坐標變換相對簡單,比用9個方向余弦表示簡潔,更適合對航天器姿態進行實時控制,因此姿態運動學方程和陀螺儀誤差方程可得：

(4)

(5)

(6)

則式(4)中的狀態變量X(t)可以表示為

(7)

系統噪聲矩陣為

(8)

整理式(7)和式(8)可得,狀態方程是非線性的,表示為

2.4.2 量測方程的建立

姿態濾波器輸入的量測值是相機信息經過處理后得到的相對姿態角θ1、θ2、θ3(3-2-1轉序),即

(9)

又由四元數和歐拉角之間的關系,得到量測方程

2.5 位置濾波方程

2.5.1 狀態方程的建立

假設目標航天器運行在圓形軌道,追蹤航天器相對目標航天器的相對運動學方程在軌道坐標系中的表示為

(10)

(11)

又有

(12)

(13)

式中,vd為加速度計的測量噪聲；d為加速度計的漂移偏置,且滿足

(14)

式中,va為加速度計偏置斜率白噪聲。

由式(13)和式(14)整理可得,選取狀態變量為

系統噪聲矩陣W(t)為

控制向量U(t)為

那么,位置濾波器的狀態方程為

(15)

式中,狀態轉移矩陣F(t)為

控制向量矩陣B(t)為

系統噪聲驅動矩陣G(t)為

2.5.2 量測方程的建立

相機進來的數據經過處理后,得到目標航天器相對追蹤航天器的相對位置在追蹤航天器中的表示,即

狀態向量中的相對位置(xH,yH,zH)表示的是追蹤航天器相對目標航天器在軌道坐標系中的分量,則有

故量測矩陣H(t)為

2.6 濾波方程的離散化

將狀態方程離散化為

Xk+1=Φk+1,kXk+Γk+1,kWk

(16)

或

Xk+1=Φk+1,kXk+Ψk+1.kUk+Γk+1,kWk

(17)

式中,Φk+1,k是狀態一步轉移陣。在本課題中,假設等時間間隔采樣,采樣間隔Ts=tk+1-tk(k=0,1,2…)。一般Kalman濾波周期Ts較短,可以近似看成Ts→0,則F(t),B(t),G(t)可以看作常數陣,即有

(18)

Qk是濾波器噪聲序列的方差強度矩陣,一般為非負定陣,即

或

將量測方程離散化為

Zk+1=Hk+1Xk+1+Vk+1

(19)

由于量測方程的離散化形式中的每個量都是在k+1時刻的值,故Hk與H(t)的形式一樣。Zk+1是系統的觀測值,由外界輸入到濾波器中。Vk+1是濾波器的量測噪聲序列,一般滿足高斯白噪聲,即

或

3 改進Kalman濾波算法的推導

改進Kalman濾波算法基于Kalman濾波基本公式的基礎上,將5個基本公式分為2個部分。

1)時間更新過程

狀態的一步預測

(20)

一步預測誤差的方差陣

(21)

2)量測更新過程

濾波的增益矩陣

(22)

狀態的估計

(23)

(24)

時間更新過程等價本課題中的慣性導航,量測更新過程等價視覺導航。當視覺信息可靠時,才啟用視覺導航,即進行時間更新和量測更新；當視覺信息不可靠時,隔離視覺導航,只進行時間更新。視覺信息的可靠性由量測噪聲方差陣Rk衡量。

在濾波時刻k時,若出現相機信息沒有更新或出現中斷,即不可靠,則量測噪聲方差陣Rk被認為趨向無窮大。由式(24)可知,濾波的增益矩陣Kk會趨向于零,那么會有

(25)

Pk=Pk,k-1

(26)

從式(25)和式(26)可以看出,狀態的估計和估計誤差方差陣與k-1時刻相比沒有變化,間接證明沒有進行量測更新。

改進Kalman濾波算法是在之前設計的濾波器基礎上進行改進的,主要增加了對視覺信息的可靠性判斷。若可靠性低,選擇很大的量測噪聲方差陣；若可靠性高,選擇很小的量測噪聲方差陣。由于慣性測量單元在很短時間內的精度是很高的,故在視覺信息可靠性低的情況下可以不進行量測校正,只采用慣性測量單元進行導航。

4 仿真驗證與分析

仿真參數的選取

1)相機

采樣時間：160ms；

相對姿態測量噪聲方差：1.95×10-6rad2；

相對位置測量噪聲方差：1.0×10-4m2。

2)陀螺儀

采樣時間：20ms；隨機常值漂移：0.05(°)/h；

漂移斜率的白噪聲：均值為0,方差為0.01；

測量白噪聲：均值為0,方差為0.1。

3)加速度計

采樣時間：20ms；隨機常值偏置：0.005m/s2；

漂移斜率的白噪聲：均值為0,方差為1.0×10-6；

測量白噪聲：均值為0,方差為1.0×10-4。

4)位置濾波器

濾波周期：20ms；估值誤差協方差陣的初始值為P0=10-3·I9×9。

狀態向量的初值為X0=[-21.5091000000]T。

激勵噪聲的協方差矩陣為Q=diag[10-110-110-110-310-310-3]。

量測噪聲的協方差矩陣為R=diag[10-110-110-1]。

5)姿態濾波器

濾波周期：20ms；估值誤差協方差陣的初始值為P0=10-2·I7×7。

狀態向量的初值為X0=[0001000]T。

激勵噪聲的協方差矩陣為Q=diag[10-210-210-210-210-210-2]。

量測噪聲的協方差矩陣為R=diag[10-510-510-5]。

為了驗證改進Kalman濾波算法對提高濾波性能的作用,需對Simulink仿真模型進行修改。濾波器中所使用的量測信息都來自于視覺信息,故需對相機信息進行處理。相機信息的輸出周期為160ms,將其輸入采樣周期為20ms延時模塊,得到的相機信息整體滯后了20ms,再將這2個信號相減得到差值信號。以相對位置的x軸方向為例,處理過程如圖2所示。

圖2 相對位置x軸方向信息的處理過程Fig.2 Processing of relative position x axis direction information

從圖 2可以看出,在一個相機信號周期內,差值信號的前20ms是有值的,后140ms保持為0。當差值信號有值時,認為是某一濾波周期里的視覺信息較上一個濾波周期進行了更新,這種更新能真實反映兩航天器之間的相對位姿變化；當差值信號為0時,表明視覺信息在剩下的140ms一直得不到更新,而這個時間段內真實的相對位姿是變化的。根據這個特點可以在前20ms時使用相機信息進行量測更新,后140ms內隔離相機信息。

另外,每個系統仿真周期里還需對相機信息進行幅值檢測。例如在交會對接中對兩航天器的相對位置檢測時,判斷相對位置的3個分量是否同時處于小范圍,這個小范圍可以根據具體情況設定,若同時處于小范圍,表明交會過程中出現了異常導致視覺系統產生的相對位置信息出現中斷,這時就需要及時隔離相機信息。

在保證各參數不變的情況下,給出了常規Kalman濾波器(KF)和改進Kalman濾波器(IKF)的估計誤差對比,如表1和表2所示。

表1 各估計誤差的最大值(10～100s)

表2 各估計誤差的均方差RMS(10～100s)

從表1和表2的數據可以看出,改進Kalman濾波器的各個估計誤差量相比于常規Kalman濾波器的估計誤差有明顯下降。雖然姿態角的估計誤差下降程度沒有位置和速度的估計誤差下降程度大,但從整體上看,改進Kalman濾波器對提高濾波效果起到了一定作用。

接下來研究視覺信息中斷對濾波效果的影響。在仿真程序中,取50～55s這一時間段的相機信號為0,給出了IKF的各個估計誤差情況。為了清晰地表示出視覺信息中斷過程,這里給出10～100s的曲線,如圖3～圖5所示。

從圖 3、圖 4和圖 5中可以看出,在視覺信息中斷的時間段內,各估計誤差開始積累,但當視覺信息恢復后,估計誤差迅速收斂,可見改進Kalman濾波器的穩定性較好。這是因為,在視覺信息中斷后,系統判斷出視覺信息不可靠,則相應地選擇很大的量測噪聲方差陣,即等價將視覺信息隔離,不采用視覺信息進行量測更新。由于系統具有冗余性,在視覺信息中斷期間可以單獨使用慣性導航,故會出現圖中50～55s時間段內的誤差積累現象。

將視覺信息中斷情景置于常規Kalman濾波器中,會出現估計誤差發散的情況,導致整個系統不能正常運行。只要視覺信息中斷的時間不太長,基于改進Kalman濾波算法的組合導航算法能保證估計誤差收斂,滿足系統最基本的穩定性要求。

圖3 視覺信息失效時改進Kalman濾波器的位置估計誤差Fig.3 The position estimation error with improved Kalman filter while visual information failure

圖5 視覺信息失效時改進Kalman濾波器的姿態估計誤差Fig.5 The attitude estimation error with improved Kalman filter while visual information failure

5 結論

論文針對近距離交會對接過程中的相對導航問題,采用視覺/慣性組合導航模式,結合了慣性測量單元高精度和視覺測量系統定時更新的優點,可獲得更高的導航精度。由于慣性信息和相機信息的頻率不同,會出現在一個濾波周期中采用不精確的相機信息去校正的情況,故需要對Kalman濾波算法進行改進。主要思想是只有當相機信息精確時才進行校正,若相機信息的可靠性低,則對相機信息進行隔離,避免污染信息。通過數字仿真結果可以看出,基于改進Kalman濾波的組合導航算法的估計誤差明顯減小,并對相機信息中斷情況有較好的魯棒性。

[1] Parten R P, Mayer J P.Development of the Gemini

operational rendezvous plan [J]. Journal of Spacecraft and Rockets, 1968,5(9):1023-1026.

[2] Mueller E, Bilimoria K D, Frost C. Dynamic coupling and control response effectson spacecraft handling qualities during docking[J]. Journal of Spacecraft and Rockets, 2009, 46(6)：1288-1297.

[3] 邊廣龍. 蘇聯/俄羅斯的空間交會對接技術[J]. 太空探索,2011(10)：34-36.

[4] 陳林. 基于動態視覺定位的慣性導航地標修正方法研究[D]. 長沙：國防科學技術大學,2007.

[5] 崔乃剛,王曉剛,郭繼峰. 基于Sigma-pointKalman濾波的INS/VISION相對導航方法研究[J]. 宇航學報,2009,30 (6)：2220-2223.

[6] 李耀軍,潘泉,趙春暉, 等.基于INS/ SMNS緊耦合的無人機導航[J].中國慣性技術學報, 2010,18 (3)：302-306.

[7] Feng G H,Wu W Q,Cao J L, et al. Algorithm for monocular visual Odometry/SINS integrated navigation [J].Journal of Chinese Inertial Technology,2011,19 (3)：210-215.

[8] 劉燾,龐秀芝,余靜,等. 基于準三維視覺模型的組合導航系統性能研究[J].彈箭與制導學報,2009,29 (6)：51-54.

[9] Strelow D W. Motion estimation from image and inertial measurements[D]. Maryland University,2004.

[10] Smyth A, Wu M. Multi-rate Kalman filtering for the data fusion of displacementand acceleration measurements[C].Proceeding of SPIE, Smart Structures and Materials,2006.

[11] Rehbinder H, Ghosh B K. Multi-rate fusion of visualand inertial data[C].Proceedings of IEEE Conference on Multi-Sensor Fusion Integration IntelligentSystems Baden-Baden,Germany,2001：97-102.

A Visual/Inertial Integrated Navigation Algorithm Based on Improved Kalman Filter

QU Zhen-shen1, CHU Xiang-yu1, ZHAO Xiao-yang2, LI Bao-hua1

(1.Space Control and Inertial Technology Research Center, Harbin Institute of Technology, Harbin 150001, China; 2. Beijing Institute of Spacecraft System Engineering, CAST, Beijing 100094, China)

A visual/inertial integrated navigation algorithm based on improved Kalman filter is presented. Firstly, the definition of the relevant coordinate systems and the error models of the sensors are analyzed. Secondly, the attitude filter equation and the position filter equation are established respectively. Then the filter equations are discretized to achieve the operation in the computer. At last, the simulations are performed. The results show that the system can meet the requirements of the task by using the method in different situations.

Improved Kalman filter; Inertial navigation; Visual navigation

2016-10-02；

2016-11-01

國家自然基金(70971030)

屈楨深(1973-),男,副教授,博導,主要從事視覺導航方面的研究。E-mail：ocicq@126.com

10.19306/j.cnki.2095-8110.2017.02.003

U666.12

A

2095-8110(2017)02-0014-07