Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 1 -
CƠ SỞ LÝ THUYẾT CHUNG VỀ PHÂN TÍCH ĐỘNG HỌC VẬT
RẮN KHÔNG GIAN

2.1 Ma trận cosin chỉ hướng
2.1.1 Định nghĩa ma trận cosin chỉ hướng của vật rắn.
Cho vật rắn B và hệ qui chiếu R
0
=
{
}
(0) (0) (0)
123
,,eee
r
rr
. Trong đó
(0)
1
e
r
,
(0)
2
e
r
,
(0)
3
e

Hình 2.1
Định nghĩa : Ma trận vuông cấp ba

=A
(0) (0) (0)
11 12 13
(0) (0) (0)
21 22 23
(0) (0) (0)
31 32 33
eeee ee
eeee ee
eeee ee
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎣⎦
rrrr rr
rrrr rr
rrrr rr
(2.1)
được gọi là ma trận cosin chỉ hướng của vật rắn B đối với hệ qui chiếu R
0
.
Nếu ta đưa vào ký hiệu :


2
e
3
e
1
e
2
X
Z
Y
X
B
A
0
0
Y
Z
0
Z
1
Y
1
X
1
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 2 -
Từ định nghĩa trên, trong hệ qui chiếu R
0
ta có các hệ thức liên hệ:


1
=e
11
21
31
a
a
a
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎣⎦
,
2
=
e
12
22
32
a
a
a
⎡
⎤
⎢
⎥
⎢
⎥
⎢

3
] (2.6)
Ma trận cosin chỉ hướng A còn được gọi là ma trận quay của vật rắn.

2.1.2 Một vài tính chất cơ bản của ma trận cosin chỉ hướng
a) Tính chất 1: Ma trận cosin chỉ hướng là ma trận trực giao.
Theo công thức (2.6) :
A=[e
1
,e
2
,e
3
]
Vậy ma trận cosin chỉ hướng A là ma trận cột có ba cột là ba vector trực
chuẩn. Do đó A là ma trận trực giao.
Hệ quả: Trong 9 thành phần của ma trận cosin chỉ hướng có 3 thành
phần độc lập.
Do tính chất của ma trận cosin chỉ hướng là ma trận trực giao nên
A.A
T
=E. Từ đó nhận được 6 phương trình liên hệ giữa các thành phần của
ma trận cosin chỉ phương như sau:

222
11 21 31
222
12 22 32
222
13 23 33

T
= E ta suy ra:
det(
A.A
T
) = det(A).det(A
T
) = det(E) = 1
Do : det(
A) = det(A
T
) nên to có det(A) =
1
±
. Ta có thể chứng minh
det(
A) = 1.
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 3 -
c) Tính chất 3 : Ma trận cosin chỉ hướng có ít nhất một trị riêng
1
1
λ
=
.

2.1.3 Ý nghĩa của ma trận cosin chỉ hướng của vật rắn
Xét hai hệ qui chiếu R
0
và R có cùng gốc O. Trong đó hệ qui chiếu R

0
y
0
z
0
là
(0)
P
x
,
(0)
P
y ,
(0)
P
z .
Ta có các hệ thức sau :

(0) (0) (0) (0) (0) (0)
123

PP P P
rxe ye ze=++
rr r r
(2.7)

123

PP P P
rxeyeze=++


(0)
11 12 33 1
(. . .)
PPPP
raxayaze=++ +
rr(0)
31 32 33 2
(. . .)
PPP
ax ay aze++ +
r
(2.11)

(0)
31 32 33 3
(. . .)
PPP
ax ay aze++
r

e
3
(0)
e
2
(0)


(0)
11 12 33

P
PPP
x
ax ay az=++(0)
31 32 33

P
PPP
yaxayaz=++ (2.12)

(0)
31 32 33

P
PPP
zaxayaz=++

Hệ phương trình (2.12) có thể viết lại dưới dạng ma trận như sau :

(0)
11 12 13
(0)
21 22 23

