Modeling, Simulation and Control of a Quadrotor Carrying a Slung Load. Master Thesis Masterarbeit. presented by von. Bähner, Franz David

Transcription

1 Modeling, Simulation and Control of a Quadrotor Carrying a Slung Load Modellierung, Simulation und Regelung eines Quadrokopters mit aufgehängter Nutzlast Master Thesis Masterarbeit presented by von Bähner, Franz David Supervisor (US-ETSI): Supervisor (RWTH-IRT): Manuel Vargas Villanueva, Ph.D. Björn Schäfer, Dipl.Ing. Departamento de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla

2

3 Abstract Autonomous quadrotor flight is already a well-investigated area of research. Nonetheless, it must still be considered contemporary engineering science. Applications span from advanced aerial maneuvering to swarm flight; in this work the case of a quadrotor carrying a slung load is analyzed. In a novelty approach the ambition of directly controlling the load position is pursued. This has become possible by the development of fast and reliable monitoring systems, as encountered in a variety of vision based control tasks. The first step toward the objective is achieved by deriving well-founded models of the system at hand, using Newton-Euler as well as Lagrange-Euler formalisms for redundancy reason. Secondly, an appropriate two-dimensional model is found. At hand of this simplified model, linear and non-linear control strategies are assessed with respect to their applicability. Beyond this, a trapezoidal trajectory generator is developed. It may take non-zero initial as well as final velocities into account and synchronizes multi-dimensional profiles. Ultimately, linear quadratic control approaches seem to be most promising and are applied successfully to the three-dimensional model.

4

5 List of Symbols a α C d D e η f i f j f j f G g G G bc h I 3 3 I B I ii I L J LQR K K κ th L λ i M ν Displacement of Rotors from COG Vector of stabilizing functions Coriolis matrix of system 1, or -1 denoting a direction Damping matrix of system Control error Vector of generalized coordinates of quadrotor-load system Thrust force of rotor Force vector of force j Total scalar value of force j Vectorial function Transfer function Gravitational constant Gravity vector of system Closed-loop transfer function Vectorial function Three times three unity matrix Inertia tensor of vehicle Inertia about axis i Inertia of payload Feedback-invariant in LQ approach Kinetic energy of a system State-feedback matrix in LQ approach Thrust coefficient Lagrangian of a system Eigenvalue of matrix Mass matrix of system Derivation of generalized coordinates w.r.t. time v

6 3 3 ω Ω i p P P φ ψ W p B W p L B p L L p B Q q q Q fr i Q Q i R r (t) r R i r j T a t i τ τ i τ i θ u u i V V v c v w Three times three zero matrix Angular velocity rates in body-fixed frame Rotor Speed Body-fixed rotational velocity about x Potential energy of a system Symmetric positive definite matrix in LQ approach Roll angle Yaw angle Position of vehicle in inertial frame Position of payload in inertial frame Position of payload in body-fixed frame Position of vehicle in load-fixed frame Weight matrix in LQ approach Generic set of generalized coordinates Body-fixed rotational velocity about y Friction force Vector of generalized forces Generalized force in i Weight matrix in LQ approach Reference signal Body-fixed rotational velocity about z Rotation about axis i Location of force j Acceleration time Inter-body force of body i Actuation vector of system Moment about axis i (vectorial) Value of moment about axis i Pitch angle Vector of manipulated variables Manipulated variable i Lyapunov function candidate Friction function Cruising velocity Peak velocity of wedge profile vi

7 W x x i F x i F y i y y i F z i z i ζ Transform time derivatives of roll, pitch, yaw to body-fixed angular velocity State vector State i x-value of position vector in frame F y-value of position vector in frame F Output vector Output i z-value of position vector in frame F Error state/signal Angle-triad in inertial coordinates vii

8

9 List of Abbreviations COG DOF GPS IMU LFC LTI LQ MEMS MIMO SISO UGAS UML VTOL Center of gravity Degree of freedom Global Positioning System Inertial measurement unit Lyapunov Function Candidate Linear time-invariant Linear quadratic Micro Electromechnical Systems Multiple input multiple output Single input single output Uniformly globally asymptotically stable Unified modeling language Vertical take-off and landing ix

10

11 Contents List of Figures List of Tables xv xvii 1 Introduction 1 2 Theoretical Background Assumptions and Parameters Deriving the Equations of Motion Kinematics Quadrotor Kinematics Load Kinematics Rigid Body Dynamics Dynamic Model: Newton - Euler Formalism Dynamic Model: Euler - Lagrange Formalism Linearization Linearized State-Space Expression Trajectory Generation Trapezoidal Velocity Profiles One- vs. Multi-Dimensional Trajectories Trapezoidal Profiles with zero Initial / Final Velocities Trapezoidal Profiles with non-zero Initial / Final Velocities Preassigned Durations with reduced Accelerations Superordinate Structure Input Shaping Smooth Acceleration Profiles xi

12 Contents 4 Planar Model Derivation Manipulated Variables Kinematics Equations of Motion - Newton-Euler Formalism Equations of Motion - Euler-Lagrange Formalism Non-Linear Model Matrix Models Linearization Linearized State Space Expression Laplace Transform Linear Control Design (Planar Case) PID Control Altitude Control Attitude Control Position Control Simulation Results Linear Quadratic Optimal Control LQ Non-Zero Set Point Control LQ Servo Control Adaptivity Approach Non-Linear Control Design (Planar Case) Lyapunov Stability Autonomous Systems Nonautonomous Systems Input-to-State Stability Backstepping Controller for Translational Sub-System Control Design Improved Air Resistance Consideration Simulation Results Extension to the Load-Controlled Flight-Mode Feedback Linearization Collocated Linearization xii

