改进A*融合机器人路径规划应用【附仿真】

✨ 长期致力于路径规划、A*算法、人工势场法、栅格地图、ROS研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）栅格地图预处理算法：

针对传统栅格地图无法处理不规则障碍物和危险区域的问题，提出预处理三步法。第一步，形态学膨胀：对障碍物栅格使用半径为2的圆形结构元素进行膨胀，填充狭窄缝隙，确保规划出的路径不会从间隙中穿过（间隙宽度小于机器人直径）。第二步，危险区域标记：用户自定义高风险区域（如陡坡、积水区），在栅格地图中将这些区域设为不可通行并附加额外代价。第三步，道路宽度检测：对每个自由栅格计算到最近障碍物的距离，若该距离小于机器人半径+安全余量0.2m，则标记为狭窄通道，路径规划时优先选择宽度较大的区域。在MATLAB中对50x50栅格地图进行测试，预处理后传统A*的搜索节点减少10-13%，时间减少27-33%。预处理后的地图路径不再出现紧贴障碍物的情况，安全性提升。

（2）五邻域搜索与启发式函数赋权A*：

改进传统A*的两个核心组件。邻域搜索从八方向改为五方向（抛弃左下、右下、左上、右上四个对角方向），对角移动用一个横向加一个纵向代替，这大幅减少了拐点数量。启发式函数h(n)采用欧氏距离乘以权重系数w，w根据当前节点附近障碍物密度动态调整：密度高时w增大，使算法更倾向远离障碍物。权重计算为w = 1 + 0.5 * (1 - 局部自由栅格占比)。在障碍率不同的四种地图上对比，改进A*的搜索节点减少4-12%，搜索时间减少10-34%。路径长度平均增加不超过4%，但路径平滑度（曲率变化）改善40%。在ROS的move_base框架中以插件形式实现该改进A*，实际测试规划时间由0.35秒降至0.23秒。

（3）A*-人工势场融合算法与实物部署：

融合设计分为两层：上层采用改进A*规划全局参考路径，下层使用改进人工势场法进行局部轨迹跟踪和避障。对人工势场的斥力函数进行改进，加入相对速度项，使机器人能够应对动态障碍物。斥力公式改为U_rep = 0.5*k_rep*(1/d - 1/d0)^2 + 0.5*k_vel*v_rel^2，其中v_rel为机器人与障碍物的相对速度。此外，为了解决目标不可达问题，当机器人接近目标点（距离<0.5m）时，将引力增益线性减小到0，同时斥力也减小，确保机器人能够到达目标。在障碍比为0.2和0.3的栅格地图中，融合算法成功规划出路径，解决了传统人工势场法陷入局部极小值的问题。将算法移植到Jetson Nano上的移动机器人（采用ROS Melodic），在室外环境下进行实验。场景一：空旷草地，融合算法路径长度比纯A*短3.6%，搜索时间少19%；场景二：有多个临时障碍物的广场，融合算法成功避障，而纯A*需要重新全局规划导致停顿。最终路径平滑，机器人运动平稳。

import numpy as np from scipy.ndimage import binary_dilation class MapPreprocessor: def __init__(self, grid, robot_radius=0.3, safety_margin=0.2): self.grid = grid self.radius = robot_radius self.safety = safety_margin def dilate_obstacles(self, kernel_size=2): struct = np.ones((kernel_size, kernel_size)) dilated = binary_dilation(self.grid, struct) return dilated def compute_distance_transform(self): from scipy.ndimage import distance_transform_edt dist_to_obstacle = distance_transform_edt(1 - self.grid) narrow_passage = dist_to_obstacle < (self.radius + self.safety) return narrow_passage def preprocess(self): grid_dilated = self.dilate_obstacles() narrow = self.compute_distance_transform() final_grid = np.logical_or(grid_dilated, narrow) return final_grid.astype(int) class AdaptiveHeuristicAStar: def __init__(self, grid, start, goal): self.grid = grid self.start = start self.goal = goal def local_free_ratio(self, node, r=3): x,y = node subgrid = self.grid[max(0,x-r):min(self.grid.shape[0],x+r+1), max(0,y-r):min(self.grid.shape[1],y+r+1)] free = np.sum(subgrid==0) total = subgrid.size return free / (total + 1e-6) def heuristic(self, node): d = np.linalg.norm(np.array(node)-np.array(self.goal)) free_ratio = self.local_free_ratio(node) w = 1 + 0.5 * (1 - free_ratio) return w * d def get_neighbors_5(self, node): x,y = node candidates = [(x+1,y),(x-1,y),(x,y+1),(x,y-1),(x+1,y+1)] neighbors = [] for nx,ny in candidates: 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 = {self.start:0} parent = {} closed = set() while open_set: f_val, g_val, cur = heapq.heappop(open_set) if cur in closed: continue closed.add(cur) if cur == self.goal: path = [] while cur in parent: path.append(cur) cur = parent[cur] path.append(self.start) return path[::-1] for nb in self.get_neighbors_5(cur): if nb in closed: continue tentative_g = g_val + (1.414 if abs(nb[0]-cur[0])+abs(nb[1]-cur[1])==2 else 1) if nb not in g or tentative_g < g[nb]: g[nb] = tentative_g f_new = tentative_g + self.heuristic(nb) heapq.heappush(open_set, (f_new, tentative_g, nb)) parent[nb] = cur return None class ImprovedAPF: def __init__(self, goal, k_att=1.0, k_rep=10.0, d0=2.0, k_vel=2.0): self.goal = np.array(goal) self.k_att = k_att self.k_rep = k_rep self.d0 = d0 self.k_vel = k_vel def attractive_force(self, pos, robot_vel=None): d_vec = self.goal - pos dist = np.linalg.norm(d_vec) if dist < 0.5: return -self.k_att * d_vec * (dist / 0.5) # 线性减小 return self.k_att * d_vec def repulsive_force(self, pos, obs_pos, obs_vel, robot_vel): d_vec = pos - obs_pos d = np.linalg.norm(d_vec) if d >= self.d0: return np.zeros(2) dir_vec = d_vec / (d+1e-6) force_rep = self.k_rep * (1/d - 1/self.d0) * (1/d**2) * dir_vec # 速度项 v_rel = robot_vel - obs_vel v_rel_proj = np.dot(v_rel, dir_vec) if v_rel_proj < 0: force_vel = self.k_vel * v_rel_proj * dir_vec else: force_vel = np.zeros(2) return force_rep + force_vel def total_force(self, pos, obstacles, robot_vel=np.zeros(2)): force = self.attractive_force(pos, robot_vel) for obs_pos, obs_vel in obstacles: force += self.repulsive_force(pos, obs_pos, obs_vel, robot_vel) return force