CN101920762B - 一种基于噪声矩阵实时修正的船舶动力定位方法 - Google Patents

Abstract

本发明涉及一种基于噪声矩阵实时修正的船舶动力定位方法，包括：由综合位置测量系统测量船舶本身位置信号，并通过基于噪声矩阵实时修正的卡尔曼数字滤波器滤去上述信号中的噪声信号，并分离出船舶的高、低频信号，并将低频信号传输给控制系统；环境测量传感器系统测量船舶所处环境的参数，将测得的环境参数信号传输给控制系统；控制系统根据滤波后的低频信号及部分环境信号计算船舶定位控制信号；推力分配器将计算出的船舶定位控制信号分配给推力系统的各推力器；各推力器根据船舶定位控制信号，将船舶推动至预期的位置。本发明通过实时计算系统噪声，将计算结果应用于修正噪声序列的方差，使动力定位系统在复杂海况下，仍能保持较好的定位精度。

Description

一种基于噪声矩阵实时修正的船舶动力定位方法

技术领域

本发明涉及船舶动力定位技术，特别是涉及一种基于噪声矩阵实时修正的船舶动力定位方法。

背景技术

动力定位Dynamic Positioning是指海上作业系统比如轮船、深海钻井平台等系统利用其本身具有的动力来克服风、波浪、海流等外部环境干扰，从而使之能够保持在一定的位置和方向上，更进一步说明是指不借助锚泊系统的作用，而用精密先进的仪器来测定海上作业系统因为风、浪、流作用而发生的位移和方向变化，同时通过计算机等自动控制系统对信息进行处理和计算并自动控制若干个不同方向的推进器的推力或力矩使得海上作业系统回复到设定位置。此外，如果将定位点以一定速度沿预定轨迹运动，也可实现航迹保持。与普通依靠舵实线航迹保持相比，动力定位具有横向运动的能力，从而为实现更高精度的航迹保持提供了可能性，随着人类对海洋的开发、探索，动力定位技术显得越来越重要。

在动力定位中，需要进行位置和速度的测量，对于位置的测量，一般分为方位和角度。其中，角度的测量，在动力定位系统中通常采用电罗经，而方位的测量通常采用卫星导航系统(GPS)，速率陀螺仪用于测量角速度、横荡速度和纵荡速度：

(1)电罗经，又称陀螺罗经，它能自动、连续地提供舰船的航向信号，并通过航向发送装置将航向信号传递到舰船需要航向信号的各个部位，从而满足舰船导航和定位的要求，是舰船必不可少的精密导航设备，被称为舰船的“眼睛”。全套设备由主罗经、分罗经和附属仪器三部分组成，核心部件是主罗经内的陀螺球。电罗经受到其机械结构和工作环境等的影响，不可避免的存在误差，需要进行滤波。

(2)卫星导航系统有两种商用的卫星导航系统，美国的Navstar GPS系统和俄罗斯的GLONASS系统。GPS定位是利用一组卫星的伪距、星历、卫星发射时间等观测量来实现的，同时还必须知道用户时钟差。因此，要获得地面点的三维坐标，必须对4颗卫星进行测量。在这一定位过程中，存在着三部分误差。一部分是对每一个用户接收机所公有的，例如，卫星钟误差、星历误差、电离层误差、对流层误差等；第二部分为不能由用户测量或由校正模型来计算的传播延迟误差；第三部分为各用户接收机所固有的误差，例如内部噪声、通道延迟、多径效应等。

利用差分GPS定位技术(DGPS)，除第三部分误差无法消除外，第一部分误差完全可以消除，第二部分误差大部分可以消除，其主要取决于基准接收机和用户接收机的距离。差分GPS定位已将卫星钟误差和星历误差消除，并将电离层延迟和对流层延迟误差部分消除，定位精度大大提高。所以，差分GPS定位技术(DGPS)在最近几年中得到了迅速发展和广泛应用。

根据差分GPS基准站发送的信息方式可将差分GPS定位技术(DGPS)分为三类，即：位置差分、伪距差分和相位差分。这三类差分方式的工作原理是相同的，即都是由基准站发送改正数，由用户站接收并对其测量结果进行改正，以获得精确的定位结果。所不同的是，发送改正数的具体内容不一样，其差分定位精度也不同。

