車輛組合導航系統自適應UKF濾波算法

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

(江蘇大學汽車與交通工程學院，江蘇鎮江212013)

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

但UKF算法是基于精確的數學模型和過程噪聲和觀測噪聲已知為基礎的，當噪聲統計特性將發生變化，UKF算法的精度和穩定性將會降低。針對此問題，S.Ranji，等［5］提出了利用神經網絡理論跟蹤噪聲統計特性;石勇，等［6］提出了一種改進的Sage-Husa濾波;周衛東，等［7］提出了 PSO-UKF 組合濾波，都取得了一定的成效。

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

1 GPS/DR組合導航系統建模

1.1 系統狀態方程

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

系統的狀態方程為：

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

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

1.2 系統的觀測方程

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

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

2 系統濾波算法設計

濾波算法流程如圖1。

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

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

1)狀態初始化

初始估計值

初始方差：

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

權值選取：

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

3)系統時間更新

4)系統測量更新

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

2.2 系統誤差協方差矩陣跟蹤

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

式中：o(k)為k時刻系統的更新權值;N0為開始時刻設定開窗大小值;r(k)為分配系數，0≤r(k)≤1。當車輛行駛姿態穩定時，分配系數r(k)取較小值。

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

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

實時估計的協方差為：

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

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

2.2.2 過程噪聲協方差矩陣跟蹤

模型系統誤差為qk，殘差Vk，在tk－i時刻：Vk=

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

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

狀態向量預報值 ^Xk的預報殘差V^Xk協方差矩陣：

取

又根據系統狀態方程(2)以及式(24)、式(26)和式(28)得到：

可以利用上式求得狀態方程過程噪聲協方差矩陣PX^k，并利用所求得的PX^k進行標準UKF濾波。

3 系統仿真與分析

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

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

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

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

表1 東向、北向誤差統計對比Table 1 East lnorth error statistics comparison

4 結語

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

［1］房建成，申功勛，萬德鈞.一種自適應聯合卡爾曼濾波器及其在車載GPS/DR組合導航系統中的應用研究［J］.中國慣性技術學報，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算法在組合導航中的應用［J］.儀器儀表學報，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］石勇，韓崇紹.自適應UKF算法在目標跟蹤中的應用［J］.自動化學報，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］周衛東，吉宇人，喬相偉，等.基于修正PSO-UKF的SINS/GPS組合導航濾波算法［J］.中國慣性技術學報，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］.哈爾濱：哈爾濱工程大學，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］.南京：南京理工大學，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的車輛組合導航［J］.系統工程與電子技術，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穩定性研究及其在相對導航中的應用［J］.宇航學報，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］楊元喜，張雙成.導航解算中的系統誤差及其協方差矩陣擬合［J］.測繪學報，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］許承權，吳文英，吳邵祖.單頻GPS精密單點定位的逐次濾波法模型及其精度分析［J］.測繪科學，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.