Báo cáo robot scara serpent
Robot scara serpent
MỤC LỤC
MỞ ĐẦU....................................................................................................................... 2
I: MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT................................3
1.1. Một số loại robot Scara của các hãng sản xuất.................................................3
1.2. Các thông số và vùng làm việc của robot Scara Serpent.................................5
1.2.1. Cấu tạo tay máy robot Scara Serpent.........................................................6
1.2.1.1. Cấu hình của robot Scara Serpent.......................................................6
1.2.1.2. Các thông số kỹ thuật của robot Scara Serpent..................................7
1.2.2. Giới hạn không gian làm việc của robot Scara Serpent............................8
1.3. Động học robot Scara Serpent............................................................................9
1.3.1. Động học thuận..............................................................................................9
1.3.2. Động học ngược...........................................................................................13
1.4. Động lực học robot Scara Serpent....................................................................15
1.4.1. Hàm Euler - Lagrange và các vấn đề động lực học.................................16
1.4.2. Động lực học robot Scara Serpent.............................................................17
1.4.2.1. Tính toán động năng và thế năng cho từng khớp.............................17
1.4.2.2. Phương trình động lực học..................................................................21
1.5. Mô tả đối tượng bằng hệ phương trình trạng thái.........................................25
II: XÂY DỰNG MÔ HÌNH HỆ THỐNG ĐIỀU KHIỂN................................27
2.1. Cấu trúc hệ thống điều khiển robot.................................................................27
2.2. Lựa chọn phương pháp điều khiển và bộ điều khiển PID...........................28
III: MÔ PHỎNG VỚI MÔ HÌNH ROBOT SCARA SERPENT..................31
3.1. Đặt vấn đề............................................................................................................31
3.2. Mô phỏng robot scara trên EASY-ROB..........................................................31
KẾT LUẬN....................................................................................................................34
TÀI LIỆU THAM KHẢO............................................................................................35
Sinh viên: Phạm Văn Khiền
1
Robot scara serpent
MỞ ĐẦU
Theo quá trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất lượng
sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất.
Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính linh hoạt cao đã hình
thành và phát triển mạnh mẽ…Vì thế ngày càng tăng nhanh nhu cầu ứng dụng người
máy để tạo ra các hệ sản xuất tự động linh hoạt.
Robot ứng dụng rộng rãi và đóng vai trò quan trọng trong sản xuất cũng như
trong đời sống. Robot là cơ cấu đa chức năng có khả năng lập trình được dùng để di
chuyển nguyên vật liệu, các chi tiết, các dụng cụ thông qua các truyền động được lập
trình trước. Khoa học robot chủ yếu dựa vào các phép toán về đại số ma trận.
-
Robot có thể thao tác như con người và có thể hợp tác với nhau một cách thông
minh.
-
Robot có cánh tay với nhiều bậc tự do và có thể thực hiện được các chuyển
động như tay người và điều khiển được bằng máy tính hoặc có thể điều khiển
bằng chương trình được nạp sẵn trong chip trên bo mạch điều khiển robot.
Để hệ điều khiển robot có độ tin cậy, độ chính xác cao, giá thành hạ và tiết
kiệm năng lượng thì nhiệm vụ cơ bản là hệ điều khiển robot phải đảm bảo giá trị yêu
cầu của các đại lượng điều chỉnh và điều khiển. Ngoài ra, hệ điều khiển robot phải
đảm bảo ổn định động và tĩnh, chống được nhiễu trong và ngoài, đồng thời không gây
tác hại cho môi trường như: tiếng ồn quá mức quy định, sóng hài của điện áp và dòng
điện quá lớn cho lưới điện v.v...
Sinh viên: Phạm Văn Khiền
2
Robot scara serpent
I. MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT
1.1. Một số loại robot Scara của các hãng sản xuất
Robot Scara là một trong những robot phổ biến nhất trong công nghiệp. Chuyển
động của robot này rất đơn giản nhưng lại phù hợp với các dây chuyền và ứng dụng
hữu hiệu trong nhiệm vụ nhặt và đặt sản phẩm. Robot Scara (Selectively Compliant
Articulated Robot Arm) có nghĩa là tay máy lắp ráp chọn lọc.
Cấu trúc động học loại tay máy này thuộc hệ phỏng sinh, có các trục quay, các
khớp đều là thẳng đứng. Nó có cấu tạo hai khớp ở cánh tay, một khớp ở cổ tay và một
khớp tịnh tiến. Các khớp quay hoạt động nhờ động cơ điện có phản hồi vị trí. Khớp
tịnh tiến hoạt động nhờ xi-lanh khí nén, trục vít hoặc thanh răng.
Một số loại robot Scara của các hãng sản xuất:
Hình 1.1a: Turbo Scara SR60 của hãng Bosch.
Hình 1.1b: Assembly Scara Robot của
Hirata.
Sinh viên: Phạm Văn Khiền
3
Robot scara serpent
Hình 1.1c: Scara Robot của EPSON.
Hình 1.1d: Scara Robot của DENSO.
Hình 1.1e: Scara Robot of ADEPT.
Hình 1.1f: Scara Robot of RANOME.
Hình 1.1g: Scara Robot of KUKA.
Hình 1.1h: Scara Robot of STAUBLI.
Sinh viên: Phạm Văn Khiền
4
Robot scara serpent
Hình 1.2: Robot Scara Serpent của FEEDBACK.
Trong phần này tôi sẽ xây dựng mô tả toán học đối tượng robot Scara Serpent
của FEEDBACK.
1.2. Các thông số và vùng làm việc của robot Scara Serpent
Do chuyển động của robot Scara đơn giản, dễ dàng nên nó được sử dụng khá
phổ biến trong công nghiệp. Ở đây nghiên cứu robot Scara Serpent (Hình 1.3) một
loại cơ bản trong nhóm robot công nghiệp này.
Hình 1.3: Robot Scara Serpent.
Chiều cao của Robot có thể thay đổi dễ dàng bằng cách thay đổi vị trí gá thân
robot trên trục cơ bản, giúp tay máy thuận lợi trong việc thay đổi công việc.
Sinh viên: Phạm Văn Khiền
5
Robot scara serpent
Với thiết kế động cơ truyền động cho cổ tay được đặt trên trục cơ bản và liên hệ
với cổ tay bằng đai truyền, nên nó đảm bảo được góc quay của cổ tay không thay đổi
trong quá trình tay máy chuyển động.
Truyền động cho 2 khớp của tay máy và cổ tay bằng động cơ servo một chiều
có phản hồi vị trí tạo thành một vòng điều khiển kín. Chuyển động thẳng đứng được
thực hiện bằng piton khí nén.
Robot Scara Serpent có thể được lập trình từ máy tính bằng cách đặt dữ liệu cho
mỗi trục. Hoặc điều khiển bằng tay sử dụng thiết bị lái điện (steering) cho tay máy
dùng các cuộn dây điện từ trong giá treo (pendant).
1.2.1. Cấu tạo tay máy robot Scara Serpent
1.2.1.1. Cấu hình của robot Scara Serpent
Bao gồm một chuỗi các thanh cứng được liên kết với nhau bởi các khớp:
Hình 1.4: Cấu hình và các hệ trục tọa độ gắn trên Robot Scara Serpent.
Robot Scara Serpent gồm 3 khớp chuyển động quay và một khớp chuyển động
tịnh tiến. Gắn cho mỗi thanh nối một hệ trục toạ độ, ta có:
-
Khớp 1 quay quanh trục z0 góc θ1.
Sinh viên: Phạm Văn Khiền
6
Robot scara serpent
-
Khớp 2 quay quanh trục z1 góc θ2.
-
Khớp 3 chuyển động tịnh tiến theo trục z2 đoạn d3.
-
Khớp 4 quay quanh trục z3 góc θ4.
1.2.1.2. Các thông số kỹ thuật của robot Scara Serpent
Thông số của động cơ 1, 2, 3 tương ứng với các khớp 1, 2, 4 của tay máy robot
Scara Serpent (xem bảng 1.1).
-
Động cơ 1 truyền động cho khớp 1 (main).
-
Động cơ 2 truyền động cho khớp 2 (fore).
-
Động cơ 3 truyền động cho khớp 4 - khớp cổ tay (wrist).
Bảng 1.1: Thông số các động cơ của robot.
TT Loại
U(V) I(A) M(Nm) N(v/p) P(W) J(Kg.m2) R() L(H
)
1
J9ZF
12
4,8
4.10-2
2100
15
0,32.104
1,38
100
0,6
2
J9ZF
12
4,8
4.10-2
2100
15
0,32.104
1,38
100
0,6
3
J12ZF 12
4,8
1,2102
2100
26
1,5.10-4
0,95
100
1
m(Kg)
Bảng 1.2: Các thông số động học của robot Scara Serpent.
TT Thông số
Kích thước động học
1
M1 = 4 Kg
Khối lượng thanh nối 1.
2
M2 = 1.5 Kg Khối lượng thanh nối 2.
3
M3 = 2 Kg
4
M4 = 0.6 Kg Khối lượng thanh nối 4.
5
a1 = 0.25 m
Chiều dài thanh nối giữa 2 khớp main và fore .
6
a2 = 0.15 m
Chiều dài thanh nối giữa 2 khớp fore và cổ tay .
7
D3
Chiều dài thanh nối d3 phụ thuộc vào chế độ làm việc của tay máy.
Khối lượng thanh nối 3.
Sinh viên: Phạm Văn Khiền
7
Robot scara serpent
1.2.2. Giới hạn không gian làm việc của robot Scara Serpent
Các biến khớp có các giới hạn góc quay như sau :
1 = -96 0 960 ( so với trục Ox ).
2 = -115 0 1150 (so với trục thanh 1).
Chuyển động quay của khớp thứ nhất có hình chiếu bằng trong hệ trục toạ độ
OX0Y0 và OX1Y1 (Hình 1.5) tương ứng với góc quay tổng trong thực tế là 192 0.
Chuyển động quay của khớp thứ hai có hình chiếu bằng trong hệ trục toạ độ OX 1Y1 và
OX2Y2 tương ứng với góc quay tổng là 2300. Từ đó ta có thể thấy được hình chiếu giới
hạn không gian làm việc của nó (Hình 1.6).
Hình 1.5: Giới hạn góc quay của 2 khớp.
Sinh viên: Phạm Văn Khiền
8
Robot scara serpent
Hình 1.6: Giới hạn không gian làm việc của robot Scara Serpent.
Như vậy khoảng không gian mà tay máy có thể với tới là toàn bộ hình trụ với
đáy có đường giới hạn bên trong là một cung tròn có bán kính r = 0.231 (m) và
đường giới hạn bên ngoài là đường tròn bán kính R = 0.4 (m).
1.3. Động học robot Scara Serpent
Robot Scara Serpent có cấu trúc động học được biểu diễn như trên Hình 1.4.
Robot có 3 trục quay và 1 bàn kẹp, tuy nhiên ba khớp động đầu tiên được gọi là bộ
phận cơ bản vì trước hết, nhờ chúng tay máy có thể thực hiện bước chủ yếu trong thao
tác định vị, tức là đưa bàn kẹp đến lân cận điểm làm việc, sau đó nhờ khớp động còn
lại bàn kẹp được định hướng và vi chỉnh đến vị trí gia công chính xác.
1.3.1. Động học thuận
Việc xây dựng các phương trình động học thuận của robot được tiến hành tuần
tự theo các bước sau:
Sinh viên: Phạm Văn Khiền
9
Robot scara serpent
Bước 1: Xác định các hệ toạ độ
Ta sử dụng quy ước Denavit-Hartenberg để mô tả đầy đủ vị trí của của toàn
thân robot công nghiệp. Hình 1.4 mô tả các hệ trục toạ độ gắn với các khúc tay của
robot Scara Serpent.
Bước 2: Xây dựng bảng thông số DH
Bảng 1.3: Tham số Denavit – Hartenberg của robot Scara Serpent.
Thanh nối
i (0)
ai
i(rad)
di (m)
Biến
Chuyển động
1
0
a1
1
0
1
Quay
2
-1800
a2
2
0
2
Quay
3
0
0
0
d3
d3
Tịnh tiến
4
0
0
4
0
4
Quay
Khảo sát với 3 trục khớp quay đầu tiên tương ứng với quỹ đạo của khớp quay 4
trong mặt phẳng OX0Y0. Ma trận T4 là ma trận biểu diễn tay máy robot trong hệ trục
tọa độ gốc: T4= A1.A2.A3.A4
cos n
sin
n
An=
0
0
sin n cos n
cos n cos n
sin n
0
sin n sin n
cos n sin n
cos n
0
Thay số liệu trong bảng tham số có:
cos 1
sin
1
A1
0
0
sin 1 0 a 1.cos 1
cos 1 0 a1 sin 1
0
1
0
0
0
1
Sinh viên: Phạm Văn Khiền
10
a n cos n
a n sin n
dn
1
(1.1)
Robot scara serpent
cos 2
sin
2
A2
0
0
1
0
A3
0
0
sin 2
cos 2
0
0
a 2 .cos 2
0 a 2 sin 2
1
0
0
1
0
0
1 0 0
0 1 d3
0 0 1
0 0
cos 4
sin
4
A4
0
0
sin 4 0 0
cos 4 0 0
0
1 0
0
0 1
Ký hiệu: S1 Sin1 ; C1 Cos1
S2 Sin2 ; C2 Cos2
S4 Sin4 ; C4 Cos4
S12 Sin(1+2); C12 Cos(1+2)
Các bước tính toán:
3
Bước 1: T4 A 4
C4
S
3
T4 A 4 4
0
0
S4
C4
0
0
0 0
0 0
1 0
0 1
2
3
Bước 2: T4 A 3 . T4
1
0
2
T4
0
0
0 C 4
1 0 0 S4
0 1 d3 0
0 0 1 0
0 0
S4
C4
0
0
Sinh viên: Phạm Văn Khiền
11
0 0 C 4
0 0 S4
1 0 0
0 1 0
S4
C4
0
0
0
0 0
1 d3
0 1
0
Robot scara serpent
1
2
Bước 3: T4 A 2 . T4
0 a 2 .C 2 C 4 S4 0 0
0 a 2 .S2 S4 C4 0 0
1
0 0
0 1 d3
0
1 0
0 0 1
C 2C 4 S2S4 C 2S4 S2C 4 0 a 2 .C 2
S C C S S S C C
0 a 2 .S2
2
4
2
4
2
4
2
4
1
T4
0
0
1 d 3
0
0
0
1
C 2
S
1
T4 2
0
0
S2
C 2
0
0
Bước 4: T4 0 T4 A1 .1 T4
C1 -S1
S C
1
0
T4 1
0 0
0 0
0 a1.C1 C 2C 4 S2S4
0 a1.S1 S2C 4 C 2S4
1
0
0
0
1
0
C1 (C 2 C 4 S2S4 ) S1 (S2 C 4 C 2S4 )
S1 (C 2 C 4 S2S4 ) C1 (S2C 4 C 2S4 )
0
0
S2C 4 C 2S4
0 a 2 .C 2
(C 2C 4 S2S4 ) 0 a 2 .S2
0
1 d 3
0
0
1
C1 (S 2 C 4 C 2 S 4 ) S1 (C 2 C 4 S 2S 4 ) 0 a 2 C12 a1C1
S1 (S 2 C 4 C 2S 4 ) C1 (C 2 C 4 S 2S 4 ) 0 a 2S12 a1S1
0
1
d3
0
0
1
(1.2)
Ma trận 0T4 biểu diễn tay máy robot trong hệ toạ độ gốc.
Mặt khác theo ký hiệu tổng quát:
nx
n
TE T4 y
nz
ox
oy
oz
ax
ay
az
px
py
pz
0
0
0
1
(1.3)
Với: n , o, a , p lần lượt là các vectơ định vị, vectơ định hướng, vectơ tới và
vectơ vị trí để biểu diễn hướng và vị trí của tay máy trong không gian làm việc.
Sinh viên: Phạm Văn Khiền
12
Robot scara serpent
Từ ma trận trên ta có hệ phương trình động học thuận tay máy robot:
nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4)
(1.4)
ny = S1(C2 C4 + S2S4) + C1(S2C4 – C2S4)
(1.5)
nz = 0
(1.6)
ox = C1(S2C4- C2 S4) + S1(C2C4 + S2S4)
(1.7)
oy = S1(S2C4 - C2 S4) - C1(C2C4 + S2S4)
(1.8)
oz = 0
(1.9)
ax = 0
(1.10)
ay = 0
(1.11)
az = -1
(1.12)
Và hệ phương trình xác định vị trí của điểm tác động cuối như sau:
x = px = a1.C1 + a2.C12
(1.13)
y = py = a1.S1 + a2.S12
(1.14)
z = pz = - d3
(1.15)
1.3.2. Động học ngược
Động học ngược: xác định các biến khớp khi biết vị trí tay.
Từ phương trình động học thuận có:
p 2x a12 .C12 a 22 .C122 2.a1.a 2 .C1.C12
p 2y a12 .S12 a 22 .S122 2.a1.a 2 .S1.S12
p 2X p 2Y a12 .(S12 C12 ) a 22 .(S122 C122 ) 2.a1.a 2 .(S1.S12 C1.C12 )
p 2x p 2y a12 a 22 2.a1.a 2 .C 2
Do đó:
(1.16)
p 2X p 2Y a12 a 22
cosθ 2
2.a1.a 2
(1.17)
sinθ 2 (1 cos 2θ 2 )
Từ đó tính được góc 2:
2= atan2(sin2,cos2)
(1.18)
Sinh viên: Phạm Văn Khiền
13
Robot scara serpent
Thế C1, S1 vào phương trình (1.13) và (1.14) thu được:
(a1+a2C2).C1 – a2S2.S1 = px
a2S2.C1 +( a1+a2C2).S1 = py
Giải phương trình bậc nhất với ẩn C1, S1 và sử dụng (1.16) thu được :
C1
S1
x (a1 a 2C 2 ).p x a 2S2 .p y
p 2x p 2y
y
(a1 a 2C 2 ).p y a 2S2 .p x
(1.19)
p 2x p 2y
1= atan2(S1,C1)
(1.20)
Từ phương trình (1.15) ta có:
d3 = - pz
(1.21)
Mặt khác từ phương trình (1.4) có:
nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4)
(1.22)
Rút gọn theo các công thức lượng giác thu được:
nX = cos1.cos(2-4) – sin1.sin(2-4) = cos(1+2-4)
sin(1+2-4) =
1 n 2x
(1.23)
(1.24)
(θ1 θ 2 θ 4 ) atan2( 1 n 2x ,n x )
θ 4 θ1 θ 2 atan2( 1 n 2x ,n x )
(1.25)
Vậy hệ phương trình động học ngược của robot Scara Serpent là:
Sinh viên: Phạm Văn Khiền
14
Robot scara serpent
(a1 a 2C 2 ).p x a 2S2 .p y
C
1
2
2
p
p
x
y
(a a C ).p a S .p
S1 1 2 22 y 2 2 2 x
px py
1 atan2 S1 ,C1
2
2
2
2
cosθ p X p Y a1 a 2
2
2.a1.a 2
sinθ 2 (1 cos 2θ 2 )
2 atan2 S2 ,C2
d p
z
3
θ θ θ atan2( 1 n 2 ,n )
1
2
x
x
4
(1.26)
1.4. Động lực học robot Scara Serpent
Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và gia tốc của đối
tượng robot, cần phải xây dựng được phương trình động lực học, từ đó phục vụ cho
công việc thiết kế và điều khiển robot. Vì vậy cần phải tính toán đầy đủ các thông số
của đối tượng trước khi đưa vào mô phỏng.
Phương trình động lực học của robot được biểu diễn như sau:
&
& h(q,q)
& g(q) (t)
H(q)q
(1.27)
Trong đó:
-
(t) - Vectơ [n x 1] lực động tạo nên ở n khớp động:
(t) = [1(t), 2(t), ..., n(t)]T
-
q(t) - Vectơ [n x 1] biến khớp:
q(t) = [q1(t), q2(t), ... qn(t)]T.
-
(1.28)
(1.29)
H(q) - Ma trận [n x n], có các phần tử Hik sau đây:
H jk
n
i max( j,k )
Trace Uij Ii UikT . (j, k =1, 2, ...n).
Sinh viên: Phạm Văn Khiền
15
(1.30a)
Robot scara serpent
0Ti
U ij
q j
-
h( q ,
)
q
(1.30b)
- Vectơ [n x 1] lực ly tâm và Coriolit:
h( q ,
)
q
n
hj
= [h1, h2, ... , hn]T.
n
k 1 m 1
h jkm q&k q&m . (j =1, 2, ...n).
h jkm Trace U ikm I i U ijT .
(1.31)
(1.32)
(1.33)
0
U ikm
-
2 Ti
q k q m
(1.34)
g(q) - Vectơ [n x 1] lực trọng trường:
g(q) = [g1, g2, ... , gn]T.
(1.35)
g j m jgUiji r .
(1.36)
n
i j
Với robot Scara Serpent gồm 4 chuyển động (3 chuyển động quay và 1 chuyển
động tịnh tiến) và mô phỏng với 3 chuyển động quay có các phương trình cụ thể sau:
Vectơ [4 1] lực động:
(t) = [1(t), 2(t), 3(t), 4(t)]T.
Vectơ [41] biến khớp:
q(t) = [q1(t), q2(t), d3(t), q4(t)]T.
1.4.1. Hàm Euler - Lagrange và các vấn đề động lực học
Lagrange định nghĩa sự khác biệt giữa động năng và thế năng của hệ thống:
L=K–P
Trong đó:
(1.37)
K : là động năng của hệ thống.
P : là tổng thế năng của hệ thống.
Do đó phương trình động lực học được xác định bằng biểu thức:
i
Trong đó:
d L L
dt q&i q i
(1.38)
qi = i đối với khớp quay; qi = ri đối với khớp tịnh tiến.
Sinh viên: Phạm Văn Khiền
16
Robot scara serpent
i : mômen lực động tại khớp thứ i.
Động năng của khớp thứ i:
Ki
1
1
m i .v i2 J i .i2
2
2
(1.39)
Với Ji là mômen quán tính của khớp thứ i.
1.4.2. Động lực học robot Scara Serpent
Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 1.4:
Bảng 1.4: Thông số của robot Scara Serpent.
Khớp 1
Khớp 2
Khớp 3
Khớp 4
Biến
1
2
d3
4
Chiều dài
a1
a2
d3
0
Khối lượng
m1
m2
m3
m4
Vận tốc
v1
v2
v3
v4
Chiều dài tâm khối
lg1
lg2
lg3
lg4
Giả sử khối lượng nằm ở đầu mút các thanh nối, ta có chiều dài tâm khối chính
là chiều dài của thanh nối:
lg1 = a1
Ký hiệu chiều dài các khớp:
lg2 = a2
a 1 = l1
lg3 = d3
a 2 = l2
d 3 = l3
Hệ qui chiếu gắn với trục toạ độ (O 0x0y0z0) trên khớp thứ nhất. Khi đó mặt
phẳng (O0x0y0) là mặt phẳng đẳng thế.
1.4.2.1. Tính toán động năng và thế năng cho từng khớp
Khớp 1:
x1 lg1.cosθ1
y1 lg1.sinθ1
z1 0
Sinh viên: Phạm Văn Khiền
17
Robot scara serpent
x. l .sinθ .θ&
g1
1
1
1
.
y1 lg1.cosθ 1.θ&1
.
z1 0
Trong đó: x1, y1, z1 là hình chiếu của thanh nối số 1 lên các trục x, y, z.
v1 x&12 y&12 z&12
v1 ( lg1.sinθ 1 .θ&1 )2 (lg1 .cosθ 1 .θ&1 ) 2
2 &2
v12 lg1
.θ 1
Trong đó: J1 momen quán tính khớp 1.
P1 0
1
1
K1 m1.v12 J1.12
2
2
.
.
1
1
2
K1 m1.lg1
.θ12 J 1.θ12
2
2
(1.40)
Khớp 2:
x 2 l1 .cosθ1 l g2 .cos(θ1 θ 2 )
y 2 l1 .sinθ1 l g2 .sin(θ1 θ 2 )
z 2 0
x2 l1 .sinθ1 .θ1 l g2 .sin(θ1 θ 2 ).(θ1 θ2 )
y2 l1 .cosθ1 .θ1 l g2 .cos(θ1 θ 2 ).(θ1 θ2 )
z 2 0
Sinh viên: Phạm Văn Khiền
18
Robot scara serpent
v2
22 y
22 z
22
x
l .sin(θ θ ).(θ
θ
) 2 l .cos θ .θ
l .cos(θ θ ).(θ
θ
v 22 l1 .sin θ1 .θ
1
g2
1
2
1
2
1
1
1
g2
1
2
1
2 l 2 .(θ
θ
) 2 2.l .l .(θ
2 θ
.θ
)cosθ
v 2 l 2 .θ
2
1
1
g2
1
2
1
g2
1
1
2
2
1
1
K 2 m 2 .v 22 J 2 .22
2
2
.
.
1
2 2 2 & & 2
1
K 2 m 2 . l1 .θ1 lg2 .(θ1 θ 2 ) 2.l1.l g2 .(θ12 θ&1.θ&2 ).cosθ 2 J 2.θ&22
2
2
P2 0
(1.41)
Khớp 3:
x 3 l1.cosθ1 l 2 .cos(θ1 θ 2 )
y 3 l1.sinθ1 l 2 .sin(θ1 θ 2 )
z -l
3 3
x. l .sinθ .θ. l .sin(θ θ ).(θ. θ. )
1
1 1
2
1
2
1
2
3
.
.
.
.
y3 l1.cosθ 1.θ1 l 2 .cos(θ 1 θ 2 ).(θ1 θ 2 )
.
.
z 3 l3
v3
.
.
.
x 32 y 32 z 32
2
.
.
.
.
.
.
v 32 l1 .sinθ1 . θ1 l 2 .sin( θ1 θ 2 ).(θ1 θ 2 ) l1 .cosθ1 . θ1 l 2 .cos(θ1 θ 2 ).(θ1 θ 2 )
.
.
.
.
.
.
.
.
v 32 l12 . θ12 l 22 .(θ1 θ 2 ) 2 2.l1 .l 2 .(θ12 θ1 . θ 2 ) l 32
.
1
K 3 m3 .v32
2
.
.
.
.
.
.
1
.
K 3 m3 . l12 .θ12 l 22.(θ1 θ 2 ) 2 2.l1.l 2.(θ 12 θ1 .θ 2 ) l 32
2
P3 m3 .g.h 3 P3 m3 .g.l3
Khớp 4:
Sinh viên: Phạm Văn Khiền
19
(1.42)
Robot scara serpent
.
.
.
.
x
l
.sin
θ
.
θ
l
.sin(
θ
θ
).(
θ
θ
1
1 1
2
1
2
1
2)
4
.
.
.
.
y
l
.cos
θ
.
θ
l
.cos(
θ
θ
).(
θ
θ
4 1
1 1
2
1
2
1
2)
.
z 4
l3
x 4 l1 .cosθ1 l 2 .cos(θ1 θ 2 )
y 4 l1 .sinθ1 l 2 .sin(θ1 θ 2 )
z -l
4 3
.
.
.
v 4 x 42 y 42 z 42
2
2
.
.
.
.
.
.
v l1.sinθ 1.θ1 l2 .sin(θ 1 θ 2 ).(θ1 θ 2 ) l1.cosθ 1.θ1 l 2 .cos(θ 1 θ 2 ).(θ 1 θ 2 ) l 32
2
4
.
.
.
.
.
.
.
.
v 24 l12 .θ 12 l 22.(θ 1 θ 2 ) 2 2.l 1.l 2.(θ 12 θ 1 .θ 2 ) l 32
.
1
1
K 4 m 4 .v 42 J 4 .w 42
2
2
.
.
.
.
.
.
.
.
.
1 .
1
K 4 m 4 . l12 .θ12 l g22 .(θ1 θ 2 ) 2 2.l1.l g2.(θ 12 θ1 .θ 2 )cosθ 2 l 32 J 4 (θ 1 θ 2 θ 4 ) 2
2
2
P4 m 4 .g.h 4
2.43
P4 m 4 .g.l3
Tổng động năng của hệ thống:
K = K1 + K2 + K3 + K4
(1.44)
Đặt tâm khối tại đầu mút của thanh nối, ta có: lg1= l1; lg2= l2; lg3= l3
.
1
(m1 m 2 m 3 m 4 ).l12 (m 2 m 3 m 4 ).l 22 J1 J 2 J 4 .θ 12 ...
2
(m 2 m 3 m 4 ).l1 .l2 .cosθ 2 .θ&12 (m2 m3 m 4 )l22 J 2 J 4 .θ&1 .θ&2 ...
K
1
(m 2 m 3 m 4 ).l1 .l2 .cosθ 2 .θ&1 .θ&2 (m 2 m3 m 4 ).l32 J 2 J 4 .θ&22 ...
2
1
1
(m3 m 4 ).&
l32 .J 4 .θ&42 J 4 .θ&1.θ&4 J 4 .θ&2 .θ&4
2
2
(1.45)
Tổng thế năng của hệ thống:
P = P1+ P2+ P3+ P4 = -(m3+ m4).g.l3
(1.46)
1.4.2.2. Phương trình động lực học
Lagrange của hệ thống:
Sinh viên: Phạm Văn Khiền
20
.
- Xem thêm -