Automatic Control II Hover V7

Download as pdf or txt
Download as pdf or txt
You are on page 1of 22

Quanser 3DOF Hover Tutorial

Automatic Control II 3DOF Hover Tutorial & Lab


Dr Ben Cazzolato
3 May 2008
Please note that a great deal of the material has been copied directly from the Quanser Users Manual for
the 3DOF Hover, in particular the background material. Some of the material in the manual has not been
reproduced here. Users of the rig should refer to the manual for safe operation.

Executive Summary
The purpose of the experiment is to design a controller that allows you to command a
desired pitch, roll and yaw angle of a model hovering platform. The model was purchased
with the financial assistance of the Sir Ross and Sir Keith Smith Fund.
You will examine the performance of the system with an operator in the loop. A joystick is
supplied that allows you to command the motors in open loop in order to compare human
operator performance with computer control. The joystick will also be used in conjunction
with a closed loop controller, which improves operator performance and makes the system
easier to use.

1. Introduction
The 3D Hover system (shown in Figure 1) consists of a frame with 4 propellers mounted
on a 3 DOF pivot joint such that the body can freely roll, pitch and yaw. The propellers
generate a lift force that can be used to control the pitch and roll angles. The total torque
generated by the propeller motors causes a yaw to the body as well. Two propellers in the
system are counter-rotating propellers such that the total torque in the system is balanced
when the thrusts of the 4 propellers are approximately equal.
Motion about the three degrees of freedom is measured using three encoders.
All electrical signals to and from the body are transmitted via a slipring thus eliminating the
possibility of tangled wires and reducing the amount of friction and loading about the
moving axes.

Page 1 of 22

Quanser 3DOF Hover Tutorial

Left
Motor

Pitch
Front
Motor

Yaw

Right
Motor

Back
Motor
Roll

Figure 1: 3DOF Hover Rig

2. Mathematical Modelling
A complete mathematical model including propeller dynamics, forces generated by the
propellers, static and dynamic friction of the bearings, etc, is very difficult to obtain. We will
instead develop a simplified model.

Page 2 of 22

Quanser 3DOF Hover Tutorial

Figure 2: Free body diagram of forces and moments

2.1 Pitch Axis


Consider the diagram in Figure 2. The pitch is defined as a rotation about the body Y-axis.
It is measured using an encoder. Assuming the roll is zero, then the pitch axis torque is
controlled by the forces, Ff and Fb , generated by the Front and Back propellers
respectively, resulting in a torque balance equation
&& l (Ff Fb )
Jp p

(1)

where J p is the moment of inertia of the body about the pitch axis and l is the distance
from the pitch axis to either prop centre. Equation (1) may be rewritten in terms of pitch
&& , and input motor voltage:
angular acceleration, p

&& = l (Ff Fb ) = l K fn (Vf Vb )


p
Jp
Jp

(2)

where K fn is the force constant that relates the forces generated by the props when the
voltages Vf and Vb are applied to the Front and Back motor respectively, that are
operating in a Normal rotational direction (as opposed to a counter-rotational direction).

Page 3 of 22

Quanser 3DOF Hover Tutorial

Figure 3: Photograph of encoders used to measure pitch and roll

2.2 Roll Axis


Consider the diagram in Figure 2. The roll is defined as a rotation about the body X-axis
and is also measured using an encoder. Assuming the pitch is zero, then the roll axis
torque is controlled by the forces, Fr and Fl , generated by the Right and Left propellers
respectively. Note that this axis uses counter-rotating propellers and thus the motor must
rotate in the opposite direction than the Front and Back motors to achieve lift. The reason
for using two types of propellers is that this balances the net torque in the yaw direction. If
all motors spun in the same direction, then the drag from the propellers would create a
bias torque in the yaw. The resulting torque balance equation for roll is

Jr r&& l (Fr Fl )

(3)

where Jr is the moment of inertia of the body about the roll axis and l is the distance from
the roll axis to either prop centre. Equation (3) may be rewritten in terms of roll angular
acceleration, r&& , and input motor voltage:

lK
l
r&& = (Fr Fl ) = fc (Vr Vl )
Jr
Jr

(4)

where K fc is the force constant that relates the forces generated by the props when the
voltages Vr and Vl are the applied to the Right and Left motor respectively, when
operating in a Counter rotational direction.

Page 4 of 22

Quanser 3DOF Hover Tutorial

2.3 Yaw Axis


