4.2.2. Multi-stage Launch Vehicle (Delta III) Ascent to Orbit#
2023 Devakumar Thammisetty
MPOPT is an open-source Multi-phase Optimal Control Problem (OCP) solver based on pseudo-spectral collocation with customized adaptive grid refinement techniques.
Download this notebook: multi_stage_launch_vehicle_ascent.ipynb
Install mpopt from pypi using the following. Disable after first usage
Import mpopt (Contains main solver modules)
[1]:
#!pip install mpopt
from mpopt import mp
import numpy as np
import casadi as ca
4.2.2.1. Defining OCP#
OCP: http://dx.doi.org/10.13140/RG.2.2.19519.79528
We first create an OCP object and then polulate the object with dynamics, path_constraints, terminal_constraints and objective (running_costs, terminal_costs)
[2]:
ocp = mp.OCP(n_states=7, n_controls=3, n_phases=4)
[3]:
# Initialize parameters
Re = 6378145.0 # m
omegaE = 7.29211585e-5
rho0 = 1.225
rhoH = 7200.0
Sa = 4 * np.pi
Cd = 0.5
muE = 3.986012e14
g0 = 9.80665
# Variable initialization
lat0 = 28.5 * np.pi / 180.0
r0 = np.array([Re * np.cos(lat0), 0.0, Re * np.sin(lat0)])
v0 = omegaE * np.array([-r0[1], r0[0], 0.0])
m0 = 301454.0
mf = 4164.0
mdrySrb = 19290.0 - 17010.0
mdryFirst = 104380.0 - 95550.0
mdrySecond = 19300.0 - 16820.0
x0 = np.array([r0[0], r0[1], r0[2], v0[0], v0[1], v0[2], m0])
[4]:
# Step-1 : Define dynamics
# Thrust(N) and mass flow rate(kg/s) in each stage
Thrust = [6 * 628500.0 + 1083100.0, 3 * 628500.0 + 1083100.0, 1083100.0, 110094.0]
mdot = [
(6 * 17010.0) / (75.2) + (95550.0) / (261.0),
(3 * 17010.0) / (75.2) + (95550.0) / (261.0),
(95550.0) / (261.0),
16820.0 / 700.0,
]
def dynamics(x, u, t, param=0, T=0.0, mdot=0.0):
r = x[:3]
v = x[3:6]
m = x[6]
r_mag = ca.sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2])
v_rel = ca.vertcat(v[0] + r[1] * omegaE, v[1] - r[0] * omegaE, v[2])
v_rel_mag = ca.sqrt(v_rel[0] * v_rel[0] + v_rel[1] * v_rel[1] + v_rel[2] * v_rel[2])
h = r_mag - Re
rho = rho0 * ca.exp(-h / rhoH)
D = -rho / (2 * m) * Sa * Cd * v_rel_mag * v_rel
g = -muE / (r_mag * r_mag * r_mag) * r
xdot = [
x[3],
x[4],
x[5],
T / m * u[0] + param * D[0] + g[0],
T / m * u[1] + param * D[1] + g[1],
T / m * u[2] + param * D[2] + g[2],
-mdot,
]
return xdot
def get_dynamics(param):
dynamics0 = lambda x, u, t: dynamics(
x, u, t, param=param, T=Thrust[0], mdot=mdot[0]
)
dynamics1 = lambda x, u, t: dynamics(
x, u, t, param=param, T=Thrust[1], mdot=mdot[1]
)
dynamics2 = lambda x, u, t: dynamics(
x, u, t, param=param, T=Thrust[2], mdot=mdot[2]
)
dynamics3 = lambda x, u, t: dynamics(
x, u, t, param=param, T=Thrust[3], mdot=mdot[3]
)
return [dynamics0, dynamics1, dynamics2, dynamics3]
ocp.dynamics = get_dynamics(0)
[5]:
# Step-2: Add path constraints
def path_constraints0(x, u, t):
return [
u[0] * u[0] + u[1] * u[1] + u[2] * u[2] - 1,
-u[0] * u[0] - u[1] * u[1] - u[2] * u[2] + 1,
-ca.sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]) / Re + 1,
]
ocp.path_constraints = [path_constraints0] * ocp.n_phases
[6]:
# Step-3: Add terminal cost and constraints
def terminal_cost3(xf, tf, x0, t0):
return -xf[-1] / m0
ocp.terminal_costs[3] = terminal_cost3
def terminal_constraints3(x, t, x0, t0):
# https://space.stackexchange.com/questions/1904/how-to-programmatically-calculate-orbital-elements-using-position-velocity-vecto
# http://control.asu.edu/Classes/MAE462/462Lecture06.pdf
h = ca.vertcat(
x[1] * x[5] - x[4] * x[2], x[3] * x[2] - x[0] * x[5], x[0] * x[4] - x[1] * x[3]
)
n = ca.vertcat(-h[1], h[0], 0)
r = ca.sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2])
e = ca.vertcat(
1 / muE * (x[4] * h[2] - x[5] * h[1]) - x[0] / r,
1 / muE * (x[5] * h[0] - x[3] * h[2]) - x[1] / r,
1 / muE * (x[3] * h[1] - x[4] * h[0]) - x[2] / r,
)
e_mag = ca.sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2])
h_sq = h[0] * h[0] + h[1] * h[1] + h[2] * h[2]
v_mag = ca.sqrt(x[3] * x[3] + x[4] * x[4] + x[5] * x[5])
a = -muE / (v_mag * v_mag - 2.0 * muE / r)
i = ca.acos(h[2] / ca.sqrt(h_sq))
n_mag = ca.sqrt(n[0] * n[0] + n[1] * n[1])
node_asc = ca.acos(n[0] / n_mag)
# if n[1] < -1e-12:
node_asc = 2 * np.pi - node_asc
argP = ca.acos((n[0] * e[0] + n[1] * e[1]) / (n_mag * e_mag))
# if e[2] < 0:
# argP = 2*np.pi - argP
a_req = 24361140.0
e_req = 0.7308
i_req = 28.5 * np.pi / 180.0
node_asc_req = 269.8 * np.pi / 180.0
argP_req = 130.5 * np.pi / 180.0
return [
(a - a_req) / (Re),
e_mag - e_req,
i - i_req,
node_asc - node_asc_req,
argP - argP_req,
]
ocp.terminal_constraints[3] = terminal_constraints3
Scale the variables
[7]:
ocp.scale_x = [
1 / Re,
1 / Re,
1 / Re,
1 / np.sqrt(muE / Re),
1 / np.sqrt(muE / Re),
1 / np.sqrt(muE / Re),
1 / m0,
]
ocp.scale_t = np.sqrt(muE / Re) / Re
Initial state and Final guess
[8]:
# Intial guess
# Initial guess estimation
def ae_to_rv(a, e, i, node, argP, th):
p = a * (1.0 - e * e)
r = p / (1.0 + e * np.cos(th))
r_vec = np.array([r * np.cos(th), r * np.sin(th), 0.0])
v_vec = np.sqrt(muE / p) * np.array([-np.sin(th), e + np.cos(th), 0.0])
cn, sn = np.cos(node), np.sin(node)
cp, sp = np.cos(argP), np.sin(argP)
ci, si = np.cos(i), np.sin(i)
R = np.array(
[
[cn * cp - sn * sp * ci, -cn * sp - sn * cp * ci, sn * si],
[sn * cp + cn * sp * ci, -sn * sp + cn * cp * ci, -cn * si],
[sp * si, cp * si, ci],
]
)
r_i = np.dot(R, r_vec)
v_i = np.dot(R, v_vec)
return r_i, v_i
# Target conditions
a_req = 24361140.0
e_req = 0.7308
i_req = 28.5 * np.pi / 180.0
node_asc_req = 269.8 * np.pi / 180.0
argP_req = 130.5 * np.pi / 180.0
th = 0.0
rf, vf = ae_to_rv(a_req, e_req, i_req, node_asc_req, argP_req, th)
Intial guess
[9]:
# Timings
t0, t1, t2, t3, t4 = 0.0, 75.2, 150.4, 261.0, 924.0
# Interpolate to get starting values for intermediate phases
xf = np.array([rf[0], rf[1], rf[2], vf[0], vf[1], vf[2], mf + mdrySecond])
x1 = x0 + (xf - x0) / (t4 - t0) * (t1 - t0)
x2 = x0 + (xf - x0) / (t4 - t0) * (t2 - t0)
x3 = x0 + (xf - x0) / (t4 - t0) * (t3 - t0)
# Update the state discontinuity values across phases
x0f = np.copy(x1)
x0f[-1] = x0[-1] - (6 * 17010.0 + 95550.0 / t3 * t1)
x1[-1] = x0f[-1] - 6 * mdrySrb
x1f = np.copy(x2)
x1f[-1] = x1[-1] - (3 * 17010.0 + 95550.0 / t3 * (t2 - t1))
x2[-1] = x1f[-1] - 3 * mdrySrb
x2f = np.copy(x3)
x2f[-1] = x2[-1] - (95550.0 / t3 * (t3 - t2))
x3[-1] = x2f[-1] - mdryFirst
# Step-8b: Initial guess for the states, controls and phase start and final
# times
ocp.x00 = np.array([x0, x1, x2, x3])
ocp.xf0 = np.array([x0f, x1f, x2f, xf])
ocp.u00 = np.array([[1, 0, 0], [1, 0, 0], [0, 1, 0], [0, 1, 0]])
ocp.uf0 = np.array([[0, 1, 0], [0, 1, 0], [0, 1, 0], [0, 1, 0]])
ocp.t00 = np.array([[t0], [t1], [t2], [t3]])
ocp.tf0 = np.array([[t1], [t2], [t3], [t4]])
Box constraints
[10]:
rmin, rmax = -2 * Re, 2 * Re
vmin, vmax = -10000.0, 10000.0
rvmin = [rmin, rmin, rmin, vmin, vmin, vmin]
rvmax = [rmax, rmax, rmax, vmax, vmax, vmax]
lbx0 = rvmin + [x0f[-1]]
lbx1 = rvmin + [x1f[-1]]
lbx2 = rvmin + [x2f[-1]]
lbx3 = rvmin + [xf[-1]]
ubx0 = rvmax + [x0[-1]]
ubx1 = rvmax + [x1[-1]]
ubx2 = rvmax + [x2[-1]]
ubx3 = rvmax + [x3[-1]]
ocp.lbx = np.array([lbx0, lbx1, lbx2, lbx3])
ocp.ubx = np.array([ubx0, ubx1, ubx2, ubx3])
# Bounds for control inputs
umin = [-1, -1, -1]
umax = [1, 1, 1]
ocp.lbu = np.array([umin] * ocp.n_phases)
ocp.ubu = np.array([umax] * ocp.n_phases)
# Bounds for phase start and final times
ocp.lbt0 = np.array([[t0], [t1], [t2], [t3]])
ocp.ubt0 = np.array([[t0], [t1], [t2], [t3]])
ocp.lbtf = np.array([[t1], [t2], [t3], [t4 - 100]])
ocp.ubtf = np.array([[t1], [t2], [t3], [t4 + 100]])
# Event constraint bounds on states : State continuity/disc.
lbe0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6 * mdrySrb]
lbe1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3 * mdrySrb]
lbe2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -mdryFirst]
ocp.lbe = np.array([lbe0, lbe1, lbe2])
ocp.ube = np.array([lbe0, lbe1, lbe2])
[11]:
ocp.validate()
4.2.2.2. Solve with atmospheric drag disabled#
[12]:
ocp.dynamics = get_dynamics(0)
mpo = mp.mpopt(ocp, 1, 11)
sol = mpo.solve()
******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
Ipopt is released as open source code under the Eclipse Public License (EPL).
For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************
Total number of variables............................: 474
variables with only lower bounds: 0
variables with lower and upper bounds: 474
variables with only upper bounds: 0
Total number of equality constraints.................: 374
Total number of inequality constraints...............: 276
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 132
inequality constraints with only upper bounds: 144
Number of Iterations....: 320
(scaled) (unscaled)
Objective...............: -2.7222444839176720e-02 -2.7222444839176720e-02
Dual infeasibility......: 1.5901114178071890e-09 1.5901114178071890e-09
Constraint violation....: 1.9999240413043351e-08 1.9999240413043351e-08
Complementarity.........: 2.5890735722630404e-09 2.5890735722630404e-09
Overall NLP error.......: 1.9999240413043351e-08 1.9999240413043351e-08
Number of objective function evaluations = 806
Number of objective gradient evaluations = 312
Number of equality constraint evaluations = 807
Number of inequality constraint evaluations = 807
Number of equality constraint Jacobian evaluations = 323
Number of inequality constraint Jacobian evaluations = 323
Number of Lagrangian Hessian evaluations = 321
Total CPU secs in IPOPT (w/o function evaluations) = 4.019
Total CPU secs in NLP function evaluations = 0.182
EXIT: Solved To Acceptable Level.
solver : t_proc (avg) t_wall (avg) n_eval
nlp_f | 2.18ms ( 2.71us) 2.15ms ( 2.66us) 806
nlp_g | 62.39ms ( 77.31us) 61.96ms ( 76.78us) 807
nlp_grad | 161.00us (161.00us) 159.72us (159.72us) 1
nlp_grad_f | 1.81ms ( 5.78us) 1.76ms ( 5.63us) 313
nlp_hess_l | 37.45ms (117.03us) 37.34ms (116.69us) 320
nlp_jac_g | 51.28ms (158.28us) 51.17ms (157.94us) 324
total | 4.24 s ( 4.24 s) 4.22 s ( 4.22 s) 1
Solve the problem with drag enabled now with revised initial guess
[13]:
ocp.dynamics = get_dynamics(1)
ocp.validate()
mpo._ocp = ocp
sol = mpo.solve(
sol, reinitialize_nlp=True, nlp_solver_options={"ipopt.acceptable_tol": 1e-6}
)
Total number of variables............................: 474
variables with only lower bounds: 0
variables with lower and upper bounds: 474
variables with only upper bounds: 0
Total number of equality constraints.................: 374
Total number of inequality constraints...............: 276
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 132
inequality constraints with only upper bounds: 144
Number of Iterations....: 119
(scaled) (unscaled)
Objective...............: -2.4977981075384650e-02 -2.4977981075384650e-02
Dual infeasibility......: 1.2976399535433416e-08 1.2976399535433416e-08
Constraint violation....: 6.8977004524795049e-09 6.8977004524795049e-09
Complementarity.........: 2.5143327708305730e-09 2.5143327708305730e-09
Overall NLP error.......: 6.8977004524795049e-09 1.2976399535433416e-08
Number of objective function evaluations = 262
Number of objective gradient evaluations = 120
Number of equality constraint evaluations = 262
Number of inequality constraint evaluations = 262
Number of equality constraint Jacobian evaluations = 120
Number of inequality constraint Jacobian evaluations = 120
Number of Lagrangian Hessian evaluations = 119
Total CPU secs in IPOPT (w/o function evaluations) = 1.941
Total CPU secs in NLP function evaluations = 0.106
EXIT: Optimal Solution Found.
solver : t_proc (avg) t_wall (avg) n_eval
nlp_f | 801.00us ( 3.06us) 779.38us ( 2.97us) 262
nlp_g | 26.59ms (101.48us) 26.88ms (102.59us) 262
nlp_grad | 183.00us (183.00us) 248.72us (248.72us) 1
nlp_grad_f | 786.00us ( 6.50us) 762.01us ( 6.30us) 121
nlp_hess_l | 36.99ms (310.85us) 37.56ms (315.60us) 119
nlp_jac_g | 31.55ms (260.72us) 32.24ms (266.46us) 121
total | 2.07 s ( 2.07 s) 2.10 s ( 2.10 s) 1
Retrive data from the solution
[14]:
post = mpo.process_results(sol, plot=False, scaling=False)
mp.post_process._INTERPOLATION_NODES_PER_SEG = 200
x, u, t, _ = post.get_data(interpolate=True)
print("Final time : ", round(t[-1], 4), "(MPOPT) vs 924.1413s(PSOPT)")
print("Final mass : ", round(-sol["f"].full()[0, 0] * m0, 4), "(MPOPT) vs 7529.7123kg (GPOPS-II)")
Final time : 924.1393 (MPOPT) vs 924.1413s(PSOPT)
Final mass : 7529.7123 (MPOPT) vs 7529.7123kg (GPOPS-II)
4.2.2.3. Plot results#
plot steering data (Thrust vector)
[15]:
figu, axsu = post.plot_u()
plot mass of the vehicle
[16]:
# Plot mass
figm, axsm = post.plot_single_variable(
x * 1e-3,
t,
[[-1]],
axis=0,
fig=None,
axs=None,
tics=["-"] * 15,
name="mass",
ylabel="Mass in tons",
)
Plot height and velocity profile
[17]:
mp.plt.rcParams["figure.figsize"] = (12,6)
# Compute and plot altitude, velocity
r = 1e-3 * (np.sqrt(x[:, 0] ** 2 + x[:, 1] ** 2 + x[:, 2] ** 2) - Re)
v = 1e-3 * np.sqrt(x[:, 3] ** 2 + x[:, 4] ** 2 + x[:, 5] ** 2)
y = np.column_stack((r, v))
fig, axs = post.plot_single_variable(y, t, [[0], [1]], axis=0)
x, u, t, _ = post.get_data(interpolate=False)
r = 1e-3 * (np.sqrt(x[:, 0] ** 2 + x[:, 1] ** 2 + x[:, 2] ** 2) - Re)
v = 1e-3 * np.sqrt(x[:, 3] ** 2 + x[:, 4] ** 2 + x[:, 5] ** 2)
y = np.column_stack((r, v))
fig, axs = post.plot_single_variable(
y, t, [[0], [1]], axis=0, fig=fig, axs=axs, tics=["."] * 15
)
axs[0].set(ylabel="Altitude, km", xlabel="Time, s")
axs[1].set(ylabel="Velocity, km/s", xlabel="Time, s")
# Plot mass at the collocation nodes
figm, axsm = post.plot_single_variable(
x * 1e-3, t, [[-1]], axis=0, fig=figm, axs=axsm, tics=["."] * 15
)
[ ]: