CN103217160A - 一种船舶动力定位卡尔曼滤波方法 - Google Patents

Abstract

本发明涉及动力定位控制领域，具体涉及一种船舶动力定位系统中的滤波方法。本发明包括如下步骤：（1）测量船舶的位置和加速度，并将数据转化到统一的坐标系下；（2）建立包含高频加速度的船舶模型；（3）建立可以对位置、速度、加速度进行估计的卡尔曼滤波器；（4）基于测量的船舶位置和加速度，利用卡尔曼滤波器滤掉高频干扰并得到船舶动力定位系统需要的低频船舶位置

速度和加速度将数据发送给动力定位系统。该方法可以估计出船舶所需要的船舶低频位置

速度

和加速度

这样通过对位置和加速度进行综合滤波估计可以获得更加精确的船舶位置、速度和加速度信息。

Description

一种船舶动力定位卡尔曼滤波方法

技术领域

本发明涉及动力定位控制领域，具体涉及一种船舶动力定位系统中的滤波方法。

背景技术

滤波是船舶动力定位系统需要考虑的重要问题。船舶动力定位系统利用状态观测器或滤波器从含有噪声的位置和艏向测量值中重构低频运动部分，其功能在于确保推进器系统只对慢变干扰力进行抵抗，而将一阶波浪力引起的高频振荡运动排除在反馈控制回路之外，从而降低机械结构磨损。目前船舶动力定位系统的滤波方法都是认为船舶速度和加速度是不可测量的，如船舶非线性无源观测器、反步观测器。上述滤波方法可以对测量的船舶位置数据进行滤波并能分离出船舶所需要的低频位置，其中在对船舶速度的估计中，它们是通过对位置进行积分运算来获得不可测量的船舶速度。由于这些滤波方法只依靠单一的位置数据进行估计，因此在一定程度上限制了滤波器的性能，可见，如果可以利用船舶位置和其它测量数据共同对船舶速度进行估计，将会一定程度上提高滤波器的性能。

随着深海资源的不断开发和发现，海上作业对船舶动力定位系统的定位精度要求越来越高，如果滤波器的滤波性能如果能够得到改善，将会在一定程度上提高动力定位系统的定位精度。目前惯性导航元件的精确性得到了不断的提高，船舶加速度信息已经可以得到精确的测量。如果滤波器可以利用测量的位置和加速度信息对船舶运动速度进行综合估计，将会在一定程度上提高动力定位系统的定位性能，因此急需发明一种能够利用测量的船舶位置和加速度量去综合估计船舶低频位置、速度和加速度的滤波方法。

发明内容

本发明的目的在于提供一种依靠船舶位置和加速度信息去综合估计位置、速度和加速度的滤波方法。

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

本发明包括如下步骤：

（1）测量船舶的位置和加速度，并将数据转化到统一的坐标系下；

（2）建立包含高频加速度的船舶模型

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=A_{\mathrm{pw}}\mathrm{\ξ}+E_{\mathrm{pw}}{w}_1\\ \stackrel{\·}{a}=A_{\mathrm{aw}}a+E_{\mathrm{aw}}{w}_2\\ {\stackrel{\·}{\mathrm{\η}}}_p=v\\ M\stackrel{\·}{v}+\mathrm{Dv}=b_p+\mathrm{\τ}+w_3\\ b_p=w_4\\ {\mathrm{\η}}_w=C_{\mathrm{pw}}\mathrm{\ξ}\\ y_1={\mathrm{\η}}_p+{\mathrm{\η}}_w+v_1\\ y_2=\mathrm{\γ}\stackrel{\·}{v}+C_{\mathrm{aw}}a\end{array}\right.,$

