1. Sơ đồ nguyên lí tay máy và bảng thông số DH

Sơ đồ nguyên lí tay máy SCARA

Hệ trục tọa độ các khớp

1 | P a g e

a1 a2

d3

d4

1 2

4

a1 a2

d3

d4

O0=O'1

O4=O'4

z0

x0

z1

x1

z2

x2

z3

x3

z4

x4

1 2

4

P4

1

P4

0

O1=O'2 O2

O3=O'3

ai di αi γi

1 a1 0 0 γ1

2 a2 0 π γ2

3 0 d3 0 04 0 d4 0 γ4

Bảng thông số DH

2. Vận tốc tay gắp

Dựa vào bảng thông số DH, ta có:

Ma trận biến đổi kết quả

Aii-1 = T(z,d) T(z, γ) T(x,a) T(x, α)

Hay

Aii-1 = [c γi −s γi . c αi

sγ i c γ i . c αi

o0

s αi

0

cγ i . sα i

−c γi . sα i

c αi

0

a i . c γi

ai . s γi

d i

1]

Suy ra:

A10 = [c γ1 −s γ1

sγ 1 c γ 1

o0

00

0010

a1 . c γ 1

a1 . s γ1

01

]A2

1 = [c γ2 sγ 2

sγ 2 −c γ2

o0

00

00

−10

a2 . c γ2

a2 . s γ 2

01

]A3

2 = [1 00 1o0

00

0010

00d3

1]

2 | P a g e

A43 = [c γ 4 −sγ 4

sγ 4 c γ 4

o0

00

0010

00d4

1]

Xác định vị trí các khâu

A20 = A1

0 . A21 = [c γ1 −s γ1

sγ 1 c γ 1

o0

00

0010

a1 . c γ 1

a1 . s γ1

01

].[c γ2 sγ 2

sγ 2 −c γ2

o0

00

00

−10

a2 . c γ2

a2 . s γ 2

01

] = [c γ12 s γ12

sγ 12 −cγ 12

o0

00

00

−10

a2 . c γ 12+a1 . c γ 1

a2 . s γ 12+a1 . s γ 1

01

]A3

0 = A20 . A3

2 = [c γ12 s γ12

sγ 12 −cγ 12

o0

00

00

−10

a2 . c γ 12+a1 . c γ 1

a2 . s γ 12+a1 . s γ 1

01

] . [1 00 1o0

00

0010

00d3

1]

= [c γ12 s γ12

sγ 12 −cγ 12

o0

00

00

−10

a2 . c γ 12+a1 . c γ 1

a2 . s γ 12+a1 . s γ 1

−d3

1]

A40 = A3

0 . A43 = [c γ12 s γ12

sγ 12 −cγ 12

o0

00

00

−10

a2 . c γ 12+a1 . c γ 1

a2 . s γ 12+a1 . s γ 1

−d3

1]. [c γ 4 −sγ 4

sγ 4 c γ 4

o0

00

0010

00d4

1]

= [c γ12−4 sγ 12−4

sγ 12−4 −c γ12−4

o0

00

00

−10

a2 . c γ 12+a1 . c γ1

a2 . s γ 12+a1 . s γ 1

−d3+d4

1]

Vận tốc của tay gắp:

3 | P a g e

[ v4

ω4]=[ J1 J 2 J3 J 4 ] .[ γ1

γ2

d3

γ 4]

Ta có:

Chiều zi-1 của các trục khớp được tính theo công thức:

z i−1=Ri−10 . [001]

Suy ra:

z0=R00 .[001]=[1 0 0

0 1 00 0 1] .[001]=[001 ]

z1=R10 .[001]=[c γ 1 −sγ 1 0

s γ1 c γ1 00 0 1] .[001]=[001]

z2=R20 .[001]=[c γ 12 sγ 12 0

s γ12 −c γ12 00 0 −1] . [001 ]=[ 0

0−1]

z3=R30 .[001]=[c γ 12 sγ 12 0

s γ12 −c γ12 00 0 −1] . [001 ]=[ 0

0−1]

Áp dụng phương trình:

Pni−1=Ri−1

0 .r ii−1+Pn

i

P43=R3

0 . r43+P4

4=[c γ 12 sγ 12 0s γ12 −c γ12 0

0 0 −1] . [a4 . c γ4

a4 . s γ 4

d 4]=[ 0

0−d4

]P4

2=R20 . r3

2+P43=[c γ12 s γ12 0

sγ 12 −cγ 12 00 0 −1] .[a3 . c γ3

a3 . s γ 3

d3]+[ 0

0−d4

]=[ 00

−d 4−d3]

4 | P a g e

P41=R1

0 . r21+P4

2=[c γ1 −s γ1 0sγ 1 c γ 1 00 0 1] .[a2 . c γ 2

a2 . s γ 2

d2]+[ 0

0−d4−d3

]=[ a2 . c γ12

a2 . s γ 12

−d4−d3]

P40=R0

0 . r10+ P4

1=[1 0 00 1 00 0 1 ] .[a1 .c γ 1

a1 . s γ1

d1]+[ a2 . c γ 12

a2 . s γ12

−d 4−d3]=[a2. c γ 12+a1. c γ1

a2 . s γ 12+a1. s γ 1

−d 4−d3]

