自適應SDV-UPF算法及其在緊組合中的應用

趙 巖，高社生，楊 一

（西北工業大學 自動化學院，西安 710072）

自適應SDV-UPF算法及其在緊組合中的應用

趙 巖，高社生，楊 一

（西北工業大學 自動化學院，西安 710072）

針對粒子濾波存在重要性密度函數難以選取和系統狀態協方差陣可能出現的負定性問題，提出一種新的自適應奇異值分解無跡粒子濾波(ASVD-UPF)算法。該算法采用自適應因子修正動力學模型誤差，通過奇異值分解抑制系統狀態協方差矩陣的負定性，并以改進的 UKF算法產生重要性密度函數，以彌補粒子濾波的缺陷，使該算法適用于非線性、非高斯系統模型的濾波計算。將提出的算法應用到所設計的GPS/SINS/PL緊組合導航系統中進行仿真驗證，結果表明，提出算法的經、緯度誤差、速度誤差和姿態誤差范圍分別控制在(-0.5″,+0.5″)、(-0.8 m/s,+0.8 m/s) 和(-1′,+1′)以內，誤差估計的精度和收斂速度明顯優于UKF和PF算法，能提高組合導航系統的解算精度。

緊組合導航系統；偽距；奇異值分解；無跡粒子濾波

目前，解決非線性系統模型常用的濾波算法主要有擴展卡爾曼濾波(Extended Kalman Filtering,EKF)、無跡卡爾曼濾波(Unscented Kalman Filtering,UKF)和粒子濾波(Particle Filtering,PF)算法。EKF算法的主要思想是對非線性函數進行線性化近似，忽略或逼近高階項，以解決非線性系統的濾波計算問題。但是，當系統非線性度較嚴重時，忽略Taylor展開式的高階項將引起較大的線性化誤差，導致 EKF的濾波誤差增大甚至發散。其次，Jacobian矩陣的計算量大[1]。為了克服EKF 存在的缺點，Julier[2]提出了基于Unscented變換(UT)的UKF算法，該算法采用確定性采樣來近似非線性函數的概率分布，避免了求解Jacobian矩陣計算量大的問題，且對高斯分布的逼近精度可達到三階[3]。但對于非高斯、強非線性系統模型，濾波精度難以保證。PF通過尋找一組在狀態空間中傳播的隨機樣本對概率密度函數進行近似，以樣本均值代替積分運算，從而獲得狀態的最小方差估計。PF適用于非線性非高斯系統的狀態估計，尤其對強非線性系統的濾波問題有獨特的優勢，擺脫了解決非線性濾波問題時，隨機量必須滿足高斯分布的制約條件。但PF存在重要性密度函數難以選取和粒子匱乏等問題[4]。為了解決系統狀態協方差陣可能出現的負定性問題和PF的缺陷，文獻[1]提出自適應奇異值分解的無跡卡爾曼濾波，將其應用到SINS/SAR組合導航系統中，在一定程度上改善了濾波性能，但由于UKF的局限性，缺乏對非高斯系統的處理能力；文獻[5]提出了一種改進的無跡粒子濾波算法。該算法在無跡粒子濾波的采樣過程中吸收權值自適應調整的優點，考慮最新量測影響，通過歐氏距離和反映量測噪聲統計特性的精度因子來自適應的調整粒子對應權值分布，增加有用粒子的權值，降低粒子退化程度，保持粒子多樣性。然而狀態協方差陣可能出現的負定性問題。

為了克服上述非線性濾波算法的不足，本文提出一種適合于非線性系統模型的自適應奇異值分解無跡粒子濾波(ASDV-UPF)算法，該算法在充分吸收自適應濾波、奇異值分解和無跡粒子濾波優點的基礎上，采用自適應因子修正動力學模型誤差，通過奇異值分解抑制系統狀態協方差矩陣的負定性，并采用改進的UKF產生重要性密度函數，以彌補PF的缺陷。將提出的ASDV-UPF算法應用到GPS/捷聯慣性導航(Strapdown Inertial Navigation System，SINS)/偽衛星(Pseudolites，PL)緊組合系統中進行仿真驗證。結果表明，提出的算法適合于非線性動態系統的濾波計算，不但具有良好的數值穩定性和魯棒性，而且能有效改善濾波性能，提高組合導航的解算精度。

