0 引　言

避碰路径规划是无人船舶自主航行和避碰研究的领域之一，属于自主式智能航行技术，具有重要的研究意义和工程应用价值^[1]。在诸如狭水道等复杂多变的水域中，船舶避碰路径规划的执行过程极为复杂。除了要考虑对所在水域的障碍物避碰外，还需进一步考虑船舶之间的动态避碰问题^[2]。

因此，在避碰路径规划系统设计中需要充分考虑无人船自身操纵性能、航行水域环境和周围航行船舶干扰等问题。

当前，国内外研究航行水域中船舶避碰路径的方法主要有群体智能算法、数据分析法、机器学习法、快速搜索随机树算法、动态窗口算法、速度障碍法和人工势场法等^[3]。但这些方法或多或少都存在一些问题，群体智能算法^[4]是基于己知不变信息，进行离线规划，并不能有效解决水域中存在干扰船舶避碰路径规划问题，更不能解决狭水道这样复杂、多变的水域环境中的避碰路径规划问题。数据分析法^[5]、机器学习法^[6]及动态窗口算法^[7]本质上同出一源，均是基于先前数据进行的二次专家决策^[8]。快速搜索随机树算法^[9]求解的最优路线存在凸点，不符合连续平滑特性，不能直接应用于船舶运动体线路规划。对于狭水道这样的复杂环境中，为避免与动态障碍物发生碰撞的船舶而言，基于速度障碍法^[10]的避碰效果很难达到，特别是在水域中存在多个障碍物时，速度和航向的寻优会产生振动^[11]。相较于上述方法，人工势场法最大的优势在于依据人的经验可任意构造障碍物势场，并且势场力能够融入到运动体动力学模型中，符合运动体连续实时避碰实际^[12]，但需要消除局部极小。

当前，基于势场法的改进策略都是根据运动体、障碍物和目标点之间的关系，对势场进行改进，却忽视了移动体的速度势场问题。一般而言，移动体和障碍物的相对速度越大、角度越小，碰撞后果越严重，基于此，本文增加三者之间的速度势场，即将无人船与障碍物和无人船与目标点间的相对速度融入到势场中，构建距离-速度势场模型。并将该方法应用于狭水道无人船自主航行避碰路径规划，以期获得符合工程实际的有效办法。

1 船舶运动数学模型

船舶的实际运动异常复杂，在一般情况下具有6个自由度。基于Fossen模型的欠驱动船舶运动控制纵不能直接求取向推力和转向力矩。而采用MMG分离型模型进行欠驱动船舶运动控制研究，可以将这些控制力和力矩进一步转换为舵角和柴油机油门量，即控制系统的输入量。由于船舶运动依赖于柴油机输出的螺旋桨转速和舵机输出的舵角，因此基于MMG模型设计的控制变量更符合实际情况。本文为了兼顾2种模型的优势，故而将2种模型进行融合，取长补短。据此，装配单桨-单舵的欠驱动USV，其“Fossen +MMG”的综合运动数学模型^[13]为：

$ \left\{ \begin{gathered} \dot x = u\cos \left( \psi \right) - v\sin \left( \psi \right) + {u_c}\cos \left( {{\psi _c}} \right)，\\ \dot y = v\cos \left( \psi \right) + u\sin \left( \psi \right) + {u_c}\sin \left( {{\psi _c}} \right) ，\\ \dot \varphi = r，\\ \dot u = \frac{{{m_{22}}}}{{{m_{11}}}}vr - \frac{{{f_u}\left( u \right)}}{{{m_{11}}}} + \frac{{T\left( \cdot \right)}}{{{m_{11}}}}n + {d_{wu}}，\\ \dot v = - \frac{{{m_{11}}}}{{{m_{22}}}}ur - \frac{{{f_v}\left( v \right)}}{{{m_{22}}}} + {d_{wv}}，\\ \dot r = - \frac{{{Y_H}{x_C}}}{{{m_{33}}}} - \frac{{{f_r}\left( r \right)}}{{{m_{33}}}} + \frac{{F\left( \cdot \right)}}{{{m_{33}}}}\delta + {d_{wr}}。\\ \end{gathered} \right. $

(1)

式中：$ x $、$ y $为USV重心在固定坐标系的位置；$ u $、$ v $和$ r $分别为船舶的纵荡速度、横荡速度、艏摇角速度；$ \psi $为航向角；$ {u_c} $为水流的流速；$ {\psi _c} $为水流的方向；$ {f_u}\left( u \right) $、$ {f_v}\left( v \right) $和$ {f_r}\left( r \right) $均为高阶流体动力项；$ {m_{11}} $、$ {m_{22}} $和$ {m_{33}} $均为包括附加质量在内的惯性参数；$ {Y_H}{x_C} $为船体黏性流体动力矩的修正项；$ T\left( \cdot \right) $为螺旋桨增益；$ F\left( \cdot \right) $为控制舵增益；$ {d_{wu}} $、$ {d_{wv}} $和$ {d_{wr}} $为不可测的外界海洋环境干扰力与力矩。

其中螺旋桨的增益$ T\left( \cdot \right) $和控制舵增益$ F\left( \cdot \right) $的表达式^[14]为：

$ \left\{ \begin{gathered} T\left( \cdot \right) = \left( {1 - {t_P}} \right)\left| n \right|\rho D_P^4{k_T}\left( {{J_P}} \right) ，\\ F\left( \cdot \right) = - 0.5\left( {{x_R} + {a_H}{x_H}} \right)\rho {A_R}{f_\alpha }U_r^2\cos \delta。\\ \end{gathered} \right. $

(2)

式中：推力减额系数$ {t_P} $与船型、螺旋桨尺度、负荷及与船体间的相对位置等因素有关，由经验公式获得，即，$ {t_P} \in \left( {0{,^{}}1} \right) $；$ {k_T}\left( {{J_P}} \right) $一般在0～1变化；$ \rho $、$ {D_P} $均是恒正，因此$ T\left( \cdot \right) $是恒正。舵上横向作用点的纵向坐标$ {x_R} $一般可取$ {x_R} = - 0.5L $，$ L $为船长；操舵诱导船体横向力的修正因子$ {a_H} $一般在0～1变化；操舵诱导船体横向力作用中心到船舶重心的距离$ {x_H} $为−0.5～−0.4；舵的约束条件$ \delta \in \left( { - 35,{}^{}35} \right) $。

2 基于势场法的船舶避碰路径建模 2.1 人工势场

人工势场是由Khatib^[15]提出，其算法原理是利用目标处势能低，障碍物处势能高生成人工势场。运动体在这样的市场中能够自动地以一定速度达到目标点。该运动体受到的总势场则是引力势场与斥力势场之和，其表达式为：

$ U\left(x\right)=U_{att}\left(x\right)+U_{rep}\left(x\right)。$

(3)

式中：$ {U_{att}}\left( x \right) $为目标产生的引力势场；$ {U_{rep}}\left( x \right) $为各个障碍物产生的斥力势场之和，即$ {U_{rep}}\left( x \right) = \displaystyle\sum\limits_{j = 1}^n {{U_{rep,j}}\left( x \right)} $。

目标点、障碍物对机器人的引力、斥力势场数学模型为：

$ {U_{att}}\left( x \right) = \frac{1}{2}\xi {d^2}\left( {x,G} \right)，$

(4)

$ {U_{rep,i}}\left( x \right) = \left\{ \begin{array}{ll} \dfrac{1}{2}{\eta _i}{\left( {\dfrac{1}{{{d_i}\left( x \right)}} - \dfrac{1}{{Q_i^*}}} \right)^2}，{d_i}\left( x \right) \leqslant Q_i^* ，\\ 0 ，d\left( {x,G} \right) > Q_i^* 。\end{array} \right. $

(5)

式中：$ \xi $为引力势的影响系数；$ d\left( {x,G} \right) $为机器人距目标的距离；$ x $为机器人当前位置；$ G $为目标点位置；$ \eta {}_i $为第$ i $个障碍物的斥力势的影响系数，静态障碍物取值较动态障碍物小一些；$ {d_i}\left( x \right) $为机器人距离第$ i $个障碍物的距离；$ Q_i^{^*} $为第$ i $个障碍物的斥力势作用范围。

2.2 融合船舶运动速度的人工势场构建

在经典人工势场算法中，当运动体、目标点和障碍物处于一条直线上时极易诱发局部极小的发生，导致运动体处于停滞状态。为此，在APF方法^{[16 − 17]}的基础上，增加运动体和障碍物及目标点之间的相对速度势场可以有效解决这一问题，如图1所示。

那么，目标点对移动体的引力势场数学表达式为，

$\begin{split}& {U_{att}}\left( {x,V} \right) =\\ &\left\{ \begin{array}{ll} \dfrac{1}{2}\xi {d^2}\left( {x,G} \right) + \dfrac{1}{2}{\xi _V} {\left\| {V - {V_{goa}}} \right\|^2} ，d\left( {x,G} \right) \leqslant d_G^* ，\\ d_G^*\xi d\left( {x,G} \right) - \dfrac{1}{2}d_G^*\xi + \dfrac{1}{2}{\xi _V}{\left\| {V - {V_{goa}}} \right\|^2} ，d\left( {x,G} \right) > d_G^*。\end{array} \right.\end{split} $

(6)

式中：$ \dfrac{1}{2}{\xi _V}{\left\| {V - {V_{goa}}} \right\|^2} $为速度引力势场；$ {\xi _V} $为速度引力系数；$ V $为当前速度；$ {V_{goa}} $为到达目标点的终止速度（通常是一个小量）；$ d_G^* $为引力作用范围界限。

基于引力势场函数的负梯度，得到目标点对船舶的引力大小：

$ \begin{split}& {F_{att}}\left( {x,V} \right) =\\ &\left\{ \begin{array}{ll} \xi d\left( {x,G} \right){e_{d,att}} + {\xi _V}{V_{R,goa}}{e_{{V_{R,goa}}}，} d\left( {x,G} \right) \leqslant d_G^* ，\\ d_G^*\xi {e_{d,att}} + {\xi _V}{V_{R,goa}}{e_{{V_{R,goa}}}}，d\left( {x,G} \right) > d_G^* 。\end{array} \right.\end{split} $

(7)

式中：$ {e_{d,att}} $为朝向目标点的单位向量；$ {e_{{V_{R,goa}}}} $为朝向目标点速度的单位向量；$ {V_{R,goa}} $为本船相对目标点速度的相对速度。

障碍物对船舶的排斥力之间的关系可以表示为：

$ \begin{split}&{U_{rep,i}}\left( {x,V} \right) = \\ &\left\{ \begin{array}{ll} \dfrac{1}{2}{\eta _i}{\left( {\dfrac{1}{{{d_i}\left( x \right)}} - \dfrac{1}{{Q_i^*}}} \right)^2} d{\left( {x,G} \right)^2} + \dfrac{1}{2}{\eta _V}{V_{R,obs}}^2 ，{d_i}\left( x \right) \leqslant Q_i^* ，\\ 0，{d_i}\left( x \right) > Q_i^* 。\end{array} \right.\end{split} $

(8)

式中：$ \dfrac{1}{2}{\eta _V}{V_{R,obs}}^2 $项为斥力势场；$ {\eta _V} $为斥力系数；$ {V_{R,obs}} = {\left( {V - {V_{obs}}} \right)^{\rm T}}{e_{R,obs}} $；$ V $为船舶目前速度；$ {V_{{\text{obs}}}} $为障碍物的移动速度。

根据梯度概念可知，$ {\nabla _R}{V_{R,obs}} = {e_{R,obs}} $，$ {\nabla _x}{V_{R,obs}} = \left({{{V_{R,obs}} \cdot {e_{R,obs}} - {V_{R,obs}}}}\right){\text /}{{{d_i}\left( x \right)}} $。令$ {V_{R,obs \bot }}{e_{R,obs \bot }} $为$ {V_{R,obs}} \cdot {e_{R,obs}} $的垂直分量，可得，$ {V_{R,obs \bot }}{e_{R,obs \bot }} = V - {V_{obs}} - {V_{R,obs}} \cdot {e_{R,obs}} $。进一步，可以得到，$ {\nabla _x} {V_{R,obs}} = - {{{V_{R,obs. \bot }}}}{\text /} {{{d_j} \left( x \right)}} $。

基于求斥力势场函数的负梯度，可求得障碍物对船舶的斥力，

$ {F_{rep,i}}\left( {x,V} \right) = \left\{ \begin{array}{ll} {F_{i,x}} + {F_{i,V}}，{d_i}\left( x \right) \leqslant Q_i^*\; {\mathrm{and}}\; V > 0，\\ 0 ，{d_i}\left( x \right) > Q_i^* \; {\mathrm{and}}\; V > 0 。\end{array} \right. $

(9)

式中：$ {F_{i,x}} $和$ {F_{j,V}} $分别为第$ i $个障碍物位置势场产生的斥力和速度势场产生的斥力。

$ \begin{split}{F_{i,x}} =& - {\nabla _x}\left\{ \frac{1}{2}{\eta _i}{{\left( {\frac{1}{{{d_i}\left( x \right)}} - \frac{1}{{Q_i^*}}} \right)}^2}d{{\left( {x,G} \right)}^2} + \right.\\ &\left.\frac{1}{2}{\eta _V}{V_{R,obs}}^2 \right\} ={F_{i,x}}1 + {F_{i,x}}2 + {F_{i,x}}3，\end{split} $

(10)

$ F_{i,x}1=-\eta_i\left(\frac{1}{d_i\left(x\right)}-\frac{1}{Q_i^*}\right)\frac{d_i\left(x,G\right)^2}{d_i\left(x\right)^2}，$

(11)

$ {F_{i,x}}2 = {\eta _i}{\left( {\frac{1}{{{d_i}\left( x \right)}} - \frac{1}{{Q_i^*}}} \right)^2}{d_i}\left( x \right){e_{R,goa}} ，$

(12)

$ {F_{i,x}}3 = \pm {\eta _V}{V_{R,obs}}\frac{{{V_{R,obs \bot }}{e_{R,obs \bot }}}}{{{d_i}\left( x \right)}}。$

(13)

式中：$ \pm $为左右方向，正号为右，负号为左；正负号根据船舶间的会遇态势及环境条件选取。

因此，可得，

$ \begin{split}F_{i,x} = & -\eta_i\left(\frac{1}{d_i\left(x\right)} - \frac{1}{Q_i^*}\right) \frac{d_i\left(x,G\right)^2}{d_i\left(x\right)^2} + \eta_i\left(\frac{1}{d_i\left(x\right)}-\frac{1}{Q_i^*}\right)^2 \times \\ & d_i\left(x\right)e_{R,goa}\pm\eta_VV_{R,obs}\frac{V_{R,obs\bot}e_{R,obs\bot}}{d_i\left(x\right)}。\end{split} $

(14)

同样地，可以得到，

$\begin{split} {F_{i,V}} = & - {\nabla _V}\left\{ \frac{1}{2}{\eta _i}{{\left( {\frac{1}{{{d_i}\left( x \right)}} - \frac{1}{{Q_i^*}}} \right)}^2}d{{\left( {x,G} \right)}^2} +\right.\\ &\left. \frac{1}{2}{\eta _V}{V_{R,obs}}^2 \right\} = - {\eta _V}{V_{R,obs}}{e_{R,obs}}。\end{split} $

(15)

进一步，可以得到障碍物对船舶的斥力：

$ \begin{split}F_{rep,i}\left(x,V\right)= & -\eta_i\left(\frac{1}{d_i\left(x\right)}-\frac{1}{Q_i^*}\right)\frac{d_i\left(x,G\right)^2}{d_i\left(x\right)^2}+ \\ & \eta_i\left(\frac{1}{d_i\left(x\right)}-\frac{1}{Q_i^*}\right)^2d_i\left(x\right)e_{R,goa}+\eta_VV_{R,obs} \\ & \frac{V_{R,obs\bot}e_{R,obs\bot}}{d_i\left(x\right)}-\eta_VV_{R,obs}e_{R,obs}。\end{split} $

(16)

可知，$ {F_{i,x}}3 = \pm {\eta _V}{V_{R,obs}}\dfrac{{{V_{R,obs \bot }}{e_{R,obs \bot }}}}{{{d_i}\left( x \right)}} $为垂直于本船于目标船位置连线的力，本船转向，如图2所示。

2.3 斥力势场作用范围设定与总势力场计算

为了和船舶实际避碰一致，斥力势场作用范围$ Q_j^{^*} $参数取值分船舶领域作用范围。为体现船舶操纵性能的影响，经过分析比较研究船舶领域模型适用情况，本文选择王宁等^[18]建立的自组织船舶领域识别模型。

由于周围存在多个障碍物，我船所受的斥力为：

$ F_{rep}=\sum\limits_{i=1}^nF_{repi}。$

(17)

式中：$ {F_{rep}}_i $为第$ i $个航行船舶或障碍物产生的斥力；$ n $为航行中遇到的航行船舶和障碍物的数量总和。

为此，我船所受到总的势场力$ {F_{all}} $可表示为：

$ {F_{all}} = {F_{att}} + {F_{rep}} 。$

(18)

那么，我船在总的势场力$ {F_{all}} $作用下采取相应避碰行动，最终向目标点运动前进。

3 算　例 3.1 场景构建 3.1.1 实验条件设置

测试范围采用平面笛卡尔坐标系绘制，单位为m。仿真实验条件设置如下：

1）本船O_s位于(1500, 10000)地理坐标，速度10 m/s，航向50°，目标点位于(17500, 21000)；

2）航行水域中存在3艘航行船舶和4种障碍物（诸如浅滩、沉船等）。其中，3艘航行船舶对应的是图3中的T_s1、T_s2和T_s3，4种障碍物对应的是图3中的O_bs1、O_bs2、O_bs3和O_bs4。T_s1位于(4000, 9000)地理坐标，速度4.0 m/s，航向45°，T_s2位于(13000, 11000)地理坐标，速度7.0 m/s，航向320°，T_s3位于(18500, 7000)地理坐标，速度9 m/s，航向358°；障碍物O_bs1位于(12500, 18500)地理坐标，障碍物O_bs2位于(18500, 15000)地理坐标，障碍物O_bs3位于(7000, 11000)地理坐标，障碍物O_bs4位于(4000, 17000)地理坐标。

在避碰过程中，干扰船(T_s1、T_s2和T_s3)的轨迹如图3所示。可知，本船出发后，受到障碍物和干扰船影响，船舶在引力和斥力共同作用下，航向和速度发生变化，但最终顺利航行至目的地。

3.1.2 参数设置

试验环境及条件：PC机-华硕天选5，i9-13980HX处理器，16GB内存，1TB硬盘，8GB显存，RTX4060显卡，操作系统为64bit Windows 10 Home Basic。采用的参数为：$ \xi = 200 $，$ {\xi _V} = 15 $，$ {\eta _V} = 150 $，静态障碍物$ \eta {}_j $=100，动态障碍物$ \eta {}_j $=300，$ Q_j^{^*} $具体计算参见文献[18]。

3.2 结果与讨论

仿真实验中，本船O_s航行操纵过程中与航行水域中存在3艘航行船舶和4种障碍物相对位置关系分别对应于图4和图5。

1）由图4可知：

①航行船舶T_s1为避离静态障碍物O_bs3，受斥力作用，其开始阶段航向向左改向较大（航向减小至20°左右）；但在被本船O_s追越后，为履行“保向保速”义务，其航向减小幅度变小（最终航向稳定在10°附近）。

②本船O_s追越航行船舶T_s1过程中，受斥力作用，航向大幅度向左改向，航向急剧减小至10°左右；在追越完毕后，为了再次避让障碍物O_bs4，其航向在斥力场的作用下，又迅速向右改向，航向快速增加到50°左右；随后在与航行船舶T_s2的对遇过程中，收到强大斥力作用，完成大幅度右转至航向增加到150°左右后，完成避让操作；最后，在引力势场的作用下，航向迅速减小至70°左右；在此过程中，遇到航行船舶T_s3和障碍物O_bs1，又受到斥力作用，航向先减小到20°附近后，在平缓增加到90°左右驶向目标点。

③船舶T_s2在航行过程中，与本船O_s距离较大，斥力作用不大，航向几乎不变。

④船舶T_s3在航行过程中，先后为了避让障碍物O_bs2、本船O_s和O_bs3，其航向先左转、再右转，航向值先快速减小、再迅速增加。

2）由图5可知：

①在仿真实验全局过程中，本船O_s与航行水域中的3艘航行船舶（T_s1、T_s2和T_s3）交汇过程中，相对距离总体呈现先减小、后增大的变化趋势；同时，最小的会遇距离是在与船舶T_s3避碰过程中降低的1800 m，该值大小约1 n mile，能够满足安全距离需求，也即表明本文的避碰规划算法对遇复杂条件下的船舶航行动态避碰有效。

②基于速度势场的复杂水域船舶避碰路径较为平滑，满足船舶操纵航行实际，符合《国际海上避碰规则》中关于“追越”、“对遇”及“交叉”三大典型局面船舶避碰规范。

4 结　语

基于本文所提算法模型，在计算机上构建了动、静态障碍物共存的复杂多变场景进行实验验证。结果表明融合速度势场的改进型人工势场算法能够有效解决在复杂水域中同时存在动态干扰船和静态障碍物时无人船的路径规划问题，能使运动体主动改向进行避碰的问题，且无人自主船舶能以主动改变航向的方式快速地避离障碍物。

航线规划技术作为智能船舶七大关键技术之一，对智能船舶发展具有关键促动作用。本文的研究从属航线规划技术领域，对实现智能船舶无人化航行和避碰控制有着实际应用价值。