đồ án thiết kế hệ thống cơ điện tử thiết kế robot scara vận chuyển sản phẩm

Khâu 1: Khâu dẫn động nằm ngang vuông góc với trục thẳng đứngtrong suốt quá trình làm việc của robot, có khả năng quay xung quanh trục Z qua khớp quay 1.Khâu 2: Khâu động có khả năng qua

ĐẠI HỌC BÁCH KHOA HÀ NỘI TRƯỜNG CƠ KHÍ

-Đồ án

Thiết kế hệ thống cơ điện tử

Thiết kế robot SCARA vận chuyển sản phẩm Mã học phần: ME5512

1 Phân tích, lựa chọn cấu trúc robot

1.1 Mục đích ứng dụng của robot

- Thiết kế mô hình robot thực hiện gắp thả đối tượng từ băng tải này sang băng tải khác Robot hoạt động linh hoạt, gọn gàng và tiết kiệm không gian làm việc tốt nhất Robot thao tác giữa 2 băng tải vuông góc với nhau và bề rộng mỗi băng tải là 30 cm.

1.2 Phân tích lựa chọn kết cấu

Robot có 3 bậc tự do, với 2 khớp xoay và 1 khớp tịnh tiến:

Khâu 0 (Khâu cố định): Giữ cố định Robot và nối với khâu động 1qua khớp quay 1

Khâu 1: Khâu dẫn động nằm ngang vuông góc với trục thẳng đứngtrong suốt quá trình làm việc của robot, có khả năng quay xung quanh trục Z qua khớp quay 1.

Khâu 2: Khâu động có khả năng quay quanh trục Z qua khớp nối 2khâu 2 với khâu 1.

Khâu 3: khâu động có khả năng chuyển động tịnh tiến theo trục Z, nhờ bộ truyền chuyển động quay sang tịnh tiến.

1.3 Xác định các thành phần của hệ thống hoạt độngĐộng cơ:

Sử dụng động cơ servo của hãng SKS

Cảm biến:

• Encoder: đi cùng động cơ servo giúp điều khiển tốc độ thuận lợi hơn.• Công tắc hành trình: sử dụng để giới hạn chuyển động tịnh tiến của

khâu 3.

Bộ nguồn:

Nguồn cho drive là nguồn AC hoặc DC.Nguồn cho servo là nguồn AC do drive cung cấp.Mạch dẫn động cơ khí:

Bộ điều khiển động cơ:

Phần mềm điều khiển động cơ:

Sử dụng phần mềm MatLab/Simulink để viết chương trình điều khiển.

2 Thiết kế quỹ đạo chuyển động, bài toán động học

2.1 Thiết kế quỹ đạo chuyển động2.1.1 Đặt bài toán

Robot thực hiện những thao tác để xử lý yêu cầu đặt ra

- Khi băng chuyền 1 đến vị trí đặt sẵn, tay gắp di chuyển từ vị trí ban đầu S đến vị trí A’ để chuẩn bị gắp vật

- Di chuyển đến vị trí điểm A để gắp vật

- Sau khi gắp vật tay gắp di chuyển về vị trí an toàn A’

- Di chuyển đến vị trí điểm B’ là điểm cần đặt vật ( tại băng chuyền 2)- Di chuyển đến điểm B rồi thả vật

BB’

A (0.1 ;0.45; 0.735)B (0.45 ; 0.1 ;0.535)B’(0.45 ; 0.1 ; 0.825)

Không gian làm việc được xây dựng trên phần mềm Matlab:Không gian làm việc của robot 2D:

Không gian làm việc của robot 3D:

2.1.2 Xây dựng quỹ đạo đường thẳng trong không gian thao tácDo yêu cầu chỉ là những bài toán di chuyển giữa điểm – điểm trong không gian thao tác, không có yêu cầu gì về vận tốc cũng như gia tốc nên ta có thể chọn quy luật chuyển động là hàm bậc 3 sẽ cho phép tạo ra được một quỹ đạo trơn tru về di chuyển.

