基于目标导向的双向RRT路径规划算法

韩丰键 ,邱书波,冯 超 ,曹启贺,李庆华*

1.齐鲁工业大学(山东省科学院) 电气工程与自动化学院,济南 250353 2.齐鲁工业大学(山东省科学院) 电子信息工程学院(大学物理教学部),济南 250353 3.济南市人机智能协同工程实验室,济南 250353

随着工业生产对圆盘移动机器人要求的不断提高,路径规划已经成为圆盘移动机器人在工业生产上的一个重要研究领域,传统路径规划算法主要有A*算法[1]、蚁群算法[2]、人工势场算法[3]、遗传算法[4]等。尽管这些路径规划问题在处理低维空间路径规划问题方面具有一定的优越性,但是当机器人路径规划的构型空间维度较高时,算法在精确表达构型空间上需要占用大量的计算资源。基于采样思想的路径规划算法,如PRM算法[5-7]、RRT算法[8-14],不需要精确表达构型空间,而是通过在构型空间内获取自由构型形成构型图,以构型图描述构型空间的连通性,这类算法在机械臂、人型机器人等高维构型空间上的所体现出的优势更为明显。

Lavalle等[9]首次提出了快速扩展随机树(rapidly exploring random trees,RRT)算法,基于随机采样的思想获取自由构型qfree,用以构建一个树形网络表达自由构型空间。该算法避免了对整个环境空间建模,在高维构型空间的路径规划问题中优势更为明显,得到了广泛的关注。但是RRT算法采用的随机采样思想,也导致了节点的扩展无目标导向性,容易出现大量冗余节点,算法的收敛速度过慢。针对RRT算法生成构型无目标导向性、收敛速度慢等缺点,Urmson等[11]提出了路径代价函数的概念以表征路径的优化程度,面向目标路径越优则路径代价越小。该算法在扩展中,不再选择距离随机构型qfree距离最近的节点,而是选择k个较近的节点进行扩展,提升了已扩展RRT树内距离随机构型较近节点的搜索性能。代价函数的引入使得RRT树的扩展算法具有较好的目标导向性,可有效提升算法的收敛速度。但是该算法在狭窄空间或障碍物密集等复杂环境下,算法收敛性能会有明显下降。

在RRT算法研究初期,RRT及相应变种均采用单一随机树生成的思想,由初始构型作为随机扩展树的初始节点,在环境空间内进行扩展。单随机扩展算法构造简单,但是无论是基础RRT算法还是其改进算法,均存在收敛速度过慢的缺点。

基于双向搜索的思想,Pohi等[12]提出了双向随机扩展算法(bidirectional RRT,BI-RRT),构造两棵分别以起始构型和目标构型为初始点的随机扩展树,递归进行节点扩展以构建可表达构型空间的树形网络。相较于RRT算法,BI-RRT算法的收敛速度更快。但是该算法采用RRT算法的随机节点扩展思想,这导致BI-RRT算法也存在构型无目标导向性的缺点。为提升BI-RRT算法的收敛速度,结合贪心思想,Kuffner等[10]提出了RRT-connect算法。在BI-RRT算法扩展过程中,从qnear到qrand仅步进一个固定步长,即使在无障碍物空间内也需多次扩展过程才可到达qrand。在RRT-connect算法的扩展过程中,从qnear到qrand会持续步进,直至遇到障碍物或到达qrand。贪心思想的应用使得RRT-connect算法具有更高的扩展效率,在自由构型空间内这一提升更为明显。但该算法扩展过程是以自由采样构型qrand为目标点进行扩展,没有改变其导向性差的缺点。Akgun等[13]基于目标导向采样策略,提出了概率优化的RRT*算法,提升了RRT*算法的收敛速度及路径质量,但是该算法采样过程中使用的局部偏置思想在复杂环境中的适应性较差,存在导致算法收敛速度变缓慢的可能。李晓伟等[14]将一种目标偏向的思想加入到BI-RRT,避免了随机树搜索全局空间,很大程度降低了算法的复杂度,但是该算法采样过程中选取的目标点是另一棵树的上一个节点,目标导向性不明确,存在导致算法收敛速度缓慢的可能。上述所提的RRT算法均存在收敛速度缓慢的缺点,没有考虑到圆盘移动机器人的碰撞检测问题。

针对BI-RRT算法研究中存在目标导向性差、收敛速度慢的问题,提出了一种目标导向的BI-RRT算法(goal-oriented BI-RRT,GOBI-RRT)。该算法是在BI-RRT算法的基础上引入了目标导向的思想。通过搜索两棵树的最近节点,利用目标导向思想产生随机点,新增节点具有良好的目标导向性,加快了路径收敛速度。本文在机器人模型选择中,以圆盘移动机器人为模型,替代质点模型,可更好的匹配真实机器人构型。为适应这一改进,还提出了适用于圆盘机器人的k点碰撞检测算法,该碰撞算法可有效检测新增节点是否为自由构型。本文算法不同于文献[14]中的算法,该算法新增节点是通过搜索一棵树的最近节点和另一棵树的上一节点来生成,而本文中的GOBI-RRT算法新增节点是通过搜索两棵树的最近节点来生成。最后,将本文方法与文献[12]基本BI-RRT算法和文献[14]中的算法作仿真实验对比,验证所提出GOBI-RRT算法在路径规划中的优势。通过实验可知,所提出的方法可以更加快速地寻得路径。本文算法的这一优势,可促进圆盘底座移动机器人能够在较短时间内获取可行路径,节约能源消耗。

1 目标导向的双向快速搜索随机树算法

1.1 BI-RRT算法

在BI-RRT中,定义了两棵随机树Ta和Tb,树Ta以qinit为树的根节点(起始点)开始扩展,树Tb以qgoal为目标点开始扩展,qrand为任意扩展的随机节点,为每次扩展时任选两棵树中距离qnear最近的节点,以qnew为新节点。首先在整个搜索空间中采取随机的方式生成随机树的随即扩展节点qrand,然后遍历当前已有的随机树,从树中的节点寻找距离qrand最近的节点qnear,在qnear向qrand延伸一定步长p之后可以得到新节点qnew,之后需要对新节点qnew进行碰撞检测,若qnew碰到障碍物便将这个节点舍去；反之,即将qnew添加到树中,可知此时qnew的父节点是qnear,按照上述方式继续扩展,直到两棵树的qnew小于一定的步长阈值时,则可确定Ta和Tb连通,即路径规划成功。图1表示BI-RRT算法随机树的生长过程。

图1 BI-RRT算法随机树生长图

1.2 GOBI-RRT算法

1.2.1 改进节点生成方式

基本思想:原方案仅采用随机生成采样点,以树中最近点沿当前方向扩展得到新的节点,该过程主要的计算任务在碰撞检测阶段。所提出的基于目标导向的方案,尽管在目标导向阶段增加了计算量,但节点的选择更具有导向性,使树的生成更偏向目标点。图2表示基于目标导向思想生成新节点的过程。

图2 GOBI-RRT算法生成qnew

改进了BI-RRT算法只生成一个qrand确定qnew,增加目标导向思想是以随机点qrand和树Ta的最近节点qnear生成扩展方向,树的最近节点qnear和树Tb的最近节点qnear'生成扩展方向,再分别以步长p和kp(k为导向系数)生成qrand'和qnear'',最后通过平行四边形法则求新的节点qnew。

y=(yrand-ynear)/(xrand-xnear)*x+b,

(1)

假设qrand'的坐标为(xrand',yrand'),步长p表示为:

(2)

(yrand-ynear)/(xrand-xnear)=

(yrand-ynear)/(xrand-xnear),

(3)

由(2)和(3)得到qrand'的坐标。

假设qnear''的坐标为(xnear'',ynear''),步长kp表示为:

(4)

