Real-Time Optimal Control for Rotary Inverted Pendulum

Problem statement: The rotary inverted pendulum system was a highly n onli ear model, multivariable and absolutely unstable dynamic syste m. It was used for testing various design control techniques and in teaching modern control. The obje ctiv s of this study were to: (i) Develop a real rotary inverted pendulum which derived the mechanic l model by using Euler-Lagrange and (ii) Design controller algorithm for self-erecting and b alancing of a rotary inverted pendulum. Approach: Research shown a convenient way to implement a real -time control in self-erecting a pendulum from downward position and balancing the pendulum in ver tical-upright position. An Energy based on PD controller was applied in self-erecting of the pend ulum while LQR controller was applied to balance the pendulum. Results: Results of both control techniques from computer s imulation and experiment were given to show the effectiveness of these contr ollers. Conclusion: Both simulations and experiments were confirmed the control efficiency o f the method.


INTRODUCTION
The Rotary Inverted Pendulum (RIP) is a challenging problem in the area of control systems. It is made to verify the performance and demonstrating the effectiveness of control algorithm techniques. This RIP is a simple structure, multi-variable and unstable nonminimum phase system subjected to many nonlinear characteristics. It has a pendulum attached to a rotary arm instead of a moving cart. The advanced of this system is that there is no end point, which makes it convenient for experimentation especially during velocity control of the arm speed. Some research on the balancing of rotary inverted pendulum has been done using the linear control theory [1] , Control law based on energy control [3] , with the use of a balancing sequence, which allows rising up the pendulum from its stable equilibrium position. Zhong and Rock [10] used a linear quadratic regulator to optimize the control gains used in the feedback controller. For the single inverted pendulum a simple bang-bang method as described by Astrom and Furuta [4] can be applied. This method regulates energy content in the system. Energy is added to the system with a stepped signal until the desired energy level is achieved. Other methods of swing-up control are outlined [5,6,8,9] . Iraj HassanZadeh and Saleh Mobayen proposed PSO-based controller for balancing rotary inverted pendulum [2] .
This study needs to propose an energy based control [4] to swing up a pendulum from downward position to steady state upright position, which is derived from the exact nonlinear mathematical model of rotary inverted pendulum. And then uses the LQR control technique was a main of controller to stabilize the rotary inverted pendulum such that the a pendulum is always to maintain it upright position and to maintain the arm position in horizontal plane by using a state feedback control to move unstable poles of a linear system to stable ones.
All experiments have been done in simulations on the mathematical model of rotary inverted pendulum and in real-time [7] experiments on the real rotary inverted pendulum system, which made in AIT Lab.

Modeling of rotary inverted pendulum:
The rotary inverted pendulum system consists of a controller, an arm, a pendulum, an actuator (a dc motor) and two increment rotary encoders. The controller makes the pendulum stand at upright position on the rotary arm by moving the arm supported on the base. The motor provides power to rotate the arm. The encoders detect the pendulum and arm angular position. θ && are angular position, velocity and acceleration of the pendulum, link 2. J 1 and J 2 represent moments of inertia of the two respective links about their center of mass. l 1 and l 2 are distances from center of rotation of the links to center of mass of the respective links. m 1 and m 2 are the masses of links 1 and 2 respectively. g is the gravitation acceleration and g = 9.81 m s −2 . L 1 and L 2 are the lengths of links 1 and 2, respectively. The variables C 1 and C 2 are the viscous damping coefficients of the bearings on which the links rotate. Derivation of mathematical equation describing dynamics of the rotary inverted pendulum system is based on Euler-Lagrange equation of motion: Where: T total = Total kinetic energy of the rotary inverted pendulum system V total = Total potential energy of the rotary inverted pendulum system From Fig. 2, the total kinetic energy of the system is the sum of kinetic energy of the rotary arm and the pendulum: The kinetic energy of link 1, the rotary arm is: The total potential energy for the system is the sum of potential energy of the rotary arm and the pendulum: V total = 0+m 2 gl 2 cosθ 2 The total loss energy of the system is the sum of loss energy of the rotary arm and the pendulum: From Eq. 2, the Lagrangian is determined as: Substitution of Eq. 5, 6, 8 and 9 into Eq. 10 yields: Euler-Lagrange equation of the motion of each variable, thus becomes: Substitution of Eq. 11 into Eq. 12-13 and solving of Euler-Lagrange equation yields: To simplify the equations, some parameters are defined as shown in Table 1.
Torque control of DC motor: A permanent dc motor generates a dc back-emf, E a that is proportional to the rotor speed as follow as: In steady state, voltage equation describing the motor is: From Eq. 18, finding current, I a , thus: Substitute Eq. 19 into Eq. 17, yield as following: where, V a is the applied dc voltage. The developed electromagnetic torque of a permanent dc motor can be readily expressed. The two main considerations in choosing a motor are needs for high torque and high speed. The torque is necessary for the rotary arm to change direction of rotation quickly in order to keep the two pendulums balanced. And high speed is needed such that the rotary arm can move faster than the pendulums can fall. The linear dynamic model is used for designing controller, which can be put into state space representation. When combining the derived mathematical model of nonlinear model and model of motor to complete set of mathematical equation of rotary inverted pendulum system for control voltage, V a and rearrangement of Eq. 16-18 based on parameters in Table 1. The nonlinear equations of motion are: To linearize, the model, the following approximations are applied: The linearized model, thus, becomes: By using the parameters defined in Table 2, the rotary inverted pendulum can be expressed as: The state and output equations for the inverted pendulum system are: By defining the state variables as followings: 1 1 x = θ (arm position), 2 1 x = θ & (arm velocity), 3 2 x = θ (pendulum position), 4 2 x = θ & (pendulum velocity).
The linearized model of the rotary inverted pendulum results in: Controller design: Self-erecting pendulum by energy based on PD controller: The self-erecting controller is used to swing the inverted pendulum up to the upright position when the pendulum is far from the upright position; for example, when the pendulum falls down by gravity. The self-erecting controller brings the pendulum upright close to the unstable point of equilibrium. Swinging up controller calculates the total system energy based on the kinetic energy of both links and the potential energy of the pendulum.
The control law function for input of the system is given by: In this case the energy equation is: The dc motor is placed under position control for the deriving force. Many schemes can be applied to provide a driving force at a suitable trajectory in the manner that energy is gradually added to the system to bring the pendulum to the inverted position. In this study, the design is based on a rate feedback and a position feedback given by: This feedback loop is shown in Fig. 2 The second-order response peak time t p , is given by: Thus, we have: Parameters of the designed rotary inverted pendulum system are identified and found as shown in Table 3. These values are then used in the state space model to obtain a numerical representation for the purpose to design the controller.
In self-erecting controller, positive feedback based on the pendulum angular position and velocity creates a trajectory with growing amplitude. To design a closedloop controller for the arm position which has the following specifications: The Percent Overshoot (PO) should be less than 10% and time to first peak should be 150 ms. Then: The minimizes the cost function is defined as: The subject to the state dynamics is defined as: x

MATERIALS AND METHODS
The overall architecture of the rotary inverted pendulum system with real time control by xPC Target is shown in Fig. 3.
A block diagram of the control system design is shown in Fig. 4. The self-erecting controller is base on energy based control and balance controller is a LQR controller.
As shown in Fig. 3, there are two PCs: Host and target PCs. The host PC with MS-Windows XP OS runs matlab, simulink, real-time workshop (RTW), xPC target and C/C++ compiler. The simulink is used to model dynamic of the physical system and controllers. RTW and C/C++ compiler convert simulink blocks into C code and build a target that is then download to the target PC through TCP/IP connection. The target PC with MS-Dos in real time, booted from floppy disk runs executable code generated from host PC.
We describe the physical hardware and software environment that used to implement the self-erecting or swing-up and balancing control for the rotary inverted pendulum. To design LQR and Energy based on PD controller, the state weighing matrix, Q, is designed in such the way that the closed loop response meets the following specifications: Arm Regulation: |θ 1 |<120°, Pendulum Regulation: |θ 2 |<3° and Control input limit: V m <±10V. Thus the controller should regulate the arm about zero degree within 120° and balance the pendulum about the upright position within 3°. A control switch is shown in Fig. 4. Its purpose is to track the pendulum angular position, θ 2 and switch between the selferecting and the balancing modes. This control switch is enable when θ 2 is in neighborhood of zero, within ±3° and as long as |θ 2 | < 25°. Thus, we are started constructing the implement diagram for closed-loop system the uses the self-erecting controller and balancing controller is shown in Fig. 3 as: Two  Fig. 6. Get analog output block from MF624 library. These model diagram, we can selected a manual mode for move the pendulum to the vertical upright to balancing control mode or selected the switch to swing-up mode.
The desired position is determined by the following control law. This positive feedback based on the pendulum angle and its velocity creates a trajectory with growing amplitude. As shown a block diagram in Fig. 7.
The control switch is shown in Fig. 8. Its purpose is to track the pendulum angle, θ 2 and facilitate switching between the self-erecting and balancing mode. This controller is to be enabled when θ 2 is in neighborhood of zero, within ±3° and for as long as |θ 2 |<25°.
The desired position is determined by the following control law. This positive feedback based on the pendulum angle and its velocity creates a trajectory with growing amplitude experimentally, obtain of the K p = 7.85, K D = 0.65. In simulations, the statecontroller gain vector, K from Eq. 44 is used to balance a pendulum, where all pendulum frictions are considered to be negligible. For the simulation results for the self-erecting and balancing controller for rotary inverted pendulum shows in Fig. 9.

RESULTS
Simulation and experimental results are discussed in this section. Fig. 9 and 10 show the position and velocity versus time and control voltage versus time for the rotary inverted pendulum. The simulation results are shown in Fig. 9a-e. The arm is rotated alternately between 0 and 50° to increase the energy of the system quickly to move the arm to its maximum allowed angle of circular of arm. In this self-erecting mode, energy based PD controller takes 4.5 sec for 5 swings which is effectively employed in the control system to obtain maximum swing in short time. The amplitude of pendulum angle becomes larger until it is close to the upright position, then the balancing mode by LQR controller is used to stabilize and maintain the pendulum in the upright position, even when the disturbance is applied to the arm position. The robustness of the rotary inverted pendulum is successfully realized by the proposed LQR controller.
From the real-time experiment as shown in Fig.  10a-e, the pendulum swings into the upright position in 5 more swings as it does in simulation. This happens because the pendulum friction was not considered. After reaching the upright position, the closed loop system is stable. But the pendulum oscillates around the equilibrium point at small amplitude.

DISCUSSION
In many paper used a rotary inverted pendulum as example for studying on controller design in optimization problem. In this study problem, we propose a controller for a real-time experiment of the system to compare with benchmark system previously studies control professionals where the arm is driven by motor. This approach appears to have worked very well for system studied here. Further study on this system would bring significant benefits to robotics and control professionals.

CONCLUSION
This study presents the simulation and experimental implementation and results of energy based PD control and LQR optimal controller applied to the self-erecting and balancing of a rotary inverted pendulum. In the both cases, computation load is using the complete optimal control problem. In the real-time control case with MATLAB, xPC-Target is used as the real-time computation platform and 0.001 sec sampling rate is achieved. The real-time experiments have been very impressive even during the activity of defects or after changing the designed the arm position. The results from both simulations and experiments confirmed the control efficiency of the method.