Đề bài
1. Tính số bậc tự do của robot
2. Vẽ các hệ trục tọa độ gắn liền các khâu theo quy tắc Denavit-Hartenberg (DH)
3. Lập bảng D-H, tính các ma trận D-H:
4. Tìm vị trí điể thao tác biểu diễn theo tọa độ khớp. Xác định hướng các khâu
thao tác.
5. Tìm vị trí điểm tác động cuối E. Tính vận tốc góc các khâu.
6. Tìm gia tốc điểm tác động cuối. Tính gia tốc các khâu thao tác.
7. Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc khâu thao
tác.
8. Tính các tọa độ khớp.
9. Tính vận tốc, gia tốc dài của các khâu tương tứng với các khớp tịnh tiến
10. Tính vận tốc góc, gia tốc góc của các khâu tương ứng với các khớp quay.
11.Khảo sát bài toán tĩnh học robot
12.Thiết kế quỹ đạo chuyển động
13.Khảo sát động lực học của robot
14. Thiết kế mô hình điểu khiển robot
15. Lập trình tính toán, vẽ đồ thị bằng phần mềm MAPLE, MATLAB, mô
phỏng điều khiển robot bằng MATLAB, SIMULINK.

Nguyễn Văn Nguyên – MSSV 20159868

Page 1


- Tính số bậc tự do của robot
F = 3(2-2) +1+1=2 robot có 2 bậc tự do
Ta xây dựng bảng thông số D-H
Joint
1
2

i

di

ai

i

q1
q2

0
0

a1
a2

0
0

- Các ma trận D-H

- Điểm tác động thao tác cuối

ZE= 0

Nguyễn Văn Nguyên – MSSV 20159868

Page 2



- Tính các ma trân Jacobi tịnh tiến
Của khâu 1
→

→
;
- Ma trận Jacobi tịnh tiến của khâu 2.
Ta có: ,

→
→

Từ ma trận truyền biến biến đổi tạo độ thuần nhất ta suy ra ma trận quay của
khâu 1 và khâu 2 để tính ma trận jacobi quay của hai khâu.
- Tính ma trận jacobi quay của khâu 1
,

ta lại có: =

- Tính ma trận jacobi quay của khâu 2
Nguyễn Văn Nguyên – MSSV 20159868

Page 3


- Tính tensor quán tính của khâu 1 và khâu 2
- ,
- Áp dụng phương trình lagrange loại II tính ma trận khối lượng M


Từ ma trận khối lượng ta suy ra ma trân lực quán tính coriolis và quán tính
ly tâm:

- Tính thế năng và lực thế của hai khâu

Tính lực suy rộng của các lực không thế

Nguyễn Văn Nguyên – MSSV 20159868

Page 4


Thế các đại lượng ta tính toán được vào phương trình Lagrange loại II ta được
phương trình vi phân chuyển động:

Phương trình vi phân chuyển động:

II. Khảo sát bài toán tĩnh học của robot

Ta có véc tơ biểu diễn lực và momen trong hệ

,, ,
Nguyễn Văn Nguyên – MSSV 20159868

Page 5


Áp dụng phương trình cân bằng lực trọng tọa độ khâu thứ 2 ta được như sau:


- Ta đặt các thông số cho robot nhứ sau:
Với các thông số:
m1 = 60kg; l1 = 2m; lg1=1m; J1=80kg.m2; m2 = 40kg; l2 = 1.5m; lg2 = 0.75m;
J2=30kg.m2; g=9.8m/s2;

Hình 1. Robot Planar 2 DOF

1. Xây dựng mô hình động lực học của robot
Phương pháp Euler-lagrange

M
Trong đó:
L = ∑Ki - ∑Pi

với: ∑Ki : tổng động năng
∑Pi : tổng thế năng

Nguyễn Văn Nguyên – MSSV 20159868

Page 6


Với robot Planar 2DOF ta có:
M = H(q) + V(q, ) + G(q)
Trong đó:

H = [H11 H12;H21 H22];
V = [V1; V2]
G = [g1; g2]


Với :
H11 = m1*lg1^2 + J1 + m2*(l1^2 + lg2^2 + 2*l1*lg2*cos(q2)) + J2;
H21 = m2*(lg2*lg2 +l1*lg2*cos(q2)) + J2;
H12 = H21;
H22 = m2*lg2*lg2 + J2;
h122 = -1*m2*l1*lg2*sin(q2);
h112 = -1*m2*l1*lg2*sin(q2);
h211 = m2*l1*lg2*sin(q2);
V1 = h122*^2 + 2*h112*()*()
V2 = h211*()^2
g1 = m1*g*lg1*cos(q1) + g*(l1*cos(q1) + lg2*cos(q1+q2))
g2 = m2*g*lg2*cos(q1+q2)
Mô hình robot được thể hiện qua sơ đồ khối:

Hình 2.Mô hình động học của Robot

2. Mô phỏng bằng simulink.
Nguyễn Văn Nguyên – MSSV 20159868

Page 7


 Viết m-file cho các tham số của robot
Dưới đây là chương trình m-file viết cho giá trị các tham số của robot
% thong so robot
m1=60;l1=2;lg1=1;J1=80;
m2=40;l2=1.5;lg2=0.75;J2=30;
g=9.8;

Dưới đây là chương trình m-file viết cho thông số dạng biến

% các thông s? d?ng bi?n
syms m1 m2 l1 l2 lg1 lg2 J1 J2 g;
syms q1(t) q2(t);
H11 = m1*lg1^2 + J1 + m2*(l1^2 + lg2^2 + 2*l1*lg2*cos(q2)) + J2;
H21 = m2*(lg2*lg2 +l1*lg2*cos(q2)) + J2;
H12 = H21;
H22 = m2*lg2*lg2 + J2;
H = [H11 H12;H21 H22];
syms X1 X2;
X = [X1;X2];
K =H*X
h122 = -1*m2*l1*lg2*sin(q2);
h112 = -1*m2*l1*lg2*sin(q2);
h211 = m2*l1*lg2*sin(q2);
V1 = h122*X2^2 + 2*h112*(X1)*(X2)
V2 = h211*(X1)^2
g1 = m1*g*lg1*cos(q1) + g*(l1*cos(q1) + lg2*cos(q1+q2))
g2 = m2*g*lg2*cos(q1+q2)
Hz= inv(H)*X

Ma trận X là ma trận 2x1, được xem là tín hiệu tổng hợp đưa vào để nhân với khối
H hoặc khối H-1. Ví dụ: trong mô hình robot
X=M–V–G
Hình ảnh bên là khối robot có đầu vào là M và đầu ra là q và
Trong khối robot đó là mô hình động lực học:

Nguyễn Văn Nguyên – MSSV 20159868

Page 8



Hình 3. Sơ đồ simulink khối robot

Trong các khối H-1*X; V; G là các khối function ( f(u) )
-

Khối H-1*X có các đầu vào là M1, M2 và q2 tương ứng với đầu vào khối fcn

là u(1), u(2), u(3).
Với:
q1cc = (u(1)*(m2*lg2^2 + J2))/(J1*J2 + l1^2*lg2^2*m2^2 + J2*l1^2*m2 +
J2*lg1^2*m1 + J1*lg2^2*m2 - l1^2*lg2^2*m2^2*cos(u(3))^2 +
lg1^2*lg2^2*m1*m2) - (u(2)*(m2*lg2^2 + l1*m2*cos(u(3))*lg2 + J2))/(J1*J2 +
l1^2*lg2^2*m2^2 + J2*l1^2*m2 + J2*lg1^2*m1 + J1*lg2^2*m2 l1^2*lg2^2*m2^2*cos(u(3))^2 + lg1^2*lg2^2*m1*m2)
q2cc = (u(2)*(m2*l1^2 + 2*m2*cos(u(3))*l1*lg2 + m1*lg1^2 + m2*lg2^2 + J1 +
J2))/(J1*J2 + l1^2*lg2^2*m2^2 + J2*l1^2*m2 + J2*lg1^2*m1 + J1*lg2^2*m2 l1^2*lg2^2*m2^2*cos(u(3))^2 + lg1^2*lg2^2*m1*m2) - (u(1)*(m2*lg2^2 +
l1*m2*cos(u(3))*lg2 + J2))/(J1*J2 + l1^2*lg2^2*m2^2 + J2*l1^2*m2 +
J2*lg1^2*m1 + J1*lg2^2*m2 - l1^2*lg2^2*m2^2*cos(u(3))^2 +
lg1^2*lg2^2*m1*m2)

Hình 4. Khối H-1*X

Nguyễn Văn Nguyên – MSSV 20159868

Page 9


-

Khối V có đầu vào là dq; q2 tương ứng với đầu vào của fcn là u(1), u(2);


