Dynamics and Control of a Quadrotor with Active Geometric ...

Dynamics and Control of a Quadrotor with Active Geometric

Morphing

Dustin A. Wallace

A thesissubmitted in partial fulfillment of the

requirements for the degree of

Master of Science in Aeronautics & Astronautics

University of Washington

2016

Reading Committee:

Kristi Morgansen, Chair

Christopher Lum

Program Authorized to Offer Degree:Aeronautics & Astronautics

c©Copyright 2016

Dustin A. Wallace

University of Washington

Abstract

Dynamics and Control of a Quadrotor with Active Geometric Morphing

Dustin A. Wallace

Chair of the Supervisory Committee:Professor Kristi MorgansenAeronautics & Astronautics

Quadrotors are manufactured in a wide variety of shapes, sizes, and performance levels to

fulfill a multitude of roles. Robodub Inc. has patented a morphing quadrotor which will

allow active reconfiguration between various shapes for performance optimization across a

wider spectrum of roles. The dynamics of the system are studied and modeled using New-

tonian Mechanics. Controls are developed and simulated using both Linear Quadratic and

Numerical Nonlinear Optimal control for a symmetric simplificiation of the system dynam-

ics. Various unique vehicle capabilities are investigated, including novel single-throttle flight

control using symmetric geometric morphing, as well as recovery from motor loss by re-

configuring into a trirotor configuration. The system dynamics were found to be complex

and highly nonlinear. All attempted control strategies resulted in controllability, suggest-

ing further research into each may lead to multiple viable control strategies for a physical

prototype.

TABLE OF CONTENTS

Page

List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii

Chapter 1: Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Outline of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.3 Contributions of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

Chapter 2: Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.1 Model Description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 Generalized Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3 System Mechanics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.4 Symmetric Analytic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.5 Selected Numerical Values . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

Chapter 3: LQR Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.1 Full System Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.2 LQR After Motor Loss Recovery . . . . . . . . . . . . . . . . . . . . . . . . . 27

Chapter 4: LQR Gain Scheduling . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

Chapter 5: Numerical Optimal Control . . . . . . . . . . . . . . . . . . . . . . . . 40

5.1 Code Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.2 1D Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

5.3 2D Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.4 3D Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

Chapter 6: Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

6.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

i

6.2 Recommendations for Future Work . . . . . . . . . . . . . . . . . . . . . . . 57

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

Appendix A: Full Form Dynamic Equations . . . . . . . . . . . . . . . . . . . . . . . 63

A.1 ¨xCG . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

A.2 ¨yCG . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

A.3 ¨zCG . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

A.4 x . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

A.5 y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

A.6 z . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

A.7 φ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

A.8 θ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

A.9 ψ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

ii

LIST OF FIGURES

Figure Number Page

1.1 Basic Quadrotor Maneuvers . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Robodub Variable Arm Angle Quadrotor . . . . . . . . . . . . . . . . . . . . 7

1.3 Robodub Telescoping Quadrotor Arms . . . . . . . . . . . . . . . . . . . . . 7

2.1 Quadrotor Body Axes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.2 Dimension Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3 Symmetrically Constrained Quadrotor . . . . . . . . . . . . . . . . . . . . . 20

3.1 Linearized LQR State Response . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.2 Nonlinear LQR State Response . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.3 Motor Loss Trirotor Configuration . . . . . . . . . . . . . . . . . . . . . . . . 28

3.4 Linearized Trirotor Response . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.5 Nonlinear Trirotor Response . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.1 2 Set Point 2 Second Control Inputs . . . . . . . . . . . . . . . . . . . . . . . 34

4.2 2 Set Point 2 Second State Trajectories . . . . . . . . . . . . . . . . . . . . . 35






5.1 Nonlinear Numerical Control Optimization Structure . . . . . . . . . . . . . 41

5.2 Optimal Control Found for 3 Second 1D Translation . . . . . . . . . . . . . . 43

5.3 Optimal State Trajectories for 3 Second 1D Translation . . . . . . . . . . . . 43

5.4 1D Translation Trajectory Visualization . . . . . . . . . . . . . . . . . . . . 44

5.5 2D Optimized Control Inputs for 4 Second Case . . . . . . . . . . . . . . . . 45



iii

5.8 2D State Trajectories for 3 Second Case . . . . . . . . . . . . . . . . . . . . 48


5.10 2D Trajectory Visualization for 3 Second Case . . . . . . . . . . . . . . . . . 49







iv

ACKNOWLEDGMENTS

I would first like to thank my advisor, Professor Kristi Morgansen. Her insight and

guidance set me on a path to learn an abundant number of new mathematical techniques

and approaches for analyzing complex dynamic and control problems such as this one. I

would also like to thank Nathan, Jake, and Natalie for their technical assistance and for

talking through and validating my approaches to problems I came across throughout the

process, as well as my mother, father, and sisters for their emotional support. Lastly, I

would like to thank Robodub Inc. for providing the subject of this research. I hope to see

further development on their part which results in a flying prototype in the near future.

v

DEDICATION

To Lillian, for without your love and support, this work would not have been possible.

vi

1

Chapter 1

INTRODUCTION

1.1 Background

Unmanned aerial vehicles (UAVs) have become an industry unto themselves in recent years.

While remotely controlled aircraft have existed in some form or another since the late nine-

teenth century when small-scale blimps were primitively controlled using radio waves created

by a sparking mechanism [1], they tended to have somewhat of a niche appeal throughout

much of the twentieth century. A surge in production and implementation of unmanned

aerial vehicles has been seen in recent years however, as the increasing power and decreasing

cost of electronic computational resources have allowed for the flying and stabilization of

the aircraft to be handled electronically while the operators focus on achieving flight objec-

tives. These objectives cover a wide spectrum of activities from military surveillance and

interdiction, to aerial photography and entertainment.

The U.S. Department of Defense (DOD) has been utilizing drones for multiple decades

as aerial targets, modifying outdated fighter jets with rudimentary control systems to create

extremely realistic test scenarios for new anti-aircraft weapons. During the Cold War, the

DOD recognized the potential benefits of purpose-built unmanned aircraft, including oper-

ator protection, force multiplication, and the removal of pilot-restricted mission limitations

on duration and maneuver intensity. Purpose-built surveillance drones began flying in the

early 1990s, eventually evolving into the modern RQ-1 Predator, which first flew in 1994. By

the late 1990s, armed versions of the Predator, which would eventually lead to the develop-

ment of the more heavily armed MQ-9 Reaper, were flying and providing strike capabilities

for the U.S. military [2]. In recent years, smaller, man-portable fixed-wing drones like the

RQ-11 Raven have entered service, providing over-the-wall type tactical military information

2

to soldiers on the battlefield [3].

While the use of fixed-wing unmanned vehicles has grown, the advent of vertical take

off and landing (VTOL) drones has truly ignited the industry in recent years. Suddenly,

drones could be flown by almost anyone, anywhere. The Los Angeles Times reports as many

as 10% of feature films and television shows now use aerial photography shot by drones [4].

Corporations including Amazon [5], DHL [6], Google [7], and more are all developing the

means to deliver packages using VTOL drones. Hobbyists have even created leagues centered

around racing drones through courses of obstacles while using virtual-reality goggles to view

the flight from the vehicle’s perspective [8].

The most common design used in VTOL drones is the quadrotor, which uses four fixed-

pitch propellers on brushless motors rigidly mounted to a frame to provide full controllability

in 3D space. The basic design requires two motors to spin clockwise and two to spin coun-

terclockwise, allowing all rotors to contribute to lifting the vehicle, while still balancing the

yaw-inducing drag torques on the propellers. Quadrotors are typically flown in two config-

urations: “plus” or “X”, depending on the yaw reference frame of the vehicle, and control

authority is derived by modulating relative propeller thrusts to affect attitude and direct

the combined lift vector in the direction of desired movement. Examples of this relative

modulation are shown in Figure 1.1.

Due to their relative simplicity and ability to be used in small spaces such as a laboratory

setting, quadrotors have been the subject of a great deal of control development and research.

A multitude of control techniques have been applied, both linear and nonlinear, and from

across the fields of both classical and modern control. In fairly early quadrotor testing dating

to 2004, researchers implemented both classical and modern techniques for quadrotor control

in simulation, on test stands, and finally in flight. Proportional-Integral-Derivative (PID)

control was found to be robust to moderate perturbations without a particularly accurate

dynamic model, however the utility of Linear Quadratic (LQ) models were found to be

limited by the fidelity of the dynamic model being used, as simulated flights went well, yet

results were mixed on the test stand and flight tests [9].

3

Figure 1.1: Relative motor throttling to perform basic quadrotor maneuvers. Arrows indicatedirection of propeller rotation.

In [10], a particularly prolific team developed a nested controller, using PD gains to

control the vehicle attitude in response to a PID controlled outer loop for translational

navigation. System observability was achieved using a VICON motion capture system, which

uses a fixed array of cameras to track markers on vehicles, providing high-fidelity position

and velocity information. The team found very favorable results in the study, showing agile,

controllable flight for both individual vehicles and small formations of multiple vehicles. The

team continued their work in [11], demonstrating the same core controller in flight through

tightly constricted spaces, as well as fast, aggressive maneuvers for perching on vertical and

inverted surfaces. The team additionally achieved robustness to failed perching and quick

return to stable flight for repeated attempts. Other work from this team includes [12], in

which a nonlinear optimal controller calculates quadrotor trajectories in real time minimizing

the second time derivative of acceleration, referred to as snap. The team found precision in

trajectory following was a trade off with vehicle speed, however in certain trials, acceptable

precision was maintained up to a speed of 10 body lengths per second.

Traditional quadrotor drones are simple and useful, however control strategies designed

4

for the basic configuration are only truly valid for quadrotors with fixed payloads at the

center of the body. Studies of moving payloads improve the utility of quadrotors greatly. An

often-studied field of quadrotor research concerns the handling of swing loads representative

of any potential unactuated payload suspended beneath the vehicle. In [13], an eight degree

of freedom (DOF) model is considered in which the position of the unactuated swing load

is described by two angles. A dynamic model was constructed using Newtonian mechanics

and validated using a Lagrangian formulation, and then Model Based Control (MBC) was

implemented. The controller was found to compensate for the swing load and hold to its

prescribed trajectory, however the load did exhibit large oscillations throughout the initial

simulations. This was alleviated through the addition of a feed-forward controller which

compensated for swinging through higher-order control of the vehicle throttles, and the

swinging was dramatically reduced. In [14], learning algorithms were applied to a similar 8

DOF model, resulting in an actual flying prototype which was able to successfully navigate

a cluttered environment while carrying an unactuated swing load.

A further extension of the idea of underslung mass is the implementation of an actuated

robotic arm mounted on the belly of a quadrotor. In [15], a quadrotor is fitted with a three

link actuated arm intended for completing construction tasks. In this study, the system was

modeled, and separate controller loops were built for the quadrotor and arm. The arm control

was physically implemented using PID, but was merely considered an outside disturbance in

this study of its effects on the helicopter dynamics. The quadrotor was initially controlled

using a traditional quadrotor PID, however this was found to be insufficient in its response

to arm-movment perturbations. The response was found to improve dramatically when the

quadrotor was converted to a Variable Parameter Integral Backstepper (VPIB) controller.

This controller allowed for rapid response to the arm states, passed along as parameters.

The vehicle was found to be capable of holding very stable despite perturbations caused by

arm movements.

In [16], researchers modeled the dynamics of a quadrotor carrying an underslung gim-

balling camera with the goal of keeping the camera pointed at a target position as the quadro-

5

tor flies a given trajectory. The study developed the dynamics of the combined quadrotor-

camera system using the Lagrangian mechanics, and then implemented both backstepping

and sliding-mode nonlinear control. The backstepper controller was found to yield moderate

results, while the sliding mode controller was found to be insufficient, and in both cases,

further work was intended.

A logical step beyond compensating for a moving load is to actively consider the moving

load as a flight control input. This type of behavior is observed not only in novel man made

aerial vehicles, but in nature as well. Research in [17] proposed the rapid articulation of

a hawk moth’s abdomen during agile flight maneuvers may be actuating flight via inertial,

rather than aerodynamic effects. They constucted a dynamic model based upon the hawk

moth and simulated the use of dynamic inertia as a control input, then compared responses

in the actual animal when responding to visual stimuli in a lab setting. Comparing the

transfer functions, it was concluded the hawk moth was indeed utilizing inertial actuation.

The hawk moth research inspired work by a previous student in the Nonlinear Dynamics

and Control Laboratory, who sought to further swing load quadrotor dynamics through ac-

tuation of the load as dynamic inertia. An 8 DOF model similar to the previously discussed

studies was constructed and modeled using Lagrangian Mechanics, however the motor throt-

tles were combined from four independent inputs to only a total throttle and a yaw input.

All pitch and roll control authority was generated by applying torques to the underslung

load. This actuated mass was found to provide sufficient control authority for a variety of

trajectories when controlled using LQR, as well as strong robustness to modeling errors [18].

Direct torque and inertia effects from actuating inert masses are only one way of uti-

lizing additional degrees of freedom of a quadrotor to alter or improve performance for a

target mission. Another notable approach is to modify how and where aerodynamic forces

are imparted on the vehicle. One example of this is through implementation of a tilt-rotor

mechanism. In [19], a tilting mechanism is modeled on a quadrotor with the goal of allowing

the vehicle to fly without requiring the complete vehicle to lean in the direction of desired

motion, thus improving the utility of body-fixed directional payloads such as non-gimbaled

6

cameras. PID control is used to fly desired trajectories, and favorable results were found,

leading the author to suggest further experimentation with a physical prototype. An addi-

tional step from here is a complete conversion into a fixed wing aircraft by mounting the

four motors into tilting airfoil wings. This approach was studied in [20]. LQR control was

implemented for the quadrotor in its VTOL configuration, and dynamics were modeled for

the vehicle both as a quadrotor and as a fixed wing aircraft for horizontal flight, using cou-

pled rotation of the rear arms as elevators for pitch actuation, and independent rotation of

the front arms as ailerons for roll actuation. The authors did not address control during

the transitional period, but found favorable results in both the horizontal and vertical flight

regimes, with future work intended concerning the conversion.

Each variation of the basic design is useful in addressing a drawback of standard quadro-

tors. The tilting motors allow a body-fixed payload to be pointed independently of the di-

rection of travel. The tilting wings allow improved speed and efficiency in horizontal travel.

A novel variable-geometry quadrotor design can address other areas of improvement, such

