趙 巖，高社生，楊 一

（西北工業(yè)大學(xué) 自動化學(xué)院，西安 710072）

自適應(yīng)SDV-UPF算法及其在緊組合中的應(yīng)用

趙 巖，高社生，楊 一

（西北工業(yè)大學(xué) 自動化學(xué)院，西安 710072）

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

緊組合導(dǎo)航系統(tǒng)；偽距；奇異值分解；無跡粒子濾波

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

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

1 ASVD-UPF算法設(shè)計(jì)

1.1 自適應(yīng)因子設(shè)計(jì)

考慮如下非線性系統(tǒng)模型

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

預(yù)測殘差含有未經(jīng)觀測信息修正的狀態(tài)[6]，更能反映動態(tài)系統(tǒng)受到的擾動。因此，以預(yù)測殘差為變量，構(gòu)造狀態(tài)模型的誤差判別統(tǒng)計(jì)量及自適應(yīng)因子。預(yù)測殘差表示為：

用預(yù)測殘差構(gòu)造狀態(tài)模型誤差判別統(tǒng)計(jì)量：

自適應(yīng)因子函數(shù)選用：

1.2 奇異值分解

1.3 提出的ASVD-UPF算法

ASVD-UPF算法步驟如下：

1）初始化

從先驗(yàn)密度分布函數(shù)中抽取l個(gè)樣本點(diǎn)，使

通過擴(kuò)維得到：

權(quán)值為：

式中，α表示 Sigma點(diǎn)相對均值的擴(kuò)散程度(通常)，β是關(guān)于系統(tǒng)先驗(yàn)信息的參數(shù)。對高斯分布，β=2最佳[1]，l為系統(tǒng)狀態(tài)維數(shù)。

2）奇異值分解，計(jì)算sigma點(diǎn)集

計(jì)算特征點(diǎn)協(xié)方差矩陣和2l+1個(gè)Sigma點(diǎn)向量：

式(10)中，ρ為縮放比例因數(shù)，較為恰當(dāng)?shù)娜≈禐?，σi,k為奇異值分解因子。

3）時(shí)間更新

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

4）量測更新

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

選擇重要性函數(shù)為：

5）從建議分布函數(shù)中抽取粒子，并進(jìn)行權(quán)值更新與歸一化：

得到歸一化權(quán)值：

6）重采樣

2 GPS/SINS/PL組合導(dǎo)航系統(tǒng)模型

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

2.1 組合系統(tǒng)的狀態(tài)方程

在東北天坐標(biāo)系中，組合系統(tǒng)的狀態(tài)量選取為

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

式中，Cω歐拉平臺誤差角矩陣，分別為導(dǎo)航坐標(biāo)系(n系)至計(jì)算坐標(biāo)系(c系)的姿態(tài)變換矩陣和機(jī)體坐標(biāo)系(b系)至c系的，為地球自轉(zhuǎn)角速度實(shí)際值和理想值，理想陀螺角速度輸出，為 n系至地心慣性坐標(biāo)系(i系)的角速度，分別為n系至地球坐標(biāo)系(e系)的角速度實(shí)際值和理想值，分別為的計(jì)算誤差，實(shí)際加速度計(jì)比力輸出和加速度計(jì)測量誤差，為實(shí)際速度和速度誤差，δg重力加速度誤差，L和h為緯度和高度、為當(dāng)?shù)刈游缛兔先η拾霃?，?32)中各參數(shù)的關(guān)系推導(dǎo)參考文獻(xiàn)[10]。

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

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

2.2 組合系統(tǒng)量測方程

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

式中，

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

SINS與第i顆衛(wèi)星之間的偽距率為：

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

組合系統(tǒng)的量測量和量測方程分別為

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

3 仿真實(shí)驗(yàn)與分析

3.1 仿真初始條件

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

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

3.2 結(jié)果分析

將現(xiàn)有的UKF、PF和本文設(shè)計(jì)的ASVD-UPF算法應(yīng)用到所設(shè)計(jì)的基于偽距/偽距率的GPS/SINS/PL緊組合導(dǎo)航系統(tǒng)中，進(jìn)行仿真實(shí)驗(yàn)，結(jié)果如圖1～3所示。

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

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

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

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

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

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

4 結(jié) 論

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

（References）：

[1]高社生，王建超，焦雅林.自適應(yīng)SVD-UKF算法及在組合導(dǎo)航的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，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]高社生，閻海峰，高怡.模糊抗差自適應(yīng)粒子濾波及其在組合導(dǎo)航中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，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]薛麗，高社生，趙巖.權(quán)值自適應(yīng)調(diào)整Unscented粒子濾波及其在組合導(dǎo)航中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，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]嚴(yán)恭敏，嚴(yán)衛(wèi)生，許德民.基于歐拉平臺誤差角的SINS非線性誤差模型研究[J].西北工業(yè)大學(xué)學(xué)報(bào)，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

國家自然科學(xué)基金（基金號61174193）；陜西省自然科學(xué)基金（基金號NBYU0004）；航天科技創(chuàng)新基金（基金號CASC201102）

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

聯(lián) 系 人：高社生（1956—），男，教授，博士生導(dǎo)師。E-mail：gshshnpu@163.com