基于IRRT*和DWA 的无人艇混合路径规划方法

张啸天，陈熙源

(东南大学仪器科学与工程学院，江苏 南京 210096)

水面无人艇(unmanned surface vehicle，USV)，是一种无人操作的带有多类型传感器的水面舰艇，具有体积小、全天时、低成本的优点。在配备先进的控制系统、传感器系统和通信系统的情况下，可用于勘探巡逻和测绘等任务，拥有着广阔的应用前景[1]。

路径规划是水面无人艇的最基本的关键技术之一，它的目的是通过接收传感器所得到的信息，结合无人艇的运动状态，确定USV 航行的最佳轨迹，并在USV 实际行驶过程中，根据实际位置与预设位置的误差，不断修正受到外界因素影响的行进路线，以达到相关的任务要求。路径规划算法的优劣，不仅决定了其自主性水平，而且也是圆满实现任务的前提[1-2]。

无人艇的路径规划从广义上属于机器人路径规划的一种。目前主流的路径规划算法可以分为三类:基于图搜索的路径规划方法，基于采样的路径规划方法以及基于智能算法的路径规划算法。其中基于随机采样原理的快速搜索随机树(RRT)算法由于无需对环境进行具体建模，以及对于具有微分约束和非线性动力学的系统有效的优点备受研究者的关注[3]。由于RRT 算法产生的路径不具备最优性，Karaman 等提出了RRT*算法，在原有算法的基础上加入了重布线过程，使算法具备渐进最优性[4]。廖彬等[5]提出了一种F-RRT*算法，通过随机创建父节点以及引入三角不等式对路径成本进行优化。Naderi[6]提出了一种RT-RRT*，通过实时构建地图，对搜索树进行更新，实现RRT*的动态规划。以上改进均针对RRT*算法的搜索效率进行提升，进而匹配应用场景。

无人艇的应用场景存在影响载体运动的风浪流，但目前无人艇的路径规划研究主要局限在静水环境，较少考虑水流影响。侯春晓等[7]提出了应用于内河无人船的一般曲线路径的LOS 循迹控制算法，能够较好地处理内河中涨潮和落潮时的路径规划问题，当处于不同水流条件时规划的路径存在较大差别。吴博等[7]提出了一种基于速度障碍的自动避碰算法，该算法考虑了无人艇与风浪流的相对速度，并分析其相对速度与位置的角度关系，从而进行路径规划。总的来说，目前解决水流影响的基本手段是将无人艇运动与水流运动进行合成，再引入具体路径规划算法进行求解。

全局路径规划通常需要根据电子海图中海域中的障碍、危险区域信息，建立包含可以确保船只航行必要信息的环境模型[9-10]，接着从建立的模型中找出一条能满足各种航行指标的安全路径[11-12]。与全局路径规划不同，水面无人艇局部路径规划算法需要根据艇上多类型传感器实时感知周围环境中的信息，根据距离、风浪等因素动态地修正局部路径，确保无人艇航行路线的安全可靠[13-14]。

因此，要综合考虑风浪对于无人艇路径规划的影响，对于规划算法进行调整，同时结合预路径，实时给出无人艇下一时刻的运动信息。本文采用基于IRRT*和DWA 的无人艇混合路径规划方法。全局路径规划方法选择改进的IRRT*算法。这种方法基于增量式搜索，特别适合大空间的路径规划问题[15]。局部路径规划方法选择动态窗口法(Dynamic Window Approach，DWA)，并在此基础上结合海浪影响进行改进，能够较好地符合无人艇的实际运动情况[16-17]。

1 全局路径规划

针对无人艇的路径规划，通常为了保证算法路径规划的实时性和有效性，分为全局路径规划和局部路径规划两个部分进行。全局路径规划通常首先通过电子海图等环境信息，根据航行指令预规划出一条全局路径。在全局规划的基础上，无人艇通过自身传感器探测装置获取周围环境信息，结合无人艇具体的运动约束，再利用局部路径规划方法实现避障。这么做的好处是，通过预先花费较长时间进行全局路径规划得到全局路径，避免了无人艇在局部避障过程中可能出现的盲目性，同时也能有效地减少局部路径规划算法所需要的时间，具体过程如图1 所示。

图1 无人艇路径规划流程

在进行路径规划过程之前，通常需要对环境进行建模，常用的方法包括栅格法、可视图法和Voronoi 图法，这个步骤对于其他路径搜索算法往往是必要的。然而RRT*算法通过对状态空间的采样点进行碰撞检测，从而避免在状态空间内显式地构造障碍物，提供了大量的计算节省。

1.1 RRT*算法

为了解决机器人路径规划问题中存在非完整性约束或微分约束时，很难逐一对轨迹片段进行求解的问题。LaValle 于1998 年提出了随机采样的RRT算法，他通过使用增量式的前向采样，构建一棵搜索树，随后再对树结构进行搜索得到路径。具体的流程为将起点位姿点加入随机树后，直接在空间中先随机采样一个位姿点，遍历整个随机树中的节点并选出距离该随机位姿点最近的节点，在满足约束的前提下，以一定的步长向着随机点方向生长，直至到达目标区域。从目标位姿点出发，依次寻找当前位姿点的父节点直至回到起始位姿点，即可得到规划路径

RRT*算法是在RRT 算法基础上提出的一种路径优化算法。在快速地找到初始路径之后继续进行采样，不断地进行优化直到找到目标点或者达到设定的最大循环次数。RRT*算法过程以伪代码形式描述如下:

算法1 RRT*算法

虽然RRT*算法相较于RRT 算法能够给出一条渐进最优的路线，但是在实际环境运用时也存在一些缺点，例如最优解收敛较慢，迭代次数以及设置的运行时间会影响最后的路线等。

采样算法的质量与规划算法的运行效率有着密不可分的关系，有效的采样策略能够节约大量的搜索时间。初始RRT*算法采取的策略是在地图中直接进行随机采样，会产生大量与最优路径明显无关的采样点，导致算法的实时性不佳。后续的研究者针对采样算法进行了改进，例如Informed-RRT*(IRRT*)，在得到可行路径后对采样范围进行缩小，初始点xinit和目标点xgoal设置为椭圆的焦点，焦距为Cmin，当前的最优路径长度Cbest为椭圆长轴，建立椭圆作为新的搜索范围，如图2 所示。由于椭圆的性质，椭圆内的任意一点到两个焦点的距离之和小于长轴长度，所以只对椭圆内部进行采样，随着采样过程的进行，路径长度Cbest不断缩短，椭圆采样面积减小，能够有效地有效提升收敛速度。采样空间可以表示为:

图2 IRRT*原理示意图

1.2 改进随机采样

在IRRT*的基础上，为了进一步提高搜索效率，降低算法的随机性，在生成新节点xrand的过程中，引入启发式函数，引导新节点朝向xgoal生长，减小了规划时间。

这里的Pr是位于(0，1)的随机数，α是给定的常数。LineTo(xgoal) 指在当前节点与xgoal的连线上进行采样；Random(χ)指在地图上随机采样；当目标点xgoal已经包含在随机树中时，xrand会使用Ellipse(xinit，xgoal)在上文提到的椭圆内部进行采样。因此，该采样算法能够更有效地生成新节点，这也意味着采样时间得到了减少，算法的实时性得到提高。

为了评估启发式IRRT*算法相较于传统RRT*算法的改进，进行仿真对比，如图3 所示，同时将寻找路径所需节点数和路径长度列入表1。

图3 3 种RRT*路径规划结果

表1 不同RRT*路径长度和时间对比