Vận tốc của tay gắp:

[ v4

ω4]=[ J1 J 2 J3 J 4 ] .[ γ1

γ2

d3

γ 4]

Tìm ma trận Jacobi:

Đối với khớp xoay:

J i=[ zi−1 .Pni−1

zi−1]

Đối với khớp tịnh tiến:

J i=[ zi−1

0 ]Khớp 1:

J1=[ z0 . P10

z0]=[[

x0 y0 z0

0 0 1a2 . c γ12+a1 . c γ1 a2 . s γ 12+a1 . s γ 1 −d4−d3

]001

] = [

−(a2 . s γ12+a1 . s γ1 )a2 . c γ12+a1 . c γ1

0001

]5 | P a g e

J2=[ z1 .P21

z1]=[[

x0 y0 z0

0 0 1a2 . c γ 12 a2 . s γ12 −d4−d3

]001

] = [−(a2 . s γ12 )

a2 . c γ 12

0001

]J3=[ z2

0 ]=[00

−1000

]J4=[ z3 . P4

3

z3]=[[

x0 y0 z0

0 0 −10 0 −d4

]00

−1] = [

00000

−1]

Vận tốc của tay gắp:

x=[ v4

ω4]=[ J1 J 2 J 3 J4 ] . [ γ 1

γ 2

d3

γ 4]

¿ [−( a2 . s γ 12+a1 . s γ 1 )

a2 . c γ 12+a1 .c γ 1

0001

−( a2 . s γ12 )a2 . c γ12

0001

00

−1000

00000

−1] .[ γ1

γ2

d3

γ4]

3. Giải bài toán động lực học tay gắp

3.1. Ma trận quán tính

I ii= 1

12mi . Ii

2 .[0 0 00 1 00 0 1]

Ma trận quán tính khâu i viết trong hệ tuyệt dối.

6 | P a g e

I i=Ri0 . I i

i .( Ri0 )T

i =1;

R10=[c γ1 −s γ1 0

s γ1 cγ 1 00 0 1] (R1

0)T=[ c γ1 s γ1 0

−sγ 1 c γ1 00 0 1]

I 1=R10 . I 1

1 . (R10 )T=[cγ 1 −sγ 1 0

s γ1 c γ1 00 0 1] . [0 0 0

01

12m1 I 1

2 0

0 01

12m1 I 1

2] . [ c γ 1 s γ1 0−s γ1 cγ 1 0

0 0 1]

= 1

12m1a1

2 .[ sγ1

2 −c γ 1 . s γ1 0

−sγ 1 . c γ 1 cγ 1

2 0

0 0 1]

i =2;

R20=[c γ12 s γ 12 0

s γ12 −c γ12 00 0 −1] (R2

0 )T=[c γ 12 sγ 12 0s γ12 −c γ12 0

0 0 −1]I 2=R2

0 . I 22 . ( R2

0 )T

¿ [cγ 12 s γ12 0s γ12 −c γ12 0

0 0 −1]. [0 0 0

01

12m2 I 2

2 0

0 01

12m2 I 2

2] .[c γ12 s γ12 0sγ 12 −c γ 12 00 0 −1]

= 1

12m2a2

2 .[ sγ 12

2 −c γ12 . s γ 12 0

−sγ 12 . c γ12 cγ12

2 0

0 0 1]

7 | P a g e

i =3;

R30=[c γ12 s γ 12 0

s γ12 −c γ12 00 0 −1] (R3

0 )T=[c γ 12 sγ 12 0s γ12 −c γ12 0

0 0 −1]

I 3=R30 . I 3

3 . ( R30 )T=[c γ12 s γ12 0

s γ 12 −c γ 12 00 0 −1] .[ 1

12m3 I3

2 0 0

01

12m3 I 3

20

0 0 0] .[c γ 12 sγ 12 0

s γ12 −c γ12 00 0 −1]

= 1

12m3 a3

2 . [1 0 00 1 00 0 0]

i =4;

R40=R3

0 . R43=[c γ12 s γ 12 0

sγ 12 −c γ12 00 0 −1] .[c γ 4 −s γ4 0

s γ4 c γ 4 00 0 1]=[c γ 12−4 s γ12−4 0

s γ12−4 −cγ 12−4 00 0 −1]

I 4=R40 . I 4

4 . (R40 )T

¿ [cγ 12−4 s γ12−4 0s γ12−4 −cγ 12−4 0

0 0 −1] .[0 0 0

01

12m4 I 4

2 0

0 01

12m4 I 4

2 ] .[c γ12−4 sγ 12−4 0sγ 12−4 −c γ12−4 0

0 0 −1]=

112

m4 a42 . [ sγ12−4

2 −c γ12−4 . s γ12−4 0

−s γ12−4 . c γ12−4 cγ 12−4

2 0

0 0 1]

8 | P a g e

3.2. Tính vận tốc khối tâm của từng khâu:

Tính ma trận Jacobi của vận tốc từng khối tâm:

Pc 10 =R0

0 . r10+Pc 1

1 =[1 0 00 1 00 0 1] . 1

2. [a1 . c γ 1

a1 . s γ1

0 ]=12

.[a1 . c γ1

a1. s γ 1

0 ]Pc 2

0 =R00 . r1

0+Pc 21 =[c γ 1 −sγ 1 0

s γ1 c γ1 00 0 1] . 1

2.[a2 . c γ2

a2. s γ 2

0 ]=12

. [a2. c γ 2

a2 . s γ2

0 ]Pc 2

1 =R10 . r2

1+ Pc22 =[c γ 1 −s γ 1 0

s γ1 cγ 1 00 0 1] . 1

2.[a2 . c γ 2

a2. s γ2

0 ]=12

.[a2. c γ2

a2 . s γ2

0 ]Pc 3

2 =R20 . r3

2+Pc 33 =[c γ 12 sγ 12 0

s γ12 −c γ12 00 0 −1] . 1

2.[ 0

0d3

]=12

. [ 00d3

]Pc 4

3 =R30 .r 4

3 +Pc 44 =[1 0 0

0 1 00 0 1 ]. 1

2.[ 0

0d4

]=12

.[ 00d4

]9 | P a g e

1 2

d3

d4

O0

O4

z0

x0

z1

x1

z2

x2

z3

x3

z4

x4

1 2

4

PC4

O1 O2

O3

1

PC40

PC10

PC21

PC32

PC43

C1 C2

C3

C4

m1 m2

m3

m4

Pc 42 =r3

2(0)+Pc 43 =R2

0 .r 32(2)+Pc 4

3 =[ 00

−d3]+[ 0

0−d4

2]=[ 0

0

−d3−d4

2]

Pc 41 =r2

1+Pc 42 =R1

0. r21(1)+Pc 4

2 =[ 00

−d3−d4

2]+[a2 . c γ12

a2 . s γ 12

0 ]=[ a2 . c γ12

a2 . s γ12

−d3−d4

2]

Pc 40 =R0

0 .r10+Pc4

1 =[1 0 00 1 00 0 1] .[a1. c γ1

a1 . s γ 1

0 ]+[ a2 . c γ 12

a2 . s γ12

−d3−d4

2]=[a1 . c γ 1+a2 . c γ12

a1 . s γ 1+a2 . s γ12

−d3−d 4

2]

Jacobi:

Jvij =z j−1 . Pci

j−1

Jωij =z j−1

Jv 11 =z0 . Pc1

0 =[ i0 j0 k0

0 0 112

a1 . c γ 112

a1 . s γ1 0 ]=12

.[−a1 . c γ 1

a1 . s γ1

0 ]

Jω 11 =z0=[001]

Jv 21 =z0 . Pc2

0

¿ [ i0 j0 k 0

0 0 112

a2. c γ12+a1 . c γ112

a2. s γ 12+a1 . s γ 1 0 ] [−(a1 . s γ 1+12

a2 . s γ 12)

12

a2 . c γ12+a1 . c γ1

0]

Jω 2(1 )=z0=[001]

10 | P a g e

Jv 22 =z1 . Pc2

1 =[ i0 j0 k 0

0 0 112

a2 .c γ 1212

a2 . s γ12 0 ]=[−( 12

a2 . s γ12)

12

a2 . c γ 12

0]

Jω 2(2 )=z1=[001]

Pc 30 =P2

0 (0)+Pc 32 =[a1 . c γ1+a2. c γ12

a1 . s γ1+a2. s γ 12

0 ]+ 12

.[ 00

−d3]=[a1 . c γ1+a2. c γ12

a1 . s γ1+a2. s γ 12

−12

d3

]Jω 3

1 =0=[000]J v 31 =[001 ]J v 3

2 =[001]=z1 J ω32 =0=[000]

Pc 31 =Pc 3

2 +r21=R2

0 . r32+R0

0 . r21

¿ [cγ 12 s γ12 0s γ12 −c γ12 0

0 0 −1]. 12

.[ 00d3

]+[a2 . c γ 2

a2. s γ2

0 ]=[a2 .c γ 2

a2 . s γ2

−12

d3 ]Jv 3

3 =[ 00

−1]=z2

Jv 41 =z0 . Pc 4

0 =[ i0 j0 k0

0 0 1

a2 . c γ12+a1 . c γ1 a2 . s γ 12+a1 . s γ 1 −d3−d4

2]

¿ [−(a¿¿1 . c γ1+a¿¿2 . c γ12)¿a1. s γ1+a2 . s γ 12¿0 ]

Jv 42 =z1 . Pc 4

1 =[ i0 j0 k0

0 0 1

a2 . c γ12 a2 . s γ12 −d3−d4

2]=[−a2 . c γ 2

a2 . s γ 2

0 ]Pc 4

2 =r32(0)+Pc 4

3 =Pc 30 =R2

0 r32(0)+Pc 4

3

11 | P a g e

¿ [ 00

−d3]+[ 0

0−d4

2]=[ 0

0

−d3−d4

2]

Pc 41 =r2

1+Pc 42 =Pc 4

2 =R10 r2

1(1)+Pc 42

¿ [ 00

−d3−d4

2]+[a2 . c γ2

a2 . s γ 2

0 ]=[ a2. c γ2

a2. c γ2

−d3−d 4

2]

