Tải bản đầy đủ - 0 (trang)
Hình 1.22:Hệ trục tọa độ gắn cho tay máy 3 bậc tự do.

Hình 1.22:Hệ trục tọa độ gắn cho tay máy 3 bậc tự do.

Tải bản đầy đủ - 0trang

Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



1.3.2.2 Các ma trận biến đổi đồng nhất:

C 1

 S 1

1

T0  

 0



 0



0 S 1

0  C 1

1

0

0

0



C 3  S 3

 S 3 C 3

3

T2  

 0

0



0

 0



0

C 2  S 2



 S 2 C 2

0

2

; T1 

l1

0

0





1

0

 0



0 l 2C 2

0 l 2 S 2 

;

1

0 



0

1 



0 l 3C 3

0 l 3S 3 

;

1

0 



0

1 



T02 T01 T12 T03 T02 T23

;



3



2



3



1



1



2



T T T



C 1C 2  C 1S 2 S 1 l 2C 1C 2

 S 1C 2  S 1C 2  C 1 l 2 S 1C 2 

2

;

T0 

 S 2

C 2

0

l 2 S 2  l1 





0

0

0

1





C 23  S 23

 S 23 C 23

3

T1  

 0

0



0

 0



0 l 3C 23  l 2C 2

0 l 3S 23  l 2S 2 



1

0



0

1





C 1C 23  C 1S 23 S 1 l 3C 1C 23  l 2C 1C 2

 S 1C 23  S 1S 23  C 1 l 3S 1C 23  l 2 S 1C 2 

3

;

T0  

 S 23

C 23

0

l 3S 23  l 2 S 2  l1 





0

0

0

1





Trong đó:  12  1   2; 123  1   2   3



1.3.2.3 Tìm ma trận Jacobian

Ta có ma trận biến số động học:

 Nx Sx

 Ny Sy



 Nz Sz



0

0



Ax

Ay

Az

0



Px 

Py 

;

Pz 



1



Trong đó: Nx,y,z là phương vng góc; Sx,y,z là phương tiếp cận; Ax,y,z là hướng của

tay kẹp; Px,y,z là chỉ vị trí của tay kẹp.

Jacobian:



Võ Hồng Trơvi – Nguyễn Sĩ Bích



37



Đồ án tốt nghiệp

 Px

  1

 Py

J 

  1

 Pz

  1



GVHD: ThS. Trần Văn Trinh



Px

 2

Py

 2

Pz

 2



Px 

 3 

Py 

;

 3 

Pz 

 3 



Với :

Px l 3C 1C 23  l 2C 1C 2

Py l 3S 1C 23  l 2S 1C 2

Pz l 3S 23  l 2 S 2  l1



Tính các đạo hàm riêng ta được ma trận Jacobian như sau:

  l 3S 1C 23  l 2S 1C 2  l 3C 1S 23  l 2C 1S 2  l 3C 1S 23

J   l 3C 1C 23  l 2C 1C 2  l 3S 1S 23  l 2C 1S 2  l 3C 1S 23 ;



0

l 3C 23  l 2C 2

l 3C 23 

Tìm các vị trí kỳ dị của cánh tay:

Định thức của Jacobian bằng không.

det  J  0   2 90  ,  3 0 

Khi đó ba khớp của cánh tay robot tạo thành một đường thẳng, ta có điểm kỳ dị trong

biên.



1.3.2.4. Viết phương trình động lực học của cánh tay robot

* Phương trình dạng tổng quát:

n



..



n



n



.



.



  Dik q k   hikm q k q m  C i

k 1



k 1 m 1



Trong đó:



D



3



ik



T



 Tr (U jk I jU ji ) ;







j max(i , k )

3







C i   mi g U ji r j



hikm 



3



 Tr (U I U



j max(i , k , m )



jkm



j



T

ji



);



j



j i



Theo bài ta có:

Các biến khớp:  1, 2, 3 đều là khớp xoay nên ta được matran Qi

0  1

1 0







Q1 Q2 Q3 0 0



0 0



0

0

0

0



0

0

0



0



* Tính thông số Dik



D



3



jk







 Tr (U I U T



j max(i , k )



jk



j



ji



)



Trong đó Ij là tensor qn tính của mỗi khâu.



D



11



T



T



T



Tr (U 11 I 1U 11)  Tr (U 21 I 2U 21)  Tr (U 31 I 3U 31)



Võ Hồng Trơvi – Nguyễn Sĩ Bích



38



Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



T



T



12



Tr (U 22 I 2U 21)  Tr (U 32 I 3U 31)