其中η_p是与北东坐标系的平行坐标系下的船舶低频位置，η_w为平行坐标下高频作用引起的船舶位置，v为船舶速度，y₁和y₂为船舶观测位置和观测加速度，映射矩阵 $\mathrm{\γ}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right];$ R(ψ)为旋转矩阵， $R\left(\mathrm{\ψ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right],$ ψ为船舶艏向角；τ是控制器输入向量；偏差

是平行坐标系下未建模的环境力和力矩；

i＝1,2,3,4是模型零均值高斯白噪声向量，

是零均值高斯白噪声测量向量；ξ为在纵荡、横荡和艏摇3个自由度上的高频作用产生的位置和速度向量；式中

为高频作用下的横向、纵向线加速度和横摇、纵摇角加速度；质量矩阵M和阻尼矩阵D定义如下：

$M=\left[\begin{array}{ccc}m-X_{\stackrel{\·}{u}}& 0& 0\\ 0& m-Y_{\stackrel{\·}{v}}& m{x}_G-Y_{\stackrel{\·}{r}}\\ 0& m{x}_G-N_{\stackrel{\·}{v}}& I_z-N_{\stackrel{\·}{r}}\end{array}\right],$ $D=\left[\begin{array}{ccc}-X_u& 0& 0\\ 0& -Y_v& -Y_r\\ 0& -N_v& -N_r\end{array}\right]$

其中X_u，

Y_v，

Y_r，

N_v，

N_r，I_z为水动力参数，m为刚体质量，x_G为刚体重心在x方向的坐标；

为高频位置系统矩阵，

为高频加速度系统矩阵、

为位置和加速度的转换矩阵，具体形式如下：

$A_{\mathrm{jw}}=\left[\begin{array}{cc}0& I\\ -{\mathrm{\Ω}}_j& -{\mathrm{\Λ}}_j\end{array}\right],$ $E_{\mathrm{jw}}=\left[\begin{array}{c}{0}_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\\ I_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\end{array}\right],$ $C_{\mathrm{jw}}=\left[\begin{array}{cc}{0}_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}& I_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\end{array}\right],$ j＝p,a，

其中 ${\mathrm{\Ω}}_j=\mathrm{diag}({\mathrm{\ω}}_{j,1}^2,\·\·\·{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}^2),$ ${\mathrm{\Λ}}_j=\mathrm{diag}(2{\mathrm{\ζ}}_{j,1}{\mathrm{\ω}}_{j,1},\·\·\·2{\mathrm{\ζ}}_{j,n_{\mathrm{yi}}}{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}),$ $n_{\mathrm{yp}}=3,n_{\mathrm{ya}}=2;{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}$ 为海浪峰值频率；

为相对阻尼系数；

（3）建立可以对位置、速度、加速度进行估计的卡尔曼滤波器：

$\left\{\begin{array}{c}\stackrel{\stackrel{\·}{^}}{z}=A\hat{z}+\mathrm{Bu}+P{C}^T{R}^{-1}(y-C\hat{z})\\ \stackrel{\·}{P}=\mathrm{AP}+P{A}^T+\mathrm{GQ}{G}^T-P{C}^T{R}^{-1}\mathrm{CP}\end{array}\right.$

其中状态矩阵

P为最优估计方差矩阵，Q(t)＝Q^T(t)＞0是模型白噪声向量的协方差矩阵，R(t)＝R^T(t)＞0是测量噪声的协方差矩阵；其它系统矩阵如下：

$A=\left[\begin{array}{ccccc}{A}_{\mathrm{pw}}& 0_{64}& 0_{63}& 0_{63}& 0_{63}\\ 0_{46}& A_{\mathrm{aw}}& 0_{43}& 0_{43}& 0_{43}\\ 0_{36}& 0_{34}& 0_{33}& 0_{33}& I_{33}\\ 0_{36}& 0_{34}& 0_{33}& 0_{33}& 0_{33}\\ 0_{36}& 0_{34}& 0_{33}& M^{-1}& -M^{-1}D\end{array}\right],$ $B=\left[\begin{array}{c}{0}_{63}\\ 0_{43}\\ 0_{33}\\ 0_{33}\\ M^{-1}\end{array}\right],$ $G=\left[\begin{array}{cccc}{E}_{\mathrm{pw}}& 0_{64}& 0_{63}& 0_{63}\\ 0_{43}& E_{\mathrm{aw}}& 0_{43}& 0_{43}\\ 0_{33}& 0_{34}& I_{33}& 0_{33}\\ 0_{33}& 0_{34}& 0_{33}& M^{-1}\end{array}\right],$ $C=\left[\begin{array}{ccccc}{C}_{\mathrm{pw}}& 0_{34}& I_{33}& 0_{33}& 0_{33}\\ 0_{33}& C_{\mathrm{aw}}& 0_{33}& M^{-1}& M^{-1}D\end{array}\right];$

（4）基于测量的船舶位置和加速度，利用卡尔曼滤波器滤掉高频干扰并得到船舶动力定位系统需要的低频船舶位置

速度

和加速度

将数据发送给动力定位系统。

本发明的有益效果在于：

该卡尔曼滤波方法采用的船舶数学模型包含高频加速度量和加速度观测量，该卡尔曼滤波方法不再只依靠测量的位置信息对船舶运动速度进行估计，而是利用测量的位置和加速度对船舶速度进行综合估计，并且该方法可以估计出船舶所需要的船舶低频位置

速度和加速度这样通过对位置和加速度进行综合滤波估计可以获得更加精确的船舶位置、速度和加速度信息。

附图说明

图1为船舶动力定位系统滤波流程图。

具体实施方式

为了使本发明的技术手段易于明白了解，下面以动力定位船S175为例，并结合附图对本发明的技术方案进行详细说明。其中附图1所示为本发明所阐述的基于测量位置和加速度的船舶动力定位滤波流程图，假设海况为3级海况，具体步骤如下：

步骤1.根据该船舶自身的参数建立一种包含高频加速度量和加速度观测量的船舶数学模型：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=A_{\mathrm{pw}}\mathrm{\ξ}+E_{\mathrm{pw}}{w}_1\\ \stackrel{\·}{a}=A_{\mathrm{aw}}a+E_{\mathrm{aw}}{w}_2\\ {\stackrel{\·}{\mathrm{\η}}}_p=v\\ M\stackrel{\·}{v}+\mathrm{Dv}=b_p+\mathrm{\τ}+w_3\\ b_p=w_4\\ {\mathrm{\η}}_w=C_{\mathrm{pw}}\mathrm{\ξ}\\ y_1={\mathrm{\η}}_p+{\mathrm{\η}}_w+v_1\\ y_2=\mathrm{\γ}\stackrel{\·}{v}+C_{\mathrm{aw}}a\end{array}\right.$

其中η_p是与北东坐标系的平行坐标系下的船舶位置，η_w为平行坐标下高频作用引起的船舶位置，v为船舶速度，y₁和y₂为船舶观测位置和观测加速度，映射矩阵 $\mathrm{\γ}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right];$ R(ψ)为旋转矩阵，ψ为船舶艏向角

$R\left(\mathrm{\ψ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right];$

τ是控制器输入向量；偏差

是平行坐标系下未建模的环境力和力矩；

i＝1,2,3,4是模型零均值高斯白噪声向量，

是零均值高斯白噪声测量向量；ξ为在纵荡、横荡和艏摇3个自由度上的高频作用产生的位置和速度向量；

式中为高频作用下的横向、纵向线加速度和横摇、纵摇角加速度；

为高频位置系统矩阵，

为高频加速度系统矩阵、

为位置和加速度的转换矩阵，具体形式如下：

$A_{\mathrm{jw}}=\left[\begin{array}{cc}0& I\\ -{\mathrm{\Ω}}_j& -{\mathrm{\Λ}}_j\end{array}\right],$ $E_{\mathrm{jw}}=\left[\begin{array}{c}{0}_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\\ I_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\end{array}\right],$ $C_{\mathrm{jw}}=\left[\begin{array}{cc}{0}_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}& I_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\end{array}\right],$ j＝p,a，

其中 ${\mathrm{\Ω}}_j=\mathrm{diag}({\mathrm{\ω}}_{j,1}^2,\·\·\·{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}^2),$ ${\mathrm{\Λ}}_j=\mathrm{diag}(2{\mathrm{\ζ}}_{j,1}{\mathrm{\ω}}_{j,1},\·\·\·2{\mathrm{\ζ}}_{j,n_{\mathrm{yi}}}{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}),$ n_yp＝3，n_ya＝2；根据3级海况可以设置为0.8；

可以设置为0.1。

该船的质量矩阵M和阻尼矩阵D定义如下：

$M={10}^7\×\left[\begin{array}{ccc}2.6415& 0& 0\\ 0& 3.3455& 1.4917\\ 0& 1.4917& 652.09\end{array}\right],$ $D={10}^5\×\left[\begin{array}{ccc}0.2220& 0& 0\\ 0& 2.2220& -17.746\\ 0& -17.746& 715.06\end{array}\right]$

步骤2.根据上述船舶数学模型建立如下的卡尔曼滤波器：

