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

Tài liệu Robot

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 nhhì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ừ đ ờngvuô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ócchung 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

Vậy biến khớp là 1 và 2

b Thiết lập hệ toạ độ

Hệ toạ độ cố định o0x0y0z0 có gốc o0 đặt tại tâm trục khớp động 1, chiều x0 hớng

từ o0 đên o1

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ủatay 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ố DHcủ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:

0 1 0 0

0 0

0 0

1 1

1 1

cos sin

sin cos

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

0 1 0 0

0 0 1 0

0 0

0 1 0 0

0 0

0 0

1 1

1 1

cos sin

sin cos

0 1 0 0

0 0 1 0

0 0

0 0

0 1

0 0

0

0

1 1 1

1

1 1 1

1

sin a cos

sin

cos a sin

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:

0 1 0 0

0 0

0 0

2 2

2 2

cos sin

sin cos

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

0 1 0 0

0 0 1 0

0 0

0 1 0 0

0 0

0 0

2 2

2 2

cos sin

sin cos

0 1 0 0

0 0 1 0

0 0

0 0

0 1

0 0

0

0

2 2 2

2

2 2 2

2

sin a cos

sin

cos a sin

0 0

0 1

0 0

0

0

1 1 1

1

1 1 1

1

sin a cos

sin

cos a sin

0 0

0 1

0 0

0

0

2 2 2

2

2 2 2

2

sin a cos

sin

cos a sin

0 0

0 1

0 0

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sin a sin

a cos

sin

cos a cos

a sin

cos

(1.3)

Hay:

3

z z z z

y y y y

x x x x

p a o n

p a o n

p a o n

0 0

0 1

0 0

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sin a sin

a cos

sin

cos a cos

a sin

3 Xây dựng phơng trình động học ngợc cho robot

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

p a o n

p a o n

p a o n

0 1 0 0

0 0

0

1 1

1 1

1

cos sin

a sin

cos

(1.6)

0 1 0 0

0 0

0

1 1

1 1

1

cos sin

a sin

z z z z

y y y y

x x x x

p a o n

p a o n

p a o n

0 0

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

P a

o n

cos P sin P cos

a sin a cos

o sin o cos

n

sin

n

a sin P cos P sin a cos a sin

o cos o sin

0 0

0 1

0 0

0

0

2 2 2

2

2 2 2

2

sin a cos

sin

cos a sin

2 2

1 1 1

sin a cos

P sin

P

cos a a sin P cos

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:

1 2 2

1 1

sin a cos P sin P

a cos a sin P cos P

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 2

2 1 2 2

a a P P cos

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

a a P P sin P cos

2 2 1

2 2

2 1 2 2 1 2 2 1 2

y x

y x y

y x

x

P P a

a a P P sin P P

P cos

2 1

2 2 2 1 2 2 1

x

y x

y x

P P

P arcsin P

P a

a a P P arcsin (1.11)

5

2 2 2 2 2

2 2 2

2 1 1

2 2

a a

a a P P cos ar

P P

P arcsin P

P a

a a P P arcsin

y x

y x x y

x y x

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àmcủa biến 1

0 0

0 1

0 0

0

0

1 1 1

1

1 1 1

1

sin a cos

sin

cos a sin

0 0

0 1

0 0

0

0

2 2 2

2

2 2 2

2

sin a cos

sin

cos a sin

0 0

0 1

0 0

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sin a sin

a cos

sin

cos a cos

a sin

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

i i i i

, z , y , x

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

0 0

0 1

0 0

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sin a sin

a cos

sin

cos a cos

a sin

z y