Xét một bài toán di chuyển giữa 2 điểm M và N biết:- Vị trí điểm đầu M: r = M [x0 y0 z0]

- Vị trí điểm kết thúc N: r = N [xeyeze]- Hướng khâu cuối tại M: θ0 (rad)- Hướng khâu cuối tại N: θe (rad)

- Vận tốc tại M: v = 0 (m/s), 0ω0 = 0 (rad/s)- Vận tốc tại N: v = 0 (m/s), eωe = 0 (rad/s)- Thời điểm bắt đầu chuyển động: t (s)0

- Thời điểm kết thúc chuyển động: t (s)e

Giải hệ ta được: { a0=0a1=0a2= 3 L

(te−t0)2a3= −2 L

(te−t0)3

s(t) = 3 L(t−t0)2

(t−t)2−2 L(t t−0)3(t−t)3

- Với tọa độ P trên quỹ đạo tại thời điểm t là rP(t)= [xp(t) yp( ) ztp(t )]Thì ta có: rP(t) = rM + rN−rM

L s(t)Quỹ đạo của hướng khâu thao tác:- Góc quay: = θe – θ0

- Gọi φ(t) là hàm góc quay theo quy luật bậc 3:

φ(t)=¿b0 + b1(t−t0)+ b2(t−t0)2+ b3(t−t0)3- Dựa theo các điều kiện đề bài ta có:

{ φ(t0)=0φ(te)=¿˙φ(t0)=0

Giải hệ ta được: { a0=0a1=0a2= 3

(te−t0)2a3= −2

(te−t0)3φ(t) = 3(t−t0)2

- Gọi θp(t) là hướng của khâu cuối tại thời điểm t:

θp(t) = θ0 + θe−θ0

❑ φ(t)

Quỹ đạo của khâu thao tác là:

- Toạ độ điểm P của khâu thao tác tại thời điểm t:rP(t) = rM + rN−rML s(t)- Hướng của khâu cuối tại thời điểm t: θp(t) = θ0 + θe−θ0

❑ φ(t)

Dựa vào kết quả của ( rP(t), θp(t)¿ giải bài toán động học ngược ta sẽ thu được kết quả của quỹ đạo trong không gian khớp:[q1(t)q2(t) q3(t )]

2.2 Tính toán động học2.2.1 Động học thuận

z

Biến khớp q = [q1 ,q2,q3]T

q1 = (-125°÷ 125°¿,q2 = (-145°÷ 145°¿, q = (0 3÷ 350mm)- Kí hiệu: cq1 ¿cos q1, sq = 1 sin q, q = 12q1+q2

- Tính các ma trận thuần nhất:

T1 = [cq1 −sq1 0 a1cq1sq1cq10 a1sq1

T2 = [cq2 −sq2 0 a2cq2sq2 cq2 0 a2sq2

- Ma trận biểu diễn hướng của điểm thao tác:

R3 = [cq12 −sq12 0sq12cq120

- Vị trí điểm thao tác:

r3 = [a cq2 12+a cq1 1a sq2 12+a sq1 1d1+d2−q3 ]

- Vận tốc của điểm thao tác cuối: Vận tốc dài:

v3 = ˙

3 = [−a2sq12 ˙q1−a sq2 12 ˙q2−a1sq1 ˙q1a2cq12 ˙q1+a2cq12 ˙q2+a cq11 ˙q1

- Vận tốc góc:

~ω3 = R˙3 R3

= [−sq12( ˙q1+ ˙q2) −cq12( ˙q1+ ˙q2) 0cq12( ˙q1+ ˙q2)−sq12( ˙q1+ ˙q2) 0

−sq12 cq12 0

¿[ 0−( ˙q1+ ˙q2) 0˙

=> Vận tốc góc điểm thao tác cuối: ω3 = [ 00˙q1+ ˙q2]

- Gia tốc điểm thao tác cuối: at 30= v˙

- Phương trình động học thuận của robot:

{x=300 cq12+300cqy=300 sq12+300sq1

(mm)2.2.2 Động học ngược:

Với bài toán động học ngược thì vị trí của khâu thao tác xem như đã biết, yêu cầu tìm giá trị của các biến khớp

Giả sử tọa độ điểm cuối là:

re = [xeyeze]

Phương trình tọa độ điểm cuối:

{xe=a2cq12+a cq11(1)ye=a sq2 12+a1sq1(2)

Từ phương trình (3) ta có:

q3=d1+d2−zeTừ phương trình (1) và (2) ta có:

{xe=a2 c2q12+a1c2

q1+2 a1a2cq12cq1(4)ye=a2s2

q1+2 a1a2sq12sq1(5)

Với c2q = cos2

q, s2q = sin2q

{cq1(a2cq2+a1)−sq1a2sq2=xecq1a2sq sq2+ 1(a2cq2+a1)= ye

Sử dụng phương pháp Cramer ta có: D = |a cq a22+1−a2sq2

a2sq2a cq22+a1| = a1 + a2 + 2a1a2cq2 =xe + yeD1 = |xe −a2sq2

yea2cq2+a1| = a1xe + a2¿ + yesq2¿

D2 = |a cq a22+1xe

a2sq2ye| = a1ye + a2¿ + xesq2¿

→cq1=¿ D1

D = a1xe+a2(xecq2+yesq2)xe+ ye

sq1 = D2

D = a1ye+a2( yecq2−xesq2)xe+ ye

→q1 = arctan(sq1

cq1) = arctan[a1ye+a2( yecq2−xesq2)a1xe+a2(xecq2+yesq2)]

Thay các tham số của robot ta có:

{q1=arctan[300 ye+300( yecq2−xesq2)300 xe+300(xecq2+ yesq2)]

q2=arccos(xe+ ye2−300 −3002

Phương trình vi phân chuyển động:

M(q)¨q + C (q,˙q) ˙q + G(q) = Q Trong đó:

M là ma trận khối lượngC là ma trận corilois

Q là vecto lực suy rộng của các lực không thế, Q = U + JEFE( )0

U là vecto lực suy rộng ứng với tọa độ khớp qicủa các lực dẫn động τicủa động cơ đặt tại các khớp: U = [τ1,τ2,τ3]T

Với khớp tịnh tiến thì τilà lực Fmi

Với khớp quay thì τi là ngẫu lực có momen Mmi

Ở nội dung báo cáo này, em thiết lập phương trình vi phân chuyển động cho robot dạng ma trận.

Ma trận khối lượng

Vị trí và hướng của hệ tọa độ Cix yci cizci gắn vào khâu thứ i tại khối tâm Ci , so với hệ tọa độ cơ sở được xác định bởi:

ACi0 = Ti

1 = [cq1 −sq1 0 a1cq1sq1cq10 a1sq1

AC 20 = T2AC 2

2 = T1T2AC 22 =

[cq1−sq10 a1cq1sq1 cq1 0 a1sq1

0001 ][cq2−sq20 a2cq2sq2 cq2 0 a2sq2

Từ AC 3

0 => Ma trận quay RC 3

0 = [cq12−sq120sq12cq120

RC 3

0 T = [cq12sq120−sq12cq120

˙RC 3

0 = [−( ˙q1+ ˙q2)sq12−( ˙q1+ ˙q2)cq12( ˙q1+ ˙q2)cq12−( ˙q1+ ˙q2)sq120

= RC 30 TR˙

C 3

0 =[cq12 sq12 0−sq12 cq12 0

001][−( ˙q1+ ˙q2) sq12−( ˙q1+ ˙q2)cq12( ˙q1+ ˙q2)cq12 −( ˙q1+ ˙q sq2) 12 0

= [ 0−( ˙q1+ ˙q2) 0˙

000] => ω3R

= [00q˙1+ ˙q2]T

Ma trận jacobi quay JR 3 = [0 00

1 10] => JR 3T

¿¿) = [m11(q)m12(q) m13(q)m21(q)m22(q) m23(q)m31(q)m32(q) m33(q)]

m11= Izz1 + Izz2 + Izz3 + m1(a1−l1)2 + m2[(a2−l2)2+a1+2a1(a2−l2)cq2] + m3(a1+a2+2

a a1 2cq2)

m22 =Izz2 + Izz3 + m2(a2−l2)2 + m3a2m33 = m3

m21 = m12 = Izz2 + Izz3 + m2[(a2−l2)2+a1(a2−l2)cq2]+ m3(a2+a a1 2cq2)

m13 = m31 = m23 = m32 = 0

Thay l1 = a12,l2 = a2

2 ta có:

m11= Izz1 + Izz2 + Izz3 + m1a1

4 + m2[a2

4 +a1+a1a2cq2] + m3(a1+a2+2a1a2cq2)

m22 =Izz2 + Izz3 + m2a2

4 + m3a2m33 = m3

m21 = m12 = Izz2 + Izz3 +m2 [a24 +

2 cq2]+ m3(a2+a1a2cq2)

m13 = m31 = m23 = m32 = 0Ma trận Corilois

Ma trận đặc trưng cho lực quán tính Coriolis và lực quán tính li tâm C(q, ˙q) xác định bởi:

C(q, ˙q) = [c11(q)c12(q) c13(q)c21(q)c22(q) c23(q)c31(q)c32(q) c33(q)]

Các phần tử của ma trận này được xác định theo công thức:

cij(q, ˙q) =12∑

k =13

¿¿ ∂mij∂qk + ∂mik

∂qj - ∂mjk∂qi )q˙kc11(q, ˙q) = −(m2

2+m3)a a sq1 2 2˙q2c12(q, ˙q) = −(m2

2+m3)a1a2sq2˙q1−¿ (m2

2 +m3)a1a2sq2q˙2c21(q, ˙q) = (m2

c13(q, ˙q) = c31(q, ˙q) = c22(q, ˙q) = c33(q, ˙q) = c23(q, ˙q) =c32(q, ˙q) = 0 Lực suy rộng của các lực có thế

Chọn gốc thế năng trùng với hệ tọa độ cơ sở, khi đó thế năng của robot được xác định:

Π = ∑

miz0cig = gm1d1 + gm2(d¿¿1+d2)¿+ gm3(l¿¿3+d1+d2−q3)¿

G(q) = (∂Π∂qi)T

= [0,0 ,−gm3]TLực suy rộng của các lực không thế

Trong nội dung đồ án, Robot được coi là mô hình lí tưởng, do đó bỏ qua lực ma sát, lực cản nhớt Lực không thế ở đây chỉ bao gồm lực do vật tác động lên khâu thao tác cuối Xét trường hợp tổng quát với lực và momen có giá trị:

F3 = [Fx Fy Fz]TM3= [Mx My Mz]TLực suy rộng được được xác định theo công thức:

Q = U + JEFE( )0

Trong đó: + Lực tác động lên khâu cuối là FE(0)

= [0 0−Fz]T + U =[τ1 τ2 τ3]T

là lực dẫn động τi của động cơ đặt tại các khớp.

Vậy: Q = [ 00−Fz] + [τ1

τ3] là vector lực suy rộng cần xác định Phương trình vi phân chuyển động:

[m11(q) m12(q)0m21(q) m22(q)0

q3]+[c11(q) c12( ) 0q

000][q˙1˙q2˙q3]+[ 0

0−gm3]−[ 0

0−Fz]= [τ1

Khối lượng các khâu:

Ta có bảng tham số động lực học của robot như sau:

Vị trí trọng tâm so với hệ tọa độ gắn trên khâu i

Khối lượng mi

Momen quán tính khối và momen tích quán tính của khâu i đối với các trục x , y , zcicici

M(q)¨q + C (q,˙q) ˙q + G(q) = Q Kết quả bài toán động lực học ngược:

τ = [τ1τ2

τ3] = [m11(q)q¨1+m12(q)q¨2+c11(q)q c˙1+ 12(q) ˙q2m21(q)¨q1+m22(q)q¨2+c21(q)q˙1

m33(q)¨q3−g m3+Fz ]

4 Thiết kế hệ thống điều khiển

4.1 Chọn quy luật điều khiển phù hợp

Ở những chương trên, nhóm đã khảo sát, xác định quy luật biến thiên của quỹ đạo trong không gian thao tác cũng như là quy luật biến thiên của các biến khớp trong không gian khớp theo thời gian Trong phần 7, nhóm sẽ đi khảo sát, thiết kế bộ điều khiển phù hợp với yêu cầu cho Robot bám theo quỹ đạo đã khảo sát ở trên.Mô hình nhóm chọn để điều khiển là Phương trình vi phân chuyển động của Robotđã nghiên cứu trong chương 5 Ta có thể nhận thấy, phương trình động lực học củaRobot là một phương trình vi phân phi tuyến Có nhiều cách để có thể thiết kế ra bộ điều khiển hệ phi tuyến, ví dụ như:

- Phương pháp tuyến tính hoá hệ phi tuyến.- Phương pháp điều khiển tối ưu.

- Phương pháp Lyapunov

Trong đó thì phương pháp được áp dụng nhiều nhất là phương pháp tuyến tính hoáthông qua việc điều khiển Momen (Lực) đối với những bài toán điều khiển Robot do tính ổn định cao mặc dù khá đơn giản.

Mục tiêu của bài toán điều khiển nhóm đề ra là làm cho robot có thể bám theo quỹ đạo đã được thiết kế Các phần tử dẫn động sẽ nhận vào lệnh điều khiển là các giá trị Momen(Lực).

Trước hếtt, ta có mô hình toán học của hệ đã được xây dựng là:M(q)¨q + C (q,˙q) ˙q + G(q) = Q

Trong phương trình này, vế trái của phương trình là mô hình với các tham số của Robot, vế phải là momen mà bộ điều khiển tác động lên Robot Momen này được sinh ra từ bộ điều khiển với giá trị phù hợp sao cho khi đưa vào Robot có thể chuyển động theo quỹ đạo đã cho.

Thiết kế hàm điều khiển theo dạng sau:

;

Kp, Kd là ma trận các giá trị của 3 khâuSơ đồ khối của bộ điều khiển và robot:

4.2 Lựa chọn các thiết bị cho hệ thống điều khiển Động cơ Servo

Ứng dụng Điều khiển vị trí, tốc độ, mô menKhâu 2,3: Sử dụng driver có mã hiệu MR-J4-10A-RJ.

USB CN8 kết nối với PC -> configured DriverRS422/RS485 CN3 (chuẩn RJ45) -> kết nối PC, điều khiển

Frame size: 63AF Dòng định mức: 10A Dòng cắt ngắn

Điện áp cách điện: 690V Kích thước:

Chức năng: Dùng để kiểm soát

Bảo vệ quá tải và ngắn mạch Ứng dụng: Dùng trong mạng lưới điện

dân dụng và công nghiệp Tiêu chuẩn: IEC/EN 60947-2

Điện áp thử nghiệmxung (Uimp): 8kV

Xuất xứ: Mitsubishi Nhật Bản Bảo hành: 12 tháng

Số lượng tiếp điểm 2 cặp NO

Chức năng: Dùng để kiểm soátBảo vệ quá tải và ngắn mạch

Chất liệu vỏ Nhựa PC chống cháy nổ

Nút nhấn

Harmony ZB4BZ101+1NO

Harmony ZB4BZ101+1NC

Harmony XB4BS9445+1NCA

Chức năng: Dùng làm nút dừng khẩn cấp cho hệ thốngCảm biến về gốc

Cả 3 khâu đều sử dụng loại Micro Proximity Sensor GX-8 của Panasonic

Thông số kĩ thuật:

4.3 Lập trình điều khiển