1 ASVD-UPF算法設計

1.1 自適應因子設計

考慮如下非線性系統模型

式中，Xk是狀態向量，zk是量測量，f(·)和h(·)是非線性函數；wk和vk分別為系統過程噪聲和量測噪聲。

預測殘差含有未經觀測信息修正的狀態[6]，更能反映動態系統受到的擾動。因此，以預測殘差為變量，構造狀態模型的誤差判別統計量及自適應因子。預測殘差表示為：

用預測殘差構造狀態模型誤差判別統計量：

自適應因子函數選用：

1.2 奇異值分解

1.3 提出的ASVD-UPF算法

ASVD-UPF算法步驟如下：

1）初始化

從先驗密度分布函數中抽取l個樣本點，使

通過擴維得到：

權值為：

式中，α表示 Sigma點相對均值的擴散程度(通常)，β是關于系統先驗信息的參數。對高斯分布，β=2最佳[1]，l為系統狀態維數。

2）奇異值分解，計算sigma點集

計算特征點協方差矩陣和2l+1個Sigma點向量：

式(10)中，ρ為縮放比例因數，較為恰當的取值為，σi,k為奇異值分解因子。

3）時間更新

式(13)中，svd{·}為SVD分解算子，

4）量測更新

式(16)中的Q與公式(20)中的R分別為系統噪聲方差和量測噪聲方差。

選擇重要性函數為：

5）從建議分布函數中抽取粒子，并進行權值更新與歸一化：

得到歸一化權值：

6）重采樣

2 GPS/SINS/PL組合導航系統模型

由于GPS提供的位置和速度并非原始量測數據，而是經過數據處理后得到的計算值，因而各量測間存在一定的相關性，這將會影響導航參數解的精確性。而衛星接收機提供的偽距、偽距率信號是接收機接收的原始信息，不需要經過導航解算，且各偽距、偽距率信號的誤差獨立、互不相關[8]。因此，基于偽距、偽距率的GPS/SINS組合系統不僅便于導航濾波器的設計與實現，而且該方式大大增強了測量方程的可觀性。同時在理論上，采用偽距、偽距率組合方式的效果比位置、速度組合方式好[9]。當GPS信號被遮掩、被干擾或者可見星數不足時，用PL代替GPS衛星，形成SINS/PL組合導航系統，可繼續保持較高精度完成導航任務。為此本文采用基于偽距/偽距率的GPS/SINS/PL緊組合方式，提高組合導航系統性能。

2.1 組合系統的狀態方程

在東北天坐標系中，組合系統的狀態量選取為

式中，δL,δλ,δh為位置誤差，為速度誤差，為姿態誤差，為陀螺隨機漂移，為陀螺一階馬爾科夫過程，為加計零偏，δtu和δtru分別為由時鐘誤差引起的等效距離誤差和時鐘誤差頻率引起的等效距離率誤差。狀態量存在如下非線性關系：

式中，Cω歐拉平臺誤差角矩陣，分別為導航坐標系(n系)至計算坐標系(c系)的姿態變換矩陣和機體坐標系(b系)至c系的，為地球自轉角速度實際值和理想值，理想陀螺角速度輸出，為 n系至地心慣性坐標系(i系)的角速度，分別為n系至地球坐標系(e系)的角速度實際值和理想值，分別為的計算誤差，實際加速度計比力輸出和加速度計測量誤差，為實際速度和速度誤差，δg重力加速度誤差，L和h為緯度和高度、為當地子午圈和卯酉圈曲率半徑，式(32)中各參數的關系推導參考文獻[10]。

組合系統的狀態方程為：

式中，f(·)為連續非線性函數，由式(32)給出，W(t)為狀態噪聲。

2.2 組合系統量測方程

設SINS在ECEF坐標系中位置為(x,y,z)，則式(34)在(x,y,z)處泰勒展開為：

式中，

GPS(或PL)接收機相對于第i顆衛星測得的偽距為

SINS與第i顆衛星之間的偽距率為：

GPS(或 PL)接收機相對于第i顆衛星測得的偽距率為：

組合系統的量測量和量測方程分別為

式(41)中，h(·)為連續非線性函數，V(t)為量測噪聲。

3 仿真實驗與分析

3.1 仿真初始條件

假設無人機初始位置為東經108.997°，北緯34.246°，高度1000 m，飛艇以20 m/s的速度勻速向北飛行。仿真時間為1000 s，SINS的采樣周期為0.01 s，GPS的采樣周期為1 s，濾波周期為1 s。其他仿真參數如表1所示。

表1 仿真初始條件Tab.1 Initial conditions of simulation

3.2 結果分析

將現有的UKF、PF和本文設計的ASVD-UPF算法應用到所設計的基于偽距/偽距率的GPS/SINS/PL緊組合導航系統中，進行仿真實驗，結果如圖1～3所示。

圖1是組合導航系統經度、緯度和高度誤差曲線，可以看出，采用UKF和PF算法所獲得的濾波精度基本相當，經度和緯度誤差穩定在(-1.2″,+1.2″)，UKF算法的高度誤差穩定在(-5 m,+7 m)，PF算法的高度誤差穩定在(-5 m,+3 m)；采用設計的ASVD-UPF算法獲得的位置誤差更小，經度、緯度和高度誤差范圍分別控制在(-0.5″,+0.5″)、(-0.5″,+0.5″)和(-2 m,+2 m)，而且收斂速度更快。

圖2是組合導航系統速度誤差曲線，通過 UKF計算得到的速度誤差范圍在(-2 m/s,+2 m/s)；由PF得到的速度誤差控制在(-1.5 m/s,+1.5 m/s)；而本文設計的算法得到速度誤差為(-0.8 m/s,+0.8 m/s)。設計的ASVD-UPF算法對于速度誤差的估計精度要高于UKF和PF算法的估計精度。

圖3是組合導航系統姿態誤差曲線，通過UKF計算得到的三個方向的姿態誤差范圍分別為(-1′,+1′)、(-1′,+1′)和(-2′,+2′)；由PF得到的三個方向的姿態誤差范圍分別為(-1′,+1′)、(-1′,+1′)和(-1.5′,+1.5′)；而本文設計的算法得到三個方向的姿態誤差范圍分別為(-0.5′,+0.5′)、(-0.5′,+0.5′)和(-1′,+1′)。設計的ASVD-UPF算法對于姿態誤差的估精度也高于 UKF和PF算法的估計精度，不僅反映出設計ASVD-UPF算法對姿態誤差估計精度的優越性，而且，從仿真圖對比可以看出，采用 ASVD-UPF算法的航向角誤差的收斂速度更快。

圖1 三種算法輸出的位置誤差曲線Fig.1 Position errors of the three filtering methods

圖2 三種算法輸出的速度誤差曲線Fig.2 Velocity errors of the three filtering methods

圖3 三種算法輸出的姿態誤差曲線Fig.3 Attitude errors of the three filtering methods

4 結 論

采用 PL輔助 GPS，設計了基于偽距/偽距率的GPS/SINS/PL緊組合導航系統，以克服GPS信號被遮擋、被干擾或是可見星數不足時導致導航精度下降的缺陷，并提出自適應奇異值分解無跡粒子濾波(ASDV-UPF)算法。該算法充分吸收自適應濾波、奇異值分解和無跡粒子濾波的優點，采用自適應因子修正動力學模型誤差，通過奇異值分解抑制系統狀態協方差矩陣的負定性，并采用改進的UKF產生重要性密度函數，以彌補PF的缺陷。通過仿真實驗驗證了設計的ASVD-UPF算法在誤差估計精度和收斂速度明顯優于UKF和PF算法，提高了非線性導航系統的導航定位精度。

（References）：

[1]高社生，王建超，焦雅林.自適應SVD-UKF算法及在組合導航的應用[J].中國慣性技術學報，2010，18(6)：737-741，765.GAO She-sheng,WANG Jian-chao,JIAO Ya-lin.Adaptive SVD-UKF algorithm and application to integrated navigation[J].Journal of Chinese Inertial Technology,2010,18(6):737-741,765.

