陸文昌，費龑棟，陳 龍，汪若塵

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

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

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

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

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

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

在陸地上車輛的運動可以近似認(rèn)為是在二維平面內(nèi)的運動，且可分解為東西方向的運動和南北方向的運動。采用“當(dāng)前”統(tǒng)計模型［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ù);τε為陀螺儀的1階馬爾科夫常數(shù);分別為載體“當(dāng)前”東向、北向加速度分量的均值;ωae，ωan，ωε，ωψ分別為均值為 0、方差為，，，的高斯白噪聲。

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

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

觀測方程離散化得到非線性方程：

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

濾波算法流程如圖1。

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

2.1 UKF 濾波算法過程［10-11］

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

初始估計值

初始方差：

2)取 k－1 時刻 Sigma點 χk－1，則：

權(quán)值選取：

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

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

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

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

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

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

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

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

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

實時估計的協(xié)方差為：

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

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

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

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

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

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

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

取

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

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

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

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

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

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

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

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

4 結(jié)語

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

［1］房建成，申功勛，萬德鈞.一種自適應(yīng)聯(lián)合卡爾曼濾波器及其在車載GPS/DR組合導(dǎo)航系統(tǒng)中的應(yīng)用研究［J］.中國慣性技術(shù)學(xué)報，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］劉國海，施維，李康吉.插值改進EKF算法在組合導(dǎo)航中的應(yīng)用［J］.儀器儀表學(xué)報，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］.自動化學(xué)報，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］.中國慣性技術(shù)學(xué)報，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è)計與實現(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］陳陽舟，孫章固，馬海波.基于平方根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)定性研究及其在相對導(dǎo)航中的應(yīng)用［J］.宇航學(xué)報，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］.測繪學(xué)報，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精密單點定位的逐次濾波法模型及其精度分析［J］.測繪科學(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.