MEM04: Rotary Inverted Pendulum

Transcription

1 MEM4: Rotary Inverted Pendulum Interdisciplinary Automatic Controls Laboratory - ME/ECE/CHE 389 April 8, 7 Contents Overview. Configure ELVIS and DC Motor Goals Rotary-Pendulum Introduction Equations of Motion 3 3 Linearization of Rotary Pendulum Dynamics 4 3. Inclusion of actuator dynamics System Identification (Experimental Determination of Model Parameters) 5 4. Calculation of Pendulum Center of Mass Analytical Calculation of Moment of inertia J p Experimental Identification of Pendulum Moment of Inertia Gantry Control 8 6 Inverted Pendulum Control 9 6. Inverted Pendulum Control via LQR A Derivation of System Dynamics by Lagrange method B Linearized Dynamics 3 C Files 4 Overview This lab addresses the control of a pendulum swinging from a rotating beam. Their are two control problems to consider: the gantry (crane) problem where we would like move the rotary section about while minimizing the swing of the pendulum and the inverted pendulum problem where we would like to balance the pendulum in the inverted position. First, the model structure for the system is derived from first principles and simple experiments are carried out to determine the model parameter values. Then, the laboratory proceeds with control design for each case separately.

2 We control the system using full state feedback, and explore both pole placement and LQR as techniques for choosing the feedback gains. The necessary files to carry out the laboratory are available in the controls drive (T drive). The drive should be visible from the windows Computer pane T:\ME389 MEM4 GANTRY\ You can read files in this folder but cannot edit them. Copy the folder to the local C drive. The folder contains LabView files for controlling the hardware and Matlab files for analyzing the data. See Appendix C for a complete list of files.. Configure ELVIS and DC Motor ELVIS (Educational Laboratory Virtual Instrumentation Suite) To startup the ELVIS follow the procedure. The main VI for this lab is QNET ROTPEN Lab 3 Gantry Control ME389.vi Found in \ME389 VIs. There is a reference printed on the wall for starting the ELVIS (Education LabView Instrumentation Suite). 3. First confirm the prototyping board power is switched OFF, down position. Also switch the Communications to BYPASS. 4. Turn on the main power to the ELVIS with the switch located on the back of the ELVIS. The system power LED should light up. 5. Run QNET init elvis bypass.vi. The bypass is successful when the LED in the VI is lit. 6. Turn on the prototyping board power. The four LEDs +B, +5, -5, +5 should light up. If any are not fully lit, consult the TA. 7. Warning: If the motor spins when switching ON the prototyping board power, immediately switch OFF the Prototyping board power.. Goals. Model the rotary pendulum system as a state space system.. Use experimental techniques to determine the model parameters. 3. Control the pendulum in the Gantry position. 4. Balance control of the pendulum in the upright (inverted) position..3 Rotary-Pendulum Introduction The rotary-pendulum system consists of an actuated rotary arm controlled by an input torque, τ, and an unactuated pendulum connected to the arm at a pivot joint.

3 Figure : Rotary-pendulum system. Equations of Motion The rotary pendulum is an example of a manipulator system, i.e. a serial chain of robotic manipulator arms. The equations of motion of all manipulator systems take the characteristic form known as the manipulator equation, H(q) q + C(q, q) q + G(q) = Bu, () where q is a generalized coordinate vector, H is the inertial matrix, C captures Coriolis forces, and G captures potentials (such as gravity). The B matrix maps control inputs u into generalized forces. The matrices of () can be determined by Lagrange s method. However, the complete derivation provided in Appendix A of system dynamics is fairly involved and it s not necessary to understand for this course. For the rotary pendulum, we have q = θ, α] T, ] Jarm + m p r + m p lp sin α m p rl p cos α H(q) =, m p rl p cos α J p + m p lp C(q, q) = m plp α sin α + C arm m plp θ ] sin α m p rl p α sin α m, plp sin α C p ] G(q) =, m p gl p sin α ] B =, () where C arm and C p are damping coefficients that result from viscous friction at the joints. Note that the system matrices H(q), C(q, q) and G(q) are independent of the state θ. 3

