Điều khiển thích nghi cho robot NDOF trên cơ sở bộ quan sát

LỜI CAM ĐOAN Em xin cam đoan đồ án tốt nghiệp: “Điều khiển thích nghi Robot nDOF sở quan sát” 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 hoàn toàn với thực tế Để hoàn thành đồ án 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 chịu hoàn toàn trách nhiệm Hà Nội, ngày 21 tháng năm 2016 Ngƣời thực Khƣơng Đức Hạnh MỤC LỤC LỜI CAM ĐOAN MỞ ĐẦU 1 Lý chọn đề tài: Mục tiêu nghiên cứu: .1 Đối tƣợng, phạm vi nghiên cứu: Phƣơng pháp nghiên cứu: .2 Ý nghĩa khoa học thực tiễn đề tài Cấu trúc luận văn: Chƣơng 1: TỔNG QUAN PHƢƠNG PHÁP ĐIỀU KHIỂN ROBOT 1.1 Tổng quan robot 1.1.1 Lịch sử phát triển robot 1.1.2 Cấu tạo robot 1.1.3 Cơ cấu khí robot 1.1.4 Các thông số đặc trƣng hệ thống robot 1.1.5 Hệ thống truyền động robot .9 1.1.6 Hệ thống điều khiển chuyển động 10 1.1.7 Cảm biến 10 1.1.8 Ứng dụng robot công nghiệp 10 1.2 Phƣơng pháp điều khiển robot 12 1.2.1 Phƣơng pháp PD bù trọng trƣờng 15 1.2.2 Thuật toán PID 16 1.2.3 Phƣơng pháp điều khiển phi tuyến sở mô hình 17 1.2.4 Phƣơng pháp điều khiển Li - Slotine 18 Chƣơng 2:BỘ ĐIỀU KHIỂN LI - SLOTINE THÍCH NGHI VÀ BỘ QUAN SÁT THÍCH NGHI 22 2.1 Cơ sở lý thuyết hệ phi tuyến 22 2.1.1 Hệ phi tuyến 22 2.1.2 Điểm cân điểm dừng hệ 24 2.1.3 Tính ổn định điểm cân 24 2.1.4 Tiêu chuẩn Lyapunov 25 2.2 Phƣơng pháp điều khiển thích nghi Li - Slotine 30 2.3 Bộ quan sát trạng thái thích nghi 32 2.3.1 Bộ quan sát trạng thái Luenberger 32 2.3.2 Bộ quan sát thích nghi cho robot 33 Chƣơng 3: MÔ HÌNH TOÁN ROBOT PLANAR 36 3.1 Cấu trúc tham số robot 36 3.2 Bài toán động học thuận vị trí 36 3.2.1 Tham số nối khớp 37 3.2.2 Phƣơng pháp thiết kế khung tọa độ 37 3.3 Bài toán động học ngƣợc vị trí 39 3.4 Động lực học robot 40 3.5 Thiết kế quỹ đạo chuyển động 43 Chƣơng 4: MÔ PHỎNG TRÊN MATLAB/SIMULINK 45 4.1 Sơ đồ nguyên lý điều khiển 45 4.2 Sơ đồ khối mô Matlab/simulink 46 4.3.Kết mô cho robot Pelican 48 4.3.1 Kết mô cho quan sát thích nghi 49 4.3.2 Kết mô điều khiển thích nghi Li-SIotine 52 KẾT LUẬN 56 TÀI LIỆU THAM KHẢO 57 PHỤ LỤC 58 DANH MỤC BẢNG BIỂU, HÌNH VẼ BẢNG Bảng 3.1: Thông số robot Pelican 36 Bảng 4.1: Tham số ƣớc lƣợng cho robot 48 Bảng 4.2: Bảng tham số điều khiển 48 Bảng 4.3: Tham số quan sát 49 HÌNH VẼ Hình 1.1 Bộ phận cấu thành Robot .5 Hình 1.2: Khớp tịnh tiến khớp quay Hình 1.3 Cấu tạo bàn tay máy .7 Hình 1.4: Dạng tay gắn vào thân Hình 1.5: Dạng hệ tọa độ cực Hình 1.6: Dạng hình trụ Hình 1.7: Dạng SCARA Hình 1.8: Robot sử dụng công đoạn cấp liệu lắp ráp 12 Hình 1.9: Sơ đồ nguyên lý điều khiển 13 Hình 1.10: Sơ đồ khối thuật toán điều khiển robot 14 Hình 1.11: Sơ đồ khối phƣơng pháp điều khiển PD bù trọng trƣờng 16 Hình 1.12: Sơ đồ điều khiển PID 16 Hình 1.13: Sơ đồ nghiệm ∆ = 17 Hình 1.14 Điều khiển phi tuyến sở mô hình 18 Hình 1.16 Sơ đồ luật điều khiển Li-Slotine 20 Hình 2.1: Minh họa khái niệm ổn định ổn định tiệm cận 26 Hình 2.2: Tƣ tƣởng phƣơng pháp Lyapunov 27 Hình 2.3: Tạo họ đƣờng cong kín chứa gốc tọa độ 27 Hình 2.4: Sơ đồ điều khiển Li-slotine thích nghi 32 Hình 3.1: Robot Pelican 36 Hình 3.2: Khung tọa độ cho robot Planar 38 Hình 3.3: Tọa độ nối robot 40 Hình 4.1: Sơ đồ nguyên lý điều khiển 45 Hình 4.2: Sơ đồ điều khiển chung 46 Hình 4.3 Khối điều khiển thích nghi Li-Slotine 46 Hình 4.4: Khối tính giá trị V, r 47 Hình 4.5: Khối robot Planar 47 Hình 4.6: Khối quan sát thích nghi 48 Hình 4.7: Khảo sát vị trí khớp 49 Hình 4.8: Khảo sát vị trí khớp với tín hiệu đặt hình sin 49 Hình 4.9: Khảo sát vị trí khớp 50 Hình 4.10: Khảo sát vị trí khớp với tín hiệu đặt hình sin 50 Hình 4.11: Khảo sát tốc độ khớp 51 Hình 4.12: Khảo sát tốc độ khớp 51 Hình 4.13: Sai lệch quan sát tốc độ 52 Hình 4.14: Khảo sát vị trí khớp tín hiệu đặt số 52 Hình 4.15: Khảo sát vị trí khớp tín hiệu đặt hàm hình sin 53 Hình 4.16: Sai lệch vị trí khớp tín hiệu đặt số 53 Hình 4.17: Khảo sát khớp tín hiệu đặt số 54 Hình 4.18: Khảo sát khớp tín hiệ u đặt hàm hình sin 54 Hình 4.19: Sai lệch vị trí khớp tín hiệu đặt số 54 MỞ ĐẦU Lý chọn đề tài: Theo trình phát triển xã hội, nhu cầu nâng cao sản xuất chất lƣợng sản phẩm ngày đò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ẽ Vì ngày tăng nhanh nhu cầu ứng dụng ngƣời máy để tạo hệ sản xuất tự động linh hoạt Cùng với phát triển khoa học công nghệ, robot công nghiệp ngày phát triển có ứng dụng hầu hết ngành công nghiệp Sự phát triển đất nƣớc yêu cầu mở rộng, nâng cao chất lƣọng sản xuất thúc đẩy phát triển robot công nghiệp đƣa công nghiệp chế tạo robot trở thành ngành hứa hẹn tƣơng lai Để đáp ứng điều đòi hỏi phải trang bị cho Robot điều khiển thông minh, phức tạp Chính thuật toán điều khiển robot đƣợc đầu tƣ nghiên cứu, ứng dụng phát triển mạnh mẽ Mục tiêu nghiên cứu: - Nắm bắt đƣợc lý thuyết phƣơng pháp điều khiển Robot, lý thuyết điều khiển thích nghi xây dựng thuật toán - Ứng dụng thuật toán thích nghi quan sát trạng thái để áp chế tính bất định mô hình động lực học Robot điều khiển bám quỹ đạo chuyển động - Sử dụng đƣợc phần mềm MATLB SIMULINK làm công cụ xây dựng mô hình mô kết Đối tƣợng, phạm vi nghiên cứu: - Robot bậc tự Planar - Bộ điều khiển thích nghi Li - Slotine quan sát thích nghi - Kết hợp điều khiển thích nghi Li – Slotine quan sát thích nghi để áp chế tính bất định mô hình động lực học Robot điều khiển bám quỹ đạo chuyển động Phƣơng pháp nghiên cứu: - Nghiên cứu tổng quan phƣơng pháp điều khiển Robot, nghiên cứu phƣơng pháp điều khiển Li – Slotine - Xây dựng thuật toán điều khiển thích nghi Li – Slotine quan sát thích nghi - Từ kết tính toán, sử dụng công cụ mô để trình bày kết nghiên cứu đạt đƣợc Ý nghĩa khoa học thực tiễn đề tài Đề tài mang lại hƣớng việc điều khiển robot công nghiệp Phƣơng pháp ứng dụng thuật toán điều khiển thích nghi giúp giải hiệu vấn đề bất định đối tƣợng nhƣ vị trí, tốc độ Qua tạo công cụ điều khiển mạnh trình tự động hóa sản xuất Cấu trúc luận văn: Cấu trúc luận văn gồm chƣơng: Chƣơng 1: Trình bày tổng quan vấn đề Robot; lịch sử phát triển Robot công nghiệp; khái niệm Robot công nghiệp Chƣơng 2: Trình bày điều khiển Li – Slotine thích nghi quan sát thích nghi Chƣơng 3: Trình bày mô hình toán học Robot Planar, toán động học thuận động học ngƣợc vị trí Chƣơng 4: Mô Matlab/Simulink Dƣới hƣớng dẫn tận tình PGS.TS Nguyễn Phạm Thục Anh luận văn em hoàn thiện Do trình công tác điều kiện lại nên luận văn nhiều hạn chế Em mong nhận đƣợc đóng góp thầy cô bạn Em xin chân thành cảm ơn! Hà Nội, ngày 21 tháng năm 2016 Ngƣời thực Khƣơng Đức Hạnh Chƣơng TỔNG QUAN PHƢƠNG PHÁP ĐIỀU KHIỂN ROBOT 1.1 Tổng quan robot 1.1.1 Lịch sửphát triển robot “Robot” đƣợc đời từ mong ƣớc ngƣời muốn có cỗ máy gần giống ngƣời làm công việc thay ngƣời Năm 1921 kịch Rosum’s Universal Robot Karel Capek Rossum trai chế tạo máy gần giống ngƣời để phục vụ ngƣời Có lẽ gợi ý cho nhà sáng chế kĩ thuật cấu máy móc bắt chƣớc hoạt động ngƣời Đầ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 đƣợc đặ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 nhƣ tay ngƣời đƣợc điều khiển tự động để thực số thao tác sản xuất đƣợc gọi robot công nghiệp Về mặt kĩ thuật robot công nghiệp ngày có nguồn gốc từ hai lĩnh vực cấu điều khiển từ xa máy công cụ điều khiển số Các cấu điều khiển từ xa phát triển mạnh chiến tranh giới lần thứ hai nhằm nghiên cứu vật liệu phóng xạ Còn máy công cụ điều khiển số đời vào năm 1949 nhằm đáp ứng yêu cầu gia công chi tiết ngành chế tạo máy bay 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, … Tính làm việc robot ngày đƣợc nâng cao, khả nhận biết xử lý Năm 1967 trƣờng Đại học tổng hợp Standford Mỹ chế tạo mẫu robot hoạt động theo mô hình “mắt – tay”, có khả nhận biết định hƣớng bàn kẹp theo vị trí vật kẹp nhờ cảm biến Năm 1974 Công ty Mỹ Cincinnati đƣa loại robot đƣợc điều khiển máy tính nâng đƣợc vật có khối lƣợng đến 40 kg Có thể nói, Robot tổ hợp khả hoạt động linh hoạt cấu điều khiển từ xa với mức độ “tri thức” ngày phong phú hệ thống điều khiển theo chƣơng trình số nhƣ kĩ thuật chế tạo cảm biến, công nghệ lập trình phát triển trí khôn nhân tạo,… Trong gần đây, việc nâng cao tính hoạt động robot không ngừng đƣợc phát triển Các hoạt động đƣợc trang bị thêm loại cảm biến khác để nhận biết môi trƣờng chung quanh với thành tựu to lớn lĩnh vực Tin học – Điện tử tạo hệ robot với nhiều tính đặc biệt Số lƣợng robot ngày gia tăng, chủng loại robot không ngừng đƣợc gia tăng, giá thành ngày giảm Nhờ vậy, robot công nghiệp có vị trí quan trọng dây truyền sản xuất đại, đặc biệt hệ thống sản xuất tự động hóa Hiện nay, giới có khoảng 200 công ty sản xuất IR Và theo ƣớc tính chƣa đầy đủ Liên đoàn Robot quốc tế (IRF), năm 2005 giới có khoảng 800.000 robot đƣợc sử dụng sản xuất công nghiệp Trong Nhật Bản nƣớc sử dụng nhiều chiếm khoảng 45,5%, EU khoảng 30,3%, Bắc Mĩ khoảng 13,5% lại nƣớc khác 1.1.2 Cấu tạo robot Trên hình 1.1 giới thiệu phận chủ yếu Robot: Tay máy cấu khí gồm khâu khớp chúng hình thành cánh tay để tạo chuyển động bản, cổ tay tạo nên khéo léo linh hoạt, bàn tay hoàn thành thao tác đối tƣợng Các phận: Đế đặt cố định gắn liền với xe di động 2, thân 3, cánh tay 4, cánh tay dƣới 5, bàn kẹp Hình 1.1 Bộ phận cấu thành Robot Hệ thống truyền động khí, thuỷ khí điện khí: phận chủ yếu tạo nên chuyển dịch khớp động Hệ thống điều khiển đảm bảo hoạt động Robot theo thông tin đặt trƣớc nhận biết trình làm việc Hệ thống cảm biến tín hiệu thực việc nhận biết biến đổi thông tin hoạt động thân Robot (cảm biến nội tín hiệu) môi trƣờng, đối tƣợng mà Robot phục vụ (cảm biến ngoại tín hiệu) 1.1.3 Cơ cấu khí robot Cấu trúc khí robot bao gồm chuỗi nối đƣợc gắn với khớp Mỗi khớp đƣợc truyền động tạo nên chuyển động độc lập Số bậc tự robot phụ thuộc vào số chuyển động độc lập độ dịch chuyển linh hoạt chuyển động tăng số bậc tự tăng Hai loại khớp hệ thống robot khớp tịnh tiến khớp quay 4.2 Sơ đồ khối mô Matlab/simulink Quỹ đạo mặt Hình 4.2: Sơ đồ điều khiển chung Hình 4.3 Khối điều khiển thích nghi Li-Slotine 46 Hình 4.4: Khối tính giá trị V, r Hình 4.5: Khối robot Planar 47 Hình 4.6: Khối quan sát thích nghi 4.3.Kết mô cho robot Pelican Robot Pelican có phƣơng trình động lực học cho (3.25) với thông số cho bảng 3.1 Các thông số mô cho bảng sau : Bảng 4.1: Tham số ƣớc lƣợng cho robot Tham số Khối lƣợng ƣớc lƣợng ban đầu nối Khối lƣợng ƣớc lƣợng ban đầu nối Độ dài khớp Độ dài khớp Ký hiệu Giá tri mˆ (0) 6kg mˆ (0) 4kg l1 l2 0.5m 0.5m Bảng 4.2: Bảng tham số điều khiển Tham số KD Giá trị 500 100 20 A 48 4.3.1 Kết mô cho quan sát thích nghi Mục đích việc mô quan sát khảo sát tốc độ vị trí thực tể robot so sánh với tốc độ vị trí ƣớc lƣợng từ quan sát Từ đƣa kết luận quan sát Bảng 4.3: Tham số quan sát Giá trị Tham số L1 1200 0 1500 L2 1200 0 1500 Hình 4.7: Khảo sát vị trí khớp Hình 4.8: Khảo sát vị trí khớp với tín hiệu đặt hàm hình sin 49 Hình 4.9: Khảo sát vị trí khớp Hình 4.10: Khảo sát vị trí khớp với tín hiệu đặt hàm hình sin Nhận xét : Từ đồ thị ta thấy vị trí khớp thực tế quan sát giống nhau, chứng tỏ quan sát hoàn toàn xác, đảm bảo sai lệch e tiến tới 50 Hình 4.11: Khảo sát tốc độ khớp Hình 4.12: Khảo sát tốc độ khớp 51 Hình 4.13: Sai lệch quan sát tốc độ Tốc độ quan sát tính toán có quỹ đạo bám theo tốc độ thực tế, thời gian độ tƣơng đối ngắn, sai lệch quan sát nhỏ Ta hoàn toàn khẳng định tính xác quan sát thích nghi Nhƣ vậy, ta sử dụng tín hiệu tốc độ từ quan sát làm tín hiệu phản hồi cho điều khiển 4.3.2 Kết mô điều khiển thích nghi Li-SIotine Bộ điều khiển thay tín hiệu qˆ q sau dựa vào luật thích nghi tính toán lại tham số ƣớc lƣợng động lực học robot đƣa moment điều khiển cho robot Hình 4.14: Khảo sát vị trí khớp tín hiệu đặt số 52 Hình 4.15: Khảo sát vị trí khớp tín hiệu đặt hàm hình sin Hình 4.16: Sai lệch vị trí khớp tín hiệu đặt số Nhận xét: Qua kết mô ta thấy góc quay khớp hoàn toàn so với tín hiệu đặt, thời gian xác lập nhỏ, sai lệch nhỏ, ổn định 53 Hình 4.17: Khảo sát khớp tín hiệu đặt số Hình 4.18: Khảo sát khớp tín hiệu đặt hàm hình sin Nhận xét : Từ đồ thị ta thấy quỹ đạo chuyển động robot bám theo quỹ đạo đặt Mặc dù rõ đƣợc tham số động học robot nhƣng điều khiển đảm bảo điều khiển xác ổn định Hình 4.19: Sai lệch vị trí khớp tín hiệu đặt số Nhận xét: Qua kết mô ta thấy góc quay khớp hoàn toàn so với tín hiệu đặt, thời gian xác lập chƣa nhỏ, sai lệch nhỏ, ổn định 54 55 KẾT LUẬN Sau thời gian nghiên cứu đề tài “Điều khiển thích nghi robot nDOF sở quan sát” em tổng hợp đƣợc nhiều lƣợng kiến thức học giảng đƣờng nhƣ trình công tác, em thu đƣợc nhƣng kết cụ thể sau: - Xây dựng đƣợc điều khiển thích nghi cho robot Planar - Xây dựng đƣợc quan sát thích nghi, tính toán điều khiển giá trị đặt cho robot - Tiến hành mô kết phần mềm Matlab/Simulink Em xin chân thành cảm ơn bảo tận tâm, nhiệt tình PGS.TS Nguyễn Phạm Thục Anh giúp em hoàn thành luận văn Do trình công tác điều kiện lại nên luận văn nhiều hạn chế Em mong nhận đƣợc đóng góp thầy cô, anh chị bạn Em xin chân thành cảm ơn! Hà Nội, ngày 21 tháng năm 2016 Ngƣời thực Khƣơng Đức Hạnh 56 TÀI LIỆU THAM KHẢO [1] Trần Trọng Minh, Nguyễn Phạm Thục Anh, 2006, Hệ thống sản xuất tự động hóa tích hợp máy tính, NXB Khoa học Kỹ thuật [2] Nguyễn Doãn Phƣớc, Phan Xuân Minh, Hán Thành Trung, 2006, Lý thuyết điều khiển phi tuyến, NXB Khoa học Kỹ thuật [3] Nguyễn Doãn Phƣớc, 2009, Lý thuyết điều khiển nâng cao, NXB Khoa học Kỹ thuật [4] Nguyễn Mạnh Tiến, 2007, Điều khiển robot công nghiệp, NXB Khoa học Kỹ thuật [5] Nguyễn Phạm Thục Anh, Bài giảng IR-liada [6] Jean-Jacques E Slotine, W.Li, 1991, Applied Nonlinear Control, Prentice Hall International, Inc Englewood Cliffs, New Jersey [7] Rolf Johanson, Anders Robertsson, 2002, Department of Automatic Control Lund Ínsitute of Technology [8] H.M.Schwartz and J.M.Daly, 2008, International Journal of Robotics and Automation, Vol.23, No.3 [9]M.Erlic and W.S.Lu, 1995, IEEE Transaction On Robotics and Automation, Vol.11, No.2 57 PHỤ LỤC P1 Chƣơng trình mô động lực học robot Hàm tính G function y = g(in) %q(1) goc theta1 %q(2) goc theta2 %dq(1) toc quay khop %dq(2) toc quay khop %cac thong so uoc luong robot m1 = 5; m2 = 3; l1 = 0.5; l2 = 0.5;rgl1 = 1/2*l1; rg2 = 1/2*l2; J1 = 1/l2*m1*l1^2; J2 = 1/l2*m2*l2^2; q(1) = in(1); q(2) = in(2); %mo men luc y(1,1)= 9.8*m1*rg1*cos(q(1))+ 9.8*m2*(l1*cos(q(1))+rg2*cos(q(1)+q(2)); y(2,1)= 9.8*m2*rg2*cos(q(1)+q(2)); end Hàm tính H function y = h(in) %q(1) goc theta1 %q(2) goc theta2 %dq(1) toc quay khop %dq(2) toc quay khop %cac thong so uoc luong robot m1 = 5; m2 = 3; l1 = 0.5; l2 = 0.5;rgl1 = 1/2*l1; rg2 = 1/2*l2; J1 = 1/l2*m1*l1^2; J2 = 1/l2*m2*l2^2; q(1) = in(1); q(2) = in(2); M01 = in(3); M02 = in(4); % ma tran quan tinh h(1,1) = m1*rg1^2 + J1 + m2*(l1^2 + rg2^2 + 2*l1*rg2*cos(q(2)) + J2; h(1,2) = m2*(rg2^2 + l1*rg2*cos(q(2)) + J2; h(2,1) = m2*(rg2^2 + l1*rg2*cos(q(2)) + J2; h(2,2) = m2*rg2^2 + J2; Hinv=inv(h); %gia toc khop y=Hinv*[M01;M01]; end Hàm tính V function y = v(in) %q(1) goc theta1 %q(2) goc theta2 %dq(1) toc quay khop %dq(2) toc quay khop %cac thong so uoc luong robot 58 m1 = 5; m2 = 3; l1 = 0.5; l2 = 0.5;rgl1 = 1/2*l1; rg2 = 1/2*l2; J1 = 1/l2*m1*l1^2; J2 = 1/l2*m2*l2^2; q(1) = in(1); q(2) = in(2); dq(1) = in(3); dq(2) = in(4); % chi tiet h3 = -m2*l1*rg2*sin(q(2)); y(1,1) = h3*dq(2)^2 + 2*h3*dq(1)*dq(2); y(2,1) = -h3*dq(1)^2; end P2 Chƣơng trình mô điều khiển quan sát Hàm tính Hˆ function y=H1(in) q1=in(1);q2=in(2); p=[in(3);in(4);in(5);in(6);in(7);in(8);in(9);in(10);in(11)]; M01=in(12); M02=in(13); H(1,1)=p(1)+p(2)+p(3)+2*p(4)*cos(q2)+p(5)+p(6); H(1,2)=p(3)+p(4)*cos(q2)+p(6); H(2,1)=p(3)+p(4)*cos(q2)+p(6); H(2,2)=p(3)+p(6); y=H\[M01;M02]; end Hàm tính Vˆ function y=V1(in) q1 = in(1);q2=in(2);dq1=in(12);dq2=in(13); p=[in(3);in(4);in(5);in(6);in(7);in(8);in(9);in(10);in(11)]; C(1,1)=-p(4)*sin(q2)*dp2; C(1,2)=-p(4)*sin(q2)*(dp1+dp2); C(2,1)=p(4)*sin(q2)*dp1;C(2,2)=0; y=C*[dq1;dq2]; end Hàm tính Gˆ function y=G1(in) q1=in(1);q2=in(2); p=[in(3);in(4);in(5);in(6);in(7);in(8);in(9);in(10);in(11)]; G(1,1)=[p(7)+p(8)]*cos(q1)+p(9)*cos(q1+q2); G(2,1)=p(9)*cos(q1+q2); y=G; end Hàm tính M function y=M(in) r(1,1)=in(1);r(2,1)=in(2); v1=in(3);v2=in(4);dv1=in(5);dv2=in(6);q1=in(7);q2=in(8);dq1=in( 9);dq2=in(10); p=[in(11);in(12);in(13);in(14);in(15);in(16);in(17);in(18);in(1 9)]; 59 H(1,1)=p(1)+p(2)+p(3)+2*p(4)*cos(q2)+p(5)+p(6); H(1,2)=p(3)+p(4)*cos(q2)+p(6); H(2,1)=p(30+p(4)*cos(q2)+p(6); H(2,2)=p(3)+p(6); C(1,1)=-p(4)*sin(q2)*dq2; C(1,2)=-p(4)*sin(q2)*(dq1+dq2); C(2,1)=p(4)*sin(q2)*dq1;C(2,2)=0; G(1,1)=[p(7)+p(8)]*cos(q1)+p(9)*cos(q1+q2); G(2,1)=p(9)*cos(q1+q2); y=H*[dv1;dv2]+C*[v1;v2]+G; end Hàm tính pˆ function p=p(in) q(1) goc theta1 q(2) goc theta2 dq(1) toc doquaykhop1 dq(2) toc doquaykhop2 cac thongsorobot K=0.01; r(1,1)=in(1);r(2,1)=in(2); v1=in(3);v2=in(4);dv1=in(5);dv2=in(6);q1=in(7);q2=in(8);dq1=in( 9);dq2=in(10); Y(1,1)=dv1; Y(1,2)=dv1; Y(1,3)=dv1+dv2;Y(2,3)=Y(1,3); Y(1,4)=2*cos(q2)*dv1+cos(q2)*dv2-sin(q2)*dq2*v1sin(q2)*(dq1+dq2)*v2; Y(2,4)=cos(q2)*dv1+sin(q2)*dq1*v1; Y(1,5)=dv1; Y(1,6)=dv1+dv2;Y(2,6)=Y(1,6); Y(1,7)=cos(q1); Y(1,8)=cos(q1); Y(1,9)=cos(q1+q2);Y(2,9)=Y(1,9); p=K*Y'*r; end 60 ... xin đƣợc trình bày thuật toán điều khiển thích nghi Li- Slotine quan sát thích nghi 21 Chƣơng BỘ ĐIỀU KHIỂN LI - SLOTINE THÍCH NGHI VÀ BỘ QUAN SÁT THÍCH NGHI 2.1 Cơ sở lý thuyết hệ phi tuyến 2.1.1... Phƣơng pháp điều khiển thích nghi Li - Slotine 30 2.3 Bộ quan sát trạng thái thích nghi 32 2.3.1 Bộ quan sát trạng thái Luenberger 32 2.3.2 Bộ quan sát thích nghi cho robot ... pháp nghi n cứu: - Nghi n cứu tổng quan phƣơng pháp điều khiển Robot, nghi n cứu phƣơng pháp điều khiển Li – Slotine - Xây dựng thuật toán điều khiển thích nghi Li – Slotine quan sát thích nghi