袁書明，程建華，馬 斌

（1. 海軍裝備研究院艦船所，北京 100086；哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001；3. 中國船舶重工集團(tuán)第701研究所，武漢 430064）

基于自適應(yīng)頻率估計的艦船瞬時線運(yùn)動測量方法

袁書明1，程建華2，馬 斌3

（1. 海軍裝備研究院艦船所，北京 100086；哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001；3. 中國船舶重工集團(tuán)第701研究所，武漢 430064）

針對傳統(tǒng)艦船瞬時線運(yùn)動信息測量過程中，IIR數(shù)字高通濾波器的輸出會存在相位超前，從而使輸出信號相對于實際信號存在時間上超前的問題。分析了超前相位對瞬時線運(yùn)動信息測量的影響，提出了一種基于自適應(yīng)頻率估計的艦船瞬時線運(yùn)動測量方法。針對IIR數(shù)字高通濾波器超前相位的大小隨輸入信號頻率變化的問題，引入了WFLC頻率估計算法來實時估計輸入信號的頻率，提出了自適應(yīng)延時校正算法來校正由數(shù)字高通濾波器帶來的輸出信息在時間上的超前量。仿真結(jié)果表明：提出的改進(jìn)艦船捷聯(lián)瞬時線運(yùn)動測量方法能夠很好地解決傳統(tǒng)信息測量方法中輸出信息在時間上存在超前的問題，實現(xiàn)了實時艦船瞬時線運(yùn)動信息精確測量，測量精度由0.13 m提高到了0.02 m。

瞬時線運(yùn)動；加權(quán)傅立葉線性組合；自適應(yīng)頻率估計；自適應(yīng)延時校正

艦船在海上航行時會產(chǎn)生包括三維線運(yùn)動和三維角運(yùn)動在內(nèi)的六自由度運(yùn)動，其中，沿載體系軸向的瞬時線運(yùn)動分量是順利安全開展遠(yuǎn)洋艦船間物資補(bǔ)給、艦載機(jī)起降和艦載武器發(fā)射所必須的重要運(yùn)動信息。捷聯(lián)式慣性導(dǎo)航系統(tǒng)（簡稱捷聯(lián)慣導(dǎo)系統(tǒng)）具有自主性強(qiáng)，且能夠連續(xù)地、實時地輸出沿載體系軸向的比力信息。因此，對艦船瞬時線運(yùn)動信息測量往往是基于捷聯(lián)慣導(dǎo)系統(tǒng)原理實現(xiàn)[1-6]。

在基于捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行瞬時線運(yùn)動信息解算時，不僅會受到低頻舒勒周期信號干擾，同時垂向信息還存在發(fā)散問題。針對這種情況，需要通過添加數(shù)字高通濾波器的方式來濾除低頻的舒勒周期信號及解決垂向信息發(fā)散的問題。文獻(xiàn)[7]采用添加數(shù)字高通濾波器的方法來濾除低頻的舒勒周期信號及解決垂向信息發(fā)散的問題，并分析對比了 FIR（Finite Impulse Response）和IIR（Infinite Impulse Response）兩種濾波器的優(yōu)缺點，最終選擇了使用IIR濾波器，但是沒有解決由于采用IIR數(shù)字高通濾波器帶來的輸出信息相位超前的問題；文獻(xiàn)[8][9]在添加IIR數(shù)字高通濾波器的基礎(chǔ)上，提出將多普勒計程儀與慣導(dǎo)系統(tǒng)做組合，并利用卡爾曼濾波實現(xiàn)對艦船姿態(tài)角誤差的準(zhǔn)確估計，從而提高了瞬時線運(yùn)動測量精度；針對輸出信息存在相位差超前的問題，文獻(xiàn)[10]采用了基于卡爾曼濾波法和應(yīng)用時間序列法兩種方法對艦船縱橫搖和升沉運(yùn)動進(jìn)行了預(yù)報，但只實現(xiàn)了短期的預(yù)報，隨著時間的增長，預(yù)報精度會大大降低，不適合長時間的瞬時線運(yùn)動的測量；文獻(xiàn)[11]簡要介紹了一種自適應(yīng)頻率估計算法，即加權(quán)傅立葉線性組合算法（Weightedfrequency Fourier Linear Combiner, WFLC），并說明了其在電腦輸入濾波、臨床震顫的量化和顯微外科的主動震顫取消上的應(yīng)用。

基于此，本文擬引入WFLC算法，設(shè)計一種基于自適應(yīng)頻率估計的艦船瞬時線運(yùn)動測量方法。通過自適應(yīng)頻率估計算法估計瞬時線運(yùn)動信號頻率，然后計算出超前相角及超前時間，進(jìn)而通過延時校正算法對超前量進(jìn)行校正，有效地對數(shù)字高通濾波器導(dǎo)致的超前時間量進(jìn)行校正，實現(xiàn)IIR實時瞬時線運(yùn)動信息精確測量。

1 傳統(tǒng)瞬時線運(yùn)動信息測量方法

1.1 基于捷聯(lián)慣導(dǎo)算法的瞬時線運(yùn)動測量

艦船的瞬時線運(yùn)動運(yùn)動，包括橫蕩、縱蕩和升沉運(yùn)動，是艦船沿半固定坐標(biāo)系（簡稱d系，由地理坐標(biāo)系順時針轉(zhuǎn)向艦船的航向角φ得到）的三個軸向相對于海平面的一種線運(yùn)動的高頻分量，分別包括沿xd、yd、zd軸向的速度和位移量。由于捷聯(lián)慣導(dǎo)系統(tǒng)能輸出艦船的三維線運(yùn)動和角運(yùn)動信息，因此通過對捷聯(lián)慣導(dǎo)算法進(jìn)行調(diào)整，即可實現(xiàn)對瞬時線運(yùn)動信息的提取。

捷聯(lián)慣導(dǎo)系統(tǒng)利用陀螺儀的測量信息，通過矩陣更新算法能實現(xiàn)姿態(tài)矩陣更新和姿態(tài)信息解算：

通過姿態(tài)矩陣可將捷聯(lián)慣導(dǎo)系統(tǒng)加速度計測量信息投影至地理系，實現(xiàn)沿地理系軸向線運(yùn)動信息解算，即：

再通過對地理系方向速度信息的積分運(yùn)算，即可獲得地理系方向的位置矢量信息r，包括經(jīng)度、緯度和高度信息。

然而，捷聯(lián)慣導(dǎo)系統(tǒng)輸出的線運(yùn)動與艦船瞬時線運(yùn)動信息有所區(qū)別：

1）由于地理系與半固定坐標(biāo)系指向差異性，需要將地理系的線運(yùn)動信息投影至半固定坐標(biāo)系，即：

式中，

式中，φ為艦船的航向角。

2）捷聯(lián)慣導(dǎo)輸出的線運(yùn)動信息是相對地球、沿地理坐標(biāo)系的絕對速度和絕對位置信息，需要通過數(shù)字濾波器實現(xiàn)其高頻分量的提取。因此，數(shù)字高通濾波器的性能指標(biāo)直接決定了艦船瞬時線運(yùn)動信息的測量精度。

1.2 傳統(tǒng)瞬時線運(yùn)動測量方法性能分析

數(shù)字濾波器分為IIR濾波器和FIR濾波器兩種，在技術(shù)指標(biāo)要求相同的情況下，F(xiàn)IR濾波器的階數(shù)要比IIR濾波器的階數(shù)高很多[12]。因此，瞬時線運(yùn)動信息的提取選用IIR高通濾波器。

以垂向通道為例，艦船升沉運(yùn)動的周期在1～20 s之間，由此可得IIR高通濾波器的各項參數(shù)為：

根據(jù)上述濾波器參數(shù)指標(biāo)，可得數(shù)字濾波器的轉(zhuǎn)移函數(shù)如式(6)所示：