13



Tr (U 33 I 3U 31)



21



Tr (U 21 I 2U 22)  Tr (U 31 I 3U 32)



22



Tr (U 22 I 2U 22)  Tr (U 32 I 3U 32)



23



Tr (U 33 I 3U 32)



31



Tr (U 31 I 3U 33)



32



Tr (U 32 I 3U 33)



33



Tr (U 33 I 3U 33)



D

D

D

D

D

D

D

D



T



T



T



T



T



T



T



T



T



Trong đó:



U

U

U



ij



 j  1 Q i ; j i

j T j 1

 T 0



0; j  i



11



Q T 0 ; U 21 Q T 0 ; U 31 Q T 0

1

1

1



22



T 0 Q



1



1



2



2



2



T ;U

1



T 0 Q



3



1



32



3



2



T ;U

1



T 0 Q

2



33



3



T



3

2



* Tính thơng số h

3



.



3



.



h   h qk qm

i



h



ikm



k 1 m 1

3



ikm



 Tr (U I U







j max(i , k , m )



T



j



jkm



ji



)



Tính h1

.



.



.



.



.



.



.



.



.



.



.



.



.



.



h h q1 q1  h q1 q2  h q1 q3  h q2 q1  h q2 q2  h q2 q3  h q3 q1 

1



111

.



112

.



.



113



121



122



123



131



.



 h132 q q  h133 q q

3 2

3 3



trong đó:



h

h

h

h

h

h

h

h

h



T



T



T



T



T



111



Tr (U 111 I 1U 11)  Tr (U 211 I 2U 21)  Tr (U 311 I 3U 31)



112



Tr (U 212 I 2U 21)  Tr (U 312 I 3U 31)



113



Tr (U 313 I 3U 31)



121



Tr (U 221 I 2U 21)  Tr (U 321 I 3U 31



122



Tr (U 222 I 2U 21)  Tr (U 322 I 3U 31



123



Tr (U 323 I 3U 31)



131



Tr (U 331 I 3U 31)



132



Tr (U 332 I 3U 31)



133



Tr (U 333 I 3U 31)



T



T



T



T



T



T



T



T



T



Tính h2



Võ Hồng Trơvi – Nguyễn Sĩ Bích



39



Đồ án tốt nghiệp

.



.



GVHD: ThS. Trần Văn Trinh

.



.



.



.



.



.



.



.



.



.



.



.



.



.



.



.



.



h2 h211 q1 q1  h212 q1 q2  h213 q1 q3  h221 q2 q1  h222 q2 q2  h223 q2 q3  h231 q3 q1 

.



.



.



.



 h232 q q  h233 q q

3 2

3 3



trong đó:



h

h

h

h

h

h

h

h

h



T



T



211



Tr (U 211 I 2U 22)  Tr (U 311 I 3U 32)



212



Tr (U 212 I 2U 22)  Tr (U 312 I 3U 32)



213



Tr (U 313 I 3U 32)



221



Tr (U 221 I 2U 22)  Tr (U 321 I 3U 32



222



Tr (U 222 I 2U 22)  Tr (U 322 I 3U 32



223



Tr (U 323 I 3U 32)



231



Tr (U 331 I 3U 32)



232



Tr (U 332 I 3U 32)



233



Tr (U 333 I 3U 32)



T



T



T



T



T



T



T



T



T



T



T



Tính h3

.



.



.



.



.



.



.



.



.



h h q1 q1  h q1 q2  h q1 q3  h q2 q1  h q2 q2  h q2 q3  h q3 q1 

3



311



312



.



313



.



.



321



322



323



331



.



 h332 q q  h333 q q

3 2

3 3



rong đó:

T



h

h

h

h

h

h

h

h

h



311



Tr (U 311 I 3U 33)



312



Tr (U 312 I 3U 33)



313



Tr (U 313 I 3U 33)



321



Tr (U 321 I 3U 33)



322



Tr (U 322 I 3U 33)



323



Tr (U 323 I 3U 33)



331



Tr (U 331 I 3U 33)



332



Tr (U 332 I 3U 33)



333



Tr (U 333 I 3U 33)



T



T



T



T



T



T



T



T



Trong đó:



U



ijk



U



111



 j  1 Q k  1 Q i ; i k  j

 T 0k  1 j T jj 11 k T ik  1

 T 0 Q T k  1 Q T j  1 ; i  j k

k

j



0

;

i



j

;

ik



Q



2



1



T



1

0



2



2



; U 211 Q1 T 0 ; U 311 Q1 T 0

2



Võ Hoàng Trơvi – Nguyễn Sĩ Bích



3



40



t



Đồ án tốt nghiệp



U

U

U

U



GVHD: ThS. Trần Văn Trinh



U 221 Q T 0 Q

1



212



2



T ;U

U Q T Q T

U Q T Q T ; U

T Q Q T

1



2



1



2



1



1



312



321



1



0



331



1



0



2



333



0



1



2

2



T



2

1



3



2



313



T 0 Q

222



3



2



3



U 332 T 0 Q

1



323



T QT

2



2



1



3



3

2



3



3



2



2



Tính Ci



C







3



i



  mi g U ji r j

j

j i





C1  m1 g U 11 r11 









m2 g U 21 r22 



C 2  m2 g U 22 r22 







m g U r33

3



31







m g U r33

3



32







C



3







m g U r33

3



33



Khối lượng được phân bố đều trên mỗi khâu.

l1 / 2

 0 

1



r1  0  ;





 1 



l 2 / 2 

 0 

2



r 2  0  ;





 1 



l 3 / 2 

 0 

3



r 3  0 





 1 



Tính tốn các giá tri như sau:

Tính Uij

  S 1

 C 1

U 11  

 0



 0



0 C 1 0

  S 1C 2 S 1S 2 C 1  l 2 S 1C 2



 C 1C 2  C 1S 2 S 1 l 2C 1C 2 

0 S 1 0



;U 21  





0 0 0

0

0

0

0







0 0 0

0

0

0

0







  C 1S 2  C 1C 2

  S 1S 2  S 1C 2

U 22  0

 S 2



0

0





0  l 2C 1S 2

0  l 2 S 1 2 

;

0

l 2C 2 



0

0





  S 1C 23 S 1S 23 C 1  l 3S 1C 23  l 2 S 1C 2

  C 1C 23  C 1S 23 S 1 l 3C 1C 23  l 2C 1C 2 

;

U 31  0



0

0

0





0

0

0

0







Võ Hồng Trơvi – Nguyễn Sĩ Bích



41



Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



  C 1S 23  C 1C 23

  S 1S 23  S 1C 23



U 32  C 23

 S 23



0

0





0  l 3C 1S 23  l 2C 1S 2

0  l 3S 1S 23  l 2 S 1S 2 

;



0

l 3C 23  l 2C 2



0

0





  C 1S 23  C 1C 23

  S 1S 23  S 1C 23



U 33  C 23

 S 23



0

0





0  l 3C 1S 23 

0  l 3S 1S 23

;

0

l 3C 23 



0

0





Tính Uijk.





U 111 







C 1

S 1

0

0



0  S 1

0 C 1

0

0

0

0



0

0

;

0



0



  C 1C 2 C 1S 2  S 1  l 2C 1C 2

  S 1C 2 S 1S 2 C 1  l 2 S 1C 2 

;

U 211  0



0

0

0





0

0

0

0





  C 1C 23 C 1S 23  S 1  l 3C 1C 23  l 2C 1C 2

  S 1C 23 S 1S 23 C 1  l 3S 1C 23  l 2 S 1C 2 

;

U 311  0



0

0

0





0

0

0

0





S 1C 2

 S 1S 2

  C 1S 2  C 1C 2

U 221  0

0



0

0



  C 1C 2 C 1S 2

  S 1C 2 S 1S 2

U 222   S 2  C 2



0

0





0 l 2S 1S 2 

0  l 2C 1S 2

;



0

0



0

0



0  l 2C 1C 2

0  l 2 SC 2 

;

0

 l 2 S 2 



0

0





Võ Hồng Trơvi – Nguyễn Sĩ Bích



42



Đồ án tốt nghiệp

 S 1S 23

  C 1S 23



U 321  0



0



  C 1C 23

  S 1C 23



U 322   S 23



0



 S 1S 23

  C 1S 23



U 331  0



0





GVHD: ThS. Trần Văn Trinh

S 1C 23 0 l 3S 1S 23  l 2 S 1S 2 

 C 1C 23 0  l 3C 1S 23  l 2C 1S 2

;



0

0

0



0

0

0



C 1S 23 0  l 3C 1C 23  l 2C 1C 2

S 1S 23 0  l 3S 1C 23  l 2 S 1C 2 

;



 C 23 0

 l 3S 23  l 2 S 2



0

0

0



S 1C 23 0 l 3S 1S 23 

 C 1C 23 0  l 3C 1S 23

;



0

0

0



0

0

0





  C 1C 23 C 1S 23

  S 1C 23 S 1S 23



U 332   S 23  C 23



0

0





0  l 3C 1C 23

0  l 3S 1C 23 

;

0

 l 3S 23 



0

0





  C 1C 23 C 1S 23

  S 1C 23 S 1S 23

U 333   S 23  C 23



0

0





0  l 3C 1C 23

0  l 3S 1C 23 

0

 l 3S 23 



0

0





Tính Dij ta được:

1

1

1

D11  m1 (l 2) 2  m 2(l 2) 2 Cos 2 ( 2)  m3(l 3) 2 Cos 2 ( 23)  m3 (l 2) 2 Cos 2 ( 2) 

3

3

3

 l 3 l 2Cos( 2) Cos( 23)

D12  D 21 0

D13  D31 0

1

1

D 22  m3 (l 3) 2  m2 (l 2) 2  m3 (l 2) 2  m3 l 3 l 2 Cos( 3)

3

3

1

1

D 23  D32  m3 (l 3) 2  m3 l 3 l 2 Cos( 3)

3

2

1

D33  m3 (l 3) 2

3



Tính h1 ta được:



Võ Hồng Trơvi – Nguyễn Sĩ Bích



43



Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



H 111 0

1

1

m 2 (l 2) 2 Sin ( 2)Cos ( 2)  m3 (l 3) 2 Sin ( 23)Cos ( 23) 

3

3

1

 m3 (l 2) 2 Sin ( 2)Cos ( 2)  m3 l 3 l 2 Sin ( 223)

2

1

1

H 113  H 131  m3 (l 3) 2 Sin ( 23)Cos ( 23)  m3 l 3 l 2 Sin ( 23)Cos ( 2)

3

2

H 122 0

H 112  H 121 



H 123  H 132 0

H 133 0

1

 1



2

2

  m2 (l 2) Sin( 2)Cos( 2)  m3 (l 3) Sin( 23)Cos ( 23)  

.

.

3

3

 2  1 2  

 h1 



 

1



2

  m3 (l 2) Sin( 2)Cos( 2)  m3 l 3 l 2 Sin( 223)





2



1

 1

  . . 

   m3 (l 3) 2 Sin( 23)Cos( 23)  m3 l 3 l 2 Sin( 23)Cos( 2)  2  1 3 

2



 3

 



Tính h2 ta được:

1

1

H 211  m3 (l 3) 2 Sin ( 23)Cos ( 23)  m2 (l 2) 2 Sin ( 2)Cos( 2) 

3

3

1

 m3 l 3 m2 Sin ( 223)  m3 (l 2) 2 Sin ( 2)Cos ( 2)

2

H 212  H 221 0; H 222 0; H 231  H 213 0

H 223  H 232 



1

m3 l 3 l 2 Sin ( 3)

2



1

m3 l 3 l 2 Sin( 3)

2

1

1



2

2

 m3 (l 3) Sin( 23)Cos ( 23)  m2 (l 2) Sin( 2)Cos ( 2)  

.

3

3

 ( 1) 2 

 h2 

 1



2

  m3 l 3 m2 Sin( 223)  m3 (l 2) Sin( 2)Cos ( 2)



 2



.

.

.

1



  m3 l 3 l 2 Sin( 3)   2 3  m3 l 3 l 2 Sin( 3)  ( 3) 2

2



H 233 



Tính h3 ta được:

1

1

H 311  m3 (l 3) 2 Sin( 23)Cos( 23)  m3 l 3 l 2 Sin( 23)Cos( 2)

3

2

H 312  H 321 0; H 313  H 331 0; H 322 0; H 323  H 332 0; H 333 0

.

1

1



 h3  m3 (l 3) 2 Sin( 23)Cos ( 23)  m3 l 3 l 2 Sin ( 23)Cos ( 2)  ( 1) 2

2

3





Võ Hồng Trơvi – Nguyễn Sĩ Bích



44



Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



Thay các giá trị vừa tính trên, ta được phương trình động lực học cho cánh

tay dạng:

  1   D11



  2    D21

 3   D31



. 

 ..  

1   h1 ( , ) 

 C1 ( ) 

D13   ..  

. 





D23  2   h2 ( , )  C 2 ( )

D33 ..   h ( , . )  C 3 ( ) 

 3  3   

  





D

D

D



12

22



32



1.3.3 Động học ngược cho tay máy 3 bậc tự do dạng RRR:

Động học ngược được giải bằng phương pháp hình học

Ta có tọa độ x,y,z và ta sẽ tính các góc



L3

B



Y

L



L2

Y



y

z



Z

A



YY



D



t



1 , 2 , 3



Y



L1

Y

x Y



C

X



Hình 1



Tính các góc:



  cos 1 (



y

x2  y 2



)



Suy ra: 1  180  

Tọa độ điểm: A(0,0, L1 ); B(x,y,z)



L  AB  x 2  y 2  ( L1  z )2

Góc 1 là góc hợp bởi L2 và AB



L22  L2  L23

1  cos (

)

2 L2 L

1



Góc  2 là góc hợp bởi AB và AD



Võ Hồng Trơvi – Nguyễn Sĩ Bích



45



Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



x2  y 2

 2  cos (

)

L

1



Tính góc 2 ta chia làm 3 trường hợp:

z > L1 : khi đó 1 ,  2 > 0 như hình 1



1.



2  90  1   2

z = L1 : khi đó 1 > 0;  2 = 0



2.



Z



L2



L3

B

t



Y



Y

L

y



D



A



L1



Y



Y



Y



z



x



Y



C

X



Hình 2



2  90  1

z < L1 : khi đó  2 > 1



3.



Y



Z



Y



L2

B

t



L3



Y



D



L



Y



L1



y



Y



Y



z



A



x



Y



C

X



Hình 3



2  90  1   2

L22  L23  L2

3  cos (

)

2 L2 L3

1



1.4 Ứng dụng MATLAB.

Võ Hồng Trơvi – Nguyễn Sĩ Bích



46



Đồ án tốt nghiệp



GVHD: ThS. Trần Văn Trinh



Chương trình MATLAB là một chương trình viết cho máy tính PC nhằm hỗ trợ

cho việc tính tốn khoa học và kĩ thuật với các phần tử cơ bản là ma trận trên máy tính

do cơng ty “ The MATHWORKS” viết ra.

Thuật ngữ MATLAB là do hai từ MATRIX và LABORATORY ghép lại. Hiện

nay chương trình được sử dụng nhiều trong vấn đề giải quết các toán bài toán kỹ thuật

như: điều khiển tự động, xử lý tín hiệu số, phân tích dữ liệu, dự báo chuỗi v.v…

MATLAB được điều khiển thông qua tập lệnh tác động qua bàn phím hoặc lập trình

thơng qua các cú pháp lệnh đặt trong File ( M – file). Chương trình có thể liên kết với

các chương trình ngơn ngữ cấp cao như C, C++, Fortran…MATLAB ngày càng được

mở rộng bởi các Toolbox hay các hàm ứng dụng được xây dựng bởi người dùng.

Trong luận văn này chúng em chỉ sử dụng 2 Toolbox là Image Processing

Toolbox và OPC Toolbox. Image Processing Toolbox dùng cho việc xử lý ảnh lấy từ

Camera để xác định tọa độ vật và OPC Toobox dùng để kết nối MATLAB với PLC.



1.4.1 Xử lý ảnh trong MATLAB.

1.4.1.1 Các khái niệm cơ bản:



 Điểm ảnh:

Ảnh trong tự nhiên là ảnh liên tục về không gian và độ sáng. Khi ảnh được lưu

vào máy tính thì ảnh đã được số hóa. Số hóa là sự biến đổi gần đúng của một ảnh liên

tục thành một tập hợp điểm phù hợp với ảnh thật về vị trí khơng gian và độ sáng. Mỗi

điểm như vậy gọi là một điểm ảnh ( PEL : Picture Element ) hay còn gọi là Pixel.

Điểm ảnh Pixel là là một phần tử của ảnh số tại tọa độ (x,y) với độ sáng hoặc màu nhất

định. Tập hợp những điểm ảnh tạo thành ma trận ảnh.

 Độ phân giải của ảnh:

Độ phân giải ảnh là mật độ điểm ảnh được ấn định trên một ảnh số được hiển

thị. Khoảng cách các điểm ảnh được chọn sao cho mắt người thấy được sự liên tục của

ảnh và tạo nên mật độ phân bố theo hai trục x và y trong không gian hai chiều.

 Mức xám:

Mỗi điểm ảnh được đặc trưng cơ bản bởi tọa độ x, y của điểm ảnh và độ xám.

Mức xám của điểm ảnh là cường độ sáng của nó được gán giá trị số tại điểm đó. Các



Võ Hồng Trơvi – Nguyễn Sĩ Bích



47



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

Hình 1.22:Hệ trục tọa độ gắn cho tay máy 3 bậc tự do.

Tải bản đầy đủ ngay(0 tr)

×