Python RRT 算法

RRT（Rapidly-Exploring Random Tree）是一种基于树形结构的路径规划算法。它通过随机采样加上局部搜索的方式，逐步建立起一棵探索树，最终找到一条从起点到终点的路径。

以下是 Python 实现 RRT 算法的步骤：

1. 定义节点数据结构

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

2. 定义 RRT 类和相关函数

class RRT:
    def __init__(self, start, goal, obstacles, max_iter=5000, step_size=0.5, goal_sample_rate=0.05):
        self.start = Node(*start)
        self.goal = Node(*goal)
        self.obstacles = obstacles
        self.max_iter = max_iter
        self.step_size = step_size
        self.goal_sample_rate = goal_sample_rate
        self.nodes = [self.start]
    
    def generate_random_node(self):
        if random.random() < self.goal_sample_rate:
            return self.goal
        else:
            x = random.uniform(0, 10)
            y = random.uniform(0, 10)
            return Node(x, y)
    
    def nearest_node(self, node):
        min_dist = float('inf')
        nearest = None
        for n in self.nodes:
            dist = math.sqrt((node.x - n.x) ** 2 + (node.y - n.y) ** 2)
            if dist < min_dist:
                min_dist = dist
                nearest = n
        return nearest
    
    def collision_free(self, node1, node2):
        for obs in self.obstacles:
            if obs.check_collision(node1, node2):
                return False
        return True
    
    def step_from_to(self, from_node, to_node):
        dist = math.sqrt((from_node.x - to_node.x) ** 2 + (from_node.y - to_node.y) ** 2)
        if dist < self.step_size:
            return to_node
        else:
            theta = math.atan2(to_node.y - from_node.y, to_node.x - from_node.x)
            x = from_node.x + self.step_size * math.cos(theta)
            y = from_node.y + self.step_size * math.sin(theta)
            return Node(x, y)
    
    def search(self):
        for i in range(self.max_iter):
            rand_node = self.generate_random_node()
            nearest_node = self.nearest_node(rand_node)
            new_node = self.step_from_to(nearest_node, rand_node)
            if self.collision_free(nearest_node, new_node):
                new_node.parent = nearest_node
                self.nodes.append(new_node)
                if self.goal.parent:
                    return self.goal
        return None
    
    def get_path(self):
        path = []
        node = self.goal
        while node:
            path.append((node.x, node.y))
            node = node.parent
        return path[::-1]

3. 定义障碍物类和相关函数

class Obstacle:
    def __init__(self, x, y, r):
        self.x = x
        self.y = y
        self.r = r
    
    def check_collision(self, node1, node2):
        x1, y1, x2, y2 = node1.x, node1.y, node2.x, node2.y
        dx, dy = x2 - x1, y2 - y1
        a = dx ** 2 + dy ** 2
        b = 2 * (dx * (x1 - self.x) + dy * (y1 - self.y))
        c = self.x ** 2 + self.y ** 2 + x1 ** 2 + y1 ** 2 - 2 * (self.x * x1 + self.y * y1) - self.r ** 2
        delta = b ** 2 - 4 * a * c
        if delta < 0:
            return False
        t1 = (-b + math.sqrt(delta)) / (2 * a)
        t2 = (-b - math.sqrt(delta)) / (2 * a)
        if (0 <= t1 <= 1) or (0 <= t2 <= 1):
            return True
        else:
            return False

4. 测试 RRT 算法

start = (0, 0)
goal = (10, 10)
obstacles = [Obstacle(5, 5, 1), Obstacle(7, 7, 0.5)]
rrt = RRT(start, goal, obstacles)
goal_node = rrt.search()
if goal_node:
    path = rrt.get_path()
    print('Path found:', path)
else:
    print('Path not found')

参考资料：

- Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research, 30(7), 846-894.