0 引　言

随着无人潜航器向着大型化、智能化、大深度应用等方向发展，UUV的路径规划能力被提出了更高的要求。路径规划是在给定的环境中，根据起点、终点、障碍物等约束条件，通过算法寻找一条从起点到终点的最优移动路线的技术^[1]。UUV常见的路径规划方法可根据信息获取程度和侧重点分为全局路径规划和局部路径规划。

全局路径规划是基于已知的任务区域完整环境信息，考虑从起始点到目标点整个工作空间，通常在任务开始前预先规划好路径^[2]。常见的全局路径规划算法有1958年提出的Dijsktra算法、将广度优先搜索（BFS）和Dijsktra算法结合的A*算法、基于采样的RRT算法以及遗传算法和粒子群算法等^{[2 - 3]}。

但是，未知海域具有复杂的海洋生物环境，难以获得完整的先验环境信息，使得UUV航行过程中具有不确定性。因此UUV航行需具备一定的局部路径规划能力。局部路径规划是基于UUV自身携带的探测设备实时收集环境信息进行决策，侧重于当下局部环境的最优解，能够有效增强UUV的灵活性和适应性。局部路径规划包括1986年Khatib提出的人工势场法^[4]，以障碍物和目标点为中心，分别建立斥力势场和引力势场，引力和斥力的合力引导被控对象运动。DWA算法是由F.D.Proentzen和O.Khati提出的一种基于预测控制理论的局部路径规划算法。其核心思想是将局部路径规划问题转化为速度矢量空间上的约束优化问题。近年来，许多学者对DWA存在的不足进行了改进。严浙平等^[5]提出一种RRT-DWA融合算法，将RRT全局规划能力和DWA实时避障能力进行了有效的结合。Ballesteros等^[6]提出一种“仿生动态窗口法”BDWA，使算法尽可能模仿人类行为。Gao等^[7]引入安全因子提高了算法的安全性和效率。常路等^[8]提出一种改进的DWA算法，在原有评价函数的基础上加入预测轨迹与历史轨迹相似程度评价和预测轨迹与目标最短距离评价。蒋华扬等^[9]在传统DWA算法中引入速度采样策略和多角度优化评价函数以改进机器人运动路径。

文献[5 - 9]注重对评价函数和全局规划能力的改进，但由于水下航行阻力大，探测能力弱，通信能力有限，全局路径规划条件受限严重。过多增加评价函数会使UUV解算效率下降。同时，考虑到海域水流速度和方向的不确定性，UUV航行不可控因素较多，在航行全程不宜频繁机动。因此，上述改进算法不适用于复杂环境下UUV的航行。

梁震宇^[10]提出一种基于路径完成度和环境复杂度的DWA权重自适应方法，将评价函数权重与预测轨迹中危险障碍物数量和相对终点距离相关联，从而实现权重的动态调整。常绪成等^[11]将当前航向和目标航向偏差程度引入DWA算法并与评价函数权重相关联，实现权重的动态调整。上述2种自适应DWA算法均未考虑障碍物所在位置对UUV航向的影响。闫林伟等^[12]提出一种基于层次分析法的自适应DWA算法，通过设计一种自反馈机制，使权重因子能够根据环境变化自适应调整，但反馈机制计算较为复杂，且因素重要程度判断较为主观。

针对上述问题，本文提出一种将权重与UUV相对障碍物的距离和方向相关联的自适应DWA算法。算法综合考虑障碍物所在位置对UUV航行方向的影响，能够使UUV根据环境的变化，动态调整航向、避障和速度评价函数的权重，提高UUV鲁棒性，减少机动频次，优化UUV航行路径。

1 UUV水平面运动模型

为简化问题研究，设定UUV在一定深度上航行，不考虑UUV垂直方向运动，忽略UUV自身形状，建立UUV水平面运动模型。如图1所示，$Oxy$为大地坐标系（定系），$O'{x_0}{y_0}$为载体坐标系（动系）。UUV纵向速度$u$沿载体坐标系${x_0}$方向，横向速度$v$沿载体坐标系${y_0}$方向。

UUV水平面运动学模型：

$ \left\{\begin{gathered} \dot x = u\cos \psi - v\sin \psi，\\ \dot y = u\sin \psi + v\cos \psi，\\ \dot \psi = r 。\\ \end{gathered} \right.$

(1)

UUV水平面动力学模型：

$ \left\{\begin{gathered} \dot u = \frac{{{m_{22}}vr + {g_{11}}u + {\tau _u}}}{{{m_{11}}}}，\\ \dot v = \frac{{ - {m_{11}}ur + {g_{22}}v}}{{{m_{22}}}}，\\ \dot r = \frac{{ - ({m_{22}} - {m_{11}})uv + {g_{33}}r + {\tau _r}}}{{{m_{33}}}}。\\ \end{gathered}\right. $

(2)

$\left\{ \begin{gathered} {\tau _u} = - {m_{22}}vr + {g_{11}}u + {k_1}{m_1}，\\ {\tau _r} = {k_2}{m_{33}} + ({m_{22}} - {m_{11}})uv - {g_{33}}r ，\\ {k_1} = - {k_u}(u - {u_d}) + {{\dot u}_d} ，\\ {k_2} = - {k_\alpha }{z_2} - {z_1} + \dot \alpha，\\ {z_1} = \psi - {\psi _d}，\\ {z_2} = r - \alpha ，\\ \alpha = - {k_r}{z_1} + {{\dot \psi }_d}。\\ \end{gathered}\right. $

(3)

式中：$r$为转艏角速度；${m_{ii}}$和${g_{ii}}$为UUV水动力参数^[13]。本文UUV采用反步法控制策略。基于反步法设计纵向速度控制器和转艏控制器，经化简如公式(3)。${\tau _u}$、${\tau _r}$分别为UUV纵向速度推力和转艏力矩；$ {u_d} $为期望速度，${\psi _d}$为期望艏向角；$\psi $和$r$为UUV艏向角和角速度；${z_1}$为艏向角跟踪误差，${z_2}$为转艏角速度误差；$\alpha $为中间虚拟控制量；${k_u}$、${k_r}$和${k_\alpha }$为设计常数^{[14 - 15]}。

2 自适应DWA路径规划算法

DWA算法是一种基于预测控制理论的局部路径规划算法。该算法起源于机器人运动规划领域对于实时性和动态环境下路径规划需求。通过考虑机器人自身运动性能的限制和实时环境信息，在预测空间中动态确定一个合适的速度和角度来进行局部路径规划，从而有效避开障碍物，使机器人向目标点前进。

2.1 传统DWA算法 2.1.1 生成预测范围

1）定义纵向速度预测范围和艏向角预测范围。

$ \left\{ \begin{gathered} {u_{\exp }} = [{u_{\min }},{u_{\max }}] \\ {\psi _{\exp }} = [{\psi _{\min }},{\psi _{\max }}] 。\\ \end{gathered} \right. $

(4)

2）为保障UUV制动时不与障碍物发生碰撞，需根据UUV制动性能设置最大速度。

$ {u_{\max }} \leqslant \sqrt {2 \cdot dist(u,\psi ) \cdot {\alpha _{\max }}}。$

(5)

式中：${\alpha _{\max }}$为UUV最大制动加速度；$dist(u,\psi )$为预测轨迹末端采样点与最近障碍物之间的距离。

2.1.2 设置预测轨迹评价函数

预测轨迹评价函数是对预测窗口内生成的每一条运动轨迹是否安全和高效进行评价的函数。包括航向评价、避障评价和速度评价。

1）航向评价

UUV艏向角越接近目标航向（UUV朝向目标航行的方向），评分越高。

$ heading({u_{\exp }},{\psi _{\exp }}) = \sum\limits_i {(\text π - \left| {{\psi _{goal}} - {\psi _i}} \right|)}。$

(6)

式中：${\psi _{\rm goal}}$为UUV当前位置的目标航向；${\psi _i}$为UUV一条预测轨迹第$i$个采样点下的航向角；$heading ({u_{\exp }}, {\psi _{\exp }})$为一条预测轨迹所有采样点的航向评分总和。

2）避障评价

UUV所在位置与最近障碍物之间的距离越远，评分越高。若距离小于避免碰撞的安全距离，则舍弃该轨迹。

$ \begin{array}{l}dist({u}_{\mathrm{exp}},{\psi }_{\mathrm{exp}})={\displaystyle \sum _{i}\mathrm{min}({\Vert \stackrel{\rightharpoonup }{{S}_{i}{O}_{j}}\Vert }_{2})}，\\ {\Vert \stackrel{\rightharpoonup }{{S}_{i}{O}_{j}}\Vert }_{2}=\sqrt{{({x}_{i}-{x}_{oj})}^{2}+{({y}_{i}-{y}_{oj})}^{2}}。\end{array} $

(7)

式中：$({x_i},{y_i})$为UUV一条预测轨迹中第$i$个采样点的位置；$ ({x_{oj}},{y_{oj}}) $为第$j$个障碍物（距离UUV最近的障碍物）的位置；$ dist({u_{\exp }},{\psi _{\exp }}) $为一条预测轨迹所有采样点与最近障碍物距离的评分总和。

3）速度评价

UUV纵向速度越快，得分越高。

$ Vel({u_{\exp }},{\psi _{\exp }}) = \sum\limits_i {\left| {{u_i}} \right|}。$

(8)

式中：${u_i}$为UUV一条预测轨迹第$i$个采样点下的纵向速度；$Vel({u_{\exp }},{\psi _{\exp }})$为一条预测轨迹所有采样点的纵向速度评分总和。

2.1.3 归一化处理

由于上述3种评价函数具有不同的量纲，为防止某一参数过大导致预测轨迹评分无法反映数据真实性，将3种评价函数进行归一化处理。

$ \left\{\begin{gathered} Ev{l_{heading}} = \frac{{heading{{({u_{\exp }},{\psi _{\exp }})}_i}}}{{\sum\limits_n {heading({u_{\exp }},{\psi _{\exp }})} }}，\\ Ev{l_{dist}} = \frac{{dist{{({u_{\exp }},{\psi _{\exp }})}_i}}}{{\sum\limits_n {dist({u_{\exp }},{\psi _{\exp }})} }} ，\\ Ev{l_{Vel}} = \frac{{Vel{{({u_{\exp }},{\psi _{\exp }})}_i}}}{{\sum\limits_n {Vel({u_{\exp }},{\psi _{\exp }})} }}。\\ \end{gathered} \right.$

(9)

定义加权系数$ (\alpha ,\beta ,\gamma ) $，可得预测轨迹最终得分：

$ F({u_{\exp }},{\psi _{\exp }}) = \alpha \cdot Ev{l_{heading}} + \beta \cdot Ev{l_{dist}} + \gamma \cdot Ev{l_{Vel}}。$

(10)

2.2 自适应DWA路径规划算法

传统DWA算法评价函数加权系数固定不变，无论在何种环境下，UUV航向评价、避障评价和速度评价对最终评价的影响始终不变，因此鲁棒性差，容易忽略局部最优轨迹。如图2(a)，若UUV注重避障评价既避障权重高于其他2种评价函数，则当UUV顺利过障后但未拉开距离时，会出现UUV继续朝远离障碍物而非最佳航向航行的情况，导致总路程增加。理想路径为图2(b)。

当UUV过于注重航向评价或速度评价，则可能出现避障不及时导致UUV频繁制动的情况，若制动距离小于安全距离，UUV会与障碍物发生碰撞；若制动距离大于安全距离，虽然UUV能够安全制动，但会增加UUV机动频次，影响全局航行效率。

针对上述问题，本文提出一种自适应DWA算法，在传统DWA算法基础上引入自适应加权系数。通过该系数，可将航向权重、避障权重和速度权重与UUV和障碍物的相对距离和相对方向关联，使UUV能够根据航行环境的变化及时调整3种评价函数的权重，优化UUV航行路径。

2.2.1 定义自适应评价函数权重系数

当UUV处于多障碍物环境时，只需避免与距离最近的障碍物发生碰撞，随着UUV航行位置的不断改变，最近障碍物也将不断变化，从而使UUV在多障碍物情况下实现避碰。影响自适应评价函数权重有2个因素，一是UUV与最近障碍物之间的距离，二是UUV与最近障碍物之间的夹角：

1）自适应航向评价权重系数

$ {\alpha _z} = {H_1}^{\left| {SO} \right| - {k_1}R} \cdot {H_2}^{\left| {\theta - {\psi _{\rm goal}}} \right|}。$

(11)

式中：$R$为碰撞判定距离；$SO$为最近障碍物与UUV之间的距离；$\theta $为UUV相对于最近障碍物方位角；${\psi _{goal}}$为当前状态UUV的目标航向；${k_1}$、${H_1}$和${H_2}$为经验系数。如图3所示。

当UUV与最近障碍物之间的距离增大，目标航向与UUV相对最近障碍物的方位角增大时，航向评价权重增加，UUV越倾向于朝目标前进。

2）自适应避障评价权重系数

$ {\beta _z} = {H_3}^{{k_2}R - \left| {SO} \right|} \cdot {H_4}^{ - \left| {\theta - {\psi _{\rm goal}}} \right|}。$

(12)

当UUV与最近障碍物之间的距离减小，目标航向与障碍物之间的夹角减小时，避障评价权重增加，UUV越倾向于躲避障碍物。${k_2}$、$ {H_3} $和${H_4}$为经验系数。

3）自适应速度评价权重系数

$ {\gamma _z} = {H_5}^{\left| {SO} \right| - R}。$

(13)

UUV与最近障碍物之间的距离越大，UUV越倾向于提高纵向速度。${H_5}$为经验系数。

其中${H_1}$、${H_2}$、${H_3}$、${H_4}$、${H_5}$、${k_1}$和${k_2}$的值受航行器性能影响。

最终得分计算公式为：

$ \begin{gathered} F({u_{\exp }}, {\psi _{\exp }}) = \alpha \cdot {\alpha _z} \cdot Ev{l_{heading}} +\beta \cdot {\beta _z} \cdot Ev{l_{dist}} + \gamma \cdot {\gamma _z} \cdot Ev{l_{Vel}}。\end{gathered} $

(14)

2.2.2 自适应DWA算法流程

自适应DWA算法流程如图4所示。

1）UUV根据当前运动状态信息，生成纵向速度和艏向角预测范围。

2）UUV根据获取的实时环境信息，计算周围障碍物与自身的距离、当前位置与目标点之间的距离，以获得当下环境态势。

3）根据障碍物和目标点位置信息，实时调节各评价函数权重。

当UUV处于较为安全环境下，即与最近障碍物之间的相对距离较大时，UUV将提高航向评价和速度评价的权重，降低避障评价的权重，使UUV以最快的速度向目标点前进，提高整体航向行效率。

当UUV处于较危险的环境，即与最近障碍物之间的相对距离较小，且周围障碍物较多时，UUV将降低航向评价和速度评价的权重，提高避障评价权重，使UUV更加注重躲避障碍物，保证UUV的航行安全性。

当UUV避障后，此时障碍物对目标航向影响较小，但与UUV的距离还处于相对较近的阶段，则算法会根据障碍物所在方位对UUV目标航向的影响大小合理降低避障评价函数的权重，提高航向的权重，使UUV能够及时调整航向角，优化航行路径。

4）根据各评价函数权重，对预测范围内的所有轨迹评分。

5）根据预测范围内的得分最高轨迹，执行相应的预测纵向速度和艏向角。

6）判断UUV是否抵达目标点，若未抵达，则返回第一步；若抵达目标点，则停止。

3 仿真验证

为验证自适应DWA算法在UUV局部路径规划上的性能，将自适应DWA算法与传统DWA算法和传统人工势场法进行对比，并对3种算法仿真结果进行分析。本文选择的研究对象为文献[12]中的欠驱动UUV“WL-Ⅱ”。根据研究内容，构建一个51×51的仿真地图，UUV起始点为原点，目标点坐标设置为[50, 50]。在UUV起始点与目标点之间设置11个固定障碍物。设定障碍物碰撞判定半径为2 m。本次仿真采用处理器为Intel(R)Core(TM)i7-10875HCPU @2.30 GHz、内存为16 GB的64位WIN10操作系统，仿真平台为Matlab R2023B。

3.1 UUV路径规划算法仿真

本次仿真设置目标点位于起始点45°方向上，UUV初始航向设为30°。图5为UUV分别在3种算法下的路径规划运动轨迹：自适应DWA为实线，传统DWA为长虚线，人工势场法为短虚线。灰色区域为固定障碍物影响范围。考虑到真实情况下的UUV经济航速，将UUV在3种算法下的最大纵向速度设定为2 m/s。

表1为传统DWA算法的初始参数，自适应DWA算法在此基础上对评价函数权重进行了改进。

表2为3种算法下UUV航行的总路程、机动次数和总步数。其中，当UUV转艏角速度≥0.3 rad/s时，判定UUV正在进行机动。假设UUV每一次决策解算时间小于采样时间，则总步数可等价于UUV航行总时间。

对比图5和表2中的3种算法下UUV的航行总路程、航行期间的机动次数和航行的总步数可以得出，UUV采用传统人工势场法航行时路径最为曲折，航行时间最久，机动次数最多，且在坐标为[15, 15]的障碍物附近有大机动动作。传统DWA算法航行路径比较保守，全程执行尽可能远离障碍物的决策，在坐标为[30, 35]的障碍物附近有较大的机动动作，该算法下的总路程相较于传统人工势场法减小了约4.52%、机动次数减少了40.83%，航行时间缩短了10.61%。UUV采用自适应DWA算法航行时，总路程相较于传统人工势场法和传统DWA算法分别缩短了约16.21%和12.25%，机动次数相较于传统人工势场法和传统DWA算法分别减少了43.2%和4%，航行时间相较于传统人工势场法和传统DWA算法分别减少了20.78%和11.38%。

结合实际航行任务，由于水下环境较为复杂，不确定因素多，过多的机动不利于UUV的安全航行。通过对比可知，UUV采用自适应DWA算法所选取的路径最为安全经济，效率最高，机动次数最少，航行时间最短。

3.2 UUV在不同算法下航行数据对比

对比UUV分别采用人工势场法、传统DWA算法和自适应DWA算法进行路径规划的航行数据。如图6～图8所示，包括UUV纵向速度、横向速度和每一步的航向角。横坐标为UUV仿真步数。由图6(c)可知，当UUV采用自适应DWA算法航行100步时，纵向速度有明显下降，结合图5和图7(c)、图8(c)可知，此时UUV进入复杂障碍物环境，自适应权重系数进行了实时调整，使UUV更加注重避障，并开始调整航向，降低速度权重，从而保证UUV航行的安全性。

由图8可知，UUV采用人工势场法时，整个航行过程中航向角振荡严重，航向在不断改变，符合图5中的航行路径；自适应DWA算法航向角在45°附近振荡，且振荡次数和幅度明显小于传统DWA算法。因此自适应DWA算法在避障的同时，能够保证UUV基本处于最佳航向状态。

在图5中，当UUV采用传统DWA算法经过最后一个障碍物时，向远离障碍物方向进行了机动，此时各评价函数权重为(0.1, 0.2, 0.1)，UUV更注重避障。而采用自适应DWA算法时，UUV并未改变航行方向，而是保持朝向目标的艏向角航行，此时各评价函数权重为(0.3073, 0.2383, 0.0970)。结合图8(b)和图8(c)的UUV航向角，UUV此时更侧重于目标航向。这是由于最后一个障碍并不会对UUV向目标航行产生影响，因此自适应DWA算法对评价函数权重系数进行了实时调整，提高了航向权重，保证了UUV的最优航向。

4 结　语

本文针对传统DWA算法应用于UUV路径规划存在机动频次高、航行轨迹差的问题，提出了一种自适应DWA算法。首先搭建UUV水平面运动模型，然后在传统DWA算法中加入设计的自适应评价函数权重系数。自适应评价函数权重主要受UUV相对最近障碍物的距离以及最近障碍物相对于UUV的方位角与目标航向差值的影响。仿真结果表明，自适应DWA算法相较于传统DWA算法在航行总路程、机动频次和航行时间上均有改善。改进的自适应DWA算法能够根据UUV当下所在环境，实时改变航向、避障和速度评价函数权重，有效缩短航行总路程，减少机动频次，在保证UUV安全的同时，优化UUV航行路径。本文主要对UUV在固定障碍物环境下的路径规划进行了研究分析，未考虑动态障碍物，下一步将对DWA算法在动态障碍物环境下的UUV路径规划展开研究。