Wall Juggling of one Ball by Robot Manipulator with Visual Servo

Transcription

1 Juggling of one Ball by obot Manipulator with Visual Servo Akira Nakashia Yosuke Kobayashi Yoshikazu Hayakawa Mechanical Science and Engineering, Graduate School of Engineering, Nagoya University, Furo-cho, Chikusa-ku, Nagoya, Japan (e-ail: {a nakashia, y kobayashi, hayakawa}@nue.nagoya-u.ac.jp) Abstract: This paper propose a ethod to achieve wall juggling of a ball without rebound fro a table by a racket attached to a robot anipulator with two visual caera sensors. The proposed ethod is coposed of juggling preservation proble and ball regulation proble. The juggling preservation proble eans going on hitting the ball iteratively. The ball regulation proble eans regulating the hitting position of the ball. The juggling preservation proble is achieved by the tracking control of the racket position for a syetry trajectory of the ball with respect to a horizontal plane. The ball regulation proble is achieved by controlling the racket orientation, which is deterined based on a discrete transition equation of the hitting position of the ball. The effectiveness of the proposed ethod is shown by an experiental result. 1. INTODUCTION Since Ball juggling is a typical exaple to represent dexterous rhythic tasks of huans, the ball juggling by robots has been studied by any researchers. The kinds of the juggling are toss juggling, paddle juggling, bouncing juggling, wall juggling and so on. Especially, the paddle juggling are studied by any researchers because it is siple iterative hitting of a ball by a racket. The paddle juggling by robots is coposed of three parts: the iteration of hitting the ball, the regulation of the ball state which are the height and the incident angle to the racket, and the regulation of the hitting position of the ball. M. Buehler et al. (1994) proposed the irror algoriths for the paddle juggling of one or two balls by a robot having one degree of freedo in two diensional space, the robot otion was syetry of the ball otion with respect to a horizontal plane. This ethod achieved hitting the ball iteratively.. Mori et al. (25) proposed a ethod for the paddle juggling of a ball in three diensional space by a racket attached to a obile robot, the trajectory of the obile robot was deterined based on the elevation angle of the ball. This ethod achieved hitting the ball iteratively and regulating the incident angle. A. Nakashia et al. (26) proposed a ethod for the paddle juggling of a ball in three diensional space by a racket attached to a robot anipulator having 5 degrees of freedo. This ethod achieved hitting the ball iteratively, regulating the incident angle and the hitting point siultaneously. These ethods were the feedback control based on the ball state. On the other hand, T. M. H. Dijkstra (24) proposed an open loop algorith without the ball state for one-diensional paddle juggling of one ball. They derived the condition for the racket trajectory to stabilize the periodic trajectory of the ball. However, the paddle juggling can be achieved easier than the other jugglings because there does not exist any contacts with environents around robots in the paddle juggling. There are few studies of the other jugglings. M. Takeuchi et al. (22) considered the wall juggling of a ball, which eans hitting a ball against a wall iteratively by a racket attached to a robot having 2 degrees of freedo. They allowed rebound fro a table after the rebound fro the wall. They analyzed the stability of periodic trajectory around its equilibriu states of the ball and proposed a feedback law to stabilize the trajectory. On the other hand, there is the another wall juggling which eans hitting the ball against the wall without the rebound fro the table. In this juggling, the racket otion is alost in the parallel direction to the wall because it is forced to hit the ball before the rebound while the racket in the forer can ove in any direction. This constrained racket otion ake the wall juggling without the rebound ore difficult. This paper propose a ethod to achieve wall juggling of a ball without rebound fro a table by a racket attached to a robot anipulator with two visual caera sensors. The proposed ethod is coposed of juggling preservation proble and ball regulation proble. The juggling preservation proble eans going on hitting the ball iteratively. The ball regulation proble eans regulating the hitting position of the ball. The juggling preservation proble is achieved by the tracking control of the racket position for a syetry trajectory of the ball with respect to a horizontal plane. The ball regulation proble is achieved by controlling the racket orientation, which is deterined based on a discrete transition equation of the hitting position of the ball. The effectiveness of the proposed ethod is shown by an experiental result. In Section 2, the syste configuration and the odels of the anipulator and the ball otion are shown. The anipulator dynaics are linearized for preliinary in Section 3. In Section 4, the control designs for the juggling preservation and the ball regulation probles are shown. An experiental result is shown in Section 5 and the conclusion is described in Section 6.

