CHƯƠNG II GIỚI THIỆU VÀ TÌM HIỂU VỀ ROBOT SCADA 2.1 Giới thiệu chung động học Robot Hệ trục tọa độ bảng D-H cho khớp tay máy Qui ước cách trình bày sau: Cos(θ23)=Cosθ2.Cosθ3-Sinθ2.Sinθ3 Sin(θ23)=Sinθ2.Cosθ3+Cosθ2.Sinθ3 Cos(θ234)=Cos(θ23).Cosθ4 – Sin(θ23).Sin θ4 Sin(θ234)=Sin(θ23).Cosθ4 + Cos(θ23).Sin θ4 Ma trận Ai-1i mơ tả hướng vị trí khâu thứ i so với khâu thứ i-1 (theo mơ hình D-H): Thay thông số khớp vào ma trận chuyển đổi đồng nhất, Từ ma trận chuyển đổi đồng ta tìm ma trận chuyển vị từ hệ tọa độ cuối hệ tọa độ chuẩn sau: Giải xong phần động học thuận cho tay máy Một ma trận T nhập người dùng thơng số ma trận T04 nx, ny,…pz biết Việc giải toán động học ngược vị trí tìm bốn thơng số khớp θ2, θ3 θ4 cách dựa vào ma trận T04 biết 2.2 Động học ngược vị trí cho tay máy Đồng ma trận tọa độ điểm Pc = (Px, Py, Pz)-1 với tọa độ điểm cuối tay máy (xc, yc, zc) ta có hệ phương trình sau: Từ (1) va (2) ta tìm Tang(θ1) = yc/xc, arctang(yc/xc) (4) suy θ1 = Và ma trận xoay tìm từ kết cấu tay máy là: Đồng ma trận chuyển đổi đồng T04 với ma trận xoay R để tìm mối liên hệ mặt đại số θ2, θ3 θ4 sau: Để tay máy hoạt động khâu cuối phải có phương thẳng đứng có chiều hướng xuống mặt phẳng bàn Vì điểm cuối tay máy có tọa độ so với O1 (x4, z4-d5) Vì O3 trùng O4 chiếu theo MP (O1x1z1) Nên vị trí hệ tọa độ O3 so với O1 (x4, z4-d5+d4) Một có vị trí hệ tọa độ O3 so với O1 ta giải nghiệm θ2 θ3 riêng từ khớp thứ hệ tọa độ O1 đến O3 để đơn giản Hệ trục tọa độ bảng D-H cho khớp 2, khớp khớp tay máy Ma trận chuyển đổi tọa độ từ khớp khớp từ khớp khớp là: Từ ma trận chuyển đổi đồng vừa ta tìm ma trận chuyển vị từ hệ tọa độ thứ III hệ tọa độ thứ I sau: Đồng vector vị trí ma trận A23 với tọa độ O3(x3, y3), ta có được: Tổng bình phương vế hệ phương trình trên, Ta có được: (d3Cos(θ23)+d2Cos(θ2))2 + (d3Sin(θ23)+d2Sin(θ2))2 = x32+y32 d32(Cos(θ23)2+ Tương đương với: Sin(θ23)2) + d22(Cos(θ2)2+ Sin(θ2)2) + d2d3(Cos(θ23) Cos(θ2)+ Sin(θ23) Sin(θ2)) = x32+y32 Rút gọn biểu thức lại: d22+d32+2 d2 d3 Cos(θ2 + θ3 – θ2) = x32+y32 Tách phần tử khác Cos(θ3) sang vế phải ta có: Cos(θ3) = (( x32+y32 ) - ( d22+d32 )) / 2d2d3 (a) Ở θ3 có nghiệm tương ứng với cấu hình tay máy thõa mãn u cầu Vì kết cấu tay máy nên nghiệm θ3 âm chọn Vì nghiệm θ3 là: θ3 = - Arcos[(( x32+y32 ) - ( d22+d32 )) / 2d2d3] (b) Thay (a) vào (1), ta tìm θ2 sau: Biểu thức (1) viết lại : d3Cos(θ23) + d2Cos(θ2) = x3 Khai triển Cos(θ23) theo qui ước trang sau: d3.Cosθ2.Cos θ3 - d3.Sinθ2.Sinθ3 + d2Cos(θ2) = x3 Rút Cosθ2 Sin θ2 ngoài: (d3.Cosθ3 + d2 ) Cosθ2 - d3.Sinθ3.Sin θ2 = x3 Đặt Cosθ2=( 1- t2 )/(1+t2), Sinθ2=2 t / ( 1+ t2) Thay Cosθ2 Sinθ2 vào biểu thức vừa nhằm hợp thành nghiệm biểu thức: (d3.Cosθ3 + d2 ) ( 1- t2 ) - d3 t.Sinθ3 = x3 ( + 2t2 ) Sắp xếp biểu thức thành dạng phương trình bậc II cho việc tìm nghiệm, ( x3 + d3.Cosθ3 + d2 ) t2 + d3.Sinθ3 t + (x3- d3Cosθ3 d 2) = ’=( d3.Sinθ3 )2 - ( x3 + d3.Cosθ3 + d2 )( x3- d3Cosθ3 - d2 ) Nghiệm t tìm là: t1,2 = (-d3.Sinθ3 ± ' ) / (x3 + d3.Cosθ3 + d2 ) Do kết cấu tay máy nghiệm θ3 chọn âm nên nghiệm t chọn dương: t = (-d3.Sinθ3 + ' ) / (x3 + d3.Cosθ3 + d2 ) ta suy ra: (c) θ2 = Arctang(2 t / ( 1- t2)) Tìm θ4: Theo biểu thức số (5) θ2 + θ3 + θ4 = -Pi/2 , ta tìm θ2 θ3 Nên suy ra: θ4 = -Pi/2 – ( θ2 + θ3 ) 2.2 Đối với Robot Scada Robot Scada loại robot có khả linh họat thao tác giám sát Robot Scada gồm có khâu hình bên Đây Robot có cấu hình kiểu RRTR(bao gồm khớp quay R khớp tịnh tiến) Bàn kẹp có chuyển động xoay xung quanh trục thẳng đứng Các hệ tọa độ gắn lên khâu hình D1=550 A1+A2=670 D3=300 Bảng D-H i 1 2 4 Khớp αi 180 0 a1 a2 0 di d1 d3 d4 Ma trận chuyển vị C1  S1 a1C1 T 01  T32  S1 C1 a1S1 0 d1 0 0 1 T21  T43  0 d3 0 T40  T10T21T32T43  C2 S2 a C2 S2 C2 a S2 0 1 0 0 C4  S4 0 S4 C4 0 0 d4 0 C12C4  S12 S4 C12 S4  S12C4 C1a1  C12 a2 S12C4  C12 S  S12 S4  C12C4 S1a1  S12 a2 0 1 d1  d3  d 0 Bái tốn vị trí ngược Biết giá trị px py tìm giá trị 1 2 Ta có hệ phương trình: a1C1 + a2C12 = px a1S1 + a2S12 = py p 2x  C12 a12  2C1C12 a1a2  C12 a22   2 2 p y  S1 a1  S1S12 a1a2  S12 a2 C2 = [(xc2 + yc2) – (a12 + a22)] / (2a1a2) (p x  a1C1 )  (a 2C12 )   2 (p y  a1S1 )  (a S12 )  2a1C1 px  2a1S1 p y  px2  p y2  a22 Đặt : A=2a1 px B=2 a1 py C=px2+py2-a22 Ta có phương trình dạng ACos+Bsin=C Đặt A A2  B B A2  B  cos( )  sin( )  cos(1   )  Bài toán động học thuận C1a1  C12 a2  S a S a  pE   1 12   d1  d  d      C A  B2  p x     p y   JA    p z   1     1   S1a1  S12 a2 C a C a 12  1    px  p x d p y p y  d p z  p z d    d  S12 a2 C12 a2 px     p y     p z          0  0  1  S12 a2 0 1   S1a1  S12 a2  C a C a 12 vE   1    C12 a2  1   ( S1a1  S12 a2 )1  S12 a22      0  2   (C1a1  C12 a2 )1  C12 a22    1   d3   d3          1  2  4  Bài toán động học ngược C12 a2   a1a2 S   C1a1  S12 a2  1 a1a2 S J A     C1a1   a1a2 S C12 a2   aa S 2 1   C a      1  S12 a2     a1a2 S  d3         C1a1   a1a2 S  0  S1a1  S12 a2   0 a1a2 S  1   S1a1 1  a1a2 S C12 p x  S12 p y   S12 a2  0   a1S a1a2 S    p x   S1a1  S12 a2     (C1a1  S12 a2 ) p x  ( S1a1  S12 a2 ) p y  0  p y      a1a2 S a1a2 S     p     1   z    p z       S1a1   C p S p    x y  1      a1a2 S a2 S   S12 a2 a1a2 S Cách 2: a2C12 = xc - a1C1 a2S12 = yc - a1S1 Bình phương hai vế hai phương trình cộng lại, ta đươc: a22 = (xc2 + yc2) - a1(xc C1 + yc S1) + a12 Phương trinh có dạng: aC1 + bS1 =c Đặt cos(λ) = a / (a2 + b2)0.5 sin(λ) = a / (a2 + b2)0.5  cos(λ - 1) = m  λ - 1 = + - n  1 = λ + - n Tìm vận tốc góc: 1 = 1 / t 1 = 2 / t Với t khoảng thời gian để đầu công tác chạy từ điểm A đến điểm B ... t2)) Tìm θ4: Theo biểu thức số (5) θ2 + θ3 + θ4 = -Pi/2 , ta tìm θ2 θ3 Nên suy ra: θ4 = -Pi/2 – ( θ2 + θ3 ) 2.2 Đối với Robot Scada Robot Scada loại robot có khả linh họat thao tác giám sát Robot. .. sau: Từ (1) va (2) ta tìm Tang(θ1) = yc/xc, arctang(yc/xc) (4) suy θ1 = Và ma trận xoay tìm từ kết cấu tay máy là: Đồng ma trận chuyển đổi đồng T04 với ma trận xoay R để tìm mối liên hệ mặt đại...Thay thông số khớp vào ma trận chuyển đổi đồng nhất, Từ ma trận chuyển đổi đồng ta tìm ma trận chuyển vị từ hệ tọa độ cuối hệ tọa độ chuẩn sau: Giải