一种基于外源卡尔曼滤波的船舶动力定位系统

本发明涉及船舶定位技术领域，尤其涉及一种基于外源卡尔曼滤波的船舶定力定位系统。

背景技术：

近十几年来，世界上许多国家都面临着陆上能源和资源不足的挑战，以往不受人们所重视的海洋开始凭借其丰富的各类资源逐渐进入人们的视野之内，世界上的各大强国也纷纷将能源战略重心转移至海洋领域内。

大力支持发展海洋工程装备及技术，改善海洋工程装备技术的研发环境，能够极大地有助于我国深海资源开发、维护我国领海权益和保障我国领海安全。伴随着海洋各类资源开发利用的渐渐深入与海洋生物化学研究的慢慢开展，海上作业对于船舶定位的精度与稳定性要求也越来越高，然而传统的锚泊定位方式受到作业水深浅、定位精度不高、位置转换不灵活等原因的影响，已然逐渐显露弊端。因此，为了满足克服海上各类复杂多变的环境的要求，提高船舶与平台等海洋结构物的作业定位精度和稳定性，船舶动力定位系统作为一种全新的定位方式被人们开发出来并得到了极大的发展。

由于环境干扰的存在，船舶的位置信息难于测量，会直接影响船舶的控制效果。针对这种情况，设计了一种基于外源信号的卡尔曼滤波器，来估计船舶位置信息。

技术实现要素：

本发明提供一种基于外源卡尔曼滤波的船舶定力定位系统，以克服上述技术问题。

一种基于外源卡尔曼滤波的船舶动力定位系统，包括船舶期望位置设置模块、控制器模块、环境力提取模块、船舶运动低频模块、船舶运动高频模块、外源卡尔曼滤波模块和船舶状态信息模块共七个模块，

船舶期望位置设置模块用于设置船舶的期望位置；

控制器模块用于获取动力定位系统需要输出的控制力和控制力矩、期望位置艏向与实际位置艏向间的偏差；

环境力提取模块用于建立风扰动数学模型、浪扰动数学模型、流扰动数学模型及其他因素扰动数学模型，传递至船舶低频运动模块；

船舶运动低频模块用于建立船舶低频运动模型，该模型中只考虑船舶纵荡、横荡和艏摇；

船舶运动高频模块用于建立船舶高频运动模型，由船舶运动低频模块所建立的船舶运动低频模型和船舶运动高频模块所建立的船舶高频运行模型获得船舶运动总模型；

外源卡尔曼滤波模块用于设计辅助状态观测器和设计卡尔曼滤波器，所述辅助状态估计器用于去除船舶运动总模型的噪声项，所述卡尔曼滤波器采用卡尔曼滤波算法；

船舶状态信息模块用于获取船舶运动信息，包括船舶在固定坐标系中的位移以及艏向位置，环境力，船舶位置和艏摇角角度，并将其返回至控制器模块。

优选地，两阶段估计包括设计辅助状态观测器和设计卡尔曼滤波器，所述辅助状态估计器具有全局稳定性和收敛性，用于去除船舶运动总模型的噪声项，所述卡尔曼滤波器采用卡尔曼滤波算法。

优选地，控制器模块包括pid控制器。

优选地，流扰动数学模型包括两种建模方法：一种是将均匀流对船舶运动的影响简化为船舶速度的变化；另一种是在船舶动力学模型中将海流作为流扰动力。

一种基于外源卡尔曼滤波的船舶动力定位装置，包括基于外源卡尔曼滤波的船舶动力定位系统。

本发明一种基于外源卡尔曼滤波的船舶动力定位系统，对处于复杂海洋噪声中的船舶，能满意的预测出船舶低频信息，并且用于控制器的输入，进而改善控制系统对保持船舶姿态的控制效果。仿真结果验证了外源卡尔曼滤波算法在动力定位系统中具有良好的状态估计效果和滤波精度，对于智能船、无人船等的状态估计极具借鉴价值。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明流程图；

图2是绝对风与相对风示意图；

图3是本发明船舶水平位置；

