a*算法路径规划matlab

以下是使用MATLAB实现A*算法路径规划的示例代码：

% 定义地图，并将起点和终点标记出来
map = zeros(10,10);
map(1:3,1) = 1;
map(8:10,10) = 1;
start = [2,1];
goal = [9,10];

% 定义启发函数
heuristic = @(pos) sum(abs(pos - goal));

% 初始化open和closed列表
openList = [start, 0, heuristic(start)];
closedList = [];

% 开始搜索
while ~isempty(openList)
    % 找到f值最小的节点
    [~, idx] = min(openList(:,4));
    current = openList(idx,:);
    openList(idx,:) = [];
    
    % 判断是否到达终点
    if isequal(current(1:2), goal)
        path = [current(1:2)];
        while ~isempty(current(5))
            path = [current(5,1:2); path];
            current = current(5,:);
        end
        break;
    end
    
    % 扩展节点
    neighbors = [];
    for i = -1:1
        for j = -1:1
            if i == 0 && j == 0
                continue;
            end
            neighbor = current(1:2) + [i,j];
            if all(neighbor >= 1) && all(neighbor <= size(map)) && ~map(neighbor(1),neighbor(2))
                neighbors = [neighbors; neighbor];
            end
        end
    end
    
    % 更新邻居节点
    for i = 1:size(neighbors,1)
        neighbor = [neighbors(i,:), current(3)+1, heuristic(neighbors(i,:))];
        for j = 1:size(closedList,1)
            if isequal(neighbor(1:2), closedList(j,1:2)) && neighbor(3) >= closedList(j,3)
                continue;
            end
        end
        for j = 1:size(openList,1)
            if isequal(neighbor(1:2), openList(j,1:2)) && neighbor(3) >= openList(j,3)
                continue;
            end
        end
        neighbor(4) = neighbor(3) + neighbor(4);
        neighbor(5,:) = current;
        openList = [openList; neighbor];
    end
    
    % 将当前节点加入closed列表
    closedList = [closedList; current];
end

% 绘制地图和路径
figure;
imagesc(map);
colormap(gray);
hold on;
plot(start(2),start(1),'ro','MarkerSize',10,'LineWidth',3);
plot(goal(2),goal(1),'go','MarkerSize',10,'LineWidth',3);
for i = 2:size(path,1)
    line([path(i-1,2),path(i,2)],[path(i-1,1),path(i,1)],'Color','b','LineWidth',3);
end