CN104787260A - 一种基于融合滤波器的水翼双体船纵向姿态估计方法 - Google Patents

Abstract

本发明具体涉及一种通过测量系统得到船体部分纵向姿态信息(含测量噪声)的基于融合滤波器的水翼双体船纵向姿态估计方法。本发明包括：一船上测量系统测出船体部分纵向姿态信息，分别传递给第一滤波器和第二滤波器，同时作用在船上的控制量也分别传递给第一滤波器和第二滤波器；由第一滤波器和第二滤波器得到船体纵向姿态估计值k＝1,2,…；分别计算第一滤波器和第二滤波器的评价函数，通过评价函数准则，得到水翼双体船纵向姿态的融合估计本发明估计结果的统计特性达到很好估计精度，两个滤波器独立并行运算，不会增加运算时间，基于波浪有色干扰而没进行状态扩维，减少了滤波估计的计算量和复杂度，提高了运算速度。

Description

一种基于融合滤波器的水翼双体船纵向姿态估计方法

技术领域

本发明具体涉及一种通过测量系统得到船体部分纵向姿态信息(含测量噪声)的基于融合滤波器的水翼双体船纵向姿态估计方法。

背景技术

水翼双体船经常航行在具有随机干扰作用的海情下，从而使船体发生摇摆，加之测量仪器中也不可避免地存在着观测噪声。为了保持船体的姿态，尤其是高海情情况下，采取一定的控制是必须的，而对此需要对船体摇摆时的状态(姿态)量进行观测，而在实际中，并不是所有需要的状态量都是可以观测到的，即使对于部分可以观测的状态量，也含有随机噪声，因此就需要对这些无法直接观测到的和含有随机噪声的状态量运用一些方法进行估计或运用易于观测的量间接求出来。

水翼双体船姿态运动具有较强的非线性，因此对水翼双体船姿态估计要选择非线性估计方法，对于非线性估计问题的处理，常用的方法用EKF(扩展卡尔曼滤波)、PF(粒子滤波)以及UKF(无迹卡尔曼滤波)，EKF虽然使用广泛，但是由于其自身存在的不足，可能导致某些非线性问题不能准确解决，而PF虽然使用范围广且精度高，但是其计算大而导致计算速度相对较慢。

本发明依据水翼船在海浪中建立的纵向运动非线性方程为基础，对船体纵向运动姿态进行估计。在分析非线性估计方法的基础上，提出一种融合滤波器。首先通过两个滤波器分别对水翼双体船姿态进行估计，将滤波器单个估计误差样本的平方作为评价函数，引入一个评价函数准则，进行两个滤波器的切换，进而达到了两个滤波器优势的融合，提高了估计精度，两个滤波器并行独立运算，不会增加运算量和运算时间。

发明内容

本发明的目的在于提供一种基于融合滤波器的水翼双体船纵向姿态估计方法。

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

一种基于融合滤波器的水翼双体船纵向姿态估计方法，包括：

(1)船上测量系统测出船体部分纵向姿态信息，分别传递给第一滤波器和第二滤波器，同时作用在船上的控制量也分别传递给第一滤波器和第二滤波器；

(2)由第一滤波器和第二滤波器得到船体纵向姿态估计值

(3)分别计算第一滤波器和第二滤波器的评价函数，通过评价函数准则，得到水翼双体船纵向姿态的融合估计

所述的第一滤波器和第二滤波器是通过sigma点发生器获取sigma点，经过非线性状态函数进行时间更新获得状态一步预报和状态一步预报协方差，经过非线性观测函数进行测量更新获得输出预测、输出预测自协方差和输出预测互方差，将观测量、输出预测、输出预测自协方差、输出预测互协方差、状态一步预报和状态一步预报协方差送给状态估计器后得到船体纵向姿态估计值 ${{\hat{x}}_k}^{\left(A\right)},{{\hat{x}}_k}^{\left(B\right)},k=\mathrm{1,2},....$

所述的非线性状态函数和非线性观测函数为：

