Giới thiệu và tìm hiểu về Robot SCADA

CHƯƠNG II. GIỚI THIỆU VÀ TÌM HIỂU VỀ ROBOT SCADA 2.1 Giới thiệu chung về động học Robot. Hệ trục tọa độ và bảng D-H cho 4 khớp của tay máy Qui ước về cách trình bày như 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à vị trí của khâu thứ i so với khâu thứ i-1 (theo mô hình D-H): Thay thông số 4 khớp vào ma trận chuyển đổi đồng nhất, Từ 4 ma trận chuyển đổi đồng nhất trên ta tìm ra ma trận chuyển vị từ hệ tọa độ cuối về hệ tọa độ chuẩn như sau: Giải xong phần động học thuận cho tay máy. Một khi ma trận T04 được nhập bởi người dùng thì các thông số trong ma trận T04 là nx, ny,…pz đã biết. Việc giải bài toán động học ngược về vị trí là tìm bốn thông số khớp θ2, θ3 và θ4 bằng cách dựa vào ma trận T04 đã biết 2.2. Động học ngược về vị trí cho tay máy . Đồng nhất 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) thì ta có hệ 3 phương trình sau: Từ (1) va (2) ta tìm ra Tang(θ1) = yc/xc, suy ra θ1 = arctang(yc/xc) (4) Và ma trận xoay tìm được từ kết cấu của tay máy là: Đồng nhất ma trận chuyển đổi đồng nhất T04 với ma trận xoay R để tìm mối liên hệ về mặt đại số giữa θ2, θ3 và θ4 như sau: Để tay máy hoạt động thì khâu cuối phải có phương thẳng đứng và có chiều hướng xuống mặt phẳng bàn. Vì thế điểm cuối tay máy có tọa độ so với O1 là (x4, z4-d5) Vì O3 trùng O4 chiếu theo MP (O1x1z1). Nên vị trí của hệ tọa độ O3 so với O1 là (x4, z4-d5+d4). Một khi đã có vị trí hệ tọa độ O3 so với O1 thì bây giờ ta giải nghiệm θ2 và θ3 riêng từ khớp thứ hệ tọa độ O1 đến O3 để đơn giản. Hệ trục tọa độ và bảng D-H cho khớp 2, khớp 3 và khớp 4 của tay máy Ma trận chuyển đổi tọa độ từ khớp 3 về khớp 2 và từ khớp 4 về khớp 3 là: Từ 2 ma trận chuyển đổi đồng nhất vừa trên ta tìm ra ma trận chuyển vị từ hệ tọa độ thứ III về hệ tọa độ thứ I như sau: Đồng nhất vector vị trí trong ma trận A23 với tọa độ O3(x3, y3), ta có được: Tổng bình phương 2 vế của hệ phương trình trên, Ta có được: (d3Cos(θ23)+d2Cos(θ2))2 + (d3Sin(θ23)+d2Sin(θ2))2 = x32+y32 Tương đương với: d32(Cos(θ23)2+ Sin(θ23)2) + d2 2(Cos(θ2)2+ Sin(θ2)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) = x3 2+y3 2 Tách các phần tử khác Cos(θ3) sang vế phải thì ta có: Cos(θ3) = (( x32+y32 ) - ( d22+d32 )) / 2d2d3 (a) Ở đây θ3 có 2 nghiệm tương ứng với 2 cấu hình của tay máy có thể thõa mãn yêu cầu. Vì kết cấu của tay máy nên nghiệm θ3 âm được chọn. Vì thế nghiệm θ3 là: θ3 = - Arcos[(( x32+y32 ) - ( d22+d32 )) / 2d2d3] (b) Thay (a) vào (1), ta tìm ra θ2 như sau: Biểu thức (1) được viết lại : d3Cos(θ23) + d2Cos(θ2) = x3 Khai triển Cos(θ23) theo qui ước ở trang trên như sau: d3.Cosθ2.Cos θ3 - d3.Sinθ2.Sinθ3 + d2Cos(θ2) = x3 Rút Cosθ2 và Sin θ2 ra 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 và Sinθ2 vào biểu thức vừa trên nhằm hợp nhất thành một nghiệm duy nhất trong biểu thức: (d3.Cosθ3 + d2 ) ( 1- t2 ) - d3. 2 t.Sinθ3 = x3 ( 1 + 2t2 ) Sắp xếp biểu thức trên thành dạng phương trình bậc II cho việc tìm nghiệm, ( x3 + d3.Cosθ3 + d2 ) t2 + 2 d3.Sinθ3. t + (x3- d3Cosθ3 - d2) = 0 ’=( d3.Sinθ3 )2 - ( x3 + d3.Cosθ3 + d2 )( x3- d3Cosθ3 - d2 ) Nghiệm t tìm được là: t1,2 = (-d3.Sinθ3 ± ' ) / (x3 + d3.Cosθ3 + d2 ) Do kết cấu của tay máy và nghiệm θ3 được chọn là âm nên nghiệm t được chọn là dương: t = (-d3.Sinθ3 + ' ) / (x3 + d3.Cosθ3 + d2 ) ta suy ra: θ2 = Arctang(2 t / ( 1- t2)) (c) Tìm θ4: Theo biểu thức số (5) thì θ2 + θ3 + θ4 = -Pi/2 , giờ ta đã tìm được θ2 và θ3 Nên suy ra: θ4 = -Pi/2 – ( θ2 + θ3 ) 2.2 Đối với Robot Scada Robot Scada là một trong những loại robot có khả năng linh họat trong các thao tác và giám sát. Robot Scada gồm có 4 khâu như hình bên dưới Đây là Robot có cấu hình kiểu RRTR(bao gồm khớp quay R và một 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 các khâu như trên hình. D1=550 A1+A2=670 D3=300 Bảng D-H Ma trận chuyển vị 1 1 1 1 1 1 1 10 1 1 0 a C 0 a 0 0 1 0 0 0 1 C S S C S T d   2 2 2 2 2 2 2 21 2 0 a 0 a 0 0 1 0 0 0 0 1 C S C S C S T   2 3 3 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 T d  4 4 4 43 4 4 0 0 0 0 0 0 1 0 0 0 1 C S S C T d   12 4 12 4 12 4 12 4 1 1 12 2 12 4 12 4 12 4 12 4 1 1 12 20 0 1 2 3 4 1 2 3 4 1 3 4 0 0 0 0 1 0 0 0 1 C C S S C S S C C a C a S C C S S S C C S a S a T T T T T d d d             Bái toán vị trí ngược Biết các giá trị px py tìm các giá trị 1 2 Ta có hệ phương trình: a1C1 + a2C12 = px a1S1 + a2S12 = py Khớp i αi ai di 1 1 0 a1 d1 2 2 180 a2 0 3 0 0 0 d3 4 4 0 0 d4 12 2 2 2 2 1 1 12 1 2 12 2 2 2 2 2 2 1 1 1 12 1 2 12 2 p 2 p 2 x y C a C C a a C a S a S S a a S a         C2 = [(xc 2 + yc 2) – (a1 2 + a2 2)] / (2a1a2) 2 2 1 1 2 12 2 2 1 1 2 12 2 2 2 1 1 1 1 2 (p a C ) (a ) (p a S ) (a ) 2 2 x y x y x y C S a C p a S p p p a            Đặt : A=2a1 px B=2 a1 py C=px 2+py 2-a2 2 Ta có phương trình dạng ACos+Bsin=C Đặt 2 2 2 2 cos( ) sin( ) A A B B A B       1 2 2 cos( ) C A B      Bài toán động học thuận 1 1 12 2 1 1 12 20 1 3 4 1 E C a C a S a S a p d d d             1 2 3 4 0 1 2 3 4 1 2 3 4 1 2 3 4 x x x x y y y y A z z z z p p p p d p p p p d J p p p p d d                                                            1 1 12 2 12 2 1 1 12 2 12 2 0 0 0 0 0 0 1 0 1 1 0 1 S a S a S a C a C a C a              1 1 12 2 12 2 1 1 1 12 2 1 12 2 2 1 1 12 2 12 20 2 1 1 12 2 1 12 2 2 3 3 4 1 2 4 0 0 ( ) 0 0 ( ) 0 0 1 0 1 1 0 1 E S a S a S a S a S a S a C a C a C a C a C a C a v d d                                                                Bài toán động học ngược 12 2 12 2 1 2 2 1 2 2 1 1 12 2 1 1 12 2 0 1 1 2 2 1 2 2 1 1 1 1 1 2 2 1 2 2 0 0 0 0 0 0 1 0 0 1 A C a S a a a S a a S C a S a S a S a a a S a a SJ C a S a a a S a a S                        12 1212 2 12 2 1 21 2 2 1 2 2 1 1 1 12 2 1 11 1 12 2 1 1 12 2 2 1 2 2 1 2 2 3 4 1 1 1 1 1 2 2 1 2 2 0 0 ( ) ( 0 0 0 0 1 0 0 1 x y x x y z C p S pC a S a a Sa a S a a S p C a S a p S aC a S a S a S a p a a S a a S pd C a S a a a S a a S                                                       12 2 1 2 2 1 1 2 2 ) y z x y S a p a a S p C p S p a S                          Cách 2: a2C12 = xc - a1C1 a2S12 = yc - a1S1 Bình phương hai vế của hai phương trình rồi cộng lại, ta đươc: a2 2 = (xc 2 + yc 2) - 2 a1(xc C1 + yc S1) + a1 2 Phương trinh trên 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 là khoảng thời gian để đầu công tác chạy từ điểm A đến điểm B