图4是本发明船舶纵荡位置；

图5是本发明船舶横荡位置；

图6是本发明艏摇角度；

图7是本发明纵荡估计误差；

图8是本发明横荡估计误差；

图9是本发明艏摇估计误差

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明的流程图，如图1所示，本实施例的方法可以包括：

基于外源卡尔曼滤波的船舶动力定位系统，该系统包括船舶期望位置设置模块、控制器模块、环境力提取模块、船舶运动低频模块、船舶运动高频模块、外源卡尔曼滤波模块和船舶状态信息模块共七个模块。

整体流程工作原理说明：

船舶期望位置设置模块用于设置船舶的期望位置；

控制器模块用于获取动力定位系统需要输出的控制力和控制力矩、期望位置艏向与实际位置艏向间的偏差，包括pid控制器，通过pid控制器，计算期望位置艏向与实际位置艏向间的偏差，利用坐标系转置矩阵将模型从北东坐标系变换至船体坐标系，并根据船舶动力定位pid控制公式(1)得到动力定位系统需要输出的控制力，控制律为：

式(1)中：ξ为船舶艏向位置期望值的积分；ξx船舶艏向位置实际值的积分；η为艏向位置期望值；ηx为艏向位置实际值；τ为控制器的输出值，即系统输出的力的大小；kd为pid控制器的微分调节系数；kp控制器的比例调节系数；r(ψ)为船舶运动学方程的旋转矩阵。

船舶在海上运动过程中，为确保动力定位船舶到达或维持在期望位置艏向，主要的作业任务就是如何抵抗风、浪、流以及其他未确定外界海洋环境因素的干扰，环境力提取模块用于建立风扰动数学模型、浪扰动数学模型、流扰动数学模型及其他因素扰动数学模型，传递至船舶低频运动模块。

海面上的风扰动力直接作用于船舶水线以上船体及建筑，导致船舶发生转向、偏航和操纵困难等不良结果，从而影响船舶航行姿态。同时风作用于海水产生海浪，从而对船舶水线以下船体造成一定的影响。环境力提取模块的风扰动数学模型建立过程为：

对于各类海洋环境扰动中的风扰动，采用绝对风与相对风来描述风扰动。如图2所示，绝对风是一种基于惯性坐标系(北东坐标系)的描述风扰动的方式，绝对风通常用ut表示，风向角定义为绝对风与北东坐标系北轴的夹角，通常用ψt表示，规定绝对风风向角为正北方向时为0°；相对风通常用ur表示，相对风向角用αr表示，规定相对风风向与船艏方向一致时风向角αr为0°，并选取顺时针方向为正方向，逆时针方向为负方向，其取值范围为[-180°，180°]。绝对风速度矢量和相对风速度矢量之间的关系，由图2可知。

为船舶航行速度的矢量。

将式(2)中的绝对风速矢量和相对风速矢量进行正交分解投影在船体坐标系下的x坐标与y坐标，得到相对风速的具体计算形式：

式(3)中，ur、vr是相对风速ur在船体坐标系下的x坐标轴与y坐标轴上的两个分量，即ψt为绝对风风向角，u、v为在船体坐标系中船舶速度在x坐标轴与y坐标轴上的两个分量，ψ为船舶艏向角。

相对风向角αr的计算过程为公式(4)：

风扰动作用于船舶水平面三自由度的力和力矩可以用类似非线性阻尼计算模型，平均风扰动的数学模型常表示为：

式(5)中，ρa是空气密度；ur为相对风速；afw、alw分别是船体水下部分的正投影面积和侧面投影面积；loa为船舶总长度；cx(αr)、cy(αr)、cn(αr)为各自由度的风压力系数。

而风压力系数的计算是风扰动模型的关键。风压力系数主要受船体几何特征的影响，船舶运动仿真中，采用简单方便的blendermann公式计算cx(αr)、cy(αr)、cn(αr)三个风压力系数。

式(6)中，cx和cy为风力系数；cn为风力矩系数；ρa为空气密度，单位为kg/m³；afw和alw分别代表正投影面积和侧面投影面积，单位为m²；loa为船舶总长，单位为m；ur为相对风速，单位为节。

在复杂的海洋环境中，波浪干扰力中低频二阶波浪力(波浪漂移力)主要改变船舶航行的航向和轨迹，因此本发明主要对低频二阶干扰力进行建模。浪扰动数学模型的建立过程为：

计算分析船舶在波浪力的作用下的运动状态，采用以下的波能谱公式：

式(7)中，s(ω)的单位为m²·s；ω为浪的频率；g为重力加速度(一般取g＝9.81m/s²)。并且，通过义波高hs，可近似地计算出风速：

为了简化计算，将波浪谱视为多个规则谐波的叠加。单个谐波对船舶的二阶波浪扰动力与力矩使用由diadola提出一种二阶波浪力和力矩的近似计算公式：

式(9)中，ρ为海水密度；g为重力加速度；loa为船舶总长；ψ为船舶艏向角；ζ为波浪幅值的平均值；μ为波浪方向角；λ为波长；cx(λ)、cy(λ)、cn(λ)为波浪漂移力系数与波浪漂移力矩系数。

波浪漂移力系数与波浪漂移力矩系数考虑水深水波时，波长λ计算方式：

λ＝2πω²/g(10)

波浪漂移力系数和波浪漂移力矩系数cx(λ)、cy(λ)、cn(λ)的计算公式：

式(11)中，loa为船舶总长，单位为m。

流扰动是指船舶航行中水流在水平面或垂直方向上海水的流动对船舶运动产生的影响。流扰动数学模型的建立过程为：

对船舶的流扰动影响有两种建模方法：一种是将均匀流对船舶运动的影响简化为船舶速度的变化；另一种是在船舶动力学模型中将海流作为流扰动力。

首先，第一种数学建模方法是速度矢量合成方法，这种方法的主要原理为船舶相对速度和绝对速度之间的转换。针对水面船而言，在北东坐标系下，海流的速度矢量为：

vc＝[vccos(ψc)vcsin(ψc)0]^t(12)

式(12)中，vc为海流流速的大小；ψc为海流流向角。

使用一阶gauss-markov过程进行模拟：

式(13)中，ω为高斯白噪声；μ≥0为常数；且规定ψc,min≤ψc(t)≤ψc,max。

因此，最终该海流速度向量合成模型为：

式(14)中，uc、vc分别为海流流速vc在船体坐标系下的x坐标轴与y坐标轴上的两个分量；ψc为海流流向角；ψ为船舶艏向角。

其次，第二种数学模型是对船舶的海流扰动力和力矩的近似计算：

式(15)中，vc为流速；afw为水线以下船舶正投影面积；asw为水线以下船舶侧投影面积；β为入射角；cx(β)、cy(β)、cn(β)分别表示为沿着x、y方向流向的作用力系数和绕z方向流的作用力矩系数，一般由实验测得。

假设船舶所受环境力模型为上述三种模型的简单叠加。然而，通过验证得知此环境力数学模型与实际应用有所出入，说明上述的建模方式依旧简单，有待做进一步研究。因此本发明中其他因素模型为使用一阶线性船舶环境力微分方程计算船舶所受环境力。

根据gauss-markov定理，在假定海洋环境扰动力在船舶三自由度上均为缓慢变化的条件下，可以用此模型来描述船舶环境力及力矩，求解环境力b即解下列一阶线性微分方程：

式(16)中，t∈r³为包含时间常数的对角矩阵；eb∈r³代表有关环境力幅度范围的系数矩阵；ωb∈r³为高斯白噪声。该模型普遍适用于表示因海风、海浪、海流、二阶波浪漂移力和其它未确定动态扰动引起的缓慢变化的环境力和力矩。

船舶运动低频模块用于建立船舶低频运动模型，在船舶低频运动的模型中，只考虑船舶纵荡、横荡和艏摇的情况下，坐标系选取固定坐标系ox0y0和随船坐标系oxy，经简化模型后研究船舶的三自由度运动。

