Đồ án Tính toán thiết kế robot

Đồ án TÍNH TOÁN THIẾT KẾ ROBOT TÍNH TOÁN THIẾT KẾ ROBOT Page 1 Mục lục Phần 1: Phân tích và lựa chọn cấu trúc. ..................................................................................... 1 1.Giới thiệu chung về Robot và nhu cầu thực tế hiện nay .................................................. 1 2.Phân tích đề bài và lựa chọn phương án thiết kế. ............................................................. 3 2.1.Phân tích đề bài.............................................................................................................. 3 2.2. Phân tích các phương án thiết kế................................................................................ 3 2.3.Mô hình nhóm thiết kế .................................................................................................. 5 Phần 2 : Giải bài toán động học. ................................................................................................. 8 1.Hệ tọa độ trục và bảng DH .................................................................................................. 8 1.1.Thiết lập hệ tọa độ trục. ................................................................................................ 8 1.2. Bảng Denavit – Hartenberg. ....................................................................................... 9 2.Tính toán các ma trận thuần nhất. ..................................................................................... 10 3.Giải bài toán động học thuận. ............................................................................................ 12 3.1 Xác định vận tốc điểm tác động cuối và vận tốc khâu thao tác............................. 12 3.2. Xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc điểm E và vận tốc góc. ...................................................................................................... 13 4.Giải bài toán động học ngược............................................................................................ 20 Phần 3: Tính toán lực ................................................................................................................. 25 1.Tính toán tĩnh . .................................................................................................................... 25 2. Tính toán lực momen lớn nhất ở trạng thái tĩnh............................................................. 29 Phần 4: Tính toán dẫn động cho robot ..................................................................................... 34 1.Thiết kế hệ dẫn động cho một khớp. ................................................................................ 35 Thiết kế hệ dẫn động cho khâu thứ nhất ......................................................................... 36 2.Chọn động cơ....................................................................................................................... 36 3.Tính toán tỉ số truyền của hộp giảm tốc. .......................................................................... 38 Phần 5 : Tính toán động lực học. .............................................................................................. 38 1.Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương trình động lực học............................................................................................................................ 38 2. Tính động năng, thế năng của robot ............................................................................... 40 2.1 Động năng, ma trân khối lượng M(q). ...................................................................... 40 TÍNH TOÁN THIẾT KẾ ROBOT Page 2 2.2 Biểu thức thế năng của hệ. ........................................................................................ 40 3. Thiết lập phương trình vi phân chuyển động của robot ................................................ 41 Phần 6 :Luật điều khiển ............................................................................................................. 47 1. Hệ thống điều khiển trong không gian khớp. ................................................................ 47 Hệ thống điều khiển phản hồi không bù G(q) ............................................................... 47 TÍNH TOÁN THIẾT KẾ ROBOT Page 1 Phần 1: Phân tích và lựa chọn cấu trúc. 1.Giới thiệu chung về Robot và nhu cầu thực tế hiện nay Robot là máy, thiết bị tự động linh hoạt phục vụ con người : - Có hình dạng giống người hoặc cánh tay người. - Có khả năng thao tác tự động. - Có khả năng bắt chước thao tác giống người. Cuộc sống ngày càng văn minh hiện đại, mức sống của người dân ngày càng được nâng cao, đòi hỏi phải nâng cao năng suất và chất lượng của sản phẩm. Vì vậy càng phải ứng dụng rộng rãi các phương tiện tự động hoá vào sản xuất nên càng tăng nhanh nhu cầu về ứng dụng Robot để tạo ra các hệ thống sản xuất tự động và linh hoạt. Robot là máy, thiết bị cố định hoặc di động, được tích hợp từ nhiều bộ phận trong đó các bộ phận chính bao gồm: - Cơ cấu chấp hành. - Hệ thống dẫn động. - Hệ thống điều khiển theo chương trình có khả năng lập trình linh hoạt. - Hệ thống thông tin giám sát. Trong những năm gần đây thì việc áp dụng các loại Robot vào các dây chuyền sản xuất ngày càng được sử dụng rộng rãi ở các doanh nghiệp. Ví dụ như các loại Robot: Robot hàn, Robot phun sơn ở các công ty sản xuất và lắp ráp ô tô. Đặc biệt là ở các công ty sản xuất và lắp ráp các linh kiện điện tử. Đây là công việc đòi hỏi chính xác cao, và thường lặp lại nên dễ gây mệt mỏi cho người làm vì vậy sử dụng Robot ở các công ty này là rất phổ biến. Ngoài ra hiện nay ở các phân xưởng sản xuất sử dụng rất nhiều loại xe Robocar hoặc ở các công việc trong môi trường độc hại, thường xuyên tiếp xúc với hoá chất độc hại thì cũng sử dụng Robot làm thay con người. TÍNH TOÁN THIẾT KẾ ROBOT Page 2 Hình 1.1 Một số hình ảnh về robot trong công nghiệp TÍNH TOÁN THIẾT KẾ ROBOT Page 3 2.Phân tích đề bài và lựa chọn phương án thiết kế. 2.1.Phân tích đề bài Yêu cầu đặt ra của để bài là thiết kế tính toán robot hàn hồ quang với quỹ đạo đường cong bất kỳ và có kích thước mối hàn là 40cm x 40 cm x 40cm - Để đảm bảo robot có khả năng đưa đầu hàn được tới mọi vị trí trong không gian làm việc thì robot cần từ 3 bậc tự do trở lên. - Để tiếp cận bề mặt chi tiết theo hướng cụ thể thì cần thêm ít nhất 2 bậc tự do, nếu chi tiết hàn có thể chuyển động theo các phương, hoặc chuyển động quay thì robot chỉ cần 4 bậc tự do. Từ những phân tích trên và dựa vào thực tế , do khả năng còn có hạn nên nhóm chúng em đã quyết định chọn thiết kế robot 3 bậc tự do. Với yêu cầu thêm là chi tiết cần hàn chỉ bị khống chế 4 bậc tự do, sẽ thực hiện 2 chuyển động quay hoặc tịnh tiến. 2.2. Phân tích các phương án thiết kế. Phương án 1 Phương án 2 TÍNH TOÁN THIẾT KẾ ROBOT Page 4 Phương án 3 Ba phương án trên là dạng trong thực tế được ứng dụng rất nhiều. Đặc biệt là phương án 1 trong thực tế dùng rất phổ biến do kết cấu của nó đơn giản toàn khớp quay.Nên việc tính toán cũng như lập trình điều khiển cũng dễ dàng hơn so với 2 phương án 2 và 3. Ở 2 phương án 2 và 3 do có khớp tính tiến nên robot chỉ thực sự linh hoạt khi nó có thêm 1 bậc tự do ở cổ bàn tay nắm bắt công cụ, do đó nó thường là 4 bậc tự do. Theo yêu cầu của đề tài thiết kế mô hình cũng như khả năng của nhóm còn hạn chế nên 2 phương án này áp dụng vào đề tài sẽ khó đáp ứng được yêu cầu của đề tài là hàn được đường cong bất kì cũng như nhóm khó có khả năng thiết kế và tính toán được. Vì vậy nhóm đã đi đến thống nhất chọn thiết kế theo phương án 1 TÍNH TOÁN THIẾT KẾ ROBOT Page 5 2.3.Mô hình nhóm thiết kế Hình 1.2 Mô hình robot hàn TÍNH TOÁN THIẾT KẾ ROBOT Page 6 Hình 1.3 Khâu đế Hình 1.4 Khâu 1 TÍNH TOÁN THIẾT KẾ ROBOT Page 7 Hình 1.5 Khâu 2 Hình 1.6 Khâu 3 TÍNH TOÁN THIẾT KẾ ROBOT Page 8 Phần 2 : Giải bài toán động học. 1.Hệ tọa độ trục và bảng DH 1.1.Thiết lập hệ tọa độ trục. Khâu đế: ta chọn hệ tọa độ XoYoZo có trục Zo chọn trùng với khớp 1, trục Xo chọn tùy ý sao cho phù hợp nhất như hình vẽ, trục Yo chọn theo quy tắc tam diện thuận. Khâu 1: ta chọn hệ tọa độ X1Y1Z1 có trục Z1 trùng với khớp 2, trục X1 ta chọn theo hướng Z0 x Z1 , trục Y1 chọn theo quy tắc tam diện thuận. Khâu 2: ta chọn hệ tọa độ X2Y2Z2 có trục Z2 trùng với khớp 3, trục X2 ta chọn theo đường vuông góc chung Z1 và Z2 , trục Y2 chọn theo quy tắc tam diện thuận. Khâu 3: ta chọn hệ tọa độ X3Y3Z3 có trục Z3 song song Z2, X2 chọn theo đường vuông góc chung Z2 và Z3, Y3 chọn theo quy tắc tam diện thuận. Các biến khớp: q1 = θ1 q2 = θ2 q3 = θ3 TÍNH TOÁN THIẾT KẾ ROBOT Page 9 Sơ đồ động học robot 1.2. Bảng Denavit – Hartenberg. Từ việc chọn hệ tọa độ ta có bảng DH sau: Khâu di Θi ai αi 1 d1 q1 a1 π/2 2 0 q2 a2 0 3 0 q3 a3 0 Trong đó các giá trị đã biết: d1 = 0.13 m , a1 = 0.155 m, a2 = 0.5 m, a3 = 0.4 m. TÍNH TOÁN THIẾT KẾ ROBOT Page 10 Để tiện cho quá trình tính toán, ta sẽ giữ nguyên các ký hiệu của các độ dài, góc trong các biểu thức, số liệu cụ thể sẽ được thay vào khi ta có kết quả cuối cùng. 2.Tính toán các ma trận thuần nhất. Ta có : Dạng tổng quát của ma trận Denavit-Hartenberg cho các khâu i-1 Ai= i i i i i i i i i i i i i i i i i cosθ sinθ cosα sinθ sinα a cosθ sinθ cosθ cosα sinα cosθ a sinθ 0 sinα cosα d 0 0 0 1             Ma trận Denavit-Hartenberg cho khâu 1: 1 1 1 1 1 1 1 10 1 1 cos( ) 0 sin( ) cos( ) sin( ) 0 cos( ) sin( ) 0 1 0 0 1 0 1 q q a q q q a q A d             Ma trận Denavit-Hartenberg cho khâu 2 : 2 2 2 2 2 2 2 21 2 cos( ) sin( ) 0 cos( ) sin( ) cos( ) 0 sin( ) 0 1 1 0 0 1 0 1 q q a q q q a q A             Ma trận truyền khâu 2 và 3 so với khâu 0: 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 20 0 1 2 1 2 2 2 1 2 2 . 0 0 0 0 1 C C C S S a C C S C S S C a S C A A A S C d a S                Ma trận Denavit-Hartenberg cho khâu 3: TÍNH TOÁN THIẾT KẾ ROBOT Page 11 3 3 3 3 3 3 3 32 3 cos( ) sin( ) 0 cos( ) sin( ) cos( ) 0 sin( ) 0 0 1 0 0 0 0 1 q q a q q q a q A             Ma trận tác động cuối: 1 23 1 23 1 1 3 23 2 2 1 23 1 23 1 1 3 23 2 20 0 1 2 3 1 2 3 23 23 1 2 2 3 23 ( ) ( ) ( ) . . 0 0 0 0 1 C C C S S C a C a C S C S S C S a C a C A q A A A S C d a S a S                   Với C1=cos(q1), S1=sin(q1), C2=cos(q2), S2=sin(q2), S23=cos(q2+q3), C23=cos(q2+q3), q=[q1, q2, q3] T Mặt khác ta lại mô tả được hướng và vị trí qua ma trận sau thông qua vector p=[xE, yE, zE, α, β, η] T α, β, η là 3 góc Cardan 0 3 cos( )cos( ) cos( )sin( ) sin( ) sin( )sin( )cos( ) cos( )sin( ) sin( )sin( )sin cos( )cos( ) sin( )cos( ) ( ) cos( )sin( )cos( ) sin( )sin( ) cos( )sin( )sin( ) cos( )cos( ) cos( )cos( ) 0 0 0 1 E E E x y A p z                                                   So sánh hai ma trận 0 A3(q), 0 A3(p) trên ta thiết lập được hệ phương trình động học sau: 0 0 1 3 1 2 3 2 1 2 1 13 3 0 0 2 3 1 2 3 2 1 2 1 13 3 0 0 3 1 33 3 ( )[1,4] )[1,4] [ cos( ) os( ) os ) os( ) cos( )] 0 ( )[2,4] ( )[2,4] [ sin( ) os( ) sin( ) os( ) sin( )] 0 ( )[3,4] ( )[3,4] [d sin( E E E f p q x a q c q q a c q c q a q f p q y a q c q q a q c q a q f p q z a A A A A A A                      2 3 2 2 0 0 4 1 2 33 3 0 0 5 1 2 33 3 0 0 6 3 3 ) sin( )] 0 ( )[1,1] ( )[1,1] os( ) os( ) os( ) os( ) 0 ( )[2,2] ( )[2,2] sin( )sin( )sin( ) sin( )sin( ) 0 ( )[3,3] ( )[3,3] os( ) os( ) 0 q q a q f p q c c c q c q q f p q q q q f p q c c A A A A A A                                    TÍNH TOÁN THIẾT KẾ ROBOT Page 12 3.Giải bài toán động học thuận. 3.1 Xác định vận tốc điểm tác động cuối và vận tốc khâu thao tác. Ở trên ta rút ra ( với q=[q1, q2, q3] T ) 1 3 2 3 2 2 1 0 1 3 2 3 2 2 1 1 2 2 3 2 3 cos( )[ cos( ) cos( ) ] sin( )[ cos( ) cos( ) ] sin( ) sin( ) E E E E x q a q q a q a r y q a q q a q a z d a q a q q                            0 0 . . 0 ( ) ( ) .E EE E d r r q J q dt q       Ma trận Jacobi JE 1 3 2 3 2 2 1 1 3 2 3 2 2 3 1 2 3 1 3 2 3 2 2 1 1 3 2 3 2 2 3 1 2 3 3 2 3 2 2 sin( )[a os( ) os( ) ] os( )[ sin( ) sin( )] os( )sin( ) os( )[a os( ) os( ) ] sin( )[ sin( ) sin( )] sin( )sin( ) 0 a os( ) os( ) a E q c q q a c q a c q a q q a q a c q q q J c q c q q a c q a q a q q a q a q q q c q q a c q                     3 2 3os( )c q q        Khi đó: 1 3 23 2 2 1 1 1 3 23 2 2 2 3 1 23 3 0 1 3 23 2 2 1 1 1 3 23 2 2 2 3 1 23 3 3 23 2 2 2 3 23 3 ( ) ( ) ( ) ( ) ( ) E E E E x S a C a C a q C a S a S q a C S q y C a C a C a q S a S a S q a S S q z a C a C q a C q                                 Từ ma trận 0A3 ta rút ra ma trận cosin chỉ hướng 1 2 3 1 2 3 1 0 3 1 2 3 1 2 3 1 2 3 2 3 cos( )cos( ) cos( )sin( ) sin( ) sin( )cos( ) sin( )sin( ) cos( ) sin( ) cos( ) 0 q q q q q q q R q q q q q q q q q q q                 1 2 3 0 0 0 3 3 3 1 2 3 1 sin( )( ) cos( )( )T q q q R R q q q q                TÍNH TOÁN THIẾT KẾ ROBOT Page 13 3.2. Xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc điểm E và vận tốc góc. Để khảo sát kết quả trên ta xây dựng quy luật chuyển động của các biến khớp q như sau: 1 2 3 2sin(3 ) 2cos(2 ) 1 sin(2 ) q t q t q t       Sử dụng phần mềm Maple ta vẽ được đồ thị quỹ đạo khâu thao tác cuối, vận tốc điểm E và vận tốc góc khâu thao tác cuối: Hình 2.1 Đồ thị quỹ đạo điểm thao tác cuối E TÍNH TOÁN THIẾT KẾ ROBOT Page 14 Hình 2.2 Đồ thị vận tốc điểm thao tác cuối E Hình 2.3 Đồ thị vận tốc góc khâu thao tác cuối  Chương trình viết bằng maple: > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 15 > > TÍNH TOÁN THIẾT KẾ ROBOT Page 16 > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 17 > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 18 > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 19 TÍNH TOÁN THIẾT KẾ ROBOT Page 20 > > > 4.Giải bài toán động học ngược. Để giải bài toán động học thuận ta có thể sử dụng phương pháp giải tích, Newton –Raphson, với phương pháp giải tích khi áp dụng cho robot có bậc tự do ít (3 bậc trở lại) thì việc tính toán có thể thực hiện được, nhưng áp dụng cho robot có nhiều bậc (4 bậc trở lên) thì việc tính toán, giải hệ phương trình rất phức tạp. Trong bài tiểu luận này nhóm em giải theo phương pháp Newton-Raphson Xét hệ:       1 1 3 2 3 2 2 1 2 1 3 2 3 2 2 1 3 1 2 2 3 2 3 1 2 3 os( ) os( ) os( ) 0 sin( ) os( ) os( ) 0 sin( ) sin( ) 0 0 E E E f x c q a c q q a c q a f y q a c q q a c q a f z d a q a q q f F f f                                 Bài toán khi biết được xE(t), yE(t), zE(t) tại mỗi thời điểm t ta sẽ tìm được vector q=[q1, q2, q3] T tại mỗi thời điểm đó. TÍNH TOÁN THIẾT KẾ ROBOT Page 21 Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson Quá trình lặp dừng lại khi sai số ở lần k+1 với lần k nhỏ hơn giá trị cho phép. Ta đi tìm vector q=[q1, q2, q3] T để quỹ đạo điểm tác động cuối E có phương trình: 0.2 0.1cos(5 ) 0.2 0.1sin(5 ) 0.5 E E E x t y t z        Sử dụng phần mềm maple, lập trình động học ngược ta thu được các kết quả sau: Hình 2.4 Quỹ đạo Elip trong không gian Hình 2.5 Đồ thị q1(t) TÍNH TOÁN THIẾT KẾ ROBOT Page 22 Hình 2.6 Quỹ đạo q2 (t) Hình 2.7 Quỹ đạo q3( t)  Chương trình viết bằng maple: > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 23 > > TÍNH TOÁN THIẾT KẾ ROBOT Page 24 > > > > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 25 > > > Phần 3: Tính toán lực 1.Tính toán tĩnh . Lực tác dụng vào khâu cuối coi như bằng không vì robot thiết kế là robot hàn điểm 0 0 3 3[0,0,0] ; [0,0,0] T T E EF M  Từ phần động học ta xác định được các ma trận cosin chỉ hướng sau: 1 1 0 1 1 1 0 0 0 1 0 C S R S C           1 2 1 2 1 0 2 1 2 1 2 1 2 2 0 C C C S S R S C S S C S C            1 23 1 23 1 0 3 1 23 1 23 1 23 23 0 C C C S S R S C S S C S C            Gọi  1 1, ,0c cx y ,  2 ,0,0cx ,  3,0,0cx lần lượt là khoảng cách từ gốc tọa độ của khâu 1, 2, 3 đến khối tâm của từng khâu 1, 2, 3. TÍNH TOÁN THIẾT KẾ ROBOT Page 26  Xét khâu 3: 3 1 23 00 3 3 3 3 1 233 3 3 23 03 3 333 3 1 233 3 00 3 3 3 3 1 233 3 23 a C C . a S C a S[ a ,0,0] , à [ 0,0, g] x C C[ x ,0,0] . x S C x S T c T T cc c c c c r r r v m r r r R P R                                    3 23 3 1 23 3 3 23 3 1 23 3 1 23 3 1 23 0 a S a S C a S 0 a C C a S C a C C 0 r           3 23 3 1 23 3 3 23 3 1 231 3 1 23 3 1 23 0 x S x S C x S 0 x C C x S C x C C 0 c c c c c c c r           Thay vào công thức đã được đã được thiết lập trong bài giảng robot 0 0 032 3 3 0 0 0 0 0 0 32 3 3 32 3 3 E E c F F P M M r F r P        Ta tính được 0 21 3 2 1 3 3 1 23 3 2 1 2 3 2 2 1 2 2 2 3 1 1 1 1 1 1 0 32 3 3 1 23 3 2 1 2 3 2 2 1 2 2 2 3 1 1 1 1 1 1 0 0 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 0 c c c c c c F m m m g a x S C m g a S C m g a x S C m g m m a S g m a x S g M a x C C m g a C C m g a x C C m g m m a S g m a x S g                                           Xét khâu 2: TÍNH TOÁN THIẾT KẾ ROBOT Page 27 2 1 2 00 2 2 2 2 1 22 2 2 2 02 2 222 2 1 22 2 00 2 2 2 2 1 22 2 2 a C C . a S C a S[ a ,0,0] , à [ 0,0, g] x C C[ a ,0,0] . x S C x S T T T cc c c c c r r r v m r r r R P R                                    2 2 2 1 2 2 2 2 2 1 2 2 1 2 2 1 2 0 a S a S C a S 0 a C C a S C a C C 0 r           2 2 2 1 2 2 2 2 2 1 2 2 1 2 2 1 2 0 x S x S C x S 0 x C C x S C x C C 0 c c c c c c c r           Thay vào công thức: 0 0 0 21 32 2 0 0 0 0 0 0 21 32 2 21 2 2c F F P M M r F r P        2 1 2 0 0 2 2 2 2 2 1 2 2 2 2 02 2 2 22 2 2 2 1 2 0 0 2 2 2 2 2 1 2 2 2 . [ ,0,0] , [0,0, ] [ ,0,0] . c c c c T c Tc c T x C C r R r x S C x Sr x P m g r a a C C r R r a S C a S                                    Thay vào công thức: 0 0 0 21 32 2 0 0 0 0 0 0 21 32 2 21 2 2c F F P M M r F r P        TÍNH TOÁN THIẾT KẾ ROBOT Page 28 Ta tính được: 0 21 3 2 3 3 1 23 3 2 1 2 3 2 2 1 2 2 0 32 3 3 1 23 3 2 1 2 3 2 2 1 2 2 0 0 ( ) ( ) ( ) ( ) ( ) 0 c c c c F m m g a x S C m g a S C m g a x S C m g M a x C C m g a C C m g a x C C m g                                  Xét khâu 1: 1 1 00 1 1 1 1 11 1 1 01 1 1 111 1 11 1 1 00 1 1 1 1 11 1 a C . a S d[ a , ,0] , à [ 0,0, g] C x[ x , ,0] . S x y T T T cc c c c c c c r r r d v m r y r r R P R                                     1 1 1 1 1 1 1 1 1 1 1 0 d a S d 0 a C a S a C 0 r           1 1 1 1 1 1 1 1 1 1 1 0 y S x y 0 C x S x C x 0 c c c c c c c r           Thay vào công thức: 0 0 010 21 1 0 0 0 0 0 0 10 21 1 10 1 1c F F P M M r F r P        Ta tính được: TÍNH TOÁN THIẾT KẾ ROBOT Page 29 0 21 3 2 1 3 3 1 23 3 2 1 2 3 2 2 1 2 2 2 3 1 1 1 1 1 1 0 10 3 3 1 23 3 2 1 2 3 2 2 1 2 2 2 3 1 1 1 1 1 1 0 0 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 0 c c c c c c F m m m g a x S C m g a S C m g a x S C m g m m a S g m a x S g M a x C C m g a C C m g a x C C m g m m a S g m a x S g                                          2. Tính toán lực momen lớn nhất ở trạng thái tĩnh Tính các momen động cơ cần để cho robot cân bằng từ các kết quả tính toán trên: Mdc1= 0 M10[2]=0(N/m) Để tính momen động cơ 2 ta phải chiếu vector 0M21 lên tọa độ khâu 2 2 M21= 0 R2 T M21 =>Mdc2= 2 M21[3]=g(C23m3g3 – C23m3x23+a2C2m3+a2C2m2 – C2xc2m2) Để tính momen động cơ 3 ta phải chiếu vector 0 M32 lên tọa độ khâu 2 3 M32= 0 R3 T M32 =>Mdc3= 3 M32[3]=m3g(a3 – xc3) C23 Với các số liệu: d1=0,13(m); a1=0,155(m); a2=0,5(m); a3=0,4(m); m1=5,133(kg); m2=6,329(kg); m3=1,415(kg) xc1=0,03982(m); yc1=0,03731(m); xc2=0,22782(m); yc2=0,107(m) Các kết quả thu được: Mdc 1 max = 0 (N.m) TÍNH TOÁN THIẾT KẾ ROBOT Page 30 Mdc 2 max = 27.906 (N.m) tại 1 2 3 1 2 ( ) 2 ( ) q q k rad k Z q k rad         Mdc 3 max = 4.067 (N.m) tại 1 2 3 1 2 ( ) 2 ( ) q q k rad k Z q k rad          Chương trình viết bằng maple: > > > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 31 > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 32 > > > > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 33 > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 34 > > > > > Phần 4: Tính toán dẫn động cho robot Thiết bị truyền động là phần quan trọng để thực hiện các sơ đồ động của cơ cấu tay máy và là phần quyết định kết cấu của robot. Các sơ đồ động cơ cấu tay máy rất đa dạng và các loại hình kết cấu tay máy cũng rất phong phú. Tuy nhiên, qua thực tế sử dụng đã dần dần định hình các xu hướng về loại hình kết cấu robot. Trong kỹ thuật robot hiện đại có 2 xu hướng cơ bản về loại hình kết cấu. Thứ nhất là robot chuyên dung để đáp ứng một công việc rất cụ thể trong dây chuyền sản xuất và robot chuyên môn hóa trang bị cho một nhóm thiết bị công nghệ nào đó để thực hiện một loại hình công việc. Các loại robot này thường có số bậc tự do không lớn nhưng lại yêu TÍNH TOÁN THIẾT KẾ ROBOT Page 35 cầu cao về mức độ thao tác nhanh, về độ chính xác và độ tin cậy. Tuy nhiên lại khó áp dụng khi cần thay đổi các thông số công nghệ. Xu hướng 2 là tạo ra các robot đa chức năng để thích nghi với nhiều loại hình công nghệ hoặc với phạm vi thay đổi tương đối rộng các thông số công nghệ. Các loại hình robot này thường có số bậc tự do cao hơn, cơ động hơn nhưng lại khó đảm bảo độ chính xác và độ tin cậy cao. Ngoài ra giá thành chế tạo lại đắt hơn khi sử dụng, tùy theo công việc có lúc lại không dung hết số bậc tự do. Mâu thuẫn trên có thể được khắc phục nếu áp dụng nguyên tắc modun hóa khi thiết kế robot. Theo đó kết cấu robot gồm các cụm chi tiết máy điển hình có chức năng hoạt động tương đối độc lập, được gọi là các modun. Các modun này có thể được thiết kế, chế tạo chuyên môn hóa đạt được những tính năng kỹ thuật cao với giá thành phải chăng. Khi nối ghép các modun theo nhiều phương án khác nhau có thể tạo ra các kết cấu robot khác nhau. Các robot thiết kế theo kiểu modun hóa được dùng rộng rãi khi tạo dựng các modun sản xuất linh hoạt. Các thiết bị truyền động là bộ phận chủ yếu để tạo ra các modun kết cấu tay máy. Về nguyên tắc trong kết cấu tay máy có thể dùng hầu hết các thiết bị truyền dẫn động thông thường. Tuy nhiên cũng có những yêu cầu riêng như là gọn nhẹ, linh hoạt dễ điều khiển, cần triệt tiêu khe hở khi quay đảo chiều… 1.Thiết kế hệ dẫn động cho một khớp. Theo nguyên tắc thiết kế, chế tạo theo modun: chọn cơ cấu dẫn động của 3 khâu là bộ truyền bánh răng. TÍNH TOÁN THIẾT KẾ ROBOT Page 36 Thiết kế hệ dẫn động cho khâu thứ nhất Trong cơ cấu trên: bánh răng được nối từ động cơ qua bánh bị dẫn truyền qua bánh dẫn tạo truyền động cho cả cụm cơ cấu phía trên. 2.Chọn động cơ. Truyền động điện được dùng khá nhiều trong kĩ thuật robot,vì có nhiều ưu điểm như là điều khiển đơn giản không phải dùng các bộ biến đổi phụ ,không gây bẩn môi trường,các loại động cơ điện hiện đại có thể lắp trực tiếp trên các khớp quay… Tuy nhiên so với truyền động thủy lực hoặc thủy khí thì truyền động điện có công suất thấp và thông thường phải cần dùng thêm hộp giảm tốc vì các khâu của robot chuyển động với tốc độ TÍNH TOÁN THIẾT KẾ ROBOT Page 37 Động cơ AC – Servo có nhiều đặc điểm phù hợp với hệ thống truyền động điện của robot công nghiệp như: + Khả năng quá tải về momen quay cao( Mmax/Mb~4..100). +Khả năng gia tốc lớn, vốn được coi là điểm quan trọng của các cơ cấu truyền động với các kết cấu đi kèm( hộp số) cần phải có quán tính nhỏ. +Công suất động cơ tối đa lớn. +Thông thường cần có một phạm vi điều khiển tuyến tính kể cả tốc độ quay điểm đứng yên. +Mặt khác hệ thống cần có một độ bền dẻo nhất định để có thể truyền lực hoặc momen với tần số cộng hưởng riêng lớn, nhắm ngăn ngừa các kích thích dao động xoắn +Tín hiệu đầu ra của động cơ được nối với một mạch điều khiển. +Giá thành phù hợp. +Dễ tìm mua. *Chọn động cơ cho khâu thứ 3: Số vòng quay của trục công tác: 12n  (rad/s) 12 .60 115 2   (vòng/phút) Chọn tỷ số truyền sơ bộ: 40sbu  Số vòng quay sơ bộ của động cơ: . 115.40 4600dc sbn nu   (vòng/phút) Momen lớn nhất của động cơ khâu 3(theo phần tĩnh học):  3 4.067• .dc maxM N m Tra bảng thông số động cơ AC- Servo:    3 . 115.40 4600 v ng / ph 4.067• t .dc ma dc s x bn n u ò M N m ú       Ta chọn động cơ với thông số: + Số hiệu động cơ: 041P1 +Số hiệu Drive: MCDDT3120 +Công suất: 400(w) TÍNH TOÁN THIẾT KẾ ROBOT Page 38 +Momen xoắn trung bình:1.3(N.m) +Momen xoắn lớn nhất: 3.82(N.m) +Số vòng quay nhỏ nhất: 3000(vòng/phút) +Số vòng quay lớn nhất: 4500(vòng/phút) 3.Tính toán tỉ số truyền của hộp giảm tốc. Tỷ số truyền thực của hệ: 4000 35 115 dc t lv n u n    Chọn tỷ số truyền của hộp giảm tốc: 40u  Phần 5 : Tính toán động lực học. 1.Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương trình động lực học. Gọi  1 1, ,0c cx y ,  2 ,0,0cx ,  3,0,0cx lần lượt là khoảng cách từ gốc tọa độ O0, O1, O2 đến khối tâm của từng khâu 1, 2, 3 và gọi i jC là tọa độ trọng tâm của khâu thứ j trên hệ Ri Các tọa độ và vận tốc góc các khâu: 1 11 1 0 1 c c c x y r             => 1 1 1 1 1 10 0 1 1 1 1 1 1 C (x +a ) S (x +a ) . y +d 1 c c c c c r A r              => 1 1 1 1 1 1 1 S (x +a ) 0 0 C (x +a ) 0 0 0 0 0 c T cJ          TÍNH TOÁN THIẾT KẾ ROBOT Page 39 2 2 2 0 0 1 c c x r             => 1 2 2 1 2 2 1 1 1 2 2 1 2 2 1 10 0 2 2 2 2 2 2 2 2 1 C C x +C a C +C a S C x +S a C +a S . S x +a S +d 1 c c c c c r A r              => 1 2 2 1 2 2 1 1 1 2 2 1 2 2 2 1 2 2 1 2 2 1 1 1 2 2 1 2 2 2 2 2 2 -S C x -S a C -S a -C S x -C a S 0 C C x +C a C +C a -S S x -S a S 0 0 C x +a C 0 c c T c c c J          3 3 3 0 0 1 c c x r             => 23 3 3 1 23 1 2 2 1 1 1 23 3 3 1 23 1 2 2 1 10 0 3 3 3 3 23 3 3 23 2 2 1 C x +a C C +C a C +C a S C x +a S C +S a C +S a . S x +a S +a S +d 1 c c c c c r A r              => 1 23 3 3 1 23 1 2 2 1 1 1 23 3 3 1 23 1 2 2 1 23 3 3 1 23 3 1 23 3 3 1 23 1 2 2 1 1 1 23 3 3 1 23 1 2 2 1 23 3 3 1 23 23 3 3 23 2 2 23 3 3 23 -S C x -a S C -S a C -S a -C S x -a C S -C a S -C S x -a C S C C x +a C C +C a +C a -S S x -a S S -S a S -S S x -a S S 0 C x +a C +a C C x +a C c c c T c c c c c J        1 0 0 1 1 1 1 1 1 1 0 0 0 0 1 0 0 0 0 00 T RR R q J                         2 1 2 2 0 0 2 2 2 2 2 2 1 2 2 0 0 0 0 0 0 1 0 T R S q S R R C q J C                         1 23 23 3 0 0 3 3 3 3 3 1 23 3 23 2 3 0 0 C 0 0 0 1 1 T R q S S R R q C J q q                          Ma trận tenxơ quán tính của hai khâu 1, 2 và 3 với trục gắn vào khối tâm song song với hệ trục của khâu cũng tương ứng là hệ quán tính chính: 1I = 1 1 1 ••••0••••••0 0•••••• •••0 0••••••0•••••• x y z I I I           , 2I = 2 2 2 ••••••0••••••0 0•••••• •••0 0••••••0•••••• x y z I I I           , 3I = 3 3 3 ••••0••••••0 0•••••• •••0 0••••••0•••••• x y z I I I           TÍNH TOÁN THIẾT KẾ ROBOT Page 40 2. Tính động năng, thế năng của robot 2.1 Động năng, ma trân khối lượng M(q).   3 1 1 1 ( ) 2 2 T T T T i i Ti Ri i Ri i T q J m J J I J q q M q q           11 12 133 21 22 23 1 31 32 33 •••• •••• ( ) ( ) •••• •••• •••• •••• T T Ti i Ti Ri i Ri i m m m M q J m J J I J m m m m m m               Trong đó:  2 2 211 3 3 3 3 3 3 3 3 23 3 2 3 3 2 3 2 3 3 1 3 3 1 23 2 2 2 2 2 3 2 2 2 2 2 2 2 2 2 2 1 3 2 1 2 2 1 2 1 1 2 1 2 2 3 1 1 1 1 1 1 ( 2 ) 2 2 2 2 ( ) ( 2 2 2 ) 2 1 c y c c c c y c c y c m m a m x I m x a c m a a m a c c m x a m a a c m a m x m x a I c m x a m a a m a a c m a m a m a m x I m x a                         12 21 13 0m m m   2 2 2 2 22 3 2 3 3 2 3 3 3 3 3 3 2 3 2 2 2 2 3 3 3 2 2 2 3 (2 2 ) 2 2 c c z z c c m m a a m a x c m x m a I I m x m a m a x m a a m a            2 23 32 3 2 3 3 2 3 3 3 3 3 3 3 3( ) 2c c z cm m m a x m a a c m x I m a x       2 33 3 3 3 3( )z cm I m a x   2.2 Biểu thức thế năng của hệ. 2 0 0 1 . . T Ci i mi g r     với 0 [0, 0, g] Tg   1 1 1 2 2 2 2 2 1 3 23 3 3 23 2 2 1m (y +d )g+m (S x +a S +d )g+m (S x +a S +a S +d )gc c c  2.3 Tổng công ảo của hệ Công ảo của các lực suy rộng không có thế ở đây giả tại điểm tác động cuối robot chịu 1 lực [ , , ]TE x y zF F F F    : 0 1 1 2 2 3 3 1 1 2 2 3 3( ) ( ) T T E EA q q q F r U q U q U q F J q                   Vector lực suy rộng viết dạng cột được lấy từ tổng công ảo có dang như sau: TÍNH TOÁN THIẾT KẾ ROBOT Page 41 1 2 3[ , , ] np T T EQ Q Q Q U J F   (4.1) Trong đó U =[U1, U2, U3] T là vector momen dẫn động, JE được tính từ phần động học: 1 3 2 3 2 2 1 1 3 2 3 2 2 3 1 2 3 1 3 2 3 2 2 1 1 3 2 3 2 2 3 1 2 3 3 2 3 2 2 sin( )[a os( ) os( ) ] os( )[ sin( ) sin( )] os( )sin( ) os( )[a os( ) os( ) ] sin( )[ sin( ) sin( )] sin( )sin( ) 0 a os( ) os( ) a E q c q q a c q a c q a q q a q a c q q q J c q c q q a c q a q a q q a q a q q q c q q a c q                     3 2 3os( )c q q          3. Thiết lập phương trình vi phân chuyển động của robot Phương trình lagrang loại 2: np i i i d T T Q dt q q q              Viết dưới dạng khai triển:   ( , )M q q q q Q U   Trong đó: , 1 ( , , ) n j k l k l j k l j q q q         1 ( , , ) 2 kj lj kl l k j m m m k l j q q q              -Thế năng tính theo biểu thức. iQ -Lực suy rộng ứng với tọa độ suy rộng iq , tính theo biểu thức: 1 1 2[ , ,..., ] ipT i ip i T n r Q F M q Q Q Q Q      U - Lực/moomen điều khiển 1 2[ , ,..., ] T nU U U U Tính các thành phần của biểu thức: 1 1 2 2 2 2 2 2 2 3 23 3 3 3 23 3 2 2 2 3 3 23 3 3 3 0 g(m C x +m a C +m C x +m a C +m a C ) m C (x +a )g c c c G q G q G q             3 3 23 3 3 3 3 23 3 3 3 3 23 3 2 2 23 3 3 2 2 2 23 3 2 2 3 23 3 2 2 23 3 m z C (x +a )+m z C x +m z a C )dq1dq3 +(2m a C S x 2m a a S -2m a S a C -2m a S C x )dq2dq3 c c c c c c c TÍNH TOÁN THIẾT KẾ ROBOT Page 42 2 2 2 2 3 2 2 23 3 3 2 2 23 3 3 2 2 3 23 3 2 2 3 23 3 23 3 23 2 2 2 2 3 3 23 23 3 23 3 2 2 2 2 2 2 2 2 2 2 2 2 2 2 3 23 23 3 23 3 3 2 1 , 3 1 2 2 33 ( , ,1) [2 2 2 2 2 2 2 2 2 2 2 2 2 4 2 y c c c y n k l k l c x x c C I S m a S C x m a C S x m a S a C mv k a C a S m C x S m a S C I S m a C S m C x S m a C S I C S I C S m C l q q C x a S m S                   3 3 1 3 3 23 1 2 2 2 1 2 2 3 2 2 23 3 3 2 2 3 23 3 23 3 23 3 3 2 2 2 1 3 2 2 1 2 2 2 2 2 2 23 23 3 23 3 23 23 3 23 3 3 23 3 23 3 1 3 3 23 1 3 3 3 +2m a2 2 2 2 2 2 2 4 1 2 2 ) 1 3 S a +2 m a S a +4 m C x a S ]dq1 (2 2 dq2c c c c y x c c c c x a m a S a m S x a a C S x m a C a S m C x S m a S C I S I C S m C x a S m S x a m a S a dq dq C C m z             23 3 3 3 3 23 3 3 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 3 3 23 3 3 3 3 23 3 3 3 3 23 3 3 23 3 3 2 2 2 2 2 2 ) 2 ( ) ) 2 3 ( ) 3[ c c c c c c c c c c c c c c x m z a C m z a C m d C x m d a C m z C x m z a C dq m z C x a m z C x m z a C dq dq m z C x a dq             2 2 2 3 2 2 23 3 3 2 2 23 3 3 2 2 3 23 3 2 2 3 23 2 2 2 2 2 3 23 3 23 3 3 23 23 23 3 23 3 2 2 2 2 2 2 2 2 2 2 2 2 2 2 3 23 23 3 23 3 2 , 3 23 1 1 ( , , 2) [ 2 2 2 2 2 2 2 2 2 2 2 2 4 2 2 2 n k l y c c c y c y x c k l C I S m a S C x m a C S x m a S a C m a C a S m C x S m a C S C I S m a C S m C x S m a C S I C S I v k l q C S C q m x a S m                   2 2 2 1 3 2 2 1 2 2 2 3 2 2 2 3 3 23 3 3 3 3 23 3 3 3 3 23 3 2 2 23 3 3 2 2 3 23 3 2 2 3 23 3 2 2 23 3 3 1 3 3 23 1 2 2 2 1 2 3 2 2 3 3 2 3 -2m a S a -2m a S a -4m C x a S ] (-m z C (x +a )+m z C x +m z a C )dq1dq3 +(2m a C S x +2m a C a S -2m 2 2 a S a C -2m a S C 1 1 (2 2 2 x )dq2dq3 + c c c c c c c c c c c S x a m a S a m S x a dq m a C S x m     2 3 2 2 3 23 3 2 2 3 23 3 2 2 23 32 2 ) 3ca C a S m a S a C m a S C x dq  2 2 3 2 2 23 3 3 2 2 3 23 3 23 3 23 3 3 23 23 23 3 23 3 23 23 2 3 23 3 3 23 3 23 3 1 3 3 23 1 3 3 23 3 3 3 3 23 3 3 23 , 3 3 1 3 1 ( 2 2 2 2 2 2 2 4 2 2 ) 1 ( ( )) 1 2 1 ( , ,3) ( 2 c c y x c c c c c c c n k l k l m a C S x m a C a S m C x S m a C S C I S I C Sv m C x a S m S x a m a S a dq m z C x m z a C m z C k l q q x a dq dq                    2 3 2 2 23 3 3 2 2 3 23 3 2 2 3 23 3 2 2 23 32 2 2 2 ) 2c cm a C S x m a C a S m a S a C m a S C x dq   Từ đó : i i iv Q     Chương trình viết bằng maple: > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 43 > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 44 > > > > > > > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 45 > > > > > > > > > > > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 46 > > > > > > > > > > > > > > > > > > TÍNH TOÁN THIẾT KẾ ROBOT Page 47 > Phần 6 :Luật điều khiển Tất cả các hệ thống điều khiển dưới đây dều tuân theo luật điều khiển PD . khi thiết kế hệ thống điều khiển ta bỏ qua động học của cơ cấu chấp hành, quán tính động cơ. Như vậy chức năng của bộ điều khiển là tạo ra momen cần thiết để truyền động khớp Robot đảm bảo khớp Robot luôn bám theo vị trí đặt . 1. Hệ thống điều khiển trong không gian khớp. Tín hiệu đặt đó là quỹ đạo bậc 3 của các biến khớp Hệ thống điều khiển phản hồi không bù G(q) Luật điều khiển: Hình 6.1 Sơ đồ cấu trúc điều khiển Robot với bộ điều khiển PD TÍNH TOÁN THIẾT KẾ ROBOT Page 48 Ta có phương trình động lực học : 1 2( , ........ )p p pndiag K K K Gọi H(q) =M(q) tránh nhầm với M la vector momen và V ( , )q q = ( , )C q q q , M=U tiến đến ta coi Robot không chịu tác dụng của ngoại lực vì luật điều khiển bám quỹ đạo F=0 như vậy phương trình động lực học được rút gọn như sau : M= H(q) q + V ( , )q q + G(q) (1) Luật điều khiển : ( ) ( )dk p d d d p dM K q q K q q K K       Trong đó : pK = 1 2( , ........ )p p pndiag K K K ma trận đường chéo các hệ số khuyếch đại của từng khớp riêng biệt . Với luật điều khiển này đã giả thiết thành phần mômen trọng lực G(p) đã được bù hoàn toàn . Hệ thống điều khiển với cấu trúc bộ điều khiển như trên, ổn định tuyệt đối toàn cục. Thực vậy chọn hàm Liapunov có dạng như sau : 1 ( ) 2 T T LV Kp q Hq   (2) Hàm LV biểu thị tổng năng lượng của hệ thống robot . Thành phần chứa Kp tỷ lệ với năng lượng đầu vào , thành phần sau là động năng của robot mà Kp và H là các ma trận có hệ số dương . Nên hàm LV >0 với q khác qd Tính đạo hàm cấp 1 của LV ta nhận được : 1 ( ) 2 T T T T T LV Kp Kp q Hq q Hq q Hq        Do tính chất đối xứng của các thành phần T Kp  và Tq Hq ta tính được 1 2 T T T LV Kp q Hq q Hq    TÍNH TOÁN THIẾT KẾ ROBOT Page 49 Từ phương trình động lực học với giả thiêt không có thành phần momen trọng lực G(q) , ta nhận được phương trình sau :   1 ( , ) 2 T T T LV Kp q Hq q M V q q     Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển (1) ta có : 1 1 ( , ) ( ) d + ( ) 2 2 T T T T T LV q Kd q c q q q q H q q q K q H c q       Trong đó : V ( , )q q = ( , )C q q q Do ma trận 1 2 H c là ma trận đối xứng ngược nên - 1 ( ) 2 Tq H c q =0 Suy ra LV d Tq K   (3) Từ 5.2 và 5.3 cho thấy rằng , mức độ dương của LV phụ thuộc vào Kp , mức độ âm của LV phụ thuộc vào Kd .Do đó tăng tốc độ hội tụ bằng tăng giá trị Kp . Nâng cao độ chính xác tinh của hệ thống đạt được bằng tăng hệ số Kp của khâu khuếch đại . Tuy nhiên , Kp và Kd quá lớn sẽ làm giảm độ ổn định và chất lượng quá trình quá độ như độ quá điều chỉnh , thời gian quá độ tăng . Ta đã thiết lập được hệ phương trình vi phân động lực học đó là mô hình toán học của Robot hàn điểm mà ta đang thiết lập, mô phỏng điều khiển ta sử dụng phương trình vi phân động lực học là đối tương điều khiển . Quỹ đạo đặt là quỹ đạo bậc 3 các biến khớp với điều khiển ĐIỂM-ĐIỂM . ta chọn 2 điểm A(x0,y0,z0) ,B(xc,yc, xc) bất kì trong không gian làm việc .Từ phương trình động học ngược ta tính ở phần trên ta xác định được góc khớp tại hai điểm  Code chương trình mô phòng bằng Open GL TÍNH TOÁN THIẾT KẾ ROBOT Page 50 //DrawModel.h - includes functions to render objects and supporting functions //Author: NGUYEN DINH DINH #ifndef _DRAW_MODEL_H_ #define _DRAW_MODEL_H_ #include "GL/Glut.H" #include "STLModel.h" #include "Materials.h" #include "Lights.h" #include "Trajectory.h" #include "DrawText.h" #ifndef PI #define PI 3.1415926535897932384626433832795 #endif /*PI*/ void CreateObjects(); void RenderObjects(double X, double Y, double Z, double angX, double angY, double angZ, double Scale = 1); //implement code here //parameters manage graphical objects int de, khau1, khau2, khau3; TÍNH TOÁN THIẾT KẾ ROBOT Page 51 //Parameters for model double H0, L1, L2, q1, q2, q3, d1 ,a1, a2, a3; //Trajector double xt, yt, zt, t, stept; int traj_size = 100; CTrajectory traj; void SolveInverseKinematic() { xt = 50+20*sin(t); yt = 50+15*cos(t); zt = 50 + 25*sin(3*t); if (2*6.28<t) { xt=15*16*sin(t)*sin(t)*sin(t);yt=500+15*(13*cos(t)-5*cos(2*t)- 2*cos(3*t)-cos(4*t));zt=500; } if (6.28+12.56<t) { xt=10*16*sin(t)*sin(t)*sin(t);yt=500+10*(13*cos(t)-5*cos(2*t)- 2*cos(3*t)-cos(4*t));zt=500; } if (6.28+18.84<t) { TÍNH TOÁN THIẾT KẾ ROBOT Page 52 xt=5*16*sin(t)*sin(t)*sin(t);yt=500+5*(13*cos(t)-5*cos(2*t)-2*cos(3*t)- cos(4*t));zt=500; } if (6.28+25.12<t) { xt=2.5*16*sin(t)*sin(t)*sin(t);yt=500+2.5*(13*cos(t)-5*cos(2*t)- 2*cos(3*t)-cos(4*t));zt=500; } if (6.28+31.4<t) { zt=600+60*cos(t)*sin(5*t) ;xt= 50*sin(5*t);yt= -600-60*sin(t)*sin(5*t); } q1=atan2(yt,xt); q2=acos((xt*xt+yt*yt+(zt-d1)*(zt-d1)+a2*a2- a3*a3)/(2*a2*sqrt(xt*xt+yt*yt+(zt-d1)*(zt-d1))))-atan2(d1- zt,sqrt(xt*xt+yt*yt)); q3=PI+acos((-xt*xt-yt*yt-(zt-d1)*(zt-d1)+a2*a2+a3*a3)/(2*a2*a3)); traj.add_point(xt, yt, zt); } void NextStep() { TÍNH TOÁN THIẾT KẾ ROBOT Page 53 t += stept; SolveInverseKinematic(); } void DeleteObjects() { if (glIsList(de)) ////SUA glDeleteLists(de, 1); if (glIsList(khau1)) glDeleteLists(khau1, 1); if (glIsList(khau2)) glDeleteLists(khau2, 1); if (glIsList(khau3)) glDeleteLists(khau3, 1); } void CreateObjects() { glEnable(GL_DEPTH_TEST); DeleteObjects(); ///SUA ReadModel("STL/De.STL", de); ReadModel("STL/Khau1.STL", khau1); TÍNH TOÁN THIẾT KẾ ROBOT Page 54 ReadModel("STL/Khau2.STL", khau2); ReadModel("STL/Khau3.STL", khau3); H0 = 100; a1 = 155; a2 = 500; a3=400; d1 = 130; q1 = q2 = q3=0; t = 0; stept = 0.05; traj_size = 1000; traj.set_size(traj_size); SolveInverseKinematic(); } void DrawNotes(double X, double Y, double Z, double projection_scale = 1, double line_space = 1.15) { double space = 0, s = projection_scale*line_space; void * font = GLUT_BITMAP_9_BY_15; X += glutBitmapWidth(font, 'H'); space += projection_scale*glutBitmapHeight(font); DrawText("INSTRUCTION:", X, Y-space, Z, 1, 0, 0, 1, font); TÍNH TOÁN THIẾT KẾ ROBOT Page 55 space += 0.2*projection_scale*glutBitmapHeight(font); DrawText("___________", X, Y-space, Z, 1, 0, 0, 1, font); font = GLUT_BITMAP_9_BY_15; space += s*glutBitmapHeight(font); DrawText("+ Press, Hold and move LEFT mouse button to ROTATE model", X, Y-space, Z, 1, 1, 0, 1, font); space += s*glutBitmapHeight(font); DrawText("+ Press, Hold and move MIDDLE mouse button to PAN model", X, Y-space, Z, 1, 1, 0, 1, font); space += s*glutBitmapHeight(font); DrawText("+ Press RIGHT mouse button to SHOW MENU", X, Y-space, Z, 1, 1, 0, 1, font); space += s*glutBitmapHeight(font); DrawText("+ Scroll up and down mouse button to ZOOM model", X, Y- space, Z, 1, 1, 0, 1, font); space += s*glutBitmapHeight(font); DrawText("+ Press 'r' to RUN and 's' to STOP simulation", X, Y-space, Z, 1, 1, 0, 1, font); space += s*glutBitmapHeight(font); TÍNH TOÁN THIẾT KẾ ROBOT Page 56 DrawText("Author:DINH DINH ----------- o0o ----------", X, Y-space, Z, 1, 1, 1, 1, GLUT_BITMAP_8_BY_13); } void RenderObjects(double X, double Y, double Z, double angX0, double angY0, double angZ0, double rotX, double rotY, double rotZ, double Scale) { glPushMatrix(); Light0(true); glTranslatef (X, Y, Z); glRotated(rotX, 1, 0, 0); glRotated(rotY, 0, 1, 0); glRotated(rotZ, 0, 0, 1); glRotated(angX0, 1, 0, 0); glRotated(angY0, 0, 1, 0); glRotated(angZ0, 0, 0, 1); glScaled(Scale, Scale, Scale); glPushMatrix(); //Ve de ////SUA ApplyMaterial(0); glCallList(de); TÍNH TOÁN THIẾT KẾ ROBOT Page 57 //Ve khau 1 glTranslated(0, 0, H0); glRotated(q1*180/3.14, 0, 0, 1); ApplyMaterial(5); glCallList(khau1); //Ve khau 2 glTranslated(L1, 0, 0); glRotated(q2*180/3.14, 0, 0, 1); ApplyMaterial(7); glCallList(khau2); //Ve khau 3 glTranslated(L2, 0, 0); glRotated(q3*180/3.14, 0, 0, 1); ApplyMaterial(7); glCallList(khau3); //Ve quy dao cua diem can the hien traj.draw(1, 1, 0, GL_POINTS, 1.5); glPopMatrix(); } #endif /*_DRAW_MODEL_H_*/