船舶运动控制系统的Kalman三自由度解耦滤波方法与流程

本发明属于船舶运动控制技术领域，具体涉及一种船舶运动控制系统的kalman三自由度解耦滤波方法。

背景技术：

当今全球各国的经济正在高速的发展中，各国对能源的消耗也在与日俱增，众多科学家早已将目光投向具有丰富自然矿藏资源的海洋领域。面对海洋环境的复杂多变，必须使得船舶拥有先进的控制系统，因此一个新的系泊方式—动力定位系统诞生了。动力定位系统可以使船舶在深水海域以及复杂环境海域下保持自身位置的恒定以及运动航迹的稳定，同时可以迅速进入以及离开工程地点。

dp船舶在海洋中运动时，周围海洋环境的扰动会明显地影响船舶的运动与控制，而且扰动具有较强的波动性和随机性，其对于研究船舶的运动以及响应特性来说，会产生一定的干扰。船舶运动过程中，主要影响船舶运动的环境因素有风、浪、流，其中海浪导致引起产生的一阶波浪力会导致船舶发生高频运动，并且这种运动是均值为零的往复运动，船舶不应抵抗的运动，如果船舶抵抗响应这种高频运动就会引起很多不必要的控制，进而使得船舶的推进系统会不断的响应这种高频运动，加速推进系统的磨损老化，消耗过多的能量。

如果在dp控制系统中加入了合适的滤波器，滤波器能将位置及航向信号中的的高频运动滤除并估计，同时保留低频信号，并估计船舶的实际位置，从而计算出测量系统中的高斯白噪声，使船舶仅仅响应低频运动的扰动，必然会提升系统的控制以响应能力，同时减少不必要的推力输出和推进器回转，提升动力定位系统的能力。

技术实现要素：

本发明的目的在于提供实现船舶的高精度运动滤波控制的一种船舶运动控制系统的kalman三自由度解耦滤波方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：依据dp船舶的船舶模型获取无因次质量矩阵m和阻尼矩阵d；

步骤2：依据dp船舶在港口不运动时获取的数据记录的样本协方差，获取测量噪声协方差矩阵r；

其中，表示第i个传感器的测量噪声的协方差，通过船舶在港口不运动时获取的数据记录的样本协方差来估计；

步骤3：设定具有正可调参数的对角线的状态噪声协方差q；

步骤4：设置误差协方差矩阵的初始值为p0＝i15×15，通过dp船舶实际位置确定初始位置x0；

步骤5：依据海浪强度的参数、阻尼系数、主导海浪频率及船舶输出推力矩计算确定偏差矩阵b、矩阵e和矩阵a；

其中：

i3×3为3阶单位矩阵；

步骤6：依据dp船舶需要滤除一阶波浪力干扰的要求，确定矩阵h；

h＝[cωi3×303×303×3]

cω＝[03×3i3×3]

步骤7：依据连续型kalman滤波方法求解总的kalman增益，并进行解耦获得单自由度kalman增益；

步骤8：将单自由度的kalman增益与对应的单自由度相乘，获得相应的过程噪声向量；

步骤9：依据矩阵a、dp船舶动力学模型和过程噪声向量，重组船舶的运动结构，获取船舶的估计运动位置和一阶波浪力干扰；

步骤10：将传感器测量的dp船舶运动位置与估计运动位置和一阶波浪力干扰之和相减，获得每个自由度的等待与下一次的单自由度的kalman增益相乘；

步骤11：依据每一时刻dp船舶的输出推力(矩)和传感器测量的船舶位置信息，使用连续型kalman滤波算法迭代计算相关矩阵，确定kalman增益，重组船舶运动结构，获得船舶的估计运动位置和一阶波浪力；在连续型kalman滤波器中，状态估计迭代式和误差协方差迭代式如下：

初始条件：p(0)＝p0；

