Dynamic path-planning method of UUV in an offshore complex environment
-
摘要: 为解决近海环境下水下无人航行器（unmanned underwater vehicle，UUV）的动态路径规划问题，本文提出一种结合全局和局部动态路径规划的算法。首先，本文提出一种基于自适应目标引导的快速拓展随机树算法，以增加随机树生长的方向性，并通过转向和重选策略减少无效拓展加快算法的收敛速度。接着，获得全局路径之后使用自适应子节点选取策略获取动态窗口法的子目标点，将复杂的全局动态任务规划分解为多个简单的动态路劲规划，从而防止动态窗口法陷入局部极小值。最后，通过UUV出港任务仿真实验验证了算法的有效性和实用性。Abstract: This paper proposes an algorithm that combines global and local dynamic path-planning methods to solve the dynamic path-planning problem for unmanned underwater vehicles (UUVs) in near-shore environments. First, a fast-expanding random tree algorithm based on adaptive target guidance is proposed to increase the directionality of random tree growth. The algorithm then achieves rapid convergence using turning and reselection strategies to reduce ineffective expansion. Further, an adaptive sub-node selection strategy is used after obtaining the global path to acquire sub-target points for the dynamic window approach. This approach decomposes the complex global dynamic task planning into multiple simple dynamic path-planning tasks, preventing the dynamic window approach from falling into local minima. Finally, the effectiveness and practicality of the algorithm are verified using simulation experiments of UUV departure tasks.
-
水下无人航行器（unmanned underwater vehicle，UUV）在水下搜救、地形勘探、探测资源等领域有着重要的地位[1]，而自主路径规划是UUV的核心技术之一，其决定着UUV执行任务的效率和安全性[2-3]。无人智能系统的路径规划分为全局路径规划和局部动态规划两部分，前者基于不完整地图快速搜寻一条可达任务目标点的可行路径[4-9]；后者则是利用智能系统的传感器对周围环境的感知及时避开全局路径上的未知障碍物[10-11]。然而，近海环境下大量礁石、人工建筑物以及航行的渔船等因素致使UUV的航行情况变得异常复杂，对路径规划的要求也相应提高[12]。当UUV停泊港内需执行港外近海任务时，若其具备近海复杂环境下的动态路径规划能力，在抵达任务区域过程中则不需要借助大型渔船及起吊装置，将极大的节省人力物力，对完善UUV功能有着重要的工程意义。
针对动态环境的路径规划问题，Chowdhury等[13]提出了一种PRM-A*算法，该算法将PRM算法和A*算法联合规划路径，解决了路径不平滑以及动态障碍物的避碰问题。赵旭等[14]提出了一种改进的A*算法，此算法将A*算法的搜索区域改为UUV可达的扇形区域，并且在路径拓展阶段以运动学为约束，实现了未知环境的路径规划问题。He等[15]提出了一种改进的蚁群算法，引入虚拟可视化概念克服了蚁群只能在相邻网格行走的限制，使用非线性惩罚机制避免路径出现大的转角。李杨等[16]在遇到动态障碍物时将人工势场法与速度障碍法相结合，根据障碍物的大小以及运动信息使UUV合理避障。
RRT(rapid-exploration random tree）算法由Lavall等[17]提出，被广泛用于解决路径规划问题，但是RRT算法存在拓展盲目性、路径冗余度高、路径不平滑等问题，主要的改进方向有增加算法的导向性和提高的路径可行性[18-20]。刘成菊等[21]提出的RRT改进算法在拓展阶段以一定概率选择目标点，采样的其他点则增加引力分量使其偏向目标点，同时使用路径缓存策略以及动态扩展策略解决动态避碰问题。尹高扬等[22]在节点拓展阶段添加动力学约束、航迹距离约以达到增加路径的可行性以及缩短距离的作用。上述提出的算法在静态环境下效果较好，但是存在计算开销大、不适用于近海环境存在动态障碍物的路径规划问题。
为了解决近海复杂环境下的动态路径规划问题，本文提出一种改进RRT和动态窗口法融合的动态避碰算法AGT-RRT(adaptive guidance target RRT)。首先对RRT算法进行改进以加快全局路径的规划速度，然后在得到的全局路径基础之上，使用自适应子节点选取策略获取DWA（dynamic window approach）算法的子目标点，将难以实现的全局动态任务规划分解成多个简单的动态规划任务，实现动态避碰功能的同时增加DWA算法的导向性。最后，通过仿真验证了算法的有效性。
1. 建立UUV模型
近海环境下由于水位较浅，为了简化系统模型，本文只讨论UUV的水平方向运动，即UUV的纵荡、横荡、艏摇3个自由度。
1.1 建立UUV运动学模型
$$ \begin{gathered} \dot \psi = r \\ \dot x = u\cos \psi - v\sin \psi \\ \dot y = u\sin \psi + v\cos \psi \\ \end{gathered} $$ (1) 式中：
$\psi $ 为UUV的艏向角，$ r、u、v $ 分别为UUV的角速度、横向线速度、纵向线速度，$ x、y $ 分别为UUV在水平位置坐标。1.2 建立UUV动力学模型
$$ \begin{gathered} \dot u = {m_2}vr{m_1}^{ - 1} - {d_1}u{m_1}^{ - 1} + {\tau _1}{m_1}^{ - 1} \\ \dot v = - {m_1}ur{m_2}^{ - 1} - {d_2}v{m_2}^{ - 1} \\ \dot r = ({m_1} - {m_2})uv{m_3}^{ - 1} - {d_3}r{m_2}^{ - 1} + {\tau _3}{m_3}^{ - 1} \\ \end{gathered} $$ (2) 式中：
$$ \begin{array}{l}{m}_{1}=m-{X}_{\dot{u}}\text{，}{m}_{2}=m-{Y}_{\dot{v}}\text{，}\\ {m}_{3}={J}_{{\textit{z}}}-{N}_{\dot{r}}\text{，}{d}_{1}={X}_{u}+{X}_{u\left|u\right|}\left|u\right|\text{，}\\ {d}_{2}={Y}_{v}+{Y}_{v\left|v\right|}\left|v\right|\text{，}{d}_{3}={N}_{r}+{N}_{r\left|r\right|}\left|r\right|\end{array} $$ (3) 其中：
$m$ 为UUV的自身质量，${X_{\dot u}}、 {Y_{\dot v}}、 {N_{\dot r}}$ 为UUV的附加质量，$ {\tau }_{1}、{\tau }_{3} $ 为UUV推进器提供的推力以及提供的转向力矩，${J_{\textit{z}}}$ 为转动惯量。1.3 建立UUV声呐模型
引入文献[23]中提到的Solstice声呐模型，该声呐为多孔径测扫声呐，当障碍物处于声呐的200 m范围内可以被其捕捉到，被捕捉到的障碍物和声呐之间的距离关系如下：
$$ {(x - {x_0})^2} + {(y - {y_0})^2} \leqslant {R^2} $$ (4) 式中：
$ {x}_{0}、{y}_{0} $ 分别为UUV在水平位置坐标，$ x、y $ 分别为障碍物在水平位置坐标，$R$ 为声呐有效工作范围。1.4 建立UUV自身物理约束模型
UUV在运动过程中由于自身性能影响，存在最大速度、转向角的限制：
$$ {V_s} = \left\{ {(v,r)|u \in [{v_{\min }},{v_{\max }}],r \in [{r_{\min }},{r_{\max }}]} \right\} $$ (5) 式中：
${V_s}$ 为速度对$(v,r)$ 的约束范围，${v_{\min }}、{v_{\max }}$ 为UUV速度$v$ 的最大最小值，${r_{\min }}、{r_{\max }}$ 为r的最大最小值。2. 全局路径规划算改进
2.1 自适应目标引导策略
本小节介绍为增加随机树生长的方向性设计的自适应目标引导策略。在任务空间
$V$ 中随机获得一个采样点，首先计算采样点与终点之间的距离$d$ ，然后将距离$d$ 乘以比例因子$p$ 得到一个距离$d'$ ，之后将采样点${x_{{\text{rand}}}}$ 朝着${x_{{\text{rand}}}}$ 与${x_{{\text{goal}}}}$ 的矢量方向移动$d'$ ，得到一个新的采样点${x'_{{\text{rand}}}}$ ，其计算公式如下：$$ {x'_{{\text{rand}}}} = {x_{{\text{rand}}}} + p \times d \times \frac{{({x_{{\text{goal}}}} - {x_{{\text{rand}}}})}}{{||{x_{{\text{goal}}}} - {x_{{\text{rand}}}}||}} $$ (6) 这样此算法不仅使拓展的方向偏向
${x_{{\text{goal}}}}$ ，而且树空间$T$ 中更加靠近${x_{{\text{goal}}}}$ 的节点被优先被选为拓展点，加快了算法收敛的速度。比例因子如果设置的过大，在复杂任务环境容易陷入局部极小值（见图1（a）），此时比例因子为0.3，即使多次迭代也不能摆脱。因此本文采用自适应比例因子。在算法开始迭代之前赋予比例因子一个初始值，然后根据每次迭代成功或是失败修改比例因子，以加快收敛速度，防止陷入局部极小值，其计算公式如下：
图 1 算法改进前后对比Fig. 1 Comparison chart before and after improvement
下载:
全尺寸图片
$$ p=\left\{\begin{array}{l}{p}_{0}+\alpha \cdot {n}_{\text{success}}\text{,}\quad 成功\\ {p}_{0}-\beta \cdot {n}_{\text{fail }}\text{,}\quad 失败\text{ }\end{array}\right. $$ (7) 迭代成功：
$$ \begin{gathered} {n_{{\text{success}}}} = {n_{{\text{success}}}} + 1 \\ {n_{{\text{fail}}}} = \chi \cdot {n_{{\text{fail}}}} \\ \end{gathered} $$ (8) 迭代失败：
$$ \begin{gathered} {n_{{\text{success}}}} = \delta \cdot {n_{{\text{success}}}} \\ {n_{{\text{fail}}}} = {n_{{\text{fail}}}} + 1 \\ \end{gathered} $$ (9) 式中：
$ {n}_{\text{success}}、{n}_{\text{fail}} $ 分别为成功迭代次数以及失败迭代次数，$ \alpha 、\beta 、\chi 、\delta $ 分别为权重，${p_0}$ 为比例因子$p$ 的初始值。改进后拓展图如图1(b)所示。2.2 避碰策略
观察算法规划过程发现采样点远大于随机树节点数量，这是由于在拓展阶段发生多次碰撞，为了进一步加快收敛速度，本文采用避碰策略。
当算法发生碰撞时不会直接放弃此条路径而是以
${x'_{{\text{rand}}}}$ 朝向$ {x'_{{\text{near}}}} $ 的矢量方向为轴线向周围辐射出一个扇形区域，在此扇形区域以一定间隔生成多条路径，如果其中有无碰路径，则按照此方向相应的进行拓展。在采用转向避碰策略之后如果仍然不能成功拓展则表示此节点大概率陷入局部极小值，为了逃离局部极小值本文采用重选节点避碰策略。在拓展的阶段放弃
${x'_{{\text{rand}}}}$ 作为拓展的随机节点，选用之前生成的${x_{{\text{rand}}}}$ 作为拓展的节点，则使拓展的方向偏离目标点，可以快速逃离局部极小值。图2中给出了避碰策略的有效性，灰色、洋红色、蓝色曲线分别表示正常拓展、转向避碰策略后拓展、重选节点避碰策略后的路径。从图中可以发现，随机树在发生碰撞之后可以根据节点所处的不同情况选取不同的策略，在随机树朝向目标点生长时如果陷入局部极小值，可以使用设计的策略快速逃离，达到了设计的目的。
图 2 避障策略仿真Fig. 2 Simulation diagram of obstacle avoidance strategy下载:
全尺寸图片
3. 局部避碰算法设计
本文采用DWA算法实现局部路径规划，但是DWA算法仅能在窗口大小的范围内规划路径，极易陷入局部最小值。为了解决这个问题，本文采用自适应子节点选取策略和重规划策略。
3.1 DWA模型设计
1) DWA规划UUV下一时刻状态方程如下：
$$ {{\boldsymbol{x}}_{t + 1}} = {\boldsymbol{f}} \times {{\boldsymbol{x}}_t} + {\boldsymbol{A}} \times {\boldsymbol{V}} $$ (10) 式中：
${\boldsymbol{x}} = {[x{\text{ }}y{\text{ }}\psi {\text{ }}v{\text{ }}r]^{\rm{T}}}$ ，${\boldsymbol{V}}$ 为速度对$\left[ {v\; r} \right]$ 。$$ {\boldsymbol{f}} = \left[ \begin{gathered} 1{\text{ 0 0 0 0}} \\ {\text{0 1 0 0 0}} \\ {\text{0 0 1 0 0}} \\ {\text{0 0 0 0 0}} \\ {\text{0 0 0 0 0}} \\ \end{gathered} \right]\text{，} {\boldsymbol{A}} = \left[ \begin{array}{c} \Delta t \times \cos \psi\quad {\rm{ }}0\\ \Delta t \times \sin \psi\quad {\rm{ }}0\\ \;\;\;\;\;{\rm{ }}0\qquad\quad\Delta t\\ \;\;\;\;{\rm{ }}1\qquad\quad\;0\\ \;\;\;\;{\rm{ }}0\qquad\quad\;1 \end{array} \right]$$ 2)制动距离计算，当制动距离
${D_{{\text{dis}}}}$ 大于UUV与障碍物的距离则舍弃相应的路径，公式如下：$$ {D_{{\text{dis}}}} = {v_t} \cdot t - {a_{\max }}{t^2}/2 $$ (11) 其中
${a_{\max }}$ 为UUV的最大加速度。3)评价函数计算，评价函数主要由艏向角得分、安全距离得分以及速度得分3部分构成。为了防止不同评价体系单差异带来的异常，对3种评价体系归一化处理，具体公式如下：
$$ {E_i} = \frac{{{E_i}}}{{\displaystyle\sum\limits_{i = 1}^n {{E_i}} }} $$ (12) 其中
${E_i}$ 为每条轨迹的参数。最后可得评价函数如下：$$ e = a \times {E_h} + b \times {E_d} + c \times {E_v} $$ (13) 其中：
$e$ 为评价函数得分，$ a、b、c $ 分别为艏向角的分系数、安全距离的分系数、速度得分系数。3.2 自适应子节点选取策略
在未知路径弯曲度时，固定距离子节点易导致DWA算法陷入局部最小值，因此本文提出自适应子节点选取策略。首先，初始长度
${d_{{\text{init}}}}$ ，在全局路径上选取距离当前位置一个初始长度的子节点${x_{{\text{child}}}}$ ，之后计算当前位置和子节点在全局路径上的距离${d_1}$ 与当前位置到子节点的直线距离${d_2}$ 的比值$k$ ，如果比值大于设置的阈值$\kappa $ 表示全局规划的路径较为曲折，则缩短子节点的选取距离，直至小于设置的阈值；如果比值小于设置的阈值$\kappa $ 则表示路径较为平滑，则增加子节点的选取距离，直至大于设置的阈值。3.3 重规划策略
全局路径规划是基于不完整地图信息规划的路径，在实际环境中可能会出现新的障碍物，当障碍物较小使用DWA即可避过障碍物，但是当障碍物过大，DWA算法可能会陷入局部极小值导致UUV无法通行。因此本文针对这种情况采用重规划策略，当DWA算法陷入局部极小值时，则返回主程序中，把雷达扫描到的障碍物加入到地图之中，然后以当前位置为起点重新规划全局路径，之后再按照之前设计的方案进行规划路径。
4. 仿真与分析
4.1 AGT-RRT算法仿真验证
AGT-RRT算法在运行时首先初始化随机树空间，初始化自适应比例因子
${p_0}$ 为0.32，初始化计数器$ {n}_{\text{success}}、{n}_{\text{fail}} $ 为0，转向避碰策略角度$\alpha $ 为30°。在任务空间获得采样点${x_{{\text{rand}}}}$ ，执行目标引导策略获得采样点${x'_{{\text{rand}}}}$ 。若${x'_{{\text{rand}}}}$ 拓展成功则将其加入随机树空间；失败则执行转向避碰策略获得采样点${x_{{\text{new}}}}$ ，若其成功拓展则将其加入随机树空间；失败则执行重选节点策略，采用${x_{{\text{rand}}}}$ 作为拓展的节点，若其成功拓展则加入随机树空间，失败则返回第一步在任务空间重新获得采样点。本文设计了map1和map2验证AGT-RRT算法的可行性。将经典RRT算法、文献[11]中提出的AWA_RRT重算法、RRT-Connect算法与本文提出的算法作对比。控制每种算法的参数一致，步长均为30，AWA-RRT算法权重的初始值为0.5，AGT-RRT算法比例因子初始值为0.32，map1和map2的大小均为1000
$ \times $ 1000，起止点为（50，50），目标点为（950，950）。由于每次规划所需时间过少，为避免偶然性和便于对比分析，表1、2为1000次规划的总时间成本，路径成本以及节点数量取1000次规划的平均值。表 1 map1 4种算法仿真数据Table 1 Map1 simulation data of four algorithms算法 时间成本/s 路径成本 节点数量 RRT 29.67 1717.61 640 AWA-RRT 20.02 1479.48 581 RRT-Connect 6.25 1656.03 123 AGT-RRT 1.75 1406.99 47 表 2 map2 4种算法实验结果Table 2 Map1 simulation data of four algorithms算法 时间成本/s 路径成本 节点数量 RRT 33.77 1894.52 764 AWA-RRT 20.99 1587.58 555 RRT-Connect 11.94 1918.77 198 AGT-RRT 7.06 1814.37 122 图3给出了4种算法在两种环境下的仿真情况。map1环境较为简单不存在局部极小值。本文算法可以使用转向避障策略顺利躲避障碍物。每次成功的拓展都会提高自适应目标引导策略比例因子的值，使得下一个采样点更加接近目标点，加快了算法的收敛速度。对比观察map1中路径生成过程发现AGT-RRT算法拓展方向具有较好的引导性，基本没有冗余节点，达到了设计的目的。
图 3 4种算法在不同环境的仿真图Fig. 3 Simulation diagrams of four algorithms in different environments下载:
全尺寸图片
在存在局部极小值的map2中，对比算法增加许多无效拓展，而本文的算法可使用重选节点避碰策略绕过局部极小值。使用重选节点避碰策略表示环境较为复杂，则会降低自适应目标引导策略比例因子的值，以增强探索能力。map2中的AGT-RRT算法在应对局部极小值，产生的无效拓展较少，证明了设计的有效性。
对比表1和表2中数据发现，AGT-RRT算法在map1中时间成本相较于RRT、AWA-RRT、RRT-Connect算法分别减少94.1%、91.3%、72%，路径成本也分别优化了18.1%、4.9%、15%。在环境更为复杂的map2中，时间成本分别减少79%、66.4%、40.8%，路径成本相较于RRT算法和RRT-Connect算法分别减少4.2%、5.4%，但是相较于AWA-RRT算法路径成本增加了14.3%，节点数量减少78%。结合map2仿真环境和图3中路径观察可知，这是由于本文算法在遇到障碍物优先执行转向避障策略，加快算法的收敛速度所致。时间成本的减少，增加的路径成本在可接受的范围。观察图4和图5箱线图中的数据可以发现，所提出的算法有较少的离散点，这表明其鲁棒性优于其他的3种算法，在不同的环境中有更强的适应能力，达到了设计的目的。
图 4 map1 4种算法时间成本箱线图Fig. 4 Map1 four algorithms time cost boxplot下载:
全尺寸图片
图 5 map2 4种算法时间成本箱线图Fig. 5 Map2 four algorithms time cost boxplot下载:
全尺寸图片
使用AGT-RRT算法生成的路径距离障碍物太近，转向次数较多，不满足实际路径规划的需求。本文将障碍物膨胀化保证路径远离障碍物，同时去除冗余节点减少路径的转向次数，使生成的路径更为平滑，处理后的路径如图6所示。
图 6 去除冗余节点路径（蓝色）Fig. 6 Remove redundant node path (blue)下载:
全尺寸图片
4.2 动态路径规划仿真分析
为证明本文所设计算法的有效性，使用文献[24]中所提出算法作对比仿真实验。动态路径规划仿真结果如图7所示，大小为500 m×500 m。固定变量，两种算法均采用图7中蓝色折线作为全局路径。二者算法参数见表3，其中d为对比算法特有的偏离函数权重，UUV性能参数见表4。路径的初始位置为（50，50），终点为（470，470），初始方向角为0。
图 7 动态路径规划仿真结果Fig. 7 Simulation result of dynamic path planning下载:
全尺寸图片
表 3 DWA算法评价函数权重Table 3 Weights of DWA algorithms evaluation function算法 $a$ $b$ $c$ $d$ 对比算法 0.4 0.3 0.2 0.2 0.1 0.3 0.4 0.1 本文算法 0.3 0.4 0.2 表 4 UUV性能参数表Table 4 UUV performance parameters最大角速度/(rad/s) 最小角速度/(rad/s) 最大角加速度/(rad/s2) 最大速度/(m/s) 最小速度/(m/s) 最大加速度/(m/s2) 1.5 0 0.5 3 0 0.6 图7中洋红色曲线为动态规划的路径，其中图7(a)和图7(b)为map3和map4的仿真结果，左图为对比算法，右图为本文所设计算法。本文算法首先执行自适应子节点选取策略，根据路径的曲折程度选取DWA算法的子目标点。自适应子节点选取策略将完整的动态规划任务分解为多个子任务分别进行规划，从而降低全局路径的曲折度对动态规划的影响。
为增加仿真结果的可靠性，分别对二者进行10次仿真实验，取平均值作为结果，具体数值见表5。从图7（a）中可以看出二者均可以较好的参考全局路径执行动态路径规划任务，通过计算发现，二者平局路径长度相差只有1%，但是本文所提出算法规划的路径速度提升了10.4%，具有较大的速度提升，加快了任务的执行速度。
表 5 地图3仿真数据Table 5 Map 3 simulation data算法 路径成本/m 平均速度/(m/s) 对比算法 618.1 1.29 本文算法 624.3 1.44 在map4中，本文所设计的算法借助自适应子节点选取策略可以较好地利用全局路径进行动态路径规划，而对比算法丢失了全局路径信息，超出任务边界无法抵达目标点。map4 比map3中全局路径转折角大，任务目标点与航行器当前艏向角相差过大，而UUV为大惯性体转向速度较慢，导致对比算法脱离全局路径，造成最优评价路径更加偏向任务目标点，使得规划路径超出任务范围，无法完成近海复杂环境中的UUV出港任务，证明了本文算法的优越性。
4.3 出港任务仿真分析
仿真实验任务为航行器自主动态规划出港路径，以抵达港外任务区域。地图实际大小为400 m×400 m，像素大小为1200×1200，比例为1∶3。
在仿真实验中，本文采用Bluefin-9型UUV参数[25]，见表4。在使用AGT-RRT规划全局路径时以UUV最大航速（3 m/s）为一个步长，DWA规划路径时取最大航行速度为3 m/s。首先，AGT-RRT算法规划出一条全局路径，虽然在狭小空间存在一些无效拓展，但是使用避障策略可轻易摆脱局部极小值，达到设计目的，之后去除初步规划全局路径的冗余节点。获得全局路径之后执行动态路径规划任务，使用自适应子节点选取策略在全局路径上不断选取子节点作为DWA算法的目标点，将复杂的路径规划任务分解成多个简单的动态路径规划任务。之后DWA算法按照顺序依次以生成的子目标点作为每阶段的目标点执行动态路径规划任务，最后获得出一条符合运动学约束的路径。
任务执行过程中UUV速度每秒变化曲线以及角速度每秒变化曲线如图8和图9所示，可以发现，速度出现的几次较大变化是由于每当DWA算法抵达目标点就会逐渐降低自身速度，保证UUV可以停止在目标点，最大速度变化率为0.6 m/s2，没有超过最大加速度；角速度出现较大变化是由于路径出现较大转角，最大角速度变化率为0.5 rad/s2，没有超过最大角加速度的范围。总路径长度为421 m，平均速度为1.5 m/s，可以较快完成出港任务。速度以及角速度的变化在切换子目标点以及在较为急促转弯时变化较为剧烈，其余规划时间变化均较为平缓，符合UUV的运动特性，达到设计的要求。
图 8 速度变化率曲线Fig. 8 Velocity change rate curve下载:
全尺寸图片
图 9 角速度变化率曲线Fig. 9 Angular velocity change rate curve下载:
全尺寸图片
5. 结束语
为了提高近海环境特别是港口环境中UUV自主路径规划的安全性、快速性和动态避障能力，本文结合全局和局部路径规划设计了一种算法。首先，分析RRT算法存在的不足提出一种AGT-RRT算法。针对RRT的拓展盲目问题提出了自适应目标引导策略，同时引入转向机制提高RRT算法的采样成功率，进一步提高全局路径规划的效率，最后对比仿真实验验证了算法的有效性。针对DWA算法难以在较为复杂的环境实现动态规划任务，采用自适应子节点选取策略获取DWA算法的子目标点，将难以实现的全局动态任务规划分解为多个简单的动态规划任务，从而实现复杂环境下的动态路径规划，仿真实验验证了算法的有效性和实用性。然而本文算法只能利用当前位置及下一个子目标点的信息，无法做到全局信息的利用，导致有些地方路径较长，速度变化不平滑，对这些问题的处理将是今后的研究工作。
-
图 1 算法改进前后对比
Fig. 1 Comparison chart before and after improvement
下载:
全尺寸图片
图 2 避障策略仿真
Fig. 2 Simulation diagram of obstacle avoidance strategy
下载:
全尺寸图片
图 3 4种算法在不同环境的仿真图
Fig. 3 Simulation diagrams of four algorithms in different environments
下载:
全尺寸图片
图 4 map1 4种算法时间成本箱线图
Fig. 4 Map1 four algorithms time cost boxplot
下载:
全尺寸图片
图 5 map2 4种算法时间成本箱线图
Fig. 5 Map2 four algorithms time cost boxplot
下载:
全尺寸图片
图 6 去除冗余节点路径（蓝色）
Fig. 6 Remove redundant node path (blue)
下载:
全尺寸图片
图 7 动态路径规划仿真结果
Fig. 7 Simulation result of dynamic path planning
下载:
全尺寸图片
图 8 速度变化率曲线
Fig. 8 Velocity change rate curve
下载:
全尺寸图片
图 9 角速度变化率曲线
Fig. 9 Angular velocity change rate curve
下载:
全尺寸图片
表 1 map1 4种算法仿真数据
Table 1 Map1 simulation data of four algorithms
算法 时间成本/s 路径成本 节点数量 RRT 29.67 1717.61 640 AWA-RRT 20.02 1479.48 581 RRT-Connect 6.25 1656.03 123 AGT-RRT 1.75 1406.99 47 表 2 map2 4种算法实验结果
Table 2 Map1 simulation data of four algorithms
算法 时间成本/s 路径成本 节点数量 RRT 33.77 1894.52 764 AWA-RRT 20.99 1587.58 555 RRT-Connect 11.94 1918.77 198 AGT-RRT 7.06 1814.37 122 表 3 DWA算法评价函数权重
Table 3 Weights of DWA algorithms evaluation function
算法 $a$ $b$ $c$ $d$ 对比算法 0.4 0.3 0.2 0.2 0.1 0.3 0.4 0.1 本文算法 0.3 0.4 0.2 表 4 UUV性能参数表
Table 4 UUV performance parameters
最大角速度/(rad/s) 最小角速度/(rad/s) 最大角加速度/(rad/s2) 最大速度/(m/s) 最小速度/(m/s) 最大加速度/(m/s2) 1.5 0 0.5 3 0 0.6 表 5 地图3仿真数据
Table 5 Map 3 simulation data
算法 路径成本/m 平均速度/(m/s) 对比算法 618.1 1.29 本文算法 624.3 1.44 -
[1] 严浙平, 王璐. UUV水下定位方法的研究现状与进展[J]. 哈尔滨工程大学学报, 2017, 38(7): 989–1000. YAN Zheping, WANG Lu. Research status and progress of UUV underwater localization[J]. Journal of Harbin Engineering University, 2017, 38(7): 989–1000. [2] LIANG Qingwei, LUO Mingyang, WANG Yida, et al. Multi-attacks effectiveness evaluation of UUV based on wake guidance[J]. Ocean engineering, 2022, 266: 112654. doi: 10.1016/j.oceaneng.2022.112654 [3] YAN Zheping, XU Da, CHEN Tao, et al. Modeling, strategy and control of UUV for autonomous underwater docking recovery to moving platform[C]//2017 36th Chinese Control Conference. Dalian: IEEE, 2017: 4807−4812. [4] Lesire, Charles. “An Iterative A* Algorithm for Planning of Airport Ground Movements. ” ECAI. Vol. 2010. 2010 [5] SONG Zhihua, ZHANG Han, ZHANG Xiaojie, et al. Unmanned aerial vehicle coverage path planning algorithm based on cellular automata[C]//2019 15th International Conference on Computational Intelligence and Security. Macao: IEEE, 2020: 123−126. [6] GUO Hong, HAN Dandan, ZHANG Hongguo. Using PSO to improve ant colony optimization algorithm[C]//International Conference on Software Intelligence Technologies and Applications & International Conference on Frontiers of Internet of Things 2014. Hsinchu: Institution of Engineering and Technology, 2014: 272−276. [7] AL-KHAYYAT A T A, IBRAHIM A. Robot pathplanning by image segmentation using the fuzzy C-means algoritm and KNN algorithm[C]//2020 4th International Symposium on Multidisciplinary Studies and Innovative Technologies. Istanbul: IEEE, 2020: 1−4. [8] WANG M, CUI P, CHEN J. Sectional polynomial smoothing method for autonomous driving reference trajectory[J]. Modern electronics technique, 2021, 44(15): 154−158. [9] 赖永林, 林茂松, 梁艳阳. 基于三次非均匀B样条曲线的机器人轨迹规划算法研究[J]. 科学技术与工程, 2013, 13(35): 10511–10517. LAI Yonglin, LIN Maosong, LIANG Yanyang. The research on robot trajectory planning algorithm based on cubic non-uniform B-spline curve[J]. Science technology and engineering, 2013, 13(35): 10511–10517. [10] LIU Chaowei, WANG Hongjian, GU Yingmin, et al. UUV path planning method based on QPSO[C]//Global Oceans 2020. Biloxi: IEEE, 2021: 1−5. [11] 刘祥, 叶晓明, 王泉斌, 等. 无人水面艇局部路径规划算法研究综述[J]. 中国舰船研究, 2021, 16(S1): 1–10. LIU Xiang, YE Xiaoming, WANG Quanbin, et al. Review on the research of local path planning algorithms for unmanned surface vehicles[J]. Chinese journal of ship research, 2021, 16(S1): 1–10. [12] 薛双飞. 基于改进A*算法的近海船舶路径规划[D]. 武汉: 武汉理工大学, 2018. XUE Shuangfei. Route planning of offshore ships based on improved A* algorithm[D]. Wuhan: Wuhan University of Technology, 2018. [13] CHOWDHURY M I, SCHWARTZ D G. UUV on-board path replanning using PRM-A[C]//Global Oceans 2020. Biloxi: IEEE, 2021: 1−8. [14] 赵旭, 杨进, 陈琛, 等. 未知环境中水下无人航行器的路径规划方法[J]. 舰船科学技术, 2022, 44(2): 117–120. doi: 10.3404/j.issn.1672-7649.2022.02.021 ZHAO Xu, YANG Jin, CHEN Chen, et al. Path planning method for unmanned undersea vehicle in unknown area[J]. Ship science and technology, 2022, 44(2): 117–120. doi: 10.3404/j.issn.1672-7649.2022.02.021 [15] HE Juyi, WANG Hongjian, LIU Chaowei, et al. UUV path planning for collision avoidance based on ant colony algorithm[C]//2020 39th Chinese Control Conference. Shenyang: IEEE, 2020: 5528−5533. [16] 李杨, 杜度, 于双宁, 等. 一种用于水下无人航行器的局部避碰路径规划方法研究[J]. 数字海洋与水下攻防, 2021, 4(6): 520–524. LI Yang, DU Du, YU Shuangning, et al. Research on the method of local collision avoidance path planning for unmanned underwater vehicle[J]. Digital ocean & underwater warfare, 2021, 4(6): 520–524. [17] LAVALLE S M, KUFFNER J J J. Randomized kinodynamic planning[J]. The international journal of robotics research, 2001, 20(5): 378–400. doi: 10.1177/02783640122067453 [18] ZHOU Qiang, LIU Guangcai. UAV path planning based on the combination of A-star algorithm and RRT-star algorithm[C]//2022 IEEE International Conference on Unmanned Systems. Guangzhou: IEEE, 2022: 146−151. [19] QURESHI A H, AYAZ Y. Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in complex cluttered environments[J]. Robotics and autonomous systems, 2015, 68: 1–11. doi: 10.1016/j.robot.2015.02.007 [20] YAN Zheping, HAO Bing, LIU Yibo, et al. DD-RRT path planning and guidance in heading-vector field for a UUV recovery[C]//OCEANS 2016 MTS/IEEE Monterey. Monterey: IEEE, 2016: 1−5. [21] 刘成菊, 韩俊强, 安康. 基于改进RRT算法的RoboCup机器人动态路径规划[J]. 机器人, 2017, 39(1): 8–15. LIU Chengju, HAN Junqiang, AN Kang. Dynamic path planning based on an improved RRT algorithm for RoboCup robot[J]. Robot, 2017, 39(1): 8–15. [22] 尹高扬, 周绍磊, 吴青坡. 基于改进RRT算法的无人机航迹规划[J]. 电子学报, 2017, 45(7): 1764–1769. YIN Gaoyang, ZHOU Shaolei, WU Qingpo. An improved RRT algorithm for UAV path planning[J]. Acta electronica sinica, 2017, 45(7): 1764–1769. [23] 王硕, 段蓉凯, 廖与禾. 机器人路径规划中快速扩展随机树算法的改进研究[J]. 西安交通大学学报, 2022, 56(7): 1–8. WANG Shuo, DUAN Rongkai, LIAO Yuhe. Research on improvement of rapidly exploring random tree algorithm in robot path planning[J]. Journal of Xi’an Jiaotong University, 2022, 56(7): 1–8. [24] 张瑞, 周丽, 刘正洋. 融合RRT*与DWA算法的移动机器人动态路径规划[J/OL]. 系统仿真学报: 1−12. [2023−04−15]. http://10.16182/j.issn1004731x.joss.22-1543. ZHAGN Rui, ZHOU Li, LIU Zhengyang. Dynamic path planning for mobile robot based on RRT* an dynamic window approach[J/OL]. Journal of system simulation: 1-12. (2023−03−24)[2023−04−15]. http://10.16182/j.issn1004731x.joss.22-1543. [25] HAJI M N, TRAN J, NORHEIM J, et al. Design and testing of auv docking modules for a renewably powered offshore auv servicing platform[C]//International Conference on Offshore Mechanics and Arctic Engineering. New York: American Society of Mechanical Engineers, 2020, 84386: V06BT06A024.
