8 Velocity Kinematics

Transcription

1 8 Velocity Kinematics Velocity analysis of a robot is divided into forward and inverse velocity kinematics. Having the time rate of joint variables and determination of the Cartesian velocity of end-effector in the global coordinate frame is the forward velocity kinematics. Determination of the time rate of joint variables based on the velocity of end-effector is the inverse velocity kinematics. z i- z i α i z i-2 B i-2 θ& i B i- Link (i-) Joint i θ& i B i Link (i) Link (i+) Joint i+ Joint i- d i a i o i- y i θ i x i- o i. d i x i x i+ FIGURE 8.. Three connected moving links with their relative velocities. 8. F Rigid Link Velocity Every link of an industrial robot has an angular velocity ω or a translational velocity ḋ with respect to its neighbor links as shown in Figure 8.. The angular velocity of link (i) in the global coordinate frame B is a summation of global angular velocities of the links (j), forj i ω i ix j j ω j (8.) R.N. Jazar, Theory of Applied Robotics, 2nd ed., DOI.7/ _8, Springer Science+Business Media, LLC 2

2 Velocity Kinematics where ½ θj ˆkj if joint i is R j ω j (8.2) if joint i is P. The velocity of the origin of B i attached to link (i) inthebasecoordinate frame is ½ ω i i i ḋi d i if joint i is R d i ˆki + ω i i d (8.3) i if joint i is P where θ and d are DH parameters, and d is a frame s origin position vector. Therefore, if a robot has n links, the global angular velocity of the final coordinate frame is ω n nx i i ω i (8.4) and the global velocity vector of the last link s coordinate frame is ḋn nx i i ḋi. (8.5) Proof. According to the DH definition, the position vector of the coordinate frame B i with respect to B i is i d i d i ˆki + a i î i (8.6) which depends on joint variables q j,forj i, and therefore, i ḋi is a function of joint velocities q j,forj i. Assume that every joint of a robot except joint i is locked. Then, the angular velocity of link (i) connecting via a revolute joint to link (i ) is: i ω i θ i ˆki if joint i is R and is the only movable joint (8.7) However, if the link (i) and(i ) are connecting via a prismatic joint then, i ω i if joint i is P and is the only movable joint. (8.8) The relative position vector (8.6) shows that the velocity of link (i) connecting via a revolute joint to link (i ) is i ḋi θ i ˆki a î i i i ω i i d i if joint i is R and is the only movable joint. (8.9) We could substitute a i î i with i d i because the x i -axis is turning about the z i -axis with angular velocity θ i and therefore, θ i ˆki d i ˆki. However, if the link (i) and(i ) are connecting via a prismatic joint then, i ḋi d i ˆki if joint i is P and is the only movable joint. (8.)

3 8. Velocity Kinematics 439 Now assume that all lower joints j i are moving. Then, the angular velocity of link (i) in the base coordinate frame is ω i ix j j ω j (8.) or ω i ix j θ i ˆki which can be written in a recursive form if joint j is R if joint j is P (8.2) ω i ω i + i ω i (8.3) or ½ ω ω i i + θ i ˆki if joint i is R (8.4) ω i if joint i is P. The velocity of the origin of the link (i) in the base coordinate frame is ½ ω i i i ḋi d i if joint i is R d i ˆki + ω i i d i if joint i is P. (8.5) The translation and angular velocities of the last link of an n link robot is then a direct application of these results. Example 239 F Serial rigid links angular velocity. Consider a serial manipulator with n links and n revolute joints. The global angular velocity of link (i) in terms of the angular velocity of its previous links is ω i ω i + θ i ˆki (8.6) or in general because ω i ix j θ j ˆkj (8.7) i ω i θ i ˆki (8.8) i 2ω i θ i ˆki 2 (8.9) i 2ω i i 2ω i + i ω i i 2ω i + θ i ˆki θ i ˆki 2 + θ i ˆki (8.2)

4 44 8. Velocity Kinematics and therefore, ω i Xi j j ω j + θ Xi i ˆki j θ j ˆkj + θ i ˆki ix θ j ˆkj. (8.2) j Example 24 F Serial rigid links translational velocity. The global angular velocity of link (i) in a serial manipulator in terms of the angular velocity of its previous links is v i v i + i ω i i d i (8.22) where or in general because v i ix j v i ḋ i (8.23) ³ ˆkj i d i θj (8.24) i v i ω i i d i (8.25) i 2v i ω i i 2d i (8.26) and therefore, i 2v i v i Xi j i 2v i + i v i i 2v i + ω i i d i ω i i 2d i + ω i i d i θ i ˆki 2 i 2d i + θ i ˆki i d i (8.27) j v j + i v i ix j ³ ˆkj i d i θj. (8.28) Example 24 F Recursive velocity in Base frame The time derivative of a homogeneous transformation matrix [T ] can be arranged in the form T [V ][T] (8.29) where [T ] may be a link transformation matrix or the result of the forward kinematics of a robot as T 6 T T 2 2 T 3 3 T 4 4 T 5 5 T 6. (8.3)

5 8. Velocity Kinematics 44 Therefore, the time derivative of a transformation matrix can be computed when the velocity transformation matrix [V ] is calculated. The transformation matrix T i is and the matrices V i and V i+ are T i T T 2 2 T 3 i T i (8.3) V i T i T i (8.32) V i+ T i+ Ti+ d dt T i i T i+ T i+ ³ T i i T i+ + T i i T i+ T i+. (8.33) These two equations can be combined as a recursive formula V i+ V i + T i i V i+ T i. (8.34) The recursive velocity transformation matrix formula can be simplified according to the type of joints connecting two links. For a revolute joint, the velocity transformation matrix is i V i+ q i+ R θ i+ R θ i+ θ i ki i+ (8.35) then we have ω i+ ω i + i ω i+ ω i + θ i+ i ˆki+ (8.36) which shows that the angular velocity of frame B i+ is the angular velocity of frame B i plus the relative angular velocity produced by joint variable q i+. Furthermore, ḋ i+ ḋ i + i ḋi+ ḋ i + ω i+ d i+ d i (8.37) which shows the translational velocity is obtained by adding the translational velocity of frame B i to the contribution of rotation of link B i+. For a prismatic joint, the velocity coefficient matrix formula is i V i+ q i+ P d i+ R d i+ d i+ i ˆki (8.38)

6 Velocity Kinematics and therefore, we have ω i+ ω i (8.39) and ḋ i+ ḋ i + ω i+ d i+ d i + d i+ i ˆki+ (8.4) which shows that the angular velocity of frame B i+ isthesameastheangular velocity of frame B i. Furthermore, the translational velocity is obtained by adding the translational velocity due to d i+ to the relative velocity due to rotation of link B i Forward Velocity Kinematics The forward velocity kinematics of a robot solves the problem of relating joint speeds, q, to the end-effector speeds Ẋ. The joint speed vector q of an ndof robot is an n vector q q n q n q n q n T (8.4) and the end-effector speed vector Ẋ, in the most general case, is a 6 vector. Ẋ T Ẋ n Ẏ n Ż n ω Xn ω Yn ω Zn ḋ n v n ω n ω n (8.42) The elements of end-effector speed vector the elements of joint speed vector, q, Ẋ are linearly proportional to Ẋ J q (8.43) where, the 6 n proportionality matrix J(q) is called the Jacobian matrix of the robot. The global-expression of velocity v n of the origin of B n is proportional to the manipulator joint speeds q D. v n J D q D, q D q (8.44) The 3 n proportionality matrix J D (q) is the displacement Jacobian matrix of the manipulator. J D d n ( q D ) T (q) (8.45) q D q The global-expression of angular velocity ω n of B n is proportional to the rotational components of q. ω n J R q (8.46)

