Đề tài Robot song song

Chương 2: Cơ sở lý thuyết chung về phân tích động học Robot song song 2.1 Cơ sở lý thuyết phân tích động học vật rắn. 2.1.1 Ma trận cosin chỉ hướng 2.1.1.1 Định nghĩa ma trận cosin chỉ hướng của vật rắn. Cho vật rắn B và hệ qui chiếu R0=.Trong đó , , là ba vector đơn vị trên các trục Ox0,Oy0,Oz0. Ta gắn chặt vào vật rắn một hệ qui chiếu R= với ,, là ba vector đơn vị trên các trục Ax,Ay,Az (Hình 2.1). Hình 2.1 Định nghĩa : Ma trận vuông cấp ba (2.1) được gọi là ma trận cosin chỉ hướng của vật rắn B đối với hệ qui chiếu R0. Nếu ta đưa vào ký hiệu : , (i,j = 1,2 3) (2.2) Thì ma trận cosin chỉ hướng (2.1) có dạng: (2.3) Từ định nghĩa trên, trong hệ qui chiếu R0 ta có các hệ thức liên hệ: (2.4) Nếu ta ký hiệu ei là ma trận cột gồm các phần tử của vector trong hệ qui chiếu R0 , , (2.5) Thì ma trận cosin chỏ hướng (2.3) có dạng: A=[e1,e2,e3] (2.6) Ma trận cosin chỉ hướng A còn được gọi là ma trận quay của vật rắn. 2.1.1.2 Một vài tính chất cơ bản của ma trận cosin chỉ hướng a) Tính chất 1: Ma trận cosin chỉ hướng là ma trận trực giao. Theo công thức (2.6) : A=[e1,e2,e3] Vậy ma trận cosin chỉ hướng A là ma trận cột có ba cột là ba vector trực chuẩn. Do đó A là ma trận trực giao. Hệ quả: Trong 9 thành phần của ma trận cosin chỉ hướng có 3 thành phần độc lập. Do tính chất của ma trận cosin chỉ hướng là ma trận trực giao nên A.AT=E. Từ đó nhận được 6 phương trình liên hệ giữa các thành phần của ma trận cosin chỉ phương như sau: , Do vậy chỉ có ba thành phần của ma trận cosin chỉ hướng là độc lập. b) Tính chất 2: Định thức của ma trận cosin chỉ hướng det(A)=1. Từ hệ thức A.AT = E ta suy ra: det(A.AT) = det(A).det(AT) = det(E) = 1 Do : det(A) = det(AT) nên to có det(A) = . Ta có thể chứng minh det(A) = 1. c) Tính chất 3 : Ma trận cosin chỉ hướng có ít nhất một trị riêng . 2.1.1.3 ý nghĩa của ma trận cosin chỉ hướng của vật rắn Xét hai hệ qui chiếu R0 và R có cùng gốc O. Trong đó hệ qui chiếu R0 Ox0y0x0 là hệ qui chiếu cố định, hệ qui chiếu R Oxyz gắn liền với vật rắn B. Lờy một điểm P bất kỳ thuộc vật rắn B. Vị trí của điểm P được xác định bởi vector định vị . (Hình vẽ 2.2) Hình 2.2 Ký hiệu các tọa độ của điểm P trong hệ qui chiếu động Oxyz là xP, yP, zP, các tọa độ của điểm P trong hệ qui chiếu cố định Ox0y0z0 là , , . Ta có các hệ thức sau : (2.7) (2.8) Thế các biểu thức (2.4) vào hệ thức (2.8) ta được : (2.10) Hay : (2.11) So sánh các biểu thức (2.7) và (2.11) ta suy ra hệ phương trình : (2.12) Hệ phương trình (2.12) có thể viết lại dưới dạng ma trận như sau : (2.13) Từ hệ phương trình (2.13) ta rút ra kết luận sau : Ma trận cosin chỉ hướng A biến đổi các tọa độ của điểm P bất kỳ thuộc vật rắn trong hệ qui chiếu động Oxyz sang các tọa độ của điểm P đó trong hệ qui chiếu cố định Ox0y0z0. 2.1.2 Các ma trận quay cơ bản Ta qui ước hướng quay dương là hướng quay ngược chiều kim đồng hồ như hình vẽ (Hình 2.3). Hình 2.3 Các phép quay quanh trục x, y, z của hệ tọa độ vuông góc Oxyz được gọi là phép quay cơ bản. Ta tìm ma trận quay của phép quay quanh trục x0 một góc (Hình 2.4). Hình 2.4 Theo công thức định nghĩa (2.1) ta có: (2.14) = (2.15) Ma trận (2.15) được gọi là ma trận quay của phép quay cơ bản quanh trục x0. Bằng cách tương tự, ta xác định được các ma trận quay cơ bản quanh các trục y0 và z0 (Hình 2.5) , (2.16) Từ các công thức (2.15) và (2.16) ta dễ dàng tính được: (2.17) Hình 2.5 2.1.3 Các tọa độ thuần nhất và ma trận biến đổi tọa độ thuần nhất Khái niệm toạ độ thuần nhất được Denavit Hartenberg đưa ra năm 1955,và hiện nay đang được dùng rất rộng rãi trong tính toán động học robot. 2.1.3.1 Các toạ độ thuần nhất Định nghĩa: Cho X={x1,x2,..xn} là một điểm trong không gian n chiều Rn .Tập hợp (n+1) phần tử (y1,y2,..yn,yn+1) với (yn+1 ạ0) và: (2.18) Gọi là toạ độ thuần nhất của X. Trong kỹ thuật,người ta thường chọn (yn+1=1). Vậy điểm P(x,y,z) trong toạ độ vật lý R3 được biểu diễn trong toạ độ thuần nhất R4 như sau: P=[x,y,z]T Û P=[x,y,z,1]T Trong R3 Trong toạ độ thuần nhất R4 Nhờ khái niệm toạ độ thuần nhất trong không gian 4 chiều ta có thể chuyển bài toán cộng ma trận cột trong không gian ba chiều sang bài toán nhân ma trận trong không gian bốn chiều.Cho và là hai vector trong không gian ba chiều,ta có: a+b= (2.19) Ta chuyển phép tính cộng (2.19) bằng phép tính nhân hai ma trận như sau: (2.20) 2.1.3.2 Ma trận biến đổi toạ độ thuần nhất Xét vật rắn B chuyển động trong hệ qui chiếu cố định OX0Y0 Z0.Lấy một điểm A nào đó của vật rắn B và gắn chặt vào vật rắn hệ qui chiếu AXYZ(Hình 2.6).Lấy P là một điểm bất kỳ thuộc vật rắn B.Trong hệ toạ độ vật lý OX0Y0 Z0 ta có: Hình (2.6) (2.21) Phương trình (2.21) có thể viết dưới dạng ma trận như sau: (2.22) Trong đó R là ma trận cosin chỉ hướng của vật rắn B, là toạ độ của vectơ trong hệ qui chiếu .Nếu sử dụng hệ các toạ độ thuần nhất phương trình (2.22) có thể được viết lại dưới dạng: (2.23) Định nghĩa: Ma trận H = (2.24) được gọi là ma trận chuyển toạ độ thuần nhất của điểm P từ hệ Axyz sang hệ Ox0y0z0. c)Các ma trận quay cơ bản thuần nhất và ma trận tịnh tiến thuần nhất: Các ma trận quay cơ bản (2.15),(2.16) mở rộng ra trong hệ toạ độ thuần nhất bốn chiều có dạng như sau: R(x,j)= (2.25) R(y,y)= (2.26) R(x,q)= (2.27) Ngoài ra ta đưa vào khai niệm ma trận tịnh tiến thuần nhất có dạng: T(a,b,c)= (2.28) Trong đó ta thực hiện chuyển động tịnh tiến theo trục toạ độ x một doạn là a,theo trục toạ độ y một đoạn b,theo trục toạ độ z một đoạn c. 2.1.4 Các góc quay Euler và ma trận quay Euler 2.1.4.1 Xác định ma trận cosin chỉ hướng từ các góc Euler Vị trí của vật rắn B quay quanh điểm O cố định được xác định bởi hệ qui chiếu động Oxyz (gắn chặt vào vật rắn B) đối với hệ qui chiếu cố định Ox0y0z0 (Hình 2.7). Giả sử giao của mặt phẳng Ox0y0 và mặt phẳng Oxy là trục OK. Trục OK này được gọi là đường nút. Hình 2.7 Ta đưa vào các ký hiệu sau : - Góc giữa trục Ox0 và OK là - Góc giữa trục Oz0 và Oz là - Góc giữa trục OK và Ox là Ba góc được gọi là góc Euler. Như thế, vị trí của vật rắn B đối với hệ qui chiếu cố định được xác định bởi ba tọa độ suy rộng . Phương trình chuyển động của vật rắn quay quanh một điểm cố định có dạng: (2.29) Từ đó suy ra, vật rắn quay quanh một điểm cố định có ba bậc tự do. Khi xác định vị trí của vật rắn bằng các góc Euler, ta có thể quay hệ qui chiếu cố định Ox0y0z0 sang hệ qui chiếu động Oxyz bằng ba phép quay Euler như sau (Hình 2.8): Hình 2.8 - Quay hệ qui chiếu R0 Ox0y0z0 quanh trục Oz0 một góc để trục Ox0 chuyển tới đường nút OK. Với phép quay này, hệ Ox0y0z0 chuyển sang hệ Ox1y1z1 với Oz0 Oz1. - Quay hệ qui chiếu R1 Ox1y1z1 quanh trục Ox1 OK một góc để trục Oz0 Oz1 chuyển tới trục Oz2 Oz. Như thế hệ qui chiếu Ox1y1z1 chuyển sang hệ qui chiếu Ox2y2z2 với Ox1Ox2OK. - Quay hệ qui chiếu R2Ox2y2z2 quanh trục Oz2Oz một góc để trục Ox2OK chuyển tới trục Ox. Với phép quay này hệ qui chiếu Ox2y2z2 chuyển sang hệ qui chiếu Oxyz với Oz2 Oz. Như thế, bằng phép quay Euler quanh trục Oz0 một góc , quanh trục OK một góc , quanh trục Oz một góc , hệ qui chiếu Ox0y0z0 chuyển sang hệ qui chiếu Oxyz. Các ma trận quay ứng với các phép quay Euler có dạng: (2.30) (2.31) (2.32) Bây giờ ta xác định ma trận quay hệ qui chiếu Ox0y0z0 sang hệ qui chiếu Oxyz (cũng là ma trận cosin chỉ hướng của hệ qui chiếu Oxyz đối với hệ qui chiếu Ox0y0z0). Ta lấy P là một điểm bất kỳ của vật rắn B và ký hiệu AE là ma trận quay nêu trên. Theo công thức (2.13) ta có : (2.33) Ta ký hiệu là tọa độ của điểm P trong hệ qui chiếu Ri Oxiyizi (i=1,2). Theo công thức (2.13) ta cá các hệ thức sau: , , (2.34) Từ đó ta suy ra : (2.35) So sánh các biểu thức (2.33) và (2.35) ta suy ra biểu thức ma trận cosin chỉ hướng : (2.36) Hay: AE= (2.37) (3.38) Ma trận cosin chỉ hướng (3.38) được gọi là ma trận quay Euler. 2.1.4.2 Xác định các góc Euler từ ma trận cosin chỉ hướng Ma trận cosin chỉ hướng có dạng : (2.39) Ta có: (2.40) Khi = n. (n=1,2,3,...) thì việc tính toán sẽ rất khó khăn, vì thế người ta phải tìm cách xác định vật rắn bởi nhiều loại tham số khác nhau. 2.1.5 Phép quay Roll - Pitch - Yaw Một phép quay khác cũng thường được dùng là phép quay Roll, Pitch và Yaw, gọi tắt là phép quay RPY. Hãy tưởng tượng, gắn hệ tọa độ xyz lên thân một con tàu.Dọc theo thân tàu là trục z (Hình 2.9). Hình 2.9 Roll là chuyển động lắc của thân tàu, tương đương với việc quay thân tàu một góc quanh trục z. Pitch là sự bồng bềnh, tương đương với góc quay quanh trục y. Yaw là sự lệch hướng, tương đương với phép quay một góc quanh trục x. Xác định thứ tự quay : quay một góc quanh trục x, tiếp theo là quay một góc quanh trục y và sau đó là quay một góc quanh trục z. Theo thứ tự đó có thể biểu diễn phép quay RPY như sau: (2.41) Với : Thay vào công thức (2.41) ta được : Hay : (2.42) 2.2 Bài toán phân tích vị trí Robot song song 2.2.1 Bài toán vị trí 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. Hình 2.10 Sử dụng 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. Trong đó 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=1,2,3) như hình vẽ. Sử dụng các ký hiệu: ARB : Ma trận cosin chỉ hướng của hệ {Pxyz} so với hệ cố định {Ox0y0z0}. ARi : Ma trận cosin chỉ hướng của hệ {Aixiyizi} so với hệ cố định {Ox0y0z0}. : Vector đại số chứa các tọa độ của điểm Ai trên hệ cố định. : Vector đại số chứa các tọa độ của điểm Bi trên hệ cố định. B: Vector đại số chứa các tọa độ của điểm Bi trên hệ động. : 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 có thể biểu diễn dưới dạng: A = (i=1,2,3) (2.43) : Là 3 vector đơn vị trên các trục Ox0, Oy0,Oz0. : 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 . Ma trận A 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à . và B : 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: (i=1,2,3) và : (i=1,2,3) Hay dưới dạng đại số: A (i=1,2,3) (2.44) và : A.B (i=1,2,3) (2.45) Kết hợp hai phương trình trên ta có: A.B = A (i=1,2,3) (2.46) Trong đó: ; B = ; A = ; A = (i=1,2,3) (2.47) Hình 2.11 Các ma trận cosin chỉ hướng: A A (2.48) A Với : Az() là ma trận cosin chỉ hướng của phép quay quanh trục z một góc. (2.49) Nếu ta đặt : = ; Cqi = cos(qi), Sqi = sin(qi) với q= Vậy ta có : A A A Ta thấy các thành phần của các ma trận chỉ chứa các thành phần liên quan đến góc và góc . Ta viết lại phương trình (2.46) dưới dạng đại số. Chú ý: Do Ai thuộc mặt phẳng X0Y0 nên = 0 (i=1,2,3) A1 trên trục X0 nên Và Bi thuộc mặt phẳng X0Y0 nên = 0 (i=1,2,3) +Với i =1: (2.50) +Với i=2 (2.51) +Với i=3 (2.52) Ta thực hiện các phép biến đổi sau: (2.53) Với , Thay các kết quả của hệ (2.50) vào hệ (2.53) ta được: (2.54) +Mặt khác, dựa vào kết cấu của bàn di động B ta có : Hình 2.12 với : A (i=1,2,3) ; ; Hay : Kết hợp với hệ (2.53) ta có hệ 6 phương trình, 6 ẩn: Hệ phương trình (2.54) chứa 9 ẩn số . Các thành phần đã xác định được, các thành phần xác định theo (2.50) 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ố. 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 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à : Chú ý là 3 phương trình sau của hệ (2.54) chỉ chứa di và 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à . Sau đó thay các giá trị của di và vào 3 phương trình đầu ta sẽ tính được các giá trị của . 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 (2.50), (2.51), (2.52). 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 , ta phải tìm độ dài các chân di (i=1,2,3) và các góc (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ị và hệ (2.54), ta sẽ được hệ 6 phương trình với 6 ẩn là : . 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 (2.50), (2.51), (2.52). 2.3 Bài toán phân tích Jacobi robot song song Chương 3: Tính toán và mô phỏng số Robot song song 3RPS 3.1 Lý thuyết chung Trên cơ sở lý thuyết tính toán được ở chương 2.Ta áp dụng 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ó +Trục Khi đó các đại lượng trong công thức (2.46) trở thành : ; ; (3.1) ; ; (3.2) Do nên = = 0 Khi đó các ma trận cosin chỉ hướng ARi trở thành: (3.3) (3.4) (3.5) Vậy hệ (2.54) : Với : = -1 ; = h; Thay vào hệ (2.54) ta được : 3.1.1 Bài toán động học thuận Bài toán động học thuận là cho trước qui luật chuyển động của di (i=1,2,3). Ta phải tính vị trí bàn máy động . Giả sử cho trứơc qui luật của di : d1=d10*(1+0.08*sin(10*t)); d2=d20*(1+0.1*sin(20*t)); d3=d30*(1+0.05*sin(30*t));