as the ability to better balance loads by actively repositioning the center of lift, navigating

tight spaces, and the ability to recover from the typically catastrophic event of a motor loss

by reconfiguring into a trirotor.

The research presented here represents an initial study into the dynamics and control of

such a morphing quadrotor through a combination of inertial actuation and aerodynamic

load shifting. The vehicle is capable of actively varying its shape by controlling the rotation

of the arms with respect to the vehicle body about their yaw axes, as well as sliding the

motors and propellers along the arms as commanded, a similar but mathematically simpler

analogue for telescoping arms. This design is patent pending by Robodub Inc. [21], whose

early prototypes are shown in Figures 1.2 and 1.3.

Energy storage constraints imposed by battery volumes and masses subject small-scale

drones to fairly limited mission durations. In order to maximize the efficiency of flight oper-

ations, and therefore longevity, Linear Quadratic optimal control is selected as the preferred

controller design for this study. Further work for early flying prototypes may benefit from

7

Figure 1.2: Early Robodub prototype exhibiting a degree of arm angle variation. Photographprovided by Robodub Inc. and used with permission.

Figure 1.3: Early Robodub prototype quadrotor with variable length arms. Photographprovided by Robodub Inc. and used with permission.

8

additional exploration of PID control design, or potentially a combination using PID tuned

by LQR as proposed and simulated for quadrotor control in [22].

It is worth noting this present study accounts for drag only in the propeller torques and

not on the body due to translation or rotation, as all maneuvers are assumed to be performed

at a substantially low velocity to where they can be neglected. Lift and drag on the propellers

are assumed to be only a point thrust and a rotational couple centered on the motors and

do not account for p-factor torques and other effects which act on propellers rotating at

high speeds. Lastly gyroscopic effects are ignored. All of these factors are areas for further

consideration beyond the scope of this research.

1.2 Outline of the Thesis

This paper will investigate various aspects of the dynamics and control of the variable-

geometry quadrotor. Chapter 2 describes the dynamic model, develops dynamic equations

governing its movement, and simplifies the dynamics to the most capable yet tractable form

presently feasible. Chapter 3 investigates the implementation of LQR control on the full

model, and then to the vehicle in a trirotor state to which it would transition in the event of

a motor loss. Chapter 4 considers Set Point LQR Gain Scheduling control in which control-

ling the vehicle is investigated during shape transitions by modeling the vehicle as a series

of unconventionally-shaped fixed-geometry quadrotors. Chapter 5 investigates nonlinear nu-

meric optimal control of the quadrotor using a single throttle input with all attitude control

achieved through geometric morphing. Finally, Chapter 6 concludes the paper by briefly

reviewing findings and making suggestions for further research.

1.3 Contributions of the Thesis

The unique contributions of this research are diverse, as no existing publications have been

found concerning the dynamics and control a quadrotor which transforms in the manner of

the vehicle in this study. First, dynamic equations governing the vehicle are methodologically

determined. Second, LQR controls are applied to the vehicle and found to be stabilizing, and

9

therefore a good candidate for development into controls for an eventual prototype. Third,

controllability was confirmed for the linearized trirotor configuration to which the vehicle can

transform to recover from a motor loss, indicating further work should make the recovery

executable for a physical prototype. Fourth, Set Point Gain Scheduling was investigated

and found to be stabilizing for the vehicle during shape transitions, and an early study into

the optimal number of set points for a transformation was conducted, determining simple

transitions can be completed with only three set point gain combinations. Fifth, and lastly,

the ability to fly the vehicle using a single throttle channel and actuating roll and pitch

through geometric morphing was verified in simulation with numerical nonlinear control.

Each of these initial examinations of various topics form a basic foundation for further

investigation into the control of a variable-geometry quadrotor.

10

Chapter 2

DYNAMIC MODEL

This chapter will define the dynamic model of the conceptual morphing quadrotor in

terms of state and parametric values. An explanation is then provided for the process

by which the complete quadrotor dynamics are derived. The model is then simplified to

a symmetric form which is more mathematically tractable while continuing to enable the

desired novel system capabilities. The chapter concludes by declaring specific numerical

values for the system parameters used for control rule calculations in the following chapters.

2.1 Model Description

The full variable geometry quadrotor is modeled as a nine-mass system consisting of a central

body, four variable-angle arms rotating about pivot points on the body, and four motors,

each sliding along one of the arms. This sliding motor model is mathematically similar to

the concept of telescoping arms, and it can be easily adapted to whichever method of motor

positioning is selected for a final physical prototype. Body-fixed coordinate axes are defined

with the origin at the center of the body, and with the body-fixed x-axis pointed forward,

the body-fixed y-axis pointed to the right, and the body-fixed z axis pointed downward

as shown in Figure 2.1. This selection allows for a right-handed coordinate convention in

which the pitch angle, θ, rotating about the y-axis is positive in a nose-up attitude. The

other two rotations used to relate the body orientation to the inertial reference frame are the

roll angle, φ, positive clockwise when viewed from behind, and the yaw angle, ψ, positive

clockwise when viewed from above.

11

Figure 2.1: Basic initial model for the quadrotor, showing the orientation of the body-frameaxes and the external forces imparted by the motors and propellers.

The orientation of the body-fixed and inertial axes are related through the use of the

Roll-Pitch-Yaw Euler angle rotation matrix. This particular rotation sequence was selected

due to its containment of singularities far from the expected maneuvering regime of the

vehicle, a reason why it is commonly used in aerospace modeling. This rotation matrix is

given below using the space-saving trigonometric notation used throughout this paper and

exemplified by sφ = sin(φ):

R =

cθcψ −cθsψ sθ

cψsθsφ + cφsψ cφcψ − sθsφsψ −cθsφ−cφcψsθ + sφsψ cψsφ + cφsθsψ cθcφ

.

The vehicle was chosen to be modeled in the “plus” configuration common in quadrotor

development due to mathematical simplicity. This configuration places the arm base loca-

tions directly along the fore-aft and lateral axes. The four arm-motor pairs are designated

A, B, C, and D, beginning with the forward arm and continuing clockwise when viewed

from above. The arm orientations are defined by angles αA, αB, αC , and αD, each quantified

as the clockwise rotation from the forward-pointing, body-fixed x-axis. The position of each

motor on its corresponding arm is described by its distance outboard from the pivot point on

12

the body, designated dA, dB, dC , and dD. Examples of these quantities are shown in Figure

2.2.

The physical properties of the quadrotor are defined by parameters of fixed distances and

masses. The body of the quadrotor is modeled as a homogeneous cylinder of mass mbody,

radius ρbody, and height hbody. Each pivot is allowed is own distance from the center of the

body along the corresponding body-fixed axis to allow for maximum versatility in adapting

the model to the eventual prototype. These distances are designated bA, bB, bC , and bD.

The arms extend outward from these pivots, modeled as homogeneous thin rods of masses

marmA, marmB, marmC , and marmD, and lengths lA, lB, lC , and lD. The motors are modeled

as point masses of masses mmotA, mmotB, mmotC , and mmotD.

Figure 2.2: Examples of one of each dimension, duplicated in the model on each of the fourarms.

2.2 Generalized Coordinates

A full state description of the system could require three translational and three rotational

states be assigned and modeled for each of the nine masses, giving a total of 54 states.

13

Considering joint constraints reduces the state quantity greatly by locking together certain

degrees of freedom of the components mounted to the body and thus reducing the model to a

minimum number of generalized coordinates. In this model, the arms are fixed translationally

to the body at the pivot points. The pivot joint constraints fix the roll and pitch of the arms

to match that of the body, only allowing differences in yaw, which are directly quantified by

the α angles. The complete position and orientation of each arm can therefore be described

relative to the body by a single generalized coordinate each, the arm angle, α. The motors

are fixed to the arms on sliding joints. Such a joint locks all orientation angles, as well as

two of the three translational dimensions. The one remaining translational dimension for

each motor is quantified by the distance, d. A full six degree of freedom state description is

thus only necessary for a single component, from which other parts can be related through

a single generalized coordinate each. The model places the body-fixed axes at the center

of the body, and thus the same states relating the body to the inertial frame can serve as

the six body states of the system. The remaining eight shape states bring the minimum full

dimensionality of the complete quadrotor system up to fourteen, given by the state vector,

q = [x, y, z, φ, θ, ψ, αA, αB, αC , αD, dA, dB, dC , dD]T .

2.3 System Mechanics

The dynamics of the full quadrotor are highly shape-dependent. As the shape changes, the

center of gravity (CG) will shift through the vehicle. Calculating the location of the CG in

the body frame requires first describing the position of each component as a function of the

shape variables and fixed geometric parameters. The model is assumed to be planar, with

all components at the same body-frame z = 0, and thus coordinates are written in x and y

only. The position vectors are thus given by:

~rbody = [0, 0]

~rarmA =

[bA +

lA2cαA ,

lA2sαA

]

14

~rarmB =

[lB2cαB , bB +

lB2sαB

]~rarmC =

[−bC +

lC2cαC ,

lC2sαC

]~rarmD =

[lD2cαD ,−bD +

lD2sαD

]~rmotA = [bA + dAcαA , dAsαA ]

~rmotB = [dBcαB , bB + dBsαB ]

~rmotC = [−bC + dCcαC , dCsαC ]

~rmotD = [dDcαD ,−bD + dDsαD ] .

Having expressions for the body-frame position of the center of mass of each component,

the shape-dependent CG location in the body frame can now be found from the position

vectors and component masses. First, for simplicity in writing, the total mass is calculated

as the sum of the constituent masses,

mtotal =9∑i=1

mi.

Now, the system’s combined CG can be expressed as,

~CG =

∑9i=1mi~rimtotal

.

The moments of inertia for each component are found, including parallel axis effects from

the shape-dependent CG. The body inertias are found using the moment of inertia equations

for a homogeneous cylinder:

Ibodyφ = mbody

(3ρ2body + h2body

12+ CG2

y

)

Ibodyθ = mbody

(3ρ2body + h2body

12+ CG2

x

)Ibodyψ = mbody

(ρ2body

2+ CG2

x + CG2y

).

15

The inertias of the arms are found using the thin rod inertia equations, considering the

length projected onto the x and y axes for roll and pitch:

Iarmiφ = marmi

((lisαi)

2

12+ (rarmiy − CGy)

2

)

Iarmiθ = marmi

((licαi)

2

12+ (rarmix − CGx)

2

)Iarmiψ = marmi

(l2i12

+ (rarmix − CGx)2 + (rarmiy − CGy)

2

).

The motors are modeled as point masses, and therefore have only parallel axis effects

contributing to their inertias:

Imotiφ = mmoti (rmotiy − CGy)2

Imotiθ = mmoti (rmotix − CGx)2

Imotiψ = mmoti

((rmotiy − CGy)

2 + (rmotix − CGx)2).

Each contributing inertia is next summed to find the total state-dependent moments of

inertia of the system:

Iφ = Ibodyφ +4∑i=1

(Iarmiφ + Imotiφ)

Iθ = Ibodyθ +4∑i=1

(Iarmiθ + Imotiθ)

Iψ = Ibodyψ +4∑i=1

(Iarmiψ + Imotiψ) .

The primary flight controls for the full quadrotor are the four throttles, modeled as thrust

inputs uA, uB, uC , and uD. These forces act in the body-frame negative z direction at the

corresponding motor positions. Rotating these forces through the system’s rotation matrix,

R, for the vehicle’s current orientation state gives the thrust in each translational coordinate

direction as trigonometric functions of the orientation:

utotal = uA + uB + uC + uD

16

Fx = −utotalsθ

Fy = utotalsφcθ

Fz = −utotalcφcθ.

One method of calculating pitching and rolling moments due to applied thrusts is to find

the lever arm between the combined Center of Lift (CL) of the four motors and the CG of

the quadrotor in each axis direction. The CL is calculated using a similar equation to the

CG, however considering motors only for positions and corresponding applied thrust forces

in place of masses,

~CL =

∑4i=1 ~rmotiuiutotal

τφ = utotal(CGy − CLy)

τθ = utotal(CLx − CGx).

Motors A and C are assumed to rotate their propellers counterclockwise, and B and

D rotate clockwise. The motors therefore exert opposite torques, assumed to be linearly

proportional to the thrust inputs by the propeller drag coefficient kd. These torques represent

the external forcing on the quadrotor in the ψ dimension,

τψ = kd(uA − uB + uC − uD).

With forces, torques, masses, and inertias known, the system dynamics are calculated

using Newton’s Second Law. The original momentum form of the equation must be used

due to the time-variance of the state-dependent moments of inertia:

F =d

dtmv = ma

τ =d

dtIω = Iω + Iω.

Internal forces used to actuate the motor positions on the arms will have no effect on the

inertial-frame behavior of the quadrotor’s CG, and they therefore are fully accounted for in

the shifting CG terms of the dynamic derivation. Considering the translational dynamics of

17

the CG rather than the body frame axes allows the translational dynamics to be easily and

compactly expressed:

xCG =Fx

mtotal

yCG =Fy

mtotal

zCG = g +Fz

mtotal

.

CG dynamics suffice for mathematical modeling and early simulations, however sensor

equipment used to detect state information for stabilizing feedback control of an eventual

prototype will be rigidly mounted to the body, and thus body-frame dynamics will be nec-

essary. These dynamics can be found using the second derivative of the CG shift rotated

through the rotation matrix, which will give much more complicated translational dynamic

equations in terms of the positions, velocities, and accelerations of the eight shape states.

These equations are given in expression form,

x = xCG − [[ ¨CGx, ¨CGy, 0]R]x

y = yCG − [[ ¨CGx, ¨CGy, 0]R]y

z = zCG − [[ ¨CGx, ¨CGy, 0]R]z.

Internal torques used to actuate the arms act entirely in the yaw direction, and the forces

which slide the motors act in the plane of body-frame z = 0, and therefore no additional

acceleration appears in the roll and pitch dynamics due to shape changes, allowing for simple

expressions of the roll and pitch dynamics:

φ =τφ − Iφφ

Iφ

θ =τθ − Iθθ

Iθ.

The torques used to actuate the arm angles will act equal and opposite on the body and

corresponding arm of the vehicle, producing accelerations in the angles of each which sum

18

to the angle change in each body-relative arm angle α. Direct control of the shape-state

accelerations is assumed to avoid necessitating consideration of constraint forces and torques

in this initial study, instead allowing the focus to be only on the desired shape changes.

Translational effects on the system CG are again accounted for in the shifting CG terms,

however the proportion of angular acceleration imparted upon the body, and therefore on

the body yaw angle ψ requires not only knowledge of the instantaneous relative inertias,

but also due to the continuous shape changes, the time derivative of not only the relative

inertias, but also the angular velocities of each portion. While not impossible to solve for,

these terms are very difficult to determine in closed form, and the current schedule for this

research will not allow for it. A similar situation arises when considering the yaw torques

imparted by the sliding motor forces when the arms are not perpendicular to the body.

The resulting unknown internal torques are expressed together in the term τψinternal in the

complete dynamic expression for the yaw,

ψ =τψ − Iψψ + τψinternal

Iψ.

One simple method of removing the need to consider internal torques from the require-

ments of the model is to constrain movement to where canceling torques will always be

applied during shape transitions, thus enforcing τψinternal = 0. This cancellation can be

achieved by requiring opposite arm-motor pairs be symmetric with matching geometric and

mass properties, as well as mirrored arm and motor movements. This simplification does

remove some degree of capability from the system, however it will nonetheless allow for

modeling all suggested novel capabilities. Further work should investigate the influence of

these internal torques, perhaps investigating the effects of using the forces and torques as the

selected control channels as opposed to the direct shape state acceleration control currently

being investigated.

19

2.4 Symmetric Analytic Model

Requiring mirrored arm properties and movements reduces the dimensionality of the system,

and makes redefining certain states favorable. The mirroring of motor positions can be

accounted for by setting dC = dA and dD = dB, thus eliminating the shape states dC and

dD. Arms A and C are also now positioned using a single angle. To avoid constant use of

phase shift angles, a new angle is defined, αφ, describing the arms’ angular displacement left

of the body-frame x axis. This orientation was selected such that a positive αφ displacement

should shift the CL in such a way it produces a positive moment in φ. Likewise, the angles

of arms B and D are defined as the angle forward of the body-frame y axis, shifting the CL

to produce a positive moment in θ. These new definitions are shown in Figure 2.3. The new

ten element state vector now contains six body states and four shape states. This new state

vector is written as,

q = [x, y, z, φ, θ, ψ, αφ, αθ, dA, dB]T .

Balancing forces and torques produced by shape changes requires geometric and mass

parameters be made symmetric. Therefore, parameters marmC , mmotC , and lC are set equiv-

alent to marmA, mmotA, and lA. An identical equivalency is set for the D parameters and

the B parameters. The eventual physical prototype will likely be constructed symmetrically,

so resulting losses in model versatility are minimal. The process continues identically to

before, however adjusted for the new matching properties and arm angle definitions. The

new position vectors are given:

~rbody = [0, 0]

~rarmA =

[bA +

lA2cαφ ,−

lA2sαφ

]~rarmB =

[lB2sαθ , bB +

lB2cαθ

]~rarmC =

[−bA − lA

2cαφ ,−

lA2sαφ

]~rarmD =

[lB2sαθ ,−bB − lB

2cαθ

]

20

Figure 2.3: Simplified model using symmetrically constrained body shapes to cancel internaltorques.

~rmotA =[bA + dAcαφ ,−dAsαφ

]~rmotB = [dBsαθ , bB + dBcαθ ]

~rmotC =[−bA − dAcαφ ,−dAsαφ

]~rmotD = [dBsαθ ,−bB − dBcαθ ] .

The CG calculation equation is identical to before, as are the equations for the moments

of inertia of the body and motors with the exception of the new mass property equivalencies.

The roll and pitch inertias of the arms change to match the new arm angle definitions:

IarmAφ = marmA

((lAsαφ)2

12+ (rarmAy − CGy)

2

)

IarmAθ = marmA

((lAcαφ)2

12+ (rarmAx − CGx)

2

)IarmBφ = marmB

((lBcαθ)

2

12+ (rarmBy − CGy)

2

)

21

IarmBθ = marmB

((lBsαθ)

2

12+ (rarmBx − CGx)

2

)

IarmCφ = marmA

((lAsαφ)2

12+ (rarmCy − CGy)

2

)

IarmCθ = marmA

((lAcαφ)2

12+ (rarmCx − CGx)

2

)

IarmDφ = marmB

((lBcαθ)

2

12+ (rarmDy − CGy)

2

)

IarmDθ = marmB

((lBsαθ)

2

12+ (rarmDx − CGx)

2

).

Once the new arm inertias have been accounted for in terms of the newly defined arm

angles, the equations of motion take the same form as they did previously, however with

internal torques now canceling each other, the yaw dynamics simplify to a tractable form,

ψ =τψ − Iψψ

Iψ.

The full expressions for the dynamics are gargantuan when calculated through, and are ex-

pressed here only in terms of constituent calculations for brevity. The full dynamic equations

for the symmetric quadrotor may be found in Appendix A.

2.5 Selected Numerical Values

Before applying Linear Quadratic Regulator (LQR) control to the quadrotor, numerical val-

ues are assigned to each geometric and mass property. The eventual physical implementation

of the system has not yet been designed, and therefore realistic numbers were estimated for

a hobby-sized vehicle. The selected values are given in Table 1.

22

Masses (kg)

mbody 0.3

marmA 0.1

marmB 0.1

mmotA 0.1

mmotB 0.1

Lengths (m)

ρbody 0.1

hbody 0.1

bA 0.1

bB 0.1

lA 0.3

lB 0.3

Gravity (m/s2)

g 9.81

Prop Drag Coefficient

kd 0.5

Table 1: Quadrotor Physical Properties

23

Chapter 3

LQR CONTROL

This chapter examines the use of Linear Quadratic control on the symmetric morphing

quadrotor including all eight control inputs. The system dynamics are linearized about a

hover condition, and LQR control gains are calculated and applied in simulation to both

the linearized and nonlinear models. An LQR controller is then examined as a possibility

for controlling the vehicle in a trirotor configuration to which the system could convert for

continued stable flight upon loss of a motor. The vehicle dynamics are linearized about this

trirotor configuration, and the resulting LQR gains are tested again on both the linearized

and nonlinear dynamic models.

3.1 Full System Control

As briefly mentioned in the proceeding chapter, direct control of shape state accelerations is

assumed, with physical realization of this left to be achieved by the prototyper. Four shape

states, combined with four throttle inputs, give the system a total of eight inputs:

~u =[uA, uB, uC , uD, αφ, αθ, dA, dB

]T.

The most fundamental flight regime for the quadrotor is that of a stationary hover, and

therefore the hover is the first condition considered for linearization. The nonlinear model is

linearized about a full zero state with the exception of placing the motors at the mid-spans

of the arms to maximize available travel in each direction and throttle inputs each equal to

a quarter of the vehicle weight, expressed in the form ~x = A~x+ Bu, where:

~x =[x, y, z, φ, θ, ψ, αφ, αθ, dA, dB, x, y, z, φ, θ, ψ, αφ, αθ, dA, dB

]T.

24

The dynamic matrices are found to be very sparse and are able to be compactly written

in equation form shown below, excluding the obvious position-velocity dependencies and

direct state control equations. The linearized equations are relatively simple, allowing easy

verification against intuition about the system’s behavior:

x = −9.81θ

y = 9.81φ

z = −0.9091(uA + uB + uC + uD)

φ = 8.0264αφ − 9.0909(uB − uD)

θ = 8.0264αθ + 9.0909(uA − uC)

ψ = 9.1743(uA − uB + uC − uD)

The controllability matrix produced using A and B was found to be full row rank, in-

dicating the linearized system is controllable. To develop an optimal control rule for the

linearized system using LQR methods, cost weighting matrices must be declared in the cost

function:

∫ ∞0

(~xT Q~x+ ~uT R~u

)dt.

The cost matrices Q and R correspond to the magnitude of the state deviation and control

inputs from the linearization condition. The first case study will investigate recovery from

perturbation and return to the origin. All states and controls are cost-weight normalized

by setting Q = I20x20 and R = I8x8. Bryson’s rule for constrained states is then applied

to constrain the arm angles to less than π4

in either direction and motor positions to stay

on the arms with less than l2

travel in either direction. The corresponding LQR gains were

calculating using Mathematica’s LQRegularGains function. The resulting 8x20 gain matrix

is shown below, expressed in block notation to fit to the page:

K =[K1(8x7), K2(8x7), K3(8x6)

]

25

K1 =

−0.7071 0 −0.5 0 4.57 0.5 0

0 −0.7071 −0.5 −4.57 0 −0.5 −0.4447

0.7071 0 −0.5 0 −4.57 0.5 0

0 0.7071 −0.5 4.57 0 −0.5 0.4447

0 −0.0032 0 −0.0424 0 0 1.4181

0.0032 0 0 0 −0.0424 0 0

0 0 0 0 0 0 0

0 0 0 0 0 0 0

K2 =

0.4447 0 0 −1.0765 0 −0.7246 0

0 0 0 0 −1.0765 −0.7246 −1.0014

−0.4447 0 0 1.0765 0 −0.7246 0

0 0 0 0 1.0765 −0.7246 0

0 0 0 0 −0.0065 0 0.0003

1.4181 0 0 0.0065 0 0 0

0 6.6667 0 0 0 0 0

0 0 6.6667 0 0 0 0

K3 =

1.0014 0.5265 0 0.0029 0 0

0 −0.5265 −0.0029 0 0 0

−1.0014 0.5265 0 −0.0029 0 0

0 −0.5265 0.0029 0 0 0

0 0 1.9586 0 0 0

0.0003 0 0 1.9586 0 0

0 0 0 0 3.7859 0

0 0 0 0 0 3.7859

.

These gains were applied first on the linear model to study the response and ensure

convergence. The dynamics were modeled for five seconds using Mathematica’s NDSolve

function with the dynamic equation ~x =(A− BK

)~x. The resultant state trajectories are

26

shown in Figure 3.1.

Figure 3.1: State trajectory of linearized quadrotor initialized at [x, y, z] = [1, 1, 1].

The linearized system response was found to be stable and controllable back to the

origin as expected. The return to the origin was achieved using a combination of differential

throttling and arm movements to actuate the vehicle in the pitch and roll directions. No

response was found on the motor positions, as their movement in the perpendicular-arm

configuration would effect only the rotational inertia of the system, and with the model

linearized about a zero angular velocity state, no other state dynamics are effected by dA

or dB. Having verified the control on the linear model, the hover controls are next applied

to the nonlinear model using the same initialization and the control substitution ~u = −K~x.

The resulting state trajectories are shown in Figure 3.2.

27

Figure 3.2: State trajectory of nonlinear quadrotor initialized at [x, y, z] = [1, 1, 1].

The dynamics are again found to be stabilized, with x and y returning to the origin

through actuation of the pitch and roll using a combination of differential throttling and

arm angle actuation in a similar pattern to that of the linearized model, however the z

displacement did not converge back to the origin. The reason for this non-convergence is

currently unknown and is an area for future investigation, however the stabilizing effect of

the controller is promising, suggesting further development could lead to an LQR controller

capable of flying the quadrotor in real time.

3.2 LQR After Motor Loss Recovery

A quadrotor which experiences a motor loss is typically uncontrollable as motor forces and

torques are impossible to balance for stable flight. Research in [23] investigates the possibility

of preventing loss of a vehicle by stabilizing the angular velocity of the vehicle rather than

angular position, and then controlling translation in the air by directing the axis of rotation

28

through use of periodic throttle controls. While this method is successful for preventing loss

of the vehicle or damage to property or observers below, it will not allow for continuation

of most missions as intended. One of the unique capabilities of the morphing quadrotor

is the potential ability to survive a motor loss and continue with the assigned mission by

transforming into a trirotor configuration.

This study investigates the controllability of the vehicle after experiencing a loss of motor

C. Motors A and C are assumed to rotate counterclockwise, while motors B and D oppose

and balance their torques on the system. Assuming the thrust-torque relationship to be

linear through the propeller drag coefficient kd, the hover input on motor A after losing

motor C must now double to equal half of the vehicle weight. This doubling will intuitively

require shortening of the moment arm for motor A by reducing dA while simultaneously

moving motors B and D to the rear to maintain pitch stability. A solver was written which

sought acceptable shape combinations by positioning the CL over the CG. The resultant

configuration selected for modeling is given by dA = 0.025, dB = 0.3, and αθ = −1.1597. An

approximation of this configuration is shown in Figure 3.3.

Figure 3.3: Selected Trirotor hover configuration which balances the doubling of uA.

29

This configuration was used as the linearization configuration for the model, again lin-

earizing the dynamics into the form ~x = A~x+ Bu, now with a reduced seven element input

vector resulting from the loss of uC . The dynamic matrices were again found to be very

sparse, and are compactly expressed in linear equation form, omitting again the position-

velocity dependencies and the direct shape control equations:

x = −9.81θ

y = 9.81φ

z = −0.9091(uA + uB + uD)

φ = −57.5535αφ + 27.3432(uB − uD)

θ = 14.8923αθ + 120.867dA − 73.7785dB + 4.7553uA − 5.4721(uB + uD)

ψ = 19.4444(uA − uB − uD).

These equations mostly match intuition for the system, apart from the φ dynamics, which

have signs opposite of what intuition would expect. Troubleshooting of the dynamic code has

not yet resulted in the discovery of any process errors, however continued work and possible

reprogramming of the code may yield improved linearized dynamics.

A standard fixed-geometry trirotor is under-actuated and thus non-controllable due to

coupling between pitch, roll, and yaw. The underactuation is typically alleviated by adding

a servo which controls axial rotation of one arm, thus vectoring thrust for independent yaw

control [24]. An initial examination of whether or not the shape modulation used in this

study is an acceptable replacement for the axial rotation can be done by examining the

controllability matrix calculated from A and B. The controllability matrix was found to be

full row rank, indicating the linearized system is controllable, and likely would remain so even

if the dynamic equations were altered slightly such as reversing the signs on the φ dynamics

to better match intuition concerning the system. The gaining of controllability via addition

of geometric modulation was further validated by calculating the controllability matrix with

30

the geometry fixed in the trirotor configuration, which was found not to be full-row rank,

and thus match the understanding of non-controllability for fixed-geometry trirotors.

Despite the conflict with intuition, the latter portion of the code executed and results

were found. First, using the same cost-weighting matrices from the quadrotor hover state

except reducing the length of the R matrix by one to match the shortened control vector

length, a gain matrix K was found, again expressed using block notation:

K =[K1(7x7), K2(7x7), K3(7x6)

]

K1 =

−0.0828 0 −0.2591 0 0.6438 0.16 0

