基于自適應的單形采樣UKF組合導航算法

黃 平, 孫婷婷, 仝彥龍

(哈爾濱工程大學自動化學院, 黑龍江 哈爾濱 150001)

0 引 言

無跡卡爾曼濾波器(unscented Kalman filter,UKF)是近些年發展起來的一種非線性濾波[1-3]。它以無跡變換(unscented transform,UT)[4]為關鍵技術,以卡爾曼濾波為框架,通過確定性采樣策略直接逼近狀態的后驗分布。UKF不僅提高了濾波精度,而且不必計算Jacobian矩陣[5-6],對系統模型沒有要求,適用于任何線性和非線性模型。但UKF通常的問題是,采樣點到中心點的距離會隨著系統維數的增加而增大,產生采樣的非局部效應問題[7-8];另外,傳統的UKF采用對稱采樣策略[9],計算量大,無法應用到對實時性要求較高的系統中。針對UKF存在上述問題,很多學者進行了相應的改進算法研究[10-14]。但是上述文獻中,作者只是在其中的一個方面改進UKF算法,并沒有同時解決兩個問題。

本文基于最小偏度單形采樣[15]的比例UT變換,分析比例因子α的影響,提出了一種自適應的選取方法,并將改進后的算法應用在SINS/GPS緊組合導航系統中,通過Matlab仿真實驗將其與傳統的UKF進行對比,分析仿真結果。

1 自適應比例UKF算法

傳統UKF算法采用對稱采樣策略,計算量較大,對于實時性要求較高的系統,如何降低計算量則成為首要的問題。對于一個n維分布的狀態空間,對稱采樣需要2n+1個采樣點,而最小偏度單形采樣只需要n+2個采樣點,雖然精度略有降低但計算量較小,綜合考慮精度和計算量等因素,本文選用最小偏度單形采樣。

1.1 最小偏度單形采樣策略

y=f(x)

(1)

首先選擇

(2)

Sigma采樣點一階二階權系數分別為

(3)

初始向量(對應于狀態維數為j=1的情況)為

(4)

當輸入維數為j=2,3,…,n時,其向量遞推公式如下

(5)

根據式(5)生成的Sigma采樣點為

(6)

1.2 最小偏度單形采樣的比例修正

UT變換雖然簡單方便,但當系統維數較大時,會產生采樣的非局部效應,導致估計精度下降,常用的解決辦法是比例采樣修正。修正算法為

(7)

(9)

(10)

(11)

式中,比例因子α取值范圍一般為10-4≤α≤1,改變α的值,可以減小UT變換的非局部效應造成的影響。參數β用來描述x的先驗分布信息,對于高斯分布的系統,β=2時最優。將此方法引入最小偏度單形采樣算法,便可得到最小偏度單形采樣的比例修正算法:

(12)

(14)

1.3 自適應比例因子的選取方法

在使用比例修正算法進行UT變換時,通常是給α取一個α∈[10-4,1]的固定值,雖然這有時也能得到滿意的值,但這并不能使系統性能最優。如果能在遞歸運算時,每一步自適應地選取一個最合適的α參數,才能充分發揮α的性能。為此提出了一種自適應選取α的方法,如圖1所示。

圖1 UKF算法流程圖Fig.1 UKF algorithm flowchart

(15)

(16)

綜合以上推導和分析,自適應選取比例因子α的具體步驟如下所示:

在每次UT變換的過程中,用自適應比例因子α對采樣策略進行修正,就得到自適應比例無跡卡爾曼濾波(adaptive scaled unscented Kalman filter, ASUKF) 算法。

2 SINS/GPS緊組合系統的模型

2.1 SINS/GPS緊組合導航系統的狀態方程

2.1.1 SINS誤差方程

(1) 平臺誤差角方程

(17)

式中,φt=[φE,φN,φU]T;陀螺漂移εt=[εE,εN,εU]T=Tεb,T為姿態矩陣。

(2) 速度誤差方程

(18)

(3) 位置誤差方程

(19)

式中,λ,L,H為當地經度、緯度和高度;RN和RM別是運載體在地球上某一點的卯酉圈的曲率半徑和子午圈的曲率半徑。

(4) 慣性儀表誤差方程

通常取陀螺漂移為

εb=εc+εr+ωg

(20)

式中,ωg為陀螺儀的隨機漂移,假定其為高斯白噪聲;εr表示陀螺儀的一階馬氏過程,假定其為當前時刻的陀螺漂移;εc表示陀螺儀的常值漂移。

加速度計的測量誤差包含零偏和隨機噪聲,可表示為

