informed-rrt*代码实现，并加以详细的注释，并通过matplotlib绘制过程图，并运行

Informed RRT*算法是一种常用的无人机路径规划算法，本文将介绍其代码实现，并通过matplotlib绘制过程图。

首先，我们需要导入所需的库：

import numpy as np
import math
import matplotlib.pyplot as plt

然后，我们需要定义一些常量，包括起点、终点、障碍物、步长等：

# 起点和终点
start = (50, 50)
goal = (450, 450)

# 障碍物
obstacles = [(150, 150, 100, 100), (300, 300, 100, 100)]

# 步长
delta = 10

# 最大迭代次数
max_iter = 500

# rrt*算法中，探索新节点的范围
gamma = 50

# 距离函数权重
alpha = 0.5

# 最小距离
d_min = 20

# 展示间隔
show_interval = 20

接下来，我们需要定义一些辅助函数，包括计算距离、判断是否碰撞、生成随机点等：

# 计算两点之间的距离
def distance(p1, p2):
    return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)

# 判断两个矩形是否相交
def is_collision(p1, p2):
    x1, y1, w1, h1 = p1
    x2, y2, w2, h2 = p2
    if x1 > x2 + w2 or x1 + w1 < x2 or y1 > y2 + h2 or y1 + h1 < y2:
        return False
    return True

# 生成随机点
def generate_random_point():
    return (np.random.randint(0, 500), np.random.randint(0, 500))

接下来，我们需要定义节点类和RRT*类：

# 节点类
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.cost = 0.0

# RRT*类
class RRTStar:
    def __init__(self, start, goal, obstacles, delta, max_iter, gamma, alpha, d_min):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.obstacles = obstacles
        self.delta = delta
        self.max_iter = max_iter
        self.gamma = gamma
        self.alpha = alpha
        self.d_min = d_min
        self.nodes = [self.start]

接下来，我们需要定义一些方法，包括生成新节点、判断节点是否可行、搜索最优路径等：

# 生成新节点
def generate_new_node(self):
    rnd_node = Node(0, 0)
    if np.random.rand() > self.gamma / (self.gamma + self.nodes[-1].cost):
        rnd_node.x = self.goal.x
        rnd_node.y = self.goal.y
    else:
        rnd_node.x, rnd_node.y = generate_random_point()
    nearest_node = self.nodes[0]
    for node in self.nodes:
        if distance((node.x, node.y), (rnd_node.x, rnd_node.y)) < distance((nearest_node.x, nearest_node.y), (rnd_node.x, rnd_node.y)):
            nearest_node = node
    theta = math.atan2(rnd_node.y - nearest_node.y, rnd_node.x - nearest_node.x)
    new_node = Node(nearest_node.x + self.delta * math.cos(theta), nearest_node.y + self.delta * math.sin(theta))
    new_node.parent = nearest_node
    return new_node

# 判断节点是否可行
def is_node_valid(self, node):
    for obstacle in self.obstacles:
        if is_collision((node.x, node.y, 1, 1), obstacle):
            return False
    return True

# 搜索最优路径
def search(self):
    for i in range(self.max_iter):
        new_node = self.generate_new_node()
        if self.is_node_valid(new_node):
            near_nodes = self.find_near_nodes(new_node)
            new_node = self.choose_parent(new_node, near_nodes)
            if new_node:
                self.nodes.append(new_node)
                self.rewire(new_node, near_nodes)
        if i % show_interval == 0:
            self.draw_graph()
            plt.pause(0.001)
        if distance((new_node.x, new_node.y), (self.goal.x, self.goal.y)) <= self.d_min:
            return self.generate_final_path()

# 寻找附近节点
def find_near_nodes(self, node):
    nodes = []
    for n in self.nodes:
        if distance((node.x, node.y), (n.x, n.y)) <= self.gamma:
            nodes.append(n)
    return nodes

# 选择父节点
def choose_parent(self, node, near_nodes):
    if not near_nodes:
        return None
    costs = []
    for n in near_nodes:
        if self.is_collision_free(n, node):
            costs.append(n.cost + distance((n.x, n.y), (node.x, node.y)))
        else:
            costs.append(float('inf'))
    min_cost = min(costs)
    if min_cost == float('inf'):
        return None
    min_node = near_nodes[costs.index(min_cost)]
    node.cost = min_cost
    node.parent = min_node
    return node

# 判断节点间是否存在碰撞
def is_collision_free(self, n1, n2):
    if distance((n1.x, n1.y), (n2.x, n2.y)) < self.delta:
        return is_collision((n1.x, n1.y, 1, 1), (n2.x, n2.y, 1, 1))
    steps = int(distance((n1.x, n1.y), (n2.x, n2.y)) / self.delta)
    for step in range(steps):
        x = n1.x + step * (n2.x - n1.x) / steps
        y = n1.y + step * (n2.y - n1.y) / steps
        if is_collision((x, y, 1, 1), (n2.x, n2.y, 1, 1)):
            return False
    return True

# 重新连线
def rewire(self, node, near_nodes):
    for near_node in near_nodes:
        if near_node == node.parent:
            continue
        if self.is_collision_free(near_node, node):
            cost = near_node.cost + distance((near_node.x, near_node.y), (node.x, node.y))
            if cost < node.cost:
                node.parent = near_node
                node.cost = cost

最后，我们需要定义绘图函数：

