PATH PLANNING AND CONTROL OF UNMANNED AERIAL VEHICLES IN THE PRESENCE OF WIND By Andrew Charles Vaughn B.S. (University of Illinois, Urbana-Champaign) 2002 A report submitted in partial satisfaction of the requirements for the degree of Master of Science, Plan II in Mechanical Engineering at the University of California at Berkeley Committee in charge: Professor J. Karl Hedrick, Chairman Professor Raja Sengupta Fall 2003
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
PATH PLANNING AND CONTROL OF UNMANNED AERIAL VEHICLES IN THE PRESENCE OF WIND
By
Andrew Charles Vaughn
B.S. (University of Illinois, Urbana-Champaign) 2002
A report submitted in partial satisfaction of the requirements for the degree of
Master of Science, Plan II
in
Mechanical Engineering
at the
University of California at Berkeley
Committee in charge: Professor J. Karl Hedrick, Chairman
Professor Raja Sengupta
Fall 2003
1
Abstract
Path planning and Control of Unmanned Aerial Vehicles in the Presence of Wind
by
Andrew Charles Vaughn
Master of Science in Mechanical Engineering
University of California, Berkeley
Professor J. Karl Hedrick, Chair
This thesis presents strategies for path planning of unmanned aerial vehicles
(UAVs) to track a moving target or point of interest. The UAVs are constrained in their
turn rate and speed. The motion of the point of interest is unconstrained and is not
predefined. Trajectories are developed based on the velocities of the UAV and the point
of interest. The trajectory generation is first given for one UAV and then expanded to
multiple UAVs flying in formation. Stable control laws were developed for the UAVs
and the formation to track the desired trajectory. Simulation and experimental results
confirm the approaches within this thesis. In order to improve the tracking of the desired
trajectories, wind disturbance rejection controls are examined. The current autopilot is
enhanced using several sensor feedback techniques. H2 and H∞ control theory is outlined
and applied to the problem of wind disturbances. Actuator bandwidth and sensor noise
constraints are included in the design process. The new control laws are tested, with
some evidence that H∞ control may help improve the control of the UAV in the presence
of wind.
To Natalie
i
Contents
List of Figures iii 1 Introduction 1 1.1 Motivation and Background .......................................................................... 1 1.2 Organization................................................................................................... 3 2 Path Planning for a Single UAV 4 2.1 Introduction.................................................................................................... 4 2.2 Path Planning Algorithm................................................................................ 4 2.3 Wind Compensation....................................................................................... 9 2.4 Simulation Implementation and Results ........................................................ 9 2.5 Experimental Results ..................................................................................... 15 3 Orbital Trajectory Specification for Multiple UAVs 19 3.1 Introduction.................................................................................................... 19 3.2 Orbital Trajectory Theory.............................................................................. 20 3.2.1 Definition ........................................................................................... 20 3.2.2 Parameterization ................................................................................ 22 3.2.3 Orbit Control...................................................................................... 28 3.3 Formation Control.......................................................................................... 30 3.4 Simulation Results ......................................................................................... 31 3.5 Experimental Results ..................................................................................... 32 4 Disturbance Rejection Control 36 4.1 Introduction.................................................................................................... 36 4.2 Overview of Aircraft Dynamics..................................................................... 37 4.3 Simulation Environment ................................................................................ 40 4.4 Current Autopilot ........................................................................................... 41 4.5 Enhancement of Current Autopilot................................................................ 47 4.5.1 Utilization of Additional Sensors....................................................... 47 4.5.2 Alternative Usage of Rate Gyros ....................................................... 50 4.5.3 Comparison of Enhancement Techniques ......................................... 51 4.6 H2 Control Theory.......................................................................................... 53 4.7 H∞ Control Theory......................................................................................... 56 4.8 Weighting Functions...................................................................................... 58
ii
4.8.1 Actuator Models and Weights ........................................................... 59 4.8.2 Sensor Models and Weights............................................................... 60 4.8.3 Wind Model and Weights .................................................................. 62 4.8.4 Autopilot Command Weight.............................................................. 65 4.8.5 Ideal Command Response.................................................................. 66 4.8.6 Performance Weights......................................................................... 67 4.9 Interconnected System ................................................................................... 69 4.10 Simulation Results and Discussion................................................................ 71 4.10.1 H2 Control Simulation and Results .................................................... 71 4.10.2 H∞ Control Simulation and Results ................................................... 74 4.11 Gain-scheduling H∞ Control .......................................................................... 79 4.12 Future Work ................................................................................................... 83 5 Conclusions 84 Bibliography 86
iii
List of Figures
2.1 Top view of path planning algorithm in sinusoidal mode ............................. 5 2.2 Speed ratio, s , vs amplitude/distance, A/D1, ratio of path planning .............. 7 2.3 Top view of path planning algorithm in the rose curve loitering mode ........ 8 2.4 Piccolo miniature avionics system................................................................. 10 2.5 Piccolo ground station.................................................................................... 10 2.6 Hardware-in-the-loop (HIL) simulation schematic with 3 aircraft................ 10 2.7 Point of interest (POI) defined in terms of the ground vehicle...................... 11 2.8 Simulation of UAV and ground vehicle with no wind .................................. 13 2.9 Simulation with no wind compensation, 10 m/s south wind ......................... 13 2.10 Simulation with wind compensation, 10 m/s south wind .............................. 14 2.11 Simulation with no wind and changing speed in the ground vehicle ............ 14 2.12 Diagram of experimental setup with 3 aircraft .............................................. 15 2.13 Data from the entire experiment, with the UAV following a truck ............... 16 2.14 Close up of experimental data highlighting loitering mode .......................... 17 2.15 Close up of experimental data highlighting sinusoidal mode........................ 17 2.16 Picture of video taken from a camera on the bottom of the UAV ................. 18 3.1(a) Lemniscate trajectory in POI coordinates...................................................... 21 3.1(b) Lemniscate trajectory in fixed reference frame coordinates.......................... 21 3.2 Trajectory parameter assignment algorithm .................................................. 26 3.3 Trajectories in the POI coordinate frame....................................................... 26 3.4 Values of p versus VP and VT ......................................................................... 27 3.5 Values of A versus VP and VT......................................................................... 27 3.6 Control law development for a single UAV following an orbit trajectory.... 29 3.7 Illustration of orbit trajectory and the formation ........................................... 30 3.8 Simulation with 3 UAVs tracking a ground vehicle, varying speed.............. 31 3.9 Simulation with 3 UAVs tracking a ground vehicle, changing heading........ 32 3.10 Picture of Rascal experimental UAV platform.............................................. 33 3.11 Top view of experiment with UAV and ground vehicle paths ...................... 33 3.12 Ground coverage plot from the UAV cameras .............................................. 34 3.13 Picture of three UAVs flying in formation .................................................... 35 4.1 Aircraft velocities, forces, and moments in body-axes.................................. 38 4.2 Picture of the Aerosonde UAV...................................................................... 40 4.3 Simulink diagram of the aircraft simulation .................................................. 41 4.4 Complete Simulink simulation setup with Piccolo autopilot......................... 42
iv
4.5 Bank angle to turning radius in coordinated turn flight................................. 44 4.6 Simulation of current autopilot for wind = 0 ................................................. 46 4.7 Simulation of current autopilot for wind = 5 m/s .......................................... 46 4.8 Washout filter placed on the accelerometer and roll angle............................ 48 4.9 Simulation of current autopilot with enhancement of ay integral, no wind ... 49 4.10 Simulation of current autopilot with enhancement of ay integral, wind ........ 49 4.11 Simulation of current autopilot with filtered roll enhancement, no wind...... 50 4.12 Simulation of current autopilot with filtered roll enhancement, wind........... 51 4.13 Comparison of the three controllers with no wind......................................... 52 4.14 Comparison of the three controllers with 5 m/s wind.................................... 52 4.15 General control configuration with generalized plant, P, and controller, K .. 54 4.16 Model of actuator with delay and mechanical model .................................... 59 4.17 Frequency response of sensor models............................................................ 62 4.18 Frequency response of the side gust velocity weighting function ................. 64 4.19 Frequency response of the roll gust velocity weighting function .................. 65 4.20 Frequency response of the autopilot command weighting function .............. 66 4.21 Step response of the ideal second-order system ............................................ 67 4.22 Frequency response of the yaw rate error weighting function....................... 68 4.23 Generalized control configuration ................................................................. 69 4.24 The interconnected system comprised of all of the weights and models....... 70 4.25 Representation of the controller, K, with input and output signals................ 70 4.26 Hankel singular values of the H2 controller, K2, with the balanced states..... 71 4.27 H∞ norm of the closed loop system with the reduced controller, K2red .......... 72 4.28 Yaw rate response for the linear aircraft model with the controller K2red ...... 73 4.29 Response of the nonlinear system with the discrete controller K2red ............. 73 4.30 Hankel singular values of the H∞ controller, K∞, with the balanced states.... 75 4.31 H∞ norm of the closed loop system with the reduced controller, K∞red ......... 75 4.32 Yaw rate response for the linear aircraft model with the controller K∞red ..... 76 4.33 Response of the nonlinear system with the discrete controller, K∞red............ 76 4.34 Response of the nonlinear system with the continuous controller, K∞red....... 77 4.35 Response of the nonlinear system with continuous controller, K∞new............ 78 4.36 H∞ gain scheduler.......................................................................................... 79 4.37 Simulation of gain scheduling discrete H∞ controller, no wind..................... 80 4.38 Simulation of gain scheduling discrete H∞ controller, wind.......................... 81 4.39 Comparison of autopilot and gain scheduling H∞ controller, no wind .......... 82 4.40 Comparison of autopilot and gain scheduling H∞ controller, wind ............... 82
v
Acknowledgements
Much thanks goes to all of the people in the Vehicle Dynamics Lab (VDL) and
the Center for Collaborative Control of Unmanned Vehicles (C3UV) who made this
research experience very enjoyable. In particular, Professor J. Karl Hedrick and
Professor Raja Sengupta assisted greatly in forming a great research project and group,
and provided valuable advice whenever needed. Particular individuals who deserve
special notice for their help in this research are Xiao Xiao, Jusuk Lee, Stephen Spry, Tim
McGee, and Eric Frew.
This work was supported, in part, by the Office of Naval Research (ONR) under
STTR Phase I Grant N00014-02-M-0236 and the 6.1 AINS Funding Program.
1
Chapter 1
Introduction
1.1 Motivation and Background
Path planning for unmanned aerial vehicles (UAVs) is not a new concept by any
means. People, especially the military, have been using UAVs to perform surveillance
activities for a long time. Today, UAVs are becoming increasingly popular to perform
more sophisticated missions that include surveillance, sensing, and combat routines [10,
13, 18]. UAVs are particularly useful in these applications because they minimize the
risk for loss of human life, they are able to perform monotonous duties for long periods
of time, and they are able to be operated by a limited number of personnel (many UAVs
operated by one person). As the missions become more demanding, UAVs need to have
better path planning techniques. This means that the control has to be refined in order to
have better tracking capabilities.
The operational needs of the UAVs address a couple of key issues that need to be
applied for UAVs to perform their tasks successfully. First, their flight time needs to be
increased. In order to maintain simplicity and a lower cost, the flight time must be
increased by making the UAVs fixed-wing aircraft. A limitation that arises from using
fixed-wing aircraft is that they have a limitation on the airspeeds that they are allowed to
fly in order to maintain stable flight. Additionally, they have a turn rate constraint, as
they have to maintain a forward velocity. Second, the UAVs must have a high degree of
autonomy in which they perform missions without personnel intervention. In convoy
protection, they must not constrain the convoy from moving in a way it wishes to move.
2
Furthermore, in convoy protection and other surveillance missions it is desired to have
several UAVs in order to promote more complete coverage to identify possible threats.
The missions that this report will focus on are those involving surveillance. In
particular, convoy protection and SAR are of major importance to the US government
with heightened activities overseas and homeland security issues. Supply convoys that
are traveling through hostile territory need over-the-horizon surveillance in order to
watch out for what is ahead of them. For example, in February of 2003 a supply convoy
was ambushed in Iraq and taken hostage [15]. There is a chance, if they would have had
look-ahead surveillance, they would have known that they were driving towards hostiles.
During Coast Guard SAR missions, the most costly part of the mission, in terms of
money and lives, is the time that it takes to find a missing boat or persons in the water
[16]. If the searching could be enhanced by being able to explore a larger extent of land
during a given amount of time, the victims would be found much quicker and the chances
of being found would increase.
The way that UAVs come into play is that they could provide direct surveillance
for the convoy or fly abreast of a SAR helicopter to help perform surveillance. In order
to conduct such a routine, the UAVs must fly along a path that is continuously changing
and still maintain a distance relative to the convoy or helicopter. Furthermore, in order to
increase the effectiveness of the UAVs we may want multiple UAVs to work
collaboratively to perform the task. Therefore, they must be controlled a manner that
defines their path explicitly. The main objectives then become to define a path for the
UAVs to follow and to have them follow that path effectively.
The approaches in this thesis are general in that the point the UAVs would follow
can be anything from a person, to a truck, to a point 500 meters ahead of a convoy. The
general problem is that the UAVs have limitations in airspeed and turn rate capabilities,
while the point of interest (POI) may be able to move at slower speeds and have a tighter
turn rate capability, often being able to change directions from 0° to 360°. The motion of
the POI is unrestricted and not known in advance. Therefore real-time path generation is
needed in order to ensure that the UAVs track the POI.
3
1.2 Organization
To fulfill these demands, algorithms were developed for calculating the desired
path for UAVs. First, a navigation scheme will be presented that commands the UAV to
fly in a sinusoidal manner and by changing the amplitude of the sinusoid, all the while
maintaining a constant velocity and tracking the POI. The navigation scheme has two
modes for different velocity ratios between the UAV and the POI. Second, an enhanced
navigation scheme will be presented that eliminates the need for separate modes while
tracking the POI and utilizes velocity vector commands such that the path can be used for
a formation of UAVs.
After the presentation of these two tracking schemes, low-level control
enhancements to the existing autopilot [5] will be examined. In particular, turbulence
disturbance rejection will be the main focus of the enhancements to the autopilot. In light
of this, H∞ and H2 control will be developed and applied to the disturbance rejection
problem.
4
Chapter 2
Path Planning for a Single UAV
2.1 Introduction
As mentioned in the first chapter, the overall objective is to have UAVs perform a
surveillance routine while tracking a point of interest that is moving unpredictably, but at
a speed less than or equal to the speed of the UAVs. The UAVs are limited in their
forward velocity capabilities, only able to travel between a small range of velocities and
never flying at a standstill.
This chapter will generate a path that is effective for the goal at hand and suitable
for fixed-wing UAVs. The path planning algorithm will then be tested in simulation and
experimentation.
2.2 Path Planning Algorithm
The path planning algorithm that is presented in this chapter is one UAV tracking
a point of interest (POI). Obviously, the central goal of the algorithm will be to
maneuver the UAV to track the POI. It accomplishes this goal by changing modes when
the ratio of the UAV speed, VP, to that of the POI speed, VT, goes above a threshold ratio.
As a result, the behavior of the UAV is separated into two modes (loitering or
sinusoidal) depending on the velocity of the POI. If the velocity of the POI is much
slower than that of the UAV (i.e. velocity ratio is above the threshold), the UAV will be
5
in the loitering mode; otherwise, the UAV will go into the sinusoidal mode. The
sinusoidal curve in Figure 2.1 illustrates the desired path the UAV will follow using the
sinusoidal algorithm. The amplitude of the sinusoid, A, varies according to the ratio of
VP and VT. The POI is situated at the same x value as the UAV. In the figure the dashed
line is the projected path the POI is to follow, it is assumed that the path is straight.
D2 ;
D1=VT*T
A
vT
y1
x1
vP
xP
yP
Figure 2.1: Top view of path planner algorithm in sinusoidal mode
In Figure 2.1, D2 is the distance in the x direction the UAV will travel in one time
period, T, of the sinusoid. It will equal the distance, D1, that the POI will travel in the
same direction. Thus, D1 = VTT, where the period T is arbitrarily chosen. Using this fact,
the equation of a sinusoid can be transformed into an equation that describes the
sinusoidal path,
=
1
2sin
Dx
Ay PP
π. (2.1)
Here, xP and yP are the desired position of the UAV relative to the stationary x1-y1
coordinate system that updates to the position of the POI every period with the heading of
the POI in the x direction. Taking the time derivative yields
PP
P xD
xAy && ⋅
=
1
2cos'
π (2.2)
6
where 12' DAA π= . The magnitude of the UAV velocity, VP, is related to its x and y
components via
222PPP Vyx =+ && . (2.3)
Substituting Equation 2.2 into 2.3 results in
22
1
222 2cos' PP
PP Vx
Dx
Ax =⋅
+ && π
, (2.4)
which after some algebraic manipulation becomes
+
=
1
2 2cos'1
Dx
A
Vx
P
PP
π& . (2.5)
Equation 2.2 and 2.5 are used in the implementation of this algorithm to calculate
the desired path of the UAV. Proceeding further, an equation is now derived that will
relate the ratio of the UAV velocity and the POI velocity with the amplitude. First, note
that dtdxx PP =& , which allows us to express Equation 2.5 as the following integral,
P
D P
P
Tdx
Dx
AV
dt ∫∫
+= 2
01
22
0
2cos'1
1 π. (2.6)
Now, since
TdtT
=∫0 and TVDT 1= (2.7)
Equation 2.6 becomes
P
D P
PT
dxD
xA
VVD
∫
+= 1
01
221 2cos'1
1 π. (2.8)
Let the velocity ratio be, Tp VV /=σ then
P
DP dx
Dx
AD ∫
+=
1
01
22
1
2cos'1
1 πσ . (2.9)
7
This equation is used in the implementation to determine the amplitude, A, of the
sinusoid, based on s . Equation 2.9 is a variation of a complete elliptic integral of the
second kind, which means it can be expressed as
))'((1 2
12AE −π
(2.10)
where E(…) is the aforementioned elliptic integral expressed in function form [1].
Figure 2.2 is a plot of s versus the ratio A/D1 based on Equation 2.9. The plot
shows that using this equation, the amplitude of the sinusoid will increase as the velocity
ratio gets larger. This makes sense because a larger s corresponds to an increasing VP or
a decreasing VT. In both cases the amplitude needs to be enlarged to slow the rate at
which the UAV follows the path of the POI.
Figure 2.2: Speed ratio, s , vs amplitude/distance, A/D1, ratio of path planning algorithm
(all D1 values overlap)
If the value of s is above a certain threshold value, hσ , the UAV will exit the
sinusoidal mode that generates the trajectory as discussed above and enter into the
loitering mode. The UAV will then loiter about the POI. The value of hσ is set to avoid
the large amplitude that would be caused by the high value of s . If the value of s
decreases below hσ , then the sinusoidal algorithm will continue.
8
In the loitering mode, the UAV enters into a circle or rose curve trajectory. The
option for using a circle or rose curve trajectory is up to the user based upon the desired
mission of the UAV. In the circle trajectory, the UAV circles about the POI and
essentially maintains a constant bank angle. The rose curve is beneficial because it will
allow a camera on the bottom of the UAV to face the ground for a greater amount of time
than when circling. The rose curve is created by giving the UAV waypoints in a line and
then after the UAV has gone through those points the line is rotated about a fixed center
located above the POI. Once the line is rotated, waypoints are given along the new line.
This pattern continues, until s decreases below hσ . A close-up for the rose curve is
shown in Figure 2.3.
Figure 2.3: Top view of path planning algorithm in the rose curve loitering mode
The reason the path has been developed in the lateral direction and not in the
longitudinal (up/down) direction is due to many factors. First, and foremost, the amount
of fuel consumed is a function of how much the altitude is increased. In a longitudinal
path the altitude would have to be increased often, thus expending more fuel. Second, the
velocity and altitude control of aircraft is more difficult than the lateral control. This
would not allow us to be as precise as we would like to be. Finally, although constant
surveillance on a straight line may be desirable, often it is more desirable to search a
wider path.
9
2.3 Wind Compensation
The ground velocity of the UAV is used for the path planning algorithm.
However, when wind is present the UAV’s ground velocity changes, while the true air
speed of the UAV is kept constant. Therefore, not only will the UAV have difficulty
following the sinusoidal path, but also the path-generation algorithm will also generate
paths with undesirable features. Fortunately, the UAV has the capability to estimate the
wind velocity, which can be used by the path-generation algorithm. This new path is
offset at a ratio of the wind velocity vector; therefore adding or subtracting to the distance
of the next waypoint for the UAV to go to. Additionally, a hysteresis was added to
eliminate frequent switching between the loitering and sinusoidal modes that is caused by
the fluctuation of the UAV’s ground speed.
2.4 Simulation Implementation and Results
The simulation and experimental platform used for testing these algorithms was
the Piccolo system built by Cloud Cap Technology [5]. The Piccolo is a self-contained
avionics and autopilot system for small aircraft (Figure 2.4). One feature of this flight
system is that it can be used in a hardware-in-the-loop simulation. The term hardware-in-
the-loop (HIL) simulation refers to a simulation in which many of the hardware
components are included in the simulation platform. Unlike a simulation run only on
computers, HIL simulation uses the wireless communication and embedded computers
that are actually used in the actual experiment. Here, the Piccolo avionics may be hooked
up to a computer that simulates the aircraft dynamics and sensors. The Piccolo avionics
communicates with a ground station, shown in Figure 2.5, which sends high-level
commands to the avionics. The radio link between the Piccolo avionics and the ground
station is a 900 MHz frequency hopping radio. The avionics only receives commands
from the ground station every 1 Hz. A single ground station is able to communicate with
multiple Piccolo avionics boxes. For visualization purposes the simulated aircraft and the
ground vehicle simulation are sent to a visualization computer. A general schematic of
the HIL simulation setup is shown in Figure 2.6. The schematic is a general schematic
10
that will be applicable when there are multiple UAVs to be simulated and controlled in a
formation, which will be addressed in the next chapter.
Figure 2.4: Piccolo miniature Figure 2.5: Piccolo ground station
avionics system
Figure 2.6: Hardware-in-the-loop (HIL) simulation schematic with 3 aircraft
Total System Visualization
Ground Vehicle Simulation Trajectory Generation
Formation Control
UAV 1 Sim. UAV 2 Sim. UAV 3 Sim.
Ground Station
Ground Vehicle Data
UAV Data
Piccolo avionics
11
We have successfully simulated the path planning algorithm using a simulation
platform built on the computer connected to the ground station. A ground vehicle
simulation was developed in order to aid in the simulation. Within the graphical user
interface that runs the program, we can define the POI as any offset distance and offset
angle with respect to the ground vehicle, as shown in Figure 2.7.
Figure 2.7: Point of interest (POI) defined in terms of the ground vehicle by an offset
distance, L, and offset angle, θ
The ground vehicle simulation is built using a simple model. The discrete time
equations for the position of the ground vehicle are given by Equations 2.11 and 2.12:
xLat(k+1) = xLat(k)+ vN? T (2.11)
xLong(k+1) = xLong(k) + vE? T (2.12)
where the position of the ground vehicle model is reported in degrees of latitude, xLat, and
longitude, xLong. The velocity magnitude in the north direction is vN and the velocity in the
east direction is vE. Since the algorithm is run once every second, an update of the
ground vehicle model’s position occurs at 1Hz, and so the ? T in Equations 2.11 and 2.12
is set to one.
12
The velocity vector of the ground vehicle is determined through the heading and
the speed. Equations 2.13 and 2.14 calculate the velocity in terms of radius of the earth
and in units of degrees.
πψ 180)cos(
⋅=Lat
TN r
Vv (2.13)
πψ 180)sin(
⋅=Long
TE r
Vv (2.14)
where rLat and rLon are the radius of the earth in latitude and longitude direction,
respectively; VT is the magnitude of the ground vehicle’s velocity vector as in the
preceding section and ? is the heading.
The ground vehicle’s position and velocity are used with the path planning
algorithm in order for a simulated UAV to follow the POI defined with respect to the
simulated ground vehicle before implementation with the actual aircraft and ground
vehicle. The simulated aircraft was previously developed by CloudCap Technology [5].
Simulations were conducted to assist in the development of the path planning
strategies and to confirm that the software would work with an actual aircraft. For the
following simulations, the POI is defined directly on top of the ground vehicle, unless
otherwise noted. The speed of the UAV is held constant at approximately 20-23 m/s
throughout all of the simulations and experiments. The results of a simulation test with
the ground vehicle heading south at a constant 10 m/s are shown in Figure 2.8. The value
of s is approximately 2, which is lower than the threshold value of hs = 3; thus, the path
is a sine wave. There is no wind in this simulation.
The next step in the simulation process is to test the wind compensation
algorithm. The simulated UAV estimates the simulated wind velocity, which is used in
the wind compensation algorithm. The wind is simulated at 10 m/s coming from the
south. First, a simulation was conducted without any wind compensation in the path
planning algorithm. The resulting path is shown in Figure 2.9. The ground vehicle is
traveling at 10 m/s to the north and then turns and heads at 10 m/s to the east. The UAV
has difficulty following the sine wave with tail wind; the UAV goes too far and then has
13
to cut back. When the UAV has a crosswind it stays too far away from the ground
A sample of the resulting orbital trajectories that were chosen for our application
are shown in Figure 3.3. The plots are shown in the POI coordinate frame. VT is varied
while VP is held constant at 20 m/s. It is evident that the amplitude decreases as the speed
of the POI increases. Also, p increases while the VT increases, which is apparent through
the narrowing of the trajectory. In Figures 3.4 and 3.5, the values for p and A are shown
versus all possible values of VT and VP for the different aircraft that may be flown (our
application works with three aircraft, having velocity ranges of 8-13 m/s, 17-24 m/s, and
21-26 m/s).
-500 -400 -300 -200 -100 0 100 200 300 400 500
-300
-200
-100
0
100
200
300
x (m)
y (m )
VT = 0 m/s
VT = 5 m/s
VT = 10 m/s
VT = 15 m/s
VT = 20 m/s
Figure 3.3: Trajectories in the POI coordinate frame with the aircraft velocity held
constant at VP = 20 m/s and the POI velocity, VT, is varied from 0 to 20 m/s
Trajectory Parameter Algorithm (1) Choose A very small (2) Calculate p to minimize the maximum turn rate over the entire orbit
- if p does not meet the maximum turn rate restriction, go to step 1 and increase A - if p meets the turn rate criteria, then make sure we meet the return time restriction
o if we do not meet the return time restrictions, then go to step 1 and increase A
(3) Done
27
Figure 3.4: Values of the trajectory parameter, p, versus the aircraft velocity, VP, and the
POI velocity, VT
Figure 3.5: Values of the trajectory parameter, A, versus the aircraft velocity, VP, and the
POI velocity, VT
28
3.2.3 Orbit Control
The stable control law that is developed for the group of UAVs to follow the
trajectory is defined in terms of the trajectory’s tangent line. Given the position, the unit
tangent vector, tLEM, is calculated in the POI coordinate frame as
θ
θ
dd
dd
LEM
LEM
LEM r
r
t = (3.19)
where the x and y components of the derivative of the position vector are given by
Equations 3.20 and 3.21
( )
−−=
−
θθθθθ
θs)(c
2c)(s
2)c()( 2/1
LEM pp
pppA
dd xr
(3.20)
( )
−−=
−
θθθθθ
θc)(c
2s)(s
2)c()( 2/1
LEM pp
pppA
d
d yr (3.21)
The two components can be collected and then divided by the magnitude to result in
( )
−−−−
+==
−
θθθθθθθθ
θθθ
θ
θ
θc)(cs)(ss)(cc)(s
)(c)(s)(t
)c(2
2
4
2/121
LEM
LEM
LEM 2 pppp
ppp
pp
dd
dd
p
p
pr
r
t . (3.22)
The control laws are defined using a vector between the aircraft’s position in the
POI coordinate frame, BPP )(: rr = and the position on the trajectory that is normal to this
rP. The normal position is found by setting the dot product of the aircraft’s position and
the tangent vector equal to zero. The trajectory will often have several positions that are
normal to the aircraft. The method of choosing which position to use is chosen by a
routine that predicts which quadrant in the POI coordinate frame that the aircraft should
be in or be heading towards, combined with choosing the position closest to the origin if
two solutions are found in the predicted quadrant. This position is called pL and the
normal vector between rP and pL is n as shown in Figure 3.6.
29
Figure 3.6: Control law development for a single UAV following an orbit trajectory
Once pL is found, the tangent vector at pL is determined, tLEM(pL), and its angle is
calculated, ζ. Also, the control angle,θc, is computed using
)(tan 1 ntc K−=θ (3.23)
where Kt is a controller gain. Kt can also be intuitively looked upon as the inverse of the
distance ahead of the pL that you would like to have the UAV head towards, L. Based on
simulations, L was chosen to be 125m, or Kt = 0.008. One reason why we chose such a
large L is due to the delay that we experience in the controller. We only receive position
data from the aircraft every 1 second.
The formation controller is given the desired velocity vector of the UAV in a
global coordinate frame. The desired velocity vector, assuming constant speed of the
UAV is
++
=)sin()cos(
:ζθζθ
c
cPd Vv (3.24)
pL
L = Kt -1
rP
θc ζ
n
30
3.3 Formation Control
The formation controller that is used was developed by Spry [12], and is a general
approach to multiple vehicle formations. The formation controller allows for control in
terms of relative and absolute motion, so it fits in perfectly with out application. The
formation of UAVs can be specified to be at a desired location with a specific speed
(absolute motion), and the individual UAVs can hold a given formation with desired
distances between them (relative motion). The controller also has inherent collision
avoidance, as by maintaining the formation it keeps a specified distance between the
UAVs. Finally, the orbit trajectory control developed above is able to command the
desired velocity vector as in Equation 3.24.
The formation is described in terms of three coordinates. The first coordinate is a
location (L) of a formation reference point (FRP). The second coordinate defines the
orientation (O) of the formation reference frame (FRF), which is specified given a
particular formation. The third coordinate defines the particular formation shape (S)
configuration relative to the FRF. The FRP is a point that remains rigidly attached to the
formation during any rigid displacement. The FRP is what is used in to follow the orbit
trajectory. Usually, two aircraft (or virtual aircraft) define the FRP and the FRF (not
necessarily the same aircraft). For our application, the FRF is specified to align the
UAVs in a straight line perpendicular to the heading of the POI. This is for maximizing
the coverage area, and could be changed for other goals that one could have for the
formation of UAVs. The concept of the orbit working in conjunction with the formation
controller is shown in Figure 3.7.
Figure 3.7: Illustration of orbit trajectory and the formation
POI coordinate frame, origin fixed to POI
Orbit shape and amplitude vary with
POI speed
FRP tracks orbit
31
3.4 Simulation Results
The orbital trajectory and formation controller were implemented on a laptop that
was connected to the ground station. The ground vehicle simulation was included as in
section 2.4. Simulations were conducted to assure that the algorithms would work
successfully once they are employed on the actual aircraft.
Figure 3.8 features a test with three UAVs following a ground vehicle. The
ground vehicle started off at a standstill and the velocity was slowly increased to the
speed of the UAV (20 m/s). The ground vehicle headed straight north during the
simulation. Notice that the UAVs orbited the ground vehicle in a figure-8 pattern and
then as the speed increased the orbit turned into a periodic wave that eventually emulated
a sine wave. Finally, as the ground vehicle’s speed reached that of the UAVs, they flew
in a straight path. In addition, the amplitude of the trajectory slowly decreased
throughout the simulation.
-6000 -4000 -2000 0 2000 4000 6000
0
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
x (m)
y (m )
UAV 0UAV 1UAV 2Grouind Vehicle
Figure 3.8: Simulation with 3 UAVs tracking a ground vehicle. The ground vehicle’s
velocity, VT, increases from 0 to 20 m/s
32
Another simulation is shown in Figure 3.9, which has the ground vehicle start out
at a standstill at position (0,0) and then increase its velocity to 12 m/s. The ground
vehicle then makes several right-hand turns followed by a left-hand turn. Notice, that
every time the ground vehicle makes a turn, the UAVs slowly realign themselves so that
they are perpendicular to the ground vehicle’s heading.
-500 0 500 1000 1500 2000 2500 3000 3500 4000
-2000
-1500
-1000
-500
0
500
1000
1500
x (m)
y (m )
UAV 0UAV 1UAV 2Ground Vehicle
Figure 3.9: Simulation with 3 UAVs tracking a ground vehicle. The ground vehicle’s
velocity, VT, is 12 m/s as it turns in many directions
3.5 Experimental Results
An experiment was conducted using the same system as in section 2.5. The
UAVs that were used in the experiment were Rascal UAVs (see Figure 3.10), which
replaced the simulated UAVs that were used in the HIL simulation in the previous
section. Two UAVs were flown in formation following a truck driving along the road.
The truck drove out of a parking lot onto a road. The truck then went north for about 3
km at 10 m/s and turned around and heading back down the road heading south.
33
Throughout the experiment the two UAVs followed the truck using the orbit trajectory
and formation control. A picture of the truck and UAV paths is shown in Figure 3.11.
Figure 3.10: Picture of Rascal experimental UAV platform
Figure 3.11: Top view of experiment with UAV and ground vehicle paths overlaid on
map of testing site
UAV 1 UAV 2 Truck
(Approx. 4km x 4km)
Truck: 10 m/s Aircraft: 20 m/s
Approx. Speeds:
34
Figure 3.12: Ground coverage plot from the UAV cameras
The camera coverage of the UAVs was also examined and is shown in Figure
3.12. The data is from the truck heading north. One of the goals of the experiment was
to perform surveillance around the truck’s path. Here, it shows that there is almost
complete coverage of the area around the road. Using more UAVs would allow for a
wider coverage area. Also, note that if the UAVs would have been flown in a formation
such that they were lined parallel to the direction of the heading of the truck there would
be a different coverage shape. The formation would allow for coverage in front of and
behind the POI.
Formation flight with three UAVs has been conducted by our group. Due to
hardware difficulties, three aircraft were never flown using the algorithm presented in
this chapter. A picture of the three UAVs is shown in Figure 3.13.
35
Figure 3.13: Picture of three UAVs flying in formation
36
Chapter 4
Disturbance Rejection Control
4.1 Introduction
Now that we have defined a subset of paths that a UAV could be commanded to
follow, we need to make sure that the lateral control of the UAVs is sufficient to keep the
UAVs following the paths. Due to the turbulence that is faced in many flying conditions
for fixed-wing aircraft UAVs, there is a need to implement a disturbance rejection
controller with the current autopilot system. The turbulence perturbs the UAV off of the
desired path, and prevents closer formation flight and optimal usage of cameras mounted
on the bottom of the UAVs.
Disturbance rejection has been a topic of control basically since its inception.
This thesis is not meant as an exhaustive search of disturbance rejection controllers,
although it is nice to mention a few for the reader who is interested in developing
controllers of their own. Bode had many groundbreaking results in the 1940s [3], and
advances have been made ever since. Many SISO disturbance rejection controllers were
developed in the 1960s and 70s, in particular, LQG and the Weiner-Hopf control
methodology [19]. In the 1980s, H8 and H2 control became very popular with the results
of Doyle and many others [6, 7, 11].
There are many limitations in making a good disturbance rejection controller.
First, sensors that are used for control (gyros, accelerometers, etc.) often have high
frequency noise (and low frequency bias) that inhibits their ability to measure high
37
frequency disturbances. Due to the structure of feedback systems, in order to reject
disturbances at a given frequency, we are not allowed to reject sensor noise at that
frequency. This concept will be addressed later in this chapter. Additionally, we have
limitations in our actuators such as bandwidth and saturation that limit the ability to reject
some disturbances. On top of all of these limitations, we must be able to follow our
command input to achieve good tracking. The goal of the work in this chapter will be to
restrict the effect of turbulence while still achieve good tracking of a desired turn rate
that, in turn, yields good tracking of a curve or line.
This chapter is laid out by opening up with a brief rundown on aircraft dynamics.
Subsequently, an explanation of the simulation environment that will be used for the
controller development will be given. After explaining the simulation environment, an
overview will be given of the current lateral autopilot that is implemented in the avionics
system. Then, simulation results for the current autopilot will be shown. Next, small
enhancements to the autopilot assisting in disturbance rejection will be discussed and
results will be shown. Following discussion of these small enhancements, more drastic
changes such as H2 and H8 control will be the talked about: first, in theory; second, in
implementation; and third, in simulation and results. During the section on H8 control,
actuator models, the wind model, and sensor models will be explained in detail.
4.2 Overview of Aircraft Dynamics
This section is not meant to give a complete explanation of aircraft dynamics, but
rather is a primer intended to enable the reader to be able to understand the nomenclature
and general dynamics of an aircraft. Most of the information in this chapter comes from
textbooks by Etkin [9] and Bryson [4].
The standard dynamical model of an aircraft is composed of twelve states:
`]`,`,,,,,,,,,,[ zyxrqpwvu ψθφ where
),,( framebody in elocity aircraft v],,[ bbb zyxwvu = (see Figure 4.1)
),,( framebody in locity angular veaircraft ],,[ bbb zyxrqp =
38
=],,[ ψθφ Euler angles of a 3-2-1 transformation between body frame and earth-
fixed frame = roll, pitch, yaw
=`]`,`,[ zyx position of the aircraft with respect to the earth-fixed frame
The nonlinear dynamical equations for an aircraft are [4]:
( )( )( ) εφθ
φθεθ
sTccgmZuqvpwmscgmYwpurvm
cTsgmXvrwqum
⋅−⋅⋅⋅+=⋅−⋅+⋅⋅⋅⋅+=⋅−⋅+⋅
⋅+⋅⋅−=⋅−⋅+⋅
&&&
(4.1)
( )( ) ( )
( ) NrqIqpIIpIrI
MprIrpIIqI
LqpIrqIIrIpI
xzxyxzz
xzzxy
xzyzxzx
=⋅⋅−⋅⋅−+⋅+⋅
=−⋅+⋅⋅−+⋅
=⋅⋅+⋅⋅−+⋅+⋅
&&
&
&&22 (4.2)
rcc
qcs
rsqc
rtcqtsp
⋅+⋅=
⋅−⋅=
⋅⋅+⋅⋅+=
θφ
θφ
ψ
φφθ
θφθφφ
&
&
&
(4.3)
u,X
w,Z
v,Y
r,N
p,L
q,M
zb
xb
yb
Figure 4.1: Aircraft velocities, forces, and moments in body-axes (courtesy of Eric Frew)
39
wccvcsusz
wcssscvccsssuscywsscscvsccssuccx
⋅⋅+⋅⋅+⋅−=⋅⋅−⋅⋅+⋅⋅+⋅⋅+⋅⋅=⋅⋅+⋅⋅+⋅⋅−⋅⋅+⋅⋅=
θφθφθψφψθφψφψθφψθψφψθφψφψθφψθ
`)()(`)()(`
&&&
(4.4)
where m = mass of aircraft, Iii are the moments of inertia, =],,[ ZYX aerodynamic forces
on the aircraft in body axes coordinates, and =],,[ NML aerodynamic moments on the
aircraft in body axes coordinates, T is the thrust, and ε is the angle of thrust with the xb-
axis.
The aerodynamic moments and forces are functions of all of the states and all of
the control inputs, for instance ( )trearqpwvuXX δδδδ ,,,,,,,,,= , where =aδ aileron
deflection, =eδ elevator deflection, =rδ rudder deflection, and =tδ throttle deflection.
The aircraft models are sometimes derived in terms of the aircraft velocity, V, and
its associated polar angles: the angle of attack, α, and the angle of sideslip, β. The
relationships between the above states and these alternative states are:
222 wvuV ++= (4.5)
( )221tan vuw += −α (4.6)
( )uv1tan−=β (4.7)
If the aircraft is assumed to be close to an equilibrium, or trim, condition then the
equations of motion can be linearized and the states can be broken into their lateral and
longitudinal states. The lateral states then become v, p, r, φ, ψ, and y` and the control
inputs are aδ and rδ . The longitudinal states are u, w, q, θ, x`, z` and the control inputs
are eδ and tδ . The longitudinal states will be assumed to be relatively constant and
maintained by the current autopilot. The lateral states are the path planning states with
the aileron and rudder as the controls.
Throughout this section the aircraft that will be used is the Aerosonde UAV [14].
The linear model given that the aircraft is flying at h = altitude = 490 meters, V = aircraft