Bai Tap Lon Robot

BÀI TẬP LỚN KỸ THUẬT ROBOT

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

VIỆN ĐIỆN

==============

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

BÀI TẬP LỚN

KỸ THUẬTROBOT

Sinh viên:

Họ và tên: Ngô Văn ĐứcLớp : ĐK&TĐH 4-K54SHSV : 20090793

1 | P a g e


ĐỀ 12

Bài 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 Robotc. Giải thích ý nghĩa của ma trận T.d. Xác định vị trí của tahy Robot trong hệ tọa độ gốc khi θ1=600 , d2 = 0,1m ,

d3 = 0,1m.

Bài 2: Cho Robot θ - r có r1=0.6 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.2 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 phản hồi PD cho từng khớp.c. Mô phỏng hệ thống

2 | P a g e


Bài 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: Sơ đồ minh họ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ạo độ 3 có gốc O3 được gắn vào khâu tác động đuôi.

Chiều của 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.

3 | P a g e


Vậy ta có hệ trục tọa độ như sau:

Hình 1.2: Hệ tọa độ cho các thanh nối

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

Theo hình 1.2 ta 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 0K2 0 d2 - 900 0K3 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-1Ai. 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:

4 | P a g e


Á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:

Từ đó thu được ma trận T như sau:

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:

5 | P a g e


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. 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ị 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 vị trí và hướng của các đ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 gải hệ phương trình động học T trên.

d. Xác định vị trí của tahy Robot trong hệ tọa độ gốc khi θ 1=60 0 , d 2 = 0,1m , d3 = 0,1m.

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

6 | P a g e


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

7 | P a g e

r1

r

X1 X2

Y

Y2

Y1 A

B

θX


Bài 2

a. Hãy xác định momen ở 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.

Ta có mô hình của Robot θ– r như sau :

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

8 | P a g e


Với khớp quay ta có phương trình động lực học như sau:

M= ddt ( ∂ L

∂ θ )−∂ L∂ θ

Với khớp tịnh tiến ta có phương trình động lực học như sau:

F= ddt ( ∂ L

∂ r )−∂ L∂ r

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:

Đạo hàm hai vế theo thời gian thu được thành phần vận tốc theo các trục:

Vận tốc điểm A:

v12= x1

2+ y12=r1

2 θ2


K1=12

m1 r12 θ2

9 | P a g e



Vị trí điểm B trong hệ tọa độ như sau:

x2=r cosθ

y2=r sin θ

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

x2=−r sin θ . θ+cosθ . r

y2=r cosθ .θ+sinθ . r

Vận tốc của B : v22= x2

2+ y22=r2 θ2+ r2


K2=12

m2 (r2 θ2+ r2 )

Xác định thế năng của hệ thống- Thế năng của khớp 1:

P1=m1 g r 1sin θ

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

P1=m2 gr sin θ

Hàm lagrange

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

L=K1+K2− ( P1+P2)=12

m1r12 θ2+ 1

2m2 (r 2 θ2+r2 )−(m1 g r1 sin θ+m2 gr sin θ )

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

10 | P a g e


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

M= ddt ( ∂ L

∂ θ )−∂ L∂ θ

Ta có :

∂ L∂θ

=m1 r12 θ+m2r2 θ

ddt ( ∂ L

∂ θ )=m1 r12 θ+m2r2 θ+2 m2r r θ

Và ∂ L∂θ

=−m1 g r1cos θ−m2 gr cosθ

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

M=m1 r12 θ+m2 r2 θ+2m2 r r θ+(m1r 1+m2 r ) g cosθ

¿(m¿¿1 r12+m2 r2) θ+2 m2r r θ+(m1 r1+m2 r ) g cosθ ¿

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:

F= ddt ( ∂ L

∂ r )−∂ L∂ r

Trong đó:

∂ L∂ r

=m2 r

ddt ( ∂ L

∂ r )=m2 r

∂ L∂ r

=m2 r θ2−m2 g sin θ

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

F=m2 r−m2r θ2+m2 g sin θ

Tím toán với tham số cụ thể

