0 引　言

近年来，无人船的相关应用和研究得到了越来越多的关注。其中除了广泛应用于现代化军事战争外，无人船在搜救、商业、科研等方面的应用也越来越多。无人船的路径跟踪问题是无人船规划控制问题中重要的一部分，路径跟踪问题是指无人船根据路径的相关几何性质完成位置跟踪的过程。

在无人船的导航算法方面，Lefeber等^[1]采用LOS算法获取船舶运动过程的期望航向，并基于期望航向和航迹偏差的取值作为算法输入从而控制航向，使船舶收敛到期望路径；Fossen等^[2]以LOS算法为基础，开展了一系列有关欠驱动船舶轨迹追踪控制的研究，提出了轨迹追踪控制算法；韩鹏等^[3]在LOS导航中引入了自适应接纳圆，使得LOS导航在拐点处具有更高的跟踪精度。

在无人船的控制算法领域，1922年，Sperry等^[4]第一次将PID控制器与船舶运动控制相结合，使之控制船舶的自动转向，并取得了一定的效果。后来Sugimoto等^[5]基于传统PID控制做出了优化，将自适应的思想融入其中，提出了自适应PID控制。张旋武^[6]提出基于DDPG的PID航向控制器参数优化方法，有效提高了PID控制的抗干扰能力。

前述研究方法存在2个问题：一是在不确定的水环境下不能实时准确的描述船体的运动状态，二是部分算法复杂度相对较高，系统的实时性不能完全得到保证。为了解决上述问题，本文基于无人船船体运动模型和海洋环境风、浪、流的数学模型，采用LOS导航算法和模糊PID控制器组合的算法解决无人船的路径跟踪问题。

1 无人船运动模型 1.1 船体运动模型

船舶运动是在三维空间的六自由度运动，如表1所示。

船舶的六自由度运动可以大致分为两类，一类为船舶低频运动，包括纵荡、横荡、首摇，其可以直接改变船舶的总体运动状态，表示为船舶的航行方向、垂直航行方向上的移动和船舶航向的转动。另一类运动为船舶高频运动，包括横摇、纵摇和垂荡，表现为船舶在运动过程中的震荡，不会影响船舶的平均位置变化，因此在对船舶运动控制进行模拟的时候通常可以忽略这部分的运动。水平面运动坐标系如图1所示。

静坐标系与动坐标系之间的旋转矩阵表示为：

${\scriptsize T = \left[ \begin{gathered} \cos \psi \cos \theta {\text{ cos}}\psi {\text{sin}}\theta {\text{sin}}\phi - \sin \psi \cos \phi {\text{ }}\cos \psi \sin \theta \cos \phi + \sin \psi \sin \phi {\text{ }} \\ \sin \psi \cos \theta {\text{ sin}}\psi {\text{sin}}\theta {\text{sin}}\phi {\text{ + cos}}\psi {\text{cos}}\phi {\text{ }}\sin \psi \sin \theta \cos \phi - \cos \psi \sin \phi \\ {\text{ }} - \sin \theta {\text{ }}\cos \theta \sin \phi {\text{ }}\cos \theta \cos \phi \\ \end{gathered} \right]。} $

(1)

只考虑纵荡、横荡、首摇等水平面内的运动，忽略横摇、纵摇和垂荡等运动，可以将旋转矩阵简化为：

$ R(\psi ) = \left[ \begin{array}{cccccccccccccc} \cos \psi & - {\text{sin}}\psi & 0 \\ \sin \psi& {\text{ cos}}\psi & {\text{ 0}} \\ 0 & 0 & 1 \end{array} \right]，$

(2)

动静坐标系速度关系：

$ \dot \xi = u\cos \psi - v\sin \psi ，$

(3)

$ \dot \eta = u\sin \psi + v{\text{cos}}\psi ，$

(4)

动静坐标系角速度关系：

$ \dot \psi = r。$

(5)

只考虑水平面上3个自由度时，坐标系原点与重心不重合时的船舶水平面运动方程可以表示为：

$ \begin{split} & m\left[ {\dot u - vr - {x_G}{r^2} - {y_G}\dot r} \right] = X = {X_H} + {X_E} + {X_T} ，\\ & m\left[ {\dot v + ur - {y_G}{r^2} + {x_G}\dot r} \right] = Y = {Y_H} + {Y_E} + {Y_T}，\\ & {I_z}\dot r + m\left[ {{x_G}(\dot v + ur) - {y_G}(\dot u - vr)} \right] = N = {N_H} + {N_E} + {N_T} 。\end{split} $

(6)

式中：$ {X_H} $、$ {Y_H} $、$ {N_H} $分别为船舶在水平面3个自由度上受到的水动力；$ {X_T} $、$ {Y_T} $、$ {N_T} $分别为推进器的推力以及推力矩；$ {X_E} $、$ {Y_E} $、$ {N_E} $分别为受到风浪流的作用下产生的干扰力和干扰力矩。

船舶在水平面上运动的水动力表示为船舶运动状态的函数：

$ G = G(u,v,r,\dot u,\dot v,\dot r)。$

(7)

其中，G可通过直航工作点下的泰勒级数展开来进行计算。假设选定的展开点是船舶沿X轴方向的匀速直线航行，则有：

$ u = U{\text{ }}v = r = \dot u = \dot v = \dot r = 0。$

(8)

在船舶直航工作点下将纵向、横向水动力以及偏航力矩进行三阶泰勒展开，保留三次项可得轴向力、横向力、偏航力矩的方程：

$ \begin{split} (m - {X_{\dot u}})\dot u =& {X_u}u + {X_{uu}}{u^2} + {X_{uuu}}{u^3} + {X_{vv}}{v^2} + {X_{rr}}{r^2} +\\ & {X_{vr}}vr + {X_E} + {X_T}，\end{split} $

(9)

$ \begin{split} (m - {Y_{\dot v}})\dot v + (m{x_G} - {Y_{\dot r}})\dot r = & {Y_v}v + {Y_r}r + {Y_{vvv}}{v^3} + {Y_{vvr}}{v^2}r +\\ & {Y_{uv}}uv + {Y_{ur}}ur + {Y_E} + {Y_T}，\\[-1pt] \end{split} $

(10)

$ \begin{split} &(m{x_G} - {N_{\dot v}})\dot v + ({I_z} - {N_{\dot r}})\dot r = {N_v}v + {N_r}r + {N_{vvv}}{v^3} +\\ & {N_{vvr}}{v^2}r + {N_{uv}}uv + {N_{ur}}ur + {N_E} + {N_T}。\end{split} $

(11)

根据上述方程求解可得：

$ \begin{split} & {f_m} = (m - {Y_{\dot v}})({I_z} - {N_{\dot r}}) - (m{x_G} - {Y_{\dot r}})(m{x_G} - {N_{\dot v}}) ，\\ & {f_x} = {X_u}u + {X_{uu}}{u^2} + {X_{uuu}}{u^3} + {X_{vv}}{v^2} + {X_{rr}}{r^2} + {X_{vr}}vr + {X_E} + {X_T}，\\ & {f_y} = {Y_v}v + {Y_r}r + {Y_{vvv}}{v^3} + {Y_{vvr}}{v^2}r + {Y_{uv}}uv + {Y_{ur}}ur + {Y_E} + {Y_T}，\\ & {f_n} = {N_v}v + {N_r}r + {N_{vvv}}{v^3} + {N_{vvr}}{v^2}r + {N_{uv}}uv + \\ & \qquad {N_{ur}}ur + {N_E} + {N_T}。\\[-1pt] \end{split} $

(12)

$ \begin{split} & \dot u = {f_x}/(m - {X_{\dot u}})，{\text{ }}\; \dot \xi = u\cos \psi - v\sin \psi，\\ & {\text{ }}\dot v = {f_y} \cdot ({I_z} - {N_{\dot r}})/{f_m} - {f_n} \cdot (m{x_G} - {Y_{\dot r}})/{f_m}，\\ & \dot \eta = u\sin \psi + v{\text{cos}}\psi，{\text{ }} \\ & \dot r = {f_n} \cdot (m - {Y_{\dot v}})/{f_m} - {f_y} \cdot (m{x_G} - {N_{\dot v}})/{f_m}，{\text{ }}\; \dot \psi = r 。\end{split} $

(13)

1.2 海洋环境风、浪、流数学模型

海洋环境中风、浪、流分别会产生海风干扰力、海浪干扰力和海流干扰力，其数学模型如下：

1）海风干扰力

绝对风是指在大地坐标系内测量到的风。$ {V_T} $为绝对风速，$ {\psi _T} $为绝对风向，其范围为0°～360°，其中规定正北方向为$ {0^ \circ } $，正东方向为90°。在船舶实际航行时，相对风的大小决定了船舶受到来自风的压力大小，相对风越大风压力越大。相对风指在船舶本身的坐标系中观测到的风，计算式为：

$ \begin{split} & {u_R} = {V_T}\cos ({\psi _T} - \psi ) + u，\\ & {v_R} = {V_T}\sin ({\psi _T} - \psi ) - v 。\end{split} $

(14)

式中：$ {u_R} $、$ {v_R} $为相对风速$ {V_R} $的2个分量。

相对风向角（风舷角）$ {\alpha _R} $计算方法如下：

$ {\alpha _R} = \left\{ \begin{split} & \arctan \dfrac{{{v_R}}}{{{u_R}}} - {\text{π}} {{\mathrm{sgn}}} ({v_R})，\qquad {u_R} \geqslant 0 ，\\ & {\text{ }}\arctan \dfrac{{{v_R}}}{{{u_R}}}，\qquad {u_R} \lt 0。\end{split} \right. $

(15)

相对风速可以通过绝对风速和船速计算得到，计算式为：

$ V_R^2 = V_T^2 + {V^2} + 2{V_T}V\cos ({\psi _T} - \beta )。$

(16)

式中：$ V $为船航速，$ V = \sqrt {{u^2} + {v^2}} $；$ \beta $为漂角。

作用在船体上的平均风力和力矩的计算式为：

$ \left\{ \begin{gathered} {X_{wind}} = \frac{1}{2}{C_X}({\alpha _R}){\rho _a}V_R^2{A_T} ，\\ {Y_{wind}} = \frac{1}{2}{C_Y}({\alpha _R}){\rho _a}V_R^2{A_L}，\\ {N_{wind}} = \frac{1}{2}{C_N}({\alpha _R}){\rho _a}V_R^2{A_L}L 。\end{gathered} \right. $

(17)

式中：$ {C_X} $和$ {C_Y} $为风力系数；$ {C_N} $为风力矩系数；$ {\rho _a} $为空气密度，$ {\mathrm{kg}}/{{\mathrm{m}}^3} $；$ {A_T} $和$ {A_L} $为正投影面积和侧投影面积，$ {{\mathrm{m}}^2} $；$ L $为船舶总长，$ {\mathrm{m}} $；$ {V_R} $为相对风速，kn。

风力和风力矩系数的近似估算常用的方法是Isherwood在1972年提出的Isherwood经验公式。其基于以往大量的船模风洞试验结果，运用多元回归技术对试验结果进行处理分析，得到了以下系数经验公式：

$ \begin{split} & {C_X} = {A_0} + {A_1}\frac{{2{A_L}}}{{{L^2}}} + {A_2}\frac{{2{A_T}}}{{{B^2}}} + {A_3}\frac{L}{B} + {A_4}\frac{S}{L} + {A_5}\frac{C}{L} + {A_6}M, \\ & {C_Y} = - \left( {B_0} + {B_1}\frac{{2{A_L}}}{{{L^2}}} + {B_2}\frac{{2{A_T}}}{{{B^2}}} + {B_3}\frac{L}{B} + {B_4}\frac{S}{L} +\right.\\ & \qquad\left.{B_5}\frac{C}{L} + {B_6}\frac{{{A_{SS}}}}{{{A_L}}} \right), \\ & {C_N} = - \left( {{C_0} + {C_1}\frac{{2{A_L}}}{{{L^2}}} + {C_2}\frac{{2{A_T}}}{{{B^2}}} + {C_3}\frac{L}{B} + {C_4}\frac{S}{L} + {C_5}\frac{C}{L}} \right)。\\[-1pt] \end{split} $

(18)

2）海流干扰力

海水通常并非静止不动，海流的大小对船舶运动也有较大的影响。海流一般由潮汐、重力、海水密度变化等因素导致，随气候变化、地理位置变化等发生改变。

将运动方程中的船舶运动参数处理成与海流的相对运动参数。设固定坐标系中的流速为$ {V_c} $，流向为$ {\psi _c} $，在运动坐标系上的分量表示为：

$ \left\{ \begin{gathered} {u_c} = {V_c}\cos ({\psi _c} - \psi )，\\ {v_c} = {V_c}\sin ({\psi _c} - \psi ) 。\end{gathered} \right. $

(19)

计算相对水流的速度：

$ \left\{ \begin{gathered} {u_r} = u - {u_c} ，\\ {v_r} = v - {v_c}。\end{gathered} \right. $

(20)

结合式（19）和式（20）求导得加速度：

$ \left\{ \begin{gathered} {{\dot u}_r} = \dot u - {V_c}r\sin ({\psi _c} - \psi ) = \dot u - r{v_c}，\\ {{\dot v}_r} = \dot v + {V_c}r\cos ({\psi _c} - \psi ) = \dot v + r{u_c}。\end{gathered} \right. $

(21)

式中，r为首摇角速度。

通过上述公式可以得到船舶在海流干扰下的相对速度和相对加速度。从而在数学模型的角度上运用船舶在水流中的相对运动参数替代原有的船舶在静水中的运动参数，使数学模型中加入了海流对船舶运动的影响。

3）海浪干扰力

海浪造成的干扰力主要包括一阶波浪力和二阶波浪力。一阶波浪力即高频波浪干扰力，对船舶横向和纵向的旋转运动影响较大，容易造成船舶大纵摇大垂荡运动，二阶波浪力则会直接影响船舶的运动轨迹以及航向，对船舶的运动状态影响更明显。

波浪漂移力和力矩可以表达为：

$ \left\{ \begin{gathered} {X_{wave}} = \frac{1}{2}\rho ghL\zeta _D^2{C_{XD}}(\lambda )\cos \chi，\\ {Y_{wave}} = \frac{1}{2}\rho ghL\zeta _D^2{C_{YD}}(\lambda )\sin \chi，\\ {N_{wave}} = \frac{1}{2}\rho ghL\zeta _D^2{C_{ND}}(\lambda )\sin \chi 。\end{gathered} \right. $

(22)

波浪力和力矩系数的公式可以表示为：

$ \begin{split} & {C_{XD}}(\lambda ) = 0.05 - 0.2\left( {\frac{\lambda }{L}} \right) + 0.75{\left( {\frac{\lambda }{L}} \right)^2} - 0.51{\left( {\frac{\lambda }{L}} \right)^3}，\\ & {C_{YD}}(\lambda ) = 0.46 - 6.83\left( {\frac{\lambda }{L}} \right) - 15.65{\left( {\frac{\lambda }{L}} \right)^2} + 8.84{\left( {\frac{\lambda }{L}} \right)^3}，\\ & {C_{ND}}(\lambda ) = - 0.11 + 0.68\left( {\frac{\lambda }{L}} \right) - 0.79{\left( {\frac{\lambda }{L}} \right)^2} + 0.21{\left( {\frac{\lambda }{L}} \right)^3}。\end{split} $

(23)

在实际计算过程中，波高与风速的关系可以参考表2中经验参数：

2 LOS导航算法与PID控制器设计 2.1 路径跟随框架

路径跟随问题是指已通过路径规划方法获取目标路径之后，利用一些算法和控制方法实现无人船沿着目标路径航行的过程。本文通过LOS导航和传统PID控制的方式解决无人船的路径跟随问题。首先LOS导航算法根据目标路径计算出无人船的目标航向，之后通过PID控制获得无人船的航行舵角从而操纵无人船的运动。整理过程如图2所示。

2.2 LOS导航算法

LOS导航算法是一种常见的路径跟随控制算法。无人水面船的运动过程主要为三自由度运动，即横向位置、纵向位置、首摇角的变化，而本文根据LOS导航算法，假设无人船运动过程中速度大小保持不变，将目标路径上的某点方向作为LOS航向，即可将三自由度的运动问题转化为一自由度的航向控制问题。

假设路径规划得到的目标路径为P₀P₁P₂，无人船当前位置为O，如图3所示。

目标航向控制问题即可转化为目标点P_LOS的坐标求解问题。本文以无人船位置为圆心、R为半径的圆与目标路径P₀P₁P₂的交点中靠近下一目标点的一个作为目标点P_LOS，则有：

$ \left\{\begin{aligned} & ({{x}}_{{\mathrm{LOS}}}{-x})^{2}+({{y}}_{{\mathrm{LOS}}}{-y})^{2}={R}^{2}，\\ & \mathrm{tan}{\phi }_{d}=\frac{{{y}}_{{\mathrm{LOS}}}{-y}}{{{x}}_{{\mathrm{LOS}}}{-x}}。\end{aligned}\right. $

(24)

式中：x_LOS、y_LOS为P_LOS的坐标值；x、y为O点坐标值；$ {\varphi _d} $为无人船航向角。

对于R的取值，一方面在无人船偏移量较大时要保证圆与路径有交点，一方面要在无人船偏移量较小时避免目标点过远导致结果受环境影响大，综合考虑本文取R大小为：

$ {R_{LOS}} = \left\{ {\begin{split} & {3L,\left| e \right| \leqslant 3L}，\\ & {e + L,\left| e \right| \gt 3L} 。\end{split}} \right. $

(25)

式中：e为无人船相对于目标路径的横向偏移量；L为船长。

另外对于路径转折处，如果仍用相同的方式选取目标点P_LOS会导致转向突然，舵角变化率过大，运动不平滑，因此采用接纳圆的方式引导无人船转向。当无人船进入距目标路径转折点R₀的范围内时开始进行路径切换。即当$ ({x}_{{}_{P1}}{-x}{)}^{2}{+}({{y}}_{{}_{P1}}{-y}{)}^{2}\leqslant {R}_{0}{}^{2} $时进行路径切换。一般的取R₀=2L即可保证大多数情况下无人船可以平滑转向。

2.3 PID控制器设计

PID控制器方法理论简单、可操作性强，广泛应用于无人船的轨迹控制中。经典PID控制器原理如图4所示。

根据LOS导航算法得到目标航向$ {\varphi _d} $，同时获取无人船实时航向角$ \varphi $，得到航向角偏差$ \Delta \varphi = {\varphi _d} - \varphi $。将航向角偏差作为输入可以得到经典PID控制器的表达式为：

$ \delta = {K_p}\Delta \varphi + {K_i}\int {\Delta \varphi {\mathrm{d}}t + } {K_d}\frac{{{\mathrm{d}}\Delta \varphi }}{{{\mathrm{d}}t}}。$

(26)

式中：$ \delta $为无人船操舵舵角；$ {K_p}、{K_i}、{K_d} $为经典PID控制的系数。

比例系数$ {K_p} $：将系统的偏差按照一定比例反馈给系统，$ {K_p} $越大，系统响应越快，但动态特性越差，可能出现振荡。

积分系数$ {K_i} $：可以迅速改变系统的稳态误差，$ {K_i} $越大，静态误差减小越快，但是过大会导致超调量变大，降低系统稳定的时间。

微分系数$ {K_d} $：可以抑制误差变化，保证系统的稳定性，减少系统稳定的时间，但是会影响系统对干扰的抑制能力。

因此合理选取$ {K_p}、{K_i}、{K_d} $的取值会对无人船的控制产生直接明显的影响。在经典PID控制中通常是根据大量的经验得到最理想的PID参数取值。但是这种方法一方面需要大量经验数据，另一方面得到的结果对外界干扰的抵抗性差，一旦有较大的干扰产生参数就可能需要重新调整，因此可以使用模糊控制的方法对经典PID控制器进行优化。

模糊控制是利用模糊规则推理，调节控制器参数的一种控制方法。模糊PID控制可以完善PID控制的自适应性能，根据控制要求对PID控制参数$ {K_p}、{K_i}、{K_d} $进行实时调整，如图5所示。

模糊推理的规则大致如下：

1）当$ \Delta \varphi $较大时，为了加快系统的响应速度，应选取较大的$ {K_p} $，同时为了避免刚开始时$ \Delta \varphi $瞬间变大导致微分部分影响超出控制范围，$ {K_d} $应取较小值，同时此时$ \Delta \varphi $大，应限制积分作用，可取$ {K_i}{\text{ = }}0 $。

2）当$ \Delta \varphi $取值适中时，为了减小最终超调量，$ {K_p} $取值应减小些，$ {K_d} $的取值此时较为重要，要大小适中，保证系统的相应速度。$ {K_i} $可适当取值。

3）当$ \Delta \varphi $较小时，此时已接近目标值，为了保证系统稳定且避免振荡，K_d取值应实时变化，根据$ \Delta \varphi $的变化率调整，变化率大时$ {K_d} $取值减小，变化率小时K_d取值变大。$ {K_p} $和$ {K_i} $应适当增大取值。

通过上述规则可以根据无人船在航行过程中的实际运动状态以及目标航向/航迹调整PID控制器的参数，从而提高控制精度。

3 仿真结果分析

针对LOS导航导航算法和模糊PID控制器设计本文进行了如下仿真：

1）无接纳圆LOS导航算法+经典PID控制轨迹跟踪仿真。目标路径为夹角90°的折线路径，从图可以看出在转角处没有接纳圆的时候转向不平滑，会有较大的超调和波动，但一段时间后也能完成路径跟踪目标，转向部分有较大的偏移量。

2）有接纳圆LOS导航算法+经典PID控制轨迹跟踪仿真。对比图7可知，转向处增加接纳圆后会根据接纳圆的引导提前转向，从而使得转向过程更平滑，横向偏移量减小。

3）有接纳圆LOS导航算法+模糊PID控制轨迹跟踪仿真。在使用模糊PID控制后，无人船可以根据航行状态实时改变PID参数大小，从而改善控制器精度，对比图7和图8可知，控制过程的横向偏移量有了明显减少。

上述仿真结果表明采用有接纳圆的LOS导航算法和模糊PID控制器的路径跟踪控制算法能够明显提高无人船的路径跟踪响应性能，使得跟踪路径更平滑。

4 结　语

针对无人船的路径跟踪控制问题，本文采用了LOS导航算法将问题转化为航向跟踪问题，并基于模糊PID控制器对无人船的航行进行控制。本文进行了无接纳圆、经典PID控制器等对照组的设计，验证了基于模糊PID控制器的LOS导航算法路径跟踪精度高、横向偏移量小、跟踪路径更平滑的特点。