基于強(qiáng)跟蹤UPF的重力梯度輔助定位方法

姚劍奇

(中國(guó)電子科技集團(tuán)公司第38研究所，合肥230088)

0　引言

重力輔助導(dǎo)航依靠重力測(cè)量的無(wú)源性可在校正INS誤差的同時(shí)，保證水下潛器航行的隱蔽性，具有較高的軍用價(jià)值[1-2]。

地球重力信息包含絕對(duì)重力與重力梯度，重力梯度因具有相關(guān)距離短[3]、變化更為劇烈、量測(cè)不受厄特弗斯效應(yīng)影響、獨(dú)立分量多等優(yōu)點(diǎn)，可獲得更優(yōu)的匹配定位效果。但因重力梯度變化劇烈，在局部區(qū)域，呈現(xiàn)較強(qiáng)的非線性，所以基于遞推濾波的重力梯度匹配定位屬于非線性最優(yōu)估計(jì)問(wèn)題。

因目前重力輔助定位精度與衛(wèi)星定位尚有差距，且水下潛器相對(duì)飛行器的機(jī)動(dòng)能力較弱，無(wú)法在短時(shí)間內(nèi)完成使系統(tǒng)誤差完全可觀測(cè)的機(jī)動(dòng)模式[4]。根據(jù)文獻(xiàn)[5]的研究，基于濾波的重力定位系統(tǒng)對(duì)慣性器件常值誤差的估計(jì)能力較弱，且濾波系統(tǒng)采用的INS誤差方程存在線性化誤差。以上因素使濾波匹配系統(tǒng)存在模型偏差，影響長(zhǎng)時(shí)間匹配時(shí)的定位精度。

本文在給出強(qiáng)跟蹤濾波(Strong Trace Filter，STF)一般形式的基礎(chǔ)上，將其引入無(wú)跡粒子濾波(Unscented Particle Filter，UPF)，形成強(qiáng)跟蹤無(wú)跡粒子濾波(Strong Trace Unscented Particle Filter，STUPF)輔助定位算法，從而有效應(yīng)對(duì)重力梯度的強(qiáng)非線性分布及系統(tǒng)模型偏差對(duì)定位的影響。經(jīng)仿真驗(yàn)證，STUPF算法的定位精度較高，定位效果穩(wěn)定且運(yùn)算量較小。

1　UPF算法原理

設(shè)非線性系統(tǒng)滿足如下?tīng)顟B(tài)空間模型

xk+1=fk(xk)+Γkwk,zk=hk(xk)+vk

(1)

其中，xk∈Rn、zk∈Rm為狀態(tài)量及量測(cè)量；wk、vk為過(guò)程及量測(cè)噪聲；fk(xk)、hk(xk)為狀態(tài)方程和觀測(cè)方程；Γk為噪聲驅(qū)動(dòng)矩陣。由式(1)的Markov性質(zhì)得xk的后驗(yàn)概率密度為：

p(x0∶k/z1∶k)=p(zk/xk)p(x0∶k/z1∶k-1)/p(zk/z1∶k-1)

p(x0∶k/z1∶k-1)=p(xk/xk-1)p(x0∶k-1/z1∶k-1)

(2)

PF存在粒子退化及樣本貧化問(wèn)題。UPF[7]是重視重要性密度函數(shù)選取的改進(jìn)PF方法，其在wk～N(0,Qk)、vk～N(0,Rk)且二者不相關(guān)的假設(shè)下，由無(wú)跡Kalman濾波(Unscented Kalman Filter，UKF)獲得重要性密度函數(shù)所需參數(shù)的改進(jìn)PF方法。因UKF獲取的重要性密度函數(shù)結(jié)合了最新的量測(cè)信息且更接近狀態(tài)真實(shí)概率分布，因此UPF可有效緩解粒子退化問(wèn)題，且所需的粒子數(shù)較少，計(jì)算量較小。當(dāng)系統(tǒng)存在模型偏差時(shí)，隨濾波進(jìn)行，量測(cè)信息對(duì)狀態(tài)估計(jì)的作用減弱，濾波精度降低甚至發(fā)散。

2　STUPF算法

2.1　算法原理

STF是針對(duì)系統(tǒng)模型偏差及狀態(tài)突變的有效方法，其根據(jù)殘差信息調(diào)節(jié)預(yù)測(cè)協(xié)方差陣中的漸消因子，達(dá)到調(diào)節(jié)增益陣，增強(qiáng)濾波系統(tǒng)狀態(tài)跟蹤能力。根據(jù)正交性定理[8]，當(dāng)系統(tǒng)模型匹配，濾波增益最優(yōu)時(shí)，有式(3)近似成立，其中γk為k時(shí)刻殘差信息。

(3)

(4)

當(dāng)存在模型偏差時(shí)，式(3)不成立，可引入漸消因子使殘差序列強(qiáng)制正交，保持濾波器跟蹤狀態(tài)的能力。引入漸消因子后的預(yù)測(cè)方差陣為：

(5)

將式(5)代入式(3)，并采用文獻(xiàn)[8]的近似計(jì)算方法可得λk的表達(dá)式為：

(6)

(7)

(8)

設(shè)Qk為對(duì)稱正定陣，可得

(9)

由式(7)～式(9)可得Nk、Mk的等價(jià)描述：

(10)

對(duì)式(5)進(jìn)行變換后，可得式(11)成立

(11)

式(6)、式(10)及式(11)構(gòu)成了STF的一般形式。因此可將STF引入U(xiǎn)PF中獲取重要性密度函數(shù)的UKF過(guò)程，得到STUPF算法。

2.2　算法步驟

由以上對(duì)STUPF所涉及理論的闡述，可得STUPF算法流程如下，其中NS為粒子總數(shù)：

初始化：Fori=1…NS

(12)

濾波過(guò)程：k≥1

(2)更新權(quán)值：按式(13)迭代更新粒子權(quán)值。

(13)

(3)根據(jù)有效抽樣尺度[5]判斷是否重采樣。

(14)

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

重力梯度濾波匹配系統(tǒng)的狀態(tài)量取載體的經(jīng)/緯度位置誤差及東/西向速度誤差，狀態(tài)及量測(cè)方程如下所示：

Xk+1=Φk+1,kXk+Γkwk

Zk=hk(Xk)+vk

(15)

Φk+1,k=I+TFk,Γk=T(I+TFk/2)

F的具體形式見(jiàn)文獻(xiàn)[10]，因篇幅原因，此處不具體給出。Zk取重力梯度向量中的5維獨(dú)立分量(Txx,Txy,Txz,Tyy,Tyz)，hk(Xk)為Xk對(duì)應(yīng)位置處的梯度量測(cè)值。

以某INS的一次真實(shí)海事測(cè)試數(shù)據(jù)進(jìn)行匹配實(shí)驗(yàn)，INS數(shù)據(jù)頻率為1幀/s，數(shù)據(jù)記錄時(shí)間8h，慣性器件精度：陀螺常值漂移0.01(°)/h，加速度計(jì)零位漂移50μg。根據(jù)記錄數(shù)據(jù)生成的INS航跡及真實(shí)航跡如圖1所示。

匹配實(shí)驗(yàn)采用的梯度基準(zhǔn)圖由文獻(xiàn)[11]中的方法，由自由空間重力異常數(shù)據(jù)生成，其Txx、Txy分量如圖2所示(分辨率0.5′)。

匹配實(shí)驗(yàn)中，設(shè)量測(cè)誤差為標(biāo)準(zhǔn)差為1E的白噪聲，濾波周期1min。以標(biāo)準(zhǔn)PF、高斯粒子濾波(Gaussian Particle Filter，GPF)、STUPF進(jìn)行實(shí)驗(yàn)，三種算法粒子數(shù)分別為10000、3000、100。每種算法各進(jìn)行20次獨(dú)立實(shí)驗(yàn)以統(tǒng)計(jì)均方根誤差(Root Mean Square Error, RMSE)，RMSE定義如下

(16)

RMSE統(tǒng)計(jì)數(shù)據(jù)及各算法單航程平均運(yùn)行時(shí)間MRT如表1所示。

表1　RMSE統(tǒng)計(jì)數(shù)據(jù)及MRT

由表1及圖3可知，標(biāo)準(zhǔn)PF運(yùn)算量最大，匹配精度較差；GPF、STUPF的匹配穩(wěn)定性近似，但STUPF定位精度略高，且運(yùn)算量較小，可見(jiàn)STUPF具有一定優(yōu)勢(shì)。

4　結(jié)論

本文在給出STF一般形式的基礎(chǔ)上，將其引入U(xiǎn)PF，形成STUPF輔助定位算法，根據(jù)殘差信息實(shí)時(shí)調(diào)整系統(tǒng)的狀態(tài)跟蹤能力，從而有效應(yīng)對(duì)重力梯度輔助定位中，重力梯度的強(qiáng)非線性分布及系統(tǒng)模型偏差對(duì)定位的影響。基于真實(shí)INS數(shù)據(jù)的匹配仿真實(shí)驗(yàn)表明，STUPF可獲得較好的定位效果，且相對(duì)標(biāo)準(zhǔn)PF、GPF算法而言，該算法在定位的精度、穩(wěn)定性及運(yùn)算量上具有一定優(yōu)勢(shì)。

[1]Bishop G C. Gravitational field maps and navigational errors[J]. IEEE Journal of Oceanic Engineering, 2002, 27(3): 726-737.

[2]崔晨風(fēng), 王濤. 重力匹配輔助導(dǎo)航誤匹配消除方法研究[J]. 測(cè)繪通報(bào), 2013(3):14-17.

[3]Metzger E H, Jircitano A. Inertial navigation performance improvement using gravity gradient matching techniques[J]. Journal of Spacecraft and Rockets, 1976, 13(6): 323-324.

[4]吳美平, 胡小平. 捷聯(lián)慣導(dǎo)系統(tǒng)誤差狀態(tài)可觀性分析[J]. 宇航學(xué)報(bào), 2002, 23(2): 54-57.

[5]Prokhorov Y G. Observability of gravimeter-aided inertial navigation systems[J]. Journal of Guidance, Control, and Dynamics, 1995, 18(6): 1416-1419.

[6]Arulampalam M S, Maskell S, Gordon N, et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE Transactions on Signal Processing, 2002, 50(2): 174-188.

[7]Cheng Q, Bondon P. A new unscented particle filter[C]// IEEE International Conference on Acoustics, Speech and Signal Processing(ICASSP). IEEE, 2008: 3417-3420.

[8]周東華, 席裕庚, 張鐘俊. 一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器[J].自動(dòng)化學(xué)報(bào), 1991, 17(6): 689-695.

[9]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 1998.

[10]王文晶. 基于重力和環(huán)境特征的水下導(dǎo)航定位方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2009: 71-75.

[11]錢(qián)東, 劉繁明, 李艷, 等. 導(dǎo)航用重力梯度基準(zhǔn)圖構(gòu)建方法的比較研究[J]. 測(cè)繪學(xué)報(bào), 2011, 40(6): 736-744.