Robot song song P3

Chia sẻ: Tan Lang | Ngày: | Loại File: PDF | Số trang:35

0
101
lượt xem
38
download

Robot song song P3

Mô tả tài liệu
  Download Vui lòng tải xuống để xem tài liệu đầy đủ

Chương 3 : Phân tích động học Robot song song 3 RPS CHƯƠNG 3: PHÂN TÍCH ĐỘNG HỌC ROBOT SONG SONG 3RPS 3.1 Bài toán phân tích vị trí 3.1.1 Các phương trình liên kết cho robot song song 3 RPS tổng quát z B3 x3 y B2 z0 x2 α 2 P B1 x α3 z3 A3 α1 x0 x1 A1 z1 O y0 A2 z2 Hình 3.1 Do yêu cầu cỉa kết cấu Robot nên AiBi ⊥ Zi (các trục quay) O và P là trọng tâm của hai tam giác A1A2A3 và B1B2B3. Ta đặt các hệ tọa độ: {Ox0y0z0} : Hệ cố định. {Pxyz} : Hệ tọa độ...

Chủ đề:
Lưu

Nội dung Text: Robot song song P3

  1. Chương 3 : Phân tích động học Robot song song 3 RPS CHƯƠNG 3: PHÂN TÍCH ĐỘNG HỌC ROBOT SONG SONG 3RPS 3.1 Bài toán phân tích vị trí 3.1.1 Các phương trình liên kết cho robot song song 3 RPS tổng quát z B3 x3 P α3 B1 y B2 x z3 α1 z0 x2 α x1 2 A3 x0 A1 O z1 y0 z2 A2 Hình 3.1 Do yêu cầu cỉa kết cấu Robot nên AiBi ⊥ Zi (các trục quay) O và P là trọng tâm của hai tam giác A1A2A3 và B1B2B3. Ta đặt các hệ tọa độ: {Ox0y0z0} : Hệ cố định. {Pxyz} : Hệ tọa độ động gắn liền với bàn máy động. {Aixiyizi}(i=1,2,3) : Hệ động gắn với chân thứ i. uuuu r Trong đó xi ≡ Ai Bi và zi ≡ trục quay, còn yi xác định theo tam diện thuận (hay qui tắc bàn tay phải). Ta đưa thêm vào 3 tọa độ suy rộng α i (i=1,2,3) như hình vẽ. α i = z0 xi Sử dụng các ký hiệu: A RB : Ma trận cosin chỉ hướng của hệ {Pxyz} so với hệ cố định {Ox0y0z0}. A Ri : Ma trận cosin chỉ hướng của hệ {Aixiyizi} so với hệ cố định {Ox0y0z0}. -1-
  2. Chương 3 : Phân tích động học Robot song song 3 RPS ai : Vector đại số chứa các tọa độ của điểm Ai trên hệ cố định. b i : Vector đại số chứa các tọa độ của điểm Bi trên hệ cố định. B b i : Vector đại số chứa các tọa độ của điểm Bi trên hệ động. P: Vector đại số chứa các tọa độ của điểm P trên hệ cố định. di : Độ dài chân thứ i. Trong đó : Các ma trận A R i có thể biểu diễn dưới dạng: r r r r r r ⎡ e01ei1 e01ei 2 e01ei 3 ⎤ R i = ⎢ e02ei1 e02ei 2 e02ei 3 ⎥ A r r r r r r (i=1,2,3) (3.1) ⎢r r r r r r ⎥ ⎢ e03ei1 e03ei 2 e03ei 3 ⎥ ⎣ ⎦ r r r e01 , e02 , e03 : Là 3 vector đơn vị trên các trục Ox0, Oy0,Oz0. r r r ei1 , ei 2 , ei 3 : Là 3 vector đơn vị trên các trục Aixi, Aiyi, Aizi (i=1,2,3). Các phần tử của ma trận này tùy theo kết cấu của bàn đế cố định, là hàm của góc α i . Ma trận A R B có thể biểu diễn dưới dạng 3 phép quay Roll, Pitch, Yaw tương ứng với 3 góc ϕ ,θ và ψ . ai và B b i : Xác định được từ hình dáng, kết cấu của Robot. Với cách đặt và biểu diễn các đại lượng như trên, vị trí của điểm Bi trên hệ cố định có thể biểu diễn dưới dạng: uuur uuur uuuu r OBi = OAi + Ai Bi (i=1,2,3) (3.2) uuur uuu uuur r và : OBi = OP + PBi (i=1,2,3) (3.3) Hay dưới dạng đại số: ⎡ di ⎤ bi = ai + R i . ⎢ 0 ⎥ A (i=1,2,3) (3.4) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ và : b i = P + A R B .B b i (i=1,2,3) (3.5) Kết hợp hai phương trình trên ta có: -2-
  3. Chương 3 : Phân tích động học Robot song song 3 RPS ⎡ di ⎤ P + R B . b i = ai + R i . ⎢ 0 ⎥ A B A (i=1,2,3) (3.6) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ Trong đó: T B T T P = ⎡ p1, p2 , p3 ⎤ ; ⎣ ⎦ b i = ⎡bix , biy , biz ⎤ ; a i = ⎡ ai1, ai 2 , ai 3 ⎤ ⎣ ⎦ ⎣ ⎦ ⎡ux vx wx ⎤ ⎡uix vix wix ⎤ A R B = ⎢u y vy wy ⎥ ; A R i = ⎢uiy viy wiy ⎥ (i=1,2,3) (3.7) ⎢ ⎥ ⎢ ⎥ ⎢uz ⎣ vz wz ⎥ ⎦ ⎢uiz ⎣ viz wiz ⎥ ⎦ A3 Z3 β3 O β2 Z2 A1 β1 X0 A2 Z1 Y0 Hình 3.2 Các ma trận cosin chỉ hướng: A R1 = A z (π / 2− β1 ) .Aα 1 A R 2 = A z (2π / 3) .A z (π / 2− β2 ) .Aα 2 (3.8) A R 3 = A z ( −2π / 3) .A z (π / 2− β3 ) .Aα 3 Với : A zϕ là ma trận cosin chỉ hướng của phép quay quanh trục z một góc ϕ. -3-
  4. Chương 3 : Phân tích động học Robot song song 3 RPS Aα i = A x (π / 2) .A z (π / 2+α i ) ⎡1 0 0 ⎤ ⎡ − sin α i − cos α i 0⎤ Aα i = ⎢ 0 0 −1⎥ . ⎢ cos α i − sin α i 0⎥ ⎢ ⎥⎢ ⎥ ⎢0 1 0 ⎥ ⎢ 0 ⎣ ⎦⎣ 0 ⎥ 1⎦ ⎡ − sin α i − cos α i 0⎤ Aα i = ⎢ 0 0 1⎥ (3.9) ⎢ ⎥ ⎢ cos α i ⎣ − sin α i 0⎥ ⎦ Nếu ta đặt : (π / 2 − β i ) = γ i ; Vậy ta có : A R1 = A z (γ 1 ).Aα 1 ⎡cos γ 1 − sin γ 1 0 ⎤ ⎡ − sin α1 − cos α1 0 ⎤ A R1 = ⎢ sin γ 1 cos γ 1 0 ⎥ ⎢ 0 0 1⎥ ⎢ ⎥⎢ ⎥ ⎢ 0 ⎣ 0 1 ⎥ ⎢ cos α1 − sin α1 0 ⎥ ⎦⎣ ⎦ ⎡ − cos γ 1 sin α1 cos γ 1 cos α1 − sin γ 1 ⎤ A R1 = ⎢ − sin γ 1 sin α1 sin γ 1 cos α1 cos γ 1 ⎥ ⎢ ⎥ ⎢ cos α1 ⎣ sin α1 0 ⎥ ⎦ A R 2 = A z (2π / 3).A z (γ 2 ).Aα 2 ⎡ −1/ 2 − 3 / 2 0 ⎤ ⎡ cos γ − sin γ 2 0 ⎤ ⎡ − sin α 2 − cos α 2 0⎤ ⎢ ⎥⎢ 2 A R 2 = ⎢ 3 / 2 −1/ 2 0 ⎥ sin γ 2 cos γ 2 0⎥ ⎢ 0 0 1⎥ ⎢ ⎥⎢ ⎥ ⎢ 0 0 ⎥⎢ 0 1⎥ ⎣ 0 1 ⎥ ⎢ cos α 2 − sin α 2 0⎥ ⎢ ⎣ ⎦ ⎦⎣ ⎦ -4-
  5. Chương 3 : Phân tích động học Robot song song 3 RPS A R2 = ⎡ 1 3 1 3 1 3 ⎤ ⎢ ( cos γ 2 + sin γ 2 )sin α 2 −( cos γ 2 + sin γ 2 )cos α 2 sin γ 2 − cos γ 2 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ 3 1 3 1 3 1 ⎥ ⎢(− cos γ 2 + sin γ 2 )sin α 2 (− cos γ 2 + sin γ 2 )cos α 2 − sin γ 2 − cos γ 2 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ cos α 2 sin α 2 0 ⎥ ⎢ ⎥ ⎣ ⎦ A R 3 = A z (2π / 3).A z (γ 3 ).Aα 3 ⎡ −1/ 2 3 / 2 0 ⎤ ⎡ cos γ 3 − sin γ 3 0 ⎤ ⎡ − sin α 3 − cos α 3 0⎤ ⎢ ⎥ A R3 = ⎢− 3 / 2 −1/ 2 0 ⎥ ⎢ sin γ 3 cos γ 3 0⎥ ⎢ 0 0 1⎥ ⎢ ⎥⎢ ⎥ ⎢ 0 0 ⎥⎢ 0 1⎥ ⎣ 0 1 ⎥ ⎢ cos α 3 − sin α 3 0⎥ ⎢ ⎣ ⎦ ⎦⎣ ⎦ A R3 = ⎡ 1 3 1 3 1 3 ⎤ ⎢ ( cos γ 3 − sin γ 3 )sin α 3 ( − cos γ 3 + sin γ 3 )cos α 3 sin γ 3 + cos γ 3 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ 3 1 3 1 3 1 ⎥ ⎢ ( cos γ 3 + sin γ 3 )sin α 3 (− cos γ 3 − sin γ 3 )cos α 3 sin γ 3 − cos γ 3 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ cos α 3 sin α 3 0 ⎥ ⎢ ⎥ ⎣ ⎦ Ta thấy các thành phần của các ma trận A R i chỉ chứa các thành phần liên quan đến góc α i và góc β i . Ta viết lại phương trình (3.6) dưới dạng đại số. Chú ý: Do Ai thuộc mặt phẳng X0Y0 nên ai 3 = 0 (i=1,2,3) A1 trên trục X0 nên a12 = 0 Và Bi thuộc mặt phẳng X0Y0 nên biz = 0 (i=1,2,3) +Với i =1: -5-
  6. Chương 3 : Phân tích động học Robot song song 3 RPS ⎧ p1 + u xb1x = a11 + u1x .d1 ⎪ ⎨ p2 + u y b1x = u1 y d1 ⎪ ⎩ p3 + u z b1x = u1z d1 ⎧ a11 + u1x d1 − p1 ⎪u x = b1x ⎪ ⎪ u d − p2 ⇒ ⎨u y = 1 y 1 (3.10) ⎪ b1x ⎪ u d − p3 ⎪u z = 1z 1 ⎩ b1x +Với i=2 ⎧ p1 + u xb2 x + vxb2 y = a21 + u2 x d 2 (a) ⎪ ⎨ p2 + u yb2 x + v yb2 y = a22 + u2 y d 2 (b) (3.11) ⎪ ⎩ p3 + u z b2 x + vz b2 y = u2 z d 2 (c) +Với i=3 ⎧ p1 + u xb3 x + vxb3 y = a31 + u3 x d3 (a) ⎪ ⎨ p2 + u yb3 x + v yb3 y = a32 + u3 y d3 (b) (3.12) ⎪ ⎩ p3 + u z b3 x + vz b3 y = u3 z d3 (c) Ta thực hiện các phép biến đổi sau: ⎧((2.51a)λ − (2.52a )) ⎪ ⎨((2.51b)λ − (2.52b)) ⎪((2.51c)λ − (2.52c)) ⎩ ⎧ p1 (λ − 1) + u x λ1 = λ (a21 + u2 x d 2 ) − (a31 + u3 x d3 ) ⎪ ⇔ ⎨ p2 (λ − 1) + u y λ1 = λ (a22 + u2 y d 2 ) − (a32 + u3 y d3 ) (3.13) ⎪ ⎩ p3 (λ − 1) + u z λ1 = λu2 z d 2 − u3 z d3 ) b3 y Với λ = , λ1 = b2 x λ − b3 x b2 y Thay các kết quả của hệ (3.10) vào hệ (3.13) ta được: -6-
  7. Chương 3 : Phân tích động học Robot song song 3 RPS ⎧ a11 + u1x d1 − p1 ⎪ p1 (λ − 1) + b1x λ1 = λ (a21 + u2 x d 2 ) − (a31 + u3 x d3 ) ⎪ ⎪ u1 y d1 − p2 ⎨ p2 (λ − 1) + λ1 = λ (a22 + u2 y d 2 ) − (a32 + u3 y d3 ) (3.14) ⎪ b1x ⎪ u d − p3 ⎪ p3 (λ − 1) + 1z 1 λ1 = λu2 z d 2 − u3 z d3 ) ⎩ b1x Mặt khác, dựa vào kết cấu của bàn di động B ta có : B3 b2 b1 B1 b3 B2 Hình 3.3 uuuur 2 B1B2 = (b1 − b 2 )T (b1 − b 2 ) =b32 uuuur 2 B1B3 = (b1 − b3 )T (b1 − b 3 ) = b22 uuuur 2 B2 B3 = (b 2 − b3 )T (b 2 − b3 ) =b12 ⎡ di ⎤ với : b i = a i + R i . ⎢ 0 ⎥ A (i=1,2,3) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ ⎡ a11 + u1x d1 ⎤ ⎡ a21 + u2 x d 2 ⎤ ⎡ a31 + u3 x d3 ⎤ ⇒ b1 = ⎢ u1 y d1 ⎥ ; b 2 = ⎢ a22 + u2 y d 2 ⎥ ; b 3 = ⎢ a32 + u3 y d3 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ u1z d1 ⎥ ⎣ ⎦ ⎢ u2 z d 2 ⎥ ⎣ ⎦ ⎢ u3 z d3 ⎥ ⎣ ⎦ T ⎡ a11 − a21 + u1x d1 − u2 x d 2 ⎤ ⎡ a11 − a21 + u1x d1 − u2 x d 2 ⎤ ⇒ ⎢ u1 y d1 − a22 − u2 y d 2 ⎥ . ⎢ u1 y d1 − a22 − u2 y d 2 ⎥ = b32 ⎢ ⎥ ⎢ ⎥ ⎢ ⎣ u1z − u2 z d 2 ⎥ ⎦ ⎢ ⎣ u1z − u2 z d 2 ⎥ ⎦ -7-
  8. Chương 3 : Phân tích động học Robot song song 3 RPS T ⎡ a11 − a31 + u1x d1 − u3 x d3 ⎤ ⎡ a11 − a31 + u1x d1 − u3 x d3 ⎤ ⎢ u d −a −u d ⎥ . ⎢ u1 y d1 − a32 − u3 y d3 ⎥ = b2 2 ⎢ 1 y 1 32 3 y 3 ⎥ ⎢ ⎥ ⎢ ⎣ u1z d1 − u3 z d3 ⎥ ⎦ ⎢ ⎣ u1z d1 − u3 z d3 ⎥ ⎦ T ⎡ a21 − a31 + u2 x d 2 − u3 x d3 ⎤ ⎡ a21 − a31 + u2 x d 2 − u3 x d3 ⎤ ⎢a − a + u d − u d ⎥ . ⎢ a22 − a32 + u2 y d 2 − u3 y d3 ⎥ = b12 ⎢ 22 32 2y 2 3y 3 ⎥ ⎢ ⎥ ⎢ ⎣ u2 z d 2 − u3 z d3 ⎥ ⎦ ⎢ ⎣ u2 z d 2 − u3 z d3 ⎥ ⎦ Hay : ⎧(a11 − a21 + u1x d1 − u2 x d 2 ) 2 + (u1 y d1 − a22 − u2 y d 2 ) 2 + (u1z d1 − u2 z d 2 ) 2 = b32 ⎪ ⎪ ⎨(a11 − a31 + u1x d1 − u3 x d3 ) + (u1 y d1 − a32 − u3 y d3 ) + (u1z d1 − u3 z d3 ) = b2 2 2 2 2 ⎪ ⎪(a21 − a31 + u2 x d 2 − u3 x d3 ) + (a22 − a32 + u2 y d 2 − u3 y d3 ) + (u2 z d 2 − u3 z d3 ) = b1 2 2 2 2 ⎩ Kết hợp với hệ (2.13) ta có hệ 6 phương trình, 6 ẩn: ⎧ a + u d − p1 ⎪ p1 (λ − 1) + 11 1x 1 λ1 = λ (a21 + u2 x d 2 ) − (a31 + u3 x d3 ) b1x ⎪ ⎪ u1 y d1 − p2 ⎪ p2 (λ − 1) + λ1 = λ (a22 + u2 y d 2 ) − (a32 + u3 y d3 ) ⎪ b1x ⎪ u d − p3 ⎨ p3 (λ − 1) + 1z 1 λ1 = λu2 z d 2 − u3 z d3 ) (3.15) ⎪ b1x ⎪ ⎪( a11 − a21 + u1x d1 − u2 x d 2 ) + (u1 y d1 − a22 − u2 y d 2 ) + (u1z d1 − u2 z d 2 ) = b3 2 2 2 2 ⎪( a − a + u d − u d ) 2 + (u d − a − u d ) 2 + (u d − u d ) 2 = b 2 ⎪ 11 31 1x 1 3 x 3 1y 1 32 3y 3 1z 1 3z 3 2 ⎪( a − a + u d − u d ) 2 + (a − a + u d − u d ) 2 + (u d − u d ) 2 = b 2 ⎩ 21 31 2 x 2 3 x 3 22 32 2y 2 3y 3 2z 2 3z 3 1 Hệ phương trình (3.15) chứa 9 ẩn số α1 ,α 2 ,α 3 , d1 , d 2 , d 3 , p1 , p2 , p3 . Các thành phần uix , uiy , uiz đã xác định được, các thành phần u x , u y , u z xác định theo (3.10) Khi giải quyết bài toán động học thuận hay ngược, ta biết trước được 3 ẩn. Công việc còn lại chỉ phải giải hệ 6 phương trình 6 ẩn số. 3.1.2 Bài toán động học thuận -8-
  9. Chương 3 : Phân tích động học Robot song song 3 RPS Bài toán động học thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị trí của bàn máy động P và ma trận ARB. Theo phần trên ta thay các giá trị di (i=1,2,3) và hệ (2.54), ta sẽ được hệ 6 phương trình với 6 ẩn là : α1 ,α 2 ,α 3 , p1 , p2 , p3 Chú ý là 3 phương trình sau của hệ (3.15) chỉ chứa di và α i nên việc giải 6 phương trình được đơn giản lại còn giải hệ 3 phương trình với 3 ẩn là α i . Sau đó thay các giá trị của di và α i vào 3 phương trình đầu ta sẽ tính được các giá trị của P. Các giá trị còn lại tính được bằng cách thay trực tiếp vào các phương trình (3.10), (3.11), (3.12). -9-
  10. Chương 3 : Phân tích động học Robot song song 3 RPS 3.1.3 Bài toán động học ngược Bài toán động học ngược là bài toán biết vị trí bàn máy động P, ta phải tìm độ dài các chân di (i=1,2,3) và các góc α i (i=1,2,3) . Tương tự như cách làm đối với bài toán động học thuận ta thay các giá trị P và hệ (3.15), ta sẽ được hệ 6 phương trình với 6 ẩn là : α1 ,α 2 ,α 3 , d1 , d 2 , d3 . Các giá trị còn lại tính được bằng cách thay trực tiếp vào các phương trình (3.10), (3.11), (3.12). 3.1.4 Tính toán vị trí cho một robot song song 3 RPS cụ thể Ta tính toán cho một robot song song 3 RPS cụ thể : - Tam giác A1A2A3 và tam giác B1B2B3 là các tam giác đều. - PB1 = h; OA1 = g; - Do kết cấu của cơ cấu ta có zi ⊥ Ai Bi - Trục zi ⊥ OAi ⇒ β i = π / 2 Khi đó các đại lượng trong công thức (3.6) trở thành : ⎡ h ⎤ ⎡ h ⎤ ⎢ −2 ⎥ ⎢ −2 ⎥ ⎡h ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ 3⎥ ⎢ 3⎥ B b1 = ⎢ 0 ⎥ ; B b2 = ⎢h ; B b3 = ⎢ − h ⎥ (3.16) ⎢ ⎥ 2 ⎥ 2 ⎢0 ⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ ⎣ ⎥ ⎦ ⎢ ⎣ ⎥ ⎦ ⎡ g ⎤ ⎡ g ⎤ ⎢ −2 ⎥ ⎢ −2 ⎥ ⎡g⎤ ⎢ ⎥ ⎢ ⎥ ⎢ 3⎥ ⎢ 3⎥ a1 = ⎢ 0 ⎥ ; a2 = ⎢ g ; a3 = ⎢− g (3.17) ⎢ ⎥ 2 ⎥ 2 ⎥ ⎢0⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ ⎣ ⎥ ⎦ ⎢ ⎣ ⎥ ⎦ Do β i = π / 2 nên γ i = (π / 2 − β i ) = 0 Khi đó các ma trận cosin chỉ hướng ARi trở thành: - 10 -
  11. Chương 3 : Phân tích động học Robot song song 3 RPS ⎡ − cos γ 1 sin α1 cos γ 1 cosα1 − sin γ 1 ⎤ A R1 = ⎢ − sin γ 1 sin α1 sin γ 1 cos α1 cos γ 1 ⎥ ⎢ ⎥ ⎢ cos α1 ⎣ sin α1 0 ⎥ ⎦ ⎡ − sin α1 cos α1 0 ⎤ A R1 = ⎢ 0 0 1⎥ (3.18) ⎢ ⎥ ⎢ cos α1 sin α1 0 ⎥ ⎣ ⎦ A R2 = ⎡ 1 3 1 3 1 3 ⎤ ⎢ ( cos γ 2 + sin γ 2 )sin α 2 −( cos γ 2 + sin γ 2 )cos α 2 sin γ 2 − cos γ 2 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ 3 1 3 1 3 1 ⎥ ⎢(− cos γ 2 + sin γ 2 )sin α 2 ( cos γ 2 − sin γ 2 )cos α 2 − sin γ 2 − cos γ 2 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ cos α 2 sin α 2 0 ⎥ ⎢ ⎥ ⎣ ⎦ ⎡ 1 1 3⎤ ⎢ sin α 2 − cos α 2 − ⎥ ⎢ 2 2 2 ⎥ ⎢ 3 3 1 ⎥ A R 2 = ⎢− sin α 2 cos α 2 − ⎥ (3.19) ⎢ 2 2 2 ⎥ ⎢ cos α 2 sin α 2 0 ⎥ ⎢ ⎥ ⎣ ⎦ A R3 = ⎡ 1 3 1 3 1 3 ⎤ ⎢ ( cos γ 3 − sin γ 3 )sin α 3 (− cos γ 3 + sin γ 3 )cosα 3 sin γ 3 + cos γ 3 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ 3 1 3 1 3 1 ⎥ ⎢ ( cos γ 3 + sin γ 3 )sin α 3 (− cos γ 3 − sin γ 3 )cos α 3 sin γ 3 − cos γ 3 ⎥ ⎢ 2 2 2 2 2 2 ⎥ ⎢ cos α 3 sin α 3 0 ⎥ ⎢ ⎥ ⎣ ⎦ ⎡ 1 1 3⎤ ⎢ sin α 3 − cos α 3 ⎥ ⎢ 2 2 2 ⎥ ⎢ 3 3 1⎥ A R 3 = ⎢ sin α 3 − cos α 3 − ⎥ (3.20) ⎢ 2 2 2⎥ ⎢ cos α 3 sin α 3 0 ⎥ ⎢ ⎥ ⎣ ⎦ - 11 -
  12. Chương 3 : Phân tích động học Robot song song 3 RPS b3 y Khi đó : λ = = -1 ; λ1 = b2 x λ − b3 x = h; b2 y Thay vào hệ (3.15) ta được : ⎧ 1 ⎪3 p1 = (d 2 sin α 2 + d3 sin α 3 ) − d1 sin α1 2 ⎪ ⎪ − 3 ⎪3 p2 = 2 (d 2 sin α 2 − d3 sin α 3 ) ⎪ ⎨3 p3 = d1 cos α1 + d 2 cos α 2 + d3 cosα 3 (3.21) ⎪ 2 ⎪3g − 3 gd1 sin α1 − 3 gd 2 sin α 2 + d1d 2 sin α1 sin α 2 + d1 + d 2 − 2d1d 2 cos α1 cos α 2 = 3h 2 2 2 ⎪ 2 ⎪3g − 3 gd1 sin α1 − 3 gd3 sin α 3 + d1d3 sin α1 sin α 3 + d1 + d3 − 2d1d3 cos α1 cos α 3 = 3h 2 2 2 ⎪3g 2 − 3 gd sin α − 3 gd sin α + d d sin α sin α + d 2 + d 2 − 2d d cos α cosα = 3h 2 ⎩ 2 2 3 3 2 3 2 3 2 3 2 3 2 3 a) Bài toán động học thuận Bài toán động học thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị trí của bàn máy động P và ma trận ARB. Theo phần trên ta thay các giá trị di (i=1,2,3) vào hệ (3.21), ta sẽ được hệ 6 phương trình với 6 ẩn là : α1 ,α 2 ,α 3 , p1 , p2 , p3 Chú ý là 3 phương trình sau của hệ (3.21) chỉ chứa di và α i nên việc giải 6 phương trình được đơn giản lại còn giải hệ 3 phương trình với 3 ẩn là α i . Sau đó thay các giá trị của di và α i vào 3 phương trình đầu ta sẽ tính được các giá trị của P Các giá trị còn lại tính được bằng cách thay trực tiếp vào các phương trình (3.10), (3.11), (3.12). b) Bài toán động học ngược Bài toán động học ngược là bài toán biết vị trí bàn máy động P, ta phải tìm độ dài các chân di (i=1,2,3) và các góc α i (i=1,2,3) . Tương tự như cách làm đối với bài toán động học thuận ta thay các giá trị P và hệ (3.21), ta sẽ được hệ 6 phương trình với 6 ẩn là : α1 ,α 2 ,α 3 , d1 , d 2 , d3 . Các giá trị còn lại tính được bằng cách thay trực tiếp vào các phương trình (3.10), (3.11), (3.12). - 12 -
  13. Chương 3 : Phân tích động học Robot song song 3 RPS 3.2 Bài toán phân tích Jacobi 3.2.1 Ma trận Jacobi của robot song song không gian Trong phần trước ta đã xây dựng được các điều kiện ràng buộc động học của cơ cấu, các điều kiện này có dạng tổng quát: f (x, p) = 0 (3.22) Trong đó: p là biến khớp tác động. x đặc trưng vị trí bệ chuyển động. f là hàm ẩn n chiều theo p và x ; 0 là vector n zero n chiều. Đạo hàm (3.22) theo thời gian ta có: ∂f ∂f x= p & & (3.23) ∂x ∂p ∂f ∂f Đặt : J x = và J P = ∂x ∂p Ta có: J x x = J Pp & & (3.24) Từ đó ta có: p = J −1.J x x & P & (3.25) Hoặc: p = J1x với J1 = J −1.J x & & P (3.26) Và x = J −1.J P .p & x & (3.27) x = J 2 .p với J 2 = J −1.J P & & x (3.28) Trong đó J1 , J 2 là các ma trận Jacobi ứng với 2 trạng thái động học thuận và động học ngược. Các điều kiện đặc biệt Với sự tồn tại hai ma trận Jacobi, cơ cấu chấp hành song song có cấu hình đặc biệt khi J x , J P hoặc cả hai ở trạng thái đặc biệt, do đó có thể tìm được ba kiểu trạng thái đặc biệt. - Trạng thái đặc biệt động học đảo Trạng thái này xảy ra khi định thức của J P tiến đến zero det(J p ) = 0 (3.29) Khi đó tồn tại các vector p khác zero dẫn đến kết qủa vector x bằng zero. & & Tức là chuyển động vi phân của bệ di động theo một số chiều không thể thực hiện, cơ cấu chấp hành bị ràng buộc lại và mất đi một số bậc tự do. Trạng - 13 -
  14. Chương 3 : Phân tích động học Robot song song 3 RPS thái đặc biệt động học đảo thường xảy ra ở biên không gian hoạt động của cơ cấu chấp hành. - Trạng thái đạc biệt động học thuận Trạng thái đặc biệt động học thuận xảy ra khi định thức của J x bằng zero det(J x ) = 0 (3.30) Khi đó tồn tại các vector x khác zero dẫn đến kết qủa vector p bằng zero. & & Trong trường hợp này bệ di động có thể có chuyển động vi phân theo một số hướng, còn mọi bộ tác động đều bị khoá. Tức là hệ sẽ tăng lên một số bậc tự do. - Trạng thái đặc biệt hỗn hợp Trạng thái đặc biệt hỗn hợp xảy ra khi cả hai định thức của J x và J P đều bằng zero. 3.2.2 Phân loại bài toán a) Bài toán động học ngược r r Biết vận tốc góc ω B (hoặc vận tốc điểm P vP ) của bàn máy động, ta cần & xác định vận tốc của các khâu dẫn v = d (i=1,2,3) i i b)Bài toán động học thuận & r Biết vi = di , ta cần xác định vận tốc góc wB của bàn máy động và vận tốc r điểm P vP . 3.2.3 Phân tích Jacobi robot song song 3 RPS tổng quát z B3 x3 P α3 B1 y B2 x z3 α1 z0 x2 α x1 2 A3 x0 A1 O z1 y0 z2 A2 Hình 3.4 - 14 -
  15. Chương 3 : Phân tích động học Robot song song 3 RPS Từ công thức (3.6) : ⎡ di ⎤ P + R B . b i = ai + R i . ⎢ 0 ⎥ A B A (i=1,2,3) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ Đạo hàm công thức (3.6) theo thời gian ta có: ⎡ di ⎤ & ⎡ di ⎤ ⎢ ⎥ P + A R B .B b i = a i + R i . ⎢ 0 ⎥ + R i . ⎢ 0 ⎥ & & & & (i=1,2,3) (3.31) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ ⎢0⎥ ⎣ ⎦ Do ai là hắng số nên ai =0. & Từ phương trình (2.58) ta có: & % & R i .R T = ωi ⇒ R i = ωi .R i % (3.32) i và : A & R B = ω.A R B % Trong đó: ⎡ 0 −ωi 3 ωi 2 ⎤ ωi = ⎢ ωi 3 % 0 −ωi1 ⎥ (i=1,2,3) ⎢ ⎥ ⎢ −ωi 2 ⎣ ωi1 0 ⎥ ⎦ ωi1 , ωi 2 , ωi 3 là các thành phần vận tốc góc của chân thứ i so với hệ cố định. ⎡ 0 −ω3 ω2 ⎤ ω = ⎢ ω3 % 0 −ω1 ⎥ ⎢ ⎥ ⎢ −ω2 ⎣ ω1 0 ⎥ ⎦ ω1 , ω2 , ω3 là các thành phần vận tốc góc của bàn máy động so với hệ cố định. r Chú ý: Do vector vận tốc góc ωi hướng theo các trục Zi tương ứng nên hình chiếu của nó lên các trục X0,Y0,Z0 cho ta: ωi 3 = 0 . ⎡ 0 0 ωi 2 ⎤ Vậy ωi = % ⎢ 0 0 −ωi1 ⎥ (i=1,2,3) ⎢ ⎥ ⎢ −ωi 2 ωi1 ⎣ 0 ⎥ ⎦ Nếu biết ωi1 (hoặc ωi 2 ) ta có thể xác định thành phần còn lại từ các tham số hình học của hệ. - 15 -
  16. Chương 3 : Phân tích động học Robot song song 3 RPS Phương trình (2.76) được viết dưới dạng: ⎡ di ⎤ & ⎡ di ⎤ ⎢ ⎥ P + ω.A R B .B b i = ω.R i . ⎢ 0 ⎥ + R i . ⎢ 0 ⎥ & % % với i=(1,2,3) (3.31) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ ⎢0⎥ ⎣ ⎦ Khai triển (3.31) ta có 9 phương trình, các đại lượng đã biết từ bài toán vị trí và tham số hình học của robot là: di, B b i , R i , A R B (i=1,2,3) & - Bài toán phân tích Jacobi thuận : Biết d i (i=1,2,3) ta có 9 phương trình đại số tuyến tính để giải 9 ẩn : p1 , p2 , p3 , ω1 , ω2 , ω3 , ω11 , ω21 , ω31 & & & - Bài toán phân tích Jacobi ngược : Biết P = [ p1 , p2 , p3 ] hoặc w ta có 9 & & & & T & & & & phương trình đại số tuyến tính để xác định 9 ẩn : d , d , d , ω , ω , ω (hoặc P 1 2 3 1 2 3 ) , ω11 , ω21 , ω31 . Ma trận Jacobi được mô tả trong mục (3.2.1) sẽ được xác định khi sắp xếp lại các số hạng của phương trình (3.31). 3.2.4 Phân tích Jacobi một robot song song 3 RPS cụ thể Đạo hàm phương trình (3.21) ta được hệ : - 16 -
  17. Chương 3 : Phân tích động học Robot song song 3 RPS ⎧ 1 & & & ⎪3 p1 = 2 ⎡ d 2 sin α 2 + α 2 d 2 cos α 2 + d3 sin α 3 + α 3d3 cos α 3 ⎤ − d1 sin α1 − α1d1 cos α1 & ⎣ & & ⎦ & ⎪ ⎪ & − 3 & & 3 p2 = ⎡ d 2 sinα 2 + α 2 d 2cosα 2 − d3 sinα 3 − α 3d3 cos α 3 ⎤ & & ⎪ 2 ⎣ ⎦ ⎪ & & & & & & ⎪3 p3 = d1 cos α1 − α1d1 sin α1 + d 2 cos α 2 − α 2 d 2 sin α 2 + d3 cos α 3 − α 3d3 sin α 3 & ⎪& ⎪d1 [ −3 g sin α1 + d 2 sin α1 sin α 2 + 2d1 − 2d 2 cos α1 cos α 2 ] + d 2 [ −3 g sin α 2 + & ⎪d sin α sin α + 2d − 2d cos α cos α ] + α [ −3 gd cosα + + d d cosα sin α &1 ⎪ 1 ⎪ 1 2 2 1 1 2 1 1 1 2 1 2 ⎨+2d1d 2 sin α1 cos α 2 ] + α 2 [ −3 gd 2 cos α 2 + d1d 2 sin α1 cos α 2 + 2d1d 2 cos α1 sin α 2 ] = 0 & ⎪ ⎪d1 [ −3g sin α1 + d3 sin α1 sin α 3 + 2d1 − 2d3 cos α1 cos α 3 ] + d3 [ −3 g sin α 3 + & & (3.32) ⎪ ⎪d1 sin α1 sin α 3 + 2d3 − 2d1 cos α1 cosα 3 ] + α1 [ −3gd1 cosα1 + d1d3 cosα1 sin α 3 + & ⎪+ 2d d sin α cos α ] + α [ −3gd cos α + d d sin α cos α + 2d d cos α sin α ] = 0 &3 ⎪ 1 3 1 3 3 3 1 3 1 3 1 3 1 3 ⎪d 2 [ −3 g sin α 2 + d3 sin α 2 sin α 3 + 2d 2 − 2d3 cos α 2 cos α 3 ] + d3 [ −3g sin α 3 + & & ⎪ ⎪d 2 sin α 2 sin α 3 + 2d3 − 2d 2 cos α 2 cos α 3 ] + α 2 [ −3 gd 2 cos α 2 + d 2 d3 cos α 2 sin α 3 + & ⎪ ⎪2d 2 d3 sin α 2 cos α 3 ] + α 3 [ −3 gd3 cos α 3 + d 2 d3 sin α 2 cos α 3 + 2d 2 d3 cos α 2 sin α 3 ] = 0 ⎩ & Do các đại lượng về vị trí đã tính được ở bài toán vị trí nên hệ (3.32) là & & & một hệ phương trình tuyến tính với pi , di ,α i (i=1,2,3). Ta đặt: a1 = −3 g sin α1 + d 2 sin α1 sin α 2 + 2d1 − 2d 2 cos α1 cos α 2 b1 = −3 g sin α 2 + d1 sin α1 sin α 2 + 2d 2 − 2d1 cos α1 cos α 2 e1 = −3gd1 cos α1 + d1d 2 cos α1 sin α 2 + 2d1d 2 sin α1 cos α 2 f1 = −3 gd 2 cos α 2 + d1d 2 sin α1 cos α 2 + 2d1d 2 cos α1 sin α 2 a2 = −3 g sin α1 + d3 sin α1 sin α 3 + 2d1 − 2d3 cos α1 cos α 3 c2 = −3 g sin α 3 + d1 sin α1 sin α 3 + 2d3 − 2d1 cos α1 cos α 3 e2 = −3 gd1 cos α1 + d1d3 cos α1 sin α 3 + 2d1d3 sin α1 cos α 3 g 2 = −3 gd3 cos α 3 + d1d3 sin α1 cos α 3 + 2d1d3 cos α1 sin α 3 b3 = −3 g sin α 2 + d3 sin α 2 sin α 3 + 2d 2 − 2d3 cos α 2 cos α 3 c3 = −3 g sin α 3 + d 2 sin α 2 sin α 3 + 2d3 − 2d 2 cos α 2 cos α 3 f3 = −3 gd 2 cos α 2 + d 2 d3 cos α 2 sin α 3 + 2d 2 d3 sin α 2 cos α 3 g3 = −3 gd3 cos α 3 + d 2 d3 sin α 2 cos α 3 + 2d 2 d3 cos α 2 sin α 3 Vậy hệ (3.32) có thể viết lại thành: - 17 -
  18. Chương 3 : Phân tích động học Robot song song 3 RPS ⎧ & 1⎡& & & ⎪3 p1 = 2 ⎣ d 2 sin α 2 + α 2 d 2 cos α 2 + d3 sin α 3 + α 3d3 cos α 3 ⎤ − d1 sin α1 − α1d1 cos α1 & & ⎦ & ⎪ ⎪ & − 3 & & ⎡ ⎤ ⎪3 p2 = 2 ⎣ d 2 sinα 2 + α 2 d 2cosα 2 − d3 sinα 3 − α 3d3 cos α 3 ⎦ & & ⎪ ⎨3 p3 = d1 cos α1 − α1d1 sin α1 + d 2 cos α 2 − α 2 d 2 sin α 2 + d3 cos α 3 − α 3d3 sin α 3 (3.33) & & & & & & & ⎪ & & ⎪d1a1 + d 2b1 + α1e1 + α 2 f1 = 0 & & ⎪& & ⎪d1a2 + d3c2 + α1e2 + α 3 g 2 = 0 & & ⎪d b + d c + α f + α g = 0 & & & 2 3 &3 3 ⎩ 2 3 3 3 a) Bài toán động học thuận & - Bài toán phân tích Jacobi thuận : Biết d i (i=1,2,3) ta có 6 phương trình đại số tuyến tính để giải 6 ẩn : p1 , p2 , p3 ,α1 ,α 2 ,α 3 . & & & & & & - Ta viết lại hệ (3.33) dưới dạng ma trận: J d ⎡ d1 , d 2 , d3 ,0,0,0 ⎤ = J p [ p1 , p2 , p3 ,α1 ,α 2 ,α 3 ] T T & & & & & & & & & ⎣ ⎦ Trong đó: ⎡ 1 1 ⎤ ⎢ − sin α1 2 sin α 2 2 sin α 3 0 0 0⎥ ⎢ ⎥ ⎢ − 3 3 ⎥ ⎢ 0 sin α 2 sin α 3 0 0 0⎥ 2 2 Jd = ⎢ ⎥ ⎢ cos α1 cos α 2 cos α 3 0 0 0⎥ ⎢ a1 b1 0 0 0 0⎥ ⎢ ⎥ ⎢ a2 0 c2 0 0 0⎥ ⎢ 0 b3 c3 0 0 0⎥ ⎣ ⎦ ⎡ 1 1 ⎤ ⎢3 0 0 d1 cos α1 − d 2 cos α 2 − d3 cos α 3 ⎥ 2 2 ⎢ ⎥ ⎢ 3 3 ⎥ ⎢0 3 0 0 d 2 cos α 2 − d3 cos α 3 ⎥ 2 2 Jp = ⎢ ⎥ ⎢0 0 3 d1 sin α1 d 2 sin α 2 d3 sin α 3 ⎥ ⎢0 0 0 −e1 f1 0 ⎥ ⎢ ⎥ ⎢0 0 0 −e2 0 g2 ⎥ ⎢0 0 0 0 f3 g3 ⎥ ⎣ ⎦ - 18 -
  19. Chương 3 : Phân tích động học Robot song song 3 RPS Các thành phần của ma trận J d và J p đã xác định được từ bài toán vị trí. b) Bài toán động học ngược - Bài toán phân tích Jacobi ngược : Biết P = [ p1 , p2 , p3 ] ta có 6 phương & & & & T & & & & & & trình đại số tuyến tính để xác định 6 ẩn : d1 , d 2 , d3 ,α1 ,α 2 ,α 3 . - Ta viết lại hệ (3.33) dưới dạng ma trận: [ p1, p2 , p3 ,0,0,0] & & & & & & T T & & & = J 2 ⎡ d1 , d 2 , d 3 ,α1 ,α 2 ,α 3 ⎤ ⎣ ⎦ Trong đó : J2 = ⎡ sin α1 sin α 2 sin α 3 d1 cos α1 d 2 cos α 2 d3 cos α 3 ⎤ ⎢− 3 6 6 3 6 6 ⎥ ⎢ ⎥ ⎢ 3 3 3 3 ⎥ ⎢ 0 − sin α 2 sin α 3 0 − d 2 cos α 2 d3 cos α 3 ⎥ 6 6 6 6 ⎢ ⎥ ⎢ cos α1 cos α 2 cos α 3 − d1 sin α1 d sin α 2 − 2 − d3 sin α 3 ⎥ ⎢ 3 3 3 3 3 3 ⎥ ⎢ a b1 0 e1 f1 0 ⎥ ⎢ 1 ⎥ ⎢ a2 0 c2 e2 0 g2 ⎥ ⎢ ⎥ ⎣ 0 b3 c3 0 f3 g3 ⎦ Các thành phần của ma trận J 2 đã xác định được từ bài toán vị trí. 3.3 Kết quả Mô phỏng số bằng chương trình MATLAB 3.3.1 Bài toán động học thuận a) Ví dụ 1 - Các điểu kiện đầu : α10 = α 20 = α 30 = π / 2 d10 = d20 = d30 = 4.44 Hai tam giác là đều và h = g = 8.89 - Qui luật chuyển động của các chân robot : d1 = d10(1+0.08sin10t) d2 = d20(1+0.1sin20t) - 19 -
  20. Chương 3 : Phân tích động học Robot song song 3 RPS d3 = d30(1+0.05sin30t) - Suy ra qui luật thay đổi của vận tốc: d1c=0.8*d10*cos(10*t); d2c=d20*cos(20*t); d3c=0.5*d30*cos(30*t) - Kết quả mô phỏng : - 20 -
Đồng bộ tài khoản