Môn học Kỹ thuật Robot Bài tập robot đề số 34

Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
BÀI TẬP ROBOT
ĐỀ SỐ 34
1. Cho Robot có cấu hình như hình vẽ: a
2
= 0,2 m
a. Xây dựng hệ tọa độ cho các thanh nối.
b. Xác định ma trận T biểu diễn hệ tọa độ tay Robot.
c. Giải thích ý nghĩa ma trận T.
d. Xác định vị trí tay Robot trong hệ tọa độ gốc khi θ
1
= 45
0
; θ
2
= 45
0
; d
3
= 0,1m.
2. Cho Robot θ – r có r
1
= 0,5 m; m
1
= m
2
= 2,5 Kg. Khớp tịnh tiến chuyển động với
tốc độ r = 0,05 m/s. Khớp quay quay với tốc độ θ = 9
0
/s. Thời gian chuyển động là

5s. Giá trị góc ban đầu là 0
o
.
a. Xác định góc θ và r của Robot ở cuối hành trình chuyển động.
b. Hãy xác định momen ở khớp quay và lực tổng hợp ở khớp tịnh tiến khi Robot
ở vị trí cuối hành trình chuyển động.
c. Thiết kế bộ điều khiển PID
d. Mô phỏng hệ thống.
Bài làm
Bài 1
1
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
a. Xây dựng khung tọa độ cho các thanh nối
 Nguyên tắc:
- Gốc khung tọa độ thanh I đặt trùng với chân pháp tuyến chung của trục i và i+1 và
nằm trên trục của khớp i+1.
- Trục z
i
đặt theo phương của trục khớp i+1.
- Trục x
i
đặt theo phương pháp tuyến chung của trục I và i+1 theo hướng từ trục i
tới i+1.
 Một vài trường hợp đặc biệt:
- Hai trục song song, sẽ có nhiều pháp tuyến chung. Khi đó sẽ chọn được pháp
tuyến chung trùng với pháp tuyến chung của khớp trước. Gốc khung tọa độ được
chọn sao cho d
i
là nhỏ nhất.

- Đối với khớp tịnh tiến: khoảng cách d
i
là biến khớp. Hướng của trục khớp tùng với
hướng di chuyển của khớp. Chiều dài a
i
= 0. Gốc tọa độ đặt trùng với gốc thanh
nối tiếp theo
b. Xác định ma trận T biểu diễn hệ tọa độ tay Robot.
Chọn khung tọa độ gốc O
0
x
0
y
0
z
0
gắn với khớp 1 như hình vẽ. Bỏ qua khoảng cách
giữa 2 trục x
1
và x
0
.
Ta thiết lập được bảng D-H:
2
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
Khung tọa
độ
a
i

α
i
θ
i
d
i
1 0 -90
0
θ
1
0
2 a
2
-90
0
θ
2
0
3 0 0 0 d
3
Thành lập các ma trận:
11
1
0
1
1
0
0
0 0
0

0
0
A
1
0
0
0 1
C S
S C
θ
θ
θ
θ
 
 
 
 
 
 
−
=
−
2 2
2 2
2 2
2
22
1
0 .
.

0 1 1 0
0 0 0
0
A
1
C S a C
S C a S
θ
θθ
θ
θ
θ
 
 
 
 
− −
 
 
−
=
3
2
3
1 0 0 0
0 1 0 0
0 0 1
0 0 0
A
1

d
 
 



 
=



Ma trận T biểu diễn hệ tọa độ tay Robot:
1 2 1 2
1 1 2
2
1 2 1 1 2
1 2 1 2 1 2
0 1 2
1 2 3
2 22 3
. 3. . 2.
. . 3.S .
. .
.
. .
0
0 0
2.S
0 1
C

2
C S d C S a
C d a
S C a
C C S C C
S C S C C
T A A A
C d S
θ θ θ θ
θ θ θ
θ θ θ
θ θ θ θ θ
θ θ θ θ θ θ
θ
− − +
 
 
 
=
 
− −
 

=
−

