基于改進(jìn)漸消記憶濾波的動(dòng)力定位狀態(tài)估計(jì)*

蔣 帆 徐海祥 余文瞾 馮 輝 李文娟 陳亞豪

(武漢理工大學(xué)交通學(xué)院1) 武漢 430063) (武漢理工大學(xué)高性能船舶技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室2) 武漢 430063)(江蘇科技大學(xué)海洋裝備研究院3) 鎮(zhèn)江 212003)

0 引 言

動(dòng)力定位船舶需要從混有高頻信號(hào)及環(huán)境噪聲的傳感器位置信息中估計(jì)出低頻運(yùn)動(dòng)信息.準(zhǔn)確的狀態(tài)估計(jì)直接影響到動(dòng)力定位船舶的控制性能.貝葉斯最優(yōu)濾波理論是非線性系統(tǒng)狀態(tài)估計(jì)的基礎(chǔ)，而基于貝葉斯理論的卡爾曼濾波也被廣泛地應(yīng)用于動(dòng)力定位船舶的狀態(tài)估計(jì)中.然而，卡爾曼濾波需要系統(tǒng)具有精確的數(shù)學(xué)模型，且復(fù)雜的海洋環(huán)境使得噪聲統(tǒng)計(jì)數(shù)據(jù)不充分，這些限制條件會(huì)導(dǎo)致新測(cè)量值對(duì)預(yù)測(cè)值的修正作用下降，而舊測(cè)量值的作用相對(duì)提高，最終導(dǎo)致常規(guī)卡爾曼濾波的精度有所下降，甚至出現(xiàn)發(fā)散.

漸消記憶濾波在卡爾曼濾波的基礎(chǔ)上，通過(guò)引入漸消記憶因子，減小陳舊測(cè)量數(shù)據(jù)對(duì)狀態(tài)估計(jì)值的影響，增大新進(jìn)測(cè)量數(shù)據(jù)的影響，從而提高濾波精度.Hajiyev等[1]提出一種多重漸消因子的自適應(yīng)濾波器，通過(guò)計(jì)算得到矢量形式的漸消記憶因子，對(duì)誤差估計(jì)量采用較小的權(quán)重，進(jìn)而修正估計(jì)誤差，提高濾波精度，并通過(guò)仿真實(shí)驗(yàn)驗(yàn)證其算法的有效性，但是這種方法的漸消因子計(jì)算復(fù)雜，且不適用于動(dòng)力定位這種每個(gè)狀態(tài)估計(jì)量等權(quán)重的狀態(tài)估計(jì).Lim等[2]利用遺傳算法優(yōu)化漸消記憶因子，實(shí)現(xiàn)對(duì)電池開(kāi)路電壓的在線估計(jì)，并通過(guò)實(shí)驗(yàn)驗(yàn)證該方法的有效性.Chen等[3]將漸消記憶的思想引入H∞濾波中，驗(yàn)證了在模型存在較大誤差的情況下該算法的魯棒性.鮑水達(dá)等[4]針對(duì)平方根容積卡爾曼濾波在系統(tǒng)模型不準(zhǔn)確和狀態(tài)突變情況下魯棒性差的問(wèn)題，引入強(qiáng)跟蹤思想，提出了一種多漸消因子平方根容積卡爾曼濾波算法，通過(guò)多漸消因子實(shí)時(shí)調(diào)整增益矩陣，以提高濾波穩(wěn)定性.Wang等[5]為提高LED芯片的定位精度，設(shè)計(jì)雙速率自適應(yīng)漸消卡爾曼濾波器，引入自適應(yīng)的漸消記憶因子，提高系統(tǒng)狀態(tài)估計(jì)的魯棒性.盡管漸消記憶濾波在慣性導(dǎo)航、芯片精確定位等方面的研究與應(yīng)用已較為廣泛，但是在動(dòng)力定位方面的相關(guān)研究與應(yīng)用卻鮮見(jiàn).張閃等[6]首先將漸消記憶濾波引入動(dòng)力定位的狀態(tài)估計(jì)中，通過(guò)濾波發(fā)散評(píng)價(jià)標(biāo)準(zhǔn)推導(dǎo)出漸消記憶因子的計(jì)算表達(dá)式，并通過(guò)仿真實(shí)驗(yàn)驗(yàn)證其有效性，但是由于該方法并不能保證新息序列協(xié)方差的不相關(guān)性，所以通過(guò)該方法得到漸消記憶因子也并不是理論上的最優(yōu)解.

