├── Compute_formula.m ├── Compute_matrixes.m ├── Compute_transition.m ├── Initialize_variable.m ├── README.md ├── Solve_convex_problem.m ├── Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time.pdf └── main.m /Compute_formula.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ceaser626/Successive-convexification/df3524f73bdd8f3afbb8874fd0a5663b0d78ac76/Compute_formula.m -------------------------------------------------------------------------------- /Compute_matrixes.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ceaser626/Successive-convexification/df3524f73bdd8f3afbb8874fd0a5663b0d78ac76/Compute_matrixes.m -------------------------------------------------------------------------------- /Compute_transition.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ceaser626/Successive-convexification/df3524f73bdd8f3afbb8874fd0a5663b0d78ac76/Compute_transition.m -------------------------------------------------------------------------------- /Initialize_variable.m: -------------------------------------------------------------------------------- 1 | function [x,u,sigma] =... 2 | Initialize_variable(k,K,m_wet,m_dry,r_I_init,v_I_init,v_I_fina,t_f_guess,g_I) 3 | 4 | alpha1 = (K-k+1)/K; 5 | alpha2 = (k-1)/K; 6 | 7 | m = alpha1*m_wet+alpha2*m_dry; 8 | r_I = alpha1*r_I_init; 9 | v_I = alpha1*v_I_init+alpha2*v_I_fina; 10 | q_B_I = [1,0,0,0]'; 11 | omiga_B = [0,0,0]'; 12 | 13 | x = [m,r_I',v_I',q_B_I',omiga_B']'; 14 | u = -m*g_I; 15 | sigma = t_f_guess; -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | The implementation of 《Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time》 2 | 3 | CVX must be installed in order to run the program :http://cvxr.com/ 4 | -------------------------------------------------------------------------------- /Solve_convex_problem.m: -------------------------------------------------------------------------------- 1 | function [X,U,sigma,V,Delta,Delta_sigma]=... 2 | Solve_convex_problem(X_old,U_old,sigma_old,A_Bar,B_Bar,C_Bar,Sigma_Bar,... 3 | Z_Bar,omiga_v,omiga_Delta,omiga_Delta_sigma,x_init,x_fina,K,m_dry,... 4 | theta_max,omiga_max,T_min,T_max,gamma_gs,delta_max) 5 | 6 | cvx_begin 7 | %------Define variables 8 | variable X(14,K); 9 | variable U(3,K); 10 | variable time; 11 | variable V(14*(K-1),1); 12 | variable Delta(1,K); 13 | variable Delta_sigma; 14 | 15 | %------Cost function 16 | minimize (time+omiga_v*norm(V,1)+omiga_Delta*norm(Delta,2)+omiga_Delta_sigma*Delta_sigma); 17 | 18 | subject to 19 | %------Boundary constraints 20 | X(1:7,1) == x_init(1:7,1); 21 | X(12:14,1) == x_init(12:14,1); 22 | X(2:14,K) == x_fina(2:14,1); 23 | 24 | %------Dynamics constraints 25 | for k = 1:(K-1) 26 | X(:,(k+1)) == A_Bar(:,(14*k-13):14*k)*X(:,k)+B_Bar(:,(3*k-2):3*k)*U(:,k)+C_Bar(:,(3*k-2):3*k)*U(:,(k+1))+Sigma_Bar(:,k)*time+Z_Bar(:,k)+V((14*k-13):14*k,1); 27 | end 28 | 29 | %------State constraints 30 | for k = 1:K 31 | X(1,k) >= m_dry; 32 | tan(gamma_gs)*norm(X(3:4,k),2) <= X(2,k); 33 | norm(X(10:11,k),2) <= sqrt((1-cos(theta_max))/2); 34 | norm(X(12:14,k),2) <= omiga_max; 35 | end 36 | 37 | %------Control constraints 38 | for k = 1:K 39 | T_min <= U_old(:,k)'*U(:,k)/norm(U_old(:,k),2); 40 | norm(U(:,k),2) <= T_max; 41 | cos(delta_max)*norm(U(:,k),2) <= U(1,k); 42 | end 43 | 44 | %------Trust regions 45 | Delta_X = X-X_old; 46 | Delta_U = U-U_old; 47 | Delta_time = time-sigma_old; 48 | for k = 1:K 49 | Delta_X(:,k)'*Delta_X(:,k)+Delta_U(:,k)'*Delta_U(:,k) <= Delta(:,k); 50 | end 51 | norm(Delta_time,1) <= Delta_sigma; 52 | 53 | cvx_end 54 | 55 | X = double(X); 56 | U = double(U); 57 | sigma = double(time); 58 | V = double(V); 59 | Delta = double(Delta); 60 | Delta_sigma = double(Delta_sigma); 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ceaser626/Successive-convexification/df3524f73bdd8f3afbb8874fd0a5663b0d78ac76/Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time.pdf -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %%Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time 2 | 3 | %-----------------------------Initialization------------------------------- 4 | clear all 5 | clc 6 | %------0.Set Parameters 7 | %---Model Parameters 8 | g_I = [-1,0,0]'; 9 | m_wet = 2; 10 | m_dry = 1; 11 | T_max = 5; 12 | T_min = 0.3; 13 | I_sp = 10; %ungiven in paper 14 | delta_max = 20/180*pi; 15 | theta_max = 90/180*pi; 16 | gamma_gs = 20/180*pi; 17 | omiga_max = 60/180*pi; 18 | J_B = 1e-2*eye(3); 19 | r_T_B = -1e-2*[1,0,0]'; 20 | %---Algorithm Parameters 21 | omiga_v = 1e5; 22 | omiga_Delta = 1e-3; 23 | omiga_Delta_sigma = 1e-1; 24 | Delta_tol = 1e-3; 25 | sigma_tol = 1e-3; %ungiven in paper 26 | v_tol = 1e-10; 27 | N_iter_max = 25; 28 | K = 50; 29 | r_I_init = [4,4,0]'; 30 | r_I_fina = [0,0,0]'; 31 | % v_I_init = [0,-1,-2]'; 32 | v_I_init = [0,-0.75,0.5]';%ungiven in paper 33 | v_I_fina = -1e-1*[1,0,0]'; 34 | q_B_I_init = [1,0,0,0]'; %ungiven in paper 35 | q_B_I_fina = [1,0,0,0]'; 36 | omiga_B_init = [0,0,0]'; 37 | omiga_B_fina = [0,0,0]'; %ungiven in paper 38 | t_f_guess = 3; %ungiven in paper 39 | %---Compute Parameters 40 | tao = 1/(K-1); 41 | alpha_m_dot = 1/I_sp/abs(g_I(1))/10; 42 | x_init = [m_wet,r_I_init',v_I_init',q_B_I_init',omiga_B_init']'; 43 | x_fina = [0,r_I_fina',v_I_fina',q_B_I_fina',omiga_B_fina']'; 44 | %---Define matrixes 45 | A_Bar = zeros(14,14*(K-1)); 46 | B_Bar = zeros(14,3*(K-1)); 47 | C_Bar = zeros(14,3*(K-1)); 48 | Sigma_Bar = zeros(14,(K-1)); 49 | Z_Bar = zeros(14,(K-1)); 50 | X = zeros(14,K); 51 | U = zeros(3,K); 52 | 53 | %------1.First Iteration 54 | for k = 1:K 55 | %---Call function 'Initialize_variable' 56 | [x,u,sigma] =... 57 | Initialize_variable(k,K,m_wet,m_dry,r_I_init,v_I_init,v_I_fina,t_f_guess,g_I); 58 | %---Store data 59 | X(:,k) = x; 60 | U(:,k) = u; 61 | end 62 | 63 | %------2.Second Iteration 64 | for k = 1:(K-1) 65 | %---Acquire x、u 66 | x = X(:,k); 67 | u = U(:,k); 68 | %---Call function 'Compute_matrixes' 69 | [A_bar,B_bar,C_bar,Sigma_bar,Z_bar]=... 70 | Compute_matrixes(x,u,sigma,alpha_m_dot,g_I,J_B,r_T_B,K,k); 71 | %---Store data 72 | A_Bar(:,(14*k-13):14*k) = A_bar; 73 | B_Bar(:,(3*k-2):3*k) = B_bar; 74 | C_Bar(:,(3*k-2):3*k) = C_bar; 75 | Sigma_Bar(:,k) = Sigma_bar; 76 | Z_Bar(:,k) = Z_bar; 77 | end 78 | 79 | 80 | %------------------------Convex Optimization Loop-------------------------- 81 | 82 | for iterate = 1:N_iter_max 83 | 84 | %------Display Loop Number 85 | display = ['Loop number ',num2str(iterate)]; 86 | disp(display) 87 | 88 | %------Solve Convexified Problem 89 | %---Call function 'Solve_convex_problem' 90 | disp('Solve convex problem') 91 | [X,U,sigma,V,Delta,Delta_sigma]=... 92 | Solve_convex_problem(X,U,sigma,A_Bar,B_Bar,C_Bar,Sigma_Bar,... 93 | Z_Bar,omiga_v,omiga_Delta,omiga_Delta_sigma,x_init,x_fina,K,m_dry,... 94 | theta_max,omiga_max,T_min,T_max,gamma_gs,delta_max); 95 | sigma_store(iterate,1) = sigma; 96 | 97 | %------Judge Iteration Conditions 98 | Diverse(1,iterate) = norm(Delta,2); 99 | Diverse(2,iterate) = norm(V,1); 100 | Diverse(3,iterate) = Delta_sigma; 101 | if (norm(Delta,2) <= Delta_tol)&&(norm(V,1) <= v_tol)&&(Delta_sigma <=sigma_tol) 102 | break 103 | end 104 | 105 | %------Update A_Bar、B_Bar、C_Bar、Sigma_Bar、Z_Bar 106 | for k = 1:(K-1) 107 | %---Acquire x、u 108 | x = X(:,k); 109 | u = U(:,k); 110 | %---Call function 'Compute_matrixes' 111 | [A_bar,B_bar,C_bar,Sigma_bar,Z_bar]=... 112 | Compute_matrixes(x,u,sigma,alpha_m_dot,g_I,J_B,r_T_B,K,k); 113 | %---Store data 114 | A_Bar(:,(14*k-13):14*k) = A_bar; 115 | B_Bar(:,(3*k-2):3*k) = B_bar; 116 | C_Bar(:,(3*k-2):3*k) = C_bar; 117 | Sigma_Bar(:,k) = Sigma_bar; 118 | Z_Bar(:,k) = Z_bar; 119 | end 120 | 121 | end 122 | 123 | 124 | %--------------------------------Plot Figure------------------------------- 125 | 126 | %------State plot 127 | figure 128 | %---Position--Time 129 | display_position(1:3,:) = X(2:4,:); 130 | subplot(3,3,1) 131 | plot(0:tao:1,display_position) 132 | grid on 133 | legend('x','y','z'); 134 | xlabel('Time') 135 | ylabel('Position') 136 | title('Position--Time') 137 | %---Velocity--Time 138 | display_velocity(1:3,:) = X(5:7,:); 139 | subplot(3,3,2) 140 | plot(0:tao:1,display_velocity) 141 | grid on 142 | legend('x','y','z'); 143 | xlabel('Time') 144 | ylabel('Velocity') 145 | title('Velocity--Time') 146 | %---Thrust--Time 147 | display_netthrust = U; 148 | subplot(3,3,3) 149 | plot(0:tao:1,display_netthrust) 150 | grid on 151 | legend('x','y','z'); 152 | xlabel('Time) 153 | ylabel('Net thrust') 154 | title('Thrust--Time') 155 | %---Mass--Time 156 | mass = X(1,:); 157 | subplot(3,3,4) 158 | plot(0:tao:1,mass) 159 | grid on 160 | xlabel('Time') 161 | ylabel('Mass') 162 | title('Mass--Time') 163 | %---Allvelocity--Time 164 | display_allvelocity = zeros(1,K); 165 | for i = 1:K 166 | display_allvelocity(i) = norm(X(5:7,i),2); 167 | end 168 | subplot(3,3,5) 169 | plot(0:tao:1,display_allvelocity) 170 | grid on 171 | xlabel('Time') 172 | ylabel('All velocity') 173 | title('Allvelocity--Time') 174 | %---Allthrust--Time 175 | display_allthrust = zeros(1,K); 176 | for i = 1:K 177 | display_allthrust(i) = norm(U(:,i),2); 178 | end 179 | subplot(3,3,6) 180 | hold on 181 | plot(0:tao:1,display_allthrust,'-') 182 | plot(0:tao:1,display_allthrust,'.') 183 | grid on 184 | xlabel('Time') 185 | ylabel('All thrust') 186 | title('Allthrust--Time') 187 | hold off 188 | %---omiga--Time 189 | display_omiga(1:3,:) = X(12:14,:); 190 | subplot(3,3,7) 191 | plot(0:tao:1,display_omiga) 192 | grid on 193 | legend('x','y','z'); 194 | xlabel('omiga_x') 195 | ylabel('omiga_y') 196 | zlabel('omiga_z') 197 | title('omiga--Time') 198 | %---Allomiga--Time 199 | display_allomiga = zeros(1,K); 200 | for i = 1:K 201 | display_allomiga(i) = norm(X(12:14,i),2)*180/pi; 202 | end 203 | subplot(3,3,8) 204 | plot(0:tao:1,display_allomiga) 205 | grid on 206 | xlabel('Time') 207 | ylabel('All omiga') 208 | title('Allomiga--Time') 209 | %---Tilt--Time 210 | display_tilt = zeros(1,K); 211 | for i = 1:K 212 | display_tilt(i) = acos(1-2*(norm(X(10:11,i),2))^2)/pi*180; 213 | end 214 | subplot(3,3,9) 215 | plot(0:tao:1,display_tilt) 216 | grid on 217 | xlabel('Time') 218 | ylabel('Tilt') 219 | title('Tilt--Time') 220 | 221 | %------Trajectory plot 222 | figure 223 | %---North--Up 224 | subplot(2,2,4) 225 | plot(X(4, :), X(2, :)) 226 | grid on 227 | xlabel('North-Position') 228 | ylabel('Up-Position') 229 | title('North--Up') 230 | %---East--Up 231 | subplot(2,2,3) 232 | plot(X(3, :), X(2, :)) 233 | grid on 234 | xlabel('East-Position') 235 | ylabel('Up-Position') 236 | title('East--Up') 237 | %---3D Trajectory 238 | subplot(2,2,2) 239 | plot3(X(3,:),X(4,:),X(2,:)) 240 | grid on 241 | xlabel('East-Position') 242 | ylabel('North-Position') 243 | zlabel('Up-Position') 244 | title('3D Trajectory') 245 | view(-142.5,30); 246 | % set(gca,'DataAspectRatio',[1 1 1]) 247 | %---East--North 248 | subplot(2,2,1) 249 | plot(X(3, :), X(4, :)) 250 | grid on 251 | xlabel('East-Position') 252 | ylabel('North-Position') 253 | title('East--North') 254 | 255 | %------Iterate plot 256 | figure 257 | %---Error state & control 258 | subplot(2,2,1) 259 | plot(1:iterate,Diverse(1,:)) 260 | grid on 261 | xlabel('Iteration times') 262 | ylabel('State & Control error') 263 | title('state & control error--iterate time') 264 | %---Error visual control 265 | subplot(2,2,2) 266 | plot(1:iterate,Diverse(2,:)) 267 | grid on 268 | xlabel('Iteration times') 269 | ylabel('Visual control') 270 | title('visual control--iterate time') 271 | %---Error time 272 | subplot(2,2,3) 273 | plot(1:iterate,Diverse(3,:)) 274 | grid on 275 | xlabel('Iteration times') 276 | ylabel('Flight time error') 277 | title('flight time error--iterate time') 278 | %---Flight time 279 | subplot(2,2,4) 280 | plot(1:iterate,sigma_store) 281 | grid on 282 | xlabel('Iteration times') 283 | ylabel('Flight time') 284 | title('flight time--iterate time') 285 | 286 | 287 | --------------------------------------------------------------------------------