dijkstra算法栅格图路径规划matlab代码

下面是Dijkstra算法在栅格图路径规划的MATLAB代码实现：

function path = Dijkstra(grid, start, goal)
% 栅格图路径规划的Dijkstra算法实现
% 输入参数：
% grid：栅格地图
% start：起点坐标
% goal：终点坐标
% 返回值：
% path：从起点到终点的路径

% 初始化
[row, col] = size(grid);            % 获取栅格地图的行列数
start_index = sub2ind([row, col], start(1), start(2));   % 将起点坐标转换为线性索引
goal_index = sub2ind([row, col], goal(1), goal(2));     % 将终点坐标转换为线性索引
dist = inf(row * col, 1);           % 初始化起点到每个点的距离为无穷大
prev = zeros(row * col, 1);         % 初始化每个点的前驱为0
visited = false(row * col, 1);      % 初始化每个点为未访问
dist(start_index) = 0;              % 起点到起点的距离为0

% 计算每个点的邻居
neighbours = [-1, 0; 1, 0; 0, -1; 0, 1; -1, -1; -1, 1; 1, -1; 1, 1]; % 8个邻居的相对坐标
neighbours_cost = [1; 1; 1; 1; sqrt(2); sqrt(2); sqrt(2); sqrt(2)]; % 8个邻居的代价
neighbours_index = repmat((1:(row * col))', 1, 8) + repmat(neighbours * [col; 1], row * col, 1); % 计算8个邻居的线性索引
neighbours_index = neighbours_index(all(neighbours_index > 0 & neighbours_index <= row * col, 2), :); % 过滤越界的邻居

% Dijkstra算法主循环
while true
    % 选择未访问中距离最小的点
    [~, current] = min(dist(~visited));
    if isempty(current) || current == goal_index
        break;
    end
    
    % 标记选中点为已访问
    visited(current) = true;
    
    % 更新所有邻居的距离
    for i = 1:size(neighbours_index, 2)
        neighbour = neighbours_index(current, i);
        if ~visited(neighbour) && grid(neighbour) ~= -1
            alt = dist(current) + neighbours_cost(i) * grid(neighbour);
            if alt < dist(neighbour)
                dist(neighbour) = alt;
                prev(neighbour) = current;
            end
        end
    end
end

% 没有找到终点，返回空路径
if prev(goal_index) == 0
    path = [];
    return;
end

% 从终点反向遍历路径
path = goal_index;
while path(1) ~= start_index
    path = [prev(path(1)); path];
end

% 将线性索引转换为坐标
[path_row, path_col] = ind2sub([row, col], path);
path = [path_row, path_col];

该函数的输入参数包括栅格地图、起点坐标和终点坐标，返回值为从起点到终点的路径。栅格地图中的-1表示障碍物，0表示自由空间，1表示半自由空间。函数的实现过程与Dijkstra算法的一般实现类似，不同之处在于计算每个点的邻居时需要考虑8个方向。在计算邻居的代价时，对角线方向的代价为sqrt(2)，其他方向的代价为1。