$\left\{\begin{array}{c}\stackrel{\stackrel{\·}{^}}{z}=A\hat{z}+\mathrm{Bu}+P{C}^T{R}^{-1}(y-C\hat{z})\\ \stackrel{\·}{P}=\mathrm{AP}+P{A}^T+\mathrm{GQ}{G}^T-P{C}^T{R}^{-1}\mathrm{CP}\end{array}\right.$

其中状态矩阵

P为最优估计方差矩阵，Q(t)＝Q^T(t)＞0是模型白噪声向量的协方差矩阵，R(t)＝R^T(t)＞0是测量噪声的协方差矩阵；其它系统矩阵如下：

$A=\left[\begin{array}{ccccc}{A}_{\mathrm{pw}}& 0_{64}& 0_{63}& 0_{63}& 0_{63}\\ 0_{46}& A_{\mathrm{aw}}& 0_{43}& 0_{43}& 0_{43}\\ 0_{36}& 0_{34}& 0_{33}& 0_{33}& I_{33}\\ 0_{36}& 0_{34}& 0_{33}& 0_{33}& 0_{33}\\ 0_{36}& 0_{34}& 0_{33}& M^{-1}& -M^{-1}D\end{array}\right],$ $B=\left[\begin{array}{c}{0}_{63}\\ 0_{43}\\ 0_{33}\\ 0_{33}\\ M^{-1}\end{array}\right],$ $G=\left[\begin{array}{cccc}{E}_{\mathrm{pw}}& 0_{64}& 0_{63}& 0_{63}\\ 0_{43}& E_{\mathrm{aw}}& 0_{43}& 0_{43}\\ 0_{33}& 0_{34}& I_{33}& 0_{33}\\ 0_{33}& 0_{34}& 0_{33}& M^{-1}\end{array}\right],$ $C=\left[\begin{array}{ccccc}{C}_{\mathrm{pw}}& 0_{34}& I_{33}& 0_{33}& 0_{33}\\ 0_{33}& C_{\mathrm{aw}}& 0_{33}& M^{-1}& M^{-1}D\end{array}\right].$

步骤3.让船舶按照某条轨迹航行或保持在某一位置，利用船舶上装载的GPS、电罗经和加速度计（或其它位置传感器，如：激光传感器，雷达，微波，张紧索，水声定位系统）测量船舶的位置（北向位置，东向位置，艏向角度）和船舶的加速度。如果是多个位置传感器对船舶位置进行测量，需要将其数据转化到统一的坐标系下，如北东地坐标系或随船坐标系。测量到的位置和加速度量是含有测量噪声的，需要经过滤波器滤掉高频干扰并估计出船舶低频位置、速度和加速度。

步骤4.利用上面的卡尔曼滤波器可以对测量的船舶位置和加速度量进行滤波并估计出想要的船舶位置速度以及加速度

把估计到的数据实时的发送给动力定位系统的控制器，控制器把这些数据与期望位置、速度和加速度进行比较并计算之间的偏差，针对位置、速度、加速度的偏差并按照一定的控制方法对推进器发出推力指令，以便让船舶保持在期望位置或者以期望速度按照期望轨迹运行。

Claims (1)

1.一种船舶动力定位卡尔曼滤波方法，其特征在于，包括如下步骤：

（1）测量船舶的位置和加速度，并将数据转化到统一的坐标系下；

（2）建立包含高频加速度的船舶模型

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=A_{\mathrm{pw}}\mathrm{\ξ}+E_{\mathrm{pw}}{w}_1\\ \stackrel{\·}{a}=A_{\mathrm{aw}}a+E_{\mathrm{aw}}{w}_2\\ {\stackrel{\·}{\mathrm{\η}}}_p=v\\ M\stackrel{\·}{v}+\mathrm{Dv}=b_p+\mathrm{\τ}+w_3\\ b_p=w_4\\ {\mathrm{\η}}_w=C_{\mathrm{pw}}\mathrm{\ξ}\\ y_1={\mathrm{\η}}_p+{\mathrm{\η}}_w+v_1\\ y_2=\mathrm{\γ}\stackrel{\·}{v}+C_{\mathrm{aw}}a\end{array}\right.,$

其中η_p是与北东坐标系的平行坐标系下的船舶低频位置，η_w为平行坐标下高频作用引起的船舶位置，v为船舶速度，y₁和y₂为船舶观测位置和观测加速度，映射矩阵 $\mathrm{\γ}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right];$ R(ψ)为旋转矩阵， $R\left(\mathrm{\ψ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right],$ ψ为船舶艏向角；τ是控制器输入向量；偏差

是平行坐标系下未建模的环境力和力矩；

i＝1,2,3,4是模型零均值高斯白噪声向量，

是零均值高斯白噪声测量向量；ξ为在纵荡、横荡和艏摇3个自由度上的高频作用产生的位置和速度向量；式中

为高频作用下的横向、纵向线加速度和横摇、纵摇角加速度；质量矩阵M和阻尼矩阵D定义如下：

$M=\left[\begin{array}{ccc}m-X_{\stackrel{\·}{u}}& 0& 0\\ 0& m-Y_{\stackrel{\·}{v}}& m{x}_G-Y_{\stackrel{\·}{r}}\\ 0& m{x}_G-N_{\stackrel{\·}{v}}& I_z-N_{\stackrel{\·}{r}}\end{array}\right],$ $D=\left[\begin{array}{ccc}-X_u& 0& 0\\ 0& -Y_v& -Y_r\\ 0& -N_v& -N_r\end{array}\right]$

其中X_u，

Y_v，

Y_r，N_v，

N_r，I_z为水动力参数，m为刚体质量，x_G为刚体重心在x方向的坐标；

为高频位置系统矩阵，

为高频加速度系统矩阵、

为位置和加速度的转换矩阵，具体形式如下：

$A_{\mathrm{jw}}=\left[\begin{array}{cc}0& I\\ -{\mathrm{\Ω}}_j& -{\mathrm{\Λ}}_j\end{array}\right],$ $E_{\mathrm{jw}}=\left[\begin{array}{c}{0}_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\\ I_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\end{array}\right],$ $C_{\mathrm{jw}}=\left[\begin{array}{cc}{0}_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}& I_{n_{\mathrm{yj}\×}{n}_{\mathrm{yj}}}\end{array}\right],$ j＝p,a，

