动态窗口法（DWA）二维路径规划MATLAB实现

动态窗口法（Dynamic Window Approach, DWA）是一种高效的局部路径规划算法，适用于移动机器人在动态环境中的实时导航。它通过动态计算速度空间窗口，评估多条候选轨迹，选择最优路径避开障碍物并接近目标点。

一、算法原理

1.1 DWA核心思想

1. 动态窗口：在速度空间(v, ω)中采样可行速度组合

2. 轨迹预测：模拟机器人在短时间内的运动轨迹

3. 轨迹评价：基于三个准则评估轨迹优劣

4. 最优选择：执行最优轨迹对应的速度指令

1.2 数学模型

• 机器人运动学模型：

  x(t+Δt) = x(t) + v·cosθ·Δt
  y(t+Δt) = y(t) + v·sinθ·Δt
  θ(t+Δt) = θ(t) + ω·Δt
  

• 动态窗口约束：

  V_r = [v_min, v_max] ∩ [v_c - a·Δt, v_c + a·Δt]
  Ω_r = [ω_min, ω_max] ∩ [ω_c - α·Δt, ω_c + α·Δt]
  

• 评价函数：

  G(v,ω) = α·heading(v,ω) + β·dist(v,ω) + γ·vel(v,ω)
  

二、MATLAB实现

2.1 主程序

function dynamic_window_planning()
    % 初始化参数
    params = init_parameters();

    % 创建环境
    [obstacles, goal] = create_environment(params);

    % 初始化机器人状态
    robot = init_robot(params);

    % 路径记录
    path = robot.position;

    % 主循环
    while norm(robot.position - goal) > params.goal_threshold
        % 动态窗口计算
        [V, Omega] = compute_dynamic_window(robot, params);

        % 轨迹预测与评价
        best_trajectory = [];
        best_score = -inf;

        for v = V(1):params.v_res:V(2)
            for omega = Omega(1):params.omega_res:Omega(2)
                % 预测轨迹
                traj = predict_trajectory(robot, v, omega, params);

                % 评价轨迹
                score = evaluate_trajectory(traj, goal, obstacles, params);

                % 选择最优轨迹
                if score > best_score
                    best_score = score;
                    best_traj = traj;
                    best_v = v;
                    best_omega = omega;
                end
            end
        end

        % 执行最优速度
        robot = update_robot_state(robot, best_v, best_omega, params);
        path = [path; robot.position];

        % 可视化
        visualize(robot, path, best_traj, obstacles, goal, params);
        pause(0.05);
    end

    % 显示结果
    disp('目标点已到达!');
    plot_final_path(path, obstacles, goal, params);
end

2.2 参数初始化

function params = init_parameters()
    % 机器人参数
    params.max_speed = 1.0;        % 最大线速度 (m/s)
    params.min_speed = 0.0;        % 最小线速度 (m/s)
    params.max_omega = pi/2;       % 最大角速度 (rad/s)
    params.min_omega = -pi/2;      % 最小角速度 (rad/s)
    params.accel = 0.5;            % 最大加速度 (m/s²)
    params.alpha = pi/4;           % 最大角加速度 (rad/s²)
    params.robot_radius = 0.2;     % 机器人半径 (m)
    params.v_res = 0.01;           % 速度分辨率 (m/s)
    params.omega_res = 0.1;        % 角速度分辨率 (rad/s)
    params.dt = 0.1;               % 时间步长 (s)
    params.predict_time = 3.0;      % 预测时间 (s)
    params.goal_threshold = 0.3;    % 目标到达阈值 (m)

    % 评价函数权重
    params.alpha_heading = 0.7;    % 航向权重
    params.alpha_dist = 0.3;       % 障碍物距离权重
    params.alpha_vel = 0.1;        % 速度权重
end

2.3 环境创建

function [obstacles, goal] = create_environment(params)
    % 障碍物定义 (x, y, 半径)
    obstacles = [
        2.0, 2.0, 0.5;
        4.0, 5.0, 0.8;
        7.0, 3.0, 0.6;
        5.0, 8.0, 0.7;
        8.0, 7.0, 0.4;
        3.0, 7.0, 0.5
    ];

    % 目标点位置
    goal = [10.0, 10.0];
end

2.4 机器人初始化

function robot = init_robot(params)
    % 初始位置和方向
    robot.position = [0.0, 0.0];
    robot.theta = 0.0;  % 朝向 (rad)
    robot.velocity = 0.0;
    robot.omega = 0.0;
end

2.5 动态窗口计算

function [V, Omega] = compute_dynamic_window(robot, params)
    % 速度窗口约束
    V_min = max(params.min_speed, robot.velocity - params.accel*params.dt);
    V_max = min(params.max_speed, robot.velocity + params.accel*params.dt);
    V = [V_min, V_max];

    % 角速度窗口约束
    Omega_min = max(params.min_omega, robot.omega - params.alpha*params.dt);
    Omega_max = min(params.max_omega, robot.omega + params.alpha*params.dt);
    Omega = [Omega_min, Omega_max];
end

2.6 轨迹预测

function trajectory = predict_trajectory(robot, v, omega, params)
    % 初始化轨迹
    trajectory = zeros(floor(params.predict_time/params.dt)+1, 2);
    x = robot.position(1);
    y = robot.position(2);
    theta = robot.theta;

    % 模拟轨迹
    for i = 1:size(trajectory, 1)
        trajectory(i, :) = [x, y];
        x = x + v * cos(theta) * params.dt;
        y = y + v * sin(theta) * params.dt;
        theta = theta + omega * params.dt;
    end
end

2.7 轨迹评价

function score = evaluate_trajectory(traj, goal, obstacles, params)
    % 1. 航向评分：终点朝向目标的程度
    last_point = traj(end, :);
    goal_vec = goal - last_point;
    goal_angle = atan2(goal_vec(2), goal_vec(1));
    heading_score = cos(goal_angle);  % 角度差越小，评分越高

    % 2. 障碍物距离评分：轨迹上离障碍物的最小距离
    min_dist = inf;
    for i = 1:size(traj, 1)
        point = traj(i, :);
        for j = 1:size(obstacles, 1)
            obs = obstacles(j, 1:2);
            obs_r = obstacles(j, 3);
            dist = norm(point - obs) - obs_r - params.robot_radius;
            if dist < min_dist
                min_dist = dist;
            end
        end
    end

    % 避免除以零
    if min_dist < 0.1
        dist_score = 0;  % 碰撞危险
    else
        dist_score = 1 / min_dist;  % 距离越大评分越高
    end

    % 3. 速度评分：鼓励快速前进
    vel_score = norm([traj(2,1)-traj(1,1), traj(2,2)-traj(1,2)]) / params.dt;

    % 综合评分
    score = params.alpha_heading * heading_score + ...
            params.alpha_dist * dist_score + ...
            params.alpha_vel * vel_score;
end

2.8 机器人状态更新

function robot = update_robot_state(robot, v, omega, params)
    % 更新位置和方向
    robot.position(1) = robot.position(1) + v * cos(robot.theta) * params.dt;
    robot.position(2) = robot.position(2) + v * sin(robot.theta) * params.dt;
    robot.theta = robot.theta + omega * params.dt;

    % 更新速度
    robot.velocity = v;
    robot.omega = omega;
end

2.9 可视化函数

function visualize(robot, path, traj, obstacles, goal, params)
    clf;
    hold on;
    axis equal;
    grid on;
    xlim([-1, 12]);
    ylim([-1, 12]);
    title('动态窗口法路径规划');
    xlabel('X (m)');
    ylabel('Y (m)');

    % 绘制障碍物
    for i = 1:size(obstacles, 1)
        rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                              obstacles(i,2)-obstacles(i,3), ...
                              2*obstacles(i,3), 2*obstacles(i,3)], ...
                  'Curvature', [1,1], 'FaceColor', [0.8,0.2,0.2]);
    end

    % 绘制目标点
    plot(goal(1), goal(2), 'gp', 'MarkerSize', 15, 'MarkerFaceColor', 'g');

    % 绘制历史路径
    plot(path(:,1), path(:,2), 'b-', 'LineWidth', 1.5);

    % 绘制预测轨迹
    plot(traj(:,1), traj(:,2), 'm--', 'LineWidth', 1.2);

    % 绘制机器人当前位置
    robot_plot = rectangle('Position', [robot.position(1)-params.robot_radius, ...
                                         robot.position(2)-params.robot_radius, ...
                                         2*params.robot_radius, 2*params.robot_radius], ...
                           'Curvature', [1,1], 'FaceColor', [0.2,0.4,0.8]);

    % 绘制机器人朝向
    quiver(robot.position(1), robot.position(2), ...
           params.robot_radius*cos(robot.theta), params.robot_radius*sin(robot.theta), ...
           'Color', 'k', 'LineWidth', 2, 'MaxHeadSize', 2);

    % 添加图例
    legend('障碍物', '目标点', '历史路径', '预测轨迹', '机器人', '朝向', 'Location', 'Best');

    drawnow;
end

2.10 最终路径绘制

function plot_final_path(path, obstacles, goal, params)
    figure;
    hold on;
    axis equal;
    grid on;
    xlim([-1, 12]);
    ylim([-1, 12]);
    title('最终路径规划结果');
    xlabel('X (m)');
    ylabel('Y (m)');

    % 绘制障碍物
    for i = 1:size(obstacles, 1)
        rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                              obstacles(i,2)-obstacles(i,3), ...
                              2*obstacles(i,3), 2*obstacles(i,3)], ...
                  'Curvature', [1,1], 'FaceColor', [0.8,0.2,0.2]);
    end

    % 绘制目标点
    plot(goal(1), goal(2), 'gp', 'MarkerSize', 15, 'MarkerFaceColor', 'g');

    % 绘制路径
    plot(path(:,1), path(:,2), 'b-', 'LineWidth', 2);
    plot(path(1,1), path(1,2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');

    % 添加标签
    text(path(1,1)+0.2, path(1,2)+0.2, '起点', 'FontSize', 12);
    text(goal(1)+0.2, goal(2)+0.2, '目标', 'FontSize', 12);

    % 添加网格和边框
    grid on;
    box on;
end

三、算法优化与扩展

3.1 自适应参数调整

function params = adaptive_parameters(robot, goal, obstacles, params)
    % 根据距离调整预测时间
    dist_to_goal = norm(robot.position - goal);
    params.predict_time = min(3.0, max(1.0, dist_to_goal/2));

    % 根据障碍物密度调整权重
    obstacle_density = count_nearby_obstacles(robot.position, obstacles, 3.0);
    if obstacle_density > 3
        params.alpha_dist = 0.6;  % 增加避障权重
        params.alpha_heading = 0.3;
    else
        params.alpha_dist = 0.3;
        params.alpha_heading = 0.7;
    end
end

function count = count_nearby_obstacles(pos, obstacles, radius)
    count = 0;
    for i = 1:size(obstacles, 1)
        if norm(pos - obstacles(i,1:2)) < radius
            count = count + 1;
        end
    end
end

3.2 多目标点支持

function [next_goal, goal_reached] = get_next_goal(robot, goals, current_index)
    current_goal = goals(current_index, :);
    if norm(robot.position - current_goal) < 0.5
        if current_index < size(goals, 1)
            next_goal = goals(current_index+1, :);
            goal_reached = false;
        else
            next_goal = current_goal;
            goal_reached = true;
        end
    else
        next_goal = current_goal;
        goal_reached = false;
    end
end

3.3 动态障碍物处理

function move_obstacles(obstacles)
    % 简单移动障碍物（示例）
    for i = 1:size(obstacles, 1)
        obstacles(i, 1) = obstacles(i, 1) + 0.02 * randn();
        obstacles(i, 2) = obstacles(i, 2) + 0.02 * randn();

        % 确保障碍物在边界内
        obstacles(i, 1) = max(0.5, min(9.5, obstacles(i, 1)));
        obstacles(i, 2) = max(0.5, min(9.5, obstacles(i, 2)));
    end
end

3.4 路径平滑处理

function smooth_path = smooth_path(path, weight_data, weight_smooth, tolerance)
    % 路径平滑算法（梯度下降）
    smooth_path = path;
    change = tolerance;

    while change >= tolerance
        change = 0;
        for i = 2:size(path, 1)-1
            for j = 1:2
                aux = smooth_path(i, j);
                smooth_path(i, j) += weight_data * (path(i, j) - smooth_path(i, j)) + ...
                                    weight_smooth * (smooth_path(i+1, j) + smooth_path(i-1, j) - 2*smooth_path(i, j));
                change += abs(aux - smooth_path(i, j));
            end
        end
    end
end

四、性能评估与比较

4.1 性能指标计算

function evaluate_performance(path, goal, obstacles, params)
    % 路径长度
    path_length = 0;
    for i = 2:size(path, 1)
        path_length = path_length + norm(path(i,:) - path(i-1,:));
    end

    % 规划时间
    planning_time = toc;

    % 平滑度（曲率变化）
    curvature = 0;
    for i = 2:size(path, 1)-1
        dx1 = path(i,1) - path(i-1,1);
        dy1 = path(i,2) - path(i-1,2);
        dx2 = path(i+1,1) - path(i,1);
        dy2 = path(i+1,2) - path(i,2);
        angle_diff = acos((dx1*dx2 + dy1*dy2)/(norm([dx1,dy1])*norm([dx2,dy2])));
        curvature = curvature + angle_diff;
    end
    smoothness = curvature / (size(path,1)-2);

    % 安全性（最小障碍物距离）
    min_dist = inf;
    for i = 1:size(path, 1)
        for j = 1:size(obstacles, 1)
            dist = norm(path(i,:) - obstacles(j,1:2)) - obstacles(j,3) - params.robot_radius;
            if dist < min_dist
                min_dist = dist;
            end
        end
    end

    % 显示结果
    fprintf('===== 性能评估结果 =====\n');
    fprintf('路径长度: %.2f m\n', path_length);
    fprintf('规划时间: %.2f s\n', planning_time);
    fprintf('路径平滑度: %.4f rad/m\n', smoothness);
    fprintf('最小障碍物距离: %.2f m\n', min_dist);
end

4.2 与其他算法比较

function compare_algorithms()
    algorithms = {
   'DWA', 'A*', 'RRT'};
    results = zeros(length(algorithms), 4); % [路径长度, 规划时间, 平滑度, 安全裕度]

    for i = 1:length(algorithms)
        [path, time, smoothness, min_dist] = run_algorithm(algorithms{
   i});
        results(i, :) = [path_length(path), time, smoothness, min_dist];
    end

    % 显示比较结果
    figure;
    subplot(2,2,1);
    bar(results(:,1));
    set(gca, 'XTickLabel', algorithms);
    title('路径长度比较');
    ylabel('长度 (m)');

    subplot(2,2,2);
    bar(results(:,2));
    set(gca, 'XTickLabel', algorithms);
    title('规划时间比较');
    ylabel('时间 (s)');

    subplot(2,2,3);
    bar(results(:,3));
    set(gca, 'XTickLabel', algorithms);
    title('路径平滑度比较');
    ylabel('曲率变化 (rad/m)');

    subplot(2,2,4);
    bar(results(:,4));
    set(gca, 'XTickLabel', algorithms);
    title('安全裕度比较');
    ylabel('最小距离 (m)');
end

参考代码 动态窗口法实现二维路径规划 www.youwenfan.com/contentalh/78891.html

五、实际应用案例

5.1 仓储物流机器人

function warehouse_robot_simulation()
    % 仓库环境设置
    obstacles = [
        % 货架
        2, 2, 0.5; 2, 4, 0.5; 2, 6, 0.5; 2, 8, 0.5;
        4, 2, 0.5; 4, 4, 0.5; 4, 6, 0.5; 4, 8, 0.5;
        6, 2, 0.5; 6, 4, 0.5; 6, 6, 0.5; 6, 8, 0.5;
        8, 2, 0.5; 8, 4, 0.5; 8, 6, 0.5; 8, 8, 0.5;

        % 柱子
        5, 5, 0.3; 7, 3, 0.3
    ];

    % 起点和终点
    start_pos = [0.5, 0.5];
    goal_pos = [9.5, 9.5];

    % 运行DWA算法
    run_dwa_simulation(start_pos, goal_pos, obstacles);
end

5.2 无人机室内导航

function drone_navigation_simulation()
    % 室内环境设置
    obstacles = [
        % 墙壁
        0, 5, 0.2; 10, 5, 0.2; 5, 0, 0.2; 5, 10, 0.2;

        % 家具
        3, 3, 0.4; 7, 3, 0.4; 3, 7, 0.4; 7, 7, 0.4;
        2, 5, 0.3; 8, 5, 0.3; 5, 2, 0.3; 5, 8, 0.3
    ];

    % 起点和终点
    start_pos = [1, 1, 1];  % 3D位置
    goal_pos = [9, 9, 2];

    % 运行3D DWA算法
    run_3d_dwa_simulation(start_pos, goal_pos, obstacles);
end

六、总结与扩展

6.1 DWA算法特点

1. 实时性：计算效率高，适合实时应用

2. 反应式：能快速响应环境变化

3. 完备性：在有限时间内找到可行解

4. 参数敏感：性能依赖参数调优

6.2 扩展方向

1. 3D路径规划：扩展Z轴运动

2. 多机器人协作：处理机器人间避碰

3. 传感器融合：结合激光雷达、视觉等

4. 机器学习优化：使用强化学习优化参数

6.3 参数调优指南

6.4 常见问题解决

1. 局部极小值：添加随机扰动或切换全局规划器

2. 振荡行为：增加角速度约束或调整评价函数

3. 路径不平滑：后处理路径平滑算法

4. 动态障碍物：增加障碍物预测模块