基于自适应频率估计的舰船瞬时线运动测量方法

袁书明，程建华，马 斌

（1. 海军装备研究院舰船所，北京 100086；哈尔滨工程大学 自动化学院，哈尔滨 150001；3. 中国船舶重工集团第701研究所，武汉 430064）

基于自适应频率估计的舰船瞬时线运动测量方法

袁书明1，程建华2，马 斌3

（1. 海军装备研究院舰船所，北京 100086；哈尔滨工程大学 自动化学院，哈尔滨 150001；3. 中国船舶重工集团第701研究所，武汉 430064）

针对传统舰船瞬时线运动信息测量过程中，IIR数字高通滤波器的输出会存在相位超前，从而使输出信号相对于实际信号存在时间上超前的问题。分析了超前相位对瞬时线运动信息测量的影响，提出了一种基于自适应频率估计的舰船瞬时线运动测量方法。针对IIR数字高通滤波器超前相位的大小随输入信号频率变化的问题，引入了WFLC频率估计算法来实时估计输入信号的频率，提出了自适应延时校正算法来校正由数字高通滤波器带来的输出信息在时间上的超前量。仿真结果表明：提出的改进舰船捷联瞬时线运动测量方法能够很好地解决传统信息测量方法中输出信息在时间上存在超前的问题，实现了实时舰船瞬时线运动信息精确测量，测量精度由0.13 m提高到了0.02 m。

瞬时线运动；加权傅立叶线性组合；自适应频率估计；自适应延时校正

舰船在海上航行时会产生包括三维线运动和三维角运动在内的六自由度运动，其中，沿载体系轴向的瞬时线运动分量是顺利安全开展远洋舰船间物资补给、舰载机起降和舰载武器发射所必须的重要运动信息。捷联式惯性导航系统（简称捷联惯导系统）具有自主性强，且能够连续地、实时地输出沿载体系轴向的比力信息。因此，对舰船瞬时线运动信息测量往往是基于捷联惯导系统原理实现[1-6]。

在基于捷联惯导系统进行瞬时线运动信息解算时，不仅会受到低频舒勒周期信号干扰，同时垂向信息还存在发散问题。针对这种情况，需要通过添加数字高通滤波器的方式来滤除低频的舒勒周期信号及解决垂向信息发散的问题。文献[7]采用添加数字高通滤波器的方法来滤除低频的舒勒周期信号及解决垂向信息发散的问题，并分析对比了 FIR（Finite Impulse Response）和IIR（Infinite Impulse Response）两种滤波器的优缺点，最终选择了使用IIR滤波器，但是没有解决由于采用IIR数字高通滤波器带来的输出信息相位超前的问题；文献[8][9]在添加IIR数字高通滤波器的基础上，提出将多普勒计程仪与惯导系统做组合，并利用卡尔曼滤波实现对舰船姿态角误差的准确估计，从而提高了瞬时线运动测量精度；针对输出信息存在相位差超前的问题，文献[10]采用了基于卡尔曼滤波法和应用时间序列法两种方法对舰船纵横摇和升沉运动进行了预报，但只实现了短期的预报，随着时间的增长，预报精度会大大降低，不适合长时间的瞬时线运动的测量；文献[11]简要介绍了一种自适应频率估计算法，即加权傅立叶线性组合算法（Weightedfrequency Fourier Linear Combiner, WFLC），并说明了其在电脑输入滤波、临床震颤的量化和显微外科的主动震颤取消上的应用。

基于此，本文拟引入WFLC算法，设计一种基于自适应频率估计的舰船瞬时线运动测量方法。通过自适应频率估计算法估计瞬时线运动信号频率，然后计算出超前相角及超前时间，进而通过延时校正算法对超前量进行校正，有效地对数字高通滤波器导致的超前时间量进行校正，实现IIR实时瞬时线运动信息精确测量。

1 传统瞬时线运动信息测量方法

1.1 基于捷联惯导算法的瞬时线运动测量

舰船的瞬时线运动运动，包括横荡、纵荡和升沉运动，是舰船沿半固定坐标系（简称d系，由地理坐标系顺时针转向舰船的航向角φ得到）的三个轴向相对于海平面的一种线运动的高频分量，分别包括沿xd、yd、zd轴向的速度和位移量。由于捷联惯导系统能输出舰船的三维线运动和角运动信息，因此通过对捷联惯导算法进行调整，即可实现对瞬时线运动信息的提取。

捷联惯导系统利用陀螺仪的测量信息，通过矩阵更新算法能实现姿态矩阵更新和姿态信息解算：

通过姿态矩阵可将捷联惯导系统加速度计测量信息投影至地理系，实现沿地理系轴向线运动信息解算，即：

再通过对地理系方向速度信息的积分运算，即可获得地理系方向的位置矢量信息r，包括经度、纬度和高度信息。

然而，捷联惯导系统输出的线运动与舰船瞬时线运动信息有所区别：

1）由于地理系与半固定坐标系指向差异性，需要将地理系的线运动信息投影至半固定坐标系，即：

式中，

式中，φ为舰船的航向角。

2）捷联惯导输出的线运动信息是相对地球、沿地理坐标系的绝对速度和绝对位置信息，需要通过数字滤波器实现其高频分量的提取。因此，数字高通滤波器的性能指标直接决定了舰船瞬时线运动信息的测量精度。

1.2 传统瞬时线运动测量方法性能分析

数字滤波器分为IIR滤波器和FIR滤波器两种，在技术指标要求相同的情况下，FIR滤波器的阶数要比IIR滤波器的阶数高很多[12]。因此，瞬时线运动信息的提取选用IIR高通滤波器。

以垂向通道为例，舰船升沉运动的周期在1～20 s之间，由此可得IIR高通滤波器的各项参数为：

根据上述滤波器参数指标，可得数字滤波器的转移函数如式(6)所示：

图1和图2按照上述的仿真条件及滤波器参数，给出了垂向速度和升沉运动的提取结果。

图1 垂向速度信息测量仿真结果Fig.1 Vertical velocity information measurement simulation results

图2 升沉信息测量仿真结果Fig.2 Simulation results of heave information measurement

通过图1、图2的仿真结果可以看出，由于IIR高通滤波器的相位超前效应，产生了滤波输出与原始信号的相位差异。通过理论计算可知，当IIR滤波器相位超前为α，对信号产生的升沉测量误差为即当超前相角为10°，升沉运动幅值为1 m时，由超前相角带来的误差大小为0.124 m。

因此，传统方法采用高通滤波器提取瞬时线运动信息时，存在超前相角导致输出信息在时间上超前的问题，进而影响到升沉信息测量的实时性与测量精度。

2 基于自适应频率估计的瞬时线运动信息测量方法

通过传统方法的问题分析可知，采用数字高通滤波器进行瞬时线运动信息提取，其问题在于滤波器对不同频率线运动信息产生的相位超前，导致提取后的信息产生误差。由于相位超前值与输入信号频率有关，因此无法设计固定的相位延迟进行补偿[13]。为此，设计自适应频率估计和自适应延时校正算法，实现对瞬时线运动信息的精确提取。

2.1 瞬时线运动信息的自适应频率估计算法

由于IIR数字高通滤波器的超前相角的大小与滤波器输入信号的频率有关，因此需要实时地估计出滤波器输入信号的频率。

舰船瞬时线运动是一种准周期运动，可视为周期性信号来处理，即在满足狄利克雷条件下，周期信号可以用傅里叶级数来表示，如下式所示：

FLC（Fourier Linear Combiner）算法通过选取合适的基频和谐波次数，使拟合信号的频带能够覆盖原信号的频率带，实现对原信号的估计。由于是自适应算法，FLC在迭代过程中能根据输入信号的变化自动调整各谐波分量的权值以达到最好的估计效果，但FLC算法中的估计信号基频分量和谐波数量都是固定的，适用于频率固定信号估计。对于舰船瞬时线运动信号，在不同海况下的信号频率在实时发生变化，因此必须使FLC算法各频率成分谐波的幅值能跟随原始信号的频率变化而实时调整。

WFLC算法是在FLC算法基础上，在估计过程中对拟合信号的基频分量不断优化以实现对拟合信号基频分量和各频率分量幅值同时进行调整的目的，最终实现最优的估计效果[14-16]。

WFLC算法示意图如图3所示，相对应的加权线性傅里叶组合递归形式为

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

式中：xk为输入参考量，Wk为xk对应谐波分量权值；w0k为基频分量；sk是k时刻瞬时线运动信号真实值；kε是k时刻瞬时线运动信号拟合误差；μ是收敛系数，其决定是否收敛以及收敛的快慢；T是采样时间。

通过式(8)～(11)的递推算法可知，WFLC算法在递推过程中实现了对基频分量的实时调整，因此可以很好地解决了FLC算法只能适用于对固定频率信号估计的问题，实现了对频率变化的信号的估计。而在FLC算法中，基频分量w0k为固定值，不参与实时递推。

2.2 自适应延时校正算法

通过WFLC自适应频率估计算法，可以估计瞬时线运动信号频率，则可以计算出由IIR数字高通滤波器带来的超前相角。以式(6)为例，其超前相角为

式中：ai和bi为滤波器的系数；T为采样周期。

由式(12)可得超前时间为

通过对IIR数字高通滤波数据进行延时输出，就可以得到真实的实时信息。例如，对于式(6)所示的IIR数字高通滤波器的设计结果，当输入信号频率为0.1 Hz，由式(22)可以计算得出超前相角θ()=0.381 78 rad，然后由式(13)就可以计算出此时的超前时间此时只需将信号延迟0.607 6 s即可。

