Professional Documents
Culture Documents
Cho vật rắn B và hệ qui chiếu R0= {e1(0) , e2(0) , e3(0) } . Trong đó e1(0) , e2(0) , e3(0)
r r r r r r
là ba vector đơn vị trên các trục Ox0,Oy0,Oz0. Ta gắn chặt vào vật rắn một hệ
r r r r r r
qui chiếu R= {e1 , e2 , e3} với e1 , e2 , e3 là ba vector đơn vị trên các trục
Ax,Ay,Az (Hình 2.1).
Z0 Z
Z1
Y
e3
e2
A Y1
(0)
B
e3 X1 e1
O X
(0) Y0
e2
(0)
X0 e1
Hình 2.1
Định nghĩa : Ma trận vuông cấp ba
r r r r r r
⎡e1(0) .e1 e1(0) .e2 e1(0) .e3 ⎤
⎢r r r r r r ⎥
A = ⎢e2(0) .e1 e2(0) .e2 e2(0) .e3 ⎥ (2.1)
⎢er3(0) .er1 er3(0) .er2 er3(0) .er3 ⎥
⎣ ⎦
đượ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 R0.
Nếu ta đưa vào ký hiệu :
r r r r
aij = ei(0) .ei = cos(ei(0) , ei ) , (i,j = 1,2 3) (2.2)
Thì ma trận cosin chỉ hướng (2.1) có dạng:
⎡ a11 a12 a13 ⎤
A = ⎢ a21 a22 a23 ⎥ (2.3)
⎢ ⎥
⎢⎣ a31 a32 a33 ⎥⎦
-1-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
Từ định nghĩa trên, trong hệ qui chiếu R0 ta có các hệ thức liên hệ:
r r r r
e1 = a11e1(0) + a12e2(0) + a13e3(0)
r r r r
e2 = a21e1(0) + a22e2(0) + a23e3(0) (2.4)
r r (0) r (0) r (0)
e3 = a31e1 + a32e2 + a33e3
r
Nếu ta ký hiệu ei là ma trận cột gồm các phần tử của vector ei trong hệ
qui chiếu R0
⎡ a11 ⎤ ⎡ a12 ⎤ ⎡ a13 ⎤
e1 = ⎢ a21 ⎥ , e 2 = ⎢ a22 ⎥ , e3 = ⎢ a23 ⎥ (2.5)
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ a31 ⎥⎦ ⎢⎣ a32 ⎥⎦ ⎢⎣ a33 ⎥⎦
Thì ma trận cosin chỉ hướng (2.3) có dạng:
A=[e1,e2,e3] (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=[e1,e2,e3]
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.AT=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:
a112 + a21
2
+ a31
2
=1 a11a12 + a21a22 + a31a32 = 0
a + a + a =1
2
12
2
22
2
32 , a11a13 + a21a23 + a31a33 = 0
a132 + a23
2
+ a33
2
=1 a12 a13 + a22 a23 + a32 a33 = 0
Do vậy chỉ có ba thành phần của ma trận cosin chỉ hướng là độc lập.
b) Tính chất 2: Định thức của ma trận cosin chỉ hướng det(A)=1.
Từ hệ thức A.AT = E ta suy ra:
det(A.AT) = det(A).det(AT) = det(E) = 1
Do : det(A) = det(AT) nên to có det(A) = ±1 . Ta có thể chứng minh
det(A) = 1.
-2-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
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 R0 và R có cùng gốc O. Trong đó hệ qui chiếu R0 ≡
Ox0y0x0 là hệ qui chiếu cố định, hệ qui chiếu R ≡ Oxyz gắn liền với vật rắn
B. Lấy một điểm Puuubất
r kỳ thuộc vật rắn B. Vị trí của điểm P được xác định
r
bởi vector định vị OP = rP . (Hình vẽ 2.2)
Z
Z0
(0)
Y0
e3 P
e3
e2 Y
e1 (0)
e2
X0 (0)
e1
B
X
Hình 2.2
Ký hiệu các tọa độ của điểm P trong hệ qui chiếu động Oxyz là xP, yP, zP,
các tọa độ của điểm P trong hệ qui chiếu cố định Ox0y0z0 là xP(0) , yP(0) , z P(0) .
Ta có các hệ thức sau :
r r r r
rP = xP(0) .e1(0) + yP(0) .e2(0) + z P(0) .e3(0) (2.7)
r r r r
rP = xP .e1 + yP .e2 + zP .e3 (2.8)
Thế các biểu thức (2.4) vào hệ thức (2.8) ta được :
r r r r
rP = xP (a11.e1(0) + a21.e2(0) + a31.e3(0) ) +
r r r
yP (a12 .e1(0) + a22 .e2(0) + a32 .e3(0) ) + (2.10)
r r r
z P (a13 .e1(0) + a23 .e2(0) + a33 .e3(0) )
Hay :
r r
rP = (a11.xP + a12 . yP + a33 .z P )e1(0) +
r
(a31.xP + a32 . yP + a33 .z P )e2(0) + (2.11)
r
(a31.xP + a32 . yP + a33 .zP )e3(0)
-3-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
O Y
ϕ
Ψ
X
Hình 2.3
Các phép quay quanh trục x, y, z của hệ tọa độ vuông góc Oxyz được gọi là
phép quay cơ bản.
Ta tìm ma trận quay của phép quay quanh trục x0 một góc ϕ (Hình 2.4).
-4-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
Z0
Z
Y
e3(0)
e3 e2
ϕ Z0
e2(0)
O
Hình 2.4
⎡1 0 0 ⎤
A x 0 (ϕ ) = ⎢ 0 cos ϕ − sin ϕ ⎥ (2.15)
⎢ ⎥
⎢⎣ 0 sin ϕ cos ϕ ⎥⎦
Ma trận (2.15) được gọi là ma trận quay của phép quay cơ bản quanh trục x0.
Bằng cách tương tự, ta xác định được các ma trận quay cơ bản quanh các
trục y0 và z0 (Hình 2.5)
⎡ cosψ 0 sinψ ⎤ ⎡cosθ − sin θ 0⎤
A y 0 (ψ ) = ⎢ 0 1 0 ⎥ , A z 0 (θ ) = ⎢ sin θ cosθ 0 ⎥ (2.16)
⎢ ⎥ ⎢ ⎥
⎢⎣ − sinψ 0 cosψ ⎥⎦ ⎢⎣ 0 0 1 ⎥⎦
Từ các công thức (2.15) và (2.16) ta dễ dàng tính được:
det Ax 0 (ϕ ) = det Ay 0 (ψ ) = det Az 0 (θ ) (2.17)
-5-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
Y0 Z0
Y Z
X
e2(0) e3(0)
e2 e1 e3
θ X0 X0
e1(0) e1(0)
O O Ψ
e1 X
Hình 2.5
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.
-6-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
⎡ a1 ⎤ ⎡b1 ⎤ ⎡ a1 + b1 ⎤
a+b= ⎢⎢ a2 ⎥⎥ + ⎢⎢b2 ⎥⎥ = ⎢⎢ a2 + b2 ⎥⎥ (2.19)
⎢⎣ a3 ⎥⎦ ⎢⎣b3 ⎥⎦ ⎢⎣ a3 + b3 ⎥⎦
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:
⎛ a1 + b1 ⎞ ⎛ 1 0 0 a1 ⎞ ⎛ b1 ⎞
⎜ ⎟ ⎜ ⎜ ⎟
⎜ a + b ⎟ = ⎜0 1 0 a2 ⎟⎟ ⎜ b2 ⎟
2 2
(2.20)
⎜ a3 + b3 ⎟ ⎜ 0 0 1 a3 ⎟ ⎜ b3 ⎟
⎜ ⎟ ⎜ ⎟⎜ ⎟
⎝1 ⎠ ⎝0 0 0 1 ⎠ ⎝1 ⎠
Z0 Z
P
SAP
rP e2 Y
e3
(0)
e
3 A
rA e1 Y0
e O
(0)
1
e2(0)
X0 X
Hình 2.6
r r r
rP = rA + s AP (2.21)
Phương trình (2.21) có thể viết dưới dạng ma trận như sau:
⎛ xP(0) ⎞ ⎛ x (0)
A
⎞ ⎛r r r13 ⎞ ⎛ sx ⎞
⎜ (0) ⎟ ⎜ (0) ⎟ ⎜ 11 12 ⎜ ⎟
⎜ yP ⎟ = ⎜ y A ⎟ + ⎜ r21 r22 r23 ⎟⎟ ⎜ s y ⎟ (2.22)
⎜ z (0) ⎟ ⎜ z (0) ⎟ ⎜ r r r33 ⎟⎠ ⎜⎝ sz ⎟⎠
⎝ P ⎠ ⎝ A ⎠ ⎝ 31 32
-7-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
⎛ cosψ 0 sinψ 0⎞
⎜ 0 1 0 0 ⎟⎟
Ay0(ψ)= ⎜ (2.26)
⎜ − sinψ 0 cosψ 0⎟
⎜ ⎟
⎝ 0 0 0 1⎠
⎛ cosθ − sin θ 0 0⎞
⎜ sin θ cosθ 0 0 ⎟⎟
Az0(θ)= ⎜ (2.27)
⎜ 0 0 1 0⎟
⎜ ⎟
⎝ 0 0 0 1⎠
Ngoài ra ta đưa vào khai niệm ma trận tịnh tiến thuần nhất có dạng:
-8-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
⎛1 0 0 a⎞
⎜0 1 0 b ⎟⎟
T(a,b,c)= ⎜ (2.28)
⎜0 0 1 c⎟
⎜ ⎟
⎝0 0 0 1⎠
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.
Z0
Z
θ
Y
Y0
O
Ψ
X0
ϕ X
K
Hình 2.7
Ta đưa vào các ký hiệu sau :
- Góc giữa trục Ox0 và OK là ψ
- Góc giữa trục Oz0 và Oz là θ
- Góc giữa trục OK và Ox là ϕ
Ba góc ψ ,θ ,ϕ đượ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.
-9-
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
Khi xác định vị trí của vật rắn bằng các góc Euler, ta có thể quay hệ qui
chiếu cố định Ox0y0z0 sang hệ qui chiếu động Oxyz bằng ba phép quay Euler
như sau (Hình 2.8):
Z0≡ Ζ1 Y Y2
Z1
ωΨ Z ≡ Z2
Z2
Y0 ωϕ
Y
1
θ
Y0 O
O Y1
ωθ
X0
Ψ
X1 ≡ Κ ϕ X
X ≡ X1
X2
Hình 2.8
- Quay hệ qui chiếu R0 ≡ Ox0y0z0 quanh trục Oz0 một góc ψ để trục Ox0
chuyển tới đường nút OK. Với phép quay này, hệ Ox0y0z0 chuyển sang hệ
Ox1y1z1 với Oz0 ≡ Oz1.
- Quay hệ qui chiếu R1 ≡ Ox1y1z1 quanh trục Ox1 ≡ OK một góc θ để trục
Oz0 ≡ Oz1 chuyển tới trục Oz2 ≡ Oz. Như thế hệ qui chiếu Ox1y1z1 chuyển
sang hệ qui chiếu Ox2y2z2 với Ox1 ≡ Ox2 ≡ OK.
- Quay hệ qui chiếu R2 ≡ Ox2y2z2 quanh trục Oz2 ≡ Oz một góc ϕ để trục
Ox2 ≡ OK chuyển tới trục Ox. Với phép quay này hệ qui chiếu Ox2y2z2
chuyển sang hệ qui chiếu Oxyz với Oz2 ≡ Oz.
Như thế, bằng phép quay Euler quanh trục Oz0 một góc ψ , quanh trục OK
một góc θ , quanh trục Oz một góc ϕ , hệ qui chiếu Ox0y0z0 chuyển sang hệ
qui chiếu Oxyz.
Các ma trận quay ứng với các phép quay Euler có dạng:
⎡ cosψ − sinψ 0⎤
A z 0 (ψ ) = ⎢ sinψ cosψ 0⎥ (2.30)
⎢ ⎥
⎢⎣ 0 0 1 ⎥⎦
⎡1 0 0 ⎤
A K (θ ) = ⎢0 cosθ − sin θ ⎥ (2.31)
⎢ ⎥
⎢⎣0 sin θ cosθ ⎥⎦
- 10 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
⎡ cos ϕ − sin ϕ 0⎤
A z (ϕ ) = ⎢ sin ϕ cos ϕ 0⎥ (2.32)
⎢ ⎥
⎣⎢ 0 0 1 ⎦⎥
Bây giờ ta xác định ma trận quay hệ qui chiếu Ox0y0z0 sang hệ qui chiếu
Oxyz (cũng là ma trận cosin chỉ hướng của hệ qui chiếu Oxyz đối với hệ qui
chiếu Ox0y0z0). Ta lấy P là một điểm bất kỳ của vật rắn B và ký hiệu AE là
ma trận quay nêu trên.
Theo công thức (2.13) ta có :
⎡ xP(0) ⎤ ⎡ xP ⎤
⎢ (0) ⎥ ⎢ ⎥
⎢ y P ⎥ = A E ⎢ yP ⎥ (2.33)
⎢ z (0) ⎥ ⎢⎣ zP ⎥⎦
⎣ P ⎦
Ta ký hiệu xP( i ) , yP(i ) , z P(i ) là tọa độ của điểm P trong hệ qui chiếu Ri ≡ Oxiyizi
(i=1,2). Theo công thức (2.13) ta cá các hệ thức sau:
Từ đó ta suy ra :
⎡ xP(0) ⎤ ⎡ xP ⎤
⎢ (0) ⎥ ⎢y ⎥
⎢ yP ⎥ = A Z 0 (ψ ) A K (θ ) A Z (ϕ ) ⎢ P⎥ (2.35)
⎢ z (0) ⎥ ⎢⎣ zP ⎥⎦
⎣ P ⎦
So sánh các biểu thức (2.33) và (2.35) ta suy ra biểu thức ma trận cosin chỉ
hướng :
A E = A Z 0 (ψ ) A K (θ ) A Z (ϕ ) (2.36)
Hay:
⎡ cosψ − sinψ 0 ⎤ ⎡1 0 0 ⎤ ⎡ cos ϕ − sin ϕ 0⎤
AE= ⎢ sinψ cosψ ⎥ ⎢
0 0 cosθ − sin θ ⎥ ⎢ sin ϕ cos ϕ 0 ⎥ (2.37)
⎢ ⎥⎢ ⎥⎢ ⎥
⎢⎣ 0 0 1 ⎥⎦ ⎢⎣0 sin θ cosθ ⎥⎦ ⎢⎣ 0 0 1 ⎥⎦
- 11 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
AE =
⎡ 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 :
⎡ a11 a12 a13 ⎤
A = ⎢ a21 a22 a23 ⎥ (2.39)
⎢ ⎥
⎢⎣ a31 a32 a33 ⎥⎦
Ta có:
a23 a
cos(ψ ) = − ,sin(ψ ) = 13 (2.40)
sin(θ ) sin(θ )
a32 a
cos(ϕ ) = ,sin(ϕ ) = 31
sin(θ ) sin(θ )
- 12 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
X
Z
Yaw Ψ
Roll ϕ
Pitch θ
Hình 2.9
Roll là chuyển động lắc của thân tàu, tương đương với việc quay thân tàu
một góc ϕ quanh trục z.
Pitch là sự bồng bềnh, tương đương với góc quay θ quanh trục y.
Yaw là sự lệch hướng, tương đương với phép quay một góc ψ quanh trục
x.
Xác định thứ tự quay : quay một góc ψ quanh trục x, tiếp theo là quay một
góc θ quanh trục y và sau đó là quay một góc ϕ quanh trục z.
Theo thứ tự đó có thể biểu diễn phép quay RPY như sau:
A RPY = A Z (ϕ ) A Y (θ ) A Z (ψ ) (2.41)
Với :
⎡1 0 0 ⎤
⎢
A X (ψ ) = 0 cosψ − sinψ ⎥
⎢ ⎥
⎢⎣0 sinψ cosψ ⎥⎦
⎡ cosθ 0 sin θ ⎤
A Y (θ ) = ⎢ 0 1 0 ⎥
⎢ ⎥
⎢⎣ − sin θ 0 cosθ ⎥⎦
⎡ cos ϕ − sin ϕ 0⎤
A Z (ϕ ) = ⎢ sin ϕ cos ϕ 0⎥
⎢ ⎥
⎢⎣ 0 0 1 ⎥⎦
Thay vào công thức (2.41) ta được :
- 13 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
c
B
O
R0 Y0
X0
Hình 2.10
r r r
r Mặt khác tích có hướng của hai vector có dạng a × c = b . Trong đó vector
r r
b vuông góc với mặt phẳng chứa hai vector a và c . Những nhận xét đó gợi
ý cho chúng ta xây dựng khái niệm vector vận tốc góc của vật rắn như sau.
Định nghĩa:
Vận tốc góc của vật rắn B đối với hệ qui chiếu R0 là một vector, ký hiệu là
r
ω sao cho :
- 14 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
R0 r
dc r r
=ω×c (2.43)
dt
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à
r r
duy nhất. Thật vậy, giả sử ω không duy nhất, sẽ tồn tại vector ω ' mà:
R0 r
dc r r
= ω '× c (2.44)
dt
Lấy biểu thức (2.43) trừ đi biểu thức (2.44) ta được
r r r r r r r
ω × c − ω '× c = (ω − ω ') × c = 0 (2.45)
r
Do c là một vector tuỳ ý khác không thuộc vật rắn B và do phương trình
r
(2.45) luôn thoả mãn với mọi c nên ta phải có hệ thức
r r r r
ω −ω'= 0 ⇒ ω =ω'
Chú ý :
Z0 Z
Y
e3
e2
B
e1
R X
Y0
X0
Hình 2.11
Ta gắn chặt vào vật rắn một hệ qui chiếu { x, y, z} với các vector đơn vị
r r r r T
e = [ e1 , e2 , e3 ] trên các trục x,y,z tương ứng (Hình 2.11)
Sử dụng ký hiệu :
r
ω = [ω1 , ω2 , ω3 ] , trong đó ω1 , ω2 , ω3 là hình chiếu của vector ω trên
T
3 trục x,y,z :
r r
ω = ωT . e (2.46)
- 15 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
r
Ma trận đối xứng lệch của vector ω có dạng :
⎡ 0 −ω3 ω2 ⎤
% = ⎢ ω3
ω 0 −ω1 ⎥ (2.47)
⎢ ⎥
⎢⎣ −ω2 ω1 0 ⎥⎦
2.6.1 Quan hệ giữa ma trận cosin chỉ hướng và vận tốc góc của vật rắn
Vật rắn B chuyển động trong hệ qui chiếu cố định OX0Y0Z0. Lấy D là một
điểm nào đó thuộc vật rắn B. Gắn chặt vào vật rắn B hệ qui chiếu động
DXYZ. Lấy P là một điểm bất kỳ thuộc vật rắn B như hình vẽ (Hình 2.12).
Z
Z0 P
SP Y
rP D
B
X
O
rD Y0
X0
Hình 2.11
r r
Gọi vP và vD là vận tốc của điểm P và điểm D trên hệ cố định R0.
A là ma trận cosin chỉ hướng của vật rắn B đối với hệ R0.
Từ hình vẽ ta có:
r r r
rP = rD + sP (2.48)
Đạo hàm phương trình (2.48) trong hệ qui chiếu cố định R0 ta được:
R0 r r r
drP R0 drD R0 dsP
= + (2.49)
dt dt dt
Theo công thức định nghĩa vận tốc góc của vật rắn B (2.43) ta có:
R0 r
dsP r r
= ω × sP
dt
Thay vào công thức (2.49) :
- 16 -
Chương 2 : Cơ sở lý thuyết chung về phân tích động học vật rắn không gian
r r r r
vP = v D + ω × s P (2.50)
Biểu diễn (2.50) dưới dạng ngôn ngữ đại số :
R0
vP =R0 vD + ω
% .R 0 s P (2.51)
Mặt khác ta biểu diễn phương trình (2.48) dưới dạng đại số:
R0
rP = R 0 rD + R 0 sP (2.52)
Do A là ma trận cosin chỉ hướng của vật rắn B nên:
R0
s P = A.s P (2.53)
Vậy (2.52) ⇔ R0
rP = R 0 rD + A.sP (2.54)
Đạo hàm phương trình (2.54) theo thời gian t ta được:
R0
v =R 0 v + A& .s (2.55)
P D P
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:
s P = A −1.R 0 s P = A T .R0 s P (2.56)
Thay (2.56) vào (2.55) ta được:
R0
v =R0 v + A & .A T .R 0 s (2.57)
P D P
- 17 -