├── Code ├── adjustement.m ├── checkDist.m ├── create_graph.m ├── fix_lambda2.m ├── formazione_rombo.m ├── gradient.m └── movimentoRombo.m ├── Consensus based algorithm for swarm of UAVs.pdf └── README.md /Code/adjustement.m: -------------------------------------------------------------------------------- 1 | function pos = adjustement(pos,obst,dest,x1,x2,dist_matrix,lambda,sec_dist,mu,Cr,G,c) 2 | %c indica da che drone iniziare il primo ciclo for. Se c == 2 vuol dire che 3 | %il leader sta andando verso il target fregandosene degli altri 4 | for i=c:size(pos,2) 5 | c1 = 0; 6 | c2 = 0; 7 | dXr = 0; 8 | dXro = 0; 9 | for j=1:size(pos,2) 10 | if ismember(j,neighbors(G,i)) 11 | dist = dist_matrix(i,j); 12 | target = pos(1:2,j)+dist/(norm(pos(1:2,i)-pos(1:2,j)))*(pos(1:2,i)-pos(1:2,j)); 13 | P1a = x1*target(2)-(x1-target(1)); 14 | P2a = target(2); 15 | Ft = [x1*x2-P1a;x2-P2a]; 16 | Ja=jacobian(Ft); 17 | A = -inv(Ja)*Ft; 18 | N = norm(A); 19 | dXa(j,:)=A/N; 20 | if norm([pos(1:2,i)-pos(1:2,j)])<=sec_dist 21 | P1o = x1*pos(2,j)-(x1-pos(1,j)); 22 | P2o = pos(2,j); 23 | Fo = [x1*x2-P1o;x2-P2o]; 24 | Jo = jacobian(Fo); 25 | R = inv(Jo)*Fo; 26 | dXr=dXr+R/(1+(norm(R)/Cr)^mu*norm(R)^3)-sec_dist/(sec_dist^3*(1+(sec_dist/Cr)^mu)); 27 | disp("PERICOLO"); 28 | end 29 | end 30 | end 31 | for s = 1:size(obst,2) 32 | if(norm(pos(:,i)-obst(:,s))<=sec_dist) 33 | if pos(2,i)<=obst(2,s) 34 | c1 = c1+1; 35 | else 36 | c2 = c2+1; 37 | end 38 | P1o = x1*obst(2,s)-(x1-obst(1,s)); 39 | P2o = obst(2,s); 40 | Fo = [x1*x2-P1o;x2-P2o]; 41 | Jo = jacobian(Fo); 42 | R = inv(Jo)*Fo; 43 | dXr=dXr+0.005*((R/(1+(norm(R)/Cr)^mu*norm(R)^3))-sec_dist/(sec_dist^3*(1+(sec_dist/Cr)^mu))); 44 | % disp("PERICOLO Ostacolo "); 45 | end 46 | end 47 | if i==1 && not(isempty(dest)) 48 | target = dest(1:2); 49 | vers = (target-pos(1:2,i))/norm(pos(1:2,i)-target); 50 | Dxr = double(sum(subs(dXr,[x1,x2],[pos(1,i),pos(2,i)]))); 51 | if c2>c1 52 | Dxr = Dxr*sign(Dxr); 53 | end 54 | if Dxr ~= 0 55 | pos(1:2,i) = pos(1:2,i)+lambda*(Dxr)'; 56 | else 57 | pos(1:2,i) = pos(1:2,i)+lambda*(vers); 58 | end 59 | else 60 | Dxa = double(sum(subs(dXa,[x1,x2],[pos(1,i),pos(2,i)]))); 61 | Dxr = double(sum(subs(dXr,[x1,x2],[pos(1,i),pos(2,i)]))); 62 | if c2>c1 63 | Dxr = Dxr*sign(Dxr); 64 | end 65 | Dx = (Dxa+Dxr)'; 66 | pos(1:2,i) = pos(1:2,i)+lambda*(Dx); 67 | end 68 | end 69 | end 70 | -------------------------------------------------------------------------------- /Code/checkDist.m: -------------------------------------------------------------------------------- 1 | function check = checkDist(pos,dist_matrix,G) 2 | N = size(pos,2); 3 | check = 0; 4 | for i = 1:N 5 | vicini = neighbors(G,i); 6 | for j=1:N 7 | if ismember(j,vicini) 8 | dist=norm([pos(:,i)-pos(:,j)]); 9 | check = check+abs(dist_matrix(i,j)-dist); 10 | end 11 | end 12 | end 13 | check = check/N; 14 | end -------------------------------------------------------------------------------- /Code/create_graph.m: -------------------------------------------------------------------------------- 1 | function [G] = create_graph(pos,d) 2 | N = size(pos,2); 3 | A=zeros(N,N); 4 | dist = zeros(N,N); 5 | for i=1:N 6 | for j=i+1:N 7 | dist(i,j) = norm(pos(:,i)-pos(:,j)); 8 | end 9 | end 10 | M = max(dist); 11 | M = max(M); 12 | if M <= d 13 | eps = 0; 14 | end 15 | for i = 1:N %creo la matrice delle distanze 16 | for j = 1:N 17 | if dist(i,j)<= M && i~=j 18 | A(i,j)=1; %creo la matrice di adiacenza 19 | end 20 | end 21 | end 22 | 23 | 24 | G = graph(A,'upper'); 25 | end -------------------------------------------------------------------------------- /Code/fix_lambda2.m: -------------------------------------------------------------------------------- 1 | function [lambda] = fix_lambda2(check1,check2) 2 | if check2 < 0.4 3 | lambda = 0.008*exp(check2-check1); 4 | else 5 | lambda =0.1*exp(check2-check1); 6 | end 7 | if exp(check2-check1)>10 8 | lambda = 0.5; 9 | end 10 | end -------------------------------------------------------------------------------- /Code/formazione_rombo.m: -------------------------------------------------------------------------------- 1 | function [pos,obst,target,dist_matrix,lambda,sec_dist,mu,Cr,d,tr1,tr2,tr3,tr4] = formazione_rombo(pos,d,target,obst,c,toll) 2 | 3 | syms x1 4 | syms x2 5 | 6 | tr1 = []; 7 | tr2 = []; 8 | tr3 = []; 9 | tr4 = []; 10 | N = 4; 11 | G=create_graph(pos,sqrt(2)*d); 12 | flag=true; 13 | pos = pos'; 14 | while flag==true 15 | for i=1:N 16 | vicini=neighbors(G,i); 17 | varz=0; 18 | for j=1:length(vicini) 19 | varz=varz + pos(i,3)-pos(vicini(j),3); 20 | end 21 | pos(i,3)=pos(i,3)-varz/length(vicini); 22 | end 23 | gradientz=gradient(pos,G); 24 | if abs(gradientz) <=10^(-5) 25 | flag = false; 26 | end 27 | end 28 | pos = pos'; 29 | lambda = 0.1; 30 | sec_dist = 1; 31 | mu = 0.1; 32 | Cr = 1; 33 | dist_matrix = [0,d,sqrt(2)*d,d;d,0,d,sqrt(2)*d;sqrt(2)*d,d,0,d;d,sqrt(2)*d,d,0]; 34 | count=0; 35 | check1 = 100; 36 | while check1>=toll && count<500 37 | disp(count) 38 | disp(check1) 39 | check1 = checkDist(pos,dist_matrix,G); 40 | pos = adjustement(pos,obst,[],x1,x2,dist_matrix,lambda,0.5,mu,Cr,G,c); 41 | G = create_graph(pos,sqrt(2)*d); 42 | check2 = checkDist(pos,dist_matrix,G); 43 | lambda = fix_lambda2(check1,check2); 44 | count = count +1; 45 | tr1 = [[tr1] [pos(:,1)]]; 46 | tr2 = [[tr2] [pos(:,2)]]; 47 | tr3 = [[tr3] [pos(:,3)]]; 48 | tr4 = [[tr4] [pos(:,4)]]; 49 | end 50 | 51 | 52 | % GENERAZIONE TARGET E OSTACOLI 53 | if isempty(target) 54 | target = [pos(1,1)+20 pos(2,1)-3.5 pos(3,1)]'; 55 | end 56 | if isempty(obst) 57 | obst = []; 58 | P = pos(1,1); 59 | b=1; 60 | for a = 1:0.001:8 61 | obst(:,b) = [pos(1,1)+1.5+a pos(2,1)-9+a pos(3,1)]; 62 | b = b+1; 63 | end 64 | for a = 2:0.001:4 65 | obst(:,b) = [pos(1,1)+7.5+a pos(2,1)-1 pos(3,1)]; 66 | b = b+1; 67 | end 68 | for a = 2:0.001:4 69 | obst(:,b) = [pos(1,1)+7.5+a pos(2,1)+1 pos(3,1)]; 70 | b = b+1; 71 | end 72 | for a = 1:0.001:8 73 | obst(:,b) = [pos(1,1)+1.5+a pos(2,1)+9-a pos(3,1)]; 74 | b = b+1; 75 | end 76 | end 77 | end -------------------------------------------------------------------------------- /Code/gradient.m: -------------------------------------------------------------------------------- 1 | function [gradientz,gradienty] = gradient(pos,G) 2 | gradienty = 0; 3 | gradientz = 0; 4 | for i=1:size(pos,1) 5 | vicini = neighbors(G,i); 6 | for j=i:size(pos,1) 7 | if ismember(j,vicini) 8 | gradienty = gradienty+abs(pos(i,2)-pos(j,2)); 9 | gradientz = gradientz+abs(pos(i,3)-pos(j,3)); 10 | end 11 | end 12 | end 13 | end 14 | 15 | -------------------------------------------------------------------------------- /Code/movimentoRombo.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | syms x1 5 | syms x2 6 | grid on 7 | hold on 8 | axis equal 9 | 10 | N = 4; 11 | d = 1.5; 12 | pos = 3*rand(2,N); 13 | pos = [pos; rand(1,N)]; 14 | pos(1:2,1) = [5;0]; 15 | [pos,obst,target,dist_matrix,lambda,sec_dist,mu,Cr,d,tr1,tr2,tr3,tr4] = formazione_rombo(pos,d,[],[],1,0.02); 16 | initial_pos = pos; 17 | plot3(initial_pos(1,1),initial_pos(2,1),initial_pos(3,1),'s','MarkerSize',10, 'MarkerFaceColor','y','HandleVisibility','off'); 18 | plot3(initial_pos(1,2:4),initial_pos(2,2:4),initial_pos(3,2:4),'s','MarkerSize',10, 'MarkerFaceColor','g'); 19 | G = create_graph(pos,sqrt(2)*d); 20 | while norm(pos(:,1)-target(:,1)) > 0.1 21 | pos(:,1) = adjustement(pos(:,1),obst,target,x1,x2,0,10*lambda,0.8,mu,Cr,G,1); 22 | pos = adjustement(pos,obst,[],x1,x2,dist_matrix,5*lambda,1,mu,Cr,G,2); 23 | G = create_graph(pos,sqrt(2)*d); 24 | check = checkDist(pos,dist_matrix,G) 25 | 26 | %ripristino la formazione se si sminchia troppo 27 | while check > 2 28 | pos = adjustement(pos,obst,[],x1,x2,dist_matrix,3*lambda,sec_dist,mu,Cr,G,1); 29 | G = create_graph(pos,sqrt(2)*d); 30 | check = checkDist(pos,dist_matrix,G) 31 | end 32 | 33 | tr1 = [[tr1] [pos(:,1)]]; 34 | tr2 = [[tr2] [pos(:,2)]]; 35 | tr3 = [[tr3] [pos(:,3)]]; 36 | tr4 = [[tr4] [pos(:,4)]]; 37 | disp(norm(pos(:,1)-target(:,1))); 38 | end 39 | G = create_graph(pos,sqrt(2)*d); 40 | check = checkDist(pos,dist_matrix,G); 41 | if check >0.15 42 | [pos,~,~,~,~,~,~,~,~,tr11,tr22,tr33,tr44] = formazione_rombo(pos,d,[],obst,2,0.02); 43 | else 44 | tr11 = []; 45 | tr22 = []; 46 | tr33 = []; 47 | tr44 = []; 48 | end 49 | tr1 = [tr1 tr11]; 50 | tr2 = [tr2 tr22]; 51 | tr3 = [tr3 tr33]; 52 | tr4 = [tr4 tr44]; 53 | plot3(tr1(1,:),tr1(2,:),tr1(3,:),'--'); 54 | plot3(pos(1,1),pos(2,1),pos(3,1),'s','MarkerSize',10, 'MarkerFaceColor','y'); 55 | plot3(pos(1,2:4),pos(2,2:4),pos(3,2:4),'s','MarkerSize',10, 'MarkerFaceColor','r'); 56 | plot3(obst(1,:),obst(2,:),obst(3,:),'*b') 57 | plot3(target(1,1),target(2,1),target(3,1),'-o','MarkerSize',15); 58 | plot3(tr2(1,:),tr2(2,:),tr2(3,:),'--'); 59 | plot3(tr3(1,:),tr3(2,:),tr3(3,:),'--'); 60 | plot3(tr4(1,:),tr4(2,:),tr4(3,:),'--'); 61 | legend('Initial Position','Trajectory','Leader','Final Position','Obstacle','Final Target','FontSize',14); -------------------------------------------------------------------------------- /Consensus based algorithm for swarm of UAVs.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/venezia-antonio/Consensus-based-Algorithm-for-Swarm-UAVs/1e24bc292858331b160b394d096234c9592fbb40/Consensus based algorithm for swarm of UAVs.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Consensus-based-Algorithm-for-Swarm-UAVs --------------------------------------------------------------------------------