Ứng dụng thuật toán lai lực vị trí điều khiển Robot Planar ba bậc tự do

LỜI CAM ĐOAN Em xin cam đoan đồ án tốt nghiệp: “Ứng dụng thuật tốn lai lực vị trí điều khiển Robot Planar ba bậc tự do” em tự thiết kế hướng dẫn cô giáo PGS.TS Nguyễn Phạm Thục Anh Các số liệu kết hoàn toàn với thực tế Để hoàn thành đồ án em sử dụng tài liệu ghi danh mục tài liệu tham khảo không chép hay sử dụng tài liệu khác Nếu phát có chép em xin chịu hoàn toàn trách nhiệm Hà Nội, ngày 08 tháng 06 năm 2018 Sinh viên thực Đào Duy Tùng Mục lục DANH MỤC HÌNH VẼ i DANH MỤC BẢNG SỐ LIỆU ii Lời nói đầu iii CHƯƠNG I: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 1.1 Sơ lược phát triển Robot công nghiệp .1 1.2 Định nghĩa robot số khái niệm 1.2.1 Các định nghĩa Robot công nghiệp 1.3 Mục tiêu ứng dụng Robot công nghiệp 1.3.1 Mục tiêu ứng dụng Robot công nghiệp 1.3.2 Ứng dụng mơ hình hóa Robot cơng nghiệp .3 1.4 Cấu trúc hệ thống Robot công nghiệp 1.4.1 Tay máy 1.4.2 Hệ thống truyền dẫn động robot .9 1.4.3 Hệ thống điều khiển 10 1.4.4 Hệ thống cảm biến .11 1.5 Hệ thống điều khiển Robot .11 1.5.1 Các phương thức điều khiển .12 1.5.2 Các phương pháp điều khiển Robot 15 1.6 Mục tiêu đề tài 15 CHƯƠNG II: CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN LỰC .16 2.1 Điều khiển độ cứng .16 2.2 Điều khiển lai lực vị trí .19 CHƯƠNG III: THIẾT KẾ BỘ ĐIỀU KHIỂN LAI LỰC VỊ TRÍ 22 3.1 Động học Robot 22 3.2 Phương trình động lực học robot .25 3.3 Bộ điều khiển lai lực vị trí 32 3.3.1 Xây dựng mơ hình robot .32 3.3.2 Thuật toán điều khiển 34 3.3 Kết mô 37 CHƯƠNG IV: BỘ ĐIỀU KHIỂN MỜ 42 4.1 Bộ điều khiển mờ .42 4.2 Luật hợp thành mờ (Mamdani) 43 4.3 Các phương pháp giải mờ (defuzzification) 46 4.4 Bộ điều khiển mờ chỉnh định thơng số tích phân 49 Kết luận 56 Tài liệu tham khảo 57 PHỤ LỤC .58 Danh mục hình vẽ DANH MỤC HÌNH VẼ Hình 1.1: Robot vận chuyển hàng Hình 1.2: Robot hàn .5 Hình 1.3: Robot lắp ráp tô Hình 4: Các phận cấu thành robot công nghiệp Hình 1.5: Sơ đồ tổng quan hệ thống điều khiển Robot 12 Hình 2.1: Tác động khâu tác động cuối lên môi trường 16 Hình 2.2: Mơ hình hóa hệ thống dạng lò xo đàn hồi 17 Hình 2.3: Sơ đồ hệ thống 17 Hình 2.4: Sơ đồ vịng kín điều khiển 17 Hình 2.5: Sơ đồ điều khiển lai lực vị trí 20 Hình 3.1: Mơ hình robot .22 Hình 3.2: Mơ hình robot ba bậc tự hệ tọa độ Oxy 25 Hình 3.3: Sơ đồ điều khiển lai lực vị trí .34 Hình 3.4: Sơ đồ điều khiển lai lực vị trí cho robot ba bậc tự .37 Hình 3.5: Tọa độ vị trí x .38 Hình 3.6: Tọa độ vị trí y .38 Hình 3.7: Kết mơ với lực đặt số .39 Hình 3.8: Sai số lực vị trí ổn định 39 Hình 3.9: Khối tín hiệu đặt hình sin .40 Hình 3.10: Kết mơ với lực đặt hình sin 40 Hình 4.1: Cấu trúc điều khiển mờ 42 Hình 4.2: Miền giá trị tập mờ B’ 47 Hình 4.3: Giá trị rõ y’ theo nguyên lý trung bình 47 Hình 4.4: Giải mờ trường hợp G khơng liên thông .48 Hình 4.5: Giải mờ phương pháp điểm trọng tâm 48 Hình 4.6: Bộ điều kiển mờ chỉnh định tham số 50 Hình 4.7: Hàm liên thuộc sai số lực đạo hàm sai số lực 51 Hình 4.8: Hàm liên thuộc dKi .51 Hình 4.9: Sơ đồ mơ hệ thống với điều khiển mờ 52 Hình 4.10: Sơ đồ điều khiển mờ 52 Hình 4.11: Tọa độ vị trí x .53 Hình 4.12: Tọa độ vị trí y .53 Hình 4.13: Kết mơ lực sử dụng điều khiển mờ 54 Hình 4.14: Kết mơ lực vị trí ổn định .54 Hình 4.15: Kết mơ với lực đặt hình sin sử dụng điều khiển mờ 55 i Danh mục bảng số liệu DANH MỤC BẢNG SỐ LIỆU Bảng 1.1: Các dạng khớp robot Bảng 3.1: Bảng D-H robot .22 Bảng 3.2: Tín hiệu vào khối sơ đồ mô 37 Bảng 4.1: Luật điều khiển mờ cho điều khiển mờ chỉnh định tham số I [3] 51 Lời nói đầu Robot ngày đóng vai trị quan trọng cơng nghiệp tự động tính linh hoạt hiệu Robot đem lại suất lao động cao, giảm chi phí nguyên vật liệu dạng lượng tiêu thụ, nâng cao chất lượng giảm giá thành sản phẩm, tạo khả cạnh tranh cao thị trường quốc tế Mặt khác, Robot giải phóng người khỏi lao động chân tay, giúp xã hội văn minh hơn, nâng cao dân trí tổ chức trình sản xuất tổ chức xã hội Chính nghiên cứu điều khiển chuyển động Robot lĩnh vực đầy thách thức nhà khoa học quan tâm Trong hai thập kỷ gần đây, vấn đề kiểm soát tương tác robot mơi trường lên, thu hút có nhiều kết nghiên cứu lĩnh vực robot Trong tất ứng dụng tương tác robot môi trường, vị trí lực tương tác khâu tác động cuối phải điều khiển đồng thời Các ứng dụng lĩnh vực thúc đẩy phát triển cho nhu cầu thực tế tự động hóa nhiệm vụ phức tạp người thực như: lắp ráp, xử lý cố, hàn, vận chuyển hàng hóa, vv… Chính vậy, mục đích nghiên cứu đồ án “Ứng dụng thuật tốn lai lực vị trí điều khiển Robot Planar ba bậc tự do” điều khiển đồng thời lực vị trí khâu tác động cuối Em xin chân thành cám ơn PGS.TS Nguyễn Phạm Thục Anh tận tình hướng dẫn giúp đỡ em suốt trình làm đồ án Do hạn chế trình độ nhiều yếu tố khác nên báo cáo cịn nhiều khiếm khuyến sai sót Em mong nhận đóng góp ý kiến thầy bạn để hồn thiện báo cáo ii Lời nói đầu Hà Nội, ngày 08 tháng 06 năm 2018 Sinh viên thực Đào Duy Tùng iii Chương 1: Tổng quan robot cơng nghiệp CHƯƠNG I TỔNG QUAN VỀ ROBOT CƠNG NGHIỆP 1.1 Sơ lược phát triển Robot công nghiệp Năm 1921, từ “Robot” xuất lần đầu kịch “Rossum’s Universal Robots” nhà viết kịch viễn tưởng người Sec, Karel Capek Trong kịch này, ông dùng từ “Robot”, biến thể từ gốc Slavơ “Rabota”, để gọi thiết bị - lao công người (nhân vật Rossum) tạo Đầu năm 60, công ty Mỹ AMF quảng cáo loại máy tự động vạn gọi “ người máy công nghiệp” ngày đặt tên Robot công nghiệp Ngày loại thiết bị có dáng dấp có vài chức tay người điều khiển tự động để thực số thao tác sản xuất gọi robot công nghiệp Tiếp theo Mỹ, nước khác bắt đầu sản xuất robot công nghiệp: Anh -1967, Thụy Điển Nhật - 1968, CHLB Đức - 1971, Pháp - 1972, Ý- 1973, … 1.2 Định nghĩa robot số khái niệm 1.2.1 Các định nghĩa Robot công nghiệp Theo IRA-Viện nghiên cứu robot Hoa Kì: Robot cơng nghiệp cấu thao tác đa chức với chương trình làm việc lập trình lại, dùng để di chuyển vật liệu, chi tiết máy, dụng cụ dùng cho công việc đặc biệt thông qua chuyển động khác lập trình nhằm mục đích hồn thành nhiệm vụ đa dạng Robot cơng nghiệp phần tử tự động hóa khả trình, phận khơng thể thiếu sản suất linh hoạt Theo ISO -Tổ chức tiêu chuẩn quốc tế: Robot công nghiệp cấu thao tác đa chức với nhiều chiều chuyển động điều khiển tự động với chương trình làm việc thay đổi cho cơng việc có mục đích khác Nó gắn cố định sàn di động Chương 1: Tổng quan robot công nghiệp Định nghĩa M.Bradky: Robot công nghiệp ghép nối thông minh từ nhận thức tới hành động Robot công nghiệp định nghĩa đơn giản thiết bị tự động linh hoạt, bắt chước chức lao động công nghiệp người Các định nghĩa robot khác thiết bị tự động hóa điều khiển theo chương trình lập trình sẵn nhờ vi xử lý mạch tích hợp chuyên dùng Một số khái niệm a Số bậc tự Bậc tự số chuyển động độc lập hay số tọa độ cần thiết để biểu diễn vị trí hướng vật thể tay robot không gian làm việc Để biểu diễn hoàn chỉnh đối tượng không gian cần tham số: tọa độ để xác định vị trí đối tượng khơng gian tọa độ biểu diễn hướng vật thể Số bậc tự tương ứng với số khớp số nối robot b Hệ tọa độ Mỗi robot thường bao gồm nhiều khâu liên kết với qua khớp tạo thành xích động học xuất phát từ khâu đứng yên Hệ tọa độ gắn với khâu gọi hệ tọa độ hệ tọa độ gắn với khâu động gọi hệ tọa độ suy rộng thời điểm hoạt động, tọa độ suy rộng xác định cấu hình robot dịch chuyển dài dịch chuyển góc khớp tịnh tiến khớp quay Các hệ tọa độ gắn lên khâu phải tuân theo quy tắc bàn tay phải c Vùng làm việc Vùng làm việc tập hợp tất điểm mà tay robot chạm tới Là tồn thể tích qt khâu chấp hành cuối robot thực tất chuyển động Vùng làm việc bị ràng buộc thơng số hình học robot ràng buộc học khớp Một số thông số đặc trưng hệ thống Robot:  Độ phân giải: đặc trưng khoảng cách nhỏ biểu diễn toàn dải chuyển động khớp Chương 1: Tổng quan robot cơng nghiệp  Độ xác: đặc trưng cho khả robot điều chỉnh điểm cuối tay máy đến điểm không gian hoạt động  Độ lặp lại: đặc trưng cho khả robot đưa đầu cuối bàn tay chạm vào điểm theo chương trình định sẵn 1.3 Mục tiêu ứng dụng Robot công nghiệp 1.3.1 Mục tiêu ứng dụng Robot công nghiệp Ưu điểm quan trọng robot công nghiệp tạo nên khả linh hoạt hoá sản xuất Việc sử dụng máy tính điện tử, robot máy điều khiển theo chương trình cho phép tìm phương thức để tạo nên dây chuyền tự động sản xuất hàng loạt với nhiều loại sản phẩm Kỹ thuật robot cơng nghiệp máy tính đóng vai trị quan trọng việc tạo dây chuyền tự động linh hoạt Robot thực quy trình thao tác hợp lý người thợ lành nghề cách ổn định suốt thời gian làm việc Vì robot góp phần nâng cao chất lượng khả cạnh tranh sản phẩm Hơn robot nhanh chóng thay đổi cơng việc để thích nghi với biến đổi mẫu mã, kích cỡ sản phẩm theo yêu cầu thị trường cạnh tranh 1.3.2 Ứng dụng mơ hình hóa Robot công nghiệp Từ mục tiêu ứng dụng robot cơng nghiệp mà ta phân chia lĩnh vực ứng dụng sau: a Trong vận chuyển, bốc dỡ vật liệu Robot nhặt chi tiết vị trí chuyển dời đến vị trí khác Robot gắp chi tiết vị trí cố định băng tải chuyển động đặt vị trí cố định khác đặt lên băng tải khác chuyển động với định hướng chi tiết khác Trong dây chuyền sản xuất robot sử dụng để đưa chi tiết lấy chi tiết khỏi máy gia công kim loại, máy CNC, máy đột dập, máy ép nhựa dây chuyền đúc Trong cơng đoạn đóng gói robot xếp vật liệu lên giá đóng gói, xếp sản phẩm vào hộp caton nhặt chi tiết khỏi hộp Chương 1: Tổng quan robot cơng nghiệp Hình 1.1: Robot vận chuyển hàng b Trong lĩnh vực gia công vật liệu Ứng dụng lĩnh vực gia công vật liệu bao gồm công nghệ hàn, sơn, gia công kim loại, … Sơn công việc nặng nhọc độc hại, đồng thời để đạt yêu cầu kĩ thuật đòi hỏi thợ sơn phải đào tạo thời gian tốn Robot học tất kiến thức phức tạp vài lặp lại xác động tác khó Robot cịn dùng phục vụ máy công cụ, làm khuôn công nghiệp đồ nhựa, gắn kính xe hơi, gắp hàng khỏi băng tải đặt chúng vào trạm chuyển trung gian Ứng dụng robot công nghệ hàn đường, vừa đạt suất cao chịu nhiệt nóng phát trình hàn Cảm biến gắn robot xác định vị trí đường hàn Chương 4: Bộ điều khiển mờ +Ngun lí trung bình +Ngun lí cận trái +Ngun lí cận phải Hình 4.2: Miền giá trị tập mờ B’ Nguyên lý trung bình Giá trị rõ y’ là: Trong đó, y1 y2 điểm cận trái cận phải tập G Nguyên lý thường dùng G miền liên thông nên y’ giá trị có độ phụ thuộc lớn Trong trường hợp B’ gồm hàm phụ thuộc có dạng y’ khơng phụ thuộc vào độ thỏa mãn luật điều khiển định (hình 4.6) Hình 4.3: Giá trị rõ y’ theo nguyên lý trung bình Nguyên lý cận trái : Giá trị rõ y’ lấy cận trái y G (y’ = y1) Giá trị phụ thuộc tuyến tính vào độ thỏa mãn luật điều khiển định 44 Chương 4: Bộ điều khiển mờ Nguyên lý cận phải : Giá trị rõ y’ lấy cận phải y2 G (y’ = y2) Giá trị phụ thuộc tuyến tính vào đáp ứng vào luật điều khiển định Như vậy, độ thỏa mãn H luật điều khiển định nhỏ sai lệch ba giá trị rõ xác định theo ba nguyên lý lớn Hình 4.4: Giải mờ trường hợp G không liên thông Phương pháp điểm trọng tâm (hình 4.8) Phương pháp cho kết giá trị rõ y’ hoành độ điểm trọng tâm miền bao đường B(y) trục hồnh Cơng thức xác định sau: = với S miền xác định tập mờ B’ Công thức cho phép xác định giá trị rõ y’ với tham gia tất tập mờ đầu luật điều khiển không để ý tới độ thỏa mãn luật định Hình 4.5: Giải mờ phương pháp điểm trọng tâm 45 Chương 4: Bộ điều khiển mờ 4.4 Bộ điều khiển mờ chỉnh định thông số tích phân Trong lý thuyết điều khiển, có nhiều phương pháp để hiệu chỉnh thông số điều khiển PI, phổ biến phương pháp Ziegler-Nichols Tuy nhiên, số hệ thống, việc hiệu chỉnh điều khiển PI phương pháp địi hỏi q trình thực nghiệm thời gian Thông thường, thông số điều khiển xác lập phương pháp Ziegler-Nichols dựa kết đo đạc từ đáp ứng hệ thống Tuy nhiên, ảnh hưởng nhiễu sai số phép đo, dẫn đến việc hiệu chỉnh thông số điều khiển PI khó đạt giá trị tốt Vì vậy, trình tinh chỉnh thực trước áp dụng điều khiển vào hệ thống Có hai kiểu tinh chỉnh điều khiển PI nghiên cứu triển khai ứng dụng Đó là, tinh chỉnh trình hoạt động hệ thống (tinh chỉnh online) tinh chỉnh lúc tắt hệ thống cho hệ thống vận hành thời gian định (tinh chỉnh off-line) Trong đó, áp dụng giải thuật di truyền xem phương pháp hiệu để tinh chỉnh off-line điều khiển Tuy nhiên, việc tìm kiếm trực tiếp giá trị tối ưu thông số điều khiển miền xác định chúng đòi hỏi nhiều thời gian, phải cần đến mơ hình tốn đối tượng Trong khuynh hướng phát triển cơng nghiệp ngày nay, q trình thiết bị địi hỏi việc tự động điều chỉnh cách hiệu quả, phạm vị hoạt động rộng, với việc thiết kế thi công điều khiển đơn giản Tinh chỉnh online điều khiển PI giải thuật thông minh xem nhiều cách thức để đạt yêu cầu Trong số kỹ thuật điều khiển thông minh, logic mờ lên phương pháp thể khả suy diễn óc người ứng dụng thành cơng nhiều lĩnh vực với vai trị quan sát Cơ chế suy diễn mờ xem phương pháp đơn giản hữu hiệu để tinh chỉnh điều khiển kinh điển 46 Chương 4: Bộ điều khiển mờ Hình 4.6: Bộ điều kiển mờ chỉnh định tham số Cấu trúc điều khiển mờ chỉnh định thông số PI gồm hai điều khiển, đó, điều khiển PI giữ vai trị điều khiển chính, tạo tín hiệu điều khiển đối tượng điều khiển mờ giữ vai trị quan sát, thay đổi thơng số điều khiển PI phù hợp theo thời gian thực Bộ điều khiển mờ đưa vào cấu trúc điều khiển hình 4.10, nhằm mục tiêu hiệu chỉnh tham số {Kp, Ki} cho điều khiển PI, dựa theo điều kiện {e, } Như điều khiển mờ có hai đầu vào sai số lực đạo hàm sai số lực Đầu vào sai số lực đạo hàm sai số lực điều khiển mờ hóa tập {NB, NS, ZE, PS, PB} Trong ký hiệu: NB Negative Big, NS Negative Small, ZE Zero, PS Position Small, PB Position Big Đầu điều khiển dKi mờ hóa tập {S,MS,M,MB,B} Trong ký hiệu: S Small, MS Medium Small, M Medium, MB Medium Big, B Big Hàm liên thuộc tập mờ sử dụng dạng tam giác xác định sau: Với a, b, c hoành độ ba đỉnh tập mờ A 47 Chương 4: Bộ điều khiển mờ Hình 4.7: Hàm liên thuộc sai số lực đạo hàm sai số lực Hình 4.8: Hàm liên thuộc dKi Luật điều khiển mờ chọn Bảng 4.1 Bảng 4.4: Luật điều khiển mờ cho điều khiển mờ chỉnh định tham số I [3] F NB NS ZE PS PB S S MS MS M S MS MS M MB MS MS M MB MB MS M MB MB B M MB MB B B dF NB NS ZE PS PB 4.6 Kết mô 48 Chương 4: Bộ điều khiển mờ Sơ đồ mơ lai lực vị trí với điều khiển mờ chỉnh định tham số điều khiển tích phân xây dựng matlab simulink sau: Hình 4.9: Sơ đồ mô hệ thống với điều khiển mờ Các tín hiệu vào khối tương tự bảng 3.1 Sơ đồ hệ thống phần thêm điều khiển mờ chỉnh định tham số tích phân Hình 4.10: Sơ đồ điều khiển mờ Bộ điều khiển mờ có hai đầu vào d, đầu Fuzzy Logic Controller dK i cộng với giá trị đặt K i sau nhân với tích phân sai số lực, tín hiệu đầu vào khối Controller Sơ đồ sử dụng khối tạo trễ để tránh vịng lặp đại số xảy với tín hiệu F mô simulink Kết mô 49 Chương 4: Bộ điều khiển mờ Các thông số hệ thống giá trị đặt giữ nguyên chương Hình 4.11: Tọa độ vị trí x Hình 4.12: Tọa độ vị trí y Nhận xét: 50 Chương 4: Bộ điều khiển mờ Kết mô tọa độ tay máy phần giống với chương Tín hiệu điều khiển nhanh chóng bám sát với tín hiệu đặt Sai số vị trí nhỏ đảm bảo điều kiện ràng buộc chuyển động robot Với kết mô ta thấy tọa độ vị trí với hệ thống sử dụng điều khiển mờ bám sát giá trị đặt so với hệ thống sử dụng điều khiển lai lực vị trí Như vậy, điều khiển mờ cho đáp ứng đầu vị trí tốt so với hệ thống không sử dụng điều khiển mờ Kết mơ lực với Fd = const Hình 4.13: Kết mô lực sử dụng điều khiển mờ Hình 4.14: Kết mơ lực vị trí ổn định 51 Chương 4: Bộ điều khiển mờ Kết mơ với Fd hình sin Hình 4.15: Kết mơ với lực đặt hình sin sử dụng điều khiển mờ Nhận xét: Cũng giống phần kết mơ vị trí, kết mô lực sử dụng điều khiển mơ cho đáp ứng giống với hệ thống không sử dụng Tuy nhiên, điều khác biệt điều khiển mờ giảm tần số dao động hệ thống cách đáng kể Kèm theo đó, điều khiển mờ giảm sai lệch vị trí ổn định, cho đáp ứng bám sát với tín hiệu đặt Như điều khiển mờ hoàn thành tốt công việc nâng cao chất lượng hệ thống 52 Kết luận Kết luận Các kết đồ án Nghiên cứu thuật toán lai lực vị trí Xây dựng mơ hình động học, động lực học Robot Planar ba bậc tự Thiết kết điều khiển dựa sở thuật toán lai lực vị trí Xây dựng mơ hình mơ matlab Simulink Xây dựng mơ hình ứng dụng điều khiển mờ chỉnh định tham số điều khiển So sánh kết hai hệ thống sử dụng điều khiển mờ khơng sử dụng Hạn chế Thuật tốn điều khiển áp dụng cho mơ hình mơ phỏng, chưa áp dụng cho mơ hình thật kết chạy mơ Mơ hình mơ bỏ qua nhiều thành phần phát sinh thực tế hệ thống thật Tất mang tính chất mơ Hướng nghiên cứu tiếp theo, ứng dụng thuật toán điều khiển để điều khiển cánh tay Robot thực tiễn cho Robot công nghiệp Tiếp tục nghiên cứu thuật tốn lai lực vị trí, tối ưu điều khiển giúp hệ thống hoạt động tốt 53 Tài liệu tham khảo Tài liệu tham khảo [1] TS Nguyễn Mạnh Tiến, Điều khiển robot công nghiệp, Nhà xuất Khoa học kỹ thuật Hà Nội, 2007 [2] http://www.robot.bmstu.ru/files/books/Robot%20Manipulator%20Control%20Theory %20and%20Practice%20-%20Frank%20L.Lewis.pdf, truy nhập ngày cuối 08/06/2018 [3] https://www.google.com.vn/url?sa=t&rct=j&q=&esrc=s&source=web&cd=2&ved=0 ahUKEwjVnfTInMbbAhUCfH0KHdRkDZcQFggsMAE&url=http%3A%2F %2Fold.tdt.edu.vn%2Fimages%2Fstories%2Ftapchikhoahocungdung %2Ftckhud20%2FUng%2520dung%2520bo%2520dieu%2520khien%2520sef %2520tuning%2520fussy-pi.pdf&usg=AOv Vaw0u5L290jc9wgTY_Gpry1nO, truy nhập ngày cuối 08/06/2018 [4] http://media.sharefoodforest.org/literatur/technik/The%20Mechanical%20Systems%2 0Design%20Handbook%20en.pdf, truy nhập ngày cuối 08/06/2018 [5] Bùi Cơng Cường, Nguyễn Dỗn Phước, Hệ mờ mạng noron ứng dụng, Nhà xuất Khoa học Kỹ thuật 54 Phụ lục PHỤ LỤC Bảng số liệu dung mô Chiều dài (m) Thanh nối thứ Thanh nối thứ hai Thanh nối thứ ba Khối lượng (kg) Mơ men qn tính (kgm2) l1 = 0,5 Chiều dài từ khớp đến tâm khối (m) lg1 = 0,25 0,3 l2 = 0,4 lg2 = 0,2 0,2 l3 = 0,35 lg3 = 0,175 0,1 Chương trình matlab % Các thơng số robot global l1 l2 l3 m1 m2 m3 lg1 lg2 lg3 J1 J2 J3 g; l1=0.5;l2=0.4;l3=0.35;lg1=0.25;lg2=0.2;lg3=0.175;m1=3;m2=2;m3=1;J1=0.3;J2=0.2; J3=0.1;g=9.8; % Động học thuận robot function y = DH(u) ThongSo; q1=u(1); q2=u(2); q3=u(3); j11= -l1*cos(q1)-l2*cos(q1+q2)-l3*cos(q1+q2+q3) ; j12= l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3) ; J=[j11;j12;0]; y=J; % Ma trận Jacobian function y = Matrix_J(u) ThongSo; q1=u(1); q2=u(2); q3=u(3); 55 Phụ lục j11= l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3) ; j12= l2*sin(q1+q2)+l3*sin(q1+q2+q3) ; j13= l3*sin(q1+q2+q3) ; j21= l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3) ; j22= l2*cos(q1+q2)+l3*cos(q1+q2+q3) ; j23= l3*cos(q1+q2+q3) ; J=[j11 j12 j13;j21 j22 j23; 0 0]; y=J; % Ma trận J(q) function y = Matrix_Jq(q1,q2,q3) ThongSo; J1 = -(l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3)) ; J2 = -l2*sin(q1+q2)-l3*sin(q1+q2+q3) ; J3 = -l3*sin(q1+q2+q3) ; J=[J1;J2;J3]; y = J; % Ma trận M function y = Matrix_M(q1,q2,q3) ThongSo; M11 = m1*lg1^2+m2*(l1^2+lg2^2+2*l1*lg2*cos(q2)) +J1+J2+J3+m3*(l1^2+l2^2+lg3^2+2*l1*lg3*cos(q2+q3)+2*l2*lg3*cos(q3)+2*l1*l2*cos (q2)); M12 = m2*(lg2^2+l1*lg2*cos(q2)) +J2+J3+m3*(l2^2+lg3^2+l1*l2*cos(q2)+l1*lg3*cos(q2+q3)+2*l2*lg3*cos(q3)); M13 = m3*(lg3^2+l1*lg3*cos(q2+q3)+l2*lg3*cos(q3))+J3; M14 = -(l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3)) ; M21 = m2*(lg2^2+l1*lg2*cos(q2)) +m3*(l2^2+lg3^2+l1*l2*cos(q2)+l1*lg3*cos(q2+q3)+2*l2*lg3*cos(q3))+J2+J3 ; M22= m2*lg2^2+m3*(l2^2+lg3^2+2*l2*lg3*cos(q3))+J2+J3; M23= m3*(lg3^2+l2*lg3*cos(q3))+J3; M24 = -(l2*sin(q1+q2)+l3*sin(q1+q2+q3)) ; M31= m3*(lg3^2+l1*lg3*cos(q2+q3)+l2*lg3*cos(q3))+J3; M32= m3*(lg3^2+l2*lg3*cos(q3))+J3; M33= m3*lg3^2+J3; M34 = -l3*sin(q1+q2+q3) ; M41 = l1*sin(q1)+l2*sin(q1+q2)+l3*sin(q1+q2+q3) ; M42 = l2*sin(q1+q2)+l3*sin(q1+q2+q3) ; M43 = l3*sin(q1+q2+q3) ; M = [M11 M12 M13 M14;M21 M22 M23 M24;M31 M32 M33 M34;M41 M42 M43 0]; y = M; % Ma trận V function y = Matrix_V(u) ThongSo; q2=u(2); q3=u(3); 56 Phụ lục dq1=u(4); dq2=u(5); dq3=u(6); V1 = -m2*dq2*(2*dq1+dq2)*l1*lg2*sin(q2)-m3*(dq2*(2*dq1+dq2)*l1*l2*sin(q2)+ (dq2+dq3)*(2*dq1+dq2+dq3)*l1*lg3*sin(q2+q3)+dq3*(2*dq1+2*dq2+dq3)*l2*lg3*sin(q 3)); V2 = -m3*(dq1*(dq2+dq3)*l1*lg3*sin(q2+q3)+dq3*(2*dq1+2*dq2+dq3)*l2*lg3*sin(q3))(m2*lg2+m3*l2)*l1*dq1*dq2*sin(q2)-m2*(dq1^2+dq1*dq2)*l1*lg2*sin(q2)m3*((dq1^2+dq1*dq2)*l1*l2*sin(q2)+dq1*(dq1+dq2+dq3)*l1*lg3*sin(q2+q3)); V3 = -m3*((dq2+dq3)*dq1*l1*lg3*sin(q2+q3)+dq3*(dq1+dq2)*l2*lg3*sin(q3))m3*(dq1*(dq1+dq2+dq3)*l1*lg3*sin(q2+q3)+ (dq1+dq2)*(dq1+dq2+dq3)*l2*lg3*sin(q3)); V = [V11;V21;V31]; y = V; % Ma trận G function y = Matrix_G(u) ThongSo; q1=u(1); q2=u(2); q3=u(3); A11= (m1*g*lg1*cos(q1)+m2*g*(l1*cos(q1)+lg2*cos(q1+q2)) +m3*g*(l1*cos(q1)+l2*cos(q1+q2)+lg3*cos(q1+q2+q3))); A21= (m2*g*lg2*cos(q1+q2)+m3*g*(l2*cos(q1+q2)+lg3*cos(q1+q2+q3))); A31= m3*g*lg3*cos(q1+q2+q3); A=[A11;A21;A31]; y=A; % Bộ điều khiển vị trí lực function y = PD_Controler(u) parametersPD; J=Matrix_J(u); Jq=Matrix_Jq(u(1),u(2),u(3)); G=Matrix_G(u); IS=[0 0;0 0;0 1]; S=[1 0;0 0; 0 0]; dq=[u(4);u(5);u(6)]; y=J'*(S*((Kp)*(Xd-DH(u))-(Kd)*J*dq))+G+J'*IS*Jq*(u(7))+Jq*(Fd+u(8)); % Thông số điều khiển giá trị đặt Kp=[9510 0; 4020 0; 0 1000]; Kd=[900 0; 400 0; 0 300]; 57 Phụ lục Fd=10; Ki=10; Xd=[0.8;0.8;0]; % Hàm tính giá trị F function y = Output_ddq(u) ThongSo; q1=u(4); q2=u(5); q3=u(6); dq1=u(7); dq2=u(8); dq3=u(9); a= -(dq1^2*l1*cos(q1)+(dq1+dq2)^2*l2*cos(q1+q2)+ (dq1+dq2+dq3)^2*l3*cos(q1+q2+q3)+14*(dq1*l1*sin(q1)+(dq1+dq2)*l2*sin(q1+q2)+ (dq1+dq2+dq3)*l3*sin(q1+q2+q3))49*(l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3)+0.8)); A=[u(1);u(2);u(3);a]; M=Matrix_M(q1,q2,q3); y=M\A; 58 ... Xây dụng mơ hình động lực học robot song phẳng 3DOF Xây dựng toán điều khiển lực Xây dựng thuật toán điều khiển lai lực vị trí cho robot Sử dụng điều khiển mờ để chỉnh định tham số điều khiển. .. điều khiển lai lực vị trí 3.3 Kết mô Sơ đồ mô matlab Simulink xây dựng dựa phương trình (3.4) thuật tốn điều khiển lai lực vị trí sau: Hình 3.4: Sơ đồ điều khiển lai lực vị trí cho robot ba bậc. .. 3.3: Sơ đồ điều khiển lai lực vị trí .34 Hình 3.4: Sơ đồ điều khiển lai lực vị trí cho robot ba bậc tự .37 Hình 3.5: Tọa độ vị trí x .38 Hình 3.6: Tọa độ vị trí y