Industrial Robot Report

Trường Đại học Bách khoa Hà Nội

Bộ môn Cơ học ứng dụng

Robot Simulator Robotics Simulation Utility using 3D Picture Control

Nguyễn Nho Ba –CĐT1 – K52

1

Tính toán thiết kế robot.



1.Tính số bậc tự do: k

f = λ (n-k) +

∑ f + f + f i

c

p

i =0

Với : + f : số bậc tự do của cơ cấu + f i : số bậc tự do chuyển động cho phép của khớp i + k : số khớp của cơ hệ + n : số khâu động của cơ hệ + λ : số bậc tự do của không gian cơ cấu thực hiện chuyển động + f c : số rằng buộc thừa + f p : số bậc tự do thừa

Thay số vào ta có: f = 6(3-3) + 3 + 0 +0 = 3

Vậy cơ cấu đã cho có 3 bậc tự do. 2.Xây dựng hệ toạ độ khảo sát:


2




3.Lập bảng Denavit-Hartenbeg(DH):

Với cách thiết lập hệ toạ độ ở mỗi khâu của cơ cấu, có thể thành lập được ma trận liên hệ giữa 2 hệ toạ độ liên tiếp. Hệ toạ độ thứ I có thể nhận được bằng cách biến đổi : + Hệ toạ độ thứ i-1 dịch chuyển theo trục z i-1 một khoảng d i + Tiếp theo, quay hệ trục toạ độ i-1 mới quanh trục z i-1 một góc θi-1 để chuyển trục x i-1 đến trục xi . + Tiếp tục thực hiện phép tịnh tiến hệ trục thu được ở bước trên dọc theo trục x i để gốc toạ độ Oi-1 chuyển đến O i. + Tiếp tục quay hệ trục toạ độ mới thu được quanh trục x i một góc αi-1 để đưa hệ trục toạ độ i1 trùng hệ trục toạ độ i. Bảng Denavit – Hartenberg (DH) Khớp thứ i 1 Nguyễn Nho Ba –CĐT1 – K52

di d1

θi θ1 3

ai 0

αi 90


Trường Đại học Bách khoa Hà Nội 2 3

0 0

Bộ môn Cơ học ứng dụng θ2 θ3

a2 = const a3 = const

0 0

4.Tính các ma trận truyền DH Ma trận truyền DH có dạng:

i-1

Ai

=

Theo bảng DH ta có

cosθ  sin θ   0  0 

1

0

A1 =

1

Đặt c1 = cos Ѳ1

0

A1

C1 S 1 = 0 0 

− sin θ

1

cos 90

sin θ1 sin 90

0cosθ1 

   

cosθ1cos90

−cosθ sin90

0sin θ1 

sin 90

cos90

d1

0

0

1

1

; s1 =sin Ѳ1

0

S 1

0

−C 1

1

0

0

0

0

  d1   1 0

cos θ 2 − sin θ2 cos 0 sin θ2 sin 0  sin θ cos θ cos 0 − cos θ sin 0 2 2 2 1 A2 =   0 sin 0 cos 0  0 0 0 


4

a 2 cos θ2 

   

a 2 sin θ2  0 1



cos θ 2 − sin θ 2  sin θ cos θ 2 2 =  0 0  0 0  Đặt c2 = cos Ѳ2


0

a 2 cos θ 2 

0

a 2 sin θ 2 

1

0

0

1

   

; s2 =sin Ѳ2

Ta được

c −s s c 1 A2 =  0 0 0 0  2

2

2

2

cos θ  sin θ 2 A3 =   0  0 

3

3

0

a 2 c2

0

a 2 s2

1

0

0

1

− sin θ

− cos θ sin 0

sin 0

cos 0

0

0

− sin θ

3

Đặt c3 = cos Ѳ3

sin θ3 sin 0

cos 0

cos θ3 cos 0

3

cos θ  sin θ =  0  0 

3

     

3

a3

  sin θ   0  1 

0

a 3 cos θ 3

cos θ3

0

a3

0

1

0

0

3

  sin 0   0  1 

a 3 cos θ3

3

; s3 = sin Ѳ3

Ta được

2

A3 =

