?

視覺(jué)/慣性導(dǎo)航抗有色噪聲濾波算法設(shè)計(jì)與仿真*

周小剛，劉潔瑜，汪立新，熊陶，楊波

(第二炮兵工程大學(xué)，陜西 西安710025)

摘要：實(shí)際視覺(jué)/慣性組合導(dǎo)航中存在的噪聲往往是相關(guān)的，必須考慮有色噪聲影響，有色噪聲的存在易導(dǎo)致視覺(jué)/慣性組合導(dǎo)航濾波算法的性能下降甚至發(fā)散。在分析視覺(jué)/慣性組合導(dǎo)航的結(jié)構(gòu)組成和建立起系統(tǒng)狀態(tài)方程的基礎(chǔ)上，分別針對(duì)過(guò)程有色噪聲和量測(cè)有色噪聲情形，提出采用狀態(tài)擴(kuò)展法和量測(cè)狀態(tài)擴(kuò)展法對(duì)導(dǎo)航系統(tǒng)方程中的有色噪聲進(jìn)行處理，并以選定的優(yōu)化準(zhǔn)則進(jìn)行濾波算法設(shè)計(jì)來(lái)解決有色噪聲干擾問(wèn)題。數(shù)值仿真實(shí)驗(yàn)證實(shí)所設(shè)計(jì)的抗有色噪聲濾波方法性能穩(wěn)定有效。

關(guān)鍵詞：慣性導(dǎo)航系統(tǒng)；視覺(jué)；濾波；有色噪聲

0引言

視覺(jué)(Vision)傳感器可實(shí)時(shí)獲取圖像，并利用其來(lái)計(jì)算運(yùn)動(dòng)物體的運(yùn)動(dòng)參數(shù)，從而提供輔助導(dǎo)航信息。視覺(jué)導(dǎo)航是一個(gè)非常具有發(fā)展前景的導(dǎo)航方式。當(dāng)前發(fā)展比較成熟的視覺(jué)導(dǎo)航技術(shù)有用于機(jī)器人的視覺(jué)導(dǎo)航技術(shù)、無(wú)人自主車、無(wú)人直升機(jī)的自主著陸控制技術(shù)等[1]?，F(xiàn)階段，國(guó)外視覺(jué)/慣性導(dǎo)航技術(shù)發(fā)展非常迅速。在高精度導(dǎo)航應(yīng)用中，由于SLAM (simultaneous iocalization and mapping)系統(tǒng)的構(gòu)建比較困難，視覺(jué)系統(tǒng)難以提供連續(xù)的導(dǎo)航定位信息，所以視覺(jué)系統(tǒng)通常被用來(lái)輔助GPS、慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)等導(dǎo)航系統(tǒng)[2-3]。Vision/INS導(dǎo)航在工業(yè)、軍事、民用、科學(xué)研究等許多方面具有廣泛的應(yīng)用前景。

卡爾曼濾波算法是組合導(dǎo)航實(shí)際應(yīng)用中最成熟的濾波算法，Vision/INS組合導(dǎo)航的濾波算法選擇卡爾曼濾波無(wú)疑是最簡(jiǎn)便的，但是經(jīng)典卡爾曼濾波應(yīng)用的一個(gè)先決條件是已知噪聲的統(tǒng)計(jì)特性，然而實(shí)際Vision/INS組合導(dǎo)航中受量測(cè)噪聲、樣本等方面限制，噪聲的先驗(yàn)統(tǒng)計(jì)未知或不準(zhǔn)確，這種情況下，采用經(jīng)典卡爾曼濾波，易引起性能下降甚至發(fā)散。因此，必須針對(duì)Vision/INS組合導(dǎo)航的特點(diǎn)，設(shè)計(jì)具有抗噪聲能力的濾波器。

1Vision/INS組合導(dǎo)航系統(tǒng)及其狀態(tài)方程建立

1.1Vision/INS組合導(dǎo)航系統(tǒng)簡(jiǎn)介

