automatic control ii hover v7

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 User’s 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.

of 22


Pitch

Yaw

Roll

RightMotor

BackMotor

FrontMotor

LeftMotor

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.

of 22


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, and , generated by the Front and Back propellers respectively, resulting in a torque balance equation

fF bF

)( bfp FFlpJ −≈&& (1)

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

pJ l

p&&

)()( bfp

fnbf

p

VVJKlFF

Jlp −=−=&& (2)

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

fnK

fV b

of 22


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, and , 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

rF lF

)( lrr FFlrJ −≈&& (3)

where r is the moment of inertia of the body about the roll axis and 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:

J l

&&

)()( lrr

fclr

r

VVJKlFF

Jlr −=−=&& (4)

where is the force constant that relates the forces generated by the props when the voltages r and lV are the applied to the Right and Left motor respectively, when operating in a “Counter” rotational direction.

fcKV

of 22


2.3 Yaw Axis Consider the diagram in Figure 2. The yaw is defined as the rotation about the body Z-axis. 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

lrbfy TTTTTyJ +++=≈ ∑&& (5)

where 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), , and input motor voltages:

yJ

y&&

)()( lry

tcbf

y

tn VVJKVV

JKy +++=&& (6)

where and 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 counter-rotational direction respectively).

tnK tcK

2.4 State Equations The equations we have just derived can be written as a state space equation of the form:

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

−

−

−−

+

⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

l

r

b

f

r

fc

r

fc

p

fn

p

fn

y

t

y

t

y

t

y

t

VVVV

JKl

JKl

JKl

JKl

JK

JK

JK

JK

rrppyy

rrppyy

000000

000000

0000

000000100000000000001000000000000010

&

&

&

&&

&

&&

&

&&

&

(7)

Assuming that the normal and counter-rotating motors and propellers are the same (apart from the blade pitch), then ffcfn KKK == and ttctn KKK ==− . The output equation is given by

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+

⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

l

r

b

f

VVVV

rrppyy

rpy

000000000000

010000000100000001

&

&

&

(8)

of 22


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.

of 22


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 (acting on the pitch and roll). We define three new states gα , β and γ resulting in the following augmented state space representation:

gG

G

rrr

VVVV

JKl

JKl

JKl

JKl

JK

JK

JK

JK

rrppyy

rrppyy

rd

pd

r

p

y

l

r

b

f

r

f

r

f

p

f

p

f

y

t

y

t

y

t

y

t

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

−

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

−

−

−−

+

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

⎟⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

=

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

000

0

000

100010001000000000000000000

000000000000

000000

000000

0000

000000000

010000000100000001

000000000000000000

000000100000000000001000000000000010

,

,

γβα

γβα

&

&

&

&

&&&&

&

&&

&

&&

&

(9) where

• α , β and γ are the integral of the yaw, pitch and roll setpoint error respectively, • , and are the desired yaw, pitch and roll (setpoint) respectively yr pr rr• is the distance from the prop centre to the pivot point, ml 1969.0=• is the moment of inertia about the pitch axis, 2.0552.0 mkgJp =

• is the moment of inertia about the roll axis, 2.0552.0 mkgJr =• is the moment of inertia about the yaw axis, 2.1104.0 mkgJJJ rpy =+≈

• is the force constant and is equal to the force generated from the fans per unit voltage into the amplifier,

VNKf /594.0=

• likewise is the torque constant and is equal to the torque generated by the fans (in the body yaw) per unit voltage into the amplifier.

VNmKt /.01800=

• and 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.

pdG , rdG ,

• 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.

of 22


3. Questions

3.1 Matlab Modelling

Question 1 Enter the non-augmented state and control matrix in Matlab. Let the state matrix be and the control matrix be . Ignore the gravitational disturbance.

mAmB

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 for the original system. mC

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. First build the augmented state space system in Matlab. Using an optimal LQR controller with a state weighting matrix given by

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

=

200000000000002000000000000020000000000000500000000000200000000000005000000000002000000000000010000000000010000

Q (10)

of 22


and an effort penalty matrix given by

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000010000100001

R (11)

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

mKiK

Question 6 Determine the poles of the feedback system alone. This means determining the poles of

, where is the original state matrix prior to augmentation and is the original control matrix prior to augmentation.

mmm KBA − mA mB

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 to place the poles of the estimator at 10 times the poles of the feedback system (determined in Question 6).

L

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.

of 22


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.

AugmentedStates

VoltageScope

x' = Ax+Bu y = Cx+Du

State-Space State Scope

0

Roll SP

pi/8

Pitch SP

This model is an observer based state controllerfor the Quanser 3DOF Hover rig.Written by B. Cazzolato 1/3/05.

Before this simulation can be run, you mustrun the fi le "d_hover_bsc_new.m"

Command orReference

Input

Observer

Controller

Plant

Manual SwitchDouble click to selectFull state control or

Observer based control

Manual Switch

L* u

L

Km* uvec

Km

Ki* uvec

Ki

y aw, pitch, roll

JoyStick Output

1s

Integrator1

1s

Integrator

Hover VRML Model

Estim State Scope

Cm* u

C_est

Cm* uvec

C

Bm* u

B_est

Am* u

A_est

0

Yaw SP

MotorInput

Voltage

MotorInput

Voltage

MotorInputVoltage

ActualStates

ActualStates

Yaw, Pitch & Roll

Yaw, Pitch & Roll

Yaw, Pitch & RollSetpointError

x^x^

x^

x^x _dot y ^

y e

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.

of 22


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.

of 22


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 real-time 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.

of 22


Appendix A – System Parameters

Component Description Symbol Value Units Total mass of body mM 4= 2.85 kg Mass per propeller section

m 0.7125 kg

Distance from pivot point to motor

l 0.1969 m

Moment of inertia about pitch axis

pJ 0.0552 Kg.m2

Moment of inertia about roll axis

rJ 0.0552 Kg.m2

Structure

Moment of inertia about yaw axis

rpy JJJ +≈ 0.1104 Kg.m2

Motor – Pittman 9234S004

Motor Mass 0.286 kg Armature Resistance Ω 0.83 Ohms Inductance 0.63 mH Torque Constant 0.0182 Nm/Amp Rotor Moment of Inertia 4.2E-06 Kgm2

Viscous Damping 2.6E-06 N.m.s Damping Constant 4.5E-04 N.m.s Rated Voltage 12 V

Propeller Motors

Running Voltage <22 V Graupner 8”-6” 2 Bladed Diameter (8”) 203 mm Pitch (6”) 152 mm Force Constant at motor fmK 0.1188 N/V Force Constant at Amp (5 times gain)

fK 0.5940 N/V

Torque Constant at motor

tmK 0.0036 Nm/V

Propellers

Torque Constant at Amp (5 times gain)

tK 0.0180 Nm/V

Yaw (in Quadrature) 8192 Counts/RevPitch (in Quadrature) 8192 Counts/Rev

Encoders

Roll (in Quadrature) 8192 Counts/Rev

of 22


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 about the body yaw (z-axis) and is defined by the following transformation matrix:

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

