TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN ĐIỆN
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG
BÁO CÁO
BÀI TẬP DÀI KỸ THUẬT ROBOT
Sinh viên thực hiện: Trần Đình Thiêm
Lớp: KSTN – ĐKTĐ – K55
MSSV: 20101639
Hà nội, ngày 5 tháng 12 năm 2013
1
BÀI TẬP LỚN KỸ THUẬT ROBOT ĐỀ 13
Câu 1: Cho Robot có cơ cấu có 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 tay robot trong hệ tọa độ gốc khi =60
o
, d
2
=0.1m, d
3
=0.1m
Câu 2:Cho Robot có r
1
=0.6m, m
1
=m
2
=1kg. Khớp tịnh tiến chuyển động với tốc độ r=0.1m/s
từ r
1

3
o
01
02
0
3
K
1
K
2
K3
X0
Y0
Z0
x1
y1
z1
x2
z2
x3
z3
b) Xác định ma trận T biểu diễn hệ tọa độ tay robot
Từ hình vẽ trên ta lập được bảng D-H các tham số sau:
KTĐ a
i
K1 0 0 0
K2 0 0
K3 0 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-1

S i C i di
θ θ α θ α θ
θ θ α θ α θ
α α
−
 
 
−
 
 
 
 
4
Áp dụng với các tham số bảng D-H ta có:
0
A
1
=
1 1
1 1
0 0
0 0
0 0 1 0
0 0 0 1
C S
S C
θ θ
θ θ
−
 

0 0 0 1
d
 
 
 
 
 
 
Từ đó ta thu được ma trận T như sau:
T=
0
A
1
.
1
A
2
.
2
A
3
=
1 1 3 1
1 1 3 1
2
0
0
0 1 0
0 0 0 1
C S d S

- Vector
n
,
o
,
a
xác định hướng của khung tọa độ tay so với khung tọa độ gốc cố định.
- Vector
p
biểu diễn vị trí của khung tọa độ tay so với khung tọa độ gốc.
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 các biến khớp thay đổi theo thời gian thì vị trí 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 hướng và vị trí 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.
6
d) Xác định vị trí tay Robot trong hệ tọa độ gốc khi =60
o
, d
2
=0.1m, d
3
=0.1m
Khi robot quay một góc =60
o
và chuyển động tịnh tiến các đoạn d
2
=0.1m, d
3

 
− −
 
 
 
 
 
 
−
 
 
 
 
Vậy vị trí của gốc tọa độ gắn lên tay có vị trí như sau:
p
=
3
20
1
20
1
10
1
 
−
 
 
 
 
 

d L L
dt
θ θ
∂ ∂
 
−
 ÷
∂ ∂
 
&
 Với khớp tịnh tiến ta có phương trình động lực học như sau:
F=
d L L
dt r r
∂ ∂
 
−
 ÷
∂ ∂
 
&
• Xác định động năng của hệ thống
Động năng của hệ thống: K = K
1
+ K
2
Trong đó:
K
1
: động năng của khớp 1

&
Vận tốc điểm A:
2 2 2 2 2
1 1 1 1
v x y r
θ
= + =
&
& &
 Động năng của khớp 1:
2 2
1 1 1
1
2
K m r
θ
=
&
- Động năng của khớp 2:
Vị trí của điểm B trong hệ tọa độ như sau:
2
cosx r
θ
=
2
siny r
θ
=
Vận tốc của điểm B theo các trục như sau:
9

&
• Xác định thế năng của hệ thống
- Thế năng của khớp 1:
1 1 1
sinP m gr
θ
=
- Thế năng của khớp 2:
1 2
sinP m gr
θ
=
• Hàm Lagrange
Từ các tính toán trên ta có hàm Lagrange như sau:
1 2 1 2
2 2 2 2 2
1 1 2 1 1 2
( )
1 1
( ) ( sin sin )
2 2
L K K P P
m r m r r m gr m gr
θ θ θ θ
= + − +
= + + − +
& &
&
 Tính Mô men khớp quay (khớp 1)