2 Two-eyes caera J 3 J 2 J 1 Z Y X Σ B ef. frae J 5 obot anipulator Juggled ball J 4 Z X Y Σ acket J 6 to tie t and getting together the resultant equations yield the velocity relationship ẋ = J (q) q, (3) J := ( h 1 / q) T ( h 2 / q) T T 5 5 x := B p T B θ T 5. Applying the relationship (3) for the dynaical equation (1) yields the dynaical equation with respect to the position and orientation of the racket M (q)ẍ + C (q, q)ẋ + N (q) = J (q) T τ, (4) Fig. 1. Juggling of a Ball by Caera-obot Syste 2.1 Syste Configuration 2. MODELING In this paper, we consider control of wall juggling of one ball by a racket attached to a robot anipulator with twoeyes caera as shown in Fig. 1. The robot anipulator has 6 joints, which is denoted by J i (i = 1,, 6) shown in Fig. 1. In this paper, the juggled ball is supposed to be saller and lighter than the racket. The physical paraeters of the anipulator, the racket and the ball are shown in Section 5. The reference frae Σ B is attached at the base of the anipulator. The position and orientation of the racket are represented by the frae Σ, which is attached at the center of the racket. The caera is calibrated with respect to Σ B. Therefore, the position of the juggled ball can be easured as the center of the ball which the two caeras syste detects (B. K. Ghosh et al. (1999)). It is obvious that the orientation of the racket about z-axis with respect to Σ B does not effect on the ball otion at the tie when the racket hits the ball. Therefore we need only 5 degrees of freedo of the anipulator. In the following discussion, the fourth joint J 4 is assued to be fixed by an appropriate control. 2.2 Dynaical Equation of Manipulator The dynaical equation of the anipulator is given by M(q) q + C(q, q) q + N(q) = τ, (1) q 5 and τ 5 describe the joint angles and the input torques respectively, and M 5 5, C 5 5 and N 5 are the inertia atrix, the coriolis atrix and the gravity ter respectively. For the latter discussion, we introduce the following coordinate transforations: B p := h 1 (q) 3, B θ := h 2 (q) 2, (2) B p = B p x B p y B p z T 3 is the position of Σ with respect to Σ B, B θ = B θ x B θ y T 2 is the orientation of Σ with respect to x- and y- axes of Σ B. The left superscripts B of vectors denote that the vectors are expressed with respect to the frae Σ B. This notation is utilized for another fraes in the following. The functions h 1 (q) and h 2 (q) describe the relationships between the position/orientation of the racket and the joint angles respectively. Differentiating (2) with respect M := J T C := J T 1 MJ CJ + J T N := J T N 5. M d dt J Equation of Motion and ebound Phenoenon of Ball For the odeling of the equation of otion and the rebound phenoenon of the ball, we ake the following assuptions: Assuption 1 There does not exist the air resistance for the ball. Assuption 2 The surfaces of the ball, the racket and the wall are unifor and sooth. Therefore, the coponents of the velocity of the ball parallel to the surfaces do not change due to the rebound phenoenon, that is, the restitution coefficients parallel to the surfaces are zero. Assuption 3 The ass of the racket is bigger than the ass of the ball such that the racket velocity does not change due to the rebound phenoenon. Define the ball position as B p b := B p bx B p by B p bz T 3. Fro Assuption 1, the equation of otion of the ball is given by Bṗ bx = x, Bṗ by = y, (5) B p bz = g, (6) x and y are the velocity coponents in the (x, y) plane respectively, kg is the ass of the ball and g/s 2 is the gravitational constant. Note that (5) represents the ball otion in the (x, y) plane, which is the unifor otion, and x and y are changed only at the rebounds fro the racket and the table. Fro Assuption 2 and 3, the atheatical odel of the rebound phenoenon at the rebound fro the racket is given by Bṗ b (t r + dt) = B E T B( B ṗ b (t r ) B ṗ (t r )) + B ṗ (t r ), (7) t r is the start tie of the collision between the ball and the racket, dt is the tie interval of the collision, B 3 3 is the rotation atrix fro Σ to Σ B and E := diag(,, e) 3 3 represents the restitution coefficients of the x, y and z directions with respect to Σ. Note that the rebound phenoenon fro the wall is the sae as (7) without B ṗ (t r ).

3 Initial point Ball trajectory Y B Ball trajectory Hitting Horizontal surface acket Desired point Stabilized Juggling O acket z trajectory (a) Saller opposite trajectory of z. t acket (x,y) trajectory (b) Trajectory of (x,y). Fig. 3. Control Schee for the acket Position Control Y B All hitting points are included in a sae (x,y) plane. ball. The regulation of the ball height at the rebound fro the wall is not included in this proble. Fig. 2. The concept for Juggling of one Ball 3. LINEAIZING COMPENSATO FO MANIPULATO As for the preliinary to control the wall juggling, we linearize the anipulator dynaics (4) by the following linearizing copensator: τ = J T (M u + C ẋ + N ), (8) u := u T p ut θ T 5 is the new input for ẍ. Substituting (8) into (4) results in M (ẍ u ) =. Since M = J T MJ 1 is the positive definite atrix because the inertia atrix M is positive definite, M always has the inverse atrix. Therefore, we get the linearized equations given by B p = u p, (9) B θ = u θ. (1) The tracking control of B p and B θ can be easily realized by PD controller for u p and u θ, for exaple. In the following discussion, let us consider the desired values of B p and B θ to realize the wall juggling of the ball. 4. CONTOL DESIGN FO WALL JUGGLING 4.1 Control Objectives Figure 2 illustrates the concept of the wall juggling. Without loss of generality, the wall is located parallel to (Y B, ) plane. The initial point of the ball is above the racket and the ball is freely released. The control purpose is to achieve the wall juggling of the ball at the desired hitting point B p d by hitting the ball with the racket iteratively. This is described by the following specified control probles: (1) Juggling Preservation Proble The juggling Preservation proble eans going on hitting the ball iteratively. The control of the ball position is not included in this proble. (2) Ball egulation Proble The ball regulation proble eans regulating the hitting position of the hit In the following sections, the control designs for these control probles are shown. 4.2 Control Design for Juggling Preservation Proble The control design for the juggling preservation proble with controlling the racket position B p is shown. We firstly consider that the ball is always hit by the racket in a sae (x, y) plane as shown in Fig. 2. More specific deterination of the racket position is shown in Fig. 3. The trajectories of the z coordinates of the ball and the racket are shown in the right figure (a), the vertical axis is the coordinate and the horizontal axis is tie t respectively. The z coordinate of the racket is forced to follow the syetric trajectory of the z coordinate of the ball with respect to a horizontal plane. This plane is called the hitting horizontal plane in this paper. Due to this deterination, the racket can hit the ball at the sae height without the prediction of the z coordinate of the ball. On the other hand, let us consider the (x, y) trajectories of the racket in order to guarantee that the racket always hits the ball. For the paddle juggling, A. Nakashia et al. (26) considered a ethod to force the (x, y) coordinates of the racket to follow the (x, y) coordinates of the ball. We odify this ethod for the wall juggling because the racket clashes the wall with the ethod. The trajectories of the (x, y) coordinates of the ball and the racket are shown in the left figure (b), the vertical and horizontal axes are the Y B and coordinates respectively. The y coordinate of the racket is forced to follow the y coordinate of the ball while the x coordinate of the racket oves to the point of fall of the ball before the z coordinate of the ball is equals to the height of the hitting horizontal plane. The prediction of the point of fall is shown in Section 4.4. To su up, the desired value of the racket position B p d to satisfy the entioned in the above is given by B p bx B p d := B p by, (11) B p h k h ( B p bz B p h ) B p bx is the predicted value of the x coordinate of the ball, B p h is the height of the hitting horizontal plane and < k h < 1 is the constant to ake the z coordinate

4 B ph Y x b i i +1 B v b i θ y x b x bw w v bw Fig. 4. Ball transition due to ebound fro l z bw of the racket sall appropriately. Note that k h effects on the height of the hitting ball. Due to deterining the desired value of the racket position B p by (11), there always exists the racket under the ball at the hit and the ball is hit in the sae horizontal plane autoatically. 4.3 Control Design for Ball egulation Proble The control design for the ball regulation proble with controlling the racket orientation B θ is shown. Suppose that the juggling preservation proble is achieved. Control of the x coordinate of the ball The transition of the x coordinate of the ball due to the rebound fro the racket and the wall in (x, z) plane is illustrated in Fig. 4. x b i is the x coordinate of the ball at the ith hit, θ y i is the racket orientation about Y B -axis at the ith hit, 2 and v b 2 are the ball velocity just before and after the hit, and V 2 is the racket velocity. The constant i (i =, 1, 2, ) represents the nuber of the hitting. We derive the discrete transition equation of the x coordinate of the ball in order to design the controller. Fro the odel of the rebound phenoenon (7), the relationship between, v b and V is given by v 1 (1 + e) sin 2 1+e θ b = y 2 sin 2θ y 1+e 2 sin 2θ y 1 (1 + e) cos 2 θ y + (1 + e) sin 2 θ y 1+e 2 sin 2θ y 1+e 2 sin 2θ y (1 + e) cos 2 θ y V, (12) e is the restitution coefficient between the ball and the racket in the noral to the racket. Sine the racket position B p follows the target position (11), the racket velocity V is expressed as the following by differentiating (11): V =. (13) k h z Substituting (13) into (12) yields v {1 (1 + e) sin b = 2 θ y }x + (1+e)(1+k h) 2 sin 2θ y z 1+e 2 sin 2θ y x +{1 (1 + e)(1 + k h ) cos 2. θ y }z (14) On the other hand, fro (5) (7), the relationship between x b i and x b i + 1 is given by x b i + 1 = e w x b i + (1 + e w )l 2e w g v bxv bz, (15) e w is the restitution coefficient between the ball and the wall in the noral to the wall and l is the distance of the wall fro the origin of the reference frae. Substituting (14) into (15), we can get the discrete transition equation of x b i. However, since the resultant equation is nonlinear, we linearize it around θ y = θ y and k h = k h for the control design: x b i+1 = e w x b i+(1+e w )l 2e w g B θy θ y k h k, (16) h B := (v bx v bz ) (v bx v bz ). (17) θ y k h θ y = θ y,k h = k h For siplicity, suppose that the control gain k h is a constant k h. Therefore, the linearized discrete equation of the x coordinate of the ball for the control design is given by x b i + 1 = e w x b i + (1 + e w )l 2e w g b 1 θ y, (18) b 1 is the first coponent of B and θ y := θ y θ y. For the syste (18), we propose the following state feedback law: θ y = k x (x b i x bd ), (19) k x is the control gain and x bd is a target value of the x coordinate of the hitting position. Substituting (19) into (18) yields the closed loop syste x b i + 1=( e w + 2e w g b 1k x )x b i + (1 + e w )l 2e w g b 1x bd k x. (2) Fro (2), the conditions to stabilize x b to x bd are given by e w + 2e w g b 1k x < 1 (21) (1 + e w )l 2e w g b 1x bd k x = x bd. (22) The condition (22) leads to the control gain k x : k x = g { } (1 + ew )l 1. (23) 2e w b 1 x bd Substituting (23) into (21) yields the range of the target value x bd : 1 + e w l < x bd < 1 + e w l. (24) 2 + e w e w Control of the y coordinate of the ball Fro Assuption 2, the transition of the y coordinate of the ball is the sae as the one of the ball without the rebound fro the wall. Therefore, it is enough to consider the rebound between the ball and the racket as shown in Fig. 5, y b i is the y coordinate of the ball at the ith hit, θ b i is the incident angle at the ith hit and θ x i is the racket orientation about -axis at the ith hit. The linearized discrete transition equation of y b i and θ b i around θ b i = and θ x i = is given by A. Nakashia et al. (26):

5 θ b i acket i θ x θ b i +1 y b i i +1 Fig. 5. Ball transition due to ebound at acket yb yb i + 1 = 1 2v2 b g i + 4v2 b g θ θ b 1 θ x i, (25) b 2 := (y i) 2 + (z i) 2. Fro the transition equation (25), the controller for θ x i to stabilize (y b i, θ b i) is easily obtained as θ x i = k yp k yθ yb i y bd θ b i y b Y B, (26) y bd is the target value and k yp > and k yθ > are the control gains for y b i and θ b i to be deterined such that the eigenvalues of the closed syste is saller than Prediction of Ball State As entioned in Section 4.2, we need the point of fall of the y coordinate of the ball p bx at the ithe hit. Furtherore, fro (14), (17), (23) and (25), we need the ball velocity just before the ith hit x i, y i and z i. Fro (26), we also need the ball position y b i and the incident angle θ b i. In this subsection, a siple prediction ethod of these variables is proposed. Note here that p bx is described by x b i. As shown in Fig. 6, (x b (t), y b (t), z b (t)) are the ball position and (x (t), y (t), z (t)) are the ball velocity at tie t between the i 1th and ith hits. y b (t) and y (t) are oitted for siplicity. Suppose the position and velocity of the ball can be detected by the visual sensor. Fro Assuption 1, the velocity (x i, y i) at the ith hit are the sae as (x (t), z (t)). For the velocity z i, the predicted z velocity z i can be obtained fro z (t) and z b (t) before the ith hit based on the energy conservation law: z i = g(z b (t) p h ) + vbz 2 (t) (27) Furtherore, the predicted incident angle θ b i can be obtained fro y i and z i by θ b i = tan 1 (y i/ z i). On the other hand, the predicted point of fall p bx := x b i can be obtained { as l ew v x b i = bx (t)(t T w ) (x > ) x(t) + v x (t)t (x < ), (28) T := z(t) + vbz 2 (t) + 2g(z b(t) p h ), T w := l x b(t) g x (t). (29) T is the tie interval fro tie t to the tie of the ith hit and T w is the tie interval fro tie t to the tie of the ith rebound fro the wall. Note that y b i can be easily predicted by the sae ethod as (28) without the rebound fro the wall. 5. EXPEIMENTAL ESULT The effectiveness of the proposed ethod is shown by an experiental result. The physical paraeters of the robot are shown in Fig 7. J i and W i (i = 1,, 6) denote the ith joint and the center of ass of the ith link respectively. The values of W 1, W 2, W 3, W 4, W 56 are 6.61, 6.6, 3.8, 2. and 2.18 kg respectively. the restitution coefficients e and e w are 4 and.91. The distance of the wall l is set to.9. The sapling period for the anipulator control is 12Hz. The racket is a square plate ade fro aluinu. The sides, the thickness and the ass are 15, 3 and.2kg respectively. The ball is a ping-pong ball with the radius and the ass being 35 and kg. The vision syste is coposed of two CCD caeras and the iage processing syste. The pixel and the focal length of the each caera are 768(H) 498(W) and 4.8 respectively. The iage processing syste can easure the (t) z z b (t) x (t) J W W 56, J 5 W 4, J 4 W 3 J 3 1 ˆ i 1 ˆ i W W 1, J 2 B ph Y ˆ i 1 B x b x b (t) xˆ b i = x b ( t + T ) l 5 J Fig. 6. Prediction of Ball State Fig. 7. The physical paraeters of the anipulator

6 juggling start Fault.9.9 x y z.6 z juggling start z Target point.4 Target point ts x y Fig. 8. Experiental result center of the ball based on its brightness. The sapling period is 12Hz. The target values are set to x bd =.65, y bd =.25 and B p h =.35. The control gains are set to k yp = π, k yθ =.625 and k h =.2. The equilibriu value for the linearization θ y is 15deg. Since enough height of the ball is necessary for the wall juggling, the wall juggling is started when the apex of the ball is higher than.75. The experiental result is shown in Fig. 8. The left figure shows the trajectories of the ball and racket positions in the tie interval 15s. The solid and the dashed trajectories represent the ball and the racket respectively. The solid horizontal lines represent the desired hitting values. The wall juggling was started at tie t = 2.2s and stopped a tie t = 14.25s because the robot couldn t hit the ball against the wall. We can confir that the wall juggling was achieved 19 ties. We can also confir that the x and y coordinates of the racket follow those of the ball and the z coordinate of the racket is syetric to the ball with respect to the horizontal surface B p h =.35. This result shows the effectiveness of the juggling preservation by controlling the racket position. On the other hand, the right figure shows the ball trajectory fro the 1st to 5th hits ( s) in the (x, z) and (y, z) planes. The blue circle is the initial position and the red circle is the desired hitting point. We can confir that the x and y coordinates of the hitting point stay near the target point. However, the average x and y positions have the errors 7.9c and 5.4c fro the desired hitting position. As for this reason, the air resistance, the calibulation errors and the uncertainty of the robot odel can effect on the errors of the hitting position. 6. CONCLUSION respect to a horizontal plane. The ball regulation proble was achieved by controlling the racket orientation, which was deterined based on a discrete transition equation of the hitting position of the ball. The effectiveness of the proposed ethod was shown by an experiental result. The proposed ethod can adjust the ball height but not control it copletely. It is our further work because the control of the height iprove the stability of the wall juggling. EFEENCES M. Buehler, D. E. Koditschek and P. J. Kindlann. Planning and Control of obotic Juggling and Catching Tasks, Int. J. obot. es., Vol. 13, No. 2, pp , Mori, K. Hashioto, F. Takagi and F. Miyazaki, Exaination of Ball Lifting Task using a Mobile obot, Proc. IEEE/SJ Inter. Conf. Int. obot. Sys., pp , 25. A. Nakashia, Y. Sugiyaa and Y. Hayakawa: Paddle Juggling of one Ball by obot Manipulator with Visual Servo, Proc. ICACV 26, pp , 26. T. M. H. Dijkstra, H. Katsuata, A. de ugy and D. Sternad, The dialogue between data and odel: Passive stability and relaxation behavior in a ball bouncing task, Nonlinear Studies, Vol. 11, No. 3, pp , 24. M. Takeuchi, F Miyazaki, M. Matsushia, M. Kawatani and T. Hashiato, Dynaic Dexterity for the Perforance of -Bouncing Tasks, Proc. ICA 22, pp , 22. B. K. Ghosh, N. Xi and T. J. Tarn, Control in obotics and Autoation: Sensor-Based Integration, Acadeic Press, This paper proposed a ethod to achieve wall juggling of a ball without rebound fro a table by a racket attached to a robot anipulator with two visual caera sensors. The proposed ethod was coposed of juggling preservation proble and ball regulation proble. The juggling preservation proble was achieved by the tracking control of the racket position for a syetry trajectory of the ball with