逯建軍,任曉軍，孫 偉，郭元江，李 群

(1.海軍裝備部，北京 100000；2.海軍駐航天某院軍代表室，北京 100000； 3.北京自動(dòng)化控制設(shè)備研究所，北京 100074)

慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航方法

逯建軍1,任曉軍2，孫 偉3，郭元江3，李 群3

(1.海軍裝備部，北京 100000；2.海軍駐航天某院軍代表室，北京 100000； 3.北京自動(dòng)化控制設(shè)備研究所，北京 100074)

針對(duì)現(xiàn)有視覺(jué)里程計(jì)測(cè)量噪聲大、匹配精度低、實(shí)時(shí)性差的問(wèn)題，研究一種基于Kalman濾波器的慣性/雙目視覺(jué)里程計(jì)組合導(dǎo)航方法。在視覺(jué)里程計(jì)中引入慣性導(dǎo)航信息，輔助完成實(shí)時(shí)圖截取、搜索區(qū)預(yù)測(cè)、輸出速度校正等功能，提高視覺(jué)里程計(jì)的測(cè)量精度與計(jì)算速度。利用Kalman濾波器，實(shí)現(xiàn)視覺(jué)里程計(jì)對(duì)慣導(dǎo)累計(jì)誤差的修正，提升組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。車載試驗(yàn)結(jié)果表明，慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航的實(shí)時(shí)定位精度優(yōu)于0.5%D(CEP)，具備工程應(yīng)用條件。

雙目視覺(jué)里程計(jì)；慣性導(dǎo)航；組合導(dǎo)航

0 引言

隨著飛行器長(zhǎng)時(shí)間、遠(yuǎn)程航行需求的不斷增強(qiáng)，對(duì)導(dǎo)航系統(tǒng)的精度和自主性要求不斷提高。純慣性導(dǎo)航系統(tǒng)的導(dǎo)航誤差隨時(shí)間積累而不斷增大，不能完全滿足實(shí)際應(yīng)用的需要[1-5]。慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航可在不提高慣性元件精度的情況下，提升導(dǎo)航系統(tǒng)的導(dǎo)航精度，節(jié)約成本。

視覺(jué)里程計(jì)技術(shù)的基本內(nèi)涵[6]為：?jiǎn)文肯鄼C(jī)通過(guò)前后幀圖像的差異，在已知距離的情況下可以測(cè)量出載體的速度。而雙目視覺(jué)里程計(jì)[7-9]技術(shù)利用雙目相機(jī)，通過(guò)左右兩個(gè)相機(jī)圖像差異測(cè)出距離，通過(guò)前后幀圖像的差異測(cè)量出載體的速度。

傳統(tǒng)的視覺(jué)里程計(jì)技術(shù)存在缺陷，受相機(jī)參數(shù)、測(cè)量距離的范圍等影響，視覺(jué)里程計(jì)輸出的速度信息與真實(shí)值將會(huì)存在一定的差異；并且，傳統(tǒng)視覺(jué)里程技術(shù)在實(shí)時(shí)性和精確性方面難以同時(shí)滿足組合導(dǎo)航的需求。

本文針對(duì)視覺(jué)里程計(jì)的技術(shù)特點(diǎn)，在求取圖像差異(圖像匹配)的過(guò)程中使用慣導(dǎo)系統(tǒng)的信息進(jìn)行深組合，可以同時(shí)提升雙目視覺(jué)里程計(jì)的實(shí)時(shí)性和精確性。應(yīng)用組合導(dǎo)航技術(shù)時(shí)，利用Kalman濾波器對(duì)慣性、雙目視覺(jué)里程計(jì)的誤差同時(shí)進(jìn)行估計(jì)，實(shí)現(xiàn)深組合導(dǎo)航。

1 雙目視覺(jué)里程計(jì)算法

1.1 圖像匹配算法

本文使用的圖像匹配算法為歸一化灰度值匹配法，其本質(zhì)是衡量?jī)煞鶊D像相似程度的度量準(zhǔn)則；其過(guò)程是在兩幅圖像中尋找相關(guān)性最高的點(diǎn)作為匹配結(jié)果。

