Design, realization and modeling of a two-wheeled mobile pendulum system

Transcription

1 Design, realization and modeling of a two-wheeled mobile pendulum system ÁKOS ODRY 1, ISTVÁN HARMATI, ZOLTÁN KIRÁLY 1, PÉTER ODRY 1 1 Department of Control Engineering and Information Technology, College of Dunaújváros, Táncsics Mihály út 1/A, H-401 Dunaújváros Department of Control Engineering and Information Technology, Budapest University of Technology and Economics, Magyar tudósok krt.., H-1117 Budapest, HUNGARY odrya@mail.duf.hu harmati@iit.bme.hu kiru@mail.duf.hu podry@mail.duf.hu Abstract: - The two-wheeled mobile pendulum system is a special mobile robot having two-wheels, two contact points with the supporting surface and its center of mass is located under the wheel axis. Due to the mechanical structure, the intermediate body tends to oscillate during the translation motion of the robot thus the application of modern control methods is essential in order to stabilize the dynamical system. In this paper, we introduce the mechatronics construction of the robot and derive its corresponding nonlinear mathematical model as well. Key-Words: - mobile robot, mathematical modelling, self-balancing robot, future transportation system 1 Introduction Wheeled mobile robots have attracted a great deal of attention in research and in industry as well due to the simple mechanical construction, cost effective production and the big variety of potential application prospects in many areas [1]. Most commonly the two-wheeled systems are characterized by either two or three contact points with the supporting surface. Three contact point configurations consist of two actuated wheels, an inner body that carries the electronics, and an additional wheel or supporting point which ensures the mechanical stabilization. The most popular twowheeled robots with two contact points are the so called self-balancing robots which balance their inner body during the translation motion []. In our project, a two-wheeled mobile pendulum system has been developed, which has also two contact points with the supporting surface, however the diameter of its inner body is smaller than diameter of the encompassing wheels resulting two equilibrium points of the system. Around the stable equilibrium point (the center of mass is located below the wheel axis) the inner body of the robot tends to oscillate when the wheels are actuated, while around the unstable equilibrium point (the center of mass is located above the wheel axis) the robot self-balances its inner body while performing translation motion. For video demonstration see the website Fig. 1. Photograph of the fabricated robot around its stable equilibrium point. Similar construction was built at the McGill University s Centre for Intelligent Machines (the prototype was named Quasimoro) [3]. It has been proven that the two contact point construction is characterized by the so called quasiholonomic property that eases the control of nonholonomic systems since the dynamical model becomes much simpler than otherwise [4]. Another corresponding two contact point construction is the electric diwheel built by the School of Mechanical Engineering at the University of Adelaide [5]. Research goals Our research goal is to investigate critical balance tasks through the design, optimization and validation of modern control methods. Thus in this paper the structure and mathematical modeling of a ISBN:

2 two-wheeled mobile pendulum system is reported that contributes and forms the basis of our research analysis. Since the fabricated mechatronic system is equipped with different sensors that measure its dynamics the implementation and validation of the theoretically proven control performances can be performed which results are left for another publication. On the other hand it is worth to mention that the wheeled mobile pendulum we developed fits into the nowadays development tendency, that the mechanical disadvantages or anomalies of a complex dynamical system are compensated with modern control solutions, since the technological evolution enables the application of cheap, high performance embedded systems. In the first part of the paper, the mechanical and electrical structure of the fabricated robot is described. While in the second part the nonlinear mathematical model of the plant is derived. Finally, simulation and measurement results are given which prove that the derived mathematical model enables the efficient design of control algorithms. Fig.. Photograph of the fabricated robot around its unstable equilibrium point. The dimensions (length, height, width) of the steel intermediate body are mm, which is encompassed by 6 mm diameter wheels. The embedded electronic parts are placed around-, while the DC motors that drive the wheels are attached to the chassis. On Fig. 3 the Solidworks CAD model is depicted which was used in the calculation of the inertia related parameters (the side and top printed circuit boards has been set invisible in order to indicate the inner parts). 3 Mechanical structure of the robot The mechanical structure consists of two wheels and a steel inner body (chassis) that forms a pendulum. The wheels are actuated through DC motors attached to the inner body. As it can be seen on the following figures, the diameter of the inner body is smaller than the diameter of the wheels, thus the robot has only two contact points with the supporting surface. Due to the mechanical structure, the inner body acts as a pendulum between the stator and robot of the applied DC motors during the translation motion of the robot. Since the location of the center of mass can be under and above the wheel axis, two equilibrium points can be distinguished. Namely, the robot is operated around its stable equilibrium point (see Fig. 1), when the center of mass is located under the wheel axis. Around this state, the translation motion of the robot is influenced by damped oscillation of its inner body. While the robot is staying around its unstable equilibrium point (see Fig. ), when the center of mass is stabilized above the wheel axis. Around the unstable equilibrium point, the robot simultaneously performs translation motion and balances its inner body which act as an inverted pendulum. It should be emphasized, that regardless the chosen equilibrium point, the translation motion of the robot exclusively in closed loop, with the application of control algorithms can be resolved. Fig. 3. Solidworks CAD model of the robot. Inner parts: (1) side PCB, () DC motor, (3) battery, (4) chassis, (5) bearing, (6) bottom PCB. 4 Embedded electronics The hardware construction is built around two 16- bit ultra-low-power Texas Instruments MSP430 F618 microcontrollers (hereinafter MCU1 and MCU). We use low cost MEMS accelerometer (type: LIS331DL) and gyroscope (type: L3G400D) sensors from STMicroelectronics to measure the dynamics of the inner body of the robot, and additionally current sensors (type: INA198) and two-channel incremental encoders (type: PA-100) are attached to both DC motors. The actuators are 3V geared DC micromotors (type: 104N003S) manufactured by Faulhaber. The motors are driven with PWM signals through Texas Instruments ISBN:

3 DRV59 drivers. The electronic system is supplied from stabilized 3.3V, the source is a 1 cell Li-Po battery. Fig. 4 shows the embedded electronic configuration. MCU works as an inertial measurement unit (IMU): it i.) collects the measurements from the accelerometer and gyroscope through SPI peripheral, ii.) performs the Kalman estimation of the inclination angle of the inner body, and iii.) sends the results to MCU1 through UART interface. MCU1 performs basically the control tasks. On the one hand it collects the measurements (from incremental encoders, current sensors and from MCU the angles and the angular velocity). On the other hand it drives the motors based on the applied control algorithm. MCU1 also sends the measurements to the PC through a Bluetooth module. A 16 MHz quartz oscillator is used as the system clock. Fig. 4. The hardware architecture of the robot. 5 Mathematical model To be able to efficiently design the control algorithms of the system, its mathematical model has to be obtained first. Most of the electrical and mechanical parameters that characterize the robot (like wheel radius, inertia matrix, resistance of the motors) are quite accurately known from direct measurements, datasheets or from calculations performed by Solidworks, the rest of the (mainly friction related) parameters were experimentally tuned based on the measurements results. Based on Fig. 5 the (geometric) variables can be introduced. We indicate with θ 1 and θ the angular displacements of the wheels, while with θ 3 the inclination angle of the pendulum (inner body). The parameters that characterize the robot are summarized in the appendix. We will also use the notations: θ as the mean value of θ 1and θ, ψ as the change of yaw angle of the robot, and s as the linear speed of the robot, i.e. θ = (θ 1 + θ )/, ψ = r(θ θ 1)/d, and s = rθ. Fig. 5. Plane and side view of the robot and its spatial coordinates. By the help of Fig. 5, the spatial coordinates of the wheels and the inner body are determined. Namely, the coordinates of the intersection of axes A and B are: x m = s cos ψ dt, y m = s sin ψ dt, z m = r. (1) Similarly, the spatial coordinates of the wheels and the inner body are given by equations () and (3) respectively: x 1 = x m 1 d sin ψ, y 1 = y m + 1 d cos ψ, x = x m + 1 d sin ψ, y z 1 = z m, z = z m. = y m 1 d cos ψ, x b = x m + l sin θ 3 cos ψ, y b = y m + l sin θ 3 sin ψ, z b = z m l cos θ 3. () (3) The motion of the system was determined by the help of the Lagrange equations [6]: d L dt q i L q i = τ i, i = 1,, N, (4) where q = (θ 1, θ, θ 3 ) T was chosen for the vector of generalized coordinates. L defines the Lagrange function, which is given as the difference of the kinetic and potential energy, i.e. L = K P. The total kinetic energy K consists of the sum of the kinetic energies that can be written for the wheels and the kinetic energy that can be written for the ISBN:

4 inner body, K = K w + K b. The kinetic energy of the wheels composed of the translational and rotational energy of the wheels: K w = 1 m w(x i + y i + z i ) translation + 1 J wθ i + 1 k J r (θ i θ 3). rotation motor (5) where J w = m w r / denotes the moment of inertia of the wheels. The kinetic energy of the pendulum consist of the energies resulting from the translational motion of the robot, oscillation of the inner body about the A axis, and the rotation about the B axis as well: K b = 1 (m b(x b + y b + z b ) translation + J A θ 3 + J B ψ ). (6) rotation The P potential energy stored in the system is: P = m w gr + m w gr + m b g(r l cos θ 3 ), (7) where g denotes the gravitational acceleration. Based on equations (5), (6), and (7) the Lagrange function of the system is derived. We denoted with τ = (τ 1, τ, τ 3 ) T the vector of generalized external forces in equation (4). The generalized external forces consist of the external torques (that are produced by the motors) and the effect of friction that is modeled in the system, τ = τ a τ f. The external torques are described by equation (8) and (9), where the input voltage and current of the motors are denoted with u = (u 1, u ) T and I = (I 1, I ) T respectively. The relationship between the currents and input voltages is given by the differential equation: I = 1 L (u k Ek [ ] q RI). (8) Furthermore, the external torques are proportional with the currents by the factor k M k, thus we get 1 0 τ a = k M k [ 0 1 ] I. (9) 1 1 We assumed a friction model that consists of only viscous frictions. Namely, viscous friction was modelled at the bearings and between the wheels and the supporting surface, i.e.: b 1 + f v1 0 b 1 τ f = [ 0 b + f v b ] q. (10) b 1 b b 1 + b By evaluating (4) the equations of motion of the mechanical system can be rewritten in the form: M(q)q + V(q, q ) = τ a τ f, (11) where M(q) denotes the 3-by 3 symmetric and positive definite inertia matrix, V(q, q ) denotes the 3-dimensional vector term including the Coriolis and centrifugal force terms and also the potential (gravity) force term. The exact elements of the matrices are described in the appendix. Based on equation (11) the state-space representation of the plant is obtained. With the state vector x = (q, q, I) T the state-space equation: M(q) x (t) = 1 (τ a τ f V(q, q )), 1 [ L (u k Ek [ ] q RI) ] q y(t) = x(t). (1) 6 Simulation and experimental results The numerical simulation of the mathematical model was performed in MATLAB Simulink environment. The state space representation defined by equation (1) was implemented by the help of the S-Function Simulink block. Fig. 6. The resulting average current and average angular speed of the motors. Since the fabricated robot is equipped with sensors, measurements of the open-loop behavior were recorded in order to compare the simulation and measurement results, and to verify the derived mathematical model. The comparison is depicted on Fig. 6 and Fig. 7. ISBN:

5 In the experiment, unit-step excitation of u 1 = u = 1.3 V was applied to both DC motors, and the average angular velocity of the rotors θ rot, the angle and the angular velocity of the inner body (θ 3 and θ 3 respectively), and the average of the motor currents I avg = (I 1 + I )/ were recorded during the translation motion of the robot. Based on the results, it can be concluded that the theoretically derived mathematical model with the nominal robot parameters fairly describe the real behavior of the system. m 11 = 3 m wr m br + k J r + l r d m b sin θ 3 + J B r m = m 11, m 33 = m b l + J A + k J r, m 1 = m 1 = 1 4 m br l r d m b sin θ 3 J B r d, m 13 = m 3 = m 31 = m 3 = 1 m blr cos θ 3 k J r. d, v 1 = l r d m b sin θ 3 cos θ 3 θ 3(θ 1 θ ) 1 m blr sin θ 3 θ 3, v = l r d m b sin θ 3 cos θ 3 θ 3(θ θ 1) 1 m blr sin θ 3 θ 3, v 3 = l r d m b sin θ 3 cos θ 3 (θ 1 θ ) + m b gl sin θ 3. (A) (B) Fig. 7. The resulting oscillation angle and angular velocity of the inner body. Notation of the robot parameters: Symbol Name Value [SI Unit] r, m w Wheel radius and mass l Distance between the COG and the wheel axle m b Mass of the inner body d Distance between the wheels J A Moment of inertia of the inner body about the wheel axle A J B Moment of inertia of the inner body about the axis B k, J r Gear ratio and rotor inertia 64; R, L Rotor resistance, inductance.3; k M, k E Torque, Back-EMF constants b Viscous friction coefficient between body - motor f v Viscous friction coefficient between wheels - ground Conclusion In this paper a wheeled mobile pendulum system was introduced and its nonlinear mathematical model was derived. It was showed that the proposed mathematical model well describes the real behavior of the dynamical system, thus it provides the basis to effectively design control algorithms. Future work will involve the identification of the unknown parameters, and the development and validation of modern control methods. Acknowledgment This publication is supported by the TÁMOP 4...D-15/1/KONV project. The project is cofinanced by the European Union and the European Social Fund. Appendix The elements of the inertia matrix M(q) = (m ij ) 3 3 and the vector V(q, q ) = (v 1, v, v 3 ) T : References: [1] L. Sciavicco and B. Siciliano, Modelling and Control of Robot Manipulators, Springer- Verlag London, 000. [] F. Grasser, A. D'Arrigo, S. Colombi and A. Rufer, JOE: a mobile, inverted pendulum, IEEE Transactions on Industrial Electronics, vol. 49, no. 1, pp , 00. [3] A. Salerno and J. Angeles, A New Family of Two-Wheeled Mobile Robots: Modeling and Controllability, IEEE Transactions on Robotics, vol. 3, no. 1, pp , 007. [4] A. Salerno and J. Angeles, On the nonlinear controllability of a quasiholonomic mobile robot, IEEE International Conference on Robotics and Automation ICRA'03, vol. 3, pp , 003. [5] B. Cazzolato, J. Harvey, C. Dyer, K. Fulton, E. Schumann, T. Zhu, Z. Prime, B. Davis, S. Hart, E. Pearce and J. Atterton, Modeling, simulation and control of an electric diwheel, Australasian Conference on Robotics and Automation, pp. 1-10, 011. [6] A. M. Bloch, Nonholonomic Mechanics and Control, Springer, 003. ISBN: