CN103576555B - 一种动力定位船舶循迹导引控制方法 - Google Patents

Abstract

本发明公开了一种动力定位船舶循迹导引控制方法。该算法首先建立大地坐标系和船体坐标系，以及船舶的数学模型；然后通过设定路径点生成的路径与船舶之间的几何位置关系生成导引策略，计算船舶当前时刻的期望艏向；应用控制算法得到控制船舶达到期望艏向所需的控制力矩；最后计算控制船舶达到期望运动速度所需要的纵向推力。该算法根据航迹信息以及航迹和船舶的相对位置计算高速循迹直线段和转弯段船舶的期望艏向，并通过纵向推力控制船舶的运动速度，计算过程简单易行，适合于工程应用。

Description

一种动力定位船舶循迹导引控制方法

技术领域

本发明涉及一种对直线航迹段和转弯航迹段采取分段几何导引策略，仅使用纵向速度控制和艏向角控制的动力定位船舶欠驱动路径跟踪控制方法，特别涉及一种动力定位船舶循迹导引控制方法。

背景技术

循迹功能，也叫路径跟踪，是动力定位船舶的重要功能，是动力定位系统的组成部分，在海洋作业和海上运输中扮演着重要的角色。根据任务和船舶运动速度的不同，循迹可分为低速循迹和高速循迹，低速循迹一般指速度在3节以下的循迹，而高速循迹的速度一般认为高于3节。对海底的搜索以及铺管、铺缆作业等特殊任务的完成都需要循迹功能的支持。比如在管道铺设时就会要求船速不能太快，船舶需要沿设定航迹以较高的精度运动，此时船舶执行功能，而且可以任意指定船舶艏向。高速循迹是相对于低速循迹而言的，海洋搜索时，要求速度较高，既要保证循迹精度，又要求对运动速度进行控制。

船舶的航迹大多由设定航迹点通过直线和圆弧的连接组成。在两个航迹点之间的长距离直线航行司空见惯，通常由传统的航向自动舵功能就可以完成。但航向自动舵不能直接控制航迹偏差，难以使船舶即满足艏向要求又保证循迹的精度，已不能满足海上运输的要求。而在船舶高速循迹时，动力定位船舶的横向推进器效能会随着速度的增加急剧减小，导致船舶出现欠驱动的现象，在船舶向下一条航迹切换是必须提前通过一个转弯圆弧进行过渡，在此过程中船舶需要通过对圆弧航迹的循迹来保证控制精度。因此，高速循迹功能的控制问题引起人们极大的关注，并很快成为当今船舶运动控制研究中的一个热点。

动力定位船舶的控制系统由导航系统、导引策略和控制算法三部分组成，其中导引策略可以为控制算法提供期望的位置和艏向信息，控制算法根据这些信息给出控制指令即可。所以对于执行高速循迹功能动力定位船舶，设计合适的导引策略是重要环节；为了保证控制的精度和鲁棒性，性能良好且适合工程应用的控制算法也必不可少。

现有技术中，关于动力定位船舶路径跟踪的文献有很多，但考虑工程实践的文献并不多。如挪威理工大学的Fossen教授和他的团队发表的“Handbookofmarinecrafthydrodynamicsandmotioncontrol（JohnWiley&SonsLtd,2011）”，宽泛地介绍了近年来对动力定位船舶的研究成果，包括先进导引策略的原理和控制算法的原理，以及动力定位系统其他方面的相关内容。哈尔滨工程大学的边信黔、付明玉、王元慧教授在其著作《船舶动力定位》（科学出版社,2011）中给出了高速航迹控制在工程实践中的方法的概略性介绍。

现有技术中，国际上主流的动力定位产品已经包含了高速循迹的功能，北欧挪威Kongsberg公司已经对循迹功能进行了研究，其产品说明书《Kongsberg-SDPDynamicPositioningSystem》中给出了一些关于高速循迹的说明和功能概况，但没有给出实现方法。哈尔滨工程大学研制的动力定位产品也实现了高速循迹功能，但没有相关文献进行介绍。

发明内容

本发明的目的在于，针对动力定位船舶的高速循迹功能提出一种分段导引策略，为控制算法提供期望艏向输入，然后结合非线性反步法设计控制算法，得到对船舶纵向和艏向的控制力和力矩，驱动船舶以较高的精度和要求的速度完成循迹功能。

为实现上述目的，本发明采用的技术方案包括下列步骤：

（1）选取海平面上任意一点为原点，以正东方向为横轴、正北方向为纵轴，建立大地坐标系EO_EN。然后，以船舶的几何中心为原点，以右舷方向为横轴、船艏方向为纵轴，建立船体坐标系YO_bX；

（2）在大地坐标系和船体坐标系中建立动力定位船舶三自由度船舶高速运动数学模型：

$\begin{array}{c}\stackrel{\·}{\mathrm{\η}}=\mathrm{Jv}\\ M\stackrel{\·}{v}+D\left(v\right)v+C\left(v\right)v=\mathrm{\τ}\end{array}$

式中，η表示船舶在大地坐标系中位置和艏向向量[n,e,ψ]^Τ；v表示船舶在船体坐标系中线速度和角速度向量[u,v,r]^Τ；τ为推进器产生的力和力矩向量[F_x,F_y,N_z]^Τ；J为从船体坐标到大地坐标的转换矩阵， $J=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right];$ M为船舶惯性矩阵， $M=\left[\begin{array}{ccc}m-X_{\stackrel{\·}{u}}& 0& 0\\ 0& m-Y_{\stackrel{\·}{v}}& {\mathrm{mx}}_g-Y_{\stackrel{\·}{r}}\\ 0& {\mathrm{mx}}_g-N_{\stackrel{\·}{v}}& I_z-N_{\stackrel{\·}{r}}\end{array}\right];$ m为船舶质量；I_z为转动惯量；x_g为船体坐标系中船舶质心纵向坐标；其他参数均为一阶水动力导数；

D(v)=D+D_n(v)为船舶水动力阻尼项；

$D=-\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& Y_r\\ 0& N_v& N_r\end{array}\right]$ 为线性部分；

$D_n\left(v\right)=\left[\begin{array}{ccc}{X}_{u\left|u\right|}\left|u\right|& 0& 0\\ 0& Y_{v\left|v\right|}\left|v\right|+Y_{\left|r\right|v}\left|r\right|& Y_{\left|v\right|r}\left|v\right|+Y_{\left|r\right|r}\left|r\right|\\ 0& N_{v\left|v\right|}\left|v\right|+N_{\left|r\right|v}\left|r\right|& N_{\left|v\right|r}\left|v\right|+N_{\left|r\right|r}\left|r\right|\end{array}\right]$ 为非线性部分；

$C=\left[\begin{array}{ccc}0& 0& -(m-Y_{\stackrel{\·}{v}})v-({\mathrm{mx}}_g-Y_{\stackrel{\·}{r}})r\\ 0& 0& (m-X_{\stackrel{\·}{u}})u\\ (m-Y_{\stackrel{\·}{v}})v+({\mathrm{mx}}_g-Y_{\stackrel{\·}{r}})r& -(m-X_{\stackrel{\·}{u}})u& 0\end{array}\right]$ 为科里奥利和向心力矩阵；

（3）通过船体设定的路径点和当前时刻船舶的大地坐标位置，获得当前时刻的船舶期望艏向。主要包括以下几个部分：

①设定路径点，生成船舶运动航迹线

设DP船舶需要跟踪的航迹由大地坐标系中的设定路径点为A,B,C三点，它们的坐标为分别为(n₁,e₁)，(n₂,e₂)，(n₃,e₃)。大地坐标系中船舶当前时刻的位置为(n,e)。

②直线航迹段船舶期望艏向的计算方法

根据船舶当前位置与直线段航迹的偏差，即船舶到直线的垂直距离计算一个修正角，在航迹向的基础上加上这个修正角，使船舶艏向偏向航迹一边。

对于一条从A点指向B点的航迹线，当前时刻船舶与航迹AB的偏差d为：

$d=\frac{(e_1-e_2)(n-n_2)-(n_1-n_2)(e-e_2)}{\sqrt{(n_2-n_1)(n_2-n_1)+(e_2-e_1)(e_2-e_1)}}$

可知，当d>0时，船舶在有向航迹线的右侧；当d<0时，船舶在有向航迹线的左侧。航迹线方向ψ_trace∈(-π,π)为，有向线段AB与大地坐标北向的夹角：

ψ_trace=arctan2((e₂-e₁)/(n₂-n₁))

此时船舶期望艏向ψ_d计算方法为：ψ_d=ψ_trace-Kt*d。Kt为从航迹偏差到修正角度的转换增益。

这样当船舶位于航迹线右侧时船舶以航迹方向为基础，通过向左转向来靠近航迹；当船舶位于航迹线左侧时，船舶通过向右转向靠近航迹；当船舶位于航迹线上时船舶的期望艏向为航迹线方向。

③转弯航迹段船舶期望艏向的计算方法

为船舶在高速循迹的航迹切换部分，设计一个和两条有向直线航迹AB和BC内切的圆弧，船舶通过对这段圆弧的循迹，平滑地从AB段过渡到下一条直线航迹BC段。

圆弧段航迹的确定通过船舶当前时刻的速度u和期望转艏角速度r_d获得，首先计算圆弧半径：R=u/r_d，以及两条航迹的航迹向差：λ=ψ_{trace_2}-ψ_{trace_1}。进而通过B点的大地坐标确定转弯圆弧的圆心大地坐标位置(n_R,e_R)：

n_R=n₂+R/(λ/2)·cos(ψ_{trace_1}+λ/2)

e_R=e₂+R/(λ/2)·sin(ψ_{trace_1}+λ/2)

船舶开始转弯后，当前期望艏向ψ_d(k)根据期望转艏角速度，AB段直线航迹的航迹向和采样时间T_s来计算：ψ_d(k)=ψ_d(k-1)+r_d·T_s，ψ_d(k)的初始值为AB段直线航迹的航迹向ψ_{trace_1}。

在转弯过程中，航迹偏差根据船舶当前位置与圆弧路径的距离获得。根据船舶当前位置(n,e)与圆心(n_R,e_R)的距离判定船舶是否位于圆弧上：d_arc=R时，船舶在圆弧上；当d_arc≠R时船舶不在圆弧上。

当船舶在圆弧上时，船舶期望艏向ψ_d计算方法为：ψ_d=ψ_d(k)；当船舶不在圆弧上时，仿照直线段循迹的计算方法来获得期望艏向，此时航迹偏差为d=d_arc-R，船舶期望艏向ψ_d计算方法为：ψ_d=ψ_d(k)-sgn·Kt·d。sgn=-1,1为一符号函数，当船舶向左转时sgn=1，当船舶向右转时sgn=-1。

当｜ψ_d-ψ_{trace_2}｜<ε时，船舶则开始对下一条直线段航迹进行循迹，期望艏向的计算方法如步骤②，直到最后一条航迹，循迹结束。

（4）利用反步法，根据当前时刻期望艏向ψ_d与船舶的期望速度u_d，计算控制船舶高速循迹所需的纵向力F_x和力矩N_z。主要包括以下几个步骤：

①定义误差z₁=ψ-ψ_d，z₂=v-ξ，并定义H=[001]

令对z₁求导有：

${\stackrel{\&CenterDot;}{z}}_1=\stackrel{\&CenterDot;}{\mathrm{\&psi;}}-{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_d=r-r_d=\mathrm{Hv}-r_d={\mathrm{Hz}}_2+{\mathrm{\&xi;}}_3-r_d;$ 对z₂求导有：

$M{\stackrel{\&CenterDot;}{z}}_2=M\stackrel{\&CenterDot;}{v}-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}}=\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}};$

②选择李雅普诺夫候选函数

则沿z₁z₂对V求导有：

$\stackrel{\&CenterDot;}{V}=z_1{\stackrel{\&CenterDot;}{z}}_1+{z_2}^{T}M{\stackrel{\&CenterDot;}{z}}_2=z_1({\mathrm{Hz}}_2+{\mathrm{\&xi;}}_3-r_d)+{z_2}^T(\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}});$

③选择虚拟控制器ξ₃=-αz₁+r_d，其中α>0

可得到： $\stackrel{\&CenterDot;}{V}=z_1{\stackrel{\&CenterDot;}{z}}_1+{z_2}^{T}M{\stackrel{\&CenterDot;}{z}}_2=-\mathrm{\&alpha;}{z_1}^2+{z_2}^T(H^T{z}_1+\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}});$

④选择控制器 $\mathrm{\&tau;}={[F_x,F_y,N_z]}^T=D\left(v\right)v+C\left(v\right)v+M\stackrel{\&CenterDot;}{\mathrm{\&xi;}}-\mathrm{\&beta;}{z}_2-H^T{z}_1,$

其中β=diag(β₁,β₂,β₃)>0为设计参数。

则：即此控制器使得系统渐进稳定。

联合步骤（2）中建立的船舶模型，令F_y=0，将所设计的控制器τ展开。

令ξ₁=u_d，得到纵向控制为： $F_x=(m-X_{\stackrel{\&CenterDot;}{u}}){\stackrel{\&CenterDot;}{u}}_d-X_{u}u-{\mathrm{\&beta;}}_1(u-u_d);$

令经整理后船舶艏向控制力矩为：

$\begin{array}{c}{N}_z=(\mathrm{\&kappa;}{\mathrm{\&beta;}}_2+\mathrm{\&kappa;}{Y}_v-N_v)v+((\mathrm{\&kappa;}({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}})-(I_z-N_{\stackrel{\&CenterDot;}{r}}))\mathrm{\&alpha;}-{\mathrm{\&beta;}}_3)(r-r_d)+(({\mathrm{mx}}_{g}u-N_r)-\mathrm{\&kappa;}(\mathrm{mu}-Y_r))r\\ +((I_z-N_{\stackrel{\&CenterDot;}{r}})-\mathrm{\&kappa;}({\mathrm{mx}}_g-N_{\stackrel{\&CenterDot;}{v}})){\stackrel{\&CenterDot;}{r}}_d-((\mathrm{\&kappa;}(\mathrm{mu}-Y_r)+{\mathrm{\&beta;}}_3)\mathrm{\&alpha;}+1)(\mathrm{\&psi;}-{\mathrm{\&psi;}}_d)-{\mathrm{\&kappa;\&beta;}}_2{\mathrm{\&xi;}}_2-\mathrm{\&kappa;}{N}_v\mathrm{\&alpha;}\end{array}$

（5）将控制力和力矩传递给动力定位船舶的数学模型，解算下一时刻船舶的状态信息，然后由当前时刻更新到下一时刻；

（6）重复执行步骤（2）--步骤（5），直到最后一条航迹循迹完成，结束计算。

本发明的技术构思为：本发明针对DP船舶对直线和圆弧所组成路径的高速循迹功能，通过为直线路径和圆弧路径分别计算船舶到路径的距离的分段导引策略，将船舶与设定路径的位置偏差转换成艏向偏差，并通过设定操作员的设定跟踪速度，采用非线性反步法为成DP船舶的高速循迹设计了欠驱动控制方法。

本发明的有益效果主要表现在：1、所提出的导引控制方法简单易行；2、保证系统的稳定性，且具有一定的鲁棒性；3、控制精度高，适用于工程实践。

附图说明

图1为船舶DP系统定位旋转导引控制方法的控制过程示意图；

图2为DP船舶船体坐标系和大地坐标系的示意图；

图3为分段导引策略流程图；

图4为DP船舶设定路径和循迹过程示意图；

图5为直线航迹段循迹导引策略原理图；

图6为圆弧航迹段循迹导引策略原理图；

图7为船舶圆弧段循迹导引策略流程图；

图8为仿真试验中应用所提出方法的DP船舶偏差响应；

图9为船舶实际运动响应与期望运动轨迹之间航迹偏差随时间变化曲线；

图10为DP船舶在高速循迹过程中船舶的艏向响应曲线；

图11为期望艏向与实际响应之间的偏差随时间变化的曲线；

图12为高速循迹过程中船舶的速度随时间变化曲线。

具体实施方式

下面结合附图对本发明作进更详细的描述：

一种动力定位船舶循迹导引控制方法，如图1所示，所述的方法包括以下步骤：

（1）建立船舶低速运动的运动学和动力学模型

如图2所示，选取大地坐标的原点为O_E，以正东方向为横轴，正北方向为纵轴，以n表示北向位置坐标，e表示东向位置坐标，ψ表示船舶艏向角（船舶艏向与正北方向的夹角，案顺时针方向为0°到360°）；船体坐标系的原点为船舶的几何中心O_b，原点指向船艏为纵轴方向X，原点指向右舷为横轴方向Y，以x表示船体坐标系中某点的纵坐标，y表示船体坐标系中某点的横坐标。以[u,v,r]^Τ表示船舶坐标系中船舶的速度向量，u为纵向速度，v为横向速度，r为转艏角速度。

（2）在大地坐标系和船体坐标系中建立动力定位船舶三自由度船舶高速运动数学模型：

$\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&eta;}}=\mathrm{Jv}\\ M\stackrel{\&CenterDot;}{v}+D\left(v\right)v+C\left(v\right)v=\mathrm{\&tau;}\end{array}$

式中，η表示船舶在大地坐标系中位置和艏向向量[n,e,ψ]^Τ；v表示船舶在船体坐标系中线速度和角速度向量[u,v,r]^Τ；τ为推进器产生的力和力矩向量[F_x,F_y,N_z]^Τ；J为从船体坐标到大地坐标的转换矩阵， $J=\left[\begin{array}{ccc}\cos \mathrm{\&psi;}& -\sin \mathrm{\&psi;}& 0\\ \sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}& 0\\ 0& 0& 1\end{array}\right];$ M为船舶惯性矩阵， $M=\left[\begin{array}{ccc}m-X_{\stackrel{\&CenterDot;}{u}}& 0& 0\\ 0& m-Y_{\stackrel{\&CenterDot;}{v}}& {\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}}\\ 0& {\mathrm{mx}}_g-N_{\stackrel{\&CenterDot;}{v}}& I_z-N_{\stackrel{\&CenterDot;}{r}}\end{array}\right];$ m为船舶质量；I_z为转动惯量；x_g为船体坐标系中船舶质心纵向坐标，其他参数均为一阶水动力导数；

D(v)=D+D_n(v)为船舶水动力阻尼项；

线性部分为 $D=-\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& Y_r\\ 0& N_v& N_r\end{array}\right];$

$D_n\left(v\right)=\left[\begin{array}{ccc}{X}_{u\left|u\right|}\left|u\right|& 0& 0\\ 0& Y_{v\left|v\right|}\left|v\right|+Y_{\left|r\right|v}\left|r\right|& Y_{\left|v\right|r}\left|v\right|+Y_{\left|r\right|r}\left|r\right|\\ 0& N_{v\left|v\right|}\left|v\right|+N_{\left|r\right|v}\left|r\right|& N_{\left|v\right|r}\left|v\right|+N_{\left|r\right|r}\left|r\right|\end{array}\right]$ 为非线性部分；

科里奥利和向心力矩阵为:

$C=\left[\begin{array}{ccc}0& 0& -(m-Y_{\stackrel{\&CenterDot;}{v}})v-({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}})r\\ 0& 0& (m-X_{\stackrel{\&CenterDot;}{u}})u\\ (m-Y_{\stackrel{\&CenterDot;}{v}})v+({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}})r& -(m-X_{\stackrel{\&CenterDot;}{u}})u& 0\end{array}\right];$

（3）通过船体设定的路径点和当前时刻船舶的大地坐标位置，获得当前时刻的船舶期望艏向,该导引过程的流程图如图3所示,主要包括以下几个部分：

①设定路径点，生成船舶运动航迹线

设DP船舶需要跟踪的航迹由大地坐标系中的设定路径点为A,B,C三点，如图4所示,它们的坐标为分别为(n₁,e₁)，(n₂,e₂)和(n₃,e₃)。大地坐标系中船舶当前时刻的位置为(n,e)。

②直线航迹段船舶期望艏向的计算方法

根据船舶当前位置与直线段航迹的偏差，即船舶到直线的垂直距离计算一个修正角，在航迹向的基础上加上这个修正角，使船舶艏向偏向航迹一边,如图5所示。

对于一条从A点指向B点的航迹线，当前时刻船舶与航迹AB的偏差d为：

$d=\frac{(e_1-e_2)(n-n_2)-(n_1-n_2)(e-e_2)}{\sqrt{(n_2-n_1)(n_2-n_1)+(e_2-e_1)(e_2-e_1)}}$

可知，当d>0时，船舶在有向航迹线的右侧；当d<0时，船舶在有向航迹线的左侧。航迹线方向ψ_trace∈(-π,π)为，有向线段AB与大地坐标北向的夹角：

ψ_trace=arctan2((e₂-e₁)/(n₂-n₁))

此时船舶期望艏向ψ_d计算方法为：ψ_d=ψ_trace-Kt*d。Kt为从航迹偏差到修正角度的转换增益。

这样当船舶位于航迹线右侧时船舶以航迹方向为基础，通过向左转向来靠近航迹；当船舶位于航迹线左侧时，船舶通过向右转向靠近航迹；当船舶位于航迹线上时船舶的期望艏向为航迹线方向。

③转弯航迹段船舶期望艏向的计算方法

为船舶在高速循迹的航迹切换部分，设计一个和两条有向直线航迹AB和BC内切的圆弧，船舶通过对这段圆弧的循迹，平滑地从AB段过渡到下一条直线航迹BC段,如图6所示。

圆弧段航迹的确定通过船舶当前时刻的速度u和期望转艏角速度r_d获得，首先计算圆弧半径：R=u/r_d，以及两条航迹的航迹向差：λ=ψ_{trace_2}-ψ_{trace_1}。进而通过B点的大地坐标确定转弯圆弧的圆心大地坐标位置(n_R,e_R)：

n_R=n₂+R/(λ/2)·cos(ψ_{trace_1}+λ/2)

e_R=e₂+R/(λ/2)·sin(ψ_{trace_1}+λ/2)

船舶开始转弯后，当前期望艏向ψ_d(k)根据期望转艏角速度，AB段直线航迹的航迹向和采样时间T_s来计算：ψ_d(k)=ψ_d(k-1)+r_d·T_s，ψ_d(k)的初始值为AB段直线航迹的航迹向ψ_{trace_1}。

在转弯过程中，航迹偏差根据船舶当前位置与圆弧路径的距离获得。根据船舶当前位置(n,e)与圆心(n_R,e_R)的距离判定船舶是否位于圆弧上：d_arc=R时，船舶在圆弧上；当d_arc≠R时船舶不在圆弧上。

当船舶在圆弧上时，船舶期望艏向ψ_d计算方法为：ψ_d=ψ_d(k)；当船舶不在圆弧上时，仿照直线段循迹的计算方法来获得期望艏向，此时航迹偏差为d=d_arc-R，船舶期望艏向ψ_d计算方法为：ψ_d=ψ_d(k)-sgn·Kt·d。sgn=-1,1为一符号函数，当船舶向左转时sgn=1，当船舶向右转时sgn=-1。

当｜ψ_d-ψ_{trace_2}｜<ε时，船舶则开始对下一条直线段航迹进行循迹，期望艏向的计算方法如步骤②，直到最后一条航迹，循迹结束,具体过程如图7所示。

（4）利用反步法，根据当前时刻期望艏向ψ_d与船舶的期望速度u_d，计算控制船舶高速循迹所需的纵向力F_x和力矩N_z。主要包括以下几个步骤：

①定义误差z₁=ψ-ψ_d，z₂=v-ξ，并定义H=[001]

令对z₁求导有：

${\stackrel{\&CenterDot;}{z}}_1=\stackrel{\&CenterDot;}{\mathrm{\&psi;}}-{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_d=r-r_d=\mathrm{Hv}-r_d={\mathrm{Hz}}_2+{\mathrm{\&xi;}}_3-r_d;$ 对z₂求导有：

$M{\stackrel{\&CenterDot;}{z}}_2=M\stackrel{\&CenterDot;}{v}-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}}=\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}};$

②选择李雅普诺夫候选函数

则沿z₁z₂对V求导有：

$\stackrel{\&CenterDot;}{V}=z_1{\stackrel{\&CenterDot;}{z}}_1+{z_2}^{T}M{\stackrel{\&CenterDot;}{z}}_2=z_1({\mathrm{Hz}}_2+{\mathrm{\&xi;}}_3-r_d)+{z_2}^T(\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}});$

③选择虚拟控制器ξ₃=-αz₁+r_d，其中α>0

可得到： $\stackrel{\&CenterDot;}{V}=z_1{\stackrel{\&CenterDot;}{z}}_1+{z_2}^{T}M{\stackrel{\&CenterDot;}{z}}_2=-\mathrm{\&alpha;}{z_1}^2+{z_2}^T(H^T{z}_1+\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}});$

④选择控制器 $\mathrm{\&tau;}={[F_x,F_y,N_z]}^T=D\left(v\right)v+C\left(v\right)v+M\stackrel{\&CenterDot;}{\mathrm{\&xi;}}-\mathrm{\&beta;}{z}_2-H^T{z}_1,$

其中β=diag(β₁,β₂,β₃)>0为设计参数。

则：即此控制器使得系统渐进稳定。

联合步骤（2）中建立的船舶模型，令F_y=0，将所设计的控制器τ展开。

令ξ₁=u_d，得到纵向控制为： $F_x=(m-X_{\stackrel{\&CenterDot;}{u}}){\stackrel{\&CenterDot;}{u}}_d-X_{u}u-{\mathrm{\&beta;}}_1(u-u_d);$

令经整理后船舶艏向控制力矩为：

$\begin{array}{c}{N}_z=(\mathrm{\&kappa;}{\mathrm{\&beta;}}_2+\mathrm{\&kappa;}{Y}_v-N_v)v+((\mathrm{\&kappa;}({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}})-(I_z-N_{\stackrel{\&CenterDot;}{r}}))\mathrm{\&alpha;}-{\mathrm{\&beta;}}_3)(r-r_d)+(({\mathrm{mx}}_{g}u-N_r)-\mathrm{\&kappa;}(\mathrm{mu}-Y_r))r\\ +((I_z-N_{\stackrel{\&CenterDot;}{r}})-\mathrm{\&kappa;}({\mathrm{mx}}_g-N_{\stackrel{\&CenterDot;}{v}})){\stackrel{\&CenterDot;}{r}}_d-((\mathrm{\&kappa;}(\mathrm{mu}-Y_r)+{\mathrm{\&beta;}}_3)\mathrm{\&alpha;}+1)(\mathrm{\&psi;}-{\mathrm{\&psi;}}_d)-{\mathrm{\&kappa;\&beta;}}_2{\mathrm{\&xi;}}_2-\mathrm{\&kappa;}{N}_v\mathrm{\&alpha;}\end{array}$

（5）将控制力和力矩传递给动力定位船舶的数学模型，解算下一时刻船舶的状态信息，然后由当前时刻更新到下一时刻；

（6）重复执行步骤（2）--步骤（5），直到最后一条航迹循迹完成，结束计算。

本发明实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改和改变，都落入本发明的保护范围。

实施例

为了验证本发明方法的有效性和效果，使用动力定位船舶半实物系统仿真平台进行了仿真试验。该实施例包含以下步骤：

1)实验条件

①采用如上所述的步骤（2）中所给出数学模型描述DP船舶，具体参数选择如下：

船长：99.5m；

船舶质量：3100吨；

船宽：15.2m；

吃水：5.6m。

②使用仿真平台自带的推进器系统和推力分配单元。

③系统采样时间为0.3s；

2)初始值设置

设定期望速度：3m/s；

设定路径点：（00）、（1006.7110.0671）、（1600.67906.644）、（1379.192114.09）、（503.3562617.45）、（-281.8792013.42）和（-291.946936.242）；

船舶初始位置和艏向：（0，0，0）；

船舶初始速度：0m/s。

3)试验结果及分析

如图8所示,其中的点实线为根据设定路径点生成的路径线，虚线为加入本发明所给出的导引算法得到的期望船舶运动轨迹，实线为船舶高速应用本发明给出的导引控制算法得到的船舶实际运动响应位置曲线。

如图10所示，图中虚线为根据导引算法得到的随时间变化的期望艏向，实线为根据导引控制算法得到的船舶实际艏向响应。

根据图8、图9、图10、图11和图12所给出的船舶动态响应过程，得到如下的分析结果：

本发明给出的导引控制算法可以通过设定路径点来得到由直线段组成的路径，并计算船舶的期望运动轨迹，获得两条直线段路径间的过渡圆弧，可以很好的控制船舶沿所得到的期望路径进行循迹，循迹误差控制在10m以内，并且艏向控制艏向沿路径或圆弧的切向方向，并且循迹速度最终可以稳定在设定速度。

由这组试验结果可见，在控制过程中基于实施步骤（3）所给出的分段导引策略和步骤（4）所给出的反步控制算法可以引导并保证DP船舶在设定的路径上行驶，效果稳定，跟踪精度较高。而且可以看到，步骤（3）所给出的分段导引策略为控制器提供了合理的期望艏向，所以此导引策略也可与其他欠驱动控制算法相结合。

Claims (2)

1.一种动力定位船舶循迹导引控制方法，其特征在于包括如下步骤：

(1)选取海平面上任意一点为原点，以正东方向为横轴、正北方向为纵轴，建立大地坐标系EO_EN；然后，以船舶的几何中心为原点，以右舷方向为横轴、船艏方向为纵轴，建立船体坐标系YO_bX；

(2)在大地坐标系和船体坐标系中建立动力定位船舶三自由度船舶高速运动数学模型：

$\stackrel{\&CenterDot;}{\mathrm{\&eta;}}=Jv$

$M\stackrel{\&CenterDot;}{v}+D\left(v\right)v+C\left(v\right)v=\mathrm{\&tau;}$

式中，η表示船舶在大地坐标系中位置和艏向向量[n,e,ψ]^Τ；v表示船舶在船体坐标系中线速度和角速度向量[u,v,r]^Τ；τ为推进器产生的力和力矩向量[F_x,F_y,N_z]^Τ；J为从船体坐标到大地坐标的转换矩阵， $J=\left[\begin{array}{ccc}cos\mathrm{\&psi;}& -sin\mathrm{\&psi;}& 0\\ sin\mathrm{\&psi;}& \cos \mathrm{\&psi;}& 0\\ 0& 0& 1\end{array}\right];$ M为船舶惯性矩阵， $M=\left[\begin{array}{ccc}m-X_{\stackrel{\&CenterDot;}{u}}& 0& 0\\ 0& m-Y_{\stackrel{\&CenterDot;}{v}}& {\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}}\\ 0& {\mathrm{mx}}_g-N_{\stackrel{\&CenterDot;}{v}}& I_z-N_{\stackrel{\&CenterDot;}{r}}\end{array}\right];$ m为船舶质量；I_z为转动惯量；x_g为船体坐标系中船舶质心纵向坐标；其他参数均为一阶水动力导数；船舶水动力阻尼项为D(v)＝D+D_n(v)； $D=-\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& Y_r\\ 0& N_v& N_r\end{array}\right]$ 为线性部分；

$D_n\left(v\right)=\left[\begin{array}{ccc}{X}_{u\left|u\right|}\left|u\right|& 0& 0\\ 0& Y_{v\left|v\right|}\left|v\right|+Y_{\left|r\right|v}\left|r\right|& Y_{\left|v\right|r}\left|v\right|+Y_{\left|r\right|r}\left|r\right|\\ 0& N_{v\left|v\right|}\left|v\right|+N_{\left|r\right|v}\left|r\right|& N_{\left|v\right|r}\left|v\right|+N_{\left|r\right|r}\left|r\right|\end{array}\right]$ 为非线性部分；

$C=\left[\begin{array}{ccc}0& 0& -\left(m-Y_{\stackrel{\&CenterDot;}{v}}\right)v-\left({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}}\right)r\\ 0& 0& (m-X_{\stackrel{\&CenterDot;}{u}})u\\ \left(m-Y_{\stackrel{\&CenterDot;}{v}}\right)v+\left({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}}\right)r& -(m-X_{\stackrel{\&CenterDot;}{u}})u& 0\end{array}\right]$ 为科里奥利和向心力矩阵；

(3)通过船体设定的路径点和当前时刻船舶的大地坐标位置，获得当前时刻的船舶期望艏向；

(4)利用反步法，根据当前时刻期望艏向ψ_d与船舶的期望速度u_d，计算控制船舶高速循迹所需的纵向力F_x和力矩N_z；

(5)将控制力和力矩传递给动力定位船舶的数学模型，解算下一时刻船舶的状态信息，然后由当前时刻更新到下一时刻；

(6)重复执行步骤(2)--步骤(5)，直到最后一条航迹循迹完成；

所述通过船体设定的路径点和当前时刻船舶的大地坐标位置，获得当前时刻的船舶期望艏向的方法为：

①定路径点，生成船舶运动航迹线

设DP船舶需要跟踪的航迹由大地坐标系中的设定路径点为A,B,C三点，它们的坐标为分别为(n₁,e₁)，(n₂,e₂)，(n₃,e₃)；大地坐标系中船舶当前时刻的位置为(n,e)；

②计算直线航迹段船舶期望艏向

根据船舶当前位置与直线段航迹的偏差，即船舶到直线的垂直距离计算一个修正角，在航迹向的基础上加上这个修正角，使船舶艏向偏向航迹一边；

对于一条从A点指向B点的航迹线，当前时刻船舶与航迹AB的偏差d为：

$d=\frac{(e_1-e_2)(n-n_2)-(n_1-n_2)(e-e_2)}{\sqrt{(n_2-n_1)(n_2-n_1)+(e_2-e_1)(e_2-e_1)}}$

当d＞0时，船舶在有向航迹线的右侧；当d＜0时，船舶在有向航迹线的左侧；航迹线方向ψ_trace∈(-π,π)为有向线段AB与大地坐标北向的夹角：

ψ_trace＝arctan2((e₂-e₁)/(n₂-n₁))

船舶期望艏向ψ_d＝ψ_trace-Kt*d，其中Kt为从航迹偏差到修正角度的转换增益；

当船舶位于航迹线右侧时船舶以航迹方向为基础，通过向左转向来靠近航迹；当船舶位于航迹线左侧时，船舶通过向右转向靠近航迹；当船舶位于航迹线上时船舶的期望艏向为航迹线方向；

③计算转弯航迹段船舶期望艏向

当船舶在高速循迹的航迹切换部分时，设计一个和两条有向直线航迹AB和BC内切的圆弧，船舶通过对这段圆弧的循迹，平滑地从AB段过渡到下一条直线航迹BC段；

通过船舶当前时刻的速度u和期望转艏角速度r_d获得圆弧段航迹的，首先计算圆弧半径：R＝u/r_d，以及两条航迹的航迹向差：λ＝ψ_{trace_2}-ψ_{trace_1}；进而通过B点的大地坐标确定转弯圆弧的圆心大地坐标位置(n_R,e_R)：

n_R＝n₂+R/(λ/2)·cos(ψ_{trace_1}+λ/2)

e_R＝e₂+R/(λ/2)·sin(ψ_{trace_1}+λ/2)

船舶开始转弯后，当前期望艏向ψ_d(k)根据期望转艏角速度，AB段直线航迹的航迹向用采样时间T_s来计算：ψ_d(k)＝ψ_d(k-1)+r_d·T_s，ψ_d(k)的初始值为AB段直线航迹的航迹向ψ_{trace_1}；

在转弯过程中，航迹偏差根据船舶当前位置与圆弧路径的距离获得；根据船舶当前位置(n,e)与圆心(n_R,e_R)的距离判定船舶是否位于圆弧上：d_arc＝R时，船舶在圆弧上；当d_arc≠R时船舶不在圆弧上；

当船舶在圆弧上时，船舶期望艏向ψ_d计算方法为：ψ_d＝ψ_d(k)；当船舶不在圆弧上时，仿照直线段循迹的计算方法来获得期望艏向，此时航迹偏差为d＝d_arc-R，船舶期望艏向ψ_d计算方法为：ψ_d＝ψ_d(k)-sgn·Kt·d；sgn＝-1,1为一符号函数，当船舶向左转时sgn＝1，当船舶向右转时sgn＝-1；

当|ψ_d-ψ_{trace_2}|＜ε时，船舶则开始对下一条直线段航迹进行循迹，期望艏向的计算方法如步骤②，直到最后一条航迹，循迹结束。

2.根据权利要求1所述的动力定位船舶循迹导引控制方法，其特征在于所述步骤(4)中还包括以下几个步骤：

①定义误差z₁＝ψ-ψ_d，z₂＝v-ξ，并定义H＝[001]

令对z₁求导有：

${\stackrel{\&CenterDot;}{z}}_1=\stackrel{\&CenterDot;}{\mathrm{\&psi;}}-{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_d=r-r_d=Hv-r_d={\mathrm{Hz}}_2+{\mathrm{\&xi;}}_3-r_d;$ 对z₂求导有：

$M{\stackrel{\&CenterDot;}{z}}_2=M\stackrel{\&CenterDot;}{v}-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}}=\mathrm{\&tau;}-D\left(v\right)v-C\left(v\right)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}};$

②选择李雅普诺夫候选函数

则沿z₁z₂对V求导有：

$\stackrel{\&CenterDot;}{V}=z_1{\stackrel{\&CenterDot;}{z}}_1+{z_2}^{T}M{\stackrel{\&CenterDot;}{z}}_2=z_1({\mathrm{Hz}}_2+{\mathrm{\&xi;}}_3-r_d)+{z_2}^T(\mathrm{\&tau;}-D(v)v-C(v)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}});$

③选择虚拟控制器ξ₃＝-αz₁+r_d，其中α＞0

可得到： $\stackrel{\&CenterDot;}{V}=z_1{\stackrel{\&CenterDot;}{z}}_1+{z_2}^{T}M{\stackrel{\&CenterDot;}{z}}_2=-{{\mathrm{\&alpha;z}}_1}^2+{z_2}^T(H^T{z}_1+\mathrm{\&tau;}-D(v)v-C(v)v-M\stackrel{\&CenterDot;}{\mathrm{\&xi;}});$

④选择控制器 $\mathrm{\&tau;}={\&lsqb;F_x,F_y,N_z\&rsqb;}^T=D\left(v\right)v+C\left(v\right)v+M\stackrel{\&CenterDot;}{\mathrm{\&xi;}}-{\mathrm{\&beta;z}}_2-H^T{z}_1,$

其中β＝diag(β₁,β₂,β₃)＞0为设计参数；

则：即此控制器使得系统渐进稳定；

联合步骤(2)中建立的船舶模型，令F_y＝0，将所设计的控制器τ展开；

令ξ₁＝u_d，得到纵向控制为： $F_x=(m-X_{\stackrel{\&CenterDot;}{u}}){\stackrel{\&CenterDot;}{u}}_d-X_{u}u-{\mathrm{\&beta;}}_1(u-u_d);$

令经整理后船舶艏向控制力矩为：

$\begin{array}{c}{N}_z=\left({\mathrm{\&kappa;\&beta;}}_2+{\mathrm{\&kappa;Y}}_v-N_v\right)v+\left(\left(\mathrm{\&kappa;}\left({\mathrm{mx}}_g-Y_{\stackrel{\&CenterDot;}{r}}\right)-\left(I_z-N_{\stackrel{\&CenterDot;}{r}}\right)\right)\mathrm{\&alpha;}-{\mathrm{\&beta;}}_3\right)\left(r-r_d\right)+\left(\left({\mathrm{mx}}_{g}u-N_r\right)-\mathrm{\&kappa;}\left(mu-Y_r\right)\right)r\\ +\left(\left(I_z-N_{\stackrel{\&CenterDot;}{r}}\right)-\mathrm{\&kappa;}\left({\mathrm{mx}}_g-N_{\stackrel{\&CenterDot;}{v}}\right)\right){\stackrel{\&CenterDot;}{r}}_d-\left(\left(\mathrm{\&kappa;}\left(mu-Y_r\right)+{\mathrm{\&beta;}}_3\right)\mathrm{\&alpha;}+1\right)\left(\mathrm{\&psi;}-{\mathrm{\&psi;}}_d\right)-{\mathrm{\&kappa;\&beta;}}_2{\mathrm{\&xi;}}_2-{\mathrm{\&kappa;N}}_v\mathrm{\&alpha;}\end{array}.$