”Ứng dụng thuật toán điều khiể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 toán điều khiển lai lực vị trí điều khiển Robot Planar ba bậc tự do” em tự thiết kế dƣới hƣớng dẫn PGS.TS Nguyễn Phạm Thục Anh Các số liệu kết hồn tồn trung thực xác Để hồn thành đồ án này, em sử dụng tài liệu đƣợc 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 hoàn toàn chịu trách nhiệm Hà Nội, ngày 11 tháng 06 năm 2018 Sinh viên thực Lê Xuân Tùng MỤC LỤC DANH MỤC HÌNH VẼ I DANH MỤC BẢNG SỐ LIỆU III LỜI NÓI ĐẦU IV CHƢƠNG 1: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP .1 1.1 SƠ LƢỢC LỊCH SỬ PHÁT TRIỂN VÀ ĐỊNH NGHĨA CỦA ROBOT CÔNG NGHIỆP .1 1.2 CẤU TRÚC CƠ BẢN CỦA ROBOT CÔNG NGHIỆP 1.2.1 Các thành phần Robot cơng nghiệp 1.2.2 Kết cấu tay máy 1.3 PHÂN LOẠI ROBOT CÔNG NGHIỆP 1.4 ỨNG DỤNG ROBOT CÔNG NGHIỆP 1.4.1 Mục tiêu ứng dụng robot công nghiệp .7 1.4.2 Một số ứng dụng điển hình robot cơng nghiệp CHƢƠNG 2: CÁC PHƢƠNG PHÁP ĐIỀU KHIỂN ROBOT CÔNG NGHIỆP 10 2.1 ĐIỀU KHIỂN QUỸ ĐẠO CHUYỂN ĐỘNG 11 2.1.1 Điều khiển không gian khớp 11 2.1.2 Điều khiển không gian làm việc 14 2.2 ĐIỀU KHIỂN KẾT HỢP QUỸ ĐẠO VÀ LỰC TƢƠNG TÁC 17 2.2.1 Vấn đề điều khiển lực vị trí robot 17 2.2.2 Phƣơng pháp điều khiển 18 CHƢƠNG : ỨNG DỤNG PHƢƠNG PHÁP LAI VỊ TRÍ LỰC ĐIỀU KHIỂN ROBOT PLANAR BẬC TỰ DO 27 3.1 ĐỘNG HỌC ROBOT 29 3.1.1 Động học thuận 29 3.1.2 Ma trận Jacobi 30 3.1.3 Phƣơng trình động lực học .31 3.2 THIẾT KẾ BỘ ĐIỀU KHIỂN LỰC VÀ QUỸ ĐẠO CỦA ROBOT 37 3.3 MÔ PHỎNG HỆ THỐNG KIỂM CHỨNG KẾT QUẢ 39 3.3.1 Mô phần mềm Matlab Simulink 39 3.3.2 Kết mô 39 CHƢƠNG 4: SỬ DỤNG BỘ ĐIỀU KHIỂN MỜ NÂNG CAO CHẤT LƢỢNG CỦA HỆ THỐNG 44 4.1 GIỚI THIỆU BỘ ĐIỀU KHIỂN MỜ 44 4.2 CÁC BƢỚC TỔNG HỢP HỆ THỐNG ĐIỀU KHIỂN MỜ 45 4.3 THIẾT KẾ BỘ ĐIỀU KHIỂN MỜ HIỆU CHỈNH THAM SỐ BỘ ĐIỀU KHIỂN TAY ROBOT 45 4.3.1 Bộ điều khiển mờ hiệu chỉnh tham số điều khiển vị trí 45 4.3.2 Bộ điều khiển mờ hiệu chỉnh tham số điều khiển lực .49 4.4 MÔ PHỎNG HỆ THỐNG KIỂM CHỨNG KẾT QUẢ .55 4.4.1 Mô Matlab Simulink 55 4.4.2 Kết mô 55 4.5 KẾT LUẬN .57 KẾT LUẬN 59 TÀI LIỆU THAM KHẢO 60 PHỤ LỤC .61 Danh mục hinh vẽ DANH MỤC HÌNH VẼ Chƣơng Hình 1.1: Hình ảnh Robot cơng nghiệp điển hình Hình 2: Các thành phần robot cơng nghiệp Hình 3: Robot kiểu tọa độ Đề-các Hình 4: Robot kiểu tọa độ trụ Hình 5: Robot kiểu tọa độ cầu Hình 6: Robot cấu dạng khớp nối Hình 7: Robot kiểu SCARA Hình 8: Ứng dụng robot công nghiệp hàn Hình 9: Ứng dụng robot công nghiệp lắp ráp ô tô Chƣơng Hình 1: Các phƣơng pháp điều khiển robot công nghiệp 10 Hình 2: Hệ thống điều khiển phi tuyến sở mô hình 11 Hình 3: Hệ thống điều khiển PD bù trọng trƣờng 13 Hình 4: Hệ thống điều khiển PID 14 Hình 5: Hệ thống điều khiển gián tiếp 15 Hình 6: Hệ thống điểu khiển trực tiếp 15 Hình 7: Hệ thống điều khiển kết hợp PD bù trọng trƣờng Jacobi chuyển vị 16 Hình 8: Hệ thống kết hợp PD bù trọng trƣờng ma trận Jacobi nghịch đảo 17 Hình 9: Sơ đồ hệ điều khiển lai trở kháng 21 Hình 10: Sơ đồ điều khiển lai vị trí lực theo phƣơng pháp tuyến tính hóa xác 24 Hình 11: Sơ đồ điều khiển lai vị trí lực sử dụng mạch phản hồi kinh điển 26 Chƣơng SVTH: Lê Xuân Tùng i Danh mục hinh vẽ Hình 1: Đặt hệ trục tọa độ cho mơ hình robot planar 27 Hình 2: Mơ tả chuyển động robot từ vị trí A đến vị trí B 28 Hình 3: Sơ đồ hệ thống Matlab Simulink 39 Hình 4: Tọa độ khâu tác động cuối robot theo phƣơng x,y,z 41 Hình 5: Lực tác động lên mơi trƣờng robot 41 Hình 6: Tọa độ khâu tác động cuối robot theo phƣơng y 42 Hình 7: Lực tay robot tác động lên môi trƣờng 43 Chƣơng Hình 1: Sơ đồ khối hệ thống mờ 44 Hình 2: Hàm liên thuộc ef 46 Hình 3: Hàm liên thuộc def 47 Hình 4: Hàm liên thuộc Δ 47 Hình 5: Cấu trúc điều khiển mờ 48 Hình 6: Hàm liên thuộc eP 50 Hình 7: Hàm liên thuộc deP 51 Hình 8: Hàm liên thuộc 51 Hình 9: Hàm liên thuộc 52 Hình 10: Hàm liên thuộc 52 Hình 11: Sơ đồ hệ thống Matlab Simulink 55 Hình 12: So sánh đáp ứng lực hai dạng điều khiển 56 Hình 13: So sánh đáp ứng vị trí theo phƣơng y hai dạng điều khiển 57 SVTH: Lê Xuân Tùng ii Danh mục bảng số liệu DANH MỤC BẢNG SỐ LIỆU Bảng 3.1: Thông số Robot 28 Bảng 3.2: Bảng DH 29 Bảng 4.1: Độ lợi K1 .48 Bảng 4.2: Bảng luật điều khiển Δ 49 Bảng 4.3: Độ lợi KP ,KD 53 Bảng 4.4: Bảng luật điều khiển Δ 53 Bảng 4.5: Bảng luật điều khiển Δ 54 SVTH: Lê Xuân Tùng iii Lời nói đầu LỜI NÓI ĐẦU Ngày nay, theo phát triển mạnh mẽ xã hội, nhu cầu nâng cao giá trị sản xuất chất lƣợng sản phẩm đòi hỏi ứng dụng rộng rãi phƣơng tiện tự động hóa sản xuất Xu hƣớng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành phát triển mạnh mẽ Nổi bật số Robot cơng nghiệp với ứng dụng quan trọng sản xuất đời sống Sau thời gian học tập nghiên cứu, em đƣợc giao đề tài tốt nghiệp với nội dung đề tài nhƣ sau: Tên đề tài: Ứng dụng thuật tốn điều khiển lai lực vị trí điều khiển Robor Planar ba bậc tự Cơ sở khoa học thực tiễn đề tài: Ứng dụng phƣơng pháp điều khiển lai vị trí – lực cho điều khiển robot không gian ràng buộc Mục đích đề tài: Xây dựng cấu trúc thuật tốn điều khiển cho robot Planar ba bậc tự Nội dung đề tài, vấn đề cần giải quyết:  Xây dựng mơ hình tốn học cho robot Planar bậc tự  Xây dựng hệ thống điều khiển quỹ đạo lực với độ xác tốt  Đề xuất phƣơng án nâng cao chất lƣợng đáp ứng hệ thống Luận văn đƣợc trình bày thành chƣơng với nội dung đƣợc trình bày nhƣ sau: Chƣơng I - Tổng quan Robot công nghiệp: mơ tả tổng quan robot vai trị robot cơng nghiệp thời kì Chƣơng II – Các phƣơng pháp điều khiển Robot công nghiệp: phân tích ƣu nhƣợc điểm số phƣơng pháp điều khiển robot điển hình khơng gian tự không gian ràng buộc Chƣơng III - Ứng dụng phƣơng pháp điều khiển lai lực – vị trí điều khiển robot planar ba bậc tự do: Thiết kế điều khiển đồng thời lực vị trí robot không gian ràng buộc Đề xuất phƣơng án nâng cao chất lƣợng điều khiển SVTH: Lê Xuân Tùng iv Lời nói đầu Chƣơng IV - Ứng dụng điều khiển mờ cho hệ thống điều khiển lai lực – vị trí robot: Kiểm tra cải thiện chất lƣợng điều khiển so với không dùng điều khiển mờ SVTH: Lê Xuân Tùng v Chương 1: Tổng quan Robot công nghiệp CHƢƠNG 1: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 1.1 Sơ lƣợc lịch sử phát triển định nghĩa robot công nghiệp Thuật ngữ Robot lần đầu xuất năm 1922 tác phẩm „Rossum‟s Universal Robots” Karel Capek Theo tiếng Séc Robot ngƣời làm tạp dịch Trong tác phẩm nhân vật Rossum trai ông tạo máy gần giống nhƣ ngƣời để hầu hạ ngƣời Hơn 20 năm sau, ƣớc mơ viễn tƣởng Karel bắt đầu thành thực Ngay sau chiến tranh giới thứ 2, Hoa kỳ xuất tay chép hình điều khiển từ xa phịng thí nghiệm vật liệu phóng xạ Năm 1954, George C.Devol thiết kế thiết bị có tên “cơ cấu lề dùng để chuyển hàng hóa theo chƣơng trình” Đến năm 1956, Devol với Joseph F.Engelber, kỹ sƣ trẻ công nghiệp hàng không, tạo loại robot công nghiệp năm 1959 công ty Unimation Từ năm 70 việc nghiên cứu nâng cao tính robot đƣợc ý nhiều đến lăp đặt thêm cảm biến ngoại tín hiệu để nhận biết mơi trƣờng làm việc Tại trƣờng đại học tổng hợp Stanford ngƣời ta tạo loại robot lắp ráp tự động điều khiển máy tính sở xử lý thơng tin từ cảm biến lực thị giác Vào thời gian này, công ty IBM chế tạo loại robot có cảm biến xúc giác Từ năm 1980, vào năm 1990, áp dụng rộng rãi tiến kỹ thuật vi xử lý công nghệ thông tin, số lƣợng robot công nghiệp gia tăng, giá thành giảm rõ rệt, tính có nhiều bƣớc tiến vƣợt bậc Nhờ robot cơng nghiệp có vị trí quan trọng dây chuyền sản xuất đại Tuy nhiên phát triển mạnh mẽ robot gây khó khăn cho việc định nghĩa robot cho phù hợp Theo học viện robot Hoa Kỳ (Robot Institute of America) (năm 1979), robot tay máy lập trình tái lập trình, thực chuyển động lập trình để di chuyển vật liệu, chi tiết, dụng cụ hay thiết bị chuyên dụng Theo hội Robot Nhật Bản (ARA, Japanese Robot Association), robot hệ thống có chuyển động tƣơng tự sinh vật có trí thông minh, hoạt động theo điều khiển ngƣời SVTH: Lê Xuân Tùng Chương 1: Tổng quan Robot công nghiệp Tự điển Oxford định nghĩa robot máy thực chuỗi hoạt động phức tạp tự động, lập trình trƣớc từ máy tính Ngồi ngành robotics ngành khoa học có nhiệm vụ: Thiết kế, chế tạo, điều khiển lập trình robot Nghiên cứu áp dụng robot Nghiên cứu trình điều khiển áp dụng vào giải thuật để điều khiển robot Hình 1.1: Hình ảnh Robot cơng nghiệp điển hình 1.2 Cấu trúc robot cơng nghiệp 1.2.1 Các thành phần Robot cơng nghiệp Trên hình giới thiệu phận chủ yếu robot công nghiệp: SVTH: Lê Xuân Tùng Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Đầu điều khiển lƣợng hiệu chỉnh cho tham số khiển lực ta đặt Δ 200;200] [-20;20] Δ điều với tập vũ trụ có đƣợc qua thử nghiệm ta chọn [- Định nghĩa tập mờ cho đầu vào đầu điều khiển Các biến đầu vào biến đầu đƣợc mờ hóa với giá trị biến ngơn ngữ nhƣ sau:  NB: Negative Big – âm nhiều  NM: negative Medium – âm vừa  NS: Negative Small – âm nhỏ  ZR: Zero  PS: Positive Small – dƣơng nhỏ  PM: Positive Medium – dƣơng vừa  PB: Positive Big – dƣơng lớn Chọn hàm liên thuộc đầu vào đầu mờ hàm tam giác – trimf đƣợc định nghĩa nhƣ hình vẽ: Hình 6: Hàm liên thuộc eP SVTH: Lê Xuân Tùng 50 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Hình 7: Hàm liên thuộc deP Hình 8: Hàm liên thuộc SVTH: Lê Xuân Tùng 51 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Hình 9: Hàm liên thuộc Chọn mơ hình mờ Mamdami Ta có cấu trúc điều khiển: Hình 10: Hàm liên thuộc c, Luật điều khiển mờ Thông số độ lợi KP KD ảnh hƣởng đến thời gian tăng (rise time), độ vọt lố (overshoot) thời gian xác lập (setting time) đáp ứng đƣợc cho bảng Các thông tin đƣợc xem sở tri thức để xác định luật chỉnh định mờ cho điều khiển SVTH: Lê Xuân Tùng 52 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Bảng 4.2: Độ lợi KP KD Độ lợi Thời gian tăng Độ vọt lố Thời gian xác lập Sai số xác lập KP Giảm Tăng Thay đổi Giảm KD Giảm Giảm Về lý thuyết Cải thiện không tác động KD nhỏ Từ sở tri thức điều khiển, luật điều khiển điều khiển mờ đƣợc thiết kế thông qua thực nghiệm đƣợc cho bảng Bảng 4.3: Luật điều khiển cho thay đổi ef/def NB NM NS ZE PS PM PB NB PB PB PB PB PM PS ZE NM PB PB PB PB M ZE ZE NS PM PM PM PM ZR PS PS ZE PM PM PS ZE NS NS NS PS PS PS ZE NM NM NM NM PM PS ZE NS NM NM NB NB PB ZE ZE NM NM NM NB NB SVTH: Lê Xuân Tùng 53 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Bảng 4.4: Luật điều khiển cho thay đổi ef/def NB NM NS ZE PS PM PB NB PS NS NB NB NB NM PS NM PS NS NB NM NM NS ZE NS ZE NS NM NS NS NS ZE ZE ZE NS NS NS NS NS ZE PS NM NS ZE PS PS PM PB PM ZE ZE ZE ZE ZE ZE ZE PB PB PM PM PM PS PS PB Có tất 49 luật điều khiển Fuzzy Thông số điều khiển lực đƣợc cập nhật thông qua luật lúc có dạng: (4.4) (4.5) Đầu điều khiển vị trí lúc này: , d, - ( ) [( )( ) ( ) ̇] (4.6) Phương pháp giải mờ Cơ chế suy luận mờ đƣợc chọn MIN – MAX giải mờ theo ngun lí trung bình phƣơng pháp cực đại, xác định phƣơng trình: ( SVTH: Lê Xuân Tùng ) ∑ ∑ (4.7) 54 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Với ( ) thông số hiệu chỉnh tham số điều khiển lực, lần lƣợt hồnh độ điểm trung bình giá trị hàm liên thuộc ngõ đƣợc xác định luật thứ i n luật tác động thời điểm xét 4.4 Mô hệ thống kiểm chứng kết 4.4.1 Mơ Matlab Simulink Hình 11: Sơ đồ hệ thống Matlab Simulink 4.4.2 Kết mơ Tín hiệu đặt Xd=[0.8;0.9;0] Fd=10N Phƣơng trình mặt phẳng: Bộ điều khiển vị trí: [ ]; [ ] Bộ điều khiển lực: Ki=0.25 SVTH: Lê Xuân Tùng 55 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Hình 12: So sánh đáp ứng lực hai dạng điều khiển SVTH: Lê Xuân Tùng 56 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Hình 13: So sánh đáp ứng vị trí theo phƣơng y hai dạng điều khiển Ta thấy chất lƣợng đáp ứng đƣợc cải thiện nhƣng điều khiển chƣơng III cho chất lƣợng đáp ứng vị trí theo phƣơng y tốt nên cải thiện nhỏ gần nhƣ không đáng kề Rõ ràng hệ thống sử dụng điều khiển mờ cho ta đáp ứng lực với chất lƣợng tốt so với hệ thống sử dụng điều khiển thông thƣờng, giảm sai số lực xuống mức thấp ( từ 0,4% xuống 0.15%) 4.5 Đánh giá kết Bằng việc sử dụng thêm điều khiển mờ để hiệu chỉnh tham số cho điều khiển lƣc, ta thu đƣợc đáp ứng lực tác động lên môi trƣờng robot với chất lƣợng tốt, đạt độ xác cao Cùng với đáp ứng quỹ đạo robot ổn định hai phƣơng pháp, ta xem nhƣ hoàn thành yêu cầu đặt toán điều khiển đồng thời quỹ đạo lực robot Biên độ thời điểm độ sai lệch quỹ đạo đƣợc giảm SVTH: Lê Xuân Tùng 57 Chương 4: Sử dụng điều khiển mờ nâng cao chất lượng hệ thống Khi chọn tập giá trị mờ tập điều khiển thích hợp luật điều khiển mờ giúp cho hệ đạt đƣợc độ xác cao, với giá trị đặt nhỏ SVTH: Lê Xuân Tùng 58 Kết luận KẾT LUẬN Sau thời gian làm việc nghiêm túc, dƣới hƣớng dẫn PGS.TS.Nguyễn Phạm Thục Anh, đồ án tốt nghiệp em hoàn thành thời gian Đồ án giải đƣợc vấn đề đặt gồm:  Xây dựng mô hình tốn học cho robot Planar bậc tự  Xây dựng hệ thống điều khiển quỹ đạo lực với độ xác tốt  Đề xuất phƣơng án làm tăng chất lƣợng đáp ứng hệ thống Kết thể chất lƣợng hệ thống tƣơng đối tốt, đáp ứng lực nhanh, sai số lực vị trí xác lập tƣơng đối nhỏ đặc biệt sử dụng điều khiển mờ Tuy nhiên điều kiện thời gian kiến thức hạn chế nên số vấn đề chƣa đáp ứng đƣợc nhƣ giảm thời gian xác lập vị trí Rất mong nhận đƣợc góp ý chân thành thầy cô Em xin chân thành cảm ơn Hà Nội, ngày 11 tháng 06 năm 2018 Sinh viên thực Lê Xuân Tùng SVTH: Lê Xuân Tùng 59 Tài liệu tham khảo TÀI LIỆU THAM KHẢO [1] Nguyễn mạnh Tiến (2007): Điều khiển Robot công nghiệp NXB Khoa học Kỹ Thuật [2] Phạm Đăng Phƣớc (2007): Robot công nghiệp NXB Xây dựng [3] Đào Văn Hiệp (2006): Kỹ thuật Robot NXB Khoa học Kỹ thuật [4] Bùi Công Cƣờng, Nguyễn Doãn Phƣớc: Hệ mờ, mạng noron ứng dụng NXB Khoa học Kỹ thuật [5] Frank L.Lewis, Darren M.Dawson, Chaouki T.Abdallah: Robot manipulator control theory and practice [6] Yong Tao, Jiaqi Zheng, Yuanchang Lin, Tianmiao Wang, Hegen Xiong, Guotian He and Dong Xu: Fuzzy PID control method of deburring industrial robots SVTH: Lê Xuân Tùng 60 Phụ lục PHỤ LỤC CHƢƠNG TRÌNH MATLAB % Cac thong so 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; % Dong hoc thuan 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]; y=J; % Ma tran J(q) function y = Matrix_Jq(u) ThongSo; q1=u(1); q2=u(2); q3=u(3); j11= -l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3) ; j21= -l2*sin(q1+q2)-l3*sin(q1+q2+q3) ; j31= -l3*sin(q1+q2+q3) ; j12= l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3) ; j22= l2*cos(q1+q2)+l3*cos(q1+q2+q3) ; j32= l3*cos(q1+q2+q3) ; J=[j11 j12;j21 j22;j31 j32]; y=J; % Ma tran JT function y = Matrix_JT(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]; SVTH: Lê Xuân Tùng 61 Phụ lục y = J; % Ma tran quan tinh function y = Matrix_M(q1,q2,q3) %#codegen 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*lg 3*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 tran tuong ho va huong tam function y = Matrix_V(u) %#codegen ThongSo; q1=u(1); q2=u(2); q3=u(3); dq1=u(4); dq2=u(5); dq3=u(6); V11 = -m3*(l2*lg3*(2*dq1+2*dq2+dq3)*dq3*sin(q3)+l1*lg3*(2*dq1+dq2+dq3 )*(dq2+dq3)*sin(q2+q3))-(m2*lg2+m3*l2)*l1*sin(q2)*(2*dq1*dq2+dq2^2); V21 = 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)+m3*(((dq1)^2+dq1*dq2)*l1*l2*sin(q2)+ ((dq1)^2+dq1*dq2+dq1*dq3)*l1*lg3*sin(q2+q3))+m2*((dq1)^2+dq1*dq2)*l1*lg2*sin(q 2); V31 = -m3*((dq2+dq3)*dq1*l1*lg3*sin(q2+q3 )+dq3*(dq1+dq2)*l2*lg3*sin(q3))+m3*((dq1)^2+dq1*dq2+dq1*dq3)*l1*lg3*sin(q2+q3) +(dq1+dq2)*(dq1+dq2+dq3)*l2*lg3*sin(q3); SVTH: Lê Xuân Tùng 62 Phụ lục V=[V11;V21;V31]; y = V; % Ma tran luc function y = Matrix_G(u) %#codegen 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; % Bo dieu khien vi tri function y = PD_Controler(u) parametersPD; J=Matrix_Jq(u); G=Matrix_G(u); IS=[0 0;0 0; 0 1]; dq=[u(4);u(5);u(6)]; y=IS*(J*(Kp*(Xd-DH(u))-Kd*J'*dq))+G; % Bo dieu khien luc function y = Force_Controller(u) parametersPD; A=Matrix_Jq(u(2),u(3),u(4)); S=[1 0;0 0; 0 0]; y=S*A*(Fd+Ki*u(1)); % Bo dieu khien vi tri dung bo dieu khien mo function y = PD_Controler2(u) q1=u(1); q2=u(2); q3=u(3); parametersPD; J=Matrix_JT(q1,q2,q3); G=Matrix_G(u); X=DH(u); A=[u(7) 0;0 u(7) 0;0 u(7)]; B=[u(8) 0;0 u(8) 0;0 u(8)]; IS=[0 0;0 0; 0 1]; dq=[u(4);u(5);u(6)]; y=IS*J*(((Kp+A)*(Xd-X)-(Kd+B)*J'*dq))+G; SVTH: Lê Xuân Tùng 63 Phụ lục % Bo dieu khien luc dung bo dieu khien mo function y = Force_Controller2(u) parametersPD; q1=u(2); q2=u(3); q3=u(4); A=Matrix_Jq(q1,q2,q3); J=Matrix_JT(q1,q2,q3); S=[1 0; 0 0; 0 0]; y=(Ki+u(6))*u(1)*S*A+A*u(5); % Ma tran gia toc khop va luc phan hoi 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)+2 *(dq1*l1*sin(q1)+(dq1+dq2)*l2*sin(q1+q2)+(dq1+dq2+dq3)*l3*sin(q1+q2+q3))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); B=M\A; y=B; SVTH: Lê Xuân Tùng 64 ... nghiệp điển hình 1.2 Cấu trúc robot cơng nghiệp 1.2.1 Các thành phần Robot cơng nghiệp Trên hình giới thiệu phận chủ yếu robot công nghiệp: SVTH: Lê Xuân Tùng Chương 1: Tổng quan Robot cơng nghiệp. .. Trong hầu hết ứng dụng cơng nghiệp, mục tiêu Robot hệ thống tự động hóa thiết lập vận hánh ổn định xác Khi thực ứng dụng công nghiệp, cánh tay Robot dù trực tiếp hay gián tiếp có ràng buộc với môi... nghịch đảo 17 Hình 9: Sơ đồ hệ điều khiển lai trở kháng 21 Hình 10: Sơ đồ điều khiển lai vị trí lực theo phƣơng pháp tuyến tính hóa xác 24 Hình 11: Sơ đồ điều khiển lai vị trí lực