Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems San Diego, CA, USA, Oct 29 - Nov 2, 2007 ThD6.5 Multirate Feedforward Control of Robot Manipulators Akira Shimada and Tsuyoshi Takeda Abstract This paper introduces a novel multirate feedforward control method for robot manipulators with elastic joints. Perfect tracking control methodology using multirate feedforward control was proposed by H. Fujimoto, et. al. Furthermore, A. Shimada, et. al. have described its application to robot manipulators; however, they only described the controller design process and gave a few simulation results. In addition, they did not provide details on how the state reference should be designed. Even if experiments are attempted, it is difficult to evaluate control performance since generally manipulators have joint angle sensors mounted on the output side of reduction gears, especially since the product of the angle of rotors and the reduction gear ratio does not always correspond with the joint angles. In order to solve this problem, this paper illustrates the design process for a state reference for manipulators, and introduces a prototype robot manipulator that has precision rotary encoders installed on the output side of the joints. This means that the manipulator as a platform not only has rotary encoders attached to servo motors but special rotary encoders. We describe experimental control results obtained by applying this control technique. I. INTRODUCTION One of the hindrances to improving the control performance of robot manipulators is residual vibrations caused by a lack of rigidity in reduction gears mounted on manipulator joints. Some papers have introduced mathematical models that model the characteristics of reduction gears[1], [2], [3], [4]. In particular, Kosaka and Shimada described a controller design technique that takes advantage of the characteristics of reduction gears. It improved control performance by including a state space expression for the disturbance observer and it considered not only the error between the reference rotor angles and the actual rotor angles but also the error between the reference joint angles and the estimated joint angles. This technique succeeded in reducing residual vibrations but it did not have a good tracking performance. Advanced digital control theory has been established[5], [6], [7]. Tomizuka et. al. defined a digital control method which achieved perfect tracking, while Fujimoto et. al. described a perfect tracking control method with multiple sampling rates[8], [9]. Further, Shimada et. al. designed a perfect tracking control system for multiple degree of freedom robot manipulators having elastic joints, that was based on the above-mentioned reports[10]. However, there are three shortcomings in their paper. The first shortcoming is that it does not include any experimental results. The second shortcoming is that it does not include a technique for validating the control method. The third is that the variables used in the controllable canonical form do not represent actual physical parameters. The method requires motion reference data that includes of all state variables. But if the variables are not physical parameters, it is not possible to use the method for design. This current study presents a solution for overcoming these three problems. II. MODELING OF MANIPULATORS This paper considers the case of a serial-link manipulator. Harmonic drive gears are mounted in its joints. To simplify the problem, this paper treats a two-link manipulator such as the one shown in Fig.1. In this figure, I oi represents the moment of inertia around the center of the i-th mass of the link, I ri represents the moment of inertia around the i-th axis, m ri represents the mass of the i-th mass, l i is the i- th length, r i is the distance between the i-th axis and the center of the i-th mass, and I mi and m mi are the moment of inertia and the mass of the rotor including the wave generator, respectively. The stator of the i-th motor is mounted on the (i-1)-th link, and the i-th rotor is connected to the i-th link by a harmonic drive gear. The moment of inertia and mass of the (i-1)-th link include those of the i-th stator. The i-th rotor is connected to the i-th stator by the bearings of the rotating axis. It is therefore translated with the (i-1)-th link, and it rotates concurrently. We next define the generalized coordinates {θ r1,θ r2,q m1,q m2 }. The first and second components are the first and second angles of the joints respectively, while the third and fourth components are the first and second rotor angles, respectively. A. Shimada and K. Gotoh are with Dept. of Electrical System Engineering, Polytechnic University, Hashimotodai, Sagamihara-shi, Kanagawa, Japan ashimada@uitec.ac.jp 1-4244-0912-8/07/$25.00 2007 IEEE. 4083 Fig. 1. Model of a two-link manipulator
We define M r1 = I r1 + I r2 + m r2 l 2 1 + I m2 + m m2 l 2 1 CR = m r2l 1 r 2 C C 1 = m r2 l 1 r 2 (2 θ r1 θr2 + θ 2 r2)sinθ r2 CC 2 = m r2 l 1 r 2 θ2 r1 sinθ r2 C G 1 = (m r1 r 1 + m r2 l 1 + m m2 l 1 )gcosθ r1 + m r2 r 2 gcos(θ r1 + θ r2 ),G 2 = m r2 r 2 gcos(θ r1 + θ r2 ).Furthermore,τ mi represents motor torque, τ di is the external torque and it is transferred via the reduction gear. D rv i,d mv i are the viscous friction coefficients of the joint and rotor respectively, while D rci,d mci are their respective Coulomb friction coefficients. k ni and d ni represent the elasticity and the viscous friction of the reduction gear on the arm side. Using these parameters we can derive the equation of motion for the system, Eq.(1), in which D ri = D rv i + d ni,d mi = n 2 i D mv i + d ni. M 11 M 12 0 n 2 I m2 θ r1 M 21 I r2 0 0 0 0 n 2 1 I m1 0 θ r2 q m1 /n 1 n 2 I m2 0 0 n 2 2 I m2 q m2 /n 2 D r1 0 d n1 0 θ r1 + 0 D r2 0 d n2 d n1 0 D m1 0 θ r2 q m1 /n 1 0 d n2 0 D m2 q m2 /n 2 + + k n1 0 k n1 0 0 k n2 0 k n2 k n1 0 k n1 0 0 k n2 0 k n2 C 1 + G 1 + D rc1 sgn( θ r1 ) C 2 + G 2 + D rc2 sgn( θ r2 ) n 1 D mc1 sgn( q m1 ) n 2 D mc2 sgn( q m2 ) = θ r1 θ r2 q m1 /n 1 q m2 /n 2 0 0 n 1 τ m1 n 2 τ m2 This equation leads to the following continuous system model. P (s) ={A c,b c,c c, 0}, (A c R 8 8,B c R 8 2,C c R 2 8 ), x =[θ r1,θ r2,q m1 /n 1,q m2 /n 2, θ r1, θ r2, q m1 /n 1, q m2 /n 2 ]. It is noted that centrifugal force, The Coriolis force and gravity are omitted, or cancelled due to additional nonlinear control inputs. III. MULTIRATE FEEDFORWARD CONTROLLER DESIGN A mathematical model of the manipulator is specially formulated in order to apply multirate feedforward control law to a manipulator described in a previous study[9]. For a manipulator which has m inputs, m outputs and n orders, the controller updates the control input and detects the output N times, which is the update ratio T l of the position reference, where T s = T l /N and N = n/m so that N is a natural number. (1) ẋ = A c x(t)+b c u(t),y(t) =C c x(t) (2) The plant model of the manipulator is digitalized using the control ratio T s, and it is expressed as P (z s ) = [A s,b s,c s,d s ], x(i, j) :=x[(i + j/n)t l ]. Eq.(3),(4) are then derived, in which 0 j<nand (i, j) refers to the time of t =(i + j/n)t r. x s (i, j +1)=A s x s (i, j)+b s u(i, j) (3) y(i, j) =C s x s (i, j)+d s u(i, j) (4) Where, A s = e AcTu,B s = T u 0 e Acτ B c dτ, C s = C c. A feedback controller is designed using the above equations. In practice, when updating occurs N times, the discrete plant model P (z l )=[A l,b l,c l,d l ] having a control ratio T l is derived. Where, x l (i) =x l [it l ] represents the state variable, u(i) =[u(i, 1),,u(i, N)] T represents the control input vector, and y =[y(i, 1),,y(i, N)] T represents the output vector. Using these parameters, Eq. (5),(6) can be derived. x l (i +1)=A l x l (i)+b l u(i) (5) y(i) =C l x l (i)+d l u(i) (6) e AcTr B 1 B N [ ] Al B l C 1 D 11 D 1N := C l D l... C N D N1 D NN B j := (1 µ (j 1) )T r (1 µ j)t r e Acτ B c dτ C k := C c e Acv kt r D kj := (vk µ µ j <v k : C j 1)T r c (v k µ j)t r e Acτ B c dτ (vk µ µ (j 1) <v k µ j : C j 1)T r c e Acτ B 0 c dτ v k µ (j 1) :0 0=µ 0 <µ 1 < <µ N =1,µ j = j/n 0 v 1 <v 2 < <v N < 1,v k =(k 1)/N B l is a n n(= m N) square matrix since it is derived from B j (j =1, 2,.., N) which is a n m matrix. Furthermore, it is a regular matrix[9]. Therefore, the feedforward input u f (i) = [u f (i, 1),,u f (i, N)] T and the reference y r (i) =[y r (i, 1),,y r (i, N)] T are derived from the state variable reference α r (i +1) R n as follows. u f (i) =B 1 l (I z 1 A l )α r (i +1) (7) y r (i) =z 1 C l α r (i +1)+D l u f (i) (8) Eq.(7) is equivalent to a stable inverse system of the plant model. The plant model can be expressed by two different models. One is the model derived directly from the physical model. The other model is derived from the MIMO controllable canonical form (see the Appendix). The control system for either model is illustrated in Fig.2. The feedback controller C 1 has no special restrictions imposed on it, but the input of the controller can use only joint angle errors. In the figure, the additional point which inputs the variable vector with suffix(i) and outputs another type of variable vector with suffix (i, j) appears to perform addition of vectors of different dimensions. However, we assume that the adder includes the multirate holder function as well as 4084
Fig. 2. Block diagram of multirate control system1 solution method is to use the controllable canonical form. This has the property that the state variables z are related to [ their respective time derivatives. ] But, although C c = 1 0 0 0 0 0 0 is a matrix for selecting the two 0 1 0 0 0 0 0 joint angles, C = C c S 1 in the output y(t) =C c S 1 z(t) of the canonical form does not select specific state variables. Where, z = [ ] T α 1 α 2 α 8. By contrast, when output y is a state variable representing the joint angles and it is used as a reference for the control system, the state variable z no longer represents the joint angles, angle velocities, or accelerations. In the case of the presented system, in practice the output equation for the controllable canonical form as a contentious system can be expressed as follows. Fig. 3. Block diagram of multirate control system2 [ θr1 θ r2 ] = [ ] V11 0 0 0 V 12 0 0 0 V 21 0 0 0 V 22 0 0 0 [ ] T α1 α 2 α 8 (9) This equation can be re-expressed as follows. [ θr1 ] [ ] [ ] V11 V = 12 α1 (10) θ r2 V 21 V 22 α 5 The coefficient [ matrix on the ] right-hand side is regular, 1 [ ] V11 V Then, by defining 12 W11 W = 12,and V 21 V 22 W 21 W 22 by considering α 1 = α 2, α 2 = α 3, α 3 = α 4, α 5 = α 6, α 6 = α 7, α 7 = α 8, the state variables are expressed by Eq.(11) using joint angle variables. Fig. 4. Block diagram of state expression for the disturbance observer the normal addition function. We further assume that the reference r(i) =α(i +1). While most manipulators have joint angle sensors such as rotary encoders mounted on servo motors, these joint angle sensors are not mounted directly on the joints. In this study, a state space expression for the disturbance observer is used to directly estimate the joint angles. Fig.3 shows a block diagram of the observer while Fig.4 illustrates its structure. IV. STATE REFERENCE DESIGN In the presented control system, the state variable vector α r (i +1) at one sample delay is utilized as the reference r(i). When the reference of the state variable vector is straightforward to design, the feedforward section should be designed based on the plant model derived from the physical model. However, for complex plants such as this system with low-stiffness, it is not easy to directly determine the values of state variables. In such cases, one possible α 1 α 2 α 3 α 4 α 5 α 6 α 7 α 8 [ ] W11 I = 4 W 12 I 4 W 21 I 4 W 22 I 4 θ r1 θ r1 θ r1 θ (3) r1 θ r2 θ r2 θ r2 θ (3) r2 (11) The suffix (3) in θ r1,θ r2 indicates the third-order derivative and we use it for the reference vector for the state vector α r (i +1). In addition, there are two ways to determine the feedforward part when specifying the reference vector. (Method-1) This method involves converting the reference vector for the state vector expressed by Eq.(11) into digital data using the control ratio T l, and then inputting the reference data into the feedforward part which is designed using the controllable canonical form. (Method-2) This method involves converting the state variable reference vector expressed by Eq.(11) into the former state variables x = S 1 z, and converting the variables into another digital data using the control ratio T l, finally, it inputs 4085
Fig. 5. Photograph of the manipulator the reference data into the feedforward part based on physical model. Both methods yield the same result. V. MANIPULATOR WITH JOINT ANGLE SENSORS This section introduces a prototype robot manipulator that has a precision rotary encoder mounted on the output side of the joints. This means that the manipulator as a platform has not only rotary encoders attached to the servo motors but also special rotary encoders. Fig.5 shows a photograph of the manipulator, in which the black circular parts mounted on the joints are the rotary encoders, each having a resolution of 1382400 pulse/rot. The outputs of the motors and the harmonic reduction gears are located in the back of the joints and are not visible in this photograph. Furthermore, the manipulator in the photograph is horizontal in order to eliminate the effect of gravity on the basic experiment. The mounted rotary encoders MEH-60-21600 PST 16 J6 are produced by Microtec Laboratory Inc., Japan, and their resolution, Res j = 1382400(pls/rot), is approximately the same as the product of the resolution of the rotary encoders TABLE I PHYSICAL PARAMETERS Symbol Unit Value Symbol Unit Value M r1 kgm 2 2.21 D m1 Nms/rad 15.88 M r2 kgm 2 0.42 D m2 Nms/rad 6.52 D r1 Nms/rad 15.88 D mc1 Nm 9.53 D r2 Nms/rad 6.52 D mc2 Nm 8.48 D rc1 Nm 18.39 k n1 Nm/rad 10000 D rc2 Nm 8.66 k n2 Nm/rad 3000 R kgm 2 0.2 d n1 Nms/rad 15.88 I m1 kgm 2 2.45 10 4 d n2 Nms/rad 6.52 I m2 kgm 2 7.62 10 5 n 1,n 2 (no-dim.) 161,121 Res m pls/rev 8, 196 Res j pls/rev 1,382,400 TABLE II CONTROL PARAMETERS Name Value Sampling time T s 2 (ms) Sampling time T l 8 (ms) Poles of 1st OB [0.68, 0.69, 0.70, 0.71, 0.72, 0.73, 0.74, 0.75] Q diag(10 4, 10 4, 10 4, 10 4, 1, 1, 1, 1, 5 10 8, 5 10 7 ) R diag(10 2, 10 2 ) and the gear ratios. Table I gives the physical parameters of the manipulator[4]. VI. EXPERIMENTAL RESULTS This section introduces some experimental results. Table II gives the control parameters for the experiment. Fig.6 shows the experimental results obtained when multirate feedforward control was applied, while Fig.7 shows the experimental results without multirate feedforward control. Both controllers use state space disturbance observers for estimating the joint angles by considering real disturbances. In these cases, the precision rotary encoders are utilized only for validation. Fig.8 shows the experimental results obtained when multirate feedforward control was applied using direct feedback of data from the precision rotary encoders. Comparing Figs.6 and 7, both the waveforms of the joint angle estimations track the reference angles. But the results when multirate feedforward control was applied are somewhat better than those obtained when it was not applied. Furthermore, there are small errors between the estimations and the real data; they seem to be due to small parameter errors in the physical model of the manipulator. The waveform of Fig.8 shows the high tracking performance of the real joint angles. This implies that if the presented control law and the encoder can be implemented in practice, good control of robot manipulators should be readily achievable. Fig.9 shows the motion of the manipulator during the experiments. VII. CONCLUSION This paper has described a practical method for designing and implementing multirate feedforward control of a robot manipulator. The experimental results indicate the realization of high tracking performance. The controller design process and implementation method are not too difficult and are natural. Therefore, it is anticipated that this technology can be applied to general robot manipulators and will lead to positive outcomes in the future. Acknowledgements: We express our thanks to Mr. Yasuo Oyama (Chairman of Microtech Laboratory Inc.) and Mr. Tsutomu Taira (President of Microtech Laboratory Inc.) for their kind support. APPENDIX Controllable canonical form for MIMO systems[11] We assume that (A c,b c ) is controllable. Defining B c = [b 1,b 2,,b m ], gives the matrix H = [b 1,A c b 1,,A n1 1 c b 1,b 2,A c b 2,,A n2 1 c b 2,, b m,a c b m,,ac nm 1 b m ], where the components should be selected such that n 1 = n 2 = = n m = n/m. Using the ζ i = n 1 + n 2 + + n i th row vector of the matrix H 1, gives the matrix S = [h 1,h 1 A c,,h 1 A n1 1,,h m,h m A c,,h m A nm 1 ] T. Furthermore, the coordinate transformation using z = S x c is performed for S. Then, the derived system {A, B, C, 0} = {SA c S 1,SB c,c c S 1, 0} is expressed in a controllable canonical form. Finally, by digitalizing 4086
Fig. 6. Experimental results obtained with feedforward control and joint angle estimation Fig. 7. Experimental results obtained without feedforward control or joint angle estimation Fig. 9. Photographs of the motion of the robot manipulator when multirate feedforward control was applied REFERENCES Fig. 8. Experimental results obtained with feedforward and high resolution rotary encoders the system, the state variables can be expressed by digital data. In this paper, this process should be applied using Eq. (1)(n =8,m=2). [1] J.Furusho,H.Nagao,M.Maruse: Multivariable Root Loci of Control System of Robot Manipulators with Flexible Driving Systems, T.JSME-C, Vo.56,No.526,pp.118-125,1990(in Japanese) [2] K.Yuki, T.Murakami, K.Ohnishi: Vibration Control of a 2 Mass Resonant System by the Resonance Ratio Control, Trans. of IEE Japan, Vol.113-D,No.10,1993(in Japanese) [3] Suguru Arimoto: Control Theory of Nonlinear Mechanical Systems: A Passivity-based and Circuit-theoretic Approach, Oxford University Press, Oxford, UK, P.280, 1996 [4] Y.Kosaka, A.Shimada,FMotion Control for Articulated Robots Based on Accurate Modeling,The 8th IEEE International Workshop on Advanced Motion Control - AMC 04, p.535-540, 2004 [5] T.Chen,B.Francis: Optimal Sampled-Data Control Systems, Communication and Control Engineering Series, Springer-Verlag, 1995 4087
[6] M. Araki,T. Hagiwara: Pole assignment by multirate-data output feedback, Int. J. Control, Vol.48,No.1,pp.305-315, 1988 [7] K. J. Astrom, P. Hangander and J. Sternby: Zeros of sampled system, Automatica, Vol.20,No.1, pp.31-38,1984 [8] Masayoshi Tomizuka: Zero phase error tracking algorithm for disital control, ASME, J.Dynami.Syst.,Measure.,and Contr.,Vol.109, pp.65-68, 1987 [9] H.Fujimoto,Y.Hori,A.Kawamura:Perfect Tracking Control Method Based on Multirate Feedforward Control, T.SICE,Vol.36,No.9,pp.766-772,2000(in Japanese) [10] A.Shimada,Y.Shinohara: Multi-rate Perfect Tracking Control on Manipulators with Elastic Joints, T.SICE,Vol.42,No.2,pp.139-146,2006(in Japanese) [11] H.Kogo,T.Mita:Introduction to System Control Theory, Jikkyo Shuppan,1980 (in Japanese) 4088