* Thiết lập phương trình động học của tay máy robot

* Thiết lập phương trình động học của tay máy robot BÀI KIỂM TRA MÔN ROBOT CÔNG NGHIỆP Đề Cho cánh tay robot 3 bậc tự do có các thông số l1 = 0 5 m; l2 = 1m; l3=0 8m a Xây dựng phương trình động học t[.]

BÀI KIỂM TRA MÔN ROBOT CÔNG NGHIỆP Đề: Cho cánh tay robot bậc tự có thơng số: l1 = 0.5 m; l2 = 1m; l3=0.8m a Xây dựng phương trình động học thuận robot c Xây dựng quỹ đạo bậc với: c Xây dựng quỹ đạo bậc với: t0=0, tf=10; θ0= 0.1, θ&0 = 0.15 , θf= 2, θ&f = 1.5 , t0=0, tf=10; θ0= -0.7, θ&0 = , θ&& = 0.5 θf= 2, θ&f = 1.5 , θ&&f = 0.1 Robot cơng nghiệp a Xây dựng phương trình động học tay máy robot - Bước 1:Gắn hệ toạ độ lên khâu: y2 x2 y3 x1 x3 y1 y0 x0 Ta gắn hệ trục tọa độ lên khâu hình vẽ: Trục z 0, z1, z2 z3 vng góc với tờ giấy - Bước 2: Lập bảng thơng số DH Thơng qua phân tích ta xác định thông số DH Robot Bảng 4.2: Bảng thông số DH Khâu θi αi di θ1 l1 θ2 l2 θ3 l3 Robot công nghiệp - Bước 3: Xác định matrận An Trên sở hệ toạ độ ấn định cho tất khâu liên kết Robot ta thiết lập mối quan hệ hệ toạ độ nối tiếp (n 0); (n-1); (n) phép quay tịnh tiến sau đây: Quay quanh trục zn-1 góc θ n Tịnh tiến dọc trục zn-1 đoạn dn Tịnh tiến dọc trục xn-1 đoạn an Quay quanh trục xn góc xoắn αn Bốn phép biến đổi đồng thể quan hệ hệ toạ độ thuộc khâu thứ n so với hệ toạ độ thuộc khâu thứ n-1 tích chúng gọi ma trận A An = Rot(z,θ).Trans(0,0,0).Rot(x,α) cos θ  sin θ An =     − sin θ cos α cos θ cos α sin α sin θ sin α − cos θ sin α cos α a cos θ  a sin θ  d    Ta quy ước cách viết tắt hàm lượng giác sau: c1 = cos θ1 ; s1 = sin θ1 ; c = cos θ ; s = sin θ ; c3 = cos θ3 , s3 = sin θ3 Từ matrận tổng quát An ta xác định matrận A n Robot cụ thể sau: Robot công nghiệp c1 s A1 =  0  0 − s1 c1 0 0 c2 s A2 =  0  0 − s2 c2 0 l c2  l s  −1    c3 s A3 =  0  0 − s3 c3 0 l3c3  l3 s3  −1    l1c1  l1 s1     - Bước 4: Tính ma trận biến đổi + Matrận T3 = A3 T3 = A1 T3 = A2 A3 + Matrận + Matrận T3 = A1 T3 = A1 A2 A3 Như vậy: c3 s T3 =  0  0 − s3 c2 s T3 =  0  0 − s2 c2 0 0 c3 0 a3 c3  a3 s3  −1    a2 c2  c3 a2 s2  s3  0   0 s3 − c3 0 a3c3  a3 s3  −1    Robot công nghiệp c2 c3 − s2 s3 s c + c s T3 =     − c s − s c3 − s s + c c3 0 c (θ + θ3 ) − s (θ + θ3 )  s (θ + θ ) c(θ + θ ) 3 T3 =   0  0  c1 s T3 = A1 T3 =  0  0 − s1 c1 0 l3c2 c3 − l3c2 s3 + l2 c2  l3 s2 c3 − l3c2 s3 + l2 s2     l3c (θ + θ3 ) + l c2  l3 s (θ + θ3 ) + l s2     l1c1  c(θ + θ3 ) l1 s1  s (θ + θ3 )    c(θ1 + θ + θ ) − s (θ1 + θ + θ )  s(θ + θ + θ ) c(θ + θ + θ ) 3 T3 =   0  0  − s (θ + θ3 ) l3c(θ + θ3 ) + l c2  c(θ + θ3 ) l3 s (θ + θ3 ) + l s2    0  l1c1 + l c(θ1 + θ ) + l3c(θ1 + θ + θ ) l1c1 + l s(θ1 + θ ) + l3 s(θ1 + θ + θ )     Ma trận 0T3 matrận xác định hướng vị trí điểm tác động cuối n x n T3 =  y nz  0 ox ax oy ay oz az 0 px  p y  pz   1 Ta có hệ phương trình động học thuận robot sau:  nx = c(θ + θ + θ )   n y = s(θ + θ + θ ) ;   nz = Robot công nghiệp  ox = − s(θ + θ + θ )   o y = c(θ + θ + θ )   oz =  p x = l1c1 + l2c(θ + θ ) + l3c(θ + θ + θ )   p y = l1s1 + l2 s(θ + θ ) + l3 s(θ + θ + θ )   pz = Với l1=0.5, l2=1, l3=0.8 ta có tọa độ điểm tác động cuối: ( cần thay số khác)  p x = 0.5 cosθ + cos(θ + θ ) + 0.8 cos(θ + θ + θ )   p y = 0.5 sin θ + sin(θ + θ ) + 0.8 sin(θ + θ + θ )   pz = Robot công nghiệp b Xây dựng quỹ đạo bậc 3: Với: t0=0, tf=10; θ0=0.1, θ&0 = , θf= 2, θ&f = 1.5 , Phương trình quỹ đạo bậc 3: θ (t ) = a3t + a2t + a1t1 + a0 ⇒ θ&(t ) = 3a t + 2a t1 + a Ta có: θ0 = a0 θ f (t f ) = a3t f + a2t f + a1t f + a0 θ& = a θ&f (t f ) = 3a3t 2f + 2a2t1f + a1 Giải hệ phương trình ta được: a3 = − a2 = 3t 2f t 2f (θ f − θ0 ) + (θ f − θ0 ) − & & (θ f + θ0 ) t 2f & & θ0 − θ f tf tf (1) a1 = θ& a0 = θ0 Thay t0=0, tf= 10; θ0= 0.1, θ&0 = ,θf= 2, θ&f = 1.5 ,vào hệ phương trình (1) ta được:   a = 0.044   a2 = −0.293  a =1   a0 = 0.1 ( thay giá trị vào (1) để tìm a0, a1,a2,a3 , mỡi người tự thay số khác nha Sau thay vào pt bên Như phương trình quỹ đạo bậc là: θ (t ) = 0, 044t − 0.293t + t + 0,1 Robot công nghiệp c Xây dựng quỹ đạo bậc 5: Với: t0=0, tf=10; θ0= -0.7, θ&0 = , θ&& = 0.5 θf= 2, θ&f = 1.5 , θ&&f = 0.1 Phương trình quỹ đạo bậc 5: θ (t ) = a5t + a4t + a3t + a2t + a1t1 + a0 ⇒ θ&(t ) = 5a t + 4a t + 3a t + 2a t1 + a & ⇒ θ& (t ) = 20a5t + 12a4t + 6a3t1 + 2a2 Ta có: θ0 = a0 θ f (t f ) = a5t f + a4t f + a3t f + a2t f + a1t f + a0 θ& = a θ&f (t f ) = 5a5t 4f + 4a4t 3f + 3a3t 2f + 2a2t1f + a1 & = 2a θ& &(t ) = 20a t + 12a t + 6a t1 + 2a θ& f f f f f Giải hệ phương trình ta được: a5 = a4 = a3 = & & & 12(θ f − θ0 ) − 6(θ&f + θ& )t f + (θ f − θ )t f 2t 5f & & & −30(θ f − θ0 ) + 2(7θ&f + 8θ& )t f − (2θ f − 3θ )t f 2t 4f & & & 20(θ f − θ0 ) − 4(2θ&f + 3θ& )t f + (θ f − 3θ )t f & θ& & a1 = θ0 2t 3f (2) a2 = & & & Thay t0=0, tf=10; θ0= -0.7, θ&0 = , θ&& = 0.5 θf= 2, θ f = 1.5 , θ f = 0.1 vào hệ phương trình (2) ta được: Robot công nghiệp  a5 = −7.08.10−4   a4 = 0.021   a3 = −0.163 (thay giá trị vào (2) để tìm a1, a2, a3, a4, a5 sau  a = 0.05   a1 = vào pt bên ( mỗi người thay số gần với số ban đầu đừng thay số lớn q nha, ví dụ θ0= -0.7 thay θ0= -0.85 hoặc θ0= -0.6) Như phương trình quỹ đạo bậc là: θ (t ) = −7, 08.10−4 t + 0.021t − 0,163t + 0, 05t + t − 0, Kết quả mô phỏng maltab .. .Robot cơng nghiệp a Xây dựng phương trình động học tay máy robot - Bước 1:Gắn hệ toạ độ lên khâu: y2 x2 y3 x1 x3 y1 y0 x0 Ta... tác động cuối n x n T3 =  y nz  0 ox ax oy ay oz az 0 px  p y  pz   1 Ta có hệ phương trình động học thuận robot sau:  nx = c(θ + θ + θ )   n y = s(θ + θ + θ ) ;   nz = Robot. .. hệ phương trình (1) ta được:   a = 0.044   a2 = −0.293  a =1   a0 = 0.1 ( thay giá trị vào (1) để tìm a0, a1,a2,a3 , mỡi người tự thay số khác nha Sau thay vào pt bên Như phương trình