其中，y＝hx+υ；y为传感器测量出的纵荡、横荡位置及航向角；x为状态变量，ηp＝r^t(ψ)η；η为在北东坐标系下dp船舶三自由度向量，η＝[neψ]^t，分别表示纵荡、横荡和艏摇；r(ψ)为转换矩阵，具体为：

v为船体坐标系下的速度向量，v＝[uvr]^t；bp＝r^t(ψ)b。

本发明的有益效果在于：

本发明设计了一种船舶运动控制系统的kalman三自由度解耦滤波方法，利用dp控制系统获取的传感器测量信息和连续型kalman滤波算法，根据dp船舶的船舶模型和海洋环境参数，设计了kalman三自由度解耦滤波方法，对船舶的运动信息进行重组和计算，实现位置及航向信息的滤波，并估计出dp船舶的实际位置、航向及一阶波浪力干扰力。通过仿真实现，证明本发明滤波效果明显，有效地消除了一阶波浪力的干扰。本发明对于dp船舶的运动控制滤波来说具有重要的研究意义和工程价值，有利于降低船舶运动控制器的设计难度，减少推进系统的无效推力输出及旋转。

附图说明

图1为三自由度kalman增益计算及其解耦仿真图。

图2为每个单自由度的滤波方法原理图。

图3为单自由度的滤波方法仿真图。

图4为dp船舶运动控制滤波的仿真图。

图5为利用matlab仿真得到的北向位置滤波估计曲线图。

图6为利用matlab仿真得到的东向位置滤波估计曲线图。

图7为利用matlab仿真得到的航向角度滤波估计曲线图。

图8为利用matlab仿真得到的100s至150s航向角度滤波估计放大曲线图。

图9为利用matlab仿真得到的一阶波浪力估计曲线图。

具体实施方式

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

本发明涉及一种船舶运动控制系统的kalman三自由度解耦滤波方法，特别是在dp船舶在海洋中进行定位时，利用dp控制系统获取的测量信息，基于kalman三自由度解耦的滤波器对测量的位置及航向信息进行滤波，估计出dp船舶的实际位置及航向。本发明充分利用dp船舶获取的船舶位置及航向的测量信息，使用dp船舶的线性模型、连续kalman滤波算法和相关滤波器参数的设定，计算出dp船舶三自由度的kalman增益，进行解耦得出纵荡、横荡及航向上的kalman增益，实现单自由度上的滤波及控制，重组dp船舶的运动，估计并滤除一阶高频波浪力，使dp船舶抵抗船舶的低频运动，实现kalman三自由度解耦滤波，有利于dp船舶精准的定位控制，减少船舶推进系统不必要的控制响应。

一种船舶运动控制系统的kalman三自由度解耦滤波方法，包括以下步骤：

步骤1：依据dp船舶的船舶模型获取无因次质量矩阵m和阻尼矩阵d；

步骤2：依据dp船舶在港口不运动时获取的数据记录的样本协方差，获取测量噪声协方差矩阵r；

其中，表示第i个传感器的测量噪声的协方差，通过船舶在港口不运动时获取的数据记录的样本协方差来估计；

步骤3：设定具有正可调参数的对角线的状态噪声协方差q；

步骤4：设置误差协方差矩阵的初始值为p0＝i15×15，通过dp船舶实际位置确定初始位置x0；

步骤5：依据海浪强度的参数、阻尼系数、主导海浪频率及船舶输出推力矩计算确定偏差矩阵b、矩阵e和矩阵a；

其中：

i3×3为3阶单位矩阵；

步骤6：依据dp船舶需要滤除一阶波浪力干扰的要求，确定矩阵h；

h＝[cωi3×303×303×3]

cω＝[03×3i3×3]

步骤7：依据连续型kalman滤波方法求解总的kalman增益，并进行解耦获得单自由度kalman增益；

步骤8：将单自由度的kalman增益与对应的单自由度相乘，获得相应的过程噪声向量；

步骤9：依据矩阵a、dp船舶动力学模型和过程噪声向量，重组船舶的运动结构，获取船舶的估计运动位置和一阶波浪力干扰；

步骤10：将传感器测量的dp船舶运动位置与估计运动位置和一阶波浪力干扰之和相减，获得每个自由度的等待与下一次的单自由度的kalman增益相乘；

步骤11：依据每一时刻dp船舶的输出推力(矩)和传感器测量的船舶位置信息，使用连续型kalman滤波算法迭代计算相关矩阵，确定kalman增益，重组船舶运动结构，获得船舶的估计运动位置和一阶波浪力；在连续型kalman滤波器中，状态估计迭代式和误差协方差迭代式如下：

初始条件：p(0)＝p0；

其中，y＝hx+υ；y为传感器测量出的纵荡、横荡位置及航向角；x为状态变量，ηp＝r^t(ψ)η；η为在北东坐标系下dp船舶三自由度向量，η＝[neψ]^t，分别表示纵荡、横荡和艏摇；r(ψ)为转换矩阵，具体为：

v为船体坐标系下的速度向量，v＝[uvr]^t；bp＝r^t(ψ)b。

本发明设计了一种船舶运动控制系统的kalman三自由度解耦滤波方法，利用dp控制系统获取的传感器测量信息和连续型kalman滤波算法，根据dp船舶的船舶模型和海洋环境参数，设计了kalman三自由度解耦滤波方法，对船舶的运动信息进行重组和计算，实现位置及航向信息的滤波，并估计出dp船舶的实际位置、航向及一阶波浪力干扰力。在matlab中进行了仿真验证，其滤波效果明显，有效地消除了一阶波浪力的干扰，证明了一种船舶运动控制系统的kalman三自由度解耦滤波方法的有效性和真实性。本发明对于dp船舶的运动控制滤波来说具有重要的研究意义和工程价值，有利于降低船舶运动控制器的设计难度，减少推进系统的无效推力输出及旋转。

本发明的推导步骤如下：

第一步，对于dp船舶动力定位系统来说，一般来说只考虑水面上的三个自由度(纵荡、横荡、艏摇)。选择两个dp系统常用的参考系，北东坐标系和船体坐标系，定义在北东坐标系下船舶三自由度向量η和船体坐标系下的速度向量v分别为：

η＝[neψ]^t(1)

v＝[uvr]^t(2)

两坐标系的转换关系如下：

式中：r(ψ)为转换矩阵。

第二步，由于dp系统以恒定的航向或缓慢的转弯速度运行，可使用以下假设：恒定航向，艏摇速率为零(r＝0)，因此

因此，使用船舶平行坐标来表示船舶dp模型，有下式成立：

ηp＝r^t(ψ)η(5)

bp＝r^t(ψ)b(6)

所以运动学模型可以近似为线性模型：

选用dp的线性运动模型为：

将公式(8)转化时域空间下，并定义和xω2＝yω作为状态变量的域，可以从中获取线性状态的空间模型。

式中：m为包括了附加质量的运动系统的惯性矩阵；d为代表了阻尼系数的矩阵；τ为船舶的推进系统输出的力及力矩矢量矩阵；bp为力(矩)的偏差向量；tb为含有正偏差时间常数的对角矩阵；ωi(i＝1,2,3)为过程噪声变量。

第三步，建立线性连续时间系统的状态空间方程和测量方程(传感器系统)为：

y＝hx+υ(14)

如果上述系统方程是可观测的，状态向量x∈rⁿ可以是通过测量向量y∈rⁿ和控制输入向量u∈rⁿ重新递推求解计算的。

连续时间的kalman滤波算法如下：

设计矩阵：q(t)＝q^t(t)＞0(通常为常数)(15)

r(t)＝r^t(t)＞0(通常为常数)(16)

初始条件：

卡尔曼增益矩阵迭代式：k(t)＝p(t)h^t(t)r^-1(t)(19)

状态估计迭代式：

误差协方差迭代式：

第四步，选用作为公式(13)的状态变量，y为传感器测量出的纵荡、横荡位置及航向角。可得矩阵a、b、e和h为：

h＝[cωi3×303×303×3](25)

系统矩阵中aω、eω和eω的设定如下：

cω＝[03×3i3×3](28)

式中，ω0为波浪pm谱中的主导海洋频率；λ为相对阻尼系数；σ为与波浪的强度相关的常数值。

第五步，在连续型kalman滤波器中，状态估计迭代式和误差协方差迭代式如下：

式中，协方差矩阵q＝q^t∈r^9×9和测量方差矩阵r＝r^t∈r^3×3必须由使用者指定；其中测量协方差矩阵r的的方法可以使用下述矩阵：

式中，表示第i个传感器的测量噪声的协方差，它可以通过船舶在港口不运动时获取的数据记录的样本协方差来估计。

一般来说，状态噪声协方差q可以选择为具有正可调参数的对角线。这些通常是通过试验和错误发现的。将这个协方差矩阵选为对角矩阵，即：

矩阵q1是噪声ω1的协方差，用来驱动代表线性波浪运动的噪声滤波器，其可与船舶wf运动模型的参数一起根据船舶运行前和运行中测量的数据进行估计；q2是噪声ω2的协方差，代表运动方程中的不确定性，同时q2也被视为位置测量噪声方差的一部分；q3是噪声ω3的协方差，代表了模拟其他环境力量的偏差项的不确定性，q3可以给滤波器提供模式各部分不确定性的适当平衡。

第六步，根据以上方法计算出的kalman增益k是15×1的矩阵，如状态估计迭代矩阵公式(29)所示，这里对其进行解耦，将三自由度分开进行运算，纵荡、横荡和艏摇分别进行单独的滤波和状态估计。即：将计算出的kalman增益k分成15组变量，第1、第4、第7、第10、第13，5个变量重新组成一组作为纵荡上的kalman增益kn；第2、第5、第8、第11、第14，5个变量重新组成一组作为横荡上的kalman增益ke；第3、第6、第9、第12、第15，5个变量重新组成一组作为艏摇上的kalman增益kp，再将三个增益kn、ke、kp分别与计算出的纵荡、横荡、艏摇上的噪声相乘，从而将三自由度解耦成3个单自由度来进行滤波，其三自由度kalman增益计算及其解耦仿真图如图1所示，此方法极大的简化了计算过程以及简便了程序设计的难度。

第七步，每个单自由度的滤波方法如下如图2所示，由此可以获得船舶的实际位置信息、三自由度上的速度(角速度)以及一阶波浪力估计结果。

由图2可知，在单自由度的滤波过程中，由于船舶模型的原因，运动重组过程中需要进行三自由度的合成，再进行矩阵运算，然后解耦，进行矩阵运算的相关矩阵为m^-1、d^-1、r^-1(ψ)和r(ψ)，从而进行船舶运动状态的重组以及一阶高频运动的估计，滤除单自由度上的一阶高频运动，输出单自由度上的低频运动，得出测量系统中的估计高斯白噪声，实现单自由度的滤波过程，其仿真图如图3所示。

将三个单自由度上的位置、航向和速度(角速度)信息进行合成，输入到控制器中，作为dp船舶实际运动的信息进行控制计算。

本发明方法给出了船舶运动控制系统的kalman三自由度解耦滤波方法，根据发明步骤并对其进行仿真验证，对于dp船舶的高精度运动控制具有重要的理论意义和工程价值。

对本发明有益的说明：

1.仿真条件设置

仿真条件下，设海浪强度的参数为0.5、阻尼系数为0.1和主导海浪频率为0.8，为已知量，dp船舶的船舶模型的无因次质量矩阵m和阻尼矩阵d为：

