35 views

Uploaded by jacksayshi

- Power Res Metrics Conf
- . Load Frequency Control for Interconnected Systems Using Optimal Controller
- Matlab Simulink Dc Motor
- Robust Control Theory Workshop rev2
- Manual
- Digital Control of Dynamic Systems (1998)
- Modern and Digital Control Engineering
- Matlab and Simulink for Control System
- Dynamic Characterization and Interaction Control of the CBM-Motus Robot for Upper-Limb Rehabilitation
- matecconf_iccma2016_01003
- Modern Control Theory
- Decoupling Control of a Twin Rotor Mimo System Using Robust Deadbeat Control Technique
- MEM 530 Final
- Model-based Control of Shake Tables With Multi-metric Feedback
- Gostin Alan
- Dr Husari Control Notes
- 8. Rigid Body Motion
- 3dcrane_cdc01
- 04605819
- CHE C441 Handout _1

You are on page 1of 33

AndrewCareagaHouck,RobertKevinKatzschmann,JoaoLuizAlmeidaSouza

Ramos

2.151 Advanced System Dynamics & Control

Fall 2013

TableofContents

ABSTRACT ................................................................................................................................... 3

I. INTRODUCTION ..................................................................................................................... 3

II. PENDULUM DESIGN AND HARWARE ............................................................................. 3

A. MECHANICAL DESIGN ............................................................................................................................ 3

III. SYSTEM MODEL ................................................................................................................... 7

A. EQUATIONS OF MOTION .............................................................................................................. 7

B. SYSTEM IDENTIFICATION ........................................................................................................ 12

IV. CONTROLLER DESIGN .................................................................................................... 17

Angular Differentiation and Noise filtering ........................................................................... 18

Linear Controller for the upright pose .................................................................................... 18

Swing up controller ......................................................................................................................... 18

V. RESULTS FROM SIMULATIONS AND EXPERIMENTS.............................................. 20

SIMULATION ...................................................................................................................................... 20

EXPERIMENT ..................................................................................................................................... 22

VI. CONCLUSIONS AND RECOMMENDATIONS .............................................................. 25

LIST OF PROJECT TASKS ...................................................................................................... 25

REFERENCES ............................................................................................................................ 26

APPENDICES ............................................................................................................................ 26

APPENDIX A: MATLAB CODE FOR EQUATIONS OF MOTION ............................................. 26

APPENDIX B: A AND B MATRIX DETAILED EQUATIONS ..................................................... 29

APPENDIX C: MATLAB CODE FOR SIMULATION .................................................................... 30

2

ABSTRACT

In this project, we set out to design, build, and control a rotational inverted

pendulum, also known as a Furuta pendulum. We succeeded in these goals, and

achieved a system capable of stability and robustness to disturbance in the upright

position, as well as swingup and swingdown control. This report describes our

physical system and dynamic model, as well as our controller design. Key results

from our simulation and experimental results are presented, as well as conclusions

and recommendations for future work. Connections to our 2.151 coursework, as

well as the realworld hurdles we encountered during our project, are emphasized.

Note: OK to publish on Stellar

I.INTRODUCTION

The Furuta pendulum was invented at the Tokyo Institute of Technology by

Katsuhisa Furuta in 1992, and has become a classic system for the application of

linear and nonlinear control theory. The Furuta pendulum is an underactuated, 2

DOF system. It is composed of a pendulum attached to the end of a motordriven

link. The motordriven link rotates in the horizontal plane, while the pendulum

moves in a vertical plane perpendicular to the driven link.

We selected this system because of it offers an exciting opportunity to

demonstrate the linear control methods we studied in 2.151. The mechanical system

itself is not overly complicated, which allowed us to fabricate it ourselves.

The primary goal of our control system is to maintain the pendulum in an

upright position by applying torque to the horizontal link, and for the system to be

able to maintain this position in the presence of disturbances. We also implemented

control methods for swingup and swingdown control by using energy shaping

methods.

II.PENDULUMDESIGNANDHARWARE

A.MECHANICALDESIGN

The pendulum was designed with the goal of developing a functional but lowcost

hardware. The round base was water jet cut from a scrap metal aluminum plate. The

first link was 3D printed and motor and sensors are all hobby grade. Figure 2.1

shows the proposed CAD model for the experimental platform.

3

Figure 2.1: Proposed version of the Furuta pendulum.

The first link and the pendulum were designed so the system center of mass would

be localized over the motor, to reduced the bending moment on the motor shaft. The

1024 pulses per revolution (PPR) encoder is connected to the pendulum shaft by a

shaft coupler. Figure 2.2 show a view of the setup proposed.

4

Figure 2.2: Proposed design for the pendulum.

The motor is a brushed DC motor with 12V nominal voltage and is driven by a motor

amplifier with build in current sensor. Figure 2.3 shows the motor and its driver.

The output motor shafts account for a resolution of 300PPR one the encoder.

Figure 2.3: Motor and driver used for the pendulum.

The system is controlled in real time using a ARM Cortex M3 Core based

microcontroller programmed in C environament (figure 2.4) through serial

communication to the host computer, This controller is able to run the control loop

at 1kHz.

5

Figure 2.4: Realtime controller used.

Figure 2.5 shows the final mechanical setup for the pendulum and figure 2.6 shows

the breadboard with the circuit implemented.

Figure 2.5: The Furuta Inverted Pendulum assembled.

6

Figure 2.6: Breadboard and circuit used to control the motor.

III.SYSTEMMODEL

A.EQUATIONSOFMOTION

