MATLAB二维航线规划算法：Dijkstra算法实现

以下是使用Dijkstra算法实现二维航线规划的MATLAB代码：

function [path, distance] = dijkstra(start, goal, map)
% start: 起点坐标
% goal: 终点坐标
% map: 地图，0表示可通过，1表示障碍物

% 计算地图大小
[nrow, ncol] = size(map);

% 初始化起点和终点的节点
start_node = sub2ind([nrow, ncol], start(1), start(2));
goal_node = sub2ind([nrow, ncol], goal(1), goal(2));

% 初始化节点距离和前驱节点
node_dist = Inf(nrow*ncol, 1);
node_prev = zeros(nrow*ncol, 1);

% 起点距离为0
node_dist(start_node) = 0;

% 初始化未访问节点集合
unvisited = 1:(nrow*ncol);

while ~isempty(unvisited)
    % 找到未访问节点中距离最小的节点
    [~, idx] = min(node_dist(unvisited));
    curr_node = unvisited(idx);
    
    % 如果当前节点就是终点，结束搜索
    if curr_node == goal_node
        break;
    end
    
    % 从未访问节点集合中移除当前节点
    unvisited(idx) = [];
    
    % 计算当前节点的邻居节点
    neighbors = [];
    if curr_node > 1 && map(curr_node-1) == 0 % 左
        neighbors = [neighbors, curr_node-1];
    end
    if curr_node <= (nrow-1)*ncol && map(curr_node+ncol) == 0 % 上
        neighbors = [neighbors, curr_node+ncol];
    end
    if curr_node < nrow*ncol && map(curr_node+1) == 0 % 右
        neighbors = [neighbors, curr_node+1];
    end
    if curr_node > ncol && map(curr_node-ncol) == 0 % 下
        neighbors = [neighbors, curr_node-ncol];
    end
    
    % 更新邻居节点的距离和前驱节点
    for i = 1:length(neighbors)
        neighbor = neighbors(i);
        alt_dist = node_dist(curr_node) + 1; % 假设每个节点的距离都是1
        if alt_dist < node_dist(neighbor)
            node_dist(neighbor) = alt_dist;
            node_prev(neighbor) = curr_node;
        end
    end
end

% 生成路径
path = [];
dist = node_dist(goal_node);
if isinf(dist)
    % 无法到达终点
    distance = NaN;
else
    % 回溯路径
    node = goal_node;
    while node ~= start_node
        path = [node; path];
        node = node_prev(node);
    end
    path = [start_node; path];
    distance = dist;
end

% 转换路径为坐标
[path_row, path_col] = ind2sub([nrow, ncol], path);
path = [path_row', path_col'];

使用示例：

% 生成地图
map = zeros(10, 10);
map(3:7, 5) = 1;
map(5, 3:7) = 1;

% 调用算法
start = [2, 2];
goal = [9, 9];
[path, distance] = dijkstra(start, goal, map);

% 可视化结果
figure;
imagesc(map);
hold on;
plot(start(2), start(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal(2), goal(1), 'rx', 'MarkerSize', 10, 'LineWidth', 2);
plot(path(:,2), path(:,1), 'g', 'LineWidth', 2);
axis equal;
title(sprintf('Distance: %.2f', distance));