Nâng cao chất lượng điều khiển cho robot Scara, chương 7 docx

Chương 7: Động lực học robot Scara Serpent Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và gia t ốc của đối tượng robot, cần phải xây dựng được phương trình động lực học, từ đó phục vụ cho công việc thiết kế và điều khiển robot. Vì vậy cần phải tính toán đầy đủ các thông số của đối tượng trước khi đưa vào mô phỏng. Phương tr ình động lực học của robot được biểu diễn như sau: H(q)q h(q,q) g(q) (t)     && & (2.27) Trong đó: - (t) - Vectơ [n x 1] lực động tạo nên ở n khớp động: (t) = [ 1 (t),  2 (t), ,  n (t)] T (2.28) - q(t) - Vectơ [n x 1] biến khớp: q(t) = [q 1 (t), q 2 (t), q n (t)] T . (2.29) - H(q) - Ma tr ận [n x n], có các phần tử H ik sau đây:   n T jk ij i ik i max( j,k ) H Trace U I U    . (j, k =1, 2, n). (2.30a) 0 i ij j T U q    (2.30b) - h( q , q  ) - Vectơ [n x 1] lực ly tâm và Coriolit: h( q , q  ) = [h 1 , h 2 , , h n ] T . (2.31) n n j jkm k m k 1 m 1 h h q q     && . (j =1, 2, n) . (2.32)     T ijiikmjkm UIUTraceh . (2.33) mk i 0 2 ikm qq T U    (2.34) - g(q) - Vectơ [n x 1] lực trọng trường: g(q) = [g 1 , g 2 , , g n ] T . (2.35)   n i j j ij i j g m gU r     . (2.36) V ới robot Scara Serpent gồm 4 chuyển động (3 chuyển động quay v à 1 chuyển động tịnh tiến) và mô phỏng với 3 chuyển động quay có các phương tr ình cụ thể sau: Vectơ [4 1] lực động: (t) = [ 1 (t),  2 (t),  3 (t),  4 (t)] T . Vectơ [41] biến khớp: q(t) = [q 1 (t), q 2 (t), d 3 (t), q 4 (t)] T . 2.4.1. Hàm Euler - Lagrange và các vấn đề động lực học Lagrange định nghĩa sự khác biệt giữa động năng và thế năng của hệ thống: L = K – P (2.37) Trong đó: K : là động năng của hệ thống. P : là tổng thế năng của hệ thống. Do đó phương tr ình động lực học được xác định bằng biểu thức: i i i d L L dt q q        & (2.38) Trong đó: q i =  i đối với khớp quay; q i = r i đối với khớp tịnh tiến.  i : mômen lực động tại khớp thứ i. Động năng của khớp thứ i: 2 2 i i i i i 1 1 K m .v J . 2 2    (2.39) V ới J i là mômen quán tính của khớp thứ i. 2.4.2. Động lực học robot Scara Serpent Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 2.4: Bảng 2.4: Thông số của robot Scara Serpent. Khớp 1 Khớp 2 Khớp 3 Khớp 4 Biến  1  2 d 3  4 Chiều dài a 1 a 2 d 3 0 Khối lượng m 1 m 2 m 3 m 4 Vận tốc v 1 v 2 v 3 v 4 Chiều dài tâm kh ối l g1 l g2 l g3 l g4 Giả sử khối lượng nằm ở đầu mút các thanh nối, ta có chiều dài tâm khối chính là chiều dài của thanh nối: l g1 = a 1 l g2 = a 2 l g3 = d 3 Ký hiệu chiều dài các khớp: a 1 = l 1 a 2 = l 2 d 3 = l 3 Hệ qui chiếu gắn với trục toạ độ (O 0 x 0 y 0 z 0 ) trên khớp thứ nhất. Khi đó mặt phẳng (O 0 x 0 y 0 ) là mặt phẳng đẳng thế. 2.4.2.1. Tính toán động năng và thế năng cho từng khớp  Khớp 1: 1 g1 1 1 g1 1 1 x l .cos θ y l .sin θ z 0         . 1 g1 1 1 . 1 g1 1 1 . 1 x l .sin θ .θ y l .cos θ .θ z 0            & & Trong đó: x 1 , y 1 , z 1 là hình chiếu của thanh nối số 1 lên các trục x, y, z. 2 2 2 1 1 1 1 2 2 1 g1 1 1 g1 1 1 2 2 2 1 1 g1 v x y z v ( l .sin θ .θ ) (l .cosθ .θ ) v l .θ        & & & & & & Trong đó: J 1 momen quán tính khớp 1. 1 2 2 1 1 1 1 1 . . 2 2 2 1 1 g1 1 1 1 P 0 1 1 K m .v J . 2 2 1 1 K m .l . θ J .θ 2 2       (2.40)  Khớp 2:         0z ) θθ.sin(lθ.sinly ) θθ.cos(lθ.coslx 2 21g2112 21g2112           0z ) θθ).(θθ.cos(lθ.θ.cosly ) θθ).(θθ.sin(lθ.θ.sinlx 2 2121g21112 2121g21112            221 2 1g21 2 21 2 g2 2 1 2 1 2 2 2 2121g2111 2 2121g2111 2 2 2 2 2 2 2 22 θ)cosθ.θθ.(.l2.l)θθ(.lθ.lv ) θθ).(θθ.cos(lθ.θ.cosl)θθ).(θθ.sin(lθ.θ.sinlv zyxv       2 2 2 2 2 2 2 . . 2 2 2 2 2 2 2 2 1 1 g2 1 2 1 g2 1 1 2 2 2 2 2 1 1 K m .v J . 2 2 1 1 K m . l . θ l .(θ θ ) 2.l .l .(θ θ .θ ).cosθ J .θ 2 2 P 0                 & & & & & (2.41)  Khớp 3:         33 212113 212113 -lz ) θ.sin(θl.sinθly ) θ.cos(θl.cosθlx . . . . 3 1 1 1 2 1 2 1 2 . . . . 3 1 1 1 2 1 2 1 2 . . 3 3 x l .sin θ .θ l .sin(θ θ ).(θ θ ) y l .cos θ .θ l .cos(θ θ ).(θ θ ) z l                   . 2 3 . 2 . 1 . 2 121 2 . 2 . 1 2 2 . 2 1 2 1 . 2 3 . 2 3 2 . 2 . 1212 . 111 2 . 2 . 1212 . 111 2 3 . 2 3 . 2 3 . 2 33 l)θ.θθ.(.l2.l)θθ(.lθ.lv l) θθ).(θθ.cos(lθ.θ.cosl)θθ).(θθ.sin(lθ.θ.sinlv zyxv                  . 2 3 3 3 . . . . . . . 2 2 2 2 2 2 3 3 1 1 2 1 2 1 2 1 1 2 3 3 3 3 3 3 3 1 K m .v 2 1 K m . l . θ l .(θ θ ) 2.l .l .(θ θ .θ ) l 2 P m .g.h P m .g.l                  (2.42)  Khớp 4:         34 212114 212114 -lz ) θ.sin(θl.sinθly ) θ.cos(θl.cosθlx           3 . 4 . 2 . 1212 . 111 . 4 . 2 . 1212 . 111 . 4 lz ) θθ).(θθ.cos(lθ.θ.cosly ) θθ).(θθ.sin(lθ.θ.sinlx  . . . 2 2 2 4 4 4 4 2 2 . . . . . . . 2 2 4 1 1 1 2 1 2 1 2 1 1 1 2 1 2 1 2 3 . . . . . . . . 2 2 2 2 2 2 2 4 1 1 2 1 2 1 2 1 1 2 3 v x y z v l .sin θ .θ l .sin(θ θ ).(θ θ ) l .cosθ .θ l .cos(θ θ ).( θ θ ) l v l .θ l .(θ θ ) 2.l .l .(θ θ .θ ) l                                . 2 2 4 4 4 4 4 . . . . . . . . . . 2 2 2 2 2 2 2 4 4 1 1 g2 1 2 1 g2 1 1 2 2 3 4 1 2 4 4 4 4 4 4 3 1 1 K m .v J .w 2 2 1 1 K m . l . θ l .(θ θ ) 2.l .l .(θ θ .θ )cosθ l J (θ θ θ ) 2 2 P m .g.h P m .g.l                      2.43 Tổng động năng của hệ thống: K = K 1 + K 2 + K 3 + K 4 (2.44) Đặt tâm khối tại đầu mút của thanh nối, ta có: l g1 = l 1 ; l g2 = l 2 ; l g3 = l 3 . 2 2 2 1 2 3 4 1 2 3 4 2 1 2 4 1 2 2 2 3 4 1 2 2 1 2 3 4 2 2 4 1 2 3 2 2 3 4 1 2 2 1 2 2 3 4 2 2 4 2 3 4 1 K (m m m m ).l (m m m ).l J J J . θ 2 (m m m ).l .l .cos θ .θ (m m m )l J J .θ .θ 1 (m m m ).l .l .cos θ .θ .θ (m m m ).l J J .θ 2 1 (m m ). 2                                            & & & & & & 2 2 3 4 4 4 1 4 4 2 4 1 l .J . θ J .θ .θ J .θ .θ 2    & & & & & & (2.45) T ổng thế năng của hệ thống: P = P 1 + P 3 + P 3 + P 4 = -(m 3 + m 4 ).g.l 3 (2.46) . từ đó phục vụ cho công việc thiết kế và điều khiển robot. Vì vậy cần phải tính toán đầy đủ các thông số của đối tượng trước khi đưa vào mô phỏng. Phương tr ình động lực học của robot được biểu. Chương 7: Động lực học robot Scara Serpent Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và gia t ốc của đối tượng robot, cần phải xây dựng được phương. quán tính của khớp thứ i. 2.4.2. Động lực học robot Scara Serpent Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 2.4: Bảng 2.4: Thông số của robot Scara Serpent. Khớp 1 Khớp 2 Khớp 3 Khớp