Experiment 9: Inverted Pendulum using Torsion Control System

Size: px

Start display at page:

Download "Experiment 9: Inverted Pendulum using Torsion Control System"

Lorraine Georgia Fisher
5 years ago
Views:

1 DEPARTMENT OF ELECTRICAL ENGINEERING UNIVERSITY OF MINNESOTA EE 4237 State Space Control Laboratory Experiment 9: Inverted Pendulum using Torsion Control System Objective: 1. To study the LQR design and control implementation of Inverted Pendulum system. References: 1. ECP systems manual (Inverted Pendulum Accessory A51 for Model 205) Apparatus: 1. Inverted Pendulum Accessory A51 2. Torsion control model 205a 3. PC 4. Control Box Prelab Report: 1. What are the design steps in any LQR design? 2. Go through the given program and try to explain what is the functionality of the program. Postlab Report: Answer the questions asked within or at the end of the procedure.

2 5.1 Model 205 Experiments Numerical Plant Models 5. Experiments 35 In the experiments that follow, the pendulum is set to the following parameters: y r = 42 cm, y m = 32 cm (5.1-1) This is the same configuration used in the Self-guided demonstration. The expressions given in Chapter 4 may be used directly for control modeling except that they must be scaled by the appropriate system gains. The form of the transfer functions is N 1 D N 2 D Control Model Control Model ' = k sys ' = k sys k encoder1 k encoder 4 N 1 D N 1 D Ch. 4 Ch. 4 (5.1-2) (5.1-3) where k' sys is the system gain as defined in the Model 205 manual, divided by the Model 205 encoder gain, and k encoder1 and k encoder4 are the gains for encoders 1 and 4 respectively. The gains used in the numerical models that follow are given in Table For the state space realizations, the scaling is accomplished by substituting for control effort ' = u = u controller counts k sys (5.1-4) and for the angular position coordinates θ i = θ iencoder counts / k encoder n, i=1,2; n=1,4 (5.1-5) (Here encoder #4 is associated with θ 2 since encoders 2 and 3 are already assigned to other mechanism locations.) Table Gains For Numerical Models Using the Model 205 Base Unit Parameter k' sys k encder1 k encder4 Value (N-m/count) 2546 (counts/radian) 2608 (counts/radian)

3 36 Note that the gain k' sys will vary somewhat from system to system due to differences in amplifier gains and motor torque constants. For a more accurate model for a particular system the user should use the identified gain k hw for that system according to the Model 205 manual. The subject gain is then found by k' sys = k hw / 2546 (5.1-6) (i.e. for the example system presented in this manual, k hw = 17.2 N-m/rad) In the expressions that follow, the angular coordinates are in units of encoder counts and the torque is in units of controller counts Inverted Configuration a) Transfer Functions θ 1 = 966.0s 2 30,890 s s s s (5.1-7) θ 2 = 806.3s s s s (5.1-8) b) State Space Realization A = , B = (5.1-9) where the state vector is as defined in Section 4.6 and the output matrix C may be chosen as per the analysis or implementation need NonInverted Configuration a) Transfer Functions θ 1 = 966.0s ,890 s s s s (5.1-10)

4 θ 2 = 806.3s s s s (5.1-11) b) State Space Realization A = , B = (5.1-12) where the state vector is as defined in Section 4.6 and the output matrix C may be chosen as per the analysis or implementation need. Note from the transfer function denominators that the inverted plant is unstable and the noninverted one is stable Self-erecting Linear Quadratic Regulator Design In this experiment a linear quadratic controller is designed that minimizes the cost function 1 J = x' Qx +u 2 rdt (5.1-13) We choose Q=C'C and C as the output θ 1 exclusively, i.e. C = (5.1-14) so that the solution minimizes the error of the base position (the base position is regulated about the reference input) subject to the control effort weighting r. The control law has the form u = -Kx (5.1-15) Students may use Matlab to solve for the controller gain vector for various specified r. Matlab program will provide the closed loop poles and system step response for each case. We choose values of r equal to 1 See for example Kwakernaak and Sivan, "Linear Optimal Control Systems", Wiley & Sons, 1972.

5 38 {1000, 100, 10, 1, 0.1}, and choose the case with the highest closed loop bandwidth subject to the highest frequency system poles being less then or equal to 3 Hz 2. Use one of the above obtained K values if it meets this criteria with appropriate r value. And enter the K value into the algorithm provided. An algorithm that implements this controller along with the self-inverting functionality is provided in InvPend205.alg and listed below. ;***********define user variables ************** #define kp_se q1 #define kd_se q2 #define kd_se_d q3 #define k1 q4 #define k2 q5 #define k3 q6 #define k4 q7 #define k2d q8 #define k4d q9 #define kpf q14 #define past_pos1 q15 #define past_pos4 q16 #define uval q17 #define Ts q18 #define gain q19 #define flag q20 #define se_cmd_pos q21 #define enc4_delta q22 #define sign q23 #define enc4_offset q24 #define enc4_inv q25 #define se_step_ampl q26 ;************Initialize variables**************** ;Set Sample period in "Setup Control Algorithm" dialog box same as below Ts= kp_se=2; Proportional gain for noninverted control kd_se=.12; Derivative gain for noninverted control kd_se_d=kd_se/ts; Derivative gain for given sample period flag=0 se_step_ampl=280; Step size for incrementally building noninverted swing amplitude se_cmd_pos=0 ;Gains k1-k4 are state feedback gains - DECOMMENT FOR THE DESIRED DESIGN/PLANT ;The following are LQR gains for the inverted plant k1=-.316 k2=-.107 k3=-1.01 k4= ;Derivitive Gains for given sample period k2d=k2/ts k4d=k4/ts kpf=k1; Input scaling gain gain=1 2 While it is possible to obtain higher closed loop bandwidth through use of lower control effort weight, 3 Hz is a reasonable practical limit. Higher bandwidths can lead to noise propagation (driven by numerical differentiation of the discrete encoder signals) and instability associated with unmodeled phase lags.

6 39 q10=0 ;********* real time code which is run every servo period *** begin enc4_delta=enc4_pos-past_pos4 if (abs(enc4_pos)/32<7600); Maintain collocated self-erecting control and add energy at verticval position after if (flag=0 and abs(enc4_pos)/32>300); Check for initial displacement and apply first self-erecting step input if disturbed flag=1 sign=enc4_delta/sqrt(enc4_delta*enc4_delta) se_cmd_pos=sign*se_step_ampl*32 if (flag=1 and abs(enc4_pos)/32<300); Apply properly signed self erecting step inputs near the vertical position sign=enc4_delta/abs(enc4_delta);sign of velocity se_cmd_pos=sign*se_step_ampl*32 control_effort=kp_se*(se_cmd_pos-enc1_pos)-kd_se_d*(enc1_pospast_pos1); if (abs(enc4_pos)/32 > 7400 and flag=1); pendulum near inverted position, identify encoder 4 offset correction sign=enc4_pos/abs(enc4_pos);sign of Encoder 4 enc4_offset=sign*8192*32 flag=2 control_effort=0 if (flag=2); pendulum near inverted position, begin inverted control enc4_inv=enc4_pos-enc4_offset; Encoder 4 position relative to vertical q11=enc4_inv q12=enc4_offset uval=kpf*cmd_pos-k1*enc1_pos-k3*enc4_inv-k2d*(enc1_pos-past_pos1)- k4d*(enc4_pos-past_pos4); Control Law control_effort=uval*gain past_pos1=enc1_pos past_pos4=enc4_pos q10=enc4_inv/32; This variable may be plotted to show theta 2 relative to upright vertical end As with all ECP Executive USR programs, the flow consists of three major sections: variable declaration, initialization, and real-time routine. (Refer to the Executive USR manual for details). Note that the encoder signals are internally multiplied by 32 and the resulting control effort is divided by 32 for improved internal computational resolution. This internal scaling is generally transparent to the user but sometimes becomes relevant as in the arguments in the relational statements in the above code Self-inverting Function

