CN111061262B - 一种减少无效操舵的航向控制方法 - Google Patents

Abstract

本发明公开了一种减少无效操舵的航向控制方法，先进行计算广谱海浪滤波器系数步骤，解算滤波控制分量步骤需要的系数矩阵，再进行卡尔曼滤波状态量计算步骤，其结果提供计算基本控制量和滤波控制分量步骤，由于基本控制量和滤波控制分量同时计算，可避免采用带阻滤波器由于先滤波再参与控制量计算引起的大相位滞后，因此，在滤波时能仍保证船舶航向控制系统的控制精度和稳定性；计算得到的最终方向舵控制量送入舵机控制单元后，在风浪干扰下能以较低的打舵频率实现对航向的有效控制，可降低船舶舵机设备机械磨损。

Description

一种减少无效操舵的航向控制方法

技术领域

本发明属于船舶自动操纵与控制技术领域，具体涉及一种减少无效操舵的航向控制方法。

背景技术

船舶在风浪中航行时，在海浪作用下产生高频艏摇运动。为克服艏摇，常规控制方法是方向舵频繁左右打舵，由于艏摇频率远远高于船舶控制系统闭环带宽，因此，方向舵左右打舵无法抑制高频的艏摇运动，不能产生有效的控制效果，在此控制过程中存在大量无效操舵。频繁打舵引起的速降增加了燃油消耗，引起的舵机磨损严重影响机械设备的使用寿命，因此，风浪干扰下的无效操舵问题是船舶操纵控制领域亟需解决的技术难题之一。

现役船舶操纵控制系统中通常采用带阻滤波器或者扩展卡尔曼滤波器的方法减小风浪干扰下的无效操舵。当采用带阻滤波器时，测量的航向信号经过带阻滤波器滤除海浪干扰后，再进行控制器需要的状态量的解算，最后由控制器计算指令舵角，由于带阻滤波器和航向微分器以串联方式工作往往会产生较大的相位滞后，严重影响航向控制品质，有时会造成控制系统失稳；采用扩展卡尔曼滤波器时，首先进行海浪谱辨识，再根据辨识的海浪谱构建扩展卡尔曼滤波器，卡尔曼滤波器在解算控制器需要的状态量的同时实现了海浪滤波，与带阻滤波器相比，相位滞后较小，但当海况较复杂时辨识的海浪谱与真实海浪谱间存在较大差异，造成滤波效果不理想，有时仍存在较多无效操舵。

因此，上述问题导致的无效操舵，同行从业人员亟待解决。

发明内容

鉴于上述问题，本发明提供了一种船舶航向控制方法，解决现有船舶航向滤波方法存在的稳定性差、滤波效果不佳的问题，可达到在风浪干扰下减少船舶无效操舵，降低燃油消耗，同时降低舵机设备机械磨损的目的。

本发明实施例提供一种减少无效操舵的航向控制方法，包括：

S1、采用广谱海浪滤波器，计算广谱海浪滤波器系数；所述广谱海浪滤波器系数的计算结果包含：卡尔曼滤波器增益矩阵和控制器系数矩阵；

S2、采用卡尔曼滤波器，并根据所述卡尔曼滤波器增益矩阵，进行卡尔曼滤波状态量计算；

S3、根据所述控制器系数矩阵及所述卡尔曼滤波状态量，进行基本控制量和滤波控制分量的计算；

S4、将所述基本控制量与所述滤波控制分量相加，获得最终方向舵控制量。

在一个实施例中，所述步骤S1包括：

(1.1)计算卡尔曼滤波器增益矩阵H_3×1、控制器系数矩阵M_1×3和偏航增益系数N：

将矩阵A_3×3、B_3×1、C_1×3以及QN和RN代入黎卡提方程，解得卡尔曼滤波器增益矩阵H_3×1；其中，A_3×3为船舶线性运动方程系统矩阵，B_3×1为船舶线性运动方程输入矩阵，C_1×3为船舶线性运动方程输出矩阵，均为所涉船舶的设计数据；QN为模型噪声协方差，RN为测量噪声协方差；

将矩阵A_3×3、B_3×1、C_1×3、Q_4×4和R代入黎卡提方程，解得控制器系数矩阵M_1×3和偏航增益系数N,其中，Q_4×4为控制误差加权系数矩阵，R为能量损耗加权系数；

(1.2)计算传递函数矩阵T(s)：

其中，s为拉普拉斯算子，I_4×4为单位矩阵；

(1.3)计算频率响应值：

滤波频带为0.5rad/s～1.2rad/s，以覆盖海浪频率，在滤波频带内选取两个频率点ω₁，ω₂，计算相应的频率响应值F(jω₁)，F(jω₂)：

F(jω₁)＝{T₁₂(jω₁)+[-T₁₁(jω₁)]T₂₁ ^-1(jω₁)T₂₂(jω₁)}^-1×[-T₁₁(jω₁)]T₂₁ ^-1(jω₁)，

F(jω₂)＝{T₁₂(jω₂)+[-T₁₁(jω₂)]T₂₁ ^-1(jω₂)T₂₂(jω₂)}^-1×[-T₁₁(jω₂)]T₂₁ ^-1(jω₂)；

(1.4)计算广谱海浪滤波器系统矩阵：

配置广谱海浪滤波器极点为λ₁＝λ₂＝λ₃＝λ₄＝λ₅＝-λ，0.1＜λ＜10，得到能控标准型的广谱海浪滤波器系统矩阵α_5×5：

(1.5)计算广谱海浪滤波器输入矩阵：

将所述F(jω₁)、F(jω₂)、μ_1×5、α_5×5、ω₁和ω₂代入下述复数方程组：

F(jω₁)＝μ_1×5(jω₁I_5×5-α_5×5)^-1β_5×1，

F(jω₂)＝μ_1×5(jω₂I_5×5-α_5×5)^-1β_5×1；

得到广谱海浪滤波器输入矩阵β_5×1＝[0β₁β₂β₃β₄]；其中，μ_1×5＝[0 1 0 0 0]。

在一个实施例中，所述步骤S2，包括：

(2.1)卡尔曼滤波器状态量z_(k)初始化：

卡尔曼滤波器状态量z_(k)为：

z_(k)＝[z_1(k) z_2(k) z_3(k)]^T

其中，z_1(k)为k时刻的横向速度估计值，z_2(k)为k时刻的航向角角速度估计值，z_3(k)为k时刻的航向角估计值，T表示对矩阵转置；k＝0、1、2、…n，表示自然数，各时刻之间的间距h＝0.01～1秒；

将卡尔曼滤波器状态量z_(k)初始化为z₍₀₎，

为系统开机时刻测量的航向角；/>

(2.2)根据所述船舶线性运动方程系统矩阵A_3×3，船舶线性运动方程输入矩阵B_3×1，卡尔曼滤波器增益矩阵H_3×1，船舶线性运动方程输出矩阵C_1×3，计算k 时刻卡尔曼滤波器状态量的微分

其中，δ_(k)为k时刻测量的方向舵舵角，

为k时刻测量的航向角，k-1表示上一时刻；

(2.3)计算k时刻卡尔曼滤波器状态量z_(k)：

在一个实施例中，所述步骤S3，包括：

(3.1)计算基本控制量

其中，/>

为指令航向；

(3.2)设定广谱海浪滤波器状态量p_(k)的初值p₍₀₎＝[0 0 0 0]^T；

(3.3)计算p_(k)的微分：

(3.4)计算广谱海浪滤波器状态量

(3.5)计算k时刻的滤波控制分量γ_(k)：γ_(k)＝μ_1×5p_(k)。

在一个实施例中，所述步骤S4，包括：

计算最终方向舵控制量

并将其送入舵机控制单元；

通过不断接收传感器系统实时测量的航向