歸一化灰度值匹配算法的計(jì)算公式[10]為

(1)

圖像匹配算法可以獲得同一標(biāo)志點(diǎn)在不同圖像中的相對(duì)偏移量，由相對(duì)漂移量可推算出在現(xiàn)實(shí)場(chǎng)景中的偏移量。

1.2 雙目視覺(jué)測(cè)距算法

雙目測(cè)距，基于空間上存在偏移的雙目圖像，完成相機(jī)到匹配物體(左、右圖像中均存在)的距離測(cè)量。測(cè)距原理示意圖，如圖1所示。

圖1 雙目測(cè)距原理Fig.1 The theory of ranging method by using two cameras

圖1中，Q是待測(cè)距點(diǎn)(某匹配點(diǎn))，其到相機(jī)的垂直距離為R，在左右相機(jī)上形成的像點(diǎn)分別是Q1和Q2。b是雙目相機(jī)的基線長(zhǎng)度，f是相機(jī)焦距，x1-x2是Q點(diǎn)在左右兩幅圖像上像點(diǎn)的位置差，又稱為視差。

利用相似三角形原理，可得相機(jī)到Q點(diǎn)的距離為

(2)

由式(2)可知，利用上述方法的測(cè)距分辨率受到相機(jī)焦距和雙目相機(jī)基線長(zhǎng)度的影響，相機(jī)焦距與基線長(zhǎng)度越短，測(cè)距分辨率越高，但會(huì)導(dǎo)致測(cè)距范圍的降低，需要根據(jù)實(shí)際使用條件來(lái)確定相機(jī)焦距和基線長(zhǎng)度。

1.3 視覺(jué)里程計(jì)算法

視覺(jué)里程計(jì)的基本原理如圖 2所示：載體t1時(shí)刻位于點(diǎn)O1，t2時(shí)刻運(yùn)動(dòng)到O2。在全局世界坐標(biāo)系中，載體位置和姿態(tài)的變化可以由六個(gè)參數(shù)(Δx,Δy,Δz,Δα,Δβ,Δγ)表示，前三個(gè)參數(shù)代表位置的變化，后三個(gè)參數(shù)代表姿態(tài)的變化。如果載體在移動(dòng)過(guò)程中每一幀都能確定其與前一幀時(shí)的位置和姿態(tài) ，那么就自然實(shí)現(xiàn)了載體的定位。對(duì)全局坐標(biāo)系中一點(diǎn)A，載體在t1時(shí)刻觀察到的A的坐標(biāo)為(x1y1z1)T，t2時(shí)刻觀察到的坐標(biāo)為(x2y2z2)T，假如載體姿態(tài)發(fā)生了變化，也就是代表姿態(tài)的航向角、滾轉(zhuǎn)角和俯仰角發(fā)生了變化。此三個(gè)角度的變化(Δα,Δβ,Δγ)T產(chǎn)生了維數(shù)為3×3旋轉(zhuǎn)矩陣R，位置變化(Δx,Δy,Δz)T產(chǎn)生了平移向量T，則有：

(3)

圖2 視覺(jué)里程計(jì)基本原理Fig.2 The basic theory of visual odometry

如果能在相鄰兩幀中實(shí)現(xiàn)像素點(diǎn)的跟蹤，即對(duì)前一幀圖像中的像素點(diǎn)，在下一幀圖像中能找到它的對(duì)應(yīng)點(diǎn)，并利用立體視覺(jué)分別計(jì)算出此像素點(diǎn)在前后兩幀中的三維坐標(biāo)，則根據(jù)式(3)，一對(duì)對(duì)應(yīng)的特征點(diǎn)就能產(chǎn)生三個(gè)方程。這樣，只要存在4個(gè)對(duì)應(yīng)的特征點(diǎn)，就能計(jì)算出旋轉(zhuǎn)矩陣R和平移矩陣T。如果特征點(diǎn)的數(shù)目大于4個(gè)，則需要通過(guò)多個(gè)方程求得(R,T)的最優(yōu)值。

