Оптимальное управление траекторией возвращения космического челнока

using JuMP
import Interpolations
import Ipopt

# Глобальные переменные
const w = 203000.0  # вес (фунтов)
const g₀ = 32.174    # ускорение (футов/с^2)
const m = w / g₀    # масса (слагов)

# Аэродинамические и атмосферные силы, действующие на летательный аппарат
const ρ₀ = 0.002378
const hᵣ = 23800.0
const Rₑ = 20902900.0
const μ = 0.14076539e17
const S = 2690.0
const a₀ = -0.20704
const a₁ = 0.029244
const b₀ = 0.07854
const b₁ = -0.61592e-2
const b₂ = 0.621408e-3
const c₀ = 1.0672181
const c₁ = -0.19213774e-1
const c₂ = 0.21286289e-3
const c₃ = -0.10117249e-5

# Начальные условия
const h_s = 2.6          # высота (футов) / 1e5
const ϕ_s = deg2rad(0)   # широта (рад)
const θ_s = deg2rad(0)   # долгота (рад)
const v_s = 2.56         # скорость (футов/с) / 1e4
const γ_s = deg2rad(-1)  # угол наклона траектории (рад)
const ψ_s = deg2rad(90)  # азимут (рад)
const α_s = deg2rad(0)   # угол атаки (рад)
const β_s = deg2rad(0)   # угол крена (рад)
const t_s = 1.00         # временной шаг (с)

# Конечные условия, так называемое управление энергией на участке предпосадочного планирования
const h_t = 0.8          # высота (футов) / 1e5
const v_t = 0.25         # скорость (футов/с) / 1e4
const γ_t = deg2rad(-5)  # угол наклона траектории (рад)

# Количество используемых точек (узлов) сетки
const n = 503

# Схема интегрирования, используемая для динамики
const integration_rule = "rectangular"

# Раскомментируйте строки ниже, чтобы передать пользовательские параметры в решатель.
user_options = (
# "mu_strategy" => "monotone",
# "linear_solver" => "ma27",
)

# Создаем модель JuMP, используя решатель Ipopt
model = Model(optimizer_with_attributes(Ipopt.Optimizer, user_options...))

@variables(model, begin
    0 ≤ scaled_h[1:n]                # высота (футов) / 1e5
    ϕ[1:n]                # широта (рад)
    deg2rad(-89) ≤ θ[1:n] ≤ deg2rad(89)  # долгота (рад)
    1e-4 ≤ scaled_v[1:n]                # скорость (футов/с) / 1e4
    deg2rad(-89) ≤ γ[1:n] ≤ deg2rad(89)  # угол наклона траектории (рад)
    ψ[1:n]                # азимут (рад)
    deg2rad(-90) ≤ α[1:n] ≤ deg2rad(90)  # угол атаки (рад)
    deg2rad(-89) ≤ β[1:n] ≤ deg2rad(1)  # угол крена (рад)
    #        3.5 ≤       Δt[1:n] ≤ 4.5          # временной шаг (с)
    Δt[1:n] == 4.0         # временной шаг (с)
end);

# Фиксируем начальные условия
fix(scaled_h[1], h_s; force = true)
fix(ϕ[1], ϕ_s; force = true)
fix(θ[1], θ_s; force = true)
fix(scaled_v[1], v_s; force = true)
fix(γ[1], γ_s; force = true)
fix(ψ[1], ψ_s; force = true)

# Фиксируем конечные условия
fix(scaled_h[n], h_t; force = true)
fix(scaled_v[n], v_t; force = true)
fix(γ[n], γ_t; force = true)

# Начальное предположение: линейная интерполяция между граничными условиями
x_s = [h_s, ϕ_s, θ_s, v_s, γ_s, ψ_s, α_s, β_s, t_s]
x_t = [h_t, ϕ_s, θ_s, v_t, γ_t, ψ_s, α_s, β_s, t_s]
interp_linear = Interpolations.LinearInterpolation([1, n], [x_s, x_t])
initial_guess = mapreduce(transpose, vcat, interp_linear.(1:n))
set_start_value.(all_variables(model), vec(initial_guess))

# Функции для восстановления `h` и `v` до истинного масштаба
@expression(model, h[j = 1:n], scaled_h[j] * 1e5)
@expression(model, v[j = 1:n], scaled_v[j] * 1e4)

# Вспомогательные функции
@expression(model, c_L[j = 1:n], a₀ + a₁ * rad2deg(α[j]))
@expression(model, c_D[j = 1:n], b₀ + b₁ * rad2deg(α[j]) + b₂ * rad2deg(α[j])^2)
@expression(model, ρ[j = 1:n], ρ₀ * exp(-h[j] / hᵣ))
@expression(model, D[j = 1:n], 0.5 * c_D[j] * S * ρ[j] * v[j]^2)
@expression(model, L[j = 1:n], 0.5 * c_L[j] * S * ρ[j] * v[j]^2)
@expression(model, r[j = 1:n], Rₑ + h[j])
@expression(model, g[j = 1:n], μ / r[j]^2)

