[Python] Satellite Orbit Raising Trajectory Optimization

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.

Continue reading...