改进a*算法 matlab源码

以下是一个简单的A*算法的Matlab实现，它用于找到从起点到终点的最短路径：

function path = astar(start, goal, map)
% A*算法实现
% 输入：起点start，终点goal，地图map
% 输出：路径path

% 初始化起点和终点节点
start_node = Node(start,0,0);
goal_node = Node(goal,0,0);

% 初始化开启列表和关闭列表
open_list = [start_node];
closed_list = [];

while ~isempty(open_list)
    % 从开启列表中选择最小代价节点
    current_node = minCostNode(open_list);
    
    % 如果当前节点为终点，返回路径
    if current_node == goal_node
        path = getPath(current_node);
        return;
    end
    
    % 将当前节点从开启列表移到关闭列表
    open_list = removeNode(open_list, current_node);
    closed_list = [closed_list, current_node];
    
    % 遍历当前节点的所有邻居
    neighbors = getNeighbors(current_node, map);
    for i = 1:length(neighbors)
        neighbor = neighbors(i);
        
        % 如果邻居节点在关闭列表中，跳过
        if isNodeInList(neighbor, closed_list)
            continue;
        end
        
        % 计算邻居的代价
        g_cost = current_node.g_cost + distance(current_node, neighbor);
        h_cost = distance(neighbor, goal_node);
        f_cost = g_cost + h_cost;
        
        % 如果邻居节点不在开启列表中，将其加入并设置父节点
        if ~isNodeInList(neighbor, open_list)
            neighbor.parent = current_node;
            neighbor.g_cost = g_cost;
            neighbor.f_cost = f_cost;
            open_list = [open_list, neighbor];
        else
            % 如果邻居节点已经在开启列表中，更新其代价和父节点
            for j = 1:length(open_list)
                if open_list(j) == neighbor
                    if g_cost < open_list(j).g_cost
                        open_list(j).parent = current_node;
                        open_list(j).g_cost = g_cost;
                        open_list(j).f_cost = f_cost;
                    end
                    break;
                end
            end
        end
    end
end

% 如果程序执行到这里，说明无法找到路径
error('No path found');

end

function node = Node(position, g_cost, f_cost)
% 定义节点类
node.position = position;   % 节点位置
node.g_cost = g_cost;       % 从起点到该节点的代价
node.f_cost = f_cost;       % 从起点到该节点的估价代价
node.parent = [];           % 父节点

end

function distance = distance(node1, node2)
% 计算两个节点之间的距离
distance = norm(node1.position - node2.position);

end

function neighbors = getNeighbors(node, map)
% 获取节点的邻居节点
neighbors = [];
for i = -1:1
    for j = -1:1
        if i == 0 && j == 0
            continue;
        end
        neighbor_pos = node.position + [i,j];
        if isValidNode(neighbor_pos, map)
            neighbors = [neighbors, Node(neighbor_pos,0,0)];
        end
    end
end

end

function isValid = isValidNode(position, map)
% 判断节点是否合法
if position(1) < 1 || position(1) > size(map,1) || ...
   position(2) < 1 || position(2) > size(map,2) || ...
   map(position(1), position(2)) == 1
    isValid = false;
else
    isValid = true;
end

end

function node = minCostNode(node_list)
% 找到开启列表中代价最小的节点
min_cost = Inf;
min_index = 0;
for i = 1:length(node_list)
    if node_list(i).f_cost < min_cost
        min_cost = node_list(i).f_cost;
        min_index = i;
    end
end
node = node_list(min_index);

end

function node_list = removeNode(node_list, node)
% 从节点列表中移除节点
node_list(node_list==node) = [];

end

function path = getPath(node)
% 从节点到起点追溯路径
path = [];
while ~isempty(node.parent)
    path = [node.position; path];
    node = node.parent;
end
path = [node.position; path];

end

function isInList = isNodeInList(node, node_list)
% 判断节点是否在列表中
isInList = false;
for i = 1:length(node_list)
    if node_list(i) == node
        isInList = true;
        break;
    end
end

end

在上面的代码中，我们定义了一个`Node`类来表示A*算法中的节点。每个节点都有一个位置、从起点到该节点的代价、从起点到该节点的估价代价和一个父节点。我们还定义了一些辅助函数，例如`distance`函数用于计算两个节点之间的距离，`getNeighbors`函数用于获取节点的邻居节点，`isValidNode`函数用于判断节点是否合法，`minCostNode`函数用于找到开启列表中代价最小的节点，`removeNode`函数用于从节点列表中移除节点，`getPath`函数用于从节点到起点追溯路径，`isNodeInList`函数用于判断节点是否在列表中。

使用该算法时，我们需要将起点、终点和地图作为输入参数传递给`astar`函数。地图是一个二维数组，其中0表示可通过的区域，1表示障碍物。`astar`函数将返回从起点到终点的最短路径，如果无法找到路径则会抛出异常。

以下是一个示例代码，演示如何在一个10x10的地图上找到从(1,1)到(10,10)的最短路径：

% 定义地图
map = zeros(10);
map(2:4,5) = 1;
map(6:8,5) = 1;
map(5,2:7) = 1;

% 执行A*算法
start = [1,1];
goal = [10,10];
path = astar(start, goal, map);

% 可视化结果
figure;
imagesc(map);
hold on;
plot(path(:,2), path(:,1), 'r', 'LineWidth', 2);

运行上面的代码，将会看到一个10x10的地图，其中红色线条表示从(1,1)到(10,10)的最短路径。