視覺(jué)圖像視覺(jué)導(dǎo)航依靠被觀測(cè)圖像框架在特定時(shí)間周期內(nèi)的連續(xù)變換實(shí)現(xiàn)的。所需的傳感器為相機(jī)，相機(jī)獲取的特征參數(shù)通常可用于推理和確定連續(xù)圖像框架間變化的數(shù)量。實(shí)現(xiàn)的方法通常有2種：第1種方法是基于光學(xué)流，試圖建立三維空間圖像框架間變化的模型。第2種方法是基于特征跟蹤，該方法辨識(shí)圖像中特征目標(biāo)物體，并實(shí)現(xiàn)其在不同圖像框架下的跟蹤?；诠鈱W(xué)流的方法主要為用戶提供一些基本的運(yùn)動(dòng)參數(shù)估計(jì)，特征跟蹤方法則通常應(yīng)用于導(dǎo)航，用于判斷被跟蹤特征的距離范圍[4]。視覺(jué)/INS結(jié)構(gòu)示意圖如圖1所示。

利用一些方法可解決光學(xué)相機(jī)與期望特征目標(biāo)的測(cè)距。這類操作類似于三角測(cè)量基線技術(shù)[5-6]。視覺(jué)圖像傳感器獲得的特征圖像是被觀測(cè)目標(biāo)的距離函數(shù)。一旦有特征目標(biāo)被識(shí)別到，則立即進(jìn)行特征關(guān)聯(lián)處理以確定不同傳感器觀測(cè)到的為同一特征物體。假設(shè)特征關(guān)聯(lián)時(shí)是靜止的，而當(dāng)其位置或者方位變化時(shí)，其與用戶平臺(tái)的位置和方位是相關(guān)聯(lián)的。通過(guò)測(cè)得的已知信息即可實(shí)現(xiàn)某些運(yùn)動(dòng)參數(shù)的估計(jì)。

圖1　視覺(jué)/INS結(jié)構(gòu)示意圖Fig.1　Vision/INS mechanization

1.2系統(tǒng)方程的建立

(1)

式中：li(tk)(i=1,2…,N)為tk時(shí)刻圖像到傳感器的距離信息，N為特征圖像中的特征平面數(shù)。

由INS解算得到的位置變化信息如下：

(2)

(3)

式中：φ為姿態(tài)角矩陣。

在tk時(shí)刻式(2)與(1)的差值：

(4)

通過(guò)變換，并考慮噪聲ζ1影響，則上式可變換為

ZINS(tk)=Hp(tk)X(tk)+ζ1(tk)，

(5)

(6)

式中：X=(δx，δy，δz，δvx，δvy,δvz,δα,δβ,δγ，ωx0，ωy0，ωz0，fx0，fy0，fz0)T；δi(i=x,y,z)為位置誤差；δvi(i=x,y,z)為速度誤差；δα，δβ，δγ為姿態(tài)角誤差；fi0(i=x,y,z)為加速度計(jì)零偏。

公式(5)和(6)可用于校正INS的位置和速度誤差，但無(wú)法實(shí)現(xiàn)對(duì)姿態(tài)誤差的估計(jì)，姿態(tài)誤差估計(jì)可采用如下公式完成：

(7)

(8)

利用矢量間轉(zhuǎn)換關(guān)系，可得

Za(tk)=Ha(tk)X(tk)+ζ2(tk),

(9)

式中：ζ2為噪聲；

(10)

利用上述公式可建立具有如下形式的矩陣：

(11)

(12)

在滿足香農(nóng)定理?xiàng)l件下進(jìn)行離散化，得離散方程：

Z(k)=H(k)X(k)+ζ(k).

(13)

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

X(k)=Φ(k)X(k-1)+υ(k)，

(14)

式中：Φ(k)為狀態(tài)轉(zhuǎn)移陣；υ為過(guò)程噪聲。

2抗有色噪聲濾波器設(shè)計(jì)

2.1濾波穩(wěn)定性判別

