Tính toán chuyển động chương trình và thiết kế robot MMR

. Cần xét đến đầy đủ các chi phí phụ và hiệu quả mang lại cho toàn bộ hệ thống. Khi xác định đưa robot vào hệ thống sản xuất thì cũng cần phải xét xem khả năng liệu robot có thay thế được hay không và có hiệu quả hơn không. Vì trong thực tế sản xuất cho thấy xu hướng thay thế hoàn toàn bằng robot nhiều khi không hiệu quả bằng việc giữ lại một số công đoạn mà cần phải có sự khéo léo của con người. Kỹ thuật robot có ưu điểm quan trọng nhất là tạo nên khả năng linh hoạt hóa sản xuất. Mà trong đó kĩ thuật robot và máy vi tính đã đóng vai trò quan trọng trong việc tạo ra các dây chuyền tự động linh hoạt.Vì vậy trong những năm gần đây không những chỉ các nhà khoa học mà cả các nhà sản xuất đã tập trung sự chú ý vào việc hình thành và áp dụng các hệ sản xuất linh hoạt. So với lúc mới ra đời, ngày nay công nghệ robot đã có những bước phát triển vượt bậc. Đặc biệt là vào những năm 60 của thế kỉ trước, với sự góp mặt của máy tính. Ở giai đoạn đầu người ta rất quan tâm đến việc tạo ra những cơ cấu tay máy nhiều bậc tự do, được trang bị cảm biến để thực hiện những công việc phức tạp. Ngày càng có những cải tiến quan trọng trong kết cấu các bộ phận chấp hành, tăng độ tin cậy của các bộ phận điều khiển, tăng mức thuận tiện và dễ dàng khi lập trình. Tăng cường khả năng nhận biết và xử lý tín hiệu từ môi trường làm việc để mở rộng phạm vi ứng dụng cho robot. Trong tương lai số lượng lao động được thay thế ngày càng nhiều vì một mặt giá thành robot ngày càng giảm do mặt hàng vi điện tử liên tục giảm giá đồng thời chất lượng liên tục tăng. Mặt khác chi phí về lương và các khoản phụ cấp cho người lao động ngày càng tăng. Robot ngày càng vạn năng hơn để có thể làm được nhiều việc trên các dây chuyền. Công đoạn lắp ráp thường chiếm tỷ lệ cao so với tổng thời gian sản xuất trên toàn bộ dây chuyền. Công việc lại đòi hỏi phải cẩn thận, nhẹ nhàng tinh tế và chính xác. Nên nếu là công nhân thì cần phải thợ có tay nghề cao và làm việc đơn điệu, căng thẳng. Robot đã có mặt nhiều trên các công đoạn lắp ráp phức tạp do được thừa hưởng kĩ thuật cảm biến, kĩ thuật tin học với những ngôn ngữ lập trình bậc cao. Robot tự hành cũng sẽ phát triển mạnh trong tương lai, có thể đi được bằng chân để thích hợp với mọi địa hình ví dụ như có thể tự leo bậc thang… Việc tạo ra các cơ cấu chấp hành cơ khí vừa bền vững, nhẹ nhàng chính xác và linh hoạt như chân tay người là đối tượng nghiên cứu chủ yếu. Kỹ thuật robot cũng từng bước áp dụng các kết quả nghiên cứu về trí khôn nhân tạo và đưa vào ứng dụng trong công nghiệp. Cải tiến và bổ xung các modul cảm biến và các modul phần mềm phù hợp có thể cải tiến và thông minh hoá nhiều loại robot. Điều quan trọng là các cơ cấu chấp hành của robot phải hoạt động chính xác. 1.1.2 Cấu trúc động học robot Ta có thể khái quát định nghĩa robot theo cách nhìn của cơ học là một chuỗi động, mỗi khâu được ghép với nhau bởi các khớp nối, hoạt động linh hoạt nhờ hệ dẫn động và được điều khiển bằng hệ thống điều khiển. Dưới đây là một số hình robot liên tục được ứng dụng nhiều trong các lĩnh vực: Trong gia công cơ khí: thường sử dụng trong các máy hàn tự động, máy khoan, trong các dây truyền lắp ráp, v…v… Trong dây truyền sản xuất: Tham gia vào một số dây truyền sản xuất như gia công, phun sơn, đóng gói bao bì, v…v… Trong vận tải thường dùng để bốc xếp hàng hóa . Hình 1.3 Robot Puma Hình 1.2 Robot Hipo Hình 1.4 robot Kuka Hình 1.5 Laser Robotic. Trong đồ án này em xin chọn mô hình robot MMR khảo sát là một chuỗi động hở, robot gồm 4 khâu và 4 khớp quay có thể thao tác trong không gian cố định (xe không di chuyển )(hình 1.6). Khâu cuối của robot có thể mang dụng cụ cắt, mỏ hàn,bàn kẹp, v…v… Hình 1.6 1.2 Bậc tự do của robot Cơ cấu tay của robot phải được cấu tạo sao cho khâu cuối phải có vị trí và theo một hướng nhất định nào đó và dễ dàng di chuyển dễ dàng trong vùng làm việc. Muốn vậy cơ cấu tay của robot phải đạt được một số bậc tự do chuyển động. Để tính số bậc tự do của robot thì ta có nhiều cách tính dưới đây ta đưa ra cách tính dựa vào định lý Gruebler. Theo Gruebler thì bậc tự do f được tính theo công thức: (1.1) Trong đó : : Là số bậc tự do của cơ cấu. : Bậc tự do của một vật rắn không chụi liên kết trong không gian làm việc của robot (l = 3 ứng với không gian làm việc trong mặt phẳng, l = 6 ứng với không gian làm việc trong không gian). : Số khâu ( kể cả giá cố định). : Số bậc tự do của khớp thứ i. : Tổng số khớp của cơ cấu. : Số bậc tự do thừa Một số ví dụ: - Số bậc tự do của mô hình robot trong đồ án = 6 (Vì không gian làm việc trong không gian ). = 5 (Số khâu của robot kể cả xe). = 1( Vì tất cả các khớp quay trong robot đều có 1 bậc tự do). = 4 (Tổng số khớp của cơ cấu). =0 (Không có bậc tự do thừa). Bậc tự do của robot là : Laser Robotic ( Hình 1.5) , , , , =0 Bậc tự do của robot là : 1.3 Phương pháp khảo sát bài toán động học Sử dụng phương pháp ma trận chuyền Denavit- Hartenberg 1.3.1 Tọa độ thuần nhất và ma trận biến đổi tọa độ thuần nhất a) Vector điểm và tọa độ thuần nhất Vector điểm dùng để mô tả vị trí của điểm trong không gian 3 chiều. Xét điểm M trong không gian 3 chiều có thể biểu diễn bằng vector r trong hệ tọa độ Oxyz: z x y r M 0 Hình 1.7 Biểu diễn một điểm trong không gian (1.2) Vector trong không gian ba chiều, được bổ sung thêm một thành phần thứ tư và thể hiện bằng một vector mở rộng: (1.3) Đó là cách biều diễn vector điểm trong không gian tọa độ thuần nhất. Như vậy có rất nhiều cách biểu diễn tọa độ trong không gian tọa độ thuần nhất, nó phụ thuộc vào giá trị của hệ số tỉ lệ m. Nếu lấy m = 1 thì các tọa độ biều diễn bằng tọa độ có thực, vector mở rộng được viết lại như sau: (1.4) Nếu lấy thì các tọa độ biều diễn gấp m lần tọa độ thực. b) Quay hệ tọa độ dùng ma trận 3x3 Z x y u w v O M Hình 1.8 Các hệ tọa độ Trước hết ta thiết lập quan hệ giữa 2 hệ tọa độ xyz và uvw chuyển động quay tương đối với nhau khi gốc O của 2 hệ vẫn trùng nhau. Gọi (ix , jy , kz ) và (iu , jv , kw) là các vector đơn vị chỉ phương các trục Oxyz và Ouvw tương ứng. Một điểm M nào đó được biểu diễn trong hệ tọa độ Oxyz bằng vector: rxyz = (rx, ry, rz)T (1.5) còn trong hệ tọa độ Ouvw bằng vector : ruvw = (ru, rv, rw)T (1.6) Như vậy: (1.7) Từ đó ta có: (1.8) Hoặc viết dưới dạng ma trận: (1.9) Gọi R là ma trận quay 3x3 với các phần tử là tích vô hướng 2 vector chỉ phương các trục tương ứng của 2 hệ tọa độ Oxyz và Ouvw. Phương trình (1.9) được viết lại: (1.10) Có thể biểu diễn các phần tử ma trận R và R-1 như sau: (1.11) (1.12) Nhận xét: c) Biến đổi tọa độ dùng ma trận thuần nhất j b a Oi xi xj zj yj yi zi Bây giờ ta thiết lập quan hệ giữa 2 hệ tọa độ: hệ tọa độ Oj xj yj zj sang hệ tọa độ mới Oi xi yi zi. Chúng không những quay tương đối với nhau mà tịnh tiến cả gốc tọa độ . Hình 1.9 Gốc Oj xác định trong hệ tọa độ Oi xi yi zi bằng vector p: p = (a, -b, -c, 1)T (1.13) Giả sử vị trí của điểm M trong hệ tọa độ Oj xj yj zj được xác định bằng vector rj : rj = (xj , yj , zj ,1)T (1.14) và trong hệ tọa độ Oi xi yi zi được xác định bằng vector ri : ri = (xi , yi , zi ,1)T (1.15) Dễ dàng thiết lập được các tọa độ: (1.16) Sắp xếp các hệ số ứng với xj , yj , zj và tj thành một ma trận: (1.17) Phương trình biến đổi tọa độ được viết lại: (1.18) Ma trận Tij biểu thị bằng ma trận 4x4 như (1.17) gọi là ma trận thuần nhất. (1.17) được viết lại : (1.19) Như vậy ta đã dùng ma trận thuần nhất để biến đổi vector mở rộng từ hệ tọa độ thuần nhất này sang hệ tọa độ thuần nhất kia. Sử dụng ma trận thuần nhất trong phép biến đổi tọa độ tỏ ra có nhiều ưu điểm, bởi vì trong ma trận 4x4 bao gồm cả thông tin về sự quay và về cả dịch chuyển tịnh tiến. Ma trận thuần nhất Tij được viết rút gọn: Tij (1.20) Trong đó: Rij: Ma trận quay 3x3. P: Ma trận 3x1 biểu thị tọa độ của điểm gốc hệ tọa độ Oj trong hệ tọa độ Oi xi yi zi . Ma trận thuần nhất T4x4 hoàn toàn xác định vị trí (ma trận P) và hướng (ma trận R) của hệ tọa độ Oj xj yj zj sang hệ tọa độ Oi xi yi zi. d) Các phép biến đổi cơ bản Phép biến đổi tịnh tiến: ta có j =0, do đó: T= (1.21) Tịnh tiến a đơn vị dọc theo trục x, b đơn vị dọc theo trục y, c đơn vị dọc theo trục z, khi đó: T (1.22) Phép quay quanh các trục tọa độ Quay quanh trục x góc q R (1.23) Quay quanh trục y góc a R (1.24) Quay quanh trục z góc j R (1.25) 1.3.2 Ma trận Denavit-Hartenberg Xét mô hình rôbốt gồm có n khâu như hình 1.10. Các khâu được đánh số tăng dần từ khâu cơ sở ( khâu 0 ) cho đến khâu thứ n. Khớp thứ k nối giữa khâu k-1 và khâu k. Hai loại khớp thường được dùng trong thiết kế rôbốt là khớp quay và khớp tịnh tiến. Mỗi khớp chỉ có một bậc tự do. Để mô tả mối quan hệ về mặt động học của hai khâu liên tiếp, người ta thường sử dụng các quy ước do Denavit-Hartenberg (DH) đề xuất năm 1955. Theo DH, tại mỗi khớp ta gắn một hệ trục toạ độ, quy ước về cách đặt hệ toạ độ này như sau: Khâu 1 Khâu 2 Khâu n Khâu cơ sở Khớp 1 Khớp 0 Khớp 2 2 Khớp n Hình 1.10 Robot n khâu - Trục được liên kết với trục của khớp thứ i+1. Chiều của được chọn tuỳ ý. - Trục được xác định là đường vuông góc chung giữa trục khớp i và khớp i+1, hướng từ điểm trục của khớp tới khớp i+1. Nếu hai trục song song, thì có thể chọn bất kỳ là đường vuông góc chung hai trục khớp. Trong trường hợp hai trục này cắt nhau, được xác định theo chiều của ( hoặc quy tắc bàn tay phải). - Trục được xác định theo và theo quy tắc bàn tay phải. Bốn thông số DH liên hệ giữa phép biến đổi của hai hệ trục toạ độ liên tiếp được xác định như sau: : Góc xoay đưa trục về quanh theo quy tắc bàn tay phải. : Dịch chuyển dọc trục đưa gốc toạ độ về nằm trên trục . : Góc xoay đưa trục về quanh theo quy tắc bàn tay phải. : Dịch chuyển dọc trục , đưa gốc toạ độ về nằm trên trục . Ma trận của phép biến đổi, ký hiệu là Hi , là tích của bốn ma trận biến đổi cơ bản và có dạng như sau Khớpi-1 Khớp i Khớp i+1 ai qi di yi-1 xi-1 zi-1 xi yi zi ai Khâu i-1 Khâu i Hình 1.11 Hai khâu liên tiếp (1.26) hay dạng thu gọn: (1.27) Do mỗi khớp chỉ có một bậc tự do nên trong bốn thông số trên chỉ có duy nhất một thông số đóng vai trò là ẩn. Nếu khớp là khớp tịnh tiến thì sẽ là ẩn. Nếu khớp là khớp quay thì sẽ là ẩn. Một cách hình thức có thể biểu diễn ma trận thuần nhất như sau: (1.28) Trong đó (3x3): Ma trận côsin chỉ hướng đưa hệ toạ độ về (3x1): Vị trí gốc toạ độ của hệ toạ độ đặt trong hệ Nếu thực hiện phép biến đổi liên tiếp, quan hệ giữa hệ toạ độ i so với khâu cơ sở (hệ toạ độ 0) được xác định bởi: (1.29) Trong đó: (3x3): Ma trận côsin chỉ hướng đưa hệ của hệ toạ độ về hệ toạ độ 0. (3x1): Vị trí gốc toạ độ của hệ toạ độ so với khâu cơ sở. Phép biến đổi ngược từ hệ toạ độ cơ sở về hệ toạ độ i chính là ma trận nghịch đảo của ma trận thuần nhất. Nếu ký hiệu ma trận nghịch đảo dạng khối: (1.30) ta có (1.31) hay (1.32) Đồng nhất từng phần tử ma trận khối của (1.31) ta được: (1.33) (1.34) Vậy: (1.35) Với việc sử dụng ma trận biến đổi thuần nhất 4x4, việc xác định vị trí và hướng của một khâu bất kỳ của rôbốt là hoàn toàn xác định. 1.4 Chuỗi động học robot Giả sử khảo sát chuỗi động học của robot STANFORD như vẽ (hình 1.12) . Các hệ tọa độ chọn theo quy tắc Denavit-Hartenberg. Bảng thông số động học DH của robot STANFORD như sau: Khâu 1 0 0 -900 2 0 900 3 0 0 0 4 0 0 -900 5 0 0 900 6 0 0 0 Hình 1.12 Robot STANFORD Các ma trận H của robot STANFORD được xác định theo công thức (1.27) Ma trận mô tả vị trí và hướng của Bx1y1z1 đối với Bx0y0z0 : 0H1 0H1= (1.36) Ma trận mô tả vị trí và hướng của Cx2y2z2 đối với Bx1y1z1 : 1H2 1H2= (1.37) Ma trận mô tả vị trí và hướng của Dx3y3z3 đối với Cx2y2z2 : 2H3 2H3 = (1.38) Ma trận mô tả vị trí và hướng của Dx4y4z4 đối với Dx3y3z3 : 3H4 3H4 = (1.39) Ma trận mô tả vị trí và hướng của Dx5y5z5 đối với Dx4y4z4 : 4H5 3H4 = (1.40) Ma trận mô tả vị trí và hướng của Dx6y6z6 đối với Dx5y5z5 : 5H6 5H6= (1.41) Từ các ma trận Denavit-Hartenberg ta tính được vị trí, hướng của khâu thao tác đối với hệ tọa độ cố định O0x0y0z0 là ma trận T6 . T6 =0H1 1H2 2H3 3H4 4H5 5H6 (1.42) Các giá trị 0H1, 1H2, 2H3, 3H4, 4H5, 5H6 được xác định từ công thức (1.36), (1.37),…, (1.41). Ma trận T6 cho ta biết hướng và vị trí của khâu thao tác trong hệ tọa độ cố định hay nói cách khác là vị trí của điểm tác động cuối và hướng của hệ tọa độ động gắn vào khâu tại điểm tác động cuối trong hệ tọa độ cố định. Mặt khác nếu ta gọi là vector mô tả trực tiếp vị trí và hướng của O6x6y6z6 trong hệ tọa độ O0x0y0z0. Trong đó là tọa độ và là các góc quay Cardan của O6x6y6z6 đối với O0x0y0z0. Khi đó ta có: (1.43) : Là ma trận Cardan mô tả hướng của O6x6y6z6 đối với O0x0y0z0 : Vector vị trí của O6x6y6z6 đối với O0x0y0z0 (1.44) Trong đó ký hiệu , , . Ma trận là ma trận mô tả vị trí và hướng của khâu thao tác trong hệ tọa độ cố định thông qua các biến khớp . Còn ma trận cũng mô tả vị trí và hướng của khâu thao tác trong hệ tọa độ cố định nhưng trực tiếp qua các góc quay Cardan và tọa độ khâu thao tác. Từ đây suy ra: (1.45) Từ phương trình (1.35) suy ra hệ 6 phương trình độc lập: (1.46) Viết lại hệ phương trình (1.36) dạng: (1.47) Trong đó: (1.48) (1.49) Nếu các tham số biết trước là các tham số cần xác định là: và ngược lại. CHƯƠNG 2 BÀI TOÁN ĐỘNG HỌC ROBOT MMR 2.1 Hệ phương trình động học cơ bản của MMR Khảo sát chuyển động của robot MMR khi xe dừng lại và cánh tay thực hiện thao tác công việc. Chọn hệ tọa độ theo quy tắc của Denavit- Hartenberg như hình 2.1. hình 2.1 Chọn hệ tọa độ O0x0y0z0 gắn tại khâu 0 và đặt tại khâu 1. Trục z0 trùng với trục quay của khâu 1, x0, y0 được chọn sao cho O0x0y0z0 là hệ quy chiếu thuận. Chọn hệ tọa độ O1x1y1z1 gắn tại khâu 1 và đặt tại khâu 2. Trục z1 trùng với trục quay của khâu 2, x1 được chọn sao cho là được vuông góc chung của z0 và z1, y1 chọn sao cho O1x1y1z1 là hệ quy chiếu thuận. Chọn hệ tọa độ O2x2y2z2 gắn tại khâu 2 và đặt tại khâu 3. Trục z2 trùng với trục quay của khâu 3, x2 được chọn sao cho là được vuông góc chung của z1 và z2, y2 chọn sao cho O2x2y2z2 là hệ quy chiếu thuận. Chọn hệ tọa độ O3x3y3z3 gắn tại khâu 3 và đặt tại khâu 4. Trục z3 trùng với trục quay của khâu 3, x3 được chọn sao cho là được vuông góc chung của z2 và z3, y3 chọn sao cho O3x3y3z3 là hệ quy chiếu thuận. Chọn hệ tọa độ O4x4y4z4 đặt ở vị trí thao tác, trục z4 trùng với trục của khâu 4, x4 là đường vuông góc chung của z3 và z4, y4 chọn sao cho O4x4y4z4 là hệ quy chiếu thuận. Từ hệ tọa độ đã chọn ta có bảng động học Denavit-Hartenberg như sau: Khâu qi di ai ai 1 q1 d1 a1 p/2 2 q2 0 a2 0 3 q3 0 a3 0 4 q4 0 a4 0 Trong đó: d1= 90, a1= 45, a2= 283, a3= 263, a4= 130 (mm) Từ cơ sở lý thuyết đã nêu ở chương 1 ta xác định các ma trận Denavit-Hartenberg như sau: Ma trận mô tả vị trí và hướng của O1x1y1z1 đối với O0x0y0z0 : 0H1 0H1= 0H1= (2.1) Ma trận mô tả vị trí và hướng của O2x2y2z2 đối với O1x1y1z1 : 1H2 1H2= 1H2= (2.2) Ma trận mô tả vị trí và hướng của O3x3y3z3 đối với O2x2y2z2 : 2H3 2H3 = 2H3 = (2.3) Ma trận mô tả vị trí và hướng của O4x4y4z4 đối với O3x3y3z3 : 3H4 3H4 = 3H4 = (2.4) Từ các ma trận 0H1, 1H2, 2H3, 3H4 được xác định theo công thức (2.1), (2.2), (2.3), (2.4) ta tính được ma trận mô tả vị trí và hướng của khâu thao tác trong hệ tọa độ cố định O0x0y0z0 theo công thức: T4 =0H1. 1H2. 2H3. 3H4 (2.5) Giả sử robot cần thực hiện thao tác đối với đối tượng như hình vẽ ( hình 2.2). Ta sử dụng hệ tọa độ gắn vào đối tượng. Khi đó ma trận mô tả vị trí và hướng của trong hệ tọa độ cố định là ma trận: Ma trận mô tả vị trí và hướng của khâu thao tác trên đối tượng đối với hệ tọa độ là ma trận: . Vậy ta có chính là ma trận mô vị trí và hướng của khâu thao tác trên vật đối với hệ tọa độ cố định. yd xd z0 x0 zd Đối tượng thao tác Hình 2.2 Theo cơ sở lý thuyết đã trình bày ở chương 1 ta có : (2.6) Trong đó và Từ đây ta rút ra 6 phương trình gồm 10 tham số: (2.7) (2.8) Vì robot có 4 bậc tự do ta chỉ thực hiện điều khiển chuyển động của robot với 4 tham số ở đây cho quy luật của điểm tác động: và một 1 tham số xác định hướng cua khâu thao tác, có thể cho trước . Từ đó giải 6 phương trình 6 ẩn số. Các phương trình động học của robot MMR như sau: (2.9) 2.2 Bài toán vị trí 2.2.1 Bài toán thuận. Biết trước giá trị của biến khớp (q1,q2,q3,q4) Yêu cầu tìm các toạ độ của khâu cuối ( xp, yp, zp, rotxp, rotyp, rotzp). Vị trí của điểm tác động cuối lên đối tượng cần thao tác được xác định bởi toạ độ điểm P(xp, yp, zp), hướng của nó được xác định bởi các góc quay (rotxp, rotyp,rotzp). Theo hệ phương trình (2.6): Ba phương trình đầu xác định được vị trí của đối tượng: (2.10) Ba phương trình sau cho ta bài toán xác định hướng của điểm tác động cuối lên đối tượng: (2.11) Ba phương trình đã được tính ở phần trên theo (2.9): Giải các phương trình trên ta sẽ tính được hướng của hệ tọa độ khâu thao tác đối với hệ tọa độ cố định. 2.2.2 Bài toán ngược Bài toán ngược là bài toán có ý nghĩa rất quan trọng trong thực tế. Khi biết quy luật chuyển động của khâu thao tác và ta phải tìm các giá trị của biến khớp. Việc xác định các giá trị của biến khớp cho phép ta điều khiển robot theo đúng quỹ đạo đã cho. Từ trên theo (2.7) ta đã có 6 phương trình với 10 tham số: Vì vậy ta phải biết trước 4 tham số hay còn gọi là biến điều khiển. Với mô hình robot MMR này ta cho biết trước . Các phương trình trên đều là các phương trình đại số phi tuyến do đó để giải các phương trình này ta dùng phương pháp lặp Newton-Raphson . 2.3 Bài toán vận tốc 2.3.1 Bài toán thuận Ta có thể viết lại phương trình (2.7) ở dạng sau: f(p,q) = 0 (2.12) Trong đó: p: là vector chứa thông số của điểm tác động cuối: p = [xp yp zp rotpx rotyp rotzp] q : Là véctơ có các thành phần là các tọa độ điều khiển: q = [q1 q2 q3 q4] Đạo hàm hai vế của phương trình (2.12) theo thời gian ta được: (2.13) Có thể viết: Đặt (2.14) (2.15) Thế vào được phương trình: (2.16) Hay (2.17) Trong đó (2.18) 2.2.2 Bài toán ngược Ta có thể viết (2.7) dưới dạng sau: (2.19) Trong đó : Đạo hàm phương trình (2.19) theo thời gian ta được: (2.20) Có thể viết: Đặt: (2.21) (2.22) Thế vào ta nhận được phương trình: (2.23) Hay (2.24) Trong đó (2.25) 2.3Bài toán gia tốc 2.3.1 Bài toán thuận Đạo hàm phương trình (2.13) theo thời gian ta có: (2.26) Hay có thể viết (2.26) ở dạng: (2.27) Với xác định theo công thức (2.14) Ở đây: (2.28) Từ hệ thức (2.27) ta nhận được: 2.3.2 Bài toán ngược Đạo hàm hệ phương trình (2.20) theo thời gian ta được (2.29) Với Có thể viết (2.29) dưới dạng: (2.30) được tính theo công thức (2.21) (2.31) Từ (2.30) ta có : 2.4 Chuyển động chương trình của robot MMR 2.4.1 Robot thao tác trong quá trình đóng gói sản phẩm Tính toán cụ thể với robot thực. Tay robot di chuyển từ vị trí A(xA,yA,zA) trong không gian đến vị trí B(xB,yB,zB) để gắp sản phẩm, sau đó mang sản phẩm từ B đến C(xc, yc,zc) cho vào thùng đóng gói như hình 2.3 Hình 2.3 Giả sử robot đi từ A đến B theo một đường thẳng.Ta có phương trình đường thẳng AB có dạng sau: (2.32) Cho robot chuyển động trong thời gian là 20s sẽ đi từ A(80,139,596) đến B(300,250,300). Tại vị trí A các khớp q1= 600, q2= 1200, q3=-600, q4= 450. Từ (2.32) ta có phương trình AB ( hình 2.4) Hình 2.4 (2.33) Rút gọn lại ta được : (2.34) Sau khi robot đi từ A đến B gắp sản phẩm và tiếp tục đi từ B đến C theo một cung tròn giả sử cùng tròn là nửa đường tròn đường kính BC. Cho tọa độ điểm C(300,50,100). Nhận thấy nằm trong mặt phẳng vuông góc với trục Ox (hình 2.5). Hình 2.5 Ta viết phương trình cung trong mặt phẳng vuông góc với trục Ox có dạng sau : (2.35) Hay: (2.36) Đặt (2.37) Ta cho chuyển động của khâu thao tác đi từ B đến C trong thời gian 20s. Tìm được có dạng như sau: (2.38) Vậy khi robot di chuyển để thực hiện công việc đi từ A đến B sau đó từ B đến C thì quỹ đạo của điểm tác động cuối có dạng như hình 2.6 Hình 2.6 2.4.2 Robot thực hiện một công việc trên bề mặt chi tiết Giả sử robot MMR cần phải hàn một bề mặt theo một quỹ đạo hình elip cho trước ( hình 2.7) Hình 2.7 Hình 2.8 Phương trình quỹ đạo khâu thao tác có dạng : (2.39) CHƯƠNG 3 XÂY DỰNG PHẦN MỀM TÍNH TOÁN VÀ MÔ PHỎNG Để giải quyết bài toán cơ học ta có thể sử dụng nhiều phần mềm tính toán khác nhau. Trong đồ án này sử dụng Maple để tính toán và ghi két quả ra File , sau đó dùng Visual C++ để đọc kết quả và mô phỏng. 3.1 Phần mềm ứng dụng tính toán 3.1.1 Giới thiệu về Maple Maple là một phần mềm được phát triển ở trường đại học Waterloo ở Canada từ năm 1990 và phát triển tiếp tục. Đây là một phần mềm rất thích hợp dùng cho PC. Maple là một môi trường tính toán số và chữ và các ứng dụng đồ hoạ của toán học. Nó không chỉ thuần túy là môi trường tính toán mà còn là một ngôn ngữ lập trình dạng biên dịch. Maple cho phép người sử dụng có thể triển khai các ứng dụng một cách nhanh chóng. Một đặc điểm nổi bật nhất của Maple mà hầu như không có một ngôn ngữ nào hiện nay có được chính là khả năng thay thế việc tính toán biến đổi bằng tay bằng việc tính toán biến đổi bằng máy. Khả năng này cực kỳ linh hoạt và phong phú. Điều này làm cho Maple trở nên rất hấp dẫn người sử dụng đặc biệt là những người làm về kỹ thuật. Maple có thể giải quyết rất nhiều vấn đề của toán học như đại số ma trận, vector, giải hệ phương trình phi tuyến, hệ phương trình tuyến tính, tích phân, giải phương trình vi phân thường, phương trình đạo hàm riêng, giải các bài toán về trị riêng, vector riêng, các bài toán đa thức…Nói chung tất cả các lĩnh vực của toán học cổ điển cũng như hiện đại đều có thể tìm thấy trên Maple. Điều này giúp chúng ta có thể giảm thiểu thời gian tính toán, dành nhiều thời gian cho việc hoàn thiện mô hình và đánh giá kết quả. Cửa sổ làm việc chính của Maple: Giao diện của Maple cũng giống như giao diện của các chương trình ứng dụng khác trên Windows. Tuy nhiên Maple là chương trình thiên về tính toán, nên nó cũng có một số chức năng đặc thù riêng. Maple là một hệ thống mở, nó cho phép ta tạo ra những ứng dụng riêng mới dựa trên những cái có sẵn rất. Nó cung cấp rất nhiều các thư viện chuẩn. Cú pháp gọi thư viện: [> with(library_name); library_name : Là tên thư viện cần gọi. Trong Maple có rất nhiều thư viện như: Linalg: Thư viện chương trình đại số tuyến tính. Plots: Thư viện đồ họa vẽ đồ thị hai chiều và ba chiều. Plottools: Thư viện đồ họa cung cấp các đối tượng hai chiều và ba chiều như hình trụ, hình cầu, hình nón… a) Một số lệnh cơ bản Các lệnh cơ bản như cộng, trừ, nhân, chia ma trận, vector có kí hiệu như toán học thông thường dễ sử dụng. [> restart: Lệnh này thường dùng khởi đầu một chương trình Maple. Sau lệnh này các biến dùng trước nó không còn. [>multiply(A,B): Lệnh nhân hai ma trận. Trong thư viện linalg [>restart; with(linalg): > A := array( [[1,2],[3,4]] ); B := array( [[0,1],[1,0]] ); C := array( [[1,2],[4,5]] ); multiply(A, B, C); [>inverse(A): Lệnh tính ma trận nghịch đảo của A. > A:=array(1..2,1..2,[[1,2],[3,4]]); > inverse(A); [>diff(f(x),x): Đạo hàm của hàm f(x) theo x. > diff(x*sin(cos(x)),x); Subs(var(1)=rep(1),…,var(n)=rep(n),expr): Lệnh thay thế giá trị vào một biểu thức. > subs( x=2, x^2+x+1 ); [>fsolve(eqns, vars, option): Giải hệ phương trình phi tuyến. eqns: Tập các phương trình của hệ var: Tập các biến của hệ. option: Các lựa chọn cho việc giải > f := sin(x+y) - exp(x)*y = 0; g := x^2 - y = 2; fsolve({f,g},{x,y},{x=-1..1,y=-2..0}); [plot(): Dùng để vẽ đồ thị [> plot([sin(x), x-x^3/6], x=0..2, color=[red,blue], style=[point,line]); b) Các kiểu dữ liệu cơ bản Để nắm được Maple phải nắm vững các kiểu dữ liệu của nó. Đây cũng là một ưu điểm nổi bật của Maple so với ngôn ngữ khác. Các kiểu dữ liệu này giúp cho việc lập trình như tính toán trở lên cực kỳ linh hoạt. Một số kiểu hay dùng: Kiểu tuần tự (sequences) đây là một kiểu đơn giản nhất của Maple, đó là một nhóm các biểu thức được viết cách nhau bởi dấu phẩy: > seq( i^2, i=1..5 ); Kiểu liệt kê ( Lists) khác với kiểu tuần tự, các thành phần của danh sách bị bao bởi cặp dấu ngoặc vuông “[” và “]” các phần tử cách nhau bởi dấu phẩy: > L := [1,[2,3],[4,[5,6],7],8,9]; Kiểu tập hợp( sets) khác với kiểu tuần tự, các phần tử của tập hợp bị bao bởi cặp các dậu móc nhọn “{“ và ”}” , cũng cách nhau bởi dấu phẩy. Ý nghĩa của kiểu tập hợp rất giống trong toán học: > S := {v,w,x,y,z}; Kiểu mảng (arrays) đây là kiểu dữ liệu thông dụng như trong các ngôn ngữ lập trình khác. Cú pháp : name:=array(…) > A:=Array([[1,2,3],[4,5,6]]); Kiểu bảng (table) giống kiểu record trong pascal hoặc kiểu Struct trong C. > S := table([(2)=45,(4)=61]); Kiểu xâu ký tự (String) để khai báo một xâu ký tự ta sử dụng hai dấy phẩy kép: > myname:="Do Viet Hung"; c) Lập trình trong Maple Các loại toán tử thường sử dụng khi lập trình: Toán tử Ký hiệu Số học +, -, *, /, ^, ** Quan hệ , =, , = Logic and, or, not, xor Các kiểu dữ liệu đã trình bày ở trên. Lệnh rẽ nhánh if if then else end if; Hoặc sử dụng nhiều lệnh rẽ nhánh if then elif then else end if; Ví dụ > a := 3; b := 5; > if (a > b) then a else b end if; Lệnh for for from by to while do end do; Ví dụ: [>for i from 6 by 2 to 100 do print(i) end do; Lệnh while while do end do; Ví dụ: [>tot := 0; for i from 11 by 2 while i < 100 do tot := tot + i end do; Xây dựng hàm thủ tục: proc Hàm và thủ tục thường hay được sử dụng để cho chương trình sáng sủa và ngắn gọn. Ý nghĩa của nó hoàn toàn giống hàm thủ tục trong bất kỳ ngôn ngữ lập trình nào. Name:=proc(agseq)::type local var1::type1, var2::type2, …; global nseq; options nseq; desription stringseq; statseq return … End proc; Thao tác với File: fopen, fclose, fprintf, fscanf, readline, readdata. Đặc điểm của File là có thể lưu bất kỳ kiểu dữ liệu với kích thước không hạn chế. Có hai loại File: dạng BINARY và dạng TEXT. Việc sử dụng các loại File này tùy thuộc vào cách thức tổ chức dữ liệu cũng như yêu cầu của từng bài toán. fopen Mở File fclose Đóng File fprintf Ghi dữ liệu từ File fscanf Đọc dữ liệu từ File readline Đọc một dòng dữ liệu từ File readdata Đọc một dòng dữ liệu từ File có cấu trúc đã định sẵn 3.1.2 Giải bài toán thuận và bài toán ngược Phần trên ta đã thiết lập cách xây dựng các phương trình của robot. Trong phần này ta trình bày cách giải bằng phần mềm Maple. Mục dữ liệu dùng lệnh đọc read để đọc File data và File Pro_matrix. File data.txt Unknowns := [q[1], q[2], q[3], q[4],rotxp,rotzp]; là các biến cần phải đi xác định trong bài toán ngược của vị trí. InitrP := [xp, yp, zp,rotxp, rotyp,rotzp]; Vector mô tả vị trí và hướng của điểm tác động cuối trong hệ tọa độ cố định O0x0y0z0. Init_1:=[1.047197551,2.094395103,-1.047197551, -.7853981635,0.538739,0.13318]: Điều kiện đầu của bài toán, hay chình la giá trị của các biến khớp của robot trước khi làm việc. #------------------ Quy luật chuyển động của khâu thao tác---------- TRAEK_P0:=[11*t+80,5.5*t+139,-14.8*t+596]; Quỹ đạo ta xây dựng trong chương 2 ## THONG SO DONG HOC DENAVIT-HATENBERG ## robot:=table([Init= [0,0,0,0,0,0], DH =[[theta[1],d[1],a[1],alpha[1]], [theta[2],d[2],a[2],alpha[2]], [theta[3],d[3],a[3],alpha[3]], [theta[4],d[4],a[4],alpha[4]]], flatform=[xp,yp,zp,rotx,roty,rotz]]); bảng động học Denavit-Hartenberg của robot. #----------Khâu1------------- d[1]:=90; a[1]:= 45; alpha[1]:= Pi/2; #-----------khâu 2----------- d[2]:=0; a[2]:=283; alpha[2]:=0; #----------khâu 3------------- d[3]:=0; a[3]:=263; alpha[3]:=0; #----------khâu 4------------- d[4]:=0; a[4]:=130; alpha[4]:=0; #--------------Các biến khớp------------------------------ theta[1]:=q[1]; theta[2]:=q[2]; theta[3]:=q[3]; theta[4]:=q[4]; #------------------------------------------------------------- dt:=0.2; Thời gian mỗi bước của robot khi di chuyển. tg:=0.0; T_:=20.0; Thời gian robot thực hiện thao tác. MaxLoop:=100; Số vòng lặp tối đa trong thuật giải Newton-Raphson. AbsErr:=0.0001; Điều kiện để hội tụ của thuật giải Newton-Raphson. ` Pro_matrix.txt Gồm các thủ tục tính ma trận Denavit-Hartenberg, Cardan, ghi File kết quả. # thủ tục tính ma trận DH Dmat:=proc(theta,d,a,alpha) local A; A:=matrix(4,4); A[1,1]:=cos(theta); A[1,2]:=-sin(theta)*cos(alpha); A[1,3]:=sin(theta)*sin(alpha); A[1,4]:=a*cos(theta); #------------------------------------------ A[2,1]:=sin(theta); A[2,2]:=cos(theta)*cos(alpha); A[2,3]:=-cos(theta)*sin(alpha); A[2,4]:=a*sin(theta); #------------------------------------------ A[3,1]:=0; A[3,2]:=sin(alpha); A[3,3]:=cos(alpha); A[3,4]:=d; #------------------------------------------ A[4,1]:=0; A[4,2]:=0; A[4,3]:=0; A[4,4]:=1; return (A); end; # thủ tục tính ma trận Cardan Cardan:=proc(x,y,z,alpha,Psi,theta) local A; A:=matrix(4,4); A[1,1]:=cos(Psi)*cos(theta); A[1,2]:=-cos(Psi)*sin(theta); A[1,3]:=sin(Psi); A[1,4]:=x; #------------------------------------------- A[2,1]:=sin(alpha)*sin(Psi)*cos(theta)+cos(alpha)*sin(theta); A[2,2]:=-sin(alpha)*sin(Psi)*sin(theta)+cos(alpha)*cos(theta); A[2,3]:=-sin(alpha)*cos(Psi); A[2,4]:=y; #-------------------------------------------- A[3,1]:= -cos(alpha)*sin(Psi)*cos(theta) +sin(alpha)*sin(theta); A[3,2]:=cos(alpha)*sin(Psi)*sin(theta)+sin(alpha)*cos(theta); A[3,3]:=cos(alpha)*cos(Psi); A[3,4]:=z; #---------------------------------------------- A[4,1]:=0; A[4,2]:=0; A[4,3]:=0; A[4,4]:=1; return (A); end; # thủ tục vẽ đồ thị của điểm tác động cuối và các biến khớp. Myplot:=proc(input,time) local n,m,k,i,j,temp,out; m:=rowdim(input); n:=coldim(input); k:=vectdim(time); if km then print(`ERROR: khong hop le\n`); return; end if; temp:=vector(n); out:=vector(n); for j from 1 by 1 to n do temp[j]:=[seq([time[i],input[i,j]],i=1..m)]; out[j]:=listplot(temp[j],color=red); end do; display([seq(out[i],i=1..n)]); end: #-------------------------------------------- plot_f:=proc(input,t) local A,i,n,dt,tg: n:=101: dt:=0.2: A:=matrix(n,3,0): tg:=0: for i from 1 by 1 to n do for k from 1 by 1 to 3 do A[i,k]:=subs(t=tg,input[k]); od: tg:=tg+dt: od: return(A): end: # Thủ tục ghi File kết quả WriteFile_in:=proc(A,File_in) local N,M,i,j,fd; N:=rowdim(A); M:=coldim(A); fd := fopen(File_in, WRITE,TEXT); for i from 1 by 1 to N do for j from 1 by 1 to M do fprintf(fd, "%.9g ",A[i,j]); end do; fprintf(fd,"\n"); end do; fclose(fd); end; Mục bài toán vị trí thuận : Đọc File main_1.txt. #-------ma tran thuan nhat mo ta vi tri va huong cac khop- for i from 1 by 1 to 4 do A[i]:=Dmat(theta[i],d[i],a[i],alpha[i]): print(A[i]); od; #--- Tinh ma tran mo ta vi tri huong cua khau cuoi ------ T:=multiply(A[1],A[2],A[3],A[4]): print(T); #----------------Ma tran mo ta khau cuoi------- A0:=Cardan(xp, yp, zp,rotxp, rotyp,rotzp): print(A0); #------------voi cac bien q[i]----------------- f[1]:=T[1,4]-A0[1,4]; f[2]:=T[2,4]-A0[2,4]; f[3]:=T[3,4]-A0[3,4]; f[4]:=T[1,2]-A0[1,2]; f[5]:=T[2,3]-A0[2,3]; f[6]:=T[3,1]-A0[3,1]; Sau phần tính toán ở mục này thì ta nhận được ma trận T4 mô tả vị trí và hướng của điểm tác động cuối. Và rút ra 6 phương trình động học như đã trình bày ở chương trước. Mục bài toán vị trí ngược đọc file main_2.txt. rotyp:=0: init_:=copy(Init_1): NZ:=round(T_/dt): TRAEK_P:=copy(TRAEK_P0): KQ:=matrix(NZ+1,9,0): ttime:=vector(NZ+1,0): tg:=0.0: dt:=0.2: for kz from 1 by 1 to NZ+1 do f_:=[seq(f[k],k=1..6)]: Jaco_:=jacobian(f_,Unknowns): for k from 1 by 1 to 3 do TRAEK_P_[k]:=evalf(subs(t=tg,TRAEK_P[k])): end do: for j from 1 by 1 to 3 do f_:=subsVectoC([InitrP[j]=TRAEK_P_[j]],f_): Jaco_:=subsMatC([InitrP[j]=TRAEK_P_[j]],Jaco_):# end do: y:=Newton_Raphson(f_,Jaco_,Unknowns,init_,AbsErr,MaxLoop): for j from 1 by 1 to 6 do init_[j]:=y[j]: KQ[kz,j]:=y[j]: end do: for i from 1 by 1 to 3 do KQ[kz,i+6]:=TRAEK_P_[i]; end do: ttime[kz]:=tg: tg:=tg+dt: end do: WriteFile_in(KQ,"kq.txt"); Sau khi đọc File main_2.txt sẽ cho ta biết được giá trị của các biến khớp và hướng của điểm tác động cuối để robot có thể đi theo quỹ đạo cho trước. Trong chương trình này đã sử dụng thuật giải Newton-Raphson để giải bài toán vị trí ngược. Thủ tục của thuật giải này được đóng gói trong thư viện tiện ích “MRMLib0206” và để sử dụng thư viện này ta có thủ tục gọi như sau: > libname:="MRMLib0206",libname: > with(BK0206); 3.1.3 Các ví dụ a) Bài toán thuận Giả sử ta cho giá trị của các biến khớp . Áp dụng Maple tính được giá trị của vị trí và hướng của điểm tác động cuối. Khi đó theo công thức (2.10) ta tính được (3.1) Từ công thức (2.11) ta có hệ phương trình xác định hướng của vị trí điểm tác động cuối : (3.2) Giải hệ phương trình trên bằng lệnh fslove . Cấu trúc ngữ pháp của câu lệnh này là: fsolve(tập hợp hệ các phương trình, tập hợp các biến, các tùy chọn điều khiển). Ta nhận được các giá trị : (3.3) b)Bài toán ngược Khi cho khâu tác động đi theo quỹ đạo là đường thẳng AB(công thức 2.34) ta nhận được các giá trị của được biểu thị trên đồ thị. Đồ thị tọa độ suy rộng Đồ thị tọa độ suy rộng Đồ thị tọa độ suy rộng Đồ thị tọa độ suy rộng Khi cho khâu tác động đi theo quỹ đạo là cung tròn (công thức 2.34) ta nhận được các giá trị của được biểu thị trên đồ thị Đồ thị tọa độ suy rộng Đồ thị tọa độ suy rộng Đồ thị tọa độ suy rộng Đồ thị tọa độ suy rộng 3.1.3 Giới thiệu về thư viện OpenGL Thư viện đồ họa OpenGL là một thiết bị và là một hệ thống các thư viện độc lập sử dụng cho không gian ba chiều. Thư viện OpenGL được phát triển bởi tập đoàn Silicon Graphic Inc (SGI). Hiện nay OpenGL đã trở thành một công cụ được sử dụng rộng rãi trong các hệ điều hành như Windows 9x, Windows NT .. a)Tổng quan về OpenGL Mục đích của thư viện OpenGL là trả về đối tượng không gian hai chiều và ba chiều vào một một bộ đêm khung ( frames buffer) như là điểm nhở của phần cứng đồ họa. Thư viện OpenGL về cơ bản là một thủ tục, do đó không cần miêu tả giống đối tượng mà phải chỉ định rõ cách mà đối tượng được vẽ. Đối tượng phức tạp này sẽ được mô tả trong một phần tử đơn giản mà ứng dụng của người dùng định nghĩa. Thư viện OpenGL cũng được thực hiện theo mô hình Client- Server. b)Khái niệm cơ bản về OpenGL Ở mức độ cơ bản thư viện OpenGL giai quyết theo từng đỉnh. Một đỉnh là một điểm, ví dụ như điểm cuối là một đường thẳng, góc của một hình đa giác. Đỉnh có Thể là hai hoặc ba chiều. Ở mức độ tiếp theo sẽ bao gồm một nhóm một hoặc nhiều đỉnh. c)Khởi tạo OpenGL Trước khi thư viện OpenGL được sử dụng, một số bước khởi tạo cơ bản sẽ được thực hiện. Hầu hết các ứng dụng Windows sử dụng OpenGL phải được kết hợp với một số ngữ cảnh thiết bị. Ngữ cảnh thiết bị phải là một ngữ cảnh hiển thị hoặc là một ngữ cảnh thiết bị nhớ tương ứng với ngữ cảnh thiết bị hiển thị. Để cài một ngữ cảnh trả về, đầu tiên phải sử dụng hàm SetPixelFormat() để thiết lập dạng của điểm cho thiết bị, tiếp theo gọi hàm wglCreateContext nếu thành công hàm này sẽ trả về biến kiểu HGLRC. d)Vẽ với OpenGL Hầu hết các chương trình vẽ dùng OpenGL sẽ bao gồm một loạt các đỉnh và được đặt trong một cặp glBegin và glEnd . 3.1.4 Mô phỏng chuyển động của robot MMR Khả năng mạnh mẽ của thư viện đồ họa OpenGL đã được giới thiệu ở trên. Chương trình sử dụng thư việc này trong việc mô phỏng robot dựa trên các giá trị tính toán của bài toán ngược. Nếu chỉ dựa trên các hàm vẽ cơ bản thì ta khó khăn trong công việc dựng hình 3D cho robot vì việc xác định đỉnh của các khâu rất khó khăn. Nhưng khi ta sử dụng một số File định dạng của một số phần mềm đồ họa 3D như: AutoCAD, Solid Works, 3Dmax. Các file này dễ dàng đọc được bằng phần mềm. Các định dạng file có có thể liệt kê ra như: + Stereo LithorGaphy(.STL) +WRML File +ASC File +SAT File Các file này đều có chung đặc điểm là lưu trữ dữ liệu của vật thể 3D dưới dạng các mặt nhỏ (FACE) và các Vector pháp tuyến cùng với một số thông tin khác về vật thể và môi trường như ánh sáng, vật liệu,… Trong đồ án này sử dụng định dạng *.STL, file này có thể được xuất ra dưới dạng nhị phân hoặc text. Khi thiết thế trên phần mềm Solid works xong ta Save đối tượng dưới dạng *. STL như hình vẽ (hình 3.1). Hình 3.1 Chú ý khi Save ta chọn Options có cửa sổ hiện ra, trong output as chọn ASCII . Hình 3.2 Sau khi save dưới định dạng .STL ta tiến hành việc mô phỏng chuyển động của robot. Để xuất các khâu của robot vào trong môi trường làm việc của OpenGL ra sử dụng hàm OnlnitialUpdate() void CAaView::OnInitialUpdate() { CView::OnInitialUpdate(); // TODO: Add your specialized code here and/or call the base class a.addObjects(CObjects("draw/xe.stl")); a.addObjects(CObjects("draw/khau1.stl")); a.addObjects(CObjects("draw/khau2.stl")); a.addObjects(CObjects("draw/khau3.stl")); a.addObjects(CObjects("draw/khau4.stl")); CVector scale(_scale, _scale, _scale); a.setScale(scale); } Sau khi insert các khâu vào trong môi trường làm việc xong ta tiến hành đưa các khâu và khớp vào đúng vị trí theo quy tắc Denavit- Hartenberg và mô phỏng bằng hàm Draw() void CAaView::Draw() { glClearColor(0.0f,0.0f,0.0f,0.0f); glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); //glShadeModel(GL_SMOOTH); CMainFrame* pMain = (CMainFrame*)AfxGetApp()->GetMainWnd(); CCcommand* pForm = (CCcommand*)pMain->m_wndSplitter.GetPane(0,0); max = pForm->max; glColor3f(1.0f,0.0f,0.0f); glLineWidth(1.0); if(pForm->run == TRUE) { glPushMatrix();//ngu canh //1 // Position / translation / scale glTranslated(0,-20,30); glRotatef(60, 0.0, 0.0, 1.0); glRotatef(3, 1.0, 0.0, 0.0); glRotatef(-6, 0.0, 1.0, 0.0); //chuot glTranslated(m_xTranslation,m_yTranslation,m_zTranslation); glRotatef(m_xRotation, 1.0, 0.0, 0.0); glRotatef(m_yRotation, 0.0, 1.0, 0.0); glRotatef(m_zRotation, 0.0, 0.0, 1.0); glScalef(m_xScaling,m_yScaling,m_zScaling); //glRotated(30,0.0,1.0,0.0); //glRotated(Index,1.0,0.0,0.0); glColor3f(0.0f,1.0f,0.0f); glLineWidth(1.0); glBegin(GL_LINES); glVertex3d(0, 0, 0); glVertex3d(0, 0, 12); glEnd(); glBegin(GL_LINES); glVertex3d(0, 0, 0); glVertex3d(0, 40, 0); glEnd(); glBegin(GL_LINES); glVertex3d(0, 0, 0); glVertex3d(50, 0, 0); glEnd(); glColor3f(0.3,0.5,0.8); glLineWidth(1.0); glPushMatrix(); // push xe //2 glColor3f(0.2,0.2,0.2); glColor3f(0.2f,0.7f,0.6f); a[0].drawObjects(); //********** khau1 ********************/ glPushMatrix(); glColor3f(0.8f,0.7f,0.1f); glRotated(90,1,0,0); glTranslated(45*_scale,90*_scale,0); a[1].drawObjects(); glRotated(pForm->q1[Index],0,0,1); //******************* khau2 *********/ glPushMatrix(); glColor3f(0.9f,0.1f,0.4f); glRotated(pForm->q2[Index],0,0,1); a[2].drawObjects(); //**************** khau3******************/ glRotated(45,0.0,0.0,1.0); glPushMatrix(); glColor3f(0.2f,0.1f,0.6f); glRotated(-(pForm->q3[Index]),0.0,1.0,0.0); a[3].drawObjects(); //**************** khau4******************/ glRotated(25,0.0,0.0,1.0); glPushMatrix(); glColor3f(0.8f,0.1f,0.6f); glRotated(pForm->q4[Index],0.0,0.0,1.0); a[4].drawObjects(); glPopMatrix(); glPopMatrix(); glPopMatrix(); glPopMatrix(); glPopMatrix(); glPopMatrix(); glPopMatrix(); } Hình 3.3: Giao diện chương trình mô phỏng. PHẦN II THIẾT KẾ VÀ CHẾ TẠO MẪU ROBOT MMR CHƯƠNG 1 GIỚI THIỆU CHUNG 1.1 Đặt vấn đề Trong điều kiện phát triển như ngày nay thì việc thay thế con người làm việc trong môi trường độc hại hay nhỏ hẹp là rất quan trọng. Các công việc như vào trong lò hạt nhân, thông cống ngầm, làm sạch các khoang tàu,…thì việc thay thế con người bằng robot là một giải pháp rất hữu hiệu và khả thi. Trong đề tài này việc nghiên cứu và chế tạo mẫu robot có kích thước nhỏ, di chuyển dễ dàng và thực hiện các thao tác linh hoạt( MRM-Mini Mobile Robot), các thao tác này có thể là: hàn, phun sơn, tháo gắp các bộ phận cần sửa chữa, loại bỏ các chi tiết thừa v.v.v…Rôbốt thông qua kết nối với máy tính bằng dây cáp sẽ được điều khiển từ xa, ngoài ra trong thực tế rôbốt phải được lắp các hệ thống camera hay sensor dẫn đường. Việc lựa chọn kết cấu sao cho robot nhỏ, di chuyển dễ dàng và thao tác linh hoạt là điều rất quan trọng vì thế trước khi chế tạo cần phải thiết kế mô hình của robot bằng phần mềm đồ họa. Phần mềm đồ họa sử dụng để thiết kế robot MMR là Solid Works và Auto CAD. Hình ảnh robot MMR đã được chế tạo 1.2 Giới thiệu về phần mềm Solid Works SolidWorks là một phần mềm thiết kế ba chiều được sử dụng rất rộng rãi trong nhiều lĩnh vực khác nhau. Ưu điểm của phần mềm này là rất dễ sử dụng, thân thiện với người dùng, và dễ chỉnh sửa lại bản thiết kế khi cần thay đổi. Khởi động Solid Works và ấn tổ hợp phím Ctrl+N hay vào thanh công cụ FILE/New hình(1.2) Hình 1.1 Có ba loại bản vẽ,tuỳ theo mục đích sử dụng mà ta mở các bản vẽ khác nhau: Part (bản vẽ chi tiết): Bản vẽ được sử dụng để tạo các chi tiết riêng lẻ, và trong một bản vẽ chi tiết ta không thể tạo được hai chi tiết.Trong thiết kế cơ khí mỗi một loại máy móc ,một cơ cấu hay một robot… thường có cấu tạo từ nhiều chi tiết khác nhau ghép lại. Mỗi bản vẽ thể hiện từng chi tiết này, và sau đó chúng được lắp ghép trên một bản vẽ khác. Do đó rất thuận tiện khi kiểm tra và thay đổi chi tiết. Các file này có phần mở rộng *.sldprt. Assembly(bản vẽ lắp):Bản vẽ này liên kết các chi tiết trong bản vẽ lắp lại với nhau, tạo thành một cụm chi tiết hoặc một sản phẩm hoàn chỉnh. Khi một bản vẽ chi tiết được thay đổi thì bản vẽ lắp tương ứng cũng thay đổi theo. Các file này có phần mở rộng *.sldasm Drawing (bản vẽ kĩ thuật ): khi đã có bản vẽ chi tiết hay bản vẽ lắp ta chọn Drawing để biểu diễn các hình chiếu các mặt cắt từ bản vẽ chi tiết hay bản vẽ lắp đã có ở trên các file này có phần mở rộng là *.slddrw. a) Bản vẽ chi tiết (Part) Để thiết kế các chi tiết 3D trước hết phải có các bản vẽ phác thảo, thông thường Solid Works mặc định mặt Front làm bản vẽ phác thỏa, tùy vào kết cấu của chi tiết thiết kế mà ta tạo ra các mặt phác thảo khác nhau. Để bắt đầu vẽ phác thảo phải khởi động thanh menu Sketch trên thanh công cụ. Khi đó giao diện màn hình như sau: Hình 1.2 Khi đó ta có thể sử dụng các công cụ vẽ trên mặt phẳng của Solid Works: Công cụ Line : Tạo đường thẳng. Công cụ Rectangle : Tạo hình chữ nhật . Công cụ Centerpoint Arc :Vẽ cung tròn có tâm xác định. Công cụ Tangent Arc:Vẽ cung tròn tiếp tuyến. Công cụ 3 Pt Arc : Vẽ cung tròn bằng ba điểm. Công cụ Circle : Vẽ đường tròn. Công cụ Ellipse : Vẽ Ellipse. Công cụ Parabola : Vẽ Parabol. Công cụ Spline : Vẽ đường cong tự do. Công cụ Centerline : Vẽ đường tâm. Ta có thể chỉnh sửa hình phác thảo bằng các công cụ tương tự như trong Auto CAD: Công cụ Mirror : Lấy đối xứng các đối tượng qua đường Centerline. Công cụ Fillet : Tạo góc lượn. Công cụ Chamfer : Vát góc. Công cụ Trim : Được dùng để xén một đoạn của đường thẳng, hoặc đường tròn. Công cụ Offset : Tạo một đối tương mới có các biên dạng song song và cách đều các biên dạng tương ứng cả đối tượng cũ một khoảng cách cho trước. Công cụ Extend : Được dùng để kéo dài đối tượng cho tới khi gặp đối tượng khác. Công cụ Linear Step and Repeat : Công cụ này được dùng để sao chép đối tượng từ đối tượng gốc thành nhiều đối tượng khác và các đối tượng đó được xắp xếp theo hàng hoặc cột. Công cụ Circular Step and Repeat : Giống như lệnh trên nhưng quỹ đạo sao chép các đối tượng là đường tròn. Các công cụ tạo mối quan hệ giữa các đối tượng : Công cụ Dimension : Tạo kích thước cho đối tượng . Công cụ Add Relation : Tạo quan hệ hình học cho các đối tượng như song song, vuông góc, trùng nhau, tiếp xúc… Sau khi tạo hình dáng xong rồi ta sử dụng các công cụ tạo khối 3D để vẽ: Công cụ Extrude Base/Boss : Nó có chức năng kéo dài đối tượng vẽ phác thành vật thể khối. Công cụ Extrude Cut : Có chức năng khoét vật thể khối theo biên dạng đã vẽ phác. Công cụ Revolve Base/Boss : Có chức năng tạo một khối Base hoặc Boss tròn xoay quanh đường Centerline. Công cụ Sweep : Tạo các khối cơ sở, khối dựng đứng ,khoét bằng phương pháp di chuyển biên dạng trên mặt phẳng vẽ phác dọc theo một đường dẫn. Công cụ Linear Pattens : Có chức năng sao chép một đặc điểm của mô hình thành nhiểu đăc điểm và được xắp xếp theo hàng hoặc cột. Công cụ Circular Pattern : Có chức năng sao chép một đặc điểm của mô hình thành nhiều đặc điểm và được xắp xếp theo một đường tròn . Các công cụ hiệu chỉnh: Công cụ Fillet : Chức năng bo tròn các cạnh hoặc các đỉnh của đối tượng. Công cụ Chamfer : Vát mép cạnh hoặc đỉnh của đối tượng. b) Bản vẽ lắp(Assembly) Sau khi đã thiết kế xong tất cả các chi tiết thì cần phải ghép chúng lại với nhau thành một chi tiết hay thành một máy công cụ. Các chi tiết trong bản vẽ lắp cần phải đúng vị trí và có mối quan hệ ràng buộc với nhau. Công cụ Mate : Cho phép ta tạo các ràng buộc hạn chế một bậc tự do tương đối giữa các chi tiết với nhau tức ghép các chi tiết theo một ràng buộc cụ thể theo cơ cấu và máy cụ thể. Ta có thể chọn các mối ghép như sau: Coincident: Cho phép ghép hai mặt phẳng tiếp xúc nhau. Concentic: Cho phép ghép hai mặt trụ, cầu đồng tâm. Parallel: Cho phép ghép hai mặt phẳng song song và cách nhau một khoảng d. Perpendicular: Cho phép ghép hai mặt phẳng vuông góc với nhau. Tangent: Cho phép ghép hai mặt cong, mặt trụ với trụ, mặt cầu với mặt phẳng, mặt trụ và mặt côn với mặt phẳng tiếp xúc với nhau. Sau khi thiết kế bước đầu của robot MMR ta lắp ghép các khâu vào như sau: Hình 1.3 CHƯƠNG 2 THIẾT KẾ CƠ KHÍ ROBOT MMR Mô hình robot MMR ban đầu thiết kế có dạng: Khâu 1 Khâu 2 Khâu 3 Khâu 4 Xe Hình 2.1 Mô hình bề ngoài của MMR ban đầu. Từ mô hình cơ bản trên chúng em đã thiết kế từng khâu, trong quá trình thiết kế cơ khí của robot MMR em đã được giao trách nhiệm quan trọng nhất là thiết kế phần xe . Vì vậy em xin trình bày đầy đủ các bước thiết kế và các bản vẽ chi tiết của xe. 2.1.Đặt vấn đề Xe là phần vừa di chuyển vừa mang tất cả khối lượng của robot, ngoài ra còn phải chứa các bộ nguồn và mạch điều khiển robot. Trong khi di chuyển xe phải rất chắc chắn, hình dạng phải gọn và phù hợp với điều kiện làm việc. Để đi đến thiết kế cụ thể thì mô hình đầu tiên của xe (hình 2.2) đã được lựa chọn. Robot MMR có thể làm việc trong những nơi nhỏ hẹp như là làm sạch khoang tàu, sửa chữa khoang tàu. Vì vậy hình dáng của xe có dạng hình thoi là rất hợp lý, hình dáng này vừa cho khoảng trống trong xe rộng vừa tạo dáng công nghiệp. Hình 2.2 Nếu hình dáng xe như vậy mà ta thiết kế theo chi tiết dạng hộp hay các tấm sắt dày ghép lại với nhau thì sẽ không khả thi mà giá cả có thể đắt. Với robot MMR này em xin đưa ra phương án thiết kế là: sử dụng các thanh ghép lại thành một khung và sau đó dùng tấm tôn bọc ngoài. 2.2 Thiết kế xe 2.2.1 Khung xe Khung xe (hình 2.3) được ghép với nhau bởi các thanh nhôm. Để liên kết các thanh nhôm này lại với nhau ta dùng vít hoặc định tán. Hình 2.3 2.2.2 Vỏ bọc của xe a) Tấm bọc ở gầm xe được làm từ tôn chiều dày của loại tôn đã chọn là 0.3mm Hình 2.4 Ta ghép phần khung xe và tấm bọc ngoài này vào như hình 2.5 và được lắp chặt nhờ đinh tán. Hình 2.5 b) Tấm bọc 2 bên sườn xe. Gồm 4 tấm như hình vẽ ( mỗi loại có 2 tấm) Ở 2 tấm to thì có bố trí các lỗ để lắp bánh xe. Hình 2.6 Kích thước của tấm cạnh ( gồm 2 tấm) Kích thước tấm ốp 2 cạnh (gồm 2 tấm) Các tấm được lắp vào khung như (hình 2.7). Hình 2.7 c) Tấm phía trước mui xe (hình 2.8) kích thước của tấm là 300 x 150 mm Hình 2.8 Ghép tấm vào xe (hình 2.9) Hình 2.9 d)Bánh : Sau khi đã nắp xong các chi tiết của xe ta phải đi lắp các bánh. Gồm 4 bánh 2 bánh nhỏ và 2 bánh to (hình 2.10). Hình 2.10 Để điều khiển xe chạy theo các hướng thì ta bố trí 2 động cơ bước gắn trực tiếp vào 2 bánh lớn và lắp vào xe (hình 2.11) Hình 2.12 Kích thước của nắp trên xe: Khi các chi tiết của xe đã thiết kế và lắp đặt xong thì phần khoảng trống trong xe là nơi đặt các thiết bị điều khiển (hình 2.13). Hình 2.13 KẾT LUẬN Về mặt lý thuyết: Đã nắm được cơ sở cơ học robot và các phương pháp nghiên cứu: + Phương pháp khảo sát bài toán động học thuận và ngược. + Phương pháp xây dựng quỹ đạo cho robot. - Đã nắm được quy trình tính toán, thiết kế và chế tạo một robot đơn giản. Về mặt thực hành: - Đã thiết kế, chế tạo mẫu được mô hình rôbốt mini điều khiển từ xa bằng máy tính. Thông qua mạch điều khiển đã thực hiện được việc kết nối giữa cơ khí và điện tử.Thiết kế được một bộ chương trình gồm các mô đun tính toán, mô phỏng hoạt động, mạch điện tử điều khiển rôbốt. Việc kết hợp học tập nghiên cứu về lý thuyết gắn liền với thực tế chế tạo đã giúp chúng em hiểu sâu hơn lý thuyết đồng thời bước đầu làm quen với công việc thực tế. Về đề tài này tuy đã có những thành công bước đầu nhưng vẫn còn nhiều hạn chế do đó chúng em đề ra hướng phát triển trong tương lai là: Hoàn thiện hơn về kết cấu cơ khí cũng như hệ thống điều khiển để robot có thể hoạt động một cách chính xác. Trang bị thêm hệ thống sensor, camera dẫn đường cho rôbốt để rôbốt có thể hoàn toàn được điều khiển từ xa trong môi trường tách biệt. Đặc biệt có thể lập trình xử lý ảnh giúp rôbốt có khả năng tự động nhận biết và giải quyết thông minh tình huống đặt ra. TÀI LIỆU THAM KHẢO [1] Nguyễn Văn Khang: Cơ sở cơ học kỹ thuật. Tập I, II. NXB Đại học Quốc gia Hà Nôi 2002. [2] Nguyễn Văn Khang: Bài giảng động lực học hệ nhiều vật. ĐHBK Hà Nội 2002. [3] Nguyễn Thiện Phúc: Robot công nghiệp. NXB Khoa học và kỹ thuật, Hà Nội 2002. [4] Trịnh Chất- Lê Văn Uyển: Tính toán thiết kế hệ dẫn động cơ khí. Tập I, II. NXB giáo dục 2001. [5] Phạm Huy Điển: Tính toán, lập trình và giảng dạy trên toán học trên MAPLE. NXB khoa học kỹ thuật Hà Nội 2002. [6] Phạm Công Ngô: Tự học lập trình VISUAL C++6.0. NXB thống kê 2002.