7 8. Velocity Kinematics 443 The 3 n proportionality matrix J R (q) is the rotational Jacobian matrix of the robot. J R ω n (8.47) q We may combine Equations (8.44) and (8.46) to show the forward velocity kinematics of a robot by (8.42). Proof. The forward velocity kinematics is: determination of the end-effector translational and angular velocities, v n, ω n, for a given set of joint speeds q i, i, 2,,n. The components of velocity vectors v n and ω n are proportional to the joint speeds q i, i, 2,,n. v n J D q (8.48) ω n J R q (8.49) The proportionality matrices J D and J R are called the displacement and rotational Jacobians. We may combine Equations (8.48) and (8.49) as Ẋ J q (8.5) by defining the Jacobian matrix J and the vectors Ẋ and q, knownas end-effector speed vector, andjoint speed vector, respectively. JD J (8.5) J R v Ẋ n (8.52) ω n q T q n q n q n q n (8.53) We may also show J D by J, whenever we analyze the velocity kinematics of a manipulator without a wrist. Consider a robot with 6 DOF that is made of a 3 DOF manipulator to position the wrist point, and a spherical wrist with 3 DOF to orient the end-effector. The coordinate transformation of a point in the end-effector coordinate frame B 6 and the base coordinate frame B is: R 6 d 6 I d 6 r T 6 (q) 6 r D R 6 R r (8.54) where, the transformation matrix T 6 is a function of 6 joint variables q i, i, 2,, 6. We can always divide the 6 joint variables into the endeffector position variables q,q 2,q 3 and end-effector orientation variables q 4,q 5,q 6. The end-effector position variables are the only variables in the

8 Velocity Kinematics position vector d 6, and the end-effector orientation variables are the only variables in the rotation transformation matrix R 6. d 6 d 6 (q,q 2,q 3 ) (8.55) R 6 R 6 (q 4,q 5,q 6 ) (8.56) The origin of end-effector frame B 6 is at 6 r which is globally at: r R 6 d 6 I d 6 D 6 d 6 (8.57) The components of end-effector displacement vector d 6 X Y Z are functions of manipulator joint variables q,q 2,q 3. X Y d (q,q 2,q 3 ) d 2 (q,q 2,q 3 ) (8.58) Z d 3 (q,q 2,q 3 ) Taking a derivative of both sides indicates that each component of v 6 ṙ is a linear combination of q, q 2, q 3. Ẋ d q q + d q 2 q 2 + d q 3 q 3 Ẏ d 2 q q + d 2 q 2 q 2 + d 2 q 3 q 3 Ż d 3 q + d 3 q 2 + d 3 q 3 (8.59) q q 2 q 3 It indicates that v 6 is a linear combination of joint speeds q,q 2,q 3. d 6 d 6 d 6 v 6 J D q D q + q 2 + q 3 (8.6) q q 2 q 3 We may show these relations by vector and matrix expressions. v 6 d 6 q q D J D q D (8.6) d d d q q 2 q 3 d 2 d 2 d 2 q q q 2 q 3 q 2 (8.62) d 3 d 3 d 3 q 3 q q 2 q 3 Ẋ Ẏ Ż The displacement Jacobian J D is equivalent to the derivative of T with respect to the manipulator joint coordinates. J D d 6 q D 6 q T 6 q T (q) q (8.63)

9 8. Velocity Kinematics 445 The angular velocity of the end-effector is: ω 6 Ṙ 6 R T 6 (8.64) However, the time derivative of the rotational transformation matrix is: Ṙ 6 d R R 2 2 R 3 3 R 4 4 R 5 5 R 6 dt (8.65) q R q R 2 2 R 3 3 R 4 4 R 5 5 R 6 + q 2 R R 2 q 2 2 R 3 3 R 4 4 R 5 5 R 6 + q 3 R R 2 2 R 3 q 3 3 R 4 4 R 5 5 R 6 + q 4 R R 2 2 R 3 3 R 4 q 4 4 R 5 5 R 6 + q 5 R R 2 2 R 3 3 R 4 4 R 5 q 5 5 R 6 + q 6 R R 2 2 R 3 3 R 4 4 R 5 5 R 6 q 6 and the transpose of R 6 is: R T 6 R R 2 2 R 3 3 R 4 4 R 5 5 R 6 T 5 R T 6 4 R T 5 3 R T 4 2 R T 3 R T 2 R T (8.66) Therefore, ω 6 Ṙ 6 R T 6 is: ω 6 q R q R T + q 2 R R 2 q 2 R T 2 + q 3 R 2 2 R 3 q 3 R T 3 + q 4 R 3 3 R 4 q 4 R T 4 + q 4 R 5 5 R 4 R5 T + q 5 R 6 6 R 5 R6 T q 5 q 6 (8.67) ω + ω 2 + 2ω 3 + 3ω 4 + 4ω 5 + 5ω 6 (8.68) It indicates that ω 6 is a linear combination of joint speeds q i, i, 2,, 6 where, ω 6 J R q q ω 6 q + q 2 ω 6 q 2 + q 3 ω 6 q 3 + q 4 ω 6 q 4 + q 5 ω 6 q 5 + q 6 ω 6 q 6 (8.69) ω 6 R R T q q (8.7) ω 6 R 2 R R2 T q 2 q 2 (8.7) ω 6 2 R 3 R 2 R3 T q 3 q 3 (8.72)

10 Velocity Kinematics ω 6 3 R 4 R 3 R4 T q 4 q 4 (8.73) ω 6 4 R 5 R 4 R5 T q 5 q 5 (8.74) ω 6 5 R 6 R 5 R6 T. q 6 q 6 (8.75) Combination of the translational and rotational velocities makes the equation (8.5) for the velocity kinematics of the robot. v Ẋ n JD q J q ω n J R (8.76) The Jacobian matrix of the robot is: d 6 d 6 d 6 J q q 2 q 3 ω 6 ω 6 ω 6 ω 6 ω 6 ω (8.77) 6 q q 2 q 3 q 4 q 5 q 6 In case the robot has n links and joints, the above equations go from to n instead of to n. So in general case, the 6 n Jacobian matrix J becomes: J d n q ω n q d n q 2 ω n q 2 d n q 3 ω n q 3 d n q n ω n q n (8.78) Example 242 Jacobian matrix for a planar polar manipulator. Figure 8.2 illustrates a planar polar manipulator with the following forward kinematics. T 2 T T 2 cos θ sin θ sin θ cos θ cos θ sin θ r cos θ sin θ cos θ r sin θ r (8.79) The tip point of the manipulator is at X r cos θ Y r sin θ (8.8)

11 8. Velocity Kinematics 447 y 2 x z 2 y r y θ x FIGURE 8.2. A planar polar manipulator. and therefore, its velocity is Ẋ cos θ Ẏ sin θ r sin θ rcos θ ṙ θ (8.8) which shows that J D X r Y r X θ Y θ cos θ sin θ r sin θ rcos θ. (8.82) There is only one rotational joint coordinate, θ. The rotation matrix R 2 indicates that: ω 2 Ṙ 2 R T 2 θ k (8.83) So, and therefore, Ẋ Ẏ ω 3 ω 2 ω ω 2 ω 3 θ (8.84) ω 3 J R θ (8.85) J R (8.86) cos θ r sin θ sin θ rcos θ ṙ θ (8.87)

12 Velocity Kinematics y 2 x 2 y y l 2 l θ 2 θ x x FIGURE 8.3. A 2R planar manipulator. Example 243 Jacobian matrix for the 2R planar manipulator. A 2R planar manipulator with two RkR links was illustrated in Figure 5.9 and is shown in Figure 8.3 again. The manipulator has been analyzed in Example 4 for forward kinematics, and in Example 84 for inverse kinematics. The angular velocity of links () and (2) are ω θ ˆk (8.88) ω 2 θ 2 ˆk (8.89) and ω 2 ω + ω 2 ³ θ + 2 θ ˆk (8.9) and the global velocity of the tip position of the manipulator is ḋ 2 ḋ + ḋ2 ω d + ω 2 d 2 θ ˆk l î + ³ θ + θ 2 ˆk l 2 î 2 l θ ĵ l 2 ³ θ + θ 2 ĵ 2. (8.9) The unit vectors ĵ and ĵ 2 can be found by using the coordinate transformation method, ĵ R ĵ Z,θ cos θ sin θ sin θ cos θ sin θ cos θ (8.92)