r d A r ) A ( dt

d dt

r d

i i

i i

T

i 2

1

i 0 i

0 0

0

i i 0 0

0 0

0

r r r

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

j i 2

i

v (2.6)

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

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

i

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

r

J

7

Có thể viết lại ma trận Ji nh sau:

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 i

, r , r , r

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

z i y i

I

2 2

i

i thanh

y i x i ixy

dm r x

m

dm r r I

1

AA

2

1

k k

j k

T

i j

1

k k

j jk h Trace

i jk

A J

A Trace h

1

0 0

T i i j

i jk

A J

A Trace h

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:

i i 0

T

i

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

i i T

T i

i T

JA

A J

A Trace

n

i

T i i

k i

j

i

k

j

k

i i j

1 1 1

0 0

i

F L

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

.

T

F P

H H

9

1

gäi lµ thµnh phÇnm«men nhít vµ híng t©m

j

k n

k

j ijk

j n

j max(

p ij

A J

A Trace h

0 0

j j

k

m k j

k , i max p

p p k j p

i

A J

A Trace

,

r

A g m g

n

p T p i

p 1

12 11 h h

h h H

1

1 1

2 1 1

1

002

00

0

1003

1

ml

m

lml

2

2 2

2 2 1

2

002

00

0

1003

1

ml

m

lml

1 1 0 1 1 1 0 11

T T

AJ

ATrace

AJ

ATrace

h

1 12 2 22 2 22 2

3

4 3

2 2 0 2 1 2 0 21

12

T T

AJ

ATrace

AJ

ATraceh

2

13

1

lml



2 2 2 0

22

TAJ

ATrace

3

1 l m

v ,

V

.

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

A Trace

A J

A Trace

A J

A Trace

2 2 2 2 2 2

1

2

l sin m l

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

1 g

g V

2 1 0

0 0

1 0 0

2 1 0

0 0

m

-1 2

1

1 1

/ T g

m

/ A g

 1 2

2 2 1

1 2 1 1

2

1 2

2 1 0

0 0

2 2

/ T g

mlm

lml

mcos

lmlmlmM

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 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

.

.

11

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

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:

0 0

0 1

0 0

0

0

1 1 2 1 2 2

1 2

1

1 1 2 1 2 2

1 2

1

sin a sin

a cos

sin

cos a cos

a sin

1 1 2 1 2

1 1 2 1 2

sin a sin

a

cos a cos

1

2

1

2 j 2 0 2

i 2 0

i

T

j

tay j

tay i

r A r

A Trace

2 1 2 1 1

2 2 1 2 1

1         

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

2

v u

u = -a1 sin  1 1 - a2 sin(  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ácphơng x và y

y

v = v = [a1 cos  1 + a2 cos(  1 2)]1 +a2 cos(  1 2) 2

x

v = u = -[a1 sin  1 + a2 sin(  1 2)]1 - a2 sin(  1 2) 2

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







 sin a

) sin(

y

2 1

2 1







 sin a

) cos(

x v

 =

2 2 1

2 1 2 1 1

) sin(

a sin a

y

v

2 2 1

2 1 2 1 1

) cos(

a cos a

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ẳngkhi đó: 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

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 [00.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ờigian [.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,om,'b') % Vi tri cua khop 1

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

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

lml

mcos

lmlmlmM

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 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

.

.

d dt

1 1 d

d dt

1 1

d

d

dt

N U H

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

Phơng trình mạch vòng ngoài nh sau:

p

d e K e e K

.

0 p

p p

0 d

d d

K

K K

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

0

0

2 2 2 2

2

1 1 1 1

p d

e K e K

e

e K e

K

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 độnghọ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

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

.

) , (

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

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

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

Tơng tự ta có: Kd2 = -6, Kp2 = 9.

V Mô phỏng hệ thống với bộ điều khiển ở câu IV của khớp 1 thay

đổi nhảy cấp từ 0 đến 1 rad/s

1 Thiết kế bộ điều khiển tốc độ cho động cơ khớp 1

Ta có:

-Vì ta cần mô phỏng hệ thống với góc quay 1 thay đổi nhảy cấp, cho nên ta

có thể cho góc 2 bằng hằng số

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:

Wc = KP + KIs

19

- Hàm truyền đạt của đối tợng là:

WĐT = K/s(s+K*Kb)Trong đó: K=KM/(r*Jqd1) =0.5/(3*0.0219) = 7.6104

- 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 g1 cos 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:

21