Jv 43 =z2 . Pc 4

2 =[ i0 j0 k0

0 0 −1

0 0 −d3−d4

2]=[000]

Jv 44 =z3 . Pc 4

3 =[ i0 j0 k 0

0 0 −1

0 0−d4

2]=[000]

Vận tốc khối tâm của cơ cấu tay máy 4 khâu:

[V c1 x0

V c1 y0

V c1 z0

ωc1 x0

ωc1 y0

ωc1 z 0

]=Jc 1. [ γ 1

γ 2

d3

γ 4]=[

−12

a1 . s γ1 0 0 0

12

a1 . c γ 1 0 0 0

0 0 0 00 0 0 00 0 0 01 0 0 0

] [ γ1

γ2

d3

γ 4]

[V c2 x0

V c2 y0

V c2 z0

ωc2 x0

ωc2 y0

ωc1 z 0

]=Jc 2. [ γ 1

γ 2

d3

γ 4]=[

−12

a2 . s γ 12 0 0 0

12

a1. c γ1 0 0 0

0 0 0 00 0 0 00 0 0 01 0 0 0

] [ γ1

γ2

d3

γ 4]

12 | P a g e

[V c3 x0

V c3 y0

V c3z0

ωc3 x0

ωc3 y0

ωc3

z0

]=J c3[ γ 1

γ 2

d3

γ 4]=[

0 0 0 00 0 0 01 1 −1 00 0 0 00 0 0 01 1 0 0

] [ γ 1

γ 2

d3

γ 4]

[V c4 x 0

V c4 y0

V c4 z0

ωc4 x0

ωc4 y0

ωc4

z0

]=J c4[ γ 1

γ 2

d3

γ4]=[

−( a2 s12+a1 s1 ) −a2 s12 0 0

a2 c12+a1 c1 a2 c12 0 00 0 0 00 0 0 00 0 0 01 1 −1 −1

][ γ1

γ2

d3

γ 4]

Ma trận quán tính cơ cấu chấp hành

Thay J,I vào phương trình M=∑

i=1

n

(J ViT mi JVi+Jωi

T Ii J ωi)

Ta được:

M=JVC 1

T m1 J VC 1+JωC1

T I1 J ωC1+JVC 2

T m2 JVC 2+J ωC2

T I 2 JωC2

+JVC 3T m3J VC 3+J ωC 3

T I 3 J ωC 3+J VC 4T m4 JVC 4+J ωC 4

T I 4 J ωC 4

13 | P a g e

JVC 1

T m1 J VC1 =[−12

a1 s112

a1c1 0

0 0 00 0 00 0 0

] .m1[ 12

a1 s1 0 0 0

12

a1 c1 0 0 0

0 0 0 0]=[ 1

4a

1

2 m1 0 0 0

0 0 0 00 0 0 00 0 0 0

]JωC 1

T I 1 J ωC1=I 1[0 0 1

0 0 00 0 00 0 0

] .[0 0 0 00 0 0 01 0 0 0 ]=[ 1

12a

1

2 m1 0 0 0

0 0 0 00 0 0 00 0 0 0

]JVC 2

T m2 JVC 2=[−(12

a2 s12+a1 s1) (12 a2 c12+a1 c1) 0

0 0 00 0 00 0 0

]. m2 [−(12 a2 s12+a1 s1) −12

a2 s12 0 0

(12

a2 c12+a1 c1) 12

a2 c12 0 0

0 0 0 0]

¿ [(a12+a1 a2 c2+

14

a22) (a1 a2 c2+

14

a22) 0 0

(12

a1 a2 c2+14

a22) 1

4a2

2 0 0

0 0 0 00 0 0 0

]. m2

JωC 2T I2 J ωC2

=I 2[0 0 10 0 10 0 00 0 0

] .[0 0 0 00 0 0 01 1 0 0 ]= 1

12a

2

2 m2[1 1 0 01 1 0 00 0 0 00 0 0 0

]JVC 3

T m3J VC3=[0 0 10 0 10 0 −10 0 0

].m3 [0 0 0 00 0 0 01 1 −1 0 ]=[1 1 −1 0

1 1 −1 00 0 0 00 0 0 0

]m3

14 | P a g e

JωC 3T I3 J ωC3

=I 3[0 0 00 0 00 0 00 0 0

] .[0 0 0 00 0 0 00 0 0 0 ]=[0 0 0 0

0 0 0 00 0 0 00 0 0 0

]JVC 4

T m4 J VC 4=[−(a2 s12+a1 s1 ) (a2 c12+a1 c1 ) 0

−a2 s12 a2 c12 00 0 00 0 0

]. m4 [−(a2 s12+a1 s1) −a2 s12 0 0

(a2 c12+a1 c1 ) a2 c12 0 0

0 0 0 0 ]¿ [(a1

2+2 a1 a2 c2+a22) (a1 a2 c2+a2

2) 0 0

(a1 a2 c2+a22) a2

2 0 0

0 0 0 00 0 0 0

] . m4

JωC 41

