SELF-ERECTING, ROTARY MOTION INVERTED PENDULUM Quanser Consulting Inc.

Transcription

1 SELF-ERECTING, ROTARY MOTION INVERTED PENDULUM Quanser Consulting Inc. 1.0 SYSTEM DESCRIPTION The sel-erecting rotary motion inverte penulum consists of a rotary servo motor system (SRV-02) which rives an inepenent output gear which also meshes with a potentiometer. The rotary penulum arm is mounte to output gear. At the en of the penulum arm is a hinge instrumente with a quarature encoer. The penulum attaches to the hinge. Figure 1.2: Schematic of self erecting experiment The potentiometer measures the rotation of the arm while the encoer measures the rotation of the penulum. The potentiometer oes not have physical stops but the measurement oes have a iscontinuity. The maximum range that can be measure with the potentiometer is 352 egrees. The Figure 1.1 : SRV-02 shown in low gear ratio configuration an the gears require for high gear ratio configuration penulum angle on the other han is measure via a quarature encoer an thus the number of turns that can be measure are virtually unlimite. The purpose of the experiment is to esign a controller that starts with the penulum in the own position, swings it up an maintains it upright. 1.1 ASSEMBLY This experiment is performe with the SRV-02 in the high gear ratio configuration shown in Figure 1.3. Use the supplie gears to achieve the configuration shown. Attach the penulum arm to the central gear of the SRV- 02 using the two brass thumb screws Calibration Figure 1.3: High gear ratio configuration Loosen the set-screw on the potentiometer gear of the SRV-02 (See figure 1.3). Attach a 6 Pin Mini-Din Cable from the SRV-02 to the Quick connect box labelle S1&S2 ( See Figure 7.1). Power up the power moule an measure the voltage at the bining post labelle S1 with respect to the groun terminal. This is the voltage of the potentiometer. Rotate the penulum arm such that it points to the zero measurement of the SRV Quanser Consulting Inc. RP_SE 1

2 Figure 1.4: Calibration position Using a small screwriver, rotate the potentiometer shaft while the arm remains pointing to the zero position an observe the voltage at S1. Ajust the position of potentiometer shaft such that when the arm is at zero the voltage measure is aroun zero volts (±0.1 V). NOTE that this can happen at two positions. One of them is wrong: You can measure zero volts either when the wiper of the potentiometer is in the mile of the range, which is correct, or when the wiper is not in contact with anything! To make sure you have the correct zero volts, turn slilghtly left an right an ensure that the measurement is continuous. It shoul not jump to +5 volts or -5 volts suenly. At the zero volts tighten the set screw. You have now calibrate the system Safe mounting Mount the system on a table such that the penulum swings in the front an ensure that the penulum will not collie with any objects while it swings. Clamp the SRV-02 to the table using a C clamp that can be obtaine from any harware store. 2.0 MATHEMATICAL MODELLING 2.1 NONLINEAR MODEL Consier the simplifie moel in Figure 2.1. Note that l p is half the actual length of the penulum (l p= 0.5 L p). Figure 2.1: Simplifie moel The nonlinear ifferential equations are erive to be: where (m p r 2 ) m p r l p cos() m p r 2 l p sin(), m p l p cos() r m p l p sin() rm p l 2 p m p g l p sin()0 T : input torque from motor ( Nm ) mp: mass of ro (Kg) lp: centre of gravity of ro (m) (half of full length) 2 : Inertia of Arm an gears(kgm ) : Deflection of arm from zero position(ra) : Deflection of penulum from vertical UP position(ra) 1996 Quanser Consulting Inc. RP_SE 2

3 2.2 LINEAR MODEL The linear equations resulting from the above are: u m p rg 0 0 u r F (2.2) u 0 g (m p r2 ) l p 0 0 u l p Note that the zero position for all the above equations is efine as the penulum being vertical up. A positive angle in the penulum is efine for a fall to the right when looking at the penulum from the motor shaft. A positive rotation of the arm is efine as clockwise when looking at the SRV-02 from above. (See Figure 2.2) Figure 2.2 Definitions of u an. (VIEW IS FROM MOTOR SHAFT LOOKING OUT TOWARDS THE PENDULUM) For the DOWN position, the following equation hols: m p rg F (2.3) 0 g (m p r 2 ) l p 0 0 r l p where =0 is efine for the penulum suspene. Its motion is relative to the own axis an is positive for clockwise rotations observe from the motor shaft. 3.0 CONTROL SYSTEM DESIGN The controller will consist of two main parts. One will be the swing up controller while the secon is the balance controller. The swing up controller will oscillate the arm until it has built up enough energy in the penulum that it is almost upright at which point the balance controller is turne on an is use to maintain the penulum vertical Quanser Consulting Inc. RP_SE 3

4 3.1 Swing up control Swing up control will essentially control the position of the arm in orer to estabilize the own position. Assume the arm position can be commane via. Then the feeback: P D can be mae to estabilize the system with the proper choice of the gains P & D. This means we want to comman the arm base on the position an rate of the penulum. This approach is intuitive: it simply makes sense that by moving the arm back an forth one can eventually bring up the penulum. For the servo arm to track the esire position, we esign a PD controller: V K p ( ) K This is a position control loop that controls the voltage applie to the motor so that tracks. Designing a swing-up controller in this fashion ensures that the arm is not commane to move beyon the potentiometer limits. By limiting, we ensure that the arm oes not reach positions that cause a collision of the penulum with the table upon which the SRV-02 rests. The motor equations are: V I m R m K m K g where V (volts) : Voltage applie to motor I m (Amp): Current in motor -1) K m(v/ (ra sec ): Back EMF Constant K g: Gear ratio in motor gearbox an extrenal gears (Ra): Arm angular position The torque generate by the motor is:, K m K g I m V 1 s ( R m sk m K g ) K m K g Substituting system parameters we obtain: 25 s 47.2 V 1996 Quanser Consulting Inc. RP_SE 4

5 an closing the loop we have the arm position control transfer function: 47K p s 2 ( 2547K p ) s 47K p which has a characteristic polynomial of the type: s o s7 2 o an t p % 7 o (1 2 ) where is the amping ratio an t is the peak time. p Now we nee to select the amping ratio an peak time to obtain Kp & K. Consier the penulum in the system. Its parameters are: lp(m) : m(kg): p 0.20 m (centre of gravity is locate at half of full length) 0.15 Kg The natural perio for small oscillations is given by: T 2 % l p g We want the arm to react to these movements. Therefore the close loop response of the arm shoul be consierably faster that the natural frequency of the penulum. The natural perio of the penulum is 0.97 secons. The natrural frequency is w p = 2 %/T = 6.45 ra/sec. It woul then be reasonable to esign a close loop controller for the arm position which has the following specifications: w o = 6 w p = ra/sec zeta = The factor of 6 is rather arbitrary an coul have been 5 or 7. Using this approach (see servpos.m), we obtain the gains: K p.55v/( K 0.01 V /(( sec 1) A SIMULINK block iagram that simulates (s_own.m) the swing up controller is shown in Figure 3.1. The open loop system consists of the state space system given in equation 2.3. the outputs are converte to egrees within the block. The input to the system is a voltage which shoul be saturate to +/- 5 Volts in orer to obtain a realistic simulation. To run the simulation you must run the m file own.m first which loas the moel parameters into the MATLAB workspace Quanser Consulting Inc. RP_SE 5

6 The results of the simulation are shown in Figure 3.2. The simulation inicates that this metho of swing up control can be use to bring up the penulum. This is not exact analysis however since the simulation is base on a linear moel an once the angle excees a few egrees, the moel is not vali. Conceptually however, the simulation shows that it can be one. Note that when the penulum amplitue gets larger, it starts interfering with the motion of the arm to the extent that the arm oes not track the esire position at all. This effect is ealt with in the implementation section by changing the limit on ynamically (see section 4.3) Quanser Consulting Inc. RP_SE 6

7 Figure 3.1: SIMULINK simulation of swing up controller (s_own.m) Figure 3.2: Simulation results showing arm comman an actual arm angle. Note how the actual oes not track the esire when the penulm angle is large. Figure 3.3: Simulation results showing that the penulum can be grought up 1996 Quanser Consulting Inc. RP_SE 7

8 3.2 Balance control Assuming the penulum is almost upright, a state feeback controller can be implemente that woul maintain it upright (an hanle isturbances up to a certain point ). The state feeback controller is esigne using the linear quaratic regulator an the linear moel of the system. The linear moel that was evelope is base on a torque T applie to the arm. The actual system however is voltage riven. From the motor equations erive above we obtain: T V K m K g R K 2 m K 2 g R substituting this into the matrix equation an inserting the parameter values results in u u V u u 47.4 which is the esire representation. Defining the LQR weighting matrices: Q iag ([.25401]) r.05 results in the feeback gains: K [ ] ( Note that gains have been converte to V/eg an V / (eg/sec). (See rotpen.m ) 3.3 Moe Control The MODE controller etermines when to switch between the two controllers. This requires some intuition (an a few tricks!). Experimentally, we note that if you maintain the estabilizing motion to a fixe saturation amplitue (as in the simulation), there will come a time when the arm is moving too much an it actually starts reucing the amplitue of oscillations (beating) Quanser Consulting Inc. RP_SE 8

9 So, to start from rest, you apply a few large movements until the amplitue of oscillation is large enough an then you switch to a smaller comman amplitue that will slowly bring the penulum higher. Next, once you have attaine an acceptable amplitue of penulum oscillations, you want to start the stabilizing controller at the right time. Given that the stabilizing controller is linear, it has a small region in which it can stabilize the moving penulum. Experimentally, we etermine that it can stabilize the penulum when it is about 15 egrees (k) from the vertical an not moving faster than 200 eg/sec. So, the swingup controller shoul ensure that when the penulum is almost upright, it is not moving too quickly. This is controlle by ajusting the gain P & D in the swing up controller. Once the penulum is up, it is easy to keep it up. But what if someone isturbs it? What shoul the system o? With the above scheme, if the isturbance causes the penulum to swing beyon the 15 egrees then the moe switches to 0 an the swing up controller kicks in... but this is not very goo because the isturbance may be too large an the arm woul move too far off from the zero position. It woul be better to switch the moe to 0 when the angle is greater than maybe one or two egrees. Once then penulum stabilizes, we shoul make the criterion a little more strict. This is one by changing the value of k ynamically. To start, we make k = 15 egrees but once the system has stabilize we change k to 1 egree. We shoul also ensure the value of MODE oes not bounce. It is a switch after all an switches nee e-bouncing. This is one by rate limiting the output of the AND gate an feeing it through a eaban. The parameters of the rate limiters an the eaban are obtaine experimentally. 4.0 CONTROL SYSTEM IMPLEMENTATION The system is implemente in SIMULINK. The controller is then run in WinCon which runs the SIMULINK iagram in realtime uner Winows. Figure 4.1 shows the SIMULINK controller q_rp_se.m supplie with the system. You can run this controller irectly from WinCon by simply loaing q_rp_se.wcl ( or.prj) into WinCon. If you want to perform changes to the parameters you nee MATLAB, SIMULINK an Realtime Workshop. If you want to esign other controller you also nee Watcom C++. The subsystems of the controller are now escribe in etail: 4.1 Calibration an ifferentiation This block simply converts the value measure from the boar to the appropriate units. The arm angle is measure in volts with a calibration constant of 35 Deg/volt. while the encoer measures egrees per count. The values are also fe through high pass filters which essentially ifferentiate the signals. Note also that the SRV Quanser Consulting Inc. RP_SE 9

10 potentiometer voltage is passe through a bias removal block which measures the initial voltage for a short uration an then hols it in memory. This initial voltage is subsequently subtracte from all following samples. 4.2 Penulum Up/Down measurements This block converts the measurement of the penulum angle from the Full position to two other frames of reference ( see figure 2.2) (Full) : The full position measures values which start at zero (hanging own) f an increases in the positive irection when the penulum rotates clockwise an ecreases when the penulum rotates counterclockwise. The value of the angle measure can excee 360 egrees many times over in both irections. This value is obtaine irectly from the encoer counter (UP) : The UP position is relative to the vertical axis in the up position. It gives u a zero value when the penulum is straight up an a positive value when the penulum is tilting to the right of the vertical an a negative value when it is to the left of the vertical. It is obtaine by performing the following operation: u sin 1 (sin( f )) -1 ( note sin returns values between ±%/2 ) (DOWN) : The DOWN position is essentially the same as the FULL position expect that it is limite to values between ±180 egrees (NOT SATURATED: Wrappe aroun!). This is the value use in the swing-up controller since the feeback is relative to the own position. If we fee back the FULL value an the penulum oes a full swing then swing up control will be ineffective! The value is obtaine using the following equation: tan2 1 (sin( f ), cos( f )) -1 ( note tan2 returns values between ±% ) Up / Down an Full measurements Note that all trigonometric operations are performe on the angles converte to raians. The results are then converte back to egrees. 4.3 Swing up control 1996 Quanser Consulting Inc. RP_SE 10

11 This is the positive feeback loop use to swing the penulum up. It consists of two loops. The inner loop performs position control of the arm : V K p ( ) K The outer loop of the system generates the comman via the following equation: P D with P = 0.5 eg/eg an D = 0.0 eg / (eg/sec). These two values are crucial in bringing up the penulum smoothly. You can tune the value of D to ajust the amping in the system. Note also that is limite to ±8 Degrees. For the first 3 (T high) secons of control however, is multiplie by (1+Kh) resulting in a high gain system for a short time (Kh = 1.5). This effectively spees up the initial response to get maximum swing in the shortest possible time. Ieally, you want to move the arm back an forth a minimum number of times an bring the penulum up quickly. If you o not have a high gain perio, the system will still come up but it may take up to 30 oscillations of the arm to bring the penulum up. Note that you may have to tap the penulum slightly to start the feeback loop. 4.4 Moe control This block etermines which of the two voltages shoul be fe to the motor. Initially, at startup, we want to fee the voltage compute by the swing up controller. When all of the conitions liste below are met, we want to switch to the stabilizing controller output voltage. Stabilizing conitions: u < k (ie angle small enough) < k (ie arm position small enough) cos( f )<0(ie penulum is up) f < k (ie penulum moving slowly enough) When all of the above conitions are true, the output of the AND gate is 1. We limit the rate of change of the output an pass it through a backlash block in orer to ebounce it. This output is the signal MODE. When MODE is 0, the voltage fe to the motor is obtaine from the swing-up controller, When MODE is 1, the voltage to the motor is obtaine from the stabilizing controller. Furthermore, MODE is fe to a elay block ( 0.2 / ( s+0.2) ) an its output is fe to a comparator. When the output of the comparator is 1, the value of k is reuce by 14 egrees to 1 egree. This now changes the stabilizing conitions to the following let 1996 Quanser Consulting Inc. RP_SE 11

12 go conitions. When any of the following conitions is NOT TRUE, the system switches MODE to 0 an goes back to swing up moe. Let Go Conitions: u < k (ie angle isturbe too much ) < k (ie arm position too far) cos( f )<0(ie penulum is not up) f < k (ie penulum moving too quickly) 4.5 Balance control This is the stabilizing state feeback controller. It simply fees back the voltage: V s (k 1 k 2 k 3 k 4 ) as obtaine in the LQR esign. This voltage maintains the penulum upright 5.0 RESULTS Typical results are shown in Figure 5.1,5.2 an 5.3. These are all obtaine from the same run. Always start the system with the cart in the mile an the penulum in the DOWN position. The controller begins by resetting the encoer counter to zero an measuring the bias in the potentiometer for subsequent subtratction. Thus the initial position is efine as zero. The controller starts by applying a e-stabilizing arm position comman limite to ±8*(1+Kh) Deg. as shown in Figure 5.1 This results in the penulum swinging with increasing amplitue. At T = 3 secons ( as set in Figure 4.4 ) the arm comman is limite to ±8 Deg. The estabilizing controller continues however to increase the amplitue of the penulum oscillations. At some point, all the following conitions are met: - Penulum UP angle < 15 egrees - Penulum spee < 200 eg/sec - Arm position < 25 eg - Penulum Full angle in 2n or 3r quarant This sets MODE = 1 an the state feeback controller output V s is fe to the motor rather than the estabilizing voltage V. This stabilizes the penulum an keeps it upright Quanser Consulting Inc. RP_SE 12

13 The Moe is maintaine to the value 1 unless one of the following conitions becomes false: - Penulum UP angle less than 2 egrees - Arm position < 25 eg - Penulum velocity < 200 eg/sec - Penulum Full angle is in 2n or 3r quarant as efine in Figure 2.2 If you apply a tap to the penulum, it will remain upright unless the tap was so har that one of the above conitions is not maintaine. If MODE switches to zero, then the penulum falls an the cart will start oscillating again until the penulum comes up an MODE switches to 1. Note however that there may be situations when the tap was so har that the penulum swinging in full rotations an never stabilizes. It is of course possible to esign a controller that avois this situation. One way woul be to implement a controller that etects full rotations an turns off the system momentarily until the penulum is simply swinging in the own moe an then resume to the swing up controller in low gain. A ifferent metho of applying a isturbance is to push against the arm until the controller gives up an lets the penulum fall Quanser Consulting Inc. RP_SE 13

14 6.0 SYSTEM PARAMETERS Parameter Symbol Value Units Motor Torque constant Km Nm/Amp (an back emf constant in SI units) V/ ra sec -1 Motor Armature Resistance Rm Internal gearbox ratio N/A 14.1 N/A External gearbox ratio(high gear ratio) N/A 5 N/A Total gear ratio Kg 60.5 N/A Motor pinion # teeth NA 24 teeth Total inertia after gearbox incluing arm J_b Kg m 2 Penulum true length Lp 0.43 m Penulum mass mp 0.14 Kg Servo position potentiometer sensitivity N/A 35 Deg/Volt Penulum angle encoer resolution N/A 1024 * 4 count/rev Penulum angle calibration constant N/A eg/count 1996 Quanser Consulting Inc. RP_SE 14

15 7.0 WIRING Wire the system as shown in Figure 7.1. The following cables are supplie with the system: Wire type From To 6 Pi Mini Din - 6 Pin Mini SRV-02 6 Pin Mini Din Quick Connect S1&S2 Din socket (either one) socket 2 Pin Din - 2 Banana Power moule Motor on cart RED to OUT BLACK to GND FORK to (-) 5 Pin Din - 5 Pin Din Penulum angle encoer Enc #0 on MultiQ terminal boar 5 Pin Din - 4 x RCA Quick Connect To A/D Potentiometer S1 to MultiQ A/D #0 Not Use S2 to MultiQ A/D #1 S3 to MultiQ A/D #2 S4 to MultiQ A/D #3 2 Pin Din - RCA Quick Connect From D/A MultiQ D/A #0 (Analog output) 1996 Quanser Consulting Inc. RP_SE 15

16 8.0 SOFTWARE SUPPLIED WITH YOUR SYSTEM File name Function Require SW rotpen.map Derive ifferential equations MAPLE rotp_m.m Linear moel for own position MATLAB rotpen_m.m Linear moel for up position MATLAB own.m Linear moel parameters for own MATLAB system servpos.m Design bring up Controller (Kp &K) MATLAB rotpen.m Design state feeback controller MATLAB s_own.m Simulate bring up controller SIMULINK etc.. q_rp_se.m SIMULINK controller SIMULINK etc.. q_rp_se.wcl Precompile WinCon Realtime controller WinCon etc.. from SIMULINK controller Questions? Contact us at 1996 Quanser Consulting Inc. RP_SE 16

17 Figure 5.1, 5.2 & 5.3: Actual results obtaine from the system. (Note MODE is multiplie by 100 for visibility)