人生若只如初见，何事秋风悲画扇
—纳兰性德《木兰花·拟古决绝词柬友》
🏰代码及环境配置：请参考 环境配置和代码运行!
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算法
基本思想
将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域时，就得到了从起点位置到目标位置的路径。
基本步骤
第3行：将起点 初始化为搜索树的根节点。 第5-8行：在构型空间内随机（一般使用均匀分布）生成一个节点，并在搜索树中取出距离采样点最近的节点，然后沿着到方向前进得到，继而得到边。 第9-11行：利用方法检测该边是否与地图边界和障碍物碰撞。 若无碰撞，则成功完成一次空间搜索拓展，并将节点和边加到搜索树中 若有碰撞，重新构建 第12-13行：若到达目标点，则搜索树构建完成。
优缺点
得到的并非最优路径，且每次搜索的路径具有随机性 在扩展节点时从无障碍区域内随机选择节点，会导致产生部分无用节点，节点利用率低，增加算法随机性的同时也降低了算法的收敛速度 适用范围广，参数简单，高维空间规划性能优秀 相比较于PRM算法，具有更高的效率 优点 缺点 参数设置
RRT算法中非常重要的参数即步长，其极大影响搜索树的形状
步长太大，可能无法成功绕过障碍物，且在终点附近来回跳动 步长太小，搜索树的构建耗时增加，且采样点非常密集
3.2.2.2 基于概率的RRT算法
基本思想
为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树的扩展的步骤中引入一个概率，以一定的概率来选择目标点作为随机点。引入向目标生长的机制可以加速路径搜索的收敛速度。
基本步骤
整体步骤和基本RRT相同，只需在选择随机点的时候稍作改变，如下：
优缺点
优点：加速路径搜索的收敛速度 缺点：如果目标概率的不断加大，当障碍物遮挡目标时容易陷入局部搜索无法跳出（随机产生的分支太少了） 参数设置引入的概率对随机树的扩展有一定的影响：
概率越小()，树的分支越多（过度随机采样，原始 RRT），会导致生长缺乏方向性 概率越小()，树的分支越少（过度趋向目标采样，类似最佳优先的贪心搜索），会导致很难绕过障碍物找到目标点
3.2.2.3 RRT Connect
基本思想
RRT-Connect分别以起点和目标点为根节点生成两棵树进行双向扩展，当两棵树建立连接时可认为路径规划成功。通过一次采样得到一个采样点，然后两棵搜索树同时向采样点方向进行扩展，加快两棵树建立连接的速度。
基本步骤
大概流程和Basic RRT类似，主要不同点在于：
在每次迭代后，检查两棵树是否“连接”，即检查随机树中的任意节点是否与随机树中的任意节点足够接近（通常是在某个设定的阈值内）。 如果找到这样的节点对，则通过它们可以构建出一条从起点到目标的路径。
在每次迭代中，随机选择一个采样点 。 从随机树和随机树中分别找到距离采样点 最近的节点 和 。 分别从 和 向 方向扩展一定步长，得到新的候选节点 和 。 检查新节点是否与障碍物碰撞，若无碰撞，则将其添加到对应的树中。
双向扩展 连接检查
优缺点 优点：具有良好的搜索特性，相较于Basic RRT的搜索速度和搜索效率均有显著提高 缺点：单查询算法，最终路径不是最优的
3.2.2.4 RRT*算法
基本思想
Basic RRT，基于概率的RRT和RRT Connect算法虽然能快速的找到路径，但却不是最优路径。而RRT*算法的目标就在于解决RRT算法难以求解最优的可行路径问题，其在路径查找的过程中持续优化路径，随着迭代次数和采样点的增加，得到的路径越来越接近最优。
基本步骤
第5-8行：和Basic RRT算法相同，包括随机采样、寻找最近节点、确定新节点和碰撞检测 第9-10行：重新选择父节点，选择标准为以为圆心，为半径，找到所有潜在的父节点集合，并与父节点的cost对比（cost为初始节点到父节点，再到新节点的总cost），若有更优cost的节点，则将其设为新节点的父节点。如下图：当路径 的cost小于路径 的cost时，算法会将节点设为节点的父节点，并将边 删除，新增边 。
第11行：将新增的节点和边 加入到随机树中。 第12行：重新对随机树进行布线，如果近邻节点的父节点修改为新增的节点，可以减少路径代价，则将新节点设为近邻节点的父节点。如下图：若路径 的cost大于路径的cost，则将设置为的父节点，并将边删除，新增边。
优缺点
优点：通过引入重新选择父节点和重新对随机树进行布线步骤，RRT*算法能够不断优化路径，提高路径的质量和长度。这使得最终生成的路径更加平滑，减少了路径中的冗余和不必要的转折。 缺点：与RRT算法相比，RRT算法需要更多的计算资源和时间来进行路径的优化和重新连接。这可能导致在实时性要求较高的场景中，算法的性能受到一定影响。 参数设置
半径对算法有一定的影响：
太小（）：每次优化的临近点很少，优化力度太小，路径类似与 RRT算法。 太大（）：每次优化的邻居点非常多(比如包括起点)，增加计算量，降低搜索效率，而且会引入一些不必要的路径，进而影响路径质量。
3.2.2.4 RRT*的拓展算法（不做详细介绍）
Kinodynamic-RRT:* 在传统的RRT算法中，节点和节点之间直接使用直线连接，其不满足智能体的动力学约束，因此可以将直线更换为其他曲线（例如贝塞尔曲线等）或者使用优化的方式求路径。Kinodynamic RRT: Optimal Motion Planning for Systems with Linear Differential Constraintshttps://www.youtube.com/watch?v=RB3g_GP0-dU Anytime-RRT:* Anytime-RRT算法能够在机器人执行当前轨迹的同时，不断地更新和优化路径树，这意味着算法能够在机器人运动过程中实时地适应环境的变化，找到更优的路径。由于算法具有实时优化的能力，因此它特别适合在动态环境中使用。在动态环境中，障碍物的位置和形状可能会发生变化，Anytime-RRT算法能够及时地调整路径规划，确保机器人能够安全地到达目的地。Anytime-RRT * 算法路径规划演示 Informed RRT:* RRT在地图空间中采样进行均匀撒点，采样点会布及整个地图从而导致很多不必要的采样。Informed RRT把采样的范围限制在一个椭圆里面，以起始点和终点作为椭圆的焦点，以 RRT*生成的路径长度 L 作为椭圆上点到焦点的距离之和，在椭圆内进行采样，随着生成的路径越来越优化，长度越来越短，椭圆也会越来越扁，从而集中采样点进行了有效的路径优化。Informed RRT: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic*
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
