基于Unscented卡爾曼濾波算法在海底地形輔助導航中的應用

潘曉宏，趙龍，2

1.北京航空航天大學數字導航中心，北京 100191

2.北京航空航天大學飛行器控制一體化技術重點實驗室，北京 100191

基于Unscented卡爾曼濾波算法在海底地形輔助導航中的應用

潘曉宏1，趙龍1，2

1.北京航空航天大學數字導航中心，北京 100191

2.北京航空航天大學飛行器控制一體化技術重點實驗室，北京 100191

為了解決SITAN海底地形輔助導航算法將非線性過程線性化，容易產生不穩定甚至濾波發散的問題，提出了一種基于Unscented卡爾曼濾波的海底地形輔助導航算法，該算法無需對非線性系統的狀態方程和觀測方程進行線性化，直接采用UT變換（unscented transform，UT）近似非線性函數的概率密度分布，并求取變換后的點集的均值和方差。利用海底地形SEAMAPnewHill100為真實海底地形數據對算法進行了測試。實驗結果表明，該算法較傳統算法有較強的精確性和魯棒性。

地形輔助導航；unscented卡爾曼濾波；UT變換；魯棒性

水下精確導航是潛艇長期安全航行和武器系統實施精確打擊的基本保障［1］。慣性導航系統由于能自主、連續、實時、準確地向潛艇提供導航信息，已成為潛艇綜合導航系統的必要設備。然而，由于慣性導航系統的導航誤差隨時間而累積，必須定期用外部信息對其進行校準。目前多采用天文導航信息、無線電導航信息和衛星導航信息來校準慣導。為接收外部校準信息，潛艇需浮出或接近水面，從而犧牲了隱蔽性，使潛艇的安全受到威脅。另外，無線電導航和衛星導航在戰時極易受電磁干擾而失去作用。海底地形匹配是為解決慣導系統水下校準問題而提出的一種新技術。該技術利用數字海底地形圖和水下地形特征探測儀作為輔助手段來修正慣導系統誤差，從而構成一種新型導航系統——海底地形匹配輔助導航系統。該系統是一種完全自主式導航系統，潛艇無需浮出或接近水面，即可在水下完成對慣導系統校正，從而滿足潛艇對導航系統的要求。

1 海底地形輔助導航原理

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

圖1 海底地形輔助導航系統原理

海底地形輔助導航系統將慣導系統提供的導航信息、測深測潛儀實測的水深數據信息送給數據處理計算機［1-5］，數據處理計算機根據慣導系統的位置信息從數字海圖中提取相關的水深數據，然后采用一定的匹配算法將實測的水深數據與從海圖上讀出的水深數據進行匹配，得到最佳匹配點。利用該匹配點位置信息對慣導系統進行校正，便可提高慣導定位精度。

2 基于擴展卡爾曼濾波的SITAN算法的缺點

SITAN使用擴展卡爾曼濾波（extended Kalman filter，EKF）將非線性過程線性化，故對于觀測量變化較小的情況，可以通過局部線性化來較好地完成定位估計。

圖2 平滑處理及線性化

如圖2所示，當觀測量起伏波動較劇烈時，EKF算法在進行局部線性化過程中，線性化直線不能很好擬合曲線，導致引入了較大誤差，這樣就會降低定位精度［6］。在實際應用中有時需要對一片比較大的區域進行線性化，這就需要對這塊區域進行平滑處理，而平滑處理最大的缺點就是會丟失很多有用的信息，這樣更有可能會導致EKF不穩定。

3 基于非線性濾波的地形匹配算法數學模型

由于潛艇正下方的地形深度只與潛艇所在的水平面上的地理位置信息有關，因此在建立數學模型時，系統狀態取潛艇的位置信息。在東北天坐標系下，系統狀態取為東向位置和北向位置，則其狀態方程為

4 UT變換

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

假設一個非線性變換y＝F（x），狀態向量x為n維隨機變量，并已知其均值x－和方差矩陣Px，則可通過UT得到2n＋1個西格馬（SIGMA）點χi和相應的權值Wi來計算y的統計特性，UT的原理圖如圖3所示。

圖3 UT原理圖

實際上UT就是將狀態變量x的取值范圍X近似為高斯分布（假設均值x－為高斯分布的數學期望，方差矩陣，然后通過在這個分布下的采樣產生有限個西格馬點χi（i＝1，2，…，2n＋1）來代替全體x∈X。將有限個西格馬點χi經過非線性變換yi＝F（χi）得到變換點yi（i＝1，2，…，2n＋1），并假設全體變換點yi所構成的值域y仍然近似服從于高斯分布，通過對變換點yi取加權均值和求加權方差，從而近似得到真實值域y＝F（X）分布的數學期望和方差。

5 UKF算法的實現過程

傳統的SITAN算法分為2個階段：搜索階段和跟蹤階段。經實驗驗證，基于UKF的地形匹配算法可以不需要進行搜索便能有很好的匹配定位性能［6］。UKF算法在地形匹配系統上實現的具體流程如下［7］：

1）預測：計算預測狀態均值Xk｜k－1和預測方差矩陣Pk｜k－1，X～表示SIGMA粒子矩陣：

2）更新：計算量測平均值Zk｜k－1，量測方差矩陣Sk以及狀態量和量測量的互相關方差矩陣Ck：

6 實驗仿真與結果分析

6.1 單變量非靜態增長模型

單變量非靜態增長模型（univariate nonstationary growth model，UNGM）由于具有強非線性和雙模特性，使用傳統線性濾波器進行匹配將會很困難。鑒于此，實驗將通過該模型來比較單純地使用UKF和EKF進行匹配時的效果，據此評價UKF和EKF的優劣。

UNGM的狀態方程和量測方程為［9-10］

式中：xi為i時刻的真實狀態值；x＾i為估計狀態值。

2種濾波器對前100次采樣數據進行仿真實驗的濾波結果如圖4所示；前100次采樣數據匹配誤差與3δ范圍的關系如圖5所示。

從圖4中可以發現UKF的匹配性能要優于EKF，例如在采樣點30～40之間UKF方法與源信號基本匹配，而EKF方法卻相差甚遠。此外，圖5是圖4對應的匹配誤差，從圖中可以看出UKF匹配誤差基本在3δ范圍之內；而EKF匹配誤差有很多在3δ范圍之外。因此，UKF濾波結果比EKF濾波結果更可靠，它們對應的均方誤差分別為64.729 1和93.842 9。

圖4 前100次采樣數據濾波結果

圖5 匹配誤差與3δ范圍

6.2 海底地形匹配仿真

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

為了便于比較，對同一航跡使用了2種匹配算法，這樣就可以清楚地看到基于UKF的地形匹配算法相比基于EKF算法的優勢，如圖6所示。從圖中可以看出與傳統基于擴展Kalman濾波的SITAN方法相比，基于UKF的地形匹配方法不需要線性化，能夠有效克服地形測量的非線性給系統帶來的不利影響，系統穩定性較好。此外，可以發現即使沒有經過搜索階段，基于UKF的地形匹配算法亦能夠很快的收斂，而且匹配性能絲毫不亞于SITAN算法，甚至比之更好。

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

7 結束語

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

［1］劉光軍，陳晶.海底地形輔助導航系統仿真技術研究［J］.計算機仿真，2000，17（2）：21-24.

［2］鄭彤，蔡龍飛，王志剛，等.地形匹配輔助導航中匹配區域的選擇［J］.中國慣性技術學報，2009，17（2）：192-196.

［3］劉準，佀文芳，陳哲.海底地形匹配技術研究［J］.系統仿真學報，2004，16（4）：700-703.

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

［5］周亦軍，鄭彤.基于卡爾曼濾波的地形輔助導航［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］.北京：北京航空航天大學，2011：30-31.

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

［9］李鵬飛，黃建軍，喻建平，等.三種非線性濾波器對UN-GM模型跟蹤性能的比較［C］//第八屆全國信號與信息處理聯合學術會議.廈門，中國，2009：90-93.

［10］蔚國強，楊建業，張和新.常用非線性濾波方法比較研究［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.

國家自然科學基金重點資助項目（61039003）；國家自然科學基金資助項目（41274038）；中國航空基礎科學基金資助項目（2013ZC51027）；中國航天創新基金資助項目（CASC201102）；中央高校基本科研業務費專項資金資助項目.

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

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