c3 −s3 s c 3 3 0 0 0 0 

0

a 3 c3 

0

a 3 s3

1

0

0

1


    

5




Vậy ta có

c3 −s3 s c 3 3 2 T3 = 2A3 =  0 0 0 0 

0

a 3 c3 

0

a 3 s3

1

0

0

1

 c −s s c  0 0 0 0  2

1

= 1A2 .2T3 =

T3

c c - s s s c + s c =  0  0  2

2

0

3

3

2

2

2

     c3 −s3  s c . 3 3  0 0  0 0  

0

a2 c2 

0

a 3 c3 

0

a2 s2

0

a 3 s3

1

0

1

0

0

1

0

1

2

3

- s 3c 2 - s 2c 3

0

a 3c 2c 3 - a 3s 2 s3 + a 2c 2

3

2

- s 2s 3 + c 2c 3

0

a 3s 2c 3 + a 3c 2 s3 + a 2 s2

0

1

0

0

0

1

    

     

= A1 .1T3 =

T3

C1 S 1 = 0 0 

S 1

0

−C 1

1

0

0

0

c c - s s s c + s c   0  0  2

2

3

3

0

0

 . d1   1 0

a3 c2 c3 - a3 s2 s3 + a2 c2 

2

3

- s3 c2 - s2 c3

0

3

2

- s2 s3 + c2 c3

0 a3 s2 c3 + a3 c2 s3 + a2 s2

0

1

0

0

0

1


6

    



c c c - c s s s c c - s s s =   sc +sc  0 


1

2

3

1 2

3

- c1s3 c2 - c1 s2 c3

s1

1

2

3

1 2

3

- s1s3 c2 - s1s2 c3

−c

−s s + c c

0

0

0

2

3

3

2

2

3

2

3

  sa c c -sa s s +sa c  a s c + a c s + a s +d   1 

c1 a3 c2 c3 - c1 a3 s2 s3 + c1 a2 c2 1

1

3

2

3

2

3

3

1

3

3

2

2

3

3

1

2

2

2

2

1

5.Thiết lập hệ phương trình đông học robot Vị trí của khâu tác động cuối với khâu cố định bởi ma trận biến đổi thuần nhất 4x4 sau: 0

T3 =

với u =

;v=

;w=

;p=

Hệ phương trình động học của robot là: ux = c1c 2c 3 - c1s 2s3 ; uy = s1 c2c 3 - s1s 2s 3 ; uz =

s2 c3 + s3 c2

;

vx = - c1s3c 2 - c1s 2 c3 ; vy = - s1s 3c 2 - s1s 2c 3 ; vz =

−s s + c c 2

3

2

3

;

wx = s1 ; wy = -c1 ; wz = 0 ; px = c1a 3 c2 c3 - c1a 3 s2 s3 + c1a 2 c2 ;


7




py = s1a 3c2c 3 - s1a 3s 2s3 + s1a 2c 2 ; pz = a 3s 2 c3 + a 3c 2 s3 + a 2 s2

+d

1

6.Bài toán động học thuận a.Gán quy luật chuyển động cho các khâu của robot + q1 = θ 1 = t + q2 = θ2 = 2t + q3 = θ3 = 3t hay ta có ma trận:

 t    q = 2t   3t 

b.Phương pháp xác định vị trí của điểm tác động cuối và hướng của khâu thao tác theo quy luật đã cho Thay các giá trị q 1 , q2, q3 vào hệ phương trình động học của robot ta có: +Vị trí của điểm tác động cuối được xác định bằng toạ độ điểm tác động cuối E theo phương x,y,z của hệ toạ độ gốc tương ứng là p x, py, pz, thể hiện trong ma trận :

p = p(q) =

c1a 3c 2c 3 - c1a 3s 2 s3 + c1a 2c 2    Với p =  s1a 3c2c3 - s1a 3s 2 s3 + s1a 2c2   a 3s 2c3 + a 3c 2 s3 + a 2 s2 + d1 


