改进双向快速搜索随机树的无人艇路径规划

2024-03-27赵贵祥李云淼王晨旭

系统工程与电子技术 2024年4期

赵贵祥, 周健, 李云淼, 王晨旭,*

(1. 天津大学海洋科学与技术学院, 天津 300110; 2. 江苏自动化研究所, 北京 100036)

0 引言

水面无人艇(unmanned surface vehicles, USV)作为一种智能化海洋装备,具有便携性好、操纵性强等优点,常被用来执行近海水域的海洋调查和监测任务,以及海上应急和救助等工作[1]。由于USV适用于复杂的工况,则要求其具有更强的自主性,而路径规划是USV自主系统的重要基础[2]。

根据环境信息的知悉情况,路径规划可分为局部路径规划和全局路径规划,前者适用于USV的局部避碰,后者能在已知环境中提前规划一条安全可靠的路径[3]。全局路径规划主要包括A-star算法[4]、遗传算法[5]、粒子群优化算法[6]、概率路线图(probabilistic road maps, PRM)算法[7]和快速搜索随机树(rapidly-exploring random tree, RRT)算法。RRT算法具有模型简单、搜索速度快且具有概率完备性等优势,在解决路径规划的问题上得到了广泛的应用[8-10]。双向RRT[11](bi-directional RRT, BI-RRT)是由两个随机搜索树同时扩展的RRT版本,提高了算法收敛速度。然而,BI-RRT仍生成大量冗余的随机点,带来了搜索效率低、路径拐点多等缺点[12-13]。

为了提高算法的搜索速度,国内外学者进行了大量的改进和研究,向金林等[14]通过动态步长的策略对BI-RRT算法进行了改进,改进后路径质量更好,算法收敛时间更短。徐秉超等[15]采用预生长和基于欧氏距离的随机点筛选机制,提高了BI-RRT算法的搜索效率。Ma等[16]提出了概率平滑的BI-RRT算法,通过引入目标偏置策略和节点校正机制减少了碰撞概率,提高了算法的搜索速度。Maseko等[17]通过知情采样和路径优化来加速基于基本RRT和优化RRT(RRT star, RRTstar)的搜索速度。为减少RRT规划路径的拐点,Karaman等[18]提出了RRTstar算法,通过迭代的方式不断更新父节点和重新布线,以确保渐进式的优化。Qureshi等[19]为了提高算法的收敛速度,在RRTstar中加入了人工势能场算法。Akgun等[20]在RRTstar的基础上提出了改进的双向RRTstar(bi-directional-RRTstar, BI-RRTstar)算法,提高了搜索效率。Qureshi等[21]随后提出智能BI-RRTstar(intelligent BI-RRTstar, IB-RRTstar)算法,通过引入智能样本、插入启发式算子,使算法快速收敛到最优的路径。RRTstar类的算法能够减少路径拐点和提高平滑度,但增加了时间成本。Liu等[22]提出一种基于贝塞尔曲线平滑和目标偏向的BI-RRT,提高了路径的平滑度。然而,上述的改进算法都是单一的,并未同时解决搜索效率低、路径拐点多等问题,改进的效果受到一定的影响。

为此,在上述研究的基础上对BI-RRT进行改进,提出了搜索树扩展策略和路径平滑策略。首先采取极度贪心的思想,提高了两颗随机树的连接概率,并采用高斯偏置随机点的采样方法,降低了搜索的盲目性,选择启发式的节点扩展策略使随机树的扩展更具导向性;其次,对节点扩展进行角度约束,对生成的路径进行剪枝和3次B样条优化处理,最终得到一条平滑的路径。

1 研究基础

1.1 环境模型描述

USV全局路径规划需要寻找一条从起始点到目标点的无碰撞路径。首先,需要将已经获得的静态环境信息进行栅格化,并进一步进行二值化处理。在此过程中,障碍物被表示为黑色,用数字1表示,可行水域被表示为白色,用数字0表示。为了简化模型,通常将USV视为质点,并对障碍物进行膨胀处理。通常使用一个形状为圆形或正方形的卷积核对障碍物进行膨胀,通过对原始图像进行扫描和卷积计算来实现。如果得到的结果全部为1,则无需进行膨胀,否则需要进行膨胀。这种方法不仅适用于凸形障碍物,也适用于凹形障碍物,如图1所示。图像膨胀的公式定义如下:

图1 障碍物膨胀示意图Fig.1 Schematic diagram of obstacle expansion

A⊕B={x|(B)X∩A≠Ø}

(1)

式中:A为需要膨胀的图像;B为卷积核; ⊕为闵可夫斯基运算符。

1.2 BI-RRT算法原理

BI-RRT是RRT算法的进化版。它从起点xstart和终点xgoal开始,分别构建两棵搜索树,直到两棵树相遇,最终实现路径规划。具体步骤如下:如图2所示,首先,对第一棵树进行扩展。在搜索空间中以一定概率选择任意随机点xrand作为采样点,然后在搜索树上找到距离xrand最近的节点xnear,并以一定步长向xrand的方向扩展得到新的节点xnew。接下来,检查xnear和xnew之间是否有障碍物,若无障碍物,则将xnew添加到搜索树中,否则放弃xnew。第二棵树的扩展步骤与第一棵树相同。两棵树不断交替扩展,直至它们之间的距离小于设定的阈值。此时,从每棵树的xnew节点开始,向各自的xnear节点回溯,生成路径的节点。最后,通过连接这些节点的线段生成路径。

图2 BI-RRT示意图Fig.2 Schematic diagram of the BI-RRT

虽然BI-RRT相比RRT搜索速度更快,但BI-RRT的扩展方向缺少导向性和启发性,冗余的扩展降低了路径的搜索效率。此外,传统BI-RRT算法生成了大量冗余节点,导致得到的路径拐点较多。本文针对上述问题,对传统BI-RRT算法进行改进,以提高路径搜索效率和路径的平滑度。

2 改进的BI-RRT算法

2.1 搜索树扩展策略

2.1.1 引入贪心思想

为了提高搜索效率,本文引入贪心思想,通过两种规则对传统BI-RRT算法进行改进:规则一,在搜索树扩展之前,首先判断起始点和目标点之间是否有障碍物。如果没有障碍物,则直接连接起始点和目标点,生成路径;规则二,在搜索树得到新节点后开始判断是否可以与另一棵树上最近的节点相连接。如果可以直接连接,则完成路径搜索。

2.1.2 高斯偏置随机点生成策略

传统的BI-RRT算法在路径搜索时存在目标导向性不足的问题,因为其采样点分布比较均匀,覆盖了整个搜索空间。这种情况导致搜索树进行了大量的盲目扩展,浪费了大量的计算资源,同时也降低了算法的收敛速度。为了改善这个问题,本文提出一种基于高斯采样的BI-RRT算法,该算法能够在一定的概率下以高斯分布的方式出现在目标点附近。改进后随机点xrand的求取如下:

(2)

式中:rand为地图中的随机点;p为(0,1)之间的随机数;η,λ为目标偏向阈值;f(x,μ,B)为二元高斯联合分布密度函数,其中x为随机点向量,μ为均值向量,B为协方差矩阵。求取公式如下:

(3)

在北东坐标系下,当协方差矩阵B的正对角线的值相同时,若反对角线的值为负,则二维高斯分布的图像的长轴方向与坐标系横轴的正方向夹角为135°;若反对角线的值为零,则不存在偏角;若反对角线的值为正,则二维高斯分布的图像的长轴方向与坐标系横轴正方向的夹角为45°,具体如图3所示。

图3 不同协方差矩阵下随机点的分布Fig.3 Random point distribution with different covariance matrixes

为了提高采样点的利用率,将二维高斯分布的图像的长轴方向与起始点和目标点连线方向垂直。为方便分析,对随机点进行旋转变换。如图4所示,θ1为起始点和目标点的连线与水平轴的正方向的夹角;θ2为高斯分布的图像的长轴与起始点和目标点连线的夹角;θ3为随机点需要旋转的角度。

图4 坐标系转换示意图Fig.4 Schematic diagram of coordinate system conversion

(4)

(5)

(6)

图5 高斯随机点分布示意图Fig.5 Distribution schematic diagram of Gaussian random points

2.1.3 启发式节点扩展方法

传统BI-RRT最近节点xnear的选择仅考虑与随机点xrand的距离,因此xnear的选择也过于随机,这也是导致搜索效率较低的原因之一。根据A-Star算法中的启发式路径代价的思想,本文对最近节点xnear进行改进,xnear的计算如下:

xnear=min(Cost(xtree,xstart)+dis(xtree,xgoal))

(7)

式中:Cost(xtree,xstart)为搜索树上的节点至起始点的路径之和;dis(xtree,xgoal)为搜索树上的节点到目标点的距离。常用的距离计算公式主要包括欧氏距离和曼哈顿距离,为进一步提高计算速度,本文通过曼哈顿距离计算搜索树和目标点之间的距离,如下所示:

dis(x,y)=|x(1)-y(1)|+|x(2)-y(2)|

(8)

2.2 路径平滑策略

2.2.1 转角约束和连接点约束

传统的BI-RRT算法在节点扩展时由于未考虑角度约束,通常会出现大角度的连接方式。此外,两颗搜索树的连接处也常不能平滑过渡,也会出现大角度的转折,因此传统的BI-RRT算法无法满足USV的运动学要求。为解决这一问题,本文设计了角度约束的策略,设USV最大转弯角度为θmax,在生成新节点xnew后,计算新扩展路径与前一段路径的夹角θ4。若θ4小于θmax,则记录并保存新扩展的节点,否则,删掉新扩展的节点。θ4的计算公式如下:

(9)

式中:A为xnear节点到xnew节点的向量;B为xparent节点到xnear节点的向量;xparent节点为xnear节点的父节点。

为解决传统BI-RRT算法在两棵树在连接点处出现大角度连接的问题,本文在连接处同样进行角度约束。如图6所示,在节点扩展无障碍的情况下,当满足θ5≤θmax且θ6≤θmax时两棵树连接,路径搜索成功。θ5和θ6的计算公式分别如下:

图6 搜索树平滑连接示意图Fig.6 Schematic diagram of smooth connection of search tree

(10)

(11)

2.2.2 搜索树剪枝优化

为减少冗余的节点,需要对得到的初始路径进行剪枝处理。改进的BI-RRT生成的初始路径为P(i)=[P(0),P(2),P(3),P(4),…,P(s)];在已知初始路径节点P(i)的情况下,从起始点xstart至目标点xgoal进行路径剪枝。具体步骤如下:

步骤 1输入初始路径的节点P(i)。P(i)=[P(0),P(2),P(3),P(4),…,P(s)]。

步骤 2将P(0)作为优化起始点P-start,Pnew=[P(0)],判断P(0)与P(s)是否可以直接连通,若可以连通则优化程序结束,否则执行下一步骤。

步骤 3依次判断P(0)与P(s-1)是否可以连通,若可以连通,将P(i)视为优化起始点P-start。否则,s=s-1,不断循环此过程,直至P(0)和P(i)可以连通,并将P(i)视为优化起始点P-start,不断循环此过程。

步骤 4判断P-start和P(s)是否可以连通。若可以连通,则结束优化程序,Pnew=[P(0),P(i),…,P(s)]。否则,s=s-1,直到P-start和P(s)可以连通,将P(s)赋值于P-start,并返回步骤3。路径剪枝的过程顺序如图7中步骤①～步骤⑤所示。

图7 路径剪枝示意图Fig.7 Schematic diagram of path cuttings

2.2.3 3次B样条优化

经过上述的改进,路径的节点减少了,但不能满足USV航行的实际情况,因此还需要将得到的路径进行平滑处理。3次B样条曲线具有二阶连续导数,因此可以满足速度和加速度对连续性的要求,更便于对USV进行控制与路径跟踪。本文选择3次B样条对得到的路径进行进一步处理,当有m+1个控制点时,3次B样条的计算公式如下:

(12)

其中,基函数Bi,3的求法如下:

(13)

2.3 算法流程

本文介绍了一种基于改进BI-RRT算法的USV路径规划算法。具体步骤如下:

步骤 1确定起始点xstart和目标点xgoal,对地图进行二值化处理,将不可航行区域表示为黑色,用数字1表示,将可航行水域表示为白色,用数字0表示,对障碍物进行膨胀处理。

步骤 2判断xstart和目标点xgoal之间的连线是否有障碍物。若无障碍物,则路径搜索成功,否则执行步骤3。

步骤 3首先对第一棵搜索树进行节点扩展。通过基于高斯分布的目标偏置确定随机点x1rand,并通过启发式代价确定x1near。

步骤 4判断x1near与x1rand的连线与上一扩展节点是否满足角度约束。若满足,则执行步骤5,否则返回步骤3。

步骤 5从x1near向x1rand方向延长步长δ的距离,判断连线是否可视。若可视,得到确定x1new,否则返回步骤3。

步骤 6判断x1new与另一棵树上最近的节点之间是否有障碍物。若无障碍物,路径搜索完成,否则执行步骤7。

步骤 7接下来进行第二棵树的节点扩展。通过基于高斯分布的目标偏置确定随机点x2rand,并通过启发式代价确定x2near。

步骤 8判断x2near与x2rand的连线与上一扩展节点是否满足角度约束。若满足,则执行步骤9,否则返回步骤7。

步骤 9从x2near向x2rand方向延长步长δ的距离,判断连线是否可视。若可视,记录该点为x2new,否则返回步骤7。

步骤 10判断x2new与第一棵树上最近的节点之间是否有障碍物。若无障碍物,则执行步骤11,否则返回步骤7。

步骤 11判断步骤10的连接是否满足角度约束。若满足,则路径搜索完成,否则返回步骤7。

步骤 12对初始路径进行剪枝和3次B样条优化处理。改进的算法流程如图8所示。

图8 路径剪枝示意图Fig.8 Schematic diagram of path cuttings

3 实验结果与分析