u(3).
Với:
V1 = -2*m2*l1*lg2*sin(u[3])*u[1]*u[2]-m2*l1*lg2*sin(u[3])*u[2]*u[2]
V2 = m2*l1*lg2*sin(u[3])*u[1]*u[1]

Hình 5. Khối V

-

Khối G có đầu vào là q1, q2 tương ứng với 2 đầu vào của fcn là u(1), u(2)

Với:
g1 = m1*g*lg1*cos(u[1])+g*(l1*cos(u[1])+lg2*cos(u[1]+u[2]))
g2 = m2*g*lg2*cos(u[1]+u[2])

Hình 6. Khối G

3. Mô phỏng mô hình robot trên simulink với các đầu vào step, pulse,
ramp.

Nguyễn Văn Nguyên – MSSV 20159868

Page 10


Hình 7. mô phỏng simulink robot với các đầu vào lần lượt là step, pulse, ramp

Đầu ra khi đầu vào của khối robot là step:


Nguyễn Văn Nguyên – MSSV 20159868

Page 11


Hình 8: góc pha q1 khi đầu vào dạng step

Nguyễn Văn Nguyên – MSSV 20159868

Page 12


Hình 9: góc q2 khi đầu vào dạng step

Thiết kế bộ điều khiển cho robot
Mạch điều khiển cho robot:

Nguyễn Văn Nguyên – MSSV 20159868

Page 13


Hình 10: mạch điều khiển robot

+ Khối động học nghịch:

Hình 11: sơ đồ khối động học nghịch

Ta có: q2= acos((Px^2+Py^2-l1^2-l2^2/2*l1*l2)
Hàm fcn có dạng: ((u(1)^2+u(2)^2-l1^2-l2^2)/2*l1*l2)

q1 = atan2(A,B)
trong đó :
A=cos(q1)=[Px(l1+l2*cos(q2))+Py*l2*sin(q2)]/[(l1+l2cos(q2))^2+(l2*sin(q2))^2]
B=sin(q1)=[Px(l1+l2*cos(q2))-Py*l2*sin(q2)]/[(l1+l2cos(q2))^2+(l2*sin(q2))^2]
Nguyễn Văn Nguyên – MSSV 20159868

Page 14


+ khối động học thuận

Hình 12: sơ đồ khối động học thuận của robot

Ta có:

x = l1*cosq1 + l2*cos(q1+q2)
y = l1*sinq1 + l2*sin(q1+q2)

Ta suy ra 2 khối fcn lần lượt có dạng:
Fcn:

l1*cos(u(1)) + l2*cos(u(1)+u(2))

Fcn1:

l1*sin(u(1)) + l2*sin(u(1)+u(2))

+ khối đặt quỹ đạo chuyển động là đường thẳng
Đặt quỹ đạo chuyển động của robot là :
y = -x + 3.5

trong đó:

x = -0.2*t + 3.5
y = -x + 3.5

với: -0.2 là vận tốc theo phương x
( 3.5; 0 ) tọa độ ban đầu của robot

Nguyễn Văn Nguyên – MSSV 20159868

Page 15

Hình 13: sơ đồ đặt vị trí theo
đường thẳng


+ Đặt quỹ đạo là đường tròn
Đặt quỹ đạo tròn có phương trình là:
(x-1.5)^2 + y^2 = 2^2
Phương trình chuyển động của robot:
x = 1.5 +2*cos(0.2*t)
y = 2*sin(0.2*t)
Hình 14: sơ đồ đặt quỹ đạo là đường tron

với: 2 là bán kính của đường tròn
0.2 là vận tốc góc của chuyển động
1. Phương pháp P-D bù trọng trường
a. Sơ đồ mô phỏng

Hinh 15: sơ đồ mạch điều khiển phương phá PD bù TT


Chọn Kp = 50000, Kd = 100000;
b. Kết quả mô phỏng
+ đặt quỹ đạo thẳng.

Nguyễn Văn Nguyên – MSSV 20159868

Page 16


1.6

 

1.4
1.2

góc pha q [rad]

1
0.8

q1
q2

0.6
0.4
0.2
0
­0.2

­0.4  
0

1

2

3

4

5
thoi gian t [s]

6

7

8

9

10

Hình 16: đồ thị góc pha q(t)
3.6

 

3.4

3.2

toa do x [m]

3
2.8
2.6
2.4
2.2
2
1.8
1.6  
0

1

2

3

4

5
thoi gian t [s]

6

7

8


9

10

Hình 17: đồ thị x(t)
2

 

1.8
1.6

toa do y [m]

1.4
1.2
1
0.8
0.6
0.4
0.2
0 
0

1

2

3


4

5
thoi gian t [s]

6

7

Hình 18: đồ thị y(t)

Nguyễn Văn Nguyên – MSSV 20159868

Page 17

8

9

10


Hình 19: so sánh quỹ đạo thực và quỹ đạo đặt đồ thị y(x)

Nguyễn Văn Nguyên – MSSV 20159868

Page 18



+ đặt quỹ đạo tròn
2

 

goc  pha q [rad]

1.5
q1
q2

1

0.5

0

­0.5  
0

1

2

3

4

5
Thoi gian t [s]


6

7

8

9

10

Hình 20: đồ thị góc pha q(t)
3.5

 

toa do x  [m ]

3

2.5
2
1.5

1
0.5  
0

1


2

3

4

5
Thoi gian t [s]

6

7

8

9

10

Hình 21: đồ thị x(t)
2.5

 

toa do y [m ]

2

1.5


1

0.5

0 
0

1

2

3

4

5
Thoi gian t [s]

Hình 22: đồ thị y(t)
Nguyễn Văn Nguyên – MSSV 20159868

Page 19

6

7

8

9


10


Hình 23: so sánh quỹ dạo thực và quỹ đạo đặt y(x)

 Nhận xét: bộ điều khiển P-D bù trọng trường là bộ điều khiển thiết lập trạng
thái ổn định nhanh, thời gian quá độ ngắn, sai lệch tĩnh về 0 nhanh làm cho
robot hoạt động một cách chính xác và đạt chất lượng tốt
2. Phương pháp momen tính toán
a. Sơ đồ mô phỏng.
Sơ đồ simulink cho bộ điều khiển momen tính toán.

Hình 24: sơ đồ bộ điều khiển momen tính toán

Trong sơ đồ này, qd được lấy từ bộ động học nghịch, là góc đặt của robot
Nguyễn Văn Nguyên – MSSV 20159868

Page 20


Trong sơ đồ trên, khối ĐK là khối có dạng:
chọn Kp = 32
chọn Kd = 8
b. Kết quả mô phỏng
+ quỹ đạo đường thẳng
1.6

 


1.4
1.2

goc pha q [rad]

1
0.8
q1
q2

0.6
0.4
0.2
0
­0.2
­0.4  
0

1

2

3

4

5
Thoi gian t [s]

6


7

8

9

10

Hình 25: đồ thị góc pha q(t)
3.5

 

toa do x [m]

3

2.5

2

1.5  
0

1

2

3


4

5
Thoi gian t [s]

Hình 26: đồ thị x(t)

Nguyễn Văn Nguyên – MSSV 20159868

Page 21

6

7

8

9

10


2.5

 

2

toa do y [m]


1.5

1

0.5

0

­0.5  
0

1

2

3

4

5
Thoi gian t [s]

6

7

8

Hình 27: đồ thị y(t)


Hình 28: so sánh quỹ dạo đặt và quỹ đạo thực y(x)
+ quỹ

đạo đường tròn

Nguyễn Văn Nguyên – MSSV 20159868

Page 22

9

10


2

 

goc pha q [rad]

1.5

1
q1
q2
0.5

0


­0.5  
0

1

2

3

4

5
Thoi gian t [s]

6

7

8

9

10

Hình 29: đồ thị góc pha q(t)
3.5

 

3


toa do x [m]

2.5

2

1.5

1

0.5  
0

1

2

3

4

5
Thoi gian t [s]

6

7

8


9

10

Hình 30: đồ thị x(t)
2.5

 

toa do y [m]

2

1.5

1

0.5

0 
0

1

2

3

4


5
Thoi gian t [m]

6

Hình 31: đồ thị y (t)

Nguyễn Văn Nguyên – MSSV 20159868

Page 23

7

8

9

10


Hình 32: so sánh quỹ đạo thực và quỹ đạo đặt y(x)

Nhận xét: bộ điều khiển momen tính toán là bộ điều khiển tính nhanh, chính
xác. Tuy nhiên, so với bộ điều khiển PD bù trọng trường thì bộ điều khiển
momen tính toán có chất lượng kém hơn, thời gian quá

Nguyễn Văn Nguyên – MSSV 20159868

Page 24