Thiết kế và điều khiển mô hình 3D cho Robot RTR
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ

ĐỒ ÁN MÔN HỌC
THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ

Giáo viên hướng dẫn : PGS.TS Phan Bùi Khôi
Sinh viên thực hiện : Trần Văn Phương
Số hiệu sinh viên : 20100530
Lớp : KT CĐT3- K55
1
Năm học 2013 - 2014
Thiết kế và điều khiển mô hình 3D cho Robot RTR
MỤC LỤC
Lời nói đầu
3
Đề tài
4
Lí do chọn đề tài
5
Phần I : Thiết kế mô hình 3D cho Robot.
6
Phần II : Cơ sở lý thuyết về động học, động lực học Robot công nghiệp.
9
Phần III: Tính toán động học cho Robot.
22
1. Bài toán động học thuận.
22
2. Bài toán động học ngược.
28

động hóa. Mục tiêu ứng dụng của Robot công nghiệp trong sản xuất là nhằm nâng
cao năng suất của các dây chuyền công nghệ, nâng cao chất lượng sản phẩm, cải
4
Thiết kế và điều khiển mô hình 3D cho Robot RTR
thiện điều kiện lao động, nhất là ở các khu vực con người không thể di chuyển
được…
Ở nước ta từ những năm 90 của thế kỉ XX đến nay, nhiều cơ sở sản xuất đã sử dụng
nhiều loại robot công nghiệp phục vụ quá trình sản xuất. Việc nghiên cứu Robot
công nghiệp nói riêng và Robot nói chung đang được nhiều trung tâm và các
trường Đại học chú ý quan tâm.
Trong quá trình thực hiện đề tài, dù đã nhận được sự dặn dò chỉ bảo nhiệt tình của
thầy PGS.TS Phan Bùi Khôi nhưng do kiến thức của em còn yếu nên không tránh
khỏi được những thiếu sót, rất mong được ý kiến của các thầy cô để em tự hoàn
thiện thêm . Em xin chân thành cảm ơn !
5
Thiết kế và điều khiển mô hình 3D cho Robot RTR
ĐỀ TÀI
Cho mô hình robot như hình vẽ:

1. Thiết kế mô hình 3D.
2. Xây dựng quy luật chuyển động phù hợp.
3. Tính toán động học, động lực học cho cơ cấu (bằng Maple) .
4. Điều khiển Robot. (Matlab Simulink) .
5. Mô phỏng kết quả.
6
Hình 1 : Cơ cấu Robot phẳng 3 bậc tự do
Thiết kế và điều khiển mô hình 3D cho Robot RTR
LÍ DO CHỌN ĐỀ TÀI
Vấn đề thiết yếu đặt ra là phải tăng nhanh lượng tự động hóa vào các quá trình sản
xuất công nghiệp. Đây cũng là một đòi hỏi cấp bách liên quan đến việc giải phóng

Hình 1.4 Khâu tịnh tiến và khớp quay thứ hai
Thiết kế và điều khiển mô hình 3D cho Robot RTR
Phần II : Cơ sở lý thuyết về động học, động lực học Robot công nghiệp
Phần này trình bày cách xây dựng các hệ tọa độ gắn với Robot, các biểu thức tính
động năng, thế năng của hệ vật rắn để từ đó ta có thể thiết lập phương trình vi phân
chuyển động của hệ vật rắn.
1. Các kí hiệu, khái niệm cơ bản, cách xác định trục của hệ tọa độ khớp và bài
toán động học.
Đối với robot công nghiệp, Denavit – Hartenberg (1955) đã đưa ra cách chọn các
hệ trục toạ độ gắn vào mỗi khâu của một tay máy Robot như sau :

11
Hình 1.1 Biểu diễn các thông số Denavit- Hartenberg giữa hai hệ trục tọa
độ
Thiết kế và điều khiển mô hình 3D cho Robot RTR
- Trục
i
z
được chọn dọc theo hướng của trục khớp động thứ (i+1), hướng của phép
quay và phép tịnh tiến được chọn tùy ý.
- Trục
i
x
được xác định dọc theo đường vuông góc chung giữa trục khớp động thứ
i và (i+1) hướng từ khớp động thứ i đến (i+1).
- Trục
i
y
xác định theo quy tắc bàn tay phải.
Vị trí của hệ toạ độ khớp

chuyển đến trục
i
x
theo quy tắc bàn tay phải.
-
i
d
: dịch chuyển tịnh tiến giữa hai đường vuông góc chung của 2 trục.
-
i
a
: khoảng dịch chuyển giữa hai trục khớp động kề nhau.
-
i
α
: góc lệch giữa trục của 2 khớp động liền kề, là góc quay quanh trục
i
x
sao cho
trục
1i
z
−
dịch chuyển đến trục
i
z
theo qui tắc bàn tay phải.
Trong bốn tham số trên , các tham số
i
a

bằng bốn phép biến đổi cơ bản sau:
- Quay quanh trục
1i
z
−
một góc
i
θ
12
Thiết kế và điều khiển mô hình 3D cho Robot RTR
- Dịch chuyển tịnh tiến dọc trục
1i
z
−
một đoạn
i
d
- Dịch chuyển tịnh tiến dọc trục
i
x
một đoạn
i
a
- Quay quanh trục
i
x
một góc
i
α
Ma trận của phép biến đổi, ký hiệu là

d
c
θ θ
α α
θ θ
α α
−
−
−
−
=
   
   
 ÷  ÷
 ÷ ÷
 ÷  ÷
 ÷ ÷
   
   
A

1
os sin os sin sin os
sin os os os sin sin
0 sin os
0 0 0 1
i i i i i i i
i i i i i i i
i
i i i

n n
−
=A A A A
0 0
0 0
0 1
n E
n E
T
 
= =
 
 
R r
A A
13
Thiết kế và điều khiển mô hình 3D cho Robot RTR
Ma trận
0
n
A
cho biết vị trí của điểm tác động cuối E và hướng của khâu thao tác
( bàn kẹp) của Robot đối với hệ quy chiếu cố định
0
R
Như vậy khi biết được các đặc tính hình học của các khâu và quy luật chuyển động
của các khớp là ta có thể xác định được vị trí và hướng của bàn kẹp.
2. Động lực học robot công nghiệp
2.1. Biểu thức động năng và thế năng của Robot
Trong tính toán động học robot, để xác định vị trí các khâu ta chỉ cần sử dụng hệ

( , )
Ci Ci
t=r r q
- Ma trận cosin chỉ hướng của khâu :
( , )
i i
t=R R q
Xét robot có liên kết hôlônôm giữ và dừng, khi đó :
( )
Ci Ci
=r r q
,
( )
i i
=R R q
(2.2)
Theo định nghĩa các ma trận Jacobi tịnh tiến và ma trận Jacobi quay được xác định
bởi công thức :
Ci
Ti
∂
=
∂
r
J
q
,
Ci i
Ri
∂ ∂

Thiết kế và điều khiển mô hình 3D cho Robot RTR
( )
Ci Ci
Ci Ri
d
dt
∂
= = =
∂
φ φ
ω q J q q
q
& &
(2.5)
a. Động năng robot
Biểu thức động năng của vật rắn được xác định bởi biểu thức :

1 1
2 2
T T
i i Ci Ci i i i
T m
= +
v vω I ω
(2.6)
Trong đó
i
I
là ma trận tenxơ quán tính khối của vật rắn đối với hệ quy chiếu đi qua
khối tâm C và song song với hệ quy chiếu cố định. Mối liên hệ giữa ma trận tenxơ

1 1 1
1 1
2 2
p p p
T T
i i Ci Ci i i i
i i i
T T m
= = =
= = +
∑ ∑ ∑
v vω I ω
(2.8)
Thay (1.4) , (1.5) vào (1.8) ta có :

1 1
1 1
( ) ( ) ( ) ( )
2 2
p p
T T
i Ti Ti Ri i Ri
i i
T m
= =
= +
∑ ∑
J q J q J q I J q
& & & &


∑
q J J J A I A J

(2.10)
thì biểu thức động năng robot (2.9) có dạng :

1
( ) ( )
2
T
T M
=
q q q
& &

(2.11)
Ma trận M(q) là ma trận vuông cấp n, và được gọi là ma trận khối lượng suy
rộng của robot.
b, Thế năng trọng lực của Robot.
Thế năng trọng lực mỗi khâu của robot được xác định bởi biểu thức :

0
T
i i Ci
m∏ = − g r

(2.12)
Trong đó :
0
g

i
i i i
d T T
Q
dt q q q
 
∂ ∂ ∂∏
− = − +
 ÷
∂ ∂ ∂
 
&
i = 1,2,3,…,n (2.14)
Ta có phương trình Lagrange loại 2 dạng ma trận:

T T T
d T T
dx
     
∂ ∂ ∂∏
− = +
 ÷  ÷  ÷
∂ ∂ ∂
     
*
f
q q q
&

(2.15)

1 1
( ) ( )
2 2
n n
T
jk j k
j k
T M m q q
= =
= =
∑∑
q q q q
& &
& &
Từ đây ta được các công thức đạo hàm:

ij
1
( )
n
j
j
i
T
m q
q
=
∂
=
∂


=
ij
ij
1 1 1
( )
( )
n n n
j k j
j j k
k
m
m q q q
q
= = =
∂
+
∂
∑ ∑∑
q
q
&& & &
(1)
18
Thiết kế và điều khiển mô hình 3D cho Robot RTR

ij
1 1
( )
1

q q
=
∂
∂∏
= − =
∂ ∂
∑
r
g q
(3)
Thế (1),(2) và (3) vào phương trình ta được:
ij ij
ij
1 1 1 1 1
( ) ( )
1
( ) ( )
2
n n n n n
j k j j k j i
j j k j k
k j
m m
m q q q q q g
q q
τ
= = = = =
∂ ∂
+ − + =
∂ ∂

q q
&&
Ta đưa vào kí hiệu:

ij ij
1 1
( ) ( )
1
( , )
2
n n
i k j
j k
k j
m m
b q q
q q
= =
 
∂ ∂
= −
 
