基于动态窗口算法的AGV仿真避障 动态窗口算法路径规划仿真 动态窗口方法(DynamicWindowApproach) 是一种可以实现实时避障的局部规划算法，通过将轮式机器人的位置约束转化为速度约束，根据约束进行速度采样，并由一系列的选定速度的动作生成轨迹，结合评价函数选择评分最高的轨迹，实现执行最优速度的问题。 可设置起点目标点，设置地图，设置移动障碍物起始点目标点，未知静态障碍物 黄色是静态障碍物，红色是可移动障碍物

最近在折腾AGV的避障算法，发现动态窗口算法（DWA）这玩意儿挺有意思。它不像A*那样需要全局地图，而是像老司机一样实时判断周围环境，特别适合应对突发障碍物。今天咱们直接上代码，手搓一个带动态/静态障碍物的仿真Demo。

基于动态窗口算法的AGV仿真避障 动态窗口算法路径规划仿真 动态窗口方法(DynamicWindowApproach) 是一种可以实现实时避障的局部规划算法，通过将轮式机器人的位置约束转化为速度约束，根据约束进行速度采样，并由一系列的选定速度的动作生成轨迹，结合评价函数选择评分最高的轨迹，实现执行最优速度的问题。 可设置起点目标点，设置地图，设置移动障碍物起始点目标点，未知静态障碍物 黄色是静态障碍物，红色是可移动障碍物

先搞个AGV的类，把核心参数塞进去：

class AGV:
    def __init__(self):
        self.max_speed = 1.0  # 最大平移速度
        self.min_speed = -0.5 # 允许倒车
        self.max_yaw_rate = 50 * np.pi / 180  # 角速度限制
        self.speed_resolution = 0.05  # 速度采样间隔
        self.yaw_rate_resolution = 1 * np.pi / 180
        self.dt = 0.2  # 单步预测时间
        self.predict_time = 2.0  # 轨迹预测时长
        self.obstacle_radius = 0.5  # 安全距离

重点来了，速度窗口生成。这里有个小技巧——不是所有速度都采样，只在当前速度附近挖个"窗口"：

def calc_dynamic_window(self):
    vs = [self.min_speed, self.max_speed,
          -self.max_yaw_rate, self.max_yaw_rate]
    
    # 考虑制动能力的动态窗口
    dw = [self.current_speed - self.max_accel * self.dt,
          self.current_speed + self.max_accel * self.dt,
          self.current_yaw_rate - self.max_dyaw_rate * self.dt,
          self.current_yaw_rate + self.max_dyaw_rate * self.dt]
    
    # 合并物理极限和动态窗口
    final_window = [max(vs[0], dw[0]), min(vs[1], dw[1]),
                    max(vs[2], dw[2]), min(vs[3], dw[3])]
    return final_window

轨迹评分是灵魂所在，三个关键指标得平衡好：

def score_trajectory(self, trajectory, goal):
    # 目标导向（离终点越近越好）
    goal_cost = np.linalg.norm(trajectory[-1][:2] - goal)
    
    # 路径平滑度（速度越大越直越好）
    speed_cost = -trajectory[-1][3]  # 平移速度
    
    # 障碍物距离（最近障碍物决定）
    obstacle_cost = 0
    for pos in trajectory:
        for obs in obstacles:
            dist = np.linalg.norm(pos[:2] - obs.position)
            if dist < self.obstacle_radius:
                return float('inf')  # 直接排除碰撞轨迹
            obstacle_cost += 1 / dist  # 距离越近惩罚越大
    
    return 0.5*goal_cost + 0.3*obstacle_cost + 0.2*speed_cost

搞个带移动障碍物的仿真场景：

# 障碍物类（红色移动障碍物）
class MovingObstacle:
    def __init__(self, start, end):
        self.pos = start
        self.end = end
        self.speed = 0.3  # 移动速度
        
    def update(self):
        direction = self.end - self.pos
        if np.linalg.norm(direction) > 0.2:
            self.pos += direction / np.linalg.norm(direction) * self.speed

# 静态障碍物直接坐标列表（黄色显示）
static_obs = [[2,3], [4,4], [3,1]] 

实际跑起来的效果很有意思——当红色障碍物突然切入时，AGV会像跳华尔兹一样扭动车身避开。调参时发现个现象：增大目标点的权重会让AGV更莽，而障碍物权重的增加会让它变得疑神疑鬼，甚至在复杂场景里原地打转。建议先用默认参数，然后微调0.1级别的变化。

完整代码跑起来的画面里，黄色圆点像地雷一样固定不动，红色障碍物则按预设路线移动。AGV的预测轨迹用半透明线条显示，能明显看到它在多个候选轨迹中选择了综合评分最高的那条。这种实时规划虽然比不上全局最优，但在动态环境中确实实用得多。