本發(fā)明涉及的是一種船舶運(yùn)動信息計(jì)算方法，特別是一種船舶升沉運(yùn)動的自主、實(shí)時(shí)、精確計(jì)算方法。

背景技術(shù)：

船舶在海上航行時(shí)會產(chǎn)生包括三維線運(yùn)動和三維角運(yùn)動在內(nèi)的六自由度運(yùn)動，其中，船舶的升沉運(yùn)動，是順利安全開展遠(yuǎn)洋船舶間物資補(bǔ)給、海上作業(yè)、飛機(jī)起降等應(yīng)用領(lǐng)域所必須的重要運(yùn)動信息。捷聯(lián)慣導(dǎo)系統(tǒng)具有自主性強(qiáng)，且能夠連續(xù)地、實(shí)時(shí)地輸出沿載體系軸向的比力信息。因此，對船舶升沉運(yùn)動信息測量往往是基于捷聯(lián)慣導(dǎo)系統(tǒng)原理實(shí)現(xiàn)。利用捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行升沉運(yùn)動信息解算時(shí)，不僅會受到低頻舒勒周期干擾，同時(shí)垂向信息還存在發(fā)散問題。針對上述問題，通過添加數(shù)字高通濾波器的方式，濾除低頻的舒勒周期信號的影響并解決垂向信息發(fā)散的問題。

《動態(tài)環(huán)境下艦船瞬時(shí)線運(yùn)動測量方法研究》(發(fā)表于期刊《系統(tǒng)仿真學(xué)報(bào)》，2013年，04期)一文中，應(yīng)用IIR高通濾波器對低頻舒勒周期信號進(jìn)行濾除并抑制垂向信息發(fā)散，同時(shí)提出將多普勒計(jì)程儀與慣導(dǎo)系統(tǒng)做組合，并利用卡爾曼濾波實(shí)現(xiàn)對艦船姿態(tài)角誤差的準(zhǔn)確估計(jì)，從而提高升沉運(yùn)動信息的計(jì)算精度，但是多普勒計(jì)程儀的引入會破壞慣導(dǎo)系統(tǒng)的自主性，多普勒計(jì)程儀一旦因?yàn)閺?fù)雜工況無法使用時(shí)，IIR高通濾波器輸出信息的超前問題就會導(dǎo)致長航時(shí)升沉信息計(jì)算精度大大降低。現(xiàn)有的船舶升沉運(yùn)動計(jì)算方法無法同時(shí)兼顧長時(shí)間計(jì)算精度和導(dǎo)航自主性，限制了捷聯(lián)慣導(dǎo)系統(tǒng)實(shí)際工作條件下升沉運(yùn)動信息的計(jì)算效果。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種高自主性、高精度的基于自適應(yīng)頻率估計(jì)的船舶升沉運(yùn)動信息估計(jì)方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

步驟一、實(shí)時(shí)采集捷聯(lián)慣導(dǎo)系統(tǒng)的陀螺和加速度計(jì)輸出數(shù)據(jù)，進(jìn)行導(dǎo)航解算，更新載體坐標(biāo)系b到地理坐標(biāo)系t的轉(zhuǎn)換矩陣進(jìn)而得到實(shí)時(shí)的姿態(tài)信息，即橫搖角γ、縱搖角θ、航向角ψ；

步驟二、利用轉(zhuǎn)換矩陣將步驟一采集的加速度計(jì)垂向比力信息投影至地理坐標(biāo)系，計(jì)算沿地理坐標(biāo)系的垂向加速度并利用初始的垂向速度和垂向位移分別對進(jìn)行一次積分和兩次積分運(yùn)算，得到地理坐標(biāo)系下的垂向速度和垂向位移h^t；

所涉及的垂向加速度的計(jì)算表達(dá)式為

式中，是地理坐標(biāo)系下的垂向比力，分別是沿地理坐標(biāo)系水平方向的速度，ω_iex、ω_iey分別是地球坐標(biāo)系e相對于慣性坐標(biāo)系i的角速度的水平分量，ω_etx、ω_ety分別是地理坐標(biāo)系t相對于地球坐標(biāo)系e的角速度的水平分量，g是對應(yīng)高度h處的重力加速度；

步驟三、根據(jù)航向角ψ，得到地理坐標(biāo)系t到半固定坐標(biāo)系d到的轉(zhuǎn)換矩陣T_t^d，并利用轉(zhuǎn)換矩陣T_t^d進(jìn)行坐標(biāo)轉(zhuǎn)化，得到半固定坐標(biāo)系下的垂向速度和垂向位移h^d；

所涉及的轉(zhuǎn)換矩陣T_t^d的表達(dá)式為

步驟四、針對船舶升沉運(yùn)動周期設(shè)計(jì)IIR高通數(shù)字濾波器，用高通數(shù)字濾波器處理半固定坐標(biāo)系下的垂向速度和垂向位移h^d，得到垂向速度和垂向位移的高頻分量；

步驟五、針對船舶不同海況下垂向運(yùn)動信號頻率的實(shí)時(shí)變化，設(shè)計(jì)自適應(yīng)頻率估計(jì)算法，將步驟四中得到的半固定坐標(biāo)系下垂向速度和垂向位移的高頻分量表示成傅里葉級數(shù)形式，通過加權(quán)線性擬合的方式，迭代調(diào)整擬合信號基頻分量和倍頻分量幅值的權(quán)值系數(shù)，不斷補(bǔ)償擬合信號與真實(shí)垂向運(yùn)動信號的擬合誤差，進(jìn)而優(yōu)化估計(jì)出升沉信息的頻率

所涉及的自適應(yīng)頻率估計(jì)算法為

W_k+1＝W_k+2μx_kε_k

式中，x_k為輸入垂向運(yùn)動信號的擬合參考量，x_k＝[x_1k x_2k … x_2Mk]^T；W_k為擬合垂向運(yùn)動信號對應(yīng)的諧波分量權(quán)值，W_k＝[w_1k w_2k … w_2Mk]^T；w_0k為輸入垂向運(yùn)動信號的頻率估計(jì)值；s_k是k時(shí)刻瞬時(shí)垂向運(yùn)動信號的真實(shí)值，即步驟四得到的半固定坐標(biāo)系下的垂向速度和垂向位移的頻率；ε_k是k時(shí)刻瞬時(shí)垂向運(yùn)動信號的擬合誤差；μ是頻率估計(jì)算法的收斂系數(shù)，決定估計(jì)算法的收斂快慢，T是采樣周期；

步驟六、利用步驟五估計(jì)的升沉信息頻率計(jì)算IIR數(shù)字高通濾波器輸出升沉信息的超前相角和超前時(shí)間

所涉及的超前相角和超前時(shí)間的計(jì)算表達(dá)式為

步驟七、根據(jù)步驟六的超前時(shí)間和采樣周期T計(jì)算數(shù)據(jù)延時(shí)位數(shù)N，設(shè)計(jì)數(shù)據(jù)緩沖區(qū)，延時(shí)N位數(shù)據(jù)校正得到實(shí)時(shí)的垂向速度信息和升沉位移信息

所涉及的數(shù)據(jù)延時(shí)位數(shù)N的計(jì)算表達(dá)式為

本發(fā)明相對現(xiàn)有技術(shù)具有如下的優(yōu)點(diǎn)和效果：

(1)本發(fā)明設(shè)計(jì)的自適應(yīng)頻率估計(jì)算法，能夠?qū)崿F(xiàn)對無規(guī)則、不確定的船舶升沉運(yùn)動信號頻率的精確估計(jì)。

(2)本發(fā)明設(shè)計(jì)的基于頻率估計(jì)的延時(shí)校正算法，能夠解決傳統(tǒng)方法應(yīng)用高通濾波器的輸出延遲問題，實(shí)現(xiàn)對升沉信息的實(shí)時(shí)校正。

(3)本發(fā)明的升沉信息計(jì)算方法僅需利用船舶自身的捷聯(lián)慣導(dǎo)系統(tǒng)信息，無需引入外部設(shè)備和其他信息輔助，方法的自主性強(qiáng)。

本發(fā)明能夠有效解決傳統(tǒng)升沉信息計(jì)算方法的時(shí)間超前以及精度不高的問題，實(shí)現(xiàn)船舶升沉運(yùn)動信息的實(shí)時(shí)、精確計(jì)算。

