S ingula r C onfigura tion Ana lys is a nd C oo rd ina te C ontro l of Robo t

Transcript

1 JOU RNAL O F SHAN GHA I J IAO TON G UN IV ER S IT Y V o l. 36 N o. 8 A ug : (2002) ,,,, (, ) : 6R,,.,,. J - 1 (J Jacob ian ). : ; ; ; : T P 391 : A S ingula r C onfigura tion Ana lys is a nd C oo rd ina te C ontro l of Robo t L IU Cheng 2liang, ZH A N G K a i, CA O Q i2x in, FU Z huang, Y IN Y ue2hong (Schoo l of M echan ical and Pow er Eng., Shanghai J iao tong U n iv., Shanghai , Ch ina) A bs tra c t: T he singu larity and coo rdinate con tro l of robo t w ere studied. A m ethod of so lving the app rox i2 m ate velocity of jo in t by adding damped vecto r w as p ropo sed and a modified algo rithm w as p rovided based on the w o rk of W amp ler. T he singu larities w ere divided in to the structu re boundary singu larity, boundary singu larity, inner singu larity and w rist singu larity. T he computer graph ic sim u lation s w ere developed. T he inner singu larity w as studied in ail. T he m ethods of u sing coo rdinate con tro l to avo id the singu lari2 ty of robo t arm and real tim e con tro l w ere p resen ted. Ke y w o rds: robo t; singu lar configu ration; coo rdinate con tro l; computer sim u lation Jacob ian J,, J,, J - 1,,. J - 1, J,,. [1 ], Jacob ian (Singu lar V alue D ecompo sition, SVD ), ;W amp ler [2 ] : : ( ) : (19642),,,,, CAD. ; Chen [3 ] ; Cheng [4 ],, S ICO P ; Grego ry [5 ] 8 ; Seng [6 ] M an ja [7 ] SVD., W amp ler [2 ] [8 ]. 6R, 4,. 6R,,

2 8, : 1139,. J - 1,. 1 RV 12L 6R, 1. 16R F ig. 1Jo int structure of 6R robo t RV 12L : Η1[ - 165, 165 ], Η2[ - 75, 75 ] Η3[ - 130, 130 ], Η4[ - 180, 180 ] Η5[ - 105, 105 ], Η6[ - 180, 180 ] a1= m, a2= m, a3= m a6= m, d 4= m, d 6= m : Ηi ( ), a i ; d i. 2Jacobian 2. 1Jacobian P ER m qr n (nm ), Υ: R n R m. P E = Υ(q) P, q g = J + (q) P g, J + = J T [J J T ] - 1 R nm Jacob ian, m = n ()J + = J - 1 ; m < n ( ) J +. q i, rankj < m, J J T R m m, J [2 ] Κ 2 q 2, q : m in P g - g q 2. 3 Κ J q g 2 + Κ 2 q g 2 Κ0 q g = J T [J J T + ΚI ] - 1 P g J J T = 0 J = = Κ0 ( [2 ] ). 0, rankj < m, Κ J J T 0 J 0,, : Κ= Κ0 (1 - k 0gK ) 2 K = m ax{k 0, kj } kj = 2 m m g2 J m F J J T : k 0 ; kj J ; J F J F ; n ; m. 3,, J E (Η) = J w (Η) = : c1= co sη1; s1= j 11 j 12 j 13 j 14 j 15 0 j 21 j 22 j 23 j 24 j j 32 j 33 j 34 j s1 - s1 c1s23 j 45 j 46 0 c1 c1 s1s23 j 55 j c23 s23s4 j 66 j 11 j 12 j j 21 j 22 j j 32 j s1 - s1 c1s23 j 45 j 46 0 c1 c1 s1s23 j 55 j c23 s23s4 j 66 sin Η1; c23= co s (Η2+ Η3) ; s23= sin (Η2+ Η3) ;. J EJ w J E = 0 d 6Z Ek - d 6Z E j I 3 - d 6Z Ek 0 d 6Z E i d 6Z Ej - d 6Z E i I 3 J w = U 66J w (1), ZE= [Z E iz E jz Ek ] T. U = 1 J w = J 11 J 22 (2) (2) 0, J 11= 0 J 22= 0,

