Post on 07-Apr-2018
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