CN108444489A - 一种改进rrt算法的路径规划方法 - Google Patents

Abstract

本发明涉及一种改进RRT算法的路径规划方法，属于机器人路径规划技术领域。本方法为克服RRT算法存在的缺点引入目标引力策略。随机树在扩展过程中当遇到障碍物进行随机扩展；当没有遇到障碍物时，引入目标引力策略修正随机树的扩展方向；引入双向扩展方法，分别从起始点和目标点进行扩展。改进的方法提高了规划效率，使规划出的路径更加平滑。最后对规划好的路径进行平滑处理，使路径更加平滑。

Description

一种改进RRT算法的路径规划方法

技术领域

本发明涉及一种改进RRT算法的路径规划方法，属于机器人路径规划技术领域。

背景技术

机器人路径规划是智能移动机器人研究的核心内容之一，路径规划是移动机器人导航技术中不可缺少的部分。所谓的路径规划就是指机器人按照外界环境信息，寻找一条能够安全到达目的地的路径。它是移动机器人智能化程度的重要标志，并且路径规划算法是目前研究领域中最重要的部分，它不仅可以有效地改善机器人导航技术，而且可以增加移动机器人的灵活性。

路径规划起始于20世纪70年代，迄今为止己有大量的研究成果报道。部分学者从机器人对环境感知的角度，将移动机器人路径规划方法分为三种类型：基于环境模型的规划方法、基于事例学习的规划方法和基于行为的路径规划方法；从机器人路径规划的目标范围看，又可分为全局路径规划和局部路径规划；从规划环境是否随时间变化方面看，还可分为静态路径规划和动态路径规划。

针对路径规划问题，曾经提出许多算法，如势场法、栅格法、仿生算法、A*算法等。传统的路径规划算法，如人工势场法为，它采用的是基于矢量合成的方法，通过障碍物对小车的排斥力与目标点对小车的吸引力的合力作用下规划机器人的运动路径。在已知局部信息的情况下，人工势场避障策略十分有效，但当吸引力与斥力的合力为零时，小车就陷入了陷入局部极小的情况。

RRT(Rapidly Random-exploring Trees)是一种数据结构和算法，可直接应用于非完整性规划和动力学规划中。由于其采用随机采样的规划方法,不需要预处理,而且搜索速度快,尤其在高维空间中速度优势尤为明显,因此这种算法得到了很多研究者的青睐。但是RRT算法具有较强的采样随机性，所以它规划路径用时较长，实时性不高且很难规划出最优路径。

本发明提出了一种路径规划方法，本方法为克服RRT算法存在的缺点引入目标引力策略。随机树在扩展过程中当遇到障碍物进行随机扩展；当没有遇到障碍物时，引入目标引力策略修正随机树的扩展方向；引入双向扩展方法，分别从起始点和目标点进行扩展。改进的方法提高了规划效率，使规划出的路径更加平滑。最后对规划好的路径进行平滑处理，使路径更加平滑。

发明内容

1、发明目的

本发明目的为克服RRT算法存在的缺点，并提供一种快速、稳定的路径规划方法。

2、技术方案

本发明所要解决的技术问题是一种机器人全局路径规划方法，该方法包括如下步骤：

准备工作：机器人的起始点X_init、目标点X_goal、障碍物的位置和规划环境范围已知。

步骤一：对随机树T_init进行扩展，随机树T_init将X_init作为起始点，将X_goal作为目标点。并将X_init作为本次扩展的父节点。

步骤二：从当前定义的父节点开始扩展，在自由空间C_free中随机选择一个探索点X_rand，然后在当前随机树中找到距离X_rand最近的点X_near。

步骤三：在点X_rand和点X_near的连线上找一点X_new，要求D(X_new,X_rand)≤ρ。当遇到障碍物时转至步骤四，当没有遇到障碍物时转至步骤五。

步骤四：当遇到障碍物时，新节点X_new的计算公式如下所示：

此时令参数k_g＝0。转至步骤六。

步骤五：当没有遇到障碍物时，新节点X_new的计算公式如下所示：

此时令参数k_g＝1.5。转至步骤六。

步骤六：计算生成随机树T_init的新节点X_new，令X_new,1＝X_new，更新随机树T_init，将X_new,1作为随机树T_init下一次扩展的父节点。

步骤七：对随机树T_goal进行扩展，随机树T_goal将X_goal作为起始点，将X_init作为目标点。并将X_init作为本次扩展的父节点。

步骤八：从当前定义的父节点开始扩展，在自由空间C_free中随机选择一个探索点X_rand，然后在当前随机树中找到距离X_rand最近的点X_near。

步骤九：在点X_rand和点X_near的连线上找一点X_new，要求D(X_new,X_rand)≤ρ。当遇到障碍物时转至步骤十，当没有遇到障碍物时转至步骤十一。

步骤十：当遇到障碍物时，新节点X_new运用步骤四中的公式(1)进行计算。此时令参数k_g＝0。转至步骤十二。

步骤十一：当没有遇到障碍物时，新节点X_new的运用步骤五中的公式(2)进行计算，此时令参数k_g＝1.5。转至步骤十二。

步骤十二：计算生成随机树T_init的新节点X_new，令X_new,2＝X_new，更新随机树T_goal，将X_new,2作为随机树T_goal下一次扩展的父节点。

步骤十三：随机树T_init当前的父节点为X_new,1，重复步骤二至步骤六后，转至步骤十四。

步骤十四：随机树T_goal当前的父节点为X_new,2，重复步骤八至步骤十二后，转至步骤十五。

步骤十五：计算两个随机树的新节点X_new,1和X_new,2间的欧氏距离D(X_new，1,X_new,2)。

步骤十六：如果D(X_new，1,X_new,2)>τ，重复步骤十三到步骤十四，使随机树T_init和随机树T_goal继续交替扩展。如果D(X_new，1,X_new,2)≤τ，认为随机树T_init和随机树T_goal相遇，路径规划完成。

步骤十七：综合随机树T_init和随机树T_goal规划好的路径点，得到路径x₁(x_start),x₂,x₃,...,x_n(x_goal)。

步骤十八：连接点x_i与点x_i+2(i＝1,2,...,n)，如果这两点间的连线不碰到任何障碍物则放弃点x_i+1，判断所有的点直到点x_n(x_goal)。

补充说明：

其中，在步骤二中提到的自由空间C_free为机器人避障环境中无障碍物的空间，即安全空间。

其中，在步骤三中的参数ρ(ρ>0)为随机树扩展的最小单位长度，即步长。它决定了随机树每次扩展的长度，应根据障碍环境的情况而定，步长一般为障碍环境的1/20—1/10。

其中，步骤五中，计算新节点时应用目标引力策略对随机树扩展方向进行修正，公式可写为：

F(n)＝R(n)+G(n) (3)

上式中，n代表第n个新节点，F(n)代表随机树遇到障碍物时的扩展函数，R(n)代表随机树的随机扩展函数，G(n)代表目标引力函数。随机扩展函数R(n)为。

目标引力函数G(n)为：

其中，在步骤四和步骤五中，计算新节点X_new时，参数k_g的选择与扩展时当前点与障碍物的位置有关，距离近时(距离≤步长)认为遇到障碍物，距离远时(距离>步长)认为没有遇到障碍物。k_g的选择方式如下所示：

其中，在步骤六和步骤十二中，为区分两个随机树新生成的节点，令X_new，1为随机树T_init最新生成的节点，X_new,2为随机树T_goal最新生成的节点。

其中，在步骤六、步骤十二中，随机树的更新函数为：

T＝T+X_new (7)

上式中，T为当前随机树，X_new为新生的节点(随机树T_init对应的新节点为X_new，1，随机树T_goal对应的新节点为X_new,2)。

其中，在步骤十六中，τ为判断两个随机树是否相遇的参数，τ选择过大使规划出的路径不安全，τ选择过小增加路径规划不必要的时间。

其中，在步骤三、步骤九、步骤十五和步骤十六中，公式D(a,b)为计算点a和点b间的欧氏距离。按如下方式计算：

上式中，ax和bx分别为a点和b点对应的x轴上的坐标，ay和by分别为a点和b点对应的y轴上的坐标。