7 40 It is possible to erect the pendulum from the non-inverted to the inverted configuration by various means. Using such a self-inverting routine makes for a stimulating demonstration of the effectiveness of closed loop control and provides a practical mechanism for precisely initializing the pendulum angle. With the pendulum in non-inverted static equilibrium, the pendulum angle, θ 2, is precisely 180 o from the unstable equilibrium (vertical) position that is the nominal inverted operating point. While the noninverted control may be implemented by initially holding (manually) the pendulum in the approximate vertical position, any errors will lead to steady state base position errors under closed loop control. It is therefore recommended that for inverted pendulum control, the system be initialized in the static vertical downward position and a self-erecting algorithm be used for transition to inverted operation. 3. In the present algorithm, referring to the real-time routine section, (the portion that lies between begin and end), the base position is controlled through simple collocated PD control as long as the pendulum is not near the inverted position. The controller is initially in a quiescent state until the pendulum angle is disturbed > 300 encoder counts. It then applies a step input to the base disk that is of the correct sign to increase the pendulum swing amplitude. In each successive crossing of the downward vertical ;position, an additional step input is applied in the correct direction to pump energy into the pendulum and increase the swing amplitude. When the amplitude increases such that the pendulum approaches the upward vertical position, the algorithm does a coordinate shift to make the origin of θ 2 to be the upward vertical position, the inverted controller is activated, and the non-inverted controller is disabled. The following are noteworthy : 1. It is important to let the pendulum come to rest completely in the noninverted (vertical downward) position prior to implementing the controller. This will assure the system initializes with no offset in the encoder 4 signal. 2. If it is desired to shorten the process to initialize and operate in inverted mode, the user may lift the rod to within 10 degrees (not closer) of vertical and then release. This is done after implementing the controller and as always, the controller should be safety-checked as per Section before touching the apparatus under closed loop control. Only a few cycles of pendulum swing will be required to complete the self-inverting operation. 3. The parameters of this algorithm may be adjusted to change its behavior. For example a larger step amplitude may be used to reduce the number of swings. However such a change can result in excessive velocity of the rod when crossing the upward vertical position and cause the closed loop "capture" to fail. The parameters selected above provide fairly reliable (but not 100%) capturing of the rod in the inverted position. 3 Alternatively, any algorithm that initializes with the pendulum in the downward vertical position and performs a 180 degree coordinate shift for inverted control may be employed.

8 LQR Controller The LQR controller is easily discernable in the above algorithm. Note that the gains are all negative. For those associated with θ 2, this is due to the arbitrary assignment of the sign of that angle, but for θ 1, the gains would be negative regardless of sign convention. Thus, positive feedback is employed to stabilize the system. The LQR synthesis solves for a stabilizing controller that minimizes the error between the base disk position, θ 1, and the reference input. The so-called prefilter constant gain is set equal to the base position gain so that the control effort is zero when the base position equals the reference input and the other states are zero. As will be seen in the experimental results and is easily shown analytically, the controller adds a nonminimum phase zero to the system Control Implementation and Characterization Implement your LQR algorithm. Students are required to pot the step, ramp tracking, and sine sweep responses. The trajectory parameters used are as follows: Step: step size = 1000, dwell time =4000 ms and number of repetitions = 1; Ramp: distance = 2000 counts, velocity = 1000 counts/sec, dwell time = 3000 ms, and number of reps =2; Sine Sweep: amplitude = 200 counts, start frequency = 0.1 Hz,, end frequency = 10 Hz, sweep time = 30 sec., and logarithmic sweeps should be selected. (The sine sweep plot shows the Encoder 1 data with Logarithmic Frequency on the horizontal axis, db scaling on the vertical axis, and Remove DC bias checked). Save your plots. Questions / Exercises: A. Report your calculated values of the closed loop poles for the various values of r and for your final design. Report the values of K, for your final design.

Experiment 2: Control of Nonlinear Compensated SISO Systems

DEPARTMENT OF ELECTRICAL ENGINEERING UNIVERSITY OF MINNESOTA EE 4237 State Space Control Laboratory Experiment 2: Control of Nonlinear Compensated SISO Systems Prelab: 1. Study Section 6.3 of manual (attached)