0 引　言

船舶动力定位（dynamic positioning，DP）系统^[1]是一种能够适用于复杂海况条件下，自动保持船舶位置、航向或者跟踪指定航迹的运动控制技术。动力定位船^[2]在海上运动时受风、浪、流等环境载荷作用，其中波浪载荷主要包括一阶波浪载荷和二阶波浪载荷，进而使船体产生一阶波浪运动和二阶漂移运动。一阶高频运动是零均值的往复振荡运动，对动力定位性能影响较大。在传感器的测量运动信号输入DP控制器之前^[3]，必须将信号中的一阶高频运动和测量白噪声滤除。

针对船舶波浪运动滤波和控制问题，国内外的许多学者都做过相关研究。Fossen等^[4]对船舶受环境扰动情况时运动控制系统展开研究。Hassani等^[5]提出一个由递归优化过程组成的自适应波浪滤波系统。李杨等^[6]在海浪干扰主频率辨识方法的基础上,设计扩展状态观测器及二次最优控制器。邓英杰等^[7]提出一种动力定位的状态观测饱和控制算法。然而Kalman滤波由于参数较多和强耦合性，未能在DP控制中很好应用。

本文针对船舶动力定位系统的一阶波浪运动滤波与控制问题，建立船舶DPS系统非线性动力学模型和波浪运动的数学模型，对船舶DPS的非线性数学模型进行线性化理处；建立一种基于位置和航向输出的DPS状态观测器，研究如何利用基于连续Kalman滤波技术解决动力定位系统三自由度解耦滤波问题，并讨论DPS滤波器的噪声协方差矩阵参数的选择方法；构建一种包含状态估计和运动控制器的DPS系统仿真模型，并在Simulink中进行了动力定位仿真分析，进而探讨非线性无源滤波和Kalman滤波方法的滤波效果和船舶动力定位性能。

1 船舶动力学与一阶波浪运动建模 1.1 船舶动力学及运动学模型

在海洋等外部环境的扰动下，船舶运动会产生6个自由度运动。对于动力定位系统来说，一般只控制水面上的3个自由度（纵荡、横荡、首摇）运动。定义在北东坐标系下^[8]船舶三自由度向量 ${\boldsymbol{\eta }} = {\left[ {\begin{array}{*{20}{c}} x\ \ y \ \ \psi \end{array}} \right]^{\text T} }$ ，船体坐标系下的速度向量 ${\boldsymbol{v}}{\text{ = }}{\left[ {\begin{array}{*{20}{c}} u\ \ v \ \ r \end{array}} \right]^\text T}$ ，船舶运动在两坐标系的转换关系为 ${\boldsymbol{\dot \eta }} = {\boldsymbol{R}}(\psi ){\boldsymbol{v}}$ 。

船舶在海面上的运动可分为由风、海流及二阶波浪力等引起的船舶低频漂移运动和由一阶波浪力引起的船舶高频运动。船舶真实的运动是二者运动的叠加，考虑到传感器的测量白噪声干扰，DP船舶三自由度低频非线性模型可以写为：

