Industrial Robotics Theory Modelling and Control Part 4 ppt

Structure Based Classification and Kinematic Analysis of … 169 When the manipulators in Table 4 are considered from the viewpoint of the so- lution procedure described above, they are accompanied by the designations CF (having inverse kinematic solution in closed-form) and PJV (having inverse kinematic solution using a parametrized joint variable). As noted, almost all the manipulators in Table 4 are designated exclusively either with CF or PJV. Exceptionally, however, the subgroups 4.3 and 4.4 have both of the designa- tions. This is because the solution type can be either CF or PJV depending on whether lj 3 = 0° or lj 3 = 90°, respectively. In this section, the inverse kinematic solutions for the subgroups 1.1 (e.g. KUKA IR 662/10), 1.7 (e.g. GMF S-3 L or R) and 4.4 (e.g. Unimate 4000) are given in order to demonstrate the solution procedure described above. As in- dicated in Table 4, the subgroup 1.1 can have the inverse kinematic solution in closed-form, whereas the subgroup 1.7 necessitates a PJV solution. On the other hand, for the subgroup 4.4, which has a prismatic joint, the inverse ki- nematic solution can be obtained either in closed-form or by using the PJV method depending on whether the structural parameter lj 3 associated with the prismatic joint is 0° or 90°. Although lj 3 = 0° for the enlisted industrial robot of this subgroup, the solution for lj 3 = 90° is also considered here for sake of dem- onstrating the application of the PJV method to a robot with a prismatic joint as well. It should be noted that, the subgroups 1.1 and 1.7 are examples to ro- bots with only revolute joints and the subgroup 4.4 is an example to robots with revolute and prismatic joints. The subgroups 1.1 and 1.7 are considered particularly because the number of industrial robots is high within these cate- gories. As an additional example, the ABB IRB2000 industrial robot is also con- sidered to demonstrate the applicability of the method to manipulators con- taining closed kinematic chains. However, the solutions for the other subgroups or a new manipulator with a different kinematic structure can be obtained easily by using the same systematic approach. 4.1 Inverse Kinematics of Subgroups 1.1 and 1.7 For all the subgroups of the main group 1, the orientation matrix is θθ θθθ =   31 223 34 25 36 uu uuu 1 ˆ Cee eee (81) Since all the subgroups have the same 1 ˆ C matrix, they will have identical equations for lj 4 , lj 5 and lj 6 . In other words, these variables can always be de- termined from the following equation, after finding the other variables some- how from the wrist location equations of the subgroups: θθθ =  34 25 36 uuu 1 ˆ eee M (82) 170 Industrial Robotics: Theory, Modelling and Control Here, θθ =  223 31 -u -u 11 ˆ ˆ Me e C and lj 23 = lj 2 + lj 3 . Since the sequence in Equation (82) is 3-2-3 , using Table 5, the angles lj 4 , lj 5 and lj 6 are obtained as follows, assum- ing that lj 1 and lj 23 have already been determined as explained in the next sub- section: θ= σ σ 4523513 atan2 ( m , m ) (83) θ= σ − 2 553333 atan2 ( 1 m , m ) (84) θ= σ σ 6532513 atan2 ( m , - m ) (85) Here, ǔ 5 = ±1 and = T ij i 1 j ˆ muMu . Note that this 3-2-3 sequence becomes singular if lj 5 = 0 or θ=± o 5 180 , but the latter case is not physically possible. This is the first kind of singularity of the manipulator, which is called wrist singularity. In this singularity with lj 5 = 0, the axes of the fourth and sixth joints become aligned and Equation (82) degener- ates into θ+θ θθ θ ===    34 6 34 36 346 u( ) uu u 1 ˆ e e e e M (86) This equation implies that, in the singularity, lj 4 and lj 6 become arbitrary and they cannot be determined separately although their combination lj 46 = lj 4 + lj 6 can still be determined as θ= 46 21 11 atan2 (m , m ). This means that one of the fourth and sixth joints becomes redundant in orienting the end-effector, which in turn becomes underivable about the axis normal to the axes of the fifth and sixth joints. 4.1.1 Inverse Kinematics of Subgroup 1.1 The wrist point position vector of this subgroup given in Equation (40) can be written again as follows by transposing the leading exponential matrix on the right hand side to the left hand side: θθθ =+  31 22 223 -u u u 11 2 1 4 3 eraeudeu (87) Premultiplying both sides of Equation (87) by TTT 123 u, u, u and using the simplification tool E.8 in Appendix A, the following equations can be obtained. θ+ θ= θ+ θ 11212 2423 r cos r sin a cos d sin (88) Structure Based Classification and Kinematic Analysis of … 171 θ− θ= 2111 r cos r sin 0 (89) =− θ + θ 3224 23 r a sin d cos (90) Here, r 1 , r 2 and r 3 are the base frame components of the wrist position vector, 11 r . From Equation (89), lj 1 can be obtained as follows by using the trigonometric equation T1 in Appendix C, provided that +≠ 22 21 rr0: θ= σ σ 11211 atan2 ( r , r ) and ǔ 1 = ±1 (91) If += 22 21 rr0 , i.e. if == 21 rr0 , i.e. if the wrist point is located on the axis of the first joint, the second kind of singularity occurs, which is called shoulder singu- larity. In this singularity, Equation (89) degenerates into 0 = 0 and therefore θ 1 cannot be determined. In other words, the first joint becomes ineffective in po- sitioning the wrist point, which in turn becomes underivable in the direction normal to the arm plane (i.e. the plane formed by the links 2 and 3). To continue with the solution, let ρ =θ+θ 11 12 1 r cos r sin (92) Thus, Equation (88) becomes ρ =θ+θ 12 24 23 a cos d sin (93) Using Equations (90) and (93) in accordance with T6 in Appendix C, lj 3 can be obtained as follows, provided that −≤ ρ ≤ 2 11 : θ= ρ σ− ρ 2 3232 atan2 ( , 1 ) and ǔ 3 = ±1 (94) Here, ρ+ − + ρ= 22 2 2 13 2 4 2 24 (r)(ad) 2a d (95) Note that the constraint on ρ 2 implies a working space limitation on the ma- nipulator, which can be expressed more explicitly as − ≤ρ + ≤ + 222 2 24 13 2 4 (a d ) r (a d ) (96) 172 Industrial Robotics: Theory, Modelling and Control Expanding sin lj 23 and cos lj 23 in Equation (90) and (93) and rearranging the terms as coefficients of sin lj 2 and cos lj 2 , the following equations can be ob- tained. ρ = ρ θ+ ρ θ 13 24 2 cos sin (97) 34 23 2 rcos sin= ρ θ− ρ θ (98) Here, 324 3 adsin ρ =+ θ (99) 44 3 dcos ρ =θ (100) According to T4 in Appendix C, Equations (97) and (98) give lj 2 as follows, pro- vided that ρ + ρ ≠ 22 34 0: () 241334331 atan2 r , rθ= ρρ − ρρ − ρρ (101) If ρ + ρ = 22 34 0 , i.e. if ρ = ρ = 34 0, the third kind of singularity occurs, which is called elbow singularity. In this singularity, both of Equations (97) and (98) de- generate into =00 . Therefore, θ 2 cannot be determined. Note that, according to Equations (99) and (100), it is possible to have ρ = ρ = 34 0 only if = 24 ad and θ=± o 3 180 . This means that the elbow singularity occurs if the upper and front arms (i.e. the links 2 and 3) have equal lengths and the front arm is folded back onto the upper arm so that the wrist point coincides with the shoulder point. In this configuration, the second joint becomes ineffective in positioning the wrist point, which in turn becomes underivable neither along the axis of the second joint nor in a direction parallel to the upper arm. As seen above, the closed-form inverse kinematic solution is obtained for the subgroup 1.1 as expressed by Equations (83)-(85) and (91)-(101). The com- pletely analytical nature of the solution provided all the multiplicities (indi- cated by the sign variables σ 1 , σ 2 , etc), the singularities, and the working space limitations alongside with the solution. 4.1.2 Inverse Kinematics of Subgroup 1.7 The wrist point position vector of this subgroup is given in Equation (46). From that equation, the following scalar equations can be obtained as done previously for the subgroup 1.1: Structure Based Classification and Kinematic Analysis of … 173 θ+ θ= θ+ θ − θ θ 11212 24235234 r cos r sin a cos d sin d cos sin (102) θ− θ= θ 211154 r cos r sin d cos (103) =− θ + θ + θ θ 3224 235234 r a sin d cos d sin sin (104) Here, r 1 , r 2 and r 3 are the components of the wrist position vector, 17 r . Note that Equations (102)-(104) contain four unknowns (lj 1 , lj 2 , lj 3 , lj 4 ). Therefore, it now becomes necessary to use the PJV method. That is, one of these four un- knowns must be parametrized. On the other hand, Equation (103) is the sim- plest one of the three equations. Therefore, it will be reasonable to parametrize either lj 1 or lj 4 . As it is shown in (Balkan et al., 1997, 2000), the solutions ob- tained by parametrizing lj 1 and lj 4 expose different amounts of explicit infor- mation about the multiple and singular configurations of the manipulators be- longing to this subgroup. The rest of the information is concealed within the equation to be solved numerically. It happens that the solution obtained by pa- rametrizing lj 4 reveals more information so that the critical shoulder singular- ity of the manipulator can be seen explicitly in the relevant equations; whereas the solution obtained by parametrizing lj 1 conceals it. Therefore, lj 4 is chosen as the parametrized joint variable in the solution presented below. As the starting step, lj 1 can be obtained from Equation (103) as follows by using T3 in Appen- dix C, provided that +> 22 21 rr0 and +≥ ρ 22 2 21 5 rr : θ= − +σ + − ρρ 222 12111255 atan2 ( r , r ) atan2 ( r r , ) and ǔ 1 = ±1 (105) Here, ρ =θ 55 4 d cos (106) If += 22 21 rr0 , which necessitates that ρ = 5 0 or θ=± o 4 90 , the shoulder singu- larity occurs. In that case, Equation (103) degenerates into 0 = 0 and therefore θ 1 becomes arbitrary. The consequences are the same as those of the subgroup 1.1. On the other hand, the inequality constraint +≥ ρ 22 2 21 5 rr indicates a working space limitation on the manipulator. Equations (102) and (104) can be arranged as shown below: =θ+θ− ρ θ 1 2 2 4 23 6 23 xacos dsin cos (107) 174 Industrial Robotics: Theory, Modelling and Control =− θ + θ + ρ θ 3 2 2 4 23 6 23 rasindcos sin (108) Here, ρ =θ 65 4 d sin (109) According to T9 in Appendix C, Equations (107) and (108) give lj 3 as follows, provided that ρ +≥ ρ 222 147 d : 222 34631477 atan2 (d , ) atan2 ( d , )θ= − ρ +σ ρ +− ρρ and ǔ 3 = ±1 (110) Here, ρ +−++ ρ ρ= 22 2 2 2 13 2 4 6 7 2 (r)(ad ) 2a (111) As noted, the inequality constraint ρ +≥ ρ 222 147 d constitutes another limitation on the working space of the manipulator. Expanding sin lj 23 and cos lj 23 in Equation (107) and (108) and collecting the relevant terms as coefficients of sin lj 2 and cos lj 2 , the following equations can be obtained. ρ = ρ θ+ ρ θ 18 29 2 cos sin (112) = ρ θ− ρ θ 39 28 2 rcos sin (113) Here, ρ =+ θ− ρ θ 824 36 3 adsin cos (114) ρ =θ+ ρ θ 94 36 3 dcos sin (115) According to T4 in Appendix C, Equation (112) and (113) give lj 2 as follows, provided that ρ + ρ ≠ 22 89 0: () 2 91339381 atan2 r , rθ= ρρ − ρρ − ρρ (116) If ρ + ρ = 22 89 0 , the elbow singularity occurs. Then, θ 2 becomes arbitrary with the same consequences as those of the subgroup 1.1. Structure Based Classification and Kinematic Analysis of … 175 Note that the matrix θθ =  223 31 -u -u 11 ˆ ˆ M e e C of this subgroup comes out to be a function of θ 4 because the angles θ 1 and θ=θ+θ 23 2 3 are determined above as functions of θ 4 . Therefore, the equation for the parametrized joint variable lj 4 is nothing but Equation (83), which is written here again as θ= θ = σ θ σ θ 444 5234 5134 f()atan2 [m(), m()] and ǔ 5 = ±1 (117) As noticed, Equation (117) is a highly complicated equation for the unknown lj 4 and it can be solved only with a suitable numerical method. However, after it is solved for θ 4 , by substituting lj 4 into the previously derived equations for the other joint variables, the complete solution is obtained. Here, it is worth to mention that, although this solution is not completely analytical, it is still ca- pable of giving the multiple and singular configurations as well as the working space limitations. Although the PJV method is demonstrated above as applied to the subgroup 1.7, it can be applied similarly to the other subgroups that require it. For ex- ample, as a detailed case study, its quantitatively verified application to the FANUC ArcMate Sr. robot of the subgroup 1.9 can be seen in (Balkan et al. 1997 and 2000). 4.2 Inverse Kinematics of Subgroup 4.4 The inverse kinematic solution for the subgroup 4.4 is obtained in a similar manner and the related equations are given in Table 6 indicating the multiple solutions by i 1σ=±. The orientation matrix 4 ˆ C is simplified using the kine- matic properties of this subgroup and denoted as 44 ˆ C . Actually, the Unimate 4000 manipulator of this subgroup does not have two versions with lj 3 = 0° and lj 3 = 90° as given below. It has simply lj 3 = 0° and the other configuration is a fictitious one. However, aside from constituting an additional example for the PJV method, this fictitious manipulator also gives a design hint for choosing the structural parameters so that the advantage of having a closed-form in- verse kinematic solution is not lost. 176 Industrial Robotics: Theory, Modelling and Control Table 6. Inverse Kinematic Solution for Subgroup 4.4 Structure Based Classification and Kinematic Analysis of … 177 4.3 Inverse Kinematics of Manipulators with Closed Kinematic Chains The method of inverse kinematics presented here is not limited to the serial manipulators only. It can also be applied to robots with a main open kinematic chain supplemented with auxiliary closed kinematic chains for the purpose of motion transmission from the actuators kept close to the base. As a typical ex- ample, the ABB IRB2000 industrial robot is selected here in order to demon- strate the application of the method to such manipulators. The kinematic sketch of this manipulator with its four-link transmission mechanism is shown in Figure 2. It can be seen from the kinematic sketch that the four-link mecha- nism can be considered in a sense as a satellite of the manipulator’s main open kinematic chain. In other words, its relative position with respect to the main chain is determined completely by the angle lj 3 . Once lj 3 is found by the inverse kinematic solution, the angular position of the third joint actuator φ 3 can be determined in terms of lj 3 as follows by considering the kinematics of the four- link mechanism: φ = ψ +θ 322 (118) Here, ψ= +σ + + 222 23 atan2(b, a) atan2( a b c , c) and σ 3 = ±1 (119) =+ θ 24 3 a a b sin , =θ 43 bbcos , ++− =−θ 2222 224324 3 22 abbbab csin 2b b (120)  G u 3 (0) θ 1  G u 3 (1 )  G u 3 (2) O 3 O 0 O 1 ,  G u 3 (5) , G u 3 (6)  G u 3 (4) O 2  G u 1 (1 )  G u 1 (3)  G u 1 (2)  G u 3 (3)  G u 1 (4) x x θ 2 <0 x  G u 1 (5) θ 5 O 4 O 5 O 6 ,, θ 3 a 2 a 3 d 4 θ 3 a 2 b 2 b 3 b 4 O 2 O 1 A B θ 2 ψ 4 = π 2 −θ 3 ψ 2 φ 3 a) Complete Kinematic Structure b) Closed Kinematic Chain Details Figure 2. Kinematic Sketch of the ABB IRB2000 Manipulator 178 Industrial Robotics: Theory, Modelling and Control However, in this particular manipulator, the four-link mechanism happens to be a parallelogram mechanism so that ψ = ψ 24 and π φ =θ + −θ 32 3 2 . Note that, if the auxiliary closed kinematic chain is separated from the main open kine- matic chain, then this manipulator becomes a member of the subgroup 1.4 and the pertinent inverse kinematic solution can be obtained in closed-form simi- larly as done for the subgroup 1.1. 4.4 Comments on the Solutions The inverse kinematic solutions of all the subgroups given in Table 4 are ob- tained. In main group 1, subgroup 1.2 has parameter d 2 in excess when com- pared to subgroup 1.1. This has an influence only in the solution of lj 1 . The re- maining joint variable solutions are the same. Similarly subgroup 1.3 has parameter a 1 and subgroup 1.5 has d 23 (d 2 +d 3 ) in excess when compared to subgroup 1.1. Considering subgroup 1.3, only the solutions of lj 2 and lj 3 are dif- ferent than the solution of subgroup 1.1, whereas the solution of subgroup 1.5 is identical to the solution of subgroup 1.2 except that d 23 is used in the formu- las instead of d 2 . Subgroup 1.6 has parameter a 1 in excess compared to sub- group 1.4. Thus, the solutions of lj 2 and lj 3 are different than the solution of subgroup 1.4. Subgroup 1.8 has parameter d 2 and subgroup 1.9 has a 1 and a 3 in excess when compared to subgroup 1.7. Considering subgroup 1.8, only the solution of lj 1 is different. For subgroup 1.9, a 1 and a 3 causes minor changes in the parameters defined in the solutions of lj 2 and lj 3 . The last subgroup, that is subgroup 1.10 has the parameters a 1 , a 3 and d 3 in excess when compared to subgroup 1.7. lj 1 , lj 2 and lj 3 have the same form as they have in the solution of subgroup 1.7, except the minor changes in the parameters defined in the solu- tions. It can be concluded that d 2 affects the solution of lj 2 and a 1 affects the so- lutions of lj 2 and lj 3 through minor changes in the parameters defined in the so- lution. In main group 2, subgroup has parameter d 2 in excess when compared to subgroup 2.3 and thus the solution of lj 1 has minor changes. Subgroup 2.5 has parameter a 1 in excess when compared to subgroup 2.4 and the solutions of lj 2 and lj 3 have minor changes. Subgroup 2.6 has parameter d 4 in excess and the term including it is identical to the term including d 2 in subgroup 2.5 ex- cept lj 1 which includes d 4 instead of d 2 . In main group 8, subgroup 8.2 has the parameter a 2 in excess compared to subgroup 8.1. This leads to a minor change in the solutions of lj 1 and lj 2 through the parameters defined in the solution. For main group 1, if lj 1 is obtained analytically lj 4 , lj 5 and lj 6 can be solved in closed-form. Any six-joint manipulator belonging to main group 2 can be solved in closed-form provided that lj 1 is obtained in closed-form. Using lj 1 , lj 234 can easily be determined using the orientation matrix equation. Since lj 4 appears in the terms including a 4 , d 5 and a 5 as lj 234 , this lead to a complete [...]... 8 θ2 - 34. 45 - 34. 45 -47 . 14 -47 . 14 1 04. 39 1 04. 39 117.07 117.07 -163.09 -163.09 -1 04. 39 -1 04. 39 -72.90 -72.90 -11.05 -11.05 θ3 4 64. 67 64. 67 -72 .4 -72 .4 64. 57 64. 57 -72.30 -72.30 θ5 86.12 -85.75 19.01 -160.03 24. 26 -156.27 68.50 -120.58 Table 2 Inverse Position Solutions for the Spherical-wrist Robot θ6 -36.06 31.73 - 84. 81 80 .47 85.22 -89.56 28.82 -33.16 -130.97 58.63 1 54. 74 -20. 04 2.38 177. 24 68.23... θ 4 (i.e a θ 4 z 4 -type rotation) The original vecˆ ˆ ˆ ˆf tor, z 6 , can be expressed with respect to the x 4 y 4 z 4 -frame in terms of local 4o coordinates ( n4o , m4o and l4o ), where n4o and m4o are unknowns to be worked ˆf ˆ out and l4o is numerically obtained from l4 o = l4 r = z 6 z 4 ˆ The vector zi6 5r ˆ which results from rotating z i6 5o ˆ by an angle θ 5 z 5 may be ex- pressed in the... 5 is directed from z 4 to z 5 (where ˆ ˆ ˆ ˆ ˆ x 5 = z 4 × z 5 ) At zero position x 4 is selected to coincide with x 5 such that two ˆ ˆ ˆ ˆ ˆ ˆ Cartesian coordinate systems x 4 y 4 z 4 and x 5 y 5 z 5 can be established Accordˆf ing to the concepts in (4) and (5), z6 can be described with respect to the ˆ ˆ ˆ ˆ x 4 y 4 z 4 -frame in terms of local coordinates ( n4r , m4r and l4r ) Also, zi6 5o can... locations of the fourth and fifth joint-axes which were displaced to (-128.0, 818.51 and 205. 04 mm) and (-130.5, 802.0 and 180 .4 mm) respectively Axes z1 z2 z3 z4 z5 z6 Direction Cosines of Joint-axes zx zy zz -0.0871557 0.02255767 0.9 848 077 -0.9961 946 0.00012 74 -0.0871557 -0.9961 947 0.05233595 0.0696266 0.02233595 -0.9993908 0.02681566 0.99975050 -0.0223359 0.00009 744 0.0 248 9 949 0.9996253 0.00012081... (20) where f, f 1 and f 2 are linear functions of s2 and c 2 ˆ Noting the properties of rotation about z 3 the following may be deduced, 2 f 12 + f 22 = f 2 ( pi 3 o pi 3 o − l3 r ) (21) 2 2 This last equation is a polynomial of s2 , c 2 , s2 , c 2 and s2 c 2 ; and can be reexpressed in the following form, 4 k =0 b4 − k t 4 − k = 0 (22) 1 94 Industrial Robotics: Theory, Modelling and Control where the... (27) where c 5 and s5 stand for cos θ5 and sin θ5 respectively ˆ A property of rotation about z 4 may be stated as, ˆ ˆ ˆ ˆ l4 o = ( n5 o s5 + m5 oc 5 ) y 5 z 4 + l5 o z 5 z 4 (28) This last expression (28) is a linear equation in s5 and c 5 This equation may be re-expressed in a polynomial form as follows, 2 k =0 b2 − k t 2 − k = 0 (29) 200 Industrial Robotics: Theory, Modelling and Control where... Six-joint Industrial Robots Surveyed (Balkan et al., 2001) 183 1 84 Industrial Robotics: Theory, Modelling and Control Appendix C Solutions to Some Trigonometric Equations Encountered in Inverse Kinematic Solutions The two-unknown trigonometric equations T5-T8 and T9 become similar to T4 and T0c respectively once j is determined from them as indicated above Then, i can be determined as described for T4 or... coordinates ( n5o , m5o and l5o ) Inverse Position Procedure for Manipulators with Rotary Joints ˆ zf6 ˆ z4 199 5 ˆ y5 4 ˆ y4 ˆ zi6 5 o ˆ x5 ˆ x4 θ5 ˆ z0 ˆ x0 Base Coordinates ˆ y0 Figure 4 A 2R Arrangement Used for Orienting Vectors in Space ˆf ˆf It is understood that the vector z6 resulted from rotating another vector z 6 4o ˆ ˆ about the z 4 axis by an angle, θ 4 (i.e a θ 4 z 4 -type rotation) The... Inverse Position Problem Using a Generic-Case Solution Methodology, Mechanism and Machine Theory, Vol 6, Vol 1, pp 91-106 Wu, C H & Paul, R P (1982) Resolved Motion Force Control of Robot Manipulator, IEEE Transactions on Systems, Man and Cybernetics, Vol SMC 12, No 3, pp 266-275 182 Industrial Robotics: Theory, Modelling and Control Appendix A Exponential Rotation Matrix Simplification Tools (Özgören,... direct kinematic procedure, and compared to the required position vector pn where the net radial error, e jk , is 0 calculated as follows, jk n e jk = p0 − p0 (32) 202 Industrial Robotics: Theory, Modelling and Control Read and Write Data Perform Inverse Position Analysis (IPA) of the Arm to Get 4 Solutions j =2 j=1 IPA of Wrist IPA of Wrist k =1 k =2 Get p jk k =2 0 j =4 j =3 IPA of Wrist k =1 k =2 . as − ≤ρ + ≤ + 222 2 24 13 2 4 (a d ) r (a d ) (96) 172 Industrial Robotics: Theory, Modelling and Control Expanding sin lj 23 and cos lj 23 in Equation (90) and (93) and rearranging the terms. (107) 1 74 Industrial Robotics: Theory, Modelling and Control =− θ + θ + ρ θ 3 2 2 4 23 6 23 rasindcos sin (108) Here, ρ =θ 65 4 d sin (109) According to T9 in Appendix C, Equations (107) and. lost. 176 Industrial Robotics: Theory, Modelling and Control Table 6. Inverse Kinematic Solution for Subgroup 4. 4 Structure Based Classification and Kinematic Analysis of … 177 4. 3 Inverse