− − +
−
c. Giải thích ý nghĩa ma trận T.
Ma trận T biểu diễn khung tọa độ tay Robot trong khung tọa độ gốc. Khung tọa độ T có:

- Gốc tọa độ O
T
.
3
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
- Các vecto đơn vị
T
,
T
,
T
.
Cột 1: Tọa độ vecto đơn vị i
T
của khung tọa độ tay Robot T trong khung tọa độ gốc
[ ]
0
1 2 1 22
. 0.
T
T
C C S Ci S
θ θ θθ θ
− =
Cột 2: Tọa độ vecto đơn vị j
T
của khung tọa độ tay Robot T trong khung tọa độ gốc
[ ]
0

1 1
0
T
T
S Cj
θ θ
− 0 =
Cột 3: Tọa độ vecto đơn vị k
T
của khung tọa độ tay Robot T trong khung tọa độ gốc
[ ]
0
1 2 1 22
.S . 0
T
T
C S Ck C
θ θ θ θ θ
− = −
Cột 4: Tọa độ điểm O
T
gốc khung tọa độ tay Robot T trong khung tọa độ gốc
[ ]
0
1 2 22 1 2 1 2 1 2 3
3. .S 3.S .2. . 2. . 2
T
T
aO d C C C d C Sd aCa CS
θ θ θ θ θ θ θ θ θ θ

= + + −− − 1−
d. Xác định vị trí tay Robot trong hệ tọa độ gốc khi θ
1
= 45
0
; θ
2
= 45
0
; d
3
= 0,1 m.
Thay các giá trị θ
1
= 45
0
; θ
2
= 45
0
; d
3
= 0,1 m vào ma trận
0
O
T
ta được tọa độ vị trí tay
Robot trong khung tọa độ gốc:
0
O

T
=
Bài 2
4
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
Bài làm:
Để xây dựng được các phương trình động lực học của Robot θ-r để đơn giản chúng ta giả
thuyết:
- Khối lượng
1
m
của thanh nối 1 tập trung tại điểm cuối A
- Khối lượng
2
m
của thanh nối 1 tập trung tại điểm cuối B.
- Mô mem quán tính ở các khớp
1 2
0J J= =
.
a. Xác định góc θ và r của Robot ở cuối hành trình chuyển động.
Góc θ được xác định theo công thức:
0 0
0
.t 0 9 .5 45
θ θ θ
•
= + = + =
Coi giá trị ban đầu của r = r1 = 0.5m và giá trị cuối được xác định theo công thức:

( )
0
. 0.5 0.05.5 0.75 r r r t m
•
= + = + =
b. Hãy xác định momen ở khớp quay và lực tổng hợp của khớp tịnh tiến khi robot
ở cuối hành trình chuyển động.
Hàm Lagrange là hiệu của tổng động năng
K
và tổng thế năng
L
của hệ:
5
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55

L K P
= −
11\* MERGEFORMAT ()
Momen (lực) theo phương trình chuyển động Lagrange:
( )
i i
i
i
d L L
M F
dt q
q
•
 

∂ ∂
 ÷
= −
 ÷
∂
∂
 
22\* MERGEFORMAT ()
Trong đó qi là biến khớp: - Góc quay θ nếu là khớp quay
- Độ dịch chuyển d nếu là khớp tịnh 'ến
Xác định tổng thế năng P của hệ :
1 2
P P P
= +
33\* MERGEFORMAT ()
- Xác định thế năng thanh số 1:
1 1 1
. . .sinP m g r
θ
=
44\* MERGEFORMAT ()
- Xác định thế năng thanh số 2:
22
. . .sinP m g r
θ
=
55\* MERGEFORMAT ()
Thay (4) và (5) vào (3) ta được tổng thế năng P:
1 2 1 1 2
. . .sin . . .sinP P P m g r m g r

θ θ
= + = +
66\*
MERGEFORMAT ()
Xác định tổng động năng K của hệ:
1 2
K K K= +
77\* MERGEFORMAT ()
6
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
- Xác định động năng thanh nối 1 (J
1
=0):
2
1
1 1
2
v
K m=
88\* MERGEFORMAT ()
Trong đó,
1
v
là tốc độ chuyển động tịnh tiến của điểm A (tâm thanh nối 1)
Vị trí điểm A so với khung tọa độ gốc:
1
1 1 1
1 1
1

1
.cos .sin .
.sin
.cos .
X
Y
X r r
Y r
r
θ θ θ
θ
θ θ
•
•
•
•


= = −
 
<=>
 
=
 
=


99\* MERGEFORMAT ()
2 2 2 2 2 2 2 2 2 2 2
1 1 1 1 1 1

.sin . .cos . .v X Y r r r
θ θ θ θ θ
= + = + =
& & &
& &
1010\*
MERGEFORMAT ()
Thay (10)

vào (8) ta được:
2
2
1 1 1
1
2
K m r
θ
•
=
1111\* MERGEFORMAT ()
- Xác định động năng thanh nối 2 (J
2
=0):
2
2
2 2
2
v
K m=
1212\* MERGEFORMAT ()

Trong đó,
2
v
là tốc độ chuyển động tịnh tiến của điểm B (tâm thanh nối 2)
Vị trí điểm B so với khung tọa độ gốc:
7
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
2
2
2
2
.cos
.sin . .cos
.sin
.cos . .sin
X r
X r r
Y r
Y r r
θ
θ θ θ
θ
θ θ θ

=

= − +

<=>

 
=
= +



&
&
&
&
&
&
1313\*
MERGEFORMAT ()
2 2 2 2 2 2
2 2 2
.v X Y r r
θ
= + = +
&
& &
&
1414\* MERGEFORMAT ()
Thay (14) vào (12) ta được:
( )
2 2 2
2 2
1
. . .
2

K m r r
θ
= +
&
&
1515\* MERGEFORMAT ()
Thay (11) và (15) vào (7) ta được tổng động năng K của hệ của hệ:
( )
2 2 2 2 2
1 2 1 1 2
1 1
. . . . . .
2 2
K K K m r m r r
θ θ
= + = + +
& &
&
1616\* MERGEFORMAT ()
Hàm Lagrange: Từ các biểu thức (6) và (16) thay vào (1) ta được:
( )
( )
2 2 2 2 2
1 1 2 1 1 2
1 1
. . . . . . . . .sin . . .sin
2 2
L K P m r m r r m g r m g r
θ θ θ θ
= − = + + − +

& &
&
1717\*
MERGEFORMAT ()
Tính momen khớp quay (
i
q
θ
=
) áp dụng (2) với
i
q
θ
=
ta được:
8
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
d L L
M
dt
θ θ
∂ ∂
 
= −
 ÷
∂ ∂
 
&
1818\* MERGEFORMAT ()

Từ (17) ta có:
( )
( )
2 2
1 1 2
2 2
1 1 2
1 1 2
. . . .
. . . 2 . .θ .
. . .cos . . .cos
L
m r m r
d L
m r m r r r
dt
L
m g r m g r
θ θ
θ
θ θ
θ
θ θ
θ
∂
= +
∂
∂
 
= + +

 ÷
∂
 
∂
= − +
∂
& &
&& &
&
&
&&
&
1919\* MERGEFORMAT
()
Thay (19) vào (18) ta được:
2 2
1 1 2 2 1 1 2
( ) 2 ( ) cosM m r m r m rr m r m r g
θ θ θ
= + + + +
&& &
&
2020\*
MERGEFORMAT ()
Tính lực khớp tịnh tiến (
i
q r=
) áp dụng (2) với
i
q r=

ta được:
d L L
F
dt r r
∂ ∂
 
= −
 ÷
∂ ∂
 
&
2121\* MERGEFORMAT ()
Từ (17) ta có:
9
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
2
2
2
2 2
.
.
. . . .sin
L
m r
r
d L
m r
dt r
L

m r m g
r
θ θ
∂
=
∂
∂
 
=
 ÷
∂
 
