Matlab实现A*算法：路径规划核心技术与实战

1. 从零实现A*算法：Matlab路径规划实战指南

路径规划是机器人导航、游戏AI和物流优化等领域的核心技术。在众多算法中，A*（A-Star）因其高效性和最优性成为最受欢迎的路径搜索算法之一。本文将带你从零开始，用Matlab完整实现A*算法，并探讨其在动态环境中的扩展应用。

提示：本文所有代码均基于Matlab R2021b开发，兼容大多数现代版本。建议读者边阅读边动手实践。

1.1 A*算法核心原理剖析

A*算法本质上是一种启发式搜索算法，它通过评估函数f(n)=g(n)+h(n)来指导搜索方向。其中：

• g(n)：从起点到节点n的实际路径代价
• h(n)：从节点n到终点的启发式估计代价（必须满足可采纳性，即不高估实际代价）

与Dijkstra算法相比，A通过引入启发式函数大幅减少了搜索范围；与贪心算法相比，A保证了找到的路径是最优的。这种平衡使其在实际应用中表现出色。

1.1.1 启发函数的选择艺术

启发函数h(n)的设计直接影响算法性能。常用的启发函数包括：

1. 曼哈顿距离：适用于只能四方向移动的网格

   matlab复制function h = manhattan(node, goal)
       h = abs(node(1)-goal(1)) + abs(node(2)-goal(2));
   end
   

2. 欧几里得距离：适用于可任意方向移动的场景

   matlab复制function h = euclidean(node, goal)
       h = norm(node - goal);
   end
   

3. 对角线距离（切比雪夫距离）：适用于八方向移动

   matlab复制function h = diagonal(node, goal)
       dx = abs(node(1)-goal(1));
       dy = abs(node(2)-goal(2));
       h = (dx + dy) + (sqrt(2)-2)*min(dx,dy);
   end
   

重要原则：启发函数必须满足h(n) ≤ h*(n)，其中h*(n)是真实最小代价，否则可能找不到最优解。

1.2 Matlab实现详解

1.2.1 环境建模与初始化

我们使用矩阵表示地图，其中0表示可行区域，1表示障碍物。为提高代码可读性，建议使用结构体封装算法参数：

matlab复制function problem = initProblem()
    problem.map = [0 0 0 0 0;
                  0 1 0 1 0;
                  0 0 0 0 0;
                  0 1 1 0 0;
                  0 0 0 1 0]; % 5x5示例地图
    problem.start = [1, 1];   % 起点坐标(行,列)
    problem.goal = [5, 5];    % 终点坐标
    problem.movement = '8way'; % 移动方式(4way/8way)
    problem.heuristic = @manhattan; % 启发函数
end

1.2.2 核心算法实现

完整A*实现需要维护三个关键数据结构：

• openSet：待探索节点（优先队列）
• closedSet：已探索节点
• cameFrom：记录路径的父节点关系

matlab复制function [path, cost] = aStar(problem)
    % 初始化
    [rows, cols] = size(problem.map);
    openSet = PriorityQueue();
    openSet.insert(problem.start, 0);
    
    cameFrom = containers.Map();
    gScore = inf(rows, cols);
    gScore(problem.start(1), problem.start(2)) = 0;
    
    fScore = inf(rows, cols);
    fScore(problem.start(1), problem.start(2)) = feval(problem.heuristic, ...
                                                     problem.start, problem.goal);
    
    % 主循环
    while ~openSet.isEmpty()
        current = openSet.pop();
        
        % 到达目标
        if isequal(current, problem.goal)
            path = reconstructPath(cameFrom, current);
            cost = gScore(current(1), current(2));
            return;
        end
        
        % 生成邻居节点
        neighbors = getNeighbors(current, problem);
        
        for i = 1:size(neighbors, 1)
            neighbor = neighbors(i,:);
            
            % 跳过障碍物和已探索节点
            if problem.map(neighbor(1), neighbor(2)) == 1