Hướng dẫn xây dựng phương trình Động lực học cho robot công nghiệp

Hướng dẫn chi tiết các bước để xây dựng phương trình động lực học cho robot công nghiệp. Ví dụ minh họa chi tiết cho robot 2 bậc. Các bài tập áp dụng. Mô phỏng chi tiết trên Matlab. Hướng dẫn mô phỏng robot 3D bằng Matlab.

BÀI 2: ĐỘNG LỰC HỌC CHO ROBOT CÔNG NGHIỆP 2.1 Mục đích Mục đíc thực hành xây dựng phương trình động lực học cho robot, tính tốn phương trình động lực học Matlab, mơ động lực học Simulink, mô 3D robot Simulink 2.2 Khái niệm phương trình động lực học robot cơng nghiệp Phương trình động lực học phương trình biểu diễn mối quan hệ lực, momen khớp với vị trí, vận tốc, gia tốc Để từ sở để xây dựng lên toán điều khiển vị trí Robot Dựa vào phương trình động lực học, tính lực, mơmen cần thiết để khớp robot chuyển động với tốc độ gia tốc mong muốn Nghiên cứu động lực học robot phục vụ cho mục đích: Mơ hoạt động khớp tay máy để khảo sát, thử nghiệm q trình làm việc đa dạng mà khơng cần sử dụng robot thật, phân tích, tính tốn kết cấu khớp tay máy thiết kế hệ thống điều khiển robot 2.3 Các bước xây dựng phương trình động lực học Bước 1: Tính động nối - Động nối: Đối với vật thể cứng có n bậc tự do, động vật Kᵢ xác định công thức: Ki  T vci mi  i I i i 2 (2.1) - ci : trọng tâm nối [kg] li : chiều dài nối [m] mi : khối lượng nối [kg] - lci : khoảng cách từ trục quay đến ci [m] - vci : tốc độ dài ci [m/s] -  i : vận tốc góc [rad/s] - Ii: ma trận quán tính xác định sau:  I xx  I i    I yz   I xz   I zx    I zy  I zz   I yx I yy  I yz I xx   ( y  z ) pdv I xy  I yx   xy pdv v v I yy   ( x  z ) pdv I xz  I zx   zxpdv v (2.2) v I zz   ( x  y ) pdv I yz  I zy   zypdv v v - Thế nối Thế khâu i có khối lượng m, trọng tâm xác định véc tơ rᵢ ( véc tơ biểu diễn trọng tâm khâu i hệ tọa độ bản) viết dạng: Pᵢ = -mᵢ.g.rᵢ Hay cịn tính theo cơng thức: Pi  mi G g Pci T (2.3) Trong đó: T - Gi : véc tơ trọng trường, tính theo cơng thức: G i  [ 0 1] T - T Pci : vị trí trọng tâm nối, tính theo cơng thức: Pci  [ x ci yci T zci ] Bước 2: Thiết lập hàm Lagrange Có nhiều phương pháp để xây dựng phương trình động lực học thường sử dụng:  Phương pháp Euler – Lagrange dựa cân lượng  Phương pháp Newton – Euler dựa cân lực, mômen nối  Trong giảng ta nghiên cứu phương pháp Euler- Lagrange: Hàm Lagrange hệ thống định nghĩa hiệu tổng động tổng hệ: L=K–P (2.4) Trong đó: K - tổng động hệ thống P - tổng hệ thống K P đại lượng vơ hướng nên chọn hệ tọa độ thích hợp để tốn đơn giản ( không thiết phải dùng hệ tọa độ Đề-các) Đối với robot có n khâu : 𝑛 𝑛 𝐾 = ∑ 𝐾ᵢ 𝑃 = ∑ 𝐾ᵢ 𝑖=1 𝑖=1 Khi hàm Lagrange xác định là: n n i 1 i 1 L   K i   Pi (2.5) Ở Kᵢ Pᵢ động khâu thứ i hệ tọa độ chọn Ta biết đại lượng Kᵢ Pᵢ hàm số phụ thuộc nhiều biến số: Kᵢ = K(qᵢ,𝑞ᵢ)̇ Pᵢ = P(qᵢ,𝑞ᵢ̇) Với qᵢ tọa độ suy rộng khớp thứ i khớp thứ i khớp quay qᵢ góc quay θᵢ, khớp tịnh tiến qᵢ độ dài tịnh tiến dᵢ 𝑞ᵢ̇ vận tốc tương ứng Bước 3: Thiết lập phương trình Lagrange Phương pháp Euler- Lagrange dựa phương trình vi phân cấp hai phi tuyến biểu diễn quan hệ mơmen  góc quay θ, tốc độ  , gia tốc  Ta định nghĩa: Lực tác dụng lên khâu thứ i ( i =1,2,3 ,n) với quan niệm lực tổng quát, lực momen( phụ thuộc vào biến khớp qᵢ tịnh tiến quay) - Đối với khớp tịnh tiến lực Fᵢ: Fi  - d  L  L   dt  ri  ri (2.6) Đối với khớp quay momen ᵢ : i  d  L  L   dt  i  i (2.7) Trong đó: - Fi : lực đặt dọc theo chuyển động khớp tịnh tiến - L : hàm Lagrange - ri , i : biến khớp - ri , i : tốc độ khớp - L L : đạo hàm riêng L theo biến ri ,i , ri i - d  L  d  L   ,   : đạo hàm riêng cấp L theo biến ri , i dt  ri  dt  i  - L L : đạo hàm riêng L theo biến ri ,i , ri i  i : momen đặt vào khớp quay Bước 4: Xác định phương trình động lực học - Phương trình động lực học cho robot n bậc tự sau τ = M     + H(, ) + G() + Fd + Fs () + τ d (2.8) Trong đó: -  :là mômen sinh cấu chấp hành đặt lên khớp (nx1)  = [1(t), 2(t), , n(t) ]T - M() : ma trận quán tính (nxn) - H( , ) : vectơ tương hỗ ly tâm (nx1) H(, ) =  h1,h h n  T - G() : vectơ trọng trường (nx1) G() = [G1,G2, Gn]T - Fd : vectơ lực ma sát động - Fs : vectơ (nx1) số ma sát nhớt - d : vectơ (nx1) mô tả nhiễu ngoại chưa biết -  : vectơ (nx1) giá trị biến khớp  = [1(t), 2(t), , n(t) ]T -  :vectơ (nx1) tốc độ biến khớp  = 1 (t), 2 (t) n (t)  T -  : vectơ (nx1) gia tốc biến khớp  = 1 (t), 2 (t) n (t)  T Trong số trường hợp để đơn giản người ta thường bỏ qua thành phần ma sát nhiễu phương trình động lực học tổng quát viết gọn dạng sau: τ = M     + H(, ) + G() (2.9) Ngồi phương trình động lực học cịn viết dạng sau: τ = M     + C(, ) + G() (2.10) Trong H( , ) tương đương với C(, ) Xác định ma trận C(, ) C thiết lập sau: -  C11 C1n  C     Cn1 Cnn  (2.11) Các hệ số Cijk (i, j, k = 1÷n) sau: - n 1  H kj H ki H ij   Cijk =  + )   C kj =  Cijk q i 2 q j q k  i=1  q i  (2.12) 2.4 Ví dụ minh họa Xây dựng phương trình động lực học cho cấu robot nối hệ tọa độ trục Oxy hình vẽ: y l1 lC2 lC1 l2 x Hình 2.1 Thơng số nối Robot Plana Cho biết: c1  s A1   0  0  s1 l1.c1   c  s  c1 l1.s1  , A2    0    0    c(1   )  s (1   )  s (   ) c(   ) T2  A1 A2   1  0  0   s c 0 l1.c1  l2 c(1   )  l1.s  l2 s (1   )     l2 c  l2 s     Ký hiệu tham số sau:  i - Góc quay khớp quay i (i=1,2); mi -Khối lượng nối i; I i - Mơ men qn tính nối i trục qua tâm khối nối li - Chiều dài nối i; lCi - Chiều dài từ khớp i đến tâm nối i Giả thiết: - Khớp sinh momen τ1 tác dụng bệ nối Khớp sinh momen τ2 tác dụng nối Trọng lực có hướng theo trục y Chọn 1  biến khớp Ký hiệu động nối i Ki Pi (i=1,2) Xây dựng phương trình động lực học cho robot nối Plana (Hình 2.1) Bước 1: Tính động thạn nối * Tính động nối thứ 1: Động nối thứ nhất: K i  2 v ci mi + 10 T i Ii i Đối với nối 1, tọa độ tâm khối xác định theo phương trình sau:  X C1  lC1 c1  PC1  YC1  lC1 s1    Z C1   X C2  l 2C1 s12  12  X C1   lC1 s1.1   2  YC1  lC1 c1.1   Y C1  l C1 c1    0  Z C1   Z C1  Bình phương tốc độ dài tâm khối nối là:  VC2  X C  YC  ZC  l 2C s12   l 2C c12  12  l 2C 12 1 1 1 Do động tâm khối nối là: K1  2 m1lC11 + I1.1 2 Thế nối thứ nhất: Pi  mi GiT Pci P1  m1 glC1 sin 1 * Tính động nối thứ 2: Đối với nối 2, tọa độ tâm khối xác định theo phương trình sau: x2  l1cos1  lC cos(1   ) y2  l1 sin 1  lC sin(1   ) Hai thành phần tốc độ tâm khối trục là: x2  l1 sin 11  lC sin(1   )(1   ) y2  l1cos11  lC cos(1   )(1   ) Bình phương tốc độ dài tâm khối nối là: v22  x22  y22 v22  l1212  lC2 (1   )  2l1lC 2cos ( 12  1 ) Do động tâm khối nối là: 1 2 K  m2 v + I1 (1  2 ) 2 1 2 2 2  m2 l1 1  lg2 (1  2 )  2l1lC2cos2 (1  1 2  + I (1  2 )   2 Thế nối tính theo: P2  m g[l1 sin 1  lC2 sin(1  2 )] Bước 2: Thiếtb lập hàm Largange Hàm Lagrange xác định là: n n i 1 i 1 L   K i   Pi Thay động nối vào hàm Largange ta được: L  ( K1  K )  ( P1  P2 ) 2 2 2 2 L  m1lC11 + I1.1  m l1 1  lC2 (1  2 )  2l1lC2cos2 (1  1 2  +   2 2 + I2 (1  2 )  m1 glC1 sin 1  m2 g[l1 sin 1  lC2 sin(1  2 )] Lấy đạo hàm Lagrange theo biến 1và1 thực số phép biến đổi ta được:   d  L   2      m1lC1 + I1  m l1  lC2  2l1lC2cos2  I  1 + dt  1  2 +  m (lC2  l1lC2cos2 )  I2  2  m 2l1lC2 sin 2 (21 2  2 )]   L  m1lC1cos1 - m2g  l1cos1  lC2cos(1  2 )  1 Tương tự đạo hàm Largarange theo biến  và nối ta nhận được:     d  L   2      m lC2  l1lC2cos2  I  1 + m lC2  I 2  m 2l1lC2 sin 212 dt  2    L  m2glC2cos(1 + 2 ) - m2l1lC2 1  12 sin 2 2 Bước 3: Thiết lập phương trình Lagrange: Ta có phương trình: τi = d  L  dt  i  L   i (2.13) τ1   m1lC2 + I1  m  l12  lC2  2l1lC cos   I  1 + +  m2 (lC2  l1lC cos )  I    m2 l1lC sin  (21    22 )]  m1lC1cos1 + m g  l1cos1  lC cos(1   )    τ   m  lC2  l1lC cos   I  1 +  m2 lC2  I      m2l1lC sin  21  m glC cos(1 +  ) + m 2l1lC  12  1 sin  Bước 4: Xác định phương trình động lực học Phương trình động lực học viết dạng ma trận trạng thái:   M()  H(, )  G()  1   M11 M12   1   H11   G1     M        21 M 22  2   H 21   G  Trong đó:   M  m l2 + I  m l  l  2l l cos  I C1 C2 C2 2  11  m2 (lC  l1lC cos )  I  M 12  M  m2 (lC2  l1lC cos )  I  21  M 22  m2 lC2  I   10 (2.14) Bước 6: Lắp ghép khối khí robot để hồn thiện robot 17 2.7 Ví dụ: Mơ cấu khí 3D cho robot bậc thơng qua lập trình Simscape Mutibody Matlab Simulink Bước 1: Xác định cấu hình robot cần mô - Động học thuận robot bậc Hàm tính động học thuận: 18 - Động học ngược robot bậc tự Hàm tính động học ngược: 19 Bước 2: Mở đặt tên file Simscare Matlab Bước 3: Cài đặt tọa độ gốc 20 Do robot bậc làm việc mặt phẳng XOY tức x=0 y=0 Bước 4: Tạo khối cho robot - Tạo khối đế cho robot bậc tự với kích thước : mặt bên 15 x15 x5 cm, trụ dường kính 5cm cao 6cm Khối mặt bên Khối 21 + Tạo khối mặt bên: Tại tọa độ mặt phẳn liên kết: mặt phẳng bên tạo tọa độ liên kết tên mặt để liên kết với trụ + Tạo khối trụ tròn Ta phải tạo mặt phẳn gắn tọa độ để gắn với khối bên cạnh 22 Do ta khối đế Hình 2.2 Hình ảnh mơ khối đế - Thanh nối thứ thứ 2: tương tự cách tạo khối ta làm nối với kích thước nhau: dài 100cm, rộng 15cm dày 5cm nối với khớp quay trụ tròn phần khối đế 23 Bước 5: Tạo khớp liên kết khối - Cả khớp nối robot khớp xoay nên ta vào thư viện Simulink để lấy khớp xoay - Tại khớp xoay phải có lực tác động cảm biến phản hồi tốc độ, lực nên ta phải cài đặt cho module khớp xoay sau: 24 Bước 6: Lắp ghép khối khí robot để hồn thiện robot 25 Hình 2.3 Hình ảnh 3D robot bậc tự Mơ động học tồn hệ thống Hình 2.4 Sơ đồ khối Matlab Simulink mơ hệ thống động lực học cho robot bậc tự Với giá trị đặt ban đầu: 26 - Kết quả: b) Quỹ đạo mơ robot chạy Hình 2.5 Kết mô 27 Nhận xét: Quỹ đạo chuyển động robot tương đồng với quỹ đạo đặt 2.8 Bài thực hành số PhẦN 1: Bài tập Bài 1: Xây dựng phương trình động lực học cho robot Almega 16 (3 bậc tự do) (Hình 2.6) cách xây dựng, tính tốn Matlab Mơ động học cấu khí 3D trênMatlab Simulink cho robot? a2 Y1 a3 lC2 3 2 Y2 Y3 lC3 C2 O1 C1 X1 O2 Z1 C0 d1 Z2 X2 O3 X3 Z3 Z0 Y0 lC1 1 O0 X0 Hình 2.6 Almega16 có gắn hệ trục tọa độ với khớp Bài Cho Robot có khâu phẳng (Hình 2.7), cấu hình RRRRR Hãy thiết lập phương trình động lực học Robot cách xây dựng, tính tốn Matlab Mơ động học cấu khí 3D trênMatlab Simulink cho robot? (Với kích thước tự cho) 28 Hình 2.7 PHẦN 2: Câu hỏi trắc nghiệm Câu 1: Bài toán động lực học nghĩa là: A Cho trước lực tác dụng B Cho trước lực tác dụng xác định chuyển vị tay kẹp C Cho trước chuyển vị chác khớp xác định lực momen tác động tương ứng D D Cả B C Câu 2: Các kết luận động khâu tay máy: A Là đại lượng vô hướng B Là đại lượng vectơ C Là đại lượng không thứ nguyên D Tồn dạng vectơ đại số Câu 3: Thế gtrọng lực khâu có chất là: A Lực tác dụng lên khâu B Độ lớn trọng lực tác dụng lên khâu C Là phần lượng khâu D Là cơng lực tác dụng lên khâu Câu 4: Xác định dạng phương trình Lagrange II cho Robot: A Ti qi d T1 dt q d T T dt q qi i Q '' ; qi Q '' ; qi 29 B T C Ti qi T1 dt ; i Câu 5: Nói ý nghĩa việc nghiên cứu động lực học robot ý kiến sau sai: A Xác định momen lực tác động xuất trình chuyển động B Xác định momen lực tác động xuất trình chuyển động phục vụ cho việc chọn công suất động C Xác định chuyển vị khớp có lực momen tác động lên khớp D Kiểm tra momen lực tác động xuất trình chuyển động Câu 6: Lực hoạt động phương trình Newton – Euler có nghĩa là: A Trọng lực B Phản lực khớp C Các lực sinh công D Tất lực trên: Câu 7: Thế trọng lực khâu i tính theo cơng thức: A  i  mi g 0T rci B  i  mi g0T rci C  i   mi g 0T rci Câu 8: Phương trình động lực học robot viết dạng: A B τ = M  q  q + C(q,q)q + G(q) + F(q) + τ d τ = M  q  q + C(q,q)q + G(q) 𝟏 C 𝝉 = 𝑴(𝒒)𝒒̈ + [𝟐 𝑴̇(𝒒) + 𝑺(𝒒, 𝒒̇ )] 𝒒̇ + 𝑮(𝒒) D Cả phương án 30 Câu 9: Ma trân Jacobian giải tích có đặc điểm: A Dùng để xác định mối quan hệ vận tốc bàn kẹp không gian công tác vận tốc khớp B Biểu diễn mối quan hệ vận tốc bàn kẹp không gian khớp vận tốc khớp đặt C Dùng chuyển trạng thái từ không giang khớp sang không gian công tác D Cả A C Câu 10: Bậc tự robot là: A Bất biến với tư B Luôn tổng số bậc tự mà tất khớp cánh tay tạo C Là đại lượng phụ thuộc vào tư D Luôn tổng số khâu hợp thành cánh tay Câu 11: Tầm với lớn tay máy đạt khi: A Không nghịch đảo ma trận Jacobian B Nghịch đảo ma trận Jacobian C Không phụ thuộc vào việc tính nghịch đảo ma trận Jacobian D Tất phương án sai 31 ... nghiên cứu động lực học robot ý kiến sau sai: A Xác định momen lực tác động xuất trình chuyển động B Xác định momen lực tác động xuất trình chuyển động phục vụ cho việc chọn công suất động C Xác... khớp Bài Cho Robot có khâu phẳng (Hình 2.7), cấu hình RRRRR Hãy thiết lập phương trình động lực học Robot cách xây dựng, tính tốn Matlab Mơ động học cấu khí 3D trênMatlab Simulink cho robot? (Với... Trọng lực có hướng theo trục y Chọn 1  biến khớp Ký hiệu động nối i Ki Pi (i=1,2) Xây dựng phương trình động lực học cho robot nối Plana (Hình 2.1) Bước 1: Tính động thạn nối * Tính động nối