∂ ∂
 
 
∑∑
q q
q q
&
Do ma trận M(q) là ma trận đối xứng nên

= +
∂ ∂ ∂
q q q
(2.18)
Thay (2.18) vào phương trình (2.17) ta nhận được:

ij
1 1
1
( , )
2
n n
jk
i k j
j k
k j
m m
b q q
q q
= =
 
∂ ∂
= −
 ÷
 ÷
∂ ∂
 
∑∑
q q
&

ij
ki ik
k j j k k i
j k j k j k
k i j
m
m m
q q q q q q
q q q
= = = = = =
 
∂
   
∂ ∂
= =
 ÷
 ÷  ÷
 ÷
∂ ∂ ∂
   
 
∑∑ ∑∑ ∑∑
q
q q
& & & & & &
Cuối cùng ta nhận được:

ij
1 1
1

j k
k j i
m m
m
q q
q q q
= =
 
∂ ∂
∂
= + −
 ÷
 ÷
∂ ∂ ∂
 
∑∑
& &

1 1
n n
ijk k j
j k
h q q
= =
=
∑∑
& &
Trong đó ta định nghĩa:
ij
1

+ + =
∑ ∑∑
q q q
&& & &
(2.19)
Biến đổi phương trình vi phân chuyển động của robot
20
Thiết kế và điều khiển mô hình 3D cho Robot RTR
Để nhận được các phương trình vi phân chuyển động của robot dạng quen biết
trong các sách về robot ở các nước, từ phương trình ….ta thực hiện một số biến đổi
sau.
Ta đưa vào kí hiệu :

1
( , ) ( )
n
ij ijk k
k
c h q
=
=
∑
q q q
&
&
(2.20)
Từ đó suy ra:

1 1 1
( , )


( ) ( , ) ( )q C q g
τ
+ + =M q q q q
&
&& &
(2.23)
Trong đó:
n n
ij
m
×
 
= ∈
 
M R
là ma trận khối lượng suy rộng,
n n
ij
c
×
 
= ∈
 
C R
là ma trận ly tâm –Coriolis.
Thành phần
( , )C qq q
&
&

& &
tương đương với hiệu ứng Coriolis ( Coriolis effect)
Một số tính chất của phương trình vi phân chuyển động:
- Ma trận khối lượng là ma trận định dương, đối xứng:
Do động năng của robot là đại lượng vô hướng không âm
1
( )
2
T
T q= q M q
&
&
nên
( )M q
là ma trận định dương. Mặt khác do
i
I
là ma trận đối xứng, nên có thể
chứng minh được
T
Ti Ti
J J
và
T
Ri i Ri
J I J
là các ma trận đối xứng, nên
( )M q
là ma trận
đối xứng.

N M C q q
q q q q
= =
 
∂ ∂ ∂
 
∂
= − = − + −
 ÷
 ÷
 ÷
∂ ∂ ∂ ∂
 
 
∑ ∑
q q q q q
&
& &
& &
=
ij
1
m
ij jk
ik
k
k
k k j i
m m m
m

 ÷
 ÷
∂ ∂
 
∑
&
Bằng cách hoán vị hai chỉ số
,i j
ta được:
1 1
( , ) ( , )
m m
jk jk
ik ik
ij k k ji
k k
j i j i
m m
m m
N q q N
q q q q
= =
   
∂ ∂
∂ ∂
= − + = − − + = −
 ÷  ÷
 ÷  ÷
∂ ∂ ∂ ∂
   

l
chiều
η
để phương trình Lagrange loại 2 của robot có
thể biểu diễn dưới dạng:

( ) ( , ) ( ) ( , , )g+ + =M q q C q q q q Y q q qη
&& & & & &&
(2.24)
Ma trận
( , , )Y q q q
& &&
được gọi là ma trận hồi quy, còn vecto
η
được gọi là vecto tham
số.
Thứ nguyên của không gian tham số, tức là số lượng các tham số cần thiết để có thể
viết phương trình vi phân chuyển động dưới dạng (2.24) là không duy nhất. Trong
trường hợp tổng quát vật rắn được mô tả bởi 10 tham số:
- 3 tham số vị trí khối tâm
( , , )
j
C
ξ ς η
.
- 1 tham số khối lượng
j
m
- 6 tham số momen quán tính (
3 3

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.
Robot phẳng 3 bậc tự do
Đối tượng khảo sát là một tay máy phẳng 3 bậc tự do với 2 khớp quay và một khớp
tịnh tiến chuyển động trong mặt phẳng thẳng đứng. Để thuận tiện cho việc tính toán
thiết kế, robot được mô hình hóa như Hình 1. Các hệ tọa độ Decard gắn vào mỗi
khâu của tay máy được đặt theo quy ước hệ tọa độ của Denavit-Hartenberg. Từ đó
dễ dàng xác định được các tham số động học Denavit-Hartenberg :
, , ,
i i i i
d a
θ α
. Kết
quả được ghi lại ở bảng 1.

24
x
0
z
0
x
1
y
0
z
2
x
2
z
3

π
2
d
0
90
o
3
3
θ
0
3
a
0
o
25
Hình 3.1 Mô hình robot phẳng 3 bậc tự do RTR