Dijkstra机器人移动规划Matlab代码

下面是Dijkstra算法的网格移动机器人路径规划的Matlab实现代码：

function path = dijkstra(start, goal, costmap% start: 起点坐 [x, y]
% goal: 终点坐标 [x, y]
% costmap: 代价地图，二维矩阵，表示每个网格的代价

% 确定起点和终点在代价地图中的索引
start_idx = sub2ind(size(costmap), start(2), start(1));
goal_idx = sub2ind(size(costmap), goal(2), goal(1));

% 初始化距离和路径
distances = Inf(size(costmap));
distances(start_idx) = 0;
prev = nan(size(costmap));

% 初始化堆
heap = [start_idx, 0];

while ~isempty(heap)
    % 取出堆中距离最小的节点
    [current, cost] = heap_pop(heap);
    % 判断是否到达终点
    if current == goal_idx
        break;
    end
    % 遍历当前节点的邻居节点
    neighbors = get_neighbors(current, size(costmap));
    for i = 1:size(neighbors, 1)
        neighbor = neighbors(i);
        % 计算到达邻居节点的代价
        new_cost = cost + costmap(neighbor);
        % 判断是否已经遍历过该节点，或者代价更大
        if new_cost < distances(neighbor)
            % 更新距离和路径
            distances(neighbor) = new_cost;
            prev(neighbor) = current;
            % 将邻居节点加入堆中
            heap_push(heap, neighbor, new_cost);
        end
    end
end

% 生成路径
path = [];
if ~isnan(prev(goal_idx))
    node = goal_idx;
    while ~isnan(node)
        path = [path; node];
        node = prev(node);
    end
    path = flip(path, 1);
end
end

function heap_push(heap, node, cost)
heap(end+1, :) = [node, cost];
idx = size(heap, 1);
while idx > 1 && heap(idx, 2) < heap(floor(idx/2), 2)
    heap([idx, floor(idx/2)], :) = heap([floor(idx/2), idx], :);
    idx = floor(idx/2);
end
end

function [node, cost] = heap_pop(heap)
node = heap(1, 1);
cost = heap(1, 2);
heap(1, :) = heap(end, :);
heap(end, :) = [];
idx = 1;
while idx*2 <= size(heap, 1)
    child = idx*2;
    if child+1 <= size(heap, 1) && heap(child+1, 2) < heap(child, 2)
        child = child + 1;
    end
    if heap(child, 2) < heap(idx, 2)
        heap([idx, child], :) = heap([child, idx], :);
        idx = child;
    else
        break;
    end
end
end

function neighbors = get_neighbors(idx, map_size)
[x, y] = ind2sub(map_size, idx);
neighbors = [x-1, y; x+1, y; x, y-1; x, y+1];
mask = neighbors(:, 1) >= 1 & neighbors(:, 1) <= map_size(1) & ...
    neighbors(:, 2) >= 1 & neighbors(:, 2) <= map_size(2);
neighbors = sub2ind(map_size, neighbors(mask, 1), neighbors(mask, 2));
end

其中，`start`和`goal`分别表示起点和终点的坐标，`costmap`是一个二维矩阵，表示每个网格的代价。在实际应用中，需要根据具体情况修改`costmap`的值。

`dijkstra`函数返回从起点到终点的最短路径，路径为一个二维数组，每一行表示一个节点的索引。`heap_push`和`heap_pop`函数分别实现堆的插入和弹出操作，`get_neighbors`函数返回一个节点的邻居节点。