Autonomous Fixed Wing Air Vehicles

Transcription

1 Guidance and Control of Autonomous Fixed Wing Air Vehicles Randal W. Beard Timothy W. McLain Brigham Young University

2 ii

3 Contents Preface vii 1 Introduction System Architecture Design Project Coordinate Frames Rotation Matrices MAV Coordinate Frames Equation of Coriolis Design Project Kinematics and Dynamics MAV State Variables MAV Kinematics Rigid Body Dynamics Design Project Forces and Moments Gravitational Forces Aerodynamic Forces Propulsion Forces Summary Design Project Nonlinear Equations of Motion Six DOF Equations of Motion Navigation Models iii

4 iv CONTENTS 5.3 Wind Models Design Project Trim Turn with a Constant Climb Rate Design Project Open-loop Linear Dynamics Transfer Functions Linear State Space Models Design Project Autopilot Design Using Successive Loop Closure Lateral Autopilot Longitudinal Autopilot Design Project Micro UAV Sensors Rate Gyros Accelerometers Pressure Sensors Magnetometers GPS Design Project State Estimation Low Pass Filters State Estimation by Inverting the Sensor Model Dynamic Observer Theory Essentials from Probability Theory Derivation of the Kalman Filter Attitude Estimation GPS Smooting Wind Estimation Design Project

5 CONTENTS v 11 Waypoint and Orbit Following Problem Description Experimental Results Conclusion Another approach to straight line tracking Design Project Path Planning Point-to-Point Algorithms Coverage Algorithms Design Project Path Manager Switching Between Waypoints Smooth transitions that satisfy kinematic constraints Smooth transitions through the waypoint Smooth transitions that preserve path length Path Smoothing Dubin s Paths Design Project Cameras on Micro UAVs Gimbal and Camera Frames and Projective Geometry Gimbal Pointing Geolocation Estimating Target Motion in the Image Plane Precision Landing Design Project A Aviones 221 B Introduction to Modeling in Simulink 223 C Useful Formulas and other Information 225 C.1 Conversion from knots to mph D Graphs Theory 227

6 vi CONTENTS Bibliography 236

7 Preface Unmanned air vehicles (UAVs) are playing increasingly prominent roles in defense programs and strategy around the world. Technology advancements have enabled the development of large UAVs (e.g., Global Hawk, Predator) and the creation of smaller, increasingly capable UAVs. As recent conflicts have demonstrated, there are numerous military applications for UAVs including reconnaissance, surveillance, battle damage assessment, and communications relays. Civil and commercial applications are not as well developed, although potential applications are extremely broad in scope. Possible applications for UAV technology include environmental monitoring (e.g., pollution, weather, and scientific applications), forest fire monitoring, homeland security, border patrol, drug interdiction, aerial surveillance and mapping, traffic monitoring, precision agriculture, disaster relief, ad-hoc communications networks, and rural search and rescue. For many of these applications to develop to maturity, the reliability of UAVs will need to increase, their capabilities will need to be extended further, their ease of use will need to be improved, and their cost will have to come down. In addition to these technical and economic challenges, the regulatory challenge of integrating UAVs into the national and international air space needs to be overcome. UAVs can generally be divided into two categories: fixed-wing aircraft, and hovercraft. Both types of aircraft have distinctive characteristics that make autonomous behavior difficult to design. In this book we focus exclusively on fixed-wing aircraft which can be roughly categorized by size. In this book we will use the term small UAV (SUAV) to refer to a fixed wing aircraft with wing span between 4-10 feet. Small UAVs are usually gas powered and typically require a runway for takeoff and landing, although the Boing ScanEagle, which uses a catapult for take-off and a skyhook for recovery, is a notable exception. SUAVs are typically designed to operate on the order of hours, with payload is on the order of pounds. The term miniature air vehicle (MAV) will be used to refer to fixed-wing aircraft with wingspan between 1-5 feet. Miniature UAVs are typically battery powered and hand launched and belly landed. They do not require a runway for take-off or landing. They are designed to operate for times that range from 20 minutes to a couple of hours. Payloads range from several ounces to vii

8 viii 2 pounds. The small payload severely restricts the sensor suite that can be placed on MAVs. It also restricts the computer that can be put on-board. These restrictions create interesting challenges for the design of autonomous modes of operation. While many of the concepts described in this book are also applicable to larger UAVs, the primary focus of the book is on the challenges that are inherent with limited-payload SUAVs and MAVs, and in particular, with MAVs. This textbook was inspired by our desire to teach a course to our graduate students that prepared them to do work in the area of cooperative control for UAVs. Most of our students come from a background in Electrical and Computer Engineering, Mechanical Engineering, and Computer Science. Very few of them have had courses in aerodynamics, and the ECE and CS students have not generally had courses in kinematics, dynamics, or fluid dynamics. However, most of our students have had courses in signals and systems, feedback control, robotics, and computer vision. There are a large number of textbooks that cover aircraft dynamics and control, however most of them assume a background in aeronautics and assume that the student has not had exposure to feedback control. Therefore, textbooks like [1, 2, 3, 4, 5, 6] present the discussion on aerodynamic forces without discussing basic ideas in fluid mechanics and aeronautics. On the other hand, they spend a great deal of material explaining introductory feedback control concepts. The textbook [7] is much more in line with what we wanted to teach our students, but the focus of that text is on stability augmentation systems as opposed to autonomous operations. Autonomous operations require more than a simple autopilot. They also require autonomous take-off and landing, path planning, and path following operations, integrated with higher level decision making processes. To our knowledge, another textbook covering aircraft dynamic models, low level autopilot design, state estimation, and high-level path planning is not available. Our hope is that this book fits that gap. Our target audience is Electrical and Computer Engineering, Mechanical Engineering, and Computer Science students who have had a course in introductory feedback control or robotics. We hope that the textbook is also interesting to Aeronautical Engineers who would like a background in autonomous systems. In writing this book, we have been influenced by the exceptional textbook by Rugh [8]. In the spirit of [8], our objective was to have 14 chapters, each of which could be covered in three, one-hour lectures. While some of the chapters are longer than we originally hoped, we feel that the book can be covered in a one semester course. The additional material will allow the instructor some flexibility in the topics that are covered. One of the unique features of the book is the associated design project. As we have taught this course, we have evolved from assigning pencil and paper homework assignments, to computer simulation assignments. We have found that students are more engaged in the material and tend to understand it better when they implement the concepts in a computer simulation.

9 Preface ix When we teach the course, we have our students develop a complete end-to-end flight simulator and includes realistic flight dynamics, sensor models, autopilot design, and path planning and tracking. By the end of the course, they have implemented each piece of the puzzle and therefore understand how each piece fits together. In addition, they understand the inner workings of a fairly sophisticated flight simulation package that can be used in future research. Unfortunately, developing an end-to-end simulator requires the use of a compiled language like C/C++. For our course at BYU, knowledge of C/C++ is a stated prerequisite. However we understand that this requirement would not be welcome by many instructors who would otherwise want to use the textbook. While Matlab/Simulink cannot be used effectively to develop an end-to-end simulator, it can be used to implement the pieces separately with increasing levels of abstraction as the book progresses. Accordingly, we have written the project exercises so that they can be done in either C/C++ or Matlab/Simulink. The accompanying CD-ROM contains the appropriate files for both methods. The ideal approach may be to work the project in Matlab/Simulink, and then port the solution to C/C++. The Appendices describe both simulation packages in detail.

10 x

11 Chapter 1 Introduction 1.1 System Architecture The objective of this book is to prepare the reader to do research in the exciting, and rapidly developing, field of autonomous navigation, guidance, and control of miniature air vehicles (MAVs). The focus is on the design of the software algorithms required for autonomous and semi-autonomous flight. To work in this area, researchers need to be familiar with a wide array of topics that include coordinate transformations, aerodynamics, autopilot design, state estimation, path planning, and computer vision. The aim of this book is to cover these essential topics, focusing in particular on their application to MAVs. In the development of the topics, we have in mind the software architecture shown in Figure 1.1. The block labeled Miniature Air Vehicle in Figure 1.1 is the six degree-of-freedom physical airframe that responds to servo command inputs (elevator, aileron, rudder, and throttle) and wind and other disturbances. The mathematical models that are required to understand fixed-wing flight are complicated and are covered in Chapters 2 7. In particular, in Chapter 2 we discuss coordinate frames and transformations between frames. A study of coordinate frames is required since most specifications for MAV are given in the inertial frame (e.g., orbit a specific coordinate), whereas most of the sensors measure information with respect to the body frame, and the actuators exert forces and torques in the body frame. In Chapter 3 we describe basic rigid body kinematics and dynamics. The mathematical models developed in Chapter 3 are applicable to any physical object. In Chapter 4 we describe the aerodynamic forces and moments that act on fixed-wing aircraft. Chapter 5 completes the development of the six DOF model by combining the results of Chapters 3 and 4. The six DOF model is complicated and cumbersome to work with, and the coefficients of 1

12 2 1.1 System Architecture Destination, obstacles List of waypoints Waypoint path planner Path manager Status Servo commands Wind Vector field path following Autopilot Micro air vehicle Tracking error Position error from camera State estimator On-board sensors Figure 1.1: The system architecture that will be assumed throughout the textbook. The waypoint path planner produces straight line paths through obstacle fields. The path manager switches between orbit following and straight line path following to maneuver along the waypoint paths. The vector field path following block produces commands to the low level autopilot, which controls the airframe. Each of the blocks relies on estimates of the states produced by filtering the on-board sensors. the model are often not explicitly known. In addition, the level of fidelity offered by the six DOF model is often not needed to design higher level path planning and trajectory following algorithms. In fact, the requirement for computational efficiency precludes the use of the full six DOF model. Therefore, Chapter 5 also describes several nonlinear lower fidelity models that can be used to effectively design higher levels of behavior. Linear models that describe small deviations from trim can also be derived. In Chapter 6 we describe trim conditions, and how they can be computed numerically from the six DOF model. Chapter 7 derives the linear models in both transfer function and state space forms. The block labeled Autopilot in Figure 1.1 refers to the low-level control algorithms that maintain roll and pitch angles, airspeed, altitude, and course heading. Two different autopilot design techniques are discussed in this book. Chapter 8 uses the standard technique of successive loop closure to design the autopilots. Nested control loops are closed one at a time with inner loops maintaining roll and pitch angles, and outer loops maintaining airspeed, altitude, and course. The autopilot and the higher level blocks rely on accurate state estimates which are obtained by dynamically filtering the on-board sensors which include accelerometers, rate gyros, pressure

13 Introduction 3 sensors, magnetometers, and GPS. A mathematical model of the sensors is given in Chapter 9. A description of several state estimation techniques that are effective for MAVs is described in Chapter 10. One of the primary challenges with MAVs is flight in wind conditions. Since airspeeds in the range of mph are typical for MAVs, and since wind speeds at several hundred feet above ground level (AGL) almost always exceed 10 mph, MAVs must be able to maneuver effectively in wind. Airframes are designed to fly efficiently at a particular airspeed. Therefore, one of the control objectives will typically be to maintain a constant airspeed. Therefore, for a constant airspeed, head winds will decrease the ground speed, and tail winds will increase the ground speed. Since the ground speed is constantly changing, we have found that traditional trajectory tracking methods used in robotics do not work well for MAVs. The primary difficulty is the time dependance of the trajectory which does not take into account the variations in ground speed. On the other hand, path following methods that simply maintain the vehicle on a desired path have proven to be very effective in flight tests. In Chapter 11 we describe a vector field path following method which is labeled Vector field path following in Figure 1.1. We will focus exclusively on straight-line paths and circular orbits. The block labeled Path manager in Figure 1.1 is a finite state machine that converts straightline waypoint paths into a sequence of straight-line paths and circular orbits, making it possible to significantly simplify the path planning problem at the highest level. The Waypoint path planner produces a sequence of waypoints that maneuver the MAV through obstacle fields. Chapter 12 describes the Waypoint path planner, while Chapter 13 describes the Path manager. For path planning we consider two classes of problems. The first class of problems is point-to-point algorithms where the objective is to maneuver from a start position to an end position while avoiding a set of obstacles. The second class of problems is search algorithms where the objective is to cover a region with a sensor footprint, with a set of potential no-go regions. Almost all applications involving MAVs require the use of an on-board electro-optical (E/O) video camera. The objective of the camera is to provide visual information to the end user. However, since payload is limited, it makes sense to also use the video camera for navigation, guidance, and control. Effective use of camera information is currently an active research area. In Chapter 14 we discuss one potential use of video cameras on MAV: namely geo-location of ground based objects. The geo-location problem is to use a sequence of images as well as the on-board sensors to estimate the world coordinates of objects on the ground. We feel that this problem provides a nice launch point for further investigations in vision based control of MAVs. In Chapter 14 we use the software architecture shown in Figure 1.2, where the Waypoint path planner has been replaced with the block Vision based guidance. However, the vision based guid-

14 4 1.2 Design Project ance laws interact with the architecture in the same manner as the waypoint path planner. Modularity of the architecture is one of its most appealing features. Targets to track/avoid Orbit / waypoints Vision based guidance law Path manager Status Servo commands Wind Vector field path following Autopilot Micro air vehicle Tracking error Position error from camera State estimator On-board sensors Camera Figure 1.2: System architecture for vision based navigation, guidance, and control. A video camera is added as an additional sensor and the waypoint path planner has been replaced with vision based guidance block 1.2 Design Project In this textbook we have decided to replace traditional pencil and paper homework problems with a complete, and rather extensive design project. The design project is an integral part of the book and we believe that it will play a significant role in helping you, the reader, to internalize the material that is presented. The design project involves building a MAV flight simulator from the ground up. Two approaches are suggested: 1) Using the public domain flight simulator Aviones available for download on Sourceforge.net, or 2) Using Matlab/Simulink. At the conclusion of each chapter, assignments are given that will direct you in the completion of the various components of the simulator.

15 Chapter 2 Coordinate Frames This chapter describes the various coordinate systems that are used to describe the position and orientation of the aircraft, and the transformation between these coordinate systems. It is necessary to use several different coordinate systems for the following reasons: Newton s equations of motion are derived relative to a fixed, inertial reference frame. However, motions are most easily described in a body-fixed frame. Aerodynamics forces and torques act on the aircraft body and are most easily described in a body-fixed reference frame. On-board sensors like accelerometers and rate gyros measure information with respect to the body frame. Alternatively, GPS measures position, ground speed, and course angle with respect to the inertial frame. Most mission requirements like loiter points and flight trajectories, are specified in the inertial frame. In addition, map information is also given in an inertial frame. One coordinate frame is transformed into another through two basic operations: rotations and translations. Section 2.1 develops describes rotation matrices and their use in transforming between coordinate frames. Section 2.2 describes the specific coordinate frames used for micro air vehicle systems. In Section 2.3 we derive the Coriolis formula which is the basis for transformations between between between translating and rotating frames. 2.1 Rotation Matrices We begin by considering the two coordinate frames shown in Figure 2.1. The vector p can 5

16 6 2.1 Rotation Matrices Figure 2.1: Rotation in 2D be expressed in both the F 0 frame (specified by (î 0, ĵ 0, ˆk 0 )) and in the F 1 frame (specified by (î 1, ĵ 1, ˆk 1 )). In the F 0 frame we have p = p 0 xî0 + p 0 yĵ0 + p 0 zˆk 0. Alternatively in the F 1 frame we have p = p 1 xî1 + p 1 yĵ1 + p 1 zˆk 1. Setting these two expressions equal to each other gives p 1 xî1 + p 1 yĵ1 + p 1 zˆk 1 = p 0 xî0 + p 0 yĵ0 + p 0 zˆk 0. Taking the dot product of both sides with î 1, ĵ 1, and ˆk 1 respectively, and stacking the result into matrix form gives p 1 = p 1 x p 1 y p 1 z î 1 î 0 î 1 ĵ 0 î 1 ˆk 0 = ĵ 1 î 0 ĵ 1 ĵ 0 ĵ 1 ˆk 0 ˆk 1 î 0 ˆk1 ĵ 0 ˆk1 ˆk 0 From the geometry of Figure 2.1 we get p 1 = R0p 1 0, (2.1) p 0 x p 0 y p 0 z. where R 1 0 = cos(θ) sin(θ) 0 sin(θ) cos(θ)

17 Coordinate Frames 7 The notation R 1 0 is used to denote a rotation matrix from coordinate frame F 0 to coordinate frame F 1. gives Proceeding in a similar way, a right-handed rotation of the coordinate system about the y-axis R 1 0 = cos(θ) 0 sin(θ) 0 1 0, sin(θ) 0 cos(θ) and a right-handed rotation of the coordinate system about the x-axis resultes in R 1 0 = cos(θ) sin(θ). 0 sin(θ) cos(θ) As pointed out in [7], the negative sign on the sin term appears above the line with only ones and zeros. The matrix R 1 0 in the above equations are examples of a more general class of rotation matrices that have the following properties: P.1. (R b a) 1 = (R b a) T = R a b. P.2. R c b Rb a = R c a. P.3. det R b a = 1. In the derivation of Equation (2.1) note that the vector p remains constant and the new coordinate frame F 1 was obtained by rotating F 0 through a righted-handed rotation of angle θ. We will now derive a formula, called the rotation formula that performs a left-handed rotation of a vector p about another vector ˆn by an angle of µ. Our derivation follows that given in [7]. Consider Figure 2.2 which is similar to Figure in [7]. The vector p is rotated, in a left-handed sense, about a unit vector ˆn by an angle of µ to produce the vector q. The angle between p and ˆn is φ. By geometry we have that The vector ˆn: q = ON + NW + W Q. (2.2) ON can be found by taking the projection of p on the unit vector ˆn in the direction of ON = (p ˆn) ˆn.

18 8 2.1 Rotation Matrices Figure 2.2: Left-handed rotation of a vector p about the unit vector ˆn by an angle of µ to obtain the vector q. The vector NW is in the direction of p ON with a length of NQ cos µ. Noting that the length NQ equals the length NP which is equal to p ON we get that The vector p sin φ we get NW = p (p ˆn) ˆn NQ cos µ p (p ˆn) ˆn = (p (p ˆn) ˆn) cos µ. W Q is perpendicular to both p and ˆn and has length NQ sin µ. Noting that NQ = Therefore Equation (2.2) becomes which is called the rotation formula. W Q = p ˆn NQ sin µ p sin φ = ˆn p sin µ. q = (1 cos µ) (p ˆn) ˆn + cos µp sin µ (ˆn p), (2.3) As an example of the application of Equation (2.3) consider a left handed rotation of a vector p 0 in frame F 0 about the z-axis as shown in Figure 2.3.

19 Coordinate Frames 9 Figure 2.3: Rotation of p about the z-axis. Using the rotation formula we get q 0 = (1 cos θ)(p ˆn)ˆn + cos θp sin θˆn p 0 p 0 x = (1 cos θ)p 0 z 0 + cos θ p 0 y sin θ 1 p 0 z cos θ sin θ 0 = sin θ cos θ 0 p = R 1 0p 0. p 0 y p 0 x 0 Note that the rotation matrix R0 1 can be interpreted in two different ways. The first interpretation is that it transforms the fixed vector p from an expression in frame F 0 to an expression in frame F 1 where F 1 has been obtained from F 0 by a right-handed rotation. The second interpretation is that it rotates a vector p though a left-handed rotation to a new vector q in the same reference frame. Right-handed rotations of vectors are obtained by using (R0) 1 T.

20 MAV Coordinate Frames 2.2 MAV Coordinate Frames For MAVs there are several coordinate systems that are of interest. In this section, we will define and describe the following coordinate frames: the inertial frame, the vehicle frame, the vehicle-1 frame, the vehicle-2 frame, the body frame, the stability frame, and the wind frame. Throughout the book we assume a flat, non-rotating earth: a valid assumption for MAVs The inertial frame F i. The inertial coordinate system is an earth-fixed coordinate system with origin at the defined home location. As shown in Figure 2.4, the unit vector î i is directed North, ĵ i is directed East, and ˆk i is directed toward the center of the earth. Figure 2.4: The inertial coordinate frame. The x-axis points North, the y-axis points East, and the z-axis points into the Earth The vehicle frame F v. The origin of the vehicle frame is at the center of mass of the MAV. However, the axes of F v are aligned with the axis of the inertial frame F i. In other words, the unit vector î v points North, ĵ v points East, and ˆk v points toward the center of the earth, as shown in Figure The vehicle-1 frame F v1. The origin of the vehicle-1 frame is identical to the vehicle frame, i.e, the the center of mass. However, F v1 is positively rotated about ˆk v by the yaw angle ψ so that if the airframe is not rolling or pitching, then î v1 would point out the nose of the airframe, ĵ v1 points out the right wing, and is aligned with ˆk v and points into the earth. The vehicle-1 frame is shown in Figure 2.6. ˆk v1

21 Coordinate Frames 11 Figure 2.5: The vehicle coordinate frame. The x-axis points North, the y-axis points East, and the z-axis points into the Earth. The transformation from F v to F v1 is given by p v1 = R v1 v (ψ)p v, where R v1 v (ψ) = cos ψ sin ψ 0 sin ψ cos ψ Figure 2.6: The vehicle-1 frame. If the roll and pitch angles are zero, then the x-axis points out the nose of the airframe, the y-axis points out the right wing, and the z-axis points into the Earth.

22 MAV Coordinate Frames The vehicle-2 frame F v2. The origin of the vehicle-2 frame is again the center of mass and is obtained by rotating the vehicle- 1 frame in a right-handed rotation about the ĵ v1 axis by the pitch angle θ. If the roll angle is zero, then î v2 points out the nose of the airframe, ĵ v2 points out the right wing, and ˆk v2 points out the belly, a shown in Figure 2.7. Figure 2.7: The vehicle-2 frame. If the roll angel is zero, then the x-axis points out the nose of the airframe, the y-axis points out the right wing, and the z-axis points out the belly. The transformation from F v1 to F v2 is given by where R v2 v1(θ) = p v2 = R v2 v1(θ)p v1, cos θ 0 sin θ sin θ 0 cos θ The body frame F b. The body frame is obtained by rotating the vehicle-2 frame in a right handed rotation about î v2 by the roll angle φ. Therefore, the origin is the center-of-gravity, î b points out the nose of the airframe, ĵ b points out the right wing, and ˆk b points out the belly. The body frame is shown in Figure 2.8. The transformation from F v2 to F b is given by p b = Rv2(φ)p b v2, where Rv2(φ) b = 0 cos φ sin φ. 0 sin φ cos φ

23 Coordinate Frames 13 Figure 2.8: The body frame. The x-axis points out the nose of the airframe, the y-axis points out the right wing, and the z-axis points out the belly. The transformation from the vehicle frame to the body frame is given by Rv(φ, b θ, ψ) = Rv2(φ)R b v1(θ)r v2 v v1 (ψ) cos θ 0 sin θ cos ψ sin ψ 0 = 0 cos φ sin φ sin ψ cos ψ 0 0 sin φ cos φ sin θ 0 cos θ cθcψ cθsψ sθ = sφsθcψ cφsψ sφsθsψ + cφcψ sφcθ, cφsθcψ + sφsψ cφsθsψ sφcψ cφcθ where cφ = cos φ and sφ = sin φ The stability frame F s. Aerodynamic forces are generated as the airframe moves through the air surrounding it. We refer to the velocity of the airframe relative to the surrounding air as the relative wind or the relative wind vector, denoted V. The magnitude of the relative wind vector is typically referred to as the airspeed, V a. To generate lift, the wings of the airframe must fly at a positive angle with respect to the wind vector. This angle is called the angle-of-attack and is denoted by α. As shown in Figure 2.9, the angle of attack is defined as a left-handed rotation about ĵ b and is such that î s aligns

24 MAV Coordinate Frames with the projection of V, onto the plane spanned by î b and ˆk b. The need for a left handed rotation is caused by the definition of positive angle of attack, which is positive for a right-handed rotation from the stability frame î s direction to the body frame î b direction. Figure 2.9: The stability frame. The x-axis points along the projection of the relative wind vector onto the x z plane of the body frame, the y-axis is identical to the y-axis of the body frame, and the z-axis is constructed to make a right handed coordinate system. Note that angle-of-attack is defined as a left-handed rotation about the body y-axis. Since α is given by a left-handed rotation, the transformation from F b to F s is given by p s = Rb(α)p s b, where cos α 0 sin α Rb(α) s = sin α 0 cos α The wind frame F w. If there is an ambient crosswind, the airframe will need to crab into the wind to maintain the desired course. The crab angle is called the side-slip angle and is denoted by β. As shown in Figure 2.10, the wind frame is obtained by rotating the stability frame by a right-handed rotation of β about ˆk s. The unit vector î w is aligned with the relative wind vector V. The transformation from F s to F w is given by p w = Rs w (β)p s,

25 Coordinate Frames 15 Figure 2.10: The wind frame. The x-axis points along the relative wind vector. where cos β sin β 0 Rs w (β) = sin β cos β The total transformation from the body frame to the wind frame is given by Rb w (α, β) = Rs w (β)rb(α) s cos β sin β 0 cos α 0 sin α = sin β cos β sin α 0 cos α cos β cos α sin β cos β sin α = sin β cos α cos β sin β sin α. sin α 0 cos α Alternatively, we have cos β cos α sin β cos α sin α Rw(α, b β) = (Rb w ) T (α, β) = sin β cos β 0. cos β sin α sin β sin α cos α When developing the dynamic equations of motion for the MAV, it is important to remember that the inertial forces experienced by the MAV are dependent on velocities and accelerations relative to a fixed (inertial) reference frame. The aerodynamic forces, however, are dependent on the velocity of the plane relative to the surrounding air. When there is no wind present, these velocities are the same. Unfortunately, wind is almost always present with MAVs and we must

26 MAV Coordinate Frames carefully distinguish between airspeed, represented by the relative wind vector V, and the ground speed, represented by the velocity with respect to the inertial frame v. These velocities are related by the expression V = v v w. where v w is the wind velocity relative to the inertial frame. The MAV velocity, v, can be expressed in the body frame in terms of components along the î b, ĵ b, and ˆk b axes u v b = v. w Be aware that v b is the velocity of the MAV with respect to the inertial frame, as expressed in the body frame. Similarly, we can write an expression for the wind velocity in the body-frame vw b = Noting that the relative wind vector V (keep in mind that this is the velocity of the MAV relative to the wind) in the wind frame can be written as and that in the body frame it is given by V b = V w = u r v r u w v w w w V a 0 0. u u w = v v w, w r w w w the following expression results u r V a V b = v r = R b w 0 w r 0 cos β cos α sin β cos α sin α V a = sin β cos β sin β sin α 0. cos β sin α 0 cos α 0

27 Coordinate Frames 17 This implies that u r v r w r cos α cos β = V a sin β. (2.4) sin α cos β Inverting this relationship gives V a = u 2 r + vr 2 + wr 2 ( ) α = tan 1 wr u ( r ) β = tan 1 v r. u 2 r + wr 2 (2.5) These expressions for V a, α, and θ are commonly used to calculate the aerodynamic forces and moments acting on the vehicle Heading versus course. For small UAVs, the wind speed is often in the range of 20 to 50 percent of the airspeed. The significant effect of wind on MAVs is important to understand, more so than for larger conventional aircraft where the airspeed is typically much larger than the wind speed. Having introduced the concepts of reference frames, airframe velocity, wind velocity, and the relative wind vector, we can discuss some important definitions relating to the navigation of MAVs. Figure 2.11 shows a MAV in level flight. This assumes that the roll angle, pitch angle, and angle of attack are near zero. The North direction is indicated by the î i vector and the direction that the MAV is pointed is shown by the î v vector, which is fixed in the direction of the body x axis. As discussed earlier, the heading, or yaw angle ψ, is the angle between î i and î v and defines the direction the MAV is pointed. The direction that the MAV is traveling with respect to the surrounding air mass is given by the relative wind vector V. In steady, level flight, V is typically aligned with î v meaning the sideslip angle β is zero. In this case, sideslip is introduced by wind gusts. In non-level flight, sideslip is more common. The direction that the MAV is traveling with respect to the ground is shown by the velocity vector v. The angle between the inertial North and the velocity vector is called the course, χ. The definition of course is the line of flight taken by an aircraft. Similar to the course is the ground track, which is the projection of the line of flight onto the surface of the earth.

28 Equation of Coriolis v w North ^i i ^i v χ V v ψ β ground track Figure 2.11: MAV in level flight (α θ φ 0). Heading is the direction that the MAV is pointed. Course is the direction of travel relative to the earth s surface. In the absence of wind, they are the same. 2.3 Equation of Coriolis In this section, we provide a simple derivation of the famous equation of Coriolis. We will again follow the derivation given in [7]. Suppose that we are given two coordinate frames F i and F b as shown in Figure For example, F i might represent the inertial frame and F b might represent the body frame of a MAV. Suppose that the vector p is moving in F b and that F b is rotating with respect to F i. Our objective is to find the time derivative of p as seen from frame F i. We will derive the appropriate equation through two steps. Assume first that F b is not rotating with respect to F i. Denoting the time derivative of p in frame F i as d dt i p we get d p = d p. (2.6) dt i dt b On the other hand, assume that p is fixed in F b but that F b is rotating with respect to F i, and let ŝ be the instantaneous axis of rotation and δφ the (right-handed) rotation angle. Then the rotation formula (2.3) gives p + δp = (1 cos( δφ))ŝ(ŝ p) + cos( δφ)p sin( δφ)ŝ p.

29 Coordinate Frames 19 Figure 2.12: Derivation of the equation of Coriolis. Using a small angle approximation and dividing both sides by δt gives δp δt δφ δt ŝ p. Taking the limit as δt 0 and defining the angular velocity of F b with respect to F i as ω b/i = ŝ φ we get d p = ω b/i p. (2.7) dt i Since differentiation is a linear operator we can combine Equations (2.6) and (2.7) to obtain which is the equation of Coriolis. d dt i p = d dt b p + ω b/i p, (2.8) 2.4 Design Project 2.1 Download the Aviones program (aviones.zip) located at tmclain/courses/me634/aviones/aviones-stripped.zip. Unzip the file to a directory. This version of the Aviones program has the code describing the equations of motion and the autopilot stripped out. A simple dynamic model of a unit mass is programmed into the code to provide an example of how Aviones functions and how your MAV equations of motion can be programmed.

30 Design Project Under the aviones directory, open the Aviones Visual Studio Solution Object. Open up the physics.cpp source to view the equations governing the simulated flight. Press Ctrl-F5 to compile and run. After the viewing window is displayed, press Ctrl-G to launch the MAV. Verify that the code compiles and runs. Under the View menu, select Physics Parameters. Change parameter values and observe the behavior of the MAV.

31 Chapter 3 Kinematics and Dynamics In this chapter, we derive the expressions for the kinematics and the dynamics of a rigid body. While the expressions derived in this chapter are general to any rigid body, we will use notation and coordinate frames that are typical in the aeronautics literature. In particular, in Section 3.1 we define the notation that will be used for the state variables of a MAV. In Section 3.2 we derive the expressions for the kinematics, and in Section 3.3 we derive the dynamics. 3.1 MAV State Variables The state variables of the MAV are the following twelve quantities p n = the inertial (north) position of the MAV along î i in F i, p e = the inertial (east) position of the MAV along ĵ i in F i, h = the altitude of the aircraft measured along ˆk i in F i, u = the body frame velocity measured along î b in F b, v = the body frame velocity measured along ĵ b in F b, w = the body frame velocity measured along ˆk b in F b, φ = the roll angle defined with respect to F v2, θ = the pitch angle defined with respect to F v1, ψ = the yaw angle defined with respect to F v, p = the roll rate measured along î b in F b, q = the pitch rate measured along ĵ b in F b, r = the yaw rate measured along ˆk b in F b. 21

32 MAV Kinematics The state variables are shown schematically in Figure 3.1. The position (p n, p e, h) of the MAV is Figure 3.1: Definition of Axes given in the inertial frame, with positive h defined along the negative ˆk i axis in the inertial frame. The velocity (u, v, w) and the angular velocity (p, q, r) of the MAV are given with respect to the body frame. The Euler angles (roll φ, pitch θ, and yaw ψ) are given with respect to the vehicle 2-frame, the vehicle 1-frame, and the vehicle frame respectively. 3.2 MAV Kinematics The position variables p n, p e, and h, which are inertial frame quantities, and the velocity variables u, v, and w, which are body frame quantities, are related by differentiation and a rotational transformation d dt p n p e h u = Rb v v w u = (Rv) b T v w cθcψ sφsθcψ cφsψ cφsθcψ + sφsψ u = cθsψ sφsθsψ + cφcψ cφsθsψ sφcψ v. sθ sφcθ cφcθ w The relationship between absolute angles φ, θ, and ψ, and the angular rates p, q, and r is also complicated by the fact that these quantities are defined in different coordinate frames. The angular rates are defined in the body frame F b. The roll angle φ is a rotation from F v2 to F b about the

33 Kinematics and Dynamics 23 î v2 = î b axis. The pitch angle θ is a rotation from F v1 to F v2 about the ĵ v1 = ĵ v2 axis. The yaw angle ψ is a rotation from F v to F v1 about the ˆk v = ˆk v1 axis. The body-frame angular rates can be expressed in terms of the derivatives of the Euler angles provided that the proper rotational transformations are carried out p φ 0 0 q = 0 + Rv2(φ) b θ + Rv2(φ)R b v1(θ) v2 0 r 0 0 ψ φ cos θ 0 sin θ 0 = cos φ sin φ θ + 0 cos φ sin φ sin φ cos φ 0 0 sin φ cos φ sin θ 0 cos θ ψ 1 0 sθ φ = 0 cφ sφcθ θ. (3.1) 0 sφ cφcθ ψ Inverting we get φ 1 sin(φ) tan(θ) cos(φ) tan(θ) p θ = 0 cos(φ) sin(φ) q. (3.2) ψ 0 sin(φ) sec(θ) cos(φ) sec(θ) r These kinematic equations present state variable derivatives in terms of states variables and are three of the twelve equations of motion required to model the dynamic behavior of the MAV. 3.3 Rigid Body Dynamics To derive the dynamic equations of motion for the MAV, we will apply Newton s second law first to the translational degrees of freedom and then to the rotational degrees of freedom. Newton s laws hold in inertial reference frames, meaning the motion of the body of interest must be referenced to a fixed (i.e., inertial) frame of reference in our case, the ground. Even though motion is referenced to a fixed frame, it can be expressed using vector components associated with other frames, such as the body frame. We do this with the MAV velocity vector v, which for convenience, is most commonly expressed in the body frame as v b = (u, v, w) T. v b is the velocity of the MAV with respect to the ground as expressed in the body frame. Newton s second law applied to translational motion can be stated as m dv dt i = f, (3.3)

34 Rigid Body Dynamics d where m is the mass of the MAV, dt i is the time derivative in the inertial frame, and f is the sum of all external forces acting on the MAV. The external forces we will consider include gravity, aerodynamic forces, and propulsion forces. Using the equation of Coriolis we can express the acceleration dv dt i dv = dv + ω b/i v (3.4) dt i dt b where ω b/i is the angular velocity of the airframe with respect to the inertial frame. Combining (3.3) and (3.4) results in an alternative representation of Newton s second law with the derivative operation carried out in the body frame ( ) dv m + ω b/i v = f. dt b In the case of a maneuvering aircraft, we can most easily apply Newton s second law by expressing the forces and velocities in the body frame. We can therefore require the velocities and forces to be expressed in the body frame where v b = (u, v, w) T, ω b b/i = (p, q, r)t, and f b = (f x, f y, f z ) T. ( ) dv b m + ω b b/i v b = f b (3.5) dt b The expression dvb dt b is the rate of change of the velocity expressed in the body frame, as viewed by an observer on the moving body. Since u, v, and w are the instantaneous projections of v b onto the î b, ĵ b, and ˆk b axes, it follows that dv b dt b = u v. ẇ Expanding the cross product in (3.5), we get u rv qw v = pw ru + 1 m ẇ qu pv f x f y f z as. (3.6) These are the three translational dynamic equations that are part of the twelve state equations used to model the MAV. For rotational motion, Newton s second law states that dh dt i = m,

35 Kinematics and Dynamics 25 where h is the angular momentum in vector form and m is the sum of all externally applied moments. This expression is true provided that moments are summed about the center of mass of the MAV. Applying the equation of Coriolis we have dh = dh + ω b/i h dt i dt b = m. As with translational motion, we can require that the angular momentum and velocity be expressed in the body frame, giving dh b + ω b b/i h b = m b. (3.7) dt b For a rigid body we have h b = Jω b where J is the inertia matrix given by (y 2 + z 2 ) dm xy dm xz dm J = xy dm (x 2 + z 2 ) dm yz dm xz dm yz dm (x 2 + y 2 ) dm = J x J xy J xz J xy J y J yz. J xz J yz J z The diagonal terms of J are called the moments of inertia, while the off-diagonal terms are called the products of inertia. Because the volume integrals above are calculated with respect to the î b, ĵ b, and ˆk b axes fixed in the body, J is constant when viewed from the body frame, hence dj dt b = 0. The expression dωb b/i dt b is the rate of change of the angular velocity expressed in the body frame, as viewed by an observer on the moving body. Since p, q, and r are the instantaneous projections of ω b b/i onto the îb, ĵ b, and ˆk b axes, it follows that ω b b/i = dωb b/i dt b = Taking derivatives and substituting into (3.7), we get ṗ q. ṙ ω b b/i = J 1 [ ω b b/i ( Jω b b/i) + m b ]. (3.8) Airframes are typically symmetric about the plane spanned by î b and ˆk b. In that case J xy = J yz = 0 which implies that J x 0 J xz J = 0 J y 0. J xz 0 J z

36 Rigid Body Dynamics Therefore J 1 = adj(j) det(j) = = J y J z 0 J y J xz 0 J x J z Jxz 2 0 J xz J y 0 J x J y 1 0 J y 0 J xz J 0 xγ Γ J x J y J z JxzJ 2 y J z J Γ 0 xz Γ, where Γ = J x J z J 2 xz. Defining m b = (l, m, n) T we can write Equation (3.8) in component form as J ṗ z J Γ 0 xz Γ 0 r q J x 0 J xz p l 1 q = 0 J y 0 r 0 p 0 J y 0 q + m J ṙ xz J 0 xγ q p 0 J Γ xz 0 J z r n J z J Γ 0 xz Γ J xz pq + (J y J z )qr l 1 = 0 J y 0 J xz (r 2 p 2 ) + (J z J x )pr + m J xz J 0 xγ (J Γ x J y )pq J xz qr n Γ 1 pq Γ 2 qr + Γ 3 l + Γ 4 n = Γ 5 pr Γ 6 (p 2 r 2 ) + 1 J y m, Γ 7 pq Γ 1 qr + Γ 4 l + Γ 8 n

37 Kinematics and Dynamics 27 where Γ 1 = J xz(j x J y + J z ) Γ Γ 2 = J z(j z J y ) + J 2 xz Γ Γ 3 = J z Γ Γ 4 = J xz Γ Γ 5 = J z J x J y Γ 6 = J xz J y Γ 7 = (J x J y )J x + J 2 xz Γ Γ 8 = J x Γ. The six degree-of-freedom model for the MAV kinematics and dynamics can be summarized as follows: 3.4 Design Project ṗ n cθcψ sφsθcψ cφsψ cφsθcψ + sφsψ u ṗ e = cθsψ sφsθsψ + cφcψ cφsθsψ sφcψ v (3.9) ḣ sθ sφcθ cφcθ w u rv qw v = pw ru + 1 f x f m y, (3.10) ẇ qu pv f z φ 1 sin φ tan θ cos φ tan θ p θ = 0 cos φ sin φ q (3.11) sin φ cos φ ψ 0 r cos θ cos θ ṗ Γ 1 pq Γ 2 qr Γ 3 l + Γ 4 n q = Γ 5 pr Γ 6 (p 2 r 2 1 ) + J y m. (3.12) ṙ Γ 7 pq Γ 1 qr Γ 4 l + Γ 8 n 3.1 Modify physics.cpp in Aviones to implement the translational and rotational kinematic equations of motion given by equations (3.9) (3.12). Assume that the inputs are forces and moments applied to the MAV in the body frame. You will specify these forces and moments.

38 Design Project 3.2 Verify your design by individually setting the forces and moments along each axis to a nonzero value and convincing yourself that the motion is appropriate. Hints (a) We will be implementing the physics for a Zagi or Unicorn style airframe throughout the semester. Assume that the mass of the airframe is m = 1.56 kg and that the inertial properties are given by J x = kg-m 2, J y = kg-m 2, J z = kg-m 2, and J xz = kg-m 2. (b) Since J xz is non-zero, there is gyroscopic coupling between roll and yaw. To test your simulation, set J xz to zero and place nonzero moments on l and n. When J xz is not zero, there should not be any coupling in the pitch axis, but there should be coupling in both roll and yaw. Test your code to make sure that you observe these phenomena.

39 Chapter 4 Forces and Moments The objective of this chapter is to describe the forces and moments that act on the UAV. Following [5], we will assume that the forces and moments are primarily due to seven sources: f = f g + f a + f p + f d m = m a + m p + m d, where f g = gravitational force (f a, m a ) = aerodynamic forces/moments, (f p, m p ) = force/moment due to propulsion, (f d, m d ) = effects of atmospheric disturbances. 4.1 Gravitational Forces The effect of the earth s gravitational field on the MAV can be modeled as a force proportional to the mass acting on the center of mass. This force acts in the î i direction and is proportional to the mass of the MAV by the gravitational constant g. In the vehicle frame F v, the gravity force acting on the center of mass is given by f v g = When applying Newton s 2nd law in Chapter 3, we summed forces along the axes in the body frame. Therefore, we must transform the gravitational force into its body frame components to mg.

40 Aerodynamic Forces give 0 fg b = Rv b 0 mg mg sin θ = mg cos θ sin φ. mg cos θ cos φ Since the gravity force acts through the center of mass of the MAV, there are no moments produced by gravity. 4.2 Aerodynamic Forces As a MAV passes through the air, a pressure distribution is generated around the MAV body, as depicted in Figure 4.1. The strength and distribution of the pressure acting on the MAV is a function of the airspeed, the air density, and the shape and attitude of the MAV. Accordingly, the dynamic pressure is define as q = 1 2 ρv 2 a where ρ is the air density and V a is the speed of the MAV through the surrounding air mass. Instead Figure 4.1: Pressure distribution around airfoil. of attempting to characterize the pressure distribution around the wing, the common approach is to capture the effect of the pressure with a combination of forces and a moment. For example, if we consider the longitudinal (î b -ˆk b ) plane, the effect of the pressure acting on the MAV body can be modeled using a lift force, a drag force, and a moment. As shown in Figure 4.2, the lift and drag forces are applied at the quarter-chord point, also known as the aerodynamic center.

41 Forces and Moments 31 Figure 4.2: Effect of pressure distribution can be modeled using a lift force, a draft force, and a moment. The lift, drag, and moment are commonly expressed as Lift = C L qs Drag = C D qs Moment = C M qsc, where C L, C D, and C M are nondimensional aerodynamic coefficients, S is the planform area of the MAV wing, and c is the mean chord of the MAV wing. For airfoils generally, the lift, drag, and pitching moment coefficients are significantly influenced by the airfoil shape, Reynolds number, Mach number, and the angle of attack. For range of airspeeds flown by MAVs, the Reynolds number and Mach number effects are not significant. We will consider the effects of the angles α and β, the angular rates p, q, and r, and the deflection of control surfaces, on the aerodynamic coefficients. It is common to decompose the aerodynamic forces and moments into two groups: longitudinal and lateral-directional. The longitudinal forces and moments act in the î b -ˆk b plane, also called the pitch plane. They include the forces in the î b and ˆk b directions (caused by Lift and Drag) and the moment about the ĵ b axis. The lateral-directional forces and moments include the force in the ĵ b direction and the moments about the î b and ˆk b axes.

42 Aerodynamic Forces Longitudinal Aerodynamics The longitudinal aerodynamic forces and moments cause motion in the pitch plane. They are the aerodynamic forces and moments with which we are perhaps most familiar: lift, drag, and pitching moment. By definition, the lift and drag force are aligned with the axes of the stability frame as shown in figure 4.2. When represented as a vector, the pitching moment also aligns with the ĵ s axis of the stability frame. Each of these forces/moments is considered to be a strong function of the angle of attack. The pitch rate q and the elevator deflection δ e also influence the longitudinal forces/moments: Lift = qsc L (α, q, δ e ) Drag = qsc D (α, q, δ e ) Moment = qscc M (α, q, δ e ). These expressions, which are nonlinear over broad ranges of α, can be linearized about a nominal operating condition to yield [ ] c Lift = qs C L0 + C Lα α + C Lq q + C Lδe δ e 2V [ a ] c Drag = qs C D0 + C Dα α + C Dq q + C Dδe δ e 2V [ a ] c Moment = qsc C M0 + C Mα α + C Mq q + C Mδ e 2V δ e a Note however, that these simple linear models are not valid for large angles of attack where flow over the wing is separated. As the angle of attack is increased, the flow over the wing separates and a stall condition occurs. In stall, the lift produced by the wing drops off rapidly as the angle of attack is increased. This simple linear model does not properly account for stall effects. The constants C L0, C D0, and C M0 model the lift, drag, and pitching moment for the condition α = q = δ e = 0 and derive their values from the way the wing is mounted to the fuselage Lateral-Directional Aerodynamics The lateral-directional aerodynamic force and moments cause translational motion in the lateral direction along the ĵ b axis as well as rotational motions in roll and yaw that will result in directional changes in the flight path of the MAV. The lateral-directional aerodynamics are most significantly influenced by the sideslip angle β. They are also influenced by the roll rate p, the yaw rate r, the deflection of the aileron δ a, and the deflection of the rudder δ r. Denoting the lateral force as f x and

43 Forces and Moments 33 the roll and yaw moments as l and n respectively, we have f y = qsc Y (β, p, r, δ a, δ r ) l = qsbc l (β, p, r, δ a, δ r ) n = qsbc n (β, p, r, δ a, δ r ) where C Y, C l, and C n are nondimensional aerodynamic coefficients. These relations can be linearized to give [ ] b b f y = qs C Y0 + C Yβ β + C Yp p + C Yr r + C Yδ a 2V a 2V δ a + C δ Yδ r r (4.1) [ a ] b b l = qsb C l0 + C lβ β + C lp p + C lr r + C lδa δ a + C lδr δ r (4.2) 2V a 2V [ a ] b b n = qsb C n0 + C nβ β + C np p + C nr r + C nδa δ a + C nδr δ r (4.3) 2V a 2V a where b is the wingspan of the MAV Aerodynamic Forces and Moments Summary We have developed equations for the longitudinal aerodynamic forces and moments and the lateraldirectional aerodynamic forces and moments. Here we will gather the forces into a single vector equation and the moments into a single vector equation. First, recall that the lift and drag are expressed in the stability frame. The lateral force acts along the ĵ b axis in the body frame which is the same as the ĵ s axis in the stability frame. As can be seen from Figure 4.2, the forces in the body axis are given by f x f y f z cos α 0 sin α Drag = f y sin α 0 cos α Lift [ C D0 cos α + C L0 sin α] + [ C Dα cos α + C Lα sin α] α + [ C Dq cos α + C Lq sin α ] c 2V a q + [ C Dδe cos α + C Lδe sin α ] δ e = qs b b C Y0 + C Yβ β + C Yp 2V a p + C Yr 2V a r + C Yδa δ a + C Yδr δ r. [ C D0 sin α C L0 cos α] + [ C Dα sin α C Lα cos α] α + [ C Dq sin α C Lq cos α ] c 2V a q + [ C Dδe sin α C Lδe cos α ] δ e

44 Aerodynamic Forces Defining C X0 C Xα = CD0 cos α + C L0 sin α = CDα cos α + C Lα sin α C Xq C Xδ e = CDq cos α + C Lq sin α = C cos α + C Dδ e L δ sin α (4.4) e C Z0 C Zα = CD0 sin α C L0 cos α = CDα sin α C Lα cos α we get f x f y f z C Zq C Zδ e = CDq sin α C Lq cos α = C sin α C Dδ e L δ cos α, e c C X0 + C Xα α + C Xq 2V a q + C Xδe δ e b b = qs C Y0 + C Yβ β + C Yp 2V a p + C Yr 2V a r + C Yδa δ a + C Yδr δ r. c C Z0 + C Zα α + C Zq 2V a q + C δ Zδ e e Gathering the aerodynamic moments, we get [ ] b b l b C l0 + C lβ β + C lp 2V a p + C lr 2V a r + C lδa δ a + C lδr δ r [ ] c m = qs c C m0 + C mα α + C mq 2V a q + C δ mδ e e [ ]. n b b b C n0 + C nβ β + C np 2V a p + C nr 2V a r + C δ nδ a a + C δ nδ r r Aerodynamic Coefficients The aerodynamic coefficients C mα, C lβ, C nβ, C mq, C lp, and C nr are referred to as stability derivatives because their values determine the static and dynamic stability of the MAV. Static stability deals with the direction of aerodynamic moments as the MAV is perturbed away from its nominal flight condition. If the moments tend to restore the MAV to its nominal flight condition, the MAV is said to be statically stable. Most aircraft are designed to be statically stable. The coefficients C mα, C lβ, and C nβ determine the static stability of the MAV. They represent the change in the moment coefficients with respect to changes in the direction of the relative wind, as represented by α and β. C mα is referred to as the longitudinal static stability derivative. For the MAV to be statically stable, C mα will be less than zero. In this case, an increase in α due to an updraft, would cause the MAV to nose down to maintain the nominal angle of attack.

45 Forces and Moments 35 C lβ is called the roll static stability derivative and is typically associated with dihedral in the wings. For static stability in roll, C lβ must be negative. A negative value for C lβ will result in rolling moments that roll the MAV away from the direction of sideslip thereby driving the sideslip angle β to zero. C nβ is referred to as the yaw static stability derivative. It is sometimes called the weathercock stability derivative. For the MAV to be stable in yaw, C nβ must be positive. This simply implies that for a positive sideslip angle, a positive yawing moment will be induced. This yawing moment will yaw the MAV into the direction of the relative wind, driving the sideslip angle to zero. Dynamic stability, familiar to those conversant with control systems, deals with the dynamic behavior of the plane in response to disturbances. If a disturbance is applied to the MAV, the MAV is said to by dynamically stable if the response of the MAV damps out in a finite amount of time. If we use a second-order mass-spring-damper analogy to analyze the MAV, the stability derivatives C mα, C lβ, and C nβ model torsional springs, while the derivatives C mq, C lp, and C nr model torsional dampers. The moments of inertia of the MAV body provide the mass. As we will see in Chapter?? when we linearize the dynamic equations of motion for the MAV, the signs of the stability derivatives must be consistent to ensure that the characteristic roots of the MAV lie in the left half of the s-plane. C nr C mq is referred to as the pitch damping derivative. C lp is called the roll damping derivative. is referred to as the yaw damping derivative. Each of these damping derivatives is usually negative, meaning that a moment is produced that opposes the direction of motion, thus damping the motion out. The aerodynamic coefficients C mδe, C lδa, and C nδr are associated with the deflection of control surfaces and are referred to as the primary control derivatives. They are primary because the moments produced are the intended result of the specific control surface deflection. For example, the intended result of an elevator deflection δ e is a pitching moment m. C and C lδ r n δ are called a cross-control derivatives. They define the off-axis moments that occur when the control surfaces are deflected. To define the appropriate signs of the control derivatives, we must first define positive directions for the control surface deflections. The positive direction of a control surface deflection can be determined by applying the right-hand rule to the hinge axis of the control surface. For example, the hinge axis of the elevator is aligned with the body ĵ b axis. Applying the right-hand rule about the ĵ b implies that a positive deflection for the elevator is trailing edge down. Similarly, positive deflection for the rudder is trailing edge left. Finally, positive aileron deflection is trailing edge down on each aileron. The aileron deflection δ a can be thought of as a composite deflection defined

46 Propulsion Forces by δ a = 1 2 (δ a-left δ a-right ). With this sign convention, a positive elevator deflection results in a nose-down (negative) pitching moment, a positive aileron deflection causes a right-wing-down rolling moment (positive), and a positive rudder deflection causes a nose-left (negative) yawing moment. We will define the signs of the primary control derivatives so that positive deflections cause positive moments. For this to be the case, C mδ e will be negative, C l δ a will be positive, and C n δ r will be negative. Stall effects Discuss stall effects and the lift-drag polar. 4.3 Propulsion Forces A simple model for the thrust generated by a propeller can be developed by applying Bernoulli s principle to calculate the pressure ahead of and behind the propeller and applying the pressure difference to the propeller area. This approach will yield a model that is correct for a perfectly efficient propeller. While overly optimistic in its thrust predictions, this model will provide a reasonable starting point for a MAV simulation. Using Bernoulli s equation, the total pressure upstream of the propeller can be written as P upstream = P ρv 2 a where P 0 is the static pressure and ρ is the air density. The pressure downstream of the propeller can be expressed as P downstream = P ρv 2 exit. where V exit is the speed of the air as it leaves the propeller. Ignoring the transients in the motor, there is a linear relationship between the pulse-width-modulation command δ t and the angular velocity of the propeller. The propeller in turn creates an exit air speed of V exit = kδ t. If S prop is the area swept out by the propeller, then the thrust produced by the motor is given by F xp = S prop (P downstream P upstream ) = 1 2 ρs [ prop (kmotor δ t ) 2 ] Va 2.

47 Forces and Moments 37 Therefore f p = 1 (k motor δ t ) 2 Va 2 2 ρs prop 0. 0 Most MAVs are designed so that the thrust acts directly along the î b body-axis of the aircraft. Therefore the thrust does not produce any moments about the center of mass of the MAV. Propeller Torque As the MAV propeller spins, it applies force to the air that passes through the propeller, increasing the momentum of the air while generating a thrust force on the MAV. Equal and opposite forces are applied by the air on the propeller. The net effect of these forces is a torque about the propeller axis of rotation applied to the MAV. The torque applied by the motor to the propeller (and then to the air) results in an equal and opposite torque applied by the propeller to the motor which is fixed to the MAV body. This torque is opposite to the direction of the propeller rotation and proportional to the square of the propeller angular velocity T p = k Tp (k Ω δ t ) 2 where Ω = k Ω δ t is the propeller speed and k Tp is a constant determined by experiment. The moments due to the propulsion system are therefore k Tp (k Ω δ t ) 2 m p = 0. 0 The effects of this propeller torque are usually relatively minor. If unaccounted for, it will cause a slow rolling motion in the direction opposite the propeller rotation. It is easily corrected by applying a small aileron deflection, which generates a rolling moment to counteract the propeller torque Density Dependence on Pressure and Temperature The density of air is dependent on the air pressure P s and the temperature T. Air density is given by the ideal gas law as ρ = P s RT,

48 Summary where R = [J/kg-K] is the specific gas constant for dry air. Notice that in this formula, temperature is expressed in units of Kelvin. The conversion from Fahrenheit to Kelvin is given by T [K] = 5 (T [F] 32) Pressure is expressed in N/m 2. Typical weather data reports pressure in inches of Mercury. The conversion from N/m 2 to inches of Mercury is given by P s [N/m 2 ] = P s [inhg]. 4.4 Summary The total forces and torques can therefore be summarized as follows: f x f y f z c mg sin θ C X0 + C Xα α + C Xq 2V a q + C Xδe δ e b b = mg cos θ sin φ + qs C Y0 + C Yβ β + C Yp 2V a p + C Yr 2V a r + C Yδa δ a + C Yδr δ r c mg cos θ cos φ C Z0 + C Zα α + C Zq 2V a q + C Zδe δ e + 1 (k motor δ t ) 2 Va 2 2 ρs prop 0. (4.5) 0 [ ] b b l b C l0 + C lβ β + C lp 2V a p + C lr 2V a r + C lδa δ a + C lδr δ r [ ] k Tp (k Ω δ t ) 2 c m = qs c C m0 + C mα α + C mq 2V a q + C δ mδ e e [ ] + 0. n b b b C n0 + C nβ β + C np 2V a p + C nr 2V a r + C δ nδ a a + C δ nδ 0 r r (4.6)

49 Forces and Moments Design Project 4.1 Add the equations for force and torque to the simulation developed in Problem 3.1. Use the following data which are the aerodynamic coefficients for an A-4D fighter aircraft [1] and the physical dimensions (approximately) of a ZAGI MAV. Longitudinal Coef. Value Lateral Coef. Value C L C Y0 0 C D C l0 0 C m0 0 C n0 0 C Lα 3.45 C Yβ C Dα 0.30 C lβ C mα C nβ 0.25 C Lq 0 C Yp 0 C Dq 0 C lp C mq -3.6 C np C Lδ e C Yr 0 C Dδ e 0 C lr 0.14 C mδ e 0.5 C nr C Yδ a 0 C lδ a 0.08 C nδ a 0.06 C Yδ r C lδ r C nδ r Table 4.1: Aerodynamic coefficients for design project. 4.2 Verify your simulation by setting the control surface deflections to different values. Observe the response of the MAV. Does it behave as you think it should?

50 Design Project Parameter m J Value 1.56 kg Kg-m S m 2 b c m m S prop m 2 ρ kg/m 3 k motor 20 Table 4.2: Simulation parameters for design project.

51 Chapter 5 Nonlinear Equations of Motion 5.1 Six DOF Equations of Motion General Force and Torque Model If we use the general force and torque models given by f x sin θ C X (x, δ) f y = mg cos θ sin φ + qs C Y (x, δ) f z cos θ cos φ C Z (x, δ) l bc l (x, δ) m = qs cc m (x, δ), n bc n (x, δ) 41

52 Six DOF Equations of Motion in the six degree-of-freedom model given by Equations (3.9)-(3.12), we get ṗ n cθcψ sφsθcψ cφsψ cφsθcψ + sφsψ u ṗ e = cθsψ sφsθsψ + cφcψ cφsθsψ sφcψ v (5.1) ḣ sθ sφcθ cφcθ w u rv qw g sin θ v = pw ru + g cos θ sin φ + qs C X (x, δ) C m Y (x, δ) (5.2) ẇ qu pv g cos θ cos φ C Z (x, δ) φ 1 sin(φ) tan(θ) cos(φ) tan(θ) p θ = 0 cos(φ) sin(φ) q (5.3) ψ 0 sin(φ) sec(θ) cos(φ) sec(θ) r ṗ Γ 1 pq Γ 2 qr b [Γ 3 C l (x, δ) + Γ 4 C n (x, δ)] q = Γ 5 pr Γ 6 (p 2 r 2 c )+ + qs J y C m (x, δ). (5.4) ṙ Γ 7 pq Γ 1 qr b [Γ 4 C l (x, δ) + Γ 8 C n (x, δ)] Linear Aerodynamic Model A variety of different models for the aerodynamic forces and moments appear in the literature. In particular, if we use the linear model described in Chapter 4 we get the following equations of

53 Nonlinear Equations of Motion 43 motion: ṗ n = (cos θ cos ψ)u + (sin φ sin θ cos ψ cos φ sin ψ)v + (cos φ sin θ cos ψ + sin φ sin ψ)w ṗ e = (cos θ sin ψ)u + (sin φ sin θ sin ψ + cos φ cos ψ)v + (cos φ sin θ sin ψ sin φ cos ψ)w ḣ = u sin θ v sin φ cos θ w cos φ cos θ (5.7) u = rv qw g sin θ + qs [ ] cq C X0 + C Xα α + C Xq + C Xδ e m 2V δ e + ρs prop a 2m C [ prop (kδt ) 2 ] Va 2 (5.8) v = pw ru + g cos θ sin φ + qs [ ] bp br C Y0 + C Yβ β + C Yp + C Yr + C Yδa δ a + C Yδr δ r m 2V a 2V a (5.9) ẇ = qu pv + g cos θ cos φ + qs [ ] cq C Z0 + C Zα α + C Zq + C Zδ e m 2V δ e (5.10) a φ = p + q sin φ tan θ + r cos φ tan θ (5.11) θ = q cos φ r sin φ (5.5) (5.6) (5.12) ψ = q sin φ sec θ + r cos φ sec θ (5.13) [ ] bp br ṗ = Γ 1 pq Γ 2 qr + qsb C p0 + C pβ β + C pp + C pr + C pδ a 2V a 2V δ a + C δ pδ r r (5.14) a q = Γ 5 pr Γ 6 (p 2 r 2 ) + qs c [ ] cq C m0 + C mα α + C mq + C mδe δ e (5.15) J y 2V a [ ] bp br ṙ = Γ 7 pq Γ 1 qr + qsb C r0 + C rβ β + C rp + C rr + C rδ a 2V a 2V δ a + C δ rδ r r, (5.16) a

54 Navigation Models where C p0 = Γ 3 C l0 + Γ 4 C n0 C pβ = Γ 3 C lβ + Γ 4 C nβ C pp = Γ 3 C lp + Γ 4 C np C pr = Γ 3 C lr + Γ 4 C nr C pδ a = Γ 3C lδ a + Γ 4C nδ a C pδ r = Γ 3C lδ r + Γ 4C nδ r C r0 = Γ 4 C l0 + Γ 8 C n0 C rβ = Γ 4 C lβ + Γ 8 C nβ C rp = Γ 4 C lp + Γ 8 C np C rr = Γ 4 C lr + Γ 8 C nr C rδ a = Γ 4C lδ a + Γ 8C nδ a C rδr = Γ 4 C lδr + Γ 8 C nδr. 5.2 Navigation Models State Navigation Equations The full 12-state equations of motion given in Equations (5.1) (5.4) are usually not needed to derive good navigation models. In this section we will show a useful method for reducing these equations. The first simplification is to note that the velocity vector in the wind frame, i.e. (V a, 0, 0) T, where V a is the airspeed, can be related to the inertial coordinates by two angles: the flight path angle γ and the heading ψ, as shown in Figure 5.1. The heading is obtained by rotating the inertial coordinate from by ψ until the x-axis is aligned with the projection of the velocity vector on the x-y plane. The appropriate transformations are given by ṗ n cos ψ sin ψ 0 cos γ 0 sin γ V a cos ψ cos γ ṗ e = sin ψ cos ψ = sin ψ cos γ V a. ḣ sin γ 0 cos γ 0 sin γ Therefore, in the absence of wind, we have ṗ n cos ψ cos γ ṗ e = sin ψ cos γ V a. ḣ sin γ

55 Nonlinear Equations of Motion 45 Flight path Figure 5.1: The flight path angles γ and χ. Coordinated Turn The following derivation draws on the discussion in [?]. Figure 5.2 shows a series of free-body diagrams of a MAV executing a climbing coordinated turn maneuver. During a coordinated turn, the bank angle is set so that there is no net side force acting on the MAV. The centrifugal force acting on the MAV is equal and opposite to the horizontal component of the lift force Dividing (5.17) by (5.18) and solving for ψ gives L sin φ = m (V a cos γ) ψ. (5.17) L cos φ = mg cos γ (5.18) ψ = g V a tan φ, (5.19) which is the equation for a coordinated turn. Given that the turning radius is given by R t = V a cos γ/ ψ we get R t = V 2 a cos γ g tan φ. (5.20) Defining the Load Factor as n = L/mg and drawing on (5.18) we find that for a steady, climbing coordinated turn the load factor is n = cos γ cos φ.

56 Navigation Models Figure 5.2: Schematic of UAV in a coordinated turn. Taken from Phillips. (FIX THIS!) In wings-level, horizontal flight, φ = γ = 0 and n = 1. Considering load factor as an input to the system, Eq. (5.19) can be rewritten as ψ = g sin φ n. (5.21) V a cos γ Accelerating Climb Our derivation of the dynamic equation for γ draws on the discussion in [2, p ]. The free body diagram of the UAV in the x z plane is shown in Figure 5.3. Since the UAV has a roll angle of φ, the projection of the lift vector onto the x z plane is L cos φ. The centripetal force due to

57 Nonlinear Equations of Motion 47 Figure 5.3: Free body diagram indicating forces on the UAV in the x z plane. The left wing is out of the page. The UAV is assumed to be in a roll angle of φ. to the pull-up maneuver is mv a γ. Therefore, summing the forces in the x z plane gives L cos φ = mv a γ + mg cos γ. Letting n = L/mg and solving for γ gives γ = g V a (n cos φ cos γ). (5.22) We will assume that the mini-uav is equipped with an autopilot that implements the following feedback loops: (1) airspeed hold, (2) roll-attitude hold, and (3) load-factor hold. In addition, we will assume that the autopilot is tuned such that the closed-loop behavior of these loops is essentially first order. Therefore, the closed loop behavior of the autopilot is given by V a = 1 (Va c V a ) τ V φ = 1 τ φ (φ c φ) (5.23) ṅ = 1 τ n (n c n), where τ > 0 are positive autopilot time constants, and V c a, φ c, and n c are the inputs to the autopilot.

58 Navigation Models In summary, the equations of motion for the MAV are given by ṗ n = V a cos ψ cos γ + w n (5.24) ṗ e = V a sin ψ cos γ + w e (5.25) ḣ = V a sin γ + w h (5.26) ψ = g V a sin φ cos γ n (5.27) γ = g V a (n cos φ cos γ) (5.28) V a = 1 (Va c V a ) (5.29) τ V φ = 1 τ φ (φ c φ) (5.30) ṅ = 1 τ n (n c n). (5.31) An equivalent, but useful alternative to these equations is to define a new parameter ν that is related to the load factor by ν = cos φ n. (5.32) cos γ In this case equations (5.27) and (5.28) become ψ = g V a tan(φ)ν (5.33) γ = g cos γ V a (ν 1). (5.34) Note that when ν = 1, the airframe is experiencing a level-flight coordinated turn State Navigation Equations If we assume that the airspeed is constant and that the load factor tracking dynamics are fast relative to the other dynamics in the system (n = n c and ν = nu c ), then the six state navigation equations

59 Nonlinear Equations of Motion 49 become where the inputs are ν c and φ c. ṗ n = V a cos ψ cos γ + w n (5.35) ṗ e = V a sin ψ cos γ + w e (5.36) ḣ = V a sin γ + w h (5.37) ψ = g V a tan(φ)ν c (5.38) γ = g cos γ V a (ν c 1) (5.39) φ = 1 τ φ (φ c φ), (5.40) State Navigation Equations There are times when we are interested in 2D path planning. In that case we select ν = ν c = 1 to obtain ṗ n = V a cos ψ + w n (5.41) ṗ e = V a sin ψ + w e (5.42) ψ = g tan φ (5.43) V a φ = 1 (φ c φ), (5.44) τ φ where φ c is the input. 5.3 Wind Models Wind Speed Above Ground According to [9], the wind-speed increases with altitude. Up to an altitude of few hundred meters, the wind speed profile v w (z) is approximately a logarithmic function of the altitude (z): ( ) z v w (z) v o (δ) ln, for z > 10z 0, z 0 where δ is the thickness of the surface boundary layer, z 0 = 0.1 m in a low density urban zone, hence z(v w ) = z 0 e v w/v 0 (δ).

60 Design Project 5.4 Design Project 5.1 In preparation for this assignment, you ll need to make a few changes to the Aviones program. You can either start with the stripped version of the program found at et.byu.edu/ tmclain/courses/me634/aviones/aviones-stripped.zip, or you can work with a copy of the Aviones code that you have already downloaded. This assignment will not build on your existing Aviones code, so make a backup copy of your Aviones project before making the edits required by this assignment. You will be simulating equations involving some new state variables that we haven t used previously (γ, n, ν, V a ). You need to make sure that these states are part of the UAVStates structure and that they are integrated properly when the equations of motion are solved. Do the following tasks: (a) Edit the UAVstates structure in AutopilotStructs.h to include γ, n, and ν. (b) In the file physics.cpp, look for the functions scalar multiply states and add states. These functions are used in the integration of the equations of motion. Edit these functions to ensure that your new states are added and multiplied along with the other states. 5.2 Implement the equations for the four-state navigation model given by Eqs. (5.41)-(5.44) in Aviones. Treat φ c, w n, and w e as input parameters that can be changed through the Aviones interface. Give these input parameters initial values of zero. Let V a = 15 m/s and τ φ = 0.7 s. Build and run your program. Fly the MAV by changing the roll angle command. Experiment with adding wind. 5.3 Implement the equations for the six-state navigation model given by Eqs. (5.35)-(5.40) in Aviones. Aviones uses the attitude states to display the attitude of the MAV in the graphics window. Since we don t have a value for the pitch state, make the assumption that θ γ and integrate the pitch state derivative along with all of the other state derivatives. This will allow the pitch attitude to be displayed. Fly the MAV using φ c and ν c as inputs. Attempt to fly climbing turn. Attempt a steeply banked turn. What is the key to doing this successfully? 5.4 Implement the equations for the eight-state navigation model given by Eqs. (5.24)-(5.31) in Aviones. Fly the MAV using Va c, φ c, and n c as inputs. Let τ n = 0.1 s and τ V = 2 s. Again, attempt some demanding maneuvers. Hopefully, you will sense the need for a control system to aid both the stable flight and navigation of the MAV.

61 Chapter 6 Trim Suppose that we are given the nonlinear system described by the differential equations ẋ = f(x, u), where f : R n R m R n, x is the state of the system and u is the input. The system is said to be in equilibrium at the state ˆx and input û if f(ˆx, û) = 0. When a MAV is in constant-altitude, wings-level steady flight, a subset of its states are in equilibrium. In particular, h, u, v, w, φ, θ, ψ, p, q, and r are all constant. In the aerodynamics literature, an aircraft in equilibrium is said to be in trim. In general, trim conditions may include states that are not constant. For example, in steady climb, wings level flight, ḣ is constant and h grows linearly. Also, in a constant turn ψ is constant and ψ has linear growth. Therefore, in general, the trim condition is given by ˆx = f(ˆx, û). The objective of this chapter is describe how to compute trim states and inputs for several common flight conditions. In particular, we will focus on the following three flight conditions. 1. Wings-level flight. In wings-level flight, airspeed is assumed to be constant. The wings are level but the flight path angle may not be zero. In other words, we allow for constant rate climbs and descents. Wings-level flight is a pure longitudinal motion. 2. Constant altitude turn. In a constant-altitude turn, the velocity is assumed constant and the altitude is held constant. To maintain constant altitude, both longitudinal and lateral controls are required. 51

62 Turn with a Constant Climb Rate 3. Turn with a constant climb rate. This flight condition is a combination of wings-level flight and constant altitude turn maneuvers. Wings level flight and constant altitude turns are special cases of this flight condition. It is particularly useful for autonomous take-off and landing of MAVs. Note that the third condition, turn with a constant climb rate, subsumes the the first two conditions. Therefore, we will focus on turns with a constant climb rate. Since these trim conditions are independent of inertial position, p n and p e do not factor into trim calculations. 6.1 Turn with a Constant Climb Rate Let x = (h, u, v, w, φ, θ, ψ, p, q, r) T and u = (δ e, δ t, δ a, δ r ) T. The objective is to find the trim states ˆx and the trim inputs û that guarantee that ˆx = f(ˆx, û), where f is given by Equations (5.7) (5.16). Consider the desired flight condition with a constant airspeed of V a, a flight path angle of γ, and a turning radius of R. The climb rate will therefore be constant and is given by ĥ = V a sin γ. The turn rate is also constant and is given by The rest of the states will be constant, therefore ˆψ = V a R. ĥ = V a sin γ û = 0 ˆv = 0 ŵ = 0 ˆφ = 0 ˆx =. (6.1) ˆθ = 0 ˆψ = V a R ˆp = 0 ˆq = 0 ˆr = 0

63 Trim 53 The first objective is to show that the state variables and the input commands can be expressed in terms of V a, γ, R, α, β, and φ. Altitude ĥ: Since ĥ = V a sin γ where V a and γ are constant, altitude is given by ĥ = ĥ(t 0) + V a sin γ(t t 0 ). However, since the right hand side of Equations (5.7) (5.16) are independent of h, this expression does not enter into the trim calculation. Body frame velocities û, ˆv, ŵ: in terms of V a, α and β as From Equation (2.4), the body frame velocities can be expressed û cos ˆα cos ˆβ ˆv = V a sin ˆβ. ŵ sin ˆα cos ˆβ Euler angles ˆθ, ˆψ: By definition of the flight path angle we have ˆθ = ˆα + γ. Since ˆψ = V a R where V a and R are constant, the yaw angle is given by ˆψ = ˆψ(t 0 ) + V a R (t t 0). However, since the right hand side of Equations (5.7) (5.16) are independent of ψ, this expression does not enter into the trim calculation. Angular rates p, q, r: The angular rates can be expressed in terms of the Euler angles by using Equation (3.1). Therefore ˆp 1 0 sin ˆθ ˆφ = 0 ˆq = 0 cos ˆφ sin ˆφ cos ˆθ ˆr 0 sin ˆφ cos ˆφ cos ˆθ ˆθ = 0 = V sin ˆθ a sin R ˆφ cos ˆθ, ˆψ = V a cos ˆφ cos ˆθ R where ˆθ has already been expressed in terms of γ and ˆα. Elevator δ e : Given ˆp, ˆq, and ˆr we can solve Equation (5.15) for δ e giving [ ] Jxz(ˆp 2 ˆr 2 )+(J x J z)ˆpˆr) cˆq 1 C 2 ˆδ e = ρv a 2 cs m0 C mα ˆα C mq 2V a. (6.2) C mδ e

64 Turn with a Constant Climb Rate Throttle δ t : Equation (5.8) can be solved for δ t giving ˆδ t = ˆrˆv + ˆqŵ + g sin ˆθ ρv 2 2m a S [ C X0 + C Xα ˆα + C Xq ρs prop C prop k 2 2m ] + C V Xδ ˆδ a e e cˆq 2 ˆ + V a 2 k, (6.3) 2 where C X = C D cos ˆα + C L sin ˆα. Aileron δ a and Rudder δ r : The aileron and rudder commands are found by solving Equations (5.14) and (5.16): ) (ˆδa = ˆδ r ( C pδ a C rδ a C pδ r C rδ r Trim Algorithm ) 1 Γ 1 ˆpˆq+Γ 2 ˆqˆr 1 2 ρv a 2 Sb Γ 7 ˆpˆq+Γ 1 ˆqˆr 1 2 ρv a 2 Sb bˆp C p0 C pβ ˆβ Cpp 2V a C pr C r0 C rβ ˆβ bˆp Crp 2V a C rr bˆr 2V a bˆr 2V a. (6.4) All of the state variables of interest and the control inputs have been expressed in terms of V a, γ, R, α, β and φ. The inputs to the trim algorithm are V, γ, and R. To find α, β, and φ we need to solve the following optimization problem: (ˆα, ˆφ, ˆβ) = arg min ˆx f(ˆx, û) 2. This can be performed numerically using a gradient descent algorithm which will be described in the next section. The trim algorithm is summarized in Algorithm Numerical implementation of Gradient Descent The objective of this section is to describe a simple gradient descent algorithm that solves the optimization problem min J(ξ), ξ where J : R m R is assumed to be continuously differentiable with well defined local minima. The basic idea is to follow the negative gradient of the function given an initial starting location ξ (0). In other words, we let ξ = κ J (ξ), (6.5) ξ

65 Trim 55 Algorithm 1 Trim for Climbing Turns 1: Input: Desired airspeed V a, desired flight path angle γ, and desired turn radius R. 2: Compute: (ˆα, ˆβ, ˆφ) = arg min ˆx f(ˆx, û) 2. 3: Compute trimmed states: û = V a cos ˆα cos ˆβ ˆv = V a sin ˆβ ŵ = V a sin ˆα cos ˆβ ˆθ = ˆα + γ ˆp = Va sin ˆθ R ˆq = Va sin ˆφ cos ˆθ R ˆr = Va cos ˆφ cos ˆθ R 4: Compute trimmed input: ˆδ e = [Equation (6.2)] ˆδ t = [Equation (6.3)] ) (ˆδa = [Equation (6.4)]. ˆδ r where κ is a positive constant that defines the descent rate. A discrete approximation of (6.5) is given by ξ (k+1) = ξ (k) κ J ξ (ξ(k) ). For the trim calculation J is difficult to determine analytically. However, it can be efficiently ξ computed numerically. By definition we have where J ξ = J ξ 1. J ξ m, J J(ξ 1,, ξ i + ɛ,, ξ m ) J(ξ 1,, ξ i,, ξ m ) = lim, ξ i ɛ 0 ɛ which can be numerically approximated as where ɛ is a small constant. J J(ξ 1,, ξ i + ɛ,, ξ m ) J(ξ 1,, ξ i,, ξ m ), ξ i ɛ

66 Turn with a Constant Climb Rate Algorithm 2 Computation of J = ˆx f(ˆx, û) 1: Input: α, β, φ, V a, R, γ. 2: Compute ˆx: 2 ˆx = [Equation (6.1)]. 3: Compute trimmed states: û = V a cos ˆα cos ˆβ ˆv = V a sin ˆβ ŵ = V a sin ˆα cos ˆβ ˆθ = ˆα + γ ˆp = Va sin ˆθ R ˆq = Va sin ˆφ cos ˆθ R ˆr = Va cos ˆφ cos ˆθ R 4: Compute trimmed input: ˆδ e = [Equation (6.2)] ˆδ t = [Equation (6.3)] ) (ˆδa = [Equation (6.4)]. ˆδ r 5: Compute f(ˆx, û): f(ˆx, û) = [Equation (5.7) (5.16)] 6: Compute J: J = ˆx f(ˆx, û) 2.

67 Trim 57 For the trim algorithm, J(α, β, φ) = ˆx f(ˆx, û) 2 which is computed using Algorithm 2. The gradient descent optimization algorithm is summarized in Algorithm 3. Algorithm 3 Minimize J(ξ) 1: Input: α (0), β (0), φ (0), V a, R, γ. 2: for k = 1 to N do 3: α + = α (k 1) + ɛ 4: β + = β (k 1) + ɛ 5: φ + = φ (k 1) + ɛ 6: 7: 8: J = J(α+,β (k 1),φ (k 1) ) J(α (k 1),β (k 1),phi (k 1) ) α ɛ J = J(α(k 1),β +,φ (k 1) ) J(α (k 1),β (k 1),phi (k 1) ) β ɛ J = J(α(k 1),β (k 1),φ + ) J(α (k 1),β (k 1),phi (k 1) ) φ ɛ 9: α (k) = α (k 1) κ J α 10: β (k) = β (k 1) κ J β 11: φ (k) = φ (k 1) κ J φ 12: end for

68 Design Project 6.2 Design Project 6.1 Write a Matlab routine that computes values for x, y, and z that minimize the function J = x 2 + y 2 + z 2 14x 8y + 6z Show that your numerical solution agrees with a solution that you determine analytically. 6.2 Write a Matlab routine that computes the trimmed state and controls for wings-level flight with V a = 10 m/s and γ = 0 rad. Execute your Aviones MAV simulation with the computed state as the initial condition and the computed controls as the input. If the trim algorithm is correct, the MAV states will remain constant during the simulation. Run the trim algorithm for various values of γ. The only variable that should change is the altitude h. Convince yourself that the climb rate is correct. 6.3 Write a Matlab routine that computes the trimmed state and controls for constant turns with with V = 10 m/s and R = 100 m. Execute your UAV simulation with the computed state as the initial condition and the computed controls as the input. If the trim algorithm is correct, the UAV states will remain constant during the simulation except for the heading ψ. 6.4 Write a Matlab routine that computes the trimmed state and controls for a pull-up maneuver with V = 10 m/s and R pullup = 100 m. Execute your UAV simulation with the computed state as the initial condition and the computed controls as the input. Note that the pull-up maneuver is only instantaneously in steady state. 6.5 Modify the constant-turn trim routine to include a climb rate ḣ. Execute for V = 10 m/s and R = 100 m and ḣ = 1 m/s. Execute your UAV simulation with the computed state as the initial condition and the computed controls as the input. If the trim algorithm is correct, the UAV states will remain constant during the simulation except for the heading ψ and the altitude h.

69 Chapter 7 Open-loop Linear Dynamics The objective of this chapter is to derive linear models of the open-loop dynamics. 7.1 Transfer Functions Lateral Transfer Functions In this section we will derive transfer function models for the lateral dynamics. From Equation (5.11) we have φ = p + q sin φ tan θ + r cos φ tan θ = p + d φ1. Differentiating and using Equation (5.14) we get φ = ṗ + d φ1 = Γ 1 pq Γ 2 qr + 1 [ ] 2 ρv a 2 bp br Sb C p0 + C pβ β + C pp + C pr + C pδ a 2V a 2V δ a + C δ pδ r r + d φ1 a = Γ 1 pq Γ 2 qr + 1 [ ] 2 ρv a 2 b Sb C p0 + C pβ β + C pp ( 2V φ br d φ1 ) + C pr + C pδ a a 2V δ a + C δ pδ r r + d φ1 ( ) ( ) a 1 = 2 ρv a 2 b 1 SbC pp φ + 2V a 2 ρv a 2 SbC pδ δ a a { + Γ 1 pq Γ 2 qr + 1 [ ] } 2 ρv a 2 b br Sb C p0 + C pβ β C pp (d φ1 ) + C pr + C pδr δ r + d 2V a 2V φ1 a = a φ1 φ + aφ2 δ a + d φ2, 59

70 Transfer Functions where 1 a φ1 = 2 ρv a 2 SbC pp a φ2 = 1 2 ρv 2 a SbC pδ a b 2V a d φ2 = Γ1 pq Γ 2 qr ρv 2 a Sb [ ] b br C p0 + C pβ β C pp (d φ1 ) + C pr + C pδr δ r 2V a 2V a + d φ1. In the Laplace domain we have ( φ(s) = A block diagram is shown in Figure 7.1. a φ2 s(s + a φ1 ) ) ( δ a (s) + 1 ) d φ2 (s). a φ2 Figure 7.1: Block diagram for roll dynamics. From Equation (5.19) we have that in a coordinated turn ψ = g tan φ. V a This equation can be rewritten as ψ = g φ + g (tan φ φ) V a V a = g φ + g d ψ, V a V a where d ψ = tan φ φ. Therefore the block diagram for the lateral dynamics is shown in Figure 7.2.

71 Open-loop Linear Dynamics 61 Figure 7.2: Block diagram for lateral dynamics Longitudinal Transfer Functions In this section we will derive a simple model for the longitudinal dynamics. From Equation (5.12) we have θ = q cos φ r sin φ = q + q(cos φ 1) r sin φ = q + d θ1, where d θ1 = q(cos φ 1) r sin φ. Differentiating we get θ = q + d θ1. Plugging in from Equation (5.15) and using the relationship θ = α + γ, where γ is the flight path angle, we get θ = J xz (r 2 p 2 ) + J z J x pr + 1 [ ] ρv 2 cq a cs C m0 + C mα α + C mq + C mδe δ e + d J y J y 2J y 2V θ1 a = J xz (r 2 p 2 ) + J z J x pr + 1 [ ] ρv 2 c a cs C m0 + C mα (θ γ) + C mq ( J y J y 2J y 2V θ d θ1 ) + C δ mδ e e a ( ) ( ) ( ) { 1 = ρva 2 c 1 1 csc mq θ + ρva 2 csc mα θ + ρv 2 Jxz a csc mδe δ e + (r 2 p 2 ) 2J y 2V a 2J y 2J y J y + J z J x pr + 1 [ ] } ρv 2 c a cs C m0 C mα γ C mq d θ1 + d J y 2J y 2V θ1 a = a θ1 θ aθ2 θ + a θ3 δ e + d θ2, + d θ1

72 Transfer Functions where 1 a θ1 = ρva 2 csc mq 2J y a θ2 = 1 2J y ρv 2 a csc mα a θ3 = 1 2J y ρv 2 a csc mδ e d θ2 c 2V a = J xz J y (r 2 p 2 ) + J z J x J y pr + 1 2J y ρv 2 a cs [ ] c C m0 C mα γ C mq d θ1 2V a + d θ1. We have therefore derived a linear model for the evolution of the pitch angle. Taking the Laplace transform we have ( a θ3 θ(s) = s 2 + a θ1 s + a θ2 ) ( δ e (s) + 1 ) d θ2 (s) a θ3 Note that in straight and level flight, r = p = φ = γ = 0 In addition, airframes are usually designed such that C m0 = 0, which implies that d θ2 = 0. In general however, we have the block diagram shown in Figure 7.3. Figure 7.3: Input disturbance form. Using the fact that θ = q + d θ1 we get the block diagram shown in Figure 7.4 From Equation (5.7) we have ḣ = u sin θ v sin φ cos θ w cos φ cos θ = V a θ + (u sin θ V a θ) v sin φ cos θ w cos φ cos θ = V a θ + d h, (7.1)

73 Open-loop Linear Dynamics 63 where Figure 7.4: Input disturbance form. d h = (u sin θ Va θ) v sin φ cos θ w cos φ cos θ. Note that in straight and level flight where v 0, w 0, u V a, φ 0 and θ is small we have d h 0. If we assume that V a is constant and θ is the input in Equation (7.1), then we have H(s) = V ) a (θ + 1Va d h, s and the resulting block diagram for the longitudinal dynamics is shown in Figure 7.5. Figure 7.5: Block diagram for longitudinal dynamics. Alternatively, if we consider θ as a constant and V a as the input, then the transfer function from V a to h is given by H(s) = θ ( V a + 1 ) s θ d h. The altitude controllers discussed in Chapter 8 will regulate altitude using both pitch angle and airspeed. Similarly, the airspeed will be regulated using both the throttle setting, as well as the pitch angle. To derive the appropriate transfer functions, note that V a = u 2 + v 2 + w 2 which implies that V a = u u + v v + wẇ V a.

74 Transfer Functions Using Equation (2.4) we get V a = u cos α cos β + v sin β + ẇ sin α cos β = u cos α + ẇ sin α + d V1, (7.2) where d V1 = u(1 cos β) cos α ẇ(1 cos β) sin α + v sin β. Note that when β = 0, we have d V 1 = 0. Substituting Equations (5.8) and (5.10) in Equation (7.2) we obtain V a = cos α {rv qw g sin θ + ρv [ ] a 2 S cq C X0 + C Xα α + C Xq + C Xδ e 2m 2V δ e a + ρs prop 2m C [ prop (kδt ) 2 ] } Va 2 + sin α {qu pv + g cos θ cos φ + ρv [ ]} a 2 S cq C Z0 + C Zα α + C Zq + C Zδe δ e + d V1. 2m 2V a Using Equations (2.4) and (4.4) and simplifying we get V a = rv a cos α sin β pv a sin α sin β where g cos α sin θ + g sin α cos θ cos φ + ρv [ ] a 2 S cq C D0 C Dα α C Dq C Dδ e 2m 2V δ e a + ρs prop 2m C [ prop (kδt ) 2 ] Va 2 + dv1 = (rv a cos α pv a sin α) sin β g sin(θ α) g sin α cos θ(1 cos φ) + ρv [ ] a 2 S cq C D0 C Dα α C Dq C Dδ e 2m 2V δ e a + ρs prop 2m C [ prop (kδt ) 2 ] Va 2 + dv1 = g sin γ + ρv [ ] a 2 S cq C D0 C Dα α C Dq C Dδe δ e 2m 2V a + ρs prop 2m C [ prop (kδt ) 2 ] Va 2 + dv2, (7.3) d V2 = (rv a cos α pv a sin α) sin β g sin α cos θ(1 cos φ) + d V1. Again note that in level flight d V2 = 0.

75 Open-loop Linear Dynamics 65 When considering airspeed V a, there are two inputs of interest: the throttle setting δ t and the flight path angle γ. Since Equation (7.3) is nonlinear in V a and delta t involving square terms that cannot be linearized using small angle approximations, we must first linearize Equation (7.3) before we can find the desired transfer functions. Equation (7.3) can be linearized around the wings-level, constant-altitude (ˆγ = 0) trim condition to give { V ρ a = (g cos ˆγ) γ + ˆV } a S ] [ C D0 C Dα ˆα C Dδe ˆδe ρs prop m m C ˆV prop a V a [ ] ρsprop + m C propk 2ˆδt δ t + d V where = a V1 V a + a V2 δ t gγ + d V, (7.4) a V1 = ρ ˆV a S m [ C D0 + C Dα ˆα + C Dδ e a V2 = ρs prop m C propk 2ˆδt, and d V includes d V2 as well as the linearization error. ] ˆδ e + ρs prop m C ˆV prop a The transfer functions from throttle and flight path angle inputs to the airspeed output are obtained by taking the Laplace transform of Equation (7.4) and manipulating to obtain the block diagram in Figure 7.6. Figure 7.6: Block diagram for airspeed dynamics. 7.2 Linear State Space Models In this section we will derive linear state space models for both longitudinal and lateral motion by linearizing Equations (5.5) (5.16) about trim conditions. The typical approach in the autopilot

76 Linear State Space Models literature is to separate the dynamics into longitudinal and lateral modes. Longitudinal modes are the dynamics in the x-z plane and are described the state variables h, V a, α, u, w, q, and θ. The lateral modes are the roll and pitch dynamics and are described by the state variables β, v, p, r, φ, and ψ. Section discusses general linearization techniques. Section derives the state space equations for the longitudinal dynamics and Section derives the state space equations for the lateral dynamics. Section describes numerical techniques for computing the state space equations from a nonlinear simulation model. Finally Section describes the reduced order modes including the short period mode, the phugoid mode, the dutch roll mode, and the spiral divergence mode Linearization The UAV dynamics can be represented by the general nonlinear system of equations ẋ = f(x, u), where x R 12 is the state, u R 4 is the control vector, and f is given by Equations (5.5) (5.16). Suppose that using the techniques discussed in Chapter 6, a trim input û and state ˆx are found such that Letting x = x ˆx we get x = ẋ ˆx ˆx = f(ˆx, û). = f(x, u) f(ˆx, û) = f(x + ˆx ˆx, u + û û) f(ˆx, û) = f(ˆx + x, û + ũ) f(ˆx, û). Taking the Taylor series expansion of the first term about the trim state we get f(ˆx, û) x = f(ˆx, û) + x + x f(ˆx, û) x + x f(ˆx, û) ũ + H.O.T f(ˆx, û) u f(ˆx, û) ũ. (7.5) u Therefore, the linearized dynamics are determined by finding f f and. x u

77 Open-loop Linear Dynamics Longitudinal State-space Equation From Equations (5.8), (5.10), (5.15), (5.12), and (5.7), we get u = rv qw g sin θ + ρv [ ] a 2 S cq C X0 + C Xα α + C Xq + C Xδ e 2m 2V δ e + ρs prop a 2m C prop ẇ = qu pv + g cos θ cos φ + ρv [ ] a 2 S cq C Z0 + C Zα α + C Zq + C Zδ e 2m 2V δ e a q = J xz (r 2 p 2 ) + J z J x pr + 1 [ ] ρv 2 cq a cs C m0 + C mα α + C mq + C mδe δ e J y J y 2J y 2V a θ = q cos φ r sin φ ḣ = u sin θ v sin φ cos θ w cos φ cos θ. [ (kδt ) 2 ] Va 2 The first step is to assume that all of the lateral states are zero, i.e., set φ = p = r = β = v = 0 and substitute ( w ) α = tan 1 u V a = u 2 + w 2 from Equation (2.4) to get u = qw g sin θ + ρ(u2 + w 2 )S 2m + ρ u 2 + w 2 S 4m ẇ = qu + g cos θ + ρ(u2 + w 2 )S 2m + ρ u 2 + w 2 S [ ( w ) ] C X0 + C Xα tan 1 + C Xδ e u δ e C Xq cq + ρs prop 2m C [ prop (kδt ) 2 (u 2 + w 2 ) ] (7.6) [ ( w ) ] C Z0 + C Zα tan 1 + C Zδe δ e u C Zq cq (7.7) 4m q = 1 [ ( w ) ] ρ(u 2 + w 2 ) cs C m0 + C mα tan 1 + C mδ e 2J y u δ e + 1 ρ u 4J 2 + w 2 SC mq c 2 q (7.8) y θ = q (7.9) ḣ = u sin θ w cos θ. (7.10)

78 Linear State Space Models The next step is to linearize Equations (7.6) (7.10) according to Equation (7.5) where f x = f u = Toward that end, note that ( w ) u tan 1 u ( w ) w tan 1 u u u ẇ u q u θ u ḣ u u δ e ẇ δ e q δ e θ δ e ḣ δ e u w ẇ w q w θ w ḣ w u δ t ẇ δ t q δ t θ δ t ḣ δ t = w2 u 2 = w2 u 2 u q ẇ q q q θ q ḣ q. ( ) w u 2 ( ) 1 u u θ ẇ θ q θ θ θ ḣ θ u h ẇ h q h θ h ḣ h = w u 2 + w = w 2 Va 2 u = u 2 + w = u, 2 V 2 where we have used Equation (2.5) and the fact that v = 0. Working out the derivatives we get that the linearized state space equations are ũ X u X w X q g cos ˆθ 0 ũ X δe X δt w Z u Z w Z q g sin ˆθ 0 w Z δe 0 ( ) q = M u M w M q 0 0 q + M δe 0 δe δ t, θ θ 0 0 h sin ˆθ cos ˆθ 0 û cos ˆθ + ŵ sin ˆθ 0 h 0 0 where the coefficients are given in Table 7.1. have The longitudinal equations are often given in terms of α instead of w. From Equation (2.4) we w = V a sin α cos β = V a sin α, where we have used the assumption that β = 0. Linearizing around α = ˆα we get w = ˆV a cos ˆα α, a which implies that α = 1 ˆV a cos ˆα w.

79 Open-loop Linear Dynamics 69 Longitudinal X u X w X q X δe X δt Z u Z w Z q Z δe M u M w M q M δe ûρs m ˆq + ŵρs m Formula [ ] C X0 + C Xα ˆα + C Xδ ˆδ e e ρsŵc Xα 2m ] [C X0 + C Xα ˆα + C Xδe ˆδe + ρs cc Xq ŵˆq 4m ˆV a ŵ + ρ ˆV a SC X q c 4m ρ ˆV a 2SC X δ e 2m ρs prop C prop k 2ˆδ t m + ρs cc Xq ûˆq 4m ˆV a [ ] ˆq + ûρs C m Z0 + C Zα ˆα + C Zδ ˆδ e e ρsc Zαŵ 2m ] [C Z0 + C Zα ˆα + C Zδe ˆδe ŵρs m [ ûρs c J y û + ρ ˆV asc Zq c 4m ρ ˆV 2 a SC Z δe 2m C m0 + C mα ˆα + C mδe ˆδe ] ŵρs c J y [C m0 + C mα ˆα + C mδ e ρ ˆV a S c 2 C mq 4J y ρ ˆV a 2 S cc mδ e 2J y + ρsc Zαû 2m ρs cc mαŵ 2J y ˆδ e ] + ρs ccm αû 2J y + ρsc Xαû 2m ρs propc prop û m + ûρsc Zq cˆq 4m ˆV a + ρŵs cc Zq ˆq 4m ˆV a + ρs c2 C mq ˆqû 4J y ˆVa + ρs c2 C m q ˆqŵ 4J y ˆVa ρs propc prop ŵ m Table 7.1: Longitudinal Coefficients State Space Models.

80 Linear State Space Models Therefore we can write the the state space equations in terms of α instead of w: ũ X u X w ˆVa cos ˆα X q g cos ˆθ 0 ũ X δe X δt α Z u Z Z q g sin ˆθ Z ˆV a cos ˆα w 0 α δ e 0 ) ˆV a cos ˆα ˆV a cos ˆα ˆV a cos ˆα ( δe q = M u M w ˆVa cos ˆα M q 0 0 q + M δe 0. δ t θ θ 0 0 h sin ˆθ ˆV a cos ˆθ cos ˆα 0 û cos ˆθ + ŵ sin ˆθ 0 h 0 0 (7.11) Lateral State-space Equation From Equations (5.9), (5.14), (5.16), (5.11), and (5.13), we get v = pw ru + g cos θ sin φ + ρ u 2 + v 2 + w 2 S b [ ] C Yp p + C Yr r 2m 2( ) + ρ(u2 + v 2 + w 2 )S [C Y0 + C Yβ tan 1 v ] + C 2m u2 + w 2 Yδa δ a + C Yδr δ r ṗ = Γ 1 pq Γ 2 qr + ρ u 2 + v 2 + w 2 S b 2 [ ] C pp p + C pr r ( ) 2 ρ(u2 + v 2 + w 2 )Sb [C p0 + C pβ tan 1 v ] + C u2 + w δ 2 pδ a a + C δ pδ r r ṙ = Γ 7 pq Γ 1 qr + ρ u 2 + v 2 + w 2 S b 2 [ ] C rp p + C rr r ( ) 2 ρ(u2 + v 2 + w 2 )Sb [C r0 + C rβ tan 1 v ] + C u2 + w 2 rδa δ a + C rδr δ r (7.12) (7.13) (7.14) φ = p + q sin φ tan θ + r cos φ tan θ (7.15) ψ = q sin φ sec θ + r cos φ sec θ, (7.16) where we have used the expressions ( ) β = tan 1 v u2 + w 2 V a = u 2 + v 2 + w 2.

81 Open-loop Linear Dynamics 71 The next step is to linearize Equations (7.12) (7.16) according to Equation (7.5) where f x = f u = v v ṗ v ṙ v φ v ψ v v δ a ṗ δ a ṙ δ a φ δ a ψ δ a v p ṗ p ṙ p φ p ψ p v δ r ṗ δ r ṙ δ r φ δ r ψ δ r v r ṗ r ṙ r φ r ψ r. v φ ṗ φ ṙ φ φ φ ψ φ v ψ ṗ ψ ṙ ψ φ ψ ψ ψ Toward that end, note that v tan 1 ( ) v = u2 + w 2 u2 + w 2 u 2 + v 2 + w 2 = u2 + w 2. Working out the derivatives we get that the linearized state space equations are ṽ Y v Y p Y r g cos ˆθ cos ˆφ 0 ṽ Y δa Y δr p L v L p L r 0 0 p L δa L δr ( ) r = N v N p N r 0 0 φ 0 1 cos ˆφ tan ˆθ ˆq cos ˆφ tan ˆθ ˆr sin ˆφ tan ˆθ r + N δa N δr δa δ r, 0 φ 0 0 ψ 0 0 cos ˆφ sec ˆθ ˆp cos ˆφ sec ˆθ ˆr sin ˆφ sec ˆθ 0 ψ 0 0 where the coefficients are given in Table 7.2. The lateral equations are often given in terms of β instead of ṽ. From Equation (2.4) we have v = V a sin β. V 2 a Linearizing around β = ˆβ we get ṽ = ˆV a cos ˆβ β, which implies that β = 1 ˆV a cos ˆβ ṽ.

82 Linear State Space Models Lateral Y v Y p Y r Y δa Y δr L v [ ] ρsbˆv 4m ˆV C Yp ˆp + C Yr ˆr a [ ρsb 2ˆv 4 ˆV a ] C pp ˆp + C pr ˆr + ρsˆv m [ + ρsbˆv Formula ] [C Y0 + C Yβ ˆβ + CYδa ˆδa + C Yδr ˆδr ŵ + ρ ˆV a Sb C 4m Y p û + ρ ˆV asb C 4m Y r ρ ˆV a 2 S C 2m Y δ a ρ ˆV a 2 S C 2m Y δr C p0 + C pβ ˆβ + Cpδ a ˆδ a + C pδ r L p Γ 1ˆq + ρ ˆV asb 2 C 4 pp L r Γ 2ˆq + ρ ˆV asb 2 L δa L δr N v [ ρsb 2ˆv 4 ˆV a ] C rp ˆp + C rr ˆr [ + ρsbˆv C 4 pr ρ ˆV a 2 Sb C 2 pδ a ρ ˆV a 2 Sb C 2 pδr C r0 + C rβ ˆβ + Crδ a ˆδ a + C rδ r N p Γ 7ˆq + ρ ˆV a Sb 2 C 4 rp N r Γ 1ˆq + ρ ˆV a Sb 2 N δa N δr C 4 rr ρ ˆV a 2 Sb C 2 rδa ρ ˆV a 2Sb C 2 rδ r + ρsc Y û2 β + ŵ 2m 2 ] ˆδ r + ρsbcp û2 β + ŵ 2 2 ] ˆδ r + ρsbcr û2 β + ŵ 2 2 Table 7.2: Lateral Coefficients State Space Models.

83 Open-loop Linear Dynamics 73 Therefore we can write the the state space equations in terms of β instead of ṽ: Y Y p v ˆV a cos β ˆβ Y r g cos ˆθ cos ˆφ ˆV a cos ˆβ ˆV a cos ˆβ 0 L v ˆVa cos ˆβ L p L r 0 0 β p N v ˆVa cos ˆβ N p N r 0 0 p r = 0 1 cos ˆφ tan ˆθ ˆq cos ˆφ tan ˆθ φ ˆr sin ˆφ tan ˆθ r + 0 ψ 0 0 cos ˆφ sec ˆθ ˆp cos ˆφ sec ˆθ φ ψ ˆr sin ˆφ sec ˆθ 0 Y δ a ˆV a cos ˆβ L δa N δa Y δ r ˆV a cos ˆβ L δr N δr ( ) δa δ r. (7.17) Numerical Approximations of A and B. Perhaps the best way to find A and B is to approximate f x f can be approximated as x and f u numerically. The ith column of f 1 x i f 2 x i. f n x i (ˆx, û) f(ˆx + ɛe i, û) f(ˆx, û), ɛ where e i has a one in the i th element and zeros elsewhere. Similarly, the i th column of f u approximated as f 1 u i f 2 u i. f n u i (ˆx, û) f(ˆx, û + ɛe i) f(ˆx, û). ɛ can be

84 Linear State Space Models Reduced Order Modes Short Period Mode If we assume a constant altitude and a constant thrust input, then we can simplify the longitudinal state space model (7.11) to ũ X u X w ˆVa cos ˆα X q g cos ˆθ ũ α q = Z u Z Z q g sin ˆθ ˆV a cos ˆα w ˆV a cos ˆα ˆV a cos ˆα α M u M w ˆVa cos α M q 0 q + θ θ X δe Z δe ˆV a cos ˆα M δe 0 0 δ e. (7.18) If we compute the eigenvalues of the A matrix, we will find that there is one fast, damped mode and one slow, lightly damped mode. The fast mode is called the short period mode. The slow, lightly damped mode is called the phugoid mode. In the next two subsections, we will approximate the poles due to these modes. For the short period mode, we will assume that u is constant, i.e., ũ = ũ = 0. Therefore the state space equations in Equation (7.18) can be written as α = Z w α + Z q θ ˆV a cos ˆα θ = M w ˆVa cos ˆα α + M q θ, g sin ˆθ θ + ˆV a cos ˆα Z δ e δ e ˆV a cos ˆα where we have substituted q = θ. Taking the Laplace transform of these equations gives ( ) ( ) ( s Z w Zqs g sin ˆθ Zδe ) + α(s) ˆV a cos ˆα ˆV a cos ˆα = ˆV a cos ˆα δ M w ˆVa cos ˆα s 2 e (s), M q s θ(s) 0 which implies that ( ) α(s) = θ(s) ( s 2 M q s Z q s g sin ˆθ ˆV a cos ˆα ˆV a cos ˆα M w ˆVa cos ˆα s Z w ( ) (s 2 M q s)(s Z w ) + M w ˆVa cos ˆα Zqs g sin ˆθ + ˆV a cos ˆα ˆV a cos ˆα ) ( Zδ ) e ˆV a cos ˆα δ e (s). 0 Assuming that we are linearized around level flight, i.e., that ˆθ = 0, the characteristic equation becomes s ( s 2 + ( Z w M q ) s + M q Z w M w Z q ) = 0. Therefore, the short period poles are approximately equal to λ short = Z (Zw ) 2 w + M q + M q ± M q Z w + M w Z q. 2 2

85 Open-loop Linear Dynamics 75 Phugoid Mode Assume that α is constant, i.e., α = α = 0, then α = ˆα and Equation (7.18) becomes X u X w ˆVa sin ˆα X q g cos ũ ˆθ Z u Z Z q g sin ˆθ ũ 0 q = ˆV a cos ˆα w ˆV a cos ˆα ˆV a cos ˆα M u M w ˆVa cos ˆα M q 0 0 q θ sin ˆθ ˆV a cos ˆθ cos ˆα 0 û cos ˆθ + ŵ sin ˆθ θ 0 Taking the Laplace transform of the first two equations gives ( s X u X q s + g cos ˆθ ) ( ) ( ũ(s) Z u Z q s + g sin ˆθ = θ(s) Again assuming that ˆθ = 0, we get that the characteristic equation is given by ( ) s 2 Zu X q X u Z q + s gz u = 0. Z q The poles of the phugoid mode are approximately given by λ phugoid = Z (Zu ) 2 ux q X u Z q X q X u Z q ± + gz u. 2Z q 2Z q Z q Rolling Mode Z q X δe Z δe ) δ e. X δe Z δ e ˆV a cos ˆα M δe 0 δ e. If we ignore the heading dynamics, and assume a constant pitch angle, i.e., θ = 0, then Equation (7.17) becomes Y β Y p v ˆV a cos p ˆβ Y r ˆV a cos ˆβ r = L v ˆVa cos ˆβ L p L r 0 N v ˆVa cos ˆβ N p N r 0 φ g cos ˆθ cos ˆφ ˆV a cos ˆβ β p r + φ The dynamics for p are obtained from Equation (7.19) as Y δ a ˆV a cos ˆβ L δa N δa p = L v ˆVa cos ˆβ β + L p p + L r r + L δa δa + L δr δr. The rolling mode is obtained by assuming that β = r = δ r = 0: p = +L p p + L δa δa. Y δ r ˆV a cos ˆβ L δr N δr 0 0 ) ( δa. (7.19) δ r

86 Linear State Space Models The transfer function is therefore p(s) = L δ a δa (s). s L p An approximation of the eigenvalue for the rolling mode is therefore given by λ rolling = L p. Spiral Mode For the spiral mode we assume that p = p = 0, and that the rudder command is negligible. Therefore, from the second and third equations in (7.19) we get 0 = L v ˆVa cos ˆβ β + L r r + L δa δa (7.20) r = N v ˆVa cos ˆβ β + N r r + N δa δa. (7.21) Solving (7.20) for β and substituting into (7.21) we obtain ( ) ( ) Nr L v N v L r Nδa L v N v L δa r = r + δ a. L v L v In the frequency domain we have r(s) = ( ) Nδa L v N v L δa L v ( s N r L v N v L r L v Therefore the pole of the spiral mode is approximately Dutch Roll Mode ) δa (s). λ spiral = N rl v N v L r L v. For the dutch roll mode, we neglect the rolling motions and focus on the equations for sideslip and yaw. From Equation (7.19) we have ( ) ( β Y v = r N v ˆVa cos ˆβ The characteristic equation is given by ( ( )) det si Y v N v ˆVa cos ˆβ Y r ˆV a cos ˆβ N r Y r ˆV a cos ˆβ N r ) ( ) β + r ( Yδ ) r ˆV a cos ˆβ δ r. N δr = s 2 + ( Y v N r )s + (Y v N r N v Y r ) = 0. Therefore, the poles so the dutch roll mode are approximated by λ dutch roll = Y (Yv ) 2 v + N r + N r ± (Y v N r N v Y r ). 2 2

87 Open-loop Linear Dynamics 77 Example Insert examples.

88 Design Project 7.3 Design Project 7.1 Using Matlab calculate values for the coefficients listed in Table 7.1. Use the coefficient values to create a state-space representation for the longitudinal dynamics of the system according to Equation (7.11). Use trim values corresponding to wings-level, constant-altitude flight at 15 m/s. Calculate the eigenvalues of this system. Identify the phugoid and short period modes of the system. 7.2 Using Matlab calculate values for the coefficients listed in Table 7.2. Use the coefficient values to create a state-space representation for the lateral-directional dynamics of the system according to Equation (7.17). Use trim values corresponding to a constant-altitude turn of 100 m radius at 15 m/s. Calculate the eigenvalues of this system. Identify the roll, spiral, and dutch roll modes of the system. 7.3 Using the information in Section calculate approximations for the phugoid and shortperiod modes of the system. How do them compare with your earlier results? 7.4 Calculate approximations for the roll, spiral, and dutch roll modes of the system. How do these approximations compare with the eigenvalues of the state-space representation obtained earlier? 7.5 Bonus Problem: Calculate state-space representations for the longitudinal and lateral-directional dynamics using the numerical approach presented in Section Compare these results to those obtained from Equations (7.11) and (7.17). Hint: You have much of the work done already from the trim calculations you did in the previous assignment.

89 Chapter 8 Autopilot Design Using Successive Loop Closure A common technique for autopilot design is a method called successive loop closure. The basic idea is that one loop is closed at a time. At each stage of the design, there is only one gain to tune. Therefore, root locus techniques are typically used to design autopilots using successive loop closure. 8.1 Lateral Autopilot Figure 8.1 shows the block diagram for a lateral autopilot using successive loop closure. There are five gains associated with the lateral autopilot. The derivative gain k dφ provides roll rate damping at the innermost loop. The roll attitude is regulated with the proportional and integral gains k pφ and k iφ. The yaw attitude is regulated with the proportional and integral gains k pψ and k iψ. The idea with successive loop closure, is that the gains are successively chosen one at a time beginning with the inner loop and working outward. In particular, k dφ is usually selected first, k pφ second, k iφ third, k pψ fourth, and k iψ last Roll Attitude Loop Design The two inner loops of the lateral-directional autopilot are used to control roll angle and roll rate as shown in Figure 8.2. If a φ1 and a φ2 are known, is there a systematic method for selecting the control gains k dφ and k pφ? 79

90 Lateral Autopilot Figure 8.1: Autopilot for lateral control using successive loop closure. Figure 8.2: Pitch attitude hold control loops. From Figure 8.2, the transfer function from φ c to φ is given by H φ/φ c(s) = k pφ a φ2 s 2 + (a φ1 + a φ2 k dφ )s + k pφ a φ2. Notice that the DC gain is equal to one, and that the closed loop poles are located at ( ) (aφ1 ) aφ1 + a φ2 k dφ + a 2 φ2k dφ ± k pφ 2 2 a φ2. If the desired response is given by the canonical second order transfer function then equating terms we get ω 2 n φ s 2 + 2ζ φ ω nφ s + ω 2 n φ, ω 2 n φ = k pφ a φ2 2ζ φ ω nφ = a φ1 + a φ2 k dφ.

91 Autopilot Design Using Successive Loop Closure 81 Solving these expressions for k pφ and k dφ we get k pφ = ω2 n φ a φ2 k dφ = 2ζ φ ω n φ a φ1 a φ2 Therefore, selecting the desired damping ratio and natural frequency fixes the value for k pφ k dφ. and Integrator on Roll Note that the open loop transfer function in Figure 8.2 is a type one system, which implies that zero steady state tracking error in roll should be achievable without an integrator. However, from Figure 7.1 we see that there is a disturbance that enters at the summing junction before δ a. Figure 8.3 shows the roll loop with the disturbance. Solving for φ(s) in Figure 8.3 we get Figure 8.3: Roll attitude hold loop with input disturbance. ( 1 φ = s 2 + (a φ1 + a φ2 k dφ )s + a φ2 k pφ ) ( d φ2 + a φ2 k dφ s 2 + (a φ1 + a φ2 k dφ )s + a φ2 k pφ ) φ c. Note that if d φ2 is a constant disturbance, i.e., d φ2 = A/s, then from the final value theorem, the A steady state error due to d φ2 is a φ2 k pφ. In a constant orbit, p, q, and r will be constants and so d φ2 will also be constant. Therefore, it is desirable to remove the steady state error using an integrator. Figure 8.4 shows the roll attitude hold loop with an integrator added to reject a constant d φ2. Solving for φ(s) in Figure 8.4 we get ( ) ( ) s a φ2 k pφ s + k i φ k φ = d s 3 + (a φ1 + a φ2 k dφ )s 2 φ2 + pφ φ c. + a φ2 k pφ s + a φ2 k iφ s 3 + (a φ1 + a φ2 k dφ )s 2 + a φ2 k pφ s + a φ2 k iφ

92 Lateral Autopilot Figure 8.4: Integrator for roll attitude hold. Note that in this case, the final value theorem predicts zero steady state error for a constant d φ2. If d φ2 is a ramp, i.e., d φ2 = A/s 2, then the steady state error is given by A a φ2 k iφ. If a φ1 and a φ2 are known, then k iφ can be effectively selected using root locus techniques. The closed loop poles of the system are given by which can be placed in Evans form as 1 + k iφ ( Yaw Attitude Hold s 3 + (a φ1 + a φ2 k dφ )s 2 + a φ2 k dφ s + a φ2 k iφ = 0, a φ2 s ( s 2 + (a φ1 + a φ2 k dφ )s + a φ2 k dφ ) The objective of the yaw attitude hold design is to select k pψ and k iψ in Figure 8.1 so that the yaw angle ψ asymptotically tracks steps in the commanded yaw angle ψ c. One method for selecting k pψ is to assume that the inner loop from φ c to φ has been adequately tuned so that H φ/φ c ) = This assumption is valid at frequencies below the designed natural frequency of the inner feedback loop ω nφ. Under this assumption, the block diagram of Figure 8.1 can be simplified to the block diagram in Figure 8.5. Accordingly, the transfer functions from the inputs ψ c and d ψ to the output ψ are given by ψ = g/v a s d s 2 ψ + k p ψ g/v a s + k iψ g/v a ψ c. (8.1) + k pψ g/v a s + k iψ g/v a s 2 + k pψ g/v a s + k iψ g/v a Note that if d ψ and ψ c are constants, then the final value theorem implies that ψ ψ c. The transfer function from ψ c to ψ has the form H ψ = 2ζ ψ ω n ψ s + ω 2 n ψ s 2 + 2ζ ψ ω nψ s + ω 2 n ψ. (8.2)

93 Autopilot Design Using Successive Loop Closure 83 Figure 8.5: Yaw attitude hold outer feedback loop. As with the inner feedback loops, we can choose the natural frequency and damping of the outer loop and from those values calculate the feedback gains k pψ and k iψ. Figure 8.6 shows the frequency response and the step response for H ψ. Note that because of the zero the standard intuition for ζ does not hold for this transfer function. Larger ζ results in larger bandwidth and smaller overshoot. Figure 8.6: Frequency and step response for second order system. Comparing coefficients in Equations (8.1) and (8.2), we find ω 2 n ψ = g/v a k iψ 2ζ ψ ω nψ = g/v a k pψ. Solving these expressions for k pψ and k iψ we get k pψ = 2ζ ψ ω nψ V a /g k pψ = ω 2 n ψ V a /g.

94 Lateral Autopilot To ensure proper function of this successive-loop-closure design, it is essential that there be sufficient bandwidth separation between the inner (φ) and outer (ψ feedback loops. Adequate separation can be achieved using the rule of thumb ω nφ > 5ω nψ. Generally, more bandwidth separation is better. More bandwidth separation requires either slower response in the ψ loop (lower ω nψ, or faster response in the φ loop (higher ω nφ ). Faster response usually comes at the cost of requiring more actuator control authority, which may not be possible given the physical constraints of the actuators Aileron Actuator Limits It is important to understand how the saturation constraints on δ a limit the achievable performance. From basic control theory, we know that the transfer function from the input to the output of a block diagram is the forward path gain divided by one plus the sum of the nested loop gains. Applying this approach to the block diagram of Figure 8.1 we see that the transfer function from ψ c to δ a is given by δ a ψ = s 2 (k pψ s + k iψ )(k pφ s + k iφ )(s + a φ1 ). c (s + a φ1 + k dφ a φ2 )s 4 + (k pφ s + k iφ )a φ2 s 2 + (k pφ s + k iφ )(k pφ s + k iφ )a φ2 g/v a Applying the initial value theorem, we can evaluate the instantaneous response of the aileron deflection in response to a step in the yaw command δ a ψ c (t = 0) = k p ψ k pφ. This gain can be expressed in terms of the parameters used to select the control gains as δ a ψ (t = 0) = ω2 n phi (2ζ c ψ ω nψ V a /g). a φ2 Defining the frequency separation between the two loops as we get f lat = ω nφ ω nψ δ a ψ c (t = 0) = 2ζ ψ V a a φ2 g f 2 latω 3 n ψ. In this particular case, the initial control effort required for a step change in the input is sensitive to the frequency spread and the bandwidth of the closed loop control. The control effort changes with

95 Autopilot Design Using Successive Loop Closure 85 the square of f lat and the cube of ω nψ. Thus, a relatively small increase in bandwidth can require significantly more control effort. Large inputs in the commanded yaw will result in large aileron deflection commands beyond physical limits of the actuator. To prevent damage to hardware it is common to employ software saturation limits on the actuator command. In the case of the yaw attitude control loop it is also common to place a saturation limit on the commanded roll angle. This prevents an excessively large roll angle command in response to large changes in the yaw command. 8.2 Longitudinal Autopilot The longitudinal autopilot is more complicated than the lateral autopilot because airspeed plays a significant role in the longitudinal dynamics. Our objective in designing the longitudinal autopilot will be to regulate airspeed and altitude using the throttle and the elevator as actuators. The method used to regulate altitude and airspeed depends on the altitude error. The flight regimes are shown in Figure 8.7 Figure 8.7: Flight regimes for the longitudinal autopilot In the take-off zone, full throttle is commanded and the pitch attitude is regulated to a fixed pitch angle θ d using the elevator. The objective in the climb zone is to maximize the climb rate given the current atmospheric conditions. To maximize the climb rate, full throttle is commanded and the airspeed is regulated using the pitch angle. If the airspeed increases above its nominal

96 Longitudinal Autopilot value, then the aircraft if caused to pitch up which results in an increased climb rate and a decrease in airspeed. Similarly, if the airspeed drops below the nominal value, the airframe is pitched down thereby increasing the airspeed but also decreasing the climb rate. Regulating the airspeed using pitch attitude effectively keeps the airframe away from stall conditions. Note however, that we would not want to regulate airspeed with pitch attitude immediately after take-off because after take-off the airframe is always trying to gain airspeed, however, pitching down will drive the aircraft into the ground. The descend zone is similar to the climb zone except that the throttle is commanded to zero. Again, stall conditions are avoided by regulating airspeed using the pitch angle thus maximizing the descent rate at a given airspeed. In the altitude hold zone, the airspeed is regulated by adjusting the throttle, and the altitude is regulated by commanding the pitch attitude. To implement the longitudinal autopilot shown in Figure 8.7 we need the following feedback loops: (1) pitch attitude hold using elevator, (2) airspeed hold using throttle, (3) airspeed hold using pitch attitude, and (4) altitude hold using pitch attitude. The design of each of these loops will be discussed in the next four subsections. Finally, the complete longitudinal autopilot will be presented in Section Pitch Attitude Hold The pitch attitude hold loop is similar to the roll attitude hold loop and we will follow a similar line of reasoning in its development. From Figure 8.8, the transfer function from θ c to θ is given by is given by k pθ a θ3 H θ/θ c(s) = s 2 + (a θ1 + k dθ a θ3 )s + (a θ2 + k pθ a θ3 ). (8.3) Note that in this case, the DC gain is not equal to one. Figure 8.8: Pitch attitude hold feedback loops. If the desired response is given by the canonical second order transfer function K θdc ω 2 n θ s 2 + 2ζ θ ω nθ s + ω 2 n θ,

97 Autopilot Design Using Successive Loop Closure 87 then equating denominator coefficients we get ω 2 n θ = a θ2 + k pθ a θ3 2ζ θ ω nθ = a θ1 + k dθ a θ3. Solving these expressions for k pθ and k dθ we get k pθ k dθ = ω2 n θ a θ2 a θ3 = 2ζ θ ω n θ a θ1 a θ3 Therefore, selecting the desired damping ratio and natural frequency fixes the value for k pθ k dθ. The DC gain of this inner-loop transfer function approaches one as the k pθ is given by K θdc = k pθ a θ3 (a θ2 + k pθ a θ3 ) and. The DC gain which for typical gain values is significantly less than one. For the design of the outer loop, we will use this DC gain to represent the gain of the inner loop over its full bandwidth. An integral feedback term could be employed to ensure unity DC gain on the inner loop. The addition of an integral term, however, can severely limit the bandwidth of the inner loop. For this reason, we have chosen not to use integral control on the pitch loop Altitude Hold Using Commanded Pitch The altitude hold autopilot utilizes a successive loop closure strategy with the pitch attitude hold autopilot as an inner loop as shown in Figure 8.9. Assuming that the pitch loops function as designed and θ K θdc θ d, the altitude hold loop using the commanded pitch is shown in Figure 8.10 In the Laplace domain we have ) K θdc V a k ph (s + k i h ( k h(s) = ph h d s (s) + s 2 + K θdc V a k ph s + K θdc V a k ih s 2 + K θdc V a k ph s + K θdc V a k ih ) d h (s), where again we see that the DC gain is equal to one, and constant disturbances are rejected. The closed loop transfer function is again independent of airframe parameters and is only dependent on the known airspeed. The gains k ph and k ih should be chosen such that the bandwidth of the

98 Longitudinal Autopilot Figure 8.9: Successive loop feedback structure for altitude hold autopilot. Figure 8.10: The altitude hold loop using the commanded pitch angle. altitude using pitch loop is less than the bandwidth of the pitch attitude hold loop. A good rule of thumb is that it at least is four times less. If the desired response of the altitude hold loop is given by the canonical second order transfer function then equating denominator coefficients we get ω 2 n h s 2 + 2ζ h ω nh s + ω 2 n h, Solving these expressions for k ih and k ph we get ω 2 n h = K θdc V a k ih 2ζ h ω nh = K θdc V a k ph. k ih = ω2 n h K θdc V a k ph = 2ζ h ω n h K θdc V a. Therefore, selecting the desired damping ratio and natural frequency fixes the value for k ph k ih. and

99 Autopilot Design Using Successive Loop Closure Airspeed Hold Using Commanded Pitch The dynamic model for airspeed using flight path angle is shown in Figure??. Disturbance rejection again requires a PI controller. The resulting block diagram is shown in Figure Figure 8.11: PI Controller to regulate airspeed using the pitch angle. In the Laplace domain we have ( gk pv2 )(s + k i V2 ( ) k pv2 ) g V a (s) = s 2 V d a a (s) + V2 d + (a V1 gk pv2 )s gk iv2 s 2 V4 (s). + (a V1 gk pv2 )s gk iv2 Note that the DC gain is equal to one and that step disturbances are rejected. The angle of attack α enters into the block diagram at the same location as d V4. If we assume that the angle of attack is constant, then the PI controller winds up the integrator to reject α. Therefore the output of the PI controller can be used as the input to the pitch attitude hold loop. It is interesting to note that the closed loop transfer function is independent of airframe parameters. The gains k pv2 and k iv2 should be chosen such that the bandwidth of the airspeed to pitch loop is less than the bandwidth of the pitch attitude hold loop. A good rule of thumb is that it is four times less Airspeed Hold Using Throttle The dynamic model for airspeed using the throttle as an input is shown in Figure 7.6. Therefore, the associated closed loop system is shown in Figure 8.12 If we use a proportional control, i.e. C(s) = k pv, then ( V a = a V2 k pv s + (a V1 + a V2 k pv ) ) ( Va d + 1 s + (a V1 + a V2 k pv ) ) d V.

100 Longitudinal Autopilot Figure 8.12: Airspeed hold using throttle. Note that the DC gain is not equal to one, and that step disturbances are not rejected. If on the other hand we use proportional-integral control, i.e., C(s) = k pv ( a V2 (k pv s + k iv ) V a = s 2 + (a V1 + a V2 k pv )s + a V2 k iv ) ( Va d + + k i V s, then 1 s 2 + (a V1 + a V2 k pv )s + a V2 k iv ) d V. It is clear that using a PI controller results in a DC gain of one, with step disturbance rejection. If a V1 and a V2 are known, then the gains k pv and k iv can be effectively determined using, for example, root locus techniques. Note that since V a = Va ˆV a and V d a = V d a ˆV a, then V d a V a = V d a V a. Therefore, the control loop shown in Figure 8.12 can be implemented without knowledge of the trim velocity ˆV a. Similarly, since the integrator will wind up to reject step disturbances, and a constant error in ˆδ t can be thought of as a step disturbance, we can set δ t = δ t Altitude Hold The feedback loops described in the previous four sections can be combined to create the altitude hold algorithm shown in Figure In the climb zone, the throttle is set to its maximum value (δ t = 1) and the pitch angle is used to control the airspeed, thus ensuring that the airframe avoids stall conditions. Similarly, in the descend zone, the throttle is set to its minimum value (δ t = 0) and the pitch angle is again used to control airspeed. In the altitude hold zone, the throttle is used to regulate the airspeed around V d a, and the pitch angle is used to regulate the altitude.

101 Autopilot Design Using Successive Loop Closure 91 Descend zone Altitude hold zone Climb zone Figure 8.13: This figure shows the hybrid control loop used for altitude hold Autonomous Take-off and Landing Autonomous Take-off The climb mode described in the previous section does not work for take-off mode. When the airspeed is low, the climb mode will pitch the nose of the airframe down to pick up speed. In takeoff mode, the airspeed is below the desired airspeed, but the desire is to climb out at a constant pitch angle, which should be above the angle-of-attack needed for level flight, and below the stall angle. The appropriate take-off routine is shown in Figure Autonomous Landing The landing algorithm developed and tested for this project requires two user-defined waypoints: the approach point and the landing point, as shown in Figure??. The MAV orbits around the

102 Longitudinal Autopilot Take-off zone Figure 8.14: Autonomous take-off routine. approach point as it descends to a specified altitude. At this end altitude, it breaks out of its orbit and follows a glide slope to the landing point. The approach point is fully defined by the following parameters: 1. Airspeed - The airspeed during the spiral-down portion of the landing. 2. Radius - The radius of the orbit during the spiral-down portion of the landing 3. Start Altitude - The altitude to which the MAV climbs or descends while en route to the approach point. The true start altitude for the descent will be the altitude to which the MAV has successfully climbed or descended by the time it is within 1.5 orbit radii of the landing point. 4. End Altitude - The altitude at which the MAV breaks out of its orbit and begins its glide slope to the landing point 5. Relative East Coordinate - The distance east of home around which the MAV orbits while descending from its start altitude to its end altitude. 6. Relative North Coordinate - The distance north of home around which the MAV orbits while descending from it start altitude to its end altitude. 7. Descent Rate - The rate at which the MAV descends while orbiting the approach point. The landing point is fully defined by the following parameters: 1. Airspeed - The airspeed during the glide slope from approach point to landing point. 2. Flare Height - The height above ground at which the MAV cuts the throttle and attempts to hold zero roll while maintaining its glide slope by commanding pitch to control around desired altitude.

103 Autopilot Design Using Successive Loop Closure Relative East Coordinate - The distance east of home at which the MAV will touch down. 4. Relative North Coordinate - The distance north of home at which the MAV will touch down. Spiral Down from 100 meters Altitude (meters) landing point approach point Distance North of Home (meters) Distance East of Home (meters) Figure 8.15: Landing spiral approach point and landing point. Altitude control throughout the landing routine is accomplished through activation of several PID loops. During the portion of the landing sequence that the MAV is en route from its last waypoint to the approach point, altitude is maintained according to the following criteria: 1. If the actual altitude is within a preset distance (usually around 15 m) of the desired altitude, then Pitch-from-Altitude and Throttle-from-Airspeed control laws are used. The Pitch-from- Altitude controller uses the altitude error to generate a pitch command for the pitch attitude control loop. The Throttle-from-Airspeed controller uses the airspeed error to generate a throttle command. 2. If the actual altitude is greater than the preset distance from the desired altitude and is less than the desired altitude, then the Pitch-from-Airspeed control law is used with full throttle. The Pitch-from-Airspeed controller uses the airspeed error to generate a pitch command for the pitch attitude control loop. 3. If the actual altitude is greater than the preset distance from the desired altitude and is greater than the desired altitude, then the Pitch-from-Airspeed control law is used with zero throttle.

104 Longitudinal Autopilot For the portion of the landing sequence in which the MAV is spiraling down to its end altitude and then following its glide slope to the landing point, altitude is maintained using the Pitch-from- Altitude and Throttle-from-Airspeed loops. Throughout the entire landing sequence, until the flare at which desired roll is held at zero, course is controlled using the Roll-from-Course loop. Roll rate and pitch rate are also fed back to provide damping. Desired courses are commanded based on the generation of a course vector field around the desired orbit radius. A full description of this algorithm is provided in the following section. The glide slope implemented in this landing routine was a simple linear glide slope calculated by decreasing desired altitude as a function of the distance from where the glide slope began. This formulation allows the desired altitude to be driven negative for distances from the initiation of the glide slope that are farther than the desired landing point. This makes the glide slope robust with respect to negative offsets between measured HAG and actual HAG. Expected overshoot or undershoot of the landing point can approximated as Expected Over / Undershoot 1 ) ( h + uh, (8.4) tan θ where θ is the glide slope, h is the altitude control error (difference between desired and actual HAG), and u h is the measurement error. The relationship shown in Equation (8.4) indicates that larger glide slopes will tend to reduce the overall overshoot or undershoot error for the landing provided the MAV is capable of matching the commanded glide slope. However, larger glide slopes also cause increased impact velocities at touch down. Testing showed that glide slopes between 7 to 12 degrees provided a good balance between accuracy and impact at touch down for the MAVs flown.

105 Autopilot Design Using Successive Loop Closure Design Project 8.1 Using the dynamics and control loops defined by the block diagram in Figure 8.1, design a lateral-directional autopilot using the principles of successive loop closure discussed in class. (If you prefer, you can move the proportional gains to the feedback path to eliminate the zero from the closed-loop transfer function.) 8.2 Implement your lateral-directional autopilot in a Matlab simulation. Use the lateral-directional dynamics defined by Equation (7.17) as your plant. Validate that your successive loop closure design works as anticipated with the more detailed plant model. 8.3 Using the dynamics and control loops defined by the block diagrams in Figures??,?? and 8.10, design an altitude-hold control loop using the principles of successive loop closure discussed in class. (If you prefer, you can move the proportional gain k ph to the feedback path to eliminate the zero from the closed-loop transfer function.) 8.4 Using the dynamics and control loops defined by the block diagrams in Figures 8.12 and 8.11 design an airspeed-hold control loops using both the throttle and the pitch angle as inputs. 8.5 Implement your longitudinal autopilot in a Matlab simulation. Combine altitude hold and airspeed hold autopilot loops using the switching control strategy shown in Figure 8.13 Use the longitudinal dynamics defined by Equation (7.11) as your plant. Validate that your successive loop closure design works as anticipated with the more detailed plant model. 8.6 Based on your results from problems 8.2 and 8.5, implement lateral-directional and longitudinal autopilots in Aviones. You may use the Aviones source code on the class website as a starting point (aviones stripped2). Test your code by giving various altitude and heading commands to the aircraft.

106 Design Project

107 Chapter 9 Micro UAV Sensors The objective of this chapter is to describe the on-board sensors typically used on a micro UAV and to quantify what they measure. The following sensors may be found on a micro UAV: Rate gyros, Accelerometers, Pressure sensors, Magnetometers, GPS. The following sections will discuss each of these sensors. 9.1 Rate Gyros A MEMS rate gyro contains a small vibrating lever. When the lever undergoes an angular rotation, Coriolis effects change the frequency of the vibration, thus detecting the rotation. A brief description of the physics of rate gyros can be found at RateSensorAppNote.pdf. The output of the rate gyro is given by y gyro = k gyro Ω + β gyro + η gyro, where y gyro is in Volts, k gyro is a gain, Ω is the angular rate in radians per second, β gyro is a bias term, and η gyro is zero mean white noise. The gain k gyro should be given on the spec sheet of the 97

108 Accelerometers sensor. However, due to variations in manufacturing it is imprecisely known. The bias term β gyro is strongly dependent on temperature and should be calibrated prior to each flight. If three rate gyros are aligned along the x, y, and z axes of the UAV, then the rate gyros measure the angular body rates p, q, and r as follows: y gyro,x = k gyro,x p + β gyro,x + η gyro,x y gyro,y = k gyro,y q + β gyro,y + η gyro,y y gyro,z = k gyro,z r + β gyro,z + η gyro,z. For simulation purposes, we may assume that the gains are identical and that the biases have been estimated and subtracted from the measurements to produce ŷ gyro,x = k gyro p + η gyro,x ŷ gyro,y = k gyro q + η gyro,y ŷ gyro,z = k gyro r + η gyro,z. MEMS gyros are analog devices that are sampled by the on-board processer. We will assume that the sample rate is given by T s. The Kestrel autopilot samples the gyros at approximately 120 Hz. 9.2 Accelerometers A MEMS accelerometer contains a small plate attached to torsion levers. The plate rotates under acceleration and changes the capacitance between the plate and the surrounding walls [10]. The output of the accelerometers is given by The output of the rate gyro is given by y acc = k acc A + β acc + η acc, where y acc is in Volts, k acc is a gain, A is the acceleration in meters per second, β acc is a bias term, and η acc is zero mean white noise. The gain k acc should be given on the spec sheet of the sensor. However, due to variations in manufacturing it is imprecisely known. The bias term β acc is strongly dependent on temperature and should be calibrated prior to each flight. Accelerometers measure the specific force in the body frame of the vehicle. A physically intuitive explanation is given in [7, p ]. Additional explanation is given in [11, p. 27].

109 Micro UAV Sensors 99 Mathematically we have a x a y a z = 1 m (F F gravity) = v + ω b b/i v 1 m F gravity. In component form we have a x = u + qw rv + g sin θ a y = v + ru pw g cos θ sin φ a z = ẇ + pv qu g cos θ cos φ. The output of accelerometers is usually in units of [g], therefore k acc = 1/g. Assuming that the biases have been removed, the output of the accelerometers is given by y acc,x = y acc,y = y acc,z = u + qw rv + g sin θ + η acc,x g v + ru pw g cos θ sin φ + η acc,y g ẇ + pv qu g cos θ cos φ + η acc,z. g From the dynamics of the UAV, we can also express the output of the accelerometers in terms of aerodynamic forces. Since cq (F F gravity ) = + 1 C X0 + C Xα α + C 2 ρv 2 + C Xq V X δ δ e e bp S C Y0 + C Yβ β + C + C br Yp 2V Y + C r 2V Y δ δ a a + C δ Yδ r r cq C Z0 + C Zα α + C + C Zq V Z δ δ e e [ + 1 C Xp (kδt ) 2 V 2] 2 ρs prop 0, 0

110 Pressure Sensors we have y acc,x = y acc,y = y acc,z = 1 ρv 2 S ( cq C 2 X0 + C Xα α + C + C ) Xq V X δe δ e + 1 ρs [ 2 propc Xp (kδt ) 2 V 2] mg 1 ρv 2 S ( bp C 2 Y0 + C Yβ β + C + C br Yp 2V Y + C ) r 2V Y δa δ a + C Yδr δ r + η acc,y mg 1 ρv 2 S ( cq C 2 Z0 + C Zα α + C + C Zq V Z δ ) δ e e + η acc,z. mg + η acc,x MEMS accelerometers are analog devices that are sampled by the on-board processer. We will assume that the sample rate is given by T s. The Kestrel autopilot samples the accelerometers at approximately 120 Hz. 9.3 Pressure Sensors Altitude Sensor Pressure is a measure of force per unit area or P = F A. We will be concerned with two types of pressure: (1) static pressure due to altitude, and (2) dynamic pressure due to airspeed. The static pressure at a particular altitude is determined by the force exerted by a column of air at that altitude: P = m columng A, where m column is the mass of the column of air, g is the gravitational constant, and A is the area upon which the column is exerting pressure. The density of the is the mass per unit volume. Since the volume is given by the area times the height we get P = ρhg, where ρ is the density of air, and h is the altitude [12]. We are interested in the altitude or heights above a ground station. Suppose that at the ground stations, the pressure sensor is calibrated to read P ground = ρh ground g.

111 Micro UAV Sensors 101 The output of the static pressure sensor is given by y static pres = ρgh + η static pres (t) = ρg(h h ground ) + ρgh ground + η static pres (t), where h ground is the altitude of the ground station. We will assume that the autopilot is calibrated to determine h ground. Therefore, we can model the output of the static pressure sensor in the simulator as y static pres = ρg(h h ground ) + η static pres (t) Air Speed Sensor When the UAV is in motion, the atmosphere exerts dynamic pressure on the UAV parallel to the direction of flow. The dynamic pressure is given by [12] P I = 1 2 ρv 2, where V is the airspeed of the UAV. Bernoulli s theorem states that [12] P s = P I + P O, where P s is the total pressure, and P O is the static pressure. Therefore, the output of the differential pressure sensor is y diff pres = P s P O + η diff pres = 1 2 ρv 2 a Density of air. The density of air ρ is dependent on the air temperature T and air pressure p s. The air density is given by ρ = p s RT, where R = [J/kgK] is the specific gas constant. Notice that in this formula, temperature is expressed in units of Kelvin. The conversion from Fahrenheit to Kelvin is given by T [K] = 5 (T [F ] 32)

112 Magnetometers Pressure is expressed in N/m 2. Typical weather data reports pressure in inches of Mercury. The conversion factor is p s = 3385H, where H is the pressure in inches of Mercury. Therefore, to get accurate measurements of altitude and airspeed, we need to know air temperature and air pressure which can be obtained from the internet. 9.4 Magnetometers The earth s magnetic field is three dimensional. There is a component that points to magnetic north, but there is also a vertical component. A three axis magnetometer measures the strength of the magnetic field along each of its axes. Let m 0 be the magnetic field vector that is fixed in the inertial or vehicle frame. Given the orientation of the airframe, the strength of the magnetic field along the body axes of the UAV are given by m(t) = Rvm b 0 cθcψ cθsψ sθ = sφsθcψ cφsψ sφsθsψ + cφcψ sφcθ m 0. cφsθcψ + sφsψ cφsθsψ sφcψ cφcθ Unfortunately, when the electric motor is activated, an additional magnetic field is activated due to the rotation of the motor coils. Fortunately, the magnetic field is sinusoidal, with a frequency approximately equal to the frequency of the rotating rotor, and can be removed with a low-pass filter. We will model the effect due to the motor as m motor = sin(ω motor t)m motor which is measured in the body frame. Including bias and white noise terms, the output of the magnetometers is given by cθcψ cθsψ sθ y mag = sφsθcψ cφsψ sφsθsψ + cφcψ sφcθ m 0 cφsθcψ + sφsψ cφsθsψ sφcψ cφcθ + sin(ω motor t)m motor + β mag,x β mag,y + η mag,x η mag,y. β mag,z η mag,z The magnetometers are sampled by the microcontroller and therefore run at about 120 Hz.

113 Micro UAV Sensors 103 Effect Ave. Horizontal Error Ave. Vertical Error Atmosphere 5.5 meters 5.5 meters Satellite Geometry (Ephemeris) data 2.5 meters 15 meters Satellite clock drift 1.5 meters 1.5 meters Multipath 0.6 meters 0.6 meters Measurement noise 0.3 meters 0.3 meters Table 9.1: GPS Error Budget 9.5 GPS There are several sources of GPS error. The following table lists the sources of error and the respective error budget. This data was taken from slides/gpsaccuracy.ppt. The current weather effects the speed of light in the atmosphere. However, this inaccuracy should be relatively constant for a given day. We will model the effect of the atmosphere by a constant random variable drawn from a Gaussian distribution with a standard deviation equal to 5 meters. The geometry of the Satellites viewed by the receiver is used to triangulate the location of the GPS receiver. Triangulation is much more effective in the horizontal plane than in the vertical direction. The satellite geometry is slowly changing in time. Therefore we will measure the effect of satellite geometry as a sinusoid with amplitude equal to (RMS=2.5), with a frequency equal to ω geometry = 10 6 and a phase that is a constant random variable drawn from a uniform distribution over [ π, π]. We will assume that the clock drift is relatively constant over time. Therefore, we will model the clock drift by a constant random variable drawn from a Gaussian distribution with standard deviation of 1.5 meters. The multipath is a function of the position of the UAV. Therefore we will assume that the error is a sinusoidal signal with a magnitude of 0.6 2, a frequency equal to ω multipath = 10 3 and a constant random phase drawn from a uniform distribution over [ π, π]. We will model the measurement noise as a stochastic Gaussian process with zero mean and variance equal to 0.3 meters.

114 GPS The model for GPS is therefore given by y GP S,N (t) = p N + ν N,atmosphere + ν clock + η N,measurement (t) sin(ω geometry t + ν N,geometry ) sin(ω multipath t + ν N,multipath ) y GP S,E (t) = p E + ν E,atmosphere + ν E,clock + η E,measurement (t) sin(ω geometry t + ν E,geometry ) sin(ω multipath t + ν E,multipath ) y GP S,h (t) = h + ν h,atmosphere + ν h,clock + η h,measurement (t), sin(ω geometry t + ν h,geometry ) sin(ω multipath t + ν h,multipath ), where p N, p E, and h are the actual earth coordinates and altitude above sea level respectively. The GPS receiver also computes estimated ground speed and heading from the measurements listed above. Accordingly, we have (ygp ) 2 ( S,N(t + T s ) y GP S,N(t) ygp S,E (t + T s ) y GP S,E (t) y GP S,Vg = + ( ) y GP S,heading = tan 1 ygp S,E (t + T s ) y GP S,E (t). y GP S,N (t + T s ) y GP S,N (t) T s The update rate of a GPS receiver is typically on the order of T s = 1 second. However, the update rate can vary between seconds. T s ) 2

115 Micro UAV Sensors Design Project 9.1 Homework problem 1.

116 Design Project

117 Chapter 10 State Estimation The objective of this chapter is to describe techniques for estimating the state of a micro UAV from sensor measurements Low Pass Filters All of the state estimation schemes require low-pass filtering of the sensor signals. The Laplace transforms representation of a simple low-pass filter is given by Y (s) = a s + a U(s), were u(t) is the input of the filter and y(t) is the output. Inverse Laplace transforming we get ẏ = ay + au. (10.1) Using a zeroth order approximation of the derivative we get y(t + T ) y(t) T = ay(t) + au(t), where T is the sample rate. Solving for y(t + T ) we get y(t + T ) = (1 at )y(t) + at u(t). For the zeroth order approximation to be valid we need at 1. If we let α = at then we get the simple form y(t + T ) = (1 α)y(t) + αu(t). 107

118 State Estimation by Inverting the Sensor Model Note that this equation has a nice physical interpretation: the new value of y (filtered value) is a weighted average of the old value of y and u (unfiltered value). If u is noisy, then α [0, 1] should be set to a small value. However, if u is relatively noise free, then α should be close to unity. In the derivation of the discrete-time implementation of the low-pass filter, it is possible to be more precise. In particular, returning to Equation (10.1), from linear systems theory, it is well known that the solution is given by T y(t + T ) = e at y(t) + a e a(t τ) u(τ) dτ. 0 Assuming that u(t) is constant between sample periods results in the expression T y(t + T ) = e at y(t) + a e a(t τ) dτu(t)y(t + T ) 0 = e at y(t) + (1 e at )u(t). (10.2) Note that since e x = 1 + x + x2 2! +... we have that e at 1 at and 1 e at at. Therefore, if the sample rate is not fixed (common for micro controllers), and it is desired to have a fixed cut-off frequency, then Equation (10.2) is the preferable way to implement a low-pass filter in digital hardware. We will use the notation LP F ( ) to represent the low-pass filter operator. Therefore ˆx = LP F (x) is the low-pass filtered version of x State Estimation by Inverting the Sensor Model In this section we will derive the simplest possible state estimation scheme based on inverting the sensor models. While this method is effective for angular rates, altitude, and airspeed, it is not effective for estimating the position and Euler angles Position and Heading The position variables p N, p E and the heading ψ can be estimated by low-pass filtering the GPS signals: ˆp N = LP F (y GPS,N ) (10.3) ˆp E = LP F (y GPS,E ) (10.4) ˆψ = LP F (y GPS,heading ). (10.5)

119 State Estimation Angular Rates Similarly, the angular rates p, q, and r can be estimated by low-pass filtering the rate gyro signals: ˆp = LP F (y gyro,x ) (10.6) ˆq = LP F (y gyro,y ) (10.7) ˆr = LP F (y gyro,z ). (10.8) Altitude GPS is not accurate enough to estimate the altitude. Therefore, we will use the absolute pressure sensor. Recall that Therefore, a simple estimation scheme is Airspeed y static pressure = ρg(h h ground ) + η static pressure. ĥ = h ground + LP F (y static pressure). (10.9) ρg Recall that y differential pressure = 1 2 ρv 2 a + η differential pressure. Therefore, a simple estimation scheme is ˆV a = Roll and Pitch Angles 2 ρ LP F (y differential pressure). (10.10) Roll and Pitch angles are the most difficult variables to estimate well on small UAVs. An extremely simple scheme, that works in unaccelerated flight can be derived as follows. Recall that y accel,x = y accel,y = y accel,z = u + qw rv + g sin θ + η accel,x g v + ru pw g cos θ sin φ + η accel,y g ẇ + pv qu g cos θ cos φ + η accel,z. g

120 State Estimation by Inverting the Sensor Model In unaccelerated flight we have u = v = ẇ = p = q = r = 0 which implies that LP F (y accel,x ) = sin θ LP F (y accel,y ) = cos θ sin φ LP F (y accel,z ) = cos θ cos φ. Solving for φ and θ we get ( ) LP F ˆφ accel = tan 1 (yaccel,y ) (10.11) LP F (y accel,z ) ( ) ˆθ accel = tan 1 LP F (y accel,x ). (10.12) LP F (yaccel,y ) 2 + LP F (y accel,z ) 2 Unfortunately, this scheme does not work very well, especially in coordinated turns. Another simple idea is to notice that we are not taking into account the rate gyro s, which give valuable information about the angles. Recalling that φ = p + sin φ tan θq + cos φ tan θr θ = cos φq sin φr and assuming that φ 0 and θ 0 we get φ = p θ = q. Therefore we can integrate these equations to obtain an additional estimate of φ and θ: ˆφ int = ˆθ int = t t p(τ) dτ q(τ) dτ. Combining the estimate from the integrator and the accelerometers we obtain ˆφ = κ ˆφ int + (1 κ) ˆφ accel ˆθ = κˆθ int + (1 κ)ˆθ accel, where κ (0, 1). Note that this scheme also has problems in turns.

121 State Estimation 111 Load Factor Estimation The objective of this section is to show how load factor can be estimated from accelerometer and attitude information. From Newton s second law we get m a x a y a z = L + D + W + T, where a x, a y, a z is the acceleration of the airframe measured along the body axis, L is the lift, D is the drag, W is the weight, and T is the thrust. At constant velocity we have T = D. In addition we have sin α cos φ L = L sin φ, cos α cos φ where α is the angle of attach, φ is the roll angle, and L is the magnitude of the lift vector. The weight vector can be expressed as sin θ cos φ W = mg sin φ, cos θ cos φ where θ is the pitch angle. Combining these expressions we get ma x = L sin α cos φ mg sin θ cos φ ma y = L sin φ mg sin φ ma z = L cos α cos φ + mg cos θ cos φ. Dividing by mg and solving for n = L/mg gives n sin α cos φ = a x g + sin θ cos φ n sin φ = a y g + sin φ n cos α cos φ = a z g + cos θ cos φ. Noting that n 0 we can solve for n as (ax ) 2 ( ) 2 ( ay n = + sin θ cos φ + g g + sin φ + a z g ) 2 + cos θ cos φ.

122 Dynamic Observer Theory 10.3 Dynamic Observer Theory The objective of this section is to briefly review observer theory. Suppose that we have a linear time-invariant system modeled by the equations ẋ = Ax + Bu y = Cx. A continuous-time observer for this system is given by the equation ˆx = Aˆx + Bu }{{} copy of the model + L (y C ˆx) }{{}, (10.13) correction due to sensor reading where ˆx is the estimated value of x. Letting x = x ˆx we observer that x = (A LC) x which implies that the observation error decays exponentially to zero if L is chosen such that the eigenvalues of A LC are in the open left half of the complex plane. In practice, the sensors are usually sampled and processed in digital hardware at some sample rate T s. How do we modify the observer equation shown in Equation (10.13) to account for sampled sensor readings? The typical approach is to propagate the system model between samples using the equation ˆx = Aˆx + Bu (10.14) and then to update the estimate when a measurement is received using the equation ˆx + = ˆx + L(y(t k ) C ˆx ), where t k is the instant in time that the measurement is received and ˆx is the state estimate produced by Equation (10.14) at time t k. Equation (10.14) is then re-instantiated with initial conditions given by ˆx +. The continuous-discrete observer is summarized in Table The observation process is shown graphically in Figure Note that it is not necessary to have a fixed sample rate. The continuous-discrete observer can be implemented using Algorithm 4.

123 State Estimation 113 Figure 10.1: sdf Algorithm 4 Continuous-Discrete Observer 1: Initialize: ˆx = 0. 2: Pick an output sample rate T out which is much less than the sample rates of the sensors. 3: At each sample time T out : 4: for i = 1 to N do {Propagate the state equation.} 5: ˆx = ˆx + ( ) T out N (Aˆx + Bu) 6: end for 7: if A measurement has been received from sensor i then {Measurement Update} 8: ˆx = ˆx + L i (y i C iˆx) 9: end if

124 Essentials from Probability Theory System model: ẋ = Ax + Bu y(t k ) = Cx(t k ) Initial Condition x(0). Assumptions: Knowledge of A, B, C, u(t). No measurement noise. In between measurements (t [t k 1, t k )): Propagate ˆx = Aˆx + Bu. Initial condition is ˆx + (t k 1 ). Label the estimate at time t k as ˆx (t k ). At sensor measurement (t = t k ): ˆx + (t k ) = ˆx (t k ) + L (y(t k ) C ˆx (t k )). Table 10.1: Continuous-Discrete Linear Observer Essentials from Probability Theory Let X = (x 1,..., x n ) T be a random vector whose elements are random variables. The mean, or expected value of X is denoted by µ = µ 1. µ n = E{x 1 }. E{x n } = E{X}, where E{x i } = ξf i (ξ) dξ, and f( ) is the probability density function for x i. Given any pair of components x i and x j of X, we denote their covariance as cov(x i, x j ) = Σ ij = E{(x i µ i )(x j µ j )}. The covariance of any component with itself is the variance, i.e., var(x i ) = cov(x i, x i ) = Σ ii = E{(x i µ i )(ξ µ i )}. The standard deviation of x i is the square root of the variance: stdev(x i ) = σ i = Σ ii.

125 State Estimation 115 The covariances associated with a random vector X can be grouped into a matrix known as the covariance matrix: Σ 11 Σ 12 Σ 1n Σ Σ = 21 Σ 22 Σ 2n..... = E{(X µ)(x µ)t } = E{XX T } µµ T. Σ n1 Σ n2 Σ nn Note that Σ = Σ T so that Σ is both symmetric and positive semi-definite, which implies that its eigenvalues are real and nonnegative. The probability density function for a Gaussian random variable is given by f x (x) = 1 e (x µx) 2πσx 2 σx 2, where µ x is the mean of x and σ x is the standard deviation. The vector equivalent is given by [ 1 f X (X) = exp 1 ] 2π det Σ 2 (X µ)t Σ 1 (X µ), in which case we write X N (µ, Σ), and say that X is normally distributed with mean µ and covariance Σ. Figure 10.2 shows the level curves for a 2D Gaussian random variable with different covariance matrices Derivation of the Kalman Filter Continuous-Update, Continuous Measurement We will assume the following system model ẋ = Ax + Bu + Gξ y = Cx + η, where ξ N (0, Q) and η N (0, R), i.e, E{ξ} = 0 and E{ξ(t)ξ(τ) T } = Qδ(t τ), and E{η} = 0 and E{η(t)η(τ) T } = Rδ(t τ). The random variable ξ is called the process noise and represents modeling error and disturbances on the system. The random variable η is called the measurement noise and represents noise on the sensors. The covariance R can usually be estimated

126 Derivation of the Kalman Filter Figure 10.2: Level curves for the pdf of a 2D Gaussian random variable. from sensor calibration, but the covariance Q is generally unknown and therefore becomes a system gain that can be tuned to improve the performance of the observer. Similar to Equation (10.13), let the observer have the following form ˆx = Aˆx + Bu + L(y C ˆx). Define the estimation error as x = x ˆx. The covariance of the estimation error is given by P (t) = E{ x(t) x(t) T } Note that P (t) is symmetric and positive semi-definite, therefore its eigenvalues are real and nonnegative. Also small eigenvalues of P (t) imply small variance, which implies low average estimation error. Therefore, we would like to choose L to minimize the eigenvalues of P (t). Recall that n tr(p ) = λ i, where tr(p ) is the trace of P and λ i are the eigenvalues. Therefore, minimizing tr(p ) minimizes the estimation error covariance. The Kalman filter is derived by choosing L such that the expected value of the estimation error is zero, and the trace of the error covariance P (t) is minimized. In the derivation that is to follow, i=1

127 State Estimation 117 we will need the following relationships: Note that x = ẋ ˆx tr(a) = I A (10.15) A tr(bad) = BT D T (10.16) A tr(abat ) = 2AB if B = B T. (10.17) = Ax + Bu + Gξ Aˆx Bu L(Cx + η C ˆx) = (A LC) x + Gξ Lη. Therefore, from linear systems theory we have x(t) = e (A LC)t x(0) + t 0 e (A LC)(t τ) Gξ(τ) dτ Differentiating the expression for P (t) we have P (t) = d dt E{ x(t) x(t)t } = E{ x x T + x x} t 0 e (A LC)(t τ) Lη(τ) dτ. = E{(A LC) x x T + Gξ x T Lη x T + x x T (A LC) T + xξ T G T xη T L T } = (A LC)P (t) + P (t)(a LC) T + GE{ξ x T } + E{ xξ T }G LE{η x T } E{ xη T }L T. We can compute E{ xξ T } as follows: E{ xξ T } = E{e (A LC)tˆx 0 ξ T (t) + = t 0 = 1 2 GQ, t e (A LC)(t τ) GQδ(t τ) dτ 0 e (A LC)(t τ) Gξ(τ)ξ T (τ) dτ t 0 e (A LC)(t τ) Lη(τ)ξ T (τ) dτ} where the 1 2 give is because we only use half of the area inside the delta function. Similar calculations E{ξ x T } = 1 2 QGT E{η x T } = 1 2 RL E{ xη T } = 1 2 LT R.

128 Derivation of the Kalman Filter Therefore we have P = (A LC)P + P (A LC) T + GQG T LRL T. If (A LC) is Hurwitz, then P approaches a constant value which implies that P approaches zero. Therefore, in steady state we have (A LC)P + P (A LC) T + GQG T LRL T = 0. We now have the following constrained optimization problem: minimize tr(p ) subject to: (A LC)P + P (A LC) T + GQG T LRL T = 0. Using a Lagrange multiplier [13] we can convert this problem to the following unconstrained optimization problem: minimize: H = tr(p ) + tr ( (A LC)P + P (A LC) T + GQG T LRL T ). Necessary conditions for a minimum are H S = (A LC)P + P (A LC)T + GQG T LRL T = 0 H P = (A LC)T S + S(A LC) + I = 0 H L = 2SP CT 2SLR = 0. Therefore L = P C T R 1 and P must satisfy the Riccati Equation AP + P A T + GQG T P C T R 1 CP = 0. The steady-state Kalman observer is given in Algorithm 5 Algorithm 5 Steady State Kalman Filter 1: Initialize: ˆx = 0. 2: Solve the Riccati Equation AP + P A T + GQG T P C T R 1 CP = 0. 3: Set the observer gain L = P C T R 1. 4: Propagate the state estimate: ˆx = ˆx + T sample (Aˆx + Bu + L(y C ˆx).

129 State Estimation Fixed Gain Kalman Filter Continuous-Discrete Kalman Filter In this section we assume the following state model: ẋ = Ax + Bu + Gξ y k = Cx k + η k, where y k = y(t k ) is the k th sample of y, x k = x(t k ) is the k th sample of x, η k is the measurement noise at time t k, ξ is a zero-mean Gaussian random process with covariance Q, and η k is a zeromean Gaussian random variable with covariance R. Note that the sample rate does not need to be be fixed. The observer will therefore have the following form: In between measurements: (t [t k 1, t k ]) Propagate ˆx = Aˆx + Bu. Initial condition is ˆx + (t k 1 ). Label the estimate at time t k as ˆx (t k ). At sensor measurement (t = t k ): ˆx + (t k ) = ˆx (t k ) + L (y(t k ) C ˆx (t k )). Our objective is to pick L to minimize tr(p (t)). Between Measurements. Differentiating x we get Therefore we have that x = ẋ ˆx = Ax + Bu + Gξ Aˆx Bu = A x + Gξ. x(t) = e At x 0 + We can therefore compute the evolution for P as P = d dt E{ x xt } t 0 e A(t τ) Gξ(τ) dτ. = E{ x x T + x x T } = E { A x x T + Gξ x T + x x T A T + xξ T G T } = AP + P A T + GE{ξ x T } T + E{ xξ T }G T.

130 Derivation of the Kalman Filter As in the previous section we get E{ξ x T } = E { ξ(t) x 0 e AT t + = 1 2 QGT, t 0 } ξ(t)ξ T (τ)g T e AT (t τ) dτ which implies that P = AP + P A T + GQG T. At Measurements. At a measurement we have that Therefore x + = x ˆx + = x ˆx L ( Cx + η C ˆx ) = x LC x Lη. P + = E{ x + x +T } { = E ( x LC x Lη ) ( x LC x Lη ) } T = E { x x T x x T C T L T x η T L T LC x x T + LC x x T C T L T + LC x η T L T = Lη x T + Lη x T C T L T + Lηη T L T } = P P C T L T LCP + LCP C T L T + LRL T. (10.18) Our objective is to pick L to minimize tr(p + ). A necessary condition is L tr(p + ) = P C T P C T + 2LCP C T + 2LR = 0 = 2L(R + CP C T ) = 2P C T Plugging back into Equation (10.18) give = L = P C T (R + CP C T ) 1. P + = P + P C T (R + CP C T ) 1 CP P C T (R + CP C T ) 1 CP + P C T (R + CP C T ) 1 (CP C T + R)(R + CP C T ) 1 CP = P P C T (R + CP C T ) 1 CP = (I P C T (R + CP C T ) 1 C)P = (I LC)P.

131 State Estimation 121 The Kalman filter for continuous-discrete systems is summarized in Algorithm 6. Algorithm 6 Continuous-Discrete Kalman Filter 1: Initialize: ˆx = 0. 2: Pick an output sample rate T out which is much less than the sample rates of the sensors. 3: At each sample time T out : 4: for i = 1 to N do {Propagate the equations.} 5: ˆx = ˆx + ( T out ) N (Aˆx + Bu) 6: P = P + ( T out ) ( N AP + P A T + GQG ) T 7: end for 8: if A measurement has been received from sensor i then {Measurement Update} 9: L i = P C T i (R i + C i P C T i ) 1 10: P = (I L i C i )P 11: ˆx = ˆx + L i (y i C iˆx). 12: end if 10.6 Attitude Estimation In this section we will discuss state estimation for roll and pitch Fixed Gain Kalman Filter To derive the fixed gain Kalman filter, we first lump all of the nonlinearities into the noise variables as follows: φ = p + q sin φ tan θ + r cos φ tan θ + ξ φ = p + ξ φ (10.19) θ = q cos φ r sin φ + ξ θ ( ) = q + q + q cos φ r sin φ + ξ θ = q + ξ θ, (10.20) where we will assume that ξ φ N (0, Q φ ) and ξ θ N (0, Q θ ). We can use Equation (10.19) and (10.20) as the model with p and q considered as inputs. Also note that the equations are not coupled so we will derive two one-state Kalman filters where

132 Attitude Estimation A φ = A θ = B φ = B θ = 0. We will use the accelerometers to produce the measurements. Therefore y φ = ˆφ accel = φ + η φ y θ = ˆθ accel = θ + η θ, where we will again assume that η φ N (0, R φ ) and η θ N (0, R θ ). Therefore C φ = C θ = 1. The steady-state Kalman filter equations are ˆx = Aˆx + Bu + L(y C ˆx) L = P C T R 1 0 = AP + P A T + GQG T P C T R 1 CP. The Riccati equation for the roll angle simplifies to Q φ P 2 φ R φ = 0. Therefore we have P φ = Q φ R φ, and the Kalman gain becomes L φ = P φ Cφ T R 1 φ Qφ R φ = R φ Q φ =. R φ We can calculate L θ using similar reasoning. Therefore, the steady state Kalman filters are given by Q ( φ ˆφ = p + ˆφaccel R ˆφ ) φ Qθ ˆθ = q + (ˆθaccel R ˆθ ). θ

133 State Estimation Continuous-Discrete Kalman Filter The equations of motion for φ and θ are given by φ = p + q sin φ tan θ + r cos φ tan θ + ξ φ θ = q cos φ r sin φ + ξ θ, where we have added the noise terms ξ φ and ξ θ to model the noise on p, q, and r. Notice that the Gaussian noise assumption, i.e., ξ φ N (0, Q φ ) and ξ θ N (0, Q θ ), are much easier to justify in this case. We would like to use the accelerometers as the output equations. From Chapter 9 the description of the accelerometers is given by y accel = u+gw rv g v+ru pw g ẇ+pv qu g + sin θ cos θ sin φ + η accel. (10.21) cos θ cos φ However we do not have a method for directly measuring u, v, ẇ, u, v, and w. We will assume that u = v = ẇ 0. From Equation (??) we have u cos α cos β v = V a sin β. w sin α cos β Assuming that α θ and β 0 we obtain u cos θ v V a 0. w sin θ Therefore, plugging into Equation (10.21) we get qv a sin θ + sin θ g rv y accel = a cos θ pv a sin θ cos θ sin φ g + η accel. cos θ cos φ qv a cos θ g Therefore letting x = (φ, θ) T, u = (p, q, r, V a ) T, ξ = (ξ φ, ξ θ ) T, and η = (η φ, η θ ) T, we get ẋ = f(x, u) + ξ y = h(x, u) + η,

134 Attitude Estimation where ( ) p + q sin φ tan θ + r cos φ tan θ f(x, u) = q cos φ r sin φ qv a sin θ + sin θ g rv h(x, u) = a cos θ pv a sin θ cos θ sin φ g. cos θ cos φ qv a cos θ g Implementation of the Kalman filter requires the Jacobians f x ( q sin φ r cos φ f x = q cos φ tan θ r sin φ tan θ cos 2 θ q sin φ r cos φ 0 h x = 0 cos φ cos θ sin φ cos θ rv a g The resulting Kalman filter is listed in Algorithm 7. h and. Accordingly we have x ) qv a cos θ + cos θ g sin θ pv a cos θ + sin φ sin θ g ). + cos φ sin θ ( qv a g Algorithm 7 Kalman Filter for Roll and Pitch 1: Initialize: ˆx = 0. 2: Pick an output sample rate T out which is much less than the sample rates of the sensors. 3: At each sample time T out : 4: for i = 1 to N do {Propagate the equations.} 5: ˆx = ˆx + ( T out ) N f(ˆx, u) 6: A = f (ˆx, u) x 7: P = P + ( T out ) ( N AP + P A T + Q ) 8: end for 9: if Accelerometer measurement received, then {Measurement Update} 10: C = h (ˆx, u) x 11: L = P C T (R + CP C T ) 1 12: P = (I LC)P 13: ˆx = ˆx + L (y accel h(ˆx, u)). 14: end if

135 State Estimation GPS Smooting Lets turn our attention to estimating p N, p E, and ψ using the GPS sensor. The model for ψ is given by ψ = q sin φ cos θ + r cos φ cos θ. We will use the following approximate model for the evolution of p N and p E : ṗ N = V g cos ψ ṗ E = V g sin ψ, where V g is the ground speed which is measured by the GPS sensor. Therefore the system model is given by ṗ N ṗ Ė = ψ V g cos ψ V g sin ψ q sin φ + r cos φ cos θ cos θ = f(x, u) + ξ, + ξ where x = (p N, p E, ψ) T, u = (V g, q, r, φ, θ) T and ξ N (0, Q). model GPS returns measurements of p N, p E, and ψ directly. Therefore we will assume the output p N y GP S = p E + η, ψ where η N (0, R) and C = I. To implement the Kalman filter we need the Jacobian of f which can be calculated as f x = 0 0 V g sin ψ 0 0 V g cos ψ The resulting Kalman filter is listed in Algorithm Wind Estimation This section is from [14]. For MAVs, winds that are a significant percentage of the airspeed are almost always present. Therefore, the airframe is typically required to crab into the wind, causing the course (direction

136 Wind Estimation Algorithm 8 Kalman Filter for p N, p E and ψ 1: Initialize: ˆx = 0. 2: Pick an output sample rate T out which is much less than the sample rates of the sensors. 3: At each sample time T out : 4: for i = 1 to N do {Propagate the equations.} 5: ˆx = ˆx + ( T out ) N f(ˆx, u) 6: A = f (ˆx, u) x 7: P = P + ( T out ) ( N AP + P A T + Q ) 8: end for 9: if GPS measurement received, then {Measurement Update} 10: L = P (R + P ) 1 11: P = (I L)P 12: ˆx = ˆx + L (y GP S h(ˆx, u)). 13: end if of travel) to deviate from the heading (direction of the body frame x-axis). Since the camera is mounted to the body, the difference between course and heading, if it is not accounted for, will cause significant errors in the geo-location estimates. In this section, the heading angle is denoted by ψ and the course angle will be denoted by χ. To illustrate the effect of wind, Figure 10.3 shows the error in the geo-location estimate generated by a simulation of a complete orbit in significant wind. The simulated MAV has an airspeed of 18 m/s and the wind is from the East at 9m/s. Note that since the MAV must crab right into the 30 Geo location Estimate Errors N E Wind Figure 10.3: Effect of wind on geo-location estimates wind, the geo-location errors shown in Figure 10.3 are significantly biased to the South. We note that wind does not introduce a constant bias in the estimate and can therefore not be removed by

137 State Estimation 127 the techniques discussed in Section??. To compensate for wind, the direction and magnitude of the wind is estimated on-line from flight data and is used to modify ψ in Eq. (??). We will assume that GPS measurements are available but that the MAV is not equipped with magnetometers. The relationship between windspeed, groundspeed, and airspeed is illustrated in Figure 10.4, where V w is the windspeed, V g is the groundspeed, V a is the airspeed, and ξ is the wind direction, and can be expressed as V g = V a cos (ψ χ) + V w cos (ξ χ). (10.22) The GPS sensor measures V g and χ, and a differential pressure sensor can be used to measure V a. The law of cosines ξ V w ξ χ V a V g course, χ heading, ψ ψ χ ground track Figure 10.4: Relationship between ground, air, and wind velocities. cos (A) = a2 + b 2 + c 2, 2bc can be used to remove ψ from Eq. (10.22), resulting in the expression V g = ( V 2 g + Va 2 Vw 2 2V g ) + V w cos (ξ χ). Rearranging we obtain V 2 g V 2 a + V 2 w 2V g V w cos (ξ χ) = 0. (10.23)

138 Wind Estimation To estimate V w and ξ we collect on-line measurements of V g, V a, and χ and use a quasi-newton nonlinear equation solver to minimize the objective function n i=0 where the index i denotes a measurement sample. ( V 2 gi V 2 a i + V 2 w 2V gi V w cos (ξ χ i ) ) 2, (10.24) To quantify the effectiveness of our wind estimation scheme, we flew a MAV in windy conditions in an orbit pattern. Since we do not have the instrumentation to measure true wind speed at the elevations that the MAV is flying ( meters), to measure the accuracy of our wind estimation method, we used the estimated windspeed and the measured airspeed to estimate ground speed, and compared the estimate with the measured GPS ground speed. Figure 10.5 shows actual flight data recorded while the MAV was flying in winds of approximately 9 m/s from the north. Figure 10.5 shows the efficacy of our method by plotting the raw airspeed measurements, as well as the estimated airspeed using the estimated windspeed and the GPS measured groundspeed. Results demonstrating the efficacy of the wind correction scheme for geo-location are discussed in Section?? V w = 9.1 m/s ξ = 136 degrees Measured Predicted 5 V g V a (m/s) χ (degrees) Figure 10.5: Wind solution for a dataset taken in high-wind conditions V w 9 m/s. The wind estimation scheme discussed in this section estimates a constant wind and does not account for gusts. On the other hand, flight test data suggest that the gusts are essentially normally distributed about the constant wind and are therefore removed by the RLS filter.

139 State Estimation Design Project 10.1 Homework problem 1.

140 Design Project

141 Chapter 11 Waypoint and Orbit Following The objective of this chapter is to develop guidance laws for tracking straight line segments and for tracking circular orbits. Chapter 12 will describe techniques for planning straight line waypoint paths through obstacle fields, and Chapter 13 will discuss techniques for tracking waypoint paths using a combination of straight line segments and circular orbits. The primary challenge in tracking straight line segments and circular orbits is the constant winds, which are almost always present. For MAVs wind speeds are commonly 20 to 60 percent of the desired airspeed. Effective path tracking strategies must overcome the effect of this ever present disturbance. For most fixed-wing MAVs, the minimum turn radius is in the range of 10 to 50 m. This places a fundamental limit on the spatial frequency of paths that can be tracked. Thus, it is important that the path tracking algorithms utilize the full capability of the MAV. Several approaches have been proposed for UAV trajectory tracking. An approach for tight tracking of curved trajectories is presented in [15]. For straight-line paths, the approach approximates PD control. For curved paths, an additional anticipatory control element that improves the tracking capability is implemented. The approach accommodates the addition of an adaptive element to account for disturbances such as wind. This approach is validated with flight experiments. Reference [16] describes an integrated approach for developing guidance and control algorithms for autonomous vehicle trajectory tracking. Their approach builds upon the theory of gain scheduling and produces controllers for tracking trajectories that are defined in an inertial reference frame. The approach is illustrated through simulations of a small UAV. Implicit in the notion of trajectory tracking is that the vehicle is commanded to be in a particular location at a particular time and that this location typically varies in time, thus causing the vehicle to move in the desired fashion. With fixed-wing MAVs, the desired position is constantly moving (at the desired air speed). The approach of tracking a moving point can result in significant problems for MAVs if disturbances, such as those due to wind, are not accounted for properly. If the MAV is 131

142 Problem Description flying into a strong wind (relative to its commanded ground speed), the progression of the trajectory point must be slowed accordingly. Similarly, if the MAV is flying down wind, the speed of the tracking point must be increased to keep the MAV from overrunning the desired position. Given that wind disturbances vary and are often not easily predicted, trajectory tracking can be very challenging in anything other than calm conditions. Rather than using a trajectory tracking approach, this chapter focuses on path following where the objective is to be on the path rather than at a certain point at a particular time. With path following, the time dependence of the problem is removed. In [17, 18], performance limits for reference-tracking and path-following controllers are investigated and the difference between them is highlighted. It is shown that there is not a fundamental performance limitation for path following for systems with unstable zero dynamics as there is for reference tracking. Building on the work presented in [19] on maneuver modified trajectory tracking, [20] develops an approach that combines the features of trajectory tracking and path following for marine vehicles. Similar to this work is that of Skjetne, et al. [21] which develops an output maneuvering method composed of two tasks: forcing the output to converge to the desired path and then satisfying a desired speed assignment along the path. The method is demonstrated using a marine vessel simulation. Ref. [22] presents a path following method for UAVs that provides a constant line of sight between the UAV and an observation target. This chapter presents several path following strategies for following straight line paths and orbits. The objective is not to track a moving point, but to get onto the path while flying at a prescribed airspeed. By using ground track instead of heading, asymptotically converging tracking error can be guaranteed, even in the presence of constant wind disturbances Problem Description Our objective is to develop a method for accurate path following in the presence of wind. For a given airframe, there is an optimal airspeed for which the airframe is the most aerodynamically efficient. Therefore, to conserve fuel, it is desirable that the MAV maintain a constant airspeed. For a straight line path, we will assume that the path is described by two vectors in R 3, namely P straight (w, q ) = { r R 3 : r = w + α q, α R }, where w R 3 is the origin of the path, and q R 3 is a unit vector whose direction indicates the desired direction of travel. Alternatively, an orbital path is described by a center c R 3, a radius

143 Waypoint and Orbit Following 133 ρ R, and a direction λ { 1, 1}, as { ( ) } T P orbit (c, ρ, λ) = r R 3 : r = c + λρ cos ϕ, sin ϕ 0, ϕ [0, 2π), where λ = 1 signifies a clockwise orbit and λ = 1 signifies a counterclockwise orbit. The guidance strategy is developed by modeling the MAV using the navigation model given by the six state navigation model ṗ n = V a cos χ cos γ + w x (11.1) ṗ e = V a sin χ cos γ + w y (11.2) ḣ = V a sin γ + w h (11.3) χ = g V a tan φ (11.4) φ = 1 τ φ (φ c φ) (11.5) γ = 1 τ γ (γ c γ). (11.6) We will further assume that the roll and pitch dynamics are much faster than the heading and altitude dynamics respectively, which implies that Equations (11.4) and (11.3) can be approximated as χ = g V a tan φ c, (11.7) ḣ = V a sin γ c.. (11.8) If the distance between a point p and a set P is defined as p P = min r P p r, and if χ q = tan 1 q e q n, and γ q = q d, then the objective of the guidance law for straight line qn+q 2 e 2 following is to develop a feedback law for φ c and γ c so that { ( T p n (t), p e (t), h(t)) Pstraight (w, q ) + χ(t) χ q + } γ(t) γ q 0. lim t Similarly, the objective of the guidance law for orbit following is to develop a feedback law for φ c so that ( T lim p n (t), p e (t), h(t)) Porbit (c, ρ) 0. t Following a Straight Path The method described in this section is a variation of the one described in [23, 24], and is based on the notion of a vector field, which calculates a desired heading based on the distance from the path.

144 Problem Description The notion of vector fields is similar to that of potential fields, which have been widely used as a tool for path planning in the robotics community (see e.g., [25]). It has also been suggested in [26] that potential fields can be used in UAV navigation for obstacle and collision avoidance applications. The method of [26] provides a way for groups of UAVs to use the gradient of a potential field to navigate through heavily populated areas safely while still aggressively approaching their targets. Vector fields are different from potential fields in that they do not necessarily represent the gradient of a potential. Rather, the vector field simply indicates a desired direction of travel. Figure 11.1: This figure shows the configuration of the MAV indicated by (p, χ), and the configuration of the MAV relative to P straight indicated by ( p, χ). Figure 11.1 shows the straight line path P straight (w, q ), and the position of the MAV p. The position of the MAV relative to P straight is given by p = p w. The heading of the MAV relative to P straight is given by χ = χ χ q, where χ q is the inertial heading of q relative to North. To simplify the notation, we define the path frame F P to be the frame centered at w, with the x-axis aligned with the projection of q onto the local North-East plane. In the path frame, the relative position vector is given by p P = Ri P (p i w i ), where R P i = cos χ p sin χ p 0 sin χ p cos χ p

145 Waypoint and Orbit Following 135 Defining p P = ( p x, p y ) T, and differentiating, we get p x p y = Ri P (ṗ i ẇ i ) ḣ cos χ p sin χ p 0 cos χ cos γ = V sin χ p cos χ p 0 sin χ cos γ sin γ cos(χ χ p ) cos γ = V sin(χ χ p ) cos γ sin γ cos χ cos γ = V sin χ cos γ. sin γ Straight Path Following We will derive the guidance law for following a straight path by decoupling the lateral and longitudinal motion. For the lateral motion we assume that γ is a constant. In the path frame p x is the position along the path, and p y is the cross track error that should be driven to zero. The lateral error dynamics are therefore given by p y = V sin χ cos γ (11.9) χ = g V tan φc. (11.10) Our path following approach is derived using the theory of nested saturations [27]. The objective is to drive p y and χ to zero while simultaneously satisfying the constraint that φ c φ. The first step is to differentiate Equation (11.9) to obtain p y = g cos χ cos γ tan φ c. Define W 1 = 1 p 2 and differentiate to obtain 2 Ẇ 1 = p y g cos χ cos γ tan φ c, (11.11) and choose ( ) tan φ c k1 p y + σ M2 (ζ) = σ M1, (11.12) g cos χ cos γ

146 Problem Description where σ M is the saturation function M, if u > M σ M (u) = M, if u < M u, otherwise, and M 1, M 2, and ζ will be selected in the discussion that follows. Substituting (11.12) into gives ( ) k1 p y + σ M2 (ζ) Ẇ 1 = p y g cos χ cos γσ M1, g cos χ cos γ which is negative when p y > M2 /k 1. Therefore, by the ultimate boundedness theorem, there exist a time T 1 such that for all t T 1 we have p y M2 /k 1. If we also select M 1 and M 2 to satisfy 2M 2 M 1 g cos ψ cos γ, (11.13) then for all t T 1, the signal in σ M1 ( ) is not in saturation and Ẇ 1 = k 1 p 2 y p y σ M2 (ζ). (11.14) Define z = k 1 p y + p y, and W 2 = 1 2 z2, and differentiate W 2 to obtain Ẇ 2 = k 1 z p y zg cos χ cos γσ M1 ( k1 p y + σ M2 (ζ) g cos χ cos γ If we let ζ = k 2 z where k 2 > 0, then for all t T 1 we have Ẇ 2 = k 1 z p y k 1 z p y zσ M2 (k 2 z) = zσ M2 (k 2 z), which is negative definite. Therefore we are guaranteed that z = k 1 p y + p y 0. Using the standard result on input-to-state stability [28], Equation (11.14) guarantees that p y 0. Since both z = k 1 p y + p y and p y converge to zero, we can conclude that ỹ 0. To ensure that φ c φ, set M 1 = tan φ. To satisfy Equation (11.13) we also need to constrain χ and γ. The constraint on γ will be discussed momentarily. For chi note that if φ c = φ, then χ = g/v tan φ and χ increases monotonically. Similarly, if φ c = φ, then χ decreases monotonically. Therefore, if we can find χ max such that the set { χ χ max } is positively invariant, then we could use the following strategy for straight line tracking: φ φ c = φ ( ) k1 p σ y+σ M2 (k 2 z) M1 g cos χ cos γ if χ < χ max if χ > χ max otherwise. ).

147 Waypoint and Orbit Following 137 To find χ max, let W 3 = 1 2 χ2 and differentiate to obtain Ẇ 3 = g χ tan φc V = g ( V χσ V M 1 k 1 g tan χ + σ ) M 2 (k 2 z), (11.15) g cos χ cos γ where we have used Equation (11.9). Equation (11.15) is negative if k 1 V/g tan χ > M 2. Therefore, the set { χ χ max } is positively invariant, where χ max = gm 2 V k 1. Theorem If the commanded roll angle is given by ( ( ( ) V V sin χ + sat(v sin χ + ỹ, M2) φ c = tan 1 sat, M 1 g V cos χ where the gain k and the angle χ satisfy and where kχ < π 2 g tan φ, V 2 + χ p )), M 1 = (1 α) g V tan φ where M 2 = 1 2 (1 α)g tan φ cos χ max, α = 2kχ V 2 πg tan φ, and something about χ max, then χ(t) + ỹ(t) 0, and φ c φ. Proof: Define ν = g tan V φc χ p, then ( ) V sin χ + sat(v sin χ + ỹ, M2) ν = sat, M 1, V cos χ and χ = ν and ÿ = V cos χν. Consider the Lyapunov function candidate W 1 = 1 2 ỹ 2. Differentiating we get Ẇ 1 = ỹ ỹ = ỹv cos χν ( ) V sin χ + sat(v sin χ + ỹ, M2) = V cos χ ỹsat, M 1, V cos χ

148 Problem Description which is less than zero if ỹ > M2 and χ < π 2. Fix: Assume for the moment that χ < π for all time. 2 Then there exists a time T 1 > 0 such that for all t T 1, we have that ỹ M2. Noting that M 1 = 2M 1 V cos χ, we see that for all t T 1 we have that W 1 = ỹ 2 ỹsat( ỹ + ỹ, M2). Define z = ỹ + ỹ, and define the Lyapunov function candidate W 2 = 1 2 z2. Differentiating we get Ẇ 2 = z ỹ + z ỹ ( ) V sin χ + sat(v sin χ + ỹ, M2) = zv cos χsat, M 1 + z ỹ. V cos χ Now for all time t T 1 we have that Ẇ 2 = z ỹ zsat(z, M 2 ) + z ỹ = zsat(z, M 2 ), which is negative definite, implying that z(t) 0. Since Ẇ 1 = ỹ 2 ỹsat(z, M 2 ), we have that z 0 also implies that ỹ 0. Since z = ỹ + ỹ 0 and ỹ 0, we have that ỹ 0. To show the contraint on φ c we note that ν = g V tan φc χ p M1 = g V tan φc M1 + χ p Therefore φ c φ. QED M 1 + χ 2 π kv (1 α) g V tan φ + χ 2 π kv = g V tan φ 2kχ V 2 πg tan phi tan φ 2 + χ π kv = g V For the longitudinal dynamics assume that tan φ. ḣ = V sin γ c,

149 Waypoint and Orbit Following 139 where γ c is the commanded flight path angle. We will require that the commanded flight path angle is bounded by Γ so that γ c Γ < π 2. Theorem If the desired flight path angle satisfies tan γ q < sin Γ, and if the commanded flight path angle is given by where h = h h d, and γ c = sin 1 (ḣd sat( h, M 1 ) V ) h d = w + x 2 + ỹ 2 tan γ q, ( ) ḣ d x cos χ + ỹ sin χ = V tan x2 + ỹ 2 γ q,, and then h 0 and γ c (t) Γ. M 1 = V sin Γ V tan γ q, Proof: Consider the Lyapunov function candidate W = 1 2 h 2. Differentiating W we obtain Ẇ = h h = h ) (ḣ ḣ d = h ) (V sin γ c ḣd = h sat( h, M 1 ), which is negative definite, and therefore guarantees that h(t) 0. then Furthermore, if we let u u = ḣd sat( h, M 1 ), V ḣd V + M 1 V (11.16) tan γ q + M 1 V (11.17) = tan γ q + sin Γ tan γ q (11.18) = sin Γ. (11.19)

150 Problem Description Therefore, since γ c = sin 1 (u), we have that γ c Γ. QED Straight Path Following Consider the straight-line path shown in Figure To follow this path, a vector field of desired Figure 11.2: Vector field for straight line path following. Far away from the waypoint path, the vector field is directed with an angle χ e from the perpendicular to the path. ground track headings is constructed. Let y be the lateral distance of the MAV from the waypoint path, and let χ be the difference between the heading of the waypoint path and the ground track of the MAV. Our objective is to construct the vector field so that when y is large the MAV is directed to approach the waypoint path with entry angle χ e, and that as y approaches zero, the heading χ also approaches zero. Toward that end, define the desired ground track heading of the MAV as χ d (y) = χ e 2 π tan 1 (ky), (11.20) where k is a positive constant that influences the width of the transition region from χ e to zero. If χ e is restricted to be in the range χ e (0, π ] then clearly 2 π 2 < χe 2 π tan 1 (ky) < π 2

151 Waypoint and Orbit Following 141 for all values of y. Therefore since tan 1 ( ) is an odd function and sin( ) is odd over ( π 2, π 2 ) we can use the Lyapunov function W 1 (y) = 1 2 y2 with Lie derivative Ẇ 1 = V g y sin( χ d (y)) ( = V g y sin χ e 2 ) π tan 1 (ky) to argue that if χ = χ d (y), then y 0 asymptotically. In this paper we use a sliding mode approach to render the set S = {(y, χ) : χ = χ d (y)} positively invariant and to ensure that the system trajectory reaches S in finite time. Let χ = χ χ d (y) and differentiate to obtain χ = χ χ d (y) = α(χ c χ) + χ e 2 π Let W 2 = 1 2 χ2 and take the derivative to obtain Ẇ 2 = χ χ = χ ( χ χ d (y) ) = χ If we choose the control signal as where and κ > 0, then ( α(χ c χ) + χ e 2 π χ c = χ 1 α χe 2 π k 1 + (ky) 2 V g sin χ. (11.21) ) k 1 + (ky) V 2 g sin χ. k 1 + (ky) V 2 g sin χ κ sign( χ), (11.22) α 1 if x > 0 sign(x) = 1 if x < 0 from which we conclude that χ 0 in finite time [28]. Ẇ 2 κ χ (11.23) It is well known that the sign function leads to chattering in the control [28]. To mitigate the adverse effects of chattering, the control signal (11.22) is replaced with χ c = χ 1 2 k α χe π 1 + (ky) V 2 g sin χ κ ( ) χ α sat, (11.24) ɛ

152 Problem Description where x if x 1 sat(x) = sign(x) otherwise, and ɛ > 0 defines the width of the boundary region around the sliding surface. Theorem The system of equations given by (??) and (11.21), where χ c is given by (11.24) and χ d (y) is given by (11.20) is globally exponentially stable if 0 < χ e < π 2 and 2κk ɛv g ( ) 2χ e > 1. (11.25) π The stability condition (11.25) makes sense physically. If V g is large, than the turning radius of the MAV increases making it more difficult to remain within boundary layer. Similarly small χ e or small k implies that there will be a rapid change in χ d close to the waypoint path which will be difficult to track. Small ɛ makes the feedback control high gain which enhances tracking. It is interesting to note that (11.25) is independent of α. However, from (11.24) it is clear that to avoid unrealistic control effort, κ is required to be proportional to α. Therefore, (11.25) is easier to satisfy if α is large, implying that the on-board autopilot can quickly track heading changes. Proof: If χ ɛ, then χ c is equivalent to Equation (11.22) which results in Equation (11.23) implying that the set is positively invariant and that χ converges to S in finite time. S ɛ = { χ ɛ} (11.26) It remains to show that inside S ɛ the system trajectories converge to the origin (y, χ) = (0, 0). Toward that end define the Lypunov function candidate Differentiating we obtain Ẇ = yẏ + χ χ = yv g sin ( χ d (y) + χ ) + χα Inside the boundary region we have χ c = χ 1 α χe 2 π W = 1 2 y χ2. ( χ c χ + 1 ) 2χ e k α π 1 + (ky) V 2 g sin χ. k 1 + (ky) V 2 g sin χ κ χ α ɛ.

153 Waypoint and Orbit Following 143 Therefore Ẇ = V g y sin ( χ d (y) + χ ) κ ɛ χ2 = κ ɛ χ2 + V g y [ sin ( χ d (y) ) + ( sin ( χ d (y) + χ ) sin ( χ d (y) ))] κ ɛ χ2 + V g y sin ( χ d (y) ) + V g y sin ( χ d (y) + χ ) sin ( χ d (y) ). Noting that sin ( χ d (y) + χ ) sin ( χ d (y) ) = sin χ d (y) cos χ + cos χ d (y) sin χ sin χ d (y) = sin χ d (y)(cos χ 1) + cos χ d (y) sin χ cos χ 1 + sin χ 2 χ, we get Let Ẇ κ ɛ χ2 + 2V g y χ + V g y sin ( χ d (y) ) = κ ( ) 2χ e ɛ χ2 + 2V g y χ V g y sin π tan 1 (ky). ( ) φ(y) = 2χ e y sin π tan 1 (ky), and note that ( ) ( ) φ 2χ e 2χ e 2χ e y = k sin π tan 1 (ky) + y cos π tan 1 (ky) π 1 + (ky) 2 2χ e π tan 1 (ky) + 2χ e ky π 1 + (ky) 2 2 y 2χe π k. Using the fact that if f and g are continuous functions such that f(0) = g(0) and f (x) g (x) then f(x) g(x) we get Ẇ κ ɛ χ2 + 2V g y χ 2V g 2χ e ) = V g ( χ ( κ ɛv y g 1 π ky2 ) ( ) 1 χ 2χe π k y (11.27) which if negative definite if Equation (11.25) is satisfied. Exponential stability comes from the fact that W and the right hand side of (11.27) are quadratic [28]. QED

154 Problem Description Orbit Following The algorithm for circular orbits creates vector fields in a manner similar to the straight-line algorithm. Consider the desired orbit path shown in Figure In this discussion, a counter-clockwise orbit will be considered. The development for clockwise orbits is similar with the exception of several sign changes. The desired orbit is assumed to have a known center and radius r. When the distance between the MAV and the center of the orbit is large, it is desirable for the MAV to fly along a heading tangent to the orbit to be followed so that transitioning into the orbit can happen with minimal transient behavior. If we define d as the radial distance of the MAV from the center of the orbit, then when d is significantly larger than r the desired ground track heading is χ d = γ 5π 6 (11.28) where γ is defined as the angular position of the MAV with respect to the orbit center as shown in Figure When d = r the desired ground track heading is χ d = γ π. Therefore let the desired ground 2 track heading be given by χ d (d) = γ π 2 2 ( 3 tan 1 k d r ), (11.29) r where k > 0 is a constant that specifies the rate of transition from γ 5π 6 to γ π 2. For orbit following, it is convenient to change the navigational dynamics to polar coordinates in terms of d and γ where the center of the orbit is the origin. From Figure 11.3, x = c x + d cos γ and y = c y + d sin γ, where (c x, c y ) is the center of the orbit. Taking the derivative and substituting into Equations (??) and (??) gives d = V g cos(χ γ) (11.30) γ = V g d sin(χ γ) (11.31) where the V g and χ are the ground track speed and relative ground track heading, respectively. We will again assume that the dynamics for ground track heading are given by χ = α(χ c χ). (11.32) Defining d = d r then we can argue that d 0 asymptotically when χ = χ d (d) by using the

155 Waypoint and Orbit Following 145 γ d χ S χ 0 r Figure 11.3: Vector field geometry for orbit tracking. When the radial distance to the MAV is much greater than the orbit radius the desired heading is calculated so that the MAV is directed to be tangent to the orbit without changing heading. Lyapunov function W 1 = 1 d 2, whose Lie derivative is 2 ( Ẇ 1 = V g d cos π 2 2 ((k/r) 3 tan 1 d ) ) ( = V g d sin 2 ((k/r) 3 tan 1 d ) ) = V g d sin ( π 3 2 π tan 1 ( (k/r) d) ), is less than zero for d 0 since the argument of sin is in the set ( π/3, π/3) for all d. We will again use a sliding mode approach to render the set S = {( d, χ) : χ = χ d (d)} positively invariant and to ensure that the system trajectory reaches S in finite time. As before, define χ = χ χ d (d) and differentiate to obtain χ = α(χ c χ) χ d (d) = αχ + αχ c V g d sin(χ γ) + 2k 3r ( (k/r) d ) 2 V g cos(χ γ).

156 Problem Description Letting W 2 = 1 2 χ2 gives Ẇ 2 = χ α(χ c χ) V g d If we choose the control signal as χ c = χ + V g 2k sin(χ γ) αd 3rα sin(χ γ) + 2k 3r ( (k/r) d 1 + ) 2 1 ( (k/r) d ) 2 V g cos(χ γ). V g cos(χ γ) κ sign( χ), (11.33) α where κ > 0, then Ẇ 2 κ χ (11.34) from which we conclude that χ 0 in finite time [28]. To avoid chattering in the orbit case we replace (11.33) with χ c = χ + V g 2k sin(χ γ) αd 3rα ( (k/r) d ) 2 V g cos(χ γ) κ ( χ α sat ɛ where ɛ > 0 defines the width of the boundary region around the sliding mode. ), (11.35) Theorem The system of equations given by (11.30) and (11.32), where χ c is given by (11.35) and χ d (d) is given by (11.29) is globally exponentially stable if Proof: 4κk 3ɛV g r > 1. (11.36) The proof follows a similar line of reasoning as the proof of Theorem If χ ɛ, then the set S ɛ = { χ ɛ} is positively invariant and χ converges to S ɛ in finite time. Inside the boundary region we use the Lyapunov function candidate W = 1 2 d χ2 to obtain Ẇ = κ ɛ χ2 + V g d sin (ˆχ d (d) + χ )

157 Waypoint and Orbit Following 147 where ˆχ d (d) = ((k/r) 2 3 tan 1 d ). Following steps that are similar to the proof of Theorem we get Ẇ κ ɛ χ2 + 2V g d χ + V g d sin (ˆχ d (d) ) = κ ( 2 ɛ χ2 + 2V g d χ V g d sin ((k/r) 3 tan 1 d ) ) κ 4k ɛ χ2 + 2V g d χ 2V g 3r d 2 ) = V g ( χ ( ) ( ) κ ɛv y g 1 χ 1 y which if negative definite if (11.36) is satisfied. QED 4k 3r Straight Line and Orbit Combination Most paths planned for UAV flight can be approximated by combinations of straight-line segments and circular arcs. One example of how line segments and arcs can be combined is presented in [29]. Figure 11.4 shows how combined paths can be utilized with waypoint planning to fly paths that preserve equal path lengths, fly directly over the waypoints, or turn in order to minimize flight time. There are also a number of other situations where a combination would be desirable. For example, following a perimeter with irregular geometry could be done effectively by approximating that geometry with a series of lines and arcs. When there is a situation where a combination is desirable, the question becomes how to construct the vector fields. In order to avoid the possibility of multiple sinks, dead zones, and singularities that are inherent in the combination of vector fields, only the vector field for the current segment or arc to be followed is calculated. For a multi-segmented path, the vector field changes each time it is determined that the MAV has reached the end of a segment or arc. Once the MAV has reached the end of a segment or arc, the entire vector field changes to a field directing the MAV onto the next segment or arc. No two fields are ever combined, thus eliminating any issues related to the combining of fields. The method for determining when to change the vector field must be specified. There are a number of methods for doing this. One way is to detect when the MAV is within a predetermined distance from the end of the segment or arc. This works well for transitioning out of a straight path segment. For transitioning out of an arc, monitoring angular travel of the MAV has proven successful. Using this approach, the MAV transitions to the next path segment when the angle through which the MAV has flown is equal to the included angle of the arc.

158 Experimental Results equal length X (North) km through waypoint 4 2 minimum time Y (East) km Figure 11.4: Straight line paths and circular orbits can be combined to produce a variety of paths. The top figure shows the combination of two straight line paths and three circular orbits that are arranged so that if the MAV is on the path, the path lenght is equal to the original waypoint path. The middle figure arranges the orbits so that the desired path transitions over the waypoint. In the bottom figure a single orbit is used to transition between straight line segments Experimental Results In order to illustrate the orbit following abilities of the algorithm, the MAV was commanded to fly a series of concentric orbits with varying radii. The results are shown in Figure There was wind from the south of 2 to 3 m/s which corresponds to approximately 15 to 25 percent of the commanded MAV airspeed. The maximum deviation from the desired path was about 9 m and the average lateral error was approximately 3.4 m. Figure 11.6 illustrates the ability of the MAV to follow straight line segments with acute angles. Excluding the transient errors from the turns, the mean following error was 0.8 m (less than a wingspan) with a standard deviation of 1.1 m. A plot of the distance from the path can be seen in Figure The wind for this flight was out of the west and was again about 2 to 3 m/s. With the straight line and orbit following algorithms working well, the transition to a combina-

159 Waypoint and Orbit Following Figure 11.5: Telemetry plot for flying orbits with radii of 150, 100, 70, and 50 m. Note the repeatability of the circular orbits. tion of the two methods was also tested. The techniques described in Section?? were implemented and the results are plotted in Figure Winds were 30 to 50 percent of the MAV s airspeed. The thicker line is the desired path in order to conserve equal path lengths. The maximum deviation from the path was about 19 m and the mean distance from path and standard deviation were 3.4 m and 5.0 m respectively. The path following error is plotted in Figure Although the transitions from the straight line to the orbit portions show some lateral following errors, the actual path length flown and the desired length are very close. Five and a half loops through the path shown in Figure 11.8 and the desired length of the path was m. The actual distance flown was 17.5 m less than the desired distance which is an error of only 0.12 percent. To further test the robustness and cabapilities of the proposed path following algorithms many other types of paths have peen flown. The path shown in Figure illustrates both obtuse and acute angles and the decision of the trajectory follower to cut the corners of the obtuse angles and flare out and around on the acute angles. Figure shows a path that might be planned through an urban type terrain. Note that although these are actual flight results, the terrain is synthetic. The straight line follower was used to follow this path. The wind speed was 20 to 30 percent of the airspeed for both of these flights.

160 Conclusion Conclusion Figure 11.6: Telemetry plot for straight line following. In this chapter, a new method for MAV path following has been introduced. The idea of vector fields has been extended to constant altitude path following. It has been shown using Lyapunov stability criteria that controlling heading rate based on ground track heading and speed in a vector field yields asymptotic following for straight line and circular paths. The effectiveness of the described following methods have been illustrated using a Zagi fixedwing MAV and the Kestrel autopilot system. The MAV followed the desired paths with asymptotically decaying error. Minimal error was observed once the MAV had converged to the path. Vector field path following also proved effective in following smoothed paths composed of circular and straight line segments. All of the flight tests were performed in moderate (20 to 50 percent of MAV airspeed) wind conditions. The implementation of vector field following is straightforward and the result is a robust method for accurate path following. Controlling heading rate based on ground track heading and ground speed automatically accounts for wind conditions, providing tight following even in the presence of wind.

161 Waypoint and Orbit Following Distance (m) Figure 11.7: Lateral following error during straight line following Another approach to straight line tracking Tracking. The final implementation issue is due to the fact that at switching time t 5, the trajectory generator, i.e., ψ(0), ˆ may not be aligned perfectly with the waypoint segment w i w i+1 ; therefore setting u = 0 will cause it to drift from the waypoint segment. During the straight-line section a tracking algorithm is needed that causes the trajectory generator to asymptotically track the waypoint segment, while still satisfying the constraint c u c. Referring to Figure 11.12, let ψ d i = tan 1 (q iy /q ix ) be the angle created by the waypoint path. To simplify the development, we will shift the origin to w i 1 and rotate the data by ψi d. Accordingly we have ( ) e = ė = ( e e ė ė = R ( φ d i ) (z wi 1 ) ) ( ) v cos(ψ ψi d ) =. v sin(ψ ψi d ) The control objective is to drive e and ψ ψ d i to zero asymptotically, given the control constraint u [ c, c]. The following theorem describes how this can be done. Theorem Given the nonlinear system ė = v sin(ψ ψ d i ) ψ = u (11.37) with control constraint u [ c, c] where v is a positive constant. If [ ( )] cos 3 (ψ ψi d ) k 1 e u = sat c γ e 2 + v + k 2 2 sin(ψ ψi d ) (11.38)

162 Another approach to straight line tracking Figure 11.8: Telemetry plot for equal path length following. where γc > 1, 0 < k 1 < γc, and k 2 > 0, then the origin is asymptotically stable and the domain of attraction is given by Ω = (, ) (ψ d i π/2, ψ d i + π/2). Proof: Using the change of variables ζ 1 = e /v ζ 2 = sin(ψ ψ d i ) (11.39) Distance (m) Figure 11.9: Lateral following error for combined orbit/straight line following.

163 Waypoint and Orbit Following Figure 11.10: Combination of equal path length and corner cutting following. and the Lyapunov function V (ζ) = k 1 ζ γ 2 ζ 2 2, 1 ζ2 2 the proof follows by standard Lyapunov arguments. For details see [30]. The phase portrait of system (11.37) with the control (11.38) after the transformation (11.39) is shown in Figure for several values of γ, k 1 and k 2. Figure shows the stability of the tracking error and the qualitative dependence of the transients on the parameters. Comparing the subplots on the left, it is seen that the damping characteristics are strongly dependent on k 2. Comparing the top subplots we see that the rise time of the tracking error is strongly dependent on k 1. It can also be seen that tracking response is not significantly influenced by γc.

164 Another approach to straight line tracking Figure 11.11: Urban terrain following using straight line following. z ż ϕi e e w i w i-1 q i e ϕ i d Figure 11.12: Waypoint tracking

165 Waypoint and Orbit Following γ c = 10, k 1 = 5, k 2 = 10 1 γ c = 10, k 1 = 1, k 2 = ζ 2 0 ζ ζ 1 ζ 1 1 γ c = 10, k 1 = 5, k 2 = 1 1 γ c = 5.1, k 1 = 5, k 2 = ζ 2 0 ζ ζ 1 ζ 1 Figure 11.13: Phase portrait for the transformed tracking problem.

166 Design Project 11.5 Design Project 11.1 Homework problem 1.

167 Chapter 12 Path Planning In the robotics literature, there are roughly two different approaches to motion planning: deliberative motion planning where explicit paths and trajectories are computed based on global world knowledge [31, 32, 33], and reactive motion planning which uses behavioral methods to react to local sensor information [34, 35]. In general, deliberative motion planning is useful when the environment is known a priori, but can become computationally intensive in highly dynamic environments. Reactive motion planning, on the other hand, is well suited for dynamic environments, particularly collision avoidance, where information is incomplete and uncertain, but lacks the ability to specify and direct motion plans. This chapter focuses on deliberative path planning techniques that have proved effective and efficient for micro air vehicles. In deliberative approaches, the MAVs trajectories are planned explicitly. The drawback of deliberative approaches is that they are strongly dependent upon the models used to describe the state of the world and to describe the motion of the vehicle. Unfortunately, precise modeling of the atmosphere and the vehicle dynamics is not possible. Therefore, to compensate for the inherent uncertainty, the path planning algorithms will need to be executed on a regular basis in an outer feedback loop. Therefore, it is essential that the path planning algorithms are computationally efficient. To reduce the computational demand, we will use simple low-order navigation models for the vehicle, and constant wind models for the atmosphere. We assume that a terrain elevation map is available to the path planning algorithms. Obstacles that are known a priori are represented on the elevation map. This chapter describes several simple and efficient path planning algorithms that are suitable for micro air vehicles. In reference to the architecture shown in Figure 1.1, this chapter describes the design of the Waypoint path planner and the Path manager. We will describe path planning algorithms for two types of problems. In Section 12.1 we will first address point-to-point problems where the objective is to plan a waypoint path from one point to another through an obstacle field. 157

168 Point-to-Point Algorithms In Section 12.2 we will then address coverage problems where the objective is to plan a waypoint path so that the MAV covers all of the area in a certain region. The path planning algorithms described in sections 12.1 and 12.2 do not recognize the kinematic or dynamic constraints of the MAV. Kinematic constraints are addressed in Section?? where we describe several techniques that convert a waypoint path to sequence of straight lines and orbits that can be tracked using the techniques described in Chapter Point-to-Point Algorithms Voronoi Graphs The Voronoi graph is particularly well suited to applications where the UAV is tasked to maneuver through a congested airspace with obstacles that are small relative to the turning radius of the UAV. The relative size allows the obstacles to be modeled as points with zero area. Given a set P of P points in R 2, the Voronoi graphs divides R 2 into P convex cells, each containing exactly one point in P. The Voronoi graph is constructed so that the interior of each convex cell is closer to its associated point, than to any other point in P. An example of a Voronoi graph is shown in Figure North (km) East (km) Figure 12.1: An example of a Voronoi graph with P = 20 point obstacles. The key feature of the Voronoi graph that makes it useful for UAV path planning is that the

169 Path Planning 159 edges of the graph are perpendicular bisectors between the points in P. Therefore, following the edges of the Voronoi graph potentially produces points that avoids the points in P. However, Figure 12.1 illustrates several potential pitfalls in using the Voronoi graph. First, graph edges that extend to infinity are obviously not good potential waypoint paths. Second, even for Voronoi cells with finite area, following the edges of the Voronoi graph will lead to unnecessarily long excursions. Finally, note that for the two points in the lower right hand corner of Figure 12.1, the Voronoi graph produces an edge between the two points, however, since the edge is so close to the points, the corresponding waypoint path for a UAV may not be feasible. There are well established and widely available algorithms for generating Voronoi graphs. For example, Matlab has a built in Voronoi function, and C++ implementations are publicly available on the internet. Given the availability of Voronoi code, we will not discuss implementation of the algorithm. For additional discussions see [36, 37, 38]. To use the Voronoi graph for point-to-point path planning, we first augment set set P with the start and end locations: P + = P {v start } {v end }. The Voronoi graph associated with P + is then augmented with edges that connect the start and end nodes to the closest nodes in Voronoi(P). The associated graph is shown in Figure North (km) East (km) Figure 12.2: The Voronoi graph of P + is augmented with edges that connect the start and end nodes to the Voronoi graph of P + The next step is to assign a cost to each edge in the Voronoi graph. Edge costs can be assigned

170 Point-to-Point Algorithms in a variety of ways. For illustrative purposes we will assume that the cost of traversing each path is a function of the path length and the closeness of the points. The geometry that illustrates the metric is shown in Figure Let the nodes of the graph edge be denoted by n 1 and n 2. The Figure 12.3: The cost penalty assigned to each edge of the Voronoi graph is proportional to the path length n 1 n 2 and the reciprocal of the minimum distance from the path to a point in P. length of the edge is given by n 1 n 2. Any point on the line segment can be written as w(σ) = (1 σ)n 1 + σn 2, where σ [0, 1]. The minimum distance between p P and the graph edge can be expressed as C(n 1, n 2, P) = min p w(σ) σ [0,1] = min (p w(σ))t (p w(σ)) σ [0,1] = min p T p 2(1 σ)σp T n 1 σp T n 2 + (1 σ) 2 n T 1 n 1 + 2(1 σ)σn T 1 n 2 + σ 2 n T 2 n 2 σ [0,1] = min p n σ(p n 1 ) T (n 1 n 2 ) + σ 2 n 1 n 2 2. σ [0,1] If σ is unconstrained, then its optimizing value is and If we define w(σ ) = σ = (n 1 p) T (n 1 n 2 ) n 1 n 2 2, p n 1 2 ((n 1 p) T (n 1 n 2 )) 2 n 1 n 2 2. w(σ ) if σ [0, 1] C (n 1, n 2, p) = p n 1 if σ < 0 p n 2 if σ > 1,

171 Path Planning 161 then we have that C(n 1, n 2, P) = min p P C (n 1, n 2, p). The cost for the edge defined by (n 1, n 2 ) is assigned as where γ 1 and γ 2 are positive weights. J(n 1, n 2 ) = γ 1 n 1 n 2 + γ 2 C(n 1, n 2, P), The final step is to search the Voronoi graph to determine the lowest cost path from the start node to the end node. Numerous graph searching techniques exist that would be appropriate to accomplish this task [38]. A well known algorithm with readily available code is the Dijkstra s algorithm[13] which has a computational complexity equal to ( V ). An example of a path found by Dijkstra s algorithm with γ 1 = 0.1 and γ 2 = 0.9 is shown in Figure North (km) East (km) Figure 12.4: Optimal path through the Voronoi graph Rapidly Exploring Random Trees We assume that obstacles are represented in a terrain map that can be queried to detect possible collisions. A tree data structure is a special case of a directed graph. Figure 12.5 is a graphical depiction of a tree. Edges in trees are directed from a child node to its parent. In a tree, every node has exactly one parent, except the root which does not have any parents. We will represent physical locations

172 Point-to-Point Algorithms by nodes, and waypoint paths between the nodes by edges. The cost associated with each edge, c ij, is the cost associated with traversing the straight-line path between the waypoints represented by the associated nodes. Figure 12.5: A tree is a special graph where every node has exactly one parent. The basic idea of the Rapidly Exploring Random Tree (RRT) Algorithm is to build a tree that uniformly explores the search space. The uniformity is achieved by randomly sampling from a uniform probability distribution. Figure 12.6 depicts the basic RRT algorithm. As shown in subfigure 12.6(a), the input to the RRT algorithm is a start waypoint w start, an end waypoint w end, and the terrain map. The first step of the algorithm is to randomly select a point p in the workspace. As shown in subfigure 12.6(b), a new waypoint w 1 selected a fixed distance D from w start along the line pw start, and inserted into the tree. At each subsequent step, a random point p is generated in the workspace, and the tree is searched to find the node that is closest to p. A shown in subfigure 12.6(c) a new waypoint is generated that is a distance D from the closest node in the tree, along the line connecting p to the closest node. Before a waypoint path is added to the tree, it needs to be checked for collisions with the terrain. If a collision is detected, as shown in subfigure 12.6(d), then the waypoint is deleted and the process is repeated. When a new node is added, its distance from the end node w end is checked. If it is less than D, then a waypoint path from w end is added to the tree, and as shown in subfigure 12.6(f), a complete path through the terrain has been found.

173 Path Planning 163 (a) (b) (c) (d) (e) (f) Figure 12.6: (a) RRT algorithm. (b) RRT algorithm. (c) RRT algorithm. (d) RRT algorithm. (e) RRT algorithm. (f) RRT algorithm.

174 Point-to-Point Algorithms (a) (b) (c) (d) Figure 12.7: (a) RRT results. (b) RRT results. (c) RRT results. (d) RRT results.

175 Path Planning Coverage Algorithms Zamboni Algorithms Exploration Maps

176 Design Project 12.3 Design Project 12.1 Homework problem 1.

177 Chapter 13 Path Manager 13.1 Switching Between Waypoints In this section we address the problem of switching from one waypoint segment to another waypoint segment. We assume that the path following algorithm described in Section is used to track each straight line segment. Consider the scenerio shown in Figure 13.1 where a MAV is currently tracking the straight-line segment from w 1 to w 2. Intuitively, when the MAV reaches w 2, we desire to switch the guidance algorithm so that the MAV will track the straight-line segment from w 2 to w 3. What is the best method for determining whether the MAV has reached w 2. One possible method is to switch when the MAV enters a ball around w 2. In other words, the guidance algorithm could switch at the first time instant when r(t) w 2 ) B, where B is the size of the ball. However, if there are disturbances like wind, or if the w 1 w 2 segment is short and the tracking algorithm has not had time to converge, then the MAV may never enter the B-ball around w 2. A better approach that is not sensitive to tracking errors is to use a half plane switching criteria. In reference to Figure 13.1, the MAV tracks the path w 1 w 2 while it is in half-plane H 1. When it enters half plane H 2 it begins to track the path w 2 w 3. In this section we discuss several implementation issues that must be addressed to ensure robust behavior of the trajectory smoother when it is implemented in digital hardware. Real-time implementation requires that the differential equations (13.8) (13.10) be solved via a numerical ODE solver (e.g., Runge-Kutta) using a fixed time-step. The fixed time-step creates several problems for detecting the switching times shown in Figure For example, suppose that the trajectory 167

178 Switching Between Waypoints Figure 13.1: When transitioning from one waypoint path to another, a criteria is need to indicated when the MAV has completed the first straight-line segment. The first option is to switch when the MAV enters a B-ball around the transition waypoint. The second option is to switch when the MAV transitions from half-plane H 1 to half-plane H 2. smoother is tracking the straight-line segment w i 1 w i and the algorithm is looking for the intersection of C L with C p(κ) in order to detect the switching time t 2, then we may have the scenario depicted in Figure The circle C L may not intersect the circle C p(κ) exactly at the sample times. Therefore, we need a robust method for detecting circle and line intersections and for indicating when the intersection has been missed. Toward that end, define the function {[( ) ( )] } Ψ(a, b) = a b sign ˆk 0 0 = sign [a x b y b x a y ] (13.1) where ˆk is the unit vector pointing into the plane. The function Ψ can be used for several purposes. First, if q i and q i+1 are unit vectors along the vectors w i 1 w i and w i w i+1, as shown in Figure 13.6, then the direction of the turn is given by Ψ(q i, q i+1 ), where Ψ(q i, q i+1 ) > 0 indicates that the turn from the current path segment to the next path segment will be clockwise (a right-handed turn), and Ψ(q i, q i+1 ) < 0 indicates that the turn will be counterclockwise (a left-handed turn). The function Ψ can also be used to partition the R 2 plane into two distinct halves, as shown in Figure 13.3.

179 Path Manager 169 w i+1 w i C p(κ) C (t[n+1]) L C L (t[n]) w i-1 Figure 13.2: Effect of fixed sample-rate on switching-time detection Smooth transitions that satisfy kinematic constraints 13.3 Smooth transitions through the waypoint 13.4 Smooth transitions that preserve path length - Simulation results for the trajectory smoother are shown in Figure?? for minimum-time transitions (κ = 1), transitions through the waypoint (κ = 0), and transitions matching the length of the original straight-line path (κ variable between 0 and 1). The smoothing algorithm was run in real time as the path was flown. As a turn on the path is completed, the smoothing algorithm looks to the next two waypoints and calculates the smooth transition for the next turn. For the minimum-time transitions shown in Figure??, it can be seen that the location of the center of the turn circle corresponding to κ = 1 varies depending on the angle β between adjoining path segments. The same is true for the κ = 0 turn circles of the paths passing through the waypoints. For the equal-length path, the κ values for each turn that result in equal path lengths depend on the angle β between adjoining segments. For the last four turns of the equal-length path the κ values were 0.266, 0.136, 0.406, and Typically, the more acute angles require smaller

180 Path Smoothing Ψ( x-a, q ) > 0 x-a x Ψ( x-a, q ) < 0 q a Figure 13.3: Ψ divides R 2 into two distinct sub-regions. κ values to equalize the length Path Smoothing This section is taken primarily from [29]. This section presents a real-time, feasible trajectory generation algorithm for unmanned air vehicles (UAVs) flying through a sequence of waypoints. The algorithm produces extremal trajectories that transition between straight-line path segments in a time-optimal fashion. In addition, the algorithm can be configured so that the dynamically feasible trajectory has the same path length as the straight-line waypoint path. Implementation issues associated with the algorithm are described in detail. Simulation studies show the effectiveness of the proposed method Introduction Recent advances in computing, sensing, and battery technology have made unmanned vehicles a viable option in both military and commercial applications[39]. For unmanned vehicle technology to be useful, autonomous, semi-autonomous, and cooperative behaviors must be developed. Central to these behaviors are automatic trajectory generation algorithms. Our particular interest has been cooperative timing problem for UAVs[40, 41], where the trajectories must be both feasible, and satisfy stringent timing or path length constraints. Our ap-

181 Path Manager X (North) km through 5 minimum waypoint equal time length Y (East) km Figure 13.4: Sample trajectories proach decomposes the trajectory generation problem into two steps: a waypoint path planning step, where the straight-line paths are not time-parameterized, and a time-parameterized trajectory generation step to smooth the waypoint paths into dynamically feasible trajectories [42]. This paper describes the trajectory generation procedure. The salient features of our approach are that the trajectory generator (1) smoothes through a set of waypoints, minimizing the deviation from the associated waypoint path, (2) satisfies the curvature and velocity constraints imposed by the dynamics of the UAV, (3) maintains the path length of the associated waypoint path, and (4) operates in real-time. While the trajectory generation technique introduced in this paper is well suited to cooperative timing missions, it can also be used in problems where timing may not be critical. For example, it may be desirable to pass directly over the waypoints while minimizing the deviation from the original waypoint path. Another possible scenario is that the UAV could carry a sensor with a finite dimensional footprint[43]. It may be desired to configure the trajectory generator so that the UAV passes within a specific distance of the waypoint while minimizing the transition time between the straight-line path segments. The technique presented in this paper is well matched to these scenarios.

182 Path Smoothing Our approach is based on the local reachability region of the aircraft and on basic geometry. In that sense, it is similar to the approaches reported in [40, 44, 45]. In [44] it is shown that the shortest path between two points satisfying curvature constraints comprises circles and straight-line path segments. Reference [45] builds upon Dubins s ideas to generate feasible trajectories for UAVs given kinematic and path constraints by algorithmically finding the optimal location of Dubins circles and straight-line paths. In [40], Dubins circles are superimposed as fillets at the junction of straight-line waypoint paths produced from a Voronoi diagram. Our approach differs from [40, 44, 45] in that the trajectory is generated in real-time by a dynamic process. Rather than inserting fillets and planning the trajectory a priori, our approach dynamically generates the trajectory in flight. The advantage of doing this is that the algorithm is more reactive to dynamically changing environments and temporally distributes the computations, making it feasible to implement in realtime. Several other approaches to UAV path planning have recently appeared in the literature including probabilistic maps [46], differential flatness [47, 48], and optimal control techniques [49, 50] Problem Statement and Preliminaries We will assume that the UAV is flying at a constant altitude and, following [51], that the UAV dynamics subject to velocity-hold and heading-hold autopilots are first order: ż x = v cos ψ (13.2) ż y = v sin ψ (13.3) ψ = α ψ (ψ c ψ) (13.4) v = α v (v c v) (13.5) where α ψ and α v are known constants that depend on the implementation of the autopilot. In addition, the underlying UAV dynamics constrain the heading rate and velocity as follows: c ψ c (13.6) 0 < v min v v max. (13.7) Definition A trajectory z d (t) = (z d x, z d y) is called dynamically feasible if there exist inputs ψ c (t) and v c (t) such that z(0) = z d (0) implies that z(t) = z d (t), and the dynamics (13.2) (13.5) and constraints (13.6) (13.7) are satisfied, for all t 0.

183 Path Manager 173 The input to the trajectory generator is a waypoint path P = {v, {w 1, w 2,..., w N }} where v [v min, v max ] is the desired velocity of the UAV, and w i R 2 denote the waypoints expressed in inertial coordinates. In our approach, the trajectory generator is given by ẑ x = ˆv cos ˆψ (13.8) ẑ y = ˆv sin ˆψ (13.9) ˆψ = u (13.10) where u [ c, c] is an input that will be selected to meet the specified objectives. Note that Eqs. (13.8) (13.10) have a mathematical structure similar to the kinematics of the UAV. We will assume that the trajectory is to be traversed at a constant velocity ˆv [v min, v max ]. Note that if ẑ(0) = z(0), then Equations (13.8) (13.10) are guaranteed to generate dynamically feasible trajectories. Therefore the feasibility issue is satisfied a priori. Equations (13.8) (13.10) are solved via a fixed-step ODE solver and propagated in real-time. In other words, the output of the trajectory smoother corresponds in time to the evolution of the UAV dynamics. If a forth-order Runge-Kutta algorithm [52] is used, then u will need to be computed four times each sample period. Therefore, the computational complexity depends on the computation of u. Note that if u = +c, then the trajectory smoother given in Equations (13.8) (13.9) traces out a right-handed circle given by ( c R (t) = ẑ(t) + R sin( ˆψ(t)) ˆψ(t))) cos(. (13.11) Similarly, if u = c, then the trajectory smoother traces out the left-handed circle ( c L (t) = ẑ + R sin( ˆψ(t)) ˆψ(t))) cos(. (13.12) The local reachability region of the trajectory smoother is bounded by these two circles which have a radius given by R = ˆv/c. Note that as the desired velocity increases, the minimum turning radius increases. Since the boundaries of the reachability region are given by circles with known centers and radii, finding the intersection of the reachability region with lines and circles can be done in a computationally efficient manner Extremal Trajectories In this section we define a class of dynamically feasible trajectories called κ-trajectories, and show that they are minimum-time extremal trajectories.

184 Path Smoothing w i+1 w i q i+1 q p C R β Consider the waypoint path defined by the three waypoints w i 1, w i, and w i+1, and let q i = (w i w i 1 ) / w i w i 1 and q i+1 = (w i+1 w i ) / w i+1 w i denote unit vectors along the corresponding path segments as shown in Figure Letting β denote the angle beq i w i-1 Figure 13.5: Inscribed circle used in the definition in κ-trajectories. tween q i and q i+1 we get β = cos 1 ( q i+1q i ). As shown in Figure 13.5, let C be a circle of radius R = ˆv/c whose center lies on the bisector of the angle formed by the three waypoints, such that the circle intersects both the lines w i 1 w i and w i w i+1 at exactly one point each. The bisector of β will intersect C at two locations. Let p be the intersection that is closest to w i. From Figure 13.5, it can be seen that sin ( β 2 ) = R/ ( p wi + R). Manipulating this expression, the distance between w i and p is found to be p w i = R ( 1/ sin ( β 2 ) 1 ). A unit vector pointing along the bisector of the angle formed by the three waypoints is given by q = q i+1 q i q i+1 q i. (13.13) The point p is therefore given by p = w i + R ( 1/ sin ( ) ) β 2 1 q. Let p(κ) denote a parameterized point on the line between w i and p, on the bisector of the angle: ( ) 1 p(κ) = w i + κr sin ( ) 1 q (13.14) β 2 where κ [0, 1]. Clearly p(0) = w i, and p(1) = p. As shown in Figure 13.6, let C p(κ) be a circle of radius R = ˆv/c whose center lies in the direction of q and intersects p(κ). Also, let C i be a circle of radius R placed such that it intersects C p(κ) and w i 1 w i in exactly one location each. Define C i+1 similarly, as shown in Figure 13.6.

185 Path Manager 175 Definition A κ-trajectory is defined as the trajectory that is constructed by following the line segment w i 1 w i until intersecting C i, which is followed until C p(κ) is intersected, which is followed until intersecting C i+1 which is followed until the line segment w i w i+1 is intersected, as shown in Figure C i+1 w i+1 w i C p(κ) β p(κ) C i w i-1 Figure 13.6: A dynamically feasible κ-trajectory. Theorem The κ-trajectory shown in Figure 13.6 is the unique, dynamically feasible, minimumtime-extremal trajectory that transitions from the waypoint segment w i 1 w i to the waypoint segment w i w i+1, passing directly through p(κ). Proof: The proof requires a change of variables followed by a standard application of Pontryagin s Minimum Principle (cf. [53]). For details see [54, 30] Dynamic Trajectory Smoothing This section describes a real-time algorithm that generates κ-trajectories for κ [0, 1]. If κ = 1, then the κ-trajectory transitions from the waypoint segment w i 1 w i to the waypoint segment w i w i+1 in minimum time. If κ = 0, then the κ-trajectory executes a minimum-time transition subject to the constraint that it passes directly over w i. We will also show in this section how to select κ so that the κ-trajectory has the same path length as the original waypoint path. The essential idea for the algorithm is shown in Figure The right and left turning con-

186 Path Smoothing C L w i+1 C L t 6 C R C L t 4 C p(κ) t 5 β t 3 t 2 C L C L C R t 1 w i-1 Figure 13.7: Graphical depiction of the essential idea behind the selection of u. straints given in Eqs. (13.11) (13.12) are denoted by C R and C L respectively at different time instances. The progression of time is denoted by t 1,... t 6. The right turning constraint C R is not shown at times t 2 and t 5 to avoid cluttering the figure. At time t 1 the trajectory smoother is tracking the waypoint segment w i 1 w i. When the left turning circle C L intersects C p(κ) at time t 2, u is set to c. The left turning constraint is followed until the right turning circle C R corresponds exactly with C p(κ) at time t 3. The trajectory smoothing input u is then set to +c and the right turning constraint is followed until the left turning constraint C L intersects the waypoint segment w i w i+1 at time t 4. The input u is again set to c until it reaches the waypoint segment at time t 5, where u is set to zero. For κ [0, 1), a flow chart of the trajectory smoothing selection scheme for u is shown in Figure The nominal state of the trajectory smoothing algorithm is to track the current waypoint path segment. Since the ODEs (13.8) (13.10) are solved using a fixed-sample-rate solver, it is not possible to track the path by simply setting u = 0. A suitable tracking algorithm is discussed in Section When the trajectory smoother begins tracking the current waypoint segment, the location of C p(κ) is computed. If the turn is clockwise, then the constraint circle C L is monitored until it intersects C p(κ), at which point u c (t 2 in Figure 13.7). When u = c, the motion of the trajectory smoother is such that C L is stationary. The constraint circle C R is monitored until it coincides exactly with C p(κ), at which time u +c (time t 3 ). C R is now stationary and C L is monitored until it intersects the line segment w i w i+1 from the right, at which time u = c (time

187 Path Manager 177 ( C ) L intersects C set u = -c set u = +c set u = -c R = C p(κ) ( C ) p(κ) ( C intersects w i w L i+1 from right ) ( C does not intersects R w i wi+1 ) track path w i-1 w i set u = 0 i = i + 1 ( C ) L = C R intersects C ( C ) ( ) p(κ) intersects R from left set u = +c set u = -c C w i w i+1 set u = +c p(κ) ( C does not intersects L w i wi+1 ) Figure 13.8: Block diagram of the trajectory smoothing algorithm for κ [0, 1). t 4 ). The constraint circle C L is stationary and C R is monitored until it only intersects w i w i+1 at one point, at which time tracking is resumed (time t 5 ). Similar steps are followed if the turn is counterclockwise. The switching times are determined by finding circle and line intersections. Practical issues associated with finding the switching times in digital hardware are discussed in Section If κ = 1, then the trajectory smoothing algorithm simplifies considerably. Similar to the case when κ [0, 1), the first step is to determine C p(κ) and the direction of the turn. As shown in Figure 13.9, for a clockwise turn the trajectory smoother tracks the straight-line path segment w i 1 w i until C R coincides with C p(κ), at which point u +c (t 2 in Figure 13.9). C R is then stationary and C L is monitored until the entire circle is to the left of w i w i+1 (time t 3 ), at which time tracking is resumed. The following theorem asserts that the trajectory smoothing algorithm implements the extremal κ-trajectories defined in Section Theorem The trajectory smoothing algorithm depicted in Figure 13.8, implements the extremal κ-trajectories defined in Section Proof: See [54]. The trajectory smoother can be configured to run in several modes, depending on the application for which it is being used. For example, it may be desirable to choose κ so that the trajectory

188 Path Smoothing C L w i+1 C L t 4 C R t 3 w i C p(κ) t 2 C L C R t 1 C L w i-1 Figure 13.9: Trajectory smoothing algorithm for κ = 1. passes a distance D from the waypoint. This mode could be used to ensure that the footprint of a sensor attached to a UAV passes over the waypoint. If D [0, R ( 1/ sin β 2 1) ], then using Equation (13.14), it is straightforward to show that the proper choice of κ is κ = D sin β 2. R 1 sin β 2 If it is desired that the trajectory pass directly over the waypoint, then choose κ = 0. On the other hand, if it is wished to transition between waypoints with only one turn, choose κ = 1. For timing critical missions it is often desirable to plan the mission based on the waypoint paths. However, if the trajectory smoothing process changes the path length of the waypoint path, then the timing of the mission will be compromised. Therefore, it is desirable to choose κ such that the path length of the κ-trajectory is equal to the path length of the waypoint path. Toward that end, the next Lemma derives an analytic expression for the path length of a κ-trajectory. Lemma If κ [0, 1], and R = ˆv/c, then the path length of the κ-trajectory shown in

189 Path Manager 179 Figure 13.6 is given by L(κ, w i+1, w i, w i 1 ) = w i+1 w i + w i w i 1 ( ) π β + 2R + 2 cos 1 1 (Ξ(κ, β)) Ξ(κ, β) 2 Ξ(κ, β) 1 (1 κ) cos β 2 2 κ cot β 2 (13.15) where ( ( ) ( ) ) β = cos 1 wi+1 w i wi w i 1 (13.16) w i+1 w i w i w i 1 and Ξ(κ, β) = (1 + κ) + (1 κ) sin β 2 2 Proof: The proof is an application in geometry. See [54, 30] for details.. (13.17) From Lemma , it is clear that the path length difference between the waypoint path w i 1 w i w i+1 and the associated κ-trajectory is given by Λ(κ, w i+1, w i, w i 1 ) = 2R ( π β cos 1 1 (Ξ(κ, β)) Ξ(κ, β) Ξ(κ, β) 1 2 (1 κ) cos β 2 κ cot β 2 ). (13.18) The following lemma will be used to find κ such that the κ-trajectory has the same path length as the waypoint trajectory. Lemma If β [0, π) and κ [0, 1], then Λ is a decreasing function of κ. In addition, Λ(0, w i+1, w i, w i 1 )) > 0 and Λ(1, w i+1, w i, w i 1 ) < 0. Proof: Λ is shown to be decreasing by differentiating Equation (13.18) with respect to κ and showing that Λ < 0. The endpoint inequalities require application of the Lagrange Remainder κ Theorem. Details are given in [54, 30]. The next theorem shows that there exists a κ [0, 1] such that the path lengths are equal. Theorem If Λ(κ, w i 1, w i, w i+1 ) is given by Equation (13.18), where β [0, π) is given by Equation (13.16) and R = ˆv/c, then there exists a unique κ [0, 1] such that Λ(κ ) = 0.

190 Path Smoothing Furthermore, the κ-trajectory corresponding to κ has the same path length as the waypoint path w i 1 w i w i+1, i.e., L = w i w i 1 + w i+1 w i. In addition, a bisection search algorithm can be used to determine κ with accuracy (2 n ) where n is the number of function evaluations of Λ. Proof: From Lemma we know that Λ is the difference in path length between the κ- trajectory and the waypoint path. Therefore, if Λ = 0, the path lengths are equal. From Lemma we know that there is a unique κ = [0, 1) such that Λ(κ ) = 0. Since Λ(0) > 0 and Λ(1) < 0 the first step of a bisection search is at κ = 0.5. The sign of Λ(0.5) determines κ to within 2 1. Subsequent function calls further refine the estimate Implementation Issues Referring to Figure 13.7, we will discuss the robust detection of switching times t 2 through t 5. Switching time t 2. Recalling that the center of circle C L is c L and that the center of circle C p(κ) is p(κ), it can be seen from Figure that the distance between the center of C L and C p(κ) is given by c L p(κ). Prior to switching time t 2, this distance is greater than 2R; after switching time t 2, this distance is less than 2R. Therefore the switching time t 2 can be determined by monitoring c L p(κ), assuming that at some sample time this quantity is less than 2R. However, if κ 1, then it is possible that c L p(κ) > 2R at the sample times both before and after the intersection at time t 2, as shown in Figure Therefore, it is also necessary to detect when c L has moved from the half plane S 1 to the half plane S 2. Guided by Figure 13.3, this can be accomplished by monitoring the sign of Ψ (c L p(κ), R(π/2)q i ), where ( ) cos φ sin φ R(φ) =. sin φ cos φ If Ψ (c L p(κ), R(π/2)q i ) > 0, then c L S 1, otherwise c L S 2. Switching time t 3. The switching time t 3 can also be detected via a half-plane argument. As shown in Figure 13.11, t 3 occurs when the center of circle C R (c R ) transitions from S 1 to S 2, which can be detected with the function ( ) p(κ) c L Ψ c R c L,. p(κ) c L

191 Path Manager 181 w i+1 w i p(κ) C p(κ) C (t[n+1]) L c L S2 S1 c L C L (t[n]) w i-1 Figure 13.10: The effect of finite sample-times for κ 1. Switching time t 4. Robustly detecting switching time t 4 can be accomplished as follows. First, reflect the location of c L at switching time t 3 through the line that bisects the angle β. The unit vector that defines the bisector of β is given by Equation (13.13). The reflection of a vector through the line described by q is given by the Householder transformation [13] H( q) = ( ) 1 2 q y 2 2 q x q y. 2 q x q y 1 2 q x 2 Therefore, as shown in Figure 13.12, we have c L (t 4 ) = w i + H( q) (c L (t 3 ) w i ). The next step is to wait until c L crosses into the half plane defined by q that contains c L (t 4 ). Referring to Figure 13.12, we are looking for a transition from half plane S 1 to S 2. This is robustly identified when Ψ(c L w i, q) switches from 1 to +1. The last step is to look for the intersection of C L with the line segment w i w i+1. With reference to Figure 13.12, this is robustly identified when c L crosses from half plane S 3 into half plane S 4, or in other words, when switches from 1 to +1. ( ) p(κ) c L (t 4 ) Ψ c L c L (t 4 ), p(κ) c L (t 4 ) Switching time t 5. The detection of switching time t 5 is similar to the detection of switching time t 3.

192 Dubin s Paths C p(κ) w i+1 w i c R C (t[n+1]) R c L S2 S1 c R C R (t[n]) w i-1 Figure 13.11: Switching time t Dubin s Paths Before proceeding we need strategies to get on the orbits from arbitrary locations. To do so, we will use the concept of Dubin s paths. It was shown in [44] that for a vehicle with kinematics given by ẋ = V cos χ ẏ = V sin χ χ = u, where V is constant and u [ ū, ū], that the time optimal path between two different configurations consists of straight lines and circles with radius V/ū. g V For MAV s we will assume that the coordinated turn constraint holds which implies that ū = tan φ. Therefore, the turning radius is given by where φ = [ φ, φ]. R = V 2 g tan φ,

193 Path Manager 183 S 3 S 4 c L (t ) 4 w i+1 S 2 S 1 w i q p(κ) c L (t ) 3 w i Definition of Dubin s Path Figure 13.12: Switching time t 4. If R is fixed, then holding a roll angle of φ = tan 1 V 2 produces a right handed orbit as shown gr in Figure Alternatively, holding a roll angle of φ = tan 1 V 2 produces a left handed orbit as shown in Figure The center of the right handed orbit is denoted as c r and the center of the left handed orbit is denoted as c l. Given any start and end configurations (position and orientation), construct the right and left turning orbits. As shown in Figure 13.14, there are four different possible orbit-straight-orbit connections between the configurations. The possible configurations are 1. c rs c re : Follow the right turning orbit until meeting the line that is tangent to both c rs and c re, then follow the tangent line until meeting c re, then follow the right turning orbit c re until arriving at the end configuration. 2. c rs c le : Follow the right turning orbit until meeting the line that is tangent to both c rs and c le, then follow the tangent line until meeting c le, then follow the left turning orbit c le until arriving at the end configuration. 3. c ls c re : Follow the left turning orbit until meeting the line that is tangent to both c ls and c re, then follow the tangent line until meeting c re, then follow the right turning orbit c re until gr

194 Dubin s Paths Figure 13.13: A constant roll angle of φ = tan 1 V 2 produces an orbit of radius R. gr arriving at the end configuration. 4. c ls c le : Follow the left turning orbit until meeting the line that is tangent to both c ls and c le, then follow the tangent line until meeting c le, then follow the left turning orbit c le until arriving at the end configuration. The path with the shortest length is defined as the Dubin s path Computing the Dubin s Path The Dubin s path is determined by computing all four possible trajectories and selecting the one with the shortest path length. Given the position p, the orientation χ, and the radius R, the centers of the right and left turning radii are given by. ) T c r = p + R (cos(χ + π2 ) sin(χ + π2 ) ) T c l = p + R (cos(χ π2 ) sin(χ π2 )

195 Path Manager 185 Figure 13.14: There are four possible turn-straight-turn transitions between any two configurations. The Dubin s path is defined as the shortest of these transitions. To compute the path length of the different trajectories we need a general equation for angular distances on a circle. Figure shows the geometry for both clockwise (CW ) and counter clockwise (CCW ) circles. We will assume that both θ 1 and θ 2 are between 0 and 2π. For clockwise circles, the angular distance between θ 1 and θ 2 is given by θ 2 θ 1 CW = 2π + θ2 θ 1, (13.19) where ϕ = ϕ mod 2π. Similarly, for counter clockwise circles we get θ 2 θ 1 CCW = 2π θ2 + θ 1. (13.20) Case 1: c rs c re The geometry for case 1 is shown in Figure 13.16, where θ is the angle formed by the line between c rs and c re. Using Eq. (13.19), the angular distance traveled along c rs is given by R 2π + θ π 2 χ s π 2. Similarly, using Eq. (13.19), the angular distance traveled along c re is given by R 2π + χ e π 2 θ π 2.

196 Dubin s Paths Figure 13.15: The angular distance between angles θ 1 and θ 2 for clockwise (CW ) and counter clockwise (CCW ) circles. The total path length for case 1 is given by L 1 = c rs c re + R 2π + θ π 2 χ s π 2 + R 2π + χ e π 2 θ π 2. Case 2: c rs c le The geometry for case 2 is shown in Figure 13.17, where θ is the angle formed by the line between c rs and c le, l = c le c rs, and θ 2 = cos 1 2R l. Using Eq. (13.19), the angular distance traveled along c rs is given by R 2π + θ θ 2 χ s π 2, Similarly, using Eq. (13.20), the angular distance traveled along c le is given by R 2π + θ θ 2 + π χ e + π 2. The total path length for case 2 is given by l 2 L 2 = 2 4 R2 + R 2π + θ θ 2 χ s π 2, +R 2π + θ θ 2 + π χ e + π 2. Case 3: c ls c re The geometry for case 3 is shown in Figure 13.18, where θ is the angle formed by the line between c ls and c re, l = c re c ls, and θ 2 = cos 1 2R l.

197 Path Manager 187 Figure 13.16: Dubins path, case 1. Using Eq. (13.20), the angular distance traveled along c ls is given by R 2π + χ s + π 2 θ + θ 2. Similarly, using Eq. (13.19), the angular distance traveled along c re is given by R 2π + χ e π 2 θ + θ 2 π. The total path length for case 3 is given by l 2 L 3 = 2 4 R2 + R 2π + χ s + π 2 θ + θ 2 + R 2π + χ e π 2 θ + θ 2 π. Case 4: c ls c le The geometry for case 4 is shown in Figure 13.19, where θ is the angle formed by the line between c ls and c le. Using Eq. (13.20), the angular distance traveled along c ls is given by R 2π + χ s + π 2 θ + π 2. Similarly, using Eq. (13.20), the angular distance traveled along c le is given by R 2π + θ + π 2 χ e + π 2. The total path length for case 4 is given by L 4 = c ls c le + R 2π + χ s + π 2 θ + π 2 + R 2π + θ + π 2 χ e + π 2.

198 Dubin s Paths Figure 13.17: Dubins path, case 2. Data Structure Representation After L 1, L 2, L 3 and L 4 have been computed, the case with the minimum path length is selected as the Dubin s path. A data structure is created to represent a Dubin s path. The data structure has the following fields. DubinsPath.c s DubinsPath.R s The 2D center of the start orbit. The radius of the start orbit. DubinsPath.dir s The direction of the start orbit, where dir = 1 for a clockwise orbit and dir = 1 for a counter clockwise orbit. DubinsPath.s s The 2D unit vector that defines switching plane for start orbit. This vector will be discussed in the next section. DubinsPath.w s DubinsPath.c e DubinsPath.R e The 2D vector that defines the start of the straight-line segment. The 2D center of the end orbit. The radius of the end orbit. DubinsPath.dir e The direction of the end orbit, where dir = 1 for a clockwise orbit and dir = 1 for a counter clockwise orbit.

199 Path Manager 189 Figure 13.18: Dubins path, case 3. DubinsPath.s e The 2D unit vector that defines switching plane for end orbit. This vector will be discussed in the next section. DubinsPath.w e The 2D vector that defines the end of the straight-line segment. DubinsPath.L The length of the Dubin s path Control law for tracking the Dubin s path The basic idea is to implement a feedback control law that tracks the start orbit until the MAV reaches the straight-line segment. The control strategy is then switched to a waypoint tracking control law until the end orbit is reached. An orbit control strategy is then used complete the Dubin s path. It is critical that a robust switching strategy be used when switching between orbit and waypoint controllers. If the switching strategy is not implemented correctly, the controller may become deadlocked in one of the modes. In [55, 29] it was demonstrated that an effective and robust switching strategy is to switch when the MAV transitions from one half-plane to another. Switching plane A switching plane in R 2 is defined by a position vector q R 2 and a unit vector s R 2 as shown in Figure The position of the MAV is denoted by p R 2. The position p is defined to be on the right hand side of the switching plane if it would be on the right hand side of an observer standing at q and facing in the direction of s. Otherwise it is on the left hand side of the switching plane.

200 Dubin s Paths Figure 13.19: Dubins path, case 4. To determine which side of the switching plane p is on, first append a zero onto p, s, and q to form p R 3, s R 3, and q R 3. The cross product between p q and s is given by p n q n s n 0 (p q) s = p e q e s e = (p n q n )s e (p e q e )s n The position p is on the right hand side of the switching plane if (p q) s 3 = (pn q n )s e (p e q e )s n > 0. Alternatively, p is on the left hand side of the switching plane if (p q) s 3 < 0. Also note from the property of cross products: a b = a b sin θ, that the shortest distance from p to the switching plane is given by (p q) s 3. Figure shows the switching planes used to navigate along a Dubin s path. The switching plane associated with the start orbit is defined by the position c s and the unit vector s s. The switching plane associated with the end orbit is defined by the position c e and the unit vector s e. The straight-line edge is defined by the waypoints w s and w e. The specific values are determined for each of the four cases defined in Section as

201 Path Manager 191 Figure 13.20: A switching plane is defined by a position vector q and a unit vector s. The position of the MAV is denoted by p. Case 1 ( s s = cos(θ π/2) ( s e = cos(θ π/2) ) T sin(θ π/2) sin(θ π/2) ) T w s = c rs + Rs s w e = c re + Rs e Case 2 ( ) T s s = cos(θ θ 2 ) sin(θ θ 2 ) ( s e = cos(θ θ 2 + π) sin(θ θ 2 + π) w s = c rs + Rs s w e = c le + Rs e ) T

202 Dubin s Paths Figure 13.21: This graphic shows the switching planes used to navigate along a Dubin s path. Case 3 ( ) T s s = cos(θ + θ 2 ) sin(θ + θ 2 ) ( s e = cos(θ + θ 2 π) sin(θ + θ 2 π) ) T w s = c ls + Rs s w e = c re + Rs e Case 4 ( s s = cos(θ + π/2) ( s e = cos(θ + π/2) w s = c ls + Rs s w e = c le + Rs e ) T sin(θ + π/2) sin(θ + π/2) ) T Roll rate command given a desired course angle The orbit and waypoint tracking strategies discussed in the next two subsections will produce a desired course angle χ d. As shown in Eq. (14.60) we control the roll rate of the MAV. The

203 Path Manager 193 objective of this section is to describe the feedback command given a desired course angle. Let χ = χ χ d. Then χ = χ χ d = g V tan φ χ d, where we have used Eq. (14.59). Therefore we select φ d = tan 1 ( V g χd k χv g ) χ. (13.21) Therefore, if φ = φ d we have χ = k χ χ. The control signal u 1 in Eq. (14.60) is selected as u 1 = sat [ k φ (φ d φ) ]. (13.22) Waypoint tracking The waypoint tracker is constructed using the vector field method described in [23, 24]. In particular, where and Differentiating χ d gives Orbit tracking χ d = χ path + χ 2 π tan 1 (k wp d), χ path = atan2(w ee w se, w en w sn ), d = q n (p e w se ) q e (p n w sn ), q = χ d = χ 2 π w e w s w e w s. k wp 1 + (k wp d) 2. If c is the center of the orbit and R is its radius, then the orbit tracker is also constructed using the vector field method described in [23, 24]. In particular, ( π ) χ d = χ orbit + dir 2 + tan 1 (k orbit (d R)), where and d = p c. Differentiating χ d we get χ orbit = atan2(p e c e, p n c n ), χ d = V d sin(χ χorbit ) k orbitv cos(χ χ orbit ) 1 + k 2 orbit (d R)2.

204 Dubin s Paths State Machine for Dubin s Path The state machine for tracking a Dubin s path is shown in Figure The algorithm begins by planning a Dubin s path from the current configuration to a goal configuration. If the current position is already to the right of s s (as shown in Figure 13.21), then the orbit is tracked until it is to the left of s s. The MAV continues to track the orbit until crossing the half plane defined by s s, at which point, the waypoint path from w s to w e is tracked. This is continued until the MAV crosses the half plane defined by s e when the MAV tracks the orbit defined by c e, R e, and dir e. Figure 13.22: This graphic shows the state machine used for tracking Dubin s paths.

205 Path Manager Design Project 13.1 Homework problem 1.

206 Design Project

207 Chapter 14 Cameras on Micro UAVs One of the primary reasons for the interest in micro air vehicles is that they offer an inexpensive platform to carry electro-optical (EO) and infrared (IR) cameras. Almost all MAV systems that are currently deployed carry either an EO or IR camera. While the camera s primary use is to relay information to a user, it makes sense to attempt to use the camera for the purpose of navigation, guidance, and control. Further motivation comes from the fact that birds and flying insects use vision as their primary guidance sensor [56]. Vision based guidance and control of MAVs is currently an active research topic (see for example [57, 58, 59, 60, 61, 62, 63, 14, 64, 65, 66, 67]). The objective of this chapter is to briefly introduce some of the issues that arise in vision based guidance and control of micro air vehicles. In Section 14.1 we revisit coordinate frame geometry and expand upon the discussion in Chapter 2 by introducing the gimbal and camera frames. We also discuss the image plane and the projective geometry that relates the position of 3D objects to their 2D projection on the image plane. In Section 14.2 we give a simple algorithm for pointing a pantilt gimbal at a known world coordinate. In Section 14.3 we describe a geo-location algorithm that estimates the position of a ground based target based on the location and motion of the target in the image sequence. In this chapter we will assume that an algorithm exists for tracking the features of a target in the video sequence. The motion of the target on the image plane is influenced by both target motion and by translational and rotational motion of the air platform. In Section 14.4 we describe a method that compensates for the apparent target motion that is induced by gimbal movement and angular rates of the air platform. As a final application of vision based guidance, in Section 14.5 we describe an algorithm that uses vision to precisely land on a ground based target. 197

208 Gimbal and Camera Frames and Projective Geometry 14.1 Gimbal and Camera Frames and Projective Geometry In this section we will assume that the origins of the camera and gimbal frames are located at the center of mass of the vehicle. For more general geometry see [14]. Figure 14.1 show the relationship between the vehicle and body frames of the MAV and the gimbal and camera frames. There are three frames of interest: the gimbal-1 frame denoted by F g1 = (î g1, ĵ g1, ˆk g1 ), the gimbal (a) Top view. (b) Side view. Figure 14.1: A graphic showing the relationship between the gimbal and camera frames and the vehicle and body frames. frame denoted by F g = (î g, ĵ g, ˆk g ), and the camera frame denoted by F c = (î c, ĵ c, ˆk c ). The gimbal-1 frame is obtained by rotating the body frame about the ˆk b -axis by an angle of α az, which is called the gimbal azimuth angle. The rotation from the body to the gimbal-1 frame is given by cos α az sin α az 0 R g1 b (α az) = sin α az cos α az 0. (14.1) The gimbal frame is obtained by rotating the gimbal-1 frame about the ĵ g1 axis by an angle of α el, which is called the gimbal elevation angle. The rotation from the gimbal-1 frame to the gimbal frame is given by cos α el 0 sin α el Rg1(α g el ) = (14.2) sin α el 0 cos α el Therefore, the rotation from the body to the gimbal frame is given by cos α el cos α az cos α el sin α az sin α el R g b = Rg g1r g1 b = sin α az cos α az 0. (14.3) sin α el cos α az sin α el sin α az cos α el

209 Cameras on Micro UAVs 199 The literature in computer vision and image processing traditionally aligns the coordinate axis of the camera such that î c points to the right in the image, ĵ c points down in the image, and ˆk c points along the optical axis. Therefore, the transformation from the gimbal frame to the camera frame is given by Camera Model Rg c = (14.4) The geometry in the camera frame is shown in Figure 14.2, where f is the focal length in units of pixels, and P is the size of the side of a pixel in meters. We will assume that the pixels and the pixel array are square. If the width of the square pixel array is M and the field-of-view of the camera, η, is known, then the focal length f can be expressed as f = M 2 tan ( η). (14.5) 2 The location of the object in the camera frame is given by (P ɛ x, P ɛ y, P f), where ɛ x and ɛ y are the pixel location (in units of pixels) of the object. The distance from the origin of the camera frame to the pixel location (ɛ x, ɛ y ), as shown in Figure 14.2 is P F where F = f 2 + ɛ 2 x + ɛ 2 y. (14.6) Using similar triangles in Figure 14.2 we get Similarly we get that l c y/l = ɛ y /F and l c z/l = f/f. Therefore l c x L = P ɛ x P F = ɛ x F. (14.7) l c = L F ɛ x ɛ y. (14.8) f Note that l c cannot be determined strictly from camera data since L = l c is unknown. However, we can determine the unit direction vector to the target as ˆl c = ˆl c x ˆl c y ˆl c z ɛ x = lc L = 1 ɛ F y. (14.9) f

210 Gimbal Pointing Image plane Figure 14.2: The camera frame. The target in the camera frame is represented by l c. The projection of the target onto the image plane is represented by ɛ. The zero pixel location corresponds to the center of the image which is assumed to be aligned with the optical axis. The distance to the target is given by L. ɛ and f are in units of pixels. l is in units of meters Gimbal Pointing Small and micro air vehicles are used primarily for intelligence, surveillance, and reconnaissance (ISR) tasks. If the MAV is equipped with a gimbal, this involves maneuvering the gimbal so that the camera points at certain objects. The objective of this section is to describe a simple gimbal pointing algorithm. We assume a pan-tilt gimbal and that the equations of motion are given by α az = u az α el = u el, where u az and u el are control variables for the gimbal s azimuth and elevation angles respectively. We will consider two pointing scenarios. In the first scenario, the objective is to point the gimbal at a given world coordinate. In the second scenario, the objective is to point the gimbal so that the optical axis aligns with a certain point in the image plane. For the second scenario, we envision a user watching a video stream from the MAV and using a mouse to click on a location in the image plane. The gimbal is then maneuvered to push that location to the center of the image plane.

211 Cameras on Micro UAVs 201 For the first scenario, let p i target be the known location of the target in the inertial frame. The objective is to align the optical axis of the camera with the relative position vector d l i = p i target p i MAV, where p i MAV = (p n, p e, h) T is the inertial position of the MAV, and where the prescript d indicates a desired quantity. The body frame unit vector that points in the desired direction of the target is given by dˆlb = 1 d l i Rb i d l i. For the second scenario, suppose that we desire to maneuver the gimbal so that the pixel location ɛ is pushed to the center of the image. Using (14.9), the desired direction of the optical axis in the camera frame, is given by dˆlc = 1 f 2 + ɛ 2 x + ɛ 2 y ɛ x ɛ y. f In the body frame, the desired direction of the optical axis is dˆlb = R b gr g c dˆlc. The next step is to determine the desired azimuth and elevation angles that will align the optical axis with dˆlb. In the camera frame, the optical axis is given by (0, 0, 1) c. Therefore, the objective is to select the desired gimbal angles d α az and d α el so that dˆlb x 0 dˆlb = dˆlb y = Rg( b d α az, d α el )Rc g 0 (14.10) dˆlb z 1 cos d α el cos d α az sin d α el sin d α el cos d α az = cos d α el sin d α az cos d α az sin d α el sin d α az (14.11) sin d α el 0 cos d α el cos d α el cos d α az = cos d α el sin d α az. (14.12) sin d α el Therefore the desired azimuth and elevation angles are given by ) ( dˆlb d α az = tan 1 y (14.13) dˆlb x ( ) d α el = sin 1 dˆlb z. (14.14)

212 Geolocation The gimbal servo commands can be selected as u az = k az ( d α az α az ) (14.15) u el = k el ( d α el α el ), where k az and k el are positive control gains Geolocation This section presents a method for determining the location of objects in world/inertial coordinates using a gimballed EO/IR camera on-board a fixed-wing MAV. We assume that the MAV can measure its own world coordinates using, for example, a GPS receiver, and that other MAV state variables are also available. Following Section 14.1, let l = p obj p MAV be the relative position vector between the target of interest and the MAV, and define L = l and ˆl = l/l. Then from geometry we have the relationship p i obj = p i MAV + RbR i gr b cl g c = p i MAV + L (R ibr bgr gc ˆl ) c, (14.16) where p i MAV = (p n, p e, h) T, Rb i = Ri b (φ, θ, ψ) and Rb g = Rg(α b az, α el ). The only element on the right hand side of Eq. (14.16) that is unknown is L. Therefore, solving the geolocation problem reduces to the problem of estimating the range to the target L Range to Target Using the Flat Earth Model If the MAV is able to measure height-above-ground, then a simple strategy for estimating L is to assume a flat earth model [14]. Figure 14.3 shows the geometry of the situation, where h is the height-above-ground, and ϕ is the angle between l and the ˆk i axis. It is clear from Figure 14.3 that where L = cos ϕ = ˆk i ˆl v h cos ϕ, = ˆk i R i br b gr g c ˆl c.

213 Cameras on Micro UAVs 203 Figure 14.3: Range estimation using the flat earth assumption. Therefore, the range estimate using the flat earth model is given by L = h. (14.17) ˆk i Rb irb grc g ˆl c The geolocation estimate is given by combining Eqs. (14.16) and (14.17) to produce p n p i obj = p e + h Ri b Rb gr g ˆl c c. (14.18) ˆk h i Rb irb grc g ˆl c Geo-location using Recursive Least Squares The geo-location estimate in Eq. (14.18) provides a one-shot estimate of the target location. Unfortunately, this equation is highly sensitive to measurement errors, especially attitude errors. Reference [62] suggests a recursive least squares technique for simultaneously estimating p i obj and L which will be less sensitive to Gaussian measurement noise. Toward that end, rewrite Eq. (14.16) as ( ) ( ) I 3. Rb irb grc g ˆl p i c obj L = p i MAV. (14.19) ) Define A[k] as the k th sample of (I 3. R ib RbgR gc ˆl c, and define b[k] as the k th sample of ( ) p i MAV = (p n, p e, h) T p i obj, and define x as the estimate of, then at the k th sample time L Eq. (14.19) can be written as A[k]x = b[k], (14.20)

214 Geolocation where A[k] is a 3 4 matrix, x is a 4 1 vector and b[k] is a 3 1 vector. Equation (14.20) is an underdetermined set of equations that may contain significant noise in A[k] and b[k]. In the following, we will derive a recursive least squares algorithms [13] for estimating x. Using the first k information samples, define the matrices A[k] = B[k] = A[1]. A[k] b[1].. Equation (14.20) can now be written as the overdetermined set of equations The least squares solution to (14.21) is given by b[k] A[k]x = B[k]. (14.21) x[k] = ( A T [k]a[k] ) 1 A T [k]b[k], (14.22) where x[k] is the least squares estimate after the k th sample. Even though the size of the matrix being inverted in (14.22) is always 4 4, the least squares solution is difficult to implement efficiently since at each sample, the sizes of A[k] and B[k] increase requiring an increasing amount of memory and a growning number of additions and multiplications at each step. The recursive least squares (RLS) algorithm is an efficient method for implementing Eq. (14.22) using a constant number of multiplies and divides at each step, and a constant amount of memory. To derive the RLS algorithm define and note that P [k] = ( A T [k]a[k] ) 1, (14.23) P [k + 1] = ( A T [k + 1]A[k + 1] ) 1 = ( A T [k]a[k] + A T [k + 1]A[k + 1] ) 1. (14.24) To reduce this equation we use the Matrix Inversion Lemma [68] which can be stated as ( B 1 + CD 1 C T ) 1 = B BC(D C T BC) 1 C T B.

215 Cameras on Micro UAVs 205 Application of the Matrix Inversion Lemma to (14.24) and using (14.23) gives where P [k + 1] = P [k] + P [k]a T [k + 1] ( I A[k + 1]P [k]a T [k + 1] ) 1 A[k + 1]P [k] = [I 4 + P [k]a T [k + 1] ( I 3 A[k + 1]P [k]a T [k + 1] ) ] 1 A[k + 1] P [k] = K[k]P [k], K[k] = I 4 + P [k]a T [k + 1] ( I 3 A[k + 1]P [k]a T [k + 1] ) 1 A[k + 1]. (14.25) Now note from (14.22) that we can write x[k + 1] = P [k + 1]A T [k + 1]B[k + 1] = K[k]P [k] ( A T [k]b[k] + A T [k + 1]b[k + 1] ) = K[k] ( P [k]a T [k]b[k] + P [k]a T [k + 1]b[k + 1] ) = K[k] ( x[k] + P [k]a T [k + 1]b[k + 1] ). (14.26) Therefore, the RLS algorithms is given by the iterated equations K[k] = I 4 + P [k]a T [k + 1] ( I 3 A[k + 1]P [k]a T [k + 1] ) 1 A[k + 1] (14.27) P [k + 1] = K[k]P [k] (14.28) x[k + 1] = K[k] ( x[k] + P [k]a T [k + 1]b[k + 1] ). (14.29) Note that Eq. (14.27) requires a 3 3 inverse as opposed to the 4 4 inverse of Eq. (14.22). Also note that the number of additions and multiplies is constant at each step. The downside to the RLS algorithms is that the recursion requires initial values for P [0] and x[0]. A simple method for initializing the recursion is to set P [0] = I and to compute x[0] using the flat earth model discussed in Section Even with the RLS algorithm, a residual geo-location error will remain due to sensing errors and camera and gimbal bias. Several techniques for removing these errors and enhancing the geo-location estimate are described in [14] Estimating Target Motion in the Image Plane We assume in this chapter that a computer vision algorithm is used to track the pixel location of the target. Since the video stream often contains noise, and the tracking algorithms are imperfect,

216 Estimating Target Motion in the Image Plane pixel location returned by these algorithms is often noisy. The guidance algorithm described in the next section needs both the pixel location of the target, as well as the pixel velocity of the target. In Section we show how to construct a simple low pass filter that returns a filtered version of both the pixel location and the pixel velocity. The pixel velocity is influenced by both the relative (translational) motion between the target and the MAV, and the rotational motion of the MAV-gimbal combination. In Section we derive an explicit expression for pixel velocity and show how to compensate for the apparent motion induced by the rotational rates of the MAV and gimbal. In the remainder of this chapter, we will need to be careful about which frame we are taking derivatives. We will use an overhead dot to denote differentiation with respect to the inertial frame. We will use an overhead prime to denote differentiation with respect to the camera frame Digital low pass filter and differentiation Let ˆɛ = (ˆɛ x, ˆɛ y ) T denote the raw pixel measurements, and ɛ = (ɛ x, ɛ y ) T denote the filtered pixel location, and ɛ = ( ɛ x, ɛ y ) T denote the filtered pixel velocity as observed in the camera frame. The basic idea is to low pass filter the raw pixel measurements as The pixel rates are obtained via a bandwidth limited differentiator as ɛ(s) = 1 (14.30) τs + 1ˆɛ. ɛ(s) = In the time domain, Equation (14.30) can be written as Solving this differential equation gives s (14.31) τs + 1ˆɛ. ɛ = 1 τ ɛ + 1 ˆɛ. (14.32) τ ɛ(t) = e (t t 0)/τ ɛ(t 0 ) + 1 τ t t 0 e (t σ)/τˆɛ(σ) dσ. Therefore, if ɛ[k] denotes the filtered pixel location at the k th sample, and T is the sample period, then In the time domain, Equation (14.31) can be written as ɛ[k] = e T/τ ɛ[k 1] + (1 e T/τ )ˆɛ[k]. (14.33) d ɛ dt c = 1 τ (dˆɛ dt ɛ).

217 Cameras on Micro UAVs 207 Integrating once gives ɛ = 1 τ (ˆɛ ɛ). (14.34) Defining x = ɛ and differentiating with respect to time gives x = ɛ = 1 (ˆɛ x) τ = 1 τ x + 1 τ ˆɛ, which is identical to Equation (14.32). Therefore, the solution x[k] is given by Equation (14.33). Combining Equation (14.33) and (14.34) we get ɛ[k] = e T/τ ɛ[k 1] + (1 e T/τ )ˆɛ[k] (14.35) ɛ[k] = 1 (ˆɛ ɛ[k]), (14.36) τ where ɛ[0] = ˆɛ[0] and ɛ[0] = Apparent Motion due to Rotation Motion of the target on the image plane is induced by both relative translational motion of the target with respect to the MAV, as well as rotational motion of the MAV and gimbal platform. For most guidance tasks, we are primarily interested in the relative translational motion and desire to remove the apparent motion due to the rotation of the MAV and gimbal platforms. Following the notation introduced in Section 14.1, let ˆl = l/l = (p obj p MAV )/ p obj p MAV be the normalized relative position vector between the target and the MAV. Using the Coriolis formula (2.8) we get dˆl = dˆl + ω c/i dt i dt ˆl. (14.37) c The expression on the left-hand-side of (14.37) is the true relative translation motion between the target and the MAV. The first expression on the right-hand-side of (14.37) is the motion of the target on the image plane that can be computed from camera information. The second expression on the right-hand-side of (14.37) is the apparent motion due to the rotation of the MAV and gimbal platform. Equation (14.37) can be expressed in the camera frame as dˆl c dt i = dˆl c dt c + ω c c/i ˆl c. (14.38)

218 Estimating Target Motion in the Image Plane The first expression on the right-hand-side of (14.38) can be computed as ɛ x ɛ x ɛ x ɛ x ɛ x ɛ y F ɛ y F ɛ y F ɛ y ɛ x ɛ x +ɛ y ɛ y F ɛ y dˆl c = d f 0 f 0 f = = dt c dt c F F 2 F 2 = 1 F 2 ɛ 2 x ɛ x ɛ y ( ) F 3 ɛ x ɛ y F 2 ɛ 2 ɛ x y = 1 ɛ 2 y + f 2 ɛ x ɛ y ɛ y F 3 ɛ x ɛ y ɛ 2 x + f 2 ɛ ɛ x f ɛ y f ɛ x f ɛ y f = Z(ɛ) ɛ. (14.39) To compute the second term on the right-hand-side of (14.38) we need an expression for ω c c/i which can be decomposed as ω c c/i = ω c c/g + ω c g/b + ω c b/i. (14.40) Since the camera is fixed in the gimbal frame, we have that ωc/g c = 0. Letting p, q, and r denote the angular body rates of the platform as measured by on-board rate gyros that are aligned with the body frame axes, gives ω b b/i = (p, q, r)t. Therefore p ωb/i c = RgR c g b ωb b/i = RgR c g b q. (14.41) r To derive an expression for ω g/b in terms of the measured gimbal angle rates α el and α az, recall that the azimuth angle α az is defined with respect to the body frame, and that the elevation angle α el is defined with respect to the gimbal-1 frame. The gimbal frame is obtained by rotating the gimbal-1 frame about its y-axis by α el. Therefore α az is defined with respect to the gimbal-1 frame, and α el is defined with respect to the gimbal frame. This implies that 0 ωg/b b = Rg1(α b az )Rg g1 (α el ) α el 0 α az 0 + Rg1(α b az ) 0 Noting that Rg g1 is a y-axis rotation we get 0 cos α az sin α az 0 0 sin(α az ) α el ωg/b b = Rg1(α b az ) α el = sin α az cos α az 0 α el = cos(α az ) α el α az g α az α az g1.

219 Cameras on Micro UAVs 209 Therefore, ω c g/b = R c gr g b ωb g/b = R c gr g b sin(α az ) α el cos(α az ) α el α az. (14.42) Equation (14.38) can therefore be expressed as dˆl c dt i = Z(ɛ) ɛ + 1 F R c gr g b p sin(α az ) α el q + cos(α az ) α el r + α az ɛ x ɛ y f. (14.43) In the following section, we will need to compute l c /L where the dot represents differentiation in the inertial frame. Differentiating ˆl c gives which results in the expression l c L = L L ˆl c + Z(ɛ) ɛ + 1 F dˆl c = d l c dt i dt i L = L l c Ll c L 2 R c gr g b = l c L L L ˆl c, (14.44) p sin(α az ) α el ɛ x q + cos(α az ) α el ɛ y, (14.45) r + α az f where L/L is the range rate divided by the range, or in other words, the inverse of time-to-impact. Computing the expression L/L is challenging using camera information. In the next two sections we will derive two simple techniques for estimating L/L Computing time-to-impact from target size If the computer vision algorithm can estimate the size of the target in the image frame, then that information can be used to estimate L/L. Consider the geometry shown in Figure Using similar triangles we obtain the relationship S L = P ɛ s P F = ɛ s F, (14.46) where the size of the target in meters is S, and the size of the target in pixels is given by ɛ s. We assume that the size of the target S is not changing in time. Differentiating Eq. (14.46) and solving

220 Estimating Target Motion in the Image Plane Figure 14.4: The size and growth of the target in the image frame can be used to estimate the time-to-impact. for L/L we obtain [ ] L L = L ɛ s F S F F ɛ s F [ ] = F ɛ s F ɛ s F F ɛ s F F = F ɛ s. (14.47) ɛ s Since ɛ s and F are scalars, differentiation in the inertial frame is equal to differentiating in the camera frame. Therefore, L L = F F ɛ s ɛ s = ɛ x ɛ x + ɛ y ɛ y F ɛ s ɛ s. (14.48) Computing time-to-impact from flat earth model A popular computer vision algorithm for target tracking is to track features on the target [69]. If a feature tracking algorithm is used, then target size information will not be available and the method described in the previous section cannot be applied. In this section we describe an alternative method for computing L/L where we assume a flat earth model. Referring to Figure 14.3 we have L = h cos ϕ.

221 Cameras on Micro UAVs 211 Differentiating in the inertial frame gives In the inertial frame we have that ( ) L L = 1 cos ϕḣ + h ϕ sin ϕ L cos 2 ϕ ( ) = cos ϕ cos ϕḣ + h ϕ sin ϕ h cos 2 ϕ = ḣ + ϕ tan ϕ. h cos ϕ = ˆl i ˆk, (14.49) where ˆk = (0, 0, 1) T. Therefore Since ˆl i z ˆl i 1 we have that cos ϕ = ˆl i z. (14.50) sin ϕ = 1 (ˆl i z) 2 (14.51) 1 (ˆl i z) 2 tan ϕ = ˆl i. (14.52) z (14.53) Differentiating Equation (14.50) and solving for ϕ gives ϕ = 1 d ˆli sin ϕ dt z. (14.54) i Therefore ϕ tan ϕ = 1 d ˆli ˆl i z dt z, (14.55) i where dˆl i z/dt i can be determined by rotating Eq. (14.43) into the inertial frame Precision Landing Our objective in this section is to use the camera to guide the MAV to precisely land on a visually distinct target. This section is taken primarily from [70].

222 Precision Landing The problem of guiding an aerial vehicle to intercept a moving target has been well studied. Proportional navigation (PN), in particular, has been an effective guidance strategy against maneuvering targets since its discovery [71]. In this section, we present a method for implementing a 3D pure PN guidance law using only vision information provided by a two-dimensional array of camera pixels. PN generates acceleration commands that are proportional to the (pursuer-evader) line-of-site (LOS) rates multiplied by the closing velocity. PN is often implemented as two 2D algorithms implemented in the horizontal and vertical planes. The LOS rate is computed in the plane of interest and PN produces a commanded acceleration in that plane. While this approach works well for roll-stabilized skid-to-turn missiles, it is not appropriate for MAV dynamics. In this section we develop 3D algorithms and we show how to map the commanded body-frame accelerations to roll angle and pitch rate commands. PN has been extensively analyzed in the literature. It has been shown to be optimal under certain conditions [72] and to produce zero miss distances for a constant target acceleration[73]. If rich information regarding the time-to-go is available, augmented proportional navigation [74] improves the performance by adding terms that account for target and pursuer accelerations. A three-dimensional expression of PN can be found in [75], where the authors compare PN to a minimum-energy optimal guidance law Navigation and Camera Models The primary focus of this paper is on micro air vehicles (MAVs). Since MAVs have limited payload capacity, most of the platforms are not fully actuated. For example, the Air Force BATCAM MAV [76] is a V-tail airframe that is actuated by ruddervators, which can be mixed to effect rudder and elevator commands. Other MAV airframes use rudder and elevator [77] as well as ailevons, which mix aileron and elevator commands. It is rare for a MAV platform to include rudder, aileron, and elevators. However, for completeness, we will consider both fully actuated platforms, as well as under-actuated platforms. For under-actuated platforms, we will assume that the actuators are ailerons and elevators, and we will assume the presence of a tail, or winglets that essentially maintain the vehicle in a coordinated turn. Therefore, in the absence of wind, the kinematics of the MAV can be modeled

223 Cameras on Micro UAVs 213 using the bank-to-turn equations of motion ṗ n = V cos χ cos γ (14.56) ṗ e = V sin χ cos γ (14.57) ḣ = V sin γ (14.58) χ = g tan φ (14.59) V φ = u 1 (14.60) γ = u 2, (14.61) where p n and p e are the north and east position of the MAV, h is the altitude, V is the groundspeed (assumed constant), χ is the course angle, γ is the flight path angle, φ is the roll angle, g is the gravitational constant, and u 1, and u 2 are control variables Vision based 3D Proportional Navigation The objective of this section is to design a vision based guidance law that causes a MAV to intercept a moving target. In this paper, we will assume that the target can move both vertically and horizontally. The position and velocity vector of the target are given by r T and v T respectively. Similarly, let r I and v I denote the position and velocity of the MAV interceptor. In the inertial frame, the position and velocity of the MAV can be expressed as ( T ri i = p n, p e, h), ( T vi i = V cos χ cos γ, V sin χ cos γ, V sin γ). However, in the vehicle-2 frame, the velocity vector of the MAV is given by ( ) v v 2 I = V, 0, 0. Define l = r T r I and l = ṙ T ṙ I, and let L = l, then the 3D equations for pure proportional navigation (PPNG) are given in [75] as a I = NΩ µv I, (14.62) where Ω = l L l L, l µ = v I,

224 Precision Landing and where a I is the commanded acceleration vector for the MAV. Furthermore, if we define ˆl = l L (14.63) then Ω can be expressed as ˆl = l L, (14.64) Ω = ˆl ˆl. (14.65) The term µ = l / v I is the normalized closing velocity and, unfortunately, cannot be determined directly by a passive sensor. Throughout this paper, we assume that the velocity of the target is slow compared to the velocity of the interceptor. Therefore we assume that µ 1. The acceleration commands must be converted to control inputs u 1 and u 2 for the bank-to-turn model and u 3 and u 4 for the skid-to-turn model. In both cases, the commanded acceleration is produced in the unrolled body frame, or the vehicle-2 frame. Therefore, the commanded acceleration a I must be resolved in the vehicle-2 frame as a v2 I = µnω v2 VI v2 Ω v2,x V = µn Ω v2,y 0 Ω v2,z 0 0 = µnv Ω v2,z µnv Ω v2,y. (14.66) It is important to note that the commanded acceleration is perpendicular (by design) to the direction of motion, which is consistent with a constant groundspeed model. We are interested in the case where Ω must be estimated from video camera data. Our basic approach will be to estimate Ω in the camera frame, and then to transform to the vehicle-2 frame using the expression Ω v2 = R v2 b R b gr g cω c. (14.67) Polar Control Logic The primary challenge with bank-to-turn aircraft is the conversion of the acceleration command produced by PN to a roll command and a normal acceleration command. The standard approach is to use polar control logic [78], which is shown in Figure From Figure 14.5 it is clear that for

225 Cameras on Micro UAVs 215 Figure 14.5: Polar converting logic that transforms an acceleration command a v2 to a desired roll command φ d and a desired normal acceleration V α el. a v2 z < 0, we have Similarly, when a v2 z > 0 we have ( a v2 c φ = tan 1 y V c γ = a v2 z ) (a v2 y ) 2 + (a v2 z ) 2. ( a v2 c φ = tan 1 y a v2 z ) V c γ = (a v2 y ) 2 + (a v2 z ) 2. Therefore, the general rule is ( a v2 ) c φ = tan 1 y a v2 z c γ = sign(a v2 z ) 1 V (14.68) (a v2 y ) 2 + (a v2 z ) 2. (14.69) Unfortunately, Equation (14.68) has a discontinuity at (a v2 y, a v2 z ) = (0, 0). For example, when < 0. The a v2 z = 0 the commanded roll angle is c φ = π/2 when a v2 y > 0 and c φ = π/2 when a v2 y discontinuity can be removed by multiplying Equation (14.68) by the signed-sigmoidal function σ(a v2 y ) = sign(a v2 y ) 1 e kav2 y 1 + e kav2 y, (14.70) where k is a positive control gain. A plot of c φ(a v2 y, a v2 z = 0) with and without the sigmoidal function, is shown in Figure The gain k adjusts the rate of the transition.