CN103345259A - 动力定位船舶实现定位旋转的几何坐标变换导引控制方法 - Google Patents

Abstract

本发明公开了一种动力定位船舶实现定位旋转的几何坐标变换导引控制方法。该方法首先设定船舶的目标艏向和旋转中心的船体坐标，结合船舶运动的数学模型，通过引入几何坐标变换导引策略计算船舶定位旋转过程中期望的轨迹；然后计算船舶当前时刻的期望艏向和期望位置；最后依据当前时刻实际位置、艏向与期望位置、艏向的偏差，应用PID控制算法得到控制船舶纵移、横移和艏摇所需的力和力矩，使船舶到达期望的位置和艏向，实现船舶绕旋转中心的定位旋转。该方法不需要改变推力分配中心，可保证定位旋转系统为闭环控制回路，提高了控制精度，计算过程简单易行，适用于海洋工程作业。

Description

动力定位船舶实现定位旋转的几何坐标变换导引控制方法

技术领域

本发明涉及一种控制动力定位船舶实现绕任意给定的旋转中心进行定位旋转的PID控制方法，特别是一种动力定位船舶实现定位旋转的几何坐标变换导引控制方法。

背景技术

动力定位船舶由于本身具有定位精度高、机动灵活的优点被广泛用于海洋工程中，由此能够保证动力定位船舶准确作业的船舶动力定位控制系统成为科学研究和工程实践领域人们研究的热点问题。船舶动力定位控制系统包括诸多功能，如联合操纵杆、自动定位、自动艏向、自动航迹、自动舵等。在这些功能中，默认的船舶运动中心均为船舶的几何中心，控制的目标为几何中心的位置和速度。但是，在一些特殊的作业中，如当船舶需要使用起重机在海床上安装设备时，为了保证作业的安全，必须保持船舶上起重机吊臂顶点（即旋转中心）在海面上的位置不变，通过绕吊臂顶点的旋转运动达到目标艏向，通过船舶动力定位控制系统实现对吊臂定点的自动定位功能，这就是所谓的定位旋转。船舶绕任意给定旋转中心达到目标艏向的定位旋转功能为保证动力定位船舶作业的一个重要功能。

关于船舶绕设定旋转中心进行位置和艏向控制的相关文献较少，张本伟在其硕士论文《船舶绕给定点旋转的控制技术研究》（哈尔滨工程大学,2008）中通过引入平行坐标系研究了船舶做不停止的转动的船舶绕给定点旋转方法，但没有考虑船舶运动到目标艏向后如何停止，而且仅针旋转中心在船艏、船舯、船侧等几种情况，不具有普遍应用性。国际上主流的动力定位产品已经包含了定位旋转的功能，北欧挪威Kongsberg公司已经对定位旋转进行了一些研究，其产品说明书《Kongsberg

-SDP Dynamic Positioning System》中给出了一些关于定位旋转的说明和功能概况，但没有给出实现方法。学者Lindegaard在其博士论文《Acceleration Feedback in Dynamic Positioning》（Norwegian University of Science andTechnology,2003）中针对为船舶设定旋转中心设计的路径跟踪问题提出了路径的产生策略，为船舶指向提供期望位置，设计了带有加速度反馈的全状态反馈非线性PID控制器，并给出稳定性证明。

发明内容

本发明的目的在于，提供一种系统稳定性更好、控制精度更高的实现动力定位船舶绕给定旋转中心到达目标艏向和目标位置的定位旋转方法。

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

(1)船舶进入定位旋转工作状态，，完成系统初始化；

设定初始速度为[u₀,v₀,r₀]^Τ=[0,0,0]^Τ；

所涉及的大地坐标系EO_EN描述为：选取海平面上任意一点为原点，以正东方向为横轴、正北方向为纵轴；船体坐标系YO_bX描述为：以船舶的几何中心为原点，以右舷方向为横轴、船艏方向为纵轴；

(2)利用船舶运动模型进行定位解算，结合船体坐标系中设定的船舶目标艏向和旋转中心的船体坐标，获得船舶几何中心的期望运动轨迹；

所涉及的动力定位船舶在大地坐标系和船体坐标系中的数学模型为：

$\stackrel{\·}{\mathrm{\η}}=\mathrm{Jv}$

$M\stackrel{\·}{v}+\mathrm{Dv}+\mathrm{Cv}=\mathrm{\τ}$

式中，η表示船舶在大地坐标系中位置和艏向向量[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-Y_{\stackrel{\·}{r}}& I_z-N_{\stackrel{\·}{r}}\end{array}\right],$ m为船舶质量，I_z为转动惯量，x_g为船体坐标系中船舶质心纵向坐标，其他参数均为一阶水动力导数；D为船舶水动力阻尼项， $D=-\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& Y_r\\ 0& N_v& N_r\end{array}\right];$ C为船舶的科里奥利力和向心力矩阵；

所述船舶几何中心的期望运动轨迹的大地坐标表达式：

e_d=e_R-R·sin(ψ+ψ_add)

n_d=n_R-R·cos(ψ+ψ_add)

式中，ψ_add为船舶几何中心指向设定旋转中心的向量与船舶艏向的夹角；R为船舶几何中心的旋转半径，且有

e_R,n_R表示船舶进入定位旋转功能时刻旋转中心的大地坐标，其表达式为：

e_R=e₀+x₀·sin(ψ₀)+y₀·cos(ψ₀)

n_R=n₀+x₀·cos(ψ₀)-y₀·sin(ψ₀)

式中，n₀,e₀和ψ₀分别为动力定位船舶几何中心在大地坐标系中的位置和艏向；(y_o,x_o)表示旋转中心的船体坐标，并在定位旋转过程中保持不变；

(3)计算当前时刻船舶的期望艏向；

当前时刻船舶的期望艏向ψ(k)_d的表达式为：

ψ(k)_d=ψ(k)+sgn·r_d·T_s；

式中，ψ(k)为当前实际艏向；ψ_d为操作员设定的目标艏向；r_d为回转角速度；T_S为采样周期；sgn为一符号函数，由目标艏向和初始艏向之差决定。

sgn的值获取方法为：首先对目标艏向和初始艏向的差值采用如下逻辑进行预处理：只要ψ_d-ψ₀>=180，ψ_d-ψ=ψ_d-ψ-360；只要ψ_d-ψ₀<180，ψ_d-ψ=ψ_d-ψ+360；然后通过ψ_d-ψ₀的符号来得到sgn的取值： $\mathrm{sgn}=\left\{\begin{array}{c}1,{\mathrm{\&psi;}}_d-{\mathrm{\&psi;}}_0>=0\\ -1,{\mathrm{\&psi;}}_d-{\mathrm{\&psi;}}_0<0\end{array}\right.;$

同时，判定当前时刻船舶艏向ψ(k)基本达到目标艏向时，需要将ψ(k)_d当前期望艏向设定为目标艏向ψ_d，使动力定位船舶在完成定位旋转功能之后，保持目标艏向并进行定位，此时采用如下判断条件进行判定：

当|ψ_d-ψ(k)|<=2·T_S·r_d时，ψ(k)_d=ψ_d；

(4)根据船舶的期望运动轨迹以及当前时刻船舶的实际艏向ψ(k)，计算当前时刻船舶的期望位置；

大地坐标系中船舶当前时刻的期望位置(e(k)_d,n(k)_d)计算方法如下：

e(k)_d=e_R-R·sin(ψ(k)+ψ_add)

n(k)_d=n_R-R·cos(ψ(k)+ψ_add)；

(5)根据船舶当前时刻的实际位置和艏向以及期望位置和艏向,计算船体坐标系中的位置偏差和艏向偏差；

船体坐标系中的横向位置偏差、纵向位置偏差和艏向偏差的表达式为：

E(k)_x=E(k)_e·sin(ψ(k))+E(k)_n·cos(ψ(k))

E(k)_y=E(k)_e·cos(ψ(k))-E(k)_n·sin(ψ(k))

E(k)_b=E(k)_ψ

式中，E(k)_e，E(k)_n，E(k)_ψ分别为大地坐标系中东向位置偏差、北向位置偏差和艏向偏差，其表达式为：

E(k)_e=e(k)_d-e(k)

E(k)_n=n(k)_d-n(k)

E(k)_ψ=ψ(k)_d-ψ(k)

(6)采用PID控制算法获得船舶定位旋转的控制力和力矩；

动力定位船舶纵向控制力F(k)_x、横向控制力F(k)_y和控制力矩N(k)_z的表达式为：

F(k)_x=K_{p_surge}·E(k)_x+K_{d_surge}·u+K_{i_surge}·S_surge

F(k)_y=K_{p_sway}·E(k)_y+K_{d_sway}·v+K_{i_sway}·S_sway

N(k)_Z=K_{p_yaw}·E(k)_b+K_{d_yaw}·r+K_{i_yaw}·S_yaw

式中，K_{p_surge}、K_{i_surge}和K_{d_surge}为船舶纵向力的PID控制参数，S_surge表示对纵向偏差的积分；K_{p_sway}、K_{i_sway}和K_{d_sway}为船舶横向力的PID控制参数，S_sway表示对横向偏差的积分；K_{p_yaw}、K_{i_yaw}和K_{d_yaw}为船舶转艏力矩的PID控制参数，S_yaw表示对艏向偏差的积分；u,v,r是通过船舶数学模型解算得到的船舶速度向量；

(7)将控制力和力矩反馈给动力定位船舶的数学模型，解算下一时刻船舶的实际位置和艏向，然后由当前时刻更新到下一时刻；

(8)重复执行步骤（2）～（7），直至船舶到达目标位置和艏向，输出结果，实现定位旋转。

本发明相对现有技术具有如下的优点及效果：

本发明的有益效果在于针对动力定位船舶定位模式下设定的船舶旋转中心，通过设计船舶几何中心的期望运动轨迹，获得船舶当前时刻期望位置和期望艏向的导引算法，在不改变原有以船舶的几何中心为分配中心点的推力分配环节的情况下，采用PID控制完成动力定位船舶绕任意给定旋转中心的定位旋转。由于不改变推力分配中心，避免了传统定位旋转方法，因计算向心力而引起的系统不再满足闭环控制回路条件的问题。所提出的导引控制方法简单易行，且控制精度高，具有更高的安全性和可靠性，具有较高工程应用价值。

附图说明

图1为船舶动力定位系统定位旋转导引控制方法基本流程框图。

图2为图1的详细流程框图。

图3为图2中艏向偏差获得环节的详细流程框图。

图4为动力定位船舶船体坐标系和大地坐标系的示意图。

图5为动力定位船舶定位旋转运动过程示意图。

图6为大地坐标系中船舶数学模型回转试验的位置曲线。

图7为仿真试验中应用所提出方法得到的动力定位船舶北向位置误差曲线。

图8为仿真试验中应用所提出方法得到的动力定位船舶东向位置误差曲线。

图9为仿真试验中应用所提出方法得到的动力定位船舶艏向偏差曲线。

图10为仿真试验中应用所提出方法得到的动力定位船舶回转率曲线。

具体实施方式

下面结合附图对本发明作进一步的详细描述。

本发明提出的一种基于PID控制方法的动力定位船舶绕给定旋转中心进行定位旋转的导引控制方法的流程图如附图1和附图2所示，图2中艏向偏差的获得方法如图3所示，该方法的主要步骤如下：

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

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

由于动力定位船舶在定位旋转运动时处于低速运动状态，本发明选取型如式(1)的一个三自由度船舶低速运动数学模型来对动力定位船舶进行建模：

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

$M\stackrel{\&CenterDot;}{v}+\mathrm{Dv}+\mathrm{Cv}=\mathrm{\&tau;}$

式中，η表示船舶在大地坐标系中位置和艏向向量[n,e,ψ]^Τ；v表示动力定位船舶在船体坐标系中线速度和角速度向量[u,v,r]^Τ；τ为推进器产生的力和力矩向量[F_x,F_y,N_z]^Τ；J为从船体坐标到大地坐标的转换矩阵，如(2)式所示；M为船舶惯性矩阵，如(3)式所示；D为船舶水动力阻尼项，如(4)式所示；C为船舶的克里奥利和向心力矩阵，如(5)式所示。

$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]---\left(2\right)$

$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-Y_{\stackrel{\&CenterDot;}{r}}& I_z-N_{\stackrel{\&CenterDot;}{r}}\end{array}\right]---\left(3\right)$

$D=-\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& Y_r\\ 0& N_v& N_r\end{array}\right]---\left(4\right)$

$C\left(v\right)=\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]---\left(5\right)$

（2）参数初始化

设动力定位船舶几何中心在大地坐标系中的初始位置和艏向向量为[n₀,e₀,ψ₀]^Τ；目标艏向为ψ_d（0°到360°之间）；设定采样周期为T_S；设定回转角速度为r_d；设定旋转中心的船体坐标为(y_o,x_o)，并在一次船舶定位旋转过程中保持不变；由于在进入定位旋转之前，船舶处于定位状态，所以设定初始速度为[u₀,v₀,r₀]^Τ=[0,0,0]^Τ。

（3）获得船舶几何中心的期望运动轨迹

通过动力定位船舶几何中心在大地坐标系中的初始值，以及设定旋转中心在船体坐标系中的坐标，得到船舶进入定位旋转功能时刻旋转中心的大地坐标(e_R,n_R)：

e_R=e₀+x₀·sin(ψ₀)+y₀·cos(ψ₀)      (6)

n_R=n₀+x₀·cos(ψ₀)-y₀·sin(ψ₀)

以及船舶的旋转半径R：

$R=\sqrt{x_0^2+y_0^2}---\left(7\right)$

根据(6)式和(7)式生成需要保持动力定位船舶设定旋转中心的大地坐标不变时，船舶几何中心运动的期望轨迹为一圆弧轨迹，轨迹参数方程为：

e_d=e_R-R·sin(ψ+ψ_add)(8)

n_d=n_R-R·cos(ψ+ψ_add)

式中，n_d和e_d分别期望圆弧轨迹上任意一点的北向位置和东向位置，由船舶的艏向决定；ψ_add为船舶几何中心指向设定旋转中心的向量与船舶艏向的夹角，如附图5所示。

（4）计算当前时刻期望艏向

船舶的期望艏向根据操作员设定的目标艏向ψ_d，设定的回转角速度为r_d，以及采样周期为T_S得到：

ψ(k)_d=ψ(k)+sgn·r_d·T_s              (9)

式中，sgn为一符号函数，由目标艏向和初始艏向之间的差值决定，总是使得船舶艏向尽快达到目标艏向的旋转方式。具体处理方法流程图如附图3所示，采用下面的逻辑将目标艏向和初始艏向之间的差值限定在-180°到180°之间：

只要ψ_d-ψ₀>=180，则ψ_d-ψ=ψ_d-ψ-360

只要ψ_d-ψ₀<180，则ψ_d-ψ=ψ_d-ψ+360

并通过ψ_d-ψ₀的符号来得到sgn的取值：

$\mathrm{sgn}=\left\{\begin{array}{c}1,{\mathrm{\&psi;}}_d-{\mathrm{\&psi;}}_0>=0\\ -1,{\mathrm{\&psi;}}_d-{\mathrm{\&psi;}}_0<0\end{array}\right.---\left(10\right)$

同时，需要考虑当前时刻船舶艏向ψ(k)基本达到目标艏向时，(9)式对目标艏向的计算已不再适用，此时需要将当前期望艏向设定为目标艏向，并在之后的时间均保持不变，以完成整个定位旋转过程，判断依据如下：

当|ψ_d-ψ(k)|<=2·T_S·r_d时，ψ(k)_d=ψ_d

（5）计算当前时刻期望位置

根据当前时刻船舶的艏向ψ(k)，计算得到当前时刻船舶几何中心位于期望圆弧轨迹上的大地坐标系中的期望位置(e(k)_d,n(k)_d)：

e(k)_d=e_R-R·sin(ψ(k)+ψ_add)              (11)

n(k)_d=n_R-R·cos(ψ(k)+ψ_add)

（6）计算当前时刻的偏差

根据船舶数学模型(1)式解算船舶当前时刻的实际位置和艏向向量[n(k),e(k),ψ(k)]^Τ，并根据(8)式和(10)式解算船舶当前时刻的期望位置和艏向向量[n(k)_d,e(k)_d,ψ(k)_d]^Τ，在大地坐标系中计算动力定位船舶几何中心与其期望的位置偏差和艏向偏差：

E(k)_e=e(k)_d-e(k)

E(k)_n=n(k)_d-n(k)              (12)

E(k)_ψ=ψ(k)_d-ψ(k)

并为了控制器设计的方便，将大地坐标系中偏差转化成船体坐标系中的偏差：

E(k)_x=E(k)_e·sin(ψ(k))+E(k)_n·cos(ψ(k))

E(k)_y=E(k)_e·cos(ψ(k))-E(k)_n·sin(ψ(k))           (13)

E(k)_b=E(k)_ψ

（7）计算动力定位船舶的控制力和力矩

为了突显本发明对实际工程应用的作用，采用PID控制算法计算动力定位船舶的控制力和力矩。选择船舶纵向力的PID控制参数为K_{p_surge}、K_{i_surge}和K_{d_surge}，S_surge表示纵向偏差表示对纵向偏差的积分；选择船舶横向力的PID控制参数为K_{p_sway}、K_{i_sway}和K_{d_sway}，S_sway表示横向偏差表示对横向偏差的积分；选择船舶转艏力矩的PID控制参数为K_{p_yaw}、K_{i_yaw}和K_{d_yaw}，S_yaw表示艏向偏差表示对艏向偏差的积分；并通过船舶数学模型(1)式解算得到的船舶速度向量[u,v,r]^Τ，利用PID控制算法得到动力定位船舶纵向控制力F(k)_x、横向控制力F(k)_y和控制力矩N(k)_z，其表达式为：

F(k)_x=K_{p_surge}·E(k)_x+K_{d_surge}·u+K_{i_surge}·S_surge        (14)

F(k)_y=K_{p_sway}·E(k)_y+K_{d_sway}·v+K_{i_sway}·S_sway        (15)

N(k)_Z=K_{p_yaw}·E(k)_b+K_{d_yaw}·r+K_{i_yaw}·S_yaw         (16)

（8）重复步骤（4）至（7），直至满足结束条件，输出结果图

将式(14)(15)(16)得到的控制力和力矩输入动力定位船舶的数学模型，解算得到下一时刻动力定位船舶的位置和艏向，时刻由当前时刻更新到下一时刻，然后判断是否到达目标位置与艏向。若未到达，则由当前时刻更新到下一时刻，重复执行步骤（4）～（7），直至到目标位置与艏向；若到达目标位置与艏向，则计算结束，输出结果。

以下描述本发明的实施例。

为了验证本发明方法的合理性、可行性，使用计算机编写了MATLAB程序，使用Simulink进行了仿真试验。该实施例包含以下步骤：

(1)试验条件

采用具体实施方式中公式(1)所给出的形式来描述动力定位船舶的数学模型，具体参数选择如下：

船长：76.2m；

船舶质量：3000吨；

船舶惯性矩阵：M=[0.0034*10⁹,0,0;0,0.0057*10⁹,-0.0170*10⁹;0,-0.0170*10⁹,2.2262*10⁹]；

船舶阻尼阵：D=[0.0004*10⁸,0,0;0,0.0013*10⁸,-0.0102*10⁸;0,-0.0034*10⁸,1.9250*10⁸]；

船舶的科里奥利和向心力矩阵：C=[0,0,0;0,0,0;0,0,0]。

(2)初始值设置

船舶几何中心大地坐标系中初始位置和艏向向量：[-50,0,0]^Τ；

设定旋转中心的船体坐标：(0,50)；

设定目标艏向：180°；

设定回转角速度：0.33°/s；

采样时间：0.3s；

设定仿真时间为：900s。

(3)试验结果及分析

附图6为整个定位旋转过程动力定位船舶在大地坐标中的位置和艏向变化曲线，图中的虚线船形为每隔300个采样点所记录的船舶的位置和艏向，实现船形为最终船舶的位置和艏向，每个船形钟星形点为船舶的几何中心，圆弧曲线为船舶几何中心在整个定位旋转过程中所走过的轨迹，小圈点表示大地坐标系中设定船舶回转中心初始位置。

附图7给出了动力定位船舶在定位旋转过程中设定回转中心与它的期望位置（初始大地坐标）的北向位置偏差变化曲线，附图8为设定回转中心与它的期望位置（初始大地坐标）的东向位置偏差变化曲线，附图9给出了动力定位船舶在运动过程中实际艏向与目标艏向间的偏差变化曲线，附图10则为船舶运动过程中回转角速度随的时间响应曲线。

根据附图6至附图10所给出的船舶动态响应过程，得到如下的分析结果：

船舶的运动过程与图5给出的期望的运动情况相同，在大地坐标系中，本发明给出的方法能够控制船舶的几何中通过绕给定旋转中心初始大地坐标位置的圆弧运动，达到目标艏向。而且北向位置偏差和东向位置偏差均能控制在2m以内，能够达到实船控制要求，在旋转过程中回转角速度可以到达设定值，并在旋转结束时回到0。

而且由这组试验结果可见，在控制过程中基于实施步骤（3）到（6）所给出的导引算法可以引导动力定位船舶在保持船上设定旋转中心大地坐标系中位置不变的情况下，自动地以最短的时间达到目标艏向。本发明设计的方法能够保持较高的控制精度，和平稳的动态响应。同时改变推力分配环节来实现定位旋转，需要获得船舶以不同旋转中心做转动的向心力和回转力矩，且无法形成控制回路，而在实船试验中无法获得船舶以不同的运动中心旋转的精确向心力，这使得本方法较改变推力分配环节的方法具有更高的安全性和可靠性，使得本发明设计的方法更具有工程应用价值。

Claims (1)

1.一种动力定位船舶实现定位旋转功能的几何坐标变换导引控制方法，其特征在于，包括如下步骤：

(1)船舶进入定位旋转工作状态，并完成系统初始化；

设定初始速度为[u₀,v₀,r₀]^Τ=[0,0,0]^Τ；

所涉及的大地坐标系EO_EN描述为：选取海平面上任意一点为原点，以正东方向为横轴、正北方向为纵轴；船体坐标系YO_bX描述为：以船舶的几何中心为原点，以右舷方向为横轴、船艏方向为纵轴；

(2)利用船舶运动模型进行定位解算，结合船体坐标系中设定的船舶目标艏向和旋转中心的船体坐标，获得船舶几何中心的期望运动轨迹；

所涉及的动力定位船舶在大地坐标系和船体坐标系中的数学模型为：

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

$M\stackrel{\&CenterDot;}{v}+\mathrm{Dv}+\mathrm{Cv}=\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-Y_{\stackrel{\&CenterDot;}{r}}& I_z-N_{\stackrel{\&CenterDot;}{r}}\end{array}\right],$ m为船舶质量，I_z为转动惯量，x_g为船体坐标系中船舶质心纵向坐标，其他参数均为一阶水动力导数；D为船舶水动力阻尼项， $D=-\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& Y_r\\ 0& N_v& N_r\end{array}\right];$ C为船舶的科里奥利力和向心力矩阵；

所述船舶几何中心的期望运动轨迹的大地坐标表达式：

e_d=e_R-R·sin(ψ+ψ_add)

n_d=n_R-R·cos(ψ+ψ_add)

式中，ψ_add为船舶几何中心指向设定旋转中心的向量与船舶艏向的夹角；R为船舶几何中心的旋转半径，且有

e_R,n_R表示船舶进入定位旋转功能时刻旋转中心的大地坐标，其表达式为：

e_R=e₀+x₀·sin(ψ₀)+y₀·cos(ψ₀)

n_R=n₀+x₀·cos(ψ₀)-y₀·sin(ψ₀)

式中，n₀,e₀和ψ₀分别为动力定位船舶几何中心在大地坐标系中的位置和艏向；(y_o,x_o)表示旋转中心的船体坐标，并在定位旋转过程中保持不变；

(3)计算当前时刻船舶的期望艏向；

当前时刻船舶的期望艏向ψ(k)_d的表达式为：

ψ(k)_d=ψ(k)+sgn·r_d·T_s；

式中，ψ(k)为当前实际艏向；ψ_d为操作员设定的目标艏向；r_d为回转角速度；T_S为采样周期；sgn为一符号函数，由目标艏向和初始艏向之差决定；

sgn的值获取方法为：首先对目标艏向和初始艏向的差值采用如下逻辑进行预处理：只要ψ_d-ψ₀>=180，ψ_d-ψ=ψ_d-ψ-360；只要ψ_d-ψ₀<180，ψ_d-ψ=ψ_d-ψ+360；然后通过ψ_d-ψ₀的符号来得到sgn的取值： $\mathrm{sgn}=\left\{\begin{array}{c}1,{\mathrm{\&psi;}}_d-{\mathrm{\&psi;}}_0>=0\\ -1,{\mathrm{\&psi;}}_d-{\mathrm{\&psi;}}_0<0\end{array}\right.;$

在判定当前时刻船舶艏向ψ(k)基本达到目标艏向时，需要将ψ(k)_d当前期望艏向设定为目标艏向ψ_d，此时采用如下判断条件进行判定：

当|ψ_d-ψ(k)|<=2·T_S·r_d时，ψ(k)_d=ψ_d；

(4)根据船舶的期望运动轨迹以及当前时刻船舶的实际艏向ψ(k)，计算当前时刻船舶的期望位置；

大地坐标系中船舶当前时刻的期望位置(e(k)_d,n(k)_d)计算方法如下：

e(k)_d=e_R-R·sin(ψ(k)+ψ_add)

n(k)_d=n_R-R·cos(ψ(k)+ψ_add)；

(5)根据船舶当前时刻的实际位置和艏向以及期望位置和艏向,计算船体坐标系中的位置偏差和艏向偏差；

船体坐标系中的横向位置偏差、纵向位置偏差和艏向偏差的表达式为：

E(k)_x=E(k)_e·sin(ψ(k))+E(k)_n·cos(ψ(k))

E(k)_y=E(k)_e·cos(ψ(k))-E(k)_n·sin(ψ(k))

E(k)_b=E(k)_ψ

式中，E(k)_e，E(k)_n，E(k)_ψ分别为大地坐标系中东向位置偏差、北向位置偏差和艏向偏差，其表达式为：

E(k)_e=e(k)_d-e(k)

E(k)_n=n(k)_d-n(k)

E(k)_ψ=ψ(k)_d-ψ(k)

(6)采用PID控制算法获得船舶定位旋转的控制力和力矩；

动力定位船舶纵向控制力F(k)_x、横向控制力F(k)_y和控制力矩N(k)_z的表达式为：

F(k)_x=K_{p_surge}·E(k)_x+K_{d_surge}·u+K_{i_surge}·S_surge

F(k)_y=K_{p_sway}·E(k)_y+K_{d_sway}·v+K_{i_sway}·S_sway

N(k)_Z=K_{p_yaw}·E(k)_b+K_{d_yaw}·r+K_{i_yaw}·S_yaw

式中，K_{p_surge}、K_{i_surge}和K_{d_surge}为船舶纵向力的PID控制参数，S_surge表示对纵向偏差的积分；K_{p_sway}、K_{i_sway}和K_{d_sway}为船舶横向力的PID控制参数，S_sway表示对横向偏差的积分；K_{p_yaw}、K_{i_yaw}和K_{d_yaw}为船舶转艏力矩的PID控制参数，S_yaw表示对艏向偏差的积分；u,v,r是通过船舶数学模型解算得到的船舶速度向量；

(7)将控制力和力矩反馈给动力定位船舶的数学模型，解算下一时刻船舶的实际位置和艏向，然后由当前时刻更新到下一时刻；

(8)重复执行步骤（2）～（7），直至船舶到达目标位置和艏向，输出结果，实现定位旋转。

Images