如何解决CVXPY再入车辆的最优控制
我试图确定再入期间对阿波罗型车辆的最佳控制。为简单起见,它仅限于二维(近距离和高度)和平坦的地球。控制是升程量。升力方向垂直于瞬时速度矢量。
已使用cvxpy解决静态问题,但此动态问题有所不同。我已经阅读了“凸凸编程”(DCP)资料,这对一些项目有所帮助,但是现在我受阻了。
我已经包含了代码和输出示例。该代码被严重注释。任何帮助,不胜感激。另外,这是我的第一篇文章,所以请原谅。
"""
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,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,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,N-1] == end_alt,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)
以下是示例输出
|-- 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,因为以下子表达式不是: |-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,因为以下子表达式不是: |---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]
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。