潘曉宏，趙龍，2

1.北京航空航天大學(xué)數(shù)字導(dǎo)航中心，北京 100191

2.北京航空航天大學(xué)飛行器控制一體化技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100191

基于Unscented卡爾曼濾波算法在海底地形輔助導(dǎo)航中的應(yīng)用

潘曉宏1，趙龍1，2

1.北京航空航天大學(xué)數(shù)字導(dǎo)航中心，北京 100191

2.北京航空航天大學(xué)飛行器控制一體化技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100191

為了解決SITAN海底地形輔助導(dǎo)航算法將非線性過(guò)程線性化，容易產(chǎn)生不穩(wěn)定甚至濾波發(fā)散的問(wèn)題，提出了一種基于Unscented卡爾曼濾波的海底地形輔助導(dǎo)航算法，該算法無(wú)需對(duì)非線性系統(tǒng)的狀態(tài)方程和觀測(cè)方程進(jìn)行線性化，直接采用UT變換（unscented transform，UT）近似非線性函數(shù)的概率密度分布，并求取變換后的點(diǎn)集的均值和方差。利用海底地形SEAMAPnewHill100為真實(shí)海底地形數(shù)據(jù)對(duì)算法進(jìn)行了測(cè)試。實(shí)驗(yàn)結(jié)果表明，該算法較傳統(tǒng)算法有較強(qiáng)的精確性和魯棒性。

地形輔助導(dǎo)航；unscented卡爾曼濾波；UT變換；魯棒性

水下精確導(dǎo)航是潛艇長(zhǎng)期安全航行和武器系統(tǒng)實(shí)施精確打擊的基本保障［1］。慣性導(dǎo)航系統(tǒng)由于能自主、連續(xù)、實(shí)時(shí)、準(zhǔn)確地向潛艇提供導(dǎo)航信息，已成為潛艇綜合導(dǎo)航系統(tǒng)的必要設(shè)備。然而，由于慣性導(dǎo)航系統(tǒng)的導(dǎo)航誤差隨時(shí)間而累積，必須定期用外部信息對(duì)其進(jìn)行校準(zhǔn)。目前多采用天文導(dǎo)航信息、無(wú)線電導(dǎo)航信息和衛(wèi)星導(dǎo)航信息來(lái)校準(zhǔn)慣導(dǎo)。為接收外部校準(zhǔn)信息，潛艇需浮出或接近水面，從而犧牲了隱蔽性，使?jié)撏У陌踩艿酵{。另外，無(wú)線電導(dǎo)航和衛(wèi)星導(dǎo)航在戰(zhàn)時(shí)極易受電磁干擾而失去作用。海底地形匹配是為解決慣導(dǎo)系統(tǒng)水下校準(zhǔn)問(wèn)題而提出的一種新技術(shù)。該技術(shù)利用數(shù)字海底地形圖和水下地形特征探測(cè)儀作為輔助手段來(lái)修正慣導(dǎo)系統(tǒng)誤差，從而構(gòu)成一種新型導(dǎo)航系統(tǒng)——海底地形匹配輔助導(dǎo)航系統(tǒng)。該系統(tǒng)是一種完全自主式導(dǎo)航系統(tǒng)，潛艇無(wú)需浮出或接近水面，即可在水下完成對(duì)慣導(dǎo)系統(tǒng)校正，從而滿足潛艇對(duì)導(dǎo)航系統(tǒng)的要求。

1 海底地形輔助導(dǎo)航原理

海底地形匹配技術(shù)與飛機(jī)中使用的地形匹配技術(shù)相類似，是將潛艇上傳感器測(cè)到的地形特征與艇載數(shù)字海圖中給出潛艇位置附近的地形特征相匹配，從而獲得潛艇精確位置。由于水深數(shù)據(jù)能直接反映海底地形變化，因此潛艇上的傳感器采用能夠測(cè)量水深數(shù)據(jù)的測(cè)深測(cè)潛儀，地形圖則采用數(shù)字水深圖。海底地形輔助導(dǎo)航系統(tǒng)由慣導(dǎo)系統(tǒng)、測(cè)深測(cè)潛儀、數(shù)字海圖及數(shù)據(jù)處理計(jì)算機(jī)等幾部分組成，如圖1所示。

圖1 海底地形輔助導(dǎo)航系統(tǒng)原理

海底地形輔助導(dǎo)航系統(tǒng)將慣導(dǎo)系統(tǒng)提供的導(dǎo)航信息、測(cè)深測(cè)潛儀實(shí)測(cè)的水深數(shù)據(jù)信息送給數(shù)據(jù)處理計(jì)算機(jī)［1-5］，數(shù)據(jù)處理計(jì)算機(jī)根據(jù)慣導(dǎo)系統(tǒng)的位置信息從數(shù)字海圖中提取相關(guān)的水深數(shù)據(jù)，然后采用一定的匹配算法將實(shí)測(cè)的水深數(shù)據(jù)與從海圖上讀出的水深數(shù)據(jù)進(jìn)行匹配，得到最佳匹配點(diǎn)。利用該匹配點(diǎn)位置信息對(duì)慣導(dǎo)系統(tǒng)進(jìn)行校正，便可提高慣導(dǎo)定位精度。

2 基于擴(kuò)展卡爾曼濾波的SITAN算法的缺點(diǎn)

SITAN使用擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）將非線性過(guò)程線性化，故對(duì)于觀測(cè)量變化較小的情況，可以通過(guò)局部線性化來(lái)較好地完成定位估計(jì)。

圖2 平滑處理及線性化

如圖2所示，當(dāng)觀測(cè)量起伏波動(dòng)較劇烈時(shí)，EKF算法在進(jìn)行局部線性化過(guò)程中，線性化直線不能很好擬合曲線，導(dǎo)致引入了較大誤差，這樣就會(huì)降低定位精度［6］。在實(shí)際應(yīng)用中有時(shí)需要對(duì)一片比較大的區(qū)域進(jìn)行線性化，這就需要對(duì)這塊區(qū)域進(jìn)行平滑處理，而平滑處理最大的缺點(diǎn)就是會(huì)丟失很多有用的信息，這樣更有可能會(huì)導(dǎo)致EKF不穩(wěn)定。

3 基于非線性濾波的地形匹配算法數(shù)學(xué)模型

由于潛艇正下方的地形深度只與潛艇所在的水平面上的地理位置信息有關(guān)，因此在建立數(shù)學(xué)模型時(shí)，系統(tǒng)狀態(tài)取潛艇的位置信息。在東北天坐標(biāo)系下，系統(tǒng)狀態(tài)取為東向位置和北向位置，則其狀態(tài)方程為

4 UT變換

UT（unscented transform）是UKF（unscented Kal-man Filter）的基礎(chǔ)，UT的思想是用固定數(shù)量的參數(shù)去近似一個(gè)高斯分布，這比近似任意的非線性函數(shù)或變換更容易。其實(shí)現(xiàn)原理為：在原狀態(tài)分布中按某一規(guī)則取一些點(diǎn)，使這些點(diǎn)的均值和方差近似等于原狀態(tài)分布的數(shù)學(xué)期望和方差；將這些點(diǎn)帶入非線性函數(shù)中，相應(yīng)得到非線性函數(shù)值點(diǎn)集，并求取變換后點(diǎn)集的均值和方差。

假設(shè)一個(gè)非線性變換y＝F（x），狀態(tài)向量x為n維隨機(jī)變量，并已知其均值x－和方差矩陣Px，則可通過(guò)UT得到2n＋1個(gè)西格馬（SIGMA）點(diǎn)χi和相應(yīng)的權(quán)值Wi來(lái)計(jì)算y的統(tǒng)計(jì)特性，UT的原理圖如圖3所示。

圖3 UT原理圖

實(shí)際上UT就是將狀態(tài)變量x的取值范圍X近似為高斯分布（假設(shè)均值x－為高斯分布的數(shù)學(xué)期望，方差矩陣，然后通過(guò)在這個(gè)分布下的采樣產(chǎn)生有限個(gè)西格馬點(diǎn)χi（i＝1，2，…，2n＋1）來(lái)代替全體x∈X。將有限個(gè)西格馬點(diǎn)χi經(jīng)過(guò)非線性變換yi＝F（χi）得到變換點(diǎn)yi（i＝1，2，…，2n＋1），并假設(shè)全體變換點(diǎn)yi所構(gòu)成的值域y仍然近似服從于高斯分布，通過(guò)對(duì)變換點(diǎn)yi取加權(quán)均值和求加權(quán)方差，從而近似得到真實(shí)值域y＝F（X）分布的數(shù)學(xué)期望和方差。

5 UKF算法的實(shí)現(xiàn)過(guò)程

傳統(tǒng)的SITAN算法分為2個(gè)階段：搜索階段和跟蹤階段。經(jīng)實(shí)驗(yàn)驗(yàn)證，基于UKF的地形匹配算法可以不需要進(jìn)行搜索便能有很好的匹配定位性能［6］。UKF算法在地形匹配系統(tǒng)上實(shí)現(xiàn)的具體流程如下［7］：

1）預(yù)測(cè)：計(jì)算預(yù)測(cè)狀態(tài)均值Xk｜k－1和預(yù)測(cè)方差矩陣Pk｜k－1，X～表示SIGMA粒子矩陣：

2）更新：計(jì)算量測(cè)平均值Zk｜k－1，量測(cè)方差矩陣Sk以及狀態(tài)量和量測(cè)量的互相關(guān)方差矩陣Ck：

6 實(shí)驗(yàn)仿真與結(jié)果分析

6.1 單變量非靜態(tài)增長(zhǎng)模型

單變量非靜態(tài)增長(zhǎng)模型（univariate nonstationary growth model，UNGM）由于具有強(qiáng)非線性和雙模特性，使用傳統(tǒng)線性濾波器進(jìn)行匹配將會(huì)很困難。鑒于此，實(shí)驗(yàn)將通過(guò)該模型來(lái)比較單純地使用UKF和EKF進(jìn)行匹配時(shí)的效果，據(jù)此評(píng)價(jià)UKF和EKF的優(yōu)劣。

UNGM的狀態(tài)方程和量測(cè)方程為［9-10］

式中：xi為i時(shí)刻的真實(shí)狀態(tài)值；x＾i為估計(jì)狀態(tài)值。

2種濾波器對(duì)前100次采樣數(shù)據(jù)進(jìn)行仿真實(shí)驗(yàn)的濾波結(jié)果如圖4所示；前100次采樣數(shù)據(jù)匹配誤差與3δ范圍的關(guān)系如圖5所示。

從圖4中可以發(fā)現(xiàn)UKF的匹配性能要優(yōu)于EKF，例如在采樣點(diǎn)30～40之間UKF方法與源信號(hào)基本匹配，而EKF方法卻相差甚遠(yuǎn)。此外，圖5是圖4對(duì)應(yīng)的匹配誤差，從圖中可以看出UKF匹配誤差基本在3δ范圍之內(nèi)；而EKF匹配誤差有很多在3δ范圍之外。因此，UKF濾波結(jié)果比EKF濾波結(jié)果更可靠，它們對(duì)應(yīng)的均方誤差分別為64.729 1和93.842 9。

圖4 前100次采樣數(shù)據(jù)濾波結(jié)果

圖5 匹配誤差與3δ范圍

6.2 海底地形匹配仿真

海底地形SEAMAPnewHill100為真實(shí)海底地形數(shù)據(jù)，地圖大小為1 800 m×1 200 m，網(wǎng)格間距為100 m，航行器真實(shí)速度v＝20 m／s，真實(shí)航向角為55°，高度表噪聲為5 m，初始東向、北向定位誤差200 m，采樣周期10 s，潛艇初始網(wǎng)格位置（40，40），速度誤差為0.2 m／s。

為了便于比較，對(duì)同一航跡使用了2種匹配算法，這樣就可以清楚地看到基于UKF的地形匹配算法相比基于EKF算法的優(yōu)勢(shì)，如圖6所示。從圖中可以看出與傳統(tǒng)基于擴(kuò)展Kalman濾波的SITAN方法相比，基于UKF的地形匹配方法不需要線性化，能夠有效克服地形測(cè)量的非線性給系統(tǒng)帶來(lái)的不利影響，系統(tǒng)穩(wěn)定性較好。此外，可以發(fā)現(xiàn)即使沒(méi)有經(jīng)過(guò)搜索階段，基于UKF的地形匹配算法亦能夠很快的收斂，而且匹配性能絲毫不亞于SITAN算法，甚至比之更好。

圖6 基于UKF的地形匹配算法與SITAN算法比較

7 結(jié)束語(yǔ)

本文通過(guò)一種單變量非靜態(tài)增長(zhǎng)模型單純的將EKF算法和UKF算法的匹配結(jié)果進(jìn)行了比較，結(jié)果表明在強(qiáng)非線性情況下，UKF算法的匹配精度明顯優(yōu)于EKF算法。隨后通過(guò)選用了一塊海底地形圖作了仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果同樣表明基于UKF算法的地形輔助導(dǎo)航算法不論從收斂速度還是精度上都明顯優(yōu)于基于EKF的地形輔助導(dǎo)航算法。這就說(shuō)明在處理非線性比較強(qiáng)的問(wèn)題時(shí)，UKF算法更勝于EKF算法。而在實(shí)際應(yīng)用中很多情況下地形變化比較復(fù)雜，存在很強(qiáng)的非線性性。所以本文對(duì)現(xiàn)實(shí)科研工作具有很強(qiáng)的指導(dǎo)意義。由于篇幅原因，本文只是選擇了一條航線對(duì)2種算法進(jìn)行了比較，以后的研究者可以在不同地形圖、不同航線和一些誤差，例如速度誤差、初始位置誤差或噪聲不同的情況下對(duì)這2種算法做進(jìn)一步的比較。

［1］劉光軍，陳晶.海底地形輔助導(dǎo)航系統(tǒng)仿真技術(shù)研究［J］.計(jì)算機(jī)仿真，2000，17（2）：21-24.

［2］鄭彤，蔡龍飛，王志剛，等.地形匹配輔助導(dǎo)航中匹配區(qū)域的選擇［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17（2）：192-196.

［3］劉準(zhǔn)，佀文芳，陳哲.海底地形匹配技術(shù)研究［J］.系統(tǒng)仿真學(xué)報(bào)，2004，16（4）：700-703.

［4］王英鈞.地形輔助導(dǎo)航綜述［J］.航空電子技術(shù)，1998（1）：24-29.

［5］周亦軍，鄭彤.基于卡爾曼濾波的地形輔助導(dǎo)航［J］.艦船電子工程，2012，32（4）：54-56.

［6］METZGER J，WISOTZKY K，WENDEL J，et al.Sigma-point filter for terrain referenced navigation［C］//AIAA Guidance，Navigation，and Control Conference and Exhibit.San Francisco，USA，2005：15-18.

［7］陸守雷.海底地形匹配算法研究［D］.北京：北京航空航天大學(xué)，2011：30-31.

［8］HAYKIN S.Kalman filtering and neural networks［M］.New York：John Wiley＆Sons，2001：221-280.

［9］李鵬飛，黃建軍，喻建平，等.三種非線性濾波器對(duì)UN-GM模型跟蹤性能的比較［C］//第八屆全國(guó)信號(hào)與信息處理聯(lián)合學(xué)術(shù)會(huì)議.廈門，中國(guó)，2009：90-93.

［10］蔚國(guó)強(qiáng)，楊建業(yè)，張和新.常用非線性濾波方法比較研究［J］.電光與控制，2009，16（12）：48-69.

App lication of an unscented Kalman filter algorithm in the seabed terrain aided navigation

PAN Xiaohong1，ZHAO Long1，2
1.Digital Navigation Center，Beihang University，Beijing 100191，China 2.Science and Technology on Aircraft Control Laboratory，Beihang University，Beijing 100191，China

In order to solve the problem that the linearization of the nonlinear process of the SITAN seabed terrain aided navigation algorithm is prone to instability and even the filtering divergence，a seabed terrain aided navigation algorithm based on unscented Kalman filter is proposed.This algorithm does not need nonlinear state and observa-tion equations to be linearized，directly using the unscented transform（UT）to approximate probability density dis-tribution of nonlinear functions，and obtains themean and variance of the transformed point set.Ituses the seabed terrain SEAMAPnewHill100 as real terrain data to test this algorithm.The experimental results show that the algo-rithm ismore precise and robust than the traditional algorithms.

terrain aided navigation；unscented Kalman filter；unscented transform；robust

TP391.41

：A

：1009-671X（2015）01-049-05

10.3969／j.issn.1009-671X.201404012

http：//www.cnki.net／kcms／detail／23.1191.U.20150112.1530.013.htm l

2014-04-18.

日期：2015-01-12.

國(guó)家自然科學(xué)基金重點(diǎn)資助項(xiàng)目（61039003）；國(guó)家自然科學(xué)基金資助項(xiàng)目（41274038）；中國(guó)航空基礎(chǔ)科學(xué)基金資助項(xiàng)目（2013ZC51027）；中國(guó)航天創(chuàng)新基金資助項(xiàng)目（CASC201102）；中央高校基本科研業(yè)務(wù)費(fèi)專項(xiàng)資金資助項(xiàng)目.

潘曉宏（1984-），男，碩士研究生；趙龍（1976-），男，副教授，博士.

趙龍，E-mail：flylong＠buaa.edu.cn.