$\left\{\begin{array}{c}{x}_k=f\left(x_k\right)+\mathrm{\Γ}{w}_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right..$

所述的sigma点发生器是指按采样策略获取sigma点

第一滤波器按如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(A\right)}={\stackrel{\‾}{x}}_k;$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k+{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=1,...,n;$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k+{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=n+1,...,2n;$

第二滤波器按如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(B\right)}={\stackrel{\‾}{x}}_k;$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k+{\left(\sqrt{h^2{P}_k}\right)}_i,i=1,...,n;$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k+{\left(\sqrt{h^2{P}_k}\right)}_i,i=n+1,...,2n.$

所述的时间更新是指将Sigma点集中的每一个点带入到非线性状态函数中去，

${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}=f\left({\mathrm{\χ}}_{k-1}^{i,\left(j\right)}\right);$

式中j＝A或j＝B代表第一滤波器或第二滤波器

状态一步预报为

${\hat{x}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)};$

状态一步预报协方差为

$P_{x,k}^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]{[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]}^T.$

所述的测量更新是指基于时间更新中的和利用sigma点发生器获得sigma点 ${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)},i=\mathrm{0,1},...,n+1,$ 经过非线性观测函数传播

${\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}=H\left({\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}\right);$

输出预测为 ${\hat{y}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)};$

输出预测自协方差为 $P_{y,k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]{[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]}^T;$

输出预测互协方差为 $P_{\mathrm{xy},k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]{[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]}^T.$

所述的状态估计器是指运用新的观测量得到船体纵向姿态估计值 ${\hat{x}}_k^{\left(j\right)}={\hat{x}}_k^{\′,\left(j\right)}+K^{\left(j\right)}(y_k-{\hat{y}}_k^{\′,\left(j\right)}).$

所述的观测量是指通过测量系统得到的船体部分纵向姿态信息；所述的船体部分纵向姿态信息是指垂荡位移和纵摇角位移；所述的测量系统是指在一定海情下，采集船舶实际航向、航速、垂荡和纵摇角的位移传感器、陀螺仪，计程仪、船载经纬仪。

所述的评价函数是指将滤波器单个估计误差样本的平方作为评价函数：i表示x_k的第i行元素；所述的评价函数准则是指将第一滤波器和第二滤波器的每一时刻的评价函数进行比较，评价函数小的滤波器的估计值即为水翼双体船纵向姿态的融合估计

${\hat{x}}_{k,i}=(1-{\mathrm{\ρ}}_i){\hat{x}}_{k,i}^{\left(A\right)}+{\mathrm{\ρ}}_i{{\hat{x}}_{k,i}}^{\left(B\right)},i=\mathrm{1,2,3,4},k=\mathrm{1,2},...;$

所述的水翼双体船纵向姿态的融合估计是指达到第一滤波器和第二滤波器优势的融合。

本发明的有益效果在于：

该方法是为解决水翼双体船航行在高海情下发生姿态摇摆的相对复杂而产生的，利用融合滤波器解决水翼船纵向姿态估计是合理可行的，并且其估计结果的统计特性达到很好估计精度，融合滤波器融合了两个滤波器的优点，两个滤波器独立并行运算，不会增加运算时间，基于波浪有色干扰而没进行状态扩维，减少了滤波估计的计算量和复杂度，提高了运算速度。

附图说明

图1为基于融合滤波器的水翼双体船纵向姿态估计的结构框图；

图2为五级海情、水翼船航速为35节，航行角为180度时基于融合滤波器的对船体升沉位移与升沉速度的估计结果与真实值的对比图；

图3为五级海情、水翼船航速为35节，航行角为180度时基于融合滤波器的对船体纵摇角与其角速度的估计结果与真实值的对比图；

图4为水翼双体船纵向(姿态)状态估计的统计特性。

具体实施方法

下面结合附图对本发明做进一步描述。

包括滤波器A、滤波器B。船上测量系统(位移传感器、陀螺仪，计程仪、船载经纬仪)测出船体部分纵向姿态信息(观测量，包括垂荡位移和纵摇角位移)，分别传递给滤波器A和滤波器B，同时作用在船上的控制量也分别传递给滤波器A和滤波器B，由滤波器A和滤波器B得到船体纵向姿态估计值分别计算滤波器A和滤波器B的评价函数，通过一个评价函数准则，得到水翼双体船纵向姿态的融合估计

本发明的基于船体姿态信息估计海浪浪高的方法还可以包括：

1、所述的滤波器是通过sigma点发生器获取sigma点，经过非线性状态函数进行时间更新获得状态一步预报和状态一步预报协方差，经过非线性观测函数进行测量更新获得输出预测、输出预测自协方差和输出预测互方差，将观测量、输出预测、输出预测自协方差、输出预测互协方差、状态一步预报和状态一步预报协方差送给状态估计器后得到船体纵向姿态估计值 ${{\hat{x}}_k}^{\left(A\right)},{{\hat{x}}_k}^{\left(B\right)},k=\mathrm{1,2},....$

2、所述的非线性状态函数和非线性量测函数为：

$\left\{\begin{array}{c}{x}_k=f\left(x_k\right)+\mathrm{\Γ}{w}_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right.---\left(15\right)$

3、所述的sigma点发生器是指按一定采样策略获取sigma点。

滤波器A按如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(A\right)}={\stackrel{\‾}{x}}_k---\left(16\right)$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k+{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=1,...,n---\left(17\right)$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k+{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=n+1,...,2n---\left(18\right)$

滤波器B按如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(B\right)}={\stackrel{\‾}{x}}_k---\left(19\right)$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k+{\left(\sqrt{h^2{P}_k}\right)}_i,i=1,...,n---\left(20\right)$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k+{\left(\sqrt{h^2{P}_k}\right)}_i,i=n+1,...,2n---\left(21\right)$

4、所述的时间更新是指将Sigma点集中的每一个点带入到非线性状态函数中去，

${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}=f\left({\mathrm{\χ}}_{k-1}^{i,\left(j\right)}\right)---\left(22\right)$

式中j＝A或j＝B代表滤波器A或滤波器B。

状态一步预报为

${\hat{x}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}---\left(23\right)$

状态一步预报协方差为

$P_{x,k}^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]{[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]}^T---\left(25\right)$

5、所述的测量更新是指基于时间更新中的和利用sigma点发生器获得sigma点 ${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)},i=\mathrm{0,1},...,n+1,$ 经过非线性观测函数传播

${\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}=H\left({\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}\right)---\left(27\right)$

输出预测为 ${\hat{y}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}---\left(28\right)$

输出预测自协方差为 $P_{y,k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]{[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]}^T---\left(29\right)$

输出预测互协方差为 $P_{\mathrm{xy},k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]{[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]}^T---\left(30\right)$

6、所述的状态估计器是指运用新的观测量得到船体纵向姿态估计值 ${{\hat{x}}_k}^{\left(A\right)},{{\hat{x}}_k}^{\left(B\right)},(k=\mathrm{1,2},...):$

${\hat{x}}_k^{\left(j\right)}={\hat{x}}_k^{\′,\left(j\right)}+K^{\left(j\right)}(y_k-{\hat{y}}_k^{\′,\left(j\right)})---\left(31\right)$

7、所述的观测量是指通过测量系统得到的船体部分纵向姿态信息

8、所述的船体部分纵向姿态信息是指垂荡位移和纵摇角位移。

9、所述的测量系统是指在一定海情下，采集船舶实际航向、航速、垂荡和纵摇角的位移传感器、陀螺仪，计程仪、船载经纬仪。

10、所述的评价函数是指将滤波器单个估计误差样本的平方作为评价函数：i表示x_k的第i行元素。。

11、所述的评价函数准则是指将滤波器A和滤波器B的每一时刻的评价函数进行比较，评价函数小的滤波器的估计值即为水翼双体船纵向姿态的融合估计

${\hat{x}}_{k,i}=(1-{\mathrm{\ρ}}_i){\hat{x}}_{k,i}^{\left(A\right)}+{\mathrm{\ρ}}_i{{\hat{x}}_{k,i}}^{\left(B\right)},(i=\mathrm{1,2,3,4},k=\mathrm{1,2},...)$

12、所述的水翼双体船纵向姿态的融合估计是指达到滤波器A和滤波器B优势的融合。

本发明的工作原理是：对原水翼双体船纵向运动建立非线性状态方程和观测方程，并离散化；将基于两个滤波器的水翼双体船纵向姿态估计结果进行优势融合；两个滤波器独立并行运算，将滤波器单个估计误差样本的平方作为评价函数，通过一个评价函数准则，进行两个滤波器的切换，进而进行估计误差和方差的控制。