两坐标系之间的关系如下：

式(17)中，x、y、ψ为船舶运动学参数，即船舶在固定坐标系中的位移以及艏向位置；u、v、r为在随船坐标系中船舶的前进速度、横移速度和转艏角速度；r(ψ)称为船舶运动学方程的旋转矩阵。

则船舶低频三自由度运动时，动力定位系统的操控模型方程为：

其中，关于式(18)中m为惯性矩阵的推导：

船体为刚性系统的惯性矩：

并设三自由度的水动力系统惯性矩阵：

其中为船舶受水动力时的各方向的附加质量，且假设附加重量的中心与重心一致。

因此，叠加后惯性矩阵：

而式(18)中的其他参数：

d(v)为水动力阻尼矩阵，其中-xu、-yv、-yr、-nv、-nr为各自由度对应的水动力系数。

b为环境扰动力；τ为控制力和力矩，即需要最终求解的船舶推进力和力矩；ω为高斯白噪声(零均值)。

船舶运动高频模块用于建立船舶高频运动模型，船舶高频运动模型的建立过程为：

采用附带阻尼的波浪模型来近似表示一阶波浪引起的高频运动。

式(19)中，kωi(i＝1,2,3)为波浪强度相关系数，计算公式为kωi＝2ξiω0iσ；

σ为波强度系数；ζi(i＝1,2,3)为相对阻尼系数，一般取值范围为[0.05,0.2]；ω0i(i＝1,2,3)为波浪谱主导频率；s＝jω代表频率响应的复数形式的虚部。

再令ξh为船舶的高频状态向量，且

令ah、eh、ch为系数矩阵；

a22＝-diag{2ξ1ω012ξ2ω022ξ3ω03}；

其中k＝diag{kω1kω2kω3}；

ch＝[o3×3i]。

ωh为高斯白噪声(零均值)；ηh为高频的位置和角度向量，并且满足ωh、ηh∈r³。

则此模型的状态空间形式为：

由船舶运动低频模块所建立的船舶运动低频模型和船舶运动高频模块所建立的船舶高频运行模型获得船舶的运动总模型，

测量系统测得的船舶信息是船舶位置和艏摇角角度，测量模型为公式(17)和公式(20)中位置向量的叠加，模型为：

y＝η+ηh+νy(21)

νy为高斯白噪声(零均值)。

综上所述，可得船舶运动总模型可以表示为：

ηh＝chξh

y＝η+ηh+νy(22)

外源卡尔曼滤波模块用于对船舶运动总模型进行两阶段估计，设计辅助状态观测器和设计卡尔曼滤波器，通过辅助状态估计器生成船舶运动总模型全局收敛的状态估计，以先行去除噪声项，根据johansen和fossen的外源卡尔曼理论和设计思路，设计一个估计器,此前已有专家证明此估计器的全局稳定性和收敛性。

上述(22)中的总系统船舶航行模型去除噪声项后，得：

式中：为估计误差；t＞0为可调标量；皆是上述船舶总模型中运动学和动力学参数的估计值。

k1、k2、k3、k4为增益矩阵。k2＝diag{k31k32k33}，k4＝diag{k41k42k43}，k3＝0.1k4；

在各增益矩阵中，k1i＝-2ωci(ζni-ζi)/ωoi，k2i＝-2ωci(ζni-ζi)，k3i＝ωci。

设计卡尔曼滤波器的过程为：

卡尔曼滤波算法寻优过程大体可分成两步：首先，设置初始时刻k，并根据k-1时刻的状态最优估计值，估计出当前时刻k的状态先验估计值以及先验估计协方差；然后，由当前时刻k的状态测量值校正当前时刻k的状态先验估计值，得出当前时刻k的后验估计值，即当前时刻k的状态最优估计值，同时更新当前时刻k的后验估计协方差。

根据船舶运动总模型式(22)和卡尔曼滤波设计步骤，其中具体过程如下：

一、选择初值

二、运用时间更新方程得出先验估计值：

xk-1|k＝φxk-1+buk-1(28)

