ĐẠI HỌC ĐÀ NẴNG
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA ĐIỆN

BÀI TẬP MÔN
HỌC PHẦN: KỸ THUẬT ROBOT
NGÀNH: KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HOÁ

GVHD: PGS.TS LÊ TIẾN DŨNG
SVTH : NGUYỄN QUANG REN
MSSV: 105190259
LỚP SH: 19TDHCLC2
NHÓM : 19NH33

Đà Nẵng, tháng 5/2022


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

MỤC LỤC
PHẦN I.

TÍNH TỐN ĐỘNG HỌC: ........................................................................3

1.1

Thực hiện các u cầu đề bài: ............................................................................3

1.2

Viết các phương trình phân tích giải bài tốn động học ngược: ........................8

1.3

Thay số cụ thể để kiểm tra lại bài toán động học thuận và động học ngược: ....9

PHẦN II.

MÔ PHỎNG: ............................................................................................ 11

2.1

Các bước thực hiện: .........................................................................................11

2.2

Kiểm nghiệm:...................................................................................................17

2.3

Kết quả mô phỏng: ...........................................................................................19

2.4

Nhận xét: ..........................................................................................................22

SVTH: NGUYỄN QUANG REN

TRANG 1



BÀI TẬP KỸ THUẬT ROBOT

GVHD: PGS.TS LÊ TIẾN DŨNG

DANH MỤC HÌNH ẢNH
Hình 1. 1 Tay máy robot 3 bậc tự do. ..............................................................................3
Hình 1. 2 Xác định đánh số thanh (link) và khớp (joint). ...............................................3
Hình 1. 3 Gắn trục zi dọc theo trục khớp thứ i. ............................................................... 3
Hình 1. 4 Kẻ đường vng góc chung giữa 2 trục tọa độ zi và zi + 1. ...........................4
Hình 1. 5 Gắn trục xi dọc theo đường vng góc chung giữa hai trục. ..........................4
Hình 1. 6 Trục tọa độ yi được xác định theo quy tắc bàn tay phải..................................4
Hình 1. 7 Gắn hệ tọa độ {0} trùng với hệ tọa độ {1} khi biến khớp đầu tiên bằng 0. ....5
Hình 1. 8 Gắn chi tiết các trục tọa độ. .............................................................................6
Hình 2. 1 Sơ đồ tổng qt mơ phỏng. ............................................................................11
Hình 2. 2 Gọi và cài đặt các khối tọa độ gốc................................................................. 11
Hình 2. 3 Cài đặt gia tốc trọng trường. ..........................................................................11
Hình 2. 4 Gọi khối tạo hình và gắn trục đất cho thanh Base. ........................................12
Hình 2. 5 Gọi khối trục xoay 1 (Joint 1) để liên kết các thanh Link 0 và Link 1..........12
Hình 2. 6 Gọi khối tạo hình và gắn trục cho thanh link 1. ............................................12
Hình 2. 7 Gọi khối trục tịnh tiến 1 Joint 1 để liên kết các thanh Link 1 và Link 2. ......13
Hình 2. 8 Gọi khối tạo hình và gắn trục cho thanh Link 2. ...........................................13
Hình 2. 9 Gọi khối trục xoay Joint 3 để liên kết các thanh Link 2 và Link 3. ..............13
Hình 2. 10 Gọi khối tạo hình và gắn trục cho thanh Link 3. .........................................14
Hình 2. 11 Gọi khối tạo hình và gắn trục cho End effector. .........................................14
Hình 2. 12 Lấy số liệu và hiển thị. ................................................................................14
Hình 2. 13 Thư viện Simulink ta vào mục User – Defined Function............................15
Hình 2. 14 Chương trình động học thuận . ....................................................................15
Hình 2. 15 Chương trình động học ngược. ....................................................................16

Hình 2. 16 Phương trình chuyển động đường trịn. .......................................................16
Hình 2. 17 Phương trình chuyển động đường thẳng. ....................................................16
Hình 2. 18 Liên kết các khối với nhau. .........................................................................16
Hình 2. 19 Tọa độ X, Y, Z trong phần cơ khí. .............................................................. 17
Hình 2. 20 Tọa độ X, Y, Z trong phần động học thuận. ................................................17
Hình 2. 21 Giá trị Theta 1 và d2 trong phần cơ khí.......................................................18
Hình 2. 22 Giá trị Theta 1 và d2 trong phần động học ngược. ......................................18
Hình 2. 23 Mơ hình 3D phần cơ khí. .............................................................................19
Hình 2. 24 Đồ thị XZ setpoint. ......................................................................................19
Hình 2. 25 Đồ thị XZ cơ khí. .........................................................................................20
Hình 2. 26 Đồ thị XZ của khối động học thuận. ...........................................................20
Hình 2. 27 Đồ thị XZ setpoint. ......................................................................................21
Hình 2. 28 Đồ thị XZ cơ khí. .........................................................................................21
Hình 2. 29 Đồ thị XZ của khối động học thuận. ...........................................................22
SVTH: NGUYỄN QUANG REN