∂
= −
∂
&
&
&
&
&
&
2222\* MERGEFORMAT ()
Thay (22) vào (21) ta được:
2
2 2 2
sinF m r m r m g
θ θ
= − +
&
&&

2323\* MERGEFORMAT
()
Tính toán mô men khớp quay M và lực ở khớp tịnh tiến khi Robot ở cuối hành trình
chuyển động:
Thay các số liệu:
1 2
2.5( )m m Kg
= =
,
1
0,5r m
=
,
0.75r m
=
,
0,05 /r m s
=
&
,
0r =
&&
0 0
45 , 9 / s (rad/ s), 0
20
π
θ θ θ
•
= = = =
&&

vào (20) và (23) ta được:
10
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
2 2
1 1 2 2 1 1 2
0
( ) 2 ( ) cos
0 2.2,5.0,75.0,05. (2,5.0,5 2,5.0,75).9,8.cos 45
20
3 245
2
320 16
M m r m r m rr m r m r g
θ θ θ
π
π
= + + + +
= + + +
= +
&& &
&
2
2 2 2
2
0
2
sin
0 2,5.0,75. 2,5.9,8.sin 45
20

3 49
2
640 4
c
F m r m r m g
θ θ
π
π
= − +
 
= − +
 ÷
 
= − +
&
&&
c. Thiết kế bộ điều khiển PID
Cấu trúc của hệ thống điều khiển phản hồi kinh điển PID
Phương trình động lực học của Robot θ – r ở trên được viết lại như sau:
2 2
1 1 2 2 1 1 2
2
2 2 2
( ) 2 ( ) cos
sin
c
M m r m r m rr m r m r g
F m r m r m g
θ θ θ
θ θ

= + + + +
= − +
&& &
&
&
&&
Viết lại dưới dạng ma trận:
( ) ( , ) ( )M H q q V q q G q= + +
&& &
Trong đó:
11
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
q
r
θ
 
=
 
 
: Ma trận biến khớp
M
M
F
 
=
 
 
: Ma trận mô-men hoặc lực của khớp
2 2

1 1 2
2
0
0
m r m r
H
m
 
+
=
 
 
: Ma trận hệ số quán tính
2
2
2
2m rr
V
m r
θ
θ
 
=
 
−
 
&
&
&
: Ma trận momen nhớt hướng tâm

1 1 2
2
( ) cos
sin
m r m r g
G
m g
θ
θ
+
 
=
 
 
: Ma trận mô-men và lực của trọng lực
Mục đích thiết kế bộ điều khiển PID: Nâng cao độ chính xác tinh của hệ thống, giảm
thời gian quá độ và khử sai lệch tĩnh của hệ thống. Trong thực tế thành phần tích phân I
sẽ làm cho sai lệch tĩnh rất nhỏ và tiến tới 0.
Luật điều khiển PID:
0
t
dk P q D q I q
M K K K dt
ε ε ε
= + +
∫
&
(24)
Trong đó:
*

q
q
q q
q q
ε
ε
∗

= −


= −


&
& &
12
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
1 2
(K ,K )
P p p
K diag=
1 2
(K ,K )
I i i
K diag
=
1 2
(K ,K )

D d d
K diag
=
 Thiết kế quỹ đạo đặt hai khớp dạng 2 – 1 – 2
Robot di chuyển từ vị trí khớp
0
q
tới vị trí khớp
c
q
trong thời gian
c
t
theo 3 giai đoạn:
• Giai đoạn khởi động trong khoảng thời gian
1
0 t−
: quỹ đạo khớp
0 1
q q
−
có dạng
parabol bậc 2, gia tốc
1
q
&&
của khớp là hằng số.
2
1 0 1 1
1

2
q q q t= +
&&
2424\* MERGEFORMAT ()
• Giai đoạn chuyển động đều trong khoảng thời gian
1 2
t t−
, quỹ đạo khớp
1 2
q q−
có
dạng đường thẳng bậc 1, để đảm bảo tính đối xứng của quỹ đạo, điểm trung bình
được chọn:
1 2
0
1 2
2 2
2 2
c
m
c
m
t
t t
t
q q
q q
q
+
= =

