Ứng dụng toolbox trong matlab xây dựng và mô phỏng mô hình toán học cánh tay robot công nghiệp

ISSN 2354-0575
Khoa học & Công nghệ - Số 10/Tháng 6 - 2016 Journal of Science and Technology 55
ỨNG DỤNG TOOLBOX TRONG MATLAB XÂY DỰNG VÀ MÔ PHỎNG
MÔ HÌNH TOÁN HỌC CÁNH TAY ROBOT CÔNG NGHIỆP
Đào Minh Tuấn, Nguyễn Thị Thúy, Nguyễn Thị Như
Trường Đại học Sư phạm Kỹ thuật Hưng Yên
Ngày nhận: 05/4/2016
Ngày sửa chữa: 08/6/2016
Ngày xét duyệt: 20/6/2016
Tóm tắt:
Để thiết kế và điều khiển cánh tay robot, bài toán động học và động lực học rất quan trọng . Động 
học robot nghiên cứu chuyển động các khâu của robot về phương diện hình học, nhằm phục vụ tính toán 
và thiết kế cánh tay robot. Động lực học nghiên cứu các lực và moment gây ra chuyển động, nhằm phục vụ 
điều khiển cánh tay robot. Tuy nhiên, việc tính toán các tham số động học và động lực học của robot bằng 
tay phức tạp, gặp rất nhiều khó khăn và dễ gây nhầm lẫn. Bài báo này đề cập đến ứng dụng Toolbox trong 
Matlab để giải quyết hai bài toán một cách dễ dàng, chính xác.
Từ khóa: Động học cánh tay robot, Động lực học cánh tay robot, Thiết kế cánh tay robot.
1. Đặt vấn đề
Bài toán động học robot để nghiên cứu 
chuyển động các khâu của robot về phương diện 
hình học nhằm phục vụ tính toán và thiết kế cánh 
tay robot. Nhiệm vụ chính của bài toán động học 
thuận là xác định vị trí và hướng của bàn kẹp dưới 
dạng hàm của các biến khớp. Các phương pháp ma 
trận 4x4 hay được sử dụng trong phân tích động học 
robot. Trong bài báo này trình bày phương pháp ma 
trận Denavit-Hartenberg xác định vị trí, vận tốc của 
bàn kẹp cánh tay robot [1].
Bài toán động lực học để nghiên cứu các 
lực và moment gây ra chuyển động nhằm phục vụ 
điều khiển cánh tay robot. Nhiệm vụ chính của bài 
toán động lực học là xây dựng mô hình toán học của 
cánh tay robot. Trong bài báo này trình bày phương 
pháp Lagrange xây dựng mô hình toán học cánh tay 
robot [1].
Trong thực tế cho thấy việc thực hiện hai 
bài toán này bằng tay rất phức tạp, gặp nhiều khó 
khăn và dễ gây nhầm lẫn. Bài báo đưa ra ứng dụng 
Toolbox trong Matlab để giải hai bài toán động học 
thuận và động lực học nhằm mô phỏng và xây dựng 
mô hình toán học một cách dễ dàng và chính xác.
2. Cơ sở lý thuyết
2.1. Mô hình robot RRR
Chuyển động của robot gồm 3 khớp.
- Khớp 1: Chuyển động quay quanh trục z
0
.
- Khớp 2: Chuyển động quay quanh trục z
1
.
- Khớp 3: Chuyển động quay quanh trục z
2
.
- q
1
, q
2
, q
3
 là các góc quay của khớp 1, khớp 
2 và khớp 3.
- a
1
, a
2
, a
3
 là độ dài của các thanh nối 1, thanh 
nối 2 và thanh nối 3.
Hình 1. Cánh tay robot RRR
2.2. Giải bài toán động học thuận và mô phỏng 
cánh tay robot
2.2.1. Thiết lập hệ phương trình động học
2.2.1.1. Lập bảng thông số DH[1]
Bảng 1.1. Bảng tham số động học của cánh tay 
robot RRR
Khớp di θi ai αi
1 0 θ
1
a
1
π/2
2 0 θ
2
a
2
0
3 0 θ
3
a
3
0
Với:
- di: dịch chuyển tịnh tiến dọc theo trục zi-1 để 
gốc tọa độ O
i-1
 chuyển đến O’i, giao điểm của trục 
xi và trục zi-1.
- ai: dịch chuyển tịnh tiến dọc theo trục xi để 
điểm O’i chuyển đến điểm Oi.
ISSN 2354-0575
Journal of Science and Technology56 Khoa học & Công nghệ - Số 10/Tháng 6 - 2016
- αi: góc quay quanh trục xi sao cho trục z’i-1 
(z’
i-1
//z
i-1
) chuyển đến trục zi .
-θi: góc quay quanh trục zi-1 để trục xi-1 
chuyển đến trục x’i (x’i // xi).
2.2.1.2. Xác định các ma trận Ai theo thông số DH 
[1]
- Ta có:
Ai
C i
S i
S iC i
C iC i
S i
S iS i
C iS i
C i
aiC i
aiS i
di0
0 0 0 1
i
i
i a
i a
a
i a
i a
a
i
i
=
-
-
J
L
KKKKKKKKKK
N
P
OOOOOOOOOO
Suy ra:
- Ma trận D-H của khâu 1:
A
C
S
S
C
a C
a S
0
0
0
0
1
0
0
0
0
1
1
1
1
1
1
1 1
1 1=
-
J
L
KKKKKKKKKK
N
P
OOOOOOOOOO
- Ma trận D-H của khâu 2:
A
C
S
S
C
a C
a S
0
0
0
0
0
0
1
0
0
1
 2
2
2
2
2
2 2
2 2=
-J
L
KKKKKKKKKK
N
P
OOOOOOOOOO
- Ma trận D-H của khâu 3:
A
C
S
S
C
a C
a S
0
0
0
0
0
0
1
0
0
1
 3
3
3
3
3
3 3
3 3=
-J
L
KKKKKKKKKK
N
P
OOOOOOOOOO
Bảng 1.2. Bảng ký hiệu
Ký hiệu Giải thích
Ci Cos(θi ) 
Cij Cos(θi + θj )
Si Sin(θi )
Sij Sin(θi + θj )
Từ đó suy ra ma trận trận thuần nhất T:
T A A A
N
N
N
O
O
O
A
A
A
P
P
P
0 0 0 1
X
Y
Z
X
Y
Z
X
Y
Z
X
Y
Z
3 1 2 3= =
J
L
KKKKKKKKKK
N
P
OOOOOOOOOO
2.2.1.3. Thiết lập các phương trình động học cơ 
bản [3]
Các thành phần của ma trận chỉ hướng:
Nx = C
23
C
1
; Ny = C
23
S
1
; Nz = S
23
;
Ox = -S
23
C
1
; Oy = -S
23
S
1
; Oz = C
23
;
Ax = S
1
; Ay = -C
1
; Az = 0;
Tọa độ điểm thao tác:
Px = C
1
(a
1
 + a
3
C
23
 + a
2
C
2
);
Py = S
1
(a
1
 + a
3
C
23
 + a
2
C
2
);
Pz = a
3
S
23
 + a
2
S
2
2.2.2. Tính toán động học thuận Robot
2.2.2.1. Vị trí điểm thao tác cuối E
( )
( )r
P
P
P
C a a C a C
S a a C a C
a S a S
E
X
Y
Z
1 1 3 23 2 2
1 1 3 23 2 2
3 23 2 2
= =
+ +
+ +
+
J
L
KKKKKKK
J
L
KKKKKKK
N
P
OOOOOOO
N
P
OOOOOOO
2.2.2.2. Vận tốc điểm thao tác cuối E
v r
P
P
P
.
.
.
.
E E
X
Y
Z
= =
J
L
KKKKKKKKKKK
N
P
OOOOOOOOOOO
Trong đó:
( ) ( )P q C a S a S q S a a C a C
q a S C
. . .
.
X 2 1 3 23 2 2 1 1 1 3 23 2 2
3 3 23 1
=- + - + +
-
( ) ( )P q C a a S a C q S a S a S
q a S S
. . .
.
Y 1 1 1 3 23 2 2 2 1 3 23 2 2
3 3 23 1
= + + - +
- 
( )P q a C a C q a C
. . .
Z 2 3 23 2 2 3 3 23= + + .
2.2.3. Mô phỏng cánh tay robot
Chọn quỹ đạo chuyển động bất kỳ như sau:
q
1
 = 3t; q
2
 = 2t; q
3
 = t;
Thay các thông số: a
1
 = 5; a
2
 = 3; a
3
 = 2;
Sử dụng Symbolic Math Toolbox và 
Robotics Toolbox ta được quỹ đạo của cánh tay 
robot trong khoảng t như sau:
Hình 2. Quỹ đạo tác động cuối trong bài toán thuận 
với t = 0 – π/16
ISSN 2354-0575
Khoa học & Công nghệ - Số 10/Tháng 6 - 2016 Journal of Science and Technology 57
Hình 3. Quỹ đạo tác động cuối trong bài toán thuận 
với t = 0 – π
Hình 4. Quỹ đạo tác động cuối trong bài toán thuận 
với t = 0 – 2π
2.3. Xây dựng mô hình toán học của robot công 
nghiệp
2.3.1. Tọa độ các điểm trọng tâm của thanh nối 
trên hệ tọa độ O(x,y,z)
r
a C C a C
a S C a S
a S
2
1
2
1
2
1
2
2 1 2 1 1
2 1 2 1 1
2 2
=
+
+
J
L
KKKKKKKKKKKK
N
P
OOOOOOOOOOOO 
- Tọa độ trọng tâm thanh nối 3
r
a C C a C C a C
a S C a S C a S
a S a S
2
1
2
1
2
1
3
3 1 23 2 1 2 1 1
3 1 23 2 1 2 1 1
2 2 3 23
=
+ +
+ +
+
J
L
KKKKKKKKKKKK
N
P
OOOOOOOOOOOO
2.3.2. Vận tốc dài tại các điểm trọng tâm của các 
thanh nối
- Vận tốc dài điểm trọng tâm thanh nối 1
V
a S q
a C q
2
1
2
1
0
.
.
1
1 1 1
1 1 1=
-
J
L
KKKKKKKKKK
N
P
OOOOOOOOOO
- Vận tốc dài điểm trọng tâm thanh nối 2
V
a S q a C S q a C S q
a S q a C C q a S S q
a C q
2
1
2
1
2
1
2
1
2
1
. . .
. . .
.
2
1 1 1 2 2 1 1 2 1 2 2
1 1 1 2 1 2 1 2 1 2 2
2 2 2
=
- - -
- -
J
L
KKKKKKKKKKKKK
N
P
OOOOOOOOOOOOO
- Vận tốc dài điểm trọng tâm thanh nối 3
V
V
V
V
3
31
32
33
=
J
L
KKKKKKK
N
P
OOOOOOO
V a S q a S C q a C S q
a S C q a C S q q2
1
2
1
. . .
. . .
31 1 1 1 2 1 2 1 2 1 2 2
3 1 23 1 3 1 23 2 3
=- - -
- - +` j
V a C q a C C q a S S q
a C C q a S S q q2
1
2
1
. . .
. . .
32 1 1 1 2 1 2 1 2 1 2 2
3 1 23 1 3 1 23 2 3
=- - - +
+ - +` j
V a C q q a C q2
1 . . .
33 3 23 2 3 2 2 2= + +` j
2.3.3. Vận tốc góc của các khớp
- Vận tốc góc khớp 1: w
q
0
01
1
=
o
J
L
KKKKKKK
N
P
OOOOOOO
- Vận tốc góc khớp 2: 
q
0
0w
.
2
2
=
J
L
KKKKKKKK
N
P
OOOOOOOO
- Vận tốc góc khớp 3: 
q
0
0w
.
3
3
=
J
L
KKKKKKKK
N
P
OOOOOOOO
2.3.4. Động năng của các khâu
- Động năng của khâu thứ 1
K m a I q8
1 4
.
1 1 1
2
1 1
2= +_ i
- Động năng của khâu thứ 2
K m a q m a a C q a C q
m a q I q
2
1
2
1
8
1
8
1
2
1
. . .
. .
2 2 1
2
1
2
2 1 2 2 1
2
2
2
2 1
2
2 2
2
2
2
2 2
2
= + + +
+ +
- Động năng khâu thứ 3
K I q a m q a m q a m q
a m q a m q a m q q a m C q q
a m C q q q a a m C q
a a m C q q q a a m C q
a a m C q a a m C q a a m C q q
2
1
2
1
2
1
16
1
8
1
8
1
4
1
2
1
16
1 2 2 2
1
4
1 2
4
1
2
1
2
1
. . . .
. . . . .
. .
. .
. . . .
3 3 3
2
1
2
3 1
2
2
2
3 2
2
3
2
3 1
2
3
2
3 2
2
3
2
3 3
2
3
2
3 2 3 2
2
3
2
2 1
2
3
2
3 2 3 1
2
1 3 3 23 1
2
2 3 3 2 3 1
2
1 2 3 2 1
2
2 3 3 3 1
2
2 3 3 3 2
2
2 3 3 3 2 3
+ + + +
= + + + +
+ + + +
+ + + +
+ + +
_
_
i
i
ISSN 2354-0575
Journal of Science and Technology58 Khoa học & Công nghệ - Số 10/Tháng 6 - 2016
- Tổng động năng của các khâu
K K K K
m a I q I q I q a m q
a m q a m q a m q a m q
a m q a m q a m q q a m C q
a m C q a m C q q q
a a m C q a a m C q q q
a a m C q a a m C q a a m C q
a a m C q a a m C q q
8
1 4 2
1
2
1
2
1
2
1
8
1
2
1
16
1
8
1
8
1
4
1
8
1
2
1
16
1 2 2
2
1
4
1 2
2
1
4
1
2
1
2
1
. . . .
. . . .
. . . . .
. .
. .
. . .
.
1 2 3
1 1
2
1 1
2
2 2
2
3 3
2
1
2
2 1
2
1
2
3 1
2
2
2
2 2
2
2
2
3 2
2
3
2
3 1
2
3
2
3 2
2
3
2
3 3
2
3
2
3 2 3 2
2
2 2
2
1
2
2
2
3 2
2
1
2
3
2
3 2 3 1
2
1 3 3 23 1
2
2 3 3 2 3 1
2
1 2 2 2 1
2
1 2 3 2 1
2
2 3 3 3 1
2
2 3 3 3 2
2
2 3 3 3 2 3
+ + + + +
= + +
= + + + + +
+ + + + +
+ + + +
+ + + +
+ + + +
+ +
_
_
_
i
i
i
2.3.5. Thế năng của các khâu
- Thế năng của khâu 1: P
1
 = 0
- Thế năng của khâu 2
P a gm S2
1
2 2 2 2=
- Thế năng của khâu 3
P gm a S a S2
1
3 3 2 2 3 23= +b l 
- Tổng động năng của các khâu
P gm a S a S a gm S2
1
2
1
3 2 2 3 23 2 2 2= + +b l 
2.3.6. Tính hàm Lagrange của cánh tay robot RRR
- Hàm Lagrange: L = K - P [1]
- Momen đặt lên khớp 1
M dt
d
q
L
q
L
1
1 12
2
2
2= -o [1]
- Momen đặt lên khớp 2
M dt
d
q
L
q
L
2
2 22
2
2
2= -o
 [1]
- Momen đặt lên khớp 3
M dt
d
q
L
q
L
3
3 32
2
2
2= -o
 [1]
Với: M
M
M
M
1
2
3
=
J
L
KKKKKKK
N
P
OOOOOOO 
Khi đó ta thiết lập phương trình động lực học 
của cánh tay robot RRR được thiết lập có dạng sau:
( ) ( , ) ( )M H q q V q q G q
.. .
= + + [1]
Thành phần quán tính H
H
H
H
H
H
H
0
0
0 011
22
32
23
33
=
J
L
KKKKKKK
N
P
OOOOOOO
Trong đó:
( )
H I a m m m a m
a m m C a m C q q
4
1
8
1
4
1
8
1 2 2
11 1 1
2
1 2 3 3
2
3
2
2
2 3 2
2
3
2
3 2 3
= + + + + +
+ + + ++
b
b
l
l
( )a a m C a a m C q q
a a m m C a a m
2
1 2
2 2
1
1 3 3 23 2 3 3 2 3
1 2 2 3 2 2 3 3
+ + +
+ +
+
+ _ i
H I a m m a a m C4
1
22 2 2
2
2 3 2 3 3 3= + + +b l
H a m4
1
23 3
2
3= ; H a m4
1
32 3
2
3= ; H I a m4
1
33 3 3
2
3= + 
Thành phần tương hỗ V
V
V
V
V
1
2
3
=
J
L
KKKKKKK
N
P
OOOOOOO
Trong đó: 
( )V a m m S q q q
a m q q S q q q
a a m q q S q
a a m q q S q q q
a a m m q S q
4
1 2
8
1 2 2 2 2
2
1 2 2
2
. .
. . .
. . .
. . .
. .
1 2
2
2 3 2 1 2
3
2
3 2 3 2 3 1
1 3 3 2 3 23 1
2 3 3 2 3 2 3 1
1 2 3 2 2 2 1
=- +
- + +
- +
- + +
- +
b
` _
`
` _
_
l
j i
j
j i
i 
( )
( )
( )
V a a m S q q a m m S q q
a m S q q q a a m S q
a a m S q q q a a m m S q
2
1
8
1
2
1 2
8
1 2 2 2
1
2
1 2 2
1
. . .
. .
. .
2 2 3 3 3 2 3 2
2
2 3 2 1
2
3
2
3 2 3 1
2
1 3 3 23 1
2
2 3 3 2 3 1
2
1 2 2 3 2 1
2
=- + + +
+ + + +
+ + + +
b
b
l
l
( )
( )
( )
V a a m S q q a m S q q q
a a m S q a a m S q q q
a a m S q q q q
2
1
8
1 2 2
2
1
4
1 2
4
1
2
1
2
1
. . .
. .
. . . .
3 2 3 3 3 2 3 3
2
3 2 3 1
1 3 3 23 1
2
2 3 3 2 3 1
2
2 3 3 3 1
2
2
2
2 3
=- + + +
+ + + +
+ + +
Thành phần trọng trường G
G gm a C gm a C a C
gm a C a C
0
2
1
2
1
2
1
2 2 2 3 2 3 3 23
3 2 3 3 23
= + +
+
J
L
KKKKKKKKKKK
b
b
N
P
OOOOOOOOOOO
l
l
3. Kết luận và đánh giá
Bài báo trình bày tổng quan về ứng dụng 
Toolbox trong Matlab để xây dựng mô hình toán 
học và mô phỏng cánh tay robot. Với kết quả thu 
được từ ứng dụng trên cánh tay robot RRR nhận 
thấy quá trình giải hai bài toán một cách dễ dàng, 
kết quả chính xác. Ưu điểm nổi bật của ứng dụng 
Toolbox là mô phỏng và xây dựng mô hình toán học 
cánh tay robot một cách đơn giản, chính xác. Điều 
này sẽ nâng cao chất lượng trong quá trình thiết kế 
và điều khiển cánh tay robot.
ISSN 2354-0575
Khoa học & Công nghệ - Số 10/Tháng 6 - 2016 Journal of Science and Technology 59
Tài liệu tham khảo
[1]. Nguyễn Văn Khang, Chu Anh Mỳ, (2011), “Cơ sở robot công nghiệp”, NXB GDVN, trang 
39 – 58.
[2]. Nguyễn Hoàng Hải, Nguyễn Việt Anh, (2006), “Lập trình Matlab và ứng dụng”, NXB KHKT, 
Hà Nội.
[3]. Phạm Đăng Phước, (2006), “Robot công nghiệp”, NXB KHKT, Hà Nội.
[4]. Peter Corke, (2011),“Robotics, Vision and Control Fundamental Agoritthms in Matlab”.
APPLY TOOLBOX FOR MATLAB TO SIMULATION 
AND MATHEMATICAL MODEL CONSTRUCTION ROBOT ARMS
Abstract:
To design and control the robot arm, problem kinetics and problem dynamics are important. 
Kinematic robot research stages of the robot motion in terms of geometry. Kinetics is serving calculation 
problem and design the robot arm. Research dynamics and torque forces causing motion. Dynamics is the 
problem of serving robot arm control. However, the calculation of the kinetic parameters and dynamics 
of complex robotic hand, face many difficult and confusing. This article refers to the Toolbox in Matlab 
application to solve two problems with ease and accuracy.
Keywords: Kinematics robot arm, the robot arm dynamics, design robotic arm.