0.1273 0.2235 −0.1247 1.4541 −1.0513 −0.1351 −1.0881

0.1273 −0.2235 −0.1247 −1.4541 −1.0513 −0.1351 1.0881

0 0.0331 0 0.2546 0 0 4.8090

−0.0815 0 −0.0138 0 0.9889 −0.0641 0

−0.6617 0 −0.1120 0 8.0256 −0.5198 0

0.4039 0 0.0683 0 −4.8989 0.3173 0

K2 =

0.2879 2.3363 −1.4261 −0.1330 0 −0.5976 0

−0.5608 −4.5518 2.7785 0.2083 0.3409 −0.2829 0.3212

−0.5608 −4.5518 2.7785 0.2083 −0.3409 −0.2829 −0.3212

0 0 0 0 0.0554 0 −0.0069

2.1964 9.7101 −5.9272 −0.1520 0 −0.0325 0

9.7101 79.808 −48.1052 −1.2336 0 −0.2636 0

−5.9271 −48.1052 30.3639 0.7530 0 0.1609 0

31

K3 =

0.1694 0.1781 0 0.0175 0.1419 −0.0866

−0.2913 −0.1398 −0.0188 −0.0380 −0.3083 0.1882

−0.2913 −0.1398 0.0188 −0.0380 −0.3083 0.1882

0 0 3.2575 0 0 0

0.3685 −0.0826 0 1.8749 1.1596 −0.7078

2.9904 −0.6707 0 1.1596 11.1431 −5.7446

−1.8254 0.4094 0 −0.7078 −5.7446 5.2386

.

These control gains were first applied to the linearized dynamics, with the same [x, y, z] =

[1, 1, 1] initialization used for the full quadrotor dynamics study. The resulting state trajec-

tories are shown in Figure 3.4.

Figure 3.4: State response of the linearized trirotor with LQR control.

The system was verified to be completely stabilized and controllable in the linear model,

now utilizing motor positions as controls as well due to their new effect on the θ dynamics.

Having verified controllability on the linearized model, the LQR control gains were next

applied to the full nonlinear trirotor dynamics. The response is shown in Figure 3.5.

32

Figure 3.5: State response of the full nonlinear trirotor with LQR control.

The nonlinear dynamics of the trirotor-configured vehicle were found to be unstable in

every degree of freedom when controlled with the calculated LQR gains. This instability is

being attributed to the unexpected signage in the φ dynamics, as well as the nonlinearity

of the system near the linearization condition, as upon parsing the dynamic equations, they

are found to be dependent on αθ through various trigonometric functions, including sin(αθ),

cos(αθ), sin2(αθ), and sin(2αθ).

33

Chapter 4

LQR GAIN SCHEDULING

This chapter proposes the strategy of utilizing traditional quadrotor LQR gains calculated

for various unconventional but fixed configurations to govern an inner control loop utilizing

the throttle inputs only while relegating shape control and actuation to an outer control

loop which commands shape transitions based upon specific mission requirements such as

navigating a constricted space. Using this controller, the quadrotor transitions through a gain

schedule to utilize the gains for the fixed geometry closest to the instantaneous configuration

while executing a transition. The objective of this study is to first test the validity of the

control strategy, and then to investigate the optimal number of gain set points throughout

the trajectory.

A basic transition is selected for the initial evaluation of the set point gain schedule

strategy. The quadrotor is assumed to begin hovering at the origin in the basic configuration

used for the initial hover linearization in the proceeding chapter. The quadrotor is then

modeled as rotating arms B and D back while extending motor A and C to the ends of their

arms. The vehicle thus transitions from an initial shape state of αθ = 0 and dA = lA2

to a

final shape of αθ = −π3

and dA = lA. The transition is modeled assuming bang-bang control

of the shape state accelerations, completing the transition in first two seconds and then one

second.

A single MATLAB script was written which linearly spaces a user-specified number of set

point geometries along the programmed transition. The script then iterates the linearization

procedure and determines optimal gain matrices using the same LQR process and cost-

weighting matrices as in the proceeding chapter for each of the linearization conditions, and

then propagates the nonlinear dynamics of the controlled quadrotor using forward Euler

34

with a time step of 1 millisecond. In each iteration, the shape state is read and the gains for

the nearest available set point geometry are applied to the throttle inputs. The first study

utilized a single transition between two set points, one each for the beginning and ending

configurations. The model is initialized stationary at the origin with the goal of maintaining

a stable hover at the origin throughout and after the transition. The control inputs and

corresponding state trajectories for the two second trial are shown in Figures 4.1 and 4.2,

respectively.

Figure 4.1: Throttle controls applied during two second transition using two set points.

35

Figure 4.2: State trajectories during two second transition using two set points. The redlines indicate the gain switching time.

Small step discontinuities appear in the throttle controls at the transition point, indicating

a non-fluid transition between the gains. The state trajectories are observed to be destabilized

as αθ deviates from a perpendicular arm configuration. Upon switching gains, the vehicle is

stabilized and returns to stable level flight, although it does not return to the origin. This

stabilization verifies the potential for set point gain scheduling control, however further work

will be necessary to isolate the cause of the nonzero position.

The previous case was repeated with a one second transition duration to study the effects

of the transition time on the dynamics of the system. The resulting controls and state

trajectories are shown in Figures 4.3 and 4.4.

36

Figure 4.3: Throttle controls applied during one second transition using two set points.

Figure 4.4: State trajectories during one second transition using two set points. The redlines indicate the gain switching time.

37

The trajectories are found to be similar in characteristics, however slightly greater pitch

deviation is noted before the system restabilizes. To allow greater time for dynamic devi-

ation during transformation, further trials were conducted using the two second duration,

examining the effects of varying the number of set points on the system behavior during

shape transitions.

The simplest additional set point to examine is that of halfway through the transition,

when αθ = −π6

and dA = 3lA4

. To adhere to the closest set point, the controller switches

gains when the position is a quarter through the transition, and then switches to the final

gains when a only a quarter remains. Half of the transition distance is thus controlled with

the new gain matrix. The controls and state trajectories produced by the three set point

controller are shown in Figures 4.5 and 4.6.

Figure 4.5: Throttle controls applied during two second transition using three set points

38

Figure 4.6: State trajectories during two second transition using three set points. The redlines indicate the gain switching times.

The magnitude of transient θ deviation is dramatically reduced, as is the final transla-

tional steady-state error. The step discontinuities in uA and uC appear to have vanished,

and those in uB and uD have become exceedingly small. A further trial was conducted

with 5 set points, with new set points evenly spaced between the starting, middle, and final

configurations. The resulting state trajectory is shown in Figure 4.7.

39

Figure 4.7: State trajectories during two second transition using five set points.

The state trajectory behavior with five set points is observed to be essentially identical

to that of the three set point model. The maximum θ deviation appears identical, as do

the steady-state translational errors. It was therefore concluded simple transitions can be

handled using three set point controllers. This was further verified by calculating the state

trajectory for a nine set point model, and again finding any improvement in state trajectories

to be negligible. More complex transitions may require a greater number of set points, and

that remains an area for further study.

40

Chapter 5

NUMERICAL OPTIMAL CONTROL

The variable geometry quadrotor presents an additional opportunity for unique develop-

ment in moving beyond the common differential throttling control and instead generating

control authority for the vehicle via geometric transformation. This chapter verifies the con-

trollability of a quadrotor using geometric modulation in place of differential throttling using

numerical simulation in MATLAB. Controls are developed using a nonlinear numerical opti-

mizer, which although impossible to use in real time due to excessive run times, can be used

to construct desired trajectories which real-time controllers can be mapped to approximate.

5.1 Code Structure

The code set used in this study consists of a series of MATLAB functions. The master file

is first used to set parameters such as control point resolution and final time, then initializes

a vector of control inputs at discrete time intervals. The master file then calls the fmincon

function, which on each iteration passes the control vector to the nonlinear constraint func-

tion. This function in turn calls the trajectory function, which propagates the dynamics of

the system for the supplied control vector, linearly interpolating the controls between the

discrete control points. The trajectory function passes the state trajectory back to the con-

straint function, which tests the trajectory against equality and inequality constraints used

to set the final condition and to bound the shape states of the quadrotor. The constraint

function passes back a pass/fail status to the optimizer. The fmincon optimizer function

also passes the control vector at each iteration to the cost function, which calculates the cost

using the target cost function,

J =

∫ tf

0

u2i + (uthrottle −mg)2dt,

41

where ui is each non-throttle input.

The optimizer builds a Hessian matrix comparing the changes in the cost with respect

to variations in each element of the control vector until a local minimum is found. Once a

feasible local minimum has been found which satisfies the supplied constraints, the optimizer

passes the optimized control vector out to the master file. The master file then passes the

optimal control vector again to the trajectory function and receives back the correspond-

ing state trajectory. This trajectory is passed to the animation function, which draws the

quadrotor in it’s instantaneous state at each control point interval, passing back the still

frames to the master file. The master file finally writes the frames to a video, then saves the

video, plots of the control inputs and state trajectories, and all control and state trajectory

data for later use. A flow chart approximating this structure is shown in Figure 5.1.

Figure 5.1: Flow diagram for the nonlinear control optimization code.

Each added control input channel increases the length of the optimization vector by the

selected control resolution, thus increasing the complexity of the optimization task exponen-

tially. As a result, the model needed to be simplified to the greatest possible extent while

still demonstrating unique capabilities. Toward this end, the four throttles were assumed to

42

remain identical, with all attitude authority derived from the motion of the motors relative

to the CG. Additionally, all motors are assumed to remain at the outer extreme of the arms,

yielding all control to the arm angles.

5.2 1D Translation

This code structure was initially developed in 2D, balancing all roll and yaw forces such

that rotation and translation occur only in the x, z, and θ directions. The first trial run to

validate the code further simplified the model to 1D by setting the vertical component of the

thrust to equal the weight of the vehicle using the reciprocal cosine function, or secant:

utotal =mtotalg

cθ= mtotalgsec(θ).

This initial test thus used only a single input: the angular acceleration of the arms, αθ.

The quadrotor was commanded to move three meters to the right from the origin, beginning

and ending at a stationary hover, and the trajectory was propagated using fixed-step 4th

order Runge-Kutta numerical approximation with a dynamics stepping 100 times faster than

the control. The control channel optimizer quickly converged to the input shown in Figure

5.2. The corresponding state trajectories are shown in Figure 5.3.

43

Figure 5.2: Optimal control input shown for three meter 1D translation in three seconds.

Figure 5.3: Optimal state trajectories shown for three meter 1D translation in three seconds.

44

As can be seen in the arm angle trajectory, the optimizer converged to a highly believable

trajectory, in which the arms move back to pitch the nose down and generate horizontal

thrust, then move forward to rotate the vehicle back and brake its movement. The arms

then cycle through this pattern one more time to level the quadrotor as it reaches the target

location. Overlaying the frames from the animated trajectory as shown in Figure 5.4, one

can readily visualize this trajectory, as well as the magnitude of arm movement necessary to

achieve the desired trajectory.

Figure 5.4: Optimal state trajectory for three meter 1D translation in three seconds, shownas overlaid animation frames.

5.3 2D Translation

The code structure has thus been verified for a very simple case. The next case to be

investigated is that of the 2D model adding vertical movement. Vertical control is achieved

by considering utotal as a second control channel. This input will provide both vertical

45

acceleration and pitching acceleration proportional to the lever arm between the CL and the

CG.

The constraint function was modified to give a final state of a stationary, level hover

at [3, 3], and the code was run for final times of 2, 3, and 4 seconds. While that shape

control was initialized again at zero input, the throttle was initialized near a hover to hasten

and improve convergence to an optimal trajectory. Modeling the dynamics using Runge-

Kutta produced convergence errors, and thus the code was converted to using Forward Euler

approximation, now 300 times faster than the control steps. The optimal control input on

both input channels is shown with a 4 second final time in Figure 5.5, and with a 3 second

final time in Figure 5.6.

Figure 5.5: Optimal control inputs for arm angle and throttle for a four second move fromthe origin to [3, 3].

46

Figure 5.6: Optimal control inputs for arm angle and throttle for a three second move fromthe origin to [3, 3].

The arm inputs again take a similar form to that observed in the 1D translational case.

The throttle control exhibits interesting behavior however, as reducing the final time leads

to an oscillatory optimal thrust. This is the result of synchronization with the vehicle pitch

and arm angles to apply maximum thrust force when it best contributes to the required

pitching and vertical actuation. This become much more noticeable when the final time is

reduced to two seconds, as shown in Figure 5.7, in which the optimal throttle input even

calls for shutting down the motors entirely and coasting for a brief period.

47

Figure 5.7: Optimal control inputs for arm angle and throttle for a two second move from theorigin to [3, 3]. Note the total shutdown of the throttle during which the vehicle is coastingin free fall.

The state trajectories for the three cases assume very similar forms to one another, with

variance observed primarily in angular and velocity magnitudes. While the four second and

three second trajectories are very similar, the two second trajectory does show noteworthy

plateauing and freefall due to the throttle shutdown. The trajectories for the three second

and two second cases are shown in Figures 5.8 and 5.9.

48

Figure 5.8: Optimal state trajectories from the origin to [3, 3] with a final time of threeseconds.

Figure 5.9: Optimal state trajectories from the origin to [3, 3] with a final time of two seconds.Note the plateau in x velocity and the free-fall in z velocity.

49

The animation frames from these two studies show an excellent comparison of the arm

angles magnitudes and spatial behavior of each trajectory. The overlaid animation frames of

the three second and two second trajectories are shown in Figures 5.10 and 5.11, respectively.

Figure 5.10: Overlaid animation frames for 2D trajectory animation with three second finaltime.

50

Figure 5.11: Overlaid animation frames for 2D trajectory animation with two second finaltime.

It is worth noting the speed of arm movement in Figure 5.11. The frames are approxi-

mately 0.1 seconds apart, and significant arm movement is observed between frames. This

can be confirmed by noting the angular velocity in Figure 5.9 reaches magnitudes of approxi-

mately four radians per second. This angular velocity is likely not physically achievable with

the oscillatory period required to actuate the vehicle. This will be worth studying before

attempting to implement geometric control on a physical prototype to quantify limits arising

from available hardware performance saturation.

51

5.4 3D Translation

The final tested version of the model adds three additional degrees of freedom: y, φ, and αφ,

as well as a single additional control channel, αφ. The 3D model was initialized at the origin

and commanded to move to [3, 3, 3]. The final time of two seconds was found to produce

