卡爾曼濾波技術(shù)在潛艇組合導(dǎo)航中的應(yīng)用研究

楊彥濤 李光磊

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

0 引言

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

采用多傳感器信息融合組合導(dǎo)航是目前通用的高精度導(dǎo)航模式。組合導(dǎo)航系統(tǒng)與單獨(dú)導(dǎo)航系統(tǒng)相比，具有較高的導(dǎo)航精度和較好的容錯(cuò)性能。實(shí)現(xiàn)組合導(dǎo)航有兩種基本方法：回路反饋法和最優(yōu)估計(jì)法。其中，以狀態(tài)空間分析法為基礎(chǔ)，以卡爾曼濾波為代表的最優(yōu)估計(jì)法應(yīng)用最為廣泛，也最為典型。由于潛艇組合導(dǎo)航的系統(tǒng)模型本質(zhì)上具有非線性特性，當(dāng)前多采用EKF進(jìn)行處理。EKF實(shí)質(zhì)上是對(duì)非線性模型進(jìn)行近似線性化，本文將EKF方法與傳統(tǒng)的DR算法進(jìn)行比較研究。

1 模型建立

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

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

式中，

圖1 可表示為線性形式

2 DR算法和EKF濾波

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

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

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

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

對(duì)于離散化后的非線性系統(tǒng)：

EKF有以下的遞推公式：

其中，

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

圖2 DR東向位置定位誤差

3 結(jié)論

由仿真結(jié)果可以看出，針對(duì)兩個(gè)方向的位置定位誤差，EKF的狀態(tài)估計(jì)精度都相對(duì)較高，濾波效果明顯。

圖3 DR北向位置定位誤差

圖4 EKF東向位置定位誤差

圖5 EKF北向位置定位誤差

[1]趙亦林（美）. 車輛定位與導(dǎo)航系統(tǒng)[M]. 北京: 電子工業(yè), 1999.

[2]周永余, 陳永冰, 李文魁. 艦船組合導(dǎo)航系統(tǒng)發(fā)展評(píng)述[J].中國慣性技術(shù)學(xué)報(bào), 2003, 11(1): 67-72.

[3]吳美平, 胡小平. 慣導(dǎo)/GNSS/羅蘭/航姿系統(tǒng)組合導(dǎo)航方案[J]. 中國慣性技術(shù)學(xué)報(bào), 2000, 8(4).

[4]陳剛, 莊良杰. INS/GPS/CNS組合導(dǎo)航系統(tǒng)的自適應(yīng)濾波技術(shù)研究[J]. 中國慣性技術(shù)學(xué)報(bào), 2000, 6(8):16-20.

[5]付夢(mèng)印, 鄧志紅, 張繼偉. 卡爾曼濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]. 北京: 科學(xué)出版社, 2003.

[6]董大瓊. 基于船舶運(yùn)動(dòng)模型的組合導(dǎo)航定位濾波算法研究[D]. 武漢: 武漢海軍工程大學(xué), 2009.