Consider the diagram in Figure 2. The yaw is defined as the rotation about the body Zaxis. Note that this angle is not directly measured but rather the yaw of the entire body
relative to the base is measured using an encoder. The only time the body yaw is
accurately given by the encoders is when the roll and pitch angles are zero. The torque
about the Yaw-axis is the sum of the torques generated by the motors of all 4 propellers.
The resulting in a torque balance equation for yaw is

Jy y&& T =Tf + Tb + Tr + Tl

(5)

where J y is the moment of inertia of the body about the yaw axis and T is the torque from
each of the motors. Equation (5) may be rewritten in terms of yaw angular acceleration
(relative to body frame), y&& , and input motor voltages:

K
K
y&& = tn (Vf + Vb ) + tc (Vr + Vl )
Jy
Jy

(6)

where K tn and K tc are the torque constants that relates the torque generated by the prop
when a voltage is applied to the motor (for normal-rotational direction and counterrotational direction respectively).

2.4 State Equations


The equations we have just derived can be written as a state space equation of the form:
y& 0
y&& 0

p& 0
&& =
p 0
r& 0
&&
r 0

0
1 0 0 0 0 y K t

0 0 0 0 0 y& J y

0 0 1 0 0 p 0
+ l K fn
0 0 0 0 0 p& J
p
0 0 0 0 1 r 0

0 0 0 0 0 r& 0

0
Kt
Jy
0
l K fn
Jp
0
0

0
Kt
Jy
0
0
0
l K fc
Jr

Vf
Vb

0 V
r

0 Vl
l K fc

J r
0
Kt
Jy
0

(7)

Assuming that the normal and counter-rotating motors and propellers are the same (apart
from the blade pitch), then K fn = K fc = K f and Ktn = Ktc = Kt .
The output equation is given by
y
y&
Vf
y
1
0
0
0
0
0

0 0 0 0
p = 0 0 1 0 0 0 p + 0 0 0 0 Vb

p&
V
r 0 0 0 0 1 0 0 0 0 0 r
r
Vl
&
r

Page 5 of 22

(8)

Quanser 3DOF Hover Tutorial

The equations of motion for a proper model of the system are actually considerably more
complex than the equations derived previously. Some of the simplifications we have made
are:
a) Many of the moments generated are functions of pitch and roll. We have assumed
that the pitch and roll is small.
b) We have assumed that the lift is a linear function of motor drive voltage. In practice
it is not.
c) There is gyroscopic coupling between the axes. This has been neglected by
assuming that the rotational velocities are small.
d) The response to a voltage input does not result in immediate response of the
propeller speeds and thus output force: the time constants of these sub-systems
are considered to be much faster than the body dynamics and can also be
neglected.
e) Centrifugal forces during a yaw rotation will affect the pitch and roll axes.
f) Air turbulence around the body will result in unknown disturbances.
g) Friction, both static and dynamic, has been completely ignored, despite the friction
in the yaw being significant.
h) The center of gravity of the system is not coincident with the pitch and roll axes,
thus a load appears about the pitch and roll rotation axes that is dependent on the
angles of rotation. The manual says that the CofG is below the rotation axis, but it is
actually above, thus making the roll and pitch unstable when uncontrolled.
i) The Yaw joint is not absolutely friction free mainly due to the sliprings contacts.
j) The wires protruding from the body offer some resistance which results in a
constant disturbance.
k) The system is not exactly balanced when initialized.
Despite all the approximations, the model is sufficient to design a working controller.
For those that are interested, a full derivation of the non-linear equations are presented in
Appendix D.

Page 6 of 22

Quanser 3DOF Hover Tutorial

2.5 Control System Design


We will now augment the system to allow us to command the yaw, pitch and roll as well as
remove steady state errors. This augmentation is also necessary given that there is a
gravitational disturbance term g (acting on the pitch and roll). We define three new states
, and resulting in the following augmented state space representation:
y&
y&&

p&
&&
p
r& =
&&
r
&
&

&

0
0

0
0

0
1

0
0

1 0 0 0 0

0 0 0 0 0
0 0 1 0 0

0 0 0 0 0
0 0 0 0 1

0 0 0 0 0
0 0 0 0 0

0 1 0 0 0
0 0 0 1 0

0
0 0 0 y K t

0 0 0 y& J y

0 0 0 p 0

l K f
0 0 0 p&
Jp

0 0 0 r + 0

