Tính toán động lực học và thiết kế điều khiển hệ tele robot SMSS trên kênh truyền thông có trễ

LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam LỜI CAM ĐOAN Học viên : Lâm Thế Kiên , Sau năm học tập nghiên cứu trƣờng Đại Học Bách khoa Hà Nội đến hoàn thành chƣơng trình đào tạo nhƣ luận văn tốt nghiệp dƣới giúp đỡ tận tình giáo viên hƣớng dẫn TS Đỗ Đức Nam giúp đỡ thầy giáo môn Cơ sở thiết kế máy Robot Với đề tài “Tính toán động lực học thiết kế điều khiển cho hệ thống TeleRobot kênh truyền thông có trễ”, xin cam kết luận văn tốt nghiệp công trình nghiên cứu thân dƣới hƣớng dẫn TS Đỗ Đức Nam Các kết nêu Luận văn tốt nghiệp trung thực, chép toàn văn công trình khác Hà Nội, ngày tháng năm 2013 Tác giả luận văn tốt nghiệp Lâm Thế Kiên HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam DANH MỤC CÁC HÌNH VẼ ĐỒ THỊ Hình 1.1 Hệ Teleoperation song phƣơng ……………………………………… 11 Hình 1.2.1a Các vệ tinh không gian trái đất ……………………… 12 Hình 1.2.1b Nasa Pathfinder ………………………………………………… 13 Hình 1.2.2 Các robot làm việc môi trƣờng nguy hiểm ………………… 14 Hình 1.2.3 Các robot làm việc dƣới môi trƣờng nƣớc ………………………… 15 Hình 1.2.3 Ứng dụng y học phẫu thuật ………………………………… 17 Hình 3.1 Khảo sát tốc độ vi khối lƣợng dm ……………………………… 32 Hình 3.2 Mômen quán trính độc cực ………………………………………… 34 Hình 3.3 Hệ thống điều khiển từ xa robot master robot slave (SMSS) 38 Hình 3.5 Robot bậc tự dạng tay nối tiếp ………………………………… 40 Hình 3.7 Mô hình môi trƣờng ………………………………………………… 44 Hình 3.8 Mô hình thời gian trễ ……………………………………………… 46 Hình 4.1 Động lực học robot master/slave với phản hồi thụ động …………… 50 Hình 4.2 Sơ đồ khối điều khiển kênh hệ SMSS …………………………… 51 Hình 4.4.3.1: Lực tƣơng tác ……………………………………………… 57 Hình 4.4.4.1.a Vị trí không gian tự độ trễ ………………… 59 Hình 4.4.4.1.b Lực không gian tự độ trễ ………………… 59 Hình 4.4.4.2.a Vị trí không gian va chạm độ trễ …………… 60 Hình 4.4.4.2.b lực không gian va chạm độ trễ ……………… 60 Hình 4.4.4.3.a Vị trí không gian tự có độ trễ ……………………… 61 Hình 4.4.4.3.b Lực không gian tự có độ trễ ………………………… 61 Hình 4.4.4.4.a Vị trí không gian va chạm có độ trễ …………………… 62 Hình 4.4.4.4.b Lực không gian va chạm có độ trễ ……………………… 62 Hình 4.4.4.5.a Vị trí không gian tự có độ trễ ……………………… 63 Hình 4.4.4.5.b Lực không gian tự có độ trễ ……………………… 63 HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam Hình 4.4.4.6.a Vị trí không gian va chạm có độ trễ …………………… 64 Hình 4.4.4.6.b Lực không gian va chạm có độ trễ ……………………… 64 DANH MỤC CÁC BẢNG Bảng 4.1 Các tham số hệ thống SMSS………………………………… 56 Bảng 4.2 Các thông số mô …………………………………………… 58 HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam MỤC LỤC CHƢƠNG GIỚI THIỆU 1.1 Hệ thống Teleoperation 1.2 Ứng dụng hệ Teleoperation 11 1.2.1 Những ứng dụng không gian 12 1.2.2 Làm việc môi trƣờng nguy hiểm 14 1.2.3 Phƣơng tiện dƣới nƣớc 15 1.2.4 Phẫu thuật từ xa 16 1.2.5 Robot di động 18 1.2.6 Những ứng dụng khác hệ thống Teleoperation 18 1.3 Mục tiêu đóng góp luận văn 19 CẤU TRÚC CỦA LUẬN VĂN 21 CHƢƠNG 22 CƠ SỞ TOÁN HỌC 22 2.1 Định nghĩa lý thuyết ổn định Lyapunov 22 2.2 Tính ổn định Lyaponov 25 CHƢƠNG 31 ĐỘNG LỰC HỌC ROBOT 31 3.1 Phƣơng trình động lực học robot 31 3.1.1 Vân tốc điểm robot 31 3.1.2 Tính động vi khối lƣợng dm 33 3.1.3 Tính robot 35 3.1.4 Hàm Lagrange 35 3.1.5 Phƣơng trình động lực học robot 36 3.2 Động lực học robot hệ SMSS 37 3.2.2 Phƣơng trình động lực học Robot Master Slaver 39 HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam 3.4 Động lực học môi trƣờng 43 3.5 Độ trễ kênh truyền thông 46 THIẾT KẾ ĐIỀU KHIỂN CHO HỆ SMSS 48 4.1 Mục tiêu điều khiển 48 4.2 Thiết kế điều khiển 49 4.2.1 Phản hồi thụ động 49 4.2.2 Luật điều khiển đồng với cấu hình giống robot master slave 51 4.3 Phân tích tính ổn định 52 4.4 Mô hệ thống điều khiển SMSS 56 4.4.1 Tham số robot hệ thống SMSS 56 4.4.2 Cấu hình ngƣời tác động môi trƣờng 56 4.4.3 Các thông số mô 58 4.4.4 Kết mô 59 4.5 Kết luận 65 CHƢƠNG 66 KẾT LUẬN 66 5.1 Kết luận 66 5.2 Hƣớng phát triển 66 PHỤ LỤC A 67 CÁC KHỐI CỦA CHƢƠNG TRÌNH MÔ PHỎNG 67 A.1 Hệ thống SMSS : 67 PHỤ LỤC B 70 CHƢƠNG TRÌNH MATLAB CHO HỆ THỐNG ĐIỀU KHIỂN 70 File Parameter 73 File in đồ thị 74 HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam TÓM TẮT NỘI DUNG LUẬN VĂN TỐT NGHIỆP Trong điều khiển từ xa với hệ thống Teleoperation, việc thực thi xác tác vụ vị trí lực phản hồi cần thiết Thực tế, thời điểm robot slave bắt đầu tƣơng tác với môi trƣờng xuất lực phản hồi chúng tăng dần Vì vậy, tín hiệu phản hồi thông tin lực cần thiết có lợi ích to lớn Thuật toán phản hồi lực (Force Reflection - FR) không thực thi tốt tác vụ trình di chuyển tự mà để truyền tải xác thông tin lực robot slave môi trƣờng Luận văn xem xét điều khiển vị trí điều khiển phản hồi lực hệ thống Teleoperation với hệ thống master hệ thống slave Để cải thiện đồng vị trí, lực hệ Teleoperation SMSS với độ trễ biến thiên kênh truyền thông luật điều khiển hệ thống SMSS (một master slave) đƣợc đề xuất dựa phƣơng pháp phản hồi thụ động với hệ số thời gian trễ biến thiên phụ thuộc vào việc đánh giá thay đổi biến thời gian trễ Trong phƣơng pháp đƣợc đề xuất, điều kiện ổn định phụ thuộc vào độ lớn độ trễ kênh truyền thông hệ thống Tính ổn định toàn cục với độ trễ đƣợc thông qua phƣơng pháp ổn định Lyapunov Và cuối cùng, số kết mô cho thấy hiệu thuật toán đƣa HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam LỜI NÓI ĐẦU Có thể nhận thấy điều sản xuất mang tính thủ công sau so với phát triển giới Những thiết bị đƣợc sử dụng thƣờng cho suất thấp, độ xác dễ gây tai nạn cho ngƣời lao động Để cải thiện tình hình việc cải tiến trang thiết bị, vấn đề cần đƣợc xem xét thay đổi phƣơng thức sản xuất từ thủ công sang tự động hóa Một vấn đề then chốt tự động hóa sản xuất phƣơng pháp điều khiển Với vấn đề trên, học viên chuyên ngành điện tử trƣờng đại học Bách Khoa Hà Nội, với mong muốn bổ sung kiến thức nâng cao kiến thức điều khiển cho trình làm việc sau lựa chọn cho luận văn tốt nghiệp với đề tài “Tính toán động lực học thiết kế điều khiển cho hệ thống Teleoperation SMSS kênh truyền thông có trễ” Đây đề tài đƣợc quan tâm lớn từ phía nhà khoa học khắp giới, tiêu biểu nhƣ Nhật Châu Âu… Trong trình thực đề tài này, đƣa đƣợc số kết khả quan Để đƣợc kết đó, xin chân thành cảm ơn giúp đỡ tận tình Thầy giáo hƣớng dẫn TS Đỗ Đức Nam thầy cô Viện Cơ khí đặc biệt thầy cô giáo môn Cơ sở thiết kế máy Robot tạo điều kiện cho hoàn thành đề tài cách tốt Tuy nhiên, gặp nhiều khó khăn vấn đề thời gian kinh nghiệm nên phát triển đề tài chƣa có nhiều đột phá thiếu xót Kính mong quý thầy cô đóng góp ý kiến để đề tài đƣợc hoàn thành tốt Trƣờng Đại học Bách Khoa Hà Nội Viện Cơ khí, Bộ môn Cơ sở thiết kế máy robot Lâm Thế Kiên HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam CHƢƠNG GIỚI THIỆU 1.1 Hệ thống Teleoperation Teleoperation hệ thống thiết bị có tƣơng tác khoảng cách khác tƣơng tự nhƣ hệ thống “điều khiển từ xa” thƣờng gặp học thuật môi trƣờng kỹ thuật Các thiết bị hệ thống Teleoperation thƣờng liên quan đến lĩnh vực robot (cố định di động) có nhiều ứng dụng khoa học kỹ thuật sống hàng ngày Các thiết bị thƣờng đƣợc điều khiển từ xa ngƣời thông qua thiết bị thuộc hệ thống Hệ thống Teleoperation hệ thống cho phép ngƣời sử dụng hiểu biết, khả tƣ hoạt động chân tay họ thông qua cộng tác điều khiển thiết bị nhƣ robot Khi điều hành làm việc môi trƣờng nguy hiểm độc hại Trong trƣờng hợp này, ngƣời sử dụng hoạt động để điều khiển robot master thao tác robot slave đƣợc thực theo điều khiển robot master robot robot trực tiếp có tƣơng tác với môi trƣờng làm việc Trong vài thập niên gần đây, hệ thống Teleoperation đƣợc phát triển với nhiều ứng dụng khác nhƣ đƣợc sử dụng vũ trụ, dƣới đáy biển, thiết bị hạt nhân, hoạt động phẫu thuật, điều khiển lái xe từ xa, cứu hộ… loại ứng dụng hệ thống Teleoperation nghiên cứu hệ thống đƣợc nhà khoa học theo đuổi Trong Teleoperation song phƣơng, robot master robot slave đƣợc sử dụng nhƣ cặp thiết bị nằm hai phía đƣợc liên kết với qua kênh truyền thông, nơi mà thông tin vị trí, vận tốc, gia tốc lực đƣợc truyền Trong trình truyền liệu robot master robot slave có tƣợng trễ HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam kênh truyền thông Trễ hệ thống vòng kín làm tính ổn định làm sai việc thực hoạt động thao tác làm giảm tính đồng hệ thống Teleoperation Trong việc hiệu chỉnh xác thông số chất luật điều khiển thông qua tác vụ, nhƣng chƣa đủ tốt để đạt đƣợc hiệu mong muốn vị trí mối quan hệ đƣợc đƣa robot Trong thực tế, thời điểm robot bắt đầu tƣơng tác với môi trƣờng, phản lực xuất tăng dần lên Nếu không ý, lực không điều khiển đƣợc trở thành nguy hiểm vài tác vụ Chính thông tin phản hồi lực quan trọng hữu ích đƣợc gọi phản hồi lực ( FR - Force Reflection ) hệ thống robot master - robot slave Sơ đồ FR thông số để đạt đƣợc hiệu chỉnh tốt chuyển động không buộc mà biểu diễn thông tin xác lực robot slave môi trƣờng Do ngƣời điều khiển cảm thấy lực nhƣ thực tế môi trƣờng với tƣơng tác robot master Cho đến nay, số nghiên cứu liên quan đến vấn đề chuyển động điều khiển lực, đặc biệt cho vị trí khâu tác động cuối robot slave va chạm với môi trƣờng Mặt khác, môi trƣờng đƣợc giả thiết nhƣ hệ động lực đơn giản đƣợc trải nhỏ nhƣng có lực tác động phản ứng môi trƣờng biến dạng hữu hạn Khi va chạm xảy ra, lực phát sinh đƣợc xác định phƣơng trình cân động lực học hệ thống robot slave môi trƣờng Để có đƣợc qui định mô tả động lực học cho robot khâu cuối tác động với môi trƣờng lực va chạm nhỏ điều khiển trở kháng đƣợc sử dụng Điều khiển để trì mối quan hệ động lực học đƣợc dẫn suất khâu cuối môi trƣờng, hệ “khối lƣợng – lò xo – giảm chấn” ( massspring-dampe ) bậc hai đƣợc sử dụng để xác định mục tiêu động lực học Nó đƣa HVTH: Lâm Thế Kiên Trang LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam gần hợp cho điều khiển robot môi trƣờng tự điều khiển chuyển động có buộc Một nghiên cứu điều khiển trở kháng đƣợc công bố Hogan (1985) [1] Trong nghiên cứu này, cần bổ sung kiến thức hoàn chỉnh mô hình động lực học robot, thông qua phƣơng pháp tính toán momen xoắn trở kháng mong muốn, dù hệ thống Teleoperation không đƣợc đề cập đến Cho Park (2005) [2] sử dụng dựa điều khiển trở kháng tính toán gần momen xoắn áp dụng phƣơng pháp cho hệ thống Teleoperation Đối tƣợng điều khiển luật điều khiển tạo đƣợc bắt chƣớc dụng cụ khí thụ động với khả phản hồi lực Tuy nhiên sai số vị trí phƣơng pháp lớn thời gian trễ không đƣợc đề cập Năm 2004 Aliagam đồng nghiệp [3] đề xuất phƣơng pháp điều khiển dựa điều khiển trở kháng có so sánh với số điều khiển cho hệ thống hai bậc tự robot master - robot slave Sự tƣơng ứng vị trí chuyển động không ràng buộc tƣơng ứng lực kết nối với tác vụ có liên quan với Một điều khiển bám khác robot dựa thích ứng đƣợc đƣa Sato, Nakashima Tsuruta (2008) [4] với đầu vào không chắn, nhiên nghiên cứu không đƣợc áp dụng cho hệ thống Teleoperation Một phƣơng pháp phản hồi lực Teleoperation với thích ứng điều khiển trở kháng đƣợc sử dụng Love Book (2004) [5] để giảm lƣợng thao tác mà không tính ổn định Mặc dù thời gian trễ đƣờng truyền thông không đƣợc đề cập Ngoài ra, để cải thiển tính đồng Teleoperation song phƣơng với trễ truyền thông, phƣơng pháp điều khiển có phản hồi lực đƣợc giải HVTH: Lâm Thế Kiên Trang 10 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam Trƣờng hợp 4.4.4.4 Robot slave chuyển động va chạm với độ trễ theo 4.4.3.2 Vị trí va chạm Hình 4.4.4.4.a Vị trí không gian va chạm có độ trễ Hình 4.4.4.4.b Lực không gian va chạm có độ trễ Nhận xét 4.4.4.4: Đồ thị hình 4.4.4.4.a cho thấy quỹ đạo robot master slave trƣờng hợp robot slave tƣơng tác với môi trƣờng, robot slave trạng thái chuyển động tự quỹ đạo robot slave bám tƣơng đối sát với quỹ đạo robot master Khi robot slave bắt đầu tƣơng tác với môi trƣờng, môi trƣờng có độ cứng nên robot slave bị chặn lại robot master tiếp tục chuyển động tay ngƣời điều khiển tác động, đồ thị hình 4.4.4.4.b cho thấy vào thời điểm robot slave bắt đầu tƣơng tác với môi trƣờng, lực môi trƣờng phản hồi lại tăng lên đạt gần với lực ngƣời tác động lên robot master HVTH: Lâm Thế Kiên Trang 62 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam Trƣờng hợp 4.4.4.5 Robot slave chuyển động tự với độ trễ theo 4.4.3.3 Hình 4.4.4.5.a Vị trí không gian tự có độ trễ Hình 4.4.4.5.b Lực không gian tự có độ trễ Nhận xét 4.4.4.5 Đồ thị hình 4.4.4.5.a cho thấy quỹ đạo robot master slave trƣờng hợp robot slave không tƣơng tác với môi trƣờng, quỹ đạo robot slave sát với quỹ đạo robot master, đƣa vào độ trễ lớn kênh truyền thông ta thấy quỹ đạo robot slave có khoảng cách trễ rõ rệt với quỹ đạo robot master Hình 4.4.4.5.b cho thấy robot slave không tƣơng tác với môi trƣờng nên lực môi trƣờng Fe = 0, đồ thị cho thấy lực Fop cho thấy lực mà ngƣời cần tác động lên robot master để chuyển động theo quỹ đạo HVTH: Lâm Thế Kiên Trang 63 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam Trƣờng hợp 4.4.4.6 Robot slave chuyển động va chạm với độ trễ theo 4.4.3.3 Vị trí va chạm Hình 4.4.4.6.a Vị trí không gian va chạm có độ trễ Hình 4.4.4.6.b Lực không gian va chạm có độ trễ Nhận xét 4.6: Đồ thị hình 4.4.4.6.a cho thấy quỹ đạo robot master slave trƣờng hợp robot slave tƣơng tác với môi trƣờng, robot slave trạng thái chuyển động tự quỹ đạo robot slave bám tƣơng đối sát với quỹ đạo robot master Khi robot slave bắt đầu tƣơng tác với môi trƣờng, môi trƣờng có độ cứng nên robot slave bị chặn lại robot master tiếp tục chuyển động tay ngƣời điều khiển tác động, đồ thị hình 4.4.4.6.b cho thấy vào thời điểm robot slave bắt đầu tƣơng tác với môi trƣờng, lực môi trƣờng phản hồi lại tăng lên đạt gần với lực ngƣời tác động lên robot master HVTH: Lâm Thế Kiên Trang 64 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam 4.5 Kết luận Phƣơng pháp đƣợc đề xuất điều khiển thụ động đƣa vào hệ số thời gian biến thiên Trong chiến lƣợc điều khiển đƣợc đề xuất này, điều kiện ổn định toàn cục với độ lớn thời gian trễ Vì thiết kế tham số điều khiển gần Sử dụng lý thuyết Lypunov đƣợc tính ổn định toàn cục với thời gian trễ Kết việc mô hiệu thuật toán đƣợc đề xuất HVTH: Lâm Thế Kiên Trang 65 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam CHƢƠNG KẾT LUẬN Luận văn tập trung nâng cao độ xác tác vụ đồng hệ thống Teleoperation thuật toán lực phản hồi Bên cạnh vị trí, lực từ hai phiá đƣợc sử dụng truyền tới qua kênh truyền thông Một số kết luận nhƣ thiếu sót hƣớng phát triển tƣơng lai đƣợc đƣa dƣới 5.1 Kết luận Chƣơng đề xuất phƣơng pháp điều khiển đồng cho hệ thống Teleoperation với cấu hình hai Robot giống thời gian trễ kênh truyền thông biến thiên Sử dụng phản hồi thụ động từ không gian làm việc, đầu vào chứa thông tin vị trí vận tốc khâu chấp hành cuối đƣợc sử dụng để đảm bảo tính thụ động Từ robot master slave đƣợc kết hợp với cách sử dụng điều khiển động tỷ lệ, độ lớn thời gian trễ kênh truyền thông biến thiên Thêm vào đó, hệ số phƣơng pháp điều khiển đồng đƣợc lựa chọn độc lập ứng với giá trị cho robot master slave Do đó, chuyển động khâu chấp hành cuối lực liên quan robot master slave đƣợc rõ chuyển động tự không gian làm việc Sử dụng tính thụ động hệ thống phƣơng pháp ổn định Lyapunov, chứng minh đƣợc hệ thống hai robot đồng dạng với tỷ lệ độ lớn ổn định tiệm cận toàn cục với thời gian trễ 5.2 Hƣớng phát triển  Cải tiến luật điều khiển để giảm thiểu nhiễu lực tƣơng tác robot  Sử dụng Internet không dây đƣờng truyền thông, cách không dễ internet phụ thuộc vào băng thông có nhiều nhiễu làm ảnh hƣơng đến tính đƣờng truyền thông HVTH: Lâm Thế Kiên Trang 66 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam PHỤ LỤC A CÁC KHỐI CỦA CHƢƠNG TRÌNH MÔ PHỎNG A.1 Hệ thống SMSS : Hình A.1.1 Khối điều khiển hệ thống SMSS Hình A.1.2 Khối điều khiển Master HVTH: Lâm Thế Kiên Trang 67 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam Hình A.1.3 Khối aster Hình A.1.4 Khối điều khiển Slave HVTH: Lâm Thế Kiên Trang 68 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam Hình A.1.5 Khối Slave Hình A.1.6 Khối Human HVTH: Lâm Thế Kiên Hình A.1.7 Khối Delay Trang 69 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam PHỤ LỤC B CHƢƠNG TRÌNH MATLAB CHO HỆ THỐNG ĐIỀU KHIỂN Phụ lục đƣa vài chƣơng trình matlab đƣợc sử dụng trình điều khiển File DDrobot.m : function [sys,x0,str,ts] = DDrobot(t,x,u,flag,M1,M2,R,theta1,theta2) %2link DD-Robot ‚ÌS-Function % The following outlines the general structure of an S-function % switch flag, %%%%%%%%%%%%%%%%%% % Initialization % %%%%%%%%%%%%%%%%%% case 0, [sys,x0,str,ts]=mdlInitializeSizes(theta1,theta2); %%%%%%%%%%%%%%% % Derivatives % %%%%%%%%%%%%%%% case 1, sys=mdlDerivatives(t,x,u,M1,M2,R); %%%%%%%%%% % Update % %%%%%%%%%% case 2, sys=mdlUpdate(t,x,u); %%%%%%%%%%% % Outputs % %%%%%%%%%%% case 3, sys=mdlOutputs(t,x,u); %%%%%%%%%%%%%%%%%%%%%%% % GetTimeOfNextVarHit % %%%%%%%%%%%%%%%%%%%%%%% case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); %%%%%%%%%%%%% % Terminate % %%%%%%%%%%%%% case 9, sys=mdlTerminate(t,x,u); %%%%%%%%%%%%%%%%%%%% HVTH: Lâm Thế Kiên Trang 70 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam % Unexpected flags % %%%%%%%%%%%%%%%%%%%% otherwise error(['Unhandled flag = ',num2str(flag)]); end % end sfuntmpl % %=========================================================================== == % mdlInitializeSizes % Return the sizes, initial conditions, and sample times for the S-function %=========================================================================== == % function [sys,x0,str,ts]=mdlInitializeSizes(theta1,theta2) % % call simsizes for a sizes structure, fill it in and convert it to a % sizes array % % Note that in this example, the values are hard coded This is not a % recommended practice as the characteristics of the block are typically % defined by the S-function parameters % sizes = simsizes; sizes.NumContStates sizes.NumDiscStates sizes.NumOutputs sizes.NumInputs sizes.DirFeedthrough sizes.NumSampleTimes = = = = = = 4;%x1,x2,x3,x4 0; 4;%y1,y2,y3,y4: -> q1,q2,dq1,dq2; 2;% - is Tau1,Tau 0; 1; % at least one sample time is needed sys = simsizes(sizes); % % initialize the initial conditions % x0 = [theta1,theta2,0,0]; % % str is always an empty matrix % str = []; % % initialize the array of sample times % ts = [0 0]; % end mdlInitializeSizes HVTH: Lâm Thế Kiên Trang 71 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam % %=========================================================================== == % mdlDerivatives % Return the derivatives for the continuous states %=========================================================================== == % function sys=mdlDerivatives(t,x,u,M1,M2,R) M=[M1+2*R*cos(x(2)) M2+R*cos(x(2));M2+R*cos(x(2)) M2];% - day la cac ma tran C=[-R*x(3)*x(4)*sin(x(2))R*x(4)*(x(4)+x(3))*sin(x(2));R*x(3)*x(3)*sin(x(2))];%- day la cac ma tran %tmp=inv(M)*(u-C-B-D);-la ma tran 2x1 -u is tau[2,1] tmp=inv(M)*(u-C); %ddq= M(-1)* (Tau - h(q,dq)) -Tinh gia toc ddb sys(1)=x(3); % dq1 sys(2)=x(4); % dq2 sys(3)=tmp(1); % ddq1 sys(4)=tmp(2); % ddq2 % end mdlDerivatives % %=========================================================================== == % mdlUpdate % Handle discrete state updates, sample time hits, and major time step % requirements %=========================================================================== == % function sys=mdlUpdate(t,x,u) sys = []; % end mdlUpdate % %=========================================================================== == % mdlOutputs % Return the block outputs %=========================================================================== == % function sys=mdlOutputs(t,x,u) sys(1) sys(2) sys(3) sys(4) = = = = x(1);% q1 x(2);% q2 x(3);%dq1 x(4);%dq2 HVTH: Lâm Thế Kiên Trang 72 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam % end mdlOutputs % %=========================================================================== == % mdlGetTimeOfNextVarHit % Return the time of the next hit for this block Note that the result is % absolute time Note that this function is only used when you specify a % variable discrete-time sample time [-2 0] in the sample time array in % mdlInitializeSizes %=========================================================================== == % function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime =1; sys = []; %=1 % Example, set the next hit to be one second later % end mdlGetTimeOfNextVarHit % %=========================================================================== == % mdlTerminate % Perform any end of simulation tasks %=========================================================================== == % function sys=mdlTerminate(t,x,u) sys = []; % end mdlTerminate File Parameter clear all Mm1=0.366; Mm2=0.0291; Rm=0.0227; qs1=-45*(pi/180); qs2=90*(pi/180); qm1=-45*(pi/180); qm2=90*(pi/180); zm1=0.2828; zm2=0; zs1=0.2828; zs2=0; x=10; y=3000; z=0; x1=40; y1=100; z1=0; HVTH: Lâm Thế Kiên Trang 73 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam vc=0.12; K=[1 0;0 1]; Km=20; Ks=1; landa=[1 0; 1]; Tm=0; Ts=0; del_Tm=0.01; del_Ts=0.01; freq_m=0; freq_s=0; t=0.015; File in đồ thị figure(1); plot(ScopeData1x.time,ScopeData1x.signals.values); grid on; h = legend('Xm','Xs',2); figure(2); plot(ScopeData1y.time,ScopeData1y.signals.values); grid on; h = legend('Ym','Ys',2); figure(5); plot(ScopeData3x.time,ScopeData3x.signals.values); grid on; h = legend('Fop','Fenv',2); figure(6); plot(ScopeData3y.time,ScopeData3y.signals.values); grid on; h = legend('Fop','Fenv',2); HVTH: Lâm Thế Kiên Trang 74 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam TÀI LIỆU THAM KHẢO [1] N Hogan, “Impedance control: An approach to manipulation”, JSME Trans Journal of Dynamics System, Measurement, and Control, Vol 107, pp.1-24, 1985 [2] H C Cho and J H Park, “Impedance control with variable damping for bilateral Teleoperation under time delay”, JSME Inter Journal, serial C, Vol 48, No.4, pp 695703, 2005 [3] I Aliagam, A Rubio and E Sanchez, “Experiment quantitative comparison of different control architectures for master-slave teleoperartion”, IEEE Transactions on Control systems Technology, Vol 12, No.1, pp.2-11, 2004 [4] K Sato, T Nakashima and K Tsuruta, “A robust adaptive H∞ control for robotic manipulators with input torque uncertainties”, in Int J of Advanced Mechatronic Systems, Vol 1, No.2 pp.116 - 124, 2008 [5] L J Love and W J Book, “Force reflecting Teleoperation with adaptive impedance control”, System, Man, and Cybernetics, Part B, IEEE Trans., Vol 34, Issue 1, pp 159165, 2004 [6 I G Polushin, P X Liu and C H Lung, “A force-reflection algorithm for improved transparency in bilateral Teleoperation with communication delay”, IEEE/ASME Transactions on Mechatronics, Vol 12, No 3, pp.361-374, 2007 [7] I G Polushin, A Teyebi, and H J Marquez, “Stabilization scheme for force reflecting Teleoperation with time-varying communication delay base on IOS small gain theorem”, Presented at the 16th IFAC World Congr., Pargue, Czech Republic, July 4-8, 2005 [8] S B Skaar and C F Ruoff (Eds), “Teleoperation and robotics in space”, Progress in astronautics and aeronautics, Vol 161, American Institute of Aeronautics and Astronautics, 1987 [9] G Clement, J Vertut, R Fournier, B Espiau, and G Andre, “ An overview of CAT control in nuclear services” In Proceedings of the IEEE international conference on robotics and automation, vol 2, pp 713-718, 1995 [10] W Wang, and K Yuan, “ Teleoperated manipulator for leak detection of sealed radioactive sources” In Proceedings of the IEEE international conference on robotics and automation, vol 2, pp 1682-1687, 2004 HVTH: Lâm Thế Kiên Trang 75 LUẬN VĂN TỐT NGHIỆP GVHD TS Đỗ Đức Nam [11] J Funda, and R P Paul, “A symbolic teleoperator interface for time-delayed underwater robot manipulation”, In Ocean technologies and opportunities in the Pacific for the 90s, pp 1526-1533, 1991 [12] R Uhrich, “Terminus controlled deep ocean manipulator”, OCEANS, 5, pp 301304, 1973 [13] A J Madhani, G Niemeyer, and Jr J K Salisbury, “The black falcon: A teleoperated surgical instrument for minimally invasive surgery”, In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems, vol 2, pp 936944, 1998 [14] N Diolaiti, and C Melchiorri, “Teleoperation of a mobile robot through haptic feedback” In IEEE international workshop on haptic virtual environments and their applications, pp 67-72, 2002 [15] D Lee, O Martinez-Palafox, and M W Spong, “Passive bilateral Teleoperation of a wheeled mobile robot over a delayed communication network” In Proceedings of the IEEE international conference on robotics and automation, pp 3298-3303, Orlando, FL, 2006 [16] H Khalil, Nonlinear systems Prentice Hall, Upper Saddle River, NJ07458, 1996 [17] Giáo trình robot công nghiệp – TS Phạm Đăng Phƣớc [18] D Lee and M.W Spong, “Bilateral Teleoperation of Multiple Cooperative Robots over Delayed Communication Networks: Theory”, Proc IEEE International Conference on Robotics and Automation, pp 362-367, Barcelona, Spain, April 2005 [19] G Tao, “A Simple Alternative to the Barbalat Lemma,” IEEE Trans on Automatic Control, Vol 42, No 5, pp 698, 1994 HVTH: Lâm Thế Kiên Trang 76 ... Hệ thống đƣa hệ SMSS với Robot có cấu trúc giống Chƣơng : Thiết kế điều khiển hệ thống SMSS Chỉ mục tiêu điều khiển hệ, đề xuất thiết kế hệ thống điều khiển, phân tích tính ổn định hệ thống điều. .. Nam 3.4 Động lực học môi trƣờng 43 3.5 Độ trễ kênh truyền thông 46 THIẾT KẾ ĐIỀU KHIỂN CHO HỆ SMSS 48 4.1 Mục tiêu điều khiển 48 4.2 Thiết kế điều khiển ... tốt nghiệp với đề tài Tính toán động lực học thiết kế điều khiển cho hệ thống Teleoperation SMSS kênh truyền thông có trễ” Đây đề tài đƣợc quan tâm lớn từ phía nhà khoa học khắp giới, tiêu biểu