8




 a3 cos q1cosq2 cosq 3 - a 3 cos q1 sin q 2sinq 3 + a 2 cos q1cosq 2    =  a 3sinq 1 cos q2cosq 3 - a 3sinq 1sinq 2sinq 3 + a 2sinq 1 cos q2   a 3 sin q2 cos q3 + a3 cos q2 sin q3 + a2 sin q2 + d1   a3 cos t.cos2t.cos3t- a 3 cos t.sin 2t.sin3t+ a 2 cos t.cos2t    ta có p(t) =  a 3 sin t.cos2 t.cos3t- a 3 sin t.sin2t.sin3 t+ a 2 sin t.cos2 t   a 3.sin 2t.cos3t + a3.cos 2 t.sin 3t + a2 sin 2 t + d1 

tại mỗi thời điểm t điểm cuối E ở 1 vị trí xác định +Hướng của khâu tác động: được xác định bằng toạ độ của các véctơ đơn vị u, v, w của hệ toạ độ gắn với khâu tác động cuối trong hệ toạ độ gốc, thể hiện trong ma trận cosin chỉ hướng:

C = C(q) =

c1c2c 3 - c1s2s3  = s1c 2c 3 - s1s2s3   s2 c3 + s3 c2

- c1s3 c2 - c1s2 c3 - s1s3 c2 - s1 s2 c3

− s2 s3 + c2 c3

 −c1  0  s1

=

cos q1 .cosq2 .cosq3 - cos q1 .sinq2 .sin q3  sinq cosq cosq - sinq sin q sinq 1 2 3 1 2 3   sinq2 cosq 3 + sinq 3 cosq 2

- cos q1 sin q3 cos q - 2 cos sinq q1 cosq 2

3

sin

−

cos

- cos t sin 3 tcos2t- cos tsin 2 tcos3 t

sin t

- sint.cos2 t.sin3 t- sin t.sin2t.cos3 t

− cos t

- sin t.sin3t + cos 2 t.cos 3 t

0

- sinq1 cosq2 sin q3sinq - sinq cosq 1 2 -sinq1 sinq3

+cosq 2

3

cosq 3

Thay q1 = t, q2 = 2t, q3 = 3t ta có: C = C(t) =

cos t.cos2t.cos3t- cos t.sin2t.sin 3 t  sin t cos 2tcos3t- sin t sin 2 tsin3t   sin2t cos 3t + sin 3t cos 2t Nguyễn Nho Ba –CĐT1 – K52

9




c. Ứng dụng matlab tính toán và vẽ quỹ đạo chuyển động của điểm tác động cuối E Dùng lập trình Matlab ta có với d 1 = a2 = a3 = 10 (cm) >> t=linspace(0,3*pi,300); >> x=10.*(cos(t).*cos(2*t)).*(cos(3*t))-10.*cos(t).*(sin(2*t)).*(sin(3*t)) +10.*cos(t).*(cos(2*t)); >> y=10.*(sin(t)).*(cos(2*t)).*(cos(3*t))-10.*(sin(t)).*(sin(2*t)).*(sin(3*t)) +10.*(sin(t)).*(cos(2*t)); >> z=10.*(sin(2*t)).*(cos(3*t))+10.*(cos(2*t)).*(sin(3*t))+10.*(sin(2*t))+10; >> plot3(x,y,z); Ta được quỹ đạo điểm cuối như hình vẽ

Quỹ đạo điểm E 7. tính toán tĩnh học.


10




Cho lực tác dụng vào khâu thao tác tại điểm E gồm vector lực F, và momen M (F=[F x , Fy , Fz ]T ; M=[Mx , My , Mz ]T). Tính lực và momen dẫn động tại các khớp đảm bảo cho robot cân bằng tĩnh. (bỏ qua ma sát và khối lượng các khâu). Ta xác định ma trận Jacobien của robot. Sử dụng:

 d x   dq  dq  d  y     d z   dq δ  = [ matran − Jacobien]  dq  x   δ y   dq    δ  z   dq

1

2

3

4

5

6

        

xác định ma trận Jacobien.

 p x  c a c c - c a s s + c a c   p   s a cc - s a s s+ s a c   y  =    z p  a s c + a c s+ a s+ d  1   1     1

3

2

3

1

3 2 3

1

2

2

1

3

2

3

1

3

