Controlling an Equilibrist Lego Robot

Transcription

1 Proceedings of the World Congress on Engineering 05 Vol I WCE 05, July - 3, 05, London, U.K. Controlling an Equilibrist Lego Robot Vinicius Silva, Pedro Leite, Filoena Soares, Gil Lopes, João Sena Esteves and Paulo Garrido Abstract This paper describes the design and ipleentation of a self-balancing two-wheeled robot, the Equilibrist Robot. The syste is siilar to the classical unstable, non-linear echanical control proble of an inverted pendulu on a cart. Using the Linear Quadratic Regulator ethod and PID for state feedbac, this paper shows a control algorith that solves this proble. This control is able to reject disturbances and stabilize the syste using a gyroscope. The control algorith is ipleented using Matlab and C- prograing. The Equilibrist anages to stabilize itself in an upright position, reject disturbances and change its position. Index Ters Inverted Pendulu, Linear Quadratic Regulator, Servo and Regulator Control, Lego Robot. M I. INTRODUCTION obile robots have been the focus of study aong the scientific counity. Their practical applications range fro search and rescue activities, aterials handling and entertainent, aong others []. The two-wheeled inverted pendulu (TWIP) robot is a specific configuration of obile robots that has been receiving special attention. In fact, their particular structure, unstable dynaic perforance, and nonlinearity ae the the ideal platfor to deonstrate and test different nonlinear control theories []. TWIP has been applied in different areas, fro coercial application to educational and research purposes. The Segway Personal Transporter [] is a well-nown exaple of a coercial product. Segway s robotics platfors are applied to different aret segents including: research and developent, indoor/outdoor navigation and path planning, industrial autoation, and defence applications. In [3], the authors present an ebedded and standalone inverted pendulu with two piezoelectric drives designed as teaching aid for Mechatronics courses. The platfor cobines Mechanics, Electronics and Process Control Manuscript received March 3, 05; revised April 06, 05. This wor has been supported by FCT - Fundação para a Ciência e Tecnologia in the scope of the project: PEst-UID/CEC/0039/03. Vinicius Siva is with the Industrial Electronics Departent, Capus de Azuré, Guiarães, Portugal (a653@aluni.uinho.pt). Pedro Leite is with the Industrial Electronics Departent, Capus de Azuré, Guiarães, Portugal (a666@alunos.uinho.pt). Filoena Soares is with the Industrial Electronics Departent, Algoriti R&D Center, Engineering School, Capus de Azuré, Guiarães, Portugal, (corresponding author phone: ; fax: ; e-ail: fsoares@dei.uinho.pt). Gil Lopes is with the Algoriti R&D Center, Engineering School, Capus de Azuré, Guiarães, Portugal, (gil@dei.uinho.pt). João Sena Esteves is with the Industrial Electronics Departent, Algoriti R&D Center, Engineering School, Capus de Azuré, Guiarães, Portugal, (sena@dei.uinho.pt). Paulo Garrido is with the Industrial Electronics Departent, Algoriti R&D Center, Engineering School, Capus de Azuré, Guiarães, Portugal, (pgarrido@dei.uinho.pt). ISBN: ISSN: (Print); ISSN: (Online) topics where the students can understand and test the ain principles of piezoelectric aterials, echanics, electricity and control theory as well as progra and control the syste with three independent icroprocessors. An application of the inverted pendulu syste focused on bioechanics is presented in [4]. The goal of the study was to ipleent an accurate control algorith (Fuzzy control) to regulate the speed of the robotic device, LOKOMAT [5], intended for rehabilitation of patients with lower extreity paralysis. The application was first virtually siulated and tested in Matlab to verify proper operation. An analogy was perfored between the robotic device and the inverted pendulu syste: the pendulu ovable ass atches with legs ass, and the pendulu ass with torso ass. Hence, to control LOKOMAT, the patient only needs to bend his/her torso. A LOKOMAT syste can be adapted to any user as it has a calibration routine (introduction of patient data height and weight) before running the fuzzy control algorith. There have been other TWIP architectures successfully designed and constructed. Jianhai et. al. (04) [6] proposed a two-wheeled, self-balancing robot using 6-axis MEMS sensors MPU6050 to easure its posture (algorith for sensor fusion of 3-axis gyroscope and 3-axis acceleroeter). The copleent filter (integrating the data of the gyroscope and acceleroeter) was designed in order to achieve a ore accurate inclination angle. A 6-bit MCU ipleents the PID control algorith to calculate the required control voltage for the otors, to adjust the robot s posture and eep the body balanced. The PID paraeters were tested through experients. In [7], the authors present the control ethod of cobining LQR and PID for a two-wheeled self-balancing robot that successfully overcoes the ipact of the constraints on the linearized syste. Juang and Lu (03) [8] report the design, construction and control of a two-wheeled self-balancing robot based on a pair of DC otors, an Arduino icrocontroller board, a single-axis gyroscope and a -axis acceleroeter. Also, a copleentary filter was ipleented to copensate the gyro drifts. PID and LQR-based PI-PD control designs were perfored on the linearized equations of otion. Experiental results showed that self-balancing can be achieved with PI-PD control in the vicinity of the upright position. The perforance of the inverted pendulu platfor when feedbac force is provided has been reported in [9]. This study presents a virtual pendulu syste based on OpenGL controlled with a joystic. The dynaical odel of the syste was obtained though atheatical anipulation and the feedbac force was introduced into the syste, providing an interactive operation ode. In [0], a two-wheeled self-balancing robot transporter odel was established. Kineatic and dynaic odels WCE 05

2 Proceedings of the World Congress on Engineering 05 Vol I WCE 05, July - 3, 05, London, U.K. were developed as well as two control ethods: the Proportional, Integral and Derivative (PID) algorith and the Linear Quadratic Regulator (LQR). The control objectives includes self-balancing (preventing syste fro falling down when it oves forward or bacward) and yaw rotation (steering angle regulation when it turns left or right). PID is used to control self-balancing and yaw rotation and LQR is used to control self-balancing only. The controllers perforance was tested using Matlab siulations. Results point out a better perforance of LQR than PID for self-balancing control. Vasudevan et. al. (05) [] presented a study on the design for wheeled inverted pendulu (WIP) platfors highlighting the effect of the design choices on the balancing perforance. In particular, they analysed the effect of soft viscoelastic tires (soft tires enhances balancing perforance); the effect of pitch rate and wheel velocity filters (suggestions for design of filters); the existence of a self-tuning liit-cycle copensation algorith; the effects of torque and velocity control of WIP otors; the effect of otor gearing on WIP dynaics. The overall goal was to define electroechanical design trade-offs to design and ipleent an efficient WIP platfor. This paper shows the design and ipleentation of a self-balancing Lego robot (Equilibrist Robot), which is an unstable syste. The odel is based on the inverted pendulu balancing on two wheels. This wor shows the atheatical equations of the odel as well as the full ipleentation of a control syste stabilizing the robot. The ais of this project includes: stabilize in an upright position; reject disturbances; forward otion, controlled via Bluetooth; avoid obstacles. II. TWO-WHEEL STRUCTURE The behaviour of this syste, the self-balancing robot, is siilar to the classical echanical syste of an inverted pendulu on a cart. It is a well-nown syste and its popularity derives in part fro the fact that it is unstable without control. The fundaental principle within this control syste can be found in any industrial applications, such as stability control of waling robots. A. Lego Robot Configuration The robot was built using Lego Mindstors, Lego NXT, which is an educational product released by Lego. The structure was priarily designed with the aid of the Lego odelling progra, Lego Digital Designer. The Equilibrist Lego Robot is presented in Fig.. The hardware included in the Lego NXT consists of two icrocontrollers, a aster 3-bit (Atel AT9SAM7) and a secondary 8-bit (ATega48), Bluetooth counication odule, display, USB counication, aong other characteristics. The NXT version used in this wor is equipped with a 3-bit ARM7 icroprocessor running the RobotC firware. Table I presents the list of paraeters and corresponding nuerical values considered in this wor for the Equilibrist Robot. Fig.. Configuration of the Equilibrist Lego Robot. TABLE I LIST OF PARAMETERS AND NUMERICAL VALUES USED Paraeter Value Description M = 0.5 [g] Body weight W = 0.4 [] Body width D = [] Body depth H = 0.67 [] Body height = [g] Wheel weight R = 0.03 [] Wheel radius L=H/ [] Distance of the centre of ass to the wheels axis J ψ = ML^/3 [g ] Body pitch inertia oent J ϕ = [g ] Body yaw inertia oent M(W +D )/ J = 0E 5 [g ] DC otor inertia oent R = 6.69 [Ω] DC otor resistance K b = [V sec/rad] EMF constant K t = 0.37 [N/A] DC otor torque constant N = Gear ratio f = 0.00 [N/(rad/s)] Friction coefficient between body and DC otor f w = 0 Friction coefficient between wheel and floor g = 9.8 [/sec ] Gravity acceleration The paraeters used for the DC otors were found in [], [3]. The expressions for the body pitch and yaw oents of inertia and the values for the friction coefficients were also taen fro [], [3]. B. Gyro Sensor The sensor used in this wor was a Gyroscope fro Dexter Industries. The proble of using a gyroscope is that its value drifts over tie. In Fig the drift of the angular position is visible. In order to solve this proble a recursive filter was ipleented following equation (): offset a * O( ) ( a ) * O ( ) () where O() is the current value being read and O( ) is the previous one. The gyro filtered value is constantly updated by adding a percentage of the previous and the current gyro values. To deterine the angular velocity, the signal fro the gyro was used directly after applying the recursive filter. In order to reconstruct the angle, the gyroscope signal was integrated. Through trial and error procedure, a value of a = 0.99 in the filter was found to correct the angle ISBN: ISSN: (Print); ISSN: (Online) WCE 05

3 Proceedings of the World Congress on Engineering 05 Vol I WCE 05, July - 3, 05, London, U.K. estiates. In Fig 3 the integration of gyroscope easureent with the filter ipleentation is shown: the angular position value is approxiately zero when the gyro is stationary. 0 0 A A ( 3, ) A ( 4, ) A ( 3, 3 ) A ( 4, 3 ) B B ( 3 ) B ( 3 ) B ( 4 ) B ( 4 ) 0 A ( 3, 4 ) A ( 4, 4 ) (4) (5) Fig.. Integration of gyroscope easureents without filter: initially the angular position is zero (gyro stationary); after altering the position of the gyro sensor the stationary value has drifted. Fig.3. Integration of gyroscope easureents with filter: the angular position value is approxiately zero when the gyro is stationary. C. Bluetooth counication The NXT has the possibility to counicate via Bluetooth with a PC, obile phones or others NXTs. In this wor, Bluetooth counication was used to control the position of the robot. The protocol used was based on the Lego protocol and it can be found in [4]. III. MATHEMATICAL MODEL The atheatical odel used in this project was based on the NXTway-GS docuentation. For ore details see [] and [3]. A. Open Loop The state-space vector considers the otor angle, the body angle, the otor velocity and the body velocity as state-space variables (). () x The state-space equation and atrices are shown in (3), (4) and (5), where u is the vector of the two voltages applied to the otors. A x B u (3) x where: alpha = n* K /R beta = n* K * K /R tp = beta + f E_ = ( * + M) * R E_ = M * L * R - * E_ = M * L t dete = E_ * E_ - E_ A (3,3) A (4,4) B (4) = - * = - alpha t w b + J = - * (tp + f n + * J * J + * n w * J A (3,) = - g * M * L * E_/detE A (4,) = g * M * L * E_/detE A (3,4) = * beta * ( E_ + E_ beta * ( E_ + E_ B (3) = alpha * ( E_ + E_ * ( E_ + E_ + * n * E_ + beta * E_ A (4,3) = * (tp * E_ + beta * E_ * J The open loop response was siulated in Matlab with different initial conditions. The syste showed an unstable behaviour. B. Closed Loop: Linear Quadratic Regulator The Linear Quadratic Regulator (LQR) ethod was ipleented to stabilize and control the syste and reduce steady-state errors by providing feedbac weighting for the input paraeters. Considering syste inputs and outputs (4 th order syste), and noting that there is now an additional state due to an integrator being included in the feedbac loop of the body pitch variable, atrices A and B are then defined as: (6) A 0 A ( 3, ) A ( 3, 3 ) A ( 3, 4 ) 0 0 A ( 4, ) A ( 4, 3 ) A ( 4, 4 ) (7) B B ( 3 ) B ( 3 ) B ( 4 ) B ( 4 ) 0 0 and the feedbac equation (8) u Kx x (8) The reason why there are two identical feedbac control ISBN: ISSN: (Print); ISSN: (Online) WCE 05

4 Proceedings of the World Congress on Engineering 05 Vol I WCE 05, July - 3, 05, London, U.K. signals is that the control action is divided between two actuators, and due to the syetry, the sae gains are used for the left and right controller. Using the NXTway-GS docuentations, the following Q and R atrices were used []: x (9) Q x R (0) The LQR function in Matlab was then used to deterine specified weights for each of the controller feedbacs. The feedbac controller weightings are listed in Table II. The closed-loop step response is shown in Fig. 4. TABLE II LIST OF GAINS. Variable Gain Value Motor Angle Gyro Angle 48.9 Motor Velocity Gyro Velocity Integral Gain IV. IMPLEMENTATION During the ipleentation of the Equilibrist Robot soe considerations were taen into account regarding sapling frequency definition, controller perforance, tass priority and requisites for servo coand ipleentation. Fig.5. Final control diagra: C ψ is a atrix whose output is the inclination angle of the robot, ψ. K i is the integral gain to iniize the error of the pitch angle and K f is the vector with the LQR gains. The Proportional-Derivative (PD) controller has the ability to predict the tendency of future errors of the syste response. In order to increase the stability of the syste, a PD controller was added (Fig. 6). This iproved the robot s response to disturbances. The proportional constant K p was deterined using a Matlab script, and the derivative constant K d, was found by trial and error (nowing that it ight have a low value because it directly aplifies process noise). Fig.6. PD controller applied to the error fro the first part of the controller (Fig. 5). The controller gains were afterwards adjusted in siulation environent (Table III). TABLE III LIST OF THE CONTROLLER GAINS OBTAINED IN SIMULATION Gain Value Proportional Gain K p Integral gain K Derivative Gain K d Saturation 00 B. Tass priority Fig. 7 presents the tass priority definition for the Equilibrist platfor. At the top of the pyraid is the highest priority tas decreasing to the low level priority at the botto. The highest priority tas is the control routine that regulates the robot s angular position. Fig.4. Closed-loop step response. A. Control Syste The sapling frequency used (which was the sae frequency used for updating the controller) was 00 Hz. The sapling frequency was chosen based on the results fro the LQR siulation shown in the previous chapter. The final control diagra is shown in Fig. 5. Fig.7. Tas priorities: the highest priority is at the top of the pyraid. The ediu priority tass are obstacles avoidance and Bluetooth counication, both executed on regular intervals of 00 s, since they are not essential to ISBN: ISSN: (Print); ISSN: (Online) WCE 05

5 Proceedings of the World Congress on Engineering 05 Vol I WCE 05, July - 3, 05, London, U.K. equilibriu, yet, they are ore iportant than local and reote interface. At the botto of the pyraid are the two final tass where the tie is not critical, so they are executed when there is not any high priority tass to execute. C. Servo control and Reote Interface The Equilibrist platfor allows changing the position of the robot by changing the reference. Also, it is possible to turn the robot by applying a specific coand value to one of the otors. A graphical user interface (Fig. 8) was developed which allows the control of the servo position of the robot. This interface was based on [5] with the addition of arrow buttons, stop button and Turn text box. Several tests were perfored in the Equilibrist platfor: the static test, the disturbance test and the servo coand function. A. Static Test Fig. 0 to Fig. 3 show the state variables profiles for the Equilibrist without disturbances nor reference changing. The coand profile is presented in Fig. 4. Analysing the graphics it is possible to conclude that the controller perfors well as the velocity and body pitch is around zero. Fig.0. Body pitch angle. Fig.. Body pitch angular velocity. Fig.8. The servo position of the Equilibrist can be changed through the blue arrows and stop button or via the two text boxes ( Int to send and Turn ). V. EXPERIMENTAL RESULTS Fig. 9 shows the ipleented robot configuration. If the body is in a positive slope (body leaning forward), the coand value applied to the otor is negative (due to orientation of the otor) and thus goes forward to copensate for the tilt. If the body is with a negative slope (body leaning bacwards), the coand value applied to the otor is positive (due to orientation of the otor) and thus goes bac to copensate for the tilt. Fig.. DC otor angle. Fig.3. DC otor angular velocity. Fig.4. Coand value. Fig.9. Direction of wheels rotation according to body lean. The controller gains were adjusted in the Equilibrist platfor taing into account the real world constraints (Table IV). TABLE IV LIST OF THE CONTROLLER GAINS EXPERIMENTALLY ADJUSTED Gain Value Proportional Gain K p Integral gain K 5.5 Derivative Gain K d Saturation 00 B. Disturbance Test In the disturbance test, an extra ass was placed on the Equilibrist, but the syste anaged to successfully respond and go bac to the equilibriu position (Fig. 5 to Fig. 8). Fig. 9 shows the coand profile ipleented. Fig.5. Body pitch angle ψ. ISBN: ISSN: (Print); ISSN: (Online) WCE 05

6 Proceedings of the World Congress on Engineering 05 Vol I WCE 05, July - 3, 05, London, U.K. Fig.6. Body pitch angular velocity. LQR siulation, the PD controller constants were deterined by Matlab (K p ) and by trial and error (K d ). The syste tass were prioritized in order to enable a good perforance of the whole syste and the experients revealed that the robot perfored well, even when subjected to an external ass disturbance. REFERENCES Fig.7. DC otor angle θ. Fig.8. DC otor angular velocity. Fig.9. Coand value. C. Servo Coand Test The robot reference position was changed through the developed interface under Bluetooth counication. The Equilibrist anage to follow the new reference and aintain equilibriu (Fig. 0). Fig.0. Servo controller: reference variation. [] R. P. M. Chan, K. A. Stol, and C. R. Halyard, Review of odelling and control of two-wheeled robots, Annual Reviews in Control,vol. 37, pp , 03. [] Segway: accessed on March 05. [3] R. Bansevicius, A. Lipnicas, V. Jusa, and V. Raudonis, Piezoelectric Inverted Pendulu as a Teaching Aid for Mechatronics Courses, IEEE International Worshop on Intelligent Data Acquisition and Advanced Coputing Systes: Technology and Applications, -3 Septeber 009, Rende (Cosenza), Italy. [4] G. Figueroa-Flores, A. Leal-Naranjo, C. R. Torres-San Miguel, G. Urriolagoitia-Sosa, and G. Urriolagoitia-Calderón, Ipleentation of an Intelligent Controller into a Robotic Device for Rehabilitation Intended for Lower Extreity Paralysis Patients, Journal of Cheical, Biological and Physical Sciences, special Issue, Section C; 30 Nov Vol. 4, No. 5, pp. 5-7, 04. [5] Looat: accessed on March 05. [6] J. Han, X. Li, and Q. Qin, Design of Two-Wheeled Self-Balancing Robot Based on Sensor Fusion Algorith, Int. J. of Autoation Technology, Vol.8, No., 04. [7] L. Sun and J. Gan, Researching of Two-Wheeled Self-Balancing Robot Base on LQR Cobined with PID, nd International Worshop on Intelligent Systes and Applications (ISA), -3 May, Wuhan, 00. [8] H. Juang and K. Lu, Design and Control of a Two-Wheel Self- Balancing Robot using the Arduino Microcontroller Board, 0th IEEE International Conference on Control and Autoation (ICCA), Hangzhou, China, June -4, 03. [9] J. Hua, Y. Cui, P. Shi, Y. Yang, and H. Li Force Feedbac Assisted Balancing of Inverted Pendulu under Manual Control, Fourth International Conference on Intelligent Control and Inforation Processing (ICICIP), June 9, Beijing, China, 03. [0] W. An and Y. Li, Siulation and Control of a Two-wheeled Selfbalancing Robot, Proceeding of the IEEE International Conference on Robotics and Bioietics (ROBIO), Shenzhen, China, Dec. 03. [] H. Vasudevan, A. M. Dollar, and J.B. Morrell, Design for control of wheeled inverted pendulu platfors, Journal of Mechaniss and Robotics, Noveber 05, Volue 7, Issue 4, 05. [] Y. Yaaoto. NXTway-GS (Self-balancing two Wheeled Robot) Controller Design, NXTway-GS--self-balancing-two-wheeled-robot--controller-design, accessed on Noveber 04. [3] GELway - A self-balancing robot, accessed on Noveber 04 [4] LEGO counication protocol, robot/downloads/detail?nae=appendix%0- LEGO%0MINDSTORMS%0NXT%0Counication%0protoco l.pdf, accessed on January 05 [5] Counicating with LEGO NXT via Bluetooth in C, LEGO-NXT-via-Bluetooth-in-C#, accessed on January 05 VI. CONCLUSION This paper described the design and ipleentation of a self-balancing two-wheeled robot. By using the Linear Quadratic Regulator ethod and PID controller for state feedbac, robot equilibriu was successfully achieved. The atheatical odel used for this robot was siulated in both open and closed loop configurations and used to base the hardware and software ipleentation, the design of the inertial syste, and the paraeters used in control. An interface and the counication protocol were also described. With a sapling frequency of 00 Hz chosen by ISBN: ISSN: (Print); ISSN: (Online) WCE 05