dijkstra法栅格地图路径规划matlab

Dijkstra算法是一种常用的最短路径算法，可以用于栅格地图路径规划。下面是一个简单的MATLAB代码实现，用于在给定的栅格地图中寻找起点到终点的最短路径。

function [path, cost] = dijkstra(map, start, goal)
% DIJKSTRA Find the shortest path in a grid map using Dijkstra's algorithm
%   [path, cost] = DIJKSTRA(map, start, goal) returns the shortest path
%   between the start and goal points in the given map using Dijkstra's
%   algorithm. The input map is a binary matrix where 0 represents an
%   obstacle and 1 represents an open space. The input start and goal are
%   vectors containing the row and column indices of the start and goal
%   points, respectively. The output path is a matrix where each row
%   represents a point on the path in the form [row, col]. The output cost
%   is the total cost of the path.

% Initialize variables
[nrows, ncols] = size(map);
start_node = sub2ind([nrows ncols], start(1), start(2));
goal_node = sub2ind([nrows ncols], goal(1), goal(2));
cost = inf(nrows, ncols);
parent = zeros(nrows, ncols);
visited = false(nrows, ncols);

% Set the cost of the starting node to 0 and add it to the heap
cost(start_node) = 0;
heap = [start_node];

% Loop until the heap is empty or the goal node is visited
while ~isempty(heap)
    % Get the node with the lowest cost
    curr_node = heap(1);
    heap(1) = [];
    visited(curr_node) = true;
    
    % Check if the goal node is reached
    if curr_node == goal_node
        break
    end
    
    % Get the neighbors of the current node
    [row, col] = ind2sub([nrows ncols], curr_node);
    neighbors = [];
    if row > 1
        neighbors = [neighbors; sub2ind([nrows ncols], row-1, col)];
    end
    if row < nrows
        neighbors = [neighbors; sub2ind([nrows ncols], row+1, col)];
    end
    if col > 1
        neighbors = [neighbors; sub2ind([nrows ncols], row, col-1)];
    end
    if col < ncols
        neighbors = [neighbors; sub2ind([nrows ncols], row, col+1)];
    end
    
    % Loop through the neighbors and update their costs
    for i = 1:length(neighbors)
        neighbor = neighbors(i);
        if visited(neighbor) || map(neighbor) == 0
            continue
        end
        new_cost = cost(curr_node) + 1;
        if new_cost < cost(neighbor)
            cost(neighbor) = new_cost;
            parent(neighbor) = curr_node;
            heap = [heap neighbor];
        end
    end
    
    % Reorder the heap based on the new costs
    [~, idx] = sort(cost(heap));
    heap = heap(idx);
end

% Reconstruct the path
path = [];
curr_node = goal_node;
while curr_node ~= start_node
    path = [path; ind2sub([nrows ncols], curr_node)];
    curr_node = parent(curr_node);
end
path = [path; ind2sub([nrows ncols], start_node)];
path = flipud(path);
cost = cost(goal_node);

end

该代码实现了Dijkstra算法的基本步骤，包括初始化变量、设置起点和终点、计算代价、更新代价等。在函数的最后，使用 parent 矩阵从终点到起点重构最短路径。你可以将地图作为二进制矩阵输入，并提供起点和终点的坐标作为输入来使用此函数。