Tài liệu ROBOT công nghiệp - Chương 7: Động lực học robot doc

Robot công nghiệp
84

chơng VII
Động lực học Robot
(Dynamic of Robot)



7.1. Nhiệm vụ và phơng pháp phân tích động lực học robot

Nghiên cứu động lực học robot là công việc cần thiết khi phân tích cũng nh tổng
hợp quá trình điều khiển chuyển động. Việc nghiên cứu động lực học robot thờng giải
quyết hai nhiệm vụ sau đây :
1/ Xác định momen và lực động xuất hiện trong quá trình chuyển động. Khi đó qui
luật biến đổi của biến khớp q
i
(t) coi nh đã biết.
Việc tính toán lực trong cơ cấu tay máy là rất cần thiết để chọn công suất động cơ,
kiểm tra độ bền, độ cứng vững, đảm bảo độ tin cậy của robot.
2/ Xác định các sai số động tức là sai lệch so với qui luật chuyển động theo chơng
trình. Lúc nầy cần khảo sát Phơng trình chuyển động của robot có tính đến đặc tính động
lực của động cơ và các khâu.
Có nhiều phơng pháp nghiên cứu động lực học robot, nhng thờng gặp hơn cả là
phơng pháp cơ học Lagrange, cụ thể là dùng phơng trình Lagrange - Euler. Đối với các
khâu khớp của robot, với các nguồn động lực và kênh điều khiển riêng biệt, không thể bỏ
qua các hiệu ứng trọng trờng (gravity effect), quán tính (initial), tơng hổ (Coriolis), ly
tâm (centripetal) mà những khía cạnh nầy cha đợc xét đầy đủ trong cơ học cổ điển; Cơ
học Lagrange nghiên cứu các vấn đề nêu trên nh một hệ thống khép kín nên đây là nguyên
lý cơ học thích hợp đối với các bài toán động lực học robot.

7.2. Cơ học Lagrange với các vấn đề động lực của robot.

Hàm Lagrange của một hệ thống năng lợng đợc định nghĩa :
L = K - P (7.1)
Trong đó : K là tổng động năng của hệ thống
P là tổng thế năng
K và P đều là những đại lợng vô hớng nên có thể chọn bất cứ hệ toạ độ thích hợp
nào để bài toán đợc đơn giản. Đối với một robot có n khâu, ta có :
và
KK
i
i
n
=

=1
PP
i
i
n
=

=1
ở đây, K
i
và P
i
là động năng và thế năng của khâu thứ i xét trong hệ toạ độ chọn.Ta
biết mỗi đại lợng K
i

và P
i
là một hàm số phụ thuộc nhiều biến số:
K
i
= K(q
i
, ) và P
i
q
&
i
= P(q
i
, )
&
q
i
Với q
i
là toạ độ suy rộng của khớp thứ i. Nếu khớp thứ i là khớp quay thì q
i
là góc
quay
i
, nếu là khớp tịnh tiến thì q
i
là độ dài tịnh tiến d
i
.

Ta định nghĩa : Lực tác dụng lên khâu thứ i (i=1, 2, , n) với quan niệm là lực tổng
quát (Generalized forces), nó có thể là một lực hoặc một momen (phụ thuộc vào biến khớp
q
i
là tịnh tiến hoặc quay), đợc xác định bởi:

F
i
=
d
dt
L
q
L
q
ii




&
(7.2)
TS. Phạm Đăng Phớc
Robot công nghiệp
85
Phơng trình nầy đợc gọi là phơng trình Lagrange-Euler, hay thờng đợc gọi tắt
là phơng trình Lagrange.

7.3. Ví dụ áp dụng :

Xét một robot có hai khâu nh hình vẽ, Các khâu có chiều dài là d
1
và d
2
với các
khối lợng tơng ứng m
1
và m
2
qui đổi về đầu mút của khâu. Robot đợc đặt thẳng đứng
chịu gia tốc trọng trờng g. Các khớp chuyển động quay với các biến khớp
1
và
2
. Tính
lực tổng quát.
Qua ví dụ nầy, chỉ với một mối liên kết hai
khâu, các vấn đề đặt ra đều đã có mặt
trong quá trình nghiên cứu động lực học,
và do đó, ví dụ nêu trên có thể mở rộng để
áp dụng trong những trờng hợp phức tạp
hơn. Đối với khâu 1 :
m
2
m
1

2

1

g = 9,81m/s
2
y
2
y
1
x
2
x
1
O
0
z
x
y

Kmvmd
111
2
11
2
1
2
1
2
1
2
==
&

(7.3)
P
1
= -m
1
gd
1
cos
1
(7.4)
Đối với khâu 2 :
Về toạ độ :
x
2
= d
1
sin
1
+ d
2
sin(
1
+
2
)
y
2
= -d
1
cos

1
- d
2
cos(
1
+
2
)
Chiều cao thế năng :
h = d
1
cos
1
+ d
2
cos(
1
+
2
)
Về mặt vận tốc :
vxy
2
2
2
2
2
2
=+
&&

Với
&
cos( )
&
cos( )(
&&
)x
d
dt
xd d
2 2 1 112 1212
== + + +

&
sin( )
&
sin( )(
&&
)y
d
dt
yd d
221112121
== + + +
2


[
]
vd d dd

2
2
1
2
1
2
2
2
1
2
12 2
2
12 2 1
2
12
22=++++ +
&
(
&&&&
)cos()(
&&&
)
Động năng và thế năng sẽ là :

[]
Kmvmdd dd
222
2
21
2

1
2
2
2
1
2
12 2
2
12 2 1
2
12
1
2
1
2
22== ++++ +
&
(
&&&&
)cos()(
&&&
) (7.5)

(7.6)
[]
Pmgd d
221121
= + +cos( ) cos( )
2

7.4. Hàm Lagrange và lực tổng quát :
áp dụng hàm Lagrange cho ví dụ trên, ta có :
L = (K
1
+ K
2
) - (P
1
+ P
2
)

L m m d md mdd=+ + +++ +
1
2
1
2
2
121
2
1
2
22
2
1
2
12 2
2
212 2 1

2
12
()
&
(
&&&&
)cos(
&&&
) +

++
+
+
()cos cos(mmgd mgd
121 122 12
)



(7.7)
Khi tính lực tổng quát, các biến của hệ : q
1
=
1
và q
2
=
2
.
Đối với khâu 1 :

L
q
L
mmd md mdd mdd
&
&
()
&
(
&&
)cos
&
cos
&
1
1
121
2
122
2
12 212 21 212 2
2==+ + ++ +

2

TS. Phạm Đăng Phớc
Robot công nghiệp
86
d
dt
L
mmd md mdd mdd



&
()
&&
(
&& &&
)sin
&&
cos
&&
1
121
2
122
2
1 2 212 221 212 21
22=+ + + +



+ mdd mdd

212 22
2
212 22
sin
&
cos
&&






L
q
L
mmgd mgd
11
121 122 12
==+ +()sin sin(

)
Vậy :
F
d
dt
LL
mmdmd mdd
md mdd mdd mdd
mmgd mgd

1
1
1
121
2
22
2
212 2 1
22
2
212 2 2 212 221 212 22
2
121122 12
2
2
==+++ +
++
++ + +







&
[( ) cos ]
&&
[cos]
&&

sin
&&
sin
&
()sin sin()
+
(7.8)
Muốn cho khâu 1 quay đợc một góc

1
thì động cơ phải tạo ra một lực tổng quát
F
1
. Lực tổng quát nầy có đặc tính phi tuyến, là hợp tác dụng của nhiều yếu tố (non linear
and cuppling).
Tơng tự, để tính lực tổng quát của khâu thứ hai , ta có :



L
md md mdd
&
&&
cos
&
2
22
2
122
2

2212 2
=++

1

d
dt
L
md md mdd mdd



&
&& &&
cos
&&
sin
&&
2
22
2
122
2
2212 21212 21
=++

2