13 Contents Control Design Simulation Results Non-Collocated Linearization - Extension to the Load-Controlled Flight- Mode Control Robustness 99 8 Full model (3D) Graphical Animation Linearization Control and Simulation Results Conclusion and Outlook 19 Bibliography 113 xiii

14

15 List of Figures 2.1 Rotor Configurations [Araar and Aouf, 214] Reference Frames for the Planar Case Trapezoidal Velocity Profile Synchronized Profiles Wedge Profile, v Max = Time-Optimal Velocity Profiles with non-zero Initial / Final Values Effect of insufficient Sign Allocation Procedure (right) Possible Velocity Profiles Procedure of Identifying and Calculating the different Profiles Reducing both: Coasting Velocity and Acceleration Superordinate Structure of Generator Gantry Crane [Singh and Singhose, 22] Shaped Inputs [Singh and Singhose, 22] Modified Control Outputs Structure of Cascaded PD Control Root Locus - Altitude Controller Root locus - Translational Control Approach Step Response of Outer Loop - Approach Trajectory Tracking of PD Control Scheme LQ Regulator LQ non-zero Set Point Controller Step Response of LQ Set Point Controller Step Response of adjusted LQ Set Point Controller Trajectory Tracking of LQ Set Point Controller LQ Servo Controller Trajectory Tracking of LQ Servo Controller xv

16 List of Figures 5.13 LQ Set Point Controller with Pre-Calculated Offset LQ Servo Controller with Pre-Calculated Offset Smooth Trajectory Trajectory Tracking of backstepping Controller System Poses (Notable Overswing) Trjactory Tracking in the Presence of 2D Attitude Dynamics Trajectory Tracking of feedback-linearized System Simplified Depiction Full Perspective Tracking in x and y xvi

17 List of Tables 2.1 Identified Parameters

18

19 1 Introduction For more or less a decade, micro aerial vehicles, often in form of an autonomously flying drones, have been receiving ever-increasing attention. This is due to many factors, among them their ability to perform vertical take-offs and landings (VTOL) as well as hovering in mid-air. Already then, [Bouabdallah et al., 24a] noted that recent developments in high density power storage, integrated miniature actuators and MEMS (Micro Electromechnical Systems) have made autonomous miniaturized flying robots possible. Great advances in the utilized equipment have been made since, and nowadays sufficiently accurate technology is affordable and diversely deployable. Hence, VTOL s, often in form of quadrotors, find frequent use not only in academic circles. In many scenarios it may be of interest to suspend a load below the vehicle. This can be motivated by the mere intent of transporting a high-priority cargo from A to B in a quick and safe manner, relieving traffic volume from the street to a widely unused flight zone. The vehicle is then referred to as a delivery drone, which have been news item recently and begin to find establishment in everyday life. In case of a cargo transport, one might think about considering the load as a parameter uncertainty or an external disturbance, and derive a sufficiently robust control, as the important requirement in this task is to ensure stability [Bisgaard et al., 21]. In the latter work, an adaptive control scheme with a slung load estimator that allows swing-stable transport has been developed. Beyond this, assistance in rescue and exploratory missions, also in contaminated and hazardous surroundings, the distribution of pesticides, environmental monitoring or cooperative manipulation [Palunko et al., 212] come to mind. Often, these applications bear the necessity of positioning the payload more or less rigorously, wherefore robust approaches without explicit consideration of the load dynamics, or even position, cease to suffice. In outdoor applications this is without question largely complicated by environmental factors like wind gusts, which are not predictable and, thus, neither fully compensable. In indoor and close to ground operations, trajectory tracking capacities are of prevailing significance. Due to its indirect actuation and free swing effects, rigorous positioning of the payload is a sophisticated task. This may be accomplished by means of differential flatness [Sreenath et al., 213a,b] or 1

20 1 Introduction model-based trajectory optimization [Tang and Kumar, 215]. We pursue the objective of accomplishing this task - notably, not in the case of aggressive maneuvers - with simpler and widely established means. Herein lies predominant motivation for the project itself, and in the following sections a variety of control approaches is to be evaluated with regard to performance as well as real-life applicability. 2

21 2 Theoretical Background The most distinctive feature of any quadrotor is given by its four pairwise counteracting rotors. This configuration allows the vehicle to carry out advanced aerial maneuvers, while furthermore putting an end to the necessity of providing a tail rotor. Torque is induced by variation of the respective rotor speeds, as pictured in Figure 2.1. In spite of these seemingly numerous configurations, a quadrotor is always an under-actuated system, as it has four actuators facing six degrees of freedom (DOF) a rigid body exhibits in Euclidean space. Figure 2.1: Rotor Configurations [Araar and Aouf, 214] 3

