﻿ 基于简化Sage-Husa自适应滤波的船舶升沉估计方法
 舰船科学技术  2023, Vol. 45 Issue (1): 45-49    DOI: 10.3404/j.issn.1672-7649.2023.01.009 PDF

1. 浙江海洋大学 船舶与海运学院，浙江 舟山 316022;
2. 舟山市质量技术监督检测研究院，浙江 舟山 316000

Research on ship heave estimation method based on simplified Sage-Husa adaptive filtering
WEN Xiao-fei1, LI Jiang-fan1, WANG Hai-rong2, ZHU Hao-gang1
1. School of Naval Architecture and Maritime, Zhejiang Ocean University, Zhoushan 316022, China;
2. Zhoushan Institute of Calibration and Testing for Quality and Technology Supervision, Zhoushan 316000, China
Abstract: In order to obtain ship heave information accurately in real time, a method for ship heave estimation based on simplified Sage-Husa adaptive filtering is proposed. The heave information measured in a short period of time is analyzed by frequency spectrum, and an approximate heave motion model is established. The heave displacement and heave velocity are used as the state variables, and the heave acceleration is used for the observation to construct a simplified Sage-Husa adaptive filter. The accuracy of the proposed algorithm is verified by three sets of simulations. The results show that compared with Kalman filter, the simplified Sage-Husa adaptive filter has better adaptability to the stochastic system, meets the requirements of ship random motion state measurement, and has higher practical value.
Key words: adaptive filter     heave motion     inertial measurement
0 引　言

1 船舶升沉运动模型

 $s(t) =\sum\limits_{j = 1}^{{N_m}} {{s_j}} (t) =\sum\limits_{j = 1}^{{N_m}} {{A_j}} \cos ({\omega _j}t + {\varphi _j}) 。$ (1)

 $\dot s(t) = \sum\limits_{j = 1}^{Nm} {{{\dot s}_j}(t)} = - \sum\limits_{j = 1}^{Nm} {{A_j}\omega j\sin (\omega jt + \varphi j)}。$ (2)

 $\ddot s{\text{(t) = }}\sum\limits_{j = 1}^{Nm} {{{\ddot s}_j}} (t) = - \sum\limits_{j = 1}^{Nm} {{A_j}} \omega _j^2\cos ({\omega _j}t + {\varphi _j}) 。$ (3)

 $- \omega _j^2A({\omega _j}) = {\ddot A_j}({\omega _j}){\text{ }}(j = 1,2\cdot {N_m})，$ (4)
 $\varphi ({\omega _j}) = \ddot \varphi ({\omega _j}) - \text{π} 。$ (5)

2 简化Sage-Husa自适应滤波算法

Suge-Husa自适应滤波可以在线估计系统状态和噪声统计特性，但其计算复杂引入了更大计算量。为减少计算量本文仅对测量噪声方差阵估计，采用了简化Suge-Husa自适应滤波算法[12]，该算法估计观测噪声方法如下：

 ${R_k} = \frac{1}{k}\sum\limits_{i = 1}^k {\left[ {{Y_i} - {H_i}{X_{i/k}}} \right]} \cdot {\left[ {{Y_i} - {H_i}{X_{i/k}}} \right]^{\rm{T}}}，$ (6)

${X}_{i/i}$ 近似代替 ${X}_{i/k}$ ，改进Sage-Husa结果，提高估值器精度得

 $\begin{split} {Y_i} - {H_i}{X_{i/k}} =& {Y_i} - {H_i}{X_{i/i}} = {Y_i} - {H_i}\left[ {{X_{i/i - 1}} + {K_k}{\varepsilon _i}} \right] =\\ & {\varepsilon _i} - {H_i}{K_i}{\varepsilon _i} 。\end{split}$ (7)

 ${R_k} = \frac{1}{k}\sum\limits_{i = 1}^k {\left[ {I - {H_i}{K_i}} \right]} {\varepsilon _i}\varepsilon _i^{\rm{T}} \cdot {\left[ {I - {H_i}{K_i}} \right]^{\rm{T}}}，$ (8)

 ${{E(}}R{\text{) = }}R - \frac{1}{k}\sum\limits_{i = 1}^k {{H_i}{P_{i/i}}H_i^{\rm{T}}}，$ (9)

 ${R_k} = \frac{1}{k}\sum\limits_{i = 1}^k {\left\{ \begin{gathered} \left[ {I - {H_i}{K_i}} \right]{\varepsilon _i}\varepsilon _i^{\rm{T}} \\ \cdot {\left[ {I - {H_i}{K_i}} \right]^{\rm{T}}} + {H_i}{P_{i/i}}H_i^{\rm{T}} \\ \end{gathered} \right\}}。$ (10)

 ${R_k} = (1 - {d_k}){R_{k - 1}} + {d_k}\left\{ \begin{gathered} \left[ {I - {H_k}{K_{k - 1}}} \right]{\varepsilon _k}\varepsilon _k^{\rm{T}}\times \\ {\left[ {I - {H_k}{K_{k - 1}}} \right]^{\rm{T}}} + {H_k}{P_{k/k}}H_k^{\rm{T}} \\ \end{gathered} \right\}。$ (11)

 ${d_k} = (1 - b)/(1 - {b^{k + 1}}) ，$ (12)

 ${d_k} = (1 - b)/(1 - {b^{k + 1}}) ，$ (13)
 ${\hat p_{k/k - 1}} = {\phi _{k/k - 1}}{\hat p_{k - 1/k - 1}}\phi _{k/k - 1}^{\rm{T}} + {Q_k} ，$ (14)
 ${\hat X_{k/k - 1}} = {\phi _{k/k - 1}}{\hat X_{k - 1/k - 1}} ，$ (15)
 ${\varepsilon _k} = {Y_k} - {H_k}{X_{k/k - 1}} ，$ (16)
 ${R_k} = (1 - {d_k}){R_{k - 1}} + {d_k}\left\{ \begin{gathered} \left[ {I - {H_k}{K_{k - 1}}} \right]{\varepsilon _k}\varepsilon _k^{\rm{T}} \times \\ {\left[ {I - {H_k}{K_{k - 1}}} \right]^{\rm{T}}} + {H_k}{P_{k/k - 1}}H_k^{\rm{T}} \\ \end{gathered} \right\} ，$ (17)
 ${K_k} = {\hat P_{k/k - 1}}H_k^{\rm{T}}{\left[ {{H_k}{{\hat P}_{k/k - 1}}H_k^{\rm{T}} + {R_k}} \right]^{ - 1}}，$ (18)
 ${\hat X_k} = {\hat X_{k/k - 1}} + {K_k}{\varepsilon _k}，$ (19)
 ${\hat P_k} = \left[ {I - {K_k}{H_k}} \right]{\hat P_{k/k - 1}}{\left[ {I - {K_k}{H_k}} \right]^{\rm{T}}} + {K_k}{R_{k - 1}}K_k^{\rm{T}}。$ (20)

 图 1 简化的Suge-Husa自适应滤波迭代流程 Fig. 1 Simplified Suge-Husa adaptive filtering iterative process
3 基于SSHAF的船舶升沉估计方法

1）升沉估计

 ${\dot v^n} = C_b^n{f^b} + {g^n}，$ (21)
 ${v^n} = \left[ \begin{gathered} \dot v_x^n \\ \dot v_y^n \\ \dot v_z^n \\ \end{gathered} \right] 。$ (22)

 图 2 升沉估计流程 Fig. 2 Heave estimation process

2）自适应滤波器设计

 ${a_z} = \sum\limits_{j = 1}^{{N_m}} {{{\ddot s}_j}} (t) + {b_z} + {\eta _z} = - \sum\limits_{j = 1}^{{N_m}} {\omega _j^2{s_j}} (t) + {b_z} + {\eta _z} 。$ (25)

k时刻第j个余弦分波升沉运动状态空间向量为：

 ${X_{j,k}} = [{s_{j,k}}{\text{ }}{\dot s_{j,k}}]，$ (26)

 ${X_k} = {[{X_{1,k}}{\text{ }}{X_{2,k}}{\text{ }} \cdots {\text{ }}{X_{{N_{m,k}}}}{\text{ }}{{\text{b}}_{z,k}}]^{\rm{T}}}，$ (27)

 ${X_k} = \phi {X_{k - 1}} + {w_k} ，$ (28)
 $\phi = \left[ {\begin{array}{*{20}{c}} {{\phi _1}}&0&0&0&0 \\ 0&{{\phi _2}}&0&0&0 \\ \vdots & \vdots & \vdots & \vdots & \vdots \\ 0&0&0&{{\phi _{{N_m}}}}&0 \\ 0&0&0&0&1 \end{array}} \right]。$ (29)

 ${\phi _j} = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t} \\ { - \omega _{j,k}^2t}&1 \end{array}} \right]{\text{ }}j = 1,2 \cdots {N_m}。$ (30)

 $\begin{split} & {{E(}}{w_k}{{) = 0}} ，\\ & {Q_k} = E({w_k}w_k^{\rm{T}})，\end{split}$ (31)

 ${Y_k} = {a_{z,k}} = - \sum\limits_{j = 1}^{{N_m}} {\omega _{j,k}^2{s_{j,k}} + {b_{z,k}} + {\eta _{z,k}}}，$ (32)

${{Y}}_{k}$ k时刻的观测量，即IMU测量的升沉加速度 ${\dot{v}}_{z}^{n}$ 。将观测方程离散化得

 ${Y_k} = H{X_k} + {v_k}，$ (33)
 $H = \left[ {\begin{array}{*{20}{c}} { - \omega _{1,k}^2}&0&{ - \omega _{2,k}^2}&0& \cdots &{ - \omega _{{N_m},k}^2}&0&1 \end{array}} \right]，$ (34)

${v}_{k}$ 为观测噪声， ${R}_{k}$ 为观测噪声协方差矩阵。设观测噪声符合正态分布， ${v}_{k}$ ${R}_{k}$ 满足如下条件：

 $\begin{split} & {{E}}({v_k}) = 0，\\ & {R_k} = {{E}}({v_k}v_k^{\rm{T}}) 。\end{split}$ (35)
4 仿真验证

 图 3 仿真1工况下算法对比 Fig. 3 Comparison of algorithms in simulation 1

 图 4 仿真2工况下算法对比 Fig. 4 Comparison of algorithms in simulation 2

 图 5 仿真3工况下算法对比 Fig. 5 Comparison of algorithms in simulation 3

5 结　语

3组仿真对比表明，简化Sage-Husa自适应滤波与传统卡尔曼滤波相比，对系统模型建立和测量噪声的依赖性降低，更加适用于实际中随机运动的船舶升沉估计。

 [1] RICHTER M., SCHNEIDER K., WALSER D., et al. Real-Time Heave Motion Estimation using Adaptive Filtering Techniques[J]. IFAC Proceedings Volumes, 2014, 47(3): 10119−10126. [2] MATHEW J , SGARIOTO D , DUFFY J , et al. An Experimental Study of Ship Motions During Replenishment at Sea Operations Between a Supply Vessel and a Landing Helicopter Dock[J]. The International Journal of Maritime Engineering, 2018, 160(A2): 97−108. [3] QUAN, WEICAI, ZHANG, et al. The nonlinear finite element modeling and performance analysis of the passive heave compensation system for the deep-sea tethered ROVs[J]. Ocean engineering, 2016. [4] 严恭敏, 苏幸君, 翁浚, 秦永元. 基于惯导和无时延滤波器的舰船升沉测量[J]. 导航定位学报, 2016, 4(2): 91-93+107. DOI:10.16547/j.cnki.10-1096.20160219 [5] 黄卫权, 李智超, 卢曼曼. 基于BMFLC算法的舰船升沉测量方法[J]. 系统工程与电子技术, 2017, 39(12): 2790-2795. DOI:10.3969/j.issn.1001-506X.2017.12.23 [6] KÜCHLER S., EBERHARTER J. K., LANGER K., et al. Heave motion estimation of a vessel using acceleration measurements[J]. IFAC Proceedings Volumes, 2011, 44(1): [7] 袁广民, 李晓莹, 常洪龙, 苑伟政. MEMS陀螺随机误差补偿在提高姿态参照系统精度中的应用[J]. 西北工业大学学报, 2008, 26(6): 777-781. DOI:10.3969/j.issn.1000-2758.2008.06.024 [8] AGGARWAL P, NIU X, EL-SHEIMY N, et al. Cost-effective testing and calibration of low cost MEMS sensors for integrated positioning, navigation and mapping systems. [9] 李果, 刘旭焱, 马建晓. Suge-Husa自适应滤波简化算法[J]. 计算机工程与设计, 2019, 40(5): 1360-1364. DOI:10.16208/j.issn1000-7024.2019.05.030 [10] 李智超. 基于惯导系统的舰船升沉测量技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2018. [11] 金文标, 蒲鹏飞. 基于频谱的海浪实时模拟[J]. 重庆邮电大学学报:自然科学版, 2009(3): 5. [12] 鲁平, 赵龙, 陈哲. 改进的Sage-Husa自适应滤波及其应用[J]. 系统仿真学报, 2007(15): 3503-3505. DOI:10.3969/j.issn.1004-731X.2007.15.034 [13] 邸瑛琳. 基于惯性导航的升沉测量系统研究[D]. 哈尔滨: 哈尔滨工业大学, 2020.