(21)

式中,Ta表示相關時間;ωa表示加速度計的隨機漂移。

系統誤差狀態方程的一般表達式為

(22)

式中

XI=[φEφNφUδυEδυNδυUδλδLδh

2.1.2 GPS誤差狀態方程

GPS的誤差狀態,在緊組合系統中,通常取時鐘誤差等效的距離誤差δtu與時鐘頻率誤差等效的速度誤差δtru。

(23)

式中

XG(t)=[δtu,δtru]T,WG(t)=[ωtu,-ωtru]T

將SINS誤差狀態方程與GPS誤差方程合并,則得到緊組合系統的狀態方程。

(24)

其中

X(t)=[XI(t),XG(t)]T,W(t)=[WI(t),WG(t)]T

2.2 SINS/GPS緊組合導航系統的量測方程

在GPS/SINS緊組合導航系統中,選擇SINS和GPS兩者偽距之差和偽距率之差作為組合導航系統的觀測量。

2.2.1 偽距誤差量測方程

Zρ(t)=Hρ(t)X(t)+Vρ(t)

(25)

2.2.2 偽距率誤差量測方程

(26)

將偽距量測方程式與偽距率量測方程式合并,則組合成組合導航系統的量測方程,組合系統的量測方程可表達為

(27)

也即

Z=HX+V

(28)

由于濾波器采用離散系統濾波方程進行濾波計算,還需要將上述方程離散化處理。

2.3 ASUKF濾波

(1) 濾波初值選取

設置初始比例因子α并計算Sigma樣本點和相應的權值,再根據第1.3節介紹的方法更新比例因子α=αk+1|k。

(2) 時間更新

將Sigma采樣點通過狀態函數求取系統的一步狀態模型值，即

(29)

預測k+1時刻的模型值，即

(30)

(31)

(32)

估計系統的量測值:

(33)

(3) 量測更新:

計算自協方差矩陣Pzz(k/k-1)和互協方差矩陣Pxz(k/k-1):

(34)

(35)

計算增益陣:

(36)

校正狀態的預測值:

(37)

更新系統的協方差:

(38)

3 仿真分析

3.1 仿真參數設置

仿真所使用的軌跡是利用Matlab產生的機載運動,系統導航信息的輸出頻率為100 Hz,GPS的輸出頻率為10 Hz,組合導航系統的數據輸出頻率與SINS系統同步。設陀螺儀常值漂移0.1°/h,白噪聲均方差0.01°/h;加速度計常值零偏1×10-4g,白噪聲均方差5×10-5g;GPS偽距白噪聲10 m,偽距率白噪聲0.5 m/s。飛機產生的航跡如圖2所示,機體在東、北、天3個方向上的位置、速度和姿態如圖3所示。

圖3 載體各方向運動Fig.3 Each direction of motion of the carrier

3.2 仿真結果

分別采用對稱采樣UKF、單形采樣UKF(記為SUKF)和采用單形采樣策略的ASUKF算法對SINS/GPS導航系統的緊組合模型進行Matlab仿真,并對實驗結果進行對比分析,如圖4和圖5所示。

圖4 定位誤差Fig.4 Positioning error

圖5 速度誤差Fig.5 Velocity error

重復進行50次MC仿真實驗,并計算3種UKF算法各類誤差信息的RMSE,統計這3種UKF算法的平均運行時間,結果如表1所示。

表1 緊組合3種UKF算法仿真效果對比

3.3 仿真結果分析

由圖4和圖5的誤差曲線圖及表1的仿真效果統計信息可得:曲線最終均收斂在零值附近,且這3種UKF算法估計輸出的速度誤差均值都小于0.1 m/s,定位誤差都不大于3 m,說明這3種算法都能取得不錯的濾波效果。對比這3種濾波算法的誤差曲線可以得出,固定比例數的SUKF濾波精度比采用對稱采樣的UKF濾波精度稍差,但用時比對稱采樣UKF少了大概有30%。ASUKF算法雖然在UT變換的過程中,計算量較固定比例參數的單形采樣UT變換策略只是略微增大,大概增大了5%,但其精度卻有很大的提高,尤其是其速度精度提高了將近20%,位置精度也有10%多的提升。而ASUKF濾波與常規的對稱采樣UKF濾波相比,在估計精度大體相同的情況下,使計算量卻減小了大約25%。綜合來看,ASUKF算法精度較高,計算量也較小,與理論分析基本一致,綜合效果最好。

4 結 論

文中針對UKF做出了一些改進,采用最小偏度單形采樣策略和比例參數自適應改進UKF算法。既降低了系統的計算量,提高了精度,也減弱了UT變換的非局部效應,但仍有一些問題尚未解決:

(1) 本文中比例參數α的取值范圍是根據經驗確定的,并未對其進行推導驗證,也不一定就是最優值,還需要對比例參數α的自適應選取問題做更深入的研究。

(2) 在做Matlab仿真時可以發現:與對稱UKF相比,系統維數越高,自適應比例參數UKF算法計算量優勢越明顯,但精度有些下降,如何選擇合適的維數還需再做深入的研究。

參考文獻：

[1] STRAKA O, DUN J, JIND K, et al. Unscented Kalman filter with advanced adaptation of scaling parameter[J]. Automatica, 2014, 50(10): 2657-2664.

[2] ANASTASIA D, ANDREOPOULOS Y. Throughput distortion computation of generic matrix multiplication: toward a computation channel for digital signal processing systems[J]. IEEE Trans.on Signal Processing, 2012, 60(4): 2024-2037.

[3] JINWHAN K, VADDI S S, MENON P K, et al. Comparison between nonlinear filtering techniques for spiraling ballistic missile state estimation[J]. IEEE Trans.on Aerospace and Electronic Systems, 2012, 48(1): 313-328.

[4] 劉慶元, 郝立良, 黃書捷,等. 改進的UKF在 GPS/INS 組合導航中的應用[J]. 測繪科學技術學報, 2014, 31(5): 450-453.

LIU Q Y, HAO L L, HUANG S J, et al. The application of improved UKF in GPS/INS integrated navigation[J]. Journal of Surveying and Mapping Science and Technology, 2014, 31(5): 450-453.

[5] 管軍, 易文俊, 常思江, 等. 基于UKF的北斗/SINS 組合導航系統研究[J]. 彈道學報, 2015, 27(1): 1-6.

GUAN J, YI W J, CHANG S J, et al. Research on the navigation system of Beidou/SINS base on UKF[J]. Journal of ballistic, 2015, 27(1): 1-6.

[6] ZHOU W D, CAI J A, SUN L, et al. Time-space difference based GPS/SINS ultra-tight integrated navigation method[J]. Measuremnet, 2014,58: 87-92.

[7] BISHT S S, SINGH M P. An adaptive unscented Kalman filter for tracking sudden stiffness changes[J]. Mechanical Systems and Signal Processing, 2014, 49(1/2): 181-195.

[8] ZHU Z L, SHAN Y D, YI Y, et al. INS/GNS integrated method based on innovation orthogonality adaptive Kalman filter[J]. Journal of Chinese Inertial Technology, 2015, 23(1): 66-70.

[9] HU G G, GAO S S, ZHONG Y M. A derivative UKF for tightly coupled INS/GPS integrated navigation[J]. ISA Transactions, 2015, 56: 135-144.

[10] CAFFERY L J, SMITH A C. Investigating the quality of video consultations performed using fourth generation (4G) mobile telecommunications[J]. Journal of Telemedicine & Telecare, 2015, 21(6):348.

[11] MOHAMMAD T S, POURIA S, MOSTAFA Z. Extended and unscented Kalman filters for parameter estimation of an autonomous underwater vehicle[J].Ocean Engineering,2014,91(15):329-339.

[12] HUANG J, YAN B. Federated filter based on dynamic information allocation with unscented Kalman filter[C]∥Proc.of the International Conference on Electrical, Computer Engineering and Electronics, 2015: 911-916.

[13] 馮鵬程,胡高歌,高社生.基于新息正交原理的抗差UKF及其在INS/GPS組合導航中的應用[J].測控技術,2015,34(10):92-95.

FENG P C, HU G G, GAO S S. Robust UKF based on the principle of new interest orthogonality and its application in INS/GPS integrated navigation[J]. Measurement and Control Technology, 2015, 34(10): 92-95.

[14] GUPTA M, BEHERA L, SUBRAMANIAN V K, et al. A robust visual human detection approach with UKF-based motion tracking for a mobile robot[J]. IEEE Systems Journal, 2015, 9(4): 1363-1375.

[15] 李曉旭, 戴彬, 曹潔. 基于多特征融合的改進UPF目標跟蹤算法[J]. 上海交通大學學報, 2014, 48(10): 1474-1478.

LI X X, DAI B, CAO J. Improved UPF target tracking algorithm based on multi feature fusion[J]. Journal of Shanghai Jiaotong University, 2014, 48(10): 1474-1478.