thiết kế hệ thống cơ điện tử - thiết kế robot 2 khâu rr

ĐỒ Á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ị

a. Luật điều khiển……………………………………………………………...26
b. Thiết kế cho robot RR………………………………………………………27
Phần VI. Các chương trình mô phỏng viết bằng ngôn ngữ Matlab………….31
1. Chương trình mô phỏng kết quả phần hệ thống điều khiển phản hồi………...31
2. Chương trình mô phỏng kết quả phần hệ thống điều khiển momen tính toán..35
3. Chương trình mô phỏng kết quả phần hệ thống điều khiển không gian làm việc
theo đường thẳng…………………………………………………………………38
4. Chương trình mô phỏng kết quả phần hệ thống điều khiển không gian làm việc
theo đường tròn từ hai điểm A, B lấy AB làm đường kính………………………41
1


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

5. Sử dụng thư viện trong Simulink trong Matlab……………………………...43
5.1. Sử dụng thư viện SimMechanics trong trong simulink khai báo chính
robot RR, coi các khâu là thanh mảnh đồng chất để so sánh kết quả…………….43
5.2. Sử dụng thư viện SimMechanis với mô hình robot RR được xuất ra từ
soid words..………………………………………………………………………46
5.3. Sử dụng phương trình động lực học mô phỏng điều khiển trong
Simulink………………………………………………………………………….53
a, sơ đồ khối hệ thống điều khiển PD không bù momen trọng lượng……..53
b, sơ đồ khối hệ thống điều khiển PD bù momen trọng lực……………….57
c, sơ đồ khối hệ thống điều khiển momen tính toán……………………….59
TÀI LIỆU THAM KHẢO………………………………………………………..60

LỜI NÓI ĐẦU

Hiện nay khoa học kĩ thuật đang phát triển rất mạnh, mang lại những lợi ích
cho con người về tất cảcác lĩnh vực vật chất và tinh thần. Để nâng cao đời sống nhân

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

Sinh viên thực hiện

THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Nhiệm vụ thiết kế : Robot 2 khâu RR
- Coi các khâu là các thanh đồng chất ,tiết diện ngang không đáng kể
-Thông số hình học và khối lượng từng thanh
a1=50(cm)=0.5 (m),a2=40(cm)=0.4 (m),m1 = 2.5 (kg),m1 =1.5(kg).
PhầnI. Thiết kế mô hình 3D
Do đồ án chỉ yêu cầu thiết kế mô hình 3D của robot nên phần thiết kế của em
mang tính chất ý tưởng của sinh viên,chứ thực tế các bộ truyền động trong robot nhỏ
gọn, phức tạp và chính xác hơn.

3


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

1. Mô hình tổng thể

Hình 1.1. Mô hình tổng thể
2. Các khâu khớp và các bộ truyền
2.1 Khâu 0 ( đế)

4


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

Hình 1.2. Khâu đế
2.2. Khâu 1 và khâu 2

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

Ta thiết lập phương trình động học theo phương
pháp ma trận Denavit Hartenberg
a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg
Khâu 0: đế ta chọn hệ tọa độ XoYoZo có trục Zo
chọn trùng với khớp 1, trục Xo chọn tùy ý sao cho phù
hợp nhất như hình vẽ , trục Yo chọn theo quay tắc tam
diện thuận.
Khâu 1: ta chọn hệ tọa độ X1Y1Z1 có trục Z1 trùng
với khớp 2, trục X1 ta chọn theo đường vuông góc chung
Zo và Z1 ta chọn như hình vẽ là phù hợp nhất, trục Y1
chọn theo quay tắc tam diện thuậnHình 2.1. Robot RR
Khâu 2: ta chọn hệ tọa độ X2Y2Z2 có trục Z2 song song với Z1, X2 chọn theo
đường vuông góc chung Z1 và Z2, Y2 chọn theo quy tắc tam diện thuận
b. Ma trận trạng thái Denavit Hartenberg
Từ việc chọn hệ tọa độ ta có bảng DH sau:
Khâu
1
2

0

A

1

=

d
0
0

− 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

A

Θ
q1
q2

0
2

2

=

=

