Professional Documents
Culture Documents
(123doc) Bai Tap Lon Ky Thuat Robot de 18
(123doc) Bai Tap Lon Ky Thuat Robot de 18
Câu 1:
1
z1
K3
02 z2 z3
03
x3
Z0 x2
K2
y1
01
x1
o Y0
K1
X0
KTĐ ai
K1 0 0
K2 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-1Ai.
Ma trận 0A1 mô tả quan hệ giữa khâu 1 và khâu 0 (khâu cố 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.
2
Cθ i − Sθ i.Cα i Sθ i.Sα i ai.Cθ i
Sθ i Cθ i.Cα i −Cθ i.Sα i ai.Sθ i
0 Sα i Cα i di
0 0 0 1
i-1
Ai =
Cθ1 0 Sθ1 0
Sθ 0 −Cθ1 0
1
0 1 0 0
0 0 0 1
0
A1 =
Cθ 2 0 − Sθ 2 0
Sθ 0 Cθ 2 0
2
0 −1 0 a2
0 0 0 1
1
A2 =
1 0 0 0
0 1 0 0
0 0 1 d3
0 0 0 1
2
A3 =
T=0A1.1A2.2A3=
3
nx ox ax px
n oy ay p y
y
nz oz az pz
0 0 0 1
T=
Trong đó:
n o a
- Vector , , xác định hướng của khung tọa độ tay so với khung tọa độ gốc cố định.
p
- Vector 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.
d) Xác định vị trí tay Robot trong hệ tọa độ gốc khi =30o, , d3=0.1m
Khi robot quay một góc =30o; , d3=0.1m thì ta có ma trận T như sau:
C 30o.C 30o -S30o − C 30o.S30 o 0,3.S30 o − 0,1.C 30 o.C 30 o
o
S30 .C 30
o
C30o − S30 o.S30o −0,3.C 30o − 0,1.S30 o.S30 o
S30o 0 C 30o 0,1.C 30o
0 0 0 1
T= =
3 1 3 0,3
− −
4 2 4 4
3 3 1 0, 6. 3 + 0,1
− −
4 2 4 4
1 3 0,1. 3
0
2 2 2
0 0 0 1
Vậy vị trí của gốc tọa độ gắn lên tay có vị trí như sau:
4
0, 3
4
0, 6. 3 + 0,1
− 4
0,1. 3
2
p 1
=
Câu 2:
a) Xác định momen khớp quay và lực tổng ở các 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 như sau:
Để 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ômen quán tính ở khớp Ji = 0
5
Trình tự xây dựng phương trình động lực học như sau:
Phương trình lagrange của một cơ cấu được xác định như sau:
Trong đó:
Y& &
1 = r1 .cosθ .θ
6
1
K1 = m1r12θ&2
2
y2 = r sin θ
7
Vận tốc của điểm B theo các trục như sau:
x&2 = −r sin θ .θ&+ cos θ .r&
1
K 2 = m2 ( r 2θ&2 + r&2 )
2
Động năng của khớp 2:
• Xác định thế năng của hệ thống
- Thế năng của khớp 1:
P1 = m1 gr1 sin θ
• Hàm Lagrange
Từ các tính toán trên ta có hàm Lagrange như sau:
L = K1 + K 2 − ( P1 + P2 )
1 1
= m1r12θ&2 + m2 (r 2θ&2 + r&2 ) − (m1 gr1 sin θ + m2 gr sin θ )
2 2
Ta có:
∂L
= m1r12θ&+ m2 r 2θ&
∂θ&
d ∂L 2&& 2&&
θ&
÷ = m1r1 θ + m2 r θ + 2m2 rr&
dt ∂θ&
Và
∂L
= −m1 gr1 cos θ − m2 gr cosθ
∂θ
= (m r 2 + m r 2 )θ&
11 2
&+ 2m rr&
2 θ&+ (m r + m r ) g cos θ
11 2
9
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 đưuọc tính như sau:
d ∂L ∂L
F= ÷−
dt ∂r& ∂r
Trong đó:
∂L
= m2 r&
∂r&
d ∂L
÷ = m2 & r&
dt ∂r&
∂L
= m2 rθ&2 − m2 g sin θ
∂r
π
θ&=
16
Mặt khác cơ cấu Robot lại quay từ 0 0 với tốc độ và thời gian khớp 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 Vậy ở cuối hành trình thì
khớp đã quay một góc:
4π π
θ = θ&.t = = ( rad )
16 4
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:
10
r&= 0
& θ&
&= 0
và
= (m r 2 + m r 2 )θ&
1 1 2
&+ 2m rr&
2θ&+ (m r + m r ) g cos θ
11 2
π π
= (2,5.0,52 + 2,5.1, 0 2 ).0 + 2.2,5.1, 0.0,125. + (2,5.0,5 + 2,5.1, 0).9,81.cos = 26,13( Nm)
16 4
m1r12 + m2 r 2 0
H =
0 m2
- ma trận mô men hoặc lực tỉ lệ với gia tốc
11
2m rr& θ&
V = 2 2
&
− m2 rθ
- ma trận mô men và lực nhớt
(m r + m2 r ) gCθ
G= 11
m2 gSθ
-ma trận mô men và lực của trọng lực
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.
M dk = K P ε + Κ D ε&
(24)
Với:
ε = qd − q
là vector sai số vị trí của khớp robot;
KP = ( K p1 , K p 2 )
diag - ma trận đường chéo các kệ số khuếch đại
KD = ( Kd1, K d 2 )
diag - ma trận đường chéo các kệ số đạo hàm
U
ĐK1 ĐK2 R
12
Để 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)
(2-3)
(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)
13
Trong đó:
ξ
- hệ số suy giảm.
4
ωn =
ξ t qd
- tần số dao động.
Hình 2.3. sơ đồ cấu trúc hệ thống điều khiển mô men tính toán
ξ = 0, 7
Chọn chất lượng điều khiển có hệ số suy giảm 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
14
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.
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 , 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:
q&0 = 0
q&c = 0
Ta giả thuyết . mặt khác ta có :
tc qc + q 0
tm = ; qm = .
2 2
qm − q1
q&1 = q&1.t1 =
& ⇒ q&
&1.t1 − q
&1.tc .t1 + qc − q0 = 0
2
tm − t1
1 2
q1 = q0 + .q& &
1t1
2
tc tc2 qc − q0
→ t1 = m −
2 4 q&
&1
tc
t1 <
2
Từ đồ thị trên chúng ta thấy . Do đó :
tc tc2 qc − q0
t1 = − − (2 − 7)
2 4 q&
&1
tc2 qc − q0
−
4 q&
&1
4. qc − q0
⇒ < q&
&1 ≤ 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:
16
1 2
0 − t1 : q ( t ) = q0 + q&
&1t .
2
t1
t1 − t2 : q(t ) = q0 + q& &1t1 (t − )
2
1
t2 − t3 : q(t ) = qc − 2 q& &1t1 (t − tc )
2
Trong đó:
q&
&1
- Chọn nằm trong khoảng theo biểu thức (2-8).
q&
&1
- Biết , tc, qc, q0 ta tính được t1 theo (2-7).
• Kết quả mô phỏng
Mô men và lực của 2 khớp
14
Mo men khop 1
12
10
6 Luc khop 2
0
0 1 2 3 4 5 6
-0.01
-0.02
-0.03
-0.05
khop 2 quay
-0.06
0 1 2 3 4 5 6
17
18
Góc quay θ khớp 1
1.4
1.2
0.8
0.6
0.4
0.2
0
0 1 2 3 4 5 6
1.2
1.1
0.9
0.8
0.7
0.6
0 1 2 3 4 5 6
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.
19
plot(At1,AeTheta,'r') % mo mem khop quay (khop 1)
hold on
plot(At1,Aer) % luc cua khop tinh tien (khop 2)
grid
% ve goc quay khop 1
plot(At1,Aq1)
grid
% do dich chuyen cua khop 2 tinh tien
plot(At1,Aq2)
grid
+ hàm RobotThetaR
function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] =
RobotThetaR(Kp,Kd)
%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;
20
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;
H = [H11, H12; H21, H22];
%Lay thong so qui dao thuc robot chuyen dong duoc qua M
[q, dq] = RobotModel(M,X0,Tk);
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);
21
+ 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
%Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguoc
dX = -Hinv * (V+G) + Hinv*[M01;F02];
22
X = [x11;x12;x21;x22];
+ hàm quiDaoKhopThetaR:
function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t)
+ hàm ControllerPD
function[M,errorTheta, errordTheta] =
ControllerPD(sd,dsd,theta,dtheta,Kp,Kd,G,V,H)
%M = Kp * errorTheta - Kd * dtheta + G;
U = dsd+Kp*errorTheta+Kd*errordTheta;
M=H*U+V+G;
23
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.
24