CN113419422A - 一种船用舵鳍联合减摇控制系统 - Google Patents

Abstract

本发明公开一种基于改进模型预测控制抗干扰的舵鳍联合减摇控制系统，旨在解决常规舵鳍联合减摇控制系统存在时变干扰、模型失配和频繁操舵操鳍问题，具体包括以下步骤：构建了考虑时变干扰的离散化三自由度船舶数学模型；以舵角鳍角作为系统输入，设计滑模观测器对干扰和系统输出进行实时观测，并将其反馈给模型预测控制器(MPC)；MPC基于扰动增量式数学模型对系统动态输出进行预测，设定期望输出值，将船舶运动控制问题转化为求解二次规划最优解问题，在添加舵角鳍角等约束条件下求解出最优控制律；滤波器对控制律进行高频降噪。本发明观测精度高，减摇效果好，抗扰能力强，避免频繁操舵操鳍引起的执行机构磨损和能源损耗。

Description

一种船用舵鳍联合减摇控制系统

技术领域

本发明属于船舶减摇领域，具体涉及一种改进模型预测控制抗干扰的舵鳍联合减摇控制系统。

背景技术

舰船在海上航行时遭受风浪和恶劣天气的影响会产生剧烈的摇晃，严重的横摇会损坏船体，甚至危机货物设备及人员的安全，从而造成重大的事故。大部分舰船常常通过操纵减摇鳍来减小干扰引起的横摇，单独利用舵来保持航向，而这种单独的控制方式不利于船舶综合控制性能的提高，伴随着舰船编队、护航补给、反潜防空等任务的增加，对舰船平台提出了更高的稳定性要求。为此，对舰船等水面航行器采用舵鳍联合减摇控制不仅可以保证更加稳定的生活平台，同时对于减少能源消耗和执行机构磨损也尤为重要。

船舶在海上航行时难以避免的遭受多来源多类型的干扰，控制系统的动态性能和稳态精度常常会受到干扰的严重影响，甚至会破坏闭环系统的稳定性。舵鳍联合减摇控制系统可以等价为一个二输入多输出的非线性系统，存在时变干扰、模型失配、频繁操舵操鳍等问题，在满足执行机构的输入约束前提下，设计舵角鳍角控制律是亟待解决的难点。

论文《基于航速损失约束的船舶横摇/航向控制研究》和《基于反馈线性化的船舶舵鳍联合减摇MPC控制》设计的模型预测控制器(MPC)，存在以下问题：

(1)线性化过后的数学模型简单，设计常规的MPC未考虑外界干扰对船舶系统输出的不利影响，难以准确的预测系统的动态输出；

(2)求解控制律的约束条件中未加入角速度约束，会频繁操舵导致航向不断发生变化，增加舵机构磨损和能源损耗。

论文《基于等价输入干扰滑模观测器的磁悬浮球系统模型预测控制》提供的方法与本发明的改进模型预测控制器相比，有以下问题：

(1)未充分利用观测得到的干扰值，只是简单的前馈结合控制律去抵消外界干扰，没有考虑补偿后的复合控制律是否满足机构输入约束问题；

(2)观测器只对规律波进行干扰观测，未观测时变未知干扰，实际意义不足。

发明内容

为解决常规舵鳍联合减摇控制系统中存在时变干扰、模型失配和频繁操舵操鳍的问题，本发明提出了一种改进模型预测控制抗干扰的舵鳍联合减摇控制系统，所述方法操作简单、计算量小、实用性强。

为实现上述目的，本发明采用以下技术方案：