Mô men quán tính được tính theo công thức:

 
= + +
 ÷
∂
 
& &
&
&& && &
&
&
Và
1 1 2
cos cos
L
m gr m gr
θ θ
θ
∂
= − −
∂
Từ đó mô men khớp quay như sau:
10
2 2
1 1 2 2 1 1 2
2 2
1 1 2 2 1 1 2
2 ( ) cos
( ) 2 ( ) cos
M m r m r m rr m r m r g
m r m r m rr m r m r g

m r
dt r
L
m r m g
r
θ θ
∂
=
∂
∂
 
=
 ÷
∂
 
∂
= −
∂
&
&
&&
&
&
Vậy lực cần tìm là:
2
2 2 2
sinF m r m r m g
θ θ
= − +
&

vậy ở cuối hành trình thì khớp đã quay
một góc
θ
= 60
0
=
π
/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:
3
6 18t
π
θ π
θ
= = =
&
(rad/s)
11
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:
0r =
&&
và
0
θ
=
&&
Mặt khác ở cuối hành trình chuyển động thì:
r = r
max

2
2 2 2
2
sin
1.0 1.1,2. 1.9,81.sin 8.46[ ]
18 3
F m r m r m g
N
θ θ
π π
= − +
 
= − + =
 ÷
 
&
&&
12
b) Thiết kế bộ điều khiển phản hổi PD cho từng khớp
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:
2
1 1 2 2 1 1 2
2
2 2 2
( . ) 2 ( )M m r m r m rr m r m r gc
F m r m r m gS
θ θ θ
θ θ
= + + + +

2
2
2
2m rr
V
m r
θ
θ
 
=
 
−
 
&
&
&
- ma trận mô men và lực nhớt
1 1 2
2
( )m r m r gC
G
m gS
θ
θ
+
 
=
 
 
-ma trận mô men và lực của trọng lực

d d
K K
- ma trận đường chéo các kệ số đạo hàm
Quỹ đạo đặt cho hai khớp có dang 2-1-2 được tính toán đảm bảo tay robot di chuyển từ vị trí
ban đầu đến vị trí cuối cùng trong thời gian
6
c
t s=
Ma trận các hệ số khuếch đại và đạo hàm được lựa chọn
6.25 0
0 6.25
250 0
0 250
d
p
K
K
 
=
 
 
 
=
 
 
14
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(x
0

0
2
1
1 1 1 1 1 1 1 0
1
2
1 0 1 1
2
0
1
1
; .
2 2
. . . . 0
1
.
2
2 4
c c
m m
m
c c
m
c c c
t q q
t q
q q
q q t q t q t t q q
t t
q q q t

<
. Do đó :
2
0
1
1
(2 7)
2 4
c c c
t t q q
t
q
−
= − − −
&&
Biểu thức (2-7) có nghĩa khi và chỉ khi :
2
0
1
0
1 1
2
4
4.
(2 8)
c c
c
cp
c
t q q

t t q t q q t t t

− = +



− = + −



− = − −


&&
&&
&&
Trong đó:
16
- Chọn
1
q
&&
nằm trong khoảng theo biểu thức (2-8).
- Biết
1
q
&&
, tc, qc, q0 ta tính được t1 theo (2-7).
17
• Kết quả mô phỏng

1
1.2
Độ dịch chuyển r khớp 2
0 1 2 3 4 5 6
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
 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
19
Chương trình mô phỏng
%======================================
% CHUONG TRINH CHINH
%=======================================
% khai bao gia tri bo dieu khien
% goi ham RobotThetaR
[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR()
% ========================================
% 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)
hold on
plot(At1,AM2) % luc cua khop tinh tien (khop 2)

end
%Tinh toan gia toc hai khop tinh tien va quay
20
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];

%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
H11 = m1*r1^2 + m2*x21^2; %r = x21
H12 = 0;
H21 = 0;
H22 = m2;

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);
end
end
+ 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
23
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;
24
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à