1
CHƯƠNG I: Cơ sở tính toán
1.1. Ma trận cosin chỉ hướng và ma trận quay của vật rắn
1.1.1. Ma trận cosin chỉ hướng
-Định nghĩa: Cho 2 hệ quy chiếu chung
gôc O:
+ Hệ Oxyz cố định
+ Hệ Ouvw động
Khi đó ma trận cosin chỉ hướng của hệ
quy chiếu B đối với hệ quy chiếu A
định nghĩa như sau:

x x x
A
B y y y
z z z
u i v i w i
u v w
R u j v j w j u v w
u v w
uk vk wk
 
 
 
 
= =
 
 
 
 
 

p u v w p
p u v w p
     
     
=
     
     
     
Hay
A
p =
A
R
B
B
p
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
* Nhận xét : Ma trận cosin chỉ hướng mô tả hướng của hệ quy chiếu
B đối với hệ quy chiếu A. Nó biến đổi tọa độ của điểm P tùy ý trong
hệ quy chiếu động B sang tọa độ của nó trong hệ quy chiếu cố định A
1.1.2. Ma trận quay
- Xét hai hệ quy chiếu chung gốc O liên hệ với nhau bới phép quay
một góc quanh trục z. Gọi p, p’ là vecto tọa độ điểm P trong hệ Oxyz và
Ox’y’z’. Ta có :
cos sin 0 '
sin cos 0 '
0 0 1 '
x x

 
là ma trận
cosin chỉ hướng
- Ma trận cosin chỉ hướng R
z
biểu diễn hướng của một hệ quy chiếu
đối với hệ quy chiếu khác, cũng chính là biểu diễn phép quay một hệ quy
chiếu. Vì vậy thông thường người ta gọi ma trận cosin chỉ hướng là ma
trận quay.
- Các ma trận quay cơ bản (giả thiết các góc quay dương) :
+ Phép quay 1 góc quay trục x
0
:
0
1 0 0
0
0
( )
x
R cos sin
sin cos
α αα
α α
 

−

=
 
 

cos 0
0 0 1
( )
z
sin
R sin
γ
γ γ
γ γ
 
 
=
 
 
 
−
1.2. Định vị, hướng và vị trí của vật rắn
-Vị trí của vật rắn trong không gian
được xác định bởi vị trí của điểm định vị
và hướng của vật rắn đối với hệ quy
chiếu đã chọn. Vị trí của điểm định vị P
xác định bởi 3 thông số. Hướng của vật
rắn đối với hệ quy chiếu cố định A chính
là hướng của hệ quy chiếu động B đối
với A.
- Có nhiều phương án xác định hướng của vật rắn :
+ Phương án 1 : Hướng của B đối với A xác định bởi ma trận cosin
chỉ hướng:
x x x
A

định có thể được mô tả bởi các góc ψ,,
như hình bên . Các góc này là các góc
Euler
- Sử dụng 3 góc Euler ta có thể quay hệ
Ox
0
y
0
z
0
sang hệ Oxyz như sau :
+ Quay hệ quy chiếu Ox
0
y
0
z
0
quanh trục Oz
0
một góc ψ, hệ Ox
0
y
0
z
0

chuyển sang hệ Ox
1
y
1

2
y
2
z
2
chuyển
sang hệ Oxyz
- Hướng của hệ quy chiếu tạo thành được mô tả bởi ma trận tích hợp từ
các ma trận mô tả phép quay thành phần:
R
E
=R
z
0
(ψ) R
ON
(θ) R
z
(φ)=
cos cos sin cos sin cos sin sin cos cos sin sin
sin cos cos cos sin sin sin cos cos cos cos sin
sin sin sin cos cos
ψ ϕ ψ θ ϕ ψ ϕ ψ θ ϕ ψ θ
ψ ϕ ψ θ ϕ ψ ϕ ψ θ ϕ ψ θ
θ ϕ θ ϕ θ
− − −
+ −
 
 
 