[2]Julier S J,Uhlmann J K.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE,2004,92(3):401-422.

[3]Julier S J,Laviola J J.On Kalman filtering with nonlinear equality constranits[J].IEEE Transactions on Signal Processing,2007,55(6):2774-2784.

[4]高社生，閻海峰，高怡.模糊抗差自適應粒子濾波及其在組合導航中的應用[J].中國慣性技術學報，2010，18(5)：561-566.GAO She-sheng,YAN Hai-feng,GAO Yi.Fuzzy robust adaptively particle filter method and application to integrated navigation system[J].Journal of Chinese Inertial Technology,2010,18(5):561-566.

[5]薛麗，高社生，趙巖.權值自適應調整Unscented粒子濾波及其在組合導航中的應用[J].中國慣性技術學報，2012，20(4)：459-463.XUE Li,GAO She-sheng,ZHAO Yan.Unscented particle filtering with adaptive adjusted weight and its application in integrated navigation[J].Journal of Chinese Inertial Technology,2012,20 (4):459-463.

[6]GAO She-sheng,GAO Yi,ZHONG Yong-min,et al.Random weighting estimation method for dynamic navigation positioning[J].Chinese Journal of Aeronautics.2011,24:318-323.

[7]GAO She-sheng,ZHONG Yong-min,LI Wei.Robust adaptive filtering method for SINS/SAR integrated navigation system[J].Aerospace Science and Technology.2011,15:425-430.

[8]GUO Zhen,HAO Yanling,SUN Feng,et al.Sigma-point Kalman filtering for tightly-coupled GPS/INS[C]//IEEE International Symposium on Knowledge Acquisition and Modeling Workshop.2008:844-847.

[9]NIE Qi,GAO Xiao-ying.Comparison of nonlinear filtering approach in tightly-coupled GPS/INS navigation system[C]//2012 10th World Congress on Intelligent Control and Automation.2012:1176-1181.

[10]嚴恭敏，嚴衛生，許德民.基于歐拉平臺誤差角的SINS非線性誤差模型研究[J].西北工業大學學報，2009，27(4)：511-516.YAN Gong-min,YAN Wei-sheng,XU De-min.A SINS nonlinear error model reflecting better characteristics of SINS errors[J].Journal of Northwestern Polytechnical University,2009,27(4):511-516.

Adaptive SVD-UPF algorithm and application to tightly-coupled integrated navigation

ZHAO Yan,GAO She-sheng,YANG Yi
(School of Automation,Northwester Polytechnical University,Xi’an 710072,China)

In view that the importance density function in particle filter is hard to select and the system state covariance matrix is negative definiteness,a new adaptive unscented particle filter based on singular value decomposition (ASVD-UPF) was proposed.The new algorithm is capable of correcting dynamic model error by adaptive factor and restraining the negative definiteness of system state covariance matrix by SVD as well as generating the importance density function to offset defects of PF.With the above advantages,the proposed algorithm is suitable to be applied to the filter calculation of nonlinear and non-Gaussian system model.The proposed algorithm was applied to the GPS/SINS/PL tightly-coupled integrated navigation system and the simulation test was made.The simulation results show that the errors of longitude,latitude,velocity and attitude could be controlled to under (-0.5″,+0.5″),(-0.8 m/s,+0.8 m/s) and (-1′,+1′) respectively by the proposed algorithm.It is proved that the error estimation precision and convergence speed of the proposed algorithm are better than that of UKF and PF,and the positioning precision of integrated navigation is improved.

tightly-coupled integrated navigation;pseudorange;singular value decomposition;unscented particle filter

U666.1

：A

1005-6734(2014)01-0083-06

10.13695/j.cnki.12-1222/o3.2014.01.017

2013-07-06；

：2013-11-01

國家自然科學基金（基金號61174193）；陜西省自然科學基金（基金號NBYU0004）；航天科技創新基金（基金號CASC201102）

趙巖（1983—），男，博士研究生，從事組合導航研究。E-mail：zytyler@163.com

聯 系 人：高社生（1956—），男，教授，博士生導師。E-mail：gshshnpu@163.com