MATLAB上的A*算法代码
时间: 2023-10-07 16:10:13 浏览: 48
以下是MATLAB上的A*算法代码示例:
function path = A_star(start_node, goal_node, node_positions, adjacency_matrix)
% A* algorithm implementation for finding the shortest path between two nodes
% Inputs:
% start_node: starting node index
% goal_node: goal node index
% node_positions: array of node positions (x,y coordinates)
% adjacency_matrix: matrix representing edges between nodes (1 if connected, 0 otherwise)
% Output:
% path: array of node indices representing the shortest path from start_node to goal_node
% initialize variables
g_scores = Inf(size(node_positions,1),1); % g score for each node (initialized to infinity)
f_scores = Inf(size(node_positions,1),1); % f score for each node (initialized to infinity)
came_from = zeros(size(node_positions,1),1); % the node we came from to reach each node (initialized to 0)
% set initial g and f scores for start node
g_scores(start_node) = 0;
f_scores(start_node) = heuristic_cost_estimate(start_node, goal_node, node_positions);
% initialize open and closed sets
open_set = [start_node];
closed_set = [];
while ~isempty(open_set)
% find node in open set with lowest f score
[~, current] = min(f_scores(open_set));
current = open_set(current);
% if we've reached the goal, reconstruct the path and return it
if current == goal_node
path = reconstruct_path(came_from, current);
return
end
% move current node from open set to closed set
open_set(open_set==current) = [];
closed_set = [closed_set current];
% for each neighbor of current node
for neighbor = find(adjacency_matrix(current,:))
if ismember(neighbor, closed_set)
continue % ignore neighbors in closed set
end
tentative_g_score = g_scores(current) + distance(current, neighbor, node_positions);
if ~ismember(neighbor, open_set) % add neighbor to open set if not already in it
open_set = [open_set neighbor];
elseif tentative_g_score >= g_scores(neighbor)
continue % this is not a better path
end
% this path is the best until now. Record it!
came_from(neighbor) = current;
g_scores(neighbor) = tentative_g_score;
f_scores(neighbor) = g_scores(neighbor) + heuristic_cost_estimate(neighbor, goal_node, node_positions);
end
end
% if we get here, there is no path from start_node to goal_node
path = [];
function h = heuristic_cost_estimate(node1, node2, node_positions)
% Euclidean distance between two nodes
h = sqrt(sum((node_positions(node1,:) - node_positions(node2,:)).^2));
function d = distance(node1, node2, node_positions)
% distance between two nodes (assuming they are neighbors)
d = sqrt(sum((node_positions(node1,:) - node_positions(node2,:)).^2));
function path = reconstruct_path(came_from, current)
% reconstruct path from start_node to current node using came_from array
path = current;
while came_from(current) ~= 0
current = came_from(current);
path = [current path];
end
end