(3)速率陀螺仪。用以直接测定运载器角速率的二自由度陀螺装置。把均衡陀螺仪的外环固定在运载器上并令内环轴垂直于要测量角速率的轴。当运载器连同外环以角速度绕测量轴旋进时，陀螺力矩将迫使内环连同转子一起相对运载器旋进。陀螺仪中有弹簧限制这个相对旋进，而内环的旋进角正比于弹簧的变形量。由平衡时的内环旋进角即可求得陀螺力矩和运载器的角速率。它在远距离测量系统或自动控制、惯性导航平台中使用较多。

由于差分GPS和速率陀螺仪同样存在误差，需要进行滤波处理。

卡尔曼滤波是卡尔曼在1960年提出的从被提取信号有关的观测量中通过算法估计出所需要信号的一种滤波算法。把状态空间的概念引入到随机估计理论中，把信号过程视为白噪声作用下的一个线性系统的输出，用状态空间来描述这种输入-输出关系，估计过程中利用系统状态方程，观测方程和白噪声激励的统计特性形成滤波算法，由于所用的信息都是时域内的量，所以不但可以对平稳的一维随机过程进行估计，也可以对非平稳的、多维随机过程进行估计。

卡尔曼滤波作为一种重要的最优估计理论被广泛应用到实际工程中，比如惯性导航、制导系统、全球定位系统、目标跟踪、通信和信号过程、金融，电机等。

卡尔曼滤波设计方法简单易行，随着其广泛的应用，也暴露出一些问题，尤其是应用到某些具体领域。如它必须在数字计算机上运行，随着微型计算机尤其是嵌入式芯片的普及，人们对卡尔满滤波数值的稳定性、计算效率、实用性和有效性的要求也越来越高。由于计算机的字长有限，使计算机舍入误差和截断误差累积、传递，造成误差方差阵失去对称正定性，数值不稳定。在卡尔曼滤波的发展中，针对这个问题提出了平方根滤波、UD分解滤波、奇异值分解滤波等一系列数值鲁棒的滤波算法。又如传统的卡尔曼滤波是建立在模型精确和随机干扰信号统计特性已知的基础上。对于一个实际系统，往往存在着模型不确定或干扰信号统计特性不完全已知，这些不确定因素使得传统的卡尔曼滤波算法失去最优性，估计精度大大降低，严重时会引起滤波发散。为此，人们将鲁棒控制的思想引入到滤波中，形成了鲁棒滤波理论。

在进行动力定位系统的相关研究中，针对动力定位中实时位置采样普遍采用的卡尔曼滤波进行较为细致的研究。发现在传统的卡尔曼滤波算法中，一般将系统噪声均看作高斯白噪声。由于白噪声信号的方差恒定，故卡尔曼滤波算法中的系统噪声矩阵Q可作为常数矩阵处理。这样做的优点，是使得系统进行卡尔曼滤波迭代时的计算更简便。通常，可以根据海试实验资料选择相应的初值。但上述做法也存在较大的问题，当实际海况比较复杂、系统未知扰动较大时，将系统噪声矩阵Q作为常数矩阵处理，将严重影响卡尔曼滤波的精度。另外，受到海试实验的局限性影响，给定的初始Q矩阵的值可能存在较大误差，将影响卡尔曼滤波的精度和快速性。

发明内容

本发明所要解决的技术问题就是为了克服上述现有技术存在的缺陷而提供一种基于噪声矩阵实时修正的船舶动力定位方法。

本发明的目的可以通过以下技术方案来实现：一种基于噪声矩阵实时修正的船舶动力定位方法，其特征在于，包括以下步骤：

由综合位置测量系统测量船舶本身位置信号，并通过基于噪声矩阵实时修正的卡尔曼数字滤波器滤去上述信号中的噪声信号，并分离出船舶位置和姿态的高、低频信号，并将低频信号传输给控制系统；

由环境测量传感器系统测量船舶所处环境的参数，将测得的环境参数信号传输给控制系统；

控制系统根据滤波后的低频信号及环境信号计算船舶定位控制信号；

推力分配器将计算出的船舶定位控制信号分配给推力系统的各推力器；

各推力器根据船舶定位控制信号，将船舶推动至预期的位置。

