38 38 CHƯƠNG III: 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 3RPS tổng quát Trên hình 3.1 mô tả sơ đồ động học của Robot song song 3RPS. Cấu trúc các khâu của Robot này được mô tả trong mục 1.6 a) Quan hệ về hình học và các hệ toạ độ Do cấu trúc Robot đảm bảo tính hợp lý nên các chân A i B i ⊥ Z i (các trục quay). Gọi O và P là trọng tâm của hai tam giác A 1 A 2 A 3 và B 1 B 2 B 3 , ta đặt lên đó các hệ toạ độ như trên hình 3.1: - Ox 0 y 0 z 0 : Hệ toạ độ cố định. - Pxyz : Hệ toạ độ động gắn liền với bàn máy động. - A i x i y i z i , (i = 1,2,3): Hệ động gắn với chân thứ i. Với x i ≡ ii BA và z i ≡ trục quay, trục y i lập với x i và z i một hệ quy chiếu thuận. Ta đưa thêm vào 3 toạ độ suy rộng α i (i = 1,2,3) là góc hợp bởi trục z 0 và trục x i như hình vẽ. A1 A2 A3 B1 B3 B2 Hình 3.1: Cấu trúc chấp hành song song 3RPS Đế cố định Bàn máy động x 0 y 0 z 0 x 1 x 1 x 2 x 3 z 1 z 2 z 3 1 α 2 α 3 α P O y 1 z 1 39 39 Ngoài ra ta sử dụng các ký hiệu: - a i : Véc tơ đại số chứa các toạ độ của điểm A i trên hệ cố định, a i = [] T iii aaa 321 ,, b i : Véc tơ đại số chứa các toạ độ của điểm B i trên hệ cố định. - B b i : Véc tơ đại số chứa các toạ độ của điểm B i trên hệ cố động, B b i = [] T iziyix b,b,b . - p: véc tơ đại số chứa các toạ độ của điểm P trên hệ cố định, p = [p 1 ,p 2 ,p 3 ] T . - d i : độ dài chân thứ i (d i = A i B i ). - b i và B b i : Xác định được từ kết cấu hình học của robot. - A R B : Ma trận cosin chỉ hướng của hệ động Pxyz so với hệ cố định Ox 0 y 0 z 0 . - A R i : Ma trận cosin chỉ hướng của hệ động A i x i y i z i so với hệ cố định Ox 0 y 0 z 0 . A R B ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = zzz yyy xxx wvt wvt wvt , A R i ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = iziziz iyiyiy ixixix wvu wvu wvu với: (i = 1,2,3). (3.1) Các phần tử của ma trận A R B và ma trân A R i tuỳ theo kết cấu của bàn đế cố định, là hàm của góc α i . Từ hình vẽ ta có: i i OAOB = + ii BA (i = 1,2,3) (3.2) OPOB i = + i PB (i = 1,2,3) (3.3) Ta có thể biểu diễn (3.2) và (3.3) dưới dạng sau: b i = a i + A R i . ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 d i (i = 1,2,3) (3.4) b i = p + A R B . ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 d i (i = 1,2,3) (3.5) Kết hợp hai phương trình trên ta có : p + A R B . B b i = a i + A R i . ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 d i (i=1,2,3) (3.6) 40 40 Hệ thức (3.6) gồm có 9 phương trình chứa các ẩn là toạ độ diểm P, độ dài các chân d i , các góc α i . Khi giải các bài toán động học thuận/ngược, ta đã biết 3 thông số p/d i nên công việc còn lại chỉ chỉ giải bài toán 6 phương trình 6 ẩn, các thông số còn lại như hướng của bài máy động, hướng của các chân ii BA sẽ được xác định khi đã biết các thông số này. Hệ thức này có ý nghĩa rất quan trọng, qua đó ta có thể giải quyết bài toán động học một cách trọn vẹn cả bài toán thuận và bài toán ngược, điều mà các phương pháp trước đây chưa giải quyết được hay mới chỉ đưa ra cách giải quyết bài toán thuận b) Tính toán các phần tử của hệ thức (3.6) Các ma trận cosin chỉ hướng A R i được xác định bởi các phép quay liên tiếp dựa vào lý thuyết trình bày ở mục 2.2, ta thực hiện các phép biến đổi liên tiếp để hệ toạ độ cố định Oxyz trùng với hệ A i x i y i z i . Hình 3.2 Áp dụng hệ thức trong tam giác ta có thể tính được độ dài các đường trung bình, như vậy sẽ xác định được ϕ 2 và ϕ 3 . cos ϕ 2 = 21 2 21 2 2 2 1 OAOA2 AAOAOA ++ cos ϕ 3 = 31 2 31 2 3 2 1 OAOA2 AAOAOA ++ Ma trận A R i được xác định bởi các phép quay liên tiếp từ hệ cố định quanh chính nó như sau: A R i = A z1 . A x2 . A z3 . A x4 Với A z1 , A x2 , A z3 , A x4 lần lượt là ma trận cosin chỉ hướng của các phép quay sau: -Quay một góc ( 1 2 β − ∏ ) quanh trục z 0 . Y 0 Z 2 Z 1 Z 3 2 ϕ 3 β 3 ϕ β 1 2 β X 0 41 41 -Quay tiếp quanh trục x của hệ mới một góc (Π/2) -Quay tiếp quanh trục z của hệ mới một góc (Π/2 + α 1 ) -Quay tiếp quanh trục x của hệ mới một góc Π Vậy : A R 1 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ α−α α−α− ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ββ β−β 100 010 001 . 000 1sincos 0cossin . 010 100 001 . 100 0sincos 0cossin 11 11 11 11 A R 1 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ αα βαβαβ− β−αβαβ− 0sincos sincoscossincos coscossinsinsin 11 11111 11111 Bằng cách tương tự ta cũng xác định được các ma trận A R 2 và A R 3 Ma trận A R 2 được xác định bởi các phép quay sau: - Quay hệ cố định một góc ( 22 2 ϕ+β− ∏ ) quanh trục z 0 . - Quay tiếp quanh trục x của hệ mới một góc (Π/2). - Quay tiếp quanh trục z của hệ mới một góc (Π/2 + α 2 ). - Quay tiếp quanh trục x của hệ mới một góc Π. A R 2 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ α−α α−α− ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ϕ−βϕ−β ϕ−β −ϕ−β 100 010 001 . 000 1sincos 0cossin . 010 100 001 . 100 0)sin()cos( 0)cos()sin( 22 22 2222 2222 A R 2 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ αα ϕ−βαϕ−βαϕ−β− ϕ−β −αϕ−βαϕ−β− 0sincos )sin(cos)cos(sin)cos( )cos(cos)sin(sin)sin( 22 22222222 22222222 Ma trận A R 3 được xác định bởi các phép rơ quay sau: - Quay hệ cố định một góc ( 33 2 ϕβ −− ∏ ) quanh trục z 0 . - Quay tiếp quanh trục x của hệ mới một góc (Π/2). 42 42 - Quay tiếp quanh trục z của hệ mới một góc (Π/2 + α 3 ). - Quay tiếp quanh trục x của hệ mới một góc Π. A R 3 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ α−α α−α− ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ϕ−βϕ−β ϕ−β−ϕ−β 100 010 001 . 000 1sincos 0cossin . 010 100 001 . 100 0)sin()cos( 0)cos()sin( 33 33 3333 3333 A R 3 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ αα ϕ−βαϕ−βαϕ−β− ϕ−β −αϕ−βαϕ−β− 0sincos )sin(cos)cos(sin)cos( )cos(cos)sin(sin)sin( 33 33333333 33333333 Ta thấy các thành phần của các ma trận A R i (i=1,2,3) chỉ chưa các ẩn là các góc α i còn ϕ 2 , ϕ 3 và β i (i=1,2,3) đã biết do kết cấu của robot. Mặt khác dựa vào kết cấu của bàn di động B ta có : ( b 1 – b 2 ) T ( b 1 - b 2 ) = 2 21 BB (3.7) ( b 1 – b 3 ) T ( b 1 - b 3 ) = 2 31 BB (3.8) ( b 2 – b 3 ) T ( b 2 - b 3 ) = 2 32 BB (3.9) Với b i = a i + A R i . ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 d i (i = 1,2,3) Như vậy các thành phần của hệ thức (3.6) được xác định thông qua 9 ẩn số α 1 , α 2 , α 3 , d 1 , d 2 , d 3 , p 1 , p 2 , p 3 . Khi giải quyết bài toá động học thuận hay ngược, ta biết trước được 3 ẩn (d 1 , d 2 , d 3 hoặc p 1 , p 2 , p 3 ). 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 Bài toán động thuận là bài toán biết độ dài các chân d i (i=1,2,3), ta phải tìm vị trí của bàn máy động p và ma trận A R B . Ta thay các giá trị d i vào hệ (3.6), ta được hệ 6 phương trình với 6 ẩn số là: α 1 , α 2 , α 3 ,, p 1 , p 2 , p 3 . 3.1.3 Bài toán động học ngược 43 43 Bài toán động ngựoc là bài toán biết vị trí của bàn máy di động p i (i=1,2,3), ta phải tìm vị trí của các chân d i (i=1,2,3) và các góc α i (i=1,2,3). Ta thay các giá trị p i vào hệ (3.6), ta được hệ 6 phương trình với 6 ẩn số là: α 1 , α 2 , α 3 , d 1 , d 2 , d 3 . 3.1.4 Tính toán vị trí cho robot song song 3RPS cụ thể Trên hình 3.3 mô tả sơ đồ động học của một con robot song song 3RPS, có đế cố định A 1 A 2 A 3 và bàn di động B 1 B 2 B 3 là các tam giác đều. Độ dài PB 1 =h: OA 1 = g. Khi các góc ϕ 2 = ϕ 3 = 2π/3. Để đảm bảo tính hợp lý của kết cấu ta có z i = A i B i , z i ⊥ OA i nên β i = π/2 Khi đó B b 1 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 h ; B b 2 = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − 0 2 3 .h 2 h ; B b 3 = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − 0 2 3 .h 2 h (3.10) A1 A2 A3 B1 B3 B2 Hình 3.3: Cấu trúc chấp hành song song 3RPS Đế cố định Bàn máy động x 0 y 0 z 0 x 1 x 1 x 2 x 3 z 1 z 2 z 3 1 α 2 α 3 α y 1 z 1 P O 44 44 a 1 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 g ; B b 2 = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − 0 2 3 .g 2 g ; B b 3 = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − 0 2 3 .g 2 g (3.11) Sử dụng công thức (3.7), (3.8), (3.9) thay vào hê thức (3.6) : Các ma trận A R 1 trở thành: A R 1 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ αα αα− 0sincos 100 0cossin 11 11 (3.12) A R 2 = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ αα −αα− −α−α− 0sincos 2 1 cos 2 3 sin 2 3 2 3 cos 2 1 sin 2 1 22 22 221 (3.13) A R 3 = ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ αα −α−α α−α 0sincos 2 1 cos 2 3 sin 2 3 2 3 cos 2 1 sin 2 1 33 33 33 (3.14) Thay vào hệ thức (3.6) ta được: + Với i = 1: p 1 + u x .h = g – d 1 .sinα 1 p 2 + u y .h = 0 p 3 + u z .h = d 1 cosα 1 Suy ra: u x = h sindpg 111 α−− 45 45 u y = h p 2 − u z = h pcosd 311 −α + Với i = 2 22xx1 sind 2 1 2 g 2 3 h.v 2 h .up α+−=+− 22yy1 sind 2 3 2 3g 2 3 h.v 2 h .up α−=+− (3.16) 22xz3 cosd 2 3 h.v 2 h .up α=+− + Với i = 3 33xx1 sind 2 1 2 g 2 3 h.v 2 h .up α+−=−− 33yy1 sind 2 3 2 3g 2 3 h.v 2 h .up α+−=−− (3.17) 33xz3 cosd 2 3 h.v 2 h .up α=−− Thực hiện các phép biến đổi (3.15), (3.16), (3.17) và thay các B b i vào (3.7), (3.8), (3.9) ta được: 3p 1 = 2 1 (d 2 sinα 2 + d 3 sinα 3 ) – d 1 sinα 1 3p 2 = 2 3 − (d 2 sinα 2 - d 3 sinα 3 ) 3p 3 = d 1 cosα 1 + d 2 cosα 2 – d 3 cosα 3 (3.18) 3g 2 – 3gd 1 sinα 1 - 3gd 2 sinα 2 +d 1 d 2 sinα 1 .sinα 2 – 2d 1 d 2 cosα 1 .cosα 2 + d 22 2 2 1 h3d =+ 3g 2 – 3gd 1 sinα 1 - 3gd 3 sinα 3 +d 1 d 3 sinα 1 .sinα 3 – 2d 1 d 3 cosα 1 .cosα 3 + 22 3 2 1 h3dd =+ 46 46 3g 2 – 3gd 2 sinα 2 - 3gd 3 sinα 3 +d 2 d 3 sinα 2 .sinα 3 – 2d 2 d 3 cosα 2 .cosα 3 + 22 3 2 3 h3dd =+ Hệ (3.18) là dạng khai triển của hệ (3.6) áp dụng cho một robot song song 3RPS cụ thể, nhờ hệ này ta có thể áp dụng tính toán và giải bài toán động học robot một cách trọn vẹn. 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 d i (i = 1,2,3) ta phải tìm vị trí của bàn máy động p và ma trận A R B . Theo phần trên ta thay các giá trị d i (i = 1,2,3) vào hệ (3.8) ta sẽ được hệ 6 phương trình 6 ẩn là α 1 , α 2 , α 3 , p 1 , p 2 , p 3 . Chú ý 3 phương trình sau của hệ (3.18) chỉ chứa d i và α i nên việc giải hệ 6 phương trình 6 ẩn đơn giản còn có 3 phương trình 3 ẩn là α i . Sau đó thay các giá trị của d i và α i vào 3 phương trình đầu ta sẽ được các giá trị của p. Các gía trị còn lại được tính bằng cách thay trực tiếp vào phương trình (3.15), (3.16), (3.17). 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 d i (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ào hệ (3.18), ta sẽ được hệ 6 phương trình với 6 ẩn số là α 1 , α 2 , α 3 , d 1 , d 2 , d 3 . Các giá trị còn lại được tính bằng cách thay trực tiếp vào phương trình (3.15), (3.16), (3.17). 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 dà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 , q ) = 0 (3.19) Trong đó: q là biến khớp tác động. x đặc trưng vị trí bàn máy di động. f là hàm ẩn n chiếu theo q và x. Đạo hàm (3.19) theo thời gian ta được: q q f x x f & & ∂ ∂ = ∂ ∂ (3.20) 47 47 Nếu đặt x f J ∂ ∂ = x và q f J ∂ ∂ = q thì hệ (3.20) có dạng : qJxJ & & . qx = (3.21) Từ đó ta có: xJJq & & x 1 q . − = (3.22) Ta có thể biểu diễn (3.22) dưới dạng sau: xJq & & 1 = với J 1 = x 1 - q .JJ (3.23) Tương tự ta cũng có được : qJJx & & q 1 x − = (3.24) qJx & & . 2 = với q 1 x2 .JJJ − = (3.25) Trong đó J 1 , 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 trạng thái đặ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 q 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: + Khi định thức của J q tiến đến zero det(J x ) = 0 (3.26) Khi đó tồn tại các véc tơ q & khác zero dẫn đến kết quả vectơ 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ơ cấu chấp hành bị dàng buộc lại và mất đi một số bậc tư do. Trạng 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. + Khi định thức J x bằng zero det(J x ) = 0 (3.27) Khi đó tồn tại các véc tơ x & khác zero dẫn đến kết quả vectơ q & bằng zero. Trong trường hợp này bệ di động có thể chuyển động vi phân theo một số chiều, còn mọi bộ tác động khác đều bị khoá tức là hệ sẽ tăng lên một số bậc tự do. +Khi cả hai định thức của J x và J q đều bằng zero. Trong phạm vi đồ án này ta không xét tới các trạng thái động học đặc biệt này. [...]...48 3.2.2 Phân tích Jacobi Robot song song 3RPS tổng quát z1 z0 B3 α3 P Bàn máy động B1 A3 α1 z1 y1 α2 z2 z3 x1 B2 x3 x2 O Đế cố định y0 A1 x0 A2 x1 Hình 3.4: Cấu trúc chấp hành song song 3RPS Từ công thức (3.6): ⎡d i ⎤ p + ARB.Bbi = ai + ARi ⎢ 0 ⎥ (i = 1,2,3) ⎢ ⎥ ⎢0⎥ ⎣ ⎦ Đạo hàm công thức (3.6) ttheo thời... xếp các số hạng của phương trình (3.30) 3.2.3 Phân tích Jacobi của một robot song song 3RPS cụ thể Đạo hàm phương trình (3.18) ta được hệ (3.31): [ ] 1& & & & & & & & & 3p1 = d2 sinα2 + α2d2 cosα2 + d3 sinα3 + α3d3 cosα3 − d1 sinα1 − α1d1 cosα1 2 − 3& & & & & & & 3p2 = d2 sinα2 + α2d2 cosα2 − d3 sinα3 − α3d3 cosα3 2 & & & & & & & 3p3 = d1 cosα1 − α1d1 sinα1 + d2 cosα2 − α2d2 sinα2 − d3 cosα3 − α3d3 sinα3... ~ & ~ p + ω.Α R B B b i = ω.R i ⎢ 0 ⎥ + R i ⎢ 0 ⎥ (i=1,2,3) ⎢ ⎥ ⎢0⎥ ⎢0⎥ ⎣ ⎦ ⎣ ⎦ (3.30) Khai triển (3.30) ta sẽ được 9 phương trình, các đại lượng đã biết từ bài toán tìm vị trí và tham số hình học của robot là: di,Bbi,Ri,ARB (i=1,2,3) 49 50 & -Bài toán 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 , p 2 , p 3 , ω1 , ω2 , ω3 , ω11 , ω21 , ω31 & & & &... thể viết thành: [ ] 1 & & & & & & & & d2 sin α2 + α2d2 cosα2 + d3 sin α3 + α3d3 cosα3 − d1 sin α1 − α1d1 cosα1 2 − 3 & & & & & & & d2 sin α2 + α2d2 cosα2 − d3 sin α3 − α3d3 cosα3 3p2 = 2 & & & & & & & 3p3 = d1 cosα1 − α1d1 sin α1 + d2 cosα2 − α2d2 sin α2 − d3 cosα3 − α3d3 sin α3 & 3p1 = [ ] & & & & d 1f 1 + d 2 f 2 + α 1 f 3 + α 2 f 4 = 0 & & & & d 1f 5 + d 3 f 6 + α 1 f 7 + α 3 f 8 = 0 & & & & d f +d . CHƯƠNG III: 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 3RPS tổng quát Trên hình. d 3 . 3.1.4 Tính toán vị trí cho robot song song 3RPS cụ thể Trên hình 3.3 mô tả sơ đồ động học của một con robot song song 3RPS, có đế cố định A 1 A 2