CN113110468B - 一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法 - Google Patents

Abstract

本发明公开了一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，包括：船舶运动模型参数辨识步骤基于扩展卡尔曼滤波方法，将实际靠泊过程发生的频繁错车、倒车等因素考虑在辨识的运动模型结构中，实现靠泊航行过程中船舶运动模型参数的自适应修正。模型预测控制步骤和PID控制步骤，利用非线性模型预测控制和PID控制技术实现靠泊过程的路径规划和跟踪控制。其中，利用非线性模型预测控制规划的航线考虑船舶非线性运动特性和实际环境与障碍物因素的影响，PID控制解决非线性模型预测控制求解速度慢、求解周期较长的局限，克服非线性模型预测周期内模型参数变化、环境干扰等因素变化对非线性模型预测控制方法生成控制指令的缺陷。

Description

一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法

技术领域

本发明涉及船舶自动控制技术领域，特别涉及一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法。

背景技术

欠驱动双桨双舵船舶主要是指军用舰船和民事专用船舶，该类船舶在靠泊阶段航速低、舵效差，通常需要螺旋桨错车辅助改变航向。再加上浅水、低速、岸壁效应以及环境干扰等因素的影响，使得靠泊操纵成为船舶操纵中最复杂、最困难的操纵之一。实际靠泊实践中，通常需要有经验的船长和引航员协助靠泊，受人为因素影响较大，可靠性难以保证。因此利用计算机程序实现船舶自身的自主靠泊控制具有重要的现实意义。

由于欠驱动船舶不能跟踪任意路径且港内操纵水域有限，靠泊路径规划需要充分考虑船舶本身操纵运动特性和控制能力等因素的影响，设计的靠泊路径需保证船舶具有误差允许范围内的航线跟踪能力，且能自主适应港内环境干扰和固定、移动碍障物变化对靠泊路径规划和控制的影响。

公开日2014年7月30日，公开号CN 103955218 A，发明名称为“一种基于非线性控制理论的无人艇轨迹跟踪控制装置及方法”的专利申请，提出一种欠驱动无人艇自适应轨迹跟踪位置及控制方法，该方法能够在惯性质量参数和水动力参数未知情况下，基于非线性控制理论实现欠驱动无人艇的轨迹跟踪控制。但该方法属于给定轨迹条件下的跟踪控制，轨迹指定缺乏动态航行时的环境适应性，且该方法最终得到的推力和转艏力矩指令信息，无法明确执行机构指令分配。

公开日2018年7月10日，公开号CN 108267955 A，发明名称为“面向无人艇自主靠泊的运动控制方法”的专利申请，将欠驱动无人艇靠泊控制器分成远端和近端两种模式，根据靠泊行为改变进行控制参数的动态自适应调整。但该专利提出的控制方法针对航速和航向进行单独控制，不能解决欠驱动双桨双舵船舶舵和螺旋桨错车协同控制改变航向的实际需求。此外，期望航速和航向提前指定，无法根据环境和码头碍障物变化实时调整，缺乏自主性。

公开日2018年9月21日，公开号CN 108563130 A，发明名称为“一种欠驱动船舶自适应神经网络自动靠泊控制方法，设备及介质”的专利申请提出采用附加控制方法解决欠驱动控制器设计的难题，利用航海动态深度信息的神经网络重构不确定的模型动态参数与未知扰动向量。但该方法没有考虑港内碍障物对控制的影响，且在艏向控制中只考虑舵的控制，尚未计及车的影响，因此不适合双桨双舵船舶的自主靠泊设计。

综上所述，现有的欠驱动水面船舶控制方法或没有考虑港内环境和碍障物变化对靠泊路径的影响；或没有考虑舵和螺旋桨错车对艏向角改变的协同控制；或没有考虑船舶自身的操纵运动特性，均无法很好解决欠驱动双桨双舵船舶自主靠泊过程中存在的实际问题。

发明内容

鉴于上述问题，本发明提供的一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，该方法考虑港内环境和碍障物变化对靠泊路径的影响，也考虑舵和螺旋桨错车对艏向角改变的协同控制，且根据船舶自身的操纵运动特性，可很好解决欠驱动双桨双舵船舶自主靠泊过程中存在的实际问题。

