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

BỘ GIÁO DỤC C VÀ ĐÀO T TẠO VIỆN N HÀN LÂM KHOA HỌC H VÀ CÔNG NGHỆ VIỆT VI NAM HỌ ỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ - NGUYỄN ĐÌNH DŨNG ĐỘNG LỰC HỌC C NGƯỢC NGƯ VÀ ĐIỀU KHIỂN N CHUYỂN CHUY ĐỘNG CỦA A ROBOT SONG SONG DELTA KHÔNG GIAN LUẬN N ÁN TIẾN TI SĨ KỸ THUẬT CƠ KHÍ VÀ CƠ KỸ Ỹ THUẬT Hà Nội – 2018 BỘ GIÁO DỤC VÀ ĐÀO TẠO VIỆN HÀN LÂM KHOA HỌC VÀ CÔNG NGHỆ VIỆT NAM HỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ - NGUYỄN ĐÌNH DŨNG ĐỘNG LỰC HỌC NGƯỢC VÀ ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA ROBOT SONG SONG DELTA KHÔNG GIAN Chuyên ngành: Cơ kỹ thuật Mã số: 52 01 01 LUẬN ÁN TIẾN SĨ KỸ THUẬT CƠ KHÍ VÀ CƠ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: GS TSKH Nguyễn Văn Khang PGS TS Nguyễn Quang Hoàng Hà Nội – 2018 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 Hoàng Các thầy tận tình hướng dẫn truyền cho tơi 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ố tố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 tố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 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