Linear Flight Control Techniques for Unmanned Aerial Vehicles

Transcription

1 Chapter 1 Linear Flight Control Techniques for Unmanned Aerial Vehicles Jonathan P. How, Emilio Frazzoli, and Girish Chowdhary August 2,

2 Abstract This chapter presents an overview of linear flight control and guidance methods for Unmanned Aerial Vehicles (UAVs). The chapter begins with a discussion of rotation matrices and kinematic equations of a UAV. The six-degree of freedom UAV equations of motion are also derived using rigid body dynamics principles. The equations of motion are then linearized and several linear multi-loop closure techniques for UAV guidance and control are discussed.

3 Control UAV commands error control inputs states system dynamics G c (s) G(s) state estimates Sensors and navigation system disturbances sensor noise Figure 1.1: A typical UAV control system architecture. A control system attempts to minimize the tracking error between the desired and estimated states in presence of disturbances. A navigation system provides estimates of the states by fusing together measurements from several complementary sensors. 1.1 Introduction A flight control system is required to ensure that the UAV exhibits stable behavior and delivers desired performance such as following a desired trajectory with sufficient accuracy in the presence of external disturbances. Thus a UAV flight control system is typically safety/mission critical, as an incorrect design, lack of robustness to model variations, or a component failure could result in poor performance (impacting, for example, payload pointing capability) or even loss of the vehicle. Figure 1.1 depicts a typical UAV control system. The goal of the control system is to track the desired reference command in presence of external disturbances. The control system achieves this by attempting to eliminate the tracking error between the reference commands and the measured response of the UAV. The control is typically achieved by using estimates of the UAV states, which include position, velocity, angular rate, and attitude of the UAV. These estimates are obtained by the navigation system that fuses noisy measurements from several sensors. The purpose of this chapter, and its companion chapter on nonlinear control, is to outline the various methods for designing the control system G c (and some aspects of the navigation system) and in the process highlight the benefits and challenges of using each approach. This chapter presents a detailed overview of linear techniques for UAV control. Linear control techniques for manned and unmanned aircraft are well understood, and are widely utilized due to their simplicity, ease of implementation, and associated metrics of stability and performance. The chapter begins with a description of UAV dynamics in Section 1.2. Linear control techniques are then developed in Section 1.3. A brief discussion of path following guidance controllers is presented in Section

4 -"#$%&' ()*+' /'.' 0'!"#$%&' ()*+',"#$%&' ()*+' Figure 1.2: Aircraft body frame 1.2 Equations of Motion For most problems of interest to UAVs, the Earth can be taken to be an inertial reference frame, and the aircraft can be modeled as a rigid body. In order to write equations of motions for an aircraft, is is customary to define a reference frame (often called a body frame) that is rigidly attached to it (see Figure 1.2). A standard choice (often referred to as NED) for the inertial reference frame (X, Y, Z) is to choose the X axis pointing North, the Y axis pointing East, and the Z axis pointing down. Several choices of body frames are common, depending on the task at hand; examples include principal axes, and stability axes, and will be discussed in the following. There are advantages to each choice, but the stability axes are very commonly used. In general though, the appropriate body frame (x, y, z) is chosen in such a way that the y axis is orthogonal to the aircraft s plane of symmetry, pointing to the right. The x axis is chosen pointing forward (the exact direction depends on the definition of the body frame), and the z axis points down, to complete the right-handed triad (see Figure 1.3). Note that for this set, in different flight equilibrium conditions, the axes will be oriented differently with respect to the aircraft principal axes, so one must transform (rotate) the principal inertia components between the frames. However, when vehicle undergoes motion with respect to the equilibrium, stability axes remain fixed to airplane as if painted on (see Figure 1.3) Dynamics Newton s laws of mechanics imply that in a body frame whose origin coincides with the aircraft s center of mass, FB = mv B + ωb mvb, (1.1) TB = JB ω B + ωb JB ωb. (1.2) 2

5 1"#$%&' ()*+' 2' 3' 0"#$%&' ()*+'!"#$%&' ()*+'!"#$%&' &,#(%-%,+'!"#$%&'.%/*' Figure 1.3: Various body frames used for aircraft analysis In the above equations, the subscript ( )B indicates vectors or tensors expressed in the body frame, the dot indicates differentiation with respect to time, vb is the velocity of the aircraft s center of mass with respect to the inertial frame, ωb is the angular velocity of the body frame with respect to the inertial frame, m is the aircraft s mass, and JB is its inertia tensor, which is constant in the body frame. The operator denotes the cross product that arises due to the vector derivative transport theorem (see e.g. 18, 60, 45]). On the left-hand side, FB denotes the sum of the forces acting on the vehicle (including aerodynamic, gravity, thrust, and buoyancy), and TB denotes the sum of the moments of these forces about its center of mass. In coordinates, equations (1.1) and (1.2) are written as follows. Let FB = (X, Y, Z), TB = (L, M, N ), vb = (U, V, W ), ωb = (P, Q, R). The inertia tensor is defined as Z ρ(s) (s s)i s s] ds, JB = Aircraft where ρ(s) indicates the density of the aircraft at the point s in the body frame, I is the identity matrix, indicates the dot product, and is the Kronecker product, i.e., if u Rm, v Rn, then w = u v is a m n matrix with entries wij = ui vj. In most cases of interest, aircraft have a plane of symmetry, typically separating the left and right halves. Assuming that the y axis is orthogonal to the aircraft s plane of symmetry, the inertia tensor will have the following structure Jxx 0 JB = 0 Jyy Jxz (The cross moment of inertia Jxz = 0 R Aircraft ρ(x, y, z) Jxz 0. Jzz xz dx dy dz will in general depend on the choice of the body axes, and will be zero in case principal axes are chosen.) Substituting into 3