⎢
⎥
⎢⎥
⎣⎦
⎣
⎦
⎣⎦
(2.13)
Từ hệ phương trình (2.13) ta rút ra kết luận sau : Ma trận cosin chỉ hướng
A biến đổi các tọa độ của điểm P bất kỳ thuộc vật rắn trong hệ qui chiếu
động Oxyz sang các tọa độ của điểm P đó trong hệ qui chiếu cố định Ox
0
-
y
0
z
0
.

2.2 Các ma trận quay cơ bản
Ta qui ước hướng quay dương là hướng quay ngược chiều kim đồng hồ
như hình vẽ (Hình 2.3).
eeee ee
ϕ
⎡⎤
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
rrrr rr
rrrr rr
rrrr rr
(2.14) 0
()
ϕ
x
A
=
10 0
0cos sin
0sin cos
ϕ
ϕ
ϕ
ϕ
⎡⎤
⎢⎥
−

⎢⎥
−
⎣⎦
,
0
()
θ
=
z
A
cos sin 0
sin cos 0
001
θ
θ
θθ
−
⎡
⎤
⎢
⎥
⎢
⎥
⎢
⎥
⎣
⎦
(2.16)
Từ các công thức (2.15) và (2.16) ta dễ dàng tính được:


2.3 Các tọa độ thuần nhất và ma trận biến đổi tọa độ thuần nhất
Khái niệm toạ độ thuần nhất được Denavit Hartenberg đưa ra năm 1955,và
hiện nay đang được dùng rất rộng rãi trong tính toán động học robot.

2.3.1 Các toạ độ thuần nhất
Định nghĩa: Cho X={x
1
,x
2
, x
n
} là một điểm trong không gian n chiều R
n
.Tập
hợp (n+1) phần tử (y
1
,y
2
, y
n
,y
n+1
) với (y
n+1
≠0) và:

12
12
11 1
; ; ;


Nhờ khái niệm toạ độ thuần nhất trong không gian 4 chiều ta có thể chuyển
bài toán cộng ma trận cột trong không gian ba chiều sang bài toán nhân ma trận
trong không gian bốn chiều. Cho
a
r
và
b
r
là hai vector trong không gian ba
chiều, ta có:
e
e
O
2
θ
1
(0)
1
e
X
0
0
Y
Y
2
(0)
e
X
O

⎢⎥⎢⎥⎢ ⎥
⎢⎥⎢⎥⎢ ⎥
+
⎣⎦⎣⎦⎣ ⎦
(2.19)
Ta chuyển phép tính cộng (2.19) bằng phép tính nhân hai ma trận như sau:

11 1
1
22 2
2
3
33 3
100
010
001
000 1
11
ab b
a
ab b
a
a
ab b
+
⎛⎞ ⎛⎞
⎛⎞
⎜⎟ ⎜⎟
⎜⎟
+

Hình 2.6 P
AAP
rrs=+
rrr
(2.21)
Phương trình (2.21) có thể viết dưới dạng ma trận như sau:

(0) (0)
11 12 13
(0) (0)
21 22 23
(0) (0)
31 32 33
PA
x
P
Ay
P
Az
xx
s
rrr
yyrrrs
rrr

(0)
e
1
e
3
(0)
e
2
e
1
r
A
e
2
r
P
S
AP
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 8 -
Trong đó R là ma trận cosin chỉ hướng của vật rắn B, ,,
x
yz
s
ss là toạ độ
của vectơ
A
P
s
r

rrrx
s
y
rrr y
rr rz
s
z
⎛⎞
⎛⎞
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
=
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
⎝⎠
⎝⎠
(2.23)
Định nghĩa

.
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:
Các ma trận quay cơ bản (2.15), (2.16) mở rộng ra trong hệ toạ độ thuần nhất
bốn chiều có dạng như sau:
A
x0
(ϕ)=
10 0 0
0cos sin 0
0sin cos 0
00 0 1
ϕϕ
ϕϕ
⎛⎞
⎜⎟
−
⎜⎟
⎜⎟
⎜⎟
⎝⎠
(2.25)
A
y0
(ψ)=
cos 0 sin 0
0100
sin 0 cos 0
0001
ψψ
ψψ