本发明描述的是一种水翼双体船纵向姿态估计方法，该方法能够能够快速并且准确的得到水翼双体船纵向姿态的估计值，操作方便且不会增加运算时间。本发明的设计方案如图1所示，具体的步骤如下：

第一步水翼双体船纵向运动离散状态空间方程的建立

水翼双体船垂荡和纵摇运动的非线性运动模型如下：

$m(\stackrel{\·\·}{z}+u\stackrel{\·}{\mathrm{\θ}})=\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{L}_{\mathrm{fi}}+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{F}_{\mathrm{ai}}+\▿\cos \mathrm{\θ}+2L_H+\mathrm{mg}\cos \mathrm{\θ}---\left(1\right)$

$I_{\mathrm{yy}}\stackrel{\·\·}{\mathrm{\θ}}=-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}(x_{\mathrm{fi}}-x_g)L_{\mathrm{fi}}-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}(x_{\mathrm{fi}}-x_g)F_{\mathrm{ai}}-(x_b-x_g)\▿\cos \mathrm{\θ}-2(x_H-x_g)L_H---\left(2\right)$

式(1)、(2)中，m为船体质量，z为船体在垂直方向上的升沉位移，u表示船体在纵向的速度(假设为定值)，θ为船体的纵摇角，L_fi表示船体水翼产生的升力，F_ai为水翼附加质量而产生的惯性力，▽表示船体所受的浮力，L_H为船体引起的升力，g表示重力加速度，I_yy为船体相对于船体的Y轴转动惯量，|x_fi|、|x_g|、|x_b|和|x_H|分别为船体水翼、重心、浮力作用点以及船体升力作用点到船体中心的距离。

将船体水翼作为刚体看待，其惯性力的大小与水翼的加速度成正比，其方向和加速度方向相反，计算公式如下：

$F_{\mathrm{ai}}=-m_{\mathrm{fi}}(\stackrel{\·\·}{z}+u\stackrel{\·}{\mathrm{\θ}}-(x_{\mathrm{fi}}-x_g)\stackrel{\·\·}{\mathrm{\θ}}-{\stackrel{\·\·}{\mathrm{\ζ}}}_i)---\left(3\right)$

式中m_fi为水翼附加质量，是海浪水质点在水翼处垂直于水面的运动加速度。

船体所产生的升力：

$L_H=-\frac{1}{2}\mathrm{\ρ}{u}^2{A}_w{C}_{\mathrm{LH}}\mathrm{\θ}---\left(4\right)$

式中，A_w为船体水线面面积，C_LH是船体升力系数的斜率。

将(3)、(4)两式分别代入到式(1)、(2)中，整理可得：

$\begin{array}{c}(m+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}})\stackrel{\·\·}{z}-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}(x_{\mathrm{fi}}-x_g)\stackrel{\·\·}{\mathrm{\θ}}+(\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}+m)u\stackrel{\·}{\mathrm{\θ}}+\mathrm{\ρ}{u}^2{A}_w{C}_{\mathrm{LH}}\mathrm{\θ}\\ -(\▿+\mathrm{mg})\cos \mathrm{\θ}=\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{L}_{\mathrm{fi}}+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}{\stackrel{\·\·}{\mathrm{\ζ}}}_i\end{array}---\left(5\right)$

$\begin{array}{c}-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}(x_{\mathrm{fi}}-x_g)\stackrel{\·\·}{z}+I_{\mathrm{yy}}\stackrel{\·\·}{\mathrm{\θ}}+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}{(x_{\mathrm{fi}}-x_g)}^2\stackrel{\·\·}{\mathrm{\θ}}-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}(f_{\mathrm{fi}}-x_g)u\stackrel{\·}{\mathrm{\θ}}\\ +(x_g-x_H){\mathrm{\ρu}}^2{A}_w{C}_{\mathrm{LH}}\mathrm{\θ}+(x_b-x_g)\▿\cos \mathrm{\θ}=-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{(x_{\mathrm{fi}}-x_g)L}_{\mathrm{fi}}-\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{m}_{\mathrm{fi}}{(x_{\mathrm{fi}}-x_g)\stackrel{\·\·}{\mathrm{\ζ}}}_i\end{array}---\left(6\right)$

为使表达简单明确，(5)、(6)两式表示为：

$(m+a_{33})\stackrel{\·\·}{z}+a_{35}\stackrel{\·}{\mathrm{\θ}}+b_{35}\stackrel{\·}{\mathrm{\θ}}+c_{35}\mathrm{\θ}-(\▿+\mathrm{mg})\cos \mathrm{\θ}=(L_{f1}+L_{f2})+(F_{w1}+F_{w2})---\left(7\right)$

$a_{53}\stackrel{\·\·}{z}+(I_{\mathrm{yy}}+a_{55})\stackrel{\·}{\mathrm{\θ}}+b_{35}\stackrel{\·}{\mathrm{\θ}}+c_{35}\mathrm{\θ}-(x_b-x_g)\▿\cos \mathrm{\θ}=(x_{f1}{L}_{\mathrm{fi}}+{x_{f2}L}_{f2})+(M_{w1}+M_{w2})---\left(8\right)$

式(7)、(8)中a_ij(i,j＝3,5)为附加质量；b_ij(i,j＝3,5)为阻尼力系数；c_ij(i,j＝3,5)为恢复力系数；将a_ij、b_ij、c_ij统称为水动力系数，F_wi和M_wi分别是第i个水翼处所受的垂向波浪力和纵摇波浪力矩。

如上(7)、(8)两式为微分方程形式的船体纵向运动方程，为便于问题的处理，将式(7)、(8)中微分方程转化为向量形式的状态方程，此时取状态向量为 表示船体在垂直方向上的升沉速度，表示船体的纵摇角速度。为此(7)、(8)两式写为：

$\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& a_{33}+m& 0& a_{35}\\ 0& 0& 1& 0\\ 0& a_{53}& 0& I_{\mathrm{yy}}+a_{55}\end{array}\right)\stackrel{\·}{x}=\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& -c_{35}& -b_{35}\\ 0& 0& 0& 1\\ 0& 0& -c_{55}& -b_{55}\end{array}\right)x+\left[\begin{array}{c}0\\ f_1\left(\mathrm{\θ}\right)\\ 0\\ f_2\left(\mathrm{\θ}\right)\end{array}\right]+\left(\begin{array}{cc}0& 0\\ 1& 1\\ 0& 0\\ x_{f1}& x_{f2}\end{array}\right)\left[\begin{array}{c}{L}_{f1}\\ L_{f2}\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{F}_w\\ M_w\end{array}\right]---\left(9\right)$

其中，F_w＝F_w1+F_w2，M_w＝M_w1+M_w2，F_w和M_w统称为波浪扰动，是有色噪声。f₁(θ)＝(▽+mg)cosθ，f₂(θ)＝(x_g-x_b)▽cosθ。

以水翼船水动力系数的水池实验数据代入式(9)，求得系统特征值，可知系统开环是不稳定的。为此引入虚拟控制

$\stackrel{\‾}{u}={\left[\begin{array}{cccc}0& c_{33}z+b_{33}\stackrel{\·}{z}+L_{f1}+L_{f2}& 0& c_{53}z+b_{53}\stackrel{\·}{z}+x_{f1}{L}_{f1}+x_{f2}{L}_{f2}\end{array}\right]}^T---\left(10\right)$

将式(10)代入式(9)得：

$\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& a_{33}+m& 0& a_{35}\\ 0& 0& 1& 0\\ 0& a_{53}& 0& I_{\mathrm{yy}}+a_{55}\end{array}\right)\stackrel{\·}{x}=\left(\begin{array}{cccc}0& 1& 0& 0\\ -c_{33}& -b_{33}& -c_{35}& -b_{35}\\ 0& 0& 0& 1\\ -c_{53}& -b_{53}& -c_{55}& -b_{55}\end{array}\right)x+\left[\begin{array}{c}0\\ f_1\left(\mathrm{\θ}\right)\\ 0\\ f_2\left(\mathrm{\θ}\right)\end{array}\right]+\stackrel{\‾}{u}+\left(\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right)\left[\begin{array}{c}{F}_w\\ M_w\end{array}\right]---\left(11\right)$

可验证引入虚拟控制后，式(11)所表示系统开环特征值具有负实部，则开环系统是稳定的。

或式(11)又可写成

$E\stackrel{\·}{x}=A^{*}x+{\left[\begin{array}{cccc}0& f_1\left(\mathrm{\θ}\right)& 0& f_2\left(\mathrm{\θ}\right)\end{array}\right]}^T+\stackrel{\‾}{u}+C^{*}w$

式中：

$E=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& a_{33}+m& 0& a_{35}\\ 0& 0& 1& 0\\ 0& a_{53}& 0& I_{\mathrm{yy}}+a_{55}\end{array}\right),A^{*}=\left(\begin{array}{cccc}0& 1& 0& 0\\ -c_{33}& -b_{33}& -c_{35}& -b_{35}\\ 0& 0& 0& 1\\ -c_{53}& -b_{53}& -c_{55}& -b_{55}\end{array}\right),C^{*}=\left(\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right),w=\left[\begin{array}{c}{F}_w\\ M_w\end{array}\right]$

则状态方程如下

$\stackrel{\·}{x}=\mathrm{Ax}+g\left(x\right)+B\stackrel{\‾}{u}+\mathrm{Cw}---\left(12\right)$

式中：

A＝E^-1A^*, B＝E^-1, C＝E^-1C^*, g(x)＝E^-1[0 f₁(θ) 0 f₂(θ)]^T

令虚拟控制(从式(10)可以看出，相当于对系统施加适当的比例微分控制)，则式(12)为：

$\stackrel{\·}{x}=\mathrm{Ax}+g\left(x\right)+\mathrm{Cw}---\left(13\right)$

选取升沉z与纵摇角θ作为系统的观测量，建立观测方程如下：

y＝hx+v(14)

式中 $h=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],$ v为系统观测噪声向量。

固定海清、航速、航向角下，方程(13)中等式右边第一项可看作是线性项，而第二项是非线性项，第三项是波浪扰动。

分别运用线性离散化和非线性龙格库塔法对式(13)、(14)进行离散化，则船体纵向运动系统的离散化方程如下：

$\left\{\begin{array}{c}{x}_k=f\left(x_k\right)+\mathrm{\Γ}{w}_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right.---\left(15\right)$

k为时间。

第二步滤波器工作原理

滤波器利用采样策略来逼近非线性分布，以UT(unscented)变换为基础，采用卡尔曼滤波框架以及确定性采样来完成滤波问题。

首先由sigma点发生器获取sigma点，经过非线性状态函数进行时间更新，然后经过非线性观测函数进行测量更新，最后运用新的观测量得到船体纵向姿态估计值，过程如下：

(1)sigma点发生器

滤波器A和滤波器B的sigma点发生器的采样策略不同。

滤波器A采用如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(A\right)}={\stackrel{\‾}{x}}_k---\left(16\right)$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k+{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=1,...,n---\left(17\right)$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k-{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=n+1,...,2n---\left(18\right)$

式中为k时刻状态均值和协方差，n是状态向量维数，κ是比例参数，常设为0或3-n。,为(n+k)p_k的平方根矩阵的第i列。

滤波器B采用如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(B\right)}={\stackrel{\‾}{x}}_k---\left(19\right)$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k+{\left(\sqrt{h^2{P}_k}\right)}_i,i=1,...,n---\left(20\right)$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k-{\left(\sqrt{h^2{P}_k}\right)}_i,i=n+1,...,2n---\left(21\right)$

式中h为尺度参数，它决定了sigma点的均值附近的分布，其它参数的意义同滤波器A。

(2)时间更新：

将Sigma点集中的每一个点带入到非线性状态函数中去，得

${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}=f\left({\mathrm{\χ}}_{k-1}^{i,\left(j\right)}\right)---\left(22\right)$

式中j＝A或j＝B代表滤波器A或滤波器B。

状态一步预报为

${\hat{x}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}---\left(23\right)$

式中

$W_i^{m,\left(A\right)}=\left\{\begin{array}{c}\mathrm{\λ}/(n+\mathrm{\λ}),i=0\\ 1/2(n+\mathrm{\λ}),i\≠0\end{array}\right.---\left(24\right)$

λ＝α²(n+κ)-n，α为比例缩放因子，用于调整sigma点与均值x的距离，通常设为较小的正数(如1e^-4≤α≤1)；

$W_i^{m,\left(B\right)}=\left\{\begin{array}{c}(h^2-n)/h^2,i=0\\ 1/2h^2,i\≠0\end{array}\right.$

状态一步预报协方差为 $P_{x,k}^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]{[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]}^T---\left(25\right)$

式中 $W_i^{c,\left(A\right)}=\left\{\begin{array}{c}\mathrm{\λ}/(n+\mathrm{\λ})+(1-{\mathrm{\α}}^2+\mathrm{\β}),i=0;\\ 1/2(n+\mathrm{\λ}),i\≠0\end{array}\right.---\left(26\right)$

β为引入非线性函数高阶项信息的参数。

$W_i^{c,\left(B\right)}=W_i^{m,\left(B\right)}.$

(3)测量更新：

基于时间更新(2)中的和利用sigma点发生器获得sigma点 ${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)},i=\mathrm{0,1},...,n+1,$ 经过非线性观测函数传播为

${\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}=H\left({\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}\right)---\left(27\right)$

输出预测为 ${\hat{y}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}---\left(28\right)$

输出预测自协方差为 $P_{y,k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]{[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]}^T---\left(29\right)$

输出预测互协方差为 $P_{\mathrm{xy},k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}]{[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}]}^T---\left(30\right)$

(4)状态估计器

运用新的观测量来修正计算的状态后验估计值：

${\hat{x}}_k^{\left(j\right)}={\hat{x}}_k^{\′,\left(j\right)}+K^{\left(j\right)}(y_k-{\hat{y}}_k^{\′,\left(j\right)})---\left(31\right)$

式中 $K^{\left(j\right)}={P^{\left(j\right)}}_{\mathrm{xy},k}{\left[P_{y,k}^{\left(j\right)}\right]}^{-1},$ 状态协方差更新为 ${P^{\left(j\right)}}_{x,k}=P_{x,k}^{\′,\left(j\right)}-K^{\left(j\right)}{P^{\left(j\right)}}_{y,k}{\left[K^j\right]}^T.$

系统观测噪声协方差Qvv＝diag([0.010.001])。

滤波器A：式(17)，式(18)，式(24)，式(26)中取α₁＝α-1,α＝1e-4，β＝2，κ＝0。基于matlab平台，根据式(31)估计出船体运动状态

滤波器B(基于中心差分卡尔曼滤波器)：式(20)，式(21)中h²＝3。基于matlab平台，根据式(31)估计出船体运动状态

两个滤波器A和B并行运算，计算各自评价函数。评价函数定义为滤波器单个估计误差样本的平方：i表示的第i行元素。

将每一时刻的评价函数进行比较，水翼双体船纵向姿态估计取评价函数小的滤波器的估计值。即: ${\hat{x}}_{k,i}=(1-{\mathrm{\ρ}}_i){\hat{x}}_{k,i}^{\left(A\right)}+{\mathrm{\ρ}}_i{{\hat{x}}_{k,i}}^{\left(B\right)},(i=\mathrm{1,2,3,4},k=\mathrm{1,2},...)$

其中，评价函数准则：

${\mathrm{\ρ}}_i=I[J_i^{\left(A\right)}-J_i^B],$ Ι是单位阶跃函数，即

图2,图3表示的是五级海情、水翼船航速为35节，航行角为180度时基于融合滤波器得到的水翼船纵向姿态估计序列。表1为水翼双体船纵向姿态估计的统计特性。

Claims (10)

1.一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：

(1)船上测量系统测出船体部分纵向姿态信息，分别传递给第一滤波器和第二滤波器，同时作用在船上的控制量也分别传递给第一滤波器和第二滤波器；

(2)由第一滤波器和第二滤波器得到船体纵向姿态估计值

(3)分别计算第一滤波器和第二滤波器的评价函数，通过评价函数准则，得到水翼双体船纵向姿态的融合估计

2.根据权利要求1所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的第一滤波器和第二滤波器是通过sigma点发生器获取sigma点，经过非线性状态函数进行时间更新获得状态一步预报和状态一步预报协方差，经过非线性观测函数进行测量更新获得输出预测、输出预测自协方差和输出预测互方差，将观测量、输出预测、输出预测自协方差、输出预测互协方差、状态一步预报和状态一步预报协方差送给状态估计器后得到船体纵向姿态估计值

