RRT算法Python

RRT算法（Rapidly-exploring Random Trees）是一种用于路径规划的算法，它通过随机地生成节点并在空间中进行探索，从而构建一棵树形结构。下面是一个示例的Python实现：

import numpy as np
import matplotlib.pyplot as plt

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None

def rrt(start, goal, obstacles, max_iter=1000, max_dist=0.5):
    nodes = [start]
    
    for _ in range(max_iter):
        x_rand = np.random.uniform(0, 10)
        y_rand = np.random.uniform(0, 10)
        node_nearest = None
        min_dist = float('inf')
        
        for node in nodes:
            dist = np.sqrt((node.x - x_rand) ** 2 + (node.y - y_rand) ** 2)
            if dist < min_dist:
                min_dist = dist
                node_nearest = node
        
        x_new = node_nearest.x + max_dist * (x_rand - node_nearest.x) / min_dist
        y_new = node_nearest.y + max_dist * (y_rand - node_nearest.y) / min_dist
        new_node = Node(x_new, y_new)
        new_node.parent = node_nearest
        
        if not collides(new_node, obstacles):
            nodes.append(new_node)
            
            if np.sqrt((new_node.x - goal.x) ** 2 + (new_node.y - goal.y) ** 2) < max_dist:
                goal.parent = new_node
                nodes.append(goal)
                return nodes
    
    return None

def collides(node, obstacles):
    for obstacle in obstacles:
        if np.sqrt((node.x - obstacle[0]) ** 2 + (node.y - obstacle[1]) ** 2) < obstacle[2]:
            return True
    return False

def plot_rrt(nodes, obstacles):
    plt.figure()
    
    for node in nodes:
        if node.parent:
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], 'k-', linewidth=0.5)
    
    for obstacle in obstacles:
        circle = plt.Circle((obstacle[0], obstacle[1]), obstacle[2], color='r')
        plt.gca().add_patch(circle)
    
    plt.xlim(0, 10)
    plt.ylim(0, 10)
    plt.gca().set_aspect('equal', adjustable='box')
    plt.show()

# 示例用法
start = Node(1, 1)
goal = Node(9, 9)
obstacles = [(5, 5, 1), (7, 7, 1)]

path = rrt(start, goal, obstacles)
if path is not None:
    plot_rrt(path, obstacles)
else:
    print('Path not found!')

这段代码实现了基本的RRT算法，使用matplotlib库进行可视化。你可以自定义起点、终点以及障碍物，然后运行代码来生成路径规划结果。