針對(duì)上述問(wèn)題，本文采用一種新息序列協(xié)方差估計(jì)器對(duì)當(dāng)前時(shí)刻的新息協(xié)方差進(jìn)行估計(jì)用以求取漸消記憶因子，避免進(jìn)入苛刻的條件判據(jù)，且這種推導(dǎo)過(guò)程是充分必要的，進(jìn)而修正一步預(yù)測(cè)狀態(tài)估計(jì)誤差協(xié)方差，最終提高濾波精度.為驗(yàn)證本文設(shè)計(jì)算法的有效性，對(duì)其進(jìn)行濾波仿真并與文獻(xiàn)[6]的算法進(jìn)行仿真結(jié)果對(duì)比.

1 數(shù)學(xué)模型

船舶非線性運(yùn)動(dòng)模型[7]為

(1)

式中：Ah為包含譜峰頻率ω0和相對(duì)阻尼系數(shù)ζ的常值矩陣；ξh為船舶高頻狀態(tài)向量；Eh為表示高頻噪聲的幅值矩陣；wh為零均值高斯白噪聲；ηh為船舶高頻運(yùn)動(dòng)向量；Ch為系數(shù)矩陣；η為低頻運(yùn)動(dòng)位移矩陣；R(ψ)為坐標(biāo)轉(zhuǎn)換矩陣；ν為速度矩陣；b為未知環(huán)境力矩陣；Tb為包含時(shí)間常數(shù)的矩陣；Eb為表示未知環(huán)境力噪聲的幅值矩陣；wb為零均值高斯白噪聲；M表示船舶慣性矩陣；D表示船舶阻尼系數(shù)矩陣；τ為推進(jìn)器產(chǎn)生的控制力和力矩；Ev為表示過(guò)程噪聲的幅值矩陣；wv為零均值高斯白噪聲；y為船舶測(cè)量位置；vy為零均值高斯白噪聲.

將非線性運(yùn)動(dòng)模型線性化，得到線性狀態(tài)空間模型為

(2)

2 常規(guī)漸消記憶濾波

將式(2)進(jìn)行離散化處理之后可得離散狀態(tài)模型和測(cè)量模型：

x(k)=Φx(k-1)+Δu(k-1)+Γw(k-1)

y(k)=Hx(k)+vy(k)

(3)

式中：Φ=exp(Ah)，其中h為采樣周期；Δ=A-1×(Φ-I)Β；Γ=A-1(Φ-I)E；w(k)和vy(k)分別為系統(tǒng)過(guò)程噪聲和測(cè)量噪聲.

系統(tǒng)過(guò)程噪聲和測(cè)量噪聲的統(tǒng)計(jì)特性滿足：

(4)

式中：Q(k)為過(guò)程噪聲協(xié)方差；R(k)為測(cè)量噪聲協(xié)方差；δkj滿足：

(5)

根據(jù)文獻(xiàn)[6]，漸消記憶濾波器方程可寫(xiě)為

(6)

ΓQ(k)ΓT

(7)

K(k)=P(k|k-1)HT·

[HP(k|k-1)+R(k)]-1

(8)

(9)

(10)

[I-K(k)H]T+K(k)R(k)K(k)T

(11)

選取不等式作為判斷濾波器發(fā)散的邊界條件：

rT(k)r(k)≤γ(k)trace(S0(k))

(12)

式中：γ(k)為冗余度儲(chǔ)備系數(shù)，γ(k)≥1；S0(k)為新息序列協(xié)方差，S0(k)=E[r(k)rT(k)].

取最嚴(yán)格的判斷濾波器是否發(fā)散的評(píng)價(jià)標(biāo)準(zhǔn)，即γ(k)=1，那么λ(k)的求解公式為

ΓTΗT-R(k))(ΗΦP(k|k-1)ΦTΗT)-1]

(13)

式中：m為測(cè)量值的維數(shù).

3 改進(jìn)漸消記憶濾波