010
001
0001
a
b
c
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
(2.28)
Trong đó ta thực hiện chuyển động tịnh tiến theo trục toạ độ x một doạn là a,
theo trục toạ độ y một đoạn b, theo trục toạ độ z một đoạn c.

2.4 Các góc quay Euler và ma trận quay Euler
2.4.1 Xác định ma trận cosin chỉ hướng từ các góc Euler
Vị trí của vật rắn B quay quanh điểm O cố định được xác định bởi hệ qui
chiếu động Oxyz (gắn chặt vào vật rắn B) đối với hệ qui chiếu cố định
Ox
0
y
0
z
0
(Hình 2.7). Giả sử giao của mặt phẳng Ox
0
y
0

θϕ
được gọi là góc Euler. Như thế, vị trí của vật rắn B đối với
hệ qui chiếu cố định được xác định bởi ba tọa độ suy rộng
,,
ψ
θϕ
. Phương
trình chuyển động của vật rắn quay quanh một điểm cố định có dạng:

(t)
ψ
ψ
=
;
(t)
θ
θ
=
;
(t)
ϕ
ϕ
=
(2.29)
Từ đó suy ra, vật rắn quay quanh một điểm cố định có ba bậc tự do.
K
0
X
X
O

0
z
0
quanh trục Oz
0
một góc
ψ
để trục Ox
0

chuyển tới đường nút OK. Với phép quay này, hệ Ox
0
y
0
z
0
chuyển sang hệ
Ox
1
y
1
z
1
với Oz
0
≡
Oz
1
.
- Quay hệ qui chiếu R

y
2
z
2
với Ox
1
≡
Ox
2
≡
OK.
- Quay hệ qui chiếu R
2
≡
Ox
2
y
2
z
2
quanh trục Oz
2
≡
Oz một góc
ϕ
để trục
Ox
2
≡
OK chuyển tới trục Ox. Với phép quay này hệ qui chiếu Ox

0
()
ψ
=
z
A
cos sin 0
sin cos 0
001
ψ
ψ
ψψ
−
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎣⎦
(2.30)

()
θ
=
K
A
10 0
0cos sin
0sin cos
θ
θ

θ
O
1
Y
Y
0
Z
1
Z
2
θ
Z
≡ Z
2
ω
ϕ
Y
2
Y
X
X
2
ϕ
0
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 11 -

()
ϕ
=

). Ta lấy P là một điểm bất kỳ của vật rắn B và ký hiệu A
E
là
ma trận quay nêu trên.
Theo công thức (2.13) ta có :

(0)
(0)
(0)
P
P
P
x
y
z
⎡⎤
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
E
A
P
P
P
x
y
z
⎡

(2)
(2)
(2)
P
P
P
x
y
z
⎡⎤
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
()
ϕ
Z
A
P
P
P
x
y
z
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎣⎦

⎤
⎢
⎥
⎢
⎥
⎢
⎥
⎣
⎦
,
(0)
(0)
(0)
P
P
P
x
y
z
⎡
⎤
⎢
⎥
=
⎢
⎥
⎢
⎥
⎣
⎦

y
z
⎡⎤
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
0
() () ()
ψ
θϕ
ZKZ
AAA
P
P
P
x
y
z
⎡
⎤
⎢
⎥
⎢
⎥
⎢
⎥
⎣
⎦

(2.37)

Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 12 -
=
E
A
cos cos sin cos sin cos sin sin cos cos sin sin
sin cos cos cos sin sin sin cos cos cos cos sin (2.38)
sin sin sin cos cos
ψ
ϕψθϕ ψϕψθϕ ψθ
ψϕ ψθϕ ψϕ ψθϕ ψθ
θϕ θϕ θ
−−−
⎡⎤
⎢⎥
+−+−
⎢⎥
⎢⎥
⎣⎦

Ma trận cosin chỉ hướng (3.38) được gọi là ma trận quay Euler.

2.4.2 Xác định các góc Euler từ ma trận cosin chỉ hướng
Ma trận cosin chỉ hướng có dạng :

=A
11 12 13
21 22 23

