CN106643728A - 基于自适应频率估计的船舶升沉运动信息估计方法 - Google Patents

Abstract

本发明提供的是一种基于自适应频率估计的船舶升沉运动信息估计方法。本发明设计了自适应频率估计算法对输入信号的频率进行实时估计，利用估计得到的信号频率计算出超前相角和超前时间，再对输出信息的时间超前量进行自适应延时校正。本发明设计的自适应频率估计算法，能够实现对无规则、不确定的船舶升沉运动信号频率的精确估计；设计的基于频率估计的延时校正算法，能够解决传统方法应用高通滤波器的输出延迟问题，实现对升沉信息的实时校正；升沉信息计算方法仅需利用船舶自身的捷联惯导系统信息，无需引入外部设备和其他信息辅助，方法的自主性强。

Description

基于自适应频率估计的船舶升沉运动信息估计方法

技术领域

本发明涉及的是一种船舶运动信息计算方法，特别是一种船舶升沉运动的自主、实时、精确计算方法。

背景技术

船舶在海上航行时会产生包括三维线运动和三维角运动在内的六自由度运动，其中，船舶的升沉运动，是顺利安全开展远洋船舶间物资补给、海上作业、飞机起降等应用领域所必须的重要运动信息。捷联惯导系统具有自主性强，且能够连续地、实时地输出沿载体系轴向的比力信息。因此，对船舶升沉运动信息测量往往是基于捷联惯导系统原理实现。利用捷联惯导系统进行升沉运动信息解算时，不仅会受到低频舒勒周期干扰，同时垂向信息还存在发散问题。针对上述问题，通过添加数字高通滤波器的方式，滤除低频的舒勒周期信号的影响并解决垂向信息发散的问题。

《动态环境下舰船瞬时线运动测量方法研究》(发表于期刊《系统仿真学报》，2013年，04期)一文中，应用IIR高通滤波器对低频舒勒周期信号进行滤除并抑制垂向信息发散，同时提出将多普勒计程仪与惯导系统做组合，并利用卡尔曼滤波实现对舰船姿态角误差的准确估计，从而提高升沉运动信息的计算精度，但是多普勒计程仪的引入会破坏惯导系统的自主性，多普勒计程仪一旦因为复杂工况无法使用时，IIR高通滤波器输出信息的超前问题就会导致长航时升沉信息计算精度大大降低。现有的船舶升沉运动计算方法无法同时兼顾长时间计算精度和导航自主性，限制了捷联惯导系统实际工作条件下升沉运动信息的计算效果。

发明内容

本发明的目的在于提供一种高自主性、高精度的基于自适应频率估计的船舶升沉运动信息估计方法。

本发明的目的是这样实现的：

步骤一、实时采集捷联惯导系统的陀螺和加速度计输出数据，进行导航解算，更新载体坐标系b到地理坐标系t的转换矩阵进而得到实时的姿态信息，即横摇角γ、纵摇角θ、航向角ψ；

步骤二、利用转换矩阵将步骤一采集的加速度计垂向比力信息投影至地理坐标系，计算沿地理坐标系的垂向加速度并利用初始的垂向速度和垂向位移分别对进行一次积分和两次积分运算，得到地理坐标系下的垂向速度和垂向位移h^t；

所涉及的垂向加速度的计算表达式为

式中，是地理坐标系下的垂向比力，分别是沿地理坐标系水平方向的速度，ω_iex、ω_iey分别是地球坐标系e相对于惯性坐标系i的角速度的水平分量，ω_etx、ω_ety分别是地理坐标系t相对于地球坐标系e的角速度的水平分量，g是对应高度h处的重力加速度；

步骤三、根据航向角ψ，得到地理坐标系t到半固定坐标系d到的转换矩阵T_t ^d，并利用转换矩阵T_t ^d进行坐标转化，得到半固定坐标系下的垂向速度和垂向位移h^d；

所涉及的转换矩阵T_t ^d的表达式为

步骤四、针对船舶升沉运动周期设计IIR高通数字滤波器，用高通数字滤波器处理半固定坐标系下的垂向速度和垂向位移h^d，得到垂向速度和垂向位移的高频分量；

步骤五、针对船舶不同海况下垂向运动信号频率的实时变化，设计自适应频率估计算法，将步骤四中得到的半固定坐标系下垂向速度和垂向位移的高频分量表示成傅里叶级数形式，通过加权线性拟合的方式，迭代调整拟合信号基频分量和倍频分量幅值的权值系数，不断补偿拟合信号与真实垂向运动信号的拟合误差，进而优化估计出升沉信息的频率

所涉及的自适应频率估计算法为

W_k+1＝W_k+2μx_kε_k

式中，x_k为输入垂向运动信号的拟合参考量，x_k＝[x_1k x_2k … x_2Mk]^T；W_k为拟合垂向运动信号对应的谐波分量权值，W_k＝[w_1k w_2k … w_2Mk]^T；w_0k为输入垂向运动信号的频率估计值；s_k是k时刻瞬时垂向运动信号的真实值，即步骤四得到的半固定坐标系下的垂向速度和垂向位移的频率；ε_k是k时刻瞬时垂向运动信号的拟合误差；μ是频率估计算法的收敛系数，决定估计算法的收敛快慢，T是采样周期；

步骤六、利用步骤五估计的升沉信息频率计算IIR数字高通滤波器输出升沉信息的超前相角和超前时间

所涉及的超前相角和超前时间的计算表达式为

步骤七、根据步骤六的超前时间和采样周期T计算数据延时位数N，设计数据缓冲区，延时N位数据校正得到实时的垂向速度信息和升沉位移信息

所涉及的数据延时位数N的计算表达式为

本发明相对现有技术具有如下的优点和效果：

(1)本发明设计的自适应频率估计算法，能够实现对无规则、不确定的船舶升沉运动信号频率的精确估计。

(2)本发明设计的基于频率估计的延时校正算法，能够解决传统方法应用高通滤波器的输出延迟问题，实现对升沉信息的实时校正。

(3)本发明的升沉信息计算方法仅需利用船舶自身的捷联惯导系统信息，无需引入外部设备和其他信息辅助，方法的自主性强。

本发明能够有效解决传统升沉信息计算方法的时间超前以及精度不高的问题，实现船舶升沉运动信息的实时、精确计算。

附图说明

图1为本发明提出的基于自适应频率估计的船舶升沉运动信息计算方法的基本流程框图。

图2为本发明设计的自适应频率估计算法原理框图。

图3为本发明设计的自适应延时校正算法原理框图。

图4为本发明方法计算垂向速度与传统方法的对比曲线。

图5为本发明方法计算升沉位移与传统方法的对比曲线。

具体实施方式

下面结合附图对本发明作详细描述。

本发明提出一种基于自适应频率估计的船舶升沉运动信息计算方法，其流程图如附图1所示，该方法的主要步骤如下：

步骤一、实时采集捷联惯导系统的陀螺和加速度计输出数据，进行导航解算，更新载体坐标系b到地理坐标系t的转换矩阵进而得到实时的姿态信息，即横摇角γ、纵摇角θ、航向角ψ；

步骤二、利用转换矩阵将步骤一采集的加速度计垂向比力信息投影至地理坐标系，计算沿地理坐标系的垂向加速度并利用初始的垂向速度和垂向位移分别对进行一次积分和两次积分运算，得到地理坐标系下的垂向速度和垂向位移h^t；

所涉及的垂向加速度的计算表达式为

式中，是地理坐标系下的垂向比力，分别是沿地理坐标系水平方向的速度，ω_iex、ω_iey分别是地球坐标系e相对于惯性坐标系i的角速度的水平分量，ω_etx、ω_ety分别是地理坐标系t相对于地球坐标系e的角速度的水平分量，g是对应高度h处的重力加速度；

步骤三、根据航向角ψ，得到地理坐标系t到半固定坐标系d到的转换矩阵T_t ^d，并利用转换矩阵T_t ^d进行坐标转化，得到半固定坐标系下的垂向速度和垂向位移h^d；

所涉及的转换矩阵T_t ^d的表达式为

步骤四、针对船舶升沉运动周期设计IIR高通数字滤波器，用高通数字滤波器处理半固定坐标系下的垂向速度和垂向位移h^d，得到垂向速度和垂向位移的高频分量；

步骤五、针对船舶不同海况下垂向运动信号频率的实时变化，设计自适应频率估计算法，将步骤四中得到的半固定坐标系下垂向速度和垂向位移的高频分量表示成傅里叶级数形式，通过加权线性拟合的方式，迭代调整拟合信号基频分量和倍频分量幅值的权值系数，不断补偿拟合信号与真实垂向运动信号的拟合误差，进而优化估计出升沉信息的频率

所涉及的自适应频率估计算法如附图2所示，相应的计算表达式为

W_k+1＝W_k+2μx_kε_k

式中，x_k为输入垂向运动信号的拟合参考量，x_k＝[x_1k x_2k … x_2Mk]^T；W_k为拟合垂向运动信号对应的谐波分量权值，W_k＝[w_1k w_2k … w_2Mk]^T；w_0k为输入垂向运动信号的频率估计值；s_k是k时刻瞬时垂向运动信号的真实值，即步骤四得到的半固定坐标系下的垂向速度和垂向位移的频率；ε_k是k时刻瞬时垂向运动信号的拟合误差；μ是频率估计算法的收敛系数，决定估计算法的收敛快慢，T是采样周期；

步骤六、利用步骤五估计的升沉信息频率计算IIR数字高通滤波器输出升沉信息的超前相角和超前时间

所涉及的超前相角和超前时间的计算表达式为

步骤七、根据步骤六的超前时间和采样周期T计算数据延时位数N，设计自适应延时校正模块如附图3所示，延时N位数据校正得到实时的垂向速度信息和升沉位移信息

所涉及的数据延时位数N的计算表达式为

为了验证本发明方法的合理性、可行性，利用C++程序对所发明的基于自适应频率估计的船舶升沉运动信息计算方法进行仿真验证。

仿真条件及仿真结果如下所示：

(1)载体运动及初始条件设置

设定船舶初始纬度初始经度λ＝108.9117°；初始速度0kn；初始横摇角0°，纵摇角0°，航向角45°；升沉幅值为1m；升沉运动频率0.1Hz。

(2)滤波器设置

所设计的IIR高通滤波器通带边缘频率f_p＝0.03Hz；阻带截止频率f_s＝0.01Hz；通带最大衰减A_p＝1dB；阻带最小衰减A_S＝30dB；采样周期T_S＝0.01s。

所设计的IIR高通数字滤波器的转移函数为

(3)仿真结果

依上述仿真条件，对所发明的基于自适应频率估计的船舶升沉运动计算方法进行仿真，幅值1m、频率0.1Hz条件下本发明方法计算的垂向速度和升沉位移与传统方法的对比曲线分别如图4、图5所示。在上述仿真条件下分别对幅值1m、不同频率下的升沉运动进行仿真，传统方法和本发明方法的升沉位移误差如表1所示。

表1不同频率的升沉位移误差仿真结果

从图4、图5可以看出，本发明计算方法能够精确估计船舶升沉运动的频率并对滤波器输出值进行延时补偿，较传统方法实时性和精确性更好。从表1可以看出，本发明计算方法较传统方法升沉位移精度实现1个数量级的提升。上述结果验证了本发明方法的可行性和有效性。

应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

Claims (6)

1.一种基于自适应频率估计的船舶升沉运动信息估计方法，其特征是：

步骤一、实时采集捷联惯导系统的陀螺和加速度计输出数据，进行导航解算，更新载体坐标系b到地理坐标系t的转换矩阵进而得到实时的姿态信息，即横摇角γ、纵摇角θ、航向角ψ；

步骤二、利用载体坐标系b到地理坐标系t的转换矩阵将步骤一采集的加速度计垂向比力信息投影至地理坐标系，计算沿地理坐标系的垂向加速度并利用初始的垂向速度和垂向位移分别对进行一次积分和两次积分运算，得到地理坐标系下的垂向速度和垂向位移h^t；

步骤三、根据航向角ψ，得到地理坐标系t到半固定坐标系d的转换矩阵T_t ^d，并利用地理坐标系t到半固定坐标系d的转换矩阵T_t ^d进行坐标转化，得到半固定坐标系下的垂向速度和垂向位移h^d；

步骤四、针对船舶升沉运动周期设计IIR高通数字滤波器，用高通数字滤波器处理半固定坐标系下的垂向速度和垂向位移h^d，得到垂向速度和垂向位移的高频分量；

