2. 动态窗口法局部避障

任务目标

在存在静态障碍物的二维场景中，实现动态窗口法（DWA）局部避障算法。输入当前机器人位姿与目标点，输出满足运动学约束且无碰撞的线速度与角速度指令，使机器人能安全到达目标。

实现方案

动态窗口法核心思想：在速度空间(v, ω)中，根据机器人当前速度与加速度限制，采样一组可达速度（动态窗口），对每个采样速度模拟未来一段轨迹，用评价函数（目标方向、障碍物距离、速度）选出最优速度指令。本案例使用Python实现，障碍物为静态圆形，机器人模型为差速驱动。

关键完整代码

import numpy as np
import math

# ---------- 参数配置 ----------
DT = 0.1          # 模拟时间步长
SIM_TIME = 3.0    # 轨迹预测时长
MAX_V = 1.0       # 最大线速度 m/s
MIN_V = 0.0
MAX_W = 1.5       # 最大角速度 rad/s
MIN_W = -1.5
MAX_ACC_V = 0.5   # 线加速度 m/s^2
MAX_ACC_W = 1.0   # 角加速度 rad/s^2
V_RESO = 0.05     # 速度采样分辨率
W_RESO = 0.1
GOAL_RADIUS = 0.3 # 到达判定半径
OBST_RADIUS = 0.4 # 障碍物膨胀半径（含车体半径）

# ---------- 核心函数 ----------
def motion_model(x, v, w, dt):
    """差速驱动机器人运动模型"""
    x[2] += w * dt
    x[0] += v * math.cos(x[2]) * dt
    x[1] += v * math.sin(x[2]) * dt
    return x

def calc_dynamic_window(x, v, w):
    """根据当前速度与加速度限制，计算可达速度窗口"""
    return [max(MIN_V, v - MAX_ACC_V * DT), min(MAX_V, v + MAX_ACC_V * DT),
            max(MIN_W, w - MAX_ACC_W * DT), min(MAX_W, w + MAX_ACC_W * DT)]

def calc_trajectory(x, v, w):
    """预测一条轨迹（未来SIM_TIME内的位姿序列）"""
    traj = np.array(x)
    time = 0.0
    while time < SIM_TIME:
        x = motion_model(x.copy(), v, w, DT)
        traj = np.vstack((traj, x))
        time += DT
    return traj

def calc_obstacle_cost(traj, obstacles):
    """轨迹与最近障碍物的最小距离；若碰撞则代价极大"""
    min_dist = float('inf')
    for pos in traj[:, :2]:
        for obs in obstacles:
            d = math.hypot(pos[0] - obs[0], pos[1] - obs[1])
            if d < OBST_RADIUS:
                return float('inf')   # 碰撞
            min_dist = min(min_dist, d)
    return min_dist

def calc_goal_cost(traj, goal):
    """轨迹终点与目标点的距离代价"""
    return math.hypot(traj[-1, 0] - goal[0], traj[-1, 1] - goal[1])

def dwa_control(x, goal, obstacles, v, w):
    """动态窗口法主函数：返回最优速度指令 (v, w)"""
    dw = calc_dynamic_window(x, v, w)
    best_v, best_w, best_score = 0.0, 0.0, float('inf')

    for v_cand in np.arange(dw[0], dw[1], V_RESO):
        for w_cand in np.arange(dw[2], dw[3], W_RESO):
            traj = calc_trajectory(x.copy(), v_cand, w_cand)
            obs_cost = calc_obstacle_cost(traj, obstacles)
            if obs_cost == float('inf'):
                continue   # 碰撞轨迹直接丢弃
            goal_cost = calc_goal_cost(traj, goal)
            # 评价函数：目标距离权重0.6，障碍物距离权重0.4（距离越大越好，取倒数）
            score = 0.6 * goal_cost + 0.4 * (1.0 / obs_cost if obs_cost > 0 else float('inf'))
            if score < best_score:
                best_score = score
                best_v, best_w = v_cand, w_cand
    return best_v, best_w

# ---------- 主循环（最小可运行示例） ----------
def run_dwa():
    # 初始状态 [x, y, theta]
    x = np.array([0.0, 0.0, 0.0])
    goal = np.array([6.0, 4.0])
    obstacles = np.array([[3.0, 2.0], [4.5, 3.5], [2.0, 3.0]])  # 静态圆形障碍物
    v, w = 0.0, 0.0
    path = np.array(x)

    for _ in range(500):   # 最大迭代步数
        if math.hypot(x[0] - goal[0], x[1] - goal[1]) < GOAL_RADIUS:
            print("到达目标点")
            break
        v, w = dwa_control(x, goal, obstacles, v, w)
        x = motion_model(x.copy(), v, w, DT)
        path = np.vstack((path, x))
    return path

if __name__ == '__main__':
    path = run_dwa()
    print(f"规划路径点数: {len(path)}")

运行与验证

运行方式：将上述代码保存为dwa_local_avoid.py，在装有NumPy的Python环境中执行python dwa_local_avoid.py。

验证方法：

• 程序输出“到达目标点”且路径点数有限（通常几十至几百），说明成功规划出无碰撞路径。
• 可自行修改obstacles数组坐标，观察算法在不同障碍布局下仍能绕行到达目标。
• 若将障碍物置于起点与目标连线中间（如[3.0, 2.0]），算法应输出绕行轨迹而非直线穿过。

判定标准：程序正常结束并打印“到达目标点”，且路径点序列中任意一点与所有障碍物中心的距离均大于OBST_RADIUS（0.4m），即视为通过。可通过在run_dwa()中增加碰撞检测断言来自动验证。