【路径规划】基于matlab Dijkstra算法多机器人路径规划【含Matlab源码 1883期】

海神之光

已于 2024-10-12 23:41:31 修改

阅读量1.6k

点赞数 2

CC 4.0 BY-SA版权

分类专栏： Matlab路径规划（进阶版）文章标签： matlab

于 2022-06-14 23:12:46 首次发布

代码事宜私信博主

本文链接：https://blog.youkuaiyun.com/TIQCmatlab/article/details/125288079

Matlab路径规划（进阶版）专栏收录该内容

1527 篇文章

订阅专栏

本文介绍了Dijkstra算法在机器人路径规划中的应用，详细阐述了算法的基本步骤和思想，并提供了部分Matlab源代码实现。通过算法，解决机器人在复杂环境中寻找最短路径的问题，但指出经典算法可能存在冗余拐点和效率不高的缺点。此外，代码展示了如何处理多个机器人路径规划，包括避障和路径优化策略。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

💥💥💥💥💥💥💞💞💞💞💞💞💞💞欢迎来到海神之光博客之家💞💞💞💞💞💞💞💞💥💥💥💥💥💥
在这里插入图片描述
✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进；
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式

⛳️座右铭：行百里者，半于九十。
更多Matlab路径规划仿真内容点击👇
①Matlab路径规划（进阶版）
②付费专栏Matlab路径规划（初级版）

⛳️关注优快云海神之光，更多资源等你来！！

⛄一、Dijkstra算法机器人编队路径规划简介

1 传统Dijkstra算法步骤
Dijkstra算法是一种经典的求解最短路径的算法，用于计算一个节点去往其他各个不相关节点的最小移动代价。本思想是把在图中无论如何出现的所有节点分离为2组，第1组包含被准确认定是最短路径的节点，第2组存放待检查的不确定的节点。根据最小移动代价逐渐增大的顺序，逐个将第2组需要检查的节点加入到第1组中，一直到从起始点出发可以到达的所有节点都包含在第1组中。Dijkstra算法运行的主要特征是以机器人抽象的出发点为中心向外层层延伸，直到延伸到整个区域的末端为止。Dijkstra算法一般能够在最终得到最优的路径，但是这个路径偶尔会出现冗余拐点，由于其遍历节点多，所以有时算法的效率不高。Dijkstra算法流程如图2所示。

分别创建2个表，为：START与FINISH。START表中存放环境中已经生成存在但是未经计算的节点，FINISH表中存放所有经过计算考察的节点。

(1）观测寻找在环境路网中离起始源点近而且是没有被计算过的点，把满足调节的点放在START数组中等待检查。

(2）从START表中找出距离起始点最小的点，然后把该点放进FINISH表中。
在这里插入图片描述
(3）遍历去寻找考察当前此点的子节点，计算出起始源点到这些子节点的距离值，将值存入数组中用来排序，将子节点放在START表中。

(4）重复（2）和（3），直到把START表中的点全部清空，或者找到真正要去的目标点。

2 Dijkstra算法的思想
(1）将环境可视图中出现的节点或是顶点分别加入2个组，其中一组存放经过求解验证为最短路径所经过的顶点，该集合用FINISH表示，刚开始时START表中只有1个起始点，在后面的运算过程中每得到1条最短路径，就把相关节点加入到集合FINISH中。如果全部的顶点都加入到FINISH后，此算法运算完成。另一组存放未经过考察且还未被认定为最短路径的顶点，用START表示。

(2）在将顶点逐渐加入到FINISH表的过程中，在运行算法期间总保持从出发点到FINISH表中各个点的最短路径的长度不超过出发点到START表中任何顶点的最短路径长度。

典型的Dijkstra算法在寻找最优路径的准则上较为单一，以某一个点为中心向外层层扩展就类似于画圆，随着搜索半径的逐渐增大，最后总能找到想要的目标点，但同时最优路径会出现一些无关的冗余点，这样在一定程度上给实际的移动机器人造成寻找路径的困难。如果能够对经典算法的计算准则进行优化，达到减少路径中拐点的目的，就可以减少一大部分机器人寻路所耗费的时间。在大面积环境中使用Dijkstra算法时，由于每次都要从START列表中挑选离原始点最近的点，这样导致规划效率不高，在排序过程消耗很多不必要的工作时间。

⛄二、部分源代码

%define a map
close all ; clear all ;
map = false(14) ;
map(3,3:4) = true ; map(3,7:8) = true ; map(3,11:12) = true ;
map(4,3:4) = true ; map(4,7:8) = true ; map(4,11:12) = true ;
map(7,3:4) = true ; map(7,7:8) = true ; map(7,11:12) = true ;
map(8,3:4) = true ; map(8,7:8) = true ; map(8,11:12) = true ;
map(11,3:4) = true ; map(11,7:8) = true ; map(11,11:12) = true ;
map(12,3:4) = true ; map(12,7:8) = true ; map(12,11:12) = true ;
%define the weight of node
weight = [0 1 0 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 0 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 0 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 0 0; 0 0 0 1;%第一列
0 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0 ;1 0 1 0 ; 1 0 0 1; %第二列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第三列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第四列
0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 0 0 1;%第五列
0 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1;%第六列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第七列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第八列
0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 0 0 1;%第九列
0 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1;%第十列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第十一列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第十二列
0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 0 0 1;%第十三列
0 0 1 0; 1 0 0 0; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 0; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 0; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 0;%第十四列
] ;
%start point array and destination point array
start_coords_array = [6,2; 1,1; 5,9; 9,14; 5,8; 12,10; 14,7; 3,14; 1,9; 14,10 ];
dest_coords_mark_array =[9 3 2 8 3 1 3 4 5 3];
FLAG = zeros(1, 10);
[map_color, route, dest_node] = multi_path_generation( start_coords_array, dest_coords_mark_array, map, weight);
%初始化机器人开始的位置并且显示出来
for j = 1:10
robot_color = 16+j;
map_color(route(j, 1)) = robot_color;
end
image(1.5, 1.5, map_color);
grid on;
axis image;
pause(0.5);
for i = 1:100
for j = 1:10
robot_color = 16+j;
if (route(j, i+1)) %如果未达到终点
[exist_robot, collision_robot_index, is_dest_node] = detect_robot_state(map_color, dest_coords_mark_array, route, [j, i], dest_node);
if (~exist_robot) %如果路径可行，则直接移动到下一个路径点
map_color(route(j, i+1)) = robot_color;
map_color(route(j, i)) = 1;
else %否则路径上存在机器人
if (is_dest_node) %如果机器人在终点，则进行“绕桌”行为
%障碍物机器人绕桌
[route, dest_node] = desk_surround(map, route, collision_robot_index, j, i, dest_node, dest_coords_mark_array);
%当前机器人等待
[nrows, ncols] = size(route);
route(j, 1:ncols+1) = add_path_point(route(j, 😃, route(j, i),i+1);
map_color(route(j, i+1)) = robot_color;
else %如果障碍物不在终点，当前机器人等待即可
%当前机器人等待
[nrows, ncols] = size(route);
route(j, 1:ncols+1) = add_path_point(route(j, 😃, route(j, i),i+1);
map_color(route(j, i+1)) = robot_color;
end
end
else %如果已经达到终点,原地等待，并且添加一个FLAG
[nrows, ncols] = size(route);
route(j, 1:ncols+1) = add_path_point(route(j, 😃, route(j, i),i+1);
map_color(route(j, i+1)) = robot_color;
FLAG(j) = 1;
end
%显示过程
image(1.5, 1.5, map_color);
grid on;
axis image;
pause(0.01);

%终止条件
if FLAG == ones(1, 10)
    disp(route');
    success = true;
    [row, col] = size(route);
    for n = 1:col
        if route(:, n) ==zeros(10,1)
            break;
        end
        for m = 1:row
            if route(m,n)
                route_final(m, n)= route(m, n);
            else
                route_final(m, n) = route_final(m, n-1);
            end
        end
    end
    disp(route_final);
    route_final = route_final';
    fid = fopen('path_point.txt', 'wt');
    [row, col] = size(route_final);
    disp(row);
    for m = 1:row
        for n = 1:col
            if route_final(m, n)
                fprintf(fid, '%g\t', route_final(m,n));
            end
        end
        fprintf(fid, '\n');
    end
    return;
end
[nrows, ncols] = size(route);
if (ncols > 500)
    success = false;
    disp('FAILED..........................!');
    return;
end
end

end

% cmap = [1 1 1; …
% 0 0 0; …
% 1 0 0; …
% 0 0 1; …
% 0 1 0; …
% 1 1 0; …
% 0.9 0.9 0.9; %路径1
% 0.8 0.8 0.8; %路径2
% 0.7 0.7 0.7; %路径3
% 0.6 0.6 0.6; %路径4
% 0.5 0.5 0.5; %路径5
% 0.4 0.4 0.4; %路径6
% 0.3 0.3 0.3; %路径7
% 0.2 0.2 0.2; %路径8
% 0.1 0.1 0.1; %路径9
% 0.05 0.05 0.05; %路径10
% 1 0.75 0.8; %机器人1
% 0.86 0.08 0.24; %机器人2
% 0.5 0 0.5; %机器人3
% 0.25 0.4 0.86; %机器人4
% 0.44 0.5 0.56; %机器人5
% 0 1 1; %机器人6
% 1 0.84 0; %机器人7
% 1 0.55 0; %机器人8
% 0.74 0.56 0.56; %机器人9
% 0.12 0.56 1]; %机器人10
% colormap(cmap);

% for i = 1:9
% dest_coords = dest_mark_2_dest_coords(i);
% for j = 1: length(dest_coords)
% map(dest_coords(j,1) , dest_coords(j,2)) = i+1;
%
% end
%
% end
% image(1.5, 1.5, map);
% grid on;
% axis image;
% disp(map)

⛄三、运行结果

在这里插入图片描述

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]陈智康,刘佳,王丹丹,张运喜.改进Dijkstra机器人路径规划算法研究[J].天津职业技术师范大学学报. 2020,30(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流人群疏散病毒扩散晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合