基于A*算法的栅格地图路径规划（MATLAB源码）

基于A*算法的栅格地图路径规划（MATLAB源码）

% 栅格地图路径规划
% 使用A*算法寻找栅格地图中的最优路径
% 输入: 栅格地图(grid)、起始点(start)、目标点(goal)
% 输出: 最优路径(path)

function path = AStar(grid, start, goal)
    % 检查输入参数是否有效
    if ~isValidGrid(grid) || ~isValidPoint(start, grid) || ~isValidPoint(goal, grid)
        error('无效的输入参数');
    end
    
    % 初始化起始节点
    startNode = Node(start, 0, heuristic(start, goal), []);
    
    % 初始化open列表和closed列表
    openList = startNode;
    closedList = [];
    
    % 开始A*算法的主循环
    while ~isempty(openList)
        % 选择open列表中的最低估价的节点作为当前节点
        currentNode = getLowestCostNode(openList);
        
        % 如果当前节点为目标节点，则已找到最优路径
        if isGoal(currentNode, goal)
            path = getPath(currentNode);
            return;
        end
        
        % 将当前节点从open列表中移除，并添加到closed列表
        openList = removeNode(openList, currentNode);
        closedList = [closedList, currentNode];
        
        % 获取当前节点的相邻节点