Dynamic Soaring Problem

Problem Description

The following optimal control problem considers optimizing the motion of a hang glider in the presence of known wind force. The probem was originally described in Ref.[1] and the problem considered here is identical to that of Ref.[1]. The objective is to minimize the average wind gradient slope $\beta$, that is, minimize

$$\begin{equation} J = \beta \label{1} \tag{1} \end{equation}$$

subject to the hang glider dynamics

$$\begin{align} \dot{x}&=v \cos \gamma \sin \psi + W_x \label{2.1} \tag{2.1} \\ \dot{y}&=v \cos \gamma \cos \psi \label{2.2} \tag{2.2} \\ \dot{h}&=v \sin \gamma \label{2.3} \tag{2.3} \\ m \dot{v}&=-D - mg \sin \gamma - m \dot{W}_x \cos \gamma \sin \psi \label{2.4} \tag{2.4} \\ m v \dot{\gamma}&=L\cos\sigma-mg\cos\gamma+m\dot{W}_x\sin\gamma\sin\psi \label{2.5} \tag{2.5} \\ mv\cos\gamma\dot{\psi}&=L\sin\sigma-m\dot{W}_x\cos\psi \label{2.6} \tag{2.6} \end{align}$$

and the boundary conditions:

$$\begin{align} (x(0),y(0),h(0)) = (x(t_f),y(t_f),h(t_f))&=(0,0,0) \tag{3.1} \\ (v(t_f)-v(0),\gamma(t_f)-\gamma(0),\psi(t_f)+2\pi-\psi(0)) &=(0,0,0) \tag{3.2} \\ \end{align}$$

where $W_x$ is the wind component along the East direction, $m$ is the glider mass, $v$ is the air-relative speed, $\psi$ is the azimuth angle (measured clockwise from the North), $\gamma$ is the air-relative flight path angle, $(x, y)$ are (East, North) position, $h$ is the altitude, $\sigma$ is the glider bank angle, $L$ is the lift force, and $D$ is the drag force. The lift and drag forces are computed using a standard drag polar aerodynamic model

$$\begin{align} D &=qSC_L, \tag{4.1} \\ L &=qSC_D, \tag{4.2} \\ \end{align}$$

where $q = \rho v^2/2$ is the dynamic pressure, $S$ is the vehicle reference area, $C_D = C_{D0} + KC_L^2$ is the coefficient of drag, and $C_L$ is the coefficient of lift (where $0 ≤ C_L ≤ C_{L,max}$). The constants for this problem ar taken verbatim from Ref.[1] and are given as $C_{D0} = 0.00873$, $K = 0.045$, and $C_{L,max} = 1.5$. Finally, it is noted that $C_L$ and $\rho$ are the controls.

Note

Due to constraint 3.2 being applied to both the initial stage and the terminal, OPTIMake does not currently release modeling constraints between these stages to users. Here, we use a very technical approach: adding an inverse dynamics model to the same trajectory optimization problem, and modeling the original problem constraint 3.2 by constraining the endpoints of the forward and inverse problems.

Modeling

Python

# ------------------ Dynamic Soaring Vehicle Example ------------------ #
# This example is taken varbatim from the following reference:          #
# Zhao, Y. J., “Optimal Pattern of Glider Dynamic Soaring,” Optimal     #
#  Control Applications and Methods, Vol. 25, 2004, pp. 67–89           #
# --------------------------------------------------------------------- #

NN = 100
prob = multi_stage_problem('dynamic_soaring', NN + 1)

""" Provide Auxiliary Data for Problem """
S = 45.09703 # Vehicle Reference Area (mˆ2)
CD0 = 0.00873
K = 0.045
W0 = 0.0
rho = 0.002378 # Sea Level Atmospheric Density (kg/mˆ3)
mass = 5.6 # Vehicle Mass (kg)
g = 32.2
mu = 3.986e14


""" Boundary Conditions """
x0, y0, z0, v0, gamma0, psi0 = prob.parameters(['x0', 'y0', 'z0', 'v0', 'gamma0', 'psi0'],  stage_dependent=False)
xf, yf, zf, vf, gammaf, psif = prob.parameters(['xf', 'yf', 'zf', 'vf', 'gammaf', 'psif'],  stage_dependent=False)

""" Limits on Variables """
tfMin, tfMax = 1.0, 30.0
xMin, xMax = -1000.0, 1000.0
yMin, yMax = -1000.0, 1000.0
zMin, zMax = 0.0, 1000.0
vMin, vMax = 10.0, 350.0
gammaMin, gammaMax = -75.0 * pi / 180.0, 75.0 * pi / 180
psiMin, psiMax = -3.0 * pi, pi / 2.0
betaMin, betaMax = 0.005, 0.15
CLMin, CLMax = -0.5, 1.5
phiMin, phiMax = -75.0 * pi / 180.0, 75.0 * pi / 180.0
lMin, lMax = -2.0, 5.0

x = prob.variable('x', hard_lowerbound=xMin, hard_upperbound=xMax)
y = prob.variable('y', hard_lowerbound=yMin, hard_upperbound=yMax)
z = prob.variable('z', hard_lowerbound=zMin, hard_upperbound=zMax)
v = prob.variable('v', hard_lowerbound=vMin, hard_upperbound=vMax)
gamma = prob.variable('gamma', hard_lowerbound=gammaMin, hard_upperbound=gammaMax)
psi = prob.variable('psi', hard_lowerbound=psiMin, hard_upperbound=psiMax)


