融合A*改进RRT算法的路径规划代码仿真 全局路径规划 - RRT算法原理 RRT算法，即快速随机树算法（Rapid Random Tree），是LaValle在1998年首次提出的一种高效的路径规划算法。 RRT算法以初始的一个根节点，通过随机采样的方法在空间搜索，然后添加一个又一个的叶节点来不断扩展随机树。 当目标点进入随机树里面后，随机树扩展立即停止，此时能找到一条从起始点到目标点的路径。 算法的计算过程如下： step1：初始化随机树。 将环境中起点作为随机树搜索的起点，此时树中只包含一个节点即根节点； stpe2：在环境中随机采样。 在环境中随机产生一个点，若该点不在障碍物范围内则计算随机树中所有节点到的欧式距离，并找到距离最近的节点，若在障碍物范围内则重新生成并重复该过程直至找到; stpe3：生成新节点。 在和连线方向，由指向固定生长距离生成一个新的节点，并判断该节点是否在障碍物范围内，若不在障碍物范围内则将添加到随机树 中，否则的话返回step2重新对环境进行随机采样； step4：停止搜索。 当和目标点之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径 。
在路径规划领域，RRT（快速随机树算法，Rapid Random Tree）可谓是一颗耀眼的“明星”算法。它由LaValle在1998年首次提出，是一种极为高效的路径规划算法，主要应用于全局路径规划。
RRT算法原理剖析
RRT算法就像是在一片未知的空间中，从一个起始点开始，像种树一样，不断长出新的“树枝”（叶节点），直到触碰到目标点。
初始化随机树
class Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None def rrt_init(start_x, start_y): start_node = Node(start_x, start_y) tree = [start_node] return tree上述代码定义了一个Node类来表示树中的节点，每个节点包含其坐标x、y以及父节点信息。rrt_init函数则以环境中的起点作为随机树搜索的起点，此时树中仅包含这个根节点。
在环境中随机采样
import random def sample_env(obstacles): while True: sample_x = random.uniform(0, 100) # 假设环境范围是0 - 100 sample_y = random.uniform(0, 100) sample_point = Node(sample_x, sample_y) if not is_in_obstacle(sample_point, obstacles): return sample_point def is_in_obstacle(point, obstacles): for obstacle in obstacles: if (point.x >= obstacle[0] and point.x <= obstacle[2]) and (point.y >= obstacle[1] and point.y <= obstacle[3]): return True return Falsesampleenv函数在环境中随机生成一个点，这里假设环境范围是0 - 100。它通过isin_obstacle函数判断该点是否在障碍物范围内，如果不在则返回该点，否则重新生成，直到找到合适的点。
生成新节点
import math def nearest_node(sample_point, tree): min_dist = float('inf') nearest = None for node in tree: dist = math.sqrt((node.x - sample_point.x) ** 2 + (node.y - sample_point.y) ** 2) if dist < min_dist: min_dist = dist nearest = node return nearest def new_node(nearest, sample_point, step_size): theta = math.atan2(sample_point.y - nearest.y, sample_point.x - nearest.x) new_x = nearest.x + step_size * math.cos(theta) new_y = nearest.y + step_size * math.sin(theta) new_node = Node(new_x, new_y) new_node.parent = nearest return new_node def add_to_tree(tree, new_node): tree.append(new_node) return treenearestnode函数计算随机树中所有节点到采样点的欧式距离，并找到距离最近的节点。newnode函数在最近节点和采样点的连线方向，由最近节点指向采样点，按照固定生长距离stepsize生成一个新的节点，并设置其父节点为最近节点。addto_tree函数将新节点添加到随机树中。
停止搜索
def stop_search(new_node, goal_node, goal_threshold): dist = math.sqrt((new_node.x - goal_node.x) ** 2 + (new_node.y - goal_node.y) ** 2) if dist < goal_threshold: new_node.parent = goal_node return True return Falsestopsearch函数判断新生成的节点和目标点之间的距离是否小于设定的阈值goalthreshold，如果小于则代表随机树已经到达了目标点，将目标点作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径。
然而，RRT算法也并非十全十美，比如其搜索过程具有一定的盲目性，可能导致路径不够优化。这时候，融合A算法来改进RRT算法就显得尤为重要，通过A算法的启发式搜索特性，让RRT算法在扩展树的时候更有“方向感”，从而更快地找到更优路径，后续我们可以进一步深入探讨这种融合改进的实现细节和效果优化。
