├── OUT.mat ├── Optimization_finalProject_report.pdf ├── README.md ├── how_to_run.pdf ├── init.m ├── obstacle_violation_check.m ├── optimizer.m ├── project_ppt_final.pptx ├── settle_check.m ├── simulate.m └── simulate_dev.m /OUT.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rajshah05/UAV-swarm-control-optimization/5998d14d8ceda87f189818f75faee686073f2216/OUT.mat -------------------------------------------------------------------------------- /Optimization_finalProject_report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rajshah05/UAV-swarm-control-optimization/5998d14d8ceda87f189818f75faee686073f2216/Optimization_finalProject_report.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Please refer "how_to_run.pdf" to run the project 2 | 3 | Please refer "Optimization_finalProject_report.pdf" for implementation and project details 4 | 5 | Video link: https://www.youtube.com/watch?v=ufc6QLEn9HE&feature=youtu.be 6 | -------------------------------------------------------------------------------- /how_to_run.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rajshah05/UAV-swarm-control-optimization/5998d14d8ceda87f189818f75faee686073f2216/how_to_run.pdf -------------------------------------------------------------------------------- /init.m: -------------------------------------------------------------------------------- 1 | rng(1); %(random seed for repeatability) 2 | dt = 1e-1; 3 | tspan = 0:dt:3000; 4 | 5 | % Number of agents/obstacles to simulate: 6 | num_agents = 4; 7 | 8 | % Distance settings: 9 | d0 = 1.5; % Desired distance from virtual leader 10 | d_react_obsr = 1.5; %Reaction distance (in terms of obstacle radius) 11 | drone_width = 2; 12 | d_min_obsr = 1; %Minimum allowable distance to obstalce (in terms of obstacle radius) 13 | % IF THE AGENT GOES BELOW THE MINIMUM DISTANCE, THE RUN FAILS. 14 | 15 | % Agent limitations: 16 | max_v = 10; %(Max velocity allowed) 17 | max_u = 2; %(Max acceleration allowed) 18 | 19 | % Virtaul leader (position and velocity): 20 | vl = [-50 -50 5 5]; %(rx,ry,vx,vy) 21 | 22 | %% Initialization: 23 | % Memory allocation: 24 | L = length(tspan); 25 | u = zeros(2*num_agents,L); 26 | r = zeros(2*num_agents,L); 27 | v = zeros(2*num_agents,L); 28 | vl_rv = zeros(4,L); 29 | 30 | % Randomly place obstacles: 31 | obs_radii = 20; 32 | obs = [0 0, obs_radii]; 33 | d_react = d_react_obsr*obs(:,3); % react distance defined off of obstacle size 34 | d_min = obs(:,3) + 1.5; 35 | 36 | % Randomly place the agents: 37 | r(:,1) = repmat(reshape(vl(1:2),[],1),num_agents,1) + [d0; 0; -d0; 0; 0; d0; 0; -d0]; 38 | v(:,1) = repmat(reshape(vl(3:4),[],1),num_agents,1) + .1*randn(2*num_agents,1); 39 | vl_rv(:,1) = vl'; -------------------------------------------------------------------------------- /obstacle_violation_check.m: -------------------------------------------------------------------------------- 1 | function [violation] = obstacle_violation_check(r,obs,d_min) 2 | violation = false; 3 | r_vec = reshape(r',2,[])'; 4 | for jj = 1:size(obs,1) 5 | [~,obs_d] = normr(r_vec - obs(jj,1:2)); 6 | if any(obs_d < d_min(jj)) 7 | violation = true; 8 | break 9 | end 10 | end 11 | end -------------------------------------------------------------------------------- /optimizer.m: -------------------------------------------------------------------------------- 1 | clear; matlabrc; clc; close all; 2 | addpath(genpath('controllers')) 3 | addpath(genpath('dynamics')) 4 | addpath(genpath('tools')) 5 | 6 | % Initial Control gains: 7 | k_ria = 20; %(inter-agent position) 8 | k_via = 30; %(inter-agent velocities) 9 | k_rvl = 50; %(virtual-leader position) 10 | k_vvl = 20; %(virtual-leader velocity) 11 | k_obs = 30; %(obstacle position) 12 | obs_dist = 50; 13 | gains = [k_ria,k_via,k_rvl,k_vvl,k_obs,obs_dist]'; 14 | % simulate_dev(gains,1,1); 15 | 16 | % Optimize: 17 | options = optimoptions('fmincon','FiniteDifferenceStepSize',[1e-2 1e-1 1e-1 1e-1 1e-2 1e-2]); 18 | % A = [4 0 0 0 30/31.5]; 19 | % b = 0; 20 | A =[]; 21 | b = []; 22 | Aeq = []; 23 | beq = []; 24 | lb = [0 0 0 0 0 20]; 25 | baseline = 1; 26 | FOV = 50; 27 | resH = 500; 28 | ub = [100 100 100 100 100 (baseline/2)/tand(((FOV/2)/(resH/2))/2)]; 29 | 30 | ts_history = []; 31 | min_dist = []; 32 | save OUT ts_history min_dist 33 | [optimized_gains,~,~,output] = fmincon(@simulate, gains, A,b,Aeq,beq,lb,ub,[], options); 34 | 35 | %% Plot 36 | load OUT 37 | figure() 38 | plot(min_dist,'*r') 39 | plot(ts_history,'LineWidth',2) 40 | grid on 41 | xlabel('Iteration') 42 | ylabel('Setting Time (sec)') 43 | title('Convergence History') 44 | % saveas(gcf,'../Report/figs/converge','png') 45 | 46 | %% Plot: 47 | % Generate plots of animation: 48 | [ts1, total_error1] = simulate_dev(gains,1,0); 49 | % saveas(gcf,'../Report/figs/trajectory_original','png') 50 | close all 51 | 52 | [ts2, total_error2] = simulate_dev(optimized_gains,1,0); 53 | % saveas(gcf,'../Report/figs/trajectory_optimized','png') 54 | close all 55 | 56 | % Gains from the GA: 57 | ga_gains = [91.9202,1.5019,82.4238,14.4036,34.2422,409.6430]; 58 | [ts3, total_error3] = simulate_dev(ga_gains,1,0); 59 | 60 | 61 | %% 62 | figure() 63 | plot(linspace(0,ts1,numel(total_error1)),total_error1,'LineWidth',2); hold on 64 | plot(linspace(0,ts2,numel(total_error2)),total_error2,'LineWidth',2) 65 | plot(linspace(0,ts3,numel(total_error3)),total_error3,'LineWidth',2) 66 | grid on 67 | ylabel('Total Error Metric') 68 | xlabel('Time (sec)') 69 | title('Error History') 70 | legend('Origina','Inter-Point','Genetic') 71 | % saveas(gcf,'../Report/figs/error','png') -------------------------------------------------------------------------------- /project_ppt_final.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rajshah05/UAV-swarm-control-optimization/5998d14d8ceda87f189818f75faee686073f2216/project_ppt_final.pptx -------------------------------------------------------------------------------- /settle_check.m: -------------------------------------------------------------------------------- 1 | function [settled,total_error] = settle_check(v,vl) 2 | allowable_error = 0.05; 3 | 4 | settled = false; 5 | 6 | % Reformatting: 7 | v_vec = reshape(v',2,[])'; 8 | 9 | % Calculate error signals: 10 | [~,e_via1] = normr(v_vec(1,:) - v_vec(2:end,:)); 11 | [~,e_via2] = normr(v_vec(2,:) - v_vec(3:end,:)); 12 | e_via3 = norm(v_vec(3,:) - v_vec(4,:)); % error for 3 13 | [~,vl_error] = normr(v_vec - vl(3:4)); 14 | ia_error = [e_via3; e_via2; e_via1]; 15 | total_error = norm(ia_error) + norm(vl_error); 16 | 17 | % Must all be past the obstacle (heading away from it): 18 | if total_error < allowable_error 19 | settled = true; 20 | end 21 | end -------------------------------------------------------------------------------- /simulate.m: -------------------------------------------------------------------------------- 1 | function [ts,total_error,broke,ii,vl_rv,r,u] = simulate(gains_avoidance) 2 | fprintf('gains: %f %f %f %f %f %f\n', gains_avoidance) 3 | init 4 | 5 | %% Run simulation: 6 | ts = 0; 7 | total_error = zeros(L-1,1); 8 | broke = 0; 9 | k_ria = .5; %(inter-agent position) 10 | k_via = .25; %(inter-agent velocities) 11 | k_rvl = .5; %(virtual-leader position) 12 | k_vvl = .25; %(virtual-leader velocity) 13 | k_obs = 0; %(obstacle position) 14 | gains_stable = [k_ria,k_via,k_rvl,k_vvl,k_obs]'; 15 | formation_start = true; 16 | 17 | load OUT 18 | min_d = inf; 19 | for ii = 1:L-1 20 | % Propagate the dynamics: 21 | X_out = RK4(@equations_of_motion,dt,[r(:,ii);v(:,ii)],u(:,ii)); 22 | r(:,ii+1) = X_out(1:2*num_agents); 23 | v(:,ii+1) = X_out(2*num_agents+1:4*num_agents); 24 | vl_rv(:,ii+1) = RK4(@equations_of_motion,dt,vl_rv(:,ii),[0;0]); 25 | 26 | % Calculate the control: 27 | [~,vl_obs_d] = normr(vl_rv(1:2,ii+1)' - obs(1:2)); 28 | if vl_obs_d < gains_avoidance(end) || ~formation_start 29 | gains = gains_avoidance(1:end-1); 30 | gains(2) = gains(2)/10; 31 | gains(3) = gains(3)/100; 32 | gains(4) = gains(4)/10; 33 | formation_start = false; 34 | else 35 | gains = gains_stable; 36 | end 37 | u(:,ii+1) = controller(r(:,ii+1),v(:,ii+1),vl_rv(:,ii+1)',obs,d0,d_react,gains); 38 | 39 | % Apply limitations on control input: 40 | u_vec = reshape(u(:,ii+1)',2,[])'; 41 | [u_norm,u_norms] = normr(u_vec); 42 | u_vec(u_norms > max_u,:) = u_norm(u_norms > max_u,:)*max_u; 43 | u(:,ii+1) = reshape(u_vec',[],1); 44 | 45 | % Check if agents hit each other: 46 | r_vec = reshape(r(:,ii)',2,[])'; 47 | ia_d = []; 48 | for jj = 1:num_agents-1 49 | [~,ia_d_temp] = normr(r_vec(jj,:) - r_vec(jj+1:end,:)); 50 | ia_d = [ia_d; ia_d_temp]; 51 | end 52 | if any(ia_d < drone_width) 53 | ts = 1e10; 54 | disp('Run Failed (Agent Collision)') 55 | break 56 | end 57 | 58 | if min(ia_d) < min_d 59 | min_d = min(ia_d); 60 | end 61 | 62 | % Check if any agents got too close to obstacle: 63 | obstacle_violation = obstacle_violation_check(r(:,ii),obs,d_min); 64 | if obstacle_violation 65 | ts = 1e10; 66 | disp('Run Failed (Obstacle Hit)') 67 | break 68 | end 69 | 70 | % Check if settled: 71 | obs2vl = vl_rv(1:2,ii)' - obs(1:2); 72 | theta = acos(dot(obs2vl,vl_rv(3:4,ii)')/(norm(vl_rv(3:4,ii)')*norm(obs2vl))); 73 | [settled,total_error(ii)] = settle_check(v(:,ii),vl_rv(:,ii)'); 74 | if theta < pi/2 % Passed the obstacle 75 | if settled 76 | break 77 | end 78 | end 79 | 80 | if ~formation_start && ~settled 81 | if ts == 0 82 | broke = ii*dt; 83 | end 84 | ts = ts+dt; 85 | end 86 | end 87 | 88 | if ts == 1e10 89 | ts_history = [ts_history; nan]; 90 | min_dist = [min_dist; nan]; 91 | else 92 | ts_history = [ts_history; ts]; 93 | min_dist = [min_dist; min_d]; 94 | end 95 | save OUT ts_history min_dist 96 | fprintf('settled: %f (sec)\n\n',ts) 97 | end -------------------------------------------------------------------------------- /simulate_dev.m: -------------------------------------------------------------------------------- 1 | function [ts,total_error,broke] = simulate_dev(gains,visualize,write_to_video) 2 | % Run simulation: 3 | init 4 | [ts,total_error,broke,ii,vl_rv,r,u] = simulate(gains); 5 | total_error(total_error == 0) = []; 6 | 7 | %% Visualize (Optional): 8 | if nargin == 1 9 | visualize = false; 10 | write_to_video = false; 11 | elseif nargin == 2 12 | write_to_video = false; 13 | end 14 | L = ii; 15 | 16 | if visualize 17 | if write_to_video 18 | vid = VideoWriter('flock_animation.avi'); 19 | open(vid) 20 | end 21 | 22 | figure('units','normalized','outerposition',[0 0 1 1]) 23 | % Initialize the animation: 24 | jj = 1; 25 | virtual_leader = plot(vl_rv(1,1),vl_rv(2,1),'sr','MarkerSize',10,'MarkerFaceColor','r'); hold on 26 | obstacles = circle(obs(1), obs(2), obs(3)); 27 | 28 | num_agents = size(r,1)/2; 29 | agents = gobjects(num_agents,1); 30 | for ii = 1:2:2*num_agents 31 | agents(jj) = plot(r(ii,1),r(ii+1,1),'.k','MarkerSize',20); 32 | jj = jj+1; 33 | end 34 | r_vec = reshape(r(:,ii)',2,[])'; 35 | u_vec = reshape(u(:,ii)',2,[])'; 36 | actuations = quiver(r_vec(:,1),r_vec(:,2),u_vec(:,1),u_vec(:,2),'color','r'); 37 | axis equal 38 | grid on 39 | legend([virtual_leader, agents(1),obstacles(1)],... 40 | 'Virtual Leader','Agents','Obstacles',... 41 | 'location','northwest') 42 | 43 | % Actuall show the animation: 44 | for ii = 1:L 45 | % Add track history: 46 | r_vec = reshape(r(:,ii)',2,[])'; 47 | u_vec = reshape(u(:,ii)',2,[])'; 48 | plot(r_vec(:,1),r_vec(:,2),'.','color',[.5 .5 .5],'MarkerSize',5); hold on 49 | for jj = 1:num_agents 50 | set(agents(jj),'XData',r_vec(jj,1),'YData',r_vec(jj,2)); 51 | end 52 | set(actuations,'XData',r_vec(:,1),'YData',r_vec(:,2),... 53 | 'UData',max_u./u_vec(:,1),'VData',max_u./u_vec(:,2)); 54 | set(virtual_leader,'XData',vl_rv(1,ii),'YData',vl_rv(2,ii)); 55 | % WIDTH = max([abs(vl_rv(1,1)), abs(vl_rv(1,end))]); 56 | % xlim([-WIDTH WIDTH]) 57 | % ylim([-WIDTH WIDTH]) 58 | 59 | drawnow 60 | if write_to_video 61 | frame = getframe(gcf); 62 | writeVideo(vid,frame); 63 | end 64 | end 65 | if write_to_video 66 | close(vid) 67 | end 68 | end 69 | end --------------------------------------------------------------------------------