import numpy as npimport heapq
# 全局路径规划 - A*算法def a_star(start, goal, grid): rows, cols = grid.shape open_list = [] heapq.heappush(open_list, (0, start)) came_from = {} cost_so_far = {start: 0} came_from[start] = None
while open_list: _, current = heapq.heappop(open_list)
if current == goal: break
for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]: neighbor = (current[0] + dx, current[1] + dy) if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols and grid[neighbor] == 0: new_cost = cost_so_far[current] + 1 if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]: cost_so_far[neighbor] = new_cost priority = new_cost + np.hypot(goal[0] - neighbor[0], goal[1] - neighbor[1]) heapq.heappush(open_list, (priority, neighbor)) came_from[neighbor] = current
# 路径重建 path = [] if goal in came_from: while goal: path.append(goal) goal = came_from[goal] path.reverse() return path
# 局部动态避障 - 简化版DWA算法def dwa(robot_pos, goal, obstacles, dt=0.1, max_speed=1.0, max_turn_rate=np.pi/4): # 生成速度组合 v_samples = np.linspace(0, max_speed, 5) w_samples = np.linspace(-max_turn_rate, max_turn_rate, 5) best_v, best_w = 0, 0 min_cost = float('inf')
for v in v_samples: for w in w_samples: new_pos = robot_pos + np.array([v * np.cos(robot_pos[2]) * dt, v * np.sin(robot_pos[2]) * dt, w * dt]) heading_cost = np.hypot(goal[0] - new_pos[0], goal[1] - new_pos[1]) obstacle_cost = min(np.hypot(new_pos[0] - obs[0], new_pos[1] - obs[1]) for obs in obstacles)
if obstacle_cost > 0.5: # 安全距离 total_cost = heading_cost + 1.0 / (obstacle_cost + 1e-5) if total_cost < min_cost: min_cost, best_v, best_w = total_cost, v, w
return best_v, best_w
# 示例环境设置grid = np.zeros((10, 10))grid[5, 3:7] = 1 # 障碍物start, goal = (0, 0), (9, 9)path = a_star(start, goal, grid)
# 机器人执行路径robot_pos = np.array([start[0], start[1], 0.0]) # 初始位置和角度obstacles =
[(5, y) for y in range(3, 7)]for target in path: while np.hypot(robot_pos[0] - target[0], robot_pos[1] - target[1]) > 0.1: v, w = dwa(robot_pos, target, obstacles) robot_pos += np.array([v * np.cos(robot_pos[2]), v * np.sin(robot_pos[2]), w]) print("当前位置:", robot_pos[:2])
