插值粒子濾波在靜基座捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用研究

高 峰

插值粒子濾波在靜基座捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用研究

高 峰

（空軍裝備部駐西安地區(qū)第七軍事代表室，西安 710068）

推導(dǎo)了大方位失準(zhǔn)角條件下，捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）靜基座初始對(duì)準(zhǔn)非線性誤差模型，提出一種基于插值粒子濾波（DDPF）的SINS靜基座初始對(duì)準(zhǔn)方法。分析了插值非線性濾波原理，結(jié)合插值濾波和粒子濾波的特點(diǎn)，利用二階插值濾波算法得到粒子濾波的重要性密度函數(shù)。在靜基座狀態(tài)下分別基于DDF和DDPF濾波算法進(jìn)行初始對(duì)準(zhǔn)仿真實(shí)驗(yàn)。仿真結(jié)果表明，大方位失準(zhǔn)角的情況下，DDPF非線性濾波具有更快的收斂速度和更高的對(duì)準(zhǔn)精度。

非線性濾波；插值粒子濾波；捷聯(lián)慣導(dǎo)系統(tǒng)；大方位失準(zhǔn)角；初始對(duì)準(zhǔn)

0 引言

初始對(duì)準(zhǔn)是捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strap-down Inertial Navigation System，SINS）的一項(xiàng)關(guān)鍵技術(shù)，對(duì)準(zhǔn)精度與對(duì)準(zhǔn)時(shí)間是衡量慣導(dǎo)系統(tǒng)性能指標(biāo)的重要因素，尤其是對(duì)準(zhǔn)精度直接影響系統(tǒng)整體的導(dǎo)航性能[1,2]。小失準(zhǔn)角條件下，SINS初始對(duì)準(zhǔn)的誤差模型近似為線性，通過(guò)標(biāo)準(zhǔn)卡爾曼濾波即可以解決初始對(duì)準(zhǔn)問(wèn)題；當(dāng)失準(zhǔn)角較大時(shí)，誤差模型近似線性化不成立，只能按照非線性模型進(jìn)行處理[3,4]。

擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）是應(yīng)用最為廣泛的一種非線性濾波方法[5]，由于略去了二階以上系統(tǒng)誤差項(xiàng)，當(dāng)系統(tǒng)模型非線性較強(qiáng)時(shí)，會(huì)給變換后的高斯隨機(jī)變量的真實(shí)驗(yàn)后均值和方差帶來(lái)較大誤差，進(jìn)而導(dǎo)致濾波精度下降，甚至發(fā)散。為了克服EKF算法的不足，2000年Magnus Nogaard等人提出一種插值濾波算法（Divided Difference Filter，DDF），與EKF相比DDF采用Stirling插值公式對(duì)非線性模型進(jìn)行線性化處理，避免了求解微分運(yùn)算過(guò)程，可應(yīng)用于任意函數(shù)，甚至非線性函數(shù)不連續(xù)且存在奇異點(diǎn)時(shí)也能進(jìn)行狀態(tài)估計(jì)，應(yīng)用范圍廣，算法簡(jiǎn)單，容易實(shí)現(xiàn)。此外，DDF在濾波過(guò)程中采用了協(xié)方差矩陣的平方根形式，保證了協(xié)方差矩陣在傳播過(guò)程的半正定性，具有更好的數(shù)值特性，在非線性濾波領(lǐng)域均得到了廣泛的關(guān)注。但是與EKF算法相同，DDF也是同樣基于高斯變量噪聲的假設(shè)條件[6,7]。粒子濾波（Particle Filter，PF）是一種在完全非線性條件下的貝葉斯估計(jì)方法[8]，易于在計(jì)算機(jī)上實(shí)現(xiàn)，并且對(duì)系統(tǒng)的動(dòng)態(tài)模型、狀態(tài)變量的概率分布以及模型階數(shù)均有良好的魯棒性，不受高斯分布假設(shè)條件的限制，受濾波初始誤差的影響小，收斂速度快[9,10]。

本文將DDF與PF算法相結(jié)合，通過(guò)DDF對(duì)粒子集進(jìn)行量測(cè)更新生成PF的建議分布，使得從重要性密度函數(shù)中取樣得到的樣本更加逼近真實(shí)的后驗(yàn)概率密度，提高粒子濾波的估計(jì)精度。并且依據(jù)SINS初始對(duì)準(zhǔn)非線性誤差模型，研究了差分粒子濾波器（Divided Difference Particle Filter，DDPF）在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用。

1 大方位失準(zhǔn)角下捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型的建立

SINS相應(yīng)的速度和失準(zhǔn)角誤差模型如式（2）和式（3）所示：

狀態(tài)向量取：

噪聲向量?。?/p>

取慣導(dǎo)兩水平速度誤差量為觀測(cè)量，建立非線性濾波模型如式（5）所示：

2 插值粒子濾波

2.1 Stirling插值近似

DDF是一種基于Stirling插值近似的非線性濾波方法。根據(jù)插值近似的階數(shù)不同，分為一階插值濾波（1st-DDF）和二階插值濾波（2nd-DDF），一階近似與EKF算法精度相當(dāng)，本文采用2nd-DDF算法。

對(duì)應(yīng)的一階、二階導(dǎo)如式（8）和式（9）所示：

分別表示局部偏差算子和局部平均算子，且分別如式（13）和式（14）所示：

因此式（10）可表示為：

2.2 Stirling插值濾波

考慮如式（16）所示離散狀態(tài)方程描述的非線性系統(tǒng)：

濾波具體步驟如下：

2）采用Cholesky分解法引入四個(gè)平方根分解算子如式（19）所示：

根據(jù)如下形式定義的四個(gè)差分矩陣如式（20）和式（21）所示：

進(jìn)一步得到相應(yīng)的狀態(tài)誤差協(xié)方差矩陣Cholesky分解因子的時(shí)間更新形式如式（22）所示：

3）狀態(tài)與協(xié)方差時(shí)間更新

進(jìn)一步得到式（25）～式（27）：

4）計(jì)算預(yù)測(cè)觀測(cè)向量及其預(yù)測(cè)協(xié)方差矩陣如式（28）所示：

5）量測(cè)更新

計(jì)算增益矩陣如式（30）所示：

狀態(tài)向量更新如式（31）所示：

協(xié)方差矩陣更新如式（32）所示：

2.3 插值粒子濾波

插值粒子濾波是在粒子濾波框架里利用插值濾波作為重要性密度函數(shù)的選取手段，利用當(dāng)前時(shí)刻最新量測(cè)數(shù)據(jù)，產(chǎn)生新的后驗(yàn)概率密度分布，同時(shí)通過(guò)新的后驗(yàn)概率密度分布重新產(chǎn)生新的粒子，計(jì)算粒子的權(quán)值并歸一化，最后進(jìn)行重采樣完成狀態(tài)估計(jì)。通過(guò)結(jié)合DDF算法選取重要性密度函數(shù)，不僅克服了標(biāo)準(zhǔn)PF難以獲得最優(yōu)重要密度函數(shù)分布的缺點(diǎn)，同時(shí)整合了最近一次的系統(tǒng)量測(cè)數(shù)據(jù)，能夠更好地逼近后驗(yàn)概率密度分布。DDPF算法流程如下：

初始權(quán)值：

返回第2）步，按新的量測(cè)量遞推計(jì)算下一時(shí)刻的狀態(tài)估計(jì)值。

3 仿真驗(yàn)證

3.1 仿真條件

從重要采樣函數(shù)中采樣的粒子數(shù)N=100，分別用DDF與DDPF非線性濾波算法進(jìn)行對(duì)準(zhǔn)仿真，結(jié)果如圖1～圖3所示，圖中實(shí)線為DDF對(duì)準(zhǔn)結(jié)果，虛線為DDPF對(duì)準(zhǔn)結(jié)果。

圖2 北向失準(zhǔn)角估計(jì)誤差

圖3 方位失準(zhǔn)角估計(jì)誤差

