当前位置:   article > 正文

从0开始实现RRT路径规划算法(快速扩展随机树原理+python伪代码)

rrt路径规划

一、RRT路径规划算法原理

        Rapidly-Exploring Random Trees (RRT) 路径规划算法是一种常用于机器人运动规划的算法,旨在通过随机采样和快速探索环境来生成可行路径。该算法由 Steven M. LaValle 在 1998 年提出,至今仍被广泛应用于各种机器人领域。

        RRT 算法的核心思想是通过随机采样,在自由空间中快速构建一个树形结构,以探索可能的路径。在每次迭代中,算法随机生成一个节点,并试图将该节点连接到最接近的树节点,形成一条边。通过反复迭代这一过程,不断扩展树的范围,直至达到目标状态或达到最大迭代次数为止。

1.RRT算法流程 

        在初始阶段,将起点Xstart添加到随机树的根节点。然后,在地图空间中随机采样一个坐标点作为随机点Xrand,并找到距离随机点Xrand最近的树节点Xnear。接下来,沿着从Xnear到Xrand的方向扩展一个给定的步长\varepsilon,得到新节点Xnew。

        新节点Xnew的生成分为两种情况:(1)如果Xnear到Xrand的距离大于步长δ,则在Xnear到Xrand的方向上扩展一个步长δ的位置作为Xnew;(2)如果Xnear到Xrand的距离小于等于步长δ,则直接将Xrand作为Xnew的位置。

        然后,检测新节点Xnew是否通过了碰撞检测:如果通过了碰撞检测,则将其添加到随机树的根节点中;如果未通过碰撞检测,则重新迭代以寻找新的节点。

        接着,判断新节点Xnew是否在目标点Xgoal的阈值范围内,若是,则返回根节点,路径规划完成。

  2.RRT算法路径规划过程图

    二、RRT算法伪代码

获取随机节点的代码为:

  1. def generate_random_node(self):
  2. delta = self.utils.delta #delta为机器人半径大小,从而考虑路径宽度,避免与障碍物碰撞
  3. return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
  4. np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) #x_range和y_range为地图长宽大小

 获取临近点的代码为:

  1. @staticmethod
  2. def nearest_neighbor(node_list, n): # node_list为随机树中的全部节点信息,n为随机点
  3. return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
  4. for nd in node_list]))]

获取新节点的代码为:

  1. def new_state(self, node_start, node_end):
  2. dist, theta = self.get_distance_and_angle(node_start, node_end) #node_start为临近点, node_end为随机点。 dist, theta分别为临近点向随机点的距离和角度
  3. dist = min(self.step_len, dist) #self.step_len为扩展步长,找出扩展步长和两点距离的最小值
  4. node_new = Node((node_start.x + dist * math.cos(theta),
  5. node_start.y + dist * math.sin(theta))) #获得新节点
  6. node_new.parent = node_start #将临近点作为新节点的父节点,用以通过父节点,反向找到路径
  7. return node_new
  8. @staticmethod
  9. def get_distance_and_angle(node_start, node_end):
  10. dx = node_end.x - node_start.x
  11. dy = node_end.y - node_start.y
  12. return math.hypot(dx, dy), math.atan2(dy, dx)

新节点在目标点范围内,获得路径的代码为:

  1. def extract_path(self, node_end): #node_end为在目标点范围的新节点
  2. path = [(self.s_goal.x, self.s_goal.y)] #先将目标点加入path列表中
  3. node_now = node_end #将该新节点作为当前节点
  4. while node_now.parent is not None: #判断当前节点的父节点是否存在,如果存在就执行以下内容
  5. node_now = node_now.parent #将当前节点的父节点作为当前节点,目的是重复找父节点,来寻找路径点
  6. path.append((node_now.x, node_now.y)) #将当前节点加入path列表中
  7. return path #重复以上内容,直到当前节点为起点时,没有父节点,结束循环,得到最终路径点
'
运行

、RRT算法仿真结果

(1)简单环境

(2)狭窄环境

在狭窄环境下,这种算法如果没有足够的迭代次数,就很难找到路径。如上图所示。

而给了它足够的迭代次数后,虽然能找到路径,但是算法所用的迭代次数和运行时间相比于在简单环境下,无疑也是大大增加。 如上图所示。

 四、RRT算法优缺点

        从以上两种地图环境可以看出:

        RRT 算法具有几个显著的优点。首先,它适用于高维和复杂环境,能够快速生成可行路径。其次,由于采用了随机采样的策略,因此能够有效应对未知环境和动态障碍物。此外,RRT 算法还能够在运行过程中动态调整树的结构,以适应不同的环境条件。

        然而,RRT 算法也存在一些局限性。由于其随机性质,算法在生成新节点时没有目标引导性,导致算法随机性较强。另外,算法在搜索过程中对空间的搜索范围较大,这样无疑是在浪费运行的计算量和运行时间。还要考虑的是,随机点引导的新节点所生成的路径可能不够优化,导致路径长度较长或存在不必要的曲折。此外,在某些情况下,算法可能会陷入局部最优解,无法找到全局路径,致使算法规划路径成功率较低。

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/空白诗007/article/detail/789422
推荐阅读
相关标签
  

闽ICP备14008679号