22 2 Theoretical Background By introducing a point mass, suspended with a taut cable, two more DOF s are added. In total, this yields a system with four degrees of under-actuation, wherefore thought must be put to which DOF s are to be controlled. Forces (and ultimately moments) exerted by the rotors may be approximated from the respective rotor speeds Ω i : f i = κ th Ω 2 i, (2.1) Here, κ th denotes the thrust factor. Roll and pitch torque are induced by rotor displacement from center of gravity a. Lastly, the yaw torque is generated by the difference in counter torque between the two pairs of rotors, in dependence on the drag factor κ dr. In the work at hand, attitude as well as trajectory tracking controllers are designed with respect to the virtual control output [f, τ φ, τ θ, τ ψ ] T, which reads total thrust, roll-, pitchand yaw-torque, in this order. Thereby, model as well as control derivations are facilitated significantly. The transformed outputs are related to the actual output in the following way: which, if brought to matrix form, may be rewritten as f = f 1 + f 2 + f 3 + f 4 (2.2) τ x = τ φ = a (f 2 f 4 ) (2.3) τ y = τ θ = a (f 1 f 3 ) (2.4) τ z = τ ψ = κ dr κ th (f 1 f 2 + f 3 f 4 ) (2.5) f f τ φ f = M 2 a a, M =. (2.6) τ θ f 3 a a κ τ ψ f dr 4 κ th κ dr κ dr κ th κ th κ dr κ th This is a regular matrix, and the inverse reads M 1 = a a a 1 4 2a κ th 4 κ dr κ th κ dr 1 κ th 4 κ dr κ th κ dr, (2.7) wherefore transforming between the two depictions is unproblematic. Thrust and drag 4

23 2.1 Assumptions and Parameters coefficients have also been determined in a previous work and are listed in Table Assumptions and Parameters As the focus of this work lies in the first assessment of control strategies for the underactuated system, the model is chosen with a suitable level of complexity. This comes at cost in accuracy, but the subsequently described simplifying assumptions should allow a realistic evaluation of various control approaches. As already stated, the quadrotor itself is considered a rigid body. In addition to the geometry specifications this finds representation in body mass m B as well as body tensor of inertia I B. The latter has been determined in a previous work and is formulated with respect to the quadrotor s principal axis, reducing it to I xx I B = I yy. (2.8) I zz In the calculation of air resistance coefficients, the body is assumed to be a sphere in a first draft. These coefficients are therefore identical for motion in all directions. It has also been part of a previous work, and the rough estimates are given in Table 2.1. In absence of direct actuation and enhanced by the free suspension effect, the load is subject to moderate angular accelerations, wherefore its representation as a point mass is a well-founded assumption. This is further supported by the fact that its moment of inertia with respect to the inherent main-axis is negligible in comparison to the acting inertia when rotated around the suspension point. Ultimately, with Steiner s Theorem, load inertia accounts to I L = m L l 2. Beyond this, the equations of motion are derived under the assumption that body center of gravity (COG) and suspension joint coincide; furthermore, that the rope is thin, nonelastic and taut at all times. Therefore, it is not provided with quantities such as mass, drag coefficients or spring constants. This is a commonly chosen simplification [Palunko et al., 212], but has been criticized and revised by [Bernard and Kondak, 29], as the rope may start to oscillate itself. However, in the aforementioned work, significantly larger weights featuring ( kg) have been transported, so it is likely an apt approach to forgo these effects in the work at hand. 5

24 2 Theoretical Background A detailed model of the electrically driven rotors with respect to mechanical and aerodynamic properties, as roughly described in Section 2, is foregone. As motor reaction times are small in comparison to the quadrotor system as a whole, this is a proper simplification [Hehn and D Andrea, 211a]. However, as actuator saturation causes the risk of yielding the actuators not responsive, their output is capped by introducing a saturation at maximum propulsion. In general, the gained facilitation in deriving control algorithms likely accounts for the loss in accuracy and, naturally, the model can be expanded by rotor dynamics as well as explicit models of the electric motors at any later point. The total of four rotors is assumed to exert f max = 2 (m B + m L ) g, (2.9) if all motors run at full speed, which is very close to the measured values. From f max, the maximum thrust of one rotor can be calculated: f i,max = 1 4 f max = 1 2 (m B + m L ) g. (2.1) m B 2.24 kg Mass of Quadrotor m L.118 kg Mass of suspended Load a.332 m Displacement of Rotors from COG I xx I yy.363 kg m kg m 2 Body Moment of Inertia - x Body Moment of Inertia - y I zz.615 kg m 2 Body Moment of Inertia - z l.62 m Length of suspension Cable κ th Thrust Coefficient κ dr Drag Coefficient b.11 Nm rad s c B 1.2 N (m/s) 2 c L.3 N (m/s) 2 Friction Coefficient in Joint Drag of Body - unidirectional Drag of Load - unidirectional Table 2.1: Identified Parameters 6

25 2.2 Deriving the Equations of Motion The resultant torques for pitch and roll are obtained easily and account to τ θ,max = τ φ,max = a f i,max, (2.11) whereas the yawing-moment is significantly smaller with τ ψ,max = 2 κdr κ th f i,max. (2.12) 2.2 Deriving the Equations of Motion The following section offers background on kinetics as well as kinematics of the multibody system. The derived model is complex, and it will be reduced accordingly for the subsequent control design. 2.3 Kinematics In order to properly describe position and pose of quad-copter and load, three frames are provided. The inertial- or world frame will be depicted with {W}. It defines the directions of the unit vectors and moreover the inertial origin. In Figure 2.2 the simplified planar case ([ˆx W, ẑ W ]) is depicted. Velocities are assumed to be slow enough to neglect Coriolis-effects caused by the earth s rotation, a common and reasonable simplification [Palunko et al., 212]. The position of the vehicle, more precisely that of its center of gravity, expressed in inertial coordinates, is given by a translation along W p B ; the load, analogously, by W p L. The vehicle s COG itself denotes the origin of the body-fixed frame {B}. The latter is a moving reference frame whose axes always parallel the body s principal axes. It accounts for the instantaneous vehicle orientation - relative to the inertial frame. The definition with respect to the principal axes is helpful in the formulation of the equations of motion, particularly in case of momentum balances, as will be seen shortly. Furthermore, all gyroscopic measurements provide data in body-fixed coordinates, wherefore means of transforming between {B} and {W} must be found in any case. Lastly, the load-frame {L} is introduced. Its origin is placed in the COG of the load itself and the ẑ L -axis is aligned such that it always points toward the suspension joint. 7

26 2 Theoretical Background a Figure 2.2: Reference Frames for the Planar Case Quadrotor Kinematics A common approach for the definition of vehicle orientation in aerospace applications is given by the Yaw-Pitch-Roll, or Tait-Bryan formalism. It denotes a sequence of rotations on SO (3), utilizing Euler angles, however, about all three axes. It is, therefore, a slightly alternated form of the Euler-formalism, and in case of intrinsic rotations about the current axes, the sequence reads z y x. In general, a rotation about the z-axis that transforms a vector from the originating to the new frame is given by cos (ψ) sin (ψ) R z (ψ) = R ψ = sin (ψ) cos (ψ). (2.13) 1 An equally directed rotation about the y-axis is achieved through cos (θ) sin (θ) R y (θ) = R θ = 1, (2.14) sin (θ) cos (θ) 8

27 2.3 Kinematics and in case of rotations about x, the matrix accounts to 1 R x (φ) = R φ = cos (φ) sin (φ). (2.15) sin (φ) cos (φ) The total rotation resulting from the series therefore reads B WR = R φ R θ R ψ = cψ cθ cθ sψ sθ cψ sφ sθ cφ sψ cφ cψ + sφ sψ sθ cθ sφ sφ sψ + cφ cψ sθ cφ sψ sθ cψ sφ cφ cθ (2.16) and denotes the total rotation of a vector from the world- to the body-fixed frame. For clarity reasons c ˆ= cos, s ˆ= sin. The reverse operation can be carried out by inverting the matrix, or, as rotational matrices are orthogonal, by reversing the sequence of transposed matrices: Therefore, the vehicle pose is unambiguously defined by W B R = R ψr θr φ. (2.17) φ ζ = θ. (2.18) ψ This transformation is applicable to thrust force and translational motion components. In case of the rotational sub-system, circumstances are less obvious, as the chosen inertial angular velocities and accelerations are defined in their respective current frames. This must be considered, and the body-fixed angular rates therefore result from a summation of non-equally transformed inertial rates: p B ω B := ω = q = B φr φ + B θ R θ + B ψr ψ. (2.19) r 9

28 2 Theoretical Background Intrinsic inertial rotations are expressed separately in their current frames, wherefore we can rewrite the above as: φ ω = B φr + B θ R θ + B ψr = ψ (2.2) 1 φ B φr + B θ R 1 + B ψr θ 1 ψ Obviously, all line-by-line particular transformations can be united in a single matrix - henceforth and commonly denoted with W. Transforming from inertial- to body-fixed coordinates, the formulation is relatively straightforward. The reciprocal operation, however, is much less intuitive. As ω is a summation of all inertial angular velocities, their representative quanta in the body-fixed frame would have to be identified, and transformed separately into the current frames. Luckily, this can be foregone by inverting the resulting overall transformation matrix W. In deriving the latter, it is advisable to start with the interdependence between p and φ. The rotation about φ with respect to the x -axis correlates the body-fixed frame with the last of the intermediate frames. As φ is directed along x, it is intuitive to state from understanding that p = φ, (2.21) which is easily proven with (2.2) in mind, as B φr = R φ = 1 cos (φ) sin (φ) sin (φ) cos (φ) (2.22) and R φ (1, 1) = 1. (2.23) If this is carried out analogously for the remaining two inertial angular velocities, B θ R = R φ R θ (2.24) B ψr = R φ R θ R ψ, (2.25) 1

29 2.3 Kinematics one ultimately obtains 1 sin (θ) W = cos (φ) sin (φ) cos (θ), (2.26) sin (φ) cos (φ) cos (θ) which allows to transform between inertial and body-fixed rotational coordinates as follows: The reverse operation requires ω = W ζ. (2.27) 1 sin (φ) tan (θ) cos (φ) tan (θ) W 1 = cos (φ) sin (φ). (2.28) sin(φ) cos(θ) cos(φ) cos(θ) Notably, as det (W ) = cos (θ), pitch angles will have be lower than 9 degrees at all times. This, however, depicts an extreme scenario, wherefore it is not problematic with the present ambitions in mind. The link between inertial and body-fixed angular accelerations is obtained by deriving (2.27) with respect to time, yielding ω = Ẇ ζ + W ζ. (2.29) Load Kinematics As mentioned above, {L} is chosen to be located in the loads COG, and accordingly, rotated around the cable suspension. It is aligned in such a way, that the translation from load to body, inside the load-frame itself, is given by a simple translation along the instantaneous ẑ L -axis: L p B = (2.3) l The assumed sequence of rotations from {W} to {L} is, intrinsically expressed, y x, and the position of the load in the inertial frame and relative to the vehicle accounts to W p L = W p B + W L R ( L p B ), (2.31) 11