步骤五、针对船舶不同海况下垂向运动信号频率的实时变化，设计自适应频率估计算法，将步骤四中得到的半固定坐标系下垂向速度和垂向位移的高频分量表示成傅里叶级数形式，通过加权线性拟合的方式，迭代调整拟合信号基频分量和倍频分量幅值的权值系数，不断补偿拟合信号与真实垂向运动信号的拟合误差，进而优化估计出升沉信息的频率

步骤六、利用步骤五估计的升沉信息频率计算IIR数字高通滤波器输出升沉信息的超前相角和超前时间

步骤七、根据步骤六的超前时间和采样周期T计算数据延时位数N，设计数据缓冲区，延时N位数据校正得到实时的垂向速度信息和升沉位移信息

2.根据权利要求1所述的基于自适应频率估计的船舶升沉运动信息估计方法，其特征是所述的地理坐标系下的垂向加速度的表达式为：

${\stackrel{\·}{V}}_z^t=f_z^t+(2{\mathrm{\ω}}_{iey}+{\mathrm{\ω}}_{ety})V_x^t-(2{\mathrm{\ω}}_{iex}+{\mathrm{\ω}}_{etx})V_y^t-g$

其中，是地理坐标系下的垂向比力，分别是沿地理坐标系水平方向的速度，ω_iex、ω_iey分别是地球坐标系e相对于惯性坐标系i的角速度的水平分量，ω_etx、ω_ety分别是地理坐标系t相对于地球坐标系e的角速度的水平分量，g是对应高度h处的重力加速度。

3.根据权利要求2所述的基于自适应频率估计的船舶升沉运动信息估计方法，其特征是所述的地理坐标系t到半固定坐标系d的转换矩阵T_t ^d的表达式为：

$T_t^d=\left[\begin{array}{ccc}cos\mathrm{\ψ}& sin\mathrm{\ψ}& 0\\ -sin\mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right].$

4.根据权利要求3所述的基于自适应频率估计的船舶升沉运动信息估计方法，其特征是所述的自适应频率估计算法为：

$x_{rk}=\left\{\begin{array}{cc}sin\left(rT\underset{t=0}{\overset{k}{\mathrm{\Σ}}}{w}_{0t}\right)& 1\≤r\≤M\\ cos\left(\right(r-M\left)T\underset{t=0}{\overset{k}{\mathrm{\Σ}}}{w}_{0t}\right)& M+1\≤r\≤2M\end{array}\right.$

${\mathrm{\ϵ}}_k=s_k-W_k^T{x}_k$

$w_{0_{k+1}}=w_{0_k}+2{\mathrm{\μ}}_0{\mathrm{\ϵ}}_k\underset{i=1}{\overset{M}{\Σ}}i(w_{i_k}{x}_{M+i_k}-w_{M+i_k}{x}_{i_k})$

W_k+1＝W_k+2μx_kε_k

其中，x_k为输入垂向运动信号的拟合参考量，x_k＝[x_1k x_2k … x_2Mk]^T；W_k为拟合垂向运动信号对应的谐波分量权值，W_k＝[w_1k w_2k … w_2Mk]^T；w_0k为输入垂向运动信号的频率估计值；s_k是k时刻瞬时垂向运动信号的真实值，即步骤四得到的半固定坐标系下的垂向速度和垂向位移的频率；ε_k是k时刻瞬时垂向运动信号的拟合误差；μ是频率估计算法的收敛系数，决定估计算法的收敛快慢，T是采样周期。

5.根据权利要求4所述的基于自适应频率估计的船舶升沉运动信息估计方法，其特征是所述的超前相角和超前时间的表达式为：

$\mathrm{\θ}\left(\widetilde{\mathrm{\ω}}\right)=\arg H\left(\exp \right(j\widetilde{\mathrm{\ω}}T\left)\right);$

$t\left(\widetilde{\mathrm{\ω}}\right)=\mathrm{\θ}\left(\widetilde{\mathrm{\ω}}\right)/\widetilde{\mathrm{\ω}}.$

6.根据权利要求5所述的基于自适应频率估计的船舶升沉运动信息估计方法，其特征是所述的所述的数据延时位数N的表达式为：

$N=t\left(\widetilde{\mathrm{\ω}}\right)/T.$