ROBOT CÔNG NGHIỆP THIẾT KẾ CÁNH TAY ROBOT tọa độ cầu

ROBOT CÔNG NGHIỆP. .THIẾT KẾ CÁNH TAY ROBOT TỌA ĐỘ CẦU..THIẾT KẾ CÁNH TAY ROBOT TỌA ĐỘ CẦU..THIẾT KẾ CÁNH TAY ROBOT TỌA ĐỘ CẦU..THIẾT KẾ CÁNH TAY ROBOT TỌA ĐỘ CẦU..THIẾT KẾ CÁNH TAY ROBOT TỌA ĐỘ CẦU..THIẾT KẾ CÁNH TAY ROBOT TỌA ĐỘ CẦU

ROBOT TỌA ĐỘ CẦU

Robot hệ tọa độ cầu có thông số bảng DH như sau

Từ bảng DH ,kí hiệu viết tắt: c i=cos ⁡(θi) , s i=sin ⁡(θ1).

Ma trận H60=[r11 r12 r13 O x

r21 r22 r23 O y

r13 r32 r33 O z

Ma trận quay :

R30=[c1s2 −s1 −c1s2

c2s1 c1 −s1s2

s2 0 c2 ]

R63

=[c4c5c6−s4s6 −c4c5s6−c4c6 −c4s5

s4c5c6+c4c4 −s4c5s6+c4c6 −s4s5

s5c6 −s5s6 c5 ]

Yêu cầu:

- Vật có hệ tọa độ so với tọa độ cạnh bàn là O p=(−0,7 ; 0,6 ; 0,01)

- Hệ tọa độ bàn đặt tại cạnh song song với hệ tọa độ robot nên ta có hệ tọa độ của vật so với hệ tọa độ robot O0x0y0z0 là (−0,7 ; 1,6 ; 1,01).

- Gắp vật vào ô thứ 10 kích thước 200 mm x 250 mm Khi đó, vật có toạ độ so với hệ tọa độ robot là (−1,01 ; 1,375; 1,41).

Bài làm.

- Tọa độ home của robot có bảng DH :

- Tìm các thông số qua các điểm.

Ma trận chuyển đổi thuần nhất có dạng tổng quát H60 là :

H60

=[r11 r12 r13 O x

r21 r22 r23 O y

r13 r32 r33 O z

Từ bảng DH có ma trận quay R30 và ma trận quay R63 được tách ra từ ma trận thuần nhất

R30

=[c1s2 −s1 −c1s2

c2s1 c1 −s1s2

s2 0 c2 ] (1)

R63=[c4c5c6−s4s6 −c4c5s6−c4c6 −c4s5

s4c5c6+c4c4 −s4c5s6+c4c6 −s4s5

s5c6 −s5s6 c5 ](2)

- Ma trận R30 được xác định nhờ vào việc giải bài toán ngược vị trí ,khi đó 01 được xác định nên các phần tử trong ma trận cũng được xác định,vì vậy phương trình định ba thông số khớp cổ tay được xác định:

R63

=(R¿¿ 3 0

)T R=¿ ¿

[ c1s2r11+c2s1r21+s2r31 c1s2r12+c2s1r22+s2r23 c1s2r31+c2s1r32+s2r33

−s1r11+c1r21 −s1r21+c1r22 −s1r13+c1r23

−c1s2r11−s1s2r21+c2r31 −c1s2r12−s1s2r22+c2r32 −c1s2r13−s1s2r23+c2r33] (3)

- Đồng nhất các phần tử (2) và (3) ta xác định được các thông số khớp cổ tay :

c4s5=−(c¿¿1 s2r31+c2s1r32+s2r33) ¿ (4)

s4s5=−(−s1r13+c1r23) (5)

c5=−c1s2r13−s1s2r23+c2r33 (6)

s5c6=−c1s2r11−s1s2r21+c2r31 (7)

s5s6 =−(−c1s2r12 −s1s2r22 +c2r32 ) (8)

- Từ các phương trình trên, ta dễ dàng xác định tọa độ tâm khớp cổ tay bằng cách:

{x c=O x−d6r13

y c=O y−d6r23

z c=O z−d6r33

(9)

Tại vị trí thứ 1

- Có toạ độ O p=(−0,7 ; 1,6 ;1,11 ), có chiều các trục tọa độ song song với tọa độ robot

tọa độ O p, có trục x6 trùng với trục x p, chiều trục z6 ngược chiều với z p Khi đó ma

trục x một góc π :

H6p=Rot (x , π )=[10

0 0

0

−1 0 0

0 0

−1 0

0 0 0

1]

- Hệ trục O p có các cạnh song song với các cạnh bàn cũng như song song với

độ cơ sở là:

H0p

=[10

0

0

0

1

0

0

0 0 1 0

x p 1

y p 1

z p 1

1 ]

định là:

H60=H0p H6p=[10

0 0

0

−1 0 0

0 0

−1 0

x p 1

y p1

z p 1

1 ]

- Có O6=O0 từ đó suy ra

Tọa độ tâm khớp cổ tay được xác định từ các phương trình (9) :

{x c=O x−d6r13=−0,7

y c=O y−d6r23= 1,6

z c=O z−d6r33=1,31

⇒ Tọa độ O c=(−0,7 ; 1,6 ; 1,31)

- Từ cấu tạo Robot tọa độ cầu chiếu O c xuống mặt phẳng O0x0y0 ta xác định được các thông số như sau:

θ1=atan2(x c , y c)=113,8294

Giả định rằng ở vị trí ban đầu robot có tọa độ θ2'=−90 và d1=1,6 Khi đó ta có :

s= z c−d1=−0,29

r =√x c2+y c2=1,7464

d3=√s2+r2=1,77033

θ2=θ2'+ arctan(r s)=−99,4281

- Tiếp theo xác định các thông số khớp cổ tay :

Vì phương trình (4) và (5) không đồng thời bằng 0 nên ta có :

θ5= ¿

atan 2(−c1s2r13−s1s2r23+c2r33, ±√1−(−c1s2r13−s1s2r23+c2r33)2)

Vì ta chọn θ5 là âm nên các thông số còn lại được xác định như sau :

Từ phương trình (6):

s4s5=−(−s1r13+c1r23)

⇒θ4=0

Từ phương trình (8):

s5s6=−(−c1s2r12−s1s2r22+c2r32)

- Có bảng DH tại vị trí 1 :

Tại vị trí thứ 2

- Có toạ độ O p=(−0,7 ; 1,6 ;1,01 ), có chiều các trục tọa độ song song với tọa độ robot

tọa độ O p, có trục x6 trùng với trục x p, chiều trục z6 ngược chiều với z p Khi đó ma

trục x một góc π :

H6p=Rot (x , π )=[10

0 0

0

−1 0 0

0 0

−1 0

0 0 0

1]

độ cơ sở là:

H0p=[10

0

0

0

1

0

0

0 0 1 0

x p

y p

z p

1]

định là:

H60=H0p H6p=[1

0 0 0

0

−1 0 0

0 0

−1 0

x p

y p

z p

1]

- Có O6=O0 từ đó suy ra

Tọa độ tâm khớp cổ tay được xác định từ các phương trình (9) :

{x c=O x−d6r13=−0,7

y c=O y−d6r23= 1,6

z c=O z−d6r33=1,21

⇒ Tọa độ O c=(−0,7 ; 1,6 ; 1,21)

- Từ cấu tạo Robot tọa độ cầu chiếu O c xuống mặt phẳng O0x0y0 ta xác định được các thông số như sau:

θ1=atan2(x c , y c)=113,8294

Giả định rằng ở vị trí ban đầu robot có tọa độ θ2'=−90 và d1=1,6 Khi đó ta có :

s= z c−d1=−0,39

r =√x c2

+y c2 =1,7464

d3=√s2

+r2 =1,7894

θ2=θ2'+ arctan(r s)=−102,6037

- Tiếp theo xác định các thông số khớp cổ tay :

Vì phương trình (4) và (5) không đồng thời bằng 0 nên ta có :

θ5= ¿

atan 2(−c1s2r13−s1s2r23+c2r33, ±√1−(−c1s2r13−s1s2r23+c2r33)2)

Vì ta chọn θ5 là âm nên các thông số còn lại được xác định như sau :

Từ phương trình (6):

s4s5 =−(−s1r13 +c1r23 )

⇒θ4=0

Từ phương trình (8):

s5s6=−(−c1s2r12−s1s2r22+c2r32)

- Có bảng DH tại vị trí 2 :

Tại vị trí 3 : Vị trí 3 trùng với vị trí 1 nên ta có bảng DH như sau :

Tại vị trí thứ 4

- Có toạ độ O p=(−0,95; 1,375 ; 1,41), có chiều các trục tọa độ song song với tọa

độ robot

ngược chiều với x0 và y p trùng y0 Gỉa sử rằng tâm O6 nằm trên tâm kẹp gắp và cơ

H o p

=Rot(y , π

2)=[ 0

0

−1 0

0 1 0 0

1 0 0 0

x p

y p

z p

1]

tọa độ O p, có trục x6 trùng với trục x p, chiều trục z6 ngược chiều với z p Khi đó ma

trục x một góc π :

H6p=Rot (x , π )=[10

0 0

0

−1 0 0

0 0

−1 0

0 0 0

1]

định là:

H60=H0p H6p=[00

1 0

0

−1 0 0

−1 0 0 0

x p

y p

z p

1]

- Có O6=O0 từ đó suy ra

Tọa độ tâm khớp cổ tay được xác định từ các phương trình (9) :

{x c=O x−d6r13=−0,75

y c=O y−d6r23= 1,375

z c=O z−d6r33=1,41

⇒ Tọa độ O c=(−0,75 ; 1,375 ; 1,41)

- Từ cấu tạo Robot tọa độ cầu chiếu O c xuống mặt phẳng O0x0y0 ta xác định được các thông số như sau:

θ1=atan2(x c , y c)=118,6105

Giả định rằng ở vị trí ban đầu robot có tọa độ θ2'=−90 và d1=1,6 Khi đó ta có :

s= z c−d1=−0,19

r =√x c2+y c2=1,5662

d3=√s2

+r2 =1,5778

θ2=θ2'

+ arctan(r s)=−96,9169

- Tiếp theo xác định các thông số khớp cổ tay :

Vì phương trình (4) và (5) không đồng thời bằng 0 nên ta có :

θ5= ¿

atan 2(−c1s2r13−s1s2r23+c2r33, ±√1−(−c1s2r13−s1s2r23+c2r33)2)

Vì ta chọn θ5 là âm nên các thông số còn lại được xác định như sau :

Từ phương trình (6):

s4s5 =−(−s1r13 +c1r23 )

⇒θ4=87,2417

Từ phương trình (8):

s5s6=−(−c1s2r12−s1s2r22+c2r32)

- Có bảng DH tại vị trí 4 :

Tại vị trí thứ 5

- Có toạ độ O p=(−1,01; 1,375 ; 1,41), có chiều các trục tọa độ song song với tọa

độ robot

ngược chiều với x0 và y p trùng y0 Gỉa sử rằng tâm O6 nằm trên tâm kẹp gắp và cơ

H o p

=Rot(y , π

2)=[ 0

0

−1 0

0 1 0 0

1 0 0 0

x p

y p

z p

1]

tọa độ O p, có trục x6 trùng với trục x p, chiều trục z6 ngược chiều với z p Khi đó ma

trục x một góc π :

H6p=Rot (x , π )=[10

0 0

0

−1 0 0

0 0

−1 0

0 0 0

1]

định là:

H60

=H0p H6p

=[00

1 0

0

−1 0 0

−1 0 0 0

x p

y p

z p

1]

- Có O6=O0 từ đó suy ra

Tọa độ tâm khớp cổ tay được xác định từ các phương trình (9) :

{x c=O x−d6r13=−0,81

y c=O y−d6r23=1,375

z c=O z−d6r33=1,41

⇒ Tọa độ O c=(−0,81 ; 1,375 ; 1,41)

- Từ cấu tạo Robot tọa độ cầu chiếu O c xuống mặt phẳng O0x0y0 ta xác định được các thông số như sau:

θ1=atan2(x c , y c)=121,5019

Giả định rằng ở vị trí ban đầu robot có tọa độ θ2'=−90 và d1=1,6 Khi đó ta có :

s= z c−d1=−0,19

r =√x c2+y c2=1,5959

d3=√s2+r2=1,6071

θ2=θ2'

+ arctan(r s)=−96,0896

- Tiếp theo xác định các thông số khớp cổ tay :

Vì phương trình (4) và (5) không đồng thời bằng 0 nên ta có :

θ5= ¿

atan 2(−c1s2r13−s1s2r23+c2r33, ±√1−(−c1s2r13−s1s2r23+c2r33)2)

Vì ta chọn θ5 là âm nên các thông số còn lại được xác định như sau :

Từ phương trình (6):

s4s5=−(−s1r13+c1r23)

⇒θ4=88,0624

Từ phương trình (8):

s5s6=−(−c1s2r12−s1s2r22+c2r32)

- Có bảng DH tại vị trí 5 :

Tại vị trí 6 : Vị trí 6 trùng với vị trí 4 nên ta có bảng DH như sau :

a i α i d i θ i