從圖1和圖2可以看出，由于水平失準(zhǔn)角為小角度，采用DDF與DDPF均能快速收斂到穩(wěn)態(tài)值，DDF估計(jì)誤差略微高于DDPF，總體來(lái)說(shuō)水平對(duì)準(zhǔn)精度大體相當(dāng)，區(qū)別不明顯。從圖3可以看出，在方位失準(zhǔn)角對(duì)準(zhǔn)過(guò)程中，DDPF無(wú)論在收斂速度還是在估計(jì)精度上均優(yōu)于DDF。在理論對(duì)準(zhǔn)精度3.24′條件下，DDF算法在300 s時(shí)的穩(wěn)態(tài)誤差為-3.45′，而采用DDPF算法在300 s時(shí)的穩(wěn)態(tài)誤差減小到-3.36′。這主要是由于DDPF濾波算法進(jìn)一步降低了模型線性化誤差對(duì)系統(tǒng)的影響，增強(qiáng)了魯棒性。由此比較可以得出，在大方位失準(zhǔn)角對(duì)準(zhǔn)過(guò)程中，DDPF具有比DDF更快的收斂速度以及更高的估計(jì)精度。

4 結(jié)語(yǔ)

對(duì)DDPF在捷聯(lián)慣性導(dǎo)航系統(tǒng)大方位失準(zhǔn)角初始對(duì)準(zhǔn)進(jìn)行了研究，分析了DDPF算法的數(shù)學(xué)機(jī)理。實(shí)驗(yàn)仿真結(jié)果表明，DDPF能夠很好地適用于大方位失準(zhǔn)角初始對(duì)準(zhǔn)中，比DDF有更高的濾波精度和更快的收斂速度。結(jié)果證實(shí)了其在大失準(zhǔn)角初始對(duì)準(zhǔn)中的可行性和優(yōu)越性，為DDPF在靜基座慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角的初始對(duì)準(zhǔn)提供了理論依據(jù)和計(jì)算方法。

[1] 萬(wàn)德鈞，房建成. 慣性導(dǎo)航初始對(duì)準(zhǔn)[M]. 南京：東南大學(xué)出版社，1998：9-22.

[2] 秦永元. 慣性導(dǎo)航[M]. 北京：科學(xué)出版社，2006.

[3] 楊靜，張洪鉞，李驥. 預(yù)測(cè)濾波理論在慣導(dǎo)非線性對(duì)準(zhǔn)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，2003，11（06）：44-52.

[4] 魏春玲，張洪鉞，郝曙光. 捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角下的非線性對(duì)準(zhǔn)[J]. 航天控制，2003，21（04）：25-35.

[5] 嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德明. 基于歐拉平臺(tái)誤差角的SINS非線性誤差模型的研究[J]. 西北工業(yè)大學(xué)學(xué)報(bào)，2009，27（04）：511-516.

[6] 王小旭，潘泉，黃鶴，等. 非線性系統(tǒng)確定采樣型濾波算法綜述[J]. 控制與決策，2012，27（06）：801-812.

[7] M. N?rgaard，N.K. Poulsen，O. Ravn. New Developments in State Estimation for Nonlinear Systems[J]. Automatica，2000，36（11）：1627-1638.

[8] M. N?rgaard，N.K. Poulsen，O. Ravn. Advances in Derivative-Free State Estimation for Nonlinear Systems[D]. Lyngby：Technical University Denmark，2000.

[9] 程向紅，李伯龍，王宇. 基于PF的SINS動(dòng)基座初始對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17（03）：267-271.

[10] 程水英，鄒繼偉，湯鵬. 免微分非線性Bayesian濾波方法評(píng)述[J]. 宇航學(xué)報(bào)，2009，30（03）：843-857.

Application Research of Interpolated Particle Filter in Initial Alignment of Strap-down Inertial Navigation System in Static

GAO Feng

The nonlinear error model of initial alignment of strap-down inertial navigation system (SINS) under the condition of large azimuth misalignment angle in static is discussed. An initial alignment method based on interpolated particle filter is proposed. The principle of the method is analyzed. Combining the characteristics of interpolated filtering and particle filtering, the importance density function of particle filtering is obtained by using the second-order interpolation filtering algorithm. The simulation results based on DDF and DDPF algorithms show that the DDPF method proposes higher accuracy and faster convergence rate.

Nonlinear Filter; Interpolated Particle Filter; Strap-down Inertial Navigation System; Large Azimuth Misalignment Angle; Initial Alignment

V249.3

A

1674-7976-(2022)-06-396-07

2022-09-08。

高峰（1980.08—），陜西乾縣人，碩士研究生，工程師，主要研究方向?yàn)閷?dǎo)航制導(dǎo)與控制。