机器人路径规划算法之Dijkstra算法详解＋MATLAB代码实现

松小白song2026-03-13 14:54

目录

一、算法概述

二、算法步骤详解

[1. 数据结构初始化](#1. 数据结构初始化)

[2. 主循环过程](#2. 主循环过程)

三、具体示例

四、算法实现（MATLAB）

一、算法概述

Dijkstra算法是荷兰计算机科学家艾兹赫尔·戴克斯特拉 于1956年提出的，用于解决带非负权值的有向或无向图中的单源最短路径问题 。它的核心思想是贪心策略：每次从未确定最短路径的节点中，选择距离起点最近的节点，然后更新其相邻节点的距离。

二、算法步骤详解

1. 数据结构初始化

  • 创建三个数据结构：

    • dist[]：记录从起点到每个节点的当前最短距离，起点设为0，其他设为无穷大（∞）

    • visited[]：标记节点是否已确定最短路径

    • prev[]：记录最短路径上的前驱节点，用于重建路径

2. 主循环过程

重复以下步骤直到所有节点都被访问：

  1. 从未访问节点 中选择dist值最小 的节点u

  2. 将u标记为已访问

  3. 对u的每个邻居节点v（且v未访问）：

    • 计算新的距离：new_dist = dist[u] + weight(u, v)

    • 如果new_dist < dist[v]：

      • 更新dist[v] = new_dist

      • 更新prev[v] = u

三、具体示例

考虑以下带权图（起点为A）：

复制代码 
A
   / \
  4   2
 /     \
B---1---C
 \     /
  5   8
   \ /
    D

执行过程：

步骤 当前节点 已访问节点 距离数组dist (A,B,C,D) 说明
初始化 - {} [0, ∞, ∞, ∞] 起点A距离为0
1 A {A} [0, 4, 2, ∞] 更新B(4), C(2)
2 C {A,C} [0, 3, 2, 10] 通过C到B更短: 2+1=3
3 B {A,B,C} [0, 3, 2, 8] 通过B到D: 3+5=8
4 D {A,B,C,D} [0, 3, 2, 8] 完成

最终最短路径：

  • A→C: 2

  • A→B: 3 (路径: A→C→B)

  • A→D: 8 (路径: A→C→B→D)

四、算法实现（MATLAB）

复制代码 
%% ==================== 主程序：Dijkstra算法演示 ====================
clear; clc; close all;

fprintf('========== Dijkstra算法路径规划演示 ==========\n');

%% 1. 创建网格地图
fprintf('1. 创建20x20网格地图...\n');
map_size = 20;
grid_map = zeros(map_size, map_size);

% 添加障碍物（墙）
grid_map(8, 3:18) = 1;      % 水平墙
grid_map(5:15, 10) = 1;     % 垂直墙

% 添加随机障碍物
rng(42);  % 固定随机种子
num_obstacles = 30;
for i = 1:num_obstacles
    r = randi([1, map_size]);
    c = randi([1, map_size]);
    grid_map(r, c) = 1;
end

% 设置起点和终点
start_pos = [2, 2];
goal_pos = [18, 18];
grid_map(start_pos(1), start_pos(2)) = 0;  % 确保起点空闲
grid_map(goal_pos(1), goal_pos(2)) = 0;    % 确保终点空闲

fprintf('   起点: (%d, %d)\n', start_pos(1), start_pos(2));
fprintf('   终点: (%d, %d)\n', goal_pos(1), goal_pos(2));

%% 2. 运行Dijkstra算法
fprintf('\n2. 运行Dijkstra算法...\n');
tic;
[path, distance_map] = dijkstra_grid_planner(grid_map, start_pos, goal_pos);
plan_time = toc;  % 记录规划时间

fprintf('   规划完成！耗时: %.4f秒\n', plan_time);

%% 3. 可视化结果
fprintf('\n3. 可视化结果...\n');
% 将plan_time作为额外参数传递给可视化函数
visualize_results(grid_map, start_pos, goal_pos, path, distance_map, plan_time);

%% ==================== 核心算法函数 ====================
function [path, distance_map] = dijkstra_grid_planner(grid_map, start_pos, goal_pos)
    % Dijkstra网格路径规划器
    % 输入：grid_map - 网格地图（0=空闲，1=障碍物）
    %       start_pos - 起点坐标 [行, 列]
    %       goal_pos  - 终点坐标 [行, 列]
    % 输出：path - 路径坐标序列
    %       distance_map - 距离地图
    
    [rows, cols] = size(grid_map);
    
    % 将坐标转换为节点编号
    start_node = pos2node(start_pos, cols);
    goal_node = pos2node(goal_pos, cols);
    
    % 初始化数据结构
    distances = inf(rows*cols, 1);      % 距离数组
    visited = false(rows*cols, 1);      % 访问标记
    prev = zeros(rows*cols, 1);         % 前驱节点
    distances(start_node) = 0;          % 起点距离为0
    
    % 定义4个移动方向（上、下、左、右）
    dirs = [-1, 0; 1, 0; 0, -1; 0, 1];
    
    % 主循环
    for i = 1:rows*cols
        % 找到未访问节点中距离最小的节点
        min_dist = inf;
        current_node = -1;
        
        for node = 1:length(distances)
            if ~visited(node) && distances(node) < min_dist
                min_dist = distances(node);
                current_node = node;
            end
        end
        
        % 如果没有可达节点或到达目标，结束
        if current_node == -1 || current_node == goal_node
            break;
        end
        
        visited(current_node) = true;
        
        % 获取当前节点的坐标
        [r, c] = node2pos(current_node, cols);
        
        % 检查4个邻居方向
        for d = 1:4
            nr = r + dirs(d, 1);
            nc = c + dirs(d, 2);
            
            % 检查边界和障碍物
            if nr >= 1 && nr <= rows && nc >= 1 && nc <= cols
                if grid_map(nr, nc) == 0
                    neighbor_node = pos2node([nr, nc], cols);
                    
                    if ~visited(neighbor_node)
                        new_dist = distances(current_node) + 1;  % 每步代价为1
                        
                        if new_dist < distances(neighbor_node)
                            distances(neighbor_node) = new_dist;
                            prev(neighbor_node) = current_node;
                        end
                    end
                end
            end
        end
    end
    
    % 重建路径
    if distances(goal_node) < inf
        path_nodes = reconstruct_path(prev, start_node, goal_node);
        
        % 将节点路径转换为坐标
        path = zeros(length(path_nodes), 2);
        for i = 1:length(path_nodes)
            [r, c] = node2pos(path_nodes(i), cols);
            path(i, :) = [r, c];
        end
    else
        path = [];
        fprintf('   警告：找不到可行路径！\n');
    end
    
    % 生成距离地图（将Inf替换为NaN以便可视化）
    distance_map = reshape(distances, [rows, cols]);
    distance_map(distance_map == inf) = NaN;
end

%% ==================== 工具函数 ====================
function node = pos2node(pos, cols)
    % 坐标转换为节点编号
    node = (pos(1)-1) * cols + pos(2);
end

function [r, c] = node2pos(node, cols)
    % 节点编号转换为坐标
    r = floor((node-1) / cols) + 1;
    c = mod(node-1, cols) + 1;
end

function path_nodes = reconstruct_path(prev, start_node, goal_node)
    % 重建路径节点序列
    path_nodes = [];
    current = goal_node;
    
    while current ~= 0
        path_nodes = [current, path_nodes];
        current = prev(current);
    end
    
    % 验证路径有效性
    if isempty(path_nodes) || path_nodes(1) ~= start_node
        path_nodes = [];
    end
end

%% ==================== 可视化函数 ====================
function visualize_results(grid_map, start_pos, goal_pos, path, distance_map, plan_time)
    % 创建可视化窗口
    figure('Position', [100, 100, 1200, 400]);
    
    % 子图1：原始地图和路径
    subplot(1, 3, 1);
    imagesc(grid_map);
    colormap(gca, [1 1 1; 0.4 0.4 0.4]);  % 白色=空闲，深灰色=障碍物
    hold on;
    
    % 标记起点和终点
    plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
    plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
    
    % 绘制路径
    if ~isempty(path)
        plot(path(:,2), path(:,1), 'b-', 'LineWidth', 2);
        plot(path(:,2), path(:,1), 'bo', 'MarkerSize', 6, 'MarkerFaceColor', 'b');
        fprintf('   路径步数: %d\n', size(path, 1));
    end
    
    axis equal tight;
    grid on;
    set(gca, 'YDir', 'reverse');
    xlabel('X (列)', 'FontSize', 10);
    ylabel('Y (行)', 'FontSize', 10);
    title('Dijkstra路径规划结果', 'FontSize', 12, 'FontWeight', 'bold');
    legend({'起点', '终点', '路径'}, 'Location', 'best');
    
    % 子图2：距离地图
    subplot(1, 3, 2);
    
    % 处理NaN值用于可视化
    dist_plot = distance_map;
    dist_plot(isnan(dist_plot)) = max(dist_plot(~isnan(dist_plot))) + 1;
    
    imagesc(dist_plot);
    colormap(gca, 'hot');
    colorbar;
    hold on;
    
    % 标记起点和终点
    plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
    plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
    
    if ~isempty(path)
        plot(path(:,2), path(:,1), 'c-', 'LineWidth', 2);
    end
    
    axis equal tight;
    set(gca, 'YDir', 'reverse');
    xlabel('X (列)', 'FontSize', 10);
    ylabel('Y (行)', 'FontSize', 10);
    title('距离地图（起点到各点的最短距离）', 'FontSize', 12, 'FontWeight', 'bold');
    
    % 子图3：简化示例演示Dijkstra算法原理
    subplot(1, 3, 3);
    
    % 创建一个简单的5x5示例
    demo_map = zeros(5, 5);
    demo_map(3, 2:4) = 1;  % 一行障碍物
    
    demo_start = [1, 1];
    demo_goal = [5, 5];
    
    % 运行简化版的Dijkstra
    [demo_path, demo_dist] = simple_dijkstra_demo(demo_map, demo_start, demo_goal);
    
    % 绘制距离场和路径
    imagesc(demo_dist);
    colormap(gca, 'cool');
    colorbar;
    hold on;
    
    % 绘制障碍物
    [obs_r, obs_c] = find(demo_map == 1);
    plot(obs_c, obs_r, 'ks', 'MarkerSize', 40, 'MarkerFaceColor', 'k', 'LineWidth', 2);
    
    % 绘制路径
    if ~isempty(demo_path)
        plot(demo_path(:,2), demo_path(:,1), 'y-', 'LineWidth', 3);
    end
    
    % 绘制起点和终点
    plot(demo_start(2), demo_start(1), 'gs', 'MarkerSize', 25, 'MarkerFaceColor', 'g', 'LineWidth', 2);
    plot(demo_goal(2), demo_goal(1), 'rs', 'MarkerSize', 25, 'MarkerFaceColor', 'r', 'LineWidth', 2);
    
    % 标注距离值
    [rows, cols] = size(demo_map);
    for r = 1:rows
        for c = 1:cols
            if demo_map(r, c) == 0
                dist_val = demo_dist(r, c);
                if ~isnan(dist_val)
                    text(c, r, sprintf('%.0f', dist_val), ...
                         'HorizontalAlignment', 'center', ...
                         'VerticalAlignment', 'middle', ...
                         'FontSize', 10, 'FontWeight', 'bold', ...
                         'Color', 'white');
                else
                    text(c, r, '∞', ...
                         'HorizontalAlignment', 'center', ...
                         'VerticalAlignment', 'middle', ...
                         'FontSize', 10, 'FontWeight', 'bold', ...
                         'Color', 'white');
                end
            end
        end
    end
    
    axis equal tight;
    set(gca, 'YDir', 'reverse');
    xlabel('X', 'FontSize', 10);
    ylabel('Y', 'FontSize', 10);
    title('Dijkstra算法演示', 'FontSize', 12, 'FontWeight', 'bold');
    
    fprintf('   可视化完成！\n');
    fprintf('\n========== 程序执行完成 ==========\n\n');
    
    % 显示结果总结
    fprintf('结果总结：\n');
    fprintf('   地图大小: %dx%d\n', size(grid_map, 1), size(grid_map, 2));
    fprintf('   障碍物比例: %.1f%%\n', 100*sum(grid_map(:))/numel(grid_map));
    fprintf('   规划时间: %.4f秒\n', plan_time);
    
    if ~isempty(path)
        manhattan_dist = abs(start_pos(1)-goal_pos(1)) + abs(start_pos(2)-goal_pos(2));
        fprintf('   最短路径步数: %d\n', size(path, 1));
        fprintf('   曼哈顿距离: %d\n', manhattan_dist);
    end
end

%% ==================== 简化演示函数 ====================
function [demo_path, demo_dist] = simple_dijkstra_demo(demo_map, demo_start, demo_goal)
    % 简化版的Dijkstra用于演示
    [rows, cols] = size(demo_map);
    
    % 初始化
    distances = inf(rows, cols);
    visited = false(rows, cols);
    prev = cell(rows, cols);
    
    distances(demo_start(1), demo_start(2)) = 0;
    dirs = [-1, 0; 1, 0; 0, -1; 0, 1];
    
    % 运行Dijkstra
    for iter = 1:rows*cols
        % 找到最小距离的未访问节点
        min_val = inf;
        min_pos = [-1, -1];
        
        for r = 1:rows
            for c = 1:cols
                if ~visited(r, c) && distances(r, c) < min_val && demo_map(r, c) == 0
                    min_val = distances(r, c);
                    min_pos = [r, c];
                end
            end
        end
        
        if min_pos(1) == -1
            break;
        end
        
        r = min_pos(1);
        c = min_pos(2);
        visited(r, c) = true;
        
        % 检查是否到达目标
        if r == demo_goal(1) && c == demo_goal(2)
            break;
        end
        
        % 更新邻居
        for d = 1:4
            nr = r + dirs(d, 1);
            nc = c + dirs(d, 2);
            
            if nr >= 1 && nr <= rows && nc >= 1 && nc <= cols
                if demo_map(nr, nc) == 0 && ~visited(nr, nc)
                    new_dist = distances(r, c) + 1;
                    if new_dist < distances(nr, nc)
                        distances(nr, nc) = new_dist;
                        prev{nr, nc} = [r, c];
                    end
                end
            end
        end
    end
    
    % 重建路径
    demo_path = [];
    if distances(demo_goal(1), demo_goal(2)) < inf
        current = demo_goal;
        while ~isempty(current)
            demo_path = [current; demo_path];
            current = prev{current(1), current(2)};
        end
    end
    
    % 处理距离矩阵显示
    demo_dist = distances;
    demo_dist(isinf(demo_dist)) = NaN;
end

1、命令行信息：

复制代码 
========== Dijkstra算法路径规划演示 ==========
1. 创建20x20网格地图...
   起点: (2, 2)
   终点: (18, 18)

2. 运行Dijkstra算法...
   规划完成！耗时: 0.0072秒

3. 可视化结果...
   路径步数: 35
   可视化完成！

========== 程序执行完成 ==========

结果总结：
   地图大小: 20x20
   障碍物比例: 13.5%
   规划时间: 0.0072秒
   最短路径步数: 35
   曼哈顿距离: 32

2、图形窗口

  • 左图：地图+路径

  • 中图：距离场热力图

  • 右图：5x5算法原理演示

相关推荐
SuperEugene1 小时前
Vue3 中后台实战：VXE Table 从基础表格到复杂业务表格全攻略 | Vue生态精选篇
前端·vue.js·状态模式·vue3·vxetable
打小就很皮...1 小时前
实现可交互的泳道图组件（React）
前端·react.js·泳道图
optimistic_chen1 小时前
【Vue3 入门】掌握这些才能优雅上手
前端·javascript·vue.js·前端框架·visual studio code
认真的小羽❅1 小时前
JavaScript完全指南：从入门到精通
开发语言·javascript·ecmascript
qq_263_tohua1 小时前
第107期 刷算法题
算法
2501_940315261 小时前
98验证二叉搜索树
java·数据结构·算法
luckycoding1 小时前
3005. 最大频率元素计数
算法·leetcode·职场和发展
像污秽一样1 小时前
算法设计与分析-算法效率分析基础-分治法
算法·排序算法
我能坚持多久1 小时前
栈与队列OJ问题详解
算法
热门推荐
01GitHub 镜像站点02OpenClaw 使用和管理 MCP 完全指南03本地部署 OpenClaw + DeepSeek-R1 完全指南04OpenClaw 飞书机器人不回复消息？3 小时踩坑总结05得物前端部门，没了06Qwen3.5 开源全解析：从 0.8B 到 397B，代际升级 + 全场景选型指南07OpenClaw macOS 完整安装与本地模型配置教程（实战版）08OpenClaw 连接飞书完整指南：插件安装、配置与踩坑记录09Window 10部署openclaw报错node.exe : npm error code 12810OpenClaw 接入 QQ Bot 完整实践指南