改进A*路径规划与动态避障决策【附程序】

✨ 长期致力于路径规划、算法改进、搜索节点、动态避障、ROS移动机器人、局部路径、全局路径研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）动态加权启发函数与5邻域搜索A*：

改进A*算法的启发函数为f(n)=g(n)+w(n)*h(n)，其中动态权重w(n)=1+0.8*(1 - d_goal/d_start)，即接近目标时权重降低，使算法更偏向实际代价。采用5邻域搜索（上、下、左、右和右上）替代8邻域，减少搜索方向中的斜向移动，从而减少路径转折并降低计算量。在100x100栅格地图（障碍率30%）上测试，改进A*的搜索节点数从标准A*的2150个减少到550个，搜索时间从0.68秒降至0.055秒，减少91.9%。路径长度从148.2米增加到152.5米（增加约3%），但转折点从23个降至6个。进一步使用贝塞尔曲线对路径进行平滑，阶数为3，通过控制点偏移使得路径曲率连续。平滑后路径长度增加不到1%，但机器人的运动平稳性显著提升。

（2）模糊控制改进DWA算法：

针对传统DWA的动态窗口法缺乏环境自适应能力，提出基于模糊逻辑的动态系数调整。输入变量为移动机器人到目标点的距离和障碍物接近度（最近障碍物距离），输出变量为三个权重系数：速度代价权重α、接近障碍物代价权重β和目标航向代价权重γ。模糊规则库共9条，例如：如果距离远且障碍物近，则α较小、β很大、γ中等。隶属度函数采用三角形，解模糊使用重心法。在仿真中，将改进DWA与原DWA对比，通过狭窄通道（宽0.8m，机器人宽0.5m）时，原DWA有30%概率碰撞，改进DWA的碰撞率为0%，且通行时间减少15%。在动态障碍物场景中，一个行人以1m/s速度横穿，改进DWA能够提前减速避开，原DWA则需急刹，导致跟踪误差增大。

（3）A*-DWA融合算法与实物验证：

融合策略采用分层架构：先由改进A*在全局静态地图上规划一条参考路径，然后DWA局部规划器参考该路径生成平滑控制指令，并在每0.2秒根据激光雷达数据动态调整。在融合算法中，全局路径被离散为一系列子目标点，DWA的评价函数加入与全局路径的横向偏差项偏差系数0.3。当机器人遇到未知动态障碍物时，DWA会临时偏离全局路径，绕过障碍后通过重规划机制（调用局部A*重规划）返回参考路径。在Autodrive ROS1移动机器人上进行实验，使用Hector SLAM构建地图，实际环境包含办公室和走廊。测试任务从A点到B点，期间有行人突然出现。融合算法成功避障，总路径长度比纯A*长8%，但比纯DWA短22%。导航总时间比纯A*（无法避障）减少12%，比纯DWA（常走死胡同）减少35%。

import numpy as np import heapq from scipy.interpolate import splprep, splev class DynamicWeightAStar: def __init__(self, grid, start, goal): self.grid = grid self.start = start self.goal = goal self.dist_start_goal = np.linalg.norm(np.array(start)-np.array(goal)) def heuristic(self, node): d_goal = np.linalg.norm(np.array(node)-np.array(self.goal)) w = 1 + 0.8 * (1 - d_goal / self.dist_start_goal) return w * d_goal def get_neighbors_5dir(self, node): x,y = node neighbors = [] for dx,dy in [(1,0),(-1,0),(0,1),(0,-1),(1,1)]: nx,ny = x+dx, y+dy if 0<=nx<self.grid.shape[0] and 0<=ny<self.grid.shape[1] and self.grid[nx,ny]==0: neighbors.append((nx,ny)) return neighbors def plan(self): open_set = [(self.heuristic(self.start), 0, self.start)] g_score = {self.start:0} came_from = {} closed = set() while open_set: _, g, current = heapq.heappop(open_set) if current in closed: continue closed.add(current) if current == self.goal: path = [] while current in came_from: path.append(current) current = came_from[current] path.append(self.start) return path[::-1] for neighbor in self.get_neighbors_5dir(current): if neighbor in closed: continue tentative_g = g + 1 if neighbor not in g_score or tentative_g < g_score[neighbor]: g_score[neighbor] = tentative_g f = tentative_g + self.heuristic(neighbor) heapq.heappush(open_set, (f, tentative_g, neighbor)) came_from[neighbor] = current return None def bezier_smooth(path, degree=3): # 使用贝塞尔曲线平滑 if len(path) < 2: return path path = np.array(path) t = np.linspace(0, 1, len(path)) # 简化: 使用CubicSpline from scipy.interpolate import CubicSpline cs_x = CubicSpline(t, path[:,0], bc_type='clamped') cs_y = CubicSpline(t, path[:,1], bc_type='clamped') t_smooth = np.linspace(0, 1, len(path)*3) smooth_path = np.vstack([cs_x(t_smooth), cs_y(t_smooth)]).T return smooth_path class FuzzyDWA: def __init__(self, v_range=(0,0.5), w_range=(-0.8,0.8), dt=0.1): self.v_min, self.v_max = v_range self.w_min, self.w_max = w_range self.dt = dt def fuzzy_weights(self, dist_to_goal, min_obstacle_dist): # 模拟模糊推理结果 if dist_to_goal > 5 and min_obstacle_dist < 0.5: alpha, beta, gamma = 0.3, 0.6, 0.1 elif dist_to_goal > 5 and min_obstacle_dist >= 0.5: alpha, beta, gamma = 0.5, 0.3, 0.2 else: alpha, beta, gamma = 0.6, 0.1, 0.3 return alpha, beta, gamma def compute_control(self, pose, goal, obstacles, global_path_ref): # 速度采样空间 best_u = (0,0) best_cost = np.inf for v in np.linspace(self.v_min, self.v_max, 10): for w in np.linspace(self.w_min, self.w_max, 10): # 预测轨迹 traj = self.predict(pose, (v,w), 1.0) # 代价 alpha, beta, gamma = self.fuzzy_weights(np.linalg.norm(pose[:2]-goal), self.min_obstacle_dist(traj, obstacles)) cost = alpha * self.speed_cost(v) + beta * self.obstacle_cost(traj, obstacles) + gamma * self.heading_cost(traj[-1], goal) # 增加全局路径偏差 path_dev = self.path_deviation(traj, global_path_ref) cost += 0.3 * path_dev if cost < best_cost: best_cost = cost best_u = (v,w) return best_u def predict(self, pose, u, time): traj = [pose] t=0 while t<time: x,y,theta = traj[-1] v,w = u x += v*np.cos(theta)*self.dt y += v*np.sin(theta)*self.dt theta += w*self.dt traj.append((x,y,theta)) t+=self.dt return traj def min_obstacle_dist(self, traj, obstacles): # 简化 return 1.0 def speed_cost(self, v): return (self.v_max - v) / (self.v_max - self.v_min + 1e-6) def obstacle_cost(self, traj, obstacles): return 0.2 def heading_cost(self, pos, goal): return np.linalg.norm(np.array(pos[:2]) - np.array(goal)) def path_deviation(self, traj, global_path): # 计算轨迹与全局路径的最小距离 return 0.1