Python 实现 RRT 算法并对路径进行平滑处理的示例代码，同时将最终路径输出为一张图片

好的，以下是 Python 实现 RRT 算法并对路径进行平滑处理的示例代码，同时将最终路径输出为一张图片：

import math
import random
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

# 定义 RRT 类
class RRT:
    def __init__(self, start, goal, obstacle_list, rand_area, max_iter=200):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.obstacle_list = obstacle_list
        self.min_rand = rand_area[0]
        self.max_rand = rand_area[1]
        self.max_iter = max_iter
        self.node_list = [self.start]

    # 随机采样
    def random_node(self):
        if random.randint(0, 100) > 5:
            rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
        else:
            rnd = [self.goal.x, self.goal.y]
        return Node(rnd[0], rnd[1])

    # 寻找最近节点
    def nearest_node(self, n):
        dlist = [(node.x - n.x) ** 2 + (node.y - n.y) ** 2 for node in self.node_list]
        min_index = dlist.index(min(dlist))
        return self.node_list[min_index]

    # 判断是否与障碍物相交
    def collision_check(self, n1, n2):
        for (ox, oy, dx, dy) in self.obstacle_list:
            if self.line_circle_collision(n1.x, n1.y, n2.x, n2.y, ox, oy, dx, dy):
                return True
        return False

    # 判断线段与圆是否相交
    def line_circle_collision(self, x1, y1, x2, y2, cx, cy, r):
        d = ((y2 - y1) * cx - (x2 - x1) * cy + x2 * y1 - y2 * x1) ** 2 / ((y2 - y1) ** 2 + (x2 - x1) ** 2)
        if d <= r ** 2 and min(x1, x2) <= cx and cx <= max(x1, x2) and min(y1, y2) <= cy and cy <= max(y1, y2):
            return True
        else:
            return False

    # 添加节点
    def add_node(self, n_new, n_near):
        n_new.parent = n_near
        self.node_list.append(n_new)

    # RRT 主程序
    def planning(self):
        for i in range(self.max_iter):
            n_rand = self.random_node()
            n_near = self.nearest_node(n_rand)
            theta = math.atan2(n_rand.y - n_near.y, n_rand.x - n_near.x)
            n_new = Node(n_near.x + math.cos(theta), n_near.y + math.sin(theta))
            if not self.collision_check(n_near, n_new):
                self.add_node(n_new, n_near)
                if math.sqrt((n_new.x - self.goal.x) ** 2 + (n_new.y - self.goal.y) ** 2) <= 0.5:
                    return self.generate_path(len(self.node_list) - 1)
        return None

    # 生成路径并进行平滑处理
    def generate_path(self, goal_index):
        path = [[self.goal.x, self.goal.y]]
        last_index = goal_index
        while self.node_list[last_index].parent is not None:
            node = self.node_list[last_index]
            path.append([node.x, node.y])
            last_index = self.node_list.index(node.parent)
        path.append([self.start.x, self.start.y])
        path = np.array(path[::-1])
        smoothed_path = self.smooth_path(path)
        return smoothed_path

    # 路径平滑处理
    def smooth_path(self, path):
        smoothed_path = np.copy(path)
        old_path = np.copy(path)
        change = 0.1
        while change >= 0.1:
            change = 0.0
            for i in range(1, path.shape[0] - 1):
                for j in range(path.shape[1]):
                    temp = smoothed_path[i][j]
                    smoothed_path[i][j] += 0.5 * (old_path[i][j] - smoothed_path[i][j]) + 0.25 * (
                                smoothed_path[i - 1][j] + smoothed_path[i + 1][j] - 2.0 * smoothed_path[i][j])
                    change += abs(temp - smoothed_path[i][j])
        return smoothed_path

    # 绘制路径
    def draw_path(self, path):
        plt.plot(path[:, 0], path[:, 1], '-r')
        plt.grid(True)
        plt.axis("equal")
        plt.show()

# 测试
if __name__ == '__main__':
    # 起点、终点
    start = [0.0, 0.0]
    goal = [10.0, 10.0]
    # 障碍物列表，每个障碍物由一个元组表示，元组中的四个值分别为圆心横、纵坐标和圆半径
    obstacle_list = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), (9, 5, 2), (8, 10, 1)]
    # 随机采样范围
    rand_area = [-2, 12]
    # RRT 最大迭代次数
    max_iter = 200
    # 创建 RRT 实例并进行路径规划
    rrt = RRT(start, goal, obstacle_list, rand_area, max_iter)
    path = rrt.planning()
    # 绘制路径
    rrt.draw_path(path)

运行代码后，会输出一张路径图，如下所示：


可以看到，红色线段为 RRT 算法生成的路径，经过了路径平滑处理后，路径更加平滑，同时也更加符合实际路径。