A*融合动态窗口法DWA，机器人路径规划算法，matlab平台。

搞路径规划的朋友们应该都听过A和DWA这对经典组合吧？今天咱们就动手在Matlab里把它们揉在一起玩玩。先剧透效果：全局路线交给A规划，DWA负责实时避障，机器人能像老司机一样在复杂环境里蛇皮走位。

先上A*的骨架代码。核心是节点扩展和代价计算：

function path = AStar(grid, start, goal)
    [rows, cols] = size(grid);
    openSet = PriorityQueue();
    openSet.insert(start, 0);
    cameFrom = containers.Map();
    gScore = inf(rows, cols);
    gScore(start(1), start(2)) = 0;
    
    while ~openSet.isempty()
        current = openSet.extractMin();
        if current == goal
            path = reconstructPath(cameFrom, current);
            return;
        end
        
        neighbors = getNeighbors(current, grid); % 获取可通行邻域
        for i = 1:size(neighbors,1)
            neighbor = neighbors(i,:);
            tentative_gScore = gScore(current(1),current(2)) + 1;
            
            if tentative_gScore < gScore(neighbor(1),neighbor(2))
                cameFrom(num2str(neighbor)) = current;
                gScore(neighbor(1),neighbor(2)) = tentative_gScore;
                fScore = tentative_gScore + heuristic(neighbor, goal);
                if ~openSet.contains(neighbor)
                    openSet.insert(neighbor, fScore);
                end
            end
        end
    end
    error('No path found');
end

这里用了优先队列加速搜索，启发函数heuristic咱们用曼哈顿距离就行。注意getNeighbors函数要过滤掉障碍物和边界，否则机器人直接穿墙了。

接下来是DWA的核心——速度空间生成。重点看动态窗口的计算：

function [v, w] = DWA(x, goal, obstacles)
    % 速度采样范围
    v_res = 0.05;
    w_res = 0.1;
    v_samples = x(4)-acc_v*dt : v_res : x(4)+acc_v*dt;
    w_samples = x(5)-acc_w*dt : w_res : x(5)+acc_w*dt;
    
    best_score = -inf;
    for v = v_samples
        for w = w_samples
            % 轨迹预测
            traj = predictTrajectory(x, v, w); 
            
            % 三项评价指标
            heading = calcHeading(traj(end,:), goal);
            dist = calcObstacleDist(traj, obstacles);
            velocity = v;
            
            % 加权得分
            score = alpha*heading + beta*dist + gamma*velocity;
            if score > best_score
                best_score = score;
                best_vw = [v, w];
            end
        end
    end
    v = best_vw(1);
    w = best_vw(2);
end

这里accv/accw是加速度限制，dt是控制周期。predictTrajectory要模拟未来0.5秒左右的轨迹，别搞太长否则计算量爆炸。评价函数里的alpha、beta、gamma这三个权重参数需要实测调教，就像调咖啡比例一样微妙。

融合的关键在于把A的全局路径喂给DWA。咱们在DWA的heading计算里，不是直接朝向终点，而是朝着A路径上的下一个航点：

function heading = calcHeading(pose, waypoints)
    % 寻找最近航点
    [~, idx] = min(vecnorm(waypoints - pose(1:2), 2, 2));
    target = waypoints(min(idx+1, size(waypoints,1)), :);
    
    % 计算朝向偏差
    theta = atan2(target(2)-pose(2), target(1)-pose(1)) - pose(3);
    heading = (pi - abs(theta)) / pi; % 归一化得分
end

这种航点跟随策略能让机器人沿着全局路径走，遇到障碍时DWA会自己绕开，绕过后又能找回原路线。就像GPS导航的"您已偏航，正在重新规划"——不过咱们这是自动纠偏不用重新算全局路径。

实测效果如何？在20x20的栅格地图里，设置8字形障碍物，融合算法生成的路径比纯A更丝滑，转弯半径也更符合运动学约束。纯DWA有时候会在复杂地形里迷路绕圈，有了A指路后基本不会出现这种情况。

最后给个主循环的骨架：

global_path = AStar(map, start, goal);
current_pose = start;
while norm(current_pose(1:2)-goal(1:2)) > 0.5
    local_obstacles = getLidarData(current_pose); % 模拟激光雷达
    [v, w] = DWA(current_pose, global_path, local_obstacles);
    current_pose = updatePose(current_pose, v, w, dt); % 运动学模型
    plotRobot(current_pose); % 实时可视化
end

注意getLidarData要模拟传感器范围，别让机器人有上帝视角。updatePose建议用差分驱动模型，这样更贴近真实机器人运动。

这种融合方案在Matlab里跑实时性可能差点意思，但作为算法验证绝对够用。真要上真机还得用C++重写，不过那就是另一个故事了。代码里那些魔数参数建议做成可配置项，不同场景下调参能救大命——别问我怎么知道的，说多了都是泪。