Bài tập dài robot công nghiệp

I. Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ tọa độ các khớp (Joints) Robot hai thanh nối là robot với 2 khâu phẳng, vì vậy cơ chế và phạm vi hoạt động của robot nằm trong mặt phẳng, bao gồm hai khớp quay và hai thanh nối nh hình vẽ: 1. Thiết kế hệ toạ độ cho các thanh nối a. Tham số của thanh nối và khớp Xác định bộ thông số Denavit -Hartenberg (bảng thông số DH) Từ hình vẽ robot ta xác định đợc các thông số sau: + Độ dài pháp tuyến chung của khớp 1 và khớp 2 là a 1 + Góc chéo giữa hai trục khớp 1 và khớp 2 là 1 . + Đây là khớp quay cho nên khoảng cách đo dọc trục khớp động 1 từ đờng vuông góc chung giữa trục khớp động 2 và trục khớp động 1 tới đờng vuông góc chung giữa khớp động 1 và gốc robot là d 1 = 0. Tơng tự ta cũng dễ dàng có d 2 = 0. + Góc giữa hai đờng vuông góc chung là 1 Tơng tự xét khớp 2 và tay robot + Ta có độ dài pháp tuyến chung là a 2 + Góc chéo giữa hai trục khớp 2 và tay robot là 1 . + Góc giữa hai đờng vuông góc chung là 2 Vậy biến khớp là 1 và 2 b. Thiết lập hệ toạ độ Hệ toạ độ cố định o 0 x 0 y 0 z 0 có gốc o 0 đặt tại tâm trục khớp động 1, chiều x 0 hớng từ o 0 đên o 1 y 0 x 0 a 1 x 1 y 1 x 2 y 2 a 2 1 2 Khớp 1 Khớp 2 0 0 0 1 0 2 Z 1 Hệ toạ độ o 1 x 1 y 1 z 1 có gốc o 1 đặt tại tâm trục khớp động 2 Hệ toạ độ o 2 x 2 y 2 z 2 có gốc o 2 đặt tại tâm trục khớp động cuối khâu 2. Ba trục z 0 , z 1 , z 2 vuông góc với mặt phẳng tờ giấy. 2. Xây dựng phơng trình động học thuận cho robot Phơng trình động học thuận là phơng trình biểu diễn quan hệ vị trí và hớng của tay robot thông qua các biến khớp. Các biến khớp này là góc quay của khớp quay và độ dịch chuyển tịnh tiến đối với khớp tịnh tiến Căn cứ vào phơng trình động học thuận này khi biết vị trí của các khớp ta có thể xác định đợc vị trí và hớng của tay robot. Căn cứ vào các thông số và hệ toạ độ đã đợc thiết lập ta có bảng thông số DH của robot nh sau: Thanh (khâu) i i a i D i 1 1 * 0 l 1 0 2 2 * 0 l 2 0 *. Quan hệ giữa hai khung toạ độ o 1 x 1 y 1 z 1 và o 0 x 0 y 0 z 0 đợc xác định nh sau: - Quay xung quanh trục z 0 một góc 1 sao cho trục x 0 trùng với phơng của trục x 1 - Tịnh tiến dọc theo trục x 1 (phơng pháp tuyến chung) một đoạn bằng a 1 Vậy phép biến đổi tổng hợp nh sau: A 1 = Rot(z 0 , 1 ) Trans (a 1 ,0,0) Trong đó: Rot(z 0 , 1 ) là phép quay quanh trục z 0 một góc 1 Rot(z 0 , 1 ) = 1000 0100 00 00 11 11 cossin sincos Trans(a 1 ,0,0) là phép tịnh tiến khung toạ độ o 0 x 0 y 0 z 0 theo trục x 0 một đoạn bằng a 1 . Trans(a 1 ,0,0) = 1000 0100 0010 001 1 a Vậy 0 A 1 đợc tính nh sau: 0 A 1 = Rot(z 0 , 1 ) Trans (a 1 ,0,0) = 1000 0100 00 00 11 11 cossin sincos 1000 0100 0010 001 1 a 0 A 1 = 1000 0100 0 0 1111 1111 sinacossin cosasincos (1.1) *. Quan hệ giữa hai khung toạ độ o 2 x 2 y 2 z 2 và o 1 x 1 y 1 z 1 đợc xác định nh sau: - Quay xung quanh trục z 1 một góc 2 sao cho trục x 1 trùng với phơng của trục x 2 - Tịnh tiến dọc theo trục x 2 (phơng pháp tuyến chung) một đoạn bằng a 2 Vậy phép biến đổi tổng hợp nh sau: 1 A 2 = Rot(z 1 , 2 ) Trans (a 2 ,0,0) Rot(z 1 , 2 ) là phép quay quanh trục z 1 một góc 2 Rot(z 1 , 2 ) = 1000 0100 00 00 22 22 cossin sincos Trans(a 2 ,0,0) là phép tịnh tiến khung toạ độ o 1 x 1 y 1 z 1 theo trục x 1 một đoạn bằng a 2 . Trans(a 2 ,0,0) = 1000 0100 0010 001 2 a Vậy 1 A 2 đợc tính nh sau: 1 A 2 = Rot(z 1 , 2 ) Trans (a 2 ,0,0) = 1000 0100 00 00 22 22 cossin sincos 1000 0100 0010 001 2 a 1 A 2 = 1000 0100 0 0 2222 2222 sinacossin cosasincos (1.2) Phơng trình động học thuận đợc xác định nh sau: 0 A 2 = 0 A 1 1 A 2 =             θθθ θθ−θ 1000 0100 0 0 1111 1111 sinacossin cosasincos             θθθ θθ−θ 1000 0100 0 0 2222 2222 sinacossin cosasincos Nh©n hai ma trËn trªn vµ rót gän ta cã: 0 A 2 = ( ) ( ) ( ) ( ) ( ) ( )             θ+θ+θθ+θθ+θ θ+θ+θθ+θ−θ+θ 1000 0100 0 0 112122121 112122121 sinasinacossin cosacosasincos (1.3) Hay: 0 A 2 =             1000 zzzz yyyy xxxx paon paon paon = ( ) ( ) ( ) ( ) ( ) ( )             θ+θ+θθ+θθ+θ θ+θ+θθ+θ−θ+θ 1000 0100 0 0 112122121 112122121 sinasinacossin cosacosasincos (1.4) Nh vËy hÖ ph¬ng tr×nh ®éng häc thuËn cña robot nh sau: n x = cos (θ 1 + θ 1 ) n y = sin (θ 1 + θ 1 ) n z = 0 o x = - sin (θ 1 + θ 1 ) o y = cos (θ 1 + θ 1 ) o z = 0 a x = 0 a y = 0 a z = 1 p x = a 1 cosθ 1 + a 2 cos (θ 1 + θ 1 ) p y = a 1 sinθ 1 + a 2 sin (θ 1 + θ 1 ) p z = 0 3. X©y dùng ph¬ng tr×nh ®éng häc ngîc cho robot Phơng trình động học ngợc robot nhằm xác định các giá trị biến khớp từ các vị trí và hớng của tay robot mong muốn. Phơng trình động học ngợc thờng khó giải và không có lời giải tổng quát cho mọi robot. Ta thấy rằng ma trận 0 A 2 đã biết, tức là vị trí và hớng của khung toạ độ tay robot, cần xác định giá trị các biến khớp 1 và 2 : 0 A 2 = 0 A 1 1 A 2 = 1000 zzzz yyyy xxxx paon paon paon (1.5) Nhân hai vế của phơng trình trên với ma trận nghịch đảo của ma trận 0 A 1 là ( 0 A 1 ) -1 ta có phơng trình sau: ( 0 A 1 ) -1 0 A 2 = 1 A 2 Ta có ( 0 A 1 ) -1 = 1000 0100 00 0 11 111 cossin asincos (1.6) Vậy ( 0 A 1 ) -1 0 A 2 = 1000 0100 00 0 11 111 cossin asincos 1000 zzzz yyyy xxxx paon paon paon ( 0 A 1 ) -1 0 A 2 = 1 A 2 tơng đơng với: ++++ ++++ 1000 11111111 111111111 zzzz yxyxyxyx yxyxyxyx Paon cosPsinPcosasinacososinocosnsinn asinPcosPsinacosasinocososinncosn = 1000 0100 0 0 2222 2222 sinacossin cosasincos (1.7) Cân bằng hai vế của hai ma trận tại cột thứ 4 ta đợc hệ phơng trình sau: =+ =+ 2211 22111 sinacosPsinP cosaasinPcosP yx yx (1.8) Với các thông số P x , P y và a 1 , a 2 đã biết giải hệ phơng trình này ta đợc các biến khớp 1 và 2 nh sau: Ta có: =+ +=+ 2211 12211 sinacosPsinP acosasinPcosP yx yx (1.9) Bình phơng ai vế 1.9, cộng vế theo vế ta đợc: 2 2221 2 1 22 2 acosaaaPP yx ++=+ Vậy + = 21 2 2 2 1 22 2 2 aa aaPP cosar yx (1.10) Bình phơng hai vế và cộng vế theo vế của 1.8 ta đợc: ( ) 2 2111 222 1 2 asinPcosPaPPa yxyx =+++ 1 2 2 2 1 22 11 2a aaPP sinPcosP yx yx ++ =+ 22 1 2 2 2 1 22 1 22 1 22 2 yx yx yx y yx x PPa aaPP sin PP P cos PP P + ++ = + + + + + ++ = 2222 1 2 2 2 1 22 1 2 yx x yx yx PP P arcsin PPa aaPP arcsin (1.11) Vậy: + = + + ++ = 21 2 2 2 1 22 2 2222 1 2 2 2 1 22 1 2 2 aa aaPP cosar PP P arcsin PPa aaPP arcsin yx yx x yx yx (1.12) II. Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay robot Để xây dựng quan hệ giữa tốc độ của khớp và tốc độ của tay robot ta cần phải thiết lập phơng trình động lực học cho robot. Gọi v là tốc độ chuyển động tịnh tiến của tâm vật thể, là tốc độ góc, m là khối lợng vật thể, J t là mômen quán tính của vật thể. Sử dụng phơng pháp Lagrange để thiết lập phơng trình động lực học cho robot. Giả thiết khớp 1 sinh ra mômen M 1 tác dụng giữa bệ và thanh nối 1, khớp 2 sinh ra mômen M 2 tác dụng giữa thanh nối 1 và 2. Trọng lực có hớng theo trục y. 1. Tính tốc độ của một điểm bất kỳ trên robot Nh trong phần I ta có phép biến đổi toạ độ từ thanh nối 2 đến hệ toạ độ gốc là 0 A 2 0 A 2 = 0 A 1 1 A 2 (2.1) Trong đó: A 1 là phép biến đổi toạ độ từ thanh nối 1 đến hệ toạ độ gốc, là hàm của biến 1 0 A 1 = 1000 0100 0 0 1111 1111 sinacossin cosasincos 1 A 2 là phép biến đổi toạ độ từ thanh nối 2 đến hệ toạ độ thanh nối 1, là hàm của biến 2 1 A 2 = 1000 0100 0 0 2222 2222 sinacossin cosasincos Và 0 A 2 = ( ) ( ) ( ) ( ) ( ) ( ) ++++ ++++ 1000 0100 0 0 112122121 112122121 sinasinacossin cosacosasincos Giả thiết biểu diễn một điểm bất kỳ nào đó trên thanh nối i nh sau [ ] T iii i ,z,y,xr 1= (2.2) Vậy vị trí của điểm đó trong hệ toạ độ gốc o 0 x 0 y 0 z 0 là: rAr i i 00 = = ( ) ( ) ( ) ( ) ( ) ( ) ++++ ++++ 1000 0100 0 0 112122121 112122121 sinasinacossin cosacosasincos 1 i i i z y x (2.3) Vì thanh nối là vật thể cứng, nên quan hệ r i trong thanh nối i là hằng số, hay: 0= dt rd i (2.4) Lấy đạo hàm phơng trình (2.3) và chú ý đến (2.4) ta có phơng trình nh sau: r dt rd Ar)A( dt d dt rd i i i i T i 2 1i i i 0 i 00 0 0 A r =+== = Với khái niệm vết của ma trận (Trace), bình phơng tốc độ của điểm đó đợc tính nh sau: = == = = 2 1 2 1 i j i 0 i i i 0 00002 A A r r r r i T j ji TT i rrTraceTracev (2.5) Sau khi biến đổi ta đợc: ji 2 1 2 1 i 0 ii i 0 2 A A = = = j k j T T i t rrTracev (2.6) 2. Động năng của robot đợc tính nh sau Động năng của một điểm bất kỳ trong thanh nối i dmrrTracedmvdK j k k T j == = = kj 2 1 2 1 i 0 T ii i 0 2 i2 A A 2 1 2 1 Do đó động năng của thanh nối i đợc tính bằng tích phân theo toàn bộ thanh nối i K i = = = = ithanh j k k T T j dmrrTracedK kj 2 1 2 1 i 0 ithanh ii i 0 2 A A 2 1 Ký hiệu J i = dmrr T ithanh ii là ma trận quán tính giả = i 2 i i i i i 2 2 i i i i i 2 2 i i i i2 2 i thanhthanh z thanh y ithanh x z thanh i z thanh i zy thanh i zx thanh i y thanh i yz thanh ii y ithanh i yx thanh i thanh xxz thanh i xy ithanh ii x ithanh i i dmdmrdmrdmr dmrdmrdmrrdmrr dmrdmrrdmrdmrr dmrdmrrdmrrdmr J Có thể viết lại ma trận J i nh sau: + + ++ = i i i i i i i i i izziyyixx iyzixz i iiyz izziyyixx ixy i iixzixy izziyyixx i mzmymxm zm III II ymI III I xmII III J 2 2 2 Trong đó [ ] T z i y i x ii ,r,r,rr 1= là bán kính vectơ biểu diễn trọng tâm của khâu i trong hệ toạ độ i. Các phần tử của J i đợc tính nh sau: ( ) += ithanh z i y i ixx dmrrI 22 ( ) = += ithanh x i i i ithanh y i x i ixy dmrxm dmrrI Các thành phần còn lại đợc tính tơng tự Tổng động năng của robot đợc tính nh sau: = = = = = 2 1 2 1 2 1 i 0 i 0 1 AA 2 1 i j k k j k T i j n i i JTraceKK (2.7) Hay = = = = 2 1 2 1 2 1 2 1 i j k k j jk hTraceK trong đó thành phần ma trận h jk đợc tính theo: = = n i k T i i j i jk A J A Traceh 1 00 Do 0 0 = j i A (j > i) nên có thể viết h jk nh sau: ( ) = = n k,jmaxi k T i i j i jk A J A Traceh 00 Do h jk = h kj nên ma trận vuông H() là ma trận đối xứng, đồng thời động năng là một đại lợng dơng với 0 , nên các thành phần của H() là các số dơng. 3. Thế năng của robot đợc tính nh sau Thế năng của thanh nối thứ i đợc xác định theo biểu thức: r i i 0T ii A g m - P = Với g = [ g x g y g z 1] T là véctơ gia tốc trọng trờng m i là khối lợng thanh nối thứ i r i là véctơ vị trí thanh nối i trong hệ toạ độ của thanh nối thứ i Tổng thế năng của hệ thống là: P = = 2 1 0 i i i T i rAgm Ta thấy rm i i là cột thứ t của ma trận J i nên ta có tổng thế năng nh sau: [ ] = = 2 1 0 1000 i T ii T JAgP (2.8) 4. Mô hình động lực học robot Hàm Lagrange của robot là: L = ( ) = n i ii PK 1 Thay lần lợt các biểu thức vào và biến đổi ta có: rAgm A J A TraceL i i n i T i i k . i j i k j . k i i j i == = = + = 1 0 2 1 1 1 00 2 1 (2.9) Phơng trình Lagrange có dạng nh sau: Mi . i . i F LL dt d = F Mi - động lực, hình thành trong khớp động thứ i khi thực hiện chuyển động. Ta có: ( ) . H KL = = ( ) ( ) . . . HH L dt d = ( ) ( ) = P H L . T . 2 1 Do đó phơng trình động lực học có dạng: ( ) ( ) ( ) ( ) M . . T . . F P HHH = + + 2 1 Ta ký hiệu: ( ) ( ) = = K qHHH,V . T . . 2 1 gọi là thành phần mômen nhớt và hớng tâm. ( ) ( ) = P G gọi là thành phần mômen trọng lực. [...]... phơng trình ở phần 1 và 2, vẽ đờng biểu diễn vị trí và tốc độ của khớp khi tay robot di chuyển từ vị trí [0.4; 0.0 m] đến [0; 0.4m] theo một quỹ đạo đờng thẳng Đồ thị tốc độ đặt trớc của tay robot dọc theo quỹ đạo cho trớc Khi robốt di chuyển từ vị trí (0.4; 0.0m) đến (0.0; 0.4m) theo một đờng thẳng khi đó: vận tốc của tay robot, quỹ đạo chuyển động, p x, py, pz là các đại lợng đã biết; thời gian chuyển... sin ( + ) + a sin 1 2 1 1 = 2 0 1 Bình phơng tốc độ của tay robot là: v 2 tay = r tay 0 T 0 r tay 0 = Trace r 0 tay r tay = T T 2 0A 2 0A2 2 2 2 Trace i r tay j r tay i =1 i j j =1 = ( a 1 sin 1 + a 2 sin ( 1 + 2 ) ) + ( a 1 cos 1 + a 2 cos( 1 + 2 ) ) 2 2 Khai triển vận tốc của tay robot theo hai thành phần trục x và trục y và ta có thể viết rằng: v2... khớp cho trớc ta giải hệ phơng trình vi phân bậc hai này thì thu đợc các biến khớp là góc 1 và 2 , vận tốc góc 1 , 2 , gia tốc góc 1 , 2 5 Quan hệ giữa tốc độ của khớp và tốc độ của tay robot Biểu diễn tay robot trên thanh nối 2 nh sau: 2 r tay = [ 0,0,0,1] T Vậy vị trí của điểm đó trong hệ toạ độ gốc o0x0y0z0 là: 0 r tay = 0 A 2 2 r tay cos( 1 + 2 ) sin( 1 + 2 ) sin ( + ) cos( + ) 1... acos((px^2+py^2-l1^2-l2^2)/(2*l1*l2)); 2 Chơng trình tính toán vận tốc độ của các khớp theo tốc độ tay rô bốt -Hàm tính tốc độ góc của thanh 1: function y = dhone(vy,vx,t1,t2) l1=.4; l2=.3; % vy van toc cua tay robot theo phuong y % vx van toc cua tay robot theo phuong x % t1 goc quay cua khop 1 % t2 goc quay cua khop 2 y = sin(t1+t2)*vy/(l1*sin(t2))+cos(t1+t2)*vx/(l1*sin(t2)); -Hàm tính tốc độ góc của thanh 2: function y = dhtwo(vy,vx,t1,t2)... 1 g1 = 1 / 2 0 0 1 1 1 = m1gl1 + m 2 gl1 cos 1 + m 2 gl 2 cos( 1 + 2 ) 2 2 1 / 2 T 0 m 2 [ 0 g 0 0] = m 2 gl 2 cos( 1 + 2 ) 2 0 1 g2 = Vậy phơng trình động lực học của robot là: M = H() + V , + G( ) 4 1 2 2 2 M 1 3 m1 l 1 + 3 m 2 l 2 + m 2 l 2 cos 2 M = 1 1 2 2 m 2 l1 + m 2 l 2 2 3 2 1 1 2 m 2 l1 + m 2 l 2 2 3 2 1 + 1 2 m2l2 2 3 2 1... trong khoảng thời gian [.75 1] Ta có: s1= vo/8 s2= vo/2 s3= vo/8 Vậy s1+ s2+ s3=3vo/4 Khi tay rô bốt di chuyển từ điểm (0.4 0) đến (0 0.4) quãng đợc đi đợc là: 0.4 2 Suy ra: vo=1.6 2 /3 Thay vào các công thức trên ta đợc: v1=6.4 2 t/3 v2=1.6 2 /3 v3=-6.4 2 (t-1)/3 Chiếu véc tơ vận tốc lên hai trục x và y ta đợc: v1x=-6.4t/3 v1y=6.4t/3 v2x=-1.6/3 v2y=1.6/3 v3x=6.4(t-1)/3 v3y=-6.4(t-1)/3 Vị trí của... trí góc quay của khớp 1: Đồ thị vận tốc góc quay của khớp 1 Đồ thị vị trí góc quay của khớp 2: Đồ thị vận tốc góc quay của khớp 2: IV Thiết kế bộ điều khiển động học ngợc Từ phơng trình động lực học của robot: M = H() + V , + G( ) 4 1 2 2 2 M 1 3 m1 l 1 + 3 m 2 l 2 + m 2 l 2 cos 2 M = 1 1 2 2 m 2 l1 + m 2 l 2 2 3 2 1 1 2 m 2 l1 + m 2 l 2 2 3 2 1 + 1 2 m2l2 2 3 2 1 2 2... ) ( đặt ) Đặt U = d + H 1 N M Vậy V+G = N e=U Từ phơng trình trên ta có đợc mômen của khớp cần điều khiển: M dk = H d U + N 1 Sơ đồ cấu trúc của hệ thống điều khiển nh sau: d H N (, ) Robot Bộ ĐK phụ Bộ điều khiển vòng trong (khử phi tuyến) Bộ điều khiển vòng ngoài (đảm bảo độ ổn định, chất lượng quá trình quá độ, chế độ tĩnh) Tiếp theo ta đi đến thiết kế bộ điều khiển U Bộ điều khiển . học ngợc -Hàm tính góc 1 : function y = teta_1(px,py) l1 = 0.4; % chieu dai cua thanh noi 1 (m) l2 = 0.3; % chieu dai cua thanh noi 2 (m) % goc cua khop mot (rad) y = asin((l1^2+px^2+py^2-l2^2)/(2*l1))-. asin(px/sqrt(px^2 + py^2)); -Hàm tính góc 2 : function y=teta_2(px,py) l1=.4; % chieu dai cua thanh noi 1 (m) l2=.3; % chieu dai cua thanh noi 2 (m) % goc cua khop hai (rad) y = acos((px^2+py^2-l1^2-l2^2)/(2*l1*l2)); 2.