Tài liệu Báo cáo robot scara serpent

Robot scara serpent MỤC LỤC MỞ ĐẦU....................................................................................................................... 2 I: MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT................................3 1.1. Một số loại robot Scara của các hãng sản xuất.................................................3 1.2. Các thông số và vùng làm việc của robot Scara Serpent.................................5 1.2.1. Cấu tạo tay máy robot Scara Serpent.........................................................6 1.2.1.1. Cấu hình của robot Scara Serpent.......................................................6 1.2.1.2. Các thông số kỹ thuật của robot Scara Serpent..................................7 1.2.2. Giới hạn không gian làm việc của robot Scara Serpent............................8 1.3. Động học robot Scara Serpent............................................................................9 1.3.1. Động học thuận..............................................................................................9 1.3.2. Động học ngược...........................................................................................13 1.4. Động lực học robot Scara Serpent....................................................................15 1.4.1. Hàm Euler - Lagrange và các vấn đề động lực học.................................16 1.4.2. Động lực học robot Scara Serpent.............................................................17 1.4.2.1. Tính toán động năng và thế năng cho từng khớp.............................17 1.4.2.2. Phương trình động lực học..................................................................21 1.5. Mô tả đối tượng bằng hệ phương trình trạng thái.........................................25 II: XÂY DỰNG MÔ HÌNH HỆ THỐNG ĐIỀU KHIỂN................................27 2.1. Cấu trúc hệ thống điều khiển robot.................................................................27 2.2. Lựa chọn phương pháp điều khiển và bộ điều khiển PID...........................28 III: MÔ PHỎNG VỚI MÔ HÌNH ROBOT SCARA SERPENT..................31 3.1. Đặt vấn đề............................................................................................................31 3.2. Mô phỏng robot scara trên EASY-ROB..........................................................31 KẾT LUẬN....................................................................................................................34 TÀI LIỆU THAM KHẢO............................................................................................35 Sinh viên: Phạm Văn Khiền 1 Robot scara serpent MỞ ĐẦU Theo quá trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất. Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính linh hoạt cao đã hình thành và phát triển mạnh mẽ…Vì thế ngày càng tăng nhanh nhu cầu ứng dụng người máy để tạo ra các hệ sản xuất tự động linh hoạt. Robot ứng dụng rộng rãi và đóng vai trò quan trọng trong sản xuất cũng như trong đời sống. Robot là cơ cấu đa chức năng có khả năng lập trình được dùng để di chuyển nguyên vật liệu, các chi tiết, các dụng cụ thông qua các truyền động được lập trình trước. Khoa học robot chủ yếu dựa vào các phép toán về đại số ma trận. - Robot có thể thao tác như con người và có thể hợp tác với nhau một cách thông minh. - Robot có cánh tay với nhiều bậc tự do và có thể thực hiện được các chuyển động như tay người và điều khiển được bằng máy tính hoặc có thể điều khiển bằng chương trình được nạp sẵn trong chip trên bo mạch điều khiển robot. Để hệ điều khiển robot có độ tin cậy, độ chính xác cao, giá thành hạ và tiết kiệm năng lượng thì nhiệm vụ cơ bản là hệ điều khiển robot phải đảm bảo giá trị yêu cầu của các đại lượng điều chỉnh và điều khiển. Ngoài ra, hệ điều khiển robot phải đảm bảo ổn định động và tĩnh, chống được nhiễu trong và ngoài, đồng thời không gây tác hại cho môi trường như: tiếng ồn quá mức quy định, sóng hài của điện áp và dòng điện quá lớn cho lưới điện v.v... Sinh viên: Phạm Văn Khiền 2 Robot scara serpent I. MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT 1.1. Một số loại robot Scara của các hãng sản xuất Robot Scara là một trong những robot phổ biến nhất trong công nghiệp. Chuyển động của robot này rất đơn giản nhưng lại phù hợp với các dây chuyền và ứng dụng hữu hiệu trong nhiệm vụ nhặt và đặt sản phẩm. Robot Scara (Selectively Compliant Articulated Robot Arm) có nghĩa là tay máy lắp ráp chọn lọc. Cấu trúc động học loại tay máy này thuộc hệ phỏng sinh, có các trục quay, các khớp đều là thẳng đứng. Nó có cấu tạo hai khớp ở cánh tay, một khớp ở cổ tay và một khớp tịnh tiến. Các khớp quay hoạt động nhờ động cơ điện có phản hồi vị trí. Khớp tịnh tiến hoạt động nhờ xi-lanh khí nén, trục vít hoặc thanh răng. Một số loại robot Scara của các hãng sản xuất: Hình 1.1a: Turbo Scara SR60 của hãng Bosch. Hình 1.1b: Assembly Scara Robot của Hirata. Sinh viên: Phạm Văn Khiền 3 Robot scara serpent Hình 1.1c: Scara Robot của EPSON. Hình 1.1d: Scara Robot của DENSO. Hình 1.1e: Scara Robot of ADEPT. Hình 1.1f: Scara Robot of RANOME. Hình 1.1g: Scara Robot of KUKA. Hình 1.1h: Scara Robot of STAUBLI. Sinh viên: Phạm Văn Khiền 4 Robot scara serpent Hình 1.2: Robot Scara Serpent của FEEDBACK. Trong phần này tôi sẽ xây dựng mô tả toán học đối tượng robot Scara Serpent của FEEDBACK. 1.2. Các thông số và vùng làm việc của robot Scara Serpent Do chuyển động của robot Scara đơn giản, dễ dàng nên nó được sử dụng khá phổ biến trong công nghiệp. Ở đây nghiên cứu robot Scara Serpent (Hình 1.3) một loại cơ bản trong nhóm robot công nghiệp này. Hình 1.3: Robot Scara Serpent. Chiều cao của Robot có thể thay đổi dễ dàng bằng cách thay đổi vị trí gá thân robot trên trục cơ bản, giúp tay máy thuận lợi trong việc thay đổi công việc. Sinh viên: Phạm Văn Khiền 5 Robot scara serpent Với thiết kế động cơ truyền động cho cổ tay được đặt trên trục cơ bản và liên hệ với cổ tay bằng đai truyền, nên nó đảm bảo được góc quay của cổ tay không thay đổi trong quá trình tay máy chuyển động. Truyền động cho 2 khớp của tay máy và cổ tay bằng động cơ servo một chiều có phản hồi vị trí tạo thành một vòng điều khiển kín. Chuyển động thẳng đứng được thực hiện bằng piton khí nén. Robot Scara Serpent có thể được lập trình từ máy tính bằng cách đặt dữ liệu cho mỗi trục. Hoặc điều khiển bằng tay sử dụng thiết bị lái điện (steering) cho tay máy dùng các cuộn dây điện từ trong giá treo (pendant). 1.2.1. Cấu tạo tay máy robot Scara Serpent 1.2.1.1. Cấu hình của robot Scara Serpent Bao gồm một chuỗi các thanh cứng được liên kết với nhau bởi các khớp: Hình 1.4: Cấu hình và các hệ trục tọa độ gắn trên Robot Scara Serpent. Robot Scara Serpent gồm 3 khớp chuyển động quay và một khớp chuyển động tịnh tiến. Gắn cho mỗi thanh nối một hệ trục toạ độ, ta có: - Khớp 1 quay quanh trục z0 góc θ1. Sinh viên: Phạm Văn Khiền 6 Robot scara serpent - Khớp 2 quay quanh trục z1 góc θ2. - Khớp 3 chuyển động tịnh tiến theo trục z2 đoạn d3. - Khớp 4 quay quanh trục z3 góc θ4. 1.2.1.2. Các thông số kỹ thuật của robot Scara Serpent Thông số của động cơ 1, 2, 3 tương ứng với các khớp 1, 2, 4 của tay máy robot Scara Serpent (xem bảng 1.1). - Động cơ 1 truyền động cho khớp 1 (main). - Động cơ 2 truyền động cho khớp 2 (fore). - Động cơ 3 truyền động cho khớp 4 - khớp cổ tay (wrist). Bảng 1.1: Thông số các động cơ của robot. TT Loại U(V) I(A) M(Nm) N(v/p) P(W) J(Kg.m2) R() L(H ) 1 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6 2 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6 3 J12ZF 12 4,8 1,2102 2100 26 1,5.10-4 0,95 100 1 m(Kg) Bảng 1.2: Các thông số động học của robot Scara Serpent. TT Thông số Kích thước động học 1 M1 = 4 Kg Khối lượng thanh nối 1. 2 M2 = 1.5 Kg Khối lượng thanh nối 2. 3 M3 = 2 Kg 4 M4 = 0.6 Kg Khối lượng thanh nối 4. 5 a1 = 0.25 m Chiều dài thanh nối giữa 2 khớp main và fore . 6 a2 = 0.15 m Chiều dài thanh nối giữa 2 khớp fore và cổ tay . 7 D3 Chiều dài thanh nối d3 phụ thuộc vào chế độ làm việc của tay máy. Khối lượng thanh nối 3. Sinh viên: Phạm Văn Khiền 7 Robot scara serpent 1.2.2. Giới hạn không gian làm việc của robot Scara Serpent Các biến khớp có các giới hạn góc quay như sau : 1 = -96 0  960 ( so với trục Ox ). 2 = -115 0  1150 (so với trục thanh 1). Chuyển động quay của khớp thứ nhất có hình chiếu bằng trong hệ trục toạ độ OX0Y0 và OX1Y1 (Hình 1.5) tương ứng với góc quay tổng trong thực tế là 192 0. Chuyển động quay của khớp thứ hai có hình chiếu bằng trong hệ trục toạ độ OX 1Y1 và OX2Y2 tương ứng với góc quay tổng là 2300. Từ đó ta có thể thấy được hình chiếu giới hạn không gian làm việc của nó (Hình 1.6). Hình 1.5: Giới hạn góc quay của 2 khớp. Sinh viên: Phạm Văn Khiền 8 Robot scara serpent Hình 1.6: Giới hạn không gian làm việc của robot Scara Serpent. Như vậy khoảng không gian mà tay máy có thể với tới là toàn bộ hình trụ với đáy có đường giới hạn bên trong là một cung tròn có bán kính r = 0.231 (m) và đường giới hạn bên ngoài là đường tròn bán kính R = 0.4 (m). 1.3. Độ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 1.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. 1.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: Sinh viên: Phạm Văn Khiền 9 Robot scara serpent 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 1.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 1.3: Tham số Denavit – Hartenberg của robot Scara Serpent. Thanh nối i (0) ai i(rad) di (m) Biến Chuyển động 1 0 a1 1 0 1 Quay 2 -1800 a2 2 0 2 Quay 3 0 0 0 d3 d3 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 OX0Y0. Ma trận T4 là ma trận biểu diễn tay máy robot trong hệ trục tọa độ gốc: T4= A1.A2.A3.A4 cos n sin  n An=   0   0  sin n cos  n cos n cos  n sin  n 0 sin n sin  n  cos n sin  n cos  n 0 Thay số liệu trong bảng tham số có: cos 1 sin  1 A1    0   0  sin 1 0 a 1.cos 1  cos 1 0 a1 sin 1   0 1 0   0 0 1  Sinh viên: Phạm Văn Khiền 10 a n cos n  a n sin n  dn   1  (1.1) Robot scara serpent cos 2 sin  2 A2    0   0 1 0 A3   0  0 sin 2  cos 2 0 0 a 2 .cos 2  0 a 2 sin 2   1 0  0 1  0 0 1 0 0  0 1 d3   0 0 1 0 0 cos 4 sin  4 A4    0   0  sin 4 0 0 cos 4 0 0   0 1 0  0 0 1  Ký hiệu: S1  Sin1 ; C1  Cos1 S2  Sin2 ; C2  Cos2 S4  Sin4 ; C4  Cos4 S12  Sin(1+2); C12  Cos(1+2)  Các bước tính toán: 3 Bước 1: T4  A 4 C4 S 3 T4  A 4   4 0  0 S4 C4 0 0 0 0 0 0  1 0  0 1 2 3 Bước 2: T4  A 3 . T4 1 0 2 T4   0  0 0  C 4 1 0 0  S4 0 1 d3   0  0 0 1  0 0 0 S4 C4 0 0 Sinh viên: Phạm Văn Khiền 11 0 0  C 4 0 0  S4  1 0  0   0 1  0 S4 C4 0 0 0 0 0  1 d3   0 1 0 Robot scara serpent 1 2 Bước 3: T4  A 2 . T4 0 a 2 .C 2 C 4 S4 0 0  0 a 2 .S2 S4 C4 0 0    1 0  0 0 1 d3    0 1  0 0 0 1 C 2C 4  S2S4 C 2S4  S2C 4 0 a 2 .C 2  S C  C S S S  C C 0 a 2 .S2  2 4 2 4 2 4 2 4 1  T4    0 0 1 d 3    0 0 0 1   C 2 S 1 T4   2 0  0 S2 C 2 0 0 Bước 4: T4  0 T4  A1 .1 T4 C1 -S1 S C 1 0 T4   1 0 0  0 0 0 a1.C1  C 2C 4  S2S4 0 a1.S1  S2C 4  C 2S4 1 0  0  0 1  0 C1 (C 2 C 4  S2S4 )  S1 (S2 C 4  C 2S4 ) S1 (C 2 C 4  S2S4 )  C1 (S2C 4  C 2S4 ) 0  0  S2C 4  C 2S4 0 a 2 .C 2  (C 2C 4  S2S4 ) 0 a 2 .S2   0 1 d 3   0 0 1  C1 (S 2 C 4  C 2 S 4 )  S1 (C 2 C 4  S 2S 4 ) 0 a 2 C12  a1C1 S1 (S 2 C 4  C 2S 4 )  C1 (C 2 C 4  S 2S 4 ) 0 a 2S12  a1S1 0 1 d3 0 0 1   (1.2)   Ma trận 0T4 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: nx n TE  T4  y nz ox oy oz ax ay az px py pz 0 0 0 1  (1.3) Với: n , o, a , p 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. Sinh viên: Phạm Văn Khiền 12 Robot scara serpent Từ ma trận trên ta có hệ phương trình động học thuận tay máy robot: nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (1.4) ny = S1(C2 C4 + S2S4) + C1(S2C4 – C2S4) (1.5) nz = 0 (1.6) ox = C1(S2C4- C2 S4) + S1(C2C4 + S2S4) (1.7) oy = S1(S2C4 - C2 S4) - C1(C2C4 + S2S4) (1.8) oz = 0 (1.9) ax = 0 (1.10) ay = 0 (1.11) az = -1 (1.12) Và hệ phương trình xác định vị trí của điểm tác động cuối như sau: x = px = a1.C1 + a2.C12 (1.13) y = py = a1.S1 + a2.S12 (1.14) z = pz = - d3 (1.15) 1.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ó: p 2x  a12 .C12  a 22 .C122  2.a1.a 2 .C1.C12 p 2y  a12 .S12  a 22 .S122  2.a1.a 2 .S1.S12 p 2X  p 2Y  a12 .(S12  C12 )  a 22 .(S122  C122 )  2.a1.a 2 .(S1.S12  C1.C12 )  p 2x  p 2y  a12  a 22  2.a1.a 2 .C 2 Do đó: (1.16) p 2X  p 2Y  a12  a 22 cosθ 2  2.a1.a 2 (1.17) sinθ 2   (1  cos 2θ 2 ) Từ đó tính được góc 2: 2= atan2(sin2,cos2) (1.18) Sinh viên: Phạm Văn Khiền 13 Robot scara serpent Thế C1, S1 vào phương trình (1.13) và (1.14) thu được: (a1+a2C2).C1 – a2S2.S1 = px a2S2.C1 +( a1+a2C2).S1 = py Giải phương trình bậc nhất với ẩn C1, S1 và sử dụng (1.16) thu được : C1  S1   x (a1  a 2C 2 ).p x  a 2S2 .p y   p 2x  p 2y y   (a1  a 2C 2 ).p y  a 2S2 .p x (1.19) p 2x  p 2y 1= atan2(S1,C1) (1.20) Từ phương trình (1.15) ta có: d3 = - pz (1.21) Mặt khác từ phương trình (1.4) có: nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (1.22) Rút gọn theo các công thức lượng giác thu được: nX = cos1.cos(2-4) – sin1.sin(2-4) = cos(1+2-4) sin(1+2-4) = 1  n 2x (1.23) (1.24) (θ1  θ 2  θ 4 )  atan2( 1  n 2x ,n x ) θ 4  θ1  θ 2  atan2( 1  n 2x ,n x ) (1.25) Vậy hệ phương trình động học ngược của robot Scara Serpent là: Sinh viên: Phạm Văn Khiền 14 Robot scara serpent (a1  a 2C 2 ).p x  a 2S2 .p y  C   1 2 2 p  p x y   (a  a C ).p  a S .p S1  1 2 22 y 2 2 2 x px  py    1  atan2  S1 ,C1  2 2 2 2   cosθ  p X  p Y  a1  a 2 2  2.a1.a 2   sinθ 2   (1  cos 2θ 2 )   2  atan2  S2 ,C2  d   p z  3  θ  θ  θ  atan2( 1  n 2 ,n ) 1 2 x x  4 (1.26) 1.4. Động lực học robot Scara Serpent Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và gia tốc của đối tượng robot, cần phải xây dựng được phương trình động lực học, từ đó phục vụ cho công việc thiết kế và điều khiển robot. Vì vậy cần phải tính toán đầy đủ các thông số của đối tượng trước khi đưa vào mô phỏng. Phương trình động lực học của robot được biểu diễn như sau: & & h(q,q) &  g(q)  (t) H(q)q (1.27) Trong đó: - (t) - Vectơ [n x 1] lực động tạo nên ở n khớp động: (t) = [1(t), 2(t), ..., n(t)]T - q(t) - Vectơ [n x 1] biến khớp: q(t) = [q1(t), q2(t), ... qn(t)]T. - (1.28) (1.29) H(q) - Ma trận [n x n], có các phần tử Hik sau đây: H jk  n  i  max( j,k ) Trace  Uij Ii UikT  . (j, k =1, 2, ...n). Sinh viên: Phạm Văn Khiền 15 (1.30a) Robot scara serpent  0Ti U ij  q j - h( q , ) q (1.30b) - Vectơ [n x 1] lực ly tâm và Coriolit: h( q , ) q n hj   = [h1, h2, ... , hn]T. n  k 1 m 1 h jkm q&k q&m . (j =1, 2, ...n). h jkm   Trace  U ikm I i U ijT  . (1.31) (1.32) (1.33) 0 U ikm -  2 Ti  q k q m (1.34) g(q) - Vectơ [n x 1] lực trọng trường: g(q) = [g1, g2, ... , gn]T. (1.35) g j    m jgUiji r  . (1.36) n i j Với robot Scara Serpent gồm 4 chuyển động (3 chuyển động quay và 1 chuyển động tịnh tiến) và mô phỏng với 3 chuyển động quay có các phương trình cụ thể sau: Vectơ [4 1] lực động: (t) = [1(t), 2(t), 3(t), 4(t)]T. Vectơ [41] biến khớp: q(t) = [q1(t), q2(t), d3(t), q4(t)]T. 1.4.1. Hàm Euler - Lagrange và các vấn đề động lực học Lagrange định nghĩa sự khác biệt giữa động năng và thế năng của hệ thống: L=K–P Trong đó: (1.37) K : là động năng của hệ thống. P : là tổng thế năng của hệ thống. Do đó phương trình động lực học được xác định bằng biểu thức:  i Trong đó: d L L  dt  q&i  q i (1.38) qi = i đối với khớp quay; qi = ri đối với khớp tịnh tiến. Sinh viên: Phạm Văn Khiền 16 Robot scara serpent i : mômen lực động tại khớp thứ i. Động năng của khớp thứ i: Ki  1 1 m i .v i2  J i .i2 2 2 (1.39) Với Ji là mômen quán tính của khớp thứ i. 1.4.2. Động lực học robot Scara Serpent Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 1.4: Bảng 1.4: Thông số của robot Scara Serpent. Khớp 1 Khớp 2 Khớp 3 Khớp 4 Biến 1 2 d3 4 Chiều dài a1 a2 d3 0 Khối lượng m1 m2 m3 m4 Vận tốc v1 v2 v3 v4 Chiều dài tâm khối lg1 lg2 lg3 lg4 Giả sử khối lượng nằm ở đầu mút các thanh nối, ta có chiều dài tâm khối chính là chiều dài của thanh nối: lg1 = a1 Ký hiệu chiều dài các khớp: lg2 = a2 a 1 = l1 lg3 = d3 a 2 = l2 d 3 = l3 Hệ qui chiếu gắn với trục toạ độ (O 0x0y0z0) trên khớp thứ nhất. Khi đó mặt phẳng (O0x0y0) là mặt phẳng đẳng thế. 1.4.2.1. Tính toán động năng và thế năng cho từng khớp Khớp 1:  x1  lg1.cosθ1   y1  lg1.sinθ1   z1  0 Sinh viên: Phạm Văn Khiền 17 Robot scara serpent  x.  l .sinθ .θ& g1 1 1  1  .  y1  lg1.cosθ 1.θ&1 .  z1  0  Trong đó: x1, y1, z1 là hình chiếu của thanh nối số 1 lên các trục x, y, z. v1  x&12  y&12  z&12 v1  (  lg1.sinθ 1 .θ&1 )2  (lg1 .cosθ 1 .θ&1 ) 2 2 &2 v12  lg1 .θ 1 Trong đó: J1 momen quán tính khớp 1. P1  0 1 1 K1  m1.v12  J1.12 2 2 . . 1 1 2 K1  m1.lg1 .θ12  J 1.θ12 2 2 (1.40) Khớp 2: x 2  l1 .cosθ1  l g2 .cos(θ1  θ 2 )  y 2  l1 .sinθ1  l g2 .sin(θ1  θ 2 )  z 2  0 x2  l1 .sinθ1 .θ1  l g2 .sin(θ1 θ 2 ).(θ1  θ2 )   y2  l1 .cosθ1 .θ1  l g2 .cos(θ1 θ 2 ).(θ1  θ2 )  z 2  0 Sinh viên: Phạm Văn Khiền 18 Robot scara serpent v2  22  y 22  z 22 x      l .sin(θ θ ).(θ θ  ) 2  l .cos θ .θ   l .cos(θ θ ).(θ θ  v 22   l1 .sin θ1 .θ 1 g2 1 2 1 2 1 1 1 g2 1 2 1 2  l 2 .(θ θ  ) 2  2.l .l .(θ 2  θ .θ  )cosθ v 2  l 2 .θ 2 1 1 g2 1 2 1 g2 1 1 2 2 1 1 K 2  m 2 .v 22  J 2 .22 2 2 . . 1 2 2 2 & & 2  1 K 2  m 2 . l1 .θ1  lg2 .(θ1  θ 2 )  2.l1.l g2 .(θ12  θ&1.θ&2 ).cosθ 2   J 2.θ&22 2   2 P2  0 (1.41) Khớp 3: x 3  l1.cosθ1  l 2 .cos(θ1  θ 2 )  y 3  l1.sinθ1  l 2 .sin(θ1  θ 2 ) z  -l 3 3  x.  l .sinθ .θ.  l .sin(θ  θ ).(θ.  θ. ) 1 1 1 2 1 2 1 2  3 . . . .   y3  l1.cosθ 1.θ1  l 2 .cos(θ 1 θ 2 ).(θ1  θ 2 )  . .  z 3   l3  v3  . . . x 32  y 32  z 32 2 . . . . . .    v 32    l1 .sinθ1 . θ1  l 2 .sin( θ1 θ 2 ).(θ1  θ 2 )   l1 .cosθ1 . θ1  l 2 .cos(θ1 θ 2 ).(θ1  θ 2 )    . . . . . . . . v 32  l12 . θ12  l 22 .(θ1  θ 2 ) 2  2.l1 .l 2 .(θ12  θ1 . θ 2 )  l 32 . 1 K 3  m3 .v32 2 . . . . . . 1  .  K 3  m3 . l12 .θ12 l 22.(θ1  θ 2 ) 2  2.l1.l 2.(θ 12  θ1 .θ 2 )  l 32  2   P3  m3 .g.h 3  P3   m3 .g.l3 Khớp 4: Sinh viên: Phạm Văn Khiền 19 (1.42) Robot scara serpent . . .  . x   l .sin θ . θ  l .sin( θ  θ ).( θ  θ 1 1 1 2 1 2 1 2)  4 . . .  . y  l .cos θ . θ  l .cos( θ  θ ).( θ  θ  4 1 1 1 2 1 2 1 2) . z 4   l3  x 4  l1 .cosθ1  l 2 .cos(θ1  θ 2 )  y 4  l1 .sinθ1  l 2 .sin(θ1  θ 2 ) z  -l 4 3 . . . v 4  x 42  y 42  z 42 2 2 . . . . . .    v  l1.sinθ 1.θ1  l2 .sin(θ 1 θ 2 ).(θ1  θ 2 )   l1.cosθ 1.θ1  l 2 .cos(θ 1 θ 2 ).(θ 1  θ 2 )   l 32     2 4 . . . . . . . . v 24  l12 .θ 12 l 22.(θ 1  θ 2 ) 2  2.l 1.l 2.(θ 12  θ 1 .θ 2 )  l 32 . 1 1 K 4  m 4 .v 42  J 4 .w 42 2 2 . . . . . . . . . 1  .  1 K 4  m 4 . l12 .θ12 l g22 .(θ1  θ 2 ) 2  2.l1.l g2.(θ 12  θ1 .θ 2 )cosθ 2  l 32   J 4 (θ 1  θ 2  θ 4 ) 2 2   2 P4  m 4 .g.h 4  2.43 P4  m 4 .g.l3 Tổng động năng của hệ thống: K = K1 + K2 + K3 + K4 (1.44) Đặt tâm khối tại đầu mút của thanh nối, ta có: lg1= l1; lg2= l2; lg3= l3 . 1 (m1  m 2  m 3  m 4 ).l12  (m 2  m 3  m 4 ).l 22  J1  J 2  J 4 .θ 12  ... 2 (m 2  m 3  m 4 ).l1 .l2 .cosθ 2 .θ&12  (m2  m3  m 4 )l22  J 2  J 4 .θ&1 .θ&2  ... K 1 (m 2  m 3  m 4 ).l1 .l2 .cosθ 2 .θ&1 .θ&2  (m 2  m3  m 4 ).l32  J 2  J 4  .θ&22  ... 2 1 1  (m3  m 4 ).& l32  .J 4 .θ&42  J 4 .θ&1.θ&4  J 4 .θ&2 .θ&4 2 2 (1.45) Tổng thế năng của hệ thống: P = P1+ P2+ P3+ P4 = -(m3+ m4).g.l3 (1.46) 1.4.2.2. Phương trình động lực học Lagrange của hệ thống: Sinh viên: Phạm Văn Khiền 20 .