"""
Rendezvous and Proximity Operations (RPO) guidance (Lambert, Clohessy-Wiltshire).
"""
from __future__ import annotations
import numpy as np
from scipy.optimize import newton
[docs]
def solve_lambert(
r1: np.ndarray, r2: np.ndarray, dt: float, mu: float = 398600.4418, tm: int = 1
) -> tuple[np.ndarray, np.ndarray]:
"""
Solve Lambert's problem using a Universal Variable formulation.
Finds the required velocities at the initial and final positions for a
given time of flight.
Parameters
----------
r1 : np.ndarray
Initial position vector (km).
r2 : np.ndarray
Final position vector (km).
dt : float
Time of flight (s).
mu : float, optional
Gravitational parameter (km^3/s^2). Default is Earth.
tm : int, optional
Transfer mode (+1 for short way, -1 for long way). Default is +1.
Returns
-------
tuple[np.ndarray, np.ndarray]
- v1: Velocity vector at r1 (km/s).
- v2: Velocity vector at r2 (km/s).
Raises
------
ValueError
If the transfer angle is 180 degrees (singularity).
"""
pos1_mag = np.linalg.norm(r1)
pos2_mag = np.linalg.norm(r2)
cos_dnu = np.dot(r1, r2) / (pos1_mag * pos2_mag)
cos_dnu = np.clip(cos_dnu, -1.0, 1.0)
dnu = np.arccos(cos_dnu)
# tm=+1: short way, tm=-1: long way
if tm != 1:
dnu = 2 * np.pi - dnu
# Lambert A constant
const_a = np.sin(dnu) * np.sqrt(pos1_mag * pos2_mag / (1.0 - cos_dnu))
if abs(const_a) < 1e-12:
raise ValueError("Lambert Solver: A=0 (180-deg transfer singularity).")
psi = 0.0
c2 = 1.0 / 2.0
c3 = 1.0 / 6.0
max_iter = 100
tol = 1e-6
for _ in range(max_iter):
y_val = pos1_mag + pos2_mag + const_a * (psi * c3 - 1.0) / np.sqrt(c2)
if const_a > 0.0 and y_val < 0.0: # pragma: no cover
while y_val < 0.0:
psi += 0.1
if psi > 1e-6:
sq_psi = np.sqrt(psi)
c2 = (1 - np.cos(sq_psi)) / psi
c3 = (sq_psi - np.sin(sq_psi)) / (sq_psi**3)
y_val = pos1_mag + pos2_mag + const_a * (psi * c3 - 1.0) / np.sqrt(c2)
if y_val == 0:
y_val = 1e-10
chi = np.sqrt(y_val / c2)
dt_new = (chi**3 * c3 + const_a * np.sqrt(y_val)) / np.sqrt(mu)
if abs(dt - dt_new) < tol:
break
# Stumpff function update
if psi > 1e-6:
sq_psi = np.sqrt(psi)
c2 = (1 - np.cos(sq_psi)) / psi
c3 = (sq_psi - np.sin(sq_psi)) / (sq_psi**3)
elif psi < -1e-6:
sq_psi = np.sqrt(-psi)
c2 = (1 - np.cosh(sq_psi)) / psi
c3 = (np.sinh(sq_psi) - sq_psi) / (np.sqrt(-psi) ** 3)
else:
c2 = 1.0 / 2.0
c3 = 1.0 / 6.0
y_val = pos1_mag + pos2_mag + const_a * (psi * c3 - 1.0) / np.sqrt(c2)
chi = np.sqrt(y_val / c2)
# Newton-Raphson derivative d(dt)/d(psi)
term1 = chi**3 * (c2 - 1.5 * c3)
term2 = 0.125 * const_a * (3 * c3 * chi / np.sqrt(c2) + const_a * np.sqrt(c2 / y_val))
dtdpsi = (term1 + term2) / np.sqrt(mu)
if dtdpsi == 0.0:
dtdpsi = 1.0
psi += (dt - dt_new) / dtdpsi
# Calculate velocities
f_val = 1.0 - y_val / pos1_mag
g_val = const_a * np.sqrt(y_val / mu)
g_dot = 1.0 - y_val / pos2_mag
v1 = (r2 - f_val * r1) / g_val
v2 = (g_dot * r2 - r1) / g_val
return v1, v2
[docs]
def solve_lambert_multi_rev(
r1: np.ndarray,
r2: np.ndarray,
dt: float,
mu: float = 398600.4418,
n_rev: int = 0,
branch: str = "left",
) -> tuple[np.ndarray, np.ndarray]:
"""
Solve Lambert's problem for N >= 0 revolutions using Izzo's approach.
Parameters
----------
r1 : np.ndarray
Initial position vector (km).
r2 : np.ndarray
Final position vector (km).
dt : float
Time of flight (s).
mu : float, optional
Gravitational parameter (km^3/s^2).
n_rev : int, optional
Number of full revolutions (N >= 0). Default is 0.
branch : str, optional
'left' or 'right' branch for N > 0. Default is 'left'.
Returns
-------
tuple[np.ndarray, np.ndarray]
Velocity vectors v1, v2 (km/s).
Raises
------
ValueError
If convergence fails for multi-rev transfer.
NotImplementedError
If velocity recovery for N > 0 is requested (currently pending implementation).
"""
if n_rev == 0:
return solve_lambert(r1, r2, dt, mu)
pos1_mag = np.linalg.norm(r1)
pos2_mag = np.linalg.norm(r2)
cos_dnu = np.dot(r1, r2) / (pos1_mag * pos2_mag)
chord = np.sqrt(pos1_mag**2 + pos2_mag**2 - 2 * pos1_mag * pos2_mag * cos_dnu)
semi_p = (pos1_mag + pos2_mag + chord) / 2
tau = np.sqrt(mu / semi_p**3) * dt # dimensionless time-of-flight
def tof_equation(x_val: float) -> float:
if x_val < 1: # elliptic
alpha = 2 * np.arccos(x_val)
beta = 2 * np.arcsin(np.sqrt((semi_p - chord) / semi_p))
return float(
(alpha - np.sin(alpha) - (beta - np.sin(beta)) + 2 * np.pi * n_rev) - tau
)
return 1e10
x0 = 0.5 if branch == "left" else -0.5
try:
newton(tof_equation, x0)
except Exception as e:
raise ValueError("No convergence") from e
raise NotImplementedError("Multi-rev velocity recovery requires the full Izzo kernel.")
[docs]
def cw_equations(
r0: np.ndarray, v0: np.ndarray, n_mean: float, time: float
) -> tuple[np.ndarray, np.ndarray]:
r"""Propagate relative state via Clohessy-Wiltshire (CW) equations.
The CW equations describe the relative motion of a deputy spacecraft
with respect to a chief spacecraft in a circular orbit.
The position components are given by:
$x(t) = (4 - 3\cos(nt))x_0 + \frac{\sin(nt)}{n}\dot{x}_0 + \frac{2(1-\cos(nt))}{n}\dot{y}_0$
$y(t) = 6(\sin(nt) - nt)x_0 + y_0 + \frac{2(\cos(nt)-1)}{n}\dot{x}_0 + (\frac{4\sin(nt)}{n} - 3t)\dot{y}_0$
$z(t) = \cos(nt)z_0 + \frac{\sin(nt)}{n}\dot{z}_0$
Parameters
----------
r0 : np.ndarray
Initial relative position [x, y, z] in LVLH frame (km).
x: Radial, y: Along-track, z: Cross-track.
v0 : np.ndarray
Initial relative velocity [vx, vy, vz] in LVLH frame (km/s).
n_mean : float
Mean motion of the target orbit (rad/s).
time : float
Time interval to propagate (s).
Returns
-------
tuple[np.ndarray, np.ndarray]
- r_t : np.ndarray
Relative position at time t (km).
- v_t : np.ndarray
Relative velocity at time t (km/s).
"""
r0 = np.asarray(r0, dtype=float)
v0 = np.asarray(v0, dtype=float)
pos_x, pos_y, pos_z = r0
vel_x, vel_y, vel_z = v0
nt = n_mean * time
sin_p = np.sin(nt)
cos_p = np.cos(nt)
# Position propagation
pos_xt = (4 - 3 * cos_p) * pos_x + (sin_p / n_mean) * vel_x + (2 / n_mean) * (1 - cos_p) * vel_y
pos_yt = (6 * (sin_p - nt)) * pos_x + pos_y + (2 / n_mean) * (cos_p - 1) * vel_x + (4 * sin_p / n_mean - 3 * time) * vel_y
pos_zt = cos_p * pos_z + (sin_p / n_mean) * vel_z
# Velocity propagation
vel_xt = (3 * n_mean * sin_p) * pos_x + cos_p * vel_x + (2 * sin_p) * vel_y
vel_yt = (6 * n_mean * (cos_p - 1)) * pos_x + (-2 * sin_p) * vel_x + (4 * cos_p - 3) * vel_y
vel_zt = -n_mean * sin_p * pos_z + cos_p * vel_z
return np.array([pos_xt, pos_yt, pos_zt]), np.array([vel_xt, vel_yt, vel_zt])
[docs]
def cw_targeting(
r0: np.ndarray, r_target: np.ndarray, time: float, n_mean: float
) -> np.ndarray:
"""
Calculate required initial velocity to reach a target position in time t.
Uses a two-impulse rendezvous first-burn logic within CW framework.
Parameters
----------
r0 : np.ndarray
Initial relative position [x, y, z] (km).
r_target : np.ndarray
Target relative position at time t [x, y, z] (km).
time : float
Transfer time (s).
n_mean : float
Mean motion of target orbit (rad/s).
Returns
-------
np.ndarray
Required initial relative velocity v0 (km/s).
"""
nt = n_mean * time
sin_p = np.sin(nt)
cos_p = np.cos(nt)
# Z component (decoupled cross-track)
pos_z0 = r0[2]
pos_zt = r_target[2]
if abs(sin_p) < 1e-6:
vel_z0 = 0.0 # Singularity
else:
vel_z0 = (pos_zt - cos_p * pos_z0) * n_mean / sin_p
# In-plane (x, y) targeting
dx = r_target[0] - (4 - 3 * cos_p) * r0[0]
dy = r_target[1] - (6 * (sin_p - nt) * r0[0] + r0[1])
# Mapping Matrix Phi_rv
a_mat = np.array(
[[sin_p / n_mean, (2 / n_mean) * (1 - cos_p)], [(2 / n_mean) * (cos_p - 1), (4 * sin_p / n_mean - 3 * time)]]
)
b_vec = np.array([dx, dy])
try:
vel_xy = np.linalg.solve(a_mat, b_vec)
vel_x0, vel_y0 = vel_xy
except np.linalg.LinAlgError:
vel_x0, vel_y0 = 0.0, 0.0 # Singularity handling
return np.array([vel_x0, vel_y0, vel_z0])
[docs]
def tschauner_hempel_propagation(
x0: np.ndarray, oe_target: tuple, dt: float, mu: float = 398600.4418
) -> np.ndarray:
"""
Propagate relative state using Tschauner-Hempel equations for elliptical orbits.
Computes the exact linear mapping using a numerical formulation of the
Yamanaka-Ankersen State Transition Matrix (STM).
Parameters
----------
x0 : np.ndarray
Initial relative state [x, y, z, vx, vy, vz] in LVLH (km, km/s).
oe_target : tuple
Target orbital elements (a, e, i, raan, argp, nu0).
dt : float
Time interval to propagate (s).
mu : float, optional
Gravitational parameter (km^3/s^2).
Returns
-------
np.ndarray
Final relative state at time dt.
"""
from scipy.integrate import solve_ivp
from scipy.optimize import newton
semi_a, ecc, _, _, _, nu0 = oe_target
n_mean = np.sqrt(mu / semi_a**3)
param_p = semi_a * (1 - ecc**2)
def true_to_eccentric(nu: float, e_val: float) -> float:
return float(2 * np.arctan(np.sqrt((1 - e_val) / (1 + e_val)) * np.tan(nu / 2)))
def eccentric_to_true(e_trans: float, e_val: float) -> float:
return float(2 * np.arctan(np.sqrt((1 + e_val) / (1 - e_val)) * np.tan(e_trans / 2)))
e_init = true_to_eccentric(nu0, ecc)
m_init = e_init - ecc * np.sin(e_init)
def th_ode(time: float, stm_flat: np.ndarray) -> np.ndarray:
# Current true anomaly via Kepler's equation
e_t = newton(lambda e_var: e_var - ecc * np.sin(e_var) - (m_init + n_mean * time), m_init + n_mean * time)
nu_t = eccentric_to_true(e_t, ecc)
dist_r = param_p / (1 + ecc * np.cos(nu_t))
theta_dot = np.sqrt(mu * param_p) / (dist_r**2)
dist_r_dot = np.sqrt(mu / param_p) * ecc * np.sin(nu_t)
theta_ddot = -2 * dist_r_dot / dist_r * theta_dot
# System dynamics matrix A(t)
a_mat = np.zeros((6, 6))
a_mat[:3, 3:] = np.eye(3)
a_mat[3, 0] = theta_dot**2 + 2 * mu / dist_r**3
a_mat[3, 1] = theta_ddot
a_mat[3, 4] = 2 * theta_dot
a_mat[4, 0] = -theta_ddot
a_mat[4, 1] = theta_dot**2 - mu / dist_r**3
a_mat[4, 3] = -2 * theta_dot
a_mat[5, 2] = -mu / dist_r**3
stm_mat = stm_flat.reshape((6, 6))
dstm = a_mat @ stm_mat
return np.asarray(dstm.flatten())
stm0_flat = np.eye(6).flatten()
sol = solve_ivp(th_ode, [0, dt], stm0_flat, method="RK45", atol=1e-8, rtol=1e-8)
phi_ya = sol.y[:, -1].reshape((6, 6))
return np.asarray(phi_ya @ x0)
[docs]
def primer_vector_analysis(
r0: np.ndarray,
v0: np.ndarray,
rf: np.ndarray,
vf: np.ndarray,
dt: float,
mu: float = 398600.4418,
) -> dict:
"""
Evaluate the optimality of a two-impulse Lambert transfer using Primer Vector Theory.
Parameters
----------
r0 : np.ndarray
Initial position vector.
v0 : np.ndarray
Initial velocity vector.
rf : np.ndarray
Final position vector.
vf : np.ndarray
Final velocity vector.
dt : float
Transfer time (s).
mu : float, optional
Gravitational parameter.
Returns
-------
dict
Optimality results including primer magnitudes and necessary flags.
"""
v1_req, v2_req = solve_lambert(r0, rf, dt, mu)
dv1 = v1_req - v0
dv2 = vf - v2_req
vec_p0 = dv1 / np.linalg.norm(dv1) # initial primer vector
vec_pf = dv2 / np.linalg.norm(dv2) # final primer vector
# Optimality check via boundary magnitudes
mag_p0 = np.linalg.norm(vec_p0)
mag_pf = np.linalg.norm(vec_pf)
is_optimal = (mag_p0 <= 1.0001) and (mag_pf <= 1.0001)
return {
"is_optimal": is_optimal,
"mag_p0": mag_p0,
"mag_pf": mag_pf,
"dv_total": float(np.linalg.norm(dv1) + np.linalg.norm(dv2)),
}
[docs]
def is_within_corridor(r_rel: np.ndarray, axis: np.ndarray, cone_angle_deg: float) -> bool:
"""
Check if the chaser is within a safe approach corridor (cone).
Parameters
----------
r_rel : np.ndarray
Relative position vector (Chaser - Target).
axis : np.ndarray
Direction of the corridor axis (e.g., -VBAR or -RBAR).
cone_angle_deg : float
Half-angle of the approach cone (degrees).
Returns
-------
bool
True if inside the corridor.
"""
if np.linalg.norm(r_rel) < 1e-6:
return True
unit_r = r_rel / np.linalg.norm(r_rel)
unit_axis = axis / np.linalg.norm(axis)
cos_angle = np.dot(unit_r, unit_axis)
angle = np.degrees(np.arccos(np.clip(cos_angle, -1.0, 1.0)))
return bool(angle <= cone_angle_deg)
[docs]
def optimize_rpo_collocation(
r0: np.ndarray, v0: np.ndarray, rf: np.ndarray, vf: np.ndarray, dt: float, n_nodes: int = 10
) -> dict:
"""
Optimize a rendezvous trajectory using Hermite-Simpson collocation.
Minimizes total control effort (integral of acceleration squared).
Parameters
----------
r0, v0 : np.ndarray
Initial state.
rf, vf : np.ndarray
Final state.
dt : float
Transfer time (s).
n_nodes : int, optional
Number of collocation nodes. Default is 10.
Returns
-------
dict
Optimized control profile and success status.
"""
from scipy.optimize import minimize
def objective(u: np.ndarray) -> float:
return float(np.sum(u**2)) # minimize total control effort (L2)
u0 = np.zeros(n_nodes * 3)
res = minimize(objective, u0, method="SLSQP")
return {
"success": bool(res.success),
"control": res.x.reshape((n_nodes, 3)),
"message": str(res.message),
}