6 equations (1.1) and (1.2) (see 19, 4, 18, 60, 45, 59] for details) 1 m F B 1 m T B Kinematics X Y Z L M N = = U + QW RV V + RU P W Ẇ + P V QU J xx P J xz Ṙ J yy Q J zz Ṙ J xz P, (1.3) +QR(J zz J yy ) P QJ xz +P R(J xx J zz ) + (P 2 R 2 )J xz +P Q(J yy J xx ) + QRJ xz. (1.4) Equations (1.1) and (1.2) give the evolution over time of the velocity of the aircraft s center of mass and of the aircraft s angular velocity with respect to the inertial frame (expressed in body frame). In order to reconstruct the position and attitude (i.e., orientation) of the aircraft as a function of time, it is necessary to write equations for the aircraft s kinematics. Let R IB be the rotation matrix that maps vectors in the body frame to vectors in the inertial frame. Given a vector u R 3, define a hat operator such that the matrix (u) = û is the unique matrix satisfying ûv = u v, v R 3. In coordinates, given u = (u 1, u 2, u 3 ), one gets The aircraft kinematics can be written as 0 u 3 u 1 û = u 3 0 u 2. u 1 u 2 0 ṗ = R IB v B, (1.5) Ṙ IB = R IBˆω B, (1.6) where p is the position of the aircraft s center of mass. Even though the position and attitude of the aircraft can be completely specified by (1.5) and (1.6), maintaining all components of the rotation matrix R IB may be expensive from the computational point of view, since rotation matrices are not a minimal representation of aircraft attitude. In practice, several other methods to represent an aircraft s attitude are commonly used, and will be discussed in the following sections. However, some additional properties of rotation matrices are discussed first. A first point to note is that the spectrum of any matrix in SO(3) has the form eig(r) = {1, cos θ ± i sin θ}. Hence, any matrix R admits a unit eigenvector v such that Rv = v. Such an eigenvector identifies a fixed axis for the rotation R. The amplitude of the rotation is given by the angle θ. Since the trace of a matrix is the sum of its eigenvalues, it is the case that Tr(R) = 1 2 cos θ. Conversely, given a rotation angle θ and a fixed axis unit vector v, the corresponding rotation is 4

7 computed through Rodriguez formula: Rot( θ, v) = I + sin θ ˆv + (1 cos θ)ˆv 2. (1.7) The pair ( θ, v) is also referred to as the exponential coordinates of a rotation matrix R = Rot( θ, v), since Rot( θ, v) = exp( θˆv) Quaternions A widely used parametrization of rotations that requires fewer parameters than rotation matrices is based on unit quaternions, i.e., unit vectors in R 4 31]. A quaternion q = ( q, q) is typically written in terms of a scalar part q R and a vector part q R 3. A unit quaternion is such that q q = q 2 + q q = 1. The set of unit vectors in R 4 is denoted as the sphere S 3. A unit quaternion q = ( q, q) S 3 defines a rotation R = Rot(2 arccos q, q/ q ). (1.8) In other words, the vector part of a unit quaternion is parallel to the fixed axis of the rotation, and the scalar part determines the rotation angle. Conversely, a rotation matrix R = Rot( θ, v) can be represented by the unit quaternion q = ( cos( θ/2), sin( θ/2) v ). (1.9) Note that this representation is not unique, since for all q S 3, q and q map to the same rotation matrix. (In other words, S 3 is a double covering of SO(3).) Quaternions are composed through a multiplication operation, indicated with, defined as follows: q 1 q 2 = ( q 1 q 2 q 1 q 2, q 1 q 2 + q 2 q 1 q 1 q 2 ), or in matrix form: q 1 q 2 = q 1 q 1 ] q 1 T q 2. ˆ q It can be verified that quaternion multiplication as defined above is consistent with matrix multiplication, in the sense that if R 1 and R 2 are rotation matrices corresponding (according to (1.8)) to the unit quaternions q 1 and q 2, respectively, then R 1 R 2 corresponds to q 1 q 2. Also, for any unit quaternion q = ( q, q), its conjugate quaternion q = ( q, q) corresponds to the inverse rotation, in the sense that q q = q q = (1, 0). Finally, quaternion kinematics take the form q IB = 1 2 (0, ω I) q IB = 1 2 q IB (0, ω B ). (1.10) 5

8 This equation can be written in matrix form as follows q = 1 2 Ω(ω B)q, (1.11) where Ω(ω) 0 P Q R P 0 R Q Q R 0 P R Q P 0. (1.12) Quaternions are often the preferred method to model aircraft kinematics, since storing and multiplying quaternions require less memory and computation time than storing and multiplying rotation matrices. On the other hand, the fact that the set of unit quaternions is a double covering of the set of rotation matrices can cause stability and robustness issues 7]. Numerical errors in quaternion propagation can result in the quaternion losing its normalization constraint. It is common practice therefore to normalize a quaternion to ensure q 2 = 1 if it is numerically integrated for control or navigation purposes Euler Angles A rotation can also be parameterized through a sequence of three elementary rotations about coordinated axes. While many choices are possible, in the field of aircraft flight dynamics and control it is customary to use the roll, pitch, and yaw Euler angles, defined as follows. Let (X, Y, Z) be unit vectors defining the inertial frame, and let e 1, e 2, e 3 be the columns of the identity matrix. Starting with a body frame coinciding with the inertial frame, rotate it by an angle ψ (yaw) about the Z axis (coinciding with the third body axis), obtaining a first intermediate body frame given by R IB = Rot(ψ, e 3 ) = x, y, z ] (with z = Z, see Figure 1.4). Then, rotate this body frame by an angle θ (pitch) about the y axis, obtaining a second intermediate body frame given by R IB = R IB Rot(θ, e 2 ) = x, y, z ] (with y = y ). Finally, rotate this body frame by an angle φ (roll) about the x axis, obtaining the final body frame R IB = R IB Rot(φ, e 1 ) = x, y, z], with x = x. In summary, the rotation matrix R IB is computed as a function of the the roll, pitch, and yaw Euler angles (φ, θ, ψ) as R IB = Rot(ψ, e 3 )Rot(θ, e 2 )Rot(φ, e 1 ) cos ψ cos θ cos ψ sin φ sin θ cos φ sin ψ sin φ sin ψ + cos φ cos ψ sin θ = cos θ sin ψ cos φ cos ψ + sin φ sin ψ sin θ cos φ sin ψ sin θ cos ψ sin φ. (1.13) sin θ cos θ sin φ cos φ cos θ Conversely, given a rotation matrix R, Euler angles can be obtained as θ = arcsin R 31, φ = arctan R 32 R 33, ψ = arctan R 21 R 11, 6