圖1和圖2按照上述的仿真條件及濾波器參數(shù)，給出了垂向速度和升沉運(yùn)動的提取結(jié)果。

圖1 垂向速度信息測量仿真結(jié)果Fig.1 Vertical velocity information measurement simulation results

圖2 升沉信息測量仿真結(jié)果Fig.2 Simulation results of heave information measurement

通過圖1、圖2的仿真結(jié)果可以看出，由于IIR高通濾波器的相位超前效應(yīng)，產(chǎn)生了濾波輸出與原始信號的相位差異。通過理論計算可知，當(dāng)IIR濾波器相位超前為α，對信號產(chǎn)生的升沉測量誤差為即當(dāng)超前相角為10°，升沉運(yùn)動幅值為1 m時，由超前相角帶來的誤差大小為0.124 m。

因此，傳統(tǒng)方法采用高通濾波器提取瞬時線運(yùn)動信息時，存在超前相角導(dǎo)致輸出信息在時間上超前的問題，進(jìn)而影響到升沉信息測量的實時性與測量精度。

2 基于自適應(yīng)頻率估計的瞬時線運(yùn)動信息測量方法

通過傳統(tǒng)方法的問題分析可知，采用數(shù)字高通濾波器進(jìn)行瞬時線運(yùn)動信息提取，其問題在于濾波器對不同頻率線運(yùn)動信息產(chǎn)生的相位超前，導(dǎo)致提取后的信息產(chǎn)生誤差。由于相位超前值與輸入信號頻率有關(guān)，因此無法設(shè)計固定的相位延遲進(jìn)行補(bǔ)償[13]。為此，設(shè)計自適應(yīng)頻率估計和自適應(yīng)延時校正算法，實現(xiàn)對瞬時線運(yùn)動信息的精確提取。

2.1 瞬時線運(yùn)動信息的自適應(yīng)頻率估計算法

由于IIR數(shù)字高通濾波器的超前相角的大小與濾波器輸入信號的頻率有關(guān)，因此需要實時地估計出濾波器輸入信號的頻率。

艦船瞬時線運(yùn)動是一種準(zhǔn)周期運(yùn)動，可視為周期性信號來處理，即在滿足狄利克雷條件下，周期信號可以用傅里葉級數(shù)來表示，如下式所示：

FLC（Fourier Linear Combiner）算法通過選取合適的基頻和諧波次數(shù)，使擬合信號的頻帶能夠覆蓋原信號的頻率帶，實現(xiàn)對原信號的估計。由于是自適應(yīng)算法，F(xiàn)LC在迭代過程中能根據(jù)輸入信號的變化自動調(diào)整各諧波分量的權(quán)值以達(dá)到最好的估計效果，但FLC算法中的估計信號基頻分量和諧波數(shù)量都是固定的，適用于頻率固定信號估計。對于艦船瞬時線運(yùn)動信號，在不同海況下的信號頻率在實時發(fā)生變化，因此必須使FLC算法各頻率成分諧波的幅值能跟隨原始信號的頻率變化而實時調(diào)整。

WFLC算法是在FLC算法基礎(chǔ)上，在估計過程中對擬合信號的基頻分量不斷優(yōu)化以實現(xiàn)對擬合信號基頻分量和各頻率分量幅值同時進(jìn)行調(diào)整的目的，最終實現(xiàn)最優(yōu)的估計效果[14-16]。

WFLC算法示意圖如圖3所示，相對應(yīng)的加權(quán)線性傅里葉組合遞歸形式為

圖3 WFLC算法原理Fig.3 Algorithm of weighted-frequency Fourier linear combiner

式中：xk為輸入?yún)⒖剂?，Wk為xk對應(yīng)諧波分量權(quán)值；w0k為基頻分量；sk是k時刻瞬時線運(yùn)動信號真實值；kε是k時刻瞬時線運(yùn)動信號擬合誤差；μ是收斂系數(shù)，其決定是否收斂以及收斂的快慢；T是采樣時間。

