A*算法matlab实现

A*算法是一种常用的启发式搜索算法，用于在图形或网络中找到最短路径。它使用估价函数来评估每个节点的优先级，并选择具有最低总代价的节点进行扩展。在实现A*算法时，需要定义一个启发式函数来估计从当前节点到目标节点的代价。常见的启发式函数包括曼哈顿距离、欧几里得距离等。

在Matlab中实现A*算法，可以按照以下步骤进行：

1. 定义地图和起点、终点坐标。
2. 定义启发式函数，例如曼哈顿距离。
3. 定义节点类，包括节点坐标、代价、父节点等信息。
4. 初始化起点节点，并将其加入开放列表。
5. 进入循环，直到开放列表为空或者找到终点节点：
a. 从开放列表中选择代价最小的节点进行扩展。
b. 对于每个相邻节点，计算其代价和启发式函数值，并更新其父节点和总代价。
c. 将相邻节点加入开放列表或者更新开放列表中已有的节点。
6. 如果找到终点节点，则回溯路径并输出结果。

下面是一个简单的A*算法Matlab实现示例：

% 定义地图和起点、终点坐标
map = [0 0 0 0 0;
       0 1 1 0 0;
       0 1 0 0 0;
       0 1 1 1 0;
       0 0 0 0 0];
start = [1,1];
goal = [5,5];

% 定义启发式函数
heuristic = @(pos) abs(pos(1)-goal(1)) + abs(pos(2)-goal(2));

% 定义节点类
classdef Node
    properties
        pos
        cost
        parent
    end
    methods
        function obj = Node(pos, cost, parent)
            obj.pos = pos;
            obj.cost = cost;
            obj.parent = parent;
        end
        function priority = getPriority(obj)
            priority = obj.cost + heuristic(obj.pos);
        end
    end
end

% 初始化起点节点，并将其加入开放列表
startNode = Node(start, 0, []);
openList = [startNode];

% 进入循环，直到开放列表为空或者找到终点节点
while ~isempty(openList)
    % 从开放列表中选择代价最小的节点进行扩展
    [~, idx] = min(arrayfun(@(x) x.getPriority(), openList));
    currentNode = openList(idx);
    openList(idx) = [];
    
    % 如果找到终点节点，则回溯路径并输出结果
    if isequal(currentNode.pos, goal)
        path = currentNode.pos;
        while ~isempty(currentNode.parent)
            currentNode = currentNode.parent;
            path = [currentNode.pos; path];
        end
        disp(path);
        break;
    end
    
    % 对于每个相邻节点，计算其代价和启发式函数值，并更新其父节点和总代价
    neighbors = getNeighbors(currentNode.pos, map);
    for i = 1:size(neighbors, 1)
        neighborNode = Node(neighbors(i,:), currentNode.cost+1, currentNode);
        % 将相邻节点加入开放列表或者更新开放列表中已有的节点
        idx = find(arrayfun(@(x) isequal(x.pos, neighborNode.pos), openList));
        if isempty(idx)
            openList = [openList; neighborNode];
        else
            if neighborNode.cost < openList(idx).cost
                openList(idx) = neighborNode;
            end
        end
    end
end

function neighbors = getNeighbors(pos, map)
    [m, n] = size(map);
    neighbors = [];
    if pos(1) > 1 && map(pos(1)-1,pos(2)) == 0
        neighbors = [neighbors; pos-[1,0]];
    end
    if pos(1) < m && map(pos(1)+1,pos(2)) == 0
        neighbors = [neighbors; pos+[1,0]];
    end
    if pos(2) > 1 && map(pos(1),pos(2)-1) == 0
        neighbors = [neighbors; pos-[0,1]];
    end
    if pos(2) < n && map(pos(1),pos(2)+1) == 0
        neighbors = [neighbors; pos+[0,1]];
    end
end