Do an tot nghiep dai hoc bach khoa ha noi

Theo thiết kế ban đầu mẫu robot RoPC01 do Viện Khoa Học Kỹ Thuật và Bảo Hộ Lao Động làm chủ đề tài nghiên cứu thì tay máy con robot này có 6 khâu, 6 bậc tự do, gồm 1 khớp trụ xoay dưới cùng, 4 khớp quay bản lề và 1 khớp tịnh tiến dọc trục phía trên. Mẫu robot này hiện nay đã được chế tạo thành công và những bước đầu thử nghiệm đã cho kết quả khả quan về cấu trúc cũng như độ chính xác hoạt động. Từ những kết quả thu được có thể thấy rằng mẫu robot RoPC01 là 1 mẫu robot rất có triển vọng. Và từ mô hình của mẫu robot này ta có thể xây dựng nên những mẫu robot đa năng khác. Trong thời gian thực tập em đã được tiếp cận với đề tài robot RoPC01 này và khi thực hiện đồ án tốt nghiệp dưới sự hướng dẫn của GS.TSKH Đỗ Sanh em đã được thầy gợi ý và hướng theo 1 mẫu mới cho robot RoPC01 đó là mẫu robot RoPC01 – vòi lắc ngang. Và em đã quyết định chọn đề tài “Tính toán động học và mô phỏng mẫu robot RoPC01 – vòi lắc ngang”.

Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 MỤC LỤC Lời mở đầu .4 Chương I Tổng quan robot công nghiệp I Giới thiệu chung robot Khái quát robot Các cấu trúc động học robot Bậc tự robot .11 Phân loại robot 11 Ứng dụng robot sống 16 II Giới thiệu robot di động .20 Tổng quan 20 Sự ứng dụng robot tự hành 21 3.Cơ sở lí thuyết khảo sát robot tự hành 21 Chương II Nghiên cứu mơ hình kết cấu mẫu robot RoPC01 22 I Khái quát cấu trúc kết cấu mẫu robot RoPC01 .22 II Một số yêu cẩu tích hợp mơđun mẫu robot RoPC01 23 III Thiết kế mơ hình cấu trúc mẫu robot RoPC01 25 Chương III Tính tốn động học mẫu robot RoPC01 .29 I Cơ sở lý thuyết tính tốn động học robot 29 Ma trận cosin hướng 29 1.1 Định nghĩa ma trận cosin hướng 29 1.2 Các ma trận quay 31 1.3 Ý nghĩa ma trận cosin hướng 32 1.4 Một số tính chất ma trận cosin hướng 33 Các tọa độ ma trận biến đổi 34 2.1 Các tọa độ 34 2.2 Ma trận biến đổi hệ tọa độ 35 Các phương pháp xác định ma trận cosin hướng 37 3.1 Phép quay theo góc Euler 37 3.2 Phép quay theo góc Cardan 39 Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -1- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 3.3 Phép quay theo góc Roll - Pitch- Yaw (RPY) 40 Phương pháp ma trận Denavit-Hartenberg 41 4.1 Các tham số động học Denavit-Hartenberg .41 4.2 Ma trận Denavit-Hartenberg .44 4.3 Ma trận quan hệ 45 II Bài toán động học thuận 45 Phương pháp ma trận truyền .45 11 Các ma trận truyền 45 1.2 Tính toán động học thuận robot RoPC01 47 Phương pháp Denavit-Hartenberg .50 2.1 Đặt trục tọa độ lập bảng thông số Denavit-Hartenberg 50 2.2 Giải toán động học thuận 52 III Bài toán động học ngược 55 Xây dựng phương trình quỹ đạo làm việc robot 55 Giải toán động học ngược .57 2.1 Điều khiển theo biến khớp q2 .59 2.2 Điều khiển theo biến khớp q3 .62 Thời gian làm việc chu trình 66 Vận tốc gia tốc di chuyển điểm làm việc 67 4.1 Vận tốc .67 4.2 Gia tốc 67 Vận tốc góc tuyệt đối khâu .68 5.1 Tính trực tiếp 68 5.2 Tính gián tiếp .71 Tọa độ trọng tâm khâu 79 Vận tốc khối tâm khâu 88 Kiểm tra kết tính tốn 96 So sánh phương án 96 Chương IV Mô hoạt động robot RoPC01 .97 I Khái quát công việc mô robot 97 II Các bước mô robot 98 Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -2- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 Xuất đối tượng sang file định dạng*.STL 98 Biên dịch file *.STL sang dạng file lưu trữ tọa độ .100 III Đọc liệu tính tốn .100 IV Cấu trúc chương trình mơ robot RoPC01 103 V Vẽ lại quỹ đạo làm việc 106 Chương V Tính toán động lực học robot RoPC01 .110 I Cơ sở lí thuyết 110 Biểu thức động rôbôt công nghiệp xác định từ ma trận Denavit – Hartenberg 110 Thiết lập phương trình vi phân chuyển động rơbơt cơng nghiệp phương pháp Lagrange loại hai 111 3.Giải tốn động học thuận robot cơng nghiệp .115 4.Giải toán động học ngược robot công nghiệp 116 III.Thiết lập phương trình vi phânchuyển động 116 IV.Giải toán động học thuận 126 V.Giải toán động học ngược 127 So sánh mẫu RoPc01 gốc RoPC01-vòi lắc ngang 129 Những vấn đề cần mở rộng nghiên cứu .130 Kết luận 131 Phụ lục 132 Tài liệu tham khảo 150 Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -3- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 LỜI MỞ ĐẦU Trong thời đại ngày nay, với phát triển vũ bão khoa học công nghệ Robot có mặt khắp nơi, lĩnh vực sản xuất đời sống Đến thời điểm không phủ nhận lợi ích to lớn mà Robot mang đến cho người., chúng ứng dụng vào hầu hết lĩnh vực sống, đặc biệt vào cơng nghiệp sản xuất Robot có ưu điểm tính ổn định, độ hoạt động xác cao, có khả chun sâu linh động cơng việc Do ứng dụng Robot ngành công nghiệp giúp nâng cao suất, chất lượng sản phẩm, giảm giá thành, tăng sức cạnh tranh cho sản phẩm Khi công nghiệp phát triển ứng dụng vai trò Robot to lớn Xuất phát thêm từ thực tế có nhiều cơng việc phải tiến hành điều kiện môi trường đặc biệt mà người không thực : làm việc khơng gian hạn hẹp, địi hỏi phải di chuyển nhiều không gian làm việc ảnh hưởng đến sức khỏe người nóng ẩm, ồn ào, khói bụi, khí độc, phóng xạ Trong điều kiện việc nghiên cứu, chế tạo, ứng dụng mẫu Robot di động điều khiển từ xa thay người để làm việc môi trường thực tế cấp thiết Là sinh viên Đại Học Bách Khoa Hà Nội đào tạo chuyên ngành Cơ Điện Tử trang bị kiến thức học, tin học, điện điện tử cho phép em có khả tìm hiểu nghiên cứu Robot Vì em định chọn đề tài Robot để làm Đồ án tốt nghiệp nhằm vận dụng, trau đồi kiến thức học để nghiên cứu xây dựng, chế tạo mẫu Robot có ứng dụng thực tế Qua thực tế tìm hiểu thơng qua phương tiện truyền thông em biết ngành cơng nghiệp đóng tàu Việt Nam phát triển đòi hỏi ứng dụng nhiều Robot Chẳng hạn công đoạn làm bề mặt vỏ tàu cần đến ứng dụng Robot điều kiện làm việc với bụi ảnh hưởng đến sức khỏe người lao động Vì dẫn đến nhu cầu chế tạo mọt loại Robot di động có khả thay người lao động Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -4- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 việc phun cát làm bề mặt bề mặt vỏ tàu Và mẫu Robot RoPC01 nghiên cứu, chế tạo để phục vụ yêu cầu Theo thiết kế ban đầu mẫu robot RoPC01 Viện Khoa Học Kỹ Thuật Bảo Hộ Lao Động làm chủ đề tài nghiên cứu tay máy robot có khâu, bậc tự do, gồm khớp trụ xoay cùng, khớp quay lề khớp tịnh tiến dọc trục phía Mẫu robot chế tạo thành công bước đầu thử nghiệm cho kết khả quan cấu trúc độ xác hoạt động Từ kết thu thấy mẫu robot RoPC01 mẫu robot có triển vọng Và từ mơ hình mẫu robot ta xây dựng nên mẫu robot đa khác Trong thời gian thực tập em tiếp cận với đề tài robot RoPC01 thực đồ án tốt nghiệp hướng dẫn GS.TSKH Đỗ Sanh em thầy gợi ý hướng theo mẫu cho robot RoPC01 mẫu robot RoPC01 – vòi lắc ngang Và em định chọn đề tài “Tính tốn động học mơ mẫu robot RoPC01 – vịi lắc ngang” Trong thời gian làm đồ án cố gắng nỗ lực kiến thức có hạn nên đồ án em khơng tránh khỏi hạn chế sai sót Em mong nhận ý kiến đóng góp thầy để đồ án em hoàn chỉnh Em xin chân thành cảm ơn GS-TSKH ĐỖ SANH, TS.TRIỆU QUỐC LỘC thầy môn hướng dẫn tận tình hướng dẫn, giúp em hồn thành đồ án tốt nghiệp Sinh viên thực Ngô Trung Dũng CHƯƠNG I Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -5- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP I GIỚI THIỆU CHUNG VỀ ROBOT Khái quát Robot 1.1 Định nghĩa Robot • Là thiết bị khí đơi giống người thức nhiều nhiệm vụ phức tạp người lệnh chương trình điều khiển • Là máy thiết bi hoạt động tự động đươc điều khiển từ xa • Nó người làm việc cách học ý thức, tức trả lời cách tự động lệnh điều khiển khác 1.2 Lịch sử đời phát triển Robot Từ robot xuất hiện, 20 năm sau ước mơ viễn tưởng Kerel Capek ( Zech - 1922 ) việc tạo robot làm nhiệm vụ tạp dịch nhà, đến trải qua trình cải tiến hồn thiện nhanh chóng Bắt đầu cấu tay máy chép hình khí, thủy lực điện từ điều khiển từ xa phịng thí nghiệm vật liệu phóng xạ Hoa Kỳ tay máy Minotaur I tay máy Handyman Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -6- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 Hình1.1 Robot Handyman 1938 Harolh Roselund Willard người Mỹ thiết kế cấu phun sơn lập trình đầu tiền cho cơng ty DeVibiss 1942 Isaac Asimov xuất “Runaround” ơng đưa quy tắc robot 1951 Pháp RayMond Goertz thiết kế cánh tay có khớp hoạt động từ xa cho dự án lượng nguyên tử Thiết kế dựa toàn vào mối nối học cánh tay cánh tay phụ (sử dụng cáp thép ròng rọc) Mẫu thiết kế ngày sử dụng cànn sử lý mẫu hạt nhân Nói chung coi bước ngoặc kỹ thuật phản hồi lực 1954 Goerge C Devol thiết kế cấu robot lập trình đặt tên Universal Automation, nguồn gốc tên công ty sau ông Unimation 1959 Marvin Minsky John McCarthy thành lập phịng thí nghiệm trí tuệ nhân tạo MIT ( Massachusetts Institute of Technology) 1959 C.DeVol công cho đời robot cơng nghiệp có tên Unimate 1960 Tập đoàn Codec mua lại công ty Unimation C.Devol bắt đầu phát triển hệ thống robot Uminate Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -7- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 1961 General Motors đa mua lại robot công nghiệp Unimate tư công ty Unimation lắp đặt cho dây truyền sản xuất Robot điều khiển tay loại hệ robot Unimate 1963 John McCarthy đứng lãnh đạo phịng thí nghiệm tí tuệ nhân tạo trường đại học Stanford 1964 Các phòng thí nghiệm nghiên cứu trí tuệ nhân tạo nhân tạo mở MIT,SRI(Stanford Research Institute), Trường Đai học Stanford, trường đại học Edinburgh 1964 hãng C&D Robotics thành lập 1964 Carnegie Mellon University thành lập học viện robot ( Robotics Institute) 1965 phép biến đổi ứng dụng cho cách tính động học robot – Nguồn gốc hình thành học thuyết robot ngày 1967 Nhật nhập robot Versatran từ hạng AMF( robot nhập vào Nhật) 1968 Kawasaki cấp giấy phép thiết kế robot thủy lực công ty Unimation bắt đầu sản xuất Nhật 1968 SRI thiết kế Shakey, robot di động có khả quan sát điều khiển máy cỡ lớn phòng 1970 Giáo sư Victor Scheinman đại học Stanford thiết kế mẫu robot Standard Arm Mà ngày cấu động học đứoc xem cách tay robot tiêu chuẩn 1973 Cincinnati Milacron phát minh loại T3, loại robot công nghiệp mang tính thương mại điều khiển máy tính mini commercially available (designed by Richard Hohn) 1974 Giao su Victor Scheinman, nhà phát triển loại cánh tay Stanford , thành lập công ty Vicarm nhắm đưa thị trường phiên cánh tay robot cho ứng dụng công nghiệp Loại cánh tay điều khiển máy tính mini Ngơ Trung Dũng K49 Lớp Cơ Điện Tử 1- -8- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 1976 Cánh tay robot sủ dụng tàu tham vũ trụ Viking Trong thiết công ty Vicarm sư dụng vi tính máy tính (microcomputer) 1977 ASEA, cơng ty robot Châu Âu giới thiệu loại robot công nghiệp chạy điện Cả sử dụng máy vi tính cho việc lập trình điều khiển hoạt động 1977 Unimation mua lại Công ty Vicarm 1978 Sử dụng công nghệ Vicarm, Unimation sản xuất robot PUMA (Programmable Universal Machine for Assembly ) Ngày thấy Puma có số phịng thí nghiệm 1979 Sankyo va IBM đưa thị trường mẫu robot Scara (Selective compliant articulated robot arm) trường Yamanashi Nhật Cho đến nhờ áp dụng có tiến kỹ thuật vi xử lý công nghệ thông tin, số lượng robot cơng nghiệp gia tăng, tính có nhiều bước tiến vượt bậc Do robot cơng nghiệp chiếm vị trí quan trọng cá dây truyền sản xuất Đã có nhiều chủng loại robot với nhiều hình dáng, kích cỡ ứng dụng nhiều lĩnh vực xã hội, : công nghiệp, khoa học , y tế , an ninh… 2.Các cấu trúc động học robot Về cấu trúc động học Robot hệ nhiều khâu ghép nối với khớp _Khâu: phận có chuyển động tương Khâu hay nhiều chi tiết máy ghép cứng với Chi tiết máy phận tháo rời máy _Khớp: Là chỗ nối động hai khâu, nghĩa có chuyển động tương Như hai khâu nối với tạo thành khớp động chỗ tiếp xúc hai khâu tạo thành khớp động gọi hai thành phần khớp động Khớp có ràng buộc vật lý chuyển động tương đối hai thành phần khớp Các loại khớp thường sử dụng: Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -9- Đồ án tốt nghiệp • Tính tốn mơ mẫu động học robot RoPC01 Khớp quay (Revolute Joint-R): Cho phép hai thành phần khớp chuyển động quay tương theo trục xác định dạng hình học khớp Khớp quay hạn chế khả chuyển động tương đối hai thành phần khớp Khớp quay gọi khớp lề Hình • Khớp lăng trụ (Prismatic-P) : Cho phép hai thành phần khớp trượt với theo trục xác định dạng hình học khớp Do khớp lăng trụ hạn chế khả chuyển động tương đối hai thành phần khớp Khớp lăng trụ gọi khớp tịnh tiến Hình • Khớp trụ (Cylindrical Joint-C) : Cho phép hai thành phần khớp có hai chuyển động độc lập, gồm chuyển động quay quanh trục chuyển động tịnh tiến dọc theo trục quay Do khớp trụ hạn chế khả chuyển động hai thành phần khớp Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -10- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 > A[3,3]:=cos(phi); > return(A); > end; > # MA TRẬN COSIN CHỈ HƯỚNG QUAY QUANH TRỤC Y > RotY:=proc(psi) > local A; > A:=Matrix(3,3); > A[1,1]:=cos(psi); > A[1,2]:=0; > A[1,3]:=sin(psi); > A[2,1]:=0; > A[2,2]:=1; > A[2,3]:=0; > A[3,1]:=-sin(psi); > A[3,2]:=0; > A[3,3]:=cos(psi); > > > return(A); > end; > > # MA TRẬN COSIN CHỈ HƯỚNG QUAY QUANH TRỤC Z > RotZ:=proc(theta) > local A; > A:=Matrix(3,3); > A[1,1]:=cos(theta); > A[1,2]:=-sin(theta); > A[1,3]:=0; > A[2,1]:=sin(theta); > A[2,2]:=cos(theta); > A[2,3]:=0; > A[3,1]:=0; > A[3,2]:=0; > A[3,3]:=1; > > > return(A); > end; > # MA TRẬN TỊNH TIẾN > TT:=proc(P) > local A; > > A:=Matrix(3,3); > A[1,1]:=1; > A[1,2]:=0; > A[1,2]:=0; > A[2,1]:=0; > A[2,2]:=1; > A[2,3]:=0; > A[3,1]:=0; > A[3,2]:=0; > A[3,3]:=1; > end; > > > "CÁC MA TRẬN COSIN CHỈ HƯỚNG."; T1 := RotZ(q1); T12:= TT([500,0,0]): T22:= RotZ(q2): T2 := Multiply(T12,T22); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -138- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 T3 := TT([q3,0,0]); T4 := RotZ(q4); T51:= Matrix([[1,0,0],[0,0,-1],[0,1,0]]); T52:= TT([200,0,0]); T5 := Multiply(T51,T52); T6 := RotZ(q5); T56 := Multiply(T5,T6); T := Multiply(Multiply(Multiply(Multiply(Multiply(T1,T2),T3),T4),T5),T6); r := Vector([200,0,0]); "VÉC TƠ TẠO ĐỘ ĐIỂM CUỐI."; m:= Multiply(T,r); px:=m[1]; py:=m[2]; pz:=m[3]; # Tính ma trận cosin hướng với hệ quy chiếu quán tính gốc A1 := T1 ; A2 := Multiply(T1,T2); A3 := Multiply(Multiply(T1,T2),T3); A4 := Multiply(Multiply(Multiply(T1,T2),T3),T4); A5 := Multiply(Multiply(Multiply(Multiply(T1,T2),T3),T4),T5); A6 := Multiply(Multiply(Multiply(Multiply(Multiply(T1,T2),T3),T4),T5),T6); # BÀI TỐN VẬN TĨC GĨC TUYỆT ĐỐI CỦA CÁC KHÂU # Tính ma trận chuyển vị "MA TRẬN CHUYỂN VỊ CỦA CÁC MA TRẬN TRUYỀN"; T1t := Transpose(T1); T2t := Transpose(T2); T3t := Transpose(T3); T4t := Transpose(T4); T5t := Transpose(T5); T6t := Transpose(T6); T56t := Transpose(T56); # Tính đạo hàm ma trận chuyền "ĐẠO HÀM CỦA CÁC MA TRẬN TRUYỀN"; T1d := map(diff,T1,q1); T2d := map(diff,T2,q2); T3d := map(diff,T3,q3); T4d := map(diff,T4,q4); T5d := map(diff,T5,q5); T6d := map(diff,T6,q6); T56d := map(diff,T56,q5); # Xác định ma trận sóng ws1 := Multiply(Transpose(T1),Map(diff,T1,q1)); ws2 := Multiply(Transpose(T2),Map(diff,T2,q2)); ws3 := Multiply(Transpose(T3),Map(diff,T3,q3)); ws4 := Multiply(Transpose(T4),Map(diff,T4,q4)); ws5 := Multiply(Transpose(T5),Map(diff,T5,q5)); ws6 := Multiply(Transpose(T6),Map(diff,T6,q6)); ws56 := Multiply(Transpose(T56),Map(diff,T56,q5)); ws11 ws21 ws31 ws41 := := := := simplify simplify simplify simplify (ws1); (ws2); (ws3); (ws4); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -139- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 ws51 := simplify (ws5); ws61 := simplify (ws6); ws561 := simplify (ws56); "TỐN TỬ SĨNG VẬN TỐC GĨC TƯƠNG ĐỐI CỦA TỪNG KHÂU"; ws111 := q11*ws11 ; ws211 := q21*ws21 ; ws311 := q31*ws31 ; ws411 := q41*ws41 ; ws511 := q51*ws51 ; ws611 := q61*ws61 ; ws5611 := q51*ws561 ; # Vận tốc góc tương đối khâu "VẬN TỐC GÓC TƯƠNG ĐỐI CỦA TỪNG KHÂU"; w1 := Vector([ws111[3,2],ws111[1,3],ws111[2,1]]); w2 := Vector([ws211[3,2],ws211[1,3],ws211[2,1]]); w3 := Vector([ws311[3,2],ws311[1,3],ws311[2,1]]); w4 := Vector([ws411[3,2],ws411[1,3],ws411[2,1]]); w5 := Vector([ws511[3,2],ws511[1,3],ws511[2,1]]); w6 := Vector([ws611[3,2],ws611[1,3],ws611[2,1]]); w56 := Vector([ws5611[3,2],ws5611[1,3],ws5611[2,1]]); # Vận tốc góc tuyệt đối khâu "VẬN TỐC GÓC TUYỆT ĐỐI CỦA TỪNG KHÂU."; omega[1] := w1; omega[2] := Multiply(T2t,omega[1]) + w2 ; omega[3] := Multiply(T3t,omega[2]) + w3 ; omega[4] := Multiply(T4t,omega[3]) + w4 ; omega[5] := Multiply(T56t,omega[4]) + w56 ; # PHƯƠNG OMEGA[1] OMEGA[2] OMEGA[3] OMEGA[4] OMEGA[5] TRÌNH VẬN TỐC GĨC TỔNG QT CỦA TỪNG KHÂU := sqrt(omega[1][1,1]^2 + omega[1][1,2]^2 + := sqrt(omega[2][1,1]^2 + omega[2][2,1]^2 + := sqrt(omega[3][1,1]^2 + omega[3][2,1]^2 + := sqrt(omega[4][1,1]^2 + omega[4][2,1]^2 + := sqrt(omega[5][1,1]^2 + omega[5][2,1]^2 + # BÀI TOÁN VẬN TỐC TRỌNG TÂM CỦA CÁC # Xác s1 := s2 := s3 := s4 := s5 := s0 := u1 := u2 := u3 := u4 := u5 := omega[1][1,3]^2); omega[2][3,1]^2); omega[3][3,1]^2); omega[4][3,1]^2); omega[5][3,1]^2); KHÂU định vector u s Vector([-250,0,0]); Vector([-250,0,0]); Vector([250,0,0]); Vector([-100,0,0]); Vector([-100,0,0]); ; Vector([250,0,0]); Vector([250,0,0]); Vector([-250,0,0]); Vector([100,0,0]); Vector([100,0,0]); d3 := Vector([q3,0,0]); d31 := Vector([q31,0,0]); "VẬN TỐC TRỌNG TÂM CỦA CÁC KHÂU."; v11 := - Multiply(Multiply(A1,ws111),s1); v21 := v11 + Multiply(Multiply(A1,ws111),u1) Multiply(Multiply(A2,ws211),s2); v31 := v21 + Multiply(A1,d31) - Multiply(Multiply(A2,ws211),d3); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -140- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 v41 := v31 + Multiply(Multiply(A3,ws311),u1) Multiply(Multiply(A4,ws411),s4); v51 := v41 + Multiply(Multiply(A4,ws411),u1) Multiply(Multiply(A6,ws5611),s5); # Ma trận tọa độ khối tâm khâu "CÁC MA TRẬN A RÚT GỌN."; A11 := Matrix([[cos(q1),-sin(q1),0],[sin(q1),cos(q1),0],[0,0,1]]); A21 := Matrix([[cos(q1+q2),-sin(q1+q2),0],[sin(q1+q2),cos(q1+q2),0], [0,0,1]]); A31 := A21; A41 := Matrix([[cos(q1+q2+q4),-sin(q1+q2+q4),0], [sin(q1+q2+q4),cos(q1+q2+q4),0],[0,0,1]]); A51 := Matrix([[cos(q1+q2+q4)*cos(q5),cos(q1+q2+q4)*sin(q5),sin(q1+q2+q4)],[sin(q1+q2+q4)*cos(q5),sin(q1+q2+q4)*sin(q5),-cos(q1+q2+q4)],[sin(q5),cos(q5),0]]); "CÁC VECTOR TỌA ĐỘ TRỌNG TÂM CỦA CÁC KHÂU."; r1 := Multiply(A11,(-s1)); r2 := r1 + Multiply(A11,u1) - Multiply(A21,s2); r3 := r2 + Multiply(A21,d3); r4 := r3 + Multiply(A31,u3) - Multiply(A41,s4); r5 := r4 + Multiply(A41,u4) - Multiply(A51,s5); "CÁC VECTOR VẬN TỐC KHỐI TÂM CỦA CÁC KHÂU"; v1 := ; v2 := ; v3 := ; v4 := v3 + ; v5 := v4 + ; q3t*cos(q1+q2) q3t*sin(q1+q2) + - Chương trình Maple thiết lập phương trình vi phân chuyển động ># THIET LAP PHUONG TRINH VI PHAN CHUYEN DONG CUA ROBOT RoPC01-VOILAC NGANG # > restart; with(linalg): > # Ki hieu cac toa suy rong,van toc suy rong va gia toc suy rong cua bien khop n:=5; q :=matrix(5,1,[q1,q2,q3,q4,q5]); > qd :=matrix(5,1,[qd1,qd2,qd3,qd4,qd5]); > qdd:=matrix(5,1,[qdd1,qdd2,qdd3,qdd4,qdd5]); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -141- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 > # Xac dinh cac matran cosin chi huong Ai cua cac khau tu cac matran Denavit-Hatenberg > A1:=matrix(3,3,[cos(q1),-sin(q1),0,sin(q1),cos(q1),0,0,0,1]); > A2:=matrix(3,3,[cos(q1+q2),sin(q1+q2),0,sin(q1+q2),cos(q1+q2),0,0,0,1]); > A3:=matrix(3,3,[cos(q1+q2),sin(q1+q2),0,sin(q1+q2),cos(q1+q2),0,0,0,1]); >A4:=matrix(3,3,[cos(q1+q2+q4),0,sin(q1+q2+q4),sin(q1+q2+q4),0,cos(q1+q2+q4),0,1,0]); > A5:=matrix(3,3,[cos(q1+q2+q4)*cos(q5),cos(q1+q2+q4)*sin(q5),sin(q1+q2+q4),sin(q1+q2+q4)*cos(q5),sin(q1+q2+q4)*sin(q5),-cos(q1+q2+q4),sin(q5),cos(q5),0]); > # Tinh toa khoi tam cac khau he toa dong rC11:=matrix(3,1,[0.25,0,0]); > rC22:=matrix(3,1,[0.25,0,0]); > rC33:=matrix(3,1,[0.25,0,0]); > rC44:=matrix(3,1,[0.1,0,0]); > rC55:=matrix(3,1,[0.1,0,0]); > # Vi tri goc toa cua cac he qui chieu Ri he qui chieu R0 > r1:=matrix(3,1,[0,0,0]); > r2:=matrix(3,1,[0.5*cos(q1),0.5*sin(q1),0]); >r3:=matrix(3,1, [0.5*cos(q1)+q3*cos(q1+q2),0.5*sin(q1)+q3*sin(q1+q2),0]); >r4:=matrix(3,1, [0.5*cos(q1)+q3*cos(q1+q2),0.5*sin(q1)+q3*sin(q1+q2),0]); >r5:=matrix(3,1, [0.5*cos(q1)+q3*cos(q1+q2)+0.2*cos(q1+q2+q4),0.5*sin(q1)+q3*sin(q1+q2) +sin(q1+q2+q4),0]); > # Tinh vi tri khoi tam cua cac khau he qui chieu R0 > rC1:=simplify(evalm(r1+A1&*rC11)); > rC2:=simplify(evalm(r2+A2&*rC22)); > rC3:=simplify(evalm(r3+A3&*rC33)); > rC4:=simplify(evalm(r4+A4&*rC44)); > rC5:=simplify(evalm(r5+A5&*rC55)); > # Tinh cac matran jacobian tinh tien cua cac khau > JacobiTinhTien:=proc(rC::matrix) local Jt,i,j; Jt:=matrix(3,n,0); for i from by to for j from by to n Jt[i,j]:=diff(rC[i,1],q[j,1]); od;od; return(evalm(Jt)); end; > Jt1:=JacobiTinhTien(rC1); > Jt2:=JacobiTinhTien(rC2); > Jt3:=JacobiTinhTien(rC3); > Jt4:=JacobiTinhTien(rC4); > Jt5:=JacobiTinhTien(rC5); > # Tinh cac vector van toc goc cua cac khau > Tinh_omega:=proc(A) local Ad,i,j,k,omega,omega_song; Ad:=matrix(3,3,0); for i from by to for j from by to for k from by to n Ad[i,j]:=Ad[i,j]+ diff(A[i,j],q[k,1])*qd[k,1]; od; od; od; Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -142- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 omega_song:=simplify(evalm(Ad&*transpose(A))); omega:=vector([-omega_song[2,3],omega_song[1,3],-omega_song[1,2]]); return(evalm(omega)); end; > omega1:=Tinh_omega(A1); > omega2:=Tinh_omega(A2); > omega3:=Tinh_omega(A3); > omega4:=Tinh_omega(A4); > omega5:=Tinh_omega(A5); > # Tinh cac matran jacobian quay cua cac khau > JacobiQuay:=proc(omega) local Jr,i,j; Jr:=matrix(3,n,0); for i from by to for j from by to n Jr[i,j]:=diff(omega[i],qd[j,1]); od; od; return(evalm(Jr)); end; > Jr1:=JacobiQuay(omega1); > Jr2:=JacobiQuay(omega2); > Jr3:=JacobiQuay(omega3); > Jr4:=JacobiQuay(omega4); > Jr5:=JacobiQuay(omega5); > # Momen quan tinh khoi cua cac khau > I1:=matrix(3,3,[[I1x,0,0],[0,I1y,0],[0,0,I1z]]); > I2:=matrix(3,3,[[I2x,0,0],[0,I2y,0],[0,0,I2z]]); > I3:=matrix(3,3,[[I3x,0,0],[0,I3y,0],[0,0,I3z]]); > I4:=matrix(3,3,[[I4x,0,0],[0,I4y,0],[0,0,I4z]]); > I5:=matrix(3,3,[[I5x,0,0],[0,I5y,0],[0,0,I5z]]); > # Thu tuc xac dinh matran khoi luong M KL:=proc(m::scalar,Ii::matrix,A::matrix,Jr::matrix,Jt::matrix) local M; M:=evalm(transpose(Jt)&*Jt*m + transpose(Jr)&*A&*Ii&*transpose(A)&*Jr); return(M); end; M1:=map(combine,matrix(KL(m1,I1,A1,Jr1,Jt1)),trig); M2:=map(combine,matrix(KL(m2,I2,A2,Jr2,Jt2)),trig); M3:=map(combine,matrix(KL(m3,I3,A3,Jr3,Jt3)),trig); M4:=map(combine,matrix(KL(m4,I4,A4,Jr4,Jt4)),trig); M5:=map(combine,matrix(KL(m5,I5,A5,Jr5,Jt5)),trig); > M:=simplify(evalm(M1+M2+M3+M4+M5)); save M,"matrankhoiluong.txt"; # Cac phan tu cua ma tran khoi luong for i from by to n for j from by to n print(m[i,j]=evalm(M[i,j])); od; od; > # Thu tuc tinh matran C[q,qd] tu matran M MatC:=proc(M::matrix,q::matrix,qd::matrix,n::integer) local C,i,j,k,x,y,h; for i from by to n for j from by to n for k from by to n h[i,j,k]:=1/2*(diff(M[i,j],q[k,1]) + diff(M[i,k],q[j,1]) diff(M[k,j],q[i,1])); od; Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -143- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 od; od; C:=matrix(n,n,0); for x from by to n for y from by to n C[x,y]:=sum(h[x,y,z]*qd[z,1],z=1 n); od; od; return(evalm(C)); end; >C:=MatC(M,q,qd,n);save C,"matran_C.txt"; > # Bieu thuc the nang cua he PI:=m1*rC1[2,1]*g + m2*rC2[2,1]*g + m3*rC3[2,1]*g + m4*rC4[2,1]*g + m5*rC5[2,1]*g; > # Tinh cac luc truong(luc co the) tac dung vao cac khau > G:=matrix(n,1,0): for i from by to n G[i,1]:=diff(PI,q[i,1]); od; print(G=evalm(G)); save G,"matran_G.txt"; > # Bieu thuc ham hao tan co dang Phi:=1/2*(b1*qd1^2+b2*qd2^2+b3*qd3^2+b4*qd4^2+b5*qd5^2); > # Tinh cac luc can nhot tac dung vao cac khop dong cua robot Lc:=matrix(n,1,0); for i from by to n Lc[i,1]:=diff(Phi,qd[i,1]); od; print(Lc=evalm(Lc)); save Lc,"matran_Lc.txt"; > # Cac luc phat dong cua dong co o cac khop Lt:=matrix(n,1,[tau[1],tau[2],tau[3],tau[4],tau[5]]); save Lt,"matran_Lt.txt"; > # Cac phuong trinh vi phan chuyen dong thu duoc vetrai:=map(convert,evalm(M&*qdd + C&*qd + G + Lc),rational); save vetrai,"VetraiPT.txt"; > vetrai[1,1]=Lt[1,1]; > vetrai[2,1]=Lt[2,1]; > vetrai[3,1]=Lt[3,1]; > vetrai[4,1]=Lt[4,1]; > vetrai[5,1]=Lt[5,1]; Chương trình Maple giải toán động lực học thuận > # GIAI BAI TOAN DONG LUC HOC THUAN ROBOT RoPC01 > > restart; with(linalg); with(plots); > read "matrankhoiluong.txt"; read "matran_C.txt"; #G:=vector([0,0,-m3*g-m4*g,0]); Lc:=vector([b1*qd1,b2*qd2,b3*qd3,b4*qd4,b5*qd5]); read "Chogiatridau.txt"; Lt:=evalm(Lt); q0:=vector([q10,q20,q30,q40,q50]); qd:=vector([qd1,qd2,qd3,qd4,qd5]): qd0:=vector([qd10,qd20,qd30,qd40,qd50]); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -144- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 qdd:=vector([qdd1,qdd2,qdd3,qdd4,qdd5]); read "matran_G.txt"; G :=evalm(G); > # Nhap cac thong so dong hoc a1:=0.425: a2:=0.375: # don vi la met L1:=a1/2: L2:=a2/2: L3:=0.15: L4:=0.1: d1:=0.16: d4:=0.275: > # Nhap vao cac thong so dong luc hoc cua robot RoPC01 I1x:=0; I1y:=0.5; I1z:=0.5; # Don vi la kgm^2 I2x:=0; I2y:=0.2; I2z:=0.2; I3x:=0; I3y:=0; I3z:=0.2 ; I4x:=0; I4y:=0; I4z:=0.1; I5x:=0; I5y:=0; I5z:=0.1; b1:=0.5; b2:=0.5; b3:=0.5; b4:=0.5; b5:=0.5; m1:=4; m2:=2; m3:=1.5; m4:=1; m5:=1; g:=9.81; > f:=proc(ti,qi,qdi) local f; f:=simplify(M&*(Lt-C&*qd-Lc-G)); return(subs(t=ti,q1=qi[1],q2=qi[2],q3=qi[3],q4=qi[4],q5=qi[5],qd1=qdi[ 1],qd2=qdi[2],qd3=qdi[3],qd4=qdi[4],qd5=qdi[5],evalm(f))); end; > K:=200; T:=5; h:= T/K; i:=0; t0:=0; qi:=evalm(q0); qdi:=evalm(qd0); > q1t:=array(1 K); q2t:=array(1 K); q3t:=array(1 K); q4t:=array(1 K); q5t:=array(1 K); qd1t:=array(1 K); qd2t:=array(1 K); qd3t:=array(1 K); qd4t:=array(1 K); qd5t:=array(1 K); tg:=array(1 K); > K :=200; i :=0; while i < K ti:=t0+i*h; k1:=simplify(f(ti,qi,qdi)*h/2); k2:=simplify(f(ti+h/2,evalm(qi+qdi*h/2),evalm(qdi+k1))*h/2); k3:=simplify(f(ti+h/2,evalm(qi+qdi*h/2+k1*h/4),evalm(qdi+k2))*h/2); k4:=simplify(f(ti+h,evalm(qi+qdi*h+k3*h),evalm((qdi+k3*2)))*h/2); i:=i+1; q1t[i]:=qi[1]; q2t[i]:=qi[2]; q3t[i]:=qi[3]; q4t[i]:=qi[4]; Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -145- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 q5t[i]:=qi[5]; qd1t[i]:=qdi[1]; qd2t[i]:=qdi[2]; qd3t[i]:=qdi[3]; qd4t[i]:=qdi[4]; qd5t[i]:=qdi[5]; tg[i]:=ti; qi:=simplify(evalm(qi+qdi*h+(k1+k2+k3)*h/3)); qdi:=simplify(evalm(qdi+(k1+2*k2+2*k3+k4)*1/3)); end do; > plot(Lt[1],t=0 T,title="Momen tac dong vao khop 1"); plot(Lt[2],t=0 T,title="Momen tac dong vao khop 2"); plot(Lt[3],t=0 T,title="Luc tac dong vao khop 3"); plot(Lt[4],t=0 T,title="Momen tac dong vao khop 4"); plot(Lt[5],t=0 T,title="Momen tac dong vao khop 5"); > PLOT(CURVES([seq([tg[j],q1t[j]], j=1 K )]),TITLE("Do thi q1(t) tim bang PPS")); PLOT(CURVES([seq([tg[j],qd1t[j]], j=1 K )]),TITLE("Do thi qd1(t) tim bang PPS")); > PLOT(CURVES([seq([tg[j],q2t[j]], j=1 K )]),TITLE("Do thi q2(t) tim bang PPS")); PLOT(CURVES([seq([tg[j],qd2t[j]], j=1 K )]),TITLE("Do thi qd2(t) tim bang PPS")); > PLOT(CURVES([seq([tg[j],q3t[j]], j=1 K )]),TITLE("Do thi q3(t) tim bang PPS")); PLOT(CURVES([seq([tg[j],qd3t[j]], j=1 K )]),TITLE("Do thi qd3(t) tim bang PPS")); > PLOT(CURVES([seq([tg[j],q4t[j]], j=1 K )]),TITLE("Do thi q4(t) tim bang PPS")); PLOT(CURVES([seq([tg[j],qd4t[j]], j=1 K )]),TITLE("Do thi qd4(t) tim bang PPS")); Chương trình Maple giải toán động lực học ngược > # DONG LUC HOC NGUOC ROBOT RoPC01 > restart; with(linalg): with(plots): > # cho biet vi tri ban kep read "Toadodiemcuoi.txt": x:=matrix(4,1,[xp,yp,zp,phi]); > # Nhap cac thong so dong hoc a1:=0.425; # don vi la met a2:=0.375; L1:=a1/2; L2:=a2/2; L3:=0.15; L4:=0.1; d1:=0.16; d4:=0.275; T:=24; > # Thay vao bai toan dong hoc nguoc q2t:= simplify(0.785+0.09*t); q5t:= simplify(arcsin(0.75*sin(pi*t))); at:= simplify(8.8*t+196.96*cos(q5t)-517.18); bt:= simplify(-50*t+34.73*cos(q5t)+1227.73); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -146- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 alphat:= simplify(arctan(bt/at)); ct:= simplify(500*sin(q2t)/sqrt(at^2+bt^2)); q12at:= simplify(arcsin(ct)); q12t:= simplify(q12at+alphat); q1t:= simplify(q12t-q2t); q4t:= simplify(3.316-q12t); > # Tinh van toc suy rong,gia toc suy rong cac khop qd1t:=diff(q1t,t); qdd1t:=diff(qd1t,t); plot(q1t,t=0 T,color=red,title="Do thi q1t"); plot(qd1t,t=0 T,color=red,title="Do thi qd1t"); plot(qdd1t,t=0 T,color=red,title="Do thi qdd1t"); > qd2t:=diff(q2t,t): qdd2t:=diff(qd2t,t): plot(q2t,t=0 T,color=red,title="Do thi q2t"); plot(qd2t,t=0 T,color=red,title="Do thi qd2t"); plot(qdd2t,t=0 T,color=red,title="Do thi qdd2t"); > qd3t:=diff(q3t,t): qdd3t:=diff(qd3t,t): plot(q3t,t=0 T,color=red,title="Do thi q3t"); plot(qd3t,t=0 T,color=red,title="Do thi qd3t"); plot(qdd3t,t=0 T,color=red,title="Do thi qdd3t"); > qd4t:=diff(q4t,t): qdd4t:=diff(qd4t,t): plot(q4t,t=0 T,color=red,title="Do thi q4t"); plot(qd4t,t=0 T,color=red,title="Do thi qd4t"); plot(qdd4t,t=0 T,color=red,title="Do thi qdd4t"); > qd5t:=diff(q5t,t): qdd5t:=diff(qd5t,t): plot(q5t,t=0 T,color=red,title="Do thi q5t"); plot(qd5t,t=0 T,color=red,title="Do thi qd5t"); plot(qdd5t,t=0 T,color=red,title="Do thi qdd5t"); > # Nhap vao cac thong so dong luc hoc cua robot RoPC01 I1x:=0; I1y:=0.5; I1z:=0.5; # Don vi la kgm^2 I2x:=0; I2y:=0.2; I2z:=0.2; I3x:=0; I3y:=0; I3z:=0.2 ; I4x:=0; I4y:=0; I4z:=0.1; I5x:=0; I5y:=0; I5z:=0.1; b1:=0.5; b2:=0.5; b3:=0.5; b4:=0.5; b5:=0.5; m1:=4; m2:=2; m3:=1.5; m4:=1; m5:=1; g:=9.81; > # Doc vao phuong trinh vi phan chuyen dong cua robot RoPC01 read "VetraiPT.txt"; Lt:=evalm(vetrai); q1:=q1t; qd1:=qd1t; qdd1:=qdd1t; q2:=q2t; qd2:=qd2t; qdd2:=qdd2t; q3:=q3t; qd3:=qd3t; qdd3:=qdd3t; q4:=q4t; qd4:=qd4t; qdd4:=qdd4t; qd5:=qd5t; qdd5:=qdd5t; > # Ve thi luc phat dong cua dong co phu thuoc thoi gian plot(Lt[1,1],t=0 T,color=red,title="Do thi tau[1]"); plot(Lt[2,1],t=0 T,color=red,title="Do thi tau[2]"); plot(Lt[3,1],t=0 T,color=red,title="Do thi tau[3]"); plot(Lt[4,1],t=0 T,color=red,title="Do thi tau[4]"); plot(Lt[5,1],t=0 T,color=red,title="Do thi tau[5]"); II CÁC CHƯƠNG TRÌNH VIẾT BẰNG MATLAP Ngơ Trung Dũng K49 Lớp Cơ Điện Tử 1- -147- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 Chương trình Matlap giải tốn động học ngược, vẽ đồ thị vận tốc góc tuyệt đối, vector bán kính khối tâm vận tốc khối tâm điều khiển theo q2 % GIAI BAI TOAN NGUOC,VE DO THI CAC VAN TOC KHI DIEU KHIEN THEO Q2 clear all; clc ; global v n m ; t = 0:0.1:24 ; q1 = sym('q1(t)'); q2 = sym('q2(t)'); q3 = sym('q3(t)'); q4 = sym('q4(t)'); q5 = sym('q5(t)'); q12 = sym('q12(t)'); q12a = sym('q12a(t)'); a = sym('a(t)'); b = sym('b(t)'); e = sym('e(t)'); v = 50 ; n = 50 ; m = 150 ; % BIEU THUC CAC BIEN KHOP q5 = asin(m/200*sin(pi/n*v*t)) ; q2 = 0.785 + 0.09*t ; a = 0.176*v*t + 196.96*cos(q5) - 517.18 ; b = -v*t + 34.73*cos(q5) + 1227.73 ; alpha = atan2(b,a); e = 500*sin(q2)./sqrt(a.*a + b.*b) ; q12a= asin(e) ; q12 = q12a + alpha ; q1 = q12 - q2 ; q4 = 3.316 - q12 ; q3 = (b-500*sin(q1))./sin(q12) ; % DAO HAM THEO THOI GIAN CUA CAC BIEN KHOP q1_dh = diff(q1) ; q2_dh = diff(q2) ; q3_dh = diff(q3) ; q4_dh = diff(q4) ; q5_dh = diff(q5) ; q124_dh = diff(q1+q2+q4); % BIEU THUC CAC VAN TOC GOC TUYET DOI w1 = q1_dh ; w2 = q1_dh + q2_dh ; w3 = w2 ; w4 = q1_dh + q2_dh + q4_dh ; w5 = sqrt((q1_dh + q2_dh + q4_dh).^2 + q5_dh.^2) ; w44 = q124_dh ; % BIEU THUC TOA DO KHOI TAM CUA CAC KHAU r1x = 250*cos(q1) ; r1y = 250*sin(q1) ; r1 = sqrt(r1x.^2 + r1y.^2) ; r2x = 500*cos(q1) + 250*cos(q1+q2) ; r2y = 500*sin(q1) + 250*sin(q1+q2) ; Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -148- Đồ án tốt nghiệp r2 r3x r3y r3 r4x r4y r4 r5x Tính tốn mơ mẫu động học robot RoPC01 = = = = = = = = sqrt(r2x.^2 + r2y.^2) ; 500*cos(q1) + 250*cos(q1+q2) + 500*sin(q1) + 250*sin(q1+q2) + sqrt(r3x.^2 + r3y.^2) ; 500*cos(q1) + cos(q1+q2).*q3 + 500*sin(q1) + sin(q1+q2).*q3 + sqrt(r4x.^2 + r4y.^2) ; 500*cos(q1) + cos(q1+q2).*q3 + 100*cos(q1+q2+q4).*cos(q5); r5y = 500*sin(q1) + sin(q1+q2).*q3 + 100*sin(q1+q2+q4).*cos(q5) ; r5z = 100*sin(q5) ; r5 = sqrt(r5x.^2 + r5y.^2 + r5z.^2) cos(q1+q2).*q3; sin(q1+q2).*q3 ; 100*cos(q1+q2+q4); 100*sin(q1+q2+q4) ; 200*cos(q1+q2+q4) + 200*sin(q1+q2+q4) + ; % BIEU THUC VAN TOC KHOI TAM CUA CAC KHAU v1x = diff(r1x); v1y = diff(r1y); v1 = sqrt(v1x.^2+v1y.^2); v2x = diff(r2x); v2y = diff(r2y); v2 = sqrt(v2x.^2+v2y.^2); v3x = diff(r3x); v3y = diff(r3y); v3 = sqrt(v3x.^2+v3y.^2); v4x = diff(r4x); v4y = diff(r4y); v4 = sqrt(v4x.^2+v4y.^2); v5x = diff(r5x); v5y = diff(r5y); v5z = diff(r5z); v5 = sqrt(v5x.^2+v5y.^2+v5z.^2); % DO THI CAC BIEN KHOP Q figure(1); plot(t(1:length(q1)),q1*180/pi); figure(2); plot(t(1:length(q2)),q2*180/pi); figure(3); plot(t(1:length(q3)),q3); figure(4); plot(t(1:length(q4)),q4*180/pi); figure(5); plot(t(1:length(q5)),q5*180/pi); % DO THI DAO HAM CAC BIEN KHOP Q figure(6); plot(t(1:length(q1_dh)),q1_dh); figure(7); plot(t(1:length(q2_dh)),q2_dh); figure(8); plot(t(1:length(q3_dh)),q3_dh); figure(9); plot(t(1:length(q4_dh)),q4_dh); figure(10); plot(t(1:length(q5_dh)),q5_dh); % DO THI VAN TOC GOC TUYET DOI CUA CAC KHAU figure(11); plot(t(1:length(w1)),w1*180/pi); Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -149- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 figure(12); plot(t(1:length(w2)),w2*180/pi); figure(13); plot(t(1:length(w3)),w3*180/pi); figure(14); plot(t(1:length(w4)),w4*180/pi); figure(15); plot(t(1:length(w5)),w5*180/pi); % DO THI TOA DO KHOI TAM CUA CAC KHAU figure(16); plot(t(1:length(r1)),r1); figure(17); plot(t(1:length(r2)),r2); figure(18); plot(t(1:length(r3)),r3); figure(19); plot(t(1:length(r4)),r4); figure(20); plot(t(1:length(r5)),r5); % DO THI VAN TOC KHOI TAM CUA CAC KHAU figure(21); plot(t(1:length(v1)),v1); figure(22); plot(t(1:length(v2)),v2); figure(23); plot(t(1:length(v3)),v3); figure(24); plot(t(1:length(v4)),v4); figure(25); plot(t(1:length(v5)),v5); figure(26); plot(t(1:length(w44)),w44*180/pi); Chương trình Matlap giải toán động học ngược, vẽ đồ thị vận tốc góc tuyệt đối, vector bán kính khối tâm vận tốc khối tâm điều khiển theo q3 % GIAI BAI TOAN NGUOC,VE DO THI CAC VAN TOC KHI DIEU KHIEN THEO Q3 clear all; clc ; global v n m ; t = 0:0.1:24 ; q1 = sym('q1(t)'); q2 = sym('q2(t)'); q3 = sym('q3(t)'); q4 = sym('q4(t)'); q5 = sym('q5(t)'); q12 = sym('q12(t)'); q12a = sym('q12a(t)'); a = sym('a(t)'); b = sym('b(t)'); e = sym('e(t)'); v = 50 ; n = 50 ; Ngô Trung Dũng K49 Lớp Cơ Điện Tử 1- -150- Đồ án tốt nghiệp Tính tốn mơ mẫu động học robot RoPC01 m = 150 ; % BIEU THUC CAC BIEN KHOP q5 = asin(m/200*sin(pi/n*v*t)) ; q3 = 900 - 10*t ; a = 0.176*v*t + 196.96*cos(q5) - 517.18 ; b = -v*t + 34.73*cos(q5) + 1227.73 ; alpha = atan2(a,b); e = (500^2 + a.*a + b.*b - q3.*q3)/1000 ; q1a = asin(e/sqrt(a.*a+b.*b)) ; q1 = q1a - alpha ; q12 = atan2(abs(b-500*sin(q1)),(a-500*cos(q1))) ; if q12