Bài tập lớn môn kỹ thuật robot

TINH TOAN DONG HOC THUAN VA NGHICH DONG HOC THUAN ROBOT Phương trình động học robot công nghiệp là phương trình mô tả quy luật thay đôi hướng và vị trí của khâu tác động cuối so với các

TRUONG DAI HOC THU DAU MOT VIEN KY THUAT - CÔNG NGHỆ

Win nivwor

2009 ~=9THU DAU MOT UNIVERSITY

Bài Tập lớn môn Kỹ Thuật Robot

TINH TOAN DONG HOC THUAN VA NGHICH

DONG HOC THUAN ROBOT

Phương trình động học robot công nghiệp là phương trình mô tả quy luật thay

đôi hướng và vị trí của khâu tác động cuối so với các biến khớp

Trên tay máy robot công nghiệp có 2 dạng khớp la: Khớp tịnh tiền và khớp

quay Biến khớp chính là các góc quay và các lượng tịnh tiền của các khớp quay và

* Z,cung hudng voi hướng của trục khớp 1+1

* X,cùng phương với phương pháp tuyến chung của trục Z ,,và trục Z,

+ Gốc tọa độ trên khâu là được xác định bởi giao điểm của trục z và trục x

đã được xác định của khâu đó Hướng của trục y được chọn theo hướng

của trục z„ x theo quy tắc bản tay phải

° _ Hệ tọa độ gốc: X› được chọn tùy ý vuông góc với Z o

\ = he <% 1 /

Quy tắc rời truc toa dé theo Denavit — Hartenberg

- Tinh tién mét doan di theo truc Z j-1 dé X ;-I nằm trên mặt phang phap tuyén cua Z j-] chira Xi

- _ Quay một góc 0i quanh trục Z.¡- để X :¡ cùng phương với Xi

- Quay l góc ơi quanh trục X¡-I (= Xi) để Z.¡-| = Zi

Hình 0.1: Mô hình tương xứng với cảnh tay robot 4 bậc tự do và đặt trục cho robot

Từ Hình 0.1 ta thanh lap được bảng Denavit-Hartenbcrg:

„đi là khoảng cách giữa góc toạ độ ¡-l tới giao điểm của trục ⁄-Ivà Xi đọc theo trục

441

9 ` r ` Xi] ,.X Zj-|

- ! là góc quay từ trục tới“ /xung quanh trục “?

h ; Ly ; Ly là chiều đài của các Link trong Robot

=Œ =0.4:7› =a) =U 3: Ly =a =0.5

Công thức tổng quát ma trận biến đối đồng nhất từ hệ trục {¡} so voi {i+1} la:

cos@, —sinØ,cosơ, sin0,sinơ, a,cos@, A= sinØ, cosØ,cosơ, —cosổ,sinơ, œ,sinổ;

"1 0 sing, cosa, d,

Su dung Matlab dé tinh toán ta tính được:

cosỹ, —Sin9, 0 a,cosé,

A= Sin@, = cos0, 0 a,Sin@,

cos6, sin@, 0 a,cosé,

A- sing, = —cos; 0 a;sinÐ;

100 0 _J0 100

A=lg 016,

000 1 cosØ, —sin@, 0 0

A- sinO, = —cos@, 0 0

0 0 0 1

B=-d

Với các mục viết tắt:

=cos: s =sin

=cos(4 +é,):s,, =sin(@ + 4, ) Code Matlab:

DONG HOC NGHICH ROBOT

“+ Phwong phap hinh hoc:

ft)

Hình 0.3: Vẽ hình các hệ trục tạo độ Robot

Ta có điểm cuối robot

p,=1,c080,+1, cos( 0661+6,)é p,=hsin 6,+1,sin(0461+0,)i p,=—q;

=0;=arctg2( Sin0;, Cos0,)

Tim ql hay thetal:

=0;=arctg2( Sin0,, Cos0,)

Với hướng điểm cuối ra có Roll, Pitch , va Yaw Giải sử ta mong muốn góc quay theta4 có định Roll =Nx=0

N =cos (yg +d,- G4 )

sin(g, +g.- gs) = Yl +A

§,=atan2(S,,C,)

§,=atan 2(S,,C,) q;=—[P,) 6,=6,+0,—artan(V1°+Nx*/Nx)

KIỂM CHỨNG ĐỘNG HỌC THUẬN VÀ NGHỊCH

Kiểm chứng bằng cách ra viết hàm cho động học thuận, cho giá trị các góc rồi tính được

vị trí, rồi viết hàm tính động học nghịch, lay vi tri tính được ở hàm động học thuận rồi thế vào

động học nghịch nếu ra chính xác các góc mà ta đã tính thì tính toán đã chính xác Nếu sai thì phân tính toán lý thuyết đã sai, và ta phải xem lai phan tính toán ở lý thuyết

Ta bảng thông số

Giới hạn quay khâu 1 Từ -140 đến 140 Giới hạn quay khâu 2 Tir -115 dén 115 Giới hạn quay khâu 4 Tir -180 dén 180

Al (0.3 ;-0.1; 0.1)

0.35) +(0.3} - (0.47 +0.3°)

cos g; =——— = 0.3046

20.4 x03 sing, =- VP - cosy,” =-0.9312

Code Matlab:

Xây dựng chương trỉnh cho động học Nghịch và thuận

c2 = (px*2 + py*2 - al*2 - a2*2)/(2*al*a2);

t4 inv = tÍ inv † t2 inv - atan2(sgrt(i-nzx^2),nx); stheta4

&) |- Gis +hs,) -Cys.) 0) | & &

> Va Bx! = (Lx > +Lo) (7 " () f& =J| &&

Ngày 28/10: Câu 3 và cầu 4

TÍNH TOÁN ĐỘNG HỌC THUẬN VÀ NGHỊCH

ĐỘNG HỌC THUẬN ROBOT

Phương trình động học robot công nghiệp là phương trình mô tả quy luật thay

đôi hướng và vị trí của khâu tác động cuối so với các biến khớp

Trên tay máy robot công nghiệp có 2 dạng khớp la: Khớp tịnh tiền và khớp

quay Biến khớp chính là các góc quay và các lượng tịnh tiền của các khớp quay và

khớp tịnh tiến

Khi cho biết trước giá trị của các khớp quay và các lượng tịnh tiễn nảy, thay vào

phương trình động học robot công nghiệp ta sẽ có hướng và vị trí các khâu công tác

= Phép biến đỗi thuần nhất các hệ toa độ theo quy tac D-H

Quy tắc đặt các trục tọa độ

* Z,cung hudng voi hướng của trục khớp 1+1

» X, cùng phương với phương pháp tuyến chung của trục Z ,;¡và trục Z,

+ Gốc tọa độ trên khâu là được xác định bởi giao điểm của trục z và trục x

đã được xác định của khâu đó Hướng của trục y được chọn theo hướng

của trục z„ x theo quy tắc bản tay phải

° _ Hệ tọa độ gốc: X› được chọn tùy ý vuông góc với Z o

GIUNTO i-1 GIUNTO i GIUNTO i+

cua Z j-] chira Xi

- _ Quay một góc 0i quanh trục Z.¡- để X :¡ cùng phương với Xi

- - Tịnh tiễn 1 đoạn ¡ a theo trục X¡- dé X j-1 = Xi

- _ Quay l góc ơi quanh trục X¡-I (= Xi) để Z.¡-| = Zi

Hình 0.4: Mô hình tương xứng với cảnh tay robot 4 bậc tự do và đặt trục cho robot

Từ Hình 0.1 ta thanh lap được bảng Denavit-Hartenbcrg:

- “a khoảng cách giữa giao điểm trục ⁄-lvà ¡ với góc toạ độ hệ trục thứ I dọc

V,

theo trục ˆ

X,

- ti góc quay của trục Zi-1 465 Zi xung quanh trục *

- đủ là khoảng cách giữa góc toạ độ ¡-l tới giao điểm của trục Zi-tya Xi doc theo truc

⁄¡ l

