﻿ 基于扩张状态卡尔曼原理的海浪滤波算法
 舰船科学技术  2023, Vol. 45 Issue (7): 62-65    DOI: 10.3404/j.issn.1672-7649.2023.07.013 PDF

1. 海军驻九江地区军事代表室，江西 九江 332007;
2. 天津航海仪器研究所九江分部，天津 300131

Study on wave filtering algorithm based on extended state Kalman Theory
ZHOU Jia-yu1, HAN Jun-qing2, LI Wei2, MENG Fan-bin2
1. Navy Military Representative Office in Jiujiang Area, Jiujiang 332007, China;
2. Jiujiang Division, Tianjin Navigation Instruments Research Institute, Tianjin 300131, China
Abstract: To solve the problem of invalid rudder in ship control system under wave disturbance and static error of observer under wind and stream disturbance, a wave filtering algorithm based on extended state Kalman theory was proposed. Firstly, the 1-order Nomoto model of the ship was discretized, and the 4-order state equation for wave filtering was established. Then, the 5-order state equation was established based on the comprehensive disturbance term composed of environmental disturbance and unmodeled dynamic disturbance in Nomoto model, and the wave filter was designed based on kalman filter algorithm, to solve the wave filtering and observer's error. The simulation results showed that the wave filtering algorithm proposed in this paper can effectively filter the high frequency signal of the ship heading and correctly estimate th ship's motion state, and reduce the problem of invalid rudder significantly when the ship is sailing.
Key words: nomoto model     wave filter     Kalman filter     invalid rudder
0 引　言

