├── Avoidance.jpg ├── Convex Optimization_ Project Proposal.docx ├── Moving Obstacle.jpg ├── Optimized Avoidance.jpg ├── Original Paper.pdf ├── Presentation.pptx ├── README.md ├── avoidance1.m ├── avoidance2.m ├── avoidance3.m └── avoidance4.m /Avoidance.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ramzihaddad12/Convex-Optimization/2b19210cdc84675241c6731d30d04e705b6f99eb/Avoidance.jpg -------------------------------------------------------------------------------- /Convex Optimization_ Project Proposal.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ramzihaddad12/Convex-Optimization/2b19210cdc84675241c6731d30d04e705b6f99eb/Convex Optimization_ Project Proposal.docx -------------------------------------------------------------------------------- /Moving Obstacle.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ramzihaddad12/Convex-Optimization/2b19210cdc84675241c6731d30d04e705b6f99eb/Moving Obstacle.jpg -------------------------------------------------------------------------------- /Optimized Avoidance.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ramzihaddad12/Convex-Optimization/2b19210cdc84675241c6731d30d04e705b6f99eb/Optimized Avoidance.jpg -------------------------------------------------------------------------------- /Original Paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ramzihaddad12/Convex-Optimization/2b19210cdc84675241c6731d30d04e705b6f99eb/Original Paper.pdf -------------------------------------------------------------------------------- /Presentation.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ramzihaddad12/Convex-Optimization/2b19210cdc84675241c6731d30d04e705b6f99eb/Presentation.pptx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Convex Optimization 2 | 3 | Generating collision-free trajectories in 3D space for multiple quadcopters within seconds by minimizing the total thrust at each time step for each quadcopter using Sequential Convex Programming (SCP). The goal is to transition from an initial to a final set of states, each consisting of position, velocity and acceleration. The vehicles must maintain a minimum distance between each other and satisfy other trajectory constraints. 4 | 5 |

6 | -------------------------------------------------------------------------------- /avoidance1.m: -------------------------------------------------------------------------------- 1 | function [Csts,obj,ops,P,V,A,J] = convprob(myCsts,OneFinalPos,TwoFinalPos,ThreeFinalPos,FourFinalPos) 2 | %% No Obstacle Avoidance constraint 3 | 4 | % Number of Drones 5 | N = 4; % Drones 6 | 7 | % Number of discrete time steps 8 | K = 50; % Steps 9 | 10 | % Discrete Time Step Resolution 11 | h = 1; % Seconds 12 | 13 | % Minimum Seperating Radius 14 | R = 10; 15 | 16 | % Gravity Vector 17 | g = [0,0,-9.81]; 18 | 19 | % Initial Positions 20 | OneInitPos = [0,0,0]; 21 | TwoInitPos = [0,0,2000]; 22 | ThreeInitPos = [0,0,-1000]; 23 | FourInitPos = [0,2000,1000]; 24 | % 25 | % OneInitPos = [0,0,1000]; 26 | % TwoInitPos = [0,0,-1000]; 27 | % ThreeInitPos = [0,1000,0]; 28 | % FourInitPos = [0,-1000,0]; 29 | 30 | % OneInitPos = [100,100,0]; 31 | % TwoInitPos = [-100,100,0]; 32 | % ThreeInitPos = [100,-100,0]; 33 | % FourInitPos = [-100,-100,0]; 34 | 35 | % Initial Velocities 36 | % OneInitVel = [0,0,0]; 37 | % TwoInitVel = [0,0,0]; 38 | % ThreeInitVel = [0,0,0]; 39 | % FourInitVel = [0,0,0]; 40 | OneInitVel = [10,10,10]; 41 | TwoInitVel = [10,20,30]; 42 | ThreeInitVel = [30,30,30]; 43 | FourInitVel = [30,40,50]; 44 | 45 | % Final Positions 46 | % OneFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 47 | % TwoFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 48 | % ThreeFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 49 | % FourFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 50 | 51 | % OneFinalPos = [4000,4000,3000]; 52 | % TwoFinalPos = [4000,4000,3000]; 53 | % ThreeFinalPos = [4000,4000,3000]; 54 | % FourFinalPos = [4000,4000,3000]; 55 | 56 | % Final Velocities 57 | OneFinalVel = [0,0,0]; 58 | TwoFinalVel = [0,0,0]; 59 | ThreeFinalVel = [0,0,0]; 60 | FourFinalVel = [0,0,0]; 61 | 62 | % Min Positions 63 | P_min = [-1000,-1000,-2000]; 64 | 65 | % Max Position Values 66 | P_max = [6000,5000,4000]; 67 | 68 | % Min Velocity 69 | V_min = [-200,-200,-200]; 70 | 71 | % % Max Velocity 72 | V_max = [200,200,200]; 73 | 74 | % Min Acceleration 75 | A_min = [-20,-20,-20]; 76 | 77 | % Max Acceleration 78 | A_max = [20,20,20]; 79 | 80 | % Jerk Min 81 | J_min = [-5,-5,-5]; 82 | 83 | % Jerk Max 84 | J_max = [5,5,5]; 85 | 86 | %% Problem Formulation 87 | 88 | A = sdpvar(3,K,N); 89 | V = sdpvar(3,K,N); 90 | P = sdpvar(3,K,N); 91 | J = sdpvar(3,K-1,N); 92 | 93 | Csts=myCsts; 94 | 95 | % Setting the Velocity and Position Vectors' Equations 96 | for i = 1:N 97 | for k = 1:K-1 98 | Csts = Csts + [V(:,k+1,i) == V(:,k,i) + h*A(:,k,i)]; 99 | Csts = Csts + [P(:,k+1,i) == P(:,k,i) + h*V(:,k,i) + (0.5*h*h)*A(:,k,i)]; 100 | end 101 | end 102 | 103 | % Setting up the Jerk Vector 104 | for i = 1:N 105 | Csts = Csts + [J(:,:,i) == (A(:,2:K,i) - A(:,1:K-1,i))/h]; 106 | end 107 | 108 | for l = 1:3 %3D 109 | for i = 1:N 110 | for k = 1:K 111 | % Velocity Bounds 112 | Csts = Csts + [V(l,k,i) <= V_max(l)]; 113 | Csts = Csts + [V(l,k,i) >= V_min(l)]; 114 | % Acceleration Bounds 115 | Csts = Csts + [A(l,k,i) <= A_max(l)]; 116 | Csts = Csts + [A(l,k,i) >= A_min(l)]; 117 | % Position Constraints 118 | Csts = Csts + [P(l,k,i) <= P_max(l)]; 119 | Csts = Csts + [P(l,k,i) >= P_min(l)]; 120 | if(k= J_min(l)]; 124 | end 125 | end 126 | end 127 | end 128 | 129 | % Initialization Position Constraints 130 | Csts = Csts + [P(:,1,1) == OneInitPos']; 131 | Csts = Csts + [P(:,1,2) == TwoInitPos']; 132 | Csts = Csts + [P(:,1,3) == ThreeInitPos']; 133 | Csts = Csts + [P(:,1,4) == FourInitPos']; 134 | 135 | % Final Position Constraints 136 | Csts = Csts + [P(:,K,1) == OneFinalPos']; 137 | Csts = Csts + [P(:,K,2) == TwoFinalPos']; 138 | Csts = Csts + [P(:,K,3) == ThreeFinalPos']; 139 | Csts = Csts + [P(:,K,4) == FourFinalPos']; 140 | 141 | % Initial Velocity Constraints 142 | Csts = Csts + [V(:,1,1) == OneInitVel']; 143 | Csts = Csts + [V(:,1,2) == TwoInitVel']; 144 | Csts = Csts + [V(:,1,3) == ThreeInitVel']; 145 | Csts = Csts + [V(:,1,4) == FourInitVel']; 146 | 147 | % Final Velocity Constraints 148 | Csts = Csts + [V(:,K,1) == OneFinalVel']; 149 | Csts = Csts + [V(:,K,2) == TwoFinalVel']; 150 | Csts = Csts + [V(:,K,3) == ThreeFinalVel']; 151 | Csts = Csts + [V(:,K,4) == FourFinalVel']; 152 | %%%%%%%%%%%%%%% 153 | 154 | obj = 0; 155 | for i = 1:N 156 | for k = 1:K 157 | obj = obj + (norm(A(:,k,i)+g'))^2; 158 | end 159 | end 160 | 161 | ops = sdpsettings('solver','sdpt3','verbose',1); 162 | optimize(Csts,obj,ops); 163 | pq = value(P); 164 | 165 | end 166 | 167 | -------------------------------------------------------------------------------- /avoidance2.m: -------------------------------------------------------------------------------- 1 | %% Initialization of Variables 2 | clc; clear all; close all; 3 | 4 | % Number of Drones 5 | N = 4; % Drones 6 | 7 | % Number of discrete time steps 8 | K = 10; % Steps 9 | 10 | % Discrete Time Step Resolution 11 | h = 1; % Seconds 12 | 13 | % Minimum Seperating Radius 14 | R = 20; 15 | 16 | % Gravity Vector 17 | g = [0,0,-9.81]; 18 | 19 | % Initial Positions 20 | OneInitPos = [0,0,0]; 21 | TwoInitPos = [0,0,2000]; 22 | ThreeInitPos = [0,0,-1000]; 23 | FourInitPos = [0,2000,1000]; 24 | 25 | % Initial Velocities 26 | OneInitVel = [10,10,10]; 27 | TwoInitVel = [10,20,30]; 28 | ThreeInitVel = [30,30,30]; 29 | FourInitVel = [30,40,50]; 30 | 31 | % Final Positions 32 | OneFinalPos = [0,4000,3000]; 33 | TwoFinalPos = [0,2000,3000]; 34 | ThreeFinalPos = [0,1000,3000]; 35 | FourFinalPos = [0,3000,3000]; 36 | 37 | % Final Velocities 38 | OneFinalVel = [0,0,0]; 39 | TwoFinalVel = [0,0,0]; 40 | ThreeFinalVel = [0,0,0]; 41 | FourFinalVel = [0,0,0]; 42 | 43 | % Min Positions 44 | P_min = [-1000,-1000,-2000]; 45 | 46 | % Max Position Values 47 | P_max = [6000,5000,4000]; 48 | 49 | % Min Velocity 50 | % V_min = [-200,-200,-200]; 51 | 52 | % % Max Velocity 53 | % V_max = [200,200,200]; 54 | 55 | % Min Acceleration 56 | A_min = [-20,-20,-20]; 57 | 58 | % Max Acceleration 59 | A_max = [20,20,20]; 60 | 61 | % Jerk Min 62 | J_min = [-5,-5,-5]; 63 | 64 | % Jerk Max 65 | J_max = [5,5,5]; 66 | 67 | %% Problem Formulation 68 | 69 | A = sdpvar(3,K,N); 70 | V = sdpvar(3,K,N); 71 | P = sdpvar(3,K,N); 72 | J = sdpvar(3,K-1,N); 73 | 74 | Csts=[]; 75 | 76 | % Setting the Velocity and Position Vectors' Equations 77 | for i = 1:N 78 | for k = 1:K-1 79 | Csts = Csts + [V(:,k+1,i) == V(:,k,i) + h*A(:,k,i)]; 80 | Csts = Csts + [P(:,k+1,i) == P(:,k,i) + h*V(:,k,i) + (0.5*h*h)*A(:,k,i)]; 81 | end 82 | end 83 | 84 | % Setting up the Jerk Vector 85 | for i = 1:N 86 | Csts = Csts + [J(:,:,i) == (A(:,2:K,i) - A(:,1:K-1,i))/h]; 87 | end 88 | 89 | for l = 1:3 %3D 90 | for i = 1:N 91 | for k = 1:K 92 | % Velocity Bounds 93 | % Csts = Csts + [V(l,k,i) <= V_max(l)]; 94 | % Csts = Csts + [V(l,k,i) >= V_min(l)]; 95 | % Acceleration Bounds 96 | Csts = Csts + [A(l,k,i) <= A_max(l)]; 97 | Csts = Csts + [A(l,k,i) >= A_min(l)]; 98 | % Position Constraints 99 | Csts = Csts + [P(l,k,i) <= P_max(l)]; 100 | Csts = Csts + [P(l,k,i) >= P_min(l)]; 101 | if(K<10) 102 | % Jerk Constraints 103 | Csts = Csts + [J(l,k,i) <= J_max(l)]; 104 | Csts = Csts + [J(l,k,i) >= J_min(l)]; 105 | end 106 | end 107 | end 108 | end 109 | 110 | % Initialization Position Constraints 111 | Csts = Csts + [P(:,1,1) == OneInitPos']; 112 | Csts = Csts + [P(:,1,2) == TwoInitPos']; 113 | Csts = Csts + [P(:,1,3) == ThreeInitPos']; 114 | Csts = Csts + [P(:,1,4) == FourInitPos']; 115 | 116 | % Final Position Constraints 117 | Csts = Csts + [P(:,K,1) == OneFinalPos']; 118 | Csts = Csts + [P(:,K,2) == TwoFinalPos']; 119 | Csts = Csts + [P(:,K,3) == ThreeFinalPos']; 120 | Csts = Csts + [P(:,K,4) == FourFinalPos']; 121 | 122 | % Initial Velocity Constraints 123 | Csts = Csts + [V(:,1,1) == OneInitVel']; 124 | Csts = Csts + [V(:,1,2) == TwoInitVel']; 125 | Csts = Csts + [V(:,1,3) == ThreeInitVel']; 126 | Csts = Csts + [V(:,1,4) == FourInitVel']; 127 | 128 | % Final Velocity Constraints 129 | Csts = Csts + [V(:,K,1) == OneFinalVel']; 130 | Csts = Csts + [V(:,K,2) == TwoFinalVel']; 131 | Csts = Csts + [V(:,K,3) == ThreeFinalVel']; 132 | Csts = Csts + [V(:,K,4) == FourFinalVel']; 133 | 134 | obj = 0; 135 | for i = 1:N 136 | for k = 1:K 137 | obj = obj + (norm(A(:,k,i)+g'))^2; 138 | end 139 | end 140 | 141 | ops = sdpsettings('solver','sdpt3','verbose',1); 142 | optimize(Csts,obj,ops); 143 | pq = value(P); 144 | 145 | %% Inserting linearized non-convex constraint and non-convex constraint 146 | 147 | % done=false; 148 | % for f=1:3 149 | 150 | yalmip('clear') 151 | 152 | Csts_lin=Csts; 153 | Csts_nc=[]; 154 | 155 | for i=1:N-1 156 | for j=i+1:N 157 | for k=1:K 158 | alpha=pq(:,k,i)-pq(:,k,j); 159 | beta=norm(alpha); 160 | eta=alpha/beta; 161 | Csts_lin=Csts_lin+[beta+(eta')*((P(:,k,i)-P(:,k,j))-alpha)>=R] 162 | % Csts_nc=Csts_nc+[norm(P(:,k,i)-P(:,k,j))>=R] 163 | end 164 | end 165 | end 166 | 167 | 168 | optimize(Csts_lin+Csts_nc,obj,ops); 169 | pq = value(P) 170 | 171 | % done=true; 172 | 173 | % if (abs(obj-f0_prev)=R]; 56 | end 57 | end 58 | end 59 | %Solving with linear constraint 60 | [Csts_new,obj,ops,P,V,A,J] = convprob(Csts_lin,OneFinalPos,TwoFinalPos,ThreeFinalPos,FourFinalPos); 61 | pq = value(P) 62 | 63 | % Non convex constraint and convergence check 64 | 65 | done=true; 66 | for i=1:N-1 67 | for j=i+1:N 68 | for k=1:K 69 | if(norm(pq(:,k,i)-pq(:,k,j))<=R) 70 | done=false; 71 | k 72 | break 73 | end 74 | end 75 | if ~done 76 | break; 77 | end 78 | end 79 | if ~done 80 | break; 81 | end 82 | end 83 | over=true 84 | if abs(value(obj)-f0_prev)>eps 85 | over=false; 86 | end 87 | f0_prev=value(obj); 88 | 89 | end 90 | pq = value(P) 91 | Graphing1(pq); -------------------------------------------------------------------------------- /avoidance4.m: -------------------------------------------------------------------------------- 1 | %% Inserting linearized non-convex constraint and non-convex constraint 2 | clc; clear all;% close all; 3 | 4 | % OneFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 5 | % TwoFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 6 | % ThreeFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 7 | % FourFinalPos = [1000*rand(),1000*rand(),1000*rand()]; 8 | 9 | % OneFinalPos = [0,0,-1000]; 10 | % TwoFinalPos = [0,0,1000]; 11 | % ThreeFinalPos = [0,-1000,0]; 12 | % FourFinalPos = [0,1000,0]; 13 | % OneFinalPos = [1000,1000,1000]; 14 | % TwoFinalPos = [1000,1000,1000]; 15 | % ThreeFinalPos = [1000,1000,1000]; 16 | % FourFinalPos = [1000,1000,1000]; 17 | OneFinalPos = [5000,4000,3000]; 18 | TwoFinalPos = [5100,4000,3000]; 19 | ThreeFinalPos = [4900,4000,3000]; 20 | FourFinalPos = [5000,4000,2900]; 21 | 22 | 23 | CenterObs=[3500,3000,2000]; 24 | Rball=1000; 25 | 26 | 27 | N = 4; % Drones 28 | 29 | % Number of discrete time steps 30 | K = 50; % Steps 31 | 32 | % Discrete Time Step Resolution 33 | h = 1; % Seconds 34 | 35 | % Minimum Seperating Radius 36 | R = 10; 37 | 38 | % Gravity Vector 39 | g = [0,0,-9.81]; 40 | %Tolerance 41 | eps=1; 42 | 43 | [Csts,obj0,ops,P,V,A,J] = convprob([],OneFinalPos,TwoFinalPos,ThreeFinalPos,FourFinalPos); 44 | %% BIGGIE LOOP 45 | over=false; 46 | done=false; 47 | f0_prev=value(obj0); 48 | fff=0; 49 | 50 | while(~over || ~done || ~finish) 51 | fff=fff+1; 52 | pq = value(P); 53 | 54 | yalmip('clear')%%%%%%%%%%%% 55 | % A = sdpvar(3,K,N); 56 | % V = sdpvar(3,K,N); 57 | % P = sdpvar(3,K,N); 58 | % J = sdpvar(3,K-1,N); 59 | % Graphing(pq); 60 | Csts_lin=[]; 61 | 62 | for i=1:N-1 63 | for j=i+1:N 64 | for k=1:K 65 | alpha=pq(:,k,i)-pq(:,k,j); 66 | beta=norm(alpha); 67 | eta=alpha/beta; 68 | Csts_lin=Csts_lin+[(beta+(eta')*((P(:,k,i)-P(:,k,j))-alpha))>=R]; 69 | 70 | 71 | end 72 | 73 | end 74 | end 75 | 76 | for i=1:N 77 | for k=1:K 78 | alpha=pq(:,k,i)- CenterObs'; 79 | beta=norm(alpha); 80 | eta=alpha/beta; 81 | Csts_lin = Csts_lin+[(beta+(eta')*((P(:,k,i)-CenterObs')-alpha))>=(Rball+R)]; 82 | 83 | end 84 | 85 | 86 | end 87 | %Solving with linear constraint 88 | % [Csts_new,obj,ops,P,V,A,J] = convprob(Csts_lin,OneFinalPos,TwoFinalPos,ThreeFinalPos,FourFinalPos); 89 | % pq = value(P) 90 | 91 | 92 | [Csts_new,obj,ops,P,V,A,J] = convprob(Csts_lin,OneFinalPos,TwoFinalPos,ThreeFinalPos,FourFinalPos); 93 | pq = value(P) 94 | % Non convex constraint and convergence check 95 | 96 | done=true; 97 | for i=1:N-1 98 | for j=i+1:N 99 | for k=1:K 100 | if(norm(pq(:,k,i)-pq(:,k,j))<=R) 101 | done=false; 102 | break 103 | end 104 | end 105 | if ~done 106 | break; 107 | end 108 | end 109 | if ~done 110 | break; 111 | end 112 | end 113 | 114 | finish=true; 115 | for i=1:N 116 | for k=1:K 117 | if(norm(pq(:,k,i)-(CenterObs)')<=Rball+R) 118 | finish=false; 119 | break 120 | end 121 | end 122 | if ~finish 123 | break; 124 | end 125 | end 126 | 127 | over=true 128 | if abs(value(obj)-f0_prev)>eps 129 | over=false; 130 | end 131 | f0_prev=value(obj); 132 | 133 | end 134 | pq = value(P) 135 | Graphing1(pq); --------------------------------------------------------------------------------