路径规划的rrt算法和rrt*算法和rrt_connect算法

### RRT、RRT* 和 RRT_Connect 算法的差异

#### RRT (Rapidly-exploring Random Tree)

RRT是一种基础性的路径规划算法，主要特点是能够在高维空间中高效探索未知区域。该算法通过在配置空间内随机选取样本点并逐步构建一棵由初始位置出发的成长树来实现这一点。每当新增加一个节点时，会检查是否存在通往目标的位置的可能性，并且遵循一定的步长限制以确保每一步都是合理的[^4]。

def rrt_algorithm(start, goal, obstacles):
    tree = {start}
    
    while True:
        random_point = sample_random_point()
        
        nearest_node = find_nearest(tree, random_point)
        new_node = extend_towards(nearest_node, random_point)
        
        if not collide(new_node, obstacles):
            add_to_tree(tree, new_node)
            
            if close_enough(new_node, goal):
                break
                
    return reconstruct_path(tree, start, goal)

#### RRT*

RRT*作为RRT的一个改进版本，在保持原有特性的同时引入了优化机制。这意味着当新节点被加入到生长中的树里时，不仅考虑如何最接近随机选定点，还会评估现有路径的成本以便调整已有的分支使之更优。这种策略使得最终得到的结果不仅是可行解而且尽可能趋向于全局最优解[^3]。

def rrt_star_algorithm(start, goal, obstacles):
    tree = {start: None}  # key is node, value is parent
    
    for _ in range(max_iterations):
        random_point = sample_random_point()
        nearest_node = find_nearest(tree.keys(), random_point)
        new_node = steer(nearest_node, random_point)
        
        if not collide_segment(new_node, nearest_node, obstacles):
            near_nodes = get_nearby_points(tree.keys(), new_node)
            best_parent = choose_best_parent(near_nodes, new_node, obstacles)
            connect_and_update_costs(best_parent, new_node, tree)
            
            rewire_neighbors(near_nodes, new_node, tree)
            
            if close_enough(new_node, goal):
                final_path = backtrack_from_goal(tree, goal)
                
    return final_path or []

#### RRT-Connect

不同于前两者单向增长的方式，RRT-Connect采用了双向搜索的方法——即同时从起点和终点各自建立一颗快速扩张随机树并向对方靠拢直到两棵树成功交汇形成完整的路径解决方案。这种方法显著提高了收敛速度特别是在面对遥远的目标或是狭窄通道等问题场景下表现尤为突出[^1]。

def rrt_connect_algorithm(start, goal, obstacles):
    forward_tree = {start: None}
    backward_tree = {goal: None}

    success = False
    while not success and len(forward_tree) < max_attempts:
        q_rand = sample_free_space(obstacles)
        q_new_forward = grow_rrt(q_rand, forward_tree, obstacles)
        if q_new_forward is not None:
            forward_tree[q_new_forward] = closest_in_tree(forward_tree, q_rand)

        q_new_backward = grow_rrt(q_new_forward, backward_tree, obstacles) if q_new_forward else None
        if q_new_backward is not None:
            backward_tree[q_new_backward] = closest_in_tree(backward_tree, q_new_forward)

        if can_connect(q_new_forward, q_new_backward, obstacles):
            path = combine_paths(forward_tree, backward_tree, q_new_forward, q_new_backward)
            success = True
            
    return path if success else None