extremely unrealistic behavior, and thus the optimization was run only for final times of five,

four, and three seconds. The results for five and four seconds were found to be very similar,

and therefore the five second case results have been omitted here for brevity. The optimal

controls for the four and three second cases can be found in Figures 5.12 and 5.13.

Figure 5.12: Optimal control inputs for arm angles and throttle for a four second move fromthe origin to [3, 3, 3].

52

Figure 5.13: Optimal control inputs for arm angles and throttle for a three second movefrom the origin to [3, 3, 3].

Behavior is again very similar to that observed in the 2D case. The arm angles are

found to match perfectly, which matches intuition considering the vehicle is seeking equal

displacement in both x and y, and thus will utilize equal rotation in φ and θ. Throttle

modulation taking advantage of configuration begins to appear in the three second case as

well. The state trajectories for both of these cases are exceedingly similar in form, as seen

in Figures 5.14 and 5.15.

53

Figure 5.14: Optimal state trajectories from the origin to [3, 3, 3] with a final time of fourseconds.

Figure 5.15: Optimal state trajectories from the origin to [3, 3, 3] with a final time of threeseconds.

54

The quadrotor’s trajectory is visualized for the three second case only in Figure 5.16.

Figure 5.16: Overlaid animation frames for 3D trajectory animation with three second finaltime.

The quadrotor has thus been verified to be controllable in simulation in three dimensions

through simultaneous actuation of pitch and roll via arm angle modulation. It is worth not-

ing in this simple model, the vehicle is unforced in the ψ dimension and thus that degree of

freedom can be excluded, however to allow a physical prototype to respond to ψ perturba-

tions, an additional control input could be necessary, either in differential throttling between

the motor pairs spinning each direction, or though the use of thrust vanes installed beneath

the propellers. Additionally, this model excludes conservation of angular momentum quan-

55

tified in the Iω term in the momentum form of Newton’s Second Law, assuming a relatively

constant momentum and/or low angular velocity in pitch and roll. Initial trials including

this conservation were run, but found due to symbolic substitution at each dynamic step

to slow the time per iteration to in excess of five minutes compared to the fractions of a

second observed in these studies necessary to execute tens or even hundreds of thousands of

iterations to reach convergence. This remains an area for future development in this topic.

56

Chapter 6

CONCLUSION

6.1 Conclusion

This research has been a broad initial examination of the dynamics and unique capabilities

of the Robodub patented variable geometry morphing quadrotor. In Chapter 1, background

material was provided pertaining to the development of the industry and utilization of drones,

and in particular quadrotors. The chapter then examined proceeding work pertinent to

quadrotor control development, and proposed the concept of the morphing quadrotor, along

with its unique target capabilities.

In Chapter 2, mathematical and geometric definitions for the variable-geometry quadro-

tor were given. A full dynamic model was then derived assuming direct shape control.

Quantification of reaction torques acting on the body was troublesome, and therefore a

simplified symmetric model was constructed which continued to offer all unique target capa-

bilities intended for investigation in this study. The dynamics of this symmetric model were

found. Lastly, numerical values for a generic hobby-sized quadrotor were declared to allow

for numerical calculations going forward into control development.

Chapter 3 investigated control of the complete system using LQR with a linearization

about a hover at the origin with arms perpendicular and motors at the mid spans of the

arms. The controller was found to be stabilizing, although it did not return to the origin in

z translation. LQR controls were next calculated for the trirotor configuration to which the

quadrotor would transform in the event of a motor loss. The linearized dynamics produced a

controllability matrix which was full row rank, indicating the system should be controllable,

however the nonlinear dynamics were found to be unstable in every dimension when applying

the LQR gains. With the controllable matrix, LQR shows promise, however without a stable

57

controller at this point, this remains an area for future work.

Chapter 4 proposed the utilization of Set Point LQR Gain Scheduling to control the

quadrotor during shape transitions by modeling it as a series of fixed-geometry quadrotors

and passing through the various gain sets during a transformation. A simple transformation

was modeled and an initial study using set points only at the beginning and end shapes

showed great promise for rapid restabilization of the system. Increasing to three set points

was found to dramatically decrease the pitch deviation experienced by the vehicle during

the transition, however further increases in set point quantity yield negligible improvement.

It was thus concluded three set points will be sufficient for simple shape transitions, al-

though more complex transitions or different LQR cost-weighting matrices may alter this

observation.

Chapter 5 considered the possibility of controlling the quadrotor using a single evenly-

distributed thrust, and actuating vehicle roll and pitch using arm rotations in place of differ-

ential throttling. This was testing using discrete time nonlinear optimal controls calculated

using the fmincon constrained numerical optimizer in MATLAB. The simulations verified

the system to be controllable using arm angle pitch and roll actuation. The results showed

strong control authority, with magnitudes of arm movements highly dependent on target

flight duration. Short time periods produced optimal inputs beyond the reasonable capabili-

ties of physical actuators however, so it will be important to consider actuator saturation for

determining limits of operation in a physical prototype. Additionally, calculation times were

long and therefore the control strategy is incapable of being applied in real time, however

the nonlinear controls could be used to develop optimal target trajectories about which a

real-time controller could be flown. The code is readily adaptable to various conditions and

situations, and therefore shows potential for future studies.

6.2 Recommendations for Future Work

The dynamic model is mostly complete and allowed for considerable development and evalu-

ation of unique vehicle capabilities. The next step for further developing the model will be to

58

quantify the internal torques resulting from asymmetric shape changes, thus allowing the full

14 DOF dimensionality of the quadrotor to be modeled. Once this is accomplished, the next

major step to advance the dynamic model is to derive the model for the exact control chan-

nels to be used in the physical prototype, whether they be torques, velocities, voltage inputs,

or any combination thereof. The fidelity of the model can further be improved by accounting

for propeller dynamics including gyroscopic effects, lift interferences, and P-factor, as well

as accounting for drag forces acting on the body and propellers. A potential reference for

accounting these effects is [25], which develops a thorough dynamic model for a rigid-body

quadrotor.

The LQR controller was found to stabilize the quadrotor, however the controller did not

produce consistent returns to the origin. The first step necessary from this point is to identify

the root cause of the nonconvergence to the origin and address it. Once that is complete, the

LQR should be developed for additional flight maneuvers, beginning with simple trajectory

tracking. The linearization of the system in the trirotor configuration after recovery from

a motor loss should be scrutinized, as there is a suspected sign inversion resulting from an

yet to be identified error in the code. Further troubleshooting of this code should result in

controllability of the nonlinear trirotor model, as the controllability of the linearized model

was confirmed using the controllability matrix.

LQR Set Point Control showed promise for controlling the vehicle during shape transfor-

mations prescribed to meet mission objectives, and therefore should be further developed as

well. Similarly to the full model LQR, the controller was found to be stabilizing, but failed to

produce a return to the origin. If the source of this error is identified for the full model, the

correction will likely be similar in application to the set point model. Once the convergence

issue has been addressed, a wide variety of transition shapes and durations should be tested

to determine in which operations the three set-point result continues to be valid. Once a

strong understanding of set point requirements has been gathered, a library of gain sets

should be calculated for expected transitions between probable operating shapes, and the

gain library should be tested on a flying prototype using shape state feedback to determine

59

which gain set is applicable.

The nonlinear control optimizer and simulator scripts are working and are written to be

easily modifiable. They are ready for additional tests such as modeling sliding motor control,

differential throttling, and any other desired combination of inputs, although increasing the

number of input channels does lead to exponential growth in run time durations. The

numerical accuracy of the simulations can be improved by converting the trajectory function

to again use 4th order Runge-Kutta, possibly through MATLAB’s ode45 function, as well as

running the model at higher control resolutions than the 41 point models used in Chapter

5. Considering arm angle pitch and roll actuation of the vehicle, optimal trajectories can

continue to be mapped using the numerical solver, however real-time control for physical

prototyping could readily be achived through the implementation of LQR on the variable

arm angle model.

60

BIBLIOGRAPHY

[1] D. Boddington, Radio-controlled Model Aircraft. Crowood Press, 2004.

[2] R. Whittle, Predator: The Secret Origins of the Drone Revolution. Macmillan, 2014.

[3] U.S. Air Force, “RQ-11B Raven.” http://www.af.mil/AboutUs/FactSheets/Display/tabid/224/Article/104533/rq-11b-raven.aspx, October 2007.

[4] R. Varrier, “Drones are providing film and tv viewers a new perspective onthe action.” http://www.latimes.com/entertainment/envelope/cotown/la-et-ct-drones-hollywood-20151008-story.html, October 2015.

[5] Amazon, “Amazon prime air.” http://www.amazon.com/b?node=8037720011, Febru-ary 2016.

[6] DHL, “Dhl parcelcopter launches initial operations for research purposes.”http://www.dhl.com/en/press/releases, September 2014.

[7] BBC, “Google plans drone delivery service for 2017.”http://www.bbc.com/news/technology-34704868, November 2015.

[8] “Drone racing league.” http://thedroneracingleague.com/, February 2016.

[9] S. Bouabdallah, A. Noth, and R. Siegwart, “Pid vs lq control techniques applied toan indoor micro quadrotor,” in Intelligent Robots and Systems, 2004. (IROS 2004).Proceedings. 2004 IEEE/RSJ International Conference on, vol. 3, pp. 2451–2456 vol.3,Sept 2004.

[10] N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, “The grasp multiple micro-uavtestbed,” IEEE Robotics Automation Magazine, vol. 17, pp. 56–65, Sept 2010.

[11] D. Mellinger, N. Michael, M. Shomin, and V. Kumar, “Recent advances in quadrotorcapabilities,” in Robotics and Automation (ICRA), 2011 IEEE International Conferenceon, pp. 2964–2965, May 2011.

[12] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control forquadrotors,” in Robotics and Automation (ICRA), 2011 IEEE International Conferenceon, pp. 2520–2525, May 2011.

61

[13] S. Sadr, S. A. A. Moosavian, and P. Zarafshan, “Dynamics modeling and control of aquadrotor with swing load,” Journal of Robotics, vol. 2014, 2014.

[14] A. Faust, I. Palunko, P. Cruz, R. Fierro, and L. Tapia, “Learning swing-free trajectoriesfor uavs with a suspended load,” in Robotics and Automation (ICRA), 2013 IEEEInternational Conference on, pp. 4902–4909, May 2013.

[15] A. E. Jimenez-Cano, J. Martin, G. Heredia, A. Ollero, and R. Cano, “Control of an aerialrobot with multi-link arm for assembly tasks,” in Robotics and Automation (ICRA),2013 IEEE International Conference on, pp. 4916–4921, May 2013.

[16] J. Villagomez, M. Vargas, and F. Rubio, “Backstepping and sliding-mode techniquesapplied to an underactuated camera onboard a rotorcraft mav,” in 3rd Workshop onVisual Control of Mobile Robots, Vicomor, pp. 1–7, 2014.

[17] J. P. Dyhr, N. J. Cowan, D. J. Colmenares, K. A. Morgansen, and T. L. Daniel, “Au-tostabilizing airframe articulation: Animal inspired air vehicle control,” in Decision andControl (CDC), 2012 IEEE 51st Annual Conference on, pp. 3715–3720, IEEE, 2012.

[18] J. D. Becker, Modeling and Control of a Quadrotor with Dynamic Inertia. PhD thesis,2013.

[19] F. Senkul and E. Altug, “Modeling and control of a novel tilt - roll rotor quadrotor uav,”in Unmanned Aircraft Systems (ICUAS), 2013 International Conference on, pp. 1071–1076, May 2013.

[20] K. T. Oner, E. Cetinsoy, M. Unel, M. F. Aksit, I. Kandemir, and K. Gulez, “Dynamicmodel and control of a new quadrotor unmanned aerial vehicle with tilt-wing mecha-nism,” World Academy of Science, Engineering and Technology, vol. 45, 2008.

[21] S. Datta, “Multicopters with variable flight characteristics,” July 2015.

[22] L. M. Argentim, W. C. Rezende, P. E. Santos, and R. A. Aguiar, “Pid, lqr and lqr-pid ona quadcopter platform,” in Informatics, Electronics Vision (ICIEV), 2013 InternationalConference on, pp. 1–6, May 2013.

[23] M. W. Mueller and R. D’Andrea, “Stability and control of a quadrocopter despite thecomplete loss of one, two, or three propellers,” in Robotics and Automation (ICRA),2014 IEEE International Conference on, pp. 45–52, IEEE, 2014.

[24] D. W. Yoo, H. D. Oh, D. Y. Won, and M. J. Tahk, “Dynamic modeling and controlsystem design for tri-rotor uav,” in Systems and Control in Aeronautics and Astronautics(ISSCAA), 2010 3rd International Symposium on, pp. 762–767, June 2010.

62

[25] A. Tayebi and S. McGilvray, “Attitude stabilization of a vtol quadrotor aircraft,” IEEETransactions on control systems technology, vol. 14, no. 3, pp. 562–571, 2006.

63

Appendix A

FULL FORM DYNAMIC EQUATIONS

This appendix shows the full-form symbolic dynamic equations for the simplified sym-

metric model, including both the CG-frame and body-frame translational dynamics. All

equations were calculated in Mathematica using the process in Chapter 2.3 and 2.4.

A.1 ¨xCG

(sθ(−uA − uB − uC − uD))/(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

mmotC +mmotD)

A.2 ¨yCG

(cθsφ(uA + uB + uC + uD))/(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

mmotC +mmotD)

A.3 ¨zCG

g− (cθcφ(uA+uB +uC +uD))/(marmA+marmB +marmC +marmD +mbody +mmotA+mmotB +

mmotC +mmotD)

A.4 x

(−cθcψ(2mmotAdAcαφ−2mmotC dAcαφ−4mmotAdAαφsαφ +4mmotC dAαφsαφ−2mmotAdAαφsαφ−

2mmotAdAαφ2cαφ + 2mmotCdAαφsαφ + 2mmotCdAαφ

2cαφ + 2mmotBdBsαθ + 2mmotDdBsαθ +

4(mmotB + mmotD)dBαθcαθ + αθ2sαθ(−(2dB(mmotB + mmotD) + lBmarmB + lDmarmD)) +

2mmotBdBαθcαθ + 2mmotDdBαθcαθ − lAmarmAαφsαφ − lAmarmAαφ2cαφ + lBmarmBαθcαθ +

lCmarmCαφsαφ + lCmarmCαφ2cαφ + lDmarmDαθcαθ) + (sθcψsφ + sψcφ)(2mmotAdAsαφ +

64

2mmotC dAsαφ + 4mmotAdAαφcαφ + 4mmotC dAαφcαφ + 2mmotAdAαφcαφ − 2mmotAdAαφ2sαφ +

2mmotCdAαφcαφ − 2mmotCdAαφ2sαφ − 2mmotBdBcαθ + 2mmotDdBcαθ + 4(mmotB −

mmotD)dBαθsαθ + αθ2cαθ(2dB(mmotB −mmotD) + lBmarmB − lDmarmD) + 2mmotBdBαθsαθ −

2mmotDdBαθsαθ + lAmarmAαφcαφ − lAmarmAαφ2sαφ + lBmarmBαθsαθ + lCmarmCαφcαφ −

lCmarmCαφ2sαφ − lDmarmDαθsαθ)− 2sθ(uA + uB + uC + uD))/(2(marmA +marmB +marmC +

marmD +mbody +mmotA +mmotB +mmotC +mmotD))

A.5 y

(cθsψ(2mmotAdAcαφ − 2mmotC dAcαφ − 4mmotAdAαφsαφ + 4mmotC dAαφsαφ − 2mmotAdAαφsαφ −

2mmotAdAαφ2cαφ + 2mmotCdAαφsαφ + 2mmotCdAαφ

2cαφ + 2mmotBdBsαθ + 2mmotDdBsαθ +

4(mmotB + mmotD)dBαθcαθ + αθ2sαθ(−(2dB(mmotB + mmotD) + lBmarmB + lDmarmD)) +

2mmotBdBαθcαθ + 2mmotDdBαθcαθ − lAmarmAαφsαφ − lAmarmAαφ2cαφ + lBmarmBαθcαθ +

lCmarmCαφsαφ + lCmarmCαφ2cαφ + lDmarmDαθcαθ) + (cψcφ − sθsψsφ)(2mmotAdAsαφ +

2mmotC dAsαφ + 4mmotAdAαφcαφ + 4mmotC dAαφcαφ + 2mmotAdAαφcαφ − 2mmotAdAαφ2sαφ +

2mmotCdAαφcαφ − 2mmotCdAαφ2sαφ − 2mmotBdBcαθ + 2mmotDdBcαθ + 4(mmotB −

mmotD)dBαθsαθ + αθ2cαθ(2dB(mmotB −mmotD) + lBmarmB − lDmarmD) + 2mmotBdBαθsαθ −

2mmotDdBαθsαθ + lAmarmAαφcαφ − lAmarmAαφ2sαφ + lBmarmBαθsαθ + lCmarmCαφcαφ −

lCmarmCαφ2sαφ− lDmarmDαθsαθ)+2cθsφ(uA+uB +uC +uD))/(2(marmA+marmB +marmC +

marmD +mbody +mmotA +mmotB +mmotC +mmotD))

A.6 z

−(cθsφ(2mmotAdAsαφ +2mmotC dAsαφ +4mmotAdAαφcαφ +4mmotC dAαφcαφ +2mmotAdAαφcαφ−

2mmotAdAαφ2sαφ + 2mmotCdAαφcαφ − 2mmotCdAαφ

2sαφ − 2mmotBdBcαθ + 2mmotDdBcαθ +

4(mmotB − mmotD)dBαθsαθ + αθ2cαθ(2dB(mmotB − mmotD) + lBmarmB − lDmarmD) +

2mmotBdBαθsαθ − 2mmotDdBαθsαθ + lAmarmAαφcαφ − lAmarmAαφ2sαφ + lBmarmBαθsαθ +

lCmarmCαφcαφ − lCmarmCαφ2sαφ − lDmarmDαθsαθ))/(2(marmA + marmB + marmC +

marmD + mbody + mmotA + mmotB + mmotC + mmotD)) − (sθ(2mmotAdAcαφ − 2mmotC dAcαφ −

4mmotAdAαφsαφ + 4mmotC dAαφsαφ − 2mmotAdAαφsαφ − 2mmotAdAαφ2cαφ + 2mmotCdAαφsαφ +

65

2mmotCdAαφ2cαφ + 2mmotBdBsαθ + 2mmotDdBsαθ + 4(mmotB + mmotD)dBαθcαθ +

αθ2sαθ(−(2dB(mmotB+mmotD)+ lBmarmB+ lDmarmD))+2mmotBdBαθcαθ +2mmotDdBαθcαθ−

lAmarmAαφsαφ − lAmarmAαφ2cαφ + lBmarmBαθcαθ + lCmarmCαφsαφ + lCmarmCαφ

2cαφ +

lDmarmDαθcαθ))/(2(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +

mmotD)) + g − (cθcφ(uA + uB + uC + uD))/(marmA + marmB + marmC + marmD + mbody +

mmotA +mmotB +mmotC +mmotD)

A.7 φ

(12((uA + uB + uC + uD)((−bBuB + bDuD − (uB − uD)cαθdB + (uA + uC)dAsαφ)/(uA + uB +

uC +uD)− (−2bBmarmB− lBcαθmarmB +2bDmarmD−2bBmmotB +2bDmmotD + lDmarmDcαθ −

2(mmotB−mmotD)cαθdB + lAmarmAsαφ + lCmarmCsαφ + 2(mmotA +mmotC)dAsαφ)/(2(marmA +

marmB+marmC+marmD+mbody+mmotA+mmotB+mmotC+mmotD)))− 112

((6mmotB(2bDmarmA−

lAsαφmarmA + 2bBmarmB + 2bDmarmB + 2bDmarmC + 2bDmbody + 2bDmmotA + 2bBmmotB +

2bDmmotB + 2bDmmotC + lBmarmBcαθ − lDmarmDcαθ + 2(marmA +marmB +marmC +marmD +

mbody+mmotA+2mmotB+mmotC)cαθdB−lCmarmCsαφ−2(mmotA+mmotC)dAsαφ)(−2(mmotA+

mmotC)sαφ dA + 2(marmA + marmB + marmC + marmD + mbody + mmotA + 2mmotB +

mmotC)cαθ dB − lBmarmBsαθ αθ + lDmarmDsαθ αθ − 2(marmA + marmB + marmC + marmD +

mbody + mmotA + 2mmotB + mmotC)dBsαθ αθ − lAmarmAcαφαφ − lCmarmCcαφαφ − 2(mmotA +

mmotC)cαφdAαφ))/((marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +

mmotD)2) + (6mmotD(2bDmarmA − lAsαφmarmA + 2bBmarmB + 2bDmarmB + 2bDmarmC +

2bDmbody + 2bDmmotA + 2bBmmotB + 2bDmmotB + 2bDmmotC + lBmarmBcαθ − lDmarmDcαθ +

2(marmA+marmB+marmC +marmD+mbody+mmotA+2mmotB+mmotC)cαθdB− lCmarmCsαφ−

2(mmotA +mmotC)dAsαφ)(−2(mmotA +mmotC)sαφ dA + 2(marmA +marmB +marmC +marmD +

mbody + mmotA + 2mmotB + mmotC)cαθ dB − lBmarmBsαθ αθ + lDmarmDsαθ αθ − 2(marmA +

marmB + marmC + marmD + mbody + mmotA + 2mmotB + mmotC)dBsαθ αθ − lAmarmAcαφαφ −

lCmarmCcαφαφ− 2(mmotA +mmotC)cαφdAαφ))/((marmA +marmB +marmC +marmD +mbody +

mmotA + mmotB + mmotC + mmotD)2) + (6mbody(−2bBmarmB − lBcαθmarmB + 2bDmarmD −

2bBmmotB+2bDmmotD+ lDmarmDcαθ−2(mmotB−mmotD)cαθdB+ lAmarmAsαφ + lCmarmCsαφ +

66

2(mmotA+mmotC)dAsαφ)(2(mmotA+mmotC)sαφ dA−2(mmotB−mmotD)cαθ dB+lBmarmBsαθ αθ−

lDmarmDsαθ αθ + 2(mmotB −mmotD)dBsαθ αθ + lAmarmAcαφαφ + lCmarmCcαφαφ + 2(mmotA +

mmotC)cαφdAαφ))/((marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +

mmotD)2) + (6mmotA(−2bBmarmB − lBcαθmarmB + 2bDmarmD − 2bBmmotB + 2bDmmotD +

lDmarmDcαθ − 2(mmotB −mmotD)cαθdB + lAmarmAsαφ + lCmarmCsαφ − 2(marmA + marmB +

marmC +marmD +mbody +mmotB +mmotD)dAsαφ)(−2(marmA +marmB +marmC +marmD +

mbody +mmotB +mmotD)sαφ dA− 2(mmotB −mmotD)cαθ dB + lBmarmBsαθ αθ− lDmarmDsαθ αθ +

2(mmotB −mmotD)dBsαθ αθ + lAmarmAcαφαφ + lCmarmCcαφαφ− 2(marmA +marmB +marmC +

marmD +mbody +mmotB +mmotD)cαφdAαφ))/((marmA +marmB +marmC +marmD +mbody +

mmotA + mmotB + mmotC + mmotD)2) + (6mmotC(−2bBmarmB − lBcαθmarmB + 2bDmarmD −

2bBmmotB+2bDmmotD+ lDmarmDcαθ−2(mmotB−mmotD)cαθdB+ lAmarmAsαφ + lCmarmCsαφ−

2(marmA +marmB +marmC +marmD +mbody +mmotB +mmotD)dAsαφ)(−2(marmA +marmB +

marmC +marmD+mbody +mmotB +mmotD)sαφ dA−2(mmotB−mmotD)cαθ dB + lBmarmBsαθ αθ−

lDmarmDsαθ αθ + 2(mmotB −mmotD)dBsαθ αθ + lAmarmAcαφαφ + lCmarmCcαφαφ − 2(marmA +

marmB +marmC +marmD +mbody +mmotB +mmotD)cαφdAαφ))/((marmA +marmB +marmC +

marmD+mbody+mmotA+mmotB +mmotC +mmotD)2)+marmD(6(2bD+ lDcαθ− (−2bBmarmB−

lBcαθmarmB + 2bDmarmD−2bBmmotB + 2bDmmotD + lDmarmDcαθ −2(mmotB−mmotD)cαθdB +

lAmarmAsαφ + lCmarmCsαφ + 2(mmotA +mmotC)dAsαφ)/(marmA +marmB +marmC +marmD +

mbody +mmotA+mmotB +mmotC +mmotD))(−lDsαθ αθ− (2(mmotA+mmotC)sαφ dA−2(mmotB−

mmotD)cαθ dB+lBmarmBsαθ αθ−lDmarmDsαθ αθ+2(mmotB−mmotD)dBsαθ αθ+lAmarmAcαφαφ+

lCmarmCcαφαφ + 2(mmotA +mmotC)cαφdAαφ)/(marmA +marmB +marmC +marmD +mbody +

mmotA+mmotB+mmotC+mmotD))− lD2s2αθ αθ)+marmA(s2αφαφlA2+6(lAsαφ−(−2bBmarmB−


lAmarmAsαφ + lCmarmCsαφ + 2(mmotA +mmotC)dAsαφ)/(marmA +marmB +marmC +marmD +

mbody +mmotA +mmotB +mmotC +mmotD))(lAcαφαφ− (2(mmotA +mmotC)sαφ dA− 2(mmotB −

mmotD)cαθ dB+lBmarmBsαθ αθ−lDmarmDsαθ αθ+2(mmotB−mmotD)dBsαθ αθ+lAmarmAcαφαφ+

lCmarmCcαφαφ + 2(mmotA +mmotC)cαφdAαφ)/(marmA +marmB +marmC +marmD +mbody +

mmotA+mmotB+mmotC+mmotD)))+marmC(s2αφαφlC2+6(lCsαφ−(−2bBmarmB−lBcαθmarmB+

67

2bDmarmD−2bBmmotB + 2bDmmotD + lDmarmDcαθ −2(mmotB−mmotD)cαθdB + lAmarmAsαφ +

lCmarmCsαφ +2(mmotA+mmotC)dAsαφ)/(marmA+marmB+marmC +marmD+mbody+mmotA+

mmotB +mmotC +mmotD))(lCcαφαφ − (2(mmotA +mmotC)sαφ dA − 2(mmotB −mmotD)cαθ dB +

lBmarmBsαθ αθ−lDmarmDsαθ αθ+2(mmotB−mmotD)dBsαθ αθ+lAmarmAcαφαφ+lCmarmCcαφαφ+

2(mmotA +mmotC)cαφdAαφ)/(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

mmotC + mmotD))) + 12marmB((bB + 12lBcαθ + (−2bBmarmB − lBcαθmarmB + 2bDmarmD −


2(mmotA +mmotC)dAsαφ)/(2(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

mmotC + mmotD)))((2(mmotA + mmotC)sαφ dA − 2(mmotB − mmotD)cαθ dB + lBmarmBsαθ αθ −

lDmarmDsαθ αθ + 2(mmotB −mmotD)dBsαθ αθ + lAmarmAcαφαφ + lCmarmCcαφαφ + 2(mmotA +

mmotC)cαφdAαφ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD) − lBsαθ αθ) − 16lB

2cαθsαθ αθ))φ))/((3mmotB(2bDmarmA − lAsαφmarmA + 2bBmarmB +

2bDmarmB + 2bDmarmC + 2bDmbody + 2bDmmotA + 2bBmmotB + 2bDmmotB + 2bDmmotC +

lBmarmBcαθ− lDmarmDcαθ +2(marmA+marmB +marmC +marmD +mbody +mmotA+2mmotB +

mmotC)cαθdB−lCmarmCsαφ−2(mmotA+mmotC)dAsαφ)2)/((marmA+marmB+marmC+marmD+

mbody+mmotA+mmotB+mmotC+mmotD)2)+(3mmotD(2bDmarmA−lAsαφmarmA+2bBmarmB+

2bDmarmB + 2bDmarmC + 2bDmbody + 2bDmmotA + 2bBmmotB + 2bDmmotB + 2bDmmotC +

lBmarmBcαθ− lDmarmDcαθ +2(marmA+marmB +marmC +marmD +mbody +mmotA+2mmotB +

mmotC)cαθdB−lCmarmCsαφ−2(mmotA+mmotC)dAsαφ)2)/((marmA+marmB+marmC+marmD+

mbody+mmotA+mmotB+mmotC+mmotD)2)+(3mmotA(−2bBmarmB−lBcαθmarmB+2bDmarmD−

2bBmmotB+2bDmmotD+ lDmarmDcαθ−2(mmotB−mmotD)cαθdB+ lAmarmAsαφ + lCmarmCsαφ−

2(marmA +marmB +marmC +marmD +mbody +mmotB +mmotD)dAsαφ)2)/((marmA +marmB +

marmC + marmD + mbody + mmotA + mmotB + mmotC + mmotD)2) + (3mmotC(−2bBmarmB −


lAmarmAsαφ + lCmarmCsαφ − 2(marmA + marmB + marmC + marmD + mbody + mmotB +

mmotD)dAsαφ)2)/((marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)2) +mbody(hbody2 + 3ρbody

2 + (3(−2bBmarmB − lBcαθmarmB + 2bDmarmD− 2bBmmotB +

2bDmmotD + lDmarmDcαθ −2(mmotB−mmotD)cαθdB + lAmarmAsαφ + lCmarmCsαφ + 2(mmotA +

68

mmotC)dAsαφ)2)/((marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)2)) + marmD(lD2c2αθ + 3(2bD + lDcαθ − (−2bBmarmB − lBcαθmarmB + 2bDmarmD −


2(mmotA + mmotC)dAsαφ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB +

mmotC + mmotD))2) + marmA(lA2s2αφ + 3(lAsαφ − (−2bBmarmB − lBcαθmarmB + 2bDmarmD −


2(mmotA + mmotC)dAsαφ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB +

mmotC + mmotD))2) + marmC(lC2s2αφ + 3(lCsαφ − (−2bBmarmB − lBcαθmarmB + 2bDmarmD −


2(mmotA+mmotC)dAsαφ)/(marmA+marmB+marmC+marmD+mbody+mmotA+mmotB+mmotC+

mmotD))2) + 12marmB( 112lB

2c2αθ + (bB + 12lBcαθ + (−2bBmarmB − lBcαθmarmB + 2bDmarmD −


2(mmotA +mmotC)dAsαφ)/(2(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

mmotC +mmotD)))2))

A.8 θ

(12((uA+uB +uC +uD)((bAuA− bCuC + (uA−uC)cαφdA+ (uB +uD)dBsαθ)/(uA+uB +uC +

uD)−(2bAmarmA+lAcαφmarmA−2bCmarmC+2bAmmotA−2bCmmotC−lCmarmCcαφ+2(mmotA−

mmotC)cαφdA + lBmarmBsαθ + lDmarmDsαθ + 2(mmotB +mmotD)dBsαθ)/(2(marmA +marmB +

marmC + marmD + mbody + mmotA + mmotB + mmotC + mmotD))) − 112

((6mmotB(2bAmarmA +

lAcαφmarmA− 2bCmarmC + 2bAmmotA− 2bCmmotC − lCmarmCcαφ + 2(mmotA−mmotC)cαφdA +

lBmarmBsαθ + lDmarmDsαθ − 2(marmA + marmB + marmC + marmD + mbody + mmotA +

mmotC)dBsαθ)(2(mmotA − mmotC)cαφ dA − 2(marmA + marmB + marmC + marmD + mbody +

mmotA + mmotC)sαθ dB + lBmarmBcαθ αθ + lDmarmDcαθ αθ − 2(marmA + marmB + marmC +

marmD + mbody + mmotA + mmotC)cαθdBαθ − lAmarmAsαφαφ + lCmarmCsαφαφ − 2(mmotA −

mmotC)dAsαφαφ))/((marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +

mmotD)2) + (6mmotD(2bAmarmA + lAcαφmarmA − 2bCmarmC + 2bAmmotA − 2bCmmotC −

lCmarmCcαφ + 2(mmotA −mmotC)cαφdA + lBmarmBsαθ + lDmarmDsαθ − 2(marmA + marmB +

69

marmC + marmD + mbody + mmotA + mmotC)dBsαθ)(2(mmotA − mmotC)cαφ dA − 2(marmA +

marmB +marmC +marmD +mbody +mmotA +mmotC)sαθ dB + lBmarmBcαθ αθ + lDmarmDcαθ αθ−

2(marmA + marmB + marmC + marmD + mbody + mmotA + mmotC)cαθdBαθ − lAmarmAsαφαφ +

lCmarmCsαφαφ− 2(mmotA−mmotC)dAsαφαφ))/((marmA +marmB +marmC +marmD +mbody +

mmotA + mmotB + mmotC + mmotD)2) + (6mbody(2bAmarmA + lAcαφmarmA − 2bCmarmC +

2bAmmotA−2bCmmotC− lCmarmCcαφ +2(mmotA−mmotC)cαφdA+ lBmarmBsαθ + lDmarmDsαθ +

2(mmotB+mmotD)dBsαθ)(2(mmotA−mmotC)cαφ dA+2(mmotB+mmotD)sαθ dB+lBmarmBcαθ αθ+

lDmarmDcαθ αθ + 2(mmotB + mmotD)cαθdBαθ − lAmarmAsαφαφ + lCmarmCsαφαφ − 2(mmotA −

mmotC)dAsαφαφ))/((marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +

mmotD)2) + (6mmotC(2bAmarmA + 2bCmarmA + lAcαφmarmA + 2bCmarmB + 2bCmarmD +

2bCmbody+2bAmmotA+2bCmmotA+2bCmmotB+2bCmmotD−lCmarmCcαφ+2(marmA+marmB+

marmC + marmD + mbody + 2mmotA + mmotB + mmotD)cαφdA + lBmarmBsαθ + lDmarmDsαθ +

2(mmotB +mmotD)dBsαθ)(2(marmA +marmB +marmC +marmD +mbody + 2mmotA +mmotB +

mmotD)cαφ dA + 2(mmotB + mmotD)sαθ dB + lBmarmBcαθ αθ + lDmarmDcαθ αθ + 2(mmotB +

mmotD)cαθdBαθ − lAmarmAsαφαφ + lCmarmCsαφαφ − 2(marmA + marmB + marmC + marmD +

mbody + 2mmotA +mmotB +mmotD)dAsαφαφ))/((marmA +marmB +marmC +marmD +mbody +

mmotA + mmotB + mmotC + mmotD)2) + 24mmotA(bA + cαφdA − (2bAmarmA + lAcαφmarmA −

2bCmarmC + 2bAmmotA− 2bCmmotC − lCmarmCcαφ + 2(mmotA−mmotC)cαφdA + lBmarmBsαθ +

lDmarmDsαθ + 2(mmotB + mmotD)dBsαθ)/(2(marmA + marmB + marmC + marmD + mbody +

mmotA+mmotB +mmotC +mmotD)))(cαφ dA−dAsαφαφ− (2(mmotA−mmotC)cαφ dA+2(mmotB +

mmotD)sαθ dB+lBmarmBcαθ αθ+lDmarmDcαθ αθ+2(mmotB+mmotD)cαθdBαθ−lAmarmAsαφαφ+

lCmarmCsαφαφ− 2(mmotA−mmotC)dAsαφαφ)/(2(marmA +marmB +marmC +marmD +mbody +

mmotA+mmotB+mmotC+mmotD)))+marmB(s2αθ αθlB2+6(lBsαθ−(2bAmarmA+ lAcαφmarmA−

2bCmarmC + 2bAmmotA− 2bCmmotC − lCmarmCcαφ + 2(mmotA−mmotC)cαφdA + lBmarmBsαθ +

lDmarmDsαθ +2(mmotB+mmotD)dBsαθ)/(marmA+marmB+marmC +marmD+mbody+mmotA+

mmotB +mmotC +mmotD))(lBcαθ αθ − (2(mmotA −mmotC)cαφ dA + 2(mmotB +mmotD)sαθ dB +

lBmarmBcαθ αθ+lDmarmDcαθ αθ+2(mmotB+mmotD)cαθdBαθ−lAmarmAsαφαφ+lCmarmCsαφαφ−

2(mmotA −mmotC)dAsαφαφ)/(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

70

mmotC + mmotD))) + marmD(s2αθ αθlD2 + 6(lDsαθ − (2bAmarmA + lAcαφmarmA − 2bCmarmC +


2(mmotB+mmotD)dBsαθ)/(marmA+marmB+marmC+marmD+mbody+mmotA+mmotB+mmotC+

mmotD))(lDcαθ αθ − (2(mmotA − mmotC)cαφ dA + 2(mmotB + mmotD)sαθ dB + lBmarmBcαθ αθ +


mmotC)dAsαφαφ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD))) + 12marmA(2(bA + 12lAcαφ − (2bAmarmA + lAcαφmarmA − 2bCmarmC + 2bAmmotA −

2bCmmotC − lCmarmCcαφ + 2(mmotA−mmotC)cαφdA + lBmarmBsαθ + lDmarmDsαθ + 2(mmotB +

mmotD)dBsαθ)/(2(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)))(−12lAsαφαφ−(2(mmotA−mmotC)cαφ dA+2(mmotB +mmotD)sαθ dB + lBmarmBcαθ αθ+


mmotC)dAsαφαφ)/(2(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +

mmotD)))− 16lA

2cαφsαφαφ) + 12marmC((bC + 12lCcαφ + (2bAmarmA+ lAcαφmarmA−2bCmarmC +


2(mmotB +mmotD)dBsαθ)/(2(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +

mmotC + mmotD)))((2(mmotA − mmotC)cαφ dA + 2(mmotB + mmotD)sαθ dB + lBmarmBcαθ αθ +


mmotC)dAsαφαφ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD) − lCsαφαφ) − 16lC

2cαφsαφαφ))θ))/((3mmotB(2bAmarmA + lAcαφmarmA − 2bCmarmC +

2bAmmotA−2bCmmotC− lCmarmCcαφ +2(mmotA−mmotC)cαφdA+ lBmarmBsαθ + lDmarmDsαθ−

2(marmA +marmB +marmC +marmD +mbody +mmotA +mmotC)dBsαθ)2)/((marmA +marmB +

marmC + marmD + mbody + mmotA + mmotB + mmotC + mmotD)2) + (3mmotD(2bAmarmA +

lAcαφmarmA− 2bCmarmC + 2bAmmotA− 2bCmmotC − lCmarmCcαφ + 2(mmotA−mmotC)cαφdA +

lBmarmBsαθ + lDmarmDsαθ − 2(marmA + marmB + marmC + marmD + mbody + mmotA +

mmotC)dBsαθ)2)/((marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)2)+(3mmotC(2bAmarmA+2bCmarmA+lAcαφmarmA+2bCmarmB+2bCmarmD+2bCmbody+

2bAmmotA + 2bCmmotA + 2bCmmotB + 2bCmmotD− lCmarmCcαφ + 2(marmA +marmB +marmC +

marmD +mbody + 2mmotA +mmotB +mmotD)cαφdA + lBmarmBsαθ + lDmarmDsαθ + 2(mmotB +

71

mmotD)dBsαθ)2)/((marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)2) + 12mmotA(bA + cαφdA − (2bAmarmA + lAcαφmarmA − 2bCmarmC + 2bAmmotA −


mmotD)dBsαθ)/(2(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)))2 +mbody(hbody2 + 3ρbody

2 + (3(2bAmarmA + lAcαφmarmA − 2bCmarmC + 2bAmmotA −


mmotD)dBsαθ)2)/((marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD)2)) +marmB(lB2s2αθ + 3(lBsαθ − (2bAmarmA + lAcαφmarmA − 2bCmarmC + 2bAmmotA −


mmotD)dBsαθ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD))2) +marmD(lD2s2αθ + 3(lDsαθ − (2bAmarmA + lAcαφmarmA − 2bCmarmC + 2bAmmotA −


mmotD)dBsαθ)/(marmA + marmB + marmC + marmD + mbody + mmotA + mmotB + mmotC +

mmotD))2) + 12marmA( 112lA

2c2αφ + (bA + 12lAcαφ − (2bAmarmA + lAcαφmarmA − 2bCmarmC +



mmotC+mmotD)))2)+12marmC( 112lC

2c2αφ+(bC+12lCcαφ+(2bAmarmA+lAcαφmarmA−2bCmarmC+



mmotC +mmotD)))2))

A.9 ψ

(12(marmA +marmB +marmC +marmD +mbody +mmotA +mmotB +mmotC +mmotD)(0.5(uA−

uB − uD) + ((8mmotAmmotCs2αφαφd2A − 4(marmBmmotA + marmCmmotA + marmDmmotA +

mbodymmotA + mmotBmmotA + 2mmotCmmotA + mmotDmmotA + 2mmotCc2αφmmotA +

marmBmmotC + marmCmmotC + marmDmmotC + mbodymmotC + mmotBmmotC +

marmA(mmotA + mmotC) + mmotCmmotD)dAdA + 2(2(mmotB + mmotD)(mmotAsαθ+αφ −

mmotCsαθ−αφ)dB + lBmarmBmmotAcαθ−αφ(αθ − αφ) − lDmarmDmmotCcαθ−αφ(αθ −

72

αφ) − 2bBmarmBmmotAcαφαφ + 2bDmarmDmmotAcαφαφ + 2bDmmotAmmotBcαφαφ −

2bBmarmBmmotCcαφαφ + 2bDmarmDmmotCcαφαφ + 2bDmmotBmmotCcαφαφ +

2bDmmotAmmotDcαφαφ + 2bDmmotCmmotDcαφαφ + 2(bA(marmBmmotA + marmCmmotA +

marmDmmotA +mbodymmotA +mmotBmmotA + 2mmotCmmotA +mmotDmmotA +marmAmmotC) +

bC(marmCmmotA + mmotC(marmA + marmB + marmD + mbody + 2mmotA + mmotB +

mmotD)))sαφαφ+2lCmarmCmmotAs2αφαφ+2lAmarmAmmotCs2αφαφ+lDmarmDmmotAcαθ+αφ(αθ+

αφ) − lBmarmBmmotCcαθ+αφ(αθ + αφ) + 2(mmotB + mmotD)dB(mmotAcαθ+αφ(αθ + αφ) −

mmotCcαθ−αφ(αθ− αφ)))dA+2(lAmarmAmmotA− lCmarmCc2αφmmotA+ lBmarmBsαθ−αφmmotA−

2bBmarmBsαφmmotA + 2bDmarmDsαφmmotA + 2bDmmotBsαφmmotA + 2bDmmotDsαφmmotA +

lDmarmDsαθ+αφmmotA+ lCmarmCmmotC−2(bA(marmBmmotA+marmCmmotA+marmDmmotA+

mbodymmotA+mmotBmmotA+2mmotCmmotA+mmotDmmotA+marmAmmotC)+bC(marmCmmotA+

mmotC(marmA+marmB+marmD+mbody+2mmotA+mmotB+mmotD)))cαφ−lAmarmAmmotCc2αφ−

lDmarmDmmotCsαθ−αφ − 2bBmarmBmmotCsαφ + 2bDmarmDmmotCsαφ + 2bDmmotBmmotCsαφ +

2bDmmotCmmotDsαφ − lBmarmBmmotCsαθ+αφ + 2(mmotB + mmotD)dB(mmotAsαθ+αφ −

mmotCsαθ−αφ))dA − 4(2c2αθmmotB2 + 2mmotB

2 + marmBmmotB + marmCmmotB +

marmDmmotB +mbodymmotB +mmotAmmotB +mmotCmmotB +marmBmmotD +marmCmmotD +

marmDmmotD +mbodymmotD +mmotAmmotD +mmotCmmotD +marmA(mmotB +mmotD))dBdB +

2((mmotB + mmotD)(lDmarmD − lBmarmBc2αθ + 2(bA(marmA + mmotA) − bC(marmC +

mmotC))sαθ − lCmarmCsαθ−αφ + lAmarmAsαθ+αφ) − 2(bB(2mmotB2 + marmB(mmotB +

mmotD)) + bD(2mmotB2 + marmCmmotB + mbodymmotB + mmotAmmotB + mmotCmmotB +

marmCmmotD + mbodymmotD + mmotAmmotD + mmotCmmotD + marmA(mmotB + mmotD) +

marmB(mmotB + mmotD)))cαθ)dB + 2bAlBmarmAmarmBcαθ αθ − 2bC lBmarmBmarmCcαθ αθ +

2bAlDmarmAmarmDcαθ αθ − 2bC lDmarmCmarmDcαθ αθ + 2bAlBmarmBmmotAcαθ αθ +

2bAlDmarmDmmotAcαθ αθ − 2bC lBmarmBmmotCcαθ αθ − 2bC lDmarmDmmotCcαθ αθ +

2bBlBmarmAmarmBsαθ αθ + 2bBlBmarmBmarmCsαθ αθ + 2bDlDmarmAmarmDsαθ αθ +

2bBlBmarmBmarmDsαθ αθ + 2bDlBmarmBmarmDsαθ αθ + 2bBlDmarmBmarmDsαθ αθ +

2bDlDmarmBmarmDsαθ αθ + 2bDlDmarmCmarmDsαθ αθ + 2bBlBmarmBmbodysαθ αθ +

2bDlDmarmDmbodysαθ αθ + 2bBlBmarmBmmotAsαθ αθ + 2bDlDmarmDmmotAsαθ αθ +

73

2bBlBmarmBmmotBsαθ αθ + 2bDlBmarmBmmotBsαθ αθ + 2bBlBmarmBmmotCsαθ αθ +

2bDlDmarmDmmotCsαθ αθ + 2bBlBmarmBmmotDsαθ αθ + 2bDlBmarmBmmotDsαθ αθ +

8mmotB2d2Bs2αθ αθ + 2lBlDmarmBmarmDs2αθ αθ + lAlBmarmAmarmBcαθ−αφ(αθ − αφ) −

lC lDmarmCmarmDcαθ−αφ(αθ − αφ) − 2bBlAmarmAmarmBcαφαφ − 2bBlCmarmBmarmCcαφαφ +

2bDlAmarmAmarmDcαφαφ + 2bDlCmarmCmarmDcαφαφ + 2bDlAmarmAmmotBcαφαφ +

2bDlCmarmCmmotBcαφαφ + 2bDlAmarmAmmotDcαφαφ + 2bDlCmarmCmmotDcαφαφ +

2bAlAmarmAmarmBsαφαφ + 2bAlAmarmAmarmCsαφαφ + 2bC lAmarmAmarmCsαφαφ +

2bAlCmarmAmarmCsαφαφ + 2bC lCmarmAmarmCsαφαφ + 2bC lCmarmBmarmCsαφαφ +

2bAlAmarmAmarmDsαφαφ + 2bC lCmarmCmarmDsαφαφ + 2bAlAmarmAmbodysαφαφ +

2bC lCmarmCmbodysαφαφ + 2bAlCmarmCmmotAsαφαφ + 2bC lCmarmCmmotAsαφαφ +

2bAlAmarmAmmotBsαφαφ + 2bC lCmarmCmmotBsαφαφ + 2bAlAmarmAmmotCsαφαφ +

2bC lAmarmAmmotCsαφαφ + 2bAlAmarmAmmotDsαφαφ + 2bC lCmarmCmmotDsαφαφ +

2lAlCmarmAmarmCs2αφαφ− lBlCmarmBmarmCcαθ+αφ(αθ + αφ) + lAlDmarmAmarmDcαθ+αφ(αθ +

αφ) + 2dB(2(bB(2mmotB2 + marmB(mmotB + mmotD)) + bD(2mmotB

2 + marmCmmotB +

mbodymmotB + mmotAmmotB + mmotCmmotB + marmCmmotD + mbodymmotD + mmotAmmotD +

mmotCmmotD + marmA(mmotB + mmotD) + marmB(mmotB + mmotD)))sαθ αθ + (mmotB +

mmotD)(2(bA(marmA + mmotA) − bC(marmC + mmotC))cαθ αθ + 2lBmarmBs2αθ αθ −

lCmarmCcαθ−αφ(αθ−αφ)+lAmarmAcαθ+αφ(αθ+αφ))))ψ)/(2(marmA+marmB+marmC+marmD+

mbody + mmotA + mmotB + mmotC + mmotD))))/(12marmAmarmBbA2 + 12marmAmarmCbA

2 +

12marmAmarmDbA2 + 12marmAmbodybA

2 + 12marmBmmotAbA2 + 12marmCmmotAbA

2 +

12marmDmmotAbA2 + 12mbodymmotAbA

2 + 12marmAmmotBbA2 + 12mmotAmmotBbA

2 +

12marmAmmotCbA2 + 12mmotAmmotCbA

2 + 12marmAmmotDbA2 + 12mmotAmmotDbA

2 +

24bCmarmAmarmCbA + 24bCmarmCmmotAbA + 24bCmarmAmmotCbA + 24bCmmotAmmotCbA +

12lAmarmAmarmBcαφbA + 12lAmarmAmarmCcαφbA + 12lCmarmAmarmCcαφbA +

12lAmarmAmarmDcαφbA + 12lAmarmAmbodycαφbA + 12lCmarmCmmotAcαφbA +

12lAmarmAmmotBcαφbA + 12lAmarmAmmotCcαφbA + 12lAmarmAmmotDcαφbA −

12lBmarmAmarmBsαθbA − 12lDmarmAmarmDsαθbA − 12lBmarmBmmotAsαθbA −

12lDmarmDmmotAsαθbA+ lA2marmA

2 + lB2marmB

2 + lC2marmC

2 + lD2marmD

2 +12bB2mmotB

2 +

74

12bD2mmotB

2 + 24bBbDmmotB2 + 6mbody

2ρbody2 + 6marmAmbodyρbody

2 + 6marmBmbodyρbody2 +

6marmCmbodyρbody2 + 6marmDmbodyρbody

2 + 6mbodymmotAρbody2 + 6mbodymmotBρbody

2 +

6mbodymmotCρbody2 + 6mbodymmotDρbody

2 + 12(marmBmmotA +marmCmmotA +marmDmmotA +

mbodymmotA+mmotBmmotA+2mmotCmmotA+mmotDmmotA+2mmotCc2αφmmotA+marmBmmotC+

marmCmmotC + marmDmmotC + mbodymmotC + mmotBmmotC + marmA(mmotA + mmotC) +

mmotCmmotD)d2A+12(2c2αθmmotB2+2mmotB

2+marmBmmotB+marmCmmotB+marmDmmotB+

mbodymmotB +mmotAmmotB +mmotCmmotB +marmBmmotD +marmCmmotD +marmDmmotD +

mbodymmotD+mmotAmmotD+mmotCmmotD+marmA(mmotB+mmotD))d2B+12bB2marmAmarmB+

4lA2marmAmarmB + 4lB

2marmAmarmB + 12bC2marmAmarmC + 4lA

2marmAmarmC +

4lC2marmAmarmC + 12bB

2marmBmarmC + 12bC2marmBmarmC + 4lB

2marmBmarmC +

4lC2marmBmarmC + 12bD

2marmAmarmD + 4lA2marmAmarmD + 4lD

2marmAmarmD +

12bB2marmBmarmD + 12bD

2marmBmarmD + 4lB2marmBmarmD + 4lD

2marmBmarmD +

24bBbDmarmBmarmD + 12bC2marmCmarmD + 12bD

2marmCmarmD + 4lC2marmCmarmD +

4lD2marmCmarmD + 4lA

2marmAmbody + 12bB2marmBmbody + 4lB

2marmBmbody +

12bC2marmCmbody + 4lC

2marmCmbody + 12bD2marmDmbody + 4lD

2marmDmbody +

4lA2marmAmmotA + 12bB

2marmBmmotA + 4lB2marmBmmotA + 12bC

2marmCmmotA +

4lC2marmCmmotA + 12bD

2marmDmmotA + 4lD2marmDmmotA + 12bD

2marmAmmotB +

4lA2marmAmmotB + 12bB

2marmBmmotB + 12bD2marmBmmotB + 4lB

2marmBmmotB +

24bBbDmarmBmmotB + 12bC2marmCmmotB + 12bD

2marmCmmotB + 4lC2marmCmmotB +

4lD2marmDmmotB + 12bD

2mbodymmotB + 12bD2mmotAmmotB + 12bC

2marmAmmotC +

4lA2marmAmmotC + 12bB

2marmBmmotC + 12bC2marmBmmotC + 4lB

2marmBmmotC +

4lC2marmCmmotC + 12bC

2marmDmmotC + 12bD2marmDmmotC + 4lD

2marmDmmotC +

12bC2mbodymmotC + 12bC

2mmotAmmotC + 12bC2mmotBmmotC + 12bD

2mmotBmmotC +

12bD2marmAmmotD + 4lA

2marmAmmotD + 12bB2marmBmmotD + 12bD

2marmBmmotD +

4lB2marmBmmotD + 24bBbDmarmBmmotD + 12bC

2marmCmmotD + 12bD2marmCmmotD +

4lC2marmCmmotD + 4lD

2marmDmmotD + 12bD2mbodymmotD + 12bD

2mmotAmmotD +

12bC2mmotCmmotD + 12bD

2mmotCmmotD + 12bBlBmarmAmarmBcαθ + 12bBlBmarmBmarmCcαθ +

12bDlDmarmAmarmDcαθ + 12bBlBmarmBmarmDcαθ + 12bDlBmarmBmarmDcαθ +

75

12bBlDmarmBmarmDcαθ + 12bDlDmarmBmarmDcαθ + 12bDlDmarmCmarmDcαθ +

12bBlBmarmBmbodycαθ + 12bDlDmarmDmbodycαθ + 12bBlBmarmBmmotAcαθ +

12bDlDmarmDmmotAcαθ + 12bBlBmarmBmmotBcαθ + 12bDlBmarmBmmotBcαθ +

12bBlBmarmBmmotCcαθ + 12bDlDmarmDmmotCcαθ + 12bBlBmarmBmmotDcαθ +

12bDlBmarmBmmotDcαθ + 6lBlDmarmBmarmDc2αθ + 12bC lAmarmAmarmCcαφ +

12bC lCmarmAmarmCcαφ + 12bC lCmarmBmarmCcαφ + 12bC lCmarmCmarmDcαφ +

12bC lCmarmCmbodycαφ + 12bC lCmarmCmmotAcαφ + 12bC lCmarmCmmotBcαφ +

12bC lAmarmAmmotCcαφ + 12bC lCmarmCmmotDcαφ + 6lAlCmarmAmarmCc2αφ +

12bC lBmarmBmarmCsαθ + 12bC lDmarmCmarmDsαθ + 12bC lBmarmBmmotCsαθ +

12bC lDmarmDmmotCsαθ − 6lAlBmarmAmarmBsαθ−αφ + 6lC lDmarmCmarmDsαθ−αφ +

12bBlAmarmAmarmBsαφ + 12bBlCmarmBmarmCsαφ − 12bDlAmarmAmarmDsαφ −

12bDlCmarmCmarmDsαφ − 12bDlAmarmAmmotBsαφ − 12bDlCmarmCmmotBsαφ −

12bDlAmarmAmmotDsαφ − 12bDlCmarmCmmotDsαφ + 6lBlCmarmBmarmCsαθ+αφ −

6lAlDmarmAmarmDsαθ+αφ − 12dB((mmotB +mmotD)(lDmarmD − lBmarmBc2αθ + 2(bA(marmA +

mmotA) − bC(marmC + mmotC))sαθ − lCmarmCsαθ−αφ + lAmarmAsαθ+αφ) − 2(bB(2mmotB2 +

marmB(mmotB + mmotD)) + bD(2mmotB2 + marmCmmotB + mbodymmotB + mmotAmmotB +

mmotCmmotB +marmCmmotD +mbodymmotD +mmotAmmotD +mmotCmmotD +marmA(mmotB +

mmotD) + marmB(mmotB + mmotD)))cαθ) − 12dA(lAmarmAmmotA − lCmarmCc2αφmmotA +

lBmarmBsαθ−αφmmotA − 2bBmarmBsαφmmotA + 2bDmarmDsαφmmotA + 2bDmmotBsαφmmotA +

2bDmmotDsαφmmotA + lDmarmDsαθ+αφmmotA + lCmarmCmmotC − 2(bA(marmBmmotA +

marmCmmotA +marmDmmotA +mbodymmotA +mmotBmmotA + 2mmotCmmotA +mmotDmmotA +

marmAmmotC) + bC(marmCmmotA + mmotC(marmA + marmB + marmD + mbody + 2mmotA +

mmotB +mmotD)))cαφ − lAmarmAmmotCc2αφ − lDmarmDmmotCsαθ−αφ − 2bBmarmBmmotCsαφ +

2bDmarmDmmotCsαφ + 2bDmmotBmmotCsαφ + 2bDmmotCmmotDsαφ − lBmarmBmmotCsαθ+αφ +

2(mmotB +mmotD)dB(mmotAsαθ+αφ −mmotCsαθ−αφ)))

Dynamics and Control of a Quadrotor with Active Geometric ...

Documents

Transcript of Dynamics and Control of a Quadrotor with Active Geometric ...