Embed Size (px)
Citation preview
DEVELOPING A GENERAL FRAMEWORK FOR A TWO STAGED ARTIFICIAL POTENTIAL CONTROLLER
By
TRISTAN J. NEWMAN
A THESIS PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF
MASTER OF SCIENCE
UNIVERSITY OF FLORIDA
2015
© 2015 Tristan J. Newman
To my colleagues, friends, and family.
4
ACKNOWLEDGMENTS
I would like to thank my research advisor, Dr. Norman Fitz-Coy, who believed in
me from the beginning; Dr. Josue Muñoz, who offered me his fantastic mentorship
during a research fellowship in New Mexico; and Roxanne Rezaei, my lovely wife who
has graced me with her unwavering support throughout my research and my studies. I
would also like to thank the department of Mechanical and Aerospace Engineering for
affording me the opportunity to study and do research, and all of the faculty and staff
within it who worked so hard to ensure that I was equipped with all of the tools
necessary to see it through.
5
TABLE OF CONTENTS Page
ACKNOWLEDGMENTS .................................................................................................. 4
LIST OF TABLES ............................................................................................................ 7
LIST OF FIGURES .......................................................................................................... 8
LIST OF ABBREVIATIONS ............................................................................................. 9
ABSTRACT ................................................................................................................... 10
CHAPTER
1 INTRODUCTION ................................................................................................ 12
Foreword ............................................................................................................ 12 A Note on Notation ............................................................................................. 13 Background ........................................................................................................ 14
Optimal Controls ...................................................................................... 14 Workspace-Discretized Pathfinding ......................................................... 20 Method of Lyapunov ................................................................................ 22
2 MATHEMATICAL OVERVIEW ........................................................................... 26
Vector Operations in Linear Algebra ................................................................... 26 Body Rotations and Quaternion Math ................................................................. 28 Derivatives of (and With Respect to) Multi-Dimensional Quantities .................... 32
3 METHODOLOGY ............................................................................................... 35
Formal Definition of an APF................................................................................ 35 Defining the Error States .................................................................................... 37 The Unconstrained Case .................................................................................... 39 General Constraints ............................................................................................ 43
First Stage Repulsive Potential ................................................................ 44 Second Stage Repulsive Potential ........................................................... 49
4 IMPLEMENTATION ............................................................................................ 54
Notable Constraints ............................................................................................ 54 The Pointing Constraint ........................................................................... 54 The Slew Rate Constraint ........................................................................ 59 Early Avoidance ....................................................................................... 60
Developing a MatLab® Simulation ...................................................................... 70 Dynamics ................................................................................................. 70
6
Integration ................................................................................................ 71 Processing ............................................................................................... 73
5 CONCLUSION .................................................................................................... 75
A Quasi-Real World Example ............................................................................. 75 Unconstrained Simulation Results ........................................................... 79 Partially Constrained Simulation Results ................................................. 82 Fully Constrained Simulation Results ...................................................... 85
A Note on Second Stage Potentials ......................................................... 88 Topics for Future Research ................................................................................ 89
Considerations for Physical Actuation Devices ........................................ 89 The State Vector Escort Problem ............................................................ 92 Mitigation of Local Extrema ...................................................................... 93
Afterword ............................................................................................................ 99 APPENDIX
A USEFUL MATRIX DERIVATIVES .................................................................... 100
Proof 1 .............................................................................................................. 100 Proof 2 .............................................................................................................. 101 Proof 3 .............................................................................................................. 102 Proof 4 .............................................................................................................. 103 Proof 5 .............................................................................................................. 105 Proof 6 .............................................................................................................. 106 Proof 7 .............................................................................................................. 107 Proof 8 .............................................................................................................. 109 Proof 9 .............................................................................................................. 111
B MISCELLANEOUS PROOFS AND LEMMAS .................................................. 112
Proof 1 .............................................................................................................. 112 Proof 2 .............................................................................................................. 113 Proof 3 .............................................................................................................. 114 Proof 4 .............................................................................................................. 116 Proof 5 .............................................................................................................. 117 Proof 6 .............................................................................................................. 119 Proof 7 .............................................................................................................. 121 Proof 8 .............................................................................................................. 123
C MATLAB® SIMULATION CODE ....................................................................... 125
LIST OF REFERENCES ............................................................................................. 133
BIOGRAPHICAL SKETCH .......................................................................................... 135
7
LIST OF TABLES Table Page
2-1 Summary of derivatives discussed in Chapter 2 ................................................. 34
4-1 Logic term values across three regions in the gamma domain ........................... 66
8
LIST OF FIGURES Figure Page
1-1 Illustration of “attitude forbidden” and “attitude mandatory” zones ...................... 15
1-2 Example of A* pathfinding with workspace discretization ................................... 22
3-1 Abstracted potential function with six sample points and two minima ................. 35
3-2 Relative orientations of inertial, body, and tracking frames................................. 38
3-3 Dependency chart for the generalized control solution ....................................... 53
4-1 Setup for the early avoidance pathway ............................................................... 62
4-2 Illustration of the double conic problem .............................................................. 64
4-3 Variation in the logic term over the middle gamma domain ................................ 66
4-4 Illustration of the solution to the double conic problem ....................................... 66
4-5 Sample profiles of the early avoidance potential ................................................ 67
5-1 AbstractoSat illustration ...................................................................................... 75
5-2 Origin and terminus points for AbstractoSat sensors .......................................... 78
5-3 Results of the unconstrained simulation ............................................................. 80
5-4 Paths traced by sensor vectors in the unconstrained simulation ........................ 81
5-5 Results of the first pointing constraint simulation ................................................ 83
5-6 Paths traced by sensor vectors in the first pointing constraint simulation ........... 84
5-7 Results of the second pointing constraint simulation .......................................... 86
5-8 Paths traced by sensor vectors in the second pointing constraint simulation ..... 87
5-9 Demonstration of simulation hang-up induced by nearby local extrema ............. 95
9
LIST OF ABBREVIATIONS
Angular Momentum Exchange Device (AMED)
An internal device on board a satellite which redistributes angular momentum, resulting in imparted torque.
Artificial Potential Function (APF)
A scalar function of the spacecraft’s orientation and/or angular velocity which determines the values of the controller output.
Control Moment Gyroscope (CMG)
A type of AMED which redistributes angular momentum by rotating at a fixed rate along a variable direction.
Direction Cosine Matrix (DCM)
A 3x3 matrix containing parameters which fully specify the rotation of one coordinate system with respect to another.
Nth Time Derivative (TDN)
A shorthand way of describing the operation 𝑑𝑁 𝑑𝑡𝑁⁄ (e.g. TD1 means “first time derivative”).
Ordinary Differential Equation (ODE)
An equation containing a function of one independent variable and its derivatives.
Potential Rate Transform (PRT)
A 3x1 vector which converts relative angular velocity into a rate of change for first-stage APFs.
Reaction Wheel Assembly (RWA)
A type of AMED which redistributes angular momentum by rotating at a variable rate along a fixed direction.
Runge-Kutta (RK) Method
A method of numerical integration which solves an ODE where the independent variable is separable from its derivative.
10
Abstract of Thesis Presented to the Graduate School of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Master of Science
DEVELOPING A GENERAL FRAMEWORK FOR A TWO STAGED ARTIFICIAL POTENTIAL CONTROLLER
By
Tristan J. Newman
August 2015
Chair: Norman Fitz-Coy Major: Aerospace Engineering
Although there are many control options available to satellite manufacturers,
artificial potential functions (APF) provide an analytical and computationally inexpensive
way for a satellite to perform multiple tasks simultaneously. By expressing mission
constraints as a series of APFs and combining the results, a control solution can always
be evaluated.
The goal of my research was to develop a general framework which evaluates
the control solution from a generalized combination of arbitrary constraints. Developing
a general framework is attractive because of its flexibility in adapting a control solution
to a landscape where the constraints may be subject to change based on the needs of
the mission.
Separating the control system into two stages – one for attitude/orientation, and
one for angular velocity – was necessary to decouple those states. The first stage
determines the angular velocity required by the spacecraft, and the second computes
the torque necessary to reach the desired state.
11
The newfound flexibility provided by this framework makes it possible to add
constraints to a control solution with unprecedented ease and efficiency. The primary
example focused on in this document is the “pointing constraint problem” – a classic
problem in spacecraft attitude control.
The research done here suggests that a wide range of uses may exist for APF
controls and including slew-rate constraints and early avoidance of pointing constraint
boundaries. Future research will be necessary to solve some issues with this approach
– particularly in the implementation of second-stage potentials. It is believed that solving
these problems will greatly increase the effectiveness of nonlinear control systems by
sparing time and control effort. Overcoming these barriers may very well lead to APF
controls becoming a valuable asset for satellites with limited computational resources.
12
CHAPTER 1 INTRODUCTION
Foreword
On-orbit attitude change maneuvers can be regarded as the astrodynamical
equivalent of looking over one’s shoulder. There are many ways to control such a
maneuver, and some are more efficient than others at the expense of higher
computational overhead. However, for such maneuvers there is a need to balance
efficiency with speed. To carry the human analogy one step farther, consider an
individual observing a plane as it flies through the daytime sky. If the plane were to fly in
front of the sun, we assume that it would cause the onlooker some discomfort if they
were to continue tracking it.
The “maneuver” performed here can be regarded as a tracking slew from one
orientation to another. However, this maneuver is subject to a “keep-out constraint” –
that is, there is a “sensor” (in this case, the onlooker’s eyes), and a direction along
which that sensor should not be directed. A buffer about this vector defines a conic
region known as the “keep-out cone.” The keep-out constraint is designed to prevent
the sensor from being directed along any vector within the keep-out cone.
To return to the human analogy, although the experiment would depend on some
unquantifiable parameters such as the pain-tolerance of the participant, or their
willingness to carry out the experiment, a surface-level reconstruction of this scenario
might show that one would instinctively avoid looking directly at the sun as the plane
crosses over its disk.
13
The operative distinction here is “instinctively,” which would imply that executing
the maneuver requires little-to-no effort on the part of the onlooker. Although it would
not necessarily follow an optimal path, the maneuver would, ideally, have little impact on
the performance of the onlooker’s mission (for example, perhaps they were commuting
to work). This example therefore illustrates the attractiveness of computationally
inexpensive maneuvers in the context of satellite control systems.
If a satellite was to be designed which could carry out a tracking maneuver
subject to the keep-out constraint, there would be several options for the control system.
In particular, three methods appeared predominantly in my research: (i) the time (or
energy) optimized control solution [1]; (ii) the workspace-discretized pathfinding solution
[2]; and (iii) the nonlinear control solution based on the method of Lyapunov [3], [4].
This document will primarily focus on the method of Lyapunov, and the concept
of artificial potential functions (APF) which stems from it. It will be shown that APF
controls provide an analytical - and therefore computationally inexpensive - way of
navigating the “field” of constraint boundaries. However, a brief study of other pre-
existing methods is necessary in order to fully appreciate the comparative efficiency of
APF controls.
A Note on Notation
Because of the breadth of subjects covered in this document, a single notation
will be used to avoid confusion. Equations brought in from other sources will thus have
their notation modified to fit this standard: All quantities will be hereafter be identified as
“scalar” – with no distinguishing marks (e.g. 𝑠 ∈ ℝ) – “vector” – marked with a single
underline (e.g. 𝑣 ∈ ℝ𝑛) – or “matrix” – marked with two underlines (e.g. 𝑀 ∈ ℝ𝑛×𝑚).
14
Because there will be such a heavy focus on linear algebra throughout this
document, the word “vector” may be used to refer to any matrix where either of the
dimensions are 1 (if both dimensions are 1, the quantity may be referred to as a scalar
instead), in which case the term “row vector” or “column vector” will be used in the case
where the number of rows or columns of said matrix are 1, respectively.
Any quantity underlined once in any of the equations appearing hereafter is
implicitly a column vector. If a row vector is to be used, the vector will be transposed as
such: 𝑣 is a column vector; 𝑣𝑇 is a row vector. Furthermore, a “hat” over a vector with
any number of dimensions (e.g. 𝑣) will serve to indicate the vector is of unit length (i.e.
the scalar components of the vector are unitless and the magnitude is equal to one).
To avoid excessive notation, all physical vectors (e.g. directions in space,
angular velocities, torques, etc.) are all implicitly parameterized in the body-fixed
coordinate frame of the satellite unless otherwise noted. If it is necessary, the frame in
which the vector is parameterized will be noted with an upper-left superscript (e.g. 𝑣 𝑁 ).
Finally, the first time derivative (TD1) of any quantity will be represented with a
single dot (e.g. 𝑑𝑠 𝑑𝑡⁄ = �̇�), the second time derivative (TD2) will be represented with
two dots (e.g. 𝑑2𝑣 𝑑𝑡2⁄ = �̈�), and so on.
Background
Optimal Controls
Summary. The optimal control solution is perhaps the most difficult one to derive
and implement. This difficulty arises because it must not only solve the problem, but do
so while minimizing a parameter of the satellite designer’s choice.
15
Unsik Lee and Mehran Mesbahi derived such a solution; minimizing either
maneuver time, or energy expenditure requirements for the maneuver using Gauss’s
pseudospectral method [1].
Citing the need for “…avoiding undesirable orientations with respect to celestial
objects…” as “…essential for science missions [1],” the problem described is exactly
the keep-out problem analogized in the “watching planes” example. As a point of
interest, [1] was mentioned to be a follow-up on research done by Lee and Mesbahi
regarding the use of Lyapunov potential functions to perform similar maneuvers [4].
As in [4], Lee and Mesbahi parameterize the rotation of the spacecraft with unit
quaternions, where any quaternion defines an attitude that the spacecraft may occupy.
Although there are an infinite number of attitudes that the spacecraft can occupy, there
exists a finite number of discrete zones, corresponding “attitude forbidden” (the keep-
out cones described in Chapter 1), and “attitude mandatory” (a conic region similar to a
keep-out cone, but whose role is reversed such that a body-fixed direction is meant to
stay within it rather than outside of it) [1]. An example for each type of zone is illustrated
in Figure 1-1 (the illustrations themselves taken from [1]).
A B
Figure 1-1. A) Illustration of an “attitude forbidden” zone [1]. B) Illustration of an “attitude mandatory” zone [1].
16
The cost function to be minimized is
𝐽 = ∫ (𝜌 + (1 − 𝜌)|𝑢(𝑡)|)𝑑𝑡𝑡𝑓
𝑡0
(1-1)
where 𝑢(𝑡) is a 3-element vector containing the control torques for each reaction wheel
[1], and 𝜌 is “a weighting factor on the time, where 𝜌 = 0 and 𝜌 = 1 yield the energy
optimal and time optimal performance cost, respectively [1].”
Equation 1-1 is subject to the following constraints:
�̇�(𝑡) =1
2𝑞(𝑡) ⊗ �̃�(𝑡), (1-2)
𝐽�̇�(𝑡) = (𝐽𝜔 + 𝐽𝑟𝜔𝑟) × 𝜔 − 𝐽𝑟𝑢, (1-3)
�̇�𝑟 = 𝑢 (1-4)
where “𝑞(𝑡) is the unit quaternion representing the attitude of the rigid body at time 𝑡”,
“𝜔(𝑡) ∈ ℝ3 denotes the angular velocity of the spacecraft in the body frame”, “�̃�(𝑡) =
[𝜔𝑇 0]4×1𝑇 ”, “ 𝐽 = diag(𝐽1, 𝐽2, 𝐽3) denotes the [centroidal] inertia matrix of the spacecraft
in the body frame”, and “𝐽𝑟 = diag(𝐽𝑟1, 𝐽𝑟2, 𝐽𝑟3) denotes the inertia matrix of the
spacecraft’s reaction wheels in the body frame [1].”
Lee and Mesbahi require the spacecraft to follow a “rest to rest” profile – i.e. the
maneuver begins (at time 𝑡 = 𝑡0) and ends (at time 𝑡 = 𝑡𝑓) with zero angular velocity.
The spacecraft’s reaction wheels are likewise assumed to be initially at rest. Thus, the
maneuver observes the following boundary conditions:
𝑞(𝑡0) = 𝑞𝑡0; 𝑞(𝑡𝑓) = 𝑞𝑡𝑓 ; 𝜔(𝑡0) = 0; 𝜔(𝑡𝑓) = 0; 𝜔𝑟(𝑡0) = 0. (1-5)
17
In addition to boundary conditions, there are three constraints on the control and
angular velocity of the spacecraft [1] which exist to prevent saturation of the reaction
wheels or harmonic oscillation of nonrigid components:
|𝑢𝑖| ≤ βu; (1-6)
|𝜔𝑖| ≤ 𝛽𝜔; (1-7)
|𝜔𝑟𝑖| ≤ 𝛽𝜔𝑟 ; (1-8)
for 𝑖 = 1, 2, 3 [1].
Finally, there are constraints corresponding to each attitude forbidden zone, and
the attitude mandatory zone [1]. These are represented by a known matrix 𝑀 that is a
function of spatial geometry. Each 𝑀 satisfies the inequality constraint
𝑞𝑇𝑀𝑓𝑖𝑞 < 0 (1-9)
for the 𝑖th forbidden zone (where 𝑖 = 1,… , 𝑛 [1]), and
𝑞𝑇𝑀𝑚𝑞 > 0 (1-10)
for the lone attitude mandatory zone [1].
The solution to the optimal control problem defined by Eqs. 1-2 through 1-10 was
determined by “a Gauss pseudospectral method (direct optimal control method) using
the open source optimal control software GPOPS® in conjunction with the nonlinear
programming problem solver SNOPT® and the automatic differentiator INTLAB® [1].”
Gauss pseudospectral method is described as: “a class of direct collocation
methods where the optimal control problem is transcribed into a nonlinear programming
problem (NLP) by approximating the state and control states using global orthogonal
polynomials and collocating the differential dynamic equations on Legendre-Gauss
18
collocation points [1].” Reference [5] expounds more upon the details of pseudospectral
methods, including Legendre-Gauss collocation.
Because collocation methods such as this are only valid on the range [−1, 1],
the generalized time interval 𝑡 = [𝑡0, 𝑡𝑓] is transformed using
𝜏 =2𝑡 − 𝑡𝑓 − 𝑡0𝑡𝑓 − 𝑡0
. (1-11)
The derivation completes as follows:
The states [sic] 𝑞(𝜏) is approximated by the polynomial �̃�(𝜏) using a basis of
𝑁 + 1 Lagrange interpolating polynomials on the time interval of [−1, 1] as
𝑞(𝜏) ≈ �̃�(𝜏) =∑𝑞(𝜏𝑖)𝐿𝑖(𝜏𝑖)
𝑁
𝑖=0
where 𝐿𝑖(𝜏) is the 𝑖th Legendre polynomial defined as
𝐿𝑖(𝜏) = ∏𝜏 − 𝜏𝑗𝜏𝑖 − 𝜏𝑗
𝑁
𝑗=0,𝑗≠𝑖
, (𝑖 = 0,… ,𝑁).
The derivative of the state approximation is similarly obtained as
�̇�(𝜏) ≈ �̇̃�(𝜏) =∑�̃�(𝜏𝑖)�̇�𝑖(𝜏𝑖)
𝑁
𝑖=0
=∑𝐷𝑘,𝑖�̃�(𝜏𝑖)
𝑁
𝑖=0
where 𝐷𝑘,𝑖 are element [sic] of non-square matrix 𝐷 ∈ ℝ𝑁×(𝑁+1) called Gauss
pseudospectral differentiation matrix … The performance cost in terms of 𝜏 is approximated using a Gauss quadrature as
𝐽 =𝑡𝑓 − 𝑡02
∑𝑤𝑘(𝜌 + (1 − 𝜌)|𝑢(𝜏𝑘)|)
𝑁
𝑘=1
where 𝑤𝑘 are the Gauss weights. The above performance index together with approximated constraints … lead to formulation of a nonlinear programming with
the initial and final states 𝑞(𝑡0) = �̃�(𝜏0) and 𝑞(𝑡𝑓) = �̃�(𝜏𝑓), respectively, which
can be solved using SNOPT nonlinear solver [1].
19
Advantages and disadvantages. The greatest benefit of the optimal control
solution is that it is, in fact, optimal. This allows for satellites operating with such a
control system to complete large-slew maneuvers while holding to a variety of
constraints in as little time as possible, or expending as little energy as possible.
However, this benefit comes at a price. In computer science terms, the product
definition for the 𝑖th Legendre polynomial implies that evaluating 𝐿𝑖(𝜏) for 𝑁 collocation
points has a time complexity of 𝑂(𝑁), as it is the product of 𝑁 terms (each with a time
complexity of 𝑂(1)). Furthermore, evaluating the state requires the summation of 𝑁 + 1
Legendre polynomials (modified by 𝑞(𝜏)), implying that the evaluation of �̃�(𝜏) has a time
complexity of 𝑂(𝑁2).
In addition, the first time derivative of 𝐿𝑖(𝜏) requires the application of the product
rule 𝑁 times. Therefore, evaluating �̇�𝑖(𝜏) (and, by extension, 𝐷𝑘,𝑖 [5]) also has a time
complexity of 𝑂(𝑁2). However, since the evaluation of �̇�(𝜏) requires the summation of
𝑁 + 1 such terms, the overall time complexity for this section of the algorithm is 𝑂(𝑁3).
This is only the time complexity involved in determining the quaternion rate at the
next collocation point. For all 𝑁 collocation points, the time complexity graduates to
𝑂(𝑁4)! This complexity does not take into account operations done internally by the
SNOPT nonlinear solver, so 𝑂(𝑁4) can be regarded as a minimum time complexity.
For comparison, a fully analytical method (such as the method of Lyapunov, as
will eventually be shown) has a time complexity of 𝑂(1). In other words, determining the
optimal control solution requires much dedication of onboard computational resources.
20
This may not be an issue for maneuvers wherein a small number of collocation
points can be used, as it can be inferred from Eq. 1-11 that short-duration maneuvers
do not progress as rapidly through the 𝜏 domain, and therefore fewer collocation points
may be used. Furthermore, larger satellites (such as GPS or military satellites) that
prioritize the speed in which a precise maneuver can be executed over the amount of
memory that maneuver takes may make better use of the optimal control solution
versus other, nonoptimal methods.
Workspace-Discretized Pathfinding
Summary. Another approach explored by Henri C. Kjellberg and E. Glenn
Lightsey discretizes the spherical space surrounding the spacecraft into hexagonal
segments to find “an admissible path between the attitude keep-out zone [using the] A*
pathfinding algorithm … chosen because of its ease of implementation [2].”
Since the workspace is discretized, each “pixel” (a single element of the unit
sphere) has its neighboring pixels identified pre-implementation and stored on-board. A
“path-cost function 𝑔(𝑝(𝑘))” and “heuristic distance function ℎ(𝑝(𝑘))” are defined where
𝑝(𝑘) is the “pixel node at step 𝑘 [2].” The “distance-plus-cost function 𝑓(𝑝(𝑘))” is
defined to be
𝑓(𝑝(𝑘)) = 𝑔(𝑝(𝑘)) + ℎ(𝑝(𝑘)).
If a pixel is outside of the keep-out region (which [2] describes as “the prohibited
constraint set”), then the cost function is given by
𝑔(𝑝(𝑘)) = 𝜃(𝑒(𝑘), 𝑒(𝑘 − 1)) + 𝑑 + 𝑔(𝑝(𝑘 − 1)),
where 𝑒(𝑘) is the vector difference between the “current” and “previous” pixels, and
𝜃(𝑒(𝑘), 𝑒(𝑘 − 1)) can be interpreted as the angle by which the path has deflected in the
21
previous two iterations, “thus penalizing a change in the eigenaxis vector direction … If,
however, the pixel is inside the prohibited constraint set, then 𝑔(𝑝(𝑘)) = 𝑁, where 𝑁 is
an arbitrarily large constant to prevent a solution from going within the constrained
region [2].”
The heuristic distance function is the same regardless of whether or not a pixel is
inside the constrained region. If 𝑝(𝑚) represents the “target” pixel, then the heuristic
distance is the angle that must be traversed in order to reach it [2]. Therefore
ℎ(𝑝(𝑘), 𝑝(𝑚)) = arccos[𝑣(𝑝(𝑘)) ∙ 𝑣(𝑝(𝑚))].
Having defined both cost functions, the A* algorithm can be run to find the path
that minimizes the total cost between the initial and final pixels. The algorithm as
described by Kjellberg and Lighsey
returns the set of pixels that provide an admissible, locally minimum path-cost [as
defined by the path-cost function 𝑓(𝑝(𝑘))] trajectory for the desired rotation of the
vehicle’s body-fixed x axis to the new attitude. The pixels that have been explored during this process are known as the closed set. The pixels that constitute the final path are reduced by eliminating all pixels that lie along approximately the same path on a great circle. The remaining pixels are “turning points,” locations where the eigenaxis vector direction must change … The resulting trajectory for a typical reorientation problem is shown in Figure 8 [of [2], collated in Figure 1-2] for the vehicle’s body-fixed x axis [2]. Advantages and disadvantages. In contrast with the optimal control solution,
discretization becomes more efficient as the number of constraints increases. This is
because more constraints reduce the number of pixels accessible to the A* algorithm
[2]. However, it is worth noting that this method provides a solution that can be quite far
from optimal since the “cost function” being optimized relates to the A* algorithm instead
of the keep-out problem optimized by Lee and Mesbahi.
22
Figure 1-2. "Example x-axis reorientation trajectory resulting from the A* pathfinder algorithm [2].”
However, Kjellberg and Lightsey note that “these advantages are particularly well
suited for small spacecraft, which have limited power and processing capabilities, yet
must meet many different attitude constraints [2].”
In terms of complexity, workspace discretization can be thought of as a middle
ground between optimality, and the computational efficiency of the Lyapunov’s method
for nonlinear control.
Method of Lyapunov
Summary. While the optimal control and workspace discretization methods
undoubtedly have their advantages, they are ultimately algorithmic approaches. The
optimal solution can only be approximated by introducing a finite number of collocation
23
points, and increasing the accuracy of the approximation drastically slows down its
computation. The workspace discretization method is purely algorithmic, requiring the
discretization of space around the satellite in order to be compatible with the A*
pathfinding algorithm proposed by Kjellberg and Lightsey.
In both cases, the time complexity of obtaining the solution depends on the
fidelity of the discretization (i.e. a better approximation can be obtained in both cases by
using either more collocation points, or more pixels respectively). An analytic and
closed-form solution has the benefit of being rapidly calculable, although the solution
obtained may not be optimal.
Colin McInnes describes a procedure to obtain such a solution: “The second
method of Lyapunov allows expressions for the required control torques to be obtained
analytically in closed form. Therefore, attitude control commands may be generated in
real time, so that the method may be suitable for autonomous, onboard operations [3].”
Defining “a scalar potential function 𝑉 … to be positive definite everywhere,
except at the target point of the system state space where it will vanish [3],”
𝑉 =1
2∑𝐼𝑖𝜔𝑖
2
3
𝑖=1
+1
2∑𝜆(𝜃𝑖 − 𝜃𝑖)
23
𝑖=1
. (1-12)
Thus taking the first time derivative (TD1) of Eq. 1-12 yields
�̇� = ∑𝜔𝑖𝑇𝑖
3
𝑖=1
+∑𝜆�̇�𝑖(𝜃𝑖 − �̃�𝑖)2
3
𝑖=1
(1-13)
where “𝜔𝑖, 𝐼𝑖, 𝑇𝑖 (𝑖 = 1,… ,3) are the satellite body rate, moment of inertia, and control
torque about the 𝑖th principal axis”, and “the second term [of Eq. 1-12] represents an
24
artificial potential energy possessed by the system relative to the target attitude
(�̃�1, �̃�2, �̃�3) [3].”
Since 𝑉 takes on its minimum value (of zero) at the desired orientation and
angular velocity, the control solution 𝑇𝑖 should be specified such that �̇� (and Eq. 1-13 by
extension) is negative semidefinite [3]. “The choice of control is obviously nonunique;
however, the simplest control is selected as
𝑇𝑖 = −𝜅𝜔𝑖 −∑𝐺𝑖𝑗𝑇𝜆𝑗(𝜃𝑗 − �̃�𝑗)
3
𝑗=1
where the gain constants 𝜅 and 𝜆𝑖 (𝑖 = 1,… ,3 [sic]) will be chosen to shape the
maneuver [3].” The matrix 𝐺𝑖𝑗𝑇 is a function of the spatial geometry of the system:
𝐺𝑖𝑗𝑇 = [
1 sin 𝜃1 tan 𝜃2 cos𝜃1 tan 𝜃20 cos𝜃1 −sin 𝜃10 sin 𝜃1 sec 𝜃2 cos𝜃1 sec 𝜃2
].
It can thus be shown that the target attitude is “the only attractive equilibrium point for
the system [3].”
In the context of “sun vector avoidance,” another scalar potential function must
be defined, although its exact form varies from author to author. McInnes uses
Gaussian functions “to represent the regions of high potential so that there are no
singularities in the potential and the controls are bounded [3].” Unsik Lee and Mehran
Mesbahi use “logarithmic barrier potentials [4].”
This is the point at which most research into APF-based controls diverges, as the
method of Lyapunov can be applied to many different potentials and constraints –
nearly all of which are defined differently from author to author. It is from here that my
own research branches off – away from the background material discussed thus far.
25
Advantages and disadvantages. It is hopefully apparent from this summary of
Lyapunov’s method that there is no one way to solve a constrained maneuver problem,
as doing so is nearly as much of an art as it is a science, which may be seen as either a
disadvantage (as some other methods yield concretely defined solutions with easily
understood physical significance), or a great advantage (since there are often as many
different ways to solve a problem using APFs, each with its own set of advantages and
disadvantages). However, it should be disclosed that the solution can never be said to
be optimal, no matter what APF is chosen.
The lack of optimality should really only be seen as a barrier to use in the case of
satellites for which quickly executing a maneuver is more important than sparing on-
board computational resources for more (subjectively) “important” tasks. For example, a
satellite conducting a survey of overhead constellations may not need to slew in an
optimal manner, but if the processor on board only has limited processing capability
then the analytical solution provided by Lyapunov’s method may be more attractive.
The greatest concerns with the application of user-defined scalar potentials is
that, when superimposed, the APFs may give rise to regions where the scalar rate
becomes zero, even though the desired state has not been reached. These situations
can be avoided through deliberate configuration of the APFs, and are oftentimes
unstable (meaning that they could be avoided using a jitter controller, which provides
numerical uncertainty to the currently adopted state). However, if such a region were
the result of a stable local minimum, then a more powerful approach is necessary. This
“local minimum problem” is of great concern to researchers studying nonlinear controls,
but it is currently beyond the scope of this document to address it directly.
26
CHAPTER 2 MATHEMATICAL OVERVIEW
Vector Operations in Linear Algebra
As noted in Chapter 1, “the word ‘vector’ may be used to refer to any matrix
where either of the dimensions are 1.” This can be done because every operation in
vector algebra has an equivalent operation in linear algebra.
For example, consider a vector 𝑣 ∈ ℝ3. If 𝑣 is parameterized in an arbitrary
reference basis 𝐴 containing orthonormal basis vectors 1̂, 2̂, and 3̂, with corresponding
parallel components 𝑣1, 𝑣2, and 𝑣3, then the vector annotated equation for 𝑣 𝐴 would be
𝑣 𝐴 = 𝑣11̂ + 𝑣22̂ + 𝑣33̂,
Thus, the equivalent matrix form conveniently allows for the use of 𝑣 𝐴 in linear algebra
operations, and can be determined by respectively representing the vectors 1̂, 2̂, and 3̂
(parameterized in 𝐴) as the 3 × 1 matrices (column vectors) [1 0 0]𝑇, [0 1 0]𝑇,
and [0 0 1]𝑇, then 𝑣 𝐴 has the equivalent matrix representation
𝑣 𝐴 ⟺ [
𝑣1𝑣2𝑣3].
Defining a second vector 𝑢 which is also parameterized in the 𝐴-fixed {1̂, 2̂, 3̂}
basis, it follows from the definition of the “dot product” 𝑢 ∙ 𝑣 that
𝑢 ∙ 𝑣 = 𝑢1𝑣1 + 𝑢2𝑣2 + 𝑢3𝑣3. (2-1)
Note that the upper-left superscripts have been omitted because Eq. 2-1 is valid in any
coordinate system provided both vectors are parameterized within it. Ergo, Eq. 2-1 has
the equivalent matrix representation
𝑢 ∙ 𝑣 ⟺ 𝑢 𝐴 𝑇 𝑣
𝐴 (2-2)
27
where the superscript 𝑇 denotes the transpose of the column vector 𝑢 𝐴 . One can verify
this by performing the multiplication in Eq. 2-2 and comparing its results to Eq. 2-1.
The result in Eq. 2-2 naturally suggests that there is an equivalent matrix form for
the cross product as well. Thus, we may once again consider the vectors 𝑢 and 𝑣 as
parameterized in the {1̂, 2̂, 3̂} basis. By definition, the cross product 𝑢 𝐴 × 𝑣
𝐴 is equal to
𝑢 𝐴 × 𝑣
𝐴 = (𝑢2𝑣3 − 𝑢3𝑣2)1̂ + (𝑢3𝑣1 − 𝑢1𝑣3)2̂ + (𝑢1𝑣2 − 𝑢2𝑣1)3̂. (2-3)
It is possible to group the terms on the right-hand side of Eq. 2-3 such that it may
be represented as the multiplication of a 3 × 3 matrix and the 3 × 1 matrix representing
the vector 𝑣 𝐴 . This yields
𝑢 𝐴 × 𝑣
𝐴 ⟺ 𝑢 𝐴 × 𝑣
𝐴 ; (2-4a)
𝑢 𝐴 × ≝ [
0 −𝑢3 𝑢2 𝑢3 0 −𝑢1−𝑢2 𝑢1 0
] (2-4b)
where the superscript × in Eq. 2-4a denotes the “skew matrix form” of the column vector
𝑢 𝐴 which is defined in Eq. 2-4b. Once again, this may be verified by executing the
matrix multiplication in Eq. 2-4a, which will consequently yield Eq. 2-4b.
The equivalencies in Eqs. 2-2 and 2-4a allow us to reconcile vector operations
with linear algebra. One example of this which we will eventually find to be particularly
useful relates the cross product of two vectors to their dot product. The identity formally
known as the “vector triple product” is most commonly cited in terms of vector
operations. However, we will hereafter refer to its equivalent matrix form:
𝑢 𝐴 × 𝑣
𝐴 × = 𝑣 𝐴 𝑢
𝐴 T − 𝑢 𝐴 T 𝑣
𝐴 𝕀 (2-5)
where 𝕀 is the 3 × 3 identity matrix. This identity is proven formally in [6], published in
1939’s Mathematical Gazette by S. Chapman and E. A. Milne.
28
Body Rotations and Quaternion Math
The rotation of a rigid body in space can always be specified using a direction
cosine matrix (DCM) – a 3x3 matrix whose terms fully specify the rotation of one
coordinate system with respect to another. For example, consider a spacecraft whose
body-fixed (i.e. fixed in frame 𝐵) 𝑥, 𝑦, and 𝑧 axes are parameterized in an inertially-fixed
(i.e. fixed in frame 𝑁) coordinate system as �̂� 𝑁 , �̂�
𝑁 , and �̂� 𝑁 , respectively. The DCM that
converts an inertially-fixed vector to body-fixed coordinates is 𝑅𝑁𝐵 = [ �̂�
𝐵 �̂� 𝐵 �̂�
𝐵 ] [7].
As a point of interest, the TD1 of any DCM is dependent on the angular velocity
of the coordinate system it transforms into. In general, for two coordinate systems fixed
in arbitrary reference frames 𝐴 and 𝐵, the TD1 of 𝑅𝐴𝐵 is
�̇�𝐴𝐵 = − 𝜔𝐴
𝐵 × 𝑅𝐴𝐵 (2-6)
where 𝜔𝐴𝐵 is the angular velocity of the 𝐵 frame relative to the 𝐴 frame [8].
More compact ways of representing an orientation are available. The axis-angle
representation identifies the transformation between two reference frames as a rotation
by a single net rotation angle 𝜃 about a unit length “Eigen-axis” �̂�. Alternatively, we may
specify the transformation as a sequence of three rotations about the basis axes. The
angles, in sequence, are known as “Euler” angles.
Either case exhibits special orientations (called “singularities” in formal literature
[9]) for which a unique set of parameters may not be determined. Many Earth-bound
applications (e.g. submarines, land vehicles, and passenger planes) may avoid these
singularities by restricting their operational envelopes (e.g. not being designed to
execute a 90° roll).
29
Satellites in space, however, do not often have such restrictions placed on the
attitudes they can occupy. Their constraints tend to be more exotic (such as the attitude
mandatory and forbidden constraints discussed in Chapter 1) thus necessitating a
singularity-free expression of attitude.
For this reason, space-based applications use four-element parameters called
“Euler symmetric parameters” or “quaternions.” Because they do not contain a minimal
set of three elements, quaternions are able to store all of the information necessary to
specify a unique orientation in space. These elements may be uniquely determined from
any DCM, including those which would create singularities in other representations.
Formally, quaternions are complex numbers consisting of one real, and three
imaginary components. However, in the context of this document, any quaternion is
treated as a 4 × 1 matrix (or four-dimensional column vector) of the form
𝑞 ≝ [�̂� sin(𝜃 2⁄ )
cos(𝜃 2⁄ )] ≝ [
휀𝜂] ≝ [
휀1휀2휀3𝜂
] (2-7)
where �̂� is the the Eigen-axis of the rotation (the parameterization of which is context-
dependent), and 𝜃 is the rotation angle. As a point of interest, it follows from Eq. 2-7 that
|𝑞| = √sin2(𝜃 2⁄ ) + cos2(𝜃 2⁄ ) ≡ 1,
thus any quaternion representing a pure rotation (which, in this context, is the only
quaternion we will use) is of unit “length.”
The inverse rotation can be represented by a quaternion obtained in one of two
ways: either by rotating the starting coordinate basis about �̂� by an angle – 𝜃, or by
rotating about −�̂� by 𝜃. Modifying Eq. 2-7, the inverse quaternion can be written as
30
𝑞−1 = [�̂� sin(−𝜃 2⁄ )
cos(−𝜃 2⁄ )] = [
�̂� sin(𝜃 2⁄ )
−cos(𝜃 2⁄ )] ≡ [
휀−𝜂] ; (2-8a)
𝑞−1 = [−�̂� sin(𝜃 2⁄ )
cos(𝜃 2⁄ )] ≡ [
−휀𝜂 ] = − [
휀−𝜂] ≡ −𝑞
−1. (2-8b)
Note that Eqs. 2-8a and 2-8b are equivalent, despite being separated by a factor of −1.
This interesting property is called “sign ambiguity”, and implies that for any quaternion
𝑞 = −𝑞.
It will also be useful to note that applying any two rotations represented by the
quaternions 𝑞1 and 𝑞2 to a starting coordinate basis is equivalent to applying a single
rotation represented by may be expressed as a single rotation represented by the
quaternion 𝑞𝑒 (also known as the “quaternion error”). If basis 1 and 2 are reached by
respectively applying 𝑞1 and 𝑞2 to the starting basis, then the quaternion error is
determined by applying the “quaternion product [10]” to 𝑞2 and 𝑞1−1
𝑞𝑒 ≝ 𝑞2⊗𝑞1−1,
which has the equivalent matrix expression
𝑞𝑒 = [−휀1
× + 𝜂2𝕀 −휀1
휀1𝑇 𝜂1
] [휀2𝜂2], (2-9)
where 𝕀 denotes the identity matrix of dimension 3x3.
Equation 2-9 shows that the quaternion error can be obtained by applying a
linear operation on 𝑞2. However, it may be useful to express the quaternion error as a
linear operation on 𝑞1 instead. Reorganization of Eq. 2-9 results in the expression
𝑞𝑒 = [휀2× − 𝜂2𝕀 휀2
휀2𝑇 𝜂2
] [휀1𝜂1], (2-10)
31
Thus, from these two equivalent forms of the quaternion error, we may define the
“quaternion error matrix” of type 1 from Eq. 2-9, and of type 2 from Eq. 2-10:
𝑄1 (𝑞) ≝ [−휀× + 𝜂𝕀 −휀
휀𝑇 𝜂] ; (2-11a)
𝑄2 (𝑞) ≝ [휀× − 𝜂𝕀 휀
휀𝑇 𝜂]. (2-11b)
If we then substitute Eqs. 2-11a and 2-11b into Eqs. 2-9 and 2-10 respectively, we find
that a more compact expression for the quaternion error may be used:
𝑞𝑒 = 𝑄1 (𝑞1) 𝑞2 = 𝑄2 (𝑞2)𝑞1. (2-12)
Equation 2-12 affords us a meaningful, compact way of expression the effective
difference between two rotations, which will become useful later. For now, however, we
must also be concerned with locating a similar expression for a quaternion rate.
Because a quaternion contains four elements, so too must its TD1. However,
recall from Eq. 2-6 that the TD1 of an orientation is dependent on its angular velocity – a
physical vector consisting of three elements. A quaternion representation of orientation
should therefore be no different, so we must introduce a rectangular matrix to reconcile
this difference in the dimensionality:
Ξ (𝑞) ≝ [휀× + 𝜂𝕀
−휀𝑇]. (2-13)
The matrix defined in Eq. 2-13 is called the “quaternion rate matrix”, and converts the
angular velocity of a coordinate system into a quaternion rate. For a coordinate system
defined by an orientation 𝑞 and angular velocity 𝜔, the TD1 of 𝑞 (i.e. �̇�) is
�̇� =1
2Ξ (𝑞)𝜔. (2-14)
32
Derivatives of (and With Respect to) Multi-Dimensional Quantities
The previous discussion about the differentiation of a quaternion serves as an
indicator of the important role of derivatives in the field of nonlinear and APF-based
control. In fact, it will often be required of us to differentiate these multi-dimensional
quantities – sometimes with respect to other quantities of the same dimensionality.
We will find that any matrix can be differentiated with respect to a scalar quantity,
but only a scalar can be differentiated with respect to a matrix. Furthermore, in the case
where a matrix represents a column vector, we may introduce new definitions to allow
for the differentiation of either a vector with respect to a vector.
In most applications, differentiation is applied to a scalar with respect to another
scalar. For example, if 𝑦 and 𝑥 are scalars, then the derivative 𝐷 of 𝑦 with respect to 𝑥
can be written as
𝐷 ≝𝑑𝑦
𝑑𝑥
if and only if 𝑦 is a function of only 𝑥. More generally, if 𝑦 is a function of other variables,
then the derivative 𝐷 can be written as
𝐷 ≝𝜕𝑦
𝜕𝑥 (2-15)
Similarly, we may differentiate a general 𝑛 × 𝑚 matrix with respect to a scalar.
This also applies to vector quantities, since they may be represented as matrices. In
general, if 𝑀 ∈ ℝ𝑛×𝑚, then the derivative 𝐷 of 𝑀 with respect to a scalar 𝑥 is defined as
𝐷 ≝
[ 𝜕𝑀11
𝜕𝑥⋯
𝜕𝑀1𝑚
𝜕𝑥⋮ ⋱ ⋮
𝜕𝑀𝑛1
𝜕𝑥⋯
𝜕𝑀𝑛𝑚
𝜕𝑥 ]
(2-16)
33
[11], [12], [13] In other words, differentiating a vector or matrix with respect to a scalar is
equivalent to differentiating every element of that vector or matrix individually.
We will also need to consider the derivative a vector (but not a matrix [11]) with
respect to another vector. Note that, just as a vector can be represented as a single-
column matrix, so too may a scalar be represented as a single-element vector. In
general, if 𝑢 ∈ ℝ𝑛 and 𝑣 ∈ ℝ𝑚, then the derivative 𝐷 of 𝑢 with respect to 𝑣 is defined as
𝐷 ≝
[ 𝜕𝑢1𝜕𝑣1
⋯𝜕𝑢1𝜕𝑣𝑛
⋮ ⋱ ⋮𝜕𝑢𝑛𝜕𝑣1
⋯𝜕𝑢𝑛𝜕𝑣𝑚]
(2-17)
[11], [12], [13] Note that in the case where 𝑢 ∈ ℝ1 → 𝑢 ∈ ℝ, Eq. 2-17 reduces to a row
vector 𝐷𝑇 (where 𝐷 ∈ ℝ𝑛). Furthermore, in the case where 𝑣 ∈ ℝ1 → 𝑣 ∈ ℝ, Eq. 2-17
reduces to Eq. 2-16. If both conditions are true, then Eq. 2-17 fully reduces to Eq. 2-15.
These reduction checks serve to indicate that the general form of the matrix derivative
in Eq. 2-17 is valid in every case that we will encounter here.
Finally (for completeness’ sake), a derivative taken with respect to a matrix is
only valid with respect to a scalar [11]. The derivative 𝐷 of the scalar 𝑦 with respect to a
matrix 𝑀 is thus defined as
𝐷 ≝
[ 𝜕𝑦
𝜕𝑀11⋯
𝜕𝑦
𝜕𝑀𝑛1
⋮ ⋱ ⋮𝜕𝑦
𝜕𝑀1𝑚⋯
𝜕𝑦
𝜕𝑀𝑛𝑚]
[11] Note that if 𝑀 ∈ ℝ𝑛×𝑚 → 𝑀 ∈ ℝ𝑛 then 𝐷 reduces to Eq. 2-17, thus we find that the
generality of these definitions hold for all special cases.
34
As a point of interest, some conventions transpose the resulting derivative
matrices for various reasons. Equations 2-15 through 2-17 thus represent the
untransposed form, giving them “the advantage of better agreement of matrix products
with composition schemes such as the chain rule [12].”
Reference [11] compactly provides the form of each derivative given the type of
quantity being differentiated, and that which it is being differentiated with respect to.
Table 2-1. Summary of derivatives discussed in this section, as outlined in [11]. The first row shows the type of quantity being differentiated; the first column shows the type of quantity it is being differentiated with respect to.
Diff. W.R.T. Diff. Of Scalar Diff. Of Vector Diff. Of Matrix
Scalar 𝑑𝑦
𝑑𝑥
𝑑𝑢
𝑑𝑥= [
𝜕𝑢𝑖𝑑𝑥]
𝑑𝑀
𝑑𝑥= [
𝜕𝑀𝑖𝑗
𝑑𝑥]
Vector 𝑑𝑦
𝑑𝑣= [
𝜕𝑦
𝜕𝑣𝑗]
𝑑𝑢
𝑑𝑣= [
𝜕𝑢𝑖𝑑𝑣𝑗
]
Matrix 𝑑𝑦
𝑑𝑀= [
𝜕𝑦
𝜕𝑀𝑗𝑖]
The properties of matrix derivatives are largely similar to those of scalar
derivatives. They can be found summarized in [13], [14], and [15]. Of particular use is
the “multivariate chain rule” which will allow us to differentiate quantities with multiple
dimensions and dependencies.
𝑑𝑓 =𝜕𝑓
𝜕𝑥1𝑑𝑥1 +⋯+
𝜕𝑓
𝜕𝑥𝑛𝑑𝑥𝑛 (2-18)
35
CHAPTER 3 METHODOLOGY
Formal Definition of an APF
An APF is a scalar potential function, hereafter denoted by 𝜑 or 𝜑 (𝓅) where
𝓅 ∈ ℛ𝑛 is a vector containing 𝑛 independent variables. For example, if the state is
written as 𝓅 = [𝑥 𝑦]𝑇 (corresponding to coordinates in a horizontal plane), then 𝜑 can
be visualized as a surface in 3-dimensional Cartesian space as in Figure 3-1.
Figure 3-1. An abstracted potential function containing a sample of six initial states, and their corresponding paths to the nearest local minimum.
36
Recall from [3] that states (coordinates on the surface) corresponding to low
values of 𝜑 are more desirable than those corresponding to high values. By inspection,
it can be seen that an arbitrarily chosen point on the surface (represented in Figure 3-1
by the six sample points 𝓅𝑖 = [𝑥𝑖 𝑦𝑖] (𝑖 = 1,… ,6)) lies on a unique path formed by the
local negative gradient. According to the method of Lyapunov, such a path must always
terminate at a local minimum. In Figure 3-1, the two possible local minima are
represented by the points 𝓅𝐴 = [𝑥𝐴 𝑦𝐴] and 𝓅𝐵 = [𝑥𝐵 𝑦𝐵]. If, instead of a fixed point
in space, the potential-bound state is visualized as a loose particle free to move along
the surface of the APF, then a particle released at any point will propagate towards a
more desirable state by following such a path.
To state this more formally: A state 𝓅 constrained to an APF 𝜑 and initial
condition 𝓅0 will always propagate towards a local minimum if and only if the rate �̇� at
which the state’s potential changes is negative-definite at every point along the path
(not inclusive of the final endpoint, at which �̇� = 0).
There are three requirements for the asymptotically stable convergence of
Lyapunov’s method, which are mathematically expressed in [16] as:
𝜑(0) = 0;𝜑 (𝓅) > 0 ∀ 𝓅 ≠ 0, (3-1)
�̇� (𝓅) ≤ 0 ∀ 𝓅, (3-2)
‖𝜑 (𝓅)‖ → ∞ as ‖𝓅‖ → ∞. (3-3)
The first requirement provided by Eq. 3-1 requires the positive semi-definiteness
of the APF. If negative values of 𝜑 are allowed, then a perpetually negative �̇� would
conceivably result in values of 𝜑 approaching (negative) infinity.
37
The second requirement (Eq. 3-2) therefore pertains to the positive semi-
definiteness of the TD1 of the APF. This requirement is more obvious than the other
two, as a positive �̇� would lead away from convergence to a local minimum.
Finally, the third requirement (Eq. 3-3) pertains to the radial unboundedness of
the APF. This is, perhaps, the least intuitive requirement. Note that the example
potential function in Figure 3-1 takes the shape of a “bowl”, which is to say that it
approaches infinity in any radial direction. This allows for asymptotic convergence for
states whose initial conditions are of large magnitude compared to the primary
workspace of the system.
Observing all requirements, the system is asymptotically stable in state space
(the space containing all possible values for the state 𝓅) for any initial condition 𝓅0 [16].
However, it is worth noting that there is no condition to guarantee convergence to the
absolute minimum, as other local minima may exist (recall the local minimum problem
described in Chapter 1).
Defining the Error States
In the absence of constraints, only an attractive potential is necessary to drive
the state of the spacecraft towards its desired value. However, the state being
manipulated must first be defined. Let 𝑞 be the quaternion representing the present
orientation of the spacecraft, and 𝑞𝑓 be the quaternion representing the orientation that
the spacecraft is attempting to reach. Recall from Chapter 2 that the quaternion error
corresponding to the difference between the current and final states is
𝑞𝑒 = [−휀𝑓
× + 𝜂𝑓𝕀 −휀𝑓
휀𝑓𝑇 𝜂𝑓
] [휀𝜂] (2-9)
38
Note that as 𝑞 → 𝑞𝑓, Eq. 2-9 reduces to 𝑞𝑒 = [0𝑇 ±1]𝑇 – the “effective zero state
orientation” which represents convergence. Thus the quaternion error represents the
relative rotation of the current “body frame” (the reference frame fixed to the satellite)
relative to the final “tracking frame” as shown in Figure 3-2.
Figure 3-2. The relative orientations of the inertial, body, and tracking frames.
The first observation we can make about the effective zero state orientation is
that its magnitude is nonzero. More specifically, since 𝑞𝑒 represents a pure rotation, it is
necessarily of unit length. However, in our discussion of the method of Lyapunov in
Chapter 1, it was stated that the purpose of the controller is to drive the relative state to
zero. This requirement has not changed: even though the effective zero state has a
nonzero magnitude, it is representative of a zero degree rotation about any axis, and
can therefore be regarded as “zero in nature” or “effectively” zero.
39
The second observation is that there is not one, but two possible values of
convergence. Specifically, the parameter 𝜂𝑒 can converge to either +1 or −1, which
corresponds to a relative rotation of either 0° or 360° about any axis. This arises from
the sign ambiguity demonstrated in Eqs. 2-8a and 2-8b. Note that, no matter which state
is converged upon, the end result is zero relative attitude.
Another quantity of importance is the “angular velocity error” 𝜔𝑒 – the angular
velocity of the body frame with respect to the tracking frame, which is implicitly
parameterized in the body frame. As such, 𝜔𝑒 can be calculated by taking the difference
of the body frame angular velocity 𝜔, and the tracking frame angular velocity 𝜔𝑒 (after it
has been re-evaluated in the body frame). Thus
𝜔𝑒 = 𝜔 − 𝑅𝑒𝜔𝑓 (3-4)
where 𝑅𝑒 is the DCM representation of the orientation represented by 𝑞𝑒.
The Unconstrained Case
With the relative states defined, we may now specify an attractive potential to
bring it to its zero value. Recall from Chapter 1 that this has been typically accomplished
by using a Lyapunov potential function of the form
𝑉 =1
2∑𝐼𝑖𝜔𝑖
2
3
𝑖=1
+1
2∑𝜆(𝜃𝑖 − 𝜃𝑖)
23
𝑖=1
, (1-12)
which was effective when the only constraints being enforced were directly placed on
the orientation of the spacecraft, and not the angular velocity. It will eventually be shown
that a general framework requires the separation of the APF into two distinct stages –
one for orientation, and one for angular velocity. For now, we will only consider an APF
that affects the former (indicated by a subscript 1):
40
𝜑1,𝑎 =1
2휀𝑒𝑇휀𝑒 +
1
2(1 − 𝜂𝑒
2). (3-5)
Note that unlike Eq. 1-13, Eq. 3-5 is in terms of the quaternion components 휀𝑒 and 𝜂𝑒.
A quick check shows that as 𝑞𝑒 → [0𝑇 ±1]𝑇, the potential 𝜑1,𝑎 approaches 0
(regardless of the unit length requirement or sign ambiguity of 𝑞𝑒). Furthermore, since
휀𝑒𝑇휀𝑒 > 0 ∀ 휀 ≠ 0, and 𝜂𝑒
2 ≝ cos2(𝜃𝑒 2⁄ ) < 1 ∀ 0° < 𝜃𝑒 < 360°, it follows that 𝜑1,𝑎 > 0
when 𝑞𝑒 ≠ [0𝑇 ±1]𝑇. Thus, Eq. 3-5 satisfies the condition of positive definiteness.
The condition of radial unboundedness may be ignored in this instance; since the
magnitude of the state 𝑞𝑒 never approaches infinity, there is no need to check for radial
unboundedness. However, in order to verify that Eq. 3-2 is satisfied, we must calculate
the TD1 of Eq. 3-5. Applying the chain rule yields
�̇�1,𝑎 =𝜕𝜑1,𝑎𝜕𝑞𝑒
𝑑𝑞𝑒
𝑑𝑡. (3-6)
Recognizing that 𝑞𝑒 = [휀𝑒𝑇 𝜂𝑒]
𝑇, it follows from Eq. 2-17 that
𝜕𝜑1,𝑎𝜕𝑞𝑒
= [𝜕𝜑1,𝑎𝜕휀𝑒
𝜕𝜑1,𝑎𝜕𝜂𝑒
] = [휀𝑒𝑇 −𝜂𝑒] = − [𝑞𝑒
−1]𝑇
. (3-7)
Considering Eq. 3-7 and Eq. 2-6, Eq. 3-6 can be rewritten as
�̇�1,𝑎 = −1
2[Ξ𝑇 (𝑞𝑒)𝑞𝑒
−1]𝑇
𝜔𝑒. (3-8)
Recall that Eq. 3-8 must be at least negative semi-definite to be in accordance
with Eq. 3-2. However, this can only be guaranteed by specifying a form for one or more
of its variables. The quaternion error 𝑞𝑒 has a known value determined by on-board
sensors, implying that its form cannot be adjusted to determine the control solution.
41
However, assume that the angular velocity error 𝜔𝑒 → 𝜔𝑒,𝑑 is a desired value,
rather than a current evaluation. Allowing this, then the form of 𝜔𝑒,𝑑 can be specified
such that Eq. 3-8 is negative semi-definite and Eq. 3-2 is satisfied. To ensure this, we
define a positive definite gain matrix 𝐾1. By definition, 𝐾1 satisfies the inequality relation
𝑣𝑇𝐾1𝑣 > 0 ∀ 𝑣 ∈ ℝ3. Thus, a negative semi-definite equation for �̇�1,𝑎 is
�̇�1,𝑎 = −1
2[Ξ𝑇 (𝑞𝑒) 𝑞𝑒
−1]𝑇
𝐾1 [Ξ𝑇 (𝑞𝑒)𝑞𝑒
−1]. (3-9)
Relating Eq. 3-8 and Eq. 3-9, it follows that
𝜔𝑒,𝑑 = 𝐾1Ξ𝑇 (𝑞𝑒) 𝑞𝑒
−1. (3-10)
Equation 3-10 indicates the “desired angular velocity error” of the spacecraft. If
mission controllers could adjust the angular velocity of the spacecraft directly without
the need for angular momentum exchange devices on board, then 𝜔𝑒,𝑑 represents the
“target” angular velocity at the current orientation relative to the tracking state. However,
since this is not the case, a second stage APF must be designed to compute the torque
that must be supplied by onboard systems.
Just as the first stage APF served to drive the relative orientation towards its
effective zero value, the second stage APF must drive the relative angular velocity
towards zero. However, in this case “relative angular velocity” refers to the difference
between the actual value of the current angular velocity error (determined from Eq. 3-4),
and its desired value (determined from Eq. 3-10). Such a potential can take the form
𝜑2,𝑎 =1
2Δ𝜔𝑒
𝑇Δ𝜔𝑒, (3-11)
where Δ𝜔𝑒 ≝ 𝜔𝑒 − 𝜔𝑒,𝑑. Note that Eq. 3-1 is satisfied since Δ𝜔𝑒𝑇Δ𝜔𝑒 > 0 ∀ Δ𝜔𝑒 ≠ 0.
42
Likewise, substituting large magnitude values of Δ𝜔 into Eq. 3-11 satisfies the
condition of radial unboundedness. However, we must take the TD1 of Eq. 3-11 to verify
that Eq. 3-2 is satisfied. Once more referring to proof 3 of Appendix A, we find that
�̇�2,𝑎 = Δ𝜔𝑒𝑇Δ�̇�𝑒, (3-12)
where Δ�̇�𝑒 contains the torque vector 𝜏 for which we can specify a form to ensure the
negative semi-definiteness of Eq. 3-12.
The TD1 of Δ𝜔𝑒 is
Δ�̇�𝑒 = �̇�𝑒 − �̇�𝑒,𝑑 , (3-13)
where �̇�𝑒,𝑑 must be computed separately. However, �̇�𝑒 can be re-written in terms of
known angular velocities (and their TD1s), as demonstrated in proof 1 of Appendix B.
Defining a new quantity 𝒻𝑒 called the “feed forward vector”, (so named because it
propagates information about the current angular velocity and orientation forward
through time) Eq. 3-13 can be rewritten as
Δ�̇�𝑒 = �̇� + 𝒻𝑒, (3-14)
where
𝒻𝑒 ≝ 𝜔×ℛ𝑒𝜔𝑓 −ℛ𝑒�̇�𝑓 − �̇�𝑒,𝑑 . (3-15)
An attractive aspect of Eq. 3-14 is that the first term �̇� – the angular velocity rate
of the body as measured in the body frame – is now the only one containing the torque
vector 𝜏. It follows from Euler’s equations for rotational motion for a rigid body that
�̇� = 𝕁−1 (𝜏 − 𝜔×𝕁𝜔), (3-16)
where 𝕁 is the centroidal moment of inertia tensor for the rigid body.
43
Substituting Eq. 3-16 into Eq. 3-14, then further substituting into Eq. 3-12 yields
�̇�2,𝑎 = Δ𝜔𝑒𝑇 [𝕁−1 (𝜏 − 𝜔×𝕁𝜔) + 𝒻𝑒], (3-17)
where we may enforce the negative definiteness of Eq. 3-17 by specifying a favorable
form for the control state. In this case, we define a second positive definite gain matrix
𝐾2 such that
�̇�2,𝑎 = −Δ𝜔𝑒𝑇𝐾2Δ𝜔𝑒. (3-18)
Equating Eq. 3-17 with Eq. 3-18, we find the favorable form of 𝜏 to be
𝜏 = 𝜔×𝕁𝜔 − 𝕁 (𝐾2Δ𝜔𝑒 + 𝒻𝑒). (3-19)
The practice of using a sequence of two APFs in this manner is called “back-
stepping”, and will eventually be used to separate repulsive potential function into APFs
that affect orientation from those which affect angular velocity. Because the angular
velocity of a system cannot be integrated to obtain its orientation, this approach is
absolutely necessary to ensure a valid control solution.
General Constraints
Using the attractive potentials of Eq. 3-5 and Eq. 3-11, the body-fixed coordinate
frame of the spacecraft will always converge asymptotically towards its desired value,
specified by the tracking frame. This is because we have specified a form for the control
state which satisfies the conditions of Eqs. 3-1 through 3-3.
Note that this analysis was predicated on the assumption that no constraints
existed – thus the case was said to be “unconstrained.” As discussed in Chapter 1, we
would eventually like to create a controller which satisfies any constraint (chief among
which is the keep-out constraint).
44
These constraints are each represented by a “repulsive potential.” As the name
implies, the repulsive potentials work against the attractive potential to ensure that the
state does not cross a constraint boundary. Since there are two stages of attractive
potential, there will also be two stages of repulsive potential. Furthermore – just as with
the attractive potentials – the first stage repulsive potential affects changes in
orientation, while the second affects changes in angular velocity.
First though, we must consider the structure of the APF common to both stages.
If the total APF 𝜑 is merely the superposition of its constituent (attractive and repulsive)
potentials, then its magnitude is
𝜑 =∑𝜑𝑖
𝑁
𝑖=0
, (3-20)
where 𝜑𝑖 is the 𝑖th potential function of any form.
Note that in either stage, there is always an attractive potential component to Eq.
3-20. If we assume that 𝑖 = 0 is “reserved” for the attractive potential, Eq. 3-20 may be
rewritten as
𝜑 = 𝜑𝑎 +∑𝜑𝑖
𝑁
𝑖=1
. (3-21)
Recall that, in order to satisfy Eq. 3-1, we require that Eq. 3-21 evaluates to zero
at the goal state. Since each potential is independent of all others, we can only
generally assume that their sum is zero if each term is itself zero. This was done by
assuming the form
𝜑 = 𝜑𝑎 +∑𝑐𝑖𝜑𝑎𝜑𝑟,𝑖
𝑁
𝑖=1
= 𝜑𝑎 (1 +∑𝑐𝑖𝜑𝑟,𝑖
𝑁
𝑖=1
) ≝ 𝜑𝑎𝜑𝑟 , (3-22)
45
which has the TD1
�̇� = �̇�𝑎𝜑𝑟 + 𝜑𝑎�̇�𝑟 . (3-23)
In Eq. 3-22, 𝑐𝑖 is a positive scaling constant, and 𝜑𝑟,𝑖 is the 𝑖th repulsive potential.
Thus, the relationship between the attractive and repulsive potential is (somewhat
unexpectedly) multiplicative rather than additive. However, being potentials, each 𝜑𝑟,𝑖
must be positive definite with a negative definite TD1. Therefore, the definition of 𝜑𝑟 (the
“total repulsive potential”) prohibits it from every being larger than one.
It then follows that Eq. 3-22 can equal zero if and only if the attractive potential is
zero. In other words, the repulsive potentials affect the path to convergence, but they do
not determine the goal state. For this reason, there is little use in expressing repulsive
potentials in terms of the error state, or even the final state.
First Stage Repulsive Potential
In general, a first stage repulsive potential may be a function of any orientation
illustrated in Figure 3-2 (i.e. 𝜑1,𝑟 = 𝜑1,𝑟 (𝑞, 𝑞𝑓, 𝑞𝑒)). Although, the quaternion error 𝑞𝑒 is a
function of the body and tracking orientations (𝑞 and 𝑞𝑓 respectively), it is a convenient
parameter to represent terms containing a variance on both 𝑞 and 𝑞𝑓. Because of this,
we will carry on treating the generalized APF as a function of all three orientations,
though we may later reduce its dependency to be only on the body-frame orientation. In
holding with the form of Eq. 3-22, the TD1 of the potential can be written thusly:
�̇�1,𝑟 =∑𝑐𝑖�̇�1,𝑟,𝑖
𝑁
𝑖=1
. (3-24)
46
The differentiation of the individual repulsive potentials must be carried out using
the multivariate chain rule of Eq. 2-18. Doing so yields:
�̇�1,𝑟,𝑖 =𝜕𝜑1,𝑟,𝑖𝜕𝑞
𝜕𝑞
𝜕𝑞𝑒�̇�𝑒 +
𝜕𝜑1,𝑟,𝑖𝜕𝑞𝑓
𝜕𝑞𝑓
𝜕𝑞𝑒�̇�𝑒 +
𝜕𝜑1,𝑟,𝑖𝜕𝑞𝑒
�̇�𝑒. (3-25)
Equation Eq. 3-25 can be reduced by expressing it in terms of the quaternion error
matrices 𝑄1 and 𝑄2. Recall from Eq. 2-12 that
𝑞𝑒 = 𝑄1 (𝑞2) 𝑞1 = 𝑄2 (𝑞1) 𝑞2. (2-12)
Proof 3 of Appendix B demonstrates that 𝑄1 and 𝑄2 are orthogonal, therefore
𝜕𝑞
𝜕𝑞𝑒= 𝑄1
𝑇 (𝑞𝑓) ; 𝜕𝑞𝑓
𝜕𝑞𝑒= 𝑄2
𝑇 (𝑞). (3-26)
Next, recall that the TD1 of any unit quaternion can be computed from Eq. 2-14
in terms of the quaternion rate matrix Ξ:
�̇� =1
2Ξ (𝑞)𝜔. (2-14)
Thus substitution of Eqs. 2-14 and 3-26 into Eq. 3-25 yields
�̇�1,𝑟,𝑖 =1
2[Ξ𝑇 (𝑞𝑒) [𝑄1 (𝑞𝑓)𝑔1,𝑟,𝑖 (𝑞) + 𝑄2 (𝑞)𝑔1,𝑟,𝑖 (𝑞𝑓) + 𝑔1,𝑟,𝑖 (𝑞𝑒)]]
𝑇
𝜔𝑒, (3-27)
where
𝑔𝑖 (𝓅) ≝ ∇𝓅(𝜑𝑖) ≝ [𝜕𝜑𝑖𝜕𝓅
]
𝑇
(3-28)
is the gradient of the APF 𝜑𝑖 with respect to the parameter 𝓅, and can generally be
calculated from Eq. 2-17.
47
Owing to APF superposition, the gradient of the total repulsive potential 𝜑1,𝑟 is
equal to the sums of the individual APF gradients:
𝑔1,𝑟 (𝑞) =∑𝑐𝑖
𝑁
𝑖=1
𝑔1,𝑟,𝑖 (𝑞) ; (3-29a)
𝑔1,𝑟 (𝑞𝑓) =∑𝑐𝑖
𝑁
𝑖=1
𝑔1,𝑟,𝑖 (𝑞𝑓) ; (3-29b)
𝑔1,𝑟 (𝑞𝑒) = ∑𝑐𝑖
𝑁
𝑖=1
𝑔1,𝑟,𝑖 (𝑞𝑒). (3-29c)
Substitution of Eq. 3-27 into Eq. 3-24 and simplified by Eqs. 3-29a through c yields
�̇�1,𝑟 =1
2[Ξ𝑇 (𝑞𝑒) [𝑄1 (𝑞𝑓) 𝑔1,𝑟 (𝑞) + 𝑄2 (𝑞)𝑔1,𝑟 (𝑞𝑓) + 𝑔1,𝑟 (𝑞𝑒)]]
𝑇
𝜔𝑒. (3-30)
Furthermore, recall that repulsive potentials are generally represented as functions of
the body state, in which case Eqs. 3-29b and 3-29c evaluate to zero. Therefore
�̇�1,𝑟 =1
2[Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)𝑔1,𝑟 (𝑞)]
𝑇
𝜔𝑒. (3-31)
Equations 3-27 and 3-30 demonstrate a pattern which inspired the definition of a
new quantity – the “potential rate transformation” (PRT) vector – which converts the
angular velocity error into an APF rate. The PRT vector is generally defined to be
Φ(𝜑) ≝ Ξ𝑇 (𝑞𝑒) [𝑄1 (𝑞𝑓)𝑔𝜑 (𝑞) + 𝑄2 (𝑞)𝑔𝜑 (𝑞𝑓) + 𝑔𝜑 (𝑞𝑒)], (3-32)
where the PRT vector (given by Eq. 3-32) satisfies the relation
�̇� =1
2Φ𝑇(𝜑)𝜔𝑒. (3-33)
48
Setting 𝜑 = 𝜑1,𝑟,𝑖 and 𝜑 = 𝜑1,𝑟 reduces Eq. 3-33 to Eqs. 3-27 and 3-31
respectively. We therefore expect the relation to hold if 𝜑 = 𝜑1,𝑎, in which case the PRT
evaluates to
Φ(𝜑1,𝑎) = −Ξ𝑇 (𝑞𝑒) 𝑞𝑒
−1. (3-34)
As expected, substitution of Eq. 3-34 into Eq. 3-33 confirms the result found in Eq. 3-8.
We may now recast Eq. 3-23 in terms of the PRT vector for first stage potentials:
�̇�1 =1
2[Φ(𝜑1,𝑎)𝜑1,𝑟 +Φ(𝜑1,𝑟)𝜑1,𝑎]
𝑇𝜔𝑒 . (3-35)
Encouragingly, Eq. 3-35 is of the same form as Eq. 3-33. Thus, it follows that the PRT
for the overall first-step potential is
Φ(𝜑1) = 𝜑1,𝑟Φ(𝜑1,𝑎) + 𝜑1,𝑎Φ(𝜑1,𝑟). (3-36)
As in the uncontrolled case, the control solution can be determined by ensuring
the negative definiteness of Eq. 3-35. Once more adopting the positive definite gain
matrix 𝐾1 from Eq. 3-9, this form was taken to be
𝜔𝑒,𝑑 = −𝐾1Φ(𝜑1). (3-37)
Note that in the case of no repulsive potentials (i.e. the unconstrained case), Eq.
3-36 reduces to Eq. 3-34, and thus Eq. 3-37 reduces to Eq. 3-10. Ergo, Eq. 3-37
represents the generalized angular velocity error – computing its value requires only
that we compute the gradients with respect to orientation for each repulsive potential.
As with the unconstrained case, this intermediate control solution determines the
desired angular velocity error. Determining the “true” control solution requires the
general implementation of second stage repulsive potentials. We therefore expect such
a solution to reduce to Eq. 3-19 in the special case of no repulsive potentials.
49
Second Stage Repulsive Potential
Recall that the TD1 of the desired angular velocity error appears in Eq. 3-15 as
part of the feed forward vector 𝒻𝑒. Differentiating Eq. 3-37, we find that
�̇�𝑒,𝑑 = −𝐾1Φ̇(𝜑1),
thus requiring us to differentiate Eq. 3-36:
Φ̇(𝜑1) =1
2[Φ(𝜑1,𝑟)Φ
𝑇(𝜑1,𝑎) + Φ(𝜑1,𝑎)Φ𝑇(𝜑1,𝑟)]𝜔𝑒
+ [𝜑1,𝑟Φ̇(𝜑1,𝑎) + 𝜑1,𝑎Φ̇(𝜑1,𝑟)].
(3-38)
The PRTs in Eq. 3-38 are all determined. However, their rates in the second term must
be determined by differentiating Eq. 3-32 evaluated at 𝜑 = 𝜑1,𝑟 and 𝜑 = 𝜑1,𝑎. An
important consequence of this is that a gradient appearing in Eq. 3-32 will be
differentiated into what is known as a Hessian matrix 𝐻𝑖 (𝓅), which is defined as
𝐻𝑖 (𝓅) ≝ ∇∇𝓅(𝜑𝑖) ≝𝜕
𝜕𝓅𝑔𝑖 (𝓅). (3-39)
Proofs 4 and 5 in Appendix B show the TD1s of the first-stage repulsive and attractive
PRTs to be respectively evaluated to
Φ̇(𝜑1,𝑟) = [Ξ𝑇 (𝑞𝑒)𝑄1 (�̇�𝑓) + Ξ
𝑇 (�̇�𝑒)𝑄1 (𝑞𝑓)] 𝑔1,𝑟 (𝑞) + Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)𝐻1,𝑟 (𝑞) �̇� (3-40)
and
Φ̇(𝜑1,𝑎) =1
2(𝕀 − 2휀𝑒휀𝑒
𝑇)𝜔𝑒 − Ξ𝑇 (�̇�𝑒) 𝑞𝑒
−1. (3-41)
From Eqs. 3-40 and 3-41 follows the total PRT rate from Eq. 3-38, and thus the
rate of the desired angular velocity error. Therefore, with knowledge about angular
velocity and acceleration of the tracking coordinate frame (information which would be
specified by mission controllers), the feed forward vector from Eq. 3-15 is known!
50
However, the feed forward vector is only part of what is necessary to compute
the control solution. We have still yet to define the total second stage potential, or the
repulsive potentials it is comprised from.
It stands to reason that the second stage attractive potential defined 𝜑2,𝑎 does
not change from the unconstrained case in Eq. 3-11, since repulsive potentials do not
affect the relative state. Therefore, if we choose the multiplicative relation between the
attractive potential and repulsive potential as defined in Eq. 3-22, then the total second
stage potential can be written as
𝜑2 =1
2Δ𝜔𝑒
𝑇(Δ𝜔𝑒𝜑2,𝑟), (3-42)
where 𝜑2,𝑟 is the total second stage repulsive term of the form defined in Eq. 3-22.
Since the second stage potentials control the angular velocity of the spacecraft, it
stands to reason that angular velocity will be one of the arguments. Perhaps less
intuitive is the dependence on orientation, which is not required for most applications,
but should be accounted for nonetheless.
Unsurprisingly, the control solution is embedded within the TD1 of Eq. 3-42.
Direct differentiation by way of product rule yields
�̇�2 = Δ𝜔𝑒𝑇 (Δ�̇�𝑒𝜑2,𝑟 +
1
2Δ𝜔𝑒�̇�2,𝑟), (3-43)
where Δ�̇�𝑒 contains the known feed forward vector and �̇� (which in turn contains the
control solution 𝜏).
Evaluating the TD1 of 𝜑2,𝑟 by chain rule, it follows that
�̇�2,𝑟 = 𝑔2,𝑟𝑇 (𝜔)�̇� +
1
2𝑔2,𝑟𝑇 (𝑞)Ξ (𝑞)𝜔, (3-44)
where 𝑔 is the gradient vector defined in Eq. 3-28.
51
Recall that, as a consequence of Eq. 3-22,
𝑔2,𝑟(𝜔) =∑𝑐𝑖𝑔2,𝑟,𝑖(𝜔)
𝑁
𝑖=1
; (3-45a)
𝑔2,𝑟 (𝑞) =∑𝑐𝑖𝑔2,𝑟,𝑖 (𝑞)
𝑁
𝑖=1
. (3-45b)
Note that the striking similarity of Eqs. 3-45a and b to Eqs. 3-29a through c, thus
indicating that the methodology for these second stage potentials will be similar to that
for the first stage potentials. This is true, to a point. Since all first stage potentials were
functions of orientation, they could be consolidated into one quantity – the PRT vector.
However, because there is no integral relationship between angular velocity and
orientation, there is no analogous “trick” for the second stage potentials. Therefore, we
must proceed with direct substitution of Eq. 3-44 into Eq. 3-43:
�̇�2 = Δ𝜔𝑒𝑇 {Δ�̇�𝑒𝜑2,𝑟 +
1
2Δ𝜔𝑒 [𝑔2,𝑟
𝑇 (𝜔)�̇� +1
2𝑔2,𝑟𝑇 (𝑞)Ξ (𝑞)𝜔]}. (3-46)
Further substitution of Eq. 3-14 into Eq. 3-46 allows us to fully separate terms
containing �̇� from those containing 𝜔 and 𝑞:
�̇�2 = Δ𝜔𝑒𝑇 {[𝜑2,𝑟𝕀 +
1
2Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝜔)] �̇� + [1
4Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝑞)Ξ (𝑞)𝜔 + 𝜑2,𝑟𝒻𝑒]}. (3-47)
Recall that the torque vector 𝜏 parameterized in the body frame is related to �̇� by
Euler’s equation for rotational motion (Eq. 3-16). As with the first stage, we once more
reclaim a positive definite gain matrix from the unconstrained case to create a form for
�̇�2 that is negative definite. In this case, equating Eq. 3-18 with Eq. 3-47 yields
−𝐾2Δ𝜔𝑒 = [𝜑2,𝑟𝕀 +1
2Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝜔)] �̇� + [1
4Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝑞)Ξ (𝑞)𝜔 + 𝜑2,𝑟𝒻𝑒]. (3-48)
52
Note that the common factor of Δ𝜔𝑒𝑇 has removed from both sides, thus ignoring
the trivial case where Δ𝜔𝑒 = 0 (which implies convergence has occurred anyway). Thus,
the left-hand side of Eq. 3-48 is representative of the negative definite form of �̇�2, while
the right-hand side is the physical expression containing the control solution.
To solve for the torque vector, we must first solve for �̇�. Consolidating the terms
of Eq. 3-48, we find that
− [𝜑2,𝑟𝕀 +1
2Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝜔)] �̇� =1
4Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝑞)Ξ (𝑞)𝜔 + 𝜑2,𝑟𝒻𝑒 + 𝐾2Δ𝜔𝑒. (3-49)
Solving for the control solution requires the inversion of [𝜑2,𝑟𝕀 +1
2Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝜔)], for which
we make use of the Kailath Variant of The Woodbury Identity provided in [13]:
[𝜑2,𝑟𝕀 +1
2Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝜔)]−1
=1
𝜑2,𝑟[𝕀 −
Δ𝜔𝑒𝑔2,𝑟𝑇 (𝜔)
2𝜑2,𝑟 + 𝑔2,𝑟𝑇 (𝜔)Δ𝜔𝑒
]. (3-50)
Thus applying Eq. 3-50 to Eq. 3-49, and rewriting �̇� in terms of 𝜏 (in accordance with
Euler’s equation of rotational motion), we find that the control solution is given by
𝜏 = 𝜔×𝕁𝜔 −1
𝜑2,𝑟𝕁 [𝕀 −
Δ𝜔𝑒𝑔2,𝑟𝑇 (𝜔)
2𝜑2,𝑟 + 𝑔2,𝑟𝑇 (𝜔)Δ𝜔𝑒
] [1
4Δ𝜔𝑒𝑔2,𝑟
𝑇 (𝑞)Ξ (𝑞)𝜔 + 𝜑2,𝑟𝒻𝑒 +𝐾2Δ𝜔𝑒]. (3-51)
Note that in the absence of repulsive APFs, the repulsive potential gradients
become zero, and the magnitude of the total repulsive potential goes to one. It follows
that Eq. 3-51 reduces precisely to Eq. 3-19:
𝜏 = 𝜔×𝕁𝜔 − 𝕁 (𝐾2Δ𝜔𝑒 + 𝒻𝑒), (3-19)
which is the control solution for the unconstrained case. This encouraging result
indicates that we have indeed found an asymptotically stable solution for general
constraints applied in the second stage.
53
Equation 3-51 belies a complex web of variables with many interdependencies
and relationships – all of which are illustrated in Figure 3-3.
Figure 3-3. Dependency chart for the generalized control solution. Although imposing, Figure 3-3 demonstrates one of the primary advantages of
APF controls: There are no algorithms; no interpolation; and every dependency is
expressed analytically and in closed-form. The advantage of this is that it makes
computing a control solution quick and efficient, which may be desired over optimality.
54
CHAPTER 4 IMPLEMENTATION
Notable Constraints
The Pointing Constraint
With the framework in place, we are now able to construct models for various
constraints that are of concern to mission controllers. Of chief importance is the
“pointing constraint” – an elegantly simple problem which has a readily apparent impact
on the performance of real-world satellites in orbit.
However, at this stage in our discussion of the problem, it may not be clear what
is pointing where. A spacecraft may occupy any one of an infinite number of attitudes,
and to simply say “a satellite cannot point in a direction” is unhelpful unless we can
quantify what exactly that means.
For the purposes of this document, consider a spacecraft to be a collection of
body-fixed “sensor” vectors. These vectors may be representative of any direction fixed
in the body-fixed coordinate frame – solar panels (barring the use of any rotational
mechanisms to which they might be affixed); directional antennas; telescopes; star
trackers; sun sensors; or arbitrary vectors defined as a point of reference are all valid
examples of what could constitute a “sensor” in this context. All sensor vectors are
implicitly of unit length; expressed in the body-fixed coordinate frame of the spacecraft;
and denoted by the unit vector �̂�.
Next, we define an inertially-fixed “constraint” vector �̂� which is of unit length and
expressed in the inertial coordinates frame. These vectors specify a direction
corresponding to an attitude which mission controllers would either like to avoid, or
55
converge to. These constraint vectors will nearly always be accompanied by a
constraint angle 𝛽, representing a range of significant attitudes.
Significant attitude ranges can be represented in space by a cone whose axis is
parallel to �̂�, and whose half-aperture angle is equal to 𝛽. Furthermore, any sensor on
the spacecraft may have a different set of significant attitude ranges associated with it,
which may be either “attitude forbidden” or “attitude mandatory.”
For example, we may consider one of the body-fixed sensor vectors to represent
a directional high-gain antenna that can only operate when aimed at an Earth-bound
tracking station accurate to within some pre-defined margin of error. Conversely, the
sensor vector may represent some light-sensitive piece of equipment, in which case we
might require it to avoid pointing at the Sun, and even specifying an angular buffer zone
about the inertially fixed body-sun vector. Despite these possibilities, for now we need
only consider the case of a satellite with one body-fixed sensor vector and one
corresponding attitude forbidden zone (or “keep-out” cone).
Single sensor – Single cone. Let 𝛾 represent the angle between the cone axis
�̂� and the sensor vector �̂�. The sensor will consequently never enter the attitude
forbidden region if 𝛾 is never allowed to become less than or equal to 𝛽. Mathematically,
this constraint can be expressed as
𝛽 < 𝛾 (4-1)
which, while compact, does not clearly demonstrate a dependence on the orientation
state 𝑞. With the benefit of foresight, we may evaluate the cosine of Eq. 4-1 (keeping in
mind that the inequality relationship between 𝛾 and 𝛽 will be inverted), yielding
cos𝛽 − cos 𝛾 > 0. (4-2)
56
Recall that �̂� ∙ �̂� = cos 𝛾, since both �̂� and �̂� are unit vectors. However, the
equivalent matrix representation �̂�𝑇�̂� is only valid if �̂� and �̂� are expressed in the same
basis. If we first rotate �̂� into the body frame, then we may rewrite Eq. 4-2 as
cos 𝛽 − �̂�𝑇𝑅𝑞�̂� > 0,
where
𝑅𝑞 = (𝜂2 − 휀𝑇휀)𝕀 + 2휀휀𝑇 − 2𝜂휀× (4-3)
is the DCM representing the rotation represented by the quaternion 𝑞.
If the goal is to create a “barrier” at the edge of the keep-out cone, then the
repulsive APF should take the form
𝜑1,𝑟,point =1
cos𝛽 − �̂�𝑇𝑅𝑞�̂� (4-4)
which tends towards infinity as 𝛾 approaches 𝛽.
Recall from our discussion in Chapter 3 that, in order to implement a constraint at
the first stage, we must first determine its gradient (first derivative) and Hessian (second
derivative) with respect to 𝑞. This can be done by applying chain rule to Eq. 4-4, yielding
𝜕𝜑1,𝑟,point
𝜕𝑞=
𝜕
𝜕�̂�𝑇𝑅𝑞�̂�(
1
cos𝛽 − �̂�𝑇𝑅𝑞�̂�)𝜕
𝜕𝑞(�̂�𝑇𝑅𝑞�̂�) (4-5)
where
𝜕
𝜕�̂�𝑇𝑅𝑞�̂�(
1
cos 𝛽 − �̂�𝑇𝑅𝑞�̂�) =
1
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 (4-6)
and
𝜕
𝜕𝑞(�̂�𝑇𝑅𝑞�̂�) = 2𝑞𝑇 [
�̂��̂�𝑇 + �̂��̂�𝑇 − �̂�𝑇�̂� 𝕀 �̂�×�̂�
(�̂�×�̂�)𝑇
�̂�𝑇�̂�] ≝ 2𝑞𝑇Λ(�̂�, �̂�). (4-7)
57
Verification of Eq. 4-7 is provided in proof 4 of Appendix A. As a point of interest,
Λ(�̂�, �̂�) is a constant, symmetrical matrix function of the geometry between the satellite’s
sensor vectors the inertial attitude vectors. Note that the vector �̂� appearing in Eq. 4-7 is
still parameterized in the inertial frame. Thus, the gradient is computed by substituting
Eqs. 4-6 and 4-7 into Eq. 4-5:
𝑔1,𝑟,point (𝑞) =2Λ(�̂�, �̂�)𝑞
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2. (4-8)
The Hessian matrix for the direct pointing constraint potential is computed by
differentiating Eq. 4-8. The process is shown in proof 5 of Appendix A shows the
derivation, yielding the result
𝐻1,𝑟,point (𝑞) =2Λ(�̂�, �̂�)
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 [𝕀 +
4𝑞𝑞𝑇Λ(�̂�, �̂�)
cos𝛽 − �̂�𝑇𝑅𝑞�̂�]. (4-9)
Given the framework derived in Chapter 3, the outputs of Eqs. 4-8 and 4-9 are
enough to compute the control solution. However, recall that this only applies for the
case of one sensor and one keep-out cone. Variations on the constraints presented in
Eqs. 4-1 and 4-2 are also of interest.
For example, consider the inverse case of an attitude mandatory zone. In such a
case, the constraint from Eq. 4-1 becomes reversed, thus yielding
𝛽 > 𝛾. (4-10)
Note that, like Eq. 4-1, Eq. 4-10 can be equivalently stated in terms of angle cosines:
cos 𝛾 − cos 𝛽 > 0.
Thus, it can be seen that the APF corresponding to this constraint – and, by extension,
its gradient and Hessian – are the negatives of Eqs. 4-4, 4-8, and 4-9 respectively.
58
Multiple sensors – Multiple cones. In general, it cannot be assume that a
satellite contains a single body-fixed sensor vector. Nor can it be assumed that the
mission will only contain one attitude range of significance. Therefore, in general we
may need to consider a system of 𝑁 sensor vectors and 𝑀 constraint regions. For
simplicity, we will assume none of the constraint regions intersect, but if they did then it
may serve us well to model them as a single constraint region anyway.
It is convenient to store this information in matrix form, making it easily read by
linear algebra systems such as MatLab®. The matrices containing sensor and cone data
could, for example, be respectively formatted:
𝑠𝑙𝑖𝑠𝑡 = [ �̂�1 𝐵 �̂�2
𝐵 �̂�3 𝐵 ⋯ �̂�𝑖
𝐵 ⋯ �̂�𝑁 𝐵 ] ∈ ℝ3×𝑁, (4-11)
𝑑𝑙𝑖𝑠𝑡 = [�̂�1 𝐼
𝛽1
�̂�2 𝐼
𝛽2
�̂�3 𝐼
𝛽3⋯
�̂�𝑗 𝐼
𝛽𝑗⋯
�̂�𝑀 𝐼
𝛽𝑀] ∈ ℝ4×𝑀 , (4-12)
where the superscripts 𝐵 and 𝐼 respectively denote body-fixed and inertially-fixed
coordinate parameterizations.
It is also useful to specify a constant “mapping” matrix, which defines the
relationship each sensor has with the constraint regions. This would be useful in cases
where some sensors may be permitted through constraint regions that others may not,
or vice versa. For example, a sensor might be constrained to point towards Earth, yet
there would be no reason why it could not be pointed at the sun. Conversely, a star
tracker would be able to safely point towards the Earth, but it may be desirable to
specify a constraint preventing it from being pointed towards the sun.
Harkening back to the general constraint form from Eq. 3-22, the coefficients 𝑐𝑖𝑗
which modify the repulsive potentials – each one representing the constraint imposed
59
on the 𝑖th sensor by the 𝑗th cone – must be positive if the relationship is attitude
forbidden, negative if it is attitude mandatory, or zero if there is no attitude constraint.
Expressed in matrix form, the mapping matrix 𝐶 is defined as
𝐶 ≝ [
𝑐11 𝑐12𝑐21 𝑐22
⋯𝑐1𝑀𝑐2𝑀
⋮ ⋱ ⋮𝑐𝑁1 𝑐𝑁2 ⋯ 𝑐3𝑀
] ; {
𝑐𝑖𝑗 > 0 if forbidden
𝑐𝑖𝑗 = 0 if no constraint
𝑐𝑖𝑗 < 0 if mandatory
. (4-13)
The total pointing constraint potential and its derivatives can be found by
summing over all indices (𝑖, 𝑗) in Eq. 4-13, then extracting �̂�𝑖 from Eq. 4-11 and �̂�𝑗 , 𝛽𝑗
from Eq. 4-12:
𝜑1,𝑟,point = ∑𝑐𝑖𝑗
cos𝛽𝑗 − �̂�𝑖𝑇𝑅𝑞�̂�𝑗
𝑁,𝑀
𝑖,𝑗=1
; (4-14)
𝑔1,𝑟,point (𝑞) = ∑2𝑐𝑖𝑗Λ(�̂�𝑖, �̂�𝑗)𝑞
(cos𝛽𝑗 − �̂�𝑖𝑇𝑅𝑞�̂�𝑗)
2
𝑁,𝑀
𝑖,𝑗=1
; (4-15)
𝐻1,𝑟,point (𝑞) = ∑2𝑐𝑖𝑗Λ(�̂�𝑖 , �̂�𝑗)
(cos𝛽𝑗 − �̂�𝑖𝑇𝑅𝑞�̂�𝑗)
2 [𝕀 +4𝑞𝑞𝑇Λ(�̂�𝑖, �̂�𝑗)
cos 𝛽𝑗 − �̂�𝑖𝑇𝑅𝑞�̂�𝑗
]
𝑁,𝑀
𝑖,𝑗=1
. (4-16)
Recall that the outputs of Eqs. 4-14 through 4-16 determine the control solution.
The Slew Rate Constraint
Sometimes, it may be desirable to limit the rate at which a satellite rotates. There
are many different reasons why mission controllers would want to do this. Chiefly, this
constraint exists to prevent saturation of on-board angular momentum exchange
devices. The constraint may also be in place to prevent mass-spring-damper type
oscillations in nonrigid components (such as solar panels, omnidirectional antennae, or
gravity gradient stabilization booms).
60
Limiting the slew rate requires the specification of a maximum allowable slew
rate, effectively creating an envelope around the angular velocity domain. It follows that
the slew rate constraint is, by definition, a second stage potential. The maximum slew
rate is necessarily a scalar quantity, and so must be related to the instantaneous
angular velocity by the dot product:
𝜔𝑚𝑎𝑥2 −𝜔𝑇𝜔 > 0. (4-17)
Just as with the pointing constraint, a barrier can be constructed at the constraint
boundary by inverting the right-hand side of Eq. 4-17, thus yielding the slew rate
potential
𝜑2,𝑟,slew =1
𝜔𝑚𝑎𝑥2 − 𝜔𝑇𝜔. (4-18)
Unlike the first stage potentials, we do not need to take the Hessian of Eq. 4-18.
Instead, we must take the gradient of it with respect to orientation (which yields a zero
vector, since Eq. 4-18 has no dependency on orientation), and angular velocity.
Applying chain rule to Eq. 4-4 yields
𝑔2,𝑟,slew =2𝜔𝑇
𝜔𝑚𝑎𝑥2 −𝜔𝑇𝜔.
Early Avoidance
While concise, the pointing constraint potential loses efficiency when the tracking
frame will place the sensor on the opposite side of an attitude forbidden region. In such
a situation, more control effort is expended reversing the direction of the sensor. In the
edge case where the zero potential is on the exact opposite side of an attitude forbidden
region, and there is no lateral component of angular velocity, the sensor will become
mired in an unstable saddle point.
61
In almost every case, interference from other potentials will perturb the sensor
from its saddle point. Even if there are no other potentials present, numerical
uncertainty in the initial condition will almost certainly result in a lateral component of
angular velocity. In light of this, it is tempting to disregard these saddle points. However,
even if they do not permanently halt the progress of the satellite in its current maneuver,
they will slow it down, or result in a longer path that would otherwise be achieved.
One of the earliest objectives for this research was to avoid such attitude
forbidden regions in advance by imparting lateral velocity as soon as it became
apparent that an attitude forbidden region was incoming. This was accomplished by
creating a pathway between two planes tangent to the keep-out cone, whose
intersection contained the sensor vector itself. As long as the sensor continues to travel
outside of this pathway, it will not enter the constraint region.
Defining the pathway vectors. This method is based on the principle that a unit
vector will only change by a direction that lies inside the plane normal to that unit vector.
Placing this plane such that it contains the point at the head of �̂� will yield a conic
intersection where the plane crosses the attitude forbidden region. The constraint
pathway is bounded by the tangents of the conic intersection as illustrated in Figure 4-1.
Note that the vectors �̂�1, �̂�2, and �̂�3, �̂�1, �̂�2, and �̂�3 in Figure 4-1 are defined
differently from those in Eqs. 4-11 and 4-12. In this context they represent the basis
vectors 𝒟 and 𝒮 respectively, and are defined so as to simplify the parameterization of
the conic tangent vectors.
62
Figure 4-1. Setup for the Early Avoidance pathway.
The cone’s axis of symmetry makes a logical choice for the first axis �̂�1 in the 𝒟
coordinate basis. However, being inertially fixed, it must be represented in the body
coordinate frame. The second axis �̂�2 is thus defined to be that orthogonal to �̂�1 in the
direction of the sensor vector �̂�, with the third axis �̂�3 being the cross product of the first
two. Thus, the basis vectors in the 𝒟 coordinate system are defined as
𝒟 ≝
{
�̂�1 = ℛ𝑞�̂�
�̂�2 =�̂�1×�̂�×�̂�1sin 𝛾
�̂�3 = �̂�1×�̂�2
. (4-19)
63
As a consequence of the way in which the cone-fixed basis vectors are defined,
the first axis �̂�1 in the 𝒮 coordinate system is coincident with �̂�3. Helpfully, this allows us
to define the third axis �̂�3 to be identical to the sensor vector �̂�, with the cross product
defining the second axis �̂�2. Thus, the basis vectors in the 𝒮 system are defined as
𝒮 ≝ {
�̂�1 = �̂�3
�̂�2 = �̂�3×�̂�1
�̂�3 = �̂�
. (4-20)
Given the coordinate systems defined in Eqs. 4-19 and 4-20, the equation for the
conic intersection of the sensor travel plane with the attitude forbidden region is
𝑥2 = −(
cos2𝛾 + cos 2𝛽
1 + cos 2𝛽)𝑦2 + 2(
sin 2𝛾
1 + cos2𝛽) 𝑦 + (
cos2𝛾 − cos2𝛽
1 + cos2𝛽)
(4-21)
as shown in proof 6 of Appendix B. Note that 𝑥 and 𝑦 correspond to the distances
between �̂�3 and a point on the conic intersection as measured along the �̂�1 and �̂�2 axes
respectively. Equation 4-21 is – not surprisingly – symmetric about the �̂�2 axis.
Let the vector 𝑣 ≝ 𝑥�̂�1 + 𝑦�̂�2 represent the position of a point on the conic
intersection relative to �̂�3. Defining an angle 𝛼 as the angle between �̂�2 and 𝑣, it follows
from geometry that 𝛼 will reach its maximum value at the point when 𝑣 → 𝑣𝑡 where 𝑣𝑡 is
tangent to the conic intersection. If the tangent coordinates in the sensor travel plane
are defined as 𝑥 = 𝑥𝑡 and 𝑦 = 𝑦𝑡, then it follows from proof 7 of Appendix B that
𝑥t =1
sin 2𝛾√(cos 2𝛽 − cos2𝛾)(1 − cos2𝛽); 𝑦𝑡 =
cos 2𝛽 − cos2𝛾
sin 2𝛾. (4-22)
Recognizing that tan 𝛼 = |𝑥𝑡 𝑦𝑡⁄ |, it follows from Eq. 4-22 that
tan𝛼 = √1 − cos2𝛽
cos 2𝛽 − cos2𝛾. (4-23)
64
The double conic problem. An interesting yet unintended consequence of the
conic section in Eq. 4-21 is the symmetry that exists about the plane normal to the cone
axis – i.e. when plotting Eq. 4-22, one can see a discontinuity as the tangent point
jumps to the lower branch of the conic section (a cone equal in size but opposite in
direction of the keep-out cone being analyzed). Figure 4-2 illustrates this phenomenon:
Figure 4-2. An illustration of the double conic problem. Note that the origin represents
the sensor axis �̂�3, and the red dot represents the cone axis �̂�1.
Ideally, tan 𝛼 would approach zero as 𝛾 approaches the safety angle 𝛾𝑠 (as
shown in Figure 4-1), which would result in a “leading edge” of the potential as the
sensor crossed into the region where it is in danger of crossing into the keep-out cone.
However, Figure 4-2 demonstrates that no such tangent actually exists. Therefore, in
order to enforce the desired behavior, we project the tangent point from the lower
branch of the hyperbola onto the upper branch with a compensating logic term ℓ:
ℓ = 𝑚(sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
(4-24a)
65
where
𝑛 = {0 if 𝛾 ≤
𝜋
2
1 if 𝛾 >𝜋
2
; 𝑚 = {0 if 𝛾 ≥ 𝛾𝑠1 if 𝛾 < 𝛾𝑠
. (4-24b)
The conditions in Eqs. 4-24a and b are derived in proof 8 of Appendix B.
Note that the safety angle 𝛾𝑠 depends on the value of 𝛽. When 𝛽 ≤𝜋
4, then the
tangent point projection represented by Eq. 4-24a is valid for any 𝛾 < 𝛾𝑠 =𝜋
2+ 𝛽. Note,
however, that when 𝛽 ≥𝜋
4, then the projection is only valid when the sensor is between
both branches of the conic intersection (which will be hyperbolic over this part of the
domain. We are able to express both possibilities compactly by taking the minimum
value to be the true safety angle:
𝛾𝑠 = min (𝜋
2+ 𝛽, 𝜋 − 𝛽)
The compensating term in Eqs. 4-24a and b serves to divide the domain of
gamma into three sectors, thus constructing a piece-wise expression for tan𝛼 that
demonstrates the behavior we are looking for. The three sectors are summarized in
Table 4-1, and the range of values over which ℓ varies is graphed in Figure 4-3.
The compensated tangent value can be obtained by multiplying the
uncompensated value from Eq. 4-23 to the logic term of Eq. 4-24a, thus yielding
tan𝛼 = 𝑚(sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
√1 − cos2𝛽
cos 2𝛽 − cos2𝛾 (4-25)
Plotting the modified pathway vectors using Eq. 4-25 demonstrates that the new logic
term does indeed solve the double conic problem, as shown in Figure 4-4.
66
Table 4-1. Values of the logic term ℓ across the three sectors of the gamma domain.
𝛾 (Range as Inequality) 𝑚 𝑛 ℓ
min (𝜋
2+ 𝛽, 𝜋 − 𝛽) ≤ 𝛾 ≤ 𝜋 𝑚 = 0 𝑛 = 1 ℓ = 0
𝜋
2< 𝛾 < min (
𝜋
2+ 𝛽, 𝜋 − 𝛽) 𝑚 = 1 𝑛 = 1 ℓ =
sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾
0 ≤ 𝛾 ≤𝜋
2 𝑚 = 1 𝑛 = 0 ℓ = 1
Figure 4-3. Example of variation in the logic term ℓ over the domain
𝜋
2< 𝛾 < 𝛾𝑠.
Figure 4-4. Results of applying the logic term ℓ to the double conic problem.
1.6 1.65 1.7 1.75 1.8 1.85 1.9 1.95 2 2.050
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Logic Term Values Between /2 and /2+ ( = /6)
[rad]
logic
term
valu
e [-]
67
The early avoidance potential. Recall from our discussion of the pathway
vectors that the sensor’s rate of change �̇̂� is contained entirely within the sensor travel
plane, and therefore is coplanar to said pathway vectors. Defining 𝛿 to be the angle
between �̇̂� and �̂�2, the early avoidance potential is defined as
𝜑2,𝑟,early = (tan𝛼) exp [𝑐𝛿
𝛿2 − 𝛼2] ; 𝛿 < 𝛼. (4-26)
where 𝑐 is a scaling constant tan𝛼 is computed from Eq. 4-25. Note that 𝛼 is a function
of 𝛾, which is in turn a function of 𝑞. Furthermore, 𝛿 is a function of �̇̂�, which is in turn a
function of 𝜔. Therefore Eq. 4-26 is a second stage potential, and must be differentiated
once with respect to orientation, and once with respect to angular velocity.
Applying chain rule to Eq. 4-26, it becomes clear that the dependency on
orientation or angular velocity is linked to the term inside the exponent. Therefore, if we
let the state vector 𝓅 denote a “dummy variable” that can be either the orientation or the
angular velocity, then the gradient of 𝜑2,𝑟,early with respect to 𝓅 is
𝜕𝜑2,𝑟,early
𝜕𝓅= [
𝜕 tan𝛼
𝜕𝓅+ (tan 𝛼)
𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2)] exp [
𝑐𝛿2
𝛿2 − 𝛼2].
With the benefit of foresight, we may factor out the tan 𝛼 term, thus yielding
𝜕𝜑2,𝑟,early
𝜕𝓅= [
1
tan 𝛼
𝜕 tan𝛼
𝜕𝓅+𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2)]𝜑2,𝑟,early , (4-27)
where
𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2) =
1
1 + tan2 𝛼
2𝑐𝛼𝛿2
(𝛼2 − 𝛿2)2𝜕 tan 𝛼
𝜕𝓅+
1
sin 𝛿
2𝑐𝛿𝛼2
(𝛼2 − 𝛿2)2𝜕 cos 𝛿
𝜕𝓅 (4-28)
as shown in proof 6 of Appendix A.
68
Therefore, by substituting Eq. 4-28 into Eq. 4-27 and refactoring expressions, we
obtain the “cleanest” possible expression for the gradient:
𝑔2,𝑟,early (𝓅) = [(1
tan 𝛼+
𝒞𝛼(𝛿)
1 + tan2 𝛼)𝜕 tan 𝛼
𝜕𝓅+𝒞𝛿(𝛼)
sin 𝛿
𝜕 cos 𝛿
𝜕𝓅]
𝑇
𝜑2,𝑟,early (4-29)
where
𝒞𝛼(𝛿) ≝2𝑐𝛼𝛿2
(𝛼2 − 𝛿2)2; 𝒞𝛿(𝛼) ≝
2𝑐𝛿𝛼2
(𝛼2 − 𝛿2)2.
Note that the apparent singularities resulting from the division by (𝛼2 − 𝛿2)2 and
sin 𝛿 are resolved in the cases where 𝛿 → 𝛼 and 𝛿 → 0 because of the presence of
numerator terms that approach zero in either case. Therefore Eq. 4-29 can be shown to
be defined everywhere except for the barrier case where 𝛼 →𝜋
2 (which only occurs as
the sensor approaches the keep-out cone). However, for coding purposes, it will be
necessary to limit the value of sin 𝛿 to a minimum value of 𝜖 – the smallest real number
that MatLab® is able to represent given a maximum of 1 – to prevent Not-a-Number
(NaN) errors from occurring.
Returning to Eq. 4-29, if we set 𝓅 → 𝑞, then the gradient becomes
𝑔2,𝑟,early (𝑞) = [(1
tan𝛼+
𝒞𝛼(𝛿)
1 + tan2 𝛼)𝜕 tan𝛼
𝜕𝑞+𝒞𝛿(𝛼)
sin 𝛿
𝜕 cos 𝛿
𝜕𝑞]
𝑇
𝜑2,𝑟,early. (4-30)
It follows from proof 7 of Appendix A that
𝜕 tan 𝛼
𝜕𝑞= 4𝑞𝑇Λ(�̂�, �̂�) [𝑛
8 sin2 2𝛽 cos2𝛾
sin4 2𝛽 − sin4 2𝛾+
1
cos2𝛽 − cos2𝛾] cos 𝛾 tan𝛼. (4-31)
Similarly, proof 8 of Appendix A shows that
𝜕 cos𝛿
𝜕𝑞=2𝑞𝑇
sin 𝛾[Λ(�̂̇�, �̂�) +
cos 𝛾 cos 𝛿
sin 𝛾Λ(�̂�, �̂�)]. (4-32)
69
Note that when 𝓅 → 𝜔, then the gradient becomes
𝑔2,𝑟,early(𝜔) = [𝒞𝛿(𝛼)
sin 𝛿
𝜕 cos𝛿
𝜕𝜔]
𝑇
𝜑2,𝑟,early . (4-33)
Recall that tan 𝛼 is solely a function of orientation, therefore its derivative with respect to
𝜔 is identically zero. It then follows from Proof 9 of Appendix A that
𝜕 cos𝛿
𝜕𝜔=[ℛ𝑞�̂�]
𝑇
sin 𝛾
�̂̇�× �̂̇�×�̂�×
|�̇̂�|. (4-34)
Since the goal of the potential in Eq. 4-26 is to repel the sensor rate vector �̇̂�
away from the most direct path towards the keep-out cone �̂�2, it can be regarded as
repelling the value of 𝛿 away from 𝛿 = 0 until 𝛿 ≥ 𝛼. Thus, Eqs. 4-30 through 4-34
represent the equations required by the APF controller framework built in Chapter 3.
Figure 4-5 shows the shape of the potential in the 𝛿 for various values of 𝛼.
Figure 4-5. The early avoidance potential as a function of 𝛿 for various values of 𝛼.
-2 -1.5 -1 -0.5 0 0.5 1 1.5 20
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
[rad]
r [
-]
Early Avoidance APF for Various Values of
= 15o
= 30o
= 45o
= 60o
= 75o
= 86o
= 90o
70
Developing a MatLab® Simulation
Because of the difficulty in visualizing the control solution as a function of multiple
potentials, it was necessary to build a simulation in MatLab® to ensure that the
previously discussed potentials functioned as desired.
Once completed, the simulation was applied to a series of test cases to
heuristically determine the scaling and gain coefficients, assess local minima, and finally
to examine a quasi-real world case.
At its most fundamental level, the simulation consists of three stages: Dynamics,
Integration, and Processing. The complete simulation code can be found in Appendix C.
Dynamics
The first stage is a 1:1 transcription of the principles discussed in Chapter 3, with
separate functions comprising the first and second stage potentials. This level of the
code receives a state vector 𝑥 ∈ ℝ21 which contains each orientation and angular
velocity corresponding to the body frame, tracking frame, and error frame respectively.
Thus, 𝑥 is of the form
𝑥𝑇 = [𝑞𝑇 𝜔𝑇 𝑞𝑓𝑇 𝜔𝑓
𝑇 𝑞𝑒𝑇 𝜔𝑒
𝑇]. (4-35)
In reality, the ultimate mission of the controller is to determine the instantaneous
control torque required to ensure the spacecraft’s orientation and angular velocity states
will converge to their desired values. However, since the controller code in this case
was written to test the APFs directly, it was made to return the TD1 of the state instead,
which was integrated to determine the motion of the spacecraft. The TD1 of Eq. 4-35 is
�̇�𝑇 = [�̇�𝑇 �̇�𝑇 �̇�𝑓𝑇 �̇�𝑓
𝑇 �̇�𝑒𝑇 �̇�𝑒
𝑇]. (4-36)
71
The orientation rates �̇�, �̇�𝑓, and �̇�𝑒 which appear in Eq. 4-36 are all computed by
applying Eq. 2-14 to the known values of the state vector in Eq. 4-35. This yields
enough information to compute the control solution from Eq. 3-51. Supplying 𝜏 and 𝜔 to
Euler’s equations of rotational motion (Eq. 3-16) yields the instantaneous angular
velocity rate �̇�. Furthermore, the angular velocity rate of the tracking frame �̇�𝑓 is
supplied to the control system (as a function of time) by mission controllers. However,
for reasons which will be expounded upon later, this should nearly always be zero.
Once the values of �̇� and �̇�𝑓 are known, the controller computes �̇�𝑒.
Integration
It has thus far been shown that the dynamics derivative �̇� (given by Eq. 4-36) is
fully parameterized in terms of the state vector 𝑥 (given by Eq. 4-35). Thus the ordinary
differential equation (ODE) describing the motion of the controlled satellite is of the form
�̇� = 𝑓(𝑡, 𝑥). (4-37)
It follows that if the state vector at the “current” simulation time 𝑡 = 𝑡0 is known
(i.e. 𝑥(𝑡0) = 𝑥0), then the corresponding value of its derivative is also known. Thus, if we
discretize the time domain over the range 𝑡 = [0, 𝑡𝑓] in intervals of length 𝑑𝑡 (ensuring its
value to be sufficiently small), then we may numerically integrate Eq. 4-37 to determine
the value of the state vector at the “next” simulation time 𝑡 = 𝑡0 + 𝑑𝑡 using an
appropriate Runge-Kutta (RK) method.
A general RK method numerically integrates an ODE of the form demonstrated in
Eq. 4-37; “an n-dimensional, first-order set of parametric equations [17].” RK methods
evaluate the independent variable at the next time using the following approximation:
72
𝑥(𝑡0 + 𝑑𝑡) ≅ 𝑥0 + 𝑑𝑡 ∙ Φ. (4-38)
The symbol Φ (not to be confused with the PRT vector from Chapter 3) is the
“increment function” and “should closely approximate the slope of the line through
(𝑡0, 𝑥0) and (𝑡0 + 𝑑𝑡, 𝑥(𝑡0 + 𝑑𝑡)) [17].” In general, the increment function is of the form
Φ =∑𝑏𝑖𝑘𝑖
𝑠
𝑖=1
; (4-39)
𝑘1 = 𝑓(𝑡0 + 𝑐1𝑑𝑡, 𝑥0); (4-40)
𝑘𝑖 =
𝑓 (𝑡0 + 𝑐𝑖𝑑𝑡, 𝑥0 + 𝑑𝑡∑𝑎𝑖𝑗𝑘𝑗
𝑖−1
𝑗=1
) ; 𝑖 = 2,… , 𝑠 . (4-41)
The coefficients 𝑎𝑖𝑗, 𝑏𝑖, 𝑐𝑖 are heuristically determined to minimize the truncation
error of the approximation, which is also dependent on the order 𝑠 of the method [17].
For example, a fourth order RK method (RK4) uses the following coefficients:
Φ𝑅𝐾4 =1
6(𝑘1 + 2𝑘2 + 2𝑘3 + 𝑘4); (4-42)
𝑘1 = 𝑓(𝑡0, 𝑥0); (4-43)
𝑘2 = 𝑓 (𝑡0 +1
2𝑑𝑡, 𝑥0 +
1
2𝑑𝑡 𝑘1) ; (4-44)
𝑘3 = 𝑓 (𝑡0 +1
2𝑑𝑡, 𝑥0 +
1
2𝑑𝑡 𝑘2) ; (4-45)
𝑘4 = 𝑓(𝑡0 + 𝑑𝑡, 𝑥0 + 𝑑𝑡 𝑘3). (4-46)
Applying Eqs. 4-42 through 4-46 (or Eqs. 4-39 through 4-41 in the most general case of
RK methods) to Eq. 4-38 “approximates the exact solution up to terms of order 𝑑𝑡4
(assuming 𝑥(𝑡) is smooth, differentiable) … comparable to that of a 4th-order Taylor
series expansion [17].”
73
Initially, an RK4 function was written to accompany the controller code. However,
this was eventually supplanted with MatLab’s® built-in ODE45 integrator: an intrinsic
RK4 method which compares its output to that of a higher order RK5 method –
modulating the time step until the relative error is within tolerance [17].
Processing
The integrator output is stored in a structure whose values are arrays containing
the body, tracking, and error orientations and angular velocities for every time step. The
rows of each array are separated for ease of plotting. The body-frame orientations and
angular velocities are then compared to their corresponding tracking frame values, and
the error with respect to time is plotted.
To concisely display the path of each sensor vector, directions in space were
projected onto an equirectangular cylinder wherein the horizontal axis represents
“longitude” in the inertial frame (i.e. the angle between the horizontal plane projection of
the vector and the inertial x-axis) and the vertical axis represents “latitude” (i.e. the
angle between the horizontal plane and the vector). For reference, constraint
boundaries were plotted alongside the sensor paths.
Constant Determination. Chapter 3 introduced many constants and gains which
require their values to be “hard coded” (i.e. unchangeable and example specific). This
produced situations where a given set of coefficients would work well for one type of
situation, but break down when applied to others. Ultimately, there is no procedural way
of deciding what the value of these constants should be; necessitating heuristic
methods to decide on values that worked well for the majority of cases.
74
A “Monte Carlo” (i.e. random case analysis) engine was developed to place a
virtual spacecraft with one body-fixed sensor at random attitudes, requiring it to
converge to a randomly generated tracking frame. A single keep-out cone was present
to account for the presence of constraint potentials.
For each set of constants, one hundred such cases were run. The case average
time to convergence and the maximum magnitude of angular velocity were checked
against their “reasonable” corresponding values – a highly subjective measure which
existed only as a temporary hold-over until more rigorous methods could be
implemented – and the constants were adjusted until the system converged
appropriately.
It was determined from this method that, when given an arbitrarily chosen
moment of inertia tensor of 𝐽 = diag(5, 3, 4), the gain matrices 𝐾1 and 𝐾2 should be
𝐾1 = 0.1 𝕀,
𝐾2 = 0.6 𝕀.
Furthermore, the nonzero scaling constants for the first-stage pointing constraint
potentials were chosen to have the same magnitude:
|𝑐𝑖𝑗| = 0.35.
It was then assumed that these constant values would be sufficient for the
majority of APF control-based applications. To demonstrate this, a quasi-real world
example was constructed with a virtual satellite to show the effectiveness of this
simulation, as well as to assess the limitations of APF controls.
75
CHAPTER 5 CONCLUSION
A Quasi-Real World Example
The final step in testing the performance of this control system was to create a
realistic scenario in which as many constraints as possible would be utilized. The
hypothetical “AbstractoSat” – a virtual construction designed to be representative of a
real-world satellite – was created for the purposes of this example. AbstractoSat is
visualized in Figure 5-1.
Figure 5-1. AbstractoSat – An illustrative example of a satellite with multiple sensors.
76
AbstractoSat contains three body-fixed sensors: �̂�1 and �̂�2 correspond to a pair of
star trackers positioned approximately 25.6° to either side of the body-fixed z axis and
coplanar to the y-z plane; �̂�3 corresponds to a mono-directional communication antenna
that is parallel to the body-fixed x axis (and thus orthogonal to the other two sensors).
Recall from Eq. 4-11 that the body-fixed sensor vectors are stored in the 𝑠𝑙𝑖𝑠𝑡
array. For this example, this matrix is assigned the following entries:
𝑠𝑙𝑖𝑠𝑡 = [0
−0.5+1.0
0+0.5+1.0
+1.000].
Note that the sensor vector magnitudes are normalized within the code itself.
Next, assume that AbstractoSat has three constraints that can be placed on it.
Let two inertially-fixed vectors �̂�1 and �̂�2 represent the direction towards the sun and
moon respectively. In order to keep track of the star field, mission controllers specify
that AbstractoSat’s star trackers must remain at least 𝛽1,2 = 20° away from either
direction vector. Furthermore, the communications antenna must remain within 𝛽3 = 40°
of the relative direction vector �̂�3 in order to uplink with a ground-based relay station.
Recall from Eq. 4-12 that the constraint direction vectors are left parameterized in
the inertial coordinate system and stored in the 𝑑𝑙𝑖𝑠𝑡 array. Thus, the following values
are assigned to the constraint regions:
𝑑𝑙𝑖𝑠𝑡 = [
+1.0+1.0+1.020.0°
0−1.00
20.0°
+2.0+2.0−3.040.0°
].
Once again, the normalization of each direction vector is carried out implicitly within the
code. Furthermore, the angles of the fourth row are converted into radians.
77
It is assumed that, over the time span of the maneuver, the constraint directions
shift very little. Furthermore, the sensors are not free to move relative to the spacecraft.
In reality, the relative motion of the sun and moon throughout the course of an orbit is
significant in real-world applications. However, this assumption suits this particular
example well, and allows us to test the performance of the control system directly.
Finally, we assume that AbstractoSat has the ability to temporarily deactivate its
star trackers and communications antenna, allowing it to avoid any of these constraints.
This is represented in the code by our ability to activate or deactivate constraints so that
we may test the performance of the satellite in the same scenario with different
constraints being applied.
The orientation of the satellite was selected so as to demonstrate the effect that
each potential has on the execution of the maneuver. Thus, AbstractoSat is initially at
rest relative to the inertial frame such that its local z axis is parallel to �̂�1, and its local x
axis is oriented such that the angle that it forms with �̂�3 is minimal.
Next, the tracking frame of the satellite is specified to be at rest with the local z
axis parallel to �̂�2, and once again the local x axis is oriented such that the angle it
forms with �̂�3 is minimal.
As a consequence of these initial conditions, it is impossible to directly transition
from one starting point to another without violating at least one constraint. Furthermore,
it will be shown that only accounting for the constraints of the first two sensors will
violate the attitude mandatory condition for the third.
The origin (starting position) and terminus (stopping position) of each sensor
vector is shown mapped upon an equirectangular projection in Figure 5-2.
78
Figure 5-2. The origin and terminus points for each sensor in the AbstractoSat example.
79
Unconstrained Simulation Results
As a basis of comparison, all constraints were deactivated (i.e. the scaling
coefficients for all constraints were set to zero). Figure 5-3 shows the history of values
for each of the four Euler parameters (left column) and angular velocity components
(right column). Solid lines denote parameters of the body-fixed coordinate frame, while
dashed lines indicate tracking frame parameters. Note that the two asymptotically
converge as time increases.
Figure 5-3 indicates that the maneuver required between 60 and 80 seconds to
converge. At 100 seconds, the rotation angle error had been reduced to approximately
500 micro-radians, and the angular velocity magnitude to approximately 20 micro-
radians per second.
Figure 5-4 shows the paths traced by the sensor vectors �̂�1-�̂�3 in this scenario.
Since the maneuver is an unconstrained slew from the starting points (the small circles
in Figure 5-2) towards the end points (the small plus signs), the constraint boundaries
are completely ignored. Furthermore, since both the body-fixed coordinate frame is
made to start and end in an inertial state (not a general constraint, but rather one
specific to this example), the controller guides each sensor vector along the most direct
route from start to finish.
Counterintuitively, this most direct route is not a line but a curved shape. This is
an artifact of the projection from spherical coordinates to equirectangular coordinates –
similar to how the shortest distance between two points on a map of the Earth is an arc
(called a “great circle”) rather than a straight line.
80
Figure 5-3. Results of the unconstrained simulation.
81
Figure 5-4. Paths traced by �̂�1-�̂�3 in the unconstrained simulation.
82
Partially Constrained Simulation Results
The second test activated the attitude forbidden constraints for the sensors �̂�1
and �̂�2. As before, Figure 5-5 shows the history of values for each of the four Euler
parameters (left column) and angular velocity components (right column). As before,
solid and dashed lines denote parameters of the body-fixed and tracking coordinate
frame respectively. Note that the two still asymptotically converge as time increases.
However, we may note that in this case the maneuver required approximately 40
seconds to converge. A full factor of two less than the unconstrained case! It is believed
that this drop in convergence time was the result of an unchanging combination of gain
coefficients in the presence of a sharper first-stage potential gradient. This yields a
larger magnitude in the desired error angular velocity, which (due to the inertially fixed
tracking frame) is equivalent to the desired body frame angular velocity. The peak
angular velocity is therefore a full order of magnitude higher (as illustrated in Figure 5-5)
than the same peak for the unconstrained case, yielding a much shorter travel time.
Regardless of the travel time, when the simulation had ended, the body frame
had converged to the tracking frame to within a rotation angle error of approximately
500 micro-radians, and an angular velocity magnitude of approximately 20 femto-
radians per second. These results demonstrated desirable convergence behavior
Figure 5-6 shows the paths traced by the sensor vectors �̂�1-�̂�3. In contrast with
the unconstrained case, the path of each sensor vector can only be known by following
the exact evolution of the local negative gradient in time. Thus the exact paths traced
are unpredictable and the only useful data our analysis can turn up is that no constraints
were violated; ergo, the control system behaves as intended.
83
Figure 5-5. Results of the first pointing constraint simulation.
84
Figure 5-6. Paths traced by �̂�1-�̂�3 in the first pointing constraint simulation.
85
Fully Constrained Simulation Results
The third test activated the attitude mandatory constraint for the sensor �̂�3 in
addition to the keep-out constraints. As illustrated in Figure 5-7, the maneuver required
between 10 and 15 seconds to converge, after which point the rotation angle error
rapidly dropped off to approximately 900 micro-radians, and the angular velocity
magnitude to approximately 600 micro-radians per second.
Note that the trend of decreasing travel time which we observed between the
unconstrained case and the first constrained case continues, as the travel time required
drops by another factor of two. However, unlike the first constrained case, this is not the
result of an increased peak angular velocity, but rather the reduction in the distance
traveled by the sensor vectors.
The paths traveled by each sensor vector in the equirectangular basis are shown
in Figure 5-8. Comparing this to Figure 5-6, we see that the constraint acting on the �̂�3
forces the �̂�2 vector away from the path that it would otherwise have taken. This
manifests itself as a sharp deflection in the path of the �̂�2 vector, resulting in a shorter
path from start to finish.
It should be apparent from these three examples that travel time and peak
angular velocity are not predictable quantities, and may vary wildly depending on the
geometry of the initial and final conditions, the number of constraints acting, and so on.
The only factor which determines these maneuver properties is the shape of the
potential function driving the controller. Thus if a more direct path from start to finish is
required, then the only way to enforce that using this framework is to implement
constraints to force the path into that shape.
86
Figure 5-7. Results of the second pointing constraint simulation.
87
Figure 5-8. Paths traced by �̂�1-�̂�3 in the second pointing constraint simulation.
88
A Note on Second Stage Potentials
The second stage potentials were activated for the fourth simulation test. Based
on observations of the angular velocity in the previous three cases, a maximum slew
rate of 5 degrees per second was imposed upon the satellite. The scaling constant for
this potential was initially set to 𝑐 = 0.5.
These results were inconclusive, as ODE45 could not integrate the differential
equation. It was concluded that nearby solutions to the ODE diverge very rapidly when
second stage potentials are taken into account, i.e. the equation is “stiff” [17]. Even
MatLab’s® intrinsic stiff equation solvers – ODE15s and ODE23s – was unable to solve
the equation, warning that MatLab® was “unable to meet integration tolerances without
reducing the step size below the smallest value allowed (3.552714e-15) at time t.”
Implementing a fixed-step numerical integrator such as RK4 eliminated the
simulation hang-up, but since no verification of the error was made, the resulting
solution diverged significantly from the expected analytical value at some points in the
simulation. These points in the simulation corresponded to noise in the control solution,
accompanied by sharp discontinuities in the state values which often caused it to violate
constraint boundaries such as the slew rate constraint.
It should be noted that this result does not indicate the failure of second stage
potentials, but rather the limitations in our ability to test their functionality. It may be
possible that the equation is nonintegrable using MatLab’s® intrinsic methods.
Alternatives to ODE15s and ODE23s are currently being explored.
89
Topics for Future Research
While these numerical simulations yielded promising results for first stage
potentials, our inability to test the second stage potentials in greater depth has halted
the development of this particular control system. Understanding the properties of the
ODE that have yielded this inconclusive result is certainly a priority for future research,
yet there is currently no shortage of related topics that must also be addressed.
Considerations for Physical Actuation Devices
The methodology of this document develops a generalized method for
determination of the torque vector control solution. However, satellites in the real world
impart torque by redistributing their momentum through internal actuators known as
angular momentum exchange devices (AMED), which principally includes reaction
wheel assemblies (RWA) and control moment gyros (CMG) [18].
Reaction wheel assemblies. RWAs redistribute angular momentum by varying
the rate at which it rotates about a fixed direction. Because of this, three are often used
to allow for independent three-axis control. Furthermore, Drs. Muñoz and Leve describe
RWAs as “physically limited by rate and acceleration limits on reaction wheels [18].”
These limits “correspond to the maximum torque available from the reaction wheel
motors [and] angular momentum saturation of the actuator” respectively [18].
Angular momentum saturation results when a reaction wheel, or pair of reaction
wheels are unable to rotate fast (or slow) enough to impart the required angular
momentum ℎ (given a desired angular momentum ℎ𝑓). In order to prevent this, a
repulsive potential is designed which corresponds to the angular momentum limitation
constraint. The resultant potential is similar to the slew rate constraint discussed in
Chapter 4, and can be written as
90
𝜑𝑒 ≅(ℎ − ℎ𝑓)
𝑇(ℎ − ℎ𝑓)
2𝜇(ℎ𝑚𝑎𝑥2 − ℎ𝑇ℎ), (4-47)
where “𝜇 > 0 is a weighting parameter of the repulsive potential”, and “ℎ𝑚𝑎𝑥 is the
maximum angular momentum allowed [18].”
Control moment gyroscopes. A CMG operates differently from a RWA in that it
redistributes angular momentum by changing the axis about which it rotates, though not
necessarily its spin rate. CMGs suffer from “internal singularities (i.e., hyperbolic and
elliptic) and external singularities i.e., angular momentum saturation) corresponding to a
rank-deficient Jacobian, where a torque in a specific direction is not available [18].”
Defining the constraint for CMGs is not as straightforward as that for RWAs. The
angular momentum must be expressed in terms of “the principal rotation DCMs about
the second and third axes” 𝐶2 and 𝐶3 [18]. Thus the angular momentum of the 𝑁 CMG
system can be written as
ℎ =∑𝐶3(𝛾𝑖)𝐶2(𝜃𝑖)𝐶3(𝛿𝑖) [
𝐼𝑓𝑤Ω
0𝐼𝑔�̇�𝑖
]
𝑁
𝑖=1
, (4-48)
where “𝛾𝑖 is the separation angle”, “𝜃𝑖 is the inclination angle”, “𝛿𝑖 is the gimbal angle”,
“Ω is the flywheel speed”, “𝐼𝑓𝑤 is the inertia of the flywheel assembly”, and “𝐼𝑔 is the
inertia of the gimbal assembly [18].” Muñoz and Leve and assume a “three orthogonal
CMG configuration” which allows Eq. 4-48 to be recast as
ℎ = 𝐼𝑓𝑤Ω[
cos(𝛿1) − sin(𝛿3)
cos(𝛿2) − sin(𝛿1)
cos(𝛿3) − sin(𝛿2)]. (4-49)
91
The Jacobian matrix corresponding to Eq. 4-49 is
𝐴(𝛿) = [
− sin(𝛿1) 0 − cos(𝛿3)
− cos(𝛿1) − sin(𝛿2) 0
0 − cos(𝛿2) − sin(𝛿3)].
Thus, a new parameter called the “singularity parameter” is defined to be
𝑚 ≝ √det (𝐴 𝐴𝑇).
When the singularity parameter is zero, a CMG singularity has been reached.
Thus, defining a minimum value for the singularity parameter gives rise to the avoidance
constraint 𝑚 ≥ 𝑚𝑚𝑖𝑛. Converting this inequality constraint into a barrier potential yields
one possible expression for the singularity avoidance potential
𝜑𝑠 ≅(𝑚 −𝑚𝑓)
2
ζ(𝑚 −𝑚𝑚𝑖𝑛), (4-50)
where “ζ > 0 is the weighting parameter of the repulsive potential”, and “𝑚𝑓 is the
singularity parameter at the final gimbal angle configuration [18].”
Framework integration. Note that the general constraint framework as it exists
currently only supports potentials that modulate orientation or angular velocity.
Equations 4-47 and 4-50 are functions of new state variables that are actuator-specific.
It may be necessary to implement these potentials as part an independent outer
loop controller which processes the torque vector outputs of the inner loop, and
converts them into a series of actuator-usable parameters that avoid these singularities.
More simulations will need to be done to verify that these potentials be reconciled with
the generalized control system framework that has already been developed.
92
The State Vector Escort Problem
One potentially troublesome consequence of the way we have defined total
potential in Eq. 3-22 is that a value of zero may result in the conflicting case where the
attractive potential is sufficiently close to zero (i.e. the state vector has converged upon
its desired value) while the repulsive potential is very large (i.e. the state vector is
impinging on a constraint boundary). In terms of the limit of 𝜑𝑎 and 𝜑𝑟, this implies that
lim(𝜑𝑎,𝜑𝑟)→(0,∞) 𝜑 = lim(𝜑𝑎,𝜑𝑟)→(0,∞)
𝜑𝑎𝜑𝑟−1
→ 0 (4-51)
which occurs if 𝜑𝑎 approaches zero faster than 𝜑𝑟 approaches infinity.
If the tracking frame is then gradually re-oriented such that the state vector is
allowed to cross a nearby constraint boundary, the attractive potential well will appear to
shift along with the state vector, resulting in a continuously low value for the total
potential. Thus the attractive potential “escorts” the state vector across the constraint
boundary, thus earning this phenomenon the name of “the escort problem.”
Solving the escort problem for first stage potentials is relatively straightforward: If
the tracking frame is constrained to be at rest then there is no possible way for it to
cross over a constraint boundary. By judiciously verifying that the tracking frame itself
does not lay on, or within, any constraint boundaries, the state vector will simply avoid
them as the controller framework dictates.
However, this solution proves problematic for second-stage potentials. The
desired angular velocity error is chosen so as to force the orientation state to converge
upon its desired value, and is therefore not generally at rest. Thus, the limit in Eq. 4-51
evaluates to zero if and only if the form of the repulsive potential is judiciously chosen
with respect to that of the attractive potential.
93
Take, for example, the slew rate constraint of Eq. 4-18:
𝜑2,𝑟,slew =1
𝜔𝑚𝑎𝑥2 − 𝜔𝑇𝜔
. (4-18)
Recall that the attractive potential for second stage potentials is
𝜑2,𝑎 =1
2Δ𝜔𝑒
𝑇Δ𝜔𝑒. (3-11)
where Δ𝜔𝑒 = 𝜔𝑒 − 𝜔𝑒,𝑑. If we apply our assumption that the tracking frame does not
rotate then 𝜔𝑒 = 𝜔. Thus, in the edge case where |𝜔𝑒,𝑑| = 𝜔𝑚𝑎𝑥 , it follows that 𝜑2,𝑟,slew−1
approaches zero at the same rate as 𝜑2,𝑎. In that case the limit in Eq. 4-51 is realized.
If, however, we modify the form of Eq. 4-18 such that 𝜑2,𝑟,slew → 𝜑2,𝑟,slew2 then
𝜑2,𝑟,slew−1 will approach zero faster than 𝜑2,𝑎 . Thus the limit in Eq. 4-51 tends towards
infinity, resolving the escort problem.
It is possible that choosing a compatible form of the repulsive potential is the only
way to guarantee resolution of this issue, but more elegant ways of doing so are being
sought. Compounding this issue is our inability to test the functionality of second stage
potentials due to the stiffness of the ODE.
It is also worth noting that simply choosing a higher order repulsive potential will
not be effective if – due to exponential decay of the error state – the attractive potential
is rounded to zero by the processor. In this case, it may be possible to induce noise or
“jitter” to create small perturbations in the state vector, resulting in a nonzero potential.
Mitigation of Local Extrema
Recall from Chapter 1 that, under certain circumstances, the TD1 of the APF
may equal zero for nonzero state vector values. These “local extrema” may permanently
or temporarily disable the controller, resulting in a false control solution.
94
Suppose that an APF is set to modify the state vector 𝓅 such that the state value
𝓅 = 𝓅0 corresponds to a local extrema. Let 𝓅0 → 𝓅0 + 𝛿𝓅 where 𝛿𝓅 is a perturbation to
the state vector of small, yet nonzero, magnitude in a random direction. At such a state,
a perturbation in one or more directions may yield one of the following results:
i. Any perturbation 𝛿𝓅 ⟹ �̇� (𝓅0 + 𝛿𝓅) > 0; 𝓅0 is a “local minimum.”
ii. Any perturbation 𝛿𝓅 ⟹ �̇� (𝓅0 + 𝛿𝓅) < 0; 𝓅0 is a “local maximum.”
iii. Some but not all perturbations 𝛿𝓅 ⟹ �̇� (𝓅0 + 𝛿𝓅) < 0; 𝓅0 is a “saddle point.”
Marginally stable points. In cases ii and iii, the state 𝓅0 corresponds to a
particular class of local extrema called “marginally stable points.” In these cases, any
noise in the control system will destabilize the point, resulting in a true control solution.
However, even under these circumstances, the controller will appear to delay or “hang”
for an often nontrivial amount of time. Thus, it may be desirable to speed the process by
introducing constant jitter to prevent such points from forming in the first place.
Consider, for example, the case of a single sensor vector and a single keep-out
constraint region such as was discussed in Chapter 4. If the tracking frame is oriented
such that the final sensor vector location is coplanar to both the cone axis and in the
initial position, then the sensor vector will invariably come to rest upon a saddle point
formed by the attractive and repulsive potentials.
In this case, a comparatively small amount of numerical uncertainty was
introduced by imparting a one degree per second spin about the sensor axis. This was
enough to bypass the saddle point, but the simulation still experienced significant
delays, as can be observed in Figure 5-9.
95
Figure 5-9. Demonstration of simulation hang-up induced by nearby local extrema.
Of note is the “flattened” portion of trajectory over the part of the time domain
between approximately 10 and 50 seconds of simulation time. After 50 seconds, the
local gradient near the saddle points becomes large enough that the control system is
able to overcome the hang-up. While this result was not obtained using a jitter inducer,
a jitter inducer would very likely produce similar results.
96
Local minima. In case i, the extrema is not a marginally stable point, but is in
fact a true local minimum. In such a case, jitter will not be sufficient to nudge the state
from its current value. Under the current framework, the only way to resolve such a
condition would be to change the structure of the potentials such that a local minimum
no longer exists at that state value.
Potential-based solution. We may, however, make a key assumption about the
nature of local minima: As long as the body frame is rotating with respect to the tracking
frame (or the inertial frame if the tracking frame is inertially fixed), the second-stage
potential is constantly changing. We may therefore assume that local minima are only of
concern for first-stage potentials.
Thus, it may be possible to develop a second stage potential which addresses
the local minimum problem for first stage potentials directly. Recall that the potential
rate of change for a first stage potential can be generally expressed in terms of the
potential rate transform (PRT) vector as
�̇� =1
2Φ𝑇(𝜑)𝜔𝑒. (3-33)
By definition, a local minimum must occur when �̇� = 0. However, if we utilize our
assumption that the body frame is continuously moving with respect to the tracking
frame, then it cannot generally be assumed that |𝜔𝑒| = 0 (recall that a zero relative
angular velocity implies that the maneuver is complete). Therefore, a local minimum
must occur either when |Φ(𝜑)| = 0 or when Φ(𝜑) is orthogonal to 𝜔𝑒. Recall that the
PRT corresponding to the total potential is
Φ(𝜑1) = 𝜑1,𝑟Φ(𝜑1,𝑎) + 𝜑1,𝑎Φ(𝜑1,𝑟). (3-36)
97
Since 𝜑1,𝑟 > 1 and 𝜑1,𝑎 = 0 only at the target state, Eq. 3-36 is zero if and only if
Φ(𝜑1,𝑟) = −𝜑1,𝑟𝜑1,𝑎
Φ(𝜑1,𝑎). (4-52)
Note that we do not concern ourselves with the singular case where 𝜑1,𝑎 = 0, since this
corresponds to an infinite PRT, which will only occur if the final state lays on a constraint
boundary. We may rewrite the constraint of Eq. 4-52 by evaluating the dot product it
forms with Φ(𝜑1,𝑟):
Φ𝑇(𝜑1,𝑟)Φ(𝜑1,𝑟)
Φ𝑇(𝜑1,𝑎)Φ(𝜑1,𝑟)+𝜑1,𝑟𝜑1,𝑎
= 0. (4-53)
From Eq. 4-53, we may construct a potential which prevents Φ(𝜑1) from approaching
zero and forming a local minimum:
𝜑2,𝑟,min1 = 𝑐 (Φ𝑇(𝜑1,𝑟)Φ(𝜑1,𝑟)
Φ𝑇(𝜑1,𝑎)Φ(𝜑1,𝑟)+𝜑1,𝑟𝜑1,𝑎
)
−2
. (4-54)
The second component of the local minimum potential would prevent Φ(𝜑1) from
becoming orthogonal with 𝜔𝑒 – another condition which may form a local minimum in
the first stage. While this condition may be expressed as Φ𝑇(𝜑1)𝜔𝑒 = 0, this creates
ambiguities in the case where either Φ(𝜑1) or 𝜔𝑒 approach zero independently.
Therefore, we may express the condition in terms of the cross product:
|Φ×(𝜑1)𝜔𝑒| = |Φ(𝜑1)||𝜔𝑒| sin 90° = |Φ(𝜑1)||𝜔𝑒|. (4-55)
Thus, from Eq. 140 we may form a second potential which also prevents the formation
of local minima:
𝜑2,𝑟,min1 = 𝑐(|Φ×(𝜑1)𝜔𝑒| − |Φ(𝜑1)||𝜔𝑒|)
−2. (4-56)
98
The combined benefits from Eqs. 4-54 and 4-56 should hopefully mitigate the
formation of any local minima in the first stage potentials. However, recall that we have
required all second stage potentials to be functions of orientation and angular velocity
alone. Since both Eqs. 4-54 and 4-56 contain error terms we require the tracking frame
to be inertially fixed for them to be valid! Therefore we would benefit from seeking out a
more elegant solution to the local minimum problem.
Internal states and emergent behavior. Mohamed H. Mabrouk and Colin R.
McInnes hypothesize that “[Natural examples of emergent behavior] among agents that
interact via pair-wise attractive and repulsive potentials [can be used] to solve the local
minima problem in the artificial potential based navigation method [19].” The general
method seeks to solve the local minimum problem by using “the agents’ internal states
to employ this emergent behavior to make the swarm of agents escape local minima by
following the boundaries of obstacles [19].”
Through simulation, Mabrouk and McInnes discovered that multiple agents
interacting with each other through attractive and repulsive potential functions tended to
settle into vortical holding patterns such that the swarm center velocity approached
zero. By “increasing the group perception about the swarm state by linking the goal
gradient potential in the equation of motion to [the swarm center velocity],” they were
able to “eliminate the local minimum from the global potential, which in turn enables the
formed vortex pattern amongst the group to solve the problem [19].” Thus
rather than moving in a static potential field, the agents are able to manipulate the potential according to their estimation of whether they are moving towards the goal or stuck in a local minimum, and the method allows a swarm of agents to escape from and to manoeuvre around a local minimum in the potential field to reach a goal [19].
99
Afterword
Overall, this research has enabled a more flexible implementation of a wider
range of constraints than was initially thought possible. Although more research will
need to be conducted to verify that these methods perform as desired, early results
appear promising. As the advantages and limitations of APF-based control systems are
explored more in depth, they are likely to see more use by small satellites with limited
on-board computational resources.
However, before this day can come we must first expand our understanding of
key obstacles: the stiffness of second-stage differential equations, the escort problem,
and the local minimum problem. In some respects, the emergence and categorization of
these problems is as important a result as any numerical simulation.
Of utmost importance is the integration of this generalized framework with the
work that has already done by other researchers in the field. Through persistence,
dedication, and a little bit of creative thinking, I believe that the full potential of APF-
based controls can be utilized, and may yet revolutionize the way small satellites are
maneuvered.
100
APPENDIX A USEFUL MATRIX DERIVATIVES
Proof 1: 𝑑𝑣
𝑑𝑣= 𝕀 (A-1)
Recall that the derivative matrix 𝐷 ≝ 𝑑𝑢 𝑑𝑣⁄ of a vector with respect to another
vector is defined in Eq. 2-17 as
𝐷 ≝
[ 𝜕𝑢1𝜕𝑣1
⋯𝜕𝑢1𝜕𝑣𝑛
⋮ ⋱ ⋮𝜕𝑢𝑛𝜕𝑣1
⋯𝜕𝑢𝑛𝜕𝑣𝑚]
. (2-17)
where 𝑢 ∈ ℝ𝑛 and 𝑣 ∈ ℝ𝑚. In the case where 𝑣 = 𝑢, Eq. 2-17 can be rewritten as
𝑑𝑣
𝑑𝑣=
[ 𝜕𝑣1𝜕𝑣1
⋯𝜕𝑣1𝜕𝑣𝑛
⋮ ⋱ ⋮𝜕𝑣𝑛𝜕𝑣1
⋯𝜕𝑣𝑛𝜕𝑣𝑛]
. (A-2)
Assuming that the elements of the vector 𝑣 are independent of one another, then
the off-diagonal elements all evaluate to zero, whereas the diagonal elements evaluate
to one. Therefore
𝑑𝑣
𝑑𝑣= 𝕀, (A-1)
where 𝕀 ≝ [1 ⋯ 0⋮ ⋱ ⋮0 ⋯ 1
] is the 𝑛 × 𝑛 identity matrix.
Q.E.D. ∎
101
Proof 2: 𝑑
𝑑𝑣(𝑢𝑇𝑀 𝑤) = 𝑤𝑇𝑀𝑇
𝑑𝑢
𝑑𝑣+ 𝑢𝑇𝑀
𝑑𝑤
𝑑𝑣; 𝑀 = constant (A-3)
Note that 𝑢 ∈ ℝ𝑛 and 𝑤 ∈ ℝ𝑛 need not have the same dimensionality as 𝑣 ∈ ℝ𝑚 ,
but they must necessarily have the same dimensionality as one another in order for the
matrix multiplication in Eq. A-2 to be valid.
We must first apply the chain rule as it appears in Eq. 2-18. Doing so yields
𝑑
𝑑𝑣(𝑢𝑇𝑀 𝑤) =
𝜕
𝜕𝑢(𝑢𝑇𝑀 𝑤)
𝑑𝑢
𝑑𝑣+𝜕
𝜕𝑤(𝑢𝑇𝑀 𝑤)
𝑑𝑤
𝑑𝑣. (A-4)
Since the dot product of two vectors is a commutative operation, its linear
algebra equivalent is also commutative. Therefore
𝑑
𝑑𝑣(𝑢𝑇𝑀 𝑤) =
𝜕
𝜕𝑢(𝑤𝑇𝑀𝑇𝑢)
𝑑𝑢
𝑑𝑣+𝜕
𝜕𝑤(𝑢𝑇𝑀 𝑤)
𝑑𝑤
𝑑𝑣. (A-5)
Assuming there is no mutual dependence between 𝑢 and 𝑤:
𝑑
𝑑𝑣(𝑢𝑇𝑀 𝑤) = 𝑤𝑇𝑀𝑇
𝑑𝑢
𝑑𝑣+ 𝑢𝑇𝑀
𝑑𝑤
𝑑𝑣. (A-3)
Q.E.D. ∎
102
Proof 3: 𝑑
𝑑𝑣(𝑣𝑇𝑣) = 2𝑣𝑇 (A-6)
Recall that
𝑑
𝑑𝑣(𝑢𝑇𝑀 𝑤) = 𝑤𝑇𝑀𝑇
𝑑𝑢
𝑑𝑣+ 𝑢𝑇𝑀
𝑑𝑤
𝑑𝑣. (A-3)
If 𝑢 = 𝑤 = 𝑣 and 𝑀 = 𝕀, then Eq. A-3 can be rewritten as
𝑑
𝑑𝑣(𝑣𝑇𝑣) = 𝑣𝑇𝕀
𝑑𝑣
𝑑𝑣+ 𝑣𝑇𝕀
𝑑𝑣
𝑑𝑣= 2𝑣𝑇
𝑑𝑣
𝑑𝑣. (A-7)
Furthermore, recall from Eq. A-1 that
𝑑𝑣
𝑑𝑣= 𝕀. (A-1)
Therefore
𝑑
𝑑𝑣(𝑣𝑇𝑣) = 2𝑣𝑇 . (A-6)
Q.E.D. ∎
103
Proof 4: 𝜕
𝜕𝑞(�̂�𝑇ℛ𝑞�̂�) = 2𝑞
𝑇Λ(�̂�, �̂�); Λ(�̂�, �̂�) ≝ [�̂��̂�𝑇 + �̂��̂�𝑇 − �̂�𝑇�̂� 𝕀 �̂�×�̂�
(�̂�×�̂�)𝑇
�̂�𝑇 �̂�] (A-8)
Recall from Eq. 4-3 that
𝑅𝑞 = (𝜂2 − 휀𝑇휀)𝕀 + 2휀휀𝑇 − 2𝜂휀×. (4-3)
Directly substituting Eq. 4-3 into Eq. A-8 yields
�̂�𝑇ℛ𝑞�̂� = �̂�𝑇�̂�(𝜂2 − 휀𝑇휀) + 2�̂�𝑇휀휀𝑇�̂� − 2𝜂�̂�𝑇휀×�̂�. (A-9)
Equation A-9 can be placed into a more “differentiable” form by rearranging some
of the operations. Recalling the commutativity of dot product and the anti-commutativity
of cross product, this yields
�̂�𝑇ℛ𝑞�̂� = (�̂�𝑇 �̂�)𝜂2 − (�̂�𝑇�̂�)휀𝑇휀 + 2휀𝑇(�̂� �̂�𝑇)휀 + 2𝜂(�̂�×�̂�)
𝑇휀. (A-10)
Furthermore, applying Eq. 2-17 to the left-hand side of Eq. A-10 yields
𝜕
𝜕𝑞(�̂�𝑇ℛ𝑞�̂�) = [
𝜕
𝜕휀(�̂�𝑇ℛ𝑞�̂�)
𝜕
𝜕𝜂(�̂�𝑇ℛ𝑞�̂�)]. (A-11)
The derivatives in the right-hand side of Eq. A-11 are equal to the sums of the
derivatives of all terms in Eq. A-10. Note that terms containing only 휀 have a zero
derivative with respect to 𝜂, and vice versa. Therefore
𝜕
𝜕휀(�̂�𝑇ℛ𝑞�̂�) = −(�̂�
𝑇�̂�)𝜕
𝜕휀(휀𝑇휀) + 2
𝜕
𝜕휀(휀𝑇(�̂� �̂�𝑇)휀) + 2𝜂(�̂�×�̂�)
𝑇 𝜕휀
𝜕휀. (A-12)
Applying Eqs. A-1, A-3, and A-6 to Eq. A-12 yields
𝜕
𝜕휀(�̂�𝑇ℛ𝑞�̂�) = −2(�̂�
𝑇�̂�)휀𝑇 + 2휀𝑇(�̂� �̂�𝑇 + �̂� �̂�𝑇) + 2𝜂(�̂�×�̂�)𝑇. (A-13)
Equation A-13 can be factored into
𝜕
𝜕휀(�̂�𝑇ℛ𝑞�̂�) = 2휀
𝑇 (�̂��̂�𝑇 + �̂��̂�𝑇 − �̂�𝑇�̂� 𝕀) + 2𝜂(�̂�×�̂�)𝑇. (A-14)
104
Equation A-14 is equivalently expressed in terms of linear multiplications as
𝜕
𝜕휀(�̂�𝑇ℛ𝑞�̂�) = 2[휀𝑇 𝜂]𝑇 [
�̂��̂�𝑇 + �̂��̂�𝑇 − �̂�𝑇�̂� 𝕀
(�̂�×�̂�)𝑇
] = 2𝑞𝑇 [�̂��̂�𝑇 + �̂��̂�𝑇 − �̂�𝑇�̂�
(�̂�×�̂�)𝑇 ]. (A-15)
Next, Eq. A-10 is differentiated with respect to 𝜂, yielding
𝜕
𝜕𝜂(�̂�𝑇ℛ𝑞�̂�) = (�̂�𝑇�̂�)
𝜕
𝜕𝜂(𝜂2) + 2(�̂�×�̂�)
𝑇휀 𝜕
𝜕𝜂(𝜂) = 2(�̂�𝑇�̂�)𝜂 + 2휀𝑇 �̂�×�̂� . (A-16)
As with Eq. A-13, Eq. A-16 may be factored into
𝜕
𝜕𝜂(�̂�𝑇ℛ𝑞�̂�) = 2휀
𝑇(�̂�×�̂�) + 2𝜂(�̂�𝑇�̂�) = 2[휀𝑇 𝜂]𝑇 [�̂�×�̂�
�̂�𝑇�̂�] = 2𝑞𝑇 [
�̂�×�̂�
�̂�𝑇 �̂�]. (A-17)
Substituting Eqs. A-15 and A-17 into Eq. A-11 yields
𝜕
𝜕𝑞(�̂�𝑇ℛ𝑞�̂�) = 2𝑞
𝑇Λ(�̂�, �̂�); Λ(�̂�, �̂�) ≝ [�̂��̂�𝑇 + �̂��̂�𝑇 − �̂�𝑇�̂� 𝕀 �̂�×�̂�
(�̂�×�̂�)𝑇
�̂�𝑇�̂�]. (A-8)
Q.E.D. ∎
105
Proof 5: 𝜕
𝜕𝑞
2Λ(�̂�, �̂�)𝑞
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 =
2Λ(�̂�, �̂�)
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 [𝕀 +
4𝑞𝑞𝑇Λ(�̂�, �̂�)
cos 𝛽 − �̂�𝑇𝑅𝑞�̂�] (A-18)
Recognizing that Λ(�̂�, �̂�) is a constant matrix, differentiating (A-18) yields
𝜕
𝜕𝑞
2Λ(�̂�, �̂�)𝑞
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 = 2Λ(�̂�, �̂�)
𝕀 (cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2
− 𝑞𝜕𝜕𝑞 (cos𝛽 − �̂�
𝑇𝑅𝑞�̂�)2
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)4 . (A-19)
It follows from chain rule that
𝜕
𝜕𝑞(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)
2
= −2(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)𝜕
𝜕𝑞(�̂�𝑇𝑅𝑞�̂�). (A-20)
Applying Eq. A-8 to Eq. A-20 yields
𝜕
𝜕𝑞(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)
2
= −4(cos𝛽 − �̂�𝑇𝑅𝑞�̂�) 𝑞𝑇Λ(�̂�, �̂�). (A-21)
Substituting Eq. A-21 into Eq. A-19 yields
𝜕
𝜕𝑞
2Λ(�̂�, �̂�)𝑞
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 =
2Λ(�̂�, �̂�)
(cos𝛽 − �̂�𝑇𝑅𝑞�̂�)2 [𝕀 +
4𝑞𝑞𝑇Λ(�̂�, �̂�)
cos 𝛽 − �̂�𝑇𝑅𝑞�̂�]. (A-18)
Q.E.D. ∎
106
Proof 6: 𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2) =
1
1 + tan2 𝛼
2𝑐𝛼𝛿2
(𝛼2 − 𝛿2)2𝜕 tan 𝛼
𝜕𝓅+
1
sin 𝛿
2𝑐𝛿𝛼2
(𝛼2 − 𝛿2)2𝜕 cos 𝛿
𝜕𝓅 (A-22)
Applying chain rule to Eq. A-22 yields
𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2) =
𝜕
𝜕𝛼(
𝑐𝛿2
𝛿2 − 𝛼2)𝜕𝛼
𝜕𝓅+𝜕
𝜕𝛿(
𝑐𝛿2
𝛿2 − 𝛼2)𝜕𝛿
𝜕𝓅. (A-23)
By evaluating the partial derivatives, the right-hand side of Eq. A-23 reduces to
𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2) =
2𝑐𝛼𝛿2
(𝛼2 − 𝛿2)2𝜕𝛼
𝜕𝓅−
2𝑐𝛿𝛼2
(𝛼2 − 𝛿2)2𝜕𝛿
𝜕𝓅. (A-24)
Recognizing that 𝛼 = tan−1(tan𝛼), it follows that
𝜕𝛼
𝜕𝓅=
1
1 + tan2 𝛼
𝜕 tan𝛼
𝜕𝓅. (A-25)
Similarly, 𝛿 = cos−1(cos 𝛿). Therefore
𝜕𝛿
𝜕𝓅=
−1
√1 − cos2 𝛿
𝜕 cos𝛿
𝜕𝓅=
−1
sin 𝛿
𝜕 cos 𝛿
𝜕𝓅. (A-26)
Substituting Eqs. A-25 and A-26 into Eq. A-24 yields
𝜕
𝜕𝓅(
𝑐𝛿2
𝛿2 − 𝛼2) =
1
1 + tan2 𝛼
2𝑐𝛼𝛿2
(𝛼2 − 𝛿2)2𝜕 tan 𝛼
𝜕𝓅+
1
sin 𝛿
2𝑐𝛿𝛼2
(𝛼2 − 𝛿2)2𝜕 cos𝛿
𝜕𝓅. (A-22)
Q.E.D. ∎
107
Proof 7: 𝜕 tan𝛼
𝜕𝑞= 4𝑞𝑇Λ(�̂�, �̂�) [𝑛
8 sin2 2𝛽 cos 2𝛾
sin4 2𝛽 − sin4 2𝛾+
1
cos2𝛽 − cos 2𝛾] cos 𝛾 tan 𝛼 (A-27)
Recall from Eq. 4-25 that
tan𝛼 = 𝑚(sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
√1 − cos2𝛽
cos 2𝛽 − cos2𝛾. (4-25)
Let tan0 𝛼 be the uncompensated tangent term, and ℓ0 be the second-sector
value of the logic term ℓ, such that
tan 𝛼 = 𝑚ℓ0𝑛 tan0 𝛼. (A-28)
Recall that 𝑛 may only be zero or one depending on the value of 𝛾. If 𝑛 = 0 then,
by necessity, 𝑚 = 1. Therefore the logic term evaluates to ℓ = 1ℓ00 = 1. In this case, Eq.
A-28 is reduced to tan𝛼 = tan0 𝛼. If 𝑛 = 1, then the value of 𝑚 is variable, in which case
the logic term evaluates to ℓ = 𝑚ℓ01 = 𝑚ℓ0.
With this in mind, differentiating Eq. A-28 yields
𝜕 tan𝛼
𝜕𝑞= 𝑚𝑛
𝜕ℓ0𝜕𝑞
tan0 𝛼 +𝑚ℓ0𝑛𝜕 tan0 𝛼
𝜕𝑞. (A-29)
It will eventually prove useful to refactor Eq. A-29 thusly
𝜕 tan 𝛼
𝜕𝑞= [
𝑛
ℓ0
𝜕ℓ0𝜕𝑞
+1
tan0 𝛼
𝜕 tan0 𝛼
𝜕𝑞]𝑚ℓ0
𝑛 tan0 𝛼. (A-30)
Noting the recurrence of Eq. A-28 within Eq. A-30, we may rewrite Eq. A-30 as
𝜕 tan𝛼
𝜕𝑞= [
𝑛
ℓ0
𝜕ℓ0𝜕𝑞
+1
tan0 𝛼
𝜕 tan0 𝛼
𝜕𝑞] tan𝛼. (A-31)
Applying the chain rule to ℓ0 yields
𝜕ℓ0𝜕𝑞
=𝜕ℓ0
𝜕 sin2 2𝛾
𝜕 sin2 2𝛾
𝜕 cos2𝛾
𝜕 cos 2𝛾
𝜕 cos 𝛾
𝜕 cos 𝛾
𝜕𝑞. (A-32)
108
Evaluating the derivatives in Eq. A-32 and multiplying them together yields
𝜕ℓ0𝜕𝑞
=16 sin2 2𝛽 cos2𝛾 cos 𝛾
(sin2 2𝛽 + sin2 2𝛾)2𝜕 cos 𝛾
𝜕𝑞. (A-33)
Note that Eq. A-31 requires us to divide Eq. A-33 by ℓ0. It follows that
1
ℓ0
𝜕ℓ0𝜕𝑞
=8 sin2 2𝛽 cos 2𝛾 cos 𝛾
sin4 2𝛽 − sin4 2𝛾
𝜕 cos 𝛾
𝜕𝑞. (A-34)
Similarly, applying chain rule to tan0 𝛼 yields
𝜕 tan0 𝛼
𝜕𝑞=
𝜕ℓ0𝜕 sin2 2𝛾
𝜕 sin2 2𝛾
𝜕 cos2𝛾
𝜕 cos2𝛾
𝜕 cos 𝛾
𝜕 cos 𝛾
𝜕𝑞. (A-35)
Evaluating the derivatives, Eq. A-35 reduces to
𝜕 tan0 𝛼
𝜕𝑞=
2 cos 𝛾
cos2𝛽 − cos 2𝛾√
1 − cos2𝛽
cos 2𝛽 − cos2𝛾
𝜕 cos 𝛾
𝜕𝑞. (A-36)
Dividing Eq. A-36 by tan0 𝛼 yields
1
tan0 𝛼
𝜕 tan0 𝛼
𝜕𝑞=
2 cos 𝛾
cos2𝛽 − cos2𝛾
𝜕 cos 𝛾
𝜕𝑞. (A-37)
Substituting Eqs. A-34 and A-37 into Eq. A-31 yields
𝜕 tan 𝛼
𝜕𝑞= 2
𝜕 cos 𝛾
𝜕𝑞[𝑛
4 sin2 2𝛽 cos2𝛾
sin4 2𝛽 − sin4 2𝛾+
1
cos 2𝛽 − cos2𝛾] cos 𝛾 tan 𝛼. (A-38)
Applying Eq. A-8 to Eq. A-38, it follows
𝜕 tan𝛼
𝜕𝑞= 4𝑞𝑇Λ(�̂�, �̂�) [𝑛
8 sin2 2𝛽 cos 2𝛾
sin4 2𝛽 − sin4 2𝛾+
1
cos2𝛽 − cos 2𝛾] cos 𝛾 tan 𝛼. (A-27)
Q.E.D. ∎
109
Proof 8: 𝜕 cos𝛿
𝜕𝑞=2𝑞𝑇
sin 𝛾[Λ(�̂̇�, �̂�) +
cos 𝛾 cos 𝛿
sin 𝛾Λ(�̂�, �̂�)] (A-39)
From the definition of 𝛿, we know that
cos 𝛿 = �̂̇�𝑇 �̂�2 = −�̂̇�𝑇 �̂�×�̂�×ℛ𝑞�̂�
sin 𝛾. (A-40)
It follows from the dynamics of a unit vector fixed in the body frame that
�̂̇�𝑇 = (�̇̂�
|�̇̂�|)
𝑇
= (𝜔×�̂�
|�̇̂�|)
𝑇
= (−�̂�×𝜔
|�̇̂�|)
𝑇
=𝜔𝑇 �̂�×
|�̇̂�|. (A-41)
Substituting Eq. A-41 into Eq. A-40 yields
cos 𝛿 = −𝜔𝑇 �̂�×�̂�×�̂�×ℛ𝑞�̂�
|�̇̂�| sin 𝛾. (A-42)
A consequence of the cross product definition is that �̂�×�̂�×�̂�× = −�̂�×, therefore
cos 𝛿 =𝜔𝑇 �̂�×ℛ𝑞�̂�
|�̇̂�| sin 𝛾=�̇̂�𝑇ℛ𝑞�̂�
|�̇̂�| sin 𝛾=�̂̇�𝑇ℛ𝑞�̂�
sin 𝛾. (A-43)
Differentiating Eq. A-43 yields
𝜕 cos𝛿
𝜕𝑞=
𝜕𝜕𝑞 (�̂̇�
𝑇ℛ𝑞�̂�) sin 𝛾 − �̂̇�𝑇ℛ𝑞�̂�
𝜕 sin 𝛾𝜕𝑞
sin2 𝛾. (A-44)
Recognizing the recurrence of Eq. A-43 allows Eq. A-44 to be rewritten as
𝜕 cos 𝛿
𝜕𝑞=
𝜕𝜕𝑞 (�̂̇�
𝑇ℛ𝑞�̂�) − cos 𝛿𝜕 sin 𝛾𝜕𝑞
sin 𝛾. (A-45)
It follows from Eq. A-8 that
𝜕
𝜕𝑞(�̂̇�𝑇ℛ𝑞�̂�) = 2𝑞
𝑇Λ(�̂̇�, �̂�). (A-46)
110
Furthermore,
𝜕 sin 𝛾
𝜕𝑞=𝜕
𝜕𝑞(√1 − cos2 𝛾) =
− cos 𝛾
√1 − cos2 𝛾
𝜕
𝜕𝑞(cos 𝛾). (A-47)
Substituting Eq. A-8 into Eq. A-47 yields
𝜕 sin 𝛾
𝜕𝑞=−2 cos 𝛾
sin 𝛾𝑞𝑇Λ(�̂�, �̂�). (A-48)
Substituting Eqs. A-46 and A-48 into A-45 yields
𝜕 cos 𝛿
𝜕𝑞=2𝑞𝑇
sin 𝛾[Λ(�̂̇�, �̂�) +
cos 𝛿 cos 𝛾
sin 𝛾Λ(�̂�, �̂�)]. (A-39)
Q.E.D. ∎
111
Proof 9: 𝜕 cos𝛿
𝜕𝜔=[ℛ𝑞�̂�]
𝑇
sin 𝛾
�̂̇�× �̂̇�×�̂�×
|�̇̂�| (A-49)
It follows from Eq. A-43 that
cos 𝛿 =�̂̇�𝑇ℛ𝑞�̂�
sin 𝛾=[ℛ𝑞�̂�]
𝑇
�̂̇�
sin 𝛾. (A-50)
The only variable in Eq. A-50 that is a function of 𝜔 is the vector �̂̇�. Therefore
𝜕 cos𝛿
𝜕𝜔=[ℛ𝑞�̂�]
𝑇
sin 𝛾
𝜕�̂̇�
𝜕𝜔. (A-51)
It follows from the differentiation of Eq. A-41 that
𝜕�̂̇�
𝜕𝜔=
1
|�̇̂�|2 [�̂�
×𝜔𝜕|�̇̂�|
𝜕𝜔−𝜕
𝜕𝜔(�̂�×𝜔)|�̇̂�|]. (A-52)
Note that |�̇̂�| = √−𝜔𝑇 �̂�×�̂�×𝜔. Thus, rewriting the first derivative in Eq. A-52 yields
𝜕|�̇̂�|
𝜕𝜔=
𝜕
𝜕𝜔√−𝜔𝑇 �̂�×�̂�×𝜔 =
−𝜔𝑇 �̂�×�̂�×
√−𝜔𝑇 �̂�×�̂�×𝜔=−�̇̂�𝑇 �̂�×
|�̇̂�|= −�̂̇�𝑇 �̂�×. (A-53)
Furthermore, the second derivative in Eq. A-52 can be rewritten as
𝜕
𝜕𝜔(�̂�×𝜔)|�̇̂�| =
�̂�×|�̇̂�|2
|�̇̂�|= −
�̂�×𝜔𝑇 �̂�×�̂�×𝜔
|�̇̂�|= −(�̂̇�𝑇 �̂�×𝜔)�̂�×. (A-54)
Substituting Eqs. A-53 and A-54 into Eq. A-52, it follows that
𝜕 cos𝛿
𝜕𝜔=[ℛ𝑞�̂�]
𝑇
sin 𝛾
1
|�̇̂�|2 [�̂̇�
𝑇(�̂�×𝜔)𝕀 − (�̂�×𝜔)�̂̇�𝑇] �̂�×. (A-55)
It follows from Eq. 2-5 that �̂̇�𝑇(�̂�×𝜔)𝕀 − (�̂�×𝜔)�̂̇�𝑇 = −�̂̇�×(�̂�×𝜔)×
, therefore
𝜕 cos𝛿
𝜕𝜔=[ℛ𝑞�̂�]
𝑇
sin 𝛾
�̂̇�× �̂̇�×�̂�×
|�̇̂�|. (A-49)
Q.E.D. ∎
112
APPENDIX B MISCELLANEOUS PROOFS AND LEMMAS
Proof 1: �̇�𝑒 = �̇� + 𝜔×ℛ𝑒𝜔𝑓 − 𝑅𝑒�̇�𝑓 (B-1)
Recall Eq. 3-4, which defines the angular velocity error to be
𝜔𝑒 = 𝜔 − 𝑅𝑒𝜔𝑓. (3-4)
Taking the TD1 (applying product rule to the second term) of Eq. 3-4 yields
�̇�𝑒 = �̇� − �̇�𝑒𝜔𝑓 − 𝑅𝑒�̇�𝑓. (B-2)
Recall from Eq. 2-6 that
�̇�𝐴𝐵 = − 𝜔𝐴
𝐵 × 𝑅𝐴𝐵 . (2-6)
Applying Eq. 2-6 to Eq. B-2 yields
�̇�𝑒 = �̇� + 𝜔𝑒×ℛ𝑒𝜔𝑓 − 𝑅𝑒�̇�𝑓 . (B-3)
Substituting Eq. 3-4 into Eq. B-3 rids us of the term containing 𝜔𝑒:
�̇�𝑒 = �̇� + (𝜔 − ℛ𝑒𝜔𝑓)×
ℛ𝑒𝜔𝑓 − 𝑅𝑒�̇�𝑓 . (B-4)
Note that the skew operator is the linear algebra equivalent of the cross product,
which is a distributive operation, therefore
�̇�𝑒 = �̇� + 𝜔×ℛ𝑒𝜔𝑓 − (ℛ𝑒𝜔𝑓)
×
ℛ𝑒𝜔𝑓 − 𝑅𝑒�̇�𝑓. (B-5)
Since the cross product of any vector with itself is zero, the third term in Eq. B-5
is eliminated. Therefore
�̇�𝑒 = �̇� + 𝜔×ℛ𝑒𝜔𝑓 − 𝑅𝑒�̇�𝑓 . (B-1)
Q.E.D. ∎
113
Proof 2: [𝑀 𝑢
𝑣𝑇 𝑐]
−1
=1
𝑘[𝑘𝑀−1 +𝑀−1𝑢𝑣𝑇𝑀−1 −𝑀−1𝑢
−𝑣𝑇𝑀−1 1] ; 𝑘 ≝ 𝑐 − 𝑣𝑇𝑀−1𝑢 (B-6)
This can be shown by Gaussian elimination. First, the problem must be
constructed as an augmented matrix:
[𝑀 𝑢
𝑣𝑇 𝑐|𝕀 0
0𝑇 1]. (B-7)
Assuming 𝑀 is invertible, we can multiply the first row of Eq. B-7 by 𝑀−1, yielding
[𝕀 𝑀−1𝑢
𝑣𝑇 𝑐|𝑀−1 0
0𝑇 1]. (B-8)
Multiplying the first row of Eq. B-8 by 𝑣𝑇 and subtracting it from the second yields
[𝕀 𝑀−1𝑢
0𝑇 𝑐 − 𝑣𝑇𝑀−1𝑢|
𝑀−1 0
−𝑣𝑇𝑀−1 1]. (B-9)
Defining 𝑘 ≝ 𝑐 − 𝑣𝑇𝑀−1𝑢, we can eliminate it by dividing the second row by 𝑘:
[𝕀 𝑀−1𝑢
0𝑇 1|
𝑀−1 0
−𝑘−1𝑣𝑇𝑀−1 𝑘−1]. (B-10)
Finally, multiplying the second row by 𝑀−1𝑢 and subtracting it from the first
eliminates 𝑀−1𝑢 from Eq. B-10 in the same way 𝑣𝑇 was eliminated from Eq. B-8:
[𝕀 0
0𝑇 1|𝑀−1 + 𝑘−1𝑀−1𝑢𝑣𝑇𝑀−1 −𝑘−1𝑀−1𝑢
−𝑘−1𝑣𝑇𝑀−1 𝑘−1]. (B-11)
The right-hand side of Eq. B-11 yields the inverse of the block matrix:
[𝑀 𝑢
𝑣𝑇 𝑐]
−1
=1
𝑘[𝑘𝑀−1 +𝑀−1𝑢𝑣𝑇𝑀−1 −𝑀−1𝑢
−𝑣𝑇𝑀−1 1] ; 𝑘 ≝ 𝑐 − 𝑣𝑇𝑀−1𝑢. (B-6)
Q.E.D. ∎
114
Proof 3: 𝑄1−1 = 𝑄1
𝑇; 𝑄2−1 = 𝑄2
𝑇 (B-12)
Recall from Eq. B-6 that
[𝑀 𝑢
𝑣𝑇 𝑐]
−1
=1
𝑘[𝑘𝑀−1 +𝑀−1𝑢𝑣𝑇𝑀−1 −𝑀−1𝑢
−𝑣𝑇𝑀−1 1]where 𝑘 ≝ 𝑐 − 𝑣𝑇𝑀−1𝑢. (B-6)
Because the steps to invert 𝑄1 are similar to those for inverting 𝑄2, this proof will
only be shown for the latter case. Thus, recall from Eq. 26b that:
𝑄2 (𝑞) ≝ [휀× − 𝜂𝕀 휀
휀𝑇 𝜂]. (26b)
Analogous terms between Eq. 26b and Eq. B-6 are 𝑀 = 휀× − 𝜂𝕀, 𝑢 = 𝑣 = 휀, and 𝑐 = 𝜂.
The matrix 𝑀 was inverted by MatLab®. Its inverse matrix 𝑀−1 was found to be
𝑀−1 = −1
𝜂(𝜂2𝕀 + 𝜂휀× + 휀휀𝑇). (B-7)
The corresponding value of 𝑘 is
𝑘 = 𝜂 +1
𝜂휀𝑇 (𝜂2𝕀 + 𝜂휀× + 휀휀𝑇) 휀. (B-8)
Equation Eq. B-8 can be simplified, ironically, by multiplying out the second term.
𝑘 = 𝜂 +1
𝜂(𝜂2휀𝑇휀 + 𝜂휀𝑇휀×휀 + 휀𝑇휀휀𝑇휀). (B-9)
The term 휀𝑇휀×휀 is instantly eliminated by definition. Furthermore, if we recognize that
any quaternion in this context is, by definition, of unit length, then after consolidating all
terms into a single fraction, we find that the numerator of Eq. B-9 evaluates to 1:
𝑘 = 𝜂 +1
𝜂(𝜂2휀𝑇휀 + 휀휀𝑇휀) =
𝜂2 + (𝜂2 + 휀𝑇휀)휀𝑇휀
𝜂=1
𝜂. (B-10)
115
After performing extensive (though not particularly insightful) algebra upon the
terms in the right-hand side of Eq. B-6, the (1,1) element of 𝑄2−1 evaluates to
𝑘𝑀−1 +𝑀−1𝑢𝑣𝑇𝑀−1 =−1
𝜂(휀× + 𝜂𝕀). (B-11)
Similarly, the (1,2) element evaluates to
−𝑀−1𝑢 =휀
𝜂. (B-12)
Finally, the (2,1) element evaluates to
−𝑣𝑇𝑀−1 =휀𝑇
𝜂. (B-13)
Consolidating Eqs. B-11 through B-13 into one equation, we find that
𝑄2−1 = [
−휀× − 𝜂𝕀 휀
휀𝑇 𝜂] = [
휀× − 𝜂𝕀 휀
휀𝑇 𝜂]
𝑇
= 𝑄2𝑇 . (B-14)
Performing the same steps on 𝑄1 yields
𝑄1−1 = [
휀𝑓× + 𝜂𝑓𝕀 휀𝑓
−휀𝑓𝑇 𝜂𝑓
] = [−휀𝑓
× + 𝜂𝑓𝕀 −휀𝑓
휀𝑓𝑇 𝜂𝑓
]
𝑇
= 𝑄1𝑇 . (B-15)
Q.E.D. ∎
116
Proof 4: Φ̇(𝜑1,𝑟) = [Ξ
𝑇 (𝑞𝑒)𝑄1 (�̇�𝑓) + Ξ𝑇 (�̇�𝑒)𝑄1 (𝑞𝑓)]𝑔1,𝑟 (𝑞)
+ Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)𝐻1,𝑟 (𝑞) �̇� (B-16)
Setting 𝜑 = 𝜑1,𝑟 , Eq. 3-32 evaluates to
Φ(𝜑1,𝑟) = Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)𝑔1,𝑟 (𝑞). (B-17)
Direct differentiation of Eq. B-17 by way of product rule yields
Φ̇(𝜑1,𝑟) =𝑑
𝑑𝑡[Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)] 𝑔1,𝑟 (𝑞) + Ξ
𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓) �̇�1,𝑟 (𝑞). (B-18)
Applying chain rule to �̇�1,𝑟 (𝑞) shows that the second term on the right-hand side
of Eq. B-18 is dependent on the Hessian matrix defined in Eq. 3-39:
�̇�1,𝑟 (𝑞) =𝜕
𝜕𝑞𝑔1,𝑟 (𝑞)
𝑑𝑞
𝑑𝑡= 𝐻1,𝑟 (𝑞) �̇�. (B-19)
Carrying out differentiation by product rule on the first term, it follows that
𝑑
𝑑𝑡[Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)] = Ξ
𝑇 (𝑞𝑒) �̇�1 (𝑞𝑓) + Ξ̇𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓). (B-20)
Since both the quaternion rate and quaternion error matrices are linear functions
of orientation, the TD1 of these matrices distributes directly to their arguments.
Therefore Eq. B-20 can be rewritten as
𝑑
𝑑𝑡[Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)] = Ξ
𝑇 (𝑞𝑒)𝑄1 (�̇�𝑓) + Ξ𝑇 (�̇�𝑒)𝑄1 (𝑞𝑓). (B-21)
Substituting Eqs. B-19 and B-21 into Eq. B-18 yields
Φ̇(𝜑1,𝑟) = [Ξ𝑇 (𝑞𝑒)𝑄1 (�̇�𝑓) + Ξ
𝑇 (�̇�𝑒)𝑄1 (𝑞𝑓)]𝑔1,𝑟 (𝑞) + Ξ𝑇 (𝑞𝑒)𝑄1 (𝑞𝑓)𝐻1,𝑟 (𝑞) �̇�. (B-16)
Q.E.D. ∎
117
Proof 5: Φ̇(𝜑1,𝑎) =1
2(𝕀 − 2휀𝑒휀𝑒
𝑇)𝜔𝑒 − Ξ𝑇 (�̇�𝑒) 𝑞𝑒
−1 (B-22)
Setting 𝜑 = 𝜑1,𝑎, Eq. 3-32 evaluates to
Φ(𝜑1,𝑎) = −Ξ𝑇 (𝑞𝑒) 𝑞𝑒
−1. (B-23)
Direct differentiation of Eq. B-23 by way of product rule yields
Φ̇(𝜑1,𝑎) = −Ξ̇𝑇 (𝑞𝑒) 𝑞𝑒−1 − Ξ𝑇 (𝑞𝑒) �̇�𝑒
−1. (B-24)
Recall from Eq. 2-8a that
𝑞−1 = [�̂� sin(−𝜃 2⁄ )
cos(−𝜃 2⁄ )] = [
�̂� sin(𝜃 2⁄ )
− cos(𝜃 2⁄ )] ≡ [
휀−𝜂]. (2-8a)
The TD1 of Eq. 9a (by way of the convention established in Eq. 2-16) is
�̇�−1 = [휀̇
−�̇�] = [
−𝕀 0
0𝑇 1] [휀̇
�̇�] = [
−𝕀 0
0𝑇 1] �̇� =
1
2[−𝕀 0
0𝑇 1]Ξ (𝑞)𝜔. (B-25)
Substitution of Eq. B-25 (evaluated at 𝑞 = 𝑞𝑒) into Eq. B-24 yields
Φ̇(𝜑1,𝑎) = −Ξ̇𝑇 (𝑞𝑒)𝑞𝑒
−1 −1
2Ξ𝑇 (𝑞𝑒) [
−𝕀 0
0𝑇 1]Ξ (𝑞𝑒)𝜔𝑒. (B-26)
Using the definition of Ξ supplied by Eq. 2-13, it follows that
Ξ𝑇 (𝑞𝑒) [−𝕀 0
0𝑇 1] Ξ (𝑞𝑒) = [−휀𝑒
× + 𝜂𝑒𝕀 −휀𝑒] [−𝕀 0
0𝑇 1] [휀𝑒× + 𝜂𝑒𝕀
−휀𝑒𝑇
]. (B-27)
Direct multiplication of Eq. B-27 yields
Ξ𝑇 (𝑞𝑒) [−𝕀 0
0𝑇 1]Ξ (𝑞𝑒) = 휀𝑒
×휀𝑒× − 𝜂𝑒
2𝕀 + 휀𝑒휀𝑒𝑇 . (B-28)
It follows from Eq. 2-5 that 휀𝑒×휀𝑒
× = 휀𝑒휀𝑒𝑇 − 휀𝑒
𝑇휀𝑒𝕀. Therefore Eq. B-28 becomes
Ξ𝑇 (𝑞𝑒) [−𝕀 0
0𝑇 1] Ξ (𝑞𝑒) = 2휀𝑒휀𝑒
𝑇 − (휀𝑒𝑇휀𝑒 + 𝜂𝑒
2)𝕀. (B-29)
118
Since 𝑞𝑒 is of unit length, Eq. B-29 becomes
Ξ𝑇 (𝑞𝑒) [−𝕀 0
0𝑇 1]Ξ (𝑞𝑒) = 2휀𝑒휀𝑒
𝑇 − 𝕀. (B-30)
Substituting Eq. B-30 into Eq. B-26 yields
Φ̇(𝜑1,𝑎) =1
2(𝕀 − 2휀𝑒휀𝑒
𝑇)𝜔𝑒 − Ξ̇𝑇 (𝑞𝑒) 𝑞𝑒
−1. (B-31)
Finally, recalling that the quaternion rate matrix is a linear function of orientation,
its derivative in the second term is distributed to its argument. Therefore
Φ̇(𝜑1,𝑎) =1
2(𝕀 − 2휀𝑒휀𝑒
𝑇)𝜔𝑒 − Ξ𝑇 (�̇�𝑒) 𝑞𝑒
−1. (B-22)
Q.E.D. ∎
119
Proof 6: 𝑥2 = −(cos2𝛾 + cos2𝛽
1 + cos2𝛽) 𝑦2 + 2(
sin 2𝛾
1 + cos 2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽
1 + cos2𝛽) (B-32)
Define 𝑟 to be a vector that points from the origin to a point on the conic
intersection. The keep-out cone is therefore defined by the equation
(𝑟𝑇�̂�1)2= 𝑟𝑇𝑟 cos2 𝛽. (B-33)
Using the basis vectors defined in Eq. 4-20, 𝑟 may be parameterized in 𝒮:
𝑟 = 𝑥�̂�1 + 𝑦�̂�2 + �̂�3, (B-34)
where 𝑥 and 𝑦 are the distances of the end or 𝑟 from �̂�3 as measured along �̂�1 and �̂�2
respectively. Since �̂�1, �̂�2, and �̂�3 are mutually orthogonal, the magnitude of Eq. B-34 is
𝑟𝑇𝑟 = 𝑥2 + 𝑦2 + 1. (B-35)
Furthermore, since 𝑟 lays on the surface of the keep-out cone, it is subject to the
constraint in Eq. B-33, from which it follows that
𝑟𝑇�̂�1 = 𝑥�̂�1𝑇�̂�1 + 𝑦�̂�2
𝑇�̂�1 + �̂�3𝑇�̂�1. (B-36)
It can be seen from the geometry in Figure 4-1 that
�̂�1𝑇�̂�1 = 0, (B-37a)
�̂�2𝑇�̂�1 = cos (
𝜋
2− 𝛾) = sin 𝛾, (B-37b)
�̂�3𝑇�̂�1 = cos 𝛾, (B-37c)
Substituting Eqs. B-37a-c into Eq. B-36 yields
𝑟𝑇�̂�1 = 𝑦 sin 𝛾 + cos 𝛾. (B-38)
Further substituting Eqs. B-35 and B-38 into Eq. B-33 yields
(𝑦 sin 𝛾 + cos 𝛾)2 = (𝑥2 + 𝑦2 + 1) cos2 𝛽. (B-39)
120
The factors in Eq. B-39 can be expanded into
𝑦2 sin2 𝛾 + 2𝑦 sin 𝛾 cos 𝛾 + cos2 𝛾 = 𝑥2 cos2 𝛽 + 𝑦2 cos2 𝛽 + cos2 𝛽. (B-40)
Terms in Eq. B-40 containing 𝑥 are then isolated from those containing 𝑦,
yielding
𝑥2 cos2 𝛽 = 𝑦2 sin2 𝛾 − 𝑦2 cos2 𝛽 + 2𝑦 sin 𝛾 cos 𝛾 + cos2 𝛾 − cos2 𝛽. (B-41)
Equation B-41 can be thus be grouped into
𝑥2 cos2 𝛽 = (sin2 𝛾 − cos2 𝛽)𝑦2 + 2 sin 𝛾 cos 𝛾 𝑦 + (cos2 𝛾 − cos2 𝛽). (B-42)
Then dividing Eq. B-42 by cos2 𝛽 yields
𝑥2 = (sin2 𝛾 − cos2 𝛽
cos2 𝛽)𝑦2 + 2(
sin 𝛾 cos 𝛾
cos2 𝛽)𝑦 + (
cos2 𝛾 − cos2 𝛽
cos2 𝛽). (B-43)
The squared trigonometric terms in Eq. B-43 can be converted into double-angle
functions, which results in fewer function calls when entered into MatLab®. Thus the
conic section equation can be written as
𝑥2 = −(cos2𝛾 + cos2𝛽
1 + cos2𝛽) 𝑦2 + 2(
sin 2𝛾
1 + cos 2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽
1 + cos2𝛽). (B-32)
Q.E.D. ∎
121
Proof 7: 𝑥t =1
sin 2𝛾√(cos 2𝛽 − cos2𝛾)(1 − cos2𝛽); 𝑦𝑡 =
cos 2𝛽 − cos2𝛾
sin 2𝛾 (B-44)
It follows from geometry that
tan 𝛼 = |𝑥
𝑦|. (B-45)
Recall that 𝛼 will have its maximum value when 𝑥 = 𝑥𝑡 and 𝑦 = 𝑦𝑡, which can be
calculated by evaluating the zero tangents of Eq. B-45:
𝑑 tan𝛼
𝑑𝑦=
𝑑𝑥𝑑𝑦 𝑦 − 𝑥
𝑦2= 0. (B-46)
Differentiating Eq. B-32 yields
2𝑥𝑑𝑥
𝑑𝑦= −2(
cos2𝛾 + cos 2𝛽
1 + cos 2𝛽)𝑦 + 2 (
sin 2𝛾
1 + cos2𝛽). (B-47)
Solving Eq. B-47 for 𝑑𝑥
𝑑𝑦𝑦 yields
𝑑𝑥
𝑑𝑦𝑦 =
−(cos2𝛾 + cos 2𝛽1 + cos 2𝛽 )𝑦2 + (
sin 2𝛾1 + cos2𝛽) 𝑦
√−(cos2𝛾 + cos2𝛽1 + cos2𝛽 ) 𝑦2 + 2(
sin 2𝛾1 + cos 2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽1 + cos2𝛽 )
. (B-48)
Returning to Eq. B-32 and dividing it by 𝑥 yields
𝑥 =−(
cos2𝛾 + cos2𝛽1 + cos2𝛽 ) 𝑦2 + 2(
sin 2𝛾1 + cos 2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽1 + cos2𝛽 )
√−(cos2𝛾 + cos2𝛽1 + cos2𝛽 )𝑦2 + 2(
sin 2𝛾1 + cos2𝛽) 𝑦 + (
cos2𝛾 − cos 2𝛽1 + cos 2𝛽 )
. (B-49)
Substituting Eqs. B-48 and B-49 into Eq. B-46 yields
𝑑 tan𝛼
𝑑𝑦=
−(sin 2𝛾
1 + cos2𝛽)𝑦 − (cos2𝛾 − cos2𝛽1 + cos2𝛽 )
𝑦2√−(cos2𝛾 + cos2𝛽1 + cos2𝛽 ) 𝑦2 + 2(
sin 2𝛾1 + cos2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽1 + cos2𝛽 )
. (B-50)
122
Equation B-50 has its maximum value when
−(sin 2𝛾
1 + cos2𝛽) 𝑦 − (
cos 2𝛾 − cos 2𝛽
1 + cos 2𝛽) = 0. (B-51)
Solving Eq. B-51 for 𝑦 yields
𝑦 → 𝑦𝑡 =cos 2𝛽 − cos2𝛾
sin 2𝛾. (B-44a)
Substituting Eq. B-44a into Eq. B-32 and evaluating the first term yields
−(cos2𝛾 + cos2𝛽
1 + cos2𝛽) 𝑦𝑡
2 = −(cos2𝛾 + cos2𝛽
1 + cos2𝛽) (cos2𝛽 − cos2𝛾
sin 2𝛾)2
. (B-52)
Direct multiplication of the right-hand side of Eq. B-52 yields
−(cos 2𝛾 + cos 2𝛽
1 + cos 2𝛽)𝑦𝑡
2 = −(cos 2𝛾 + cos 2𝛽
1 + cos 2𝛽) ((cos 2𝛽 − cos 2𝛾)(cos 2𝛽 − cos 2𝛾)
sin2 2𝛾). (B-53)
Multiplying (cos2𝛽 + cos 2𝛾) with (cos 2𝛽 − cos2𝛾), Eq. B-53 can be rewritten as
−(cos2𝛾 + cos2𝛽
1 + cos2𝛽) 𝑦𝑡
2 − (cos2 2𝛽 − cos2 2𝛾
sin2 2𝛾) (cos2𝛽 − cos2𝛾
1 + cos2𝛽). (B-54)
Similarly, the second term of Eq. B-32 can be rewritten as
2 (sin 2𝛾
1 + cos2𝛽) 𝑦𝑡 = 2(
sin 2𝛾
1 + cos 2𝛽)(cos2𝛽 − cos 2𝛾
sin 2𝛾) = 2 (
cos2𝛽 − cos2𝛾
1 + cos2𝛽). (B-55)
Substituting Eqs. B-53 and B-55 into Eq. B-32 yields
𝑥𝑡2 =
1
sin2 2𝛾(cos 2𝛽 − cos2𝛾)(1 − cos2𝛽). (B-56)
Taking the square root of Eq. B-56 yields
𝑥𝑡2 =
1
sin 2𝛾√(cos2𝛽 − cos2𝛾)(1 − cos 2𝛽). (B-44b)
Q.E.D. ∎
123
Proof 8: ℓ = 𝑚(sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
; 𝑛 = {0 if 𝛾 ≤
𝜋
2
1 if 𝛾 >𝜋
2
; 𝑚 = {0 if 𝛾 ≥ 𝛾𝑠1 if 𝛾 < 𝛾𝑠
(B-57)
Recall from Eq. B-32 that
𝑥2 = −(cos2𝛾 + cos2𝛽
1 + cos2𝛽) 𝑦2 + 2(
sin 2𝛾
1 + cos 2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽
1 + cos2𝛽). (B-32)
Factoring out the leading coefficient of the first term makes Eq. B-32 monic:
𝑥2 = −cos 2𝛾 + cos2𝛽
1 + cos2𝛽[𝑦2 + 2(
sin 2𝛾
cos 2𝛾 + cos 2𝛽)𝑦 + (
cos2𝛾 − cos2𝛽
cos2𝛾 + cos2𝛽)]. (B-58)
By completing the square, Eq. B-58 can be alternatively expressed as
𝑥2 = −cos 2𝛾 + cos2𝛽
1 + cos2𝛽[(𝑦 −
sin 2𝛾
cos2𝛾 + cos 2𝛽)2
− (sin 2𝛾
cos2𝛾 + cos2𝛽)2
]. (B-59)
Equation B-59 implies that the conic intersection is symmetric about the line
𝑦 → 𝑦𝑐 =sin 2𝛾
cos 2𝛾 + cos 2𝛽. (B-60)
Defining ∆𝑦 ≝ 𝑦𝑡 − 𝑦𝑐 where 𝑦𝑡 is provided by Eq. B-44a, it follows that
Δ𝑦 =− sin2 2𝛽
sin 2𝛾 (cos 2𝛽 + cos2𝛾). (B-61)
Note that when 𝛾 ≤𝜋
2, we would like to take the value of Eq. B-61 as-is – i.e.
𝑦𝑡 = 𝑦𝑐 + ∆𝑦. However, when 𝛾 >𝜋
2, we must project the point from the lower branch of
the conic intersection to the upper branch. In doing so, we must instead reverse the sign
of Eq. B-61 – i.e. 𝑦𝑡 = 𝑦𝑐 − ∆𝑦 = 𝑦𝑐 + |∆𝑦|. In general, the term which reverses the sign
of ∆𝑦 based on the domain constraint is the sin 2𝛾 in the denominator. Therefore we let
Δ𝑦 → Δ�̃� =− sin2 2𝛽
|sin 2𝛾|(cos2𝛽 + cos2𝛾). (B-62)
124
It follows from Eqs. B-60 and B-62 that
𝑦𝑡 = 𝑦𝑐 + Δ�̃� =sin 2𝛾 |sin 2𝛾| − sin2 2𝛽
|sin 2𝛾|(cos2𝛽 + cos2𝛾). (B-63)
Note that when 𝛾 ≤𝜋
2, |sin 2𝛾| → + sin 2𝛾, yielding
𝑦𝑡 =sin2 2𝛽 + sin2 2𝛾
sin 2𝛾 (cos2𝛽 + cos 2𝛾). (B-64)
Substituting Eqs. B-44b and B-64 into Eq. B-45 yields
tan 𝛼 =𝑦𝑡𝑥𝑡= (
sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)√
1 − cos2𝛽
cos2𝛽 − cos2𝛾. (B-65)
Note that without the leading coefficient, Eq. B-65 reduces to Eq. 4-23. Thus
tan 𝛼 = (sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
√1 − cos2𝛽
cos2𝛽 − cos 2𝛾; 𝑛 = {
0 if 𝛾 ≤𝜋
2
1 if 𝛾 >𝜋
2
. (B-66)
We define the leading coefficient in Eq. B-66 to be ℓ, such that
ℓ = (sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
; 𝑛 = {0 if 𝛾 ≤
𝜋
2
1 if 𝛾 >𝜋
2
. (B-67)
Furthermore, recall that there exists a safety angle defined in Eq. 4-25, inside of
which there is no applicable value for the tangent pathway vector. Therefore, we
introduce another constant to the compensating term in Eq. B-66, which yields
ℓ = 𝑚(sin2 2𝛽 − sin2 2𝛾
sin2 2𝛽 + sin2 2𝛾)
𝑛
; 𝑛 = {0 if 𝛾 ≤
𝜋
2
1 if 𝛾 >𝜋
2
; 𝑚 = {0 if 𝛾 ≥ 𝛾𝑠1 if 𝛾 < 𝛾𝑠
. (B-57)
Q.E.D. ∎
125
APPENDIX C MATLAB® SIMULATION CODE
%------------------------------------------------------------------------- % Main Function function APFSim() % initialize the dynamics state x = [q0; w0; qf; wf; qe; we] q0 = Normalized([-0.0507; 0.4569; 0.5549; 0.6934]); w0 = [0; 0; 0]; qf = Normalized([0.6533; 0.2706; -0.2706; 0.6533]); wf = [0; 0; 0]; % specify the rate of the angular velocity of the tracking frame % as a function of time (free choice) wfDot = @(t) [0; 0; 0]; % specify gains and MOI matrix K1 = 0.10 * eye(3); K2 = 0.60 * eye(3); J = diag([5 3 4]); % define body-fixed sensor vectors (si), important attitude axes (di) % with their respective cone angles (betai), and an integer to % determine if the region is attitude forbidden or mandatory. s1 = Normalized([0;-0.5;1]); s2 = Normalized([0;+0.5;1]); s3 = Normalized([+1.0;0;0]); sList = [s1 s2 s3]; d1 = Normalized([1;1;1]); beta1 = deg2rad(20); d2 = Normalized([0;-1;0]); beta2 = deg2rad(20); d3 = Normalized([2;2;-3]); beta3 = deg2rad(40); dList = [d1 d2 d3; beta1 beta2 beta3]; % rows correspond to sensors; columns correspond to cones % +1 denotes attitude forbidden % -1 denotes attitude mandatory % 0 denotes no dependancy sdMap = [+1 +1 0 ; +1 +1 0 ; 0 0 -1]; % specify time interval and step for simulation tf = 15; dt = 0.1; times = 0:dt:tf; % calculate the number of time steps in the simulation nTSteps = ceil(tf/dt); % initialize logs logs = struct('q0', zeros(4,nTSteps),... 'w0', zeros(3,nTSteps),... 'qf', zeros(4,nTSteps),... 'wf', zeros(3,nTSteps),... 'qe', zeros(4,nTSteps),... 'we', zeros(3,nTSteps),... 'tau', zeros(3,nTSteps)); % initialize percent counter percentComplete = 0; fprintf('Running APF Simulation:\nPercent Complete = 0.00%%');
126
% run the simulation for t = times; % update progress counter nextPercent = 0.01*floor(t/tf*100/0.01); if percentComplete < nextPercent percentComplete = nextPercent; fprintf([repmat('\b',1,6),'%5.2f%%'], percentComplete) if percentComplete == 100 fprintf('\nDone!\n') end end % determine the required dynamics and control from inner loop [q0,w0,qf,wf,qe,we,tau] = ControllerMain(t,dt,q0,w0,qf,wf,wfDot,... K1,K2,J,sList,dList,sdMap); % update logs i = nearest(t/dt + 1); logs.q0(:,i) = q0; logs.w0(:,i) = w0; logs.qf(:,i) = qf; logs.wf(:,i) = wf; logs.qe(:,i) = qe; logs.we(:,i) = we; logs.tau(:,i) = tau; end % choose to graph or animate the data from the logs. GraphStateLogs(times,logs) PlotSensorPaths(times,logs,sList,dList) end %------------------------------------------------------------------------- % Processing Tools function GraphStateLogs(times,logs) [q0Row1,q0Row2,q0Row3,q0Row4] = SeparateLogRows(logs.q0); [w0Row1,w0Row2,w0Row3] = SeparateLogRows(logs.w0); [qfRow1,qfRow2,qfRow3,qfRow4] = SeparateLogRows(logs.qf); [wfRow1,wfRow2,wfRow3] = SeparateLogRows(logs.wf); [qeRow1,qeRow2,qeRow3,qeRow4] = SeparateLogRows(logs.qe); [weRow1,weRow2,weRow3] = SeparateLogRows(logs.we); figure(1) set(gcf,'units','normalized','outerposition',[0 0 1 1]) subplot(2,3,1) hold on plot(times,q0Row1,times,q0Row2,... times,q0Row3,times,q0Row4,... 'linewidth',2) plot(times,qfRow1,'--',times,qfRow2,'--',... times,qfRow3,'--',times,qfRow4,'--',... 'linewidth',2) hold off grid on title('Evolution of Body and Tracking Orientations','fontsize',16) xlabel('Time - t [s]','fontsize',16) ylabel('Quaternion Elements - q_i [-]','fontsize',16) legend('\epsilon_0_,_1','\epsilon_0_,_2','\epsilon_0_,_3','\eta_0',... '\epsilon_f_,_1','\epsilon_f_,_2','\epsilon_f_,_3','\eta_f',... 'location', 'east') set(gca,'FontSize',16)
127
subplot(2,3,2) plot(times,qeRow1,times,qeRow2,... times,qeRow3,times,qeRow4,... 'linewidth',2) grid on title('Evolution of Orientation Error','fontsize',16) xlabel('Time - t [s]','fontsize',16) ylabel('Quaternion Elements - q_i [-]','fontsize',16) legend('\epsilon_e_,_1','\epsilon_e_,_2','\epsilon_e_,_3','\eta_e',... 'location', 'east') set(gca,'FontSize',16) subplot(2,3,3) angleError = 2*acos(min(abs(qeRow4),1)); semilogy(times,angleError,'linewidth',2) grid on title('Evolution of Rotation Angle Error','fontsize',16) xlabel('Time - t [s]','fontsize',16) ylabel('Angle Error - \theta [rad]','fontsize',16) set(gca,'FontSize',16) subplot(2,3,4) hold on plot(times,w0Row1,times,w0Row2,times,w0Row3,... 'linewidth',2) plot(times,wfRow1,'--',times,wfRow2,'--',times,wfRow3,'--',... 'linewidth',2) hold off grid on title('Evolution of Body and Tracking Slew Rates','fontsize',16) xlabel('Time - t [s]','fontsize',16) ylabel('Angular Velocity Elements - \omega_i [rad/s]','fontsize',16) legend('\omega_0_,_1','\omega_0_,_2','\omega_0_,_3',... '\omega_f_,_1','\omega_f_,_2','\omega_f_,_3',... 'location', 'east') set(gca,'FontSize',16) subplot(2,3,5) plot(times,weRow1,times,weRow2,times,weRow3,'linewidth',2) grid on title('Evolution of Angular Velocity Error','fontsize',16) xlabel('Time - t [s]','fontsize',16) ylabel('Angular Velocity Elements - \omega_i [rad/s]','fontsize',16) legend('\omega_e_,_1','\omega_e_,_2','\omega_e_,_3',... 'location', 'east') set(gca,'FontSize',16) subplot(2,3,6) angVelNorm = sqrt(w0Row1.^2 + w0Row2.^2 + w0Row3.^2); semilogy(times,angVelNorm,'linewidth',2) grid on title('Evolution of Angular Velocity Magnitude','fontsize',16) xlabel('Time - t [s]','fontsize',16) ylabel('Angular Velocity - \omega [rad/s]','fontsize',16) set(gca,'FontSize',16) end function varargout = SeparateLogRows(log) nRows = size(log,1); varargout = cell(1,nRows); for row = 1:nRows varargout{row} = log(row,:); end end
128
function PlotSensorPaths(times,logs,sList,dList) nSensors = numel(sList)/3; sPaths = cell(1,nSensors); sGoals = cell(1,nSensors); sStrgs = cell(1,4*nSensors+1); for i = 1:nSensors sPaths{i} = zeros(2,length(times)); sGoals{i} = zeros(2,length(times)); sStrgs{4*i-3} = ['sensor ', num2str(i), ' path']; sStrgs{4*i-2} = ['sensor ', num2str(i), ' target']; sStrgs{4*i-1} = ['sensor ', num2str(i), ' origin']; sStrgs{4*i} = ['sensor ', num2str(i), ' terminus']; end sStrgs{end} = 'constraint boundary'; for i = 1:length(times) q0 = logs.q0(:,i); R0 = QuaternionToDCM(q0); % plot the sensors for j = 1:nSensors s = R0' * sList(:,j); sProj = EquirectangularProjection(s); sPaths{j}(:,i) = sProj; end % Plot the sensor destination path in the same way as with q0. qf = logs.qf(:,i); Rf = QuaternionToDCM(qf); for j = 1:nSensors s = Rf' * sList(:,j); sProj = EquirectangularProjection(s); sGoals{j}(:,i) = sProj; end end figure(2) set(gcf,'units','normalized','outerposition',[0 0 1 1]) title('Projection of Paths Traced by Body-Fixed Sensor Vectors',... 'fontsize',14) xlabel('Longitude [degrees]','fontsize',14) ylabel('Latitude [degrees]','fontsize',14) set(gca,'xTickMode','manual','xTick',-180:30:180,... 'yTickMode','manual','yTick', -90:30:90 ,... 'fontsize',14) axis([-180 180 -90 90]) grid on hold on for i = 1:nSensors % cycle through colors based on the number of sensors if mod(i,6) == 0 colorStr = 'y'; elseif mod(i,5) == 0 colorStr = 'm'; elseif mod(i,4) == 0 colorStr = 'c'; elseif mod(i,3) == 0 colorStr = 'r'; elseif mod(i,2) == 0 colorStr = 'g'; else colorStr = 'b'; end
129
plot(sPaths{i}(1,:),sPaths{i}(2,:),['-', colorStr],... 'linewidth',2) plot(sGoals{i}(1,:),sGoals{i}(2,:),['--',colorStr],... 'linewidth',2) plot(sPaths{i}(1,1),sPaths{i}(2,1),['o', colorStr],... 'Markersize',10,'linewidth',2) plot(sPaths{i}(1,end),sPaths{i}(2,end),['+',colorStr],... 'Markersize',10,'linewidth',2) end nCones = numel(dList)/4; for i = 1:nCones % plot any cones from the APF section coneProj = ConeProjection(dList(1:3,i),rad2deg(dList(4,i))); plot(coneProj(1,:),coneProj(2,:),'k','linewidth',2) end legend(sStrgs, 'location', 'northeast') % plot the axes plot([-180 180],[0 0],'k') plot([0 0],[-90 90],'k') hold off end function coneProj = ConeProjection(d,beta) % specify the cone-fixed z axis to be along the cone axis d3 = d / norm(d); % pick a random direction normal to d3 for the cone-fixed x axis d1 = cross(d3,rand(3,1)); d1 = d1 / norm(d1); % complete the basis by cross product d2 = cross(d3,d1); d2 = d2 / norm(d2); % specify a ring of points around the cone in the cone-fixed basis coneProj = zeros(2,360); for theta = 1:360 conePoint = [d1 d2 d3]*[cosd(theta); sind(theta); cotd(beta)]; coneProj(:,theta) = EquirectangularProjection(conePoint); end end function pProj = EquirectangularProjection(pCart) long = atan2d(pCart(2),pCart(1)); lat = atan2d(pCart(3),norm(pCart(1:2))); pProj = [long; lat]; end %-------------------------------------------------------------------------- % Main APF Controller function [q0,w0,qf,wf,qe,we,tau] = ControllerMain(t,dt,q0,w0,qf,wf,wfDot,... K1,K2,J,sList,dList,sdMap) % compute the error quantities qe = QuaternionErrorMatrix(qf) * q0; Re = QuaternionToDCM(qe); we = w0 - Re*wf; % construct the dynamics vector at time t x0 = [q0; w0; qf; wf; qe; we];
130
% construct the dynamics ODE xDot = @(t,x) DynamicsODE(t,x,wfDot,K1,K2,J,sList,dList,sdMap); % integrate over the next time step to propagate the dynamics x = ode45(xDot,[t,t+dt],x0); x = x.y(:,end); % extract the updated orientations and angular velocities q0 = x(1:4); w0 = x(5:7); qf = x(8:11); wf = x(12:14); qe = x(15:18); we = x(19:21); % compute the control state from the dynamics x0Dot = xDot(t,x0); w0Dot = x0Dot(5:7); tau = J*w0Dot + cross(w0,J*w0); end function xDot = DynamicsODE(t,x,wfDot,K1,K2,J,sList,dList,sdMap) q0 = x(1:4); w0 = x(5:7); qf = x(8:11); wf = x(12:14); % calculate error parameters qe = QuaternionErrorMatrix(qf) * q0; Re = QuaternionToDCM(qe); we = w0 - Re*wf; % update the quaternion rates q0Dot = QuaternionRateMatrix(q0)' * w0/2; qfDot = QuaternionRateMatrix(qf)' * wf/2; qeDot = QuaternionRateMatrix(qe)' * we/2; % calculate first step APF quantities a1Phi = 1 - qe(4)^2; [r1Phi,r1q0Gradient,r1q0Hessian] = FirstStepAPFs(q0,sList,dList,sdMap); % calculate attractive potential PRT and its rate a1PRT = -QuaternionRateMatrix(qe) * QuaternionInverse(qe); a1PRTDot = (eye(3) - 2*(qe(1:3)*qe(1:3)')) * we/2 ...
- (QuaternionRateMatrix(qeDot) * QuaternionInverse(qe)); % calculate repulsive potential PRT and its rate r1PRT = (QuaternionRateMatrix(qe) * QuaternionErrorMatrix(qf))... * r1q0Gradient; r1PRTDot = ((QuaternionRateMatrix(qe) * QuaternionErrorMatrix(qfDot))... + (QuaternionRateMatrix(qeDot) * QuaternionErrorMatrix(qf)))... * r1q0Gradient... + (QuaternionRateMatrix(qe) * QuaternionErrorMatrix(qf))... * (r1q0Hessian * q0Dot); % calculate total potential PRT and its rate t1PRT = (a1Phi * r1PRT) + (r1Phi*a1PRT); t1PRTDot = (a1PRT*r1PRT' + r1PRT*a1PRT') * we/2 ... + (a1Phi*r1PRTDot + r1Phi*a1PRTDot);
131
% with that, calculate the desired angular velocity error and its rate. wed = -K1 * t1PRT; wedDot = -K1 * t1PRTDot; % then some quantities necessary for the second step of calculation. weDiff = we - wed; feedFwd = cross(w0,Re*wf) - Re*wfDot(t) - wedDot; % and onto the second step APF quantities [r2Phi,r2q0Gradient,r2w0Gradient] = SecondStepAPFs(q0,w0); % calculating the control solution invA = (eye(3)-(weDiff*r2w0Gradient')/(2*r2Phi+weDiff'*r2w0Gradient))... / r2Phi; tau = cross(w0,J*w0) - J*invA*((weDiff * r2q0Gradient' * q0Dot / 2) ... + (r2Phi * feedFwd + K2 * weDiff)); % calculate the initial and final angular velocity rates w0Dot = J\(tau - cross(w0,J*w0)); wfDot = wfDot(t); weDot = w0Dot + cross(we,Re*wf) - Re*wfDot; % calculate the dynamics derivative xDot = zeros(14,1); xDot(1:4) = q0Dot; xDot(5:7) = w0Dot; xDot(8:11) = qfDot; xDot(12:14) = wfDot; xDot(15:18) = qeDot; xDot(19:21) = weDot; end %-------------------------------------------------------------------------- % First Step APFs function [phi,q0Gradient,q0Hessian] = FirstStepAPFs(q0,sList,dList,sdMap) phi = 1; q0Gradient = zeros(4,1); q0Hessian = zeros(4,4); for i = 1:numel(sList)/3 s = sList(:,i); for j = 1:numel(dList)/4 d = dList(1:3,j); beta = dList(4,j); % incorporate the APF for the ith sensor on the jth cone APFij_coeff = 0.35*sdMap(i,j); [APFij, APFij_q0Grad, APFij_q0Hess] = ConeAvoidanceAPF(q0,... s,d,beta); phi = phi + APFij_coeff * APFij; q0Gradient = q0Gradient + APFij_coeff * APFij_q0Grad; q0Hessian = q0Hessian + APFij_coeff * APFij_q0Hess; end end end function [phi,q0Gradient,q0Hessian] = ConeAvoidanceAPF(q0,s,d,beta) phi = 1/(cos(beta) - s'*QuaternionToDCM(q0)*d); q0Gradient = 2 * ConeAPFMatrix(s,d) * q0 * phi^2; q0Hessian = 2 * ConeAPFMatrix(s,d) * (eye(4) + 4 * (q0 * q0') * ... ConeAPFMatrix(s,d) * phi) * phi^2; end
132
%-------------------------------------------------------------------------- % Second Step APFs function [phi,q0Gradient,w0Gradient] = SecondStepAPFs(q0,w0) c1 = 0; % scaling constant for repulsive potential 1. [APF1, APF1q0Grad, APF1w0Grad] = SlewRateAPF(w0); % apply superposition to calculate the magnitude, gradient, and hessian. phi = 1 + c1*APF1; q0Gradient = c1*APF1q0Grad; w0Gradient = c1*APF1w0Grad; end function [phi,q0Gradient,w0Gradient] = SlewRateAPF(w0) wm = 0.0873; phi = 1/(wm^2 - w0'*w0); q0Gradient = [0; 0; 0; 0]; w0Gradient = 2 * w0 / (wm^2 - w0'*w0)^2; end %-------------------------------------------------------------------------- % Mathematical Definitions function qInv = QuaternionInverse(q) qInv = [-q(1:3); q(4)]; end function qDCM = QuaternionToDCM(q) eps = q(1:3); eta = q(4); qDCM = (eta^2 - eps'*eps)*eye(3) + 2*(eps*eps') - 2*eta*Skew(eps); end function qErrorMatrix = QuaternionErrorMatrix(q) qErrorMatrix = [QuaternionRateMatrix(q); q']; end function qRateMatrix = QuaternionRateMatrix(q) % really the *transpose* of the quaternion rate matrix % because the transpose appears more frequently eps = q(1:3); eta = q(4); qRateMatrix = [-Skew(eps)+eta*eye(3) -eps]; end function vSkew = Skew(v) vSkew = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; end function lambda = ConeAPFMatrix(a,b) lambda = [(a*b'+b*a'-a'*b*eye(3)) cross(a,b); cross(a,b)' a'*b]; end function vUnit = Normalized(v) if norm(v) == 0 vUnit = v; else vUnit = v / norm(v); end end
133
LIST OF REFERENCES
[1] U. Lee and M. Mesbahi, "Quaternion Based Optimal Spacecraft Reorientation Under Complex Attitude Constrained Zones," in AAS/AIAA Astrodynamics Specialist Conference, Hilton Head, SC, 2013.
[2] H. C. Kjellberg and E. G. Lightsey, "Discretized Constrained Attitude Pathfinding and Control for Satellites," Journal of Guidance, Control, and Dynamics, vol. 36, no. 5, pp. 1301-1309, 2013.
[3] C. R. McInnes, "Large Angle Slew Maneuvers with Autonomous Sun Vector Avoidance," Journal of Guidance, Control, and Dynamics, vol. 17, no. 4, pp. 875-877, 1994.
[4] U. Lee and M. Mesbahi, "Spacecraft Reorientation in Presence of Attitude Constraints via Logarithmic Barrier Potentials," in American Control Conference, San Fransisco, CA, 2011.
[5] D. Garg, M. A. Patterson, W. W. Hager, A. V. Rao, D. Benson and G. T. Huntington, "An Overview of Three Pseudospectral Methods for the Numerical Solution of Optimal Conrol Problems," in AAS/AIAA Astrodynamics Specialist Conference, Pittsburgh, PA, 2009.
[6] S. Chapman and E. A. Milne, "The Proof of the Formula for the Vector Triple Product," The Mathematical Gazette, vol. 23, no. 253, pp. 35-38, 1939.
[7] C. D. Crane III and J. Duffy, Kinematic Analysis of Robot Manipulators, New York, NY: Cambridge University Press, 1998.
[8] H. Schaub and J. L. Junkins, Analytical Mechanics of Space Systems, 2nd ed., Reston, VA: American Institute of Aeronautics and Astronautics, Inc. (AIAA), 2009.
[9] J. Diebel, "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors," Stanford Univeristy, Stanford, CA, 2006.
[10] W. E. Dixon, A. Behal, D. M. Dawson and S. P. Nagarkatti, Nonlinear Control of Engineering Systems: A Lyapunov-Based Approach, Boston, MA: Birkhäuser, 2003.
[11] Y. Zhang, Matrix Calculus and Algebra, Pittsburgh, PH: Carnegie Mellon University, 2010.
[12] C. A. Felippa, "Appendix F," in Introduction to Finite Element Methods, Boulder, CO, University of Colorado, 2004, pp. F1-F9.
134
[13] K. B. Petersen and M. S. Pedersen, "The Matrix Cookbook," Massachusetts Institute of Technology (MIT), Cambridge, MA, 2006.
[14] S. M. Selby, Standard Mathematical Tables, Boca Raton, FL: CRC Press, 1974.
[15] D. Joyce, The chain rule, part 1, Worcester, MA: Clark University, 2014.
[16] H. K. Khalil, Nonlinear Systems, Upper Saddle River, NJ: Prentice-Hall, Inc., 1996.
[17] J. W. Conklin, Lecture 4: Numerical Simulation, Gainesville, FL: University of Florida, 2015.
[18] J. D. Muñoz and F. A. Leve, "Artificial Potential Steering for Angular Momentum Exchange Devices," in American Astronomical Society (AAS), Austin, TX, 2012.
[19] M. H. Mabrouk and C. R. McInnes, "Wall Following to Escape Local Minima for Swarms of Agents Using Internal States and Emergent Behaviour," in World Congress on Engineering, London, U.K., 2008.
135
BIOGRAPHICAL SKETCH
Tristan Newman graduated a master’s student of aerospace engineering at the
University of Florida (Go Gators!) on August 8th, 2015. Before that, he received his
bachelor’s degree from that same institution on August 11th, 2012 in the field of nuclear
engineering, having served for four years in the Naval Reserve Officer Training Corps.
His transfer to aerospace engineering was motivated by a long-standing fascination with
the mechanics of motion and the natural beauty of the universe in which we live. In
pursuing his passion, he was awarded the Dr. Charles Stein Outstanding Scholar Award
for his research. He is happily married to his wife, Roxanne Rezaei, with whom he lives
in Gainesville, Florida.
