RRT*算法程序代码

RRT*（Rapidly-exploring Random Tree Star）算是一种用于路径规划的算法，它在RRT算法的基上进行了改进，能够更好地优化路径。下面是一个简单的RRT*算法的程序代码示例：

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

class RRTStar:
    def __init__(self, start, goal, obstacles, max_iter=1000, step_size=0.5, goal_sample_rate=0.05, radius=2.0):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.obstacles = obstacles
        self.max_iter = max_iter
        self.step_size = step_size
        self.goal_sample_rate = goal_sample_rate
        self.radius = radius
        self.nodes = []

    def planning(self):
        self.nodes.append(self.start)
        for i in range(self.max_iter):
            rnd_node = self.generate_random_node()
            nearest_node = self.find_nearest_node(rnd_node)
            new_node = self.steer(nearest_node, rnd_node)
            if self.check_collision(new_node):
                near_nodes = self.find_near_nodes(new_node)
                min_cost_node = nearest_node
                min_cost = self.calculate_cost(nearest_node) + self.calculate_distance(nearest_node, new_node)
                for node in near_nodes:
                    cost = self.calculate_cost(node) + self.calculate_distance(node, new_node)
                    if cost < min_cost:
                        min_cost_node = node
                        min_cost = cost
                new_node = self.steer(min_cost_node, new_node)
                self.nodes.append(new_node)
                self.rewire(new_node, near_nodes)
                if self.calculate_distance(new_node, self.goal) <= self.step_size:
                    final_node = self.steer(new_node, self.goal)
                    if self.check_collision(final_node):
                        self.nodes.append(final_node)
                        return self.generate_path()
        return None

    def generate_random_node(self):
        if np.random.rand() < self.goal_sample_rate:
            return Node(self.goal.x, self.goal.y)
        else:
            x = np.random.uniform(0, 10)
            y = np.random.uniform(0, 10)
            return Node(x, y)

    def find_nearest_node(self, rnd_node):
        distances = [self.calculate_distance(node, rnd_node) for node in self.nodes]
        nearest_node = self.nodes[np.argmin(distances)]
        return nearest_node

    def steer(self, from_node, to_node):
        distance = self.calculate_distance(from_node, to_node)
        if distance <= self.step_size:
            return to_node
        else:
            theta = np.arctan2(to_node.y - from_node.y, to_node.x - from_node.x)
            x = from_node.x + self.step_size * np.cos(theta)
            y = from_node.y + self.step_size * np.sin(theta)
            return Node(x, y)

    def check_collision(self, node):
        for obstacle in self.obstacles:
            if self.calculate_distance(node, obstacle) <= self.radius:
                return False
        return True

    def find_near_nodes(self, node):
        distances = [self.calculate_distance(node, n) for n in self.nodes]
        near_nodes = [n for n, d in zip(self.nodes, distances) if d <= 2 * self.radius]
        return near_nodes

    def calculate_cost(self, node):
        cost = 0
        while node.parent:
            cost += self.calculate_distance(node, node.parent)
            node = node.parent
        return cost

    def rewire(self, new_node, near_nodes):
        for node in near_nodes:
            cost = self.calculate_cost(new_node) + self.calculate_distance(new_node, node)
            if cost < self.calculate_cost(node):
                node.parent = new_node

    def calculate_distance(self, node1, node2):
        return np.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)

    def generate_path(self):
        path = []
        node = self.nodes[-1]
        while node:
            path.append([node.x, node.y])
            node = node.parent
        path.reverse()
        return path

# 示例使用
start = (1, 1)
goal = (9, 9)
obstacles = [(5, 5), (7, 7)]
rrt_star = RRTStar(start, goal, obstacles)
path = rrt_star.planning()

if path is not None:
    print("找到路径！")
    for point in path:
        print(point)
else:
    print("未找到路径！")

这段代码实现了一个简单的RRT*算法，其中包括节点类`Node`和RRT*类`RRTStar`。在示例中，我们定义了起点、终点和障碍物，并通过调用`planning`方法来进行路径规划。如果找到了路径，将输出路径上的点坐标；否则，将输出未找到路径的提示。