(ynear'-ynear)/(xnear'-xnear)=

(ynear''-ynear)/(xnear''-xnear),

(5)

由(4)和(5)得到qnear''的坐标。

依据qrand'和qnear''的坐标,qnear的坐标,假设qnew的坐标为(xnew,ynew),依据平行四边形法则可得:

(6)

针对一次节点扩展,利用目标导向得到新节点qnew的计算过程如下:

Step1:给定树Ta的最近点qnear和随机点qrand的坐标,给定树Tb的最近点qnear'的坐标。

Step2:根据给定的步长p和kp求解式(2)、(3)、(4)和(5)得到qrand'、qnear''的坐标。

1.2.2 圆盘k点碰撞检测算法

假设圆盘的圆心坐标为(x0,y0),半径为r。将圆盘以圆心将周长k等分,设圆上任意一点坐标为(xi,yi),i=1,2,3……k,设该点与圆心的连线和平行于x轴的直线y=y0的夹角为α。图3表示圆盘k点碰撞检测算法。

坐标(xi,yi)表示为:

(7)

图3 圆盘k点碰撞检测算法

1.3 复杂度分析

单节点扩展总的计算复杂度由两部分组成:

第一部分:目标导向的复杂度

由式(3)可以得到

yrand'=(yrand-ynear)(xrand-xnear)·

(xrand'-xnear),

(8)

再将yrand'带入到式(2)中得到

p=(xrand'-xnear)2+[(xrand'-xnear)·

(yrand-ynear)(xrand-xnear)-ynear]2。

图4 正、反向生长区

第二部分:圆盘k点碰撞检测复杂度

由式(7),可见求解(xi,yi)包含二次乘法和二次加法,圆盘多点碰撞检测算法的复杂度为2k次乘法和2k次加法。圆盘多点碰撞检测的复杂度为O(n2log2k)。

图5为BI-RRT和GOBI-RRT复杂度对比图,蓝色线为BI-RRT计算复杂度,红色线为GOBI-RRT计算复杂度,与随机采样比较,目标导向可以减少无效随机采样点生成,随着扩展节点数量的增长,通过目标导向思想的算法改进的更明显。

图5 BI-RRT和GOBI-RRT复杂度对比图

2 伪代码实现

2.1 BI-RRT算法

算法1给出了BI-RRT算法的轮廓,首先初始状态被添加到搜索树,主循环是line3-24,在n次迭代后终止。显然,BI-RRT算法通过采样随机点,扩展完树Ta的新节点qnew后,以qnew作为Tb的扩展方向。同时树Tb首先会扩展第一步得到qnew',如果没有碰撞,继续向着相同的方向扩展第二步,直到扩展失败或者qnew=qnew'表示与树Ta相连了,即整个算法结束。每次迭代中必须考虑两棵树的平衡性,即两棵树的节点数的多少(也可以考虑两棵树总共花费的路径长度),交换次序选择“小”的那棵树进行扩展,BI-RRT的构造方法如表1所示。

表1 BI-RRT算法

2.2 GOBI-RRT算法

2.2.1 改进节点生成方式

表2 节点生成

图6 目标导向生成qnew

2.2.2 圆盘k点碰撞检测算法

Checkpath:改进了单点碰撞检测函数,提出了一种圆盘多点碰撞检测算法,具体操作:假设圆盘机器人的圆心为xd,首先将圆盘机器人分割成k等份,本文中取k=50,再对分割出来点的坐标合成一个集合{qi},i=1,2,3…50,对{qi}碰撞检测,来检测圆盘上的点是否在障碍物上,当这50个点都不在障碍物里和地图外时,将qnew加入到路径中。Checkpath构造方法如表3所示。

表3 k点碰撞检测

3 实验与分析

仿真实验是在Windows10,内存为16GB的电脑安装有Mtalab R2019a仿真平台。仿真实验环境设定在不同的环境(宽阔环境、通道环境、栅格环境、迷宫环境)中的路径规划,设置实验空间尺寸为500 m×500 m,起始点坐标设置成(10,10),终点坐标设置成(490,490)。考虑到是现实生活,本文取圆盘移动机器人直径为1 m。分别对文献[12]BI-RRT算法、文献[14]目标偏向BI-RRT算法和本文GOBI-RRT算法在四种不同地图上进行仿真对比,每种对比实验进行50次仿真,取均值进行比较。图7至图10,分别表示宽阔环境、通道环境、栅格环境、迷宫环境下的路径规划图,图 a)表示文献[11],图 b)表示文献[14],图 c)表示本文算法,蓝色线为树Ta的路径规划,红色线为树Tb的路径规划,绿色线为树Ta和树Tb节点的相连。

a)BI-RRT b)目标偏向BI-RRT c)GOBI-RRT

a)BI-RRT b)目标偏向BI-RRT c)GOBI-RRT

a)BI-RRT b)目标偏向BI-RRT c)GOBI-RRT

a)BI-RRT b)目标偏向BI-RRT c)GOBI-RRT

通过在四种不同的环境下的仿真实验,可以明显看出,相比BI-RRT算法和目标偏向BI-RRT算法,GOBI-RRT算法使随机树向着目标点生长,提高了收敛速度并减少了大量的节点。

表4至表7表示分别在宽阔地图、通道地图、栅格地图、迷宫地图四种环境下,选取路径长度、航迹点数、规划时间三方面进行对比并得到的仿真实验数据。上述实验数据,均通过Matlab内运行仿真算法提取。

表4 宽阔地图下的仿真数据比较

在宽阔地图上,GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了53.49%、平均规划时间减少了14.2%,相比目标偏向BI-RRT算法平均轨迹点减少了5%、平均规划时间减少了6.5%。

表5 通道地图下的仿真数据比较

在通道环境上,GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了50%、平均规划时间减少了25.58%,相比目标偏向BI-RRT算法平均轨迹点减少了3.8%、平均规划时间减少了4.78%。

表6 栅格地图下的仿真数据比较

在栅格环境上,GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了51.16%、平均规划时间减少了30.71%,相比目标偏向BI-RRT算法平均轨迹点减少了4.5%、平均规划时间减少了6.6%。

表7 迷宫地图下的仿真数据比较

在迷宫环境上,GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了54.54%、平均规划时间减少了30.5%,相比目标偏向BI-RRT算法平均轨迹点减少了3.8%、平均规划时间减少了10.78%。

由表4至表7可知,GOBI-RRT算法能够在很短的时间里寻得路径。在同样的环境和参数下,本文算法的平均轨迹点的个数约为BI-RRT算法的一半,相比目标偏向BI-RRT算法平均减少了4.28%；在平均规划时间上,本文算法相比BI-RRT算法平均减少了25.25%,相比目标偏向BI-RRT算法平均减少了6.64%。GOBI-RRT算法相比目标偏向BI-RRT算法更具有目标导向性,随着节点数量的增加,GOBI-RRT算法相比BI-RRT算法复杂度得到了优化。因此本文提出的GOBI-RRT算法适用于多种不同环境,能够以更少的搜索节点、更快的收敛速度得到路径,有较大的实用价值。

4 结 论

针对BI-RRT算法路径规划中存在目标导向性差、收敛速度缓慢的问题提出了GOBI-RRT算法。该算法通过目标导向思想对随机树中随机点的产生进行改进,同时和圆盘k点碰撞算法相结合,通过数学模型分析,相比于BI-RRT算法,降低了算法复杂度。在4种不同地图环境的仿真实验中,验证了GOBI-RRT算法在圆盘移动机器人上的优势。通过与BI-RRT算法和目标偏向BI-RRT算法相比,该算法大幅减少了平均航迹点的个数和平均规划时间,在路径规划中,有较大的实用价值。在后续的研究中,将考虑在平均路径长度与路径平滑上进行持续优化。