Bài tập lớn Robotics Đại Học Bách Khoa Hà Nội

Bài tập lớn Robotics đại học bách khoa hà nội. Giáo viên hướng dẫn Thầy Phan Bùi Khôi. Bài tập này giải các bài toán động học thuận, động học ngược robot, thiết kế quỹ đạo, tĩnh học robot, động lực học robot, điều khiển robot

https://khuonghust.wordpress.com/ TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ - - BÀI TẬP LỚN HỌC PHẦN ROBOTICS Họ tên: Nguyễn Văn Khương Lớp: Cơ điện tử MSSV: 20142377 Mã lớp: 95966 Mã học phần: ME3168 GVDH: PGS.TS PHAN BÙI KHÔI Hà Nội ngày… tháng… năm 2016 https://khuonghust.wordpress.com/ MỤC LỤC LỜI MỞ ĐẦU CHƯƠNG 1: ĐỘNG HỌC ROBOT 1.1 Xây dựng cấu trúc robot 1.1.1 Mô hình khảo sát 1.1.2 Đặt hệ trục tọa độ 1.1.3 Bộ thông số Denavit-Hartenbeg 1.2 Bài toán động học thuận 1.2.1 Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp, hướng khâu thao tác 1.2.2 Vận tốc điểm tác động cuối E vận tốc góc khâu 1.2.3 Tính gia tốc điểm tác động cuối Tính gia tốc góc khâu thao tác 1.3 Bài toán động học ngược 1.3.1 Tính tọa độ khớp 1.3.2 Vận tốc dài khâu tương ứng với khớp tịnh tiến vận tốc góc ứng với khớp quay 1.3.3 Gia tốc dài khâu tương ứng với khớp tịnh tiến gia tốc góc ứng với khớp quay 10 CHƯƠNG 2: TĨNH HỌC ROBOT 11 2.1 Mô hình khảo sát toán tĩnh học 11 2.2 Tính toán toán tĩnh học 11 CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 15 3.1 Thiết kế quỹ đạo không gian khớp 15 3.2 Thiết kế quỹ đạo không gian thao tác 17 CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 20 5.1 Các tham số động lực học 20 5.2 Ma trận Jacobi tịnh tiến khâu 20 5.3 Tính ma trận jacobi quay khâu 21 5.4 Tính tensor quán tính khâu 22 5.5 Tính ma trận khối lượng 23 5.6 Tính toán năng, lực 24 CHƯƠNG 5: ĐIỀU KHIỂN 25 https://khuonghust.wordpress.com/ 6.1 Điều khiển phản hồi điều khiển vòng kín 25 6.2 Thiết kế điều khiển không gian khớp 26 PHỤ LỤC 27 KẾT LUẬN 31 https://khuonghust.wordpress.com/ LỜI MỞ ĐẦU Với phát triển mạnh mẽ công nghiệp Robot môn Robotics trang bị cho sinh viên kiến thức sở tính toán thiết kế Robot Đây kiến thức sở quan trọng Trong tập lớn để cập tới toán động học, tĩnh học, động lực học, thiết kế quỹ đạo chuyển động điều khiển phục vụ cho trình thiết kế Robot Mô hình robot khảo sát tập lớn robot bậc tự Để thực tập lớn này, em xin cảm ơn thầy giáo PGS.TS Phan Bùi Khôi giúp đỡ nhiều trình học tập lớn Xin chân thành cảm ơn thầy Hà nội, Ngày… Tháng… Năm … Sinh viên thực Nguyễn Văn Khương https://khuonghust.wordpress.com/ CHƯƠNG 1: ĐỘNG HỌC ROBOT Trong chương giải vấn đề sau: ➢ Tính số bậc tự robot ➢ Vẽ hệ trục tọa độ gắn liền với khâu theo quy tắc DH ➢ Lập bảng DH Tính ma trận DH ➢ Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp Xác định hướng khâu thao tác ➢ Tính vận tốc điểm tác động cuối E Tính vận tốc góc khâu ➢ Tính gia tốc điểm tác động cuối E Tính gia tốc góc khâu thao tác ➢ Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc góc khâu thao tác ➢ Tính tọa độ khớp ➢ Tính vận tốc, gia tốc dài khâu tương ứng với khớp tịnh tiến ➢ Tính vận tốc góc, gia tốc góc khâu ứng với khớp quay 1.1 Xây dựng cấu trúc robot 1.1.1 Mô hình khảo sát Hình 1: Mô hình Robot khảo sát Bậc tự mô hình khảo sát: https://khuonghust.wordpress.com/ f=6.(3-3)+3.1=3 Robot có khớp quay khớp tịnh tiến 1.1.2 Đặt hệ trục tọa độ Hình 2: Đặt hệ trục tọa độ theo phương pháp DH 1.1.3 Bộ thông số Denavit-Hartenbeg Từ mô hình hệ trục tọa độ ta xây dựng bảng thông số DanavitHartenbeg sau: Joint i di i 1 d1 a1 d2 0 3 a3 Trong đó: - Các thông số biển diễn hình vẽ - Các biến khớp tương ứng là 1 , d2 ,3 Các ma trận biến đổi tọa độ Denavit-Hartenbeg dựa vào thông số là: https://khuonghust.wordpress.com/ cos(1 )  sin(1 )  sin( ) cos( ) 1 A1    0   1 0 A2   0  0 0 a1 cos(1 )  a1 sin(1 )   d1   ; 0 0  d2   1 cos(3 )  sin(3 )  sin( ) cos( ) 3 A3    0   0 a1 cos(3 )  a1 sin(3 )     1.2 Bài toán động học thuận 1.2.1 Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp, hướng khâu thao tác Ta có ma trận biến đổi biểu diễn vị trí hướng hệ tọa độ khâu thứ i hệ tọa độ sở Oo xo yo zo là: c(1 )  s(1 )  s( ) c( ) A1    0   0 a1 c(1 )  a1 s(1 )  d1    c(1 )  s(1 )  s( ) c( ) 0 A  A1 A    0   c(13 )  s(13 )  s( ) c( ) 13 0 A3  A A3   13  0   0 a1 c(1 )  a1 s(1 )  d1  d    a1 c(1 )  a 3c(13 )  a1 s(1 )  a 3s(13 )   d1  d   Từ ma trận ta xác định được: ➢ Ma trận biểu diễn vị trí điểm thao tác biểu diễn theo tọa độ khớp https://khuonghust.wordpress.com/  x E  a1 c(1 )  a 3c(13 )  r E   y E    a1 s(1 )  a 3s(13 )   z E    d1  d ➢ Hướng hệ tọa độ khâu thác tác so với hệ tọa độ sở xác định từ góc Roll-Pitch-Yaw, với kí hiệu   Roll ,   Pitch ,   Yaw phép quay tương ứng sau: Hình 3: Mô tả phép quay RPY Ma trận quay biểu diễn phép quay RPY: R RPY cos .cos   sin .cos   cos .sin .sin  sin .sin   cos .sin .cos     sin .cos  cos .cos   sin .sin .sin   cos .sin   sin .sin .cos     sin  cos .sin  cos .cos   Để tính góc , ,  so sánh với ma trận quay khâu cuối: c(13 )  s(13 )  R E   s(13 ) c(13 )   0  Giải hệ phương trình tìm góc sau:           arctan  sin(13 )    cos(13 )  1.2.2 Vận tốc điểm tác động cuối E vận tốc góc khâu ➢ Xác định vận tốc điểm tác động cuối E Vận tốc điểm tác động cuối E có từ việc đạo hàm theo t ma trận định vị điểm tác động cuối E, nhận ma trận biểu diễn vận tốc tuyệt đối hệ sở: https://khuonghust.wordpress.com/  a (1  3 ).s(13 )  a1.1.s(1 )    v E  rE   a (1  3 ).c(13 )  a1.1.c(1 )    d2   ➢ Xác định vận tốc góc khâu Các ma trận quay biểu diễn hướng hệ tọa độ khâu i so với hệ tọa độ - sở Oo xo yo zo c(1 )  s(1 )  c(1 )  s(1 )  c(13 )  s(13 )      0 R1   s(1 ) c(1 )  R   s(1 ) c(1 )  R   s(13 ) c(13 )     0    Đạo hàm ma trận quay:  1.s(1 ) 1.c(1 )   1.s(1 ) 1.c(1 )      R1   1.c(1 ) 1.s(1 )  R   1.c(1 ) 1.s(1 )    0  0     (1  3 ).s(13 ) (1  3 ).c(13 )    R   (1  3 ).c(13 ) (1  3 ).s(13 )   0   Chuyển vị ma trận quay: -  c(1 ) s(1 )   c(1 ) s(1 )  R1T    s(1 ) c(1 )  R T2    s(1 ) c(1 )    0    c(13 ) s(13 )  R    s(13 ) c(13 )   0  T - Các ma trận sóng tương ứng:  1    1  R1.0 R1T  1 0  0 0    1    2  R R T2  1 0  0 0   https://khuonghust.wordpress.com/   3  R R  1  3   0 1  3 T 0 0  0   Vận tốc góc khâu hệ cố định là:   0 0                 1  3  1  1  1.2.3 Tính gia tốc điểm tác động cuối Tính gia tốc góc khâu thao tác ➢ Xác định gia tốc điểm tác động cuối: Gia tốc điểm tác động cuối E có từ việc đạo hàm theo t ma trận biểu diễn vận tốc điểm tác động cuối E hệ sở, nhận ma trận biểu diễn gia tốc điểm tác động cuối E hệ sở:  a (1  3 ).s(13 )  a (1  3 ) c(13 )  a1.1.s(1 )  a1.12 c(1 )    a E  v E   a (1  3 ).c(13 )  a (1  3 ) s(13 )  a1.1.c(1 )  a1.12 s(1 )    d2   ➢ Xác định gia tốc góc khâu thao tác Gia tốc góc khâu thao tác có từ việc đạo hàm theo t ma trận biểu diễn vận tốc góc khâu thao tác hệ tọa độ sở, nhận ma trận biểu diễn gia tốc góc khâu thao tác hệ tọa độ sở:    E  E  3    1  3  1.3 Bài toán động học ngược Đã biết: p   x E , y E , z E T  T  p   x E , y E , z E   T p   x E , y E , z E  Cần tìm: https://khuonghust.wordpress.com/ Hình 7: Đồ thị khớp 3.2 Thiết kế quỹ đạo không gian thao tác Thiết kế quỹ đạo cho điểm tác động cuối từ điểm A tới điểm B nhận AB làm đường kính Dùng phương trình tròn không gian mặt phẳng điểm A(x , y0 ,0),B(x e , ye ,0) lấy AB làm đường kính Phương trình đường tròn (x  x i )2  (y  yi )2  R Với x0  xe  xi   y0  ye   yi    R  (x e  x )  (y e  y )   Phương trình viết dạng tham số sau:  x  x i  R cos((t))   y  yi  R sin((t)) z   Để thỏa mãn điều kiện vận tốc tính toán hàm (t) hàm bậc theo thời gian: (t)  a  a1t  a t  a 3t 17 https://khuonghust.wordpress.com/ s(0)  A(x , y ,0)  v(0)  v   Thỏa mãn điều kiện:  s(t e )  B(x e , y e ,0)  v(t e )  v e  Từ điều kiện tìm hệ số a ,a1 ,a ,a sau:   x0  xi  a  arccos  R     a   3k  a   t e2  2k  a  t e   x  xi   xe  xi  k  arccos    arccos    R   R  Giả sử cần tạo quỹ đạo chuyển động từ điểm A(0.7, 0) đến B(-0.7, 0) thời gian te=5(s).Ta tính giá trị sau: a  1.5708  a   a  2.3562 a  0.7854  x   0.7sin (t)  => (t)  1.5708  0.t  2.3563t  0.7854t   y   0.7 cos (t) z   Sử dụng Matlab vẽ đồ thị: 18 https://khuonghust.wordpress.com/ Hình 8: Đồ thị x(t) vx(t) Hình 9: Đồ thị y(t) vy(t) 19 https://khuonghust.wordpress.com/ CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 4.1 Các tham số động lực học Coi đồng chất, tiết diện ngang không đáng kể, trọng tâm khâu đặt trung điểm Các tham số động học: Vị trí trọng tâm Khối xC yC zC lượng I xx I yy I zz I xy I yz I zx a1 0 m1 m1a12 12 m1a12 12 0 0 L2 m2 m2 L2 12 m2 L2 12 0 0 a 0 m3 m 3a 12 m 3a 12 0 Khâu Ma trận momen quán tính 4.2 Ma trận Jacobi tịnh tiến khâu Khâu 1: Ta có: c(1 )  s(1 )  s( ) c( ) A1    0    a1 c(1 )  1   a1 s(1 )  ; A c1  0 d1  0    0  c(1 )  s(1 )  => A c1   s(1 ) c(1 )  0   0 0 0 0 0 a        a   a1  c1  c1   2    a1  a 1 s1  rc1  A1 rc1   s1   2     d1   d1     20 https://khuonghust.wordpress.com/    a s  0  1     a1s1    0 J T1   a1c1 0  ; J TT1        0        a1c1   0 0   Khâu 2: Tính toán tương tự khâu ta có:     a1c1   rc2  A rc2   a1s1    d  d  L2   2    J T2  a1s1 0   a1s1 a1c1    T   a1c1 0  ; J T2   0    0 0  Khâu 3:   a1c1  a 3c(13 )    0  rc3  A3 rc3  a1s1  a 3s(13 )      d1  d     J T3 JT3 1    a s   a s(  )  a s(  ) 1 13 13   2   1   a1c1  a 3c(13 ) a 3c(13 )    2       1    a1s1  a 3s(13 ) a1c1  a 3c(13 )     0 1   1 a 3c(13 ) 0   a 3s(13 ) 2   4.3 Tính ma trận jacobi quay khâu Khâu 1: 21 https://khuonghust.wordpress.com/  c(1 )  s(1 )  A c1   s(1 ) c(1 )   0   0 0 a1  c1  c(1 ) s(1 )   a  s1  R c1   s(1 ) c(1 )        0  d1    c(1 ) s(1 ) 0  s(1 ) c(1 ) 0    R   s(1 ) c(1 ) 0 ; R c1   c(1 ) s(1 ) 0 1   0  0 T c1    R r T c1  1  0   r R  1 0  => 1      0  1  0  T c1 0 0  0    T  J R1  0 0  ; J R1  0 0  1 0  0 0  Khâu 2: Tính toán tương tự khâu ta có:  1  0   T T r2  R c2 R c2  1 0   1r    0 1  0    J R 0 0  0   0 0  ; J TR  0 0  1 0  0 0  Khâu 3:   R r  J R T c3   R  1  3   T c3 1  3     r 0   3    1  3  0  0 0  0   0 0  ; J TR  0 0  1  0  4.4 Tính tensor quán tính khâu Coi khâu robot đồng chất tiết diện bề ngang không 22 https://khuonghust.wordpress.com/ đáng kể Ta có:  0  c1 c1 = 0   0  m1a12 12  m2L22   12      ;c2c2=     m1a12    12   m2L22 12   0 0    c3  ; c3= 0    0 0    m 3a 12       m 3a  12  4.5 Tính ma trận khối lượng Áp dụng công thức tính ma trận khối lượng sau:  n  M(q)    J TTi mi J Ti  J TRi i ci J Ri    i1 n x n Sử dụng Maple tính ma trận M(q) sau:  m11 m12 M(q)   m 21 m 22  m31 m32 m13  m 23  m33  Với: 1  2 2 m11  m1a1  m 2a1  c(3 )a1a 3m3  m3a1  m3a  m12   1 m13  c(3 )a1a 3m3  m3a 32 2  m 21   m 22  m  m3 m   23  1 m31  c(3 )a1a 3m3  m3a  m32   m33  m3a 32  23 https://khuonghust.wordpress.com/ 4.6 Tính toán năng, lực   1    3 1  m1gz c1  m1gd1     m 2gz c2  m 2g  d1  d  L    3  m3gz c3  m3g(d1  d )      m1gd1  m 2g  d1  d  L   m3g(d1  d )      q1   G  1           G(q)  G      g(m  m3 )  q  G          q  24 https://khuonghust.wordpress.com/ CHƯƠNG 5: ĐIỀU KHIỂN 5.1 Điều khiển phản hồi điều khiển vòng kín Khi xét toán điều khiển cho tay máy công nghiệp, trước hết phải mô hình hóa tay máy cấu – đối tượng điều khiển, cảm biến đặt khớp để giám sát trạng thái khớp cấu dẫn động đặt khớp để sinh lực (momen) dẫn động khâu Chúng ta muốn điều khiển khớp robot bám theo quỹ đạo thiết kế Các phần tử dao động làm việc để nhận lệnh sinh lực (momen) Do phải sử dụng hệ điều khiển để tính toán lệnh phù hợp cho phần tử cho chúng thực quy luật chuyển động mong muốn Từ giá trị ban đầu quy luật qd(t), quy luật vận tốc q d (t) , quy luật gia tốc q d (t) mong muốn, điều khiển có nhiệm vụ tính toán lực F, nguồn động lực để hệ robot chuyển động với quy luật q q Nhờ cảm biến mà điều khiển đọc giá trị theo thời gian thực Việc tính toán lực F sử dụng phương trình động lực học, với đầu vào qd (t),qd (t) q d (t) Từ kết tính toán động lực học, F xác định theo công thức: F  M(q).q  C(q,q)q  G(q) F tính công thức giúp cho chuyển động theo quỹ đạo mong muốn, mô hình động lực hoàn toàn đúng, hệ không chịu tác động nhiễu Nhìn chung nhật để xây dựng hệ điều khiển có hiệu cao phải sử dụng tín hiệu phản hồi Theo đó, tín hiệu phản hồi sử dụng để tính toán sai lệch vị trí e sai lệch vận tốc e e  qd  qd e  qd  q Với điều khiển phản hồi kín ta có: F  qd  k ve  k q e F  mq  bq  kq F  F    e  k ve  k pe  25 https://khuonghust.wordpress.com/ 5.2 Thiết kế điều khiển không gian khớp Giả sử bàn kẹp E vị trí (x E , yE ,z E ) ứng với tọa độ khớp qi, nhiệm vụ điều khiển phải đưa điện áp đặt vào động để bàn kẹp di chuyển đến hay gần đến vị trí mong muốn rE với sai lệch cho phép, tương ứng vị trí mong muốn bàn kẹp rE tọa độ khớp qi Sau xét luật điều khiển đơn giản đáp ứng yêu cầu trên, luật điều khiển PID với bù trọng lượng Lực điều khiển tính từ vị trí mong muốn (ứng với tọa độ biến khớp qi) trạng thái chuyển động robot thời điểm mà thu thập từ cảm biến đặt khớp Sau tìm phương trình vi phân chuyển động, ta thiết kế điều khiển PID có bù trọng lượng Simulink sau: Hình 10: Bộ điều khiển PID bù trọng lượng Trong điều khiển PID thiết kế sau: Hình 11: Thiết kệ điều khiển PID 26 https://khuonghust.wordpress.com/ PHỤ LỤC Chương trình tính động học thuận Maple > > > CẤU HÌNH 27 https://khuonghust.wordpress.com/ > CÁC MA TRẬN DH > CÁC MA TRẬN BIẾN ĐỔI THUẦN NHẤT CỦA KHÂU I SO VỚI KHÂU GỐC > KẾT QUẢ VỊ TRÍ ĐIỂM THAO TÁC CUỐI > > VẬN TỐC ĐIỂM TÁC ĐỘNG CUỐI E 28 https://khuonghust.wordpress.com/ > GIA TỐC ĐIỂM TÁC ĐỘNG CUỐI > VỊ TRÍ CỦA CÁC KHỚP > VẬN TỐC GÓC VÀ GIA TỐC GÓC CỦA CÁC KHÂU Các ma trận cosin phương > Đạo hàm ma trận cosin phương > Chuyển vị ma trận cosin phương > =>Ma trận sóng > => Vận tốc góc khâu (trong hệ tọa độ cố định) > > 29 https://khuonghust.wordpress.com/ => Gia tốc góc khâu (trong hệ tọa độ cố định) > 30 https://khuonghust.wordpress.com/ KẾT LUẬN Bài tập lớn tìm kết sau: ➢ Bài toán động học ➢ Bài toán động lực học ➢ Bài toán tĩnh học ➢ Thiết kế quỹ đạo chuyển động ➢ Điều khiển Tuy nhiên nhiều phần tập lớn bỏ qua sai sótvì khả chưa cho phép Trong phần điều khiển chưa đủ thời gian khả để lập trình Simulink phần mềm Matlab Rất mong có thông cảm thầy cô Chân thành cảm ơn 31