Nghiên cứu giải bài toán động học ngược bằng phương pháp tách nhóm 3 khâu và phương pháp số

ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA HHÕÕÕII TRẦN THỊ LIÊN NGHIÊN CỨU GIẢI BÀI TOÁN ĐỘNG HỌC NGƯC BẰNG PHƯƠNG PHÁP TÁCH NHÓM KHÂU VÀ PHƯƠNG PHÁP SỐ Chuyên ngành : CƠ KHÍ CHẾ TẠO MÁY Mã số ngành : 2.01.00 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH - THÁNG 09 NĂM 2006 ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA HHÕÕÕII TRẦN THỊ LIÊN NGHIÊN CỨU GIẢI BÀI TOÁN ĐỘNG HỌC NGƯC BẰNG PHƯƠNG PHÁP TÁCH NHÓM KHÂU VÀ PHƯƠNG PHÁP SỐ Chuyên ngành : CƠ KHÍ CHẾ TẠO MÁY Mã số ngành : 2.01.00 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH - THÁNG 09 NĂM 2006 CÔNG TRÌNH ĐƯC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH -[\ - Cán hướng dẫn khoa học: PGS TS LÊ HOÀI QUỐC Cán chấm nhận xét 1: TS BÙI TRỌNG HIẾU Cán chấm nhận xét 2: TS TRẦN THIÊN PHÚC Luận văn thạc só bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA Ngày 19 tháng năm 2006 TRƯỜNG ĐẠI HỌC BÁCH KHOA PHÒNG ĐÀO TẠO SAU ĐH CỘÄNG HÒA Xà HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP – TỰ DO – HẠNH PHÚC NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên : Trần Thị Liên Ngày tháng năm sinh : 18 tháng năm 1965 Chuyên ngành : Chế tạo máy Phái : nữ Nơi sinh : Hà nội Mã số học viên : : 2.01.00 I- TÊN ĐỀ TÀI : Nghiên cứu giải toán động học ngược phương pháp tách nhóm khâu phương pháp số II- NHIÊM VỤ VÀ NỘI DUNG : Xây dựng giải thuật giải toán động học ngược tay máy – Bài toán vị trítheo phương pháp tách nhóm ba khâu phương pháp số III- NGÀY GIAO NHIỆM VỤ : 7-2005 IV- NGÀY HOÀN THÀNH NHIỆM VỤ : 19-9-2006 V- CÁN BỘ HƯỚNG DẪN : PGS.TS LÊ HOÀI QUỐC CÁN BỘ HƯỚNG DẪN CN BỘ MÔN QL CHUYÊN NGÀNH Nội dung đề cương Luận văn thạc só Hội đồng chuyên ngành thông qua Ngày TRƯỞNG PHÒNG ĐT- SĐH tháng năm TRƯỞNG KHOA QL NGÀNH LỜI CẢM ƠN Trong thực tập luận án này, nhận giúp đỡ tận tình Quý thầy cô bạn đồng nghiệp Hôm nay, luận án hoàn thành, xin chân thành cám ơn : - Thầy hướng dẫn khoa học : PGS TS Lê Hoài Quốc - Thầy chấm nhận xét : TS Bùi Trọng Hiếu - Thầy chấm nhận xét : TS Trần Thiên Phúc dành thời gian quý báu để duyệt cho ý kiến giúp hoàn thiện luận án - Quý thầy cô giảng dạy môn học khóa Cao học K14 nhiệt tình bảo cho kiến thức hữu dụng cho việc xây dựng luận án mà cho công tác sau - Đồng nghiệp KS Lê Đình Phong Trần Đức Trọng hết lòng hỗ trợ để hoàn thành nhiệm vụ - Quý Thầy Cô Chủ tịch, Phản biện y viên Hội đồng dành nhiều thời gian quý báu để nhận xét tham gia Hội đồng chấm Luận án TÓM TẮT LUẬN VĂN Các nội dung nghiên cứu luận văn bao gồm: • Xây dựng giải thuật giải toán động học ngược vị trí loại tay máy bậc tự phương pháp tách nhóm ba khâu Đó loại tay máy với ba khâu định vị là: tọa độ Đề (x, y, z), tọa độ trụ (α, d2, d3), tọa độ cầu (β, γ, d3), tay máy dạng SCARA (ω1, ω2, d3), tay máy toàn khớp lề (θ1, θ3, θ3) Ba khâu định hướng RPY Sử dụng phần mềm Matlab để giải toán động học ngược vị trí tay máy tọa độ Cartesian tay máy tọa độ Cylindrical bậc tự • Xây dựng giải thuật giải toán động học ngược vị trí loại tay máy bậc tự phương pháp bình phương bé MỤC LỤC Tóm tắt luận văn …………………………………………………………………………………………………………………………5 Mục lục …………………………………………………………………………………………………………………………………………….6 Danh mục hình ảnh ………………………………………………………………………………………………………… Chương 1: TỔNG QUAN VỀ NGƯỜI MÁY, TAY MÁY………………………………………11 1.1 Giới thiệu người máy - tay máy……………………………………………………………………….12 1.1.1 Sơ lược trình hình thành phát triển robot ………………………….12 1.1.2 Robot công nghiệp (Industrial Robot – IR) ………………………………………… 14 1.1.3 Tình hình tiếp cận ứng dụng Robot công nhgiệp Việt Nam……15 1.1.4 Kết cấu robot công nghiệp …………………………………………………… 18 1.2 Giải thuật giải toán động học thuận tay máy…………………………………………….21 1.2.1 Mô tả vật rắn không gian ……………………………………………………………… 22 1.2.2 Phân tích động học tay máy phương pháp lượng giác…………….23 1.2.3 Phân tích động học tay máy phương pháp ma trận …………………24 1.2.4 Mô tả chuyển động tay máy phương pháp tọa độ ……………………………………………………………………………………………………………………….28 1.2.4.1 Giới thiệu phương pháp tọa độ (homogeneous coordinate) ………………………………………………………………………………………………….28 1.2.4.2 Mô tả quy tắc Denavit – Hartenberg ………………………………………… 33 1.2.4.3 p dụng phương pháp tọa độ để giải toán động học thuận ………………………………………………………………………………………35 Chương 2: BÀI TOÁN ĐỘNG HỌC NGƯC TAY MÁY (bài toán vị trí) MỤC TIÊU CỦA LUẬN VĂN………………………………………………………………………36 2.1 Phương pháp lượng giác ……………………………………………………………………………………………38 2.2 Phương pháp biến đổi ngược ma trận 4x4 ……………………………41 2.3 Mục tiêu luận văn…………………………………………………………………………………………………47 Chương 3: PHƯƠNG PHÁP TÁCH NHÓM BA KHÂU……………………………………… 48 3.1 Giải thuật giải toán động học ngược tay máy phương pháp tách nhóm khâu…………………………………………………………………………………………………………………….49 3.1.1 Trường hợp tay máy Cartesian (3T3R)…………………………………………….57 3.1.2 Trường hợp tay máy Tọa độ trụ (Cylindrical – R2T3R)…………….58 3.1.3 Trường hợp tay máy Tọa độ cầu (Spherical – T2R)…………………….59 3.1.4 Trường hợp tay máy SCARA – 2RT)…………………………………………………60 3.1.5 Trường hợp tay máy toàn khớp lề (6R)…………………………………….61 3.2 Ví dụ minh họa 3.2.1 Xét tay máy Cartesian …………………………………………… 65 3.2.2Xét tay máy Cylindrical ……………………………………… …73 Chương 4: PHƯƠNG PHÁP SỐ …………………………………………………………………………………………81 4.1 Giới thiệu phương pháp số …………………………………………………………………………………82 4.1.1 Phương pháp chia đôi…………………………………………………………………………………….82 4.1.2 Phương pháp lặp……………………………………………………………………………………………….84 4.13 Phương pháp Niutơn (tiếp tuyến)……………………………………………………………….87 4.1.4 Phương pháp Gaoxơ……………………………………………………………………………………….90 4.1.5 Phương pháp bình phương bé nhất……………………………………………………………95 4.2 p dụng phương pháp bình phương bé giải toán động học ngược vị trí tay máy…………………………………………………………………………………………………………………95 Chương 5: KẾT LUẬN …………………………………………………………………………………………………………98 Phụ lục A ………………………………………………………………………………………………………………………………… 101 Phụ lục B ……………………………………………………………………………………………………………………………………108 Tài liệu tham khảo ……………………………………………………………………………………………………………… 115 DANH MỤC CÁC HÌNH ẢNH Hình 1.1 Xe tự hành thám hiểm mặt trăng Lukonohod Hình 1.2 Robot giải trí (chú chó AIBO hãng Sony) Hình 1.3 Một số mẫu robot sơn - hàn Kuka Hình 1.4 Một số mẫu robot công ngiệp Hình 1.5 Các phận cấu thành robot Hình 1.6 Vị trí hướng vật rắn không gian Hình 1.7 Tay máy hai khâu phẳng Hình 1.8 Chuyển đổi hệ tọa độ tịnh tiến Hình 1.9 Chuyển đổi hệ tọa độ quay Hình 1.10 Quay quanh trục z Hình 1.11 Quay quanh trục y Hình 1.12 Quay quanh trục x Hình 1.13 Sự hình thành góc Euler ZYZ Hình 1.14 Sự hình thành góc RPY Hình 1.15 Biểu diễn thông số động học theo quy tắc Denavit - Hartenberg Hình 2.1 The Planar 3-R manipulator Hình 2.2 Hai vị trí đáp ứng tay máy Hình 2.3 Các vị trí đáp ứng khớp B Hình 2.4 Tay máy bậc tự toàn khớp lề Hình 3.1 Hai vị trí đáp ứng Tay máy khâu Hình 3.2 Minh họa vị trí hướng đối tượng công tác so với hệ quy chiếu cố định gắn với giá tay máy Hình 3.3 Sơ đồ thể hướng đối tượng công tác so với vị trí ban đầu Tay máy Hình 3.4a Tay máy Cartesian Hình3.4b Minh họa hướng tiếp cận đối tượng công tác tay máy Cartesian Hình 3.5a Tay máy Cylindrical Hình 3.5b Minh họa hướng tiếp cận đối tượng công tác tay máy Cylindrical Hình 3.6a Tay máy Spherical Hình 3.6b Minh họa hướng tiếp cận đối tượng công tác tay máy Spherical Hình 3.7a Tay máy SCARA Hình 3.7b Minh họa hướng tiếp cận đối tượng công tác tay máy SCARA Hình 3.8a Tay máy Articulated Hình 3.8b Minh họa hướng tiếp cận đối tượng công tác tay Máy Articulated Hình 3.9 Không gian hoạt động tay máy Cartesian Hình 3.10 Không gian hoạt động tay máy Cylindrical Hình 3.11 Không gian hoạt động tay máy Spherical Hình 3.12 Không gian hoạt động tay máy SCARA Hình 3.13 Tọa độ tay máy SCARA Hình 3.14 Không gian hoạt động tay máy Articulated Hình 3.15 Lược đồ cấu Tay máy PUMA bậc tự liên kết toàn khớp lề Hình 3.16 Minh họa hướng tiếp cận đối tượng công tác Tay Máy Hình 3.17 Minh họa hướng tiếp cận Tay máy đến đối tượng công tác Hình 3.18 Lược đồ cấu Tay máy CARTESIAN bậc tự Hình 3.19 Các hệ tọa độ khâu định vị Hình 3.20 Các hệ tọa độ khâu định hướng Hình 3.21 Lược đồ cấu Tay máy CYLINDRICAL bậc tự Hình 3.22 Các hệ tọa độ khâu định vị Phụ lục A4=subs(A,{delta,d,a,alfa},{q4t,0,L4,-pi/2}) A5=subs(A,{delta,d,a,alfa},{q5t+pi/2,L5,0,pi/2}) A6=subs(A,{delta,d,a,alfa},{q6t,L6,0,0}) TRPY=A4*A5*A6 3/- Xac dinh ma tran chuyen doi tong the : -M = T3*TRPY 4/- Xac dinh Toa diem cong tac M : M_end = subs(M,{q4t,q5t,q6t, d1,d2,d3}, {pi/12,pi/10,pi/8, 0.8,0.8,0.8}) II- GIAI BAI TOAN NGUOC 1/- XAC DINH GOC RPY a)- Tiep can theo phuong X q4m = sym(pi/12) q5m = sym(pi/10) 102 Phuï luïc q6m = sym(pi/8) A = sin(q4m)*sin(q5m)*cos(q6m)-cos(q4m)*sin(q6m) B = -cos(q4m)*sin(q5m)*cos(q6m)-sin(q4m)*sin(q6m) C = -cos(q5m)*cos(q6m) eq1 = -cos(q4)*sin(q5)*cos(q6)-sin(q4)*sin(q6) - A eq2 = sin(q4)*sin(q5)*cos(q6)+cos(q4)*sin(q6) - B eq3 = -cos(q5)*cos(q6) - C [q4nx,q5nx,q6nx] = solve(eq1,eq2,eq3) b)- Tiep can theo phuong Y G = -sin(q4m)*sin(q5m)*sin(q6m)-cos(q4m)*cos(q6m) H = cos(q4m)*sin(q5m)*sin(q6m)-sin(q4m)*cos(q6m) K = cos(q5m)*sin(q6m) eq4 = cos(q4)*sin(q5)*sin(q6)-sin(q6)*cos(q4) - G eq5 = sin(q4)*sin(q5)*sin(q6)+cos(q4)*cos(q6) - H eq6 = cos(q5)*sin(q6) - K [q4ny,q5ny,q6ny] = solve(eq4,eq5,eq6) -c)-Tiep can theo phuong Z -P = -sin(q4m)*cos(q5m) 103 Phuï luïc Q = cos(q4m)*cos(q5m) R = -sin(q5m) eq7 = cos(q4)*cos(q5) - P eq8 = sin(q4)*cos(q5) - Q eq9 = -sin(q5) - R [q4nz,q5nz] = solve(eq7,eq8,eq9) 2/ - XAC DINH DIEM F : Af_1=inv(A6) F = M_end*Af_1 a)- Tiep can theo phuong x : TD_Fx=subs(F,{q6t},{q6nx(1,1)}) b)- Tiep can theo phuong y : TD_Fy=subs(F,{q6t},{q6ny(1,1)}) -3/-XAC DINH DIEM E: Ae_1=inv(A5) E = F*Ae_1 a)- Tiep can theo phuong x : TD_Ex=subs(E,{q5t,q6t},{q5nx(1,1),q6nx(1,1)}) b)- Tiep can theo phuong y : 104 Phuï luïc TD_Ey=subs(E,{q5t,q6t},{q5ny(1,1),q6ny(1,1)}) -4/- XAC DINH DIEM D : aRPY_1=inv(TRPY) D=M_end*aRPY_1 a)- Tiep can theo phuong x : TD_Dx=subs(D,{q4t,q5t,q6t},{q4nx(1,1),q5nx(1,1),q6nx(1,1)}) b)- Tiep can theo phuong y : TD_Dy=subs(D,{q4t,q5t,q6t},{q4ny(1,1),q5ny(1,1),q6ny(1,1)}) 5/- XAC DINH CAC BIEN CUA KHAU DINH VI : a)-Tiep can theo phuong x : d1x = TD_Dx(1,4) d2x = TD_Dx(2,4) d3x = TD_Dx(3,4) b)-Tiep can theo phuong y : d1y = TD_Dy(1,4) d2y = TD_Dy(2,4) d3y = TD_Dy(3,4) 6/- DIEU KIEN BIEN CUA CAC KHAU 1, 2, 105 Phuï luïc d1_min= 0.2; d1_max= 0.8; d2_min= 0.2; d2_max= 1.2; d3_min= 0.2; d3_max= 1.2; [n,m]=size(d1x)%gan so nghiem cho bien n for i=1:n if (eval(d1x(i,1)) >= d1_min) & (eval(d1x(i,1)) = d2_min & eval(d2x(i,1)) = d3_min & eval(d3x(i,1)) = d1_min) & (eval(d1y(i,1)) = d2_min & eval(d2y(i,1)) = d3_min & eval(d3y(i,1)) = q1_min) & (eval(q1x(i,1)) = d2_min & eval(d2x(i,1)) = d3_min & eval(d3x(i,1)) = q1_min) & (eval(q1y(i,1)) = d2_min & eval(d2y(i,1)) = d3_min & eval(d3y(i,1))