A*算法程序代码

function[distance, path] = a_star(map_size, neighbors, start, goal)

%A* 算法, 找不到路径时返回 error

%map_size      输入 地图尺寸 == size(mymap)

%neighbors     输入 邻居 (cell)

%start         输入 起点

%goal          输入 目标点

%distance      输出 距离

%path          输出 路径

nodes_num =map_size(1) * map_size(2);

open_set =start;

close_set =[];

came_from =inf(nodes_num, 1);

gscore =inf(nodes_num, 1);

fscore =inf(nodes_num, 1);

gscore(start)= 0;

fscore(start)= gscore(start) + h_manhattan(map_size, start, goal);

while~isempty(open_set)

    % 取出 open_set 中 f 最小的点 current

    min_f = inf;

    current = 0;

    for i = 1:length(open_set)

        node = open_set(i);

        if fscore(node) < min_f

            min_f = fscore(node);

            current = node;

        end

    end

    if current == goal

        distance  = gscore(current);

        path = reconstruct_path(came_from,current);

        return;

    end

    open_set = setdiff(open_set, current);

    close_set = [close_set current];

    for i = 1:length(neighbors{current})

        neighbor = neighbors{current}(i);

        if any(ismember(close_set, neighbor))

            continue;

        end

        tentative_gscore = gscore(current) +get_distance(map_size, current, neighbor);

        if ~any(ismember(open_set, neighbor))

            open_set = [open_set neighbor];

        elseif tentative_gscore >=gscore(neighbor)

            continue;

        end

        came_from(neighbor) = current;

        gscore(neighbor) = tentative_gscore;

        fscore(neighbor) = gscore(neighbor) +h_manhattan(map_size, neighbor, goal);

    end

end

error("Failure!");

end

%返回两点曼哈顿距离

functionhscore = h_manhattan(map_size, current, goal)

[current_row,current_col] = ind2sub(map_size, current);

[goal_row,goal_col] = ind2sub(map_size, goal);

hscore =abs(current_row - goal_row) + abs(current_col - goal_col);

end

%构造路径

function path= reconstruct_path(came_from, current)

path =[current];

whilecame_from(current) <= length(came_from)

    path = [came_from(current) path];

    current = came_from(current);

end

end

=========

从上面可以看出，neighbors,start, goal三个参数必不可少，而map_size参数可以用nodes_num替换，还需要用到三个函数，一个是计算h值的函数，一个是计算某点与邻居距离的函数，还有一个是构造路径的函数。

找栅格地图中两点间最短距离

如下图所示栅格地图，指定起始点和目标点，智能体（或机器人）只能在“上、下、左、右”四个方向移动，找出最短路径：

结果如下：

=========