本发明实施例提供一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，包括：

船舶运动模型参数辨识步骤：获取船舶自身运动状态测量设备采集到的船舶运动历史数据，采用扩展卡尔曼滤波算法，生成欠驱动双桨双舵船舶运动模型，得到模型参数的估计值；

模型预测控制步骤：根据所述欠驱动双桨双舵船舶运动模型，获取目标泊位和固定、移动碍障物相对船舶的位置信息形成约束条件，并获取船舶周围风速和流速信息形成运动干扰输入；作为当前状态条件；

利用非线性模型预测控制求取基于所述当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列；

PID控制步骤：基于模型预测控制步骤获得的靠泊路线和期望状态；所述期望状态包括运动状态和期望位置；利用船舶当前位置和艏向角与期望位置、艏向角的偏差结合PID控制形成反馈控制指令；根据所述控制指令实现船舶自主靠泊的控制。

在一个实施例中，所述控制指令的分配，包括：

(1)纵向速度控制由左侧和右侧螺旋桨的合力决定；

(2)艏向角控制由舵角决定，当舵角控制不足时，利用左侧和右侧螺旋桨的转速差形成的力矩补充。

在一个实施例中，根据所述控制指令实现船舶自主靠泊的控制，包括：根据所述控制指令的分配，通过调整PID参数，确定左右侧螺旋桨转速和舵角指令。

在一个实施例中，所述欠驱动双桨双舵船舶运动模型如下：

(1)式中，船纵向速度u、横荡速度v、艏摇角速度r、左侧螺旋桨转速n_l、右侧螺旋桨转速n_r以及舵角δ为需要的量测数据，通过船舶自身运动状态测量设备获得；a_i(i＝1～7)、b_i(i＝1～7)、c_i(i＝1～8)为需要辨识的未知模型参数。

在一个实施例中，所述未知模型参数的计算过程，包括：

将(1)式变形为一般的系统方程：

(2)式中：t代表时间，s(t)代表t时刻的输入值，w(t)代表t时刻的动态噪声，e(t)代表t时刻的量测噪声；X(t)代表t时刻由船舶运动状态和运动模型待辨识参数组成的列向量，y(t)代表t时刻船舶运动状态的量测值，H为量测矩阵；

将(2)式离散化可得:

(3)式中k代表采样的当前时刻，k+1代表采样的下一时刻，s(k)代表T(k)和T(k+1)两个时刻输入的采样均值，T代表采样的时间间隔，ω(k)和e(k)代表T(k)时刻的动态噪声和量测噪声，方差分别为Q和R；

根据(1)式与(4)式得到

到

的表达式为：

(5)式中dt为数值仿真时间步长，k为采样的当前时刻；

综上得扩展卡尔曼滤波递推公式为：

式中：P(k+1|k)为预报误差协方差阵，P(k+1)为估计误差协方差阵，K(k+1)为滤波增益阵，

为状态预报，

为状态估计，

进行递推计算，最终得到模型参数a_i、b_i和c_i的估计值。

在一个实施例中，获取目标泊位和固定、移动碍障物相对船舶的位置信息形成约束条件，包括两类船舶运动的边界约束条件；其中，设定S_S为港口区域，S_b为船舶运动区域，S_o为浮标障碍物的等效圆区域，则船舶运动的边界约束分为两类：

第一类：船舶运动始终在港口区域内，

第二类：船舶不与障碍物发生碰撞，S_b∩S_S；

对于第一类约束，转化为S_b边界上的点始终位于凸多边形S_S内来处理，即：

S_s＝{x^NED|A_sx^NED＜b_s}………………………………(8)

(8)式中，x^NED为大地坐标系中S_b边界的顶点坐标组成的列向量，A_s为与S_b边界的直线斜率组成的系数矩阵，b_s为S_b边界的直线截距组成的列向量；

利用随船坐标系和大地坐标系之间的转化关系，式(8)中的约束以式(9)中的非线性约束来表示；

(9)式中，ψ为船舶的艏向角，

为船舶重心在大地坐标系中的坐标，R为局部坐标系和大地坐标系的坐标变换矩阵，即：

对于第二类约束，用下式表达：