0 0 0 r& 0

0 0 0

0 0 0

0 0 0

0
0 0 0

0
0 0 0

0
0 0 0
V
0
0 f 0 0 0 ry Gd , p


Vb
0
0 V 0 0 0 rp + 0 g
l K f l K f r 0 0 0 rr Gd ,r
0
Jr
Jr Vl 1 0 0
0

0 0 0 0

0
0 1 0
0 0 0 0

0
0 0 1

0 0 0 0

0
Kt
Jy
0
l Kf
Jp
0

0
Kt
Jy
0

0
Kt
Jy
0

(9)

where
, and are the integral of the yaw, pitch and roll setpoint error respectively,
ry , rp and rr are the desired yaw, pitch and roll (setpoint) respectively

l = 0.1969m is the distance from the prop centre to the pivot point,
J p = 0.0552kg.m 2 is the moment of inertia about the pitch axis,

Jr = 0.0552kg.m 2 is the moment of inertia about the roll axis,

J y J p + Jr = 0.1104kg.m 2 is the moment of inertia about the yaw axis,

K f = 0.594N / V is the force constant and is equal to the force generated from the
fans per unit voltage into the amplifier,
likewise K t = 0.0180 Nm / V is the torque constant and is equal to the torque
generated by the fans (in the body yaw) per unit voltage into the amplifier.
Gd , p and Gd ,r are constants that relate the gravitational field to the rotational
acceleration in pitch and roll respectively. These non-linear terms are a function of
pitch and roll and are very difficult to model.
The sub-matrices in the state matrix and control matrix are simply the matrices prior
to state augmentation, in other words Equation (7).

If you are wondering what such a system may look like, please jump ahead to Figure 3,
which is a Simulink block diagram of what we are about the design and build.

Page 7 of 22

Quanser 3DOF Hover Tutorial

3. Questions
3.1 Matlab Modelling
Question 1
Enter the non-augmented state and control matrix in Matlab. Let the state matrix be A m
and the control matrix be B m . Ignore the gravitational disturbance.
What are the poles of the open loop system? Describe what this means in relation to the
physical system.

Question 2
Determine the condition number of the controllability matrix. From this determine if the
system is controllable, noting how controllable it is.

Question 3
From the earlier discussion, the encoders provide us with pitch, roll and yaw. Determine
the output matrix C m for the original system.

Question 4
Determine the condition number of the observability matrix. From this determine if the
system is observable, noting how observable it is.

Question 5
We can now proceed in designing a full-state feedback controller for the system.
build the augmented state space system in Matlab.
Using an optimal LQR controller with a state weighting matrix given by
0
0
0
0
0
0
0
10000 0
0
100
0
0
0
0
0
0
0

0
0 20000 0
0
0
0
0
0

0
0
500
0
0
0
0
0
0
Q= 0
0
0
0 20000 0
0
0
0

0
0
0
0
500
0
0
0
0
0
0
0
0
0
0 20000
0
0

0
0
0
0
0
0
20000
0
0
0
0
0
0
0
0
0
0
20000
Page 8 of 22

(10)

First

Quanser 3DOF Hover Tutorial

and an effort penalty matrix given by


1
0
R=
0

0
1
0
0

0
0
1
0

0
0

(11)

determine the optimal (4x9) controller gains K = [K m , K i ] . Note the (4x6) gain vector, K m ,
for the feedback controller and the (4x3) controller gains, K i , for the integral command
tracking.

Question 6
Determine the poles of the feedback system alone. This means determining the poles of
A m B mK m , where A m is the original state matrix prior to augmentation and B m is the
original control matrix prior to augmentation.

Question 7
Now build the augmented control system using full state feedback and integral command
tracking. This will require you to define a new [9x3] state input matrix for the command
inputs.
Using the step command, look at the plant response to a 1 radian command into the yaw,
pitch and roll. In particular, look at the settling time of the angles. Also quantify the
maximum control voltages into the fan motors. This requires additional rows in the state
output matrix.

Question 8
Now we will build an estimator. Using pole placement, determine the (6x3) observer gain
matrix L to place the poles of the estimator at 10 times the poles of the feedback system
(determined in Question 6).
Building a regulator and then feeding it back into the plant becomes very difficult in Matlab,
especially keeping tracking of all the states and inputs. We will now move to Simulink as
this allows us to model the same system using block diagrams.

Page 9 of 22

Quanser 3DOF Hover Tutorial

3.2 Simulink Modelling


Now that the controller is designed we can see what happens in a fully developed Simulink
model.
Open the Simulink model called hover_model_estim_bsc.mdl. It should look like the
model shown in Figure 3.
Command or
Reference
Input

This model is an observer based state controller


for the Quanser 3DOF Hover rig.
Written by B. Cazzolato 1/3/05.
Before this simulation can be run, you must
run the file "d_hover_bsc_new.m"

0
Yaw SP
pi/8
Pitch SP

Manual Switch

Setpoint
Error

Ki* uvec

1
s

Plant

Motor
Input
Voltage

Augmented
States

Voltage
Scope

Motor
Input
Voltage

Integrator
Ki

x' = Ax+Bu
y = Cx+Du
State-Space

Actual
States

Cm* uvec
C

Y aw, Pitch & Roll

State Scope

0
Roll SP

Controller
Actual
States

Km* uvec
Km
y aw, pitch, roll

Y aw, Pitch & Roll

Manual Switch
Double click to select
Full state control or
Observer based control

JoyStick Output

Observer
Motor
Input
Voltage

Bm* u

1
s

x^_dot

B_est

x^

Cm* u

Integrator1
Am* u

y^

C_est

x^
ye

A_est

Hover VRML Model

L* u
L

x^

x^

Estim State
Scope
Y aw, Pitch & Roll

Figure 4: Simulink state space model of 3DOF Hover with integral command following using an estimator. The
red constant boxes are the setpoints for the yaw, pitch and roll. The orange gain blocks are the feedback
controller and the integral tracking controller. The plant is modelled in light blue. The observer is yellow. The
magenta blocks are scopes to allow you to view the control signals, states and state estimates. The manual switch
allows one to change from full-state control to a control system using an estimator. This can be done by double
clicking the block.

To run the Simulink model you will need to ensure that the terms you have used match
those in the model.
You can view the response of the system using the VRML (virtual reality model) by double
clicking on the block. The model is shown in Figure 5. Please note that if you have a
joystick on you, then you can control the plant using this.

Page 10 of 22

Quanser 3DOF Hover Tutorial

Figure 5: VR model of 3DOF Hover.

Question 9
First look at the response of a pitch setpoint of 1 radian using full-state feedback. If you
have done everything correctly, the settling times should be in the order of 4 seconds.
Look at system output and the input control voltage. Note the maximum of each. This
should match your answers from Question 7.
Now repeat this when using an observer to estimate the states. Note the system output
and control voltages as done before and comment as to why they might be different from
full state feedback.

Page 11 of 22

Quanser 3DOF Hover Tutorial

3.3 Experimental Rig


Now for the fun stuff. If all looks well in the Simulink model, then you are ready to try the
real thing.
A modified version of the Simulink model in Figure 4 has been compiled to run on a realtime control system called Quanser WinCon. This platform allows for rapid prototyping
and testing of real-time controllers. The Quanser control board takes the inputs from the
encoder and a block converts the pulses generated by the encoder to angles for the yaw,
pitch and roll. The computer processes the control algorithm before outputting analog
voltages to drive the fans via the power amplifiers.
Please note that the Hover rig is an expensive piece of equipment. Please be careful and
do not abuse it.
The controller running on the platform is both a full state controller and an observer based
controller. For full-state feedback, the angular velocities are estimated using an
approximate derivative. This is not recommended in practice unless the model of plant is
extremely poor (which in this case it is). You will note that by differentiating the angular
outputs from the encoder we get very spiky angular velocity estimates which of course is
undesirable as these feed into the control signal (since the control signal is proportional to
the sum of the weighted states).

Question 10
With the controller turned off, and running the system in open loop try to control the yaw,
pitch and roll using the joystick. When in open loop the joystick position simply controls fan
speed. Comment on the ease or difficulty of this.

Question 11
Now turn on your feedback controller. Use the joystick to change the angular position
setpoint, or alternatively type a setpoint in directly.
Compare the settling times for the real system against the Simulink model, and comment
as to why they might differ. Please note that the input into the amplifiers is limited to 5V.
Note if there has been an improvement in the ease of control compared to the open loop.
Look at the controlled system when using the estimator. Note if the performance is better
or worse than before.
Finally, look at the fan control voltages when using full state feedback and when using
control vie the state estimates. Comment on any differences you see.

Page 12 of 22

Quanser 3DOF Hover Tutorial

