In this tutorial we will develop a coordinated control strategy to keep a group of satellites in a minimum-thrust formation.
This will demonstrate several use-cases and useful features of Collimator:
In addition, the tutorial shows how the block diagram abstraction can be used to construct a simulation of a relatively complex controller.
We will implement the algorithm described in Ref [1], which deals with maintaining a local satellite formation undergoing several configuration changes.
Maintaining a formation of satellites is difficult in general because satellites following elliptic orbits do not maintain fixed relative positions.
However, it is possible to determine certain special formations in which the relative positions can be maintained with minimal control effort [2].
For instance, a simple "in-plane" formation consists of two satellites on the same orbit but with different anomalies.
Given such a formation, the Clohessy-Wiltshire-Hill (CWH) equations can then be used to determine initial conditions and predict the relative motion.
The CWH equations are based on a linearization of the equations of motion in the local (noninertial) reference frame about a circular "reference" orbit.
Typically this represents the orbit of some other satellite, in which case it is natural to consider planning rendezvous maneuvers in this local frame.
However, the reference orbit can simply represent the notional "center" of the formation, whether or not there is a physical satellite at the local origin.
In either case, we will work with the equations of motion in the local frame.
Suppose the reference orbit in an Earth-fixed (i.e. approximately inertial) reference frame is
$$ X_\mathrm{ref}(t) = r_0 \cos \omega t, \qquad Y_\mathrm{ref}(t) = r_0 \sin \omega t, $$
with $\omega = \sqrt{\mu / r_0^3}$ and $\mu = 3.986 \times 10^{14}~\mathrm{m}^3/\mathrm{s}^2$.
This coordinate system is defined so that the $X-Y$ axes are in the plane of the reference orbit, with the $Z$ axis normal.
Next, define a local (non-inertial) reference frame $xyz$ centered on $(X_\mathrm{ref}(t), Y_\mathrm{ref}(t))$, with $y$ the along-track direction and $x$ pointing away from the center of Earth.
The equations of motion for a satellite moving in this local frame are
$$ \begin{gathered} \ddot{x} - 2 \omega \dot{y} - \omega^2 (r_0 + x) \left\{ 1 - r_0^3 \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} \right\} = u_1 \\ \ddot{y} + 2 \omega \dot{x} - \omega^2 y \left\{ 1 - r_0^3 \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} \right\} = u_2 \\ \ddot{z} + \omega^2 r_0^3 z \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} = u_3, \end{gathered} \tag{1} $$
where $\mathbf{u} = \begin{bmatrix} u_1 & u_2 & u_3 \end{bmatrix}^T$ are the control inputs.
Although satellites will follow elliptical orbits in the inertial frame, this is not necessarily the case in the local frame, unless the orbits are designed carefully and controlled.
The CWH equations are the linearization of (1):
$$ \begin{gathered} \ddot{x} - 2 \omega \dot{y} - 3 \omega^2 x = u_1 \\ \ddot{y} + 2 \omega \dot{x} = u_2 \\ \ddot{z} + \omega^2 z = u_3, \end{gathered} \tag{2} $$
which can also be written in the first-order form $\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u}$, with
$$ A = \begin{bmatrix} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 3\omega^2 & 0 & 0 & 0 & 2\omega & 0 \\ 0 & 0 & 0 & -2\omega & 0 & 0 \\ 0 & 0 & -\omega^2 & 0 & 0 & 0\end{bmatrix}, \qquad B = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{bmatrix} \tag{3} $$
The formation design approach taken in Ref [2] is to look for solutions to the CWH equations that satisfy certain constraints corresponding to desired formation geometries.
Although the true nonlinear dynamics will disturb the analytic trajectories, the idea is that these formations will still only require minimal formation-keeping effort.
One such solution corresponds to a circular orbit in the local frame (but elliptic in the Earth-referenced frame) inclined at $30^\circ$ to the cross-track/along-track plane, in which case the solution to the CWH equations is
$$ \begin{aligned} x(t) &= \frac{\dot{x}_0}{\omega} \sin \omega t + x_0 \cos \omega t \\ y(t) &= \frac{2\dot{x}_0}{\omega} \cos \omega t - 2x_0 \sin \omega t \\ z(t) &= \frac{\sqrt{3}\dot{x}_0}{\omega} \sin \omega t + \sqrt{3} x_0 \cos \omega t, \end{aligned} \tag{4} $$
which in turn fully specifies the initial conditions $\mathbf{x}_0 = \begin{bmatrix} x_0 & y_0 & z_0 & \dot{x}_0 & \dot{y}_0 & \dot{z}_0 \end{bmatrix}^T$.
Before we get into coordinated control of a multi-satellite formation, let's first tackle the problem of maintaining _one_ satellite in this orbit.
We'll start by simulating the linearized equations (2) to check that the solution matches what we expect, then compare with a simulation of the fully nonlinear equations (1).
We expect that the satellite will depart from the desired circular trajectory, so after simulating the natural dynamics we'll turn to the problem of developing a feedback controller.
The easiest way to simulate a linear time-invariant (LTI) system like (2) and (3) is to define the arrays in Python in a script in the Collimator project, then choose that script as the "init" script for the model:
Then the arrays and parameters will be available in the model and we can simulate the linearized system using a StateSpace block:
As expected, the solutions are periodic
To visualize the trajectory in local coordinates, let's use the Python API to run a simulation remotely and retrieve the results
We can also do some quick checks that the result from the StateSpace block matches the analytic solution result, for instance:
The difference between the analytic and numerical solutions is around $10^{-6}$, which is consistent with the tolerance of the continuous-time solver.
Next we'll add the nonlinear dynamics alongside the linear state-space model for comparison. SatDynamics3D implements equation (1) in blocks, but is abstracted into a submodel so we can reuse it easily.
As with the analytic solution, we can compare the linearized and nonlinear simulations using an Adder block. For the most part the relative errors are on the order of 0.1%, but the satellite does drift away from its target trajectory over time, especially in the local $y$-direction.
In order to stabilize the satellite in its target trajectory, we will develop a feedback controller.
However, before we get to that, let's take a moment to zoom out and consider how this relates to the larger goal of distributed control of the satellite formation.
We started by defining a circular "reference trajectory" around the Earth and using as our basic model the nonlinear equations of motion for a satellite in the local reference frame defined by that reference trajectory.
Ultimately our goal is to define a multi-satellite formation in the local frame which can be maintained using minimal thrust.
In order to find such a formation, we looked for initial conditions for the Clohessy-Wiltshire-Hill equations corresponding to circular solutions.
These initial conditions define a particular elliptical orbit around the Earth which appears as a circular trajectory in the local frame.
If the satellite is initialized perfectly with these initial conditions, it will stay relatively close to its target trajectory.
However, we will want to introduce feedback control for three reasons:
Following Ref [1], we will use a strategy of feedback linearization to stabilize this orbit. First, we define a nonlinear control law $\mathbf{u} = \mathbf{u}(\mathbf{x}, \mathbf{u}')$ such that the dynamics _become_ the CWH equations $\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u}'$, with $A$ and $B$ given by equation (3). The linearization control is
$$ \begin{aligned} u_1 &= -\omega^2 (r_0 + x) \tilde{r} + 3 \omega^2 x + u_1' \\ u_2 &= -\omega^2 y \tilde{r} + u_2' \\ u_3 &= -\omega^2 z \tilde{r} + u_3', \end{aligned} $$
where $\tilde{r} = \left\{ 1 - r_0^3 \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} \right\}$.
Substituting this into the nonlinear equations of motion (1) results in the much simpler LTI system.
We still need to determine the secondary control inputs $\mathbf{u}'$, but now we can use any linear control design technique.
For this case we will use a simple LQR controller with a strong actuation penalty. This is easy to do with the `python-control` library. All we have to do is add a few lines to our "init" script:
Now the secondary controls $\mathbf{u}'$ can be determined from the linear feedback law $\mathbf{u}' = -K \left(\mathbf{x} - \mathbf{x}_d(t) \right)$, where $\mathbf{x}_d(t)$ is the "desired" trajectory given by equation (4).
Here's what the whole feedback linearization controller looks like:
Note that the state is labeled `q` in the block diagram to distinguish from the variable $x$.
The TargetTrajectory block generates the analytic solution `q_ref` using equation (4).
The LQRController3D block then uses the LQR feedback law `up = -K @ (q - q_ref)` to determine the secondary control inputs $\mathbf{u}'$.
Finally, the FeedbackLinearization3D block combines the secondary controls $\mathbf{u}'$ with the state to determine the actual control inputs $\mathbf{u}$ that are fed back to the system.
This feedback linearization approach is a convenient way to define an effectively nonlinear controller using basic linear systems tools like LQR design.
Since the target state is time-varying, there is a persistent ~0.1% relative tracking error (which equates to a few meters of absolute error).
Between this error and the fact that the thrust is not tightly regulated, the LQR controller is not ideal for real applications.
However, it is a good illustration of a subsystem-level controller that can be combined with the top-level coordination we will explore next.
Now the controlled nonlinear trajectory (yellow) stabilizes to the target trajectory (blue):
Now that we understand how to keep a satellite on a particular trajectory in the local reference frame, we can consider the problem of formation control.
Let's say we have a group of $N$ satellites that we want to maintain in a distributed, low-thrust, collision-free configuration.
One approach to achieving this is to use a centralized planning approach such as trajectory optimization.
Nonlinear trajectory optimization has many advantages, including the ability to explicitly set constraints such as minimum pairwise distances and maximum thrust commands.
However, a proper implementation of this type of centralized open-loop control can be difficult, particularly if there is any state uncertainty or model error.
Coordinated control offers a straightforward alternative that requires minimal communication between the subsystems (individual satellites) and allows for the use of standard feedback control at the subsystem level.
The approach described by Ref [1] is to use a "reference projection" that essentially moves the target trajectory from being absolute (e.g. referenced to time) to being relative (e.g. referenced to a phase difference).
As a simple concrete example, take a circular formation of two satellites A and B, where B is intended to follow A with a phase lag of $\phi$.
We can extract the phase $\theta_A$ of satellite A in the circular trajectory with
$$ \theta_A = \tan^{-1} \left( \frac{x_A + \sqrt{3} z_A}{2 y_A} \right) $$
and likewise for $\theta_B$.
Tracking an absolute target trajectory involves feedback control of phase differences like $(\theta_A - \omega t)$ and $(\theta_B - \omega t + \phi)$.
On the other hand, suppose we replace the absolute target phase $\omega t (+ \phi)$ in each phase difference with reference projections $\gamma$, where $\gamma_A = \theta_A $ and $\gamma_B = \theta_A - \phi$.
The reference-projected phase difference vanishes for satellite A since $\theta_A - \gamma_A = 0$, and for satellite B the phase difference simplifies to $\theta_B - (\theta_A - \phi)$.
In other words, satellite A is the "leader" of the formation in that it may have any phase and no feedback control is applied, but satellite B uses feedback control to approach the target phase difference of $\phi$.
The formation is therefore controlled without centralized planning of the dynamics and control of each subsystem.
Instead, the reference projection simply acts as a top-level coordination between the subsystems that can be used to easily specify different formations.
This approach can be extended to any number of satellites, with any number of leader/follower groups defining sub-formations.
Since the target trajectory is a circle, the reference projection requires both a phase and a radius variable.
A simple way to define a variety of formations is to define the target radius variables $\boldsymbol{\gamma}_r$ as absolutes and the phase variables $\boldsymbol{\gamma}_\theta$ relative to a leader satellite: $\boldsymbol{\gamma}_\theta = Y \boldsymbol{\theta} + \mathbf{b}$, where $Y$ is a matrix that defines the phase combinations.
In the simple two-satellite example above, the radius of the target circular orbit is $x_0$, so $\boldsymbol{\gamma}_r = \begin{bmatrix} x_0 & x_0 \end{bmatrix}^T $ and
$$ \boldsymbol{\gamma}_\theta = \begin{bmatrix} 1 & 0 \\ -1 & 1 \end{bmatrix} \begin{bmatrix} \theta_A \\ \theta_B \end{bmatrix} + \begin{bmatrix} 0 \\ \phi \end{bmatrix} $$
This approach can also be used to switch between multiple formations by defining $\{Y^{(i)}, \mathbf{b}^{(i)}, \boldsymbol{\gamma}_r^{(i)}\}$ for each stage and switching between formation specifications using a StateMachine or predefined transition times.
The subsystem-level LQR controllers will then use feedback control based on the reference projections to steer the satellites into the new formation.
Extending the single-satellite model to multi-satellite formation control in Collimator is straightforward, but it does require a few modifications.
Here's what the full model looks like:
Let's break down what each component is doing.
The Replicator block copies a Group or Submodel $N$ times.
In our case the Group contains essentially the same model as we used above for the single satellite with feedback linearization and LQR control.
Each copy represents an individual satellite model:
Note that the Submodel blocks LQRController3D, FeedbackLinearization3D, and SatDynamics3D are all reused from the single-satellite control case we developed above.
Importantly, the information fed to each replicated satellite model is only about that individual satellite and not global information about the entire formation.
The Replicator block expects inputs with leading dimension $N$ and then feeds the $i$-th entry of the inputs to the $i$-th copy.
To understand this, it is helpful to turn on the "Show dimensions" layer in Collimator:
Here we have $N=6$ satellites. Both the state `q` and reference projections `gamma` that are fed to the Replicator have leading dimension $N$, so that `q` has shape `(6,6)` since there are 6 degrees of freedom per satellite.
Similarly `gamma` has shape `(6,2)` since there are two reference projection variables.
Inside the Replicator, each subsystem only sees its own state and reference projection, so `q` has shape `(6)` and `gamma` has shape `(2)`.
If you're familiar with machine learning frameworks like PyTorch and Jax, you can think of this as a "batch dimension" over which the Replicator is vectorized.
The Replicator therefore nicely mirrors the structure of distributed or system-of-systems control problems, where each subsystem is primarily only aware of its own local conditions and there is only a limited amount of top-level coordination.
A simple PythonScript block specifies the desired formation in terms of the configuration $\{Y^{(i)}, \mathbf{b}^{(i)}, \boldsymbol{\gamma}_r^{(i)}\}$ defined above.
In a more realistic setting, we might use a StateMachine to transition between formations depending on task-dependent criteria (e.g. completed rendezvous or orbit-change maneuver), but for this tutorial we just predefine a series of transition times between seven formation stages:
These stages are defined with standard Python code in the Formation block, which returns the configuration specification `Y`, `b`, and `r`.
This Group block implements the calculation of the reference-projected variables $\gamma_\theta$ and $\gamma_r$ based on the configuration specification $\{Y \mathbf{b}, \boldsymbol{\gamma}_r\}$ and the current state.
This is the only step that requires full knowledge of the state of all satellites, and must be vectorized in order to pass the "batched" signal with leading dimension $N$ to the Replicator block.
For the sake of simplicity, the calculation of the phases $\theta$ is carried out in a simple PythonScript block. As with the Formation block, this could easily be rewritten as a CFunction for deployment.
Because we have represented the subsystem dynamics in the Replicator block, the state signal is matrix-valued. This is not a problem for the Integrator block, but the Pandas-based data collection and visualization tools in Collimator are not currently compatible with matrix-valued signals.
In order to collect the data locally for visualization, we can just Demux the signal along the first dimension, which produces $N$ vector signals corresponding to the individual satellites.
Similarly, we can calculate useful global observables like the local radius $r_i = \sqrt{x_i^2 + y_i^2 + z_i^2}$.
We can clearly see the stage transitions happening, as satellite 5 moves to the center $r_5 = 0$, then satellite 4 moves to the parking orbit, etc.
The non-monotonic convergence to new radial positions is a reflection of the nonlinear control law.
Intuitively, this is a result of working in a non-inertial reference frame.
Since each satellite is on an elliptic trajectory in the inertial frame, the most efficient path to the desired trajectory in the non-inertial frame may look unexpected.
Similarly, when the satellites redistribute themselves in a new stage, the configuration will temporarily depart from the circular trajectory in order to redistribute its phases.
To get a better visualization we can also run the simulation via the Python API as in the single-satellite case and use Matplotlib animation tools here.
If you want to build on this demo and experiment more with Collimator and system-of-systems control, here are a few things you could try:
As noted by Ref [1], the LQR controller directly penalizes the _secondary_ control inputs $u'$, but not the actual thrust commands $u$, which are modified by the feedback linearization.
However, fuel consumption is among the most important considerations when planning control strategies for orbital mechanics, so a more realistic subsystem control strategy would account for fuel use directly. To explore this, try:
Here we have constructed a control strategy based on a desired circular formation in the local reference frame.
However, Ref [[2]](https://arc.aiaa.org/doi/10.2514/2.3681) describes three other formations: in-plane, in-track, and projected circular.
Depending on the application, a formation like in-track may be more relevant than the circular formation.
To explore this try:
The equations of motion (1) are highly idealized, neglecting many important astrodynamics effects.
While they may be reasonable for prototyping and controller design, it is often necessary to simulate at higher fidelity, for instance to quantify formation-keeping costs.
To explore this, try: