Bài tập dài robot công nghiệp

Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệ giữa hệ toạ độ tay Robot End effector và hệ tọa độ các khớp JointsRobot hai thanh nối là robot với 2 khâu phẳng, vì vậy

I Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ tọa độ các khớp (Joints)

Robot hai thanh nối là robot với 2 khâu phẳng, vì vậy cơ chế và phạm vi hoạt

động của robot nằm trong mặt phẳng, bao gồm hai khớp quay và hai thanh nối nh hình vẽ:

1 Thiết kế hệ toạ độ cho các thanh nối

a Tham số của thanh nối và khớp

Xác định bộ thông số Denavit -Hartenberg (bảng thông số DH)

Từ hình vẽ robot ta xác định đợc các thông số sau:

+ Độ dài pháp tuyến chung của khớp 1 và khớp 2 là a1

+ Góc chéo giữa hai trục khớp 1 và khớp 2 là α1

+ Đây là khớp quay cho nên khoảng cách đo dọc trục khớp động 1 từ đờng vuông góc chung giữa trục khớp động 2 và trục khớp động 1 tới đờng vuông góc chung giữa khớp động 1 và gốc robot là d1 = 0 Tơng tự ta cũng dễ dàng có d2 = 0.+ Góc giữa hai đờng vuông góc chung là θ1

Tơng tự xét khớp 2 và tay robot

+ Ta có độ dài pháp tuyến chung là a2

+ Góc chéo giữa hai trục khớp 2 và tay robot là α1

+ Góc giữa hai đờng vuông góc chung là θ2

Hệ toạ độ o1x1y1z1 có gốc o1 đặt tại tâm trục khớp động 2

Hệ toạ độ o2x2y2z2 có gốc o2 đặt tại tâm trục khớp động cuối khâu 2

Ba trục z0, z1, z2 vuông góc với mặt phẳng tờ giấy

2 Xây dựng phơng trình động học thuận cho robot

Phơng trình động học thuận là phơng trình biểu diễn quan hệ vị trí và hớng của tay robot thông qua các biến khớp Các biến khớp này là góc quay của khớp quay và

độ dịch chuyển tịnh tiến đối với khớp tịnh tiến

Căn cứ vào phơng trình động học thuận này khi biết vị trí của các khớp ta có thể xác định đợc vị trí và hớng của tay robot

Căn cứ vào các thông số và hệ toạ độ đã đợc thiết lập ta có bảng thông số DH của robot nh sau:

* Quan hệ giữa hai khung toạ độ o1x1y1z1 và o0x0y0z0 đợc xác định nh sau:

- Quay xung quanh trục z0 một góc θ1 sao cho trục x0 trùng với phơng của trục x1

- Tịnh tiến dọc theo trục x1 (phơng pháp tuyến chung) một đoạn bằng a1

Vậy phép biến đổi tổng hợp nh sau:

θ

−θ

1000

0100

00

00

1 1

1 1

cossin

sincos

Trans(a1,0,0) là phép tịnh tiến khung toạ độ o0x0y0z0 theo trục x0 một đoạn bằng a1

0100

0010

00

Vậy 0A1 đợc tính nh sau:

−θ

1000

0100

00

00

1 1

1 1

cossin

sincos

0100

0010

00

θ

θθ

−θ

10

00

01

00

0

0

1 1 1

1

1 1 1

1

sinacos

sin

cosasin

cos

(1.1)

* Quan hệ giữa hai khung toạ độ o2x2y2z2 và o1x1y1z1 đợc xác định nh sau:

- Quay xung quanh trục z1 một góc θ2 sao cho trục x1 trùng với phơng của trục x2

- Tịnh tiến dọc theo trục x2 (phơng pháp tuyến chung) một đoạn bằng a2

Vậy phép biến đổi tổng hợp nh sau:

θ

−θ

1000

0100

00

00

2 2

2 2

cossin

sincos

Trans(a2,0,0) là phép tịnh tiến khung toạ độ o1x1y1z1 theo trục x1 một đoạn bằng

0100

0010

00

θ

−θ

1000

0100

