ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ RRR

ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
PHẦN 1: THIẾT KẾ MÔ HÌNH 3D ROBOT RRR
Sử dụng phần mền solidwork để thiết kế mô hình 3D của robot
RRR
1.thiết kế các khâu:
Khâu đế:

Khâu 1:


Khâu 2:

Khâu 3:


2.Các chi tiết khác

Bánh răng

trục bánh răn

Nắp bộ truyền bánh răng

Xylanh khí nén


3.các bộ truyền động:
Khớp 1:

Bộ truyền bánh răng

Khớp 2:


Bộ truyền bánh răng
Khớp 3:

Bộ truyền bánh răng

Tay kẹp:


Sử dụng xylanh khí nén

PHẦN 2: TÍNH TOÁN ĐỘNH HỌC THUẬN VÀ NGƯỢC ROBOT

Cấu hình robot RRR
1.bảng tham số DH
a.Xây dựng hệ tọa độ DH:
•

Hệ tọa độ :

Gốc tọa độ đặt tại tâm của khớp động thứ nhất
Trục dọc theo hướng của trục khớp động thứ nhất,hướng từ dưới lên
Trục có phương vuông góc với hướng từ ngoài vao trong như hình vẽ
Trục xác định theo quy tắc tam diện thuận
•

Hệ tọa độ :

Gốc tọa độ đặt tại tâm của khớp động thứ 2


Trục phương dọc theo trục khớp động thứ 2 hướng từ trong ra ngoài như
hình vẽ
Trục có phương nằm trên đường vuông góc chung của trục và hướng từ
tới
Trục xác định theo quy tắc tam diện thuận
•

Hệ tọa độ :

Gốc tọa độ đặt tại tâm của khớp động thứ 3
Trục phương dọc theo trục khớp động thứ 2 hướng từ trong ra ngoài như
hình vẽ
Trục có phương nằm trên đường vuông góc chung của trục và hướng từ
tới
Trục xác định theo quy tắc tam diện thuận
•

Hệ tọa độ :

Gốc tọa độ đặt tại tâm của tay kẹp
Trục phương dọc theo trục khớp động thứ 2 hướng từ trong ra ngoài như
hình vẽ
Trục có phương nằm trên đường vuông góc chung của trục và hướng từ
tới
Trục xác định theo quy tắc tam diện thuận
b.cách xác định các tham số động học DH
vị trí của hệ tọa độ khớp đối với hệ tọa độ khớp được xác định bởi 4 tham

số , , , như sau:
: góc quay quanh trục để trục trùng với trục ()
: dịch chuyển tịnh tiế dọc trục để gốc tọa độ chuyể đến là giao điểm của
trục và trục
: dịch chuyển dọc trục để điểm chuyển đến điểm
: góc quay quanh trục sao cho trục () trùng với trục


c.bảng tham số động học DH

1
2
3
•

0
0
0

90ᶱ
0
0

Các ma trận truyền:

Ma trận truyền khâu i so với khâu i-1:
 cos q1

sin q1
0

A1 = 
0

 0

0 sin q1
0 − cos q1
1
0
0
0

 cos q3

sin q3
2
A3 = 
0

 0

− sin q3
cos q3
0
0

0
÷
0÷

l1 ÷
÷
1 ÷

 cos q2

sin q2
1
A2 = 
0

 0

− sin q2
cos q2
0
0

0 l2 cos q2 
÷
0 l2 sin q2 ÷
1
0 ÷
÷
0
1 ÷

0 l3 cos q3 
÷
0 l3 sin q3 ÷

1
0 ÷
÷
0
1 ÷

Ma trận truyền khâu 2 và 3 so với khâu 0:
 cos q1 cos q2

sin q1 cos q2
0
0 1
A2 = A1 A2 = 
sin q2

0


− cos q1 sin q2 sin q1 l2 cos q1 cos q2 
÷
− sin q1 sin q2 − cos q1 l2 sin q1 cos q2 ÷
cos q2
0
l2 sin q2 + l1 ÷
÷
÷
0
0
1


 cos q1 cos(q2 + q3 ) − cos q1 sin(q2 + q3 ) sin q1 l3 cos q1 cos(q2 + q3 ) + l2 cos q1 cos q2 

÷
sin q1 cos( q2 + q3 ) − sin q1 sin(q2 + q3 ) − cos q1 l3 sin q1 cos(q2 + q3 ) + l2 sin q1 cos q2 ÷
0
0
2

A3 = A2 A3 =
 sin(q2 + q3 )
÷
cos(q2 + q3 )
0
l3 sin(q2 + q3 ) + l2 sin q2 + l1

÷
0
0
0
1



2.bài toán động học thuận
Các thong số đầu vào :q1, q2, q3


Thông số cần xác định: điểm tác động cuối P=( Px, Py,Pz),hướng của khâu
cuối

•

Tọa độ điểm tác động cuối:

Px = 0 A3 1,4 = l3 cos q1 cos(q2 + q3 ) + l2 cos q1 cos q2
Py = 0 A3  2,4 = l3 sin q1 cos(q2 + q3 ) + l2 sin q1 cos q2
Pz = 0 A3 3,4 = l3 sin(q2 + q3 ) + l2 sin q2 + l1
•

0

Hướng của khâu thao tác có ma trận cosin chỉ hướng:

 cos q cos(q + q )
1
2
3

R3 =  sin q1 cos(q2 + q3 )

sin(q2 + q3 )


− cos q1 sin(q2 + q3 ) sin q1 
÷
− sin q1 sin(q2 + q3 ) − cos q1 ÷
cos(q2 + q3 )
0 ÷

3.bài toán động học ngược

Thông số đầu vào là tọa độ điểm tác động cuối: P=( Px; Py; Pz) và hướng
khâu cuối
Thông số cần xác định:các biến khớp (q1; q2; q3)
Ta có ma trận cấu hình của khâu thao tác:
 a11 ( x) a12 ( x) a13 ( x) a14 ( x) 

÷
a21 ( x) a22 ( x) a23 ( x) a24 ( x) ÷

T3 ( x) = 
a ( x) a32 ( x) a33 ( x) a34 ( x) ÷
 31
÷
÷
0
0
0
1



Trong đó , , là các tọa độ của điểm thao tác E:
=

xE(0)

,=

y E(0)

,=

z E(0)

Còn là các phần tử của ma trậ cosin chỉ hướng của khâu thao tác.
Từ bài toán động học thuận ta có các ma trận:


 cos q1

sin q1
0
A1 (q) = 
0

 0

0 sin q1
0 − cos q1
1
0
0
0

 cos q3

sin q3
2
A3 = 
0


 0

0
÷
0÷

 cos q2

sin q2
1
A2 (q) = 
0

 0

l1 ÷
÷
1 ÷

− sin q3
cos q3
0
0

0 l3 cos q3 
÷
0 l3 sin q3 ÷
1
0 ÷

÷
0
1 ÷

− sin q2
cos q2
0
0

0 l2 cos q2 
÷
0 l2 sin q2 ÷
1
0 ÷
÷
0
1 ÷

(2)

 cos q1 cos(q2 + q3 ) − cos q1 sin(q2 + q3 ) sin q1 l3 cos q1 cos(q2 + q3 ) + l2 cos q1 cos q2 

÷
sin q1 cos(q2 + q3 ) − sin q1 sin(q2 + q3 ) − cos q1 l3 sin q1 cos(q2 + q3 ) + l2 sin q1 cos q2 ÷
0
0 1 2

T3 ( x) = A3 = A1 A2 A3 =
(3)
 sin(q + q )

÷
cos(
q
+
q
)
0
l
sin(
q
+
q
)
+
l
sin
q
+
l
2
3
2
3
3
2
3
2
2 1

÷÷

0
0
0
1



cho các phần tử (1,4) và (2,4) của hai ma trận (1) và (3) bằng nhau ta đươc:

(
(

)
)

