車輛組合導(dǎo)航系統(tǒng)自適應(yīng)UKF濾波算法

陸文昌，費(fèi)龑棟，陳 龍，汪若塵

(江蘇大學(xué)汽車與交通工程學(xué)院，江蘇鎮(zhèn)江212013)

車輛組合導(dǎo)航中Global Positioning System/Dead Reckoning(GPS/DR)是較常用的組合導(dǎo)航方法。GPS/DR組合導(dǎo)航定位濾波算法多采用擴(kuò)展卡爾曼濾波(EKF)或改進(jìn)EFK實(shí)現(xiàn)［1-2］。EKF方法忽略了泰勒展開(kāi)式的高次項(xiàng)，當(dāng)誤差較大，泰勒展開(kāi)式高次項(xiàng)不可忽略時(shí)，濾波精度會(huì)降低，不穩(wěn)定甚至發(fā)散。Julier和Uhlmann提出的無(wú)跡卡爾曼濾波Unscented Kalman filter(UKF)算法［3-4］，UKF 算法基于 UT 變化，利用非線性模型，避免引入線性化誤差，從而提高了濾波精度，并且省去計(jì)算Jacobi矩陣的過(guò)程，更加容易實(shí)現(xiàn)。

但UKF算法是基于精確的數(shù)學(xué)模型和過(guò)程噪聲和觀測(cè)噪聲已知為基礎(chǔ)的，當(dāng)噪聲統(tǒng)計(jì)特性將發(fā)生變化，UKF算法的精度和穩(wěn)定性將會(huì)降低。針對(duì)此問(wèn)題，S.Ranji，等［5］提出了利用神經(jīng)網(wǎng)絡(luò)理論跟蹤噪聲統(tǒng)計(jì)特性;石勇，等［6］提出了一種改進(jìn)的Sage-Husa濾波;周衛(wèi)東，等［7］提出了 PSO-UKF 組合濾波，都取得了一定的成效。

筆者利用一種改進(jìn)的開(kāi)窗估計(jì)算法，對(duì)噪聲進(jìn)行實(shí)時(shí)跟蹤，而后利用在線估計(jì)得到的噪聲協(xié)方差矩陣進(jìn)行UKF濾波，從而得到狀態(tài)估計(jì)值與預(yù)測(cè)值，并與UKF方法進(jìn)行對(duì)比。

1 GPS/DR組合導(dǎo)航系統(tǒng)建模

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

在陸地上車輛的運(yùn)動(dòng)可以近似認(rèn)為是在二維平面內(nèi)的運(yùn)動(dòng)，且可分解為東西方向的運(yùn)動(dòng)和南北方向的運(yùn)動(dòng)。采用“當(dāng)前”統(tǒng)計(jì)模型［8-9］，系統(tǒng)狀態(tài)向量 X=［xeveaexnvnanε ψ］T，其中：xe，ve，ae表示載體東向位置分量、速度分量、加速度分量;xn，vn，an表示載體北向位置分量、速度分量、加速度分量;ε為陀螺儀的漂移誤差;ψ為里程儀標(biāo)定系數(shù)。

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

設(shè)采樣周期為T，將式(1)離散化后得離散化方程：

式中：τae，τan分別為載體東向北向加速度變化率的相關(guān)時(shí)間常數(shù);τε為陀螺儀的1階馬爾科夫常數(shù);分別為載體“當(dāng)前”東向、北向加速度分量的均值;ωae，ωan，ωε，ωψ分別為均值為 0、方差為，，，的高斯白噪聲。

1.2 系統(tǒng)的觀測(cè)方程

GPS接收機(jī)輸出的信號(hào)分別為xeo，xno，單位為m。陀螺儀輸出角速度ω，里程儀在一個(gè)采樣周期內(nèi)輸出距離S，令：式中：ωye，ωyn，εyω，εys分別為東向、北向、陀螺儀和里程儀的觀測(cè)噪聲，其近似均值為0、方差為，，，的高斯白噪聲。

觀測(cè)方程離散化得到非線性方程：

2 系統(tǒng)濾波算法設(shè)計(jì)

濾波算法流程如圖1。

圖1 濾波算法流程Fig.1 Filter algorithm process

2.1 UKF 濾波算法過(guò)程［10-11］

1)狀態(tài)初始化

初始估計(jì)值

初始方差：

2)取 k－1 時(shí)刻 Sigma點(diǎn) χk－1，則：

權(quán)值選?。?/p>

式中：尺度因素λ=α2(n+Γ)－n，Γ取0，α為一很小正數(shù)，β在高斯分布下最優(yōu)取2。

3)系統(tǒng)時(shí)間更新

4)系統(tǒng)測(cè)量更新

由于現(xiàn)實(shí)車輛運(yùn)動(dòng)中噪聲協(xié)方差矩陣具有隨機(jī)性和時(shí)變性，在傳統(tǒng)UKF濾波過(guò)程中，通過(guò)先驗(yàn)公式確定的噪聲統(tǒng)計(jì)特性在整個(gè)過(guò)程中是不變的，故當(dāng)噪聲協(xié)方差矩陣變化時(shí)，UKF濾波系統(tǒng)的精度和魯棒性會(huì)產(chǎn)生影響。

2.2 系統(tǒng)誤差協(xié)方差矩陣跟蹤

由于“當(dāng)前”統(tǒng)計(jì)模型系統(tǒng)誤差在觀測(cè)周期內(nèi)為常量或圍繞常量變化，可以采用固定窗口內(nèi)取平均值對(duì)誤差協(xié)方差矩陣進(jìn)行擬合［12］。通常情況下，開(kāi)窗大小N的取值決定了估計(jì)的精度。N取越大，精度越高［13］，但同時(shí)計(jì)算量也加大。因此，筆者提出一種自適應(yīng)方法來(lái)動(dòng)態(tài)調(diào)整開(kāi)窗大小，平衡濾波精度與算法計(jì)算速度：

式中：o(k)為k時(shí)刻系統(tǒng)的更新權(quán)值;N0為開(kāi)始時(shí)刻設(shè)定開(kāi)窗大小值;r(k)為分配系數(shù)，0≤r(k)≤1。當(dāng)車輛行駛姿態(tài)穩(wěn)定時(shí)，分配系數(shù)r(k)取較小值。

2.2.1 觀測(cè)噪聲協(xié)方差矩陣跟蹤根據(jù)開(kāi)窗估計(jì)法以及新息序列的定義：

利用SIGMA點(diǎn)集，得到UKF算法中，新息序列的協(xié)方差矩陣：

實(shí)時(shí)估計(jì)的協(xié)方差為：

式中：z0=k－Nk+1。觀測(cè)噪聲向量協(xié)方差矩陣Rk可以根據(jù)式(21)、式(22)在線估計(jì)得：

由式(23)可以得到由當(dāng)前觀測(cè)新息以及之前的迭代新息對(duì)觀測(cè)噪聲進(jìn)行協(xié)方差在線跟蹤后得到的估計(jì)值。

2.2.2 過(guò)程噪聲協(xié)方差矩陣跟蹤

模型系統(tǒng)誤差為qk，殘差Vk，在tk－i時(shí)刻：Vk=

由于“當(dāng)前”統(tǒng)計(jì)模型在短時(shí)間內(nèi)誤差在微小范圍內(nèi)浮動(dòng)，故滿足E(V^Xk－i)=0，根據(jù)開(kāi)窗估計(jì)原理，并由式(24)能得到模型系統(tǒng)誤差qk的無(wú)偏估計(jì)：

將式系統(tǒng)方程(2)代入式(25)，令 Uk=0，有：

狀態(tài)向量預(yù)報(bào)值 ^Xk的預(yù)報(bào)殘差V^Xk協(xié)方差矩陣：

取

又根據(jù)系統(tǒng)狀態(tài)方程(2)以及式(24)、式(26)和式(28)得到：

可以利用上式求得狀態(tài)方程過(guò)程噪聲協(xié)方差矩陣PX^k，并利用所求得的PX^k進(jìn)行標(biāo)準(zhǔn)UKF濾波。

3 系統(tǒng)仿真與分析

為了驗(yàn)證設(shè)計(jì)濾波系統(tǒng)的性能，比較與傳統(tǒng)UKF方法的濾波效果，設(shè)計(jì)了如下的仿真試驗(yàn)。濾波時(shí)間為200 s，采樣周期為1 s，車輛以東向速度10 m/s，北向速度10 m/s，做勻速直線運(yùn)動(dòng)，仿真參數(shù)：過(guò)程噪聲觀測(cè)噪聲m/s2)2。時(shí)間常數(shù)τae=τan=1 s，開(kāi)窗大小Nk取固定值 10，狀態(tài)初值取 X0=［0，10，0，0，10，0，0，0］T。其中實(shí)際過(guò)程噪聲Qk和觀測(cè)噪聲Rk按如下規(guī)律變化：

分別采用UKF和自適應(yīng)UKF方法進(jìn)行MATLAB仿真，得到如圖2的仿真曲線。從圖2東向位置誤差上來(lái)看，從50 s開(kāi)始當(dāng)噪聲統(tǒng)計(jì)特性變化以后，傳統(tǒng)UKF開(kāi)始有較大震蕩而自適應(yīng)UKF濾波能保持較好的收斂性能，濾波精度更高，誤差基本維持在3 m以內(nèi)。

圖2 東向、北向位置誤差(自適應(yīng)UKF/UKF)Fig.2 Position error in north direction(adaptive UKF/UKF)

通過(guò)對(duì)表1與圖2的觀察，可以看出，當(dāng)噪聲統(tǒng)計(jì)特性存在變化時(shí)，自適應(yīng)UKF對(duì)比UKF有著明顯的優(yōu)勢(shì)。UKF算法會(huì)出現(xiàn)局部發(fā)散，整體震蕩的現(xiàn)象。自適應(yīng)UKF方法，加入了誤差估計(jì)，雖然增加了算法計(jì)算時(shí)間，但比UKF方法精度更高，魯棒性更強(qiáng)。

表1 東向、北向誤差統(tǒng)計(jì)對(duì)比Table 1 East lnorth error statistics comparison

4 結(jié)語(yǔ)

針對(duì)車輛定位過(guò)程中的噪聲擾動(dòng)問(wèn)題，將基于開(kāi)窗估計(jì)觀測(cè)噪聲和過(guò)程協(xié)方差矩陣跟蹤的自適應(yīng)UKF濾波算法應(yīng)用于陸地車輛GPS/DR組合導(dǎo)航定位過(guò)程。并通過(guò)MATLAB軟件進(jìn)行仿真試驗(yàn)，結(jié)果表明，自適應(yīng)UKF算法要優(yōu)于傳統(tǒng)UKF。

［1］房建成，申功勛，萬(wàn)德鈞.一種自適應(yīng)聯(lián)合卡爾曼濾波器及其在車載GPS/DR組合導(dǎo)航系統(tǒng)中的應(yīng)用研究［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，1998，6(4)：1-6.Fang Jianchen，Shen Gongxun，Wan Dejun.An adaptive combined Kalman filter used for GPS/DR integrated navigation system in vehicle［J］.Journal of Chinese Inertial Technology，1998，6(4)：1-6.

［2］劉國(guó)海，施維，李康吉.插值改進(jìn)EKF算法在組合導(dǎo)航中的應(yīng)用［J］.儀器儀表學(xué)報(bào)，2007，28(10)：1897-1901.Liu Guohai，Shi Wei，Li Kangji.The application of Interpolation improved EKF algorithm used in the integrated navigation［J］.Journal of Instrument and Meters，2007，28(10)：1897-1901.

［3］Julier S J，Uhlmann J K.A General Method for Approximating Nonlinear Transformations of Probability Distributions［R］.Oxford：Robotics Research Group，Department of Engineering Science，University of Oxford，1994.

［4］Julier S J，Uhlmann J K.A new method for the nonlinear transformation of means and covariance in filters and estimators［J］.IEEE transportation.A.C.，2000，45(3)：476-481.

［5］Ramji S，Latha G，Ramakrishnan S.Estimation and interpolation of underwater low frequency ambient noise spectrum using artificial neural networks［J］.Applied Acoustics，2009，70(8)：1111-1115.

［6］石勇，韓崇紹.自適應(yīng)UKF算法在目標(biāo)跟蹤中的應(yīng)用［J］.自動(dòng)化學(xué)報(bào)，2011，37(6)：755-759.Shi Yong，Han Chongshao.Adaptive UKF algorithm used in target tracking［J］.ACTA Automatica Sinica，2001，37(6)：755-759.

［7］周衛(wèi)東，吉宇人，喬相偉，等.基于修正PSO-UKF的SINS/GPS組合導(dǎo)航濾波算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(5)：565-570.Zhou Weidong，Ji Yuren，Qiao Xiangwei，et al.An SINS/GPS integrated navigation filter algorithm based on modified PSP-UKF［J］.Journal of Chinese Inertial Technology，2011，19(5)：565-570.

［8］張文乾.基于GPS/DR和GSM的車輛定位導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)［D］.哈爾濱：哈爾濱工程大學(xué)，2009.Zhang Wenqian.The Design and Implementation of Vehicle Navigation System Based on GPS/DR and GSM［D］.Harbin：Harbin Engineering University，2009.

［9］岳海波.GPS/DR車載組合導(dǎo)航定位系統(tǒng)研究［D］.南京：南京理工大學(xué)，2009.Yue Haibo.Research of Vehicle Integrated Navigation Positioning System Based on GPS and DR［D］.Nanjing：Nanjing University of Science and Technology，2009.

［10］陳陽(yáng)舟，孫章固，馬海波.基于平方根UKF的車輛組合導(dǎo)航［J］.系統(tǒng)工程與電子技術(shù)，2008，33(5)：926-928.Chen Yangzhou，Sun Zhanggu，Ma Haibo.Vehicle integrated navigation based on square root UKF［J］.Journal of Systems Engineering and Electronics，2008，33(5)：926-928.

［11］劉濤，解永春.UKF穩(wěn)定性研究及其在相對(duì)導(dǎo)航中的應(yīng)用［J］.宇航學(xué)報(bào)，2010，31(3)：739-748.Liu Tao，Xie Yongchun.UKF stability study and its application in the relative navigation［J］.Journal Acta Astronautica，2010，31(3)：739-748

［12］楊元喜，張雙成.導(dǎo)航解算中的系統(tǒng)誤差及其協(xié)方差矩陣擬合［J］.測(cè)繪學(xué)報(bào)，2004，33(5)：20-23.Yang Yuanxi，Zhang Shuangchen.The error of navigation system and the calculating and fitting of covariance matrix［J］.Journal of Surveying and Mapping，2004，33(5)：20-23.

［13］許承權(quán)，吳文英，吳邵祖.單頻GPS精密單點(diǎn)定位的逐次濾波法模型及其精度分析［J］.測(cè)繪科學(xué)，2010，35(4)：23-25.Xu Chenquan，Wu Wenying，Wu Shaozu.Single frequency GPS precise point positioning of successive filter model and its accuracy analysis［J］.Surveying and Mapping，2010，35(4)：23-25.