式(28)中，xk-1k为k时刻系统状态先验估计值；xk-1为k-1时刻系统后验估计值(系统在k-1时刻的最优估计值)；uk-1为k-1时刻控制输入量；φ、b皆为状态转移矩阵。

三、运用时间更新方程得出先验预测误差协方差：

pk-1|k＝φpk-1φ^t+γqkγ^t(29)

式(29)中，pk-1k为k时刻先验估计误差协方差；pk-1为k-1时刻后验估计误差协方差；

qk为k时刻零均值高斯白噪声矩阵；φ为状态转移矩阵；γ模型相关系数矩阵。

四、预测系统的测量值：

式(30)中，为k时刻系统测量值的预测值；h测量系统模型的转换矩阵。

五、计算系统实际测量值和预测出的系统测量值之间的偏差：

式(31)中,yk为k时刻系统测量出的实际值，由系统传感器直接测得；zk为偏差。

六、卡尔曼增益系数：

kk＝pk-1|kh^t[hpk-1|kh^t+rk]^-1(32)

式(32)中，kk为滤波增益矩阵；rk为噪声矩阵。

七、运用时间更新方程得出当前状态最优的后验估计值：

xk＝xk-1|k+kk[zk-hxk-1|k](33)

八、后验估计误差协方差：

pk＝[i-kkh]pk-1|k[i-kkh]^t+kkrkkk^t(34)

九、保存并输出k时刻下的最优估计值(后验估计值)xk与后验估计误差协方差pk，接着令k＝k+1，并判断是否结束滤波，若继续则重返至第二步继续执行循环，持续更新估计值和协方差。

十、结束滤波算法。

船舶状态信息模块用于获取船舶运动信息并将其返回至pid控制器。

为验证外源卡尔曼滤波器的有效性，本发明以northernclipper船舶为仿真对象进行了验证，船长l＝76.2m，重量m＝4.591×10⁶kg，船舶的惯性矩阵和阻尼矩阵为：

初始位置设为[0m,0m,0°]，期望位置设为[5m,5m,10°]。

仿真结果中，图3为船舶纵荡和横荡位置在地球惯性坐标系下的变化情况，具体描述了船舶在海平面上由初始坐标(0，0)最终稳定在期望坐标(5，5)处，说明在所设计的状态估计器和控制闭环系统下，船舶具有满意的定位效果。

图4、5、6的虚线段分别描述了船舶纵荡、横荡和艏摇的船舶综合位置变化情况，即测量位置信号始终在剧烈震荡，说明船舶所处的海洋环境干扰较大；而经过外源卡尔曼滤波后的船舶纵荡、横荡和艏摇的估计位置信号为实线段所表示，可以看出滤波器估计位置输出平稳，说明所设计的滤波器能很好地滤除海洋环境噪声，并能估计船舶低频信息，同时平稳的估计位置信息传给控制器后，也保证了控制器的控制效果的满意度。

图7、8、9分别描述了所设计滤波器对船舶三自由度的状态估计误差，可以看出纵荡、横荡相对船舶真实位置的误差基本稳定于1米以内，艏摇角度相对船舶真实位置的误差稳定于1.5°以内，说明所设计的外源卡尔曼滤波的估计精度良好并且稳定。

结论：仿真结果也同样验证了外源卡尔曼滤波算法在动力定位系统中具有良好的状态估计效果和滤波精度。

整体有的有益效果：通过研究船舶运动状态的滤波估计方法，设计了一种基于外源信号的卡尔曼估计器，即设计一个辅助状态估计器，将估计后的信号都进行卡尔曼滤波，对船舶动力定位系统位置信息进行两阶段估计。最终，仿真结果表明，对处于复杂海洋噪声中的船舶，外源卡尔曼滤波能满意的预测出船舶低频信息，并且用于控制器的输入，进而改善控制系统对保持船舶姿态的控制效果。仿真结果验证了外源卡尔曼滤波算法在动力定位系统中具有良好的状态估计效果和滤波精度，对于智能船、无人船等的状态估计极具借鉴价值。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。