Đồ án cánh tay robot PUMA
Lời nói đầu Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa sản xuất có vai trò đặc biệt quan trọng . Nhằm nâng cao năng suất dây chuyền công nghệ, nâng cao chất lượng và khả năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động , nâng cao năng suất lao động…đặt ra là hệ thống sản xuất phải có tính linh hoạt cao. Robot công nghiệp là một bộ phận quan trọng để tạo ra những hệ thống tự động đó. Robot công nghiệp đã có mặt trong sản xuất từ nhiều năm trước, ngày nay robot công nghiệp được dùng ở nhiều lĩnh vực sản xuất. đó là xuất phát từ những ưu điểm mà robot đó đã được chọn và đúc kêt lại trong quá trình sản xuất,robot có những tính năng mà con người không thể có được, khả năng làm việc ổn định,làm việc trong môi trường độc hại… Do đó việc đầu tư nghiên cứu, chế tạo ra ngững loại robot phục vụ cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại và trong tương lai. Môn học ĐỒ ÁN CƠ ĐIỆN TỬ giúp chúng em bước đầu làm quen,tìm hiểu, học cách chế tạo và rất có ích cho chúng em sau này. Trong quá trình học môn ĐỒ ÁN CƠ ĐIỆN TỬ chúng em còn nhiều thiếu sót mong các thầy chỉ bảo thêm cho chúng em. Chân thành cảm ơn Nhóm 3 1 I-Xây dựng sơ đồ động học và các ma trận biểu diễn cấu trúc 1-Sơ đồ động học của PUMA. 2- Các ma trận chuyển biểu diễn cấu trúc của PUMA. − = 1000 100 0066 0066 6 6 d CS SC A − = 1000 0010 0505 0505 5 CS SC A − − = 1000 010 0404 0404 4 4 d CS SC A − = 1000 0010 3303 3303 6 3 3 3 aSCS aCSC A − = 1000 100 2022 2022 2 2 2 2 d aSCS aCSC A − − = 1000 0010 0101 0101 1 CS SC A Trong đó: để tiện theo dõi ta ký hiệu Cx =cos(x): x là các biến khớp Sx= sin(x) II-Tính Toán Động Học Thuận: 2 Bảng thông số DH: khâu i θ i α i a i d 1 1 * θ -90 0 0 2 * 2 θ 0 a2 d2 3 * 3 θ 90 a3 0 4 * 4 θ -90 0 d4 5 * 5 θ 90 0 0 6 * 6 θ 0 0 d6 − = 1000 100 0066 0066 6 6 d CS SC A − = 1000 0010 0505 0505 5 CS SC A −−− − == 1000 0066 556565 556565 . 6 6 6 5 56 4 CS dCCSSCS dSSSCCC TAT − − = 1000 010 0404 0404 4 4 d CS SC A +− +−+ −−− == 1000 556565 54546465464654 54546465464654 . 46 6 6 6 4 46 3 ddCCSSCS dSSSSCCSCSSCCCS dSCSCCSSCCSSCCC TAT 3 − = 1000 0010 3303 3303 6 3 3 3 aSCS aCSC A 6 3 36 2 . TAT = các hệ số trong ma trận: .654)4,3( ;54)3,3( ;64654)2,3( ;64654)1,3( ;33)465(36543)4,2( ;53543)3,2( ;653)64654(3)2,2( ;653)64654(3)1,2( ;53543)3,1( ;653)64654(*3)2,1( ;653_64654(2)1,1( 6 2 6 2 6 2 6 2 6 2 6 2 6 2 6 2 6 2 6 2 6 2 dSST SST CCSCST SCCCST aSddCCdSCST CCSCST SSCCSSCCST CSCSSCCCT CSSCCT SSSSSSCCCT SSSSSCCCCT = = +−= += ++−= −= −−−= +−= += +−−= −−= − = 1000 100 2022 2022 2 2 2 2 d aSCS aCSC A 6 2 26 1 . TAT = các hệ số trong 6 1 T là: 4 2654)4,3( 54)3,3( 64654)2,3( 64654)1,3( 22 ]33)465(3643[2]33)465(36543[2)4,2( )53543(2)53543(2)3,2( ]653)64654(3[3]653)64664(3[2)2,2( ]653)64654(3[2]653)64654(3[2)1,2( 22 ]33)465(36543[2]33)465(36543[2)4,1( )53543(2)53543(2)3,1( 6523)64654(23)2,1( 6523)64654(2)1,1( 6 1 6 1 6 1 6 1 6 1 6 1 6 1 6 1 6 1 6 1 6 1 6 1 ddSST SST CCSCST SCCCST aS aSddCCdCSCaCddCSdSCCST CCSCSCCSSCCST SSCCSSCCSCSSSCSSCCCST CSCSSCCCSCCSSSSCCCCST aC aSddCCdSCSSaCddCSdSCCCT CCSCSSCSSCCCT SSSCSSCCCT CSSSSCCCCT += = +−= += + ++−++++= −++= −−−++−−= +−+−−= + ++−−+++= −−+= ++−= −−= − − = 1000 0010 0101 0101 1 CS SC A 6 1 16 0 . TAT = các hệ số trong 6 0 T là: 1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6] x N C C C C C S S S S S S S C C C S= − − − + 1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6) x O C C C C S S C S S S S C S C C= − + + − − + 1[ 2( 3 4 5 3 5) 2( 3 4 5 3 5) 1 4 5 x A C C C C S S C S S C S C C S S S= + − − − 5 1[ 2[ 3 4 5 6 3( 5 6 4) 3 3] 2[ 3 4 5 6 3( 5 6 4) 3 3] 2 2] 1( 4 5 6 2) x P C C C C S d S C d d C a S S C S d C C d d S a C a S S S d d = + + + − − + + + − + 1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6) y N S C C C C S S S S S C S C S C S= − − − + + 1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6) y O S C C C S S C S S S C S C S C C= − + + + − + 1[ 2( 3 4 5 3 5) 2( 3 4 5 3 5) 1 4 5 y A S C C C S S C S S C S C C C S S= + − − + 1[ 2[ 3 4 5 6 3( 4 6 4) 3 3] 2[ 3 4 5 6 3( 5 6 4)] 2 2] 1( 4 5 6 2) Py S C C C S d S C d d C a S S C S d C d d C a C S S d d = + + + − − + + + + [ 2( 3( 4 5 6 4 6) 3 5 6) 2( 3( 4 5 6 4 6) 3 5 6)] z N S C C C C S S S S C C S C C C S S C S S= − − − + − + [ 2( 3( 4 5 6 4 6) 3 5 6) 2( 3( 4 5 6 4 6) 3 5 6)] z O S C C C S S C S S S C S C C S S C C S S= − − − + + − − − [ 2( 3 4 5 3 5) 2( 3 4 5 3 5)] z A S C C S S C C S C S C C= − + + − [ 2( 33 4 5 6 3( 5 6 4) 3 3) 2[ 3 4 5 6 3( 5 6 4) 3 3] 2 2] z P S C C S d S C d d C a C S C S d C C d d S a S a= − + + + + − + + + III. PHƯƠNG TRÌNH ĐỘNG HỌC NGƯỢC ROBOT PUMA Bài toán động học ngược là rất khó, đối với tay máy 6 khớp có ba khớp cuối đồng quy tại một điểm, ta có thể tách bài toán động học ngược thành hai bài toán đơn giản hơn là động học ngược vị trí và động học ngược hướng + Tìm vị trí giao điểm các trục cổ tay (tâm cổ tay) + Tìm hướng của cổ tay. Ta biểu diễn thành hai hệ phương trình như sau: Trong đó O và R là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ tọa độ cố định bên ngoài (world coordinate system). Ta phải giải bài toán trên đối với các ẩn 61 ,, qq . Cơ cấu 6 khớp quay có 3 khớp cuối giao nhau có phương pháp giải như sau: Vị trí của tâm cổ tay,P C xác định qua vị trí công cụ (The given tool position) và phương của Tool pointing ( 6 Z ).Do đó vị trí của tâm cổ tay phụ thuộc vào 3 biến khớp đầu tiên. The relative wrist oriention 6 3 R 6 Các biến khớp 4 5 6 , , θ θ θ xác định từ ma trận định hướng cổ tay (The arm orientation) 3 0 R và ma trận định hướng công cụ (The given tool orientation) 0 6 R . + Ma trân trạng thái (The given tool pose) 0 6 T + Solve porions của động học ngược để tìm ra 0 3 R 1 2 3 ( , , ) θ θ θ và 3 6 4 5 6 ( , , )R θ θ θ . + Định vị trí của tâm cổ tay c O có tọa độ cho trước như sau: 0 6 0 . . 0 1 C O O d R = − trong đó O và R là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ tọa độ cố định bên ngoài (world coordinate system). c P as 6 d (cột cuối cùng của 0 6 T ) – 6 d (tool offset length)* 6 Z (3 cột của ma trận 0 6 T ) + Thiết lập c P = cột cuối cùng của 0 3 R 1 2 3 ( , , ) θ θ θ để tìm ra các biến khớp 1 2 3 , , θ θ θ Tính 1 3 0 0 6 3 6 R R R − = sau khi đã thay giá trị của các biến khớp vào 1 2 3 , , θ θ θ vào 0 3 R 1 2 3 ( , , ) θ θ θ +So sánh 3 6 R và 3 6 4 5 6 ( , , )R θ θ θ để rút ra 4 5 6 , , θ θ θ Tọa độ của điểm C trong hệ tọa độ 6 R : 6 r C = 0 0 6 d t − Tọa độ của C trong hệ tọa độ 0 R : r C = E T . 6 r E = 6 6 6 . 0 0 . . 6 1 1 0 0 0 1 x p y p z p n s a p a d x x x x x n s a p a d y y y y y d a d z n s a p z z z z − + − + = − − + (*) Ma trận chuyển từ hệ tọa độ 0 → 3: 7 0 R 3= A 1. A 2. A 3 = 23 3 23 2 2 1 2 3 23 2 2 1 2 3 23 2 2 . ( . . ) . 1 23 1 1 1 . ( . . ) . 1 1 23 1 1 . . 0 23 123 0 1 0 0 C S C C C a C a C S d S S S S S a C a C C d C S a S a S d C + − − + − − − − + ⇒ Tọa độ của C trong 0 R : r C = 0 R 3 . 4 r C = 4 3 23 2 1 2 1 2 4 3 23 2 1 2 1 2 4 23 3 23 2 2 1 . . . . . . 1 23 1 . . . . . . 1 23 1 . . . C S d a C C a C C S d S S d a S C a S C C d d C a S a S d t + + − + + + − − + (**) Đối chiếu (*) và (**) ta được hệ 3 phương trình 3 ẩn : 1 23 4 3 23 2 2 1 2 6 1 23 4 3 23 2 2 1 2 6 4 23 3 23 2 2 6 1 ( . . . ) . . ( . . . ) . . . . . . p x p y p z C S d a C a C S d x a d S S d a C a C C d y a d d C a S a S z a d d + + − = − + + + = − − − = − − Nhân (2) với cos 1 θ trừ đi (1) nhân với sin 1 θ : 2 6 1 6 1 2 1 6 6 2 2 6 6 ( ) os ( )sin cos tan 2( , ) ( ) ( ) p y p x p x p y p y p x d y a d c x a d d Ar A x a d y a d y a d x a d θ θ θ = − − − ÷ ⇒ = ± − − − ÷ − + − Nhân (2) với sin 1 θ cộng với (1) nhân vói cos 1 θ và kết hợp với (3) ta có hệ 23 4 3 23 2 2 6 6 1 23 4 3 23 2 2 6 1 . . . ( . )sin ( . ) os . . . . p y p x x p z y S d a C a C y a d x a d c P C d a S a S z a d d P θ θ + + = − + − = − − = − − = Bình phương 2 vế cộng lại ta được: 2 2 2 2 2 4 3 2 4 2 3 2 3 3 2 2 2 2 2 4 2 3 2 3 3 3 2 4 2 sin 2 os 2 sin 2 os ( ) x y x y d a a d a a a c P P d a a a c P P a a d θ θ θ θ + + + + = + ⇒ + = + − + + 2 2 2 2 2 3 2 4 3 4 2 2 3 2 2 2 4 3 ( ) cos tan 2( , ) 2 x y P P a a d Arc A d a a a a d a θ + − + + ÷ ⇒ = ± + ÷ + Khai triển hệ phương trình liên kết ta được: 8 3 3 2 3 4 2 3 4 3 3 2 3 4 3 3 2 2 3 4 3 3 2 ( . . ) ( . .sin )sin ( . . ) os ( . . os )sin x y a C a S d C C d a P C d a S c a S d a c P θ θ θ θ θ + + + − = − − + + = 2 2 3 3 2 3 4 3 4 3 3 3 3 2 3 4 3 4 3 3 4 3 3 3 2 4 3 3 3 ( . . ) ( . . ) ( . . ) ( . . ) ( . ) ( . ) c x y s x y a C a S d C d a S a C a S d P C d a S P d C a S P a d S a C P ∆ = − + + + − ∆ = − + + + − ∆ = − − − + + 2 tan 2( , ) s c A θ ∆ ∆ = ∆ ∆ 2 4 3 3 3 2 4 3 3 3 2 4 3 3 3 4 3 3 3 tan 2 ( ) ( ) ,( ) ( ) x y x A d C a S P a d S a C P a d S a C P d C a S θ ⇒ = − − + + + + + − Ta có 3 6 R = 4 5 6 A A A = 4 4 4 4 4 0 0 0 0 0 1 1 0 0 0 1 C S S C d − − 5 5 5 5 0 0 0 0 0 1 0 0 0 0 0 1 C S S C − 6 6 6 6 6 0 0 0 0 0 0 1 0 0 0 1 C S S C d − = Mặt khác: 1 2 3 A A A 4 5 6 A A A = E T 0 3 3 6 . E R R T⇔ = 3 0( 1) 6 3 . E R R T − ⇒ = 1 23 1 23 23 1 23 1 23 23 1 23 1 23 23 14 1 1 1 1 1 1 24 1 23 1 23 23 1 23 1 23 23 1 23 1 23 23 34 0 0 0 1 x y z x y z x y z x y x y x y x y z x y z x y z C C n S C n S n C C s S C s S s C C a S C a S a f S n C n S s C s S a C a f C S n S S n C n C S s S S s C s C S a S S a C a f + − + − + − − + − + − + + − + − + − Chỉ cần quan tâm tới phần định hướng tức là ma trận [3,3]. So sánh các phần tử của 2 ma trận [3,3] : [ ] 3 6 3,3R ⇒ cos 5 θ = 23 1 1 23 ( ) x y z S C a S a C a+ − ⇒ 5 θ = 23 1 1 23 cos ( ) x y z Arc S C a S a C a ± + − Nếu sin 5 0 θ ≠ ⇒ 9 [ ] [ ] 3 6 4 5 1 1 1 1 4 5 3 6 4 5 23 1 1 23 23 1 1 23 4 5 1 1 23 1 1 23 4 5 5 2,3 sin *sin * * * * sin sin 1,3 os *sin ( * * ) ( * * ) os sin * * ( * * ) tan 2( , ) sin sin x y x y x y z x y z x y x y z R S a C a S a C a R c C C a S a S a C C a S a S a c S a C a C C a S a S a A θ θ θ θ θ θ θ θ θ θ θ ⇒ = − + − + ⇒ = ⇒ = + − + − ⇒ = − + + − = [ ] [ ] 3 6 6 5 1 23 1 23 23 1 23 1 23 23 6 5 3 6 6 5 1 23 1 23 23 1 23 1 23 23 6 5 23 1 1 23 23 1 1 6 5 3,2 sin *sin sin sin 3,1 os *sin os sin ( ) ( ) tan 2( , sin x y z x y z x y z x y z x y z x y R C S s S S s C s C S s S S s C s R c C S n S S n C n C S n S S n C n c S C s S s C s S C n S n C A θ θ θ θ θ θ θ θ θ θ ⇒ = + + + + ⇒ = ⇒ − = + + + + ⇒ = − + + + + = − 23 5 ) sin z n θ Vậy bài toán động học ngược cho ta kết quả như sau: 2 1 6 6 2 2 6 6 cos tan 2( , ) ( ) ( ) p x p y p y p x d Ar A x a d y a d y a d x a d θ ÷ = ± − − − ÷ − + − 2 4 3 3 3 2 4 3 3 3 2 4 3 3 3 4 3 3 3 tan 2 ( ) ( ) ,( ) ( ) x y x A d C a S P a d S a C P a d S a C P d C a S θ = − − + + + + + − 2 2 2 2 2 3 2 4 3 4 2 2 3 2 2 2 4 3 ( ) cos tan 2( , ) 2 x y P P a a d Arc A d a a a a d a θ + − + + ÷ = ± + ÷ + 1 1 23 1 1 23 4 5 5 * * ( * * ) tan 2( , ) sin sin x y x y z S a C a C C a S a S a A θ θ θ − + + − = 5 23 1 1 23 cos ( ) x y z Arc S C a S a C a θ = ± + − 23 1 1 23 23 1 1 23 6 5 5 ( ) ( ) tan 2( , ) sin sin x y z x y z S C s S s C s S C n S n C n A θ θ θ + + + + = − 10 [...]... =pp.GetX (); float py=pp.GetY (); float pz=pp.GetZ (); //joint vars are contained here float tt1,tt2,tt3,tt4,tt5,tt6; //link and joint configue float a2 =ROBOT_ A2; float a3 =ROBOT_ A3; float d1 =ROBOT_ D1; float d2 =ROBOT_ D2; float d4 =ROBOT_ D4; float d6 =ROBOT_ D6; // calculate theta 1 18 float m =px -ax*d6; float n =py -ay*d6; tt1= acosf(d2/sqrt(m*m+n*n)) -atan2(m,n); float s1=sinf(tt1); float c1=cosf(tt1);... s1 =sinf(d2r(tt1)); float s2 =sinf(d2r(tt2)); float s3 =sinf(d2r(tt3)); 21 float s4 =sinf(d2r(tt4)); float s5 =sinf(d2r(tt5)); float s6 =sinf(d2r(tt6)); float d6 =ROBOT_ D6; float d4 =ROBOT_ D4; float d2 =ROBOT_ D2; float a3 =ROBOT_ A3; float a2 =ROBOT_ A2; float px =d6*(c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*c2*c3 c1*s2*s3))) - d2*s1 + d4*(c1*c2*s3 + c1*c3*s2) + a2*c1*c2 - a3*c1*s2*s3 + a3*c1*c2*c3;...V.THIẾT KẾ MÔ HÌNH ROBOT PUMA DÙNG SOLIDWORK 1 Nhiệm vụ của robot Gắp một vật nặng khối lượng dưới 1Kg di chuyển trong miền làm việc 2 Phân tích và lựa chọn kết cấu 2.1 Lựa chọn ổ bi, ổ lăn cho cơ cấu - Do tính chất làm việc chịu tải tương đối nhẹ (tải trọng có khối lượng dưới 1kg) của Robot nên ta sẽ chọn ổ lăn cho các khâu như sau: + Khâu 1: Dùng Ổ bi... số kỹ thuật các động cơ như sau: 12 13 14 3 Thiết kế mô hình dùng phần mềm SolidWorks Bản vẽ chi tiết phần thiết kế (2D và 3D) của các khâu của robot được kèm theo ở phần phụ lục Say đây là một số hình ảnh mô hình 3D của robot 15 VI MÔ PHỎNG ĐỘNG HỌC ROBOT PUMA DÙNG VISUAL C++ VÀ OPENGL Giao diện phần mềm mô phỏng được xây dựng như sau: 16 Phần mềm được xây dựng bằng visual C++ Phần đồ họa được lập... một vật: 24 25 KẾT LUẬN Robot công nghiệp là một bộ phận rất quan trọng trong nền sản xuất hiện đại Nó góp phần thay thế sức lao động của con người, tăng năng suất lao động và tạo ra những sản phẩm ngày càng phức tạp và đáp ứng nhu cầu ngày càng cao của con người Thông qua đồ án này, chúng em đã phần nào hiểu rõ được tầm quan trọng đó Với đề tài tìm hiểu về robot Puma, một loại robot công nghiệp phổ... được lập trình bằng các thư viện OpenGL Giao diện chương trình gồm hai phần chính Khung bên phải chứa các nút lệnh điều khiển, và chọn các nhiệm vụ của Robot cần thực hiện Khung bên phải dùng để thể hiện mô hình của robot được dùng để mô phỏng Mô hình robot được vẽ bằng phần mềm SolidWork và xuất ra file cad 3D dưới dạng STL sau đó dùng Visual C++ đọc các file này vào chương trình Sau đó dùng các hàm... cơ cho Robot Do yêu cầu điều khiển chính xác vị trí điểm tác động cuối của robot, ta chọn động cơ cho các khớp quay là loại động cơ Servo 11 Đây là loại động cơ cho phép ta điều khiển chính xác cả tốc độ và vị trí của rotor Động cơ tích hợp một mạch encoder, phản hồi trạng thái vị trí của trục ra, dựa vào đó ta giảm thiểu được sai số do tải trọng bên ngoài thay đổi… Dựa vào yêu cầu làm việc của robot, ... các hàm OpenGL để vẽ các mô hình này Phần động học thuận và ngược trong chương trình được tính trong các hàm thành viên SolveInverseKinematic và SolveForwardKinematic của lớp CSerialRobot Các hàm này như sau: void CSerialRobot::SolveInverseKinematic(const CFrame& end ,float* jointVar) { CVector nn =end.GetXDir ().GetDirection (); 17 CVector ss=end.GetYDir ().GetDirection (); CVector aa=end.GetZDir ().GetDirection... =r2d(tt4); tt5 =r2d(tt5); 20 tt6 =r2d(tt6); jointVar[0] =tt1; jointVar[1] =tt2; jointVar[2] =tt3; jointVar[3] =tt4; jointVar[4] =tt5; jointVar[5] =tt6; MoveTo (tt1,tt2,tt3,tt4,tt5,tt6); } void CSerialRobot::SolveForwardKinematic (const float tt1,const float tt2,const float tt3,const float tt4, const float tt5,const float tt6,CFrame* frm) { float c1 =cosf(d2r(tt1)); float c2 =cosf(d2r(tt2)); float c3... chúng em đã phần nào hiểu rõ được tầm quan trọng đó Với đề tài tìm hiểu về robot Puma, một loại robot công nghiệp phổ biến và điển hình, nhóm đã phân tích, thiết kế và mô phỏng được hoạt động của loại robot này Các vấn đề cơ bản đã được giải quyết như tính toán động học thuận, động học ngược, thiết kế dùng phần mềm CAD, và mô phỏng bằng Visual C++ và OpenGL Bên cạnh đó, do thời gian gấp rút, vẫn còn . tt1,tt2,tt3,tt4,tt5,tt6; //link and joint configue float a2 =ROBOT_ A2; float a3 =ROBOT_ A3; float d1 =ROBOT_ D1; float d2 =ROBOT_ D2; float d4 =ROBOT_ D4; float d6 =ROBOT_ D6; // calculate theta 1 18 float m =px -ax*d6; float. thiết kế (2D và 3D) của các khâu của robot được kèm theo ở phần phụ lục. Say đây là một số hình ảnh mô hình 3D của robot. 15 VI. MÔ PHỎNG ĐỘNG HỌC ROBOT PUMA DÙNG VISUAL C++ VÀ OPENGL Giao diện. học ngược vị trí và động học ngược hướng + Tìm vị trí giao điểm các trục cổ tay (tâm cổ tay) + Tìm hướng của cổ tay. Ta biểu diễn thành hai hệ phương trình như sau: Trong đó O và R là hướng