(11)式中，x_i ^NED、y_i ^NED为大地坐标系下S_b顶点的横纵坐标，O_x、O_y为障碍物区域等效圆的圆心坐标，r为障碍物区域的等效圆半径。

在一个实施例中，利用非线性模型预测控制求取基于所述当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列；包括：

令期望靠泊点的坐标和姿态η_d＝[x_d,y_d,ψ_d]^T,则靠泊路径规划和控制问题转化为如下非线性连续最优控制问题；

式(12)中，F(X_p(t),u_p(t))为代价函数，t代表时间，X_p＝[u_p,v_p,r_p,x_p,y_p,ψ_p]^T为规划的状态量,u_p为规划的纵向速度，v_p为规划的横向速度，r_p为规划的转艏角速度，x_p为规划的重心北向位置，y_p为规划的重心东向位置，ψ_p为规划的艏向角；u_p＝[n_lp,n_rp,δ_p]^T为规划的输入指令,n_lp为规划的左侧螺旋桨转速；n_rp为规划的右侧螺旋桨转速；δ_p为规划的舵角；n_p为规划的左右侧螺旋桨转速；约束条件如下：

(13)式中，δ_max和n_max分别为舵角和转速幅值限制；代价函数F(X_p(t),u_p(t))由下式表达：代价函数F(X_p(t),u_p(t))由下式表达：

(14)式中，pseudo-Huber损失函数H(a)的形式为

利用非线性模型预测控制技术，求解式(14)和式(15)组成的约束优化问题，获得所述当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列。

在一个实施例中，利用船舶当前位置和艏向角与期望位置、艏向角的偏差结合PID控制形成反馈控制指令，包括：

采用基于MPC的靠泊路径规划和基于PID控制的靠泊路径跟踪控制分级实现策略；

其中，

为实际运动位置与规划位置的偏差；η(t)＝[x(t),y(t),ψ(t)]^T为实际运动位置，η_p(t)＝[x_p(t),y_p(t),ψ_p(t)]^T为规划运动位置；K_p、K_i、K_d分别为PID反馈控制器比例系数、积分系数和微分系数。

与现有技术相比，本发明公开提供了一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，具有如下优点：

本发明针对欠驱动双桨双舵船舶自主靠泊时的控制问题，基于扩展卡尔曼滤波方法，将实际靠泊过程发生的频繁错车、倒车等因素考虑在辨识的运动模型结构中，实现靠泊航行过程中船舶运动模型参数的自适应修正。在此基础上，利用非线性模型预测控制和PID控制技术实现靠泊过程的路径规划和跟踪控制。其中，利用非线性模型预测控制规划的航线考虑船舶非线性运动特性和实际环境与障碍物因素的影响，PID控制解决非线性模型预测控制求解速度慢、求解周期较长的局限，克服非线性模型预测周期内模型参数变化、环境干扰等因素变化对非线性模型预测控制方法生成控制指令的缺陷。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明实施例提供的参数辨识流程图；

图2为本发明实施例提供的卡尔曼滤波器、船舶运动动态系统和量测之间的关系示意图；

图3为本发明实施例提供的大地坐标系下码头水域靠泊边界信息示意图；

图4为本发明实施例提供的模型预测控制算法原理图；

图5为本发明实施例提供的实际靠泊过程的靠泊路径规划和跟踪控制分级实现策略示意图；

图6为本发明实施例提供的船舶自动靠泊实例示意图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

本发明提供的应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其整体包括三部分：船舶运动模型参数辨识步骤、模型预测控制步骤以及PID控制步骤。

其中，船舶运动模型参数辨识步骤：获取船舶自身运动状态测量设备采集到的船舶运动历史数据，采用扩展卡尔曼滤波算法，生成欠驱动双桨双舵船舶运动模型，得到模型参数的估计值；

在该步骤中，针对船舶运动历史数据采用扩展卡尔曼滤波方法获得船舶在某一段时间内的运动模型。为考虑靠泊过程中的螺旋桨错车及倒车工况，欠驱动双桨双舵船舶运动模型辨识结构如下：

(1)式中，船纵向速度u、横荡速度v、艏摇角速度r、左侧螺旋桨转速n_l、右侧螺旋桨转速n_r以及舵角δ为需要的量测数据，可通过船舶自身运动状态测量设备获得；a_i(i＝1～7)、b_i(i＝1～7)、c_i(i＝1～8)为需要辨识的未知模型参数。

