I am trying to determine the optimal control of an Apollo-type vehicle during reentry. For simplicity, it is limited to two dimensions (down range and altitude) and a flat Earth. The control is the amount of lift. The lift direction is perpendicular to the instantaneous velocity vector.
Have used cvxpy for static problems but this dynamic one is different. I have gone through the Disciplined Convex Programming (DCP) material and that helped with a couple of items but now I am stymied.
I have included the code and a sample of the output. The code is heavily commented. Any help greatly appreciated. Also, this is my first post so please excuse the sloppiness.
"""
Created on Sun Aug 9 13:41:56 2020
This script determines optimal control for lifting, 2D Apollo-type
reentry. "y" is the downrange and "z" is the altitude (both in meters).
The Earth is assumed flat for simplicity.
The state vector "x" is 4x1, [y, ydot, z, zdot] (meters, meters/sec)
^
"z" |
|
|
|
|
|___________________> "y"
(0,0)
The control, u[i], is the amount of lift acceleration in the vertical (z)
direction. The target point is 3000 meters above (0,0) where the chutes would
deploy. The problem starts to the far left (negative y)
The objective is to minimize the norm of the control vector, i.e.
sqrt( sum(u[0]*u[0] + u[1]*u[1] + ... u[N-1]*u[N-1])). This seemed like
a cheap way to mimic the total integral of control effort. It is also
desired to minimize the number of g's experienced by the astronauts.
In this version W02, the density function is replaced to see if that was the
problem with the original script "entry.py".
"""
import numpy as np
import cvxpy as cv
import math
# Set constants
g = 9.8 # Earth grav constant (m/s^2)
mass = 5560 # mass of capsule (kg)
cd = 1.2 # Coefficient of drag (assumed constant)
area = 4.0*math.pi # capsule reference area (m^2)
L2D = 0.5 # lift to drag ratio (assumed constant)
maxg = 4 # maximum allowable g's
K = 0.5*cd*area/mass # constant for drag computation
T = 600 # total time in seconds
dt = 0.5 # Time interval (sec)
N = int(np.floor(T/dt)) # number of integration points
time = np.linspace(0, T, N) # time array
# Initial conditions (the final point is 3000 meters in altitude with near zero
# lateral speed). Assumes the initial flight path angle is -6.49
# degrees (velocity vector is below the local horizon)
init_alt = 122000 # (z) meters
init_alt_rate = -11032*math.sin(6.49/57.295) # (zdot) m/s
init_dr = -2601000 # meters from 0,0 (y)
init_dr_rate = 11032*math.cos(6.49/57.295) # (ydot) meters/sec
# Final conditions (same units as initial conditions)
end_alt = 3000 # meters
end_dr = 0.0
end_dr_rate = 20.0
#-------------------- Set up the problem----------------------------
# Set up cvxpy variables and parameters
x = cv.Variable((4,N)) # state variable (y, ydot, z, zdot)
u = cv.Variable(N) # control variable (lift, m/sec^2)
gs = cv.Variable(N) # number of g's experienced
x0 = cv.Parameter() # initial conditions
# Set initial conditions
x0 = np.array([init_dr, init_dr_rate, init_alt, init_alt_rate])
cons = [] # list of constraints
cons += [x[0,0]==x0[0], x[1,0]==x0[1], x[2,0]==x0[2], x[3,0]==x0[3] ]
cons += [u[0] == 0, gs[0]== 0 ]
# Loop over the N points in trajectory and integrate state
for i in range(1,N):
# Use simple Taylor series for integration of states
# compute the accelerations in x and z
velocity = np.array([x[1,i-1].value, x[3,i-1].value])
vm2 = cv.sum_squares(velocity)
vm = cv.sqrt(vm2)
# atmospheric density
h0 = x[2,i-1]/7000. # scale the altitude by 7000
ro = 1.3*cv.exp(-h0) #atmospheric density kg/m^3, h0 in meters
#D = 0.5*ro*vm2*(cd*area/mass) # drag acceleration (m/sec^2)
D = K*ro*vm2
L = D*L2D # maximum lift acceleration possible (m/sec^2)
# trig functions are in general neither concave or convex so compute
# sin and cosine from the state elements
sinFpa = x[3,i-1].value / vm
cosFpa = x[1,i-1].value / vm
# rates of change of state vector elements
xd0 = x[1,i-1]
xd1 = -D*cosFpa - u[i-1]*sinFpa
xd2 = x[3,i-1]
xd3 = u[i-1]*cosFpa - D*sinFpa - g
# single step state integration and add to constraint list.
cons += [x[0,i] == x[0,i-1] + x[1,i-1]*dt + 0.5*xd1*dt**2] # y[i]
cons += [x[1,i] == x[1,i-1] + xd1*dt] # ydot[i]
cons += [x[2,i] == x[2,i-1] + x[2,i-1]*dt + 0.5*xd3*dt**2 ] # z[i]
cons += [x[3,i] == x[3,i-1] + xd3*dt] # zdot[i]
cons += [gs[i] == cv.sqrt(xd1**2 + xd3**2)/9.8 ] # number of g's
cons += [gs[i] <= maxg] # don't exceed mag g level (crew saftey)
cons += [u[i] <= L] # can't produce more lift than the maximum
cons += [x[0,N-1] == end_dr , cv.abs(x[1,N-1]) <= end_dr_rate,
x[2,N-1] == end_alt, x[3,N-1] <= 100]
# Set up CVXPY problem
cost = cv.norm(u)
objective = cv.Minimize(cost)
problem=cv.Problem(objective, cons)
obj_opt=problem.solve(solver=cv.ECOS,verbose=False,feastol=5e-20)
The following is a sample output
|-- 0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2))
var1752319[3, 1199] == var1752319[3, 1198] + (var1752320[1198] * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) + -0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) + -9.8) * 0.5 , because the following subexpressions are not: |-- 0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) var1752321[1199] == power(power(-0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) + -var1752320[1198] * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)), 2) + power(var1752320[1198] * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) + -0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) + -9.8, 2), 1/2) / 9.8 , because the following subexpressions are not: |-- -0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) |-- 0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * (nan / power(quad_over_lin([nan nan], 1.0), 1/2)) var1752320[1199] <= 0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * 0.5 , because the following subexpressions are not: |-- var1752320[1199] <= 0.0013560831598229323 * 1.3 * exp(-var1752319[2, 1198] / 7000.0) * quad_over_lin([nan nan], 1.0) * 0.5
