[发明专利]基于三角形内心引导RRT算法的路径规划方法

说明书

本发明涉及一种基于三角形内心引导RRT算法的路径规划方法。本方法为了克服RRT算法存在的缺点及将目标点以一定概率出现在随机点中会导致陷入局部最小的危险,提出利用三角形内心来引导随机树的方法。通过将随机函数生成的随机点、随机树中与该随机点距离最近的点及目标点三点构成三角形的三个顶点，再计算该三角形的内心坐标，用该内心坐标作为随机树的生长方向；另外通过在一定循环次数下，记录使用内心引导的次数及随机树的生长情况来调整采样方式。这样不仅对随机树的生长进行了引导，还有效的避免了陷入局部最小风险。本发明的方法提高了规划的效率，规划路径所需时间更少，迭代次数更少，路径更短。

技术领域

本发明涉及路径规划领域，具体涉及一种基于三角形内心引导RRT算法的路径规划方法。

背景技术

路径规划问题一直以来都是移动机器人领域的研究热点。路径规划主要任务就是在空间中找到一条从初始状态点到目标状态点的连续无碰撞路径，同时满足相应的条件约束。

常用的轨迹规划算法有图搜索算法、仿生智能算法、人工势场法等。传统的路径规划算法，如人工势场法，该方法基于矢量合成，通过障碍物对移动机器人的排斥力和目标位置对移动机器人的吸引力的合力作用下规划机器人的运动路径，但当吸引力与斥力的合力为零时，机器人就陷入了局部最小的情况。

RRT(快速搜索随机树)算法作为一种基于采样的增量式快速搜索算法，可以直接用于非完整性规划和动力学规划中。但由于基本RRT算法采用均匀采样的策略，缺乏引导信息，由此得出的路径迂回曲折，在较复杂的障碍物分布下，规划时间长；另外，对于将目标点以一定概率作为随机点出现在采样点中，这样虽然可以提高规划效率，但也可能导致陷入局部最小，最终在规定时间或迭代次数下找不到路径。因此，对于以上问题，急需提出一种既能够引导RRT算法快速找到路径，又应当避免陷入局部最小风险的算法。

发明内容

有鉴于此，本发明的目的在于提供一种基于三角形内心引导RRT算法的路径规划方法，克服当前RRT算法进行路径规划存在的缺点，在引入三角形内心降低搜索的随机性，提高收敛速率，另外通过对利用三角形内心进行采样点的数量和随机采样点的数量控制，可以有效避免陷入地图陷阱中，使得算法能够快速稳定的规划出相应路径。

为实现上述目的，本发明采用如下技术方案：

一种基于三角形内心引导RRT算法的路径规划方法，提供规划路径的起始点qstart、目标点qgoal、障碍物位置和规划地图，包括以下步骤：

步骤1：定义步长eps和最大迭代次数n，以qstart为随机树T的根节点，并将其作为下一次扩展随机树的父节点，以qgoal为随机树T的目标节点，三角形内心采样点数Ndc初始值为0，随机点采样数Rdc初始值为0，初始化随机树；

步骤2：通过随机函数生成随机点qrand的坐标值；

步骤3：遍历随机树T中的每个节点，通过计算T中节点与随机生成的节点qrand之间的距离，找出离节点qrand距离最小的节点设为qnear；

步骤4：在Ndc数值小于100的情况下，以随机节点qrand、离节点qrand距离最小的节点qnear及目标节点qgoal三点的坐标值作为三角形三个顶点的坐标值，计算以这三点组成的三角形的内心坐标节点qinh重新赋值给qrand；

步骤5：从节点qnear向节点qrand方向延伸步长eps，得到新节点qnew；

步骤6：判断新节点qnew与节点qnear之间是都存在障碍物，若不存在，则将生成的新节点qnew加入的随机树T中，并判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，若在目标点qgoal坐标值步长eps范围内，表明已经找到路径，跳出循环，否者进行下一步；若存在障碍物，则舍弃节点qnew；

步骤7：更新三角形内心采样点数Ndc及随机点采样数Rdc；