LAN ` Xin gee X Zj-|

- —! là góc quay từ trục “ ”Ì tới“ xung quanh trục “?

h ; Ù ; Ly là chiều đài của các Link trong Robot

Ly =a, =0.4:L) =a, =0.3: Ly =a =U.Š

Công thức tổng quát ma trận biến đối đồng nhất từ hệ trục {¡} so với {i+1} là:

cos@, —sinØ,cosơ, sin@,sina, a,cosé, A= sinØ, cosổ,cosơ, —cos@,sina, q,sin0,

"7 0 sina, cosa, d,

Su dung Matlab dé tinh toán ta tính được:

cos6, —Sin@, 0 a,cosé,

A- Sin@, cos@, OQ a,Sing,

A=lg g1 9,

000 1

cos@, —sin@, 0 0 A.—|Sin@, —cos@, 0 0

“1 0 0 10

0 0 0 1

ma trận biến đôi đồng nhất {4} so với {0}là

=0;=arctg 2( Sin0;, Cos0;)

Tim ql hay thetal:

=0¡=arctg 2( Sin0,, Cos0))

Với hướng điểm cuối ra có Roll, Pitch , va Yaw Giải sử ta mong muốn góc quay theta4 có định Roll =Nx=0

N, =cos(qg, +95 - gy) Í f 14

sin(g, +g.- g,) = Jl +N

§,=atan2(S,,C,) 6,=atan 2(S,,C,) q3=—(P.) 6,=0,+0,—artan(V1°+Nx*/Nx)

Hoặc nếu góc luôn hướng theo trục x ta có

0, =-( +0.) KIỂM CHỨNG ĐỘNG HỌC THUẬN VÀ NGHỊCH

Kiểm chứng bằng cách ra viết hàm cho động học thuận, cho giá trị các góc rồi tính được

vị trí, rồi viết hàm tính động học nghịch, lay vi tri tính được ở hàm động học thuận rồi thế vào

động học nghịch nếu ra chính xác các góc mà ta đã tính thì tinh toán đã chính xác Nếu sai thì phân tính toán lý thuyết đã sai, và ta phải xem lai phan tính toán ở lý thuyết

Thay điểm B: (0.35;0.2; 0.4)phương trình trên ta được

| -92.20°

‘Dong hoc Nghicht Bong hoe thuant

Hình 0.6: Matlab Simulink mô phỏng kiểm chứng động học cho Robot Code Matlab:

Xây dựng chương trỉnh cho động học Nghịch và thuận

c2 = (px*2 + py*2 - al*2 - a2*2)/(2*al*a2);

t4 inv = tl_inv + t2_inv - atan2(sqrt(l-nx*2),nx); stheta4

OY) (Lys, + Lys, Mdg, /dt)- (Ls, dg, dt)

v=]! =) (1.6, + he, Meg, dt) + (Loe, dg, / dt)

Bx) | - CGys.t+hs,) -Css,) 01) & &

gq (=a tar tattt,

KO) =3a,e° + 2a,t +ứ,

@&Á()=tut + 24,

Chiều dài khâu 1 (L1) 0.4m Chiều dài khâu 2 (L2) 0.3m Chiêu dài tôi đa khâu 3 (L3) 0.5m Khối lượng khâu 1 (m1) 10kg Khỗi lượng khâu 2 (m2) 5ke Khối lượng khâu 3 (m3) 7kg Khối lượng khâu 4 (m4) Ike Moment quanh tinh khéi tam khau 1 (J1) 0.728kgm2 Moment quanh tính khối tâm khâu 2 (J2) 0.19køm2 Moment quánh tính khối tâm khâu 4 (14) 0.03kem2 Khoảng cách thừ khớp 1 đến tâm khâu 1 (Ig1) 0.2m

Khoảng cách thừ khớp 1 đến tâm khâu 2 (Is2) 0.15m

Khoảng cách thừ khớp 1 đến tâm khâu 4 (Is4) 0m

Giới hạn quay khâu | -140 -> 140

Giới hạn quay khâu 2 -115->115

Giới hạn quay khâu 4 -180 ->180

Dựa vào phân tính toán ở trên ta có điểm

AI(0.3 ;-0.1; 0.1)bao gom các góc q1=29.34; q2=-128.7; q4=99.34

A2(0.35 ;0.2; 0.4)bao gồm các góc q1=73.61; q2=-111.4; q4=37.37

Ta có: Khoảng cách giữa 2 điểm AI và A2 là:

=Cx - 6} + {ý} +, - z} =0.4272

Thời gian đi từ AI đến A2 với vận toc v la 200mm/s = 0.2m/s la : ) 0.2

Dựa vào phân tích:

A(t) =3a,t + atta > | &(r) =3 x(-3.8328)r +2 «(10.3738)1

| Blr) =0a,1 + 2a &47) =o x(-3.8328)r + 2 x(Ie.3738)

Thoi gian di tir Al dén A2 voi van toc v la 200mm/s = 0.2m/s la : ' 0.2

Dựa vào phân tích:

<a, XQ.8480) +2 xa, x(2.8480)+0 =0 | |a2 =0.3980

Vay quy luật của khớp | la:

gq (=ar tar +attt, ạ (r) =(-1.497§)¿` + (6.3980)

A(t) =3a,t + atta > | &(r) =3 x(-1.4978 +2 x(o.3980)/

&Li) =0u.t + 2ú ¡ &) =6 x(-1.497§)/ + 3 x(6.3986)

Sy -(r)=o x(-1.497§)0 + 3 x(a.39§6) =I2.7973

Ta thay rang vận tốc biến thiên theo quy luật bậc 2 với giả trị cực đại tại thời gian t/2

Gia toc biên thiên theo quy luật bậc 3 với gí trị cực đại tại tCŨ và tcCthời gian đến điểm A2

Như vậy ta thấy Quy luật chuyên động của khâu số 3 và 4 tương tự điểm cuối của khâu số 1

chỉ khác nhau tọa độ Z

Viết chương trình matlab ta thu được các đồ thị

Đồ thị vị trí góc tính theo độ:

A1(0.3 ;-0.1; 0.1)bao gồm các góc q1=29.34; q2=-128.7; q4=99.34

A2(0.35 ;0.2; 0.4)bao gồm các góc ql=73.61; q2=-I11.4; q4=37.37

ĐỎ THỊ CÁC GOC DOI CHIEU QUAN HE VAN TOC

Do thi van toc

Do thi van toc Q3

Q3 0.14

y(m/s”) oO

-0.05 + -0.1 7

———94

0 0.5 1 1.5 2 2.5 3 Đôi chiều với các

truc x(s)

tính toán bên trên ta nhận thấy rằng việc mô phỏng và vẽ đồ thị đã chính xác Nhật là đôi với gia tốc đôi đa và vận tộc đôi đa

KHONG GIAN LAM VIEC

Một trong những thông số động học quan trọng của tay máy là vùng làm việc (Workspace) của nó Đó là không gian mà gốc toạ độ của phần công tác có thê với tới được, tức là không tính đến sự định hướng của phân công tác Đôi khi người ta phân biệt vùng làm việc nói trên (gọi là Reachable Workspace) với vùng lảm việc có tính đến sự định hướng của phần công tác (Dexterous Workspace)

Chiều dài khâu | (L1) 0.4m Chiều dài khâu 2 (L2) 0.3m Chiều dài tôi đa khâu 3 (L3) 0.5m Khối lượng khâu | (m1) 10kg Khối lượng khâu 2 (m2) 5kg Khỗi lượng khâu 3 (m3) 7kg

Khối lượng khâu 4 (m4) lke

Moment quanh tinh khoi tam khâu 1 (J1) 0.728kem2

Moment quanh tinh khéi tam khau 2 (J2) 0.19kem2

Momert quánh tính khối tâm khâu 4 (J4) 0.03kem2 Khoảng cách thừ khớp 1 đến tâm khâu 1 (1g1) 0.2m

Khoảng cách thừ khớp 1 đến tâm khâu 2 (1g2) 0.15m

Khoảng cách thừ khớp 1 đến tâm khâu 4 (Ie4) 0m

Giới hạn quay khâu | -140 -> 140

Giới hạn quay khâu 2 -115->115

Giới hạn quay khâu 4 -180 ->180

150 mm/s

Ta có không gian làm việc:

Giới hạn quay khâu | -140 -> 140

Giới hạn quay khâu 2 -115->115

Giới hạn quay khâu 4 -180 ->180 Chiều dài khâu 1 (L1) 0.4m Chiều dài khâu 2 (L2) 0.3m Chiều dài tối đa khâu 3 (L3) 0.5m

for the4=-180*pi/180:0.5:180*pi/180

DONG LUC HOC ROBOT

Hệ quy chiếu gán với trục tọa độ 0o Xo Yo Zo trên khớp thứ nhất khi đó mặt phẳng

0o Xo Yo Zo la map phang dang thé

Bằng việc sử dụng solidwork thiết kế các khâu của roobot với vật liệu là thép chế tạo máy

ta có được khôi lượng các khâu của roobot như sau :

Chiều dài khâu 1 (L1) 0.4m Chiều dài khâu 2 (L2) 0.3m Chiều dài tôi đa khâu 3 (L3) 0.5m Khối lượng khâu 1 (m1) 10kg Khối lượng khâu 2 (m2) 5kg Khối lượng khâu 3 (m3) 7kg Khối lượng khâu 4 (m4) Ike Moment quanh tinh khéi tam khau 1 (J1) 0.728kgm2 Moment quanh tính khối tâm khâu 2 (J2) 0.19kgm2 Moment quanh tính khối tâm khâu 4 (J4) 0.03kem2

1, lực tác động lên các khâu

a, khau sé 1:

- La khâu nối trục máy và khâu số 2 và chuyên động của khâu số 1 là chuên động quay

- Các lực tác động lên khâu số 1 bao gồm : trọng lực của khâu , lực tác động với khâu

trục ( khâu số 0 ) và với khâu 2,3,4

b, khâu số 2 :

- Nối với khâu 1 và khâu 3 ; chuyên động là chuyên động quay

- Lực tác động lên khâu 2 bao gồm trọng lực của khâu , lực tương tác với khâu I và

- Chuyện động quay , làm nhiệm vụ gắp phôi

- Lực tác đụng bao gồm trọng lượng khâu , lực tương tác với khâu 3 và trọng lượng

của phôi

2, Động năng của các khâu tay máy và thế năng

Ta sử dụng bộ công thức sau:

¢ Điều kiện ban đầu:

Vận tôc góc của hệ tọa độ gốc như sau:

v =| 0 0 0

Gia tốc trọng trường của hệ tọa độ gốc theo phương Z và hướng xuống:

Toa d6 trong tam tai mỗi khâu:

Moment quản tính tại mỗi khâu:

[ -] /

I / / -/

-[ -] / / -[ -]

Trong đó: I la moment quản tính tác động lên trục X của hệ trục tọa độ thứ {1}, I, " là

moment quản tính tác động lên trục Y của hệ trục tọa độ thứ {1}, 1„ là moment quản tính tác động lên trục Z của hệ trục tọa độ thứ {1}, Tụ là moment quán tính tác động lên trục XY của

hệ trục tọa độ thứ {1}, _ la moment quản tính tác động lên trục XZ của hệ trục tọa độ thứ

{i}, I la moment quản tính tác động lên trục YZ của hệ trục tọa độ thứ {1}

- Tai khau 1:

@,—=,Ñ@ +07 Vận tốc góc của khâu I so với hệ trục toa độ góc

u„ = R{U + @ <p )

- Tại khâu 4:

Van toc góc của khâu 4 so với hệ trục tọa độ {4}

@ =.ÑR@.+0 2 Vận tốc góc của khâu 4 so với hệ trục toa độ góc

1u = RUu + @ xp )

i1 IR it!

Trong đó: ' : là ma trận xoay của hệ trục thứ {1} lên hệ truc thir {1+1}, ‘ “+1 1a van toc

¬-= A nog: "O 4 nk ˆ pony O NA XÂ cự sae Ũ

góc của khâu thir {i+1}, “2: là vận tốc góc của khâu thứ {1}, ˆ'*!: là vận tộc góc tại khớp thứ

ial „ 4

¡*! : là vecto của trục Z tai hé truc thir {1+1}, Mel là van toc tuyên tính ở điểm

i+l >

titty,“

As LA cự Ta CA XÁ Kup pp aR ko ca Pu

gốc của hệ trục thứ {1+1}, ”': là vận tốc tuyên tính ở điểm gốc của hệ trục thir {i}, "1: là vị

i+1

tri diém goc của hé truc thir {1+1} chiêu lên hệ trục thứ {1}, “1; 1a vi tri trọng tâm của

khớp {i+1} chiéu lén toa d6 {1}

s Tính toán động lực học robot