T I 4 JωC 4 =I 4 [0 0 10 0 10 0 −10 0 −1

]. [0 0 0 00 0 0 01 1 −1 −1 ]=I 4 [ 1 1 −1 −1

1 1 −1 −1−1 −1 1 1−1 −1 1 1

]M=[M 11 M 12 M 13 M 14

M 21 M 22 M 23 M 24

M 31 M 32 M 33 M 34

M 41 M 42 M 43 M 44]

M 11=14

m1 a12+1

12m1a1

2+m2(14 a22+a1

2+a1a2c2)+112

m2a22+m3

+m4 (a22+a1

2+2a1 a2c2)+112

m4 a42

¿13

m1a12+m2(13 a2

2+a12+a1a2 c2)+m3+m4 (a2

2+a12+2 a1a2c2)+1

12m4a4

2

15 | P a g e

M 12=14

m2a22+

12

m2 a1a2c2+112

m2a22+m3+m4 a2

2+m4a1a2c2+112

m4a42

¿13

m2a22+1

2m2a1a2c2+m3+m4a2

2+m4 a1 a2c2+112

m4 a42

M 13=−m3−1

12m4 a4

2

M 14=− 112

m4 a42

M 21=14

m2a22+

12

m2 a1a2c2+112

m2a22+m3+m4 a2

2+m4 a1a2 c2+112

m4a42

¿13

m2a22+1

2m2a1a2c2+m3+m4a2

2+m4 a1 a2c2+112

m4 a42

M 22=14

m2a22+

112

m2 a22+m3+m4 a2

2+112

m4 a42

¿13

m2a22+m3+m4a2

2+112

m4 a42

M 23=−m3−1

12m4 a4

2

;M 31=−m3−

112

m4 a42

M 24=− 112

m4 a42

;M 32=−m3−

112

m4 a42

M 33=m3+1

12m4 a4

2

;M 41=− 1

12m4 a4

2

M 34=1

12m4 a4

2

;M 42=− 1

12m4 a4

2

M 43=M 44=1

12m4 a4

2

Tính Vi và Gi

V i=∑j=1

n

∑k=1

n

(∂ M ij

∂ qk

−12

∂ M jk

∂ q i) qk q j

16 | P a g e

⇒V 1=(∂ M 11

∂ q1

−12

∂ M 11

∂ q1) γ 1

2+(∂ M 11

∂ q2

−12

∂ M 12

∂ q1) γ1 . γ 2+(∂ M 11

∂q3

−12

∂ M 13

∂q1) γ1 . d3

+(∂ M 11

∂ q4

−12

∂ M 14

∂ q1) γ 1. γ4+(∂ M 12

∂q1

−12

∂ M 21

∂ q1) γ 1. γ2+(∂ M 12

∂ q2

−12

∂ M 22

∂q1) γ 2

2

+(∂ M 12

∂ q3

−12

∂ M 23

∂ q1) γ 2 . d3+(∂ M 12

∂q4

−12

∂ M 24

∂ q1) γ 2 . γ 4+(∂ M13

∂ q1

−12

∂ M 31

∂q1) γ1 . d3

+(∂ M 13

∂ q2

−12

∂ M 32

∂ q1) γ 2 . d3+(∂ M 13

∂q3

−12

∂ M 33

∂ q1) d3

2+(∂ M 13

∂ q4

−12

∂ M 34

∂q1) d3 . γ 4

+(∂ M 14

∂ q1

−12

∂ M 41

∂q1) γ1 . γ 4+(∂ M 14

∂q 2

−12

∂ M 42

∂ q1) γ 2 . γ 4+(∂ M 14

∂ q3

−12

∂ M 43

∂ q1) d3 . γ 4

+(∂ M 14

∂ q4

−12

∂ M 44

∂q1) γ 4

2

=⇔V 1=−m2 a 1 a2 s2( γ 1 γ 2+

12

γ2

2)−m4 a 1 a2 s2(2 γ1˙γ2 +γ2

2)

V 2=( 12

m2 a1 a2 s2+m4 a1a2s2) γ12

V3 = 0

V4 = 0

Tính Gi : Gi=−∑

j=1

n

mi gT J Vji

17 | P a g e

G1=−m1gT J VC1

1 −m2 gT JVC2

1 −m3 gT JVC3

1 −m4 gT JVC 41

=−m1 [0 0 −g ] [−12

a1 s1

12

a1c1

0]−m2 [ 0 0 −g ] [a1 s1−

12

a2 s12

a1 c1+12

a2 c12

0]

−m3 [ 0 0 −g ] [001 ]−m4 [0 0 −g ] [−(a1s1+a2s12 )a1c1+

12

a2c12

0]

G2=−m1 gT J VC1

2 −m2 gT JVC2

2 −m3 gT JVC3

2 −m4 gT JVC 42

= m1[0¿0¿−g ] [000] - m2 [0¿0¿−g ][−12

a2 Sγ12

12

a2Cγ12

0] – m3 [0¿0¿−g ][001]

– m4 [0¿0¿−g ] [−a2 Sγ 12

a2 Cγ12

0 ]G4 = – m1 gTJvc 1

(3 ) – m2 gTJvc 2(3 ) – m3 gTJvc 3

(3 ) – m4 gTJvc 4(3 )

= m1[0¿0¿−g ] [000] – m2 [0¿0¿−g ][000] – m3 [0¿0¿−g ][001] – 0

