[发明专利]基于UKF滤波和约束模型预测控制的动力定位控制方法

权利要求书

1.一种基于UKF滤波和约束模型预测控制的动力定位控制方法，其特征在于，所述方法包含以下步骤：

步骤1、通过位置参考系统和罗经传感器获取船舶的实际船位(x，y)和航向信息Ψ，传送给UKF状态估计滤波器；

步骤2、UKF状态估计滤波器根据接收到的船位和航向信息(x，y，Ψ)，滤除掉测量信息中的噪声干扰以及其中的高频振荡成分，得到低频船位和航向估计值并对未测量状态进行状态估计，得出纵向/横向线速度和转首角速度估计值并将状态估计信息传送给MPC控制器；

UKF状态估计滤波模型包含船舶低频运动和由波浪引起的高频运动模型，测量信号视为船舶低频运动、波浪高频运动以及测量噪声之和，从而UKF状态估计滤波器可以实现对未测量状态的估计，同时滤除测量信号中的高频成分和测量噪声；UKF状态估计与滤波模型为：

y＝η+η_ω+n

其中：ξ＝[ξ_x，ξ_y，ξ_ψ，x_w，y_w，ψ_w]^T表示波浪引起的船舶高频运动向量，η＝[x，y，ψ]^T表示船舶低频运动分量，b＝[b₁，b₂，b₃]^T表示由风、流、二阶波浪力和未建模动态引起的偏差项，v＝[u，v，r]^T表示船舶的纵/横向速度和转首角速度向量，y表示由传感器测量的船位和航向，其中包含低频运动η和波浪引发的高频振荡运动η_ω＝[x_w，y_w，ψ_w]^T；w_i∈R³，i＝1，2，3为零均值高斯白噪声，表达对过程模型的随机干扰，n∈R³也为零均值高斯白噪声，表达测量系统中的随机干扰；

基于所述UKF状态估计与滤波模型，按以下步骤进行UKF状态估计和滤波：

(1)初始化P₀，确定过程噪声和测量噪声协方差Q，R；

(2)将以上模型表达为状态空间表达式，并在每个时刻步对其进行离散化，得到其离散化状态空间模型：

x_k+1＝f(x_k，u_k)+Γw_k

y_k＝h(x_k)+n_k

其中下标k表示时间步，f(·)为非线性过程模型，h(·)为测量模型，k时刻过程噪声和测量噪声w_k和n_k对应的方差矩阵分别为Q和R；

(3)计算k时刻sigma点：

(4)基于步骤(2)中该时刻滤波模型，采用无迹变换的方法进行状态预测：

(5)获取该时刻测量船位和航向信号y_k；

(6)计算UKF增益矩阵K_k，状态误差协方差矩阵P_k，及状态估计值

(7)将状态估计值传递给MPC控制器，同时返回第(2)步，继续执行UKF状态估计与滤波程序，直到运算结束；

步骤3、MPC控制器将接收到的UKF状态估计滤波值作为预测未来动态的初始条件，通过预测模型预测未来动态，与设定船位(x_d，y_d，ψ_d)相比较，在线考虑系统的推力器约束、操作区约束，对目标函数进行最优求解，产生最优控制指令(X，Y，N)传送给被控的动力定位船舶，从而控制船舶保持在设定船位和航向；采用基于机理模型的模型预测控制，由于不需要对高频运动状态进行响应，因而控制器设计仅需考虑船舶的低频运动模型，同时引入平行坐标系，得到线性化的船舶低频运动控制模型：

其中η_p表示平行坐标系下船舶运动向量，基于以上模型，根据预测控制基本原理，模型预测控制基本步骤如下：

(1)将低频运动模型表达为状态空间表达式，并进行离散化处理，得到离散时间模型：

x(k+1)＝A_dx(k)+B_du(k)

y(k)＝H_dx(k)

(2)确定预测模型，根据预测模型预测未来动态：以最新状态估计值为初始条件，设定预测时域p，控制时域m且m＜p；定义：Δu(k+i)＝u(k+i)-u(k+i-1)，并假定控制时域以外，控制量不变，即：Δu(k+i)＝0，i＝m，m+1，…，p-1，系统未来p步的预测输出方程为：

其中：

(3)定义目标函数，在线考虑系统约束，求解约束优化问题：定义MPC优化控制的性能指标函数为：J(x(k)，ΔU(k))＝||W_y(Y_p(k+1)-R(k+1))||²+||W_uU(k)||²+||W_ΔuΔU(k)||²

其中W_y，W_u，W_Δu为加权矩阵，设定为：

W_y＝blkdiag{Q_y，…，Q_y}，W_u＝blkdiag{R_u，…，R_u}，

W_Δu＝blkdiag{R_Δu，…，R_Δu}Q_y，R_u，R_Δu＞0

R(k+1)为给定的控制输出参考序列，为：

指标函数需满足系统动力学方程，以及推力器和操作区约束，如下：

u_min≤u(k+i)≤u_max，i＝0，1，…，m-1

Δu_min≤Δu(k+i)≤Δu_max，i＝0，1，…，m-1

y_min≤y(k+i)≤y_max，i＝1，…，p

以上优化问题为有不等式约束的二次规划(QP)问题，其解可记为ΔU^*(k)；

(4)将优化解ΔU^*(k)的第一个元素作用于系统，在下一采样时刻，将新的状态估计值作为初始条件，重新求解，进行滚动优化，约束MPC的闭环控制率定义为：

(5)将控制量u(k)＝u(k-1)+Δu(k)作用于系统；

(6)重复进行UKF滤波以及MPC控制过程，直至结束。