ứng:R
CD
= R
x
0
( R
y
1
(R
z
2
(η=
cos cos cos sin sin
sin sin cos cos sin sin sin sin cos cos cos sin
cos sin cos sin sin cos sin sin sin cos cos cos
β η β η β
α β η α η α β η α η β α
α β η α η α β η α η α β
−
+ − + −
−
 
 
 

+
 
+

1.2.3. Các góc Roll-Pitch-Yaw

cos cos cos sin sin sin cos cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
sin cos sin cos cos
ϕ θ ϕ θ ψ ϕ ψ ϕ θ ψ ϕ ψ
ϕ θ ϕ θ ψ ϕ ψ φ θ ψ ϕ ψ
θ θ ψ θ ψ
− +
+ −
−
 
 
 
 
 
1.3. Vận tốc góc và gia tốc góc của vật rắn
1.3.1. Vận tốc góc của vật rắn
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
- Định nghĩa: vận tốc góc của vật rắn là
một vecto mà khi ta nhân nó với một véc
tơ bất kỳ tùy ý khác không thì được đạo
hàm của vecto đó:
dc
c
dt
ω
× =
r
ur r

(n+1) hệ quy chiếu ta có:
1 2

a r r e
ω ω ω ω
= + + +
uur uuur uuur uur
- Công thức cộng gia tốc góc
a r e e r
ε ε ε ω ω
= + + ×
uur uur uur uur uur
Trong đó:
a
ε
uur
là gia tốc góc tuyệt đối của vật rắn
là gia tốc góc tương đối của vật rắn
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
e
ε
uur
là gia tốc góc theo
e r
ω ω
×
uur uur
là gia tốc góc Resal

 
- Cho 2 hệ quy chiếu Oxyz và Quvw
như hình vẽ, ta có :
A
r
P
=
A
r
Q
+
A
R
B
B
s
p
hay
Phương trình trên có cấu trúc không gọn vì ma trận 33 không biểu diễn
cho các phép dịch chuyển tịnh tiến. Nếu sử dụng tọa độ thuần nhất thì
phương trình trên viết lại như sau :
1 0 0 0 1
1
pu
px x x x Qx
pv
py y Qy
pw
pz z
y y

Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
Trong đó
A
T
B
=
0 0 0 1
y y y
z z
x x x Qx
Qy
Qzz
u v w r
u v w r
u v w r
 
 
 
 
 
 
gọi là ma trận biến đổi thuần nhất
1.4.2. Các ma trận quay cơ bản thuấn nhất và ma trận tịnh tiến thuần
nhất
- Ma trận biến đổi tọa độ thuần nhất cho phép quay cơ bản quanh trục x:
A
T
B
(x,)=

 
 
 
 
−
- Ma trận biến đổi tọa độ thuần nhất cho phép quay cơ bản quanh
trục z:
A
T
B
(z,)=
cos sin 0 0
sin cos 0 0
0 0 1 0
0 0 0 1
γ γ
γ γ
−
 
 
 
 
 
 
- Ma trận biến đổi tọa độ thuần nhất phép tịnh tiến:
A
T
B
(x,y,z,a,b,c)=
1 0 0

i
z
i
là hệ tọa độ thuận.
1.5.2. Các tham số động học Denavit-Hartenberg
Vị trí của hệ tọa độ khớp (Oxyz)
i
đối với hệ tọa độ khớp (Oxyz)
i-1

được xác định bởi bốn tham số
i
, d
i
, a
i
,
i
như sau:
-
i
là góc quay quanh trục z
i-1
để trục x
i-1
chuyển đến trục x
’
i
(x
’

i
là góc quay quanh trục z
i
sao cho trục z
’
i-1
(z
’
i-1
// z
i-1
) chuyển đến trục
z
i
.
1.5.3. Ma trận Denavit-Hartenberg
Ta có thể chuyển hệ tọa độ khớp (Oxyz)
i-1
sang hệ tọa độ khớp (Oxyz)
i

bằng bốn phép biến đổi cơ bản như sau:
- Quay quanh trục z
i-1
một góc
i
.
- Dịch chuyển tịnh tiến dọc trục z
i-1
một đoạn d

0 1 0
in c
0
s
0 1
o
i i i
i i i i
i
i
i i i
a
A
d
θ θ
θ θ α α
α α
−
       
       
       
       
       
    
−

=
 
−
1i

Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
CHƯƠNG III: Tính toán động học robot
3.1. Cấu trúc động học robot
Ta có mô hình cấu trúc 3 khâu, 3 khớp quay, 3 bậc tự do (3DOF) như
hình vẽ :
3.2. Thiết lập hệ phương trình động học của robot
3.2.1. Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác
Sử dụng các góc Cardan xác định hướng vật rắn ta xác định ma trận trạng
thái khâu thao tác:
0
3
cos cos cos sin sin
sin sin cos cos sin sin sin sin cos cos cos sin
cos sin cos sin sin cos sin sin sin cos
(
cos co
0
s
)
0 0 1
P
P
P
x
y
A t

=
1 1 1 1
1 1 1 1
cos sin cos
sin cos s
0
0
0 0 1 0
0 0 0 1
in
a
a
θ θ θ
θ θ θ
 
−
 
 
 
 
 
1
A
2
=
2 2 2 2
2 2 2 2
cos sin cos
sin cos s
0

θ θ θ
θ θ θ
 
−
 
 
 
 
 
Suy ra
0
A
3
(q)=
0
.
1
.
2
=
123 123 1 1 2 12 3 123
123 123 1 1 2 12 3 123
0
0
0 0 1 0
0 0 0 1
C S a C a C a C
S C a S a S a S
 
 

Nhiệm vụ chủ yếu của bài toán động học thuận là xác định vị trí và
hướng của khâu thao tác dưới dạng hàm của các biến khớp.
3.3.1. Vị trí điểm thao tác P và hướng của bàn kẹp
Từ hệ phương trình động học ở trên, ta rút ra :
- Vị trí điểm thao tác P :
( ) ( )
( ) ( )
1 1 2 1 2 3 1 2 3
0
1 1 2 1 2 3 1 2 3
cos cos cos
sin sin sin
0
P
P
P
P
x a
ar
a a
y a a
z
θ θ θ θ θ θ
θ θ θ θ θ θ
 
 
 
 
= =
 


- Sử dụng phần mềm maple cho biết
1 2 3
0.5a a a m
= = =
và
[ ]
0,2t
π
=
ta vẽ được đồ thị điểm thao tác P như sau:
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
3.3.2. Vận tốc và gia tốc điểm thao tác P
- Vận tốc điểm thao tác P:
== . =
˙
1
1 1 2 12 3 123 2 12 3 123 3 123
˙
1 1 2 12 3 123 2 12 3 123 3 123 2
˙
3
0 0 0
a S a S a S a S a S a S
a C a C a C a C a C a C
θ
θ
θ

s cos
a a a
a a a
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
− − + + − + + + +
+ + + + + + + +
 
 
 
 
 
 
& & & & & &
& & & & & &
Ở đây gọi là ma trận Jacobian tịnh tiến của khâu thao tác
- Gia tốc điểm thao tác P:
===. +
=
+) =
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
+) =0
3.3.3. Vận tốc góc và gia tốc góc khâu thao tác
- Vận tốc góc khâu thao tác :
=. =
( ) ( )
( ) ( )
123 1 2 3 123 1 2 3



−
& & & & & &
& & & & & &
=
( )
( )
1 2 3
1 2 3
0 0
0 0
0 0 0
θ θ θ
θ θ θ
− + +
+ +
 
 
 
 
 
 
& & &
& & &
=
1 2 3
0
0
θ θ θ

+ +
 
&& && &&
3.3. Tính toán động học ngược robot.
- Nội dung của bài toán động học ngược là xác định chuyển động của các
tọa độ khớp khi đã biết quy luật chuyển động của các tọa độ thao tác.
3.3.1. Bài toán 1
Ở bài toán này, ta giả thiết đã biết x
P
(t), y
P
(t) và (t)=. Nhiệm vụ là xác
định , .
- Ta có hệ phương trình :
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
(1)
Bình phương 2 vế của các biểu thức trên rồi cộng lại ta được:

Từ đó suy ra:
Vậy atan2(, )
Khi đó, ta viết lại (1) dưới dạng :
 (2)
Giải hệ phương trình đại số tuyến tính (2) ta được :
=
==
=
= atan2(
Lại có :=

x a x x y b y y− − + − − =
hay
( ) ( )
2
cos . sin . 0 ( )
P P
R t x x R t y y− + − = ∆
- Ở đây ta giả thiết khâu thao tác luôn chuyển động phía bên ngoài
đường tròn tâm I, bán kính R. Do đó hệ số góc của đường
1
( )
∆
luôn lớn
hơn hệ số góc đường
2
( )
∆
một góc
30
o
=>
0 0
( 90 ) 30t
θ
= + +
=>
120
o
t
θ

) cho biết trước và khâu thao tác luôn tạo với
đường thẳng AB một góc
const
α
=
. Yêu cầu tìm , .
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
- Ta sẽ đưa bài toán này về bài toán 1. Thật vậy, phương trình đường
thẳng AB là:
A A
A B A B
x x y y
x x y y
− −
=
− −
, đưởng thẳng này tạo với trục hoành góc
'
arctan( )
A B
A B
y y
x x
θ
−
=
−
. Ta lại có

= − + − = − + ≈
. Điểm P chuyển động
trên đoạn AB với vận tốc 0.1m/s đi từ A đến B, như vậy quỹ đạo P có
dạng:
0, 1.16t
 
∈
 
- Áp dụng kết quả bài toán 1 ta được (ở đây ta chọn các góc dương):
1
Tính chọn động cơ
tỷ số truyền và thiết kế hộp giảm tốc cho robot
CHƯƠNG IV: Tính toán động lực học robot
4.1. Các đại lượng đặc trưng trong động lực học
4.1.1. Ma trận quán tính.
Ta có :
( )
( )
0
2
0
( )
u
B
T
u
B
I p u p dm
I p u p u p dm
= × ×

 
 
 
 
 
 
 
=
0
.u
Trong đó : J
xx
=
2
2
( )
B
dm
y
z
+
∫
, J
yy
=
2
2
( )
B
dm

yzdm
∫
, J
xz
= J
zx
= -
B
xzdm
∫
Ma trận
0
được gọi là ma trận quán tính hoặc ten xơ quán tính của vật rắn
B đối với điểm O.
4.1.2. Moment động lượng của vật rắn đối với 1 điểm
- Moment động lượng của vật rắn đối với
điểm O nằm ngoài vật rắn được định nghĩa
như sau :
0
( )
d p
d L p dm
dt
= ×
ur
uur ur
Biến đổi ta có :
0
( )
c c c