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

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

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

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

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

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

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

改進Kalman濾波；慣性導(dǎo)航；視覺導(dǎo)航

0 引言

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

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

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

1 導(dǎo)航原理

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

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

2 數(shù)學(xué)模型

2.1 陀螺誤差模型

陀螺能輸出追蹤航天器的本體坐標系相對于慣性坐標系的姿態(tài)角速度在追蹤航天器本體坐標系中的分量,其優(yōu)點是可靠性高,依靠自主導(dǎo)航,短時間內(nèi)精度較好,能夠提供較高頻率的姿態(tài)信息。根據(jù)大量的統(tǒng)計規(guī)律,陀螺儀的誤差主要由以下三部分構(gòu)成：

ε=εc+ωg+εr

(1)

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

2.2 加速度誤差模型

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

(2)

2.3 相機誤差模型

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

ζ=ωc

(3)

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

2.4 姿態(tài)濾波方程

2.4.1 狀態(tài)方程的建立

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

(4)

(5)

(6)

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

(7)

系統(tǒng)噪聲矩陣為

(8)

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

2.4.2 量測方程的建立

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

(9)

又由四元數(shù)和歐拉角之間的關(guān)系,得到量測方程

2.5 位置濾波方程

2.5.1 狀態(tài)方程的建立

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

(10)

(11)

又有

(12)

(13)

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

(14)

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

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

系統(tǒng)噪聲矩陣W(t)為

控制向量U(t)為

那么,位置濾波器的狀態(tài)方程為

(15)

式中,狀態(tài)轉(zhuǎn)移矩陣F(t)為

控制向量矩陣B(t)為

系統(tǒng)噪聲驅(qū)動矩陣G(t)為

2.5.2 量測方程的建立

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

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

故量測矩陣H(t)為

2.6 濾波方程的離散化

將狀態(tài)方程離散化為

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

(16)

或

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

(17)

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

(18)

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

或

將量測方程離散化為

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

(19)

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

或

3 改進Kalman濾波算法的推導(dǎo)

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

1)時間更新過程

狀態(tài)的一步預(yù)測

(20)

一步預(yù)測誤差的方差陣

(21)

2)量測更新過程

濾波的增益矩陣

(22)

狀態(tài)的估計

(23)

(24)

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

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

(25)

Pk=Pk,k-1

(26)

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

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

4 仿真驗證與分析

仿真參數(shù)的選取

1)相機

采樣時間：160ms；

相對姿態(tài)測量噪聲方差：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；估值誤差協(xié)方差陣的初始值為P0=10-3·I9×9。

狀態(tài)向量的初值為X0=[-21.5091000000]T。

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

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

5)姿態(tài)濾波器

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

狀態(tài)向量的初值為X0=[0001000]T。

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

量測噪聲的協(xié)方差矩陣為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可以看出,在一個相機信號周期內(nèi),差值信號的前20ms是有值的,后140ms保持為0。當差值信號有值時,認為是某一濾波周期里的視覺信息較上一個濾波周期進行了更新,這種更新能真實反映兩航天器之間的相對位姿變化；當差值信號為0時,表明視覺信息在剩下的140ms一直得不到更新,而這個時間段內(nèi)真實的相對位姿是變化的。根據(jù)這個特點可以在前20ms時使用相機信息進行量測更新,后140ms內(nèi)隔離相機信息。

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

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

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

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

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

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

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

將視覺信息中斷情景置于常規(guī)Kalman濾波器中,會出現(xiàn)估計誤差發(fā)散的情況,導(dǎo)致整個系統(tǒng)不能正常運行。只要視覺信息中斷的時間不太長,基于改進Kalman濾波算法的組合導(dǎo)航算法能保證估計誤差收斂,滿足系統(tǒng)最基本的穩(wěn)定性要求。

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

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

5 結(jié)論

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

[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] 邊廣龍. 蘇聯(lián)/俄羅斯的空間交會對接技術(shù)[J]. 太空探索,2011(10)：34-36.

[4] 陳林. 基于動態(tài)視覺定位的慣性導(dǎo)航地標修正方法研究[D]. 長沙：國防科學(xué)技術(shù)大學(xué),2007.

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

[6] 李耀軍,潘泉,趙春暉, 等.基于INS/ SMNS緊耦合的無人機導(dǎo)航[J].中國慣性技術(shù)學(xué)報, 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] 劉燾,龐秀芝,余靜,等. 基于準三維視覺模型的組合導(dǎo)航系統(tǒng)性能研究[J].彈箭與制導(dǎo)學(xué)報,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-),男,副教授,博導(dǎo),主要從事視覺導(dǎo)航方面的研究。E-mail：ocicq@126.com

10.19306/j.cnki.2095-8110.2017.02.003

U666.12

A

2095-8110(2017)02-0014-07