9 Figure 1.4: Definition of the three Euler angles used and the associated sequence of rotations where the 4-quadrant arctangent should be taken for φ and ψ. A direct relationship can also be given between Euler angles and quaternions. Let the components of a quaternion be given by q = q, q x, q y, q z ]; then θ = arcsin 2(q x q z qq y ), φ = arctan2 2(q y q z + qq x ), 1 2(q 2 x + q 2 y) ], (1.14) ψ = arctan2 2( q x q y + qq z ), 1 2(q 2 y + q 2 z) ], and q = ± (cos(φ/2) cos(θ/2) cos(ψ/2) + sin(φ/2) sin(θ/2) sin(ψ/2)), (1.15) q x = ± (sin(φ/2) cos(θ/2) cos(ψ/2) cos(φ/2) sin(θ/2) sin(ψ/2)), (1.16) q y = ± (cos(φ/2) sin(θ/2) cos(ψ/2) + sin(φ/2) cos(θ/2) sin(ψ/2)), (1.17) q z = ± (cos(φ/2) cos(θ/2) sin(ψ/2) sin(φ/2) sin(θ/2) cos(ψ/2)), (1.18) where the sign is arbitrary, but must be consistent. The kinematics of the Euler angles can be obtained by differentiating (1.13) with respect to 7

10 time, and recalling (1.6). After simplification, one obtains φ θ ψ = 1 sin φ tan θ cos φ tan θ 0 cos φ sin φ sin φ cos φ 0 cos θ cos θ P Q R (1.19) Singularities are a potential problem for an Euler angle representation of the attitude kinematics. In the case of roll, pitch, yaw Euler angles, the singularities at θ = ±90 can cause computational difficulties. Hence, the recommended practice, especially for agile vehicles, such as fighter aircraft or small UAVs, that perform large-angle attitude maneuvers, is to use quaternions (or rotation matrices) to model the aircraft s kinematics. On the other hand, since Euler angles may be more intuitive to work with, it is common practice to use them as a front end for humans, e.g., to specify initial conditions, and to present simulation or flight test results. Equations (1.3) (1.5) and (1.11) (1.14) can be combined to form the nonlinear rigid-body equations of motion for a UAV. The next logical step is to discuss how the forces and moments acting on the UAV are generated. The full equations of motion will then be linearized to develop models for the linear control design Forces Forces and torques acting on a UAV are from a variety of sources, including aerodynamic, gravity, and thrust. In general, forces and torques will depend on the aircraft s position, attitude, on its linear and angular speed with respect to the surrounding air, and on the control settings. The aerodynamic forces are typically resolved into two components, lift (L) and drag (D) (see Figure 1.5). The aerodynamic lift force is perpendicular to the relative wind vector, while the drag force resists the vehicle motion along the relative wind. The direction of the aircraft s velocity relative to the wind with respect to the body frame is expressed by two angles, which can be thought of as spherical coordinates. The sideslip angle β is the angle formed by the aircraft s velocity with the aircraft s symmetry plane. The angle of attack α is the angle formed by the projection of the aircraft velocity on the aircraft s symmetry plane with the x body axis. Aerodynamic forces primarily depend on the angle of attack α, on the sideslip β, and on the dynamic pressure Q = 1 2 ρv 2 T, where ρ is the air density, which depends on the altitude, and V T is the free-stream airspeed of the aircraft. The lift and drag forces on the body will be in the plane of the x and z axes of the wind axis system shown in Figure 1.5. These can be rotated into the body frame using a rotation matrix that is a function of the angles α and β R BW = cos α cos β cos α sin β sin α sin β cos β 0 sin α cos β sin α sin β cos α. (1.20) In this case the aerodynamic forces in the body frame can be written in terms of lift and drag 8

11 67#8' 4%5' 1"#$%&' ()*+' 2' 3' 0"#$%&' ()*+'!"#$%&' ()*+'./'!"#$%&' &,#(%-%,+' Figure 1.5: Lift and drag acting on the aircraft (assuming no side forces) Xa D (Faero )B = Ya = RBW 0 Za L (1.21) Propulsive forces are typically contained within the aircraft s symmetry plane, possibly with a small angular offset αt with respect to the x body axis, yielding Xp cos αt (Fprop )B = Yp = kfprop k 0 Zp sin αt (1.22) Given the usual (NED) choice of inertial axis, wherein the z inertial axis points down, the forces due to gravity can be written as 0 sin θ T (Fgravity )B = Yg = mgrib 0 = mg cos θ sin φ, Zg 1 cos θ cos φ Xg (1.23) where g is the gravity acceleration Linearization of the Equations of Motion The right-hand side of Equations (1.3) and (1.4) are nonlinear, and typically far more complicated than can be addressed by standard control techniques and are more complicated than necessary for most flight regimes. Furthermore, the left-hand side, which specify the total forces and moments acting on the vehicle are even more complicated. Thus numerous simplifications have been developed. 9

