3. 动态窗口法局部规划
任务目标
在仿真环境中实现动态窗口法（Dynamic Window Approach, DWA）局部规划器，使车辆在跟踪全局路径的同时避开动态障碍物。本案例聚焦于DWA核心算法本身，不涉及全局路径规划、传感器融合或底盘控制，全局路径与障碍物状态由仿真环境直接提供。
环境与依赖
- Python 3.8+，仅需NumPy与Matplotlib（用于可视化验证）
- 无需ROS或Autoware环境，本案例以独立仿真验证DWA算法正确性
实现方案
DWA核心思想：在速度空间（线速度v、角速度ω）中采样多组候选速度，根据车辆运动学模型模拟未来一段时间的轨迹，然后通过代价函数评估每条轨迹（目标方向对齐、障碍物距离、速度大小），选择最优速度作为当前控制指令。动态窗口即根据当前速度与加速度限制，只采样可达的速度范围，而非全速度空间。
仿真环境设定：车辆在100m×100m平面内运动，全局路径由一系列离散路径点构成（本案例提供一条示例路径），动态障碍物以圆形表示并沿直线运动。车辆运动学采用差速模型：x' = x + v·cos(θ)·dt，y' = y + v·sin(θ)·dt，θ' = θ + ω·dt。
关键完整代码
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
# ---------- 车辆运动学模型 ----------
def motion_model(x, v, omega, dt):
"""差速模型运动更新"""
x[0] += v * np.cos(x[2]) * dt
x[1] += v * np.sin(x[2]) * dt
x[2] += omega * dt
x[2] = normalize_angle(x[2])
return x
def normalize_angle(angle):
while angle > np.pi: angle -= 2.0 * np.pi
while angle < -np.pi: angle += 2.0 * np.pi
return angle
# ---------- 轨迹预测 ----------
def predict_trajectory(x_init, v, omega, dt, predict_time):
"""从初始状态以固定速度模拟轨迹"""
x = np.array(x_init, dtype=np.float64)
traj = np.array(x)
for _ in range(int(predict_time / dt)):
x = motion_model(x, v, omega, dt)
traj = np.vstack((traj, x))
return traj
# ---------- 动态窗口采样 ----------
def calc_dynamic_window(x, config):
"""根据当前速度与加速度限制计算可达速度范围"""
vs = config['max_speed'] - config['min_speed']
ws = config['max_yawrate'] - config['min_yawrate']
# 车辆动力学限制
vd = [x[3] - config['max_accel'] * config['dt'],
x[3] + config['max_accel'] * config['dt']]
wd = [x[4] - config['max_dyawrate'] * config['dt'],
x[4] + config['max_dyawrate'] * config['dt']]
# 最终动态窗口（取交集）
v_min = max(config['min_speed'], vd[0])
v_max = min(config['max_speed'], vd[1])
w_min = max(config['min_yawrate'], wd[0])
w_max = min(config['max_yawrate'], wd[1])
return [v_min, v_max, w_min, w_max]
# ---------- 代价评估 ----------
def calc_evaluation(x, v, omega, config, goal, obstacles, global_path):
"""评估单条轨迹的代价，返回总代价"""
traj = predict_trajectory(x, v, omega, config['dt'], config['predict_time'])
# 1. 目标方向代价：轨迹末端朝向与目标方向的夹角
dx = goal[0] - traj[-1, 0]
dy = goal[1] - traj[-1, 1]
goal_angle = np.arctan2(dy, dx)
angle_diff = abs(normalize_angle(goal_angle - traj[-1, 2]))
heading_cost = config['w_heading'] * angle_diff
# 2. 障碍物距离代价：轨迹上各点到最近障碍物的最小距离
min_dist = float('inf')
for point in traj[:, :2]:
for obs in obstacles:
dist = np.hypot(point[0] - obs[0], point[1] - obs[1]) - obs[2]
if dist < min_dist:
min_dist = dist
if min_dist < config['robot_radius']:
return float('inf') # 碰撞轨迹直接淘汰
obstacle_cost = config['w_obstacle'] * (1.0 / max(min_dist, 1e-3))
# 3. 速度代价：鼓励高速行驶
speed_cost = config['w_speed'] * (config['max_speed'] - v)
# 4. 全局路径跟踪代价：轨迹末端到最近路径点的距离
path_dist = min(np.hypot(global_path[:, 0] - traj[-1, 0],
global_path[:, 1] - traj[-1, 1]))
path_cost = config['w_path'] * path_dist
return heading_cost + obstacle_cost + speed_cost + path_cost
# ---------- DWA主函数 ----------
def dwa_control(x, config, goal, obstacles, global_path):
"""DWA核心：采样→评估→选择最优速度"""
dw = calc_dynamic_window(x, config)
v_min, v_max, w_min, w_max = dw
best_cost = float('inf')
best_u = [0.0, 0.0]
# 速度空间采样
for v in np.arange(v_min, v_max, config['v_reso']):
for w in np.arange(w_min, w_max, config['w_reso']):
cost = calc_evaluation(x, v, w, config, goal, obstacles, global_path)
if cost < best_cost:
best_cost = cost
best_u = [v, w]
return best_u
# ---------- 仿真主循环 ----------
def run_simulation():
# 配置参数
config = {
'dt': 0.1, # 控制周期
'predict_time': 3.0, # 轨迹预测时长
'v_reso': 0.1, # 线速度采样分辨率
'w_reso': 0.1, # 角速度采样分辨率
'min_speed': 0.0, 'max_speed': 2.0,
'min_yawrate': -1.5, 'max_yawrate': 1.5,
'max_accel': 0.5, 'max_dyawrate': 1.0,
'robot_radius': 1.0, # 车辆碰撞半径
'w_heading': 0.8, # 目标方向权重
'w_obstacle': 1.0, # 障碍物距离权重
'w_speed': 0.1, # 速度权重
'w_path': 0.5 # 路径跟踪权重
}
# 初始状态: [x, y, theta, v, omega]
x = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
# 目标点
goal = [90.0, 90.0]
# 全局路径（示例：从起点到目标点的直线路径）
global_path = np.array([[i, i] for i in range(0, 100, 5)], dtype=np.float64)
# 动态障碍物: [x, y, radius, vx, vy]
obstacles = [
[30.0, 25.0, 3.0, 0.8, 0.3],
[55.0, 60.0, 2.5, -0.5, 0.6],
[70.0, 40.0, 2.0, 0.4, -0.8]
]
# 仿真记录
traj_history = [x[:2].copy()]
goal_reached = False
# 仿真主循环
for step in range(1000):
# 更新障碍物位置（动态）
for obs in obstacles:
obs[0] += obs[3] * config['dt']
obs[1] += obs[4] * config['dt']
# 边界反弹
if obs[0] < 5 or obs[0] > 95: obs[3] *= -1
if obs[1] < 5 or obs[1] > 95: obs[4] *= -1
# 调用DWA计算最优速度
v_opt, w_opt = dwa_control(x, config, goal, obstacles, global_path)
# 更新车辆状态
x = motion_model(x, v_opt, w_opt, config['dt'])
traj_history.append(x[:2].copy())
# 到达判定
if np.hypot(goal[0] - x[0], goal[1] - x[1]) < 2.0:
goal_reached = True
print(f"第{step}步到达目标点")
break
# 碰撞检测
for obs in obstacles:
if np.hypot(x[0] - obs[0], x[1] - obs[1]) < (obs[2] + config['robot_radius']):
print(f"第{step}步发生碰撞！")
break
else:
continue
break
# ---------- 可视化验证 ----------
traj_history = np.array(traj_history)
fig, ax = plt.subplots(figsize=(10, 10))
ax.plot(global_path[:, 0], global_path[:, 1], 'b--', label='全局路径')
ax.plot(traj_history[:, 0], traj_history[:, 1], 'r-', linewidth=2, label='实际轨迹')
ax.plot(goal[0], goal[1], 'g*', markersize=15, label='目标点')
for obs in obstacles:
ax.add_patch(Circle((obs[0], obs[1]), obs[2], color='gray', alpha=0.5))
ax.legend()
ax.set_xlim(-5, 105); ax.set_ylim(-5, 105)
ax.set_aspect('equal')
ax.grid(True)
plt.title('DWA局部规划仿真结果')
plt.savefig('dwa_result.png', dpi=100)
print("仿真结束，结果图已保存为 dwa_result.png")
print(f"最终位置: ({x[0]:.2f}, {x[1]:.2f}), 到达目标: {goal_reached}")
if __name__ == '__main__':
run_simulation()运行方式
python dwa_planner.py验证方法与判定标准
运行程序后，控制台输出仿真过程信息，并生成dwa_result.png可视化结果图。判定通过的观察条件：
- 车辆轨迹（红色实线）从起点(0,0)出发，最终到达目标点(90,90)附近（距离<2m），控制台显示“到达目标点”
- 轨迹全程未与任何灰色障碍物圆相交，控制台无“发生碰撞”输出
- 实际轨迹整体沿全局路径（蓝色虚线）方向前进，在遇到障碍物时产生明显的绕行偏移，绕过障碍物后回到路径方向
若车辆在到达前发生碰撞或长时间徘徊（超过1000步），可调整代价函数权重（w_obstacle调大增强避障，w_heading调大增强目标趋向）后重新运行。