文獻(xiàn)[6]通過(guò)最嚴(yán)格的判斷濾波器是否發(fā)散的評(píng)價(jià)標(biāo)準(zhǔn)，即γ(k)=1，以推導(dǎo)出漸消記憶因子的計(jì)算表達(dá)式.但是，在實(shí)際工程應(yīng)用中γ(k)的取值會(huì)受到環(huán)境隨機(jī)性以及人為因素的影響，很難得到合理有效的經(jīng)驗(yàn)值[7].為此，文獻(xiàn)[8]采用一種指數(shù)漸消因子濾波算法，當(dāng)動(dòng)力定位系統(tǒng)模型不精確、噪聲統(tǒng)計(jì)數(shù)據(jù)不充分時(shí)，通過(guò)實(shí)測(cè)新息平方和與理論新息協(xié)方差的跡的比值來(lái)確定儲(chǔ)備系數(shù)γ(k)，進(jìn)而確定漸消因子，該算法具有一定工程實(shí)際應(yīng)用價(jià)值，然而，通過(guò)收斂判據(jù)得到的漸消記憶因子并不是最優(yōu)的，也就是說(shuō)該算法對(duì)新息序列的利用程度不是最高的，所以此時(shí)濾波器的性能也不是最優(yōu)的.

針對(duì)上述問(wèn)題，本文采用一種新息協(xié)方差估計(jì)器對(duì)當(dāng)前時(shí)刻k的新息協(xié)方差進(jìn)行估計(jì)，通過(guò)新息協(xié)方差估計(jì)值，將新息序列的弱自相關(guān)性作為衡量濾波性能的標(biāo)準(zhǔn)，推導(dǎo)出漸消記憶因子.濾波器的收斂性證明見(jiàn)文獻(xiàn)[9].

新息序列協(xié)方差S0(k)為[10]

S0(k)=E[r(k)rT(k)]=λ(k)HΦP(k|k-1)·

ΦTΗT+ΗΓQ(k-1)ΓTΗT+R(k)

(14)

為滿足式(14)，此時(shí)式中的S0(k)應(yīng)為實(shí)測(cè)的新息序列協(xié)方差而非理論值，因?yàn)橐坏㏒0(k)取理論值，那么式(14)就成為一個(gè)恒等式，也就無(wú)法求解漸消記憶因子λ(k).由此得到λ(k)的表達(dá)式為

R(k))(ΗΦP(k|k-1)ΦTΗT)-1]

(15)

對(duì)于新息協(xié)方差的估計(jì)值，文獻(xiàn)[5]采用算數(shù)平均法得到k時(shí)刻新息協(xié)方差的估計(jì)值，表達(dá)式為

(16)

采用算數(shù)平均法的缺點(diǎn)是，只對(duì)k時(shí)刻之前的新息序列平方和進(jìn)行算數(shù)平均作為k時(shí)刻新息協(xié)方差的估計(jì)值，對(duì)新舊數(shù)據(jù)的利用程度都相同，隨著周期的增加，新息協(xié)方差的估計(jì)精度也會(huì)下降[11].為此，本文設(shè)計(jì)了一種基于指數(shù)加權(quán)的新息序列協(xié)方差估計(jì)器.

對(duì)于k時(shí)刻之前的數(shù)據(jù)，其加權(quán)系數(shù)為

(17)

式中：β(i)為第i時(shí)刻數(shù)據(jù)的加權(quán)系數(shù)；b為衰減因子，滿足0

式(17)可以轉(zhuǎn)化為

β(k)bk-1+β(k)bk-2+…+β(k)b+β(k)=1

(18)

在衰減因子b確定之后，β(k)就可以用含b的代數(shù)式表達(dá)，即

(19)

那么第i時(shí)刻數(shù)據(jù)的加權(quán)系數(shù)β(i)可表達(dá)為

β(i)=β(k)·bk-i

(20)

β(k)r(i)rT(i)+

(21)

由式(19)可得：

(22)

(23)

4 仿真結(jié)果與分析

為驗(yàn)證設(shè)計(jì)的濾波器在魯棒性和濾波精度方面的性能，以一艘1∶20縮尺比的動(dòng)力定位平臺(tái)供應(yīng)船模為對(duì)象對(duì)改進(jìn)后的漸消記憶濾波算法(improved fading memory filter，IFMF)進(jìn)行仿真，并與文獻(xiàn)[6]提出的漸消記憶濾波算法(fading memory filter，F(xiàn)MF)進(jìn)行濾波結(jié)果對(duì)比.船模相關(guān)參數(shù)見(jiàn)表1.

