Họ và tên: Vũ Ngọc Long (Nhóm 7)

MSSV:20162542
BÁO CÁO BÀI TẬP LỚN ROBOT (BÀI TẬP 2).

Đề bài
Cho Robot θ – r có r1 = 0,6 m; m1 = m2 = 3 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 độ θ = π/15 rad/s(12 độ/s). Thời
gian chuyển động là 5s. Giá trị góc ban đầu là 0o.
Xác định góc θ của Robot ở cuối hành trình chuyển động.
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.
Thiết kế bộ điều khiển “Bù trọng lượng”
Mô phỏng hệ thống.

BÀI LÀM
a. Xác định góc θ của Robot ở cuối hành trình chuyển động.
Góc θ của Robot ở cuối hành trình chuyển động:
θc =θ 1+θ . t=0+12.5=60(độ)
b. 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.
Hàm Lagrange là hiệu của tổng động năng K và tổng thế năng L của hệ:
L K P 11\* MERGEFORMAT ()
Momen (lực) theo phương trình chuyển động Lagrange:
d  L  L
M i  Fi    
dt  q i  qi
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 tiến
Xác định tổng thế năng P của hệ :
P  P1  P2 33\* MERGEFORMAT ()
Thế năng thanh số 1:
 P1  m1.g.r1.sin  44\* MERGEFORMAT ()
Thế năng thanh số 2:
P2  m2 .g.r.sin  55\* MERGEFORMAT ()
Thay (4) và (5) vào (3) ta được tổng thế năng P:
P  P1  P2  m1.g.r1.sin   m2 .g .r.sin  66\* MERGEFORMAT ()
Xác định tổng động năng K của hệ:
K  K1  K 2 77\* MERGEFORMAT ()
Động năng thanh nối 1 (J1=0):
v12
K1  m1
2 88\* MERGEFORMAT ()

Trong đó, v1 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:
 X 1  r1.cos   X  r1.sin  .
   1
 Y1  r1.sin   Y1   r1.cos  . 99\* MERGEFORMAT ()
 

v12  X 12  Y12  r12 .sin  2 . 2  r12 .cos  2 . 2  r12 . 2 1010\*
MERGEFORMAT ()
Thay (10) vào (8) ta được:
1
K1  m1r12 2
2 1111\* MERGEFORMAT ()
Động năng thanh nối 2 (J2=0):
v22
K 2  m2
2 1212\* MERGEFORMAT ()
Trong đó, v2 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:
 X 2  r.cos   X  r.sin  .  r .cos 
   2
 Y2  r.sin   Y2  r.cos  .  r .sin 
 
1313\*
MERGEFORMAT ()
v22  X 22  Y22  r 2 . 2  r 2 1414\* MERGEFORMAT ()
Thay (14) vào (12) ta được:
1

K 2  .m2 . r 2 . 2  r 2
2

1515\* MERGEFORMAT ()
Thay (11) và (15) vào (7) ta được tổng động năng K của hệ của hệ:
 1 1

K  K1  K 2  .m1.r12 . 2  .m2 . r 2 . 2  r 2
2 2

1616\*
MERGEFORMAT ()

Hàm Lagrange
Từ các biểu thức (6) và (16) thay vào (1) ta được:
1 1
 
L  K  P  .m1.r12 . 2  .m2 . r 2 . 2  r 2   m1.g .r1.sin   m2 .g.r.sin  
2 2 1717\*
MERGEFORMAT ()
Tính momen khớp quay ( qi   ):
Áp dụng (2) với qi   ta được:
d  L  L
M  
dt     1818\* MERGEFORMAT ()

Từ (18) ta có:
L
 m1.r12 .  m2 .r.

d  L 
dt   
2 

    m1.r1 .  m2 . 2.r.r .θ  r .
 2 

L
   m1.g .r1.cos   m2 .g .r.cos  
 1919\* MERGEFORMAT ()

Thay (19) vào (18) ta được: Momen khớp quay

M  (m1r12  m2 r 2 )  2m2 rr  (m1r1  m2 r ) g cos  2020\*
MERGEFORMAT ()
Tính lực khớp tịnh tiến ( qi  r ):
Áp dụng (2) với qi  r ta được:
d  L  L
F  
dt  r  r 2121\* MERGEFORMAT ()
Từ (21) ta có:
L
 m2 .r
