aerial robotics lecture 3a_2 3-d quadrotor control

7/21/2019 Aerial Robotics Lecture 3A_2 3-D Quadrotor Control

http://slidepdf.com/reader/full/aerial-robotics-lecture-3a2-3-d-quadrotor-control 1/5

Lecture 3A.2

3-D Quadrotor Control

Having looked at the 2-dimensional quadrotor, we now turn our attention to thedynamics of the 3-dimensional model, and we'll talk about trajectory planning and

control

!ecall that we have a body-fi"ed frame attached to the quadrotor and we have an

inertial frame#

$he state of the quadrotor consists of position and orientation $he position is the

position-vector of the centre-of-mass, and the roll, pitch and yaw angles tell us the

orientation $his state is composed of a si"-dimensional vector and its rate of change

%in other words, the velocity&

q

q x

z

y

x

q

,

$he roll, pitch and yaw angles follow the usual convention# first the yaw above the -

a"is, and then the roll ( pitch as we've seen before $he angular velocity components

in the body frame, p q and r, are related to the derivatives of the roll, pitch and yaw

angles through this coefficient matri"#

coscos)sin

sin*)

sincos)cos

r

q

p



$his set of equations tells us everything we need to know about the kinematics of the

vehicle

+hat wed like to do is to track arbitrarily-specified trajectories in three-dimensions

+e assume were given a trajectory $his trajectory consists of a position vector that

varies as a function of time, and a yaw angle, which also varies as a function of time#

&%&%

&%

&%

&%

t t z

t y

t x

t r T

+e want this four-dimensional vector to be differentiable, and we want to be able to

obtain not only its derivative, but also its second derivative

s before, we're interested in the difference between the desired position and the

actual position .ut now we're also interested in the difference between the desired

yaw angle and the actual yaw angle $hat gives us the error vector and its derivative#

r -%t&r $ pe

r -%t&r $

ve

nd again, we want the error vector to go e"ponentially to ero

)&r -%t&r % c$ p pvd e K e K

$his lets us calculate the commanded acceleration, cr , whether it's the 2nd-derivative

of the position vector, or the 2nd-derivative of the yaw angle

/et's take another look at the equations of motion, and the nested-feedback-loop we

described for the two-dimensional quadrotor model#



s in the planar case, we have nested feedback loops $he inner loop corresponds to

attitude control, and the outer loop corresponds to position control 0n the inner loop,

we specify the orientation either using a rotation matri" or a series of roll, pitch, and

yaw angles +ell feedback the actual attitude and angular velocity, or the roll, pitch

and yaw angles and the angular rates 1rom that, we calculate the input u 2 u2 is a

function of the thrusts and moments that we get from the motors

0n the equations-of-motion at the bottom, we have to calculate the value of u 2 basedon the desired attitude

+e now turn our attention to the outer-loop, which is a position feedback loop 0n this

loop, we take the specified position vector ( specified yaw angle from the $rajectory

lanner +e compare that with the actual position and velocity, and from that we

calculate u*, u* is essentially the sum of all the thrust forces

!ather than looking at the general trajectory-following problem, let's initially focus on

a very specific case, the case of hovering#



o in hovering, the robots position and orientation are fi"ed 0n other words, all

velocities are ero 1urther, the roll and pitch angles are also equal to ero +e want

to consider small perturbations around the hover position ccordingly, we'll linearise

the dynamics around this current configuration

0n order for the hover position to be one of equilibrium, the input u2 has to be ), the

angular velocity components p, q, and r are equal to ), and their derivatives are also

equal to ) /ikewise, the sum of the thrusts has to compensate for the weight of the

robot, but the yaw angle can be non-ero as long as it's fi"ed

/inearising around this point, means that we are assuming u* is almost equal to mg

+e assume the roll and pitch angles are close to ero, and we assume that the yaw

angle is fi"ed, or close to fi"ed at a given value )

&4,)4,)4,4% )* mg u

+hen we linearise the equations, we end up with these simplified equations#

&sincos%* g xr

&cossin%2 g yr

+e can now design the inner feedback loop, by simply specifying u2 using a

proportional plus derivative controller#

&%&%

&%&%

&%&%

,,

,,

,,

2

r r K K

qq K K

p p K K

u

cd c p

cd c p

cd c p

Here we assume that we have the commanded roll, pitch, and yaw angles and their

derivatives nd all we require for feedback are the actual roll, pitch, and yaw angles,

and their derivatives

5ow, let's consider the outer feedback loop 0f we linearise the equations-of-motion,

the e"pressions for the first two components of acceleration can be written in the form

shown below#

&sincos%* g xr

&cossin%2 g yr

+e can now turn our attention to the error equation describing the error in position,

using these linearised equations of motion !emember, we want this error to satisfythe second-order differential equation#

)&%&%&% ,,,,,, idesii pidesiid cidesi r r K r r K r r

$his equation contains terms to do with the desired trajectory, desir , , and terms to do

with the actual trajectory being followed, ir +e will use this equation to calculate the

commanded acceleration, cir , $his commanded acceleration will, in turn, tell us what

thrust we need to apply with the rotors

&% ,3* cr g mu



1inally, we need to determine the commanded roll, pitch, and yaw using the linearised

equations 0f we know that the commanded acceleration needs to be in the 6 and 7

direction, we can calculate the roll and pitch angles and their derivatives

&cossin%*

,2,* descdescc r r

g

&sincos%*

,2,* descdescc r r g

+e can determine the commanded yaw angles in a similar manner

des

c

aerial robotics lecture 3a_2 3-d quadrotor control

Documents