1

2

2

3 2

3

3

2 3

2 3

2

2

1

=> các thành phần của một hàng của ma trận Jacobien.

 ∂ p x = = − as c c + s a s -s s a J  θ   ∂ p x = = − ca s c - c a c s - c a J  θ   ∂ p  J = x − ca s s - c a s c θ  11

1

3

2

3

1

3

2

3

3

2

3

1

3

2

3

1

c2

2

1

12

1

3

2

3

2

3

3

1

s2

2

2

13

1

1

3

∂ p y  = = ac c c - c a s s+c a c J  θ   ∂ p y J = = − sa s c - s a c s - s a s  θ   ∂ p  J = y − sa c s - s a s c θ  21

1

3

2

3

1

3

2

3

1

2

2

1

22

1

3

2

3

2

3

3

1

3

2

3

2

3

3

1

3

2

2

23

1

1

3


11




 ∂ p z = =0 J  θ   ∂ p z J = =a  θ   ∂ p  J = z − a θ  31

1

32

3

c2 c3 - a 3 s2 s3 + a 2 c2

2

33

3

s2s3 + a 3 c2 c3

3

=> ta thu được ma trận Jacobien 6x6. Xác định ma trận Jacobien

T 3

J so với khung tọa độ tay T 3.

Các khớp của robot đều là khớp quay : d i=0. Nên ta có.

T  T  T 

3

3

3

= T dxi = − uxi . pyi + uyi . pxi T J i = dyi = − vxi . pyi + vyi . pxi Ji = T dzi = − w xi . pyi + w yi . pxi

J1i

3

3

2

3

3

Các thành phần u, v, w, p xác định từ các ma trận biến đổi thuần nhất.

ui T 3 =  0

i

vi

wi

0

0

pi  1

 

Qua đó ta xác định được các thành phần của ma trận Jacobien

T 3

J so với khung tọa độ tay

robot T3. 

Lực và mô men của các khớp.

M

=

T 3

J

T T 3

.

F

Sau khi xác định được ma trận Jacobien Nếu biết lực tác dụng vào khâu thao tác E => ta sẽ xác định được mô men cần thiết của các khớp dựa vào phương trình trên.


12




Nếu biết mô men tác động vào khâu thao tác E tac cũng xác định được lực cần thiết của các khớp. 8. Tính toán động lực học. Yêu cầu : tính mô men động lượng của khâu 2 đối với khối tâm của nó. Khâu 2 quay với vận tốc góc. • •   ω = θ 1 + θ 2   

Dối với khâu 2, tọa độ khối tâm được xác định theo phương trình :

= l cosθ + l g cos (θ + θ ) y = l sin θ + g l sin ( θ + θ )

x2

1

2

1

1

2

1

2

1

1

2

2

Hai thành phần tốc độ của khối tâm trên 2 trục là : •

•

• •   x2 = − 1lsin θ1 θ 1 − g l2 sin ( θ1 + θ 2 )  θ 1+ θ 2    • • • •   y2 = l1 cosθ1 θ 1 + l g 2 cos ( θ 1 + θ 2 )  θ 1 + θ 2   

Động lượng của khối tâm khâu 2 là : •

k x

= m. x •

k y

= m. y

=>

k

=

k x2

+ k y

2

Mô men động lượng :

L = k. r Với r là vector tọa độ của khối tâm khâu 2.


13




9. Thiết kế và mô phỏng robot. Sử dụng CAD để thiết kế mô hình 2D :

Sử dụng labview để thiết kế và mô phỏng robot.

LABVIEW (Laboratory Virtual Instrumention Engineering Workbench). Hãng National Instruments cung cấp công cụ để thiết kế và mô phỏng robot. Robotics Simulation Utility using 3D Picture Control

VI này sử dụng để điều khiển và mô phỏng robot hoặc một đối tượng 3D bất kì. VI sử dụng cây điểu khiển (tree control) , các quan hệ trong sơ đồ hình cây là quan hệ cha-con giữa các đối tượng điều khiển.


14




Giao diện cửa sổ block diagram : dùng G-code để viết code cho hệ thống trên giao diện front panel. Trên block diagram, sử dụng các giả lập tín hiệu điều khiển thông qua các VI và các Sub’VI của chương trình để thay thế cho các thiết bị điều khiển thật. (VI=Virtual Instruments).


15




Sau khi chạy chương trình, giao diện trên cửa sổ front panel có các nút điều khiển và các nút nhập giá (numeric controls) trị thông số cánh tay robot. Các thông số điều khiển sẽ được hiển thị thông qua numeric indicators. Vị trí và trạng thái của robot thay đổi trong quá trình hoạt động sẽ được hiển thị thông qua numeric indicator. (trên front panel ). Trình tự thiết kế và mô phỏng robot.

Chạy chương trình (VI). – cần có đầy đủ Sub’VI và symbols hỗ trợ đi kèm, vì trong block diagram windown có sử dụng các khối thiết bị ảo Sub’VI để hỗ trợ việc điều khiển mô phỏng robot. Trên front panel windown, click Run (hình mũi tên play). Thiết lập khâu 0 (giá cố định).

Trong giao diện cửa sổ robot tree có các table robot configuration , type (kiểu hình dáng khâu robot), Joint type (kiểu khớp), Geometry Definition (kích thước của khâu), Pos WRT Parent (X,Y,Z) (vị trí của khâu trên trục tọa độ gắn với khâu/khớp trước đó), Rot WRT (X,Y,Z,Angle) (quay trục tọa độ một góc Angle), Color (màu sắc khâu).


16




Tạo giá cố định (khâu Base) . Thiết kế trong table edit/add/delete item. Ta sẽ tạo khâu base và add khâu này vào World Space. Mục name (nhập tên khâu = base), color (màu sắc), type (chọn box ), Geometry Definition (kích thước các chiều, Length X,Y,Z ),

(nhập các thông số như sau).


17




Kết quả thu được trên cửa sổ Camera :

Thiết lập khâu 1.

Khâu 1 là khâu cố định, ta add khâu 1 vào World Space. Name (K1), type (cylinder-trụ), color (red), Geometry Definition (height=chiều cao khối trụ, Radius=bán kính trụ, detail=chi tiết). Pos WRT Parent (X,Y,Z), Rot WRT (X,Y,Z,Angle) nhập như hình vẽ.

Ta thu được kết quả như hình sau : Nguyễn Nho Ba –CĐT1 – K52

18




Thiết lập khớp 1. (Joint1)

Name (Joint1), type (Joint), Joint type (Rot_Z =>khớp quay quanh trục Z). Thiết lập khâu 2 (K2)

Thiết kế trong table edit/add/delete item. Như hình vẽ, và kết quả thu được như sau :


19




Trong khâu 2 ta add thêm một khối cylinder để mô phỏng cho khớp ( chú ý : khớp (Joint) và khâu hoàn toàn khác nhau, ta add thêm thành phần này vào khâu 2 chỉ để cho minh họa cho khớp Joint2 add vào khâu 2 )


20




Hoàn toàn tương tự cho Joint2, Joint3 và khâu 2, khâu 3. Ta thu được kết quả như hình sau.

Ta thiết lập ở trạng thái các khâu thẳng hướng theo các trục để dễ dàng thiết kế (nhập vị trí, góc xoay các khâu và thiết lập vị trí các khớp).

Các khâu liên kết với nhau nhờ các khớp. Như vậy ta đã thiết kế xong phần tạo mô hình cho robot. Chạy mô phỏng nhờ việc thay đổi các thông số ở trên bảng điều khiển (thay đổi góc quay của các khớp). Trên bảng điều khiển sẽ mặc định có 7 thông số khớp điều khiển. Robot đang mô phỏng có 3 bậc tự do (DOF=3), vì vậy ta chỉ điều khiển 3 thông số khớp đầu tiên.


21




Thay đổi số khớp điều khiển bằng cách tăng hoặc giảm số khớp (sử dụng nút bấm button numeric control). Dừng chương trình (bấm Stop). Khi ta thay đổi thông số góc quay của các khớp => các khâu sẽ quay. Một vài hình ảnh kết quả thu được.


22





23





24


Industrial Robot Report

Recommend Documents