視覺/慣性導航抗有色噪聲濾波算法設計與仿真*

?

視覺/慣性導航抗有色噪聲濾波算法設計與仿真*

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

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

摘要：實際視覺/慣性組合導航中存在的噪聲往往是相關的，必須考慮有色噪聲影響，有色噪聲的存在易導致視覺/慣性組合導航濾波算法的性能下降甚至發散。在分析視覺/慣性組合導航的結構組成和建立起系統狀態方程的基礎上，分別針對過程有色噪聲和量測有色噪聲情形，提出采用狀態擴展法和量測狀態擴展法對導航系統方程中的有色噪聲進行處理，并以選定的優化準則進行濾波算法設計來解決有色噪聲干擾問題。數值仿真實驗證實所設計的抗有色噪聲濾波方法性能穩定有效。

關鍵詞：慣性導航系統；視覺；濾波；有色噪聲

0引言

視覺(Vision)傳感器可實時獲取圖像，并利用其來計算運動物體的運動參數，從而提供輔助導航信息。視覺導航是一個非常具有發展前景的導航方式。當前發展比較成熟的視覺導航技術有用于機器人的視覺導航技術、無人自主車、無人直升機的自主著陸控制技術等[1]。現階段，國外視覺/慣性導航技術發展非常迅速。在高精度導航應用中，由于SLAM (simultaneous iocalization and mapping)系統的構建比較困難，視覺系統難以提供連續的導航定位信息，所以視覺系統通常被用來輔助GPS、慣性導航系統(inertial navigation system，INS)等導航系統[2-3]。Vision/INS導航在工業、軍事、民用、科學研究等許多方面具有廣泛的應用前景。

卡爾曼濾波算法是組合導航實際應用中最成熟的濾波算法，Vision/INS組合導航的濾波算法選擇卡爾曼濾波無疑是最簡便的，但是經典卡爾曼濾波應用的一個先決條件是已知噪聲的統計特性，然而實際Vision/INS組合導航中受量測噪聲、樣本等方面限制，噪聲的先驗統計未知或不準確，這種情況下，采用經典卡爾曼濾波，易引起性能下降甚至發散。因此，必須針對Vision/INS組合導航的特點，設計具有抗噪聲能力的濾波器。

1Vision/INS組合導航系統及其狀態方程建立

1.1Vision/INS組合導航系統簡介

視覺圖像視覺導航依靠被觀測圖像框架在特定時間周期內的連續變換實現的。所需的傳感器為相機，相機獲取的特征參數通常可用于推理和確定連續圖像框架間變化的數量。實現的方法通常有2種：第1種方法是基于光學流，試圖建立三維空間圖像框架間變化的模型。第2種方法是基于特征跟蹤，該方法辨識圖像中特征目標物體，并實現其在不同圖像框架下的跟蹤。基于光學流的方法主要為用戶提供一些基本的運動參數估計，特征跟蹤方法則通常應用于導航，用于判斷被跟蹤特征的距離范圍[4]。視覺/INS結構示意圖如圖1所示。

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

圖1　視覺/INS結構示意圖Fig.1　Vision/INS mechanization

1.2系統方程的建立

(1)

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

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

(2)

(3)

式中：φ為姿態角矩陣。

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

(4)

通過變換，并考慮噪聲ζ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)為速度誤差；δα，δβ，δγ為姿態角誤差；fi0(i=x,y,z)為加速度計零偏。

公式(5)和(6)可用于校正INS的位置和速度誤差，但無法實現對姿態誤差的估計，姿態誤差估計可采用如下公式完成：

(7)

(8)

利用矢量間轉換關系，可得

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

(9)

式中：ζ2為噪聲；

(10)

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

(11)

(12)

在滿足香農定理條件下進行離散化，得離散方程：

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

(13)

系統的狀態方程為

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

(14)

式中：Φ(k)為狀態轉移陣；υ為過程噪聲。

2抗有色噪聲濾波器設計

2.1濾波穩定性判別

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

2.2抗有色噪聲濾波算法設計

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

對于存在過程有色噪聲情況，采用狀態擴充法，將狀態方程中的有色噪聲表達式轉換成白噪聲表示形式[12]。

令新的狀態變量為

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

過程有色噪聲滿足如下方程：

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

(15)

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

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)。

此時，式(16)和(17)的系統方程將過程有色噪聲轉換為白噪聲，如果系統中還包括量測有色噪聲，則進一步對量測狀態方程進行擴充。令新的狀態變量：

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

量測有色噪聲滿足如下方程：

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

(18)

式中：μ(k)為白噪聲。擴充后系統方程為

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

(19)

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

(20)

至此，將有色噪聲方程轉換為白噪聲方程(19)和(20)。在滿足某種優化準則的條件下，可進行濾波器設計。上述過程對于僅含量測噪聲情況，同樣適用，這里以最小方差為準則，通過推導可得到如下濾波算法：

一步預測

(21)

狀態估計：

(22)

增益陣：

(23)

一步預測誤差方差陣：

(24)

2.3濾波算法分析

前面濾波算法解決有色噪聲干擾問題時，采用狀態擴展和量測狀態擴展的維數可達30+2N維，雖計算量較大，但求逆矩陣的維數與擴展前量測方程的維數2N相同，通過優先選擇最佳匹配圖像限制其值，可避免引起維數災難，因此抗有色噪聲濾波算法本身是可以實現的。

3數值實例

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

圖2　Vision/INS導航抗有色噪聲濾波仿真結果Fig.2　Vision/INS anti-colored noise filter　　　simulation result

4結束語

采用狀態擴展法可對Vision/INS系統方程中的過程有色噪聲進行濾波處理；采用量測狀態擴展法則可對存在量測有色噪聲進行有效處理。當2種有色噪聲同時存在時，同時采用狀態擴展和量測狀態擴展方法可進行有效處理。系統仿真實驗證實采用論文設計的抗有色噪聲濾波方法能夠保證Vision/INS系統的性能穩定。

參考文獻：

[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圖像輔助導航系統研究[J].第二炮兵工程學院學報，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]丁少文，王林.基于連續三角測量法的機器人定位方法[J].微型電腦應用，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立體視覺輔助慣性導航與仿真實驗研究[J]，現代防御技術，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]宋國東，姜守達，林連雷.復雜有色噪聲廣義系統信息融合Kalman濾波器[J].儀器儀表學報，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]許濤，祝小平,楊軍.有色噪聲條件下的目標被動定位算法研究[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]趙琳.非線性系統濾波理論[M]. 北京：國防工業出版社，2012.

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

[12]鄭大鐘.線性系統理論[M].北京：清華大學出版社，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

中圖分類號：V448.122；TP391.9

文獻標志碼：A

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

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

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

作者簡介：周小剛(1979-)，男，四川南充人。講師，博士后，研究方向為慣性技術與信號處理。

基金項目：校青年基金(XY2011JJB20)

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