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

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

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

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

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

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

改進(jìn)Kalman濾波；慣性導(dǎo)航；視覺(jué)導(dǎo)航

0 引言

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

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

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

1 導(dǎo)航原理

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

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

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

2.1 陀螺誤差模型

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

ε=εc+ωg+εr

(1)

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

2.2 加速度誤差模型

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

(2)

2.3 相機(jī)誤差模型

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

ζ=ωc

(3)

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

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

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

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

(4)

(5)

(6)

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

(7)

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

(8)

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

2.4.2 量測(cè)方程的建立

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

(9)

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

2.5 位置濾波方程

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

假設(shè)目標(biāo)航天器運(yùn)行在圓形軌道,追蹤航天器相對(duì)目標(biāo)航天器的相對(duì)運(yùn)動(dòng)學(xué)方程在軌道坐標(biāo)系中的表示為

(10)

(11)

又有

(12)

(13)

式中,vd為加速度計(jì)的測(cè)量噪聲；d為加速度計(jì)的漂移偏置,且滿足

(14)

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

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

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

控制向量U(t)為

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

(15)

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

控制向量矩陣B(t)為

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

2.5.2 量測(cè)方程的建立

相機(jī)進(jìn)來(lái)的數(shù)據(jù)經(jīng)過(guò)處理后,得到目標(biāo)航天器相對(duì)追蹤航天器的相對(duì)位置在追蹤航天器中的表示,即

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

故量測(cè)矩陣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è)等時(shí)間間隔采樣,采樣間隔Ts=tk+1-tk(k=0,1,2…)。一般Kalman濾波周期Ts較短,可以近似看成Ts→0,則F(t),B(t),G(t)可以看作常數(shù)陣,即有

(18)

Qk是濾波器噪聲序列的方差強(qiáng)度矩陣,一般為非負(fù)定陣,即

或

將量測(cè)方程離散化為

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

(19)

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

或

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

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

1)時(shí)間更新過(guò)程

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

(20)

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

(21)

2)量測(cè)更新過(guò)程

濾波的增益矩陣

(22)

狀態(tài)的估計(jì)

(23)

(24)

時(shí)間更新過(guò)程等價(jià)本課題中的慣性導(dǎo)航,量測(cè)更新過(guò)程等價(jià)視覺(jué)導(dǎo)航。當(dāng)視覺(jué)信息可靠時(shí),才啟用視覺(jué)導(dǎo)航,即進(jìn)行時(shí)間更新和量測(cè)更新；當(dāng)視覺(jué)信息不可靠時(shí),隔離視覺(jué)導(dǎo)航,只進(jìn)行時(shí)間更新。視覺(jué)信息的可靠性由量測(cè)噪聲方差陣Rk衡量。

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

(25)

Pk=Pk,k-1

(26)

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

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

4 仿真驗(yàn)證與分析

仿真參數(shù)的選取

1)相機(jī)

采樣時(shí)間：160ms；

相對(duì)姿態(tài)測(cè)量噪聲方差：1.95×10-6rad2；

相對(duì)位置測(cè)量噪聲方差：1.0×10-4m2。

2)陀螺儀

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

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

測(cè)量白噪聲：均值為0,方差為0.1。

3)加速度計(jì)

采樣時(shí)間：20ms；隨機(jī)常值偏置：0.005m/s2；

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

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

4)位置濾波器

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

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

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

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

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

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

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

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

量測(cè)噪聲的協(xié)方差矩陣為R=diag[10-510-510-5]。

為了驗(yàn)證改進(jìn)Kalman濾波算法對(duì)提高濾波性能的作用,需對(duì)Simulink仿真模型進(jìn)行修改。濾波器中所使用的量測(cè)信息都來(lái)自于視覺(jué)信息,故需對(duì)相機(jī)信息進(jìn)行處理。相機(jī)信息的輸出周期為160ms,將其輸入采樣周期為20ms延時(shí)模塊,得到的相機(jī)信息整體滯后了20ms,再將這2個(gè)信號(hào)相減得到差值信號(hào)。以相對(duì)位置的x軸方向?yàn)槔?處理過(guò)程如圖2所示。

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

從圖 2可以看出,在一個(gè)相機(jī)信號(hào)周期內(nèi),差值信號(hào)的前20ms是有值的,后140ms保持為0。當(dāng)差值信號(hào)有值時(shí),認(rèn)為是某一濾波周期里的視覺(jué)信息較上一個(gè)濾波周期進(jìn)行了更新,這種更新能真實(shí)反映兩航天器之間的相對(duì)位姿變化；當(dāng)差值信號(hào)為0時(shí),表明視覺(jué)信息在剩下的140ms一直得不到更新,而這個(gè)時(shí)間段內(nèi)真實(shí)的相對(duì)位姿是變化的。根據(jù)這個(gè)特點(diǎn)可以在前20ms時(shí)使用相機(jī)信息進(jìn)行量測(cè)更新,后140ms內(nèi)隔離相機(jī)信息。

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

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

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

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

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

接下來(lái)研究視覺(jué)信息中斷對(duì)濾波效果的影響。在仿真程序中,取50～55s這一時(shí)間段的相機(jī)信號(hào)為0,給出了IKF的各個(gè)估計(jì)誤差情況。為了清晰地表示出視覺(jué)信息中斷過(guò)程,這里給出10～100s的曲線,如圖3～圖5所示。

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

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

圖3 視覺(jué)信息失效時(shí)改進(jìn)Kalman濾波器的位置估計(jì)誤差Fig.3 The position estimation error with improved Kalman filter while visual information failure

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

5 結(jié)論

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

[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)/俄羅斯的空間交會(huì)對(duì)接技術(shù)[J]. 太空探索,2011(10)：34-36.

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

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

[6] 李耀軍,潘泉,趙春暉, 等.基于INS/ SMNS緊耦合的無(wú)人機(jī)導(dǎo)航[J].中國(guó)慣性技術(shù)學(xué)報(bào), 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] 劉燾,龐秀芝,余靜,等. 基于準(zhǔn)三維視覺(jué)模型的組合導(dǎo)航系統(tǒng)性能研究[J].彈箭與制導(dǎo)學(xué)報(bào),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

國(guó)家自然基金(70971030)

屈楨深(1973-),男,副教授,博導(dǎo),主要從事視覺(jué)導(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