基于采样空间约束的改进RRT算法

李玮炜，张文波，张林丛

(沈阳理工大学 信息科学与工程学院，沈阳 110159)

水下机器人(Autonomous Underwater Vehicle，AUV)路径规划问题是进行水下网络部署、目标追踪以及多机器人协同等工作的基础，可以提高水下机器人检测效率[1]，完成指定区域的覆盖[2]和固定目标的追踪[3]。多机器人路径规划可以实现对目标的水下围捕[4]以及协同探测[5]。由于无缆AUV具有活动范围大、机动性好、不受电缆限制、隐蔽性好、安全性高等优点，被广泛应用于水下作业。

水下与陆地不同:水下不仅环境复杂，且由于水下更加空旷，需要进行大面积路径规划的可能性较大；水下机器人获取能源手段单一，算力和工作时长有限；采用水声通信，信息传播慢、损耗大。国内外常用路径规划算法有A*算法[6]、人工势场算法[7]、基于仿生学算法[8-9]、基于深度学习算法[10]、快速扩展随机树(RRT)算法[11]、RRT*算法[12]及其改进算法[13-15]等。

RRT算法原理简单、算力占用小，具有高随机性和强适用性，具有出色的探索能力，适用于水下路径规划，但存在一定的缺陷。一方面，由于采用随机采样的方法，随机点没有方向性，探索时间会随着解空间的增大而急速增加，给AUV带来算力上的极大挑战；另一方面，RRT算法生成的路径不光滑，规划出的路径长度相较最优路径更长，增加了AUV的工作时长，给AUV带来能源上的挑战。

针对RRT算法的改进算法中，RRT*算法是较为经典的一种。该算法通过为新节点重新选择父节点和根据代价有条件地将其他节点的父节点替换为新节点两个步骤，保证所有节点到起始点的代价和最小。其优点是规划出的路径相对平滑，路径长度比RRT算法生成的路径长度短；其缺点是路径的优劣非常依赖生成节点的数量和时间，每生成一个新节点，都需要对已经存在的路径的代价重新计算。因此，RRT*算法对AUV的算力有一定要求。

基于上述RRT算法存在问题的分析，本文重点解决随机性过高和路径质量低两个问题，提出一种基于采样空间约束的改进RRT算法。首先通过约束RRT算法采样点范围改善随机性过大的问题，实现当规划面积较大时，该算法能够更快收敛；然后通过随机节点的特征完成概率计算，利用节点概率与既定阈值的对比，完成随机节点概率选取，从而实现路径的优化，提升路径质量。

1 相关算法描述

1.1 RRT算法

RRT算法是一种传统的基于采样的路径规划算法，具有算法简单、高效等优势。其路径规划过程示意图如图1所示。图1中:Xstart为初始节点；Xgoal为目标节点；Xrand为随机采样节点；Xnear为距离随机采样节点最近的节点；Xnew为新生成节点；L为步长。

图1 RRT算法路径规划过程示意图

RRT算法以Xstart节点作为根节点，在解空间内随机生成Xrand节点，寻找与Xrand节点最近的Xnear节点，连接两节点，在连线上距离Xnear节点距离为步长L的位置，生成新节点Xnew，不断重复上述过程生成随机扩展树，当随机树中的Xnew节点包含了目标点或进入了目标区域，随机扩展树停止扩展。通过回溯节点的方式在随机树中生成一条从Xstart节点到Xgoal节点的路径。

1.2 凸包算法

凸包在图形学中指在一个实数向量空间V中，对于给定集合M，所有包含M的凸集的交集N称为M的凸包。

1.3 膨胀算法

膨胀算法以数学形态为基础对图像进行分析，用具有一定形态的结构元素，度量和提取图像中的对应形状，从而达到对图像分析和识别的目的。其定义为

式中:A⊕B表示使用B对A进行膨胀；()z表示B平移z后得到的结果。所有满足上式的点z组成的集合即是A被B膨胀后的结果。

2 RRT算法改进

本文的改进方向是通过对采样空间进行条件约束，缩小采样区域的范围，进而提高随机节点的有效性，再通过对随机产生的节点特征进行概率评价以选取节点，进而达到优化路径的目的。

2.1 采样空间约束

RRT算法的采样区域通常为整个空间，本文通过两个算法对采样空间进行限制。首先对障碍区域以及目标节点进行节点提取，建立集合X，在集合X中提取凸节点组成集合S，然后利用凸包算法划分出不规则区域。假设不规则障碍区域与凸包形成的区域如图2与图3所示。

图2 不规则障碍区域

图3 凸包形成区域

对划分出的障碍区域和目标区域做膨胀处理，其算法执行过程如下。

首先定义一个结构元素，其主要由尺寸、中心点和结构形状三部分决定，用该结构元素扫描集合X的每一个节点；再将结构元素的中心点与集合X中的节点重合；最后提取结构元素的外围节点形成新的区域。膨胀处理效果如图4所示。图4中方形节点是障碍区域提取的外围节点，圆形节点是以方形节点为中心做出的膨胀范围。

图4 膨胀处理效果图

膨胀区域的大小是由结构元素的尺寸、结构形状以及障碍区域决定。如果膨胀区域过小，则规划出的路径距离障碍区域过近，由于AUV自身存在一定的体积，在按规划路径运行时容易与障碍区域发生碰撞；如果膨胀区域过大，则对采样区域的限制减小。故需要根据障碍区域的形态、AUV的体积、RRT算法的步长不断调试结构元素。

经过凸包算法的区域划分以及膨胀算法的处理，将终点膨胀区域设定为A区，将障碍物膨胀区域设定为B区，将起始节点与终止节点形成的矩形区域设定为C区，将其他区域设定为D区，如图5所示。

图5 空间划分图

2.2 节点概率计算