$ \begin{split} \left\{ \begin{aligned} & {{\boldsymbol{\dot \eta }}= {\boldsymbol{R}}(\psi ){\boldsymbol{v}}}\;，\\ & {{\boldsymbol{M\dot v}} = - {\boldsymbol{Dv}} + {{\boldsymbol{R}}^{\text{T}}}{\text{(}}\psi {\text{)}}{\boldsymbol{b}} + {\boldsymbol{\tau }} + {{\boldsymbol{\tau }}_{\text{w}}} + {{\boldsymbol{\omega }}_{\boldsymbol{v}}}}\;，\\ & {\boldsymbol{\dot b}} = - {{\boldsymbol{T}}^{{{ - 1}}}}{\boldsymbol{b}} + {{\boldsymbol{\omega }}_b}{\text{(}}{\boldsymbol{\dot b}} = {{\boldsymbol{\omega }}_b}{\text{)}}\;。\end{aligned} \right. \end{split}$

(1)

式中： ${\boldsymbol{M}} \in {R^{3 \times 3}}$ 为运动系统的惯性矩阵； ${\boldsymbol{D}} \in {R^{3 \times 3}}$ 为阻尼系数的矩阵； ${\boldsymbol{\tau }} \in {R^{3 \times 1}}$ 为船舶推进系统输出的推力矢量矩阵； ${{\boldsymbol{\tau }}_{\text{w}}} \in {R^{3 \times 1}}$ 为风前馈； ${\boldsymbol{T}} = {\rm{diag}}\{ {T_1},{T_2},{T_3}\}$ 为含有正偏差时间常数的对角矩阵； ${\boldsymbol{b}}$ 为偏差向量，是由二阶波浪漂移力、洋流和未建模的动力学作用而产生的一种漂移力； ${{\boldsymbol{\omega }}_v}$ 和 ${{\boldsymbol{\omega }}_b}$ 为零均值高斯白噪声信号。

1.2 一阶海浪干扰模型

一阶海浪干扰力作用到船体上表现为和波高成线性关系且同频的波浪运动，DP船舶所受的高频往复运动是一阶波浪干扰力引起的，且在横荡、纵荡和首摇3个自由度上互不影响，可看成是单独的运动，都看作是一个附加阻尼项的二阶谐波振荡器^[9]：

$ {h} (s) = \frac{{{K_\omega }s}}{{{s^2} + 2\lambda {\omega _0}s + \omega _0^2}}。$

(2)

式中： $ \lambda $ 为取值范围在0.05~0.3相对阻尼系数； $ \sigma $ 为与波浪的强度相关的常数值； $ {\omega _0} $ 为与波浪有义波高关联的波浪响应主导频率。将上式转换为时间域中表示，获得线性状态空间模型，并定义 ${{\boldsymbol{\dot x}}_{{\boldsymbol{\omega }}1}} = {{\boldsymbol{x}}_{{\boldsymbol{\omega }}2}}$ 和 ${{\boldsymbol{x}}_{{\boldsymbol{\omega }}2}} = {{\boldsymbol{y}}_{\boldsymbol{\omega }}}$ 作为状态变量的域，则状态空间模型为：

$ \begin{split} \left\{ {\begin{aligned} &{{\boldsymbol{\dot \xi }} = {{\boldsymbol{A}}_{\boldsymbol{\omega }}}{\boldsymbol{\xi }} + {{\boldsymbol{E}}_{\boldsymbol{\omega }}}{\boldsymbol{\omega }}} ，\\ &{{{\boldsymbol{y}}_{\boldsymbol{\omega }}} = {\boldsymbol{C}}_{\boldsymbol{\omega }}^{\text{T}}{\boldsymbol{\xi }}} 。\end{aligned}} \right. \end{split} $

(3)

式中： ${\boldsymbol{\xi }}$ 为状态向量； ${{\boldsymbol{A}}_{\boldsymbol{\omega}} }$ ， ${{\boldsymbol{E}}_{\boldsymbol{\omega}} }$ 和 ${\boldsymbol{C}}_{\boldsymbol{\omega}}^{\text{T}}$ 为描述海洋状态的常数矩阵； ${{\boldsymbol{y}}_{\boldsymbol{\omega }}}$ 为波浪运动的线性响应； $ {\mathbf{\omega }} $ 为均值为0的高斯白噪声信号。上式可以变为：

$ \begin{split} \left\{ {\begin{aligned} &{\left[ {\begin{aligned} {{{{\boldsymbol{\dot x}}}_{{\boldsymbol{\omega }}1}}} \\ {{{{\boldsymbol{\dot x}}}_{{\boldsymbol{\omega }}2}}} \end{aligned}} \right] = \left[ {\begin{aligned} 0&\;1 \\ { - \omega _0^2}&{ - 2\lambda {\omega _0}} \end{aligned}} \right]\left[ {\begin{aligned} {{{\boldsymbol{x}}_{{\boldsymbol{\omega }}1}}} \\ {{{\boldsymbol{x}}_{{\boldsymbol{\omega }}2}}} \end{aligned}} \right] + \left[ {\begin{aligned} 0 \\ {{K_\omega }} \end{aligned}} \right]{\boldsymbol{\omega }}} \;，\\ &{{{\boldsymbol{y}}_{\boldsymbol{\omega }}} = \begin{aligned} [ 0&\;1] \end{aligned} \left[ {\begin{aligned} {{{\boldsymbol{x}}_{{\boldsymbol{\omega }}1}}} \\ {{{\boldsymbol{x}}_{{\boldsymbol{\omega }}2}}} \end{aligned}} \right]} \;\text{。} \end{aligned}} \right. \end{split} $

(4)

2 非线性无源估计滤波器的设计 2.1 非线性无源估计滤波器的观测方程

DP船舶测量系统测量出的船舶位置以及首摇角度的信息是带有噪声干扰的^[10]，因此选用的系统测量模型为：

$ {\boldsymbol{y}} = {\boldsymbol{\eta }} + {{\boldsymbol{\eta }}_{\boldsymbol{\omega }}} + {{\boldsymbol{\omega }}_{\boldsymbol{y}}} \;，$

(5)

在非线性无源估计滤波器中不再采用零转首率的假设，增加2个新的假设。

假设1　忽略均值为零的高斯白噪声。

假设2　即使在极端海况下，船舶对一阶波浪力响应后的首摇角度不超过5°，得到非线性无源估计滤波器的观测方程为：

$ \begin{split} \left\{ \begin{aligned} &{{\boldsymbol{\dot {\hat \xi} }} = {{\boldsymbol{A}}_{\boldsymbol{\omega }}}{\boldsymbol{\hat \xi }} + {{\boldsymbol{K}}_1}{\boldsymbol{\tilde y}}} \;，\\ &{{\boldsymbol{\dot {\hat \eta} }} = {\boldsymbol{R}}{\text{(}}\psi {\text{)}}{\boldsymbol{\hat v}} + {{\boldsymbol{K}}_2}{\boldsymbol{\tilde y}}}\;，\\ &{{\boldsymbol{M\dot {\hat v}}} = - {\boldsymbol{D\hat v}} + {{\boldsymbol{R}}^{\text{T}}}{\text{(}}\psi {\text{)}}{\boldsymbol{\hat b}} + {\boldsymbol{\tau }} + {{\boldsymbol{\tau }}_{\text{w}}} + {{\boldsymbol{R}}^{\text{T}}}(\psi ){{\boldsymbol{K}}_3}{\boldsymbol{\tilde y}}}\;，\\ &{{\boldsymbol{\dot {\hat b}}} = - {{\boldsymbol{T}}^{{{ - 1}}}}{\boldsymbol{\hat b}} + {{\boldsymbol{K}}_4}{\boldsymbol{\tilde y}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} ({\boldsymbol{\dot {\hat b}}} = {{\boldsymbol{K}}_4}{\boldsymbol{\tilde y}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} )}\;，\\ &{{\boldsymbol{\hat y}} = {\boldsymbol{\hat \eta }} + {{\boldsymbol{C}}_{\boldsymbol{\omega }}}{\boldsymbol{\hat \xi }}} \;。\end{aligned} \right. \end{split}$

(6)

式中： ${{\boldsymbol{A}}_{\boldsymbol{\omega }}}$ 是描述海洋状态的常数矩阵； ${\boldsymbol{\tilde y}} = {\boldsymbol{y}} - {\boldsymbol{\hat y}}$ 为输出的估计误差。

2.2 观测器的参数设定方法

为保证式（6）满足KYP引理，使得系统是严格无源系统，设计增益矩阵结构如下：

$\begin{split} &{{\boldsymbol{K}}}_{1}=\left[\begin{array}{ccc} {k}_{11}&0&0\\ 0&{k}_{12}&0\\ 0& 0&{k}_{13}\\ {k}_{21}&0&0\\ 0&{k}_{22}&0\\ 0&0&{k}_{23}\end{array}\right]\text{，} {{\boldsymbol{K}}}_{2}\text=\left[\begin{array}{ccc} {k}_{31}&0&0\\ 0&{k}_{32}&0\\ 0&0&{k}_{33}\end{array}\right]\text{，}\\ &{{\boldsymbol{K}}}_{3}\text=\left[\begin{array}{ccc} {k}_{41}&0&0\\ 0&{k}_{42}&0\\ 0&0&{k}_{43}\end{array}\right]\text{，} {{\boldsymbol{K}}}_{4}\text=\left[\begin{array}{ccc} {k}_{51}&0&0\\ 0&{k}_{52}&0\\ 0&0&{k}_{53}\end{array}\right]\;。\end{split} $

(7)

非线性无源估计滤波器增益矩阵中的参数如下：

$ \begin{split} \left\{ {\begin{aligned} & {{k_{1i}} = \dfrac{{ - 2{\omega _{ci}}({\zeta _{ni}} - {\zeta _i})}}{{{\omega _{oi}}}}}\;，\\ & {{k_{2i}} = 2{\omega _{oi}}({\zeta _{ni}} - {\zeta _i})}\;，\\ & {{k_{3i}} = {\omega _{ci}}} \;。\end{aligned}} \right. \end{split} $

(8)

式中： $ {\omega _{ci}} $ 为陷波滤波器的截止频率； $ {\varsigma _{ni}} $ 为决定陷波的性能的对阻尼率； $ {\varsigma _i} $ 为波谱的相对阻尼率； $ {\omega _{oi}} $ 为波谱的峰值频率。

滤波器增益选取理想情况下需要考虑到波谱的峰值频率，为了满足正实引理，重要的是3个解耦函数都有一个−90°相位滞后，系统参数必须满足：

$ \frac{1}{{{T_i}}} \ll \frac{{{k_{5i}}}}{{{k_{4i}}}} < {\omega _{oi}} < {\omega _{ci}} 。$

(9)

式中： ${T_i}$ 为定义偏差估计器的积分时间常数。

3 连续型Kalman滤波器的设计 3.1 连续型Kalman滤波算法

建立线性连续时间系统的状态空间方程^[11]：

$ {\boldsymbol{\dot x}} = {\boldsymbol{Ax}} + {\boldsymbol{Bu}} + {\boldsymbol{E\omega }} 。$

(10)

式中：过程噪声 ${\boldsymbol{\omega }}$ 被假定为具有协方差矩阵且均值为零的高斯白噪声过程，其协方差矩阵 ${\boldsymbol{Q}} = {{\boldsymbol{Q}}^{\text{T}}} > 0$ ，在一维情况下， ${\boldsymbol{Q}}$ 对应于标准差的平方，即 ${{\boldsymbol{\sigma }}^2}$ 。

测量方程（传感器系统）为：

$ {\boldsymbol{y}} = {\boldsymbol{Hx}} + {\boldsymbol{\upsilon }} 。$

(11)

式中：测量噪声 ${\boldsymbol{\upsilon }}$ 假定为具有协方差矩阵且均值为零的高斯白噪声过程，其协方差矩阵 ${\boldsymbol{R}} = {{\boldsymbol{R}}^{\text{T}}} > 0$ 。如果上述系统方程是可观测的，状态向量 ${\boldsymbol{x}} \in {{\text{R}}^n}$ 可以通过测量值 ${\boldsymbol{y}} \in {{{R}}^n}$ 和控制输入值 ${\boldsymbol{u}} \in {{{R}}^n}$ 重新递推求解计算的。

连续时间Kalman滤波算法及噪声方差阵如下：

$ \begin{split} \left\{ {\begin{aligned} &{{\boldsymbol{Q}}(t) = {{\boldsymbol{Q}}^{\text{T}}}(t) > 0} ，\\ &{{\boldsymbol{R}}(t) = {{\boldsymbol{R}}^{\text{T}}}(t) > 0} ，\\ &{{\boldsymbol{\hat x}}(0) = {{\boldsymbol{x}}_0}} \end{aligned}} \right. \end{split}$

(12)

初始条件：

$ P(0) = E[(x(0) - \hat x(0)){(x(0) - \hat x(0))^{\rm{T}}}] = {{\boldsymbol{P}}_0} 。$

(13)

卡尔曼增益矩阵迭代式：

$ K(\text t) = {\boldsymbol{P}}(t){{\boldsymbol{H}}^{\text{T}}}(t){{\boldsymbol{R}}^{ - 1}}(t) ，$

(14)

状态估计迭代式：

$ \dot {\hat x}(\text t) = {\boldsymbol{A}}(t){\boldsymbol{\hat x}}(t) + {\boldsymbol{B}}(t){\boldsymbol{u}}(t) + {\boldsymbol{K}}(t)[{\boldsymbol{y}}(t) - {\boldsymbol{H}}(t){\boldsymbol{\hat x}}(t)] \;，$

(15)

误差协方差迭代式：

$\begin{split} \dot P(\text t) = &{\boldsymbol{A}}(t){\boldsymbol{P}}(t) + {\boldsymbol{P}}(t){{\boldsymbol{A}}^{\text{T}}}(t) + {\boldsymbol{E}}(t){\boldsymbol{Q}}(t){\boldsymbol{E}}(t) -\\ &{\boldsymbol{P}}(t){{\boldsymbol{H}}^{\text{T}}}(t){{\boldsymbol{R}}^{ - 1}}(t){\boldsymbol{H}}(t){\boldsymbol{P}}(t)。\end{split}$

(16)

3.2 线性DP状态空间模型的设计

选用的线性DP模型设计结果模型为15阶状态空间模型^[12]：

$ \begin{split} \left\{ {\begin{aligned} &{{\boldsymbol{\dot x}} = {\boldsymbol{Ax}} + {\boldsymbol{Bu}} + {\boldsymbol{E\omega }}} ，\\ &{{\boldsymbol{y}} = {\boldsymbol{Hx}} + {\boldsymbol{\upsilon }}} 。\end{aligned}} \right. \end{split} $

(17)

式中： ${\boldsymbol{x}} = {[{{\boldsymbol{\xi }}^{\text{T}}},{\boldsymbol{\eta }}_p^{\text{T}},{{\boldsymbol{v}}^{\text{T}}},{\boldsymbol{b}}_p^{\text{T}}]^{\text{T}}} \in {{{R}}^{15}}$ 是状态变量； ${\boldsymbol{u}} \in {{{R}}^p}$ $(p \geqslant 3)$ 控制变量； ${\boldsymbol{\omega }} = {[{\boldsymbol{\omega }}_1^{\text{T}},{\boldsymbol{\omega }}_2^{\text{T}},{\boldsymbol{\omega }}_3^{\text{T}}]^{\text{T}}} \in {{{R}}^9}$ 代表过程噪声变量； ${\boldsymbol{\upsilon }} \in {{{R}}^3}$ 是测量噪声变量。

系统矩阵为：

$\begin{split}&{\boldsymbol{A}}=\left[\begin{array}{cccc}{{\boldsymbol{A}}}_{\omega }& {0}_{6\times 3}& {0}_{6\times 3}& {0}_{6\times 3}\\ {0}_{3\times 6}& {0}_{3\times 3}& {{\boldsymbol{I}}}_{3\times 3}& {0}_{3\times 3}\\ {0}_{3\times 6}& {0}_{3\times 3}& {\boldsymbol{-M}}^{-1}{\boldsymbol{D}}& {{\boldsymbol{M}}}^{-1}\\ {0}_{3\times 6}& {0}_{3\times 3}& {0}_{3\times 3}& {\boldsymbol{-T}}^{-1}\end{array}\right]\text{，}\\ &{\boldsymbol{B}}=\left[\begin{array}{c}{0}_{6\times p}\\ {0}_{3\times p}\\ {{\boldsymbol{M}}}^{-1}{{\boldsymbol{B}}}_{u}\\ {0}_{3\times p}\end{array}\right]\text{，}{\boldsymbol{E}}=\left[\begin{array}{ccc}{{\boldsymbol{E}}}_{\omega }& {0}_{6\times 3}& {0}_{6\times 3}\\ {0}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}\\ {0}_{3\times 3}& {{\boldsymbol{M}}}^{-1}& {0}_{3\times 3}\\ {0}_{3\times 3}& {0}_{3\times 3}& {{\boldsymbol{I}}}_{3\times 3}\end{array}\right]。\end{split} $

(18)

3.3 协方差矩阵的设定方法

在连续时间Kalman滤波器中：

$ \begin{split} \left\{ \begin{aligned} &{\boldsymbol{\hat{\dot{x}}}}={\boldsymbol{A\hat{x}}}+{\boldsymbol{Bu}}+{\underset{K}{\underbrace{{\boldsymbol{P}}{{\boldsymbol{H}}}^{\text{T}}{\boldsymbol{R}}}}}^{-1}\left({\boldsymbol{y}}-{\boldsymbol{H\hat{x}}}\right)，\\ &{\boldsymbol{\dot{P}}}={\boldsymbol{AP}}+{\boldsymbol{P{A}}}^{\text{T}}+{\boldsymbol{EQ{E}}}^{\text{T}}-{\boldsymbol{P{H}}}^{\text{T}}{{\boldsymbol{R}}}^{-1}{\boldsymbol{HP}}。\end{aligned} \right. \end{split}$

(19)

协方差矩阵 ${\boldsymbol{Q}} = {{\boldsymbol{Q}}^{\text{T}}} \in {{\text{R}}^{9 \times 9}}$ 和 ${\boldsymbol{R}} = {{\boldsymbol{R}}^{\text{T}}} \in {{\text{R}}^{3 \times 3}}$ 必须由使用者指定，测量协方差矩阵 ${\boldsymbol{R}}$ 的方法可以使用下述公式：

$ {\boldsymbol{R}} = {\rm{diag}}\{ {\sigma} _{{\text{v}}1}^2,{\sigma} _{{\text{v}}1}^2, \cdots ,{\sigma} _{{\text{vp}}}^2\}。$

(20)

式中： $ \sigma _{{\text{vi}}}^2 $ 代表第i个传感器的测量噪声的协方差，通常以采集船舶在静水中的位姿数据等方法来获得数据。

协方差矩阵 ${\boldsymbol{Q}}$ 具体的结构形式，要充分考虑船舶在实际航行过程会受到不同海况，是一个强非线性过程具有不确定性的模型。在仿真模拟时通过不断地试错得到，确定将主对角线的数都设成正可调参数。最终将这个协方差矩阵选为对角矩阵 ${\boldsymbol{Q}} = {\rm{diag}}\{ {Q_1},{Q_2},{Q_3}\}$ 。

矩阵 ${{\boldsymbol{Q}}_1} \in {{{R}}^{3 \times 3}}$ 代表噪声 $ {\mathbf{\omega }} $ 的协方差，取值与船舶的波浪响应参数有关。可读取船舶波浪运动的历史数据来确定这一噪声滤波器的取值设计，矩阵 ${{\boldsymbol{Q}}_2}$ 和 ${{\boldsymbol{Q}}_3}$ 也设置为对角矩阵， ${{\boldsymbol{Q}}_2} \in {{{R}}^{3 \times 3}}$ 代表在位置测量过程中噪声方差的部分，是噪声 ${{\boldsymbol{\omega }}_v}$ 的协方差； ${{\boldsymbol{Q}}_3} \in {{{R}}^{3 \times 3}}$ 一方面可以给其他协方差的取值提供平衡，也代表着环境建模时具有一定的不确定性，是噪声 ${{\boldsymbol{\omega }}_b}$ 的协方差。

3.4 三自由度解耦滤波方法

通过以上的推导计算，可得到一个维度为 $ 15 \times 1 $ 的Kalman增益矩阵 $ K $ 。考虑到DP系统中滤波器的实时性要求，不能直接将增益与 ${\boldsymbol{y}}(t) - {\boldsymbol{H}}(t){\boldsymbol{\hat x}}(t)$ 简单相乘。一方面，会使得协方差矩阵的形式不在清晰；另一方面，高维度的矩阵运算所需计算时间较长，也容易产生不可逆的矩阵。

综上，可对DP船三自由度的运动进行解耦运算。即在实际计算过程中纵荡、横荡、首摇这三自由度各取增益 $ K $ 的5个变量，形成针对纵荡方向的增益 $ {K_N} $ ，针对横荡方向的增益 $ {K_E} $ ，针对首摇方向的增益 $ {K_P} $ 。增益 $ {K_N} $ 取第1/4/7/10/13个；增益 $ {K_N} $ 取第2/5/8/11/14个；增益 $ {K_P} $ 取第3/9/12/15个。解耦后的系统计算速度极大提升，也释放了计算的存储量压力。

在每个自由度进行单独的滤波后，考虑到这3个自由度的耦合关系，仍需要进行一定程度的重组工作。在与质量矩阵的逆 ${{\boldsymbol{M}}^{ - 1}}$ 、阻尼矩阵的逆 ${{\boldsymbol{D}}^{ - 1}}$ 、转置矩阵的逆 ${{\boldsymbol{R}}^{ - 1}}\left( \psi \right)$ 等运算之前需要将其重组起来，运算结束之后进行解耦。能够有效得到和估计船舶低频运动，并大幅滤除控制系统接收到的由一阶波浪运动和高斯白噪声等引起的高频分量。

对单自由度的滤波过程进行设计，具体DP运动滤波的内部原理如图1所示。

4 滤波控制仿真及结果分析 4.1 滤波仿真过程及滤波器参数设置

使用之前数学模型和滤波算法，以某DP实船模型为研究对象，通过经验公式的估算，该船舶模型的无因次质量矩阵 $M$ 和阻尼矩阵 $D$ 分别为：

$ \begin{split} {\boldsymbol{M}} = \left[ {\begin{array}{*{20}{c}} {25.800}&0&0 \\ 0&{33.800}&{1.0115} \\ 0&{1.0115}&{2.7600} \end{array}} \right],{\boldsymbol{D}} = \left[ {\begin{array}{*{20}{c}} {2.0}&0&0 \\ 0&{7.0}&{0.1} \\ 0&{0.1}&{0.5} \end{array}} \right] 。\\ \end{split}$

(21)

取海浪强度的参数 $ \sigma $ 为0.5，阻尼系数 $ \lambda $ 为0.1，主导海浪频率 $ {\omega _0} $ 为0.8，滤波截止频率为1.04，偏差矩阵如下：

$ b = \left[ {\begin{array}{*{20}{c}} {100}&0&0 \\ 0&{100}&0 \\ 0&0&{100} \end{array}} \right]，$

(22)

其中，在非线性无源估计滤波器中可以设定增益。

$ \begin{array}{l}{k_{11}} = \left[ {\begin{array}{*{20}{c}} { - 2.34}&0&0 \\ 0&{ - 2.34}&0 \\ 0&0&{ - 2.34} \end{array}} \right],{k_{12}} = \left[ {\begin{array}{*{20}{c}} {1.44}&0&0 \\ 0&{1.44}&0 \\ 0&0&{1.44} \end{array}} \right],\\ {k_3} = {k_4} = \left[ {\begin{array}{*{20}{c}} {0.1}&0&0 \\ 0&{0.1}&0 \\ 0&0&{0.01} \end{array}} \right]\;。\\[-21pt]\end{array}$

(23)

在连续型Kalman滤波器中，取初始矩阵 ${P_0} = $ ${I_{15 \times 15}}$ ，测量噪声协方差矩阵 $ R $ 中的 $ \sigma _{{\text{vi}}}^2 = 200 $ ，状态噪声协方差为 $ {Q_1}= {\rm{diag}}{\text{\{ 100,100,100\} }} $ ， ${Q_2}={\rm{diag}}\{ 0.01,$ $0.01,0.1\}$ 。

4.2 不同滤波方法的运动及滤波效果对比

在Matlab中构建船舶运动滤波及控制的仿真系统，船舶水动力参数和运动控制器取上文中的参数值，滤波器参数分别取非线性无源滤波器和Kalman滤波器的值，仿真时间200 s，设置N，E期望位置均为0，期望航向角为10°，得出船舶运动位置和航向角滤波的仿真结果如图2所示。

通过对这三自由度的频率-振幅对应的面积可以展示滤波效果的好坏，如表1所示。

由表1可知，非线性无源估计滤波器在正北、正东上滤除了82%以上的一阶高频波浪运动；Kalman滤波器在正东和正北方向上滤除了99%以上的一阶波浪运动，而且对于传感器测量白噪声的抑制效果也很理想。亦可知，非线性无源估计滤波器在航向上滤除了83%的一阶高频波浪运动，Kalman滤波器在航向上滤除了超过98%的一阶波浪运动，而且传感器测量白噪声的滤波效果也较好。

由此看出，Kalman滤波器使得船舶的位置、航向的估计值和理想的低频漂移运动输出值基本一致，基本滤除了一阶波浪运动，其中航向角滤除了98%以上的一阶波浪运动，北东方向滤除了99%以上的一阶波浪运动。同非线性无源滤波器相比，Kalman滤波器的运动滤波效果更好，能够较好估计出船舶期望的低频运动。同时也较好抑制了传感器的测量白噪声信号，有利于减少控制器的设计难度和实现高精度运动控制。

5 结　语

本文针对船舶动力定位系统设计中遇到的波浪运动滤波问题，提出一种动力定位系统三自由度解耦Kalman滤波方法，设计了动力定位系统的状态观测器和运动控制器，通过动力定位系统滤波与运动控制仿真，可以得到如下结论：

1）本文方法使得DP船舶在北东坐标系中的位置和航向上滤波效果和估计出的运动曲线更平滑稳定，有效滤除了一阶波浪运动的干扰，其中航向滤除了98%以上的一阶波浪运动，北东方向滤除了99%以上的一阶波浪运动，对传感器测量白噪声信号也有较好的抑制作用。

2）与非线性无源估计滤波器相比，本文方法所得到的运动曲线更平滑、稳定，可有效减小动力定位控制器的设计难度，对船舶动力定位高精度控制系统设计而言，本文方法是一种有效且实用的运动滤波方法。