4/30/2013

BÁCH KHOA HÀ NỘI BÀI TẬP LỚN ROBOT CÔNG NGHIỆP

| VAN TRONG
BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

============
BỘ MÔN TỰ ĐỘNG HÓA

ĐỀ 4

BÀI TOÁN 1. Cho robot có cơ cấu như hình vẽ

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 của ma trận T.
d. Xác định vị trí của ta Robot trong hệ tọa độ gốc khi θ1=300 , d2 = 0,2m ,
d3 = 0,1m.

Hình 1.1 kí hiệu minh họa của cơ cấu Robot

BÀI TOÁN 2. Cho Robot θ– r có r1 = 0,5 m ; m1 = m2 = 1Kg . Khớp tịnh tiến chuyển
động với tốc độ r = 0,1 m/s từ r1 đến rmax = 1 m. Robot quay từ góc ban đầu 00 đến góc
600

a. Hãy xác định mô mem ở khớp quay và lực tổng hợp ở khớp tịnh tiến khi Robot ở
cuối hành trình chuyển động.
b. Thiết kế bộ điều khiển “mô mem tính toán ” cho Robot
c. Mô phỏng hệ thống

TRƯƠNG VĂN TRỌNG Page 1

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

BÀI TOÁN 1

a. Xây dựng hệ tọa độ cho các thanh nối

Ta có sơ đồ cánh tay của Robot như sau:

Hình 1.1. kí hiệu minh họa của cơ cấu Robot

Từ sơ đồ cánh tay ta gắn các hệ trục tọa độ như sau:

 Vị trí gốc tọa độ:
- Khâu số 0 thân Robot là khâu cố định có gốc tọa độ O được đặt tại khớp 1
- Khung tọa độ 1 có gốc O1 được gắn vào khớp thứ 2
- Khung tọa độ 2 có gốc O2 được gắn vào khớp thứ 3
- Khung tọa độ 3 có gốc O3 được gắn vào khâu tác động cuối
 Chiều của các khung tọa độ :
- Khung tọa độ số 0 có trục Z0 trùng với trục của khớp 1, trục X0 được chọn
vuông góc với mặt giấy
- Khung tọa độ số 1 có trục Z1 trùng với trục của khớp số 2 và trục X1 chọn
vuông góc với mặt giấy
- Khung tọa độ số 2 có trục Z2 trùng với trục của khớp số 3 và trục X2 được
chọn vuông góc với mặt giấy
- Khung tọa độ số 3 có trục Z3 trùng với phương của trục Z2 và trục X3 vuông
góc với mặt giấy
- Trục Y0, Y1, Y2, Y3 được xác định theo quay tắc bàn tay phải

Từ đó ta xây dựng được hệ trục tọa độ như sau:

TRƯƠNG VĂN TRỌNG Page 2

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

Hình 1.2. vị trí và hướng của các khung tọa độ

b. Xác định ma trận T biểu diễn hệ tọa độ tay Robot

Từ hình 1.2 lập được bảng D-H các tham số của khâu như sau:

KTĐ θi di αi ai
K1 θ1 0 0 0
K2 0 d2 - 900 0
K3 0 d3 0 0

Vị trí của hai khâu liền kề nhau được mô tả bởi một ma trận biến đổi đồng nhất i-
i-1
Ai .Ma trận 0A1 mô tả quan hệ giữa khâu 1 và khâu 0 (khâu cố định gắn với thân
Robot ) , Ma trận 1A2 mô tả quan hệ giữa khâu 2 và khâu 1, Ma trận 2A3 mô tả quan
hệ giữa khâu 3 và khâu 2. Cấu hình của khâu mộtt bất kì nào đó của Robot có thể thu
được bằng cách nhân một số thích hợp các ma trận i-1Ai lại với nhau
Dạng tổng quát của ma trận i-1Ai như sau:

Ci  Si C i Si S i ai Ci 

 S Ci C i Ci S i ai Si 
i 1
Ai   i (1.2)
 0 S i C i di 
 
 0 0 0 1 

TRƯƠNG VĂN TRỌNG Page 3

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

Áp dụng (1.2) để tính toán các ma trân i-1Ai với tham số cho trong bảng D-H như
sau:
C1  S1 0 0 
 S C1 0 0 
0
A1   1
 0 0 1 0
 
 0 0 0 1
1 0 0 0
0 0 1 0 
1
A2  
 0 1 0 d2 
 