Appendix A System Parameters


Component Description
Symbol
Total mass of body
Structure
M = 4m
Mass per propeller
m
section
Distance from pivot
l
point to motor
Moment of inertia about J p
pitch axis
Moment of inertia about Jr
roll axis
Moment of inertia about J y J p + J r
yaw axis
Motor Pittman
Propeller
9234S004
Motors
Motor Mass
Armature Resistance

Inductance
Torque Constant
Rotor Moment of Inertia
Viscous Damping
Damping Constant
Rated Voltage
Running Voltage
Graupner 8-6 2 Bladed
Propellers
Diameter (8)
Pitch (6)
Force Constant at motor K fm
Force Constant at Amp
Kf
(5 times gain)
Torque Constant at
K tm
motor
Torque Constant at Amp K t
(5 times gain)
Yaw (in Quadrature)
Encoders
Pitch (in Quadrature)
Roll (in Quadrature)

Page 13 of 22

Value
2.85
0.7125

Units
kg
kg

0.1969

0.0552

Kg.m2

0.0552

Kg.m2

0.1104

Kg.m2

0.286
0.83
0.63
0.0182
4.2E-06
2.6E-06
4.5E-04
12
<22

kg
Ohms
mH
Nm/Amp
Kgm2
N.m.s
N.m.s
V
V

203
152
0.1188
0.5940

mm
mm
N/V
N/V

0.0036

Nm/V

0.0180

Nm/V

8192
8192
8192

Counts/Rev
Counts/Rev
Counts/Rev

Quanser 3DOF Hover Tutorial

Appendix B Coordinate Transformations


Transforming from the Base coordinate frame to the Body coordinate frame
In the previous section you will have noted that commanding the yaw axis results in a yaw
about the yaw encoder axis - not the body yaw axis. This is because the feedback is from
the yaw encoder and the command in the yaw axis is compared to the yaw axis encoder
measurement to generate the feedback error signal. If we would like to command a yaw
about the body yaw axis, we have to develop a transformation as discussed next.
Consider the diagram in the figure below. This shows the sequence of rotations as they
occur in the base of the system. These are true joint rotations in the base.

Figure 6 : Coordinate transformations from the base frame to the body frame

The first rotation is


transformation matrix:
cos(y ) sin( y )
Ty = sin( y ) cos( y )

0
0

about the body yaw (z-axis) and is defined by the following

0
0

1
Page 14 of 22

Quanser 3DOF Hover Tutorial

The rotation about the y-axis is the pitch transformation given by:
cos( p) 0 sin( p)
Tp = 0
1
0

sin( p) 0 cos( p)

The rotation about the x-axis is the roll given by:


0
0
1

Tr = 0 cos(r ) sin(r )

0 sin(r ) cos(r )

The total transformation from base to body is:


Tmeasured = TyTpTr
cos( y ) cos( p) sin( y ) cos(r ) + cos( y ) sin( p) sin(r ) sin( y ) sin(r ) cos( y ) sin( p) cos(r )
cos( y ) sin(r ) sin( y ) sin( p) cos(r )
= sin( y ) cos( p) cos( y ) cos(r ) + sin( y ) sin( p) sin(r )

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

Now consider the rotations about the body frame. In order to command yaw about the
body Z axis, we need to perform the same transformations but in a different sequence.
We need to perform the Yaw transformation after we perform the pitch and roll transforms.
Tbody = TpbTrbTyb

Therefore in the imaginary body frame the final transform is given by:
with the transforms being the same in nature but with the angles referenced to the body
frame. That means
Tpb = Tp |p = pb
Trb = Tr |r =rb
Tyb = Ty |y = y b

Now if we compare the final transformations we have:


Tbody =
cos( pb ) cos( y b ) + sin( pb ) sin(rb ) sin( y b ) cos( pb ) sin( y b ) + sin( pb ) sin(rb ) cos( y b ) sin( pb ) cos(rb )

cos(rb ) sin( y b )
cos(rb ) cos( y b )
sin(rb )

sin( pb ) cos( y b ) cos( pb ) sin(rb ) sin( y b ) sin( pb ) sin( y b ) cos( pb ) sin(rb ) cos( y b ) cos( pb ) cos(rb )

We can now solve for y , p and r for a given y b , pb and rb . The solution to this can be
found be equating each term in Tbody and Tmeasured . The following expression coverts from
the body frame to the base frame:
Page 15 of 22

Quanser 3DOF Hover Tutorial

cos(rb ) sin( y b )

arctan 2

y
cos( pb ) cos( y b ) + sin( pb ) sin(rb ) sin( y b )

p =
arcsin(sin( pb ) cos( y b ) cos( pb ) sin(rb ) sin( y b ))

r
arctan 2((tan( pb ) sin( y b ) + sin(rb ) cos( y b )) / cos(rb ))

The following expression coverts from the base frame to the body frame:

cos( p) sin( y )

arctan 2
+
r
y
y
p
r
cos(
)
cos(
)
sin(
)
sin(
)
sin(
)

y b
p = arctan 2 sin( y ) sin(r ) + cos( y ) sin( p) cos(r )

b
cos( p) cos(r )

rb
arcsin( cos( y ) sin(r ) + sin( y ) sin( p) cos(r ))

These solutions are implemented in a Simulink S-function (solve_base.c ) so they can be


incorporated into a WinCon controller. You need to mex the file to use it in a simulation
(See Simulink manuals).
Using the above transformation, we can now command the system in the body coordinate
frame and obtain commands in the base - or measurement coordinate frame. We then
feed these commands into the controller implemented in the previous section.

Page 16 of 22

Quanser 3DOF Hover Tutorial

Appendix C Operation of Instrumentation


Never lift the system using the body. Always carry from the base with one hand and
stabilize the body with the other hand. Please take care of the system. Although robust,
you should treat it delicately.
Note: Rotating propellers can injure fingers.

The rig should already be assembled. You may need to connect the rig to the amplifiers
and control board. To do this:
1. Plug in the cable for the pitch encoder. Attach the flat 5 pin encoder connector to
the elevation encoder as labeled. Align connector pin #1 (GND) with the encoder
body labeled GND and insert the connector.
2. Plug in the cable for the roll encoder. Attach the flat 5 pin encoder connector to
the elevation encoder as labeled. Align connector pin #1 (GND) with the encoder
body labeled GND and insert the connector.

3. Plug in the cable for the motors. Attach the 6 pin DIN cable protruding from the
body to the connector on the base.

Page 17 of 22

Quanser 3DOF Hover Tutorial

4. Balance the system. Use the supplied weight clips to balance the system such
that it is horizontal when no power is applied to the motors. Move the clips around
the edge of the circular frame. The system is supplied already balanced but you
may have to re-adjust the weights as the wires from the body to the base do affect
the balance.

5. Motor connections to amplifiers. Connect the motor drive cables from the rig to
the power amplifiers as per table below.

From Power
Modules
UPM2405 # 0
UPM2405 # 0
UPM2405 # 0
UPM2405 # 0

To Experiment

Cable

Gain

Front Motor
Back Motor
Left Motor
Right Motor

6 Pin Din x 4 Pin Din


6 Pin Din x 4 Pin Din
6 Pin Din x 4 Pin Din
6 Pin Din x 4 Pin Din

5
5
5
5

Page 18 of 22

Quanser 3DOF Hover Tutorial

6. Analog output connections to the amplifiers.


From MultiQ
Terminal board
Analog Output D/A
#0
Analog Output D/A
#1
Analog Output D/A
#2
Analog Output D/A
#3

Results in

Cable

UPM2405 #0

Driving Front Motor

5PIN DIN / RCA

UPM2405 #1

Driving Back Motor

5PIN DIN / RCA

UPM2405 #2

Driving Left Motor

5PIN DIN / RCA

UPM2405 #3

Driving Right Motor

5PIN DIN / RCA

To UPM
From
D/A

