# -*- coding: utf-8 -*- # File: rrt_algorithm.py # Purpose: 实现RRT(快速随机树)路径规划算法，支持避开危险区域 import numpy as np import math import random from typing import List, Tuple, Dict, Optional class Node: """RRT树节点""" def __init__(self, x: float, y: float): self.x = x self.y = y self.parent = None self.cost = 0.0 # 从起点到当前节点的代价 class RRTAlgorithm: """RRT路径规划算法""" def __init__(self, grid_resolution: float = 5.0, max_iterations: int = 2000, step_size: float = 20.0, goal_sample_rate: float = 0.1, search_radius: float = 50.0): """ 初始化RRT算法 Args: grid_resolution: 网格分辨率 max_iterations: 最大迭代次数 step_size: 步长 goal_sample_rate: 采样目标点的概率 search_radius: 搜索半径 """ self.grid_resolution = grid_resolution self.max_iterations = max_iterations self.step_size = step_size self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius def plan(self, start: Tuple[float, float], goal: Tuple[float, float], map_width: int, map_height: int, threat_areas: List[Dict], obstacles: List[Tuple[float, float]] = None) -> List[Tuple[float, float]]: """ 使用RRT算法规划路径 Args: start: 起点坐标 (x, y) goal: 终点坐标 (x, y) map_width: 地图宽度 map_height: 地图高度 threat_areas: 危险区域列表 obstacles: 障碍物列表 Returns: 路径点列表，如果找不到路径返回空列表 """ # 创建起点和终点节点 start_node = Node(start[0], start[1]) goal_node = Node(goal[0], goal[1]) # 初始化树 tree = [start_node] # 迭代构建树 for i in range(self.max_iterations): # 以一定概率直接取目标点，提高搜索效率 if random.random() < self.goal_sample_rate: random_point = (goal_node.x, goal_node.y) else: # 随机采样点 random_point = ( random.uniform(0, map_width), random.uniform(0, map_height) ) # 找到树中离随机点最近的节点 nearest_node = self._find_nearest_node(tree, random_point) # 朝随机点方向扩展固定步长 new_node = self._steer(nearest_node, random_point, self.step_size) # 检查路径是否有效 if new_node and self._is_path_valid(nearest_node, new_node, threat_areas, obstacles): # 将新节点添加到树中 new_node.parent = nearest_node new_node.cost = nearest_node.cost + self._distance((nearest_node.x, nearest_node.y), (new_node.x, new_node.y)) tree.append(new_node) # 检查是否可以连接到目标点 dist_to_goal = self._distance((new_node.x, new_node.y), (goal_node.x, goal_node.y)) if dist_to_goal < self.step_size: # 尝试直接连接到目标点 if self._is_path_valid(new_node, goal_node, threat_areas, obstacles): goal_node.parent = new_node goal_node.cost = new_node.cost + dist_to_goal # 找到路径，提前结束 return self._extract_path(goal_node) # RRT* 优化 (可选，提高路径质量) # 尝试重新连接附近节点以获得更优路径 self._rewire(tree, new_node, threat_areas, obstacles) # 如果达到最大迭代次数但仍未找到到达目标的路径 # 尝试找到离目标最近的节点，并返回到该节点的路径 closest_node = self._find_nearest_node(tree, (goal_node.x, goal_node.y)) path = self._extract_path(closest_node) # 如果最近节点离目标足够近，则认为找到了路径 if self._distance((closest_node.x, closest_node.y), (goal_node.x, goal_node.y)) < self.step_size * 2: return path # 否则找不到有效路径，返回空列表 return [] def _find_nearest_node(self, tree: List[Node], point: Tuple[float, float]) -> Node: """找到树中离指定点最近的节点""" min_dist = float('inf') nearest_node = None for node in tree: dist = self._distance((node.x, node.y), point) if dist < min_dist: min_dist = dist nearest_node = node return nearest_node def _steer(self, from_node: Node, to_point: Tuple[float, float], step_size: float) -> Optional[Node]: """从起始节点向目标点方向移动固定步长""" dx = to_point[0] - from_node.x dy = to_point[1] - from_node.y dist = math.sqrt(dx*dx + dy*dy) if dist < 0.0001: # 距离近乎为0 return None # 按照指定的步长移动 ratio = min(step_size / dist, 1.0) new_x = from_node.x + dx * ratio new_y = from_node.y + dy * ratio new_node = Node(new_x, new_y) return new_node def _is_path_valid(self, from_node: Node, to_node: Node, threat_areas: List[Dict], obstacles: List[Tuple[float, float]] = None) -> bool: """检查两个节点之间的路径是否有效（不经过障碍物和危险区域）""" # 采样点检测 dist = self._distance((from_node.x, from_node.y), (to_node.x, to_node.y)) num_samples = max(int(dist / self.grid_resolution), 5) for i in range(num_samples + 1): t = i / num_samples x = from_node.x + t * (to_node.x - from_node.x) y = from_node.y + t * (to_node.y - from_node.y) # 检查是否在危险区域内 if self._is_in_threat_areas(x, y, threat_areas): return False # 检查是否与障碍物碰撞 if obstacles and self._is_in_obstacles(x, y, obstacles): return False return True def _rewire(self, tree: List[Node], new_node: Node, threat_areas: List[Dict], obstacles: List[Tuple[float, float]] = None) -> None: """重新连接树中节点以优化路径代价 (RRT* 算法的核心步骤)""" # 找到搜索半径内的所有节点 near_nodes = [] for node in tree: if node is not new_node: # 排除新节点本身 dist = self._distance((node.x, node.y), (new_node.x, new_node.y)) if dist < self.search_radius: near_nodes.append(node) # 对于搜索半径内的每个节点，检查是否通过新节点创建的路径更好 for node in near_nodes: # 计算通过新节点到达当前节点的潜在代价 potential_cost = new_node.cost + self._distance((new_node.x, new_node.y), (node.x, node.y)) # 如果代价更低且路径有效，则重新连接 if potential_cost < node.cost and self._is_path_valid(new_node, node, threat_areas, obstacles): node.parent = new_node node.cost = potential_cost def _extract_path(self, goal_node: Node) -> List[Tuple[float, float]]: """从目标节点回溯生成路径""" path = [] current = goal_node # 回溯到起点 while current: path.append((current.x, current.y)) current = current.parent # 反转路径顺序（从起点到终点） return list(reversed(path)) def _distance(self, p1: Tuple[float, float], p2: Tuple[float, float]) -> float: """计算两点间欧几里得距离""" return math.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2) def _is_in_threat_areas(self, x: float, y: float, threat_areas: List[Dict]) -> bool: """检查点是否在危险区域中""" for area in threat_areas: area_type = area.get('type', '') if area_type == 'circle': center = area.get('center', (0, 0)) radius = area.get('radius', 0) distance = math.sqrt((x - center[0]) ** 2 + (y - center[1]) ** 2) if distance <= radius: return True elif area_type == 'rectangle': rect = area.get('rect', (0, 0, 0, 0)) x1, y1, width, height = rect if x1 <= x <= x1 + width and y1 <= y <= y1 + height: return True elif area_type == 'polygon': points = area.get('points', []) if self._point_in_polygon(x, y, points): return True return False def _point_in_polygon(self, x: float, y: float, polygon: List[Tuple[float, float]]) -> bool: """检查点是否在多边形内（射线法）""" if len(polygon) < 3: return False inside = False j = len(polygon) - 1 for i in range(len(polygon)): xi, yi = polygon[i] xj, yj = polygon[j] # 检查点是否在多边形边上 if (yi == y and xi == x) or (yj == y and xj == x): return True # 射线法判断点是否在多边形内部 intersect = ((yi > y) != (yj > y)) and (x < (xj - xi) * (y - yi) / (yj - yi) + xi) if intersect: inside = not inside j = i return inside def _is_in_obstacles(self, x: float, y: float, obstacles: List[Tuple[float, float]]) -> bool: """检查点是否在障碍物中""" for obstacle_x, obstacle_y in obstacles: distance = math.sqrt((x - obstacle_x) ** 2 + (y - obstacle_y) ** 2) if distance < self.grid_resolution: return True return False def smooth_path(self, path: List[Tuple[float, float]], threat_areas: List[Dict], obstacles: List[Tuple[float, float]] = None, weight_data: float = 0.5, weight_smooth: float = 0.3, tolerance: float = 0.000001) -> List[Tuple[float, float]]: """ 使用平滑算法优化路径 (与A*接口兼容) Args: path: 原始路径 threat_areas: 危险区域 obstacles: 障碍物 weight_data: 数据权重 weight_smooth: 平滑权重 tolerance: 收敛阈值 Returns: 平滑后的路径 """ # 如果路径点太少，不需要平滑 if len(path) <= 2: return path # 将路径转换为numpy数组，方便操作 path_array = np.array(path) smooth_path = path_array.copy() # 迭代优化 change = tolerance while change >= tolerance: change = 0.0 # 对每个点进行优化（除了起点和终点） for i in range(1, len(path) - 1): for j in range(2): # x, y aux = smooth_path[i][j] smooth_path[i][j] += weight_data * (path_array[i][j] - smooth_path[i][j]) smooth_path[i][j] += weight_smooth * (smooth_path[i-1][j] + smooth_path[i+1][j] - 2.0 * smooth_path[i][j]) change += abs(aux - smooth_path[i][j]) # 检查平滑后的路径是否经过危险区域或障碍物 for i in range(len(smooth_path)): x, y = smooth_path[i] if self._is_in_threat_areas(x, y, threat_areas) or (obstacles and self._is_in_obstacles(x, y, obstacles)): return path # 如果平滑后路径穿过危险区域，返回原路径 return [(x, y) for x, y in smooth_path]