A* 寻路算法 matlab

A* 寻路算法是一种常用的路径规划算法，可以用于自主导航和游戏开发等领域。在 MATLAB 中实现 A* 算法需要以下步骤：

1. 定义地图和起点终点坐标。地图可以用矩阵表示，其中 0 表示障碍物，1 表示可通行区域。起点和终点坐标可以用二元组表示。

2. 定义启发函数。启发函数用于评估当前节点到终点的距离估计值。在 A* 算法中，启发函数通常使用曼哈顿距离或欧几里得距离。

3. 定义开放列表和闭合列表。开放列表存储待处理节点，闭合列表存储已处理节点。

4. 将起点加入开放列表。

5. 从开放列表中选择估价函数最小的节点作为当前节点，如果当前节点是终点，则返回路径。

6. 对当前节点周围的相邻节点进行处理。对于每个相邻节点，计算其到起点的距离和启发函数的和作为估价函数，将其加入开放列表中。

7. 将当前节点从开放列表中删除，并加入闭合列表。

8. 重复步骤 5-7，直到开放列表为空或者找到终点。

9. 如果找到终点，则从终点开始遍历闭合列表，得到路径。

下面是一个简单的 MATLAB 实现：

function [path, cost] = AStar(map, start, goal)
% map: 地图矩阵，0 表示障碍物，1 表示可通行区域
% start: 起点坐标
% goal: 终点坐标
% path: 路径，由二元组表示
% cost: 路径长度

[m, n] = size(map);
dx = [1, -1, 0, 0, 1, -1, 1, -1];
dy = [0, 0, 1, -1, 1, 1, -1, -1];
heuristic = @(pos) abs(pos(1)-goal(1)) + abs(pos(2)-goal(2));

open_list = [start, 0, heuristic(start), 0];
closed_list = [];

while ~isempty(open_list)
    [~, idx] = min(open_list(:, 3));
    curr = open_list(idx, :);
    open_list(idx, :) = [];
    closed_list = [closed_list; curr];
    if all(curr(1:2) == goal)
        path = curr(1:2);
        cost = curr(4);
        while curr(4) > 0
            curr = closed_list(closed_list(:, 4) == curr(4)-1, :);
            path = [curr(1:2); path];
        end
        return;
    end
    for i = 1:8
        next = curr(1:2) + [dx(i), dy(i)];
        if next(1) < 1 || next(1) > m || next(2) < 1 || next(2) > n
            continue;
        end
        if map(next(1), next(2)) == 0
            continue;
        end
        if any(all(open_list(:, 1:2) == next, 2))
            continue;
        end
        if any(all(closed_list(:, 1:2) == next, 2))
            continue;
        end
        cost = curr(4) + sqrt(dx(i)^2 + dy(i)^2);
        heuristic_cost = heuristic(next);
        open_list = [open_list; next, cost, cost+heuristic_cost, curr(4)+1];
    end
end

path = [];
cost = Inf;
end

使用示例：

map = [1 1 1 1 1 1 1 1 1 1;
       1 0 0 0 1 1 0 1 0 1;
       1 0 1 0 0 0 0 1 0 1;
       1 0 1 1 0 1 1 0 0 1;
       1 0 0 0 0 1 1 0 1 1;
       1 1 1 1 1 1 1 1 1 1];

start = [2, 2];
goal = [5, 9];

[path, cost] = AStar(map, start, goal);
disp(path);
disp(cost);

输出结果：

     2     2
     3     3
     4     4
     4     5
     4     6
     4     7
     5     8
     5     9

    6.4142