通過式(8)～(11)的遞推算法可知，WFLC算法在遞推過程中實現(xiàn)了對基頻分量的實時調(diào)整，因此可以很好地解決了FLC算法只能適用于對固定頻率信號估計的問題，實現(xiàn)了對頻率變化的信號的估計。而在FLC算法中，基頻分量w0k為固定值，不參與實時遞推。

2.2 自適應(yīng)延時校正算法

通過WFLC自適應(yīng)頻率估計算法，可以估計瞬時線運(yùn)動信號頻率，則可以計算出由IIR數(shù)字高通濾波器帶來的超前相角。以式(6)為例，其超前相角為

式中：ai和bi為濾波器的系數(shù)；T為采樣周期。

由式(12)可得超前時間為

通過對IIR數(shù)字高通濾波數(shù)據(jù)進(jìn)行延時輸出，就可以得到真實的實時信息。例如，對于式(6)所示的IIR數(shù)字高通濾波器的設(shè)計結(jié)果，當(dāng)輸入信號頻率為0.1 Hz，由式(22)可以計算得出超前相角θ()=0.381 78 rad，然后由式(13)就可以計算出此時的超前時間此時只需將信號延遲0.607 6 s即可。

在已知信號采樣周期T的情況下，可根據(jù)超前時間t()和采樣周期T計算數(shù)據(jù)延時位數(shù)N在設(shè)計N位數(shù)據(jù)緩沖區(qū)，實現(xiàn)延時N位數(shù)據(jù)達(dá)到延時校正目的。具體算法如圖4所示。

圖4 自適應(yīng)延時算法原理Fig.4 Principle of adaptive delay algorithm

IIR數(shù)字高通濾波器的輸出信息經(jīng)過自適應(yīng)頻率估計和自適應(yīng)延時校正模塊，就可以得到實時的瞬時線運(yùn)動信息，提高信息測量精度。

2.3 改進(jìn)方法的測量實現(xiàn)步驟

改進(jìn)的艦船瞬時線運(yùn)動信息測量流程圖如圖5所示。

圖5 實時瞬時線運(yùn)動信息測量流程圖Fig.5 Real-time measurement of the instantaneous linear motion information

由于仍然采用捷聯(lián)慣導(dǎo)系統(tǒng)測量瞬時線運(yùn)動信息，因此改進(jìn)方法相比傳統(tǒng)方法的區(qū)別主要是IIR數(shù)字高通濾波后的自適應(yīng)頻率估計和自適應(yīng)延時校正兩個模塊。其中，自適應(yīng)頻率估計模塊能夠?qū)崟r地估計出升沉信號的頻率大小，自適應(yīng)延時校正可以根據(jù)自適應(yīng)頻率估計模塊估計出的頻率計算出輸出信號超前量的大小并進(jìn)行延時校正。通過添加這兩個模塊，能夠有效地解決由IIR數(shù)字高通濾波器帶來的輸出信息超前的問題，實現(xiàn)實時的瞬時線運(yùn)動信息測量。

3 仿真驗證

基于自適應(yīng)頻率估計和自適應(yīng)延時校正的改進(jìn)艦船瞬時線運(yùn)動信息測量方法，可以有效抑制輸出信息相對于真實信息存在的超前相位差，并提高測量的精度。為了驗證改進(jìn)的瞬時線運(yùn)動信息測量方法可行性，按照與第2.2節(jié)相同的仿真初始條件，進(jìn)行幅值頻率升沉信號的仿真，基于改進(jìn)方法和傳統(tǒng)方法的瞬時線運(yùn)動信息測量與傳統(tǒng)方法做對比的方式，驗證改進(jìn)方法的實時性和精確性。仿真結(jié)果如圖6、圖7所示。

對于幅值為1 m的不同頻率的升沉運(yùn)動進(jìn)行了仿真，傳統(tǒng)方法和改進(jìn)方法的誤差大小如表1所示。