2 慣性輔助視覺(jué)里程計(jì)方法

慣性導(dǎo)航技術(shù)基于牛頓力學(xué)原理，使用陀螺和加速度計(jì)測(cè)量載體運(yùn)動(dòng)的角速度和加速度，獲取載體的姿態(tài)、速度和位置信息，其優(yōu)點(diǎn)是短時(shí)精度高，能夠準(zhǔn)確地測(cè)量載體姿態(tài)變化的情況等。

本文考慮利用慣性導(dǎo)航技術(shù)的優(yōu)點(diǎn)，以提升視覺(jué)里程計(jì)性能為目的，提出幾種方法。

2.1 視覺(jué)里程計(jì)實(shí)時(shí)圖截取方法

為了保證視覺(jué)里程計(jì)測(cè)量的速度范圍達(dá)到最大，希望截取的實(shí)時(shí)圖像在當(dāng)前幀圖像中的位置坐標(biāo)點(diǎn)與在上一幀圖像中的位置坐標(biāo)點(diǎn)的連線的中點(diǎn)位于基準(zhǔn)圖的中心位置坐標(biāo)。本方法主要介紹應(yīng)用于視覺(jué)里程計(jì)的實(shí)時(shí)圖中心坐標(biāo)計(jì)算，為便于分析，將分成兩種情況進(jìn)行討論：

(1)無(wú)航向角變化情況

截取的實(shí)時(shí)圖像中心在當(dāng)前幀圖像中的坐標(biāo)計(jì)算公式如下所示。

(4)

式中，ureal_m、vreal_m表示截取的實(shí)時(shí)圖像中心在當(dāng)前幀圖像中的坐標(biāo)；unow_m、vnow_m表示當(dāng)前時(shí)刻圖像中心坐標(biāo)；duimu、dvimu表示利用慣性信息計(jì)算得到的特征點(diǎn)偏移。

(2)有航向角變化情況

有航向角變化情況如圖 3所示。

圖3 實(shí)時(shí)圖截取示意圖Fig.3 The schematic diagram of real-time images capture

如圖 3所示，實(shí)線邊框?yàn)樯弦粫r(shí)刻圖像，命名為Pbefore，代表基準(zhǔn)圖；點(diǎn)劃線邊框?yàn)闊o(wú)航向角變化情況的現(xiàn)時(shí)刻圖像，命名為Pnow_o；虛線邊框?yàn)橛泻较蚪亲兓闆r的現(xiàn)時(shí)刻圖像，命名為Pnow。Obefore為基準(zhǔn)圖中心位置，也代表上一時(shí)刻載體中心位置；Onow為現(xiàn)時(shí)刻圖中心位置，也代表現(xiàn)時(shí)刻載體中心位置；Δφ表示兩幀圖像采樣間隔時(shí)間內(nèi)航向角變化，Obefore到Onow連線的中點(diǎn)為期望實(shí)時(shí)圖截取位置，用五角星表示。

若按照式(4)計(jì)算實(shí)時(shí)圖中心在當(dāng)前時(shí)刻圖像中的坐標(biāo)，由于航向角發(fā)生變化，將會(huì)導(dǎo)致實(shí)際實(shí)時(shí)圖截取位置偏離期望實(shí)時(shí)圖截取位置，故需要根據(jù)航向角變化情況，調(diào)整實(shí)時(shí)圖中心坐標(biāo)，使得實(shí)時(shí)圖截取位置與期望實(shí)時(shí)圖截取位置一致。為方便計(jì)算速度，定義兩個(gè)新變量：ureal_m_cut、vreal_m_cut為實(shí)時(shí)圖中心在當(dāng)前時(shí)刻圖像中截取用的坐標(biāo)，這兩個(gè)變量只用于實(shí)時(shí)圖截取，不參與速度計(jì)算，其計(jì)算公式如下。

(5)

式中，HΔ表示姿態(tài)變化矩陣，與變化的航向角相關(guān)。

2.2 視覺(jué)里程計(jì)搜索區(qū)預(yù)測(cè)方法

傳統(tǒng)視覺(jué)里程計(jì)在圖像匹配的過(guò)程中，往往將整張基準(zhǔn)圖作為匹配搜索的區(qū)域，造成系統(tǒng)資源的浪費(fèi)以及實(shí)時(shí)性的降低，本文利用慣性信息對(duì)上一幀圖像中的匹配區(qū)位置進(jìn)行預(yù)測(cè)，提高匹配搜索的速度及精度，增強(qiáng)視覺(jué)里程計(jì)的實(shí)時(shí)性和精確性。

本方法主要介紹利用慣性速度信息來(lái)確定搜索區(qū)中心坐標(biāo)，其中(us_m,vs_m)表示搜索區(qū)的中心坐標(biāo)。主要分三種情況進(jìn)行討論：

(1)無(wú)姿態(tài)角變化情況

搜索區(qū)中心坐標(biāo)計(jì)算公式如下：

(6)

(2)僅有滾轉(zhuǎn)角變化情況

設(shè)前后幀載體滾轉(zhuǎn)角變化為Δγ，則滾轉(zhuǎn)角變化情況示意圖如圖4所示。

圖4 滾轉(zhuǎn)角變化情況示意圖Fig.4 The schematic diagram of the condition of the roll angle changing

圖4中，實(shí)線邊框的矩陣為基準(zhǔn)圖像，命名為Pbefore，即上一幀圖像；有兩個(gè)虛線邊框的矩陣，它們分別是無(wú)滾轉(zhuǎn)角變化拍攝圖像和有滾轉(zhuǎn)角變化拍攝圖像，分別命名為Pnow_o、Pnow；Onow_o、Onow分別是對(duì)應(yīng)圖像的中心點(diǎn)；綠色填充的五角星為無(wú)滾轉(zhuǎn)角變化實(shí)時(shí)圖位置，無(wú)填充的五角星為有滾轉(zhuǎn)角變化實(shí)時(shí)圖位置。

由圖 4可知，u方向搜索區(qū)中心坐標(biāo)計(jì)算公式不變。有滾轉(zhuǎn)角變化實(shí)時(shí)圖為無(wú)滾轉(zhuǎn)角變化實(shí)時(shí)圖沿OV軸移動(dòng)了FuΔγ，故v方向搜索區(qū)中心坐標(biāo)計(jì)算公式為

(7)

(3)僅有俯仰角變化情況

設(shè)前后幀載體俯仰角變化為Δθ，如圖 5所示。

圖5 俯仰角變化情況示意圖Fig.5 The schematic diagram of the condition of the pitch angle changing

圖5中，相關(guān)變量的定義與圖 4一致，Pnow_o沿OU軸的反方向移動(dòng)了FuΔθ，變成Pnow。

由圖 5可知，v方向搜索區(qū)中心坐標(biāo)計(jì)算公式不變。有滾轉(zhuǎn)角變化實(shí)時(shí)圖為無(wú)滾轉(zhuǎn)角變化實(shí)時(shí)圖沿OU軸反方向移動(dòng)了FuΔθ，故u方向匹配區(qū)中心坐標(biāo)計(jì)算公式為

(8)

v方向匹配區(qū)中心坐標(biāo)計(jì)算公式不變。

(4)綜合分析

當(dāng)Δφ、Δγ、Δθ均較小時(shí)可認(rèn)為其是解耦的，則搜索區(qū)中心坐標(biāo)計(jì)算公式如式(9)。

(9)

2.3 視覺(jué)里程計(jì)輸出速度校正方法

傳統(tǒng)的視覺(jué)里程計(jì)在計(jì)算速度時(shí)，若載體姿態(tài)角發(fā)生變化，將影響視覺(jué)里程計(jì)測(cè)速精度，且影響程度與姿態(tài)角變化幅度、相機(jī)到特征點(diǎn)的距離成正比。本項(xiàng)目利用慣性的姿態(tài)角信息，對(duì)視覺(jué)里程計(jì)輸出的速度信息進(jìn)行高精度補(bǔ)償，能夠極大提升視覺(jué)里程計(jì)的測(cè)速精度。

視覺(jué)里程計(jì)輸出速度的校正公式如下

(10)

3 深組合導(dǎo)航方法

利用慣性信息輔助視覺(jué)里程計(jì)得到測(cè)速信息后，即可進(jìn)行組合導(dǎo)航計(jì)算。

3.1 狀態(tài)量定義

本文設(shè)計(jì)的慣性/雙目視覺(jué)里程計(jì)組合導(dǎo)航方法其誤差來(lái)源主要包括3個(gè)方面：

1)慣導(dǎo)的導(dǎo)航誤差；

2)慣導(dǎo)的慣性器件誤差；

3)視覺(jué)里程計(jì)的誤差。

在常規(guī)卡爾曼濾波[11]中加入視覺(jué)里程計(jì)測(cè)速誤差狀態(tài)量，共選取17個(gè)系統(tǒng)狀態(tài)：

δVn,δVu,δVe,δL,δh,δλ,φn,φu,φe,x,y,z,εx,εy,εz,δVx_odo,δVz_odo。

其中：δVn,δVu,δVe分別表示捷聯(lián)慣導(dǎo)系統(tǒng)北向、天向、東向的速度誤差；

δL,δh,δλ分別表示捷聯(lián)慣導(dǎo)系統(tǒng)的緯度誤差、高度誤差、經(jīng)度誤差；

φn,φu,φe分別表示捷聯(lián)慣導(dǎo)系統(tǒng)導(dǎo)航坐標(biāo)系內(nèi)北、天、東三個(gè)方向的失準(zhǔn)角；

εx,εy,εz分別表示捷聯(lián)慣導(dǎo)系統(tǒng)載體坐標(biāo)系內(nèi)X、Y、Z三個(gè)方向的陀螺漂移；

δVx_odo,δVz_odo分別表示視覺(jué)里程計(jì)沿x、z軸的測(cè)速誤差。

3.2 系統(tǒng)狀態(tài)方程與量測(cè)方程定義

系統(tǒng)狀態(tài)方程為

(11)

式中：X(t)為上述17個(gè)狀態(tài)；W(t)為系統(tǒng)白噪聲；系統(tǒng)矩陣F(t)根據(jù)誤差方程求取。

現(xiàn)簡(jiǎn)要描述F(t)方程

(12)

其中，F(xiàn)visual為視覺(jué)里程計(jì)的誤差，采用一階馬爾科夫過(guò)程描述。

(13)

式中：TVx,TVz和ξVx,ξVz為各自對(duì)應(yīng)的相關(guān)時(shí)間和白噪聲。

濾波器量測(cè)方程形式如下

Z=HX+V

(14)

量測(cè)值Z為慣導(dǎo)系統(tǒng)和景象匹配分別給出的位置的差值，實(shí)際上是兩者誤差的差值

(15)

式中，V為量測(cè)噪聲，考慮為白噪聲。

(16)

3.3 組合導(dǎo)航步驟

慣性/雙目視覺(jué)深組合導(dǎo)航的基本步驟如下：

1)相關(guān)變量初始化，Kalman濾波器初始化；

2)開始導(dǎo)航解算，并計(jì)算系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣

(17)

3)到達(dá)卡爾曼濾波周期時(shí)，進(jìn)行時(shí)間更新：

(18)

4)獲取雙目相機(jī)數(shù)據(jù)，利用標(biāo)定好的相機(jī)基線距離以及相機(jī)焦距，通過(guò)雙目測(cè)距算法，可以計(jì)算出相機(jī)與特征點(diǎn)的距離。

5)通過(guò)相機(jī)獲取特征點(diǎn)數(shù)據(jù)流后，利用慣性信息輔助視覺(jué)里程計(jì)計(jì)算，獲得載體速度信息。