00

00

2 2

2 2

cossin

sincos

0100

0010

00

θ

θθ

−θ

10

00

01

00

0

0

2 2 2

2

2 2 2

2

sinacos

sin

cosasin

cos

(1.2)Phơng trình động học thuận đợc xác định nh sau:

θθ

−θ

10

00

01

00

0

0

1 1 1

1

1 1 1

1

sinacos

sin

cosasin

θ

θθ

−θ

10

00

01

00

0

0

2 2 2

2

2 2 2

2

sinacos

sin

cosasin

θ+θθ

+θθ

+θ

θ+

θ+θθ

+θ

−θ+θ

10

00

01

00

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sinasin

acos

sin

cosacos

asin

z z z z

y y y y

x x x x

paon

paon

paon

+θθ

+θ

θ+θ+θθ

+θ

−θ+θ

10

00

01

00

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sinasin

acos

sin

cosacos

asin

Phơng trình động học ngợc robot nhằm xác định các giá trị biến khớp từ các

vị trí và hớng của tay robot mong muốn Phơng trình động học ngợc thờng khó giải

và không có lời giải tổng quát cho mọi robot

Ta thấy rằng ma trận 0A2 đã biết, tức là vị trí và hớng của khung toạ độ tay robot, cần xác định giá trị các biến khớp θ1 và θ2:

z z z z

y y y y

x x x x

paon

paon

paon

−

−θ

θ

1000

0100

00

0

1 1

1 1

1

cossin

asin

−

−θ

θ

1000

0100

00

0

1 1

1 1

1

cossin

asin

z z z z

y y y y

x x x x

paon

paon

paon

θ

−θ+

θ

−θ+

θ

−θ+

θ

−

−θ+

θθ

+θθ

+θθ

+

θ

10

00

1 1

1 1

1 1

1 1

1 1 1

1 1

1 1

1 1

z z

z z

y x

y x

y x

y x

y x

y x

y x

y x

Pa

on

cosPsinPcos

asinacos

osinocos

n

sin

n

asinPcosPsinacosasin

ocososin

θ

θθ

−

θ

10

0

0

01

2

2 2 2

2

sinacos

sin

cosasin

θ

2 2 1 1

2 2 1 1 1

sinacosPsin

P

cosaasinPcos

P

y x

y x

(1.8)Với các thông số Px, Py và a1 , a2 đã biết giải hệ phơng trình này ta đợc các biến khớp θ1 và θ2 nh sau:

−

+θ

=θ+

θ

2 2 1 1

1 2 2 1 1

sinacosPsinP

acosasinPcosP

y x

y x

(1.9)Bình phơng ai vế 1.9, cộng vế theo vế ta đợc:

2 2 2 2 1

2 1 2

2 1

2 2

2 1 2 2

2

2 aa

aaPPcos

ar x y (1.10)

Bình phơng hai vế và cộng vế theo vế của 1.8 ta đợc:

2 1 1

1 2 2

2 1 2 2

1 1

2a

aaPPsinPcos

=θ+

θ

2 2 1

2 2

2 1 2 2

1 2 2 1 2

y x

y x

y

y x

x

PPa

aaPPsinPP

Pcos

=θ+

+θ+

=

θ

2 2 2

2 1

2 2

2 1 2 2

1

x

y x

y x

PP

ParcsinP

Pa

aaPP

=θ

2 1

2 2

2 1 2 2

2

2 2 2

2 1

2 2

2 1 2 2

1

2

2

aa

aaPPcosar

PP

ParcsinP

Pa

aaPParcsin

y x

y x

x

y x

y x

(1.12)

II Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay robot

Để xây dựng quan hệ giữa tốc độ của khớp và tốc độ của tay robot ta cần phải thiết lập phơng trình động lực học cho robot

Gọi v là tốc độ chuyển động tịnh tiến của tâm vật thể, ω là tốc độ góc, m là khối lợng vật thể, Jt là mômen quán tính của vật thể

Sử dụng phơng pháp Lagrange để thiết lập phơng trình động lực học cho robot Giả thiết khớp 1 sinh ra mômen M1 tác dụng giữa bệ và thanh nối 1, khớp 2 sinh ra mômen

M2 tác dụng giữa thanh nối 1 và 2 Trọng lực có hớng theo trục y

1 Tính tốc độ của một điểm bất kỳ trên robot

Nh trong phần I ta có phép biến đổi toạ độ từ thanh nối 2 đến hệ toạ độ gốc

là 0A2

0A2 = 0A1 1A2 (2.1)

Trong đó: A1 là phép biến đổi toạ độ từ thanh nối 1 đến hệ toạ độ gốc, là hàm của biến θ1

θ

θθ

−θ

10

00

01

00

0

0

1 1 1

1

1 1 1

1

sinacos

sin

cosasin

θ

θθ

−θ

10

00

01

00

0

0

2 2 2

2

2 2 2

2

sinacos

sin

cosasin

θ+θθ

+θθ

+θ

θ+

θ+θθ

+θ

−θ+θ

10

00

01

00

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sinasin

acos

sin

cosacos

asin

cos

Giả thiết biểu diễn một điểm bất kỳ nào đó trên thanh nối i nh sau

[ ]T

i i i

ir = x ,y ,z ,1 (2.2)Vậy vị trí của điểm đó trong hệ toạ độ gốc o0x0y0z0 là:

rA

i 0

+θθ

+θ

θ+

θ+θθ

+θ

−θ+θ

10

00

01

00

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sinasin

acos

sin

cosacos

asin

zy

rdAr)A(dt

ddt

rd

i i

i i

T

i 2

1

i i

i 0 i

0 0

0

r

∂

=+

0 i

i i

0 0

0 0

0

r rr

Sau khi biến đổi ta đợc:

j i 2

∂

=∑∑

T T

i

2 Động năng của robot đợc tính nh sau

Động năng của một điểm bất kỳ trong thanh nối i

dmr

rTrace

dmvdK

0 2

i 2

A

A2

12

∂

= = i

T T

j

dmrrTrace

dK

k j 2

1

2

1

i 0

i thanh

i i i 0 2

A

A2

1

Ký hiệu Ji = ∫ r rTdm

i thanh

i

i

i

i

i

2

2

i

i

i

i

i

2

2

i

i

i

i 2

2

i

thanh thanh

z thanh

y i

thanh

x

z thanh

i z

thanh

i z

y thanh

i z

x thanh

i

y thanh

i y

z thanh

i i

y i thanh

i y

x thanh

x x

z thanh

i x

y i thanh

i i

x i thanh

i

i

dmdm

rdm

rdm

r

dmrdm

rdm

rrdm

rr

dmrdm

rrdm

rdm

rr

dmrdm

rrdm

rrdm

+

−

++

−

=

i i

i i

i i

i

i i izz iyy ixx iyz

ixz

i i iyz

izz iyy ixx ixy

i i ixz

ixy izz

iyy ixx

i

mz

my

mx

m

zmIIII

I

ymI

IIII

xmI

II

II

J

22

2

z

i y

i x i

ir= r , r , r ,1 là bán kính vectơ biểu diễn trọng tâm của khâu i trong hệ toạ độ i

Các phần tử của Ji đợc tính nh sau:

( )

=

i thanh

z

i y

i ixx r r dm

I

2 2

i i

i

i thanh

y

i x

i ixy

dmrx

m

dmrrI

∂θ

1

AA

2

1

k k

j k

T

i j

j jk

hTrace

∂

= n

T i i j

i jk

AJ

ATraceh

1

0 0

0

=θ

∂

j i

∂

= n

k , max

T i i j

i jk

AJ

ATraceh

0 0

Do hjk = hkj nên ma trận vuông H(θ) là ma trận đối xứng, đồng thời động năng là một đại lợng dơng với θ• ≠0, nên các thành phần của H(θ) là các số dơng

3 Thế năng của robot đợc tính nh sau

Thế năng của thanh nối thứ i đợc xác định theo biểu thức:

r

i i 0 T i

i -m g A

P =

Với g = [ gx gy gz 1]T làvéctơ gia tốc trọng trờng

mi là khối lợng thanh nối thứ i

T

ig A rm

Ta thấy miir là cột thứ t của ma trận Ji nên ta có tổng thế năng nh sau:

i

T i

i

T A Jg

1

Thay lần lợt các biểu thức vào và biến đổi ta có:

rAgm

AJ

ATrace

i

T i i

k i

j

i

k

j

k

i i j

∂

=

1

0 2

1 1 1

0 0

2

1

(2.9)Phơng trình Lagrange có dạng nh sau:

Mi i

.

i

FLL

∂

∂

=θ

H

21

Do đó phơng trình động lực học có dạng:

.

T

F

PH

∂

−θθ+

θ

21

∂

∂

−θθ

θ, H H Hq K

T

.

2

1

gọi là thành phần mômen nhớt và hớng tâm

( ) ( )θ = ∂Pθθ

k n

k

j ijk

j n

∂

= ∑

T p p j

p

) i max(

p ij

AJ

ATraceh

0 0

j

j

k

m

k j

k , i max p

p p k j

p

i

AJ

ATrace,

r

Ag

m

p T p

i

p 1

22 21

12 11

hh

hhH

1

1 1

00

0

1003

1

ml

m

lml

2

2 2

2 2 1

2

002

00

0

1003

1

ml

m

lml

mJ

∂+

∂

=

1 2 0 2 1 2 0

1 1 0 1 1 1 0 11

T

J

ATrace

AJ

ATrace

∂

=

=

1 2 0 2 2 2 0

2 2 0 2 1 2 0 21

12

T T

AJ

ATrace

AJ

ATrace

h

2 2

2 1 2

2

13

1

lml

∂

=

2 2 0 2 2 2 0 22

T

AJ

ATrace

2 2

3

1lm

=

2 2 1 2 0 2 2 2 2 0 2

1 1 2 0 2 1 2 2 0 1

1 1 1 0 1 1 1 1 0

1

.

.

J

ATrace

AJ

ATrace

AJ

ATrace

∂+

θθ

∂+

θθ

∂

=

2 1

2 1 2 2

2 2

2 2 2 2

1

2

lsinml

sin

m

2 1 2 2 2 2 2

2 2 2 0 2 2 2 2 0 2

1 2 2 0 2 1 2 2 0

2

2

.

.

l sin m

A J

A Trace

A J

A Trace

∂ θ

∂

∂ +

θ θ

∂ θ

2

1

g

gV

210

00

100

210

00

m

-1

2 1

1 1

/Tg

m

/Ag

( 1 2)

2 2 1

1 2 1 1

2

12

210

00

2 2

/Tg

Vậy phơng trình động lực học của robot là:

( )θ+

ml

m

lml

mcos

lmlml

mM

M

2

1 2

2 2

2 2 2

2 1 2

2 2 2

2 1 2 2

2 2 2

2 2 2

2 1 1

2

1

3

12

13

13

13

43

θ+θ+

θθθ

−θθ

−

+

2 1 2 2

2 1 2 2 1

1 2 1 1 2

1

2 2 2 2

2 1 2 1 2 2

2 2 2 2 2 2

2

12

cosglmcos

glmglml

sinm

lsinml

sin

m

.

.

Với các mômen khớp cho trớc ta giải hệ phơng trình vi phân bậc hai này thì thu

đợc các biến khớp là góc θ1và θ2, vận tốc góc θ.1,θ.2 , gia tốc góc θ 1, θ 2

5 Quan hệ giữa tốc độ của khớp và tốc độ của tay robot

Biểu diễn tay robot trên thanh nối 2 nh sau:

θ+θθ

+θθ

+θ

θ+

θ+θθ

+θ

−θ+θ

10

00

01

00

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sinasin

acos

sin

cosacos

asin

θ+θ

θ+

θ+θ

=

10

1 1 2 1 2

1 1 2 1 2

sinasin

a

cosacos

T tay

tay 0 0

1

2

1

2 j 2

0 2

i 2 0

i

T

j

tay j

tay i

r

Ar

2 2 1 2 1

= a sin a sin a cos a cos

Khai triển vận tốc của tay robot theo hai thành phần trục x và trục y và ta có thể viết rằng:

2 2

u = -a1sinθ1θ1 - a2sin(θ +1 θ2)(θ1+θ2)

Nh vậy u và v chính là các véc tơ vận tốc của tay rô bốt chiếu theo các phơng x và y

Từ hệ phơng trình trên ta có quan hệ giữa tốc độ các khớp và tốc độ của tay rô bốt

nh sau:

1

θ =

2 1

2 1

θ

θ+θ

sina

)sin(

y

2 1

2 1

θ

θ+θ

sina

)cos(

2 1 2 1 1

θ

θ+θ+

θ

−

sinaa

)sin(

asina

y

v

2 2 1

2 1 2 1 1

θ

θ+θ+

θ

−

sinaa

)cos(

acosa

x

v

III Viết hàm MATLAB thực hiện các phơng trình ở phần 1 và 2, vẽ đờng biểu diễn vị trí và tốc độ của khớp khi tay robot di chuyển từ vị trí [0.4; 0.0 m] đến [0; 0.4m] theo một quỹ đạo đờng thẳng Đồ thị tốc độ đặt trớc của tay robot dọc theo quỹ đạo cho trớc

Khi robốt di chuyển từ vị trí (0.4; 0.0m) đến (0.0; 0.4m) theo một đờng thẳng khi đó: vận tốc của tay robot, quỹ đạo chuyển động, px, py, pz là các đại lợng đã biết; thời gian chuyển động t = 1s Cần tính tốc độ của các khớp θ1 và θ2 theo t

1 Chơng trình tính toán động học ngợc

-Hàm tính góc θ1:

function y = teta_1(px,py)

l1 = 0.4; % chieu dai cua thanh noi 1 (m)

l2 = 0.3; % chieu dai cua thanh noi 2 (m)

% goc cua khop mot (rad)

y = asin((l1^2+px^2+py^2-l2^2)/(2*l1))- asin(px/sqrt(px^2 + py^2));

function y=teta_2(px,py)

l1=.4; % chieu dai cua thanh noi 1 (m)

l2=.3; % chieu dai cua thanh noi 2 (m)

% goc cua khop hai (rad)

% vy van toc cua tay robot theo phuong y

% vx van toc cua tay robot theo phuong x

% t1 goc quay cua khop 1

% t2 goc quay cua khop 2

3 Xây dựng mối quan hệ giữa vx,vy,px và py theo thời gian t

Gọi v1 là vận tốc của tay rô bốt trên đoạn [0 0.25], v2 là vận tốc của tay rô bốt trên đoạn [0.25 0.75], v3 là vận tốc của tay rô bốt trên đoạn [0.75 1]

Ta có:

v1=4vot

v2=vo

v3=-4vo(t-1) với v0 là vận tốc ban đầu

Gọi s1 là quãng đờng mà tay rô bốt dich chuyển đợc trong khoảng thời gian [0 0.25], s2 là quãng đờng mà tay rô bốt dich chuyển đợc trong khoảng thời gian [0.25 0.75], i s3 là quãng đờng mà tay rô bốt dich chuyển đợc trong khoảng thời gian [.75 1]

Khi tay rô bốt di chuyển từ điểm (0.4 0) đến (0 0.4) quãng đợc đi đợc là: 0.4

2 Suy ra: vo=1.6 2/3 Thay vào các công thức trên ta đợc:

v1=6.4 2t/3

v2=1.6 2/3

v3=-6.4 2 (t-1)/3 Chiếu véc tơ vận tốc lên hai trục x và y ta đợc:

% Vi tri va toc do cac khop trong khoang thoi gian tu 0 den 0.25

t=0:.01:.24;

vx=-6.4*t/3; % toc do cua tay ro bot theo truc x

vy=6.4*t/3; % toc do cua tay ro bot theo truc y

px=0.4-3.2*t.^2/3; % vi tri cua tay ro bot theo truc x

py=3.2*t.^2/3; % vi tri cua tay ro bot theo truc y

t1=tone(px,py); % vi tri cua khop 1

t2=ttwo(px,py); % vi tri cua khop 2

dh1=dhone(vy,vx,t1,t2); % toc do goc cua khop 1

dh2=dhtwo(vy,vx,t1,t2); % toc do goc cua khop 2

plot(t,dh,'k') % Toc do cua khop 2

- Ch¹y ch¬ng tr×nh ta cã kÕt qu¶ nh sau:

§å thÞ vÞ trÝ gãc quay cña khíp 1:

§å thÞ vËn tèc gãc quay cña khíp 1

m

lml

mcos

lmlml

mM

M

2

1 2

2 2

2 2 2

2 1 2

2 2 2

2 1 2 2

2 2 2

2 2 2

2 1 1

2

1

3

12

13

13

13

43

θ+θ+

θθθ

−θθ

−

+

2 1 2 2

2 1 2 2 1

1 2 1 1 2

1

2 2 2 2

2 1 2 1 2 2

2 2 2 2 2 2

2

12

cosglmcos

glmglml

sinm

lsinml

sin

m

.

.

θ

−θ

=θ

−θ

=

2 2

1 1

d

d dt

−θ

θ

−θ

=θ

−θ

=

2 2

1 1 d

d

dt

.

−θ

θ

−θ

=θ

−θ

=

2 2

1 1

d

= − 1

Sơ đồ cấu trúc của hệ thống điều khiển nh sau:

Tiếp theo ta đi đến thiết kế bộ điều khiển U

Bộ điều khiển U có thể điều khiển PD, PID, bộ điều khiển phản hồi trạng thái.Thiết kế bộ điều khiển PD

.

e

e

.) , (

Bộ điều khiển vòng trong

(khử phi tuyến) Bộ điều khiển vòng ngoài (đảm bảo độ ổn định, chất lượng quá

trình quá độ, chế độ tĩnh)

Phơng trình cho 2 khớp nh sau:

0

0

2 2 2 2

2

1 1 1 1

1

=+

+

=+

+

p d

p d

eKeK

e

eKeK

e

Tính toán các hệ số Kd1, Kd2, Kp1, Kp2 bằng phơng pháp dựa vào các khâu động học điển hình:

+ Thiết kế bộ điều khiển cho khớp 1:

Theo phơng pháp đặt nghiệm ta mong muốn:

( 3)( 3) 0

1 1

Chọn nh sau: θ2=60o , suy ra θ2 và θ2đều bằng không

Thay các giá trị vào ta đợc:

H11 = 3.0905 Kgm2

H22 = 0.3025 Kgm2

- Hệ số cản của động cơ và phụ tải coi bằng không

- Mô men quan tính tổng hiệu dụng của hai động cơ là:

+ Động cơ khớp 1:

Jqd1 =Jđ + i2 H11= 0004 + 3.0905/122 =0.0219 Kgm2

+ Động cơ khớp 2:

Jqd2 =Jđ + i2 H22= 0004 + 0.3025/122 =0.0025 Kgm2

Trong đó i là tỉ số truyền: i = tốc độ của động cơ / tốc độ của khớp = 12

- Ta có sơ đồ hệ thống điều khiển tốc độ quay nh sau:

- Với các thông số:

Kb = Ka = 0.5

Ra = 3

- Sử dụng bộ điều khiển PI: Các thông số đợc xác định theo phơng pháp gán cực

- Hàm truyền đạt của bộ điều khiển có dạng:

- Cho tín hiệu đặt góc quay của khớp 1 là tín hiệu step.

- Cho tín hiệu đặt tốc độ góc quay của khớp 1 là bằng không

- g1 đợc xác định nh sau:

g1 = m1gl g1cosθ1 + m2g[l1cosθ1+l g2cos(θ1+θ2)]

=10*9.7*0.2*cos(θ1)+5*9.7*(.4*cos(θ1)+.15*cos(θ1+pi/3))

- KP và KD đợc xác định nh ở trên

- Sau khi chạy chơng trình ta đợc đồ thị góc quay của khớp nh sau: