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

一、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列表中挑选离原始点最近的点，这样导致规划效率不高，在排序过程消耗很多不必要的工作时间。

二、部分源代码

clc;
clear all;
close all;
cmap = [1 1 1; ...
        0 0 0; ...
        1 0 0; ...
        0 0 1; ...
        0 1 0; ...
        1 1 0; ...
	0.5 0.5 0.5];

colormap(cmap);
%% Define a small map
map_size = 100;
map = false(map_size);
%% Read Picture For Goal Position
picture=imread('1.png');   %Read a picture 
GoalPosition = ReadPictureForSketch(picture, map_size);
%Problem Defination
nVar = 2;                                              %Number of Variables
VarSize = [1, nVar];                                %Matrix Size of Decision Variables
nPop = length(find(GoalPosition==1));                                             %Population Size(Robot Number)
%Initialization

%The Particle Template
empty_particle.Position = [];
empty_particle.NextPosition = [];
empty_particle.Velocity = [];
empty_particle.Cost = [];
empty_particle.Goal = [];
empty_particle.Best.Position = [];
empty_particle.Best.Cost = [];
empty_particle.Best.Index = [];

%Create Population Array
particle = repmat(empty_particle, nPop, 1);

%Initialize Global Best
GlobalBest.Cost = inf;

%%Initialize Position and Goal
[goal_location_x_array goal_location_y_array] = find(GoalPosition==1);
 for i = 1:nPop
     [start_x start_y] = ind2sub(size(map), i+0.5*size(map, 1)*size(map, 1));
     particle(i).Position  = [start_y start_x];
     particle(i).Goal = [goal_location_x_array(i)  goal_location_y_array(i)];
 end

%initial the map to show
 map_plot = zeros(map_size);
 map_plot(~map) = 1;   % Mark free cells
 map_plot(map)  = 2;   % Mark obstacle cells
 
%% 

while(1)
    for i = 1:nPop
             neighbours = GetSurroundingPositions(particle(i).Position);
             MinCost = inf;
             particle(i).NextPosition =zeros(VarSize);
             %Get the cloest point to goal
             for j = 1:size(neighbours, 1)
                 current_x = neighbours(j, 1);
                 current_y = neighbours(j, 2);
                 if (current_x >0 && current_x <= size(map, 1) && current_y >0 && current_y <= size(map, 1)) %the point in the field
                     if (map_plot(current_x, current_y) ~=2)                                                                               %the point is not collision
                         DistanceToGoal = abs(particle(i).Goal(1) - current_x) + abs(particle(i).Goal(2) - current_y);
                         if(DistanceToGoal < MinCost)       %the point is closer than others
                             particle(i).NextPosition(1) = current_x;
                             particle(i).NextPosition(2) = current_y;
                             MinCost = DistanceToGoal;
                         end
                     end
                 end
             end
    end
GoalExchangeArray = zeros(1, nPop);
for i = 1:nPop
        for j = i:nPop
            if(GoalExchangeArray(i) || GoalExchangeArray(j))
                break
            end
             GoalExchange = false;
             if (isempty(setdiff(particle(i).NextPosition,  particle(j).Position)) && isempty(setdiff(particle(j).NextPosition, particle(i).Position)))
                 GoalExchange = true;
             end
             
%               if (isempty(setdiff(particle(i).NextPosition, particle(j).Position)) && isempty(setdiff(particle(j).NextPosition, particle(j).Position)))
%                  GoalExchange = true;
%               end
%              
%               if (isempty(setdiff(particle(j).NextPosition, particle(i).Position)) && isempty(setdiff(particle(i).NextPosition,  particle(i).Position)))
%                  GoalExchange = true;
%               end
              if(GoalExchange && (mod(randperm(100,1), 10) > 5))
                  TempGoal = particle(i).Goal;                           %exchange the goal
                   particle(i).Goal =  particle(j).Goal;
                   particle(j).Goal =TempGoal;
                   particle(i).NextPosition = particle(i).Position;  %keep the particel static
                   particle(j).NextPosition = particle(j).Position;
                   GoalExchangeArray(i) = 1;
                   GoalExchangeArray(j) = 1;
              end
        end
end
  %check if the NextPosition will colllision with other particles  or not 
    for i= 1:nPop
        if(map_plot(particle(i).NextPosition(1), particle(i).NextPosition(2)) ~=1)
            particle(i).NextPosition = particle(i).Position;
        end
        %Update the map_plot
        map_plot(particle(i).Position(1), particle(i).Position(2)) = 1;
        particle(i).Position = particle(i).NextPosition;
        map_plot(particle(i).Position(1), particle(i).Position(2)) = 4;
    end
    %change the goal according to the position of the particle randomly 
    if (mod(randperm(100,1), 11) > 3)
        goal_assigned = GoalAssignment(reshape([particle(1:nPop).Position], 2, nPop)', reshape([particle(1:nPop).Goal], 2, nPop)', map_size);
        for i=1:nPop
            particle(i).Goal(:) = goal_assigned(i, :); 
        end
    end
    %Display
        ShowImage(map_plot);
    %check the task is finished or not
    
end
    
        

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]陈智康,刘佳,王丹丹,张运喜.改进Dijkstra机器人路径规划算法研究[J].天津职业技术师范大学学报. 2020,30(03)

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

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/125288014