所述的综合位置测量系统包括处理器、用于测量艏摇角度的电罗经、用于测量横荡位置和纵荡位置的差分卫星导航系统(GPS)以及用于测量角速度、横荡速度和纵荡速度的速率陀螺仪，所述的电罗经、差分GPS以及速率陀螺仪均与处理器连接，该处理器采用基于噪声矩阵实时修正的卡尔曼数字滤波器进行滤波。

所述的基于噪声矩阵实时修正的卡尔曼数字滤波器通过以下步骤进行滤波：

(1)给定初始值：

$\left(2\right),\hat{x}\left(k\right|k-1)=\stackrel{\‾}{A}\hat{x}(k-1)+\stackrel{\‾}{B}U;$

$\left(3\right),\mathrm{\ω}(k-1)={(H\stackrel{\‾}{E}-\mathrm{\ψ})}^{+}[H\stackrel{\‾}{A}\hat{X}(k-1)+H\stackrel{\‾}{B}U-\tilde{Z}\left(K\right)],$ 通过更新ω(k-1)来更新Q(k-1)；

(4)P(k|k-1)＝AP(k-1)A^T+EQ(k-1)E^T；

(5)K(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R(k)]^-1；

$\left(6\right),\hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+K\left(k\right)[Z\left(k\right)-H\hat{x}\left(k\right|k-1)];$

(7)P(k)＝(I-K(k)H)P(k|k-1)；

(8)k＝k+1，返回到(2)。

所述的处理器采用Atmage8处理器。

与现有技术相比，本发明通过实时计算系统噪声，将计算结果应用于修正噪声序列的方差，使动力定位系统在复杂海况下，仍能保持较好的定位精度，该方法具有一定普遍应用价值，可应用于未知环境(环境扰动未知程度大，统计特性未知)或大扰动环境(环境扰动剧烈，对具有卡尔曼滤波的系统影响较大，明显不能将其看做具有常值方差的白噪声)中的卡尔曼滤波。通过实时更新噪声矩阵，有效克服上述环境的影响，提高滤波精度。

附图说明

图1为本发明的原理图；

图2为本发明的综合位置测量及处理系统的结构示意图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，一种基于噪声矩阵实时修正的船舶动力定位方法，包括以下步骤：

由综合位置测量系统测量船舶本身位置信号，并通过基于噪声矩阵实时修正的卡尔曼数字滤波器滤去上述信号中的噪声信号，并分离出船舶位置和姿态的高、低频信号，并将低频信号传输给控制系统；

由环境测量传感器系统测量船舶所处环境的参数，将测得的环境参数信号传输给控制系统；

控制系统根据滤波后的低频信号及环境信号计算船舶定位控制信号；

推力分配器将计算出的船舶定位控制信号分配给推力系统的各推力器；

各推力器根据船舶定位控制信号，将船舶推动至预期的位置。

如图2所示，综合位置测量系统包括处理器1、用于测量艏摇角度的电罗经2、用于测量横荡位置和纵荡位置的差分GPS3以及用于测量角速度、横荡速度和纵荡速度的陀螺仪4，所述的电罗经2、差分GPS3以及陀螺仪4均与处理器1连接，该处理器1采用基于噪声矩阵实时修正的卡尔曼数字滤波器进行滤波，将电罗经，差分GPS、陀螺仪测得的数据，发送到Atmage8处理器中，处理器内置已经提出的改进型卡尔曼滤波算法，经行滤波处理。将滤波后的信号送船舶动力定位系统的控制器，控制船舶有效实现定位；所述的基于噪声矩阵实时修正的卡尔曼数字滤波器通过以下步骤进行滤波：

(1)给定初始值：

$\left(2\right),\hat{x}\left(k\right|k-1)=\stackrel{\‾}{A}\hat{x}(k-1)+\stackrel{\‾}{B}U;$

$\left(3\right),\mathrm{\ω}(k-1)={(H\stackrel{\‾}{E}-\mathrm{\ψ})}^{+}[H\stackrel{\‾}{A}\hat{X}(k-1)+H\stackrel{\‾}{B}U-\tilde{Z}\left(K\right)],$ 通过更新ω(k-1)来更新Q(k-1)；

(4)P(k|k-1)＝AP(k-1)AT^T+EQ(k-1)E^T；

(5)K(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R(k)]^-1；

$\left(6\right),\hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+K\left(k\right)[Z\left(k\right)-H\hat{x}\left(k\right|k-1)];$

(7)P(k)＝(I-K(k)H)P(k|k-1)；

(8)k＝k+1，返回到(2)。

所述的处理器采用Atmage8处理器，此款单片机芯片具有速度快，实时性好，性价比高的特点，并且其接口等资源满足需要，适合于进行改进型的卡尔曼滤波算法的实现。

当实际海况比较复杂、系统未知扰动较大时，噪声矩阵仍作为常值矩阵处理，将成为桎梏卡尔曼滤波性能的重要因素。针对这种状况，这里提出一种基于噪声矩阵实时修正的卡尔曼滤波方法。该方法通过实时计算系统噪声，将计算结果应用于修正噪声序列的方差。从而提升了卡尔曼滤波在复杂扰动环境下(通常这种环境的扰动不能看做是统计特性已知的白噪声)的滤波精度。在动力定位系统的实际研究中发现，采用此种方法可以使动力定位系统在复杂海况下，仍能保持较好的定位精度。

系统模型如下：

X(k)＝AX(k-1)+BU+Eω(k-1)

Z(k)＝HX(k)+V(k)

其中， $X=[X_L^T,X_H^T]$ 为待估测状态量，表示三个自由度低频和高频的位置及速度状态，U代表推力器产生的控制输入，Z代表船舶低频纵荡、横荡位置和艏摇角度，ω为6×1系统噪声矩阵，V为3×1量测误差矩阵。

假设为测量值，Z′(k)为实际输出值，由于在模型中已假设系统噪声和量测噪声为加性噪声，可以得到

$\tilde{Z}\left(k\right)-Z^{\′}\left(k\right)=\mathrm{HAX}(k-1)+\mathrm{HBU}+\mathrm{HE\ω}(k-1)+V\left(k\right)-\mathrm{HAX}(k-1)-\mathrm{HBU}$

$=\mathrm{HE\ω}(k-1)+V\left(k\right)$

$={\mathrm{\ω}}^{\′}(k-1)+V\left(k\right)---\left(1\right)$

其中，ω′(k-1)＝ψω(k-1)

ω(k-1)左乘ψ矩阵，将每个自由度的系统高频和低频噪声相加，作为总的系统噪声，从而使6维ω(k-1)矩阵转化为3维ω′(k-1)矩阵

Z(k)＝HX(k)+V(k)为量测方程，将系统方程代入可得

Z(k)＝H[AX(k-1)+BU+Eω(k-1)]+V(k)    (2)

由式(1)可得

$V\left(k\right)=\tilde{Z}\left(k\right)-Z^{\′}\left(k\right)-{\mathrm{\ω}}^{\′}(k-1)---\left(3\right)$

将式(3)代入(2)式可得

$Z\left(k\right)=H[\mathrm{AX}(k-1)+\mathrm{BU}+\mathrm{E\ω}(k-1)]+\tilde{Z}\left(k\right)-\mathrm{HAX}(k-1)-\mathrm{HBU}-{\mathrm{\ω}}^{\′}(k-1)---\left(4\right)$

根据卡尔曼滤波算法，可以用

来估计X(k-1)，用 $\hat{Z}\left(k\right)=H\hat{X}\left(k\right)$ 来估计Z(k)。

经过合并移项和化简，更新的系统噪声可以从式(4)得到

$\mathrm{\ω}(k-1)={(\mathrm{HE}-\mathrm{\ψ})}^{+}[\mathrm{HA}\hat{X}(k-1)+\mathrm{HBU}-\tilde{Z}\left(k\right)]---\left(5\right)$

其中，(HE-ψ)⁺为广义逆矩阵。

经过上述步骤，即可得到一步更新的系统噪声值，将当前值与之前所有的历史值作为一个新的噪声序列求方差，即可实现对Q矩阵的实时更新。

根据上述方法，将卡尔曼滤波算法修正如下

(1)给定初始值：

$\left(2\right),\hat{x}\left(k\right|k-1)=\stackrel{\‾}{A}\hat{x}(k-1)+\stackrel{\‾}{B}U;$

$\left(3\right),\mathrm{\ω}(k-1)={(H\stackrel{\‾}{E}-\mathrm{\ψ})}^{+}[H\stackrel{\‾}{A}\hat{X}(k-1)+H\stackrel{\‾}{B}U-\tilde{Z}\left(K\right)],$ 通过更新ω(k-1)来更新Q(k-1)；

