基于rrt三维路径规划代码

RRT（Rapidly-exploring Random Trees）是一种常见的路径规划算法之一，它能够在高维空间中快速生成一颗树，并且在树上搜索出一条从起点到终点的路径。在三维空间中，RRT算法也被广泛应用于机器人路径规划、航空器路径规划等领域。

以下是一个基于RRT算法的三维路径规划代码示例：

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class RRT:
    def __init__(self, start, goal, obstacles, step_size, max_iter):
        self.start = start
        self.goal = goal
        self.obstacles = obstacles
        self.step_size = step_size
        self.max_iter = max_iter
        self.vertices = [start]
        self.edges = []
        self.dim = len(start)
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111, projection='3d')
        self.ax.set_xlim3d(-10, 10)
        self.ax.set_ylim3d(-10, 10)
        self.ax.set_zlim3d(-10, 10)
        self.ax.set_xlabel('X')
        self.ax.set_ylabel('Y')
        self.ax.set_zlabel('Z')
        self.ax.scatter(start[0], start[1], start[2], c='g', marker='o')
        self.ax.scatter(goal[0], goal[1], goal[2], c='r', marker='o')
        for obstacle in obstacles:
            self.ax.scatter(obstacle[0], obstacle[1], obstacle[2], c='k', marker='x')

    def run(self):
        for i in range(self.max_iter):
            q_rand = np.random.rand(self.dim) * 20 - 10
            q_near = self.nearest(q_rand)
            q_new = self.steer(q_near, q_rand)
            if self.collision_free(q_near, q_new):
                self.vertices.append(q_new)
                self.edges.append((q_near, q_new))
                self.ax.plot([q_near[0], q_new[0]], [q_near[1], q_new[1]], [q_near[2], q_new[2]], c='b')
                if np.linalg.norm(q_new - self.goal) < self.step_size:
                    self.vertices.append(self.goal)
                    self.edges.append((q_new, self.goal))
                    self.ax.plot([q_new[0], self.goal[0]], [q_new[1], self.goal[1]], [q_new[2], self.goal[2]], c='b')
                    return

    def nearest(self, q_rand):
        distances = [np.linalg.norm(q_rand - v) for v in self.vertices]
        return self.vertices[np.argmin(distances)]

    def steer(self, q_near, q_rand):
        direction = q_rand - q_near
        length = np.linalg.norm(direction)
        if length > self.step_size:
            direction = direction / length * self.step_size
        return q_near + direction

    def collision_free(self, q_near, q_new):
        for obstacle in self.obstacles:
            distance = np.linalg.norm(np.cross(obstacle - q_near, obstacle - q_new)) / np.linalg.norm(q_new - q_near)
            if distance < 1.0:
                return False
        return True

    def visualize(self):
        plt.show()

if __name__ == '__main__':
    start = np.array([-5, -5, -5])
    goal = np.array([5, 5, 5])
    obstacles = [np.array([0, 0, 0])]
    rrt = RRT(start, goal, obstacles, 0.5, 1000)
    rrt.run()
    rrt.visualize()

在这个示例中，我们定义了一个RRT类，它包含了起点、终点、障碍物、步长、最大迭代次数、顶点和边等属性，以及一些方法，例如nearest、steer、collision_free和visualize等。

在run方法中，我们迭代生成随机点，并找到最近的顶点，然后在从最近顶点到随机点的方向上前进一步，生成一个新的顶点。如果新的顶点与最近顶点之间没有障碍物，则将新的顶点添加到树中，并将最近顶点和新的顶点之间的边添加到边列表中。

在collision_free方法中，我们检查最近顶点和新顶点之间是否有障碍物。这里我们假设障碍物为球形，因此使用球面与线段的距离公式来检查是否有碰撞。

最后，我们使用matplotlib库可视化生成的树和路径。