BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI ——————— * ——————— LƯU THỊ HUẾ PHÁT TRIỂN CÁC THUẬT TỐN THƠNG MINH ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA HỆ THỐNG ROBOT DẠNG TAY MÁY ĐÔI LUẬN ÁN TIẾN SĨ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA Hà Nội - 2021 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI ——————— * ——————— LƯU THỊ HUẾ PHÁT TRIỂN CÁC THUẬT TỐN THƠNG MINH ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA HỆ THỐNG ROBOT DẠNG TAY MÁY ĐÔI Ngành: Kỹ thuật điều khiển tự động hóa Mã số: 9520216 LUẬN ÁN TIẾN SĨ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC PGS TS NGUYỄN PHẠM THỤC ANH TS DƯƠNG MINH ĐỨC Hà Nội - 2021 LỜI CAM ĐOAN Tơi xin cam đoan cơng trình nghiên cứu hướng dẫn PGS TS Nguyễn Phạm Thục Anh TS Dương Minh Đức Các kết trình bày luận án trung thực chưa tác giả khác công bố Các thơng tin trích dẫn luận án xác, rõ nguồn gốc Hà Nội, ngày tháng năm 2021 Người hướng dẫn khoa học Nghiên cứu sinh PGS.TS Nguyễn Phạm Thục Anh Lưu Thị Huế TS Dương Minh Đức i LỜI CẢM ƠN Luận án hồn thành Bộ mơn Tự động hóa cơng nghiệp viện Điện, trường Đại học Bách Khoa Hà Nội, hướng dẫn tận tình, chu đáo, đầy trách nhiệm PGS.TS Nguyễn Phạm Thục Anh TS Dương Minh Đức Tác giả xin bày tỏ lòng biết ơn sâu sắc đến cô Nguyễn Phạm Thục Anh thầy Dương Minh Đức cảm thấy may mắn vinh dự nhận dìu dắt, hướng dẫn thầy đường nghiên cứu khoa học Tác giả xin gửi lời cảm ơn sâu sắc tới TS Nguyễn Danh Duy TS Võ Duy Thành, giảng viên mơn Tự động hóa cơng nghiệp – Viện Điện, Đại học Bách khoa Hà nội giúp đỡ phần xây dựng thực nghiệm, mạch đo cho luận án Tác giả xin trân trọng gửi lời cảm ơn đến Ban Giám hiệu, Phòng sau Đại học, Ban Chủ nhiệm viện Điện, Trường Đại học Bách Khoa Hà Nội, đặc biệt thầy cô giáo Bộ mơn Tự động hóa cơng nghiệp ln giúp đỡ, động viên, tạo môi trường học tập nghiên cứu thuận lợi cho tác giả Tác giả xin bày tỏ lòng biết ơn đến Ban Giám hiệu trường Đại học Điện lực, anh chị đồng nghiệp công tác Khoa Kỹ thuật điện, Trường Đại học Điện lực tạo điều kiện thuận lợi, giúp đỡ động viên tác giả suốt trình học tập nghiên cứu Lời cảm ơn sau cùng, tác giả xin dành cho gia đình, người ln u thương, chia sẻ, động viên tác giả vượt qua khó khăn để hoàn thành luận án Nghiên cứu sinh ii Mục lục Lời cam đoan i Lời cảm ơn ii Danh mục chữ viết tắt vi Danh mục kí hiệu tốn học vii Danh mục hình vẽ x Danh mục bảng biểu xi Mở đầu Chương TỔNG QUAN VỀ CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO HỆ THỐNG ROBOT DẠNG TAY MÁY ĐÔI 1.1 Tổng quan hệ thống robot dạng tay máy đôi ứng dụng 1.2 Tổng quan cơng trình nghiên cứu phương pháp điều khiển hệ thống robot dạng tay máy đôi 1.2.1 Các phương pháp điều khiển 1.2.2 Các phương pháp điều khiển nâng cao 1.3 Bộ quan sát 1.4 Kết luận chương Chương ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CỦA HỆ TAY MÁY ĐÔI 2.1 Nguyên lý cấu tạo làm việc hệ tay máy đôi 2.2 Các ràng buộc hình học để giữ liên lạc khâu tác động cuối robot với đối tượng 2.2.1 Mối quan hệ tay máy thứ đối tượng 2.2.2 Mối quan hệ tay máy thứ hai đối tượng 2.2.3 Mối quan hệ gữa vị trí đối tượng khâu tác động cuối tay máy 2.3 Động lực học hệ thống 2.3.1 Động hệ thống 2.3.2 Thế hệ thống 2.3.3 Phương trình động lực học hệ thống 2.4 Kết luận chương 4 10 13 15 16 17 17 18 18 19 20 20 21 23 24 30 Chương THIẾT KẾ ĐIỀU KHIỂN CƠ BẢN CHO HỆ TAY MÁY ĐÔI 31 3.1 Thiết kế điều khiển cho hệ tay máy đơi dùng thuật tốn tựa mơ hình 31 3.1.1 Điều khiển lực cho hệ tay máy đôi 32 iii 3.1.2 Điều khiển góc xoay đối tượng 3.1.3 Điều khiển vị trí đối tượng 3.1.4 Mô hệ tay máy dùng điều khiển tựa mơ hình 3.2 Thiết kế điều khiển cho hệ tay máy đơi dựa mơ hình quan sát GPI 3.2.1 Thiết kế quan sát GPI cho hệ tay máy đôi 3.2.2 Phân tích kẹp ổn định đối tượng robot tay máy đôi 3.2.3 Mô điều khiển cho hệ tay máy đôi dùng quan sát GPI 3.3 Kết luận chương 34 36 39 42 42 45 48 51 Chương NGHIÊN CỨU ĐIỀU KHIỂN THÔNG MINH CHO HỆ TAY MÁY ĐÔI 4.1 Điều khiển thích nghi cho hệ tay máy đôi sử dụng mạng noron xuyên tâm 4.1.1 Thuật toán điều khiển 4.1.2 Thiết kế mạng noron RBF 4.1.3 Xây dựng luật cập nhật trọng số 4.1.4 Mô kiểm chứng 4.2 Điều khiển thích nghi lai lực/ vị trí 4.2.1 Thiết kế lực tác động lên đối tượng 4.2.2 Điều khiển lai lực/ vị trí 4.2.3 Mô kiểm chứng Matlab/Simulink 4.3 Điều khiển thích nghi lai lực/vị trí sử dụng quan sát thích nghi 4.3.1 Thiết kế quan sát thích nghi 4.3.2 Điều khiển thích nghi lai lực/vị trí dựa quan sát thích nghi noron 4.4 Kết luận chương 53 53 53 57 59 60 64 64 65 66 71 71 74 87 Chương THỰC NGHIỆM 89 5.1 Thiết kế chế tạo mơ hình thực nghiệm tay máy đôi 89 5.2 Động lực học hệ tay máy đôi - đối tượng thực nghiệm 90 5.3 Mô với tay máy đôi – đối tượng, tay máy có hai bậc tự 91 5.4 Cấu trúc hoạt động hệ thống thực nghiệm 97 5.4.1 Các thiết bị phần cứng 98 5.4.2 Phần mềm điều khiển 98 5.4.3 Board kết nối 99 5.5 Mơ hình thực nghiệm toàn hệ thống 100 5.6 Thử nghiệm hệ thống 100 5.6.1 Điều khiển hỗn hợp lực vị trí đối tượng chuyển động theo trục x 101 5.6.2 Điều khiển hỗn hợp lực vị trí đối tượng chuyển động theo trục y 103 5.6.3 Điều khiển hỗn hợp lực hướng đối tượng 104 5.6.4 Điều khiển hỗn hợp lực, vị trí chuyển động theo trục x hướng đối tượng 105 iv 5.6.5 Điều khiển hỗn hợp lực, vị trí chuyển động theo trục y hướng đối tượng 106 5.6.6 Điều khiển hỗn hợp lực, vị trí chuyển động theo trục x trục y đối tượng 107 5.7 Kết luật chương 108 Kết luận chung luận án 110 Danh mục cơng trình khoa học 111 TÀI LIỆU THAM KHẢO 112 Phụ lục 122 v DANH MỤC CÁC CHỮ VIẾT TẮT Ký hiệu viết tắt Tiếng Anh Tiếng Việt ĐK Control Điều khiển GPI Generalized Integral GA Genetic Algorithm Thuật toán di truyền NN Neural network Mạng noron DOF Degrees of freedom Bậc tự RBFNN Radial basis function neural Mạng noron xuyên tâm network RBF Radial basis function SOSMC Second-order sliding mode Điều khiển trượt bậc hai control MRAC Model-reference control FFNN Feedforward neural network Mạng noron truyền thẳng NAO Neuro-adaptive observer Quan sát thích nghi noron Proportional Tỉ lệ tích phân tổng quát Xuyên tâm adaptive Điều khiển thích nghi tham chiếu mơ hình vi DANH MỤC CÁC KÝ HIỆU TỐN HỌC TT 10 11 12 13 14 15 Ký hiệu 16 (.) 17 18 19 20 21 22 23 24 25 26 27 (.) (.) ε Γ Ks , Λ g qi s fi λi θij θ ∆θ ∆(.) τ (.)T (.)−1 (.)+ I tr(·) diag(·) sgn(·) Mô tả Góc quay khớp tay máy thứ i Mặt trượt Lực vng góc với bề mặt đối tượng Lực song song với bề mặt đối tượng Góc khớp thứ j tay máy thứ i Góc xoay đối tượng Sai số góc xoay đối tượng Sai số Mô men tác động lên khớp Tham số, hàm ước lượng Ma trận chuyển vị Ma trận nghịch đảo Ma trận giả đảo Sai số ước lượng Tổng Đạo hàm cấp Đạo hàm cấp Chuẩn Sai số xấp xỉ Ma trận tham số Tham số điều khiển Gia tốc trọng trường g = 9, 8m/s2 Định nghĩa Ma trận đơn vị Vết ma trận Ma trận đường chéo Hàm dấu vii DANH MỤC CÁC HÌNH VẼ Hình Hình Hình Hình Hình Hình Hình Hình Hình 1.1: 1.2: 1.3: 1.4: 1.5: 1.6: 1.7: 1.8: 1.9: Định nghĩa phân cấp thao tác robot dạng tay máy đôi Hệ thống robot dạng tay máy đôi phối hợp FRIDA robot tay máy đôi ABB Lắp ráp với tay máy đôi công nghiệp thuộc dự án SMErobotics Robot tay máy đôi Motoman DA20 lắp ráp ghế Robot hạng nặng Hitachi để dọn dẹp FUKUSHIMA Bộ điều khiển tay máy đôi Hai robot PUMA uốn cong linh hoạt Robot tay máy đôi Stanford khám phá tàu đắm bị bỏ hoang Hình 1.10: Robot RIBA Nhật Bản nâng bệnh nhân từ giường lên xe lăn Hình 1.11: Robot tay máy đôi Motoman pha chế thuốc chống ung thư Hình 1.12: Tổng quan phương pháp điều khiển phối hợp chuyển động hệ tay máy đôi 10 Hình Hình Hình Hình Hình 2.1: 2.2: 2.3: 2.4: 2.5: Mơ hình tay máy đơi thao tác đối tượng Mối quan hệ vị trí đối tượng tay máy thứ Mối quan hệ vị trí đối tượng tay máy thứ hai Mô tả trọng tâm khâu tay máy thứ Mô tả trọng tâm khâu tay máy thứ hai Hình Hình Hình Hình Hình Hình Hình Hình Hình Hình Hình Hình Hình 3.1: 3.2: 3.3: 3.4: 3.5: 3.6: 3.7: 3.8: 3.9: 3.10: 3.11: 3.12: 3.13: Lực khâu tác động cuối tay máy tác động lên đối tượng Mô men khớp tay máy thứ Mô men khớp tay máy thứ hai Vị trí đối tượng chuyển động theo trục x Vị trí đối tượng chuyển động theo trục y Hướng đối tượng Sơ đồ khối hệ thống, điều khiển quan sát Vận tốc khớp tay máy thứ ước lượng chúng Vận tốc khớp tay máy thứ hai ước lượng chúng Vận tốc đối tượng ước lượng chúng Lực tiếp xúc ước lượng chúng Sai số ước lượng lực tay máy đôi Lực tác động khâu tác động cuối 40 40 40 41 41 41 48 49 49 49 50 50 51 Hình Hình Hình Hình 4.1: 4.2: 4.3: 4.4: Sơ đồ khối điều khiển sử dụng RBFNN Cấu trúc RBFNN Quỹ đạo chuyển động đối tượng theo trục x Quỹ đạo chuyển động đối tượng theo trục y 57 58 61 62 viii 17 18 19 21 22 • CN1B: Đưa tín hiệu điều khiển từ bên tới driver, sơ đồ kết nối driver với board kết nối Hình • CN1A: Đưa tín phản hồi góc, tốc độ đến điều khiển Sơ đồ kết nối driver hộp giao diện DS1103 (chân INC) thể Hình • CN2: Nhận tín hiệu phản hồi từ encoder động tới driver Sơ đồ kết nối hình • CN3: Được sử dụng để kết nối với máy tính, để thực giám sát, nhập lưu tham số, hiển thị đồ thị vận hành thử nghiệm Trong phần thực nghiệm tác giả không sử dụng tới cổng Một số chân cổng CN1A, CN1B ý nghĩa chúng ghi Bảng sau: Bảng Ý nghĩa chân Drive MR-J2S-40A Chân LA LAR LB LBR LZ LZR RD Pin 16 17 15 19 Cổng CN1A CN1A CN1A CN1A CN1A CN1A CN1A SON CN1B EMG RS1 RS2 15 CN1B CN1B TC VLAA 12 CN1B Ý nghĩa Xung pha A encoder Đảo LA Xung pha B encoder Đảo LB Xung pha Z encoder Đảo LZ Chân ready, RD bật động servo bật driver động sẵn sàng vận hành Khi SON (servo on) bật, RD bật, driver động sẵn sàng vận hành Nút dừng khẩn cấp Dùng để cài đặt chiều quay động servo Với momen điều khiển dương: RS1=0, RS2=0 không quay RS1=1, RS2=0 quay thuận RS1=0, RS2=1 quay ngược RS1=1, RS2=1 không quay Với momen điều khiển âm: RS1=0, RS2=0 không quay RS1=1, RS2=0 quay ngược RS1=0, RS2=1 quay thuận RS1=1, RS2=1 không quay CN1B Sử dụng để điều khiển momen động Với giới hạn ± 8V/momen lớn Sử dụng để giới hạn tốc độ Khi tốc độ đạt đến tốc độ giới hạn chế độ điều khiển momen ổn định Với dải điều chỉnh ± 10V 123 Hình Sơ đồ kết nối CN1A với INC Dspace 1103 Hình Sơ đồ kết nối cổng CN1B với board kết nối Driver MR-J2S-40A có số chế độ làm việc, để lựa chọn chế độ làm việc cho driver, phải cài đặt tham số tương ứng với chế độ + Chế độ điều khiển mô men: Cài đặt tham số No.0 số 0004 + Chế độ bật tín hiệu SON tự động mà khơng cần dây bên ngồi: Cài đặt tham số N0.41 số 001 + Cài đặt xung kênh A, B encoder theo driver: Cài đặt tham số No.27 số 1, đồng thời N0.54 số 1000, lúc xung driver đo lớn lần 124 số xung thực tế kênh A 3) Hệ thống cảm biến phản hồi góc xoay đối tượng Sau tín hiệu phản hồi từ Arduino DS1103, tác giả sử dụng lọc Kalman để lọc nhiễu cho tín hiệu góc phản hồi điều khiển Mơ hình lọc Kalman thiết kế sau: X ma trận trạng thái Y ma trận đầu P ma trận phương sai U ma trận tín hiệu điều khiển W ma trận nhiễu trạng thái Q ma trận nhiễu môi trường A, B, C ma trận thích nghi Z ma trận nhiễu đo R ma trận nhiễu tín hiệu cảm biến K hệ số kalman Các bước thực phép xấp xỉ Kalman Bước 1: Dự đoán trạng thái Xkp = A Xk−1 + B Uk + Wk Pkp = A Pk−1 AT + Qk Bước 2: Tính toán trạng thái xấp xỉ thời điểm Yk = C Xkm + Zk Pkp H K= H P H T + R Xk = Xkp + K [Y − H Xkp ] Bước 3: Tính tốn lại sai lệch phép xấp xỉ Pk = (I − K H) Pkp Hệ số Kalman lớn phép đo xác (sai số đo nhỏ), phép xấp xỉ thiếu ổn định, tức lọc tin tưởng kết đo nhiều kết dự đoán Điều ngược lại hệ số Kalman nhỏ dần ∗ Chọn thông số cho lọc kalman: Chu kì trích mẫu T = 7e − 4; Nhiễu môi trường phép đo độ, nhiễu tín hiệu cảm biến chọn 0.02 độ; Chọn tín hiệu cần lọc nhiễu X0 = θ, tín hiệu điều khiển U = dθ/dt; Các hệ số biến đổi A, C, H = 1; B = T ; Các nhiễu R = 0.02; Q = 2; chọn Z = W = cho đơn giản mơ hình; Chọn hệ số Kalman K = 0.01 Tín hiệu góc xoay đối tượng sau đo được, kết nối với card DS1103, tín hiệu lấy Matlab/simulink sau phản hồi lại cho 125 điều khiển minh họa Hình Hình Khối lọc Kalma phản hồi đo góc xoay đối tượng xây dựng Hình Hình Bộ phản hồi tín hiệu đo góc Hình Khối do_Goc Hình Khối Kalman 4) Card điều khiển thời gian thực dSPACE 1103 Hình ảnh card DS1103 minh họa Hình 126 Hình Card DS1103 Bộ dSPACE 1103 chứa nhiều tài nguyên để thực toán điều khiển khác Các tài nguyên dSPACE 1103 Hình gồm: Hình Tài nguyên DS1103 • 20 đầu vào tương tự ADC với chân cắm trực tiếp hộp giao diện (interface box) mở rộng thêm ADC khác Những ADC đọc giá trị từ cảm biến hệ thống điều khiển • đầu tương tự DAC với chân cắm trực tiếp hộp giao diện Đầu DAC điện áp đưa tới khuếch đại công suất để điều khiển động Trong phần thực nghiệm, tác giả sử dụng đầu DAC kết nối với board kết nối, đưa tín hiệu điều khiển tới driver điều khiển cho động • encoder gắn trực tiếp hộp giao diện để đo vị trí trục động • 32 đầu vào/ra số (Digital I/O) • PWM pha 1PWM ba pha • ngắt (External Interup) • định thời (Timer) • cổng RS232, cổng RS422 cổng CAN 5) Board kết nối Hình board mạch kết nối card DS11103 driver xây dựng 127 Hình Board giao tiếp card DS1103 driver Board giao tiếp bao gồm hai phần: Phần nguồn phần kết nối: • Phần nguồn: Có hai nguồn chiều ± 15VDC thiết kế mạch, ngồn 24VDC Nguồn ±15V để cung cấp nguồn cho mạch, nguồn 24V dùng để cấp điện cho role cho phanh động Bộ nguồn có sơ đồ ngun lý Hình 10 • Phần kết nối bao gồm bốn phần mạch tương tự để kết nối bốn driver bốn động với card DS1103, giúp điều khiển bốn khớp hệ tay máy đơi Phần kết nối có sơ đồ nguyên lý Hình 11 – Hình 14 Hình 10 Sơ đồ mạch nguồn 128 Hình 11 Sơ đồ nguyên lí phần kết nối thứ Hình 12 Sơ đồ ngun lí phần kết nối thứ hai 129 Hình 13 Sơ đồ ngun lí phần kết nối thứ ba Hình 14 Sơ đồ nguyên lí phần kết nối thứ tư 6) Mơ hình thực nghiệm tồn hệ thống Như phần trước tác giả giới thiệu hệ thống thực nghiệm mà tác giả xây dựng gồm hai phần chính: phần cứng tay máy đơi - đối tượng điều khiển DS1103 Phần tác giả trình bày kết xây dựng thực nghiệm đạt Phần cứng: Sơ đồ mạch lực, mạch điều khiển bốn động trình bày hình 15 hình 16 Hình ảnh thực nghiệm hệ tay máy đôi kết nối với driver, driver kết nối với board mạch kết nối minh họa Hình 17 130 Hình 15 Sơ đồ mạch lực bốn động Hình 16 Sơ đồ mạch điều khiển động servo 131 Hình 17 Mơ hình tay máy đơi sau đấu nối Bộ điều khiển xây dựng Matlab/simulink, sử dụng DS1103 để giao tiếp tín hiệu điều khiển phần cứng đối tượng Các thông số, giá trị đo hiển thị phần mềm Controldesk 7) Thử nghiệm hệ thống Bộ điều khiển hỗn hợp lực, vị trí chuyển động theo trục x, theo trục y hướng đối tượng xây dựng Matlab/simulink DS1103 hình 18– hình 20 Hình 18 Sơ đồ điều khiển tay máy đơi thiết kế Matlab/simulink 132 Hình 19 Sơ đồ khối quỹ đạo Hình 20 Sơ đồ khối tín hiệu phản hồi 8) Code dùng thực nghiệm 8.1 Kalman function function [X_k, P_k, K] = Kalman(X_mea, P_old, t , X_k_old) %#codegen dt = 7e-4; A = [1,0;0,1]; 133 B = [dt;0]; U = X_mea(2); if abs(U)