模型预测控制步骤：根据欠驱动双桨双舵船舶运动模型，获取目标泊位和固定、移动碍障物相对船舶的位置信息形成约束条件，并获取船舶周围风速和流速信息形成运动干扰输入；作为当前状态条件；利用非线性模型预测控制求取基于当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列。

在模型预测控制步骤中，基于参数辨识获得的运动模型，利用船舶自身安装的激光雷达和导航雷达获取目标泊位和固定、移动碍障物相对船舶的位置信息形成约束条件，利用船舶自身安装的气象仪和声学多普勒流速剖面仪获取周围风速和流速信息形成运动干扰输入。在上述信息获取的基础上，利用非线性模型预测控制思想求取基于当前状态的靠泊路线、运动状态沿靠泊路线分布、控制指令序列。

PID控制步骤：基于模型预测控制步骤获得的靠泊路线和期望状态；期望状态包括运动状态和期望位置；利用船舶当前位置和艏向角与期望位置、艏向角的偏差结合PID控制形成反馈控制指令；根据控制指令实现船舶自主靠泊的控制。

PID控制部分是考虑模型预测控制的求解周期和控制周期的差，基于模型预测控制获得的靠泊路线和期望状态，利用当前位置和艏向角与期望位置、艏向角的偏差结合PID控制形成反馈控制指令。指令分配的原则如下：

(1)纵向速度控制由左侧和右侧螺旋桨的合力决定；

(2)艏向角控制主要由舵角决定，当舵角控制不足时，利用左侧和右侧螺旋桨的转速差形成的力矩补充。

根据上述原则，通过调整PID参数，确定左右侧螺旋桨转速和舵角指令。

下面分别对上述步骤进行详细的说明：

在船舶运动模型参数辨识步骤中，通过船舶港内操纵运动机理分析，确定运动模型的结构如(1)式所示：

从式(1)可知，为了辨识船舶的运动模型，需要的量测数据为：船纵向速度u、横荡速度v、艏摇角速度r、左侧螺旋桨转速n_l、右侧螺旋桨转速n_r以及舵角δ的时域历程。a_i(i＝1～7)、b_i(i＝1～7)、c_i(i＝1～8)为需要辨识的未知模型参数。

将式(1)变形为一般的系统方程有：

(2)式中：t代表时间，s(t)代表t时刻的输入值，w(t)代表t时刻的动态噪声，e(t)代表t时刻的量测噪声；X(t)代表t时刻由船舶运动状态和运动模型待辨识参数组成的列向量，y(t)代表t时刻船舶运动状态的量测值，H为量测矩阵；

将(2)式离散化可得:

(3)式中k代表采样的当前时刻，k+1代表采样的下一时刻，s(k)代表T(k)和T(k+1)两个时刻输入的采样均值，T代表采样的时间间隔，ω(k)和e(k)代表T(k)时刻的动态噪声和量测噪声，方差分别为Q和R；

根据(1)式与(4)式得到

到

的表达式为：

(5)式中dt为数值仿真时间步长，k为采样的当前时刻；

综上可得扩展卡尔曼滤波递推公式为：

(6)式中：P(k+1|k)为预报误差协方差阵，P(k+1)为估计误差协方差阵，K(k+1)为滤波增益阵，

为状态预报，

为状态估计，

船舶运动数学模型的参数辨识过程可参照图1所示，其中辨识准则采用基于最小方差估计的扩展卡尔曼滤波方法。对于一般的线性离散系统：

卡尔曼滤波器、动态系统和量测之间的关系如图2所示，其中k代表当前采样时刻，k+1代表下一时刻，X代表状态向量，Y代表输出向量，Φ为状态转移矩阵，G为输出矩阵，Z代表离散时间信号的Z变换，C为输入矩阵。利用卡尔曼滤波算法进行船舶运动最优状态估计时，假设动态噪声ω(k)和量测噪声v(k)均为高斯白噪声序列，均方差分别为R₁(k)和R₂(k)。假设X(0)为高斯随机向量，状态向量的均值选为初值即

方差阵Var[X(0)]＝P(0)，则船舶运动状态预报公式由下式给出：

预报误差协方差阵为：

P(k+1|k)＝ΦP(k)Φ^T+R₁(k)………………………(7.3)

卡尔曼滤波增益阵的计算公式：

K(k+1)＝P(k+1|k)C^T[CP(k+1|k)C^T+R₂(k+1)]^-1…………(7.4)

状态估计更新公式：

状态滤波误差协方差计算公式：

P(k+1)＝[I-K(k+1)C]P(k+1|k)……………………(7.6)

由式(6)进行递推计算，经过一定的次数，船舶运动模型参数会达到一个收敛状态(收敛状态的判别标准与模型的准确性要求有关)，从而状态值x(k)可以得到，最终得到模型参数a_i、b_i和c_i的估计值。在实际的应用中还需要对数据进行平滑处理，采用在线辨识的方法，对于某些受海况影响较大的参数可以自适应修正。

在模型预测控制步骤中，在获得港内操纵运动模型的基础上，利用非线性模型预测控制技术获得靠泊过程的路径规划的期望运动状态。

针对码头水域大小和障碍物信息，将靠泊区域用图3所示的多边形来描述。其中，N表示正北，E表示正东，S_S为港口区域，S_b为船舶运动区域，S_o为浮标等障碍物的等效圆区域，则船舶运动的边界约束主要分为两类：

(1)船舶运动始终在港口区域内，即

(2)船舶不与障碍物发生碰撞，即S_b∩D_s＝Φ。

对于第一类约束，可转化为S_b边界上的点始终位于凸多边形S_S内来处理，即：

S_s＝{x^NED|A_sx^NED＜b_s}………………………………(8)

(8)式中，x^NED为大地坐标系中S_b边界的顶点坐标组成的列向量，A_s为与S_b边界的直线斜率组成的系数矩阵，b_s为S_b边界的直线截距组成的列向量；利用随船坐标系和大地坐标系之间的转化关系，式(8)中的约束以式(9)中的非线性约束来表示；

(9)式中，ψ为船舶的艏向角，

为船舶重心在大地坐标系中的坐标，R为局部坐标系和大地坐标系的坐标变换矩阵，即：

对于第二类约束，用下式表达：

(11)式中，x_i ^NED、y_i ^NED为大地坐标系下S_b顶点的横纵坐标，O_x、O_y为障碍物区域等效圆的圆心坐标，r为障碍物区域的等效圆半径。

令期望靠泊点的坐标和姿态η_d＝[x_d,y_d,ψ_d]^T,则靠泊路径规划和控制问题转化为如下非线性连续最优控制问题；

式(12)中，F(X_p(t),u_p(t))为代价函数，t代表时间，X_p＝[u_p,v_p,r_p,x_p,y_p,ψ_p]^T为规划的状态量,u_p为规划的纵向速度，v_p为规划的横向速度，r_p为规划的转艏角速度，x_p为规划的重心北向位置，y_p为规划的重心东向位置，ψ_p为规划的艏向角；u_p＝[n_lp,n_rp,δ_p]^T为规划的输入指令,n_lp为规划的左侧螺旋桨转速；n_rp为规划的右侧螺旋桨转速；δ_p为规划的舵角；n_p为规划的左右侧螺旋桨转速；约束条件如下：

(13)式中，δ_max和n_max分别为舵角和转速幅值限制；代价函数F(X_p(t),u_p(t))由下式表达：

(14)式中，pseudo-Huber损失函数H(a)的形式为

利用非线性模型预测控制技术，求解式(14)和式(15)组成的约束优化问题，获得当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列。模型预测控制的原理如图4所示。其中，k表示当前时刻，y_d为船舶运动状态的设定输出，在靠泊时为靠泊结束时期望的船舶运动状态。y_r表示系统的参考轨迹，u为控制输入，y为系统的实际输出值，y_m为模型输出，y_p为预测输出，i为预测时域的长度，m为控制时域的长度，m≤i。利用内点法求解非线性优化问题，控制序列的第一个元素作为实际控制量。

利用模型预测控制技术求解实际问题时，靠泊控制规划和控制指令同时生成，但直接采用上述靠泊指令会存在一定的风险。由于波浪增阻、环境干扰和建模误差等因素的存在，利用模型预测控制技术生成的靠泊控制指令的控制效果会存在偏差。此外，模型预测控制求解速度较慢，不利于稳定船舶运动状态。鉴于上述原因，还需要PID控制步骤，实际靠泊控制采用基于MPC的靠泊路径规划和基于PID控制的靠泊路径跟踪控制分级实现策略，原理如图5所示。

根据泊位点和障碍物信息，生成靠泊路径规划和船舶运动状态η(t)，控制系统则通过PID反馈控制，控制左右螺旋桨转速和舵角指令按照既定路径运动。在运动过程中，根据实际船舶运动状态，实时反馈，重新调整靠泊路径并调整PID反馈控制，最终实现船舶靠泊到位。

其中，

为实际运动位置与规划位置的偏差；

η(t)＝[x(t),y(t),ψ(t)]^T为实际运动位置，η_p(t)＝[x_p(t),y_p(t),ψ_p(t)]^T为规划运动位置；K_p、K_i、K_d分别为PID反馈控制器比例系数、积分系数和微分系数。

利用本发明提出的算法获得的船舶靠泊实例在X-Y坐标系的位置变化如图6所示。其中，XY坐标轴分别表示两个方向的距离。

本发明针对欠驱动双桨双舵船舶自主靠泊时的控制问题，基于扩展卡尔曼滤波方法，将实际靠泊过程发生的频繁错车、倒车等因素考虑在辨识的运动模型结构中，实现靠泊航行过程中船舶运动模型参数的自适应修正。在此基础上，利用非线性模型预测控制和PID控制技术实现靠泊过程的路径规划和跟踪控制。其中，利用非线性模型预测控制规划的航线考虑船舶非线性运动特性和实际环境与障碍物因素的影响，PID控制解决非线性模型预测控制求解速度慢、求解周期较长的局限，克服非线性模型预测周期内模型参数变化、环境干扰等因素变化对非线性模型预测控制方法生成控制指令的缺陷。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (6)

1.一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其特征在于，包括：

船舶运动模型参数辨识步骤：获取船舶自身运动状态测量设备采集到的船舶运动历史数据，采用扩展卡尔曼滤波算法，生成欠驱动双桨双舵船舶运动模型，得到模型参数的估计值；

模型预测控制步骤：根据所述欠驱动双桨双舵船舶运动模型，获取目标泊位和固定、移动碍障物相对船舶的位置信息形成约束条件，并获取船舶周围风速和流速信息形成运动干扰输入；作为当前状态条件；

利用非线性模型预测控制求取基于所述当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列；

PID控制步骤：基于模型预测控制步骤获得的靠泊路线和期望状态；所述期望状态包括运动状态和期望位置；利用船舶当前位置和艏向角与期望位置、艏向角的偏差结合PID控制形成反馈控制指令；根据所述控制指令实现船舶自主靠泊的控制；

其中，获取目标泊位和固定、移动碍障物相对船舶的位置信息形成约束条件，包括两类船舶运动的边界约束条件；其中，设定S_S为港口区域，S_b为船舶运动区域，S_o为浮标障碍物的等效圆区域，则船舶运动的边界约束分为两类：

第一类：船舶运动始终在港口区域内，

第二类：船舶不与障碍物发生碰撞，S_b∩S_S；

对于第一类约束，转化为S_b边界上的点始终位于凸多边形S_S内来处理，即：

S_s＝{x^NED|A_sx^NED＜b_s}………………………………(8)

(8)式中，x^NED为大地坐标系中S_b边界的顶点坐标组成的列向量，A_s为与S_b边界的直线斜率组成的系数矩阵，b_s为S_b边界的直线截距组成的列向量；

利用随船坐标系和大地坐标系之间的转化关系，式(8)中的约束以式(9)中的非线性约束来表示；

(9)式中，ψ为船舶的艏向角，

为船舶重心在大地坐标系中的坐标，R为局部坐标系和大地坐标系的坐标变换矩阵，即：

对于第二类约束，用下式表达：

(11)式中，x_i ^NED、y_i ^NED为大地坐标系下S_b顶点的横纵坐标，O_x、O_y为障碍物区域等效圆的圆心坐标，r为障碍物区域的等效圆半径；

利用非线性模型预测控制求取基于所述当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列；包括：

