Nâng cao chất lượng điều khiển cho robot Scara, chương 7 docx

Chương 7: Động lực học robot
Scara Serpent
Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và
gia t
ốc của đối tượng robot, cần phải xây dựng được phương trình
động lực học, từ đó phục vụ cho công việc thiết kế và điều khiển
robot. Vì vậy cần phải tính toán đầy đủ các thông số của đối tượng
trước khi đưa vào mô phỏng.
Phương tr
ình động lực học của robot được biểu diễn như
sau:
H(q)q h(q,q) g(q) (t)
   
&& &

(2.27)

Trong đó:
- (t) - Vectơ [n x 1] lực động tạo nên ở n khớp động:
(t) = [
1
(t), 
2
(t), , 
n
(t)]
T

(2.28)
- q(t) -
Vectơ [n x 1] biến khớp:

q(t) = [q
1
(t), q
2
(t), q
n
(t)]
T
.
(2.29)
- H(q) - Ma tr
ận [n x n], có các phần tử H
ik
sau đây:
 
n
T
jk ij i ik
i max( j,k )
H Trace U I U



. (j, k =1, 2, n).
(2.30a)
0
i
ij
j
T

U
q




(2.30b)
- h(
q
,
q

) - Vectơ [n x 1] lực ly tâm và Coriolit:
h(
q
,
q

) = [h
1
, h
2
, , h
n
]
T
.
(2.31)
n n
j jkm k m

k 1 m 1
h h q q
 


&&
. (j =1, 2, n)
.
(2.32)




T
ijiikmjkm
UIUTraceh
.
(2.33)
mk
i
0
2
ikm
qq
T
U




(2.34)
- g(q) -
Vectơ [n x 1] lực trọng trường:
g(q) = [g
1
, g
2
, , g
n
]
T
.
(2.35)
 
n
i
j j ij
i j
g m gU r

 

.
(2.36)
V
ới robot Scara Serpent gồm 4 chuyển động (3 chuyển
động quay v
à 1 chuyển động tịnh tiến) và mô phỏng với 3 chuyển
động quay có các phương tr
ình cụ thể sau:

Vectơ [4 1] lực động: (t) = [
1
(t), 
2
(t), 
3
(t), 
4
(t)]
T
.
Vectơ [41] biến khớp: q(t) = [q
1
(t), q
2
(t), d
3
(t), q
4
(t)]
T
.
2.4.1. Hàm Euler - Lagrange và các vấn đề động lực học
Lagrange định nghĩa sự khác biệt giữa động năng và thế năng của
hệ thống: L = K – P
(2.37)
Trong đó: K : là động năng của hệ thống.
P : là tổng thế năng của hệ thống.
Do đó phương tr
ình động lực học được xác định bằng biểu thức:

i
i i
d L L
dt q q
 
  
 
&

(2.38)
Trong đó: q
i
= 
i
đối với khớp quay; q
i
= r
i
đối với khớp tịnh
tiến.

i
: mômen lực động tại khớp thứ i.
Động năng của khớp thứ i:
2 2
i i i i i
1 1
K m .v J .
2 2
  

(2.39)
V
ới J
i
là mômen quán tính của khớp thứ i.
2.4.2. Động lực học robot Scara Serpent
Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 2.4:
Bảng 2.4: Thông số của robot Scara Serpent.
Khớp 1 Khớp 2 Khớp 3 Khớp 4
Biến

1

2
d
3

4
Chiều dài a
1
a
2
d
3
0
Khối lượng m
1
m
2

m
3
m
4
Vận tốc v
1
v
2
v
3
v
4
Chiều dài tâm
kh
ối
l
g1
l
g2
l
g3
l
g4
Giả sử khối lượng nằm ở đầu mút các thanh nối, ta có chiều
dài tâm khối chính là chiều dài của thanh nối: l
g1
= a
1
l
g2

= a
2

l
g3
= d
3
Ký hiệu chiều dài các khớp: a
1
= l
1
a
2
= l
2
d
3
=
l
3
Hệ qui chiếu gắn với trục toạ độ (O
0
x
0
y
0
z
0
) trên khớp thứ
nhất. Khi đó mặt phẳng (O

0
x
0
y
0
) là mặt phẳng đẳng thế.
2.4.2.1. Tính toán động năng và thế năng cho từng khớp
 Khớp 1:
1 g1 1
1 g1 1
1
x l .cos
θ
y l .sin
θ
z 0








.
1 g1 1 1
.
1 g1 1 1
.
1

x l .sin
θ .θ
y l .cos
θ .θ
z 0

 








&
&
Trong đó: x
1
, y
1
, z
1
là hình chiếu của thanh nối số 1 lên các trục x,
y, z.
2 2 2
1 1 1 1
2 2
1 g1 1 1 g1 1 1
2 2 2

1
1 g1
v x y z
v ( l .sin
θ .θ ) (l .cosθ .θ )
v l .θ
  
  

& & &
& &
&
Trong đó: J
1
momen quán tính khớp 1.
1
2 2
1 1 1 1 1
. .
2 2 2
1 1 g1 1 1 1
P 0
1 1
K m .v J .
2 2
1 1
K m .l .
θ J .θ
2 2


  
 

