PID CONTROL FOR TWO-WHEELED INVERTED PENDULUM (WIP) SYSTEM Bogdan Grămescu, Constantin Niţu, Nguyen Su Phuong Phuc, Claudia Irina Borzea University POLITEHNICA of Bucharest 313, Splaiul Independentei, 060042 Bucharest, Romania bogdan.gramescu@upb.ro, constantin.nitu@upb.ro Abstract - The complexity of the inverted pendulum system has drawn the interest of many researchers around the world. Its instability feature is a good example for a control engineer, with the purpose of verifying the control theory. To verify this system, equilibrium can be maintained, being possible to do the simulation using a method with a closed loop controller, such as LQR or PID. The article presents the achievement of a two-wheeled inverted pendulum (WIP) system, having an opensource development Arduino platform as control centre, which is programmed with Processing opensource environment. With the purpose of optimizing the system operation, a Kalman filter is implemented on the Arduino platform. Keywords: Mechatronics, Inverted Pendulum, Arduino, Kalman Filter, Control Algorithm. 1. Introduction The stabilization issue has been increasingly approached in recent years, being found in a very vast application range. One of the classical stabilization control issues is the inverted pendulum. The process is nonlinear and unstable, with a single input signal and several output signals. The inverted pendulum presented in this article is a two-wheeled inverted pendulum system. The stabilization of the inverted pendulum represents a basic model for the control of other similar processes, such as: rocket takeoff, equilibration of heavy loaded cranes, etc. In recent researches, the balancing of the inverted pendulum has often been studied in the modern control theories, such as fuzzy control, variable gain control, nonlinear control and neural networks. The main goal was to design a mechatronic system consisting of an inverted pendulum articulated on the wheels axis of a two-wheeled system, with constructive solutions to correct the design errors of existent products, and to simplify the realization of a control and actuation system. 2. The structure of Two-Wheeled Inverted Pendulum (WIP) System The design principle was the following: the mechanical components consisted of LEGO components, the centre of gravity being chosen as high as possible in regard to the axis of rotatio. Figure 1: The structure of the two-wheeled inverted pendulum system Besides these components used as support, the system also consists of motors (Lego NXT with 1:48 integrated reduction gearbox and rotation transducer with 720 pulses per rotation), the Arduino Mega 2560 v3 control platform and an electronic block consisting of L298 motor driver circuit, a DC-DC convertor with 3.3V DC, and a Bluetooth HC-05 communication module for the connection with a computer system. 246 The Romanian Review Precision Mechanics, Optics & Mechatronics, 2015, Issue 48
Figure 2: The electronic block The control system also integrates a GY-521 sensor based on MPU-6050. This one is used to provide the necessary information to the control algorithm, regarding angles, speeds and angular accelerations. Thus, an inertial sensor had to be installed, being necessary to read the data provided by this one, but also to filter this data, in order to reduce noise and errors as much as possible. MPU-6050 is the first integrated 6 axis motion tracking device that combines a 3 axis gyroscope, a 3 axis accelerometer, and a digital motion processor (DMP), all in a small package, of 4mmx4mmx0.9mm. The values received from MPU-6050 sensor are transmitted to the Arduino development platform through I2C interface. 3. The control program implemented on the Arduino platform Starting from the mathematical model necessary for the implementation of the control algorithm, it was possible to elaborate the control program of the system, which was implemented in the Arduino Mega platform. As programming environment, the open-source Arduino software (IDE) was used, which uses Java programming language that is based on Processing open-source language. The control algorithm for the inverted pendulum system is similar to the one presented in figure 3 Figure 3: The control algorithm used for the robot The entire control process is a closed loop, the steps repeating with a predetermined frequency (figure 4). Initially, all parameters have to be declared and all the necessary conditions have to be set for the following steps. This first step of initialization is included in setup() function of the program, and is passed through once. After passing through the first step, the program will run a closed loop, at the end of each set of steps being obtained the command for each motor. The loop of the program implemented on Arduino platform is loop() function. The PWM signal for each motor is equal to the sum of the outputs of PID 1, respectively PID 2 controllers, plus the output of PID 3 controller on left motor, respectively minus the output of PID 3 controller on right motor, or vice versa. Depending on the output reference of PID 1 controller, the entire inverted pendulum structure can be controlled for moving forwards or backwards. Depending on the output reference of PID 3 controller, the structure can be controlled so that to turn or rotate. The frequency of passing through the loop is a very important factor, with direct effect on the stability level of the system. In figure 4, the main steps are presented briefly, behind whom there are various other steps such as: calibration of the signals received from sensors, filtering these signals, communication with computer, checking the battery level, etc. The Romanian Review Precision Mechanics, Optics & Mechatronics, 2015, Issue 48 247
Figure 4: Main processing steps of the program implemented in the Arduino platform Figure 5 presents the interface generated by the program. The upper part contains the graphics which present interest, and the bottom part contains the parameters of the three PID controllers, that can be adjusted directly from here. By means of this program, the time necessary for the control process of the robot is significantly reduced. Figure 5: The communication interface between Arduino platform and computer 248 The Romanian Review Precision Mechanics, Optics & Mechatronics, 2015, Issue 48
4. Implementation of Kalman filter on the Arduino platform For the statistically optimum estimation of the state of the basic system, the implementation of a Kalman filter is very useful. This algorithm uses a series of measurements observed in time, which contain noise (random variations) and other inaccuracies, and realizes estimations of the unknown variables that tend to be more accurate than the ones based on a single measurement. For the implementation of the filter on the Arduino platform, the functional model has been used, which was modified according to proper necessities. The original source code [4] belongs to Kristian Lauszus, cofounder of TKJ Electronics, an enterprise in Denmark specialized in electronic components and their applications. The implementation steps of Kalman filter are: Step 1: Predict a priori state. (1) (2) Based on this equation, the code implemented in the Arduino platform is: Step 2: Predict a priori error covariance. (3) (4) Step 3: Compute residual. (5) Step 4: Compute [S k] matrix. (6) The Romanian Review Precision Mechanics, Optics & Mechatronics, 2015, Issue 48 249
Step 5: Compute Kalman gain [ ]. (7) (8) In case S is a matrix, P cannot be divided by S, and therefore the inverse matrix has to be computed. Step 6: Obtain a posteriori state estimate at k moment. (9) (10) Step 7: Obtain a posteriori error covariance estimate (final step). (11) (12) A posteriori error covariance is reduced because the state estimation error has been diminished. Based on this equation, the code implemented on the Arduino platform (the following variations being chosen for most IMU) is: Instead of Kalman filter, a complementary filter can be implemented: 250 The Romanian Review Precision Mechanics, Optics & Mechatronics, 2015, Issue 48
where the sum of the coefficients is 1. This filter is simpler, but less accurate, providing however good results when used for simpler systems. (13) Figure 6: Graphics obtained in the following cases: unfiltered, filtered with Kalman Filter and filtered with Complementary Filter The information provided by gyroscope is integrated by time and used to update the angular position, the step by step increment being useful to prevent that the drift occurs in time, characteristic for the gyroscope. To assure enhanced disturbance sensitivity, the information from accelerometer is also used in a small amount. 5. Conclusions The equilibrium of an inverted pendulum, articulated on the wheels axis of a two wheeled system, has been successfully maintained. Between two controllers, PID and LQR, PID controller has been chosen to realize the program implemented on the Arduino platform. Kalman filter has been successfully implemented. The drift of the gyroscope was efficiently removed, so that to allow an exact estimation of the inclination angle and its drift, for the system. The program for the registration of data provided by the inertial sensor and for applying Kalman filter works properly. A greater importance is given to the fact that data from the serial port has been successfully received, and a control algorithm has been elaborated on the basis of a mathematical model, for which the program outputs have been observed depending on various inclinations and accelerations tested on the system structure, without recording abnormal values or errors. 6. References [1] Balancing a two-wheeled Autonomous Robot, Rich Chi Ooi, School of Mechanical Engineering, The University of Western Australia [2] Advance Control of Wheeled Inverted Pendulum System, Zhijun Li, College of Automation Science and Engineering, South China University of Technology, Chenguang Yang, School of Computing and Mathematics, University of Plymouth, Plymouth, Devon, UK, Liping Fan, Departament of Automation, Shanghai Jiao Tong University Shanghai, People s Republic of China [3] https://github.com/tkjelectronics/example- Sketch-for-IMU-including-Kalman-filter [4] http://blog.tkjelectronics.dk/2012/09/a-practicalapproach-to-kalman-filter-and-how-to-implementit/comment-page-1/ [5] http://www.plethoraproject.com/education/2011/09/12/processing-tutorials/ [6] NXTway-GS Model-Based Design-Control of selfbalancing two-wheeled robot built with LEGO Mindstorms NXT, Yorihisa Yamamoto, Aplication Engineering, Applied Systems First Division, CYBERNET SYSTEMS CO.,LTD. [7] Progetto e implementazione del Sistema di controllo per un pendolo inverso, Marco Triverio, Departamento di Elettronica e Informazione, Corso di Laurea in Ingegneria Informatica, Politehnico di Milano. The Romanian Review Precision Mechanics, Optics & Mechatronics, 2015, Issue 48 251