✨ 长期致力于路径规划、A*算法、人工势场法、栅格地图、ROS研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1栅格地图预处理算法针对传统栅格地图无法处理不规则障碍物和危险区域的问题提出预处理三步法。第一步形态学膨胀对障碍物栅格使用半径为2的圆形结构元素进行膨胀填充狭窄缝隙确保规划出的路径不会从间隙中穿过间隙宽度小于机器人直径。第二步危险区域标记用户自定义高风险区域如陡坡、积水区在栅格地图中将这些区域设为不可通行并附加额外代价。第三步道路宽度检测对每个自由栅格计算到最近障碍物的距离若该距离小于机器人半径安全余量0.2m则标记为狭窄通道路径规划时优先选择宽度较大的区域。在MATLAB中对50x50栅格地图进行测试预处理后传统A*的搜索节点减少10-13%时间减少27-33%。预处理后的地图路径不再出现紧贴障碍物的情况安全性提升。2五邻域搜索与启发式函数赋权A*改进传统A*的两个核心组件。邻域搜索从八方向改为五方向抛弃左下、右下、左上、右上四个对角方向对角移动用一个横向加一个纵向代替这大幅减少了拐点数量。启发式函数h(n)采用欧氏距离乘以权重系数ww根据当前节点附近障碍物密度动态调整密度高时w增大使算法更倾向远离障碍物。权重计算为w 1 0.5 * (1 - 局部自由栅格占比)。在障碍率不同的四种地图上对比改进A*的搜索节点减少4-12%搜索时间减少10-34%。路径长度平均增加不超过4%但路径平滑度曲率变化改善40%。在ROS的move_base框架中以插件形式实现该改进A*实际测试规划时间由0.35秒降至0.23秒。3A*-人工势场融合算法与实物部署融合设计分为两层上层采用改进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_radius0.3, safety_margin0.2): self.grid grid self.radius robot_radius self.safety safety_margin def dilate_obstacles(self, kernel_size2): 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, r3): x,y node subgrid self.grid[max(0,x-r):min(self.grid.shape[0],xr1), max(0,y-r):min(self.grid.shape[1],yr1)] free np.sum(subgrid0) 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 [(x1,y),(x-1,y),(x,y1),(x,y-1),(x1,y1)] neighbors [] for nx,ny in candidates: if 0nxself.grid.shape[0] and 0nyself.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_att1.0, k_rep10.0, d02.0, k_vel2.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_velNone): 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 / (d1e-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_velnp.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