误差协方差矩阵的初始值为p0＝i15×15，测量噪声协方差矩阵r、偏差矩阵b以及状态噪声协方差矩阵q1、q2和q3如下所示：

设定初始位置为北东坐标系下，纵荡位置为0m，横荡位置为0m，航向角为0°，仿真时间为200s。

2.仿真实验过程

根据dp船舶运动控制滤波的仿真模型在matlab中进行仿真分析，具体过程如下：

将上述参数输入到连续型kalman滤波器中，再simulink中进行dp船舶控制系统的滤波和状态估计仿真分析，dp船舶运动控制滤波的仿真图如图4所示。

设定北东坐标系下，期望纵荡位置为0m，期望横荡位置为0m，期望航向角为20°。

将期望位置、航向及估计速度输入到dp船舶控制器中，通过pid控制器计算出dp船舶所需三自由度推力(矩)。

将推力(矩)输入到dp船舶的动力学模型中，计算出船舶的三自由度运动位置信息，加上均值为零的一阶波浪力干扰，作为传感器获得的测量信息。

将传感器获得的测量信息输入到连续型kalman滤波器中，经过上述方法计算，可得估计的dp船舶实际位置、航向及估计的一阶波浪力干扰。

将估计的dp船舶实际位置和运动速度反馈到dp船舶控制器中，进行下一次计算。

将滤波前后dp船舶的运动状态及估计的一阶波浪力进行保存，输出仿真曲线。

将根据滤波前dp船舶的位置、航向及一阶波浪力的仿真曲线进行对比及结果分析，得出结论。

3.仿真实验结果

上述仿真结果表明：

(1)本发明一种船舶运动控制系统的kalman三自由度解耦滤波方法可以在纵荡、横荡及航向上有效的滤除一阶波浪力的干扰，估计出dp船舶的实际运动情况。

(2)本发明一种船舶运动控制系统的kalman三自由度解耦滤波方法可以滤除超90％的一阶波浪力，估计的一阶波浪力与实际的一阶波浪力基本吻合。

(3)本发明一种船舶运动控制系统的kalman三自由度解耦滤波方法可以获得实际的dp船舶运动情况，有利于船舶精准的定位控制，减少推进器的无效响应。

本发明的具体实施方式为：

第一步，依据dp船舶的船舶模型获取无因次质量矩阵m和阻尼矩阵d。

第二步，依据dp船舶在港口不运动时获取的数据记录的样本协方差来估计矩阵r；

第三步，通过试验和错误发现具有正可调参数的对角线的状态噪声协方差q，一般取值如下：

第四步，设置误差协方差矩阵的初始值为p0＝i15×15，通过dp船舶实际位置确定初始位置x0；

第五步，依据海浪强度的参数、阻尼系数、主导海浪频率及船舶输出推力(矩)计算确定偏差矩阵b、矩阵eqe^t和矩阵a；

第六步，依据dp船舶需要滤除一阶波浪力干扰的要求，确定矩阵h；

第七步，依据连续型kalman滤波方法求解总的kalman增益，并进行解耦获得单自由度kalman增益；

第八步，将单自由度的kalman增益与对应的单自由度相乘，获得相应的过程噪声向量，

第九步，依据矩阵a、dp船舶动力学模型公式(8)至(11)和过程噪声向量，重组船舶的运动结构，获取船舶的估计运动位置和一阶波浪力干扰；

第十步，将传感器测量的dp船舶运动位置与估计运动位置和一阶波浪力干扰之和相减，获得每个自由度的等待与下一次的单自由度的kalman增益相乘；

第十一步，依据每一时刻dp船舶的输出推力(矩)和传感器测量的船舶位置信息，使用连续型kalman滤波算法迭代计算相关矩阵，确定kalman增益，重组船舶运动结构，获得船舶的估计运动位置和一阶波浪力。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。