1 船舶Nomoto模型与参数辨识

 $\left\{ \begin{gathered} \dot x = u\cos \psi - v\sin \psi ，\\ \dot y = u\sin \psi + v\cos \psi ，\\ \dot \psi = r ，\\ \dot u = \frac{{\left( {m + {m_y}} \right)vr + {X_H} + {X_P} + {X_R} + {X_{disturb}}}}{{m + {m_x}}} ，\\ \dot v = \frac{{\left( {m + {m_x}} \right)ur + {Y_H} + {Y_P} + {Y_R} + {Y_{disturb}}}}{{m + {m_y}}} ，\\ \dot r = \frac{{{N_H} + {N_P} + {N_R} + {N_{disturb}}}}{{{I_{zz}} + {J_{zz}}}}。\\ \end{gathered} \right.$ (1)

 $T\dot r + r = K\delta + d\;。$ (2)

 $\begin{split} & \frac{{T\dot \psi (k) - T\dot \psi (k - 1)}}{{T{}_s}} + \frac{{\psi (k) - \psi (k - 1)}}{{{T_s}}} = k\delta (k - 1) ，\\ & \frac{{T\psi (k) - T\psi (k - 1)}}{{{T_s}}} - \frac{{T\psi (k - 1) - T\psi (k - 2)}}{{{T_s}}}+\\ & \psi (k) - \psi (k - 1) = {T_s}K\delta (k - 1) ，\\ & T\psi (k) - T\psi (k - 1) - T\psi (k - 1) + T\psi (k - 2)+\\&{T_s}\psi (k) - {T_s}\psi (k - 1) = T_s^2K\delta (k - 1) ，\\ & (T + {T_s})\psi (k) - (2T + {T_s})\psi (k - 1) + T\psi (k - 2) = T_s^2K\delta (k - 1) ，\\ &\psi (k) = \frac{{2T + {T_s}}}{{(T + {T_s})}}\psi (k - 1) - \frac{T}{{(T + {T_s})}}\psi (k - 2) +\\ & \frac{{T_s^2}}{{(T + {T_s})}}K\delta (k - 1) 。\\[-15pt] \end{split}$ (3)

 $\psi (k) = {\phi ^{\rm{T}}}(k - {\text{1}})\theta \;。$ (4)

 $\begin{gathered} {\phi ^{\rm{T}}}(k - 1) = \left[ {\psi (k - 1),\psi (k - 2),\delta (k - 1)} \right]，\\ {\theta ^{\rm{T}}} = \left[ {{\theta _1},{\theta _2},{\theta _3}} \right] \;。\\ \end{gathered}$

 $\left\{ \begin{gathered} K\left( k \right) = \frac{{P\left( {k - 1} \right)\phi \left( {k - 1} \right)}}{{\lambda + {\phi ^T}\left( {k - 1} \right)P\left( {k - 1} \right)\phi \left( {k - 1} \right)}} ，\\ \theta \left( k \right) = \theta \left( {k - 1} \right) + K\left( k \right)\left[ {\psi \left( k \right) - {\phi ^T}\left( k \right)\theta \left( {k - 1} \right)} \right]，\\ P\left( k \right) = \frac{1}{\lambda }\left[ {I - K\left( k \right){\phi ^T}\left( {k - 1} \right)} \right]P\left( {k - 1} \right) 。\end{gathered} \right.$ (5)

 $\begin{split} &T=\frac{{\theta }_{1}-1}{2-{\theta }_{1}}{T}_{s}，\\ & K=\frac{{\theta }_{3}\left(T+{T}_{s}\right)}{{T}_{s}^{2}}。\end{split}$ (6)
2 海浪滤波算法设计

 ${\psi _H}(s) = \frac{{{K_\omega }s}}{{{s^2} + 2\zeta {\omega _n}s + {\omega _n}^2}}{w_H}(s) \;。$ (7)

 $\begin{split} &{{\dot \xi }_H}{\text{ = }}{\psi _H} ，\\ &{{\dot \psi }_H}{\text{ = }} - 2\zeta {\omega _n}{\psi _H} - {\omega _n}^2{\xi _H} + {K_w}{w_H} \; 。\end{split}$ (8)

 $\psi = {\psi _L} + {\psi _H} + {\upsilon _H}，$ (9)

 $\begin{split} \left[ {\begin{array}{*{20}{c}} {{{\dot \psi }_L}} \\ {{{\dot r}_L}} \\ {{{\dot \xi }_H}} \\ {{{\dot \psi }_H}} \end{array}} \right] =\;& \left[ {\begin{array}{*{20}{c}} 0&1&0&0 \\ 0&{\dfrac{{ - 1}}{T}}&0&0 \\ 0&0&0&1 \\ 0&0&{-2\zeta {\omega _n}}&{{\omega _n}^2} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\psi _L}} \\ {{r_L}} \\ {{\xi _H}} \\ {{\psi _H}} \end{array}} \right]+\\ & \left[ {\begin{array}{*{20}{c}} 0 \\ {\dfrac{K}{T}} \\ 0 \\ 0 \end{array}} \right]\delta + \left[ {\begin{array}{*{20}{c}} 0 \\ 0 \\ 0 \\ {{K_w}} \end{array}} \right]{w_H} \; 。\end{split}$ (10)

 $\begin{split} \begin{bmatrix} {{{\dot \psi }_L}} \\ {{{\dot r}_L}} \\ {{{\dot \xi }_H}} \\ {{\dot \psi }_H} \\ {\dot d} \end{bmatrix} = & \underbrace {\begin{bmatrix} 0&1&0&0&0 \\ 0&{ - 1/T}&0&0&1 \\ 0&0&0&1&0 \\ 0&0&{{{ - }}2\zeta {\omega _n}}&{{\omega _n}^2}&0 \\ 0&0&0&0&0 \end{bmatrix}}_A\underbrace{\begin{bmatrix} {{\psi _L}} \\ {{r_L}} \\ {{\xi _H}} \\ {\psi _H} \\ d \end{bmatrix}}_X + \underbrace{\begin{bmatrix} 0 \\ {K/T} \\ 0 \\ 0 \\ 0 \end{bmatrix}}_B\underbrace \delta _U ，\\ \underbrace \psi _Z =\; & \underbrace {\left[ {\begin{array}{*{20}{c}} 1&0&0&1&0 \end{array}} \right]}_CX 。\\[-15pt] \end{split}$ (11)

 $\left\{ \begin{gathered} {X_k} = {A_d}{X_{k - 1}} + {B_d}{U_{k - 1}} ，\\ {Z_k} = {C_d}{X_k} 。\\ \end{gathered} \right.$ (12)

1）确定过程噪声方差Q、测量噪声方差R、初始状态X0以及初始协方差矩阵P0。

2）根据前一时刻的状态估计值推算当前时刻的状态先验估计值，如下式：

 ${X_{k - 1|k}} = {A_d}{X_{k - 1}} + {B_d}{U_{k - 1}} ，$ (13)
 ${P_{k - 1|k}} = {A_d}{P_{k - 1}}A_d^T + Q \;。$ (14)

3）计算卡尔曼增益，如下式：

 ${K_k} = {P_{k - 1|k}}C_d^T{\left( {{C_d}{P_{k - 1|k}}C_d^T + R} \right)^{ - 1}}\;。$ (15)

4）根据测量值与先验估计值计算状态后验估计值与误差协方差后验估计值，如下式：

 ${X_k} = {X_{k - 1|k}} + {K_k}\left( {{Z_k} - {C_d}{X_{k - 1|k}}} \right)，$ (16)
 ${P_k} = \left( {I - {K_k}{C_d}} \right){P_{k - 1|k}} \;。$ (17)
3 仿真验证

 图 1 Z型试验仿真结果 Fig. 1 Simulation result of Z−type test

 图 2 K，T参数辨识结果 Fig. 2 Identification results of K and T parameters

KT参数的辨识结果中可以看出，2个参数在50 s后趋于稳定。为了准确的求解KT参数，对100 s后的数据求取平均值，最终求解出的KT参数为：

 $K=0.1249，T=2.0187 。$

 图 3 航向观测结果对比图 Fig. 3 Comparison of course observation results

 图 4 航向角速度观测结果对比图 Fig. 4 Comparison of course velocity observation results

 图 5 航向角仿真曲线 Fig. 5 Simulation curve of course

 图 6 舵角仿真曲线 Fig. 6 Simulation result of rudder

4 结　语

 [1] 彭秀艳, 胡忠辉. 带有海浪滤波器的船舶航向反步自适应输出反馈控制[J]. 控制理论与应用, 2013, 30(7): 863-868. DOI:10.7641/CTA.2013.21104 [2] 王元慧, 边信黔, 施小成. 海浪噪声的仿真与滤波[J]. 计算机仿真, 2007, 24(4): 318-321. DOI:10.3969/j.issn.1006-9348.2007.04.086 [3] 樊冀生, 袁伟, 李文娟. 船舶动力定位自适应滤波器设计[J]. 舰船科学技术, 2017, 39(10): 79-83. DOI:10.3404/j.issn.1672-7649.2017.10.015 [4] 李杨, 徐雪峰, 赵光. 基于扩展状态观测器的海浪滤波算法及仿真[J]. 中国舰船研究, 2020, 15(1): 1-5. DOI:10.19693/j.issn.1673-3185.01651 [5] DO K D, JIANG Z P, PAN J. Global robust adaptive path fllowing of underactuated ships[J]. Automatica, 2006, 42(10): 1713-1722. DOI:10.1016/j.automatica.2006.04.026 [6] 李奕. 无人艇航向运动控制技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2018. [7] 周怡, 王俊雄. 自适应粒子群算法在AUV水动力参数识别中的应用[J]. 舰船科学技术, 2021, 43(11): 90-95. [8] FOSSEN T I, PEREZ T. Kalman filtering for positioning and heading control of ships and offshore rigs[J]. IEEE Control Systems, 2009, 29(6): 32-46. DOI:10.1109/MCS.2009.934408 [9] FOSSEN T I. Guidance and control of ocean vehicles[M]. Chichester: John Wiley & Sons, 1994.