TRANG 2


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

BÀI TẬP SỐ 5
Cho tay máy robot 3 bậc tự do như Hình 1.1:

Hình 1. 1 Tay máy robot 3 bậc tự do.
PHẦN I. TÍNH TỐN ĐỘNG HỌC:
1.1 Thực hiện các u cầu đề bài:
➢ Gắn hệ tọa độ lên các khâu của tay máy robot và lập bảng tham số Denavit –

Hartenberg (DH) theo phương pháp Denavit – Hartenberg sửa đổi:
-

Bước 0: Xác định đánh số thanh (link) và khớp (joint)

Link 1

Joint 0

Link 2

Link 3

Link 0 (base )

Hình 1. 2 Xác định đánh số thanh (link) và khớp (joint).
-

Bước 1: Gắn trục 𝑧̂𝑖 dọc theo trục khớp thứ i
𝑧̂1

𝑂1

𝑂2

𝑧̂2

𝑂3

𝑧̂3


𝑧̂0

𝑂0

Hình 1. 3 Gắn trục 𝑧̂𝑖 dọc theo trục khớp thứ i.
SVTH: NGUYỄN QUANG REN

TRANG 3


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

-

Bước 2: Kẻ đường vng góc chung giữa 2 trục tọa độ 𝑧̂𝑖 và 𝑧̂𝑖+1 . Gốc Oi của
hệ tọa độ {i} được đặt tại điểm giao nhau của đường vng góc chung ai với
trục tọa độ 𝑧̂𝑖 .
𝑧̂1
𝑂1

𝑂2

𝑧̂3

𝑧̂2 𝑂3

𝑧̂0


𝐿1

𝐿2
𝑂0

Hình 1. 4 Kẻ đường vng góc chung giữa 2 trục tọa độ 𝑧̂𝑖 và 𝑧̂𝑖+1 .
-

Bước 3: Gắn trục 𝑥̂𝑖 dọc theo đường vng góc chung giữa hai trục, hướng từ
trục khớp i đến i + 1. Nếu trường hợp khớp i và i +1 cắt nhau thì trục 𝑥̂𝑖 vng
góc với mặt phẳng chứa hai trục khớp i và i +1.
𝑧̂1

𝑥̂1

𝑥̂3

𝑥̂2
𝑂3

𝑂1

𝑂2
𝑧̂0

𝑧̂3

𝑧̂2
𝐿2


𝐿1
𝑥̂0
𝑂0

Hình 1. 5 Gắn trục 𝑥̂𝑖 dọc theo đường vng góc chung giữa hai trục.
-

Bước 4: Trục tọa độ 𝑦̂𝑖 được xác định theo quy tắc bàn tay phải
𝑦̂2

𝑧̂1
𝑦̂1

𝑥̂1

𝑦̂3
𝑥̂2

𝑥̂3
𝑂3

𝑂1
𝑧̂ 0

𝑧̂2

𝑂2

𝑧̂3


𝐿2

𝐿1
𝑦̂0

𝑥̂0

𝑂0

Hình 1. 6 Trục tọa độ 𝑦̂𝑖 được xác định theo quy tắc bàn tay phải.

SVTH: NGUYỄN QUANG REN

TRANG 4


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

-

Bước 5: Gắn hệ tọa độ {0} trùng với hệ tọa độ {1} khi biến khớp đầu tiên bằng
0.
𝑑2

𝑧̂1

𝑦̂2

𝑥̂1

𝑦̂1

𝑂1 ≡ 𝑂0

𝑦̂3
𝑥̂2

𝑥̂3

𝑂2 𝑧̂2

𝑂3

𝐿2

𝜃3

𝑧̂3

𝜃1

Hình 1. 7 Gắn hệ tọa độ {0} trùng với hệ tọa độ {1} khi biến khớp đầu tiên bằng 0.
➢ Sau khi gắn hệ trục tọa độ lên các khâu của tay máy ta thành lập bảng tham số
Denavit – Hartenberg (DH):
Bảng 1. 1 Tham số Denavit – Hartenberg (DH).
i

𝛼𝑖−1


𝑎𝑖−1

