Thông tin tài liệu
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ ──────── * ─────── BÀI TẬP MƠN HỌC TÍNH TỐN THIẾT KẾ ROBOT NGÀNH CƠ ĐIỆN TỬ LỚP: NUT14 Giáo viên hướng dẫn: PGS.TS Phan Bùi Khôi Sinh viên thực hiện: Nguyễn Mạnh Tiến-20158374 Trần Hồng Minh-20158263 Nhóm Page Mục Lục Chương 1: PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC 1.1 Phân tích mục đích ứng dụng robot: (Robot hàn hồ quang) 1.2 Phân tích yêu cầu kỹ thuật thao tác: .4 a Đối tượng thao tác, dạng thao tác b Yêu cầu vị trí: Yêu cầu xác vị trí c Hướng khâu thao tác: d Yêu cầu vận tốc, gia tốc thao tác: .4 e Yêu cầu không gian thao tác: 1.3 Xác định đặc trưng kỹ thuật: a Số bậc tự cần thiết: b Vùng làm việc với tới: .4 c Yêu cầu tải trọng: 1.4 Các phương án thiết kế cấu trúc robot, cấu trúc khâu khớp, phân tích, chọn phương án thực hiện: 1.5 Phân tích thiết kế cấu trúc chọn: .6 CHƯƠNG 2: THIẾT KẾ 3D MƠ HÌNH ROBOT 2.1 Thiết kế 3D 2.2 Lập vẽ 2D CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 8 16 17 3.1 Khảo sát động học thuận, khảo sát động học ngược 17 3.2 Thiết kế quỹ đạo chuyển động robot theo mục đích ứng dụng 25 CHƯƠNG 4: PHÂN TÍCH TRẠNG THÁI (TĨNH) YÊU CẦU LỰC/MOMEN ĐỘNG CƠ LỚN NHẤT 30 CHƯƠNG 5: TÍNH TỐN ĐỘNG LỰC HỌC 39 5.1 Thiết lập phương trình vi phân chuyển động robot phương pháp học 39 Nhóm Page CHƯƠNG 6: THIẾT KẾ DẪN ĐỘNG ROBOT 52 6.1 Thiết kế hệ dẫn động (cho khớp) .52 6.2 Chọn động phù hợp .55 6.3 Tính chọn hộp giảm tốc 57 CHƯƠNG 7: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 59 7.1 Chọn luật điều khiển phù hợp, thiết kế mơ hình điều khiển……59 7.2 Mơ Matlab…………………………………………60 7.3 Code khối MATLAB Function………………………70 CHƯƠNG 8: KHẢO SÁT ĐỘ CHÍNH XÁC, ĐÁNH GIÁ ĐẶC TÍNH KỸ THUẬT VÀ HIỆU CHỈNH THIẾT KẾ 80 8.1 Hệ số Kp, Kd khâu………………………………… 80 8.2 Đồ thị biểu biến sai số………………………………….80 TÀI LIỆU THAM KHẢO 84 LỜI NÓI ĐẦU Cơ điện tử ngành công nghệ mũi nhọn kỉ 21 Ở Việt Nam vòng 10 năm trở lại đây, đặc biệt năm tới, xuất nhu cầu lớn đào tạo nhân lực điện tử Trong nghiệp cơng nghiệp hóa, đại hóa đất nước vấn đề tự động hóa có vai trị đặc biệt quan trọng Nhóm Page 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, v.v…vấn đề đặt hệ thống sản xuất phải có tính linh hoạt cao Robot cơng nghiệp, đặc biệt tay máy robot phận quan trọng để tạo hệ thống Tay máy Robot có mặt sản xuất từ nhiều năm trước, ngày tay máy Robot dùng nhiều lĩnh vực sản xuất, xuất phát từ ưu điểm mà tay máy Robot đúc kết lại trình sản xuất làm việc, tay máy 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, v.v…Do việc đầu tư nghiên cứu, chế tạo tay máy Robot phục vụ cho cơng tự động hóa sản xuất cần thiết cho tương lai Việc tìm hiểu nghiên cứu Robot khuôn khổ môn học Robotic tính tốn thiết kế Robot sở để chúng em tính tốn, thiết kế điều khiển loại Robot công nghiệp phục vụ sản xuất Cụ thể, chúng em chọn đề tài tính tốn, thiết kế mơ hình Robot ứng dụng bốc xếp kho hàng Nhóm Page CHƯƠNG 1: PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC 1.1 Phân tích mục đích ứng dụng robot: (Robot bốc xếp kho hàng) Robot bốc xếp kho hàng công nghiệp thay cho người lao động, tăng suất lao động 1.2 Phân tích yêu cầu kỹ thuật thao tác: a Đối tượng thao tác, dạng thao tác: Đối tượng thao tác: thùng hang 40x25x30 (cm3), nặng 20kg Dạng thao tác: kẹp chặt, di chuyển vị trí b Yêu cầu vị trí: Yêu cầu xác vị trí, phương chiều (vì để gắp vật cần xác vị trí, phương chiều khâu cuối tiếp xúc với vật gắp) c Hướng khâu thao tác: Khâu thao tác có hướng đảm bảo đầu kẹp di chuyển đến vị trí cần gắp hàng thực thao tác kẹp d Yêu cầu vận tốc, gia tốc thao tác: Tốc độ, gia tốc: điểm gắp thả vật, vận tốc gia tốc 0, vận tốc, gia tốc thay đổi chuyển điểm cần hàn e Yêu cầu không gian thao tác: Robot di chuyển linh hoạt không gian thao tác để đến vị trí cần gắp vật với phương chiều hợp lý đến vị trí cần thả vật với phương chiều hợp lý 1.3 Xác định đặc trưng kỹ thuật: a Số bậc tự cần thiết: Robot có bậc tự để đến vị trí gắp thả vật, cần thêm khớp xoay để đảm bảo lực kẹp chặt kẹp phương vng góc với mt hp Nhúm Page BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư b Vựng lm việc với tới: Được xác định thơng số hình học khâu Robot Robot cần di chuyển không gian, nhằm đưa đầu hàn đến vị trí cần thiết thực thao tác gắp thả vật c Yêu cầu tải trọng: Robot có tải trọng phù hợp đảm bảo độ cứng vững thao tác, để thao tác xác 1.4 Các phương án thiết kế cấu trúc robot, cấu trúc khâu khớp, phân tích, chọn phương án thực hiện: Nhóm Page BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư 1.5 Thụng s k thut: robot thiết kế, đối tượng hệ thống thao tác: Nhóm Page BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư Sau phõn tớch, cấu trúc đề xuất, ta thấy cấu trúc đầu có bậc tự do, linh hoạt, không tối gọn việc bốc xếp kho hàng Tuy nhiên cấu trúc thứ với bậc tự linh hoạt, gọn hơn, tốt khơng gian làm việc Chính nhóm lựa chọn cấu trúc thứ 2: Robot bậc tự do, gồm khớp quay để đảm bảo linh hoạt, tốn không gian thao tác lúc bốc xếp kho hàng Khớp khớp quay nối khâu (bệ máy) khâu Khớp khớp quay nối khâu khâu Khớp khớp quay nối khâu khâu Khớp khớp quay nối khâu khâu Khớp khớp quay nối khâu khâu (khâu thao tác) Khâu dài 950 mm, nặng m1=199,366 kg Khâu dài 800 mm, nặng m2=31,744 kg Khâu dài 700 mm, nặng m3=29,766 kg Khâu dài 270 mm, nặng m4=6,256 kg Khâu dài 250 mm, nặng m5=2,892 kg Nhóm Page BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư Phn tớnh chn ng c v bỏnh rng truyền chuyển động thông số chi tiết cho kết cấu trình bày kĩ phần Chương Sau lựa chọn kết cấu chọn sơ khâu, khớp tiến hành giải toán động học, động lực học, chọn động cơ, mơ CHƯƠNG 2: THIẾT KẾ 3D MƠ HÌNH ROBOT 2.1 Thiết kế 3D Robot bốc xếp kho hàng bc t Nhúm Page BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư Nhúm Page BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư 7.3 Code cỏc khối MATLAB Function: -Khối MATLAB Function DK_Momen function U = PTVPCD(t) d1=0.95; a2=0.8; a3=0.7; d5=0.47; l4=0.27; l5=0.25; m1=119.366; m2=31.744; m3=29.766; m4=6.256; m5=2.892; g=9.81; Fz=20*g; %Dau vao q = t(1:5); dq = t(6:10); u = t(11:15); % Ma tran khoi luong cua he m11=(1/12)*((-6*d5^2+6*d5*l5-2*l5^2)*m52*m4*l4^2)*cos(2*q(4)+2*q(3)+2*q(2))- (1/2)*((-2*d5+l5)*m5+m4*l4)*a3*sin(q(4)+2*q(3)+2*q(2))+(1/2)*a3^2* (m4+m5+(1/3)*m3)*cos(2*q(3)+2*q(2))-(1/2)*((-2*d5+l5)*m5+m4*l4)*a2* sin(q(4)+q(3)+2*q(2))+a2*a3*(m4+m5+(1/2)*m3)*cos(q(3)+2*q(2))+(1/2)* (m4+m5+(1/3)*m2+m3)*a2^2*cos(2*q(2))-(1/2)*((2*d5+l5)*m5+m4*l4)*a2* sin(q(4)+q(3))+a2*a3*(m4+m5+(1/2)*m3)*cos(q(3))-(1/2)*((2*d5+l5)*m5+m4* l4)*a3*sin(q(4)) +(1/12)*(6*m4+6*m5+2*m2+6*m3)*a2^2+(1/12)*(6*a3^2+6* d5^2-6*d5*l5+2*l5^2)*m5+(1/12)*(6*m4+2*m3)*a3^2+(1/6)*m4*l4^2+ (1/12)*m1*d1^2; m12 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m13 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m14 =-(1/24)*(l4^2*m4+l5^2*m5)*(cos(2*q(4)+2*q(3)+2*q(2))-1); m15 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m21 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; Nhúm Page 72 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư m22 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+2*a2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(3)+q(4)) +a2* a3*(m3+2*m4+2*m5)*cos(q(3))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)* sin(q(4))+(1/24)*(24*a2^2+24*a3^2+24*d5^224*d5*l5+7*l5^2)*m5+(1/24)* (8*m2+24*m3+24*m4)*a2^2+(1/24)*(7*m3+24*m4)*a3^2+7*m4*l4^2*(1/24); m23 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+a2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(3)+q(4))+ (1/2)*a2*a3*(m3+2*m4+2*m5)*cos(q(3))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)* m4*l4)*sin(q(4))+(1/24)*(7*m3+24*m4+24*m5)*a3^2+(1/24)* (24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m24 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a2*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(3)+q(4))+a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)* sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m25 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m31 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m32 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+a2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(3)+q(4))+ (1/2)*a2*a3*(m3+2*m4+2*m5)*cos(q(3))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)* m4*l4)*sin(q(4))+(1/24)*(7*m3+24*m4+24*m5)*a3^2+(1/24)* (24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m33 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(4))+ (1/24)*(7*m3+24*m4+24*m5)*a3^2+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m34 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a3*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m35 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); Nhúm Page 73 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư m41 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a2*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(3)+q(4))+a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)* sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m42 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a3*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m43 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a3*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m44 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+(1/24)*(24*d5^224* d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m45 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m51 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m52 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m53 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m54 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m55 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); M = [m11, m12, m13, m14, m15; m21, m22, m23, m24, m25; m31, m32, m33, m34, m35; m41, m42, m43, m44, m45; m51, m52, m53, m54, m55]; % Ma tran Coriolis*dq c1 = 0; c2 = (1/24)*(((-12*d5^2+12*d5*l5-4*l5^2)*m5-4*m4*l4^2)*dq(1)^2+(-2*l5^2* (dq(2)+dq(3)+dq(4)+dq(5))*m5-2*l4^2*m4*(dq(2)+dq(3)+dq(4)))*dq(1)l5^2* (dq(2)+dq(3)+dq(4)+dq(5))^2*m5l4^2*m4*(dq(2)+dq(3)+dq(4))^2)*sin(2*q(4)+2*q(3)+2*q(2)) -((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*a3*dq(1)^2*cos(q(4)+2*q(3)+2*q(2)) +(1/6)* a3^2*((3*m4+3*m5+m3)*dq(1)^2+(1/2)*m3*(dq(2)+dq(3))*dq(1)+(1/4)*m3* (dq(2)+dq(3))^2)*sin(2*q(3)+2*q(2))+(1/6)*a2*(((6*d5+3*l5)*m5+3*m4*l4)* Nhúm Page 74 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư cos(q(4)+q(3)+2*q(2))+3*a3*(m3+2*m4+2*m5)*sin(q(3)+2*q(2)) +a2*sin(2*q(2))* (3*m4+3*m5+m2+3*m3))*dq(1)^2; c3 = (1/24)*(((-12*d5^2+12*d5*l5-4*l5^2)*dq(1)^22*l5^2*(dq(2)+dq(3)+dq(4)+dq(5)) *dq(1)-l5^2*(dq(2)+dq(3)+dq(4)+dq(5))^2)*m5m4*l4^2*(4*dq(1)^2+(2*dq(2)+2*dq(3)+2* dq(4))*dq(1)+(dq(2)+dq(3)+dq(4))^2))*sin(2*q(4)+2*q(3)+2*q(2))a3*dq(1)^2*((-(1/2)*l5 +d5)*m5-(1/2)*m4*l4)*cos(q(4)+2*q(3)+2*q(2)) +(1/24)*a3^2*(12*dq(1)^2*m5+ (4*m3+12*m4)*dq(1)^2+2*m3*(dq(2)+dq(3))*dq(1)+m3*(dq(2)+dq(3))^2)*sin(2 *q(3)+2*q(2)) +(1/2)*(-dq(1)^2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*cos(q(4)+q(3)+2*q(2))+ (1/2)*a3*dq(1)^2*(m3+2*m4+2*m5)*sin(q(3)+2*q(2))(2*((1/2)*dq(1)^2+dq(2)* (dq(2)+dq(3)+dq(4))))*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*cos(q(3)+q(4))+a3* sin(q(3))*(m3+2*m4+2*m5)*((1/2)*dq(1)^2+dq(2)*(dq(2)+dq(3))))*a2; c4 = (1/24)*(((-dq(2)^2+(-2*dq(3)-2*dq(4)-2*dq(5)-2*dq(1))*dq(2)-4*dq(1)^2+(2*dq(3)- 2*dq(4)-2*dq(5))*dq(1)-(dq(3)+dq(4)+dq(5))^2)*l5^2+12*d5*l5*dq(1)^212*dq(1)^2*d5^2) *m5m4*l4^2*(dq(2)^2+(2*dq(3)+2*dq(4)+2*dq(1))*dq(2)+4*dq(1)^2+(2*dq(3)+2*dq (4))* dq(1)+(dq(3)+dq(4))^2))*sin(2*q(4)+2*q(3)+2*q(2))((1/2)*cos(q(4)+2*q(3)+2*q(2))*a3* dq(1)^2+(1/2)*a2*cos(q(4)+q(3)+2*q(2))*dq(1)^2+(dq(2)^2+ (dq(3)+dq(4))*dq(2)+(1/2)* dq(1)^2)*a2*cos(q(4)+q(3)) +a3*cos(q(4))*(dq(2)^2+(2*dq(3)+dq(4))*dq(2)+(1/2)*dq(1)^2+ dq(3)*(dq(3)+dq(4))))*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4); c5 =0; C=[c1;c2;c3;c4;c5]; %Ma tran luc co the g1 =0; g2 =(1/2)*m2*g*a2*cos(q(2))+m3*g*((1/2)*a3*cos(q(2)+q(3))+a2*cos(q(2))) Nhúm Page 75 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư +m4*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l4+a3*cos(q(2)+q(3))+a2*cos(q(2))) +m5*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l5+sin(q(2)+q(3)+q(4))*d5+a3* cos(q(2)+q(3))+a2*cos(q(2))); g3 =(1/2)*m3*g*a3*cos(q(2)+q(3))+m4*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l4+ a3*cos(q(2)+q(3)))+m5*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l5+sin(q(2)+ q(3)+q(4))*d5+a3*cos(q(2)+q(3))); g4 =(1/2)*m4*g*sin(q(2)+q(3)+q(4))*l4+m5*g*(-(1/2)*sin(q(2)+q(3)+q(4)) *l5+sin(q(2)+q(3)+q(4))*d5); g5 =0; G = [g1; g2; g3; g4; g5]; %Ma tran luc khong the Q1=0; Q2=Fz*((-(1/2)*l5+d5)*sin(q(2)+q(3)+q(4))+a3*cos(q(2)+q(3))+a2*cos(q(2))); Q3=Fz*((-(1/2)*l5+d5)*sin(q(2)+q(3)+q(4))+a3*cos(q(2)+q(3))); Q4=Fz*(-(1/2)*l5+d5)*sin(q(2)+q(3)+q(4)); Q5=0; Q=[Q1;Q2;Q3;Q4;Q5]; %Phuong trinh vi phan chuyen dong U = M*[u(1);u(2);u(3);u(4);u(5)]+C+G+Q; -Khối MATLAB Function Robot RRRRR function ddq = donghocnguoc(t) d1=0.95; a2=0.8; a3=0.7; d5=0.47; l4=0.27; l5=0.25; m1=119.366; m2=31.744; m3=29.766; m4=6.256; m5=2.892; g=9.81; Fz=20*g; %Dau vao q = t(1:5); dq = t(6:11); U = t(11:15)'; % Ma tran khoi luong cua he m11=(1/12)*((-6*d5^2+6*d5*l5-2*l5^2)*m52*m4*l4^2)*cos(2*q(4)+2*q(3)+2*q(2))- (1/2)*((-2*d5+l5)*m5+m4*l4)*a3*sin(q(4)+2*q(3)+2*q(2))+(1/2)*a3^2* (m4+m5+(1/3)*m3)*cos(2*q(3)+2*q(2))-(1/2)*((-2*d5+l5)*m5+m4*l4)*a2* sin(q(4)+q(3)+2*q(2))+a2*a3*(m4+m5+(1/2)*m3)*cos(q(3)+2*q(2))+(1/2)* Nhúm Page 76 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư (m4+m5+(1/3)*m2+m3)*a2^2*cos(2*q(2))-(1/2)*((2*d5+l5)*m5+m4*l4)*a2* sin(q(4)+q(3))+a2*a3*(m4+m5+(1/2)*m3)*cos(q(3))-(1/2)*((2*d5+l5)*m5+m4* l4)*a3*sin(q(4)) +(1/12)*(6*m4+6*m5+2*m2+6*m3)*a2^2+(1/12)*(6*a3^2+6* d5^2-6*d5*l5+2*l5^2)*m5+(1/12)*(6*m4+2*m3)*a3^2+(1/6)*m4*l4^2+ (1/12)*m1*d1^2; m12 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m13 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m14 =-(1/24)*(l4^2*m4+l5^2*m5)*(cos(2*q(4)+2*q(3)+2*q(2))-1); m15 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m21 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m22 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+2*a2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(3)+q(4)) +a2* a3*(m3+2*m4+2*m5)*cos(q(3))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)* sin(q(4))+(1/24)*(24*a2^2+24*a3^2+24*d5^224*d5*l5+7*l5^2)*m5+(1/24)* (8*m2+24*m3+24*m4)*a2^2+(1/24)*(7*m3+24*m4)*a3^2+7*m4*l4^2*(1/24); m23 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+a2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(3)+q(4))+ (1/2)*a2*a3*(m3+2*m4+2*m5)*cos(q(3))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)* m4*l4)*sin(q(4))+(1/24)*(7*m3+24*m4+24*m5)*a3^2+(1/24)* (24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m24 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a2*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(3)+q(4))+a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)* sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m25 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); Nhóm Page 77 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư m31 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+(1/24)*m3*a3^2+(1/24)*m4*l4^2+(1/24)*m5*l5^2; m32 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+a2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(3)+q(4))+ (1/2)*a2*a3*(m3+2*m4+2*m5)*cos(q(3))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)* m4*l4)*sin(q(4))+(1/24)*(7*m3+24*m4+24*m5)*a3^2+(1/24)* (24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m33 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2)) +(1/24)*m3*a3^2* cos(2*q(3)+2*q(2))+2*a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*sin(q(4))+ (1/24)*(7*m3+24*m4+24*m5)*a3^2+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m34 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a3*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m35 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m41 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a2*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(3)+q(4))+a3*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)* sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m42 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a3*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m43 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+a3*(((1/2)*l5+d5)* m5-(1/2)*m4*l4)*sin(q(4))+(1/24)*(24*d5^2-24*d5*l5+7*l5^2)* m5+7*m4*l4^2*(1/24); m44 =(1/24)*(-l4^2*m4-l5^2*m5)*cos(2*q(4)+2*q(3)+2*q(2))+(1/24)*(24*d5^224* d5*l5+7*l5^2)*m5+7*m4*l4^2*(1/24); m45 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m51 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m52 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); Nhúm Page 78 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư m53 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m54 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); m55 =-(1/24)*m5*l5^2*(cos(2*q(4)+2*q(3)+2*q(2))-1); M = [m11, m12, m13, m14, m15; m21, m22, m23, m24, m25; m31, m32, m33, m34, m35; m41, m42, m43, m44, m45; m51, m52, m53, m54, m55]; % Ma tran Coriolis*dq c1 = 0; c2 = (1/24)*(((-12*d5^2+12*d5*l5-4*l5^2)*m5-4*m4*l4^2)*dq(1)^2+(-2*l5^2* (dq(2)+dq(3)+dq(4)+dq(5))*m5-2*l4^2*m4*(dq(2)+dq(3)+dq(4)))*dq(1)l5^2* (dq(2)+dq(3)+dq(4)+dq(5))^2*m5l4^2*m4*(dq(2)+dq(3)+dq(4))^2)*sin(2*q(4)+2*q(3)+2*q(2)) -((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*a3*dq(1)^2*cos(q(4)+2*q(3)+2*q(2)) +(1/6)* a3^2*((3*m4+3*m5+m3)*dq(1)^2+(1/2)*m3*(dq(2)+dq(3))*dq(1)+(1/4)*m3* (dq(2)+dq(3))^2)*sin(2*q(3)+2*q(2))+(1/6)*a2*(((6*d5+3*l5)*m5+3*m4*l4)* cos(q(4)+q(3)+2*q(2))+3*a3*(m3+2*m4+2*m5)*sin(q(3)+2*q(2)) +a2*sin(2*q(2))* (3*m4+3*m5+m2+3*m3))*dq(1)^2; c3 = (1/24)*(((-12*d5^2+12*d5*l5-4*l5^2)*dq(1)^22*l5^2*(dq(2)+dq(3)+dq(4)+dq(5)) *dq(1)-l5^2*(dq(2)+dq(3)+dq(4)+dq(5))^2)*m5m4*l4^2*(4*dq(1)^2+(2*dq(2)+2*dq(3)+2* dq(4))*dq(1)+(dq(2)+dq(3)+dq(4))^2))*sin(2*q(4)+2*q(3)+2*q(2))a3*dq(1)^2*((-(1/2)*l5 +d5)*m5-(1/2)*m4*l4)*cos(q(4)+2*q(3)+2*q(2)) +(1/24)*a3^2*(12*dq(1)^2*m5+ (4*m3+12*m4)*dq(1)^2+2*m3*(dq(2)+dq(3))*dq(1)+m3*(dq(2)+dq(3))^2)*sin(2 *q(3)+2*q(2)) +(1/2)*(-dq(1)^2*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*cos(q(4)+q(3)+2*q(2))+ (1/2)*a3*dq(1)^2*(m3+2*m4+2*m5)*sin(q(3)+2*q(2))(2*((1/2)*dq(1)^2+dq(2)* (dq(2)+dq(3)+dq(4))))*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4)*cos(q(3)+q(4))+a3* sin(q(3))*(m3+2*m4+2*m5)*((1/2)*dq(1)^2+dq(2)*(dq(2)+dq(3))))*a2; Nhúm Page 79 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư c4 = (1/24)*(((-dq(2)^2+(-2*dq(3)-2*dq(4)-2*dq(5)-2*dq(1))*dq(2)-4*dq(1)^2+(2*dq(3)- 2*dq(4)-2*dq(5))*dq(1)-(dq(3)+dq(4)+dq(5))^2)*l5^2+12*d5*l5*dq(1)^212*dq(1)^2*d5^2) *m5m4*l4^2*(dq(2)^2+(2*dq(3)+2*dq(4)+2*dq(1))*dq(2)+4*dq(1)^2+(2*dq(3)+2*dq (4))* dq(1)+(dq(3)+dq(4))^2))*sin(2*q(4)+2*q(3)+2*q(2))((1/2)*cos(q(4)+2*q(3)+2*q(2))*a3* dq(1)^2+(1/2)*a2*cos(q(4)+q(3)+2*q(2))*dq(1)^2+(dq(2)^2+ (dq(3)+dq(4))*dq(2)+(1/2)* dq(1)^2)*a2*cos(q(4)+q(3)) +a3*cos(q(4))*(dq(2)^2+(2*dq(3)+dq(4))*dq(2)+(1/2)*dq(1)^2+ dq(3)*(dq(3)+dq(4))))*((-(1/2)*l5+d5)*m5-(1/2)*m4*l4); c5 =0; C=[c1;c2;c3;c4;c5]; %Ma tran luc co the g1 =0; g2 =(1/2)*m2*g*a2*cos(q(2))+m3*g*((1/2)*a3*cos(q(2)+q(3))+a2*cos(q(2))) +m4*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l4+a3*cos(q(2)+q(3))+a2*cos(q(2))) +m5*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l5+sin(q(2)+q(3)+q(4))*d5+a3* cos(q(2)+q(3))+a2*cos(q(2))); g3 =(1/2)*m3*g*a3*cos(q(2)+q(3))+m4*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l4+ a3*cos(q(2)+q(3)))+m5*g*(-(1/2)*sin(q(2)+q(3)+q(4))*l5+sin(q(2)+ q(3)+q(4))*d5+a3*cos(q(2)+q(3))); g4 =(1/2)*m4*g*sin(q(2)+q(3)+q(4))*l4+m5*g*(-(1/2)*sin(q(2)+q(3)+q(4)) *l5+sin(q(2)+q(3)+q(4))*d5); g5 =0; G = [g1; g2; g3; g4; g5]; %Ma tran luc khong the Q1=0; Q2=Fz*((-(1/2)*l5+d5)*sin(q(2)+q(3)+q(4))+a3*cos(q(2)+q(3))+a2*cos(q(2))); Q3=Fz*((-(1/2)*l5+d5)*sin(q(2)+q(3)+q(4))+a3*cos(q(2)+q(3))); Q4=Fz*(-(1/2)*l5+d5)*sin(q(2)+q(3)+q(4)); Q5=0; Q=[Q1;Q2;Q3;Q4;Q5]; Nhúm Page 80 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư % Returning Robot's accelerations ddq = inv(M)*([U(1);U(2);U(3);U(4);U(5)]-C-G-Q); CHƯƠNG 8: KHẢO SÁT ĐỘ CHÍNH XÁC, ĐÁNH GIÁ ĐẶC TÍNH KỸ THUẬT VÀ HIỆU CHỈNH THIẾT KẾ 8.1 Hệ số Kp, Kd khâu: -Khâu 1: Kp=269; Kd=169 -Khâu 2: Kp=669; Kd=50 -Khâu 3: Kp=2000; Kd=369 -Khâu 4: Kp=4500; Kd=300 -Khâu 5: Kp=625; Kd=50 Nhúm Page 81 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư 8.2 thị biểu biến sai số: Góc sai số khâu Góc sai số khâu Nhóm Page 82 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư Gúc v sai s khõu Góc sai số khâu Nhóm Page 83 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư Gúc v sai s khõu Gúc khâu theo thới gian Nhóm Page 84 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư Vn tc gúc ca khõu theo thi gian TÀI LIỆU THAM KHẢO Phan Bùi Khôi: Slide môn học Robotic Nguyễn Văn Khang : Cơ học kỹ thuật NXB Giáo dục, 2012 Nguyễn Văn Khang : Động lực học hệ nhiều vật NXB Khoa học kỹ thuật, 2007 Nguyễn Văn Khang : Cơ sở robot công nghiệp NXB Khoa học kỹ thuật, 2011 Nhúm Page 85 BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư BI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằưBI.tỏưp.mn.hỏằãc.TãNH.TOãN.THIỏắT.kỏ.ROBOT.NGNH.cặĂ.iỏằn.tỏằư
Ngày đăng: 09/11/2023, 00:53
Xem thêm:
