python - Satellite Orbit Raising Trajectory Optimization - Stack Overflow

admin管理员组

文章数量:1404924

I am trying to get an optimal solution for a trajectory optimization problem for a satellite orbit raising. My main objective is to minimize fuel. I have written a code as given below

import casadi as ca
import numpy as np
from poliastro.bodies import Earth
from poliastro.twobody import Orbit
from poliastro.core.propagation import func_twobody

from astropy import units as u
from astropy.time import TimeDelta
from poliastro.twobody.propagation import CowellPropagator
from poliastro.twobody.sampling import EpochsArray
from poliastro.core.elements import rv2coe

k = Earth.k.to(u.km**3 / u.s**2).value

def gve(r_norm, a, e, inc, Omega, omega, theta, dv_R, dv_T, dv_N):
    p = a * (1 - e**2)  # Semi-latus rectum
    h = np.sqrt(k * p)  # Specific angular momentum

    da = 2 * a**2 / h * (e * ca.sin(theta) * dv_R + p/r_norm *  dv_T )
    de = p/h * (ca.sin(theta) *  dv_R + (ca.cos(theta) + (e + ca.cos(theta))/(1 + e*ca.cos(theta)))* dv_T)
    di = (r_norm / h) * ca.cos(theta + omega) * dv_N
    dOmega = (r_norm / (h * ca.sin(inc))) * ca.sin(theta + omega) * dv_N
    domega = p/(h*e)*(-ca.cos(theta)* dv_R + ca.sin(theta)*(1+r_norm/p)*dv_T) - r_norm/(h*ca.tan(inc))*ca.sin(theta + omega)*dv_N
    dtheta = h / r_norm**2 * raise_dt.value + (1 / (e * h)) * (p * ca.cos(theta) * dv_R - (p + r_norm) * ca.sin(theta) * dv_T)
    return da, de, di, dOmega, domega, dtheta

    
    


def f(t0, state, k):
    du_kep = func_twobody(t0, state, k)
    return du_kep

# Define satellite mass, Cd, maxthrust, area
# Define Initial Orbit at epoch J2000
# Increase sma (semi major axis)
# Add Optimizer
# Get solution



def traj_optim(init_coes, raise_prop_duration, raise_dt, maxThrust, mass):
    # Define initial orbit
    orb = Orbit.from_classical(Earth, init_coes[0] * u.km, init_coes[1] * u.one, init_coes[2] * u.rad, init_coes[3] * u.rad, init_coes[4] * u.rad, init_coes[5] * u.rad)

    # Constants
    N = int((raise_prop_duration.to(u.s)/raise_dt).value)  # Number of discrete time steps
    print(f"Number of Nodes: {N}")

    tofs = TimeDelta(np.linspace(0 * u.h, raise_prop_duration, num=N))
    rr, vv = orb.to_ephem(EpochsArray(orb.epoch + tofs, method=CowellPropagator(f=f))).rv()

    coe_final = init_coes
    coe_final[0] = coe_final[0] + 0.005

    coes_init_sol = np.zeros((N, 6))

    dv_max = maxThrust*raise_dt.value/mass*1e-3  # Max velocity change per step (km/raise_dt)
    print(f"Max Δv per step: {dv_max:.2f} km/s")

    # OPTIMIZATION PROCESS
    # Define optimization variables
    opti = ca.Opti()

    coe = opti.variable(6, N)  # Position (x, y)
    DV = opti.variable(3, N-1) # Change in velocity (Δvx, Δvy)

    T = opti.variable()  # Final time

    print(f"Optimization Variables: {coe.shape}, {DV.shape}")

    # Gauss' Variational Equations
    for i in range(N-1):
        coes_init_sol[i, :] = rv2coe(k, rr[i, :], vv[i, :])

        a = coe[0, i]
        e = coe[1, i]
        inc = coe[2, i]
        Omega = coe[3, i]
        omega = coe[4, i]
        theta = coe[5, i]
        
        r_norm = np.linalg.norm(rr[i, :]).value
        
        # Compute orbital rates using Gauss' equations
        da, de, di, dOmega, domega, dtheta = gve(r_norm, a, e, inc, Omega, omega, theta, DV[0, i], DV[1, i], DV[2, i])

        # Forward Euler integration
        coe_next = coe[:, i] + ca.vertcat(da, de, di, dOmega, domega, dtheta)
        opti.subject_to(coe[:, i+1] == coe_next)
        opti.subject_to(ca.fabs(ca.sqrt(DV[0, i]**2 + DV[1, i]**2 + DV[2, i]**2)) <= dv_max)
        opti.subject_to(coe[0, i] >= 6400.0)
        opti.subject_to(coe[1, i] >= 0.0)

    coes_init_sol[N-1, :] = rv2coe(k, rr[N-1, :], vv[N-1, :])

    #breakpoint()
    # Initial and final conditions
    for j in range(6):
        opti.subject_to(ca.fabs(coe[j, 0] - coes_init_sol[0, :]) <= 1e-5)
    opti.subject_to(ca.fabs(coe[0, N-1] - coe_final[0]) <= 1e-5)
    #opti.subject_to(ca.fabs(coe[-1, 1] - coe_final[1]) <= 1e-4)

    for i in range(N):
        opti.set_initial(coe[:, i], coes_init_sol[i, :])
    opti.set_initial(DV, np.ones((3, N-1))*1e-7)  # Start with zero thrust

    # Objective: Minimize total velocity change (sum of all Δv magnitudes)
    opti.minimize(ca.sumsqr(DV))

    # Solve optimization
    opti.solver('ipopt', {
        'print_time': True, 
        'ipopt': {
            'tol': 1e-5,
            'max_iter': 100,
            'mu_strategy': 'adaptive',  
            'print_level': 5,
            'linear_solver': 'mumps'
        }
    })


    sol = opti.solve()

    print("Final SMA:", opti.debug.value(coe[-1, 0]))
    print("Final eccentricity:", opti.debug.value(coe[-1, 1]))
    print("Applied Δv:", opti.debug.value(dv))

    # Extract results
    a_solution = sol.value(coe[:, 0])
    e_solution = sol.value(coe[:, 1])
    dv_sol = sol.value(dv)

    # Print total Δv used
    print(f"Total Δv used: {np.sum(np.linalg.norm(dv_sol, axis=1)):.2f} m/s")


if __name__ == "__main__":
    # INPUTS
    mass = 12           # kg
    maxThrust = 200   # N
    init_coes = np.array([7000, 0.0, 98, 0, 270, 0])  # [sma_km, ecc, incl_deg, Om_deg, om_deg, ta_deg]
    raise_prop_duration = 2*u.s
    raise_dt = 1*u.s

    init_coes[2] = np.radians(init_coes[2])
    init_coes[3] = np.radians(init_coes[3])
    init_coes[4] = np.radians(init_coes[4])
    init_coes[5] = np.radians(init_coes[5])

    traj_optim(init_coes, raise_prop_duration, raise_dt, maxThrust, mass)

However this problem does not ever converge. I have tried increasing the number of nodes, increasing the time of propagation and other changes. I am applying Gauss Variational Equations for the system dynamics. I am very new to casadi and might have made a simple mistake. However even after surfing through the internet, I am unable to find anything of consequence to me. It would be great is someone can help me figure this out.

I am trying to get an optimal solution for a trajectory optimization problem for a satellite orbit raising. My main objective is to minimize fuel. I have written a code as given below

import casadi as ca
import numpy as np
from poliastro.bodies import Earth
from poliastro.twobody import Orbit
from poliastro.core.propagation import func_twobody

from astropy import units as u
from astropy.time import TimeDelta
from poliastro.twobody.propagation import CowellPropagator
from poliastro.twobody.sampling import EpochsArray
from poliastro.core.elements import rv2coe

k = Earth.k.to(u.km**3 / u.s**2).value

def gve(r_norm, a, e, inc, Omega, omega, theta, dv_R, dv_T, dv_N):
    p = a * (1 - e**2)  # Semi-latus rectum
    h = np.sqrt(k * p)  # Specific angular momentum

    da = 2 * a**2 / h * (e * ca.sin(theta) * dv_R + p/r_norm *  dv_T )
    de = p/h * (ca.sin(theta) *  dv_R + (ca.cos(theta) + (e + ca.cos(theta))/(1 + e*ca.cos(theta)))* dv_T)
    di = (r_norm / h) * ca.cos(theta + omega) * dv_N
    dOmega = (r_norm / (h * ca.sin(inc))) * ca.sin(theta + omega) * dv_N
    domega = p/(h*e)*(-ca.cos(theta)* dv_R + ca.sin(theta)*(1+r_norm/p)*dv_T) - r_norm/(h*ca.tan(inc))*ca.sin(theta + omega)*dv_N
    dtheta = h / r_norm**2 * raise_dt.value + (1 / (e * h)) * (p * ca.cos(theta) * dv_R - (p + r_norm) * ca.sin(theta) * dv_T)
    return da, de, di, dOmega, domega, dtheta

    
    


def f(t0, state, k):
    du_kep = func_twobody(t0, state, k)
    return du_kep

# Define satellite mass, Cd, maxthrust, area
# Define Initial Orbit at epoch J2000
# Increase sma (semi major axis)
# Add Optimizer
# Get solution



def traj_optim(init_coes, raise_prop_duration, raise_dt, maxThrust, mass):
    # Define initial orbit
    orb = Orbit.from_classical(Earth, init_coes[0] * u.km, init_coes[1] * u.one, init_coes[2] * u.rad, init_coes[3] * u.rad, init_coes[4] * u.rad, init_coes[5] * u.rad)

    # Constants
    N = int((raise_prop_duration.to(u.s)/raise_dt).value)  # Number of discrete time steps
    print(f"Number of Nodes: {N}")

    tofs = TimeDelta(np.linspace(0 * u.h, raise_prop_duration, num=N))
    rr, vv = orb.to_ephem(EpochsArray(orb.epoch + tofs, method=CowellPropagator(f=f))).rv()

    coe_final = init_coes
    coe_final[0] = coe_final[0] + 0.005

    coes_init_sol = np.zeros((N, 6))

    dv_max = maxThrust*raise_dt.value/mass*1e-3  # Max velocity change per step (km/raise_dt)
    print(f"Max Δv per step: {dv_max:.2f} km/s")

    # OPTIMIZATION PROCESS
    # Define optimization variables
    opti = ca.Opti()

    coe = opti.variable(6, N)  # Position (x, y)
    DV = opti.variable(3, N-1) # Change in velocity (Δvx, Δvy)

    T = opti.variable()  # Final time

    print(f"Optimization Variables: {coe.shape}, {DV.shape}")

    # Gauss' Variational Equations
    for i in range(N-1):
        coes_init_sol[i, :] = rv2coe(k, rr[i, :], vv[i, :])

        a = coe[0, i]
        e = coe[1, i]
        inc = coe[2, i]
        Omega = coe[3, i]
        omega = coe[4, i]
        theta = coe[5, i]
        
        r_norm = np.linalg.norm(rr[i, :]).value
        
        # Compute orbital rates using Gauss' equations
        da, de, di, dOmega, domega, dtheta = gve(r_norm, a, e, inc, Omega, omega, theta, DV[0, i], DV[1, i], DV[2, i])

        # Forward Euler integration
        coe_next = coe[:, i] + ca.vertcat(da, de, di, dOmega, domega, dtheta)
        opti.subject_to(coe[:, i+1] == coe_next)
        opti.subject_to(ca.fabs(ca.sqrt(DV[0, i]**2 + DV[1, i]**2 + DV[2, i]**2)) <= dv_max)
        opti.subject_to(coe[0, i] >= 6400.0)
        opti.subject_to(coe[1, i] >= 0.0)

    coes_init_sol[N-1, :] = rv2coe(k, rr[N-1, :], vv[N-1, :])

    #breakpoint()
    # Initial and final conditions
    for j in range(6):
        opti.subject_to(ca.fabs(coe[j, 0] - coes_init_sol[0, :]) <= 1e-5)
    opti.subject_to(ca.fabs(coe[0, N-1] - coe_final[0]) <= 1e-5)
    #opti.subject_to(ca.fabs(coe[-1, 1] - coe_final[1]) <= 1e-4)

    for i in range(N):
        opti.set_initial(coe[:, i], coes_init_sol[i, :])
    opti.set_initial(DV, np.ones((3, N-1))*1e-7)  # Start with zero thrust

    # Objective: Minimize total velocity change (sum of all Δv magnitudes)
    opti.minimize(ca.sumsqr(DV))

    # Solve optimization
    opti.solver('ipopt', {
        'print_time': True, 
        'ipopt': {
            'tol': 1e-5,
            'max_iter': 100,
            'mu_strategy': 'adaptive',  
            'print_level': 5,
            'linear_solver': 'mumps'
        }
    })


    sol = opti.solve()

    print("Final SMA:", opti.debug.value(coe[-1, 0]))
    print("Final eccentricity:", opti.debug.value(coe[-1, 1]))
    print("Applied Δv:", opti.debug.value(dv))

    # Extract results
    a_solution = sol.value(coe[:, 0])
    e_solution = sol.value(coe[:, 1])
    dv_sol = sol.value(dv)

    # Print total Δv used
    print(f"Total Δv used: {np.sum(np.linalg.norm(dv_sol, axis=1)):.2f} m/s")


