Giải bài toán động học ngược robot dạng chuỗi dư dẫn động có chú ý đến yếu tố động lực học

THÁI PHƯƠNG THẢO BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - THÁI PHƯƠNG THẢO CƠ ĐIỆN TỬ GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT DẠNG CHUỖI DƯ DẪN ĐỘNG CÓ CHÚ Ý ĐẾN YÊU TỐ ĐỘNG LỰC HỌC LUẬN VĂN THẠC SĨ KHOA HỌC CƠ ĐIỆN TỬ 2010B Hà Nội – 2012 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI THÁI PHƯƠNG THẢO GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT DẠNG CHUỖI DƯ DẪN ĐỘNG CÓ CHÚ Ý ĐẾN YẾU TỐ ĐỘNG LỰC HỌC Chuyên ngành : Cơ Điện tử LUẬN VĂN THẠC SĨ KHOA HỌC CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC : TS NGUYỄN QUANG HOÀNG Hà Nội – 2012 LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi, chƣa đƣợc cơng bố cơng trình khác Các số liệu, kết nêu luận án trung thực Tác giả luận án Thái Phƣơng Thảo i MỤC LỤC Trang LỜI CAM ĐOAN i MỤC LỤC ii DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT THƢỜNG DÙNG v DANH MỤC BẢNG vii DANH MỤC HÌNH VẼ .viii LỜI MỞ ĐẦU Chƣơng I TỔNG QUAN 1.1 Tổng quan robot công nghiệp 1.2 Một số vấn đề đặt nghiên cứu robot công nghiệp 1.3 Nhiệm vụ cần giải luận văn Chƣơng II CƠ SỞ LÝ THUYẾT VỀ ĐỘNG HỌC THUẬN VÀ ĐỘNG LỰC HỌC ROBOT CÔNG NGHIỆP 2.1 Phân tích động học thuận robot cơng nghiệp 2.1.1 Giải toán động học thuận robot công nghiệp phƣơng pháp ma trận Denavit – Hartenberg 2.1.2 Xác định vận tốc, gia tốc điểm tác động cuối vận tốc góc, gia tốc góc khâu robot phƣơng pháp trực tiếp 12 2.1.3 Xác định vận tốc, gia tốc điểm thuộc vật rắn 13 2.2 Động lực học robot công nghiệp 15 2.2.1 Biểu thức động robot 15 2.2.2 Thiết lập dạng thức Lagrange loại 18 2.2.3 Công suất công sinh robot thực chuyển động 23 ii 2.3 Thiết lập phƣơng trình động học thuận phƣơng trình vi phân chuyển động cho số robot công nghiệp 23 2.3.1 Robot phẳng bậc tự 23 2.3.2 Robot hàn AII-V6 bậc tự 30 2.3.3 Robot bậc tự RRRRRRR 41 Chƣơng III LẬP TRÌNH QUỸ ĐẠO 52 3.1 Thiết lập toán 52 3.1.1 Khái niệm lập trình quỹ đạo 52 3.1.2 Nhiệm vụ lập trình quỹ đạo robot 53 3.1.3 Một vài tiêu chuẩn điều kiện phụ 53 3.2 Lập trình quỹ đạo động học 54 3.2.1 Mô tả đƣờng cong không gian 55 3.2.2 Một vài đƣờng cong không gian đơn giản 56 3.3 Việc xây dựng quỹ đạo tổng quát đa thức nội suy 60 Chƣơng IV CÁC PHƢƠNG PHÁP GIẢI BÀI TOÁN ĐỘNG HỌC NGƢỢC ROBOT DƢ DẪN ĐỘNG 70 4.1 Thiết lập toán 70 4.2 Ma trận tựa nghịch đảo nghiệm phƣơng trình Ax  b 71 4.2.1 Ma trận tựa nghịch đảo 71 4.2.2 Tìm nghiệm hệ phƣơng trình đại số tuyến tính phƣơng pháp nhân tử Lagrange 73 4.2.3 Nghiệm tổng qt hệ phƣơng trình đại số tuyến tính 74 4.3 Các phƣơng pháp giải toán động học ngƣợc 74 4.3.1 Giải tốn mức độ vị trí 74 iii 4.3.2 Giải toán mức độ vận tốc 75 4.3.3 Giải toán mức độ gia tốc 87 4.4 Các phƣơng pháp hiệu chỉnh toán ngƣợc 91 4.4.1 Phƣơng pháp hiệu chỉnh gia lƣợng 91 4.4.2 Hiệu chỉnh phƣơng pháp chiếu 94 KẾT LUẬN 107 TÀI LIỆU THAM KHẢO 108 iv DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT THƢỜNG DÙNG Ai Ma trận cosin hƣớng khâu thứ i aE(0) Vectơ gia tốc dài bàn kẹp hệ cố định C (q, q ) Ma trận ly tâm Coriolis ci cos(qi ) cijk cos(qi  q j  qk ) Di Ma trận chuyển toạ độ từ (Oxyz )0 vê hoạ toạ độ (Oxyz )i qk Số gia q k e(t ) Sai số vị trí f* Vectơ lực suy rộng lực không g(q ) Vectơ suy rộng lực trọng trƣờng Ii Ma trận tenxơ quán tính khối vật rắn thứ i In Ma trận đơn vị J Ma trận Jacobi J Ma trận tựa nghịch đảo ma trận Jacobi J Ri Ma trận Jacobi quay JTi Ma trận Jacobi tịnh tiến  Nhân tử Lagrange m Số bậc tự bàn kẹp mi Khối lƣợng vật rắn thứ i M (q ) Ma trận khối lƣợng suy rộng robot n Số bậc tự robot q Vectơ toạ độ suy rộng robot v q Đạo hàm bậc vectơ toạ độ suy rộng q theo thời gian q Đạo hàm bậc hai vectơ toạ độ suy rộng q theo thời gian qk Giá trị gần q k i Thế trọng lực khâu thứ i rCi Toạ độ khối tâm khâu thứ i rE(0) Vectơ toạ độ bàn kẹp hệ toạ độ (Oxyz )0 si sin(qi ) sijk sin(qi  q j  qk ) S(q ) Hàm ẩn T Động  Vectơ momen dẫn động i Vận tốc góc vật rắn thứ i i  Tốn tử sóng vectơ vận tốc góc thứ i W Ma trận trọng số vE(0) Vectơ vận tốc bàn kẹp vi DANH MỤC BẢNG Trang Bảng 2.1 Các tham số Denavit – Hartenberg robot phẳng bậc tự 23 Bảng 2.2 Các tham số động lực học robot phẳng bậc tự 24 Bảng 2.3 Các tham số Denavit – Hartenberg robot hàn AII-V6 31 Bảng 2.4 Các tham số động lực học robot hàn AII-V6 31 Bảng 2.5 Các tham số Denavit – Hartenberg robot bậc tự 42 Bảng 2.6 Các tham số động lực học robot bậc tự 42 Bảng 3.1 Biểu diễn toạ độ, vận tốc gia tốc qua tham số s 55 Bảng 3.2 Các giá trị cho biết qi (t ), qi (t ), qi (t ) 61 vii DANH MỤC HÌNH VẼ Trang Hình 2.1 Biểu diễn thơng số Denavit-Hartenberg khớp quay Hình 2.2 Robot n khâu 11 Hình 2.3 13 Hình 2.4 Hệ toạ độ khâu 16 Hình 3.1 Mơ tả đƣờng cong khơng gian 56 Hình 3.2 Mơ tả tham số đƣờng trịn khơng gian 57 Hình 3.3 Các prophin vận tốc quỹ đạo điển hình 58 Hình 3.4 Prophin vận tốc hình thang dạng đối xứng 59 Hình 3.5 Xấp xỉ đa thức pi (t ) 61 Hình 3.6 67 Hình 3.7 Quỹ đạo đƣờng bậc 68 Hình 4.1 Đồ thị biến khớp theo thời gian q(t ) 78 Hình 4.2 Đồ thị đạo hàm biến khớp theo thời gian q(t ) 79 Hình 4.3 Đồ thị sai số bám quỹ đạo theo thời gian e(t ) 79 Hình 4.4 Đồ thị biến khớp theo thời gian q(t ) 80 Hình 4.5 Đồ thị sai số bám quỹ đạo theo thời gian e(t ) 80 Hình 4.6 Đồ thị biến khớp theo thời gian q(t ) 81 Hình 4.7 Đồ thị sai số bám quỹ đạo theo thời gian e(t ) 82 Hình 4.8 Cơng sinh q trình chuyển động 83 Hình 4.9 Quỹ đạo chuyển động robot 83 Hình 4.10 Đồ thị biến khớp theo thời gian q(t ) 84 Hình 4.11 Đồ thị sai số bám quỹ đạo theo thời gian e(t ) 84 Hình 4.12 Cơng sinh q trình chuyển động 85 Hình 4.13 Quỹ đạo chuyển động robot 85 Hình 4.14 Đồ thị đạo hàm biến khớp theo thời gian q(t ) 88 viii b Robot hàn AII-V6 Giải toán động học ngƣợc mức độ vận tốc hiệu chỉnh phƣơng pháp chiếu Từ toán động học thuận, ta có mối liên hệ x q nhƣ sau: (c s c  s (c c c  s s ))(l  l )  (l  l )c s  c c l  l c  23 4 23 2 1  23 x(q )  (s1s23c5  s5 (s1c23c4  c1s ))(l5  l6 )  (l  l )s1s23  s1c2l  l1s1    (s23c4s5  c23c5 )(l5  l6 )  (l  l )c23  s2l2   Mô quỹ đạo bàn kẹp theo quỹ đạo tròn, với tâm đƣờng trịn có toạ độ 0.6895; 0.225; 0.9805 , bán kính R  0.2 m   Ma trận Jacobi : j  11 f J (q )    j21 q j  31 j12 j13 j14 j15 j22 j23 j24 j25 j 32 j 33 j 34 j 35 j16  j26  j 36   Mơ Matlab, ta có kết nhƣ sau: q1 q2 q3 q4 q5 q6 1.5 0.5 -0.5 10 t [s] Hình 4.23 Đồ thị biến khớp theo thời gian q(t ) 99 15 1.5 qd1 qd2 qd3 qd4 qd5 qd6 0.5 -0.5 -1 -1.5 -2 -2.5 -3 10 15 t [s] Hình 4.24 Đồ thị đạo hàm biến khớp theo thời gian q(t ) 1.5 x 10 -14 1.4 1.3 1.2 e 1.1 0.9 0.8 0.7 0.6 0.5 10 t [s] Hình 4.25 Đồ thị sai số bám quỹ đạo e(t ) 100 15 c Robot không gian bậc tự (1) Giải toán ngược mức độ vận tốc, hiệu chỉnh phương pháp chiếu Nhờ toán động học thuận, ta tìm đƣợc mối liên hệ x q nhƣ sau:  ((c c c  s s )s  c s c )d  c s d  ((((c c c  s s )c  3 4 3    c s s )c  (c c s  s c )s )s  ((c c c  s s )s  c s c )c )d  3 3 4  x    G  ((s c c  c s )s  s s c )d  s s d  ((((s c c  c s )c  s s s ) 3 4 3 4  x (q )  yG     c  (  s c s  c c ) s ) s )  (( s c c  c s ) s  s s c ) c ) d  z   3 3 4   g   (s c s  c c )d  c d  d  (((s c c  c s )c  s s s )s   4 3 4 5    (s 2c3s  c2c )c6 )d7    Mô quỹ đạo bàn kẹp theo quỹ đạo tròn, với tâm đƣờng trịn có toạ độ 0.4638; 0.3191; 0.3692 , bán kính R  0.15 m   Ma trận Jacobi J (q ) j  11 f J (q )    j21 q j  31 j12 j13 j14 j15 j16 j22 j23 j24 j25 j26 j 32 j 33 j 34 j 35 j 36 j17  j27  j 37   Mô Matlab, thu đƣợc kết sau: q1 q2 q3 q4 q5 q6 q7 0.5 -0.5 -1 -1.5 10 t [s] Hình 4.26 Đồ thị biến khớp theo thời gian q(t ) 101 15 qd1 qd2 qd3 qd4 qd5 qd6 qd7 -1 -2 -3 -4 10 15 t [s] Hình 4.27 Đồ thị đạo hàm biến khớp theo thời gian q(t ) x 10 -16 -1 -2 -3 10 t [s] Hình 4.28 Đồ thị sai số bám quỹ đạo e(t ) 102 15 (2) Giải toán ngược mức độ gia tốc, hiệu chỉnh phương pháp hiệu chỉnh gia lượng q1 q2 q3 q4 q5 q6 q7 0.5 -0.5 -1 -1.5 10 15 t [s] Hình 4.29 Đồ thị biến khớp theo thời gian q(t ) 2.5 qd1 qd2 qd3 qd4 qd5 qd6 qd7 1.5 0.5 -0.5 -1 -1.5 -2 -2.5 10 t [s] Hình 4.30 Đồ thị đạo hàm biến khớp theo thời gian q(t ) 103 15 15 qdd1 qdd2 qdd3 qdd4 qdd5 qdd6 qdd7 10 -5 -10 -15 -20 10 15 t [s] Hình 4.31 Đồ thị đạo hàm biến khớp lần theo thời gian q(t ) x 10 -16 -1 -2 -3 10 t [s] Hình 4.32 Đồ thị sai số bám quỹ đạo e(t ) 104 15 c Giải toán mức độ vận tốc, hiệu chỉnh phƣơng pháp chiếu vận tốc q1 q2 q3 q4 q5 q6 q7 0.5 -0.5 -1 -1.5 10 15 t [s] Hình 4.33 Đồ thị biến khớp theo thời gian q(t ) 1.5 x 10 -15 0.5 -0.5 -1 -1.5 10 t [s] Hình 4.34 Đồ thị sai số bám quỹ đạo e(t ) 105 15 Kết luận Chƣơng tập trung giải toán động học ngƣợc robot dƣ dẫn động dựa phƣơng trình liên hệ vận tốc gia tốc Các phƣơng pháp hiệu chỉnh gia lƣợng hiệu chỉnh phƣơng pháp chiếu đƣợc đƣa vào để tăng độ xác nghiệm tốn Ngồi ra, không gian bù ma trận Jacobi đƣợc khai thác để đảm bảo cho biến khớp nằm giới hạn tránh đƣợc va chạm vào giới hạn khớp Cách sử dụng phƣơng pháp phản hồi động học làm giảm sai số q trình tích phân tìm nghiệm Tính đắn tin cậy phƣơng pháp đƣợc khẳng định thông qua mô số tay máy phẳng bậc tự do, robot hàn AII-V6, robot bậc tự 106 KẾT LUẬN Để thiết kế chế tạo robot phục vụ cho việc đẩy mạnh cơng nghiệp hố đại hố cơng nghiệp, việc nghiên cứu động học động lực học robot cơng việc cần thiết Bài tốn động học ngƣợc tiền đề cho toán điều khiển robot, việc nghiên cứu phƣơng pháp giải tốn ngƣợc đóng vai trị quan trọng Luận án nghiên cứu phƣơng pháp giải toán ngƣợc động học robot dạng chuỗi dƣ dẫn động, có ý đến yếu tố động lực học” Tác giả đƣa mức giải toán cách hiệu chỉnh để sai số toán đƣợc nhỏ nhất, xác Những điểm nhấn luận án:  Tính tốn động học thuận rút phƣơng trình vi phân chuyển động cho mơ hình robot khác  Liệt kê cách lập trình quỹ đạo cho robot  Đƣa phƣơng pháp giải toán ngƣợc động học theo mức độ khác nhau, cách hiệu chỉnh sai số, tính tốn kiểm chứng phần mềm MATLAB/ SIMULINK Một số vấn đề liên quan hƣớng để phát triển nghiên cứu đề tài tƣơng lai :  Do có bậc tự cịn dƣ mà có khớp động bị kẹt robot vân có khả thực u cầu tốn cơng nghệ, ta phát triển nghiên cứu tƣợng kẹt khớp trình làm việc robot dƣ dẫn động  Nghiên cứu ứng dụng phƣơng pháp điều khiển khác để lựa chọn giải pháp điều khiển tối ƣu  Nghiên cứu triển khai ứng dụng thực tế kết nghiên cứu đƣợc 107 TÀI LIỆU THAM KHẢO [1] Nguyễn Văn Khang, Động lực học hệ nhiều vật, Nhà xuất Khoa học Kỹ thuật, Hà Nội, 2007 [2] Nguyễn Văn Khang, Chu Anh Mỳ, Cơ sở robot công nghiệp, Nhà xuất Giáo dục, Hà Nội, 2011 [3] Trần Hoàng Nam, Giải toán ngược động học, động lực học điều khiển trượt robot dư dẫn động dựa thuật toán hiệu chỉnh gia lượng vectơ toạ độ suy rộng, Luận án tiến sỹ kỹ thuật, Viện Cơ học, Hà Nội, 2010 [4] Nguyễn Nhật Lệ, Tối ưu hoá ứng dụng, Nhà xuất Khoa học kỹ thuật, Hà Nội, 2004 [5] Đinh Văn Phong, Phương pháp số học, Nhà xuất Khoa học kỹ thuật, Hà Nội, 2006 [6] Đinh Văn Phong, Mô số điều khiển hệ học, Nhà xuất Giáo dục Việt Nam, 2010 [7] Nguyễn Thiện Phúc, Robot công nghiệp, Nhà xuất Khoa học Kỹ Thuật, Hà Nội, 2004 [8] Nguyễn Quang Hồng, Thái Phƣơng Thảo, Giải tốn động học ngược robot dư dẫn động phương pháp chiếu toạ độ chiếu vận tốc, Tạp chí Tin học Điều khiển học, Tập 28, Số 1, Hà Nội, 2012 [9] Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam, Về thuật toán giải toán động học ngược robot dạng chuỗi, Tuyển tập Hội nghị Cơ học toàn quốc lần thứ VIII, Tập 1, Hà Nội, 2008 [10] Nguyen Van Khang, Nguyen Quang Hoang, Tran Hoang Nam, On an efficient method for improving the accuracy of the inverse kinematics of robotic manipulators, Int Conference on Engineering Mechanics and Automation (ICEMA 2010), Ha Noi, July 1-2, 2010 [11] Nguyen Quang Hoang, Nguyen Van Khang, On kinematic inverse and control of redundant manipulators under consideration of jammed joint, 108 Proceed Iftomm International Symposium on Robotics and Mechatronics, Hanoi, Vietnam, 2009 (201-207) [12] C R Rao, Generalized Inverse of Matrices and its Applications, New York, Wiley, 1971 [13] Jingguo Wang, Yangmin Li, Xinhua Zhao, Inverse Kinematics and Control of a 7-dof Redundant Manipulator based on the Closed-Loop Algorithm, Internation Journal of Advanced Robotic Systems, Vol 7, No 4, 2010 [14] E Bayo, R Ledesma, Augmented Lagrangian and Mass-Orthogonal Projection Methods for Constrained Multibody Dynamics, Nonlinear Dynamics 9: 113-130, 1996 [15] R N Jazar, Theory of Applied Robotics, 2nd edition, Springer Science + Business Media, LLC 2010 [16] John J Craig, Introduction to Robotics mechanics and control, 3rd edition, Upper Saddle River, New Jersey [17] Y Nakamura, Advanced Robotics/ Redundancy and Optimization, AddisonWesley Publishing Company, Reading 1991 109 ... ? ?Bài toán ngƣợc động học robot dạng chuỗi dƣ dẫn động có ý đến yếu tố động lực học? ?? bao gồm bốn chƣơng : Chƣơng : Tổng quan robot công nghiệp Chƣơng : Cơ sở lý thuyết động học thuận động lực học. .. DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI THÁI PHƯƠNG THẢO GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT DẠNG CHUỖI DƯ DẪN ĐỘNG CÓ CHÚ Ý ĐẾN YẾU TỐ ĐỘNG LỰC HỌC Chuyên ngành : Cơ Điện... n robot, ta phân loại robot thành robot chuẩn m  n , robot dƣ dẫn động n  m Khi nghiên cứu robot, việc thiết kế thƣờng phải giải toán động học, động lực học điều khiển Bài toán động học robot