13 8. Velocity Kinematics 449 ĵ 2 R 2ĵ Z,θ +θ 2 2 cos (θ + θ 2 ) sin (θ + θ 2 ) sin (θ + θ 2 ) cos(θ + θ 2 ) sin (θ + θ 2 ) cos (θ + θ 2 ). (8.93) Substituting back shows that ḋ 2 l θ sin θ cos θ l 2 ³ θ + 2 θ sin (θ + θ 2 ) cos (θ + θ 2 ) (8.94) which can be rearranged to have Ẋ l sθ l 2 s (θ + θ 2 ) l 2 s (θ + θ 2 ) Ẏ l cθ + l 2 c (θ + θ 2 ) l 2 c (θ + θ 2 ) J D θ θ 2 θ θ 2. (8.95) Taking advantage of the structural simplicity of the 2R manipulator, we may find its Jacobian simpler. The forward kinematics of the manipulator was found as: T 2 T T 2 (8.96) c (θ + θ 2 ) s (θ + θ 2 ) l cθ + l 2 c (θ + θ 2 ) s (θ + θ 2 ) c (θ + θ 2 ) l sθ + l 2 s (θ + θ 2 ) which shows the tip position d 2 of the manipulator is at X l cos θ + l 2 cos (θ + θ 2 ). (8.97) Y l sin θ + l 2 sin (θ + θ 2 ) Direct differentiating gives Ẋ l θ sin θ l 2 ³ θ + 2 θ sin (θ + θ 2 ) Ẏ l θ cos θ + l 2 ³ θ + 2 θ cos (θ + θ 2 ) (8.98) which can be rearranged in a matrix form Ẋ l sθ l 2 s (θ + θ 2 ) l 2 s (θ + θ 2 ) Ẏ l cθ + l 2 c (θ + θ 2 ) l 2 c (θ + θ 2 ) θ θ 2 (8.99)

14 45 8. Velocity Kinematics or Ẋ J D θ. (8.) J D is the Jacobian of the 2R manipulator. X X J D θ θ 2 Y Y (8.) θ θ 2 l sin θ l 2 sin (θ + θ 2 ) l 2 sin (θ + θ 2 ) l cos θ + l 2 cos (θ + θ 2 ) l 2 cos (θ + θ 2 ) Employing the absolute slope angles of the links, θ, θ + θ 2,wecanalso write the velocity equation of a 2R manipulator as: Ẋ l sin θ l 2 sin (θ + θ 2 ) θ Ẏ l cos θ l 2 cos (θ + θ 2 ) θ + θ (8.2) 2 Example 244 Columns of the Jacobian for the 2R manipulator. The Jacobian of the 2R planar manipulator can be found systematically by using the column-by-column method. The global position vector of the coordinate frames are: and therefore, d 2 l î 2 2 (8.3) d 2 l î + l î 2 2 (8.4) ḋ 2 ω d 2 + ω 2 d 2 θ ˆk l î + l î θ2 ˆk l î 2 2 ˆk l î + l î 2 2 ˆk l î θ 2 2 θ 2 (8.5) which can be set in the following form. ḋ 2 ˆk d 2 ˆk d 2 θ ω 2 ˆk ˆk θ 2 J θ (8.6) Example 245 An articulated manipulator. Figure 8.4 illustrates an articulated manipulator. The links of the manipulator are R`R(9), RkR(), R`R(9), and their associated transformation matrices between coordinate frames are: T cos θ sinθ sin θ cos θ l (8.7)

15 l 2 8. Velocity Kinematics 45 z x 2 θ x 3 l 3 P z 3 d P θ 3 z 2 x l θ 2 z y x FIGURE 8.4. An R`RkR articulated manipulator. T 2 2 T 3 cos θ 2 sin θ 2 l 2 cos θ 2 sin θ 2 cos θ 2 l 2 sin θ 2 cos θ 3 sinθ 3 sin θ 3 cos θ 3 (8.8) (8.9) Let us show the tip point of the manipulator by P. The global coordinates of P is: r P X P Y P Z P T 3 l 3 cos θ (l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 )) sin θ (l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 )) l l 3 cos (θ 2 + θ 3 )+l 2 sin θ 2 (8.) The coordinates of r P must be used to determine the Jacobian of the manipulator. X P X P X P θ θ 2 θ 3 J Y P Y P Y P θ θ 2 θ 3 (8.) Z P Z P Z P θ θ 2 θ 3

16 Velocity Kinematics X P θ (l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 )sinθ Y P θ (l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 )cosθ Z P θ (8.2) X P θ 2 (l 3 cos (θ 2 + θ 3 ) l 2 sin θ 2 )cosθ Y P θ 2 (l 3 cos (θ 2 + θ 3 ) l 2 sin θ 2 )sinθ Z P θ 2 l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 (8.3) X P θ 3 l 3 cos (θ 2 + θ 3 )cosθ Y P θ 3 l 3 cos (θ 2 + θ 3 )sinθ Z P θ 3 l 3 sin (θ 2 + θ 3 ) (8.4) J (l 3sθ 23 + l 2 cθ 2 ) sθ (l 3 cθ 23 l 2 sθ 2 ) cθ l 3 cθ 23 cθ (l 3 sθ 23 + l 2 cθ 2 ) cθ (l 3 cθ 23 l 2 sθ 2 ) sθ l 3 cθ 23 sθ (8.5) l 3 sθ 23 + l 2 cθ 2 l 3 sθ 23 θ 23 θ 2 + θ 3 (8.6) 8.3 Jacobian Generating Vectors The Jacobian matrix is a linear transformation, mapping joint speeds to Cartesian speeds Ẋ J q (8.7) and is equal to ˆk J d n ˆk d n ˆkn n d n. (8.8) ˆk ˆk ˆkn J canbecalculatedcolumnbycolumn.theith column of J is called Jacobian generating vector and is denoted by c i (q). c i (q) ˆki i d n ˆki (8.9)

17 8. Velocity Kinematics 453 To calculate the ith column of the Jacobian matrix, we need to find two vectors i d n and ˆki. These vectors are position of origin and the joint axis unit vector of the frame attached to link (i ),bothexpressedinthe base frame. Calculating J, based on the Jacobian generating vectors, shows that forward velocity kinematics is a consequence of the forward kinematics of robots. Proof. Let d i and d i be the global position vector of the frames B i and B i, while i d i is the position vector of the frame B i in B i as shown in Figure 8.5. These three position vectors are related according to avectoraddition d i d i + R i i d i d i + d i ˆki + a i î i. (8.2) in which we have used Equation (8.6). Taking a time derivative, ḋ i ḋ i + Ṙ i i d i + R i i ḋ i ḋ i + Ṙ i ³ d i i ˆki + a i i î i + R i d i i ˆki (8.2) shows that the global velocity of the origin of B i is a function of the translational and angular velocities of link B i.however, i ḋi ḋ i ḋ i (8.22) and Ṙ i i d i ω i R i i d i ω i i d i θ i ˆki i d i (8.23) R i d i i ˆki d i R i i ˆki d i ˆki (8.24) therefore, i ḋi θ i ˆki i d i + d i ˆki. (8.25) Since at each joint, either θ or d is variable, we conclude that i ḋi ω i i d i if joint i is R (8.26) or i ḋi d i ˆki + ω i i d i if joint i is P. (8.27)

18 Velocity Kinematics B i- z i- z i B i z Joint i Link (i) Joint i+ i- d i x i- x i B d i- d i x y FIGURE 8.5. Link (i) and associated coordinate frames. The end-effector velocity is then expressed by nx nx and ḋn i ω n i ḋi nx i i i ω i θ i ˆki i d n (8.28) nx θ i ˆki. (8.29) i They can be rearranged in a matrix form. ḋ nx n ˆki θ i d n i ˆki ω n i ˆk d n ˆk d n ˆkn n d n ˆk ˆk ˆkn θ θ 2. θ n (8.3) We usually show this equation by a short notation as Equation (8.7) Ẋ J q (8.3) where, the vector q q q 2 q n T is the joint speeds vector and J is the Jacobian matrix. ˆk J d n ˆk d n ˆkn n d n ˆk ˆk ˆkn (8.32)

19 8. Velocity Kinematics 455 x 2 d 3 z x 3 z z 2 z 3 x l x FIGURE 8.6. A spherical manipulator. Practically, we find the Jacobian matrix column by column. Each column is a Jacobian generating vector, c i (q), and is associated to joint i. If joint i is revolute, then ˆki c i (q) i d n (8.33) ˆki and if joint i is prismatic then, c i (q) simplifies to c i (q) ˆki. (8.34) Equation (8.7) provides a set of six equations. The first three equations relate the translational velocity of the end-effector joint speeds. The rest of the equations relate the angular velocity of the end-effector frame to the joint speeds. Example 246 Jacobian matrix for a spherical manipulator. Figure 8.6 depicts a spherical manipulator. To find its Jacobian, we start with determining the ˆk i axes for i, 2, 3. It would be easier if we use the homogeneous definitions and write, ˆk (8.35)

20 Velocity Kinematics ˆk T ˆk (8.36) cos θ sin θ sin θ sin θ cosθ l cos θ ˆk2 T 2 2ˆk2 (8.37) cθ cθ 2 sθ cθ sθ 2 cos θ sin θ 2 cθ 2 sθ cθ sθ sθ 2 sθ 2 cθ 2 l sin θ sin θ 2 cos θ 2. Then, the vectors i d n must be evaluated. d 3 l ˆk + d 3 ˆk2 d 3 d 3 ˆk2 d 3 cos θ sin θ 2 d 3 sin θ sin θ 2 l + d 3 cos θ 2 d 3 cos θ sin θ 2 d 3 sin θ sin θ 2 d 3 cos θ 2 (8.38) (8.39) Therefore, the Jacobian of the manipulator is J ˆk d 3 ˆk d 3 ˆk2 ˆk ˆk d 3 sin θ sin θ 2 d 3 cos θ cos θ 2 cos θ sin θ 2 d 3 cos θ sin θ 2 d 3 cos θ 2 sin θ sin θ sin θ 2 d 3 sin θ 2 cos θ 2 sin θ cosθ. (8.4) Example 247 Jacobian matrix for an articulated robot. The Jacobian matrix of a 6 DOF articulated robot is a 6 6 matrix. The robot was shown in Figure 6.6 and its transformation matrices are calculated in Example 86. The ith column of the Jacobian, c i (q), is c i (q) ˆki i d 6 ˆki. (8.4)

21 8. Velocity Kinematics 457 For the first column of the Jacobian matrix, we need to find ˆk and d 6. The direction of the z -axis in the base coordinate frame is ˆk (8.42) and the position vector of the end-effector frame B 6 is d 6,whichcanbe directly determined from the fourth column of the transformation matrix, T 6, T 6 T T 2 2 T 3 3 T 4 4 T 5 5 T 6 R 6 d 6 t t 2 t 3 t 4 t 2 t 22 t 23 t 24 t 3 t 32 t 33 t 34 (8.43) which is d 6 t 4 t 24 (8.44) t 34 where, t 4 d 6 (sθ sθ 4 sθ 5 + cθ (cθ 4 sθ 5 c (θ 2 + θ 3 )+cθ 5 s (θ 2 + θ 3 ))) +l 3 cθ s (θ 2 + θ 3 )+d 2 sθ + l 2 cθ cθ 2 t 24 d 6 ( cθ sθ 4 sθ 5 + sθ (cθ 4 sθ 5 c (θ 2 + θ 3 )+cθ 5 s (θ 2 + θ 3 ))) +sθ s (θ 2 + θ 3 ) l 3 d 2 cθ + l 2 cθ 2 sθ t 34 d 6 (cθ 4 sθ 5 s (θ 2 + θ 3 ) cθ 5 c (θ 2 + θ 3 )) +l 2 sθ 2 + l 3 c (θ 2 + θ 3 ). (8.45) Therefore, ˆk d 6 t 4 t 24 t 24 t 4 t 34 (8.46) and the first Jacobian generating vector is: c ˆk d 6 ˆk t 24 t 4 (8.47)

22 Velocity Kinematics For the 2nd column we need to find ˆk and d 6.Thez -axis in the base frame can be found by ˆk R ˆk R cθ sθ sθ cθ sin θ cos θ. (8.48) The first half of c 2 is ˆk d 6.Thevector d 6 is the position of the end-effector in the coordinate frame B, however it must be expressed in the base frame to be able to perform the cross product. An easier method is to find ˆk d 6 and transform the resultant into the base frame. The vector d 6 is the fourth column of T 6 T 2 2 T 3 3 T 4 4 T 5 5 T 6,which,from Example 86, is equal to d 6 l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 ) l 2 sin θ 2 l 3 cos (θ 2 + θ 3 ). (8.49) d 2 Therefore, the first half of c 2 is ˆk d 6 R ³ ˆk d 6 and c 2 is: c 2 R ˆk d 6 ˆk l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 ) l 2 sin θ 2 l 3 cos (θ 2 + θ 3 ) d 2 cos θ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) sin θ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 ) cos θ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) sin θ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 ) sin θ cos θ (8.5) (8.5) The 3rd column is made by ˆk 2 and 2d 6. The vector 2d 6 is position of the end-effector in the coordinate frame B 2 and is the fourth column of 2 T 6 2 T 3 3 T 4 4 T 5 5 T 6.Thez 2 -axis in the base frame can be found by ˆk2 R 2 2ˆk2 R R 2 sin θ cos θ (8.52)

23 8. Velocity Kinematics 459 and the cross product ˆk 2 2d 6 can be found by transforming the resultant of 2ˆk 2 2 d 6 into the base coordinate frame. 2ˆk2 2 d 6 l 3 cos θ 3 l 3 sin θ 3 (8.53) ³ ˆk2 2d 6 R 2 2ˆk2 2 d 6 l 3 cos θ sin (θ 2 + θ 3 ) l 3 sin θ sin (θ 2 + θ 3 ) (8.54) l 3 cos (θ 2 + θ 3 ) Therefore, c 3 is: l 3 cos θ sin (θ 2 + θ 3 ) l 3 sin θ sin (θ 2 + θ 3 ) ˆk2 c 3 2d 6 l 3 cos (θ 2 + θ 3 ) ˆk2 sin θ (8.55) cos θ The 4th column needs ˆk 3 and 3d 6.Thevector ˆk 3 can be found by transforming 3ˆk 3 to the base frame ˆk3 R R 2 2 R 3 cos θ (cos θ 2 sin θ 3 +cosθ 3 sin θ 2 ) sin θ (cos θ 2 sin θ 3 +sinθ 2 cos θ 3 ) cos (θ 2 + θ 3 ) (8.56) and the first half of c 4 can be found by calculating 3ˆk3 3 d 6 and transforming the resultant into the base coordinate frame. ³ R 3 3ˆk3 3 d 6 R 3 (8.57) l 3 Therefore, c 4 is: c 4 ˆk3 3d 6 ˆk3 cos θ (cos θ 2 sin θ 3 +cosθ 3 sin θ 2 ) sin θ (cos θ 2 sin θ 3 +sinθ 2 cos θ 3 ) cos (θ 2 + θ 3 ) (8.58) The 5th column needs ˆk4 and 4 d 6.Wecanfind the vector ˆk4 by transforming 4ˆk4 to the base frame. ˆk4 R 4 cθ 4sθ cθ sθ 4 c (θ 2 + θ 3 ) cθ cθ 4 sθ sθ 4 c (θ 2 + θ 3 ) (8.59) sθ 4 s (θ 2 + θ 3 )

24 46 8. Velocity Kinematics The first half of c 5 is 4ˆk 4 4 d 6,expressedinthebasecoordinateframe. ³ R 4 4ˆk4 4 d 6 R 4 (8.6) Therefore, c 5 is: c 5 ˆk4 4d 6 ˆk4 cos θ 4 sin θ cos θ sin θ 4 cos (θ 2 + θ 3 ) cos θ cos θ 4 sin θ sin θ 4 cos (θ 2 + θ 3 ) sin θ 4 sin (θ 2 + θ 3 ) (8.6) The 6th column is found by calculating ˆk 5 and ˆk 5 5d 6. The vector ˆk5 is ˆk5 R 5 (8.62) cθ cθ 4 s (θ 2 + θ 3 ) sθ 4 (sθ sθ 4 + cθ cθ 4 c (θ 2 + θ 3 )) sθ cθ 4 s (θ 2 + θ 3 ) sθ 4 ( cθ sθ 4 + sθ cθ 4 c (θ 2 + θ 3 )) cθ 4 c (θ 2 + θ 3 ) 2 s (θ 2 + θ 3 ) s2θ 4 and the first half of c 6 is 5ˆk5 5 d 6, expressed in the base coordinate frame. ³ R 5 5ˆk5 5 d 6 R 5 (8.63) Therefore, c 6 is c 6 ˆk5 5d 6 (8.64) ˆk5 cθ cθ 4 s (θ 2 + θ 3 ) sθ 4 (sθ sθ 4 + cθ cθ 4 c (θ 2 + θ 3 )) sθ cθ 4 s (θ 2 + θ 3 ) sθ 4 ( cθ sθ 4 + sθ cθ 4 c (θ 2 + θ 3 )) cθ 4 c (θ 2 + θ 3 ) 2 s (θ 2 + θ 3 ) s2θ 4 and the Jacobian matrix for the articulated robot is calculated. J c c 2 c 3 c 4 c 5 c 6 (8.65)

25 8. Velocity Kinematics 46 Example 248 The effect of a spherical wrist on Jacobian matrix. The Jacobian matrix for a robot having a spherical wrist is always of the form J ˆk d 6 ˆk d 6 ˆk2 2d 6 ˆk ˆk ˆk2 ˆk3 ˆk6 ˆk5 (8.66) which shows the upper 3 3 submatrix is zero. This is because of the spherical wrist structure and having a wrist point as the origin of the wrist coordinate frames B 4, B 5,andB 6. Example 249 F Jacobian matrix for an articulated manipulator using the direct differentiating method. Figure 6.6 illustrates an articulated robot with transformation matrices given in Example 86. Using the result of the forward kinematics T 6 R 6 d 6 t t 2 t 3 t 4 t 2 t 22 t 23 t 24 t 3 t 32 t 33 t 34 we know that the position of the end-effector is at where, (8.67) d 6 X 6 Y 6 t 4 t 24 (8.68) Z 6 t 34 t 4 d 6 (sθ sθ 4 sθ 5 + cθ (cθ 4 sθ 5 c (θ 2 + θ 3 )+cθ 5 s (θ 2 + θ 3 ))) +l 3 cθ s (θ 2 + θ 3 )+d 2 sθ + l 2 cθ cθ 2 t 24 d 6 ( cθ sθ 4 sθ 5 + sθ (cθ 4 sθ 5 c (θ 2 + θ 3 )+cθ 5 s (θ 2 + θ 3 ))) +sθ s (θ 2 + θ 3 ) l 3 d 2 cθ + l 2 cθ 2 sθ t 34 d 6 (cθ 4 sθ 5 s (θ 2 + θ 3 ) cθ 5 c (θ 2 + θ 3 )) +l 2 sθ 2 + l 3 c (θ 2 + θ 3 ). (8.69) Taking the derivative of X 6 yields Ẋ 6 X 6 θ θ + X 6 θ 2 θ2 + + X 6 θ 6 θ6 J θ + J 2 θ2 + + J 6 θ6 t 24 θ +cosθ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) θ 2 +l 3 cos θ sin (θ 2 + θ 3 ) θ 3 (8.7)

26 Velocity Kinematics that shows J t 24 J 2 cosθ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) J 3 l 3 cos θ sin (θ 2 + θ 3 ) J 4 J 5 J 6. (8.7) Similarly, the derivative of Y 6 and Z 6 Ẏ 6 Y 6 θ θ + Y 6 θ 2 θ2 + + Y 6 θ 6 θ6 J 2 θ + J 22 θ2 + + J 26 θ6 t 4 θ +sinθ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) θ 2 +l 3 sin θ sin (θ 2 + θ 3 ) θ 3 (8.72) show that Ż 6 Z 6 θ θ + Z 6 θ 2 θ2 + + Z 6 θ 6 θ6 J 3 θ + J 32 θ2 + + J 36 θ6 (l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 )) θ 2 l 3 cos (θ 2 + θ 3 ) θ 3 (8.73) J 2 t 4 J 22 sinθ ( l 2 sin θ 2 + l 3 cos (θ 2 + θ 3 )) J 23 l 3 sin θ sin (θ 2 + θ 3 ) J 24 J 25 J 26 (8.74) J 3 J 32 l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 ) J 33 l 3 cos (θ 2 + θ 3 ) J 34 J 35 J 36. (8.75) There is no explicit equation for describing the rotations of the endeffector s frame about the axes. So, there is no equation to find differential

27 8. Velocity Kinematics 463 rotations about the three axes by differentiating. This is a reason for searching indirect or more systematic methods for evaluating the Jacobian matrix. However, the next three rows of the Jacobian matrix can be found by calculating the angular velocity vector based on the angular velocity matrix ω 6 Ṙ 6 R6 T ω Z ω Y ω Z ω X (8.76) ω Y ω X ω 6 ω X ω Y (8.77) ω Z and then rearranging the components to show the Jacobian elements. ω X ω X θ + ω X θ2 + + ω X θ6 (8.78) θ θ 2 θ 6 ω Y ω Y θ + ω Y θ2 + + ω Y θ6 (8.79) θ θ 2 θ 6 ω Z ω Z θ + ω Z θ2 + + ω Z θ6 (8.8) θ θ 2 θ 6 Expanding (8.76) for the articulated manipulator shows that the angular velocity vector of the end-effector frame is: ω X sinθ θ2 +sinθ θ3 +cosθ sin θ 23 θ4 +(cosθ 4 sin θ cos θ sin θ 4 cos θ 23 ) θ 5 (cθ cθ 4 sθ 23 + sθ 4 (sθ sθ 4 + cθ cθ 4 cθ 23 )) θ 6 (8.8) ω Y cos θ θ2 cos θ θ3 +sinθ sin θ 23 θ4 +( cos θ cos θ 4 sin θ sin θ 4 cos θ 23 ) θ 5 +( sθ cθ 4 sθ 23 sθ 4 ( cθ sθ 4 + sθ cθ 4 cθ 23 )) θ 6 (8.82) and therefore, ω Z θ cos (θ 2 + θ 3 ) θ 4 sin θ 4 sin (θ 2 + θ 3 ) θ 5 + µcos θ 4 cos θ 23 2 sin θ 23 sin 2θ 4 θ 6 (8.83) J 4 J 42 sinθ J 43 sinθ J 44 cosθ (cos θ 2 sin θ 3 +cosθ 3 sin θ 2 ) J 45 cosθ 4 sin θ cos θ sin θ 4 cos (θ 2 + θ 3 ) J 46 cθ cθ 4 s (θ 2 + θ 3 ) sθ 4 (sθ sθ 4 + cθ cθ 4 c (θ 2 + θ 3 )) (8.84)

28 Velocity Kinematics J 5 J 52 cos θ J 53 cos θ J 54 sinθ (cos θ 2 sin θ 3 +sinθ 2 cos θ 3 ) J 55 cos θ cos θ 4 sin θ sin θ 4 cos (θ 2 + θ 3 ) J 56 sθ cθ 4 s (θ 2 + θ 3 ) sθ 4 ( cθ sθ 4 + sθ cθ 4 c (θ 2 + θ 3 )) (8.85) J 6 J 62 J 63 J 64 cos (θ 2 + θ 3 ) J 65 sin θ 4 sin (θ 2 + θ 3 ) J 66 cosθ 4 cos (θ 2 + θ 3 ) 2 sin (θ 2 + θ 3 )sin2θ 4. (8.86) Example 25 F Analytical Jacobian and geometrical Jacobian. Assume the global position and orientation of the end-effector frames are specified by a set of six parameters r X n (8.87) φ n where φ n φ n (q) (8.88) are three independent rotational parameters such as Euler angles, and r n r n (q) (8.89) is the Cartesian position of the end-effector frame, both functions of the joint variable vector, q. The translational velocity of the end-effector frame can be expressed by ṙ n r q q J D(q) q (8.9) and the rotational velocity of the end-effector frame can be expressed by φn φ q q J φ(q) q. (8.9) The rotational velocity vector φ in general differs from the angular velocity vector ω. The combination of the displacement Jacobian matrices J D and angular Jacobian J φ in the form of JD J A (8.92) J φ

29 8. Velocity Kinematics 465 is called analytical Jacobian to indicate its difference with geometrical Jacobian J. Having a set of orientation angles, φ, it is possible to find the relationship between the angular velocity ω and the rotational velocity φ. Asanexample, consider the Euler angles ϕθψ about zxz axes defined in Section 6.3. The global angular velocity, in terms of Euler frequencies, is found in 2.3 ω X cosϕ sin θ sin ϕ ϕ ω Y sinϕ cos ϕ sin θ θ (8.93) ω Z cosθ ψ ω G R E φ. (8.94) The Eulerian frequencies ϕ, θ, ψ are functions of joint speeds ϕ θ ψ J φ q (8.95) and therefore, J R G R E J φ (8.96) JD J. (8.97) When the angular velocity of the end-effector is expressed in Cartesian frequencies as ω ω X ω Y (8.98) ω Z then, Jacobian matrix is called geometric (8.97). When the angular velocity of the end-effector is expressed in non-cartesian frequencies such as Eulerian, then Jacobian matrix is called analytic (8.92). J R 8.4 Inverse Velocity Kinematics The inverse velocity kinematics problem, alsoknownastheresolved rates problem, is searching for the joint speeds vector q corresponding to the end-effector speeds vector Ẋ. SixDOF are needed to be able to move the end-effector in an arbitrary direction with an arbitrary angular velocity. The speeds vector of the end-effector Ẋ is related to the joint speeds vector q by the Jacobian matrix J. v Ẋ n JD q J q (8.99) ω n J R

30 Velocity Kinematics Consequently, for the inverse velocity kinematics, we require the differential change in joint coordinates expressed in terms of the Cartesian translation and angular velocities of the end-effector. If the Jacobian matrix is nonsingular at the moment of calculation, the inverse Jacobian J exists and we are able to find the required joint speeds vector as: q J Ẋ (8.2) Singular configuration is where the determinant of the Jacobian matrix is zero and therefore, J is indeterminate. Equation (8.2) determines the speeds required at the individual joints to produce a desired end-effector speeds Ẋ. Since the inverse velocity kinematics is a consequence of the forward velocity and needs a matrix inversion, the problem is equivalent to the solution of a set of linear algebraic equations. To find J,everymatrix inversion method may be applied. Example 25 Inverse velocity of a planar polar manipulator. Figure 8.2 illustrates a planar polar manipulator with the following forward velocity equation. Ẋ Ẏ cos θ sin θ r sin θ rcos θ ṙ θ (8.2) To determine the inverse velocity, we need to determine the inverse of the Jacobian matrix J. X X J r θ cos θ r sin θ (8.22) Y Y sin θ rcos θ r θ J X Y r θ X Y θ r cos θ sin θ r sin θ cos θ Y θ Y r X θ X r (8.23) Therefore, ṙ θ cos θ sin θ r sin θ cos θ Ẋ Ẏ (8.24) Example 252 Inverse velocity of a 2R planar manipulator. Forward and inverse kinematics of a 2R planar manipulator have been analyzed in Example 4 and Example 84. Its Jacobian and forward ve-

31 locity kinematics are also found in Example 243 as: Ẋ Ẏ 8. Velocity Kinematics 467 Ẋ J q (8.25) l sθ l 2 s (θ + θ 2 ) l 2 s (θ + θ 2 ) (8.26) l cθ + l 2 c (θ + θ 2 ) l 2 c (θ + θ 2 ) θ θ 2 The inverse velocity kinematics needs to find the inverse of the Jacobian. Therefore, θ θ 2 q J Ẋ (8.27) l sθ l 2 s (θ + θ 2 ) l 2 s (θ + θ 2 ) Ẋ (8.28) l cθ + l 2 c (θ + θ 2 ) l 2 c (θ + θ 2 ) Ẏ where, J l l 2 sθ 2 and hence, l 2 c (θ + θ 2 ) l 2 s (θ + θ 2 ) l cθ + l 2 c (θ + θ 2 ) l sθ + l 2 s (θ + θ 2 ) (8.29) θ Ẋc(θ + θ 2 )+Ẏs(θ + θ 2 ) (8.2) l sθ 2 θ 2 Ẋ (l cθ + l 2 c (θ + θ 2 )) + Ẏ (l sθ + l 2 s (θ + θ 2 )). (8.2) l l 2 sθ 2 Example 253 Singular configuration of a 2R manipulator. Singularity of a 2R manipulator occurs when determinant of the Jacobian (8.26) is zero. From Example 252, we have: l sθ J l 2 s (θ + θ 2 ) l 2 s (θ + θ 2 ) (8.22) l cθ + l 2 c (θ + θ 2 ) l 2 c (θ + θ 2 ) The determinant of J is: J l l 2 sin θ 2 (8.23) Therefore, the singular configurations of the manipulator are θ 2 θ 2 8 deg (8.24) corresponding to the fully extended or fully contracted configurations, as showninfigure8.7(a) and(b) respectively. At the singular configurations, the value of θ is indeterminate and may have any real value. The two columns of the Jacobian matrix become parallel because Equation (8.26) becomes Ẋ sθ sθ 2l θ Ẏ cθ + l 2 θ cθ 2 ³ 2l θ + l 2 θ2 sθ. (8.25) cθ In this situation, the endpoint can only move in the direction perpendicular to the arm links.

32 Velocity Kinematics l 2 y 2 x x 2 y y θ θ 2 x y l l 2 θ y l x o θ 2 8 x x 2 y 2 (a) (b) FIGURE 8.7. Singular configurations of a 2R planar manipulator. Example 254 Analytic method for inverse velocity kinematics. Theoretically, we must be able to calculate the joint velocities from the equations describing the forward velocities, however, such a calculation is not easy in a general case. As an example, consider a 2R planar manipulator shown in Figure 8.3. The endpoint velocity of the 2R manipulator was expressed in Equation (8.5) as: ḋ 2 θ ˆk l î + l 2 î 2 + θ2 ˆk l 2 î 2 (8.26) A dot product of this equation with î 2,gives ḋ 2 î 2 θ ³ ˆk l î î 2 l θ ˆk î î 2 l θ ˆk î 2 sin θ 2 l θ sin θ 2 (8.27) and therefore, θ ḋ 2 î 2. l sin θ 2 (8.28) Now a dot product of (8.26) with î reduces to ḋ 2 î θ ³ ˆk l î 2 2 î + θ ³ 2 ˆk l î 2 2 î l 2 ³ θ + 2 θ ˆk î 2 î l 2 ³ θ + θ 2 sin θ 2 (8.29)

33 8. Velocity Kinematics 469 and therefore, θ ḋ 2 î 2 θ. (8.22) l 2 sin θ 2 Therefore, we can determine the joint speeds θ,and θ 2 when the speeds of the end point ḋ 2 Ẋ Ẏ is given. Example 255 F Inverse Jacobian matrix for a robot with spherical wrist. The Jacobian matrix for an articulated robot with a spherical wrist is calculated in Example 247. ˆk J d 6 ˆk d 6 ˆk2 2 d 6 (8.22) ˆk ˆk ˆk2 ˆk3 ˆk4 ˆk5 The upper right 3 3 submatrix of J is zero. This is because of spherical wrist structure and having the last three position vectors as zero. Let us split the Jacobian matrix into four 3 3 submatrices and write it as: A B A J (8.222) C D C D where [A] ˆk d 6 ˆk d 6 ˆk2 2d 6 (8.223) [C] ˆk ˆk ˆk2 (8.224) [D] ˆk3 ˆk4 ˆk5. (8.225) Inversion of such a Jacobian is simpler if we take advantage of B.The forward velocity kinematics of the robot can be written as: Ẋ J q (8.226) θ θ 2 ḋ 2 A θ 3 (8.227) ω 2 C D θ 4 θ 5 θ 6 The upper half of the equation is ḋ 2 [A] θ θ 2 θ 3 (8.228) which can be inverted as: θ θ 2 θ 3 A ḋ 2 (8.229)

34 47 8. Velocity Kinematics The lower half of the equation is θ ω 2 C D θ 2 θ 3 [C] θ 4 θ 5 θ 6 θ θ 2 θ 3 +[D] θ 4 θ 5 θ 6 (8.23) and therefore, θ 4 θ 5 θ 6 D ω 2 [C] θ θ 2 θ 3 D ³ ω 2 [C] A ḋ 2. (8.23) Example 256 Inverse velocity of an articulated manipulator. The end point of the articulated manipulator of Figure 8.4 is found in Exercise 245 at: r P X P Y P Z P cos θ (l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 )) sin θ (l 2 cos θ 2 + l 3 sin (θ 2 + θ 3 )) l l 3 cos (θ 2 + θ 3 )+l 2 sin θ 2 (8.232) Using the components of r P, we calculated the Jacobian matrix of the manipulator X P X P X P θ θ 2 θ 3 J Y P Y P Y P θ θ 2 θ 3 Z P Z P Z P (8.233) θ θ 2 θ 3 to solve the forward kinematics of the manipulator. Ẋ P θ ẎP ŻP J θ 2 θ 3 (8.234) To solve the inverse velocity kinematics of the manipulator, we need to calculate J. X P X P X P θ θ 2 θ 3 J Y P Y P Y P θ θ 2 θ 3 a a 2 a 3 a 2 a 22 a 23 (8.235) Z P Z P Z P a 3 a 32 a 33 θ θ 2 θ 3

35 sin θ a l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 8. Velocity Kinematics 47 a 2 (sin (θ 2 + θ 3 )) cos θ l 2 cos θ 3 cos θ a 3 (l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 ) (8.236) l 2 l 3 cos θ 3 cos θ a 2 l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 a 22 sin (θ 2 + θ 3 ) sin θ l 2 cos θ 3 sin θ a 32 (l 3 sin (θ 2 + θ 3 )+l 2 cos θ 2 ) (8.237) l 2 l 3 cos θ 3 a 3 a 23 cos (θ 2 + θ 3 ) l 2 cos θ 3 a 33 (l 3 cos (θ 2 + θ 3 ) l 2 sin θ 2 ) (8.238) l 2 l 3 cos θ 3 Therefore, the joint speeds of the manipulator θ, θ 2, θ 3 are: θ Ẋ P θ 2 J ẎP (8.239) θ 3 ŻP

36

37 8. Velocity Kinematics Summary Each link of a serial robot has an angular and a translational velocity. The angular velocity of link (i) in the global coordinate frame can be found as a summation of the global angular velocities of its lower links ω i ix j j ω j. (8.24) Using DH parameters, the angular velocity of link (j) with respect to link (j ) is: ½ θj j ω ˆkj if joint i is R j (8.24) if joint i is P The translational velocity of link (i) is the global velocity of the origin of coordinate frame B i attached to link (i) ½ ω i i i ḋi d i d i ˆki + ω i i d i if joint i is R if joint i is P (8.242) where θ and d are DH parameters, and d is the frame s origin position vector. The velocity kinematics of a robot is defined by the relationship between joint speeds q q q n q n q n q n T (8.243) and global speeds of the end-effector Ẋ. Ẋ Ẋ n Ẏ n Ż n ω Xn ω Yn ω Zn T (8.244) Such a relationship introduces Jacobian matrix J. Ẋ J q (8.245) Having J, weareabletofind the end-effector speeds for a given set of joint speeds and vice versa. Jacobian is a function of joint coordinates and is the main tool in velocity kinematics of robots. We practically calculate J by Jacobian generating vectors denoted by c i (q) ˆki c i (q) i d n (8.246) ˆki where c i (q) makes the column i of the Jacobian matrix. J c c c 2 c n (8.247)

38

39 8. Velocity Kinematics Key Symbols a kinematic length of a link A, B, C, D submatrices of J B body coordinate frame c cos c Jacobian generating vector d differential, prismatic joint variable e rotation quaternion d x,d y,d z elements of d d translation vector, displacement vector D displacement transformation matrix G, B global coordinate frame, Base coordinate frame î, ĵ, ˆk local coordinate axes unit vectors ĩ, j, k skew symmetric matrices of the unit vector î, ĵ, ˆk Î,Ĵ, ˆK global coordinate axes unit vectors I [I] identity matrix J Jacobian, geometric Jacobian J D displacement Jacobian J R rotational Jacobian J φ angular Jacobian J A analytic Jacobian l length P prismatic joint, point q joint coordinate, q vector joint coordinates r position vectors, homogeneous position vector r i the element i of r r ij the element of row i and column j of a matrix R rotation transformation matrix, revolute joint s sin t ij the element of row i and column j of T T homogeneous transformation matrix T arm manipulator transformation matrix T wrist wrist transformation matrix T a set of nonlinear algebraic equations of q v velocity vector V velocity transformation matrix û unit vector along the axis of ω ũ skew symmetric matrix of the vector û u,u 2,u 3 components of û x, y, z local coordinate axes X, Y, Z global coordinate axes, coordinates of end-effector

40 Velocity Kinematics Greek α, β, γ angles of rotation about the axes of global frame δ Kronecker function, small increment of a parameter small test number to terminate a procedure θ rotary joint angle θ ijk θ i + θ j + θ k ϕ, θ, ψ angles of rotation about the axes of body frame φ angle of rotation about û ω angular velocity vector û unit vector along the axis of ω ω skew symmetric matrix of the vector ω ω,ω 2,ω 3 components of ω Symbol DOF degree of freedom [ ] inverse of the matrix [ ] [ ] T transpose of the matrix [ ] ` orthogonal (i) link number i k parallel perpendicular e conjugate of e