方向舵角δ_(k)，循环运行步骤S2～S4，产生实时的方向舵控制量/>

在一个实施例中，所述步骤(1.1)包括：

(A)卡尔曼滤波器增益矩阵H_3×1由Matlab控制系统工具箱中的指令 kalman(sys,Qn,Rn,Nn)解得，其过程为：

由矩阵A_3×3、B_3×1、C_1×3搭建线性系统sys的系数矩阵：

sys.a＝A_3×3，sys.b＝[B_3×1[0 1 0]^T]，sys.c＝C_1×3，sys.d＝[0 0]，

将sys、QN、RN和Nn代入Matlab的所述指令kalman(sys,Qn,Rn,Nn)中，解得增益矩阵H_3×1；

其中模型噪声协方差QN取值范围为0.1～10，测量噪声协方差RN取值范围为100～2000，Nn＝0；

(B)控制器系数矩阵M_1×3和偏航增益系数N由Matlab控制系统工具箱中的指令lqr(AA,BB,Q,R,N)解得，其过程为：

将矩阵

BB＝[0 0 0 1]^T，Q_4×4,R和N代入Matlab的指令lqr(AA,BB,Q,R,N)，解得增益矩阵K_1×4＝[k₁ k₂ k₃ k₄]；

其中，

控制误差加权系数矩阵Q_4×4为对角矩阵，各元素取值范围均为0～10，能量损耗加权系数R取值范围为0.001～1， N＝0；选取原则为：要提高控制的快速响应特性，则增大Q_4×4中相应元素的取值，要减小控制量的幅值及其引起的能量消耗，则增大R的取值；

计算控制器系数矩阵M_1×3和偏航增益系数N：

M_1×3＝[m₁ m₂ m₃]，N＝k₃；

其中，

m₃＝k₂。

在一个实施例中，所述步骤(1.3)中，在滤波频带内选取两个频率点ω₁、ω₂时，先选择ω₁＝0.5，ω₂＝0.55，观察传递函数矩阵T(s)的第一个输入到第一个输出的幅频响应特性，当幅频响应值减小的频率范围未覆盖滤波频带时，增大ω₂的取值，直到幅频响应值减小的频率范围能够覆盖滤波频带0.5rad/s～ 1.2rad/s。

本发明实施例提供的上述技术方案的有益效果至少包括：

本发明实施例提供的一种减少无效操舵的航向控制方法，先进行计算广谱海浪滤波器系数步骤，解算滤波控制分量步骤需要的系数矩阵，再进行卡尔曼滤波状态量计算步骤，其结果提供计算基本控制量和滤波控制分量步骤，由于基本控制量和滤波控制分量同时计算，可避免采用带阻滤波器由于先滤波再参与控制量计算引起的大相位滞后，因此，在滤波时能仍保证船舶航向控制系统的控制精度和稳定性；计算得到的最终方向舵控制量送入舵机控制单元后，在风浪干扰下能以较低的打舵频率实现对航向的有效控制，可降低船舶舵机设备机械磨损。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明实施例提供的减少无效操舵的航向控制方法流程图；

图2为本发明实施例提供的传递函数矩阵结构图；

图3为本发明实施例提供的控制系统幅频响应特性曲线图(ω₂＝0.55)；

图4为本发明实施例提供的控制系统幅频响应特性曲线图(ω₂＝0.76)；

图5为本发明实施例提供的海浪波谱密度曲线图；

图6为本发明实施例提供的三级海况下的航向曲线图(500s后并联广谱海浪滤波器)；

图7为本发明实施例提供的三级海况下的舵角曲线图(500s后并联广谱海浪滤波器)。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

参照图1所示，本发明实施例提供的一种减少无效操舵的航向控制方法，包括：

S1、采用广谱海浪滤波器，计算广谱海浪滤波器系数；所述广谱海浪滤波器系数的计算结果包含：卡尔曼滤波器增益矩阵和控制器系数矩阵；

