改進(jìn)的基于奇異值分解的抗差容積卡爾曼濾波算法在全球定位導(dǎo)航中的應(yīng)用

王姚宇, 陳仁文*, 張 祥

(1.南京航空航天大學(xué)機(jī)械結(jié)構(gòu)力學(xué)及控制國家重點(diǎn)實(shí)驗(yàn)室, 南京 210016； 2.東南大學(xué)信息科學(xué)與工程學(xué)院, 南京 211102)

非線性動態(tài)系統(tǒng)的非線性和噪聲不確定問題,導(dǎo)致傳統(tǒng)卡爾曼濾波不適用于非線性模型的狀態(tài)估計(jì)[1-2]。擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)雖然可用于非線性系統(tǒng)濾波[3-5],但是其線性化環(huán)節(jié)會引入高階截?cái)嗾`差,且Jacobi矩陣的計(jì)算會增加運(yùn)算的難度,在實(shí)際工程應(yīng)用中效果不佳。無跡卡爾曼濾波(unscented Kalman filter，UKF)克服了EKF的局限性[6-10],UKF基于(unscented transformation，UT)變換構(gòu)建了一系列Sigma點(diǎn),以此逼近狀態(tài)向量的后驗(yàn)概率密度函數(shù),實(shí)現(xiàn)簡單且精度遠(yuǎn)遠(yuǎn)高于EKF,但是在狀態(tài)發(fā)生突變的情況下,UKF的魯棒性不佳,精度容易受到影響。容積卡爾曼濾波(cubature Kalman filter，CKF)利用三階球面徑向容積準(zhǔn)則對概率密度函數(shù)進(jìn)行近似[11-14],其采樣點(diǎn)的權(quán)值均為正數(shù),相較于上述濾波算法,CKF的精度和穩(wěn)定性都有所提高,為解決非線性估計(jì)問題提供了一個新思路。

但是非線性濾波普遍存在跟蹤能力弱和自適應(yīng)能力差的問題,當(dāng)系統(tǒng)受到異常觀測值或者狀態(tài)突變的影響時,濾波器容易產(chǎn)生精度降低甚至發(fā)散的情況。文獻(xiàn)[15]提出了均值濾波的思想對采樣數(shù)據(jù)進(jìn)行了預(yù)處理,降低了異常觀測值的干擾,增快了系統(tǒng)的響應(yīng)時間。文獻(xiàn)[16-17]中引入了奇異值分解(singular value decomposition，SVD)的辦法,減小了先驗(yàn)協(xié)方差矩陣負(fù)定性變化,保證算法可以平穩(wěn)地迭代。文獻(xiàn)[18]提出一種抗差方法,能夠有效減弱波動較大數(shù)據(jù)對于濾波器穩(wěn)定性的影響。

在卡爾曼濾波器進(jìn)行迭代更新時,波動較大的數(shù)據(jù)采集點(diǎn)會減慢濾波器的收斂速度,影響數(shù)據(jù)的穩(wěn)定性。在工程試驗(yàn)中,環(huán)境干擾會引入較大的噪聲,對量測數(shù)據(jù)產(chǎn)生影響,嚴(yán)重偏離真實(shí)值。為了解決濾波更新狀態(tài)不穩(wěn)定和異常觀測值影響大的問題,提出一種改進(jìn)的基于SVD的CKF抗差算法,采用均值濾波的方法,對干擾大的數(shù)據(jù)、瞬時脈沖信號等進(jìn)行處理,同時使用抗差方法減弱異常觀測值的影響,使濾波器可以更好地運(yùn)行下去,改善濾波效果,提高濾波器更新迭代的穩(wěn)定性,對異常觀測值有較好的修正效果。計(jì)算仿真驗(yàn)證該算法的有效性。

1 算法描述

1.1 均值濾波

均值濾波的思想是先選取一個長度為N的均值濾波窗口,從頭開始,以N個采樣數(shù)據(jù)為一組,求取其均值作為一個新的采樣點(diǎn),隨后將窗口向后移動一個數(shù)據(jù),用新的采樣數(shù)據(jù)替換掉窗口的第1個采樣數(shù)據(jù),以此類推,獲得一組求取均值后的新的采樣點(diǎn),作為濾波器的輸入值[19],其基本工作原理如圖1所示。

圖1 均值濾波基本工作原理Fig.1 Basic working principle of mean filtering

均值濾波的原理用算式可以表示為

(1)

式(1)中：y(i)為原采樣數(shù)據(jù)；Y(n)為均值濾波后新產(chǎn)生的數(shù)據(jù),對于長度為M的數(shù)據(jù)列而言,n=1,2, 3,…,M-N,N為均值濾波窗口長度。均值濾波窗口的長度可以根據(jù)實(shí)際采樣點(diǎn)數(shù)的多少進(jìn)行調(diào)節(jié),合適的窗口大小可以獲得更好的濾波效果。

1.2 SVD分解

隨著容積卡爾曼濾波器的不斷迭代,狀態(tài)向量和觀測向量的協(xié)方差陣可能會出現(xiàn)負(fù)定的情況,導(dǎo)致濾波器的狀態(tài)無法準(zhǔn)確更新,甚至出現(xiàn)無法收斂的現(xiàn)象。SVD分解也叫奇異值分解,其數(shù)值計(jì)算具有較強(qiáng)的魯棒性,用SVD分解代替Cholesky分解,計(jì)算容積點(diǎn)協(xié)方差矩陣,可以有效增強(qiáng)濾波更新的穩(wěn)定性。

SVD分解的方法是,對于一個m×n維的實(shí)數(shù)矩陣A,可以分解為

A=UΛVT

(2)

式(2)中：U、V均為單位正交陣,分別稱為左奇異矩陣和右奇異矩陣；Λ僅在主對角線上有值,其余元素均為0,一般形式表示為

(3)

式(3)中：S=diag(σ1,σ2,…,σr)為奇異值矩陣,且σ1≥σ2≥…≥σr≥0,r為矩陣S的秩。

1.3 抗差方法

觀測信息在不斷參與更新過程,異常的觀測值會嚴(yán)重影響濾波結(jié)果,需要引入抗差因子減小異常觀測值對CKF濾波過程的干擾[20],當(dāng)觀測信息精度高時,加大觀測值在狀態(tài)估計(jì)中的權(quán)值；反之,當(dāng)觀測信息誤差偏大時,降低觀測值在狀態(tài)估計(jì)中的權(quán)重。

類似IGGIII等價權(quán)函數(shù)模型,將觀測殘差值進(jìn)行分類,分為3個等級進(jìn)行篩選,分別對應(yīng)3種不同的抗差因子,即

(4)

式(4)中：k0、k1為常值,通常選取k0=1.5～2.0,k1=3.0～8.5；sv為標(biāo)準(zhǔn)化殘差[21]。這里要注意的是,抗差因子不能設(shè)置為0,否則可能影響觀測向量協(xié)方差矩陣的迭代更新。

1.4 改進(jìn)的抗差SVD-CKF算法

考慮一個非線性系統(tǒng):

xk=f(xk-1)+ωk

(5)

zk=h(xk)+vk

(6)

改進(jìn)的抗差SVD-CKF算法步驟如下。

1.4.1 狀態(tài)參數(shù)初始化

(7)

(8)

1.4.2 時間更新過程

使用SVD分解代替?zhèn)鹘y(tǒng)Cholesky分解,對協(xié)方差矩陣Pk-1|k-1進(jìn)行分解,并計(jì)算容積點(diǎn)Xi,k-1|k-1。

(9)

(10)

通過狀態(tài)方程來傳播容積點(diǎn)：

(11)

預(yù)測狀態(tài)值：

(12)

式中：m為容積點(diǎn)集矩陣的列數(shù),通常為狀態(tài)維數(shù)n的2倍；ωi為容積點(diǎn)相應(yīng)的權(quán)值,平均分配權(quán)值為ωi=1/2n。

預(yù)測協(xié)方差矩陣：

(13)

1.4.3 量測更新過程

對時間更新過程中求得的協(xié)方差矩陣進(jìn)行SVD分解,并計(jì)算容積點(diǎn)：

(14)

(15)

通過量測方程來傳播容積點(diǎn)：

Zi,k|k-1=h(Xi,k|k-1)

(16)

加權(quán)求和預(yù)測量測值：

(17)

使用均值濾波后的觀測值計(jì)算殘差值：

(18)

標(biāo)準(zhǔn)化觀測殘差值：

svk=yi,k/σi,k

(19)

式(19)中：yi,k為殘差向量的第i個分量；σi,k為其標(biāo)準(zhǔn)差。

預(yù)測觀測協(xié)方差矩陣,這里要根據(jù)觀測殘差值引入抗差因子rk,對觀測噪聲方差陣進(jìn)行修正，即

(20)

預(yù)測互協(xié)方差陣：

(21)

1.4.4 濾波更新

計(jì)算卡爾曼濾波增益：

Kk=Pxz,k/Pzz,k

(22)

利用先驗(yàn)估計(jì)值和均值濾波后的觀測值進(jìn)行狀態(tài)估計(jì)更新：

(23)

進(jìn)行協(xié)方差矩陣更新：

(24)

算法的具體流程如圖2所示。

圖2 改進(jìn)的抗差SVD-CKF算法流程Fig.2 Flow chart of improved robust SVD-CKF algorithm

2 運(yùn)動模型建立

2.1 狀態(tài)方程

本文的算法應(yīng)用于基于GPS導(dǎo)航的智能割草機(jī)平臺,首先需要建立模型來描述其運(yùn)動狀態(tài)。由于割草機(jī)主要在平面草地上工作,其滾轉(zhuǎn)角和俯仰角可近似為0°,只考慮其在水平平面內(nèi)的運(yùn)動狀態(tài),其主要運(yùn)動狀態(tài)為勻速直線運(yùn)動,因此以東向位置xe,東向速度ve,北向位置xn,北向速度vn作為狀態(tài)變量來建立狀態(tài)方程,k+1時刻的狀態(tài)可由k時刻的狀態(tài)得到,可以得到如下的狀態(tài)方程:

(25)

式(25)中：δ為過程噪聲；Δt為采樣時間間隔。狀態(tài)方程反應(yīng)了不同時刻下狀態(tài)量之間的相互關(guān)系,其可簡單地表示為

X(k+1)=FX(k)+Q(k)

(26)

式(26)中：X為狀態(tài)量；F為狀態(tài)轉(zhuǎn)移矩陣；Q為系統(tǒng)噪聲矩陣。

2.2 量測方程

量測方程是針對GPS導(dǎo)航系統(tǒng)量測過程的模型假設(shè),GPS所獲得的量測值有緯度L,經(jīng)度λ,對地航速v,對地航向α。GPS所測得的原始位置坐標(biāo)處于地理坐標(biāo)系,需將其轉(zhuǎn)換到東北天坐標(biāo)系,在考慮地球偏心率e不是很大的情況下,可以近似用以下公式進(jìn)行轉(zhuǎn)換，即

(27)

式(27)中：a為地球的長半徑；ΔL為當(dāng)前位置點(diǎn)與起始點(diǎn)的緯度差；Δλ為當(dāng)前位置點(diǎn)與起始點(diǎn)的經(jīng)度差。同時,也需要將GPS所測得的速度轉(zhuǎn)換到東北天坐標(biāo)系,整合后的量測方程為

(28)

式(28)中：ε為量測噪聲；v為測得的載體對地航速；α為載體坐標(biāo)y軸與正北方向的夾角,范圍為0°～360°。量測方程可簡單地表示為

Z(k)=HX(k)+R(k)

(29)

式(29)中：Z為觀測量；H為量測矩陣；R為量測噪聲矩陣。

3 仿真結(jié)果與分析

研究的GPS系統(tǒng)運(yùn)用于割草機(jī)平臺,該平臺主要進(jìn)行U字形自主作業(yè),需要保證直線段勻速運(yùn)動的位置準(zhǔn)確性。為了驗(yàn)證算法的可行性,采用MATLAB進(jìn)行仿真分析。假設(shè)GPS平臺的采樣頻率為5 Hz,選取一段采樣時長為100 s的勻速直線運(yùn)動段,其對地航速v=0.15 m/s,對地航向角α=75.7°。以此運(yùn)動狀態(tài)為真實(shí)值,以東向位置、北向位置、東向速度和北向速度為狀態(tài)量進(jìn)行分析,驗(yàn)證算法的有效性。

3.1 抗差SVD-CKF濾波效果分析

由于在狀態(tài)轉(zhuǎn)移和量測的過程中存在噪聲的不確定性,在位置真實(shí)值上引入一組均值為0,方差為0.01的高斯白噪聲,在速度真實(shí)值上引入一組均值為0,方差為4×10-4的高斯白噪聲,以此來作為GPS系統(tǒng)采集處理后的觀測值。圖3和圖4所示分別為經(jīng)過傳統(tǒng)CKF濾波后的位置和速度誤差,圖5和圖6所示分別為經(jīng)過抗差SVD-CKF濾波后的位置和速度誤差?？梢钥闯?，經(jīng)過傳統(tǒng)CKF濾波后的誤差相較于原始誤差值波動幅度有所減小,但是波動仍然較為劇烈；經(jīng)抗差SVD-CKF濾波后的誤差值明顯平緩,說明其狀態(tài)估計(jì)更具穩(wěn)定性。

圖3 傳統(tǒng)CKF濾波后位置誤差Fig.3 Position error after traditional CKF filtering

圖4 傳統(tǒng)CKF濾波后速度誤差Fig.4 Velocity error after traditional CKF filtering

圖5 抗差SVD-CKF濾波后位置誤差Fig.5 Position error after robust SVD-CKF filtering

圖6 抗差SVD-CKF濾波后速度誤差Fig.6 Velocity error after robust SVD-CKF filtering

兩種算法的誤差統(tǒng)計(jì)對比結(jié)果如表1所示,抗差SVD-CKF算法的標(biāo)準(zhǔn)差均優(yōu)于傳統(tǒng)CKF算法,經(jīng)抗差SVD-CKF算法濾波后的數(shù)據(jù)具有更平穩(wěn)的性能。

表1 傳統(tǒng)CKF和抗差SVD-CKF的標(biāo)準(zhǔn)差比較Table 1 Standard deviation comparison between traditional CKF and robust SVD-CKF

3.2 改進(jìn)的抗差SVD-CKF抗差效果分析

為了更好地驗(yàn)證改進(jìn)的抗差SVD-CKF算法在減小異常觀測值干擾的效果,考慮到GPS系統(tǒng)在失鎖時沒有信號,數(shù)據(jù)傳輸異常的情況以及割草機(jī)平臺打滑導(dǎo)致速度突變的狀況,以東向位置和東向速度為例,人為地在數(shù)據(jù)中加入異常觀測值,加入異常后如圖7所示。

圖7 東向加入異常后觀測值Fig.7 Observation after adding anomalies to the east

(1)位置異常:在東向位置觀測值10、25、70 s處加入2 m的觀測粗差。

(2)速度異常:在東向速度觀測值20 s處加入0.5 m/s的觀測粗差,在55 s處加入-0.2 m/s的觀測粗差。

在仿真實(shí)驗(yàn)中,均值濾波選取的窗口長度N=5,抗差方法中的閾值選取k0=2,k1=4。加入異常觀測值后,其經(jīng)過傳統(tǒng)CKF算法處理和改進(jìn)的抗差SVD-CKF算法的處理后,異常點(diǎn)處的誤差值如表2所示,東向位置誤差和東向速度誤差對比如圖8所示。

圖8 傳統(tǒng)CKF和改進(jìn)抗差SVD-CKF處理結(jié)果對比Fig.8 Comparison of traditional CKF and improved robust SVD-CKF processing results

表2 異常點(diǎn)處誤差比較Table 2 Comparison of errors at abnormal points

可以看出,傳統(tǒng)CKF算法對于異常觀測值有一定的抑制效果,但是在出現(xiàn)異常時,產(chǎn)生的誤差波動仍然較大；改進(jìn)的抗差SVD-CKF算法在初始狀態(tài)波動較大,運(yùn)行一段時間后,即使遇到異常觀測值,也能迅速回到一個較為平穩(wěn)的狀態(tài),其對異常觀測值的校正能力相對傳統(tǒng)CKF算法要更強(qiáng)。

放大速度誤差0～40 s時間段的濾波處理結(jié)果如圖9所示。

圖9 0～40 s東向速度誤差對比結(jié)果Fig.9 0～40 s east speed error comparison result

由于有均值濾波預(yù)處理,當(dāng)遇到異常觀測值時,濾波器可以及時作出響應(yīng),同時減弱大幅異常觀測值的干擾,減小了數(shù)據(jù)的波動性,使其能較快恢復(fù)到平穩(wěn)的狀態(tài),保證了后續(xù)迭代運(yùn)算的準(zhǔn)確性。而沒有經(jīng)過均值處理和抗差算法處理的CKF算法,雖然一定程度上對異常觀測值有修正效果,但是需要較長的時間恢復(fù)到穩(wěn)定狀態(tài)。

4 結(jié)論

提出一種改進(jìn)的抗差SVD-CKF算法,經(jīng)過仿真分析得到以下結(jié)論：

(1)均值濾波預(yù)處理對異常觀測值有較好的抑制作用,能及時降低異常觀測值所帶來的影響。

(2)SVD分解有效避免了先驗(yàn)協(xié)方差矩陣負(fù)定導(dǎo)致濾波發(fā)散的情況,增強(qiáng)了濾波穩(wěn)定性,保證了濾波器的持續(xù)運(yùn)行。

(3)改進(jìn)的SVD-CKF算法相較于傳統(tǒng)CKF算法有效降低了觀測數(shù)據(jù)的波動性,狀態(tài)更新值保持在一個相對穩(wěn)定的狀態(tài),能夠提高GPS導(dǎo)航的精度。