本文通过对随机生成节点的特征做综合评价，生成该节点的概率数据，通过概率判定是否采用该节点作为下一节点，进而优化路径，提高路径质量。

为方便后续描述和表达，首先给出坐标系确定方式。由图6所示，以目标节点Xgoal与Xnear节点的连线为x轴且目标节点方向为正方向、Xnear节点为中心点建立坐标系。

图6 坐标轴及其夹角示意图

其次确定Xrand的四个特征，即空间区域贡献度、与Xgoal的斜率差值D1、与Xnear的斜率差值D2、与Xnear的角度θ。

由图5可知，路径空间划分为四个区域，不同区域内Xrand对结果的贡献度大小不同。A区域随机生成的节点对结果的贡献度最大，其次依序是B、C、D区域，以此设定节点特征概率P1为

Xrand与Xnear之间直线的斜率和Xgoal与Xnear之间直线的斜率差值D1计算表达式为

式中:x_Xrand和y_Xrand分别为Xrand节点的横、纵坐标值；x_Xnear和y_Xnear分别表示为Xnear节点的横、纵坐标值；x_Xgoal和y_Xgoal分别为Xgoal节点的横、纵坐标值。以此设定节点特征概率P2为

D1越小，代表Xrand与Xgoal之间方向角越小，二者方向更加一致，该随机生成节点对结果的贡献度越大。

Xrand与Xnear之间直线的斜率和Xnear与Xnear-p之间直线的斜率差值D2计算表达式为

式中x_Xnear-p和y_Xnear-p分别为Xnear父节点的横、纵坐标值。

令节点特征概率P3为

D2越小，代表在该Xnear处的路径越平滑。

由于以某一点为中心、呈中心对称的两个点与该点连线所得到直线的斜率相等，因此为增加随机生成节点的导向性，减少与目标节点相反方向的随机节点生成，增加Xrand与Xnear构成的直线和x轴正方向的夹角变量θ。

令节点特征概率P4为

根据随机生成节点的四个特征概率的重要性分别赋予权重，并满足权重和为1。根据公式(8)计算该随机生成节点的概率P。

式中:ωk为节点的第k个特征的权重；n表示节点特征总数。

为判断是否采用该随机节点为Xnew，设定节点阈值。节点阈值影响路径生成速度和路径质量。当阈值设定过高时，对随机采样节点要求高，耗费时间多，路径规划时间变长；当阈值设定过低时，随机采样节点对最终路径的贡献变小，路径的平滑程度变差，路径变长。因此，当对路径要求高、节点算力强时可以将阈值提高，当需要快速规划路径且对路径要求不高时，可以选择降低阈值。本实验中阈值确定为0.35。

当该点的概率大于设定阈值时，则采用该随机节点，若小于设定阈值弃用该节点，重新生成随机节点。

2.3 算法执行流程

改进的RRT算法流程如图7所示。

图7 改进的RRT算法流程图

首先对采样空间进行划分，通过限制采样区域改善随机性过大的问题。再对随机生成的节点根据公式(2)~(8)计算其概率，最后将概率与设定的阈值进行比较，判定是否采用该随机节点。

3 仿真实验

3.1 仿真实验及结果

为验证改进RRT算法的有效性，在python3.7环境下对改进RRT算法、RRT*算法以及RRT算法进行仿真实验。

实验条件为:仿真环境大小为60×60 m2、起始节点为(-25，-25)、终止节点为(40，40)、步长为1、最大轮数为500。

改进的RRT算法、RRT*算法以及RRT算法的执行结果如图8所示。三角点表示起始节点、圆点表示终止节点、实心区域为障碍区域、加粗线为规划后的最终路径、常规线为寻路时随机扩展树的树枝。

图8 不同算法运行结果(单位:m)

通过仿真实验运行结果可知，RRT算法和RRT*算法的随机扩展树比较分散，扩展结点较多，扩展范围较大，无效扩展区域多，改进的RRT算法由于限定了采点空间的范围，因此扩展树生长范围集中，多分布于障碍物周围，且路径相对光滑。

3.2 性能对比分析

三种算法的性能对比如表1所示。

表1 三种算法性能对比

由表1可得，在相同的路径规划面积下，改进RRT算法的路径规划时间和组成路径的节点数量均远小于RRT*算法和RRT算法，有效解决因随机采点机制没有方向性产生随机点的问题。

图9和图10是寻路面积从40×40 m2逐渐递增至180×180 m2时，对三种算法分别进行100次实验，并对各项数据求平均值所得到的结果。

图9 路径规划时间随寻路面积变化图

图10 路径节点数量随寻路面积变化图

由图9、图10可知，当寻路面积增加时，改进的RRT算法在时间方面的增长幅度远小于RRT*算法和RRT算法，同时在组成路径节点数量方面也小于RRT*算法和RRT算法并且数量更加稳定，意味着在采样空间更大时，改进的RRT算法适用性更强、路径更加稳定，对AUV的算力和能量要求更小，在水下大面积水域的路径规划中效果更好。

4 结论

提出了一种基于采样空间约束的改进RRT算法，首先针对RRT算法具有的随机性，利用凸包算法和膨胀算法思想，约束采点空间的范围；针对生成路径质量低的问题，通过计算随机采样点的概率并与阈值比较的方式，决定是否采用该随机点为产生下一新节点的基础。该方法使采样工作更具有方向性，减少了无效点的数量，使得计算出的路径相较RRT算法，路径长度与路径规划时间均更短，路径更平滑。

通过PyCharm进行仿真实验与数据分析，实验结果表明，改进的RRT算法在采样空间面积较大的情况下，具有更好的路径规划能力，并且路径规划时间与路径长度均有所下降，可以极大地减少AUV能耗，提高AUV的续航，适用于水下大面积路径规划。