在已知信号采样周期T的情况下，可根据超前时间t()和采样周期T计算数据延时位数N在设计N位数据缓冲区，实现延时N位数据达到延时校正目的。具体算法如图4所示。

图4 自适应延时算法原理Fig.4 Principle of adaptive delay algorithm

IIR数字高通滤波器的输出信息经过自适应频率估计和自适应延时校正模块，就可以得到实时的瞬时线运动信息，提高信息测量精度。

2.3 改进方法的测量实现步骤

改进的舰船瞬时线运动信息测量流程图如图5所示。

图5 实时瞬时线运动信息测量流程图Fig.5 Real-time measurement of the instantaneous linear motion information

由于仍然采用捷联惯导系统测量瞬时线运动信息，因此改进方法相比传统方法的区别主要是IIR数字高通滤波后的自适应频率估计和自适应延时校正两个模块。其中，自适应频率估计模块能够实时地估计出升沉信号的频率大小，自适应延时校正可以根据自适应频率估计模块估计出的频率计算出输出信号超前量的大小并进行延时校正。通过添加这两个模块，能够有效地解决由IIR数字高通滤波器带来的输出信息超前的问题，实现实时的瞬时线运动信息测量。

3 仿真验证

基于自适应频率估计和自适应延时校正的改进舰船瞬时线运动信息测量方法，可以有效抑制输出信息相对于真实信息存在的超前相位差，并提高测量的精度。为了验证改进的瞬时线运动信息测量方法可行性，按照与第2.2节相同的仿真初始条件，进行幅值频率升沉信号的仿真，基于改进方法和传统方法的瞬时线运动信息测量与传统方法做对比的方式，验证改进方法的实时性和精确性。仿真结果如图6、图7所示。

对于幅值为1 m的不同频率的升沉运动进行了仿真，传统方法和改进方法的误差大小如表1所示。

从图6、图7和表1中可以看出，基于自适应频率估计的舰船瞬时线测量方法能够精确估计舰船瞬时线运动的频率并对滤波器输出值进行延时补偿。多组仿真结果表明，通过精确延时将瞬时线运动信息的测量精度由之前的0.13 m提高到了0.02 m，验证了算法的有效性。

图6 改进前后垂向速度信息测量仿真对比结果Fig.6 Comparison on simulation results of vertical velocity information measurements before and after improvement

图7 改进前后升沉信息测量仿真对比结果Fig.7 Comparison on simulation results of heave information measurements before and after improvement

表1 不同频率的升沉运动仿真结果Tab.1 Simulation results of heave motion with different frequencies

4 结 论

针对舰艇瞬时线运动精确测量的需求，设计了一种基于自适应频率估计的舰船瞬时线运动信息测量方法，解决了传统方法因IIR数字高通滤波器相位超前存在测量误差大的问题。仿真结果表明，改进方法能起到很好的频率估计和延时校正作用，实现了实时、精确的瞬时线运动信息测量。这对于满足舰艇海上作业具有重要的工程借鉴价值。

（References）：

[1]孙伟, 孙枫. 基于惯导解算的舰船升沉测量技术[J].仪器仪表学报, 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]刘星伯. 基于捷联航姿系统的舰船瞬时线运动信息测量研究[D]. 哈尔滨: 哈尔滨工程大学, 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]王奎明, 王大海, 宋万成, 等. 基于捷联惯导系统的舰艇垂向位移测量技术[J]. 传感器技术学报, 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]龚晶. 舰船瞬时线运动参数惯性测量技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2010.Gong Jing. Research on strap-down inertial navigation system of Ship’s instantaneous line motion parameters[D].Harbin: Harbin Engineering University, 2010.

[8]孙伟, 孙枫, 杨琳. 动态环境下舰船瞬时线运动测量方法研究[J]. 系统仿真学报, 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]曹冰. 动态环境下舰船瞬时线运动测量技术[D]. 哈尔滨, 哈尔滨工程大学, 2011.Cao Bing. The measurement technology of ship’s instantaneous line motion under multi dynamic environment[D].Harbin: Harbin Engineering University, 2011.

[10]王允峰. 船舶纵横摇和升沉运动预报方法研究[D]. 哈尔滨, 哈尔滨工程大学, 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的数字滤波器设计及其在捷联惯导系统中的应用[J]. 中国惯性技术学报,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

国家自然科学基金（61374007，62173081，61104036）；中央高校基本科研业务费专项资金（HEUCFX41309）

袁书明（1965—），男，博士，高级工程师。E-mail : ysmnavigation@sina.com

联 系 人：程建华（1977—），男，博士，教授，博士生导师，从事组合导航研究。E-mail: ins_cheng@163.com