從圖6、圖7和表1中可以看出，基于自適應(yīng)頻率估計的艦船瞬時線測量方法能夠精確估計艦船瞬時線運(yùn)動的頻率并對濾波器輸出值進(jìn)行延時補(bǔ)償。多組仿真結(jié)果表明，通過精確延時將瞬時線運(yùn)動信息的測量精度由之前的0.13 m提高到了0.02 m，驗證了算法的有效性。

圖6 改進(jìn)前后垂向速度信息測量仿真對比結(jié)果Fig.6 Comparison on simulation results of vertical velocity information measurements before and after improvement

圖7 改進(jìn)前后升沉信息測量仿真對比結(jié)果Fig.7 Comparison on simulation results of heave information measurements before and after improvement

表1 不同頻率的升沉運(yùn)動仿真結(jié)果Tab.1 Simulation results of heave motion with different frequencies

4 結(jié) 論

針對艦艇瞬時線運(yùn)動精確測量的需求，設(shè)計了一種基于自適應(yīng)頻率估計的艦船瞬時線運(yùn)動信息測量方法，解決了傳統(tǒng)方法因IIR數(shù)字高通濾波器相位超前存在測量誤差大的問題。仿真結(jié)果表明，改進(jìn)方法能起到很好的頻率估計和延時校正作用，實現(xiàn)了實時、精確的瞬時線運(yùn)動信息測量。這對于滿足艦艇海上作業(yè)具有重要的工程借鑒價值。

（References）：

[1]孫偉, 孫楓. 基于慣導(dǎo)解算的艦船升沉測量技術(shù)[J].儀器儀表學(xué)報, 2012, 33(1): 167-172.Sun Wei, Sun Feng. Measurement technology of ship heave movement based on SINS resolving[J]. Chinese Journal of Scientific Instrument, 2012, 33(1): 167-172.

[2]劉星伯. 基于捷聯(lián)航姿系統(tǒng)的艦船瞬時線運(yùn)動信息測量研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2009.Liu Xing-bo. Study on the measuring of the instaneous movements of the ships based on SINS[D]. Harbin:Harbin Engineering University, 2009.

[3]Shi Bu-hai, Xian Ling, Wu Qi-peng, et al. Active heave compensation prediction research for deep sea homework crane based on KPSO – SVR[C]//Proceedings of the 33rd Chinese Control Conference. 2014: 7637-7642.

[4]王奎明, 王大海, 宋萬成, 等. 基于捷聯(lián)慣導(dǎo)系統(tǒng)的艦艇垂向位移測量技術(shù)[J]. 傳感器技術(shù)學(xué)報, 2011, 24(12):1696-1700.Wang Kui-min, Wang Da-hai, Song Wan-cheng, et al. The measurement technology about vertical displacement of vessel movement based on SINS[J]. Chinese Journal of Sensors and Actuators, 2011, 24(12): 1696-1700.

[5]Auestad ? F, Gravdahl J T, Fossen T I. Heave motion estimation on a craft using a strapdown inertial measurement unit[C]//9th IFAC Conference on Control Applications in Marine Systems. 2013: 298-303.

[6]Richter M, Schneider K, Walser D, et al. Real-time heave motion estimation using adaptive filtering techniques[J].Preprints of the 19th World Congress. 2014, 10119-10125.

[7]龔晶. 艦船瞬時線運(yùn)動參數(shù)慣性測量技術(shù)研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2010.Gong Jing. Research on strap-down inertial navigation system of Ship’s instantaneous line motion parameters[D].Harbin: Harbin Engineering University, 2010.

[8]孫偉, 孫楓, 楊琳. 動態(tài)環(huán)境下艦船瞬時線運(yùn)動測量方法研究[J]. 系統(tǒng)仿真學(xué)報, 2013, 25(4): 839-844.Sun Wei, Sun Feng, Yang Lin. Research on measurement method of warship instantaneous line motion under condition of dynamic motion[J]. Journal of System Simulation, 2013, 25(4): 839-844.

