CN108445762B - 基于ukf滤波和约束模型预测控制的动力定位控制方法 - Google Patents

Abstract

本发明提供了一种基于UKF滤波和约束模型预测控制的动力定位控制方法，该方法包括：通过传感器获取船舶的位置和航向信息；UKF滤波器接收该测量信息，滤除其中的噪声干扰和波浪高频振荡成分，同时对未测量状态进行状态估计；模型预测控制器接收UKF滤波的低频状态估计值作为预测未来动态的初始条件，在线考虑系统推力器约束和操作区约束，对目标函数进行最优求解，产生最优控制指令传送给被控船舶，从而控制船舶保持在设定船位和航向。方法能避免推力器的频繁动作，减少推力器磨损及能耗，同时提高模型预测控制的控制效果和稳定性，具有较强的工程运用价值。

Description

基于UKF滤波和约束模型预测控制的动力定位控制方法

技术领域

本发明涉及船舶动力定位控制领域，具体涉及一种基于UKF滤波和约束模型预测控制的船舶动力定位控制方法。

背景技术

船舶动力定位(Dynamic Positioning,DP)是深海开发关键技术。动力定位系统对极深海域和恶劣海况具有极强适应性，定位能力强，故广泛地用于深海石油钻井平台、海洋考察船、半潜船、水下潜器、海洋补给/装卸船、海底电缆铺设、海上打捞救生以及军用舰船的定点、循迹、循线或跟踪控制上。

动力定位系统的核心是有计算机组成的控制系统，控制系统的关键是控制器。作为一种智能的计算机控制算法，模型预测控制(Model Predictive Control,MPC)不需要精确的过程模型、在线计算方便、能在线处理约束问题，具有较高的鲁棒性和控制效果。模型预测控制非常适用于船舶动力定位系统这种复杂的多变量控制系统，特别在处理约束问题方面有着明显的优势，使动力定位系统工作点可以接近允许的操作范围边缘，从而可以获得更好的性能与经济效益。

船舶定位过程中，会受到风、浪、流等环境干扰力的作用而使船舶偏离设定船位。其中，一阶波浪力将使船舶产生高频振荡运动，控制器在运算时并不需要对其进行响应，否则将造成推进器频繁动作加剧磨损，同时增加能耗。因而，需要通过滤波器将运动信号中的高频成分和测量噪声滤除，而仅将低频运动分量作为控制器的输入。同时，当状态不可测时，也需要通过状态观测以估计出未知状态。

王元慧等(王元慧，施小成，边信黔.基于模型预测控制的船舶动力定位约束控制.船舶工程,2007,29(3):22-25)在进行模型预测控制器设计时，并未考虑状态估计和滤波，当状态不可测时控制效果会受到影响。王刚等(王刚，李文华，晨海泉，蔺本洁.基于状态估计器的船舶动力定位模型预测控制.大连海事大学学报,2015,41(3):24-28)虽然考虑了对不可测状态进行估计，但并未考虑滤波器设计，则由于测量信号中的噪声和船舶高频运动的存在，会使推进器频繁动作而造成不必要的能量消耗及磨损。

发明内容

本发明的目的是为解决以上问题，提供一种基于UKF滤波和约束模型预测控制的船舶动力定位控制方法，将无迹卡尔曼滤波(Unscented Kalman Filtering，UKF)和MPC控制相结合，通过UKF进行动力定位系统状态估计和滤波设计，以估计出未知状态，并滤除掉高频运动分量和测量噪声，仅将低频运动分量作为控制器的输入设计模型预测控制器，以进一步提高MPC控制器的稳定性和可靠性。

本发明的目的可以通过以下技术方案实现：

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

UKF状态估计滤波器根据接收到的船位和航向信息(x,y,Ψ)，滤除掉测量信息中的噪声信号以及其中的高频振荡成为，得到低频船位和航向估计值

并对未测量状态进行状态估计，得出纵向/横向线速度和转首角速度估计值

并将状态估计信息传送给MPC控制器；

MPC控制器将接收到的UKF状态估计滤波值作为预测未来动态的初始条件，通过预测模型预测未来动态，与设定船位(x_d,y_d,ψ_d)相比较，在线考虑系统的推力器约束、操作区约束，对目标函数进行最优求解，产生最优控制指令(X,Y,N)传送给被控的动力定位船舶，从而控制船舶保持在设定船位和航向。

UKF状态估计滤波器通过以下方法实现：

考虑动力定位船舶由一阶波浪力引起的高频运动，将传感器测量船位和航向看成是低频运动和高频振荡运动之和，从而将UKF状态估计滤波模型及测量模型表达以下状态空间形式：

y＝Hx+n

其中：x＝[ξ^T,η^T,b^T,v^T]^T为系统的状态向量，包括波浪高频运动向量、低频运动向量、偏差项和船舶速度向量；w＝[w₁ ^T,w₂ ^T,w₃ ^T]^T为系统的过程噪声向量，A、B、E、H为相应的系数矩阵。

按以下步骤进行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)步，继续执行程序，直到运算结束。

MPC控制按以下方法实现：

采用基于机理模型的模型预测控制，由于不需要对高频运动状态进行响应，因而控制器设计仅需考虑船舶的低频运动模型。

由于旋转矩阵的存在，以上船舶运动模型是非线性的，为简化问题，引入平行坐标系对其进行线性化。用η_p表示平行坐标系下船舶运动向量，其与地球坐标系下运动量的转换关系为：η_p＝R^T(ψ)η，从而：

得到平行坐标系下船舶线性低频运动状态空间模型：

y_c＝H_cx_c+n_c

其中x_c＝[η_p ^T,v^T]^T为状态变量，y_c为被控输出变量，u＝[X,Y,N]^T表示船舶的纵向、横向控制力和转艏力矩，w_c为包含未建模动态等在内的扰动向量，n_c为测量高斯白噪声，A_c,B_c,E_c,H_c为对应的系数矩阵。

基于以上模型，根据预测控制基本原理，模型预测控制基本步骤如下：

(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为加权矩阵，设定为：

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

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

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

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

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

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

本发明的有益效果为：

本发明提供一种基于UKF滤波和约束模型预测控制的船舶动力定位控制方法，该方法利用UKF方法不需对非线性系统进行线性化、滤波精度和稳定性高的优点，对动力定位系统进行状态估计和滤波，滤除掉高频波浪干扰和测量噪声干扰，同时对不可测量状态进行状态估计，而将低频状态估计值作为控制输入传递给模型预测控制器。模型预测控制器以估计状态做输入，在线考虑系统推力约束和操作区约束，通过在线求解约束优化问题，求解出最优控制力，传递给系统，使动力定位船舶能保持在设定船位。方法能避免推力器的频繁动作，减少推力器磨损及能耗，同时提高模型预测控制的控制效果和稳定性。

附图说明

图1为基于UKF滤波的模型预测控制系统原理图。

图2为不同海况MPC控制的船位输出曲线。

图3为轻浪海况下船舶的控制力输入曲线。

具体实施方式

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

一种基于UKF滤波和约束模型预测控制的船舶动力定位控制方法，通过UKF进行动力定位系统状态估计和滤波设计，以估计出未知状态，并滤除掉高频运动分量和测量噪声，仅将低频运动分量作为控制器的输入设计模型预测控制器，以进一步提高MPC控制器的稳定性和可靠性。

如图1所示，本发明实施步骤如下：

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

步骤2、UKF状态估计滤波器根据接收到的船位和航向信息(x,y,Ψ)，滤除掉测量信息中的噪声信号以及其中的高频振荡成为，得到低频船位和航向估计值

并对未测量状态进行状态估计，得出纵向/横向线速度和转首角速度估计值

并将状态估计信息传送给MPC控制器；

步骤3、MPC控制器将接收到的UKF状态估计滤波值作为预测未来动态的初始条件，通过预测模型预测未来动态，与设定船位(x_d,y_d,ψ_d)相比较，在线考虑系统的推力器约束、操作区约束，对目标函数进行最优求解，产生最优控制指令(X,Y,N)传送给被控的动力定位船舶，从而控制船舶保持在设定船位和航向。

步骤2中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,(i＝1,2,3)}∈R³为零均值高斯白噪声，表达对过程模型的随机干扰，n∈R³也为零均值高斯白噪声，表达测量系统中的随机干扰。

将以上模型表达为状态空间形式为：

y＝Hx+n

其中：x＝[ξ^T,η^T,b^T,v^T]^T为系统的状态向量，w＝[w₁ ^T,w₂ ^T,w₃ ^T]^T为系统的过程噪声向量，A、B、E、H为相应的系数矩阵。

基于以上模型，按以下步骤进行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控制按以下方法实现：

采用基于机理模型的模型预测控制，由于不需要对高频运动状态进行响应，因而控制器设计仅需考虑船舶的低频运动模型，如下：

由于旋转矩阵的存在，以上船舶运动模型是非线性的，为简化问题，引入平行坐标系对其进行线性化。用η_p表示平行坐标系下船舶运动向量，其与地球坐标系下运动量的转换关系为：ηp＝RT(ψ)η，从而：

得到平行坐标系下船舶线性低频运动状态空间模型：

y_c＝H_cx_c+n_c

其中x_c＝[η_p ^T,v^T]^T为状态变量，y_c为被控输出变量，u＝[X,Y,N]^T表示船舶的纵向、横向控制力和转艏力矩，w_c为包含未建模动态等在内的扰动向量，n_c为测量高斯白噪声，A_c,B_c,E_c,H_c为对应的系数矩阵。

基于以上模型，根据预测控制基本原理，模型预测控制基本步骤如下：

(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为加权矩阵，设定为：

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

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

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

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

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

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

以下对本发明的实施例进行说明，应当理解，此处所描述的实施例仅用于说明和解释本发明，并不用于限定本发明。

为验证所发明的基于UKF滤波的约束模型预测控制方法的有效性，以CSII船舶为对象，基于图1所示原理建立Matlab模型进行仿真。CSII为某海洋供应船的1:70比例船模，其所能提供的最大纵/横向推力为2N，最大艏摇力矩为1.5N·m，分别对应真实尺寸船舶参数为686kN和36015kN·m。

MPC控制器约束设定为：

船舶的初始位置位于η₀＝[0m,0m,0°]^T，设定期望位置r＝[1m,0.5m,10°]^T。取预测时域p＝10，控制时域m＝3，采样时间取0.1s。考虑船舶分别受到轻浪(有义波高H_s＝1.0m)、中浪(H_s＝2.5m)和大浪(H_s＝4.0m)作用，分析MPC控制系统对不同海况的适应能力。

图2所示为三种海况下MPC控制器作用下船舶的船位输出，由图可见，MPC控制器能很好的控制船舶到达及保持在期望船位上。

图3为轻浪环境下MPC控制算法求解得出的船舶控制力。

仿真分析结果表明对于不同海况的控制效果良好，验证了控制算法的有效性。

Claims (1)

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控制过程，直至结束。

Images