(2.40)
 Khớp 2:








0z
)
θθ.sin(lθ.sinly
)
θθ.cos(lθ.coslx
2
21g2112
21g2112











0z
)
θθ).(θθ.cos(lθ.θ.cosly
)
θθ).(θθ.sin(lθ.θ.sinlx
2
2121g21112
2121g21112







   
221
2
1g21
2
21
2
g2
2
1
2
1
2
2

2
2121g2111
2
2121g2111
2
2
2
2
2
2
2
22
θ)cosθ.θθ.(.l2.l)θθ(.lθ.lv
)
θθ).(θθ.cos(lθ.θ.cosl)θθ).(θθ.sin(lθ.θ.sinlv
zyxv






2 2
2 2 2 2 2
. .
2 2 2 2 2 2
2 2 1 1 g2 1 2 1 g2 1 1 2 2 2 2
2
1 1
K m .v J .

2 2
1 1
K m . l .
θ l .(θ θ ) 2.l .l .(θ θ .θ ).cosθ J .θ
2 2
P 0
  
 
     
 
 

& & & & &

(2.41)
 Khớp 3:








33
212113
212113
-lz
)
θ.sin(θl.sinθly

)
θ.cos(θl.cosθlx
. . . .
3 1 1 1 2 1 2 1 2
. . . .
3 1 1 1 2 1 2 1 2
. .
3 3
x l .sin
θ .θ l .sin(θ θ ).(θ θ )
y l .cos
θ .θ l .cos(θ θ ).(θ θ )
z l

    


   


 


.
2
3
.
2
.
1

.
2
121
2
.
2
.
1
2
2
.
2
1
2
1
.
2
3
.
2
3
2
.
2
.
1212
.
111
2
.

2
.
1212
.
111
2
3
.
2
3
.
2
3
.
2
33
l)θ.θθ.(.l2.l)θθ(.lθ.lv
l)
θθ).(θθ.cos(lθ.θ.cosl)θθ).(θθ.sin(lθ.θ.sinlv
zyxv


















.
2
3 3 3
. . .
. . . .
2 2 2 2 2 2
3 3 1 1 2 1 2 1 2 1 1 2 3
3 3 3 3 3 3
1
K m .v
2
1
K m . l .
θ l .(θ θ ) 2.l .l .(θ θ .θ ) l
2
P m .g.h P m .g.l

 
     
 
 
   

(2.42)
 Khớp 4:








34
212114
212114
-lz
)
θ.sin(θl.sinθly
)
θ.cos(θl.cosθlx











3

.
4
.
2
.
1212
.
111
.
4
.
2
.
1212
.
111
.
4
lz
)
θθ).(θθ.cos(lθ.θ.cosly
)
θθ).(θθ.sin(lθ.θ.sinlx

. . .
2 2 2
4 4 4 4
2 2
.
. . . . . .

2 2
4 1 1 1 2 1 2 1 2 1 1 1 2 1 2 1 2 3
. . . .
. . . .
2 2 2 2 2 2 2
4 1 1 2 1 2 1 2 1 1 2 3
v x y z
v l .sin
θ .θ l .sin(θ θ ).(θ θ ) l .cosθ .θ l .cos(θ θ ).(
θ θ ) l
v l .θ l .(θ θ ) 2.l .l .(θ θ .θ ) l
  
   
         
   
   
     
.
2 2
4 4 4 4 4
. . .
. . . . . . .
2 2 2 2 2 2 2
4 4 1 1 g2 1 2 1 g2 1 1 2 2 3 4 1 2 4
4 4 4
4 4 3
1 1
K m .v J .w
2 2
1 1

K m . l .
θ l .(θ θ ) 2.l .l .(θ θ .θ )cosθ l J (θ θ θ )
2 2
P m .g.h
P m .g.l
 
 
        
 
 


 
2.43

Tổng động năng của hệ thống:
K = K
1
+ K
2
+ K
3
+ K
4

(2.44)
Đặt tâm khối tại đầu mút của thanh nối, ta có: l
g1
= l
1

; l
g2
= l
2
; l
g3
= l
3
.
2 2 2
1 2 3 4 1 2 3 4 2 1 2 4 1
2 2
2 3 4 1 2 2 1 2 3 4 2 2 4 1 2
3 2
2 3 4 1 2 2 1 2 2 3 4 2 2 4 2
3 4
1
K (m m m m ).l (m m m ).l J J J .
θ
2
(m m m ).l .l .cos
θ .θ (m m m )l J J .θ .θ
1
(m m m ).l .l .cos
θ .θ .θ (m m m ).l J J .θ
2
1
(m m ).
2
 

          
 
 
        
 
 
        
 
 
& & &
& & &
2 2
3 4 4 4 1 4 4 2 4
1
l .J .
θ J .θ .θ J .θ .θ
2
  
&
& & & & &

(2.45)
T
ổng thế năng của hệ thống:
P = P
1
+ P
3
+ P
3

+ P
4
= -(m
3
+ m
4
).g.l
3
(2.46)