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

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

慣性/雙目視覺里程計深組合導航方法

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

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

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

雙目視覺里程計；慣性導航；組合導航

0 引言

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

視覺里程計技術的基本內(nèi)涵[6]為：單目相機通過前后幀圖像的差異，在已知距離的情況下可以測量出載體的速度。而雙目視覺里程計[7-9]技術利用雙目相機，通過左右兩個相機圖像差異測出距離，通過前后幀圖像的差異測量出載體的速度。

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

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

1 雙目視覺里程計算法

1.1 圖像匹配算法

本文使用的圖像匹配算法為歸一化灰度值匹配法，其本質(zhì)是衡量兩幅圖像相似程度的度量準則；其過程是在兩幅圖像中尋找相關性最高的點作為匹配結果。

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

(1)

圖像匹配算法可以獲得同一標志點在不同圖像中的相對偏移量，由相對漂移量可推算出在現(xiàn)實場景中的偏移量。

1.2 雙目視覺測距算法

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

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

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

利用相似三角形原理，可得相機到Q點的距離為

(2)

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

1.3 視覺里程計算法

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

(3)

圖2 視覺里程計基本原理Fig.2 The basic theory of visual odometry

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

2 慣性輔助視覺里程計方法

慣性導航技術基于牛頓力學原理，使用陀螺和加速度計測量載體運動的角速度和加速度，獲取載體的姿態(tài)、速度和位置信息，其優(yōu)點是短時精度高，能夠準確地測量載體姿態(tài)變化的情況等。

本文考慮利用慣性導航技術的優(yōu)點，以提升視覺里程計性能為目的，提出幾種方法。

2.1 視覺里程計實時圖截取方法

為了保證視覺里程計測量的速度范圍達到最大，希望截取的實時圖像在當前幀圖像中的位置坐標點與在上一幀圖像中的位置坐標點的連線的中點位于基準圖的中心位置坐標。本方法主要介紹應用于視覺里程計的實時圖中心坐標計算，為便于分析，將分成兩種情況進行討論：

(1)無航向角變化情況

截取的實時圖像中心在當前幀圖像中的坐標計算公式如下所示。

(4)

式中，ureal_m、vreal_m表示截取的實時圖像中心在當前幀圖像中的坐標；unow_m、vnow_m表示當前時刻圖像中心坐標；duimu、dvimu表示利用慣性信息計算得到的特征點偏移。

(2)有航向角變化情況

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

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

如圖 3所示，實線邊框為上一時刻圖像，命名為Pbefore，代表基準圖；點劃線邊框為無航向角變化情況的現(xiàn)時刻圖像，命名為Pnow_o；虛線邊框為有航向角變化情況的現(xiàn)時刻圖像，命名為Pnow。Obefore為基準圖中心位置，也代表上一時刻載體中心位置；Onow為現(xiàn)時刻圖中心位置，也代表現(xiàn)時刻載體中心位置；Δφ表示兩幀圖像采樣間隔時間內(nèi)航向角變化，Obefore到Onow連線的中點為期望實時圖截取位置，用五角星表示。

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

(5)

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

2.2 視覺里程計搜索區(qū)預測方法

傳統(tǒng)視覺里程計在圖像匹配的過程中，往往將整張基準圖作為匹配搜索的區(qū)域，造成系統(tǒng)資源的浪費以及實時性的降低，本文利用慣性信息對上一幀圖像中的匹配區(qū)位置進行預測，提高匹配搜索的速度及精度，增強視覺里程計的實時性和精確性。

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

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

搜索區(qū)中心坐標計算公式如下：

(6)

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

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

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

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

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

(7)

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

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

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

圖5中，相關變量的定義與圖 4一致，Pnow_o沿OU軸的反方向移動了FuΔθ，變成Pnow。

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

(8)

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

(4)綜合分析

當Δφ、Δγ、Δθ均較小時可認為其是解耦的，則搜索區(qū)中心坐標計算公式如式(9)。

(9)

2.3 視覺里程計輸出速度校正方法

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

視覺里程計輸出速度的校正公式如下

(10)

3 深組合導航方法

利用慣性信息輔助視覺里程計得到測速信息后，即可進行組合導航計算。

3.1 狀態(tài)量定義

本文設計的慣性/雙目視覺里程計組合導航方法其誤差來源主要包括3個方面：

1)慣導的導航誤差；

2)慣導的慣性器件誤差；

3)視覺里程計的誤差。

在常規(guī)卡爾曼濾波[11]中加入視覺里程計測速誤差狀態(tài)量，共選取17個系統(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)慣導系統(tǒng)北向、天向、東向的速度誤差；

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

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

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

δVx_odo,δVz_odo分別表示視覺里程計沿x、z軸的測速誤差。

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

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

(11)

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

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

(12)

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

(13)

式中：TVx,TVz和ξVx,ξVz為各自對應的相關時間和白噪聲。

濾波器量測方程形式如下

Z=HX+V

(14)

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

(15)

式中，V為量測噪聲，考慮為白噪聲。

(16)

3.3 組合導航步驟

慣性/雙目視覺深組合導航的基本步驟如下：

1)相關變量初始化，Kalman濾波器初始化；

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

(17)

3)到達卡爾曼濾波周期時，進行時間更新：

(18)

4)獲取雙目相機數(shù)據(jù)，利用標定好的相機基線距離以及相機焦距，通過雙目測距算法，可以計算出相機與特征點的距離。

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

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

(19)

(20)

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

4 試驗驗證與分析

4.1 試驗條件

試驗設備為：

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

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

·相機安裝方式如圖6(a)所示，慣性導航設備安裝方式如圖6(b)所示，且慣導的zb軸與xb、yb軸為右手定則關系。

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

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

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

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

4.2 試驗結果

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

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

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

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

全程的深組合導航結果如圖 9所示。

圖9 慣性/雙目視覺里程計深組合導航誤差Fig.9 The errors of INS/stereo visual odometry deeply integrated navigation.

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

5 總結

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

通過車載試驗表明，慣性/雙目視覺里程計深組合導航的實時定位精度較高，具備工程應用條件。

[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)慣導/重力匹配/高度計組合導航方法[J].現(xiàn)代防御技術，2014，42(3)：53-57.

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

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

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

[6] 李宇波，朱效洲，盧惠民，張輝.視覺里程計技術綜述[J].計算機應用研究，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] 吳乃亮，閆飛，卜春光.基于視覺里程計的移動機器人三維場景重構[J].華中科技大學學報(自然科學版)，2015，43(1)：337-340.

[9] 吳偉仁，王大軼，邢琰，等.月球車巡視探測的雙目視覺里程計算法與實驗研究[J].中國科學：信息科學，2011，41(12)：1415-1422.

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

[11] 陳楸，曹衛(wèi)濤.基于場景識別的無人機自主著陸組合導航研究[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。

總裝預研課題(51309030203)

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

U666.1

A

2095-8110(2016)03-0037-07