ĐỒ ÁN CƠ ĐIỆN TỬ

MỤC LỤC
LỜI NÓI ĐẦU ……………………………………………………………………3
THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ………………………………………...4
Phần I. Thiết kế mô hình 3D……………………………………………………..4
1. Mô hình tổng thể……………………………………………………………….4
2. Các khâu khớp và các bộ truyền……………………………………………….4
Phần II. Tính toán động học thuận, động học ngược Robot RR………………8
1. Động học thuận…………………………………………………………….......8
a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg…………………..........8
b. Ma trận Denavit Hartenberg…………………………………………………8
c. Xác định vị trí điểm tác động cuối P…………………………………………9
2. Động học ngược………………………………………………………………..9
Phần III. Thiết kế quỹ đạo chuyển động cho Robot RR……………………...10
1. Qũy đạo trong không gian khớp……………………………………………...10
2. Qũy đạo trong không gian làm việc………………………………………......12
a. Qũy đạo điểm tác động cuối theo đường thẳng từ A đến B trong tc (s)…....12
b. Qũy đạo điểm tác động cuối theo đường đương tròn từ A đến B trong tc (s)
lấy đường kính AB làm đường kính……………………………………………...13
Phần IV. Khảo sát động lực học ……………………………………………….14
Phần V. Thiết kế điều khiển…………………………………………………….17
1. Hệ thống điều khiển trong không gian khớp…………………………………17
1.1. Hệ thống điều khiển phản hồi…………………………………………….17
a. Luật điều khiển…………………………………………………………….17
b. Thiết kế cho Robot RR……………………………………………………19
1.2. Hệ thống điều khiển momen tính toán……………………………………21
a. Luật điều khiển…………………………………………………………….21
b. Thiết kế cho Robot RR……………………………………………………23
2. Hệ thống điều khiển trong không gian làm việc……………………………...25
Hệ thống điều khiển ma trận Jacobien chuyển vị

dân và hòa nhập với sự phát triển chung của thế giới, Đảng và Nhà nước ta đã đề ra
những mục tiêu đưa đất nước ta đi lên thành một nước công nghiệp hóa hiện đại hóa.
Để thực hiện điều đó, một trong những ngành cần quan tâm phát triểnlà ngành cơ khí
nói chung ngành cơ điện tử nói riêng vì nó đóng vai trò quan trọng trong việc sản xuất
ra các thiết bị, công cụ ( máy móc, robot ..) của mọi ngành kinh tế quốc dân. Muốn
thực hiện việc phát triển ngành cơ khí cần đẩy mạnh đào tạo đội ngũ cán bộ kĩ thuật
có trình độ chuyên môn đáp ứng được yêu cầu của công nghệ tiên tiến, công nghệ tự
động hóa theo dây chuyền trong sản xuất.
2


ĐỒ ÁN CƠ ĐIỆN TỬ

Tính toán thiết kế hệ thống cơ điện tử là nội dung không thể thiếu trong
chương trình đào tạo kỹ sư cơ điện tử. Đồ án môn học này giúp cho sinh viên có thể
hệ thống hóa lại các kiến thức của môn học như: Lí thuyết điều khiển tử động,
Robotics, Robot công nghiệp, cơ sở máy CNC, Tính toán thiết kế Robot, Động lực
học hệ nhiều vật, Chi tiết máy, Vẽ kĩ thuật, Cơ học kĩ thuật, Nguyên lí máy…Đồng
thời giúp cho sinh viên làm quen với công việc thiết kế và làm đồ án tốt nghiệp sau
này.
Dù đã cố gắng hoàn thành đồ án này với cường độ làm việc cao, kỹ lưỡng cùng
sự hướng dẫn rất cụ thể của các thầy trong bộ môn, nhưng do hiểu biết còn hạn chế
cộng với chưa có kinh nghiệm thực tiễn nên chắc chắn đồ án này không tránh khỏi
được khả năng thiết sót và bất cập. Vì vậy em rất mong sự sửa chữa và góp ý của các
quý thầy cô để em rút ra kinh nghiệm và bổ sung thêm kiến thức cho mình.
Cuối cùng em xin chân thành cảm ơn sự quan tâm chỉ bảo của các thầy cô
trong khoa Cơ Khí trường Đại học Bách khoa Hà Nội và đặc biệt sự hướng dẫn tận
tình của thầy Đỗ Đức Nam đã giúp em hoàn thành đồ án này.

Giảng viên hướng dẫn: TS.Đỗ Đức Nam

Khâu 1 kích thước 2 trục là 500 (mm); khâu 2 kích thước 400 (mm).

5


ĐỒ ÁN CƠ ĐIỆN TỬ

Hình 1.3. Khâu 1, khâu 2
2.3. Các bộ truyền động
+ Bên trái là bộ truyền bánh răng giúp tăng momen của động cơ nên gắn trên
khâu không tác dụng dẫn động cho khâu 1
+ Bên phải là bộ truyền bánh răng giúp tăng momen của động cơ nên gắn trên
khâu 2 không tác dụng dẫn động cho khâu 2 với việc tạo vận tốc góc tương đối mô
hình như bánh răng hành tinh
+ Bên dưới là xilanh khí nén dẫn động cho cơ cấu tay kẹp

6


ĐỒ ÁN CƠ ĐIỆN TỬ

Hình 1.4. các bộ truyền

7


ĐỒ ÁN CƠ ĐIỆN TỬ

Phần II. Tính toán động học thuận, động học ngược Robot RR
1. Động học thuận

− sin(q1 ) 0 a1cos( q1 ) 
cos(q1 ) 0 a1 sin( q1 ) 

0
1
0

0
0
1


 cos(q1 )
sin(q )
1

 0

 0

0



0



A


sin(q )
cos( q2 )
2

 0
0

0
 0

α
0
0
0 a2 cos( q2 ) 
0 a2 sin( q2 ) 

1
0

0
1


1

A A
1

.


c.Xác định vị trí điểm tác động cuối P
0

A

2

Từ ma trận trạng thái của hê X2Y2Z2 đối với hệ cố định XoYoZo
ta xác
định được điểm tác động cuối P của khâu 2 so với hệ cố định, và ma trận cosin chỉ
hướng khâu 2 so với XoYoZo
0

0

r

P

R

[ a1cos(q1 ) + a2 cos(q1 + q2 )

=

2

=

cos(q1 + q2 )


cosin chỉ hướng

R

2

từ đó xác định được các góc Cardan hay Euler

2. Động học ngược
Khi giải bài toán động học thuận người ta xác định được quan hệ dước dạng
ma trận như sau: x=f(q) . Từ quan hệ đó ta suy ra một cách hình thức q=f-1(x)
trong đó x = [x1,x2,…,xm]T ,q = [q1,q2,…,qn]T.
Như vậy ta có hai chiều hướng để giải đó là phương pháp số và phương pháp
giải tích với bài toán RobotRR đơn giản ta dùng phương pháp giải tích là đơn giản
và dễ giải nhất nhưng với những bài toán robot nhiều bậc phương pháp số là
phương pháp hữu hiệu

từ hệ

 x p = a1cos( q1 ) + a2 cos( q1 + q2 )

 y p = a1 sin( q1 ) + a2 sin( q1 + q2 )

z p = 0

(2.1)
9




∆=

a1 + a2C2

∆1 =

∆2 =

− a2 S 2
a1 + a2C2

a2 S2
xp

− a2 S 2

yp

a1 + a2C2

a1 + a2C2

xp

a2 S2

yp

= a12 + a22 + 2a1a2C2 = x 2p + y 2p

Phần III: Thiết kế quỹ đạo chuyển động cho Robot
1. Qũy đạo trong không gian khớp
Ta chọn 2 điểm A(x0,y0), B(xc,yc) bất kì trong không gian làm việc, Từ
phương trình động học ngược ta tính ở phần trên ta xác định được góc khớp tại hai
điểm A,B là A(qia,qia), B(qib,qib).
Từ hai cặp góc khớp đã tìm được đó ta đi thiết kế quỹ đạp qi (i=1:2) biến thiên
từ qia đến qib theo quỹ đạo bậc 3 ( đáp ứng về mặt tốc độ góc khớp ) trong vòng

qi (t ) = aoi + a1it + a2it 2 + a3it 3
tc(s). Như vậy:

(3.1)

qi (0) = qia
q&(0) = q& = 0
 i
ia

qi (tc ) = qib
q&i (tc ) = q&ib = 0

Với các điều kiện như sau:
(thông thường ta yêu cầu vận
tốc góc khớp tại điểm đầu và điểm cuối bằng 0 ). Từ các điều kiện trên ta thay vào
(3.1) giải ta được các hệ số

 a0 i
a
 1i



11

π
6

3

) đến B(0,5 0,4

q1b =

,

π
3

q2 a =

và

π
6

,

3

)


 a22

a
32



=

π

6
=0
=

π

50
−π
=
375

π π 2 π 3

q1 = 6 + 50 t − 375 t

q = π + π t 2 − π t 3
 2 6 50
375



ĐỒ ÁN CƠ ĐIỆN TỬ

cũng các điều kiện
kiện như quỹ đạo.

Vậy

 s (0) = A( x0 , y0 )
v (0) = v = 0

0

 s (tc ) = B( xc , yc )