tf = prob.variable('tf', hard_lowerbound=tfMin, hard_upperbound=tfMax)
CL = prob.variable('CL', hard_lowerbound=CLMin, hard_upperbound=CLMax) # CL
phi = prob.variable('phi', hard_lowerbound=phiMin, hard_upperbound=phiMax) # phi
beta = prob.variable('beta', hard_lowerbound=betaMin, hard_upperbound=betaMax) # beta param
# inverse soaring variables
x_ = prob.variable('x_', hard_lowerbound=xMin, hard_upperbound=xMax)
y_ = prob.variable('y_', hard_lowerbound=yMin, hard_upperbound=yMax)
z_ = prob.variable('z_', hard_lowerbound=zMin, hard_upperbound=zMax)
v_ = prob.variable('v_', hard_lowerbound=vMin, hard_upperbound=vMax)
gamma_ = prob.variable('gamma_', hard_lowerbound=gammaMin, hard_upperbound=gammaMax)
psi_ = prob.variable('psi_', hard_lowerbound=psiMin, hard_upperbound=psiMax)
CL_ = prob.variable('CL_', hard_lowerbound=CLMin, hard_upperbound=CLMax) # CL
phi_ = prob.variable('phi_', hard_lowerbound=phiMin, hard_upperbound=phiMax) # phi

""" Compute the Aerodynamic Quantities """
singamma = sin(gamma)
cosgamma = cos(gamma)
sinpsi = sin(psi)
cospsi = cos(psi)
sinphi = sin(phi)
cosphi = cos(phi)
wx = beta * z + W0
DWxDt = beta * v * singamma
vcosgamma = v * cosgamma
DWxDtsinpsi = DWxDt * sinpsi


""" Evaluate Right-Hand Side of the Dynamics """
xdot = vcosgamma * sinpsi + wx
ydot = vcosgamma * cospsi
zdot = v * singamma
term1 = rho * S / 2 / mass
term2 = 1
term3 = g * term2
CLsq = CL ** 2
vsq = v ** 2
vdot = -term1*(CD0+K*CLsq)*vsq-(term3)*singamma-term2*DWxDtsinpsi*cosgamma
gammadot = term1*CL*v*cosphi-(term3)*cosgamma/v+term2*DWxDtsinpsi*singamma/v
psidot = (term1*CL*v*sinphi-term2*DWxDt*cospsi/v)/cosgamma
ngconstant = (0.5*rho*S/mass/g)
ng = ngconstant*CL*v**2

""" Compute the Aerodynamic Quantities """
singamma_ = sin(gamma_)
cosgamma_ = cos(gamma_)
sinpsi_ = sin(psi_)
cospsi_ = cos(psi_)
sinphi_ = sin(phi_)
cosphi_ = cos(phi_)
wx_ = beta * z_ + W0
DWxDt_ = beta * v_ * singamma_
vcosgamma_ = v_ * cosgamma_
DWxDtsinpsi_ = DWxDt_ * sinpsi_
""" Evaluate Right-Hand Side of the Inverse Dynamics """
xdot_ = vcosgamma_ * sinpsi_ + wx_
ydot_ = vcosgamma_ * cospsi_
zdot_ = v_ * singamma_
term1_ = rho * S / 2 / mass
term2_ = 1
term3_ = g * term2_
CLsq_ = CL_ ** 2
vsq_ = v_ ** 2
vdot_ = -term1_*(CD0+K*CLsq_)*vsq_-(term3_)*singamma_-term2_*DWxDtsinpsi_*cosgamma_
gammadot_ = term1_*CL_*v_*cosphi_-(term3_)*cosgamma_/v_+term2_*DWxDtsinpsi_*singamma_/v_
psidot_ = (term1_*CL_*v_*sinphi_-term2_*DWxDt_*cospsi_/v_)/cosgamma_
ng_ = ngconstant*CL_*v_**2
dynamics = [xdot, ydot, zdot, vdot, gammadot, psidot, 0, 0, -xdot_, -ydot_, -zdot_, -vdot_, -gammadot_, -psidot_]

ts = tf / NN
ode = differential_equation(
    state=[x, y, z, v, gamma, psi, tf, beta, x_, y_, z_, v_, gamma_, psi_],
    state_dot=dynamics, 
    stepsize=ts, 
    discretization_method='trapezoid')
prob.equality(ode)

obj = general_objective(7 * beta)
prob.end_objective(obj)

seq = general_equality([x - x0, y - y0, z - z0, x_ - x0, y_ - y0, z_ - z0, v - v_, gamma - gamma_, psi - psi_ - 2.0 * pi])
prob.start_equality(seq)

eeq = general_equality([x - xf, y - yf, z - zf, x_ - xf, y_ - yf, z_ - zf, v - v_, gamma - gamma_, psi - psi_ + 2.0 *pi])
prob.end_equality(eeq)
ineq = general_inequality(
    expr=[ng, ng, ng_, ng_],
    sign=['>=', '<=', '>=', '<='],
    bound=[lMin, lMax, lMin, lMax])
prob.inequality(ineq)

option = codegen_option()
option.common_subexpression_elimination = 'off'
option.hessian_approximation = 'exact'
option.platform = 'linux-x86_64-gcc'
codegen = code_generator()
codegen.codegen(prob, option)

Solution

The forward and inverse solution to dynamic soaring optimal control problem using OPTIMake is shown in the figures below.

---

[1]. Zhao, Y. J., “Optimal Pattern of Glider Dynamic Soaring,” Optimal Control Applications and Methods, Vol. 25, 2004, pp. 67–89