【多机器人路径规划】基于Dijkstra算法实现机器人编队路径规划问题附matlab代码

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 mapmap_size = 50;map = false(map_size);% Add an obstacle% map (1:20, 6) = true;% map(20:30, 30:35) = true;% map(45:50, 20:30) = true;% map(30:35,1:5) = true;% calculate the potential field for  robotsstart_coords = [1, 1];dest_coords  = [50, 50];[distanceFromStart_ ] = DijkstraGrid (map,  dest_coords, start_coords);distanceFromStart_(dest_coords(1), dest_coords(2)) = 0;disp(distanceFromStart_ );%% path planning for swarm robots using PSO%Problem DefinationCostFunction = distanceFromStart_ ;   %CostFunctionnVar = 2;                                              %Number of VariablesVarSize = [1, nVar];                                %Matrix Size of Decision Variables%Parameters of PSOnPop = 20;                                             %Population Size(Robot Number)w = 0;   %w = 1;                                                   %Intertia Coefficientwdamp = 0.9;                                            %Damping Ratio of Inertia Coefficienntc1 = 0;  %c1 = 1.5                                                   %Personal Acceleration Coefficienntc2 = 2;                                                   %Social Acceleration Coefficient%Initialization%The Particle Templateempty_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 Arrayparticle = repmat(empty_particle, nPop, 1);%Initialize Global BestGlobalBest.Cost = inf;%Initialize Population Members%%Initialize Position and Goalparticle(1).Position = [1, 1];    particle(1).Goal   = [50, 50];particle(2).Position = [1, 2];    particle(2).Goal   = [50, 49];particle(3).Position = [1, 3];    particle(3).Goal   = [50, 48];particle(4).Position = [1, 4];    particle(4).Goal   = [50, 47];particle(5).Position = [2, 1];    particle(5).Goal   = [50, 46];particle(6).Position = [2, 2];    particle(6).Goal   = [49, 50];particle(7).Position = [2, 3];    particle(7).Goal   = [49, 49];particle(8).Position = [2, 4];    particle(8).Goal   = [49, 48];particle(9).Position = [3, 1];    particle(9).Goal   = [49, 47];particle(10).Position = [3, 2];  particle(10).Goal = [49, 46];particle(11).Position = [3, 3];  particle(11).Goal = [48, 50];particle(12).Position = [3, 4];  particle(12).Goal = [48, 49];particle(13).Position = [4, 1];  particle(13).Goal = [48, 48];particle(14).Position = [4, 2];  particle(14).Goal = [48, 47];particle(15).Position = [4, 3];  particle(15).Goal = [48, 46];particle(16).Position = [4, 4];  particle(16).Goal = [47, 50];particle(17).Position = [5, 1];  particle(17).Goal = [47, 49];particle(18).Position = [5, 2];  particle(18).Goal = [47, 48];particle(19).Position = [5, 3];  particle(19).Goal = [47, 47];particle(20).Position = [5, 4];  particle(20).Goal = [47, 46];%initial the map to show     map_plot = zeros(map_size);     map_plot(~map) = 1;   % Mark free cells     map_plot(map)  = 2;   % Mark obstacle cells for i = 1:nPop    %Initialize Velocity    particle(i).Velocity = zeros(VarSize);        %Evaluation    particle(i).Cost = CostFunction(particle(i).Position(1), particle(i).Position(2));        %Update the Personal Best    particle(i).Best.Position = particle(i).Position;    particle(i).Best.Cost = particle(i).Cost;    particle(i).Best.Index = i;    map_plot(particle(i).Position(1), particle(i).Position(2)) = 4;        %Update Global Best    if (particle(i).Best.Cost < GlobalBest.Cost )        GlobalBest = particle(i).Best;    endendmap_plot(GlobalBest.Position(1), GlobalBest.Position(2)) = 3;ShowImage(map_plot); %% Main Loop of PSOwhile (GlobalBest.Position(1) ~= dest_coords(1)  || GlobalBest.Position(2) ~= dest_coords(2)  )    for i = 1:nPop        %Judge Leader        if (i == GlobalBest.Index)            %Get the Surrouding Positions             neighbours = GetSurroundingPositions(particle(i).Position);             p_x = particle(i).Position(1);             p_y = particle(i).Position(2);             MinCost = inf;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), 10) > 4)        goal_assigned = GoalAssignment(reshape([particle(1:nPop).Position], 2, nPop)', reshape([particle(1:nPop).Goal], 2, nPop)');        for i=1:nPop            particle(i).Goal(:) = goal_assigned(i, :);         end    end    %Display    ShowImage(map_plot);    %check the task is finished or not    end