附圖說明

圖1為本發(fā)明提出的基于自適應(yīng)頻率估計(jì)的船舶升沉運(yùn)動信息計(jì)算方法的基本流程框圖。

圖2為本發(fā)明設(shè)計(jì)的自適應(yīng)頻率估計(jì)算法原理框圖。

圖3為本發(fā)明設(shè)計(jì)的自適應(yīng)延時(shí)校正算法原理框圖。

圖4為本發(fā)明方法計(jì)算垂向速度與傳統(tǒng)方法的對比曲線。

圖5為本發(fā)明方法計(jì)算升沉位移與傳統(tǒng)方法的對比曲線。

具體實(shí)施方式

下面結(jié)合附圖對本發(fā)明作詳細(xì)描述。

本發(fā)明提出一種基于自適應(yīng)頻率估計(jì)的船舶升沉運(yùn)動信息計(jì)算方法，其流程圖如附圖1所示，該方法的主要步驟如下：

步驟一、實(shí)時(shí)采集捷聯(lián)慣導(dǎo)系統(tǒng)的陀螺和加速度計(jì)輸出數(shù)據(jù)，進(jìn)行導(dǎo)航解算，更新載體坐標(biāo)系b到地理坐標(biāo)系t的轉(zhuǎn)換矩陣進(jìn)而得到實(shí)時(shí)的姿態(tài)信息，即橫搖角γ、縱搖角θ、航向角ψ；

步驟二、利用轉(zhuǎn)換矩陣將步驟一采集的加速度計(jì)垂向比力信息投影至地理坐標(biāo)系，計(jì)算沿地理坐標(biāo)系的垂向加速度并利用初始的垂向速度和垂向位移分別對進(jìn)行一次積分和兩次積分運(yùn)算，得到地理坐標(biāo)系下的垂向速度和垂向位移h^t；

所涉及的垂向加速度的計(jì)算表達(dá)式為

式中，是地理坐標(biāo)系下的垂向比力，分別是沿地理坐標(biāo)系水平方向的速度，ω_iex、ω_iey分別是地球坐標(biāo)系e相對于慣性坐標(biāo)系i的角速度的水平分量，ω_etx、ω_ety分別是地理坐標(biāo)系t相對于地球坐標(biāo)系e的角速度的水平分量，g是對應(yīng)高度h處的重力加速度；

步驟三、根據(jù)航向角ψ，得到地理坐標(biāo)系t到半固定坐標(biāo)系d到的轉(zhuǎn)換矩陣T_t^d，并利用轉(zhuǎn)換矩陣T_t^d進(jìn)行坐標(biāo)轉(zhuǎn)化，得到半固定坐標(biāo)系下的垂向速度和垂向位移h^d；

所涉及的轉(zhuǎn)換矩陣T_t^d的表達(dá)式為

步驟四、針對船舶升沉運(yùn)動周期設(shè)計(jì)IIR高通數(shù)字濾波器，用高通數(shù)字濾波器處理半固定坐標(biāo)系下的垂向速度和垂向位移h^d，得到垂向速度和垂向位移的高頻分量；

步驟五、針對船舶不同海況下垂向運(yùn)動信號頻率的實(shí)時(shí)變化，設(shè)計(jì)自適應(yīng)頻率估計(jì)算法，將步驟四中得到的半固定坐標(biāo)系下垂向速度和垂向位移的高頻分量表示成傅里葉級數(shù)形式，通過加權(quán)線性擬合的方式，迭代調(diào)整擬合信號基頻分量和倍頻分量幅值的權(quán)值系數(shù)，不斷補(bǔ)償擬合信號與真實(shí)垂向運(yùn)動信號的擬合誤差，進(jìn)而優(yōu)化估計(jì)出升沉信息的頻率

所涉及的自適應(yīng)頻率估計(jì)算法如附圖2所示，相應(yīng)的計(jì)算表達(dá)式為

W_k+1＝W_k+2μx_kε_k

