Human-friendly Motion Control of a Wheeled Inverted Pendulum by Reduced-order Disturbance Observer

Transcription

1 2008 IEEE International Conference on Robotics an Automation Pasaena, CA, USA, May 19-23, 2008 Human-frienly Motion Control of a Wheele Inverte Penulum by Reuce-orer Disturbance Observer DONGIL CHOI an Jun-Ho Oh Abstract This paper introuces a novel approach to control a wheele inverte penulum when a isturbance is applie by a human. The interaction between a human an a wheele inverte penulum involves a pulling or pushing force. This type of action is a severe isturbance for a wheele inverte penulum, as the wheele inverte penulum tens to maintain its initial position if there is no esire input. Thus, there are many possibilities for the wheele inverte penulum to be unstable as a result of interactions with humans. To solve this problem, the control algorithm of a wheele inverte penulum was esigne to move in coorination with the external force of a human. This control algorithm is terme human-frienly motion control. It contains an optimal controller using a full-state feeback an a reuce-orer isturbance observer. The isturbance torque from a human was estimate, an the estimate isturbance torque was use to generate a position reference for the human-frienly motion. This control algorithm keeps the wheele inverte penulum stable even when a severe isturbance is applie. I I. INTRODUCTION N THE PAST, the humanoi robots use a three or four stable wheel system. In the case of Haaly-2 [7], two riving wheels an two passive wheels were use as a caster. The mobility of such a stable wheel system was worse than that of a two-wheele self-balancing system. A two-wheele system is essentially unstable but can be stabilize inepenently in the same manner as a human. Therefore, a two-wheel system has mobility that is more natural, which allows it to interact with humans. When a two-wheele humanoi robot interacts with a human, the stability of the robot can be isturbe by the pulling an pushing forces from the human. The robot can fall own ue to these isturbances. A human can even be hurt by the falling robot. To prevent this type of angerous situation, a novel control algorithm is esigne here for human safety issues. This control metho is terme the human-frienly motion control [8], [9]. Thus far, a number of stuies concerning two-wheele self-balancing mechanisms have been conucte [1]-[5]. These stuies eal with a control metho for balancing. However, they i not consier the problem of maintaining Manuscript receive September 13, D. CHOI is with Humanoi Robot Research Center, KAIST, Daejeon, KOREA (phone: ; fax: ; ci8419@gmail.com) J. Oh is with Humanoi Robot Research Center, KAIST, Daejeon, KOREA ( jhoh@kaist.ac.kr) the stability after an external force is applie to the mechanism. Another stuy consiere cooperative transportation [6]. An observer was use to estimate isturbance forces an a esigne force an position controller was utilize for object transportation after an external force from a human or a robot. However, they assume that the external force woul be applie to the boy of the wheele inverte penulum at the axle height horizontally. In this case, the stability of the system is not affecte from external force as much as when the force is applie to the upper part of the center of gravity (CG) of the penulum. In such a case, serious stability problems arise. In this paper, a wheele inverte penulum was chosen as a simplifie moel of a two-wheele humanoi robot. A case in which an external force from a human was applie to the upper part of a wheele inverte penulum is consiere. To stabilize the wheele inverte penulum even when a severe external force is applie, a human-frienly motion control algorithm was evelope. This algorithm allows the penulum to move naturally coorinate with the external force an maintains a safety environment for the human. For the construction of this algorithm, a position controller for a wheele inverte penulum was esigne using a LQR metho for self-balancing an tracking esire position. In aition, a reuce-orer isturbance observer was esigne to estimate isturbances by external forces. The estimate external force is use to generate position references. Experiments were conucte to test the human-frienly motion control. Rate gyro an accelerometer (Insie the boy) Two c-motors with encoers (Insie the boy) Micro-controller an motor amplifier (Lower part) Two 24V-7Ah Lea batteries II. SYSTEM DESIGN Weight (Upper part) Penulum Wheel an pulley Yaw Figure 1. The structure of the wheele inverte penulum z Roll Pitch x y /08/$ IEEE. 2521

2 The structure of the wheele inverte penulum use in this research is shown in Figure 1. It was assume that a wheele inverte penulum coul be use as a service robot in a public area. Accoringly, it was esigne to be one meter in height. At the upper part of the penulum, an ajustable weight was ae. This makes testing the various mass an inertia conitions easier. At the lower part of the penulum, two c-motors, two encoers, two 24V-7Ah batteries, a micro-controller, a two-channel motor amplifier, a rate gyro an an accelerometer were ae. A two-stage pulley-belt system with a 14:1 reuction ratio an a quiet rive without backlash were also ae to the evice. For control of the motor, a micro-controller an a two-channel motor amplifier were evelope. For the micro-controller, a TI TMS320F2808 igital signal controller was use. A custom-mae motor amplifier hanle two high-power c-motors (1kW) in a single boar espite its relatively small size (100x100x40mm 3 ). A rate gyro an an accelerometer were use to measure the tilt angle in the pitch irection. These two sensors were combine with complementary filters to provie more accurate tilt information. The accelerometer provies accurate static tilt information when the robot is not accelerating. The rate gyro can be integrate to provie accurate ynamic tilt information, but the integration tens to rift over time. By combining two sensors, it was possible to measure robust inertial information. problem of controlling the penulum system, as it was unstable. Therefore, it was assume that no yaw motion exists in the penulum system. Aitionally, it was assume that the wheels of the inverte penulum are always in contact with the groun an that no slip exists at the contact patches. Therefore, no roll motion exists. Finally, only the longituinal motion along the x-axis an the pitch motion are consiere. The longituinal movement of the penulum is characterize by the angular isplacement of the wheel φ an the pitch motion is characterize by the tilt angle of the penulum θ. For an efficient analysis, a free boy iagram of a simplifie moel is shown in Figure 3. This simplifie moel can be consiere two ivie parts: a penulum part an a wheel part. III. MODELING A mathematical moel of the evelope control system of the wheele inverte penulum is escribe in Figure 2. (a) The penulum part (b) The wheel part Figure 3. Free boy iagram of the wheele inverte penulum Newtonian reference frame The upper mass of the penulum part Wheels an pulleys of the wheel part The lower mass of the penulum part Figure 2. A simplifie iagram of the wheele inverte penulum The wheele inverte penulum can be ecouple in two ifferent subsystems [1]. The first of these is a penulum system an the secon is a rotation system. The penulum system is an unstable system an the rotation system is a neutrally stable system. Effort here was focuse on the TABLE I PARAMETERS OF THE WHEELED INVERTED PENDULUM Symbol Quantity M The total mass of the penulum part l CG Length between the CG an the wheel axis Length between the point of application of external force F an the wheel axis I CG Moment of inertia of the penulum m Mass of a wheel an a pulley r Raius of a wheel J Moment of inertia of a wheel an a pulley g The constant of acceleration of gravity J m Moment of inertia of a motor rotor B m Viscous friction coefficient of a motor K t Torque constant of a motor K e Electrical constant of a motor The armature resistance of a motor R m In the penulum part, there is an upper mass an a lower mass. The CG of the penulum is locate near the mile of these two masses. The total mass of the penulum part is characterize as M. It was assume that external force F is applie to the upper mass of the penulum part as a 2522

3 isturbance from a human. Counter torque from two motors 2T w is applie in the penulum in the other irection compare to the torque of wheel part. In aition, the reaction forces between the penulum part an the wheel part R x, R y are applie on the axle. The wheel part contains two wheels an two secon-stage pulleys. As there is no yaw motion, two wheels always rotate ientically. Therefore, the two wheels an the two pulleys are consiere as one boy. The friction forces between the wheels an the groun is enote as F r. In an actual control system, the control variable is the voltage of the motor V instea of the motor torque T w. Therefore, a ynamic equation for the motor an was erive, an that relationship is containe in the equation of motion. Moreover, it is assume that it is possible to linearize the equations of motion aroun the operating point ( θ = 0 ). The equations of motion of this moel are as follows: KK ( ) ( 2 2 ) 2 2 t e ICG + MlCG + n J m θ + MrlCG n J m φ + n bm + θ Rm 2 KK t e Kt 2 n bm + φ MlCGg θ + F = 2n V Rm Rm ( ) θ ( ) Mrl CG 2n J m + 2J + M + 2m r + 2n J m φ 2 KK t e 2 KK t e Kt 2n bm + θ + 2 n bm + φ+ Fr = 2n V Rm Rm Rm where the parameters are efine in Table I. IV. CONTROL SYSTEM DEVELOPMENT In this section, the evelopment of control system is iscusse. The equations of motion for the system (1), (2) can be written in state-space form, as follows: (1) (2) x = Ax + Bu + W F (3) φ φ 0 a a a b = = = = = θ θ 0 a a a b x, A, B, W, u V where a 1, a 2, a 3, a 4, a 5, a 6, b 1, b 2, 1, an 2 are efine as functions of the parameters of the wheele inverte penulum. With this state-space equation, the following control strategies were esigne to maintain the penulum in equilibrium an to maintain a stable posture even when a isturbance is applie. A. Position controller using LQR metho Before the position controller was esigne, it was assume that there was no external force (F = 0). In orer to control the position an posture of the wheele inverte penulum, it was necessary to control the rotation of the wheels as a control output V which is the voltage of the wheel. For the wheele inverte penulum use here, the pitch angle of the penulum θ was measure from a combination of two sensors: a rate gyro an an accelerometer. Also from the rate gyro, the angular velocity of the pitch motion of the penulum θ was obtaine. The angular isplacement an angular velocity of the wheel φ, φ were measure with the rotary encoer. Therefore, all of the state-variables from the sensor signal were use as feeback. Using this full-state feeback, it was possible to construct control input u, as follows: u = K( x x ref ) (4) where x is the state variable vector, x ref is the reference vector, an K is the feeback gain vector K= [K 1 K 2 K 3 K 4 ]. The feeback gain K was obtaine using LQR metho, which results in some balance between system errors an control efforts. To use this LQR metho, it was necessary to etermine three parameters: the performance inex R, the state-cost matrix Q, an the weight factors. For simplicity, the performance inex matrix was efine as equal to 1. The weighting factors in the state-cost matrix were chosen by trial an error. If the weighting factors of φ, φ are increase, the penulum becomes very sensitive to the isturbance, but more accurate position tracking performance is given. On the other han, if these factors are ecrease, the penulum is less sensitive to the isturbance but weak to track esire position. In the control strategy, the high weighting factors of φ, φ were chosen for better position tracking performance. For the isturbance problem, a position reference to offset isturbance was planne using the observer escribe in the next section. B. Reuce-orer Disturbance Observer In this system, it was possible to obtain the state vector, x using the sensor signal. However, it was not possible to measure the external force irectly. The purpose of the reuce-orer isturbance observer is to make estimations of the external force F. The estimate external force will be use in the human-frienly motion control. The reuce-orer observer reuces the orer of the estimator by the number of measurable outputs. In this system, the external force is the only unmeasure variable. Therefore, the state vector can be ivie into two parts: x a, which is irectly measurable by sensors as x, an x b, which must be estimate as F. By assuming that the erivative of the external force equals zero ( F = 0 ), it was possible to moify the state-space form, as follows: 2523

4 x a Aaa Aab xa Ba = + u x A A x B b ba bb b b xa y = [ 1 0] x b where T xa = φ φ θ θ, xb = F, u = V a1 a2 a 3 1 b 1 Aaa =, Aab =, Ba = a4 a5 a6 2 b2 A = , A = 0, B = 0 [ ] ba bb b (5) (6) position reference uner various conitions. No matter how well the observer is esigne, it is not possible to estimate the exact external force owing to sensor noise an moel mismatch. In some cases, the position reference can be generate by an erroneous estimation, even when an external force is not applie. To prevent this unesire case, the threshol was set so that it ignores estimate external forces that are less than the limit value. By aing the position reference to the feeback system, the penulum was mae coorinate its movement with the external force from a human. A block iagram of the human-frienly motion control system is shown in Figure 5. With this state-space form, the reuce-orer isturbance observer was constructe. The block iagram of the observer is escribe in Figure 4. A etaile esign of the observer [12] is straightforwar an is not presente here. Figure 5. A block iagram of the human-frienly motion control system V. EXPERIMENTS Figure 4. A block iagram of reuce-orer isturbance observer In the above iagram, the observer gain L was obtaine using a pole placement metho by trial an error by changing the settling time of the observer to balance between a goo transient response an a sufficiently low sensor noise. Using this observer, the external force coul be estimate. C. Human-frienly Motion Control The position controller maintains its original position well against isturbances. If a severe isturbance is applie, it can break its controllable range. To solve this problem, a control algorithm was formulate to generate position reference to prevent falls as a result of an external force. This implies that if the penulum is pushe or pulle, it moves the way it is commene. But if no external force exists, it maintains its original position. For the generation of the position reference, the following equations were evelope: ref H ( F ) t ( ref ref ) φ = (7) u = K x x V φ (8) where H is a gain obtaine experimentally to make the best A. Estimation of the external force with position control Using the control law shown in Figure 5, the penulum was mae to stan stably an the external force was estimate. As shown in Figure 6, a static external force was applie. As the external force increase, the penulum incline graually to sustain the force. If the weight was too heavy, the penulum was not able to maintain a stable range of the pitch angle of ± 20. Hence, the weight was increase by 0.5kg stepwise within the limit of the pitch angle every 6~7 secons. The human-frienly motion control was eactivate for the test of the estimation of the external force in this experiment. Figure 6. Experiment of estimation of the external force 2524

5 As the external force is applie horizontally, the estimate external force was converte as the torque using following relationship: T = F cosθ (9) T estimate(nm) mean of T estimate(nm) T applie(nm) where is the height from the axle which is the point of application of the external force an θ is the pitch angle of the penulum. B. Experiment of Human-frienly motion control The purpose of human-frienly motion control is to guarantee human safety when a human pulls or pushes the wheele inverte penulum. In Figure 7, a human is shown pushing the upper part of the penulum. Uner a conition in which the human-frienly motion control was eactivate, the pushing force was strong enough to cause the penulum to topple over. However, the penulum pushe away stably ue to the generate position reference in an integrate manner. Detaile motion ata is presente in the next chapter time(sec) Figure 8. The result of the estimation of the external force In this figure, it is clear that there is strong vibration in the estimate isturbance torque. However, the average of the estimate value is fairly close to the real value. For a more etaile analysis, the true value, mean value, an the stanar eviation of the isturbance torque are shown in Figure 9. Figure 7. The penulum coorinating with an external force applie by a human Figure 9. The result of the estimation of the external force VI. EXPERIMENTAL RESULTS In Experiment A, the estimation of the external force while the penulum stoo stably by the control system was teste. The result of Experiment A is presente in Figure 8. It shows the estimate isturbance torque, its mean value, an the actual applie torque. Figure 9 shows that the stanar eviation of the estimate value is within the true value, although a vibration exists. Thus, it is confirme that the external force can be estimate using the reuce-orer isturbance observer. Using this observer, Experiment B was conucte. The result of the human-frienly motion control is presente in Figure

6 T (Nm) φ (ra) φ (ra/sec) In the future, the authors aim to aopt the human-frienly motion control to an actual two-wheele humanoi robot for an evaluation of the control system esigne in this research θ (ra) θ (ra/sec) time(sec) Figure 10. The result of the experiment of human-frienly motion control After the isturbance ha starte for approximately 1 secon, the angular isplacement of the wheel φ increase ue to the generation of the position reference. It stoppe at approximately -10 ra. In the same time, the pitch angle was incline to its maximum of nearly 0.3 ra ( 17 ). This value is within the safe tilt angle range of the penulum. This implies the penulum offsets the isturbance by moving smoothly in the irection from which the external force is applie. Experiments were conucte regaring the performance of the human-frienly motion control. VII. CONCLUSION In this research, it was consiere that a stability problem may occur with a wheele inverte penulum when it interacts with a human. To aress this issue, a human-frienly motion control was constructe. This control strategy allows the penulum to offset the isturbance from the human safely using an integrate control system. To realize this control strategy, a position control system was built using full-state feeback to guarantee the stability of the penulum, an a reuce-orer isturbance observer was utilize to estimate the external force. Through the use of the estimate external force, a control algorithm was esigne to make proper position references to offset the isturbance. Experiments showe that the human-frienly motion control was able to estimate the external force while staning safely an that the penulum can be stabilize even when a severe isturbance is applie. REFERENCES [1] Felix Gasser, Alo D Arrigo, Silvio Colomi, JOE: A Mobile, Inverte Penulum, IEEE Transactions on Inustrial Electronics, vol. 49, NO. 1, pp , Feb [2] Kaustubh Pathak, Jaume Franch, an Sunil K. Agrawal, Velocity an Position Control of a Wheele Inverte Penulum by Partial Feeback Linearization, IEEE Transactions on Robotics, vol. 21, NO. 3, pp , June [3] Salerno, A. an Angeles, J., Nonlinear Controllability of Quasiholonomic Mobile Robot, Proc. IEEE ICRA, pp , April [4] Kim, Y.H., Kim, S.H., an Kwak, Y.K., Dynamic Analysis of a Nonholonomic Two-wheele Inverte Penulum Robot, Proc. of the Eighth Int. Symp. on Artificial Life an Robotics (AROB8th, 03). pp , Jan [5] Nawawi S.W, Ahma M.N, Osman J.H.S, Control of Two-wheels Inverte Penulum Mobile Robot using Full Orer Sliing Moe Control, Proc. of Int. Conference on Man-Machine Systems, Sep [6] Naoji SHIROMA, Osamu MATSUMOTO, Shuuji KAJITA, Kazuo TANI, Cooperative Behavior of a Wheele Inverte Penulum for Object Transportation, Proc. IROS 96, vol. 2, pp , Nov [7] Toshio MORITA, Koji SHIBUYA, Shigeki SUGANO, Design an Control of Mobile Manipulation System for Human Symbiotic Humanoi: Haaly-2, Proc. of the 1998 IEEE ICRA, vol. 2, pp , May [8] Jochen Heinzmann an Alexaner Zelinsky, The Safe Control for Human-Frienly Robots, Proc. of the 1999 IEEE/RSJ, Int. Conference on Intelligent Robots an Systems, vol. 2, pp , 1999 [9] Sehoon Oh, Naoki Hata, Yoichi Hori, Proposal of Human-frienly Motion Control: Control Design for Power Assistance Tools an its Application to Wheelchair, IECON th Annual Conference of IEEE, vol. 1, pp , Nov [10] Chia-Shang Liu, Huei Peng, Disturbance Observer Base Tracking Control, Journal of Dynamic Systems, Measurement, an Control, vol. 122, June [11] Joseph A. Profeta, William G. Vogt, Marin H. Mickle, Disturbance Estimation an Compensation in Linear Systems, IEEE Transactions on Aerospace an Electronic Systems, vol. 26, NO. 2, March [12] Gene F. Franklin, J. Davi Powell, Abbas Emami-Naeini, Feeback Control of Dynamic Systems, 4 th Eition., Prentice Hall, 2002 [13] Katsuhiko Ogata, Moern Control Engineering, 4th eition, Prentice Hall,