【启发式算法】RRT*算法详细介绍（Python）

RRT*算法（Rapidly-exploring Random Tree Star）是一种用于机器人路径规划的算法，旨在为机器人找到从起点到目标的最短路径，同时避免障碍物。它是基于RRT（Rapidly-exploring Random Tree）算法的改进版，具有更高的路径质量和优化能力。RRT*的关键特点是它能够在搜索过程中逐渐优化路径，最终找到一条接近最短的路径。

不去幼儿园

826人浏览 · 2025-06-25 15:18:35

不去幼儿园 · 2025-06-25 15:18:35 发布

📢本篇文章是博主人工智能（AI）领域学习时，用于个人学习、研究或者欣赏使用，并基于博主对相关等领域的一些理解而记录的学习摘录和笔记，若有不当和侵权之处，指出后将会立即改正，还望谅解。文章分类在👉启发式算法专栏：

【启发式算法】（9）---《RRT*算法详细介绍（Python）》

【启发式算法】RRT*算法详细介绍（Python）

1. RRT*算法

2.算法原理

想象你在一个森林里，从某个点出发（起点），想找到一条通向小屋（终点）的路径。森林里有很多树（障碍物），你看不到小屋在哪里。你开始随机地朝不同方向走，记录你走过的路，如果碰到障碍就绕开。

这就是 RRT 的基本思路：“快速而随机地扩展路径”。

而 RRT* 在这个基础上更聪明：每走一步，它还会看一下有没有更短的路线可以替换原来的路线。

**RRT*与RRT的区别**

RRT*算法与普通的RRT算法相比，最大的区别在于优化过程。RRT只能快速探索空间，但不保证找到最优路径。而RRT*则通过不断优化已经找到的路径，保证能找到一条接近最优的路径。

3.算法步骤

RRT*的核心思想是通过不断扩展一个随机树，探索搜索空间，同时在扩展过程中优化路径。算法的基本步骤如下：

**初始化树**：从起点出发，初始化一个树（可以是RRT树），树的根节点是起点。
**随机采样**：在空间中随机采样一个点，这个点是机器人可能需要经过的位置。
**扩展树**：通过找到树中距离该随机采样点最近的节点，然后朝这个随机点扩展一小步。树会不断向外扩展，探索新的区域。
**连接路径**：扩展树的过程中，算法会检查是否能够找到更优的路径。如果新的节点能够连接到树中某个更接近目标的节点，或者可以替换掉原本的路径，那么就会更新树的结构，寻找更短的路径。
**优化路径**：算法会在树扩展的过程中，反复优化路径，以确保最终路径是最优的。

步骤详细说明

假设我们有一个机器人，起点在`(0, 0)`，目标点在`(10, 10)`，空间中有障碍物。我们来一步步看RRT*算法是如何工作的：

1. 初始化树

我们从起点`(0, 0)`开始，构建一个初始树。这个树只有一个节点，即起点。

2. 随机采样

我们从空间中随机选择一个点，假设选择的是`(7, 4)`。这个点并不一定在树上，但我们希望树可以通过某种方式扩展到这个点。

3. 扩展树

找到树中离`(7, 4)`最近的节点，比如起点`(0, 0)`，然后从`(0, 0)`朝着`(7, 4)`方向扩展，假设扩展的步长是1，所以新节点的位置可能是`(1, 0.57)`。这个新节点被加入到树中，树就变得更大了。

4. 检查路径优化

如果新的节点在扩展时发现了更短的路径，或者有可能替代之前的路径，算法就会更新路径。例如，如果扩展的过程中，我们发现从`(1, 0.57)`到目标点`(10, 10)`的路径比原来的路径更短，算法就会更新路径。

5. 重复以上步骤

算法会继续重复以上步骤，直到找到一条从起点到目标点的路径。随着树的扩展，路径不断优化，最终得到一条最短的路径。

6. 结束

当树扩展到目标点附近，算法停止，输出路径。

4.RRT*的关键原理

RRT*的核心原理不仅仅是树的扩展，而是在每次扩展时进行路径优化。为了保证路径的最优性，RRT*会在每次扩展时，检查是否能找到更短的路径。这个过程实际上是一个渐进最优的过程。

1. 树的扩展

树的扩展是RRT*的基本操作，在空间中随机选择一个点，并扩展树的结构。这一过程类似于RRT算法中的“树扩展”步骤，但RRT*在扩展时增加了路径优化步骤。

给定一个随机采样点 $q_{\text{rand}}$ ，树中某个节点 $q_{\text{nearest}}$ ，RRT*的目标是从 $q_{\text{nearest}}$ 朝 $q_{\text{rand}}$ 的 $q_{\text{new}}$

$q_{\text{new}} = q_{\text{nearest}} + \Delta t \cdot \frac{q_{\text{rand}} - q_{\text{nearest}}}{\|q_{\text{rand}} - q_{\text{nearest}}\|}$

其中， $\Delta t$ 是扩展步长， $q_{\text{new}}$ 是新节点。

计算新节点的代价：假设在某一时刻，树中有一个节点 $q_{\text{parent}}$ ，我们从这个节点扩展到新节点 $q_{\text{new}}$ ，则新节点的代价可以定义为：

$C(q_{\text{new}}) = C(q_{\text{parent}}) + \text{cost}(q_{\text{parent}}, q_{\text{new}})$
其中， $\text{cost}(q_{\text{parent}}, q_{\text{new}})$ 是从父节点到新节点的代价，通常计算为两点之间的欧几里得距离：

$\text{cost}(q_{\text{parent}}, q_{\text{new}}) = \|q_{\text{new}} - q_{\text{parent}}\|$

2. 路径优化

在RRT*中，路径优化是一个至关重要的步骤，它能够保证路径的最优性。优化过程的核心思想是检查新扩展的节点是否可以通过某些已有节点来形成更短的路径。具体来说，如果我们有两个节点 $q_{\text{new}}$ 和 $q_{\text{neighbour}}$ ，那么如果经过 $q_{\text{new}}$ 到 $q_{\text{neighbour}}$ 的路径比原有路径更短，就更新路径。

在这个过程中，RRT*需要计算从新节点到树中某些节点的路径长度，并选择能够带来路径优化的节点。这个步骤在每次扩展时都进行，最终产生一条近似最优的路径。

假设新节点 $q_{\text{new}}$ 通过与树中的节点 $q_{\text{neighbour}}$ 连接来优化路径。如果经过 $q_{\text{neighbour}}$ 的路径比原路径更短，则更新路径。即，若有：

$C(q_{\text{neighbour}}) > C(q_{\text{new}}) + \text{cost}(q_{\text{new}}, q_{\text{neighbour}})$

则更新 $q_{\text{neighbour}}$ 的父节点为 $q_{\text{new}}$ ，并更新路径。

3. 连接最短路径

RRT*的优化不仅体现在路径本身，还包括通过连接新节点和树中的某些节点，生成最短路径。假设已经有了一条路径 $\pi_1$ ，从起点到目标点。现在我们希望通过添加新的节点来更新这条路径。若新的路径 $\pi_2$ 比原路径更短，则替换原路径。更新过程可以通过“Rewiring”来完成，即调整树结构，使路径变得更短。

4. 渐进最优性

RRT*的一个显著特征是它的渐进最优性。随着扩展步数的增加，RRT*会不断优化路径，最终趋近于最优路径。这种渐进最优性可以通过以下公式描述：

在给定的空间中，如果我们对树进行足够多的扩展，路径的代价 $C_{\text{path}}$ 会趋近于最优路径的代价 $C^*$ ，满足：

$C_{\text{path}} \to C^* \quad \text{as} \quad n \to \infty$

其中， $n$ $是扩展的次数。

[Python] RRT*算法实现

RRT*算法我也是基于一位大佬的算法应用的，在大佬的代码基础上做了一点修改。

项目代码我已经放入下面链接里面，可以通过下面链接跳转：🔥

【启发式算法】--- RRT*算法

若是下面代码复现困难或者有问题，也欢迎评论区留言。

"""《RRT*算法实现》
    时间：2025.06.25

"""
import math
import os
import sys

import matplotlib.pyplot as plt
from celluloid import Camera  # 保存动图时用，pip install celluloid
sys.path.append("../RRT")
try:
    from rrt_planning import RRT
except ImportError:
    raise

show_animation = True