𝑑𝑖

𝜃𝑖

1

0

0

0

𝜃1

2

90

0

𝑑2

0

3

0


0

𝐿2

𝜃3

➢ Từ kết quả bảng tham số DH, hãy xác định các ma trận biến đổi
0 1 2 0
1𝑇, 2𝑇, 3𝑇, 3𝑇:

-

Ta có cơng thức tổng qt sau:
𝑐𝜃𝑖
−𝑠𝜃𝑖
𝑠𝜃𝑖 𝑐𝛼𝑖−1 𝑐𝜃𝑖 𝑐𝛼𝑖−1
𝑖−1
𝑖𝑇 = [
𝑠𝜃𝑖 𝑠𝛼𝑖−1 𝑐𝜃𝑖 𝑠𝛼𝑖−1
0
0
- Từ công thức tổng quát ta đi tính:
𝑐𝜃1 −𝑠𝜃1 0 0
𝑠𝜃
𝑐𝜃1 0 0
]
+ 01𝑇 = [ 1
0
0

1 0
0
0
0 1
1 0 0
0
0 0 −1 −𝑑2
]
+ 12𝑇 = [
0 1 0
0
0 0 0
1
𝑐𝜃3 −𝑠𝜃3 0 0
𝑠𝜃
𝑐𝜃3 0 0
]
+ 23𝑇 = [ 3
0
0
1 𝐿2
0
0
0 1
SVTH: NGUYỄN QUANG REN

0
−𝑠𝛼𝑖−1
𝑐𝛼𝑖−1
0


𝛼𝑖−1
−𝑠𝛼𝑖−1 − 𝑑𝑖
]
𝑐𝛼𝑖−1 − 𝑑𝑖
1

TRANG 5


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

Từ kết quả trên ta tính được:
𝑐𝜃1 . 𝑐𝜃3 −𝑐𝜃1 . 𝑠𝜃3 𝑠𝜃1
(𝐿2 + 𝑑2 )𝑠𝜃1
𝑠𝜃1 . 𝑐𝜃3 −𝑠𝜃1 𝑠𝜃3 −𝑐𝜃1 −(𝐿2 + 𝑑2 )𝑐𝜃1
0
0 1 2
]
3𝑇 = 1𝑇. 2𝑇. 3𝑇 = [
𝑠𝜃3
𝑐𝜃3
0
0
0
0
0
1

𝑤
➢ Xác định các ma trận biến đổi: Ma trận 0𝑇 biến đổi giữa hệ tọa độ {0} và hệ tọa
-

độ {W}; ma trận 3𝑒𝑇 biến đổi giữa hệ tọa độ {3} và hệ tọa độ {e}:
𝑑2
𝑦̂2

𝑧̂1

𝑥̂1

𝑥̂2

𝑂1 ≡ 𝑂0

𝑥̂3

𝑧̂2

𝑂3
𝑧̂3

𝑂2

𝑦̂1
𝐿1

𝐿2


𝑦̂𝑤

𝑂𝑒

𝑥̂𝑒

𝜃3 𝑧̂𝑒

𝜃1

𝑂𝑤

𝑦̂𝑒

𝑦̂3

𝐿3
𝑥̂𝑤

𝑧̂𝑤

Hình 1. 8 Gắn chi tiết các trục tọa độ.
-

Ta có cộng thức tổng quát sau:
𝐴
𝐵𝑇

+


𝑊
0𝑇

=[

𝑊
0𝑅

0 0 0

Tính 𝑊0𝑅?
o 𝑊0𝑅
o

o

𝑊

=[

𝐴
𝐵𝑅

𝐴

0 0 0

𝑃𝐵𝑂𝑅𝐺 ]
1


𝑃0_𝑂𝑅𝐺
]
1

= [ 𝑊𝑋̂0 𝑊𝑌̂0 𝑊𝑍̂0 ]
𝑋̂0 . 𝑋̂𝑊 𝑌̂0 . 𝑋̂𝑊 𝑍̂0 . 𝑋̂𝑊
= [ 𝑋̂0 . 𝑌̂𝑊 𝑌̂0 . 𝑌̂𝑊 𝑍̂0 . 𝑌̂𝑊 ]
𝑋̂0 . 𝑍̂𝑊 𝑌̂0 . 𝑍̂𝑊 𝑍̂0 . 𝑍̂𝑊
cos(𝑋̂0 , 𝑋̂𝑊 )
= [ cos(𝑋̂0 , 𝑌̂𝑊 )
cos(𝑋̂0 , 𝑍̂𝑊 )

o

cos(90)
= [ cos(90)
cos(180)

o

0
= [0
−1

−1
0
0

SVTH: NGUYỄN QUANG REN


cos(𝑌̂0 , 𝑋̂𝑊 )
cos(𝑌̂0 , 𝑌̂𝑊 )

cos(𝑍̂0 , 𝑋̂𝑊 )
cos(𝑍̂0 , 𝑌̂𝑊 ) ]

cos(𝑌̂0 , 𝑍̂𝑊 )

cos(𝑍̂0 , 𝑍̂𝑊 )

cos(180)
cos(90)
cos(90)

cos(90)
cos(0) ]
cos(90)

0
1]
0

TRANG 6


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

Tính 𝑊𝑃0_𝑂𝑅𝐺 ?

0
o
𝑃0_𝑂𝑅𝐺 = [𝐿1 ]
0
𝑊
Vậy ta tính được 0𝑇:
0 −1 0 0
0
0 1 𝐿1
]
o 𝑊0𝑇 = [
−1 0 0 0
0
0 0 1
3
3
𝑅
𝑃𝑒_𝑂𝑅𝐺
]
+ 3𝑒𝑇 = [ 𝑒
0 0 0
1
Tính 3𝑒𝑅?
o 𝑊0𝑅 = [ 3𝑋̂𝑒 3𝑌̂𝑒 3𝑍̂𝑒 ]
𝑋̂𝑒 . 𝑋̂3 𝑌̂𝑒 . 𝑋̂3 𝑍̂𝑒 . 𝑋̂3
o
= [ 𝑋̂𝑒 . 𝑌̂3 𝑌̂𝑒 . 𝑌̂3 𝑍̂𝑒 . 𝑌̂3 ]
𝑋̂𝑒 . 𝑍̂3 𝑌̂𝑒 . 𝑍̂3 𝑍̂𝑒 . 𝑍̂3
𝑊


cos(𝑋̂𝑒 , 𝑋̂3 )
= [ cos(𝑋̂𝑒 , 𝑌̂3 )
cos(𝑋̂𝑒 , 𝑍̂3 )

o

cos(90)
= [cos(90)
cos(0)

o

0
= [0
1
3
Tính 𝑃𝑒_𝑂𝑅𝐺 ?

0
1
0

o

o

3

𝑃𝑒_𝑂𝑅𝐺


cos(𝑌̂𝑒 , 𝑋̂3 )
cos(𝑌̂𝑒 , 𝑌̂3 )

cos(𝑍̂𝑒 , 𝑋̂3 )
cos(𝑍̂𝑒 , 𝑌̂3 ) ]

cos(𝑌̂𝑒 , 𝑍̂3 )

cos(𝑍̂𝑒 , 𝑍̂3 )

cos(90)
cos(0)
cos(90)

cos(180)
cos(90) ]
cos(90)

−1
0]
0

0
= [0]
𝐿3

Vậy ta tính được
0
0
o 3𝑒𝑇 = [

1
0

3
𝑒𝑇:

0
1
0
0

SVTH: NGUYỄN QUANG REN

−1
0
0
0

0
0
]
𝐿3
1

TRANG 7


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT


➢ Từ các kết quả trên, xác định được ma trận 𝑤𝑒𝑇 và viết các phương trình động
học thuận tính tốn các tọa độ 𝑋𝑒 , 𝑌𝑒 , 𝑍𝑒 của khâu chấp hành cuối mô tả trong hệ
tọa độ {W} theo các biến khớp 𝑞1 , 𝑞2 , 𝑞3 :
-

Ta có:

= 𝑤0𝑇. 03𝑇. 3𝑒𝑇
(𝐿2 + 𝑑2 )𝑠𝜃1
𝑐𝜃1 . 𝑐𝜃3 −𝑐𝜃1 . 𝑠𝜃3 𝑠𝜃1
0 −1 0 0
0
0 1 𝐿1
(
)
] × [𝑠𝜃1 . 𝑐𝜃3 −𝑠𝜃1 𝑠𝜃3 −𝑐𝜃1 − 𝐿2 + 𝑑2 𝑐𝜃1 ] × 3𝑒𝑇
=[
−1 0 0 0
𝑠𝜃3
𝑐𝜃3
0
0
0
0 0 1
0
0
0
1
(𝐿2 + 𝑑2 )𝑐𝜃1

−𝑠𝜃1 . 𝑐𝜃3 𝑠𝜃1 𝑠𝜃3
𝑐𝜃1
0 0 −1 0
0 1 0
0
𝑠𝜃3
𝑐𝜃3
0
𝐿1
]×[
]
=[
1 0 0 𝐿3
−𝑐𝜃1 . 𝑐𝜃3 𝑐𝜃1 . 𝑠𝜃3 −𝑠𝜃1 −(𝐿2 + 𝑑2 )𝑠𝜃1
0 0 0
1
0
0
0
1
𝑐𝜃1
𝑠𝜃1 𝑠𝜃3 𝑠𝜃1 . 𝑐𝜃3 (𝐿2 + 𝐿3 + 𝑑2 )𝑐𝜃1
0
𝑐𝜃3
−𝑠𝜃3
𝐿1
]
=[
−𝑠𝜃1 𝑐𝜃1 . 𝑠𝜃3 𝑐𝜃1 . 𝑐𝜃3 −(𝐿2 + 𝐿3 + 𝑑2 )𝑠𝜃1
0

0
0
1
- phương trình động học thuận tính tốn các tọa độ 𝑋𝑒 , 𝑌𝑒 , 𝑍𝑒 của khâu chấp hành
o

𝑤
𝑒𝑇

cuối mô tả trong hệ tọa độ {W} theo các biến khớp 𝑞1 , 𝑞2 , 𝑞3 :
𝑋𝑒 = (𝐿2 + 𝐿3 + 𝑑2 )𝑐𝜃1
{ 𝑌𝑒 = 𝐿1
𝑍𝑒 = −(𝐿2 + 𝐿3 + 𝑑2 )𝑠𝜃1
1.2 Viết các phương trình phân tích giải bài tốn động học ngược:
-

-

Chúng ta có 2 phương trình để giải tìm 𝜃1 và 𝑑2 :
𝑋 = (𝐿2 + 𝐿3 + 𝑑2 )𝑐𝜃1 (1)
{ 𝑒
𝑍𝑒 = −(𝐿2 + 𝐿3 + 𝑑2 )𝑠𝜃1 (2)
𝑋𝑒 2 = (𝐿2 + 𝐿3 + 𝑑2 )2 𝑐𝜃1 2 (3)
⟺{ 2
𝑍𝑒 = (𝐿2 + 𝐿3 + 𝑑2 )2 𝑠𝜃1 2 (4)
Cộng (3) và (4) ta được:
𝑋𝑒 2 + 𝑍𝑒 2 = (𝐿2 + 𝐿3 + 𝑑2 )2 𝑐𝜃1 2 + (𝐿2 + 𝐿3 + 𝑑2 )2 𝑠𝜃1 2
⟺ 𝑋𝑒 2 + 𝑍𝑒 2 = (𝐿2 + 𝐿3 + 𝑑2 )2
⟺ ±√𝑋𝑒 2 + 𝑍𝑒 2 = 𝐿2 + 𝐿3 + 𝑑2
Mà 𝐿2 + 𝐿3 + 𝑑2 > 0 nên t lấy giá trị dương của ±√𝑋𝑒 2 + 𝑍𝑒 2

⟺ 𝑑2 = √𝑋𝑒 2 + 𝑍𝑒 2 −𝐿2 − 𝐿3

SVTH: NGUYỄN QUANG REN

TRANG 8


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

-

Chia (2) cho (1) ta được:
𝑍𝑒
= −tan 𝜃1
𝑋𝑒
⟺ 𝜃1 = tan−1 (−

-

𝑍𝑒
)
𝑋𝑒

Vậy từ các tọa độ của khâu chấp hành cuối ta xác định được phương trình các
biến khớp.
𝑑2 = √𝑋𝑒 2 + 𝑍𝑒 2 −𝐿2 − 𝐿3
{


𝜃1 = tan−1 (−

𝑍𝑒
)
𝑋𝑒

1.3 Thay số cụ thể để kiểm tra lại bài toán động học thuận và động học ngược:
a) Trường hợp các tham số của robot và các biến khớp có giá trị cụ thể: L1 =
0.7m, L2 = 0.5m, L3 = 0.6m:
𝑋𝑒 = (𝐿2 + 𝐿3 + 𝑑2 )𝑐𝜃1
{ 𝑌𝑒 = 𝐿1
𝑍𝑒 = −(𝐿2 + 𝐿3 + 𝑑2 )𝑠𝜃1
𝑋𝑒 = (0.5 + 0.6 + 𝑑2 )𝑐𝜃1
⟺ { 𝑌𝑒 = 0.7
𝑍𝑒 = −(0.5 + 0.6 + 𝑑2 )𝑠𝜃1
𝑋𝑒 = (1.1 + 𝑑2 )𝑐𝜃1
⟺ {𝑌𝑒 = 0.7
𝑍𝑒 = −(1.1 + 𝑑2 )𝑠𝜃1
b) Lấy giá trị số cụ thể 𝑋𝑒 , 𝑌𝑒 , 𝑍𝑒 của khâu chấp hành cuối ở kết quả câu 3a) thay

-

vào phương trình tính tốn động học ngược của câu 2) để có kết quả giá trị số
cụ thể của q1, q2 và q3.
Ta có phương trình động học ngược:
𝑑2 = √𝑋𝑒 2 + 𝑍𝑒 2 −𝐿2 − 𝐿3
{

-


𝜃1 = tan−1 (−

𝑍𝑒
)
𝑋𝑒

Chọn giá trị cụ thể 𝑋𝑒 , 𝑌𝑒 , 𝑍𝑒 lần lượt là 1, 0.7, 1
−11 + 10√2
{ 𝑑2 =
10
𝜃1 = −450

SVTH: NGUYỄN QUANG REN

TRANG 9


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

-

Để kiểm nghiệm ta thay các giá trị biến khớp vừa tính được vào phương trình
động học thuận.
𝑋𝑒 = (1.1 + 𝑑2 )𝑐𝜃1
{𝑌𝑒 = 0.7
𝑍𝑒 = −(1.1 + 𝑑2 )𝑠𝜃1
−11 + 10√2
) cos(−45)

10
𝑌𝑒 = 0.7
⟺
−11 + 10√2
𝑍𝑒 = − (1.1 +
) cos(−45)
10
{
𝑋𝑒 = (1.1 +

𝑋𝑒 = 1
⟺ { 𝑌𝑒 = 0.7
𝑍𝑒 = 1
c) Nhận xét:
- Ta thấy kết quả của các biến khớp khi thay số cụ thể của tọa độ điểm cuối
𝑋𝑒 , 𝑌𝑒 , 𝑍𝑒 vào phương trình động học ngược và kết quả của các tọa độ 𝑋𝑒 , 𝑌𝑒 , 𝑍𝑒
khi thay giá trị các biến khớp tính được vào phương trình động học thuận hồn
tồn giống nhau.
- Vậy ta kết luận phương trình động học nghịch được thành lập từ phương trình
động học thuận là đúng.

SVTH: NGUYỄN QUANG REN

TRANG 10


BÀI TẬP KỸ THUẬT ROBOT

GVHD: PGS.TS LÊ TIẾN DŨNG


PHẦN II. MÔ PHỎNG:
2.1 Các bước thực hiện:
- Sơ đồ tổng quát mơ phỏng:

Hình 2. 1 Sơ đồ tổng qt mơ phỏng.
a) Xây dựng khối cơ khí:
➢ Bước 1: Gọi và cài đặt các khối tọa độ gốc

Hình 2. 2 Gọi và cài đặt các khối tọa độ gốc.
-

Vì hệ tọa độ w và e của cánh tay có trục Y hướng lên nên ta cần đặt lại gia tốc
trọng trường tại trục Y và có chiều ngược chiều dương.

Hình 2. 3 Cài đặt gia tốc trọng trường.
SVTH: NGUYỄN QUANG REN

TRANG 11


BÀI TẬP KỸ THUẬT ROBOT

GVHD: PGS.TS LÊ TIẾN DŨNG

➢ Bước 2: Gọi khối tạo hình và gắn trục đất cho thanh Base (Link 0)

Hình 2. 4 Gọi khối tạo hình và gắn trục đất cho thanh Base.
-

Tại thanh Base ta gắn hệ tọa độ {w} và {𝑜0 }


-

Tiếp theo ta gắn hệ tọa độ {w} vào trục đất

-

Còn lại hệ tọa độ {𝑜0 } ta sẽ gắn vào khớp quay ở bước 3

➢ Bước 3: Gọi khối trục xoay 1 (Joint 1) để liên kết các thanh Link 0 và Link 1

Hình 2. 5 Gọi khối trục xoay 1 (Joint 1) để liên kết các thanh Link 0 và Link 1.
- Ở bước này ta thực hiện liên kết link 0 ở bước 1 và link 1 ở bước tiếp theo
-

Ta gắn hệ tọa độ {𝑜0 } của link 0 vào cổng B của khối trục xoay

➢ Bước 3: Gọi khối tạo hình và gắn trục cho thanh link 1:

Hình 2. 6 Gọi khối tạo hình và gắn trục cho thanh link 1.
-

Ở đây ta thấy có hai khối tạo hình liên kết nào tạo thành thanh Link 1

SVTH: NGUYỄN QUANG REN

TRANG 12


BÀI TẬP KỸ THUẬT ROBOT


GVHD: PGS.TS LÊ TIẾN DŨNG

-

Ta gắn hệ tọa độ {𝑜0 } của link 1 vào cổng F của khối trục xoay Joint 1

-

Ta gắn hệ tọa độ {𝑜1 ’} trên link 1 vào trục quay Joint 2 ở bước tiếp theo

➢ Bước 4: Gọi khối trục tịnh tiến 1 Joint 1 để liên kết các thanh Link 1 và Link 2

Hình 2. 7 Gọi khối trục tịnh tiến 1 Joint 1 để liên kết các thanh Link 1 và Link 2.
-

Ta gắn hệ tọa độ {𝑜1 ’} của link 1’ vào cổng B của khối trục tịnh tiến Joint 2

➢ Bước 5: Gọi khối tạo hình và gắn trục cho thanh Link 2

Hình 2. 8 Gọi khối tạo hình và gắn trục cho thanh Link 2.
-

Ở đây ta thấy có hai khối tạo hình liên kết nào tạo thành thanh Link 2

-

Ta gắn hệ tọa độ {𝑜1 ’} của link 1 vào cổng F của khối trục xoay Joint 2

-


Ta gắn hệ tọa độ {𝑜3 ’} trên link 1 vào trục quay Joint 3 ở bước tiếp theo

➢ Bước 6: Gọi khối trục xoay Joint 3 để liên kết các thanh Link 2 và Link 3

Hình 2. 9 Gọi khối trục xoay Joint 3 để liên kết các thanh Link 2 và Link 3.
-

Ta gắn hệ tọa độ {𝑜3 ’} của link 2 vào cổng B của khối trục xoay Joint 3

SVTH: NGUYỄN QUANG REN

TRANG 13


BÀI TẬP KỸ THUẬT ROBOT

GVHD: PGS.TS LÊ TIẾN DŨNG

➢ Bước 7: Gọi khối tạo hình và gắn trục cho thanh Link 3

Hình 2. 10 Gọi khối tạo hình và gắn trục cho thanh Link 3.
-

Ta gắn hệ tọa độ {𝑜3 } của link 3 vào cổng F của khối trục xoay Joint 3

-

Ta gắn hệ tọa độ {e} trên link 3 vào các khối tạo hình của end effector ở bước
tiếp theo


➢ Bước 8: Gọi khối tạo hình và gắn trục cho End effector

Hình 2. 11 Gọi khối tạo hình và gắn trục cho End effector.
- Ta gắn hệ tọa độ {e} của link 3 vào {e} của khối tạo hình End effector
➢ Bước 9: Lấy số liệu và hiển thị
-

Thêm các khối hiện thị
Thêm các khối lấy giá trị các khớp quay và tọa độ điểm cuối của tay gắp.
Xuất đồ thị X và Z để phục vụ kiểm nghiệm.

Hình 2. 12 Lấy số liệu và hiển thị.
SVTH: NGUYỄN QUANG REN

TRANG 14


BÀI TẬP KỸ THUẬT ROBOT

GVHD: PGS.TS LÊ TIẾN DŨNG

b) Xây dựng khối động học thuận Forwarknematics:
- Từ thư viện Simulink ta vào mục User – Defined Function và lấy ra khối
MATLAB Function

Hình 2. 13 Thư viện Simulink ta vào mục User – Defined Function.
-

Sau đó ta viết chương trình động học thuận trong khối này


Hình 2. 14 Chương trình động học thuận .
Trong đó, các biến đầu vào là Theta 1, d2, Theta 3 và đầu ra là các giá trị X, Y,
Z và đây cũng là kết quả tọa độ điểm cuối của cách tay robot.
c) Xây dựng khối động học ngược Inverseknematics:
- Cũng như khối Forwarknematics, ta cũng vào thư viện và lấy ra một khối
MATLAB Function
-

SVTH: NGUYỄN QUANG REN

TRANG 15


BÀI TẬP KỸ THUẬT ROBOT

-

GVHD: PGS.TS LÊ TIẾN DŨNG

Sau đó ta viết chương trình động học ngược trong khối này

Hình 2. 15 Chương trình động học ngược.
- Trong đó, các biến đầu vào là tọa độ X, Y và các biến đầu ra là Theta1 và d2
d) Xây dựnh khối truyền động đầu vào:
- Chuyển động theo một đường trịn:

Hình 2. 16 Phương trình chuyển động đường trịn.
-


Chuyển động theo một đường thẳng:

Hình 2. 17 Phương trình chuyển động đường thẳng.
Ta thêm khối Graph để xuất ra đồ thị mong muốn của X, Z phục vụ kiểm
nghiệm.
e) Liên kết các khối với nhau:
-

Hình 2. 18 Liên kết các khối với nhau.
SVTH: NGUYỄN QUANG REN

TRANG 16


BÀI TẬP KỸ THUẬT ROBOT

-

GVHD: PGS.TS LÊ TIẾN DŨNG

Tại khối cơ khí ta chọn Theta 3 là một hằng số có giá trị khơng phụ thuộc vào
setpoint.
Xuất đồ thị theo X và Z để kiểm nghiệm so với đồ thị X, Z của cơ khí và

setpoint.
2.2 Kiểm nghiệm:
➢ Kiểm nghiệm phần động học thuận:
- Để kiểm nghiệm động học thuận ta so sánh các giá trị X, Y, Z là tọa độ của
điểm cuối của chu trình chuyển động của tay Robot trong phần cơ khí và phần
động học thuận.

o Tọa độ X, Y, Z trong phần cơ khí:

Hình 2. 19 Tọa độ X, Y, Z trong phần cơ khí.
o Tọa độ X, Y, Z trong phần động học thuận:

Hình 2. 20 Tọa độ X, Y, Z trong phần động học thuận.
-

Nhận xét:
o Ta thấy giá trị tọa độ điểm cuối của cách tay Robot ở phần cơ khí và
phần tính tốn động học thuận hồn tồn bằng nhau
o Vậy ta kết luận phần động học thuận là chính xác so với cơ khí

SVTH: NGUYỄN QUANG REN

TRANG 17


BÀI TẬP KỸ THUẬT ROBOT

GVHD: PGS.TS LÊ TIẾN DŨNG

➢ Kiểm nghiệm phần động học ngược:
- Để kiểm nghiệm phần động học ngược ta đi so sánh giá trị của hai biến khớp là
Theta 1 và d2 trong hai phần cơ khí và động học ngược.
o Giá trị Theta 1 và d2 trong phần cơ khí:

Hình 2. 21 Giá trị Theta 1 và d2 trong phần cơ khí.
o Giá trị Theta 1 và d2 trong phần động học ngược:


Hình 2. 22 Giá trị Theta 1 và d2 trong phần động học ngược.
-

Nhận xét:
o Ta thấy giá trị Theta 1 và d2 ở phần cơ khí và phần tính tốn động học
ngược hoàn toàn bằng nhau
o Vậy ta kết luận phần động học ngược là chính xác so với cơ khí

SVTH: NGUYỄN QUANG REN

TRANG 18


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

2.3 Kết quả mơ phỏng:
➢ Mơ hình 3D phần cơ khí:

Hình 2. 23 Mơ hình 3D phần cơ khí.
➢ Mơ phỏng cho Robot chuyển động theo đường trịn:
- Đồ thị XZ setpoint:

Hình 2. 24 Đồ thị XZ setpoint.

SVTH: NGUYỄN QUANG REN

TRANG 19



GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

-

Đồ thị XZ cơ khí:

Hình 2. 25 Đồ thị XZ cơ khí.
-

Đồ thị XZ của khối động học thuận:

Hình 2. 26 Đồ thị XZ của khối động học thuận.

SVTH: NGUYỄN QUANG REN

TRANG 20


GVHD: PGS.TS LÊ TIẾN DŨNG

BÀI TẬP KỸ THUẬT ROBOT

➢ Mô phỏng cho Robot chuyển động theo đường thẳng:
- Đồ thị XZ setpoint:

\
Hình 2. 27 Đồ thị XZ setpoint.

-

Đồ thị XZ cơ khí:

Hình 2. 28 Đồ thị XZ cơ khí.

SVTH: NGUYỄN QUANG REN

TRANG 21


BÀI TẬP KỸ THUẬT ROBOT

-

GVHD: PGS.TS LÊ TIẾN DŨNG

Đồ thị XZ của khối động học thuận:

Hình 2. 29 Đồ thị XZ của khối động học thuận.
2.4 Nhận xét:
➢
➢
➢
-

Phần cơ khí:
Thiết kế chính xác giúp đơn giản hóa khi kiểm nghiệm
Tổng thể trực quang, thể hiện đầy đủ các khớp nối, thanh dẫn
Kích thước cơ khí phù hợp với mục tiêu đề bài.

Phần động học thuận:
Tính tốn động học thuận chính xác so với cơ khí.
Phần động học nghịch:
Tính tốn động học ngược chính xác so với cơ khí.

SVTH: NGUYỄN QUANG REN

TRANG 22