艦艇運(yùn)動(dòng)姿態(tài)實(shí)時(shí)預(yù)測(cè)方法研究

張 憶， 宋俊才， 李 坤

(中國(guó)船舶重工集團(tuán)有限公司第七一○研究所，湖北 宜昌 443003)

艦艇在較高的海況條件下作戰(zhàn)時(shí)，艦艇上的武器發(fā)射裝置會(huì)隨著海浪的波動(dòng)產(chǎn)生搖擺，這直接影響發(fā)射裝置發(fā)射角的精度，進(jìn)而影響武器系統(tǒng)整體作戰(zhàn)效能的發(fā)揮。發(fā)射裝置在發(fā)射過(guò)程中，發(fā)射角的坐標(biāo)與目標(biāo)位置的坐標(biāo)需要保持相對(duì)固定，所以需要對(duì)發(fā)射裝置進(jìn)行穩(wěn)定控制。穩(wěn)定控制系統(tǒng)主要是通過(guò)姿態(tài)方位參考系統(tǒng)采集艦艇的運(yùn)動(dòng)姿態(tài)，控制系統(tǒng)對(duì)運(yùn)動(dòng)姿態(tài)數(shù)據(jù)進(jìn)行處理和運(yùn)算，輸出控制信號(hào)對(duì)發(fā)射裝置進(jìn)行控制，從而保持發(fā)射裝置的相對(duì)穩(wěn)定。在穩(wěn)定控制系統(tǒng)中姿態(tài)方位參考系統(tǒng)采集姿態(tài)數(shù)據(jù)有一定的采樣周期，數(shù)據(jù)傳輸?shù)椒€(wěn)定控制系統(tǒng)中需要傳輸時(shí)間，所以需要實(shí)時(shí)對(duì)艦艇的運(yùn)動(dòng)姿態(tài)數(shù)據(jù)進(jìn)行預(yù)測(cè)。

艦艇運(yùn)動(dòng)姿態(tài)的預(yù)測(cè)主要采用自回歸(Autoregressive Model,AR)模型、灰色GM(1,1)預(yù)測(cè)模型、人工神經(jīng)網(wǎng)絡(luò)方法、BP預(yù)測(cè)模型、極限學(xué)習(xí)機(jī)方法、長(zhǎng)短記憶網(wǎng)絡(luò)、Volterra級(jí)數(shù)模型等方法[1-6]，大多數(shù)預(yù)測(cè)算法具有動(dòng)態(tài)更新困難、預(yù)測(cè)效率差、算法復(fù)雜度高等特點(diǎn)，不能滿足本發(fā)射裝置穩(wěn)定控制系統(tǒng)中對(duì)于實(shí)時(shí)預(yù)測(cè)的效率和計(jì)算需求。Kalman濾波是一種適用于實(shí)時(shí)計(jì)算的遞推線性最小方差濾波器，具有實(shí)時(shí)性高和計(jì)算復(fù)雜程度低的特點(diǎn)，可以用于實(shí)時(shí)預(yù)測(cè)艦艇的運(yùn)動(dòng)姿態(tài)。

本文基于Kalman濾波預(yù)測(cè)算法，提出了組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法對(duì)艦艇的橫縱搖運(yùn)動(dòng)姿態(tài)進(jìn)行實(shí)時(shí)預(yù)測(cè)，首先對(duì)組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法進(jìn)行了介紹，然后采用算法流程調(diào)整法、衰減記憶法和平方根法對(duì)預(yù)測(cè)算法改進(jìn)優(yōu)化，最后采用Matlab對(duì)優(yōu)化前后的算法進(jìn)行仿真實(shí)驗(yàn)。通過(guò)對(duì)比分析仿真結(jié)果可知，改進(jìn)優(yōu)化的算法解決了運(yùn)動(dòng)姿態(tài)數(shù)據(jù)因采樣和傳輸產(chǎn)生的延遲問(wèn)題，滿足發(fā)射裝置對(duì)艦艇姿態(tài)實(shí)時(shí)預(yù)測(cè)的需求。

1 組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法

1.1 組合Kalman隔點(diǎn)預(yù)測(cè)法原理