模拟环境地图尺寸为500 m×500 m，设置无人艇路径规划起点位于左上角，终点位于右下角，坐标分别为(25，25)和(475，475)，白色和黑色分别代表可通行区域和不规则障碍物。通过传统RRT*算法所获得的路径如图中实线所示，所需的扩展节点为1 450 个，路径长度为733.32 m。使用IRRT*算法获得路径如图中点划线所示，所需的扩展节点为613 个，路径长度为722.09 m，而加入了启发式函数的IRRT*算法所得路径如图中虚线所示，所需的扩展节点为526 个，路径长度为701.31 m。

可以看出，使用启发式IRRT*算法能够减少所需扩展节点数量，由于RRT*算法主要耗时在碰撞检测过程，而每个新生成的节点都需要调用该过程，因此可减少碰撞检测部分，以降低算法所需时间。根据式(2)，控制采样算法的一个关键参数α，调整该参数能够改变采样算法对于目标点的趋向，这里的α＝0.3。与IRRT*算法相比，引入启发式函数后，所需节点减少了14%。因此，当无人艇在宽阔水面进行预规划时，启发式IRRT*算法能够有效地给出一条全局路径。

2 动态窗口法

由于无人艇的航行环境往往较为开阔且复杂，不但需要对地图上已标记的障碍物进行规划，也需要通过无人艇所搭载的传感器检测到的动态障碍物信息进行实时躲避，而局部路径规划能够较好地解决该问题。结合上文通过启发式IRRT*算法给出的全局路径，同时考虑到海流的影响对于局部路径规划方法进行改进，使得无人艇在运行过程中能够更好地对水面的情况做出及时的响应。

动态窗口法(Dynamic Window Approach，DWA)是一种基于速度采样的局部规划方法，在速度空间采样多组速度，并模拟一定时间内无人艇的轨迹，根据预设的评价函数来比较这些轨迹的优劣，从中选取最优轨迹对应的速度执行。动态窗口的具体含义是根据载体的运动特性，将其速度采样空间限制在一个可行的动态范围内。

2.1 速度采样

根据无人艇本身的限制和环境约束，可以将速度的采样空间限定在一定范围内，如式(3)～式(5)所示。

式中，vmin、vmax、ωmin、ωmax分别表示速度和角速度的最小值和最大值，vc、ωc表示当前的速度、角速度，表示加速度和角加速度的最大值。

同时基于无人艇运动安全的考虑，需要在碰到障碍物前停下来，因此速度和角速度必须满足:

式中，dist(v，ω)表示在速度(v，ω)所对应轨迹上与障碍物最近的距离。

2.2 考虑流场评价函数

对于动态窗口法采样得到多组速度后，根据预设的时间窗口和前向模拟时间，模拟得到对应的多组轨迹，从中剔除不安全的轨迹，计算其他轨迹的评价函数。

式中，G(v，ω)表示处于当前速度对应轨迹的总评价函数值，hea(v，ω)用于评价当前航向与目标点方向之间的偏差，dis(v，ω)用于评价当前轨迹上各点与障碍物之间的最小距离，vel(v，ω)用于评价当前速度的大小，wav(v，ω)用于评价当前轨迹与全局规划得到轨迹的偏离程度，α、β、γ、η分别为各项参数。

区别于移动机器人，无人艇路径规划问题中不能忽略流场的影响。考虑流场的无人艇路径规划方法的优势主要在于，基于无人艇体积小续航能力不强的前提，利用流场有效地减少行驶耗能。流场类型主要分为两种:一种流速和流向相对固定的内河流场，另一种则是时变的海洋流场。为了反映洋流扰动的实际情况，其数学模型如下:

在式(9)中，采用与时间相关的无维度函数B(t)来表征曲流振幅的振荡，且B(t)＝B0＋εcos(ξt＋ψ)，其中B0、ε、ξ、ψ、k、c是用来表征时变流场的参数。因此，在某时刻t，无人艇当前位置所在流场的水流流速vs在直角坐标系下的投影vsx和vsy为:

在无人艇路径规划中，水流的影响应当从流向和流速两方面考虑，流场对于无人艇的影响体现在其航行速度上。因此为了综合考虑海流对于无人艇运动的影响，本文在传统动态窗口法基础上引入了海流评价函数wav(v，ω)

式中，〈v，vs〉代表无人艇当前速度v与流场的夹角，范围为[0，π]。这里假定无人艇能够克服流场带来的不良影响，满足:

同时，为了避免流场评价函数wav(v，ω)的某项在评价函数太占优势，也需要对其进行归一化处理:

2.3 改进DWA 算法仿真

为了验证改进动态窗口法的效果，进行对比仿真实验，分别对原始DWA 算法和考虑流场的DWA算法进行验证，仿真结果如图4 和图5 所示。

图4 不考虑流场的DWA 仿真

图5 考虑流场的DWA 仿真

两种方法验证实验初始设置均相同，航行环境设为50 m×50 m，图中的黑色圆点代表障碍物位置，障碍物半径设为2 m，箭头代表流场，其中箭头长短代表流速大小，箭头方向代表流速方向。起始点设为左下角，坐标为(5，5)；目标点为右上角，坐标为(45，45)，曲线代表无人艇实时航行轨迹。

对比分析图4、图5 轨迹，相较于传统动态窗口法仅考虑路径最短，可以明显看出考虑流场的动态窗口法能够使无人艇在行驶过程中尽可能避开逆流场方向，在条件允许的情况下选择沿流场方向。虽然此方法规划的路径长度相比传统DWA 算法更长，但是能够在成功避障的基础上，考虑到能量优化目标，计算出一条高质量的路径。

3 混合路径规划

虽然动态窗口法在局部避障时具有良好性能，但在实际运行中规划的路径极易出现缺乏目的性的情况，基于此本文在原有动态窗口法的基础上将改进IRRT*生成的全局路线作为参数输入到DWA中，作为局部目标点，以此保证规划器得到的路线既有良好的避障性能，也不会出现抵达不了目标点的情况。

为了验证混合路径规划算法的可行性，利用实际场景进行仿真实验，图6 是海域的卫星地图，对其进行二值化处理得到地图如图7 所示，白色代表可航行区域。

图6 无人艇航行区域卫星地图

图7 二值化地图

仿真航行区域为712 m×400 m，起始点坐标为(360，300)，目标点为(45，80)。无人艇的最大速度设为10 m/s，最大加速度为2.5 m/s，最大角速度为20°/s，水流流向为右偏下45°。

如仅采用DWA 算法进行路径规划，结果如图8所示，由于行驶路径较为复杂，算法易陷入局部最小“陷阱”，导致任务失败。

图8 仅使用DWA 路径规划失败

采用改进IRRT*给出一条预规划路径，如图9折线所示。路径长度为675.018 4 m。在全局路径的基础上，使用DWA 算法进行动态验证。规划航行结果如另一曲线所示，长度为726.167 5 m。

图9 基于全局路径的混合路径规划结果

仿真结果能够很好体现混合路径规划的优势，将改进的IRRT*算法和DWA 算法相结合，能克服局部规划缺乏目的性，易陷入局部最优的问题，也能实现根据无人艇具体的运动学约束和实时的障碍物信息进行避障的要求，更有利于USV 在复杂水面条件下实现路径规划。

4 结论

分别对路径规划算法中的全局路径规划算法和局部路径规划算法进行改进，提出了改进IRRT*的全局路径规划算法和考虑流场的DWA 算法。本文采用的改进IRRT*算法与传统IRRT*算法相比，搜索节点减少了14%，有效提升了搜索效率。考虑流场的DWA 算法在有流场情况下能够较好地利用流场影响，规划出相对节能的局部路径。将两种方法联合使用，通过混合路径规划仿真验证，能够实现无人艇在行进时对路径进行修正，安全完成任务。