式中，x_k為輸入垂向運(yùn)動信號的擬合參考量，x_k＝[x_1k x_2k … x_2Mk]^T；W_k為擬合垂向運(yùn)動信號對應(yīng)的諧波分量權(quán)值，W_k＝[w_1k w_2k … w_2Mk]^T；w_0k為輸入垂向運(yùn)動信號的頻率估計(jì)值；s_k是k時(shí)刻瞬時(shí)垂向運(yùn)動信號的真實(shí)值，即步驟四得到的半固定坐標(biāo)系下的垂向速度和垂向位移的頻率；ε_k是k時(shí)刻瞬時(shí)垂向運(yùn)動信號的擬合誤差；μ是頻率估計(jì)算法的收斂系數(shù)，決定估計(jì)算法的收斂快慢，T是采樣周期；

步驟六、利用步驟五估計(jì)的升沉信息頻率計(jì)算IIR數(shù)字高通濾波器輸出升沉信息的超前相角和超前時(shí)間

所涉及的超前相角和超前時(shí)間的計(jì)算表達(dá)式為

步驟七、根據(jù)步驟六的超前時(shí)間和采樣周期T計(jì)算數(shù)據(jù)延時(shí)位數(shù)N，設(shè)計(jì)自適應(yīng)延時(shí)校正模塊如附圖3所示，延時(shí)N位數(shù)據(jù)校正得到實(shí)時(shí)的垂向速度信息和升沉位移信息

所涉及的數(shù)據(jù)延時(shí)位數(shù)N的計(jì)算表達(dá)式為

為了驗(yàn)證本發(fā)明方法的合理性、可行性，利用C++程序?qū)λl(fā)明的基于自適應(yīng)頻率估計(jì)的船舶升沉運(yùn)動信息計(jì)算方法進(jìn)行仿真驗(yàn)證。

仿真條件及仿真結(jié)果如下所示：

(1)載體運(yùn)動及初始條件設(shè)置

設(shè)定船舶初始緯度初始經(jīng)度λ＝108.9117°；初始速度0kn；初始橫搖角0°，縱搖角0°，航向角45°；升沉幅值為1m；升沉運(yùn)動頻率0.1Hz。

(2)濾波器設(shè)置

所設(shè)計(jì)的IIR高通濾波器通帶邊緣頻率f_p＝0.03Hz；阻帶截止頻率f_s＝0.01Hz；通帶最大衰減A_p＝1dB；阻帶最小衰減A_S＝30dB；采樣周期T_S＝0.01s。

所設(shè)計(jì)的IIR高通數(shù)字濾波器的轉(zhuǎn)移函數(shù)為

(3)仿真結(jié)果

依上述仿真條件，對所發(fā)明的基于自適應(yīng)頻率估計(jì)的船舶升沉運(yùn)動計(jì)算方法進(jìn)行仿真，幅值1m、頻率0.1Hz條件下本發(fā)明方法計(jì)算的垂向速度和升沉位移與傳統(tǒng)方法的對比曲線分別如圖4、圖5所示。在上述仿真條件下分別對幅值1m、不同頻率下的升沉運(yùn)動進(jìn)行仿真，傳統(tǒng)方法和本發(fā)明方法的升沉位移誤差如表1所示。

表1不同頻率的升沉位移誤差仿真結(jié)果

從圖4、圖5可以看出，本發(fā)明計(jì)算方法能夠精確估計(jì)船舶升沉運(yùn)動的頻率并對濾波器輸出值進(jìn)行延時(shí)補(bǔ)償，較傳統(tǒng)方法實(shí)時(shí)性和精確性更好。從表1可以看出，本發(fā)明計(jì)算方法較傳統(tǒng)方法升沉位移精度實(shí)現(xiàn)1個(gè)數(shù)量級的提升。上述結(jié)果驗(yàn)證了本發(fā)明方法的可行性和有效性。

應(yīng)理解，這些實(shí)施例僅用于說明本發(fā)明而不用于限制本發(fā)明的范圍。此外應(yīng)理解，在閱讀了本發(fā)明講授的內(nèi)容之后，本領(lǐng)域技術(shù)人員可以對本發(fā)明作各種改動或修改，這些等價(jià)形式同樣落于本申請所附權(quán)利要求書所限定的范圍。