0 引　言

水下滑翔机（underwater glider）是一种利用净浮力驱动，以锯齿形状轨迹进行航行的新型水下自主机器人，具有成本低、续航强、能耗少、噪声小等特点，在海洋研究、环境监测、资源勘探以及军事侦察等领域都有着广阔的应用前景^[1]。

路径规划问题是水下滑翔机研究领域的热点问题之一。很多学者对水下滑翔机的路径规划问题做了研究：Alvarez研究了水下滑翔机与浮标组成的优化采样问题，利用遗传算法对水下滑翔机进行路径规划^[2]。文献[3]采用改进的A^*算法，考虑了水下滑翔机水下不可控的特性，对其进行路径规划。文献[4]根据海洋环境和深度数据，结合RRT算法和A^*算法，规划了能耗较优的路径。Pereira^[5]将最小化碰撞风险作为目标，对水下滑翔机进行路径规划。朱心科^[6]基于能耗模型，采用两步链式Lin-Kemighan算法对水下滑翔机的海洋采样路径进行优化。

本文通过改进传统的人工势场法来解决水下滑翔机在海流作用下躲避静态障碍物、动态障碍物的路径规划问题。提出对传统人工势场法的改进方法，将水下滑翔机的运动约束引入到人工势场法中；之后，将海流力引入势场力模型，并推出航向角公式，最后，以HUST-2号水下滑翔机为例在不同环境下进行仿真试验。

1 改进的人工势场法

人工势场法算法高效、生成路径平滑，适用于路径实时性及安全性要求较高的路径规划任务^[7]。但本身也存在局部极值、目标不可达差等缺陷。

其原理是在目标点处设置引力场，障碍物周围设置斥力场，物体在2种势场力共同作用下运动，最终到达目标位置。

1.1 改进斥力势场函数

采用传统的斥力势场函数，当目标点在障碍物势场的影响距离内时，运动物体无法到达目标位置，因此考虑在斥力势场函数中加入与目标点的位置关系^[8]。新的斥力势场函数为：

$ {U_{reps}}(X) = \left\{ \begin{aligned} & {\frac{1}{2}{\eta _r}{{\left( {\frac{1}{{\rho (X,{X_o})}} - \frac{1}{{{\rho _0}}}} \right)}^2}{\rho ^2}(X,{X_g})} {,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \rho (X,{X_o}) \leqslant {\rho _0}} {\text{；}}\\ & 0,\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad {\rho (X,{X_o}) > {\rho _0}} {\text{。}} \end{aligned} \right. $

(1)

式中： ${\eta _r}$ 为斥力势场系数； $X$ ， ${X_o}$ 和 ${X_g}$ 分别为运动物体，障碍物和目标点的空间位置坐标； $\rho (X,{X_o})$ ， $\rho (X,{X_g})$ 分别为运动物体与障碍物，目标点的距离； ${\rho _0}$ 为斥力势场的作用范围，通常称为障碍物的影响半径。

相应斥力函数为（用 $\overrightarrow F $ 和 $F$ 区分矢量和标量，且 $F = |\overrightarrow {F{\kern 1pt} {\kern 1pt} } {\kern 1pt} |$ ，其他变量类似）：

$ {\overrightarrow F _{reps}}(X) = \left\{ \begin{aligned} &{{\overrightarrow F }_{reps1}}(X) \cdot \nabla \rho (X,{X_o}) - {{\overrightarrow F }_{reps2}}(X) \times \\ & \nabla \rho (X,{X_g}),\quad {\rho (X,{X_o})\leqslant {\rho _0}}{\text{；}}\\ &0, \quad\quad\quad\quad\ {\rho (X,{X_o}) > {\rho _0}}{\text{。}} \end{aligned} \right. $

(2)

式中： ${F_{reps1}}(X)$ 方向由障碍物指向运动物体， ${F_{reps2}}(X)$ 方向由运动物体指向目标点。

1.2 改进引力势场函数

物体在运动过程中，除目标点外，当斥力与引力的合力为0时，会陷入局部极值，最简单的情况如障碍物处于运动物体与目标点的连线上会出现受力为0，从而停止运动或震荡不前^[9]。考虑引入局部引力势场，驱使物体逃逸出来继续运动。局部引力势场为：

$ {U_{local}}(X) = \left\{ {\begin{array}{*{20}{c}} {\dfrac{\varepsilon }{{{\rho ^2}(X,{X_g})}}}&{,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \rho (X,{X_g}) > {\rho _g}} {\text{；}}\\ 0&{,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \rho (X,{X_g}) \leqslant {\rho _g}} {\text{。}} \end{array}} \right. $

(3)

式中： $\varepsilon $ 为局部引力势场系数； ${\rho _g}$ 为物体是否到达目标位置的评判距离，认为 $\rho (X,{X_g}) \leqslant {\rho _g}$ 时，运动物体到达目标点。若物体没陷入局部极值， ${U_{local}}(X) = 0$ 。

改进后的引力场 ${U_{att}}(X)$ 变为：

$ {U_{att}}(X) = \left\{ \begin{aligned} &{\dfrac{1}{2}{\xi _a}{\rho ^2}(X,{X_g})} , \quad\quad\quad\quad\quad\rho (X,{X_g}) > {\rho _g}\\ &{\text{且}}{F_{total}}\left( X \right) \ne 0{\text{；}}\\ &{\dfrac{1}{2}{\xi _a}{\rho ^2}(X,{X_g}) + \frac{\varepsilon }{{{\rho ^2}(X,{X_g})}}} , \ \rho (X,{X_g}) > {\rho _g}\\ &{\text{且}}{F_{total}}\left( X \right) = 0{\text{；}}\\ &0{, \quad\quad\quad\quad\quad\quad\quad\quad\quad\ \ \rho (X,{X_g}) \leqslant {\rho _g}}{\text{。}} \end{aligned} \right. $

(4)

式中， ${\xi _a}$ 为引力势场系数。

1.3 引入速度势场

水下滑翔机在水中航行时可能会遇到动态障碍物，引入速度势场可以把动态障碍物考虑进来^[10]。

将水下滑翔机与动态障碍物的速度分别用 $\overrightarrow V $ 和 ${\overrightarrow V _o}$ 来表示，用 ${U_{repv}}$ 表示相对速度 $(\overrightarrow V - {\overrightarrow V _o})$ 产生的速度斥力势场。速度斥力势场函数表示为：

$ {U_{repv}}(X) = \left\{ \begin{aligned} & {{\eta _v}\left| {\left\| {\overrightarrow V - {{\overrightarrow V }_o}} \right\|\sin \phi } \right| = {\eta _v}{V_{or}}\left| {\sin \phi } \right|} ,\\ &\quad\quad\quad{\rho (X,{X_o}) \leqslant {\rho _0}}{\text{；}}\\ & 0, \quad\quad {\rho (X,{X_o}) > {\rho _0}} {\text{。}} \end{aligned} \right. $

(5)

式中： ${\eta _v}$ 为速度斥力势场系数； ${V_{or}}$ 为障碍物与水下滑翔机的相对速度大小； $\phi $ 为相对速度矢量 ${\overrightarrow V _{or}}$ 与水下滑翔机与障碍物的中心连线形成的夹角，设水下滑翔机、动态障碍物的位置坐标分别为 $X = (x,y),{X_o} = ({x_o},{y_o})$ ，二者相对位置坐标 $(X - {X_o})$ = $({x_{ro}},{y_{ro}})$ 。

速度斥力函数 $F_{repv}^{}(X)$ 可由 $U_{repv}^{}(X)$ 的位置微分求得，方向垂直于水下滑翔机与障碍物的中心连线。

$\cos \phi > 0$ 表示水下滑翔机在朝向障碍物运动，此时需要相对速度斥力； $\cos \phi \leqslant 0$ 表示水下滑翔机在远离障碍物运动，此时就算没有速度斥力也不会与障碍物发生碰撞，因此 $\cos \phi \leqslant 0$ 时 ${U_{repv}} = 0$ 。于是距离和速度产生的总斥力势场函数 ${U_{rep}}$ 为：

$ {U_{rep}}(X) = \left\{ \begin{aligned} &{{U_{reps}}(X) \!+\! {U_{repv}}(X)}, {\rho (X,\!{X_o}) \leqslant{\rho _0} {\text{且}} \cos \phi > 0}{\text{，}}\!\\ &{{U_{reps}}(X)},{\rho (X,{X_o}) \leqslant {\rho _0} {\text{且}} \cos \phi \leqslant 0}{\text{，}}\!\\ &0, {\rho (X,{X_o}) > {\rho _0}}{\text{。}} \end{aligned} \right. $

(6)

2 考虑水下滑翔机的运动特点及约束 2.1 水下滑翔机的运动特点及约束

水下滑翔机在航行过程中，主要是沿锯齿形状轨迹做周期性运动。在理想情况下（没有海流以及水中障碍物），以一定滑翔角 $\theta $ 和航速 $V$ 做定常滑翔运动。本文研究水下滑翔机在单个周期下的避障问题，路径规划范围如图1所示。

路径起点的运动参数会影响之后路径的形状，所以将路径规划起点处的运动参数约束为水下滑翔机的速度大小及滑翔角方向。此外，与一些智能水下机器人相比，水下滑翔机机动性和回转性相对较差，主要体现为有最小回转半径 ${R_{\min }}$ 这一约束条件^[11]。

2.2 确定障碍物的影响半径

传统人工势场法中， ${\rho _0}$ 是不确定值，由经验而定。本文根据 ${R_{\min }}$ 来约束 ${\rho _0}$ 大小，从而把水下滑翔机的运动约束引入到人工势场法中。

为简化讨论，不考虑障碍物具体形状，表示为包含障碍物轮廓的外包圆，如图2所示。将水下滑翔机外包圆半径 ${r_u}$ 计在障碍物半径 $r$ 内，即 $r = {r_o} + {r_u}$ ， ${r_o}$ 为障碍物实际半径， ${r_u}$ 一般为1 m左右。根据水中障碍物性质， ${r_o}$ 大体包含2种情况：一种是无危险范围的障碍物， ${r_o}$ 指包含障碍物轮廓的外包圆半径；一种是像水雷这种有一定危险爆炸范围的障碍物， ${r_o}$ 指包含水雷及其危险爆炸范围在内的圆形范围半径，这时 ${r_o}$ 取值较大。

考虑水下滑翔机运动轨迹相切于障碍物边界的临界情况，水下滑翔机受到虚拟向心力的作用以恒定速率 $V$ 沿障碍物法线方向运动，假设虚拟向心力中心位置为 $O'$ 处。

将圆 $O$ 与圆 $O'$ 的坐标方程联立，得

$ \begin{split} &(4{R_{\min }}^2 + 4{\rho _0}^2){x^2} + [8{R_{\min }}r({\rho _0} - r) + 4{R_{\min }}{\left( {{\rho _0} - r} \right)^2}- \\ &8{R_{\min }}{\rho _0}^2]x + {[2r({\rho _0} - r) + {({\rho _0} - r)^2}]^2} = 0{\text{。}} \end{split} $

(7)

为使圆 $O$ 与圆 $O'$ 相切，令判别式为0，得 $\rho _0^2 + $ $ 2r{\rho _0} - 2{R_{\min }}r = 0$ 。

因为 ${\rho _0} \geqslant r,{R_{\min }} > 0,r > 0$ ，所以可求得临界状态下 ${\rho _{\min }}$ 为：

${\rho _{\min }} = \sqrt {{r^2} + 2Rr}{\text{，}} $

(8)

由于水下滑翔机的回转半径 $R \geqslant {R_{\min }}$ ，所以 ${\rho _0} \geqslant {\rho _{\min }}$ ，即 ${\rho _0} \geqslant \sqrt {{r^2} + 2Rr} $ 。

当障碍物半径足够大时，相对于水下滑翔机的最小回转半径而言，障碍物表面接近平面，此时易得 ${\rho _{\min }}$ 为：

${\rho _{\min }} = {R_{\min }} + r{\text{，}}$

(9)

对应的 ${\rho _0}$ 取值范围为 ${\rho _0} \geqslant {R_{\min }} + r$ 。

3 海流影响

水下滑翔机在海洋中航行的速度较小（实际应用的水下滑翔机航速 $V$ 一般为1～2 kn），对海流影响比较敏感，路径规划时要考虑海流的影响。

在海洋环境中，大部分海流的整体流动方向呈水平方向，而且在有限的区域和时间段内流动比较稳定^[12]。这里考虑水平方向上的定常海流力 ${\overrightarrow F _c}$ 对水下滑翔机运动的影响，水下滑翔机在势场中的总受力分析图如图3所示。

图中， $F$ 表示考虑海流力后水下滑翔机所受合力， $\varphi $ 表示水下滑翔机的航向角。定常海流力 ${\overrightarrow F _c}$ 与海流流速 ${\overrightarrow V _c}$ 成正比： ${\overrightarrow F _c} = \lambda {\overrightarrow V _c}$ ， $\lambda $ 为海流力系数。设在直角坐标系下，水下滑翔机、障碍物、目标点的位置坐标 $X、{X_o}、{X_g}$ 分别为 $(x,y)$ ， $({x_o},{y_o})$ ， $({x_g},{y_g})$ ，则可求得水下滑翔机与目标点、障碍物的中心连线与 $y$ 轴夹角 $\alpha $ ， $\beta $ 分别为： $\alpha = \arctan \dfrac{{{x_g} - x}}{{{y_g} - y}}$ ， $\beta = \arctan \dfrac{{x - {x_o}}}{{y - {y_o}}}$ 。由此，可得水下滑翔机的航向角 $\varphi $ ：

$ \varphi \! =\! \arctan \frac{{{F_{att,x}} \!\!+\!\! \displaystyle\sum\limits_{i \!=\! 1}^n {(F_{reps1,x}^i \!\!+\!\! F_{reps2,x}^i \!\!+\! F_{repv,x}^i)} \!\!+\!\! {F_c}}}{{{F_{att,y}} \!\!+\!\! \displaystyle\sum\limits_{i \!=\! 1}^n {(F_{reps1,y}^i \!\!+\! F_{reps2,y}^i \!\!+\!\! F_{repv,y}^i)} }}{\text{。}} $

(10)

其中， ${F_{att,x}} = {F_{att}} \cdot \sin \alpha $ ， $F_{reps1,x}^i = F_{reps1}^i \cdot \sin \beta $ ， $F_{reps1,x}^i = $ $ F_{reps2}^i \cdot \sin \alpha $ ， $F_{repv,x}^i \!=\! F_{repv}^i \cdot \sin \beta $ ， $F_{reps1,y}^i \!=\! F_{reps1}^i \cdot \cos \beta $ ， $F_{reps1,y}^i = F_{reps2}^i \cdot \cos \alpha $ ， $F_{repv,y}^i = F_{repv}^i \cdot \cos \beta $ 。

每运动一个步长后重新进行受力分析，并根据式（10）重新计算航向角 $\varphi $ 。根据航向角 $\varphi $ 以及水下滑翔机的速度 $V$ ，计算下一步的位置： ${x_{k + 1}} ={x_k} + $ $ V \cdot \delta t\cos \varphi $ ， ${y_{k + 1}} = {y_k} + V \cdot \delta t\sin \varphi $ ，直到水下滑翔机到达目标点的评判范围内。

4 仿真试验 4.1 无海流环境的路径规划

设定路径规划范围为水下滑翔机单个周期内的上升阶段，起始点坐标为（5 m，–145 m），目标点坐标为（150 m，0 m）。水下滑翔机速度 $V$ 、滑翔角 $\theta $ 、最小回转半径 ${R_{\min }}$ 之间存在一定约束关系^[15]，参考文献[13]中对HUST-2号水下滑翔机的参数优化研究，取航速 $V$ 为0.8 m/s、滑翔角 $\theta $ 为45°、最小回转半径 ${R_{\min }}$ 为10 m，以此约束路径起点的运动参数。

静态障碍物信息见表1，动态障碍物的参数及运动信息见表2。考虑最小回转半径约束，由式（8）计算障碍物1的影响半径为 ${\rho _{\min }}$ =6.63 m，实取 ${\rho _0}$ =6.63 m。由于水下滑翔机尺寸（机长约2 m）比障碍物2、障碍物3、障碍物4小很多，按式（9）取障碍物2、障碍物3的 ${\rho _0}$ 值为18 m。为验证改进人工势场法解决目标不可达问题的效果，将障碍物4的影响半径取为28 m，以包含目标点。为验证改进人工势场法对解决局部极值问题的效果，将障碍物1、障碍物2取在路径规划起点与目标点连线上，且障碍物1、障碍物2有交叉范围。

水下滑翔机进入、离开动态障碍物影响范围的过程如图4所示。最终路径规划结果如图5所示。

由图4和图5可知，采用改进的人工势场法可使水下滑翔机有效避开静态和动态障碍物，规划出安全的航行路径到达目标点。过程中没有出现局部极值以及目标不可达现象，说明对人工势场法的改进有效。且得到的路径较平滑，符合滑翔机运动特点。

4.2 定常海流对路径规划的影响

在同样的动态环境引入海流作用，设定海流方向水平向右，流速 ${V_c} = 0.2 \; {\rm{m/s}}$ ，作用区域为y轴–120~–90 m区域，仿真结果如图6所示。

通过比较图6中2条路径可知：在海流作用下，路径会发生较为明显的偏移，而且会影响离开海流区域后的航路走向，但势场法的原理决定最终仍会到达目标点。

为对比不同流速的常值海流对路径规划的影响，分别引入0.2 m/s，0.4 m/s，0.6 m/s三种常值海流，仿真结果如图7所示。可知，海流流速越大，对路径干扰越大。且在障碍物影响范围外，没有海流作用的路径方向稳定，而海流作用的路径则呈现不规则变向运动，这在实际航行中会增加水下滑翔机的能耗。

5 结　语

本文提出一种改进的人工势场方法，对海流作用下水下滑翔机在单个周期下的避障问题作了研究。改进的人工势场法综合考虑了传统人工势场法缺陷、动态障碍物、水下滑翔机运动约束以及海流的影响。结果显示采用的改进人工势场法能使水下滑翔机在无海流和有海流的情况下成功避开静态和动态障碍物。另外，本文只考虑了定常海流对路径规划的影响，对于复杂海流等情况对路径规划的影响，还需作更多研究。