1000)cos()sin(0)sin()cos(

yyyy

Ty

of 22


The rotation about the y-axis is the pitch transformation given by:

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

)cos(0)sin(010

)sin(0)cos(

pp

ppTp

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

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−=

)cos()sin(0)sin()cos(0

001

rrrrTr

The total transformation from base to body is:

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−+−−+−

=

=

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

rprpprpyryrpyrypyrpyryrpyrypy

TTTT rpymeasured

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.

by

br

bpbody TTTT =

Therefore in the “imaginary” body frame the final transform is given by:

being the same in nature but with the angles referenced to the body

Now if we compare the final transformations we have:

with the transforms frame. That means

We can now solve for y , p and , bp and br . The solution to this can be found be equating each term in bodT

r for a given and . The following expression coverts from

e body frame to the base frame:

b

b

b

yyyb

y

rrrb

r

pppb

p

T

TT

TT

=

=

=

=

=

=

|

|

|

⎦⎣sin()sin()cos()cos()sin()cos(

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

bbbbbbbbbbbb

bbbbb

bbbbbbbbbbbb

body

rpyrpypyrpypryryr

rpyrpypyrpyp

T

by

y measuredT

T

⎥⎥⎥⎤

⎢⎢⎢⎡

−−−

−+−+

=

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

th

of 22


( )( )( )

⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢

⎣

⎡

+−

⎟⎟⎠

⎞⎜⎜⎝

⎛+

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

)cos(/)cos()sin()sin()tan(2arctan)sin()sin()cos()cos()sin(arcsin)sin()sin()sin()cos()cos(

)sin()cos(2arctan

bbbbb

bbbbb

bbbbb

bb

ryrypyrpypyrpyp

yr

rpy

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

on controller. You need to mex the file to use it in a simulation ee Simulink manuals).

e. We then ed these commands into the controller implemented in the previous section.

( )⎥⎥⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎢⎢⎡

+−−

⎟⎠

⎞⎜⎝

⎛ +

⎟⎠

⎞⎜⎝

⎛+

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

)cos()sin()sin()sin()cos(arcsin)cos()cos(

)cos()sin()cos()sin()sin(2arctan

)sin()sin()sin()cos()cos()sin()cos(2arctan

rpyryrp

rpyryrpyyr

yp

rpy

b

b

b

⎦⎣ These solutions are implemented in a Simulink S-function (solve_base.c ) so they can be incorporated into a WinC(S Using the above transformation, we can now command the system in the body coordinate frame and obtain commands in the base - or measurement – coordinate framfe

of 22


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.

of 22


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 To Experiment Cable Gain

UPM2405 # 0 Front Motor 6 Pin Din x 4 Pin Din 5 UPM2405 # 0 Back Motor 6 Pin Din x 4 Pin Din 5 UPM2405 # 0 Left Motor 6 Pin Din x 4 Pin Din 5 UPM2405 # 0 Right Motor 6 Pin Din x 4 Pin Din 5

of 22


6. Analog output connections to the amplifiers.

From MultiQ Terminal board

To UPM “From D/A”

Results in Cable

Analog Output D/A #0

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

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)

of 22


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 Emami-Naeini “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 non-linear equation:

aTmmDmmmm ikkJ =++ θθθµθ &&&&& || (D1) where

mJ is the moment of inertia of the propeller and motor rotor

mµ is the viscous friction in the motor rotor 2)( mDk θ& is the reactive torque, in free air, by the prop due aerodynamic drag

0>Dk is the drag constant of the props, which is a factor of the air density, the prop dimensions and other factors

Tk is the torque constant of the motor

ai 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:

meaaaaa kViRiL θ&& −=+ (D2) where

aL is the armature inductance

aR is the armature resistance

ek is the back EMF constant and is equal to the torque constant of the motor when in SI units

aV is the voltage applied to the motor

We then have the state equations which govern the body as a whole: aaff GII τ+−Ω×Ω−=Ω& (D3)

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,

33xfI ℜ∈ is a symmetric positive definite constant inertia matrix of the airframe with

respect to the body-fixed frame whose origin is at the center of mass, A 321 ,, eeeA = is the frame of the body, where is the axis joining the front and

rear fans, is the axis joining the right and left fans and is the vertical axis, 1e

2e 3e zyx eeeI ,,= is the inertial frame,

Ω denotes the angular velocity of the airframe expressed in the body-fixed frame , A× denotes the vector cross-product,

of 22


aG 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

imi

zi

ra eIG ,1

4

1)1)(( θ&+

=

−×Ω= ∑( T

aaaa 3,2,1, ,, ττττ = ) are the airframe torques generated by the rotors, given by

( )2,

2,1, )()( leftmrightmla lk θθτ && −=

( )2,

2,2, )()( backmfrontmla lk θθτ && −=

( )2,

2,

2,

2,1, )()()()( leftmrightmbackmfrontmDa k θθθθτ &&&& −−+=

l is the distance from the rotor to the pivot point, 0>lk 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,

afI τ=Ω& (D4) 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

meaaa kViR θ&−= (D5) Combining this with Equation D1, we get

)()( 2mea

a

TmDmmmm kV

RkkJ θθθµθ &&&&& −=++ (D6)

Rearranging we are left with an expression for the rotor speed in terms of applied voltage

aa

TmDm

a

eTmmm V

Rkk

RkkJ =+⎟⎟

⎠

⎞⎜⎜⎝

⎛++ 2)(θθµθ &&&& (D7)

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 , the torque

from the drag then becomes a linear function of speed, ie , thus D7 is linearised,

0,mθ&

0,2)( mmDmD kk θθθ &&& →

aa

TmmD

a

eTmmm V

Rkk

RkkJ =⎟⎟

⎠

⎞⎜⎜⎝

⎛+++ θθµθ &&&&

0, (D8)

Equation D4 can also be linearised using a similar approach on the airframe torques

, ie ( )Taaaa 3,2,1, ,, ττττ =

( )leftmrightmmla lk ,,0,1, θθθτ &&& −=

( )backmfrontmmla lk ,,0,2, θθθτ &&& −=

( )leftmrightmbackmfrontmmDa k ,,,,0,1, θθθθθτ &&&&& −−+= (D9)

of 22


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

mmDa

eTm

T

aa k

Rkk

kRV θθµ &&

⎟⎟⎠

⎞⎜⎜⎝

⎛++= 0, (D10)

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

of 22

automatic control ii hover v7

Documents

dof hover tutorial figure

dof hover rig

pitch axis torque

desired pitch

d hover system

body yaxis

dof pivot joint

quanser users manual