3、特色与优势

本发明引入目标引力策略修正随机树的扩展方向，并运用双向RRT减少了规划时间。改进了传统RRT算法规划时不稳定、无方向等缺点，提高了规划的效率。本发明同样可运用到航空、航天、航海、无人汽车等领域的路径规划。

附图说明

图1是本发明的总流程图；

图2是双向RRT算法的示意图；

图3是目标引力策略矫正随机树扩展方向的示意图；

图4是平滑路径的示意图；

图5是运用改进RRT算法进行路径规划的结果图；

图6是对规划好的路径进行平滑的结果图。

具体实施方式

下面结合具体实施过程对本发明进行进一步描述，以图5的环境为例进行说明：

准备工作：规划环境的范围为400×400，设机器人的起始点为X_init＝(50,0)，目标点为X_goal＝(350,400)。障碍物的位置如图5所示，图中含有三个方形障碍物和三个圆形障碍物，方形障碍物位置为：方形障碍物1[0,250,250,0；120,120,170,170]，方形障碍物2[0,100,100,0；275,275,325,325]。上述方形障碍物每个顶点的坐标为(x_i,y_i),i＝1,2,...n，障碍物的位置为[x₁,x₂,...,x_n；y₁,y₂,...,y_n]，其中x₁,x₂,...,x_n为障碍物的横坐标，y₁,y₂,...,y_n为障碍物的纵坐标。圆形障碍物的位置：圆形障碍物1的圆心为(50,100)半径为25，圆形障碍物2的圆心为(350,100)半径为40，圆形障碍物3的圆心为(200,350)半径为40。随机树扩展的最小单位长度ρ＝10。具体步骤如下所示：

步骤一：先扩展随机树T_init，将X_init＝(50,0)作为随机树的起始点，将X_goal＝(350,400)作为随机树的目标点。将X_init作为本次扩展的父节点。

步骤二：从父节点(50,0)开始扩展，在无障碍的自由空间C_free中随机选择一个探索点X_rand，并在随机树T_init中找到距离X_rand最近的点X_near。

步骤三：在点X_rand和点X_near的连线上找一点X_new，要求D(X_new,X_rand)≤10。随机树在扩展时，如果遇到障碍物转到步骤四，如果没有遇到障碍物转至步骤五。

步骤四：当遇到障碍物时，令公式中的参数k_g＝0。计算公式如下所示：

转至步骤六。

步骤五：当没有遇到障碍物时，令公式中的参数k_g＝1.5。计算公式如下所示：

示意图如图3所示，运用目标引力对随机树扩展方向进行矫正。转至步骤六。

步骤六：计算生成随机树T_init的新节点X_new。

步骤七：令X_new,1＝X_new，更新随机树T_init(T_init＝T_init+X_new，1)，并将X_new,1作为随机树T_init下一次扩展的父节点。

步骤八：对随机树T_goal进行扩展，将X_init＝(350,400)作为随机树起始点，将X_goal＝(50,0)作为随机树目标点。将X_init作为本次扩展的父节点。

步骤九：从当前父节点开始扩展，在无障碍的自由空间C_free中随机选择一个探索点X_rand，并在当前随机树中找到距离X_rand最近的点X_near。

步骤十：在点X_rand和点X_near的连线上找一点X_new，要求D(X_new,X_rand)≤10。随机树在扩展时，如果遇到障碍物转至步骤十一，如果没有遇到障碍物转至步骤十二。

步骤十一：当遇到障碍物时，令公式中的参数k_g＝0。计算公式如下所示：

转至步骤十三。

步骤十二：当没有遇到障碍物时，令公式中的参数k_g＝1.5。计算公式如下所示：

转至步骤十三。

步骤十三：计算生成随机树T_goal的新节点X_new。

步骤十四：令X_new,2＝X_new，更新随机树T_goal(T_goal＝T_goal+X_new，2)，并将X_new,2作为随机树T_goal下一次扩展的父节点。

步骤十五：计算两个新节点X_new,1和X_new,2间的欧氏距离D(X_new，1,X_new,2)。

步骤十六：如果D(X_new，1,X_new,2)>10时，重复步骤二到步骤十五，使两个随机树T_init和T_goal继续交替扩展。随机树交替扩展示意图如图2所示。如果D(X_new，1,X_new,2)≤10时，认为两个随机树T_init和T_goal相遇，路径规划完成。

步骤十七：综合随机树T_init和随机树T_goal规划好的路径点，得到路径x₁(x_start),x₂,x₃,...,x_n(x_goal)。如图5图中的路线，该路线由多个路径点组成。

步骤十八：对规划好的路径进行平滑，连接点x_i与点x_i+2(i＝1,2,...,n)，如果这两点间的连线不碰到任何障碍物则放弃点x_i+1,如图4所示。判断所有的点直到点x_n(x_goal)。如图6图中展示了平滑后得到的路径。

Claims (1)

1.一种改进RRT算法的路径规划方法，机器人的起始点X_init、目标点X_goal、障碍物的位置和规划环境范围已知；

其特征在于，该方法包括如下步骤：

步骤一：对随机树T_init进行扩展，随机树T_init将X_init作为起始点，将X_goal作为目标点；并将X_init作为本次扩展的父节点；

步骤二：从当前定义的父节点开始扩展，在自由空间C_free中随机选择一个探索点X_rand，然后在当前随机树中找到距离X_rand最近的点X_near；

步骤三：在点X_rand和点X_near的连线上找一点X_new，要求D(X_new,X_rand)≤ρ；当遇到障碍物时转至步骤四，当没有遇到障碍物时转至步骤五；

步骤四：当遇到障碍物时，新节点X_new的计算公式如下所示：

此时令参数k_g＝0；转至步骤六；

步骤五：当没有遇到障碍物时，新节点X_new的计算公式如下所示：

此时令参数k_g＝1.5；转至步骤六；

步骤六：计算生成随机树T_init的新节点X_new，令X_new,1＝X_new，更新随机树T_init，将X_new,1作为随机树T_init下一次扩展的父节点；

步骤七：对随机树T_goal进行扩展，随机树T_goal将X_goal作为起始点，将X_init作为目标点；并将X_init作为本次扩展的父节点；

步骤八：从当前定义的父节点开始扩展，在自由空间C_free中随机选择一个探索点X_rand，然后在当前随机树中找到距离X_rand最近的点X_near；

步骤九：在点X_rand和点X_near的连线上找一点X_new，要求D(X_new,X_rand)≤ρ；当遇到障碍物时转至步骤十，当没有遇到障碍物时转至步骤十一；

步骤十：当遇到障碍物时，新节点X_new运用步骤四中的公式(1)进行计算；此时令参数k_g＝0；转至步骤十二；

步骤十一：当没有遇到障碍物时，新节点X_new的运用步骤五中的公式(2)进行计算，此时令参数k_g＝1.5；转至步骤十二；

步骤十二：计算生成随机树T_init的新节点X_new，令X_new,2＝X_new，更新随机树T_goal，将X_new,2作为随机树T_goal下一次扩展的父节点；

步骤十三：随机树T_init当前的父节点为X_new,1，重复步骤二至步骤六后，转至步骤十四；

步骤十四：随机树T_goal当前的父节点为X_new,2，重复步骤八至步骤十二后，转至步骤十五；

步骤十五：计算两个随机树的新节点X_new,1和X_new,2间的欧氏距离D(X_new，1,X_new,2)；

步骤十六：如果D(X_new，1,X_new,2)>τ，重复步骤十三到步骤十四，使随机树T_init和随机树T_goal继续交替扩展；如果D(X_new，1,X_new,2)≤τ，认为随机树T_init和随机树T_goal相遇，路径规划完成；

步骤十七：综合随机树T_init和随机树T_goal规划好的路径点，得到路径x₁(x_start),x₂,x₃,...,x_n(x_goal)；

步骤十八：连接点x_i与点x_i+2(i＝1,2,...,n)，n为路径上点的总数；如果这两点间的连线不碰到任何障碍物则放弃点x_i+1，判断所有的点直到点x_n(x_goal)。