目录
一、 核心原理：速度空间采样
二、 算法流程
三、 优缺点分析
四、 应用场景
五、MATLAB实现
动态窗口法（Dynamic Window Approach, DWA）是一种经典的局部路径规划算法，主要用于解决移动机器人（如AGV、扫地机器人、无人车）在未知或动态环境中实时避障和导航的问题。它的核心思想非常直观：在机器人当前速度附近，模拟未来一小段时间内所有可能的运动轨迹，然后从中选出一条“最优”的轨迹来执行。
简单来说，DWA 就是让机器人在每一个控制周期都问自己：“以我现在的速度，接下来几秒钟能怎么走？哪个方向最安全、最快、最接近目标？”
一、 核心原理：速度空间采样
DWA 不直接规划几何路径，而是规划速度。它认为，只要控制好了机器人的线速度（v）和角速度（w），就能控制它的运动轨迹。
动态窗口（Dynamic Window）：
-
速度约束：机器人有物理极限，速度不能无限大（电机性能限制）。
-
加速度约束：由于电机扭矩有限，机器人在一个控制周期（如0.1秒）内，速度不能从0瞬间飙到最大，只能从当前速度（v, w）加减一个有限的加速度值。
-
动态窗口就是由这两个约束共同划定的一个二维速度范围（v, w）。在这个窗口内的速度，是机器人当前物理上能够达到的速度。
轨迹模拟（Trajectory Simulation）：
对于动态窗口内的每一组（v, w）采样点，算法会假设机器人保持这个速度运动一段预演时间（sim_time，如3秒），模拟出未来的一条圆弧轨迹。
评价函数（Evaluation Function）：
模拟出所有轨迹后，算法用一个打分函数来评判哪条最好。评价标准通常包括：
-
朝向（Heading）：轨迹终点方向与目标点方向的夹角，越小越好（对准目标）。
-
距离（Dist）：轨迹上离最近障碍物的距离，越大越好（保证安全）。
-
速度（Velocity）：轨迹的线速度，越大越好（追求效率）。
最终得分公式通常为：Score = α*heading + β*dist + γ*velocity（α, β, γ 为权重系数）。
二、 算法流程
获取状态：获取机器人当前位姿（x, y, θ）和当前速度（v, w）。
速度采样：在动态窗口内离散采样多组（v, w）。
轨迹模拟：对每组速度进行前向模拟，得到轨迹。
轨迹评价：
-
剔除非法轨迹：如果模拟轨迹上撞到了障碍物，直接丢弃该速度组。
-
打分排序：对剩余的安全轨迹计算评价函数得分。
执行：选择得分最高的速度组，发送给机器人底层执行。
循环：回到步骤1，进入下一个控制周期。
三、 优缺点分析
-
优点：
-
实时性强：计算量小，适合嵌入式系统实时控制。
-
动力学可行：考虑了机器人的加速度约束，规划出的速度平滑，不会出现“急转弯”等物理上无法执行的动作。
-
避障灵敏：只考虑短期未来，对动态障碍物反应快。
-
-
缺点：
-
局部最优陷阱：容易陷入U型障碍物或死胡同出不来（因为它只往前看几秒，看不到全局出口）。
-
参数敏感：评价函数的权重系数（α, β, γ）和预演时间（sim_time）需要大量调试，调不好容易“智障”。
-
前瞻性不足：在复杂迷宫或需要提前减速的弯道，表现不如全局规划算法。
-
四、 应用场景
DWA 通常不单独使用，而是作为局部规划器与全局规划器（如A*、Dijkstra）配合使用：
-
全局规划：负责“宏观导航”，告诉机器人从A到B的大致路线。
-
DWA：负责“微观执行”，沿着全局路线走，同时实时躲避突然出现的行人或障碍物。
五、MATLAB实现
%% 动态窗口法（Dynamic Window Approach, DWA）MATLAB实现
clear; close all; clc;
%% 仿真参数设置
dt = 0.1; % 时间步长
sim_time = 50.0; % 总仿真时间
goal = [10, 10]; % 目标位置
obstacle = [2, 2; 3, 2; 4, 2; 5, 2; 6, 2; 7, 2; 8, 2; 9, 2; 2, 4; 3, 4; 4, 4; 5, 4; 6, 4; 7, 4; 8, 4; 9, 4]; % 障碍物位置
%% 机器人参数
robot_radius = 0.5; % 机器人半径
max_speed = 1.0; % 最大速度
min_speed = -0.5; % 最小速度
max_yawrate = 40.0 * pi / 180.0; % 最大角速度
max_accel = 0.2; % 最大加速度
max_dyawrate = 40.0 * pi / 180.0; % 最大角加速度
v_reso = 0.01; % 速度分辨率
yawrate_reso = 0.1 * pi / 180.0; % 角速度分辨率
predict_time = 3.0; % 预测时间
to_goal_cost_gain = 0.15; % 目标代价增益
speed_cost_gain = 1.0; % 速度代价增益
obstacle_cost_gain = 1.0; % 障碍物代价增益
%% 初始化状态
state = [0, 0, pi/2, 0, 0]; % [x, y, yaw, v, w]
trajectory = state; % 轨迹记录
goal_dis = calc_distance(state, goal);
%% 主循环
for i = 1:sim_time/dt
% 计算动态窗口
[v_min, v_max, w_min, w_max] = calc_dynamic_window(state, max_speed, min_speed, max_yawrate, max_accel, max_dyawrate, dt);
% 评估窗口内的所有可能轨迹
[eval_v, eval_w, eval_traj] = evaluate_trajectory(state, goal, obstacle, v_min, v_max, v_reso, w_min, w_max, yawrate_reso, dt, predict_time, robot_radius, max_speed, to_goal_cost_gain, speed_cost_gain, obstacle_cost_gain);
if isempty(eval_v)
disp('找不到可行路径！');
break;
end
% 更新状态
state = motion(state, eval_v, eval_w, dt);
% 记录轨迹
trajectory = [trajectory; state];
% 检查是否到达目标
if calc_distance(state, goal) <= 0.5
disp('到达目标！');
break;
end
% 实时显示
plot_animation(state, trajectory, goal, obstacle, robot_radius, eval_traj, i*dt);
drawnow;
pause(0.1);
end
%% 动态窗口计算函数
function [v_min, v_max, w_min, w_max] = calc_dynamic_window(state, max_speed, min_speed, max_yawrate, max_accel, max_dyawrate, dt)
% 当前速度限制
v_current = state(4);
w_current = state(5);
% 考虑加速度的动态窗口
v_min_dyn = max(min_speed, v_current – max_accel * dt);
v_max_dyn = min(max_speed, v_current + max_accel * dt);
w_min_dyn = max(-max_yawrate, w_current – max_dyawrate * dt);
w_max_dyn = min(max_yawrate, w_current + max_dyawrate * dt);
v_min = v_min_dyn;
v_max = v_max_dyn;
w_min = w_min_dyn;
w_max = w_max_dyn;
end
%% 轨迹评估函数
function [best_v, best_w, best_traj] = evaluate_trajectory(state, goal, obstacle, v_min, v_max, v_reso, w_min, w_max, yawrate_reso, dt, predict_time, robot_radius, max_speed, to_goal_cost_gain, speed_cost_gain, obstacle_cost_gain)
best_cost = inf;
best_v = 0;
best_w = 0;
best_traj = [];
% 遍历所有可能的速度组合
for v = v_min:v_reso:v_max
for w = w_min:yawrate_reso:w_max
% 生成轨迹
traj = generate_trajectory(state, v, w, dt, predict_time);
% 计算代价
to_goal_cost = calc_to_goal_cost(traj, goal);
speed_cost = calc_speed_cost(v, max_speed);
obstacle_cost = calc_obstacle_cost(traj, obstacle, robot_radius);
% 总代价
total_cost = to_goal_cost_gain * to_goal_cost + speed_cost_gain * speed_cost + obstacle_cost_gain * obstacle_cost;
% 更新最佳轨迹
if total_cost < best_cost
best_cost = total_cost;
best_v = v;
best_w = w;
best_traj = traj;
end
end
end
end
%% 轨迹生成函数
function traj = generate_trajectory(state, v, w, dt, predict_time)
time = 0;
traj = [state];
while time <= predict_time
state = motion(state, v, w, dt);
traj = [traj; state];
time = time + dt;
end
end
%% 运动模型函数
function state_next = motion(state, v, w, dt)
state_next(1) = state(1) + v * cos(state(3)) * dt;
state_next(2) = state(2) + v * sin(state(3)) * dt;
state_next(3) = state(3) + w * dt;
state_next(4) = v;
state_next(5) = w;
end
%% 目标代价计算函数
function cost = calc_to_goal_cost(traj, goal)
% 使用轨迹终点到目标的距离作为代价
dx = goal(1) – traj(end, 1);
dy = goal(2) – traj(end, 2);
cost = sqrt(dx^2 + dy^2);
end
%% 速度代价计算函数
function cost = calc_speed_cost(v, max_speed)
% 鼓励高速运动
cost = max_speed – v;
end
%% 障碍物代价计算函数
function cost = calc_obstacle_cost(traj, obstacle, robot_radius)
min_dis = inf;
for i = 1:size(traj, 1)
for j = 1:size(obstacle, 1)
dx = traj(i, 1) – obstacle(j, 1);
dy = traj(i, 2) – obstacle(j, 2);
dis = sqrt(dx^2 + dy^2) – robot_radius;
if dis < min_dis
min_dis = dis;
end
end
end
% 如果距离太小，给予很大代价
if min_dis <= 0
cost = inf;
elseif min_dis <= robot_radius
cost = 1.0 / min_dis;
else
cost = 0;
end
end
%% 距离计算函数
function dis = calc_distance(state, goal)
dx = goal(1) – state(1);
dy = goal(2) – state(2);
dis = sqrt(dx^2 + dy^2);
end
%% 绘图函数
function plot_animation(state, trajectory, goal, obstacle, robot_radius, eval_traj, time)
clf;
hold on;
grid on;
axis equal;
% 设置坐标轴范围
xlim([-1, 12]);
ylim([-1, 12]);
xlabel('X [m]');
ylabel('Y [m]');
title(sprintf('DWA路径规划 (时间: %.1fs)', time));
% 绘制目标
plot(goal(1), goal(2), 'r*', 'MarkerSize', 15, 'LineWidth', 2);
text(goal(1)+0.3, goal(2), '目标');
% 绘制障碍物
plot(obstacle(:,1), obstacle(:,2), 'ks', 'MarkerSize', 10, 'MarkerFaceColor', 'k');
% 绘制历史轨迹
plot(trajectory(:,1), trajectory(:,2), 'b-', 'LineWidth', 2);
% 绘制评估的最佳轨迹
if ~isempty(eval_traj)
plot(eval_traj(:,1), eval_traj(:,2), 'g–', 'LineWidth', 1);
end
% 绘制机器人当前位置
draw_robot(state, robot_radius);
% 绘制机器人方向
quiver(state(1), state(2), cos(state(3)), sin(state(3)), 0.5, 'r', 'LineWidth', 2, 'MaxHeadSize', 2);
% 添加图例
legend('目标', '障碍物', '历史轨迹', '预测轨迹', '机器人', '方向', 'Location', 'best');
hold off;
end
%% 绘制机器人函数
function draw_robot(state, robot_radius)
theta = linspace(0, 2*pi, 100);
x = robot_radius * cos(theta) + state(1);
y = robot_radius * sin(theta) + state(2);
fill(x, y, [0.8, 0.8, 0.8], 'EdgeColor', 'k', 'LineWidth', 1);
% 绘制机器人前方标识
front_x = [state(1), state(1) + robot_radius * cos(state(3))];
front_y = [state(2), state(2) + robot_radius * sin(state(3))];
plot(front_x, front_y, 'r-', 'LineWidth', 2);
end
这段代码是用MATLAB实现的动态窗口法(DWA)路径规划算法。其结构主要分为以下几个部分：
1. 主脚本部分
-
初始化与参数设置：包括仿真参数、机器人物理参数、代价增益系数、初始状态等。
-
主循环：核心控制流程，在每一步中动态计算速度窗口、评估候选轨迹、选择最佳控制量、更新状态并可视化。
2. 辅助计算函数
-
calc_dynamic_window: 计算满足机器人运动学和加速度约束的速度动态窗口。
-
calc_distance: 计算两点间欧氏距离的通用函数。
3. 核心规划函数
-
evaluate_trajectory: 遍历所有候选速度对，生成对应轨迹，并评估其总代价，选出最优轨迹。
-
generate_trajectory: 根据给定速度和角速度，在预测时间内模拟生成一条轨迹。
4. 代价计算函数
-
calc_to_goal_cost: 评估轨迹终点到目标点的距离。
-
calc_speed_cost: 评估速度，鼓励高速运动。
-
calc_obstacle_cost: 评估轨迹到最近障碍物的距离，距离越近代价越高，碰撞时代价为无穷大。
5. 运动与模型
-
motion: 机器人运动学模型，根据当前状态、控制量（速度、角速度）和时延更新下一状态。
6. 可视化函数
-
plot_animation: 主绘图函数，绘制目标、障碍物、历史轨迹、当前最佳预测轨迹、机器人当前位置和朝向。
-
draw_robot: 绘制机器人轮廓和方向标识。
代码运行逻辑总结：
设定仿真场景和机器人参数。
每一步循环中，基于当前状态和物理限制，计算下一时刻可选的速度-角速度组合空间（动态窗口）。
在动态窗口内离散采样，为每对候选控制量生成一条预测轨迹。
用多目标加权的方式评估每条轨迹的代价，包括趋近目标、速度、避障三项。
选择总代价最小的轨迹，其对应的控制量（速度和角速度）将应用于机器人。
更新状态，绘制结果，直到到达目标或仿真结束。
