[发明专利]一种减少无效操舵的航向控制方法

权利要求书

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)。