mport numpy as np
import heapq
import math
import csv
from scipy.spatial import KDTree
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
地形参数
MAP_WIDTH = 40000 # 40km (Y方向)
MAP_HEIGHT = 120000 # 120km (X方向)
GRID_SIZE = 25 # 网格大小(m)
MIN_ALTITUDE = 50 # 最低离地高度(m)
MAX_ALTITUDE = 5000 # 最大海拔(m)
SAFE_DISTANCE = 50 # 山体安全距离(m)
无人机参数
MIN_TURN_RADIUS = 15 # 最小转向半径(m)
MIN_SPEED = 40 # 最小速度(m/s)
MAX_SPEED = 50 # 最大速度(m/s)
MAX_FLIGHT_RANGE = 200000 # 最大航程(m)
MAX_CLIMB_RATE = 5 # 最大爬升率(m/s)
起点终点区域 (单位:m)
START_X = 0
START_Y_RANGE = (20000, 30000)
GOAL_X = 120000
GOAL_Y_RANGE = (10000, 18000)
算法参数
MAX_ITER = 1000000
STEP_SIZE = 35 # RRT步长(m)
GOAL_SAMPLE_RATE = 0.1 # 目标区域采样概率
NEIGHBOR_RADIUS = 150 # 邻域搜索半径(m)
class Node:
def init(self, x, y, z, cost=0, parent=None):
self.x = x
self.y = y
self.z = z
self.cost = cost # 累计被发现概率
self.parent = parent
self.children = []
def position(self): return np.array([self.x, self.y, self.z]) def __lt__(self, other): return self.cost < other.cost
class RRTStarPlanner:
def init(self, terrain_map):
self.terrain_map = terrain_map
self.nodes = []
self.path = []
self.kd_tree = None
self.setup_terrain()
def setup_terrain(self): # 创建网格坐标 (120km x 40km 区域) self.x_grid = np.arange(0, MAP_HEIGHT + GRID_SIZE, GRID_SIZE) self.y_grid = np.arange(0, MAP_WIDTH + GRID_SIZE, GRID_SIZE) self.xx, self.yy = np.meshgrid(self.x_grid, self.y_grid, indexing='ij') # 确保地形图尺寸匹配 assert self.terrain_map.shape == (len(self.x_grid), len(self.y_grid)), \ f"地形图尺寸不匹配: 期望({len(self.x_grid)}, {len(self.y_grid)}), 实际{self.terrain_map.shape}" # 创建KDTree用于快速高程查询 points = np.vstack([self.xx.ravel(), self.yy.ravel()]).T self.terrain_kdtree = KDTree(points) def get_ground_altitude(self, position): # 获取最近网格点的高程 dist, idx = self.terrain_kdtree.query([position[0], position[1]]) z_idx = np.unravel_index(idx, self.xx.shape) return self.terrain_map[z_idx] def detect_probability(self,x0,y0,z0): Pd=0 data2=[[13300,15425,4786],[38875,25175,4807],[79600,13175,5080],[100325,24450,4738]] Rmax=40000 gailv=[] for num1 in range(0,4): deltaX=data2[num1][0]-x0 deltaY=data2[num1][1]-y0 deltaZ=data2[num1][2]-z0 r=(deltaX**2+deltaY**2+deltaZ**2)**0.5 #分段函数 Ro=12000 Rmax=40000 Pdmax=0.9 Pdmin=0.5 b=float((Rmax*Pdmax-Ro*Pdmin)/(Rmax-Ro)) if r<=Ro: Pd=float(Pdmax) elif Ro<r<=Rmax: Pd=b-float(r*(Pdmax-Pdmin)/(Rmax-Ro)) elif r>Rmax: Pd=0 gailv.append(Pd) an=sum(gailv) return an def is_collision_free(self, position): x, y, z = position ground_z = self.get_ground_altitude([x, y]) # 检查高度约束 if z < ground_z + MIN_ALTITUDE or z > MAX_ALTITUDE: return False # 检查山体安全距离 (简化版) safe_z = ground_z + SAFE_DISTANCE if z < safe_z: return False return True def distance(self, p1, p2): return np.linalg.norm(np.array(p1) - np.array(p2)) def dynamic_feasible(self, from_node, to_node): """检查动力学约束""" # 计算高度变化率 dz = abs(to_node.z - from_node.z) dt = self.distance(from_node.position(), to_node.position()) / MIN_SPEED climb_rate = dz / dt # 检查爬升率约束 if climb_rate > MAX_CLIMB_RATE: return False # 检查转向半径 (简化版) if from_node.parent: v1 = np.array(from_node.position()) - np.array(from_node.parent.position()) v2 = np.array(to_node.position()) - np.array(from_node.position()) if np.linalg.norm(v1) > 1e-5 and np.linalg.norm(v2) > 1e-5: cos_theta = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2)) turn_radius = STEP_SIZE / (2 * math.sin(math.acos(cos_theta) / 2)) if turn_radius < MIN_TURN_RADIUS: return False return True def steer(self, from_node, to_point): """生成新节点""" direction = to_point - from_node.position() dist = np.linalg.norm(direction) direction = direction / dist # 应用步长限制 step = min(STEP_SIZE, dist) new_position = from_node.position() + direction * step new_z = new_position[2] # 高度约束调整 ground_z = self.get_ground_altitude(new_position) new_z = max(ground_z + MIN_ALTITUDE + SAFE_DISTANCE, min(new_z, MAX_ALTITUDE)) # 创建新节点 new_node = Node(new_position[0], new_position[1], new_z) new_node.parent = from_node # 计算代价 detect_cost = self.detect_probability(new_node.x, new_node.y, new_node.z) new_node.cost = from_node.cost + detect_cost return new_node def find_nearest_neighbors(self, new_node): """在半径内寻找邻居节点""" neighbors = [] for node in self.nodes: if self.distance(node.position(), new_node.position()) <= NEIGHBOR_RADIUS: neighbors.append(node) return neighbors def plan(self): # 初始化起点 start_y = np.random.uniform(START_Y_RANGE[0], START_Y_RANGE[1]) start_z = self.get_ground_altitude([START_X, start_y]) + MIN_ALTITUDE + SAFE_DISTANCE start_node = Node(START_X, start_y, start_z) self.nodes.append(start_node) for _ in range(MAX_ITER): # 随机采样 (有一定概率采样目标区域) if np.random.rand() < GOAL_SAMPLE_RATE: goal_y = np.random.uniform(GOAL_Y_RANGE[0], GOAL_Y_RANGE[1]) goal_z = self.get_ground_altitude([GOAL_X, goal_y]) + MIN_ALTITUDE + SAFE_DISTANCE sample = np.array([GOAL_X, goal_y, goal_z]) else: sample = np.array([ np.random.uniform(0, MAP_HEIGHT), np.random.uniform(0, MAP_WIDTH), np.random.uniform(MIN_ALTITUDE, MAX_ALTITUDE) ]) # 找到最近节点 nearest_node = min(self.nodes, key=lambda node: self.distance(node.position(), sample)*0.1+abs(node.position()[0]-sample[0])) if abs(nearest_node.x-GOAL_X)<=1500: nearest_node = min(self.nodes, key=lambda node: abs(node.position()[0]-sample[0])) # 生成新节点 new_node = self.steer(nearest_node, sample) # 检查碰撞和动态约束 if self.is_collision_free(new_node.position()) and self.dynamic_feasible(nearest_node, new_node): neighbors = self.find_nearest_neighbors(new_node) # 寻找最优父节点 min_cost = float('inf') best_node = new_node for neighbor in neighbors: if self.dynamic_feasible(neighbor, new_node): # 计算通过邻居到新节点的代价 detect_cost = self.detect_probability(new_node.x, new_node.y, new_node.z) cost = neighbor.cost + detect_cost if cost < min_cost: min_cost = cost best_node = neighbor # 添加新节点到树 if best_node: new_node.parent = best_node new_node.cost = min_cost best_node.children.append(new_node) self.nodes.append(new_node) print(new_node.x) # 重布线 for neighbor in neighbors: if neighbor != best_node and self.dynamic_feasible(new_node, neighbor): # 计算经由新节点到邻居的代价 detect_cost = self.detect_probability(neighbor.x, neighbor.y, neighbor.z) new_cost = new_node.cost + detect_cost if new_cost < neighbor.cost: neighbor.parent.children.remove(neighbor) neighbor.parent = new_node neighbor.cost = new_cost new_node.children.append(neighbor) # 检查是否到达目标区域 if (abs(new_node.x - GOAL_X) < STEP_SIZE and GOAL_Y_RANGE[0] <= new_node.y <= GOAL_Y_RANGE[1]): self.reconstruct_path(new_node) return True return False def reconstruct_path(self, goal_node): """回溯路径""" self.path = [] current = goal_node while current: self.path.append(current) current = current.parent self.path.reverse() def discretize_path(self): """每秒离散化路径""" if not self.path: return [] discrete_path = [] current_point = self.path[0] discrete_path.append((current_point.x, current_point.y, current_point.z)) for i in range(1, len(self.path)): start = np.array([current_point.x, current_point.y, current_point.z]) end = np.array([self.path[i].x, self.path[i].y, self.path[i].z]) distance = np.linalg.norm(end - start) num_points = max(2, int(distance / MIN_SPEED)) # 每秒至少一个点 # 线性插值 for j in range(1, num_points): ratio = j / num_points x = start[0] + ratio * (end[0] - start[0]) y = start[1] + ratio * (end[1] - start[1]) z = start[2] + ratio * (end[2] - start[2]) # 确保高度约束 ground_z = self.get_ground_altitude([x, y]) z = max(ground_z + MIN_ALTITUDE + SAFE_DISTANCE, min(z, MAX_ALTITUDE)) discrete_path.append((x, y, z)) discrete_path.append((end[0], end[1], end[2])) current_point = self.path[i] return discrete_path def discretize_path_to_time(self, dt=1.0, cruise_speed=MIN_SPEED): """ 将路径离散化为每秒一个点的时间序列轨迹 同时计算各方向的速度 (vx, vy, vz) 参数: dt: 时间步长，默认 1 秒 cruise_speed: 巡航速度（m/s），用于估算飞行时间，也可动态计算 返回: trajectory: list of dict -> [{'t': t, 'x': x, 'y': y, 'z': z, 'vx': vx, 'vy': vy, 'vz': vz}, ...] """ if not self.path or len(self.path) < 2: return [] trajectory = [] total_time = 0.0 # 第一个点：t=0，速度设为下一个段的速度 first_node = self.path[0] trajectory.append({ 't': 0.0, 'x': first_node.x, 'y': first_node.y, 'z': first_node.z, 'vx': 0.0, 'vy': 0.0, 'vz': 0.0 }) cumulative_distance = 0.0 current_segment_index = 0 remaining_in_segment = 0.0 # 遍历路径中的线段 for i in range(1, len(self.path)): start_node = self.path[i - 1] end_node = self.path[i] segment_start_pos = np.array([start_node.x, start_node.y, start_node.z]) segment_end_pos = np.array([end_node.x, end_node.y, end_node.z]) segment_vector = segment_end_pos - segment_start_pos segment_length = np.linalg.norm(segment_vector) if segment_length == 0: continue # 单位方向向量 direction = segment_vector / segment_length segment_duration = segment_length / cruise_speed # 当前段所需时间 num_steps = max(1, int(round(segment_duration / dt))) # 在当前线段上均匀插入时间点 for step in range(1, num_steps + 1): t_local = step * dt # 当前段内经过的时间 if total_time + t_local > segment_duration: break # 超出本段，留到下一段处理 s = t_local / segment_duration # 插值比例 pos = segment_start_pos + s * segment_vector # 获取地面高度并约束 z ground_z = self.get_ground_altitude(pos) z_clamped = max(ground_z + MIN_ALTITUDE + SAFE_DISTANCE, min(pos[2], MAX_ALTITUDE)) # 构造新位置（z 可能被调整） final_pos = np.array([pos[0], pos[1], z_clamped]) # 计算速度：使用巡航速度的方向分量 speed_vector = direction * cruise_speed trajectory.append({ 't': round(total_time + t_local, 3), 'x': final_pos[0], 'y': final_pos[1], 'z': final_pos[2], 'vx': speed_vector[0], 'vy': speed_vector[1], 'vz': speed_vector[2] }) total_time += segment_duration # 对时间做重新对齐，确保是严格每秒一次（可选：插值到整数秒） timed_trajectory = [] next_output_time = 0.0 index = 0 while index < len(trajectory): point = trajectory[index] if abs(point['t'] - next_output_time) < 1e-3: timed_trajectory.append(point) next_output_time += dt elif point['t'] > next_output_time: # 插值两个点之间 prev_point = trajectory[index - 1] if index > 0 else trajectory[0] ratio = (next_output_time - prev_point['t']) / (point['t'] - prev_point['t']) interp_pos = (1 - ratio) * np.array([prev_point['x'], prev_point['y'], prev_point['z']]) + \ ratio * np.array([point['x'], point['y'], point['z']]) interp_vel = (1 - ratio) * np.array([prev_point['vx'], prev_point['vy'], prev_point['vz']]) + \ ratio * np.array([point['vx'], point['vy'], point['vz']]) timed_trajectory.append({ 't': round(next_output_time, 3), 'x': interp_pos[0], 'y': interp_pos[1], 'z': interp_pos[2], 'vx': interp_vel[0], 'vy': interp_vel[1], 'vz': interp_vel[2] }) next_output_time += dt else: index += 1 return timed_trajectory def export_to_excel(self, timed_trajectory, filename='C-Solution.xlsx'): """ 将定时轨迹导出到 Excel 文件 从第2行开始写入数据，第1行写入列名 """ from openpyxl import Workbook # 创建工作簿和工作表 wb = Workbook() ws = wb.active ws.title = "Flight Trajectory" # 写入表头（第一行） headers = ['t(s)', 'X(m)', 'Y(m)', 'Z(m)', 'Vx(m/s)', 'Vy(m/s)', 'Vz(m/s)'] for col_idx, header in enumerate(headers, 1): ws.cell(row=1, column=col_idx, value=header) # 写入数据（从第二行开始） for idx, point in enumerate(timed_trajectory, start=2): # 从第2行开始 ws.cell(row=idx, column=1, value=point['t']) ws.cell(row=idx, column=2, value=point['x']) ws.cell(row=idx, column=3, value=point['y']) ws.cell(row=idx, column=4, value=point['z']) ws.cell(row=idx, column=5, value=point['vx']) ws.cell(row=idx, column=6, value=point['vy']) ws.cell(row=idx, column=7, value=point['vz']) # 保存文件 wb.save(filename) print(f"飞行轨迹已成功导出至 {filename}")
def load_terrain_from_csv(file_path):
“”"
从CSV文件加载地形高程数据
文件格式: 每行代表一个X坐标(行数)，每列代表Y坐标(列数)，值为海拔高度
“”"
terrain_data = []
with open(file_path, ‘r’) as f:
csv_reader = csv.reader(f)
for row in csv_reader:
# 将字符串转换为浮点数
terrain_data.append([float(value) for value in row])
# 转换为numpy数组并转置为正确的方向 terrain_array = np.array(terrain_data).T # 验证尺寸 (X坐标点数: 120km/25m = 4800, Y坐标点数: 40km/25m = 1600) expected_x_size = int(MAP_HEIGHT / GRID_SIZE) + 1 expected_y_size = int(MAP_WIDTH / GRID_SIZE) + 1 # 自动裁剪或填充到预期尺寸 if terrain_array.shape[0] > expected_x_size: terrain_array = terrain_array[:expected_x_size, :] elif terrain_array.shape[0] < expected_x_size: padding = np.zeros((expected_x_size - terrain_array.shape[0], terrain_array.shape[1])) terrain_array = np.vstack([terrain_array, padding]) if terrain_array.shape[1] > expected_y_size: terrain_array = terrain_array[:, :expected_y_size] elif terrain_array.shape[1] < expected_y_size: padding = np.zeros((terrain_array.shape[0], expected_y_size - terrain_array.shape[1])) terrain_array = np.hstack([terrain_array, padding]) print(f"地形图加载完成, 尺寸: {terrain_array.shape}") return terrain_array
主程序
if name == “main”:
# 1. 从CSV文件加载地形数据
csv_file = “附件2：三维地形.csv” # 替换为实际文件路径
print(f"从{csv_file}加载三维地形图…")
terrain_map = load_terrain_from_csv(csv_file)
# 2. 初始化路径规划器 print("初始化RRT*路径规划器...") planner = RRTStarPlanner(terrain_map) # 3. 执行路径规划 print("开始路径规划...") if planner.plan(): print("\n成功找到路径!") # 4. 生成定时轨迹 print("生成定时飞行轨迹...") timed_path = planner.discretize_path_to_time(dt=1.0, cruise_speed=MIN_SPEED) # 假设最小速度即巡航速度 # 5. 打印带时间和速度的轨迹 print("\n飞行轨迹 (每秒一个点):") print(f"{'t(s)':>6} {'X(m)':>10} {'Y(m)':>10} {'Z(m)':>10} " f"{'Vx(m/s)':>10} {'Vy(m/s)':>10} {'Vz(m/s)':>10} {'发现概率':>10}") detector = lambda x, y, z: max(0.01, 1.0/(z - planner.get_ground_altitude([x,y]) + 1) + z/10000) for point in timed_path: prob = detector(point['x'], point['y'], point['z']) print(f"{point['t']:>6.1f} {point['x']:>10.1f} {point['y']:>10.1f} {point['z']:>10.1f} " f"{point['vx']:>10.2f} {point['vy']:>10.2f} {point['vz']:>10.2f} {prob:>10.4f}") print(f"\n总飞行时间: {timed_path[-1]['t']} 秒") print(f"总路径点数（每秒）: {len(timed_path)}") print(f"平均速度: {MIN_SPEED:.1f} m/s (假设恒定)") # 6. 导出到 Excel planner.export_to_excel(timed_path, filename='C-Solution.xlsx')
这代码跑到后面时收敛的速度很慢，给出原因和具体的解决方案，并对原代码进行修改
本文介绍了一种在已知数组被旋转的情况下查找最小元素的方法，特别考虑了数组中可能存在重复元素的情况。通过遍历的方式找到最小值，并给出了具体的Java代码实现。
扫一扫
1187
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
