On modelling and control design for self-Balanced two-wheel vehicle

Vietnam Journal of Mechanics, VAST, Vol. 30, No. 3 (2008), pp. 158 – 166 ON MODELLING AND CONTROL DESIGN FOR SELF-BALANCED TWO-WHEEL VEHICLE Nguyen Quang Hoang Department of Applied Mechanics, Hanoi University of Technology Abstract. In this paper, the modeling and control design of a self-balancing mobile robot are presented. The method of sub-structures is employed to derive the differential equations of motion of the robot. Based on the linearized equations of motion, a controller is designed to maintain a stable motion of the robot. Some numerical simulation results are shown to clarify the designed controller. 1. INTRODUCTION A two-wheel vehicle with a type of inverted pendulum has a commercial name, so- called Segway, which was invented by an American named Dean Kamen. This vehicle has been investigated and designed for 10 years ago, and was introduced to public since 2001. Nowadays, thousands of such vehicles were sold in the USA and Europe. It is convenient to use this vehicle to travel within 10 km distance. These vehicles can reach a velocity of 20 km/h. Main mechanical structures of the vehicle consist of a bodywork and two wheels those two axes coincides. Two wheels are driven by two independent electric motors. Rotors are fixed with the wheels, while stators with the bodywork, or in inverse order. Power supports for the vehicle is an accumulator on the bodywork. In order to maintain a stationary motion of the vehicle – bodywork is directed above -, a controller is necessary. In addition, there is a screen to display motion states such as velocity, some bottoms to set motion parameters. In recent years, several authors have investigated this new vehicle type [2-5]. In this paper, a dynamic model of the vehicle moving on a straight line and the designing of a controller based on linearized model are presented. Based on differential equations of motion obtained by the method of substructures, a controller to ensure the stable motion of the vehicle is designed. Finally, some numerical simulations are given to verify the reliability of the control law. 2. DIFFERENTIAL EQUATION OF MOTION OF VEHICLE Mechanically, this vehicle consists of three major members: a bodywork and two inde- pendent wheels. In case the vehicle moving on a straight line – the motion of two wheels is identical - we can combine two wheels together. So the vehicle is understood as a system with two bodies in a vertical plane: a bodywork and a wheel connecting with each other by a hinge (Fig. 1). In order to establish the differential equations of motion of the vehicle, the substructure method is exploited in this study. After resolving the vehicle into two substructures: the bodywork and the wheel, the Newton-Euler equations [1, 6] are applied to planar motion of bodies. On modelling and control design for self-balanced two-wheel vehicle 159 - The wheel together with the rotor: (m1 +mro)x¨C1 = F −X0 (m1 +mro)y¨C1 = N − (m1 +mro)g − Y0 (J1 + Jro)ϕ¨1 = τ − Fr −Ml (1) - The bodywork: m2x¨C2 = X0 m2y¨C2 = Y0 − (m2 +msta)g J2ϕ¨2 = −τ −X0(yC2 − yO) + Y0(xC2 − xO) (2) where m1 is the mass of the wheel, r – wheel radius, mro – rotor mass, J1 and Jro are the mass inertias of the wheel and robot, respectively, Ml – damping moment at contact area between the wheel and a floor. The interactive moment between the rotor and the stator τ is created by an electric motor that is related to input current or voltage, and damping moment at the journal; m2 – the mass of the bodywork, J2 – mass inertia of the bodywork with respect to the axis past through the center of mass C2. Fig. 1. Vehicle model and two substructures From Fig. 1. a) and b) with an assumption that the wheel rolls without slipping we have some kinematic relations as following: xC1 = s yC1 = r ϕ1 = s/r x¨C1 = s¨ y¨C1 = 0 ϕ¨1 = s¨/r xC2 = s + u2x sin θ − u2y cos θ yC2 = r+ u2x cos θ + u2y sin θ ϕ2 = θ (3) x¨C2 = s¨ + (u2x cos θ + u2y sin θ)θ¨ + (−u2x sin θ + u2y cos θ)θ˙ 2 y¨C2 = (−u2x sin θ + u2y cos θ)θ¨ − (u2x cos θ + u2y sin θ)θ˙ 2 ϕ¨2 = θ¨ (4) In order to determine the moment acting between the wheel and the bodywork, a model of a permanentmagnet DC motor is considered (Fig. 2). 160 Nguyen Quang Hoang Fig. 2. Diagram of a DC motor A torque of the motor is proportional to current passing the armature M = Kmi. (5) A voltage Vemf is generated across its terminals that is proportional to the angular velocity of rotor is called back emf Vemf = Keθ˙. (6) By applying the theorem of momentum and Kirchoff law, the differential equation describ- ing dynamics of DC motor is obtained as follows: - mechanical equation Jmω˙ + bω = Kmi− Tload, (7) - electrical equation L di dt + Ri = V −Keω. (8) Frequently, it is assumed that the “electric time constant” R/L is much smaller than the “mechanical time constant” Jm/b. This is a reasonable assumption for many electro- mechnical systems and leads to a reduced order model of the actuator dynamics. From equation (8) with L/R ≈ 0 we have i = (V −Keθ˙)/R. (9) Substituting (9) into (7) one obtains the equation presenting the relation between input voltage V and motion of rotor θ˙ Jmθ¨ + (b+KmKe/R)θ˙ = V/R− Tload, (10) and torque at the motor journal M = Kmi = Km(V −Keθ˙)/R = (Km/R)V − (KmKe/R)θ˙. (11) Torque interacting between the bodywork and the wheel is a summation of the motor torque and friction torque at the journal. In this paper, it is assumed that a friction torque is proportional to the relative angular velocity of the bodywork and the wheel. So we have τ = Mdc +Mdamp = Kmi− bω = (Km/R)V − (KmKe/R+ b)(s˙/r − θ˙) (12) with ω = ϕ˙1 − ϕ˙2 = s˙/r − θ˙. In all above equations the following quantities have been used: V – applied voltage of the motor, R - armature resistance, L - armature inductance, Jm - mass inertia of the rotor, Vemf - back emf voltage, b - damping coefficient, M – torque due to the current, ω On modelling and control design for self-balanced two-wheel vehicle 161 - relative angular velocity of the rotor respect to the stator, Km - torque constant of the motor, Ke – back emf constant, b1 - roll damping constant, Ml = b1ω1. Eliminating the constraint forces X0, Y0, and F from equation (1) and (2) one obtains the differential equations of motion of the vehicle in matrix form as M(q)q¨+ h(q, q˙) = LV, (13) with q = [s, θ]T is a generalied coordinate vector of the vehicle. To simplify the model one takes u2y = 0, in this case the mass matrixM(q), vector h and matrix L are obtained as: M(q) =   J1 + Jro r2 + (m1 +mro +m2) m2u2x cos θ J1 + Jro r + r(m1 +mro +m2) +m2u2x cos θ J2 +m2u 2 2x + rm2u2x cos θ   , (14) h(q, q˙) = [ −m2u2x sin θθ˙ 2 + KmKe Rr (s˙/r − θ˙) + b/r(s˙/r − θ˙) + b1s˙/r 2 b1s˙/r − rm2u2x sin θθ˙ 2 −m2gu2x sin θ ] , (15) L0 =   Km/Rr 0   . (16) Now, we will examine whether there exists a motion state with constant velocity and the bodywork direct above, i.e: θ˙ = 0, s˙ = vd = const, θ = const ≈ 0. (17) Substituting (17) into (13) one obtains a system of algebraic equations:[ KmKe+R(b+b1) Rr2 vd b1vd/r−m2gu2x sin θ0 ] = [ Km Rr 0 ] u0. (18) Solving the algebraic equations (18) one can determine a slope of the bodywork and an applied voltage corresponding to the constant velocity of the vehicle, vd = const θ0 = f(vd) = arcsin b1vd m2gu2xr , u0 = g(vd) = KmKe +R(b+ b1) Kmr vd (19) Linearilizing the differential equations of motion (13) about a desired motion (17), one obtains differential equations of disturbed motion. The equations of motion after linearilizing have a form as follows M0δq¨+D0δq˙+K0δq+ h(q0, q˙0) = L(u0 + δu), (20) with δq = [δs δθ]T is a vector containing disturbed motion, h0(q0, q˙0) = [ KmKe+R(b+b1) Rr2 s˙ b1s˙/r −m2gu2x sin θ0 ] = Lu0, M0 = [ m11 m12 m21 m22 ] , m11 = (J1 + Jro)/r 2 + (m1 +mro +m2) m12 = m2u2x cos θ0 m21 = 1 r (J1 + Jro) + (m1 +mro +m2)r +m2u2x cos θ0 m22 = J2 +m2(u 2 2x + ru2x cos θ0) 162 Nguyen Quang Hoang D0 = [ KmKe+R(b+b1) Rr2 − KmKe+bR Rr b1 r 0 ] , K0 = [ 0 0 0 −m2gu2x cos θ0 ] , L = [ Km/Rr 0 ] . Putting x = [δq δq˙]T , equation (20) can be transformed in the state equation x˙ = Ax+Bδu, (21) where two matrices A and B are given as following A = [ 0 E2×2 −M−10 K0 −M −1 0 D0 ] ,B = [ 0 −M−10 L ] . (22) If a solution x(t) = 0 of equation (21) is stable, the motion (17) will be stable. It is clear that the equilibrium (θ = θ0 ≈ 0) of inverted pendulum without control is always instable. So we can conclude that the desired motion (17) is instable. The problem of stabilization of the vehicle will be presented in the following section. 3. CONTROL DESIGN The objectives of a control problem is to find a law of input voltage of the motor to ensure a given desired motion s = sd(t) while the bodywork is directed above, θ = const ≈ 0. A feedback control needs the actual state of the vehicle, so sensors to collect actual motion x(t), x˙(t), θ(t), θ˙(t) of the vehicle are necessary. There are many methods to design a controller for a linear system (21), in this study a linear feedback control will be applied to stabilize the motion of the vehicle. The diagram of this control law is given in Fig. 3. The control voltage u = u0 + δu depends on desired and actual motion of the vehicle. The nominal voltage u0 = u(v0) coresponding to a constant velocity v0 is determined from equation (19). The stabilization of desired motion is ensured by the voltage δu, which is proportional to state vector x with a gain matrix K δu = −Kx = −[k1 k2 k3 k4]x. (23) Substituting (23) into (21) one yields x˙ = Ax−BKx = (A−BK)x. (24) The elements of gain matrix K are chosen so that matrix owns negative eigenvalues. Firstly, we can choose eigenvalues of the matrix , and then determine the gain matrix K. Let p be a vector containing eigenvalues of , one can obtain the gain matrix K by mean of Matlab. On modelling and control design for self-balanced two-wheel vehicle 163 Fig. 3. Block-diagram of velocity control Table 1. Vehicle’s parameters Parameters of bodywork and wheel Notation value unit Mass of wheel m1 0.51 kg Mass inertia of wheel J1 5.1 x 10-4 kgm2 Radius of wheel r 0.062 m Damping coefficient floor - wheel b1 5.73 x 10-3 Nm/(rad/s) Mass of bodywork m2 9.01 kg Mass inertia of bodywork J2 0.228 kgm2 Center of mass u2x 0.138 m Gravitational acceleration g 9.81 m/s2 Parameters of DC motor Mass inertia of rotor Jro 3.2 x 10-6 kgm2 Damping coefficient journal - bodywork b 4.25 x 10-3 Nm/(rad/s) Armature resistance R 1 Ω Moment constant Km 1 Nm/A back emf constant Ke 1 Vs 4. NUMERICAL SIMULATION In oder to simulate by mean of Matlab, the parameters of the vehicle from [2] are used (Table 1). When the vehicle has a speed of 5 m/s (18 km/h), from equation (19) the orientation of the bodywork is calculated: θ0 = 0.0379 rad ( = 2.17 0), Because the angle θ0 is small, instead of studying stability of the system at θ0 we can consider the stability at the vertical position of the bodywork, θ0 = 0. Using parameters in table 1, two matrices A and B are given as: 164 Nguyen Quang Hoang A =   0 0 1 0 0 0 0 1 0 −6.6997 −55.2757 3.4108 0 51.3728 212.5356 −13.1264   , B =   0 0 −3.3963 13.0709   (25) Eigenvalues of the matrixA are [0, -68.8401, -4.3213, 4.7592], therefore at the position where the bodywork is directed towards above, the vehicle without control is instable, one eigenvalue of matrix A is positive. Controlling the vehicle can be divided into two problems: position control and velocity control. In position control, the vehicle need to be kept at a certain position (s, 0). To do this we have to choose all four eigenvalues of Ac to be negative. For example, here we choose p = [-2 -3 -5 -6]T and a gain matrix K is obtained by mean of Matlab as follows K = [-2.0711 -11.3372 -18.6895 -0.8472]. The simulation results with above gain matrix K are shown in Fig. 4. The vehicle reaches the desired position after about 4 s. Fig. 4. Position control with different initial conditions In velocity control, the vehicle need to be kept at a constant velocity, while its orien- tation is small. To do this we set the first eigenvalue of Ac to be zero and other three to be negative. For example, here we choose p = [0 − 3 − 4 − 3.5]T and a gain matrix K is obtained by mean of Matlab as follows K = [0 − 6.6772 − 16.7229 0.0843]. On modelling and control design for self-balanced two-wheel vehicle 165 Fig. 5. Results of velocity control The simulation results are shown in Fig. 5. After about 3 seconds the vehicle reaches the desired velocity. 5. CONSLUSION In this study, deriving the differential equations of motion of self-balanced two-wheel vehicle and designing a controller to maintain vehicle motion are presented. The method of substructures has been exploited successfully in establishing equations of motion of the vehicle. Because the system has two degrees of freedom but only one actuator, a consideration of controllability become necessary. The disturbed motion is examined by using linearilzed equations, so it is not difficult to choose parameters of the feedback controller by mean of universal softwareMatlab. Some other problems should be considered further such as the uncertainties of vehicle parameters. The contact between the wheel and the surface of the road should be modeled in more details. ACKNOWLEDGMENT This paper was completed with the financial support of National Foundation for Sci- ence and Technology Development (NAFOSTED). REFERENCES 1. Nguyen Van Khang, Dynamics of Multibody Systems, Technical and Scientific Publisher, Hanoi 2007 (in Vietnamese). 2. Yun-Su Ha, Shinichi Yuta, Trajectory tracking control for navigation of the inverse pendulum type self-contained mobile robot, Robotics and Autonomous Systems 17 (1996) 65-80. 3. Kaustubh Pathak, Jaume Franch, Sunil K. Agrawal, Velocity Control of a Wheeled Inverted Pendulum by Partial Feedback Linearization, 43rd IEEE Conference on Decision and Control, December, 2004, Atlantis, Paradise Island, Bahamas. 4. Felix Grasser, Aldo D’Arrigo, Silvio Colombi,and Alfred C. Rufer. “JOE: A Mobile, Inverted Pendulum”. Ieee Transactions on Industrial Electronics 49 (1) (2002) 107-114. 166 Nguyen Quang Hoang 5. Michael Baloh and Michael Parent, Modeling and Model Verification of an Intelligent Self- Balancing Two-Wheeled Vehicle for an Autonomous Urban Transportation System, The Con- ference on Computational Intelligence, Robotics, and Autonomous Systems, Dec. 15 2003, Singapore. 6. W. Schiehlen, and P. Eberhard, Technische Dynamik: Modelle fur Regelung und Simulation. Teubner Verlag, Wiesbaden 2004. Received September 25, 2008 VỀ VIỆC MÔ HÌNH HÓA VÀ ĐIỀU KHIỂN XE HAI BÁNH KIỂU CON LẮC NGƯỢC TỰ CÂN BẰNG Trong bài báo này, bài toán mô hình hóa và thiết kế điều khiển cho rôbốt di động kiểu con lắc ngược được trình bày. Phương pháp tách cấu trúc được sử dụng để thiết lập phương trình vi phân chuyển động của rôbốt. Phương trình chuyển động sau khi tuyến tính hóa là cơ sở để thiết kế bộ điều khiển phản hồi cho rôbốt. Các kết quả mô phỏng số chỉ ra tính hiệu quả của bộ điều khiển.