CN103970021B - 一种基于模型预测控制的松弛动力定位控制系统 - Google Patents

Abstract

本发明公开了一种基于模型预测控制的松弛动力定位控制系统，包括：传感器模块、扩展卡尔曼滤波器、海图计算单元、环境干扰补偿器、控制器、船舶动力装置。传感器模块包括位置参考系统和罗经，将北东坐标信息x、y和航向信息ψ传给扩展卡尔曼滤波器；扩展卡尔曼滤波器估计出系统的状态估计信息，海图计算单元将产生状态期望信息，环境干扰补偿器得到环境干扰力传送给控制器，控制器包括优化器和非线性模型预测控制器，优化器产生最优初始控制值传送给非线性模型预测器，非线性模型预测器产生控制指令传送给船舶动力装置。本发明产生的控制信号是缓慢变化的，从能量角度来看是最优的，有效的减少能耗和推进装置的磨损。

Description

一种基于模型预测控制的松弛动力定位控制系统

技术领域

本发明属于船舶运动控制领域，尤其涉及船舶动力定位的一种基于模型预测控制的松弛动力定位控制系统。

背景技术

随着技术的发展，人们的活动范围也逐渐从沿岸和近海扩展到深远海海域。各项海洋工程都需要装有动力定位的船舶保障和支持。动力定位系统是一种由传感器系统(位置参考系统、风传感器等)、推进装置、动力装置和控制器等组成的闭环反馈控制系统，如附图1所示。动力定位系统采用推力器来提供抵抗风、浪、流等作用在船上的环境力，从而使船尽可能地保持在海平面上要求的位置上。其定位成本不会随着水深增加而增加，并且操作也比较方便，因此动力定位技术成为深海开发的关键技术之一，广泛应用于海洋钻井船、平台支持船、潜水器支持船、管道和电缆敷设船、科学考察船、深海救生船等船舶上。根据动力定位作业所需定位精度不同，船舶动力定位系统一般包括高精度定位模式和松弛定位模式。

随着海上油气资源的开发不断向深海拓展，传统的锚泊定位方式正逐渐被动力定位所取代，依靠海洋结构物的自身动力进行定位已经成为深海油气生产设施的主要选择。船舶在进行不同的动力定位作业时，对动力定位系统的定位精度要求也不一样。在某些海上作业中，对动力定位系统的定位精度要求比较松弛，允许船舶定位在某一范围内，而不是精确的某点上。

发明内容

本发明的目的是提供具有高控制性能的一种基于模型预测控制的松弛动力定位控制系统。

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

一种基于模型预测控制的松弛动力定位控制系统，包括传感器模块、扩展卡尔曼滤波器、海图计算单元、环境干扰补偿器、控制器、船舶动力装置；

传感器模块包括位置参考系统和罗经，位置参考系统采集船舶实际位置经纬度转化为北东坐标系信息x、y后传送给扩展卡尔曼滤波器，罗经采集航向信息ψ传送给扩展卡尔曼滤波器；

扩展卡尔曼滤波器根据接收的北东坐标信息x、y和航向信息ψ估计出的状态估计信息，状态估计信息包括：北向坐标估计值东向坐标估计值航向角估计值纵向速度估计值横向速度估计值转首角速度估计值动力定位流流速估计值和动力定位流流向估计值将状态估计信息分别传送给海图计算单元、环境干扰补偿器和控制器；

海图计算单元根据接收的北向坐标估计值东向坐标估计值航向角估计值和控制台人机界面输入的指令，产生动态的北向坐标期望值x_d、东向坐标期望值y_d、航向角期望值ψ_d，传送给控制器；

环境干扰补偿器根据接收的状态估计信息，使用历史控制信息，得到环境干扰力以缓慢变化的形式传送给控制器；

控制器包括优化器和非线性模型预测控制器，优化器产生最优初始控制值传送给非线性模型预测器，非线性模型预测器产生控制指令传送给船舶动力装置。

本发明一种基于模型预测控制的松弛动力定位控制系统还可以包括：

1、优化器采用线性二次最优控制，

船舶的数学模型为：

$\left[\begin{array}{c}\stackrel{\·}{u}\\ \stackrel{\·}{v}\\ \stackrel{\·}{r}\end{array}\right]=M^{-1}\left[\begin{array}{c}{X}_u{u}_r+X_{\left|u\right|u}\left|u_r\right|u_r+X\\ Y_v{v}_r+Y_{\left|v\right|v}\left|v_r\right|v_r+Y_{r}r+Y\\ N_v{v}_r+N_{r}r+N_{\left|r\right|r}\left|r\right|r+N\end{array}\right]$

