inform rrt

### 关于快速探索随机树算法在机器人学或路径规划中的应用

#### 定义与基本原理
快速探索随机树（Rapidly-exploring Random Tree, RRT）是一种用于解决高维空间中运动规划问题的概率完备算法。该方法通过构建一棵从起始位置开始生长的树来逐步覆盖整个配置空间，直到找到到达目标节点的路径为止[^1]。

#### 工作机制
- **初始化**：创建根节点作为起点。
- **迭代过程**：
- 随机选取一个新的样本点。
- 寻找离新选定点最近的一个已有节点。
- 向选定的新方向扩展一小步形成新的子节点并加入到树结构里。

当任意一次生成的新节点恰好位于终点附近时，则认为找到了一条可行解；此时可以沿着父指针回溯得到完整的路径序列[^4]。

#### 特点优势
相比其他传统搜索策略而言，RRT具有如下几个显著优点：

- 对障碍物分布情况不做严格假设，适用于未知动态场景；
- 计算资源消耗相对较低，在线实时调整能力强。

import numpy as np
from scipy.spatial import KDTree


class Node(object):
    """Node class for RRT."""
    
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.path_x = []
        self.path_y = []
        self.parent = None
        
def get_random_point(minimum, maximum):
    return minimum + (maximum-minimum)*np.random.rand(2)

def nearest_node(tree_nodes, rand_point):
    distances = [(node.x-rand_point[0])**2+(node.y-rand_point[1])**2 for node in tree_nodes]
    min_index = int(np.argmin(distances))
    return tree_nodes[min_index]

def steer(from_node, to_node, extend_length=0.5):
    new_node = copy.deepcopy(from_node)
    theta = math.atan2(to_node[1]-from_node.y,to_node[0]-from_node.x)
    new_node.path_x.append(new_node.x)
    new_node.path_y.append(new_node.y)
    
    new_node.x += extend_length * math.cos(theta)
    new_node.y += extend_length * math.sin(theta)
    new_node.path_x.append(new_node.x)
    new_node.path_y.append(new_node.y)
    
    new_node.parent = from_node
    
    return new_node

start_position=(0., 0.)
goal_position=(9., 8.)

rrt_tree=[Node(start_position[0], start_position[1])]
while True:
    rnd=get_random_point(-2., 10.) # Generate random point within map bounds.
    near_n=nearest_node(rrt_tree,rnd) # Find closest existing vertex.
    new_v=steer(near_n,(rnd)) # Create child towards it.

    if check_collision_free((new_v.x,new_v.y)): 
        rrt_tree.append(new_v)
        
        distance_to_goal=np.sqrt(sum([(a-b)**2 for a,b in zip([new_v.x,new_v.y], goal_position)]))

        if distance_to_goal<=extend_dist:
            final_path=retrace_path(new_v)
            break
            
print('Path found:',final_path)