系統(tǒng)可看作為隨機(jī)線性系統(tǒng)，根據(jù)濾波穩(wěn)定性原理，如果隨機(jī)線性系統(tǒng)是一致完全可控和完全一致完全可觀測(cè)的，則濾波器是一致漸進(jìn)穩(wěn)定的[9]。顯然，判斷濾波器是否一致漸進(jìn)穩(wěn)定，只需判斷原系統(tǒng)是否一致完全可控和完全一致完全可觀測(cè)。對(duì)于式(13)和(14)組成的時(shí)變離散隨機(jī)線性系統(tǒng)是否一致完全可控和完全一致完全可觀測(cè)可分別按照能控性格拉姆矩陣判據(jù)和能觀測(cè)性格拉姆矩陣判據(jù)[10]進(jìn)行判斷。通過(guò)分析知，式(13)和(14)組成的離散隨機(jī)線性定常系統(tǒng)是穩(wěn)定的。

2.2抗有色噪聲濾波算法設(shè)計(jì)

視覺(jué)導(dǎo)航提供的信息非常豐富，然而這些信息中往往包括噪聲成分，信號(hào)處理中的白噪聲只是一種理論上假設(shè)的理想噪聲，實(shí)際工程系統(tǒng)中的噪聲總是相關(guān)的，只是在相關(guān)性比較弱的前提下近似的表示成白色噪聲，而相關(guān)性比較強(qiáng)的條件下就必須考慮有色噪聲影響，有色噪聲的存在易導(dǎo)致濾波性能下降乃至發(fā)散，對(duì)此需要進(jìn)行抗有色噪聲濾波設(shè)計(jì)[11]。有色噪聲主要包括過(guò)程有色噪聲υ(k)和量測(cè)有色噪聲ζ(k)2種，要通過(guò)濾波消除其中的1種或2種噪聲，可采用如下方法進(jìn)行設(shè)計(jì)。

對(duì)于存在過(guò)程有色噪聲情況，采用狀態(tài)擴(kuò)充法，將狀態(tài)方程中的有色噪聲表達(dá)式轉(zhuǎn)換成白噪聲表示形式[12]。

令新的狀態(tài)變量為

Xa(k)=(XT(k),υT(k))T.

過(guò)程有色噪聲滿足如下方程：

υ(k)=A(k,k-1)υ(k-1)+η(k-1),

(15)

式中：η(k-1)為白噪聲，擴(kuò)充后系統(tǒng)方程為

Xa(k)=Φa(k)Xa(k-1)+Γaη(k)，

(16)

Z(k)=Ha(k)Xa(k)+ζ(k)，

(17)

Γa(k,k-1)=(0，I)T，Ha(k)=(H(k)，0)。

此時(shí)，式(16)和(17)的系統(tǒng)方程將過(guò)程有色噪聲轉(zhuǎn)換為白噪聲，如果系統(tǒng)中還包括量測(cè)有色噪聲，則進(jìn)一步對(duì)量測(cè)狀態(tài)方程進(jìn)行擴(kuò)充。令新的狀態(tài)變量：

Xb(k)=(Xa(k)，ζT(k))T，

量測(cè)有色噪聲滿足如下方程：

ζ(k)=φ(k,k-1)ζ(k-1)+μ(k-1)，

(18)

式中：μ(k)為白噪聲。擴(kuò)充后系統(tǒng)方程為

Xb(k)=Φb(k)Xb(k-1)+Γbηb(k),

(19)

Z(k)=Hb(k)Xb(k)，

(20)

至此，將有色噪聲方程轉(zhuǎn)換為白噪聲方程(19)和(20)。在滿足某種優(yōu)化準(zhǔn)則的條件下，可進(jìn)行濾波器設(shè)計(jì)。上述過(guò)程對(duì)于僅含量測(cè)噪聲情況，同樣適用，這里以最小方差為準(zhǔn)則，通過(guò)推導(dǎo)可得到如下濾波算法：

一步預(yù)測(cè)

(21)

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

(22)

增益陣：

(23)

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

(24)

2.3濾波算法分析

前面濾波算法解決有色噪聲干擾問(wèn)題時(shí)，采用狀態(tài)擴(kuò)展和量測(cè)狀態(tài)擴(kuò)展的維數(shù)可達(dá)30+2N維，雖計(jì)算量較大，但求逆矩陣的維數(shù)與擴(kuò)展前量測(cè)方程的維數(shù)2N相同，通過(guò)優(yōu)先選擇最佳匹配圖像限制其值，可避免引起維數(shù)災(zāi)難，因此抗有色噪聲濾波算法本身是可以實(shí)現(xiàn)的。

3數(shù)值實(shí)例

仿真僅考慮量測(cè)方程包含有色噪聲情況，視覺(jué)/慣性組合導(dǎo)航系統(tǒng)裝在載體上并以3×104m/s速度運(yùn)動(dòng)，所用INS的參數(shù)如下：陀螺零偏為0.1(°)/h，陀螺量測(cè)噪聲N(0.001,(0.03)2)(°)/h，加速度計(jì)零偏為5×10-5g。采用的視覺(jué)傳感器的測(cè)距絕對(duì)誤差峰值2 cm，視覺(jué)圖像傳感器平面中心測(cè)量噪聲(1)2cm，按照一定規(guī)律在量測(cè)方程中引入有色噪聲，采樣文中方法后濾波結(jié)果如圖2所示。圖中橢圓所圈部分為進(jìn)行區(qū)域視覺(jué)導(dǎo)航誤差修正段，進(jìn)行誤差修正后可有效消除累積誤差，通過(guò)采用視覺(jué)導(dǎo)航前后對(duì)比不難得出：利用視覺(jué)導(dǎo)航信息后不僅可保證INS長(zhǎng)行程和長(zhǎng)時(shí)間的導(dǎo)航精度，而且系統(tǒng)具有較強(qiáng)的抗有色噪聲能力。

圖2　Vision/INS導(dǎo)航抗有色噪聲濾波仿真結(jié)果Fig.2　Vision/INS anti-colored noise filter　　　simulation result

4結(jié)束語(yǔ)

采用狀態(tài)擴(kuò)展法可對(duì)Vision/INS系統(tǒng)方程中的過(guò)程有色噪聲進(jìn)行濾波處理；采用量測(cè)狀態(tài)擴(kuò)展法則可對(duì)存在量測(cè)有色噪聲進(jìn)行有效處理。當(dāng)2種有色噪聲同時(shí)存在時(shí)，同時(shí)采用狀態(tài)擴(kuò)展和量測(cè)狀態(tài)擴(kuò)展方法可進(jìn)行有效處理。系統(tǒng)仿真實(shí)驗(yàn)證實(shí)采用論文設(shè)計(jì)的抗有色噪聲濾波方法能夠保證Vision/INS系統(tǒng)的性能穩(wěn)定。

參考文獻(xiàn)：

[1]SUBRAMANIAN V.Development of Mchine Vision and Laser Radar Based Autonomous Vehicle Guidance Systems for Citrus Grove Navigation[J]. Computers and Electronics in Agriculture，2006,53(2):130-143.

[2]周小剛，汪立新,劉潔瑜,等.車載3D圖像輔助導(dǎo)航系統(tǒng)研究[J].第二炮兵工程學(xué)院學(xué)報(bào)，2012,26(4)：28-31.

ZHOU Xiao-gang, WANG Li-xin, LIU Jie-yu,et al. The Car 3D Stereo-Vision Aid Navigation System Research[J].Journal of Second Artillery Engineering University, 2012，26(4)：28-31.

[3]Daniel Sullivan.High Accuracy Autonomous Image Georefereneing Using a GPS Inertial-Aided Digital Imaging System[C]∥Proeeedings of ION NTM，2002：1-6.

[4]BEIS J S, LOWE D G.Indexing Without Invariants in Vision Object Recognition[J].IEEE Transactions on Pattern Analysis and Machine Intelligence, 1999(21):236-243.

[5]UIJT M, VENABLE D, SMEARCHECK M. Integration of an Inertial Measurement Unit and Vision Imaging Sensor for Urban and Indoor Navigation of Unmanned Vehicles[C]∥Proceedings of the National Technical Meeting of the ION, January, 2007：22-24.

[6]丁少文，王林.基于連續(xù)三角測(cè)量法的機(jī)器人定位方法[J].微型電腦應(yīng)用，2013,30(5)：76-80.

DING Shao-wen, WANG Lin, One Mobile Robot Localization Method Based on Continuous Triangulation[J], Microcomputer Applications, 2013,30(5)：76-80.

[7]Evan T Dill. Integration of Vision and 2D Imaging Data for Assured Navigation in Unknown Environments[D].The Russ College of Engneering and Technology of Ohio University, 2011.

[8]周小剛，劉潔瑜，汪立新,等.車載3D立體視覺(jué)輔助慣性導(dǎo)航與仿真實(shí)驗(yàn)研究[J]，現(xiàn)代防御技術(shù)，2013，41(5)：36-40.

ZHOU Xiao-gang, LIU Jie-yu, WANG Li-xin,et al.Vehicle Mounted 3D Stereo Vision Aided IMU Navigation System and Simulation Experiment[J].Modern Defence Technology, 2013，41(5)：36-40.

[9]宋國(guó)東，姜守達(dá)，林連雷.復(fù)雜有色噪聲廣義系統(tǒng)信息融合Kalman濾波器[J].儀器儀表學(xué)報(bào)，2013，34(5)：107-110.

SONG Guo-dong, JIANG Shou-da, LIN Lian-lei.Information Fusion Kalman Filter with Complex Colored Noise for Descriptor Systems[J]. Chinese Journal of Scientific Instrument, 2013，34(5)：107-110.

[10]許濤，祝小平,楊軍.有色噪聲條件下的目標(biāo)被動(dòng)定位算法研究[J].計(jì)算機(jī)仿真，2013，30(5)：67-71.

XU Tao,ZHU Xiao-ping, YANG Jun.Research on Passive Location Algorithm for Colored Measurement Noise[J].Computer Simulation, 2013，30(5)：67-71.

[11]趙琳.非線性系統(tǒng)濾波理論[M]. 北京：國(guó)防工業(yè)出版社，2012.

ZHAO Lin. Nonlinear System Filter Theory[M].Beijing:National Defense Industry Publishing Company,2012.

[12]鄭大鐘.線性系統(tǒng)理論[M].北京：清華大學(xué)出版社，2002.

ZHENG Da-zhong. Linear System Theory[M].Beijing:Tsinghua University Press,2002.

Design of Anti-Colored Noise Filter for Vision/INS Integrated Navigation System and Simulation Experiment

ZHOU Xiao-gang，LIU Jie-yu, WANG Li-xin, XIONG Tao, YANG Bo

(The Second Artillery Engineering University,Shaanxi Xi′an 710025, China)

Abstract:The filtering algorithm performance of vision/INS usually declinesand even scattersdue tosystem colored noise. The methods of state extended and measurement state extended areput forward to solve the problems of processing and measuringcolored noise based on analyzing the composition of vision/INS and building the state equation. The filer algorithm is also designed by using the selected optimization rules. The simulation experiment proves that the method mentioned above is stable and valid.

Key words:inertial navigation system；vision；filter；colored noise

中圖分類號(hào)：V448.122；TP391.9

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1009-086X(2015)-02-0199-04

doi:10.3969/j.issn.1009-086x.2015.02.032

通信地址：710025西安第二炮兵工程大學(xué)304教研室E-mail：sheqiangzhou@163.com

作者簡(jiǎn)介：周小剛(1979-)，男，四川南充人。講師，博士后，研究方向?yàn)閼T性技術(shù)與信號(hào)處理。

基金項(xiàng)目：校青年基金(XY2011JJB20)

* 收稿日期：2014-03-22；
修回日期：2014-05-20