目录

一、 核心原理：速度空间采样

二、 算法流程

三、 优缺点分析

四、 应用场景

五、MATLAB实现

动态窗口法（Dynamic Window Approach, DWA）是一种经典的局部路径规划算法，主要用于解决移动机器人（如AGV、扫地机器人、无人车）在未知或动态环境中实时避障和导航的问题。它的核心思想非常直观：在机器人当前速度附近，模拟未来一小段时间内所有可能的运动轨迹，然后从中选出一条“最优”的轨迹来执行。

简单来说，DWA 就是让机器人在每一个控制周期都问自己：“以我现在的速度，接下来几秒钟能怎么走？哪个方向最安全、最快、最接近目标？”

一、 核心原理：速度空间采样

DWA 不直接规划几何路径，而是规划速度。它认为，只要控制好了机器人的线速度（v）和角速度（w），就能控制它的运动轨迹。

1. 动态窗口（Dynamic Window）：

   • 速度约束：机器人有物理极限，速度不能无限大（电机性能限制）。

   • 加速度约束：由于电机扭矩有限，机器人在一个控制周期（如0.1秒）内，速度不能从0瞬间飙到最大，只能从当前速度（v, w）加减一个有限的加速度值。

   • 动态窗口就是由这两个约束共同划定的一个二维速度范围（v, w）。在这个窗口内的速度，是机器人当前物理上能够达到的速度。

2. 轨迹模拟（Trajectory Simulation）：

   对于动态窗口内的每一组（v, w）采样点，算法会假设机器人保持这个速度运动一段预演时间（sim_time，如3秒），模拟出未来的一条圆弧轨迹。

3. 评价函数（Evaluation Function）：

   模拟出所有轨迹后，算法用一个打分函数来评判哪条最好。评价标准通常包括：

   • 朝向（Heading）：轨迹终点方向与目标点方向的夹角，越小越好（对准目标）。

   • 距离（Dist）：轨迹上离最近障碍物的距离，越大越好（保证安全）。

   • 速度（Velocity）：轨迹的线速度，越大越好（追求效率）。

   最终得分公式通常为：Score = α*heading + β*dist + γ*velocity（α, β, γ 为权重系数）。​​​​​​​

二、 算法流程

1. 获取状态：获取机器人当前位姿（x, y, θ）和当前速度（v, w）。

2. 速度采样：在动态窗口内离散采样多组（v, w）。

3. 轨迹模拟：对每组速度进行前向模拟，得到轨迹。

4. 轨迹评价：

   • 剔除非法轨迹：如果模拟轨迹上撞到了障碍物，直接丢弃该速度组。

   • 打分排序：对剩余的安全轨迹计算评价函数得分。

5. 执行：选择得分最高的速度组，发送给机器人底层执行。

6. 循环：回到步骤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: 绘制机器人轮廓和方向标识。

代码运行逻辑总结：

1. 设定仿真场景和机器人参数。

2. 每一步循环中，基于当前状态和物理限制，计算下一时刻可选的速度-角速度组合空间（动态窗口）。

3. 在动态窗口内离散采样，为每对候选控制量生成一条预测轨迹。

4. 用多目标加权的方式评估每条轨迹的代价，包括趋近目标、速度、避障三项。

5. 选择总代价最小的轨迹，其对应的控制量（速度和角速度）将应用于机器人。

6. 更新状态，绘制结果，直到到达目标或仿真结束。​​​​​​​