Đang tải... (xem toàn văn)
MỤC LỤCLỜI CẢM ƠN1MỤC LỤC2DANH MỤC KÝ HIỆU4DANH MỤC HÌNH ẢNH5DANH MỤC BẢNG BIỂU5MỞ ĐẦU61. Tính cấp thiết của đề tài62. Mục tiêu nghiên cứu73. Đối tượng, phạm vi nghiên cứu của đề tài74. Phương pháp nghiên cứu, ý nghĩa của đề tài75. Nội dung của đề tài7Chương 1 GIỚI THIỆU TỔNG QUAN91.1. Robotics91.1.1. Tự động hoá và robot công nghiệp91.1.2. Lịch sử phát triển của Robot111.1.3. Các khái niệm và định nghĩa về robot công nghiệp141.1.4. Cấu trúc cơ bản của robot công nghiệp181.1.5. Kết cấu của tay máy :191.1.6. Phân loại Robot công nghiệp221.2. Bài toán động học thuận231.2.1. Phương pháp DenavitHartenberg231.2.2. Phương pháp Craig251.4. Động lực học robot291.5. Phương pháp điều khiển PD bù trọng trường30Chương 2 ỨNG DỤNG TOOLBOX XÂY DỰNG MÔ HÌNH TOÁN HỌC VÀ MÔ PHỎNG CÁNH TAY ROBOT342.1. Cấu trúc động học của Robot342.2. Bài toán động học thuận342.2.1. Thiết lập hệ phương trình động học342.2.2 Tính toán động học thuận cánh tay robot372.2.3 Mô phỏng cánh tay robot382.3. Bài toán động học ngược402.3.1. Giải bài toán động học ngược bằng phương pháp giải tích402.3.2. Mô phỏng cánh tay robot422.4. Xây dựng mô hình toán học của robot công nghiệp442.4.1. Tọa độ các điểm trọng tâm của thanh nối trên hệ tạo độ O(x,y,z)442.4.2. Vận tốc dài tại các điểm trọng tâm của các thanh nối442.4.3. Vân tốc góc của các khớp452.4.4. Động năng của các khâu452.4.5. Thế năng của các khâu46Chương 3 ĐIỀU KHIỂN CÁNH TAY ROBOT BẰNG PHƯƠNG PHÁP PD BÙ TRỌNG TRƯỜNG493.1. Sơ đồ sử dụng thuật toán điều khiển chuyển động của cánh tay robot RRR trong không gian khớp49KẾT LUẬN VÀ KIẾN NGHỊ521. Kết quả đạt được:522. Những hạn chế:523. Hướng phát triển của để tài:524. Kiến nghị53PHỤ LỤC54
LỜI CẢM ƠN Đồ án tốt nghiệp hoàn thành lúc sinh viên năm cuối trường Đại học Sư phạm Kỹ thuật Hưng Yên chuẩn bị tốt nghiệp trường bước sang cánh cửa đời Sau bốn năm học tập rèn luyện, hẳn tích lũy cho kinh nghiệm học quý giá học tập sống, hành trang để sau phần nhỏ vào công xây dựng đất nước Để hoàn thành đồ án này, khơng có giúp đỡ động viên từ nhiều phía chắn chúng em khó đạt thành Đó hướng dẫn nhiệt tình thầy khoa Điện – Điên tử, thầy cô tổ môn đặc biệt thầy Đào Minh Tuấn, tạo điều kiện, giúp đỡ hướng dẫn tận tình chúng em nhiều trình tìm hiểu thực đề tài Các thầy ln khuyến khích mặt giúp chúng em có tinh thần thoải mái để làm việc Đó ủng hộ động viên vật chất tinh thần từ phía gia đình, bạn bè chỗ dựa vững giúp chúng em lấy lại động lực để tiếp tục hoàn thành gặp khó khăn Khơng có mà qua đồ án tốt nghiệp này, chúng em học hỏi trưởng thành lên nhiều từ thái độ công việc, tác phong làm việc, cộng tác nhóm hay cách xử lý tình huống… Chúng em xin chân thành cảm ơn! MỤC LỤC LỜI CẢM ƠN MỤC LỤC DANH MỤC KÝ HIỆU .4 DANH MỤC HÌNH ẢNH DANH MỤC BẢNG BIỂU MỞ ĐẦU .6 Tính cấp thiết đề tài Mục tiêu nghiên cứu .7 Đối tượng, phạm vi nghiên cứu đề tài .7 Phương pháp nghiên cứu, ý nghĩa đề tài Nội dung đề tài .7 Chương GIỚI THIỆU TỔNG QUAN .9 1.1 Robotics 1.1.1 Tự động hố robot cơng nghiệp 1.1.2 Lịch sử phát triển Robot 12 1.1.3 Các khái niệm định nghĩa robot công nghiệp 14 1.1.4 Cấu trúc robot công nghiệp 18 1.1.5 Kết cấu tay máy .19 1.1.6 Phân loại Robot công nghiệp 21 1.2 Bài toán động học thuận .22 1.2.1 Phương pháp Denavit-Hartenberg 22 1.2.2 Phương pháp Craig 25 1.4 Động lực học robot .29 1.5 Phương pháp điều khiển PD bù trọng trường 30 Chương ỨNG DỤNG TOOLBOX XÂY DỰNG MƠ HÌNH TỐN HỌC VÀ MƠ PHỎNG CÁNH TAY ROBOT 34 2.1 Cấu trúc động học Robot 34 2.2 Bài toán động học thuận .34 2.2.1 Thiết lập hệ phương trình động học 34 2.2.2 Tính tốn động học thuận cánh tay robot 37 2.2.3 Mô cánh tay robot 37 2.3 Bài toán động học ngược 40 2.3.1 Giải toán động học ngược phương pháp giải tích .40 2.3.2 Mơ cánh tay robot 42 2.4 Xây dựng mơ hình tốn học robot công nghiệp 44 2.4.1 Tọa độ điểm trọng tâm nối hệ tạo độ O(x,y,z) 44 2.4.2 Vận tốc dài điểm trọng tâm nối .44 2.4.3 Vân tốc góc khớp .45 2.4.4 Động khâu 45 2.4.5 Thế khâu 46 Chương ĐIỀU KHIỂN CÁNH TAY ROBOT BẰNG PHƯƠNG PHÁP PD BÙ TRỌNG TRƯỜNG 49 3.1 Sơ đồ sử dụng thuật toán điều khiển chuyển động cánh tay robot RRR không gian khớp 49 KẾT LUẬN VÀ KIẾN NGHỊ 53 Kết đạt được: 53 Những hạn chế: 54 Hướng phát triển để tài: 54 Kiến nghị 54 PHỤ LỤC 56 DANH MỤC KÝ HIỆU STT Ký hiệu Ý nghĩa C1 Cos ( q1 ) C2 Cos ( q2 ) C3 Cos ( q3 ) C22 Cos ( 2q2 ) C23 Cos ( q2 + q3 ) C223 Cos ( 2q2 + q3 ) C2223 Cos ( 2q2 + 2q3 ) S1 Sin ( q1 ) S2 Sin ( q2 ) 10 S3 Sin ( q3 ) 11 S 22 Sin ( 2q2 ) 12 S 23 Sin ( q2 + q3 ) 13 S 223 Sin ( 2q2 + q3 ) 14 S 2223 Sin ( 2q2 + 2q3 ) DANH MỤC HÌNH ẢNH DANH MỤC BẢNG BIỂU MỞ ĐẦU Tính cấp thiết đề tài Robot công nghiệp từ đời áp dụng nhiều lĩnh vực góc độ thay sức người Mục tiêu ứng dụng robot công nghiệp nhằm góp phần nâng cao suất, giảm giá thành, tăng chất lượng khả cạnh tranh sản phẩm đồng thời cải thiện điều kiện lao động Đạt mục tiêu nhờ vào khả to lớn robot như: làm việc mệt mỏi, chịu phóng xạ mơi trường làm việc độc hại, nhiệt độ cao Trong ngành khí, robot sử dụng nhiều cơng nghệ đúc, công nghệ hàn, cắt kim loại, sơn, phun phủ kim loại, tháo lắp vận chuyển phôi, lắp ráp sản phẩm Ngoài phân xưởng, nhà máy, kỹ thuật robot sử dụng việc khai thác thềm lục địa, đại dương, y học, quốc phòng, vũ trụ, công nghiệp nguyên tử lĩnh vực xã hội khác Rõ ràng khả làm việc robot số điều kiện cụ thể vượt trội khả người; phương tiện hữu hiệu để tự động hoá, nâng cao suất lao động, giảm nhẹ cho người cơng việc nặng nhọc độc hại Để có sản phẩm robot hồn thiện đòi hỏi kết hợp nghiên cứu nhiều lĩnh vực khoa học khác như: khí, điện, điện tử, kỹ thuật điều khiển, cơng nghệ thơng tin, Chính robot thực kết hợp hồn hảo cơng trình nghiên cứu đa lĩnh vực Cánh tay robot đối tượng sử dụng phổ biến mang lại hiệu sản xuất sinh hoạt Các toán động học, động lực học đề đnag đề cập rộng khắp nằm nâng cao chất lượng điều khiển thao yêu cầu, nâng cao tuổi thọ, độ bền cấu… Các toán điều khiển dành quan tâm nhà khoa học nghiên cứu nâng cao chất lượng cánh tay robot, giảm nhiễu tác động Chính vậy, xây dựng mơ hình tốn học, mơ cánh tay robot nâng cao chất lượng hệ thống điều khiển cánh tay robot hướng mà đề tài chọn để nghiên cứu Mục tiêu nghiên cứu Mục tiêu đề tài nghiên cứu ứng dụng Toolbox vào việc xây dựng mơ hình tốn học, mơ cánh tay robot điều khiển cánh tay robot phương pháp PD bù trọng trường Đối tượng, phạm vi nghiên cứu đề tài - Đối tượng nghiên cứu: Cánh tay robot RRR bậc tự - Phạm vi nghiên cứu: Nghiên cứu toán động học thuận, tốn động học ngược để mơ cánh tay robot Nghiên cứu toán động lực học để xây dựng mơ hình tốn học robot Nghiên cứu phương pháp điều khiển PD bù trọng trường để điều khiển cánh tay robot Phương pháp nghiên cứu, ý nghĩa đề tài - Phương pháp nghiên cứu: + Phương pháp nghiên cứu lý thuyết + Phương pháp nghiên cứu thực nghiệm - Ý nghĩa luận án: + Ý nghĩa khoa học: Luận án nghiên cứu toán động học động lực học để xây dựng mơ hình tốn học cánh tay robot, phương pháp PD bù trọng trường để điều khiển cánh tay robot + Ý nghĩa thực tiễn: Luận án mô cánh tay robot kiểm chứng ứng dụng thực tế thuật toán PD bù trọng trường luận án Toolbox Matlab Nội dung đề tài Bố cục đề tài bao gồm: Mở đầu Nội dung phần tính cấp thiết đề tài, mục tiêu đề tài, đối tương phạm vi nghiên cứu, phương pháp nghiên cứu, ý nghĩa luận án, nội dung đề tài Chương 1: Giới thiệu tổng quan Nội dung chương robotics, tốn động học thuận, toán động học ngược, toán động lực học, phương pháp điều khiển PD bù trọng trường Chương 2: Ứng dụng Toolbox xây dựng mơ hình tốn học mơ cánh tay robot Nội dung chương giải toán động học thuận, giải toán động học ngược, giải toán động lực học để xây dựng mơ hình tốn học canh tay robot mô cánh tay robot Chương 3: Ứng dụng phương pháp PD bù trọng trường để điều khiển cánh tay robot Nội dung chương mạch mô phương pháp PD bù trọng trường kết điều khiển cánh tay robot Chương 4:Kết luận Kết đạt được, hạn chế hướng phát triển đề tài Chương GIỚI THIỆU TỔNG QUAN 1.1 Robotics 1.1.1 Tự động hoá robot cơng nghiệp Thuật ngữ robot có nguồn gốc từ khoa học viễn tường Từ "Robota" lần xuất năm 1920 kịch nhà viết kịch người Tiệp Karel Capek, ơng mơ tả "nhân vật" ứng xử người, có khả làm việc khoẻ gấp đơi người, khơng có cảm tính, cảm giác người Sự phát triển lĩnh vực robot 40 năm sau Robot định nghĩa dạng khía cạnh khác Robot coi tay máy có vài bậc tự do, điều khiển máy tính Một định nghĩa khác robot công nghiệp chấp nhận là: robot cơng nghiệp cấu khí lập trình thực cơng việc có ích cách tự động khơng cần giúp đỡ trục tiếp người Hiệp hội nhà chế tạo nhà sử dụng đưa định nghĩa robot sau: Robot thiết bị thực chức bình thường người hợp tác cách thơng minh để có trí tuệ người Trong Bách khoa toàn thư (phiên 7.0 1995) viết: "Robot định nghĩa thiết bị tự điều khiển hoàn toàn bao gồm phận điện tử, điện khí, " Tự động hoá (Automation) kỹ thuật robot (Robotics) hai lĩnh vực có liên quan mật thiết với Về phương diện cơng nghiệp, tự động hố cơng nghệ liên kết với sử dụng hệ thống khí, điện tử hệ thống máy tính vận hành điều khiển q trình sản xuất Ví dụ, dây chuyền vận chuyển, máy lắp ráp khí, Các hệ thống điều khiển phản hồi, máy công cụ điều khiển chương trình số robot Như vậy, coi robot dạng thiết bị tự động hố cơng nghiệp Có ba loại thống tự động hố cơng nghiệp: Tự động hố cố định, tự động hố lập trình tự động hố linh hoạt Tự động hoá cố định sử dụng dây chuyền sản xuất với số lượng sản phẩm lớn, dó cần thiết kế thiết bị đặc biệt để sản xuất sản phẩm với số lượng lớn hiệu suất cao Công nghiệp sản xuất ơtơ coi ví dụ điến hình Tính kinh tế tự động hố cố định cao giá thành thiết bị chuyên dụng đuợc chia cho số lượng lớn đơn vị sản phẩm, dẫn đến giá thành đơn vị sản phẩm thấp so với phương pháp sản xưất khác Tuy nhiên vốn đầu tư hệ thống tự động hố cố định cao, số lượng sản phẩm nhỏ thiết kế, giá thành sản phẩm cao Mặt khác, thiết bị chuyên dùng thiết kế cho sản xuất loại sản phẩm, sau chu kỳ sản phẩm kết thúc, thiết bị chuyên dụng trở thành lạc hậu Tự động hóa lập trình sử dụng q trình sản xuất với sản phẩm đa dạng số lượng sản phẩm tương đối thấp Trong hệ thống tự động hoá này, trang thiết bị sản xuất thiết kế để thích nghi với dạng sản phẩm khác Chương trình lập trình đọc vào thiết bị sản xuất ứng với loại sản phẩm cụ thể Về khía cạnh kinh tế, giá thành trang thiết bị lập trình phân cho số lượng lớn sản phẩm, với loại sản phẩm khác nhâu Tự động hoá linh hoạt hệ thống sản xuất linh hoạt (FMS), hệ thống sản xuất tích hợp máy tính (hình 1.1) Ý tưởng dạng tự động hoá linh hoạt pháp triển áp dụng vào thực tế quãng 20-25 năm cho thấy phạm vi ứng dụng thích hợp q trình sản xuất có số lượng sản phẩm trung bình Dạng tự động hố linh hoạt bao gồm đặc điểm hai dạng tự động hoá cố định lập trình Nó cần lập trình cho loại sản phẩm khác nhau, số dạng sản phẩm khác hạn chế loại tự dộng hố lập trình Hê thống sản xuất bao gồm nhiều trạm làm việc đật nối tiếp dây chuyền Máy tính trung tâm hệ thống điều khiển trung tâm điều khiển thời trạm hoạt động 10 1 H 22 = I + a22 m2 + m3 ÷+ a2 a3 m3C3 4 H 23 = a3 m3 H 32 = a3 m3 H 33 = I3 + a32 m3 Thành phần tương hỗ V V1 ÷ V = V2 ÷ V ÷ 3 Trong đó: 1 V1 = −a22 m2 + m3 ÷S 22 q1 q2 − a32 m3 q2 + q3 ÷S 2223 q1 − a1a3m3 q2 + q3 ÷S 23 q1 4 − a2 a3 m3 2q2 + q3 ÷S 223 q1 − a1a2 ( 2m3 + m2 ) q2 S q1 1 1 V2 = − a2 a3 m3 S3 q2 q3 + a22 m2 + m3 ÷S 22 q12 + a32 m3 S2223 q12 2 8 1 1 + a1a3 m3 S 23 q12 + a2 a3m3 S 223 q12 + a1a2 m2 + m3 ÷S q12 2 2 1 1 V3 = − a2 a3m3 S3 q2 q3 + a32 m3 S 2223 q1 + a1a3m3 S 23 q12 + a2 a3m3 S 223 q12 1 + a2 a3 m3 S3 ( q12 + q22 + q2 q3 ) 2 Thành phần trọng trường G ÷ ÷ 1 ÷ G = gm2 a2C2 + gm3 a2C3 + a3C23 ÷÷ ÷ 2 ÷ gm3 a2C3 + a3C23 ÷ ÷ 48 Chương ĐIỀU KHIỂN CÁNH TAY ROBOT BẰNG PHƯƠNG PHÁP PD BÙ TRỌNG TRƯỜNG 3.1 Sơ đồ sử dụng thuật toán điều khiển chuyển động cánh tay robot RRR khơng gian khớp Hình 20 Sơ đồ khối điều khiển cánh tay robot RRR không gian khớp Từ phương trình động học robot, xác định giá trị biến khớp: q1 ÷ q = q2 ÷ q ÷ 3 Từ cơng thức &= H −1 (T − V − G ) q& T moment đặt vào khớp lấy từ đầu điều khiển H thành phần quán tính cánh tay robot 49 H11 H = H 22 H 32 ÷ H 23 ÷ H 33 ÷ Trong đó: 1 1 H11 = I1 + a12 m1 + m2 + m3 ÷+ a32 m3 + a22 m2 + m3 ÷C22 + a32 m3C2223 4 4 1 + a1a3 m3C23 + a2 a3 m3C223 + a1a2 ( m2 + 2m3 ) C2 + a2 a3 m3 2 1 H 22 = I + a22 m2 + m3 ÷+ a2 a3m3C3 4 H 23 = a3 m3 H 32 = a3 m3 H 33 = I + a32 m3 V thành phần tương hỗ cánh tay robot V1 ÷ V = V2 ÷ V ÷ 3 Trong đó: 1 V1 = −a22 m2 + m3 ÷S 22 q1 q2 − a32 m3 q2 + q3 ÷S 2223 q1 − a1a3m3 q2 + q3 ÷S 23 q1 4 − a2 a3 m3 2q2 + q3 ÷S 223 q1 − a1a2 ( 2m3 + m2 ) q2 S q1 1 1 V2 = − a2 a3 m3 S3 q2 q3 + a22 m2 + m3 ÷S 22 q12 + a32 m3 S2223 q12 2 8 1 1 + a1a3 m3 S 23 q12 + a2 a3m3 S 223 q12 + a1a2 m2 + m3 ÷S q12 2 2 50 1 1 V3 = − a2 a3m3 S3 q2 q3 + a32 m3 S 2223 q1 + a1a3m3 S 23 q12 + a2 a3m3 S 223 q12 1 + a2 a3 m3 S3 ( q12 + q22 + q2 q3 ) 2 G thành phần trọng trường cánh tay robot ÷ ÷ 1 ÷ G = gm2 a2C2 + gm3 a2C3 + a3C23 ÷÷ ÷ 2 ÷ gm3 a2C3 + a3C23 ÷ ÷ Lựa chọn hệ số khuếch đại tỷ lệ Kd: K1 Kd = 0 K2 0 ÷ ÷= K3 ÷ 65 ÷ 150 ÷ 0 100 ÷ Lựa chọn hệ số khuếch đạo hàm Kp K1 Kp = K2 0 65 0 ÷ ÷ ÷ = 150 ÷ K3 ÷ 100 ÷ 0 3.2 Kết mơ điều khiển cánh tay robot RRR - Góc quay khớp 51 Hình 21 Góc quay khớp - Góc quay khớp Hình 22 Góc quay khớp - Góc quay khớp 52 Hình 23 Góc quay khớp KẾT LUẬN VÀ KIẾN NGHỊ Kết đạt được: Sau thời gian tìm tòi, nghiên cứu lựa chọn hướng dẫn tận tình thầy giáo Đào Minh Tuấn đến đề tài chúng em hoàn thành Q trình nghiên cứu thực hiện, nhóm hoàn thành nội dung đề tài đề ra: - Giải toán động học thuận, động học ngược cánh tay robot RRR - Mô toán động học thuận, động học ngược cánh tay robot RRR - Xây dựng thành cơng mơ hình tốn học cánh tay robot RRR - Xây dựng sơ đồ khối điều khiển cánh tay robot RRR không gian khớp - Điều khiển cánh tay robot RRR phương pháp PD bù trọng trường Với cá nhân nhóm, sau thực xong đề tài có thêm nhiều kiến thức việc ứng dụng tiện ích số phần mềm phục vụ sống sinh hoạt nghề nghiệp 53 Những hạn chế: - Mô cánh tay robot RRR không gian 2D - Điều khiển cánh tay robot RRR chưa đạt tối ưu Hướng phát triển để tài: Chúng em nhận thấy đề tài có tính ứng dụng cao, kiến thức cần tìm hiểu mẻ với đối tượng sinh viên Do nhiều nguyên nhân mặt khách quan chủ quan mà phần lớn giới hạn thời gian góc độ nghiên cứu sinh viên mà hệ thống chưa đạt hết mục tiêu đặt ban đầu Chúng em xin đưa hướng phát triển cho đề tài sau: - Thiết kế cánh tay robot - Điều khiển thích nghi cánh tay robot Kiến nghị Do thời gian ngắn cộng với trình độ nhóm thực hạn chế, kiến thức chúng em chưa có nhiều ứng dụng vào thực tế nên đề tài giải số vấn đề tránh khỏi số thiếu sót Hy vọng đồ án tài liệu tham khảo cho bạn sinh viên khóa sau quan tâm để hoàn thiện cho hệ thống bước đưa vào ứng dụng thực tế Cuối chúng em xin chân trành cảm ơn thầy cô bạn bè đặc biệt thầy giáo Đào Mạnh Tuấn đẫ giúp đỡ chúng em hoàn thành đề tài Chúng em mong nhận đóng góp ý kiến thầy giáo tồn bạn để đề tài chúng em hoàn thiện Chúng em xin chân thành cảm ơn! 54 55 PHỤ LỤC Chương trình gải tốn động học thuận, động học ngược, động lực học clear all % xoa tat ca cac bien co o Workspace clc % xoa moi dong tren Command Window startup_rvc % khoi dong robotics Toolbox ( file startup_rvc cua robotics toolbox phai nam o %thu muc hien thoi cua matlab( Current Directory cua Matlab) % khai bao bien syms q1 q2 q3 a1 a2 a3 dq1 dq2 dq3 t g m1 m2 m3 real % q1 q2 q3 la cac bien khop % a1 a2 a3 la dai cac % dq1 dq2 dq3 la dao ham cua q1 q2 q3 % m1 m2 m3 la khoi luong cac % t la bien thoi gian % g la bien gia toc truong % vector toa suy rong q % q1 q2 q3 a1 a2 a3 dq1 dq2 dq3 t g deu la cac bien thuc q = [q1;q2;q3]; % vector van toc dai dq = [dq1;dq2;dq3]; % nhap ma tran D-H A_01=[ cos(q1) sin(q1) a1*cos(q1); sin(q1) -cos(q1) a1*sin(q1); 0;0 0 1]; A_12=[ cos(q2) -sin(q2) a2*cos(q2); sin(q2) cos(q2) a2*sin(q2); 0 0;0 0 1]; A_23=[ cos(q3) -sin(q3) a3*cos(q3); sin(q3) cos(q3) a3*sin(q3); 0 0;0 0 1]; % Tinh cac ma tran truyen A_02 = simplify(A_01*A_12); % Sau tinh toan ta thu gon ket qua bang lenh simplify R_02 = A_02(1:3,1:3); disp('Ma tran chuyen tu khau sang khau tac dong cuoi la') A_03 = simplify(A_01*A_12*A_23) R_03 = A_03(1:3,1:3); disp(' ') % Tao mô hình Robot Robotics toolbox L(1)=Link([0,0,5,pi/2,0]); % Lenh Link tao mot khâu cua Robot L(2)=Link([0,0,3,0,0]); L(3)=Link([0,0,2,0,0]); rob=SerialLink(L) % Lenh SerialLink(L) tao mot robot noi tiep gom khâu cua L % Giai bai toan dong hoc thuan disp('Giai bai toan dong hoc thuan') %1 tim phuong trinh dong hoc 56 Nx = A_03(1,1) Ny = A_03(2,1) Nz = A_03(3,1) Ox = A_03(1,2) Oy = A_03(2,2) Oz = A_03(3,2) Ax = A_03(1,3) Ay = A_03(2,3) Az = A_03(3,3) Px = A_03(1,4) Py = A_03(2,4) Pz = A_03(3,4) % tim vi tri va tinh van toc dai cua khau thao tac cuoi rE = A_03(1:3,4) % Vector toa khau thao tac cuoi v_qE = simplify(jacobian(rE,q)*dq) % Tinh vector van toc khau tac dong cuoi %3 tinh van toc goc cua cac khau thao tac R_0E = A_03 (1:3,1:3) diff_R_0E = diff(R_0E,q1)*dq1+diff(R_0E,q2)*dq2+diff(R_0E,q3)*dq3; %Tinh dao ham cua R omega_curve = diff_R_0E*R_0E.'; omega_curve = simplify(omega_curve) disp('Van toc goc:') omega = [omega_curve(3,2) omega_curve(1,3) omega_curve(2,1)] % Thay so bai toan dong hoc thuan disp(' ') disp('Thay so') % Khi thay so ta su dung lenh subs sub_rE = simplify(subs(rE,{q1 q2 q3 a1 a2 a3},{3*t 2*t t 2})) disp('Van toc dai:') sub_diff_qE = simplify(subs(v_qE,{q1 q2 q3 dq1 dq2 dq3 a1 a2 a3},{3*t 2*t t 2})) disp('Van toc goc:') sub_R_0E = simplify(subs(R_0E,{a1 a2 a3},{5 2})); sub_omega = simplify(subs(omega,{q1 q2 q3 dq1 dq2 dq3 a1 a2 a3},{3*t 2*t t 2})) % Tính tốn ve thi time=0:0.02:pi/12; num_rE=zeros(3,length(time)); for j=1:length(time) num_rE(:,j) = subs(sub_rE,t,time(j)); end figure(1) % tao moi hinh anh thi clf % xoa figure title('Quy dao cua khau tac dong cuoi bai toan thuan') 57 % ghi nhan cho thi hold on % giu ham thi grid on % hien thi luoi toa axis([-10, 10, -10, 10 ,-5, 5]) % dat cac thong so cho truc for j=1:length(time) plot3(num_rE(1,j),num_rE(2,j),num_rE(3,j),'b+'); % Ve quy dao chuyen dong bang MATLAB plot(rob,[3*time(j),2*time(j),time(j)]); % Ve hình anh chuyen dong 3D cua Robot theo quy dao pause(1/30) % Dùng lenh pause de tao cam giac giong nhu mot doan phim end % Giai bai toan dong hoc nguoc figure(2) clf hold on grid on rE_solve = subs(rE,{a1 a2 a3},{5 2}); time=0:0.1:2*pi/3; j=length(time); q1_num=zeros(2,j); q2_num=zeros(2,j); q3_num=zeros(2,j); xE = zeros(1,j);yE = zeros(1,j);zE = zeros(1,j); for i=1:j xE(i) = (2*cos((3*time(i))/2)^2 - 1)*(4*cos((3*time(i))/2)^2 + 6*cos(time(i))^2); yE(i) = sin(3*time(i))*(4*cos((3*time(i))/2)^2 + 6*cos(time(i))^2); zE(i) = 3*sin(2*time(i)) + 2*sin(3*time(i)); f1 = rE_solve(1,1)-xE(i); % phuong trinh dong hoc Robot f2 = rE_solve(2,1)-yE(i); f3 = rE_solve(3,1)-zE(i); f = [f1 f2 f3]; [q1_num(:,i) q2_num(:,i) q3_num(:,i)] = solve(f1,f2,f3); % Dùng lenh solve() de tim nghiem cua he phuong trinh dong hoc end subplot(311) plot(time,q1_num(1,:)) grid subplot(312) plot(time,q2_num(1,:)) grid subplot(313) 58 grid plot(time,q3_num(1,:)) grid figure(3) clf title('Quy dao cua khau tac dong cuoi bai toan nguoc') hold on grid on axis([-10, 10, -10, 10 ,-5, 5]) pause close(2) for i=1:length(time) plot(rob,[q1_num(1,i),q2_num(1,i),q3_num(1,i)]) plot3(xE(i),yE(i),zE(i),'b+') pause(1/5) end % Giai toán dong luc hoc(Solve the dynamics problem) syms d1 a1 a2 a3 m1 m2 m3 g t I1 I2 I3 q1=sym('q1(t)'); q2=sym('q2(t)'); q3=sym('q3(t)'); c1 = cos(q1); s1 = sin (q1); c2 = cos(q2); s2 = sin(q2); c3 = cos(q3); s3 = sin(q3); c23 = cos(q2 +q3); s23 = sin(q2+q3); A_01=[ c1 s1 a1*c1; s1 -c1 a1*s1; 0;0 0 1] A_12=[ c2 -s2 a2*c2; s2 c2 a2*s2; 0 0;0 0 1]; A_23=[ c3 -s3 a3*c3; s3 c3 a3*s3; 0 0;0 0 1]; A_02 = simplify(A_01*A_12) A_03 = simplify(A_01*A_12*A_23) % toa tam khop r1 = [a1/2*c1; a1/2*s1; 0] % toa tam khop r2 = [a2/2*c1*c2; a2/2*s1*c2; a2/2*s2] + [a1*c1; a1*s1; 0] % toa tam khop r3 = [a3/2*c1*c23; a3/2*s1*c23; a3/2*s23] + [a2*c1*c2; a2*s1*c2; a2*s2] + [a1*c1; a1*s1; 0] % van toc dai khop V1 = diff(r1,t); 59 V1 = V1(1:3) % van toc dai khop V2 = diff(r2,t); V2 = V2(1:3) % van toc dai khop V3 = diff(r3,t); V3 = V3(1:3) % van toc goc cua khop w1 = [0; 0; diff(q1,t)] % van toc goc cua khop w2 = [0; 0; diff(q2,t)] % van toc goc cua khop w3 = [0; 0; diff(q3,t)] % dong nang khau K1 = 1/2*(V1.'*m1*V1 + w1.'*I1*w1) % dong nang khau K2 = 1/2*(V2.'*m2*V2 + w2.'*I2*w2) % dong nang khau K3 = 1/2*(V3.'*m3*V3 + w3.'*I3*w3) % the nang cua khau 1: P1 = m1*g*r1(3,1) % the nang cua khau 2: P2 = m2*g*r2(3,1) % the nang cua khau 3: P3 = m3*g*r3(3,1) % tong dong nang cac khau K = K1 + K2 + K3 % tong the nang cac khau P =P1 + P2 + P3 % ham lagrange cua robor RRR duoc tinh L=K-P % momen dat len khop dq1 = diff(q1,t) Z1 = diff(L,dq1) U1 = diff(Z1,t) O1 = diff(L,q1) M1 = simple(U1 - O1) % momen dat len khop dq2 = diff(q2,t) Z2 = diff(L,dq2) U2 = diff(Z2,t) O2 = diff(L,q2) M2 = simple(U2 - O2) 60 % momen dat len khop dq3 = diff(q3,t) Z3 = diff(L,dq3) U3 = diff(Z3,t) O3 = diff(L,q3) M3 = simple(U3 - O3) 61 TÀI LIỆU THAM KHẢO [1] Gs Tskh Nguyễn Văn Khang, Ts Chu Anh Mỳ, (2011), “Cơ sở robot công nghiệp”, NXB GDVN, pp 39 – 58 [2] Ths Nguyễn Hồng Hải, Ths Nguyễn Việt Anh, (2006), “Lập trình Matlab ứng dụng”, NXB KHKT, Hà Nội [3] Phạm Đăng Phước, (2006), “Robot công nghiệp”, NXB KHKT, Hà Nội [4].Ts Nguyễn Mạnh Tiến,(2007), “Điều khiển robot công nghiệp”, NXB KHKT, Hà Nội, pp 7- 12, pp 166-169 [5] Peter Corke, (2011),“Robotics, Vision and Control fundamental algoritthms in Matlab” http://www.mathworks.com https://sites.google.com/site/matlab4vn 62 ... tốn điều khiển ln dành quan tâm nhà khoa học nghiên cứu nâng cao chất lượng cánh tay robot, giảm nhiễu tác động Chính vậy, xây dựng mơ hình tốn học, mơ cánh tay robot nâng cao chất lượng hệ thống. .. thống điều khiển cánh tay robot hướng mà đề tài chọn để nghiên cứu Mục tiêu nghiên cứu Mục tiêu đề tài nghiên cứu ứng dụng Toolbox vào việc xây dựng mơ hình tốn học, mơ cánh tay robot điều khiển cánh. .. động học ngược để mô cánh tay robot Nghiên cứu tốn động lực học để xây dựng mơ hình toán học robot Nghiên cứu phương pháp điều khiển PD bù trọng trường để điều khiển cánh tay robot Phương pháp