Nâng cao chất lượng điều khiển cho robot Scara, chương 6 pptx

Chương 6: Động học robot Scara Serpent Robot Scara Serpent có cấutrúcđộng họcđượcbiểu diễn nhưtrên Hình 2.4. Robot có 3 trụcquayvà 1 bàn kẹp,tuynhiên ba kh ớpđộngđầutiênđượcgọilà bộphậncơbảnvì trướchết,nhờ chúng tay máy có thểthựchiệnbướcchủyếutrongthaotácđịnh vị,tứclàđưabànkẹpđếnlâncậnđiểmlàm việc,sauđónhờkhớp độngc òn lạibàn kẹpđượcđịnhhướngvà vi chỉnhđếnvịtrígia công chính xác. 2.3.1. Động học thuận Việcxâydựngcácphươngtrình độnghọcthuậncủarobot đượctiếnh ành tuầntựtheocácbướcsau: Bước 1:Xácđịnhcáchệtoạđộ Ta sửdụngquyướcDenavit-Hartenberg đểmôtảđầyđủvị trí củacủatoàn thân robot công nghiệp. Hình 2.4 mô tảcáchệtrục toạđộ gắnvớicáckhúctaycủarobotScaraSerpent. Bước 2: Xây dựngbảngthôngsốDH Bảng 2.3: Tham sốDenavit– Hartenberg củarobotScaraSerpent. Thanh n ối  i ( 0 ) a i  i (rad) d i (m) Biến Chuyển động 1 0 a 1  1 0  1 Quay 2 -180 0 a 2  2 0  2 Quay 3 0 0 0 d 3 d 3 Tịnhtiến 4 0 0  4 0  4 Quay Kh ảosátvới3trụckhớpquayđầutiêntươngứngvớiquỹ đạocủakhớpquay4trong mặtphẳngOX 0 Y 0 . Ma trậnT 4 là ma tr ận biểu diễn tay máy robot trong hệ trục tọa độ gốc: T 4 = A 1 .A 2 .A 3 .A 4 A n = n n n n n n n n n n n n n n n n n cos sin cos sin sin a cos sin cos cos cos sin a sin 0 sin cos d 0 0 0 1                             (2.1) Thay s ốliệutrongbảngthamsốcó: 1 1 1 1 1 1 1 1 1 cos sin 0 a .cos sin cos 0 a sin A 0 0 1 0 0 0 0 1                     2 2 2 2 2 2 2 2 2 cos sin 0 a .cos sin cos 0 a sin A 0 0 1 0 0 0 0 1                      3 3 1 0 0 0 0 1 0 0 A 0 0 1 d 0 0 0 1              4 4 4 4 4 cos sin 0 0 sin cos 0 0 A 0 0 1 0 0 0 0 1                    Ký hiệu: S 1  Sin 1 ; C 1  Cos 1 S 2  Sin 2 ; C 2  Cos 2 S 4  Sin 4 ; C 4  Cos 4 S 12  Sin( 1 + 2 ); C 12  Cos( 1 + 2 )  Cácbướctínhtoán: Bước1: 3 4 4 T A  4 4 4 4 3 4 4 C S 0 0 S C 0 0 T A 0 0 1 0 0 0 0 1                Bước2: 2 3 4 3 4 T A . T  4 4 4 4 4 4 4 4 2 4 3 3 1 0 0 0 C S 0 0 C S 0 0 0 1 0 0 S C 0 0 S C 0 0 T 0 0 1 d 0 0 1 0 0 0 1 d 0 0 0 1 0 0 0 1 0 0 0 1                                         Bước3: 1 2 4 2 4 T A . T  2 2 2 2 4 4 2 2 2 2 4 4 1 4 3 2 4 2 4 2 4 2 4 2 2 2 4 2 4 2 4 2 4 2 2 1 4 3 C S 0 a .C C S 0 0 S C 0 a .S S C 0 0 T 0 0 1 0 0 0 1 d 0 0 0 1 0 0 0 1 C C S S C S S C 0 a .C S C C S S S C C 0 a .S T 0 0 1 d 0 0 0 1                                                  Bước4: 4 1 14 0 4 T.ATT  1 1 1 1 2 4 2 4 2 4 2 4 2 2 1 1 1 1 2 4 2 4 2 4 2 4 2 2 0 4 3 C -S 0 a .C C C S S S C C S 0 a .C S C 0 a .S S C C S (C C S S ) 0 a .S T 0 0 1 0 0 0 1 d 0 0 0 1 0 0 0 1                                  C (C C S S ) S (S C C S ) C (S C C S ) S (C C S S ) 0 a C a C 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12 1 1 S (C C S S ) C (S C C S ) S (S C C S ) C (C C S S ) 0 a S a S 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12 1 1 0 0 1 d 3 0 0 0 1                           (2.2) Ma tr ận 0 T 4 biểudiễntaymáyrobottronghệtoạđộgốc. Mặtkháctheokýhiệutổngquát: x x x x y y y y E 4 z z z z n o a p n o a p T T n o a p 0 0 0 1   (2.3) V ới: p a o n     , , , lần lượt là các vectơ định vị, vectơ định hướng,vectơtớivàvectơvịtríđểbiểudiễnhướngv à vịtrícủatay máy trong không gian làm việc. Từ ma trận trên ta có hệ phương trình động học thuận tay máy robot: n x = C 1 (C 2 C 4 + S 2 S 4 ) – S 1 (S 2 C 4 – C 2 S 4 ) (2.4) n y = S 1 (C 2 C 4 + S 2 S 4 ) + C 1 (S 2 C 4 – C 2 S 4 ) (2.5) n z = 0 (2.6) o x = C 1 (S 2 C 4 - C 2 S 4 ) + S 1 (C 2 C 4 + S 2 S 4 ) (2.7) o y = S 1 (S 2 C 4 - C 2 S 4 ) - C 1 (C 2 C 4 + S 2 S 4 ) (2.8) o z = 0 (2.9) a x = 0 (2.10) a y = 0 (2.11) a z = -1 (2.12) Và h ệphươngtrình xác địnhvịtrícủađiểmtácđộngcuốinhưsau: x = p x = a 1 .C 1 + a 2 .C 12 (2.13) y = p y = a 1 .S 1 + a 2 .S 12 (2.14) z = p z = - d 3 (2.15) 2.3.2. Động học ngược Độnghọcngược:xácđịnhcácbiếnkhớpkhibiếtvịtrítay. Từphươngtrình độnghọcthuậncó: 2 2 2 2 2 x 1 1 2 12 1 2 1 12 2 2 2 2 2 y 1 1 2 12 1 2 1 12 p a .C a .C 2.a .a .C .C p a .S a .S 2.a .a .S .S       2 2 2 2 2 2 2 2 X Y 1 1 1 2 12 12 1 2 1 12 1 12 p p a .(S C ) a .(S C ) 2.a .a .(S .S C .C )        2 2 2 2 x y 1 2 1 2 2 p p a a 2.a .a .C      (2.16) Dođó: 2 2 2 2 X Y 1 2 2 1 2 2 2 2 p p a a cosθ 2.a .a sin θ (1 cos θ )        (2.17) T ừđótínhđượcgóc 2 :  2 = atan2(sin 2 ,cos 2 ) (2.18) Th ếC 1 , S 1 vàophươngtrình (2.13) và (2.14) thu được: (a 1 +a 2 C 2 ).C 1 – a 2 S 2 .S 1 = p x a 2 S 2 .C 1 +( a 1 +a 2 C 2 ).S 1 = p y Giải phương trình bậc nhất với ẩn C 1 , S 1 và sử dụng (2.16) thu được: 1 2 2 x 2 2 y x 1 2 2 x y y 1 2 2 y 2 2 x 1 2 2 x y (a a C ).p a S .p C p p (a a C ).p a S .p S p p               (2.19)  1 = atan2(S 1 ,C 1 ) (2.20) T ừphươngtrình (2.15) ta có: d 3 = - p z (2.21) M ặtkháctừphươngtrình (2.4) có: n x = C 1 (C 2 C 4 + S 2 S 4 ) – S 1 (S 2 C 4 – C 2 S 4 ) (2.22) Rút g ọntheocáccôngthứclượnggiácthuđược: n X = cos 1 .cos( 2 - 4 ) – sin 1 .sin( 2 - 4 ) = cos( 1 + 2 - 4 ) (2.23) sin( 1 + 2 - 4 ) = 2 x 1 n  (2.24) 2 1 2 4 x x ( θ θ θ ) atan2( 1 n ,n )     2 4 1 2 x x θ θ θ atan2( 1 n ,n )     (2.25) V ậyhệphươngtrình độnghọcngượccủarobotScaraSerpentlà:     1 2 2 x 2 2 y 1 2 2 x y 1 2 2 y 2 2 x 1 2 2 x y 1 1 1 2 2 2 2 X Y 1 2 2 1 2 2 2 2 2 2 2 3 z 2 4 1 2 x x (a a C ).p a S .p C p p (a a C ).p a S .p S p p atan2 S ,C p p a a cos θ 2.a .a sin θ (1 cos θ ) atan2 S ,C d p θ θ θ atan2( 1 n ,n )                                             (2.26) . Chương 6: Động học robot Scara Serpent Robot Scara Serpent có cấutrúcđộng họcđượcbiểu diễn nhưtrên Hình 2.4. Robot có 3 trụcquayvà 1 bàn kẹp,tuynhiên. độnghọcthuậncủa robot đượctiếnh ành tuầntựtheocácbướcsau: Bước 1:Xácđịnhcáchệtoạđộ Ta sửdụngquyướcDenavit-Hartenberg đểmôtảđầyđủvị trí củacủatoàn thân robot công nghiệp tảcáchệtrục toạđộ gắnvớicáckhúctaycủa robot ScaraSerpent. Bước 2: Xây dựngbảngthôngsốDH Bảng 2.3: Tham sốDenavit– Hartenberg của robot ScaraSerpent. Thanh n ối  i ( 0 ) a i  i (rad) d i