编写一个路径规划A星算法代码matlab

以下是一个简单的 MATLAB 实现 A*（A-star）算法的示例代码：

function [path, closed] = AStar(start, goal, map)
% A*算法的 MATLAB 实现
% 输入参数：
% start：起点的坐标
% goal：终点的坐标
% map：地图，0 表示可以通行，1 表示障碍物
% 返回值：
% path：路径，返回一个 n*2 的矩阵，n 表示路径节点数，第一列是 x 坐标，第二列是 y 坐标
% closed：经过的节点，返回一个 m*2 的矩阵，m 表示经过的节点数，第一列是 x 坐标，第二列是 y 坐标

% 初始化起点和终点节点
start_node.x = start(1);
start_node.y = start(2);
goal_node.x = goal(1);
goal_node.y = goal(2);

% 初始化 open 和 closed 列表
open = [start_node];
closed = [];

% 初始化节点的代价和父节点
for i = 1:size(map,1)
    for j = 1:size(map,2)
        node(i,j).f = Inf;
        node(i,j).g = Inf;
        node(i,j).h = Inf;
        node(i,j).parent = [];
        node(i,j).x = i;
        node(i,j).y = j;
        node(i,j).visited = false;
        node(i,j).obstacle = map(i,j);
    end
end

% 设置起点的代价
node(start_node.x, start_node.y).f = 0;
node(start_node.x, start_node.y).g = 0;
node(start_node.x, start_node.y).h = heuristic(start_node, goal_node);

% 开始搜索
while ~isempty(open)
    % 找到 F 值最小的节点
    [min_f, idx] = min([open.f]);
    current = open(idx);
    
    % 如果当前节点是终点，则搜索完成
    if current.x == goal_node.x && current.y == goal_node.y
        path = getPath(current);
        return;
    end
    
    % 将当前节点从 open 列表中删除，并添加到 closed 列表中
    open(idx) = [];
    closed = [closed; current];
    
    % 扩展当前节点的邻居节点
    neighbors = getNeighbors(current, node);
    for i = 1:length(neighbors)
        neighbor = neighbors(i);
        
        % 如果邻居节点已经在 closed 列表中，则跳过
        if neighbor.visited
            continue;
        end
        
        % 如果邻居节点没有被访问过，则更新其代价和父节点
        if ~neighbor.visited
            neighbor.g = current.g + distance(current, neighbor);
            neighbor.h = heuristic(neighbor, goal_node);
            neighbor.f = neighbor.g + neighbor.h;
            neighbor.parent = current;
            neighbor.visited = true;
            open(end+1) = neighbor;
        end
    end
end

% 如果搜索完成后仍未找到终点，则返回空路径
path = [];
end

% 计算两个节点之间的距离
function dist = distance(node1, node2)
dist = sqrt((node1.x - node2.x)^2 + (node1.y - node2.y)^2);
end

% 计算节点到终点的启发式估价函数
function h = heuristic(node, goal)
h = sqrt((node.x - goal.x)^2 + (node.y - goal.y)^2);
end

% 获取从起点到当前节点的路径
function path = getPath(node)
path = [node.x node.y];
while ~isempty(node.parent)
    node = node.parent;
    path = [node.x node.y; path];
end
end

% 获取当前节点的邻居
function neighbors = getNeighbors(node, graph)
[x, y] = meshgrid(-1:1,-1:1);
x = x + node.x;
y = y + node.y;
valid = x >= 1 & y >= 1 & x <= size(graph,1) & y <= size(graph,2);
x = x(valid);
y = y(valid);
neighbors = [graph(x + (y-1)*size(graph,1))];
neighbors = neighbors(~[neighbors.obstacle] & ~[neighbors.visited]);
end

以上代码中，我们使用了以下几个函数：

- `AStar(start, goal, map)`：A* 算法的主函数，输入起点、终点和地图，返回路径和经过的节点。
- `distance(node1, node2)`：计算两个节点之间的距离。
- `heuristic(node, goal)`：计算节点到终点的启发式估价函数。
- `getPath(node)`：获取从起点到当前节点的路径。
- `getNeighbors(node, graph)`：获取当前节点的邻居。