S2、采用卡尔曼滤波器，并根据所述卡尔曼滤波器增益矩阵，进行卡尔曼滤波状态量计算；

S3、根据所述控制器系数矩阵及所述卡尔曼滤波状态量，进行基本控制量和滤波控制分量的计算；

S4、将所述基本控制量与所述滤波控制分量相加，获得最终方向舵控制量。

本发明的实施例，包括计算广谱海浪滤波器系数步骤、卡尔曼滤波器状态量计算步骤、计算基本控制量和滤波控制分量步骤以及最终方向舵控制量计算步骤。即：先进行计算广谱海浪滤波器系数步骤，解算滤波控制分量步骤需要的系数矩阵，再进行卡尔曼滤波状态量计算步骤，其结果提供计算基本控制量和滤波控制分量步骤，由于基本控制量和滤波控制分量同时计算，可避免采用带阻滤波器由于先滤波再参与控制量计算引起的大相位滞后，因此，在滤波时能仍保证船舶航向控制系统的控制精度和稳定性；计算得到的最终方向舵控制量送入舵机控制单元后，在风浪干扰下能以较低的打舵频率实现对航向的有效控制，可降低船舶舵机设备机械磨损。

下面分别对上述步骤进行详细说明：

步骤S1中，计算广谱海浪滤波器系数步骤，包括下述子步骤：

(1.1)计算卡尔曼滤波器增益矩阵H_3×1、控制器系数矩阵M_1×3和偏航增益系数N：

将矩阵A_3×3、B_3×1、C_1×3以及QN和RN代入黎卡提方程，解得卡尔曼滤波器增益矩阵H_3×1，其中，A_3×3为船舶线性运动方程系统矩阵，B_3×1为船舶线性运动方程输入矩阵，C_1×3为船舶线性运动方程输出矩阵，均由所涉船舶的设计单位提供；QN为模型噪声协方差，RN为测量噪声协方差；

将矩阵A_3×3、B_3×1、C_1×3、Q_4×4和R代入黎卡提方程，解得控制器系数矩阵M_1×3和偏航增益系数N,其中，Q_4×4为控制误差加权系数矩阵，R为能量损耗加权系数；

以某船舶巡航航速线性运动方程为例，本文中的其他计算均基于以下数据：

C_1×3＝[0 0 1]，

计算得：QN＝1，RN＝200，

/>

R＝0.5，

可计算得：M_1×3＝[0.56772 -25.8478 -2.432039],N＝-0.141421；

(1.2)计算传递函数矩阵T(s)：

其中，s为拉普拉斯算子，I为单位矩阵。

T₁₁(s)、T₁₂(s)、T₂₁(s)、T₂₂(s)为卡尔曼滤波器和控制器传递函数矩阵T(s)的元素，分别表示航向到舵角、航向到卡尔曼滤波器估计偏差、滤波控制分量到舵角、滤波控制分量到卡尔曼滤波器估计偏差的传递函数。

(1.3)计算频率响应值：

滤波频带为0.5rad/s～1.2rad/s，以覆盖海浪频率，在滤波频带内选取两个频率点ω₁，ω₂，计算相应的频率响应值F(jω₁)，F(jω₂)；

比如在滤波频带内选取两个频率点ω₁＝0.5，ω₂＝0.76，计算相应的频率响应值F(jω₁)，F(jω₂)：

(1.4)计算广谱海浪滤波器系统矩阵：

配置广谱海浪滤波器极点为λ₁＝λ₂＝λ₃＝λ₄＝λ₅＝-λ，0.1＜λ＜10，得到能控标准型的广谱海浪滤波器系统矩阵α_5×5：

比如：λ₁＝λ₂＝λ₃＝λ₄＝λ₅＝-0.52，得到能控标准型的广谱海浪滤波器系统矩阵α_5×5：

(1.5)计算广谱海浪滤波器输入矩阵：

将F(jω₁)、F(jω₂)、α_5×5、ω₁、ω₂和μ_1×5＝[0 1 0 0 0]代入下述复数方程组：

F(jω₁)＝μ_1×5(jω₁I_5×5-α_5×5)^-1β_5×1，

F(jω₂)＝μ_1×5(jω₂I_5×5-α_5×5)^-1β_5×1；

得到广谱海浪滤波器输入矩阵β_5×1＝[0 β₁ β₂ β₃ β₄]；其中，β₁＝5.181396,β₂＝-8.937623,β₃＝11.997644,β₄＝-13.683687；

步骤S2中，卡尔曼滤波器状态量计算步骤，包括下述子步骤：

(2.1)卡尔曼滤波器状态量z_(k)初始化：

卡尔曼滤波器状态量z_(k)为：

z_(k)＝[z_1(k) z_2(k) z_3(k)]^T

其中，z_1(k)为k时刻的横向速度估计值，z_2(k)为k时刻的航向角角速度估计值，z_3(k)为k时刻的航向角估计值，T表示对矩阵转置；k＝0、1、2、…n，表示自然数。还是以上述某船舶为例，各时刻之间的间距，比如选取h＝0.1秒；

将卡尔曼滤波器状态量z_(k)初始化为z₍₀₎，

为系统开机时刻测量的航向角；比如可以选取：/>

作为系统开机时刻测量的航向角；

(2.2)根据所述船舶线性运动方程系统矩阵A_3×3，船舶线性运动方程输入矩阵B_3×1，卡尔曼滤波器增益矩阵H_3×1，船舶线性运动方程输出矩阵C_1×3，计算k 时刻卡尔曼滤波器状态量的微分

其中，δ_(k)为k时刻测量的方向舵舵角，

为k时刻测量的航向角，k-1表示上一时刻；

(2.3)计算k时刻卡尔曼滤波器状态量z_(k)：

(3)计算基本控制量和滤波控制分量步骤，包括下述子步骤：

(3.1)计算基本控制量

其中，指令航向/>

为0°；

(3.2)设定广谱海浪滤波器状态量p_(k)的初值p₍₀₎＝[0 0 0 0]^T；

(3.3)计算p_(k)的微分：

(3.4)计算广谱海浪滤波器状态量

(3.5)计算k时刻的滤波控制分量γ_(k)：γ_(k)＝μ_1×5p_(k)；

由于基本控制量和滤波控制分量同时计算，可避免采用带阻滤波器由于先滤波再参与控制量计算引起的大相位滞后，因此，在滤波时能仍保证船舶航向控制系统的控制精度和稳定性。

(4)计算最终方向舵控制量步骤：

计算最终方向舵控制量

并将其送入舵机控制单元。

通过不断接收传感器系统实时测量的航向

方向舵角δ_(k)，循环运行步骤S2～S4，产生实时的方向舵控制量/>

通过计算基本控制量和滤波控制分量步骤，可实现在较宽频带范围内对控制器输出幅值的有效抑制，可减小控制系统在滤波频带0.5rad/s～1.2rad/s 的频响幅值，该滤波频带对应海浪干扰主频率段，已覆盖五级和五级以下海浪的频谱，因此无需经过海浪谱辨识就能实现对五级和五级以下海浪的有效滤波，避免了采用传统扩展卡尔曼滤波器时由于海浪谱辨识不准确而造成的滤波效果差的问题；计算得到的最终方向舵控制量送入舵机控制单元后，在风浪干扰下能以较低的打舵频率实现对航向的有效控制，可降低船舶舵机设备机械磨损。

本实施例中，上述计算广谱海浪滤波器系数步骤的子步骤(1.1)中，卡尔曼滤波器增益矩阵H_3×1由Matlab控制系统工具箱中的指令 kalman(sys,Qn,Rn,Nn)解得，其过程为：

由矩阵A_3×3、B_3×1、C_1×3搭建线性系统sys的系数矩阵：

sys.c＝[0 0 1]，sys.d＝[0 0],

