├── target.txt ├── data ├── x.mat ├── stopp.mat ├── trajp.mat ├── target.mat ├── storagepoint.mat └── storagerock.mat ├── caculatett.m ├── x.txt ├── ifoccupy.m ├── calculatedist.m ├── printpathIndex.m ├── printPath.m ├── robotavoid.m ├── genellipse.m ├── ellipse.m ├── README.md ├── Createorderform.m ├── getpath.m ├── robotCount.m ├── bidfunc.m ├── Floyd1.m ├── preditflow.m ├── Executetask.m ├── mainfunc.m └── simenvironment.m /target.txt: -------------------------------------------------------------------------------- 1 | 17.5 22 2 | 32.5 11.5 3 | 16.5 2 4 | 1.5 12.5 -------------------------------------------------------------------------------- /data/x.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bkhnk48/AGV-dynamic-path-planning/HEAD/data/x.mat -------------------------------------------------------------------------------- /data/stopp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bkhnk48/AGV-dynamic-path-planning/HEAD/data/stopp.mat -------------------------------------------------------------------------------- /data/trajp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bkhnk48/AGV-dynamic-path-planning/HEAD/data/trajp.mat -------------------------------------------------------------------------------- /data/target.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bkhnk48/AGV-dynamic-path-planning/HEAD/data/target.mat -------------------------------------------------------------------------------- /data/storagepoint.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bkhnk48/AGV-dynamic-path-planning/HEAD/data/storagepoint.mat -------------------------------------------------------------------------------- /data/storagerock.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bkhnk48/AGV-dynamic-path-planning/HEAD/data/storagerock.mat -------------------------------------------------------------------------------- /caculatett.m: -------------------------------------------------------------------------------- 1 | function [tt] = caculatett(d,maxv) 2 | % Calculate the travel time of the robot 3 | tt=d/maxv; 4 | end 5 | -------------------------------------------------------------------------------- /x.txt: -------------------------------------------------------------------------------- 1 | 16.5 23 2 | 16.5 22.5 3 | 16.5 22 4 | 17.5 2 5 | 17.5 1.5 6 | 17.5 1 7 | 0.5 11.5 8 | 1 11.5 9 | 1.5 11.5 10 | 33.5 12.5 11 | 33 12.5 -------------------------------------------------------------------------------- /ifoccupy.m: -------------------------------------------------------------------------------- 1 | function s = ifoccupy(x,p) 2 | %Determine whether there is a parking lot in a certain location 3 | s=0; 4 | for i=1:size(x,1) 5 | if p(1)==x(i,1)&&p(2)==x(i,2) 6 | s=1; 7 | end 8 | end 9 | end 10 | -------------------------------------------------------------------------------- /calculatedist.m: -------------------------------------------------------------------------------- 1 | function [dist] = caculatedist(x,xpath,point,goal) 2 | %Calculate the distance from the current vehicle's current position to the target point 3 | dist = 0; 4 | task=[xpath;point;goal] 5 | for i=1:size(task,1) 6 | dist=dist+norm(x-task(i,:)); 7 | end 8 | 9 | end 10 | -------------------------------------------------------------------------------- /printpathIndex.m: -------------------------------------------------------------------------------- 1 | function [pathindex] = printpathIndex(ind1,ind2,path) 2 | %Recursively return flody algorithm path 3 | node=path(ind1,ind2); 4 | 5 | if node==ind1 6 | pathindex=ind2; 7 | 8 | return; 9 | end 10 | [pathindex] = printpathIndex(ind1,node,path); 11 | % xpath=[xpath node]; 12 | pathindex=[pathindex printpathIndex(node,ind2,path)]; 13 | 14 | end 15 | -------------------------------------------------------------------------------- /printPath.m: -------------------------------------------------------------------------------- 1 | function [finalpath] = printPath(x,goal,trajp,path,dis) 2 | %Convert the path number to a specific coordinate point 3 | 4 | [lib1,ind1]=ismember(x,trajp,'rows'); 5 | [lib2,ind2]=ismember(goal,trajp,'rows'); 6 | pathind=printpathIndex(ind1,ind2,path); 7 | finalpath=[]; 8 | for i=1:size(pathind,2) 9 | finalpath(i,:)=trajp(pathind(i),:); 10 | end 11 | d=dis(ind1,ind2); 12 | end 13 | -------------------------------------------------------------------------------- /robotavoid.m: -------------------------------------------------------------------------------- 1 | function [crashstate,crash] = robotavoid(x,xstate,state,xorder,xpath,xpoint,n,pn,target,stopp,trajp,path,dis) 2 | % 3 | tempx=x;tempxstate=xstate;tempstate=state;tempxorder=xorder;tempn=n;tempxpath=xpath; 4 | temppn=pn; 5 | crashstate=false; 6 | crash=zeros(size(x,1)); 7 | % for i=1:5 8 | % [tempx] = Executetask(tempx,tempxstate,tempstate,tempxorder,tempxpath,xpoint,tempn,temppn,crash,target,stopp,trajp,path,dis); 9 | % end 10 | for i=1:size(x,1) 11 | for j=1:size(x,1) 12 | if i~=j&&norm(tempx(i,:)-tempx(j,:))<0.6 13 | crashstate=true; 14 | crash(i,j)=1; 15 | end 16 | end 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /genellipse.m: -------------------------------------------------------------------------------- 1 | function [xAgent, yAgent] = genellipse(m) 2 | xc=50; %xCenter 3 | %xc = input('Enter the coord of xCenter: '); 4 | yc=50; %yCenter 5 | %yc = input('Enter the coord of yCenter: '); 6 | a=25; %xRad 7 | b=100; %yRad 8 | %m = 1000; 9 | x = zeros(m,1); 10 | y = zeros(m,1); 11 | theta = linspace(0,2*pi,m); 12 | for k = 1:m 13 | x(k) = a * cos(theta(k)); 14 | y(k) = b * sin(theta(k)); 15 | end 16 | %alpha = input('Enter the rotation angle: '); 17 | alpha = 17; %unit: radian 18 | R = [cos(alpha) -sin(alpha); ... 19 | sin(alpha) cos(alpha)]; 20 | rCoords = R*[x' ; y']; 21 | xr = rCoords(1,:)'; 22 | yr = rCoords(2,:)'; 23 | 24 | xAgent = xr + xc; 25 | yAgent = yr + yc; 26 | end -------------------------------------------------------------------------------- /ellipse.m: -------------------------------------------------------------------------------- 1 | clc; 2 | %xc=50; %xCenter 3 | xc = input('Enter the coord of xCenter: '); 4 | %yc=50; %yCenter 5 | yc = input('Enter the coord of yCenter: '); 6 | a=25; %xRad 7 | b=100; %yRad 8 | m = 1000; 9 | x = zeros(m,1); 10 | y = zeros(m,1); 11 | theta = linspace(0,2*pi,m); 12 | for k = 1:m 13 | x(k) = a * cos(theta(k)); 14 | y(k) = b * sin(theta(k)); 15 | end 16 | alpha = input('Enter the rotation angle: '); 17 | R = [cos(alpha) -sin(alpha); ... 18 | sin(alpha) cos(alpha)]; 19 | rCoords = R*[x' ; y']; 20 | xr = rCoords(1,:)'; 21 | yr = rCoords(2,:)'; 22 | plot(x+xc,y+yc,'r'); 23 | grid on; 24 | hold on; 25 | axis equal; 26 | plot(xr+xc,yr+yc,'b'); 27 | xAgent = xr + xc; 28 | yAgent = yr + yc; 29 | plot(xAgent(1), yAgent(1), 'h'); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AGV-dynamic-path-planning 2 | 3 | Robot-simulation-in-a-warehouse-environment 4 | 5 | Just run mainfunc.m 6 | 7 | This is the simulation code for the task allocation and path planning of many robots in a large-size warehouse environment. If you use this code for your research, please cite 8 | 9 | @inproceedings{shi2021task, 10 | title={Task Allocation and Path Planning of Many Robots with Motion Uncertainty in a Warehouse Environment}, author={Shi, Yinbin and Hu, Biao and Huang, Ran }, 11 | booktitle={International Conference on Real-time Computing and Robotics}, 12 | year={2021}, 13 | organization={IEEE} 14 | } 15 | 16 | Simulation 17 | 18 | ![warehouse_robot_anmi](https://user-images.githubusercontent.com/48162469/142749858-0cf64c8f-417d-453d-a6ac-6e3889b0d671.gif) 19 | -------------------------------------------------------------------------------- /Createorderform.m: -------------------------------------------------------------------------------- 1 | function [point,goal] = Createorderform(target) 2 | %Generate orders 3 | point=[]; 4 | global storagepoint; 5 | %global target; 6 | N=size(storagepoint,1); 7 | n=size(target,1); 8 | nodenumber=unidrnd(3); %Number of mission points generated 9 | %list of intermediate working points of AGV 10 | printf("Node number = %d\n", nodenumber); 11 | 12 | task=randperm(N,nodenumber); 13 | for i=1:nodenumber 14 | point(i,:)=storagepoint(task(i),:); 15 | printf("Intermediate dest: [%.2f %.2f]\n", point(i, 1), point(i, 2)); 16 | end 17 | 18 | goalind=unidrnd(n); %randomize the final point of the AGV in it's journey 19 | %n is the number of all final points in the environment 20 | goal=target(goalind,1:2); 21 | printf("Goal: [%.2f %.2f]\n", goal(1, 1), goal(1, 2)); 22 | end 23 | -------------------------------------------------------------------------------- /getpath.m: -------------------------------------------------------------------------------- 1 | function [finalpath,d,dis] = getpath(x,point,goal,trajp,path0,dis)%The position of the car, the passing point, the target point 2 | %Shortest path for multiple mission points 3 | path=[]; 4 | 5 | n=length(dis); 6 | ind=[]; 7 | [lib1,ind1]=ismember(x,trajp,'rows'); 8 | [lib2,ind2]=ismember(goal,trajp,'rows'); 9 | for i=1:size(point,1) 10 | [lib3,ind3]=ismember(point(i,:),trajp,'rows'); 11 | ind=[ind3 ind]; 12 | end 13 | finalpath=[]; 14 | 15 | n=size(point,1); 16 | num=perms(ind); 17 | N=factorial(n); %Calculate the total number of permutations and combinations 18 | 19 | %Traverse all permutations and combinations to find the shortest one 20 | d=10000; 21 | for i=1:N 22 | dd=0; 23 | temp=ind1; 24 | for j=1:n 25 | dd=dd+dis(temp,num(i,j)); 26 | temp=num(i,j); 27 | end 28 | dd=dd+dis(num(i,n),ind2); 29 | if dd2&&x(i,1)<11 7 | if x(i,2)>16&&x(i,2)<21 8 | M(1)=M(1)+1; 9 | end 10 | end 11 | if x(i,1)>11&&x(i,1)<23 12 | if x(i,2)>16&&x(i,2)<21 13 | M(2)=M(2)+1; 14 | end 15 | end 16 | if x(i,1)>23&&x(i,1)<31 17 | if x(i,2)>16&&x(i,2)<21 18 | M(3)=M(3)+1; 19 | end 20 | end 21 | if x(i,1)>2&&x(i,1)<11 22 | if x(i,2)>8&&x(i,2)<16 23 | M(4)=M(4)+1; 24 | end 25 | end 26 | if x(i,1)>11&&x(i,1)<23 27 | if x(i,2)>8&&x(i,2)<16 28 | M(5)=M(5)+1; 29 | end 30 | end 31 | if x(i,1)>23&&x(i,1)<31 32 | if x(i,2)>8&&x(i,2)<16 33 | M(6)=M(6)+1; 34 | end 35 | end 36 | if x(i,1)>2&&x(i,1)<11 37 | if x(i,2)>2&&x(i,2)<8 38 | M(7)=M(7)+1; 39 | end 40 | end 41 | if x(i,1)>11&&x(i,1)<23 42 | if x(i,2)>2&&x(i,2)<8 43 | M(8)=M(8)+1; 44 | end 45 | end 46 | if x(i,1)>23&&x(i,1)<31 47 | if x(i,2)>2&&x(i,2)<8 48 | M(9)=M(9)+1; 49 | end 50 | end 51 | 52 | 53 | 54 | end 55 | -------------------------------------------------------------------------------- /bidfunc.m: -------------------------------------------------------------------------------- 1 | function [finalpath,ind,xstate,dis] = bidfunc(x,xstate,xpath,point,goal,duration,trajp,pn,path0,dis) 2 | %Bidding, choose the robot with the lowest cost, and return the path at the same time 3 | minb=999999; 4 | global v; 5 | 6 | for i=1:size(x,1) 7 | taskp=[]; 8 | if xstate(i)==0 %When the robot currently has no task 9 | [finalpath0,d,dis] = getpath(x(i,:),point,goal,trajp,path0,dis); 10 | [flow] = preditflow(finalpath0,i,x,xpath); 11 | 12 | m=caculatett(d,v)+duration*size(point,1); 13 | tt=caculatett(d,v); 14 | bid=0.5*m+0.5*tt+15*flow; 15 | else 16 | for j=1:size(xpath,1) 17 | if xpath(j,2*i-1)~=0||xpath(j,2*i)~=0 18 | taskp=[taskp;xpath(j,2*i-1:2*i)]; 19 | else 20 | break; 21 | end 22 | end 23 | 24 | d1=norm(x(i,:)-taskp(1,:)); 25 | for k=1:size(taskp,1)-1 26 | d1=d1+norm(taskp(k,:)-taskp(k+1,:)); 27 | end 28 | [finalpath2,d2,dis] = getpath(taskp(size(taskp,1),:),point,goal,trajp,path0,dis); 29 | d=d1+d2; 30 | 31 | finalpath0=[taskp;finalpath2]; 32 | [flow] = preditflow(finalpath0,i,x,xpath); 33 | m=caculatett(d,v)+duration*(pn(i)+size(point,1)); 34 | tt=caculatett(d,v); 35 | bid=0.5*m+0.5*tt+15*flow; 36 | end 37 | if biddis(i,k)+dis(k,j) 87 | dis(i,j)=dis(i,k)+dis(k,j); 88 | path(i,j)=k; 89 | end 90 | end 91 | end 92 | end 93 | %} 94 | dis = importdata('dis.txt') ; 95 | path = importdata('path.txt'); 96 | 97 | end 98 | -------------------------------------------------------------------------------- /preditflow.m: -------------------------------------------------------------------------------- 1 | function [flow] = preditflow(finalpath,i,x,xpath) 2 | %Calculate the total density of robots passing through each area 3 | 4 | tempx=x;tempxpath=xpath; 5 | tempxpath(1:size(finalpath,1),2*i-1:2*i)=finalpath; 6 | 7 | f=zeros(1,9); 8 | 9 | for m=1:fix(size(finalpath,1)/4) 10 | for n=1:100 11 | tempx(n,:)=tempxpath(4*m,2*n-1:2*n); 12 | end 13 | 14 | if tempx(i,1)>3&&tempx(i,1)<11 15 | if tempx(i,2)>16&&tempx(i,2)<21 16 | [M] = robotCount(tempx); 17 | if M(1)>f(1) 18 | f(1)=M(1)/40; 19 | end 20 | end 21 | end 22 | if tempx(i,1)>11&&tempx(i,1)<23 23 | if tempx(i,2)>16&&tempx(i,2)<21 24 | [M] = robotCount(tempx); 25 | if M(2)>f(2) 26 | f(2)=M(2)/60; 27 | end 28 | end 29 | end 30 | if tempx(i,1)>23&&tempx(i,1)<31 31 | if tempx(i,2)>16&&tempx(i,2)<21 32 | [M] = robotCount(tempx); 33 | if M(3)>f(3) 34 | f(3)=M(3)/40; 35 | end 36 | end 37 | end 38 | if tempx(i,1)>3&&tempx(i,1)<11 39 | if tempx(i,2)>8&&tempx(i,2)<16 40 | [M] = robotCount(tempx); 41 | if M(4)>f(4) 42 | f(4)=M(4)/64; 43 | end 44 | end 45 | end 46 | if tempx(i,1)>11&&tempx(i,1)<23 47 | if tempx(i,2)>8&&tempx(i,2)<16 48 | [M] = robotCount(tempx); 49 | if M(5)>f(5) 50 | f(5)=M(5)/96; 51 | end 52 | end 53 | end 54 | if tempx(i,1)>23&&tempx(i,1)<31 55 | if tempx(i,2)>8&&tempx(i,2)<16 56 | [M] = robotCount(tempx); 57 | if M(6)>f(6) 58 | f(6)=M(6)/64; 59 | end 60 | end 61 | end 62 | if tempx(i,1)>3&&tempx(i,1)<11 63 | if tempx(i,2)>3&&tempx(i,2)<8 64 | [M] = robotCount(tempx); 65 | if M(7)>f(7) 66 | f(7)=M(7)/40; 67 | end 68 | end 69 | end 70 | if tempx(i,1)>11&&tempx(i,1)<23 71 | if tempx(i,2)>3&&tempx(i,2)<8 72 | [M] = robotCount(tempx); 73 | if M(8)>f(8) 74 | f(8)=M(8)/60; 75 | end 76 | end 77 | end 78 | if tempx(i,1)>23&&tempx(i,1)<31 79 | if tempx(i,2)>3&&tempx(i,2)<8 80 | [M] = robotCount(tempx); 81 | if M(9)>f(9) 82 | f(9)=M(9)/40; 83 | end 84 | end 85 | end 86 | 87 | end 88 | flow=sum(f); 89 | end 90 | -------------------------------------------------------------------------------- /Executetask.m: -------------------------------------------------------------------------------- 1 | function [x,xstate,state,xpath,n,pn,xorder] = Executetask(x,xstate,state,xorder,xpath,xpoint,n,pn,crash,target,stopp,trajp,path,dis) % 2 | % After the bidding is completed, the order task is executed and the robot starts to move 3 | 4 | dt=0.1;v=0.5; 5 | 6 | for i=1:size(x,1) 7 | if xstate(i)==1 8 | if state(i)==0 9 | unitp=(xorder(n(i),2*i-1:2*i)-x(i,:))/norm(xorder(n(i),2*i-1:2*i)-x(i,:)); %The unit vector of the current robot pointing to the current target 10 | dist=dt*(v+normrnd(0,0.1)); %Calculate the movement distance and add a random deviation to the speed 11 | 12 | %Determine whether there is a conflict between robots and act accordingly 13 | for j=1:size(x,1) 14 | unitpj=(xorder(n(j),2*j-1:2*j)-x(j,:))/norm(xorder(n(j),2*j-1:2*j)-x(j,:)); %The unit vector of the robot j pointing to its current target 15 | if crash(i,j)==1&&xstate(j)==1 16 | if state(j)==1 17 | x(i,:)=x(i,:); 18 | break; 19 | else 20 | if unitp~=unitpj 21 | if i0 29 | if x(i,1)x(j,1) 36 | x(i,:)=x(i,:); 37 | else 38 | x(i,:)=unitp*dist+x(i,:); 39 | end 40 | end 41 | if unitp(2)>0 42 | if x(i,2)x(j,2) 49 | x(i,:)=x(i,:); 50 | else 51 | x(i,:)=unitp*dist+x(i,:); 52 | end 53 | end 54 | end 55 | end 56 | end 57 | end 58 | if j==size(x,1) 59 | x(i,:)=unitp*dist+x(i,:); 60 | end 61 | else 62 | x(i,:)=x(i,:); 63 | end 64 | end 65 | 66 | if norm(xorder(n(i),2*i-1:2*i)-x(i,:))<0.06 67 | x(i,:)=xorder(n(i),2*i-1:2*i); 68 | n(i)=n(i)+1; 69 | xpath(:,2*i-1:2*i)=[xorder(n(i):size(xorder,1),2*i-1:2*i);zeros(n(i)-1,2)]; 70 | for k=1:3 71 | if xorder(n(i)-1,2*i-1:2*i)==xpoint(k,2*i-1:2*i) %Determine whether the node is a working point 72 | pn(i)=pn(i)-1; 73 | state(i)=1; 74 | end 75 | end 76 | end 77 | %Ready to enter the robot station after reaching the end 78 | if xstate(i)==1 79 | if xorder(n(i),2*i-1)==0&&xorder(n(i),2*i)==0 80 | [lib1,ind1]=ismember(xorder(n(i)-1,2*i-1:2*i),target,'rows'); 81 | if ind1~=0 82 | for ii=1:size(stopp,2)/2 83 | s=ifoccupy(x,stopp(ind1,2*ii-1:2*ii)); 84 | if s==0 85 | [finalpath] = printPath(xorder(n(i)-1,2*i-1:2*i),stopp(ind1,2*ii-1:2*ii),trajp,path,dis); 86 | xorder(n(i):n(i)+size(finalpath,1)-1,2*i-1:2*i)=finalpath; 87 | xpath(:,2*i-1:2*i)=[xorder(n(i):size(xorder,1),2*i-1:2*i);zeros(n(i)-1,2)]; 88 | break; 89 | end 90 | end 91 | end 92 | end 93 | end 94 | 95 | if xorder(n(i),2*i-1)==0&&xorder(n(i),2*i)==0 %Complete the task, clear the list 96 | xstate(i)=0; 97 | n(i)=1; 98 | xorder(:,2*i-1:2*i)=zeros(size(xorder,1),2); 99 | end 100 | end 101 | 102 | end 103 | -------------------------------------------------------------------------------- /mainfunc.m: -------------------------------------------------------------------------------- 1 | %octave --force-gui 2 | %command to open octave in MacOS 3 | close all; 4 | clear all; 5 | pkg load statistics; 6 | 7 | load('./data/trajp.mat') % Track point of the storage environment 8 | %luu tru cac diem ma AGV co the di qua. 9 | %trajp stores reachable points for AGV 10 | %dlmwrite('trajp.txt', trajp, 'delimiter','\t','newline','pc'); => How to export .mat to txt 11 | %load('./data/x.mat') %100 initial positions of robots 12 | x = importdata('x.txt') ; 13 | %luu tru cac diem ma AGV co the xuat phat 14 | %x stores points where AGVs would start 15 | load('./data/storagepoint.mat') %Corresponding working point on the shelf 16 | %You could read the meaning of storagepoint at: 17 | %https://github.com/bkhnk48/AGV-dynamic-path-planning/issues/3#issue-1061956103 18 | load('./data/storagerock.mat') %Shelf position 19 | %meaning of storagerock: 20 | %https://github.com/bkhnk48/AGV-dynamic-path-planning/issues/3#issuecomment-977472672 21 | %load('./data/target.mat') %Final goal point 22 | %meaning of target: 23 | %https://github.com/bkhnk48/AGV-dynamic-path-planning/issues/3#issuecomment-977492322 24 | target = importdata('target.txt') ; 25 | load('./data/stopp.mat') 26 | %Corresponding to the target point, the stopping point in each robot station 27 | %meaning of stopp: 28 | %https://github.com/bkhnk48/AGV-dynamic-path-planning/issues/3#issuecomment-977555334 29 | 30 | xpath=zeros(1500,200);%The remaining tasks used to store the current order of the robot 31 | xstate=zeros(1,100); %Used to indicate the status of the robot, 0 has not accepted the order 1 is executing the order 32 | state=zeros(1,100); %Used to judge whether the robot has reached the task point and is ready to perform the task 33 | xpoint=zeros(3,200); 34 | xorder=zeros(1500,200); %?Order task matrix, used to store the current path of all robots 35 | 36 | dt=0.1; 37 | global v 38 | v = 0.5; 39 | duration = 1; %Robot working hours 40 | grid on 41 | [t,dis,path] = Floyd1(trajp); %Generate the adjacency matrix of the graph 42 | time=0; 43 | worktime=zeros(1,size(x,1));%Record the time that the robot has been working at the current working point 44 | [point,goal] = Createorderform(target); %Generate order 45 | num=1; 46 | pn=zeros(1,100) 47 | n=ones(size(x,1),1); %Used to record which node the robot has reached on its current path 48 | printf("size(x, 1) = %d\n", size(x, 1)); 49 | t=0; 50 | m = 1000; 51 | step = 0; 52 | [xAgent, yAgent] = genellipse(m); 53 | %% 54 | while 1 55 | if num<2 %Generate an order every -s 56 | [point,goal] = Createorderform(target); %Generate order 57 | % point=[3 3.5] 58 | % goal=[4.5 2] 59 | num=num+1; 60 | [crashstate,crash] = robotavoid(x,xstate,state,xorder,xpath,xpoint,n,pn,target,stopp,trajp,path,dis); 61 | [finalpath,ind,xstate] = bidfunc(x,xstate,xpath,point,goal,duration,trajp,pn,path,dis); %Bidding, giving the path to the lowest cost car 62 | printf("num = %d, ind = %d\n", num, ind); 63 | xorder(:,2*ind-1:2*ind)=zeros(1500,2); 64 | 65 | n(ind)=1; 66 | xorder(1:size(finalpath,1),2*ind-1:2*ind)=finalpath; 67 | xpath(1:size(finalpath,1),2*ind-1:2*ind)=finalpath; 68 | xpoint(:,2*ind-1:2*ind)=[point;zeros(3-size(point,1),2)]; 69 | pn(ind)=size(point,1); 70 | time=0; 71 | end 72 | while 1 73 | 74 | hold off 75 | [crashstate,crash] = robotavoid(x,xstate,state,xorder,xpath,xpoint,n,pn,target,stopp,trajp,path,dis); 76 | [x,xstate,state,xpath,n,pn,xorder] = Executetask(x,xstate,state,xorder,xpath,xpoint,n,pn,crash,target,stopp,trajp,path,dis); %Perform task 77 | time=time+dt; 78 | t=t+dt; 79 | worktime; 80 | step = mod((step + 1), m); 81 | plot(xAgent(step), yAgent(step), 'h'); 82 | plot(finalpath(:,1),finalpath(:,2),'--');hold on; 83 | plot(x(:,1),x(:,2),'ro','MarkerFaceColor','r');hold on; 84 | plot(storagerock(:,1),storagerock(:,2),'sk');hold on; 85 | simenvironment(target) 86 | plot(target(:,1),target(:,2),'xr');hold on; 87 | grid on 88 | drawnow; 89 | for j=1:size(x,1) 90 | if state(j)==1 %If the robot reaches the working point, start timing until the end of the task time 91 | worktime(j)=worktime(j)+dt; 92 | end 93 | if worktime(j)>duration 94 | worktime(j)=0; 95 | state(j)=0; 96 | end 97 | end 98 | if time>2 99 | break; 100 | end 101 | 102 | end 103 | if num == 2 &&(~any(xstate)) 104 | %Note: the any(xstate) returns 1 if there is at least one non-zero value in xstate 105 | %in case all elements of xstate are zero, the any returns 0. The ~any(xstate) 106 | % returns 1 then. 107 | break; 108 | end 109 | 110 | end 111 | printf("FINISH!\n"); -------------------------------------------------------------------------------- /simenvironment.m: -------------------------------------------------------------------------------- 1 | function simenvironment(target) 2 | %Simulation environment 3 | global storagerock; 4 | %global target; 5 | global trajp; 6 | axis([-1 35 -1 25]); 7 | % grid on 8 | 9 | %Draw the storage rock? 10 | rectangle('Position',[2.9,2.9,1.2,0.2]);hold on;rectangle('Position',[2.9,8.9,1.2,0.2]);hold on; 11 | rectangle('Position',[2.9,4.9,1.2,0.2]);hold on;rectangle('Position',[2.9,10.9,1.2,0.2]);hold on; 12 | rectangle('Position',[2.9,6.9,1.2,0.2]);hold on;rectangle('Position',[2.9,12.9,1.2,0.2]);hold on; 13 | rectangle('Position',[5.9,4.9,1.2,0.2]);hold on;rectangle('Position',[2.9,14.9,1.2,0.2]);hold on; 14 | rectangle('Position',[5.9,2.9,1.2,0.2]);hold on;rectangle('Position',[2.9,16.9,1.2,0.2]);hold on; 15 | rectangle('Position',[5.9,6.9,1.2,0.2]);hold on;rectangle('Position',[2.9,18.9,1.2,0.2]);hold on; 16 | rectangle('Position',[8.9,6.9,1.2,0.2]);hold on;rectangle('Position',[2.9,20.9,1.2,0.2]);hold on; 17 | rectangle('Position',[8.9,4.9,1.2,0.2]);hold on;rectangle('Position',[5.9,8.9,1.2,0.2]);hold on; 18 | rectangle('Position',[8.9,2.9,1.2,0.2]);hold on;rectangle('Position',[5.9,10.9,1.2,0.2]);hold on; 19 | rectangle('Position',[11.9,6.9,1.2,0.2]);hold on;rectangle('Position',[5.9,12.9,1.2,0.2]);hold on; 20 | rectangle('Position',[11.9,4.9,1.2,0.2]);hold on;rectangle('Position',[5.9,14.9,1.2,0.2]);hold on; 21 | rectangle('Position',[11.9,2.9,1.2,0.2]);hold on;rectangle('Position',[5.9,16.9,1.2,0.2]);hold on; 22 | rectangle('Position',[5.9,18.9,1.2,0.2]);hold on;rectangle('Position',[14.9,2.9,1.2,0.2]);hold on; 23 | rectangle('Position',[5.9,20.9,1.2,0.2]);hold on;rectangle('Position',[14.9,4.9,1.2,0.2]);hold on; 24 | rectangle('Position',[8.9,8.9,1.2,0.2]);hold on;rectangle('Position',[14.9,6.9,1.2,0.2]);hold on; 25 | rectangle('Position',[8.9,10.9,1.2,0.2]);hold on;rectangle('Position',[14.9,8.9,1.2,0.2]);hold on; 26 | rectangle('Position',[8.9,12.9,1.2,0.2]);hold on;rectangle('Position',[14.9,10.9,1.2,0.2]);hold on; 27 | rectangle('Position',[8.9,14.9,1.2,0.2]);hold on;rectangle('Position',[14.9,12.9,1.2,0.2]);hold on; 28 | rectangle('Position',[8.9,16.9,1.2,0.2]);hold on;rectangle('Position',[14.9,14.9,1.2,0.2]);hold on; 29 | rectangle('Position',[8.9,18.9,1.2,0.2]);hold on;rectangle('Position',[14.9,16.9,1.2,0.2]);hold on; 30 | rectangle('Position',[8.9,20.9,1.2,0.2]);hold on;rectangle('Position',[14.9,18.9,1.2,0.2]);hold on; 31 | rectangle('Position',[11.9,8.9,1.2,0.2]);hold on;rectangle('Position',[14.9,20.9,1.2,0.2]);hold on; 32 | rectangle('Position',[11.9,10.9,1.2,0.2]);hold on; 33 | rectangle('Position',[11.9,12.9,1.2,0.2]);hold on; 34 | rectangle('Position',[11.9,14.9,1.2,0.2]);hold on; 35 | rectangle('Position',[11.9,16.9,1.2,0.2]);hold on; 36 | rectangle('Position',[11.9,18.9,1.2,0.2]);hold on; 37 | rectangle('Position',[11.9,20.9,1.2,0.2]);hold on; 38 | rectangle('Position',[17.9,2.9,1.2,0.2]);hold on; 39 | rectangle('Position',[17.9,4.9,1.2,0.2]);hold on; 40 | rectangle('Position',[17.9,6.9,1.2,0.2]);hold on; 41 | rectangle('Position',[17.9,8.9,1.2,0.2]);hold on; 42 | rectangle('Position',[17.9,10.9,1.2,0.2]);hold on; 43 | rectangle('Position',[17.9,12.9,1.2,0.2]);hold on; 44 | rectangle('Position',[17.9,14.9,1.2,0.2]);hold on; 45 | rectangle('Position',[17.9,16.9,1.2,0.2]);hold on; 46 | rectangle('Position',[17.9,18.9,1.2,0.2]);hold on; 47 | rectangle('Position',[17.9,20.9,1.2,0.2]);hold on; 48 | rectangle('Position',[20.9,2.9,1.2,0.2]);hold on; 49 | rectangle('Position',[20.9,4.9,1.2,0.2]);hold on; 50 | rectangle('Position',[20.9,6.9,1.2,0.2]);hold on; 51 | rectangle('Position',[20.9,8.9,1.2,0.2]);hold on; 52 | rectangle('Position',[20.9,10.9,1.2,0.2]);hold on; 53 | rectangle('Position',[20.9,12.9,1.2,0.2]);hold on; 54 | rectangle('Position',[20.9,14.9,1.2,0.2]);hold on; 55 | rectangle('Position',[20.9,16.9,1.2,0.2]);hold on; 56 | rectangle('Position',[20.9,18.9,1.2,0.2]);hold on; 57 | rectangle('Position',[20.9,20.9,1.2,0.2]);hold on; 58 | rectangle('Position',[23.9,2.9,1.2,0.2]);hold on; 59 | rectangle('Position',[23.9,4.9,1.2,0.2]);hold on; 60 | rectangle('Position',[23.9,6.9,1.2,0.2]);hold on; 61 | rectangle('Position',[23.9,8.9,1.2,0.2]);hold on; 62 | rectangle('Position',[23.9,10.9,1.2,0.2]);hold on; 63 | rectangle('Position',[23.9,12.9,1.2,0.2]);hold on; 64 | rectangle('Position',[23.9,14.9,1.2,0.2]);hold on; 65 | rectangle('Position',[23.9,16.9,1.2,0.2]);hold on; 66 | rectangle('Position',[23.9,18.9,1.2,0.2]);hold on; 67 | rectangle('Position',[23.9,20.9,1.2,0.2]);hold on; 68 | rectangle('Position',[26.9,2.9,1.2,0.2]);hold on; 69 | rectangle('Position',[26.9,4.9,1.2,0.2]);hold on; 70 | rectangle('Position',[26.9,6.9,1.2,0.2]);hold on; 71 | rectangle('Position',[26.9,8.9,1.2,0.2]);hold on; 72 | rectangle('Position',[26.9,10.9,1.2,0.2]);hold on; 73 | rectangle('Position',[26.9,12.9,1.2,0.2]);hold on; 74 | rectangle('Position',[26.9,14.9,1.2,0.2]);hold on; 75 | rectangle('Position',[26.9,16.9,1.2,0.2]);hold on; 76 | rectangle('Position',[26.9,18.9,1.2,0.2]);hold on; 77 | rectangle('Position',[26.9,20.9,1.2,0.2]);hold on; 78 | rectangle('Position',[29.9,2.9,1.2,0.2]);hold on; 79 | rectangle('Position',[29.9,4.9,1.2,0.2]);hold on; 80 | rectangle('Position',[29.9,6.9,1.2,0.2]);hold on; 81 | rectangle('Position',[29.9,8.9,1.2,0.2]);hold on; 82 | rectangle('Position',[29.9,10.9,1.2,0.2]);hold on; 83 | rectangle('Position',[29.9,12.9,1.2,0.2]);hold on; 84 | rectangle('Position',[29.9,14.9,1.2,0.2]);hold on; 85 | rectangle('Position',[29.9,16.9,1.2,0.2]);hold on; 86 | rectangle('Position',[29.9,18.9,1.2,0.2]);hold on; 87 | rectangle('Position',[29.9,20.9,1.2,0.2]);hold on; 88 | 89 | %Stopping points of robots 90 | %rectangle('Position',[4.3,21.8,1.4,1.4]);hold on;rectangle('Position',[7.3,21.8,1.4,1.4]);hold on; 91 | %rectangle('Position',[10.3,21.8,1.4,1.4]);hold on;rectangle('Position',[13.3,21.8,1.4,1.4]);hold on; 92 | rectangle('Position',[16.3,21.8,1.4,1.4]);hold on; 93 | %rectangle('Position',[19.3,21.8,1.4,1.4]);hold on; 94 | %rectangle('Position',[22.3,21.8,1.4,1.4]);hold on; 95 | %rectangle('Position',[25.3,21.8,1.4,1.4]);hold on;rectangle('Position',[28.3,21.8,1.4,1.4]);hold on; 96 | 97 | %rectangle('Position',[0.3,19.3,1.4,1.4]);hold on;rectangle('Position',[0.3,17.3,1.4,1.4]);hold on;rectangle('Position',[0.3,15.3,1.4,1.4]);hold on; 98 | %rectangle('Position',[0.3,13.3,1.4,1.4]);hold on; 99 | rectangle('Position',[0.3,11.3,1.4,1.4]);hold on; 100 | %rectangle('Position',[0.3,9.3,1.4,1.4]);hold on; 101 | %rectangle('Position',[0.3,7.3,1.4,1.4]);hold on;rectangle('Position',[0.3,5.3,1.4,1.4]);hold on;rectangle('Position',[0.3,3.3,1.4,1.4]);hold on; 102 | 103 | %rectangle('Position',[32.3,19.3,1.4,1.4]);hold on;rectangle('Position',[32.3,17.3,1.4,1.4]);hold on;rectangle('Position',[32.3,15.3,1.4,1.4]);hold on; 104 | %rectangle('Position',[32.3,13.3,1.4,1.4]);hold on; 105 | rectangle('Position',[32.3,11.3,1.4,1.4]);hold on; 106 | %rectangle('Position',[32.3,9.3,1.4,1.4]);hold on; 107 | %rectangle('Position',[32.3,7.3,1.4,1.4]);hold on;rectangle('Position',[32.3,5.3,1.4,1.4]);hold on;rectangle('Position',[32.3,3.3,1.4,1.4]);hold on; 108 | 109 | %rectangle('Position',[4.3,0.8,1.4,1.4]);hold on;rectangle('Position',[7.3,0.8,1.4,1.4]);hold on;rectangle('Position',[10.3,0.8,1.4,1.4]);hold on; 110 | %rectangle('Position',[13.3,0.8,1.4,1.4]);hold on; 111 | rectangle('Position',[16.3,0.8,1.4,1.4]);hold on; 112 | %rectangle('Position',[19.3,0.8,1.4,1.4]);hold on; 113 | %rectangle('Position',[22.3,0.8,1.4,1.4]);hold on;rectangle('Position',[25.3,0.8,1.4,1.4]);hold on;rectangle('Position',[28.3,0.8,1.4,1.4]);hold on; 114 | 115 | rectangle('Position',[0,0,34,24]);hold on; 116 | plot(storagerock(:,1),storagerock(:,2),'sk');hold on; 117 | plot(target(:,1),target(:,2),'xr');hold on; 118 | 119 | plot([2.5 31.5],[6 6],'--k');hold on;plot([2.5 31.5],[12 12],'--k');hold on;plot([2.5 31.5],[18 18],'--k');hold on; 120 | plot([2.5 31.5],[4 4],'--k');hold on;plot([2.5 31.5],[14 14],'--k');hold on;plot([2.5 31.5],[20 20],'--k');hold on; 121 | plot([2.5 31.5],[8 8],'--k');hold on;plot([2.5 31.5],[16 16],'--k');hold on; 122 | plot([2.5 31.5],[10 10],'--k');hold on; 123 | plot([5 5],[2.5 21.5],'--k');hold on;plot([14 14],[2.5 21.5],'--k');hold on;plot([20 20],[2.5 21.5],'--k');hold on; 124 | plot([8 8],[2.5 21.5],'--k');hold on;plot([17 17],[2.5 21.5],'--k');hold on;plot([23 23],[2.5 21.5],'--k');hold on; 125 | plot([11 11],[2.5 21.5],'--k');hold on;plot([26 26],[2.5 21.5],'--k');hold on;plot([29 29],[2.5 21.5],'--k');hold on; 126 | 127 | end 128 | --------------------------------------------------------------------------------