11 | P a g e


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,6 [m] tới vị trí rmax = 1.2 [m] với vận tốc r = 0,1 [m/s] là:

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:

˙θ=

θt=

π36

=π

18[rad /s ]

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:

r=0và θ=0

Mặt khác ở cuối hành trình chuyển động thì :

r = rmax = 1.2 [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:M=m1 r1

2 θ+m2 r2 θ+2m2 r r θ+(m1r 1+m2 r ) g cosθ

¿(m¿¿1 r12+m2 r2) θ+2 m2r r θ+(m1 r1+m2 r ) g cosθ ¿

¿ (1∗0.62+1∗1.22 )∗0+ 2∗1∗1.2∗0.1∗π18

+ (1∗0.6+1∗1.2 ) .9,81. cosπ3=8,87 [Nm]

- Lực tổng của khớ tịnh tiến:F=m2 r−m2r θ2+m2 g sin θ

¿1∗0−1∗1.2∗( π18 )

2

+1∗9,81. sinπ3=8,5 [N ]

b. Thiết kế bộ điều khiển phản hồi PD cho từng khớp

12 | P a g e


Sơ đồ cấu trúc hệ thống điều khiển Robot với bộ điều khiển phản hồi PD có dạng

như sau :

Phương trình luật điều khiển có dạng :

M đk=K P ε−K D q .

Trong đó :

- ε=q¿−q : sai số vị trí của khớp Robot .

- q là tốc độ của khớp Robot .

- q là biến khớp .

- K P=diag ( K p1 ,K p 2 ) - ma trận đường chéo các hệ số khuếch đại .

Ta chọn :

K P=[8000 00 8000]

- K D=diag ( Kd 1 , Kd 2) - ma trận đường chéo các hệ số đạo hàm.

Ta chọn :

K D=[100 00 100]

- M đ k là momen điều khiển .

13 | P a g e


Với robot thì ta có:

Phương trình động lực học dạng ma trận của Rob

M=H (q ) q+V (q , q )+G(q)

Mà :

Ta có các ma trận thành phần của phương trình động lực học Robot :

Bộ điều khiển phản hồi PD cho từng khớp của robot :

Khớp quay:

Khớp tịnh tiến:

Với :

Để đơn giản ta sẽ xây dựng quỹ đạo đặt cho hai khớp là quĩ đạo kép dạng 2-1-2.

Ta tính toán để đảm bảo tay Robot di chuyển từ vị trí ban đầu (x0 y0) đến vị trí cuối cùng là (xc yc) trong thời gian là tc(s). Cách tính toán được trình bày như sau:

14 | P a g e


Hình 2.2 : Dạng quỹ đạo 2-1-2

Quá trình khởi động bậc 2, quá trình chuyển động đều bậc 1, và quá trình hãm dừng cũng là bậc 2.

Ta giả thiết . Mặt khác ta có:

15 | P a g e


Từ đồ thị trên ta nhận thấy t1 < tc /2 (do ta mong muốn chọn như vậy). Do đó ta có:

.(*)

Biểu thức chỉ có nghĩa khi hàm dưới dấu căn lớn hơn không.

Vậy: > 0 .(**)

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

Chọn nằm trong khoảng theo biểu thức (**).

Biết , tc, qc, q0 ta tính được t1 theo (*).

Khi đó quĩ đạo trong các khoảng thời gian như sau:

Ma trận hệ số khuyếch đại tỷ lệ và đạo hàm được chọn dựa theo các yêu cầu về chất lượng truyền động như là thời gian quá độ và độ quá điều chỉnh.

c. Mô phỏng hệ thống bằng Matlab - Simulink

Chương trình mô phỏng được xây dựng bằng phần mềm MatLab Simulink.

Chương trình có các tính năng chính sau:

16 | P a g e


Có thể điều chỉnh các tham số của bộ điều khiển phản hồi PD ở chương

trình thông qua đó xem xét các đáp ứng, từ đó tìm ra bộ điều khiển mang

lại chất lượng cao nhất.

Có thể xem ngay đáp ứng của hệ thống sau khi đã lựa chọn tham số điều

khiển cũng như các tham số về vị trí đầu, cuối và thời gian mô phỏng.

Khi chạy chương trình ta có chế độ mặc định như sau:

Vị trí ban đầu (0.6 0)

Vị trí cuối (0 1.2)

Thời gian đáp ứng của Robot t = 6(s)

K P=[8000 00 8000]

K D=[100 00 100]

17 | P a g e


Đồ thị mô phỏng thu được.

18 | P a g e


Code các file chương trình mô phỏng.

1. RobotThetaR.m:

%chuong trinh mo phong he thong dieu khien vi tri robot hai khop thetaR%controller: Dieu khien vi tri voi bo dieu khien phan hoi PDfunction[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd)tc = 6; %Dat thoi gian di chuyen cua tay Robotm1 = 1 ; %Khoi luong thanh 1m2 = 1 ; %Khoi luong thanh 2r1 = 0.6; %Chieu dai thanh noi 1Kp=8000;% chon he so khuyech daiKd=100;%chon he so dao ham%Vi tri cua tay ban dau va cuoiS0 = [0.6 0];Sc = [0 1.2]; %Chuyen vi tri ban dau cua tay Robot sang vi tri cac khopq0 = DHN(S0);qc = DHN(Sc); %Khac phuc truong hop chia 0if (qc(2)==q0(2)); qc(2) = qc(2)-0.001;endif (qc(1)==q0(1)); qc(1) = qc(1)-0.001;end %Tinh toan gia toc hai khop tinh tien va quayddq1 = 1.3*4*abs(qc(1)-q0(1))/tc^2; %Gia toc khop quayddq2 = 1.3*4*abs(qc(2)-q0(2))/tc^2; %Gia toc khop tinh tien %Xac dinh cac khoang thoi gian chuyen dongkhoi dong, deu va ham cho cac khopt11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang toct21 = 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/2t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2; %Thoi gian tang toct22 = 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('text.txt','w');i = 0; %Bien dung de demfor t = 0:0.001:tc; i = i+1; j(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

19 | P a g e


%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]; %Tinh momen can thiet de thuc hien chuyen dong [M,xe,fe] = Controller(qd,dqd,q,dq,Kp,Kd); %Lay thong so qui dao thuc robot chuyen dong duoc qua M [q, dq] = Robot(M,X0,Tk); %Lay cac thong so de ve do thi %------------------------------------------------------ qdd1(i)= qd(1); qdd2(i) = qd(2); q1(i) = q(1); %Goc quay khop quay q2(i) = q(2); %r dq1(i) = dq(1);%Toc do khop quay dq2(i) = dq(2);%Toc do khop tinh tien M1(i) = M(1); %Mo men khop quay M2(i) = M(2); %Luc truyen dong cho khop tinh tien eTheta(i) = qd(1) - q(1); %Sai lech goc quay er(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 %Luu du lieu vao file fprintf(file1,'%3.4f %3.4f %3.4f %3.4f %2.4f %3.4f %3.4f\n',t,qd(1),q(1),qd(2),q(2),M(1),M(2));end%ve do thi mo phongsubplot(3,2,1)plot(j,qdd1);hold on;%ve do thi gia tri dat khop quayplot(j,q1,'r--');%ve do thi gia tri thuc khop quaygrid on;title('Hinh1.goc quay cua khop 1(rad)')xlabel('t(s)')%--------------------subplot(3,2,2)plot(j,qdd2);hold on;% ve do thi gia tri dat khop tinh tienplot(j,q2,'r--');%ve do thi gia tri thuc khop tinh tiengrid on;title('Hinh2.quang duong cua khop tinh tien 2(m)')xlabel('t(s)')ylabel('r(m)')%---------------subplot(3,2,3)plot(j,eTheta);hold on;% do thi bieu dien sai lech giua tin hieu thuc %va tin hieu dat khop quayplot(j,er,'r--');% do thi bieu dien sai lech giua tin hieu thuc %va tin hieu dat khop tinh tiengrid on;title(' Hinh3.sailechgiuatinhieuthucvadat')xlabel('t(s)')

20 | P a g e


ylabel('e')%---------------subplot(3,2,4)plot(j,M1);hold on;%ve di thi momen khop quayplot(j,M2,'r--');%ve di thi luc khop tinh tiengrid on;title(' Hinh4.momen(N.m)va luc(N)')xlabel('t(s)')ylabel('M')%---------------------subplot(3,2,5)plot(j,dq1);%do thi bieu dien toc do khop quaygrid on;title('Hinh5.toc do khop quay(rad/s)')xlabel('t(s)')%--------------------subplot(3,2,6)plot(j,dq2);%do thi bieu dien toc do khop quaygrid on;title(' Hinh6.toc do khoptinh tien (m/s)')xlabel('t(s)') fclose(file1);

2. Robot.m: Mô hình động lực học của Robot, được xây dựng dựa trên mô hình động lực học của Robot Theta-R đã trình bày ở trên.

function[q,dq] = Robot(M,X0,Tk)%-------------------------------------------M01 = M(1); %Mo men dieu khien cho khop quayF02 = M(2); %Luc dieu khien cho khop tinh tien x11 = X0(1); %Goc Theta1x12 = X0(2); %Toc do goc khop quayx21 = X0(3); %Gia tri rx22 = X0(4); %Toc do khop tinh tien%-------------------------------------------%Cac thong so cua Robotm1 = 2 ; %Khoi luong thanh 1m2 = 2 ; %Khoi luong thanh 2r1 = 0.55; %Chieu dai thanh noi 1%-------------------------------------------C1 = cos(x11);S1 = sin(x11);%---------------------------------------------%Ma tran quan tinhH11 = m1*r1^2 + m2*x21^2; %r = x21H12 = 0;H21 = 0;H22 = m2;H = [H11, H12; H21, H22]; %Momen nhot va momen huong tamv1 = 2*m2*x21*x22*x12;v2 = -m2*x22*x12*x12;

21 | P a g e


V = [v1;v2]; %Momen trong lucG1 = (m1*r1 + m2*x21)*9.81*C1;G2 = m2*9.81*S1;G = [G1;G2]; %Nghich dao ma tran HHinv = inv(H); %Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguocdX = -Hinv * (V+G) + Hinv*[M01;F02]; %Phuong trinh trang thaix11p = x12; %Toc do khop quayx21p = x22; %Toc do khop tinh tienx12p = dX(1); %Gia toc khop quayx22p = dX(2); %Gia toc khop tinh tien %Tinh gan dung phuong trinh vi phanx11 = x11 + Tk * x11p;x21 = x21 + Tk * x21p;x12 = x12 + Tk * x12p;x22 = x22 + Tk * x22p; X = [x11;x12;x21;x22]; %Tinh vi tri tay Robots = [x21; 0];q = [x11; x21];dq = [x12; x22];

3. DHN.m: hàm động học ngược của Robot, cho phép tính toán từ vị trí của tay ra

vị trí của các khớp.

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];

4. quiDaoKhop.m: tính toán lượng đặt vị trí khớp và tốc độ các khớp sao cho đảm

bảo đúng quĩ đạo 2-1-2 mong muốn trong khoảng thời gian tc.

function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t) if t<=t1 %Khoi dong q = q0 + ddq*t^2/2;

22 | P a g e


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

5. Controller.m: thực hiện luật điều khiển PD kinh điển.

function[M,errorTheta, errordTheta] = Controller(sd,dsd,theta,dtheta,Kp,Kd) %Cai dat tham so bo dieu khien vi tri phan hoi PDkp1 = Kp; kp2 = Kp; kd1 = Kd; kd2 = Kd; %Tinh sai lech goc quay va dao hamerrorTheta = sd - theta;errordTheta = dsd - dtheta; %Momen dieu khienKp = [kp1,0; 0,kp2];Kd = [kd1,0; 0,kd2]; M = Kp * errorTheta - Kd * dtheta;

23 | P a g e

Documents

Bai Tap Lon Robot