[9]曹冰. 動態(tài)環(huán)境下艦船瞬時線運(yùn)動測量技術(shù)[D]. 哈爾濱, 哈爾濱工程大學(xué), 2011.Cao Bing. The measurement technology of ship’s instantaneous line motion under multi dynamic environment[D].Harbin: Harbin Engineering University, 2011.

[10]王允峰. 船舶縱橫搖和升沉運(yùn)動預(yù)報方法研究[D]. 哈爾濱, 哈爾濱工程大學(xué), 2010.Wang Yun-feng. Research on prediction method of ship pitch, roll and heave motion[D]. Harbin: Harbin Engineering University, 2010.

[11]Riviere C N, Thakor N. Modeling and canceling tremor in human-machine interfaces[J]. IEEE Engineering in Medicine and Biology, 1996, 15(3): 29-36.

[12]陳江良, 陸志東. 基于 Matlab的數(shù)字濾波器設(shè)計及其在捷聯(lián)慣導(dǎo)系統(tǒng)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報,2005, 13(6): 10-12.CHEN Jiang-liang, LU Zhi-dong. Digital filter design based on Matlab software and its application in SINS[J].Journal of Chinese Inertial Technology, 2005, 13(6): 10-12.

[13]Hu Yongpan, Tao Limin. Real-time zero phase filtering for heave measurement[C]//The 11th IEEE International Conference on Electronic Measurement & Instruments, 2013.

[14]Vaz C, Kong X, Thakor N. An adaptive estimation of periodic signals using a Fourier linear combiner[J]. IEEE Transactions on Signal Processing, 1994, 42: 1-10.

[15]Latt W T. Real-time estimation and prediction of periodic signals from attenuated and phase-shifted sensed signals [C]//2009 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Singapore, 2009.

[16]Küchler S, Eberharter J K, Langer K, et al. Heave motion estimation of a vessel using acceleration measurements[C]//Preprints of the 18th IFAC World Congress, 2011,14742- 14747.

Measurement method for ship instantaneous linear movement based on adaptive frequency estimation

YUAN Shu-ming1, CHENG Jian-hua2, MA Bin3
(1. The Research Institute of Ships, Navy Academy of Armament, Beijing 100086, China;2. Automation College, Harbin Engineering University, Harbin 150001, China;3. 701 Institute, China Shipbuilding Industry Corporation, Wuhan 430064, China)

In measuring the instantaneous linear motion by traditional ship, there usually exists phase advance in the output of IIR digital high-pass filter, which cause the phase of the output signal is ahead of the original signal. To solve this problem, a new measurement method for the instantaneous linear motion is proposed based on adaptive frequency estimation. In view that the magnitude of phase advance is varied with the frequency of the original signal, a WFLC (Weighted-frequency Fourier Linear Combiner) frequency estimation algorithm is used to estimate the frequency of the original signal, and an adaptive delay correction algorithm is designed to correct the phase advance caused by the digital high-pass filter. Simulation results show that the new measurement method based on adaptive frequency estimation can solve the phase advance problem and accurately measure the ship instantaneous linear movement information. The measurement accuracy is increased to 0.02 m from the original 0.13 m.

instantaneous linear movement; weighted-frequency Fourier linear combiner; adaptive frequency estimation; adaptive delay correction

U666.1

A

1005-6734(2016)05-0565-06

10.13695/j.cnki.12-1222/o3.2016.05.002

2016-06-13；

2016-08-03

國家自然科學(xué)基金（61374007，62173081，61104036）；中央高?；究蒲袠I(yè)務(wù)費(fèi)專項資金（HEUCFX41309）

袁書明（1965—），男，博士，高級工程師。E-mail : ysmnavigation@sina.com

聯(lián) 系 人：程建華（1977—），男，博士，教授，博士生導(dǎo)師，從事組合導(dǎo)航研究。E-mail: ins_cheng@163.com