好的，下面是一个完整的 RRT (Rapidly-exploring Random Tree) 算法的 Python 实现，包含了初始化环境、树的扩展、障碍物检测以及路径绘制的完整代码。

完整代码：RRT算法实现

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

# 定义环境尺寸
WIDTH = 100
HEIGHT = 100

# 障碍物定义
# 格式: (x, y, 半径)
obstacles = [(30, 30, 20), (60, 60, 15), (80, 20, 10)]

# 起始点和目标点
start = (10, 10)
goal = (90, 90)

# 树的节点类
class Node:
    def __init__(self, x, y, parent=None):
        self.x = x
        self.y = y
        self.parent = parent  # 父节点

# 检查两点之间的路径是否碰撞障碍物
def collision_free(x1, y1, x2, y2):
    for (cx, cy, r) in obstacles:
        # 计算线段与圆的距离
        dx = x2 - x1
        dy = y2 - y1
        fx = x1 - cx
        fy = y1 - cy
        a = dx * dx + dy * dy
        b = 2 * (fx * dx + fy * dy)
        c = fx * fx + fy * fy - r * r
        discriminant = b * b - 4 * a * c
        if discriminant >= 0:  # 存在交点，说明有碰撞
            return False
    return True

# RRT算法
def rrt(start, goal, max_iter=1000, step_size=1.0):
    # 初始化树
    tree = [Node(start[0], start[1])]
    
    for _ in range(max_iter):
        # 随机采样目标点
        rand_x = random.uniform(0, WIDTH)
        rand_y = random.uniform(0, HEIGHT)
        
        # 找到树中离随机点最近的节点
        nearest_node = min(tree, key=lambda node: (node.x - rand_x) ** 2 + (node.y - rand_y) ** 2)
        
        # 计算从最近节点到随机点的单位向量
        theta = np.arctan2(rand_y - nearest_node.y, rand_x - nearest_node.x)
        new_x = nearest_node.x + step_size * np.cos(theta)
        new_y = nearest_node.y + step_size * np.sin(theta)
        
        # 检查是否与障碍物碰撞
        if collision_free(nearest_node.x, nearest_node.y, new_x, new_y):
            new_node = Node(new_x, new_y, parent=nearest_node)
            tree.append(new_node)
            
            # 如果新的节点接近目标，停止算法
            if np.hypot(new_x - goal[0], new_y - goal[1]) < step_size:
                print("Goal reached!")
                return tree
    
    print("Max iterations reached!")
    return tree

# 绘制环境和路径
def plot_tree(tree, goal):
    plt.figure(figsize=(10, 10))
    
    # 绘制障碍物
    for (cx, cy, r) in obstacles:
        circle = plt.Circle((cx, cy), r, color='r', alpha=0.5)
        plt.gca().add_patch(circle)
    
    # 绘制树
    for node in tree:
        if node.parent:
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], 'b-', lw=1)
    
    # 绘制起点和目标点
    plt.plot(start[0], start[1], 'go', label="Start")
    plt.plot(goal[0], goal[1], 'ro', label="Goal")
    
    plt.xlim(0, WIDTH)
    plt.ylim(0, HEIGHT)
    plt.gca().set_aspect('equal', adjustable='box')
    plt.legend()
    plt.show()

# 执行RRT算法
tree = rrt(start, goal)

# 绘制树和路径
plot_tree(tree, goal)

代码详细解释：

  1. 环境定义：
    • 环境尺寸为 100x100 的二维空间。
    • 使用 obstacles 定义障碍物，其中每个障碍物是一个圆形障碍物，格式为 (x, y, radius)。
  2. 节点类 Node：
    • 每个节点有 x、y 坐标和 parent（父节点），用于追踪树的结构。
  3. 碰撞检测 collision_free：
    • 该函数检测两点之间的线段是否与障碍物发生碰撞。通过计算线段与每个障碍物圆形的交点来判断。
  4. RRT算法 rrt：
    • 从起始点开始，算法随机生成一个目标点，并找到距离该点最近的树节点。然后从该节点扩展到新的点（步长为 step_size）。
    • 每次扩展后，会检查该新点是否与障碍物发生碰撞。如果没有碰撞，就将新点加入树。
    • 如果某个节点接近目标点（小于步长），就认为已经找到路径并返回树。
  5. 绘图函数 plot_tree：
    • 使用 matplotlib 来绘制环境、障碍物、树和最终的路径。
    • 绘制每个障碍物为红色圆形，树的扩展路径为蓝色线段，起始点为绿色圆点，目标点为红色圆点。

运行结果

运行上述代码后，会生成一个图像，显示：

  • 障碍物（以红色圆圈表示）。
  • 树的扩展路径（以蓝色线段表示）。
  • 起点和目标点。

如果目标点被成功到达，程序会打印出 "Goal reached!"。

优化建议

  1. RRT*算法：
    • 在RRT算法的基础上进行优化，逐步调整已生成路径，使路径更短和更平滑。
  2. 路径平滑：
    • 对RRT生成的路径进行后处理，例如使用贝塞尔曲线或者B样条（B-spline）进行路径平滑。
  3. 更高效的碰撞检测：
    • 在复杂环境下，可以使用网格或者树结构来加速碰撞检测过程。
  4. 动态环境：
    • 如果机器人在一个动态环境中运行，障碍物可能会发生变化，可以采用动态更新的RRT算法来适应新的障碍物位置。

希望这个完整的代码示例和解释能帮助你理解如何实现RRT算法。如果有进一步问题，或者需要优化或改进的部分，随时告诉我！