2016 l9 quadrotor control - eth z...robot dynamics -rotary wing uas: control of a quadrotor...

||Autonomous Systems Lab

151-0851-00 V

Marco Hutter, Roland Siegwart and Thomas Stastny

07.11.2016Robot Dynamics - Rotary Wing UAS: Control of a Quadrotor 1

Robot DynamicsRotary Wing UAS: Control of a Quadrotor


1. Introduction - Design and Propeller Aerodynamics

2. Propeller Analysis and Dynamic Modeling

3. Control of a Quadrotor

4. Rotor Craft Case Study


Contents | Rotary Wing UAS

||Autonomous Systems Lab 07.11.2016Robot Dynamics - Rotary Wing UAS: Control of a Quadrotor 3

Multi-body Dynamics | General Formulation

Generalized coordinates Mass matrix

, Centrifugal and Coriolis forces

Gravity forces Actuation torque

External forces (end-effector, act

ex

b

g

F

M

ground contact, propeller thrust, ...)

Jacobian of external forces Selection matrix of actuated jointsexJ

S

, T Tex ex actb g F M J S


Controller SynthesisRotary Wing UAS: Control of a Quadrotor



Dynamic stability Calculate the operational point (equilibrium point) Derivative of the states must be zero (static point)

Linearize the dynamic equations

Calculate the eigenvalues of the state matrix A

Exponentially stable if Stable if


Stability Analysis

Exponentially stable equilibrium

Unstable equilibrium

Stable equilibrium

0 0 0 0 00 , and x f x u x u

0 0, ,

; ,o ox u x u

df dfx Ax Bu A Bdx du

eig( )A 0i i

0i i


Goal: Move the Quadrotor in space Six degree of freedom Four motor speeds as system input Under-actuated system Not all states are independently controllable Fly maneuvers

Control using full state feedback Assume all states are known Needs an additional state estimator

Assumption Neglect motor dynamics Low speeds

→ only dominate aerodynamic forces considered→ linearization around small attitude angles


Control of a Quadrotor | General Overview


Change of momentum and spin in the body frame

Position in the inertial frame and the Attitude

Forces and moments Aerodynamics and gravity


Modeling of Rotorcraft | Body Dynamics 2

MF

ωIωvω

ωv

I B

B

BB

BB

B

Bx mmE 0

033

vCx BEBE ωJ Br

Aero

AeroG

BB

BBB

MMFFF

4

1

0 00 0T

B G EB B Aeroi

iE B

F C Fmg T

E3x3: Identity matrix

4 2

1 34

1

1

00

01

B Aero B T B

iBi

iB

l T TM M Q l T T

Q


Relation between Tait-Bryan angles and angular velocities

Linearized relation at hover

Trust and drag of Propellers


Modeling of Rotorcraft | Rotational Velocity 2

ωJ Br

coscossin0cossincos0

sin01rJ

ωJ Br 100010001

0,0

Singularity for = 90o

2 2, , i i p i i i p iT b Q d


Rotational dynamics near hover

Full control over all rotational speeds Not dependent on position states


Control of a Quadrotor | Analysis of the Dynamics 1

2 2,4 ,2

2 2,1 ,3

2 2 2 2,1 ,2 ,3 ,4

( ) ( )

( ) ( )

( )

xx yy zz p p

yy zz xx p p

zz p p p p

r B

B

I p qr I I lb

I q rp I I lb

I r d

pJ q

r

,

: thrust constant: drag constant

: distance of propeller from the CoG: rotational speed of propellor p i

bdl

i

MF

ωIωvω

ωv

I B

B

BB

BB

B

Bx mmE 0

033

0 00 00 0

zz yyxx

B B yy xx zz

zz yy xx

qr I Ip I pI q I q rp I I

r I r pq I I

0


Translational dynamics in hover

Only direct control of the body z velocity Use attitude to control full position


Control of a Quadrotor | Analysis of the Dynamics 2

2 2 2 2,1 ,2 ,3 ,4

( ) sin ( ) sin cos ( ) cos cos ( )p p p p

EB EB B

B

mu m rv qw mgmv m pw ru mgmw m qu pv mg b

ux C v C v

w

,

: thrust constant: rotational speed of propellor p i

bi

MF

ωIωvω

ωv

I B

B

BB

BB

B

Bx mmE 0

033

B B

p u m qw rvm v q m v m ru pw

r w m pv qu

0 sin0 sin cos

cos cos

TB G EB

E

F C mgmg


Define virtual control inputs to simplify the equations Get decoupled and linear inputs Moments along each axis

Total thrust


Control of a Quadrotor | Virtual Control Input

2

3

4

( )

( )xx yy zz

yy zz xx

zz

I p qr I I U

I q rp I I U

