[PYTHON] Basics of orbit generation (solving nonlinear optimal control problems using Scipy's optimize)
Explanation of how to find modern numerical solutions of orbit generation, orbit planning, and optimal control problems.
[Addition March 2017] I released it as a python library as an extension of the content written here. Please note that the theory implemented below is different from the Legendre-Gauss-Lobatto pseudo-spectral method, and the content written on the current page is different from the Legendre-Gauss-Gaussian pseudo-spectral method. https://github.com/istellartech/OpenGoddard https://istellartech.github.io/OpenGoddard/
How to use OpenGoddard 1 How to use OpenGoddard 2 How to use OpenGoddard 3 How to use OpenGoddard 4
[Addition here]
Personally, I intend to be the basis for rocket orbit planning and orbit generation.
Orbit generation is, for example, to find the angle for flying to the target location by minimizing the fuel of the projectile. Mathematically, it can be defined as an optimal control problem. In rocket textbooks, a method called the variational method is used in the indirect method. This is to convert the optimal control problem into a Hamiltonian boundary value problem and solve it.
The variational method is guaranteed to be optimal, but it is not versatile because the expression transformations differ from problem to problem and define variables that are not intuitive. A solution for smart people who can play with mathematical formulas. The variational method seems to be used even in modern times, but now that abundant computational resources can be used, a method called the direct method is also practical.
The direct method discretizes the original continuous-time nonlinear optimal control problem, converts it into a nonlinear programming problem, and solves it. Among the direct methods, there are two methods: the shooting method, which discretizes only control variables, and the Direct Collocation method, which discretizes control variables and state variables. Furthermore, the Direct Collocation method also adopts a method called the pseudo-spectral method, which does not divide the discretization monotonically, so that the solution converges relatively stably even in a strong nonlinear problem. The mathematical basis of the pseudo-spectral method is Gaussian quadrature, and it is difficult to understand without mathematical education (I gave up).
Here, the nonlinear continuous-time optimal control problem for orbit generation is converted into a nonlinear optimization problem using the pseudo-spectral method, which is a kind of Direct Collocation method, especially the Legendre-Gauss pseudo-spectral method, and the Scipy optimization module I solved it numerically using the SLSQP solver inside.
By the way, I think that the reason why aerospace optimization problems are usually "constrained nonlinear minimization problems" is difficult to understand. With no constraints, linear or quadratic forms, there are tons of useful solvers rolling around, but with "constrained non-linearity" it quickly becomes difficult. Sequential quadratic programming (SQP) seems to be effective as a solver in modern times.
Rather than simply using a general-purpose solver for nonlinear optimization problems, it seems that there is a craftsmanship part that can be adapted to large-scale problems and converges faster by devising various ways. There are examples of using solvers such as IPOPT and SNOPT, but here, the solver of scipy.optimize is used because the source code is simple and priority is given.
While writing this, I tried to compare the shooting methods, but it didn't converge at all, only Tsurai. The shooting method is difficult to use. The idea is simple, but it is better not to adopt it.
Example
As an example, solve a simple one.
A rocket that flies over a two-dimensional plane and receives neither gravity nor air force creates an orbit with the minimum fuel (minimum flight time) in order to obtain the target altitude and lateral velocity. However, there is no increase or decrease in mass, and the acceleration is constant. It's a bold simplification. At this time, the initial value and the termination condition are determined, and the termination time is set as the evaluation function (the fastest trajectory). The optimal control problem has the following evaluation function and constraints. (Actually, it is quite important whether it can be described by mathematical formulas as the equation of motion and the optimal control problem)
\begin{aligned}
{\rm minimize} & : t_f \\
{\rm subject \ to} & : u(0) = v(0) = x(0) = y(0) = 0 \\
& : u(t_f) = 1,\ v(t_f) = 0, \ y(t_f) = 1 \\
& : -\frac{\pi}{2} \leq \beta \leq \frac{\pi}{2}
\end{aligned}
The equation of motion (equation of state) is as follows
\begin{aligned}
\dot{u} & = a \cos \beta \\
\dot{v} & = a \sin \beta \\
\dot{x} & = u \\
\dot{y} & = v
\end{aligned}
The source code is below. 5 points
--There is a function to find the discretized points (Legandre-Gauss node, Gauss point), weights, and differential matrix --The variables to be optimized are arranged in a column with the state variables and control variables discretized. --Constrains for the matching of numerical integration of the Direct Collocation method are included. --In the numerical integration part, there is a constraint condition whether the result of trapezoidal integration from the beginning to the end so that the system is closed is (last value)-(first value). --Normalize the time direction to (-1,1) in the optimization calculation? Since it is calculated afterwards, it will be returned to the original time axis later.
# -*- coding: utf-8 -*-
#Rocket optimal control problem, extremely simplified problem
#Convert the optimal control problem to a nonlinear programming problem and search for a solution using Scipy's sequential least squares method (SLSQP).
#Among the direct methods, the method of changing the problem is the Direct Collocation method, and further
# Legendre-Gauss's pseudo-spectral method is used.
#
# Copyright (c) 2016 Takahiro Inagawa
# Released under the MIT license
import sys
reload(sys)
import platform
#Change the default character code.
sys.setdefaultencoding('utf-8')
import numpy as np
from scipy.integrate import odeint
from scipy.optimize import minimize
import matplotlib as mpl
import matplotlib.pyplot as plt
import matplotlib.font_manager
from matplotlib.font_manager import FontProperties
from matplotlib.backends.backend_pdf import PdfPages
import time
if 'Windows' == platform.system():
font_path = r'C:\WINDOWS\Fonts\MSGothic.ttf'
if 'Darwin' == platform.system(): # for Mac
font_path = '/Library/Fonts/Osaka.ttf'
font_prop = FontProperties(fname=font_path)
mpl.rcParams['font.family'] = font_prop.get_name()
plt.close('all')
plt.style.use('ggplot')
mpl.rcParams['axes.grid'] = True
mpl.rcParams['savefig.dpi'] = 300 #default dpi=Change from 100
# plt.ion()
def make_node_derivative_matrix(n):
# Legendre-From the Gauss pseudospectral method, as a collocation point
# Legendre-Gauss node and weight, Gauss differential matrix(Spectral differential matrix)Calculate
# tau: [-1,1]Legendre-Gauss node(collocation point)
# w:Gaussian numerical integration formula weights(n real numbers)
# D:Gauss differential matrix
beta = np.array([0.5 / np.sqrt(1-(2*i)**(-2)) for i in range(1,n)])
T = np.diag(beta,1) + np.diag(beta,-1) #Jacobian
D_, V = np.linalg.eig(T) #Eigenvalue decomposition
tau = np.sort(D_) # Legendre-Gauss node (Gauss point)
i = np.argsort(D_)
w = 2 * (V[0,i]**2) #Quadrature weight
tau = np.hstack((-1,tau,1)) # -1,1(0,N+1)Add
D = np.zeros([n,n+1]) #Gauss differential matrix(If the Gaussian differential matrix is applied, the derivative of that point will be obtained.)
for k in range(1,n+1):
for l in range(0,n+1):
if k==l:
D[k-1,l] = 0
for m in range(0,n+1):
if m != k:
D[k-1,l] += 1.0/(tau[k]-tau[m])
else:
D[k-1,l] = 1.0/(tau[l]-tau[k])
for m in range(0,n+1):
if m != k and m != l:
D[k-1,l] *= (tau[k]-tau[m])/(tau[l]-tau[m])
return tau, w, D
def dynamics(p, div): #Equation of motion
u = p[1:div[0]-1]
v = p[div[0]+1:div[1]-1]
x = p[div[1]+1:div[2]-1]
y = p[div[2]+1:div[3]-1]
beta = p[div[3]+1:div[4]-1]
dx = np.zeros(0)
a = 1.0 # acceleration
dx0 = a * np.cos(beta)
dx1 = a * np.sin(beta)
dx2 = u
dx3 = v
dx = np.hstack((dx0, dx1, dx2, dx3))
return dx
def evaluation(p, div): #Evaluation function
return p[-1]
def equality(p, D, w, t0, div, n): #Equality constraint
u = p[0:div[0]-1]
v = p[div[0]:div[1]-1]
x = p[div[1]:div[2]-1]
y = p[div[2]:div[3]-1]
beta = p[div[3]:div[4]-1]
dx = dynamics(p, div)
result = np.zeros(0)
derivative = np.hstack((D.dot(u), D.dot(v), D.dot(x), D.dot(y)))
result = np.hstack((result, derivative - (p[-1]-t0) / 2.0 * dx))
result = np.hstack((result, p[div[0]-1] - p[0] - np.sum(D.dot(u)*w)))
result = np.hstack((result, p[div[1]-1] - p[div[0]] - np.sum(D.dot(v)*w)))
result = np.hstack((result, p[div[2]-1] - p[div[1]] - np.sum(D.dot(x)*w)))
result = np.hstack((result, p[div[3]-1] - p[div[2]] - np.sum(D.dot(y)*w)))
result = np.hstack((result, u[0] - 0.0))
# result = np.hstack((result, p[div[0]-1] - 1))
result = np.hstack((result, v[0] - 0.0))
result = np.hstack((result, p[div[1]-1] - 0))
result = np.hstack((result, x[0] - 0.0))
result = np.hstack((result, y[0] - 0.0))
result = np.hstack((result, p[div[3]-1] - 1))
return result
def inequality(p, D, w, t0, div, n): #Inequality constraints
u = p[0:div[0]-1]
v = p[div[0]:div[1]-1]
x = p[div[1]:div[2]-1]
y = p[div[2]:div[3]-1]
beta = p[div[3]:div[4]-1]
dx = dynamics(p, div)
result = np.zeros(0)
result = np.hstack((result, beta + np.pi/2))
result = np.hstack((result, np.pi/2 - beta))
result = np.hstack((result, p[div[0]-1] - 1))
return result
if __name__ == '__main__':
t0 = 0 #Start time
n = 20 #Discretized mesh (Legendre)-Number of Gauss nodes)
num_p = 5 #Total types of state variables and control variables
iterator = 10 #Optimization iterations default:1
tau, w, D = make_node_derivative_matrix(n)
div = [i*(n+2) for i in range(1,num_p+1)] #Index at the transition of variables
num_variables = div[-1] + 1 #Total number of state variables, control variables, and static variables
#Upper and lower limit vectors
# vec_lower_limit = np.zeros(num_variables)
# vec_upper_limit = np.ones(num_variables) * 100
# vec_upper_limit[-1] = 0.5
p = np.ones(num_variables) * 0.5 #Initial values of variables to be optimized
cons = ({'type': 'eq',
'fun': lambda p, D, w, t0, div, n: equality(p, D, w, t0, div, n),
'args': (D, w, t0, div, n,)},
{'type': 'ineq',
'fun': lambda p, D, w, t0, div, n: inequality(p, D, w, t0, div, n),
'args': (D, w, t0, div, n,)})
for i in range(iterator):
opt = minimize(evaluation, p, args=(div,), constraints=cons,
method='SLSQP')
p = opt.x
print(u"iteration: %d" % (i+1))
#Decompose the resulting variable
u = p[0:div[0]]
v = p[div[0]:div[1]]
x = p[div[1]:div[2]]
y = p[div[2]:div[3]]
beta = p[div[3]:div[4]]
tf = p[-1]
time = (tf-t0)/2.0 * tau + (tf+t0)/2.0
#Result PLOT
plt.figure()
plt.subplot(3,1,1)
plt.plot(time,u,label=u"vel x")
plt.plot(time,v,label=u"vel y")
plt.legend(loc="best")
plt.ylabel(u"velocity [m/s]")
plt.subplot(3,1,2)
plt.plot(time,x,label=u"x")
plt.plot(time,y,label=u"y")
plt.legend(loc="best")
plt.ylabel(u"position [m]")
plt.subplot(3,1,3)
plt.plot(time,beta,label=u"beta")
plt.legend(loc="best")
plt.ylabel(u"angle [rad]")
plt.xlabel(u"time [s]")
plt.show()
References
- Benson, David A., et al. "Direct trajectory optimization and costate estimation via an orthogonal collocation method." Journal of Guidance, Control, and Dynamics 29.6 (2006): 1435-1440.
https://goo.gl/BdLsvH （PDF） ↑ It is recommended to read the paper of the original material of the method.
Kanichiro Kato "Engineering Optimal Control-Approach to Nonlinearity" 1988 Since it uses the variational method, it is not directly related, but it seems to be famous as a book dealing with optimal control problems.
Takeshi Tsuchiya, and Shinji Suzuki. "Study on Optimal Control Problem Solving Using Mathematical Programming (Part 2) Proposal of Block Diagonal Cyan Method." Journal of Japan Aerospace Society 46.536 (1998): 497-503. https://www.jstage.jst.go.jp/article/jjsass1969/46/536/46_536_497/_article/-char/ja/
Nobuhiro Yokoyama, Shinji Suzuki, and Takeshi Tsuchiya. "Study on optimization of ascending orbit of spaceplane to ISS." JOURNAL OF THE JAPAN SOCIETY FOR AERONAUTICAL AND SPACE SCIENCES 49.570 (2001): 208-213. https://www.jstage.jst.go.jp/article/jjsass/49/570/49_570_208/_article/-char/ja/
↑ There are several papers by Suzuki and Tsuchiya Labs of the University of Tokyo, but they are very helpful.
Recommended Posts