雷曙光，張　柳，林列書(shū)

(廣州民航職業(yè)技術(shù)學(xué)院，廣東廣州510403)

卡爾曼濾波在水下慣性導(dǎo)航系統(tǒng)中的研究及實(shí)現(xiàn)

雷曙光，張柳，林列書(shū)

(廣州民航職業(yè)技術(shù)學(xué)院，廣東廣州510403)

摘要:首先建立慣性導(dǎo)航系統(tǒng)中的速度誤差、位置誤差、陀螺儀誤差模型，然后與現(xiàn)實(shí)相聯(lián)系研究帶偏移量的卡爾曼濾波，同時(shí)對(duì)偏移量進(jìn)行估計(jì)，最后通過(guò)實(shí)驗(yàn)驗(yàn)證卡爾曼濾波對(duì)航向、陀螺儀誤差等的修正，極大地提高了水下慣性導(dǎo)航系統(tǒng)的結(jié)算能力和準(zhǔn)確性。

關(guān)鍵詞:卡爾曼濾波;慣性導(dǎo)航系統(tǒng);偏移量;估計(jì)量

The research and implementation of kalman filtering in INS integrated navigation

LEI Shu-guang，ZHANG Liu，LIN Lie-shu
(Guangzhou Civil Aviation College，Guangzhou 510403，China)

Abstract:First，this article established speed error，position error，gyro error model in inertial navigation system．Then linked with reality and researched on Kalman filter with offsets．At the same time estimated offset．Finally，experiments show that Kalman filter amendmented to the heading，gyroscope error，etc．This algorithm greatly improved the billing capability and accuracy of underwater inertial navigation system．

Key words:Kalman filter; INS integrated navigation; bias federated filter;estimator

0　引言

隨著科技的發(fā)展，利用導(dǎo)航對(duì)潛艇等非固定目標(biāo)進(jìn)行定位、定向等應(yīng)用越來(lái)越廣泛，在復(fù)雜的海洋中對(duì)潛艇進(jìn)行高精度的導(dǎo)航定位可以有效的預(yù)防意外事故的發(fā)生。慣性導(dǎo)航系統(tǒng)全天候都可以工作，隱蔽性強(qiáng)、帶寬寬，能夠平穩(wěn)的提供才捕捉到數(shù)據(jù)信息。在狀態(tài)空間中卡爾曼濾波是典型的最優(yōu)估計(jì)法。

本文首先建立了慣性導(dǎo)航系統(tǒng)中的速度誤差、位置誤差、陀螺儀誤差模型，然后與現(xiàn)實(shí)相聯(lián)系研究了帶偏移量的卡爾曼濾波，同時(shí)對(duì)偏移量進(jìn)行估計(jì)，最后通過(guò)實(shí)驗(yàn)驗(yàn)證卡爾曼濾波在水下慣性導(dǎo)航系統(tǒng)中的作用。

1　建立慣性導(dǎo)航系統(tǒng)的數(shù)學(xué)誤差模型

在現(xiàn)實(shí)中船舶上的微機(jī)電系統(tǒng)在通電工作后，隨著時(shí)間的增長(zhǎng)，元件自身的溫度會(huì)升高，同時(shí)環(huán)境、元件材質(zhì)等影響使元器件的電壓處在一個(gè)動(dòng)態(tài)變化的狀態(tài)，從而影響了慣性導(dǎo)航系統(tǒng)的精度，所以本文著重研究這種帶有偏置的卡爾曼濾波，在利用卡爾曼濾波之前應(yīng)該獲取到慣性導(dǎo)航系統(tǒng)的數(shù)學(xué)誤差模型。

慣性導(dǎo)航系統(tǒng)的誤差有速度誤差、位置誤差、平臺(tái)誤差、陀螺儀和加速計(jì)等誤差［1－3］。

1)陀螺儀誤差方程

陀螺儀直接關(guān)系到慣性導(dǎo)航系統(tǒng)的姿態(tài)誤差。本文選用的陀螺偏移模型是AIC最小的AR(1)。

將AR(1)的數(shù)學(xué)表達(dá)式xk=φ1xk－1+αk轉(zhuǎn)換成連續(xù)的一階馬爾科夫x·(t) +βx(t) = z(t)，從而可以得到:

式中wg(t)為陀螺儀的隨機(jī)信號(hào)。

2)位置誤差模型

由

從而可以獲得空間直角坐標(biāo)的位置誤差為:

3)航速誤差

根據(jù)慣性導(dǎo)航方程

可知:

通常重力加速度被認(rèn)為是常數(shù)，故σg= 0。令

則:

式中: fp為實(shí)際的加速度;Δp為測(cè)量的偏差。

則:

從而可得:

代入式(9)得到速度的誤差方程:

2　帶偏移量的卡爾曼濾波

參考文獻(xiàn)根據(jù)［4］可知，帶偏置的科爾曼濾波的估計(jì)狀態(tài)方程為:

觀測(cè)方程為:

對(duì)式(16)和(17)進(jìn)行離散可得:

式中: xn為第n個(gè)更新點(diǎn)變化向量; yn為第n個(gè)更新點(diǎn)的觀測(cè)向量; b為偏置向量; An，Bn，Hn，Cn均為隨時(shí)間變化的矩陣系數(shù);ξn為系統(tǒng)數(shù)學(xué)期望為0時(shí)的噪聲;ηn為觀測(cè)噪聲。

在沒(méi)有偏置的理想情況下，得到的狀態(tài)方程為:

從而得到狀態(tài)的估計(jì)方程為:

式中: x為理想狀態(tài)下的估計(jì); p為x的誤差協(xié)方差矩陣; kx為卡爾曼濾波的增益矩陣。

當(dāng)偏置已知時(shí)，b就是固定值，則可以得到x的估計(jì):

由式(20)和式(21)可得2個(gè)狀態(tài)的估計(jì)值關(guān)系為:

從而得到狀態(tài)的加權(quán)陣:

同時(shí)可知:

因此，可得關(guān)于每一步估計(jì)的狀態(tài)矩陣:

式中:

由式(24)～式(27)可以得到加權(quán)矩陣的類(lèi)推方法。

沒(méi)有偏置情況下，濾波器的測(cè)量誤差為:

對(duì)上式作如下變換可得:

式中: vn為濾波估計(jì)的測(cè)量誤差，表述了與自估計(jì)偏移量之間的關(guān)系; vn不是一個(gè)固定的值，因此測(cè)量值與估計(jì)值沒(méi)有關(guān)系。

對(duì)偏移向量b進(jìn)行估計(jì):

式中: px和rn根據(jù)沒(méi)有偏移量的情況估算求得; Sn通過(guò)式(24)～式(27)可得。

從而得到狀態(tài)估計(jì)的修正值，利用式(30)修正沒(méi)有偏移的x，從而得到最小方差估計(jì)表示為:

求偏移向量b的估計(jì)值

故而得到最小方差估計(jì)為:

通過(guò)以上推理可知卡爾曼濾波是利用兩部分之間的相互關(guān)系估算狀態(tài)偏差。步驟如下估算出在偏移量作用下的，同時(shí)可以求得協(xié)方差矩陣P，P有系統(tǒng)狀態(tài)X和b有關(guān)，根據(jù)二者影響的不同，可以將b對(duì)之的影響分離出來(lái)，從而求出偏移向量b。

3　帶偏移的卡爾曼濾波在慣性導(dǎo)航系統(tǒng)的應(yīng)用實(shí)驗(yàn)結(jié)果

本文通過(guò)建立六狀態(tài)下的卡爾曼濾波方程估算航行方向、陀螺儀角速度等的誤差。要估算的六狀態(tài)誤差為Δθ，Δγ，Δψ，Δwx，Δwy，Δwz。

由加速度計(jì)信號(hào)確定的姿態(tài)角θ0= 2.45°， γ0= 1.55°。令ψ0= 1°，從而獲得初始姿態(tài)矩陣，實(shí)驗(yàn)結(jié)果如圖1所示(橫坐標(biāo)表示時(shí)間，單位: s)。

圖1　六個(gè)狀態(tài)量的卡爾曼濾波誤差曲線Fig.1　Kalman filter error curve of six state quantity

從圖1可看出，航向角誤差和姿態(tài)角誤差在很短的時(shí)間內(nèi)就接近于0，由此可知在經(jīng)過(guò)卡爾曼濾波后，大大提升了系統(tǒng)的結(jié)算能力，并且準(zhǔn)確度得以提高。角度的偏移量最大不超過(guò)±0.05°，一定時(shí)間后系統(tǒng)趨于平穩(wěn)，從陀螺的角速度誤差曲線可以看到處于穩(wěn)定的狀態(tài)。由此說(shuō)明了帶偏移量的卡爾曼濾波通過(guò)偏移量修正改善了實(shí)際應(yīng)用中因?yàn)槠屏克鶐?lái)的不準(zhǔn)確性，提高了系統(tǒng)的穩(wěn)定性。

4　結(jié)語(yǔ)

本文首先建立了慣性導(dǎo)航系統(tǒng)中的速度誤差、位置誤差、陀螺儀誤差模型，然后與現(xiàn)實(shí)相聯(lián)系研究了帶偏移量的卡爾曼濾波，同時(shí)對(duì)偏移量進(jìn)行估計(jì)，最后通過(guò)實(shí)驗(yàn)驗(yàn)證了通過(guò)卡爾曼濾波對(duì)航向、陀螺儀誤差等的修正，極大提高了水下慣性導(dǎo)航系統(tǒng)的結(jié)算能力和準(zhǔn)確性。

［1］LIU Hai-tao．Study and realization of the low-cost INS/GPS integrated navigation system algorithms［J］．Journal of Telemetry，Tracking and Command，2003，24(2) :36－41．

［2］WEN Wu．Federated filter of INS/GPS integrated navigation ［J］．Journal of North China Institute of Technology，2004，25 (4)．

［3］YAN Xian-gong．Research of kalman filter and its application in ship GPS/SINS integrated navigation［J］．Ship Science and Technology，2014，36(12) :128－131．

［4］BERNARD F．Treatment of bias in recursive filtering［C］/ / IEEE Transactions on Automatic Control，volAC－14，NO．4，AUGUST 1969:359－367．

作者簡(jiǎn)介:雷曙光(1973－)，男，副教授，研究方向?yàn)轱w機(jī)電子設(shè)備維修研究。

收稿日期:2014－10－26;修回日期: 2014－12－27

文章編號(hào):1672－7649(2015) 07－0200－03doi:10.3404/j.issn.1672－7649.2015.07.047

中圖分類(lèi)號(hào):U665．26

文獻(xiàn)標(biāo)識(shí)碼:A