其中： $\left[\begin{array}{c}X\\ Y\\ N\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_X+w_X\\ {\mathrm{\τ}}_Y+w_Y\\ {\mathrm{\τ}}_N+w_N\end{array}\right],$ ν＝[u v r]^Τ为船舶附体坐标系下的纵向、横向速度和回转速率，η＝[x y ψ]^Τ为北东大地坐标系下的北东坐标位置和航向角度，M为惯性矩阵，包括船体惯量M_RB和附加质量矩阵M_A， $M=\left[\begin{array}{ccc}m-X_{\stackrel{\·}{u}}& 0& 0\\ 0& m-Y_{\stackrel{\·}{v}}& 0\\ 0& 0& I_z-N_{\stackrel{\·}{r}}\end{array}\right],$ 和推进器的推力τ＝[τ_X τ_Y τ_N]^Τ；

得到船舶数学模型的向量形式：

$\stackrel{\·}{x}=f(x,u)$

$x_0=\hat{x}$

其中x₀为当前时刻得到的状态估计值x＝[u v r]^Τ，u＝[X Y N]^Τ，

优化器采用的最优控制为：

u^*(t,x)＝-R^-1(t)B^Τ(t)K(t)x

其中K(t)是黎卡提矩阵微分方程：

$\stackrel{\·}{K}+\mathrm{KA}\left(t\right)+A^T\left(t\right)K+Q\left(t\right)-\mathrm{KB}\left(t\right)R^{-1}\left(t\right)B^T\left(t\right)K=0$

K(t_f)＝F

的唯一非负解值，状态矩阵 $B\left(t\right)=\frac{\∂f}{\∂u}=M^{-1}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$

性能指标为：

$J\left[u(\·)\right]=\frac{1}{2}{x}^T\left(t_f\right)\mathrm{Fx}\left(t_f\right)+\frac{1}{2}{\text{\∫}}_{t_0}^{t_f}(x^T\left(t\right)Q\left(t\right)x\left(t\right)+u^T\left(t\right)R\left(t\right)u\left(t\right))\mathrm{dt}$

其中对于任意t∈[t₀,t_f]，都有F≥0，Q≥0，R＞0。

2、非线性模型预测控制器为：

$\underset{U,X}{\min }\mathrm{\Φ}(u,x)=\frac{1}{2}\underset{k=1}{\overset{N-1}{\mathrm{\Σ}}}{L}_k+V_N$

L_k＝(x_k-x_d)^TQ(x_k-x_d)+u_k ^TRu_k+Δu_k ^TSΔu_k

V_N＝(x_N-x_d)^TQ_N(x_N-x_d)+u_N ^TR_Nu_N+Δu_N ^TS_NΔu_N)

其中：

Q＝diag{Q_u,Q_v,Q_r,Q_x,Q_y,Q_ψ}

R＝diag{R_X,R_Y,R_N}

S＝diag{S_ΔX,S_ΔY,S_ΔN}

x_k＝[u_k,v_k,r_k,x_k,y_k,ψ_k]^T

x_d＝[0,0,0,x_d,y_d,0,0,0,0]^T

u_k＝[X,Y,N]^Τ。

3、环境干扰补偿器为：

$\stackrel{\·}{\widehat{\mathrm{\ξ}}}=A_w\widehat{\mathrm{\ξ}}+K_1\tilde{y}$

$\stackrel{\·}{\widehat{\mathrm{\η}}}=J\left(y\right)\hat{v}+K_2\tilde{y}$

$\stackrel{\·}{\hat{b}}=-T^{-1}\hat{b}+K_3\tilde{y}$

$M\stackrel{\·}{\hat{v}}=-D\left({\hat{v}}_r\right){\hat{v}}_r+J^T\left(y\right)(\hat{b}+K_4\tilde{y})+\mathrm{\τ}$

$\hat{y}=\widehat{\mathrm{\η}}+C_w\widehat{\mathrm{\ξ}}$

其中：为系统输出估计误差，为船舶高频运动估计向量，为低频运动估计向量，为环境干扰力，K₁、K₂、K₃和K₄为增益矩阵，T为包含时间常数的三维对角矩阵，为阻尼矩阵。

4、扩展卡尔曼滤波器的扩展状态为x＝[x y ψ u v r U_c ψ_c]^Τ，扩展后的系统状态方程为：

$x_{k+1}=\left[\begin{array}{c}{x}^{k+1}\\ y^{k+1}\\ {\mathrm{\ψ}}^{k+1}\\ u^{k+1}\\ v^{k+1}\\ r^{k+1}\\ {U_c}^{k+1}\\ {{\mathrm{\ψ}}_c}^{k+1}\end{array}\right]=\left[\begin{array}{c}{x}^k+T(u^k\cos {\mathrm{\ψ}}^k-v^k\sin {\mathrm{\ψ}}^k)\\ y^k+T(u^k\sin {\mathrm{\ψ}}^k+v^k\cos {\mathrm{\ψ}}^k)\\ {\mathrm{\ψ}}^k+{\mathrm{Tr}}^k\\ u^k+T\frac{X_u}{m-X_{\stackrel{\·}{u}}}{u_r}^k+T\frac{X_{\left|u\right|u}}{m-X_{\stackrel{\·}{u}}}\left|{u_r}^k\right|{u_r}^k+\frac{T}{m-X_{\stackrel{\·}{u}}}{{\mathrm{\τ}}_X}^k\\ v^k+T(\frac{Y_v}{m-Y_{\stackrel{\·}{v}}}{v_r}^k+\frac{Y_{\left|v\right|v}}{m-Y_{\stackrel{\·}{v}}}|{v_r}^k|{v_r}^k+\frac{Y_r}{m-Y_{\stackrel{\·}{v}}}{r}^k)+\frac{T}{m-Y_{\stackrel{\·}{v}}}{{\mathrm{\τ}}_Y}^k\\ r^k+T(\frac{N_v}{I_z-N_{\stackrel{\·}{r}}}{v_r}^k+\frac{N_r}{I_z-N_{\stackrel{\·}{r}}}{r}^k+\frac{N_{\left|r\right|r}}{I_z-N_{\stackrel{\·}{r}}}|r^k\left|r^k\right)+\frac{T}{I_z-N_{\stackrel{\·}{r}}}{{\mathrm{\τ}}_N}^k\\ {U_c}^k\\ {{\mathrm{\ψ}}_c}^k\end{array}\right]=f(x_k,u_k)$

测量向量y＝[x y ψ]^Τ，测量方程为：y_k＝hx_k，h＝[I_3×3 0_3×5]，I为单位阵，0为零阵，

扩展卡尔曼滤波器的滤波过程为：

步骤一：初始扩展卡尔曼滤波器和P₀，

步骤二：获得传感器模块输出的北东坐标信息x、y和航向信息ψ，

步骤三：执行扩展卡尔曼滤波算法：

$\hat{x}(k+1|k)=f(\hat{x}\left(k\right),u\left(k\right))$

P(k+1|k)＝F(k)·P(k)·F^Τ(k)

K(k+1)＝P(k+1|k)·h^Τ(k+1)·[h(k+1)·P(k+1|k)·h^Τ(k+1)]^-1，

$\hat{x}(k+1)=\hat{x}(k+1|k)+K(k+1)[y(k+1)-h(k+1)\hat{x}(k+1|k)]$

P(k+1)＝[I-K(k+1)·h(k+1)]·P(k+1|k)

步骤四：得到状态估计信息，并传送给海图计算单元、环境干扰补偿器和控制器。

5、优化器的最优性能指标为：

$J^{*}=\frac{1}{2}{x_0}^{T}K\left(t_0\right)x_0.$

本发明包括四个主要部分，每个部分的特点为：

1、环境干扰补偿器

推进器根据指令产生缓慢变化的控制力，用于补偿平均环境力。环境干扰补偿器在补偿环境力时产生慢变、光滑的控制力，虽然不能完全补偿环境力，但能使船舶对环境响应变弱，这时船舶仍然是可以随着环境变化产生微小变化，从而减小推进器能耗及磨损。

2、卡尔曼滤波器

预测控制器中需要的位置、艏向、速度、角速度和海流等信息均通过卡尔曼滤波器估计。

3、优化器

基于当前系统状态信息，对预测模型进行线性化，将非线性控制问题转化为二次最优控制问题，优化器以解析的形式得到控制系统的最优解。以此最优解作为非线性模型预测控制器的初始解，以保证控制的实时性，同时有效避开局部极值，改善控制系统的控制品质。

4、非线性模型预测控制器

非线性模型预测控制器，在满足自定义约束前提下，通过采用实时优化技术来获得使动力定位船舶表现最优性能的控制指令。

预测控制器既可以预测船位运动轨迹，同时也能根据所预测的船位轨迹计算出最优控制力，使船舶定位在给定范围内部。假定预测步长为N个采样步长，当预测船位趋向于远离工作区域，控制器会缓慢地增加控制力使船舶回到工作区域；当预测船位在某步长之后会超出工作区域，控制器会平滑地、更快地增加控制力使船舶保持在工作区域附近或者内部。

