BÀI TẬP

MÔN HỌC ROBOT CÔNG NGHIỆP

Số:
Họ và tên sinh viên: Trần Mạch Tuấn Kiệt
Lớp: K13 – Tự động hóa Khóa: 13 Khoa: Điện – Điện
Giảng viên hướng dẫn: Khổng Minh

NỘI DUNG
1. Tên đề tài: Mô hình hóa, mô phỏng robot công nghiệp SCARA 4 bậc tự do

2. Yêu cầu báo cáo:

- Quyển: 01 quyển báo cáo A4

- File: 01 file m tính toán gửi vào email: minh.khong@phenikaa-uni.edu.vn

- Định dạng: Hovaten_mssv.m

Ngày giao đề tài: 18/11/2022 Ngày hoàn thành: 28/11/2022

GIÁO VIÊN HƯỚNG DẪN

Khổng Minh
 NỘI DUNG

A. Mô tả hoạt động của robot SCARA

Robot công nghiệp SCARA vận chuyển phôi giữa hai băng tải. Thời gian
chuyển phôi từ giữa hai băng tải là t (chưa kể thời gian đóng, mở tay kẹp). Khoảng
cách giữa vị trí đầu và cuối của phôi lần lượt là dx ,dy, dz; tương quan về góc giữa
vị trí đầu và vị trí cuối của phôi là anpha (hình vẽ).

B. Yêu cầu
1. Tính toán kích thước các khâu của tay máy và lập sơ đồ động học
Vị trí đặt robot ta có thể chọn chính giữa 2 băng tải và ở trung điểm 2 phôi đầu
và cuối, từ đó giúp cho robot dễ chuyển hơn và các khớp tay có thể ngắn hơn
như hình 1 mô tả. Từ đó ta có thể xác định ra vị trí điểm đầu và điểm cuối của
phôi lần lượt là A(−210 ,210 ,−10 0) và B(210 ,−210 ,−5 20)
Để cánh tay robot có thể hoạt động trên các điểm A và B ta có
l 1 +l 2 ≥ √210 + 210 =296.98 , ta có thể chọn l 1 +l 2=300=¿ l 1=150 , l 2=150 , l 3=50 mm
2 2

Chiều dài tối thiểu của khâu thứ 3 là q 3 ≥ d z=420 từ đó ta chọn chiều cao Robot
có thể l0=800mm
Hình 1 Vị trí đặt cánh tay robot (chấm xanh) so với băng chuyển và vị trí phôi đầu và cuối
L1 L2

L3
L0

2. Tính các ma trận truyền DH, thiết lập phương trình động học tay máy
Bảng D – H

Khâu di θi ai αi

1 0 q1 L1 = 150 0

2 0 q2 L2 = 150 π
 3 q3 0 0 0

4 L3 q4 0 0

Từ bảng D – H ta có thể lập ra các ma trận truyền cho từng hàng của bảng D – H

[ ][ ]
cos ⁡(q 1)−sin (q 1) 0 L 1cos (q 1) cos ⁡(q 2) −sin(q 2) 0 L 2 cos(q 2)
0 sin(q 1) cos (q 1) 0 L 1 sin(q 1) 1 sin(q 2) cos( q 2) 0 L2 sin(q 2)
H 1= H 2=
0 0 1 0 0 0 −1 0
0 0 0 1 0 0 0 1

[ ][ ]
100 0 cos ⁡(q 4) −sin( q 4) 0 0
2 010 0 3 sin( q 4) cos(q 4) 0 0
H 3= H4=
0 0 1 q3 0 0 1 l3
000 1 0 0 01

Từ các ma trận truyền H i ta tính ra được ma trận truyền tổng

0 0 1 2 3
H 4 =H 1 × H 2 × H 3 × H 4

[ ]
cos (q 1+q 2−q 4) sin(q 1+q 2−q 4) 0 l 2 cos( q 1+ q 2)+l 1 cos(q 1)
0 sin (q 1+q 2−q 4 ) −cos(q 1+ q 2−q 4) 0 l2 sin( q 1+ q 2)+l 1 sin(q 1)
H4=
0 0 −1 −q 3−L 3
0 0 0 1

Từ ma trận truyền D – H ta có thể xác định phương trình động học tay máy xác
định vị trí khâu tác động cuối như sau

{
x 4=l 2× cos ( q 1+q 2 )+l 1 ×cos ⁡(q 1)
y 4=l 2× sin ( q 1+q 2 ) +l 1× sin ⁡(q 1)
z 4=−q 3−L 3

3. Xây dựng bài toán động học ngược

Dựa vào bài toán động học thuận ta có:

{
x=l2 ×cos ( q 1+q 2 ) +l 1× cos ⁡( q 1)
y =l 2 ×sin ( q 1+ q 2 )+ l1 ×sin ⁡(q 1)
z =−q 3−L3
Suy ra:

{
2
x =( l 1 cos ( q 1) ) + [ l 2 cos ( q1 +q 2 ) ] +2l 1 cos ( q 1 ) l 2 cos ( q 1+ q2 )
2 2

2 2 2
y =(l 1 sin ( q 1) ) +[l 2 sin ⁡(q 1+ q2 )] + 2l 1 sin ⁡(q 1)l 2 sin ⁡(q 1+ q2)

Từ đó ta suy ra
2 2 2 2
 x + y =( l1 ) + ( l2 ) + 2l1 l2 cos ( q2 )
x 2 + y 2−( l1 )2 −( l 2 )2
 cos ( q 2 )=
2 l 1 l2

 q 2=arccos ( x 2+ y 2−( l 1 )2−( l 2 )2

2l 1 l 2 )
Xét trên mặt phẳng x 0 y 0 ta có:

Theo hình trên ta có:

q 1=q 12−q21=arctan ()y

x
−arctan ⁡(
l 2 sin ⁡(q2 )
l 1 +l 2 cos ⁡(q 2)
)

Xét phép quay zxz ta có ma trận:

R zxz=R z ( α ) R x ( β ) R z ( γ )=¿
[ ]
cos ⁡(α )cos ⁡(γ )−cos ⁡(β) sin ⁡(α )sin ⁡(γ) −cos ⁡(α) sin ⁡( γ )−cos ⁡(β )cos ⁡( γ )cos ⁡( α ) sin ⁡(α )sin ⁡(β)
sin ⁡( α )cos ⁡( γ )+cos ⁡(α )cos ⁡( β) sin ⁡(γ ) −sin ⁡(α) sin ⁡( γ )+cos ⁡(β) cos ⁡( γ )cos ⁡(α ) −cos ⁡(α )sin ⁡( β)
sin ⁡( β)sin ⁡(γ ) cos ⁡( γ )sin ⁡( β ) cos ⁡(β)

Từ ma trận H 04 ta có:

[ ]
cos ⁡( q1 +q 2−q 4 ) sin ⁡(q1 +q 2−q 4 ) 0
R zxz= sin ⁡(q1 +q 2−q 4) −cos ⁡(q 1+ q2−q4 ) 0
0 0 −1

→
{ cos ⁡(β )=−1
cos ⁡( α )cos ⁡(γ )−cos ⁡(β )sin ⁡(α )sin ⁡( γ)=cos ⁡(q1 +q2 −q 4)

→
{ β=π
cos ⁡(α−γ )=cos ⁡( q1 +q2 −q 4)

→ q4 =q 1+ q2 +γ −α

Như vậy ta có hệ phương trình động học ngược tay máy:

{
( yx )−arctan ⁡( l +l cos ⁡( q ) )
l 2 sin ⁡(q 2)
q1=arctan
1 2 2
2 2 2 2
x + y −( l 1 ) −( l 2 )
q 2=arccos ⁡( )
2l 1 l 2
q3 =−z−l 3
q 4=q1 +q2 + γ −α

Tại thời diểm t=0 cánh tay đang ở vị trí A (−210 , 210 ,−100 ) , alpha a=γ −α =90 ° ta
thay tọa độ điểm A vào phương trình động học ngược ta có:

{
q1A =−53.13 °
A
q2 =16.26 °
q 3A=50
q A4 =53.13 °

Tại thời điểm t = 9.6s cánh tay đang ở vị trí B ( 210 ,−210 ,−520 ) , alph ab=γ −α =32 ° ta
thay tọa độ điểm B vào phương trình động học ngược ta có:
 {
q1B=126.87 °
q2B=16.26∘
B
q3 =470
q B4 =175.13∘

4. Xây dựng ma trận Jacobi, ma trận Jacobi nghịch đảo, xác định vận tốc góc
các khớp q1, q2, q3 khi cho trước vận tốc dài của các khâu tác động cuối
vx , v y , vz

Từ phương trình động học thuận ta có ma trận Jacobi được tính theo công thức sau:

[ ]
∂ xp ∂ xp ∂ xp
∂q1 ∂q2 ∂q3
∂ yp ∂ yp ∂ yp
J ( q) =
∂q1 ∂q2 ∂q3
∂ zp ∂ zp ∂ zp
∂q1 ∂q2 ∂q3

[ ]
−l 1 sin ( q1 ) −l 2 sin ( q 1+ q2 ) −l 2 sin ( q 1+ q2 ) 0
J ( q ) = l 1 cos ( q1 ) +l 2 cos ( q 1+ q2 ) l 2 cos ( q 1+ q2 ) 0
0 0 −1

Ma trận Jacobi nghịch đảo là

[ ]
l 2 cos ( q 1+ q2 ) l 2 sin ( q1 +q 2 ) 0
−1 1
J ( q) = −l 1 cos ( q 1 )−l 2 cos ( q 1+ q2 ) −l 1 sin ( q 1 )−l 2 sin ( q1 +q 2 ) 0
l 1 l 2 sin ( q 2 )
0 0 −1

Vận tốc góc của các khớp q1, q2, q3 được tính là

[] []
q̇1 x˙ p
−1
q̇2 =J ( q ) y˙ p
q̇3 z˙p

[] [ ][ ]
q̇1 l 2 cos ( q 1+ q2 ) l 2 sin ( q 1+ q2 ) 0 x˙ p
1
q̇2 = −l1 cos ( q1 )−l 2 cos ( q 1+ q2 ) −l 1 sin ( q 1 )−l 2 sin ( q1 +q 2 ) 0 y˙ p
l 1 l 2 sin ( q 2 )
q̇3 0 0 −1 z˙p
 [ ]
x˙p cos ( q 1+ q2 ) + y˙ p sin ( q 1+ q2 )

[]
q̇1 l 1 sin ( q2 )
q̇2 = − x˙ p l 1 cos ( q 1) + ẏ p l 1 sin ( q1 ) + x˙p l 2 cos ( q1 +q 2) + ẏ p l 2 sin ( q1 +q 2 ) (1)
q̇3 l1 l 2 sin ( q2 )
−z .p

[ ] [ ]( ) [ ]
x˙p 1.8 1800 mm
m
Với y˙ p = 1.8 s = 1800 ( s ) ta thay vào phương trình (1)
z˙p 1.8 1800
Ta có:
Vận tốc góc của các khớp q 1 , q 2 , q 3 tại vị trí A là:

[ ][ ]
¿ q̇1 8.571
¿ q̇2 = −3.9
¿ q̇3 −1800

Vận tốc góc của các khớp q 1 , q 2 , q 3 tại vị trí B là:

[ ][ ]
¿ q̇1 8.571
¿ q̇2 = −3.9
¿ q̇3 −1800

5. Tìm quy luật chuyển động của các khớp q1, q2 khi cho trước các vị trí khâu
tác động cuối
Ta có 6 điều kiện biên là vị trí, vận tốc và gia tốc ở hai điểm đầu vào cuối nên chọn
quỹ đạo chuyển động là một đa thức bậc 5 có dạng:

{
5 4 3 2
q ( t )=a5 t + a4 t +a 3 t + a2 t +a 1 t+ a0
4 3 2
q̇(t )=5 a5 t +4 a4 t + 3 a3 t +2 a2 t
3 2
q̈ ( t )=20 a 5 t + 12a 4 t +6 a3 t +2 a2

Ta tính quy luật chuyển động của từng khớp:

- Khớp q1
Tại thời diểm t=0 cánh tay đang ở vị trí A (−210 , 210 ,210 ) ta có:

{
q01 =−53.13 °
q02 =16.26 °
 {
q01 =a0 =−53.13
 q̇10=a1=0
0
q̈1 =2 ×a2 =190

{
a0 =−53.13
 a1=0
a2=95

Tại thời điểm t = 9.6s cánh tay đang ở vị trí B ( 210 ,−210 ,−210 ) ta có:

{
qt1 =126.87°
qt2 =16.26°

{
qt1 =t 5 a5 +t 4 a 4 +t 3 a3 +t 2 a2 +t a 1+ a0=126.87
 q̇ t1=5 t 4 a5 +4 t 3 a4 + 3t 2 a3 +2 t a2 +a 1=0
t 3 2
q̈ 1=20 t a5 +12 t a 4 +6 t a 3+ 2a 2=−190

{
a5=−0.205
 a4=4.93
a3=−38.14

Quy luật chuyển động của q 1 ( t ) là

{
q1 ( t ) =−0. 205× t 5 + 4.93 ×t 4−38.14 × t 3 + 95× t 2 −53.13
4 3 2
q̇1 ( t )=−1. 025× t +19.72× t −114.42×t +190 ×t
q̈1 ( t )=−4 ,1 ×t 3+ 59.16× t 2−228.84 ×t +190

- Khớp q2
Tại thời điểm t=0

{
q02 =a0 =16.26
 q̇02 =a1=0
0
q̈2 =2× a2=190

{
a0 =16.26
 a1=0
a2=95

Tại thời điểm t =9.6 s

 {
qt2 =t 5 a5 +t 4 a 4 +t 3 a3 +t 2 a2 +t a 1+ a0=16.26
 q̇ t2=5 t 4 a5 +4 t 3 a4 + 3t 2 a3 +2 t a2 +a 1=0
t 3 2
q̈ 2=20 t a5 +12 t a 4 +6 t a 3+ 2 a2=−190

{
a5=−0.214
 a4 =5.154
a3=−39.583

Quy luật chuyển động của q 2 ( t ) là

{
q2 ( t ) =−0.214 × t 5 +5.154 × t 4 −39.583 ×t 3+ 95 ×t 2+16.26
4 3 2
q̇ 2 ( t )=−1.07 × t + 20.616 ×t −118.748 ×t +190 × t
q̈2 ( t ) =−4,28 ×t 3 +61.848 ×t 2−237.498 ×t+ 190

6. Xây dựng bài toán động lực học thuận, động lực học ngược tay máy,
tính momen cần thiết tại các khớp q1 và q2