1 Introduction. 2 Process description

Transcription

1 1 Introduction This document describes the backround and theory of the Rotary Inverted Pendulum laboratory excercise. The purpose of this laboratory excercise is to familiarize the participants with state machines in control applications. The process must be controlled differently at different times. For example, the pendulum must first be initialized and brought to a suitable starting position, then swung up to the upper position and finally balanced. A single control algorithm is not suitable for all situations, and a method to automatically decide the used control is needed. Although an inverted pendulum is not a commonplace industrial application, the mode switching methodology can be generalized to other processes even the whole plant. 2 Process description The process analyzed in this laboratory excercise is a rotary inverted pendulum (RIP), also known as a Furuta pendulum. The pendulum arm is attached to a horizontal arm which is actuated by a DC servo motor. The objective of the system is to balance the pendulum arm in the upright position. The control is implemented with Simulink on a separate PC. The control is based on the measurements of the angles and angular velocities of the two arms. The angle of the pendulum arm is measured using an optical encoder, which transforms and quantizes the angle into a binary signal. The angle of the horizontal arm detemined using a potentiometer attached to the shaft of the motor. The measurements are sent to the control system for further processing. The motor is controlled by a motor control board which transforms the 0 5 V control signal to a correct motor voltage. To prevent the horizontal arm from rotating too far from the center (this might damage the wires), safety switches have also been installed in the process. These switches immediately cut the power from the motor control board if crossed. The pendulum control is implemented in a dedicated PC with xpc target, which is a real-time software environment enabling the use of Simulink and Stateflow to control actual processes. The Simulink model can be divided into three main parts: Filtering and transforming the raw measurement data (and calculating the derivatives), calculating the state and control signal, 1

2 and finally transforming the calculated control signal to a voltage signal sent to the motor control board. 3 Mathematical model of the pendulum A mathematical model of the pendulum is introduced in this section. The details of the derivation can be found in [2]. We must first describe the physical equipment. At the center is a pillar with moment of inertia J. Rigidly connected to it is a horizontal arm with length l a and homogenously line distributed mass m a. The pendulum arm, with length l p and homogenously line distributed mass m p hangs from the end of the horizontal arm. At the end of the pendulum arm is a balancing body with point distributed mass M. The angle of the horizontal arm is φ and counterclockwise (when viewed from above) is taken as the positive direction. Pendulum arm angle is denoted by θ where clockwise direction (when viewed from the front) is positive. Angle θ = 0 corresponds the labile equiliribium point at the top. The equations of motion will be derived using Lagrangian mechanics. For this, we need to know the kinetic energy T and the potential energy V of the system. These are most easily calculated one section at a time. Calculation of the center pillar s kinetic energy is straightforward. The pillar has no effect on the potential energy of the system. T c = 1 2 J φ 2 V c = 0 The horizontal arm s energies are equally simple. T a = 1 6 m al a φ2 V a = 0 For the pendulum arm, the kinetic energy consists of three terms: One describing the revolution around the center pillar, one describing the revolution around the horizontal arm and one describing the cross effect of these. The potential energy is considered to be zero when the pendulum arm is 2

3 horizontal. T p = 1 2 m p V p = 1 2 m pgl p cosθ (l 2a + 13 ) (l p sin θ) 2 φ m plp 2 θ m pl a l p cos θ φ θ The balancing mass s energies are calculated in a similar manner. T m = 1 2 M ( la 2 + (l p sin θ) 2) φ Ml2 θ p 2 + Ml a l p cos θ φ θ V m = Mgl p cos θ The kinetic and potential energies can be added up to get the total kinetic and potential energy of the system. To simplify these expressions, we introduce new variables: α = J + (M + 13 ) m a + m p la 2 ( β = M + 1 ) 3 m p lp 2 ( γ = M + 1 ) 2 m p l a l p ( δ = M + 1 ) 2 m p gl p With these new variables, the total kinetic and potential energies can be formulated more conveniently. T = T c + T a + T p + T m = 1 2 α φ ( ( 2 β sin θ φ ) 2 + θ2) + γ cos θ φ θ V = V c + V a + V p + V m = δ cos θ Lagrangian mechanics is based on the difference of these two, L = T V. For each degree of freedom in the system (here the φ- and θ-joint), an equation based on partial derivates describes the external forces acting on the system. d dt d dt ( ) L φ ( ) L θ L φ = τ φ L θ = τ θ 3