r
d  L  ¨

    m2 . r
dt  r 
L
 m2 .r . 2  m2 .g .sin 
r 2222\* MERGEFORMAT ()
Thay (22) vào (21) ta được: Lực khớp tịnh tiến
 F  m2 r  m2 r 2  m2 g sin  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:
m1  m2  3( Kg )

r1  0, 6m , r  0,1m / s

t  5s  r2  rc  r1  rt
  0.6  5.0, 05  0.85(m)
,
r  0 (do khớp tịnh tiến chuyển động với vận tốc không đổi)

 2   c   / 3, 2  c   / 15,2  c  0 (do khớp quay chuyển động với vận

tốc không đổi)

vào (20) và (23) ta được:
Mc  ( m1r12  m2 rc 2 )  2m2rc r  (m1r1  m2 rc ) g cos  c
 
 0  2  3  0,85  0, 05 
 (3  0, 6  3  0,85)  9,8  cos
15 3
 0, 017  21,315  21.368

Fc  m2 r  m2 rc 2  m2 g sin  c

2
  
 0  3  0,85     2  9,8  sin
 15  3
17 2
  16,974  17.086
1500
c. Thiết kế bộ điều khiển PD “Bù trong lượng” :
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  (m1r12  m2 r )  2m2 rr  (m1r1  m2 r ) g cos 
F m2 r  m r 2
2  m2 g sin 

Viết lại dưới dạng phương trình ma trận:

M  H (q )q  C (q , q )q  G (q )

Trong đó,
 
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,
 m1r12  m2 r 2 0
H  
 0 m2 
: Ma trận thành phần tỉ lệ với gia tốc
 m rr m2 r 
C 2 
 m2 r
 0 
: Ma trận thành phần tỉ lệ với tốc độ
(m r  m2 r ) g cos  
G 11 
 m2 g sin   : Ma trận mô-men và lực của trọng lực.

Mục đích bộ điều khiển: khử ảnh hưởng của G (q ) tới độ chính xác tĩnh (
eq  0
không phụ thuộc G).
Phương trình luật điều khiển PD bù trọng lượng có dạng:
M dk  G (q )  K P  K D q 2424\* MERGEFORMAT ()

Trong đó:




    *    q
q      *   
 r   r  r  r 
 k p 0
KP  
 0 k pr 
k 0
K D   d
 0 kdr 

Thay các biểu thức trên vào (24) ta được:


M dk  (mˆ 1 r1  mˆ 2 r2 ).g .cos   k p    k d 

 Fdk  mˆ 2 .g.sin   k pr  r  kdr r
a. 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:
  60 0 
KP   
 0 60 
10 0 
KD   
 0 10 
 Phụ lục

1. File chương trình mô phỏng

[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer]
= deal([]);
% ------------------------------------------
Kp = 60;
Kd = 10;
m1 = 3; %Khoi luong thanh 1
m2 = 3; %Khoi luong thanh 2
r1 = 0.6; %Chieu dai thanh noi 1
r0 = r1; %Vi tri dau khop tinh tien
rc = 0.85; %Vi tri cuoi khop tinh tien
theta0 = 0; %Vi tri dau khop quay
thetac = pi/3; %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];
%q0 = [0;0.55];
%qc = [pi/6;1.2];
%tc = 6;

%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 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; %Chu ky lay mau cua mo hinh robot

%Dieu kien ban dau (So kien)

q = q0; dq = [0;0];
%Mo phong
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
%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 toan trong luong

G1 = (m1*r1 + m2*q(2))*9.81*cos(q(1));
G2 = m2*9.81*sin(q(1));
 G = [G1;G2];

%Tinh momen can thiet de thuc hien chuyen dong

[M,xe,fe] = Controller(qd,dqd,q,dq,Kp,Kd,G);

%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');
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
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,errorTheta, errordTheta] =
Controller(sd,dsd,theta,dtheta,Kp,Kd,G)
% Bo dieu khien bu trong luc
%Tham so bo dieu khien vi tri phan hoi PD
Kpp = diag([Kp Kp]);
Kdd = diag([Kd Kd]);
%Tinh sai lech goc quay va dao ham goc quay
errorTheta = sd - theta;
errordTheta = dsd - dtheta;
%Momen dieu khien bu trong luc
M = Kpp * errorTheta - Kdd * dtheta + G;

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));
%---------------------------------------------
%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);