0 0 0 1
1 0 0 0
0 1 0 0 
2
A3  
0 0 1 d3 
 
0 0 0 1
Từ đó thu được ma trận T như sau:
T  0 A11 A2 2 A3
C1  S1 0 0  1 0 0 0  1 0 0 0
 S C1 0 0  0 0 1 0  0 1 0 0 
 1
 0 0 1 0  0 1 0 d 2  0 0 1 d3 
   
 0 0 0 1  0 0 0 1  0 0 0 1
C1 0  S1 d3 S1 
 S 0 C1 d3C1 
 1
 0 1 0 d2 
 
 0 0 0 1 

c. Giải thích ý nghĩa của ma trận T

Ma trận T có dạng tổng quát như sau:
 nX oX aX pX 
n oy ay p y 
T  y
 nz oz az pz 
 
0 0 0 1 
Trong đó:
- Véc tơ n , o, a xác định hướng của khung tọa độ tay so với khung tọa độ cố
định ,
- Véc tơ p biểu diễn vị trí của khung tọa độ tay so với khung tọa độ gốc

TRƯƠNG VĂN TRỌNG Page 4

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

 Qua ma trận T người ta có thể phân tích sự hoạt động và lập trình điều khiển
cho Robot.
 Ma trận T có ý nghĩa rất lớn trong bài toán động học thuận và bài toán động
học ngược :
- Động học thuận : khi biết giá trị của biến khớp thay đổi theo thời gian thì vị
ví và hướng của tay Robot sẽ hoàn toàn xác định tại mọi thời điểm
- Động học ngược : khi biết vị trí và hướng của điểm tác động cuối ta hoàn
toàn có thể xác định được giá trị của các biến khớp từ việc giải hệ phương
trình động học T trên.

d. Xác định vị trí Robot trong hệ tọa độ gốc khi 𝜽1=300 , d2 = 0,2[m] , d3 = 0,1 [m]

Khi robot quay một góc θ1 = 300 và chuyển động tịnh tiến các đoạn d2= 0,2m và
d3 = 0,1 m thì ta có ma trận T có giá trị như sau:

C 300 0  S 300 0,1.S 300 

 
S 300 0 C 300 0,1.C 300 
T 
 0 1 0 0, 2 
 
 0 0 0 1 
 3 1 1 
 0  
 2 2 20 
 1 3 3
 2
0
2

20 

 1 
 0 1 0
5 
 
 0 0 0 1 

Vậy vị trí của gốc tọa độ gắn lên tay có vị trí như sau:

 1 
 20 
 
 3
p   20 
 
 1 
 5 
 
 1 

TRƯƠNG VĂN TRỌNG Page 5

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

BÀI TOÁN 2

a. Xác định mô mem khớp quay và lực tổng ở khớp tịnh tiến khi Robot ở cuối hành
trình chuyển động
Ta có mô hình của Robot θ– r như sau :

Y2
r B

Y1
A
r1

θ
X1 X2 X

Hình 2.1. mô hình robot θ-r

Để 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 m1 của xilanh tập trung tại điểm cuối của xilanh tức là điểm A
- Khối lượng m2 của pittong tập trung ở bàn tay Robot tức là tập trung tại điểm
B
- Mô mem quán tính ở các khớp Ji = 0
Trình tự xây dựng phương trình động lực học như saU:
 Xác định hàm lagrange
Phương trình lagrange của một cơ cấu được xác định như sau:
L=K–P
Trong đó :
- K : Tổng động năng của hệ thống
- P : Tổng thế năng của hệ thống
 Với khớp quay ta có phương trình động lực học như sau:

( )
̇

TRƯƠNG VĂN TRỌNG Page 6

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

 Với khớp tịnh tiến ta có phương trình động lực học như sau:
( )
̇
 Xác định động năng của hệ thống
Động năng của hệ thống : K = K1 + K2
Trong đó:
K1 – động năng của khớp 1
K2 – động năng của khớp 2
Theo hình 2.1 ta có:
- Động năng của khớp 1:
Vị trí của điểm A trong hệ tọa độ như sau:
X1  r1.cos
Y1  r1.sin
Đạo hàm hai vế theo thời gian thu được thành phần vận tốc theo các trục:
 
X 1   r1.sin  .
 
Y1  r1.cos .
Vận tốc điểm A:
̇ ̇ ̇
 Động năng của khớp 1:
̇
- Động năng của khớp 2:
Vị trí điểm B trong hệ tọa độ như sau:

Vận tốc của điểm B theo các trục như sau:

̇ ̇ ̇
̇ ̇ ̇
Vận tốc của B :
̇ ̇ ̇ ̇
 Động năng của khớp 2:

( ̇ ̇ )

TRƯƠNG VĂN TRỌNG Page 7

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

 Xác định thế năng của hệ thống

- Thế năng của khớp 1:

- Thế năng của khớp 2:

 Hàm lagrange
Từ các tính toán trên ta có hàm lagrange như sau:
( )
̇ ( ̇ ̇ ) ( )

 Tính Mô men khớp quay (khớp 1)

Mô men quán tính được tính theo công thức :

( )
̇
Ta có :

̇ ̇
̇

( ) ̈ ̈ ̇ ̇
̇
Và

Từ đó mô men khớp quay như sau:

̈ ̈ ̇ ̇ ( )

( ) ̈ ̇ ̇ ( )

 Tính lực khớp tịnh tiến (khớp 2

Lực tác động lên khớp tịnh tiến được tính như sau:

( )
̇

TRƯƠNG VĂN TRỌNG Page 8

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

Trong đó:

̇
̇
( ) ̈
̇
̇

Vậy lực cần tìm là :

̈ ̇
 Tím toán với tham số cụ thể
Robot θ-r có hai bậc tự do gồm khớp 1 là khớp quay, khớp thứ hai là khớp tịnh
tiến. Giả thuyết chuyển động của các khớp này là chuyển động đều. Đồng thời thời
gian để khớp quay quay hết quỹ đạo yêu cầu bằng thời gian mà khớp tịnh tiến thực
hiện hết hành trình làm việc của mình.
Thời gian để cơ cấu tịnh tiến chuyển động từ r1 = 0,5 [m] tới vị trí rmax = 1 [m]
với vận tốc r = 0,1 [m/s] là:
rmax  r1 1  0,5
t   5 [s]
r 0,1
Mặt khác, cơ cấu Robot lại quay từ 00 đến 600 vậy ở cuối hành trình thì khớp đã
quay một góc θ = 600 = π/3 [rad] và thời gian khớp quay quay đúng bằng thời gian
khớp tịnh tiến thực hiện hết hành trình của mình nên ta có vận tốc góc của khớp
quay tính như sau:
̇ ⁄
̇
Tiếp theo ta có khớp quay và khớp tịnh tiến chuyển động đều với vận tốc không
đổi nên:
̈ ̈
Mặt khác ở cuối hành trình chuyển động thì :
r = rmax = 1 [m] và θ = π/3
thay vào các công thức đã xây dựng ở trên được
- Mô mem của khơp quay:
̈ ̈ ̇ ̇ ( )
( ) ̈ ̇ ̇ ( )
( ) ( )

TRƯƠNG VĂN TRỌNG Page 9

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

- Lực tổng của khớ tịnh tiến:

̈ ̇

( )
b. Thiết kế bộ điều khiển “Mô mem tính toán” cho Robot
Phương trình động lực học của cơ cấu Robot θ-r đã trình bày ở trên được viết lại
như sau:

M  (m1.r12  m2 r )  2m2 rr  (m1r1  m2 r ) gc

F m2 r  m2 r 2  m2 gS

Hay viết dưới dạng ma trận:

̅ ( ̅) ̅̈ ( ̅ ̅̇ ) ( ̅) (2-1)
Trong đó:

M 
M    - mô men hoặc lực của khớp
F 

 m r 2  m2 r 2 0
H  11  - ma trận mô men hoặc lực tỉ lệ với gia tốc
 0 m2 

 2m rr 
V   2 2  - ma trận mô men và lực nhớt
 m2 r 

(m r  m2 r ) gC 
G 11  -ma trận mô men và lực của trọng lực
 m2 gS 

Nguyên lý thiết kế bộ điều khiển mô men tính toán là :khử các thành phần phi
tuyến của phương trình động lực học của Robot và phân ly đặc tính động học của các
thanh nối.Kết quả nhận được một hệ thống tuyến tính từ đó dễ dàng sử dụng các
phương pháp thiết kế kinh điển của hệ thống tuyến tính.

TRƯƠNG VĂN TRỌNG Page 10

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

Từ ý tưởng đó ta có cấu trúc bộ điều khiển :

𝑞̅ ∗
U
̅
𝑀 𝐾 𝑞̅ 𝑞̅̇ 𝑞̅̈
ĐK 2 ĐK 1 R
𝑞̅
1
Tuyến tính phi tuyến

Hình 2.2. cấu trúc bộ điều khiển

Để làm được điều đó chọn phương trình mô tả bộ điều khiển mô men tính toán
như sau:
̅̅̅̅̅̅ ( ̅) ̅ ( ̅ ̅̇ ) ( ̅) (2-2)
Từ (2-1) và (2-2) ta có:

̅̈ ̅ (2-3)
Thiết kế luật điều khiển PD cho Robot ta có:

̅ ̅̈ ̇̅ ̅ (2-4)

Từ (2-3) và (2-4) có :

̈̅ ̇̅ ̅

Laplace hóa hai vế của phương trình thu được phương trình sau:

p2  Kd p  K p  0 (2  5)

Mong muốn hệ thống có đáp ứng như khâu giao động bặc 2 có dạng như
sau :

p 2  2n p  n2  0 (2  6)

Trong đó:
 -hệ số suy giảm

4
n  -tần số giao động
 t qd

TRƯƠNG VĂN TRỌNG Page 11

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

Cân bằng phương trình (2-5) và (2-6) thu được:

4 8
K di  2n  2 . 
 tqd tqd
2
 4 
K pi    
2

 t
n
 qd 

Hình 2.3. sơ đồ cấu trúc hệ thống điều khiển mô men tính toán

Chọn chất lượng điều khiển có hệ số suy giảm   0, 7 và thời gian quá độ tqd =
1s thu được tham số bộ điều khiển của khớp i như sau:

8
K di  8
1
2
 4 
K pi     32, 65
 0, 7.1 

Trong bài này bộ điều khiển thiết kế cho hai khớp là giống nhau nên các thông
số bộ điều khiển của hai khớp giống nhau

Suy ra các hệ số bộ điều khiển:

8 0 
Kd   
0 8 
32, 65 0 
Kp  
 0 32, 65

TRƯƠNG VĂN TRỌNG Page 12

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

c. Mô phỏng hệ thống
 Thiết kế quỹ đạo chuyển động cho hai khớp dạng 2 – 1 – 2
Để đảm bảo tay Robot di chuyển từ vị trí ban đầu A(x0 , y0 ) đến vị trí cuối cùng
là B(xc , yc) trong khoảng thời gian tc (s) ta có thể tính toán quỹ đạo như sau:

Hình 2.4. quỹ đạo 2-1-2

Khi nó Robot di chuyển từ A tới B xảy ra 3 quá trình :
- Đầu tiên là quá trình khởi động có dạng bậc 2
- Quá trình chuyển động đều có dạng là đường thẳng bậc 1
- Quá trình hãm có dạng đường cong bậc 2

q0  0
Ta giả thuyết  . mặt khác ta có :
 c
q  0

TRƯƠNG VĂN TRỌNG Page 13

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

 tc qc  q 0
tm  ; qm  .
 2 2
 qm  q1
q1  q1.t1   q1.t12  q1.tc .t1  qc  q0  0
 tm  t1
 1 2
q1  q0  .q1t1
 2
t tc2 qc  q0
 t1  c 
2 4 q1

tc
Từ đồ thị trên chúng ta thấy t1  . Do đó :
2

tc tc2 qc  q0
t1    (2  7)
2 4 q1

Biểu thức (2-7) có nghĩa khi và chỉ khi :

tc2 qc  q0

4 q1
4. qc  q0
  q1  q1cp (2  8)
tc2

Giới hạn của gia tốc phụ thuộc vào độ bền cơ khí của Robot . Như vậy quỹ đạo
thiết kế như sau:

 1 2
0  t1 : q(t )  q0  2 q1t .

 t1
t1  t2 : q(t )  q0  q1t1 (t  )
 2
 1
t 2  t3 : q (t )  qc  q1t1 (t  tc ) 2
2

Trong đó:

- Chọn q1 nằm trong khoảng theo biểu thức (2-8).

- Biết q1 , tc, qc, q0 ta tính được t1 theo (2-7).

TRƯƠNG VĂN TRỌNG Page 14

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

 Kết quả mô phỏng

Mô men và lực của 2 khớp Sai lệch của biến khớp
12 0.02

mo men khop 1
10 0
luc khop 2

8 -0.02
khop 1 tinh tien
6 -0.04

4 -0.06

2 -0.08

0 -0.1
khop 2 quay

-2 -0.12
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Góc quay θ khớp 1 Độ dịch chuyển r khớp 2

1.6 1.2

1.4 1.1

1.2
1

1
0.9
0.8
0.8
0.6

0.7
0.4

0.2 0.6

0 0.5
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Hình 2.5. các tín hiệu thu được từ mô phỏng

 Nhận xét: Góc quay của các biến khớp là θ và r bám sát quỹ đạo 2-1-2 cho trước
với sai số nhỏ và các khớp thực hiện tới đúng điểm dừng cuối của mình

TRƯƠNG VĂN TRỌNG Page 15

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

 Chương trình mô phỏng

 Chương trình chính

%======================================
% CHUONG TRINH CHINH
%=======================================

% khai bao gia tri bo dieu khien

Kp = 32.65
Kd = 8
% goi ham RobotThetaR
[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd)
% ========================================
% do thi tin hieu thu duoc
%=========================================
% ve mo men va luc cua robot tren cung do thi
plot(At1,AM1,'r') % mo mem khop quay (khop 1)
text (1,11,'mo men khop 1')
hold on
plot(At1,AM2) % luc cua khop tinh tien (khop 2)
text(4,11,'luc khop 2')
grid
% ve sai so goc quay va tinh tien tren cung do thi
plot(At1,AeTheta,'r') % mo mem khop quay (khop 1)
text (2.5,-0.11,' khop 2 quay')
hold on
plot(At1,Aer) % luc cua khop tinh tien (khop 2)
text(2,-0.036,'khop 1 tinh tien')
grid
% ve goc quay khop 1
plot(At1,Aq1)
grid
% do dich chuyen cua khop 2 tinh tien
plot(At1,Aq2)
grid

 Xây dựng các hàm

+ hàm RobotThetaR
function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] =
RobotThetaR(Kp,Kd)

