├── Documents └── swarm_drone_doc.pdf ├── Movies ├── 3D_test.avi ├── 3D_test1.avi ├── 3D_test1.avi.zip ├── 3D_test2.avi ├── 3D_test3.avi ├── 3D_test3.avi.zip ├── test.avi ├── test0.avi ├── test2.avi ├── test3.avi ├── test4.avi ├── test_gradient.avi ├── test_gradient0.avi ├── test_gradient1.avi ├── test_gradient2.avi └── test_gradient3.avi ├── README.md └── src ├── BucketMergingColors.m ├── Drone.m ├── FinalLocationPotential.m ├── GradientDescentUpdate.m ├── Obstacle.m ├── PlotCenterPotential.m ├── Queue.m ├── SolveData_Freelance.m ├── SwarmAlgorithm.m ├── TestGradient_Swarm.m ├── TestSwarmTrajectory.m ├── Test_Swarm_3d.m ├── Test_Swarm_3d_IndepWaypoints.m ├── Test_Swarm_3d_IndepWaypoints2.m ├── Waypoints.m ├── drawObject.m ├── drawWaypoints.m ├── getTotalPotential.m └── potential_field_test2.avi /Documents/swarm_drone_doc.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Documents/swarm_drone_doc.pdf -------------------------------------------------------------------------------- /Movies/3D_test.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/3D_test.avi -------------------------------------------------------------------------------- /Movies/3D_test1.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/3D_test1.avi -------------------------------------------------------------------------------- /Movies/3D_test1.avi.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/3D_test1.avi.zip -------------------------------------------------------------------------------- /Movies/3D_test2.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/3D_test2.avi -------------------------------------------------------------------------------- /Movies/3D_test3.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/3D_test3.avi -------------------------------------------------------------------------------- /Movies/3D_test3.avi.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/3D_test3.avi.zip -------------------------------------------------------------------------------- /Movies/test.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test.avi -------------------------------------------------------------------------------- /Movies/test0.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test0.avi -------------------------------------------------------------------------------- /Movies/test2.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test2.avi -------------------------------------------------------------------------------- /Movies/test3.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test3.avi -------------------------------------------------------------------------------- /Movies/test4.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test4.avi -------------------------------------------------------------------------------- /Movies/test_gradient.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test_gradient.avi -------------------------------------------------------------------------------- /Movies/test_gradient0.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test_gradient0.avi -------------------------------------------------------------------------------- /Movies/test_gradient1.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test_gradient1.avi -------------------------------------------------------------------------------- /Movies/test_gradient2.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test_gradient2.avi -------------------------------------------------------------------------------- /Movies/test_gradient3.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/Movies/test_gradient3.avi -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SwarmAlgorithms 2 | Algorithms developed to make drone swarm move together and strive not to collide with each other. 3 | 4 | ## Notes on Algorithms 5 | - This project was based on constraints set by other individuals 6 | - In all cases, the swarm maneuvers to a target location via the use of a Cost Function that is minimized as they get closer to the goal location 7 | - To minimize the cost, a form of Gradient Descent and a Particle Swarm Optimization are options one can use 8 | - To avoid collisions with each other and obstacles, high cost regions are centered at each particle/obstacle to help the swarm avoid these areas via the optimization 9 | - The way the problem is formulated and solved, it is possible for the swarm to get stuck in local minimums of the cost function. 10 | - In these events, it could be a fix to use a discrete approach like a graph algorithm treating the cost function as a way of weighting each path and then solving the problem using an approximate Traveling Salesman approach. This will have to be future work. 11 | -------------------------------------------------------------------------------- /src/BucketMergingColors.m: -------------------------------------------------------------------------------- 1 | function NetColor = BucketMergingColors( colors, volumes) 2 | net = sum(volumes); 3 | vol_portion = volumes./net; 4 | 5 | NetColor = [0, 0, 0]; 6 | 7 | for i = 1:length(volumes) 8 | NetColor = NetColor + colors(i,:)*vol_portion(i); 9 | end 10 | 11 | end -------------------------------------------------------------------------------- /src/Drone.m: -------------------------------------------------------------------------------- 1 | classdef Drone < handle 2 | 3 | properties 4 | radius; % Cushion radius to ensure nothing hits the drone 5 | pos; % The position vector of the drone 6 | type; % type = 1 is an outside drone, type = 2 is a center drone (only one of these) 7 | dim; % dimensions of the problem 8 | end 9 | 10 | 11 | 12 | 13 | % 14 | % 15 | % These are public functions for the user to call 16 | % 17 | % 18 | methods 19 | 20 | 21 | % 22 | % Constructor method for drones 23 | % 24 | function obj = Drone(radius, pos, type) 25 | obj.radius = radius; 26 | obj.pos = pos; 27 | obj.type = 1; 28 | obj.dim = length(pos); 29 | end % End constructor method 30 | 31 | 32 | 33 | % 34 | % Find potential energy another drone feels based on the position of this drone 35 | % 36 | function val = GetPotential(obj, pos_drone ) 37 | del = pos_drone - obj.pos; % Find the difference in their position vectors 38 | r2 = dot(del,del); % Find the dot product of the difference vector 39 | r = sqrt(r2); % Find the distance of the difference vector; 40 | 41 | if( obj.type <= 2 ) % If the drone is an outside drone 42 | 43 | val = obj.GetOutsidePotential( r ); 44 | 45 | else % If the drone is a center drone 46 | val = obj.GetCenterPotential( r ); 47 | 48 | end % End obj.type if statement 49 | 50 | end % End function to get potential 51 | 52 | 53 | % 54 | % End the public functions 55 | % 56 | end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 57 | 58 | 59 | 60 | 61 | % 62 | % 63 | % These are private functions for only internal methods to call 64 | % 65 | % 66 | methods %(Access = private) 67 | 68 | 69 | % 70 | % Get potential if this drone is a center drone 71 | % 72 | function val = GetCenterPotential( obj, r ) 73 | % Create a potential function that increases for r > 74 | % obj.radius and for r <= obj.radius such that r = obj.radius 75 | % becomes a minimum 76 | R = obj.radius; 77 | 78 | if( r < R ) 79 | val = 1e3*exp( -(r/R)^2 ); 80 | else 81 | val = 10*(r-(R+.3) )^2; % Initial value for potential 82 | end 83 | 84 | end% End center drone potential function 85 | 86 | 87 | % 88 | % Get potential if this drone is not a center drone 89 | % 90 | function val = GetOutsidePotential( obj, r ) 91 | % Create a potential function that increases for r > 92 | % obj.radius and for r <= obj.radius such that r = obj.radius 93 | % becomes a minimum 94 | R = obj.radius; 95 | if( r < R ) 96 | val = 1e3*exp( -(r/R)^2 ); 97 | else 98 | val = 0; 99 | end 100 | 101 | 102 | end% End center drone potential function 103 | 104 | % 105 | % End private methods 106 | % 107 | end 108 | 109 | 110 | % 111 | % End the drone class details 112 | % 113 | end -------------------------------------------------------------------------------- /src/FinalLocationPotential.m: -------------------------------------------------------------------------------- 1 | function potential = FinalLocationPotential( drone_pos, final_pos ) 2 | del = drone_pos-final_pos; 3 | r2 = dot(del,del); 4 | r = sqrt(r2); 5 | 6 | potential = 100*r*r; 7 | 8 | end -------------------------------------------------------------------------------- /src/GradientDescentUpdate.m: -------------------------------------------------------------------------------- 1 | function dx = GradientDescentUpdate(pos, drone_index, drones, obst, end_loc) 2 | dim = length(pos); 3 | arr = [1e-1, 1e-2, 1e-3, 1e-4]; 4 | 5 | Jx = 0; Jy = 0; Jz = 0; J = 0; grad = zeros(dim,1); 6 | 7 | % Check how many dimensions in this problem and find gradient based 8 | % on that 9 | if( dim == 2 ) 10 | Jx = getTotalPotential( pos+1e-4*[1;0], drone_index, drones, obst, end_loc ); % J(x + eps*e1) 11 | Jy = getTotalPotential( pos+1e-4*[0;1], drone_index, drones, obst, end_loc ); % J(x + eps*e2) 12 | J = getTotalPotential( pos, drone_index, drones, obst, end_loc ); % J(x) 13 | 14 | dJdx = (Jx-J)/1e-4; %Get derivative with respect to each dimension 15 | dJdy = (Jy-J)/1e-4; 16 | grad = [dJdx, dJdy]'; 17 | else 18 | Jx = getTotalPotential( pos+1e-4*[1;0;0], drone_index, drones, obst, end_loc ); % J(x + eps*e1) 19 | Jy = getTotalPotential( pos+1e-4*[0;1;0], drone_index, drones, obst, end_loc ); % J(x + eps*e2) 20 | Jz = getTotalPotential( pos+1e-4*[0;0;1], drone_index, drones, obst, end_loc ); % J(x + eps*e2) 21 | J = getTotalPotential( pos, drone_index, drones, obst, end_loc ); % J(x) 22 | 23 | dJdx = (Jx-J)/1e-4; %Get derivative with respect to each dimension 24 | dJdy = (Jy-J)/1e-4; 25 | dJdz = (Jz-J)/1e-4; 26 | grad = [dJdx, dJdy, dJdz]'; 27 | end 28 | 29 | % Find the magnitude of the gradient 30 | mag = sqrt(dot(grad,grad)); 31 | 32 | % Find the unit vector of the gradient 33 | u = grad./mag; 34 | 35 | % Cap the speed of descent to 10 36 | if( mag >= 10 ) 37 | mag = 10; 38 | end 39 | 40 | Jtmp = J; 41 | ind_p = 1; 42 | 43 | if( mag ~= 0 ) % If the magnitude is nonzero, find the nonzero update vector 44 | p = 1; 45 | 46 | % Find the optimal step size 47 | while( p <= length(arr) ) 48 | dp = pos - arr(p)*u*mag; 49 | J0 = getTotalPotential( dp , drone_index, drones, obst, end_loc ); 50 | if( J0< Jtmp ) 51 | Jtmp = J0; 52 | ind_p = p; 53 | end 54 | p = p + 1; 55 | end 56 | 57 | %Create the optimal update vector 58 | dx = -arr(ind_p)*u*mag; 59 | 60 | else% If the gradient magnitude is zero, don't update the vector 61 | dx = u.*0; 62 | end 63 | 64 | end% End of gradient descent update -------------------------------------------------------------------------------- /src/Obstacle.m: -------------------------------------------------------------------------------- 1 | classdef Obstacle < handle 2 | properties 3 | pos; % position vector of obstacle 4 | radius; % cushion radius so that nothing hits the obstacle 5 | dim; % dimensions of the problem 6 | end 7 | 8 | 9 | % 10 | % 11 | % Public methods for user to call 12 | % 13 | % 14 | methods 15 | 16 | % 17 | % Constructor function for obstacles 18 | % 19 | function obj = Obstacle(pos, radius) 20 | obj.pos = pos; 21 | obj.radius = radius; 22 | obj.dim = length(pos); 23 | 24 | end% End the constructor function 25 | 26 | 27 | % 28 | % Find potential obstacle creates for a drone at position pos_drone 29 | % 30 | function val = GetPotential( obj, pos_drone) 31 | % Find the position difference between the 32 | % drone and the obstacle 33 | del = pos_drone - obj.pos; 34 | 35 | % Find the dot product of the difference vector 36 | r2 = dot(del,del); 37 | 38 | % Find the length of the difference vector 39 | r = sqrt(r2); 40 | 41 | val = obj.GetObstaclePotential( r ); 42 | 43 | 44 | end% End Potential Function 45 | 46 | 47 | % 48 | % End public methods 49 | % 50 | end 51 | 52 | 53 | % 54 | % 55 | % Private Methods 56 | % 57 | % 58 | methods (Access = private) 59 | 60 | % 61 | % Get potential caused by obstacle 62 | % 63 | function val = GetObstaclePotential( obj, r ) 64 | % Create a potential function that increases for r > 65 | % obj.radius and for r <= obj.radius such that r = obj.radius 66 | % becomes a minimum 67 | R = obj.radius; 68 | if( r < R ) 69 | val = 1e3*exp( -(r/R)^2 ); 70 | else 71 | val = 0; 72 | end 73 | 74 | end% End center drone potential function 75 | 76 | 77 | % 78 | % End private methods 79 | % 80 | end 81 | 82 | 83 | 84 | % 85 | % End the class definition 86 | % 87 | end -------------------------------------------------------------------------------- /src/PlotCenterPotential.m: -------------------------------------------------------------------------------- 1 | x = linspace(0,3,100); 2 | y = zeros(size(x)); 3 | 4 | for i = 1:length(y) 5 | y(i) = drones(1).GetCenterPotential(x(i)); 6 | end 7 | 8 | 9 | figure(1) 10 | plot(x,y,'b-') 11 | title('Center Potential versus radial distance') 12 | xlabel('Radial Distance') 13 | ylabel('Potential Value') -------------------------------------------------------------------------------- /src/Queue.m: -------------------------------------------------------------------------------- 1 | classdef Queue < handle 2 | 3 | properties 4 | items; 5 | count; 6 | end 7 | 8 | 9 | methods 10 | 11 | function obj = Queue() 12 | obj.items = []; 13 | obj.count = 0; 14 | end 15 | 16 | function push( obj, v ) 17 | if( ~isempty(v) ) 18 | obj.count = obj.count + 1; 19 | obj.items(obj.count).d =v; 20 | end 21 | end 22 | 23 | function v = pop( obj ) 24 | if( obj.count > 0 ) 25 | v = obj.items(1).d; 26 | obj.items = obj.items(2:end); 27 | obj.count = obj.count - 1; 28 | else 29 | v = [ ]; 30 | end 31 | end 32 | 33 | end 34 | 35 | 36 | 37 | 38 | end -------------------------------------------------------------------------------- /src/SolveData_Freelance.m: -------------------------------------------------------------------------------- 1 | %Test Neural Network 2 | %clear all 3 | addpath ../ 4 | addpath ../NeuralNetwork 5 | load('solve_data.mat') 6 | [rows,cols] = size(Prediction_Data); 7 | x = Prediction_Data(:,1:cols-1); 8 | y = Prediction_Data(:,cols); 9 | 10 | NN1 = NeuralNetwork(@SquaredError); 11 | f2 = @(x) LogisticFunc(x); 12 | f1 = @(x) Linear(x); 13 | f = @(x) Quadratic(x); 14 | %Create the layer 15 | NN1.addLayer(Neural_Layer(cols-1,5, f1 )); 16 | 17 | NN1.addLayer(Neural_Layer(5,1, f1 )); 18 | 19 | NN1.addLayer(Neural_Layer(1,0, f1 )); 20 | 21 | Y = zeros(size(y)); 22 | i = 1; 23 | 24 | NN1.formNetwork(x,y , 100 ) 25 | 26 | for i = 1:length(y) 27 | Y(i,:) = NN1.feedforward(x(i,:)); 28 | end 29 | 30 | Results = [x,Y] -------------------------------------------------------------------------------- /src/SwarmAlgorithm.m: -------------------------------------------------------------------------------- 1 | function [x_best, J_best, swarm] = SwarmAlgorithm( dims_span, cost_func, num_swarm, num_iters ) 2 | [r,c] = size(dims_span); 3 | N = r; 4 | 5 | % Initialize the Swarm 6 | i = 1; 7 | swarm = []; 8 | dd = dims_span(:,2) - dims_span(:,1); 9 | while( i <= num_swarm ) 10 | swarm(i).x = rand(r,1).*dd + dims_span(:,1); 11 | swarm(i).J = 0; 12 | i = i + 1; 13 | end 14 | 15 | % Do the optimization iterations 16 | x_best = []; 17 | J_best = 1e50; 18 | J = 0; 19 | it = 0; 20 | 21 | while( it <= num_iters ) 22 | Jb = 1e50; 23 | sb = 0; 24 | 25 | % Evaluate the Swarm Cost 26 | s = 1; 27 | while( s <= num_swarm ) 28 | J = cost_func( swarm(s).x ); 29 | swarm(s).J = J; 30 | 31 | if( J < Jb ) 32 | Jb = J; 33 | sb = s; 34 | 35 | if( J < J_best ) 36 | J_best = J; 37 | x_best = swarm(s).x; 38 | 39 | end% End J_best if 40 | end% End Jb if 41 | 42 | s = s + 1; 43 | end% End computing costs of swarm particles 44 | 45 | % Update locations based on best value 46 | s = 1; 47 | while( s <= num_swarm ) 48 | if( s ~= sb ) 49 | del = swarm(sb).x - swarm(s).x; 50 | coef = (swarm(s).J - swarm(sb).J )/swarm(sb).J; 51 | coef = .1*max(1,1./(1+exp(-coef) ) ); 52 | swarm(s).x = swarm(s).x + coef.*del; 53 | end 54 | 55 | s = s + 1; 56 | end% End updating swarm 57 | 58 | it = it + 1; 59 | end% End iteration loop 60 | 61 | 62 | 63 | end% End Swarm Algorithm -------------------------------------------------------------------------------- /src/TestGradient_Swarm.m: -------------------------------------------------------------------------------- 1 | %Test Swarm Trajectory 2 | clear all; 3 | false = 0; 4 | true = 1; 5 | 6 | %Setup the movie stuff 7 | make_movie = false; 8 | writerObj = []; 9 | if( make_movie ) 10 | writerObj = VideoWriter('test_gradient0.avi'); 11 | writerObj.FrameRate = 15; 12 | open(writerObj); 13 | end 14 | 15 | % Parameter specifications for drones 16 | Nd = 1+50; % number of drones in swarm 17 | ind_c = -1; % index for center/lead drone 18 | radius = 2; % radius for drones and possibly obstacles 19 | dims = [0, 100; 0, 100; 0, 100]; % first row is lower and upper x bounds 20 | % second row is lower and upper y bounds 21 | % third row is lower and upper z bounds 22 | xc = [80,80]'; % initial location for central/lead drone 23 | end_loc = [5,5]'; 24 | 25 | % Parameter specifications for the obstacles 26 | No = 10; 27 | i = 1; 28 | obst = []; 29 | dims_o = [0, 20; 0, 20]; % first row is lower and upper x bounds 30 | % second row is lower and upper y bounds 31 | while( i <= No ) 32 | %randomly generate position of obstacle within bounds 33 | pos = rand(2,1).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,1); 34 | 35 | % Add new obstacle to obstacle array obst 36 | obst = [obst, Obstacle(pos,radius)]; 37 | i = i + 1; 38 | end 39 | 40 | 41 | % Create drones and obstacle arrays 42 | i = 1; 43 | drones = []; 44 | while( i <= Nd ) 45 | if( i == ind_c ) 46 | % Add lead drone to drone array 47 | drones = [drones, Drone(radius, xc , 2)]; 48 | else 49 | % Add a follower drone to drone array 50 | DEL = [xc - 10, xc + 10]; % make swarm initialize around lead drone randomly 51 | pos = rand(2,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1); 52 | drones = [drones, Drone(radius, pos, 1)]; 53 | end 54 | i = i + 1; 55 | end 56 | 57 | % Start updating the drones 58 | close all; 59 | h = figure('Position', [10, 10, 800, 400]); 60 | 61 | % Do the initial drawing 62 | i = 1; 63 | hold on 64 | while( i <=Nd ) % loop through drones 65 | drawObject(drones(i)); 66 | i = i + 1; 67 | end 68 | 69 | i = 1; 70 | while( i <= No ) % loop through obstacles 71 | drawObject(obst(i)); 72 | i = i + 1; 73 | end 74 | hold off 75 | axis([ dims(1,1),dims(1,2),dims(2,1),dims(2,2) ]) 76 | xpos = []; 77 | 78 | % Do iterations 79 | it = 1; 80 | count = 0; 81 | done = 0; 82 | while( done == 0 ) 83 | i = 1; 84 | 85 | % Compute new locations 86 | while( i <= Nd ) 87 | drones(i).pos = drones(i).pos + GradientDescentUpdate(drones(i).pos, i, drones, obst, end_loc); 88 | 89 | if( count > 20 || it >= 200 ) 90 | done = 1; 91 | end 92 | i = i + 1; 93 | end 94 | 95 | % Draw drones in new position 96 | i = 1; 97 | clf(h) 98 | hold on 99 | while( i <= Nd ) % loop through drones 100 | drawObject(drones(i)); 101 | i = i + 1; 102 | end 103 | 104 | i = 1; 105 | while( i <= No ) % loop through obstacles 106 | drawObject(obst(i)); 107 | i = i + 1; 108 | end 109 | hold off 110 | axis([ dims(1,1),dims(1,2),dims(2,1),dims(2,2) ]) 111 | pause(.01) 112 | 113 | % Add frame to movie, if you are wanting to make the movie 114 | if( make_movie ) 115 | frame = getframe; 116 | writeVideo(writerObj,frame); 117 | end 118 | 119 | it = it + 1; 120 | end 121 | 122 | % Close movie file, if you are recording a movie 123 | if( make_movie ) 124 | close(writerObj); 125 | end -------------------------------------------------------------------------------- /src/TestSwarmTrajectory.m: -------------------------------------------------------------------------------- 1 | %Test Swarm Trajectory 2 | clear all; 3 | 4 | %Setup the movie stuff 5 | make_movie = 1; 6 | writerObj = []; 7 | if( make_movie ) 8 | writerObj = VideoWriter('test0.avi'); 9 | writerObj.FrameRate = 15; 10 | open(writerObj); 11 | end 12 | 13 | % Parameter specifications for drones 14 | Nd = 1+50; % number of drones 15 | ind_c = -1; % index for lead/center drone 16 | radius = 1; % radius for drones and possibl obstacles 17 | dims = [0, 100; 0, 100]; % the x and y dimensions for the domain 18 | xc = [80,80]'; % initial location for center/lead drone 19 | end_loc = [5,5]'; % desired final location for drone to go towards 20 | 21 | 22 | % Parameter specifications for the obstacles 23 | No = 10; 24 | i = 1; 25 | obst = []; 26 | dims_o = [0, 20; 0, 20]; 27 | while( i <= No ) % randomly initialize location of obstacles and generate obstacle array 28 | pos = rand(2,1).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,1); 29 | obst = [obst, Obstacle(pos,radius)]; 30 | i = i + 1; 31 | end 32 | 33 | 34 | % Create drones and obstacle arrays 35 | i = 1; 36 | drones = []; 37 | while( i <= Nd ) 38 | if( i == ind_c ) % add lead drone to drone array 39 | drones = [drones, Drone(radius, xc , 2)]; 40 | else 41 | % randomly generate position of follower drones based on location 42 | % of center/lead drone 43 | DEL = [xc - 10, xc + 10]; 44 | pos = rand(2,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1); 45 | 46 | % Add follower drone to drone array 47 | drones = [drones, Drone(radius, pos, 1)]; 48 | end 49 | i = i + 1; 50 | end 51 | 52 | % Start updating the drones 53 | close all; 54 | h = figure('Position', [10, 10, 800, 400]); 55 | 56 | % Do the initial drawing 57 | i = 1; 58 | hold on 59 | while( i <=Nd ) % loop through the drones 60 | drawObject(drones(i)); 61 | i = i + 1; 62 | end 63 | 64 | i = 1; 65 | while( i <= No ) % loop through the obstacles 66 | drawObject(obst(i)); 67 | i = i + 1; 68 | end 69 | hold off 70 | axis([ dims(1,1),dims(1,2),dims(2,1),dims(2,2) ]) 71 | 72 | 73 | % Do iterations 74 | it = 1; 75 | count = 0; 76 | done = 0; 77 | while( done == 0 ) 78 | i = 1; 79 | 80 | % Compute new locations 81 | while( i <= Nd ) 82 | 83 | %specify doing PSO around a 1 unit box around the ith drone 84 | dims_span = [drones(i).pos - 1, drones(i).pos + 1]; 85 | 86 | % Do the PSO optimziation 87 | [xb, Jb ] = SwarmAlgorithm(dims_span, @(x) getTotalPotential( x, i, drones, obst ,end_loc ) , 10, 1); 88 | 89 | % Find the cost at the current drone's position 90 | Jt = getTotalPotential( drones(i).pos, i, drones, obst, end_loc ); 91 | 92 | if( Jb < Jt ) % if best PSO point is better than current location, update to new location 93 | drones(i).pos = xb; 94 | if( i == ind_c) 95 | count = 0; 96 | end 97 | else 98 | if( i == ind_c) % If the center drone isn't moving, add one to the counter 99 | count = count + 1; 100 | end 101 | 102 | if( count > 20 || it > 200 ) % if the center drone hasn't moved for more than 20 iterations, end the simulation 103 | done = 1; 104 | end 105 | end 106 | i = i + 1; 107 | end 108 | 109 | % Draw drones in new position 110 | i = 1; 111 | clf(h) 112 | hold on 113 | while( i <= Nd ) % loop through drones 114 | drawObject(drones(i)); 115 | i = i + 1; 116 | end 117 | 118 | i = 1; 119 | while( i <= No ) % loop through obstacles 120 | drawObject(obst(i)); 121 | i = i + 1; 122 | end 123 | hold off 124 | axis([ dims(1,1),dims(1,2),dims(2,1),dims(2,2) ]) 125 | pause(.01) 126 | 127 | % Add frame to movie, if you are filming 128 | if( make_movie ) 129 | frame = getframe; 130 | writeVideo(writerObj,frame); 131 | end 132 | 133 | it = it + 1; 134 | end 135 | 136 | % Close movie if you are filming 137 | if( make_movie ) 138 | close(writerObj); 139 | end -------------------------------------------------------------------------------- /src/Test_Swarm_3d.m: -------------------------------------------------------------------------------- 1 | %% Test Swarm Trajectory in 3-D 2 | % Author: Christian Howard 3 | % Date : 2/17/2015 4 | % Info : This is a script developed to show the swarm navigation 5 | % algorithms in 3-D. This script also allows for the user to specify 6 | % various waypoints so that they can maneuver around a space 7 | 8 | clear all; 9 | false = 0; 10 | true = 1; 11 | 12 | %% Setup the movie stuff 13 | make_movie = true; 14 | make_pot = false; % make video showing potential field | Takes a LONG time to make, so usually set to false 15 | writerObj = []; 16 | 17 | %% Define the plane of points to evaluate potential function, if you are making video of potential function 18 | x = linspace(0, 50, 100); 19 | y = linspace(0, 50, 100); 20 | z = 5; 21 | 22 | [X,Y] = meshgrid(x,y); 23 | [rx, cx] = size(X); 24 | Z = zeros(rx, cx); 25 | 26 | %% Setup the movie file and frame rate 27 | if( make_movie ) 28 | writerObj = VideoWriter('3D_test3.avi'); 29 | writerObj.FrameRate = 15; 30 | open(writerObj); 31 | end 32 | 33 | % initial perspective in video 34 | az = -60; 35 | el = 20; 36 | 37 | % final perspective in video 38 | azf = -60; 39 | elf = 20; 40 | 41 | %% Parameter specifications for drones 42 | Nd = 1+20; % number of drones in swarm 43 | ind_c = -1; % index for center/lead drone 44 | radius = 2; % radius for drones and possibly obstacles 45 | dims = [0, 50; 0, 50; 0, 50]; % first row is lower and upper x bounds 46 | % second row is lower and upper y bounds 47 | % third row is lower and upper z bounds 48 | xc = [40, 40, 40]'; % initial location for central/lead drone 49 | end_loc = [5, 5, 5]'; % Desired end location 50 | 51 | %% Setup the waypoint object 52 | dist_thresh = 2; % The distance threshold the swarm must be within of the waypoint 53 | % to make the waypoint change to the new one 54 | wypt = Waypoints( dist_thresh ); 55 | wypt.addPoint( [ 15; 50; 20 ] ); % first waypoint 56 | wypt.addPoint( [ 0; 50; 0 ] ); % second waypoint 57 | wypt.addPoint( end_loc ); % last waypoint 58 | 59 | wypt2 = Waypoints( dist_thresh ); 60 | wypt2.addPoint( [ 15; 50; 20 ] ); % first waypoint 61 | wypt2.addPoint( [ 0; 50; 0 ] ); % second waypoint 62 | wypt2.addPoint( end_loc ); % last waypoint 63 | 64 | 65 | %% Parameter specifications for the obstacles 66 | No = 20; 67 | i = 1; 68 | obst = []; 69 | dims_o = [0, 10; 0, 10; 0, 10]; % first row is lower and upper x bounds 70 | % second row is lower and upper y bounds 71 | while( i <= No ) 72 | %randomly generate position of obstacle within bounds 73 | pos = rand(3,1).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,1); 74 | 75 | % Add new obstacle to obstacle array obst 76 | obst = [obst, Obstacle(pos,radius)]; 77 | i = i + 1; 78 | end 79 | 80 | 81 | %% Create drones and obstacle arrays 82 | i = 1; 83 | drones = []; 84 | while( i <= Nd ) 85 | if( i == ind_c ) 86 | % Add lead drone to drone array 87 | drones = [drones, Drone(radius, xc , 2)]; 88 | else 89 | % Add a follower drone to drone array 90 | DEL = [xc - 10, xc + 10]; % make swarm initialize around lead drone randomly 91 | pos = rand(3,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1); 92 | drones = [drones, Drone(radius, pos, 1)]; 93 | end 94 | i = i + 1; 95 | end 96 | 97 | %% Setup figure traits for the movie 98 | close all; 99 | h = figure('Position', [10, 10, 1000, 800]); 100 | set(gca,'nextplot','replacechildren'); 101 | set(gcf,'Renderer','zbuffer'); 102 | dims2 = dims'; 103 | 104 | %% Do the initial drawing 105 | i = 1; 106 | 107 | figure(1) 108 | N = 1; 109 | if( make_pot ) 110 | N = 2; 111 | end 112 | 113 | 114 | subplot(1,N,1) 115 | while( i <=Nd ) % loop through drones 116 | drawObject(drones(i)); 117 | if( i == 1 ) 118 | hold on 119 | end 120 | i = i + 1; 121 | end 122 | 123 | i = 1; 124 | while( i <= No ) % loop through obstacles 125 | drawObject(obst(i)); 126 | i = i + 1; 127 | end 128 | hold off 129 | grid on 130 | view(az, el) 131 | axis(dims2(:)) 132 | xpos = []; 133 | 134 | %% Do iterations of the drone moving to final location 135 | it = 1; 136 | count = 0; 137 | done = 0; 138 | while( done == 0 ) 139 | i = 1; 140 | 141 | % Compute new locations 142 | pt = wypt.getWaypoint( drones(1).pos ); 143 | while( i <= Nd ) 144 | drones(i).pos = drones(i).pos + GradientDescentUpdate(drones(i).pos, i, drones, obst, pt ); 145 | 146 | if( count > 20 || it >= 150 ) 147 | done = 1; 148 | end 149 | i = i + 1; 150 | end 151 | 152 | % Draw drones in new position 153 | i = 1; 154 | 155 | figure(1) 156 | subplot(1,N,1) 157 | 158 | while( i <= Nd ) % loop through drones 159 | drawObject(drones(i)); 160 | if( i == 1 ) 161 | hold on 162 | end 163 | i = i + 1; 164 | end 165 | 166 | drawWaypoints( wypt2 ); 167 | 168 | i = 1; 169 | while( i <= No ) % loop through obstacles 170 | drawObject(obst(i)); 171 | i = i + 1; 172 | end 173 | hold off 174 | coef = it/100; 175 | 176 | %zoom(2) 177 | grid on 178 | axis(dims2(:)) 179 | view(az + coef*(azf - az) , el + coef*(elf - el) ) 180 | %rotate(h, [0, 0, 1], 1) 181 | pause(.01) 182 | 183 | % Add frame to movie, if you are wanting to make the movie 184 | if( make_movie && it > 3 ) 185 | ind = 1; 186 | if( N > 1 ) 187 | subplot(1, N, 2) 188 | ind = 1; 189 | for ii = 1:rx 190 | for jj = 1:cx 191 | Z(ii, jj) = getTotalPotential( [X(ii,jj); Y(ii, jj); z(1)], ind, drones, obst, end_loc ); 192 | end 193 | end 194 | 195 | surf(X,Y,Z) 196 | axis( [x(1), x(end), y(1), y(end)] ) 197 | end 198 | 199 | 200 | frame = getframe(gcf); 201 | writeVideo(writerObj,frame); 202 | end 203 | 204 | it = it + 1; 205 | end 206 | 207 | %% Close movie file, if you are recording a movie 208 | if( make_movie ) 209 | close(writerObj); 210 | end -------------------------------------------------------------------------------- /src/Test_Swarm_3d_IndepWaypoints.m: -------------------------------------------------------------------------------- 1 | %% Test Swarm Trajectory in 3-D 2 | % Author: Christian Howard 3 | % Date : 2/17/2015 4 | % Info : This is a script developed to show the swarm navigation 5 | % algorithms in 3-D. This script also allows for the user to specify 6 | % various waypoints so that they can maneuver around a space 7 | 8 | clear all; 9 | false = 0; 10 | true = 1; 11 | 12 | %% Setup the movie stuff 13 | make_movie = false; 14 | make_pot = false; % make video showing potential field | Takes a LONG time to make, so usually set to false 15 | writerObj = []; 16 | 17 | %% Define the plane of points to evaluate potential function, if you are making video of potential function 18 | x = linspace(0, 50, 100); 19 | y = linspace(0, 50, 100); 20 | z = 5; 21 | 22 | [X,Y] = meshgrid(x,y); 23 | [rx, cx] = size(X); 24 | Z = zeros(rx, cx); 25 | 26 | %% Setup the movie file and frame rate 27 | if( make_movie ) 28 | writerObj = VideoWriter('3D_test3.avi'); 29 | writerObj.FrameRate = 15; 30 | open(writerObj); 31 | end 32 | 33 | % initial perspective in video 34 | az = -60; 35 | el = 20; 36 | 37 | % final perspective in video 38 | azf = -60; 39 | elf = 20; 40 | 41 | %% Parameter specifications for drones 42 | Nd = 1+4; % number of drones in swarm 43 | ind_c = -1; % index for center/lead drone 44 | radius = 2; % radius for drones and possibly obstacles 45 | dims = [0, 50; 0, 50; 0, 50]; % first row is lower and upper x bounds 46 | % second row is lower and upper y bounds 47 | % third row is lower and upper z bounds 48 | xc = [40, 40, 40]'; % initial location for central/lead drone 49 | end_loc = [5, 5, 5]'; % Desired end location 50 | 51 | %% Setup the waypoint object 52 | dist_thresh = 2; % The distance threshold the swarm must be within of the waypoint 53 | % to make the waypoint change to the new one 54 | droneWaypoints = []; 55 | copyWaypoints = []; 56 | 57 | for i = 1:Nd 58 | wypt = Waypoints( dist_thresh ); 59 | 60 | % Add waypoint structure to drone waypoint structure 61 | % The drone waypoint structure represents individual waypoints for each 62 | % drone, so they don't have to move in the same direction. 63 | droneWaypoints(i).w = wypt; 64 | 65 | wypt2 = Waypoints( dist_thresh ); % copy waypoints for use in plotting waypoints 66 | 67 | % Add waypoint structure to copy waypoint structure 68 | copyWaypoints(i).w = wypt2; 69 | 70 | end 71 | 72 | 73 | 74 | % Initialize the waypoints 75 | % For simplicity in this example, will initilize waypoint paths to be the 76 | % same thing for each drone. 77 | pt1 = [15;50;20]; 78 | pt2 = [0; 50; 0]; 79 | pt3 = end_loc; 80 | 81 | for i = 1:Nd 82 | 83 | droneWaypoints(i).w.addPoint( pt1 ); 84 | droneWaypoints(i).w.addPoint( pt2 ); 85 | droneWaypoints(i).w.addPoint( pt3 ); 86 | 87 | copyWaypoints(i).w.addPoint( pt1 ); 88 | copyWaypoints(i).w.addPoint( pt2 ); 89 | copyWaypoints(i).w.addPoint( pt3 ); 90 | 91 | end 92 | 93 | 94 | %% Parameter specifications for the obstacles 95 | No = 4; 96 | i = 1; 97 | obst = []; 98 | dims_o = [0, 10; 0, 10; 0, 10]; % first row is lower and upper x bounds 99 | % second row is lower and upper y bounds 100 | while( i <= No ) 101 | %randomly generate position of obstacle within bounds 102 | pos = rand(3,1).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,1); 103 | 104 | % Add new obstacle to obstacle array obst 105 | obst = [obst, Obstacle(pos,radius)]; 106 | i = i + 1; 107 | end 108 | 109 | 110 | %% Create drones and obstacle arrays 111 | i = 1; 112 | drones = []; 113 | while( i <= Nd ) 114 | if( i == ind_c ) 115 | % Add lead drone to drone array 116 | drones = [drones, Drone(radius, xc , 2)]; 117 | else 118 | % Add a follower drone to drone array 119 | DEL = [xc - 10, xc + 10]; % make swarm initialize around lead drone randomly 120 | pos = rand(3,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1); 121 | drones = [drones, Drone(radius, pos, 1)]; 122 | end 123 | i = i + 1; 124 | end 125 | 126 | %% Setup figure traits for the movie 127 | close all; 128 | h = figure('Position', [10, 10, 1000, 800]); 129 | set(gca,'nextplot','replacechildren'); 130 | set(gcf,'Renderer','zbuffer'); 131 | dims2 = dims'; 132 | 133 | %% Do the initial drawing 134 | i = 1; 135 | 136 | figure(1) 137 | N = 1; 138 | if( make_pot ) 139 | N = 2; 140 | end 141 | 142 | 143 | subplot(1,N,1) 144 | while( i <=Nd ) % loop through drones 145 | drawObject(drones(i)); 146 | if( i == 1 ) 147 | hold on 148 | end 149 | i = i + 1; 150 | end 151 | 152 | i = 1; 153 | while( i <= No ) % loop through obstacles 154 | drawObject(obst(i)); 155 | i = i + 1; 156 | end 157 | hold off 158 | grid on 159 | view(az, el) 160 | axis(dims2(:)) 161 | xpos = []; 162 | 163 | %% Do iterations of the drone moving to final location 164 | it = 1; 165 | count = 0; 166 | done = 0; 167 | while( done == 0 ) 168 | i = 1; 169 | 170 | % Compute new locations 171 | while( i <= Nd ) 172 | pt = droneWaypoints(i).w.getWaypoint( drones(i).pos ); 173 | drones(i).pos = drones(i).pos + GradientDescentUpdate(drones(i).pos, i, drones, obst, pt ); 174 | 175 | if( count > 20 || it >= 150 ) 176 | done = 1; 177 | end 178 | i = i + 1; 179 | end 180 | 181 | % Draw drones in new position 182 | i = 1; 183 | 184 | figure(1) 185 | subplot(1,N,1) 186 | 187 | while( i <= Nd ) % loop through drones 188 | drawObject(drones(i)); 189 | if( i == 1 ) 190 | hold on 191 | end 192 | i = i + 1; 193 | end 194 | 195 | i = 1; 196 | while( i <= Nd ) 197 | drawWaypoints( copyWaypoints(i).w ); 198 | i = i + 1; 199 | end 200 | 201 | i = 1; 202 | while( i <= No ) % loop through obstacles 203 | drawObject(obst(i)); 204 | i = i + 1; 205 | end 206 | hold off 207 | coef = it/100; 208 | 209 | %zoom(2) 210 | grid on 211 | axis(dims2(:)) 212 | view(az + coef*(azf - az) , el + coef*(elf - el) ) 213 | %rotate(h, [0, 0, 1], 1) 214 | pause(.01) 215 | 216 | % Add frame to movie, if you are wanting to make the movie 217 | if( make_movie && it > 3 ) 218 | ind = 1; 219 | if( N > 1 ) 220 | subplot(1, N, 2) 221 | ind = 1; 222 | for ii = 1:rx 223 | for jj = 1:cx 224 | Z(ii, jj) = getTotalPotential( [X(ii,jj); Y(ii, jj); z(1)], ind, drones, obst, end_loc ); 225 | end 226 | end 227 | 228 | surf(X,Y,Z) 229 | axis( [x(1), x(end), y(1), y(end)] ) 230 | end 231 | 232 | 233 | frame = getframe(gcf); 234 | writeVideo(writerObj,frame); 235 | end 236 | 237 | it = it + 1; 238 | end 239 | 240 | %% Close movie file, if you are recording a movie 241 | if( make_movie ) 242 | close(writerObj); 243 | end -------------------------------------------------------------------------------- /src/Test_Swarm_3d_IndepWaypoints2.m: -------------------------------------------------------------------------------- 1 | %% Test Swarm Trajectory in 3-D 2 | % Author: Christian Howard 3 | % Date : 2/17/2015 4 | % Info : This is a script developed to show the swarm navigation 5 | % algorithms in 3-D. This script also allows for the user to specify 6 | % various waypoints so that they can maneuver around a space 7 | 8 | clear all; 9 | false = 0; 10 | true = 1; 11 | 12 | %% Setup the movie stuff 13 | make_movie = false; 14 | make_pot = false; % make video showing potential field | Takes a LONG time to make, so usually set to false 15 | writerObj = []; 16 | 17 | %% Define the plane of points to evaluate potential function, if you are making video of potential function 18 | x = linspace(0, 50, 100); 19 | y = linspace(0, 50, 100); 20 | z = 5; 21 | 22 | [X,Y] = meshgrid(x,y); 23 | [rx, cx] = size(X); 24 | Z = zeros(rx, cx); 25 | 26 | %% Setup the movie file and frame rate 27 | if( make_movie ) 28 | writerObj = VideoWriter('3D_test3.avi'); 29 | writerObj.FrameRate = 15; 30 | open(writerObj); 31 | end 32 | 33 | % initial perspective in video 34 | az = -60; 35 | el = 20; 36 | 37 | % final perspective in video 38 | azf = -60; 39 | elf = 20; 40 | 41 | %% Parameter specifications for drones 42 | Nd = 3; % number of drones in swarm 43 | ind_c = -1; % index for center/lead drone 44 | radius = 2; % radius for drones and possibly obstacles 45 | dims = [0, 50; 0, 50; 0, 50]; % first row is lower and upper x bounds 46 | % second row is lower and upper y bounds 47 | % third row is lower and upper z bounds 48 | xc = [40, 40, 40]'; % initial location for central/lead drone 49 | end_loc = [5, 5, 5]'; % Desired end location 50 | 51 | %% Setup the waypoint object 52 | dist_thresh = 2; % The distance threshold the swarm must be within of the waypoint 53 | % to make the waypoint change to the new one 54 | droneWaypoints = []; 55 | copyWaypoints = []; 56 | 57 | for i = 1:Nd 58 | wypt = Waypoints( dist_thresh ); 59 | 60 | % Add waypoint structure to drone waypoint structure 61 | % The drone waypoint structure represents individual waypoints for each 62 | % drone, so they don't have to move in the same direction. 63 | droneWaypoints(i).w = wypt; 64 | 65 | wypt2 = Waypoints( dist_thresh ); % copy waypoints for use in plotting waypoints 66 | 67 | % Add waypoint structure to copy waypoint structure 68 | copyWaypoints(i).w = wypt2; 69 | 70 | end 71 | 72 | 73 | 74 | % Initialize the waypoints 75 | % For simplicity in this example, will initilize waypoint paths to be the 76 | % same thing for each drone. 77 | pt1 = [15;50;20]; 78 | pt2 = [0; 50; 0]; 79 | pt3 = end_loc; 80 | pt4 = [20;20;20]; 81 | pt5 = [30; 30; 30]; 82 | pt6 = [10; 40; 20]; 83 | pt7 = [10;10;10]; 84 | pt8 = [15; 15; 15]; 85 | pt9 = [10; 20; 20]; 86 | 87 | 88 | i = 1; 89 | 90 | droneWaypoints(i).w.addPoint( pt1 ); 91 | droneWaypoints(i).w.addPoint( pt2 ); 92 | droneWaypoints(i).w.addPoint( pt3 ); 93 | 94 | 95 | copyWaypoints(i).w.addPoint( pt1 ); 96 | copyWaypoints(i).w.addPoint( pt2 ); 97 | copyWaypoints(i).w.addPoint( pt3 ); 98 | 99 | i = 2; 100 | 101 | droneWaypoints(i).w.addPoint( pt4 ); 102 | droneWaypoints(i).w.addPoint( pt5 ); 103 | droneWaypoints(i).w.addPoint( pt6 ); 104 | 105 | 106 | copyWaypoints(i).w.addPoint( pt4 ); 107 | copyWaypoints(i).w.addPoint( pt5 ); 108 | copyWaypoints(i).w.addPoint( pt6 ); 109 | 110 | 111 | i = 3; 112 | 113 | droneWaypoints(i).w.addPoint( pt7 ); 114 | droneWaypoints(i).w.addPoint( pt8 ); 115 | droneWaypoints(i).w.addPoint( pt9 ); 116 | 117 | 118 | copyWaypoints(i).w.addPoint( pt7 ); 119 | copyWaypoints(i).w.addPoint( pt8 ); 120 | copyWaypoints(i).w.addPoint( pt9 ); 121 | 122 | 123 | 124 | %% Parameter specifications for the obstacles 125 | No = 4; 126 | i = 1; 127 | obst = []; 128 | dims_o = [0, 10; 0, 10; 0, 10]; % first row is lower and upper x bounds 129 | % second row is lower and upper y bounds 130 | while( i <= No ) 131 | %randomly generate position of obstacle within bounds 132 | pos = rand(3,1).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,1); 133 | 134 | % Add new obstacle to obstacle array obst 135 | obst = [obst, Obstacle(pos,radius)]; 136 | i = i + 1; 137 | end 138 | 139 | 140 | %% Create drones and obstacle arrays 141 | i = 1; 142 | drones = []; 143 | while( i <= Nd ) 144 | if( i == ind_c ) 145 | % Add lead drone to drone array 146 | drones = [drones, Drone(radius, xc , 2)]; 147 | else 148 | % Add a follower drone to drone array 149 | DEL = [xc - 10, xc + 10]; % make swarm initialize around lead drone randomly 150 | pos = rand(3,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1); 151 | drones = [drones, Drone(radius, pos, 1)]; 152 | end 153 | i = i + 1; 154 | end 155 | 156 | %% Setup figure traits for the movie 157 | close all; 158 | h = figure('Position', [10, 10, 1000, 800]); 159 | set(gca,'nextplot','replacechildren'); 160 | set(gcf,'Renderer','zbuffer'); 161 | dims2 = dims'; 162 | 163 | %% Do the initial drawing 164 | i = 1; 165 | 166 | figure(1) 167 | N = 1; 168 | if( make_pot ) 169 | N = 2; 170 | end 171 | 172 | 173 | subplot(1,N,1) 174 | while( i <=Nd ) % loop through drones 175 | drawObject(drones(i)); 176 | if( i == 1 ) 177 | hold on 178 | end 179 | i = i + 1; 180 | end 181 | 182 | i = 1; 183 | while( i <= No ) % loop through obstacles 184 | drawObject(obst(i)); 185 | i = i + 1; 186 | end 187 | hold off 188 | grid on 189 | view(az, el) 190 | axis(dims2(:)) 191 | xpos = []; 192 | 193 | %% Do iterations of the drone moving to final location 194 | it = 1; 195 | count = 0; 196 | done = 0; 197 | while( done == 0 ) 198 | i = 1; 199 | 200 | % Compute new locations 201 | while( i <= Nd ) 202 | pt = droneWaypoints(i).w.getWaypoint( drones(i).pos ); 203 | drones(i).pos = drones(i).pos + GradientDescentUpdate(drones(i).pos, i, drones, obst, pt ); 204 | 205 | if( count > 20 || it >= 150 ) 206 | done = 1; 207 | end 208 | i = i + 1; 209 | end 210 | 211 | % Draw drones in new position 212 | i = 1; 213 | 214 | figure(1) 215 | subplot(1,N,1) 216 | 217 | while( i <= Nd ) % loop through drones 218 | drawObject(drones(i)); 219 | if( i == 1 ) 220 | hold on 221 | end 222 | i = i + 1; 223 | end 224 | 225 | i = 1; 226 | while( i <= Nd ) 227 | drawWaypoints( copyWaypoints(i).w ); 228 | i = i + 1; 229 | end 230 | 231 | i = 1; 232 | while( i <= No ) % loop through obstacles 233 | drawObject(obst(i)); 234 | i = i + 1; 235 | end 236 | hold off 237 | coef = it/100; 238 | 239 | %zoom(2) 240 | grid on 241 | axis(dims2(:)) 242 | view(az + coef*(azf - az) , el + coef*(elf - el) ) 243 | %rotate(h, [0, 0, 1], 1) 244 | pause(.01) 245 | 246 | % Add frame to movie, if you are wanting to make the movie 247 | if( make_movie && it > 3 ) 248 | ind = 1; 249 | if( N > 1 ) 250 | subplot(1, N, 2) 251 | ind = 1; 252 | for ii = 1:rx 253 | for jj = 1:cx 254 | Z(ii, jj) = getTotalPotential( [X(ii,jj); Y(ii, jj); z(1)], ind, drones, obst, end_loc ); 255 | end 256 | end 257 | 258 | surf(X,Y,Z) 259 | axis( [x(1), x(end), y(1), y(end)] ) 260 | end 261 | 262 | 263 | frame = getframe(gcf); 264 | writeVideo(writerObj,frame); 265 | end 266 | 267 | it = it + 1; 268 | end 269 | 270 | %% Close movie file, if you are recording a movie 271 | if( make_movie ) 272 | close(writerObj); 273 | end -------------------------------------------------------------------------------- /src/Waypoints.m: -------------------------------------------------------------------------------- 1 | classdef Waypoints < handle 2 | 3 | properties 4 | queue; 5 | cwp; % Current Way Point 6 | thresh; 7 | 8 | end 9 | 10 | 11 | methods 12 | 13 | function obj = Waypoints( thresh ) 14 | obj.queue = Queue(); 15 | obj.cwp = []; 16 | obj.thresh = thresh; 17 | end% Waypoints End 18 | 19 | function addPoint( obj, point ) 20 | obj.queue.push( point ); 21 | end% addPoint End 22 | 23 | function out = getWaypoint( obj, pos ) 24 | if( isempty( obj.cwp ) ) 25 | obj.cwp = obj.queue.pop(); 26 | else 27 | del = pos - obj.cwp; 28 | d = sqrt( dot(del,del) ); 29 | if( d < obj.thresh && obj.queue.count > 0 ) 30 | obj.cwp = obj.queue.pop(); 31 | end 32 | end 33 | 34 | out = obj.cwp; 35 | end% getWaypoint End 36 | 37 | end 38 | 39 | 40 | end -------------------------------------------------------------------------------- /src/drawObject.m: -------------------------------------------------------------------------------- 1 | function drawObject( object ) 2 | 3 | color = 'k'; % Color to fill the object with 4 | 5 | if( object.dim == 2 ) 6 | if( isa(object,'Obstacle') ) %Check if the object is an obstacle 7 | color = 'r'; 8 | 9 | else % else the object is a drone 10 | if( object.type == 1 ) 11 | color = 'g'; 12 | else 13 | color = 'b'; 14 | end% end type if 15 | 16 | end% end dim if 17 | 18 | plot(object.pos(1), object.pos(2), '.', 'MarkerFaceColor', color, ... 19 | 'MarkerEdgeColor',color, ... 20 | 'MarkerSize', 10 ) 21 | 22 | 23 | elseif( object.dim == 3 ) 24 | [X,Y,Z] = sphere(16); 25 | [row,col] = size(X); 26 | c = zeros(row,col,3); 27 | pos = object.pos; 28 | r = object.radius/3; 29 | 30 | if( isa(object,'Obstacle') ) 31 | c(:,:,1) = 1; 32 | else 33 | c(:,:,2) = 1; 34 | end 35 | 36 | surf(r.*X + pos(1), r.*Y + pos(2), r.*Z + pos(3), c ); 37 | light('Position',[80, 0, 80]); 38 | h = findobj('Type','surface'); 39 | set(h,'FaceLighting','gouraud',... 40 | 'FaceColor','interp',... 41 | 'EdgeColor',[.4 .4 .4],... 42 | 'LineStyle','none',... 43 | 'BackFaceLighting','lit',... 44 | 'AmbientStrength',0.4,... 45 | 'DiffuseStrength',0.6,... 46 | 'SpecularStrength',0.5); 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /src/drawWaypoints.m: -------------------------------------------------------------------------------- 1 | function drawWaypoints( wpt ) 2 | 3 | color = 'k'; % Color to fill the object with 4 | dim = length(wpt.queue.items(1).d); 5 | if( dim == 2 ) 6 | color = 'b'; 7 | 8 | for k = 1:wpt.queue.count 9 | pos = wpt.queue.items(k).d; 10 | plot(pos(1), pos(2), '.', 'MarkerFaceColor', color, ... 11 | 'MarkerEdgeColor',color, ... 12 | 'MarkerSize', 10 ) 13 | end 14 | 15 | 16 | elseif( dim == 3 ) 17 | [X,Y,Z] = sphere(16); 18 | [row,col] = size(X); 19 | c = zeros(row,col,3); 20 | r = .5; 21 | 22 | c(:,:,3) = 1; 23 | for k = 1:wpt.queue.count 24 | pos = wpt.queue.items(k).d; 25 | surf(r.*X + pos(1), r.*Y + pos(2), r.*Z + pos(3), c ); 26 | light('Position',[80, 0, 80]); 27 | h = findobj('Type','surface'); 28 | set(h,'FaceLighting','gouraud',... 29 | 'FaceColor','interp',... 30 | 'EdgeColor',[.4 .4 .4],... 31 | 'LineStyle','none',... 32 | 'BackFaceLighting','lit',... 33 | 'AmbientStrength',0.4,... 34 | 'DiffuseStrength',0.6,... 35 | 'SpecularStrength',0.5); 36 | end 37 | end 38 | 39 | end -------------------------------------------------------------------------------- /src/getTotalPotential.m: -------------------------------------------------------------------------------- 1 | function J = getTotalPotential( pos, drone_index, drone_array, obst_array , end_loc) 2 | % 3 | % pos | This is the position of the current drone that you 4 | % want to find the potential for 5 | % 6 | % drone_index | This is the index of the drone in the drone_array that you 7 | % want to find the potential for 8 | % 9 | % drone_array | This is the array holding all the data for each drone 10 | % 11 | % obst_array | This is the array holding all the data for each obstacle 12 | % 13 | % end_loc | This is the end location for the center of the swarm 14 | 15 | 16 | 17 | 18 | % Initialize the potential to zero 19 | J = 0; 20 | 21 | % Get the potential from the obstacles first 22 | N_o = length(obst_array); 23 | 24 | i = 1; 25 | while( i <= N_o ) 26 | J = J + obst_array(i).GetPotential( pos ); 27 | i = i + 1; 28 | end 29 | 30 | 31 | % Check if this drone is the center or not 32 | if( drone_array(drone_index).type <= 2 ) % This is an outside drone 33 | 34 | % Find the potential based on the drones 35 | N_d = length(drone_array); 36 | 37 | i = 1; 38 | while( i <= N_d ) 39 | if( i ~= drone_index) 40 | J = J + drone_array(i).GetPotential( pos ); 41 | end 42 | 43 | i = i + 1; 44 | end 45 | 46 | J = J + FinalLocationPotential( pos, end_loc); 47 | 48 | else % This is a center drone 49 | 50 | % Find the potential based on the final location 51 | J = J + FinalLocationPotential( pos, end_loc); 52 | 53 | end 54 | 55 | 56 | end -------------------------------------------------------------------------------- /src/potential_field_test2.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/choward1491/SwarmAlgorithms/bd693adbf8d78b81fbd91edf851f4254ee411f95/src/potential_field_test2.avi --------------------------------------------------------------------------------