4 The left hand sides of the equations contain second order derivatives of φ and θ, allowing the solution of these. 1 ( φ = αβ + β 2 sin 2 βγ cos 2 θ sin θ θ γ 2 cos 2 θ φ 2 2β 2 cos θ sin θ φ θ +βγ sin θ θ ) 2 γδ cos θ sin θ + βτ φ γ cos θτ θ 1 θ = αβ + β 2 sin 2 θ γ 2 cos 2 θ ( β ( α + β sin 2 θ ) cos θ sin θ φ 2 2βγ cos 2 θ sin θ φ θ γ 2 cos θ sin θ θ 2 δ ( α + β sin 2 θ ) sin θ γ cos θτ φ + ( α + β sin 2 θ ) τ θ ) Linearizing these expressions at the origin simplifies them greatly. φ = γδ αβ γ θ + β 2 αβ γ τ γ 2 φ αβ γ τ 2 θ αδ θ = αβ γ θ γ 2 αβ γ τ α 2 φ + αβ γ τ 2 θ The external forces τ φ and τ θ are caused by friction and actuators. The simulation model is assumed frictionless and the force generated by the motor is independent of the current state. Therefore, the control signal determines these forces. τ φ = u τ θ = 0 4 Swing-up control of the pendulum Before the pendulum can be balanced at the upper position, it must first be swung up from its rest position. This swing-up problem is inherently nonlinear, and various methods to accomplish this have been developed. The control methods which have been implemented in this pendulum system are brifly described. 4.1 Energy control Instead of controlling the pendulum angle θ directly, energy control tries to drive energy of the system to zero (upwards position). The method has been 4

5 proposed by Åström and Furuta [5]. Control strategy estimates the energy of the process at a given time instant from the measured state variables using a simplified process model. It assumed that the pendulum arm is fixed and the controller torque is applied directly to the pendulum pivot point. This assumption holds approximately if the arm is relatively heavy compared to the pendulum. Also, reaction force from the pendulum to the arm is not considered in this simplified model [1]. The control law (applied torque to pendulum pivot point, u) is the following: u = sat ng (k(e E 0 )) sign( θ cos θ), (1) where k is a positive scaling coefficient, E is the energy of the system, E 0 is the target energy (zero when swinging up). sat ng ( ) is the saturation function, limiting the energy difference, E E 0 to interval ng... ng. k and ng are design parameters of the controller. With small differences the control is linear w.r.t energy difference, and with large differences the control is saturated to ±ng. 4.2 Swing-up using a PID positive feedback controller This swing-up strategy uses a cascaded PID controller structure. The method has been proposed by Wang et Al. [4]. The outer loop outputs a reference trajectory for the horizontal arm using measurements of the angle and speed of the pendulum, i.e. φ d = P θ + D θ. (2) This reference is then input to the inner loop, which performs position control of the horizontal arm using the horizontal arm angle and speed. The output of the inner loop controller is the control signal which is fed to the servo motor: u = K p (φ d φ) + K d φ (3) As an added heuristic, the reference φ d is saturated at some small angle to increase the smoothness of the swing-up process. 5

6 4.3 Heuristic swing-up method The heuristic swing up controller has been designed manually with the single objective that works. Because of this, it is specific to this particular system and cannot easily be adopted to other situations. The control signal is the sum of three terms. The first one aims to accelerate the pendulum, the second one to keep the horizontal arm near the center and the third term attempts to restrict the pendulum s horizontal movement. Each term is multiplied by some weighting coefficient and the combined value is saturate to [ 1, 1]. The acceleration term is greater when the pendulum s movement is horizontal, since it has a bigger impact in this situation. The acceleration is applied only when the pendulum is below the horizontal arm. Its precise format is the following: u a = K a min (0, cos θ) sign θ For the second term, φ is first scaled to an appropriate region. The tangent of this new angle is then used to determine the magnitude of the control. ( ) π/2 u c = K c tan φ M φ The third term uses a weighted average of the horizontal velocities of the horizontal arm and the balancing mass to calculate the horizontal velocity of the pendulum. ( ) u s = K d φ + mr θ cos θ At a general level, heuristic controller corresponds to control tailor made rule based algorithms. These can be developed by taking advantage of expertise of the system or by trial and error. Such a controller will usually be very process specific. 4.4 Recorded joystick control signal The pendulum was initially swung up manually using a joystick. The control sequence of a successful swing up was recorded and can be repeated precisely. No feedback is used when implementing the recorded swing up. Because of this, it is completely indefferent of measurement noise, which is a significant benefit. The downside is that even a slight change to the physical system can cause the swing up to fail. In such a situation, it is difficult to fix the swing up without completely rerecording it. 6

7 5 Balancing control The pendulum is balanced at the upper position using a state feedback controller. For more information about state feedback and LQ control, refer to the laboratory instructions of the ball balancing system Halvari. 6 Switching between control modes The desired control behavior of the pendulum is not the same at all times. A different control behavior is desired when swinging up and when balancing. Also, there are some preliminary phases which need to be done, such as calibrating the encoder and centering the horizontal arm. The switching between different control phases is controlled using a state machine. 6.1 Finite State Machines Finite state machine (FSM) is a way to model digital logic. State machine consists of states, transitions between them and actions (i.e., outputs) related to states and transitions. Transitions define what state to activate next, given the current state and inputs. The state machine is initialized with a default state (start state). Each transition has optional conditions (guards) which describe when the transition is taken. There are many different notations to model finite state machines and the MATLAB StateFlow is one of them. StateFlow represents the stateflow using a visual model where the states are represented using boxes and the transitions between them using arrows. Transition conditions are written next to these arrows. Actions can be considered the outputs of the state machine. In StateFlow, every state can have entry, exit and during state actions. Entry and exit actions are executed when state is entered or exited, respectively. During state action is executed when the state is active and when there has been no transition. The transition can also have actions which are executed on transition. See chapter for more details StateFlow also has a concept of superstate which refers to a state that haves substates. The substates work roughly the same way as a separate state machine inside the state machine. When the superstate is activated, 7

8 the corresponding default substate is also activated, and as long as the superstate is active the active substate changes through transitions between the substates. Substates are an effective way to divide the state machine implementation to larger entities. This aids chart readability, and makes the validation easier. 6.2 StateFlow specific details The purpose of this section is to cover most important aspects (in terms of the laboratory project) of MATLAB StateFlow. For interested readers MATLAB StateFlow User s Guide [3] provides a good start-up point for more advanced StateFlow features such as history-preserving states and parallel states State Actions The three most important state actions are entry, exit and during actions. Entry and exit actions are executed when state is entered or exited, respectively. During state action is executed when the state is active and when there has been no transition. Also the transition can have actions which are executed on transition. State actions are specified in the state label. The label has the following syntax: name/ entry:entry actions during:during actions exit:exit actions on event_name:on event_name actions All the above fields are optional. Name gives the state an unique identifier which can be user to reference the state elsewhere in the state chart. Entry, during and exit can be abbreviated as en, du and ex, respectively. Multiple actions are separated by comma. Actions can be many things but maybe the most common is to set a variable value using standard MATLAB syntax Events Events are special objects in MATLAB StateFlow for sending messages to other states and Simulink components. In the laboratory work, input signal 8

9 triggers an event on every sample which makes temporal logic possible (see 6.2.3). Another application of events is synchronization of parallel states Transition Labels Transition conditions are written to the transition arrow label. The label has the following syntax: event [condition]{condition_action}/transition_action Here the event triggers the transition, provided that the condition is true. Condition action is is executed if condition evaluates to true. Transition action is executed after the source state is exited but before the destination state is exited. Conditions can use normal MATLAB comparison operators (<, <=, >=, >, =, ==) and math functions (sin, cos, etc.). In addition to these there are several operators related to events called temporal logic operators. The most important operators of these are the after, before, at and every operators. All these are boolean functions that calculate the occurrences of events since the activation of the state. For example, after(n, E) evaluates to true after event E has occurred at least n times. Other functions follow the same syntax. 7 Preliminary Excercises 7.1 Design a state machine for the laboratory pendulum In normal operation, the pendulum is first initialized (calibrated). The horizontal arm is then brought to the center position. After the pendulum is in position and sufficiently at rest, the swing-up phase commences. Once the pendulum arm is sufficiently near to the upper position, the system switches to the balancing controller. If the balancing fails at some point, the system should let the pendulum fall down on its own (release control). Design a state machine for the pendulum system fulfilling the requirements. Return (on paper) a sketch of the state machine and a description of the states and the transition conditions (i.e. what is done at each state and when do we switch states). The state machine can use the angles of the horizontal and pendulum arms and their derivatives to determine the 9

10 transition conditions time can also be used. Be prepared to explain your solution to the instructor. 7.2 Filtering the pendulum angle The angle of the pendulum arm is measured in the range of [ π, π], with 0 representing the upper position. This means that in the lower position, the angle wraps from π to π or vica versa. What problem does this pose when filtering the signal? What kind of filter structure could be used to circumvent this problem? (Mathematical formulation of the filter is not required) References [1] F. Gordillo, J. A. Acosta, and J. Aracil. A new swing-up law for the furuta pendulum. Int. J. of Control, 76: , doi: / [2] M. Gäftvert. Modelling the furuta pendulum. Technical report, Department of Automatic Control, Lund University, Sweden, isrn: LUTFD2/TFRT7574SE. [3] StateFlow documentation. The Mathworks Inc. URL mathworks.com/help/toolbox/stateflow/. Fetched [4] Z. Wang, Y. Chen, and N. Fang. Minimum-time swing-up of a rotary inverted pendulum by iterative impulsive control. In American Control Conference. Proceedings of the 2004, volume 2, pages vol.2, doi: /ACC [5] K. J. Åström and K. Furuta. Swinging up a pendulum by energy control. Automatica, 36(2): , ISSN doi: /S (99)