一种基于Informed-RRT*的路径规划方法

本发明涉及路径规划，特别涉及一种基于informed-rrt*的路径规划方法。

背景技术：

1、移动机器人的路径规划是指机器人在获取到当前环境的地图之后，规划出一条无碰撞的从起点到终点的最优路径。移动机器人的路径规划在各行业的机器人作业中尤为重要。

2、目前的路劲规划主要基于rrt算法实现，rrt(rapidly-exploring random treesstar)算法是一种传统基于概率的路径搜索的算法，经常用在具有复杂、不规律的障碍物的空间内，找到一条从起点到终点的路径，并且在找到路径后继续进行迭代、枝剪优化，尽可能的使得结果靠近最优解。rrt*算法具有一些独特的优势：rrt*算法可以在整个配置地图中进行路径的搜索，适合于寻找全局的最优路径。rrt*算法相比于rrt算法，增加了枝剪过程，以实现重新连接的核心优化，这使得随着迭代次数的增加，rrt*算法可以逐渐找到近似最优的路径，可以提供相对较优的解决方案。rrt*算法不仅仅适用于低维度的地图，在高维度的配置地图空间中的适应性同样较强，可以适应各种形状、各种大小障碍物的环境。而informed-rrt*算法就是rrt*算法下的一种分支，区别在于将原先无规律的随机采样的策略，加上限制条件，以起点和终点为焦点，建立了一个椭圆的区域，让其随机点完全在椭圆的区域内采样，减少了大量的无效采样点，在rrt*算法的基础上大大增加了算法运行的效率。但是基于informed-rrt*算法的路径规划方法通过随机采样来探索空间，当在狭窄区域时，由于空间受限，障碍物分布密集，随机采样的点很可能落在障碍物中或无法有效引导树的生长，采样点正好在地图的窄距位置的概率很低，因此在寻找最优路径的过程中产生大量无效采样点，最终导致移动机器人在通过窄距区域时路径规划效率低。而随机采样点正好在地图的窄距位置的概率很低还会导致有效采样点少，从而无法有效探索全局路径。

技术实现思路

1、本发明目的是为了解决现有路径规划方法还存在通过窄距区域时无法高效获取全局路径的问题，而提出了一种基于informed-rrt*的路径规划方法。

2、一种基于informed-rrt*的路径规划方法，具体为：

3、步骤一、设置路径规划参数，并将起始节点代价值初始化为0，目标节点代价值初始化为0；

4、所述路径规划参数包括：起始节点坐标，目标节点坐标，扩展节点步长t、起始副节点坐标、目标副节点坐标、采样区域和迭代总次数；

5、步骤二、获取研究区域代价地图，利用研究区域代价地图识别出研究区域中所有窄距区域，并生成偏置点，获得生成偏置点后的研究区域代价地图；

6、步骤三、利用起始节点坐标、起始副节点坐标、目标节点坐标和目标副节点坐标建立正反双向树，并生成新的节点更新正反双向树，判断更新后的正反方向树是否有连接路径，若有连接路径则执行步骤五；若没有连接路径则判断采样点数量是否大于迭代总次数；若采样点数量大于迭代总次数，则输出路径规划失败；若采样点数量小于等于迭代总次数，则执行步骤四；

7、步骤四、利用步骤二生成的偏置节点引导步骤三更新后正反双向树中节点的生长方向，再次更新正反方向树，然后消除偏置点产生的引力和障碍物产生的斥力，并返回步骤三；

8、步骤五、利用步骤三获得的正反方向树获得移动机器人规划路径。

9、进一步地，所述步骤二中的获取研究区域代价地图，利用研究区域代价地图识别出研究区域中所有窄距区域，并生成偏置点，获得生成偏置点后的研究区域代价地图，具体为：

10、步骤二一、获取研究区域地图，按照预设栅格数量对研究区域地图划分栅格，将研究区域地图的左上角作为坐标轴原点，将研究区域地图的横向和纵向方向设为x轴和y轴方向；

11、步骤二二、将研究区域地图中包含障碍物的栅格代价值设置为1，没有障碍物的自由区域栅格的代价值设置为0，从而获得研究区域代价地图；

12、所述包含障碍区的栅格为障碍区；

13、步骤二三、从坐标轴原点开始先按照x方向遍历再按照y方向遍历的顺序遍历研究区域代价地图，从而设置偏置点。

14、进一步地，所述步骤二三中的从坐标轴原点开始先按照x方向遍历再按照y方向遍历的顺序遍历研究区域代价地图，从而设置偏置点，获得生成偏置点后的研究区域代价地图，具体为：

15、步骤二三一、以按照x方向遍历研究区域代价地图的每一列为例，初始化当前列标号i＝1；初始化第i列行中的元素编号j＝1，初始化l＝1；

16、步骤二三二、获取研究区域代价地图中第i列的第j个元素值，第j+1个元素值，若第j个元素值为1且第j+1个元素值为0，则转到步骤二三三；否则，执行步骤二三七；

17、步骤二三三、获取第i列的第j+l个元素值，若第j+l个元素值为0且第j+l+1个元素值为1，则记录第i列中j+1个元素对应的位置ai,j+1和第i列第j+l个元素值对应的位置ai,j+l，然后执行步骤二三四；否则执行步骤二三六；

18、步骤二三四、判断ai,j+1与ai,j+l是否为相同位置坐标，若为相同位置坐标，则在位置ai,j+1设置偏置点，然后执行步骤二三七；否则，获取ai,j+1和ai,j+l之间距离li，执行步骤二三五；

19、步骤二三五、将li与预设窄距参数阈值narrow比较，若li＞narrow，则执行步骤二三七；若li≤narrow，则ai,j+1和ai,j+l之间的区域为窄距区域，在ai,j+1和ai,j+l的中点位置生成偏置点，然后执行步骤二三七；

20、步骤二三六、判断j+l+1是否小于当前列中元素总个数j，若j+l+1＜j，则令l＝l+1，并执行步骤二三三；否则，执行步骤二三八；

21、步骤二三七、判断当前元素标号j是否小于当前列中元素总个数j，若j＜j，则令j＝j+1，并返回步骤二三二；否则，则执行步骤二三八；

22、步骤二三八、判断i是否小于n，若i＜n，则令i＝i+1，并返回步骤二三二；否则，执行步骤二三九；

23、其中，n是研究区域代价地图中的总列数；

24、步骤二三九、判断每列中是否出现了相邻偏置点，若出现相邻偏置点，则获取相邻偏置点中起始偏置点坐标a和终止偏置点坐标b，获取a和b的中点坐标，并删除当前相邻偏置点，将a和b的中点坐标作为偏置点，输出生成偏置点后的研究区域代价地图；否则，直接输出生成偏置点后的研究区域代价地图。

25、进一步地，所述步骤三中的利用起始节点坐标、起始副节点坐标、目标节点坐标和目标副节点坐标建立正反双向树，并生成新的节点更新正反双向树，判断更新后的正反方向树是否有新的连接路径，若有新的连接路径则执行步骤五；若没有新的连接路径则判断采样点数量是否大于迭代总次数；若采样点数量大于等于迭代总次数，则输出路径规划失败；若采样点数量小于迭代总次数，则执行步骤四，具体为：

26、步骤三一、利用起始节点坐标、起始副节点坐标、目标节点坐标和目标副节点坐标建立正反双向树；

27、步骤三二、在采样区域内生成一个新的随机点；

28、步骤三三、在正反方向树上分别获取与新生成随机点最近的节点m、n，利用节点m、n和新生成随机点更新正反方向树；

29、步骤三四、判断更新后的正反方向树是否有新的连接路径，若有新的连接路径则执行步骤五；若没有新的连接路径则判断采样点数量是否大于迭代总次数；若采样点数量大于等于迭代总次数，则输出路径规划失败；若采样点数量小于迭代总次数，则执行步骤四；

30、所述采样点数量为步骤三二生成随机点数量和正反向树生长的新节点的总数量。

31、进一步地，所述步骤三三中的在正反方向树上分别获取与新生成随机点最近的节点m、n，利用节点m、n和新生成随机点更新正反方向树，具体为：

32、步骤三三一、在正反方向树上分别获取与新生成随机点最近的节点m、n，获取新生成随机点与节点m的距离lm，新生成随机点与节点n的距离ln，判断lm、ln分别与扩展节点步长t的大小关系；

33、若lm＜t则将新生成随机点作为新节点，然后执行步骤三三二，否则正方向树按扩展节点步长t生长获得一个新节点，然后执行步骤三三二；

34、若ln＜t则将新生成随机点作为新节点，然后执行步骤三三二，否则反方向树按扩展节点步长t生长获得一个新节点，然后执行步骤三三二；

35、步骤三三二、获取新节点与正方向树中最近的节点之间是否存在障碍区，若存在，则返回步骤三二；否则，把新节点添加到正方向树中，并更新正方向树；

36、获取新节点与反方向树中最近的节点之间是否存在障碍区，若存在，则返回步骤三二；否则，把新节点添加到反方向树中，并更新反方向树。

37、进一步地，所述步骤三四中的判断更新后的正反方向树是否有新的连接路径具体为：判断更新后的正方向树中的节点m'与更新后的反向树中的节点n'之间的距离是否小于等于扩展节点步长t，若m'与n'之间的距离小于等于t，则表示节点m'与节点n'之间的路径为连接路径；否则，表示节点m'与节点n'之间的路径不是连接路径；

38、若当前连接路径为更新后正反方向树后新出现的，则当前连接路径为新的连接路径。

39、进一步地，所述步骤四中的利用步骤二生成的偏置节点引导步骤三更新后正反双向树中节点的生长方向，再次更新正反方向树，然后消除偏置点产生的引力和障碍物产生的斥力，并返回步骤三，具体为：

40、步骤四一、判断步骤三三二生成的新节点q是否落在以偏置点qs(xb,yb)为圆心，以arrange为半径的引力区域；若是，则执行步骤四二；否则，执行步骤三三；

41、其中，arrange是预设引力区域半径；

42、步骤四二、判断当前新节点q是否为第一个落在当前偏置点qs(xb,yb)的引力区域的节点且当前新节点q与当前偏置点qs(xb,yb)的连线上是否没有障碍物，若是，则记录新节点q的坐标(xa,ya)并执行步骤四三；否则，不记录新节点q的坐标，直接执行步骤四三；

43、步骤四三、获取偏置点qs(xb,yb)的引力势场函数uatt(q)和障碍物的斥力势场函数urep(q)；

44、步骤四四、利用偏置点qs(xb,yb)的势场函数获取偏置点的引力梯度函数fatt(q)和障碍物的斥力梯度函数frep(q)；

45、步骤四五、利用偏置点的引力梯度函数和障碍物的斥力梯度函数获取新节点q所受引力和斥力的合力f，并获取合力f的单位向量：

46、首先，利用偏置点的引力梯度函数和障碍物的斥力梯度函数获取新节点q所受引力和斥力的合力f：

47、f＝fatt(q)+frep(q)

48、然后，获取新节点q所受引力和斥力的合力f的欧几里德距离，然后利用合力f的欧几里德距离获取合力f的单位向量；

49、步骤四六、判断合力f是否为0，若f不等于0，则正反方向树朝着f的单位向量方向再次生成新节点记为q'并把q'添加到正反方向树上，执行步骤四七；如果合力f为0无法生成单位向量，则返回步骤三三；

50、步骤四七、判断新节点q'与偏置点qs(xb,yb)是否重合；若重合则执行步骤四九；否则，执行步骤四八；

51、步骤四八、获取与之间的夹角θ，获取当前偏置(xb,yb)点分别到两侧障碍物边缘的夹角θ1、θ2，将θ与θ1、θ2中较大值比较，若θ大于θ1、θ2中较大值，则执行步骤四九，否则执行步骤四六；

52、步骤四九、将当前偏置点qs(xb,yb)和障碍物的势场函数置0，然后令节点q＝q'，并返回步骤四三。

53、进一步地，所述步骤四三中的获取偏置点qs(xb,yb)的引力势场函数uatt(q)和障碍物的斥力势场函数urep(q)，具体为：

54、

55、

56、其中，uatt(q)是采样点q受到引力的势场函数，η是引力势场增益系数，qs是偏置点位置，q是采样点位置，ρ(q,qs)是点q到点qs的欧几里德距离，arrange是预设偏置点位于引力区域边界值，k是斥力势场增益系数，qo是障碍物，ρ(q,qo)是点q到点qo的欧几里德距离，ρ0是斥力边界值，urep(q)是采样点q受到斥力的势场函数。

57、进一步地，所述步骤四四中的利用偏置点(xb,yb)的势场函数获取偏置点的引力梯度函数fatt(q)和障碍物的斥力梯度函数frep(q)，具体为：

58、

59、

60、其中，fatt(q)是采样点q受到引力的梯度函数，frep(q)是采样点q受到斥力的梯度函数。

61、进一步地，所述步骤五中的利用步骤三获得的正反方向树获得移动机器人规划路径，具体为：

62、步骤五一、获取步骤三获得的正反方向树的新的连接路径r及当前连接路径的代价值；

63、步骤五二、将当前连接路径r的代价值与保存的连接路径代价值比较，若当前连接路径r的代价值小于保存的连接路径代价值，则将保存的连接路径代价值更新为当前连接路径的代价值，然后执行步骤五三；否则，直接执行步骤五三；

64、保存的连接路径代价值初值为正无穷；

65、步骤五三、判断获得步骤三输出的正反方向树的采样点数量是否大于迭代总次数；若采样点数量大于等于迭代总次数，则输出记录的代价值对应的连接路径作为规划路径；若采样点数量小于迭代总次数，则返回步骤三二。

66、本发明的有益效果为：

67、本发明提出了一种基于informed-rrt*的路径规划方法，首先采用了窄距地图识别策略，根据窄距地图代价值的变化规律识别出窄距的位置，生成偏置点。通过采样引导策略，在偏置点和障碍物上加入势场函数，将偏置点附近区域内生成的点全部引导至偏置点处，当路径通过偏置点的时候，通过引力消除策略将引力函数初始化为零，以免影响进一步迭代，找到路径后进行枝剪优化，选取最优的路径作为通过窄距区域的路径，从而达到快速通过窄距通道的目的，提升了通过窄距地图时的规划效率，从而能够高效获取全局路径。在本发明的运行过程当中引入双向随机树来加快规划速度，进一步提高了路径规划的效率。本发明采用窄距地图识别策略结合偏置点引入人工势场的方法，提升了有效采样点的数量，从而能够有效获取的全局路径。