và
)sin()sin()sin(

2122
2
12212212212
2



+= gdmddmddm
L
&&&

Vậy :

)sin()sin(
]cos[
2122
2
12212
2
2
2212212
2
22
22
2

++
++==
gdmddm
dmddmdm
LL
dt
d
F
&
&&&&
&
(7.9)

Để phân tích ý nghĩa các thành phần trong biểu thức tính lực tổng quát, ta viết lại
các biểu thức F
1
, F
2
nh sau :


FD D D D D D D
1 11 1 12 2 111 1
2
122 2
2
112 1 2 121 1 2 1
=++ + + + +
&& && & & & & & &

FD D D D D D D
2 12 1 22 2 211 1
2
222 2
2
212 1 2 221 1 2 2
=++ + + + +
&& && & & && &&

Hiệu ứng Hiệu ứng Hiệu ứng Hiệu ứng
quán tính ly tâm tơng hổ trọng trờng
Effective inertias Centripetal effect Coriolis effect Gravity

(Trong đó : D
111
= 0; D
222
= 0; D
112
= D
121
= D
212
= D
221
=-m
2
d

1
d
2
sin
2
)

Trong các biểu thức trên, các hệ số dạng D
ii
hoặc thể hiện hiệu ứng quán tính tại
khớp i hoặc j gây ra bởi gia tốc tại khớp i hoặc j. Các số hạng có dạng
ij
D
2
ijj
D
j

&
là lực ly tâm
tác động lên khớp i gây ra bởi vận tốc tại khớp j. Số hạng dạng
là lực
Cariolis tác động lên khớp thứ i gây ra do vận tốc tại khớp j và k. Số hạng có dạng D
jkkj

&&&&
ikjijk
DD +
i
là lực

trọng trờng tác động lên khớp i.


TS. Phạm Đăng Phớc
Robot công nghiệp
87
7.5. Phơng trình động lực học robot :

Xét khâu thứ i của một robot có n khâu. Tính lực tổng quát F
i
của khâu thứ i với
khối lợng vi phân của nó là dm. Lực tổng quát F
i
đóng vai trò rất quan trọng khi xây dựng
sơ đồ khối để thiết lập hàm điều khiển cho robot có n bậc tự do.

7. 5. 1.
Vận tốc của một điểm trên robot :
Một điểm trên khâu thứ i đợc mô tả trong hệ toạ độ cơ bản là :
r = T
i
.
i
r (7.10)
Trong đó :
i
r là toạ độ của điểm xét đối với khâu thứ i,
i
r không thay đổi theo thời
gian. T

i
là ma trận chuyển đổi từ khâu thứ i về hệ toạ độ gốc : T
i
= A
1
A
2
A
i
. Nh vậy r là
một hàm của thời gian t.
Tốc độ của vi khối lợng dm

đợc tính bởi công thức :

&
r
dr
dt
d
dt
Tr
T
q
q
i
i
i
j
j

i
j
i
== =








=



1
&
r (7.11)

Khi tính bình phơng của vận tốc nầy ta có :

(7.12)
&
.
&
(
&
,
&

,
&
)(
&&
)rr rxyz Trrr
ooo
T
==

2














z
x
y
i
r
dm

Khâu i
O
0
T
i
r
Hình 7.1. Khảo sát tốc độ của vi khối lợng dm.

Với r
T
là chuyển vị vectơ và Tr là viết tắt của Trace (vết của ma trận) :

Trace
aa a
aa a
aaaa
a
n
n
nn nn
ii
i
n

11 12 1
21 22 2
1211
1

=
=

Hay :


[]

2
2
2
y
x
= zyx .
zz
y
x
Do vậy

&
(
&
.
&
)( .rTrrr Tr
d
dt
Tr

d
dt
Tr
T
i
i
i
T
iT2
== )
TS. Phạm Đăng Phớc
Robot công nghiệp
88
=








==

Tr
T
q
qr
T
q

qr
i
j
j
i
i
T
k
k
iT
k
i
j
i




&
.
&
11

=

==
i
j
kj
k
T
i
Tii
j
i
i
k
qq
q
T
rr
q
T
Tr
11
.
&&

(7.13)

7. 5. 2.
Tính động năng của vi khối lợng dm.

Ký hiệu K
i
là động năng của khâu thứ i. dK
i
là động năng của vi khối lợng dm đặt
tại vị trí
i
r trên khâu thứ i.
dK Tr
T
q
rr
T
q
qq
i
k
i
i
j
iiT
i
T
k
jk

j
i
=








==

1
2
11




.
&&
dm

=

==

1
2
11
Tr
T
q
rdm r
T
q
qq
k
i
i
j
iiT
i
T
k
jk
j
i

(. . ).
&&
(7.14)
Và do đó động năng của khâu thứ i sẽ là :









==




==
i
j
kj
k
T
i
Khau
Tii
j
i
i

k
i
qq
q
T
dmrr
q
T
TrdKK
1
i
1
) (
2
1
&&




i Khau
(7.15)
Đặt
gọi là ma trận giả quán tính (Pseudo inertia matrix).

=
i
Tii
rr.
Khau

i
dmJ
ý nghĩa "giả quán tính" đợc sử dụng vì khi thiết lập đầy đủ các phần tử của ma trận J
i
ta
có thể liên hệ với các khái niệm "mômen quán tính độc cực" và trình bày các phần tử của J
i

giống nh các phần tử của mômen quán tính độc cực. Ta xét mối quan hệ nầy nh sau :
Theo định nghĩa ta có :

= J

=
i
Tii
rr.
Khau
i
dmJ
i
= (7.16)

dmzdmydmxdm
zdmdmzzdmyzdmx
ydmzdmydmyydmx
xdmzdmxydmxdmx
iii
iiiiii
iiiiii
iiiiii
2
2
2
Bây giờ ta nhắc lại mômen quán tính độc cực của
một vật thể bất kỳ nh hình vẽ.
z
y
x

Theo định nghĩa ta có :

+= dmzy

xx
)(I
22




+= dmzx
yy
)I
22

+= dmyx
zz
)(I
22

H
ình 7.2 : Mômen quán tính độc cực
Và vì :
)(
2
1
)(
2
1
)(
2
1
x

2222222
yxzxzy +++++=
Vậy :
; .v.v 2/)I I I(
zzyyxx
2
++=

dmx
Ngoài ra ta còn có :

; ;

= xydm
xy
I

= yzdm
yz
I

= xzdm
xz
I

; ;

= xdmmx

= ydmmy

= zdmmz
TS. Phạm Đăng Phớc
Robot công nghiệp
89
Đối chiếu với ma trận giả quán tính J
i
, ta có thể trình bày J
i
nh sau :


















+
+

++
=
mmzmymx
mz
2
III
II
myI
2
III
I
mxII
2
III
j
zzyyxx
yzyz
zy
zzyyxx
xy
zxyx
zzyyxx
i
(7.17)
Nh vậy ý nghĩa biểu trng của J
i
đã rõ.
Vậy ta có :

=

==
i
j
kj
k
T
i
i
j
i
i
k
i
qq
q
T
J
q
T
TrK
11
2

1
&&




(7.18)
Cuối cùng, Động năng của một robot có n khâu đợc tính :

(7.19)

=
=
n
i
i
KK
1

7. 5. 3. Tính thế năng của robot :

Thế năng của khâu i có khối lợng m
i
, trọng tâm đợc xác định bởi vectơ r
i
(vectơ
biểu diễn trọng tâm của khâu i trong hệ toạ độ cơ bản) là :
P
i
= -m

i
. g. r
i
= -m
i
. g. T
i

i
r
i
(7.20)
Trong đó, vectơ gia tốc trọng trờng g đợc biểu diễn dới dạng một ma trận cột :













=

=
0
8,9
0
0
0
z
y
x
g
g
g
g

Thế năng của toàn cơ cấu robot n khâu động sẽ là :

=
=
n
i
i

i
ii
rgTmP
1
(7.21)

7. 5.4. Hàm Lagrange :
Sau khi xác định động năng và thế năng của toàn cơ cấu, ta có hàm Lagrange của
robot có n bậc tự do :


====
+












=
n
i
i
i

iikj
n
i
i
j
i
k
k
T
i
i
j
i
rgTmqq
q
T
J
q
T
TraceL
1111
2
1
2
1
&&
(7.22)

Chúng ta chú ý rằng, trong hàm Lagrange vẫn cha đề cập đến ảnh hởng của
nguồn truyền động (gồm các phần tĩnh (stator) và phần động (Rotor) của động cơ điện).

7. 5. 5. Phơng trình động lực học robot :

Ta đã biết lực tổng quát đặt lên khâu thứ i của robot có n khâu (Phơng trình
Lagrange - Euler) :
F
i
=
d
dt
L
q
L
q
ii




&
(7.23)
Sau khi thiết lập hàm Lagrange, với p = 1 n, ta tính đợc :
TS. Phạm Đăng Phớc
Robot công nghiệp
90
(p là chỉ số lần lợt lấy theo j và k)

j
n
i

i
j
p
T
i
i
j
i
k
n
i
i
k
k
T
i
i
p
i
p
q
q
T
J
q
T
Trq
q
T
J

q
T
Tr
q
L
&&
&

====












+

=


1111
2
1
2
1
(7.24)
Thay đổi chỉ số giả j thành k trong số hạng thứ hai ,và để ý rằng :













=

=












j
T
i
i
p
i
T

p
T
i
i
j
i
p
T
i
i
j
i
q
T
J
q
T
Tr
q
T
J
q
T
Tr
q
T
J
q
T
Tr

(7.25)
ta có :
k
n
i
i
k
p
T
i
i
k
i
p
q
q
T
J
q
T
Tr
q
L
&
&

==

=


11
(7.26)
Cũng để ý rằng : trong T
i
(q
1
, q
2
, . . . , q
i
), với q
i
là các biến khớp của i khớp đầu tiên. Do
vậy, nếu
i < p thì
0=

p
i
q
T
.
Cuối cùng ta có :
k
n
pi
i
k
p
T
i
i
k
i
p
q
q
T
J
q
T
Tr
q
L
&
&

==












=


1
(7.27)
Lấy vi phân theo thời gian t của phơng trình trên :

k
n
pi
i
k
p
T
i
i
k

i
p
q
q
T
J
q
T
Tr
dt
d
q
L
dt
d
&
&

1

==

=




+












+

=

=====
mqq
q
T
J
qq
T
Trq
q
T
J
q
T
Tr
k
n
pi
i
k
i
m
p
T
i

i
mk
i
k
n
pi
i
k
p
T
i
i
k
i
&&&&

11
2
1


mqq
q
T
J
qq
T
Tr
k
n

pi
i
k
i
m
k
T
i
i
mp
i
&&

===












+
11
2
(7.28)

(Biến đổi theo chú ý (7.25))
Số hạng cuối của phơng trình Lagrange Euler là :

+












=



===
k
n
pi
i
j
i
k
k
T

i
i
pj
i
p
qq
q
T
J
qq
T
Tr
q
L
&&
j
11
2

2
1

i
i
n
pi
p
i
ik
n

i
i
j
j
i
k
j
T
i
i
pk
i
r
q
T
gmqq
q
T
J
qq
T
Tr

====


+

+
&&
111
2

2
1
(7.29)

Cuối cùng ta có lực tổng quát của khâu p :
pp
p
q
L
q
L
dt
d
F

=
&


Thay thế các chỉ số p và i thành i và j, ta sẽ có :

j
j
n
ij
i
j
jk
n
ij
j
k
j
m
i
T
j
j
mk
j
k
n
ij

j
k
i
T
j
j
k
j
i
r
q
T
gmmqq
q
T
J
qq
T
Trq
q
T
J
q
T
TrF

======

+












=
&&&&

11
2
1

(7.30)
Với một robot có n bậc tự do thì :
TS. Phạm Đăng Phớc
Robot công nghiệp
91
q = [q
1
, q
2
, . . . ,q
n
]
T
q =
&
[
]
n21
q , ,q ,q
&&&
T
và F = F[F
1
, F
2
, . . . , F
n
]
T

Để cho gọn, ta biểu diễn :

)(),()( qGqqqCqq
J
F
+
+
=
&&&&
(7.31)

Trong đó :
J thể hiện tác dụng của quán tính, là một ma trận đối xứng (n x n);
C thể hiện tác dụng của lực ly tâm và Cariolis, là một vectơ (n x 1);
G thể hiện tác dụng của lực trọng trờng, cũng là một vectơ (n x 1).
Đây là phơng trình động lực học của robot.

Nếu thêm vào phơng trình trên các tác dụng khác nh : F
EX
đặc trng cho các
ngoại lực tác dụng lên trục, V đặc trng cho hiệu ứng ma sát, ta có :


EX
FqVqGqqqCqqJF
+
+
+
+= )()(),()(
&&&&&

(7.32)
TS. Phạm Đăng Phớc