class RRTStar(RRT):
    """
    Class for RRT Star planning
    """

    class Node(RRT.Node):
        def __init__(self, x, y):
            super().__init__(x, y)
            self.cost = 0.0

    def __init__(self,
            start,
            goal,
            obstacle_list,
            rand_area,
            expand_dis=3.0,
            goal_sample_rate=20,
            max_iter=50000,
            connect_circle_dist=50.0,
            search_until_max_iter=False,
            robot_radius=0.0):
        """
        Setting Parameter

        start:Start Position [x,y]
        goal:Goal Position [x,y]
        obstacleList:obstacle Positions [[x,y,size],...]
        randArea:Random Sampling Area [min,max]

        """
        super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
                          goal_sample_rate, max_iter,
                         robot_radius=robot_radius)
        self.connect_circle_dist = connect_circle_dist
        self.goal_node = self.Node(goal[0], goal[1])
        self.search_until_max_iter = search_until_max_iter

    def planning(self, animation=True, camera=None):
        """
        rrt star path planning

        animation: flag for animation on or off .
        """

        self.node_list = [self.start]
        for i in range(self.max_iter):
            if i % 100 == 0:
                print("Iter:", i, ", number of nodes:", len(self.node_list))
            # print("Iter:", i, ", number of nodes:", len(self.node_list))
            rnd = self.sample_free()
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
            new_node = self.steer(self.node_list[nearest_ind], rnd,
                                  self.expand_dis)
            near_node = self.node_list[nearest_ind]
            # 计算代价,欧氏距离
            new_node.cost = near_node.cost + math.hypot(new_node.x-near_node.x, new_node.y-near_node.y)

            if self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
                near_inds = self.find_near_nodes(new_node) # 找到x_new的邻近节点
                node_with_updated_parent = self.choose_parent(new_node, near_inds) # 重新选择父节点
                # 如果父节点更新了
                if node_with_updated_parent:
                    # 重布线
                    self.rewire(node_with_updated_parent, near_inds)
                    self.node_list.append(node_with_updated_parent)
                else:
                    self.node_list.append(new_node)

            if animation and i % 100 ==0:
                self.draw_graph(rnd, camera)

            if ((not self.search_until_max_iter) and new_node):  # if reaches goal
                last_index = self.search_best_goal_node()
                if last_index is not None:
                    return self.generate_final_course(last_index)

        print("reached max iteration")

        last_index = self.search_best_goal_node()
        if last_index is not None:
            return self.generate_final_course(last_index)

        return None

    def choose_parent(self, new_node, near_inds):
        """
        在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$，
        作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选
	    我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价 加上近邻节点 $X_{near}$ 到 $x_{new}$ 的路径代价，
        取路径代价最小的近邻节点$x_{min}$作为 $x_{new}$ 新的父节点
        Arguments:
        --------
            new_node, Node
                randomly generated node with a path from its neared point
                There are not coalitions between this node and th tree.
            near_inds: list
                Indices of indices of the nodes what are near to new_node
        Returns.
        ------
            Node, a copy of new_node
        """
        if not near_inds:
            return None

        # search nearest cost in near_inds
        costs = []
        for i in near_inds:
            near_node = self.node_list[i]
            t_node = self.steer(near_node, new_node)
            if t_node and self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
                costs.append(self.calc_new_cost(near_node, new_node))
            else:
                costs.append(float("inf"))  # the cost of collision node
        min_cost = min(costs)

        if min_cost == float("inf"):
            print("There is no good path.(min_cost is inf)")
            return None

        min_ind = near_inds[costs.index(min_cost)]
        new_node = self.steer(self.node_list[min_ind], new_node)
        new_node.cost = min_cost

        return new_node

    def search_best_goal_node(self):
        dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
        goal_inds = [
            dist_to_goal_list.index(i) for i in dist_to_goal_list
            if i <= self.expand_dis
        ]

        safe_goal_inds = []
        for goal_ind in goal_inds:
            t_node = self.steer(self.node_list[goal_ind], self.goal_node)
            if self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
                safe_goal_inds.append(goal_ind)

        if not safe_goal_inds:
            return None

        min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
        for i in safe_goal_inds:
            if self.node_list[i].cost == min_cost:
                return i

        return None

    def find_near_nodes(self, new_node):
        """
        1) defines a ball centered on new_node
        2) Returns all nodes of the three that are inside this ball
            Arguments:
            ---------
                new_node: Node
                    new randomly generated node, without collisions between
                    its nearest node
            Returns:
            -------
                list
                    List with the indices of the nodes inside the ball of
                    radius r
        """
        nnode = len(self.node_list) + 1
        r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
        # if expand_dist exists, search vertices in a range no more than
        # expand_dist
        if hasattr(self, 'expand_dis'):
            r = min(r, self.expand_dis)
        dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2
                     for node in self.node_list]
        near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
        return near_inds

    def rewire(self, new_node, near_inds):
        """
            For each node in near_inds, this will check if it is cheaper to
            arrive to them from new_node.
            In such a case, this will re-assign the parent of the nodes in
            near_inds to new_node.
            Parameters:
            ----------
                new_node, Node
                    Node randomly added which can be joined to the tree

                near_inds, list of uints
                    A list of indices of the self.new_node which contains
                    nodes within a circle of a given radius.
            Remark: parent is designated in choose_parent.

        """
        for i in near_inds:
            near_node = self.node_list[i]
            edge_node = self.steer(new_node, near_node)
            if not edge_node:
                continue
            edge_node.cost = self.calc_new_cost(new_node, near_node)

            no_collision = self.obstacle_free(
                edge_node, self.obstacle_list, self.robot_radius)
            improved_cost = near_node.cost > edge_node.cost

            if no_collision and improved_cost:
                near_node.x = edge_node.x
                near_node.y = edge_node.y
                near_node.cost = edge_node.cost
                near_node.path_x = edge_node.path_x
                near_node.path_y = edge_node.path_y
                near_node.parent = edge_node.parent
                self.propagate_cost_to_leaves(new_node)

    def calc_new_cost(self, from_node, to_node):
        d, _ = self.calc_distance_and_angle(from_node, to_node)
        return from_node.cost + d

    def propagate_cost_to_leaves(self, parent_node):

        for node in self.node_list:
            if node.parent == parent_node:
                node.cost = self.calc_new_cost(parent_node, node)
                self.propagate_cost_to_leaves(node)