+
+
= =
2525\* MERGEFORMAT ()
Tốc độ không đổi trong giai đoạn này bằng tốc độ cuối giai đoạn khởi động:
1
1 1
1
m
m
q q
q t
t t
−
=
−
&&
2626\* MERGEFORMAT ()
• Giai đoạn hãm trong khoảng thời gian
2 c
t t−
, quỹ đạo khớp
2 c
q q−
có dạng
parabol bậc 2.
13
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
Thay (26) vào(27), kết hợp với (25) ta thu được phương trình:

2
1 1 1 1 0
0
c c
q t q t t q q− + − =
&& &
2727\* MERGEFORMAT ()
Giải (28), xác định thời gian
1
2
c
m
t
t t< =
:
2
0
1
1
2 4
c c c
t t q q
t
q
−
= − −
&&
2828\* MERGEFORMAT ()
Biểu thức (29) có nghĩa khi và chỉ khi:
14

Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
2
0
1
0
4
c c
t q q
q
−
− ≥
&&
2929\* MERGEFORMAT ()
Giới hạn của gia tốc phải nhỏ hơn gia tốc cho phép từ độ bền cơ khí của Robot. Kết hợp
với (29), gia tốc chuyển động khớp được chọn phải thỏa mãn bất đẳng thức:
0
1 1
2
4
c
cp
c
q q
q q
t
−
< ≤
&& &&
3030\* MERGEFORMAT ()

Với các thông số
1
q
&&
được xác định bởi (31) và
1
t
được xác định bởi (29), phương trình
quỹ đạo chuyển động được thiết kế là:
( )
( )
( )
2
0 1 1
1
0 1 1 1 1
2
1 1
1
0
2
( )
2
1
( )
2
c
c c c c
q q t t t
t

q t q q t t t t t t
q q t t t t t t

+ ≤ ≤



 
= + − ≤ ≤ −

 ÷
 


− − − ≤ ≤


&&
&&
&&
d. Mô phỏng hệ thống
Chương trình mô phỏng được viết bằng ngôn ngử MATLAB được trình bày ở phần
phụ lục.Sau đây là kết quả mô phỏng ứng với bộ điều khiển PD với các ma trận thông số
như sau:
4500 0
0 4500
P
K
 
=

 
 
400 0
0 400
D
K
 
=
 
 
200 0
0 200
D
K
 
=
 
 
1) Chương trình mô phỏng
[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = deal([]);
%
m1 = 2.5; %Khoi luong thanh 1
m2 = 2.5; %Khoi luong thanh 2
15
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
r1 = 0.5; %Chieu dai thanh noi 1
r0 = r1; %Vi tri dau khop tinh tien
rc = 0.75; %Vi tri cuoi khop tinh tien
theta0 = 0; %Vi tri dau khop quay

thetac = pi/4; %Vi tri cuoi khop quay
tc = 5; %Thoi gian di chuyen cua tay Robot
%
% Vi tri cac khop thoi diem dau va cuoi
q0 = [theta0;r0];
qc = [thetac;rc];

%Khac phuc truong hop chia 0
if (qc(2)==q0(2));
qc(2) = qc(2)-0.001;
end
if (qc(1)==q0(1));
qc(1) = qc(1)-0.001;
end

%Tinh toan gia toc hai khop tinh tien va quay
ddq1 = 1.3*4*abs(qc(1)-q0(1))/tc^2; %Gia toc khop quay
ddq2 = 1.3*4*abs(qc(2)-q0(2))/tc^2; %Gia toc khop tinh tien

%Xac dinh cac khoang thoi gian khoi dong, dac diem la chuyen dong
nhanh dan deu tang toc, deu va giam toc cho
%cac khop
t11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang
toc
t21 = tc - t11; %t21 - t11/2 se la thoi gian chuyen dong deu, tc-t21
se la
%thoi gian giam toc ve 0. Tuc la thoi gian tang va giam toc
%deu bang t11/2
t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2; %Thoi gian tang
toc

t22 = tc - t12;

Tk = 0.01; %Chu ky lay mau cua mo hinh robot

%Dieu kien ban dau (So kien)
q = q0; dq = [0;0];
%Mo phong
tpcu = 0;
file1 = fopen('Robot_data.txt','w');
i = 0; %Bien dung de dem
for t = 0:0.001:tc;
i = i+1;
At1(i)=t; %Lay thoi gian de ve do thi
16
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
%Tinh toan gia tri dat cho cac khop
[qd1, dqd1] = QuiDaoKhop(q0(1),qc(1),ddq1,t11,t21,tc,t); %dqd1 la
van
%toc cua khop 1, ddq1 la gia toc khop 1
[qd2, dqd2] = QuiDaoKhop(q0(2),qc(2),ddq2,t12,t22,tc,t);
qd = [qd1; qd2];
dqd = [dqd1; dqd2];

%Tinh momen can thiet de thuc hien chuyen dong
[M,xe,ye,tpmoi] = Controller(qd,dqd,q,dq,tpcu);
tpcu = tpmoi;
%Lay thong so qui dao thuc robot chuyen dong duoc qua M
[q, dq] = RobotModel(M,q,dq,Tk);

%Lay cac thong so de ve do thi
%
qdd1(i) = qd(1);
qdd2(i) = qd(2);
Aq1(i) = q(1); %Goc quay khop quay
Aq2(i) = q(2); %r
Adq1(i) = dq(1); %Toc do khop quay
Adq2(i) = dq(2); %Toc do khop tinh tien
AM1(i) = M(1); %Mo men khop quay
AM2(i) = M(2); %Luc truyen dong cho khop tinh tien
AeTheta(i) = qd(1) - q(1); %Sai lech goc quay
Aer(i) = qd(2) - q(2); %Sai lech chuyen dong tinh tien
%
%Luu du lieu vao file
fprintf(file1,'%2.4f%2.4f%2.4f%2.4f%2.4f%3.4f
%3.4f\n',t,qd(1),q(1),qd(2),q(2),M(1),M(2));
end
fclose(file1);

%
% Ve do thi ket qua mo phong
%
figure
% Mo men khop
subplot(2,2,1,'FontSize',14)
plot1 = plot(At1,AM1,At1,AM2);
title('Momen khop quay va luc khop tinh tien');
set(plot1(1),'LineWidth',2.5,'DisplayName','Momen khop 1');
set(plot1(2),'LineWidth',2.5,'LineStyle',' ','Color','r',
'DisplayName','Luc khop 2');

xlabel('Thoi gian (s)');
ylabel('');
legend('show');
17
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
grid on;
% Sai so goc quay va tinh tien tren cung do thi
subplot(2,2,2,'FontSize',14)
plot1 = plot(At1,AeTheta,At1,Aer);
title('Sai so cac bien khop');
set(plot1(1),'LineWidth',2.5,'DisplayName','e(theta)');
set(plot1(2),'LineWidth',2.5,'LineStyle',' ','Color','r',
'DisplayName','e(r)');
xlabel('Thoi gian (s)');
ylabel('');
legend('show');
grid on;

% Goc quay khop 1
subplot(2,2,3,'FontSize',14)
plot1 = plot(At1,Aq1,At1,qdd1);
title('Bien khop quay');
set(plot1(1),'LineWidth',2.5,'DisplayName','theta thuc');
set(plot1(2),'LineWidth',2.5,'LineStyle',' ','Color','r',
'DisplayName','theta dat');
xlabel('Thoi gian (s)');
ylabel('');
legend('show','Location','NorthWest');
grid on;

% Do dich chuyen cua khop 2 tinh tien
subplot(2,2,4,'FontSize',14)
plot1 = plot(At1,Aq2,At1,qdd2);
title('Bien khop tinh tien');
set(plot1(1),'LineWidth',2.5,'DisplayName','r thuc');
set(plot1(2),'LineWidth',2.5,'LineStyle',' ','Color','r',
'DisplayName','r dat');
xlabel('Thoi gian (s)');
ylabel('');
legend('show','Location','NorthWest');
grid on;

% Phong to hinh
drawnow
jFig = get(handle(gcf), 'JavaFrame');
jFig.setMaximized(true);
2) Hàm tính lượng đặt
function[q,dq] = QuiDaoKhop(q0,qc,ddq,t1,t2,tc,t)
% Quy dao chuyen dong 2-1-2
18
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
if t<=t1 %Tang toc bac 2
q = q0 + ddq*t^2/2;
dq = ddq * t;
elseif((t>t1) && (t<=t2)) %Chuyen dong deu bac 1
q = q0 + ddq*t1*(t-t1/2);
dq = ddq * t1;
elseif((t>t2)&&(t<=tc)) %Giam toc do ve khong
q = qc - ddq*(t-tc)^2/2;

dq = ddq*t1 - ddq*(t-t2);
end
end
3) Hàm tính luật điều khiển
function[M,saisovitri,saisotocdo,tichphanmoi] =
Controller(sd,dsd,qthuc,dqthuc,tichphancu)
% Bo dieu khien bu trong luc
%Tham so bo dieu khien vi tri phan hoi PD
Kp = 4500;
Ki = 400;
Kd = 200;
Kpp = [Kp,0;0,Kp];
Kii = [Ki,0;0,Ki];
Kdd = [Kd,0;0,Kd];
%Tinh sai lech goc quay va dao ham goc quay
saisovitri = sd - qthuc;
tichphanmoi = tichphancu + saisovitri*0.001;
saisotocdo = dsd - dqthuc;
%Momen dieu khien bu trong luc
M = Kpp * saisovitri + Kii * tichphanmoi + Kdd * saisotocdo;
end
4) Hàm tính biến khớp từ mô hình Robot
function[q,dq] = RobotModel(M,q,dq,Tk)
%Mo hinh mo phong robot
%Dau vao: momen M, bien trang thai q,dq khi bat dau chu ky lay mau,
%thoi gian lay mau Tk,
%Dau ra: bien trang thai q,dq khi ket thuc chu ky lay mau.
%
M01 = M(1); %Mo men dieu khien cho khop quay
F02 = M(2); %Luc dieu khien cho khop tinh tien

%
%Cac thong so cua Robot
m1 = evalin('base','m1'); %Khoi luong thanh 1
m2 = evalin('base','m2'); %Khoi luong thanh 2
r1 = evalin('base','r1'); %Chieu dai thanh noi 1
%
C1 = cos(q(1));
S1 = sin(q(1));
19
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
%
%Ma tran quan tinh
H11 = m1*r1*r1 + m2*q(2)*q(2); %r = q(2)
H12 = 0;
H21 = 0;
H22 = m2;
H = [H11, H12; H21, H22];
%Momen nhot va momen huong tam
v1 = 2*m2*q(2)*dq(2)*dq(1);
v2 = -m2*q(2)*dq(1)*dq(1);
V = [v1;v2];
%Momen trong luc
G1 = (m1*r1 + m2*q(2))*9.81*C1;
G2 = m2*9.81*S1;
G = [G1;G2];
%Nghich dao ma tran H
Hinv = inv(H);
%Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguoc
ddq = -Hinv * (V+G) + Hinv * [M01;F02];

%Bien nho trang thai
dq1 = dq(1); %Toc do khop quay
dq2 = dq(2); %Toc do khop tinh tien
%Tinh gan dung phuong trinh vi phan
q(1) = q(1) + Tk * dq1;
q(2) = q(2) + Tk * dq2;
dq(1) = dq(1) + Tk * ddq(1);
dq(2) = dq(2) + Tk * ddq(2);
5) Kết quả mô phỏng
20
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
21
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
22
Môn học Kỹ thuật Robot Họ tên: Nguyễn Văn Tiến
MSV: 20102312 – Lớp: ĐK-TDDH4-K55
23