融合跳点A*和动态窗口法 路径规划 DWA

在机器人路径规划领域，总有个经典难题：全局规划器规划的路径像高速公路一样完美，结果路上突然冒出来的动态障碍物瞬间让导航计划泡汤。这时候就需要把全局路径规划和局部避障算法组合使用，今天咱们来聊聊如何让跳点搜索（JPS）和动态窗口法（DWA）这对组合拳打出最佳效果。

先看个典型场景：仓库AGV从A点出发到B点，全局规划了一条最优路径。当执行到某个转弯处时，突然冒出个临时堆放货物的工人。这时候全局路径直接废了？当然不是！我们的DWA就该登场了，它会实时计算可行的速度组合，让机器人既能避障又不偏离大方向。

先说JPS的核心优势——它像开了透视挂一样，能跳过大量无用节点的扩展。看这段跳点检测的Python实现：

def jump(self, x, y, dx, dy):
    # 强制邻居检测
    if self.has_forced_neighbor(x, y, dx, dy):
        return (x, y)
    
    # 直线跳跃
    nx, ny = x + dx, y + dy
    if not self.is_free(nx, ny):
        return None
    
    # 对角线跳跃特殊处理
    if dx != 0 and dy != 0:
        if self.jump(nx, ny, dx, 0) or self.jump(nx, ny, 0, dy):
            return (nx, ny)
    
    return self.jump(nx, ny, dx, dy)

这段代码实现的是跳点传播的核心逻辑。其中hasforcedneighbor检测是否存在必须处理的拐点，is_free判断节点是否可通行。重点注意对角线移动时的双重检测机制，这种设计大幅减少了节点扩展数量，比传统A*快3-5倍不是吹的。

融合跳点A*和动态窗口法 路径规划 DWA

但JPS生成的路径就像用直尺画出来的折线，直接让机器人执行肯定会卡死。这时候DWA的局部调整就派上用场了。来看DWA的速度采样循环：

def compute_velocities(self):
    for v in np.arange(-max_v, max_v, v_resolution):
        for w in np.arange(-max_w, max_w, w_resolution):
            if abs(w) < 0.01: continue  # 过滤微小旋转
            
            traj = simulate_trajectory(current_pose, v, w)
            cost = obstacle_cost(traj) + path_cost(traj) 
            
            if cost < best_cost and feasible_check(traj):
                best_vel = (v, w)
    return best_vel

这里用了网格搜索遍历所有可能的速度组合，simulatetrajectory模拟未来0.5秒的运动轨迹。关键点在于代价函数的设计——既要考虑与障碍物的距离（obstaclecost），也要评估轨迹与全局路径的偏离程度（path_cost）。我通常会给路径偏离项加个权重系数，避免机器人为了避障彻底跑偏。

融合时的注意事项：

1. 全局路径的"引力"设置：在DWA的代价函数中，全局路径的引导权重需要动态调整。离障碍物近时降低权重，空旷区域提高权重
2. 重规划触发机制：当局部避障超过10秒仍未回归原路径，触发全局重规划
3. 速度衔接处理：JPS路径的转折点需要做速度衔接处理，避免DWA在拐弯时急刹

实测中发现个有趣现象：在走廊环境里，纯DWA容易让机器人走"之"字形路线，而加入JPS引导后轨迹明显平滑很多。这是因为全局路径提供了方向性约束，相当于给DWA的随机搜索加了导航箭头。

最后分享一个调参技巧：把DWA的轨迹模拟时间设置为机器人最大减速度停止所需时间的1.2倍。比如机器人最大减速度是0.5m/s²，当前速度0.6m/s，则模拟时间设为(0.6/0.5)*1.2≈1.44秒。这样既能保证安全，又不会因过于保守影响效率。