0 引　言

水下航行器（Underwater Vehicle）是一种能在水中实现自主运行或者遥控操作的设备，在海洋科学研究、资源勘探、环境监测以及军事任务等众多领域都有广泛应用，按照其工作方式以及功能，水下航行器可划分成多种不同类型，主要包括载人水下航行器（HOV）、遥控水下航行器（ROV）和自主水下航行器（AUV）。

目前，针对路径规划已有很多成熟的算法,常见的算法有Dijkstra算法、A*算法、遗传算法、人工势场法、RRT算法等。LaValle所提出的快速探索随机树（Rapidly-exploring Random Tree，RRT）算法，凭借其独特的随机采样规划策略，显著地减少了对状态空间预处理的需求，实现了高效的搜索性能。在搜索进程里，该算法全面考量了机器人本身有的多种约束条件，例如非完整性约束、动力学约束以及运动学约束等，如此便可较好地应对复杂环境下的运动规划挑战。近些年来，由于其性能较为出色，RRT算法在水下航行器运动规划领域获得了广泛认可并得以应用，成为该领域的研究热点。

大量学者对于RRT算法进行研究与改进，李金良等^[1]将目标偏向策略（Goal Biasing）和RRT算法相结合，通过在随机采样过程中增加对目标点方向的采样倾向，使算法更快地向目标点扩展。这种改进显著提高了搜索效率和收敛速度，但是这种方法对于目标点的依赖性较强，如果目标点选择不合适或者目标点与起始点之间存在较大障碍物，可能会导致路径搜索效率下降；段云涛等^[2]采用双向扩展RRT（Bidirectional RRT），同时从起始点和目标点出发，在2个方向上生成2棵树，并通过不断扩展这2棵树来搜索可行路径。这种改进方法可以快速收敛，有效避免局部最优，但是该算法对环境要求高，复杂环境下可能会增加计算复杂度，并且初始路径的质量可能不佳，通常较长且曲折，需要后续的优化处理，如使用曲线拟合或简化算法来优化路径；张帅等^[3]采用动态步长调整, 根据环境的不同情况动态地调整树的扩展步长。在空旷区域使用较大的步长以加快搜索速度，在狭窄或障碍物密集区域使用较小的步长以提高搜索的精度和安全性。这种改进方法可以根据环境特征灵活调整步长，从而提高搜索效率和路径质量，但这种方法计算成本高，参数设置复杂；李丁等^[4]采用RRT*算法, RRT算法在路径搜索过程中进行了优化，包括动态重新连线、最优路径搜索和代价优化等。这种改进方法显著提高了RRT算法在全局路径规划方面的准确性和效率，但是存在计算较复杂、实现复杂以及内存消耗大的问题^[5]。

对于路径规划的研究大多都是在二维空间进行的，但是对于水下航行器而言，需要在三维空间进行路径规划，这对算法提出了更高的要求，在三维空间用RRT算法路径规划随机性更大，生成最优路径的难度更高^[6]。与上述方法不同，本文是在RRT算法生成三维路径的基础上，提出了一种新的路径优化方法。针对传统RRT算法路径的不平滑性、随机性以及对于航行器来说执行难度过大问题，将RRT生成的路径点进行蒙特卡洛算法运算。在原始RRT路径中随机选择一个节点，并在其周围随机扰动，生成一个新的路径, 计算新路径的代价（通常是路径长度），并与当前最佳路径的代价进行比较。如果新路径的代价更小且没有碰撞，则接受该路径作为当前最佳路径。设计了基于视线（Line-Of-Sight，LOS）算法的航向修正模块，以增强航行器在复杂水下环境中的导航精度与稳定性^[7]。

1 环境地图建立 1.1 环境地图

水下航行器路径规划的基础是构建环境地图，它界定了航行器运动的空间范围以及障碍物的分布状况，要保障航行器在复杂水下环境里安全且高效地执行任务，建立环境地图就得精准地描绘出空间的边界以及障碍物的形状、位置等信息，地图被构建成三维空间。边界界定了航行器运动的空间范围，能防止航行器超出预定区域。球体障碍物依靠其球心坐标和半径给予描述，立方体障碍物借助定义最小角与最大角的坐标来描述。

边界：$ Q=\left\{{Q}_{k}\right\} $，其中每个$ {Q}_{k}=\left({x}_{k},{y}_{k},{z}_{k}\right) $，$ {x}_{k} $为$ x $轴的范围，$ {y}_{k} $为$ y $轴的范围，$ {z}_{k} $为$ z $轴的范围。球体障碍物：$ C=\{{C}_{j}\} $，其中每个$ {C}_{j}=({p}_{j},{q}_{j}) $，$ {p}_{j}= ({x}_{j},{y}_{j}, {z}_{j}) $为球体的中心坐标，$ {q}_{j}=r $为球体的半径。立方体障碍物：$ L=\left\{{L}_{i}\right\} $，其中每个$ {L}_{i}=\left({p}_{i},{q}_{i}\right) $，$ {q}_{i}=(x\_ \min,y\_ \min, z\_ \min) $为立方体的最小角坐标，$ {p}_{i}=(x\_ \max,y\_ \max, z\_ \max) $为立方体的最大角坐标。

1.2 碰撞检测

碰撞检测作为路径规划里相当关键的一步，其作用在于判定航行器于某个特定点或者某一条路径之上，是不是会和障碍物产生碰撞的情况。碰撞检测主要可以划分成两种类型，分别是点碰撞检测以及线段碰撞检测。点碰撞检测通过给定一个点$ A=\left(x,y,z\right) $检测其是否在任意障碍物内。对于球形障碍物（$ r $为球形障碍物的半径）:

$ {\left(x-{x}_{j}\right)}^{2}+{\left(y-{y}_{j}\right)}^{2}+{\left(z-{z}_{j}\right)}^{2}\leqslant {r}^{2} 。$

(1)

则发生碰撞。对于立方体障碍物：

$ \begin{split}&{x}_{\min}\leqslant x\leqslant {x}_{\max},\\ &{y}_{\min}\leqslant y\leqslant {y}_{\max}，\\ &{z}_{\min}\leqslant z\leqslant {z}_{\max}。\end{split} $

(2)

离散化为单位长度为1的小步长$ {\Delta }A $，并逐步检查每个中间点是否发生碰撞。

在本文算法对路径进行优化后的基础上，将优化后的路径作为水下航行器的目标路径，通过LOS导航算法初步实现水下航行器的3D避障。由于水下航行器的形状复杂不规则，在实际航行中容易出现不可预估的意外，本文在碰撞检测中引入了安全距离f的概念。碰撞条件不再仅仅是几何上的干涉，而是要求路径点与障碍物之间必须保持至少f的距离，进而在仿真时把水下航行器简化成一个球体，以航行器的尾部为球体的球心，航行器总长为球体的半径，安全距离$ d $的设置需要大于或等于球体的半径。

更新后球形障碍物的碰撞检测：

$ b=\sqrt{{\left(x-{x}_{j}\right)}^{2}+{\left(y-{y}_{j}\right)}^{2}+{\left(z-{z}_{j}\right)}^{2}}。$

(3)

点$ A $到球心$ {p}_{j} $的欧几里得距离，如果满足$ b＜r+f $则发生碰撞。

计算点$ A $到立方体每个坐标轴的最小距离：