设计了一种基于改进模型预测控制抗干扰的舵鳍联合减摇控制系统，具体包括MPC模型预测控制器、滑模观测器、船舶运动系统、滤波器。首先构建了考虑时变干扰的离散化三自由度船舶数学模型；其次，以舵角鳍角作为系统输入，设计滑模观测器对时变干扰和系统输出进行实时观测，并将其反馈给MPC；然后，MPC基于扰动增量式数学模型对系统未来动态进行预测，设定期望输出值，搭建目标函数，将船舶运动控制问题转化为求解二次规划最优解问题，在添加舵鳍等约束条件的情况下求解出最优控制律；最后通过滤波器对控制律进行高频降噪和柔顺处理，仿真结果验证该系统观测精度高，减摇效果好，抗扰能力强，避免了频繁操舵操鳍引起的执行机构磨损和能源损耗。具体包括以下步骤：

步骤一、构建考虑时变干扰的离散线性化的船舶三自由度(横摇、艏摇、横荡)数学模型：

选取采样k(k>0)时刻的状态变量

v、r、

ρ、φ分别为横荡速度、艏摇角速度、艏摇角、横摇角速度、横摇角，右上标T代表矩阵转置，x(k+1)为k+1时刻的状态变量，控制输入量u(k)＝[δ α]^T，δ、α分别为舵角、鳍角，广义时变干扰d(k)＝[d1d2]^T，d1为影响角度的干扰，d2为影响角速度的干扰，实际输出值

建立的船舶数学模型如下：

式中：ψ、G、G_d、H是系统状态空间矩阵。

步骤二、设计离散的滑模观测器对实际干扰值和输出值进行实时观测：

设计滑模观测器的目的是对系统输出值和外界干扰进行实时观测，并将干扰观测值增量

和输出观测值

反馈到MPC，对系统状态变量进行实时补偿，MPC在添加舵角鳍角等显示约束的条件下求解出最优控制律，来抵消下一时刻干扰对系统产生的不利影响。

选取连续的切换函数作为滑模控制律，设计的观测器结构如下：

式中：

分别为k时刻的系统状态观测值、输出观测值、输入观测值，

是k+1时刻的状态观测值，L为增益矩阵，K_s为饱和增益，

为切换函数，ε为滑模控制常数。

取变量

Δ为切换常数，则sat[s]如式(3)：

定义系统状态观测误差为：

式中：e_x(k)为k时刻的状态观测误差，e_x(k+1)为k+1时刻的状态观测误差；

则实际输出值与观测输出值误差为：

推导出干扰观测值

的最小二乘解为：

式中：最小二乘广义逆矩阵G⁺＝(G_d ^TG_d)^-1G_d ^T。

引入单位延迟环节q，并定义干扰观测值增量如下：

通过选取适当滑模观测器的参数能够确保观测值

收敛到真实值d(k)，则滑模控制器实现对d(k)的准确观测，进而实现对输出值y(k)的准确观测，最后将干扰观测值增量

和输出观测值

反馈给MPC。

步骤三、将船舶三自由度数学模型改写成扰动增量式数学模型：

MPC的原理是基于精确的数学模型预测系统的动态输出，为了便于直观地推导输入与输出间的关系，将式(1)扩展状态变量并改写成扰动增量式数学模型，状态增量Δx(k)＝x(k)-x(k-1)，扩展状态变量后新的状态变量x_m(k)＝[Δx(k)^T y(k)]^T，同理，x_m(k+1)＝[Δx(k+1)^T y(k+1)]^T，控制增量为Δu(k)＝u(k)-u(k-1)，扰动增量为Δd_e(k)＝d_e(k)-d_e(k-1)，改写后的扰动增量式数学模型为：

式中：A_m，B_u，B_d，C_m为系统状态空间增广矩阵。

步骤四、MPC基于扰动增量式数学模型预测系统未来的动态输出：

在某一采样时刻k，状态变量x_m(k)通过观测得到，取m为控制时域，p为预测时域(m＜p)，定义系统动态输出为Y_p(k+p)，控制增量为ΔU(k)，干扰观测值增量ΔD_e(k)，y_p(k+p)表示k+p时刻的输出预测值，Δu(k+m-1)、Δd_e(k+m-1)为k+m-1时刻的控制输入增量和干扰值观测增量，如下：

基于扰动增量式数学模型(8)推导出MPC预测系统的动态输出为：

Y_p(k+p)＝S_xx_m(k)+S_uΔU(k)+S_dΔD_e(k) (10)

式中：S_x、S_u、S_d为常系数矩阵。

与常规MPC相比，式(10)考虑了时变干扰对系统的不利影响，ΔD_e(k)不再是定常值，而是观测器观测到的时变干扰，MPC采集到k时刻的x_m(k)和k+m时刻内的ΔU(k)、ΔD_e(k)实现预测系统未来k+p步的动态输出。

步骤五、将船舶运动控制问题转化为求解二次规划问题：

为保证船舶实际输出能够快速地跟踪设定的期望艏摇角和横摇角，同时控制增量较小，设计了一种带有权重因子的目标函数J，添加舵角鳍角等约束条件下求解最优控制律。设置期望输出值R(k+p)，option功能为选取最接近期望输出值的预测值y_p(k)和观测值

作为最优输出值

通过计算最优输出值与期望输出值的差值来搭建目标函数。为减少舵机和鳍机的频繁操纵，设计了舵角鳍角及其增量约束，为防止求解失败，引入松弛因子ρ₁和ρ₂，同时增加状态约束如横摇速度约束、艏摇速度约束。MPC带约束的目标函数优化求解问题如下：

J＝[R(k+p)-Y_p(k+p)]^TQ[R(k+p)-Y_p(k+p)]+ΔU(k)^TWΔU(k) (11)

式中：Q和W为权重矩阵。

满足的扰动增量式数学模型和约束条件如下：

式中：x_m(k)_min、x_m(k)_max分别是状态变量x(k)最小值、最大值矩阵，u(k)_min、u(k)_max分别是舵角鳍角的最小值、最大值矩阵，Δu(k)_min、Δu(k)_max分别是舵角增量和鳍角增量的最小值、最大值矩阵。

步骤六、添加舵角鳍角等约束条件下求解最优控制律：

在添加上述约束条件下求解出最优控制律：

式中：K_r、K_x、K_y、K_d均是常值增益矩阵，K_r＝(S_u ^TQS_u+W)^-1S_u ^T，K_rS_x＝[K_x K_y]，K_d＝K_rS_d，期望输出R(k)＝R(k+p)。

MPC只需输入四个参数R(k)、y(k)、Δx(k)、ΔD_e(k)就可计算出最优控制律ΔU^*(k)，并且控制律满足执行机构输入约束。

步骤七、反馈校正：

将得到ΔU^*(k)中的第1个元素Δu^*(k)作为实际控制输入增量，可得到未来时刻的控制输入量

分别通过滤波器F(k)作用于系统和进入滑模观测器预测系统输出值和干扰值，系统执行控制输入量直到下一采样时刻，根据观测信息重新预测下一段时刻的输出值，再次求解优化目标函数，得到一个新的控制律，再将其作用于系统的下个时刻，循环往复，形成最优控制。

本发明具有如下有益效果：

(1)本发明在传统模型预测控制的基础上，考虑时变干扰对系统状态变量的影响，基于干扰和观测信息预测系统动态输出，设计的舵鳍联合减摇控制系统较好的减摇效果和抗干扰性能，方法操作简单，计算量小，实用性强；

(2)在约束条件中增加舵角鳍角及增量约束，又对控制律进行滤波处理，避免频繁操舵操鳍引起的执行机构磨损和能源损耗；

(3)滑模观测器对正弦波、矩形波干扰可以达到95％的观测精度，输出值观测精度达到94％，对海浪时变干扰仍能较好的观测出干扰值的形状和趋势，输出值观测精度达到99.7％。

附图说明

图1为舵鳍联合减摇控制系统框图；

图2为基于改进模型预测抗干扰的舵鳍联合减摇控制系统结构图；

图3为改进模型预测控制算法的流程图；

图4为矩形波干扰作用下观测器的仿真结果图；

图5为正弦波干扰作用下观测器的仿真结果图；

图6为海浪干扰作用下观测器的仿真结果图；

图7为海浪作用下舵鳍联合减摇控制系统的仿真结果图。

具体实施方式

图1为舵鳍联合减摇控制系统框图，图2为基于改进模型预测抗干扰的舵鳍联合减摇控制系统结构图，图3为改进模型预测控制算法的流程图，具体包括以下步骤：

步骤一、构建考虑时变干扰的离散线性化的船舶三自由度数学模型：

船舶在海上受风浪影响会产生6自由度的运动，本文只考虑对船舶影响较大的横摇、横荡、艏摇运动，并将系统不确定性、模型失配和外部干扰等效为系统输入端的广义时变扰动，假设干扰有界，构建考虑时变干扰的线性离散化船舶三自由度数学模型。选取采样k(k>0)时刻的状态变量

v、r、

ρ、φ分别为横荡速度、艏摇角速度、艏摇角、横摇角速度、横摇角，右上标T代表矩阵转置，x(k+1)为k+1时刻的状态变量，控制输入量u(k)＝[δ α]^T，δ、α分别为舵角、鳍角，广义时变干扰d(k)＝[d1 d2]^T，d1为影响横摇角和艏摇角的干扰，d2为影响横摇角速度和艏摇角速度的干扰，实际输出值

建立的船舶数学模型如下：

式中：ψ、G、G_d、H是系统状态空间矩阵。

离散系统完全能观测的条件为矩阵Q＝[H Hψ Hψ² Hψ³ Hψ⁴]^T满秩，经验证系统状态可观测，然后进入步骤二；

步骤二、设计离散的滑模观测器对实际干扰值和输出值进行实时观测：

设计滑模观测器的目的是对系统输出值和外界干扰进行实时观测，并将干扰观测值增量

和输出观测值

反馈到MPC，对系统状态变量进行实时补偿，MPC在添加舵角鳍角等显示约束的条件下求解出最优控制律，来抵消下一时刻干扰对系统产生的不利影响。

离散的滑模观测器具有较好的鲁棒性和观测精度，选取连续的切换函数作为滑模控制律，可以有效地抑制抖动，对式(1)构建的滑模观测器结构如下：

式中：

分别为k时刻的系统状态观测值、输出观测值、输入观测值，

是k+1时刻的状态观测值，L为增益矩阵，K_s为饱和增益，

为切换函数，ε为滑模控制常数。

取变量

Δ为切换常数，则sat[s]如式(3)：

定义系统状态观测误差为：

式中：e_x(k)为k时刻的状态观测误差，e_x(k+1)为k+1时刻的状态观测误差；

则实际输出值与观测输出值的误差为：

将式(4)带入式(1)可得：

假设干扰增量Δd(k)存在且满足：

G_dΔd(k)＝ψe_x(k)-e_x(k+1) (20)

广义干扰d(k)的观测值

为：

将式(7)、(8)带入式(9)可得：

将式(5)带入式(2)可得：

联立式(9)和式(10)，求解出干扰观测值

的最小二乘解为：

式中：最小二乘广义逆矩阵G⁺＝(G_d ^TG_d)^-1G_d ^T。

引入单位延迟环节q，并定义干扰观测值增量如下：

通过选取适当滑模观测器的参数确保观测值

收敛到真实值d(k)，则滑模控制器实现对d(k)的准确观测，进而实现对输出值y(k)的准确观测，最后将干扰观测值增量

和输出观测值

反馈给MPC。

步骤三、将船舶三自由度数学模型改写成扰动增量式数学模型：

MPC的原理是基于精确的数学模型预测系统的动态输出，但必须考虑时变干扰对系统的影响，同时设计的滑模观测器必须准确的观测到实际值，否则将会发生模型失配问题(预测输出与实际输出差值较大)。首先定义状态增量Δx(k)＝x(k)-x(k-1)，控制增量为Δu(k)＝u(k)-u(k-1)，扰动增量为Δd_e(k)＝d_e(k)-d_e(k-1)，将式(1)改写为增量式数学模型：

式中：A＝ψ，B＝G，C＝H，D＝G_d，y(k-1)为k-1时刻实际输出值。

扩展后的新的状态变量为x_m(k)、x_m(k+1)且此存在矩阵B_m有以下关系：

根据式(13)有：

y(k+1)-y(k)＝CΔx(k+1)＝CAΔx(k)+CBΔu(k)+CDΔd_e(k) (28)

联立式(13)、(14)和(15)，形成以下扰动增量式数学模型：

式中：o＝[0 0 … 0]_1×N，N为向量y(k)的维数，A_m、B_u、B_d、C_m被称为增广状态空间矩阵。简写成下式：

步骤四、MPC基于扰动增量式数学模型预测系统的动态输出：

在某一采样时刻k，状态变量x_m(k)通过观测得到，取m为控制时域，p为预测时域(m＜p)，定义系统动态输出为Y_p(k+p)，控制增量为ΔU(k)，干扰观测值增量ΔD_e(k)，y_p(k+p)表示k+p时刻的输出预测值，Δu(k+m-1)、Δd_e(k+m-1)分别为k+m-1时刻的控制输入增量和干扰值观测增量，如下：

基于扰动增量式数学模型(17)推导未来的状态变量为：

式中：

代表p-m个矩阵A_m的乘积。

MPC预测系统未来k+p步的动态输出为：

简写为：

Y_p(k+p)＝S_xx_m(k)+S_uΔU(k)+S_dΔD_e(k) (34)

与常规MPC相比，式(21)考虑了时变干扰对系统的不利影响，ΔD_e(k)不再是定常值，而是观测器观测到的时变干扰，大大提高了预测精度。MPC采集到k时刻的x_m(k)和k+m时刻内的ΔU(k)、ΔD_e(k)就可以实现预测系统未来k+p步的动态输出。S_x、S_u、S_d为常系数矩阵如下：

步骤五、将船舶运动控制问题转化为求解二次规划问题：

为保证船舶实际输出能够快速地跟踪设定的期望艏摇角和横摇角，同时控制增量较小，设计了一种带有权重因子的目标函数J，添加舵角鳍角等约束条件下求解最优控制律。设置期望输出值R(k+p)，option功能为选取最接近期望输出值的预测值y_p(k)和观测值

作为最优输出值

通过计算最优输出值与期望输出值的差值来搭建目标函数。为减少舵机和鳍机的频繁操纵，设计了舵角鳍角及其增量约束，为防止求解失败，引入松弛因子ρ₁和ρ₂，同时增加状态约束如横摇速度约束、艏摇速度约束。定义期望输出值如下：

式中：r(k+p)为k+p时刻的期望输出值。

目标函数J由两部分构成，第一项R(k+p)-Y_p(k+p)的差值越小表示期望跟踪性能好(航向角艏摇角逼近期望输入值)，第二项ΔU(k)越小代表控制增量较小(舵角鳍角增量小)。目标函数如下：

J＝[R(k+p)-Y_p(k+p)]^TQ[R(k+p)-Y_p(k+p)]+ΔU(k)^TWΔU(k) (38)

式中：Q和W为权重矩阵：

式中：λ_yj,i是在预测时刻i对预测输出值第j个分量的权重因子，λ_Δuj,i是预测时刻i对控制增量第j个分量的权重因子，权重因子越大，表明期望对应的实际输出值越接近给定的期望输出值，控制增量越小，控制量越小。

满足的扰动增量式数学模型和约束条件如下：

式中：x_m(k)_min、x_m(k)_max分别是状态变量x(k)最小值、最大值矩阵，u(k)_min、u(k)_max分别是舵角鳍角的最小值、最大值矩阵，Δu(k)_min、Δu(k)_max是舵角增量和鳍角增量的最小值、最大值矩阵。

步骤六、添加舵角鳍角等约束条件下求解最优控制律：

将式(21)带入式(25)中整理得：

求偏导得：

取得J最小值的必要条件为：

求出控制增量的最优解为：

式中：期望输出R(k)＝R(k+p)，增益矩阵K_r＝(S_u ^TQS_u+W)^-1S_u ^T，增益矩阵K_mpc＝K_rS_x，增益矩阵K_d＝K_rS_d。

考虑

将K_mpc分解成增益矩阵K_x和K_y，如下：

K_mpc＝[K_x K_y] (45)

则有：

满足上述约束条件下求解出最优控制律为：

式中：K_r、K_x、K_y、K_d均是常值增益矩阵，可以离线计算。

MPC只需输入四个参数R(k)、y(k)、Δx(k)、ΔD_e(k)就可计算出最优控制律ΔU^*(k)，并且控制律满足执行机构硬性约束。

步骤七、反馈校正；

将得到ΔU^*(k)中的第1个元素Δu^*(k)作为实际的控制输入增量，可得到未来时刻的控制输入量：

引入q为单位延迟环节，则

分别通过滤波器F(k)作用于系统和进入滑模观测器预测系统输出值和干扰值，系统执行控制输入量直到下一采样时刻，根据观测信息重新预测下一段时刻的输出值，再次求解优化目标函数，得到一个新的控制律，再将其作用于系统的下个时刻，循环往复，形成最优控制。

下面给出本发明所述的基于改进模型预测抗干扰的舵鳍联合减摇控制系统离线仿真验证。

图4和图5是观测器对矩形波干扰和正弦波干扰作用下的系统观测结果，观测值与实际值的误差很小，通过局部放大图可得干扰值的观测精度达到了95％，输出值的观测精度达到94％，规律波作用下的观测器得到的观测值较为准确，为下一步MPC预测系统动态输出提供了准确的数据来源。图6是海浪干扰作用下观测器的仿真结果图，从图中可以看出海浪波形是不规则且无规律，但观测器仍能较好的观测出干扰的形状和趋势，对输出值达到了99.7％的观测精度，观测器设计合理可靠。图7是海浪作用下舵鳍联合减摇控制系统的仿真结果图，可以看出横荡速度和艏摇角速度收敛于常值，横摇角的幅值较小，艏摇角趋近于常值，调节时间略长，但很好的保持了航向。

Claims (1)

1.本发明提出了一种改进模型预测控制抗干扰的舵鳍联合减摇控制系统，其特征在于，包括以下步骤：

步骤一、构建考虑时变干扰的离散线性化的船舶三自由度(横摇、艏摇、横荡)数学模型：

选取采样k(k>0)时刻的状态变量

v、r、

ρ、φ分别为横荡速度、艏摇角速度、艏摇角、横摇角速度、横摇角，右上标T代表矩阵转置，x(k+1)为k+1时刻的状态变量，控制输入量u(k)＝[δ α]^T，δ、α分别为舵角、鳍角，广义时变干扰d(k)＝[d1 d2]^T，d1为影响角度的干扰，d2为影响角速度的干扰，实际输出值

建立的船舶数学模型如下：

式中：ψ、G、G_d、H是系统状态空间矩阵；

步骤二、设计离散的滑模观测器对实际干扰值和输出值进行实时观测：

滑模观测器结构如下：

式中：

分别为k时刻的系统状态观测值、输出观测值、输入观测值，

是k+1时刻的状态观测值，L为增益矩阵，K_s为饱和增益，

为切换函数，ε为滑模控制常数；

取变量

Δ为切换常数，则sat[s]如式(3)：

定义系统状态观测误差为：

式中：e_x(k)为k时刻的状态观测误差，e_x(k+1)为k+1时刻的状态观测误差；

则实际输出值与观测输出值的误差为：

推导出干扰观测值

的最小二乘解为：

式中：最小二乘广义逆矩阵G⁺＝(G_d ^TG_d)^-1G_d ^T；

引入单位延迟环节q，并定义干扰观测值增量如下：

通过选取适当滑模观测器的参数确保观测值

收敛到真实值d(k)，则滑模控制器实现对d(k)的准确观测，进而实现对输出值y(k)的准确观测，最后将干扰观测值增量

和输出观测值

反馈给模型预测控制器(MPC)；

步骤三、将船舶三自由度数学模型改写成扰动增量式数学模型：

状态增量Δx(k)＝x(k)-x(k-1)，将式(1)扩展状态变量后新的状态变量x_m(k)＝[Δx(k)^T y(k)]^T，同理，x_m(k+1)＝[Δx(k+1)^T y(k+1)]^T，控制增量为Δu(k)＝u(k)-u(k-1)，扰动增量为Δd_e(k)＝d_e(k)-d_e(k-1)，改写后的扰动增量式数学模型为：

式中：A_m，B_u，B_d，C_m为系统状态空间增广矩阵；

步骤四、MPC基于扰动增量式数学模型预测系统的动态输出：

在某一采样时刻k，状态变量x_m(k)通过观测得到，取m为控制时域，p为预测时域(m＜p)，定义系统动态输出为Y_p(k+p)，控制增量为ΔU(k)，干扰观测值增量为ΔD_e(k)，y_p(k+p)表示k+p时刻的输出预测值，Δu(k+m-1)、Δd_e(k+m-1)分别为k+m-1时刻的控制输入增量和干扰值观测增量，如下：

基于扰动增量式数学模型(8)推导出MPC预测系统的动态输出为：

Y_p(k+p)＝S_xx_m(k)+S_uΔU(k)+S_dΔD_e(k) (10)

式中：S_x、S_u、S_d为常系数矩阵；

与常规MPC相比，式(10)考虑了时变干扰对系统的不利影响，ΔD_e(k)不再是定常值，而是观测器观测到的时变干扰，MPC采集到k时刻的x_m(k)和k+m时刻内的ΔU(k)、ΔD_e(k)实现预测系统未来k+p步的动态输出；

步骤五、将船舶运动控制问题转化为求解二次规划问题：

设置期望输出值R(k+p)，为保证船舶实际输出能够快速地跟踪设置的期望艏摇角和横摇角，同时控制增量较小，搭建如下目标函数J：

J＝[R(k+p)-Y_p(k+p)]^TQ[R(k+p)-Y_p(k+p)]+ΔU(k)^TWΔU(k) (11)

式中：Q和W为权重矩阵；

为减少舵机和鳍机的频繁操纵，设计舵角鳍角及其增量约束，为防止求解失败，引入松弛因子ρ₁和ρ₂，同时增加横摇速度约束、艏摇速度约束，满足的扰动增量式数学模型和约束条件如下：

式中：x_m(k)_min、x_m(k)_max分别是状态变量x(k)最小值、最大值矩阵，u(k)_min、u(k)_max分别是舵角鳍角的最小值、最大值矩阵，Δu(k)_min、Δu(k)_max分别是舵角增量和鳍角增量的最小值、最大值矩阵；

步骤六、添加舵角鳍角等约束条件下求解最优控制律：

在上述条件下求解出最优控制律ΔU^*(k)为：

式中：K_r、K_x、K_y、K_d均是常值增益矩阵，K_r＝(S_u ^TQS_u+W)^-1S_u ^T，K_rS_x＝[K_x K_y]，K_d＝K_rS_d，期望输出R(k)＝R(k+p)；

MPC只需输入四个参数R(k)、y(k)、Δx(k)、ΔD_e(k)就可计算出最优控制律ΔU^*(k)，并且控制律满足执行机构输入约束；

步骤七、反馈校正：

将得到ΔU^*(k)中的第1个元素Δu^*(k)作为实际控制输入增量，可得到未来时刻的控制输入量

分别通过滤波器F(k)作用于系统和进入滑模观测器预测系统输出值和干扰值，系统执行控制输入量直到下一采样时刻，根据观测信息重新预测下一段时刻的输出值，再次求解优化目标函数，得到一个新的控制律，再将其作用于系统的下个时刻，循环往复，形成最优控制。

Images