表1 船模主要參數(shù)

船舶初始位置設(shè)置為[0,0,0]，定位點(diǎn)位置設(shè)置為[10 m,10 m,20°].采樣周期為0.5 s，衰減因子b取為0.95.為模擬實(shí)際定位過(guò)程，在仿真中加入船位推算模型和控制算法模塊，與狀態(tài)估計(jì)模塊形成閉環(huán)系統(tǒng).

在狀態(tài)估計(jì)的過(guò)程中，系統(tǒng)過(guò)程噪聲的統(tǒng)計(jì)特性主要由高頻項(xiàng)、未知環(huán)境力項(xiàng)以及模型不確定項(xiàng)共同決定[12]，系統(tǒng)真實(shí)過(guò)程噪聲Q為Q=diag(0.1,0.1,0.000 1,0.000 1,0.000 1,0.000 1,0.000 1,0.000 1,0.000 1)，為檢驗(yàn)噪聲統(tǒng)計(jì)特性不匹配時(shí)兩種濾波算法的濾波性能和魯棒性，在仿真中將系統(tǒng)過(guò)程噪聲擴(kuò)大10倍，即Q′=10Q.

仿真結(jié)果見(jiàn)圖1～3.圖1為船舶在水平面內(nèi)運(yùn)動(dòng)軌跡.圖2為船舶定位過(guò)程中北向、東向和首向參考位置，以及兩種方法的估計(jì)位置對(duì)比.圖3為兩種方法船舶北向、東向及首向的估計(jì)誤差對(duì)比.

圖1 船舶在水平面內(nèi)的運(yùn)動(dòng)軌跡

圖2 三種參考位置及兩種方法的估計(jì)位置對(duì)比

圖3 兩種方法的估計(jì)誤差對(duì)比

由圖1可知，F(xiàn)MF和IFMF都能使船舶達(dá)到目標(biāo)位置，但是IFMF使得船舶運(yùn)動(dòng)軌跡更加平滑.由圖2可知，F(xiàn)MF和IFMF都能有效地估計(jì)出船舶的運(yùn)動(dòng)狀態(tài)，但是IFMF的濾波曲線較FMF的濾波曲線更為平滑，這也能一定程度上說(shuō)明針對(duì)系統(tǒng)噪聲統(tǒng)計(jì)特性不匹配的情況，IFMF的魯棒性要優(yōu)于FMF .從橫向?qū)Ρ鹊慕Y(jié)果來(lái)看，船舶艏向位置的濾波效果較北向和東向相對(duì)較差，這是由于艏搖運(yùn)動(dòng)的輸入為轉(zhuǎn)矩，而縱蕩和橫蕩運(yùn)動(dòng)的輸入為力，而力矩比力更難執(zhí)行.由圖3可知，IFMF在定點(diǎn)定位過(guò)程中的估計(jì)誤差要明顯小于FMF，說(shuō)明IFMF具有更好的收斂性與抗干擾能力，面對(duì)系統(tǒng)過(guò)程噪聲統(tǒng)計(jì)特性不匹配的情況，IFMF能更好地跟蹤船舶運(yùn)動(dòng)狀態(tài)，為控制系統(tǒng)提供更精確更平滑的輸入，提高動(dòng)力定位系統(tǒng)整體的穩(wěn)定性.

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

文中針對(duì)復(fù)雜海況下動(dòng)力定位船舶存在的系統(tǒng)過(guò)程噪聲統(tǒng)計(jì)數(shù)據(jù)不充分以及常規(guī)漸消記憶濾波無(wú)法得到最優(yōu)漸消記憶因子的問(wèn)題，設(shè)計(jì)了一種改進(jìn)的漸消記憶濾波算法.該算法通過(guò)設(shè)計(jì)新息協(xié)方差估計(jì)器能夠?yàn)闉V波器提供準(zhǔn)確的漸消記憶因子，從而達(dá)到自適應(yīng)調(diào)整增益矩陣和改善濾波器魯棒性的目的.仿真實(shí)驗(yàn)驗(yàn)證了所設(shè)計(jì)算法的有效性.