cos q l cos(q + q ) + l cos q = a = P
P
P
14
x
1 3
2
3
2
2

⇒ tan q1 = y ⇒ q1 = arctan  y

Px
sin q1 l3 cos(q2 + q3 ) + l2 cos q2 = a4 = Py

 Px


÷
÷


Từ phương trình ma trận (3) ta suy ra ;
H1 ( q1 ) H 2 (q2 ) = D3 ( x ) ( H 3 (q3 ) )

 C3 S3
 −S C
3
 3
 0
0

−1
0
H 3 (q3 )  0
=

−1

trong đó :

0 −l3 
0 0 ÷
÷
1 0 ÷

÷
0 1 

 C3a11 − S3a12
C a − S a
−1
3 22
D3 ( x) ( H 3 (q3 ) ) =  3 21
 C3a31 − S3a32

0


(8)
S3a11 + C3a12
S3a21 + C3a22
S3a31 + C3a32
0

a13
a23
a33
0

−l3a11 + a14 
−l3a21 + a24 ÷
÷
−l3a31 + a34 ÷
÷
1



(9)


Mặt khác ta có :
−C1C2
− S1S 2
C2
0

 C1C2
SC
 1 2
 S2

H1 (q1 ) H 2 ( q2 ) =  0

S1 l2C1C2 
−C1 l2 S1C2 ÷
÷
0
l2 S2 ÷
÷
0
1 

(10)

So sánh các phần tử ở các ô (1,4) , (2,4 ) và (3,4) của các ma trận (9) và (10)

ta được :
l2C1C2 = −l3a11 + a14

(11)

l2 S1C2 = −l3a21 + a24

(12)

l2 S2 = −l3a31 + a34

(13)

Bình phương hai vế của phương trình (11) và (12) rồi cộng lại ta được :
l22C22 = ( −l3a11 + a14 ) + ( −l3a21 + a24 )
2

2

(14)

Từ đó suy ra :
l2C2 = ±

( −l3a11 + a14 )

2

+ ( −l3a21 + a24 )

2

(15)

Kết hợp (13) và (15) ta có :
tan(q 2) =

S2
=
C2 ±


q2 = arctan 
±


−l3a31 + a34

( −l3a11 + a14 )

2

+ ( −l3a21 + a24 )

−l3a31 + a34

( −l3a11 + a14 )

2

+ ( −l3a21 + a24 )

2

2


÷
÷


So sánh các phần tử (3,1) và (3,2) của hai ma trận (1) và (2 ) ta được :


S23 = a31 C23 = a32 ⇒
,

⇒

q2 + q3 = arctan(

tan( q2 + q3 ) =

S23 a31
=
C23 a32

a31
a
)

q3 = arctan( 31 ) − q2
a32 ⇒
a32

4.tính vận tốc và gia tốc điểm tác động cuối
vận tốc dài và gia tốc dài của điểm tác động cuối được suy ra từ đạo hàm của
véc tơ tọa độ :
•

Vận tốc của điểm tác động cuối:
vE(0) =

R0

d (0)
rE
dt

d
& &
&
&
Px = −a3q&
1 sin q1 cos( q2 + q3 ) − a3 ( q2 + q3 ) cos q1 sin( q2 + q3 ) − a2 q1 sin q1 cos q2 − a2 q2 cos q1 sin q2
dt
d
vPy = Py = a3q&1 cos q1 cos( q2 + q3 ) − a3 (q&2 + q&3 )sin q1 sin( q2 + q3 ) + a2 q&1 cos q1 cos q2 − a2 q&2 sin q1 sin q2
dt
d
vPz = Pz = a3 (q&2 + q&3 ) cos(q2 + q3 ) + a2 q&2 cos q2

dt
vPx =

•

gia tốc của điểm tác động cuối:
a

(0)
E

=

R0

d (0)
vE
dt

5.vận tốc góc và gia tốc góc các khâu
•

các ma trận cosin chỉ hướng của các khau so với R0:

khâu 1:
 cos q1 0 sin q1 

÷
0
R1 =  sin q1 0 − cos q1 ÷

 0
1
0 ÷



khâu 2:
0

 cos q1 cos q2

R2 =  sin q1 cos q2
 sin q
2


− cos q1 sin q2 sin q1 
÷
− sin q1 sin q2 − cos q1 ÷
cos q2
0 ÷

khâu 3:

0

 cos q1 cos(q2 + q3 ) − cos q1 sin(q2 + q3 ) sin q1 

÷
R3 =  sin q1 cos(q2 + q3 ) − sin q1 sin(q2 + q3 ) − cos q1 ÷


sin(q2 + q3 )
cos(q2 + q3 )
0 ÷

•

Vận tốc góc các khâu:

tính vận tốc góc khâu thứ i của robot dựa trên công thức tính vận tốc góc
của vật rắn thong qua ma trận cosin chỉ hướng của nó:
0 &0 T
ω%
i = Ri R i

(sử dụng phần mềm maple)
Khâu 1:
0
 0 −q&
1 0
 ÷

÷
0 &0 T
&1 0 0 ÷ ⇒ ω1 =  0 ÷
ω%
1 = R1 R 1 =  q
0
 q&÷
0 0 ÷


 1

Khâu 2:

0 &0 T
ω%
2 = R2 R 2


0

=
q&
1

 cos q q&
1
2


(

Khâu 3:

)

−q&
1
0

( sin q1 ) q&2

− ( cos q1 ) q&2 

(

)

 sin q q& 
1
2 ÷
÷

− sin q1 q&2 ÷⇒ ω2 =  − cos q1 q&2 ÷
÷

÷
÷

÷
&
q
÷

÷
0
1





(

)

(

)


0 &0 T
ω%
3 = R3 R 3


0

=
q&
1

 cos q ( q& + q&)
1
2
3


(

)

(

)

− ( cos q1 ) ( q&2 + q&3 ) 

−q&
1

÷

− ( sin q1 ) (q&2 + q&3 ) ÷

0

( sin q1 ) (q&2 + q&3 )

÷
÷
÷


0

 sin q ( q& + q&) 
1
2
3 ÷


⇒ ω3 =  − cos q1 (q&2 + q&3 ) ÷

÷

÷
q&
÷
1



(

)

gia tốc góc các khâu:

•

khâu 1:
0
dω1  ÷
γ1 =
= 0÷
dt
 q&
÷
1
&

R0

khâu 2:
&
&&
 q&
2 sin q1 + q1q2 cos q1 
dω 2 
÷
&
&&
γ2 =
=  q&
2 cos q1 − q1q2 sin q1 ÷
dt

÷
&
q&
1


R0

khâu 3:
&
&
&
& & &
 ( q&

2 + q3 ) sin q1 + q1 ( q2 + q3 ) cos q1 
dω 3 
÷
&
&
&
& & &
γ3 =
=  − ( q&
2 + q3 ) cos q1 + q1 ( q2 + q3 ) sin q1 ÷
dt

÷
&
q&
1


R0

PHẦN 3:ĐỘNG LỰC HỌC CỦA ROBOT
Dạng ma trận của phương trình lagrange II:
Động năng của robot có dạng:
T=

1 T
1
q& M ( q ) q&= q&T b( q, q&)
2
2

Trong đó

b(q, q&) M (q )q& b = [ b1...bn ]

=

,

T


Phương trình lagrange II có dạng :
T

T

T

d  ∂T   ∂T   ∂π 

÷ −
÷ +
÷ =τ
dt  ∂q&  ∂q   ∂q 
T

 ∂T 
∂T
= q&T M (q ) ⇔ 

÷ = M (q )q&
∂q&
 ∂q&
T

d  ∂T 
&+ M&( q) q&
 &÷ = M (q )q&
dt  ∂q 

Sử dụng định lý đạo hàm riêng theo vecto tích của hai ma trận ta có:
∂T 1 ∂
1  ∂q&T
∂b 
=
(∂q&T b) = 
(b × I n ) + q&T
÷
∂q& 2 ∂q&
2  ∂q&
∂q&

(1)

Trong đó:
 b1I n 
 . 
∂q&T
 = b eT I ...b eT I 
(b × I n ) = e1T ...enT  

 .   1 1 n n n n
∂q&


T
= [ b1...bn ] = bT = ( M (q ) q&) = qT M (q )
bn I n 

(2)

Mặt khác:
q&T

 ∂M ( q )
∂b
∂
∂q&
= q&T ( M (q )q&) = q&T 
(q&× I n ) + M (q) ÷ =
∂q&
∂q&
∂q& = q&T ( 0 + M (q ) I n ) = q&T M (q )
 ∂q&

Thay (2) và (3) vào (1) ta được:
T

 ∂T 
∂T
= q&T M (q ) ⇔ 

÷ = M (q )q&
∂q&
 ∂q&

,

Tính toán tương tự ta có:

T

d  ∂T 
&+ M&(q )q&

÷ = M (q) q&
dt  ∂q&

(4)

(3)


∂T 1 ∂
1  ∂q&T
∂b 
=
(∂q&T b) = 
(b × I n ) + q&T
÷=
∂q 2 ∂q
2  ∂q

∂q 

1
T ∂b 
 0 + q& ÷
2
∂q 

=

1 T ∂
1  M (q )
∂q&
q& ( M (q)q&) = q&T 
(q&× I n ) + M ( q) ÷
2 ∂q&
2  ∂q
∂q 

=

 1 M ( q)
1 T  M ( q)
q& 
(q&× I n ) + 0 ÷ = q&T
(q&× I n )
2  ∂q
∂q
 2

(5)

Thay (4) và (5) vào phương trình lagrange II ta được:
T

T

  ∂π 
1  M ( q)
&+ M&( q) q&−  q&T
M ( q) q&
( q&× I n ) ÷ + 
÷ =τ
2
∂q
  ∂q 
∂M (q )
M&( q) =
( I n × q&)
∂q

Đặt

T
 ∂M (q )
 
1  M (q)
v( q, q&) = 
( I n × q&) − 
(q&× I n ) ÷ ÷q&= C ( q, q&)q&

 ∂q
2
∂
q

 ÷



ta có:

T


1  ∂M ( q)
v( q, q&) = M&( q) q&− 
( q&× I n ) ÷ q&
2  ∂q


(6)

Theo định lý đạo hàm toàn phần và đạo hàm riêng ta có:
∂M (q)
M&( q) =
( I n × q&)
∂q

thay vào (6) ta được:

T
 ∂M ( q)

1  M (q)
v(q, q&) = 
( I n × q&) − 
(q&× I n ) ÷
 ∂q
2
∂
q



T


∂M ( q)
1  M ( q)
C (q, q&) =
( I n × q&) − 
(q&× I n ) ÷
∂q
2  ∂q


Ma trận ly tâm và coriolis có dạng:
T

C (q, q&) =


∂M (q )
1  M (q )
( I n × q&) − 
(q&× I n ) ÷
∂q
2  ∂q



÷q&= C ( q, q&)q&
÷



Khi đó phương trình vi phân chuyển đông của các khâu :
T

 ∂π 
&+ C (q, q&)q&+ 
M (q )q&
÷ =τ
 ∂q 

1.bảng tham số động lực học của robot
khâu
1
2
3

Vị trí trọng tâm
0
0
0

0
0
0

Khối
lượn
g

Ma trận mômen quán tính
0
0
0

0
0
0

0
0
0

2.ma trận Jacobi của các khâu
Từ cấu tạo hình học của robot ta xác định được tọa độ khối tâm của các khâu
so với hệ tọa độ cố định :

0
 ÷
rC1 =  0 ÷
l ÷
 C1 

 lC 2 cos θ1 cos θ 2 

÷
rC 2 =  lC 2 sin θ1 cos θ 2 ÷
 l + l sin θ ÷
2 
 1 C2

 l2 cos θ1 cos θ 2 + lC 3 cos θ1 cos(θ 2 + θ3 ) 

÷
rC 3 =  l2 sin θ1 cos θ 2 + lC 3 sin θ1 cos(θ 2 + θ3 ) ÷

÷
l1 + l2 sin θ 2 + lC 3 cos(θ1 + θ 2 )



a.các ma trận Jacobi tịnh tiến của các khâu
 0 sin θ 1 sin θ 1 
∂ω3 
÷
J R3 = & =  0 − cos θ 1 − cos θ ÷
∂θ 

0
0 ÷
1


 −lC 2 sin θ1 cos θ 2

J T2 =
=  lC 2 cos θ1 cos θ 2
∂θ 
0

∂rC2

−lC 2 cos θ1 sin θ 2
−lC 2 sin θ1 sin θ 2
lC 2 cos θ 2

0
÷
0÷
0÷


 −l2 sin θ1 cos θ2 − lC 3 sin θ1 cos(θ 2 + θ3 ) −l2 cos θ1 sin θ 2 − lC 3 cos θ1 sin(θ 2 + θ 3 ) −lC 3 cos θ1 sin(θ 2 + θ 3 ) 

÷
J T3 =
=  l2 cos θ1 cos θ 2 + lC 3 cos θ1 cos(θ 2 + θ3 ) −l2 sin θ1 sin θ 2 − lC 3 sin θ1 sin(θ 2 + θ 3 ) −lC 3 sin θ1 sin(θ 2 + θ 3 ) ÷
∂θ 

0
lC 2 cos θ 2 + lC 3 cos(θ 2 + θ3 )
lC 3 cos(θ 2 + θ3 ) ÷

∂rC3

b.ma trận Jacobi quay của các khâu :


J R1

0 0 0
∂ω1 
=
= 0 0 0÷
÷
∂θ& 
÷
1 0 0

J R2

 0 sin θ 1 0 
∂ω2 
÷
=
=  0 − cos θ 1 0 ÷
&
∂θ 
0

0÷
1


 0 sin θ 1 sin θ 1 
∂ω3 
÷
J R3 =
=  0 − cos θ 1 − cos θ ÷
&
∂θ 
0
0 ÷
1


3.ma trận khối lượng suy rộng của robot:
 m11 m12

3
 m21 m22
T
T
T
M = ∑ (mi J Ti J Ti + J Ri Ai I i Ai J Ri ) 
 m31 m32
i =1

=

m13 
÷
m23 ÷
m33 ÷


Trong đó:
2
2
2
m11 = I1z + I 2 y C22 + I 2 x S 22 + I 3 x S 23
+ I 3 y C23
+ m2lc22C22 + m3 (2l2lc 3C2C23 + l22C22 + lc23C23
)

m12 = m13 = m21 = m31 = 0
m22 = I 2 z + I 3 z + m2lc22 + m3 (lc23 + l22 + 2l2lc 3C3 )
m23 = m32 = I 3 z + m3 (lc23 + l2lc 3C3 )
m33 = I 3 z + m3lc23

4.Ma trận ly tâm và coriolis :
T

C (q, q&) =


∂M (q)
1  ∂M ( q)
( I n * q&) − 
(q&* I n ) ÷

∂q
2  ∂q


=

 c11 c12
c
 21 c22
c
 31 c32

Sử dụng phần mềm Maple nhân ra ta được :

c13 
c23 ÷
÷
c33 ÷



c11 = −m2lc22 S2C2 q&2 − 2m3 (lc23 S 23C23 + l22 S 2C2 + l2lc3 (2S 2C23 − S3 ))q&2
−2m3 (lc23 S23C23 + l2lc3C2 S 23 )q&3 + 2( I 2 x − I 2 y )S 2C2 q&2 + 2( I 3 x − I 3 y ) S 23C23 (q&2 + q&3 );

c12 = c13 = 0
&
c21 = (m2lc22 S 2C2 + m3 (lc23 S 23C23 + l22 S 2C2 + l2lc 3 (2C2 S 23 − S3 )))q&
1 − (2( I 2 x − I 2 y ) S 2C2 + ( I 3 x − I 3 y )C 23 S 23 )q1
c22 = −2m3lc 3l2 S3q&3
c23 = −m3lc 3l2 S3q&3

c31 = (m3lc 3 S 23 (l2C2 + lc 3C23 ) − ( I 3 x − I 3 y ) S 23C23 )q&1
1
m3l2lc 3 S3 (2q&2 − q&3 )
2
1
c33 = m3l2lc 3 S3q&2
2
c32 =

5.Thế năng của robot
π = m1 gd1 + m2 g (d1 + lc 2 S 2 ) + m3 g (d1 + l2 S 2 + lc 3 S 23 )

Từ đó suy ra :

0


∂π 
g=
=  m2 glc 2C2 + m3 gl2C2 + m3 glc 3C23 
∂q


m3 glc 3C23

6.phuơng trình vi phân chuyển động của các khâu
Thế các biểu thức vào phương trình Lagrange loại hai :
&+ C (q , q&)q&+ g (q ) = τ
M (q )q&

Ta nhận được hệ phương trình vi phân chuyển động của robot ba khâu trong
không gian :
Khâu 1 :
 I + I C 2 + I S 2 + I S 2 + I C 2 + m l 2 C 2 + m (2l l C C + l 2C 2 + l 2 C 2 )  q
&
2y 2
2x 2
3 x 23
3 y 23
2 c2 2
3
2 c3 2 23
2 2
c3 23  &
1
 1z
 −m l 2 S C q& − 2m (l 2 S C + l 2 S C + l l (2S C − S ))q&

2 c2 2 2 2
3 c3 23 23
2 2 2
2 c3
2 23
3
2
 q&
+
1 = τ1
 −2m3 (lc23S23C23 + l2lc3C2 S23 )q&


&
&
&
+
2(
I
−
I
)
S
C
q
+
2(
I
−
I
)
S
C
(
q
+
q
)
3
2x
2y
2 2 2
3x

3y
23 23 2
3 


Khâu 2 :


2
 I 2 z + I 3 z + m2lc22 + m3 (lc23 + l22 + 2l2lc3C3 )  q&

&
&
&&
2 +  m32 = I 3 z + m3 (lc 3 + l2lc3C3 )  q
3 − 2m3lc3l2 S3 q
2 q3

 &

+ { (m2lc22 S2C2 + m3 (lc23 S23C23 + l22 S2C2 + l2lc3 (2C2 S23 − S3 )))q&1 − (2( I 2 x − I 2 y )S2C2 + ( I3 x − I 3 y )C23 S23 )q&1} q&1
−m3lc3l2 S3 q&32 + m2 glc 2C2 + m3 gl2C2 + m3 glc 3C23 = τ 2

Khâu 3 :

(I

3z

)

(

)

2
&
&
&
+ m3 (lc23 + l2lc3C3 ) q&
2 + I 3 z + m3lc 3 q
3

 &2 + m l l S q&2 + m gl C = τ
+ (m3lc3S23 (l2C2 + lc3C23 ) − ( I3 x − I3 y )S23C23 )q&
1 q
1
3 2 c3 3 2
3 c3 23
3

PHẦN 4: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT
1.thiết kế quỹ đạo trong không gian khớp :
Là xác định đường biểu diễn của vị trí góc khớp(góc quay của khớp với khớp
quay và độ di chuyển của khớp đối với khớp tịnh tiến) theo thời gian khi di
chuyển từ vị trí ban đầu (A) tới vị trí cuối (B) trong thời gian với q là biến
khớp tổng quát. Quỹ đạo di chuyển của khớp giữa hai vị trí sẽ thỏa mãn 4
điều kiện : vị trí ban đầu và vị trí cuối cùng, tốc độ tại vị trí ban đầu và tốc độ
tại vị trí cuối cùng. Do đó đa thức bậc 3 sẽ phù hợp với quỹ đạo của khớp
robot:

(t)=

q&i (t ) = a1i + 2a2it + 3a3it 2
giả thiết vận tốc tại điểm đầu và điểm cuối bằng 0:
;
Ta có hệ phương trình:

⇔

a = q
i ( A)
 0i
a = 0
 1i

3(qi( B) − qi( A) )
a =
 2i
t2

−2(qi( B) − qi( A) )

a
=
 3i
t3


q (t ) = q
i ( A)

 i 0

qi (tc ) = qi ( B )

&i( A)
q&
i (t0 ) = q

q&
(tc ) = q&i( B)
 i

= a0i = 0
= a0i + a1itc + a2itc 2 + a3itc3
= a1i = 0
= a1itc + 2a2itc + 3a3itc 2 = 0


Giải hệ ta được các hệ số của phương trình quỹ đạo.
Bài toán cụ thể:
cho điểm A(90; 0; 55) và điểm B(0; 43.30; 120). Thời gian chuyển động
Từ bài toán ngược ta xác định được các thong số biến khớp:
q
o
 1( A) = 0

o
q2( A) = 0

q3( A) = 0o


•
=0

q
o
 1( B ) = 90

o
q2( B) = 30

q3( B ) = 60o


và

hệ số của phương trình quỹ đạo của khớp 1:

=0

=10.8

=-1.44

Phương trình động học của khâu 1:
 q (t ) = 10.8t 2 −1.44t 3
 2
2
 q&
2 (t ) = 21.6t − 4.32t

&
2 (t ) = 21.6 − 8.64t
 q&


Đường biểu diễn vị trí, vận tốc và gia tốc của khớp 1 thao tác theo thời gian
•

Hệ số của phương trình quỹ đạo của khớp 2:

0
Phương trình động học của khâu 2:
q (t ) = 3.6t 2 − 0.48t 3
 2

2
q&
2 (t ) = 7.2t −1.44t

&
&(t ) = 7.2 − 2.88t
q
 2


Đường biểu diễn vị trí, vận tốc và gia tốc của khớp 2 thao tác theo thời gian
•

0

Hệ số của phương trình quỹ đạo của khớp cuối:
0

7.2

Phương trình động học của khâu :
q (t ) = 7.2t 2 − 0.96t 3
 3

&3 (t ) = 14.4t − 2.88t 2
q

&
&(t ) = 14.4 − 5.76t
q
 3


Đường biểu diễn vị trí, vận tốc và gia tốc của khớp 3 thao tác theo thời gian.
2.thiết kế quỹ đạo trong không gian làm việc

Tay robot di chuyển từ vi trí A() trong không gian làm việc tới vị trí B() theo
một đường thẳng. ta có phương trình đường thẳng AB có dạng sau:
x − xA
y − yA
z − zA
=
=
xB − x A y B − y A z B − z A

Cho robot chuyển động trong thời gian 5(s) đi từ vị trí A(90; 0; 55) tớ vị trí
B(0; 43.3; 120). Tại vị trí A các khớp =0, =0, =0
Phương trình đường thẳng AB:

x − 90
y−0
z − 55
t
=
=
=
0 − 90 43.3 − 0 120 − 55 5 ⇔

 x = 90 − 18t

 y = 8.66t
 z = 120 + 13t


PHẦN 5: ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT


1.Hệ thống điều khiển phản hồi trong không gian khớp
Chức năng của hệ thống điều khiển là tạo ra một momen cần thiết để truyền
động khớ p robot đảm bảo khớp robot bám theo vị trí đặt

Sử dụng cấu trúc bộ điều khiển có dạng tỉ lệ -tích phân- đạo hàm (PID):

Trong đó:

ε = qd − q

:là sai số vị trí của khớp robot

ε&= q&d − q&
: là sai số tốc độ của khớp robot

KP

=diag(,,…,) :ma trận đường chéo các hệ số khuếch đại từng khớp

riêng biệt
=diag(, ,….,):ma trận đường chéo các hệ số đạo hàm từng khớp riêng
biệt
=diag(,….): ma trận đường chéo các hệ số tích phân từng khớp riêng
biệt