写出A-star算法利用切比雪夫距离求出路径的matlab代码

A-star算法利用切比雪夫距离求出路径的MATLAB代码如下：

function [path, cost] = astar_chebyshev(start_node, goal_node, obstacles)
% start_node: 起点
% goal_node: 终点
% obstacles: 障碍物，格式为 [x1, y1; x2, y2; ...]

% 计算起点到终点的切比雪夫距离
heuristic = @(node) chebyshev(node, goal_node);

% 初始化起点
start_node.g = 0;
start_node.f = heuristic(start_node);

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

% 开始搜索
while ~isempty(open_list)
    % 选取f值最小的节点作为当前节点
    [current_node, idx] = min([open_list.f]);
    current_node = open_list(idx);
    
    % 如果当前节点是目标节点，则返回路径
    if isequal(current_node, goal_node)
        path = trace_path(current_node);
        cost = current_node.g;
        return
    end
    
    % 将当前节点从open表中删除，并加入closed表
    open_list(idx) = [];
    closed_list(end+1) = current_node;
    
    % 遍历当前节点的所有邻居
    for neighbor = get_neighbors(current_node, obstacles)
        % 如果邻居节点已经在closed表中，则跳过此节点
        if ismember(neighbor, closed_list)
            continue
        end
        
        % 计算邻居节点的g值
        tentative_g = current_node.g + euclidean(current_node, neighbor);
        
        % 如果邻居节点不在open表中，则将其加入open表
        if ~ismember(neighbor, open_list)
            neighbor.g = tentative_g;
            neighbor.f = neighbor.g + heuristic(neighbor);
            neighbor.parent = current_node;
            open_list(end+1) = neighbor;
        else
            % 如果邻居节点已经在open表中，且新的g值更优，则更新其g值和父节点
            idx = find(ismember(open_list, neighbor));
            if tentative_g < open_list(idx).g
                open_list(idx).g = tentative_g;
                open_list(idx).f = open_list(idx).g + heuristic(open_list(idx));
                open_list(idx).parent = current_node;
            end
        end
    end
end

% 如果open表为空，说明找不到路径
path = [];
cost = Inf;

end

function h = chebyshev(node1, node2)
% 计算两个节点之间的切比雪夫距离
h = max(abs(node1.x - node2.x), abs(node1.y - node2.y));
end

function h = euclidean(node1, node2)
% 计算两个节点之间的欧几里得距离
h = norm([node1.x node1.y] - [node2.x node2.y]);
end

function path = trace_path(node)
% 从终点开始回溯，追踪路径
path = [node];
while isfield(node, 'parent')
    node = node.parent;
    path = [node path];
end
end

function neighbors = get_neighbors(node, obstacles)
% 获取节点的所有邻居
x = node.x;
y = node.y;
neighbors = [];
for dx = -1:1
    for dy = -1:1
        % 忽略当前节点
        if dx == 0 && dy == 0
            continue
        end
        
        % 计算邻居节点的位置
        nx = x + dx;
        ny = y + dy;
        
        % 如果邻居节点在障碍物内，则跳过此节点
        if ismember([nx ny], obstacles, 'rows')
            continue
        end
        
        % 创建邻居节点
        neighbor.x = nx;
        neighbor.y = ny;
        neighbors(end+1) = neighbor;
    end
end
end

需要注意的是，在上述代码中，我同时实现了切比雪夫距离和欧几里得距离两种估价函数，可以通过修改函数句柄heuristic来选择使用哪种估价函数。另外，为了方便起见，我将节点的坐标和g值存储在结构体中，而不是使用矩阵。