令期望靠泊点的坐标和姿态η_d＝[x_d,y_d,ψ_d]^T,则靠泊路径规划和控制问题转化为如下非线性连续最优控制问题；

式(12)中，F(X_p(t),u_p(t))为代价函数，t代表时间，X_p＝[u_p,v_p,r_p,x_p,y_p,ψ_p]^T为规划的状态量,u_p为规划的纵向速度，v_p为规划的横向速度，r_p为规划的转艏角速度，x_p为规划的重心北向位置，y_p为规划的重心东向位置，ψ_p为规划的艏向角；u_p＝[n_lp,n_rp,δ_p]^T为规划的输入指令,n_lp为规划的左侧螺旋桨转速；n_rp为规划的右侧螺旋桨转速；δ_p为规划的舵角；n_p为规划的左右侧螺旋桨转速；约束条件如下：

(13)式中，δ_max和n_max分别为舵角和转速幅值限制；代价函数F(X_p(t),u_p(t))由下式表达：

(14)式中，pseudo-Huber损失函数H(a)的形式为

利用非线性模型预测控制技术，求解式(14)和式(15)组成的约束优化问题，获得所述当前状态条件的靠泊路线、运动状态沿靠泊路线分布、控制指令序列。

2.如权利要求1所述的一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其特征在于，所述控制指令的分配，包括：

(1)纵向速度控制由左侧和右侧螺旋桨的合力决定；

(2)艏向角控制由舵角决定，当舵角控制不足时，利用左侧和右侧螺旋桨的转速差形成的力矩补充。

3.如权利要求2所述的一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其特征在于，根据所述控制指令实现船舶自主靠泊的控制，包括：根据所述控制指令的分配，通过调整PID参数，确定左右侧螺旋桨转速和舵角指令。

4.如权利要求1所述的一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其特征在于，所述欠驱动双桨双舵船舶运动模型如下：

(1)式中，船纵向速度u、横荡速度v、艏摇角速度r、左侧螺旋桨转速n_l、右侧螺旋桨转速n_r以及舵角δ为需要的量测数据，通过船舶自身运动状态测量设备获得；a_i(i＝1～7)、b_i(i＝1～7)、c_i(i＝1～8)为需要辨识的未知模型参数。

5.如权利要求4所述的一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其特征在于，所述未知模型参数的计算过程，包括：

将(1)式用船舶运动非线性数学模型的状态空间形式描述：

(2)式中：t代表时间，s(t)代表t时刻的输入值，w(t)代表t时刻的动态噪声，e(t)代表t时刻的量测噪声；X(t)代表t时刻由船舶运动状态和运动模型待辨识参数组成的列向量，y(t)代表t时刻船舶运动状态的量测值，H为量测矩阵；

将(2)式离散化可得:

X(k+1)＝f^a(X(k),s(k),k)+ω(k)

y(k)＝HX(k)+e(k)

(3)(3)式中k代表采样的当前时刻，k+1代表采样的下一时刻，s(k)代表T(k)和T(k+1)两个时刻输入的采样均值，T代表采样的时间间隔，ω(k)和e(k)代表T(k)时刻的动态噪声和量测噪声，方差分别为Q和R；

根据(1)式与(4)式得到f₁ ^a到

的表达式为：

(5)式中dt为数值仿真时间步长，k为采样的当前时刻；

综上得扩展卡尔曼滤波递推公式为：

式中：P(k+1|k)为预报误差协方差阵，P(k+1)为估计误差协方差阵，K(k+1)为滤波增益阵，

为状态预报，

为状态估计，

进行递推计算，最终得到模型参数a_i、b_i和c_i的估计值。

6.如权利要求1所述的一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法，其特征在于，利用船舶当前位置和艏向角与期望位置、艏向角的偏差结合PID控制形成反馈控制指令，包括：

采用基于MPC的靠泊路径规划和基于PID控制的靠泊路径跟踪控制分级实现策略；

其中，

为实际运动位置与规划位置的偏差；η(t)＝[x(t),y(t),ψ(t)]^T为实际运动位置，η_p(t)＝[x_p(t),y_p(t),ψ_p(t)]^T为规划运动位置；K_p、K_i、K_d分别为PID反馈控制器比例系数、积分系数和微分系数。

Images