卡爾曼濾波技術在潛艇組合導航中的應用研究

楊彥濤 李光磊

（1. 海軍駐武漢四六一廠軍事代表室，武漢 430085；2. 海軍駐九江地區軍事代表室，江西九江 332007）

0 引言

潛艇因其具有隱蔽性能好、機動能力強、突擊威力大等特點，自誕生之日起，就以其獨特性能而被譽為“水中蛟龍”，并在戰爭中發揮著日益突出的作用，大有主導未來海戰之勢。潛艇為了發揮其自身強大的優勢，必須具備高精度導航定位的能力。在復雜的海洋環境中執行任務，有時還可能遭遇敵對環境，高精度的導航定位不僅是潛艇獲取有效信息的必要條件，而且決定了潛艇是否能夠順利完成任務、安全返回。因此，高精度的導航定位是潛艇研究的關鍵技術之一。

采用多傳感器信息融合組合導航是目前通用的高精度導航模式。組合導航系統與單獨導航系統相比，具有較高的導航精度和較好的容錯性能。實現組合導航有兩種基本方法：回路反饋法和最優估計法。其中，以狀態空間分析法為基礎，以卡爾曼濾波為代表的最優估計法應用最為廣泛，也最為典型。由于潛艇組合導航的系統模型本質上具有非線性特性，當前多采用EKF進行處理。EKF實質上是對非線性模型進行近似線性化，本文將EKF方法與傳統的DR算法進行比較研究。

1 模型建立

針對不同的導航原理，有不同的系統模型。潛艇導航方法依據原理不同主要可以分為三大類：其一，聲學導航定位方法；其二，利用地球物理特性進行導航，比如利用重力異常、地磁場導航、測海數據導航、利用海底特征地形的并行匹配定位等方法；其三，以慣性導航為主的航跡推算和最優估計組合導航方法。聲學定位方法需要安裝聲波發送和接收設備，這些設備的安裝和標定比較困難，定位的作用范圍也有限，因而不適合遠距離、長航時、任意海域的潛艇導航。利用地球物理特性進行水下導航是最近幾年國外研究的熱點問題，理論分析和內場實驗結果都顯示了這類方法具有很大發展潛力，但是目前技術上還不夠成熟，離實際應用還有差距。當前廣泛采用第三類組合導航方法，即以慣性導航為主的航跡推算和最優估計組合導航。本文就針對這一類組合導航方法展開分析和研究。

水下組合導航建模需要考慮的因素很多，外部環境不確定性很強。如果把每個方面都納入到模型中，必然會導致模型極其復雜，以致不可用。在研究中，需要對實際問題進行適當簡化，使模型既能夠比較準確地反映真實系統的狀態，又便于分析研究我們所重點關注的問題。據此，我們只選取了有代表性的潛艇位置、速度、方位角、方位角速度作為系統的狀態，而不考慮洋流干擾、陀螺漂移、位置誤差、速度誤差、標定誤差等物理量。上述狀態取舍還考慮到了當前典型的潛艇導航的多傳感器組合方式，即GPS/ DGPS+INS：/磁通量羅盤+DVL。其中，GPS/DGPS（差分GPS）提供準確的初始位置，INS/磁通量羅盤測量方位角和方位角速度，多普勒速度計測量潛艇相對于海底的速度。僅考慮潛艇在固定水深航行，模型可簡化為在二維平面內的運動，如圖 1所示。設：潛艇的位置 x 、y，方位角ψ，方位角速度 r，沿潛艇縱軸方向的對地速度u，垂直于縱軸方向的對地速度 v，潛艇導航模型則可建立如下非線性狀態方程：

式中，

圖1 可表示為線性形式

2 DR算法和EKF濾波

航跡推算（DR）是常用的一種數據融合方法，它采用慣性傳感器和里程計（速度計）等裝置估計載體的位置和航向。航跡推算的基本原理是：通常物體的運動可以近似看作是在平面上的二維運動，如果已知物體t時刻的位置和航向，還可以測量從kt時刻到1kt+時刻這段時間間隔內的速度和航向角變化率，則可以計算1it+時刻的位置和航向。已知初始位置和航向，依次遞推，就可以計算出任意時刻的位置和航向。

考慮到實際情況，可將（3）式改寫為：

其中：θi是ti時刻的航向角（和北方向的夾角）；di是 ti時刻到 ti+1時刻物體行駛過的距離。

航跡推算的特點是原理比較簡單，易于實現，具有短時間內定位精度高的特點，但定位誤差隨時間積累而變化嚴重。因此，應用最優估計方法對系統進行濾波處理，能夠有效提高系統的定位精度和可靠性。目前常用的濾波方法是擴展卡爾曼濾波（EKF）。其主要思想將非線性方程近似線性化，實際應用中還要考慮離散化的問題。其基本公式如下：

對于離散化后的非線性系統：

EKF有以下的遞推公式：

其中，

通過上述分析，可進行計算機仿真研究，以進一步比較EKF方法和DR算法在潛艇組合導航中應用的性能。仿真結果如圖2、3、4、5所示。

圖2 DR東向位置定位誤差

3 結論

由仿真結果可以看出，針對兩個方向的位置定位誤差，EKF的狀態估計精度都相對較高，濾波效果明顯。

圖3 DR北向位置定位誤差

圖4 EKF東向位置定位誤差

圖5 EKF北向位置定位誤差

[1]趙亦林（美）. 車輛定位與導航系統[M]. 北京: 電子工業, 1999.

[2]周永余, 陳永冰, 李文魁. 艦船組合導航系統發展評述[J].中國慣性技術學報, 2003, 11(1): 67-72.

[3]吳美平, 胡小平. 慣導/GNSS/羅蘭/航姿系統組合導航方案[J]. 中國慣性技術學報, 2000, 8(4).

[4]陳剛, 莊良杰. INS/GPS/CNS組合導航系統的自適應濾波技術研究[J]. 中國慣性技術學報, 2000, 6(8):16-20.

[5]付夢印, 鄧志紅, 張繼偉. 卡爾曼濾波理論及其在導航系統中的應用[M]. 北京: 科學出版社, 2003.

[6]董大瓊. 基于船舶運動模型的組合導航定位濾波算法研究[D]. 武漢: 武漢海軍工程大學, 2009.