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

1 简介

移动机器人的避障问题是移动机器人控制领域的研究热点。针对给定的移动机器人避障问题 , 探讨了最短路径及最短时间路径的路径规划问题。对于最短路径问题 ,建立 了简化的路径网格模型 ,将其抽象为由节点及边构成的两维图,再使用经典的Dijkstra算法获得可行的最短路径；对于最短时间路径问题 , 通过分析移动机器人弯道运行的速度曲线, 基于几何方法得出了移动时间与过渡圆弧圆心之间严格的数学关系 , 此后借助matlab优化函数获得最佳的移动路径算法可为类似机器人避障问题的解决提供借鉴。

2 部分代码

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 Defination​CostFunction = 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          

3 仿真结果

4 参考文献

[1]邹益民, 高阳, 高碧悦. 一种基于Dijkstra算法的机器人避障问题路径规划[J]. 数学的实践与认识, 2013, 043(010):111-118.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。

​