(4)P(k|k-1)＝AP(k-1)A^T+EQ(k-1)E^T；

(5)K(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R(k)]^-1；

$\left(6\right),\hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+K\left(k\right)[Z\left(k\right)-H\hat{x}\left(k\right|k-1)];$

(7)P(k)＝(I-K(k)H)P(k|k-1)；

(8)k＝k+1，返回到(2)。

上述方法用c/c++语言编写成程序，实现基于噪声矩阵实时修正的船舶动力定位滤波算法。

本发明由以下三个主要部分组成：测量系统、推力系统、控制系统。测量系统是动力定位系统的基础，没有准确的位置测量信息，便没有位置控制的输入。测量系统分为综合位置、速度测量系统以及环境参数测量系统，位置、速度测量主要采用差分卫星导航系统(DGPS)、陀螺仪、电罗经；环境参数测量通常称其为传感器系统，一般采用风传感器，用来测量风速及攻角。

推力系统是动力定位系统的一个子系统，其基本作用是提供控制系统所需要的推力和力矩，满足定位的需要，推力系统主要由动力机械、如原动机、发电机组、输电设备、推进器、其它辅助设备如管道系统等组成；

控制系统读取位置测量系统所得到的位置信号，将其船舶实际位置与预定的目标值作比较，通过控制规律运算，得到抵消位置偏差和外界干扰力所需要的推力，然后对推力器发出指令，以产生推力使船尽可能靠近所希望的位置。推力分配器是控制系统的一部分，由于一艘动力定位船拥有多个推进器，需要对各推进器行推力分配，推力分配单元计算分配到每个推进单元上的力和方向指令。

Claims (4)

1.一种基于噪声矩阵实时修正的船舶动力定位方法，其特征在于，包括以下步骤：

由综合位置测量系统测量船舶本身位置信号，并通过基于噪声矩阵实时修正的卡尔曼数字滤波器滤去上述信号中的噪声信号，并分离出船舶位置与姿态的高、低频信号，并将低频信号传输给控制系统；

由环境测量传感器系统测量船舶所处环境的参数，将测得的环境参数信号传输给控制系统；

控制系统根据滤波后的低频信号及环境信号计算船舶定位控制信号；

推力分配器将计算出的船舶定位控制信号分配给推力系统的各推力器；

各推力器根据船舶定位控制信号，将船舶推动至预期的位置。

2.根据权利要求1所述的一种基于噪声矩阵实时修正的船舶动力定位方法，其特征在于，所述的综合位置测量系统包括处理器、用于测量艏摇角度的电罗经、用于测量横荡位置和纵荡位置的差分卫星导航系统(GPS)以及用于测量角速度、横荡速度和纵荡速度的速率陀螺仪，所述的电罗经、差分GPS以及速率陀螺仪均与处理器连接，该处理器采用基于噪声矩阵实时修正的卡尔曼数字滤波器进行滤波。

3.根据权利要求1或2所述的一种基于噪声矩阵实时修正的船舶动力定位方法，其特征在于，所述的基于噪声矩阵实时修正的卡尔曼数字滤波器通过以下步骤进行滤波：

(1)给定初始值：

P(0)；

(2) $\hat{x}\left(k\right|k-1)=\stackrel{\‾}{A}\hat{x}(k-1)+\stackrel{\‾}{B}U;$

(3) $\mathrm{\ω}(k-1)={(H\stackrel{\‾}{E}-\mathrm{\ψ})}^{+}[H\stackrel{\‾}{A}\hat{X}(k-1)+H\stackrel{\‾}{B}U-\tilde{Z}\left(k\right)],$ 通过更新ω(k-1)来更新Q(k-1)；

(4)P(k|k-1)＝AP(k-1)A^T+EQ(k-1)E^T；

(5)K(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R(k)]^-1；

(6) $\hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+K\left(k\right)[Z\left(k\right)-H\hat{x}\left(k\right|k-1)];$

(7)P(k)＝(I-K(k)H)P(k|k-1)；

(8)k＝k+1，返回到(2)。

4.根据权利要求3所述的一种基于噪声矩阵实时修正的船舶动力定位方法，其特征在于，所述的处理器采用Atmage8处理器。

Images