姿態(tài)方位參考系統(tǒng)測(cè)量的艦艇運(yùn)動(dòng)姿態(tài)數(shù)據(jù)需要傳輸給控制系統(tǒng)，其中數(shù)據(jù)采集和數(shù)據(jù)傳輸存在時(shí)間延遲，因此需要將應(yīng)用當(dāng)前采集數(shù)據(jù)預(yù)測(cè)下一點(diǎn)采集數(shù)據(jù)的Kalman濾波預(yù)測(cè)法修改為預(yù)測(cè)第2點(diǎn)采集數(shù)據(jù)的方法，即Kalman隔點(diǎn)預(yù)測(cè)法[7]。將兩個(gè)Kalman隔點(diǎn)預(yù)測(cè)法組合起來(lái)，稱(chēng)為組合Kalman隔點(diǎn)預(yù)測(cè)法，其預(yù)測(cè)周期Tt為數(shù)據(jù)采樣周期的2倍。以數(shù)據(jù)前面的4個(gè)點(diǎn)為已知采樣數(shù)據(jù)，組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的原理如圖1所示。

圖1 組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的原理圖

1.2 Kalman濾波預(yù)測(cè)算法

艦艇運(yùn)動(dòng)姿態(tài)的狀態(tài)方程和量測(cè)方程為[8-13]

X(t)=AX(t-1)+BU(t-1)+W(t-1)

(1)

Z(t)=HX(t)+V(t)

(2)

式中，X(t)為艦艇t時(shí)刻的運(yùn)動(dòng)狀態(tài)；A為狀態(tài)轉(zhuǎn)移陣，將t-1時(shí)刻狀態(tài)和t時(shí)刻狀態(tài)聯(lián)系起來(lái)；X(t-1)為艦艇t-1時(shí)刻的運(yùn)動(dòng)狀態(tài)；B為艦艇姿態(tài)控制輸入U(xiǎn)(t-1)的增益矩陣；U(t-1)為t-1時(shí)刻可選的艦艇姿態(tài)控制輸入；W(t-1)為艦艇姿態(tài)輸入高斯白噪聲，為零均值、方差陣為Q的不相關(guān)白噪聲；Z(t)為對(duì)艦艇運(yùn)動(dòng)姿態(tài)的測(cè)量信號(hào)；H為測(cè)量矩陣，表示狀態(tài)變量X(t)對(duì)測(cè)量變量Z(t)的增益；V(t)為測(cè)量噪聲，為零均值、方差陣為R的不相關(guān)白噪聲。

預(yù)測(cè)估計(jì)的公式為

(3)

(4)

濾波增益矩陣的公式為

(5)

式中，Kg(t)為t時(shí)刻艦艇姿態(tài)的濾波增益矩陣；HT為測(cè)量矩陣的轉(zhuǎn)置；R為測(cè)量噪聲的方差。

濾波估計(jì)的公式為

(6)

(7)

式中，Xt為t時(shí)刻艦艇姿態(tài)的濾波估計(jì)值;Pt為t時(shí)刻艦艇姿態(tài)的濾波誤差協(xié)方差矩陣;I為單位矩陣。

本研究中假設(shè)在艦艇沒(méi)有控制輸入的情況下對(duì)運(yùn)動(dòng)姿態(tài)進(jìn)行實(shí)時(shí)預(yù)測(cè)，式(3)變?yōu)?/p>

(8)

根據(jù)上述Kalman濾波公式的描述，可得到Kalman濾波預(yù)測(cè)流程,如圖2所示。

圖2 Kalman濾波預(yù)測(cè)流程圖

2 組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法優(yōu)化

對(duì)組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法進(jìn)行分析，其算法是先輸出預(yù)測(cè)值，之后加入測(cè)量值計(jì)算估計(jì)值，預(yù)測(cè)值與測(cè)量值間存在滯后；實(shí)際的控制系統(tǒng)不能得到準(zhǔn)確的艦艇運(yùn)動(dòng)姿態(tài)模型，導(dǎo)致預(yù)測(cè)算法中的新測(cè)量值對(duì)預(yù)測(cè)值的修正作用下降，舊測(cè)量值的修正作用相對(duì)上升；預(yù)測(cè)算法在計(jì)算過(guò)程中存在舍入誤差積累，可能導(dǎo)致計(jì)算發(fā)散?；谝陨显?，需要采用算法流程調(diào)整法、衰減記憶法和平方根法對(duì)預(yù)測(cè)算法進(jìn)行改進(jìn)優(yōu)化。

2.1 算法流程調(diào)整法

采用算法流程調(diào)整法解決預(yù)測(cè)值與測(cè)量值間存在滯后的問(wèn)題，調(diào)整組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的計(jì)算流程，先對(duì)艦艇姿態(tài)的濾波增益矩陣和濾波估計(jì)的公式進(jìn)行計(jì)算，然后對(duì)預(yù)測(cè)估計(jì)的公式進(jìn)行計(jì)算。則式(4)～式(8)調(diào)整為

Kg(t)=Pt-1HT/(HPt-1HT+R)

(9)

Xt=Xt-1+Kg(t)(Zt-HXt-1)

(10)

Pt=(I-Kg(t)H)Pt-1

(11)

(12)

(13)

調(diào)整后的Kalman濾波預(yù)測(cè)流程如圖3所示。

圖3 調(diào)整后的Kalman濾波預(yù)測(cè)流程圖

2.2 衰減記憶法

針對(duì)預(yù)測(cè)算法中的新測(cè)量值對(duì)預(yù)測(cè)值的修正作用下降、舊測(cè)量值的修正作用相對(duì)上升的問(wèn)題，采用衰減記憶法予以解決。實(shí)際運(yùn)用中采用的衰減記憶法通過(guò)逐漸減小舊測(cè)量值的權(quán)重，同時(shí)增加新測(cè)量值的權(quán)重[14]來(lái)實(shí)現(xiàn)。與組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法方程相比，衰減記憶法的不同之處在于艦艇姿態(tài)的預(yù)測(cè)誤差協(xié)方差矩陣的計(jì)算公式(式(13))中多了一個(gè)標(biāo)量因子s，即

(14)

式中，s為標(biāo)量因子。

2.3 平方根法

組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法在計(jì)算過(guò)程中的舍入誤差積累會(huì)導(dǎo)致Pt-1和Pt喪失非負(fù)定性，Pt-1和Pt的非負(fù)定性將使Kg(t)的計(jì)算失真，從殘差中提取的補(bǔ)償信息越來(lái)越不準(zhǔn)確，最后造成計(jì)算發(fā)散。采用平方根法解決計(jì)算發(fā)散的問(wèn)題，在計(jì)算過(guò)程中不是計(jì)算Pt-1和Pt，而是計(jì)算Pt-1和Pt的平方根，從而抑制計(jì)算發(fā)散[15]。

由矩陣?yán)碚摽芍我夥橇憔仃嘗n×m與其轉(zhuǎn)置矩陣LT的乘積是非負(fù)定的，L稱(chēng)為矩陣的平方根。進(jìn)行平方根計(jì)算時(shí)采用喬萊斯基(Cholesky)分解法對(duì)誤差協(xié)方差矩陣進(jìn)行下三角分解。

設(shè)非負(fù)定陣P3×3為

(15)

式中，Pij為非負(fù)定陣P的元素。

設(shè)P的下三角分解矩陣Δ和其轉(zhuǎn)置矩陣ΔT為

(16)

(17)

P=Δ×ΔT

(18)

式中，δij為非負(fù)定陣Δ的元素；Δ為P的下三角分解矩陣；ΔT為矩陣Δ的轉(zhuǎn)置矩陣。

將式(9)和式(11)調(diào)整為

(19)

(20)

