Spacecraft Constrained Maneuver Planning Using Positively Invariant

Constraint Admissible Sets

Avishai Weiss, Morgan Baldwin, R. Scott Erwin and Ilya Kolmanovsky

Abstract— The paper considers spacecraft motion planning

based on the use of safe positively invariant sets. In this

approach, a connectivity graph is constructed between a set

of forced equilibria, forming a virtual net that is centered

around a nominal orbital position. The connectivity between

two equilibria is determined based on safe positively invariant

sets in order to guarantee that transitions between equilibria

can be effected while spacecraft actuator limits are adhered

to and debris collisions are avoided. A graph search algorithm

is implemented to find the shortest path around the debris.

Simulation results are presented that illustrate this approach.

I. INTRODUCTION

With the growing amount of debris in Earth orbit, space-craft maneuver planning procedures have to address debrisavoidance requirements. While obstacle avoidance is a stan-dard problem in robotics [2], [3], the related spacecraftproblems have several unique features. In particular, thespace environment is relatively uncluttered, thus permittingfor a variety of maneuvers. Spacecraft dynamics are quitedifferent from those of typical robots. Maneuver efficiencywith respect to time and fuel consumption is a criticalconsideration. The states of the spacecraft and the debris canonly be estimated, often with a significant estimation error.Finally, computational algorithms must be fast and optimizedgiven moving objects and the limited computing power on-board most spacecraft. These unique features of spacecraftmaneuver planning problems provide the motivation for thedevelopment of specialized algorithms.

In [1], we have introduced an on-board maneuver planningapproach based on the use of constraint-admissible positivelyinvariant sets to determine connectivity between a set offorced and unforced spacecraft equilibria forming a virtualnet in the vicinity of the spacecraft. Two equilibria are con-nected if a choice of a Linear Quadratic (LQ) feedback gaincan be made that results in a transition between the equilibriawhich avoids the debris collision and satisfies the limits onthrust. The connectivity graph for all the equilibria in thenet is constructed and real-time graph search algorithms areused to optimize an equilibria hopping sequence that avoidsthe debris collisions.

Unlike the open-loop trajectory optimization approaches,we do not rely on precise assignment of spacecraft position

A. Weiss is a Graduate Student, Department of Aerospace Engineering,The University of Michigan, Ann Arbor, MI.

M. Baldwin is a Research Aerospace Engineer, Space Vehicles Direc-torate, Air Force Research Laboratory, Albuquerque, NM.

R. Scott Erwin is a Principal Research Aerospace Engineer, SpaceVehicles Directorate, Air Force Research Laboratory, Albuquerque, NM.

I. Kolmanovsky is a Professor, Department of Aerospace Engineering,The University of Michigan, Ann Arbor, MI.

to the time instants along the trajectory, but instead switch tothe next set-point and controller gain once appropriate con-ditions are satisfied. While this approach is conservative, itfacilitates fault-tolerant and disturbance-tolerant execution ofthe maneuvers. Furthermore, by using disturbance-invariantsets [13] in the construction, we can assure robustness tounmeasured (but set-bounded) disturbances and uncertain-ties. This extension to handling unmeasured disturbances anduncertainties using techniques of [13] is not pursued here andis left to a longer version of this paper.

To facilitate the on-board computations of the connec-tivity graph, a fast growth distance computation procedurebetween two ellipsoidal sets has been proposed in [1]. Inthis approach, using the Karush-Kuhn-Tucker conditions, thegrowth distance computations are reduced to a root findingproblem for the scalar value of the Lagrange multiplier. Thena predictor-corrector dynamic Newton-Raphson algorithmis used to update the Lagrange multiplier thereby rapidlyestimating the growth distance from different equilibria inthe virtual net to the debris.

In this paper, we incorporate limited thrust requirementsinto the computation of thrust limit on the growth distance,and we simulate maneuvers that adhere to the limited thrustconstraints. Even though the computation of thrust limitson the growth distance can be performed offline for thenominal operating conditions, fast computational proceduresare beneficial in case of thruster failures, degradations, andrestrictions on thrust directions (e.g., caused by the presenceof other spacecraft nearby), all of which can lead to changingconstraints on thrust during spacecraft missions. If the thrustlimits are prescribed in the form of 2-norm bounds, theoptimization problem involved in computing the thrust limiton the growth distance is non-convex (unlike computing thegrowth distance itself). Consequently, we advocate the useof polyhedral norm bounds on thrust, that lend themselvesto explicit solutions.

The related literature on spacecraft trajectory optimizationwith obstacle/debris avoidance is surveyed in [1]. Previousresearch addresses topics in spacecraft trajectory optimiza-tion [5], collision avoidance strategies based on risk as-sessment [6], the use of artificial potential functions [7],[8], and the use of conventional and mixed integer linearprogramming techniques [9], [10], [11], [12].

The paper is organized as follows. In Section II we discussthe nonlinear and the linearized models used to representspacecraft relative motion dynamics. In Section III we reviewour approach to constructing the virtual net based on a setof forced and unforced equilibria and using this virtual net

for debris avoidance. The procedure to compute the thrustlimit on the growth distance is presented. Simulation resultsare reported in Section IV. Finally, concluding remarks aremade in Section V.

II. RELATIVE MOTION MODEL

The spacecraft relative motion model represents the space-craft dynamics in the (non-inertial) Hill’s frame with theorigin at a target location on a nominal circular orbit. Forsmall distances, the linearization of the relative motion modelgiven by the Hill-Clohessy-Wiltshire (CWH) equations isused [14].

A. Nonlinear equations of motion

The relative position vector of the spacecraft with respectto a target location on a circular orbit is expressed as

�~r = xı+ yı+ zˆk,

where x, y and z are the components of the position vectorof the spacecraft relative to the target location and ı, |, ˆk arethe unit vectors of the Hill’s frame. The Hill’s frame has itsx-axis along the orbital radius, y-axis along the orbital track,and z-axis is orthogonal to orbital plane.

The position vector of the spacecraft with respect to thecenter of the Earth can be expressed as ~R =

~R0 + �~r =

(R0+x)ı+y|+ zˆk, where R0 is the nominal orbital radius.The nonlinear equations of motion for the spacecraft (relativeto an inertial frame) can be expressed in vector form as

¨~R = �µ~R

R3+

1

mc

~F , (1)

where ~F is the vector of external forces applied to thespacecraft, R = |~R|, m

c

is the mass of the spacecraft, µis the gravitational constant, and

¨~R = (x� 2ny � 3n2x)ı+ (y + 2nx)|+ (z + n2z)ˆk.

In these equations, n =

qµ

R

30

denotes the mean motion ofthe nominal orbit. Similar equations can be used to describethe motion of the debris.

B. Linearized HCW equations in discrete-time

For �r << R, the linearized Hill-Clohessy-Wiltshire(HCW) equations [14] approximate the relative motion ofthe spacecraft on a circular orbit as

x� 3n2x� 2ny =

Fx

mc

,

y + 2nx =

Fy

mc

,

z + n2z =

Fz

mc

,

(2)

where Fx

, Fy

, Fz

are components of the external force vector(excluding gravity) acting on the spacecraft. The linearizeddynamics account for differences in gravity between thespacecraft and nominal orbital location, and for relativemotion effects. The spacecraft relative motion dynamics inthe orbital plane (x and y) and in the out-of-orbital plane

(z) are decoupled. The in-plane dynamics are Lyapunovunstable, while the out-of-plane dynamics are Lyapunovstable. The in-plane dynamics are completely controllablefrom F

y

input but are not controllable from Fx

input. Theout-of-plane dynamics are controllable from F

z

input. Thesedynamics are thus different from typical robots.

Assuming a sampling period of �T sec, we can convertthe model (2) to a discrete-time form

X(t+ 1) = AX(t) +BU(t), (3)

where X(t) = [x(t), y(t), z(t), x(t), y(t), z(t)]T isthe state vector at the time instant t 2 Z+, U(t) =

[Fx

(t), Fy

(t), Fz

(t)]T is the control vector of thrust forcesat the time instant t 2 Z+, and A = exp(A

c

�T ), B =R�T

0 exp(Ac

(�T � ⌧))d⌧Bc

are the discretized matricesobtained based on the continuous-time system realization(A

c

, Bc

) . Alternatively, the control vector U can representan instantaneous change in the velocity of the spacecraft, �v,induced by thrust, with an appropriately re-defined B-matrix,

B�v

= eAc�T

2

6666664

0 0 0

0 0 0

0 0 0

1 0 0

0 1 0

0 0 1

3

7777775.

III. DEBRIS AVOIDANCE BASED ON A VIRTUAL NET