if __name__ == "__main__":
    # INPUTS
    mass = 12           # kg
    maxThrust = 200   # N
    init_coes = np.array([7000, 0.0, 98, 0, 270, 0])  # [sma_km, ecc, incl_deg, Om_deg, om_deg, ta_deg]
    raise_prop_duration = 2*u.s
    raise_dt = 1*u.s

    init_coes[2] = np.radians(init_coes[2])
    init_coes[3] = np.radians(init_coes[3])
    init_coes[4] = np.radians(init_coes[4])
    init_coes[5] = np.radians(init_coes[5])

    traj_optim(init_coes, raise_prop_duration, raise_dt, maxThrust, mass)

However this problem does not ever converge. I have tried increasing the number of nodes, increasing the time of propagation and other changes. I am applying Gauss Variational Equations for the system dynamics. I am very new to casadi and might have made a simple mistake. However even after surfing through the internet, I am unable to find anything of consequence to me. It would be great is someone can help me figure this out.

• python
• nonlinear-optimization
• casadi

Share Improve this question Follow edited Mar 9 at 19:27 Progman 19.7k77 gold badges5555 silver badges8282 bronze badges asked Mar 9 at 18:20 NIKITA SEHRAWATNIKITA SEHRAWAT 1 1

• If your UL has it then there is a standard reference text by Battin on orbital dynamics that will help here. Otherwise this simplified NASA introduction might be good enough to allow you to get the code to work. A small typo will break it. – Martin Brown Commented Mar 9 at 20:18

Add a comment  | 

1 Answer 1

Sorted by: Reset to default -2

IT IS NOT IMPORTANT , if there are no need in further automatisation .

Governor should know the answer of optimization . This question is not only about the ability to be solved . I am doing for practice usually matrix division twice or map origami in real for training .

I do not involved in library stack casadi to answer specifically to code . Something smart and funny about Gauss Variational Equations: "Linear is just a lot of vector points divided twice" It is not clay and with metal plate it is really feeling lice to make a circle with metal scissors simply . Ethernet of Octet stream doing chunks or batches in size of 1Kb file transfer to make it smooth , the ethernet cable is 8 wires as 0x010101 or 1x2fde0b e.g. density of stream as representation of BIN digits .

GAUSS was explained theories with practical advance of easiest ways to fold paper , it is all about the cycle of that automatism with making it half a/2 other the specified segment b is for a

本文标签： pythonSatellite Orbit Raising Trajectory OptimizationStack Overflow