tc = 5; %Dat thoi gian di chuyen cua tay Robot.

m1 = 1 ; %Khoi luong thanh 1
m2 = 1 ; %Khoi luong thanh 2
r1 = 0.5; %Chieu dai thanh noi 1
%===Vi tri cua tay ban dau` va` cuoi ==
S0 = [0.5 0]; % r1=0,5 m
Sc = [0 1]; % rmax =1 m

%Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop

TRƯƠNG VĂN TRỌNG Page 16

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP
q0 = DHN(S0);
qc = DHN(Sc);
%Khac phuc truong hop chia 0
if (qc(2)==q0(2));
qc(2) = qc(2)-0.0001;
end
if (qc(1)==q0(1));
qc(1) = qc(1)-0.0001;
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 chuyen dong 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; %Sau Tk(s) ta se tinh toan cac tham so cua robot

%Dieu kien ban dau (So kien)

q = q0; dq = [0;0];
X0 = [q0(1);dq(1);q0(2);dq(2)]; %So kien bien trang thai X

file1 = fopen('RobotThetaR.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
%Tinh toan gia tri dat cho cac khop trong tung khoang thoi gian chuyen
%dong
[qd1, dqd1] = quiDaoKhopThetaR(q0(1),qc(1),ddq1,t11,t21,tc,t); %dqd1
la van
%toc cua khop 1, ddq1 la gia toc
khop 1
[qd2, dqd2] = quiDaoKhopThetaR(q0(2),qc(2),ddq2,t12,t22,tc,t);
qd = [qd1; qd2];
dqd = [dqd1; dqd2];
G11 = (m1*r1 + m2*X0(3))*9.81*cos(X0(1));
G12 = m2*9.81*sin(X0(1));
G1 = [G11;G12];
v1 = 2*m2*X0(3)*X0(4)*X0(2);
v2 = -m2*X0(4)*X0(1)*X0(1);
V1 = [v1;v2];
H11 = m1*r1^2 + m2*X0(3)^2; %r = x21
H12 = 0;
H21 = 0;
H22 = m2;

TRƯƠNG VĂN TRỌNG Page 17

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP
H = [H11, H12; H21, H22];

%Tinh momen can thiet de thuc hien chuyen dong

[M,fe] = ControllerPD(qd,dqd,q,dq,Kp,Kd,G1,V1,H);

%Lay thong so qui dao thuc robot chuyen dong duoc qua M
[q, dq] = RobotModel(M,X0,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
%--------------------------------------------------------

X0 = [q(1); dq(1); q(2); dq(2)]; %Dat lai so kien cho lan tinh sau
%q(1)-> Theta; q(2)->r
% plot(At1,q(1))
%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);

+ hàm RobotModel :
function[q,dq] = RobotModel(M,X0,Tk)
%-------------------------------------------
M01 = M(1); %Mo men dieu khien cho khop quay
F02 = M(2); %Luc dieu khien cho khop tinh tien

x11 = X0(1); %Goc Theta1

x12 = X0(2); %Toc do goc khop quay
x21 = X0(3); %Gia tri r
x22 = X0(4); %Toc do khop tinh tien
%-------------------------------------------
%Cac thong so cua Robot
m1 = 1 ; %Khoi luong thanh 1
m2 = 1 ; %Khoi luong thanh 2
r1 = 0.5; %Chieu dai thanh noi 1
%-------------------------------------------
C1 = cos(x11);
S1 = sin(x11);
%---------------------------------------------
%Ma tran quan tinh

TRƯƠNG VĂN TRỌNG Page 18

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP
H11 = m1*r1^2 + m2*x21^2; %r = x21
H12 = 0;
H21 = 0;
H22 = m2;
H = [H11, H12; H21, H22];

%Momen nhot va momen huong tam

v1 = 2*m2*x21*x22*x12;
v2 = -m2*x22*x12*x12;
V = [v1;v2];

%Momen trong luc

G1 = (m1*r1 + m2*x21)*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
dX = -Hinv * (V+G) + Hinv*[M01;F02];

%Phuong trinh trang thai

x11p = x12; %Toc do khop quay
x21p = x22; %Toc do khop tinh tien
x12p = dX(1); %Gia toc khop quay
x22p = dX(2); %Gia toc khop tinh tien

%Tinh gan dung phuong trinh vi phan

x11 = x11 + Tk * x11p;
x21 = x21 + Tk * x21p;
x12 = x12 + Tk * x12p;
x22 = x22 + Tk * x22p;

X = [x11;x12;x21;x22];

%Tinh vi tri tay Robot

s = [x21; 0];
q = [x11; x21];
dq = [x12; x22];

+ hàm quiDaoKhopThetaR:
function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t)

if t<=t1 %Tang toc

q = q0 + ddq*t^2/2;
dq = ddq * t;
elseif((t>t1) & (t<=t2)) %Chuyen dong deu
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);

TRƯƠNG VĂN TRỌNG Page 19

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP
end
end

+ hàm DHN tính toán động học ngược

function[q] = DHN(S)

%Px = S(1); Py = S(2)

%Theta = q(1); r = q(2)

Px = S(1);
Py = S(2);

Theta = atan2(Py,Px);
r = Px*cos(Theta) + Py*sin(Theta);

q = [Theta; r];

+ hàm ControllerPD
function[M,errorTheta, errordTheta] =
ControllerPD(sd,dsd,theta,dtheta,Kp,Kd,G,V,H)

%Cai dat tham so bo dieu khien vi tri phan hoi PD

kp1 = Kp; kp2 = Kp; kd1 = Kd; kd2 = Kd;

%Tinh sai lech goc quay va dao ham

errorTheta = sd - theta;
errordTheta = dsd - dtheta;

%Momen dieu khien

Kp = [kp1,0; 0,kp2];
Kd = [kd1,0; 0,kd2];

%M = Kp * errorTheta - Kd * dtheta + G;
U = dsd+Kp*errorTheta+Kd*errordTheta;
M=H*U+V+G;

TRƯƠNG VĂN TRỌNG Page 20

BÀI TẬP LƠN ROBOT CÔNG NGHIỆP

Tài liệu tham khảo:

1. TS. Nguyễn Mạnh Tiến . Điều Khiển Robot Công Nghiệp . Nhà xuất bản Khoa
Học và Kỹ Thuật. Hà Nội 2006.
2. TS. Nguyễn Mạnh Tiến . Bài Giảng Robot Công Nghiệp .
3. GS.TSKH . Nguyễn Thiện Phúc . Robot Công Nghiệp . Nhà xuất bản Khoa
Học và Kỹ Thuật. Hà Nội 2006.

TRƯƠNG VĂN TRỌNG Page 21