30 2 Theoretical Background where W L R = L WR 1 = (R x (φ L ) R y (θ L )) 1. (2.32) Expressing the vehicle s position with respect to the load is accomplished by bringing the second term of (2.31) to the other side. Thereby, one may easily choose to express the system in [x B, y B, z B ] T, or, if desired, featuring [x L, y L, z L ] T. Velocities as well as accelerations are obtained without problems by deriving the respective components. 2.4 Rigid Body Dynamics In order to develop a dynamic model of the copter-load system, the equations of motion are to be derived according to both, Newton-Euler as well as Lagrange-Euler formalisms. The Newton-Euler approach, being the less abstract of both, combines Newton s second law for describing translational - with Euler s equation for rotational motion of a rigid body. Assuming a body-fixed frame that origins in its center of mass and whose axes align with the principal axes, the equations of motion can be expressed as m I J v ω + ω mv ω Jω = f τ, (2.33) where J denotes the rigid body s tensor of inertia and I 3 3 a three-dimensional identity matrix. v is the translational velocity vector. f and τ are the externally induced forces and torques that are assumed to act directly on the COG. In case of a multi-body system, as present, these equations must be set up for each component, taking the inter-body forces and torques into account. For redundancy reasons, the multi-body systems is to be described by means of the Lagrange-Euler formalism - a good overview of which is found in [Sagatun and Fossen, 1991]. The Lagrangian of a system is given by L = K P, (2.34) where K denotes the total kinetic energy, and P a referenced potential energy. These energies must be set up utilizing a suitable set of generalized coordinates q, with f i (q 1,..., q n, t) =, i = 1,..., k, (2.35) 12

31 2.4 Rigid Body Dynamics namely, coordinates that are independent of each other. For a conservative system, the Lagrangian equations of the second kind read = d dt ( ) L q i L q i. (2.36) To a certain extent, they are applicable on non-conservative systems. A vector of generalized forces, usually denoted with Q, may be introduced. Its entries are defined as Q i = j f j r j q i, (2.37) and represents a projection of the induced forces f j into the configuration space spanned by the chosen generalized coordinates. The introduction of a dissipation function V, with Q fr i = V q i allows the inclusion of velocity-dependent dissipation, such as air resistance or friction terms. With generalized forces and a dissipation function, Lagrangian equations of the second kind can be set up for the system of interest as Q i = d ( ) L L + V, (2.38) dt q i q i q i thereby furthermore yielding equations of motion of a form that shows certain advantages in control design tasks (c.f. Section 6.2) Dynamic Model: Newton - Euler Formalism It is convenient to derive the translational EQM s by formulating the equilibria of forces in the inertial instead of the body-fixed frame, thereby avoiding the necessity of including fictional forces. This is carried out for vehicle as well as load and, for the former, gives m B p B = f + m B g + t L f r, (2.39) with W p B := p B for readability reasons - this notational simplification will be applied to all vectors expressed in the world-frame. The induced forces represent thrust, gravitational effects, the tension force in the suspension, and lastly an optional air resistance term - in 13

32 2 Theoretical Background this order, and expressed in the inertial frame. In case of the load, which is not directly actuated, the according term is dropped, yielding m L p L = m L g + t B f r. (2.4) The notation is chosen in the same simplified manner from above. These equations are linked via the tension force. By elimination, three translational equations of motion are obtained. However, as thrust is expressed in the body-fixed frame, it must be transformed accordingly f = W B R. (2.41) f The inclusion of linear friction is trivial, and air resistance in form of quadratic drag accounts to f r,i = v i v i c j,i, (2.42) for each translational velocity component i, body j, and associated air resistance coefficient c j,i. Transformations can be forgone under the assumption of a sphere, which can be extended to better accuracy at any later point in time. The formulation of the rotational equations of motion must be performed with care. If they were to be set up in the inertial frame, this would require the utilization of a non-diagonal inertia tensor as well as accordingly transformed torques. Therefore, it is advisable to utilize the body-fixed frame in the formulation (2.33) and transform the obtained angular accelerations subsequently, according to (2.29). The load is considered by formulating a momentum balance around the suspension, which, expressed in the world frame, accounts to m L W L R p L = m L l W L R g, (2.43) l denoting the cross product of vectors. Thereby, the set of equations is complete and may be solved for the respective accelerations with regard to a formulation in either load or vehicle coordinates. 14

33 2.4 Rigid Body Dynamics Dynamic Model: Euler - Lagrange Formalism With the kinematic relations at hand, the Lagrangian equations can be set up with comparably little effort. Translational kinetic energy K trans = 1 2 vt mv (2.44) must be set up for copter and load. Special attention must be paid to the formulation of the rotational energy term of the copter, which accounts to K rot = ζ T W T I B W ζ. (2.45) Expanded, this reads K rot =I xx ( φ ψ sin (θ) ) 2 + Iyy ( θ cos (φ) ψ sin (φ) cos (θ) ) 2 + I zz ( θ sin (φ) ψ cos (φ) cos (θ) ) 2, (2.46) which can be confirmed from [Bouabdallah, 27]. Potential energy in relation to the inertial system is given by P = mgz, (2.47) and, analogous to the above, has to be set up for copter as well as load. Up to this point, developing the Lagrangian system is certainly as straightforward as it is the case in the Newton derivation. The constitutive equations are hereby given, and by utilization of the already established kinematic relations, the un-actuated model is complete. However, regard must be paid to the formulation of the generalized forces and moments as stated in (2.37). This denotes a crucial part in the derivation of the Lagrangian equations, as these expressions change when a different set of generalized coordinates is chosen. In any scenario at hand, this is straightforward for the generalized forces - one merely needs to select the respective element of f. It is, however, less obvious in case of the generalized moments, as they must be transformed into their respective sub-frames. Therefore, in case of the attitude-dynamics, neither matrix W nor its inverse are applicable. The generalized moment in the φ-sub-frame, resulting from the actual moments expressed in the body-frame requires the transformation Rφ 1, from (2.22). Analogously, the θ-subspace requires the calculation of Rθ 1 R 1 φ, and in case of ψ one needs R 1 ψ R 1 θ R 1 φ = W B R. Subsequently, the respective entries must be selected, and ultimately the generalized 15

34 2 Theoretical Background moments may be obtained: Q φ 1 Q θ = R 1 φ + 1 R 1 θ R 1 φ Q ψ τ φ + R 1 ψ R 1 θ R 1 φ τ θ 1 τ ψ 1 τ φ = cos (φ) sin (φ) τ θ sin (θ) cos (θ) sin (φ) cos (θ) cos (φ) τ ψ (2.48) A similar procedure is of need, if the generalized moments acting in the load-sub-frames are to be determined. Again, r, from (2.37), denotes the location of the exerted force or moment. We can take from (2.32) that r = f (p L, φ L, θ L ) if a set of generalized coordinates with [x L, y L, z L ] T is chosen. However, if a set of generalized coordinates featuring [x B, y B, z B ] T is elected, this yields r = f (p B ) = p B. As we derive with respect to φ L or θ L, the latter results in zero-entries at the respective positions of the generalized force vector. Therefore, only the second of the two sets of coordinates is examined in detail, at least in case of the full 3D system. This is subsequently checked against the derivation by means of Newton-Euler to ascertain that both are flawless. As stated in Section 2.3.2, an expression in [x L, y L, z L ] T then merely requires an inversion of the kinematics in case of the Newton-Euler approach, as the constitutive equations remain unchanged. The desired set of equations is thereby obtained with less effort and nevertheless certain to be correct. This procedure will be presented in more detail in Chapter 8. Non-conservative velocity dependent dissipative forces are calculated making use of dissipation functions. These account to V 1 = 1 2 b i w rel 2 (2.49) in the case of linear (here: angular)- and V 2 = 1 3 c i,j v rel 3 (2.5) 16