$ \left\{\begin{aligned}&{b}_{x}=\max\left(0,x-{x}_{\max},{x}_{\min}-x\right)，\\ &{b}_{y}=\max\left(0,y-{y}_{\max},{y}_{\min}-y\right)，\\ &{b}_{z}=\max\left(0,z-{z}_{\max},{z}_{\min}-z\right)。\end{aligned}\right. $

(4)

距离为$ {b}^{'}=\sqrt{{{{b}_{x}}}^{2}+{{{b}_{y}}}^{2}+{{{b}_{z}}}^{2}} $。若满足$ {b}^{'}＜d $，则发生碰撞。

通过引入安全距离，本文的碰撞检测机制能够更好地模拟实际航行中的安全需求，确保航行器在复杂环境中能够有效避开障碍物，保障航行安全。

2 路径规划算法 2.1 传统RRT算法

RRT（Rapidly-explanding Random Tree）是一种基于随机采样的路径规划算法，尤其适用于高维空间和复杂障碍环境中的运动规划^[8]。该算法无需全局环境建模，通过随机采样和逐步构建探索树，高效搜索从起点到目标点的可行路径，具有较强的实时性和适应性^[9]。

相比传统方法，RRT不依赖环境的完整先验模型，而是通过随机采样与局部扩展动态探索空间，因而更适用于水下航行器等动态复杂环境。

RRT的基本步骤包括初始化、随机采样、最近节点查找、扩展树、碰撞检测、迭代和路径生成。尽管生成的路径不一定最优，但可通过多次运行或结合优化算法进一步提升路径质量^{[10 - 13]}。

初始化，建立一个包含起点的树结构，并将起点作为树的根节点。同时，设置相关参数，如步长、最大迭代次数等。

起点表示为 $ {q}_{{\mathrm{start}}}\in C $，其中$ C $是配置空间，是一个三维空间。

目标区域通常定义为以$ {q}_{{\mathrm{goal}}} $为中心的球域（或超球域），其数学形式为：

$ {q}_{{\mathrm{goal}}}=\left\{q\in C\big|\ \parallel q-{q}_{{\mathrm{goal}}}\parallel \leqslant \epsilon \right\} 。$

(5)

式中：$ \epsilon $为目标区域的半径。在三维空间中，目标区域球形为：

$ {\left(x-{x}_{g}\right)}^{2}+{\left(y-{y}_{g}\right)}^{2}+{\left(z-{z}_{g}\right)}^{2}\leqslant {\epsilon }^{2} 。$

(6)

树$ T $初始化为仅包含起点 $ {q}_{{\mathrm{start}}} $ 的节点集合，边集合为空：

$ T=\left(V,E\right),V=\left\{{q}_{\text{start}}\right\},E=\varnothing 。$

(7)

式中：$ V $为节点集合；$ E $为边集合。

随机采样生成探索方向，其核心是通过随机性引导树向未探索区域扩展。

使用均匀随机采样，即在配置空间$ C $内以均匀概率生成随机点。

配置空间为三维空间，随机采样公式为：

$ {q}_{\text{rand}}={\left[{x}_{\text{rand}},{y}_{\text{rand}},{z}_{\text{rand}}\right]}^{\mathrm{{T}}} 。$

(8)

其中：每个维度独立采样：

$ \left\{\begin{aligned} &{x}_{\text{rand}}\sim \boldsymbol{U}\left({x}_{\min },{x}_{\max }\right)，\\& {y}_{\text{rand}}\sim \boldsymbol{U}\left({y}_{\min },{y}_{\max }\right)，\\ &{z}_{\text{rand}}\sim \boldsymbol{U}\left({z}_{\min },{z}_{\max }\right) 。\end{aligned} \right.$

(9)

式中：$ \boldsymbol{U}\left(a,b\right) $表示区间$ [a,b]\text{上的均匀分布} $。

在环境空间中随机采样一个点，这个点可能位于自由空间，也可能位于障碍物中。如果采样点落在障碍物内，则需要重新进行随机采样。

工作空间范围为：$ x\in \left[0{,}100\right]，y\in \left[0{,}100\right]，z\in [0{,}100] $，因此：

$ {{q}_{\text{rand}}= \left({\mathrm{random}}\left(0{,}100\right),{\mathrm{random}}\left(0{,}100\right),{\mathrm{random}}\left(0{,}100\right)\right) 。}$

(10)

寻找最近节点是通过距离度量确定树中离随机采样点$ {{q}}_{\text{rand}} $最近的节点$ {{q}}_{\text{near}}\mathrm{} $。

树$ T $的节点集合$ V $中，遍历所有的节点，并计算距离，找到与$ {q}_{\text{rand}} $距离最小的节点$ {q}_{\text{near}}\colon $

$ {{q}}_{\text{near}}{}=\mathrm{arg}{}_{{q}\in {V}}^{\min }\left({q},{{q}}_{\text{rand}}\mathrm{}\right) 。$

(11)

从$ {q}_{{\mathrm{near}}} $向$ {q}_{{\mathrm{rand}}} $进行扩展新的节点$ {q}_{{\mathrm{new}}} $，扩展步长为固定值$ \sigma $：

$ {q}_{\mathrm{{new}}}={q}_{\mathrm{{near}}}+\sigma \frac{{q}_{\mathrm{{rand}}}-{q}_{\mathrm{{new}}}}{\left|\left|{q}_{\mathrm{{rand}}}-{q}_{{\mathrm{new}}}\right|\right|} 。$

(12)

从最近节点向采样点方向按预设步长移动一小步，生成新节点。若该节点位于自由空间（无碰撞），则将其加入树中，并与最近节点连接。

算法通过重复采样、扩展和碰撞检测持续迭代，直至到达目标区域或达到最大迭代次数。每次迭代均尝试扩展树结构并判断是否抵达目标。

路径生成通过回溯实现：从目标节点开始，沿父节点逐级回溯至起点，形成一条连续路径。该路径不一定最优，但能在复杂环境中快速找到可行解。

2.2 蒙特卡洛方法

蒙特卡洛方法作为一种依靠概率统计理论的数值计算方式，常用于解决难以通过确定性算法处理的优化问题。其核心思想是通过引入随机性来探索解空间，并通过大量重复采样以概率形式逼近最优解^[14]。在此种状况下，本文给出了一种以蒙特卡洛随机扰动策略为基础的路径优化办法，依靠对路径里的节点实施随机扰动，生成新的路径，并且凭借评估路径的代价来挑选更为优良的路径，以此提升路径的平滑程度以及全局最优程度，达成了对规划路径的渐进式优化。该优化方法在一个迭代框架中执行，每次迭代包含以下4个步骤^[15]：

步骤1　随机扰动路径。在当前路径内随机挑选一个非端点节点，并于其周围进行随机扰动，改变节点的空间坐标，生成一条新的路径。

步骤2　评估路径代价。计算新路径的代价，并把它和当前最佳路径的代价做比较。

步骤3　接受更优路径。若新路径的代价更小且不存在碰撞，那就接受该路径作为当前最佳路径。

步骤4　路径平滑。在完成上述蒙特卡洛优化迭代后，对最终得到的最优路径进行一次平滑后处理。

在依据RRT算法所生成的初始路径当中，随机挑选出一个节点，接着在该节点的周围进行随机扰动（扰动为均匀分布，设置固定值），生成一条全新的路径，扰动的具体方式是，于节点的邻域范围之内随机生成一个全新的位置，用以替换原来的节点。具体公式为：

$ \begin{matrix}\left(\begin{matrix}x_{i}^{\mathrm{'}}\\ y_{i}^{\mathrm{'}}\\ z_{i}^{\mathrm{'}}\\ \end{matrix}\right)=\left(\begin{matrix}{x}_{i}\\ {y}_{i}\\ {z}_{i}\\ \end{matrix}\right)+\left(\begin{matrix}\rho \cdot \sin \left(\theta \right)\cdot \cos \left(\varphi \right)\\ \rho \cdot \sin \left(\theta \right)\cdot \sin \left(\varphi \right)\\ \rho \cdot \cos \left(\theta \right)\\ \end{matrix}\right) 。\\ \end{matrix} $

(13)

式中：$ \theta $为极角（${\Delta }p $与z轴的夹角，取值范围$ [0,\text{π} ] $）；$ \varphi $为方位角（$ {\Delta }p $在xy平面上投影与x轴的夹角，取值范围$ [0{,}2\text{π} ] $）；$ \rho $为$ \mathit{\Delta }p $的模长，用于控制扰动的幅度。通过调整扰动半径，可以控制路径优化的精细程度。较大的扰动半径可以在全局范围内探索更优的路径，而较小的扰动半径则可以在局部范围内进行精细调整。

生成新的路径后，需要计算其代价（通常是路径长度），并与当前最佳路径的代价进行比较。路径长度的计算公式为：

$ \mathrm{cost}=\sum \limits_{i=1}^{n}\text{distance}\left({p}_{i},{p}_{i-1}\right) 。$

(14)

式中：$ {p}_{i} $为路径中的第$ i $个节点；$ {\mathrm{distance}}({p}_{i},{p}_{i-1}) $为节点$ {p}_{i} $和$ {p}_{i-1} $之间的欧几里得距离。通过比较新路径与当前最佳路径的代价，决定是否接受新路径。

若新路径的代价相对较小并且未与障碍物产生碰撞，那么便接受此路径作为当下的最佳路径，这历经多次迭代，逐渐朝着全局最优解靠近，在得到当前最佳路径之后，针对路径展开平滑处理，以此来降低路径的曲折程度，提升路径的可执行性能。

路径平滑通过一种结合线性插值与随机扰动的策略来实现。对于路径中的每个节点$ {p}_{i} $，计算其与前后节点的中点：

$ {p}_{{{i}^{'}}}=\frac{{p}_{i-1}+{p}_{i+1}}{2}。$

(15)

并引入随机扰动${\Delta }{p}' $：

$ \begin{matrix}\left(\begin{matrix}x_{i}^{\mathrm{''}}\\ y_{i}^{\mathrm{''}}\\ z_{i}^{\mathrm{''}}\\ \end{matrix}\right)=\left(\begin{matrix}x_{i}^{\mathrm{'}}\\ y_{i}^{\mathrm{'}}\\ z_{i}^{\mathrm{'}}\\ \end{matrix}\right)+\left(\begin{matrix}\rho \cdot \sin \left(\theta \right)\cdot \cos \left(\varphi \right)\\ \rho \cdot \sin \left(\theta \right)\cdot \sin \left(\varphi \right)\\ \rho \cdot \cos \left(\theta \right)\\ \end{matrix}\right)。\end{matrix} $

(16)

式中：$ \theta $为极角（$ {\Delta }{p}' $与z轴的夹角，取值范围$ [0,\text{π} ] $）；$ \varphi $为方位角（${\Delta }{p}^{'} $在xy平面上投影与x轴的夹角，取值范围$ [0{,}2\text{π} ] $）；$ \rho $为$ \mathit{\Delta }{p}^{'} $的模长。

在平滑过程中，确保新生成的路径点不与障碍物发生碰撞。通过引入安全距离，确保路径点与障碍物之间的距离不小于安全距离，从而保障航行器的安全性。

蒙特卡洛方法的流程图如图1所示，此流程图完整展示出路径优化的全过程，该方法借助随机扰动、路径评估、路径接受以及平滑处理等步骤，可于复杂的三维环境里寻得一条相对较优的路径^[16]。

蒙特卡洛方法通过随机扰动和路径平滑来优化路径，能够在复杂的环境中找到一个较优的路径。虽然该方法依赖于随机采样，但通过多次迭代，可以逐步逼近最优解。

2.3 LOS控制算法

LOS控制算法作为一种经典策略在路径跟踪控制领域有着广泛应用，其核心要点是借助几何关系引领载体依照预定路径行进，此算法会对载体当前位置和目标路径间的虚拟视线加以计算，同时把视线与载体前进方向的角度偏差考虑进来，动态调整载体的航向以及速度，达成对预定路径的精准跟踪。LOS控制算法有结构简单、计算效率较高的特点，并且针对路径形状以及环境扰动拥有较强的鲁棒性^[17]。

LOS控制算法主要有以下步骤：路径定义与航点生成、计算最近点、确定前视点、计算期望速度方向、计算期望姿态角以及计算期望姿态角。

根据RRT+蒙特卡洛算法生成的路径，并将其分解为一系列航点。将路径离散化为一系列航点$ {P}_{K}\left( k=1{,}2,3{,}4\ldots \right) $，确定每个航段的起点$ {P}_{K} $和终点$ {P}_{K+1} $。

找到载体当前位置$ Q $到当前航段的最短距离点$ {P}_{\mathrm{{closest}}} $。

设当前航段起点为$ {P}_{K} $，终点为$ {P}_{K+1} $，路径向量为：

$ {\overrightarrow{R}=P}_{K+1}-{P}_{K}，$

(17)

载体位置为Q，载体到航段最近点$ {P}_{{\mathrm{closest}}} $：

$ t=\max\left(0,\min\left(1,\frac{\left(Q-{P}_{k}\right)\cdot \mathbf{R}}{\parallel \mathbf{R}{\parallel }^{2}}\right)\right)，$

(18)

$ {P}_{\mathrm{{closest}}}={P}_{K}+t\cdot R 。$

(19)

在路径上确定一个前视点$ {P}_{\text{los}} $，作为载体运动方向的参考点。

沿路径方向从$ {P}_{\text{closest}} $前移前视距离Δ：

$ {P}_{\text{los}}={P}_{\text{closest}}+\Delta \cdot \frac{\mathbf{R}}{\parallel \mathbf{R}\parallel }。$

(20)

前视距离是LOS控制算法中最重要的参数之一，它决定了载体在路径跟踪过程中参考点的位置。如果前视距离较大，载体运动更加平滑，适合长距离路径跟踪，对路径曲率变化不敏感。但是跟踪精度降低，可能导致载体偏离路径较远。前视距离较小，跟踪精度提高，载体更贴近路径。但是载体运动可能变得不稳定，对路径曲率变化敏感，容易产生振荡。

如果$ {P}_{\text{los}} $超出当前航段终点$ {P}_{K+1} $，则将其截断至$ {P}_{K+1} $，并切换到下一航段。

计算载体到前视点的方向向量，作为期望速度方向。

载体到前视点的方向向量：

$ {\boldsymbol{v}}_{\text{desired}}={P}_{\text{los}}-Q ，$

(21)

$ {\boldsymbol{u}}_{\text{desired}}=\frac{{\boldsymbol{v}}_{\text{desired}}}{{\| \boldsymbol{v}}_{\text{desired}}\| }。$

(22)

姿态角计算：

偏航角：

$ {\psi }_{\text{desired}}=\arctan 2\left({u}_{\text{desired},y},{u}_{\text{desired},x}\right)，$

(23)

俯仰角：

$ {\theta }_{\text{desired}}=\arcsin \left(-{u}_{\text{desired},z}\right)。$

(24)

当载体接近航段终点时，自动切换至下一航段。

检查载体和当前航段终点的距离：

$ d=\parallel Q-{P}_{k+1}\parallel。$

(25)

如果$ d $小于预设阈值，则切换到下一航段$ {P}_{k+1} $和$ {P}_{k+2} $。重复上述步骤，直到载体完成所有航段的跟踪。

3 仿真结果分析

本文所有实验均基于Pycharm平台，运行在Windows系统之上。在仿真中，本文以（0 m, 0 m, 0 m）为坐标原点建立三维坐标系，设置三维空间的边界x、y、z均为（0 m, 100 m）。水下航行器的初始位置和目标位置分别设置为（1 m, 1 m, 1 m）和（95 m, 95 m, 95 m），RRT最大迭代次数设置为10000次，固定步长设置为1 m，蒙特卡洛迭代次数为5000次。为了验证改进的算法在不同障碍环境下的实用性和可用性，分别在3种不同的障碍环境下对传统RRT算法、RRT*算法以及经过蒙特卡洛方法优化后的算法进行了路径规划仿真。障碍物以网格形式呈现，以便更直观地观察算法的避障效果。在此条件下，试验次数为 50次，仿真图如图2～图4所示。

1）障碍环境1的情况

障碍环境1设置了5个大小不一、分布相对分散的球体障碍物。

2）障碍环境2的情况

障碍环境2设置了5个大小不一、分布相对分散的立方体障碍物。与障碍环境1相比，障碍环境2中的障碍物具有尖锐的边和角，提高了避障难度。

3）障碍环境3的情况

障碍环境3设置了大小不一、分布相对分散的3个球体障碍物与2个立方体障碍物。障碍环境3采取混合障碍物更加贴近真实环境。

从表1～表3可以得出，经过50次实验的平均路径长度，在障碍环境1中本文算法比RRT算法缩短23.93%，比RRT*算法缩短11.17%，在障碍环境2中本文算法比RRT算法缩短25.13%，比RRT*算法缩短10.66%，在障碍环境3中本文算法比RRT算法缩短24.22%，比RRT*算法缩短10.57%。因此，对比传统的RRT算法，本文算法在三维全局路径规划中生成的路径更优，具有更好的路径质量。

可以看出经过蒙特卡洛方法的随机扰动路径节点和路径代价评估，显著提高了路径的平滑度和全局最优性。

3种算法均可基于LOS控制算法进行航向修正。在仿真中，为了更加贴近真实环境，本文设置安全距离为2 m，采用最复杂的障碍环境3为仿真环境，试验次数为 50次，其余条件不变。3种算法在添加了安全距离后生成路径距离障碍物的最短距离和平均距离如表四所示。

从表4可以看出，3种算法在添加了安全距离后，均能成功保持与障碍物的安全距离，确保航行器在水下运动的安全性。最短距离和平均距离均满足设定的安全距离要求（2 m），表明本文算法在复杂环境中能够有效避免碰撞风险。

图5呈现出了本文算法与LOS导航算法相结合之后的仿真动画截图，借助LOS控制算法，航行器可以依据路径规划算法在复杂的水下环境里达成精确的避障导航，仿真结果显示，本文所提出的RRT算法联合蒙特卡洛方法以及LOS控制算法，提高了路径规划的效率和路径质量，也提高了航行器在复杂环境中的导航能力。该算法为水下仿生航行器的三维路径规划提供了有效的解决办法，有较高的实用价值以及广泛的应用前景。

4 结　语

1）本文算法在复杂的三维障碍环境里呈现出较强的适应能力，可有效地避开障碍物，生成较为优良的路径，适用于水下航行器的实际应用场景。

2）通过仿真可知，本文提出的RRT算法结合蒙特卡洛方法，使得路径长度得到了显著缩短，本文算法在全局路径规划方面有更高的效率以及更优的路径质量。

3）本文算法与LOS导航算法结合后，可以证明本文算法路径优化结果是有效的、可执行的 ，具备较强的实用性。