其中 ${\mathrm{\Ω}}_j=\mathrm{diag}({\mathrm{\ω}}_{j,1}^2,\·\·\·{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}^2),$ ${\mathrm{\Λ}}_j=\mathrm{diag}(2{\mathrm{\ζ}}_{j,1}{\mathrm{\ω}}_{j,1},\·\·\·2{\mathrm{\ζ}}_{j,n_{\mathrm{yi}}}{\mathrm{\ω}}_{j,n_{\mathrm{yj}}}),$ n_yp＝3，n_ya＝2；

为海浪峰值频率；

为相对阻尼系数；

（3）建立可以对位置、速度、加速度进行估计的卡尔曼滤波器：

$\left\{\begin{array}{c}\stackrel{\stackrel{\·}{^}}{z}=A\hat{z}+\mathrm{Bu}+P{C}^T{R}^{-1}(y-C\hat{z})\\ \stackrel{\·}{P}=\mathrm{AP}+P{A}^T+\mathrm{GQ}{G}^T-P{C}^T{R}^{-1}\mathrm{CP}\end{array}\right.$

其中状态矩阵

P为最优估计方差矩阵，Q(t)＝Q^T(t)＞0是模型白噪声向量的协方差矩阵，R(t)＝R^T(t)＞0是测量噪声的协方差矩阵；其它系统矩阵如下：

$A=\left[\begin{array}{ccccc}{A}_{\mathrm{pw}}& 0_{64}& 0_{63}& 0_{63}& 0_{63}\\ 0_{46}& A_{\mathrm{aw}}& 0_{43}& 0_{43}& 0_{43}\\ 0_{36}& 0_{34}& 0_{33}& 0_{33}& I_{33}\\ 0_{36}& 0_{34}& 0_{33}& 0_{33}& 0_{33}\\ 0_{36}& 0_{34}& 0_{33}& M^{-1}& -M^{-1}D\end{array}\right],$ $B=\left[\begin{array}{c}{0}_{63}\\ 0_{43}\\ 0_{33}\\ 0_{33}\\ M^{-1}\end{array}\right],$ $G=\left[\begin{array}{cccc}{E}_{\mathrm{pw}}& 0_{64}& 0_{63}& 0_{63}\\ 0_{43}& E_{\mathrm{aw}}& 0_{43}& 0_{43}\\ 0_{33}& 0_{34}& I_{33}& 0_{33}\\ 0_{33}& 0_{34}& 0_{33}& M^{-1}\end{array}\right],$ $C=\left[\begin{array}{ccccc}{C}_{\mathrm{pw}}& 0_{34}& I_{33}& 0_{33}& 0_{33}\\ 0_{33}& C_{\mathrm{aw}}& 0_{33}& M^{-1}& M^{-1}D\end{array}\right]$

（4）基于测量的船舶位置和加速度，利用卡尔曼滤波器滤掉高频干扰并得到船舶动力定位系统需要的低频船舶位置

速度

和加速度

将数据发送给动力定位系统。

Images