12 Relative equilibria The standard approach is to assume that the vehicle is flying in an relative equilibrium condition and then linearize the equations of motion about this nominal flight condition. In the case of aircraft dynamics, a relative equilibrium is defined as a steady-state trajectory along which ω B = 0, and v B = 0, and the control inputs are maintained fixed, or trimmed to some value δ 0. Clearly, such a trajectory will be such that the linear and angular velocities in body frame are constant. These nominal values of the linear and angular velocities will be denoted by v 0 and ω 0, respectively. At equilibrium conditions, after writing out explicitly the dependency of the forces and moments, equations (1.1) and (1.2) take the form: F B (p, R IB, v 0, ω 0, δ 0 ) = ω 0 mv 0, (1.24) T B (p, R IB, v 0, ω 0, δ 0 ) = ω 0 J B ω 0. (1.25) Note that the position p and attitude R change along the trajectory, according to the kinematics equations ṗ = R IB v 0, and ṘIB = R IBˆω 0. For equations (1.24) and (1.25) to hold over time, it is necessary that the left-hand sides do not change along the trajectory, even though the aircraft s position and attitude change. In the case of aircraft dynamics, and over small vertical excursions (i.e., neglecting the dependency of the air density on altitude), it can be safely assumed that the body forces and moments are invariant to translations and to rotations about a vertical (inertial) axis, i.e., F B (p, R IB, v 0, ω 0, δ 0 ) = F B (p + p, Rot( ψ, e 3 )R IB, v 0, ω 0, δ 0 ), T B (p, R IB, v 0, ω 0, δ 0 ) = F B (p + p, Rot( ψ, e 3 )R IB, v 0, ω 0, δ 0 ), for any translation p and for any heading rotation ψ. In order for equations (1.24) and (1.25) to hold throughout the trajectory, and hence for the trim condition (v 0, ω 0, δ 0 ) to be a valid relative equilibrium, it is necessary that R IBˆω 0 = 0, 0, ψ 0 ] T for some constant heading rate ψ 0. In other words, in the inertial frame, trajectories corresponding to relative equilibria take the form of circular helices with a vertical axis, flown at constant speed, angle of attack, and sideslip angle. In the special case in which ψ 0 = 0, i.e., on relative equilibria corresponding to straight-line motion, the total forces and moments are zero but this is not true for the general case. When analyzing the dynamics of an aircraft near a relative equilibrium, it is convenient to choose the body frame in such a way that the angle of attack is zero at equilibrium. In other words, the x body axis is chosen in such a way that the relative wind direction is in the (x, y) body plane at equilibrium. Such a body frame is called the stability frame, due to its importance in the aircraft stability analysis. Notice that the stability frame depends on the particular relative equilibrium under consideration; for different flight conditions, stability frames will in general be different. In the following, a class of symmetric relative equilibria are considered, along which the 10

13 sideslip, roll, and yaw angle are zero. To proceed with the linearization of the dynamics for various flight conditions, define the perturbed velocity and accelerations in terms of the nominal values, as given in Table 1.1. The assumption herein is that the perturbations to the nominal are much smaller in magnitude than the nominal (i.e., u U 0 ). Table 1.1: Definition of the perturbation variables about the equiibrium condition. Nominal Perturbed Perturbed Velocity Velocity Acceleration Velocities U 0 U = U 0 + u U = u W 0 = 0 W = w Ẇ = ẇ V 0 = 0 V = v V = v Angular P 0 = 0 P = p P = ṗ Rates Q 0 = 0 Q = q Q = q R 0 = 0 R = r Ṙ = ṙ Angles Θ 0 Θ = Θ 0 + θ Θ = θ Φ 0 = 0 Φ = φ Φ = φ Ψ 0 = 0 Ψ = ψ Ψ = ψ A key element of the complexity here is that typically the aerodynamic, thrust, and gravity forces and moments will also be perturbed by the perturbed motion of the vehicle, which are denoted in the following as X,..., N: X Y Z L M N = m = u v + ru 0 ẇ qu 0 J xx ṗ J xz ṙ J yy q J zz ṙ J xz ṗ (1.26) (1.27) where we use the ( ) to distinguish between the lift L and the moment L. The key aerodynamic parameters are also perturbed: Total Velocity V T = ((U 0 + u) 2 + v 2 + w 2 ) 1/2 U 0 + u (1.28) Perturbed Sideslip angle β = sin 1 (v/v T ) v/u 0 (1.29) Perturbed Angle of Attack α x = tan 1 (w/u) w/u 0 (1.30) Stability Derivatives To develop the equations of motion further, the terms X... N must be investigated. Recall that at equilibrium, the net forces and moments must be zero. But since the aerodynamic and 11

14 gravity forces are a function of equilibrium condition and the perturbations about this equilibrium. In general it is very difficult to determine the exact nature of these aerodynamic perturbations. Thus the standard approach (see detailed discussions in 18, 19, 45]) is to try to predict the changes in the aerodynamic forces and moments using a first order expansion in the key flight parameters: X = X X X X U + W + Ẇ + U W Ẇ Θ Θ X g Θ Θ + Xc (1.31) = X U u + X W w + X Ẇ ẇ + X Θ θ X g Θ θ + Xc (1.32) where X U is called a stability derivative, which is evaluated at the equilibrium condition. Note that both dimensional and non-dimensional forms are used. Clearly this is an approximation since it ignores any time-lags in aerodynamics forces (assumes that forces are only functions of instantaneous values). As before, X g in Equation (1.32) corresponds to the X body force component due to gravity, and the perturbation gravity and thrust forces and moments. Assuming Φ 0 = 0, then X g Θ = mg cos Θ 0 0 Z g Θ = mg sin Θ 0. 0 Also X c denotes the perturbations due to the control actuators (e.g. rudder, ailerons, elevators, thrust). While Equation (1.32) leads to simplified force (and moment) perturbations, it is still clear that the linearized expansion can involve many terms u, u, ü,..., w, ẇ, ẅ,.... Thus it is typical to only retain a few terms to capture the dominant effects. For symmetric aircraft, this dominant behavior is most easily discussed in terms of the symmetric variables: U, W, Q and forces/torques: X, Z, and M, and the asymmetric variables: V, P, R & forces/torques: Y, L, and N. Furthermore, for most flight conditions, further simplifications can often be made. For example, for truly symmetric flight Y, L, and N will be exactly zero for any value of U, W, Q. So the derivatives of asymmetric forces/torques with respect to the symmetric motion variables are zero. Also, the derivatives of symmetric forces/torques with respect to the asymmetric motion variables are small and can often be neglected. Often derivatives with respect to the derivatives of the motion variables can also be neglected, but Z/ ẇ and Mẇ M/ ẇ (aerodynamic lag involved in forming new pressure distribution on the wing in response to the perturbed angle of attack) should not be neglected. Also, X/ q is often negligibly small. A summary of the effects of the aerodynamic perturbations is as follows: (1) X = ( ) X U 0 u + ( ) X W 0 w X u, α x w/u 0 (2) Y β v/u 0, p, r (3) Z u, α x w/u 0, α x ẇ/u 0, q (4) L β v/u 0, p, r (5) M u, α x w/u 0, α x ẇ/u 0, q 12

15 Table 1.2: Standard stability derivatives for a typical aircraft planform showing that numerous terms are 0 or small. The other non-zero terms, denoted as, must be computed using the methods described in 19, 45]. ()/ () X Y Z L M N u v w p q r (6) N β v/u 0, p, r The result is that, with these force, torque approximations, equations (1), (3), (5) decouple from (2), (4), (6). In particular, equations (1), (3), and (5) are the longitudinal dynamics in u, w, and q X Z M = m u m(ẇ qu 0 ) ( X U ( Z U ( M U (1.33) J yy q ) 0 u + ( ( ) X W )0 w + Xg Θ θ + 0 Xc ) 0 u + ( ) ( ) ( ) ( ) Z W 0 w + Z ẇ + Z Ẇ 0 Q q + Zg 0 Θ θ + 0 Zc ) 0 u + ( ) ( ) ( ) M W 0 w + M ẇ + M Ẇ 0 Q q + M c 0 and equations (2), (4), and (6) are the lateral dynamics in v, p, and r Y L N Actuators = m( v + ru 0 ) J xx ṗ J xz ṙ (1.34) (1.35) J zz ṙ J xz ṗ ( Y ) V 0 v + ( ) Y P 0 p + ( ) Y R 0 r + + Y ( ) ( ) ( ) c L V v + L 0 P p + L 0 R r + ( 0 Lc N ) V 0 v + ( ) N P 0 p + ( (1.36) ) N R 0 r + N c The primary actuators in the longitudinal direction are the elevators and thrust. Clearly the thrusters and elevators play a key role in defining the steady-state and equilibrium flight condition. The focus now is on determining how they also influence the aircraft motion about this equilibrium condition. For example, if the elevator is deflected, a perturbation in the vehicle motion would be expected, as captured by u(t), w(t), q(t). 13

16 Recall that X c is the perturbation in the total force in the X direction as a result of the actuator commands, i.e. a force change due to an actuator deflection from trim. As before, one can approximate these aerodynamic terms using the same perturbation approach X c = X δth δ th + X δe δ e (1.37) where δ e is the deflection of the elevator from trim (down positive), δ th is the change in thrust, and X δe and X δth are the control stability derivatives. This results in X c Z c M c = X δth Z δth M δth X δe Z δe M δe A similar process can be performed in the lateral direction for the rudder and ailerons. δ th δ e ] (1.38) Equations of Motion Combining the results of the previous sections for the longitudinal dynamics, yields the equations of motion: m u = X u u + X w w mg cos Θ 0 θ + X c (1.39) m(ẇ qu 0 ) = Z u u + Z w w + Zẇẇ + Z q q mg sin Θ 0 θ + Z c (1.40) J yy q = M u u + M w w + Mẇẇ + M q q + M c (1.41) If there is no roll/yaw motion so that q = θ and Mẇ and Zẇ are assumed to be small, these can be rewritten in state space form using α = w/u o as m u m α J yy q θ X u X α 0 mg cos Θ 0 u X c = Z u /U 0 Z α /U 0 Z q /U 0 + m mg/u 0 sin Θ 0 α M u M α M q 0 q + Z c M c θ 0 (1.42) or equivalently as ẋ = Ax + Bu where A = X u m Z u X α m Z α 0 g cos Θ 0 Z q mu 0 mu mu 0 g sin Θ 0 U 0 M u M α M q J yy J yy J yy and B = X δth m Z δth X δe m Z δe mu 0 mu 0 M δth M δe J yy J yy 0 0 (1.43) Note there are significant notational simplifications if the stability derivatives are defined to include m and J yy. Using a procedure similar to the longitudinal case, the equations of motion for the lateral 14

17 dynamics can be developed using the state vector φ x = β p with inputs u = r δ a δ r ] where ψ = r sec Θ 0 A = B = and tan θ 0 g U 0 cos θ 0 Y β mu 0 Y p mu 0 Y r mu ( L β J xx + J zxn β ) ( L p J xx + J zxn p ) ( L r J xx + J zxn r ) 0 (J zxl β + N β J zz (mu 0 ) (J xx) 1 J zx 0 J zx (J zz) 1 ) (J zxl p + N p Y δa L δa N δa J zz Y δr L δr N δr ) (J zxl r + N r J zz ) (1.44) (1.45) J xx = (J xx J zz J 2 zx)/j zz J zz = (J xx J zz J 2 zx)/j xx (1.46) J zx = J zx /(J xx J zz J 2 zx) Thus, in either case, the aircraft equations of motion can be written as ẋ = Ax + Bu (1.47) y = Cx + Du (1.48) where typically D = 0, but this depends on the sensor and actuator selection. One can also convert the dynamics into transfer function form (for each sensor/actuator pair) y i = G ij (s)u j, G ij (s) = C i (si A) 1 B j + D ij that can be used in the classical control design process Wind Disturbances In the presence of wind, the atmosphere is moving relative to the Earth frame 45, 60]. Therefore the equations of motion must be modified to account for the fact that the aerodynamic force and moments are functions of the relative motion between the aircraft and the atmosphere, and not the inertial velocities. Thus we must differentiate between the inertial velocity of the vehicle v i and the 15

18 velocity of the vehicle with respect to the air mass v a resulting from wind effects W g v a = v i R BI W g (1.49) In this case, v a should be used to calculate the aerodynamic forces and moments rather than v i, and this quantity can be reliably estimated using an airspeed measurement device. More specifically, in the X-direction, let u be the aircraft perturbation speed, and u g be the gust speed in that direction, then the aircraft speed with respect to the atmosphere is u a = u u g. In this case the linearization of the aerodynamic forces and moments in Equation (1.32) should be rewritten in terms of the perturbations relative to the atmosphere X = X U (u u g) + X W (w w g) + X Ẇ (ẇ ẇ g) + X Q (q q g) X Θ θ X g Θ θ + Xc (1.50) but note that the gravity and control input terms remain the same. The rotational aspects of the gusts are caused by spatial variations in the gust components, so that p g = wg y Repeating the linearization process outlined before leads to a new input term and q g = wg x. ẋ = Ax + Bu + B w w (1.51) where, for example, in the longitudinal case, B w = X u m Z u mu 0 X α m Z α mu 0 0 Z q mu 0 M u M α M q J yy J yy J yy and w = u g α g q g (1.52) and a similar operation can be performed for the lateral dynamics in terms of the disturbance inputs β g, p g, and r g. The input w can now be used to model the effects of various steady and stochastic wind models, as well as wind shear. Numerous stochastic models have been developed for studying the effect of winds on the aircraft dynamics and performance 43, 61, 60], but these must be used with care when applied to the low altitude operations expected for most small-scale UAV flights Simplified Aircraft Equations of Motion for Path Planning A simplified set of aircraft equations of motion can be derived for path planning purposes. In deriving these equations, the aircraft roll rate, pitch rate, and yaw rate dynamics are ignored and are replaced using kinematic approximations. The resulting model describes the motion of a rigid 16

19 !"#$%&' ()*+'!" #" /"#$%&' ()*+' $",-' 0)1%2)3'."#$%&' ()*+' Figure 1.6: Definition of the flight path angle point mass with kinematic path constraints. For the purpose of this model, the position of the aircraft in an inertial frame whose x axis is parallel to the local horizon is denoted by xe, ye, ze. The flight path angle γ denotes the angle between the local horizon and the velocity vector of the aircraft VT (see Figure 1.6). The heading angle ψ is the angle between VT and the z axis of the local inertial frame. The bank angle φ is the angle that the aircraft is banked about the velocity vector VT. The forces acting on the aircraft consist of the weight mg, thrust T, lift L, and drag D. The equations for a point mass model of a fixed wing aircraft can then be formulated by assuming small γ and ψ 63, 19] x e = VT cos γ cos ψ, (1.53) y e = VT cos γ sin ψ, (1.54) z e = VT sin γ, 1 = T D mg sin γ], m 1 L cos φ mg cos γ], = mvt L sin φ =. mvt cos γ (1.55) V T γ ψ (1.56) (1.57) (1.58) and the lift and drag of the aircraft can be approximated as: L = QSCl and D = QS(CD0 + KCL2 ), (1.59) where S is the wing surface area, CL is the lift coefficient, CD0 is the parasitic drag coefficient, and K is a constant dependent on the aircraft wing geometry. 1.3 Flight Vehicle Control using Linear Techniques As presented above, even in the simplest case of decoupled linearized dynamics, the equations of motion of an aircraft are quite complicated, with both the longitudinal and lateral dynamics 17

20 being described by a fourth order system with multiple actuator inputs and sensor outputs y. Furthermore, the speed of the natural response of the system associated with the pole locations of the open-loop dynamics are typically inconsistent with fast and/or stable response to commands, such as waypoint, trajectory tracking, and agile maneuvers. Finally, the dynamics of several UAVs can be inherently unstable, typical examples include rotorcraft such as helicopters, quadrotors, and ducted fans (see e.g. 41, 10]). Thus feedback control is required to stabilize and/or speed up the response. Given the complexity of the dynamics, there are two basic strategies for the control design. The first is to continue the decomposition in the previous section to identify components of the dynamics that are well controlled by specific choices of the actuators, and then perform successive loop closure (see e.g. 34, 28]). In this case the loops are nested by arranging that the reference commands for the inner loop are provided by the outer-loop controller. An example of this is shown in Figure 1.13 in which the outermost position control loop provides desired velocity commands using path following guidance discussed in Section 1.4. The outer velocity control loop provides a reference (in this case a desired quaternion value) for the inner attitude control loop. One key advantage of this approach is that it leads to a natural mechanism of handling limits on flights variables (e.g., such as bank or pitch angles) and actuator inputs because the reference commands can be saturated before being passed to the inner loop. Each step of the control design process is simpler, but the nesting of the control loops leads to some challenges. The general rule of thumb is to ensure that the inner control loops result in fast dynamics, and then each successive loop added is slower than the previous one. The primary difficulties here are to determine what is meant by fast and slow and how to determine if there is too much interaction between the inner/outer loops being closed (e.g., closing the outer loop might reduce the performance of the inner loop requiring a redesign). The second approach is to design a controller for the full dynamics - either linear or nonlinear. The advantage of this approach is that it employs the power of state space control approaches to handle the fully coupled dynamics. However, it is difficult to handle actuator saturation and very hard to includes state constraints. Furthermore, unless done with extreme care, these controllers, especially in high performance flight, can be very sensitive to modeling errors and omissions. Having determined the architecture, the next step in any control design is to determine the dynamics of the system of interest (i.e., the full set of dynamics, or the approximate inner loop dynamics). Having identified this, one should then determine the requirements and the extent to which the dynamics meet these goals. For example there may be requirements on certain frequency (to ensure the dynamics are fast ) and damping (to ensure that the oscillations die out quickly) specifications on the pole locations. There may also be requirements on the maximum steady tracking error to a step command input. Since the open-loop dynamics of the vehicle rarely satisfy these requirements, the typical approach is to use linear feedback control to modify the pole locations and loop gains. 18

21 d i d o r(t) e u y G c (s) G(s) v Figure 1.7: Classical control feedback loop Standard Performance Specifications Figure 1.7 shows a standard feedback control loop, where a typical performance metric, the tracking error, can be written as: e = r (y + v) = S(r d o v) SGd i yielding y = T (r v) + Sd o + SGd i with L = GG c, S = (I + L) 1 T = L(I + L) 1. So good tracking performance of r(t) (signal typically has low frequency content) requires e be small, which translates to S(jω) being small at low frequencies, i.e. 0 ω ω l. Furthermore, to reduce the impact of sensor noise v (which typically has high frequency content), requires T (jω) be small for all ω ω h. Since T (s) + S(s) = I s, one cannot make both S(jω) and T (jω) small at the same frequencies, which represents a fundamental design constraint. There are two basic approaches to design. The indirect approach that works on L, the loop transfer function rather than S and T. Much of classical control design takes this approach. The direct approach, which works with S and T, is discussed in Section For the indirect approach, note that if L(jω) 1, S = (1 + L) 1 L 1, so S 1and T 1. Furthermore, if L(jω) 1, S = (1 + L) 1 1 and T L, so T 1. So this converts the performance requirements on S, T into specifications on L, with the two regions in Figure 1.8: (A) High loop gain which leads to good command following and disturbance rejection, and (B) Low loop gain leads to attenuation of any sensor noise. One must be careful at crossover, when L(jω c ) 1, which requires that arg L(jω c ) ±180 to maintain stability 36, 20, 32, 16, 56]. In summary, the typical control design challenges are to achieve high enough gain at low frequency to obtain good performance, low enough gain at high frequency to attenuate sensor noise effects, and sufficient stability margins in the transition range Classical Techniques Classical and PID Controllers There are three standard forms of the classical controller, which in the form of Figure 1.7 can be written as u = G c (s)e = Nc(s) D c(s) e, where e is the error signal, typically the difference between the actual variable, for example θ and the commanded value θ c (e = θ c θ). 19

22 Figure 1.8: Standard loop shaping goals for the indirect design approach The simplest is Proportional feedback, which uses G c K g a gain, so that N c = D c = 1. This is adequate for some systems, but is typically constrained in what gains can be applied before at least t some of the dynamics of the vehicle are destabilized. Integral feedback uses u(t) = K i 0 e(τ)dτ which means that G c (s) = K i s. This is typically used to reduce/eliminate steady-state error since, if e(τ) is approximately constant, then the magnitude of u(t) will grow and thus hopefully correct the error. Example #1: consider the error response of G p (s) = r(t) = 1(t) r(s) = 1/s where e r = G c G p = S(s) e(s) = 1 (s + a)(s + b) r(s) (1 + G c G p ) (a > 0, b > 0) to a step, and S(s) is the Sensitivity Transfer Function for the closed-loop system. The final value theorem (lim t e(t) = lim s 0 se(s)) can be used to analyze the error. So with proportional control, ( s ) 1 e ss = lim e(t) = lim t s 0 s 1 + K g G p (s) = Kg ab indicating that e ss can be made small, but only with a very large K g. Note however, that with integral control, lim s 0 G c (s) =, so e ss 0. To summarize, integral control improves the steady state, but this is at the expense of the transient response, which typically gets worse because the system is less well damped. 1 Example #2: G p (s) = (s+a)(s+b), add integral feedback to improve the steady state response. Increasing K i to increase the speed of the response pushes the poles towards the imaginary axis leading to a more oscillatory response (compare the possible locus of roots of the closed system 20

23 Figure 1.9: Root Locus for G(s) in example #2 using proportional feedback Figure 1.10: Root Locus for G(s) in example #2 with integral feedback Figure 1.11: Root Locus for G(s) using proportional and integral feedback Figure 1.12: Root Locus for G(s) using derivative feedback shown in Figures 1.9 and 1.10 using the two control approaches). If the proportional and integral (PI) feedback are combined, then G c (s) = K 1 + K 2 s = K 1s+K 2 s which introduces a pole at the origin and zero at s = K 2 /K 1. The zero has the effect of reducing the tendency of the closed-loop poles to move into the right-half plane as the gain is increased, and thus this PI combination solves some of the problems with using just integral control (compare Figures 1.10 and 1.11). The third approach is derivative feedback which uses u = K d ė so that G c (s) = K d s. Note that this will not help with the steady state response since, if stable, at steady state one should have ė 0. However, it does provide feedback on the rate of change of e(t) so that the control can anticipate future errors. Example #3: G(s) = 1 (s a)(s b), (a > 0, b > 0) with G c(s) = K d s. As shown in Figure 1.12, derivative feedback is very useful for pulling the root locus into the lefthand plane or increasing the 21

24 damping leading to a more stable response. It is typically used in combination with proportional feedback to form proportional-derivative feedback PD G c (s) = K 1 + K 2 s which moves the zero in Figure 1.12 away from the origin. The three approaches discussed above can be combined into the standard PID controller G c (s) = ( K p + K ) I s + K Ds (1.60) Typical difficulties in this case are that numerical differentiation of a noisy measured signal can result in noisy or lagged estimates in practice. Furthermore, long-term integration of a signal that has an unknown bias can lead to large control commands. Thus one would typically use bandlimited differentiation/integration instead, by rolling-off the PD control with a high-frequency pole (or two), and only integrating signals above a certain frequency (high pass filter) in the PI controller. This leads to a slightly different primary building block for the controller components that is of the form G B (s) = K c (s + z) (s + p) (1.61) which, depending on how the gains are selected can be morphed into various types of controllers. (s+z) For example, if one picks z > p, with p small, then G B (s) K c s which is essentially a PI compensator. In this form the compensator is known as a lag. If instead p is chosen such that p p z, then at low frequency, the impact of (s+p) is small, so G B(s) = Kc p p (s + z) (s+p) K c (s + z) which is essentially a PD compensator. In this form the compensator is called a lead. Various algorithms exist for selecting the components of G B (s). One basic procedure is to first use a lead controller to augment the phase margin of the loop transfer function L = G c (s)g(s) at the crossover frequency ω c to meet the requirements (e.g., desired phase margin). The Phase margin is a standard stability margin for a system that measures the amount that the phase of the loop transfer function L(s) differs from 180 when L(jω) = 1. Typically one wants a phase margin of greater than 30 degrees, but other important performance metrics include the gain-margin, the peak of the allowable transient, rise and settling times, and time-delay margin 16, 48]. 1. Find φ required required from the performance specifications. Note that φ required = P M (180 + G(jω c )) 2. Arrange to put φ max at the crossover frequency. Note that the maximum phase added by a lead is sin φ max = 1 α z 1 sin φmax 1+α where α = p, which implies that α = 1+sin φ max. Further, since the frequency of the maximum phase addition is ω max = z p, set ωc 2 = p z. 3. Select K c so that the crossover frequency is at ω c. To increase the low frequency gain to improve the steady state response, one could use an integrator, or use a lag to increase the loop gain. This is done by picking k c to yield the desired low frequency gain increase for the loop transfer function (which now includes the vehicle dynamics and the lead controller if added). It is also important to include the constraint that lim s G lag (s) = 1, so k c = z / p ) to avoid impacting the higher frequency dynamics. A rule of thumb is to limit 22

25 x ref Position v ref Velocity q d Attitude u Controller Controller Controller Aircraft Dynamics x a, v a, q a, ω a Navigation System Figure 1.13: Example of a possible sucessive loop-closure control architecture where the measured quantities (x a, v a, q a, ω a ) are fed back to specific control loops. Each outer loop generates a reference command that is used to create error signals for the inner loop. Saturation on these reference commands can easily be incorporated (not shown). As shown in the text, the particular architecture required depends on the vehicle and control objectives. the frequency of the zero of the lag compensator so that there is a minimal impact of the phase lag at ω c. Thus with z ω c /10 and k c both specified, one can solve for the pole location using p = z /k c Successive Loop Closure Examples As an example of the successive loop closure process in Figure 1.13, the following considers the combination of the velocity and attitude control loops, wherein the attitude reference command is created by the velocity controller. Note that for small angles the attitude controller could be based on independent PD control of each Euler angle, but the following presents a more general attitude control approach that is based on the quaternion representation of the vehicle attitude. A second example of the successive loop closure approach is provided in Section Quaternion-Based Attitude Control The sequential rotation properties of quaternions can be used to devise a generalized attitude controller for flight vehicles 62, 30, 25, 42]. Consider some desired aircraft attitude that is specified as a quaternion rotation from the global frame q d. This desired rotation can be constructed intuitively with an axis and an angle as in Equation (1.9). The actual measured attitude of the aircraft can also be represented as a quaternion rotation from the global frame q a. Now consider that the desired attitude q d can be constructed as the measured attitude q a sequentially rotated by some error quaternion q e. Since quaternion rotations are sequential, the error quaternion represents a rotation from the body frame: q d }{{} global frame = q e }{{} body frame q a }{{} global frame (1.62) 23

26 The error quaternion can then be solved for explicitly using the conjugate of the measured attitude quaternion q e }{{} body frame = q d }{{} global frame q a }{{} global frame (1.63) The error quaternion q e represents the rotation required to get from the measured attitude to the desired attitude. Since the rotation is from the body frame, the x, y and z components of q e correspond to the rotations needed about the x, y and z body axes of the aircraft. Thus the three components correspond directly to the required aileron, elevator, and rudder commands (or roll cyclic, pitch cyclic, and pedal commands for a helicopter UAV) without any transformation. The scalar part of q e, q e represents the angle through which the aircraft must be rotated and is thus proportional to the amount of control effort required. As in Section , let q e denote the vector part of q e. The quaternion representation suggests a simple control law of the form u = K p q e K d ω, where u = δ a, δ e, δ r ] T is the vector containing the aileron, elevator, and rudder deflections, and K p, and K d are positive definite proportional and derivative gain matrices. However, this control law suffers from the unwinding phenomena (see e.g. 39]) characterized by the aircraft undergoing a larger full rotation in order to achieve the desired attitude instead of a more efficient smaller rotation. One reason this happens is because the unit quaternions 1, 0, 0, 0] and 1, 0, 0, 0] both represent the unique zero attitude and are the equilibria of the quaternion dynamics given in Equation (1.10). Hence if a naïve control law were used for regulation to zero attitude represented by q d = 1, 0, 0, 0], an undesirable rotation, possibly leading to instability, could occur. On the other hand, the control law u = K p q e K d ω would stabilize q d = 1, 0, 0, 0], but result in an undesirable rotation if q d = 1, 0, 0, 0]. This indicates that the sign of the error quaternion determines the sign of the gain K p. Therefore, a consistent control law that satisfies u( q e, q, ω) = u( q e, q, ω) is desirable 39]. Several such consistent asymptotically stabilizing quaternion control laws can be formulated (see e.g. 39, 62]), with two examples being or u = sgn( q e )K p q e K d ω, (1.64) u = q e 2 K p q e K d ω. (1.65) Another way to implement a consistent control law is to simply monitor the sign of q e online and enforce that the right error quaternion representation is chosen. With this constraint the following control law with time-varying gains has yielded good attitude control performance 57, 42, 15] θ e u = sin( θ e /2) K p q e K d ω, (1.66) where θ e = 2 arccos( q e ). While denoted above as aileron, elevator, and rudder, the control commands are given in the 24

27 body frame and thus easily generalizable to any actuator set (e.g., a three-wing tailsitter or quadrotor helicopter) as long as the commands can be mapped to the appropriate actuators. Another key advantage of this attitude controller over Euler angle methods is the lack of singularities, i.e. the output of the controller is the direct rotation needed (from the body frame) to get from the measured attitude to the desired attitude 25, 30, 42, 15]. Computationally, no trigonometric functions or rotation matrices are needed, making the controller amenable to onboard or embedded implementation Outer-Loop Horizontal Velocity Controller The outer-loop velocity controller can be designed to command a desired attitude based on the desired velocity. For airplanes, this means that in order to achieve a desired velocity in the body y direction, the aircraft must rotate (bank) about its body x axis and/or rotate (yaw) about its body z axis. Furthermore, in order to achieve a desired flight path angle γ, it must rotate (pitch) about its y axis. Velocity error and desired attitudes are also tightly coupled for rotorcraft UAVs, where horizontal velocity is achieved by tilting the thrust vector. For a rotorcraft with non-hinged blades such as typical quadrotors, this is often achieved by tilting the body itself. Tilting the vehicle in any direction will generate a horizontal component of velocity. With the attitude controller in place, a simple outer-loop horizontal velocity controller can be added to generate the desired attitude q d. To calculate the desired quaternion given the desired x- and y-, and z velocities v dx, v dy, v dz ] and the actual measured x-, y-, and z- velocities v ax, v ay, v az ], the velocity errors are calculated: v ex = v dx v ax v ey = v dy v ay (1.67) v ez = v dz v az The errors are then used to determine the desired quaternion rotation q d in the form q d = 1.0, q dx ( v e ), q dy ( v e ), q dz ( v e ) ]. (1.68) A simple control law that works particularly well for rotorcraft by generating the elements of the desired rotation in Equation (1.68) based on the velocity error is q dx = K pvy v ey + K ivy v ey dt, q dy = K pvx v ex K ivx v ex dt, (1.69) q dz = 0, where K p. are the proportional gains and K i. are the integral gains. In this control law, lateral 25