3 J w = 0,. J 11= 0 ; J 22= 0. J w = J 11 = J 21 = J 22 = RV 12L 6R. J J 21 J 22 - d 4s1c23 - a2s1c2 - d 4c1s23 - a2c1s2 - d 4s23c1 d 4c1c23 + a2c1c2 - d 4s1s23 - a2s1s2 - d 4s23s1 0 - s1 - s1 0 c1 c d 4c23 - a2c2 - d 4c23 c1c23 c1s23s4 - s1c4 - c1c4s23s5 - s1s4s5 + c1c23c5 s1c23 s1s23s4 + c1c4 - s1c4s23s5 + c1s4s5 + s1c23c5 - s23 c23s4 - s23c5 - c23c4s5 J 11= - a2d 4s3 (d 4c23+ a 2c2) J 22= - s5 J 11= 0 sing1 = {ΗR 6 ga 2d 4s3 = 0} (3) Η3 = 0 sing2 = {ΗR 6 gd 4c23 + a2c2 = 0} (4) : sing1 ; sing2., 5, J 0,, Η5= Ηm inη5= Ηm ax. J 22= 0 : sing3 = {ΗR 6 gs5 = 0} (5) Η5= 0 4, 6R 3, sing = {sing1 sing2 sing3} (6),, J 0,. 4. 1, sing4 = {ΗR 6 gηi = Ηi,m in Ηi,m ax} (7), Ηi,m inηi,m ax i,. (6) sing = {sing1 sing2 sing3 sing4} (8) RV 12L,. RV 12L6, , 8, Η1[ - 165, 165 ], Η2[ - 75, 75 ] Η3[ - 130, 130 ], Η5[ - 75, 75 ] 4. 2,., L 1 L 2 L 3 L 4 L 4 L 3 L 2 L 1, L 1: {Η1, Η2, Η3, Η4, Η5, Η6}= {0,, 0,, 0, } L 2: {Η1, Η2, Η3, Η4, Η5, Η6}= {0, 75,,, 0, } L 3: {Η1, Η2, Η3, Η4, Η5, Η6}= {0, 75, 130,,, } L 4: {Η1, Η2, Η3, Η4, Η5, Η6}= {0,, 130, 0, 105, } L 1= L 1 L 2: {Η1, Η2, Η3, Η4, Η5, Η6}= {0, - 75,,, 0, } L 3: {Η1, Η2, Η3, Η4, Η5, Η6}= {0, - 75, - 130,,, } L 4: {Η1, Η2, Η3, Η4, Η5, Η6}= {0,, - 130,, - 105, },, Ηi[Ηi,m in, Ηi,m ax ] F ig. 2Compo sition of singular boundary RV 12L S1 S2 S3 S4, S1: {Η1, Η2, Η3, Η4, Η5, Η6}= {,, 0,, 0, } S2: {Η1, Η2, Η3, Η4, Η5, Η6}= {, 75,,, 0, } S3: {Η1, Η2, Η3, Η4, Η5, Η6}= {, 75, 130,,, } S4: {Η1, Η2, Η3, Η4, Η5, Η6}= {,, 130, 0, 105, } 3,, RV 12L, , (4),

4 8, : Η2- Η3 F ig. 5D ependence of Η2 versus Η3, RV 12L 6R, 3 Η1 Η2 Η3,. 3 F ig. 3Surface of singular boundary, 3 Η 1Η 2 Η 3,. : Η u= [Η 1Η 2Η 3 ] T, Η l= [Η 4Η 5Η 6 ] T Ξ= J 21Η u+ J 22Η l Η1 Η2 Η3, Η 4= co s Η6 Ξx - sin Η6 sin Η5 Ξy sin Η5 (9) Η 5= Ξx sin Η6- Ξy co s Η6 (10) 4 F ig. 4W o rk ing space.,, Η2 Η3: d 4co s (Η2 + Η3) + a2co s Η2 = 0 Η2 Η3 5 (a),., d 4a2, Η2 Η3. d 4= a2, co s (Η2 + Η3) + co s Η2 = 0 Η2 Η3 5 (b), Η2 Η3 : Η3= - 2Η2-180 Η2< 0-2Η Η2> 0 5, J,,..,, gj (Η) g0, Η 6= Η 4 co s Η5- Ξz (11) (9) (11), Η5 0, Ξ Η 4, Η 6,. : Η5> Α(Α). Η5< Α Αsgn (sin Η5) sin Η5. Α,. Α, ; Α,. Η5= 180, : Η5= 0, 4 6 ; Η5= 180, 4 6,. RV 12L 0 6T= 0 1T (Η1) 1 2T (Η2) 2 3T (Η3) 3 4T (Η4) 4 5T (Η5) 5 6T (Η6) 3 6T= 3 4T (Η4) 4 5T (Η5) 5 6T (Η6) = c4c5c6- s4s6 - c4c5s6- s4s6 - c4s5 0 s5s6 - s5s6 c6 d 4 - s4c5c6- c4c6 s4c5s6- c4c6 s4s co s (Η4+ Η6) - sin (Η4+ Η6) co s Η6 d 4 sin (Η4+ Η6) co s (Η4+ Η6) = (12)

5 , (Η4+ Η6). (Η4+ Η6), Η4 Η6. Η4, 5, gj (Η) g 0, Η4 Η6. (9) Η g 4sin Η5 = Ξx co s Η6 - Ξy sin Η6 (13) Η4 5 z 5 Ξ x 6Oy 6 Ξx y, 6, : Ξx gξy = sin Η6gco s Η6 (14) Η4 5,. 6 F ig. 6D eterm ination of coo rdinate angle z 5 Ξx y : Ξx = Ξx y sin Η6, Ξy = Ξx yco s Η6 ( 13) : Η 5 = Ξ. Η 4 = 0, ( 14) : Η 6= Ξx y [9 ], gη5g Α,,,. z 4 Ξx yχ, 4 z 4 Χ, 6 z 6 Χ. Χ, Η 4= 0, Η 5= Ξ, Η 6= Ξx y. Η5, gη5gα,.,,,. : [ 1 ]N akam ura Y, H annafusa H. Inverse k inem atic so lu2 tions w ith singularity robustness fo r robo t m anipula2 to r contro l [J ]. con trol, 1986, 108: ASM E D ynam System M easurmen t [ 2 ]W amp ler C W. M anipulato r inverse k inenm atic so lu2 tions based on vecto r fo rm ultions and damped least2 squares m ethods [J ]. net, 1986, 16 (1): IEEE Tran s Sys M an Cyber2 [ 3 ]Chen Y C, Seng J, N eil O. A p redicative algo rithm fo r rate contro l of m echanism s near singularities[j ]. The In ternational Journal of Robotics Research, 1998, 17 (6): [ 4 ]Cheng F T, Hour T L, Sun Y Y. Study and reso lu2 tion of singularities fo r a 62DO F PUM A m anipulato r [J ]. IEEE Tran saction s on System s, M an and Cyber2 netics, 1997, 27 (2): [ 5 ]Grego ry L, R ichard P. Singularity avo idance and the contro l of an eigh t2revo lute2jo int m anipulato r [ J ]. The In ternational Journal of Robotics Research, 1997, 16 (1): [ 6 ]Seng J, N eil O, Kevin A, et al. E scapability of sin2 gular configuration fo r redundant m anipulato rs via self2m o tion [A ]. Proceed ings of the 1995 IEEEgRSJ In ternational Conference on In tell igen t Robots and System ṡ Part 3 [C ]. N ew Yo rk: IEEE P ress, [ 7 ]M anja V K, M irko D B. Sym bo lic singular value de2 compo sition fo r a PUM A robo t and its app lication to a robo t operation near singularities[j ]. The In ternational Journal of Robotics Research, 1993, 12 (5): [ 8 ],,. [J ]., 1996, 18 (5): ZHU X iang2yang, lun. ZHON G B ing2lin, X ION G You2 Singularities treatm ent fo r m anipulato r inverse k inem atics evalution [J ]. Robot, 1996, 18 (5): [ 9 ],. [J ]., 1994, 16 (4): ZOU T ing, LU Zen. A vo iding singularities of m a2 nipulato rs w ith the m ethod of mo tion coo rdinational contro l[j ]. Robot, 1994, 16 (4):