Box control in satellite Formation Flying#
We show how heyoka.py’s event detection machinery can be used in an innovative formulation of the box-control problem for formation flying satellites. This is the problem of controlling a Deputy satellite to remain within a predefined control box defined in the Local Horizontal Local Vertical frame of the Chief satellite. Our solution is to apply impulsive DV when certain events trigger.
To derive all equations we used and abused the Vectrix notation from Hugues.
Preliminaries#
We consider the fundamental problem of satellite formation flying. A main satellite called Chief orbits some primary body (here the Earth). We consider two reference frames. \(\mathcal F_i=\left[\hat{\mathbf e}_i, \hat{\mathbf e}_j, \hat{\mathbf e}_k \right]\) is the inertial frame where the motion of both satellites will be described, while \(\mathcal F_L=\left[\hat{\mathbf e}_r, \hat{\mathbf e}_\theta, \hat{\mathbf e}_h \right]\) is the Local Horizontal Local Vertical frame referred to the Chief satellite. Its definition in terms of the Chief satellite position \(\overrightarrow{\mathbf r}\) and velocity \(\overrightarrow{\mathbf v}\) is:
We indicate with capital letters \(X,Y,Z\) the components of the satellite position vector in the frame \(\mathcal F_i\), we introduce the quantities \(r = \vert\overrightarrow{\mathbf r}\vert\), \(r = \vert\overrightarrow{\mathbf h}\vert\) and \(\sigma = \overrightarrow{\mathbf r}\cdot \overrightarrow{\mathbf v}\) and write the rotation matrix \(\mathbf C_{Li} = \mathcal F_L^T \cdot \mathcal F_i\) for the LHLV frame:
which allows to find the \(\mathcal F_i\) components of a vector \(\mathbf v_i\) from its \(\mathcal F_L\) components as \(\mathbf v_i = \mathbf C_{Li} \mathbf v_L\). The angular velocity of the LHLV frame is:
where \(f_h\) is the perturbative acceleration acting on the satellite along the \(\hat{\mathbf e}_h\) direction.
The Equations of Motion#
We consider the dynamics of both the Chief and the Deputy expressed in terms of their position and velocity in \(\mathcal F_i\) and subject to the J2 term only:
The Initial conditions#
The initial conditions for the Chief satellite can be generic and are given in the \(\mathcal F_i\) frame. The initial conditions for the Deputy, instead, are given in the \(\mathcal F_L\) frame,. This simple fact couples the system breaking the symmetry of the equations of motion written above. Expressing the relative position and velocity as absolute and viceversa requires the use of \(\mathbf C_{Li}\), and most importantly of \(\omega_L\) which is, in turn, affected by the perturbative accelerations acting on the Chief. The following transformations hold:
where we have indicated with small letters \(x,y,z\) the components of the relative position vector of the Deputy.
We can now start to code this in heyoka.py, we will introduce later the control part of the problem.
# The usual main imports
import heyoka as hy
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
We start writing down the Equation of Motion (EOM):
# The state (using km as unit length)
xC, yC, zC, vxC, vyC, vzC, xD, yD, zD, vxD, vyD, vzD = hy.make_vars("xC", "yC", "zC", "vxC", "vyC", "vzC", "xD", "yD", "zD", "vxD", "vyD", "vzD")
# Parameters
mu = 398600.4418 #km^3/sec^2
J2 = 1082.645E-6
Re = 6371 #km
c = 3./2. * J2*mu*Re**2
# Auxiliary variables
rC2 = xC**2+yC**2+zC**2
rD2 = xD**2+yD**2+zD**2
# EOM
# Chief
dxC = vxC
dyC = vyC
dzC = vzC
dvxC = (- mu * xC/rC2 - c * xC / rC2**2 * (1 - 5*zC**2/rC2)) / hy.sqrt(rC2)
dvyC = (- mu * yC/rC2 - c * yC / rC2**2 * (1 - 5*zC**2/rC2)) / hy.sqrt(rC2)
dvzC = (- mu * zC/rC2 - c * zC / rC2**2 * (3 - 5*zC**2/rC2)) / hy.sqrt(rC2)
# Deputy
dxD = vxD
dyD = vyD
dzD = vzD
dvxD = (- mu * xD/rD2 - c * xD / rD2**2 * (1 - 5*zD**2/rD2)) / hy.sqrt(rD2)
dvyD = (- mu * yD/rD2 - c * yD / rD2**2 * (1 - 5*zD**2/rD2)) / hy.sqrt(rD2)
dvzD = (- mu * zD/rD2 - c * zD / rD2**2 * (3 - 5*zD**2/rD2)) / hy.sqrt(rD2)
# Equations
eqns = [(xC, dxC), (yC, dyC), (zC, dzC), (vxC, dvxC), (vyC, dvyC), (vzC, dvzC), (xD, dxD), (yD, dyD), (zD, dzD), (vxD, dvxD), (vyD, dvyD), (vzD, dvzD)]
And some helper functions allowing conversions between inertial and LHLV frames:
def compute_LHLV_rot(state, fh):
"""Computes the rotation matrix C from inertial to LHLV. i.e. v_{LHLV} = C v_{inertial}
and the angular velocity of the LHLV frame
Args:
state (1D np.array): the satellite state in the inertial frame
fh (float): disturbance acting on the satellite along the i_h axis (aligned with angular momentum)
Returns:
np.matrix (3x3): the rotation matrix
np.matrix (3x1): the angular velocity
"""
# We dispatch to the correct implementation of sqrt according to the state type
if type(state[0]) == type(xC):
sqrt = hy.sqrt
zero = hy.expression(0)
else:
sqrt = np.sqrt
state_type = float
zero = 0.
# Rotation Matrix
X, Y, Z, DX, DY, DZ = state
r2 = X*X+Y*Y+Z*Z
r = sqrt(r2)
h2 = (Y*DZ-Z*DY)**2 + (Z*DX-X*DZ)**2 + (X*DY-Y*DX)**2
h = sqrt(h2)
sigma = X*DX+Y*DY+Z*DZ
retval1 = np.matrix([[h*X, h*Y, h*Z], [r2*DX-sigma*X, r2*DY-sigma*Y, r2*DZ-sigma*Z], [r*(Y*DZ-Z*DY), r*(Z*DX-X*DZ), r*(X*DY-Y*DX)]]) / h / r
# Angular velocity
retval2 = np.matrix([[r/h*fh],[zero],[h/r2]])
return retval1, retval2
def to_relative(stateC, stateD, fh):
"""Transforms the state of the Deputy to the LHLV frame attached to the Chief
Args:
stateC (1D np.array): the Chief state in the inertial frame
stateD (1D np.array): the Deputy state in the inertial frame
fh (float): disturbance acting on the Chief satellite along the i_h axis (aligned with angular momentum)
Returns:
np.array, np.array: r,v in the LHLV frame
"""
# We reshape the input state into column vectors and alike
X, Y, Z, DX, DY, DZ = stateC
XD, YD, ZD, DXD, DYD, DZD = stateD
rC = np.matrix([[X],[Y],[Z]])
rD = np.matrix([[XD],[YD],[ZD]])
vC = np.matrix([[DX],[DY],[DZ]])
vD = np.matrix([[DXD],[DYD],[DZD]])
# We compute the LHLV rotation matrix and angular velocity
C, omega = compute_LHLV_rot(stateC, fh)
# We compute the relative state
xD = C * (rD - rC)
dxD = C * (vD - vC) - np.cross(omega.transpose(), xD.transpose()).transpose()
return [xD[0,0], xD[1,0], xD[2,0], dxD[0,0], dxD[1,0], dxD[2,0]]
def to_absolute(stateC, stateD, fh):
"""Transforms the state of the Deputy to the inertial frame
Args:
stateC (1D np.array): the Chief state in the inertial frame
stateD (1D np.array): the Deputy state in the LHLV frame attached to the Chief
fh (float): disturbance acting on the Chief satellite along the i_h axis (aligned with angular momentum)
Returns:
np.array, np.array: r,v in the LHLV frame
"""
# We reshape the input state into column vectors and alike
X, Y, Z, DX, DY, DZ = stateC
x, y, z, dx, dy, dz = stateD
rC = np.matrix([[X],[Y],[Z]])
xD = np.matrix([[x],[y],[z]])
vC = np.matrix([[DX],[DY],[DZ]])
dxD = np.matrix([[dx],[dy],[dz]])
# We compute the LHLV rotation matrix and angular velocity
C, omega = compute_LHLV_rot(stateC, fh)
C = C.transpose()
# We compute the absolute state
rD = rC + C * xD
vD = vC + C * (dxD + np.cross(omega.transpose(), xD.transpose()).transpose())
return [rD[0,0], rD[1,0], rD[2,0], vD[0,0], vD[1,0], vD[2,0]]
def J2_LHLV(state, c):
"""Computes the J2 perturbation in the LHLV frame
Args:
state (1D np.array): the state in the inertial frame
c (float): the constant 3/2 * J2 * mu * Re**2
Returns:
np.array: the resulting J2 perturbation in the LHLV frame
"""
# We dispatch to the correct implementation of sqrt according to the state type
if type(state[0]) == type(xC):
sqrt = hy.sqrt
state_type = object
else:
sqrt = np.sqrt
state_type = float
X, Y, Z, DX, DY, DZ = state
r2 = X*X+Y*Y+Z*Z
r = sqrt(r2)
h2 = (Y*DZ-Z*DY)**2 + (Z*DX-X*DZ)**2 + (X*DY-Y*DX)**2
h = sqrt(h2)
sigma = X*DX+Y*DY+Z*DZ
return np.array([c/r2**2*(3*Z/r2-1), -2*c/h/r2**3*Z*(r2*DZ-sigma*Z), - 2*c/h/r2**2/r*Z*(X*DY-Y*DX)], dtype = state_type)
Testing the Adaptive Taylor Integration and plotting the trajectories#
Lets visualize the solution of the numerical integration in the LHLV frame for the Deputy and in the inertial frame for the Chief.
We use, as reference orbit for the Chief, a circular orbit at 750km of altitude and an inclination of 98.2 degrees. The deputy is set to follow behind in the LHLV frame at a distance of 100m. We also add 1m offset in the z direction to make the problem third dimension count.
# Chief Orbit
incl = 98.2/180*np.pi
h = 750.
a = Re+h
# Deputy orbit
trailing = -0.1 # 100 m
hover = -6.7567E-7 # accounts for non linearities and puts the Deputy trailing the Chief.
# Initial Conditions for the Chief
chief_ic = [a,0.,0.,0.,np.sqrt(mu/a)*np.cos(incl),np.sqrt(mu/a)*np.sin(incl)]
# Initial Conditions for the Deputy (we start in the LHLV frame)
deputy_ic_r = [0., trailing, hover, 0., 0., 0.]
fh = J2_LHLV(chief_ic, c)[2]
# Initial Conditions for the Chief (in the inertial frame)
deputy_ic = to_absolute(chief_ic, deputy_ic_r, fh)
we are now able to instantiate the the Taylor integrator:
# The Taylor integrator
ta = hy.taylor_adaptive(eqns, chief_ic + deputy_ic)
and to perform the integration:
# As a test we propagate for 10 orbits
# Number of points in the time grid
N = 1500
t_grid = np.linspace(0, 20*np.pi*np.sqrt(a**3/mu), N)
# Propagate and return the state at the grid points
oc, _, _, _, _, res = ta.propagate_grid(t_grid)
Since we wrote the equation in the inertial frame we need to retreive the Deputy state in the LHL frame:
# We compute the results in the LHLV frame for the Deputy
deputy_rel = np.zeros((N,6))
for i, item in enumerate(res):
fh = J2_LHLV(item[:6], c)[2]
deputy_rel[i] = to_relative(item[:6], item[6:], fh)
We can finally plot the resulting orbit:
# And plot
fig = plt.figure(figsize = (9,9))
ax1 = fig.add_subplot(1, 2, 1, projection='3d')
ax2 = fig.add_subplot(1, 2, 2, projection='3d')
ax1.plot3D(res[:, 0], res[:, 1], res[:, 2])
ax1.set_title("Chief (inertial frame)")
ax1.set_xlim([-6000,6000])
ax1.set_ylim([-6000,6000])
ax1.set_zlim([-6000,6000])
ax2.plot3D(deputy_rel[:, 0], deputy_rel[:, 1], deputy_rel[:, 2])
ax2.set_title("Deputy (LHLV frame)");
Controlling the formation within a predefined box#
Now that we have established the equation of motion, we can simulate a box-control strategy. In essence, we want to keep the Deputy within a box centered around its initial trailing position and of a predefined size.
To do so, each time the box border is reached, we will apply a \(\Delta V\) flipping the relative velocity component along the corresponding axis, thus keeping the Deputy within the desired box.
First we write two callbacks which will flip or cancel the velocity and update the \(\Delta V\) count.
# This callback flips the selected component (x = 3, y = 4, z = 5)
# of the relative velocity
def cb_flip_rel_component(ta, component):
global DV
# Compute the perturbation effect on w
fh = J2_LHLV(ta.state[:6], c)[2]
# Getting the relative state of the Deputy
rel_state = to_relative(ta.state[:6], ta.state[6:], fh)
# Flipping the corresponding component
rel_state[component] = -rel_state[component]
# Getting the absolute state back
new_abs_state = to_absolute(ta.state[:6], rel_state, fh)
# Updating the Taylor integrator internal state
ta.state[6:] = new_abs_state
# Updating the DV count
DV+=2*np.abs(rel_state[component])
return True
# This callback flips the selected component (x = 3, y = 4, z = 5)
# of the relative velocity
def cb_zero_rel_component(ta, component):
global DV
# Compute the perturbation effect on w
fh = J2_LHLV(ta.state[:6], c)[2]
# Getting the relative state of the Deputy
rel_state = to_relative(ta.state[:6], ta.state[6:], fh)
# Zeroing the corresponding component
rel_state[component] = 0.
# Getting the absolute state back
new_abs_state = to_absolute(ta.state[:6], rel_state, fh)
# Updating the Taylor integrator internal state
ta.state[6:] = new_abs_state
# Updating the DV count
DV+=np.abs(rel_state[component])
return True
We then define the various events triggering when the box boundary is reached. Since the control box is defined in the LHLV frame, we first need to compute the expressions of the relative state as a function of the absolute state:
fh_sym = J2_LHLV([xC, yC, zC, vxC, vyC, vzC], c)[2]
state_rel_sym = to_relative([xC, yC, zC, vxC, vyC, vzC],[xD, yD, zD, vxD, vyD, vzD], fh_sym)
The events can now be defined:
# The size of the control box is 10 cm
box_size = 0.0001
# We define one event per cube side
ev_left = hy.t_event(state_rel_sym[0] - box_size / 2, callback = lambda ta, d_sgn: cb_flip_rel_component(ta, 3), direction=hy.event_direction.positive)
ev_right = hy.t_event(state_rel_sym[0] + box_size / 2, callback = lambda ta, d_sgn: cb_flip_rel_component(ta, 3), direction=hy.event_direction.negative)
ev_front = hy.t_event(state_rel_sym[1] - trailing - box_size / 2, callback = lambda ta, d_sgn: cb_flip_rel_component(ta, 4), direction=hy.event_direction.positive)
ev_back = hy.t_event(state_rel_sym[1] - trailing + box_size / 2, callback = lambda ta, d_sgn: cb_flip_rel_component(ta, 4), direction=hy.event_direction.negative)
ev_top = hy.t_event(state_rel_sym[2] - hover - box_size / 2, callback = lambda ta, d_sgn: cb_flip_rel_component(ta, 5), direction=hy.event_direction.positive)
ev_bottom = hy.t_event(state_rel_sym[2] - hover + box_size / 2, callback = lambda ta, d_sgn: cb_flip_rel_component(ta, 5), direction=hy.event_direction.negative)
# We define events when crossing the cube halves.
# Here the callback will cancel the velocity along that direction, so we need a cooldown
# As to avoid the event triggering immediately again.
ev_x = hy.t_event(state_rel_sym[0] , callback = lambda ta, d_sgn: cb_zero_rel_component(ta, 3), cooldown=20.)
ev_y = hy.t_event(state_rel_sym[1] - trailing , callback = lambda ta, d_sgn: cb_zero_rel_component(ta, 4), cooldown=20.)
ev_z = hy.t_event(state_rel_sym[2] - hover, callback = lambda ta, d_sgn: cb_zero_rel_component(ta, 5), cooldown=20.)
# We put all the control box events in a list as to pass them to the adaptive Taylor constructor.
bounce_events = [ev_top, ev_bottom, ev_front, ev_back, ev_left, ev_right]
# We put all the stop events in a list as to pass them to the adaptive Taylor constructor.
stop_events = [ev_x, ev_y, ev_z]
We may now instantiate the Taylor integrator, we use the compact mode here to obtain a faster compilation time, since the integration time is anyway very small:
ta = hy.taylor_adaptive(eqns, chief_ic + deputy_ic, t_events=bounce_events+stop_events, compact_mode=True)
And propagate, taking care to reset first the \(\Delta V\) count:
# Resetting the DV counter
DV = 0
# We propagate for one day
N = 1500
t_grid = np.linspace(0, 60*60*24, N)
oc, _, _, _, _, res = ta.propagate_grid(t_grid)
print("DV to maintain the formation for one day:", DV*1000, " m/s")
DV to maintain the formation for one day: 0.059948956216402026 m/s
We may now plot the trajectory as done above, first computing the LHLV Deputy position:
# We compute the results in the LHLV frame for the Deputy
deputy_rel = np.zeros((N,6))
for i, item in enumerate(res):
fh = J2_LHLV(item[:6], c)[2]
deputy_rel[i] = to_relative(item[:6], item[6:], fh)
… then plotting the trajectories:
# And plot
limit = N
fig = plt.figure(figsize = (9,9))
ax1 = fig.add_subplot(1, 2, 1, projection='3d')
ax2 = fig.add_subplot(1, 2, 2, projection='3d')
ax1.plot3D(res[:limit, 0], res[:limit, 1], res[:limit, 2])
ax1.set_title("Chief (inertial frame)")
ax2.plot3D(deputy_rel[:limit, 0], deputy_rel[:limit, 1], deputy_rel[:limit, 2])
ax2.set_title("Deputy (LHLV frame)")
ax2.set_xlim([-box_size/2, box_size/2])
ax2.set_ylim([trailing - box_size/2, trailing + box_size/2])
ax2.set_zlim([hover-box_size/2, hover + box_size/2]);
A different plot shows the x,y,z, components of the relative Deputy position along time:
fig, axs = plt.subplots(1, 3, figsize=(13,4))
axs[0].plot(t_grid[:limit], deputy_rel[:limit, 0])
axs[0].hlines([ - box_size / 2, + box_size / 2], 0, t_grid[:limit][-1], 'k')
axs[1].plot(t_grid[:limit], deputy_rel[:limit, 1])
axs[1].hlines([ trailing - box_size / 2, trailing + box_size / 2], 0, t_grid[:limit][-1], 'k')
axs[2].plot(t_grid[:limit], deputy_rel[:limit, 2])
axs[2].hlines([hover - box_size / 2,hover + box_size / 2], 0, t_grid[:limit][-1], 'k');
Thats all folks!