Motion Control of Inverted Pendulum Robots Using a Kalman Filter Based Disturbance Observer
Total Page：16
File Type：pdf, Size：1020Kb
SICE Journal of Control, Measurement, and System Integration, Vol. 2, No. 1, pp. 050–055, January 2009 Motion Control of Inverted Pendulum Robots Using a Kalman Filter Based Disturbance Observer ∗ ∗ Akira SHIMADA and Chaisamorn YONGYAI Abstract : A high-speed motion control technique for inverted pendulum robots, utilizing instability and a disturbance observer, based on the Kalman filtering technique, is introduced. Inverted pendulums are basically controlled as they do not topple. Shimada and Hatakeyama developed a contrary idea and presented a controller that deliberately off balanced the robot when it moved. To implement the idea, a controller was designed using zero dynamics, which was derived by partial feedback linearization. However, the control system was not robust or sufficiently reliable. Although they presented a revised method using H∞ control law, it was complex. Shimada et al. also presented a design method for a disturbance observer using Kalman filtering. This paper presents the latest control technique, combining both control laws to solve the problem, and introduces an application with respect to inverted pendulum robots. It further shows experimental results to confirm its validity. Key Words : inverted pendulum, robot, disturbance observer, Kalman filter, motion control. 1. Introduction ing very fast, but such rapid moves cannot be realized with- This paper introduces a high-speed motion control technique out tilting. This paper discusses only straight motion control, for inverted pendulum robots, based on the concept of instabil- however it introduces a tilt angle control technique for imple- ity and a disturbance observer based on Kalman filtering tech- menting high speed motion. In the future, we expect to see in- niques. It is well known that an inverted pendulum is a self- novative mobile robots being developed using this new control regulated system with a to-and-fro motion similar to the motion technique based on the concept of instability. of a child swinging an umbrella or a stick [1]–[5]. However, The first and second control systems presented by Shimada designing a control system for various pendulums has been a and Hatakeyama, which were based on instability, were not ro- ffi challenge since the 1970s. Later, machines based on the same bust or su ciently reliable [8],[9]. The details are as follows. principles were developed for human riding [6],[7], and many Their first paper [8] presented the basic control scheme based biped walking robot designs are also based on this principle. on the instability which achieved the high-speed straight mo- Those inverted pendulums are automatically controlled, as they tion. However, it needed two control modes-stable and un- do not continuously toppled. stable. Although the unstable control mode implemented the Shimada and Hatakeyama presented a contrary theory [8]– tilt angle control function, it often caused positioning errors. [10], using a controller that deliberately unbalanced a robot in Therefore, the stable control mode, based on a conventional motion. To implement this concept, a controller was designed servo control technique, was used to fix the error problem. The using partial feedback linearization [11],[12], which controls second paper [9] introduced a control scheme based on a 3D only the tilt and orientation angles of the robot. The robot’s mathematical model. This scheme achieved straight motion and position is controlled indirectly, rather than directly, although swerve and pivoting motions as well. However, the positioning the orientation is directly controlled. In machines designed for error problem remained. To solve this problem, a revised con- humans to ride, which are based on the principle of instability, troller, using the H∞ control law, was presented, but its structure the unstable states are acquired from operators, not controllers. was too complex [10]. However, these robots generate unstable states automatically. The authors and their colleague [14] presented a design Applications using the same concept existed [13], but the idea method for a disturbance observer using a Kalman filtering of [8]–[10] was unique, since it was based on the concept of the technique. It is well known that disturbance observer based robots having zero dynamics. The proposed unstable motion is similar to the sprinting of speed skaters or the motion of a rocket when it starts. Fig- ure 1 illustrates the robot motion with a picture of imaginary robots moving independently. While in motion, the robot tilts, swerves, and makes K-turns. Moreover, it seems to be mov- ∗ Department of Electrical System Engineering, Polytechnic Uni- versity, 4–1–1 Hashimotodai, Sagamihara-shi, Kanagawa 229– 1196, Japan E-mail: [email protected] (Received October 21, 2008) (Revised November 25, 2008) Fig. 1 Motion image of inverted pendulum robot. JCMSI 0001/09/0201–0050 c 2008 SICE SICE JCMSI, Vol. 2, No. 1, January 2009 51 Fig. 2 Exterior view of the robot. Fig. 3 Variables and coordinate frames. control technology can provide high control performance for at z from ΣB. Furthermore, the wheel angles are defined as θw. a variety of mechanical systems. However, we sometimes Normally, the wheel angles should be expressed as θrw and θlw encounter involuntary events, as conventional disturbance ob- respectively, since the robot has two wheels. However, since server based controllers often cause noisy or unstable motion. this paper treats only straight motion. The relation θrw = θlw is One reason is quantization error caused by the low resolution of assumed and the angles are expressed as θw. sensors mounted on the joints of mechanical systems that can The equations of motion of the inverted pendulum robot are inadvertently estimate real disturbance as observation noise. To derived as reconsider the disturbance observer based control technique, 2 J2θ¨w + J3θ¨b cos θb − J3θ˙b sin θb = τs (1) this paper introduces a design method based on a steady state θ¨ θ + θ¨ − / θ = −τ Kalman filter design and applies it to inverted pendulum robots. J3 w cos b J1 b J3g R sin b s (2) The presented control technique is useful for fixing the error = 4 2 + 2 = 2 + + , = where J1 mb( 3 z zb), and J2 R (mb mw) Iw J3 mbzR. problem simply and to make the robot robust against distur- Further, mb, mw refer to the mass of the body and the wheel. It bances. is assumed that z is half of the body height, the width is 2zb, and the radius of the wheels is R. θw is the wheel angle. θb is 2. Inverted Pendulum Robot the tilt angle of the body, and τs is the driving torque. Using the Figure 2 shows the exterior view of an inverted pendulum equation of motion, the nonlinear state equation is derived as ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ robot. It is a wheeled inverted pendulum and consists of a body, θ˙ θ˙ ⎢ w ⎥ ⎢ w ⎥ ⎢ 0 ⎥ a pair of wheels, and a contact terminal to detect the body tilt ⎢ θ˙ ⎥ ⎢ θ˙ ⎥ ⎢ ⎥ ⎢ b ⎥ = ⎢ b ⎥ + ⎢ 0 ⎥ τ angle. The body includes a pair of DC servo motors with rotary ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ s (3) ⎢ θ¨w ⎥ ⎢ f3 ⎥ ⎢ g3 ⎥ encoders and gear boxes. The base block of the contact termi- ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ θ¨ f g nal is set at the center of the body. The robot body has two b 4 4 θ = − 2 2 θ , small arms with free rollers connected at the terminals of the where Det( b) J1 J2 J3 cos b J1 2 J3 cos θb arms. During the initial stages of development, the robot con- f3 = (J3 sin θb · θ˙ ) − (J3g/R · sin θb), Det(θb) b Det(θb) −J3 cos θb 2 J2 troller and power amplifier are set outside the robot for conve- f4 = (J3 sin θb · θ˙ ) + (J3g/R · sin θb), Det(θb) b Det(θb) + θ − θ − nience and are connected to the motors and sensors via electri- J1 J3 cos b J3 cos b J2 g3 = θ , g4 = θ . cal wires. Generally, a two-wheeled pendulum cannot move in Det( b) Det( b) a direction inline with the drive shaft without slipping—a prop- 4. Kalman Filter Based Identity Disturbance Observer erty referred to as velocity constraint. A K-turn motion turns 4.1 Identity Disturbance Observer [15] the robot. This is a typical problem associated with nonlinear Now, using the conventional linearization of Eqs. (1) and (2), control techniques for non-holonomic systems. This robot is the continuous state and output equations are derived as a type of inverted pendulum and does not have an actuator to x˙ = A x˙ + B u − B d (4) regulate the tilt angle of the body directly: therefore, it can be s c s c s s = considered an under-actuated system. the robot has two types of ys Cs xs (5) T T non-linear characteristics: however, this paper deals with only where xs = [θω,θb, θ˙ω, θ˙b] , us = τs, ys = [θω,θb] ,andd straight motion control of the inverted pendulum and does not means equivalent input disturbance. discuss curved motion and K-turn control. ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ 0010⎥ ⎢ 0 ⎥ ⎢ ⎥ ⎢ ⎥ 3. Coordinate Frames and Modeling of the Inverted ⎢ 0001⎥ ⎢ 0 ⎥ ⎢ 2 ⎥ ⎢ ⎥ A = ⎢ J g ⎥ , B = ⎢ J1+J3 ⎥ Pendulum Robot s ⎢ − 3 ⎥ s ⎢ ⎥ ⎢ 0 Det·R 00⎥ ⎢ Det ⎥ ⎣⎢ ⎦⎥ ⎣ + ⎦ The coordinate frames used in this paper are illustrated in J2 J3g − J2 J3 0 Det·R 00 Det Fig. 3. ΣF refer to the floor coordinate frame, ΣV is the vehicle Σ 1000 coordinate frame. B is the body coordinate frame, and xv is C = s 0100 the position of ΣV from ΣF . ΣB is located at the origin of ΣV .It θ = θ = − 2. rotates b about the YV axis. The center of mass of the body is Det max(Det( b)) J1 J2 J3 52 SICE JCMSI, Vol. 2, No. 1, January 2009 Fig. 4 A Block diagram of the digital identity disturbance observer.