The fixed (XYZ) and body (xyz for the first link and x'y'z' for the pendulum) frames

are shown in figure 3.1 and 3.2. Where is positive in the Z//z direction and is

positive in the x direction.

Figure 3.1: Reference frames for the Furuta inverted pendulum.

7

Figure 3.2: Pendulum fixed frame (x'y'z').

Figure 3.3: Center of mass position on the first link frame.

The following assumptions and simplifications where used for the model:

Motor and pendulum have only viscous friction, static friction is not taken

into account;

Rigid connections between motor/link 1 and encoder/pendulum shaft

Backlash effects are not being modeled;

8

All materials are homogeneous;

The encoder is a solid homogeneous object with known weight (for inertia

estimation);

When is in a range between 15 degrees the system dynamics can be

considered to be linear.

No base dynamics

The frame rotations were:

From the previously defined frames and according to figure 3.3, we can write the

position of the centers of mass for the first link (C1) and for the pendulum (C2) as:

cos cos sin sin

sin ; sin sin cos

0 0 cos

At xyz' frame, the angular velocity can be written as:

0

0 ; 0

On the other hand, the linear velocity for C1 and C2 are:

0 0 0

; 0 sin

0 0 cos

sin

cos

sin

Thus, we can write the kinetic energy for the system:

1 1 1 1

2 2 2 2

Where the inertia tensor at the xyz frame, which is calculated rotating the x'y'z'

inertia tensor (constant) along the x axis:

9

1 0 0 0 1 0 0

0 cos sin 0 0 0 cos sin

0 sin cos 0 0 sin cos

sin cos

sin 2

sin cos sin

2

sin 2

cos cos sin

2

And the inertia matrix for the first link is simply:

0 0

0 0

0 0

In which only I1zz has influence over the system dynamics.

The same way, the potential energy for the pendulum:

cos

From these we can write the Lagrangian,

And the equations of motion for each angle:

In which u is the motor input torque over the first link.

The equations of motion can be written in matrix form:

, , , ,

0

And the nonlinear representation:

10

, , , ,

0

And can be written in the linearized state space form about the upright position

0 0 0 0 :

0 0 1 0 0

0 0 0 1 0

Where:

; ; ;

; ; ;

;

The previous equation of motion where calculated using the Symbolic

Toolbox in MATLAB (the respective code is shown in APPENDIX A). The inputs for

the Amatrix are shown in APPENDIX B.

The linear dynamic equations for the DC brushed motor are broadly known

and can be written as:

Which introduces another state variable for the current and defines the

motor input voltage . These equations can be added to the previous matrix

expression before linearization, thus:

0

0 ;

0 0

11

The final system is 5th order in which both angles and angular velocities and

motor current. The input signal is the voltage applied to the motor terminals.

Considering the system has encoders for angle feedback and a current

sensor, the Cmatrix and Dmatrix are:

1 0 0 0 0

0 1 0 0 0 ; 0

0 0 0 0 1

B.SYSTEMIDENTIFICATION

The inertia and length parameters for the pendulum where extracted from

the CAD model developed in SolidWorks. Figure 3.4 shows the assemblies for the

first link and for the pendulum, the inertia tensors where calculated at each center

of mass and aligned to the xyz and x'y'z' frames.

Figure 3.4: First and second link assemblies for inertia estimation.

12

For angle about 15 degrees around the down position, the dynamic

equation for the pendulum can be approximated as linear and can be written as

0;

Or

2 0;

The damped oscillating frequency is given as

1 ;

Figure 3.5 shows the experimental decay of the free pendulum in time. It's

noticeable that the amplitude decay is very close to linear, thus the exponential

decay estimated in this work will just be an approximation. Then, we can write:

1

; ; 2 ;

2

1

Figure 3.5 Pendulum damped oscillation

The friction in the bearings for the pendulum was estimated using a second

order model for the pendulum and dropping it form a known initial angle, allowing

it to swing until all energy was totally dissipated.

13

Similarly, the motor dynamic parameters were estimated applying different

voltages and analyzing the no load steady state response. The response can be

visualized in figures 3.6, 3.7 and 3.8.

Figure 3.6 Time evolution of the armature current.

Figure 3.7: Motor position over time.

14

Figure 3.8: Time evolution of the angular velocity of the motor shaft

For each voltage we can write that, in steady state:

0; 0; ; .

The final values for the motor constants were calculating using the average

for each experiment:

1 1

;

5 5

Values for the motor impedance and resistance were measured according to figure

3.9.

15

Figure 3.9: Measurement of armature internal resistance and winding impedance.

Finally, all the parameters for the system are shown in table 2.1.

Table 2.1: Estimated system parameters:

Parameter Value Parameter Value

9.8 . 0.0013

6.0 10 / 5.34 10

5.52 10 / 8.41 10

0.1862 3.10 10

0.065 2.40 10

0.0955 0.182

0.03478 3.6

0.2983 1.845

0.05847 0.001

Thus, the numeric Amatrix is given by:

0 0 1 0 0 0

0 0 0 1 0 0

A 0 0.0152 0.0003 0.0002 0.0928 .10 3 ; B 0

0 0.0613 0.0002 0.0009 0.0740 0

0 0 0.0986 0 1.9512 542.0

Using the obsv() commands from MATLAB we can check if the system is observable,

which means all variables can be estimated or measured by the model:

rank(obsv(A,C)) = 5;

16

Numerically, the rank of the observability matrix is apparently full, but the condition

number is considerably high:

cond(obsv(A,C)) = 1.4409e+13;

The system is at the borderline of being observable. Matlab calculates that the rank

of the controllability matrix is not full:

rank(ctrb(A,B)) = 4;

Once again, having a closer looking at the condition number reveals an even higher

condition number than for the observability matrix.

cond(obsv(A,C)) = 1.6016e+15;

Matlab interprets the system to be not fully controllable, we assume for now that

the high condition numbers for both the observability and the controllability are

caused by the coarsely estimated values for the inductance of the motor circuit and

the inertia of the motor shaft. Since the bandwidth of the electrical system part is

not overall determining, we conclude that we will still be able to control and

observe the system.

The eigenvalues and eigenvectors for the Amatrix are

0; 1946.5; 7.1; 9.3; 3.7;

1 0.0000 0.0218 0.0429 0.2470

0 0.0000 0.1377 0.0982 0.0729

0 ; 0.0476 ; 0.1545 ; 0.3981 ; 0.9256 ;

0 0.0380 0.9781 0.9108 0.2733

0 0.9981 0.0078 0.0202 0.0469

The first eigenvalue ( 0) corresponds to the rigid body mode when the

first link spins and the pendulum angle is fixed. The second eigenvalue (

1946.5), the fastest and stable one is the electrical part of the system, with very

fast dynamics. The third eigenvalue ( 7.1), corresponds to the unstable

equilibrium for the pendulum in the upright position, when 0. The last two

eigenvalues ( 9.3; 3.7) correspond to the slower and stable dynamics

from the mechanical part. The system has only real valued eigenvalues due to the

friction; there's no oscillation within the linear range considered.

IV.CONTROLLERDESIGN

For the upright control of the pendulum, a fullstate feedback controller was

designed under the assumption that we can measure all system states directly. The

tuning of the feedback gains was done using a linear quadratic regulator design

approach (LQR). The swingup from the stable equilibrium and the swingdown

from the upright position were achieved by a nonlinear controller approach called

energy shaping. These two controller implementations will be further elaborated in

the following sections.

17

A.DIFFERENTIATIONANDNOISEFILTERING

Both rotating axis have a quadrature encoder connected to it, which allows

us to directly measure the first two system states. We did not implement a physical

device like a tachometer to directly measure the velocity, we therefore looked into

either using an observer or design a differentiator. An observer would be based

upon an uncertain system model, which is why we opted for combining a discrete

differentiator with a first order lowpass filter to derive the angular velocities from

the noisy quadrature encoder outputs. We manually tuned the rolloff frequencies of

each velocity filter to achieve a balance between an acceptable velocity phase offset

and a desirable noise rejection.

The measured motor current is quite noisy due to the nature of commutator

brushes. We also applied a first order low pass filter to the current signal before

feeding it back to the controller algorithm.

B.LINEARCONTROLLERFORUPRIGHTPOSE

We calculated the initial gains K of the full state feedback controller using a

conservative, expensive control ratio between the system Q and the input R

weighting matrix in order to avoid saturating the output of our small motor. This

first gains estimate was sufficient enough to allow the controller to balance the

pendulum upright during the very first try run.

Gradually adjusting the ratio between Q and R towards cheap control

improved the robustness of the pendulum to disturbance. We settled at a ratio that

allowed the motor to rarely saturate while in action. We were also able to observe a

better and more stable control behavior by decreasing the maximal allowable

angular position deviations while keeping the angular velocities at a similar order of

magnitude.

The control architecture of the linear controller is depicted in Figure 4.1. A

pulsewidth modulated signal is generated by an mbed NXP LPC1768

microcontroller an applied to a motor controller, which in turn applies the amplified

voltage signal e onto the motor of axis 1 of the Furuta pendulum. The quadrature

encoded pulses of the motor encoder and the shaft encoder are counted by the

microcontroller and are translated into angular measurements. At the same time, a

current sensor measures the motor current and provides its readings to the

microcontroller. The angles are differentiated and filtered to obtain the velocities,

and the current itself is also lowpass filtered. The unfiltered angles, the derived

velocities and the filtered current, encompassing all states of the system, are then

multiplied by the LQR determined gain Kupright and are feed back to the PWM.

C.SWINGUPCONTROLLER

The nonlinear swing up from the stable equilibrium is achieved by applying

the concept of energy shaping (see notes of class 6.832 on underactuated robotics

by Russ Tedrake). The controller architecture is shown in Figure 4.2. The controller

only requires the angular position and velocity of the pendulum axis 2. The maximal

potential energy is achieved at the upright, instable equilibrium. The difference

between the currently given energy and the maximal energy is multiplied by the

axis 2 velocity and a manually designed gain value Kswing. The obtained control value

18

is used as the PWM signal value. The energy of the pendulum for any given angle is a

long sum of inertial energy terms and the potential energy of the current height.

Computational limitations of the used microcontroller lead to the effect that a swing

up was not possible. No matter how we designed the gain value Kswing . Reducing the

energy calculation to the only significantly large inertial energy term and the

potential energy, as shown in Figure 4.2, enabled the controller to swing up the

pendulum to the upright position range.

The Furuta pendulum demonstration is started with the pendulum itself at

the stable equilibrium and in no motion. The filter initialization of axis 2 is

intentionally offset by 0.2 percent in order to give the pendulum an automatic initial

impulse. It then swings up to the upright position and switches to the linear

controller the moment the pendulum passes through the instable equilibrium. After

20 seconds, the controller switches back to the energy shaping based controller, but

applies this time a negative gain in order to swing down.

Figure 4.1: Linear Controller within 15 offset of the instable upright equilibrium

19

Figure 4.2: Swing Up Controller using Energy Shaping

V.RESULTSFROMSIMULATIONSANDEXPERIMENTS

SIMULATION

The system dynamics were simulated using the ode45 function on MATLAB,

this function is able to solve nonlinear ODEs numerically. The controller was first

designed and tuned using the simulation and the obtained gains were applied to the

real system. The nonlinear equations of motion were always used during the

simulation, including when the pendulum state lies within the considered to be

linear region. Even though not used in practice, the simulation includes a state

observer with poles eight times faster than the system poles.

The voltage applied to the motor is shown in figure 5.1. Notice that the motor

never saturates (nominal voltage 12V), even during the swingup control.

The MATLAB code for the simulation can be found in appendix C.

20

Figure 5.1: Input voltage for pendulum stabilization.

The angular positions and velocities are shown in figure 5.2 and 5.3 along

with the estimated velocities for the observer. We can see that the observer is not

able to satisfactory estimate the velocities on the nonlinear regime once it's a linear

observer.

The pendulum starts from equilibrium at the down position where 0,

swingsup, stabilizes and swing back down to the initial state.

Figure 5.2: Pendulum position during swingup, stabilization and swingdown

control.

21

Figure 5.3: Pendulum real and estimated angular velocities during swingup,

stabilization and swingdown control.

EXPERIMENT

The following data was gathered during swingup and stabilization, and

demonstrates the good performance of our controller. The trial begins with the

pendulum swinging up to its upright position. This employed a simple energy

addition control, and swung the pendulum upright in approximately 4 seconds.

After the pendulum crossed the upright position, the stabilization controller was

activated. The system exhibited some persistent highfrequency oscillation in the

upright position. This can be mostly attributed to backlash in our motor.

We disturbed the system by hitting the pendulum several times during the

trial. The controllers response to these disturbances can be seen best in Figure 5.5,

where they appear as impulses to the pendulum velocity. The system recovers to

equilibrium quickly with negligible overshoot. We iterated our LQR design multiple

times to achieve this performance. At first, an expensive control strategy was used,

since we doubted the strength of our small motor. We gradually shifted our

optimization parameters towards cheap control to improve performance while

avoiding actuator saturation.

22

Figure 5.4: Angular Position During SwingUp and Stabilization

Figure 5.5: Angular Velocities During SwingUp and Stabilization

23

Figure 5.6: Motor Voltage During SwingUp and Stabilization

Figure 5.7: Motor Current During SwingUp and Stabilization

Figure 5.7 shows the motor current during operation. The blue data shows

raw current measurements, while the red shows the data after being conditioned

with a firstorder low pass filter. The break frequency of this filter was tuned

manually. We experimented with different values until we found a value that

tracked current spikes adequately, while filtering most of the unwanted high

frequency noise. This process was also used to set the break frequencies of the

differentiators used to estimate d1/dt and d2/dt.

24

VI.CONCLUSIONSANDRECOMMENDATIONS

In this project, we successfully designed, built, and controlled a Furuta

pendulum. Our fullstate controller demonstrates good performance, and maintains

the pendulum in the upright position. The system demonstrates some oscillatory

behavior, mostly as the result of backlash in our motor's gearbox. Careful system

modeling and parameter estimation, as well as a simple and robust physical system,

were essential for this success.

Although the application of linear methods we studied in 2.151 were central

to the project, we learned most by working through the nonlinear behaviors that

come with a real physical system. Highly nonlinear, unmodeled behaviors such as

static friction and backlash weighed heavily into the behavior of the controlled

system, and minimizing them required adjustments based more on intuition than

theory. Additional problems with seemingly straightforward procedures, such as

measuring motor resistance or reading encoder pulses, provided unexpected and

timeconsuming hurdles. These problems gave us even more appreciation for the

difficulty of implementing a control system on hardware (as well as the satisfaction

when it actually works).

The Furuta pendulum is a classic system for the application of various

control methodologies, and offers many opportunities for future work. One step

could be the application of nonlinear control methods, functional outside of the

small linear range used in this project. Another interesting development would be

the addition of adaptive control, in which the mass and geometry of the pendulum

could be changed during operation, and responded to appropriately by the

controller.

LISTOFPROJECTTASKS

Electronics

Controller Hardware (Joao)

Sensor and Actuator Model Identification (Andrew)

RealTime Code on Microcontroller (Robert, Joao)

Mechanics

CAD Design of System (Joao, Robert, Andrew)

Purchase of Parts (Joao)

Printing Parts/ Manufacture Parts (Andrew, Robert)

Assembly of System (Joao, Andrew, Robert)

Parameter Identification/Estimation (Friction, Motor Model) (Andrew)

Controls Simulation in MATLAB

Modeling a physical system (Joao)

Simplification/Linearization into statedetermined form (Andrew)

Fullstate feedback control and LQ design (Robert)

Observer design (Joao)

Differentiator (Andrew)

Robustness to modeling uncertainties (Robert)

25

REFERENCES

Bernard Friedland, Control system design: an introduction to statespace

methods. Dover Books on Engineering, 2005

Eric R. Westervelt et al, Feedback Control of Dynamic Bipedal Robot Locomotion,

CRC Press, 2007

Neville Hogan, 2.151 Advanced System Dynamics and Control course notes

Russ Tedrake, Underactuated Robotics: Learning, Planning, and Control for Efficient

and Agile Machines (Course Notes for MIT 6.832), 2009

Wikipedia, Furuta Pendulum, last accessed 12/5/13,

[http://en.wikipedia.org/wiki/Furuta_pendulum]

APPENDICES

APPENDIXA:MATLABCODEFOREQUATIONSOFMOTION

clc;

close all;

global ssA ssB

syms theta1 dtheta1 theta2 dtheta2 ddtheta1 ddtheta2 tau1 real

syms vc1 vc2 w1 w2 real

syms g m1 m2 I1x I1y I1z I2x I2y I2z Ixz2 l1 L1 l2 L2 b1 b2 e real

syms T P real

pC1 = l1*[cos(theta1) sin(theta1) 0]';

pA = L1*[cos(theta1) sin(theta1) 0]';

pC2 = pA + l2*[sin(theta2)*sin(theta1) -sin(theta2)*cos(theta1)

cos(theta2)]';

pT = pA + L2*[sin(theta2)*sin(theta1) -sin(theta2)*cos(theta1)

cos(theta2)]';

w1 = [0 0 dtheta1]';

w2 = [dtheta2 0 dtheta1]';

vc1 = [0 dtheta1*l1 0]';

vA = [0 dtheta1*L1 0]';

%vc2 = vA + w2 x rAc2

vc2 = [l2*dtheta1*sin(theta2) dtheta1*L1-l2*dtheta2*cos(theta2) -

l2*dtheta2*sin(theta2)]';

26

%Inertia Matrices

I1 = diag([I1x I1y I1z]);

I2 = [[I2x 0 Ixz2];

[0 I2y 0 ];

[Ixz2 0 I2z];];

R = [1 0 0;

0 cos(theta2) -sin(theta2);

0 sin(theta2) cos(theta2);];

I2r = simplify(R'*I2*R);

%Kinetic Energy

Tp = m2*vc2'*vc2/2 + w2'*I2r*w2/2;

T = m1*vc1'*vc1/2 + w1'*I1*w1/2 + m2*vc2'*vc2/2 + w2'*I2r*w2/2;

T = simplify(T);

%Potential Energy

P = m2*g*l2*cos(theta2);

Ep = simplify(Tp + P);

TE = simplify(T + P);

dTE = diff(TE,dtheta1)*ddtheta1 + diff(TE,theta1)*dtheta1...

+ diff(TE,dtheta2)*ddtheta2 + diff(TE,theta2)*dtheta2;

%The Lagrangian

Lg = simplify(T - P);

partial_dtheta1 = simplify(diff(Lg,dtheta1));

partial_dtheta2 = simplify(diff(Lg,dtheta2));

%Time derivative

time_der1 = diff(partial_dtheta1,dtheta1)*ddtheta1 +

diff(partial_dtheta1,theta1)*dtheta1...

+ diff(partial_dtheta1,dtheta2)*ddtheta2 +

diff(partial_dtheta1,theta2)*dtheta2;

time_der1 = simplify(time_der1);

time_der2 = diff(partial_dtheta2,dtheta1)*ddtheta1 +

diff(partial_dtheta2,theta1)*dtheta1...

+ diff(partial_dtheta2,dtheta2)*ddtheta2 +

diff(partial_dtheta2,theta2)*dtheta2;

time_der2 = simplify(time_der2);

partial_theta1 = simplify(diff(Lg,theta1));

partial_theta2 = simplify(diff(Lg,theta2));

%Equations of motion

27

Eq1 = simplify(time_der1 - partial_theta1);

Eq2 = simplify(time_der2 - partial_theta2);

%Matrix terms for Eq1

H11 = diff(Eq1,ddtheta1);

H12 = diff(Eq1,ddtheta2);

G1 = diff(Eq1,g)*g;

C1 = Eq1 - H11*ddtheta1 - H12*ddtheta2 - G1;

H21 = diff(Eq2,ddtheta1);

H22 = diff(Eq2,ddtheta2);

G2 = diff(Eq2,g)*g;

C2 = simplify(Eq2 - H21*ddtheta1 - H22*ddtheta2 - G2);

syms kt Rm Lm Jm im dim

% D matrix combines mechanical inertia with electrical inductance

D_mtx = [H11 H12 0;

H21 H22 0;

0 0 Lm];

C_vec = [C1-kt*im; C2; (kt*dtheta1+Rm*im);];

G_vec = [G1; G2; 0;];

B_mtx = diag([b1 b2 0]);

% ---------------------------------------------------------------------

---

% Create MATLAB functions for each EOM entry

param_list = {'g','p(1)'; 'L1','p(2)'; 'l1','p(3)'; 'm1','p(4)';

'I1x','p(5)';'I1y','p(6)';'I1z','p(7)';...

'L2','p(8)';'l2','p(9)'; 'm2','p(10)';

'I2x','p(11)';'I2y','p(12)';'I2z','p(13)';'b1','p(14)';'b2','p(15)';'Ix

z2','p(16)';...

'kt','p(17)';'Rm','p(18)';'Lm','p(19)';'Jm','p(20)';};

list_dq = {'dtheta1','q(1)'; 'dtheta2','q(2)'; 'dim','q(3)'};

write_fcn('D_matrix.m',{'q','p'},[list_q; param_list],{D_mtx,'D'});

write_fcn('C_vector.m',{'q','dq','p'},[list_q; list_dq;

param_list],{C_vec,'C'});

write_fcn('G_vector.m',{'q','p'},[list_q; param_list],{G_vec,'G'});

write_fcn('KinEnergy.m',{'q','dq','p'},[list_q; list_dq;

param_list],{T,'KE'});

write_fcn('PotEnergy.m',{'q','p'},[list_q; param_list],{P,'PE'});

write_fcn('PendEnergy.m',{'q','dq','p'},[list_q; list_dq;

param_list],{Ep,'Pend_E'});

write_fcn('CG1.m',{'q','p'},[list_q; param_list],{pC1,'pC1'});

write_fcn('CG2.m',{'q','p'},[list_q; param_list],{pC2,'pC2'});

write_fcn('pAxis.m',{'q','p'},[list_q; param_list],{pA,'pA'});

write_fcn('pTip.m',{'q','p'},[list_q; param_list],{pT,'pT'});

28

% ---------------------------------------------------------------------

---

%Linearization about initial position

% q = [theta1 theta2 dtheta1 dtheta2];

f = simplify(D_mtx\(-C_vec - G_vec + [0 0 e]' - B_mtx*[dtheta1;

dtheta2; im;]));

subs_vec = [theta1 theta2 dtheta1 dtheta2 im];

lin_point = [0 0 0 0 0];

A11 = simplify(subs(diff(f(1),theta1),subs_vec,lin_point));

A12 = simplify(subs(diff(f(1),theta2),subs_vec,lin_point));

A13 = simplify(subs(diff(f(1),dtheta1),subs_vec,lin_point));

A14 = simplify(subs(diff(f(1),dtheta2),subs_vec,lin_point));

A15 = simplify(subs(diff(f(1),im),subs_vec,lin_point));

A21 = simplify(subs(diff(f(2),theta1),subs_vec,lin_point));

A22 = simplify(subs(diff(f(2),theta2),subs_vec,lin_point));

A23 = simplify(subs(diff(f(2),dtheta1),subs_vec,lin_point));

A24 = simplify(subs(diff(f(2),dtheta2),subs_vec,lin_point));

A25 = simplify(subs(diff(f(2),im),subs_vec,lin_point));

A31 = simplify(subs(diff(f(3),theta1),subs_vec,lin_point));

A32 = simplify(subs(diff(f(3),theta2),subs_vec,lin_point));

A33 = simplify(subs(diff(f(3),dtheta1),subs_vec,lin_point));

A34 = simplify(subs(diff(f(3),dtheta2),subs_vec,lin_point));

A35 = simplify(subs(diff(f(3),im),subs_vec,lin_point));

B1 = subs(diff(f(1),e),subs_vec,lin_point);

B2 = subs(diff(f(2),e),subs_vec,lin_point);

B3 = subs(diff(f(3),e),subs_vec,lin_point);

ssA = [[0 0 1 0 0];

[0 0 0 1 0];

[A11 A12 A13 A14 A15];

[A21 A22 A23 A24 A25];

[A31 A32 A33 A34 A35];];

ssB = [0; 0; B1; B2; B3;];

APPENDIXB:AANDBMATRIXDETAILEDEQUATIONS

Thus the values for the Amatrix and Bmatrix are:

A11 = 0

A12 = (g*l2*m2*(Ixz2 L1*l2*m2))/(I2x*I1z Ixz2^2 + I2x*I2z + I2x*L1^2*m2 +

I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A13 = (b1*(m2*l2^2 + I2x))/(I2x*I1z Ixz2^2 + I2x*I2z + I2x*L1^2*m2 +

I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

29

A14 = (b2*(Ixz2 L1*l2*m2))/(I2x*I1z Ixz2^2 + I2x*I2z + I2x*L1^2*m2 +

I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A15 = (kt*(m2*l2^2 + I2x))/(I2x*I1z Ixz2^2 + I2x*I2z + I2x*L1^2*m2 +

I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A21 = 0

A22 = (g*l2*m2*(m2*L1^2 + m1*l1^2 + I1z + I2z))/(I2x*I1z Ixz2^2 + I2x*I2z +

I2x*L1^2*m2 + I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A23 = (b1*(Ixz2 L1*l2*m2))/(I2x*I1z Ixz2^2 + I2x*I2z + I2x*L1^2*m2 +

I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A24 = (b2*(m2*L1^2 + m1*l1^2 + I1z + I2z))/(I2x*I1z Ixz2^2 + I2x*I2z +

I2x*L1^2*m2 + I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A25 =(kt*(Ixz2 L1*l2*m2))/(I2x*I1z Ixz2^2 + I2x*I2z + I2x*L1^2*m2 +

I2x*l1^2*m1 + I1z*l2^2*m2 + I2z*l2^2*m2 + 2*Ixz2*L1*l2*m2 +

l1^2*l2^2*m1*m2)

A31 = 0

A32 = 0

A33 = kt/Lm

A34 = 0

A35 = Rm/Lm

B1 = 0

B2 = 0

B3 = 1/Lm

APPENDIXC:MATLABCODEFORSIMULATION

clc;

close all;

clear AXIS TIP KIN POT

%List of parameters

global p K L TAU ssA ssB ssC ssD t0 theta10 theta20 m20 g0 l20

g0 = 9.8;

b10 = 6e-4;

b20 = 5.52e-04;

m10 = 0.1862;

m20 = 0.0391 + 0.0259;

L10 = (51+44.55)*0.001;

l10 = -34.78e-3;

L20 = 0.2983;

l20 = 58.47e-3;

I1x0 = 0;

I1y0 = 0;

I1z0 = 3.13e-4 + 0.001;

I2x0 = 5.34e-4;

30

I2y0 = 8.41e-4;

I2z0 = 3.1e-4;

Ixz20 = -2.4e-4;

kt0 = 0.182;

Rm0 = 3.6;

Lm0 = 1.845e-3;

Jm0 = 0.001;

p = [g0 L10 l10 m10 I1x0 I1y0 I1z0 L20 l20 m20 I2x0 I2y0 I2z0 b10 b20

Ixz20...

kt0 Rm0 Lm0 Jm0];

ssA = eval(subs(ssA,[g L1 l1 m1 I1x I1y I1z L2 l2 m2 I2x I2y I2z b1 b2

Ixz2 kt Rm Lm Jm],p));

ssB = eval(subs(ssB,[g L1 l1 m1 I1x I1y I1z L2 l2 m2 I2x I2y I2z b1 b2

Ixz2 kt Rm Lm Jm],p));

ssC = [[1 0 0 0 0];

[0 1 0 0 0];

[0 0 0 0 1]];

ssD = [0; 0; 0];

%Controller design:

%Pole placement

Poles = [-5 -20 -30 -40 -50];

K = place(ssA, ssB, Poles);

Q = diag([.01 0.1 0.05 0.05 1]);

R = 1;

K = lqr(ssA,ssB,Q,R);

Pob = 8*eig(ssA-ssB*K);

L = place(ssA', ssC', Pob);

L = L';

t0 = 0;

TAU = [];

%Relative coordinates initial conditions q(0) = [theta10 theta20

dtheta10 dtheta20];

%Four last variables are estimated

q0 = [0 pi .2 .2 0]';

q0 = [q0 q0];

theta10 = q0(1);

theta20 = q0(2);

[t, Qp] = ode45('Pendulum', [0 15], q0);

Qp(:,1) = Qp(:,1) - pi/2 - pi/4 - 0.2;

for i=1:length(Qp)

AXIS(i,:) = pAxis(Qp(i,1:2),p);

TIP(i,:) = pTip(Qp(i,1:2),p);

31

KIN(i) = KinEnergy(Qp(i,1:2),Qp(i,3:4),p);

POT(i) = PotEnergy(Qp(i,1:2),p);

end

f = figure;

set(f, 'doublebuffer', 'on');

for k = 1:1:length(Qp)

%Desenhando

plot3([0 0 -.5*AXIS(k,1) AXIS(k,1) TIP(k,1)], [0 0 -.5*AXIS(k,2)

AXIS(k,2) TIP(k,2)], [-0.05 0 AXIS(k,3) AXIS(k,3) TIP(k,3)], 'k',

'LineWidth', 2)

hold on

axis equal

axis ([-1.1*L10 1.1*L10 -1.1*L10 1.1*L10 -1.1*L10 1.1*L10]);

grid on

hold off

drawnow;

end

figure()

plot(TAU(1,:),TAU(2,:),'k')

title('Applied Voltage')

ylabel('Voltage input e [V]')

xlabel('Time [s]')

figure()

plot(t,Qp(:,1),'k')

hold on

plot(t,Qp(:,2),'r')

title('Angular position for pendulum and base')

ylabel('Angular position [rad]')

xlabel('Time [s]')

legend('\theta_1','\theta_2')

figure()

plot(t,Qp(:,3),'k')

hold on

plot(t,Qp(:,8),'r')

plot(t,Qp(:,4),'b')

plot(t,Qp(:,9),'y')

title('Estimated velocities')

legend('Real','Estimation')

ylabel('Angular velocity [rad/s]')

xlabel('Time [s]')

legend('d\theta_1','d\theta_1 hat','d\theta_2','d\theta_2 hat')

function dq = Pendulum(t, q)

global p K L TAU ssA ssB ssC t0 theta10 theta20 m20 g0 l20

%Evaluating the dynamic equations matrices

%D(q)ddq + C(q,dq) + G(q) = tau

32

D = D_matrix([q(1:2); q(5)],p);

C = C_vector([q(1:2); q(5)],q(3:4),p);

G = G_vector([q(1:2); q(5)],p);

% xhat = q(6:10);

% e = -K*xhat;

%Joint torques

%Full state Feedback

e = -K*q(1:5);

%Swing-up

Energy = PendEnergy(q(1:2),q(3:4),p);

Ed = m20*g0*l20;

k = 5;

e = -k*q(4)*(Ed - Energy);

%Swing-up + LQR

if(cos(q(2)) > 0.95)

e = -K*q(1:5);

else

e = -k*q(4)*(Ed - Energy);

end

%Swing-down

if (t > 10)

e = 1.5*k*q(4)*(Ed - Energy);

end

%Saturation

Saturation = 12;

if (abs(e) > Saturation)

e = sign(e)*Saturation;

end

tau = [0 0 e]';

%Model Estimation

dxhat = ssA*q(6:10) + ssB*e + L*(ssC*q(1:5) - ssC*q(6:10));

dq = D\(tau - G - C);

dq = [q(3); q(4); dq; dxhat;];

end

33

- Power Res Metrics ConfUploaded byMarysol Ayala
- . Load Frequency Control for Interconnected Systems Using Optimal ControllerUploaded bypvpavan
- Matlab Simulink Dc MotorUploaded bykillua142
- Robust Control Theory Workshop rev2Uploaded bytotoler
- ManualUploaded byAlex Cotofana
- Digital Control of Dynamic Systems (1998)Uploaded byNgọcBảo
- Modern and Digital Control EngineeringUploaded byNishant Saxena
- Matlab and Simulink for Control SystemUploaded byLakshmi
- Dynamic Characterization and Interaction Control of the CBM-Motus Robot for Upper-Limb RehabilitationUploaded byAurel GS
- matecconf_iccma2016_01003Uploaded bynguyenvanquyetvtt
- Modern Control TheoryUploaded bySaravanan Smasher
- Decoupling Control of a Twin Rotor Mimo System Using Robust Deadbeat Control TechniqueUploaded byEyad Al-Mohatwary
- MEM 530 FinalUploaded byChristopher Braun
- Model-based Control of Shake Tables With Multi-metric FeedbackUploaded byDiego Fernando Camacho Gómez
- Gostin AlanUploaded byToufik Souanef
- Dr Husari Control NotesUploaded byزهديابوانس
- 8. Rigid Body MotionUploaded bySemwanga Godfrey
- 3dcrane_cdc01Uploaded byHoang Vu Nguyen
- 04605819Uploaded bySukma Firdaus
- CHE C441 Handout _1Uploaded bytfhrfth
- 10.1.1.38Uploaded byArjun Sharman
- Control System Examples_Ch 1 - 7_Sept 2017Uploaded byKarthick PN
- Chen2013 Study on Interactions of DFACTS Devices Operating in Distribution SystemUploaded byGiovanni Hernández
- SLIDING MODE CONTROL BASED WIND ENERGY POWER GENERATIONUploaded byIjctJournals
- EIE810_Lecture Modules & ContentsUploaded byFavour Adenugba
- 60b7d5298a9bbe9984Uploaded byFernando Ramos
- calculo edificios sismicosUploaded byaldo_memo
- T10KT MW TKG Lesson 29a Controller-SV-Module9p1 11122014Uploaded byMani Vannan Soundarapandiyan
- 15 - Torque.pptxUploaded byKeith Smith
- 04739675Uploaded byStefania Oliveira

- ARM Design Guide Introduction to Rotational MoldingUploaded bykpramodb1
- Souflle PancakesUploaded byjacksayshi
- Ex.47 - NON-ISOTHERMAL FLOW IN A TWIN SCREW EXTRUDER.pdfUploaded byjacksayshi
- Ex.108 - Sliding Mesh for a Single Screw ExtruderUploaded byjacksayshi
- AST_3018_Manual_v9 (1)Uploaded byjacksayshi
- Ex.112 - Effects of Slipping Along Screws and Back Pressure on Energy Dissipation and Backflows in a Twin Screw ExtruderUploaded byjacksayshi
- Ex.66 - Adaptive Meshing for Moving Parts (II) Twin Screw ExtruderUploaded byjacksayshi
- Ex.38 - Twin Screw ExtruderUploaded byjacksayshi
- Ex.115 - Non-Isothermal Flow in a Single Screw ExtruderUploaded byjacksayshi
- RetrogradeUploaded byjacksayshi
- The Five PDFUploaded byjacksayshi
- Carbide Tipped Pens Seventeen Tales of Hard 9781466810198Uploaded byjacksayshi
- Adventure Skeletons One-PageUploaded byjacksayshi
- Fundamentals of Music Theory RedditUploaded byjacksayshi
- AutoCAD Mechanical 2015 Productivity Study (1).pdfUploaded byEdson Ramos
- Dimensiones y acotaciones.pdfUploaded byAdam Gordon
- Lesson 34 - Viscous Flow in Pipes - Head Loss and Moody Chart - Handout VersionUploaded byjacksayshi
- Lesson 24 - Experimental Testing - Handout VersionUploaded byjacksayshi
- Recitation 15 1Uploaded byjacksayshi
- Lesson 35 - Viscous Flow in Pipes - Minor Losses - Handout VersionUploaded byjacksayshi
- Lesson 28 and 29- Isentropic Flow With Area Change - Handout VersionUploaded byjacksayshi
- Recitation 14 - Flow DeformationsUploaded byjacksayshi
- Abaqus Thin Plate TutorialUploaded byhfathollahi
- Proteus 200 Flowmeter Data SheetUploaded byjacksayshi
- nelsondiana_eml4501_report1Uploaded byjacksayshi
- Solar Observing Project 2017 (3)Uploaded byjacksayshi
- Heidarsson-2011-693.pdfUploaded byjacksayshi
- HW2 - ABC Model.pdfUploaded byjacksayshi

- rlexec.pdfUploaded byTrần Đức Anh
- 0071490140_ar014Uploaded byS3ppuku
- Final VersionUploaded byquantum16
- roboticsUploaded byTauseef Khan
- Marlboro Working Paper Sun Chen SinghUploaded byManuel F. Rubio Gallego
- GATE 2014Uploaded byChandra Mani
- J. S. Nicolis & I. Tsuda (1985) The Magic Number Seven Plus or Minus Two RevisitedUploaded byBasios Vasileios
- SVV_Flight_Dynamics_assignment.pdfUploaded byJamie Tjoe Ny
- Matlab Robust Control Toolbox (1)Uploaded bykastolo
- 3.2 M.E. SyllabusUploaded byAshish Sharma
- Digital Control of Dynamic Systems (1998)Uploaded bySaba Anwar
- PHU_M.Tech Power Electronics.pdfUploaded bysushil4056
- Quanser2DOFheliUploaded byUlises Aguilar
- control system question bank wbutUploaded bysubhroaue
- j 01126974Uploaded bypudnchina
- controlabilituUploaded byJanko Jaridic
- Skirms StabilityUploaded byFrancisco Salto Alemany
- 1.Vibration Analysis (1)Uploaded bykailash
- State SpaceUploaded bykanchi
- M.E CONTROL AND INSTRUMENTATION ENGINEERING.pdfUploaded byAmit Saurabh
- Dynamic Modeling and Simulation of a PalmUploaded byIndra Alamsyah
- Pole PlacementUploaded bycallsandhya
- Damping of Sub Synchronous Resonance Oscillations With TCSC and PSS and Their Control InteractionUploaded bytariq76
- Robust Decoupling Control Design for Twin Rotor System Using Hadamard WeightsUploaded byqadeer62
- 2Uploaded byragupa
- Electrical ElectronicsUploaded byPavithra Murthy
- State Space Circuit TutorialUploaded byamir400
- Hybrid Control SystemsUploaded bymtichyscribd
- Thinking SystemicallyUploaded bykrainajacka
- LabVIEW%20MathScriptUploaded byKemal Maulana Alhasa