Báo cáo tổng kết thiết kế bộ điều khiển cho mô hình robot 4 bậc tự do

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC PHENIKAA _ BÁO CÁO TỔNG KẾT Thiết kế bộ điều khiển cho mô hình Robot bậc tự Sinh viên thực hiện: Lê Viết Đạt MSSV: 19010206 Trần Mạch Tuấn Kiệt HÀ NỘI, 12/2022 19010214 MỤC LỤC LỜI MỞ ĐẦU 2 TỔNG QUAN TÌNH HÌNH NGHIÊN CỨU 3 2.1 Tổng quan tình hình nghiên cứu lí lựa chọn đề tài 2.2 Mục tiêu, nội dung phương pháp nghiên cứu 2.3 Đối tượng phạm vi nghiên cứu CỞ SỞ LÝ THUYẾT 3.1 Cấu trúc robot tay máy công nghiệp 3.2 Thiết lập hệ tọa độ khảo sát lập bảng D – H 3.3 Vùng hoạt động khâu tác động cuối .12 3.4 Bài toán động học ngược 14 PHÂN TÍCH THIẾT KẾ HỆ THỐNG .15 4.1 Ý tưởng thiết kế 15 4.2 Phần cứng .15 4.2.1 Động Step KV4234 .15 4.2.2 Driver A4988 17 4.2.3 Board điều khiển Arduino Uno 18 4.2.4 Board CNC Shield V3 .20 4.2.5 Servo MG90S 21 4.3 Phần mềm .22 4.3.1 Arduino IDE 22 4.3.2 LabVIEW 23 4.3.3 MATLAB 25 4.4 Giải thuật điều khiển cho động Step 26 4.5 Giải thuật điều khiển cho động Servo 30 4.6 Thành lập các phương trình động học 32 4.6.1 Xây dựng toán động học thuận tay máy 32 4.6.2 Xây dựng toán động học ngược tay máy 35 TRIỂN KHAI TRÊN THỰC NGHIỆM VÀ KÊT QUẢ .38 5.1 Thiết kế và lắp ráp robot 38 5.2 Kiểm tra thuật toán điều khiển bằng mô phỏng 40 5.3 Vùng làm việc của robot 42 5.4 Thuật toán Arduino .43 5.5 Ứng dụng điều khiển LabVIEW 44 5.6 Robot ứng dụng thực tế 47 KẾT LUẬN VÀ KIẾN NGHỊ 50 TÀI LIỆU THAM KHẢO 51 MỤC LỤC HÌNH ẢNH Hình Sơ đồ cấu trúc robot công nghiệp 10 Hình Trường công tác robot .12 Hình Các hệ tọa độ cánh tay robot .13 Hình Chiều dài góc khâu 13 Hình Bộ thông số Denavit – Hartenberg 14 Hình Sơ đồ chuyển vị khâu cuối so với sở .15 Hình Mô tả không gian làm việc cấu hai khâu phẳng 16 Hình Sơ đồ khối thiết kế Robot 18 Hình Động Step KV4234 .19 Hình 10 Driver A4988 20 Hình 11 Module ESP32-WROOM-32 21 Hình 12 Board CNC Shield V3 23 Hình 13 Servo MG90S 24 Hình 14 Giao diện phần mềm lập trình Arduino IDE 25 Hình 15 Giao diện phần mềm lập trình LabVIEW .27 Hình 16 Giao diện phần mềm lập trình MATLAB 29 Hình 17 Đồ thị góc quay và vận tốc với chu kỳ cấp xung cố định .30 Hình 18 Qũy đạo mong muốn của động 31 Hình 19 Nguyên lý hoạt động của động servo 33 Hình 20 Thuật toán điều khiển động servo 34 Hình 21 Hệ tọa độ tay máy bậc tự 35 Hình 22 Động học ngược tay máy không gian Oxyz 38 Hình 23 Động học ngược tay máy mặt phẳng zOO zO 1: 39 Hình 24 Robot IBR 460 .41 Hình 25 Mẫu robot được sử dụng 42 Hình 26 Sơ đờ ghép nới mạch điều khiển 43 Hình 27 Kết quả mô phỏng thuật toán điều khiển động 44 Hình 28 Kết quả mô phỏng Robot không gian ba chiều .45 Hình 29 Vùng làm việc của robot .46 Hình 30 Cấu trúc thuật toán Arduino 47 Hình 31 Mô tả hoạt động phần mềm điều khiển labVIEW .48 Hình 32 Giao diện điều khiển phần mềm LabVIEW 49 Hình 33 Khối xử lý phương trình động học thuận (bên trái) nghịch (bên phải) 49 Hình 34 Cấu trúc thuật toán Arduino 51 Hình 35 Hình ảnh thử nghiệm robot di chuyển không giật 51 Hình 36 Hình ảnh robot gắp thả vật theo tọa độ đặt bởi người dùng 52 MỤC LỤC BẢNG BIỂU Bảng Bảng tham số động học D – H 15 Bảng Thông số hoạt động đợng Step KV4234 .19 Bảng Thơng số hoạt động Driver A4988 21 Bảng Các thông số kỹ thuật Arduino Uno 22 Bảng Các thông số kỹ thuật Board CNC Shield V3 23 Bảng Các thơng số kỹ thuật Servo MG90S .24 Bảng Thông số D-H của robot 36 LỜI MỞ ĐẦU Robot nói chung robot cơng nghiệp nói riêng tiến bước dài để xâm nhập vào đời sống xã hội hoạt động sản xuất người Việc ứng dụng robot trở nên quan trọng ngành tự động hóa nhằm nâng cao suất chất lượng, hạn chế cải thiện sức lao động người Robot giúp người hoạt động mơi trường nguy hiểm với tính mạng khu vực hầm mỏ, khư vực nhiễm phóng xạ, trì hoạt động dây chuyền 24/7 Robot đặc biệt cánh tay robot có ứng dụng rộng khắp nhà máy khắp giới Ngày robot đối tượng quan tâm nhiều lĩnh vực sản xuất mà cịn lĩnh vực nghiên cứu để cải tiến, tối ưu hoạt động sử dụng để giảng dạy Đồ án chuyên ngành lần nhằm nghiên cứu chuyên sâu robot, tập trung vào việc tính tốn lý thuyết xây dựng thuật toán điều khiển giao diện điều khiển cánh tay robot bậc tự Cơ sở lý thuyết robot xây dựng dựa phương trình động học thuận, phương trình động học ngược, phương trình động lực robot nhóm tác giả tính tốn mơ phần mềm MATLAB Từ xây dựng thuật tốn điều khiển vận tốc theo bậc thang thiết kế phần mềm LabVIEW để điều khiển Đề tài có sử dụng kết cấu phần cứng in 3D nhựa để điều khiển Kết cấu không đáng tin thực tế khn khổ đề tài sử dụng để mô tả khâu hay giai đoạn q trình sản xuất thực tế Mơ hình thuật toán điều khiển hoạt động tốt giới hạn làm việc robot mà động chạy mượt mà không xảy tượng giật Thử nghiệm đặt ly nước nhỏ khâu tác động cuối, nước không bị rơi vãi khỏi ly suốt q trình hoạt động TỔNG QUAN TÌNH HÌNH NGHIÊN CỨU 2.1.Tổng quan tình hình nghiên cứu lí lựa chọn đề tài Những ý niệm vật tương tự robot xuất từ thời cổ đại ngày câu chuyện thần thoại xưa Những vật tạo nhằm mục đích phục vụ chủ nhân cách tự động, có khả tương tác nghe lệnh người Tuy nhiên đến năm 1921, thuật ngữ “robotics” lần xuất đề cập kịch “Rossum’s Universal Robots” nhà viết kịch viễn tưởng tiếng người Czech, Karel Capek Ông sử dụng từ “robot” biến thể từ tiếng Czech cổ “robota” có nghĩa lao động cưỡng để gán vào máy mà Rossum trai chế tạo để phục vụ người [1] Vào năm 40 kỷ trước, nhà văn Nga, Issac Asimov đưa mô tả gần robot khái niệm ngày ông thêm vào việc điều khiển hệ thần kinh khả trình gọi “Positron” người lập trình Ông đề nguyên tắc mà robot cần phải có là: “Khơng tổn hại người”, “Tuân theo nguyên tắc người đặt ra”, “Cần phải tự bảo vệ khơng vi phạm hai quy tắc trước” [2] Từ mà “robot” dần vượt khỏi biên giới hư cấu trừu tượng để trở thành niềm cảm hứng thiết kế kỹ sư khí với cấu, thiết bị mang dáng dấp người, điều khiển phục vụ đời sống cơng nghiệp Mặc dù vậy, có thực tế robot sử dụng chủ yếu công nghiệp, gọi robot công nghiệp Về mặt kỹ thuật chúng có nguồn gốc từ hai lĩnh vực phát triển từ lâu cấu điều khiển từ xa máy công cụ điều khiển số Các người thao tác tách biệt khỏi mơi trường làm việc lý chẳng hạn an toàn chế tạo chi tiết Những robot nối kết thiết bị điều khiển khí từ xa khả lập trình máy điều khiển Tuy ngày robot cơng nghiệp phải có kết cấu, chức phải phù hợp với quy chuẩn định nghĩa quy định ngày Trong định nghĩa sử dụng nhiều kể đến viện nghiên cứu Robot Mỹ đề xuất: “Robot công nghiệp tay máy vạn hoạt động theo chương trình lập trình lại để hồn thành nâng cao hồn thành nhiệm vụ khác cơng nghiệp, vận chuyển nguyên vật liệu, chi tiết, sản phầm, dụng cụ thiết bị chuyên dụng khác” Một robot cần phải thể đặc trưng thiết bị vạn lập trình gọi robot cơng nghiệp Ngồi robot cịn phải đảm bảo chức điều khiển trình sản xuất Một robot công nghiệp bao gồm thiết bị thừa hành tay máy lập trình điều khiển đề hoàn thành chức vận động Với đặc trưng robot công nghiệp thiết bị tự động hóa khả trình mà khơng thể thiếu hệ thống sản xuất linh hoạt Robotics phát triển mạnh mẽ từ năm sau chiến tranh giới thứ hai với nhiều thành tự lớn robot T3 nâng khối lượng lên đến 40Kg vào năm 1974 công ty Cincinnati đến từ Mỹ Hay có dự án nghiên cứu robot chiến tranh mơ chó gồ hàng người lính cơng ty Boston Dynamic Trong lĩnh vực nghiên cứu, năm 1967, Đại học Stanford cho đời mẫu robot trang bị cảm biến để nhận biết định hướng đối tượng vật kẹp bàn kép Ngồi cịn nhiều lĩnh vực ứng dụng robot công nghiệp vũ trụ, chế tạo xe, máy móc, Ở Việt Nam, kỹ thuật robot bắt đầu du nhập nhiều vào ngành công nghiệp nước nhà từ năm 1990 sở cơng nghiệp bắt đầu đổi quy trình nhập nhiều loại robot phục vụ mục đích công nghiệp Năm 2002, công ty TOSY giám đốc người Việt Nam thành lập bắt đầu sản xuất robot giải trí, cơng nghiệp đem đến nhiều giải thưởng ngồi nước Bên cạnh cịn có đơn vị sản xuất robot nước ngồi có trụ sở Việt Nam ABB Ngày 12/10/2003, đời Hội khoa học công nghệ Robot Việt Nam (Vietnamese Assiciation of Robotics – VAR) thúc đẩy lĩnh vực nghiên cứu ứng dụng Robot Việt Nam phát triển Ngày hầu hết trường kỹ thuật Việt Nam trang bị môn kỹ thuật Robot vào chương trình giảng dạy giúp sinh viên tiếp cận tốt phát triển khía cạnh kỹ thuật điều khiển, mơ phát triển tốt Robot cánh tay công nghiệp loại robot phổ biến công nghiệp nay, nghiên cứu giúp nhóm tác giả tiếp cận tốt diễn cơng nghiệp Ngồi ngành robot chưa phát triển Việt Nam nên nhóm tác giả muốn xây dựng mơ hình để sinh viên thực nghiệm nghiên cứu tốt hơn, hỗ trợ tối đa cho giảng viên việc giảng dạy 2.2.Mục tiêu, nội dung phương pháp nghiên cứu Từ phân tích nêu trên, đề tài nghiên cứu tập trung thiết kế, xây dựng phát triển bộ điều khiển dùng cho mô hình Robot bậc tự Mục tiêu: - Nghiên cứu cánh tay robot được dùng cơng nghiệp và giảng dạy Hồn thành mơ hình cánh tay robot dùng đợng step Hồn thiện giao diện điều khiển mô hình máy tính Phân tích, tính tốn, lựa chọn giải tḥt điều khiển thích hợp cho robot Nội dung nghiên cứu: - Tìm hiểu về các loại tay máy được dùng nhiều công nghiệp và chọn lựa mô hình 3D của tay máy dựa theo các open source - Tính toán lựa chọn các linh kiện cho tay máy bao gồm động cơ, vi điều khiển, driver, các chi tiết khí… và lắp ráp sản phẩm thực tế - Xác định không gian làm việc của tay máy - Tính toán các phương trình động học và động lực học cho tay máy - Thiết lập giải thuật điều khiển vận tốc theo dạng hình thang cho động step - Kiểm tra lại các thuật toán và phương trình bằng mô phỏng Matlab - Kiểm tra thuật toán mô hình robot đã xây dựng, - Xây dựng giao diện và các chế độ điều khiển cho tay máy bằng LabView, - Đánh giá sản phẩm hoàn thành với số tiêu chí đề ra, - Chạy thử Robot cho vài ứng dụng thực tế Phương pháp nghiên cứu: - Thu thập, tham khảo phân tích tài liệu lĩnh vực, đề tài nghiên cứu liên quan, linh kiện thiết bị sử dụng, - Nghiên cứu, chỉnh sửa lại Robot cho phù hợp yêu cầu đặt ra, - Tìm hiểu kỹ thuật lập trình, xây dựng giải thuật áp dụng phần mềm lập trình thơng dụng, - Kiểm tra thực nghiệm thiết robot môi trường thực tế, - Tổng hợp, phân tích, đánh giá kết đạt được, - So sánh đánh giá sản phẩm hoàn thành với số thiết bị có sẵn thị trường 2.3.Đối tượng phạm vi nghiên cứu Trong nghiên cứu này, tác giả tập trung thiết kế chế tạo bộ điều khiển dành cho Robot bậc tự sử dụng động Step không phản hồi Bộ điều khiển này được phát triển dựa sở các phương trình động học của Robot để đưa thuật toán Phần mềm Matlab dùng để mô phỏng, LabVIEW để điều khiển Robot bốn bậc tự và phần khí để chế tạo cánh tay Robot Tối ưu sản phẩm ứng dụng thuật tốn trí tuệ nhân tạo xử lý ảnh nhóm tác giả tiếp tục nghiên cứu phát triển thời gian tới Hình 30 Cấu trúc thuật toán Arduino Chương trình chính có tác dụng cấu hình cho ngoại vi ngắt, sau đó đọc lệnh gửi đến từ Serial để chọn chế độ điều khiển phù hợp Nếu cần điều khiển động step, timer1 bắt đầu chạy để tính toán các khoảng thời gian trễ cần thiết giữa hai lần cấp xung bằng giải thuật phần 2.4 Bằng cách thử nghiệm, nhóm tác giả nhận thấy với chu kỳ lặp lại =100μs, động hoạt động đúng mong muốn Bên cạnh đó timer2 được dùng để cấp xung liên tiếp với khoảng thời gian duty cycle được tính phần 2.5 chu kỳ lặp lại =16μs 5.5.Ứng dụng điều khiển LabVIEW LabVIEW phần mềm thiết kế giao diện lập trình dựa ngơn ngữ đồ họa sử dụng rộng rãi phổ biến lĩnh vực đo lường liệu, điều khiển thiết bị tự động hóa cơng nghiệp LabVIEW hỗ trợ nhiều module dùng để giao tiếp với thiết bị ngoại vi cách đơn giản xác Giao diện lập trình đồ họa, diễn đạt cú pháp thơng qua hình ảnh trực quan giúp mơi trường soạn thảo trực quan trình thiết kế thuật tốn điều khiển 46 Bên hình mơ tả lưu đồ thuật tốn q trình điều khiển sử dụng LabVIEW Trong phần mềm này, người dùng lựa chọn hai chế độ điều khiển Auto Manual Ở chế độ Manual người dùng điều khiển thơng qua nút nhấn điều hướng có hình người dùng điều khiển cách nhập tọa độ x , y , z góc khớp q , q , q , q sử dụng lệnh “Move to point” Ở chế độ Auto người dùng điều khiển thông qua nút điều hướng mà nhập giá trị vào text sử dụng nút “Move to point” để điều khiển Sau nhấn Run, LabVIEW gửi lệnh giúp robot nhận biết bắt đầu phải chạy tự động, LabVIEW load điểm lưu bắt đầu ghi vào Arduino Giao diện cung cấp việc lưu điểm liệu cài đặt vào file máy tính, giúp thuận tiện cho việc tái sử dụng sau Ngoài chức mở file trang bị để load liệu từ file text nhớ lệnh LabVIEW Hình 31 Mô tả hoạt động phần mềm điều khiển labVIEW Phương trình động học thuận phương trình động học ngược tính tốn phần mềm điều khiển, giúp cho việc đồng người dùng nhập điều khiển thông qua tọa độ khâu cuối giá trị góc quay Các khối xử lý sử dụng Module Formula Node hỗ trợ LabVIEW giúp q trình tính tốn trở nên dễ dàng hơn, mô tả khối xuất hình 10 47 Hình 32 Giao diện điều khiển phần mềm LabVIEW Hình 33 Khối xử lý phương trình động học thuận (bên trái) nghịch (bên phải) Giao tiếp Arduino LabVIEW thông qua cổng Serial Tốc độ cổng Serial đủ nhanh để đảm bảo liệu ghi đọc cách kịp thời Dữ liệu điều khiển 48 chuyển lên câu lệnh theo định dạng quy định board Arduino tương ứng với hành vi mà Robot hoạt động Arduino trả lệnh để xác thực nhận mệnh lệnh điều khiển từ LabVIEW LabVIEW sử dụng module VISA Read để ghi lại liệu 5.6.Robot ứng dụng thực tế Hình ảnh phiên thiết bị đo đưa Hình 34 Các thành phần bợ điều khiển lắp ráp bên hộp nhựa Phần khung robot chế tạo công nghệ in 3D, sử dụng chất liệu nhựa in PLA Các động được lắp đặt thân robot dựa theo những thông số đã chọn và được nối dây cho không làm ảnh hưởng đến quá trình hoạt động của robot chạy vùng làm việc Để cấp ng̀n cho robot, người dùng sử dụng đầu ng̀n 12V Một số hình ảnh ứng dụng robot chạy lượt thể Hình 35 36 Các thơng số đo về bị trí của khâu tác động cuối cũng góc quay của từng động hiển thị giao diện phần mềm LabVIEW giúp người dùng có thể dễ dàng điều khiển và thao tác Robot có thể được ứng dụng nhiều các phòng thí nghiệm hóa chất để di chuyển các ống ngiệm mà không làm đổ dung dịch ngoài, các mô hình dây chuyền bang tải vừa và nhỏ giúp gắp thả vật theo đúng nơi quy định hay hoạt động môi trường độc hại mà người khó có thể làm việc Bên cạnh đó, ta còn có thể sử dụng robot môi trường giáo dục để giảng dạy, tạo cảm giác hứng thú cho người học với bộ môn điều khiển robot Robot tiếp tục nghiên cứu hoàn thiện để nâng cao độ xác, tăng thẩm mỹ, hạ giá thành 49 Hình 34 Cấu trúc thuật toán Arduino Hình 35 Hình ảnh thử nghiệm robot di chủn khơng giật 50 Hình 36 Hình ảnh robot gắp thả vật theo tọa độ đặt bởi người dùng 51 KẾT LUẬN VÀ KIẾN NGHỊ Kết luận: Đề tài nghiên cứu hoàn thành số nội dung sau đây: - Thiết kế lắp ráp phiên robot bốn bậc tự do, ứng dụng các phòng thí nghiệm hoặc giảng dạy - Xây dựng được giải thuật điều khiển vận tốc theo dạng hình thang cho động step - Hoàn thành mô phỏng robot MATLAB - Xây dựng được giao diện điều khiển cho robot bằng LabVIEW - Bên cạnh đó, đề tài giúp cho tác giả củng cố kiến thức, hiểu biết vận dụng số phương pháp, phần mềm kỹ thuật, quy trình chế tạo sản phẩm điện tử, lập trình vi điều khiển, in 3D… thơng qua thu thập, phân tích, tổng hợp, đánh giá tài liệu tham khảo chủ đề nghiên cứu linh kiện, thiết bị sử dụng Kiến nghị: Tác giả mong muốn tiếp tục thực đề tài để phát triển tối ưu sản phẩm, bổ sung thêm phần ứng dụng trí tuệ nhân tạo kết hợp xử lý ảnh để giúp robot trở nên thông minh hơn, xác và có nhiều ứng dụng thực tế 52 TÀI LIỆU THAM KHẢO [ J M Jordan, “The Czech Play That Gave Us the Word ‘Robot’,” The MIT 1] Press Reader, 19 July 2019 [Trực tuyến] Available: https://thereader.mitpress.mit.edu/origin-word-robot-rur/#:~:text=The%20word %20itself%20derives%20from,were%20neither%20metallic%20nor%20mechanical [Đã truy cập 27 December 2022] [2] A Tikkanen., “Article History: Three laws of robotics,” Britannica, [Trực tuyến] Available: https://www.britannica.com/topic/Three-Laws-of-Robotics [3] K Minh, Giáo trình Robot cơng nghiệp, Hanoi: Khoa học kỹ thuật, 2019 53 Phụ lục I: tính toán động lực học cho robot - Tính động tay máy Tay máy gồm khâu, tổng động tay máy là: K= K 1+ K 2+ K 3+ K Trong đó: K : động của khâu quay quanh trục cố định z 2 K 1= J ω 1= J q˙1 2 K : động của khâu quay quanh trục di động z 1 K 2= J C ω22 + m2 v 2C 2 2 Với: vC2 =xC2 + y 2C + z 2C [ ][ 2 C2 C2 C2 x ][ ] ][ ] ] cos ( q ) sin ( q ) x C y yC 0 r C =H r C ⇔ = sin ( q ) −cos ( q ) l z1 1 z C 0 1 2 [ ][ x 0C 2 cos ⁡( q 1) sin ⁡(q 1) r cos ⁡(q 2) sin ⁡( q 1) −cos ⁡(q 1) r sin ⁡(q 2) ⇔ = l1 1 zC 0 1 y 0C 2 [ ][ xC r cos (q 1) cos( q2 ) ⇔ = r cos(q 2) sin( q1) l +r sin(q 2) zC 1 y 0C 2 [ ][ x˙ 0C −q˙1 r cos(q 2) sin( q1)−q˙ r cos( q1 )sin(q2 ) ⇔ y˙ C = q˙1 r cos(q 1)cos( q2 )−q˙2 r sin(q1)sin(q2 ) q˙2 r cos(q2) z˙ 0C 2 [] ( x˙ 0C ) ( y˙ 0C ) =¿ ( z˙ 0C ) ⇔ 2 54 ] 2 ⇔ v 2C =( q˙1 r cos ( q 2) sin ( q ) + q˙ r cos(q1 )sin(q2 ) ) + ( q˙1 r cos(q1 )cos (q 2)− q˙2 r sin (q1 )sin (q 2) ) + K : động của khâu quay quanh trục di động z 2 2 K 3= J C ω3 + m3 v C 2 3 Với: 2 2 vC =x C + y C + z C 3 3 2 2 q˙ r 2 2 l q˙1 2 q˙1 r cos(2 ( q 2+ q3 ) ) l q˙1 cos (2 q2 ) ⇔v = + q˙2 r + q˙3 r + +l q˙2 +2 q˙2 q˙3 r + + + 2 2 K : động của khâu quay quanh trục di động z 2 C3 2 2 K = J C ω 4+ m v C 2 4 Với: vC2 =x 2C + y 2C + z 2C 4 4 2 ⇔ v 2C =( l cos ( ( q2 +q3 ) ) ( q˙ 2+ q˙3 ) +l q2 cos( q2 )) + ( cos (q1 ) σ 1+ q1 sin(q 1)σ ) + ( sin (q1 ) σ 1−q˙1 cos (q Trong đó: σ 1=l sin ( ( q2 +q ) ) ( q˙2+ q˙3 ) +l q˙2 sin ( q 2) σ 2=r +l cos ( ( q 2+ q3 ) )+ l cos (q2 ) - Tính thế tay máy - Tính các lực không thế: P=P1 + P2 + P3 + P4 =m1 g r +m2 g [ r sin ( q 2) + l1 ] + m3 g ¿ ¿ Q 1=M m −b1 q˙1 ¿ Q 2=M m −b2 q˙2 ¿ Q =M m −b3 q˙3 ¿ Q =M m −b4 q˙4 Trong đó M m , M m , M m , M m là mômen tại các khớp 1, 2, và 4 b , b2 , b3 , b4 là hệ số cản nhớt tại các khớp 1, 2, và - Phương trình Lagarange biểu diễn động lực học tay máy: 55 ( ) d ∂K ∂K ∂P ¿ − = +Q i (3) dt ∂ q˙ i ∂ qi ∂ qi - Tính các đạo hàm riêng ∂K =0 ∂ q1 ∂ K −1 2 2 2 = dq (l m4 sin (2 q2 +2 q3 )+m3 r sin(2 q2 +2 q 3)+l m3 sin(2 q2 )+l m sin(2 q 2)+m2 r sin(2 q ∂ q2 2 2 2 dq1 l l m4 sin (q3 ∂ K −dq1 l m4 sin(2 q 2+ 2q 3) dq m r sin(2 q 2+2 q 3) = − −dq l m4 r sin(q 2+ q3 )− ∂ q3 2 ∂K =0 ∂ q4 ( ) d ∂K =−dq l l m sin(q )−dq l m r sin( q )−dq l m r sin(q )−2 dq dq l l m sin (q )−2 dq dt ( ∂ q˙ ) d ∂K =−dq l m r sin(q )−dq dq l l m sin(q )−dq dq l m r sin (q )−dq l m r sin (q +q ) ( d dt ( ∂ q˙ ) d ∂K =−m r (l sin(q + q ) ( dq +dq ) + dq l sin( q )) dt ( ∂ q˙ ) d ∂ K −1 2 2 = dq (2 dq l m3 sin(2 q2 )+2 dq2 l m4 sin(2 q2 )+2 dq2 m2 r sin (2 q2 )+l m4 sin(2 q2 +2 q 3)2 dt ∂ q˙1 2 2 4 4 2 2 2 4 3 2 3 2 2 3 3 3 3 4 3 ∂P =0 ∂ q1 ∂P =g m4 ( l cos (q 2+ q3 )+ l cos (q 2) ) + g m3 ( r cos (q 2+ q3 )+ l2 cos (q2 ) ) + g m2 r cos( q2 ) ∂ q2 ∂P =g cos (q2 +q 3) ( l m4 +m3 r ) ∂ q3 ∂P =0 ∂ q4 Thay tất cả vào phương trình (3): Khâu 1: Mm = ( ) d ∂K ∂K ∂P −1 2 − − + b1 q˙1= dq ( dq2 l m3 sin(2q 2)+2 dq l m sin(2q 2)+2 dq m r sin(2 q dt ∂ q˙1 ∂ q1 ∂ q1 56 Khâu 2: Mm = ( ) d ∂K ∂K ∂P − − + b2 q˙2=−dq32 l l m4 sin ( q3 ) −dq22 l2 m4 r sin ( q )−dq32 l m r sin ( q )−2 dq2 dt ∂ q˙2 ∂ q2 ∂ q2 Khâu 3: Mm = ( ) d ∂K ∂K ∂P − − +b3 q˙3=−dq 22 l m4 r sin ( q2 ) −dq2 dq l l m sin ( q3 ) −dq dq l m3 r sin ( q )− dt ∂ q˙3 ∂ q3 ∂ q3 Khâu 4: Mm = ( ) d ∂K ∂ K ∂P − − + b4 q˙4 =−m4 r ( l sin ( q 2+ q3 ) ( dq 2+ dq3 ) +dq 22 l sin ( q2 ) )+ b4 q˙3 +b4 q˙2 dt ∂ q˙4 ∂ q4 ∂ q Do công thức tính mômen tại các khớp có sự xuất hiện của hệ số ma sát, mô phỏng ta sẽ giả sử các giá trị này bằng 0, còn làm mô hình phần cứng, các chỉ số này không kiểm soát được nhóm tác giả gặp hạn chế về thiết kế khí nên khối lượng vật nâng tối đa mô hình thực sẽ được xác định bằng phương pháp thực nghiệm Phụ lục II: Thiết kế và các thông số của robot 57 58 59 Thông số robot: Tham số Giá trị l1 150mm l2 140mm l3 140mm l4 54mm q1 -90~90 độ q2 0~120 độ q3 -136~33 độ Khối lượng vật nâng tối đa 85g Vận tốc tối đa 600 rpm Gia tốc tối đa 2500 độ/sq(s) 60