v (tc ) = vc = 0

 s (0) = A( x0 , y0 )
v (0) = v = 0

0

 s (tc ) = B( xc , yc )
v (tc ) = vc = 0

 a0 = x0
a = 0
 1

3( xc − x0 )
 a2 =


ĐỒ ÁN CƠ ĐIỆN TỬ

Hình 2.3. Đồ thị x(t), y(t)

Hình 2.4. Quan hệ y=f(x)

b.Thiết kế quỹ đạo điểm tác động tác động cuối di chuyển theo đường tròn từ A
đến B trong tc(s) lấy AB làm đường kính.
Ta có phương trình đường tròn trong không gian làm là mặt phẳng giữa hai
( x − xi ) 2 + ( y − yi ) 2 = R 2
điểm A(x0,y0), B(xc,yc) lấy AB làm đường kính
xi =

với

xc + x0
y + y0
1
yi = c
R=
( xc − xo ) 2 + ( yc − y0 ) 2
2
2
2

 x = xi + R sin(a (t ))

 y = yi + R cos(a (t ))


Và phải thỏa mãn điều kiện:
Từ đó ta tìm được các hệ sô a0,a1,a2,a3 như sau :
 x −x 
a0 = arcsin  0 i ÷
R

 a1 = 0

 x − xi 
 x0 − xi 
w = arcsin  c
÷− arcsin 
÷


 R
 R

đặt
3w
−2 w
a2 = 2 a3 = 3
tc
tc
Suy ra :
,
Ví dụ : Cần thiết kế di chuyển theo đường thẳng từ điểm A(0.7;0) đến
B(-0.7;0) trong 2(s) từ các công thức ở trên ta tính được :
,


Ixx Iyy Izz Ixy Iyz Izx
0 I1y I1z 0 0 0

m2 0

I2y I2z 0

0

I1 y = I1z =

Với a1 = 0.5 (m), a2 = 0.4 (m),
I2 y = I2z =

0
1
m1a12
12

1
m2 a22
12

Chọn tọa độ suy rộng tương đối q = [q1,q2]T
Từ phần động học ta xác định được :

15

,












0

r

0
C2

,

ω%= RT . R&

0 
1
ω1 =  0 


1
 q&

ω%= RT . R&

2

J T1 =


J

J

=
T2

∂(

0

r

∂q

C2

R2

)

=

=>




0

1

R.r
2

C2

=










=

,

 1
 − 2 a1 sin(q1 )

 1 a cos( q )

∂q&

,

=

,

(4.1)

)

=

(4.2)

1

 − a1 sin(q1 ) − 2 a2 sin( q1 + q2 )

 a cos(q ) + 1 a cos( q + q )
1
2
1
2
 1
2




I

1

0

0
0


0 0 

I1 y 0 
0 I1z 

I

2

=
,
=
=> Ma trân khối lượng:

0

0
0



m1a12
12

,

ta được tính được :



1
a2
m11 = I1z + I 2 z + m1a12 + m2  a12 + 2 + 2a1a2 cos( q2 ) ÷
4
4


=

1
1
m1a12 + m2 a22 + m2 a12 + m2 a1a2 cos(q2 )
3
3

 a22 a1a2
 1
1
m12 = m21 = I 2 z + m2 
+
cos(q2 ) ÷ = m2 a22 + m2 a1a2 cos( q2 )


.  m1 a12 + m2 a22 + m2 a12 + m2 a1 a2 cos(q2 ) ÷q&12 + m2 a22 q&22 + 2  m2 a22 + m2 a1 a2 cos(q2 ) ÷q&&
1 q2 
2  3
3
3
2

3


2

Π = −∑ mi. g .

T 0
0

r

Ci

i =1

Biểu thức thế năng của hệ :
với go = [ 0 ,0 ,-g ]T
1
1



∂Π
=−
+ Qi*

÷−
dt  ∂q&
∂qi
∂qi
i 

Ta thiết lập được hệ phương trình động lực học sau :

1
1
1

1

2
2
2
2
&
&2
m2 a1a2 cos(q2 ) ÷q&
1 +  m2 a2 +
 M 1 =  3 m1 a1 + 3 m2 a2 + m2 a1 + m2 a1a2 cos(q2 ) ÷q&
3
2


2




Viết dưới dạng ma trận nhự sau :

&+ C (q, q&).q&+ G ( q) = M ( q).q&
&+ V ( q, q&) + G ( q)
τ = M (q).q&
.
Với τ = [M1,M2]T, q = [ q1 , q2 ]T, M(q) ma trận khối lượng bên trên
1


m2 a1 a2 sin(q2 )q&22 
1 q2 −
 −m2 a1 a2 sin(q2 )q&&
2
V (q, q&) = 

1
2


