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

摘要

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

主权项

1.一种基于三角形内心引导RRT算法的路径规划方法，提供规划路径的起始点qstart、目标点qgoal、障碍物位置和规划地图，其特征在于，包括以下步骤：/n步骤1：定义步长eps和最大迭代次数n，以qstart为随机树T的根节点，并将其作为下一次扩展随机树的父节点，以qgoal为随机树T的目标节点，三角形内心采样点数Ndc初始值为0，随机点采样数Rdc初始值为0，初始化随机树；/n步骤2：通过随机函数生成随机点qrand的坐标值；/n步骤3：遍历随机树T中的每个节点，通过计算T中节点与随机生成的节点qrand之间的距离，找出离节点qrand距离最小的节点设为qnear；/n步骤4：在Ndc数值小于100的情况下，以随机节点qrand、离节点qrand距离最小的节点qnear及目标节点qgoal三点的坐标值作为三角形三个顶点的坐标值，计算以这三点组成的三角形的内心坐标节点qinh重新赋值给qrand；/n步骤5：从节点qnear向节点qrand方向延伸步长eps，得到新节点qnew；/n步骤6：判断新节点qnew与节点qnear之间是都存在障碍物，若不存在，则将生成的新节点qnew加入的随机树T中，并判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，若在目标点qgoal坐标值步长eps范围内，表明已经找到路径，跳出循环，否者进行下一步；若存在障碍物，则舍弃节点qnew；/n步骤7：更新三角形内心采样点数Ndc及随机点采样数Rdc；/n步骤8：重复步骤2-7，直到节点到达目标点的eps范围内或达到最大迭代次数；/n步骤9：将位于目标节点eps范围内节点作为目标节点的父节点，从目标节点qgoal开始，根据父节点回溯到起点qstart，得到最终路径。/n