1

,

A

a
a1
a2

2

=

cos(q2 ) − sin(q2 )

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

.

2

cos( q1 + q2 )
sin(q + q )
1
2


0

0


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

8


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

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 )

sin(q + q )
1
2


0



a1 sin( q1 ) + a2 sin( q1 + q2 )

0]

T

− sin(q1 + q2 ) 0 
cos( q1 + q2 ) 0 
0
1 

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

Chú ý: Ta có robot phẳng hai bậc tự do khi tính toán hay điều khiển ta quan
tâm đến điểm tác động cuối trong phẳng không quan tâm đến hướng khấu cuối vì
khi ta tìm được các biến khớp từ điểm tác động cuối và từ đó tìm được ma trận
0

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

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

Kí hiệu C1 = cos(q1), S1 = sin(q1), C2 = cos(q2), S2 = sin(q2),C12 = cos(q1+q2),
S12 = sin(q1+q2),

x 2p + y 2p = a12 + a22 + 2a1a2 (C1C12 + S1S12 ) = a12 + a22 + 2a1a2C2
Từ hệ (2.1) =>

=>


x 2p + y 2p − a12 − a22
cos(q2 ) =
2a1a2


2
sin(q2 ) = ± 1 − cos (q2 )

q2 = a tan 2(sin( q2 ), cos( q2 ))
vậy

.

 x p = (a1 + a2C2 )C1 − a2 S 2 S1

 y p = a2 S 2C1 + (a1 + a2C2 ) S1

Từ hệ (2.1) ta viết lại như sau
Giải hệ phương trình đại số tuyến tính này ta được :

∆=

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

= a1 x p + a2 ( x pC2 + y p S 2 )

= a1 y p + a2 ( y pC2 − x p S 2 )

a1 x p + a2 ( x pC2 + y p S 2 )

cos(
q
)
=
1

x 2p + y 2p


sin( q ) = a1 y p + a2 ( y p C2 − x p S2 )
1

x 2p + y 2p


q1 = a tan 2 ( sin( q1 ), cos( q1 ) )
=>

10


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

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


 a2 i


 a3i


= qia
=0
=

3(qib − qia )
tc2

=

−2( qib − qia )
tc3

Ví dụ : Robot di chuyển A(0,2+0,25

3

0,25+0,2
q1a =

trong 5(s) từ phương trình động học ngược =>
π
q2b =

3

11

π
6

3

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

q1b =

,

π
3

q2 a =

và

π
6

,

3

)

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




π

 a01 = 6

 a11 = 0


π
 a21 = 50

 a = −π
31

375


và


 a02

 a12



 a22

a
32



=

π

6
=0
=

π

50
−π
=
375

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

Hình 2.2. Đồ thị (góc khớp, vận tốc và gia tốc khớp )

2. Quỹ đạo trong không gian làm việc
Để phục vụ cho tín hiệu vào cho bộ điều khiển trong không gian làm việc nên
chỉ xét quỹ đạo di chuyển của robot RR giữa hai điểm A(x0,y0), B(xc,yc) là đường
thẳng và đường tròn ( ta thiết kế ở đây đó là đường tròn nhận AB làm đường kính)
a.Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong tc (s)
Ta có phương trình đường thẳng trong không gian làm là mặt phẳng giữa hai
x − x0
y − y0
=
xc − x0 yc − y0
điểm A(x0,y0), B(xc,yc)
với (x,y) là tọa độ điểm tác động cuối
yc − y0
y0 xc − yc x0
y=
x+
xc − x0
xc − x0
=>
.
Cũng như cách thiết lập quỹ đạo góc khớp ta để thỏa mãn điều kiện về vận tốc
đầu và cuối ta thiết lập quan hệ x=x(t) là đa thức bậc 3.

x(t ) = ao + a1t + a2t 2 + a3t 3
(3.2)

12

ĐỒ Á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 =

tc2


−2( xc − x0 )
 a3 =
tc3


mà quan hệ y =k.x+b => x=x(t) cũng có điều

Thay vào (3.2) vx(t) ta rút ra các hệ số :

y (t ) =

khi thiết kế x=x(t) =>

yc − y0
y x −y x
x (t ) + 0 c c 0
xc − x0
xc − x0

Ví dụ : Cần thiết kế di chuyển theo đường thẳng từ điểm A(0.8;0.1) đến
B(-0.2;0.5) trong 2(s) từ các công thức ở trên ta tính được :
x=
y
và quan hệ y =

13

ĐỒ Á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 ))

Viết dưới dạng tham số như sau :
cũng để thỏa mãn điều kiện về vận tốc a(t) ta thiết kế cũng phải là bậc 3

a (t ) = ao + a1t + a2t 2 + a3t 3
 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
(
t
)
=
B
( xc , yc )
 c

v (tc ) = vc = 0

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 :
,

14


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

Quan hệ

Hình 2.5. Đồ thi x(t), y(t
Hình 2.6. Quan hệ y=f(x)
Phần IV: Khảo sát động lực học
Các tham số động lực học sau xét trên hệ gắn với
khâu (coi các khâu là thanh mảnh đồng chất các biểu
thức I đối với khối tâm Ci trong hệ XcYc ZcCi song
song với hệ tọa độ khâu ) :
Khâu
1
2

xc
a1
2
a2
2

yc zc mi
0 0 m1
0

0

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

,

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

0

r

0
C1

1
 2 a1cos(q1 )

 1 a sin(q )
1
2 1


 0

1

R. r
1

C1

=

=










0

r

0
C2

,

ω%= RT . R&

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

ω%= RT . R&

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

0

1

0

1

2

1

1

0

=>

0

2

2

2

J T1 =


J

J

=
T2

∂(

0

r

∂q

C2

R2

)

=

=>

∂(

0

r C1)

∂q
∂(

2

ω

∂q&

=
2

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



0

1

R.r
2

C2

=










=

,

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

1
 2 1



0










J

R1

=

∂(

ω)

1

1

∂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




0

1
− a2 sin( q1 + q2 )
2
1
a2 cos( q1 + q2 )
2
0










=
.
(4.3)
Ma trận ten xơ quán tính của hai khâu 1 và 2 với trục gắn vào khối tâm song
song với hệ trục của khâu :

16


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

I

1

0

0
0


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

I

2

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

0

0
0


0 0 

I2 y 0 
0 I 2 z 

(4.4)

 m11 m12 
T
M (q ) = ∑ ( J TiT mi J Ti + J Ri
I i J Ri ) = 

m
m
i =1
 21
22 
2

I1 y = I1z =

Thế các biểu thức (4.1),(4.2),(4.3),(4.4) vào (4.5) và
I2 y = I2z =

1
m2 a22
12

(4.5)
1

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 )

2
2
 4
 3
m22 = I 2 z +

m2 2 1
a2 = m2 a22
4
3

T
=> Động năng của cả hệ :

1 T
= .q
&.M (q) .q&
2

=

(4.6)


1  1
1
1
1

1


.  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



Π = m1 ga1 sin( q1 ) + m2 g  a1 sin( q1 ) + a2 sin( q1 + q2 ) ÷
2
2





17

(4.7)


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

δ ( A) = M 1δ (q1 ) + M 2δ (q2 )
Công ảo của các lực suy rộng không có thế :



*
Q1 = M 1
 *
Q2 = M 2

(4.8)

Thế các biểu thức (4.6), (4.7), (4.8) vào phương trình Lagrange loại II.

d  ∂T  ∂T

∂Π
=−
+ 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






(m + 2m2 )

1
1
m2 a1a2 sin(q2 )q&22 + 1
ga1 cos(q2 ) + m2 ga2 cos(q1 + q2 )
− m2 a1a2 sin(q2 )q&&
1 q2 −
2
2
2


1
1
1
1
& 1
2
2
&
& m2 a1a2 sin(q2 )q&12 + m2 ga2 cos(q1 + q2 )
1 + m2 a2 q2 +
 M 2 =  3 m2 a2 + 2 m2 a1 a2 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

18


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

1
 (m1 + 2m2 )

ga1 cos( q2 ) + m2 ga2 cos(q1 + q2 ) 

2
2
G (q ) = 

1


m2 ga2 cos(q1 + q2 )


2

Phần V: Thiết kế điều khiển
Tất cả các hệ thống điều khiển nêu dưới đây đều theo luật điều khiển PD. Khi
thiết kế hệ thống điều khiển ta bỏ qua động học của cơ cấu chấp hành, quán tính
động cơ. Như vậy chức năng của bộ điều khiển là tạo ra một moomen cần thiết để
truyền động khớp robot đảm bảo khớp robot luôn bám theo vị trí đặt.
1. Hệ thống điều khiển trong không gian khớp
Tín hiệu đặt đó là quỹ đạo bậc 3 của các khớp đã được tính toán ở phần thiết
kế quỹ đạo.

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

đại đạo hàm của từng khớp riêng biệt.
Với luật điều khiển này đã giả thiết thành phần momen trọng lực G(p) đã
được bù hoàn toàn.
Hệ thống điều khiển với cấu trúc bộ điều khiển như trên, ổn định tuyệt đối
toàn cục. Thực vậy chọn hàm Liapunov có dạng như sau:

V

L

=

1 T
.(ε .Kp.ε + q&T .H .q&
)
2

(5.2)
Hàm VL biểu thị tổng năng lượng của hệ thống robot: Thành phần chứa Kp tỷ
lệ với năng lượng đầu vào, thành phần sau là động năng của robot mà Kp và H là
các ma trận có hệ số dương .Nên hàm VL> 0 với q khác qd.
Tính đạo hàm cấp 1 của VL ta nhận được:

1 T
T
T
&
&
&
&

.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

)]
V& = ε&.Kp.ε + .q&.H&.q&+ q&.[M −V (q,q&
2
L

Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển
(5.1) ta có

20


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

1
1
T
T
).q&+ .q&T.H&(q).q&= −q&T .Kd .ε&+ q&T.( .H&−C )q&
V& = q&.Kd .ε&− q&.C (q,q&
2
2
L

V (q, q&) = C (q, q&).q&
Trong đó:
.
Do ma trận



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

với H=M(q)=
m11 =

1
1
m1 a12 + m2 a22 + m2 a12 + m2 a1 a2 cos(q2 )
3
3

m12 = m21 =
m22 =

 h11 h12 
h

 21 h22 

1
1
m2 a22 + m2 a1 a2 cos(q2 )
3
2

1
m2 a22
3

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


21


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

1


m2 ga2 cos( q1 + q2 )

2


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

+ Phương trình luật điều khiển có dạng :

ε = qd − q ε&= q&d − q&
,

-sai số vị trí của khớp, sai số tốc độ khớp của robot

K p = diag ( K p1 , K p 2 )

-ma trận đường chéo các hệ sốkhuếch đại của khớp 1, 2

K d = diag ( K d 1 , K d 2 )
-ma trận đường chéo các hệ số khuếch đại đạo hàm của khớp
1, 2.
+ Qũy đạo đặt cho hai khớp có dạng bậc ba đã thiết kế đảm bảo tay máy di
chuyển từ vị trí ban đầu A(0,6 0,1) đến điểm cuối B(0,1 0,7) trong 2 (s) .
+ Ma trận Kp, Kd được lựa chọn :
1000
 0


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

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

Hình 5.5. Qũy đạo điểm tác động cuối

1.2. Hệ thống điều khiển momen tính toán
a.Luật điều khiển .

Hình 5.6. Sơ đồ điều khiển

24


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

Phươngpháp cơ bản của luật điều khiển là lựa chọn luật điều khiển sao cho
khử được các thành phần phi tuyến của phương trình động lực học và phân li đặc
tính động lực của thanh nối. Kết quả sẽ nhận được một hệ thống tuyến tính đảm
bảo độ chính xác chuyển động yêu cầu.

&+ V (q, q&) + G (q )
M = H (q ).q&
Dựa trên phương trình động lực học :

(5.4)

 Phương trình mô tả Luật điều khiển có dạng như sau :
M dk = H (q )U dk + V (q, q&) + G (q )
(5.5)
Cân bằng hai phương trình trên và đựa trên tính chất H(q) là ma trận thực

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

25