预测控制器有两种启动模式：普通海况模式和极端海况模式。普通海况模式下，控制器能将船舶有效地定位在工作区范围内。极端海况模式下，要加入环境干扰补偿器，与控制器共同将船舶定位在工作区范围内。

本发明的有益效果为：

本发明将一种基于模型预测控制的控位方法应用于动力定位系统，使船舶定位在某范围内而不超出其边界。此动力定位系统产生的控制信号是缓慢变化的，从能量角度来看是最优的，这样既可以减少能耗和推进装置的磨损，又能满足绿色节能的经济要求。以最优解作为非线性模型预测控制器的初始解，以保证控制的实时性，同时有效避开局部极值，提高控制系统的控制性能。

附图说明

图1为松弛动力定位系统总体结构图；

图2为松弛动力定位系统功能示意图；

图3为实船试验中北东坐标系的轨迹图。

具体实施方式

下面将结合附图对本发明做进一步详细说明。

如图1所示，基于模型预测控制的松弛动力定位控制装置包括海图计算单元1，船舶2，传感器3，扩展卡尔曼滤波器6，环境干扰补偿器10和控制器7。传感器3包括位置参考系统4和罗经5，控制器3包括优化器8和非线性模型预测控制器9。各模块的建立和设计过程如下，其中主要工作包括四部分：环境干扰补偿器、卡尔曼滤波器、优化器和非线性模型预测控制器。

本发明在于将一种基于模型预测控制的控位方法应用于动力定位系统，使船舶定位在某范围内而不超出其边界，如附图2所示。此动力定位系统产生的控制信号是缓慢变化的，从能量角度来看是最优的，这样既可以减少执能耗和推进装置的磨损，又能满足绿色节能的经济要求。

1.传感器模块

传感器模块3包括位置参考系统4和罗经5，位置参考系统4采集的动力定位船舶实际位置经纬度信息转化为北东坐标信息x、y和罗经5采集的航向角信息ψ传送到扩展卡尔曼滤波器单元6。

2.海图计算单元

海图计算单元1根据从扩展卡尔曼滤波器6接收的船舶信息和控制台人机界面输入的指令，经过选定的路径规划或者海图计算算法产生动态的期望信息x_d、y_d和并将期望信息传送到控制单元7。

3.扩展卡尔曼滤波器

扩展卡尔曼滤波器单元6根据接收的位姿信息x、y和ψ，经过扩展卡尔曼滤波算法估计出系统的状态变量，包括北向坐标东向坐标航向角纵向速度横向速度转首角速度动力定位流流速和动力定位流流向并为海图计算单元1、环境干扰补偿器10和控制器7提供所需的状态信息。具体算法如下。

扩展卡尔曼滤波器在对船舶各状态进行估计时，采用的数学模型为：

$\left[\begin{array}{c}{u}_{k+1}\\ v_{k+1}\\ r_{k+1}\end{array}\right]=\left[\begin{array}{c}{u}_k\\ v_k\\ r_k\end{array}\right]+T\·M^{-1}\left[\begin{array}{c}{X}_u{u}_{\mathrm{rk}}+X_{\left|u\right|u}\left|u_{\mathrm{rk}}\right|u_{\mathrm{rk}}+{\mathrm{\τ}}_{\mathrm{Xk}}\\ Y_v{v}_{\mathrm{rk}}+Y_{\left|v\right|v}\left|v_{\mathrm{rk}}\right|v_{\mathrm{rk}}+Y_r{r}_k+{\mathrm{\τ}}_{\mathrm{Yk}}\\ N_v{v}_{\mathrm{rk}}+N_r{r}_k+N_{\left|r\right|r}\left|r_k\right|r_k+{\mathrm{\τ}}_{\mathrm{Nk}}\end{array}\right]---\left(1\right)$

此模型中并未包含环境干扰力项，而是将船舶所受到的环境力认为是定常流的作用，由此估计出的海流不是真实的海流，而是包含未建模、海浪等干扰在内的总称，称其为DP流，代表的是一种综合的海洋环境干扰。系统的扩展状态为x＝[x y ψ uv r U_c ψ_c]^Τ，扩展后的系统状态方程为：

$x_{k+1}=\left[\begin{array}{c}{x}^{k+1}\\ y^{k+1}\\ {\mathrm{\ψ}}^{k+1}\\ u^{k+1}\\ v^{k+1}\\ r^{k+1}\\ {U_c}^{k+1}\\ {{\mathrm{\ψ}}_c}^{k+1}\end{array}\right]=\left[\begin{array}{c}{x}^k+T(u^k\cos {\mathrm{\ψ}}^k-v^k\sin {\mathrm{\ψ}}^k)\\ y^k+T(u^k\sin {\mathrm{\ψ}}^k+v^k\cos {\mathrm{\ψ}}^k)\\ {\mathrm{\ψ}}^k+{\mathrm{Tr}}^k\\ u^k+T\frac{X_u}{m-X_{\stackrel{\·}{u}}}{u_r}^k+T\frac{X_{\left|u\right|u}}{m-X_{\stackrel{\·}{u}}}\left|{u_r}^k\right|{u_r}^k+\frac{T}{m-X_{\stackrel{\·}{u}}}{{\mathrm{\τ}}_X}^k\\ v^k+T(\frac{Y_v}{m-Y_{\stackrel{\·}{v}}}{v_r}^k+\frac{Y_{\left|v\right|v}}{m-Y_{\stackrel{\·}{v}}}|{v_r}^k|{v_r}^k+\frac{Y_r}{m-Y_{\stackrel{\·}{v}}}{r}^k)+\frac{T}{m-Y_{\stackrel{\·}{v}}}{{\mathrm{\τ}}_Y}^k\\ r^k+T(\frac{N_v}{I_z-N_{\stackrel{\·}{r}}}{v_r}^k+\frac{N_r}{I_z-N_{\stackrel{\·}{r}}}{r}^k+\frac{N_{\left|r\right|r}}{I_z-N_{\stackrel{\·}{r}}}|r^k\left|r^k\right)+\frac{T}{I_z-N_{\stackrel{\·}{r}}}{{\mathrm{\τ}}_N}^k\\ {U_c}^k\\ {{\mathrm{\ψ}}_c}^k\end{array}\right]=f(x_k,u_k)---\left(2\right)$

记成紧凑的向量形式：

x_k+1＝f(x_k,u_k)

y_k＝hx_k (3)

量测向量y＝[x y ψ]^Τ，因此有：

h＝[I_3×3 0_3×5] (4)

其中，I为单位阵，0为零阵。

扩展卡尔曼滤波器的步骤如下：

1)初始化扩展卡尔曼滤波器和P₀，

2)获取传感器测量值，

3)执行扩展卡尔曼滤波算法

$\begin{array}{c}\hat{x}(k+1|k)=f(\hat{x}\left(k\right),u\left(k\right))\\ P(k+1|k)=F\left(k\right)\·P\left(k\right)\·F^T\left(k\right)\\ K(k+1)=P(k+1|k)\·h^T(k+1)\·[h(k+1)\·P(k+1|k)\·h^T(k+1){]}^{-1}\\ \hat{x}(k+1)=\hat{x}(k+1|k)+K(k+1)[y(k+1)-h(k+1)\hat{x}(k+1|k)]\\ P(k+1)=[I-K(k+1)\·h(k+1)]\·P(k+1|k)\end{array}---\left(5\right)$

4)将估计值传送到需要的模块，

5)判断是否需要终止滤波，若继续，转步骤2)，若终止，转步骤6)

6)终止滤波。

4.环境干扰补偿器

根据接收的扩展卡尔曼滤波器6估计出的系统状态信息，环境干扰补偿器10)使用历史控制信息，基于预测模型，估计出作用于预测模型的环境力，经过一定的低通滤波处理后，环境干扰力以缓慢变化的形式传送给控制器7，用于补偿平均环境力。环境干扰补偿器在补偿环境力时产生慢变、光滑的控制力，虽然不能完全补偿环境力，但能使船舶对环境响应变弱，从而减小推进器的频繁变化，降低能耗及推进器的磨损。

由于海洋环境扰动作用力的检测和计算十分复杂和困难，没有精确的数学模型来描述，鉴于干扰作用在纵荡、横荡及艏摇三个自由度上是缓慢变化的，因此将环境力估计模型表述为一阶高斯——马尔可夫过程：

$\stackrel{\·}{b}=-T^{-1}b+E_b{\mathrm{\ω}}_b---\left(6\right)$

其中，b为三维向量，表示环境干扰力和力矩；T为包含时间常数的三维对角矩阵；E_b为三维对角矩阵，表示环境干扰力的幅值范围；ω_b为零均值高斯白噪声向量。该模型可以描述由以下因素引起的慢变环境力和力矩：

1)风；

2)二阶波浪漂移力；

3)海流；

4)未建模动态。

在考虑未建模动态和环境干扰合力时，本发明统一将其称为环境干扰，环境干扰估计器方程如下：

