(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO

(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO(Luận văn thạc sĩ) Nhận dạng thông số của Rôbôt bằng giải thuật PSO

LỜI CAM ĐOAN Tơi cam đoan cơng trình nghiên cứu Các số liệu, kết nêu luận văn trung thực chưa cơng bố cơng trình khác Tp Hồ Chí Minh, ngày… tháng … năm 2013 Đặng Thị Mỹ Chi Trang ii LỜI CẢM ƠN Em xin cảm ơn thầy TS Nguyễn Minh Tâm suốt thời gian làm luận văn tận tình bảo cung cấp cho em nhiều tài liệu quý giá giúp em hoàn thành luận văn Em xin chân thành cảm ơn thầy, q trình giảng dạy em lớp cao học kĩ thuật điện tử khóa 2010A, thầy mơn Kĩ Thuật Điện Tử - Khoa Điện Điện Tử Trường Đại Học Sư Phạm Kĩ Thuật Tp HCM trang bị cho em kiến thức tảng để em thực hồn thành luận văn Và cuối em xin gửi lời biết ơn tới cha me, bạn đồng nghiệp, bạn lớp trao đổi góp ý, tạo điều kiện thuận lợi cho em suốt trình thực luận văn Xin chân thành cảm ơn Đặng Thị Mỹ Chi Trang iii TÓM TẮT LUẬN VĂN Tên đề tài: Nhận dạng thông số robot giải thuật PSO Địa điểm nghiên cứu: Trường Đại Học Sư Phạm Kĩ Thuật Hồ Chí Minh Thời gian: 2011 - 2013 Luận văn trình bày việc xác định thơng số qn tính phương trình động lực học cánh tay robot bậc tự gồm khớp xoay sử dụng thuật toán bầy đàn PSO (Particle Swarm Optimization) Đề tài thực phương pháp mơ chương trình Simulink Matlab Đồng thời tiến hành đánh giá so sánh độ xác thuật tốn bầy đàn PSO với thuật toán di truyền GA (Genetic Algorithm) Kết luận văn xác định mơ hình động lực học cánh tay robot bậc tự do, xây dựng thành cơng mơ hình mơ để nhận dạng 13 thơng số qn tính cánh tay robot thuật toán bầy đàn PSO thuật toán di truyền GA Sai số việc nhận dạng hai phương pháp PSO GA tính tốn, từ việc so sánh sai số hai thuật toán cho ta thấy phương pháp ước lượng thông số PSO có độ xác tốt so với phương pháp GA Trang iv THESIS SUMMARY Name of subject: Dynamic identification of arm robot DoF using PSO Study location: University of Technical Education in HCMC Time: 2011 -2013 This thesis deals with the dynamic modeling and identification of an arm robot having DoF (Degrees of Freedom) of rotation joints PSO (Particle Swarm Optimization) technique and GA (Genetic Algorithm) were used to estimate 13 distinct inertia parameters of motion equation of the above arm robot The study was conducted by simulation using Simulink and Matlab The results of the thesis show that the dynamic model of the DoF robot arm was defined; simulation models were successfully built to identify the 13 inertial parameters of the robot arm using swarm algorithm PSO and genetic algorithm GA The identification errors of both PSO and GA methods were calculated to compare the performance of these two algorithms This showed that the method for estimating parameters using PSO have better accuracy than GA Trang v MỤC LỤC Trang tựa TRANG Quyết định giao đề tài Lý lịch cá nhân i Lời cam đoan ii Cảm tạ iii Tóm tắt iv Mục lục vi Danh sách chữ viết tắt viii Danh sách hình ix Danh sách bảng xi Chƣơng 1: Tổng quan 1.1 Tổng quan hướng nghiên cứu 1.2 Kết nghiên cứu nước 1.3 Mục tiêu đối tượng nghiên cứu Chƣơng 2: Cơ sở lý thuyết 2.1 Lý thuyết robot 2.1.1 Giới thiệu chung Robot 2.1.2 Các thông số cánh tay robot a Bậc tự robot (DOF: Degrees Of Freedom) b Hệ tọa độ (co-ordinate frames) c Các phép biến đổi 2.1.3 Phương trình động học robot (Kinematic Equations) 2.1.4 Động lực học robot (dynamic of robot) 12 a Cơ học Lagrange với vấn đề động lực robot 13 b Phương trình động lực học cánh tay robot n bậc tự 13 2.2 Các phương pháp nhận dạng thông số động lực học cho robot 15 2.2.1 Tổng quan phương pháp nhận dạng thông số động lực học robot 15 2.2.2 Các phương pháp nhận dạng thông số động lực học 17 2.3 Thuật toán bầy đàn (PSO: Particle Swarm Optimization) 22 Trang vi 2.3.1 Giới thiệu thuật toán PSO: 22 2.3.2 Xây dựng giải thuật PSO: 25 a) Các bước giải thuật PSO: 25 b) Lưu đồ giải thuật thuật toán PSO: 26 c) Những vấn đề cần quan tâm xây dựng giải thuật PSO 27 2.3.3 Đặc điểm ứng dụng giải thuật PSO 31 2.4 Sơ lược thuật toán di truyền GA (Genetic Algorithm) 32 2.4.1 Giới thiệu thuật toán di truyền GA 32 2.4.2 Thuật toán di truyền – GA (Genetic algorithm) 33 Chƣơng 3: Mơ nhận dạng thơng số qn tính cánh tay robot bậc 3.1 Cấu trúc cánh tay robot bậc tự gồm khớp xoay 42 3.1.1 Các thông số Denavit-Hartenberg cánh tay robot bậc 42 3.1.2 Phương trình động lực học cánh tay robot bậc 43 3.1.3 Các thông số mơ hình cần nhận dạng 51 3.2 Nhận dạng thông số quán tính 54 3.2.1 Sơ đồ khối thuật toán nhận dạng PSO 56 3.2.2 Sơ đồ mơ hình mơ phương trình động lực học cánh tay robot 55 3.2.3 Thuật tốn PSO để nhận dạng thơng số 56 3.2.4 Thuật tốn GA việc nhận dạng thơng số 60 3.3 Tiến trình mơ 64 3.4 Kết mô nhận xét 65 3.5 Đánh giá kết nhận dạng 77 Chƣơng 4: Kết luận 81 Tài liệu tham khảo 83 Phụ lục Code chương trình PSO viết Matlab 86 Phụ lục Code chương trình GA viết Matlab 98 Trang vii DANH SÁCH CÁC CHỮ VIẾT TẮT/ KÍ HIỆU KHOA HỌC DOF (Degrees Of Freedom) DH (Denavit- Hartenberg) PSO (Particle Swarm Optimization) GA (Genetic Algorithm) NST (Nhiễm Sắc Thể) Trang viii DANH SÁCH CÁC HÌNH HÌNH TRANG Hình 2.1 Các tọa độ suy rộng robot Hình 2.2 Chiều dài góc xoắn khâu 10 Hình 2.3 Các thơng số khâu: θ, d, a α 11 Hình 2.4 Khái niệm thay đổi điểm tìm kiếm PSO 24 Hình 2.5 Chuyển động cá thể 28 Hình 2.6 Sự tiến hóa tự nhiên 32 Hình 2.7 Phương pháp giải tốn dùng GA 33 Hình 2.8 Lưu đồ giải thuật di truyền toán kỹ thuật 34 Hình 2.9 Mã hóa số thực 36 Hình 2.10 Lai ghép rời rạc 36 Hình 2.11 Lai ghép BLX-  37 Hình 2.12 Lai ghép số học   0.5 37 Hình 2.13 Lai ghép đường thẳng 38 Hình 2.14 Lai ghép trực giác 38 Hình 2.15 Đột biến ngẫu nhiên 38 Hình 2.16 Đột biến khơng đồng 39 Hình 3.1 Sơ đồ cánh tay robot bậc tự khớp xoay (RRR) 42 Hình 3.2 Hệ thống nhận dạng sử dụng PSO 54 Hình 3.3 Mơ hình mơ hệ thống nhận dạng PSO 55 Hình 3.4 Tín hiệu mơ men tham chiếu momen nhận dạng PSO theo quĩ đạo mô (1) (2) 66 Hình 3.5 Tín hiệu mơ men tham chiếu momen nhận dạng PSO theo quĩ đạo mơ (3) (4) 67 Hình 3.6 Tín hiệu mơ men tham chiếu momen nhận dạng PSO theo quĩ đạo mô (5) (6) 68 Hình 3.7 Tín hiệu mơ men tham chiếu momen nhận dạng GA theo quĩ đạo mơ (1) (2) Trang ix 69 Hình 3.8 Tín hiệu mơ men tham chiếu momen nhận dạng GA theo quĩ đạo mô (3) (4) 70 Hình 3.9 Tín hiệu mơ men tham chiếu momen nhận dạng GA theo quĩ đạo mơ (5) (6) 71 Hình 3.10 Sai số trình nhận dạng PSO ứng với quĩ đạo mơ 73 Hình 3.11 Sai số trình nhận dạng GA ứng với quĩ đạo mơ 74 Hình 3.12 Momen tính với thông số nhận dạng phương pháp PSO ứng với quĩ đạo đánh giá 78 Hình 3.13 Momen tính với thơng số nhận dạng phương pháp GA ứng với quĩ đạo đánh giá Trang x 79 DANH SÁCH CÁC BẢNG BẢNG TRANG Bảng 3.1: Bảng thông số DH cánh tay robot bậc RRR 42 Bảng 3.2 Các biến để nhận dạng thơng số qn tính 53 Bảng 3.3 Một số trường hợp mô để lựa chọn hệ số c1, c2 thuật toán PSO 58 Bảng 3.4 Một số trường hợp mô để lựa chọn hệ số w thuật toán PSO 58 Bảng 3.5 Một số trường hợp mơ để lựa chọn kích thước quần thể thuật toán PSO 59 Bảng 3.6 Một số trường hợp mơ để lựa chọn số vịng lặp tối đa thuật toán PSO 60 Bảng 3.7 Một số trường hợp mô để lựa chọn hệ số lai chéo thuật toán GA 62 Bảng 3.8 Một số trường hợp mô để lựa chọn hệ số đột biến thuật toán GA 63 Bảng 3.9 Một số trường hợp mơ để lựa chọn kích thước quần thể thuật toán GA 63 Bảng 3.10 Một số trường hợp mô để lựa chọn số vịng lặp tối đa thuật tốn GA 64 Bảng 3.11 Giá trị ước lượng thông số a thuật toán PSO GA 75 Bảng 3.12 Giá trị ước lượng thành phần ma trận quán tính phương pháp PSO GA 76 Bảng 3.13: Trị trung bình (mean), độ lệch chuẩn phương sai giá trị ước lượng 77 Bảng 3.14: Sai số (RAE) phương pháp theo quĩ đạo đánh giá 80 Trang xi HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm Bảng 3.13: Trị trung bình (mean), độ lệch chuẩn (𝜎) phương sai (𝜎 ) giá trị ước lượng Thông số Gá trị tham chiếu Axx2 0.25600 0.256695 Axx3 0.13200 Byy1 Trung bình (Mean) PSO GA Độ lệch chuẩn (𝜎 ) PSO GA Phương sai (𝜎 ) PSO GA 0.256046 0.002894 0.001885 8.38E-06 3.55E-06 0.133552 0.133714 0.00445 0.005601 1.98E-05 3.14E-05 0.01737 0.019412 -0.03453 0.005241 0.094737 2.75E-05 0.008975 Byy2 0.12900 0.12973 0.149475 0.008985 0.020323 8.07E-05 0.000413 Byy3 0.41250 0.417898 0.422863 0.029912 0.017815 0.000895 0.000317 Czz2 1.98640 1.993953 1.981984 0.020438 0.080124 0.000418 0.00642 Czz3 0.54720 0.547225 0.540774 3.53E-05 0.048441 1.24E-09 0.002347 D2 -0.04050 -0.04064 -0.03309 0.001541 0.031182 2.37E-06 0.000972 D3 2.15700 2.1589 2.16863 0.002719 0.033667 7.39E-06 0.001133 E2 0.04980 0.047943 0.042109 0.002697 0.024347 7.28E-06 0.000593 E3 0.16600 0.164616 0.165807 0.003015 0.011379 9.09E-06 0.000129 F2 -0.25430 -0.25469 -0.25457 0.000739 0.005832 5.47E-07 3.4E-05 F3 0.29390 0.294509 0.292797 0.001231 0.029555 1.51E-06 0.000873 Từ bảng 3.13, ta thấy phương pháp PSO có khả tìm thơng số với xác suất có giá trị trung bình gần với giá trị tham chiếu cao so với phương pháp GA Phần lớn thông số nhận dạng phương pháp PSO có độ lệch chuẩn nhỏ so với phương pháp GA trừ trường hợp thông số Axx2 Byy3 3.5 Đánh giá kết nhận dạng Để đánh giá kết ước lượng phương pháp nhận dạng PSO, giá trị trung bình thơng số quán tính sau nhận dạng (bảng 3.5) sử dụng để tính lại momen theo quĩ đạo đánh giá((3.100), (3.101), (3.102)) kết so sánh momen tham chiếu (T_ref) momen tính từ thông số nhận dạng (Torque) tương ứng với quĩ đạo đánh giá trình bày hình 3.12 3.13 Quĩ đạo đánh giá 1: 𝜋 𝜃1 𝑡 = −3 + 0.5 sin 𝑡 - Trang 77 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm 𝜃2 𝑡 = 1.2 + 0.7 cos 𝑡 𝜃3 𝑡 = −2 cos 2𝑡 (3.100) Quĩ đạo đánh giá 2: 𝜋 𝜃1 𝑡 = 0.5 cos 𝑡 𝜋 𝜃2 𝑡 = −1.2 + 0.1 cos 𝑡 - 𝜃3 𝑡 = −0.7 + sin 𝜋 𝑡 (3.101) Quĩ đạo đánh giá 3: π θ1 t = cos t θ2 t = + 0.2 cos 2t θ3 t = −1.5 + cos t (3.102) Hình 3.12 Momen tính với thơng số nhận dạng phương pháp PSO ứng với quĩ đạo đánh giá Trang 78 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm Hình 3.13 Momen tính với thơng số nhận dạng phương pháp GA ứng với quĩ đạo đánh giá Kết mô đạt từ phần đánh giá cho ta thấy có trùng khớp tốt momen tham chiếu momen tính từ thông số ước lượng hai phương pháp PSO GA Để định lượng độ xác tập giá trị ước lượng được, sai số T_ref Torque kết mô quĩ đạo đánh giá phân tích áp dụng tiêu chuẩn sai số RAE (Relative Absolute Error) theo biểu thức (3.93) Bảng 3.14 cho ta thấy giá trị RAE tính tương ứng với quĩ đạo đánh giá hai phương pháp Trang 79 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm Bảng 3.14: Sai số (RAE) phương pháp theo quĩ đạo đánh giá Quĩ đạo đánh giá Khớp RAE1 PSO Khớp RAE2 Khớp RAE1 GA Khớp RAE2 Khớp RAE3 Khớp RAE3 0.01479 0.00037 8.75E-05 0.00866 0.00640 0.00849 0.01103 0.00055 0.00024 0.01551 0.01221 0.01385 0.01791 0.00383 0.00737 0.12528 0.01895 0.02384 Từ kết mô từ bảng 3.14 cho ta thấy rằng, bên cạnh phương pháp sử dụng để nhận dạng dạng quĩ đạo sử dụng q trình nhận dạng có ảnh đáng kể đến độ xác q trình ước lượng thơng số động lực học cánh tay robot Thông qua số liệu sai số RAE khớp quĩ đạo, ta thấy hiệu nhận dạng thơng số có khác biệt sai số RAE khớp thường cao so với khớp Trong quĩ đạo đánh giá phương pháp PSO thường cho giá trị sai số nhỏ so với phương pháp GA, trừ trường hợp sai số khớp quĩ đạo đánh giá Tập giá trị xác định phương pháp PSO có độ xác caovới ngưỡng sai số thấp (từ 8.75e-5 – 0.01791), tốt so với phương pháp GA (0.0064 – 0.12528) Trang 80 HVTH: Đặng Thị Mỹ Chi Tâm GVHD: TS Nguyễn Minh CHƯƠNG KẾT LUẬN 4.1 Kết đạt : Qua trình thực đề tài, người nghiên cứu thành cơng việc tìm hiểu cấu trúc mơ hình cánh tay robot, hiểu cách thức xây dựng mơ hình động lực học cánh tay robot, đồng thời hiểu thuật toán bầy đàn PSO việc tìm kiếm tối ưu hóa thơng số mơ hình động lực học cánh tay robot Đã thực mô thành công việc sử dụng thuật toán PSO để nhận dạng thơng số qn tính có ảnh hưởng đến phương trình động lực học cánh tay robot bậc tự gồm khớp xoay 13 giá trị ma trận quán tính xác định qua lần mô theo quĩ đạo khác Đồng thời tiến hành mô để nhận dạng thông số phương pháp GA để so sánh đánh giá kết mơ thuật tốn PSO Kết ước lượng thông số đánh giá lại quĩ đạo đánh giá khác kết chứng tỏ phương pháp PSO có khả tìm kiếm giá trị với độ xác cao ngưỡng sai số từ 8.75e-5 – 0.01791 Các thơng số ước lượng có khả cho phương trình động lực học xác, vào kết đạt từ mô hình đánh giá: momen ước lượng có độ trùng khớp cao với momen tham chiếu Độ xác q trình ước lượng thơng số động lực học cánh tay robot phụ thuộc vào phương pháp sử dụng để nhận dạng mà cịn chịu ảnh hưởng nhiều vào dạng quĩ đạo sử dụng q trình nhận dạng Do đó, để nâng cao hiệu suất việc nhận dạng, ta nên tính tốn lựa chọn dạng quĩ đạo cho kết đạt tốt Trang 81 HVTH: Đặng Thị Mỹ Chi Tâm GVHD: TS Nguyễn Minh 4.2 Hạn chế đề tài : Đề tài tồn nhiều giới hạn, : - Khơng có mơ hình robot thực để kiểm nghiệm thực tế tính khả thi phương pháp tìm kiếm - Chỉ tiến hành nhận dạng thông số quán tính mơ hình động lực học, chưa thực việc nhận dạng giá trị cho thông số khác thơng số có liên quan đến ma sát, vị trí trọng tâm link - Chưa đưa phương pháp để tìm quĩ đạo tối ưu để sử dụng trình nhận dạng nhằm tối đa hiệu việc nhận dạng 4.3 Hướng phát triển đề tài : - Tiến hành xây dựng mơ hình robot thực nghiệm để kiểm chứng tính khả thi mơ hình nhận dạng thực tế - Nhận dạng thêm thơng số khác mơ hình động lực học thơng số có liên quan đến ma sát - Áp dụng phương pháp nhận dạng khác để so sánh đánh giá, từ chọn phương pháp nhận dạng hiệu - Đi tìm quĩ đạo tối ưu để nâng cao hiệu việc nhận dạng thông số Trang 82 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm TÀI LIỆU THAM KHẢO [1] Nguyễn Phùng Quang, Điều khiển Robot công nghiệp - Những vấn đề cần biết, Tạp chí Tự động hố ngày - số tháng 4, 5, 6/2006 [2] TS Phạm Đăng Phước, "Robot công nghiệp", NXB Xây Dựng Hà Nội – 2007 [3] John J Craig, “Introduction to Robotics: Mechanical and Control”, third edition, Pearson Prentice Hall, 2005 [4] Huỳnh Đức Chấn, “Ứng dụng giải thuật bầy đàn để xác định thông số PID điều khiển tốc độ động không đồng ba pha”, Hội nghị toàn quốc điều khiển tự động hóa – VCCA, 2011 [5] Man Zhihon, “ Robotics”, second edition, Pearson Prentice Hall, 2005 [6] Mark W Spong, Seth Hutchinson, and M Vidyasagar, “Robot Modeling and Control”, first edition, John Wiley & Sons, Inc 2006 [7] Mehrzad Namvar and Hossein Jahandideh, “Use of PSO in parameter estimation of robot dynamics”, 16th International Conference on Systems Theory, Control and computing, 2012 [8] Zafer Bingul and Oguzhan Karahan, “Dynamic identification of Staubli RX-60 robot using PSO and LS methods”, Expert systems with Application, Volume 38, 2011 [9] Jun Wu, Jinsong Wang, Zheng You, “An overview of dynamic parameter identification of robot”, Robotics and Computer-Integrated Manufacturing 26 (2010) 414–419 [10] Tayebeh Mostajabi and Javad Poshtan, “Control and System indentification via Swarm and Evolutionary Algorithms”, Intern J Scientific and Engineering Research Vol 2, Issued 10, October, 2011 [11] Dragan Kostic, Bram de Jager and Ron Hesen, “Modeling and Identification for high-performance robot control: an RRR- robotic arm case study”, IEEE transactions on control systems technology, vol 12, No.6, Nov, 2004 [12] Katayon Radkhah, Dana Kulic and Elilzabeth Croft, “Dynamic Parameter identification for the CRS A460 robot”, Processings of the 2007 IEEE/RSJ Trang 83 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm International conference on Intelligent robots and systems, USA, Oct 29 – Nov 2, 2007 [13] Guanbin Gao, WenWang, Keng Lin and Zichen Chen, “Kinematic calibration for articulated arm coordinate measuring machines base on particle swarm optimization”, second international conference on intelligent computation technology and automation, 2009 [14] Swevers J, Verdonck W, Schutter JD “Dynamic model identification for industrial robots” IEEE Control Systems Magazine 2007 [15] Gautier M, Janot A, Vandanjon DIDIM: “A new method for the dynamic identification of robots from only torque data” In: Proceedings of the IEEE international conference on robotics and automation, Pasadena, CA, USA, 2008 [16] Farhat N, Mata V, Page A, Valero F “Identification of dynamic parameters of a 3-DOF RPS parallel manipulator”, Mechanism and Machine Theory 2008 [17] Cheng Guo and Xiaoyong Yang, “A programming of Genetic Algorithm in Matlab 7.0”, Modern Applied Science, Vol.5, No 1, Feb 2011 [18] Xinchao Zhao and Xiao-Shan Gao, “Evolutionary programming based on NonUniform mutation”, MM research preprints, 352-374, MMRC, AMSS, Academia Sinica, N0 23, Dec 2004 [19] Rahul Malhotra, Narinder Singh and Yaduvir Singh, “Genetic Algorithms: Concepts, design for Optimization process controllers”, computer and information science, Vol 4, No 2, March, 2011 [20] Tayebeh Mostajabi and Javad Poshtan, “Control and System indentification via Swarm and Evolutionary Algorithms”, International Journal of Scientific & Engineering research, Vol 2, Issue 10, Oct – 2011 [21] Huang Jin, Ma Zi, Hu Ying, Wang Yang, Yu Shuanghe, “Industry robot and external axle calibration using particle swarm optimization algorithm”, Nation research center, Dalian Maritime University, P.R China, 2002 Trang 84 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm [22] Sameer Gokhale, “Design of fuzzy controller for three degree of robot arm”, a thesis submitted to the college of graduate studies Texas A&M University, Kingsville, 2006 [23] Chao Ou, Weixing Lin, “Comparision between PSO and GA for parameters optimization of PID controller”, Facuty of Information science and technology university of NingBo Pp 2471 – 2475 Trang 85 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm Phụ lục Code chương trình PSO viết Matlab clc clear all n = 50; dim = 13; maxit = 150; c1 = 1.2; % cognitive parameter c2 = 0.5 %social parameter w = 0.9; %inertia or momentum of pso R1 = rand(dim,n); R2 = rand(dim,n); % Initializing swarm and velocities current_position = rand(dim,n); vel = rand(dim,n); % Evaluate initial population cost = zeros(1,n); for k = 1:n alpha = setvalue_fn(current_position(:,k)); a1 = alpha(1);a2 = alpha(2);a3 = alpha(3);a4 = alpha(4); a5 = alpha(5);a6 = alpha(6);a7 = alpha(7);a8 = alpha(8); a9 = alpha(9);a10 = alpha(10);a11 = alpha(11);a12 = alpha(12); a13 = alpha(13);a14 = alpha(14);a15 = alpha(15);a16 = alpha(16); sim ('nhandang_pso'); N = length(TSE); cost(k) = sum(TSE)/N; end open 'nhandang_pso'; local_fitness = cost; local_position = current_position; [global_fitness,indx] = min(local_fitness); global_position = local_position(:,indx); iter = 0; while iter < maxit iter = iter + 1; vel = w*vel+ c1*(R1.*(local_position - current_position)) + c2*(R2.*(global_position*ones(1,n) - current_position)); current_position = current_position + vel; % Evaluate the new swarm for k = 1:n alpha = setvalue_fn(current_position(:,k)); a1 = alpha(1);a2 = alpha(2);a3 = alpha(3);a4 = alpha(4); a5 = alpha(5);a6 = alpha(6);a7 = alpha(7);a8 = alpha(8); a9 = alpha(9);a10 = alpha(10);a11 = alpha(11);a12 = alpha(12); a13 = alpha(13);a14 = alpha(14);a15 = alpha(15);a16 = alpha(16); sim ('nhandang_pso'); N = length(TSE); cost(k) = sum(TSE)/N; end %Updating the best of local position and local fitness bettercost = cost < local_fitness; local_fitness = local_fitness.*not(bettercost) + cost.*bettercost; p = find(bettercost); q = find(not(bettercost)); t1 = zeros(dim,n); t2 = zeros(dim,n); t1(:,q) = local_position(:,q); Trang 86 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm t2(:,p) = current_position(:,p); local_position = t1 + t2; % Updating global fitness [temp,r] = min(local_fitness); if temp < global_fitness global_fitness = temp; indx = r; global_position = local_position(:,indx); end end %while % tinh RAE x1 = error(1,:); x2 = error(2,:);x3 = error(3,:); T_ref1 = T_ref(1,:); T_ref2 = T_ref(2,:); T_ref3 = T_ref(3,:); RAE1 = cal_RAE(N,T_ref1,x1); RAE2 = cal_RAE(N,T_ref2,x2); RAE3 = cal_RAE(N,T_ref3,x3); alpha_est = setvalue_fn(global_position); para_est = zeros(1,13); l2c = 0.173; l3c = 0.06; l2 = 0.29; m2 =8.56; m3 = 3.6;g =9.81; Axx2 = alpha_est(5); Axx3 = alpha_est(6); Byy1 = alpha_est(7); Byy2 = alpha_est(1)- l2*l2*m3 -m2*l2c*l2c; Czz3 = alpha_est(2)- m3*l3c*l3c; Byy3 = alpha_est(3)+ Czz3; Czz2 = alpha_est(14)+Byy2; D2 = alpha_est(8); D3 = alpha_est(9); E2 = alpha_est(10); E3 = alpha_est(12); F2 = alpha_est(11); F3 = alpha_est(13); para_est(1,1) = Axx2; para_est(1,2)= Axx3; para_est(1,3)= Byy1; para_est(1,4) = Byy2; para_est(1,5)= Byy3; para_est(1,6)= Czz2; para_est(1,7) = Czz3; para_est(1,8)= D2; para_est(1,9)= D3; para_est(1,10)= E2; para_est(1,11)= E3; para_est(1,12)= F2; para_est(1,13)=F3; function aa = setvalue_fn(x) mm2 = 8.56; mm3 = 3.6; d2 = 0.29; d2c = 0.173; d3c = 0.06; g0 = 9.81; aa(1) = mm2*d2c*d2c + mm3*d2*d2 + x(1); aa(2) = mm3*d3c*d3c + x(2); aa(3) = x(3)-x(2); aa(4) = mm3*d2*d3c; aa(5:13) = x(4:12); aa(14) = x(13)-x(1); aa(15) = mm2*g0*d2c + mm3*g0*d2; aa(16)= mm3*g0*d3c; end Phụ lục Code chương trình GA viết Matlab clc clear all popsize = 65; dim = 13;maxit = 20; gmax = 150; t = 0; terminal = 0; epsilon = 0.000000001; mutrate = 0.05; crossrate = 0.9; elitism = 1; alpha= 0.5; par = rand(popsize,dim); %tao quan the ban dau ngau nhien pop = par; % danh gia fitness cua quan the ban dau Trang 87 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm fitness = zeros(1,popsize); for k = 1:popsize alpha = setvalue_fn(pop(k,:)); a1 = alpha(1);a2 = alpha(2);a3 = alpha(3);a4 = alpha(4); a5 = alpha(5);a6 = alpha(6);a7 = alpha(7);a8 = alpha(8); a9 = alpha(9);a10 = alpha(10);a11 = alpha(11);a12 = alpha(12); a13 = alpha(13);a14 = alpha(14);a15 = alpha(15);a16 = alpha(16); sim ('nhandang_GA'); Nt = length(e); fitness(k)= sum(e)/Nt; end [bestfit0, bestchrom]= min(fitness); bestfit = zeros(1,gmax); while ~ terminal t = t+1; % select parents - rank slection parent = selectmate(pop,fitness,dim); % cross-over - BLX_alpha child = cross_BLXalpha(parent,popsize,dim,alpha,crossrate); % mutation - non-uni form newpop = mutation(child,popsize,dim,mutrate,t,gmax,alpha); ss = child - newpop; for i = 1:popsize if elitism ==1 && i == bestchrom newpop(i,:) = pop(i,:); end end pop = newpop; for k = 1:popsize alpha = setvalue_fn(pop(k,:)); a1 = alpha(1);a2 = alpha(2);a3 = alpha(3);a4 = alpha(4); a5 = alpha(5);a6 = alpha(6);a7 = alpha(7);a8 = alpha(8); a9 = alpha(9);a10 = alpha(10);a11 = alpha(11);a12 = alpha(12); a13 = alpha(13);a14 = alpha(14);a15 = alpha(15);a16 = alpha(16); sim ('nhandang_GA'); Nt = length(e); fitness(k)= sum(e)/Nt; end [bestfit(t), bestchrom]= min(fitness); if t == gmax terminal =1; elseif t>1 if abs(bestfit(t)-bestfit(t-1)) == epsilon; terminal =1; end end end %while % tinh RAE N = length(T_ref); x1 = error(1,:); x2 = error(2,:);x3 = error(3,:); T_ref1 = T_ref(1,:); T_ref2 = T_ref(2,:); T_ref3 = T_ref(3,:); RAE1 = cal_RAE(N,T_ref1,x1); RAE2 = cal_RAE(N,T_ref2,x2); RAE3 = cal_RAE(N,T_ref3,x3); open 'nhandang_GA'; Trang 88 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm alpha_est = setvalue_fn(pop(bestchrom,:)); para_est = zeros(1,13); l2c = 0.173; l3c = 0.06; l2 = 0.29; m2 =8.56; m3 = 3.6;g =9.81; Axx2 = alpha_est(5); Axx3 = alpha_est(6); Byy1 = alpha_est(7); Byy2 = alpha_est(1)- l2*l2*m3 -m2*l2c*l2c; Czz3 = alpha_est(2)- m3*l3c*l3c; Byy3 = alpha_est(3)+ Czz3; Czz2 = alpha_est(14)+Byy2; D2 = alpha_est(8); D3 = alpha_est(9); E2 = alpha_est(10); E3 = alpha_est(12); F2 = alpha_est(11); F3 = alpha_est(13); para_est(1,1) = Axx2; para_est(1,2)= Axx3; para_est(1,3)= Byy1; para_est(1,4) = Byy2; para_est(1,5)= Byy3; para_est(1,6)= Czz2; para_est(1,7) = Czz3; para_est(1,8)= D2; para_est(1,9)= D3; para_est(1,10)= E2; para_est(1,11)= E3; para_est(1,12)= F2; para_est(1,13)=F3; function parent = selectmate(pop,fitness,dim) [cost,ind] = sort(fitness,'descend'); parent = zeros(2,dim); parent(1,:)= pop(ind(end),:); parent(2,:)= pop(ind(end-1),:); function child = cross_BLXalpha(parent,popsize,dim,alpha,crossrate) child = zeros(popsize, dim); for i = 1: popsize if crossrate > rand for k =1:dim ckmin = min(parent(1,k),parent(2,k))- alpha*abs(parent(1,k)parent(2,k)); ckmax = max(parent(1,k),parent(2,k))+ alpha*abs(parent(1,k)parent(2,k)); child(i,k)=ckmin(k)+rand*(ckmax(k)-ckmin(k)); end else child(i,:) = parent(1,:); end end function newpop = mutation(child,popsize,dim,mutrate,t,gmax,alpha) r = rand*1; y = 1-r.^((1-t/gmax).^alpha); newpop = child; nmut =ceil((popsize-1)*dim*mutrate) ; %total # of mutations mrow = sort(ceil(rand(1,nmut)*(popsize-1))+1); mcol = ceil(rand(1,nmut)*dim); lbmin = -10; ubmax = 10; for ii = 1:nmut to = round(rand); ck = newpop(mrow(ii),mcol(ii)); if to ==0 ck_ = ck + (ubmax - ck)*y(1); else ck_ = ck - (ck - lbmin)*y(1); end newpop(mrow(ii),mcol(ii)) = ck_; end function EE = cal_RAE(N,T_ref,e) Trang 89 HVTH: Đặng Thị Mỹ Chi GVHD: TS Nguyễn Minh Tâm x = abs(e); Tt = sum(T_ref)/N; %mean of T y = abs(T_ref-Tt); ee = zeros(1,N); for j = 1:N ee(j) = x(j)/y(j); end EE = sum(ee)/N; Trang 90 ... tổng quan nghiên cứu nhận dạng thông số động lực học từ trước đến Áp dụng phương pháp nhận dạng thông số giải thuật bầy đàn (PSO: Particle Swarm Optimization) để nhận dạng thơng số qn tính cánh tay... khối thuật toán nhận dạng PSO 56 3.2.2 Sơ đồ mô hình mơ phương trình động lực học cánh tay robot 55 3.2.3 Thuật toán PSO để nhận dạng thông số 56 3.2.4 Thuật toán GA việc nhận dạng. .. pháp nhận dạng thông số động lực học cho robot 15 2.2.1 Tổng quan phương pháp nhận dạng thông số động lực học robot 15 2.2.2 Các phương pháp nhận dạng thông số động lực học 17 2.3 Thuật