# Движение летательного аппарата как дифференциально-алгебраическая система уравнений (ДАУ)
@expression(model, δh[j = 1:n], v[j] * sin(γ[j]))
@expression(
    model,
    δϕ[j = 1:n],
    (v[j] / r[j]) * cos(γ[j]) * sin(ψ[j]) / cos(θ[j])
)
@expression(model, δθ[j = 1:n], (v[j] / r[j]) * cos(γ[j]) * cos(ψ[j]))
@expression(model, δv[j = 1:n], -(D[j] / m) - g[j] * sin(γ[j]))
@expression(
    model,
    δγ[j = 1:n],
    (L[j] / (m * v[j])) * cos(β[j]) +
    cos(γ[j]) * ((v[j] / r[j]) - (g[j] / v[j]))
)
@expression(
    model,
    δψ[j = 1:n],
    (1 / (m * v[j] * cos(γ[j]))) * L[j] * sin(β[j]) +
    (v[j] / (r[j] * cos(θ[j]))) * cos(γ[j]) * sin(ψ[j]) * sin(θ[j])
)

# Динамика системы
for j in 2:n
    i = j - 1  # индекс предыдущего узла

    if integration_rule == "rectangular"
        # Интегрирование по формуле прямоугольников
        @constraint(model, h[j] == h[i] + Δt[i] * δh[i])
        @constraint(model, ϕ[j] == ϕ[i] + Δt[i] * δϕ[i])
        @constraint(model, θ[j] == θ[i] + Δt[i] * δθ[i])
        @constraint(model, v[j] == v[i] + Δt[i] * δv[i])
        @constraint(model, γ[j] == γ[i] + Δt[i] * δγ[i])
        @constraint(model, ψ[j] == ψ[i] + Δt[i] * δψ[i])
    elseif integration_rule == "trapezoidal"
        # Интегрирование методом трапеций
        @constraint(model, h[j] == h[i] + 0.5 * Δt[i] * (δh[j] + δh[i]))
        @constraint(model, ϕ[j] == ϕ[i] + 0.5 * Δt[i] * (δϕ[j] + δϕ[i]))
        @constraint(model, θ[j] == θ[i] + 0.5 * Δt[i] * (δθ[j] + δθ[i]))
        @constraint(model, v[j] == v[i] + 0.5 * Δt[i] * (δv[j] + δv[i]))
        @constraint(model, γ[j] == γ[i] + 0.5 * Δt[i] * (δγ[j] + δγ[i]))
        @constraint(model, ψ[j] == ψ[i] + 0.5 * Δt[i] * (δψ[j] + δψ[i]))
    else
        @error "Unexpected integration rule '$(integration_rule)'"
    end
end

# Цель: максимизировать курсовой диапазон
@objective(model, Max, θ[n])

set_silent(model)  # Скрываем подробные выходные данные решателя
optimize!(model)  # Решение для управляющей переменной и переменных состояния
@assert is_solved_and_feasible(model)

# Вывод получившегося в решении курсового диапазона
println(
    "Final latitude θ = ",
    round(objective_value(model) |> rad2deg; digits = 2),
    "°",
)

Final latitude θ = 34.18°

using Plots
ts = cumsum([0; value.(Δt)])[1:end-1]
plt_altitude = plot(
    ts,
    value.(scaled_h);
    legend = nothing,
    title = "Altitude (100,000 ft)",
)
plt_longitude =
    plot(ts, rad2deg.(value.(ϕ)); legend = nothing, title = "Longitude (deg)")
plt_latitude =
    plot(ts, rad2deg.(value.(θ)); legend = nothing, title = "Latitude (deg)")
plt_velocity = plot(
    ts,
    value.(scaled_v);
    legend = nothing,
    title = "Velocity (1000 ft/sec)",
)
plt_flight_path =
    plot(ts, rad2deg.(value.(γ)); legend = nothing, title = "Flight Path (deg)")
plt_azimuth =
    plot(ts, rad2deg.(value.(ψ)); legend = nothing, title = "Azimuth (deg)")

plot(
    plt_altitude,
    plt_velocity,
    plt_longitude,
    plt_flight_path,
    plt_latitude,
    plt_azimuth;
    layout = grid(3, 2),
    linewidth = 2,
    size = (700, 700),
)

function q(h, v, a)
    ρ(h) = ρ₀ * exp(-h / hᵣ)
    qᵣ(h, v) = 17700 * √ρ(h) * (0.0001 * v)^3.07
    qₐ(a) = c₀ + c₁ * rad2deg(a) + c₂ * rad2deg(a)^2 + c₃ * rad2deg(a)^3
    # Аэродинамический нагрев на передней кромке крыла летательного аппарата
    return qₐ(a) * qᵣ(h, v)
end

plt_attack_angle = plot(
    ts[1:end-1],
    rad2deg.(value.(α)[1:end-1]);
    legend = nothing,
    title = "Angle of Attack (deg)",
)
plt_bank_angle = plot(
    ts[1:end-1],
    rad2deg.(value.(β)[1:end-1]);
    legend = nothing,
    title = "Bank Angle (deg)",
)
plt_heating = plot(
    ts,
    q.(value.(scaled_h) * 1e5, value.(scaled_v) * 1e4, value.(α));
    legend = nothing,
    title = "Heating (BTU/ft/ft/sec)",
)

plot(
    plt_attack_angle,
    plt_bank_angle,
    plt_heating;
    layout = grid(3, 1),
    linewidth = 2,
    size = (700, 700),
)

plot(
    rad2deg.(value.(ϕ)),
    rad2deg.(value.(θ)),
    value.(scaled_h);
    linewidth = 2,
    legend = nothing,
    title = "Space Shuttle Reentry Trajectory",
    xlabel = "Longitude (deg)",
    ylabel = "Latitude (deg)",
    zlabel = "Altitude (100,000 ft)",
)