Luận án động lực học ngược và điều khiển chuyển động của robot song songh delta không gian

I LỜI CẢM ƠN Lời đầu tiên, tác giả xin gửi lời cảm ơn chân thành sâu sắc tới GS TSKH Nguyễn Văn Khang PGS TS Nguyễn Quang Hồng Các thầy tận tình hướng dẫn truyền cho kinh nghiệm quý báu nghiên cứu khoa học Tác giả xin gửi lời cảm ơn tới Viện Cơ học – Viện Hàn lâm Khoa học Công nghệ Việt Nam tạo điều kiện thuận lợi suốt trình làm luận án Tác giả xin cảm ơn ủng hộ bạn bè, đồng nghiệp giúp đỡ tạo điều kiện thuận lợi trình làm luận án Cuối cùng, tác giả xin chân thành cảm ơn đến gia đình động viên ủng hộ suốt trình làm luận án II LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi, kết nghiên cứu trình bày luận án trung thực, khách quan chưa công bố cơng trình khác Tơi xin cam đoan giúp đỡ cho việc thực luận án cám ơn, thơng tin trích dẫn luận án rõ nguồn gốc Hà Nội, ngày tháng năm 2018 Tác giả luận án Nguyễn Đình Dũng III MỤC LỤC LỜI CẢM ƠN I LỜI CAM ĐOAN II DANH MỤC CÁC BẢNG VIII DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ VIII MỞ ĐẦU Chương TỔNG QUAN VỀ BÀI TOÁN ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN ROBOT SONG SONG 1.1 Robot có cấu trúc song song 1.2 So sánh robot nối tiếp robot song song 1.3 Giới thiệu hai robot song song Delta không gian 3RUS 3PUS chế tạo 1.4 Ứng dụng robot song song 1.4.1 Ứng dụng công nghiệp 1.4.2 Ứng dụng mô 1.4.3 Ứng dụng y học 10 1.4.4 Các ứng dụng khác 11 1.5 Một số nghiên cứu động lực học điều khiển robot song song nước 12 1.5.1 Động lực học robot song song 12 1.5.2 Điều khiển bám quỹ đạo robot song song 14 1.6 Các nghiên cứu Việt Nam 15 1.7 Xác định vấn đề cần nghiên cứu luận án 17 Kết luận chương 18 Chương XÂY DỰNG MƠ HÌNH CƠ HỌC VÀ MƠ HÌNH TỐN HỌC CHO ROBOT SONG SONG DELTA KHÔNG GIAN 19 2.1 Mơ hình động học robot song song Delta không gian 19 2.1.1 Mơ hình động học robot song song Delta không gian 3RUS 20 2.1.2 Mơ hình động học robot song song Delta không gian 3PUS 22 2.2 Mô hình động lực robot song song Delta khơng gian 25 2.2.1 Mơ hình động lực robot song song Delta không gian 3RUS 25 2.2.2 Mơ hình động lực robot song song Delta không gian 3PUS 26 2.3 Dạng ma trận phương trình Lagrange dạng nhân tử [51] 26 IV 2.4 Thiết lập phương trình chuyển động robot song song Delta khơng gian 3RUS 28 2.4.1 Thiết lập phương trình chuyển động cho mơ hình robot 3RUS 28 2.4.2 Thiết lập phương trình chuyển động cho mơ hình robot 3RUS 38 2.5 Thiết lập phương trình chuyển động robot song song Delta không gian 3PUS 43 2.5.1 Thiết lập phương trình chuyển động mơ hình robot 3PUS 43 2.5.2 Thiết lập phương trình chuyển động mơ hình robot 3PUS 50 2.6 So sánh phương trình chuyển động mơ hình robot 53 Kết luận chương 53 Chương MÔ PHỎNG SỐ ĐỘNG HỌC NGƯỢC VÀ ĐỘNG LỰC HỌC NGƯỢC ROBOT SONG SONG DELTA KHÔNG GIAN 54 3.1 Tính tốn động học ngược robot song song phương pháp Newton – Raphson cải tiến 54 3.1.1 Thiết lập công thức tính vận tốc gia tốc suy rộng 54 3.1.2 Các công thức xác định véc tơ tọa độ suy rộng q t  55 3.1.3 Thuật toán hiệu chỉnh độ xác véc tơ tọa độ suy rộng q t  bước tính 56 3.1.4 Đánh giá sai số 57 3.2 Phương pháp số giải toán động lực học ngược robot song song 58 3.2.1 Bài toán động lực học ngược 58 3.2.2 Giải toán động lực học ngược phương pháp khử nhân tử Lagrange [4] 59 3.3 Mơ số tốn động học ngược robot song song Delta không gian 61 3.3.1 Mơ số tốn động học ngược robot 3RUS 61 3.3.2 Mô số toán động học ngược robot Delta 3PUS 66 3.4 Mô số toán động lực học ngược robot song song Delta khơng gian 68 3.4.1 Mơ số tốn động lực học ngược robot Delta 3RUS 68 3.4.2 Mơ số tốn động lực học ngược robot Delta không gian 3PUS 72 Kết luận chương 73 Chương ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT SONG SONG DELTA KHƠNG GIAN DỰA TRÊN CÁC MƠ HÌNH CƠ HỌC 74 4.1 Tổng quan điều khiển bám quỹ đạo khâu thao tác 74 4.1.1 Giới thiệu chung 74 4.1.2 Bài toán điều khiển không gian khớp 74 4.1.3 Bài tốn điều khiển khơng gian thao tác 75 V 4.2 Điều khiển bám quỹ đạo robot song song khơng gian khớp dựa phương trình Lagrange dạng nhân tử 75 4.2.1 Cơ sở động lực học hệ nhiều vật có cấu trúc mạch vịng 75 4.2.2 Cơ sở lý thuyết xây dựng thuật toán điều khiển 78 4.3 Mô số luật điều khiển cho robot song song Delta không gian dựa mơ hình động lực 89 4.3.1 Sơ đồ mô số phương pháp điều khiển 89 4.3.2 Mô số phương pháp điều khiển robot song song Delta không gian 3RUS 90 4.3.3 Mô số phương pháp điều khiển robot song song Delta không gian 3PUS 98 Kết luận chương 106 KẾT LUẬN 107 DANH MỤC CƠNG TRÌNH CỦA TÁC GIẢ 110 TÀI LIỆU THAM KHẢO 111 PHỤ LỤC 119 VI DANH MỤC CÁC KÝ HIỆU M Ma trận khối lượng C Ma trận quán tính ly tâm Coriolis Φs Ma trận Jacobi g Véc tơ gia tốc trọng trường s Véc tơ tọa độ suy rộng dư s s Véc tơ vận tốc suy rộng dư q Véc tơ tọa độ khớp q Véc tơ vận tốc khớp  q Véc tơ gia tốc khớp qa Véc tơ tọa độ suy rộng khớp chủ động (active joints) q a Véc tơ vận tốc suy rộng khớp chủ động a q Véc tơ gia tốc suy rộng khớp chủ động qp Véc tơ tọa độ suy rộng khớp bị động (passive joints) q p Véc tơ vận tốc suy rộng khớp bị động  p q Véc tơ gia tốc suy rộng khớp bị động x Véc tơ tọa độ suy rộng khâu thao tác x Véc tơ vận tốc suy rộng khâu thao tác x Véc tơ gia tốc suy rộng khâu thao tác z Véc tơ tọa độ suy rộng phụ thuộc dư f Véc tơ phương trình liên kết τa Véc tơ mô men/ lực dẫn động λ Véc tơ nhân tử Lagrange u Véc tơ lực điều khiển R Bán kính bàn máy cố định r Bán kính bàn máy di động L1 Chiều dài khâu chủ động L2 Chiều dài khâu bị động I Ma trận ten xơ quán tính khối hệ quy chiếu gắn với khối tâm Véc tơ gia tốc suy rộng dư VII JT Ma trận Jacobi tịnh tiến JR Ma trận Jacobi quay ω Véc tơ vận tốc góc khâu T Động hệ  Thế hệ V Hàm Lyapunov ea Véc tơ sai số vị trí khớp chủ động ex Sai số vị trí khâu thao tác theo phương x ey Sai số vị trí khâu thao tác theo phương y ez Sai số vị trí khâu thao tác theo phương z DANH MỤC CÁC CHỮ VIẾT TẮT RUS Revolute-Universal-Spherical PUS Presmatic - Universal-Spherical PD Proportional - Derivate PID Proportional - Integral – Derivate RBF Radial Basic Functions VIII DANH MỤC CÁC BẢNG Bảng 1.1: So sánh robot chuỗi robot song song Bảng 1.2: Tham số robot Delta 3RUS Bảng 1.3: Bảng tham số robot 3PUS Bảng 1.4: Thông số kỹ thuật động bước hai robot 3RUS 3PUS Bảng 2.1: So sánh phương trình chuyển động mơ hình 53 Bảng 3.1: Các tham số robot theo tài liệu [61] 64 Bảng 4.1: Bảng so sánh sai số bám quỹ đạo robot 91 Bảng 4.2: Bảng so sánh lực điều khiển robot 95 Bảng 4.3: Bảng so sánh sai số bám quỹ đạo robot 99 Bảng 4.4: Bảng so sánh mô men điều khiển 103 DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ Hình 1.1 Cấu trúc robot song song Hình 1.2: Robot Puma [67] Hình 1.3: Robot thực 3RUS chế tạo Hình 1.4: Robot Delta 3PUS chế tạo Hình 1.5: Sơ đồ điều khiển robot Hình 1.6: Cơ cấu song song Gough [67] Hình 1.7: Robot Delta ứng dụng công nghệ thực phẩm [29] Hình 1.8: Cơ cấu song song Stewart [67] Hình 1.9: Sản phẩm Persival École National d'Elquitation (Pháp) [67] Hình 1.10: Bộ mô xe đạp Viện KAIST sản phẩm Caren Motek [67] Hình 1.11: Sản phẩm SuriScope vận hành, Đại học Humboldt (Berlin, Đức) [43] 10 Hình 1.12: Robot CRIGOS dùng để phẫu thuật tái tạo xương 10 Hình 1.13: Robot song song kiểu Stewart [67] 11 Hình 1.14: Robot Delta [29] 11 Hình 1.15: Cấu trúc chấp hành song song 3RPS [96] 12 Hình 2.1: Robot song song Delta khơng gian 3RUS 20 Hình 2.2: Mơ hình động học robot Delta 3RUS 21 Hình 2.3: Tọa độ suy rộng định vị 21 Hình 2.4: Mơ hình thiết kế robot Delta 3PUS 23 Hình 2.5: Mơ hình động học robot 3PUS 24 Hình 2.6: Định vị khâu BiDi 24 Hình 2.7: Mơ hình robot 3RUS 25 Hình 2.8: Mơ hình robot 3RUS 25 Hình 2.9: Mơ hình robot 3PUS 26 Hình 2.10: Mơ hình robot 3PUS 26 Hình 3.1: Sơ đồ khối toán động lực học ngược 58 Hình 3.2: Robot Delta 3RUS chế tạo Đại học Kinh doanh Cơng nghệ Hà Nội 61 Hình 3.3 Tọa độ khớp chủ động 1 ,  ,  62 Hình 3.4 Tọa độ khớp bị động ,  ,  62 Hình 3.5: Tọa độ khớp bị động  ,  ,  62 IX Hình 3.6: Vận tốc góc khâu chủ động 63 Hình 3.7: Gia tốc góc khâu chủ động 63 Hình 3.8: Sai số phương trình liên kết 1, 2, 63 Hình 3.9: Sai số phương trình liên kết 4, 5, 63 Hình 3.10: Sai số phương trình liên kết 7, 8, 64 Hình 3.11: So sánh kết toán động học ngược với tài liệu [61] 65 Hình 3.13: Tọa độ suy rộng khớp chủ động 66 Hình 3.12: Robot thực chế tạo 3PUS 66 Hình 3.14: Tọa độ suy rộng khớp bị động 66 Hình 3.15: Tọa độ suy rộng khớp bị động 67 Hình 3.16: Vận tốc suy rộng khớp chủ động 67 Hình 3.17: Gia tốc suy rộng khớp chủ động 67 Hình 3.18: Đồ thị sai số phương trình liên kết 1, 2, 67 Hình 3.19: Kết mơ robot 3RUS 69 Hình 3.20: So sánh kết luận án với cơng trình [61] 70 Hình 3.21: So sánh kết luận án với cơng trình [92] 71 Hình 3.22: Kết mô số động lực học ngược robot 3PUS 72 Hình 4.1: Sơ đồ điều khiển không gian khớp 75 Hình 4.2: Sơ đồ điều khiển không gian thao tác 75 Hình 4.3: Mạng nơ ron RBF 86 Hình 4.4: Sơ đồ điều khiển robot 89 Hình 4.5: Sai số vị trí khớp chủ động 91 Hình 4.6: Sai số vị trí khớp chủ động 91 Hình 4.7: Sai số vị trí bàn máy động 91 Hình 4.8: Sai số vị trí bàn máy động 91 Hình 4.9: Sai số vị trí khớp chủ động 91 Hình 4.10: Sai số vị trí khớp chủ động 91 Hình 4.11: Sai số vị trí bàn máy động 91 Hình 4.12: Sai số vị trí bàn máy động 91 Hình 4.13: Sai số vị trí khớp chủ động 92 Hình 4.14: Sai số vị trí khớp chủ động 92 Hình 4.15: Sai số vị trí bàn máy động 92 Hình 4.16: Sai số vị trí bàn máy động 92 Hình 4.17: Sai số vị trí khớp chủ động 92 Hình 4.18: Sai số vị trí khớp chủ động 92 Hình 4.19: Sai số vị trí bàn máy động 92 Hình 4.20: Sai số vị trí bàn máy động 92 Hình 4.21: Sai số vị trí khớp chủ động 93 Hình 4.22: Sai số vị trí khớp chủ động 93 Hình 4.23: Sai số vị trí bàn máy động 93 Hình 4.24: Sai số vị trí bàn máy động 93 Hình 4.25: Sai số vị trí khớp chủ động 93 Hình 4.26: Sai số vị trí khớp chủ động 93 X Hình 4.27: Sai số vị trí bàn máy động 93 Hình 4.28: Sai số vị trí bàn máy động 93 Hình 4.29: Sai số vị trí khớp chủ động 94 Hình 4.30: Sai số vị trí khớp chủ động 94 Hình 4.31: Sai số vị trí bàn máy động 94 Hình 4.32: Sai số vị trí bàn máy động 94 Hình 4.33: Sai số vị trí khớp chủ động 94 Hình 4.34: Sai số vị trí khớp chủ động 94 Hình 4.35: Sai số vị trí bàn máy động 94 Hình 4.36: Sai số vị trí bàn máy động 94 Hình 4.37: Sai số vị trí khớp chủ động 99 Hình 4.38: Sai số vị trí khớp chủ động 99 Hình 4.39: Sai số vị trí bàn máy động 99 Hình 4.40: Sai số vị trí bàn máy động 99 Hình 4.41: Sai số vị trí khớp chủ động 99 Hình 4.42: Sai số vị trí khớp chủ động 99 Hình 4.43: Sai số vị trí bàn máy động 99 Hình 4.44: Sai số vị trí bàn máy động 99 Hình 4.45: Sai số vị trí khớp chủ động 100 Hình 4.46: Sai số vị trí khớp chủ động 100 Hình 4.47: Sai số vị trí bàn máy động 100 Hình 4.48: Sai số vị trí bàn máy động 100 Hình 4.49: Sai số vị trí khớp chủ động 100 Hình 4.50: Sai số vị trí khớp chủ động 100 Hình 4.51: Sai số vị trí bàn máy động 100 Hình 4.52: Sai số vị trí bàn máy động 100 Hình 4.53: Sai số vị trí khớp chủ động 101 Hình 4.54: Sai số vị trí khớp chủ động 101 Hình 4.55: Sai số vị trí bàn máy động 101 Hình 4.56: Sai số vị trí bàn máy động 101 Hình 4.57: Sai số vị trí khớp chủ động 101 Hình 4.58: Sai số vị trí khớp chủ động 101 Hình 4.59: Sai số vị trí bàn máy động 101 Hình 4.60: Sai số vị trí bàn máy động 101 Hình 4.61: Sai số vị trí khớp chủ động 102 Hình 4.62: Sai số vị trí khớp chủ động 102 Hình 4.63: Sai số vị trí bàn máy động 102 Hình 4.64: Sai số vị trí bàn máy động 102 Hình 4.65: Sai số vị trí khớp chủ động 102 Hình 4.66: Sai số vị trí khớp chủ động 102 Hình 4.67: Sai số vị trí bàn máy động 102 Hình 4.68: Sai số vị trí bàn máy động 102 136 t=0.01*(i-1); tg(i)=t; if i==1 q(i,:)=q0; else q(i,:)=q(i-1,:)+dq(i-1,:)*delta_t+1/2*ddq(i1,:)*(delta_t)^2; end % Vec to vi tri cua P % Dau vao x(i)=0.3*cos((2*pi/tf)*t); y(i)=0.3*sin((2*pi/tf)*t); z(i)=-0.7; vitri(:,i)=[x(i);y(i);z(i)]; % Vec to van toc cua P dx(i)=-(0.6*pi/tf)*sin((2*pi/tf)*t); dy(i)=(0.6*pi/tf)*cos((2*pi/tf)*t);dz(i)=0; vantoc(:,i)=[dx(i);dy(i);dz(i)]; % Vec to gia toc ddx(i)=-(1.2*pi*pi/(tf*tf))*cos((2*pi/tf)*t); ddy(i)=(1.2*pi*pi/(tf*tf))*sin((2*pi/tf)*t); ddz(i)=0; giatoc(:,i)=[ddx(i);ddy(i);ddz(i)]; % Giai bai toan dong hoc nguoc delta_q(i,:)=[1,1,1]; Max=0; buoclap(i)=0; while(((sqrt(delta_q(i,1)*delta_q(i,1)')>=epsilon)|(sqrt(delta_q(i,2 )*delta_q(i,2)')>=epsilon)|(sqrt(delta_q(i,3)*delta_q(i,3)')>=epsilon))&& (Max