35 2.5 Linearization in case of quadratic resistance (c.f. (2.42)). Ultimately, with these tools at hand, the non-linear model of the copter-load system can be developed. 2.5 Linearization In order to deploy linear control strategies such as pole-placement or linear quadratic regulation, a linear time-invariant (LTI) model of the system is required. This will happen at hand of the previously derived non-linear equations. In general, linearization in the proximity of an operating point [ x 1... x n ] is achieved by means of a Taylor-series, which is discontinued after the linear term. In the following, x δ denotes deviations from the stationary point x i = x i + x δ i. (2.51) Then, for an arbitrary non-linear function h, the Taylor-series is given by h (x 1...x n ) h( x 1... x n ) + i=n i=1 h ) ( xi + x δ i x i, (2.52) x i x1... x n Linearized State-Space Expression A linear state space representation of the model bears many advantages with regard to system analysis in general, and is obligatory in the development of any LQ regulator. A non-linear model of the form dx dt = f(x, u) (2.53) is required. Here, u denotes a vector containing the manipulated variables, the vector y = g (x, u) (2.54) measured or observed output. Deviations from the operating point are depicted as follows x = x + x δ, u = ū + u δ, y = ȳ + y δ, (2.55) 17

36 2 Theoretical Background and the Taylor-series is formulated. Here, the derivatives are replaced by Jacobian matrices of the vectorial functions, giving x + ẋ δ f( x, ū) + f x δ + f u δ, (2.56) x u x,ū x,ū and ȳ + ẏ δ g( x, ū) + g x δ + g u δ. (2.57) x u x,ū x,ū If the locations of world and load frame are selected to coincide, 2.56 is reduced to ẋ δ f x δ + f u δ, (2.58) x u x,ū x,ū and under the assumption that ȳ g( x, ū), Equation 2.57 becomes ẏ δ g x δ + g u δ. (2.59) x u x,ū x,ū Consecutively, the Jacobian matrices A = f x x,ū, B = f u x,ū, C = g x x,ū, D = g (2.6) u x,ū are introduced. In a breach of notation δ is omitted, and as of now, x, u and y describe the vicinity around the stationary point in the linearized model. Thus, equations (2.58) and (2.59) may be rewritten as ẋ (t) = Ax (t) + Bu (t) (2.61) and y (t) = Cx (t) + Du (t), (2.62) the common way of depiction for a LTI system. 18

37 3 Trajectory Generation In general, a trajectory generation problem arises when any automatic machine is to be subjected to a desired change in state. In many cases, such as autonomous flight, a change in position is denoted. Any controlled motion requires the generation of a feasible profile with respect to the physical restrictions, e.g. actuator limitations; insight that is gained from knowledge of the system dynamics. Ultimately, this allows generating a path characterized by a correlation between time and position at any given point on the former. Various approaches to trajectory generation exist, and one distinguishes between trajectories in the operational space, which, in this case, are given by the position of the vehicle in time and space, and trajectories in the actuator- or joint space. The latter represents the time-evolution of the joint or actuator states that may have to be calculated from the operational space trajectory in accordance with the chosen controller structure, so that the desired motion is achieved. However, as all controllers designed later on are fed with load- (or body) position coordinates, this issue is not of concern in the ongoing work. A functional way of defining a trajectory is specifying a certain amount of intermediate points on a path and finding some means of filling the gaps. Often this is achieved by interpolation, utilizing a polynomial of adequate order to satisfy preset boundary (or even intermediate) conditions. Low-order polynomials naturally reduce the achievable performance, whereas more complex profiles allow defining a variety of constraints. State of the art tools for efficient global optimization even allow real-time generation of complex trajectories. Regarding quadrotors, the reader is referred to [Mellinger and Kumar, 211], featuring a flatness-based approach for the generation of minimum-snap trajectories for obstacle-inhibited indoor flight, or to [Hehn and D Andrea, 211b], achieving extreme aerial maneuvers with close to model-predictive performance by calculating optimal jerklimited trajectories. Differential flatness for the quadrotor-load system with a set of generalized coordinates featuring body- or load position has been shown in [Sreenath et al., 213b], and feed-forward inputs have been calculated successfully even for loadposition controlled flight. In the course of this project, however, simpler means will be applied. This is justified by the fact that much focus is put upon linear control strategies. 19

38 3 Trajectory Generation Therefore, the controllers will not be able to cope with necessary large deviations from hover, which are to be expected as a result of any aggressive trajectory. Furthermore, it is again interesting to determine if the comparably sophisticated task of controlling the position of a suspended load may be successfully mastered with established and simple means. 3.1 Trapezoidal Velocity Profiles Trajectories utilizing trapezoidal velocity profiles are applied in many areas of automatic control. They are fairly easy to generate using analytical means and offer a sufficient amount of adjustment options. A characteristic feature is the linear velocity constraint that yields constant accelerations and, ultimately, a second order polynomial for the position, following p (t) = p + v t at2. (3.1) To achieve the trapezoidal form, the entire stretch between any initial and final position is divided into three segments. Motion is initialized by a phase of constant acceleration which has to be chosen in accordance with the actuator limitations. As soon as the desired velocity is reached, the cruising or coasting phase begins. This section denotes movement with a constant velocity, which is perceived as a natural way being in motion. The process is concluded with a deceleration phase, and ultimately a profile as pictured in Figure 3.1 is obtained. It is notable, that the inconsistencies encountered in the time-evolution of all acceleration profiles theoretically yield infinite jerk. Unlike the constant motion bit, this may be perceived unnatural by humans and is somewhat unfavorable for actuators and equipment. However, as all exerted forces are comparably small and slack will be induced due to the pure feedback-structure of the controllers, this should not be considered a cause of problems. 2

39 3.1 Trapezoidal Velocity Profiles Trapezoidal Trajectory 4 Position [m] Velocity [m/s] Acceleration [m/s 2 ] Time [s] Figure 3.1: Trapezoidal Velocity Profile One- vs. Multi-Dimensional Trajectories Fed a desired destination (p 3 ), the trajectory generator is to calculate a time-optimal profile subject to the velocity and acceleration constraints defined by the user. In the case of a one-dimensional trajectory this is straightforward. If the action is exerted in more than one dimension, however, some more thought should be put to the arising circumstances. In case of time-optimal trajectories only, the object would normally come to a halt in some dimension(s), before eventually reaching the desired final position. This is, again, unnatural. More importantly, as the workload of the commands is distributed among the same actuators, it is also prone to creating problems caused by saturating actuators in the acceleration phase. Therefore, a different approach is proposed, and the end-times are synchronized (Figure 3.2). To achieve this, time optimal profiles are calculated for each of the DOF s, and the largest value is used as a reference for the remainder. 21

40 3 Trajectory Generation Synchronized Trajectories 4 Position [m] Velocity [m/s] Acceleration [m/s 2 ] Time [s] Figure 3.2: Synchronized Profiles Trapezoidal Profiles with zero Initial / Final Velocities Constraining the initial and final velocities to zero exhibits great shortcomings when a path with one or more intermediate points is defined, as it is not possible to create a continuous motion. This effect can be attenuated by planning and introducing a new profile before motion comes to a halt [Biagiotti and Melchiorri, 28, p:67], but this procedure is somewhat imprecise and better solutions are possible. Therefore, in this section, the implemented procedure is presented in a simplified manner in order to facilitate understanding the problems of greater complexity that are presented subsequently. Time-Optimal Profiles In general - as cruising velocity (v c ) and acceleration (a) are predefined - the necessary calculations are reduced to determining the switching times t 1 and t 2 that define the 22

41 3.1 Trapezoidal Velocity Profiles acceleration and deceleration phases. However, one must account for two special cases. Firstly, the case of backward-motion; secondly, a distance that is not sufficiently extensive to allow reaching and maintaining maximum velocity. Assuming that none of the latter is the case, the standard calculations may be carried out. Defining the acceleration time as T a = v c a, (3.2) (by definition, the input values of v c and a are positive at this point) the significant times may be calculated easily: t 1 = t + T a t 2 = t 1 + p 3 p at 2 a v c t 3 = t 2 + T a Hereafter, the respective polynomial expression for the position can be formulated: p(t) = p a Acc (t t ) 2, t t < t 1 p 1 + v c (t t 1 ), t 1 t < t2 (3.3) p 2 + v c (t t 2 ) a Dec (t t 2 ) 2, t 2 t < t 3 Due to the simple expressions and the integrative connection between acceleration, velocity and position, only the latter is presented in detail. Heading backward is easily included by providing an a priori sign testing procedure d = sign (p 3 p ), d {1,, 1} (3.4) directions of cruising, acceleration as well as deceleration are then set as v c = d v c (3.5) a Acc = d a (3.6) a Dec = a Acc (3.7) In case t 2 t 1 at 2 a p 3 p, (3.8) 23

42 3 Trajectory Generation there is not enough room to establish a coasting motion, in which case a wedge profile (Figure 3.3) is generated..5 Position [m] Velocity [m/s] Acceleration [m/s 2 ] Time [s] Figure 3.3: Wedge Profile, v Max = 1 The peak velocity can be calculated analytically to v w = a p 3 p, (3.9) and the trajectory may then be calculated similar to the trapezoidal one, the differences 24

43 3.1 Trapezoidal Velocity Profiles being that t 1 and t 2 correspond and that v c is being replaced by v w. Preassigned Durations Introducing a given end-time, which was the degree of freedom in the calculation of timeoptimal profiles, creates the need of relaxing the velocity constraint, the acceleration constraint, or both of the former. A reduced coasting velocity can, for instance, be achieved by shortening acceleration as well as deceleration time by the same amount. In case of null initial and final velocities, an analytical solution is found by calculating this time-interval = A 2 + A 2 4, (3.1) which is taken from [Haschke et al., 28] and slightly alternated, with A = T p 3 (p + (p 1 p ) + (p 3 p 2 )) v c (3.11) and T = t 3,New t. (3.12) The new switching times are then obtained easily as t 1 = t 1, t 2 = T ((t 3 t 2 ) ), t 3 = T, (3.13) and by substituting them into the set of equations given in (3.3), the adjusted profile is completed Trapezoidal Profiles with non-zero Initial / Final Velocities As mentioned above, it is necessary to allow for arbitrary velocity specifications on the section bounds if continuous path motion is to be achieved. Hereby, the task of generating time-optimal as well as time-constrained trajectories is complicated significantly, particularly in case of the latter, as will be seen shortly. 25

44 3 Trajectory Generation Time-Optimal Profiles The first major deviation from the procedure presented above lies in the fact that trapezoidal and wedge profiles are not sufficient for achieving time-optimality with all constellations of initial / final velocities and positions. Therefore, a profile that features an acceleration or deceleration phase only, must be introduced. The resulting velocity profile is a line connecting v and v 3 (Figure 3.4), which, however, may be followed by a cruising phase. Figure 3.4: Time-Optimal Velocity Profiles with non-zero Initial / Final Values Furthermore, caution must be exercised when identifying the signs of constant velocities and accelerations. The assignment procedure from section must be replaced by a more extensive one: Firstly, the minimally required time for a transition from initial to final velocity is calculated as t Min = v 3 v a, (3.14) which is then succeeded by identifying the corresponding travel distance: The signs are then identified according to p Min = t Min v + v 3 2. (3.15) d = sign (p 3 p Min ), (3.16) 26

45 3.1 Trapezoidal Velocity Profiles which is also taken partly from [Haschke et al., 28] and extended in such a way that it may take commanded final velocities into account. Having a closer look at the physical context, it becomes apparent that not only the displacement defines the direction of cruising, but rather the entirety of velocities and positions. The generator as designed even incorporates demands such as positive displacements that end with negative final velocities, or displacements that are not reachable without a period of translation in the opposite direction, as pictured in Figure New sign Allocation.4 Classical sign Allocation Position [m] Velocity [m/s] Acceleration [m/s 2 ] Time [s] Time [s] Figure 3.5: Effect of insufficient Sign Allocation Procedure (right) 27

46 3 Trajectory Generation Furthermore, another practical side benefit is exerted: Any scenario with p Min = p 3 will feature the line-profile. In this - and only in this - case, d will turn zero - an eventuality that may be identified easily with a simple if-construct, thereby putting an end the necessity of any further, perhaps sophisticated, calculations. The interpolation is trivial, however, one aspect has to be noted: The need to identify the sign remains, and in this case, one must use an implicit formulation setting a := v 3 v T Min. The scenarios featuring trapezoidal as well as wedge profiles remain unchanged, and the procedure is similar to the above. Firstly, the characteristic points in time and position are calculated: t1 = v c v a t3 = v 3 v c a (3.17) (3.18) p1 = v + v c t1 (3.19) 2 p3 = v 3 + v c t3 (3.2) 2 Again, the calculation of feasible time-spans is guaranteed as a is positive by definition. For a proper computation of any p, the sign-allocation to v c must be carried out in advance, and subsequently one obtains: t 1 = t + t1 (3.21) p 1 = p + p1 (3.22) p 2 = p 3 p3 (3.23) At this point, the check for a straight-line profile has been carried out inevitably and the remaining cause to be identified is whether to generate a wedge- or a trapezoidal profile. This is achieved by calculating the duration of the cruising period, which, with the previously obtained data, accounts to t2 = p 3 (p + p1 + p3 ) v c. (3.24) In case the calculation yields a negative time-span, coasting velocity cannot be reached and a wedge-profile must be calculated. For the case of an existing coasting segment, 28