broom balancer

Upload: mohamad-magdy

Post on 07-Apr-2018

257 views

Category:

Documents


0 download

TRANSCRIPT

  • 8/6/2019 Broom Balancer

    1/118

    LDesign and Simulationof the Broom-balance Vehicle System

    A Thesis Presented toThe Faculty of the College of Engineering and Technology

    Ohio Univers i ty

    In Par t ia l Fulf i l lmentof the Requirements for the Degree

    Master of Science

    b yDejun -u n g /

    N o v e m b e r , 1993

  • 8/6/2019 Broom Balancer

    2/118

    Acknowledgements

    I wish to express my deep appreciation to my thesisadvisor Dr. Brian Fabien for his valuable guidance, assistance,patience and encouragement while developing this thesis.

    I also wish to express my sincere gratitude to Dr. SunilAgrawal of Mechanical Engineering Department and Dr. LarrySnyder of Mathematics Department for their serving on thecommittee.

  • 8/6/2019 Broom Balancer

    3/118

    Table of ContentsPage...Acknowledgements ........................................................................................... 111

    Table of Contents ................................................................................................ i v........................................................................................................ist of Tables v i

    List of Figures ..................................................................................................... viiChapter 1 Introduction ..................................................................................... 1

    .....................................................................................................1 Problem I..................................................................................................2 Objective 2

    . ............................................................................3 Theory and Method 3................................................hapter 2 Broom-balance Vehicle Design 6

    2 .1 Basic Components of the System .................................................. 62.2 Basic Design Models ........................................................................... 7

    . ......................................................................................3 Decided Model 1 2..............................3.1 Cart. Pendulum and Pulley System 1 2

    ...................................................................................3 .2 DC Motor 1 62.3.3 Sensors ..................................................................................... 1 62.3 .4 Controller and Amplifier .................................................. 1 7

    Chapter 3 Control System Analysis and Design ................................... 1 93 .1 Dynamic Equations ........................................................................... 1 93 .2 Control System Equations .............................................................. 2 63 .3 System Analysis ................................................................................ 3 6

    3 .3 .1 Stability .................................................................................. 3 63.3 .2 Controllability ...................................................................... 3 9

    ........................................................................3 .3 Observability 4 23 .4 Controller Design ............................................................................... 4 3

  • 8/6/2019 Broom Balancer

    4/118

    P a g e.................................................................................5 Observer Design 5 5

    Chapter 4 Simulation of System Response ......................................... 6 2Chapter 5 Experiment ...................................................................................7 6

    5 . 1 Experimen t Equipments ............................................................ 7 65 .2 Th e Coefficients of S ensors ..........................................................7 6

    ...............................................................................3 Control Program 7......................................3 .1 Equil ibrium Position Readings 7 9

    5 .3 .2 Observer Based Feedback Control Method ............. 8 0........................... 3 .3 Full State Feedback Control Method 8 2

    Chapter 6 Conclusion ...................................................................................... 0References .......................................................................................................... 9 4Append i x A Matalb Program for Control ler and Observer Design 9 6

    .......ppendix B Simulat ion Program of Broom-balance Sys tem 9 8Appendix C Cont ro l Program wi th Observer Basd Feedback

    .....................................................................ontrol Method 10 2Appendix D Control Program with Ful l State Feedback Control

    Method .....................................................................................0 6.........................................ppendix E Procedure of the Experiment 1 10

  • 8/6/2019 Broom Balancer

    5/118

    List of Table

    T a b l e P a g e3. 1 Open-loop System State Values at Time tl ............................ ... 4 7

  • 8/6/2019 Broom Balancer

    6/118

    viiList of Figures

    Figure Page.................................................................room-balancing Vehicle 1

    Block Diagram of a Closed-loop Control System ..................... 6.............omponents of the Broom-balance Vehicle System 7

    Broom-balance System Design Model #1 .................................. 8Broom-balance System Design Model #2 ................................ 1 0Broom-balance System Design Model #3 ................................ 1 1Front View of Broom-balance System ...................................... 13

    .........................................op View of Broom-balance System 14........................................ide View of Broom-balance System 1 5

    Inverted Pendulum with Cart ...................................................... 1 9Free Body Diagram of the Cart .................................................... 2 0Free Body Diagram of the Pendulum ....................................... 2 1

    ..................................................room-balance Vehicle System 2 7.........................................................ircuit Diagram of DC Motor 2 8.......................................................C Motor and Pulley System 3 0

    Variation of Control Gain k l with Different R ...................... 4 9......................ariation of Control Gain k2 with Different R 5 0......................ariation of Control Gain k3 with Different R 5 1......................ariation of Control Gain kq with Different R 5 2

    ....................igenvalues of (A-BK) with the Variation of R 5 3

    ....................igenvalues of (A-BK) with the Variation of R 5 4Feedback Control Input U with Variation of R at the

    .............................................................................................tate x(tl) 5 6

  • 8/6/2019 Broom Balancer

    7/118

    Figure3 .144 . 14.24.34 .44 .54 .64.74 .84.94 .105 . 1

    P a g eBlock Diagram of Feedback System with Observer ........... 5 8Block D iagram of Simulation Program .................................... 6 6

    ....................................ime Response Simulation of State xl 6 7Time Response Simulation of State x2 ..................................... 6 8

    ..................................ime Response Simulation of State x3 6 9Time Response Simulation of State x4 ..................................... 7 0Time Response Simulation of Feedback Control Input U .. 7 1

    hTime Response Simulation of Estimated State X I ............... 7 2/C. .................ime Response Simulation of Estimated State x2 7 3h .................ime Response Simulation of Estimated State x3 7 4h .................ime Response Simulation of Estimated State x4 7 5

    Electric Circuit Connection of Broom-balance Vehicle.................................................................................ontrol System 7 7

    Block Diagram of Control Program Using Observer BasedFeedback Co ntrol Method ............................................................ 8 3Block Diagram of Control Program Using Full StateFeedback Control Method ............................................................ 8 4Tim e Response of S tate x l ......................................................... 8 5Tim e Response of State xz ......................................................... 8 6

    ..........................................................ime Response of State x3 8 7

    ...........................................................ime Response of State x4 8 8.........................ime Response of Feedback Control Input U 8 9

  • 8/6/2019 Broom Balancer

    8/118

    Chapter 1 Introduction

    1 . Problem

    The Broom-balancing vehic le is a dynamic sys tem which canbe shown s imply as f igure 1 I , I t consis ts of an inver ted pendulumand a ca r r i age on which the pendu lum i s mounted wi th the p ivo t .The carr iage should move proper ly in hor izonta l d i rec t ion in order tokeep the pendu lum upr igh t . Th is sys tem i s commonly known as theB r o o m - b a l a n c e r , b e c a u s e t h e p r o b l e m i s v e r y s i m i l a r t o t h a t o fbalancing a broomst ick on one 's hand. The more pract ica l appl ica t ionof the Broom-balancer i s control l ing the a t t i tude of a miss i le or arocket dur ing the in i t ia l s tages of launch. A sa te l l i te booster rocket

    Figure 1.1 Broom-balancing Vehicle

  • 8/6/2019 Broom Balancer

    9/118

    has aerodynamics resembling those of a giant broomstick. The rocketmust be balanced on its own thrust vector by rotating the engine.

    In this thesis , the problem wil l be more s imple than that ofbalancing a rocket which needs to be control led in three dimensions.We s impl i fy t he Broom-ba lanc ing p rob lem in to a two d imens iona lp rob lem. Then we on ly d i scuss t he p rob lem tha t t he pendu lummotion is constrained to be on a plane, and car t is constrained to bealong the horizontal direct ion only.

    1.2 Objec t i v e

    The bas ic goa l of th i s thes i s i s to des ign and cons t ruc t ab r o o m - b a l a n c i n g v e h i c l e w h i c h w i l l a c c o m p l i s h t h e r e q u i r e dper formance . The des ign process wi l l inc lude the mechanica l sys temdes ign and cont ro l sys tem des ign . Mechanica l sys tem des ign i s todes ign the mechanica l s t ruc ture of the Broom-balancer sys tem, tochoose and des ign the components of the sys tem and to f ind theappl icable methods and ava i lab le sensors of measur ing the l inearand angular displacements and veloci t ies . Control system design is toana lyze the f eedback con t ro l sys t em and to des ign the con t ro l l e rthrough es tab l i sh ing the mathemat ica l model of the dynamic sys tem,d e r i v i n g t h e d i f f e r e n t i a l e q u a t i o n s o f m o t i o n s , l i n e a r i z i n g t h eequa t ion wi th t he a s sumpt ion tha t t he va r i ab l e s w i l l s t ay c loseenough to a given operat ing point , analyzing the character is t ics of thefeedback cont ro l sys tem, and des igning the feedback cont ro l le r and

  • 8/6/2019 Broom Balancer

    10/118

    obse rve r w i th s t a t e f eedback con t ro l me thods and l i nea r op t ima lcon t ro l t echn iques .

    1.3 Theory and Method

    Mechanica l des ign knowledge and technique a re used in thedesign of Broom-balancer . The mechanical design includes two basictypes of design problems. One kind of problem is the detai led designof the machine part ; another kind of problem is broader in designscope and deal with synthesizing a mechanical system from a varietyof components such as motors , c lu tches , gearboxes , and brakes . Inthis type of problem, the component is t reated as a black box, onlythe propert ies of the black box wil l be s tudied so that i t can besynthes ized wi th o ther components .

    N e w t o n ' s L a w s a r e a p p l i e d i n o b t a i n i n g t h e d i f f e r e n t i a lequat ions of the system. Also the technique of l inear izing nonl inearsys t ems i s u sed , t h i s t echn ique i s impor t an t s ince i t p rov ides as tandard approach to ana lys i s . In genera l , ana lys i s and des ign a remuch s impler for the l inear control system, most systems are l inearwithin som e range of the variables and they becom e nonlinear i f thevar iab les move over wide enough ranges . The impor tance in prac t iceof l inear techniques re l ies on l inear iza t ion based on the assumpt ionthat the var iables s tay close enough to a given operat ing point andthe limits of operation are small enough to allow a l inear analysis tobe made .

  • 8/6/2019 Broom Balancer

    11/118

    Feedback con t ro l theory i s used fo r des ign ing the con t ro lsys tem and analyzing the proper t ies of the control sys tem, such ass tabi l i ty , control labi l i ty and observabi l i ty of the sys tem. The ful ls ta te feedback control method is used in the feedback control sys temd e s i g n .

    Mo d e r n l i n e a r c o n t r o l t h e o r y i s a p o we r f u l a n d e mi n e n t l yprac t i ca l too l fo r the so lu t ion o f l inea r feedback con t ro l p rob lems .The important character is t ics of modern l inear control theory are thes ta te space descr ip t ion of sys tem in terms of a se t of f i rs t orderd i f fe ren t ia l equa t ions which a re wr i t t en compac t ly in a s t andardmat r ix fo rm, and op t imiza t ion in t e rms o f quadra t i c pe r fo rmancec r i t e r i a . S o t h e s t a t e v a r i a b l e me t h o d i s u s e d i n d e v e l o p i n g t h esys tem equa t ions . The s ta te va r iab le fo rm i s ve ry conven ien t fo rdeve lopment o f compute r p rograms which can be used fo r ana lys i sand design of the system.

    L i n e a r o p t i m a l c o n t r o l t h e o r y i s u s e d i n t h e f e e d b a c kcon t ro l l e r des ign . Cont ro l sys tem des ign i s to ob ta in the des i redperformance of the sys tem, i t can be considered to be a manipula t ionto achieve or to fulfi l a given objective, such as to transfer the stateto a des i rable one . When more than a s ingle unique control can ful f i lthe g iven ob jec t ive , the mos t des i rab le con t ro l in the sense o fminimizing a g iven cr i ter ion funct ion can be used, th is i s the opt imalcontrol . Linear optimal control is a special sort of optimal control , thep lan t tha t i s con t ro l l ed i s a s sumed l inea r and the con t ro l l e r , thedevice that genera tes the opt imal control i s const ra ined to be l inear .Linear control ler i s achieved by working wi th quadrat ic performance

  • 8/6/2019 Broom Balancer

    12/118

    i n d i c e s . N u m e r i c a l m e t h o d i s u s e d f o r s o l v i n g t h e d i f f e r e n t i a lequa t ions in the s imula t ion o f sys tem per fo rmance . A F O R T R A Nprogram i s genera ted to s imula te the response o f the sys tem wi ththe given in i t ia l condi t ions . The Runge-Kut ta-Fehlberg method, whichis one of the most commonly used method for solving in i t ia l va luep r o b l e m s o f o r d i n a r y d i f f e r e n t i a l e q u a t i o n s , i s i n t r o d u c e d i n t h es i m u l a t i o n p r o g r a m b y c a l l i n g a I MS L s u b r o u t i n e I VP R K i n t h eIMSLIB.

    M e c h a n i c a l k n o w l e d g e a n d t e c h n i q u e a r e u s e d i n t h ec o n s t r u c t i o n a n d t h e e x p e r i m e n t o f t h e B r o o m - b a l a n c e v e h i c l es y s t e m . A d a t a a c q u i s it i o n s o f t w a r e N I - D A Q i s u s e d i n t h eexper iment , and the control programs in C language are developed tocon t ro l the b room-ba lance veh ic le sys tem.

  • 8/6/2019 Broom Balancer

    13/118

    Chapter 2 Broom-balance Vehicle Design

    2.1 Basic Components of the System

    F e e d b a c k c o n t r o l s y s t e m a r e c l o s e d - l o o p c o n t r o l s y s t e mswh i c h a r e u s u a ll y c o n s i s t o f : ( 1 ) th e p l a n t , t h e s y s t e m t o b econtrol led; (2 ) measurements , obta in the informat ion about the p lantand send back to con t ro l l e r ; and (3 ) the con t ro l l e r , the cen t ra lprocess ing uni t of the control sys tem which compares the measuredvalues of the plant to the desired values and adjus ts the input valuesto mainta in the output a t the des i red value .

    The c losed- loop con t ro l sys tem can be s imply i l lus t ra ted asf i g u r e 2 .1 . T h e b a s i c c o mp o n e n t s o f t h e B r o o m- b a l a n c e s y s t e mshould include: (1) the inver ted pendulum and the carr iage on whicht h e p i v o t o f t h e p e n d u l u m i s mo u n t e d ; ( 2 ) d r i v i n g s y s t e m, t h eactuat ing uni t tha t controls the mot ion of the carr iage; (3) sensors ,

    -Controller

    Input Output

    Figure 2.1 Block Diagram of a Closed-loop Control system

    Measurementslant -

  • 8/6/2019 Broom Balancer

    14/118

    the measurement componen ts , and (4) control ler , the in te l l igent par to f the Broom-ba lance sys tem, which p rocesses the feedback s igna l sand provides the ac tuat ing s ignals to the dr iv ing sys tem. These canalso be displayed in block diagram as f igure 2.2.

    Figure 2.2 Components of the Broom-balance Vehicle System

    InputController

    Sensors

    2.2 Basic Design Models

    DrivingSystem*

    c

    During the des ign of the Broom-balance sys tem, severa l k indsof models were considered. Three of them wil l be in t roduced in thefo l lowing pages .

    The f i rs t des ign model i s shown in f igure 2.3. The pendu lumis mounted on the car t which has four wheel tha t i t can move f ree lya long the hor izon ta l d i rec t ions . The ca r t i s connec ted to a pu l leysystem which is f ixed on the p la t form. The DC control motor shaf t i sconnected to the pulley wheel shaft . The l inear motion of the cart iscontrol led by the DC motor . Potent iometer sensors are connected to

    Inverted Pendulumand Cart

    Output

  • 8/6/2019 Broom Balancer

    15/118

    Figure 2.3 Broom-balance System Design Model #1I . Inverted Pendulum with Cart , 2. Patform, 3. Pul ley sys tem,4. D C Motor , 5 . Controller

  • 8/6/2019 Broom Balancer

    16/118

    the pivot of the pendulum and the pul ley wheel shaf t to measure theangula r pos i t ion of the pendulum and the angula r pos i t ion of thepul ley whee l , t achometer sensor can be connec ted to pu l ley whee lshaf t or motor sha f t to measure the angular veloci ty of the pul leyw hee l . F l ex ib l e w i r e s a r e u sed t o connec t t he s enso r s and thecontroller so that the wires will not affect the motion of the cart . Aamplif ier can be connected between the control ler and DC motor toamplify the control s ignals .

    The second design model , as shown in f igure 2.4, has a similardr iving system as the f i rs t model . The dif ference between the secondmodel and the f i rs t model is that the car t of the second model moveson the guided t racks ra ther than on the pla t form with the wheels .The ca r t i s d r iven by t he DC motor th rough the pu l ley sys tem.Po ten t iome te r s a r e u sed t o measu re t he angu la r pos i t ion o f t hependulum and the pu l ley whee l , t achometer i s used to measure therotating speed of the pulley wheel.

    F i g u r e 2 .5 show s the th i rd mode l o f the Broo m-b a l ancevehicle system design. The s t ructure of th is model is d if ferent f romthe f i r s t and second models , f rom so me po ints th i s des ign m odel i smore l ike a vehicle than the other two models . The car t , wi th fourwheels , i s dr iven by the control motor ins ta l led on the car t . a gearbox t ransmission may be used to connect the motor and car t wheel toob ta in the be t te r s t ruc ture and per formance of the sys tem. Angula rpos i t ion sensors can be p laced on the pendulum p ivo t and the ca r twh eel shaf t to mea sure the angular posi t ions of pendulum and car twheel . Also, the angular veloci ty sensor can be placed on the car t

  • 8/6/2019 Broom Balancer

    17/118

    &$ z 0= 3 5CE C ZCa s 03 ua- ? c .m a mC ,-g a s

    w 2 . z'" L.i,D

  • 8/6/2019 Broom Balancer

    18/118

    Figure 2.5 Broom-balance System Design Model #31 . Inverted Pendulum with Cart. 2. G ear Box , 3. DC Motor ,3 . Contro l ler

  • 8/6/2019 Broom Balancer

    19/118

    wheel shaft or the motor shaft to measure the angular velocity of thecar t wheel . The control ler is connected by f lexible wires to sensorsand D C con t ro l mo to r . Th i s des ign mode l does no t need spec i a lplatform, i t can work at any horizontal surface.

    2.3 Decided Model

    To choose the des ign mode l t o bu i ld up fo r ana lys i s andexper iment , there a re many fac tors need to be cons idered , such asthe ava i lab le mater ia l s , the ava i lab le components , the d i f f icu l ty ofmaking the par ts , the complicat ion of the mechanical s t ructures andthe cost of the instruments and materials, etc.

    After consider a l l these factors , we decided the second designmodel of the Broom-balance vehic le sys tem discussed in proceedingsection as the model to analyze and to set up for the experiment. Theassembly drawing of the system is shown as f igure 2.6.

    2.3.1 Cart , Pendulum and Pul ley System

    As shown in f igure 2 .6 , the car t i s mounted on the t rackwhich is fixed on the platform. The cart can be moved along the trackby apply ing the torque on the pul ley wheel . Between the car t andtrack, bearing balls are used to reduce the friction. On the top of thecar t , a L shape seat is mounted to support the potent iometer which isu s e d t o m e a s u r e t h e a n g u l a r p o s i t i o n o f t h e p e n d u l u m . T h ependulum is f ixed on the shaf t of the potent iometer by a screw. The

  • 8/6/2019 Broom Balancer

    20/118

  • 8/6/2019 Broom Balancer

    21/118

  • 8/6/2019 Broom Balancer

    22/118

    Unit: mm

    Figure 2.6(c) Side View of Broom-balance System

  • 8/6/2019 Broom Balancer

    23/118

    pu l l ey sy s t em inc lude a cha in and fou r cha in w hee l s , e ach tw owhee ls a re ins ta l led on a L shape suppor t which i s f ixed on thep l a t f o r m .

    The dimension of the pendulum and the car t are i l lus t ra ted inf igure 2 .6 . The mater ia l of the pendulum is Aluminium, the mass ofthe pendulum is 0 .0829 kg. In measur ing the mass of the car t , a l lmoving par t s should be cons idered , tha t wi l l inc lude the ca r t , thepotent iometer , the potent iometer seat and the chain, so the mass ofthe car t is 1 .2698 kg.

    2.3.2 DC Motor

    In the Broom-balance system, the DC motor is f ixed on a Lshape seat, the posit ion of the seat is adjustable by loose the bolts inorder to ob ta in a good connec t ion be tween motor shaf t and cha inwheel shaf t . The shaf t of the DC motor is connected with the chainwheel shaft by a flexible connector.

    The character is t ics of the DC motor which was chosen in thedesign are as fol lowings:

    The torque constant , KT = 0 .0459 N-mlamp;The e .m.f . constant , K b = 0.0217 V-slrad;The armature res is tance, R, = 2.3 ohm;The armature inductance, L = 20 Millihenries;The Moment of inertia of motor shaft, J, = 0.000005 kg-m2.

  • 8/6/2019 Broom Balancer

    24/118

    2 .3 .3 Sensors

    T w o p o t e n t io m e t e r s a r e u s e d f o r m e a s u r i n g t h e a n g u l a rposi t ion of the inver ted pendulum and the angular posi t ion of thechain wheel . The l inear posi t ion of the car t can be determined fromthe angular posi t ion of the chain wheel .

    The po t en t iome te r t ha t u sed fo r measu r ing t he pendu luma n g u l a r p o s i t i o n i s M o d e l S J 6 6 0 , m a d e b y H e l i p o t C o r p . I t scharac te r i s t i cs a re :

    Resistance: 30 K ohms +I- 5% ;Linearity: +I- 0.15%.

    The po ten t iometer tha t used for measur ing the cha in whee l angula rpos i t i on i s SP IRA LPO T 85 172K- 10-15C, made by Giannin i Con t ro l sCorp . It has fol lowing proper t ies :

    Resis tance: 1500 ohms;Turns: 10;Linea rity: +I- 0. 1%.

    T h e a n g u l a r v e l o c it y o f t h e m o t o r i s m e a s u r e d b y atachometer which is mounted direct ly to the end of the DC motor , asshown in f igure 2 .6 . The veloci ty of the car t can be calculated f romthe angular velocity of the motor.

  • 8/6/2019 Broom Balancer

    25/118

    2.3.4 Control ler and Amplif ier

    The control ler that used for the exper iment is a Macintosh IIxcomputer . The sof tware used for the con t ro l exper iment i s the N I-DAQ Sof tware fo r Macin tosh which i s a da ta acquis i t ion sof twaredeve loped by Nat iona l Ins t rument .

    An ampl i f ie r i s used to ampl i fy the ou tpu t con t ro l s igna lsfrom computer and send the enlarged signals to the DC control motor .The power ampl i f ie r i s Model PA118, made by Labworks Inc . Thelargest amplification ratio of the amplifier is 55.

  • 8/6/2019 Broom Balancer

    26/118

    1 9Cha pt er 3 Control System Analys is and Design

    3.1 Dynamic Equations

    To ana lyze the dynamic sys tem of the Broom-Balancer , f i r s twe need to de t e rmine the equa t ions o f mo t ion tha t gove rn ing thesys t em. The Broom-ba lanc ing sys t em i s shown as f i gu re 3 . 1 wh ichcons is t s of a car t and an inver ted pendulum a t tached to the car t .Where M is the m ass of ca rt; m is the m ass of p endu lum ; 1 is the halflength of pendulum; u is the force applying on the car t to keep thependulum upr ight ; z i s the pos i t ion of car t and 8 i s t he angu la rposi t ion of the pendulum.

    Figure 3.1 Inverted Pendulum with Cart

  • 8/6/2019 Broom Balancer

    27/118

    The free-body diagram of the system can be drawn as figure3.2 and 3.3 where Hf and Vf are forces exerted by the cart on thependulum; b2 is the viscous friction coefficient for rotary motion ofthe pendulum; bl is the viscous friction coefficient for linear motionof the cart; g is the gravitational acceleration and J I is the momentinertia of pendulum with respect to the center of gravity.

    By applying Newton's second law to the horizontal motion ofthe cart, and to the horizontal, vertical and rotary motions of thependulum, we have

    z- H f

    The cart :

    U e

    The pendulum: d LHf = m-(1 sine + z )d t

    . . . . . . . . . . . . . . . . . . . . .Figure 3.2 Free Body Diagram of the Cart

    IM 4 b l i

  • 8/6/2019 Broom Balancer

    28/118

    I V fFigure 3.3 Free Body Diagram of the Pendulum

    where 1Jr = -m l23Equation (3.3) can also be written in the form

    To eliminate Hf and Vf, substitute Hf and Vf in equations (3.1)and (3.4) by equations (3.2) and (3.5) to get

  • 8/6/2019 Broom Balancer

    29/118

    d2 dz d2u - m - ( l s i n e + z ) - b l - = M -zdt (3.6)dt2 dt2a n d

    [ m g + m d-2_(1 ose) ] 1s ine - [m L ( l in edt2 dt2d2 de+ z ) ] lc o s 8 = I - I+ b2-dt (3.7)dt2d 2 d 2 . .Since - ( 1 s i n ~ ) = - ( l c o s ~ 8 ) = - l s i n ~ 8 + 1 c o s 0 0 (3.8)d t 2 d t

    d . 2 .and d-1 cos 8) =- -1 sin0 8 ) = - 1 cose 9 +l sine (3 (3.9)d t 2 d tEqu ations (3.6 ) and (3.7) can be written as:

    Obviously , these differential equations are nonlinear , they cann o t b e s o l v e d e x c e p t by n u m e r i c a l m e t h o d s , s o a f u r t h e rsimplif icat ion is needed.

    S ince the ob ject o f con t ro l l ing the sys tem is to keep thependulum upr igh t , i t i s reasonab le to assume that 8 and 8 willremain c lose to zero . With th is assumpt ion , the equat ions can belinearized by retaining only those terms which are linear in 0 and 8,

  • 8/6/2019 Broom Balancer

    30/118

    . 2and neglecting higher order terms such as e2,8 and 88, becau se theywill be insignificantly small when 0 and e are close to zero.

    From trigonometric expansions, there will be

    Subst i tu te these two equat ions in to equat ions (3 .10) and(3.11) and drop the higher order terms. The l inearized different ialequations for the cart and pendulum system are:

    These approximate equations will be valid and accurate aslong as the condition is maintained such that 0 z 0 r 0.

    To solve equations (3.14) and (3.15), rewrite them as

  • 8/6/2019 Broom Balancer

    31/118

    Then plug equation (3.16) into equation (3.15), and equation(3.17) into equation (3.14) we get

    Let A = ( M + m ) J I + M m 1 2

    Equations (3.18) and (3.19) can be written as:

    Also, we can havedz = i and - 8 = 8d t d t

    Then equations (3.20), (3.21) and (3.22) can be expressed asfollowing

  • 8/6/2019 Broom Balancer

    32/118

    They can be written in matrix form as

    This is the linearized differential equations which governs thebehavior of the broom-balancer under the condition that 9 remains m a l l .

  • 8/6/2019 Broom Balancer

    33/118

    3.2 Control System Equations

    To derive the feedback system equations, we wil l consider thewhole system, the inverted pendulum with the cart and the drivingsystem, which are shown as figure 3.4. The variables appeared in thef igure are :

    V,: The input armature voltage of the DC motor; (Volt)i: The arma ture current of the DC motor; ( A )Rm : The resistance of the DC motor; (ohm)Tm: Th e output torque of the DC motor; (Nm )KT: The torque constant of the DC motor; (Nm/A)r: Th e radius of the chain wheel; (m )em: The angular position of the motor shaft; (rad)e m : The angular velocity of the motor shaft; (rad/s)u: The force that the chain applying on the cart; (N )z: The horizontal position of the cart; (m)z: The velocity of the cart; (m/s)0 : Th e angular position of the pendulum ; (rad)0: Th e angular velocity of the pendulum . (rad/s)

    Th e l inea r i zed d i f fe ren t i a l eq ua t ion s o f mo t ions o f theinver t ed pendu lum and the ca r t have been der ived a s equat ion(3 .27) . Next , we need to der ive the d i f feren t ia l equat ions o f thecontro l sys tem.

    Since the DC motor is the armature control, i ts output torqueis proport ional to the armature cur ren t[ l] , th is can be expressed as:

  • 8/6/2019 Broom Balancer

    34/118

  • 8/6/2019 Broom Balancer

    35/118

    The armature circuit of DC motor can be shown as Figure 3.5.L is the armature inductance; R, is the armature resistance; V, isthe input voltage of the motor; i is the armature current; K b is thee.m.f. constant; e m is the angular position of the motor shaft and e m isthe angular velocity of the motor. The voltage equation of the circuiti s

    divide the equation with R,, we have

    I

    Figure 3.5 Circuit Diagram of DC Motor

  • 8/6/2019 Broom Balancer

    36/118

    the value of L/R, is very small, the first term in the equation (3.30)can be neglected. So equation (3.30) becomes

    Then the torque T, can be expressed as

    The shaft of the DC motor is connected directly to the shaft ofthe chain wheel, we can derive the equation of motion with figure3.6. J m is the moment of inertia of the motor shaft, the equation ofmotion can be written as

    The differential equations of the cart and the pendulum havebeen derived, so the differential equations of the control system canbe obtained by combining the differential equations of cart andpendulum with the differential equation of the motor.

    Since the relation between z , the horizontal position of thecart, and 8 , the angular position of the motor shaft is

    also

  • 8/6/2019 Broom Balancer

    37/118

    z = re,. .z = re, (3.36)

    and the angular position of the motor shaft will be measured insteadof the position of the cart, the linearized differential equations of thecart with inverted pendulum (3.14) and (3.15) can be rewritten as

    . . .(M + m) re, + ml 9 = - b l r e m + u

    Simplify equations (3.33) and (3.37) by eliminating the forceu, we get

    0

    v m

    u 0

    Figure 3.6 DC Motor and Pulley System

  • 8/6/2019 Broom Balancer

    38/118

    3 1.. . .(M + m)r29, + mlr 9 = -blr29, + Tm - Jm9, (3.39)

    substitute Tm by equation (3.32), we can have. .[(M + m)r2+ Jm]e m+ m118 +(blr2+ -10 m -- T Vm (3.40)

    Rm Rm

    equations (3.38) and (3.40) can be written in the form. .e m = - - - 1 ( J ~ m12)e - bz + g o (3.41)

    m r l m r l..e = -- M r 2 + m r 2 + l m ) 8 , - 1 ( b lr 21 ~ ~ ~ ~ ) 0 ~ +T Vm (3 .42)m r l m r l Rm mrlR,

    .Su bs titu te 9, in equation (3.40 ) by equatio n (3.4 1) and e inequation (3.38) by equation (3.42), we have

    ( ~ r ~ + m r ~ + ~ ~ ) ~ ~ + M m r ~ l ~ + m ~ ~ l ~0 = (b l r 2 KT&) 0mm r l Rm+ M r 2 + m r 2 + l m ) g - ( M r 2 + m r 2 + ~m ) b 2 (3 .4 3)r m r l Rm

    ( ~ r ~ + r n r ~ + ~ ~ ) ~ I + ~ r n r ~ l ~ + m ~ ~ l ~.( ~ ~ + m l ~ ) ( b ~ r ~ m

    em =- Rm e mm r l m r l

    Let A , = (Mr2+ mr2 + J m ) J I+ ~ m r ~ 1 ~ml,12

  • 8/6/2019 Broom Balancer

    39/118

    equations (3.44) and (3.43) will become

    2+ m rlb2 + ( J ~ + m lKTv m (3.45)A s A s R m

    also we can have

    Then e quat ions (3 .45 ) , (3 .46) , (3 .47) and (3 .48 ) can beexpressed in matrix form

  • 8/6/2019 Broom Balancer

    40/118

    w h e r ea 3 2 = - r n 2 1 ~ ~ r1

    As

    This is the different ia l equat ions of the feedback controlsystem. Define the state vector x that

    The system equation (3.49) can be expressed as

  • 8/6/2019 Broom Balancer

    41/118

    This is the linearized feedback control system equation in thestate variable format. The system equation can also be written in thefo rm

    where A and B are matrices, they are expressed as equations ( 3 . 5 3 )and (3.54),U is V,.

    wh e re

  • 8/6/2019 Broom Balancer

    42/118

    The output of the plant can be given by

  • 8/6/2019 Broom Balancer

    43/118

    where C is the output matrix, for the system that x l , x2 and x3 will bemeasured, C is equal to

    3.3 S y s t e m A n a l y s i s

    In this section, several properties of the feedback controlsystem will be discussed.

    3.3.1 Stability

    The stability of the state differential equations can be definedas Kwakernaak [2] discussed.

    Consider the state differential equation

    with the nominal solution xo(t). Then the nominal solution is stable inthe sense of Lyapunov if for any to and any e > 0, there exists a6 ( & , o) > 0 such that I x(t0) - xo(tO) I I 6 implies I x(t) - xo(t) I < E for allt 2 t o .

    The nominal solution xo(t) of the state differential equation(3.57) is asymptotically stable if it is stable in the sense of Lyapunov,

  • 8/6/2019 Broom Balancer

    44/118

    and fo r all to there exists a p( t ) > 0 such that I x(t0) - xo(tO) I < pimplies I x(t) -xo(t) I -+ 0 as t -+ 00.

    For the t ime-invariant l inear system

    The necessary and suff icient condi t ion to be asymptot ical lystable is that all i ts characteristic roots have strictly negative realparts. So for the system

    If there is a small but unknown perturbation of the initialstate and if u is an open-loop control, the equilibrium point would beasymptotically stable if all the eigenvalues of A had negative realparts, and it would be unstable if at least one eigenvalue of A had apositive real part. The eigenvalues of A are the roots of

    T h e A mat r ix has been deve loped as equa t ion (3 . 53 )Introducing the values of all parameters in the matrix, which are

    M = 1.2698 kg;m = 0.0829 kg;1 = 0.1745 m;

  • 8/6/2019 Broom Balancer

    45/118

    J, = 0.000005 kg-m2;R, = 2.3 ohm;K T = 0.0459 N-m/amp;K b = 0.0217 V-s/rad;b l = 0.05 N-s/m;b2= 0 .005 N-m-s / rad ;g = 9.8066 kg-m/s2;r = 0.0127 m.

    the A matrix yields

    Th e e igenva lues of m at r ix A can be calculated by usingMatlab, eigenvalues of matrix A are

    Since there a re pos i t ive rea l e igenva lues , the equi l ib r iumpoin t o f the open- loop sys tem i s uns tab le . The s tab i l i za t ion ofinstable system is one of the major aims of feedback design.

    The l inear ized system

  • 8/6/2019 Broom Balancer

    46/118

    with state feedback

    would be asymptotically stable if all the eigenvalues of A - BK, that isall the roots of

    had negative real parts.The feedback control gain matrix K could be chosen to make

    the coefficients of the characteristic equation (3.65) have any desiredvalues, not only the eigenvalues to have the negative real parts.

    If the linearized system x = A x + B U is controllable, then thematrix K can always be chosen to attain any desired set ofeigenvalues of (A - BK).

    3.3.2 Controllability

    When we solving the control problems, it is important toknow whether or not the system which will be analyzed has theproperty that it may be steered from any given state to any othergiven state, this is the controllability of the system.

    For the linear system given by equation (3.63), if there existsan input U which transfers the initial state x(0) = xo to the zero statex( t l ) = 0 in a finite time tl , the state xo is said to be controllable. If all

  • 8/6/2019 Broom Balancer

    47/118

    ini t ial s tates are control lable, the system is said to be completelycon t ro l l ab l e . [3 ]

    To determine a n-dimensional l inear t ime- invar iant sys tem,such as equat ion (3.63) , whether or not is completely control lable,the necessary and sufficient condition is that the column vectors ofthe control labi l i ty matr ix

    span the n-dimensional space. This condition can also be expresseda s

    rank r, = n (3.67)

    Since matrix A has been given in equation (3.61), before weconstruct the control labi l i ty matr ix, subst i tut ing the parameters inmatrix B by their values. From equation (3.54) we know matrix B is

    w h e r eAs = (hTr2+ mr2 + Jm)J I+ ~ m r ~ l ~rnJm12;

  • 8/6/2019 Broom Balancer

    48/118

    the parameters a re

    M = 1.2698 kg;m = 0.0829 kg;1 = 0.1745 m;r = 0.0127 m;R m = 2.3 ohm;K T = 0.0459 N-mlamp;

    After introducing the values, matrix B becomes

    Then the controllabil i ty matrix and i ts rank can be calculatedby using Matlab. we get the controllabil i ty matrix as

    and the rank of the controllability matrix is

    Rank T , = 4 (3.71)

  • 8/6/2019 Broom Balancer

    49/118

    So the system (3.63) is completely controllable.

    3.3.3 Observability

    The controllability condition assures the existence of the inputwhich to transfer the state of the system from the initial state to adesired state. On the other hand, it is sometimes necessary to knowall state variables from the measurement of the output whosedimension is less than that of the state. The observability conditionassures the construction of the state from the output. If the system isobservable as well as controllable, the system can be made stable byfeedback through an observer.

    The observability of the system can be determined bychecking the observability matrix. The necessary and sufficientcondition for the system to be completely observable is that theobservability matrix has full rank.[4]

    For the system (3.63), the observability matrix is

    where C is the output matrix of the system. If the rank of matrix r,equal to 4, the system is completely observable.

    Assuming that the outputs of the system are Om, 0 and o m , i tmeans that only xl x2 and x3 three states can be measured, then theoutput matrix C is

  • 8/6/2019 Broom Balancer

    50/118

    so that

    Also, the observability matrix and its rank can be calculatedby using Matlab, we get

    a n dRank T o = 4 (3.76)

    So, when only xl, x2 and x3 can be measured, the system iscomple te ly observable .

    3.4 Control ler Des ign

    In the design of feedback controller in state space, there aretwo major methods, pole assignment method and opt imal controlmethod. In pole ass ignment , the feedback cont ro l ga ins can beachieved by placing the closed-loop poles in specified locations, asJohn Van de Vegte [ 5 ] discussed, for the feedback control system

  • 8/6/2019 Broom Balancer

    51/118

    any specified set of closed-loop eigenvalues can be obtained by statefeedback with K consisting of constant gains if and only if the pair (A ,B) is controllable.

    The eigenvalues in pole assignment design could be placed farfrom the imaginary axis, making the speed of response arbitrarilyfast. But this would require large control inputs and actuatorcapacities, this implies a high cost of control. The choice of desiredeigenvalues in pole assignment techniques implies a trade-offbetween performance and cost of control.

    In optimal control, the controller is achieved by working withquadratic performance indices and minimizing a specifiedperformance criterion. The control is sought which gives the besttrade-off between performance and cost of control.

    To design the controller of the Broom-balancing feedbackcontrol system, for the controllable system

    the optimal control which minimizes the value of a performanceindex J of the form

  • 8/6/2019 Broom Balancer

    52/118

    i s g iv ing by

    w h e r e P i s a s y m m e t r i c a n d p o s i t i v e d e f i n i t e m a t r i x ; Q i s asymmet r ic and pos i t ive semidef in i t e ma t r ix wr i t t en as Q=H~Hwithobservable pair (A , H), and rank (H) = rank (Q); R is a symmetric andposi t ive def in i te matr ix and P is the solution of the algebraic matrixRiccati equation, which is

    Q and R are weighting matrices and are usually diagonal. Theterm s xTQx and U ~ R U f the in tegrand a re quadra t i c fo rms whichmeasure , respect ively , the performance and the cos t of control . So,o n e o f t h e i m p o r t a n t p r o c e d u r e s i n t h e c o n t r o l l e r d e s i g n i s todetermine the weight ing matr ices Q and R .

    Suppose Q matrix has the form

    t h e n

  • 8/6/2019 Broom Balancer

    53/118

    To determine the values of q l , q2, q 3 and q4, we have to findthe relations between the states xl x2 and x3 x4 . These relations aredetermined by the system equations of motion. Apparently, therelations of xl x2 and x3 x4 are not constant values, they will varywith the changes of the states, and they also can be effected byinitial conditions of the system and the input control force.

    Since we have assumed that x2 and x4 will remain to smallvalues, approximated values can be made to represent the relationsbetween the states xl x2 and the states x3 xq.

    Using the open-loop system equation

    with the initial condition x(0) = 0 and U(0) = Uo, and a constant inputU(t) = UO, e can get x(tl), the values of the states at time tl, fromx ( t l ) the relations among the states can be decided withapproximation.

    A FORTRAN program is used with the IMSL subroutine IVPRKto solve the system differential equations.

  • 8/6/2019 Broom Balancer

    54/118

    When the response of the open- loop sys tem have beencalculated, we can choose certain values of the state variables fordiscussion, that is at time tl, the value of state variable x2 which isthe angular position of the pendulum, closes to + 5 O (+0.08727 rad) or- 5 O (-0.08727 rad). With three different init ial inputs, we have x(tl)shown in Table 3.1.

    From equation (3.84) and Table 3.1, we can find that

    Table 3.1 Open-loop System State Values at Time t l

    Since the main objective of the control is to maintain thependulum in upright position, i t is obvious that the states xa and x4which represent the angular position and the angular velocity of thependulum are more impor tant than the s ta tes x l and x3 w h i c hrep res en t the position and the velocity of the cart . S o the relative

    UO(v)2 .55 O

    7 . 5

    x3( t l )( r a d l s )2 4 . 8 8 9 43 8 . 4 2 7 14 5 . 7 7 5 3

    t 1( s )0 . 1 2

    0 . 0 90 . 0 7

    ~ 4 0)( r a d l s )- 1 . 3 7 7 4- 2 . 0 7 8 0- 2 . 4 5 2 8

    x l ( t l >( r a d )1 . 5 5 3 5

    1 . 7 8 2 31 . 6 3 9 0

    x2(t 1)@a- 0 . 0 8 4 1- 0 . 0 9 5 7- 0 . 0 8 7 9

  • 8/6/2019 Broom Balancer

    55/118

    weighting of x2 and x4 should be larger than those of xl and x3, so Qmatrix can be determined as

    Since Q=H=H, H matrix could be

    so that H can meet the requirements that (A , H) should be observable

    and rank (H ) = rank (Q).For single input control problem, R should be a scalar, the

    sui table value of R for the optimal control can be obtained bystudying the feedback control gains and the eigenvalues of ( A - BK)with R changes. Figures 3.7, 3.8, 3.9 and 3.10 show respectively thechanges of control gain K = [ k l , k2 , k3, k4] when the value of Rincreas es f rom zero to inf in i ty; Figures 3.1 1 and 3.12 show thevariations of the eigenvalues of ( A - BK), the system poles, with thedifferent values of R.

  • 8/6/2019 Broom Balancer

    56/118

    Figure 3.7 Variation of Control Gain k l with Different R

  • 8/6/2019 Broom Balancer

    57/118

    Figure 3.8 Variation of Control Gain k2 with Different R

  • 8/6/2019 Broom Balancer

    58/118

    Figure 3.9 Variarion of Control Gain k3 with Different R

  • 8/6/2019 Broom Balancer

    59/118

    Figure 3.10 Variation of Control Gain k4 with Different R

  • 8/6/2019 Broom Balancer

    60/118

    Figure 3.1 1 Eigenvalues of (A-BK) with the Variation of R

  • 8/6/2019 Broom Balancer

    61/118

    Figure 3.12 Eigenvalues of (A - BK) with the Varaition of R

  • 8/6/2019 Broom Balancer

    62/118

    From figures 3.7, 3.8, 3.9, and 3.10, we can see the variationsof control gain with the changes of R, all the values of control gainbecome smaller when R increases. How to choose the appropriatecontrol gain is related to the applicable range of the DC motor inputvoltage and the possible largest state values. The input voltage of theDC motor can be ranged from -10 V to 10 V. If we consider one setof state values in Table 3.1 as the largest possible state value whichis shown as equation (3.90). Then required input voltages can becalculated with the state values and different control gains by theequation (3.91). Since the control gains are corresponding to R, therelation between U and R for certain state values can be plotted asfigure 3.13.

    From figure 3.13, a suitable value of R can be chosen suchthat the control input voltage U is in the available range. Thecontroller gain is decided as K = [-0.0707, -43.4045, -0.1 3 10 -6.04711at R = 200.

    3.5 Observer Des ign

    When the state variables of the system are not all availablesince some reasons, such as the difficulties of measurements or thecost. Full state feedback control method is not applicable. For the

  • 8/6/2019 Broom Balancer

    63/118

    Figure 3.13 Feedback Control Input U with Variation of R

  • 8/6/2019 Broom Balancer

    64/118

    system which is completely observable, the output feedback controlmethod can be used. An observer can be designed to determine anestimate ; f the state x from the measured output y, and then sendthe estimated state to the controller.[6]

    For the system

    where

    the state equation of observer can be written as

    and the feedback control law can be formed as

    the block diagram of the system with observer is shown as figure3.14.

    The error of the state estimation by observer can be defined

  • 8/6/2019 Broom Balancer

    65/118

  • 8/6/2019 Broom Balancer

    66/118

    then from equations (3.92) and (3.94), we have

    From equation (3.99), we can see that the error will becometo zero as time goes to infinite, if all the eigenvalues of (A - MC) havenegative real parts.

    Substitute U in equation (3.92) with equation (3.95), we get

    equation (3.96) can also be expressed as

    then equation (3.100) becomes to

    x = (A - BK)x + BKe (3.102)

    Combine equation (3.102) with equation (3.99), we can have

    0 (A-MC)

  • 8/6/2019 Broom Balancer

    67/118

    So the closed-loop system with initial conditions

    is stable if all the eigenvalues of (A - BK) and (A - MC) have negativereal parts.

    For the completely controllable system, K can be chosen toyield any set of desired eigenvalues of (A - BK) , these have beendiscussed and designed in the proceeding sections. Also the systemhas been proved to be completely observable, so M can be chosen toyield any desired set of eigenvalues of (A - MC). Apparently, thedesign of M is an important procedure. Similar to the techniquesused for the design of feedback control matrix K, M can be decidedby placing the observer eigenvalues in specified locations. Theselocations could be chosen to the left of those used for design of K tomake the observer faster. K has been chosen as

    the eigenvalues of (A - BK) are

  • 8/6/2019 Broom Balancer

    68/118

    I f we make the observer two t imes fas te r , the e igenvalues of (A -MC) should be

    Since the eigenvalues of (A - MC) are equal to the eigenvaluesT T T Tof ( A ~ C M ), M can be calculated from ( A ~ C M ) with thedecided eigenvalues. B y using the Matlab program PLACE, M matrix

    is obtained as fol lowing

  • 8/6/2019 Broom Balancer

    69/118

    Chapter 4 Simulat ion of System Response

    After f inish the control system design, a computer s imulat ionof the system performance is usual ly a necessary procedure to s tudythe sys t em re sponse , t o ve r i fy t he ma thema t i ca l mode l s o f t hesystem and to evaluate the design of the system.

    Th e respon se of the sys tem can be obta ined by u t il iz ing ad iscre te - t ime approximat ion , the sys tem is represented by a s ta tev e c t o r d i f f e r e n t i a l e q u a t i o n . T h e d i s c r e t e - t i m e a p p r o x i m a t i o n i sbased on the divis ion of the t ime axis into suff ic ient ly small t imeincrements . Then the va lues of the s ta te var iab les a re eva lua ted a tthe successive time intervals; such as t = 0, T , 2 T , 3T, a m . , where T isthe increment of t ime At = T. If the T is suff ic ient ly small , there sponse eva lua t ed by d i sc re t e - t ime me thods wi l l be r ea sonab lya c c u r a t e .[7]

    The feedback control system can be represented as

    where

    the ful l s ta te observer equat ion is

  • 8/6/2019 Broom Balancer

    70/118

    the feedback control is

    In the designed feedback control system, the state variablesX I , xz and x j which are Om,O and 8, can be measured and all the statewill be estimated by the observer from the output, then theestimated state will be sent back to the controller. In the simulationof the system response, the state values will be calculated fromsystem differential equations and assumed initial conditions.

    With the given initial conditions of the system and with thetime increment At = T, The state, the system output and theestimated state at t = T can be obtained by solving the differentialequations (4.1) and (4.3) with the initial values x(0) = xo, c(0) = 0 andU(0) = Uo, the state is x(T), the output is y(T) and the estimated stateis ;(T), the feedback control input is U(T) which is obtained fromhx(T). In each time interval T, the system equation (4.1) and observerequation (4.3) can be solved as the initial value problem of ordinarydifferential equations. At time t, the state is x(t), the estimated stateis ;(t) and the control input is U(t). At time t+T the state x(t+T) can beobtained by solving the differential equations with the initial valuesx(t) and U(t), the output at time t+T is y(t+T) = C x(t+T), then theestimated state at time t+T, ;(t+~), can be calculated by solving thestate equations of the observer with the initial values ;(t) and U(t) ,

  • 8/6/2019 Broom Balancer

    71/118

    and then U(t+T) can be calculated through the feedback control lawu ( ~ + T )= -K;(~+T).

    The t ime increment procedure of the discrete- t ime equat ionscan be shown as following

    with init ial condit ions:x(0) = xo, ;(o) = 0 and U(0) = Uo (4.5)

    at time t = T,x(T) = A x ( t ) d t + B U ( O )I' (4.6)

    at t ime t = ( n+ l ) T ,

  • 8/6/2019 Broom Balancer

    72/118

  • 8/6/2019 Broom Balancer

    73/118

    Read the system dataA, B, C, M and K

    Set time increment TI(0) = xo , xh(0) = 0 and

    II Set time t to zero ICalculate state xt(t+T) at time t+T from

    Calculate the output y(t+T) at time t+TCalculate estimated state xht(t+T) at timet+T from estimated state xh(t) and input

    U(t) at time t, and output y(t+T) at time t+T

    time t+T, U(t+T) = -Kxht(t+T)Time increment, t = t + T

    IOutput time t, state xt(t), estimated state xht(t)

    and control input U(t)III Set x(t) = xt(t) I

    Figure 4.1 Block Diagram of Simulation Program

  • 8/6/2019 Broom Balancer

    74/118

    Time (second)

    Figure 4 .2 Time Response Simulation of State xl

  • 8/6/2019 Broom Balancer

    75/118

    4 6

    Time ( second)

    Figure 4.3 Time Response Simulation of State x2

  • 8/6/2019 Broom Balancer

    76/118

    0 2 4 6 8 1 0

    Time (second)

    Figure 4.4 Time Response Simulation of State x3

  • 8/6/2019 Broom Balancer

    77/118

    0 2 4 6 8 10Time ( second)

    Figure 4.5 Time Response Simulation of State x4

  • 8/6/2019 Broom Balancer

    78/118

    4 6

    Time ( second)

    Figure 4.6 Time Response Simulation of FeedbackControl Input U

  • 8/6/2019 Broom Balancer

    79/118

    Time (second)

    Figure 4.7 Time Response Simulation of Estimated State

  • 8/6/2019 Broom Balancer

    80/118

    0 2 4 6 8 1 0

    Time ( second)

    Figure 4.8 Time Response Simulation of Estimated State G2

  • 8/6/2019 Broom Balancer

    81/118

    4 6

    Time (second)

    Figure 4.9 Time Response Simulation of Estimated State x3

  • 8/6/2019 Broom Balancer

    82/118

    0 2 4 6 8 1 0

    Time (second)

    Figure 4.10 Time Response Simulation of Estimated State g4

  • 8/6/2019 Broom Balancer

    83/118

    Chapter 5 Experiment5 .1 E x pe r im e nt E quipm e nts

    To set up the experiment equipments, it includes two steps.First step is to assemble the Broom-balance system, this is mainlythe mechanical works. The second step is to connect the electriccircuit of the control system.

    The Broom-balance system is constructed as shown in figure2.6. During the assembling of the system, it is important that to makesure the screws of the connections are tight.

    The electric circuit connections of the Broom-balancefeedback control system can be shown as figure 5 .1 . Before toconnect the circuit, a test of the potentiometers should be taken todetermine the wiper end and other two ends of resistance element,also the tachometer should be tested to determine the relationbetween the voltage outputs and the motor rotating directions.

    5.2 The Coeff ic ients of Sensors

    Since the outputs of the sensors are all voltages, and what weneed for the control calculation are the position and the velocityvariables. First, we need to find the relations between the voltagereadings of the sensors and the displacements and velocities whichbeing measured. These relations can be denoted as coefficients Kpp,K ~ m nd Kt, which represent the coefficient of potentiometer

  • 8/6/2019 Broom Balancer

    84/118

  • 8/6/2019 Broom Balancer

    85/118

    m e a s u r i n g t h e p e n d u l u m a n g u l a r p o s i t i o n , t h e c o e f f i c i e n t o fpotent iometer measur ing the motor shaf t angular posi t ion and thec o e f f i c i e n t o f t a c h o m e t e r m e a s u r i n g t h e m o t o r r o t a t i n g s p e e dr e s p e c t i v e l y .

    For the potent iometers , a constant vol tage is appl ied by theampli f ier to the two ends of the potent iometer , the output of thepotent iometer i s a var iable vol tage which is measure between thewiper end and one of the two ends. The re la t ions of the outputvoltage and the posit ion variable can be expressed as

    w here Vp p is the output voltage of the potent iometer m easuring theangular posit ion of the pendulum; O p is the angular displacement oft he pend u l um ; V p m i s t he ou t pu t vo l tage of t he po t en t iome t e rmeasuring the angular posit ion of the motor shaft ; 8 is the angulardisplacement of the motor shaft .

    Make a cer ta in angular displacement of the pendulum, usethe vol tmeter to measure the output vol tage of the potent iometer ,the coeff ic ient Kp p is determined as 0 .16552 Vol t l rad. By the samemethod, Kp m is determined as 0 .01720 Vol t l rad.

    The output voltage of the tachometer is proportional to theangular velocity which it measured, the relation can be written as

  • 8/6/2019 Broom Balancer

    86/118

    w h e r e Vt, is the output voltage of the tachometer; om s the angularvelocity of the motor.

    Apply a constant vol tage to the motor , the rotat ing speed ofthe motor can be obtained by using a s tandard tachometer , measurethe output vol tage of the tachometer wi th a vol tmeter , the Kt, isde te rmined f rom equat ion ( 5 . 3 ) , Kt, is equal to 0.0304 Vol t - shad .

    5 .3 Control Program

    T h e s o f t w a r e N I - D A Q S o f t w a r e f o r M a c i n t o s h , a d a t aacqu i s i t ion so f tware deve loped by N a t iona l In s t rumen t C orp . , wasused for the exper iment . The sof tware can be used by running acont ro l program wr i t ten in C l anguage . Two cont ro l methods wereused in the exper iment , one i s the observer based feedback cont ro lmethod, another one is the ful l s ta te feedback control method.

    Since the amplif ier is used to amplify the control s ignals , thevalues of control gains used in the control program should be dividedby the amplif icat ion rat io .

    5.3.1 Equil ibr ium Posi t ion Readings

    In the cont ro l program, the center pos i t ion readings of thepotent iometers should be set to indicate the equi l ibr ium posi t ion oft h e s y s t e m . T h e s e r e a d i n g s a r e t h e o u t p u t v o l t a g e s o f th e

  • 8/6/2019 Broom Balancer

    87/118

    potentiometers when the pendulum is at zero degree upright positionand the cart is at the center of the track.

    Since there are cer ta in er rors be tween the data computerobtained and the sensor outputs, the calibration of the data readingin the control program is necessary. This work can be done bykeeping the system in the equilibrium position, at this position, allthe output values of the sensors are same as the values that set inthe program, set the amplification ratio of the amplifier to zero, thenrun the control program to get the data that read by the computer.The values of all three measured state variables should close to zeroat this time, if there are errors, they can be corrected by adding orsubtracting the same value of errors to the state variables in theprogram so that the state values become to zero.

    5.3.2 Observer Based Feedback Control Method

    Since the state are not all measured in the experiment, weneed to get the unmeasured values of the state, the angular velocityof the pendulum, to complete the feedback control of the system. Toestimate the state by using an observer is one method.

    The analysis and design of observer have been discussed inChapter 3. To apply the observer method in the control program, aconversion of observer equat ion from cont inuous t ime model todiscrete t ime model is needed. In the s imulat ion program thatdiscussed in preceding chapter, the Runge-Kutta method was used tosolve the differential equations. In the control program, the observer

  • 8/6/2019 Broom Balancer

    88/118

    equation is changed into a discrete t ime state space model by usingt h e M a t l a b f u n c t i o n C 2 D . T h e c o n v e r s i o n c a n b e e x p r e s s e d a sf o l l o w i n g

    for the sys temx = A x + B U

    y = C x

    the observer equat ion i sAx = A ~ + B u + M ( ~ - c ; ~ )

    the equation can also be wri t ten asAx =(A - BK - M C); My (5.7)

    le t A= - BK - MC and B = M; the equation (5.7) becomesA - -x = A ~ + B ~ ( 5 . 8 )

    by using the Matlab function - -[@,I?] = C2D (A, B, T)where T is the sample t ime.the continuous t ime system is converted to the discrete t ime system

    Ax(n+ l ) = @;n) + r y(n) (5.10)

  • 8/6/2019 Broom Balancer

    89/118

    The block diagram of the cont rol program that use theobserver method is shown as figure 5.2.

    5.3.3 Full State Feedback Control Method

    The second method used in the experiment is the full statefeedback control method. Since the angular velocity of the pendulumis not measured, to achieve the ful l s tate feedback control , thecalculation of the angular velocity of the pendulum is needed. Wecalculate the angular velocity of the pendulum from the measuredangular positions of the pendulum. The calculation can be written as

    x4(t+At ) is the calculated angular velocity of the pendulum at timet+At, x2(t+At ) is the angular position of the pendulum at time t+At,x2(t) is the angular position of the pendulum at time t, A t is the timeinterval between the two readings, it is equal to the time interval ofeach loop that the computer program runs. The value of the t imeinterval is determined by the speed of the computer, for MacintoshIIx, th e A t is close to 0.013 second. So, when we calculate the angularvelocity of pendulum using equation (5 .11 ), the value of A t should beclose to that value. This time interval value is also applicable for thesample t ime which i s d i scussed in the preceding sec t ion aboutconversion of state space model from continuous to discrete time byusing the Matlab function C2D.

  • 8/6/2019 Broom Balancer

    90/118

    Define Variables1b e t Parameter Values I

    I

    Read the Voltage Output of Sensors

    Calculate the State ValuesvCalculate the Estimated State Valueswith the Observer EquationL a t e he Control Voltage (

    Output the Control Voltage CommandcFigure 5.2 Block Diagram of Control Program Using

    Observer Based Feedback Control Method

  • 8/6/2019 Broom Balancer

    91/118

    The block diagram of control program that using the full statef eedback con t r o l me t hod i s shown a s f i gu r e 5 .3 . The sys t emresponses in the exper iment are shown as f igures 5 .4 , 5.5 , 5 .6 , 5 .7and 5.8.

    Define Variables

    I Set Parameter Values I

    Read the Voltage Output of Sensors

    Calculate State x4 with the Equationx4 (t + At) = X 2 (t+At) - X 2 (t)At

    I

    Calculate the Control Voltage

    1 Output the Control Voltage Command 1

    Figure 5.3 Block Diagram of Control Program UsingFull State Feedback Control Method

  • 8/6/2019 Broom Balancer

    92/118

    Time (second)

    Figure 5.4 Time Response of State xl

  • 8/6/2019 Broom Balancer

    93/118

    Time ( second)

    Figure 5.5 Time Response of State x2

  • 8/6/2019 Broom Balancer

    94/118

    Time (second)

    Figure 5.6 Time Response of State x3

  • 8/6/2019 Broom Balancer

    95/118

    Time (second)

    Figure 5.7 Time Response of State x4

  • 8/6/2019 Broom Balancer

    96/118

    4 6 8 10 1 2 1 4

    Time (second)

    Figure 5.8 Time Response of Feedback Control Input U

  • 8/6/2019 Broom Balancer

    97/118

    Chap ter 6 Con clusion

    Dur ing the des ign o f b room-ba lance sys t em, t h ree des ignmodels of the sys tem were cons idered and d iscussed , one of themwas chosen and bui l t up for the experiment . Basic methods of designthe cont ro l sys tem and mechanica l sys tem were deve loped . Also themechanica l measurement techniques were appl ied in the exper iment .

    In e s t ab l i sh ing the dynamic mode l o f t he Broom-ba lancesystem, the different ia l equat ions of motions of the dynamic systemw e r e d e v e l o p e d . T h e l i n e a r i z a t i o n f o r t h e n o n l i n e a r d i f f e r e n t i a lequa t ion o f t he sys t em was made wi th t he a s sumpt ion tha t t hedeviat ion angle and angular veloci ty of the pendulum should remainclose to zero.

    The s ta te space model of the feedback cont ro l sys tem wases tab l i shed for the ana lys i s and des ign of the cont ro l sys tem. Thes tab i l i ty , cont ro l lab i l i ty and observabi l i ty of the feedback cont ro lsys tem were d iscussed and tes ted . The Broom-balance sys tem, whichhas the inverted pendulum and the car t , is an unstable system. Thesys t em i s comple t e ly con t ro l l ab l e . When the s t a t e x4, the angularvelocity of the pendulum, is not measured, the system is observable.

    The op t ima l con t ro l me thod was app l i ed in t he con t ro l l e rdes ign . A M at lab program was used for the sys tem ca lcu la tion andcon t ro l l e r des ign . A l so , t he obse rve r me thod was d i scussed fo rest imating the s ta te values when some of the s ta tes of the systemcan not be measured . The observer was des igned by using the PoleP l a c e m e n t M e t h o d .

  • 8/6/2019 Broom Balancer

    98/118

  • 8/6/2019 Broom Balancer

    99/118

    readings, these random reading errors will destroy the continuity ofthe state estimation.

    The loose of the mechanical connections in the driving system.This delays the actions of the control input.

    By using the full state state feedback control method, thesystem can be controlled much better than using the observer basedfeedback control method. The advantages of this method are that thefeedback control input is calculated directly from the measured statevalues, only the state x4 is calculated from state x2 with a certaintime interval At, this will avoid the calculation errors produced insolving the observer equation. Also the full state feedback controlsystem are affected less by the random reading errors caused bynoises than the system with observer, since only one state variablehas the cont inu i ty problem in the fu l l s ta te feedback cont ro lprogram. As same as using the other method, any loose of th emechanical connection in the system such as the shaft connection willmake the system lose control.

    Some suggestions may be helpful for the further study on thistopic or related problems.

    Another kind of full state feedback control method can bedeveloped to improve the experiment. To use a differential circuit toobtain the angular velocity of the pendulum from the output of thepotentiometer which measuring the pendulum angular position.

    Based on the techniques developed for the two dimensionalBroom-balance feedback control system, the future work on t h i s

  • 8/6/2019 Broom Balancer

    100/118

    topic may proceed to analyze and develop the three dimensionalBroom-balance feedback control system.

  • 8/6/2019 Broom Balancer

    101/118

    References

    1 . C a rs te n s, J . R . , A u t o m a t i c C o n t r o l S y s t em s a n d C o m ~ o n e n t s ,Prent ice-Hal l , Inc . , New Jersey, 1990.

    2 . K w a k er na ak , H . an d S iv a n, R . , L i ne ar O ~ t i m a l Con t ro l Svs t ems ,John Wiley & Sons, 1972.

    3 . Hostetter, G. H. , Design Of Feedback Control Svstem s, CBS CollegePubl ishing, New York, 1982.

    4. D'azzo , J . J . and Houpis, C . H., Linear Control Svsv tem An alysis andDesign, McGraw-Hil l , Inc . , New York, 1975.

    5 . Van De Vegte , J . , Feedback Cont ro l Sys tem s , Pren t ice -Hal l, NewJe r sey , 1990 .

    6 . Furu ta , K . , S ta te Var iab le M ethods in Au toma t ic C ont ro l , JohnWiley & Sons, New York, 1988.

    7 . W h i c k e r , M . L . a n d S i g e l m a n , L . , C o m p u t e r S i m u l a t i o nApplicat ions , Sage Publ icat ions , Inc . , Newbury Park, CA, 1991.

    8 . IM SL M A TH I L IBRA RY , U SER 'S M A N U A L, Fo r t r an Sub rou t ine sFor Mathematical Appl icat ion, Vol 2 , Chapter 5, P633-639 .

    9 . Anderson , B . 0. and Moore, J . B., Optimal Control Linear Ouadrat icMethods, Prent ice-Hal l , Inc . New Jersey, 1990.

    1 0 Thaler, G. J . , Autom atic Control Svs tem, Wes t Publ i sh ing Company ,M N , 1989 .

    1 1 Bollinger, J . G. and Duffie, N. A., Computer Control of Machines andProcesses , Addison Wes ley Publ i sh ing Company Inc . , 1988 .

    1 2 Nise, N. S. , Con trol Systems Engineer ing, The BenjaminIC umm ingsPubl ishing Company, Inc . , Redwood City , CA, 1992.

  • 8/6/2019 Broom Balancer

    102/118

    1 3 Dorf , R . C . , M odern Cont ro l Sys tems , A ddison W es ley Publ i sh ingCompany , Inc . , Massachuse t t s , 1989 .

    1 4 Derusso , P . M. , S ta te Var iab les fo r Engineers , John Wiley & Sons ,New York, 1965.

    1 5 Smith , J . M. , Mathem at ica l M odel ing and Dig i ta l S im ula t ion forEng ineers and Scientists , John wiley & Son, Inc . , Canada, 1987.

    1 6 NI-DAQ for Macin tosh , Sof tware Reference User 's M anual .1 7 Shearer, J . L . and Kulakowski, B. T., Dynamic Model ing and Cont ro l

    of Engineer ing Sys tems , Macmi l lan Publ i sh ing Company , NewYork , 1990 .

  • 8/6/2019 Broom Balancer

    103/118

    Appendix A Matlab Program for Controllerand Observer Design

    % Program for the des ign of feedback control ga in us ing opt imal control% method and the design of observer using pole placement method.% Calculate the discrete-time state space system with function C2D% m l : Mass of the car t (kg);% m2: Mass of the pendulum(kg);% 1: Half length of the pendulum(m);% fc : Viscous-fr ic t ion coeff ic ient of ro tary motion(N-m-shad);% f f : Viscous-fr ic t ion coeff ic ient of l inear mot ion(N-dm);% jm: Moment of inetia of the motor shaft(kg-m*m);% r: Resistance of motor(ohm );% kt : Coeff ic ient of torque(N-mlamp);% kb: E.M.F. constant of the motor(V-shad);% rr: Radius of chain wheel(m);m1=1 .2698 ;m 2 = 0 . 0 8 2 9 ;1=0.1745;fc=0 .005 ;f f=0 .05 ;g=9.8;j m = 0 . 0 0 0 0 0 5 ;r = 2 . 3 ;r r = 0 . 0 1 2 7 ;k t=0 .0459 ;k b = 0 . 0 2 1 7 ;j = m 2 * 1 * 1 / 3 ;delta=j*((ml+m2)*rr*rr+jm)+mlm 2 * r r * r r * l * l + m 2 * l * l * j r n ;a 3 2 = - m 2 * m 2 * r r * l * l * g / d e l t a ;a33=-(ff*rr*rr*r+kt*kb)*(j+m2*l*l)/(delta*r);a 3 4 = m 2 * r r * l * f c / d e l t a ;a 4 2 = ( ( m 1+ m 2 ) * r r * r r + j m ) * m 2 * l * g / d e l t a ;a43=(ff*rr*rr*r+kt*kb)*m2*rr*l/(delta*r);a44=-fc*((ml+m2)*rr*rr+jm)/delta;b 3 = ( j + m 2 * l * l ) * k t / ( d e l t a * r ) ;b 4 = - m 2 * r r * l * k t / ( d e l t a * r ) ;% A matrix;a=[O 0 1 0;O 0 0 1;O a32 a33 a34;O a42 a43 a441;% B matrix;b=[O 0 b3 b41';%C matr ix ;c=[l 0 0 0; 0 1 0 0; 0 0 1 01;%Q matrix;q= [l 0 0 0; 0 342.25 0 0; 0 0 1 0; 0 0 0 342.251;%Input the value of R;r o l = i n p u t ( ' i n p u t r o l = ' ) ;d i s p ( ' a = ' ) , d i s p ( a ) ;d i s p ( ' b = ' ) , d i s p ( b ) ;d i s p ( ' c = ' ) , d i s p ( c ) ;

  • 8/6/2019 Broom Balancer

    104/118

    d i s p ( ' q = ' ) , d i s p ( q ) ;d i s p ( ' r o l = ' ) , d i s p ( r o l ) ;%Calcu la te the con t ro l l ab i l i ty ma t r ix ;conmtx=[b a*b a*a*b a*a*a*b] ;%Calcu la te the obse rvab i l i ty ma t r ix ;obrmtx=[c l a ' *c ' a t*a ' *c ' a ' *aa ' *a ' *c ' ] ;c a b l t y = r a n k ( c o n m t x ) ;o a b l t y = r a n k ( o b r m t x ) ;d i sp ( 'Rank o f con t ro l l ab i l i ty ma t r ix= ' ) ,d i sp (cab l ty ) ;d i s p ( ' R a n k o f o b s e r v a b i l i t y m a t r i x = ' ) , d i s p ( o a b l t y ) ;e i g a = e i g ( a ) ;disp( 'E igenva1ues of A matr ix= ' ) ,d isp(e iga) ;%Calcula te the opt imal feedback control ga in K ;k = l q r ( a , b , q , r o l ) ;d i sp ( 'Opt ima1 feedback con t ro l ga in K= ' ) ,d i sp (k ) ;e i g k = e i g ( a - b * k ) ;disp( 'E igenva1ues of (A-BK)=' ) , d isp(e igk);%Decide the pos i t ion of observer poles ;t left= inpu t( ' inp ut the t imes of poles go left , t left=' );p o l e = ( t l e f t - l ) * r e a l ( e i g k ) + e i g k ;disp( 'po1e locat ions = ' ) ,d isp(pole) ;%Calcula te the M matr ix ;m=(place(a l , c ' , pole)) ' ;d isp( 'The M matr ix = ' ) ,d isp(m);e i g m = e i g ( a - m * c ) ;disp( 'E igenva1ues of (A-MC)=' ) , d isp(e igm);%Change the obse rve r equa t ion to d i s c re te - t ime mode l ;s p l t = 0 . 0 2 5 ;a b a r = a - b * k - m * c ;b b a r = m ;[ph i , gamma]=c2d(aba r , bba r , sp l t ) ;d i s p ( ' P h i ='), disp(phi) ;d i s p ( ' G a m m a ='), disp(gamma) ;

  • 8/6/2019 Broom Balancer

    105/118

    Appendix B Simulation Program of Broom-balanceS y s t e m

    C PROGRAM T O SIMULATE THE BROOM-BALANCE SYSTEM RESPONSEC USING THE RUNGE-KUTTA-VERNER FIFTH-ORDER AND SIXTH-ORDER ME THODC SOLVE THE INITIAL-VALUE PROBLEM FOR ORDINARY DIFFERENTIALEQUATIONC VARIABLESEXTERNAL FCN1 FCN2,IVPRK,SSETREAL PARAM(SO),T,TENDR EAL X(4),XT(4),XH(4),XHT(4),A(4,4),B(4),Y(3),K(4)REAL M(4,3),C(3,4),MC(4,4),U,UO,UT,TA,TIME,DTC INTEGER ID0C CONSTANTREAL EPS,TOL

    INTEGER NEQ,MXPARMPARAM ETER (EPS= 1 OE-05 TOL= 1 OE-05 NEQ =4,MXPA RM= 50)C GLOBLE VARIABLESCOMM ON ISYSI A,B,UCOMM ON IOBSI M,MC,YC ARGUMENTSC I D 0 - FLAG INDICATING THE STATE OF THE COM PUTATION.C NEQ - NUMBE R O F DIFFERENTIAL EQUATIONS. (INPUT)C FCN1,2 - USER-SUPPLIED SUBROUTINE TO EVALUATE FUNCTIONS.C T - INDEPENDENT VARIABLE. (INPUTIOUTPUT)C TEND - VALUE O F T AT WHICH THE SOLUTION IS DESIRED. (INPUT)C TOL - TOLERANCE FOR ERROR CONTROL. (INPUT)C PARAM - VECTOR O F LENGTH 50 CONTAINING OPTIONAL PARAMETERS.C X,XH - VECTOR OF LENGTH NEQ OF DEPENDENT VARIABLES.CC BEGINC SET PARAM TO DEFAULTCALL SSET (MXPARM, 0 .0 , PARAM ,l)C SELECT ABSOLUTE ERROR CONTROLPAR AM(l0 ) = 1.0C READ DATA OF SYSTEM, OBSERVER AN D CONTROLLEROPEN (3, FILE='sim.datal, STAT US='OLDt)

    DO 10 I=1,4READ (3,*) (A(I,J),J=1,4)10 CONTINUEREAD (3,") (B(I), I=1,4)READ (3,") (K(I), I=1,4)DO 20 I=1,4R EAD (3,*) (M(I,J),J=1,3)20 CONTINUECLOSE (3)C SET ELEMENT VALUES OF C MATRIX

  • 8/6/2019 Broom Balancer

    106/118

    DO 30 I= 1,3DO 40 J= 1,4C(I,J)=O40 CONTINUE30 CONTINUEC ( l , l ) = lC(2,2)=1C(3,3)=1C SET THE INITIAL VALUESDO 50 I=1,4X(I)=OXH(I)=O50 CONTINUETIME=ONT=ON=OC INPUT INITIAL POSITION OF MOTOR SHAFTWRITE (6,") 'ENTER THE INITIAL POSITION OF MOTOR SHAFT 'READ(5,") X(l )C INPUT INITIAL POSITION OF PENDULUMWRITE (6,") 'ENTER THE INITIAL POSITION OF PENDULU M'READ(5,*) X(2)TIME=ONT=ON=OC INPUT INITIAL VALUE OF INPUT VOLTAGEWR ITE (6,") 'ENTER THE INITIAL VALU E OF INPUT VOL TAGE UO'RE AD (5,") UOC INPUT TIME INCREMENT STEP DTWRITE (6,") 'ENTER THE TIME INCREMENT ST EP DT'

    READ (5,") DTC INPUT THE OUTPUT STEP NOUT (WRITE OUTPUT EVERY NOUT LOOPS)WRITE (6,") 'ENTER THE OUTPUT STE P NOUT'READ (5 , " ) NOUTC MATRIX MULTIPULATION, MC=M*CDO 70 I= 1,4DO 80 J=1,4MC(I,J)=M(I,l)*C(l,J)+M(I,2)*C(2,J)+M(I,3)*C(3,J)80 CONTINUE70 CONTINUEC OUTP UT INITIAL VALUESOPEN (2, FILE='simout.dat' , STATUS='NEW')WRITE(2,71) X(1),X(2),U07DT,NOUT71 FORMAT(' I,' Xl= ',F7.4, ' X2=',F7.4, ' UO=',F5.2, ' DT =', F7.5, ' NOUT=',I4,)WRITE(2,") ' K=',KDO 85 I=1,4WRITE(2,86) (I,J,M(I,J),J=1,3)86 FOR M AT(",3(2X , 'M(' , I l ' , ', I1 ' )= ' ,F9.5))85 CONTINUEWR ITE(2,5) TIM E,(X(I), 1=1,4), UOOPEN (7,FILE='obsvr.dat ', STATU S='NEW1)WR ITE(7,5) TIM E, (XH(I), 1=1,4), UO

  • 8/6/2019 Broom Balancer

    107/118

    u=uoC LOOP110 CONTINUEDO 140 I=1,3Y (I)=X(I)140 CONTINUEC SOLV E EQUATION DX/DT=A*X+B*UC INITIAL ENTRYIDO= 1T=TIMETEND=TIME+DTCAL L IV PRK(IDO,NEQ ,FCNl ,T,TEND,TOL,PARAM,X)DO 120 I=1,4XT(I)=X(I)120 CONTINUEC FINAL C ALL TO R E L E A S E WOR KSPAC EIDO=3CALL IVPRK(IDO,NEQ,FCNl ,T,TEND,TOL,PARAM,X)CC SOLVE EQUATION DHX/DT=(A-M*C)*XH+B*U+M*YC INITIAL ENTRYI D O = lT=TIMETEND=TIME+DTCALL IVPRK(IDO,NEQ,FCN2,T,TEND,TOL,PARAM,XH)DO 130 I=l ,NEQXHT(I)=XH(I)130 CONTINUEC FINAL CALL TO RELEASE WORKSPACEIDO=3

    CALL IVPRK(IDO,NEQ,FCN2,T,TEND,TOL,PARAM,XH)C UT=ODO 180 I=1,4UT=UT-K(I)*XHT(I)180 CONTINUEC NT=NT+lTIME=NT*DTN = N + lC OUTPUTIF(N.EQ.NOUT) THEN

    WRITE(2,5) TIME,(XT(I), 1=1,4),UTWRITE (7,5) T IME,(XHT(I), I= 1,4),UTN=OENDIFC CHECK THE MAXIM UM DEVIATION OF ANGLE FROM VERTICAL POSITIONIF(TIME.LT.TA) G O T 0 185IF(((ABS(XT(2)).LT.EPS).AND.(ABS(XT(4)).LT.EPS)).OR.(TIME.GT.& 15)) G OT 0 999C185 CONTINUE

  • 8/6/2019 Broom Balancer

    108/118

    DO 190 I=1,4X(I)=XT(I)XH(I)=XHT(I)190 CONTINUEU=UTC ENDLOOPG O T 0 110999 CONTINUECLOSE (2)CLOSE (7)5 FORMAT(",2X,F8.3,2X,4F10.6,2X,E11.4)STOPENDCC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .SUBROUTINE FCN l(NEQ,T,X,XPRIM)INTEGER NEQR EAL T,X(4),XPRIM(4),A(4,4),B(4),UCOMM ON /SYS/ A,B,UD O 1 0 I = l , 4XPRIM(I)=ODO 20 J= l , 4XPRIM(I)=A(I,J)*X(J)+XPRIM(I)20 CONTINUEXPRIM(I)=XPRIM(I)+B I)*U10 CONTINUERETURNEND

    CSUBRO UTINE FCN2(NEQ,T,XH,XHPRIM)INTEGER NEQR EAL T,XH(4),XHPRIM(4),A(4,4),B(4),U,M(4,3)REAL MC(4,4),Y(3)COMMON /SYS/ A,B,UCOMM ON /OBS/ M,MC,YD O 1 0 I = 1 , 4XHPRIM(I)=OD O 2 0 J = l , 4XHPRIM(I)=(A(I,J)-MC(I,J))*XH(J)+XHPRIM(I)20 CONTINUEXHPRIM(I)=XHPRIM(I)+B(I)*U+M(I,l)*Y(l)+M(I,2)*Y(2)+M(I,3)*Y(3)

    10 CONTINUERETURNEND

  • 8/6/2019 Broom Balancer

    109/118

    Appendix C Control Program with Observer BasedFeedback Control Method

    #include # inc lude < conso le .h># inc lude < m ath .h># inc lude < s td l ib .h># inc lude < s t r ing .h>#include "NI-DAQ-MAC.hU#define COUNT 1OOOL#define COUNT-2 3072#def ine SWITCH 512#define OK 1#def ine PI 3.141592654void pause( void );f loat save[7] [COU NT];in t 16 b rd ;i n t l 6 c h a n - o u t ;in t l6 *b in -v -ou t ;int16 out-mode, upd ate-mode, update-signal , update-edge;float volt-out;f loat ou t - range;i n t l 6 c h a n - i n - 0 ;in t16 chan- in-1 ;in t16 chan- in-2 ;in t l 6 inpu t-m ode , i npu t- r ange , po la r i t y ;int 16 gain-0, *bin-v-in-0;in t l 6 gain-1, *bin-v-in-1;int 16 gain-2, *bin-v-in-2;float *volt- in-0;float *volt-in-1 ;f loat *volt- in-2;float pos-err;float x-1, x-2, x-3, x-1-0, x-2-0, x-3-0, x-4-0;flo at x-1-o-old, x-2-o-old, x-3-o-old, x-4-o-old;flo at aa-1, aa -2, aa-3, aa-4, bb-1, bb-2, bb-3, bb-4;fl oa t v-gain-1, v-gain-2, v-gain-3, v-gain-4;f loa t cen te r -pos , cen te r -pend ;flo at mtr-pos-fac, mtr-speed-fac, pend-pos-fac;float *volt-in-0;float *volt-in-1 ;f loat *volt- in-2;floa t sgn-x, v-frict;l o n g * c o u n t ;m a i n ( ){ i n t l6 i ;long j;

  • 8/6/2019 Broom Balancer

    110/118

    brd = 2;chan-out = 0 ;out-mode = 0;out-range = 10.0;update-mode = 0;update-signal = 0 ;update-edge = 0;chan-in-0 = 0;chan-in-1 = 1;chan-in-2 = 2 ;input-mode = 0 ;input-range = 10 ;polarity = 0;gain-0 = 1;gain-1 = 1;gain-2 = 1;/*set the coeff ic ien ts o f the sensors* /mtr-pos-fac = 94.510.128; I* mmlvolt * Imtr-speed-fac = 41 7.7632; I* mm/volt-sec */pend-pos-fac = PI/0.520; /* mm/volt *// * se t t he cen te r pos i t ions* /center-pos = 0.298; /* voltage when x-1 = 0 * Icenter-pend = 0.396; I* voltage when x-2 = 0 *//*vol tage when car t can be moved against f r ic t ion*/v-frict = 0.042;/*In i t ia l ize board and input - output* /AO -Setup( brd , chan-out, out-mode, out-range, update -mod e, update-sign al,u p d a t e - e d g e ) ;

    AI-Config( brd, input-mode, input-range, polarity );volt-out = 0.0;

    AO -VScale( brd, chan-out, volt-out, bin-v-out );AO-Write ( brd, chan-out, *bin-v-out );/ * se t f eedback con t ro l ga in* /v-gain-1 = -0.001 2855 ;v-gain-2 = -0.789173;v-gain-3 = -0.00238 18;v-gain-4 = -0.109947;/ * s t a r t l o o p * /for( j = 0; j < COUNT; j++ ) {/ *Read m o to r po ten t iom ete r vo l t age* /AI-Read ( brd, chan -in-0, gain-0, bin-v-in-0 );AI-S cale( brd , gain-0, *bin-v-in-0, volt-in-0 );x-1 = (float)*volt-in-0;

    x-1 = (center-pos-x-l)*mtr-pos-facll2.7;x-1 = x-1 - 2.92479001e-01;

  • 8/6/2019 Broom Balancer

    111/118

    / * R e a d p e n d u l u m p o t e n t i o m e t e r v o l t a g e * /AI-Read ( brd , chan-in-2, gain-2, bin-v-in-2 );AI-S cale( brd , gain-2, *bin-v-in-2, volt-in-2 );x-2 = (float)*volt-in-2;x-2 = (center-pend-x-2)*pend_pos_fac;x-2 = x-2 - 3.247318e-02;

    / * R e a d m o t o r t a c h o m e t e r v o l t a g e * /AI-Read ( brd , chan-in-1, gain-1, bin-v-in-1 );AI-S cale( brd , gain-1, *bin-v-in-1, volt-in-1 );x-3 = (float)*volt-in-1 ;x-3 = x-3*mtr-speed-facll2.7;x-3 = x-3 +1.6061885649263e-01;

    volt-out = -v-gain-l*x-1-0 - v-gain-2*x-2-0 - v-gain-3*x_3-0 -v-gain-4*x-4-0 ;/*decide the s ign of f r ic t ion*/if( volt-out < -0.0001 )sgn-x = -1.0;else if( volt-out > 0.0001 )sgn-x = 1.0;elsesgn-x = 0.0;

    volt-out = volt-out + v-frict*sgn-x;

  • 8/6/2019 Broom Balancer

    112/118

    /*set the bounds of the control voltage*/if( volt-out > 0.17 )volt-out = 0.17;else if(vo1t-out < -0.17)volt-out = -0.17;elsevolt-out = volt-out;

    / *Ou tpu t t he vo l t age com m and* /AO -VSc ale( brd , chan-out, volt-out, bin-v-out );AO-Write ( brd , chan-out, *bin-v-out );

    /*save the values of states, est imated states and control voltage*/save[O]lj] = x-1;save[l] l j] = x-2;save[2]ti] = x-3;save[3]lj] = x-1-0;save[4] ti] = x-2-0;save[5]u] = x-3-0;save[6]b] = x-4-0;save[7]lj] = volt-out;

    volt-out = 0.0;AO -VSc ale( brd, chan-out, volt-out, bin-v-out );AO-Write ( brd, chan-out, *bin-v-out );AO-Update(2);pause();/*print out the values of states, est imated states and control voltages*/for( j = 0; j < COUNT; j++)pr in t f ("%e\ t%e\ t%e\ t%e\ t%e\nU, save[O]h] , save [ l ] [ j ] , save [3]u] , save[4] t i ] ,

    s a v e [ 7 l [ j l ) ;p a u s e ( ) ;for( j = 0; j < COUNT; j++ )prin t f ("%e\ t%e\ t%e\ t%e\nW, save[2] lj], save[5]l j] , save[6] t i ] , save[7] t i ]) ;1vo id pause( ){ char c;

    printf("\nPause . . . ' I ) ;c = getchar();c = c;1

  • 8/6/2019 Broom Balancer

    113/118

    Appendix D Control Program with Full StateFeedback Control Method#include #include #include #include #include #include "NI-DAQ-MAC.h"#define COUNT lOOOL#defin e COUNT-2 3072#define SWITCH 512#define OK 1#define PI 3.141592654void pause( void );float save[7] [COU NT];in t l 6 b r d ;in t l6 chan -ou t ;in t l6 *bin-v-out;in tl 6 out-mode, update-mode, update-signal, update-edge;float volt-out;f loat out- range;int 16 chan-in-0;in t l6 chan - in -1 ;int 16 chan-in-2;int 16 input-mode, input-range, polarity ;in t 16 gain-0, *bin-v-in-0;i n t l 6 gain-1, *bin-v-in-1;int 1 6 g ain-2, *bin-v-in-2;float *volt-in-0;float *volt-in-1;float *volt-in-2;f loat pos-err;float x-1, x 2 , x-3, x-4;float x-2_old, x-2-old-l ;flo at v-gain-1, v-gain-2, v-gain-3, v-gain-4;f loa t center-pos, center-pend;floa t mtr-pos-fac, mtr-speed-fac, pend-pos-fac;float *volt-in-0;float *volt-in-1;float *volt-in-2;flo at sgn-x, v-frict, sgn-x-1, x-frict;float dt, t, tstep;long *count;FILE *ofp;

    long j;

  • 8/6/2019 Broom Balancer

    114/118

    brd = 2;chan-out = 0;out-mode = 0;out-range = 10.0;update-mode = 0;update-signal = 0;update-edge = 0;chan-in-0 = 0;chan-in-1 = 1;c h a n - i n = 2;input-mode = 0;input-range = 10;polarity = 0;gain-0 = 1;gain-1 = 1;gain-2 = 1;

    /* set the coefficients of sensors*/mtr-pos-fac = 94.510.128; /* mm/volt */mtr-speed-fac = 417.7632; /* mmlvolt-sec *Ipend-pos-fac = PI/0.520; I* radlvolt *//*set the center posit ions*/center-pos = 0.280; /* voltage when x-1 = 0 */center-pend = 0.396; /* voltage when x-3 = 0 *//*set the voltages against the frict ions*/v-frict = 0.03;x-frict = 0.02;/*set the t ime interval*/dt = 0.0328;

    tstep = 0.0129;t = 0 ;/* open file 0UT.data for saving output data */ofp = fopen("OUT.data","w");/* Initialize board and input - output */

    AO _Setup ( brd, chan-out, out-mode, out-range, update-mode, update-signal,u p d a t e - e d g e ) ;AI-Config( brd, input-mode, input-range, polarity );

    volt-out = 0.0;AO-VScale( brd, chan-out, volt-out, bin-v-out );AO-Write ( brd , chan-out, *bin-v-out );

  • 8/6/2019 Broom Balancer

    115/118

    /*set the feedback contro l gain*/v-gain-1 = - 1.28563e-03;v-gain-2 = -0.775473;v-gain-3 = -2.37054e-03;v - g a i n = -0.105458;

    for( j = 0; j < COUNT; j++ ) I/*Read motor potent iometer vol tage*/AI-Read ( brd, chan-in-0, gain -0, bin-v-in-0 );AI-S cale ( brd, gain-0 , *bin-v-in-0, volt-in-0 );x-1 = (float)*volt-in-0;x-1 = (center-pos-x-l)*mtr-pos-facll2.7;x-1 = x-1 - 9.764431e-02;/*Read pendu lum po ten t iometer vo l tage* /AI-Read ( brd , chan-in-2, gain -2, bin-v-in-2 );AI-Scale( brd, gain-2, *bin-v-in-2, volt-in-2 );x-2 = (float)*volt-in-2;x-2 = (center-pend-x-2)*pend_pos_fac;x-2 = x-2 - 3.2473 18e-02;/*Read motor tachometer vol tage*/

    AI-Read ( brd , chan-in-1, gain -1, bin-v-in-1 );AI-S cale ( brd , gain -1, *bin-v-in-1, volt-in-1 );x-3 = (float)*volt-in-1 ;x-3 = x-3*mtr-speed-facll2.7;x-3 = x-3 + 1.60618856549263e-01;/*Calcu la te pendu lum angu la r ve loc i ty* /x-4 = (x-2 - ~-2-old-l)/(2*dt);x-2-old-1 = x-2-old;x-2-old = x-2;/*Ou tpu t vo l tage* /volt-out = -v-gain- 1*x- 1 - v-gain-2*x-2 - v-gain-3 *x-3 - v-gain-4*x-4;/*decide the sign of fr iction*/if( volt-out c -0.0001)sgn-x = -1 O;else if( volt-out > 0.0001 )sgn-x = 1.0;elsesgn-x = 0.0;

    if( x-1 < -1 )sgn-x-1 = -1.0;else if ( x-1 > 1 )sgn-x-1 = 1 O;

  • 8/6/2019 Broom Balancer

    116/118

    elsesgn-x-1 = 0.0;volt-out = volt-out + v~frict*sgn~x+x~frict*sgn~x~l+x~2*0.18;

    /*set the bounds of the control voltage*/if ( volt-out > 0.17)volt-out = 0.17;else if(vo1t-out c -0.17)volt-out = -0.17;elsevolt-out = volt-out;

    /* Output the voltage command*/AO-V Scale( b rd, chan-out, volt-out, bin-v-out );AO-Write ( brd , chan-out, *bin-v-out );

    /*save the value of the states and control voltages*/save[O]U] = x-1;save[l]Q] = x-2;save[2]lj] = x-3;save[3]lj] = x-4;save[4]lj] = volt-out;

    volt-out = 0.0;AO-V Scale( brd , chan-out, volt-out, bin-v-out );AO-Write ( brd , chan-out, *bin-v-out );

    /*print the time, state values and control voltages to a file