（7-4-02）RRT算法：基于Gazebo仿真的路径规划系统(2)

7.4.4  实现RRT、RRT*和RRT*-FN算法

文件algorithm.py实现了多种RRT路径规划算法，包括RRT、RRT*和RRT*-FN。该算法利用随机采样和树结构构建来探索环境中的可行路径，并通过优化树结构来改进路径的质量。该代码通过图形表示节点和边，并提供了可视化功能来显示算法的执行过程和执行时间。通过迭代的方式，逐步优化树结构，直到找到最优路径或达到最大迭代次数为止。

（1）time_function是一个装饰器，用于计时函数执行时间。当应用于一个函数时，它会在函数执行前记录开始时间，然后在函数执行完毕后记录结束时间，并计算函数执行时间。最后，它会打印出函数执行时间，并返回函数的结果。

def time_function(func):

    @wraps(func)
    def wrapper(*args, **kwargs):
        start_time = perf_counter()

        args = func(*args, **kwargs)

        end_time = perf_counter()
        total_time = round(end_time - start_time, 2)

        print(f"Execution time of {func.__name__}: {total_time}")
        return args

    return wrapper

（2）函数new_node用于生成新的节点，并返回新节点的位置、新节点的ID、最近节点的ID以及新节点与最近节点的距离。函数new_node的具体实现流程如下所示。

1. 生成一个随机节点 q_rand，检查随机节点是否在障碍物内，如果是，则返回 None 值。
2. 创建KD树（2D树）以加速搜索最近节点的过程，并将其传递给 nearest_node_kdtree 函数。
3. 寻找离随机节点最近的节点 q_near，并获取其ID id_near。
4. 如果找不到最近节点或者随机节点与最近节点重合，则返回 None 值。
5. 使用 steer 函数生成新节点 q_new，该函数用于生成从最近节点到新节点的路径，并限制路径长度为 step_length。
6. 将新节点添加到图中，并计算新节点与最近节点的距离。
7. 返回新节点的位置、新节点的ID、最近节点的ID以及新节点与最近节点的距离。

def new_node(G: Graph, map: Map, obstacles: list, step_length: float, bias: float):

    q_rand = G.random_node(bias=bias)  # 生成一个新的随机节点
    if map.is_occupied_c(q_rand):  # 如果新节点的位置在障碍物内
        return None, None, None, None
    potential_vertices_list = list(G.vertices.values())
    kdtree = cKDTree(
        np.array(potential_vertices_list))  # 创建KD树（2D树）并将其传递给nearest_node函数以加快搜索速度
    q_near, id_near = nearest_node_kdtree(G, q_rand, obstacles, kdtree=kdtree)
    if q_near is None or (q_rand == q_near).all():  # 无法将随机节点连接到最近节点（可能因为随机节点与最近节点重合）
        return None, None, None, None
    q_new = steer(q_rand, q_near, step_length)
    id_new = G.add_vertex(q_new)
    distance = calc_distance(q_new, q_near)

    return q_new, id_new, id_near, distance

（3）函数RRT实现了RRT（Rapidly-exploring Random Tree）算法，用于在给定地图上生成路径。该算法通过指定的迭代次数，在图中随机生成节点，并尝试连接到最近的节点，直到达到最大迭代次数或找到路径到达目标节点为止。通过可选参数，可以调整算法的行为，例如允许的最大边长、节点半径、偏向目标节点的程度以及是否实时更新算法过程。