41 8. Velocity Kinematics 477 Exercises. Notation and symbols. Describe the meaning of a- i d i b- i d i c- i i d i d- i i d i e- i i d i g- i ḋ i h- i ḋi i- i i ḋi j- i iḋi k- i i ḋi f- i d i l- i ḋi m- ˆki n- ˆki o- i ˆk i 2 p- i v i q- i i v i r- i v i. 2. 3R planar manipulator velocity kinematics. Figure 5.2 illustrates an RkRkR planar manipulator. The forward kinematics of the manipulator provides the following transformation matrices: cθ 3 sθ 3 l 3 cθ 3 cθ 2 sθ 2 l 2 cθ 2 2 T 3 sθ 3 cθ 3 l 3 sθ 3 T 2 sθ 2 cθ 2 l 2 sθ 2 T cθ sθ l cθ sθ cθ l sθ Calculate the Jacobian matrix, J, using direct differentiating, and find the Cartesian velocity vector of the endpoint for numerical values. θ 56deg θ 2 28 deg θ 3 deg l cm l 2 55cm l 3 3cm θ 3deg/ sec θ2 deg/ sec θ3 deg / sec 3. Spherical wrist velocity kinematics. Figure 5.26 illustrates a schematic of a spherical wrist. The associated transformation matrices are given below. Assume that the frame B 3 is the base frame. Find the angular velocity vector of the coordinate frame B 6. cθ 4 sθ 4 cθ 5 sθ 5 3 T 4 sθ 4 cθ 4 4 T 5 sθ 5 cθ 5