为了验证改进的BI-RRT算法的有效性,本文通过计算机进行仿真实验。实验中,将全局路径规划的起始点设置为(30,30),目标点设为(470,470),步长设为30,USV的船长设为5 m,最大角度变换不超过90°。将USV的船长设为卷积核的长度,对栅格化的地图进行膨胀处理。地图中的黑色代表障碍物,白色代表可行水域。在狭窄通道水域和复杂水域的环境中,分别使用改进BI-RRT、BI-RRT、BI-RRTstar和IB-RRTstar进行仿真实验。其中,BI-RRTstar和IB-RRTstar算法的最大迭代次数设为1 500。

在狭窄通道水域环境中,对BI-RRT、BI-RRTstar、IB-RRTstar以及改进的BI-RRT算法分别进行了100次实验,每次试验结果都具有随机性,因此随机选取了每种算法的一次实验结果进行比较,比较结果如图9所示。在BI-RRT算法中,由于随机点缺乏目标导向性,导致生成了大量冗余的节点,而搜索树的扩展和连接处也存在大角度转折,因此路径的拐点较多,如图9(a)中的椭圆1和椭圆2处所示。BI-RRTstar在BI-RRT的基础上进行了大量的父节点更新和重连接的操作,减少了路径的拐点,但大量的迭代过程需要更多的时间,如图9(b)所示。IB-RRTstar同样需要进行大量的迭代,结果如图9(c)所示。改进的BI-RRT算法在未进行后期优化的情况下,路径较原始的BI-RRT算法更加平滑,扩展节点更少,搜索树的扩展和连接处也不存在大角度转折,从而减少了路径的拐点,如图9(d)中较粗的折线所示。在剪枝优化后,改进的BI-RRT算法进一步减少了路径的拐点,如图9(d)中较细的折线所示。最终,在改进的BI-RRT算法的基础上进行了3次B样条优化处理,生成的路径不存在拐点,如图9(e)所示。表1展示了狭窄通道水域环境中各算法100次实验的结果。

表1 狭窄通道水域中仿真数据对比Table 1 Comparison of simulation data in narrow-passage waters

图9 狭窄通道水域中的对比实验Fig.9 Comparative experiments in narrow-passage waters

在复杂环境水域中,同样进行了100次BI-RRT、BI-RRTstar、IB-RRTstar以及改进的BI-RRT算法的实验。随机选取了每种算法的一次实验结果进行比较,如图10所示。实验结果显示,BI-RRT算法存在大量冗余的扩展,搜索树的扩展和连接处存在大角度转折,路径的拐点较多,如图10(a)中标注的椭圆1和椭圆2处。BI-RRTstar的实验结果如图10(b)所示,IB-RRTstar的实验结果如图10(c)所示。虽然路径平滑度得到了改进,但消耗了大量时间。改进BI-RRT的结果如图10(d)所示,较粗的折线表示未进行后期优化的结果,而较细的折线表示剪枝优化后的结果。改进后的算法搜索树的节点较少,路径的转角大和拐点多的问题得到了有效的改善。图10(e)表示改进BI-RRT的结果,优化的路径平滑无拐点。上述4种算法的100次仿真实验结果如表2所示。

表2 复杂环境水域中仿真数据对比Table 2 Comparison of simulation data in complex waters

图10 复杂环境水域中对比实验Fig.10 Comparative experiments in complex waters

(1) BI-RRT算法的全局路径规划中采样点过多,搜索效率较低。改进的BI-RRT算法采用了极度贪心算法和高斯偏置随机点采样,并引用了启发式节点扩展,从而提高了路径的搜索效率。表1和表2的数据显示,改进后的BI-RRT算法采样点最少,时间最短,相对于改进前平均时间减少了40.5%,随机采样点数减少了65.0%。

(2) BI-RRT算法的全局路径规划存在较多的拐点,路径不平滑,不能适用于欠驱动的USV的路径规划。改进的算法通过节点扩展和搜索树连接的角度约束,有效减少了全局路径的拐点数。随后,改进算法采用剪枝策略和3次B样条进行路径优化和平滑处理,优化后的路径是一条平滑的曲线,能够满足USV的运动学要求。表1和表2的数据显示,改进后的BI-RRT算法路径拐点最少,平滑度最高。

(3) 表1和表2的数据显示,相对于BI-RRT算法,BI-RRTstar、IB-RRTstar和改进的BI-RRT算法的平均路径长度分别减少了23.5%、24.3%和24.0%。BI-RRTstar算法和IB-RRTstar算法可以得到相对较短的路径。若只考虑规划的路径长度,IB-RRTstar算法得到的路径较本文改进算法得到的路径更短,但IB-RRTstar算法需要大量的迭代,增加了算法的耗时。此外,IB-RRTstar算法得到的路径仍存在一些拐点,不能满足USV的运动学要求。相比之下,改进的算法既能够减少算法的时间、满足USV运动学的约束,同时也能够减少路径的长度。