m2 a1a2 sin(q2 )q&1


2


1.1. Hệ thống điều khiển phản hồi
a, Luật điều khiển

Hình 5.1.Sơ đồ cấu trúc hệ thống điều khiển robot với bộ điều khiển PD

19


ĐỒ ÁN CƠ ĐIỆN TỬ

&+ V (q, q&) + G (q)
M = τ = M (q).q&
Ta có phương trình động lực học :
H(q)=M(q)tránh nhầm với M là vector momen như vậy

, gọi

&+ V ( q, q&) + G ( q)
M = H ( q).q&

M dk = K p (qd − q) + K d (q&d − q&) = K p ε + K d ε&

Luật điều khiển :
(5.1)
K p = diag ( K p1 , K p 2 ,..., K pn )
Trong đó :
- ma trận đường chéo các hệ số khuếch
đại của từng khớp riêng biệt.
K d = diag ( K d 1 , K d 2 ,..., K dn )
-ma trận đường chéo các hệ số khuếch

.H .q&+ q&T .H&.q&+ q&T .H .q
)
V& = .(ε&.Kp.ε +ε .Kp.ε&+ q
2
L

T

Do tính chất đối xứng của các thành phần

ε T .Kp.ε q&.H .q&
,

, ta rút gọn được

1 T
T
T
&
&
V& = ε&.Kp.ε + .q&.H&.q&+ q&.H .q
2
L

Từ phương trình động lực học với giả thiết không có thành phần momen trọng
lực G(q), nhận được phương trình sau :

1 T
T
T

1 &
.H − C
2

là ma trận đối xứng ngược =>

1
q&T .( .H&− C )q&= 0
2

T
V&L =−q&.Kd .q&≤ 0

(5.3)
Từ (5.2) ,(5.3) cho thấy rằng, mức độ dương của VL phụ thuộc vào Kp; mức độ

V&L

âm của
phụ thuộc vào Kd . Do đó tăng tốc độ hội tụ bằng tăng giá trị Kd. Nâng
cao độ chính xác tinh của hệ thống điều khiển đạt được bằng tăng hệ số Kp của
khâu khuếch đại. Tuy nhiên ,Kp và Kd quá lớn sẽ làm giảm độ ổn định và chất
lượng quá trình quá độ như độ quá điều chỉnh , thời gian quá độ tăng.
b. Thiết kế cho robot RR
Các tham số như sau : a1= 0.5 (m), a2 = 0,4 (m)
m1=2.5 (kg), m2=1.5 (kg)
+ Hệ phương trình động lực học có dạng như sau :
&
&+ V ( q, q&) + G ( q )
M = H ( q).q


m2 a1 a2 sin(q2 )q&22 
1 q2 −
 − m2 a1 a2 sin(q2 )q&&
2
V (q, q&) = 

1
2


m2 a1a2 sin(q2 )q&
1

2


21


ĐỒ ÁN CƠ ĐIỆN TỬ
1
 (m1 + 2m2 )

ga1 cos( q2 ) + m2 ga2 cos( q1 + q2 ) 

2
2
G (q) = 



0 
1000 

500
0


0
500 

Kp =
, Kd =
.
+ Chương trình mô phỏng được viết bằng matlab ( có so sánh kết quả với dùng thư
viện simulink trong matlab) , Ta được kết quả như sau :

Hình 5.2. Góc khớp q1, q2 đặt và điều khiển

22


ĐỒ ÁN CƠ ĐIỆN TỬ

Hình 5.3. Sai số và Momen khớp 1, 2

Hình 5.4. Vận tốc khớp 1, 2 đặt và điều khiển

23


dương nên có thể lấy nghịch đảo, ta nhận được phương trình vi phân tuyến tính

&= U dk
q&
cấp hai như sau:

đây là phương trình vi phân tuyến tính cấp 2 độc lập

giữa các khớp. Do đó có thể thiết kế các bộ điều khiển độc lập có cấu chúc PD
&d + K p ε + K d ε&
U dk = q&

cho từng khớp như sau :

(5.6)

Từ (5.4),(5.5) và (5.6) ta rút ra được phương trình vi phân sai số vị trí của hệ
&+ K d ε&+ K p ε = 0
ε&

thống kín có dạng như sau :
s2 I + Kd s + K p = 0

Phương trình đặc tính ở dạng toán tử Laplace là :
Viết cho từng khớp riêng lẻ (đó là khâu quán tính bậc hai):
s 2 + K di s + K pi = 0

Các hệ số Kdi, Kpi được chọn luôn dương nên đảm bảo ổn định, và chúng được
tính toán theo yêu cấu về chất lượng điều khiển như độ quá điều chỉnh σ , Thời
than quá độ Tqd