cos( ) ,sin( )
sin( ) sin( )
aa
ϕϕ
θ
θ
==Khi
θ
= n.
π
(n=1,2,3, ) thì
2
cos ( ) 1,sin( ) 0
θθ
=
= việc tính toán sẽ rất
khó khăn, vì thế người ta phải tìm cách xác định vật rắn bởi nhiều loại tham
số khác nhau.

2.5 Phép quay Roll - Pitch - Yaw
Một phép quay khác cũng thường được dùng là phép quay Roll, Pitch và
Yaw, gọi tắt là phép quay RPY.
Hãy tưởng tượng, gắn hệ tọa độ xyz lên thân một con tàu. Dọc theo thân
tàu là trục z (Hình 2.9).
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 13 -


Với :

()
ψ
X
A
10 0
0cos sin
0sin cos
ψ
ψ
ψ
ψ
⎡⎤
⎢⎥
=−
⎢⎥
⎢⎥
⎣⎦()
θ
=
Y
A
cos 0 sin
010
sin 0 cos
θ

X
Y
Z
Yaw
Roll
Pi
tc
h
Ψ
ϕ
θ
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 14 -
=
RPY
A
10 0 cos 0sincos sin0
0 cos sin 0 1 0 sin cos 0
0sin cos sin 0cos 0 0 1
θ
θϕ ϕ
ψψ ϕϕ
ψψ θ θ
−
⎡⎤⎡⎤⎡⎤
⎢⎥⎢⎥⎢⎥
−
⎢⎥⎢⎥⎢⎥
⎢⎥⎢⎥⎢⎥
−

như hình vẽ (Hình
2.10). Lấy
c
r
là một vector tùy ý khác không thuộc vật rắn B. Do
2
()c const=
r
nên đạo hàm theo t biểu thức này ta được .0
dc
c
dt
=
r
r
. Như thế
dc
dt
r
là một vector vuông góc với vector c
r
.

Hình 2.10

Z
0
c
0
O
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 15 -

0
ω
=×
r
r
r
R
dc
c
dt
(2.43)
Chú ý : Vận tốc góc của vật rắn B được định nghĩa bởi biểu thức (2.43) là
duy nhất. Thật vậy, giả sử
ω
r
không duy nhất, sẽ tồn tại vector
'
ω
r
mà:

0

−=
rr

⇒

'
ω
ω
=
r
r

Chú ý :

Hình 2.11

Ta gắn chặt vào vật rắn một hệ qui chiếu
{
}
,,
x
yz với các vector đơn vị
[
]
123
,,
T
eeee=
rrrr
trên các trục x,y,z tương ứng (Hình 2.11)

e
1
e
2
X
Z
Y
X
B
0
0
Y
Z
0
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 16 -
Ma trận đối xứng lệch của vector
ω
r
có dạng :

%
ω
=
32
31
21
0
0
0

v
r
và
D
v
r
là vận tốc của điểm P và điểm D trên hệ cố định R
0
.
A là ma trận cosin chỉ hướng của vật rắn B đối với hệ R
0
.
Từ hình vẽ ta có:

PD P
rrs=+
rrr
(2.48)
Đạo hàm phương trình (2.48) trong hệ qui chiếu cố định R
0
ta được:

000
RRR
P
DP
dr dr ds
dt dt dt
=+
rrr

P
r
D
r
P
S
Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
- 17 -

ω
=+×
r
rr r
P
DP
vv s
(2.50)
Biểu diễn (2.50) dưới dạng ngôn ngữ đại số :

00 0
RR R
=.
PD P
vv s+
%
ω (2.51)
Mặt khác ta biểu diễn phương trình (2.48) dưới dạng đại số:

00 0
RR R

vvAs
&
(2.55)
Vì
A là ma trận cosin chỉ hướng nên là ma trận trực giao. Từ công thức
(2.53) ta suy ra:

00
1

−
==
RR
T
PPP
sA sAs (2.56)
Thay (2.56) vào (2.55) ta được:

00 0
=+
RR R
T
PD P
vvAAs
&
(2.57)
So sánh (2.51) và (2.57) ta có .
T
AA
=