$\begin{array}{c}\stackrel{\·}{\widehat{\mathrm{\ξ}}}=A_w\widehat{\mathrm{\ξ}}+K_1\tilde{y}\\ \stackrel{\·}{\widehat{\mathrm{\η}}}=J\left(y\right)\hat{v}+K_2\tilde{y}\\ \stackrel{\·}{\hat{b}}=-T^{-1}\hat{b}+K_3\tilde{y}\\ M\stackrel{\·}{\hat{v}}=-C\left({\hat{v}}_r\right){\hat{v}}_r+J^T\left(y\right)(\hat{b}+K_4\tilde{y})+\mathrm{\τ}\\ \tilde{y}=\widehat{\mathrm{\η}}+C_w\widehat{\mathrm{\ξ}}\end{array}---\left(7\right)$

其中：为系统输出估计误差；为船舶高频运动估计向量；为低频运动估计向量；为缓变环境干扰力的估计向量；K₁、K₂、K₃和K₄为估计器的增益矩阵。

控制系统的上一采样时刻的状态和控制输入为已知，可以通过环境干扰估计器来得到缓变、光滑的环境干扰作用 $\hat{b}={\left[\begin{array}{ccc}{w}_X& w_Y& w_N\end{array}\right]}^T.$

5.控制器

控制器7包括优化器8和非线性模型预测控制器9，控制器7从海图计算单元1接收期望信息、从扩展卡尔曼滤波器单元6接收状态估计信息、从环境干扰补偿器10接收环境干扰估计力信息，并将这些信息传送给优化器8和非线性模型预测控制器9，得到最优的控制输入——控制力，将控制指令传送到船舶动力装置。该控制力减少了能源消耗和推进装置的磨损，推力变化平滑，使船舶定位在某范围内而不超出其范围边界。

6.优化器

优化器采用的是线性二次最优控制。在每个采样周期内，获取已经估计出来的当前系统状态信息，对预测模型进行线性化，将非线性控制问题转化为二次最优控制问题。根据非线性船舶数学模型，记：

$\left[\begin{array}{c}X\\ Y\\ N\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_X+w_X\\ {\mathrm{\τ}}_Y+w_Y\\ {\mathrm{\τ}}_N+w_N\end{array}\right]---\left(8\right)$

则有：

$\left[\begin{array}{c}\stackrel{\·}{u}\\ \stackrel{\·}{v}\\ \stackrel{\·}{r}\end{array}\right]=M^{-1}\left[\begin{array}{c}{X}_u{u}_r+X_{\left|u\right|u}\left|u_r\right|u_r+X\\ Y_v{v}_r+Y_{\left|v\right|v}\left|v_r\right|v_r+Y_{r}r+Y\\ N_v{v}_r+N_{r}r+N_{\left|r\right|r}\left|r\right|r+N\end{array}\right]---\left(9\right)$ 以向量形式重述此模型，形式如下：

$\begin{array}{c}\stackrel{\·}{x}=f(x,u)\\ x_0=\hat{x}\end{array}---\left(10\right)$

其中x0为当前时刻得到的系统状态估计值x＝[u v r]^Τ，u＝[X Y N]^Τ。

将非线性矢量函数f在当前状态值附近进行泰勒展开，有：

$\begin{array}{c}\stackrel{\·}{x}=A\left(t\right)x+B\left(t\right)u\\ x_0=\hat{x}\end{array}---\left(11\right)$

其中

$A\left(t\right)=\frac{\∂f}{\∂x}=M^{-1}\left[\begin{array}{ccc}{X}_u+2X_{\left|u\right|u}\left|u_r\right|& 0& 0\\ 0& Y_v+2Y_{\left|v\right|v}\left|v_r\right|& Y_r\\ 0& N_v& N_r+2N_{\left|r\right|r}\left|r\right|\end{array}\right]---\left(12\right)$

$B\left(t\right)=\frac{\∂f}{\∂u}=M^{-1}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(13\right)$

性能指标为

$J\left[u(\·)\right]=\frac{1}{2}{x}^T\left(t_f\right)\mathrm{Fx}\left(t_f\right)+\frac{1}{2}{\text{\∫}}_{t_0}^{t_f}(x^T\left(t\right)Q\left(t\right)x\left(t\right)+u^T\left(t\right)R\left(t\right)u\left(t\right))\mathrm{dt}---\left(14\right)$

其中对于任意t∈[t₀,t_f]，都有F≥0，Q≥0，R＞0。

综上所述的线性二次调节器的最优控制可表达为：

u^*(t，x)=-R^-1(t)B^T(t)K(t)x (15)

其中K(t)是如下黎卡提矩阵微分方程

$\begin{array}{c}\stackrel{\·}{K}+\mathrm{KA}\left(t\right)+A^T\left(t\right)+Q\left(t\right)-\mathrm{KB}\left(t\right)R^{-1}\left(t\right)B^T\left(t\right)K=0\\ K\left(t_f\right)=F\end{array}---\left(16\right)$

的唯一非负解值。而最优性能指标为

$J^{*}=\frac{1}{2}{x_0}^{T}K\left(t_0\right)x_0---\left(17\right)$

7.非线性模型预测控制器

实际动力定位控制装置属于非线性有约束优化范畴，包括船舶动态特性约束和控制输入饱和约束两部分。在设计船舶运动控制系统的时候要考虑到船舶动力系统的能力限制。本动力定位船舶的推进器饱和约束条件为：

X_P∈[-9.8×10⁵N,1.5×10⁶N] (18)

Y_P∈[-4.17×10⁵N,4.17×10⁵N] (19)

N_P∈[-1.8×10⁷Nm,1.8×10⁷Nm] (20)

为了避免出现饱和现象，在控制器设计过程中必须满足约束条件，这里的控制算法采用的是能够直接处理约束条件的非线性模型预测控制算法。预测控制问题本质上是一类优化控制问题，是以当前和预测船位偏离定位中心点的程度和控制力、控制力变化率为目标函数的优化问题。

假定船舶期望点为原点，期望艏向为正北0度，各方向速度均为0。任意期望位置点和艏向角度都可以通过代数换算转化为上述零点，不失一般性，后续均以零点为船舶动力定位目标点，记为x_d＝[0 0 0 0 0 0]^Τ。预测控制器中的状态向量为x＝[u v r x y ψ]^Τ，分别代表船舶重心的前进速度、横移速度、回转角速度、大地坐标系中北向坐标位置、东向坐标位置和船舶艏向角度，由卡尔曼滤波器给出；控制向量为u＝[X Y N]^Τ，分别代表作用到船舶上的纵向、横向控制力和回转控制力矩指令。模型预测控制器的目的之一在于减小船舶当前状态与目标状态的偏差，因此控制器性能函数中要包含状态偏差项x^ΤQx；目的之二在于以最小的控制器力和力矩达到最优的控制效果，因此控制器性能函数中要包含控制力指令项u^ΤRu；目的之三在于减小控制器指令的跳变，使控制力缓慢变化，因此引入控制力增量项Δu^ΤSΔu。适当的调节控制器参数，从而使船舶尽可能的沿径向线回归定位中心附近。数学描述如下：

$\underset{U,X}{\min }\mathrm{\Φ}(u,x)=\frac{1}{2}\underset{k=1}{\overset{N-1}{\mathrm{\Σ}}}{L}_k+V_N---\left(21\right)$

L_k＝(x_k-x_d)^TQ(x_k-x_d)+u_k ^TRu_k+Δu_k ^TSΔu_k (22)

V_N＝(x_N-x_d)^TQ_N(x_N-x_d)+u_N ^TR_Nu_N+Δu_N ^TS_NΔu_N) (23)

其中：

Q＝diag{Q_u,Q_v,Q_r,Q_x,Q_y,Q_ψ}

R＝diag{R_X,R_Y,R_N}

S＝diag{S_ΔX,S_ΔY,S_ΔN}

x_k＝[u_k,v_k,r_k,x_k,y_k,ψ_k]^T

x_d＝[0,0,0,x_d,y_d,0,0,0,0]^T

u_k＝[X,Y,N]^Τ.

8.实船试验

本装置在实船上进行了动力定位功能海上实船试验。动力定位船舶初始位置为(102.0,259.0)，初始艏向为83.0°。在三级海况下进行松弛定位试验，期望定位中心点设置与初始点重合，工作区范围设为半径10m。试验如图3所示，图3为船舶历史轨迹图，横向坐标轴方向表示大地坐标系东向位置坐标，纵向坐标轴方向代表大地坐标系正北方向，小红圈代表期望定位中心点，大红圈代表工作区边界，为半径10m的圆，蓝线代表实船的位置轨线。从图3中可以看到，普通海况下船舶被有效的控制在工作区范围内部。鉴于实船试验期间未遭遇极端海况，对极端海况模式未进行实船验证。

Claims (6)

1.一种基于模型预测控制的松弛动力定位控制系统，其特征在于：包括传感器模块、扩展卡尔曼滤波器、海图计算单元、环境干扰补偿器、控制器、船舶动力装置；

传感器模块包括位置参考系统和罗经，位置参考系统采集船舶实际位置经纬度转化为北东坐标系信息x、y后传送给扩展卡尔曼滤波器，罗经采集航向信息ψ传送给扩展卡尔曼滤波器；扩展卡尔曼滤波器根据接收的北东坐标信息x、y和航向信息ψ估计出的状态估计信息，状态估计信息包括：北向坐标估计值东向坐标估计值航向角估计值纵向速度估计值横向速度估计值转首角速度估计值动力定位流流速估计值和动力定位流流向估计值将状态估计信息分别传送给海图计算单元、环境干扰补偿器和控制器；

海图计算单元根据接收的北向坐标估计值东向坐标估计值航向角估计值和控制台人机界面输入的指令，产生动态的北向坐标期望值x_d、东向坐标期望值y_d、航向角期望值ψ_d，传送给控制器；

环境干扰补偿器根据接收的状态估计信息，使用历史控制信息，得到环境干扰力以缓慢变化的形式传送给控制器；

控制器包括优化器和非线性模型预测控制器，优化器产生最优初始控制值传送给非线性模型预测器，非线性模型预测器产生控制指令传送给船舶动力装置。

2.根据权利要求1所述的一种基于模型预测控制的松弛动力定位控制系统，其特征在于：

所述的优化器采用线性二次最优控制，

船舶的数学模型为：

其中：ν＝[u v r]^Τ为船舶附体坐标系下的纵向、横向速度和回转速率，η＝[x y ψ]^Τ为北东大地坐标系下的北东坐标位置和航向角度，M为惯性矩阵，包括船体惯量M_RB和附加质量矩阵M_A，和推进器的推力τ＝[τ_X τ_Y τ_N]^Τ；

得到船舶数学模型的向量形式：

其中x₀为当前时刻得到的状态估计值x＝[u v r]^Τ，u＝[X Y N]^Τ，

优化器采用的最优控制为：

u^*(t，x)=-R^-1(t)B^T(t)K(t)x

其中K(t)是黎卡提矩阵微分方程：

K(t_f)＝F

的唯一非负解值，状态矩阵

性能指标为：

其中对于任意t∈[t₀,t_f]，都有F≥0，Q≥0，R＞0。

3.根据权利要求2所述的一种基于模型预测控制的松弛动力定位控制系统，其特征在于：所述的非线性模型预测控制器为：

L_k＝(x_k-x_d)^TQ(x_k-x_d)+u_k ^TRu_k+Δu_k ^TSΔu_k

V_N＝(x_N-x_d)^TQ_N(x_N-x_d)+u_N ^TR_Nu_N+Δu_N ^TS_NΔu_N)

其中：

Q＝diag{Q_u,Q_v,Q_r,Q_x,Q_y,Q_ψ}

R＝diag{R_X,R_Y,R_N}

S＝diag{S_ΔX,S_ΔY,S_ΔN}

x_k＝[u_k,v_k,r_k,x_k,y_k,ψ_k]^T

x_d＝[0,0,0,x_d,y_d,0,0,0,0]^T

u_k＝[X,Y,N]^Τ。

4.根据权利要求3所述的一种基于模型预测控制的松弛动力定位控制系统，其特征在于：所述的环境干扰补偿器为：

其中：为系统输出估计误差，为船舶高频运动估计向量，为低频运动估计向量，为环境干扰力，K₁、K₂、K₃和K₄为增益矩阵，T为包含时间常数的三维对角矩阵，为阻尼矩阵。

5.根据权利要求4所述的一种基于模型预测控制的松弛动力定位控制系统，其特征在于：所述的扩展卡尔曼滤波器的扩展状态为x＝[x y ψ u v r U_c ψ_c]^Τ，扩展后的系统状态方程为：

测量向量y＝[x y ψ]^Τ，测量方程为：y_k＝hx_k，h＝[I_3×3 0_3×5]，I为单位阵，0为零阵，

扩展卡尔曼滤波器的滤波过程为：

步骤一：初始扩展卡尔曼滤波器和P₀，

步骤二：获得传感器模块输出的北东坐标信息x、y和航向信息ψ，

步骤三：执行扩展卡尔曼滤波算法：

P(k+1|k)＝F(k)·P(k)·F^Τ(k)

K(k+1)＝P(k+1|k)·h^Τ(k+1)·[h(k+1)·P(k+1|k)·h^Τ(k+1)]^-1，

P(k+1)＝[I-K(k+1)·h(k+1)]·P(k+1|k)

步骤四：得到状态估计信息，并传送给海图计算单元、环境干扰补偿器和控制器。

6.根据权利要求5所述的一种基于模型预测控制的松弛动力定位控制系统，其特征在于：所述的优化器的最优性能指标为：