Báo cáo bài tập lớn học phần robotics

BÁO CÁO BÀI TẬP LỚN Học phần Robotics Sinh viên thực hiện: Nguyễn Văn Huy MSSV: 20131782 MỤC LỤC ĐỀ BÀI BÀI LÀM BÀI TOÁN ĐỘNG HỌC ROBOT 1.1 Vẽ hệ trục tọa độ theo quy tắc D-H lập bảng thông số D-H 1.2 Tính ma trận D-H: 1.3 Xác định vị trí điểm tác động cuối khâu thao tác, hướng khâu thao tác: 1.4 Vận tốc, gia tốc điểm tác động cuối 1.5 Vận tốc góc, gia tốc góc khâu cuối 1.6 Bài toán động học ngược 10 TĨNH HỌC ROBOT 11 2.1 Lực dẫn động khâu 3: 12 2.2 Lực dẫn động khâu 2: 14 2.3 Lực dẫn động khâu 1: 15 ĐỘNG LỰC HỌC ROBOT 16 3.1 Động 17 3.1.1 Ma trận Jacobi tịnh tiến trọng tâm khâu 17 3.1.2 Ma trận Ten-xơ quán tính 18 3.1.3 Ma trận khối lượng 18 3.2 Thế 18 3.3 Lực suy rộng 18 3.4 Tính tốn phần mềm MAPLE 19 THIẾT KẾ QUỸ ĐẠO 26 ĐIỀU KHIỂN ROBOT 28 ĐỀ BÀI Chọn mơ hình robot nghiên cứu theo nội dung đây: 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 Denavit – Hartenberg (DH) Lập bảng D-H Tính ma trận D-H: i-1Ai, i=1,2… 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 vận tốc điểm tác động cuối 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 10 Tính vận tốc góc, gia tốc góc khâu tương ứng với khớp quay 11 Khảo sát toán tĩnh học robot theo câu: 13, 14, 15 12 Thiết kế quỹ đạo chuyển động câu 16 13 Khảo sát động lực học theo nội dung câu 17, 18 14 Thiết kế mô hình điều khiển robot theo câu 19, 20, 21 15 Lập trình tính tốn, vẽ đồ thị phần mềm MAPLE, MATLAB, mô điều khiển robot MATLAB, SIMULINK BÀI LÀM Robot có bậc tự Có thơng số sau: L2=0.6 ; a3 = 0.4; d1 = 0.6 (m) m1 = 16; m2 = 12; m3 = (kg) BÀI TOÁN ĐỘNG HỌC ROBOT 1.1 Vẽ hệ trục tọa độ theo quy tắc D-H lập bảng thông số D-H Bảng thông số D-H tay máy robot: Joint θi θ1 di d1  d2 θ3 a3 αi   1.2 Tính ma trận D-H: ci  s i 1 Ai   i 0  0 c i si s i si c i ci s i ci s i c i 0 c1  s A1   0  0  s1 c1 1 0 c3  s A3       s3 c3  s1s3  c s A3    c3   0 ci  si  di    0  1 A2    1  0 0  d1   1 0 0  d2   1 a3c3  a3 s3     s1c3 c1 c1c3 s1  s3 0 a3 s1s3  d s1  a3c1s3  d 2c1   a3c3  d1   1.3 Xác định vị trí điểm tác động cuối khâu thao tác, hướng khâu thao tác: Vị trí điểm tác động khâu thao tác E:  a3 s1s3  d s1  rE  r3   a3c1s3  d 2c1    a3c3  d1 Hướng khâu thao tác: s1c3 c1   s1s3  RE  R3   c1s3 c1c3 s1   c3  s3  So sánh với ma trận Cardan RCD c c     s s  c  c s  c s c  s s c s  s s  s  c c c s s  s c s   s c  ta c c  có hệ phương trình:  f1  c c   tìm góc Cardan  f  s  c1   f  c s  s c   1.4 Vận tốc, gia tốc điểm tác động cuối Vận tốc điểm tác động cuối: vE  rE 0 r3  J TE q JTE ma trận Jacobi tịnh tiến khâu cuối E JTE  a3c1s3  d 2c1   a3 s1s3  d s1   s1 c1 a3 s1c3  a3c1c3  ; a3 s3   1    q  d2       a3c1s31  d c11  s1d  a3 s1c33    vE   a3 s1s31  d s11  c1d  a3c1c33    a3 s33   Gia tốc điểm cuối E: Với JTE J TE  J11   J 21  J 31 J12 J 22 J 32 aE  vE  rE  J TE q  JTE q  J11 J13    J 23  J TE   J 21  J 31 J 33   J12 J 22 J 32  a3 s 1s31  d s11  c1d  a3c1c33    a3c1s31  d c11  s1d  a3 s1c33   J13   J 23  J 33  c11  s11 a3c1c31  a3 s1s33   a3c1c31  a3c1s33   a3c33   1    Và q   d  thay vào ta gia tốc aE:     1.5 Vận tốc góc, gia tốc góc khâu cuối - Vận tốc góc khâu cuối: s1c3 c1   s1s3  RE  R3   c1s3 c1c3 s1  ;  c3  s3  c1s31  s1c33 c1c31  s1s33  RE   s1s31  c1c33 s1c31  c1s33  s33 c31   s1s3 R   s1c3  c1  s11   c11   T E c1s3 c3   s3   c1c3 s1 Vận tốc góc khâu cuối biểu diễn hệ tọa độ khâu cuối: c1s31  s1c33 c1c31  s1s33  s11   s1s3 c1s3 c3    E 0 RE RET   s1s31  c1c33 s1c31  c1s33 c11   s1c3 c1c3 s3   s33 c31   c1 s1        1   s13  c13  s13     c13   E   s13  ; E  c13  1  c13    0 c3  Ma trận Jacobi quay khâu cuối: J RE  0  s3  1 0  Gia tốc     J RE q  J RE q 1 Trong đó: J RE   s33      c33   T s13 1   J RE q   s33    c33   0  0 0  0   1   c3   0   d     s3 0  3   0   1    s313  1c3      0   d    c313  1s3    3   3   Cho quy luật chuyển động khâu sau: Ta vẽ có đồ thị biểu diễn vị trí, vận tốc, gia tốc điểm thao tác E: Vị trí: Vận tốc: Gia tốc: Và đồ thị vận tốc góc, gia tốc góc khâu 3: Vận tốc góc: Gia tốc góc: 1.6 Bài tốn động học ngược  xE  0.5  xE  0.3  xE  0.1         Cho rE   yE   0.8  m  ; vE   yE   0.2  m / s  ; aE   yE   0   m / s   zE  0.6  zE  0.2  zE  0.1 Ta có phương trình  xE  a3 s1s3  d s1  xE   a3 s1s3  d s1   y   r   a c s  d c    y  a c s  d c  E 1 3  E E  3  z  a c  d  zE    a3c3  d1 3  E (*) Sử dụng hỗ trợ phần mềm MAPLE: > > > 10  r   0, d1 , 0 ; r  R r với T 1 0 1 0   d1    r  0   r2   d1  d1   0  1 c1 R1   s1  0 0 0  s1  c1  1         T    d1  d1  1 1 1 rc1  0, , 0 ; rc1  R1 rc1  rc1  0   rc1      d1       2  d1 0  0  0     Thay tất vào hệ phương trình ban đầu ta được:  80  0   N  F1,0  0    392.2   3.003    M  100.067   Nm    1,0    6.686   ĐỘNG LỰC HỌC ROBOT Phương trình động lực học Lagrange loại II: d  T  dt  qi  T    Qi  U i , i  n  qi  qi Trong đó: o o o o o o T – Động hệ robot Π – Thế hệ robot qi – tọa độ suy rộng thứ i Qi – Lực suy rộng lực không ứng với tọa độ suy rộng qi Ui – Lực/Momen điều khiển ứng với tọa độ suy rộng qi n – Số bậc tự robot Phương trình động lực học dạng ma trận: M ( q ) q  C ( q, q ) q  G ( q )  Q  U Trong đó: 16   o M (q)    JTiT mi JTi  J RiT cir J Ri  n  i 1  nn o C (q, q)q  c j (q, q)q  n1 ; c j (q, q)q  o G j (q)   g j (q)  n 1 ; g j (q )  n   k , l; j  q q k ,l 1 k l  q j o Q  Bq  Dsignq  Q a đó: Bq ma sát nhớt, Dsignq ma sát khô, Q a lực suy rộng lực không 3.1 Động T T   q M (q)q  qT   J TiT mi J Ti  J RiT cir J Ri   q 2  i 1  3.1.1 Ma trận Jacobi tịnh tiến trọng tâm khâu Ma trận Jacobi tịnh tiến trọng tâm khâu: Aci 0 Ai i Aci 0 rci 0 JTi  r q Ta tính được: 1  l2 c1  d c1 0      JT  0 0  ; JT   l2 s1  d s1 2 0 0     1  a3c1s3  d c1  JT   a3 s1s3  d s1 2     s1 c1   s1   c1  ;  0    a3 s1c    a3c1c     a3 s   Ma trận Jacobi quay khâu: i 0 Ri RiT 0 i  JRi   q Ta tính được: 17 0 0 0 0 0 c1      JR1  0 0  ; JR  0 0  ; JR1  0 s1  1 0  1 0  1 0  3.1.2 Ma trận Ten-xơ quán tính Coi khâu robot đồng chất tiết diện bề ngang không đáng kể Ta có:  m1d12   12 c1      0  m2l2     12   ; c     m1d12    12  0   0     ; c  0    m2l2  0 12   m3a32 12      2 m3a3  12  3.1.3 Ma trận khối lượng Với hỗ trợ phần mềm MAPLE ta tính M:  Động T: 3.2 Thế    g.mi zci với m   m1 m2 i 1 m3  zc1  d1 ; zc  d1; zc3  a3c3  d1 2  Π= 3.3 Lực suy rộng T T Với lực không tác dụng lên RB F   Fx Fz  ; M   Mx 0 18 Lực suy rộng Q    JTiT Fi  J RiT M i  F1 = F2 = 0; M1 = M2 = 0; F3 = F; M3 i 1 = M ta tính Q: 3.4 Tính tốn phần mềm MAPLE Chương trình MAPLE tính toán động lực học thuận: > Ma tran Denavit Hartembeg: DH(theta, d, a, alpha) Nhan ma tran: nhan2(A,B) Nhan ma tran: nhan3(A,B,C) ma tran Jacobi 3dof : J(A,q1,q2,q3) ma tran Jacobi 2dof: J(A,q1,q2) Rut gon: rutgon(A) tao ma tran song: w2ws(w) van toc goc tu ma tran song: ws2w(ws) > # Nhập biến khớp robot: -# > # ma trận D-H -# 19 > # Nhập tọa độ trọng tâm khâu rb -# > # -Ma trận Jacobi tịnh tiến trọng tâm khâu # 20 > tao ma tran omega tu ma tran DH: w(A) > 21 > ma tran Jacobi quay tu ma tran omega: jr(w) > > # Tính ma trận khối lượng: # # -ma trận quán tính khâu hệ tọa độ qua trọng tâm khâu đó: 22 > > # =>ĐỘNG NĂNG - 23 > # Tính ma trận C # hàm tính ma trận C # > 24 > > # Tính PI, ma trận G -> > > # -Tính lực suy rộng Q > # lực dẫn động U - > 25 THIẾT KẾ QUỸ ĐẠO Cho điểm A(0.2, 0, 0.5) B( 0.6, 0.8, 0.7) không gian thao tác robot Quỹ đạo điểm tác động cuối robot theo đường thẳng từ A đến B khoảng thời gian te(s) Phương trình đường thẳng từ A(xA,yA,zA) đến B(xB, yB, zB) : xxA y yA zzA   xB  x A y B  y A z B  z A yB  y A y A xB  y B x A  y  x  x x  x  x  B A B A   z  z B  z A x  z A xB  z B x A  xB  x A xB  x A Ta thiết lập quan hệ x=x(t) đa thức bậc 3: x(t )  a0  a1t  a2t  a3t Với điều kiện vận tốc đầu cuối ta được: a0  x A a   x(0)  x A  t 0 v (0)  v   3( x  x ) x0  x => a2  B A te  x(te )  xB  t  te    2( xB  x A ) vx (te )  vxB  a3  te3  Cho te = 5s Thay tọa độ A B vào ta 26 a0  0.2 a    3(0.6  0.2)  x  0,  0.048t  0.0064t a2   0.048   2(0.6  0.2)  0.0064 a3  53   x  0,  0.048t  0.0064t    y  0.96t  0.128t  z  0.5  0.24t  0.32t  Đồ thị biểu diễn vị trí điểm tác động cuối robot theo phương: Đồ thị không gian: 27 ĐIỀU KHIỂN ROBOT Điều khiển hệ thống theo luật điều khiển PD Phương trình động lực học lấy từ phần 3-động lực học: M ( q ) q  C ( q, q ) q  G ( q )  Q  U Kv, Kp ma trận đường chéo:  kv1 Kv    0 kv  k p1 0    ; Kp   0 kv   k p2 0    k p  Được xác định từ phương trình: E  KV E  K p E  Xây dựng mô MATLAB-SIMULINK sau: 28 Khối controller-PD: Khối control-nonlinear: 29 Khối robot system: 30