Matlab实现A*算法代码详解：高效寻路的实战代码

```matlab function [path, cost] = AStar( start, goal, graph ) % 定义启发函数（曼哈顿距离） heuristic = @(x,y) abs(x - y); openList = {}; closedList = {}; % 初始化起始节点 startNode = struct('state', start,... 'costFromStart', 0,... 'estimatedCostToGoal', heuristic(start, goal)); % 将起始节点加入开放列表 openList{end + 1} = startNode; while ~isempty(openList) % 找到开放列表中代价最小的节点 [minCost, minIndex] = min([cellfun(@(x) x.costFromStart + x.estimatedCostToGoal, openList)]); currentNode = openList{minIndex}; % 将当前节点从开放列表移除并加入关闭列表 openList(minIndex) = []; closedList{end + 1} = currentNode; % 如果到达目标节点，返回路径和代价 if currentNode.state == goal path = reconstructPath(closedList, currentNode); cost = currentNode.costFromStart; return; end % 扩展当前节点的邻居节点 neighbors = findNeighbors(currentNode.state, graph); for i = 1:length(neighbors) neighbor = neighbors(i); % 计算到邻居节点的新代价 newCost = currentNode.costFromStart + graph(currentNode.state, neighbor); % 检查邻居节点是否在关闭列表中 foundInClosed = false; for j = 1:length(closedList) if closedList{j}.state == neighbor foundInClosed = true; break; end end if foundInClosed continue; end % 检查邻居节点是否在开放列表中 foundInOpen = false; newNode = struct('state', neighbor,... 'costFromStart', newCost,... 'estimatedCostToGoal', newCost + heuristic(neighbor, goal)); for j = 1:length(openList) if openList{j}.state == neighbor foundInOpen = true; if newCost < openList{j}.costFromStart openList{j} = newNode; end break; end end if ~foundInOpen openList{end + 1} = newNode; end end end % 如果没有找到路径，返回空路径和无穷大代价 path = []; cost = Inf; end function neighbors = findNeighbors(state, graph) neighbors = []; [row, col] = ind2sub(size(graph), state); possibleMoves = [-1 0; 1 0; 0 -1; 0 1]; for i = 1:size(possibleMoves, 1) newRow = row + possibleMoves(i, 1); newCol = col + possibleMoves(i, 2); if newRow >= 1 && newRow <= size(graph, 1) && newCol >= 1 && newCol <= size(graph, 2) neighbor = sub2ind(size(graph), newRow, newCol); neighbors = [neighbors neighbor]; end end end function path = reconstructPath(closedList, goalNode) path = [goalNode.state]; currentNode = goalNode; while isfield(currentNode, 'parent') currentNode = currentNode.parent; path = [currentNode.state path]; end end ``` ######[AI问答 | 853点数解答 | 2025-05-19 10:50:12]