I r U

)( 24,

23,

22,

21,1 ppppbU

2 22 ,4 ,2

2 23 ,1 ,3

2 2 2 24 ,1 ,2 ,3 ,4

( )

( )

( )

p p

p p

p p p p

U lb

U lb

U d

1

( ) sin ( ) sin cos ( ) cos cos

mu m rv qw mgmv m pw ru mgmw m qu pv mg U


Rotational dynamics independent of translational dynamics Use a cascaded control structure Inner loop: Control of the attitude Input: U2, U3, U4

Outer loop: Control of the position Input: U1, ϕ, θ


Control of a Quadrotor | Hierarchical Control

rqp

wvuZYX

432 ,, UUU

1U


Linearize attitude dynamic at hover Equilibrium point:


Control of a Quadrotor | Linearization of Attitude Dynamics

This model has no coupling We use 3 individual controller

2

3

4

( )

( )xx yy zz

yy zz xx

zz

r

B

I p qr I I U

I q pr I I U

I r U

pJ q

r

21 U

I xx

31 U

I yy

41 UIzz

2 3 4 10 ; 0 ; p q r U U U U mg


PID control strategy Generic non model based control Consist of three terms P - Proportional I - Integral D - Derivative

Different tuning methods Ziegler-Nichols method Pole placement

Widely used


Control of a Quadrotor | PID Control

0

( )( ) ( ) ( )t

p i dd e tu t k e t k e t dt k

dt

PlantCr(t) y(t)e(t) u(t)


Consider roll subsystem

P controller can‘t stabilize the system Harmonic oscillation

Use a PD controller


Control of a Quadrotor | PID Attitude Control

21 U

I xx

)(1 despxx

kI

1 ( ) ( )p des d desxx

k kI


3 separate control loops 1 additional thrust input

Aspect on the implementation Get angle derivatives from

transformation of the body angular rates

Avoid integral element in controller


Control of a Quadrotor | Attitude Control Design

1

2

3

4

( )

( )

( )

des

des pRoll dRoll

des pPitch dPitch

des pYaw dYaw

U TU k k

U k k

U k k

real

real

real

2Ud

d

d

3U

4U


kp is chosen to meet desired convergence time kd is chosen to get desired damping In theory any convergence time can be reached Actuator dynamics allow

for a limited control signal bandwidth Closed loop system dynamics

must respect the bandwidth of actuator dynamics

Actuator signals shall not saturate the motors


Control of a Quadrotor | Parameter Tuning


Transformation of virtual control inputs to motor speeds Rewrite virtual control inputs in matrix form Invert matrix to get motor speeds

Limit motor speeds to feasible (positive) values07.11.2016Robot Dynamics - Rotary Wing UAS: Control of a Quadrotor 18

Control of a Quadrotor | Control Allocation

4

3

2

1

24,

23,

22,

21,

14101

211

41

1411

2101

41

14101

211

41

1411

2101

41

UUUU

dlbb

dlbb

dlbb

dlbb

p

p

p

p

24,

23,

22,

21,

4

3

2

1

0000

p

p

p

p

ddddlblb

lblbbbbb

UUUU


A possible control flow for altitude control

Control attitude and the altitude Attitude controller to control the angles Parallel control loop. Altitude controller to control the horizontal thrust

and nonlinear transformation to calculate the corresponding thrust input


Control of a Quadrotor | Altitude Control

ϕdes,θdes,ΨdesU2,U3,U4

zdes Tz U1

ωi,des


Rewrite translational dynamicsin the inertial frame

Only look at the altitude dynamics Virtual Control

PD controller for the height

Transformation to the body frame


Control of a Quadrotor | Altitude Control 2

1

0010

0

UmgB

EB

E

CV

VX

11cos cos z g Um

11 cos cos Z Zz g T T Um

( )z p des dT k z z k z mg

coscos1zTU


Possible control structure for full position control

Control position and yaw angle Hierarchical controller Outer loop: Position control Thrust vector control. Desired thrust vector in inertial frame Transform to thrust and roll and pitch angles Inner loop: Attitude control


Control of a Quadrotor | Position Control

U1

Ψdes

ϕ, θ ωi,desxdesydeszdes

TxTyTz

U2U3U4


Thrust vector control Assume that the control input is the thrust in the inertial frame Inertial position system dynamics

Use 3 separate PD controller for each direction Transform to desired thrust, roll and pitch


Control of a Quadrotor | Position Control 2

gTTT

mzyx

z

y

x

00

1

2 2 21

sin cos1 ( , ) sin

cos cos

xT

x y z yE

z

TT T T T C z T

TT

2016 l9 quadrotor control - eth z...robot dynamics -rotary wing uas: control of a quadrotor...

Documents