= – m3g

G4 = 0

Phương trình chuyển động Lagrang:

Lực tổng quát: Q = τ + JTFc – fr

(Nếu bỏ qua lực ma sát tại các khớp thì fr = 0).

Jacobin cho khâu tác động cuối:

18 | P a g e

J =

[−(a1sγ1+a1sγ12) −a1sγ12 0 0

a1cγ 1+a1 cγ 12 a1 cγ12 0 0

0 0 −1 00 0 0 00 0 0 01 1 0 −1

]JTF =

[−(a1sγ1+a1sγ12) a1cγ1+a1 cγ12 0 0 0 1−a1sγ12 a1cγ12 0 0 0 1

0 0 −1 0 0 00 0 0 0 0 −1

]

[Fxn

Fyn

Fzn

Mxn

Myn

Mzn

] =

[F1

F2

F3

F4]

[Q1

Q2

Q3

Q4] =

[τ1

τ2

τ3

τ4] +

[F1

F2

F3

F4]

Phương trình động lực học Lagrang:

∑j=1

n

M ij q ij+v i+Gi=Qi=τ i+ Fi ị (i= 1,2,3,4)

τ1+F1=[(13 m1+m2+m4 )a12+(m1+2 m4) a1a2 c2+m3+(13 m2+m4)a2

2] γ1

+[(13 m2+m4)a22+(12 m2+m4)a1 a2c2+m3+

112

m4 a42 ] γ 2+

[(−m3−112

m4 a42) γ3−(112

m4 a42) γ4 ]+[−m2a1a2 s2( γ1 γ2+

12

γ2

2 )][−m4 a1a2 s2 (2 γ1 γ2 +γ

2

2)]+m3 g

19 | P a g e

τ2+F2=[(13 m2+m4)a22+(1

2m2+m4)a1a2c2+m3+

112

m4 a42 ] γ 1

+[(13 m2+m4)a22+m3+

112

m4 a42 ] γ2−

[(m3+112

m4 a42) γ 3−(112

m4 a42 ) γ 4 ]+[−1

2m2 a1a2 s2 ]

[−m4 a1 a2s2 (γ 1

2) ]+m3 g

τ3+F3=[−(m3+112

m4 a42)] γ 1−[m3+

112

m4 a42 ] γ 2+

[(m3+112

m4 a42) γ 3+(112

m4 a42) γ 4]−m3 g

τ4+F4=[−(112m4 a4

2)] γ1−[112m4 a4

2 ] γ2+

[(112m4 a4

2) γ 3+(112

m4a42) γ 4 ]

4.1. Chọn động cơ:

- Loại động cơ

Truyền động điện được dùng khá nhiều trong kỹ thuật robot, vì có nhiều ưu điểm như điều khiển đơn giản không phải dùng các bộ biến đổi phụ, không gây ô nhiễm môi trường, các động cơ điện có thể lắp trực tiếp trên các khớp quay.

Tuy nhiên so với truyền đọng thủy lực hoặc khí nén thì thuyền động điện có côngg suất thấp và thường phải gắn thêm hộp giảm tốc(vì các khâu của robot có chuyển động thấp).

Trong kỹ thuật robot, về nguyên tác thì động cơ chia làm 2 loại: một chiều hoặc xoay chiều. Nhưng thực tế dùng loại đọng cơ DC nhiều hơn, đó là động cơ một chiều và đọng cơ bước.

Ngày nay, do công nghệ phát triển trong việc nghiên cứu động cơ xoay chiều nên có xu hướngchuyển sang động cơ xoay chiều để tránh may thêm bộ thiết bị nguồn điện một chiều. Ngoài ra bộ động cơ điện một chiều không có chổi góp cũng được sử dụng vào kỹ thuật robot.

Động cơ điện DC:

Động cơ điện một chiều gồm có 2 phần:

20 | P a g e

Stato cố định với các cuộn dây có dòng điện cảm hoặc dùng nam châm vĩnh cửu. Phần này còn được gọi là phần cảm. Phần cảm được tạo nên từ thông trong khe hở không khí.

Rôt với các thanh dẫn. Khi có dòng điện chạy qua với các dòng từ thông xá định, rôt sẽ quay. Phần này gọi là phần ứng.

Tùy cách đấu day giữa phần cảm và phần ứng, ta có những loại động cơ điện một chiều khác nhau:

Động cơ kích từ nối tiếp:

Động cơ kích từ song song:

- Công suất động cơ:

Thông số của robota1 = 425mm m1 = 25kg tải m5 = 5kga2 = 375mm m2 = 20kgd3 = 100mm m3 = 10kgd4= 150mm m4 = 2kgTổng moment xoắn cực đại tại khớp 1

MZ0 =

12

a1 P1+(a1+12

a2 ) P2+(a1+a2 )(P3+P4+P5 )

= 53,125 + 122,5+136= 311,625 (Nm)

Tốc độ tối đa của tay máy là1m/sTốc độ quay khớp 1 tối đa

( 12π . 0,8 )/( 1

60 )=11 ,37vòng /phút

Công suất động cơ:

N=311 ,625 .11 ,379550

=0 , 37(kW)

Chọn động cơ DC công suất 0.5kW