4 3 Linearization of Rotary Pendulum Dynamics As explained in Appendix B, we can use taylor series expansion to obtain a linearized form of the manipulator equation. ẋ = Ax + Bu, (3) where x is the full system state (x = q, q] T = θ, α, θ, α] T ), and u = τ is the input torque. For rigid body systems that can be described by the manipulator equation (), the first order taylor series approximation around a fixed point (x, u ) is given by, A = ] I H(q), and B = C(q, q) x,u H(q) G(q) H B] x,u. (4) Computing H(q) = ] mp lp + J p l p m p r cos α l p m p r cos α m p lp sin α + m p r, (5) + J arm where = J p J arm + m pl 4 p sin α + m pl pr + J arm m p l p + J p m p r + J p l pm p sin α l pm pr cos α, (6) and G(q) = ], (7) gl p m p cos α we can write A = glpm pr cos α m plpr 3 cos α sin α (8) (gl p m p cos α(m p lp sin α + m p r + J arm )) ( m plp sin α(m p lp sin α + m p r + J arm )) B = where we have used θ = α and C arm = C p. 3. Inclusion of actuator dynamics x,u m p lp + J p, (9) l p m p r cos α x,u In the actual physical system the control input is not the torque, but is actually the voltage applied to the motor. The actuator dynamics must be included into the state equations since the computer does not control the motor torque directly but controls the voltage being applied to the motor. The torque generated at the arm pivot from the motor voltage, V m, is given by ) K t (V m K m θ τ =. () R m 4

5 The linear dynamics of the motor can be combined with the state space system (3)-(4) to obtain the complete dynamics ẋ = Ax + Bu, () where x = θ, α, θ, α] T, u = V m is the input motor voltage. This is achieved by the following transformation A A KtKm R m B ] B K t B, () R m which results in A = glp m p r cos α m p l3 p r cos α sin α K tk m (m R m plp + Jp) (gl pm p cos α(m plp sin α + m pr + J arm)) ( mpl p sin α(mpl p sin α + m pr + J arm)) + K tk m (m R m pl pr cos α) (3) x,u B = Kt R m m plp + Jp. (4) l pm pr cos α x,u For the inverted fixed point x =, π,, ] T, A = m pgrlp K tk m(j p+m pl p) J pj arm+j pm pr +J armm plp R m(j pj arm+j pm pr +J armm pl p ), B = m pgl p(j arm+m pr ) J pj arm+j pm pr +J armm pl p For the hanging fixed point x =,,, ] T, K tk m(m prl p) R m(j pj arm+j pm pr +J armm pl p ) A = m pgrlp K tk m(j p+m pl p) J pj arm+j pm pr +J armm plp R m(j pj arm+j pm pr +J armm pl p ), B = m pgl p(j arm+m pr ) J pj arm+j pm pr +J armm pl p K tk m(m prl p) R m(j pj arm+j pm pr +J armm pl p ) K t(j p+m pl p ) R m(j pj arm+j pm pr +J armm pl p ) K t(m prl p) R m(j pj arm+j pm pr +J armm pl p) K t(j p+m pl p ) R m(j pj arm+j pm pr +J armm pl p ) K t(m prl p) R m(j pj arm+j pm pr +J armm pl p).. 4 System Identification (Experimental Determination of Model Parameters) To use the dynamic model described by (), we have to plug in values for all the parameters. Some of these are known constants, such as the gravitational acceleration, g. Others are easy to measure directly, such as the pendulum length. The system identification procedure consists of three parts: Note, the system exhibits a hanging fixed point for any value of θ, thus there are actually a continuum of hanging fixed points, and any value of θ can be used to solve for the linear dynamics. 5

6 (a) Front View Pendulum where l p is the distance to the (b) Moment Inertial Diagram center of mass Figure : Free body diagrams of pendulum. Parameter Description Value Unit M p Mass of pendulum link.8 kg M p Mass of pendulum weight.9 kg L p Length of pendulum link.7 m L p Length of pendulum weight.9 m Table : Parameter values for Figure (b). 4. Calculation of Pendulum Center of Mass For a composite object, made of several bodies, the center of mass is given by x cm = n i= m ix i n i= m, (5) i where x cm is the distance from some reference point to the center of mass of the composite object and x i is the distance to the center of mass for each individual object. 6

7 Lab Work :. Using Table and equation (5) calculate the center of mass of the pendulum relative to the pivot location, l p. 4. Analytical Calculation of Moment of inertia J p The generalized equation for moment of inertia of an arbitrary rigid body about its axis of rotation is J = r dm, (6) M where r is the perpendicular distance between the element mass, dm, and the axis of rotation and the integral is calculated over the object s entire mass, M. Lab Work :. Use (6) to write an expression for the moment of inertia in terms of the parameters M p, M p, L p, and L p (a) Hint: For a uniform rod of length L and mass M, the elemental mass is dm = M L dx. Use Table and the obtained expression to determine the moment of inertia J p. 4.3 Experimental Identification of Pendulum Moment of Inertia Consider the unforced dynamics of the pendulum member alone, J p α + m p gl p sin α =. (7) The linearized dynamics of this system about and angle of α = are given by Recalling that the second order system J p α + m p gl p α =. (8) ÿ + ω ny =, (9) describes an oscillator with natural frequency ω n, we see that the simple pendulum has a natural frequency mgl p ω n =. () J p Measurements of the natural frequency and pendulum center of mass length can be used to calculate the pendulum moment of inertia. 7

8 Lab Work 3: Collect data to determine the natural frequency of the pendulum alone. This can be done by letting the pendulum swing while holding the rotary beam fixed and observing the time it takes for the pendulum to swing through a single period. The natural frequency is equivalent over the elapsed time required to complete a period cycle.. Use the Labview VI -QNET ROTPENT Simple Modeling.vi Found in directory: C:\ME389 VIs\QNET ROTPEN SimpleModeling. Enter amplitude =. V 3. Bring the pendulum to some angle and let it go allowing it to swing freely. What initial angle makes sense for this test? Think about problems that occur in the limit of small to large angles. 4. Determine the natural frequency from f n = n cyc t = number of cycles elapsed time () 5. Note, ω n = πf n. Use this value to calculate J p. 5 Gantry Control In this section, we consider control of the rotary system in the Gantry (crane) position. Imagine the rotary beam as a crane with the pendulum in the down position as a load to be displaced. We wish to move the load as fast as possible but minimize the amount of swinging. The model, determined in the previous sections, is given in state space form, ẋ = Ax + Bu, () y = Cx, (3) where x = θ α θ α] T and u is the input voltage to the motor. Lab Work 4: Gantry Model Analysis. Compute the open-loop poles of the system. Is the system stable? We assume measurements of all the states are available, i.e. y = x and C is the identity matrix. The controller is called a linear state feedback controller, i.e. it is of the form u = K(x x d ), (4) where K is a matrix of control gains. The control gains are K = k p,θ, k p,α, k d,θ, k d,α ] T, where k p,θ is a proportional gain on θ, k p,α is a proportional gain on α, k d,θ is a derivative gain on θ, and k d,α is a derivative gain on α: u = K(x x d ) = k p,θ (θ θ d ) k p,α (α α d ) k d,θ θ kd,α α. (5) 8

9 Lab Work 5: Gantry Proportional Controller Use proportional control to move the gantry to the desired position. Carry out the control design by first simulation and then implementation on the real hardware.. The proportional control can be implemented by setting the gain matrix to K = kp ].. This applies a proportional gain to the state θ relative to the desired angle, θ d, i.e. u = K(x x d ) = k p (θ θ d ). (6) 3. Use the Control implementation tab of the VI and set the first LQR gain to Kp and the others to zero. 4. Note the beam tracks the desired location, but there is lots of oscillation in the pendulum. Lab Work 6: Gantry LQR Controller Since, there is only one control variable, R is a scalar and the control law used to minimize the cost the function J is given by u = K(x x d ) = k p,θ (θ θ d ) k p,α (α α d ) k d,θ θ kd,α α. (7) Note that K is a 4 matrix. Now design and LQR controller to improve the pendulum damping. Use a Q matrix that penalizes non zero pendulum angles and non zero rotary beam locations. Carry out the control design by first simulation and then implementation on the real hardware. 6 Inverted Pendulum Control In this section, we consider control of the rotary system in the Inverted Pendulum position. The model, determined in the previous sections, is given in state space form, ẋ = Ax + Bu, (8) y = Cx, (9) where x = θ α θ α] T and u is the input voltage to the motor. Lab Work 7: Inverted Pendulum Model Analysis. Compute the open-loop poles of the system. Is the system stable? 9

10 Lab Work 8: Inverted Pendulum Pole-Placement Controller Carry out the control design by first simulation and then implementation on the real hardware.. Create a state space system in Matlab for the inverted pendulum case. Plot the open-loop pole locations of the inverted pendulum. 3. Verify that your system is controllable by using the Matlab ctrb command. 4. Choose the closed loop pole locations to be all in the LHP. You can mirror any RHP poles into the LHP. Use Matlab place command to do the pole placement. 5. Simulate the system response to initial conditions with Matlab initial. 6. If the system is stabilized with reasonable input voltages ( V), try it out on the real system. 7. Remember to start the pendulum in the upright position. Lab Work 9: Inverted Pendulum LQR Controller Since, there is only one control variable, R is a scalar and the control law used to minimize the cost the function J is given by u = K(x x d ) = k p,θ (θ θ d ) k p,α (α α d ) k d,θ θ kd,α α. (3) Note that K is a 4 matrix. Now design and LQR controller to improve the pendulum damping. Use a Q matrix that penalizes non zero pendulum angles and non zero rotary beam locations. Carry out the control design by first simulation and then implementation on the real hardware.

11 A Derivation of System Dynamics by Lagrange method The kinematics can be determined from Lagrange s equation, d L dt q L = Q D q (3) where L = T U, T is the kinetic energy, U is the potential energy, Q is a generalized force vector, and represents dissipative energy from damping. D The kinetic energy of the rotary arm is defined by the rotational kinetic energy T arm = J arm θ (3) and the kinetic energy of the pendulum is the sum of rotational kinetic energy and translational kinetic energy T pend = J p α + m pṙ T pṙ p (33) where r p is the position vector of the pendulum center of mass in 3D space, x p r sin θ + l p sin α cos θ r θc θ + l p αc α c θ l p θsα s θ r p = y p = h l p cos α, ṙ p = l p αs α, (34) z p r cos θ l p sin α sin θ r θs θ l p αc α s θ l p θsα c θ where c θ = cos θ, s θ = sin θ, c α = cos α, and s α = sin α. The coordinate values of x p and z p are determined from the top view of the system, figure??. To find T, expand the term ṙ T pṙ p, ṙ T pṙ p = Figure 3: Top view of the rotary pendulum system. ( r θc ) ( θ + l p αc α c θ l p θsα s θ + (lp αs α ) + r θs ) θ l p αc α s θ l p θsα c θ (35)

12 expanding the quadratics, combining terms, and making cancellations, ṙ T pṙ p = r θ c θ +r θc θ l p αc α c θ l p θsα s θ + l p α c αc θ + r θ s θ +r θs θ l p αc α s θ + l p θsα c θ + l p α c αs θ ṙ T pṙ p = r θ + lp α + lp θ s α + rl p θ αcα. 4 Thus, the energy terms can be written as T = J arm θ + J p α + m p U = m p gl p c α, 4 4 l p α θc α c θ s α s θ 5 + l p α θc α s θ s α c θ 5 + lp θ s αs θ + l p α s α 6 + lp θ s αc θ, 6 4 (36) (r θ + l p α + l p θ s α + rl p θ αcα ), (37) D = C arm θ + C p α, (39) and the Lagrange s equation terms can be written as ( L q = Jarm + m p r + m p lps αh ) ] θ + mp rl p αc α, (4) (J p + m p lp) α + m p rl p θcα ( ) d L (Jarm + m p r ) θ + m p l = ps α θ + m p l θ ] p αs α c α + m p rl p αc α m p rl p α s α, (4) dt q (J p + m p lp) α + m p rl p θcα m p rl p θ αsα ] L = m p lp θ. (4) s α c α m p rl p θ αsα m p gl p s α Finally, the resulting dynamics can be rearranged into the manipulator equation form (38) H(q) q + C(q, q) q + G(q) = Bu, (43) where ] Jarm + m p r + m p l H(q) = ps α m p rl p c α, m p rl p c α J p + m p lp C(q, q) = m plp α sin α + C arm m plp θ ] sin α m p rl p αs α m, plp sin α C p ] ] G(q) =, B =. m p gls α (44) Note, that the choice of C is not unique, and the identity sin α cos α = sin α was used to determine C.

13 B Linearized Dynamics For a general dynamic system ẋ = f(x, u), (45) a fixed point is an input and state combination (x, u ), such that the function f evaluates to zero, i.e. f(x, u ) =. The linearized dynamics of any system, ẋ = f(x, u), around a fixed point can be determined by Taylor series expansion thus at a fixed point ẋ = f(x, u) f(x, u ) + ẋ f x] f x] x=x,u=u (x x ) + x=x,u=u (x x ) + f u] For the general manipulator system, the dynamics are given by f u] x=x,u=u (u u ), (46) x=x,u=u (u u ). (47) q = H (q) Bu C(q, q) q G(q)]. (48) Defining the states as x = q, q] T, we can rewrite the dynamics in the state space form ] ] ] q q f (x, u) ẋ = = q H f(x, u) = (q) Bu C(q, q) q G(q)] f (x, u) Using (48) the terms of the Taylor series expansion can be written as ] f u = f I H, and (q)b x = f f q (49) ]. (5) Taking the partial derivative of f with respect to q, f = H Bu C(q, q) q G(q)] + H C(q, q) (q) q G(q) ], (5) which reduces to f = H (q) G(q), (5) x=x,u=u since at a fixed point q = and Bu C(q, q) q G(q)] =. Now, taking the partial derivative of f with respect to q, ] f C q = H (q) (q, q) q + C(q, q) = H (q)c(q, q), q (53) which reduces to f q = H (q)c(q, q). x=x,u=u (54) Thus, the linearized dynamics of the manipulator equation around a fixed point are given by where f A = x] f B = u] x=x,u=u = x=x,u=u = ẋ = Ax + Bu, (55) ] I H(q), (56) C(q, q) x,u H(q) G(q) H (q)b] 3 x,u. (57)

14 C Files The necessary files to carry out the lab can be found in T:\ME389 MEM4 GANTRY\. The files are separated in three directories. \ME389 VIs - Includes LabView files for controlling the hardware QNET ROTPEN Lab 3 Gantry Control ME389.vi - Can be used for carrying out control experiments in the hanging position. QNET ROTPEN Lab 4 Inv Pendulum Control.vi - Can be used for carrying out control experiments in the vertical balance position. Note: Ignore the warning for finding the missing file.. \GANTRY WorkingData - MATLAB files for plotting LabView Output 3. \Pendulum Simulation - MATLAB files for simulation Use ROTPEN ME389 GANTRY.m to simulate control the rotary pendulum in the hanging position for the gantry control problem. Plot Gantry SimData.m can be used to plot the results of the gantry control simulation. Use ROTPEN ME389 InvPend.m to simulate control the rotary pendulum in the inverted position for the balance control problem. Plot InvPend SimData.m can be used to plot the results of the gantry control simulation. 4