def main():
    print("Start " )
    fig = plt.figure(1)

    camera = Camera(fig) # 保存动图时使用
    # camera = None # 不保存动图时，camara为None
    show_animation = True
    # ====Search Path with RRT====
    
    # # 生成100个随机元组的列表
    import random
    # obstacle_list = [(random.randint(0, 200), random.randint(0, 200), random.randint(1, 10)) for _ in range(100)]
    # 分别生成100个x、y坐标对和100个size值

    positions = [(random.randint(0, 180), random.randint(0, 180)) for _ in range(100)]
    sizes = [random.randint(1, 10) for _ in range(100)]

    # 合并为100个三元组
    obstacle_list = [(x, y, size) for (x, y), size in zip(positions, sizes)]

    while True:
        goal = [random.randint(180, 199), random.randint(180, 199)]
        goal_ = tuple(goal)
        if goal_ not in positions:
            break

    # Set Initial parameters
    rrt_star = RRTStar(
        start=[0, 0],
        goal=goal,
        rand_area=[0, 200],
        obstacle_list=obstacle_list,
        expand_dis=3,
        robot_radius=0.8)
    path = rrt_star.planning(animation=show_animation,camera=camera)

    if path is None:
        print("Cannot find path")
    else:
        print(f"found path!!,路径长度{len(path)},路径{path}")

        # Draw final path
        if show_animation:
            rrt_star.draw_graph(camera=camera)
            plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
            plt.grid(True)
            if camera!=None:
                camera.snap()
                animation = camera.animate()
                animation.save('trajectory.gif')
    plt.show()


if __name__ == '__main__':

    main()

如果需要完整代码实现，可在下面自行前往大佬的github，链接。

[Results] 运行结果

[Notice] 注意事项

# 环境配置
Python                  3.11.5
torch                   2.1.0
torchvision             0.16.0
gym                     0.26.2

5.优点与不足

优点：

1. **最优性**：通过不断优化路径，RRT\*能够提供最短路径（在有限的计算时间内）。
2. **灵活性**：适用于高维空间和复杂环境，能够处理障碍物的影响。
3. **渐进性**：随着搜索的进行，路径会越来越优化，适合动态环境。

不足：

1. **计算量大**：由于需要在每一步优化路径，计算量较大，可能不适合实时性要求高的应用。
2. **收敛速度慢**：对于复杂环境，算法可能需要较长时间才能找到最优路径。

6.总结

RRT*算法是一种高效的路径规划算法，其关键在于通过路径优化和渐进最优性来生成接近最优的路径。通过在树扩展过程中不断进行路径优化，RRT*能够解决复杂环境中的路径规划问题。它通过多次扩展和优化，在计算资源允许的情况下，可以产生最优或近似最优的路径。RRT*算法特别适用于复杂的环境。虽然计算量较大，但它的最优性使得它在许多应用中仍然很有价值。