ĐỒ án THIẾT kế hệ THỐNG cơ điện tử PHẦN 1 THIẾT kế mô HÌNH 3d ROBOT RRR sử dụng phần mền solidwork để thiết kế mô hình 3d của robot RRR

PHẦN 2: TÍNH TOÁN ĐỘNH HỌC THUẬN VÀ NGƯỢC ROBOTCâu hí*nh rộbột RRR 1.bảng tham số DH a.Xây dựng hệ tọa độ DH:  Hế tộâ độ O0x0y0z0: Gộc tộâ độ đât tâi tâm cu&â khớp động thử nhât Truc O0

ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ PHẦN 1: THIẾT KẾ MÔ HÌNH 3D ROBOT RRR

Sử dụng phần mền solidwork để thiết kế mô hình 3D của robot RRR

1.thiết kế các khâu:

Khâu đế:

Khâu 1:

Khâu 2:

Khâu 3:

2.Các chi tiết khác

Bânh râng truc bânh rân

Nâp bộ truyến bânh râng

Xylânh khí nến

PHẦN 2: TÍNH TOÁN ĐỘNH HỌC THUẬN VÀ NGƯỢC ROBOT

Câu hí*nh rộbột RRR

1.bảng tham số DH

a.Xây dựng hệ tọa độ DH:

 Hế tộâ độ O0x0y0z0:

Gộc tộâ độ đât tâi tâm cu&â khớp động thử nhât

Truc O0z0 dộc thếộ hửớng cu&â truc khớp động thử nhât,hửớng tử* dửới lếnTruc O0x0 cộ phửớng vuộng gộc với O z0 hửớng tử* ngộâ*i vâộ trộng nhử hí*nh vế2

Truc O0y0 xâc đinh thếộ quy tâc tâm diến thuân

 Hế tộâ độ O1x1 y1z1:

Gộc tộâ độ đât tâi tâm cu&â khớp động thử 2

Truc O1z1 phửớng dộc thếộ truc khớp động thử 2 hửớng tử* trộng râ ngộâ*i nhử hí*nh vế2

Truc O1x1 cộ phửớng nâ4m trến đửớ*ng vuộng gộc chung cu&â truc O0z0 vâ* O1z1

hửớng tử* O0 tới O1

Truc O1y1 xâc đinh thếộ quy tâc tâm diến thuân

 Hế tộâ độ O2x2y2z2:

Gộc tộâ độ đât tâi tâm cu&â khớp động thử 3

Truc O2z2 phửớng dộc thếộ truc khớp động thử 2 hửớng tử* trộng râ ngộâ*i nhử hí*nh vế2

Truc O2x2 cộ phửớng nâ4m trến đửớ*ng vuộng gộc chung cu&â truc O1z1 vâ* O2z2

hửớng tử* O1 tới O2

Truc O2y2 xâc đinh thếộ quy tâc tâm diến thuân

 Hế tộâ độ O3x3y3z3:

Gộc tộâ độ đât tâi tâm cu&â tây kếp

Truc O3z3 phửớng dộc thếộ truc khớp động thử 2 hửớng tử* trộng râ ngộâ*i nhử hí*nh vế2

Truc O3x3 cộ phửớng nâ4m trến đửớ*ng vuộng gộc chung cu&â truc O2z2 vâ* O3z3

θ i : gộc quây quânh truc z i−1 đế7 truc x i−1 tru*ng với truc x' i(x i/ ¿x ' i)

d i : dich chuyế7n tinh tiế dộc truc z i−1 đế7 gộc tộâ độ O i−1 chuyế7 đến O ' i lâ* giâộ

điế7m cu&â truc x i vâ* truc z i−1

a i : dich chuyế7n dộc truc x i đế7 điế7m O ' i chuyế7n đến điế7m O i

α i : gộc quây quânh truc x i sâộ chộ truc z' i−1(z' i−1/ ¿z i−1) tru*ng với truc z i

 Câc mâ trân truyến:

Mâ trân truyến khâu i sộ với khâu i-1:

Mâ trân truyến khâu 2 vâ* 3 sộ với khâu 0:

2.bài toán động học thuận

Câc thộng sộ đâu vâ*ộ :q1, q2, q3

Thộng sộ cân xâc đinh: điế7m tâc động cuội P=( Px, Py,Pz),hửớng cu&â khâu cuội

 Tộâ độ điế7m tâc động cuội:

 Hửớng cu&â khâu thâộ tâc cộ mâ trân cộsin chí& hửớng:

3.bài toán động học ngược

Thộng sộ đâu vâ*ộ lâ* tộâ độ điế7m tâc động cuội: P=( Px; Py; Pz) vâ* hửớng khâu cuội

Thộng sộ cân xâc đinh:câc biến khớp (q1; q2; q3)

Tâ cộ mâ trân câu hí*nh cu&â khâu thâộ tâc:

Trộng độ a14, a24, a34 lâ* câc tộâ độ cu&â điế7m thâộ tâc E:

a14= ,a24= , a34=

Cộ*n a ijlâ* câc phân tử& cu&â mâ trâ cộsin chí& hửớng cu&â khâu thâộ tâc

Tử* bâ*i tộân động hộc thuân tâ cộ câc mâ trân:

(2)

chộ câc phân tử& (1,4) vâ* (2,4) cu&â hâi mâ trân (1) vâ* (3) bâ4ng nhâu tâ đửớc:

Tử* phửớng trí*nh mâ trân (3) tâ suy râ ;

trộng độ :

= (8)

(9)Mât khâc tâ cộ :

(10)

Sộ sânh câc phân tử& ớ& câc ộ (1,4) , (2,4 ) vâ* (3,4) cu&â câc mâ trân (9) vâ* (10)

tâ đửớc :

(11) (12) (13)Bí*nh phửớng hâi vế cu&â phửớng trí*nh (11) vâ* (12) rội cộng lâi tâ đửớc :

(14)Tử* độ suy râ :

(15)Kết hớp (13) vâ* (15) tâ cộ :

Sộ sânh câc phân tử& (3,1) vâ* (3,2) cu&â hâi mâ trân (1) vâ* (2 ) tâ đửớc :

,

4.tính vận tốc và gia tốc điểm tác động cuối

vân tộc dâ*i vâ* giâ tộc dâ*i cu&â điế7m tâc động cuội đửớc suy râ tử* đâộ hâ*m cu&â vếc tớ tộâ độ r(0)E :

 Vân tộc cu&â điế7m tâc động cuội:

 giâ tộc cu&â điế7m tâc động cuội:

 Vân tộc gộc câc khâu:

tính vân tộc gộc khâu thử i cu&â rộbột dửâ trến cộng thửc tính vân tộc gộccu&â vât rân thộng quâ mâ trân cộsin chí& hửớng cu&â nộ:

(sử& dung phân mếm mâplế)

khâu 3:

PHẦN 3:ĐỘNG LỰC HỌC CỦA ROBOT

Dạng ma trận của phương trình lagrange II:

Động nâng cu&â rộbột cộ dâng:

Trộng độ = , Phửớng trí*nh lâgrângế II cộ dâng :

Sử& dung đinh ly đâộ hâ*m riếng thếộ vếctộ tích cu&â hâi mâ trân tâ cộ:

(1)Trộng độ:

(2)Mât khâc:

(3)

Thây (2) vâ* (3) vâ*ộ (1) tâ đửớc:

, (4) Tính tộân tửớng tử tâ cộ:

(5)Thây (4) vâ* (5) vâ*ộ phửớng trí*nh lâgrângế II tâ đửớc:

(6)Thếộ đinh ly đâộ hâ*m tộâ*n phân vâ* đâộ hâ*m riếng tâ cộ:

thây vâ*ộ (6) tâ đửớc:

Mâ trân ly tâm vâ* cộriộlis cộ dâng:

Khi độ phửớng trí*nh vi phân chuyế7n động cu&â câc khâu :

1.bảng tham số động lực học của robot

khâu Vi trí trộng tâm Khội

lửớng

Mâ trân mộmến quân tính

1 0 −l C 1 0 m1 I 1x I 1 y I 1z 0 0 0

2 −l C 2 0 0 m2 I 2x I 2 y I 2 z 0 0 0

3 −l C 3 0 0 m3 I 3x I 3 y I 3z 0 0 0

2.ma trận Jacobi của các khâu

Tử* câu tâộ hí*nh hộc cu&â rộbột tâ xâc đinh đửớc tộâ độ khội tâm cu&â câc khâu sộ với hế tộâ độ cộ đinh R0:

3.ma trận khối lượng suy rộng của robot:

=Trộng độ:

4.Ma trận ly tâm và coriolis :

=Sử& dung phân mếm Mâplế nhân râ tâ đửớc :

5.Thế năng của robot

Tử* độ suy râ :

6.phuơng trình vi phân chuyển động của các khâu

Thế câc biế7u thửc vâ*ộ phửớng trí*nh Lâgrângế lộâi hâi :

Tâ nhân đửớc hế phửớng trí*nh vi phân chuyế7n động cu&â rộbột bâ khâu trộng khộng giân :

Khâu 1 :

Khâu 2 :

Khâu 3 :

PHẦN 4: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT

1.thiết kế quỹ đạo trong không gian khớp :

Lâ* xâc đinh đửớ*ng biế7u diếLn cu&â vi trí gộc khớp(gộc quây cu&â khớp với khớp quây vâ* độ di chuyế7n cu&â khớp đội với khớp tinh tiến) thếộ thớ*i giân khi di chuyế7n tử* vi trí bân đâu q0(A) tới vi trí cuội q c(B) trộng thớ*i giân t cvới

q lâ* biến khớp tộ7ng quât Quy2 đâộ di chuyế7n cu&â khớp giử2â hâi vi trí sế2 thộ&â mâ2n 4 điếu kiến : vi trí bân đâu vâ* vi trí cuội cu*ng, tộc độ tâi vi trí bân đâu vâ* tộc độ tâi vi trí cuội cu*ng Dộ độ đâ thửc bâc 3 sế2 phu* hớp với quy2 đâộ cu&â khớp rộbột:

q i(t)=a 0i+a1i t +a 2i t2+a 3i t3

giâ& thiết vân tộc tâi điế7m đâu vâ* điế7m cuội bâ4ng 0:

˙q i 0=0 ; ˙q ic=0

Tâ cộ hế phửớng trí*nh:

Giâ&i hế tâ đửớc câc hế sộ cu&â phửớng trí*nh quy2 đâộ.

Đửớ*ng biế7u diếLn vi trí, vân tộc vâ* giâ tộc cu&â khớp 1 thâộ tâc thếộ thớ*i giân

 Hế sộ cu&â phửớng trí*nh quy2 đâộ cu&â khớp 2:

a02= ¿0 a12=0 a22=3.6 a32=−0.48

Phửớng trí*nh động hộc cu&â khâu 2:

Đửớ*ng biế7u diếLn vi trí, vân tộc vâ* giâ tộc cu&â khớp 2 thâộ tâc thếộ thớ*i giân

 Hế sộ cu&â phửớng trí*nh quy2 đâộ cu&â khớp cuội:

a03= ¿0 a13= ¿0 a23= ¿7.2 a33=−0.96

Phửớng trí*nh động hộc cu&â khâu :

Đửớ*ng biế7u diếLn vi trí, vân tộc vâ* giâ tộc cu&â khớp 3 thâộ tâc thếộ thớ*i giân.

2.thiết kế quỹ đạo trong không gian làm việc

Tây rộbột di chuyế7n tử* vi trí A(x A ; y A ; z A) trộng khộng giân lâ*m viếc tới vi trí B(x B ; y B ; z B) thếộ một đửớ*ng thâQng tâ cộ phửớng trí*nh đửớ*ng thâQng AB cộ dâng sâu:

Chộ rộbột chuyế7n động trộng thớ*i giân 5(s) đi tử* vi trí A(90; 0; 55) tớ vi trí B(0; 43.3; 120) Tâi vi trí A câc khớp q1=0, q2=0, q3=0

Phửớng trí*nh đửớ*ng thâQng AB:

PHẦN 5: ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT

1.Hệ thống điều khiển phản hồi trong không gian khớp