綜上所述，改進(jìn)優(yōu)化后的組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法與改進(jìn)優(yōu)化前的算法相比，具有如下優(yōu)點(diǎn)：① 前者改變了算法的計(jì)算流程，先對(duì)艦艇姿態(tài)的濾波增益矩陣和濾波估計(jì)的公式進(jìn)行計(jì)算，然后對(duì)預(yù)測(cè)估計(jì)的公式進(jìn)行計(jì)算；② 前者在預(yù)測(cè)誤差協(xié)方差矩陣的計(jì)算式子中添加了一個(gè)標(biāo)量因子s；③ 前者對(duì)Pt-1和Pt進(jìn)行下三角分解，運(yùn)用矩陣的平方根參與計(jì)算。改進(jìn)優(yōu)化算法可以提高預(yù)測(cè)算法的預(yù)測(cè)精度，增加新測(cè)量值對(duì)預(yù)測(cè)值的修正作用，同時(shí)抑制預(yù)測(cè)算法在計(jì)算過(guò)程中的計(jì)算發(fā)散；其比改進(jìn)優(yōu)化前的算法更符合實(shí)際運(yùn)用的需求，更能滿足穩(wěn)定控制系統(tǒng)對(duì)艦艇運(yùn)動(dòng)姿態(tài)預(yù)測(cè)值的需要。

3 仿真實(shí)驗(yàn)

3.1 仿真數(shù)據(jù)模型

本文以姿態(tài)方位參考系統(tǒng)測(cè)量的艦艇近海橫搖數(shù)據(jù)為仿真原始數(shù)據(jù)，數(shù)據(jù)的采樣周期T為0.5 s，共有480個(gè)橫搖數(shù)據(jù)。前40個(gè)橫搖數(shù)據(jù)如表1所示。

表1 前40個(gè)橫搖數(shù)據(jù)表

3.2 初始值和參數(shù)輸入

艦艇的橫搖數(shù)據(jù)是通過(guò)姿態(tài)方位參考系統(tǒng)測(cè)量得到的解耦數(shù)據(jù)，其橫搖運(yùn)動(dòng)為非線性的，為了簡(jiǎn)化運(yùn)動(dòng)模型，假設(shè)艦艇橫搖兩點(diǎn)之間做等加速直線運(yùn)動(dòng)，并通過(guò)實(shí)時(shí)更新橫搖角度、角速度和角加速度減小非線性對(duì)預(yù)測(cè)精度的影響。由等加速直線運(yùn)動(dòng)的運(yùn)動(dòng)規(guī)律可以得到狀態(tài)方程的公式為

(21)

式中，T為數(shù)據(jù)的采樣周期。

所以，可以得到狀態(tài)轉(zhuǎn)移陣A為

(22)

根據(jù)橫搖數(shù)據(jù)前3個(gè)點(diǎn)可以得到初始角度、初始角速度和初始角加速度，其共同組成了初始值矩陣Xt-1；組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的預(yù)測(cè)步長(zhǎng)Tt為1 s；初始矩陣Pt-1的元素取任意假定的非零值，本文中取值如式(23)所示；查閱資料可以得到，在工程實(shí)際中艦艇姿態(tài)輸入高斯白噪聲的方差陣Q一般取單位矩陣，如式(24)所示，方差陣Q中的元素值增大，預(yù)測(cè)算法的均方根誤差減小；測(cè)量噪聲的方差R是根據(jù)姿態(tài)方位參考系統(tǒng)的型號(hào)和技術(shù)指標(biāo)，其測(cè)量橫縱搖的誤差為0.1°，R確定為0.01，方差R增大，預(yù)測(cè)算法的均方根誤差增大；由姿態(tài)方位參考系統(tǒng)得到的數(shù)據(jù)為艦艇橫搖角度位置，所以測(cè)量矩陣H為式(25)。在仿真實(shí)驗(yàn)中，測(cè)量值直接讀取對(duì)應(yīng)時(shí)刻的480個(gè)橫搖數(shù)據(jù)，衰減記憶濾波的標(biāo)量因子s在仿真中確定為100。

(23)

(24)

H=[1 0 0]

(25)

3.3 仿真結(jié)果

根據(jù)初始值和參數(shù)的輸入，采用Matlab對(duì)組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法和優(yōu)化后的組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法進(jìn)行仿真，仿真結(jié)果如圖4和5圖所示。

圖4 橫搖角度預(yù)測(cè)結(jié)果圖

圖5 橫搖角度預(yù)測(cè)誤差圖

圖4中，實(shí)線S為姿態(tài)方位參考系統(tǒng)測(cè)量的近海艦艇橫搖數(shù)據(jù)，點(diǎn)線S1為組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法預(yù)測(cè)的數(shù)據(jù)，點(diǎn)畫(huà)線S2為優(yōu)化后組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法預(yù)測(cè)的數(shù)據(jù)。從圖4的仿真結(jié)果可以分析，組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的預(yù)測(cè)數(shù)據(jù)與測(cè)量數(shù)據(jù)相比，存在數(shù)據(jù)滯后，優(yōu)化后預(yù)測(cè)算法的預(yù)測(cè)數(shù)據(jù)滯后情況得到明顯改善。

圖5中，點(diǎn)線e1為組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法預(yù)測(cè)數(shù)據(jù)與測(cè)量數(shù)據(jù)的誤差，實(shí)線e2為優(yōu)化后組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法預(yù)測(cè)數(shù)據(jù)與測(cè)量數(shù)據(jù)的誤差。從圖5的仿真結(jié)果可以分析，優(yōu)化后預(yù)測(cè)算法預(yù)測(cè)數(shù)據(jù)的誤差比優(yōu)化前預(yù)測(cè)數(shù)據(jù)的誤差要小。

優(yōu)化后的預(yù)測(cè)算法與優(yōu)化前的算法相比，前者主要在采用平方根法對(duì)矩陣Pt-1和Pt計(jì)算時(shí)增加了計(jì)算量，根據(jù)仿真計(jì)算的時(shí)間估計(jì)，優(yōu)化后的預(yù)測(cè)算法增加的計(jì)算量可以忽略不計(jì)。分別對(duì)兩種算法的橫搖角度預(yù)測(cè)誤差求均方根，組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的均方根誤差為0.3935°，優(yōu)化后組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法的均方根誤差為0.1173°。優(yōu)化后的預(yù)測(cè)算法的均方根誤差比原算法減小0.2762°，根據(jù)穩(wěn)定控制系統(tǒng)對(duì)運(yùn)動(dòng)姿態(tài)實(shí)時(shí)預(yù)測(cè)的精度要求，其均方根誤差需要小于0.2°，優(yōu)化后的預(yù)測(cè)算法滿足姿態(tài)數(shù)據(jù)實(shí)時(shí)預(yù)測(cè)的需求。

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

本文提出了組合Kalman濾波隔點(diǎn)預(yù)測(cè)算法對(duì)艦艇的運(yùn)動(dòng)姿態(tài)數(shù)據(jù)進(jìn)行預(yù)測(cè)，針對(duì)預(yù)測(cè)算法存在的預(yù)測(cè)值與測(cè)量值滯后、新測(cè)量值對(duì)預(yù)測(cè)值的修正作用下降和計(jì)算發(fā)散的問(wèn)題，采用算法流程調(diào)整法、衰減記憶法和平方根法對(duì)預(yù)測(cè)算法進(jìn)行改進(jìn)優(yōu)化。依據(jù)姿態(tài)方位參考系統(tǒng)測(cè)量的近海艦艇橫搖數(shù)據(jù)，輸入實(shí)際發(fā)射裝置穩(wěn)定控制系統(tǒng)的參數(shù)，采用Matlab軟件對(duì)優(yōu)化前后的預(yù)測(cè)算法進(jìn)行仿真實(shí)驗(yàn)。通過(guò)將仿真結(jié)果與測(cè)量數(shù)據(jù)進(jìn)行對(duì)比計(jì)算可知，改進(jìn)優(yōu)化后的預(yù)測(cè)算法的均方根誤差小于0.2°，滿足艦艇姿態(tài)數(shù)據(jù)實(shí)時(shí)預(yù)測(cè)的需求。后續(xù)將會(huì)把預(yù)測(cè)算法運(yùn)用到實(shí)際艦艇上的發(fā)射裝置穩(wěn)定控制系統(tǒng)中，達(dá)到預(yù)測(cè)姿態(tài)數(shù)據(jù)的目的。