4.2. Xây dựng hàm truyền cho bộ điều khiển:

Điều khiển tỉ lệ sai lệch (PE: Proptional Error)

Nguyên tắc cơ bản của phương pháp là làm cho hệ thống thay đổi theo chiều hướng có sai lệch nhỏ nhất.

Điều khiển tỉ lệ - đạo hàm (PD)

Điều khiển tỉ lệ tích phân – đạo hàm (PID)

21 | P a g e

Hàm truyền chuyển động của mỗi khớp động:

- Xét sơ đồ truyền động của động cơ điện một chiều với tín hiệu vào là điện áp Ua

đặt vào phần ứng, tín hiệu ra là góc quay γ của trục động cơ kiểu kích từ độc lập.

Sơ đồ động cơ điện một chiều

Trong thực tế, trục động cơ được nối với hộp giảm tốc rồi tới trục phu tải như hình 9.4. Gọi n là tỉ số truyền, γ là góc quay của trục phụ tải, ta có:

γL(t) = n γm(t)

γ L(t) = n γ m(t)

γ L(t) = n γ m(t)

Sơ đồ động cơ điện cùng phụ tải.

Momen trên trục động cơ bằng tổng momen cần để động cơ quay, cộng với momen phụ tải quy về trục động cơ:

M(t) = Mm(t) + ML*(t)

Ký hiệu:

Jm : Momen quãn tính của động cơ.

JL : Momen quán tính phụ tải.

22 | P a g e

Ta có:

Mm(t) = Jm γ m(t) + fm γ m(t)

ML(t) = JL γ L(t) + fL γ L(t)

Trong đó: fm và fL là hệ số cản của động cơ và của phụ tải.

Theo định luật bảo tồn năng lượng, công do phụ tải sinh ra, tính trên trục phụ tải là ML γm phải bằng công quy về trục động cơ M L

¿γm. Từ đó ta có:

M L¿ (t) =

M L( t)γ L(t )γ m(t) = nML(t)

= n2[ JLγ m(t) + fL γ m(t) ]

= (Jm + n2JL) γ m(t) + fmn2fL)γ m(t)

Hay: M(t) = Jγ m(t) + fγ m(t)

Với: J = Jm + n2JL : Momen quán tính tổng hiệu dụng.

f = fm + n2fL : Hệ số ma sát tổng hiệu dụng.

Momen trên trục động cơ phụ thuộc tuyến tính với cường độ dòng điện phần ứng và không phu thuộc vào góc quay và vận tốc góc, ta có:

M(t) = Kaia(t)

Với: ia : Cường độ dòng điện phần ứng.

Ka : Hệ số tỉ lệ momen.

Áp dụng định luật Kirchhoff cho mạch điện phần ứng:

Ua(t) = Raia(t) + Lad ia(t )

dt + eb(t)

Với Ra, La: Điện trở và điện cảm phần ứng.

eb : Sức phản điện động của động cơ.

Eb(t) = Kbγ m(t)

Kb : Hệ số tỉ lệ của sức phản điện động.

Sử dụng phép biến đổi Laplace, ta có:

Ia(s) = U a (s )−s Kb γ m(s)

Ra+s La

M(s) = s2Jγ m(s) + sfγ m(s) = KaIa(s)

γ m(s) = Ka I a (s )s2 J+sf

23 | P a g e

= Ka[U a ( s )−s Kb γ m ( s)

(s2 J +sf )(Ra+s La)]

U a (s )−s Kb γ m(s)

γm(s) = (s2 J +sf )(Ra+s La)

Ka

U a (s )γ m(s ) =

(s2 J+sf ) ( Ra+s La )+s K b Kb

Ka

Hay: γ m(s )U a (s ) = Ka

(s2 J+sf ) ( Ra+s La )+s K b Kb

Đây là hàm truyền cần xác định, nó là tỉ số giữa tín hiệu ra (góc quayγ m ) và tín hiệu vào của hệ thống (điện áp Ua). Vì hệ thống gồm có động cơ và phụ tải nên tín hiệu ra thực tế là góc quay của trục phụ tải γL , do đó hàm truyền chuyển động 1 bậc tự do của tay máy là:

γ m(s )U a (s ) = n Ka

s [ (sJ +f ) ( Ra+s La )+ Kb K b]

Và ta có sơ đồ khối tương ứng với hàm truyền trên là:

Sơ đồ khối hàm truyền

Vì thành phần điện cảm của phần ứng La thường quá nhỏ so với các nhân tố ảnh hưởng cơ khí khác nên bỏ qua.

Nên:γ L(s)U a (s ) = n Ka

s (s Ra J+ Ra f +K b Kb)(¿)

Điều khiển vị trí:

Mục đích của việc điều khiển là làm sao cho động cơ chuyển dịch khớp di động đi một góc bằng góc quay đã tính toán để đảm bảo quỹ đạo đã chọn trước. Việc điểu

24 | P a g e

khiển được thực hiện như sau: theo tín hiệu sai lệch giữa giá trị thực tế và giá trị tính toán của góc mà điều chỉnh điện áp Vq(t) đặt vào động cơ. Nói cách khác, để điều khiển động cơ theo quỹ đạo mong muốn phải đặt động cơ điện áp tỉ lệ thuận với độ sai lệch góc quay của khớp động.

U a (t )=K p e(t )

n=

K p(~γ L (t )−γ L (t ))n

¿

Trong đó: Kp : Hệ số truyền tín hiệu phản hồi vị trí.

e (t )=~γ L ( t )−γ L (t ): độ sai lệch góc quay.

Giá trị góc quay túc thời ~γ L(t) được đo bằng cảm biến quang học hoặc chiếc áp.

Biến đổi laplace của phương trình (*) ta được:

Ua ( s)=K p(~γ L (s )−γ L ( s))

n=

K p E(s)n

Thay (**) vào (*) :

γ L (s)E(s)

=Ka K p

s (s Ra J +Ra f + Ka K b)=G(s)

Sau khi biến đổi đại số ta có hàm truyền:

γ L (s)~γ L (s )

=G(s)

1+G(s)=

Ka K p

s2 Ra J +s ( Ra f +K a K b )+Ka Kb

=

Ka K P

Ra J

s2+( Ra f + Ka K b )

Ra Js+

Ka Kb

Ra J

(***)

Phương trình (***) cho thấy rằng hệ điều khiển tỉ lệ của một khớp đọng là môt hệ bâc 2 nó sẽ luôn ổn định nếu các hệ số của phương trình bậc 2 là dương. Để nâng cao đặc tính động lực học và giảm sai số trạng thái ổn định của hệ thống, người ta có thể tằng hệ số phản hồi vị trí Kp và kết hợp giảm dao động trong hệ bằng cách thêm vào thành phần đạo hàm của sai số vị trí. Với việc thêm phản hồi này, điẹn áp đặt lên động cơ sẽ tỉ lệ tuyến tính với sai số vị trí và đạo hàm của nó.

U a ( t )=K p (~γ L (t )−γ L (t ) )+K v (~γ L (t )− γ L ( t ))

n=

K p e ( t )+ K v e(t )n

Trong đó: Kv –Hệ số phản hồi của sai số vận tốc.

25 | P a g e

Với phản hồi nêu trên, hệ thống trở thành khép kín và có hàm truyền như thể hiện trên sơ đồ khối hình dưới. Đây là phương pháp điều khiển tỉ lệ -đạo hàm,

Biến đổi Laplace phương trình (9.23) và thay Ua(s) vào (9.21) ta có:

γ L (s)E(s)

=K a

(K ¿¿ p+s K v )s (s Ra J +Ra f +K a K b)

=K a K v+K a K p

s (s Ra J+ Ra f +K a Kb)=G(s)¿

Từ đó ta có

γ L (s)~γ L (s )

=G(s)

1+G(s)=

K a(K v s+K p)s2 Ra J +s ( Ra f +K a K b+K a K v )+Ka K p

4.4. Thiết kế bộ điều khiển PID

Cho hệ thống điều khiển như hình vẽ

Hãy xác định thông số của bộ điều khiển PID sao cho hệ thống thỏa mãn yêu cầu:

- Hệ có cặp nghiệm phức với ᵹ=0.5, ωn=8

- Hệ số vận tốc K v=100

Lời giải:

Hàm truyền bộ điều khiển PID cần thiết kế:

GC (s )=K P+K I

s+ KD s

Hệ số vận tốc của hệ sau khi hiệu chỉnh:

26 | P a g e

KV =lims →0

sGC (s )G ( s)=lims →0

s(¿¿K P+K I

s+K D s)( 1

Ra Jn Ka

s2

+( Ra fn Ka

+Kb

n ) s

)¿¿

→ KV =K I

2

Theo yêu cầu đề bài KV =100 nên suy ra:

K I=200

Phương trình đặc tính của hệ sau khi hiệu chỉnh là:

1+GC (s )G ( s )=0

1+( K ¿¿P+K I

s+K D s)( 1

Ra J

n Ka

s2

+( Ra f

n Ka

+Kb

n )s )=0¿

↔ s ( Ra Jn Ka

s2

+( Ra fn Ka

+Kb

n )s)+( KD s2+K P s+K I )=0

↔Ra J

n Ka

s3+( Ra f

n Ka

+Kb

n )s2+K D s2+KP s+K I=0

↔Ra Jn Ka

s3+(( Ra f

nK a

+Kb

n )+K D)s2+¿(K P¿ s + K I = 0 (1)

Để hệ thống có cặp cực phức với ᵹ=0.5, ωn=8 thì phương trình đặc tính phải có dạng: (s+a)¿

↔ (s+a ) ( s2 8 s+64 )=0

↔¿(8 a+64¿ + 64 a = 0 (2)

Cân bằng các hệ số hai phương trình (1) và (2), suy ra:

{( Ra fn Ka

+K b

n )+KD=a+8

K P=8 a+64K I=64 a

VớiK I=200, giải hệ phương trình trên ta được:

{ a=3.125K P=89

KD=11.125−( Ra f

n Ka

+Kb

n )27 | P a g e

Vậy hàm truyền của khâu hiệu chỉnh PID cần thiết kế là:

GC (s )=89+ 200s

+11.125−( Ra f

n Ka

+K b

n )s

28 | P a g e