2. 动态窗口法局部避障

任务目标

在包含动态障碍物的二维平面环境中，实现动态窗口法（DWA）局部规划器。规划器接收机器人当前位姿、速度以及障碍物状态，输出一个安全且可达的速度指令（线速度 v 与角速度 ω），使机器人在避开动态障碍物的同时向目标点移动。

核心原理与实现策略

动态窗口法在速度空间 (v, ω) 中进行采样，仅考虑机器人当前速度在一个控制周期内能够实际达到的速度集合（即动态窗口）。对窗口内的每组速度进行轨迹推演，评估其安全性（是否碰撞）、方向性（与目标夹角）与速度大小，加权求和后选择最优速度。针对动态障碍物，轨迹推演时使用障碍物的预测位置（假设其保持当前速度匀速运动），从而在规划层面实现避障。

环境依赖

仅需 Python 3.6+ 与 NumPy 库，无需其他第三方依赖。

关键完整代码

import numpy as np
import math

# ---------- 参数配置 ----------
class Config:
    def __init__(self):
        # 机器人运动学限制
        self.max_speed = 1.0          # 最大线速度 [m/s]
        self.min_speed = -0.2         # 最小线速度（允许后退）
        self.max_yaw_rate = 40.0 * math.pi / 180.0  # 最大角速度 [rad/s]
        self.max_accel = 0.5          # 最大线加速度 [m/s^2]
        self.max_delta_yaw_rate = 80.0 * math.pi / 180.0  # 最大角加速度 [rad/s^2]
        # 规划参数
        self.dt = 0.1                 # 轨迹推演时间步长 [s]
        self.predict_time = 2.0       # 轨迹推演总时长 [s]
        self.v_resolution = 0.05      # 线速度采样分辨率
        self.yaw_rate_resolution = 0.1 * math.pi / 180.0  # 角速度采样分辨率
        # 评价函数权重
        self.alpha = 0.8              # 方向性权重
        self.beta = 0.6               # 速度权重
        self.gamma = 0.4              # 障碍物距离权重
        # 安全距离
        self.robot_radius = 0.3       # 机器人半径 [m]
        self.safe_dist = 0.5          # 期望保持的障碍物距离 [m]

# ---------- 核心函数 ----------
def motion_model(x, u, dt):
    """根据速度指令更新位姿"""
    x_new = np.zeros(3)
    x_new[0] = x[0] + u[0] * math.cos(x[2]) * dt
    x_new[1] = x[1] + u[0] * math.sin(x[2]) * dt
    x_new[2] = x[2] + u[1] * dt
    return x_new

def predict_trajectory(x_init, v, w, config, dynamic_obs):
    """推演轨迹，返回轨迹点序列与最小障碍物距离"""
    traj = [x_init.copy()]
    x = x_init.copy()
    min_dist = float('inf')
    time = 0.0
    while time <= config.predict_time:
        x = motion_model(x, [v, w], config.dt)
        traj.append(x.copy())
        # 计算与所有障碍物（含动态预测位置）的最小距离
        for obs in dynamic_obs:
            # 动态障碍物按当前速度匀速运动预测
            obs_x = obs[0] + obs[2] * time
            obs_y = obs[1] + obs[3] * time
            dist = math.hypot(x[0] - obs_x, x[1] - obs_y)
            if dist < min_dist:
                min_dist = dist
        time += config.dt
    return traj, min_dist

def calc_dynamic_window(x, config):
    """根据当前速度计算机器人实际可达的速度窗口"""
    vs = [config.min_speed, config.max_speed,
          -config.max_yaw_rate, config.max_yaw_rate]
    # 考虑加速度限制
    v_high = min(vs[1], x[3] + config.max_accel * config.dt)
    v_low = max(vs[0], x[3] - config.max_accel * config.dt)
    w_high = min(vs[3], x[4] + config.max_delta_yaw_rate * config.dt)
    w_low = max(vs[2], x[4] - config.max_delta_yaw_rate * config.dt)
    return [v_low, v_high, w_low, w_high]

def dwa_control(x, config, goal, dynamic_obs):
    """动态窗口法主函数，返回最优速度指令 [v, w]"""
    dw = calc_dynamic_window(x, config)
    v_range = np.arange(dw[0], dw[1], config.v_resolution)
    w_range = np.arange(dw[2], dw[3], config.yaw_rate_resolution)

    best_u = [0.0, 0.0]
    best_score = -float('inf')
    best_traj = [x.copy()]

    for v in v_range:
        for w in w_range:
            traj, min_dist = predict_trajectory(x, v, w, config, dynamic_obs)
            # 碰撞检测：最小距离小于安全阈值则丢弃
            if min_dist < config.safe_dist:
                continue
            # 评价函数
            goal_dist = math.hypot(traj[-1][0] - goal[0], traj[-1][1] - goal[1])
            heading_score = math.pi - abs(math.atan2(goal[1] - traj[-1][1],
                                                     goal[0] - traj[-1][0]) - traj[-1][2])
            # 归一化处理
            heading_norm = heading_score / math.pi
            vel_norm = abs(v) / config.max_speed
            dist_norm = min(min_dist / (config.predict_time * config.max_speed), 1.0)
            score = config.alpha * heading_norm + config.beta * vel_norm + config.gamma * dist_norm
            if score > best_score:
                best_score = score
                best_u = [v, w]
                best_traj = traj
    return best_u, best_traj

# ---------- 最小调用示例 ----------
if __name__ == "__main__":
    config = Config()
    # 机器人状态: [x, y, theta, v, omega]
    robot_state = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
    goal = np.array([5.0, 3.0])
    # 动态障碍物: [x, y, vx, vy] (匀速运动)
    dynamic_obstacles = [
        [3.0, 1.5, 0.3, 0.0],   # 向右移动
        [2.0, 2.5, -0.2, 0.1]   # 向左上方移动
    ]
    u, traj = dwa_control(robot_state, config, goal, dynamic_obstacles)
    print(f"最优速度指令: v={u[0]:.2f} m/s, omega={u[1]:.2f} rad/s")
    print(f"推演轨迹点数: {len(traj)}")

运行与验证

运行方式：将上述代码保存为 dwa_local_planner.py，在终端执行 python dwa_local_planner.py。

验证方法：

程序应输出一个具体的速度指令（v 与 ω 均为有限数值），且该指令对应的推演轨迹上所有点与动态障碍物预测位置的最小距离均大于 safe_dist（0.5m）。
修改 goal 为机器人后方位置（如 [-2.0, 0.0]），重新运行，观察输出速度应包含负的线速度（后退），证明方向性评价函数生效。
将 dynamic_obstacles 中某个障碍物直接放置在机器人正前方 0.6m 处（如 [0.6, 0.0, 0.0, 0.0]），运行后输出的轨迹应绕开该点，不会直线撞向障碍物。

通过条件：上述三种场景下程序均能输出合理的避障速度指令，且推演轨迹与所有障碍物预测位置保持安全距离，即可判定实现正确。

常见问题

无可行速度输出：当动态窗口内所有速度均导致碰撞时，函数会返回 [0,0] 并输出全零速度。实际工程中应增加安全停止逻辑或扩大安全距离阈值。
动态障碍物预测模型：本实现假设障碍物匀速直线运动。若障碍物运动模式更复杂，需替换 predict_trajectory 中的位置预测公式。

📚 课程目录