7. Joystick connections to the amplifier. Attach the joystick to the quick connect
module on UMP#1, X to S3 & Y to S4. The joystick voltages are measured in the
simulink block using Analog Input Channels 2 (X) & 3 (Y). They may be used to
create commands to the system instead of the square waves.
8. Analog input connections to the amplifier. Attach from the UPM which has the
joystick attached a cable from To A/D to channels 0, 1 2 & 3 of the MultiQ
Terminal board Analog inputs. ( S1 to 0, S2 to 1, S3 to & S4 to 3. (Cable: 5 Pin Din
to 4 x RCA)

Page 19 of 22

Quanser 3DOF Hover Tutorial

Appendix D Full State Equations


The equations of motion for the rig are considerably more complicated than presented in
the beginning of this document. The following is an attempt to better capture the true
dynamics. The derivation below has used pages 54-55 in Franklin, Powell and EmamiNaeini Feedback Control of Dynamic Systems and also a paper by Tayebi & McGilvray
Attitude stabilization of a four-rotor aerial robot, in 43rd IEEE CDC Conference, 2004.
If we start with the torque balance from the DC motor, then we have the following nonlinear equation:
(D1)
J m&&m + m&m + kD | &m | &m = kT i a
where
Jm is the moment of inertia of the propeller and motor rotor
m is the viscous friction in the motor rotor
k (& ) 2 is the reactive torque, in free air, by the prop due aerodynamic drag
D

k D > 0 is the drag constant of the props, which is a factor of the air density, the prop
dimensions and other factors
kT is the torque constant of the motor
i a is the armature current
m is the angular position of the rotor
Another system of equations that governs the dynamics is the electrical equation of the
DC motor:
La i&a + Ra i a = Va k e&m
(D2)
where
La is the armature inductance
Ra is the armature resistance
k e is the back EMF constant and is equal to the torque constant of the motor when
in SI units
Va is the voltage applied to the motor
We then have the state equations which govern the body as a whole:
& = I G +
(D3)
If
f
a
a
where
the first term represents the inertial acceleration, the second term is the gyroscopic
moments due to the airframe itself and the third term is the gyroscopic moments
from the interaction of the rotors and the frame,
If 3 x 3 is a symmetric positive definite constant inertia matrix of the airframe with
respect to the body-fixed frame A whose origin is at the center of mass,
A = {e1, e2 , e3 } is the frame of the body, where e1 is the axis joining the front and
rear fans, e2 is the axis joining the right and left fans and e3 is the vertical axis,
I = {e x , e y , ez } is the inertial frame,
denotes the angular velocity of the airframe expressed in the body-fixed frame A ,
denotes the vector cross-product,

Page 20 of 22

Quanser 3DOF Hover Tutorial

Ga is a vector containing the gyroscopic torques, due to the combination of the


rotation of the airframe and the four rotors, and is given by
4

Ga = I r ( ez )( 1)i +1&m,i
i =1

a = ( a,1, a,2 , a,3 ) are the airframe torques generated by the rotors, given by
T

a,1 = lk l ((&m,right ) 2 (&m,left ) 2 )

a,2 = lk l ((&m,front ) 2 (&m,back )2 )

a,1 = k D ((&m,front ) 2 + (&m,back ) 2 (&m,right ) 2 (&m,left ) 2 )

l is the distance from the rotor to the pivot point,


k l > 0 is the lift constant of the props, which is a factor of the air density, the prop
dimensions and other factors.

Now, if the airframe velocity is low, then the gyroscopic terms are negligible leaving us with
a simplified expression for Equation D3,
& =
(D4)
If
a
In many cases the relative effect of the inductance is negligible compared to the
mechanical motion and can be neglected in Equation D2. This leaves us with
Ra i a = Va k e&m
(D5)
Combining this with Equation D1, we get
k
J m&&m + m&m + k D (&m ) 2 = T (Va k e&m ) (D6)
Ra
Rearranging we are left with an expression for the rotor speed in terms of applied voltage

k k
k
Jm&&m + m + T e &m + k D (&m )2 = T Va (D7)
Ra
Ra

This expression is non-linear and must be simplified. There are a number of ways that this
can be done. One way is to choose an operating speed for the rotor, say &m,0 , the torque
from the drag then becomes a linear function of speed, ie k (& ) 2 k & & , thus D7 is
D

D m m ,0

linearised,

k k
k
J m&&m + m + T e + k D&m,0 &m = T Va
Ra
Ra

(D8)

Equation D4 can also be linearised using a similar approach on the airframe torques
T
a = ( a,1, a,2 , a,3 ) , ie
a,1 = lk l&m,0 &m,right &m,left
= lk & &
&
a ,2

(
(
(&

m ,0

m,front

a,1 = k D&m,0

m,front

m,back

+ &m,back &m,right &m,left

Page 21 of 22

(D9)

Quanser 3DOF Hover Tutorial

Finally, many of the terms in D8 are small if the fan speed is high enough. The intertial
term J m&&m can be neglected, leaving us with

Va =

Ra
kT

k k
m + T e + k D&m,0 &m
Ra

(D10)

Equations D9 and D10 are the linearised state equations used in the front part of this
report.

Page 22 of 22

You might also like