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.
Lời nói đầu Trong nghiệp cơng nghiệp hóa, đạ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 suất dây chuyền công nghệ, nâng cao chất lượng khả cạnh tranh sản phẩm, cải thiện điều kiện lao động , nâng cao suất lao động…đặt hệ thống sản xuất phải có tính linh hoạt cao Robot cơng nghiệp phận quan trọng để tạo hệ thống tự động Robot cơng nghiệp có mặt sản xuất từ nhiều năm trước, ngày robot công nghiệp dùng nhiều lĩnh vực sản xuất xuất phát từ ưu điểm mà robot chọn đúc kêt lại trình sản xuất,robot có tính mà người khơng thể có được, khả làm việc ổn định,làm việc mơi trường độc hại… Do việc đầu tư nghiên cứu, chế tạo ngững loại robot phục vụ cho cơng tự động hóa sản xuất cần thiết cho 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 có ích cho chúng em sau Trong q trình học mơn ĐỒ ÁN CƠ ĐIỆN TỬ chúng em cịn nhiều thiếu sót mong thầy bảo thêm cho chúng em Chân thành cảm ơn Nhóm sinh viên : Nguyễn Đình Quảng Nguyễn Đăng Thi Nguyễn Văn Thảo Ngơ Văn Thắng I-Xây dựng sơ đồ động học ma trận biểu diễn cấu trúc 1-Sơ đồ động học PUMA 2- Các ma trận chuyển biểu diễn cấu trúc PUMA C S S C6 A6 0 0 C S3 A3 0 0 0 0 d6 1 S C 3a3 C S 3a3 06 0 C S5 A5 0 0 S5 C5 0 C S S2 C2 A2 0 0 0 0 0 1 C 2a2 S 2a2 d2 C S S4 C A4 1 d4 0 1 0 C1 S1 S1 C1 A1 0 1 0 0 0 0 1 Trong đó: để tiện theo dõi ta ký hiệu Cx =cos(x): x biến khớp Sx= sin(x) II-Tính Tốn Động Học Thuận: Bảng thông số DH: khâu i * * * 4* 5* 6* C S S C6 A6 0 0 C S5 A5 0 0 i di -90 90 -90 90 0 a2 a3 0 0 d2 d4 d6 0 0 d6 1 S5 C5 0 0 0 0 1 S 5d C 5C C 5S S S 5C S 5S C C 5d 6 T6 A5 T6 S6 C6 0 0 C S S4 C A4 1 d4 0 1 0 C 4C 5C S S C 4C 5S S 4C C S C S 5d S 4C 5C C S S 4C 5S C 4C S S S 4S 5d T6 A4 T6 S 5C S 5S C C 5d d 0 C S3 A3 0 0 S C 3a3 C S 3a3 06 0 T6 A3 T6 hệ số ma trận: T6 (1,1) C 2(C 4C 5C S S _ S 3S 5S 6; T6 (1,2) C * ( C 4C 5S S 4S 6) S 3S 5S 6; T6 (1,3) C 3C S S 3C 5; T6 (2,1) 3(C 4C 5C S S 6) C 3S 5C 6; T6 (2,2) S 3( C 4C 5S S 4C 6) C 3S 5S 6; T6 (2,3) S 3C S C 3C 5; T6 (2,4) S 3C S 5d C 3(C 5d d 4) S 3a3; T6 (3,1) S 4C 5C C S 6; T6 (3,2) S 4C 5S C 4C 6; T6 (3,3) S S 5; T6 (3,4) S S 5d C S S2 C2 A2 0 0 C 2a2 S 2a2 d2 T6 A2 T6 hệ số 1T6 là: T6 (1,1) C 2(C 4C 5C S S 6) S 23S 5C T6 (1,2) C 23(C 4C 5S S 4C 6) S 23S 5S T6 (1,3) C 2(C 3C S S 3C 5) S 2( S 3C S C 3C 5) T6 (1,4) C 2[C 3C S 5d S 3(C 5d d 4) C 3a3] S 2[ S 3C S 5d C 3(C 5d d 4) S 3a3] C 2a T6 (2,1) S 2[C 3(C 4C 5C S S 6) S 3S 5C 6] C 2[ S 3(C 4C 5C S S 6) C 3S 5C 6] T6 (2,2) S 2[C 3( C 4C S S 4C 6) S 3S 5S 6] C 3[ S 3( C 4C 5S S 4C 6) C 3S 5S 6] T6 (2,3) S 2(C 3C 4S S 3C 5) C 2( S 3C S C 3C 5) T6 (2,4) S 2[C 3C 4S 5d S 3(C 5d d 4) C 3a3] C 2[ S 3C 4d C 3(C 5d d 4) S 3a3] S 2a T6 (3,1) S 4C 5C C S T6 (3,2) S 4C 5S C 4C T6 (3,3) S S T6 (3,4) S S 5d d C1 S1 S1 C1 A1 0 1 0 0 0 0 0 1 T6 A1.1T6 hệ số T6 là: N x C1[C 23(C 4C 5C S 4S 6) S 23S 5S 6] S1( S 4C 5C C 4S 6] Ox C1[C 23(C 4C 5S S 4C 6) S 23S 5S 6] S1(4C 5S C 4C 6) Ax C1[C 2(C 3C 4S S 3C 5) S 2( S 3C 4S C 3C 5) S1S S 5 Px C1[C 2[C 3C S 5d S 3(C 5d d 4) C 3a3] S 2[ S 3C S 5d C 3(C 5d d 4) S 3a3] C a 2] S1( S S 5d d 2) N y S1[ C 23(C 4C 5C S 4S 6) S 23S 5S 6] C1( S 4C 5S C S 6) Oy S1[C 23(C 4C 5S S 4C 6) S 23S 5S 6] C1( S 4C 5S C 4C 6) Ay S1[C 2(C 3C 4S S 3C 5) S 2( S 3C 4S C 3C 5) C1S 4S Py S1[C 2[C 3C 4S 5d S 3(C 4d d 4) C 3a3] S 2[ S 3C 4S 5d 3(C 5d d 4)] C 2a 2] C1( S S 5d d 2) N z [ S 2(C 3(C 4C 5C S 4S 6) S 3S 5C 6) C 2( S 3(C 4C 5C S 4S 6) C 3S 5S 6)] Oz [ S 2(C 3(C 4C 5S S 4C 6) S 3S 5S 6) C 2( S 3(C 4C 5S S 4C 6) C3S 5S 6)] Az [ S 2(C 3C S S 3C 5) C 2( S 3C S C 3C 5)] Pz [ S 2(C 33C S 5d S 3(C 5d d 4) C 3a3) C 2[ S 3C 4S 5d C 3(C 5d d 4) S 3a3] S 2a 2] III PHƯƠNG TRÌNH ĐỘNG HỌC NGƯỢC ROBOT PUMA Bài tốn động học ngược khó, tay máy khớp có ba khớp cuối đồng quy điểm, ta tách tốn động học ngược thành hai toán đơn giản động học ngược vị trí động học ngược hướng + Tìm vị trí giao điểm trục cổ tay (tâm cổ tay) + Tìm hướng cổ tay Ta biểu diễn thành hai hệ phương trình sau: Trong O R hướng vị trí dụng cụ, biểu diễn hệ tọa độ cố định bên (world coordinate system) Ta phải giải toán ẩn q1 , , q6 Cơ cấu khớp quay có khớp cuối giao có phương pháp giải sau: Vị trí tâm cổ tay,P C xác định qua vị trí cơng cụ (The given tool position) phương Tool pointing ( Z ).Do vị trí tâm cổ tay phụ thuộc vào biến khớp The relative wrist oriention R36 Các biến khớp ,5 , xác định từ ma trận định hướng cổ tay (The arm orientation) R03 ma trận định hướng công cụ (The given tool orientation) R60 + Ma trân trạng thái (The given tool pose) T60 + Solve porions động học ngược để tìm R30 (1 , ,3 ) R63 ( ,5 , ) + Định vị trí tâm cổ tay Oc có tọa độ cho trước sau: �� OC O d R �� O R hướng vị trí dụng cụ, biểu �� �� �� diễn hệ tọa độ cố định bên (world coordinate system) Pc as d (cột cuối T60 ) – d (tool offset length)* Z (3 cột ma trận T60 ) + Thiết lập Pc = cột cuối R30 (1 , ,3 ) để tìm biến khớp 1 , , 3 Tính R63 R30 R60 sau thay giá trị biến khớp vào 1 , ,3 vào 1 R30 (1 , , 3 ) +So sánh R63 R63 ( , , ) để rút ,5 , Tọa độ điểm C hệ tọa độ R6 : rC = �0 � � � �0 � � d � � 6� �t � � � Tọa độ C hệ tọa độ R0 : � n �x � ny rC = TE 6rE = � � n �z � �0 sx ax sy ay sz az 0 p x ��0 � �� � p y ��0 � �� � d6 � p z �� �� � 1 � �� � � a d x � � x p� � a y d y p � � �(*) az d6 z p � � � � � � Ma trận chuyển từ hệ tọa độ � 3: � CC �1 23 �S R3= A1.A2.A3 = � � S � 23 � �0 S1 C1 S23 C1 ( a3 C23 a2 C2 ) S1 d � � C1 S1 S23 S1 ( a3 C23 a2 C2 ) C1 d � � a3 S23 a2 S2 d1 � C23 � � � � Tọa độ C R0 : � C S d a C C a C C S d � �1 23 23 2 � �S S d a S C a S C C d � rC =0R3.4rC = �1 23 23 2 �(**) � � d C23 a3 S23 a2 S2 d1 � � t � � Đối chiếu (*) (**) ta hệ phương trình ẩn : C1 ( S 23 d a3 C23 a2 C2 ) S1.d x p ax d S1 ( S 23 d a3 C23 a2 C2 ) C1.d y p a y d d C23 a3 S23 a2 S z p az d d1 Nhân (2) với cos 1 trừ (1) nhân với sin 1 : d ( y p a y d )cos1 ( x p ax d )sin1 � d2 � 1 �Ar cos � � ( y p a y d ) ( x p ax d ) � � � A tan 2( x p a x d , y p a y d ) � � Nhân (2) với sin 1 cộng với (1) nhân vói cos 1 kết hợp với (3) ta có hệ S23 d a3 C23 a2 C2 ( y p a y d ) sin ( x p ax d )cos1 Px C23 d a3 S23 a2 S2 z p az d d1 Py Bình phương vế cộng lại ta được: d 42 a32 a22 2d a2 sin 3 2a2 a3cos Px2 Py2 � 2d a2 sin 2a2 a3cos3 Px2 Py2 (a32 a22 d 42 ) �Px2 Py2 (a32 a22 d 42 ) � � A tan 2(d a2 , a2 a3 ) � 3 �Arc cos � � a d a2 � � � Khai triển hệ phương trình liên kết ta được: (a3 C3 a2 S3 d4 )C2 (C3 d4 a3 sin 3 )sin 2 Px (C3 d a3 S3 )cos2 (a2 S3 d4 a3 cos3 ) sin 2 Py � (a3 C3 a2 S3 d ) (C3 d a3 S3 ) � � � c � (a3 C3 a2 S3 d ) Px (C3 d a3 S3 ) Py � � � s � (d C3 a3 S3 ) Px (a2 d S3 a3 C3 ) Py � � � A tan 2( s c , ) � A tan � (d 4C3 a3 S3 ) Px (a2 d S3 a3C3 ) Py , (a2 d S3 a3C3 ) Px (d 4C3 a3S ) � � � C4 � � S4 Ta có R63 = A4 A5 A6 = � �0 � �0 S4 C4 1 0 �� C5 � � �� S5 d ��0 �� ��0 S5 C5 0 0 �� C6 � � �� S6 ��0 �� ��0 S6 C6 0 0� 0� � d6 � � 1� = Mặt khác: A1 A2 A3 A4 A5 A6 = TE � R30 R63 TE � R63 R30( 1) TE C1C23nx S1C23 n y S 23 nz � � S1nx C1n y � � C1S 23nx S1S 23 n y C23nz � � C1C23 s x S1C23 s y S 23 s z S1sx C1s y C1S 23 sx S1S 23 s y C23 s z C1C23a x S1C23a y S 23a z S1a x C1a y C1S 23a x S1S 23a y C23a z f14 � f 24 � � f 34 � � � Chỉ cần quan tâm tới phần định hướng tức ma trận [3,3] So sánh phần tử ma trận [3,3] : R63 3,3 � cos 5 S 23 (C1ax S1a y ) C23a z � 5 �Arc cos � S 23 (C1a x S1a y ) C23a z � � � Nếu sin 5 �0 � R63 2,3 � sin *sin S1 * ax C1 * a y � sin S1 * ax C1 * a y sin 5 R63 1,3 � cos *sin 5 C23 (C1 * ax S1 * a y ) S23 az � cos C23 (C1 * ax S1 * a y ) S 23az sin 5 A tan 2( S1 * a x C1 * a y C23 (C1 * a x S1 * a y ) S23a z , ) sin 5 sin 5 R63 3, 2 � sin 6 *sin 5 C1S23 sx S1S23 s y C23 sz � sin C1S 23 sx S1S23 s y C23 s z sin 5 R63 3,1 � cos6 *sin 5 C1 S23 nx S1S 23n y C23 nz � cos A tan 2( C1S23nx S1S23n y C23nz sin 5 S23 (C1sx S1s y ) C23 sz sin 5 , S23 (C1nx S1n y ) C23nz sin 5 ) Vậy toán động học ngược cho ta kết sau: � d2 1 �Ar cos � � ( y p a y d )2 ( x p ax d ) � � � A tan 2( x p a x d , y p a y d ) � � A tan � (d 4C3 a3 S3 ) Px ( a2 d S3 a3C3 ) Py ,(a2 d S3 a3C3 ) Px (d 4C3 a3 S ) � � � �P Py2 (a32 a22 d 42 ) � � 3 �Arc cos � x A tan 2(d a2 , a2 a3 ) � a d a2 � � � A tan 2( S1 * ax C1 * a y C23 (C1 * ax S1 * a y ) S 23a z , ) sin 5 sin 5 5 �Arc cos � S 23 (C1a x S1a y ) C23a z � � � A tan 2( S23 (C1sx S1s y ) C23 s z sin 5 , S23 (C1nx S1n y ) C23nz sin ) 10 13 14 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 3D) khâu robot kèm theo phần phụ lục Say số hình ảnh mơ hình 3D 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ô xây dựng sau: 16 Phần mềm xây dựng visual C++ Phần đồ họa lập trình thư viện OpenGL Giao diện chương trình gồm hai phần Khung bên phải chứa nút lệnh điều khiển, chọn nhiệm vụ Robot cần thực Khung bên phải dùng để thể mơ hình robot dùng để mơ Mơ hình robot vẽ phần mềm SolidWork xuất file cad 3D dạng STL sau dùng Visual C++ đọc file vào chương trình Sau dùng hàm OpenGL để vẽ mơ hình Phần động học thuận ngược chương trình tính hàm thành viên SolveInverseKinematic SolveForwardKinematic lớp CSerialRobot Các hàm 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 (); //CVector aa =CrossProduct (nn,ss); CPoint3D pp =end.GetOrigin (); float nx=nn.GetX (); float ny=nn.GetY (); float nz=nn.GetZ (); float sx =ss.GetX (); float sy =ss.GetY (); float sz =ss.GetZ (); float ax =aa.GetX (); float ay =aa.GetY (); float az =aa.GetZ (); float px =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 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); float K1 = n*s1 + m * c1; float K2 = pz -az*d6 -d1; //calulate theta float p=K1*K1+K2*K2 -(a3*a3+a2*a2 +d4*d4); float q=2*a2*sqrt(d4*d4+a3*a3); tt3= acosf(p/q) + atan2(d4,a3); //check the criteria float c3=cosf(tt3); float s3=sinf(tt3); //calculate theta p=a3*c3 +a2 +s3 *d4; q=d4*c3-a3*s3; float delta =p*p +q*q; float deltac =p*K1 +q*K2; float deltas =q*K1 - p*K2; tt2= atan2(deltas,deltac); float s2=sinf(tt2); float c2=cosf(tt2); //calculate theta 5; 19 float s23 =sinf(tt2+tt3); float c23 =cosf(tt2+tt3); m=c1*ax+s1*ay; tt5 = acosf(s23*m- c23*az); float c5= cosf(tt5); float s5= sinf(tt5); //calculate theta4 m=c1 *ax +s1*ay; n=-s1*ax +c1*ay; if(tt5!=0) { tt4 =atan2(n/s5, (c23*m - s23*az)/s5); } else { tt4=0; } //calculate theta tt6 =atan2((s23*(c1*sx +s1*sy)+c23*sz)/s5, -(s23*(c1*nx +s1*ny)+c23*nz)/s5); tt1=r2d(tt1); tt2 =r2d(tt2); tt3 =r2d(tt3); tt4 =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 =cosf(d2r(tt3)); float c4 =cosf(d2r(tt4)); float c5 =cosf(d2r(tt5)); float c6 =cosf(d2r(tt6)); //// float 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; float py =c1*d2 + d6*(c5*(c2*s1*s3 + c3*s1*s2) + s5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))) + d4*(c2*s1*s3 + c3*s1*s2) + a2*c2*s1 - a3*s1*s2*s3 + a3*c2*c3*s1; float pz =d4*(c2*c3 - s2*s3) - a2*s2 + d6*(c5*(c2*c3 - s2*s3) c4*s5*(c2*s3 + c3*s2)) - a3*c2*s3 - a3*c3*s2; frm->SetOrigin (CPoint3D(px,py,pz)); } Một số kết mô phỏng: - Điểm tác động cuối chạy theo đường xoắn ốc: 22 - Biên dạng chạy theo đường helixcal: 23 - Mô gắp vật: 24 25 KẾT LUẬN Robot công nghiệp phận quan trọng sản xuất đại Nó góp phần thay sức lao động người, tăng suất lao động tạo sản phẩm ngày phức tạp đáp ứng nhu cầu ngày cao người Thông qua đồ án này, chúng em phần hiểu rõ tầm quan trọng Với đề tài tìm hiểu robot Puma, loại robot cơng nghiệp phổ biến điển hình, nhóm phân tích, thiết kế mô hoạt động loại robot Các vấn đề giải tính tốn động học thuận, động học ngược, thiết kế dùng phần mềm CAD, mô Visual C++ OpenGL Bên cạnh đó, thời gian gấp rút, vấn đề khác cần tiếp tục giải Chúng em xin ý kiến đóng góp thầy để đề tài hoàn thiện Một lần nhóm xin chân thành cảm ơn hướng dẫn tận tình thầy Nguyễn Tuấn Khoa thầy Đặng Bảo Lâm thời gian qua 26 PHỤ LỤC 27 ... 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 18 float m =px -ax*d6;... 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... SolidWorks Bản vẽ chi tiết phần thiết kế (2D 3D) khâu robot kèm theo phần phụ lục Say số hình ảnh mơ hình 3D robot 15 VI MÔ PHỎNG ĐỘNG HỌC ROBOT PUMA DÙNG VISUAL C++ VÀ OPENGL Giao diện phần mềm