6)將視覺(jué)里程計(jì)速度信息與慣性導(dǎo)航系統(tǒng)的速度信息之差作為Kalman濾波器的輸入，并計(jì)算卡方，當(dāng)卡方超過(guò)限定門限，不進(jìn)行量測(cè)更新，即Kk+1=0；否則進(jìn)行量測(cè)更新

(19)

(20)

8)若進(jìn)行量測(cè)更新，對(duì)估計(jì)出的系統(tǒng)誤差進(jìn)行閉環(huán)修正，轉(zhuǎn)入步驟(2)；若未進(jìn)行量測(cè)更新，直接轉(zhuǎn)入步驟(2)。

4 試驗(yàn)驗(yàn)證與分析

4.1 試驗(yàn)條件

試驗(yàn)設(shè)備為：

·慣性導(dǎo)航設(shè)備采用光纖捷聯(lián)慣導(dǎo)系統(tǒng)，陀螺漂移：0.03～0.05(°)/h，加表零偏：100μg。

·可見光相機(jī)采用加拿大灰點(diǎn)公司的大黃蜂二代雙目相機(jī)，分辨率：640×480，焦距：7.4μm。

·相機(jī)安裝方式如圖6(a)所示，慣性導(dǎo)航設(shè)備安裝方式如圖6(b)所示，且慣導(dǎo)的zb軸與xb、yb軸為右手定則關(guān)系。

行駛軌跡如圖 7所示，行駛了將近6000s，由于車輛行駛的起點(diǎn)與終點(diǎn)為同一地點(diǎn)，在行駛軌跡中體現(xiàn)為“去程”和“回程”的兩條曲線。

圖6 (a)相機(jī)安裝方式Fig.6 (a)The installation of camera

圖6 (b)慣性導(dǎo)航設(shè)備安裝方式Fig.6 (b)The installation of IMU

圖7 行駛軌跡Fig.7 The vehicle path

4.2 試驗(yàn)結(jié)果

為便于觀察視覺(jué)里程計(jì)與GPS輸出速度的差異，取一段時(shí)間內(nèi)的數(shù)據(jù)進(jìn)行對(duì)比，視覺(jué)里程計(jì)原始輸出結(jié)果如圖8所示。

圖 8 (a)為視覺(jué)里程計(jì)原始輸出，可知單純使用視覺(jué)里程計(jì)進(jìn)行測(cè)速，噪聲較大，使用本文提出的新方法進(jìn)行輸出速度優(yōu)化后的視覺(jué)里程計(jì)輸出為圖 8(b)，可見測(cè)量噪聲有一定程度的降低。

圖8 (a)視覺(jué)里程計(jì)輸出速度對(duì)比Fig.8 (a)The comparison of the output velocity of visual odometry

圖8 (b)優(yōu)化后的視覺(jué)里程計(jì)輸出速度對(duì)比Fig.8 (b)The comparison of the output velocity of optimized visual odometry

全程的深組合導(dǎo)航結(jié)果如圖 9所示。

圖9 慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航誤差Fig.9 The errors of INS/stereo visual odometry deeply integrated navigation.

由圖 9可知，對(duì)里程大于5km以后的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)，慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航的定位精度優(yōu)于0.5%D(CEP)，可以媲美傳統(tǒng)的慣性/里程計(jì)組合導(dǎo)航精度。

5 總結(jié)

本文基于機(jī)器視覺(jué)導(dǎo)航原理，合理分析了視覺(jué)里程計(jì)的技術(shù)特點(diǎn)，利用雙目相機(jī)測(cè)量到特征點(diǎn)的距離，通過(guò)視覺(jué)里程計(jì)獲得速度信息，并通過(guò)慣性信息可以輔助提升視覺(jué)里程計(jì)的測(cè)速范圍、測(cè)速精度以及實(shí)時(shí)性。將視覺(jué)里程計(jì)和慣導(dǎo)系統(tǒng)的速度信息作為觀測(cè)量，通過(guò)濾波器估計(jì)視覺(jué)里程計(jì)和慣導(dǎo)系統(tǒng)的誤差并進(jìn)行修正，實(shí)現(xiàn)較高精度的組合導(dǎo)航精度。

通過(guò)車載試驗(yàn)表明，慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航的實(shí)時(shí)定位精度較高，具備工程應(yīng)用條件。

[1] Feng Yang, Cheng Cheng, Quan Pan,et al.Practical integrated navigation fault detection algorithm based on sequential hypothesis testing[J].Journal of Systems Engineering and Electronics, 2011,22(1):146-149.

[2] 楊波，王躍鋼，孟朝，郭志斌.車載捷聯(lián)慣導(dǎo)/重力匹配/高度計(jì)組合導(dǎo)航方法[J].現(xiàn)代防御技術(shù)，2014，42(3)：53-57.

[3] 韓齡，熊智，于永軍，劉建業(yè).基于SAR輔助的慣導(dǎo)/星光組合導(dǎo)航系統(tǒng)研究[J].傳感器與微系統(tǒng)，2011，30(7)：16-19.

[4] 于永軍，徐錦法，張梁，熊智.慣導(dǎo)/雙目視覺(jué)位姿估計(jì)算法研究[J].儀器儀表學(xué)報(bào)，2014，35(10)：2170-2176.

[5] 蘇哲，許錄平，張華，謝強(qiáng).基于XPNAV和SINS的容錯(cuò)組合導(dǎo)航系統(tǒng)[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2011，39(6)：19-23.

[6] 李宇波，朱效洲，盧惠民，張輝.視覺(jué)里程計(jì)技術(shù)綜述[J].計(jì)算機(jī)應(yīng)用研究，2012，29(8)：2801-2805.

[7] Pretto A，Menegatti E，Bennewitz M，et al.A visual odometry framework robust to motion blur[C]//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ: IEEE Press，2009: 1685-1692.

[8] 吳乃亮，閆飛，卜春光.基于視覺(jué)里程計(jì)的移動(dòng)機(jī)器人三維場(chǎng)景重構(gòu)[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2015，43(1)：337-340.

[9] 吳偉仁，王大軼，邢琰，等.月球車巡視探測(cè)的雙目視覺(jué)里程計(jì)算法與實(shí)驗(yàn)研究[J].中國(guó)科學(xué)：信息科學(xué)，2011，41(12)：1415-1422.

[10] 張青濤，楊學(xué)友，劉濤，等.基于快速模板匹配的智能視覺(jué)傳感器設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào)，2013，26(8)：1040-1044.

[11] 陳楸，曹衛(wèi)濤.基于場(chǎng)景識(shí)別的無(wú)人機(jī)自主著陸組合導(dǎo)航研究[J].計(jì)算機(jī)仿真，2011，28(2):84-87,150.

INS/Stereo Visual Odometry Deeply Integrated Navigation Method

LU Jian-jun1，REN Xiao-jun2, SUN Wei3, GUO Yuan-jiang3, LI Qun3

(1.Equipment Department of the Navy, Beijing 100000,China;2.Naval Representative Office of the Research Academy, CASIC, Beijing 100000,China;3.Beijing Institute of Automatic Control Equipment, Beijing 100074, China)

To resolve the problem of large measurement error, low precision of matching, poor real timing, the INS/stereo visual odometry integrated navigation method based on Kalman filter was designed.The INS is used to help the stereo visual odometry faster and more accuracy by the real-time images capture, search area prediction and measuring velocity adjusting.By the use of kalman filter, visual odometry can correct the accumulated error of INS to improve the precision of integrated navigation system.The vehicle-carried result shows that this method improves the precision of INS/stereo visual odometry deeply integrated navigation to 0.5%D(CEP) and possess the condition of engineering use.

Stereo visual odometry;INS;Integrated navigation

10.19306/j.cnki.2095-8110.2016.03.007

2015-12-17；

2016-01-26。

總裝預(yù)研課題(51309030203)

逯建軍(1978-)，男，主要從事制導(dǎo)與控制方面的研究。

U666.1

A

2095-8110(2016)03-0037-07