42 Velocity Kinematics 5 T 6 cθ 6 sθ 6 sθ 6 cθ 6 4. Spherical wrist and tool s frame velocity kinematics. Assume that we attach a tools coordinate frame, with the following transformation matrix, to the last coordinate frame B 6 of a spherical wrist. 6 T 7 d 6 The wrist transformation matrices are given in Exercise 3. Assume that the frame B 3 isthebaseframeandfind the translational and angular velocities of the tools coordinate frame B SCARA manipulator velocity kinematics. An RkRkRkP SCARA manipulator is shown in Figure 5.23 with the following transformation matrices. Calculate the Jacobian matrix using the Jacobian-generating vector technique. cθ sθ l cθ cθ 3 sθ 3 T sθ cθ l sθ 2 T 3 sθ 3 cθ 3 T 2 cθ 2 sθ 2 l 2 cθ 2 sθ 2 cθ 2 l 2 sθ 2 3 T 4 d 6. R`RkR articulated arm velocity kinematics. Figure 5.22 illustrates a 3 DOF R`RkR manipulator with the following transformation matrices. Find the Jacobian matrix using direct differentiating, and Jacobian-generating vector methods. cθ sθ cθ 3 sθ 3 T sθ cθ d 2 T 3 sθ 3 cθ 3 T 2 cθ 2 sθ 2 l 2 cθ 2 sθ 2 cθ 2 l 2 sθ 2 d 2 r P T 3 l 3

43 8. Velocity Kinematics 479 y 3 x 2 x 3 d 3 y 2 y y x θ 2 ϕ θ l x FIGURE 8.8. A RRP planar redundant manipulator. 7. A RRP planar redundant manipulator. Figure 8.8 illustrates a 3 DOF planar manipulator with joint variables θ, θ 2,andd 3. (a) Determine the link transformation matrices and calculate T 3. (b) Solve the inverse kinematics of the manipulator for a given values of X, Y, ϕ,where,x, Y are global coordinates of the end-effector frame B 3,andϕ is the angular coordinate of B 3. (c) Show that the following equation can be a set of solution for inverse kinematic problem. θ 2 tan β ± p X2 + Y β 2 sin (θ β 2 2 α) l θ tan Y X (θ 2 α) α ϕ tan Y X q d 3 l 2 + p X2 + Y 2 2l X2 + Y 2 cos (θ 2 α) (d) Determine the Jacobian matrix of the manipulator and show that the following equation solve the forward velocity kinematics. Ẋ θ Ẏ ϕ J θ 2 d 3

44 l Velocity Kinematics y 3 x 2 x 3 y y y 2 θ 2 x ϕ θ d x FIGURE 8.9. A RP R planar redundant manipulator. J l sθ d 3 s (θ + θ 2 ) d 3 s (θ + θ 2 ) c (θ + θ 2 ) l cθ + d 3 c (θ + θ 2 ) d 3 c (θ + θ 2 ) s (θ + θ 2 ) (e) Determine J and solve the inverse velocity kinematics. 8. A RPR planar redundant manipulator. (a) Figure 8.9 illustrates a 3 DOF planar manipulator with joint variables θ, d 2,andθ 2. (b) Determine the link transformation matrices and calculate T 3. (c) Solve the inverse kinematics of the manipulator for a given values of X, Y, ϕ,where,x, Y are global coordinates of the end-effector frame B 3,andϕ is the angular coordinate of B 3. (d) Determine the Jacobian matrix of the manipulator to solve the forward velocity kinematics. Ẋ Ẏ ϕ J (e) Determine J and solve the inverse velocity kinematics. θ θ 2 d 3 9. An offset articulated manipulator. Figure 8. illustrates an offset articulated manipulator. (a) Determine the forward kinematics of the manipulator.

45 l 2 8. Velocity Kinematics 48 θ x 3 l 3 z x 2 P z 3 d P θ 3 z 2 l d θ 2 x z y x FIGURE 8.. An offset articulated manipulator. (b) Determine the global coordinates of the tip point P. (c) Determine the Jacobian of the manipulator, using direct differentiating. (d) Determine the Jacobian of the manipulator, using generating vectors. (e) Determine the inverse Jacobian matrix to solve the inverse velocity kinematics.. Articulated robots. Attach the spherical wrist of Exercise 22 to the articulated manipulator of Figure 8. and make a 6 DOF articulated robot. Determine the Jacobian of the robot, using generating vectors.. Spherical robots. Attach the spherical wrist of Exercise 22 to the spherical manipulator of Exercise 8 and make a 6 DOF spherical robot. Determine the Jacobian of the robot, using generating vectors. 2. Cylindrical robots. Attach the spherical wrist of Exercise 22 to the cylindrical manipulator of Exercise 2 and make a 6 DOF cylindrical robot. Determine the Jacobian of the robot, using generating vectors.

46 Velocity Kinematics Z l B z B 2 z 3 z 2 θ 3 B 3 θ l 2 x 2 G θ 2 x 3 x d 4 X B 4 y 4 x 4 z 4 FIGURE 8.. A SCARA robot. 3. SCARA robot inverse velocity kinematics. Figure 8. illustrates a SCARA robot. (a) Determine the coordinates of the origin of B 4 in G B. (b) Determine the Jacobian of the manipulator, using direct differentiating. (c) Determine the Jacobian of the manipulator, using generating vectors. (d) Determine the inverse Jacobian matrix to solve the inverse velocity kinematics. 4. F SCARA robot with B on the ground. Figure 8.2 illustrates a SCARA robot. (a) Determine the coordinates of the origin of B 4 in G B. (b) Determine the Jacobian of the manipulator, using direct differentiating. (c) Determine the Jacobian of the manipulator, using generating vectors. (d) Determine the inverse Jacobian matrix to solve the inverse velocity kinematics.

47 8. Velocity Kinematics 483 G Z l B z B 2 z 3 z 2 θ 3 B 3 θ l 2 x 2 θ 2 x 3 d Y B 4 x d 4 X y 4 x 4 z 4 FIGURE 8.2. A SCARA robot with B on the ground. 5. Rigid link velocity. Figure 8.3 illustrates the coordinate frames and velocity vectors of a rigid link (i). Find (a) velocity v i of the link at C i in terms of ḋi and ḋi (b) angular velocity of the link ω i in terms of ḋi and ḋi (c) velocity v i of the link at C i in terms of proximal joint i velocity (d) velocity v i of the link at C i in terms of distal joint i+ velocity (e) velocity of proximal joint i in terms of distal joint i + velocity (f) velocity of distal joint i + in terms of proximal joint i velocity 6. F Jacobian of a PRRR manipulator. Determine the Jacobian matrix for the manipulator shown in Figure F Spherical robot velocity kinematics. A spherical manipulator R`R`P, equipped with a spherical wrist, is shown in Figure The transformation matrices of the robot are given in Example 69. Find the Jacobian matrix of the robot. 8. F Space station remote manipulator system velocity kinematics.

48 Velocity Kinematics B i- z i- z i B i G v i Joint i Link (i) C i Joint i+ Z. d i- ωi. d i x i r i o i d i O d i- X Y o i- x i- FIGURE 8.3. Rigid link velocity vectors. The transformation matrices for the shuttle remote manipulator system (SRMS), shown in Figure 5.24, are given in the Example 59. Solve the velocity kinematics of the SRMS by calculating the Jacobian matrix.