Nghiên cứu các phương pháp điều khiển Robot công nghiệp

LỜI MỞ ĐẦU 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 Robot ứng dụng rộng rãi đóng vai trò quan trọng sản xuất đời sống Robot cấu đa chức có khả lập trình dùng để di chuyển nguyên vật liệu, chi tiết, dụng cụ thông qua truyền động lập trình trước Robot đóng vai trò quan trọng tự động hoá linh hoạt công tác vận chuyển bổ trợ cho máy CNC, dây chuyền lắp ráp, sơn hàn tự động, thao tác lặp lặp lại, vùng nguy hiểm Một robot chuyển động từ vị trí sang vị trí khác để cung cấp chi tiết đồng thời giao tiếp với thiết bị ngoại vi PLC, bàn điều khiển hệ thống mạng truyền thông công nghiệp Ưu điểm quan trọng kỹ thuật robot tạo nên khả linh hoạt hóa 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 mẻ để tạo nên dây chuyền tự động cho sản xuất hàng loạt với nhiều mẫu, loại sản phẩm Kỹ thuật robot công nghiệp máy vi tính đóng vai trò quan trọng việc tạo dây chuyền tự động linh hoạt (Hệ sản xuất hàng loạt FMS) Sau thời gian thực tập em giao đề tài tốt nghiệp với nội dung “Nghiên cứu phương pháp điều khiển Robot công nghiệp”, sau mười hai tuần em hoàn thành xong đồ án Bản đồ án em chia làm ba chương với nội dung sau: Chương Tổng quan Robot công nghiệp Chương trình bày khái niệm Robot, loại Robot điển hình, ứng dụng Robot Chương Các phương pháp điều khiển Robot công nghiệp Chương trình bày phương pháp điều khiển Robot không gian khớp không gian làm việc Chương Robot ba bậc tự Chương trình bày Robot ba bậc tự do, xác định động học vị trí, xây dựng phương trình động lực học, mô bốn phương pháp điều khiển robot phần mềm Malab-Simulink Sau mười hai tuần làm đồ án tốt nghiệp, hướng dẫn cô giáo ThS Phạm Thị Hồng Anh, đồ án em hoàn thành đầy đủ nội dung yêu cầu đề Do thời gian trình độ có hạn nên đồ án tốt nghiệp em không tránh khỏi thiếu sót Em mong nhận đóng góp ý kiến thầy cô bạn bè để đồ án tốt nghiệp em hoàn thiện Em xin chân thành cảm ơn! Sinh viên thực Lê Thành Trung CHƯƠNG TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 1.1 KHÁI QUÁT CHUNG VỀ ROBOT CÔNG NGHIỆP (RBCN) Robot công nghiệp thuật ngữ có nhiều quan điểm khác Có thể định nghĩa là: Robot công nghiệp cấu khí lập trình thực công việc có ích cách tự động không cần giúp đỡ trực tiếp người Theo ISO “Robot công nghiệp tay máy đa mục tiêu, có số bậc tự do, dễ dàng lập trình, điều khiển tự động, dùng để tháo lắp phôi, dụng cụ vật dụng khác” Do chương trình thao tác thay đổi, thực nhiều nhiệm vụ đa dạng nên nói robot công nghiệp hiểu thiết bị tự động, linh hoạt, bắt chước chức lao động người Theo đó, robot công nghiệp hệ thống tự động hóa lập trình được, giống NC, CNC, DNC AC Điểm khác biệt robot NC NC điều khiển chuyển động bề mặt, theo trục hệ tọa độ robot điều khiển chuyển động không gian Yếu tố đa chức nhấn mạnh robot có khả thực nhiều chức năng, phụ thuộc vào chương trình công cụ làm việc Ví dụ dây chuyền sản xuất ô tô, robot gắn mỏ hàn để thực công nghệ hàn phân xưởng Tại phân xưởng khác, robot có cấu hình tương tự với khâu tác động cuối thay mỏ hàn bàn kẹp điều khiển để vận chuyển chi tiết lắp ráp vào vị trí yêu cầu Ứng với chức khác nhau, chương trình điều khiển robot lập trình lại cho phù hợp Yếu tố đa chức điểm để phân biệt robot với máy tự động sử dụng sản xuất Hình 1.1 Robot công nghiệp IRB – 7600 1.2 TỰ ĐỘNG HÓA VÀ ROBOT CÔNG NGHIỆP Hai lĩnh vực tự động hóa (Automation) kỹ thuật robot (Robotics) có nhiều liên quan mật thiết với Về phương diện công nghiệp, tự động hóa công nghệ liên kết với sử dụng hệ thống khí, điện tử hệ thống máy tính vận hành điều khiển sản xuất Ví dụ, dây chuyền vận chuyển, máy lắp ráp khí, hệ thống điều khiển phản hồi, máy công cụ điều khiển chương trình số robot Như vậy, coi robot dạng thiết bị tự động hóa công nghiệp - Có ba loại hệ thống tự động hóa công nghiệp: Tự động hóa cố định, tự động hóa lập trình tự động hóa linh hoạt + Tự động hóa cố định hệ thống sản xuất mà trình tự hoạt động cố định, xác lập sẵn thiết bị Mỗi hoạt động trình thường đơn giản Các máy móc kết hợp hoạt động lại hệ thống phức tạp + Tự động hóa lập trình đặc trưng khả thay đổi trình tự sản xuất theo loại sản phẩm Trình tự sản xuất điều khiển chương trình + Tự động hóa linh hoạt bước phát triển cao tự động hóa lập trình được, hệ thống đáp ứng yêu cầu thay đổi sản phẩm mà không thời gian để thiết lập lại trình tự hoạt động, hệ thống sản xuất loại sản phẩm khác theo lịch trình khác Robot có liên quan mật thiết với tự động hóa lập trình Robot máy có khả lập trình có số đặc tính giống người Robot lập trình để di chuyển cách tay thông qua trình tự chuyển động có tính chu kỳ để thực nhiệm vụ khác Ví dụ, máy bốc dỡ hàng, robot hàn, sơn… Robot sử dụng rộng rãi hệ thống sản xuất linh hoạt hệ thống tự động hóa cố định Hệ thống gồm số máy, robot làm việc điều khiển máy tính điều khiển lập trình Ví dụ, dây chuyền hàn vỏ ô tô gồm nhiều cánh tay robot có nhiệm vụ hàn phận khác Chương trình lưu trữ máy tính nạp cho robot làm việc phận dây chuyền hàn ô tô Như dây chuyền sản xuất linh hoạt với mức độ tự động hóa cao 1.3 SƠ LƯỢC VỀ LỊCH SỬ PHÁT TRIỂN CỦA ROBOT CÔNG NGHIỆP Thuật ngữ “Robot” xuất phát từ tiếng Sec (Czech) “Robota” (có nghĩa công việc tạp dịch) kịch Rossum’s Universal Robots Karel Capek vào năm 1920 Trong kịch này, nhân vật Rossum trai ông ta chế tạo máy ứng xử người, có khả làm việc khỏe gấp đôi người, cảm tính, cảm giác người Về mặt kỹ thuật, robot công nghiệp ngày có nguồn gốc từ hai lĩnh vực kỹ thuật đời sớm cấu điều khiển từ xa (Teleoperators) máy công cụ điều khiển số (NC-Numerically Controlled machine tool) Năm 1952, mẫu máy điều khiển số trưng bày Viện Công Nghệ Massachuasetts sau vài năm nghiên cứu chế tạo Các cấu điều khiển từ xa (hay thiết bị kiểu chủ-tớ) phát triển mạnh Chiến tranh giới lần thứ II nhằm nghiên cứu vật liệu phóng xạ Người thao tác tách biệt khỏi khu vực phóng xạ tường có vài cửa quan sát để nhìn thấy công việc bên Các cấu điều khiển từ xa thay cho cánh tay người thao tác; gồm có kẹp bên (tớ) hai tay cầm bên (chủ) Cả hai, tay cầm kẹp, nối với cấu sáu bậc tự để tạo vị trí hướng tùy ý tay cầm kẹp Cơ cấu dùng để điều khiển kẹp theo chuyển động tay cầm Vào khoảng năm 1949, máy công cụ điều khiển số đời, nhằm đáp ứng yêu cầu gia công chi tiết nghành chế tạo máy bay Những robot thực chất nối kết khâu khí cấu điều khiển từ xa với khả lập trình máy công cụ điều khiển số Một robot công nghiệp chế tạo robot Versatran công ty AMF Mỹ vào năm 1960 Cũng vào khoảng thời gian Mỹ xuất loại robot Unimate dùng kỹ nghệ ô tô Tiếp theo Mỹ, nước khác bắt đầu sản xuất robot công nghiệp: Anh1967, Thụy Điển Nhật-1968 theo quyền Mỹ; CHLB Đức-1971; Pháp-1972; Ý-1973… Tính làm việc robot ngày nâng cao, khả nhận biết xử lý Năm 1968, trường đại học tổng hợp Stanford (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 Cincinnati Mỹ đưa loại robot điều khiển máy vi tính, gọi robot T3 (The Tomorrow Tool: Công cụ tương lai) Robot nâng vật có khối lượng đến 40kg Năm 1976, cánh tay robot không gian tàu thám hiểm Viking quan không gian Nasa Hoa Kỳ để lấy mẫu đất Hỏa Từ năm 70, việc nghiên cứu nâng cao tính robot ý nhiều đến lắp đặt thêm cảm biến để 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 dùng để lắp ráp tự động điều khiển máy vi tính sở xử lý thông tin từ cảm biến lực thị giác Cũng vào thời gian công ty IBM chế tạo loại robot có cảm biến xúc giác cảm biến lực, điều khiển máy tính để lắp ráp máy in gồm 20 cụm chi tiết Năm 1990, có 40 công ty Nhật Bản có công ty khổng lồ công ty Hitachi công ty Mitsubishi đưa thị trường quốc tế nhiều loại robot tiếng Trong năm sau này, việc nâng cao tính hoạt động robot không ngừng phát triển Các robot trang bị thêm loại cảm biến khác để nhận biết môi trường xung 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 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ố kỹ thuật chế tạo cảm biến, công nghệ lập trình phát triển trí không nhân tạo, hệ chuyên gia… Số lượng robot ngày 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 chuyền sản xuất đại 1.4 CÁC ĐẶC TÍNH CỦA ROBOT CÔNG NGHIỆP 1.4.1 Tải trọng Tải trọng trọng lượng robot mang giữ đảm bảo số đặc tính Tải trọng lớn lớn tải trọng định mức nhiều robot mang tải trọng lớn định mức robot không đảm bảo độ xác di chuyển Tải trọng robot thông thường nhỏ trọng lượng robot 1.4.2 Tầm với Tầm với khoảng cách lớn robot vươn tới phạm vi làm việc Tầm với hàm phụ thuộc vào cấu trúc robot 1.4.3 Độ phân giải không gian Độ phân giải không gian lượng gia tăng nhỏ robot thực di chuyển không gian, phụ thuộc vào độ phân giải điều khiển sai số khí Độ phân giải điều khiển, kí hiệu CR, xác định độ phân giải hệ thống điều khiển vị trí hệ thống phản hồi: tỷ số phạm vi di chuyển số bước di chuyển khớp địa hóa điều khiển robot: CR = (dải chuyển động)/2n Trong n số bit để biểu diễn số hệ thống điều khiển Sai số khí phụ thuộc vào khe hở hộp truyền, rò rỉ hệ thống thủy lực, tải trọng tay robot, tốc độ di chuyển, điều khiện bảo dưỡng robot Nói chung sai số khí tuân theo phân bố xác suất chuẩn Độ phân giải không gian, kí hiệu SR, xác định sau: SR = CR+6 (độ lệch chuẩn phân bố sai số khí) 1.4.4 Độ xác Độ 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 Độ xác = CR/2+3 (độ lệch chuẩn phân bố sai số khí) Độ xác = SR/2 1.4.5 Độ lặp lại Độ lặp lại đánh giá độ xác robot di chuyển để với tới điểm nhiều lần hoạt động Do sai số khí mà robot với tới điểm nhiều lần hoạt động, mà điểm với robot nằm vòng tròn với tâm điểm đích mong muốn Bán kính đường tròn độ lặp lại Độ lặp lại = (+/-) 3.(độ lệch chuẩn phân bố sai số khí) 1.4.6 Độ nhún Độ nhún biểu thị dịch chuyển điểm cuối cổ tay robot đáp ứng lại lực momen tác dụng Độ nhún lớn có nghĩa tay robot dịch chuyển nhiều lực tác dụng nhỏ ngược lại Độ nhún có ý nghĩa quan trọng làm giảm độ xác dịch chuyển robot mang tải trọng Nếu robot mang tải trọng nặng, trọng lượng tải trọng làm cho cánh tay robot bị dịch chuyển 1.5 HỆ THỐNG ROBOT TRONG CÔNG NGHIỆP - Một hệ thống RBCN điển hình gồm phận sau: + Hệ thống chuyển động + Hệ thống truyền động + Hệ thống điều khiển + Hệ thống cảm biến 1.5.1 Hệ thống chuyển động robot Các robot công nghiệp ngày thường đặt đế gắn chặt sàn Cơ thể gắn với đế, tổ hợp cánh tay gắn với thể Cuối cánh tay cổ tay Hệ thống chuyển động RBCN đảm bảo cho robot thực nhiệm vụ không gian làm việc bao gồm chuyển động thân, cánh tay, cổ tay vị trí chuyển động theo quỹ đạo đặt trước Bộ phận robot cánh tay (arm), cánh tay cấu thành nối liên kết với qua khớp nối mềm (joint), nhờ có khớp nối mà có chuyển động tương đối hai nối liền Cánh tay robot gắn lên thân (bệ – base), cổ tay (wrist) gắn nối cuối cánh tay robot, bàn tay (hand – gọi cấu tác động cuối(end effector)) gắn lên cổ tay để thực nhiệm vụ theo yêu cầu công nghệ: cầm nắm gia công Hình 1.2 Hình dạng khí RBCN a Bậc tự robot Bậc tự robot 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 đối tượng Như robot công nghiệp điển hình có số bậc tự Nếu số bậc tự nhỏ không gian chuyển động tay robot bị hạn chế Với robot bậc tự do, tay robot 10 Toa Z cua diem tac dong cuoi 0.45 0.4 0.35 0.3 0.25 Toa (m) 0.2 0.15 0.1 0.2 0.4 0.6 0.8 1.2 1.4 1.6 1.8 Thoi gian(s) Hình 3.21 Tọa độ z điểm tác động cuối Nhận xét kết mô phỏng: +) Từ hình 3.19, 3.20, 3.21 ta thấy hệ thống điều khiển ma trận Jacobian chuyển vị thiết kế ổn định với tiêu chất lượng: - Không có độ điều chỉnh - Sai lệch tĩnh - Thời gian độ tương đối nhỏ(khoảng 2s) Giải thích mục 3.5.1 +) ảnh hưởng ma trận hệ số Kp, Kd tới chất lượng hệ thống: - Giữ nguyên kd: tăng kp khoảng [100 , 200] thời gian độ giảm giảm kp ngược lại - Giữ nguyên kp: tăng kd khoảng [5 , 50] thời gian độ giảm, độ điều chỉnh giảm giảm kd ngược lại 70 b Hệ thống điều khiển ma trận Jacobian nghịch đảo + Các thông số robot giữ nguyên mục 3.5.1 + Luật điều khiển có dạng (2-27):  - X)   + G(q) τ = J 1  K p (X d - X) + K d (X d  với  R3 vectơ mô men khớp; J  R 3x3 ma trận Jacobian, xác định chương 2; Kp  R 3x3 , Kd  R 3x3 ma trận đường chéo dương; Xd  R , X  R tương ứng vectơ vị trí đặt vectơ vị trí thực tay robot,   R tương ứng vectơ tốc độ đặt vectơ tốc độ thực tay   R3 , X X d robot; q  R vị trí thực khớp robot; G(q)  R thành phần trọng lực robot, xác định chương Sơ đồ mô Simulink hình 3.22 luật điều khiển khác (các file có đuôi “.m” kèm theo trình bày phần phụ lục) V MATLAB Function (V.m) dyna MATLAB Function s qd s (dyna.m) q DHT_s MATLAB Function momen toa X (X.m) Luat DK Jacobian_sdot MATLAB Function MATLAB Function (J_nghichdao.m) Xd X toa Y (Xdot.m) Xd toa Z 71 Hình 3.22 Sơ đồ Simulink mô hệ thống điều khiển ma trận Jaccobian nghịch đảo + Để điều khiển điểm đặt với vectơ vị trí đặt tay Xd =  x d , yd ,z d  T = [0.3, 0.16, 0.25]T, thay đổi hệ số kp khoảng [50 , 150] thay đổi hệ số kd khoảng [5 , 30], ta chọn ma trận hệ số Kp Kd cho chất lượng hệ thống tốt nhất: 0  120   K p =  120   0 120   15 0    K =  15  d  0 15   Kết mô tọa độ x, y z sau: Toa X cua diem tac dong cuoi 0.45 0.4 0.35 Toa (m) 0.3 0.25 0.2 0.4 0.6 0.8 1.2 1.4 1.6 1.8 Thoi gian(s) Hình 3.23 Tọa độ x điểm tác động cuối 72 Toa Y cua diem tac dong cuoi 0.35 0.3 0.25 0.2 0.15 Toa (m) 0.1 0.05 0 0.2 0.4 0.6 0.8 1.2 1.4 1.6 1.8 1.8 Thoi gian(s) Hình 3.24 Tọa độ y điểm tác động cuối Toa Z cua diem tac dong cuoi 0.3 0.25 0.2 Toa 0.15do (m) 0.1 0.05 0.2 0.4 0.6 0.8 1.2 1.4 1.6 Thoi gian(s) Hình 3.25 Tọa độ z điểm tác động cuối Nhận xét kết mô phỏng: 73 +) Từ hình 3.23, 3.24, 3.25 ta thấy hệ thống điều khiển ma trận Jacobian nghịch đảo ổn định với tiêu chất lượng: - Không có độ điều chỉnh - Sai lệch tĩnh - Thời gian độ tương đối nhỏ (nhỏ 1s) Giải thích mục 3.5.1 +) ảnh hưởng ma trận hệ số Kp Kd tới chất lượng hệ thống: - Giữ nguyên kd: tăng kp khoảng [50 , 120] thời gian độ giảm tăng kp khoảng [120 , 150] xảy tượng điều chỉnh - Giữ nguyên kp: tăng kd khoảng [5 , 15] độ điều chỉnh giảm, thời gian độ giảm tăng kd khoảng [15 , 30] thời gian độ tăng 74 KẾT LUẬN Sau mười hai tuần làm đồ án tốt nghiệp, hướng dẫn cô giáo ThS Phạm Thị Hồng Anh đồ án tốt nghiệp em hoàn thành Đồ án thực vấn đề sau: -Nghiên -Xây cứu phương pháp điều khiển Robot công nghiệp dựng phương trình động học vị trí động lực học Robot ba bậc tự -Mô phỏng, khảo sát đáp ứng Robot công nghiệp với bốn phương pháp điều khiển Robot công nghiệp Tuy nhiên, thời gian trình độ có hạn nên đồ án em chưa giải vấn đề sau: -Mô hình mô nhiều hạn chế Dù cố gắng đồ án em không tránh khỏi thiếu sót Em mong nhận đóng góp ý kiến thầy cô giáo để đồ án tốt nghiệp em hoàn thiện Em xin chân thành cảm ơn! 75 TÀI LIỆU THAM KHẢO [1] GS.TSKH Nguyễn Thiện Phúc, “ Robot công nghiệp”, Nhà xuất Khoa học kỹ thuật [2] TS Phạm Đăng Phước, “Giáo trình Robot công nghiệp”, Đại học Bách khoa Đà Nẵng [3] 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 [4] Nguyễn Doãn Phước, Lý thuyết điều khiển tuyến tính , Nhà xuất Khoa học kỹ thuật [5] Nguyễn Doãn Phước, Lý thuyết điều khiển phi tính , Nhà xuất Khoa học kỹ thuật [6] Phạm Công Ngô (2001), Lý thuyết điều khiển tự động, Nhà xuất khoa học kỹ thuật, Hà nội [7] Bộ môn Điện tự động công nghiệp, “Bài giảng Điều khiển robot”, Đại học Hàng hải Việt Nam [8] Nguyễn Phùng Quang (2005), “Matlab Simulink dành cho kỹ sư điều khiển tự động”, Nhà xuất khoa học kỹ thuật [9] Tài liệu tham khảo Internet 76 PHỤ LỤC PL1 Các file có đuôi “.m” kèm với sơ đồ Simulink hình 3.10 để mô hệ thống điều khiển phản hồi PD bù trọng lực không gian khớp % File “thongso_robot.m” global l1 l2 l3 m1 m2 m3 ; % cac thong so cua robot m1=15 m2=12 m3=8 l1=0.135 l2=0.225 l3=0.225 % File “M_term.m” function out= M_term(q1,q2,q3); % ham tinh ma tran M(q) thongso_robot lg1=l1/2 lg2=l2/2 lg3=l3/2 % khoang cach tu dau noi den tam khoi r1=0.03 J1=m1*r1*r1 J2=m2*l2*l2/3 J3=m3*l3*l3/3 % momen quan tinh cac noi qk11=m2*lg2^2*cos(q2)^2+J1 77 qk12=m3*(lg3^2*cos(q2+q3)^2+2*lg3*lg2*cos(q2)*cos(q2+q3)+l2^2*cos(q2) ^2) out(1,1)=qk11+qk12; % phan tu M11 out(1,2)=0; % phan tu M12 out(1,3)=0; % phan tu M13 out(2,1) =out(1,2); % phan tu M21 out(2,2) =m2*lg2^2+J2+J3+ m3*(lg3^2+l2^2+2*l2*lg3*cos(q3)); % phan tu M22 out(2,3)=m3*lg3^2+J3+m3*l2*lg3*cos(q3); % phan tu M23 out(3,1)=out(1,3); % phan tu M31 out(3,2)=out(2,3); % phan tu M32 out(3,3)=m3*lg3^2+J3; % phan tu M33 % File “V.m” function out = V(u); % ham tinh V(q, ) thongso_robot lg1=l1/2 lg2=l2/2 lg3=l3/2 q1=u(1);q2=u(2);q3=u(3);% vi tri khop dq1=u(4); dq2=u(5); dq3=u(6); % toc khop lsq31=-2*m2*lg2^2*sin(q2)*cos(q2)*dq2*dq1 lsq32=-2*m3*(lg3*cos(q2+q3) +l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2))*dq1*dq2 lsq33=-2*m3*(lg3*cos(q2+q3)+l2*cos(q2))*lg3*sin(q2+q3)*dq1*dq3 out(1)=lsq31+lsq32+lsq33; % phan tu V1 78 lsq41=-m2*lg2^2*sin(q2)*cos(q2)*dq1^2; lsq42=-m3*dq1^2*(lg3*cos(q2+q3)+l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2)); lsq43=-m3*lg3*l2*sin(q3)*(2*dq2*dq3+dq3^2); out(2)=lsq41+lsq42+lsq43; % phan tu V2 lsq51= -m3*lg3*sin(q2+q3)*dq1^2*(lg3*cos(q2+q3)+l2*cos(q2)); lsq52=-m3*lg3*l2*sin(q3)*dq2*(dq2+dq3+1); out(3)=lsq51+lsq52; % phan tu V3 % File “con_para.m” Kp=[200 0; 200 0; 0 200]; % ma tran he so Kp Kd=[25 0; 25 0; 0 25]; % ma tran he so Kd qd=[1;1.5;1.3]; % vecto vi tri dat cua cac khop % File “luat_dkpd” function out = luat_dkpd(u); % ham tinh momen theo luat dieu khien con_para; q= [u(1);u(2);u(3)] delq= qd-q % sai lech vi tri khop dq=[u(4);u(5);u(6)] % sai lech toc khop T=Kp*delq-Kd*dq % luat dieu khien phan hoi PD khong gian khop out=[T;qd]' % File “dyna.m” 79 function out = dyna(u); % tinh gia toc khop q1=u(4) q2=u(5) q3=u(6) M=M_term(q1,q2,q3) TU=[u(1);u(2);u(3)]; out=[M\TU]'; PL2 Các file “thongso_robot.m”,“M_term.m”,“V.m”, “G.m” tương tự PL1 có đuôi “.m” kèm với sơ đồ Simulink hình 3.14 để mô hệ thống điều khiển Momen tính không gian khớp % File “luat_momen” function out = luat_momen(u); % ham tinh momen theo luat dieu khien Kp=[200 0; 200 0; 0 200]; % ma tran he so Kp Kd=[25 0; 25 0; 0 25]; % ma tran he so Kd % vecto vi tri dat cua cac khop q= [u(1);u(2);u(3)]; qd=[u(4);u(5);u(6)]; delq= qd-q ;% sai lech vi tri khop deldq=[u(7);u(8);u(9)]; % sai lech toc khop qdd=[u(10);u(11) ;u(12)] T=Kp*delq-Kd*deldq +; % luat dieu khien Momen tinh toan out=T'; PL3 Các file có đuôi “.m” kèm với sơ đồ Simulink hình 3.18 để mô hệ thống điều khiển ma trận Jaccobian chuyển vị Các file “thongso_robot.m”, “M_term.m”, “Vg.m”, “G.m”, “dyna.m” giống PL1 % File “con_para.m” 80 Kp=[200 0; 200 0; 0 200]; Kd=[50 0; 50 0; 0 50]; Xd=[0.3;0.16;0.25]; % vecto dat vi tri cua tay % File “J_term.m” function out= J_term(q1,q2,q3); % ham tinh ma tran Jacobian global l1 l2 l3; out(1,1)= -l2*sin(q1)*cos(q2)-l3*sin(q1)*cos(q2+q3); out(1,2)= -l2*cos(q1)*sin(q2)-l3*cos(q1)*sin(q2+q3); out(1,3)= -l3*cos(q1)*sin(q2+q3); out(2,1)= l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3); out(2,2)= l2*sin(q1)*sin(q2)-l3*sin(q1)*sin(q2+q3); out(2,3)= -l3*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= l2*cos(q2)+l3*cos(q2+q3); out(3,3)= l3* cos(q2+q3); 81 % File “X.m” function out= X(u); % ham tinh X tu q qua phuong trinh dong hoc thuan global l1 l2 l3; q1= u(1); q2= u(2); q3= u(3); out(1)=l2 * cos(q1) * cos(q2) + l3* cos(q1) * cos(q2+q3); out(2)=l2 * sin(q1) * cos(q2) + l3* sin(q1) * cos(q2+q3); out(3)= l1+ l2*sin(q2) + l3* sin(q2+q3); % File “Xdot.m” function out= Xdot(u); % ham tinh toc cua tay q1= u(1); q2= u(2); q3= u(3); J= J_term(q1,q2,q3); out= J*[u(4);u(5);u(6)]; % File “J_chuyenvi.m” function out = J_chuyenvi(u); % ham tinh momen theo luat dieu khien Jacobian chuyen vi con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; % sai lech vi tri cua tay deldX= [u(4);u(5);u(6)]; % sai lech toc cua tay q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); 82 T= J'*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian chuyen vi out=[T;Xd]; PL4 Các file có đuôi “.m” kèm với sơ đồ Simulink hình 3.22 để mô hệ thống điều khiển ma trận Jaccobian nghịch đảo Tất file giống mục PL2 trừ file “con_para.m” % File “con_para.m” Kp=[120 0; 120 0; 0 120]; Kd=[25 0; 25 0; 0 25]; Xd=[0.3;0.16;0.25]; % File “J_nghichdao.m” function out = J_nghichdao(u); % ham tinh momen theo luat dieu khien Jacobian nghich dao con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; deldX= [u(4);u(5);u(6)]; q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); 83 T= inv(J)*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian nghich dao out=[T;Xd]; 84