Our approach to debris avoidance is based on utilizingconstraint-admissible positively invariant sets [13], [15] cen-tered around the spacecraft forced and unforced equilibria.A finite set of these equilibria used for constructing debrisavoidance maneuvers is referred to as a virtual net. Givenan estimate of the debris position, we build a connectivitygraph that identifies the equilibria in the virtual net betweenwhich the spacecraft can move, with guaranteed collision-free motion and within the available thrust authority. We thenemploy graph search to determine an efficient path betweenthe equilibria that ensures debris avoidance.

A. Virtual Net

The virtual net comprises a finite set of equilibria, Xe

(r),corresponding to a finite set of prescribed spacecraft posi-tions r 2 N = {r1, r2, . . . , rn} ⇢ R3,

Xe

(rk

) =

rk

0

�=

2

6666664

rkx

rky

rkz

0

0

0

3

7777775, k = 1, · · · , n, (4)

whose velocity states are zero, and where n is the numberof equilibria in the virtual net. See Figure 1. We assume thatfor all r 2 N , the corresponding values of control necessaryto support the specified equilibria in steady-state satisfy theimposed thrust limits.

Debris

Fig. 1: The virtual net for debris avoidance. Dots correspondto positions at equilibria, X

e

(r), on a virtual net. Theellipsoid represents the debris position and uncertainty.

B. LQ Controller with Gain Switching

A conventional Linear-Quadratic (LQ) feedback is used tocontrol the spacecraft to a commanded equilibrium in (4),

U = K(X �Xe

(r)) + �r = KX +H(K)r, (5)

where

� =

2

4�3n2m

c

0 0

0 0 0

0 0 n2mc

3

5 ,

H(K) = ��K

I303

�,

and where I3 denotes the 3 ⇥ 3 identity matrix and 03

denotes the 3 ⇥ 3 zero matrix. This LQ controller providesan asymptotically stable closed-loop system but does notenforce the constraints.

To provide greater flexibility in handling constraints, amultimode controller architecture is employed [15]. Specif-ically, we assume that a finite set of LQ gains K 2 K =

{K1, · · · ,Km

} is available to control the spacecraft. Byusing a large control weight in the LQ cost functional,motions with low fuel consumption yet large excursions canbe generated; using a large control weight in the LQ cost,motions with short transition time can be generated [16]. Weassume that a preference ordering has been defined and thegains are arranged in the order of descending preference,from K1 being the highest preference gain to K

m

being thelowest preference gain.

C. Positively Invariant Sets

The ellipsoidal set

¯C(r,K) =

{X 2 R6:

1

2

(X �Xe

(r))TP (K)(X �Xe

(r)) 1} ⇢ R6,

(6)

where (A + BK)

TP (A + BK) � P < 0, P = P (K) > 0,is positively invariant. Positive invariance implies that anytrajectory of the closed-loop system that starts in ¯C(r,K) isguaranteed to stay in ¯C(r,K) as long as the same LQ gainK is used and the set-point command r is maintained. To

achieve the positive invariance, the matrix P can be obtainedas the solution of the discrete-time Riccati equation or of theabove Lyapunov equation for the closed-loop asymptoticallystable system. We note that, because the system is linear, thepositive invariance of ¯C(r,K) implies the positive invarianceof the scaled set

C(r,K, ⇢) = {X 2 R6:

1

2

(X �Xe

(r))TP (K)(X �Xe

(r))

⇢2}, ⇢ � 0.

Geometrically, the set C(r,K, ⇢) corresponds to an ellipsoidscaled by the value of ⇢ and centered around X

e

(r), r 2 N .

D. Debris Representation

We use a set, O(z,Q), centered around the position z 2R3, to over-bound the position of the debris, i.e.,

O(z,Q) = {X 2 R6: (SX � z)TQ(SX � z) 1}, (7)

where Q = QT > 0 and

S =

2

41 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

3

5 . (8)

The set O(z,Q) can account for the debris and spacecraftphysical sizes and also for the uncertainties in the estimationof the debris/spacecraft position. Note that the set O(z,Q)

has an ellipsoidal shape in the position directions and it hasa cylindrical shape in the velocity directions. Ellipsoidal setsrather than polyhedral sets are used to over-bound the debrissince ellipsoidal bounds are typically produced by positionestimation algorithms, such as the Extended Kalman Filter(EKF).

E. Debris Avoidance Approach

Consider now ri

2 N , representing a possible positionon the net that the spacecraft can move to as a part of thedebris avoidance maneuver. Suppose that the current state ofthe spacecraft is X(t0) at the time instant t0 2 Z+. If thereexists a ⇢ � 0 and K

j

2 K such that

X(t0) 2 C(ri

,Kj

, ⇢) and O(z,Q) \C(ri

,Kj

, ⇢) = ;, (9)

the spacecraft can move to the position ri

2 N by engagingthe control law with r(t) = r

i

and K(t) = Kj

, t � t0,and without hitting the debris confined to O(z,Q). Thisidea underlies our subsequent approach to debris avoidance,where we maintain the spacecraft trajectories within the tubeformed by positively invariant sets that do not overlap withthe debris.

To avoid a non-stationary debris, its path can be coveredby a union of a finite number of sets of ellipsoidal shape,

D =

l=nd[

l=1

O(zl

, Ql

), (10)

where the center of the lth set is denoted by zl

2 R3 and thelth set shape is defined by Q

l

= QTl

> 0. Then the debris

path avoidance condition is

X(t0) 2 C(ri

,Kj

, ⇢) and O(zl

, Ql

) \ C(ri

,Kj

, ⇢) = ;,for all l = 1, · · · , n

d

.(11)

The same approach, with larger nd

, can be used to handlemultiple non-stationary debris. Note, however, that this ap-proach is conservative as it does not account for the debrisprogression along its path.

F. Growth Distances

The minimum value of ⇢ � 0 for whichO(z,Q)

TC(r,K, ⇢) 6= ; is referred to as the growth

distance, similarly to [17]. This growth distance can alsobe viewed as the least upper bound on the values of ⇢for which O(z,Q) and C(r,K, ⇢) do not intersect. SeeFigure 2. We use the notation ⇢

g

(r,K,Q, z) to reflect thedependence of the growth distance on the set-point r 2 N ,the control gain K 2 K and the obstacle parameters Q andz.

Note that the growth distance depends on the positionof the debris which may be unknown in advance. Con-sequently, the growth distance computations have to beperformed online. The computations of the growth distance,⇢g

(r,K,Q, z), have been addressed in [1]; for completeness,these computations are summarized in this paper.

Debris

zO(z,Q)

ri

S C(ri ,K,x

S

Fig. 2: The positively invariant set is grown till touching thedebris. The spacecraft can move from any of the equilibria onthe virtual net inside the positively invariant set C(r,K, ⇢)to X

e

(ri

) marked by ’x’ without colliding with the debris.

Since the spacecraft maneuvers have to be performed usinglimited thrust, we additionally define a maximum value of⇢ = ⇢

u

(r,K) for which X 2 C(r,K, ⇢u

(r,K)) implies thatthe thrust U = KX + H(K)r satisfies the imposed thrustlimits. We refer to ⇢

u

as the thrust limit on growth distance.Unlike ⇢

g

, the value of ⇢u

does not depend on the positionor shape of the debris and can be pre-computed off-line.

Finally, we define the thrust limited growth distance

⇢⇤(r,K,Q, z) = min{⇢g

(r,K,Q, z), ⇢u

(r,K)}. (12)

Note that X(t0) 2 C(ri

,Kj

, ⇢⇤(ri

,Kj

, z)) implies that theensuing closed-loop spacecraft trajectory under the control(5) with r(t) = r

i

and K(t) = Kj

for t � t0 satisfies the

thrust limits and avoids debris collisions for a debris confinedto O(z,Q).

The above definitions were given for the case of a singlestationary debris, O(z,Q). In the case of multiple debris orwhen avoiding a predicted debris path based on (10), thegrowth distance is replaced by the multi-growth distance,which is the minimum growth distance to each of O(z

l

, Ql

),l = 1, · · · , n

d

.

G. Growth Distance Computations

Define ¯X = X � Xe

(r) and ↵ = 2⇢2. The problem ofdetermining the growth distance ⇢

g

(r,K,Q, z), reduces tothe following constrained optimization problem:

min

↵,X

↵

subject to¯XTP ¯X ↵,

((S( ¯X +Xe

(r))� z)TQ((S( ¯X +Xe

(r))� z) 1.(13)

To solve this optimization problem, we use the Karush-Kuhn-Tucker (KKT) conditions. Note that the standard linearindependence constraint qualification conditions hold giventhat P > 0. We define

L = ↵+ �1(¯XTP ¯X � ↵)

+�2((S( ¯X +Xe

(r))� z)TQ(S( ¯X +Xe

(r))� z)� 1),

where �1 and �2 are Lagrange multipliers. The stationarityof the Lagrangian (setting partial derivative equal to zero)with respect to ↵ yields �1 = 1. The stationarity of theLagrangian with respect to ¯X leads to [1]:

¯X =

¯X(�2, r, z) = �(P+�2STQS)�1STQ(SX

e

(r)�z)�2,(14)

where �2 � 0 is a scalar to be determined. Note that P > 0,STQS � 0, �2 � 0 (as the Lagrange multiplier correspond-ing to an inequality constraint) imply that (P + �2STQS)is invertible. The problem reduces to finding a nonnegativescalar �2 which is the root of the equation

F (�2, r, z) = ((SX � z)TQ(SX � z)� 1 = 0, (15)

where

X =

¯X(�2, r, z) +Xe

(r).

The scalar root finding problem (15) has to be solvedonline multiple times for different r 2 N , and in the caseof avoiding a predicted debris path also for different z’s. Tosolve this problem fast and to re-use the previously foundsolutions as approximations, in [1] we have proposed adynamic Newton-Raphson’s algorithm. This algorithm usespredictor-corrector updates to track the root as a function of

z and r:

�k+1,+2 = �k

2 + { @F@�2

(�k

2 , zk, rk)}�1{�F (�k

2 , zk, rk)

� @F

@z(�k

2 , zk, rk)(zk+1 � zk)

� @F

@r(�k

2 , zk, rk)(rk+1 � rk)},

�k+12 = max{0,�k+1,+

2 }.(16)

To implement the algorithm, we take advantage of knownfunctional form for F and compute explicitly the partialderivatives,

@ ¯X

@�2= (P + �2S

TQS)�1��STQ(SX

e

(r)� z) ,

@F

@�2= 2(S ¯X � r)TQ(S

@ ¯X

@�2),

@ ¯X

@r= (P + �2S

TQS)�1��STQS⌦

�2,

@F

@r= 2(S ¯X � z + r)TQ(S

@ ¯X

@r+ I3),

@ ¯X

@z= (P + �2S

TQS)�1STQS⌦�2,

@F

@z= 2(S ¯X � z + r)TQ(S

@ ¯X

@z� I3).

In these equations,

Xe

(r) = ⌦r, where ⌦ =

I30

�,

and I3 denotes the 3⇥ 3 identity matrix.Figures 4-5 illustrate the growth distance tracking in the

case when z = [0, 0, 0]T, Q = 4I3, and P = PT > 0 isdetermined as a solution to the Lyapunov equation,

P =0

BBBBB@

530.67 0.066 0 �14.97 �0.69 00.066 531.17 0 0.69 �15.03 00 0 531.04 0 0 �15.06

�14.97 0.69 0 1.43 0 0�0.69 �15.03 0 0 1.43 0

0 0 �15.06 0 0 1.43

1

CCCCCA.

For the first 20 iterations, rk is a constant to enable initialconvergence of the algorithm to take place. Then rk startsto vary through the virtual net, see Figure 3. One iterationof the Newton-Raphson algorithm per value of rk is usedto update the root, �k+1

2 . Figure 4 demonstrates that thegrowth distance tracking is accurate. The growth distanceoccasionally becomes zero indicating the overlap betweenthe debris and several of rk. This is confirmed from Figure 5,which illustrates the trajectory of rk in the three dimensionalHills’ frame relative to the debris.

0 50 100 150 200−1

−0.5

0

0.5

1

Iteration Number

r x,r y,r z

rxryrz

Fig. 3: Components of r, rx

, ry

and rz

varying versus theiteration number.

0 50 100 150 2000

2

4

6

8

Iteration NumberG

row

th d

ista

nce

approxactual (fmincon)

Fig. 4: Growth distance versus iteration number computedby dynamic Newton-Raphson algorithm.

H. Thrust Limit on Growth Distance Computations

In [1], the following procedure to compute ⇢u

(r,K) hasbeen discussed. Suppose that the thrust limits are expressedin the form ||LU || 1 for an appropriately defined matrix Land norm || · ||. The computational procedures to determine⇢u

(r,K) involves solving a bilevel optimization problemwhere ||L(KX + H(K)r)|| is maximized subject to theconstraint X 2 C(r,K,↵) and bisections are performedon the value of ↵ so that the maximum value is driven to1. As we demonstrate in this paper, in special cases thiscomputation can be greatly simplified.

Fig. 5: The trajectory of r and the debris.

Suppose that the thrust constraints are prescribed in termsof polyhedral norm bounds, specifically, as

eTi

(KX +Hr) umax

, i = 1, 2, · · · ,m, (17)

where ei

are the vertices of the unit norm polytope and umax

is the norm bound. The infinity norm, for instance, has m =

6, and

e1 =

2

41

0

0

3

5 e2 =

2

4�1

0

0

3

5 e3 =

2

40

1

0

3

5

e4 =

2

40

�1

0

3

5 e5 =

2

40

0

1

3

5 e6 =

2

40

0

�1

3

5 . (18)

In the case of non-polyhedral norm bounds, such as the 2-norm, an approximation by a polyhedral norm bound can beemployed.

Then, the determination of the thrust limit on the growthdistance is based on solving a sequence of the followingoptimization problems for i = 1, · · · , n,

eTi

(KX +Hr) ! max, (19)

subject to1

2

(X �Xe

(r))TP (X �Xe

(r)) c. (20)

If the value of c is found for which the solutions of (19)-(20), X⇤

i

, satisfy max

i

{eTi

(KX⇤i

+ Hr)} = umax

, then⇢u

(r,K) =

pc. The problem (19)-(20) can be solved by

diagonalizing P , using an orthogonal matrix, V ,

P = V T⇤V, ⇤ = diag[�2

1, · · · ,�26],�i

> 0.

By defining, z = X �Xe

(r), and ⇣ so that

z = V T⇤

� 12 ⇣,

it follows that

zTPz = ⇣T⇤� 12V PV T

⇤

� 12 ⇣

= ⇣T⇣.

The problem (19)-(20) can now be re-written as

hTi

⇣ + eTi

�r ! max, (21)

subject to1

2

⇣T⇣ c, (22)

wherehTi

= eTi

KV T⇤

� 12 .

The solution to the constrained maximization problem(21)-(22) of maximizing the inner-product of two vectorsover a unit 2-norm ball is given by

⇣i

=

hi

||hi

||p2c, (23)

where || · || denotes the vector 2-norm. The maximum valueof the objective function is

||hi

||p2c+ eT

i

�r.

Consequently, to satisfy (17), we use the following relations:

c = 0 if for any i, umax

eTi

�r.

c = min

i

1

2

(

umax

� eTi

�r

||hi

|| )

2 otherwise.(24)

Thus the problem of finding the thrust limit on the growthdistance, when polyhedral norm bounds on thrust are em-ployed, has an explicit solution given by (24).

We note that the condition umax

� max

i

{eTi

�r} issatisfied if the available thrust can maintain the equilibriumX

e

(r) in steady-state. We also note that, based on the formof �, c is independent of r

y

, the in-track component of theequilibrium in the virtual net. Hence the computations of⇢u

(r,K) need to be only performed with ry

= 0.In the case when the spacecraft does not have independent

thrusters in x, y and z directions, the 2-norm thrust limitis more practical. Unfortunately, maximizing a quadraticfunction in (19) subject to (20) is, in general, a non-convexproblem. In this case, the 2-norm bound can be approximatedby a polyhedral norm bound (17), with the vertices e

i

selected on the unit 2-norm ball in R3. We note that higheraccuracy of this approximation requires a higher number ofvertices in (17), which thus, complicates (24).

Finally, we note that in the case the �v’s are treated ascontrol inputs, the thrust limit on the growth distance isinduced by the available �v. Computing the thrust limit onthe growth distance in this case is completely analogous tocomputing it in the case when the control input is the thrustforce or thrust acceleration.

I. Connectivity Graph and Graph Search

We now introduce a notion of connectivity between twovertices of the virtual net, r

i

2 N and rj

2 N . The vertexri

is connected to the vertex rj

if there exists a gain K 2 Ksuch that

Xe

(ri

) 2 intC(rj

,K, ⇢⇤(rj

,K, z)), (25)

where int denotes the interior of a set. The connectivityimplies that a spacecraft located close to an equilibriumcorresponding to r

i

can transition to an equilibrium Xe

(rj

)

by using limited thrust and avoiding collision with the debris.We note that if r

i

is connected to rj

this does not imply that,in turn, r

j

is connected to ri

. We also note that connectivitydepends on the existence of an appropriate control gain fromthe set of gains K but the condition (25) does not need tohold for all gains.

The on-line motion planning with debris avoidance is per-formed according to the following procedure (for simplicity,described here for the case of a single debris):

Step 1: Determine the debris location and shape (i.e., zand Q).

Step 2: By using fast growth distance computations,determine thrust limited growth distance basedon (12), with ⇢

g

computed online and ⇢u

pre-computed off-line.

Step 3: Construct a graph connectivity matrix betweenall r

i

, rj

2 N . In the graph connectivity matrix, if

two vertices are not connected, the correspondingmatrix element is zero; if they are connected thecorresponding matrix element is 1. In parallel, buildthe control gain selectivity matrix, which identifiesthe index of the highest preference gain K forwhich r

i

and rj

are connected. This gain willbe applied if the edge connecting r

i

and rj

istraversed.

Step 4: Perform graph search to determine a sequenceof connected vertices r[k] 2 N and control gainsK[k] 2 K, k = 1, · · · , l

p

, such that r[1] satisfiesthe initial constraints, r[l

p

] satisfies the final con-straints, and the path length l

p

is minimized.Per the above algorithm, a graph search is utilized to

determine the minimum number of equilibrium hops arounda piece of debris. After the path has been determined asa sequence of the set-points and the corresponding controlgains, the execution of the path proceeds by checking ifthe current state, X(t) is in the safe positively invariant setcorresponding to the next reference r+ and next control gainK+ in the sequence; if it is, then the controller switches tothis reference and control gain:

X(t) 2 C(r+,K+, ⇢⇤(r+,K+, z)) ! r(t) = r+, K(t) = K+.

IV. SIMULATION RESULTS

Simulations are now provided that illustrate our debrisavoidance approach. We consider a nominal circular orbit of850 km and discretize the HCW equations with a samplingperiod, �T , of 30 seconds. We construct an approximately2 km cubed virtual net. We let R = 2 ⇥ 10

7I3 and Q =

diag(100, 100, 100, 107, 107, 107).An ellipsoidal set over-bounds a piece of debris,

O(z1, Q1), centered at z1 = [0.3 0.4 0.5]T km and withQ1 = 100I3. In this example, we use the fast distancecomputation technique of [1] based on bisections appliedto (15) to determine the growth distance to the debris fromeach node in the net. The spacecraft’s initial condition isX(0) = X

e

(r0), where r0 = [0.45 0 2.25]T. The targetequilibrium node is X

e

(0). Finally, we impose a maximumthrust constraint of 10 N in each axis. Dijkstra’s algorithm isused to find the shortest cost path from initial node to finalnode.

Figure 6 shows the path the spacecraft takes under closed-loop control in order to avoid the debris. The spacecraft isable to complete the desired maneuver well within maximumthrust constraints while successfully avoiding the debris.

In Figure 7 we rerun the above simulation for a grid ofinitial conditions. The maneuvers shown in Figure 7 clearlydemonstrate the initial conditions for which the maneuverpath is perturbed from that which the spacecraft would havetaken had there been no debris.

Finally, we add a second piece of debris, O(z2, Q2),centered at z2 = [�0.3 0.4 0.5]T and with Q2 = 100I3.In calculating the growth distance, we take the minimumdistance to each of O(z

i

, Qi

), i = 1, 2 . Figure 8 shows thepath the spacecraft takes under closed-loop control in orderto avoid the debris.

(a) Debris Avoidance Path.

(b) Norm of Thrust Profile.

Fig. 6: Debris Avoidance Path. The green x marks theinitial node. The blue x marks the final node. The redellipsoid represents the debris. The blue line is the paththe spacecraft takes in order to avoid the debris. The blueellipsoids represent the invariant sets along the path.

V. CONCLUSION

This paper described a technique for spacecraft maneuverplanning that uses positively-invariant sets in order to avoidcollisions with debris, while adhering to specified thrustlimits. The approach is based on hopping between neigh-borhoods of equilibria in a virtual net, and maintaining thespacecraft trajectory within a tube formed by safe positively-invariant sets. For the case where thrust limits can bespecified as polyhedral norm bounds, we have shown that thethrust limit on the growth distance can be easily computed;it is, in fact, feasible to perform these computations onboarda spacecraft in order to account for thruster failure ordegradation.

REFERENCES

[1] Baldwin, M., Weiss, A., Kolmanovsky, I., and Erwin, R.S.,“Spacecraftdebris avoidance using constraint admissible positively invariant sets,”Proceedings of AAS Space Flight Mechanics Meeting, Paper AAS 12-250, Charleston, South Carolina, 2012.

−0.50

0.51

−0.50

0.51

−2

−1

0

1

2

y (km)x (km)

z (k

m)

Fig. 7: Debris Avoidance Paths for many initial conditions.Each green x marks an intial condition. The blue x marksthe final node. The red ellipsoid represents the debris. Theblue lines are the paths that the spacecraft takes from eachinitial condition in order to avoid the debris. We do not showthe invariant set ellipsoids for visual clarity.

[2] LaValle, S.M., Planning Algorithms, Cambridge University Press,1996.

[3] Maia, M.H. and Galvao, R.K.H., “On the use of mixed-integer linearpredictive control with avoidance constraints,” International Journalof Robust and Nonlinear Control, Vol. 19, pp. 822-828.

[4] Ranieri, C“Path-constrained trajectory optimization for proximity op-erations,” AIAA/AAS Astrodynamics Specialist Conference, Honolulu,HI, August 2008.

[5] Epenoy, R., “Fuel optimization for continuous-thrust orbital ren-dezvous with collision avoidance constraint,” Journal of Guidance,Control, and Dynamics, Vol. 34, No. 2, March-April 2011.

[6] Sanchez-Ortiz, N., Bello-Mora, M., and Klinkrad, H., “Collisionavoidance manoeuvres during spacecraft mission lifetime: Risk re-duction and required � V,” Advances in Space Research, Vol. 38,2006.

[7] Martinson, N., Munoz, J., and Wiens, G., “A new method of guidancecontrol for autonomous rendezvous in a cluttered space environment,”Proceedings of AIAA Guidance, Navigation, and Control Conference,Hilton Head, SC, August 2007.

[8] Martinson, N.,“Obstacle avoidance guidance and control algorithmsfor spacecraft maneuvers,” Proceedings of AIAA Guidance, Naviga-tion, and Control Conference, Chicago, IL, August 2009.

[9] Mueller, J., Griesemer, P., and Thomas, S., “Avoidance maneuverplanning incorporating station-keeping constraints and automatic re-laxation,” AIAA Infotech@Aerospace 2010, Atlanta, GA, April 2010.

[10] Ulybyshev, Y., “Trajectory optimization for spacecraft proximity op-erations with constraints,” AIAA Guidance, Navigation, and ControlConference, Portland, OR, August 2011.

[11] Richards, A., Schouwenaars, T., How, J., and Feron, E., “Spacecrafttrajectory planning with avoidance constraints using mixed-integer lin-ear programming,” AIAA Journal of Guidance, Control, and Dynamics,Vol. 25, No. 4, August 2002.

[12] Mueller, J.,“Onboard planning of collision avoidance maneuvers usingrobust optimization,” AIAA Infotech@Aerospace 2009, Seattle, WA,April 2009.

[13] Kolmanovsky I.V., Gilbert E.G., “Theory and computation of distur-bance invariant sets for discrete-time linear systems,” MathematicalProblems in Engineering, Vol. 4(4), pp. 317-367, 1998.

[14] Wie, B., Spacecraft Dynamics and Control, 2nd Ed. Reston, VA:AIAA,2010.

[15] Kolmanovsky, I.V., and Gilbert, E., “Multimode regulators for systemswith state and control constraints and disturbance inputs,” Proceedingsof Workshop Control Using Logic-Based Switching, pp. 104-117,published in an edited volume by Springer-Verlag, Editor: Morse A.S,Block Island, Rhode Island, Fall, 1997.

[16] Park, H., Di Cairano, S., and Kolmanovsky, I.V., “Linear Quadratic

(a) Debris Avoidance Path.

(b) Norm of Thrust Profile.

Fig. 8: Debris Avoidance Path for 2 pieces of debris. Thegreen x marks the initial node. The blue x marks the finalnode. The red ellipsoid represents the debris. The blue line isthe path the spacecraft takes in order to avoid the debris. Theblue ellipsoids represent the invariant sets along the path.

Model Predictive Control approach to spacecraft rendezvous anddocking,” Proceedings of 21st AAS/AIAA Space Flight MechanicsMeeting, New Orleans, Louisiana, USA, Paper AAS-142, 2011.

[17] Ong C.J., Gilbert E.G., “Growth distances: New measures for objectseparation and penetration,” IEEE Transactions on Robotics andAutomation, Vol. 12(6), pp. 888-903, 1996.