Vectơ ωtlà vận tốc góc của hệ tọa độ quay đối với hệ tọa độ cố định tại thời điểm t... Mục tiêu phần này là tìm mối liên hệ giữa vận tốc thẳng và vận tốc góc của cơ cấu tác động cuối với
Trang 1Động Học Vận Tốc
5.1 Ma trận Skew Symmetric
Ma trận skew symmetric giúp cho việc đơn giản nhiều việc tính toán.
Định nghĩa 1: Một ma trận S được gọi là skew symmetric nếu và chỉ nếu
0
=
S T (5.1)
Từ (5.1), ta thấy rằng các phần tử trên đường chéo s ii =0, và các phần tử ngoài đường chéo s ij =−s ji,i≠ j Vì vậy, S chỉ chứa 3 thành phần độc lập có dạng như sau:
−
−
−
=
0 0 0
1 2
1 3
2 3
s s
s s
s s
S (5.2)
Ta biểu diễn tập hợp mọi ma trận 3x3 skew symmetric bằng ký hiệu SS(3).
Nếu a=(a x,a y,a z)T, ta định nghĩa ma trận skew symmetric S (a)như sau:
−
−
−
=
0 0
0 ) (
3
x y
x z
y
a a
a a
a s a
S (5.3)
Ví dụ 5.1: Ta biểu diễn 3 vectơ tọa độ đơn vị cơ sở j i, và k
T T
T; (0,1,0) ; (0,0,1) )
0 , 0 , 1
i
Theo định nghĩa, ma trận skew symmetric tương ứng là
=
−
=
−
=
0 0 0
0 0 1
0 1 0 ) (
; 0 0 1
0 0 0
1 0 0 ) (
; 0 1 0
1 0 0
0 0 0 )
5.2 Tính chất ma trận skew symmetric:
Trang 2(a) Tính chất 1: tuyến tính (Linearity)
Với mọi vectơ a,b∈R3và 2 số vô hướng α và βta có
) ( ) ( )
S α +β =α +β (5.5)
(b) Tính chất 2:
Với mọi vectơ p=(p x,p y,p z)T
p a p
a) = x (
S (5.6)
y y y
x x x
k j i
) (
) (
) ( 2 3 3 2 3 1 1 3 1 2 2 1 3
2 1
3 2
=
c x
θ
sin
y
x
c =
Nếu R∈SO(3)và a,b∈R3
b a b
R( x )= x (5.7)
Biểu thức (5.7) chỉ đúng khi R là trực giao
Từ (5.6)-(5.7) ta có:
b a
b a
b a
b a b a
) (
) (
) ( ) (
) (
) (
R S R
RR R
R R R RS
T
T T
=
=
=
=
x
x
x
(5.8)
Ta có một kết quả rất quan trọng sau:
) ( )
(a R S Ra
RS T = (5.9)
Vế trái của (5.9) biểu diễn một phép biến đổi tương đương của ma trận S(a); có nghĩa
là, sự biểu diễn ma trận S(a) trong hệ tọa độ được quay bởi R thì giống với ma trận skew symmetric S( aR )tương ứng với vectơ a quay bởi R
Giả sử R=R(θ)∈SO(3) Vì R trực giao với mọi θ ta có:
I R
R(θ) (θ)T = (5.10)
Vi phân (5.10) ta có
Trang 30 )
( )
θ θ θ
dR R R
d
T (5.11)
Định nghĩa ma trận R T
d
dR
θ
= (5.12)
θ θ θ
dR R R
d
dR S
T T
T
= (5.13) Biểu thức (5.11) được viết lại như sau:
0
= +S T
S (5.14)
Vậy ma trận S định nghĩa bởi (5.12) là skew symmetric
Nhân 2 vế của (5.12) cho R cho
) (θ
d
dR
= (5.15)
Biểu thức (5.15) rất quan trọng Nó có ý nghĩa rằng việc tính toán vi phân của ma trận
quay R tương đương với việc nhân ma trận quay đó với một ma trận skew symmetric S
.
Hơn nữa, hầu hết các trường hợp ma trận R là ma trận quay cơ bản hay tích số của các
ma trận quay cơ sở
Ví dụ 5.2: Xét ma trận quay cơ sở R=R x, θ
) ( 0 1 0
1 0 0
0 0 0 cos
sin 0
sin cos
0
0 0
1 sin cos
cos sin
0
0 0
0
i S R
d
dR
−
=
−
−
−
−
=
=
θ θ
θ
θ θ
θ
θ θ
θ
θ
, () x
x S i R d
dR
=
⇒ (5.17)
,
) ( y
y
R j S d
dR
, ( ) z
z S k R d
dR
= (5.18)
Ví dụ 5.3: Xét ma trận quay R quanh trục k bất kỳ, ta có k, θ
Trang 4θ
, ( ) k
k S k R d
dR
= (5.19)
5.3 Vận tốc góc: trường hợp tổng quát
Xét vận tốc góc quay quanh trục tùy ý Giả sử ma trận quay R thay đổi theo thời gian,
nghĩa là, R=R(t)∈SO(3), ta có:
) ( ) ( ) (t S t R t
R = (5.20)
Vì S (t)là skew symmetric, nó có thể biểu diễn như S ( tω( ))đối với vectơ duy nhất ω(t) Vectơ ω(t)là vận tốc góc của hệ tọa độ quay đối với hệ tọa độ cố định tại thời điểm t
) ( )) ( ( ) (t S t R t
R = ω (5.21)
Ví dụ 5.4: Giả sử R(t)=R x,θ t)
) ( )) ( ( ) ( ) (i R t S t R t S
dt
d d
dR
=
⇒ (5.22) trong đó ω=iθ
5.4 Cộng vận tốc góc
Khảo sát vận tốc góc của 2 hệ tọa độ o1x1y1z1và o2x2y2z2 chuyển động tương đối với hệ
tọa độ cố định o0x0y0z0 Ta có
) ( ) ( )
2
0 1
0
2 t R t R t
R = (5.23) Theo kết quả (5.22), vi phân (5.23) ta được
0 2
0 2
1 2
0 1
1 2
0 1
0
2 R R R R S( )R
R = + = ω (5.24)
2 0 1
2
0 1 0 1
2
0
1R S( )R R S( )R
R = ωa = ωa (5.25)
Tính số hạng thứ 2:
0 2 1 0 1
1 2
0 1 1 0 1
1 2
0 1
0 1 1 0 1
1 2 1 0 1
1 2
0 1
) (
) ( )
(
) (
R R S
R R R S R R R S R
R S R R R
b
b
T b b
ω
ω ω
ω
=
=
=
=
(5.26)
2 1 0 1 0
0 2
0
S ω = ωa + ωb (5.27)
Vì S(a)+S(b)=S(a+b)ta có
1 0 1 0 0
2 ωa R ωb
ω = + (5.28)
Trang 5Kết quả trên có thể được mở rộng cho bất kỳ hệ trục tọa độ
1 1 2
0 1
0 = n−
n
R (5.29)
0 0
0 ( n) n
R = ω (5.30) Trong đó 0 = 10 + 10 21+ 20 32+ 30 43+ + 0−1 n−1
n n
ω (5.31)
5.5 Vận tốc thẳng của một điểm gắn vào một hệ tọa độ chuyển động
Giả sử điểm p được gắn cứng vào hệ tọa độ o1x1y1z1và o1x1y1z1quay tương đối với hệ tọa độ o0x0y0z0 Ta có
1 0 1
0 R ( p t)
p = (5.32) Vận tốc điểm đó được tính như sau:
0 0 0 0
1 0 1 0
1 0 1 1 0 1 0
) (
) ( ) (
) ( )
(
p p
S
p t R S
p t R p t R p
x
ω ω
ω
=
=
=
+
(5.33)
Lưu ý: trong biểu thức (5.33), p1 =0 vì tọa độ của điểm đó không đổi đối vớio1x1y1z1 Bây giờ, xét ma trận thuần nhất biểu diễn chuyển động tương đối của o1x1y1z1đối với
0
0
0
0x y z
o phụ thuộc thời gian:
=
1 0
) ( ) ( )
(
0 1
0 1 0
1
t O t R t
H (5.34)
Để đơn giản việc ký hiệu ta bỏ tham số t và các chỉ số
O Rp
p0 = 1+ (5.35)
Vi phân (5.35) ta được
v r
O Rp S
O p R p
+
=
+
=
+
=
x
ω
1
1 0
) ( (5.36)
trong đó r=Rp1là vectơ từ O đến p được biểu diễn theo hướng của hệ tọa độ 1 o0x0y0z0
, và v là tốc độ tại đó gốc O sẽ dịch chuyển.1
5.6 Xây dựng ma trận Jacobian
Trang 6Xét tay máy n khâu với các biến khớp q , ,1 q n Đặt
=
1 0
) ( ) ( )
(
0 0
q
n (5.37)
biểu diễn phép biến đổi từ hệ tọa độ cơ cấu tác động cuối đối với hệ tọa độ nền, và
T n
q
q
q=( 1, , ) là vectơ biến khớp Khi robot chuyển động, q , i O n0(q)và R n0(q)thay đổi theo thời gian Mục tiêu phần này là tìm mối liên hệ giữa vận tốc thẳng và vận tốc góc của cơ cấu tác động cuối với vận tốc khớp q (t) Đặt
T n n
S(ω0)= 0( 0) (5.38) biểu thị vận tốc góc 0
n
ω của cơ cấu tác động cuối, và đặt
0 0
n
v = (5.39) biểu thị vận tốc thẳng của cơ cấu tác động cuối Ta tìm biểu thức có dạng
q J
q J v
n
v n
ω
ω =
=
0
0
(5.40)
hay v J n q
n
0
0
=
ω (5.41)
trong đó J và v J là các ma trận 3xn, và Jacobian ω n
J
J
n0 ∈6x
=
ω
, n là số khâu
Ta sẽ tìm một biểu thức Jacobian đơn giản cho bất kỳ tay máy nào
(1) Vận tốc góc
Đặt ωi i−1là vận tốc góc khâu i tương đối với hệ tọa độ o i− 1x i− 1y i− 1z i− 1gây bởi chuyển
động quay của khớp i
k
i
i i i
i
i =q z− =q
−
1 1
ω (5.42)
Nếu khớp i là khớp trượt, chuyển động của hệ tọa độ i đối với hệ tọa độ (i−1)là chuyển động tịnh tiến, nghĩa là i− 1 =0
i
ω
Vì vậy, vận tốc góc toàn bộ của cơ cấu tác động cuối, ωn0, trong hệ tọa độ nền được xác định như sau:
Trang 7+ + +
i
i i i n
n n
1
0 1
0 1
0 1 2 2 1 1
trong đó ρi =1đối với khớp quay, và ρi =0đối với khớp trượt, vì
k
0
1
0
z
Dĩ nhiên z0 (0,0,1)T
0 = , và phần tử Jω =[ρ1z0 ρn z n−1]
5.6.2 Vận tốc thẳng
Vận tốc thẳng của cơ cấu tác động cuối chỉ là O , ta có n0
i n
n
q
O
∂
=
1
0 0
(5.44)
Cột thứ i của J được cho như sau: v
i
n vi
q
O J
∂
∂
= 0 (5.45)
Cột thứ i của Jacobian có thể tìm được bằng cách giữ tất cả khớp cố định trừ khớp thứ i
và kích hoạt khớp i với vận tốc đơn vị.
Bây giờ ta xét 2 trường hợp: khớp quay và khớp trượt
(1) Trường hợp 1: khớp trượt
=
=
=
=
−
−
−
−
−
−
−
−
−
1 0
1 0 1 0 1 0
1 0
0 1 1 0 1 0
1 1 0 1 1
1 0 1
0 0 0
i
i i i
i n i
o n
i n
i n
i i
i i i
o i
i n
i i i
n n n
O O R O R R
O R O R O R
T T T
T O R
(5.46)
Như vậy 0 = 0 + 0−1 i−1+ i0−1
i i
i n i
O
Nếu chỉ có khớp i chuyển động thì O và n i 0
1
−
i
O là hằng số Hơn nữa, nếu khớp i là khớp
trượt thì ma trận quay R cũng là hằng số Cuối cùng, theo qui ước D-H thì i0−1
T i i i i
i
i
i
i a c a s s d
O− 1=( , , , ) Các ma trận quay R và i0 0
1
−
i
R cũng là hằng số đối với d Ta có i
Trang 80 1
0 1 1
1 1 0
1 0
0
−
−
−
−
=
∂
∂
=
∂
∂
=
∂
∂
i i i
i i
i i
i i
i
i i
i i
i i i i
d
s a
c a d R O R d q
(5.47)
hay J vi =z i− 1
(2) Trường hợp 2: khớp quay
Ta có q i =θi Vì R không là hằng số đối với i0 θi, ta thu được
) (
) )(
(
] )[
(
) ( )
(
0 1 0 0
1
0 1 0 0 1
1 0 1 0
0 1
1 0 1
0 1 0
0 1
1 0
1 0
1 0 1 0
0
−
−
−
−
−
−
−
−
−
−
−
−
−
−
−
−
=
−
=
+
=
+
=
∂
∂ +
∂
∂
=
+
∂
∂
=
∂
∂
i n i
i
i n i i
i i i
i n i i i
i i i i i
i n i i i
i i i i
i n i i
i i i
i n i i
n i
O O z
O O z S
O R O R z S
O R z S O R z S
O R
O R
O R O R O
x
θ θ θ θ θ θ θ θ θ (5.48) Tính 1 0 1 0 1 1 0 1 0 1 1 0 1 0 1 0 1 1 0 1 0 1 0 1 ) ( ) ( ) )( ( ) ( 0 − − − − − − − − − − − − − − = = = = − = ∂ ∂ i i i i i i i i i i i i i T i i i i i i i i i i i i i i i i i i i i O R z S O R k R S O R R k S R O k S R c a s a R d s a c a R θ θ θ θ θ θ
(5.49)
Vì vậy,
)
J x (5.50)
Trang 9Công thức (5.50) được minh họa trên hình 5.1
Hình 5.1: Chuyển động của cơ cấu tác động cuối do khâu i
Kết hợp Jacobian góc và Jacobian thẳng
- Nữa trên của Jacobian J được cho như sau: v
[ v vn]
J = 1 (5.52)
Nếu khớp i là khớp quay, phần tử cột thứ i được tính:
)
J x (5.53)
Nếu khớp i là khớp trượt,
1
−
vi z
J (5.54)
- Nữa dưới của Jacobian J được cho như sau:ω
[J J n]
Jω = ω1 ω (5.55)
Nếu khớp i là khớp quay, phần tử cột thứ i được tính:
1
−
i z
Jω (5.56)
Nếu khớp i là khớp trượt,
0
=
i
Jω (5.57)
Trang 10Như vậy, Jacobian cho tay máy n khâu có dạng như sau:
[J J J n]
J = 1 2 (5.58)
trong đó phần tử cột thứ i :
=
−
−
−
1
1
i
i n i
O O z
J x , nếu khớp i là khớp quay (5.59)
0
1
i i
z
J , nếu khớp i là khớp trượt (5.60)
Công thức trên làm cho việc xác định Jacobian của bất kỳ tay máy nào trở nên đơn giản
vì có sẳn tất cả các đại lượng cần tính một khi bài toán động học thuận tính được Các đại lượng để tính Jacobian chỉ là vectơ đơn vị z và tọa độ của các gốc i O , ,1 O n Kết quả trên rất quan trọng khi ta cần tính vận tốc của khối tâm trên các khâu để tính phương trình động lực học của chuyển động
Ví dụ 5.5: Xét tay máy 3 khâu phẳng (hình 5.2) Tính vận tốc thẳng v và vận tốc góc ω.
Hình 5.2: Tìm vận tốc khâu 2 của robot 3 khâu phẳng.
Ta có
[J J J ]q
v
3 2 1
=
ω (5.61)
trong đó các cột của Jacobian được xác định theo công thức (5.59) và (5.60) với O n
được thay bằng O : c
Trang 11) (
) (
3
1 1
2
0 0
1
=
−
=
−
=
J
O O z J
O O z J
c
c
x
x
(5.62)
Vì vận tốc của khâu 2 không bị ảnh hưởng bởi sự chuyển động của khâu 3 (chỉ xét về
mặt động học) Thật vậy, phản lực của khâu 3 sẽ ảnh hưởng đến sự chuyển động của
khâu 2; vấn đề này sẽ được xét trong phần khảo sát động lực học.
Ví dụ 5.6: Xét tay máy 2 khâu phẳng như hình vẽ
Ma trận Jacobian có dạng như sau:
=
1 0
1 2 1 0 2
) (
z z
O O z O O z q
J x x (5.63)
Hình 5.3: Tay máy 2 khâu phẳng
Các đại lượng trên được tính như sau:
=
0 0
0 0
=
0
1 1
1 1
1 a s
c a
+
+
=
0
12 2 1 1
12 2 1 1
2 a s a s
c a c a
=
=
1 0
0 1
0 z
z (5.64)
Trang 12
−
−
−
−
−
=
⇒
1 1
0 0
0 0
0 0
12 2 12 2 1 1
12 2 12 2 1 1
c a c a c a
s a s
a s a
J (5.65)
Ta thấy rằng 2 hàng đầu tiên của (5.65) là Jacobian vận tốc thẳng của O tương đối với2
nền Hàng thứ 3 là Jacobian vận tốc thẳng theo hướng z , bằng 0 trong trường hợp này.0
Ba hàng cuối biểu diễn vận tốc góc của hệ tọa độ cuối cùng, chỉ đơn giản là phép quay quanh trục thẳng đứng với tốc độ θ1+θ2
Ví dụ 5.7: Tay máy SCARA
Jacobian là một ma trận 6x4 vì SCARA chỉ có 4 bậc tự do Ta cần tính ma trận
j
T0 = 1
Jacobian có dạng sau
=
3 1
0
2 1 4 1 0 4 0
0
0 )
( )
(
z z
z
z O O z O O z
J x x (5.65)
=
0
1 1
1 1
1 a s
c a
+
+
=
0
12 2 1 1
12 2 1 1
2 a s a s
c a c a
− +
+
=
4 3
12 2 1 1
12 2 1 1 4
d d
s a s a
c a c a
O (5.66)
Tương tự z0 =z1=k, và z2 =z3 =−k Vì vậy, Jacobian tay máy SCARA là
−
− +
−
−
−
=
1 0 1 1
0 0 0 0
0 0 0 0
0 1 0
0
0 0
0 0 12 2 12 2 1 1
12 2 12 2 1 1
c a c a c a
s a s
a s a
J (5.67)
5.7 Kỳ dị (Singularities)
Trang 13Jacobian J(q)∈(6xn) xác định một ánh xạ giữa q (vận tốc khớp) và X =( ω v, )T(vận tốc cơ cấu tác động cuối):
q q
X =J( ) (5.68)
(5.68) xác định một phép biến đổi tuyến tính vi phân giữa qd và dX
q q
d = ( ) (5.69) Các vi phân này có thể có thể nghĩ như là việc xác định hướng trong R và 6 R , tương n
ứng
Các vị trí mà tại đó hạng của Jacobian giảm có tầm quan trọng đặc biệt Vị trí đó gọi là
vị trí kỳ dị (singularities hay singilar configuration) Việc xác định điểm kỳ dị cho tay máy là quan trọng vì nhiều lý do:
(1) Các điểm kỳ dị biểu diễn các vị trí tại đó hướng của chuyển động không tính được (2) Tại các điểm kỳ dị, vận tốc bao của cơ cấu tác động cuối có thể tương đương với vận tốc khớp không bị bao
(3) Tại các điểm kỳ dị, lực bao của cơ cấu tác động cuối có thể tương đương với mômen quay của khớp không bị bao
(4) Các điểm kỳ dị thường (không luôn luôn) tương ứng với các điểm trên biên không gian làm việc của tay máy, có nghĩa là, tầm với tối đa của tay máy
(5) Các điểm kỳ dị tương ứng với các điểm trong không gian làm việc tay máy không thể đạt tới khi có sự thay đổi nhỏ của tham số khâu, như chiều dài khâu, độ dời, v.v (6) Gần các điểm kỳ dị sẽ không tồn tại nghiệm duy nhất cho bài toán động học nghịch Trong trường hợp như thế có thể vô nghiệm hay vô số nghiệm
Ví dụ 5.8: Xét phương trình hệ thống hai chiều
q q
=
=
0 0
1 1
(5.70) tương đương với 2 phương trình
0
2 1
=
+
=
dy
dq dq dx
(5.71) 1
)
(J =
rank và ta có thể thấy rằng với bất kỳ giá trị của dq và 1 dq không làm thay đổi2
dy Nên bất kỳ vectơ dXcó thành phần thứ 2 khác không biểu diễn hướng không thể đạt được của chuyển động tức thời
5.8.1 Tách các điểm kỳ dị
Trang 14Ta thấy rằng có thể tính được một tập các phương trình động học thuận cho tay máy bất
kỳ bằng cách gắn cứng một hệ tọa độ vào từng khâu theo cách bất kỳ, tính các phép biến đổi homogenous và nhân chúng lại với nhau Qui tắc D-H chỉ là một cách để thực hiện điều này Dù các phương trình kết quả phụ thuộc vào các hệ tọa độ đã chọn, thì vị trí của tay máy, về bản chất là các đại lương hình học, độc lập với việc gán các hệ tọa độ đó Điều này cho phép ta tách việc xác định các vị trí kỳ dị, cho các tay máy khớp cầu, thành 2 bài toán đơn giản hơn Bài toán 1 là xác định vị trí kỳ dị cho cánh tay, bao gồm
3 khâu hay nhiều hơn, bài toán thứ 2 là xác định kỳ dị cho cổ tay từ chuyển động của cổ tay khớp cầu
Xét tay máy có 3 bậc tự do cho cánh tay và 3 bậc tự do cho cổ tay Trong trường hợp
này, Jacobian là ma trận 6x6 và một vị trí q là kỳ dị nếu và chỉ nếu
0 ) ( detJ q = (5.72) Jacobian có thể viết như sau:
=
=
22 21
12 11
J J
J J J J
J p O (5.73)
Sau đó, vì 3 khớp cuối cùng luôn là khớp quay
=
5 4
3
5 6 5 4 6 4 3 6
z z
z
O O z O O z O O z
J O x x x (5.74)
Vì các trục cổ tay đồng quy tại một điểm chung O , nếu ta chọn các hệ tọa độ sao cho
0 6 5
4
3 =O =O =O =
O , thì J được tính như sau: O
=
5 4 3
0 0 0
z z z
J O (5.75)
Cột thứ i của J là p
=
−
−
−
1
1
i
i i
O O z
J x , nếu khớp i là khớp quay (5.76)
0
1
i i
z
J , nếu khớp i là khớp trượt (5.77)
Ma trận Jacobian có dạng khối tam giác
