基于蚁群-D_star融合算法的自主式水下潜器路径规划方法改进研究

杨卓然，王俊雄

（上海交通大学 船舶海洋与建筑工程学院，上海 200240）

0 引言

自主式水下潜器（Autonomous Underwater Vehicle，以下简称AUV，或水下机器人），是水下搜救、水下围捕、水下养殖等众多作业任务的重要组成部分。因为AUV技术能够克服很多人类无法克服的条件，例如低温、高静水压力、生物威胁等，应用领域遍布科学研究、军事、商业等重要，具有广阔的应用前景[1，2]。因此，吸引了很多国家对其进行研究与开发，随着AUV在科学研究、军事方面等领域的应用越来越多，AUV智能化的需求也越来越高[3]。与陆地上的各类机器人可以凭借停止运动并在遇到突发障碍物时及时转弯从而避障不同，海洋中的AUV需要在各种场景下快速、精准地规划出一条高效路径以应对场景动态变化。其中路径规划在AUV正确导航和避开障碍物中发挥着重要作用，即路径规划模块的性能水平与AUV运行路径的选择和运行的流畅度呈正相关。并且路径规划的核心是指使用路径规划算法，基于环境模型生成AUV的可行路径[4]。

典型的路径规划问题是指在有障碍物的情况下，规划出一条最佳运动路径，该条路径需要使水下机器人能从起始点安全稳定无碰撞地抵达终点，并且在此过程中满足一定的评价标准（例如能量最小、距离最短等）。针对路径规划问题，许多富有成效的解决方法已经被探索出来，其分类方式多样。其中按照对环境信息的掌握途径与程度可将路径规划算法划分为全局算法与局部算法，前者一般是基于先验环境信息的静态路劲规划方法，全部环境信息已知，海域信息的掌握情况较大程度上会影响算法的准确性与可靠性；后者则是基于传感器实时信息的动态路径规划算法，一般只能得知AUV附近较近海域的相关信息的局部路径规划，所得到的信息量和准确度与传感器性能高度相关[5，6]。全局路径规划算法按照原理主要可以分为3类，即传统算法、智能算法、传统与智能相结合的算法[7-10]。

Dijkstra算法（又称迪杰斯特拉算法），是1959年由荷兰著名计算机科学家E.W.Dijkstra[11]提出的最为经典的算法之一，它实现了在指定地图中搜索并生成从起点到终点的最短路径。为了提高路径搜索的效率，同时保证准确地找到一条最短路径。P.E.Hart，N.J.Nilsson&B.Raphael在1968年将优先搜索算法（Best First Search Algorithm，BFS）与Dijkstra算法组合形成了A_star算法[12，13]。A_star算法是启发式搜索算法，具有搜索时间短、运行速度快、不易陷入“早熟”、可遍及所有可达到的节点、便于计算、易于实现等优点。但传统A_star算法由于搜索节点过多，计算量会随着地图增大而迅速增加，进而造成算法搜索效率下降、规划路径不平滑等问题，同时其仅适用于静态环境[14-16]。由于A_star算法仅适用于静态环境这一特性，Anthony Stentz在此基础上做出改进，并于1994年提出了一种全新的反向增量式算法——D_star算法（又称Dynamic A_star算法）[17]。它具备与A_star算法相同的完备性，易于计算和实现的特点，虽然计算量与计算时间会略大，路径平滑性不足与易陷入局部最优的问题仍未解决，但其反向搜索机制能够适应动态复杂环境下的运算[18-20]。结合D_star算法作为增量算法的优点，基于A_star算法，逐渐演变出了一种增量启发式搜索算法Life Planning A_star算法，即LPA_star算法，其第一次正式出现于2001年Sven Koenig和Maxim Likhachev的相关论文[21]。相较A_star算法，LPA_star算法大幅提高了搜索效率，但仍无法应用于动态环境。基于LPA_star算法，Sven Koenig和Maxim Likhachev于2002年提出了D_star lite算法，它是一种增量启发式反向搜索算法，通过假设未知地形都是自由空间，以当前位置作为新的起始网格点，通过反复计算终点到各个中间节点（亦是新的起点）的最短距离，增量式地实现路径规划[21，22]。D_star Lites相较D_star算法，在需要重新规划的情况下，需要重新搜索路径的次数明显减小，受影响的节点显著减少，效率明显提高。此算法已广泛应用于火星探测车等众多实际场景[22，23]。当实际应用场景对应面积较大时，单独的D_star Lite算法为了提高路径规划算法的精度，在对海洋环境进行栅格化建模时通常选择将栅格尺寸尽可能地缩小，以更细粒度的海况建模实现较优的路径解，但由于规划序列的数量迅速提高，将导致重新进行路径搜索与规划以及全新路径规划的执行所需的时间均迅速提高，其与D_star算法共有的搜索效率容易降低、规划路径不平滑、易陷于局部最优，难以搜寻到全局最优路径等问题仍未得到解决。

智能算法中，蚁群算法（即ACO算法，全称为Ant Colony Optimization）是借鉴蚂蚁觅食方法而设计的一种一元启发式随机搜索寻优方法，于1992年由Marco Dorigo[24]提出。该方法具有正反馈、鲁棒性强、对初始路线的选择要求较低等特性，同时具有易与其他算法结合的突出优点[25，26]。但是该算法亦有计算量大、局部搜索能力弱、收敛速度慢、易陷入局部最优、初值依赖性较强等缺陷，使其应用场景受到一定限制[27，28]。此外，蚁群算法本身并不能适应动态复杂环境下的运算，为了使其具备动态避障功能，Zhao[29]提出通过在既定的最短路径上找到合适的节点进行局部路线规划从而实现避障的思路。此方法虽然提供了蚁群算法进行动态路径规划的基本思路，但实际意义并不大，仍有较大改进空间。

结合水下机器人实际所需的复杂工作环境，在诸如水产养殖的大量实际应用场景中，AUV将同时面对大面积的全局最优静态路径规划与小面积的局部动态实时路径规划需求。基于上述分析，目前已有的单一算法均不能较好地同时满足此类需求，故而笔者提出了一种基于蚁群-D_star融合算法的改进路径规划算法，较大程度上避免了D_star算法本身搜索效率容易降低、规划路径不平滑、易陷于局部最优，难以搜寻到全局最优路径等问题，亦满足了水下复杂变化环境所需的动态路径规划能力。同时文中引入Floyd-Warshall算法[30]，通过这种简单且广泛使用的动态规划算法，任意两点（一般为起始点与终点或各类转折点）之间在计算最短路径时只需建立两点间路径长度的二维数组，无须设定相应的参数以及估计函数，并且可有效改进融合算法规划路径不平滑的问题[31，32]。

1 栅格法环境建模与原始路径导航算法

1.1 栅格法环境建模

与AUV的实际应用环境不同，为了方便仿真实验，一般采用栅格法进行环境建模，将原有的三维立体水下环境降维简化为含有一定数量的大小相同均匀排布的栅格方块的二维空间[33]。

在将复杂的海洋环境降维并抽象成均匀栅格地图后，为表示每个区域（海域/障碍物等）的不同属性，每个栅格均会进行相应的标记处理（一般采用颜色标记法），除颜色外，每个栅格的其他属性（主要是尺寸）均保持相同。

如图1所示，经过白色标记的栅格将用于表示可通行的海域；对应的经过黑色标记的栅格则表示障碍物，即不可通行的海域，此外，亦有红色栅格表示新检测到的障碍物，即不再可通行的海域，以便与黑色的原始障碍栅格区分；绿色、紫色栅格则分别对应起始点与终点等。虽然海洋中各位置对应的障碍物通常形状不规则且体积有一定差异，但在路径规划处理时，一般对它们采取膨化处理，即用黑色填充满整个栅格，整个栅格都将被视为不可通行的海域，既减少了计算量，又能在一定程度上降低AUV与障碍物的碰撞风险，提高安全系数。

图1 栅格法环境建模地图对应关系

除颜色标记外，为方便进行程序化处理，在坐标化前，一般采取二进制参数对各栅格进行赋值，每个栅格均可由0表示空，即可通行无障碍；由1表示非空，即有障碍不可通行。当每个栅格均已被赋值后整个二维平面便已一一映射成了对应的0-1矩阵，以便进一步处理，如图1所示。

在将栅格坐标化后，每个栅格通常有两种表示方法，既可采用坐标系的横纵坐标表示，又可通过一定规则的序号来表示，两者关系为：

式（1）中i为栅格原始序号号，row为坐标化后的横坐标，即行序号，col为纵坐标，即列序号。实验中水下机器人航迹的起点和终点分别为ID为21的栅格和ID为322的栅格，如图2所示。

图2 实验所用栅格地图及对应ID示意图

AUV每次移动的最小距离为栅格边长，对应移动方向为如图3所示的8个方向，当前节点为O点，在剔除已经经过的栅格以及不可通过的海域，即障碍物对应栅格之后，其余的各栅格对应的相邻节点均可用于下一步的对应节点，对应节点分别为A、B、C、D、E、S、W、N，如图3所示。

图3 栅格法八向行进（子节点选取）基础

1.2 原始D_star算法

D_star算法是基于A_star算法改进得出的一种反向增量式搜索算法，与A_star算法由起始点向目标点逐步正向搜索相反，D_star搜索始于目标点，终于起始点，每搜索过一个节点，会计算其相应的H（x），即该节点对应的距离度量信息，由于算法的增量式特点，当动态环境中的突发障碍物阻碍了原有路径的搜索进程时，算法将会不再需要重新规划，而是可以借助已获得的各节点的H（x）实现动态环境中的路径再规划。其中，各节点的距离度量信息为：

式（2）中H（x）为终点与起始点，即x点之间的距离度量，H（y）为终点与当前节点，即y点之间的距离度量，C（y，x）表示当前节点与起始点，即y点与x点之间的距离度量。为求计算方便，距离度量一般取欧式距离，本研究引入洋流、转弯等因素提出全新的总代价式，用以表示距离度量。

类似于A_star算法，D-star算法在搜索最优路径的过程中需要通过建立并更新一个优先队列（Openlist）从而对实际海洋工况中的各节点（State）进行搜索。搜索的关键为路径节点的传递，即由终点向AUV所在位置进行搜索的过程，这种传递过程是以持续地从当前Openlist中取出代价值最小的State来实现的，每当一个State由Openlist中移出时，它会将开销传递给它的邻域State，这些邻域State会被置于Openlist中，并不断进行该循环直至机器人所在State的状态转变为Closed（即当前路径节点已在Closelist中），或者Openlist为空（即不存在到目标点的路径），过程中存在对Openlist中各节点按照代价降序排列的过程，其中通常取此时代价最小的点为Nextnode（即下一个节点）继续搜索与判断，而在检索Nextnode的过程中通常会对比其邻域八子节点（即Subnode），具体步骤与流程见图4。

1.3 原始蚁群算法

蚁群算法是受自然界蚂蚁觅食行为启发的一种随机搜索寻优方法[34]。该算法基于正反馈机制，核心为信息素的分布与浓度，信息素产生于每只蚂蚁进行路径搜索的过程中，其浓度随着路径长度的减少以及经过的蚂蚁的增多而提升，信息素浓度越高，对应的解的质量也越高；在整个觅食过程中，初始时各路径信息素量相同，各蚂蚁随机移动；之后的每次搜索（即每次迭代）的过程中，大部分蚂蚁将逐渐向信息素浓度更高的方向移动，而少部分蚂蚁则仍会继续随机寻找未知但更优的行进路线。当蚂蚁搜索次数足够多（迭代次数达到预设上限），或一段时间内蚂蚁觅食路线趋于固定，对全新路径的搜索趋于停滞，则可认为此时蚂蚁们的觅食路线即为蚁群算法得出的全局最优路径。

其概率选择表达式为：

信息素浓度更新表达式为：

式（3式）～（5式）中Pij（t）表示选择最短路径ij的概率，表示在最短路径ij上的蚂蚁k会在经过的区域释放一只蚂蚁浓度的信息素（一般为蚂蚁k本轮构建路径长度的倒数），△τij表示t时间内的信息素总增量，N代表蚂蚁个数，α代表信息素因子，β代表信启发式因子，ρ代表信息素因子的更新参数（未蒸发率）。

2 蚁群-D_star融合算法与改进

2.1 蚁群-D_star融合算法描述

蚁群-D_star融合算法步骤见图4。

图4 蚁群-D_star融合算法流程逻辑框图

步骤1：如上文1.1中所述，利用栅格法将原有的三维水下环境抽象成存在随机障碍物的二维栅格模型，建立起始点与目标点，引入蚁群算法，初始化各项信息：包括路径总长度、各表项信息、信息素数量与分布情况、节点数目与对应距离度量、迭代次数等蚁群算法核心参数。开启D_star算法，初始化各对应参数，从目标点开始进行反向增量式搜索，并在Openlist表中添加目标点。

步骤2：如上文1.2中所述，判断Openlist表是否为空，若是，则表示无可行路径；若非空，则将Openlist表中节点按成本估计排序，取其中对应值最小的节点作为Nextnode，即下一个节点，同时将对应节点从Openlist表中移除，并在Closelist表中添加对应节点。判断Nextnode是否为起始点，若是，则结束D_star算法，并跳转到步骤3；若否，则继续此步骤。判断完成后，将此时Nextnode的子节点，即其周围的8个相邻节点添加到Subnode列表中，计算各自对应的代价估计，并与它们的父节点，即此时的Nextnode节点比较，取其中的最小值对应节点为新的Nextnode节点，并更新各项对应数据，重复此步骤。

步骤3：D_star算法完成，已实现路径规划并已建立全局信息场信息，将得到的路径转换为蚁群算法的初始路径，将其对应的节点的信息素浓度增大，其余节点信息素浓度保持不变均匀分布。

步骤4：如上文1.3中所述，利用蚁群算法进行全局静态路径优化，将各节点代入转移概率函数[式（3）]来计算其对应的转移概率数据，并通过轮盘赌法实现节点选择。更新路径表与路径总长度，即在路径表中添加已被选择的节点ID。然后判断禁忌表中是否仍含有目标点，如有，则在迭代次数上增加一，并重复此步骤。

步骤5：每次循环将得到若干路径，计算各自对应的距离度量，并进行比较，取其中最小值，同时完成其与对应路径的记录。信息素亦将根据规则进行对应挥发与更新。

步骤6：比较当前迭代次数与预设的最大迭代次数，若前者大于等于后者，则结束蚁群算法，跳转到步骤7；若前者仍小于后者，则重置路径信息、禁忌列表等，并将迭代次数加一，并跳转到步骤3进行新一轮全局静态路径规划。

步骤7：根据传感器反馈的最新数据进行判断，是否探测到突发障碍物，若不存在，则跳到步骤9；若存在，则基于D_star算法已形成的全局信息场信息，结合新发现的障碍物，更新信息场，具体方式为将新障碍物所在节点与其对应父节点的代价值提高到障碍物的对应值，更新完毕后，可迅速形成全新的全局动态路径。

步骤8：D_star算法完成，已实现路径规划更新，根据传感器反馈的最新数据进行判断，当再次探测到突发障碍物时，重复步骤7以再次更新优化路径。

步骤9：获得最优路径，进行平滑性处理，输出最优路径，算法结束。

2.2 子节点选择策略优化

传统D_star算法在考量当前栅格周围的8个方向，即8个子节点时，仅考虑是否为障碍物以及是否为已路过的节点，会忽略两大重要因素。其一是海洋环境中有洋流、水流等因素存在，本身向8个方向移动代价并不相同；其二是由于经过栅格法建模，通常不会再考虑AUV本身的形状、体积、可操纵性等因素，而是将其简化成一个纯粹的质点进行路径规划。由此形成对质点非常理想且最优的路径[35]。但这对于在水下工作的AUV则并不完全匹配，常会有诸如从两障碍物顶点间穿越、紧贴障碍物侧面顶点的路径存在，对全局路径规划的安全性与可靠性造成了一定的威胁。规划路径风险如图5所示。

当AUV从栅格A移动到栅格B时，考虑到AUV具有一定的体积，若障碍物的体积较AUV自身体积大且未经过明显膨化处理，则在如图5所示红点位置，AUV存在较高与障碍物侧壁发生摩擦甚至碰撞的风险，对AUV的水密性能与操纵性造成了一定的负担。

图5 典型路径规划风险情况示意图

对此，笔者在原D_star算法子节点的选取策略的基础上，引入洋流等重要影响因素，其核心是在原有的检索顺序中引入分级分组的方法。具体分组方法分3级：（1）根据与中心栅格接触方式对周围8节点预分为两组（前面图3），其中W、E、N、S这4个与中心栅格有公共边的子节点划入原始高级组，并取其中与洋流或主要水流方向夹角小于90°的归入高级组；（2）夹角大于等于90°的归入低级组，另外的4个与中心栅格仅有公共顶点的子节点，划入原始中级组，检索原始高级组中是否存在障碍物，如存在，则将其两侧节点从原始低级组去除（前面图3）。（3）如N为障碍物，则去除A、B子节点；如M、N为障碍物，则去除A、B、C子节点，剩余子节点组成中级组，取其中与洋流或主要水流方向夹角小于90°的归入高级组，夹角大于等于90°的归入低级组。分组后的子节点在选取时不再进行等价处理，而是优先检索高级组中的邻域节点、再对低级组中的邻域节点进行检索。

此外，在原始用于判断路径最优的欧几里得度量基础上，引入洋流等因素，将洋流方向单位化后按照横纵轴矢量分解，引入单位洋流方向向量为：

笔者为度量路径总代价，引入总代价C*，并提出新度量式：

式（7）中m、n取自式（6）对应系数，x、y表示路径每两次转弯点坐标的差值，o表示转弯总次数，Rk表示第k次转弯转过的弧度，为转弯代价系数1，取1，R为转弯代价系数2，本文取2，新总代价度量公式相比路径总长度更具实际意义。

2.3 操纵性引入与路径平滑性处理

在对蚁群-D_star融合算法进行子节点选取策略优化后，虽然全局路径规划的安全性与可靠性明显提升，但并未克服规划路径转弯次数多，平滑性不足、不便于AUV实际操纵等问题[36]，甚至为提高路径的安全性，转弯次数可能略有提升。因此本文基于Floyd-Warshall算法中对蚁群-D_star融合算法进行进一步改进，同时，在不改变质点设定的前提下，引入安全距离参数，进一步避免碰撞风险。

通过Floyd-Warshall算法这种简单且广泛使用的动态规划算法，任意两点（一般为起始点与终点或各类转折点）之间在计算最短路径时只需建立两点间路径长度的二维数组，无须设定相应的参数以及估计函数，并且可有效改进融合算法规划路径不平滑的问题[30]。在原始Floyd-Warshall算法进行单相路径优化的基础上，引入从目标点T到起始点S的反向优化，实现对蚁群-D_star融合算法的双向平滑优化。

步骤1：删除冗余节点：如图6所示，从目标点T开始，将其与起始点S都视为拐点，删除所规划路径中每相邻两拐点中间的其它节点，规划路径处理后仍存在的节点依次分别为T、P3、P2、P1、S。

图6 改进Floyd-Warshall算法示意图

步骤2：设置安全距离D：首先从从起始点S开始，在目前保留的T、P3、P2、P1、S每相邻两节点间检索是否存在障碍物，如存在，则保留现节点；如不存在，则连接此二节点，取附近最近的障碍物顶点，计算其与两节点连线间的欧氏距离。

令待选取的路径S Pi所在直线为

与之最接近的障碍物顶点为O（x0，y0），则该顶点与对应路径的最短距离为

安全距离D一般取栅格边长的0.5-1倍，本研究中取0.5 m，比较最短距离与安全距离D大小关系，当且仅当安全距离更小时，对应路径方可使用，完成安全距离校核后保留的节点依次为T、P*3、S，依次连接即为此时的路径。

步骤3：双向优化：将搜索方向对调，即目标点T开始检索障碍物，重复步骤2操作，再次完成安全距离校核后保留的节点依次为T、P*2、S，依次连接即为双向平滑优化后的目标路径。

步骤4：输出优化后路径。

3 仿真结果与分析

通过Matlab2019b[37]对在引入静态与动态障碍物的AUV全局路径规划进行了仿真实验并进行了分析，同时对比几种算法在仿真地图下的欧氏距离与总代价，结果见表1。

表1 几种算法的欧式距离与总代价对比

对环境地图进行降维与栅格化建模处理，取栅格边长为1 m，水流纵向向下，约1 m/s，黑色栅格为初始障碍物区域，占地图总面积的18.42%，斜线纹理栅格为起始点与目标点，如图7（a）所示，纵横交叉线纹理形栅格为新增的动态障碍物区域，占地图总面积的2.05%，如图7（b）所示，图7（c）（d）（e）为几次不同初值情况下的D_star算法路径规划示意图，其中图7（d）所示第二次路径规划与蚁群-D_star算法路径规划轨迹一致，结合几次路径的总长度与总代价，可以看出D_star算法对初值依赖性较强，无法较为稳定地得出全局最优解，而蚁群-D_star算法则可较大程度上改善这一问题，可以较好地兼顾全局最优性以及时效性，规划出较优秀路径。

图7（f）为引入动态障碍物区域后通过蚁群-D_star算法进行路径规划的效果图，图7（g）为引入子节点选择策略优化方法后的路径示意图，图7（h）为操纵性引入与路径平滑性处理后的路径示意图。结合表1中数据，可以看出引入优化子节点策略后，总长度与总代价均略有增加，再进行路径平滑处理后，总长度重新减少，但仍比原始长度略大，由于删除了大量冗余节点，转弯次数大幅减少，相较最初减少了37.5%，相较子节点选取策略优化后减少了50%，引入了洋流与转弯因素的总代价亦明显减少，较最初减少了23.53%，相较子节点选取策略优化后减少了31.23%，优势十分明显，具有较大实际意义。

图7 不同算法仿真对比试验示意图

由图7的仿真波形可知，以传统蚁群-D_star算法为基础规划得出的路径常出现斜穿障碍物顶点的情况，普适性较差。改进后的蚁群-D_star算法在设置安全距离后，规划路径与障碍物的距离始终大于Dmin，避免了AUV距障碍物过近而发生碰撞的风险，提高了路径的安全性。同时，借助路径平滑处理所增加的总距离几乎可忽略不计，在实际应用中，可以大幅度减少AUV运动过程中的转动次数和总角度，有效提高移动机器人运动的平稳性和工作效率，具有较高的实用价值。

4 结论

（1）针对实际环境中的AUV的工作路径规划可能会受突发障碍物干扰，本研究基于各类全局静态路径规划算法，在蚁群算法的基础上，引入D_star算法，并将两者有机结合起来，发挥各自的优势，在面对诸如诸如水产养殖等实际动态环境时，既保留了全局路径规划的最优性，又实现了局部实时路径规划的时效性。

（2）为提高蚁群-D_star融合算法的安全性、可靠性与搜索速度，有效降低全局路径规划所需时间，并引入实际工作环境中洋流、水流等的真实干扰影响，本文提出将D_star算法的子节点选取策略进行优化，将地图中任一节点周围的8个子节点分层处理，经预分组与校核再分组后划分为高级组和低级组，从而有效缩短寻优时间，减少诸如穿越两障碍物间顶点等危险路径的存在，提高路径规划的安全性与可靠性，亦能尽量避免因不必要逆流造成的过多能量损失与航行风险。

（3）针对蚁群-D_star融合算法所规划路径通常以折现为主、平滑性不足、转折点较多、不利于AUV的实际操纵等问题，尤其是引入子节点选取策略优化后转折点存在一定程度的增加，引入Floyd-Warshall算法，基于其对原融合算法所得全局路径进行正反双倍平滑性处理，处理后改进后的路径长度几乎不变，而转弯次数与实际操纵总代价方面均明显优于优化前算法，同时，所规划路径由近似折线转化为近似拟合曲线，曲率保持连续，与AUV的操纵指标适配性更好，实际应用价值较高。