Bài tập lớn Mô phỏng hệ cơ điện tử

MỤC LỤC THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC: Họ và tên MSSV Phân công Đánh giá. Phạm Thanh Tú 20156794 Động học (thuận ngược code maple, matlab) + động lực học (tổng quát+ngược) + thiết kế quỹ đạo 10/10 Tích cực Nguyễn Trọng Huy 20155706 3d + xuất stl + hỗ trợ mô phỏng Simulink (sửa các lỗi xuất, trục, body), open GL 10/10 Tích cực Nguyễn Tiến Lợi 20155993 Động học (thuận, code maple) + động lực học( hỗ trợ sửa lỗi, sửa code) 10/10 Tích cực Bùi Văn Trường 3d+Mô phỏng Open GL+ PID+ động lực học ( tham gia hỗ trợ giải, sửa lỗi) 10/10 Tích cực Nguyễn Mạnh Thắng 20156502 3d+Mô phỏng Simulink +PID +động lực học (viết khối simulink + thuận+ngược) 10/10 Tích cực Đỗ văn Đức 20155389 PID + Hỗ trợ mô phỏng Simulink + hỗ trợ sửa lỗi code. 10/10 Tích cực NOTE: - Các kết quả tính toán quá dài sẽ được trình bày trong phụ lục. TIẾN ĐỘ HOÀN THÀNH VÀ CHƯA HOÀN THÀNH STT Mục Công việc Hoàn thành 1 Động học Thuận Quy luật quỹ đạo điểm cuối Yes Mô phỏng số (Đặc biệt và bất kì) Yes Ngược Mô phỏng số đường thẳng Yes Mô phỏng số tròn Yes Kiểm tra bài toán ở mức vận tốc Yes 2 Thiết kế quỹ đạo Đơn giản Quỹ đạo tròn thẳng theo thời gian Yes 5 điểm Thiết kế ra quy luật khớp, vận tốc Yes 3 Động lực Tổng quát Tính các ma trận khối lượng, ma trận C, các phương trình vi phân Yes Ngược Tính được các mô men đối vơi dạng quỹ đạo thẳng, tròn Yes Thuận Sử dụng kết quả tính ngược tính ngược lại các biến khớp Yes 4 PID Thiết kế Thiết kế xong PID Yes Chọn hệ số Từ bài toán động lực học thuận ko có PID so sánh độ lệch đồ thị (tính toán, thực) tìm các hệ số. Yes Mô phỏng Mô phỏng PID trên simulink Yes 5 Mô phỏng Open GL Chạy thành công quỹ đạo thẳng tròn Yes Mô phỏng trên Matlab Chạy thành công quỹ đạo thẳng tròn trên matlab. Mô hình có gắn PID và sử dụng kết quả tính toán động lực học. Yes 6 Báo cáo Words Hoàn thành báo cáo chi tiết Yes Power point Hoàn thành báo cáo No DANH MỤC HÌNH ẢNH Hình 1 Mô hình Robot và hệ trục Dh tương ứng 8 Hình 2 Minh họa về phương pháp đặt hệ trục D-H: 9 Hình 3 Khâu đế 13 Hình 4 Cách đặt Hệ trục khâu đế 13 Hình 5 Khâu 1 14 Hình 6 Cách đặt hệ trục khâu 1 14 Hình 7 Khâu 2 15 Hình 8 Cách đặt hệ trục khâu tịnh tiến ( khâu 2) 15 Hình 9 Khâu 3 16 Hình 10 Cách đặt hệ trục khâu quay (Khâu 3) 16 Hình 11 Khâu 4 17 Hình 12 Cách đặt hệ trục khâu quay (khâu 4) 17 Hình 13 Miền làm việc theo phương X-Y 25 Hình 14 Miền làm việc trong không gian Y-Z 26 Hình 15 Miền làm việc trong không gian X-Z 26 Hình 16 Miền làm việc vẽ trong không gian 3D 27 Hình 17 Quỹ đạo điểm cuối bài toán thuận kiểm tra 29 Hình 18 Quỹ đạo mô phỏng thuận 30 Hình 19 Đồ thị vận tốc, gia tốc điểm E theo phương x 31 Hình 20 Đồ thị vận tốc, gia tốc điểm E theo phương y 31 Hình 21 Đồ thị vận tốc gia tốc điểm E theo phương z 31 Hình 22 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo x. 32 Hình 23 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo y 33 Hình 24 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo z. 33 Hình 25 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo x. 34 Hình 26 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo y 34 Hình 27 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo z 35 Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x 35 Hình 29 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo y 36 Hình 30 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo z 36 Hình 31 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x 37 Hình 32 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo y 37 Hình 33 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo z 38 Hình 34 Vận tốc gia tốc góc khâu 1 theo x,y,z 39 Hình 35 Vận tốc gia tốc góc khâu 2 theo x,y,z 40 Hình 36 Vận tốc gia tốc góc khâu 1 theo x,y,z 41 Hình 37 Vận tốc gia tốc góc khâu 1 theo x,y,z 42 Hình 38 Quỹ đại điểm cuối theo quy luật cho trước 44 Hình 39 Đồ thị kiểm tra 45 Hình 40 Vẽ hai đồ thị trên cùng một miền 45 Hình 41 Đồ thị quy luật cho trước 46 Hình 42 Đồ thị kiểm tra 47 Hình 43 Vẽ hai đồ thị trên cùng một miền 47 Hình 44 Đồ thị q1 48 Hình 45 Đồ thị q2 49 Hình 46 Đồ thị q3 50 Hình 47 Đồ thị q4 51 Hình 48 Đồ thị vận tốc thực và vận tốc kiểm tra phương x 52 Hình 49 Đồ thị vận tốc thực và vận tốc kiểm tra phương y 53 Hình 50 Đồ thị vận tốc thực và vận tốc kiểm tra phương z 53 Hình 51 Quỹ đạo đoạn 1 59 Hình 52 Quỹ đạo đoạn 2 60 Hình 53 Quỹ đạo đoạn 3 60 Hình 54 Quỹ đạo đoạn 4 61 Hình 55 Quỹ đọa đi qua 5 điểm 61 Hình 56 Đồ thị vận tốc theo phương x 63 Hình 57 Đồ thị vận tốc theo phương y 64 Hình 58 Đồ thị vận tốc theo phương z 65 Hình 59 Mô phỏng quỹ đạo tròn 79 Hình 60 Mô hình động học Robotác thông số đầu vào: 81 Hình 61 Mô tả sumlink giải động học ngược 85 Hình 62 Đồ thị MoMen khâu 1 85 Hình 63 Đồ thị Lực khâu 2 86 Hình 64 Đồ thị Momen khâu 3 86 Hình 65 Đồ thị Momen khâu 4 87 Hình 66 Đồ thị Momen khớp 1 87 Hình 67 Đồ thị Lực khớp 2 88 Hình 68 Đồ thị Momen khớp 3 88 Hình 69 Đồ thị Momen khớp 4 89 Hình 70 Mô tả simulink giải động học thuận 90 Hình 71 Đồ thị tính toán và so sánh q1 91 Hình 72 Đồ thị tính toán và so sánh q2 91 Hình 73 Đồ thị tính toán và so sánh q3 92 Hình 74 Đồ thị tính toán và so sánh q4 92 Hình 75 Mô hình điều khiển tổng quát 93 Hình 76 Mô hình điều khiển PID thiết kế Thiết kế: 93 Hình 77 Toàn bộ sơ đồ PID 95 Hình 78 Tín hiệu đặt 96 Hình 79 Khối tính Mô men, Lực 96 Hình 80 Khối tính các biến khớp thực tế theo lực, mô men điều khiển 97 Hình 81 Khối so sánh lực thực tế và tính toán 97 Hình 82 Khối PID 97 Hình 83 quỹ đạo điểm cuối bài toán thuận theo quỹ đạo tùy ý bên trên. 102 Hình 84 Đồ thị vận tốc gia tốc điểm cuối 103 Hình 85 Vận tốc gia tốc góc khâu 1 theo x,y,z 104 Hình 86 Vận tốc gia tốc góc khâu 2 theo x,y,z 105 Hình 87 Vận tốc gia tốc góc khâu 1 theo x,y,z 106 Hình 88 Vận tốc gia tốc góc khâu 1 theo x,y,z 107 Hình 89 Kiểm tra quỹ đạo thẳng 108 Hình 90 Kiểm tra quỹ đạo tròn 109 Hình 91 Kiểm tra khớp 1 110 Hình 92 Kiểm tra khớp 2 110 Hình 93 Kiểm tra khớp 3 111 Hình 94 Kiểm tra khớp 4 111 Hình 95 kiểm tra vận tốc phương x 112 Hình 96 kiểm tra vận tốc phương y 112 Hình 97 Kiểm tra vận tốc phương z 113 Hình 98 Đồ thị quỹ đạo và các điểm đã chọn 115 CHƯƠNG 1: PHÂN TÍCH, THIẾT KẾ CẤU TRÚC 3D 1.1 Phân tích cấu trúc. Hình 1 Mô hình Robot và hệ trục Dh tương ứng Sử dụng phương pháp DH : Theo DH tại mỗi khớp của robot đặt một hệ chục tọa độ quy ước về việc đặt hệ trục tọa độ này như sau: -Trục zi được đặt tại khớp i+1, chiều của trục zi tùy ý. -Trục xi được xác định là đường vuông góc chung giữa khớp i và khớp i+1, nếu hai khớp i và i+1 song song xi có thể là bất cứ đường vuông góc chung nào. Nếu hai trục i và i+1 cắt nhau xi được xác định dựa vào zi và zi+1 theo quy tắc bàn tay phải. -Trục yi xác định dựa vào xi và zi theo quy tắc bàn tay phải. Hình 2 Minh họa về phương pháp đặt hệ trục D-H: - Lập ma trận Denavit – Hartenberg Các thông số động học Denavit – Hartenberg được xác định như sau: : khoảng cách Oi-1 và Oi theo trục zi-1. : góc giữa 2 đường vuông góc chung. Là góc quay quanh trục zi-1 để trục xi-1 chuyển đến trục xi theo qui tắc bàn tay phải. : góc xoay đưa trục zi-1về zi quanh zi theo quy tắc bàn tay phải. : khoảng dịch chuyển giữa 2 trục khớp động kề nhau. Lặp được bảng DH sau: Các biến khớp:q=θ1 , θ2 , d3 , θ3=q1 , q2 , q3 , q4 Các thông số động học thiết kế: a1=,a2=400, a3=400 (mm) d1=600 (mm) Khâu Θ d a α 1 q1 d1 a1 0 2 0 q2 0 900 3 q3 0 a3 0 4 q4 0 a4 0 1.2 Thiết kế 3D Robot trên Solidworks. 1.2.1 Thiết kế từng khâu và đặt hệ trục. Cách đặt hệ trục mong muốn trong solidword: 1. Chọn Move/copy..., bôi đen toàn bộ khâu. 2. Chọn tọa độ điểm đầu và điểm cần đặt hệ trục: 3. Chọn để hiển thị hệ trục: - Hình ảnh thiết kế 3D ( từng khâu): Hình 3 Khâu đế Hệ trục đặt tại khâu 0 khớp 1: O0x0y0z0 Hình 4 Cách đặt Hệ trục khâu đế Hình 5 Khâu 1 Hệ trục đặt tại khớp 2 khâu 1: O1x1y1z1 Hình 6 Cách đặt hệ trục khâu 1 Hình 7 Khâu 2 Hệ trục đặt tại khớp 3 khâu 2: O2x2y2z2 Hình 8 Cách đặt hệ trục khâu tịnh tiến ( khâu 2) Hình 9 Khâu 3 Hệ trục đặt tại khớp 4 khâu 3 : O3x3y3z3 Hình 10 Cách đặt hệ trục khâu quay (Khâu 3) Hình 11 Khâu 4 Hệ trục đặt tại điểm lắp dụng cụ khâu 4: O4x4y4z4 Hình 12 Cách đặt hệ trục khâu quay (khâu 4) 1.2.2 Lắm ghép và xuất file. - Hình ảnh lắm ghép ( từng khâu) - Các bước xuất ra file dùng để mô phỏng: CHƯƠNG 2: PHẦN ĐỘNG HỌC 2.1 Động học thuận. Mục tiêu: - Khảo sát chuyển động khâu cuối phụ thuộc vào quy luật biến khớp cho trước và tìm miền làm việc của Robot. Ứng dụng trong thực tế: Từ các quan hện giữa quỹ đạo điểm cuối và biến khớp, giúp thiết lập miền không gian làm việc phù hợp, thiết kế chiều dài các khâu phù hợp với yêu cầu làm việc. Tình các phương trình động học cần thiết. Từ bảng D-H ta lập được ma trận biến đổi thuần nhất Aii-1từ hệ i-1 sang hệ i. Tính ma trận chuyển từ hệ 0 sang hệ i bằng công thức: T40=T10.T21.T32.T43 Ma trận biến đổi tọa độ thuần nhất Denavit – Hartenberg: Tii-1=cosθ -sinθ.cosα sinθsinα a.cosθsinθ cosθcosα -cosθsinα a.sinθ0 sinα cosα d0 0 0 1 (2.1) Ta quy ước như sau : cos(rotx) = cx sin(rotx) = sx cos(roty) = cy sin(roty) = sy cos(rotz) = cz sin(rotz) = sz C = cycz-cysz syxEsxsycz+cxsz-cxsycz+sxsz-sxsysz+cxczcxsysz+sxcz-sxcycxcyyEzE0 0 01 (2.2) Từ bảng D-H lập được thông qua tính toán maple ta tính được ma trận thuần nhất các khâu: Quy ước: sinq1=s1, sinq2=s2, sinq4=s4 cosq1=c1, cosq2=c2, cosq4=c4 Khâu 0 sang khâu 1: T10=c1 -s1 0a1*c1s1 c1 0a1*s100 0 1 0 0d11 (2.3) Khâu 1 sang khâu 2: T21=1 0 000 0 -1000 1 00 0q21 (2.4) Khâu 2 sang khâu 3: T32=c3 -s3 0a3*c3s3 c3 0a3*s300 0 1 0 001 (2.5) Khâu 3 sang khâu 4: T43=c4 -s4 0a4*c4s4 c4 0a4*s400 0 1 0 001 (2.6) Ma trận chuyển từ khâu 0 sang khâu 4 được tính theo công thức: A40=A10*A21*A32*A43 (2.7) Note: Kết quả chi tiết ở phụ lục Lập các phương trình động học: f1=T401,4-C1,4 f2=T402,4-C2,4 f3=T403,4-C3,4 f4=T403,3-C3,3 f5=T401,3-C1,3 f6=T401,2-C1,2 f7=T401,1-C1,1 f8=T402,1-C2,1 f9=T402,2-c2,2 f10=T402,3-C2,3 f11=T403,1-C3,1 f12=T403,2-C3,2 Note: Kết quả chi tiết ở phụ lục Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc. Ta giải quỹ đạo điểm cuối bằng cách chọn giải 3 trong tổng số 12 phương trình lập được ở mục 2.1.1 f1=T401,4-C1,4 f2=T402,4-C2,4 f3=T403,4-C3,4 Giải được tọa độ điểm cuối như sau: xE=cosq1.(a4*cosq4+a3*cosq3-sin(q4)*sin⁡(q3)+a1) yE=sinq1.a4*cosq4*cosq3-a4*sinq4*sinq3+a3*cos⁡(q3+a1) zE=(a4*cosq4+a3*sinq3-sin(q4)*cos⁡(q3)+q2+d1 Vận tốc điểm cuối: Vận tốc điểm E: ve=qE=xEyEzE Gia tốc điểm cuối: Gia tốc điểm E: aE=vE=vxEvyEvzE Vận tốc gia tốc khối tâm khâu: Tọa độ khối tâm khâu trong hệ tọa độ gắn với khâu: (các thông số để dạng chữ. Số liệu cụ thể trích dẫn từ solidworks) khâu1: u1=-L1x-L1y-L1z1=-3150-3551(mm) khâu2: u2=-L2x-L2y-L2z100-2111(mm) khâu 3: u3=-L3x-L3y-L3z1-197001(mm) khâu 4: u4=-L4x-L4y-L4z1-222001(mm) Tọa độ khối tâm khâu trong hệ cơ sở được tính: rCi0=Ti0*ui rC10=-258*cos⁡(q1)-258*sin⁡(q1)2451 rC20=-211*sinq1+30*cos⁡(q1)211*sinq1+30*cos⁡(q1))600+q21 rC30=203*cosq1cos⁡(q3)+30*cos⁡(q1)211*sinq1*(203*cos⁡(q3)+30*cos⁡(q1))203*sinq3+600+q21 rC40= 2*cosq1*(89*cosq3*cosq4-89*sinq3*sinq4+200*cosq3+15)2*sinq1*(89*cosq3*cosq4-89*sinq3*sinq4+200*cosq3+15)178*cosq4+400*sinq3+178*cosq3*sinq4+q2+6001 Vận tốc khối tâm khâu-ma trận jacobi tịnh tiến khối tâm khâu vi=rCi0=xCiyCizCi Ji= ∂rGi∂qj=∂xGi∂q1⋯∂xGi∂qj∂yGi∂q1⋱∂yGi∂qj∂zGi∂q1⋯∂zGi∂qj Note: kết quả chi tết ở phụ lục Miền làm việc của robot: Từ giới hạn từng khâu và phương trình quỹ đạo điểm cuối ta tính toán miền làm việc: Giới hạn các biến khớp: q1 -π2 ÷π2 q2 50 ÷350mm q3 (-π2 ÷π2 ) q4 (-π3 ÷π3 ) Sử dụng tính toán lập trình trên matlab ta có các đồ thị miền làm việc: Hình 13 Miền làm việc theo phương X-Y Hình 14 Miền làm việc trong không gian Y-Z Hình 15 Miền làm việc trong không gian X-Z Hình 16 Miền làm việc vẽ trong không gian 3D Mô phỏng động học thuận. Mô phỏng bằng quy luật đặc biệt ( kiểm tra bài toán thuận): Chọn các quy luật biến khớp đặc biệt: ( thuộc giới hạn biến khớp) q1=0 q2=50 q3=23*π*sin⁡(π12*t) t=0 thì q3=0 t=2 thì q3=π3 q4=-23*π*sin⁡(π12*t) t=2 thì q4=0 t=2 thì q4=-π3 Tính toán các tọa độ điểm cuối bằng cả phương pháp hình học và phương pháp DH so sánh 2 kết quả để kiểm tra. Tọa độ điểm cuối dễ dàng được xác định bằng phương pháp hình học: Tại t=0 thì [q1,q2,q3,q4]=[0, 50, 0, 0] UAxE=a1+a3+a4=350+400+400=1150 (mm)yE=0 (mm)zE= d1+q2=770+50=810 (mm) Tại t=2 s thì [q1,q2,q3,q4]=[0, 50, π3, π3] UB=xE=a1+a3*cos(q3)+a4=350+400*cos⁡(π3)+400=950 (mm)yE=0 (mm)zE= d1+q2+a3*sinq3=770+50+400*sinπ3=1166 (mm) Tọa độ điểm cuối được tính bằng phương pháp giải số: Hình 17 Quỹ đạo điểm cuối bài toán thuận kiểm tra Từ đồ thị ta thấy: t=0 : UAsốxE=1150 (mm)yE=0 (mm)zE=810 (mm) t=2 : UBsốxE=950 (mm)yE=0 (mm)zE=1166 (mm) Kết luận: Ta thấy kết quả tính toán hình học và kết quả tính toán bằng ma trận DH trên maple hoàn toàn trùng nhau nên kết quả giải động học thuận là đúng. Thiết kế mô phỏng theo quỹ đạo bất kì: q1=π*sin⁡(π60*t) q2=200 q3=π*sin⁡(π60*t) q4=π*sin⁡(π60*t) t=[0:0.04:4] Quỹ đạo điểm E Hình 18 Quỹ đạo mô phỏng thuận Vận tốc, gia tốc điểm E: Hình 19 Đồ thị vận tốc, gia tốc điểm E theo phương x Hình 20 Đồ thị vận tốc, gia tốc điểm E theo phương y Hình 21 Đồ thị vận tốc gia tốc điểm E theo phương z Đồ thị vận tốc, gia tốc khối tâm các khâu: Khâu 1 Hình 22 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo x. Hình 23 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo y Hình 24 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo z. Khâu 2 Hình 25 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo x. Hình 26 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo y Hình 27 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo z Khâu 3 Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x Hình 29 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo y Hình 30 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo z Khâu 4 Hình 31 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x Hình 32 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo y Hình 33 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo z Đồ thị vận tốc góc các khâu: Khâu 1: Hình 34 Vận tốc gia tốc góc khâu 1 theo x,y,z Khâu 2: Hình 35 Vận tốc gia tốc góc khâu 2 theo x,y,z Khâu 3: Hình 36 Vận tốc gia tốc góc khâu 1 theo x,y,z Khâu 4: Hình 37 Vận tốc gia tốc góc khâu 1 theo x,y,z Động học ngược Mục tiêu: - Khảo sát quy luật biến khớp phụ thuộc vào quy luật chuyển động điểm cuối cho trước. Nhằm phục vụ bài toán thiết kế quỹ đạo. Ứng dụng trong thực tế: Giúp quá trình mô phỏng chuyển động và khảo sát các biến khớp khi cho chuyển động theo quỹ đạo mong muốn. Giải bài toán động học ngược bằng phương pháp số (Newton-raphson). Lý thuyết tính toán: Trong giải tích số, phương pháp Newton-Rapshon là một phương pháp tìm nghiệm xấu xỉ gần đúng của một hàm số có tham số thực dựa vào tính hội tụ và ý nghĩa hình học của hàm số: Phương pháp được thực hiện như sau: Phương pháp bắt đầu với hàm f(x) là hàm cần tìm xấp sỉ ở đây là 3phương trình giải góc Cardan (rotx roty rotz) mà ta đã chọn. Tính các đạo hàm của 3 phương trình trên thực hiện đáp ứng ta tính được x1 là 1 xấp xỉ tốt hơn x0: x1=x0-fx0f'x0. (2.9) x0 là nghiệm đầu ta giải được tại t=0. Lặp đi lặp lại quá trình trên ta được xấp xỉ : xn=xn-1-fxn-1f'xn-1. (2.10) Ta chọn các giải các phương trình: f1=T401,4-C1,4 f2=T402,4-C2,4 f3=T403,4-C3,4 f7=T401,1-C1,1 Mô phỏng số bài toán ngược (đường thẳng ): Quỹ đạo đặt: xEt=50*t+600. yEt=0. zEt=100*t+300. rotz=13π Bộ nghiệm đầu: thay t=0 vào 4 phương trình được chọn giải ra bộ nghiệm. Các đồ thị vẽ được: Đồ thị thật: Đồ thị được vẽ bằng phương trình quỹ đạo đặt cho trước. Hình 38 Quỹ đại điểm cuối theo quy luật cho trước Đồ thị bài toán ngược: sau khi gải bài toán ngược được các giá trị biến khớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểm cuối. Hình 39 Đồ thị kiểm tra Lắp trùng 2 đồ thị này trên một miền duy nhất ta thấy trùng nhau nên kết quả giải là chính xác Hình 40 Vẽ hai đồ thị trên cùng một miền Mô phỏng số bài toán ngược (đường tròn): Quỹ đạo đặt: xEt=100*cos15πt+400. yEt=0. zEt=100*cos15πt+1000.. rotz=13π Bộ nghiệm đầu: [q1,q2,q3,q4]=[0, 147.22008, -1.1863995, .13920200] Các đồ thị vẽ được: Đồ thị thật: Đồ thị được vẽ bằng phương trình quỹ đạo đặt cho trước. Hình 41 Đồ thị quy luật cho trước Đồ thị bài toán ngược: sau khi gải bài toán ngược được các giá trị biến khớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểm cuối. Hình 42 Đồ thị kiểm tra Kiểm tra chính xác về mặt quỹ đạo t vẽ chung 2 đồ thị trên cùng một miền . Hình 43 Vẽ hai đồ thị trên cùng một miền Kết luận: Bài toán ngược chính xác về mặt quỹ đạo. Kết quả tính toán các biến khớp, vận tốc khớp, giá tốc khớp Để kiểm tra bài toán ngược ta có các điều khiện sau: Tại các tọa độ cực trị của đồ thị biến khớp q ( q chuyển từ tăng thành giảm hoặc ngược lại) thì vận tốc của khớp tại thời điểm đó phải bằng 0. Tại các điểm cực trị của vận tốc khớp ( vận tốc chuyển từ tang thành giảm hoặc ngược lại) thì gia tốc tại thời điểm đó phải bằng 0. Đồ thị đối với q1 Hình 44 Đồ thị q1 Kiểm tra: Tại các điểm A, B trên đồ thị khi đồ thị q1 đổi chiều thì vận tốc vq1=0. Tại các điểm C trên đồ thị khi đồ thị vq1 đổi chiều thì vận tốc aq1=0. Đồ thị đối với q2 Hình 45 Đồ thị q2 Kiểm tra: Tại A, B, Đồ thị q2 đặt cực trị ( đổi chiều tăng giảm) thì vq2=0. Tại C Đồ thị vq2 đặt cực trị ( đổi chiều tăng giảm) thì aq2=0. Đồ thị đối với q3 Hình 46 Đồ thị q3 Kiểm tra: Tại A Đồ thị q3 đặt cực trị ( đổi chiều tăng giảm) thì vq3=0. Tại B,C Đồ thị vq3 đặt cực trị ( đổi chiều tăng giảm) thì aq3=0. Kết luận: Bài toán ngược giải ra q3 đúng. Đồ thị đối với q4 Hình 47 Đồ thị q4 Kiểm tra: Tại A Đồ thị q4 đặt cực trị ( đổi chiều tăng giảm) thì vq4=0. Tại B,C Đồ thị vq4 đặt cực trị ( đổi chiều tăng giảm) thì aq4=0. Kết luận: Bài toán ngược giải ra q4 đúng. Kiểm tra tính chính xác của bài toán ngược: Phương pháp: Tính ma trận Jacobi đối với khâu cuối. Vận tốc điểm E tính theo Jacobi có công thức: vE=i=1nduEdqi*q So sánh với vận tốc được tính từ quy luật chúng ta đặt ban đầu Các đồ thị kiểm tra: Hình 48 Đồ thị vận tốc thực và vận tốc kiểm tra phương x Hình 49 Đồ thị vận tốc thực và vận tốc kiểm tra phương y Hình 50 Đồ thị vận tốc thực và vận tốc kiểm tra phương z Nhận xét: Các kết quả đồ thị nhận được tương đối chính xác với mức sai tối ta trên đồ thị là 0.6 mm/s. Kết luận chung: Bài toán ngược giải bằng phương pháp số trên là chính xác. CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO Thiết kê quỹ đạo hình học đơn giản: Mục tiêu: thiết kế các quỹ đạo chuyển động khâu cuối theo thời gian phù hợp với chuyển động, miền làm việc của Robot. Ứng dụng trong thực tế: Áp dụng cho bài toán động học ngược hoặc bài toán động lực học ngược. Quỹ đạo hình học đơn giản làm đầu vào kiểm tra mô phỏng bài toán nghịch, ta chỉ cần tìm các tọa độ điểm E là hàm theo thời gian. Đường thẳng đi qua 2 điểm: A ( 600,0 , 300) và B(900, 0, 900) chuyển động chỉ trong mặt Oxz. Phương trình đường thẳng đi qua A, B: z=2x+9. Chiếu lên 2 trục : x=a1*t+b1 , z=a2*t+b2. Thay các điều kiện: t=0:x=600, z=300. t=6 :x=900, z=900 Được quy luật điểm cuối: xEt=100*t+600; yEt=0; zEt=100*t+300. Sử dụng phương pháp tang thức bậc 3 ta tính được các phương trình q: Đường tròn đi tâm I ( 600,0 , 200) và bán kính R =100 chuyển động trong mặt phẳng song song Oxz. Phương trình đường thẳng đi qua A, B: xEt=100*cosφ+600. yEt=100*sinφ,                         zEt=200. Thay các điều kiện: t=0:x=500, y=0, z=200. t=5:x=700, y=0,z=200. Được quy luật điểm cuối: xEt=100*cos15πt+400; yEt=0; zEt=100*sin15πt+1000. 3.2 Thiết kế quỹ đạo đi qua 5 điểm: Yêu cầu toán: Thiết kế quỹ đạo trơn đi qua 5 điểm bất kì trong miền làm việc của Robot. Lý thuyết: Với N+1=5 điểm thuộc quỹ đạo cần thiết kế ta sử dụng phương pháp Spline thiết kế N quỹ đạo đoạn là các đa thức bậc <N (ở đây ta chọn thiết kế theo quỹ đạo bậc 3) là đa thức nội suy Lagrange, nội suy Newton,.... Các bước tính toán