基于A星与DWA算法融合的动态路径规划，可实现静态避障碍及动态避障
深夜撸代码的时候突然想到，路径规划这玩意儿不就是既要全局最优又得能躲开外卖小哥吗？传统A星在静态地图里确实好用，但遇到动态障碍物直接傻眼。DWA（Dynamic Window Approach）虽然能实时避障，可全局路线容易跑偏。这俩货要是能组CP，效果应该不错吧？
先整点A星的干货。核心思想其实就一句话：把地图网格化，用优先队列找最短路径。咱们用Python简单实现个节点类：
class Node: def __init__(self, parent=None, position=None): self.parent = parent self.position = position self.g = 0 # 实际代价 self.h = 0 # 启发式估计 self.f = 0 # 总代价 def __eq__(self, other): return self.position == other.position def __lt__(self, other): return self.f < other.f重点在代价计算这块，g值是真实走过的距离，h值用曼哈顿距离做启发。有个小技巧是在计算相邻节点时，可以给障碍物设置惩罚项，让路径自然远离危险区域：
if grid[new_x][new_y] > obstacle_threshold: current_node.g += penalty_cost # 惩罚系数根据实际情况调整然后是DWA的骚操作，这算法像老司机开车——不断根据当前速度预测未来轨迹。速度采样是关键，别傻乎乎全遍历，用动态窗口缩小范围：
def velocity_samples(v_current, w_current, dt): # 速度增量控制在物理极限内 v_samples = np.linspace(max(0, v_current - a_max*dt), min(v_max, v_current + a_max*dt), num=5) w_samples = np.linspace(max(-w_max, w_current - alpha_max*dt), min(w_max, w_current + alpha_max*dt), num=5) return [(v, w) for v in v_samples for w in w_samples]评价函数得兼顾目标导向、速度和障碍物距离。有个坑要注意——别把动态障碍物当静态处理，得预测它们的运动轨迹：
def evaluation(v, w, robot_pose, goals, dynamic_obstacles): traj = simulate_trajectory(robot_pose, v, w) # 预测轨迹 heading_score = angle_to_goal(traj[-1], goals) # 朝向目标得分 velocity_score = v # 速度得分 obstacle_score = 0 for obs in dynamic_obstacles: predicted_pos = obs.predict(traj[-1][2]) # 根据当前时间戳预测障碍物位置 obstacle_score += 1.0 / (distance(traj, predicted_pos) + 1e-5) return heading_score + velocity_score - obstacle_score * 10融合的核心在于让A生成全局航点，DWA负责局部跟踪。但直接这么搞会翻车——当动态障碍物挡住全局路径时，得让A重新规划。这里用了个状态机机制：
class HybridPlanner: def __init__(self): self.global_path = [] self.current_waypoint = 0 def update(self, obstacles): if need_replan(obstacles): # 检测是否被障碍物阻断 self.global_path = a_star_replan() self.current_waypoint = find_nearest_waypoint() local_goal = self.global_path[self.current_waypoint] dwa_velocity = dwa_plan(current_pose, local_goal, obstacles) if reach_threshold(current_pose, local_goal): self.current_waypoint += 1实测时发现个有趣现象：当动态障碍物速度超过机器人最大速度时，系统会自动生成绕行路径，而不是死磕原路线。这得益于DWA的实时避障和A*的周期重规划形成的正反馈。
最后丢个调参经验：A*的启发式权重别设太高，否则容易贴着障碍物走；DWA的障碍物惩罚系数要和传感器刷新率匹配，否则会抽搐。代码里那些magic number最好做成配置文件，实测时改参数比改代码频繁多了。
这种融合方案在实验室小车上跑出了不错的效果，但放到真实停车场环境还是得考虑点云噪声和通信延迟的问题。下次有机会再聊聊怎么用IMU数据做运动补偿吧。
附Matlab代码)