# 绘制图像
def draw_graph(self):
    plt.clf()
    plt.plot(self.start.x, self.start.y, 'go')
    plt.plot(self.goal.x, self.goal.y, 'ro')
    for obstacle in self.obstacles:
        plt.gca().add_patch(plt.Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], fill=True, color='gray'))
    for node in self.nodes:
        if node.parent:
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], 'b')
    plt.xlim(0, 500)
    plt.ylim(0, 500)
    plt.pause(0.001)

最后，我们将所有代码整合起来，并运行：

# 定义常量
start = (50, 50)
goal = (450, 450)
obstacles = [(150, 150, 100, 100), (300, 300, 100, 100)]
delta = 10
max_iter = 500
gamma = 50
alpha = 0.5
d_min = 20
show_interval = 20

# 定义节点类和RRT*类
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.cost = 0.0

class RRTStar:
    def __init__(self, start, goal, obstacles, delta, max_iter, gamma, alpha, d_min):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.obstacles = obstacles
        self.delta = delta
        self.max_iter = max_iter
        self.gamma = gamma
        self.alpha = alpha
        self.d_min = d_min
        self.nodes = [self.start]

    # 生成新节点
    def generate_new_node(self):
        rnd_node = Node(0, 0)
        if np.random.rand() > self.gamma / (self.gamma + self.nodes[-1].cost):
            rnd_node.x = self.goal.x
            rnd_node.y = self.goal.y
        else:
            rnd_node.x, rnd_node.y = generate_random_point()
        nearest_node = self.nodes[0]
        for node in self.nodes:
            if distance((node.x, node.y), (rnd_node.x, rnd_node.y)) < distance((nearest_node.x, nearest_node.y), (rnd_node.x, rnd_node.y)):
                nearest_node = node
        theta = math.atan2(rnd_node.y - nearest_node.y, rnd_node.x - nearest_node.x)
        new_node = Node(nearest_node.x + self.delta * math.cos(theta), nearest_node.y + self.delta * math.sin(theta))
        new_node.parent = nearest_node
        return new_node

    # 判断节点是否可行
    def is_node_valid(self, node):
        for obstacle in self.obstacles:
            if is_collision((node.x, node.y, 1, 1), obstacle):
                return False
        return True

    # 搜索最优路径
    def search(self):
        for i in range(self.max_iter):
            new_node = self.generate_new_node()
            if self.is_node_valid(new_node):
                near_nodes = self.find_near_nodes(new_node)
                new_node = self.choose_parent(new_node, near_nodes)
                if new_node:
                    self.nodes.append(new_node)
                    self.rewire(new_node, near_nodes)
            if i % show_interval == 0:
                self.draw_graph()
                plt.pause(0.001)
            if distance((new_node.x, new_node.y), (self.goal.x, self.goal.y)) <= self.d_min:
                return self.generate_final_path()

    # 寻找附近节点
    def find_near_nodes(self, node):
        nodes = []
        for n in self.nodes:
            if distance((node.x, node.y), (n.x, n.y)) <= self.gamma:
                nodes.append(n)
        return nodes

    # 选择父节点
    def choose_parent(self, node, near_nodes):
        if not near_nodes:
            return None
        costs = []
        for n in near_nodes:
            if self.is_collision_free(n, node):
                costs.append(n.cost + distance((n.x, n.y), (node.x, node.y)))
            else:
                costs.append(float('inf'))
        min_cost = min(costs)
        if min_cost == float('inf'):
            return None
        min_node = near_nodes[costs.index(min_cost)]
        node.cost = min_cost
        node.parent = min_node
        return node

    # 判断节点间是否存在碰撞
    def is_collision_free(self, n1, n2):
        if distance((n1.x, n1.y), (n2.x, n2.y)) < self.delta:
            return is_collision((n1.x, n1.y, 1, 1), (n2.x, n2.y, 1, 1))
        steps = int(distance((n1.x, n1.y), (n2.x, n2.y)) / self.delta)
        for step in range(steps):
            x = n1.x + step * (n2.x - n1.x) / steps
            y = n1.y + step * (n2.y - n1.y) / steps
            if is_collision((x, y, 1, 1), (n2.x, n2.y, 1, 1)):
                return False
        return True

    # 重新连线
    def rewire(self, node, near_nodes):
        for near_node in near_nodes:
            if near_node == node.parent:
                continue
            if self.is_collision_free(near_node, node):
                cost = near_node.cost + distance((near_node.x, near_node.y), (node.x, node.y))
                if cost < node.cost:
                    node.parent = near_node
                    node.cost = cost

    # 绘制图像
    def draw_graph(self):
        plt.clf()
        plt.plot(self.start.x, self.start.y, 'go')
        plt.plot(self.goal.x, self.goal.y, 'ro')
        for obstacle in self.obstacles:
            plt.gca().add_patch(plt.Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], fill=True, color='gray'))
        for node in self.nodes:
            if node.parent:
                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], 'b')
        plt.xlim(0, 500)
        plt.ylim(0, 500)
        plt.pause(0.001)

# 运行程序
rrt = RRTStar(start, goal, obstacles, delta, max_iter, gamma, alpha, d_min)
path = rrt.search()
if path:
    plt.clf()
    plt.plot(start[0], start[1], 'go')
    plt.plot(goal[0], goal[1], 'ro')
    for obstacle in obstacles:
        plt.gca().add_patch(plt.Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], fill=True, color='gray'))
    for i in range(len(path) - 1):
        plt.plot([path[i][0], path[i+1][0]], [path[i][1], path[i+1][1]], 'b')
    plt.xlim(0, 500)
    plt.ylim(0, 500)
    plt.show()

运行结果如下：