人生若只如初见，何事秋风悲画扇

—纳兰性德《木兰花·拟古决绝词柬友》

🏰代码及环境配置：请参考 环境配置和代码运行!

---

3.2.1 RRT算法概述

• 定义与提出者：RRT算法（Rapidly-exploring Random Tree，快速探索随机树）由Steven M. LaValle和James J. Kuffner Jr.提出，通过随机构建Space Filling Tree实现对非凸高维空间的快速搜索。
• 应用场景：广泛应用于各种机器人的运动规划场景，特别是包含障碍物和差分运动约束的环境。
• 算法变种：包括Basic RRT、基于概率的RRT、RRT Connect和RRT算法，其中前三种属于单查询方法，而RRT属于渐近最优算法

3.2.2 RRT算法详解

3.2.2.1 Basic RRT算法

• 基本思想

  将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域时，就得到了从起点位置到目标位置的路径。

• 基本步骤

1. 第3行：将起点 $x_{init}$初始化为搜索树$\mathcal{T}$的根节点。
2. 第5-8行：在构型空间内随机（一般使用均匀分布）生成一个节点$x_{rand}$，并在搜索树$\mathcal{T}$中取出距离采样点$x_{rand}$最近的节点$x_{near}$，然后沿着$x_{near}$到$x_{rand}$方向前进$StepSize$得到$x_{new}$，继而得到边$E_i(x_{new},x_{near})$。
3. 第9-11行：利用$CollisionFree$方法检测该边是否与地图边界和障碍物碰撞。
   1. 若无碰撞，则成功完成一次空间搜索拓展，并将节点$x_{new}$和边$E_i$加到搜索树$\mathcal{T}$中
   2. 若有碰撞，重新构建
4. 第12-13行：若$x_{new}$到达目标点$x_{goal}$，则搜索树$\mathcal{T}$构建完成。

• 优缺点

  • 优点
    • 适用范围广，参数简单，高维空间规划性能优秀
    • 相比较于PRM算法，具有更高的效率
  • 缺点
    • 得到的并非最优路径，且每次搜索的路径具有随机性
    • 在扩展节点时从无障碍区域内随机选择节点，会导致产生部分无用节点，节点利用率低，增加算法随机性的同时也降低了算法的收敛速度

• 参数设置

  RRT算法中非常重要的参数即步长$StepSize$，其极大影响搜索树的形状

  • 步长太大，可能无法成功绕过障碍物，且在终点附近来回跳动
  • 步长太小，搜索树的构建耗时增加，且采样点非常密集

3.2.2.2 基于概率的RRT算法

• 基本思想

  为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树的扩展的步骤中引入一个概率$p$，以一定的概率$p$来选择目标点$x_{goal}$作为随机点。引入向目标生长的机制可以加速路径搜索的收敛速度。

• 基本步骤

  整体步骤和基本RRT相同，只需在选择随机点的时候稍作改变，如下：

  $$x_{rand}\leftarrow ChooseTarget\left(x_{goal},Sample\left(\mathcal{M}\right),p\right)$$

• 优缺点

  • 优点：加速路径搜索的收敛速度
  • 缺点：如果目标概率的不断加大，当障碍物遮挡目标时容易陷入局部搜索无法跳出（随机产生的分支太少了）

• 参数设置
  引入的概率$p$对随机树的扩展有一定的影响：

  • 概率$p$越小($p=0.001$)，树的分支越多（过度随机采样，原始 RRT），会导致生长缺乏方向性
  • 概率$p$越小($p=0.95$)，树的分支越少（过度趋向目标采样，类似最佳优先的贪心搜索），会导致很难绕过障碍物找到目标点

3.2.2.3 RRT Connect

• 基本思想

  RRT-Connect分别以起点和目标点为根节点生成两棵树进行双向扩展，当两棵树建立连接时可认为路径规划成功。通过一次采样得到一个采样点，然后两棵搜索树同时向采样点方向进行扩展，加快两棵树建立连接的速度。

• 基本步骤

  大概流程和Basic RRT类似，主要不同点在于：

  1. 双向扩展
     1. 在每次迭代中，随机选择一个采样点$x_{rand}$ 。
     2. 从随机树${\mathcal{T}}_{start}$和随机树${\mathcal{T}}_{goal}$中分别找到距离采样点$x_{rand}$ 最近的节点$x_{near,start}$ 和$x_{near,goal}$ 。
     3. 分别从$x_{near,start}$ 和$x_{near,goal}$ 向$x_{rand}$ 方向扩展一定步长$StepSize$，得到新的候选节点$x_{new,start}$ 和$x_{new,goal}$ 。
     4. 检查新节点是否与障碍物碰撞，若无碰撞，则将其添加到对应的树中。
  2. 连接检查
     1. 在每次迭代后，检查两棵树是否“连接”，即检查随机树${\mathcal{T}}_{start}$中的任意节点是否与随机树${\mathcal{T}}_{goal}$中的任意节点足够接近（通常是在某个设定的阈值内）。
     2. 如果找到这样的节点对，则通过它们可以构建出一条从起点到目标的路径。

• 优缺点
  • 优点：具有良好的搜索特性，相较于Basic RRT的搜索速度和搜索效率均有显著提高
  • 缺点：单查询算法，最终路径不是最优的

3.2.2.4 RRT*算法

• 基本思想

  Basic RRT，基于概率的RRT和RRT Connect算法虽然能快速的找到路径，但却不是最优路径。而RRT*算法的目标就在于解决RRT算法难以求解最优的可行路径问题，其在路径查找的过程中持续优化路径，随着迭代次数和采样点的增加，得到的路径越来越接近最优。

• 基本步骤

1. 第5-8行：和Basic RRT算法相同，包括随机采样、寻找最近节点、确定新节点和碰撞检测
2. 第9-10行：重新选择父节点，选择标准为以$x_{new}$为圆心，$R$为半径，找到所有潜在的父节点集合，并与父节点$x_{near}$的cost对比（cost为初始节点$x_{init}$到父节点，再到新节点$x_{new}$的总cost），若有更优cost的节点，则将其设为新节点$x_{new}$的父节点。如下图：当路径 { x i n i t → x p o t → x n e w } \left \{ x_{init}\overset{}{\rightarrow}x_{pot}\overset{}{\rightarrow} x_{new}\right \} {xinit​→xpot​→xnew​} 的cost小于路径 { x i n i t → x n e a r → x n e w } \left \{ x_{init}\overset{}{\rightarrow}x_{near}\overset{}{\rightarrow} x_{new}\right \} {xinit​→xnear​→xnew​} 的cost时，算法会将节点$x_{pot}$设为节点$x_{new}$的父节点，并将边$E_i$ 删除，新增边$E_{i1}$ 。

3. 第11行：将新增的节点$x_{new}$和边$E_{i1}$ 加入到随机树中。
4. 第12行：重新对随机树进行布线，如果近邻节点的父节点修改为新增的节点$x_{new}$，可以减少路径代价，则将新节点$x_{new}$设为近邻节点的父节点。如下图：若路径 { x i n i t → x n e a r 1 → x n e a r 3 } \left \{ x_{init}\overset{}{\rightarrow}x_{near1}\overset{}{\rightarrow} x_{near3}\right \} {xinit​→xnear1​→xnear3​} 的cost大于路径 { x i n i t → x n e a r 2 → x n e w → x n e a r s } \left \{ x_{init}\overset{}{\rightarrow}x_{near2}\overset{}{\rightarrow} x_{new}\overset{}{\rightarrow}x_{nears} \right \} {xinit​→xnear2​→xnew​→xnears​}的cost，则将$x_{new}$设置为$x_{near3}$的父节点，并将边 { x n e a r 1 → x n e a r 3 } \left \{ x_{near1} \overset{}{\rightarrow} x_{near3}\right \} {xnear1​→xnear3​}删除，新增边 { x n e w → x n e a r 3 } \left \{ x_{new} \overset{}{\rightarrow} x_{near3}\right \} {xnew​→xnear3​}。

• 优缺点

  • 优点：通过引入重新选择父节点和重新对随机树进行布线步骤，RRT*算法能够不断优化路径，提高路径的质量和长度。这使得最终生成的路径更加平滑，减少了路径中的冗余和不必要的转折。
  • 缺点：与RRT算法相比，RRT*算法需要更多的计算资源和时间来进行路径的优化和重新连接。这可能导致在实时性要求较高的场景中，*算法的性能受到一定影响。

• 参数设置

  半径$R$对算法有一定的影响：

  • $R$太小（$R=0.5$）：每次优化的临近点很少，优化力度太小，路径类似与 RRT算法。
  • $R$太大（$R=500$）：每次优化的邻居点非常多(比如包括起点)，增加计算量，降低搜索效率，而且会引入一些不必要的路径，进而影响路径质量。

3.2.2.4 RRT*的拓展算法（不做详细介绍）

1. Kinodynamic-RRT:* 在传统的RRT算法中，节点$x_{new}$和节点$x_{near}$之间直接使用直线连接，其不满足智能体的动力学约束，因此可以将直线更换为其他曲线（例如贝塞尔曲线等）或者使用优化的方式求路径。
   Kinodynamic RRT: Optimal Motion Planning for Systems with Linear Differential Constraints
   https://www.youtube.com/watch?v=RB3g_GP0-dU
2. Anytime-RRT:* Anytime-RRT算法能够在机器人执行当前轨迹的同时，不断地更新和优化路径树，这意味着算法能够在机器人运动过程中实时地适应环境的变化，找到更优的路径。 由于算法具有实时优化的能力，因此它特别适合在动态环境中使用。在动态环境中，障碍物的位置和形状可能会发生变化，Anytime-RRT算法能够及时地调整路径规划，确保机器人能够安全地到达目的地。
   Anytime-RRT * 算法路径规划演示
3. Informed RRT:* RRT在地图空间中采样进行均匀撒点，采样点会布及整个地图从而导致很多不必要的采样。Informed RRT把采样的范围限制在一个椭圆里面，以起始点和终点作为椭圆的焦点，以 RRT*生成的路径长度 L 作为椭圆上点到焦点的距离之和，在椭圆内进行采样，随着生成的路径越来越优化，长度越来越短，椭圆也会越来越扁，从而集中采样点进行了有效的路径优化。
   Informed RRT: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic*

4. Cross-entropy motion planning(交叉熵运动规划): 利用交叉熵方法（Cross-Entropy Method, CEM）来估计稀有事件概率，并将其与采样运动规划方法（如RRT*）相结合。CEM通过迭代地更新采样分布来指导采样过程，使其更加关注于生成低成本的路径或轨迹。这种自适应采样策略有助于在复杂环境中找到高质量的解决方案。
   Cross-Entropy Randomized Motion Planning

3.2.c RRT代码解析

本节提供了RRT和RRT Star算法的代码测试.

python3 tests/sampling_based_planning/rrt_test.py
python3 tests/sampling_based_planning/rrt_star_test.py

两个算法运行的环境（地图和障碍物分布均相同），并通过函数实现（这一部分可根据自己需求进行修改）：

def construct_env_info():
    # Set map [min, max].
    map_area = [-4, 15]

    # Set obstacle list info [x, y, radius].
    obstacleList = [
        (5, 2, 3),
        (10, 4, 2),
        (3, 12, 2),
        (0, 6, 2),
        (5, 8, 1.5),
        (10, 10, 3),
    ]
    return map_area, obstacleList

3.2.c.1 RRT算法实现

在tests/sampling_based_planning/rrt.py 中给定start point和goal point，在上述构建的地图环境中进行测试，代码如下：

get_random_node : 获取随机点

get_nearest_node_index ：在已有 的node list中找到最近点

steer ：按照一定的步长往前计算得到新节点

check_if_outside_play_area：判断新节点是否出了地图

check_collision ：判断是否节点和边有碰撞

calc_dist_to_goal ：判断是否到终点了，即算法结束

generate_final_course ：使用回溯的方式得到最终的路径

def planning(self, animation=True):
        # rrt path planning.
        self.node_list = [self.start]
        for i in range(self.max_iter):
            rnd_node = self.get_random_node()
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
            nearest_node = self.node_list[nearest_ind]
            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
            if self.check_if_outside_play_area(
                new_node, self.play_area
            ) and self.check_collision(new_node, self.obstacle_list, self.robot_radius):
                self.node_list.append(new_node)

            # Visualization.
            if animation and i % 2 == 0:
                self.draw_graph(rnd_node)

            # Check whether the path has arrived the goal point.
            if (
                self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y)
                <= self.expand_dis
            ):
                final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
                if self.check_collision(
                    final_node, self.obstacle_list, self.robot_radius
                ):
                    return self.generate_final_course(len(self.node_list) - 1)

        return None  # cannot find path

测试结果如下，可以看到RRT算法可以找到一条从起点到终点的路径（但不是最优的）。

3.2.c.2 RRT Star算法实现

RRT Star的算法实现和RRT的算法实现大体相同，为了保证代码的独立性，将其定义在tests/sampling_based_planning/rrt_star.py 中，并在上述构建的地图环境中进行测试，代码如下：

除了与RRT算法中相同的函数，此处不再赘述，其他的函数定义如下：

new_node.cost ：每个节点定义了cost，此处用的是欧式距离

choose_parent ：给新节点重新寻找父节点

rewite ：随机树的重布线

search_best_goal_node ：在当前的迭代情况下，找寻最优的目标节点

def planning(self, animation=True):
        # rrt star path planning
        self.node_list = [self.start]
        for i in range(self.max_iter):
            rnd = self.get_random_node()
            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.check_collision(new_node, self.obstacle_list, self.robot_radius):
                near_inds = self.find_near_nodes(new_node)
                node_with_updated_parent = self.choose_parent(new_node, near_inds)
                if node_with_updated_parent:
                    self.rewite(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 % 2 == 0:
                self.draw_graph(rnd)

            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)

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

        return None

测试结果如下，可以看到RRT Star算法得到的路径相比RRT算法得到的路径更优。

---

🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn