基于DWA算法的机器人避障路径规划 matlab程序 机器人路径优化dwa算法，栅格栏 直接修改数据运行即可

% 初始化参数
robot.vel = [0, 0];       % 当前速度 [v, w]
robot.pose = [0, 0, 0];   % 初始位姿 [x,y,θ]
goal = [10, 10];          % 目标点
obstacles = [3,4; 7,8; 2,9]; % 障碍物坐标
dt = 0.1;                 % 时间步长
max_speed = 1.0;          % 最大线速度
max_rot = pi/2;           % 最大角速度

% 生成栅格地图
map_size = 15;
grid_map = randi([0 1], map_size);  % 随机生成障碍物

这里用随机生成的栅格地图模拟真实环境，重点在于动态窗口法的核心逻辑。算法主要分三步走：速度采样、轨迹预测、评价选择。

速度采样部分特别有意思，不是无脑遍历所有可能速度，而是生成动态窗口：

function samples = velocity_samples(current_vel, max_accel, dt)
    % 生成速度样本
    v_min = max(current_vel(1) - max_accel(1)*dt, 0);
    v_max = min(current_vel(1) + max_accel(1)*dt, max_speed);
    w_min = current_vel(2) - max_accel(2)*dt;
    w_max = current_vel(2) + max_accel(2)*dt;
    
    [V, W] = meshgrid(linspace(v_min, v_max, 10), linspace(w_min, w_max, 10));
    samples = [V(:), W(:)];
end

这段代码用meshgrid生成速度组合，比双重for循环效率更高。注意这里限制了速度变化范围，避免机器人急刹急转。

轨迹预测部分需要处理运动学模型：

function traj = predict_trajectory(vel, init_pose, dt)
    time_steps = 3;  % 预测3秒内的轨迹
    traj = zeros(time_steps/dt, 3);
    current_pose = init_pose;
    
    for i = 1:size(traj,1)
        current_pose = [current_pose(1) + vel(1)*dt*cos(current_pose(3)),...
                        current_pose(2) + vel(1)*dt*sin(current_pose(3)),...
                        current_pose(3) + vel(2)*dt];
        traj(i,:) = current_pose;
    end
end

这里用的是最基础的运动学模型，实际可能需要考虑轮式机器人的运动约束。轨迹预测时长建议设置在2-5秒之间，太短反应不及，太长计算量大。

评价函数是算法的灵魂，直接决定避障效果：

function score = evaluate(traj, goal, obstacles)
    % 三个评价维度
    dist_score = min(sqrt(sum((traj(:,1:2) - obstacles).^2, 2))); % 最近障碍物距离
    goal_score = norm(traj(end,1:2) - goal);  % 终点接近程度
    speed_score = traj(end,1);  % 当前线速度
    
    % 权重调节是关键
    score = 0.5*dist_score + 0.3*(1/goal_score) + 0.2*speed_score; 
end

注意这里三个权重的调节技巧：障碍物距离权重最大，但实际调试中发现当目标点周围布满障碍时，需要适当提高目标权重。建议初始设置按5:3:2分配。

最后是主循环的逻辑：

while norm(robot.pose(1:2) - goal) > 0.5
    % 生成速度样本
    candidates = velocity_samples(robot.vel, [0.5, pi/4], dt);
    
    % 评估所有候选速度
    scores = zeros(size(candidates,1),1);
    for i = 1:size(candidates,1)
        traj = predict_trajectory(candidates(i,:), robot.pose, dt);
        if check_collision(traj, grid_map)
            scores(i) = -inf;  % 碰撞的直接排除
            continue
        end
        scores(i) = evaluate(traj, goal, obstacles);
    end
    
    [~, idx] = max(scores);
    robot.vel = candidates(idx,:);  % 选择最优速度
    
    % 更新位置
    robot.pose = predict_trajectory(robot.vel, robot.pose, dt);
    robot.pose(3) = wrapToPi(robot.pose(3));  % 角度归一化
    
    % 可视化
    plot_robot(robot, traj, grid_map);
    pause(0.1)
end

实际跑起来可能会发现机器人有时会在障碍物附近震荡，这时候需要调整速度采样的分辨率，或者增加航向角对准目标的评分项。有个实用技巧——在评价函数里加入路径平滑度评分，可以有效减少抖动。

最后提醒，栅格地图的处理要注意坐标系转换。我们的代码里直接把地图矩阵当坐标系用，实际部署时需要处理地图分辨率参数。遇到死胡同的情况，建议增加随机扰动策略或者切换全局路径规划算法。