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

摘要

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

主权项

一种动力定位船舶循迹导引控制方法，其特征在于包括如下步骤：(1)选取海平面上任意一点为原点，以正东方向为横轴、正北方向为纵轴，建立大地坐标系EO_EN；然后，以船舶的几何中心为原点，以右舷方向为横轴、船艏方向为纵轴，建立船体坐标系YO_bX；(2)在大地坐标系和船体坐标系中建立动力定位船舶三自由度船舶高速运动数学模型：$\stackrel{·}{\eta }=Jv$$M\stackrel{·}{v}+D\left(v\right)v+C\left(v\right)v=\tau$式中，η表示船舶在大地坐标系中位置和艏向向量[n,e,ψ]^Τ；v表示船舶在船体坐标系中线速度和角速度向量[u,v,r]^Τ；τ为推进器产生的力和力矩向量[F_x,F_y,N_z]^Τ；J为从船体坐标到大地坐标的转换矩阵，$J=\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & \cos \psi & 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& -\left(m-Y_{\stackrel{·}{v}}\right)v-\left({\mathrm{mx}}_g-Y_{\stackrel{·}{r}}\right)r\\ 0& 0& (m-X_{\stackrel{·}{u}})u\\ \left(m-Y_{\stackrel{·}{v}}\right)v+\left({\mathrm{mx}}_g-Y_{\stackrel{·}{r}}\right)r& -(m-X_{\stackrel{·}{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}|＜ε时，船舶则开始对下一条直线段航迹进行循迹，期望艏向的计算方法如步骤②，直到最后一条航迹，循迹结束。