第四章：动态窗口法(DWA)实战
动态窗口法，简称DWA，是我个人非常喜欢的一个局部路径规划算法。为什么？因为它简单、直观，而且效果出奇的好。说白了，DWA就是在每一时刻，只考虑机器人当前速度附近的一个小窗口，在这个窗口里采样、预测、评价，然后选出最优的那组速度。
我刚开始接触DWA时，觉得这名字挺唬人。后来发现，核心思想就三个字：找速度。找什么速度？能让机器人既跑得快，又不撞墙的速度。
核心思想：DWA将路径规划问题转化为速度空间中的优化问题。在每一控制周期，算法在允许的速度范围内采样多组(v, ω)，预测每组速度对应的轨迹，通过评价函数选出最优轨迹对应的速度。
4.1 速度空间采样
速度空间采样，说白了就是「我能用哪些速度」。但这里有个关键约束——不是所有速度都能用。
我遇到过不少新手，上来就把速度范围设得很大，结果机器人疯狂打转。嗯，这里要注意，DWA的采样空间受三个因素限制：
- 机器人自身限制：最大/最小线速度、角速度。比如差速底盘，v一般在[-0.5, 0.5] m/s，ω在[-π/3, π/3] rad/s。
- 电机加减速能力：当前速度不能突变。假设当前v=0.2 m/s，最大加速度0.5 m/s²，那下一时刻v只能在[0.2-0.5*dt, 0.2+0.5*dt]范围内。
- 安全约束：采样速度必须能在撞到障碍物前停下来。这个我后面细说。
采样过程其实很简单，就是遍历所有可能的(v, ω)组合。我习惯用这样的参数：
# 速度空间采样参数
v_min = -0.5 # 最小线速度 (m/s)
v_max = 0.5 # 最大线速度 (m/s)
w_min = -np.pi/3 # 最小角速度 (rad/s)
w_max = np.pi/3 # 最大角速度 (rad/s)
v_res = 0.01 # 线速度分辨率 (m/s)
w_res = np.pi/180 # 角速度分辨率 (rad/s)
# 当前速度
v_current = 0.2
w_current = 0.1
# 加速度限制
v_acc_max = 0.3 # 最大线加速度 (m/s²)
w_acc_max = 0.5 # 最大角加速度 (rad/s²)
# 动态窗口范围
v_dw_min = max(v_min, v_current - v_acc_max * dt)
v_dw_max = min(v_max, v_current + v_acc_max * dt)
w_dw_min = max(w_min, w_current - w_acc_max * dt)
w_dw_max = min(w_max, w_current + w_acc_max * dt)
# 生成采样空间
v_samples = np.arange(v_dw_min, v_dw_max, v_res)
w_samples = np.arange(w_dw_min, w_dw_max, w_res)我的经验：分辨率别设太细。v_res=0.01，w_res=π/180就够用了。太细了计算量大，效果提升不明显。我曾经试过把分辨率提高10倍，结果机器人反应慢了半拍，反而更容易撞。
4.2 轨迹预测
有了速度样本，下一步就是预测轨迹。说白了，就是假设机器人以(v, ω)匀速运动一段时间，看看会走到哪里。
对于差速底盘，运动模型很简单：
def predict_trajectory(x, y, theta, v, w, dt, steps):
"""
预测轨迹
x, y, theta: 当前位姿
v, w: 线速度、角速度
dt: 时间步长
steps: 预测步数
"""
trajectory = []
for _ in range(steps):
x += v * np.cos(theta) * dt
y += v * np.sin(theta) * dt
theta += w * dt
trajectory.append([x, y, theta])
return np.array(trajectory)这里有个细节：预测步数steps很关键。步数太少，机器人看不到远处的障碍物；步数太多，计算量大，而且预测不准。我一般设8-12步，每步0.1秒，总共预测0.8-1.2秒。
注意：轨迹预测假设机器人匀速运动。实际中机器人速度会变化，但DWA在每个控制周期都会重新规划，所以这个假设可以接受。如果你发现机器人轨迹和预测偏差太大，检查一下控制周期是不是太长了。
4.3 评价函数
评价函数是DWA的灵魂。它决定了机器人「喜欢」什么样的速度。标准DWA用三个指标：
| 指标 | 含义 | 计算公式 |
|---|---|---|
| 方位角评价 | 轨迹终点朝向与目标方向的偏差 | heading = 180° - |目标角 - 终点朝向角| |
| 距离评价 | 轨迹与最近障碍物的距离 | dist = min(轨迹点到所有障碍物的距离) |
| 速度评价 | 速度大小，鼓励快速移动 | vel = |v| |
最终得分是加权和：
def evaluate_trajectory(traj, goal, obstacles, v):
"""
评价一条轨迹
traj: 轨迹点序列
goal: 目标点 (x, y)
obstacles: 障碍物列表 [(x, y), ...]
v: 线速度
"""
# 1. 方位角评价
end_x, end_y, end_theta = traj[-1]
goal_angle = np.arctan2(goal[1] - end_y, goal[0] - end_x)
heading = np.pi - abs(end_theta - goal_angle)
# 2. 距离评价
min_dist = float('inf')
for obs in obstacles:
for point in traj:
dist = np.sqrt((point[0] - obs[0])**2 + (point[1] - obs[1])**2)
min_dist = min(min_dist, dist)
# 如果距离太近，直接淘汰
if min_dist < 0.2: # 安全距离阈值
return -float('inf')
# 3. 速度评价
vel = abs(v)
# 加权求和（需要归一化）
alpha, beta, gamma = 0.5, 0.3, 0.2
score = alpha * heading + beta * min_dist + gamma * vel
return score归一化很重要：三个指标的量纲不同，必须归一化到同一量级。我习惯用最大最小值归一化，或者除以理论最大值。不归一化的话，某个指标会主导决策，机器人行为会很奇怪。
4.4 参数调优
参数调优是DWA最磨人的部分。我当年调参数调了整整一周，才摸清门道。这里分享几个关键参数：
- α (方位角权重)：调大，机器人会直奔目标，但容易撞墙。我一般设0.4-0.6。
- β (距离权重)：调大，机器人会远离障碍物，但可能绕远路。我一般设0.2-0.4。
- γ (速度权重)：调大，机器人跑得快，但可能不够安全。我一般设0.1-0.3。
- 预测时间：0.8-1.5秒。场景复杂用短点，空旷用长点。
- 安全距离：0.15-0.3米。取决于机器人尺寸和传感器精度。
调参技巧：先固定α和γ，只调β。β调到机器人能在障碍物前平稳停下，再调α让机器人更直接地走向目标。最后调γ，在安全和速度之间找平衡。我曾经在一个仓库项目里，花了三天才找到最优参数组合。
4.5 在仿真中部署DWA
部署DWA到仿真环境，我推荐用ROS + Gazebo。下面是一个完整的部署流程：
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist, PoseStamped
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
import numpy as np
class DWAController:
def __init__(self):
rospy.init_node('dwa_controller')
# 参数
self.v_max = 0.5
self.w_max = np.pi/3
self.v_res = 0.01
self.w_res = np.pi/180
self.dt = 0.1
self.predict_steps = 10
self.safe_dist = 0.2
# 权重
self.alpha = 0.5
self.beta = 0.3
self.gamma = 0.2
# 状态
self.pose = None
self.goal = None
self.laser_data = None
# 发布器与订阅器
self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy.Subscriber('/odom', Odometry, self.odom_callback)
rospy.Subscriber('/scan', LaserScan, self.laser_callback)
rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.goal_callback)
def odom_callback(self, msg):
# 提取位姿
self.pose = msg.pose.pose
def laser_callback(self, msg):
# 将激光数据转为障碍物点
self.laser_data = msg
# 转换逻辑略...
def goal_callback(self, msg):
self.goal = msg.pose.position
def run(self):
rate = rospy.Rate(10) # 10Hz控制频率
while not rospy.is_shutdown():
if self.pose and self.goal and self.laser_data:
# 1. 速度空间采样
v_samples, w_samples = self.sample_velocity_space()
# 2. 轨迹预测与评价
best_score = -float('inf')
best_v, best_w = 0, 0
for v in v_samples:
for w in w_samples:
traj = self.predict_trajectory(v, w)
score = self.evaluate_trajectory(traj, v)
if score > best_score:
best_score = score
best_v, best_w = v, w
# 3. 发布控制指令
cmd = Twist()
cmd.linear.x = best_v
cmd.angular.z = best_w
self.cmd_pub.publish(cmd)
rate.sleep()
if __name__ == '__main__':
controller = DWAController()
controller.run()部署注意事项：
- 控制频率至少10Hz，否则机器人反应迟钝
- 激光雷达数据要过滤掉无效点（NaN或Inf）
- 目标点到达判断：距离小于0.1米就认为到达
- 如果机器人卡住，可以加一个「原地旋转」的逃逸策略
4.6 DWA核心逻辑流程图
下面这张图，是我自己总结的DWA核心流程。每次调参时我都会看一眼，提醒自己别漏了关键步骤。
这张图把DWA的五个核心步骤串起来了。你想想看，从获取数据到执行速度，每一步都环环相扣。我曾经在一个项目中，因为忘了过滤激光雷达的无效点，机器人对着空气绕了半天圈。嗯，从那以后我再也不敢跳过数据预处理了。
DWA虽然简单，但用好了效果惊人。它不像A*那样需要全局地图，也不像MPC那样计算量大。说白了，DWA就是「在有限的选择里，做出当下最好的决定」。这种贪心策略在大多数场景下都够用，而且实时性极好。
如果你在仿真中部署DWA，建议先从简单场景开始——空旷环境，一个目标点，一个障碍物。调通后再加复杂场景。我当年就是这么一步步来的，稳扎稳打，反而比一上来就搞复杂场景快得多。
最后说一句：DWA不是万能的。在狭窄通道、U型障碍物等场景，DWA可能会陷入局部最优。这时候需要结合全局规划（比如A*）或者增加记忆机制。但作为局部规划器，DWA绝对是最值得掌握的算法之一。
可可精品资料库