Chửc nâng cu&â hế thộng điếu khiế7n lâ* tâộ râ một mộmến cân thiết đế7 truyếnđộng khớ p rộbột đâ&m bâ&ộ khớp rộbột bâm thếộ vi trí đât

Sử& dung câu truc bộ điếu khiế7n cộ dâng tí& lế -tích phân- đâộ hâ*m (PID):

Trộng độ:

:lâ* sâi sộ vi trí cu&â khớp rộbột

: lâ* sâi sộ tộc độ cu&â khớp rộbột

=diâg(K P1,K P2,…,K Pn) :mâ trân đửớ*ng chếộ câc hế sộ khuếch đâi tử*ng khớp riếng biết

K D=diâg(K D1, K D2,….,K Dn):mâ trân đửớ*ng chếộ câc hế sộ đâộ hâ*m tử*ng khớp riếng biết

K I=diâg(K I 1 , K I 2,….K¿): mâ trân đửớ*ng chếộ câc hế sộ tích phân tử*ng khớp riếng biết

Hệ thống điều khiển với cấu trúc điều khiển trên ổn định tuyệt đối toàn cục thực vậy :

Hế tây mây với phửớng trí*nh động lửc hộc cộ dâng:

Luât điếu khiế7n cộ dâng:

Với giâ& thiết bộ& quâ thâ*nh phân cu&â mộmến trộng lửc tâ nhân đửớc hế điếukhiế7n vộ*ng kín:

Xết hâ*m lyâpunộv cộ dâng nhử sâu:

Tâ cộ M lâ* câc mâ trân dửớng Vây V luộn khộng âm Dâộ hâ*m V với

ε=q d −q tâ cộ :

=Thâ*nh phân vếctộ đửớc viết ớ& dâng mâ trân :

Trộng độ cộ quân hế với M(q) nhử sâu :

Với S lâ* mâ trân đội xửng lếch nến =0 dộ vây :

Nến : ≤ 0

Bât đâQng thửc chộ thây hế thộng ộ7n đinh tuyết đội

2.Các thao tác thực hiện mô phỏng

Khội lửớng cu&â câc khâu :

Khâu đế m0=10 kg, khâu 1m1=8 kg, khâu 2 m2=3 kg, khâu 3 m3=1.5 kg

Phửớng trí*nh động lửc hộc rộbột cộ dâng :

Luân điếu khiế7n cộ dâng sâu :

Với quy2 đâộ cu&â câc khớp đâ2 thiết lâp trộng phân thiết lâp quy2 đâộ đâ&m bâ&ộ điế7m tâc động cuội đi tử* điế7m A(90; 0; 55) tới điế7m B(0; 43.30; 120) trộng thớ*i giân 5(s)

Câc hế sộ khuếch đâi tí& lế, tích phân vâ* đâộ hâ*m đửớc lửâ chộn nhử sâu :

, ,

Tâ cộ mộ hí*nh simulink cu&â rộbột :

Mộ hí*nh bộ điếu khiế7n PID :

Trộng độ :

q d, v d lâ* tín hiếu quy2 đâộ đât vâ* vân tộc đât vâ*ộ câc khớp cu&â rộbột đửớc thiết lâp trộng phân thiết lâp quy2 đâộ

q, v lâ* câc tín quy2 độâ vâ* vân tộc phâ&n hội lâi tử* rộbột(quy2 đâộ thửc)

ớ& đây tâ sử& dung câc khội:

+khội Gâin du*ng đế7 khếch đâi câc tín hiếu đâu vâ*ộ

+khội mux châp câc tín hiếu đớn thâ*nh câc tín hiếu tộ7ng hớp vâ* tâch thâ*nh nhiếu tín hiếu

+khội input, ộuput đâu vâ*ộ vâ* râ cu&â tín hiếu

+khội hâ*m biế7u diếLn một hâ*m tộân hộc khi cộ tín hiếu đi vâ*ộ lâ* câc biến vâ* tín hiếu đâu râ lâ* giâ tri cu&â hâ*m

Vi trí câc khớp thếộ thớ*i giân

Vân tộc câc khớp thếộ thớ*i giân

Sâi sộ vi trí khớp thếộ thớ*i giân