@time_function
def RRT(G: Graph, iter_num: int, map: Map, step_length: float, node_radius: int, bias: float = .0, live_update: bool = False):
    """
    RRT算法。
    :param G: 图
    :param iter_num: 算法迭代次数
    :param map: 地图
    :param step_length: 两个节点之间允许的最大边长
    :param node_radius: 节点的半径
    :param bias: 0-1之间的偏向目标节点的参数
    :param live_update: 布尔值，如果要在图中实时更新算法
    :return: 迭代次数
    """
    pbar = tqdm(total=iter_num)
    obstacles = map.obstacles_c

    iter = 0
    while iter < iter_num:

        q_new, id_new, id_near, distance = new_node(G, map, obstacles, step_length, bias)
        if q_new is None:
            continue
        G.add_edge(id_new, id_near, distance)

        if check_solution(G, q_new, node_radius):
            path, cost = find_path(G, id_new, G.id_vertex[G.start])
            plot_path(G, path, "RRT", cost)
            break

        pbar.update(1)
        iter += 1

        if live_update:
            plt.pause(0.001)
            plt.clf()
            plot_graph(G, map.obstacles_c)
            plt.xlim((-200, 200))
            plt.ylim((-200, 200))

    pbar.close()
    return iter

（4）函数RRT_star实现了RRT*（Rapidly-exploring Random Tree Star）算法，用于在给定地图上生成路径。该算法通过指定的迭代次数，在图中随机生成节点，并尝试连接到最近的节点，直到达到最大迭代次数或找到路径到达目标节点为止。与标准的RRT算法相比，RRT*算法在每次迭代后尝试重新连接节点以改进路径质量，并通过维护一个以节点为中心的半径范围来进行优化。函数的可选参数允许调整算法的行为，例如允许的最大边长、节点半径、偏向目标节点的程度以及是否实时更新算法过程。执行函数RRT_star后，返回迭代次数以及找到的最佳路径及其代价。

@time_function
def RRT_star(G, iter_num, map, step_length, radius, node_radius: int, bias=.0, live_update=False) -> tuple:
    """
    RRT*算法。
    :param G: 图
    :param iter_num: 算法的迭代次数
    :param map: 地图
    :param step_length: 两个节点之间允许的最大边长
    :param radius: 重新连接算法将在其上执行的圆形区域的半径
    :param node_radius: 节点的半径
    :param bias: 0-1之间，偏向目标节点的程度
    :param live_update: 在图上实时显示算法的布尔值
    :return: 迭代次数，具有最佳代价的最佳路径
    """
    pbar = tqdm(total=iter_num)
    obstacles = map.obstacles_c
    best_edge = None
    solution_found = False  # 标志是否已经找到解决方案
    best_path = {"path": [], "cost": float("inf")}  # 具有最小代价的路径及其代价
    finish_nodes_of_path = []  # 找到的路径中最后一个节点的ID
    iter = 0
    while iter < iter_num:
        q_new, id_new, id_near, cost_new_near = new_node(G, map, obstacles, step_length, bias)
        if q_new is None:
            continue
        best_edge = (id_new, id_near, cost_new_near)
        G.cost[id_new] = cost_new_near  # 计算从最近节点到新节点的代价
        G.parent[id_new] = id_near

        # KDTree执行比暴力更好
        kdtree = cKDTree(list(G.vertices.values()))
        choose_parent_kdtree(G, q_new, id_new, best_edge, radius, obstacles, kdtree=kdtree)
        # choose_parent(G, q_new, id_new, best_edge, radius, obstacles)
        G.add_edge(*best_edge)

        # 重新连接
        rewire_kdtree(G, q_new, id_new, radius, obstacles, kdtree=kdtree)
        # rewire(G, q_new, id_new, radius, obstacles)

        # 检查解决方案
        if check_solution(G, q_new, node_radius):
            path, cost = find_path(G, id_new, G.id_vertex[G.start])
            finish_nodes_of_path.append(id_new)
            solution_found = True

            best_path["path"] = path
            best_path["cost"] = cost
            # plot_path(G, path, "RRT_STAR", cost)
            # break

        # 更新路径的代价
        for node in finish_nodes_of_path:
            path, cost = find_path(G, node, G.id_vertex[G.start])
            if cost < best_path["cost"]:
                best_path["path"] = path
                best_path["cost"] = cost

        pbar.update(1)
        iter += 1
        if live_update:
            plt.pause(0.001)
            plt.clf()
            plot_graph(G, map.obstacles_c)
            if solution_found:
                plot_path(G, best_path["path"], "RRT_STAR", best_path["cost"])

    if solution_found:
        plot_path(G, best_path["path"], "RRT_STAR", best_path["cost"])

    pbar.close()

    return iter, best_path

（5）函数RRT_star_FN实现了RRT*FN（Rapidly-exploring Random Tree with Forced Node removal）算法，用于在给定地图上生成路径。在迭代过程中随机生成节点，并尝试将其连接到最近的节点，同时通过强制删除节点来限制节点数量。可通过调整参数来控制算法行为，如最大迭代次数、允许的最大边长、节点半径、偏向目标节点的程度、是否实时更新算法过程以及最大节点数等。

@time_function
def RRT_star_FN(G, iter_num, map, step_length, radius, node_radius: int, max_nodes=200, bias=.0,
                live_update: bool = False):
    """
    RRT star FN算法。
    :param G: 图
    :param iter_num: 算法的迭代次数
    :param map: 地图
    :param step_length: 两个节点之间允许的最大边长
    :param radius: 重新连接算法将在其上执行的圆形区域的半径
    :param node_radius: 节点的半径
    :param max_nodes: 最大节点数
    :param bias: 0-1之间，偏向目标节点的程度
    :param live_update: 在图上实时显示算法的布尔值
    :return: 迭代次数，具有最佳代价的最佳路径
    """

    pbar = tqdm(total=iter_num)
    obstacles = map.obstacles_c
    best_edge = None
    n_of_nodes = 1  # 初始只有起始节点
    solution_found = False  # 标志是否已经找到解决方案
    best_path = {"path": [], "cost": float("inf")}  # 具有最小代价的路径及其代价
    finish_nodes_of_path = []  # 找到的路径中最后一个节点的ID
    iter = 0
    while iter < iter_num:
        q_new, id_new, id_near, cost_new_near = new_node(G, map, obstacles, step_length, bias)
        if q_new is None:
            continue
        best_edge = (id_new, id_near, cost_new_near)
        G.cost[id_new] = cost_new_near  # 计算从最近节点到新节点的代价
        G.parent[id_new] = id_near
        n_of_nodes += 1

        kdtree = cKDTree(list(G.vertices.values()))
        choose_parent_kdtree(G, q_new, id_new, best_edge, radius, obstacles, kdtree=kdtree)
        # choose_parent(G, q_new, id_new, best_edge, radius, obstacles)
        G.add_edge(*best_edge)

        # 重新连接
        rewire_kdtree(G, q_new, id_new, radius, obstacles, kdtree=kdtree)
        # rewire(G, q_new, id_new, radius, obstacles)

        # 如有必要，删除随机的无子节点节点
        if n_of_nodes > max_nodes:
            id_removed = forced_removal(G, id_new, best_path["path"])
            if id_removed in finish_nodes_of_path:
                finish_nodes_of_path.remove(id_removed)
            n_of_nodes -= 1

        # 检查解决方案
        if check_solution(G, q_new, node_radius):
            path, _ = find_path(G, id_new, G.id_vertex[G.start])
            finish_nodes_of_path.append(id_new)
            solution_found = True
            # break

        # 更新路径的代价
        for node in finish_nodes_of_path:
            path, cost = find_path(G, node, G.id_vertex[G.start])
            if cost < best_path["cost"]:
                best_path["path"] = path
                best_path["cost"] = cost

        pbar.update(1)
        iter += 1
        if live_update:
            plt.pause(0.001)
            plt.clf()
            plot_graph(G, map.obstacles_c)
            if solution_found:
                plot_path(G, best_path["path"], "RRT_STAR_FN", best_path["cost"])

    if solution_found:
        plot_path(G, best_path["path"], "RRT_STAR_FN", best_path["cost"])
    pbar.close()
    return iter, best_path