再将sys、QN＝1、RN＝200和Nn＝0代入Matlab指令kalman(sys,Qn,Rn,Nn) 中，可得增益矩阵：

其中，sys.a等于系统矩阵A_3×3，sys.b等于系统矩阵B_3×1，QN和RN由系统噪声数据计算得到或根据设计经验选择合理的值。

模型噪声协方差QN取值范围为0.1～10，测量噪声协方差RN取值范围为 100～2000，Nn＝0，选取原则为：当航向传感器测量精度较低时可适当增大RN 的取值，减小QN取值；

(B)控制器系数矩阵M_1×3和偏航增益系数N由Matlab控制系统工具箱中的指令lqr(AA,BB,Q,R,N)解得，其过程为：

将矩阵

BB＝[0 0 0 1]^T，根据设计经验选择合理的 Q_4×4,R和N值后，代入Matlab指令lqr(AA,BB,Q,R,N)，解得增益矩阵 K_1×4＝[k₁ k₂ k₃ k₄]；/>

R＝0.5，N＝0，

再将AA、BB、Q_4×4、R和N代入Matlab指令lqr(AA,BB,Q,R,N)中，解得增益矩阵：

其中，

控制误差加权系数矩阵Q_4×4为对角矩阵，各元素取值范围均为0～10，能量损耗加权系数R取值范围为0.001～1，N＝0；选取原则为：要提高控制的快速响应特性，则增大Q_4×4中相应元素的取值，要减小控制量的幅值及其引起的能量消耗，则增大R的取值；

然后，计算控制器系数矩阵M_1×3和偏航增益系数N：

M_1×3＝[m₁ m₂ m₃]＝[0.56772 -25.847841 -2.43203]，

N＝-0.141421；

其中，

m₃＝k₂，a₁₁、a₂₁、b₁、b₂为系统矩阵 A_3×3，B_3×1的元素。

本实施例中，计算广谱海浪滤波器系数步骤的子步骤(1.3)中，在滤波频带内选取两个频率点ω₁、ω₂时，先选择ω₁＝0.5，ω₂＝0.55，观察传递函数矩阵 T(s)的第一个输入到第一个输出的幅频响应特性，当幅频响应值减小的频率范围未覆盖滤波频带时，增大ω₂的取值，直到幅频响应值减小的频率范围能够覆盖滤波频带0.5rad/s～1.2rad/s。

频率点ω₁、ω₂选取原则为：选择的频率点ω₁、ω₂能实现对控制系统在滤波频带响应幅值的有效抑制，即通过并联广谱海浪滤波器减小控制系统T(s)的第一个输入(航向)到第一个输出(方向舵舵角)的幅频响应特性曲线在滤波频带0.5rad/s～1.2rad/s的幅值；

广谱海浪滤波器传递函数为F(s)＝μ_1×5(sI_5×5-α_5×5)^-1β_5×1，在控制系统T(s)上并联广谱海浪滤波器后传递函数矩阵结构如图2，通过广谱海浪滤波器F(s)可改变系统T(s)第一个输入(航向)到第一个输出(指令输出)的幅频响应特性。不并联广谱海浪滤波器F(s)，ω₁＝0.5，ω₂＝0.55时，系统T(s)第一个输入(航向) 到第一个输出(指令输出)的幅频响应特性如图3中的虚线；并联广谱海浪滤波器F(s)后，系统T(s)第一个输入(航向)到第一个输出(指令输出)的幅频响应特性如图3中的实线，由图3可知并联广谱海浪滤波器后，控制系统的频响幅值在0.5rad/s～0.6rad/s频带范围内大幅衰减，我国海区海浪谱密度曲线如图5，由于图3中频响幅值衰减的频带无法覆盖五级和五级以下海浪的滤波频带0.5rad/s～1.2rad/s，需要增大ω₂取值，ω₂＝0.76时系统频响特性如图4，因此，系统频响特性幅值衰减的频带已覆盖船舶正常航行的五级和五级以下海况的峰值频率，可实现对五级和五级以下海况的有效滤波。

利用船舶运动模拟器在三级海况下对闭环控制过程进行数字仿真，舵角曲线如图6、航向曲线如图7，由仿真结果可以看出在风浪干扰下，500s后利用广谱海浪滤波器可大幅减少方向舵的无效操舵，同时可以保持较高的航向控制精度

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和 /或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/ 或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定功能的步骤。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (4)

1.一种减少无效操舵的航向控制方法，其特征在于，包括：

S1、采用广谱海浪滤波器，计算广谱海浪滤波器系数；所述广谱海浪滤波器系数的计算结果包含：卡尔曼滤波器增益矩阵和控制器系数矩阵；

S2、采用卡尔曼滤波器，并根据所述卡尔曼滤波器增益矩阵，进行卡尔曼滤波状态量计算；

S3、根据所述控制器系数矩阵及所述卡尔曼滤波状态量，进行基本控制量和滤波控制分量的计算；

S4、将所述基本控制量与所述滤波控制分量相加，获得最终方向舵控制量；

所述步骤S1包括：

(1.1)计算卡尔曼滤波器增益矩阵H_3×1、控制器系数矩阵M_1×3和偏航增益系数N：

将矩阵A_3×3、B_3×1、C_1×3以及QN和RN代入黎卡提方程，解得卡尔曼滤波器增益矩阵H_3×1；其中，A_3×3为船舶线性运动方程系统矩阵，B_3×1为船舶线性运动方程输入矩阵，C_1×3为船舶线性运动方程输出矩阵，均为所涉船舶的设计数据；QN为模型噪声协方差，RN为测量噪声协方差；

将矩阵A_3×3、B_3×1、C_1×3、Q_4×4和R代入黎卡提方程，解得控制器系数矩阵M_1×3和偏航增益系数N,其中，Q_4×4为控制误差加权系数矩阵，R为能量损耗加权系数；

(1.2)计算传递函数矩阵T(s)：

其中，s为拉普拉斯算子，I_4×4为单位矩阵；

(1.3)计算频率响应值：

滤波频带为0.5rad/s～1.2rad/s，以覆盖海浪频率，在滤波频带内选取两个频率点ω₁，ω₂，计算相应的频率响应值F(jω₁)，F(jω₂)：

F(jω₁)＝{T₁₂(jω₁)+[-T₁₁(jω₁)]T₂₁ ^-1(jω₁)T₂₂(jω₁)}^-1×[-T₁₁(jω₁)]T₂₁ ^-1(jω₁)，

F(jω₂)＝{T₁₂(jω₂)+[-T₁₁(jω₂)]T₂₁ ^-1(jω₂)T₂₂(jω₂)}^-1×[-T₁₁(jω₂)]T₂₁ ^-1(jω₂)；

(1.4)计算广谱海浪滤波器系统矩阵：

配置广谱海浪滤波器极点为λ₁＝λ₂＝λ₃＝λ₄＝λ₅＝-λ，0.1＜λ＜10，得到能控标准型的广谱海浪滤波器系统矩阵α_5×5：

(1.5)计算广谱海浪滤波器输入矩阵：

将所述F(jω₁)、F(jω₂)、μ_1×5、α_5×5、ω₁和ω₂代入下述复数方程组：

F(jω₁)＝μ_1×5(jω₁I_5×5-α_5×5)^-1β_5×1，

F(jω₂)＝μ_1×5(jω₂I_5×5-α_5×5)^-1β_5×1；

得到广谱海浪滤波器输入矩阵β_5×1＝[0 β₁ β₂ β₃ β₄]；其中，μ_1×5＝[0 1 0 0 0]；

所述步骤S2，包括：

(2.1)卡尔曼滤波器状态量z_(k)初始化：

卡尔曼滤波器状态量z_(k)为：

z_(k)＝[z_1(k) z_2(k) z_3(k)]^T

其中，z_1(k)为k时刻的横向速度估计值，z_2(k)为k时刻的航向角角速度估计值，z_3(k)为k时刻的航向角估计值，T表示对矩阵转置；k＝0、1、2、…n，表示自然数，各时刻之间的间距h＝0.01～1秒；

将卡尔曼滤波器状态量z_(k)初始化为z₍₀₎，

为系统开机时刻测量的航向角；

(2.2)根据所述船舶线性运动方程系统矩阵A_3×3，船舶线性运动方程输入矩阵B_3×1，卡尔曼滤波器增益矩阵H_3×1，船舶线性运动方程输出矩阵C_1×3，计算k时刻卡尔曼滤波器状态量的微分

其中，δ_(k)为k时刻测量的方向舵舵角，

为k时刻测量的航向角，k-1表示上一时刻；

(2.3)计算k时刻卡尔曼滤波器状态量z_(k)：

所述步骤S3，包括：

(3.1)计算基本控制量

其中，/>

为指令航向；

(3.2)设定广谱海浪滤波器状态量p_(k)的初值p₍₀₎＝[0000]^T；

(3.3)计算p_(k)的微分：

(3.4)计算广谱海浪滤波器状态量

(3.5)计算k时刻的滤波控制分量γ_(k)：γ_(k)＝μ_1×5p_(k)。

2.如权利要求1所述的一种减少无效操舵的航向控制方法，其特征在于，所述步骤S4，包括：

计算最终方向舵控制量

并将其送入舵机控制单元；

通过不断接收传感器系统实时测量的航向

方向舵角δ_(k)，循环运行步骤S2～S4，产生实时的方向舵控制量/>

3.如权利要求1所述的一种减少无效操舵的航向控制方法，其特征在于，所述步骤(1.1)包括：

(A)卡尔曼滤波器增益矩阵H_3×1由Matlab控制系统工具箱中的指令kalman(sys,Qn,Rn,Nn)解得，其过程为：

由矩阵A_3×3、B_3×1、C_1×3搭建线性系统sys的系数矩阵：

sys.a＝A_3×3，sys.b＝[B_3×1[010]^T]，sys.c＝C_1×3，sys.d＝[0 0]，

将sys、QN、RN和Nn代入Matlab的所述指令kalman(sys,Qn,Rn,Nn)中，解得增益矩阵H_3×1；

其中模型噪声协方差QN取值范围为0.1～10，测量噪声协方差RN取值范围为100～2000，Nn＝0；

(B)控制器系数矩阵M_1×3和偏航增益系数N由Matlab控制系统工具箱中的指令lqr(AA,BB,Q,R,N)解得，其过程为：

将矩阵

BB＝[0001]^T，Q_4×4,R和N代入Matlab的指令lqr(AA,BB,Q,R,N)，解得增益矩阵K_1×4＝[k₁ k₂ k₃ k₄]；

其中，

控制误差加权系数矩阵Q_4×4为对角矩阵，各元素取值范围均为0～10，能量损耗加权系数R取值范围为0.001～1，N＝0；选取原则为：要提高控制的快速响应特性，则增大Q_4×4中相应元素的取值，要减小控制量的幅值及其引起的能量消耗，则增大R的取值；

计算控制器系数矩阵M_1×3和偏航增益系数N：

M_1×3＝[m₁ m₂ m₃]，N＝k₃；

其中，

m₃＝k₂。

4.如权利要求1所述的一种减少无效操舵的航向控制方法，其特征在于，所述步骤(1.3)中，在滤波频带内选取两个频率点ω₁、ω₂时，先选择ω₁＝0.5，ω₂＝0.55，观察传递函数矩阵T(s)的第一个输入到第一个输出的幅频响应特性，当幅频响应值减小的频率范围未覆盖滤波频带时，增大ω₂的取值，直到幅频响应值减小的频率范围能够覆盖滤波频带0.5rad/s～1.2rad/s。

Images