3.根据权利要求2所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的非线性状态函数和非线性观测函数为：

$\left\{\begin{array}{c}{x}_k=f\left(x_k\right)+{\mathrm{\Γw}}_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right..$

4.根据权利要求2所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的sigma点发生器是指按采样策略获取sigma点

第一滤波器按如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(A\right)}={\stackrel{\‾}{x}}_k;$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k+{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=1,...,n;$

${\mathrm{\χ}}_k^{i,\left(A\right)}={\stackrel{\‾}{x}}_k-{(\sqrt{(n+\mathrm{\κ})P_k}+{\mathrm{\α}}_1\sqrt{(n+\mathrm{\κ})P_k})}_i,i=n+1,...,2n;$

第二滤波器按如下采样策略获取sigma点：

${\mathrm{\χ}}_k^{0,\left(B\right)}={\stackrel{\‾}{x}}_k;$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k+{\left(\sqrt{h^2{P}_k}\right)}_i,i=1,...,n;$

${\mathrm{\χ}}_k^{i,\left(B\right)}={\stackrel{\‾}{x}}_k-{\left(\sqrt{h^2{P}_k}\right)}_i,i=n+1,...,2n.$

5.根据权利要求2所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的时间更新是指将Sigma点集中的每一个点带入到非线性状态函数中去，

${\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}=f\left({\mathrm{\χ}}_{k-1}^{i,\left(j\right)}\right);$

式中j＝A或j＝B代表第一滤波器或第二滤波器

状态一步预报为

${\hat{x}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)};$

状态一步预报协方差为

$P_{x,k}^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}][{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}{]}^T.$

6.根据权利要求2所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的测量更新是指基于时间更新中的和利用sigma点发生器获得sigma点经过非线性观测函数传播

${\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}=H\left(x_{k|k-1}^{i,\left(j\right)}\right);$

输出预测为 ${\hat{y}}_k^{\′,\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right),\left(j\right)}{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)};$

输出预测自协方差为 $P_{y,k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}][{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\hat{y}}_k^{\′,\left(j\right)}{]}^T;$

输出预测互协方差为 $P_{\mathrm{xy},k}^{\left(j\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right),\left(j\right)}[{\mathrm{\χ}}_{k|k-1}^{i,\left(j\right)}-{\hat{x}}_k^{\′,\left(j\right)}][{\mathrm{\γ}}_{k|k-1}^{i,\left(j\right)}-{\widehat{\mathrm{\γ}}}_k^{\′,\left(j\right)}{]}^T.$

7.根据权利要求2所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的状态估计器是指运用新的观测量得到船体纵向姿态估计值

${\hat{x}}_k^{\left(j\right)}={\hat{x}}_k^{\′,\left(j\right)}+K^{\left(j\right)}(y_k-{\hat{y}}_k^{\′,\left(j\right)}).$

8.根据权利要求2所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的观测量是指通过测量系统得到的船体部分纵向姿态信息；所述的船体部分纵向姿态信息是指垂荡位移和纵摇角位移；所述的测量系统是指在一定海情下，采集船舶实际航向、航速、垂荡和纵摇角的位移传感器、陀螺仪，计程仪、船载经纬仪。

9.根据权利要求1所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的评价函数是指将滤波器单个估计误差样本的平方作为评价函数：i表示x_k的第i行元素；所述的评价函数准则是指将第一滤波器和第二滤波器的每一时刻的评价函数进行比较，评价函数小的滤波器的估计值即为水翼双体船纵向姿态的融合估计

${\hat{x}}_{k,i}=(1-{\mathrm{\ρ}}_i){\hat{x}}_{k,i}^{\left(A\right)}+{\mathrm{\ρ}}_i{{\hat{x}}_{k,i}}^{\left(B\right)},i=\mathrm{1,2,3,4},k=\mathrm{1,2},...;$

10.根据权利要求9所述的一种基于融合滤波器的水翼双体船纵向姿态估计方法，其特征在于：所述的水翼双体船纵向姿态的融合估计是指达到第一滤波器和第二滤波器优势的融合。