Embed Size (px)
Citation preview
Aggressive Landing Maneuvers for
Unmanned Aerial Vehicles
by
Selcuk Bayraktar
Submitted to the Department of Aeronautics and Astronauticsin partial fulfillment of the requirements for the degree of
Master of Science in Aeronautics and Astronautics
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
February 2006
@ Massachusetts Institute of Technology 2006. All rights reserved.
A uthor ................................Department of Aeronautics and Astronautics
February 3, 2006
C ertified by ....... ...............................Eric Feron
Visiting ProfessorThesis Supervisor
Accepted by..Jaime Peraire
Professor of Aeronautics and AstronauticsChairman, Department Committee on Graduata tudents
AERO
MASSACHUSETTS INSTITUTEOF TECHNOLOGY
JUL 10 2006
LIBRA~fES
2
Aggressive Landing Maneuvers for
Unmanned Aerial Vehicles
by
Selcuk Bayraktar
Submitted to the Department of Aeronautics and Astronauticson February 10, 2006, in partial fulfillment of the
requirements for the degree ofMaster of Science in Aeronautics and Astronautics
Abstract
VTOL (Vertical Take Off and Landing) vehicle landing is considered to be a criticallydifficult task for both land, marine, and urban operations. This thesis describesone possible control approach to enable landing of unmanned aircraft systems at allattitudes, including against walls and ceilings as a way to considerably enhance theoperational capability of these vehicles. The features of the research include a novelapproach to trajectory tracking, whereby the primary system outputs to be trackedare smoothly scheduled according to the state of the vehicle relative to its landingarea. The proposed approach is illustrated with several experiments using a low-costthree-degree-of-freedom helicopter. We also include the design details of a testbed forthe demonstration of the application of our research endeavor. The testbed is a modelhelicopter UAV platform that has indoor and outdoor aggressive flight capability.
Thesis Supervisor: Eric FeronTitle: Visiting Professor
3
4
Acknowledgments
First and foremost, I would like to extend my infinite thanks to my Dear LORD, who
is the most gracious, and the most merciful. Few words can express the gratitude, I
feel from the depth of my heart, for His countless bounties.
I would like continue by thanking my advisor Prof. Eric Feron for his mentorship,
his sincerity and his friendship. He means much more than just an advisor to me.
It has been a great pleasure for me to meet my dear lab mates, and even certainly
a greater one to spend countless hours with them in the lab. In alphabetical order:
Shan-Yuan Ho aka HoHo for the great coffee and chat sessions after midnight.
Bernard Mettler for his advice, Navid Sabbaghi for our deep conversations, Tom
Schouwenaars for having such a pure heart, Olivier Toupet for all the great times of
friendship, Glenn Tournier for being such a helpful and a warm face.
The MIT experience has been a personal transformation for me, mainly because
of my dear friends and their beautiful example. Very special thanks go to my friend
Abdulbasier Aziz, who is dear to me like my own brother, and the members of the
MITMSA community who affected me deeply.
There is many other names I would like to thank. In order not to be unfair to
anyone by forgetting their names, I am rather choosing to thank them in person.
Lastly, I would like to thank my dear Mother and my dear Father for their con-
tinuous love, support and sacrifice, none of which I can pay back.
Selcuk Bayraktar
MIT 10-Feb-2006
5
6
Contents
1 Introduction 13
2 Method 17
2.1 Overview of the Proposed Approach . . . . . . . . . . . . . . . . . . . 17
2.1.1 Generic Vehicle longitudinal dynamics . . . . . . . . . . . . . 18
2.1.2 Nonlinear Controller Design . . . . . . . . . . . . . . . . . . . 19
2.1.3 Penalty Based Controller . . . . . . . . . . . . . . . . . . . . . 22
2.1.4 Stability Verification and Tracking Performance . . . . . . . . 25
3 Experimental Setup and Experiments 27
3.1 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.1.1 3DOF Helicopter . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.2 Experim ents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4 Testbed Development 35
4.1 Overall Testbed Description . . . . . . . . . . . . . . . . . . . . . . . 35
4.2 Active Stereo Tracking System . . . . . . . . . . . . . . . . . . . . . . 39
4.2.1 Overall System Design . . . . . . . . . . . . . . . . . . . . . . 39
4.2.2 Camera Calibration . . . . . . . . . . . . . . . . . . . . . . . . 40
4.2.3 Forward Kinematics Map for the Pan-Tilt Unit . . . . . . . . 41
4.2.4 Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.5 3D Localization . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.2.6 Error Covariance Calculation . . . . . . . . . . . . . . . . . . 45
7
5 Conclusions and Future Directions
A Extended Kalman Filter 49
A.1 Filter Description ....... ............................. 49
A.1.1 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . 50
A.1.2 Linearization around Estimated state . . . . . . . . . . . . . . 52
A.1.3 Prediction Step: Solutions of the Nonlinear Equations of Motion 55
A.1.4 Sensor Noise Models . . . . . . . . . . . . . . . . . . . . . . . 55
A.1.5 Simulink Implementation . . . . . . . . . . . . . . . . . . . . . 56
A.1.6 Analysis of the EKF Performance . . . . . . . . . . . . . . . . 57
B EKF Codes 61
8
47
List of Figures
2-1 Planar UAV model: helicopter and fixed wing configuration. . . . . .
2-2 Range of parameters (di d 2 da) . . . . . . . . . . . . . . . . . . . . .
3-1 An illustration of the 3DOF helicopter.[13] . . . . . . . . . . . . . . .
3-2 Schematic of the 3DOF helicopter.[13] . . . . . . . . . . . . . . . . .
3-3 Top view . [13] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3-4 Motor Volt to Thrust Relationship. . . . . . . . . . . . . . . . . . . .
3-5 Vertical landing experiment, actual and desired trajectory plot. . . .
3-6 Vertical landing experiment, actual and desired trajectory error plot.
3-7 Weight parameter plot for the experiment.
3-8 Snapshots of the vertical landing maneuver.
4-1
4-2
4-3
4-4
4-5
4-6
4-7
A-1
A-2
A-3
A-4
Eolo Pro model helicopter . . . . . . . . . . . . . . . . . . .
Piccolo II avionics box . . . . . . . . . . . . . . . . . . . . .
Helicopter UAV hardware block diagram . . . . . . . . . . .
Ground station hardware block diagram . . . . . . . . . . .
Pan-Tilt unit with mounted cameras .. . . . . . . . . . . . .
Calibration Rig . . . . . . . . . . . . . . . . . . . . . . . . .
Pan-tilt unit coordinate frames and revolute joint diagrams .
EKF Filter block diagram "tightly coupled" EKF filter . . .
Accelerometer sensor sample plot . . . . . . . . . . . . . . .
Gyro sensor sample plot . . . . . . . . . . . . . . . . . . . .
XYZ position and velocity state estimation error plots . . .
19
24
28
28
28
30
32
33
. . . . . . . . . . . . . . 3 3
. . . . . . . . . . . . . . 3 4
36
36
37
38
40
40
42
. . . . . 50
. . . . . 56
. . . . . 56
. . . . . 58
9
A-5 Attitude state estimation error plots . . . . . . . . . . . . . . . . . . 59
10
List of Tables
3.1 Parameter Values . . . . . .. . . . . . . . . . . . . . . . . . . . . . . 30
11
12
Chapter 1
Introduction
Vertical Take-Off and Landing vehicles represent a fundamental component of Naval,
Air and Ground support operations, because they are able to take-off and land in
confined spaces and can therefore be deployed quite rapidly in unequipped or hostile
areas. These characteristics also yield multiple uses in civilian operations, including
police, firefighting, search and rescue, newscasting and other operations. Among the
tasks associated with VTOL vehicle operations, take-off and landing certainly stand
among the most critical ones, since the vehicles must operate and establish physical
contact with the platform upon which they land. Unlike their fixed-wing counter-
parts, which must rely on a paved or unpaved runway system and therefore require
a nontrivial infrastructure, the freedom and flexibility offered by VTOL operations
are often leveraged together with the pilot expertise to perform landing operations
in a wide variety of environments. However, the recent advent of Unmanned Aerial
Vehicles has removed the pilot from the immediate controls of the vehicle, revealing
many of the difficulties associated with VTOL landing operations. During landing,
several problems must be considered, including those of (i) properly sensing the en-
vironment at or near the landing platform as addressed in [14] and (ii) maneuvering
the vehicle in that environment in such a way that landing is possible and safe. In
this thesis, we assume the environment to be known and available to the vehicle in
real-time, and we concentrate on the task of landing the vehicle. In an attempt to
push the state-of-the-art, our effort deliberately emphasizes all-attitude landing, i.e,
13
landing of vehicles on flat, inclined or vertical surfaces and possibly inverted landings
as well. Following an argument adopted in prior research on aerobatic flight [101, our
motivation for such an effort is twofold:
" Demonstrate that landings are possible at unusual attitudes, thereby extending
the range of conditions over which hover-capable vehicles, small and big, might
be able to stop and resume flight.
" Indirectly demonstrate the safety and robustness of more traditional landings,
by displaying a much larger range of landing capabilities for the machine.
When concerned with landing operations, several factors need to be accounted for,
including the fact that many VTOL vehicles are underactuated, that is, the number
of control variables available to them (pitch/roll cyclics, collective and tail rotor in
the case of helicopters) is smaller than the number of possible outputs of interest,
which includes all six degrees of freedom of the vehicle (which evolves in SE(3) if
the vehicle is seen as a rigid body). When the vehicle is limited to three degrees of
freedom (such as the system described and used later in this thesis), the number of
actuation mechanisms is often limited to two, enough to manage the vehicle attitude
or its position, but not both at the same time. However, it is often possible to
"focus" attention onto subsets of these outputs: For example, while vehicle position
(e.g deviations from the glideslope) might be of primary interest during approach,
vehicle attitude (or relative attitude with respect to the landing site) may become
more important when the vehicle is about to land. Similar statements hold true about
many other vehicles, with car driving and maneuvering a familiar everyday example.
In this thesis, based on an aggressive landing scenario, we describe a possible con-
trol approach towards managing system outputs and corresponding control strategies
for underactuated systems. Our approach provides smooth control law transition
strategies when the "critical outputs" of the system vary as a function of state-
dependent constraints, such as vehicle proximity to obstacles. The basic principle of
our approach is, for a desired nominal (and feasible) trajectory to be followed by the
vehicle, given a number n of inputs and m > n of outputs, to first design separate
14
compensators for a given family of subsets of n outputs. We then apply a weighted
least-squares scheme to come up with a control action that properly trades off the
relative importance of these outputs, by correspondingly weighting the corresponding
control actions.
Finally we conclude with the design details of a testbed for the demonstration
of the application of the research endeavor. The testbed is a model helicopter UAV
platform and has indoor and outdoor aggressive flight capability.
15
16
Chapter 2
Method
2.1 Overview of the Proposed Approach
We assume that we are given a trajectory pair (x(t), u(t)) which is compatible with
the vehicle dynamics, which fall within the broad form of control affine systems:
± = f(x) + g(x)u where x E R', u(t) E {Actuation Set} C R' (2.1)
Together with the trajectory, we also assume the existence of a penalty function
on the system's most important outputs. We assume this penalty function to be a
function of the states of the system. The penalty function could be interpreted either
as a measure of convergence priority or tracking tolerance of the selected outputs in
different regions of interest.
Consider the following example: Assume we would like to land our helicopter UAV
vertically on a wall or conventionally (horizontally) on a ship deck. In both cases we
achieve this by a sequence maneuvers: Sloped descent followed by a flare maneuver in
the case of ship deck landing, or followed by a pull up maneuver in the case of landing
on a vertical wall. In both cases the term "maneuver" refers to a trajectory which
is not a trivial element of the equilibrium manifold of the equations of motion with
respect to spatial position or spatial velocity. Thus in a conventional ship landing case,
approaching the ship with a configuration very close to hover and then descending
17
vertically and slowly would not be referred to as a "maneuver" since the vehicle
would almost be always close to trimmed conditions. Landing procedures based on
keeping the vehicle in the state of "quasi-equilibrium" are standard practice and are
not the object of our research. To motivate the necessity of the penalty function
or, equivalently, a tracking tolerance function, consider the longitudinal dynamics
of a vehicle when performing vertical and horizontal landings. During the approach
phase, we do not care about the vehicle's attitude, but rather focus on altitude and
forward position deviations from the nominal trajectory. During final flare or pull-up
maneuver however, we require tighter tracking on the pitch angle.
The change of "important outputs" is unavoidable when landing on a vertical
wall, since then vertical vehicle position is uncontrollable when the vehicle is close to
landing. So in that sense and in the case of an underactuated and aerobatic vehicle,
changing weight on the relative importance of outputs becomes necessary. We now
detail our approach.
2.1.1 Generic Vehicle longitudinal dynamics
In the rest of this thesis, we consider only planar, longitudinal dynamics of the heli-
copter and fixed wing UAVs derived in the coordinate frame shown in figure 2-1, and
the equations of motion given by 2.2 are essentially equivalent for our purposes.
J= ui sin O + n(x)
S u cosO - g + n(x) (2.2)
6 = 1u 2 + no(x)
In these equations, the boldface x E R' is the state vector x = (x i z i 6 #)T and m
and J are the mass and the inertia of the UAV. The terms given by n,(x), nz(x), no(x)
represent forces that we will neglect in the rest of the thesis. We will extend our work
to a broader class of vehicle dynamics as our research progresses.
The justification for our choice of the resulting simplified planar dynamics is as
follows: For many miniature UAVs that our application is concerned with the thrust
18
Z U, E) U1
2u2
9
x
Figure 2-1: Planar UAV model: helicopter and fixed wing configuration.
to weight and the torque to inertia ratio are easily on the orders of a 2 : 1 for
weight, and ~ 5 : 1 for inertia if not more. Henceforth, given the high bandwidth
actuator authority on the one hand and our specific region of interest in the flight en-
velope being "aggressive region" for our particular application on the other hand, the
aerodynamics effects like drag and lift on the vehicle nacelle are somewhat negligible.
2.1.2 Nonlinear Controller Design
We use the feedback linearization technique to derive our controllers . This is by
now a standard technique to design tracking controllers. One very important and
fundamental limitation of this and all other control design techniques is the fact that
the number of outputs must be equal to the number of inputs, i.e the system must
be "square" [18] to achieve perfect tracking. This is a significant limitation if the
outputs of interest need to change during a trajectory execution. Let us motivate the
problem and our devised method by designing the controllers for our application at
hand : Land vertically on a wall.
We first derive the feedback linearizing controller using the dynamic extension
algorithm mentioned in [19]. Choosing our outputs of interest to be (x, y) as in the
beginning of landing we need to approach the landing pad. We differentiate our out-
puts until we get a locally smooth diffeomorphism back to our inputs. Applying the
dynamic extension algorithm, we get the equation 2.3 and checking the singularity
19
of the mapping Tm in equation 2.4 we see that the we can only achieve feedback
linearization with a dynamic compensator - specifically we must have a double inte-
grator to get back to our input so our dynamics are augmented with the compensator
dynamics [12].
(4) sinG u1 cos i 1 F 2bcos oi,-62
U, sin 1] m Jm + . m (2.3)Z(4) cos 0 u sinO u2 [-62u, cos -2#u. sin0
m Jm U
Det(Tm) = 2 Rank(Tm) = 2 Vu 1 , ui 4 0 assuming J, m > 0 (2.4)
Since Tm is invertible as long as ui # 0, we can proceed to define a new input
pair (v2, vz) through (2.5)
u = T- 1 (v - b) where V = [v2 vz]T, (2.5)
that decouples the (x, y) dynamics into two subsytems and puts them in the control-
lable canonical form as given by the equation (2.6). This also leaves the internal 0
dynamics to be as shown in (2.7).
5(4) = v (2.6)
z (4) = oz
6(2) = (1/ui)(mv2 cos 9 - 2i 1 - mvz sin 9) (2.7)
Having designed our (x, y) controller with our linearizing inputs (v2, vz) we can
easily design a trajectory tracker by essentially placing the poles of error dynamics
wherever desired since the resulting two subsystems are decoupled, and both are
controllable. One easy choice for a tracker would be as given in (2.8) below:
20
VX= X()- 4Ax ((x 3) X(3) 6A2 ( 2
) X (2)) 4A3 - X )- A -(X )
v2 = z4 - 4Az(z( 3) - z - 6A 2 (Z2) - z - 4A3(z(1) - z(1)) - A4(z - zd)(2.8)
where Ax, Az > 0
So the total relative degree of this system is 8, whereas the total dimension of
the original system was 6. Since the dynamic extension procedure augmented the
system with the compensator dynamics, namely the double integrator on input ui,
our total system dimension is 6 + 2 = 8. Since this is equal to the total relative degree
of the system, there is no observable internal dynamics with this choice of outputs
as suggested by [19]. So with the aid of this controller (2.8), we are guaranteed
asymptotic and exponential convergence to any sufficiently smooth trajectory in our
output space x(t), y(t) E C4, assuming also that the desired trajectory does not
violate our actuation constraints. Once the system reaches the feasible trajectory, and
if the system dynamics are exact, the system is guaranteed to stay on the trajectory
(in the absence of perturbations). (if 3 tc s.t. e(tc) = 0 => e(t) = 0 Vt > tc).
However, if we drift away from the reference trajectory, we have no control over the 9-
pitch axes convergence to the nominal trajectory. All we know a priori is exponential
convergence in the error dynamics of (x, y) output pair.
In order to explain the significance of controlling the convergence behaviour, con-
sider the following scenario: Imagine we have a feasible reference trajectory for land-
ing on a vertical wall. Assume we begin on the reference trajectory using our (x, y)
feedback linearization controller. At the final phase of the landing, assume that the
vehicle drifts along the z axis (altitude), due to wind disturbance . Since the vehicle
is almost vertical, the controller will try to generate large torque inputs to pitch the
vehicle (9 axis) in order to generate the necessary exponential convergence in both
cases. This can be seen by looking at the mapping from (v., vz) to input pair (ui, U2 ).
We can also see this intuitively from the equations of motion that there is no way of
instantenously generating forces in the directions we require in such configurations.
21
Such pitching motion is undesirable since the vehicle is very close to the landing pad
it could easily result the vehicle to hit its tail or its skids and crash.
One way around this problem would be to design other controllers with different
outputs of interest and switch controllers whenever the tracking or the convergence
behaviour in the specific outputs became more important. The natural choice of
output pairs other than (x, y) would be the (x, 6) and (z, 9) pairs. So proceeding as
before we can easily generate the input-output linearizing controllers as before. Please
note that this procedure will render the internal dynamics unobservable in both cases.
Namely the behavior of the states (z z(1))T in the former and (x x(1))T in the latter
case will totally be disregarded by the controller in favor of achieving perfect tracking
in the outputs of interest. Following a simple analysis, one can easily see that even
for trivial constant trajectories the internal dynamics might easily become unstable.
2.1.3 Penalty Based Controller
In our quest to achieve tracking and convergence to reference trajectory without
totally sacrificing one of the outputs we propose the following approach. We define our
output to be (x z 6)T. Proceeding as before with the dynamic extension algorithm for
the output pair (x, y) and augmenting this with the 0 pitch dynamics, our equations
now have the form:
X(4 Tm - - b
E-- + (2.9)
0(2) oi LU
~~ U
TM b
We see that we could invert the dynamics or achieve desired convergence in all
outputs with a controller, only when (v - >) is in the column space of im where
v = (v2 V vO)T. Since this is not necessarily the case, we construct the following
quadratic form or loosely an inner product and also define again loosely its induced
22
semi-norm as follows:
d1 0 0
< a,b >= aTWb and |a|112 =aTWa, W= 0 d2 0 (2.10)
0 0 d3
where a, b E R3 and d1 , d2 , d3 E R U+ 0} only one of d1 , d2 , d3 can be 0
We then take the weighted least squares solution to the problem at hand with
respect to the semi-norm just defined to get the control input:
U = (TWT .)- TW(v-) where u = (i61 u 2 )T and v, b as defined before. (2.11)
Figure 2-2 shows the available range of weight parameter selections for di, d2 and
d3 values. The vertices represent extreme cases where one of the outputs is not effec-
tively weighted. Therefore only the remaining two outputs are weighted for matching
the desired value of the left hand side, in which case we get perfect linearization for
those outputs. By varying the weight parameter selection in the convex hull of the 3
vertices, which represent (x, z) (x, 0) (z, 0) controllers respectively, we get the possi-
ble range of different weight penalties which lets us control the specific convergence
of our outputs back on to the desired trajectory whenever the system is drifted off
the trajectory.
Using the method outlined above and evaluating the value of u symbolically by
plugging it back into the equations of motion (2.9), we get the equivalent dynamics
of our system to be:
(4[) d1 vx (d3 m2
+2d 2U2 -d 3 m2 cos (26))+4d 2dam cos Oil+2d2 d3 cos 0(ulve+mvz sin 0)I D
Z d 2 vz (d 3 m2+2dlu2+dsm
2 cos (20))+2d1 dam(-ulvo+mVx cos 0-26i1) sin0I DIg(2) m(2d1d2u1vx cos +d 3 mve(d1+d 2 +(d 2 -di) cos (20)))-2d1d 2u1 (26iti+mvz sinG)
(2.12)
where
D = (di + d2)d3m2 + 2did 2Un + (d2 - di)d 3m2 cos (26)
23
[x z] Controller
[d1 d2 d3] = [11 0]
[d, d2 d3] = [2/3 213 2/3]
[z E] Controller [x e] Controller
[d1 d2 d3 ] = [0 1 1] [d1 d2 d3 ] = [1 0 1]
Figure 2-2: Range of parameters (di d 2 d3 )
Also evaluating the same procedure again for the values of (di d2 d3 ) at vertices of the
triangle given in the figure 2-2 we get the equivalent linearized dynamics in equations
(2.13) (2.14) (2.15) for the outputs (x, y), (x, 0) , and (z, 0) respectively.
x (4) vx
z (4) = o (2.13)
0(2) (1/ui)(mv cos 0 - 26it1 - mvz sin (
Xv(4) v
Z(4) (1/m) csc 0(-uivo + mvx cos 0 - 2#it1) (2.14)
0(2) VO
X(4) sec (1/m)(uivo + 24t 1 + mv sin 0
Z(4) voz (2.15)
0(2) I,One more important point to note here is that there are some intrinsic singularities
of our controllers for some regions of the system state space. They are mainly due
to singularities of the affine mapping Tm , b to the output space. They represent
the regions of the state space where Tm loses its full rank, specifically corresponding
to the regions where the actuation space has no projection over the output space.
24
They are given in (2.16) - (2.19) for the general values of the weights. (di d2 d3) and
specifically for the values of the weights at vertices of the triangle.
Det(TiW Tm) = (di+d 2 ) d3 m2 +2 did 2 ul+(-d1+d 2 ) d3 m
2 cos 29) =0 (2.16)2
Det(TmWTm) - U =0 (2.17)j2m4
Det(TmTWTm) - ( ) 0 (2.18)
Det(TZWTm) - (cos)2 0 (2.19)J2m2
2.1.4 Stability Verification and Tracking Performance
We have not formally verified the stability and performance of the proposed controller
so far. Instead we extensively tested the controller with the planar model, including
realistic actuator saturations in our simulations. We tried different combinations of
the (di, d2, d3 ) in the convex hull of the triangle given by the figure. We also tested
cases where the parameters were varying with time. The results were as expected:
We obtained appropriate trade offs between the tracking error bounds (i.e in the
interior of the triangle) versus perfect convergence on the chosen outputs (i.e at the
vertices) depending on the weight parameters. Errors on all variables remain bounded
if the weighting function keeps the "controller trade-off" sufficiently far away from
the vertices.
25
26
Chapter 3
Experimental Setup and
Experiments
3.1 Experimental Setup
3.1.1 3DOF Helicopter
Figure 3-1 shows our test setup which essentially emulates the flight of a reduced
degrees of freedom (DOF) helicopter. Instead of the usual six DOF of a free-flying
helicopter, the Quanser [17] only exhibits three: the pitch motion 0, the roll motion
# and the travel motion 0. The 3DOF helicopter is mounted on a table top and
its primary components are the main beam (mounted on a slip-ring), the twin rotor
assembly and the counterweight. The system is actuated by two rotors driven each
by an electric motor. The DC motors can provide either collective or cyclic voltage.
System Description
In order to derive the dynamic equations of the system, a coordinate system is used
with its origin at the bearing and slip-ring assembly, with travel (40) being the circular
motion of the main beam, (0) being the vertical motion of the beam, and pitch (#)
being the motion of the rotor assembly. The collective (Tc01 = TL + TR) and cyclic
(TcY = TL - TR) thrust serve as system inputs. The vertical motion is controlled
27
Figure 3-1: An illustration of the 3DOF helicopter.[13]
Left
L Right
Figure 3-2: Schematic of the 3DOF helicopter.[13] Figure 3-3: Top view. [13]
by collective thrust and is defined to be positive up from the horizontal position.
The cyclic thrust produces a positive change in the pitch angle. If the pitch angle is
non-zero, then the components of thrust will produce a torque around the travel axis.
Positive roll results in positive change of travel angle. The schematics of helicopter
are shown in Figures 3-2 and 3-3.
The equations of motion of 3DOF helicopter model are given in (3.2) as derived in
[13] using Newton's second law to the rate of change of angular momentum. Let J2,
J~Y and Jzz denote the corresponding principle moments of inertia. The moment of
inertia matrix was calculated by disassembling the plant, measuring the dimensions
and mass of each component and then drawing the system in CAD software. The
inertia terms and other model parameters are shown in Table 3.1. As can be seen in
the table the product of inertia terms are small and hence are neglected in the further
28
model derivation.
The equations of motion are as follows:
Jzz y = (TL + TR)L cos(O) sin(#) - (TL - TR)lh sin(9) sin(q) - Drag
Jyy# = -MglO sin(9 + 00 ) + (TL + TR)L cos(#) (3.1)
J..5 = -mgl, sin($) + (TL - TR)lh
" M is the total mass of the helicopter assembly
" m is the mass of the rotor assembly
e L is the length of the main beam from the slip-ring pivot to the rotor assembly
" lh is the distance from the rotor pivot to each of the propellers
" Drag = i p( L)2 (So + SO sin(p))L
" So and SO are the effective drag coefficients times the reference area and p is
the density of air
Since the actual system inputs are motor voltages, a thrust to voltage relationship
has been empirically determined and implemented using a lookup table. Figure 3-4
shows a relationship between the input voltage and the thrust for one of the motors.
The thrust values determined empirically and the steady state thrust values were
used in generating the graph. The dynamics of the DC motors were ignored as they
are much faster compared to the rest of system dynamics.
Tracking Controllers
In order to apply te same procedure of Section 2.1.3 to the dynamics at hand, we
used a simplified version of the dynamics in favor of removing the weak coupling of
the Tcy input to the travel O dynamics. The model with dominant terms is given as
in (3.3).
29
-5 -
Figure 3-4: Motor Volt to Thrust Relationship.
Table 3.1: Parameter ValuesParameter Value Unit Description
m 1.15 kg mass of the rotor assemblyM 3.57 kg mass of the whole setupmo 0.031 kg effective mass at hoverL 0.66 m length from pivot point to the heli bodylh 0.177 m length from pivot point to the rotorJxx 0.0344 kgm 2 moment of inertia about x-axis
Jyy 1.0197 kgm2 moment of inertia about y-axisJzz 1.0349 kgm2 moment of inertia about z-axis
J___ -0.0018 kgm2 product of inertiaJZY -0.0018 kgm 2 product of inertiaR 0.1 m radius of the rotor
g 9.8 m/s2 gravitational constant14 0.004 m length of pendulum for roll axis10 0.014 m length of pendulum for pitch axis
)= co cos(9) sin(#)To0
= C1sin(6 + 00) + C2 cos(#)Tco
= C3Tcc + c4 sin(#)
L -Mgle LCO ~~ C1 = C2
zz -Zjyy iy
(3.2)
C 3 = Ch 4 = g 1-
xx iz
Applying the procedure of Section 2.1.3 we get the equivalent equations of the
form given by (3.3).
30
[ 1 co cos 0 sin # coc 3Te0 i cos cos 0 - 1
(4) = C2 cos# -c 2c 3 Teo1 sin # + b2 (3.3)I (2) L 0 C3 J I
L L 0 C3 . L b3 JU
Tm b
where
b1 = - (co (ui(2 + 62 - C4 cos(#)) cos(9) sin(#) + u1(2 O cos(#)
+Osin(#)) sin(9) + it,(-2 cos(#) cos(9) + 2#sin(#) sin()))
-2 = (c 2 ?2U cos(#)) + c15 cos(9 + 9o) - c2 sin(#)(2i + c4ui sin(#))
-c 1 52 sin(6 + Oo)
= c4 sin(#)
3.2 Experiments
A series of aggressive vertical landing experiments with a set of different trajectories
were performed with the 3DOF setup to test the validity of our approach. The tra-
jectories used were feasible for the dynamics and they were experimentally obtained.
approach. Figures 3-5 through 3-8 show one of the experimental results. In figure
3-5 the dashed lines represents the desired trajectory. In both figures 3-5 and 3-6
the solid line represents the actual trajectory. And the two bounding lines, upper
and lower, represent equal penalty boundaries for the specific output. As can be seen
from the figures as we increase the penalty on a specific output (which corresponds
to narrowing of the equi-penalty boundary), the tracking error decreases as desired.
31
X - Travel (#) axis plot)200
150-
100
50 -. '
06
0 1 2 3 4 5
Z - Altitude (0) axis plot
60-
40-
20-
01
-20 r0
100 -
50
0
-50
-100 -0
1 2 3 4 5
Pitch - ($) axis plot
6 7 8
6 7 8
1 2 3 4 5 6 7 8
Figure 3-5: Vertical landing experiment, actual and desired trajectory plot.
32
X - Travel error (qy) axis plot)
2 0 - -- - -- -- - - - -0........ ....................
-20 ..... ............... ........ .........0 - -- - - - ------- -
- 0 - --. ..- -.-- -. ..-- -.-- - - - ..- . - -
0 1 2 3 4 5 6 7 8Z - Altitude error (0) axis plot
20 [- .
-20 - - - -
0 1 2 3 4 5 6 7 8Pitch - ($) error axis plot
20 - - --
0 .......... . ............... .........
-20 - -.-.-.-
0 1 2 3 4 5 6 7 8
Figure 3-6: Vertical landing experiment, actual and desired trajectory error plot.
0.
0.
0.
0.
-0.
Weight Parameter Plot
4d
--. d2
2 /= 4 I . M . . . . .0 ~ ~ ............ 1
0 1 2 3 4 5 6 7 8
Figure 3-7: Weight parameter plot for the experiment.
33
(a) 1 (b)2
(d) 4 (e) 5
Figure 3-8: Snapshots of the vertical landing maneuver.
34
(f) 6
(c) 3
Chapter 4
Testbed Development
4.1 Overall Testbed Description
In order to demonstrate autonomous aggressive flight capabilities both indoors and
outdoors we designed a helicopter UAV platform. As the engineering of the total
system is still in the development phase, some of the subsystems like the active stereo
positioning have been completed. Details of this system are given in the next section.
We have also successfully conducted manual flight experiments with the helicopter.
In some of the flight tests, dummy weights equaling the weight of the avionics suite
was also added to the airframe.
The airframe of the UAV is the commercially available Eolo Pro model helicopter
(80 cm rotor) shown in figure 4-1. The choice of this model was based on its agility
and being able to carry our minimum required payload for the autopilot without
compromising either of the indoor or outdoor flight capability.
Our autopilot is based on the CloudCap Technology's Piccolo II [3]. Weighing
only around 220 grams , Piccolo II is a generic sensor suite for small UAVs and
also has built in capability to fly fixed-wing aircraft autonomously and also waypoint
navigation capability. However Piccolo II lacks the capability to track aggressive
trajectories to perform agile maneuvers. Integrating it with our custom software and
additional sensors, we hope to be able to develop an aggressive helicopter autopilot
system capable of indoor and outdoor flight. Also due to our modular software
35
Figure 4-1: Eolo Pro model helicopter
Figure 4-2: Piccolo II avionics box
development effort, only small modifications will be needed to control code in the
software for the system to be also used as an agile fixed-wing aircraft autopilot.
Representative block diagrams of the hardware components of the designed heli-
copter and the ground systems are given in the following figures 4-3 and 4-4 .Dashed
blocks represent optional components of the system. As can be understood from the
figures, with all the optional blocks removed the system will still have the necessary
hardware to perform autonomous functions such as waypoint navigation outdoors
only. With the aid of the "active stereo tracking system" the UAV will be localized
precisely in close proximity of the ground station (~50 meters) both indoors and
outdoors. This will enable precise tracking of aggressive trajectories and also will be
used to aid in accurate landing of the UAV. Furthermore a third system which is a
36
Jrswooloi _
Piccolo il
CPU |R 3|Radi
MPC 555
PRES GPS IMUStatic/ Ublox 3 acceloremeter
Dynamic Pressure Tim LP 3 gyros16 bit A/D 4 Hz 16 bit A/D
PWM ||RS232 RS 232
-- - - - - AnalogUltrasonic I Honeywell I I Camera
S Altimeter 1 3 axis I & TXI Magnetometer 1 II
Figure 4-3: Helicopter UAV hardware block diagram
novel relative position and orientation [7] aiding system will be utilized once the UAV
is in even closer around 10 meter proximity to the landing pad. A typical application
of the two redundant aiding systems would be all weather robust landing on moving
platforms. A more exotic application would be "all attitude landings" i.e landing on
ceilings and vertical walls. An example scenario would be a ship deck landing. The
helicopter is navigates to close proximity of the ship via GPS. Then the active stereo
system starts tracking the UAV and brings it closer and finally with the aid of the
"moire fringe landing sensor system" the automated landing is performed robustly
nearly in all-weather conditions. One might naturally ask the question that why a
very primitive and a fundamental function such as landing or takeoff has to rely on
so many systems working in harmony. The answer is all of the systems mentioned
will be utilized on their availability without failure basis. Meaning they will only be
used in improving the solution as long as they supply valuable information to the
solution of the problem. Since the information they provide is redundant in nature
with different characteristics, this architecture will improve the robustness of the total
system.
37
GROUNDI
PC I I
0w
0
0Ea
S §23-21 Ground Station
CANI CPU IR S 232| RadioLMHXMPC 555 .4--."
PPM 1
- Radio
-- I
Active StereoTracking System
iIjiCa
2 x IEEE 1394 | 2 x RS 232
Active Landing Pad System
AnalogVideo RX
TILT igi Cam
UNIT
L PANI ~ TILT I
UNIT I- -I
MOIRE FRINGELANDING AID
SENSOR SYSTEM
IJ
Figure 4-4: Ground station hardware block diagram
38
OPERATORINTERFACE
CUSTOMSOFTWARE
-7
I
4.2 Active Stereo Tracking System
4.2.1 Overall System Design
In our pursuit for a positioning system, we started looking for solutions preferably for
COTS (Commercial Off The Shelf). The system had to meet our specific requirements
which are:
o Able to work both indoors and outdoors also be able to work together with
GPS (Global Positioning System).
o Needs to be have a range of at least 50m. It should be able to localize our
helicopter UAV.
o Has the necessary update rate around 20 Hz and positioning accuracy of roughly
20 cm.
o Needs to be able track for our experimental research platform which is a mini
helicopter UAV as it performs aggressive maneuvers in the specified range,
o Needs to be relatively cheap.
After searching for products, we concluded that we had to engineer such a system
for our application. The active stereo solution with active beacons, seemed to be an
attractive choice. Since it matches up with the criteria mentioned above relatively
well compared to other options like, "RF tagging", and ultrasonic beacons.
The system consist of two cameras mounted on two pan tilt units and IR beacons
(infrared LEDs) to be mounted on the helicopter. A picture of the system is given in
4-5. The cameras are IEEE-1394 DragonFly Cameras from PointGrey [161. The pan
tilt unit is PTU-D46-17 from Directed Perception [4]. The cameras were modified with
IR (infrared) bandpass filters in order to ease the detection of the active beacons. For
the remaining part of this section, we assume our camera model and the corresponding
projection model to be of a pinhole camera's. So all the expressions, equations in the
following sections are based on this assumption.
39
Figure 4-5: Pan-Tilt unit with mounted cameras .
Figure 4-6: Calibration Rig
4.2.2 Camera Calibration
In order to reconstruct 3D position of the target using active stereo camera setup,
the camera positions and orientations need to be known with respect to a reference
coordinate frame. Camera calibration is the procedure to determine these positions
and orientations of the cameras. It is generally achieved using images taken from
cameras of a "calibration rig" which has a known position and orientation [8). There
are readily available tools to achieve calibration conveniently. We used the Caltech
Camera Toolbox [2] along with a checkerboard pattern calibration rig. One such
sample image of the checkerboard taken from our cameras is given in figure 4-6.
We devised the following camera calibration procedure for the pan-tilt unit camera
40
combination we have. First a local reference coordinate frame is chosen, and the
calibration rig is aligned and positioned at the origin of this frame. Then the cameras
are placed with a sufficient baseline (distance between camera optical centers) that is
determined a priori. This is an important parameter since it determines the disparity
in the images and hence affects the uncertainty of the reconstruction. The field
of view of the cameras at their initial orientation must contain the calibration rig.
Then, several of the rig images are taken with the cameras to extract intrinsic and
extrinsic parameters of the cameras. For more detail regarding camera calibration
the interested reader can see the reference [8].
4.2.3 Forward Kinematics Map for the Pan-Tilt Unit
Since our cameras are not stationary, the camera calibration matrices change, as the
system tracks the target by actively panning and tilting the cameras. We need to
compensate for this motion of the cameras to achieve stereo reconstruction of the
3D world point of the target. In other words we need to know position and the
orientation of the frames which are attached to the centers of the cameras. We will
use the notation used in [15].
We will explain the forward kinematics map associated with the panning and
tilting angles of the pan-tilt units with the use of the figure 4-7. Let W denote an
absolute world frame aligned with our calibration rig. The origins of both W and
the rig are coincident as well. Let gs E SE(3) be our initial configuration at the
time of our initial calibration and we will also define our panning and tilting angles
61,62 respectively to be equal to 0. As our pan-tilt angles change we will define
gt : S x S - SE(3) to be the current configuration of our camera center with
respect to the world frame. S1 denotes the unit circle since we are dealing with
angles. From here, we need to determine, gwt(1,02) E SE(3) where 01,02 E S'. We
can easily see that:
9wt(01, 62) = 9ws g.t( 6 1, 0 2) (4.1)
Then we must determine gst(61,62). This may be achieved by using the product of
41
z
q1 :=[O, 0, -11] y
X S: PinholeCamera Center
x
T: PinholeCamera Center
E)2
%2:=[0, -12, -11]
Yw : World Frame
z
Figure 4-7: Pan-tilt unit coordinate frames and revolute joint diagrams
exponentials formula given in [15]. But we first need to define 1, 2 E se(3) as the
corresponding twist motions for the revolute joints of the pan-tilt unit represented
in frame S. To clarify the notation before proceeding any further, we give the def-
initions of (, by the expressions in 4.2 where they represent the vector and the
skew-symmetric forms of the same twist, respectively. We can evaluate the expres-
sions for these twist motions as given in 4.4.
= where v, w E R3 E so(3) (4.2)w 0 0
42
0 0
q,1= I 01
-l1, 0
-wi x qi
Wi
II
q2=
Now we can proceed to evaluate gt (1, 02)
mula given by 4.4.
-121
01
-1l
W2= 0
0
(4.3)
-W 2 X 2
2
with the products of exponentials for-
gst(61, 02) = exp ( 101) exp ( 2 2 )gst(0) where got(0) = 14x4 (4.4)
Using equation 4.1 we can easily evaluate gt(01,02) with the aid of a symbolic
mathematics package to be as given in 4.5.
gwt(1, 62) =
cos(01)
0
- sin(6 1)
0
sin(O1) sin(62)
cos(6 2)
cos(01) sin(62)
0
cos(92) sin(0 1)
- sin(02 )
cos(61) cos(62)
0
di
d2
da3Iwhere [d1 d2 d3 ]T is given by:
sin( 1)1i + sin(01) ((-1 + cos(02)) 11 + sin(02 )12 )
= -(sin( 2 )l1 ) - (1 - cos(62)) 12 (4.5)
(-1 + cos(0 1)) l1 + cos(6 1) ((-1 + cos(92)) 1i + sin(0 2)12)
4.2.4 Detection
Once the cameras are calibrated, we can determine the 3D position of our target.
Our target as mentioned before was an IR beacon (infrared LEDs). With the help
of our bandpass infrared filters, under most lighting conditions, if the aperture and
43
di
d2
d3
1i
exposure parameters of the cameras are properly setup, the target shows up as the
brightest region in the image. So the detection problem breaks down simply into
thresholding and the connected region detection problem. Both thresholding region
detection can be done extremely efficiently on relatively fast computer. Once the
region which represents the target is detected in both images, we use the "center of
mass" of the region as the corresponding image point.
4.2.5 3D Localization
Since we have detected the corresponding projection of the target in both images from
the two cameras, we can proceed to reconstructing the actual location of the target
in the world frame W that we defined before. We use the simple linear triangulation
method given in [11]. Let X = [X. Y" z, 1 T E R4 be the homogeneous coordinate
representation of target's position in the world frame W. Furthermore define, Xj =
[Xi yi s] E R3 for i = 1,2 be the homogeneous coordinate representations of the
image coordinates in the cameras 1 and 2 respectively. We have the following set of
projection equations 4.6 from both cameras:
xi = PiX.
X2 = P2X. (4.6)
= , E 3,4 -T i1 nwhere Pi = [Ki 1 03x1]gt( 6 1,02) , Pi E R gt, - g, for i 1, 2. And
Ki E R3 x3 are the intrinsic camera calibration matrices as given in [8]. We have six
equations and three unknowns, though two of the equations provide no information
since they are scale factors. Following the same procedure as in [11] to eliminate
the scale factors, we operate on both sides of the equations with the cross product
xj x xi = xi x (PiX,) to get the equivalent set of homogeneous equations xi x (PiXW) =
0. Eliminating the linearly dependent equation from both sets we end up with four
equations and three unknowns. Writing these equations in matrix form as AX, = b
where A E R4x
3 , b E R. The explicit form of A, and b are given in 4.7.
44
(zip 1 - P11) (XiP32 - P12) (XiP 3a - P13)
A = (Y1P3i - P21 ) (YiP32 - P22 ) (Y1PMa - P23 )
(x 2p31 - P11) (x 2P32 - P12) (x 2Pis - P13 )
(y2P$1 - P2i) (Y2P32 - P22) (Y2P33 - P23)
1 1P14 - Xip 3 4
b =b = P24 - Y1P342 P2
P14 - X 2 J3 4
p2 _ 2p24 - y2P34
where p, are the corresponding elements of Pk (4.7)
We are left with overdetermined sets of equations,and then we proceed to take the
minimum least squares given by 4.8.
Xw= (AT A)- AT b (4.8)
Please note that only under perfect measurements will the vector b be in the col-
umn space of the matrix A. In other words, the rays that are back-projected from the
corresponding image plane points (through the respective camera centers), will only
intersect in the three dimensional world frame when we have noiseless measurements.
For this to happen is practically almost impossible, since even due finite camera res-
olution will the measurements be noisy. Thus, we are forced to such a "error norm"
minimizing solution.
4.2.6 Error Covariance Calculation
As mentioned in the prior section, the measurements will be noisy due many sources
error. Some of these are, errors due to imperfect calibration, which can arise from
mechanical precision tolerances, detection errors in the image plane, quantization
errors, etc. Nevertheless, since the world position estimates will be fed into an EKF
(Extended Kalman Filter) estimator, we would like to determine the uncertainties of
45
our position estimates. A reasonable model for our measurement noise would be the
white Gaussian noise assumption. We assume that our image plane measurements
given by xj = xi + vi are corrupted with vi ~ N(0, Aj) where Ai E R 2x
2 are
the associated covariance matrices for the noise in the images. Since the mapping
Xw = ff(x)1, x 2 ) given in 4.8 is nonlinear. The probability distribution of the error
of f(-, -) will not necessarily be Gaussian. This would violate the requirement of
our estimator and also the math involved would be too complicated to be practical.
Instead, we use the Taylor expansion of f(-,-) given in 4.9 up to the first order as
suggested by [6]:
f(x 1 + v 1 , x 2 + v 2 ) f(x1, x 2 ) + J(x1, x 2 ) v1 (4.9)V2
where J(x 1 ,x 2) = (Vx1 ,x 2 f)T(xi,x 2 ). So the covariance of Vx in , = XW + Vx
could be approximated as given below in 4.10.
Aw = J(x1, x 2 ) A1 02x2 1 (x1, x 2 ) (4.10)02x2 A 2
AV
which follows from:
Vx J(x1,x 2)vc where vc = [vi vi]T (4.11)
E [VxVx] E [JvevTJT] = JE [vevT] jT = JAV JT (4.12)
46
Chapter 5
Conclusions and Future Directions
In this thesis, we have presented a novel approach. to nonlinear vehicle trajectory
tracking, when the important vehicle outputs change as a function of its state rel-
ative to some external environment. Considering the scenario of aggressive vehicle
landing in demanding conditions, we have illustrated our method, which consists of
generating control inputs that properly "interpolate" the system output of interest,
with initial emphasis on position, but later emphasis on attitude as the vehicle comes
close to landing. While the theoretical foundation of our work remains to be estab-
lished, several simulations and laboratory experiments already indicate the potential
usefulness of our approach. Natural extensions to this approach would be the:
" Incorporation of the full vehicle dynamics, including the lateral dynamics along
with the longitudinal dynamics.
" A systematic analysis and inclusion of actuator constraints in the given frame-
work could potentially be very useful for practical implementations.
" Much also remains to be done in terms of both the modeling environment and
obstacles, either of which could be static or dynamic.
" Exploitation of the symmetries and invariants suggested by [9), in the state
space with regards to the dynamics, could potentially generalize the use of
many stored maneuvers to land at different locations and configurations.
47
48
Appendix A
Extended Kalman Filter
A.1 Filter Description
There has been a wealth of literature published over the last decades regarding the use
of Kalman filters for the purposes of GPS aided inertial navigation. In this section, we
will explain the details of our implementation of an Extended Kalman Filter, which
will be used to estimate the states necessary for our controllers. We will explain
the details of our implementation without going too much into the theory as it is
well known and well documented in the literature. Interested reader may look at the
references listed [1].
Our model implementation is based on real sensors available on a typical UAV
avionics sensor suite (Piccolo II by CloudCap Tech.) [3]. Specifically, our sensors
consist of three strapdown gyros, three accelerometers in conventional orthogonal
placement and a GPS sensor. The gyros and the accelerometers were sampled at
a rate of 20 Hz, the GPS gives positional and velocity information at 4 Hz. Data
from the IMU (inertial sensors, gyros, accelerometers) were collected in the lab for
characterizing the spectral properties of the noise of the sensors. GPS noise model
was assumed to be Gaussian white with typical covariance levels documented from the
literature and based on prior experience with this sensor. Please note that all noise
sources from the sensors were assumed to be white Gaussian as this is a necessary
assumption for filter optimality.
49
Figure A-1: EKF Filter block diagram "tightly coupled" EKF filter
A schematic of the filter implementation is given in the figure A-1. This is a
typical example of a GPS aided INS ( Inertial Navigation System) implementation
with an Extended Kalman Filter. This specific block diagram resembles the "tightly
coupled EKE" as it is referred to in the literature. [20].
A.1.1 Dynamic Model
Our model dynamics are based on kinematic equations of motion as given in [10]. This
property makes the filter independent of the specifics of the vehicle dynamics. Thus
the filter could be as used as well with any other system having similar sensors i.e
helicopters, fixed-wing aircraft, land based vehicles, etc. For attitude representation
quaternions were used due to significant advantages they have in terms of being
singularity free and numerical error behaviour [5]. Our equations of motion are given
below in A.1.
i= Rtb(a,-ab) -g+Rtbua
q = ( m- b)q-+igq
ab = 0
S= 0 (A.1)
50
The system has sixteen states and they are as follows.
States and Relevant Terms
" x: Position in the local inertial NED (North East Down) frame. [x y z]
" v: Inertial velocity in the NED frame [vx vy ,]
e q: Quaternion representation of attitude [qo qi q2 q3]
" ab: Accelerometer biases on 3 accelerometers [abx aby abz]
" WIb: Gyros biases on 3 gyros ['bx gby 4z]
Note: hat over Tb, T,, and Ug as given in A.1 denotes
matrix form used for quaternions as given below A.2.
1
0
-@2
@y-O2
0z
0
-OX
-0y
-0y
0
-Oz
02
0
in 4x4 skew symmetric
(A.2)
* Rtb: 3x3 Rotation matrix element of the group SO(3) . It is a function of the
quaternions in this case and is used to transform body accelerations in to NED
inertial frame. The matrix in terms of quaternion attitude representation is
given below A.3.
F2 2 2 2qO + q3 - q, - q2
Rtb = 2(qoq1 + q2q 3 )
2(qoq2 - q1q3)
Inputs
2(qoq 1 - q2q3)
2 2 _ 2 - 2
q1 + q3 q 3 2
2(qlq2 + qOq 3)
2(qoq2 + q1q3)
2(q1 q2 - qoq 3 )2 2 _ 2 2 gq2 + q3 -O q1
* am: Measured acceleration vector in vehicle body frame [am, amy amz]
* 'm: Measured angular rate vector in vehicle body frame [?/mx?/)my 4mz]
* g: Local gravity vector assumed to be [0 0 g]
51
] (A.3)
* Ua: Accelerometer noise input vector in vehicle body frame (Gaussian white)
[Uax Uay Uaz|
* ug: Gyro noise input vector (Gaussian white) [ugx Ugy u9,]
Outputs
" z.: GPS position in the local inertial NED (North East Down) frame. [x y z|
" z,: GPS velocity in the NED frame [vx v, vz]
* uX: Gaussian white GPS position noise
e un: Gaussian white GPS velocity noise
A.1.2 Linearization around Estimated state
We need to linearize our nonlinear equations around the estimated state in order
to propagate Gaussian process noise covariance with the 1st order approximation
of the nonlinear equations of motion. Linearized continuous time state propagation
equations will be of the form A.4.
=Fx + Bu+Gn (A.4)
where E [n(t)nT(T)] = W6(t - r). Note x denotes the full state as given by our
model (16x1). u denotes our inputs and n denotes white Gaussian noise inputs.
Our discrete time measurement equations will be of the form A.5
z[k] = Hx[k] + v[k] (A.5)
where E[v[iJ v T[jll = R when i = j and E[v[iI VT(j) = 0 when i $ j.
We will continue by explicitly constructing the terms of the equation A.4.
52
The F Jacobian matrix will be as A.6:
03x3
0 3x3F(x, am, im) =
0 4x3
06x3
I3x3 0 3x4 0 3x3 0 3x3
03x3 3x4 R 3 x 3
03x3
0 4x3 5'4x4 0 4x3 Q4x3
0 6x3 0 6x4 0 6x3 0 6x3
The Ixj and Oixj denote identity and zero matrices of the corresponding size.
The sub block of the matrix marked with the 83x4 symbol is:
9--Rb(am - ab)c9
which when evaluated symbolically is too big to fit in here. So it is not given.
The code is included in Appendix B.
The sub block marked with R is:
Rtb
The sub block given by 'I (in skew symmetric form) is
q/m - b
Lastly the sub block marked with Q4x3 is:
q3 -q2
1q2 q3Q4x3 ~ -- ~q q
2 -q 1 qo
-qo -q 1
-qo
q3
-q2
The B matrix and the G matrix of A.4 are the same and they are given by:
53
(A.6)
q1
0 3x3 0 3x3
B=G= Rtb 0 3x3
0 4x3 Q4x3
0 6x3 0 6x3
Furthermore we need to discretize the linearized continuous state propagation
equations to put them into the form necessary for digital computer implementation.
Our IMU measurements were sampled at a rate 20 Hz. We assume that they are
approximately constant between samples.
The linearized discrete state propagation equations will be of the form given in
A.7.
x[k+1] =4x[k] + F u[k] + Qn[k] (A.7)
where E[n(i)nT(j)] = W when i = j and E[n(i)nT(j)] = 0 when i 5 j. Along with
measurement equations above one more assumption we need to make here is state
noise nfk] is not correlated with u[k] . Formally stating E[n[i]vT[j]] = 0 for all i, j.
The expressions for evaluating the 1 and the Q matrices are as given by the [1].
They are repeated here in A.9 for clarity.
-F GWGTA 0 FT At (A.8)
M = expA = [ Q (A.9)
Using the two subblocks of the matrix M we can easily evaluate the state transition
matrix (D and the process noise covariance matrix Q. The F and the Q matrices will
be omitted since they are not necessary for the implementation. Measurement update
covariance propagation equations are also omitted since they are standard. Interested
readers can refer to [1].
54
A.1.3 Prediction Step: Solutions of the Nonlinear Equations
of Motion
Though generally it not possible to obtain analytical solutions for nonlinear odes
(Ordinary Differential Equations), it was possible in this case.In the implementation
the nonlinear equations were solved numerically. They are not given here interested
reader may contact the author.
A.1.4 Sensor Noise Models
The IMU ( inertial measurement unit) of the Piccolo II was used to collect data from
the 3 gyros and 3 accelerometers. As noted earlier, the Gaussian white noise assump-
tion had to be made on the sensor characteristics. The IMU sensor noises were also
assumed to be statistically independent of each other. The resulting covariance ma-
trices based on 5000 sample data points for accelerometers and gyros were evaluated
to be:
E[uau T] = diag [9.05 x 10-05 , 1.96 x 10-04 , 1.43 x 10~04]
E[u T,u) = diag [2.12-05 , 8.72 x 10~06 , 1.51 x 10-05]
GPS measurement noise were again assumed to be a white noise sequence with
Gaussian distribution and based on crude approximations which are based on previous
experience with this sensor before. They are given below .
E[u,,u1 T] diag [5 , 5 , 25]
E[uvu T] = diag [0.5 , 0.5, 1]
In addition a sample plot of the typical sensor noise is given in figures A-2 and
A-3 based on collected data.
An interesting point worth noting here is that without an absolute heading ref-
erence during steady and level flight with constant velocity, the heading becomes
55
-0.7
C0.,1
Figure A-2: Accelerometer sensor sample plot
0.0
Figure A-3: Gyro sensor sample plot
unobservable with the combination of sensors used [4] . A detailed analysis of the
observability of the certain modes of the system along various trajectories is omitted
here for simplicity sake.
A.1.5 Simulink Implementation
Despite the wealth of literature on the subject matter, implementation of the filter
turned out to be a very painful task due to complicated symbolic expressions that
needed to be evaluated and also due to the relatively large dimensions of the filter,
namely sixteen. Simulink codes of the overall EKF implementation are given in the
next section of the appendix. Implementation of the filter is mainly done in the
custom written in an embedded m-function block.
56
A.1.6 Analysis of the EKF Performance
As noted before getting the filter to work properly was a very painful task . Also
given the fact that EKF is an ad hoc procedure with no theoretical guarantees of
converges of the state error estimates, a lot of the sources of implementation mistakes
were hard to identify.
The EKF does a reasonable job with respect to keeping the state estimate errors
low. Please note that as predicted for steady level flight the heading error diverges
over long time horizons as it was expected. So some maneuvering in the bank was
added to achieve convergent behavior. Error plots of estimated states for sample run
of 50 seconds are given in figures A-4 and A-5.
57
XYZ position error in inertial NED frame
0 5 10 15 20 25 30 35 40 45 50
XYZ velocity error inertial NED frame
0 5 10 15 20 25 30 35 40 45 50
Figure A-4: XYZ position and velocity state estimation error plots
58
Bank angle error in degrees
Pitch angle error in degrees
5 10 15 20 25 30 35 40 45
Heading error in degrees
5 10 15 20 25 30 35 40 45
Figure A-5: Attitude state estimation error plots
59
10
8
4
2
0
-4
10
5
-5
-10o-
015 f I I I I I I I I I1
60
Appendix B
EKF Codes
function [Pout,ex,DCM] = EKF3D(PSIm,Am,x,z,t)
X function [Pout,ex] = EKF3D(PSIm,Am,x,z,t)
X Selcuk Bayraktar Dec 2005 - MIT
% EKF 3D update equations
X inputs are as follows
X PSIm: measured body rotations rates in body frame rad/s [p q r]
X Am: measured body accelerations in body frame m/s^2 [ax ay az]
% x: State vector [px py pz vx vy vz qO q1 q2 q3 abx aby abz psibx
psiby psibz]
X px-z are positions vx-z are velocities
X qO - q4 are quaternions for attitude rep.
X abx-z are accelerometer biases psix-z are gyro biases
X z: GPS measurement update [x y z vx vy vz]
X t: is simulation time
X Outputs are as follows:
X Pout: 16x16 State Covariance
X ex: Estimated State error
% Big Time this was a lot of trouble
61
XY This block supports an embeddable subset of the MATLAB language.
persistent F;
persistent Pminus;
persistent Q;
persistent R;
persistent exhat;
persistent H;
persistent time_t;
persistent Rtb;
persistent W;
if (isempty(H))
H=[eye(6) zeros(6,10)];
end
if (isempty(F))
F=zeros(16);
end
if (isempty(P-minus))
P_minus=eye(16)*0.001;
P_minus(1:6,1:6)=1*eye(6);
P_minus(11:16,11:16)=0.1*eye(6);
end
if (isempty(Q))
Q=eye(16)*1e-6;
Q(11:13,11:13)=2e-4*eye(3);
Q(14:16,14:16)=2e-5*eye(3);
62
W=Q(11:16,11:16);
end
if (isempty(R))
R=0.5*eye(6);
R(1:3,1:3)=diag([5 5 25]);
end
if (isempty(ex-hat))
exhat=zeros(16,1);
end
if (isempty(time-t))
time_t=t;
end
if (isempty(Rtb))
Rtb=eye(3);
end
XXDoImuUpdate T=0.05s (20Hz)
if((mod(round(t*1000),0.05*1000)==0)&&((time_t~=t)))
X init states here as well
XX and everything else that needs to be
P=zeros(16);
X Let me pull some of the states necessary from the state vector x
qO=x(7);
ql=x(8);
q2=x(9);
q3=x(10);
63
abx=ex-hat(11);
aby=ex-hat(12);
abz=ex-hat(13);
Pb=ex-hat(14);
Qb=ex-hat(15);
Rb=ex-hat(16);
% Let me pull the IMU measurements
Pm=PSIm(1);
Qm=PSIm(2);
Rm=PSIm(3);
amx=Am(1);
amy=Am(2);
amz=Am(3);
%% Jacobian Matrix
Rtb=[(qO^2)-(q1^2)-(q2^2)+(q3^2) 2*(qO*ql+q2*q3) 2*(qO*q2-q1*q3);...
2*(qO*ql-q2*q3) -(qO^2)+(q1^2)-(q2^2)+(q3^2) 2*(ql*q2+qO*q3); ...
2*(qO*q2+ql*q3) 2*(ql*q2-qO*q3) -(qO^2)-(q1^2)+(q2^2)+(q3^2)];
Rtb=Rtb';
delqORt=[(2*(-abx + amx)*qO+2*(-aby + amy)*ql + 2*(-abz + amz)*q2);...
(-2*(-aby + amy)*qO + 2*(-abx + amx)*ql - 2*(-abz + amz)*q3);...
(-2*(-abz + amz)*qO + 2*(-abx + amx)*q2 + 2*(-aby + amy)*q3)];
delqlRt=[(2*(-aby + amy)*qO - 2*(-abx + amx)*qi + 2*(-abz + amz)*q3);...
(2*(-abx + amx)*qO + 2*(-aby + amy)*ql + 2*(-abz + amz)*q2);...
(-2*(-abz + amz)*ql + 2*(-aby + amy)*q2 - 2*(-abx + amx)*q3)];
64
delq2Rt=[(2*(-abz + amz)*qO - 2*(-abx + amx)*q2 - 2*(-aby + amy)*q3);...
(2*(-abz + amz)*ql - 2*(-aby + amy)*q2 + 2*(-abx + amx)*q3);...
(2*(-abx + amx)*qO + 2*(-aby + amy)*ql + 2*(-abz + amz)*q2)];
delq3Rt=[(2*(-abz + amz)*ql - 2*(-aby + amy)*q2 + 2*(-abx + amx)*q3); ...
(-2*(-abz + amz)*qO + 2*(-abx + amx)*q2 + 2*(-aby + amy)*q3);
(2*(-aby + amy)*qO - 2*(-abx + amx)*ql + 2*(-abz + amz)*q3)];
SkewPsiM=[O Rm/2 -Qm/2 Pm/2; ...
-Rm/2 0 Pm/2 Qm/2;...
Qm/2 -Pm/2 0 Rm/2; ...
-Pm/2 -Qm/2 -Rm/2 0];
SkewPsiB=[0 Rb/2 -Qb/2 Pb/2; ...
-Rb/2 0 Pb/2 Qb/2; ...
Qb/2 -Pb/2 0 Rb/2;...
-Pb/2 -Qb/2 -Rb/2 0];
PsiToQuat= 0.5*[q3 -q2 q1;...
q2 q3 -qO;...
-qi qO q3;...
-qO -qi -q2];
XX Let us Start Forming the Jacobian Matrix
F=zeros(16);
F(1:3,4:6)=eye(3);
F(4:6,7:10)=[delqORt delq1Rt delq2Rt delq3Rt];
F(4:6,11:13)=[-Rtb];
F(7:10,7:10)=[SkewPsiM-SkewPsiB];
F(7:10,14:16)=[-PsiToQuat];
65
XLet us Form the Transition Matrix Now
tao=0.05; XX Sampling interval for IMU
PHI=eye(16);
PHI=expm(F*tao);
G=[zeros(3,6);...
Rtb zeros(3);...
zeros(4,3) PsiToQuat;...
zeros(6,6)];
A=[-F G*W*G';...
zeros(16) F'];
B=zeros(32);
temp=zeros(16);
B=expm(A*tao);
temp=B(1:16,17:32);
Q=PHI*temp;
XX Add some very small noise for the bias terms
XX So that the P covariance matrix does become singular
Q(11:16,11:16)=le-9*eye(6);
XX Update Covariance here
if(mod(round(t*1000),0.25*1000)==O)
X (DoMeasurementUpdate)%X Do GPS measurent update at T=0.25s (4Hz)
XX Let us Calculate Calculate optimal K
K=Pminus*(H')*(inv(H*P-minus*(H')+R));
I=eye(16);
XX Covariance update due to measurement
P_minus=(I-K*H)*Pminus*((I-K*H)')+K*R*K';
66
XXLet us do the update on the state
XX x.hat= xhatminus + K (z-z-hat)
x_hatminus=x;
z_hat=H*xhatminus;
ex-try=K*(z-z-hat);
ex-hat(1:6)=ex-hat(1:6)+ex-try(1:6);
ex-hat(7:16)=ex-try(7:16);
% The following is only one of the many thousand things I tried.
Xq-new=zeros(4,1);
%q-norm=norm([ex-hat(7:10)+x(7:10)]);
Xq_new=[exhat(7:10)+x(7:10)]/q-norm;
Xexhat(7:10)=q-new-x(7:10);
end X GPS measurement update
XX Covariance update for the prediction step
P_minus=PHI*Pminus*(PHI')+Q;
XX In the end Assign the new covariance matrix to the old one to keep
P_minus=Pminus;
end
time_t=t;
XX Now proceed with outputs
ex=exhat;
Pout=trace(P-minus);
DCM=Rtb';
67
68
Bibliography
[1] Robert Grover Brown and Patrick Y. C. Hwang. Introduction to Random Signals
and Applied Kalman Filtering. John-Wiley Sons, 1997.
[2] Caltech Camera Toolbox Website. www.vision.caltech.edu, last accessed Feb
2006.
[3] CloudCap Technology Website. www.cloudcaptech.com, last accessed Feb 2006.
[4] Directed Perception Website. www.dperception.com, last accessed Feb 2006.
[5] J. Farrel and M. Barth. The Global Positioning System and Inertial Navigation.
McGraw-Hill, 1997.
[6] Olivier Faugeras. Three-Dimensional Computer Vision. The MIT Press, 1993.
[7] Eric Feron and Jim Paduano. A passive sensor for position and attitude estima-
tion using an interferometric target. In Proc. 43rd IEEE Conference on Decision
and Control, pages 1663-1669, Bahamas, December 2004.
[8] David A. Forsyth and Jean Ponce. Computer Vision: A Modern Approach.
Prentice Hall, 2002.
[9] Emilio Frazzoli. Robust hybrid control for autonomous vehicle motion planning.
PhD thesis, Massachusetts Institute of Technology, Massachusetts, 2001.
[10] Vladislav Gavrilets. Autonomous aerobatic maneuvering of miniature helicopters.
PhD thesis, Massachusetts Institute of Technology, Massachusetts, August 2003.
69
[11] Richard Hartley and Andrew Zisserman. Multiple View Geometry in Computer
Vision. Cambridge University Press, 2004.
[12] John Hauser, Shankar Sastry, and George Meyer. Nonlinear control design for
slightly non-minimum phase systems: application to VSTOL aircraft. Automat-
ica, 28(4):665-679, 1992.
[13] Masha Ishutkina. Design and implimentation of a supervisory safety controller
for a 3DOF helicopter. Master's thesis, Massachusetts Institute of Technology,
2004.
[14] Luis 0. Mejias, Srikanth Saripalli, Pascual Cervera, and Gaurav S. Sukhatme.
Visual servoing of an autonomous helicopter in urban areas using feature track-
ing. Journal for Field Robotics, 2006.
[15] Richard M. Murray. A Mathematical Introduction to Robotic Manipulation. CRC,
1994.
[16] Pointgrey Website. www.ptgrey.com, last accessed Feb 2006.
[17] Quanser Website. www.quanser.com, last accessed Feb 2006.
[18] Shankar Sastry. Nonlinear Systems Analysis, Stability and Control. Springer,
1999.
[19] J.J.E. Slotine and W. Li. Applied Nonlinear Control. Pearson Education, 1990.
[20] Salah Sukkarieh. Low Cost, High Integrity, Aided Inertial Navigation Systems for
Autonomous Land Vehicles. PhD thesis, University of Sydney, Sdyney,Australia,
March 2000.
70
