bai tap nghia

8/2/2019 Bai Tap Nghia

http://slidepdf.com/reader/full/bai-tap-nghia 1/20

1

Đề 22

1. Cho Robot có cấu hình như hình vẽ : d2 = 0,3 m ;

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 θ1 = 30o ; θ2 = 30

0;

d3 = 0,1 m .

Hình 1 : Cơ cấu Robot.

2. Cho Robot r - θ có r 1 = 0,55 m ; m1 = m2 = 2 Kg. Khớ p tịnh tiến dịch chuyển từ vị

trí ban đầu r1 đến vị trí cuối ứng vớ i rmax = 1,1 m. Khớ p quay quay vớ i tốc độ

θ = 18 độ /s từ góc ban đầu là 0o đến góc 90

0.

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

b. Thiết k ế bộ điều khiển bù trọng lượ ng .

c. Mô phỏng hệ thống .

a

d

2

3



2

Bài 1

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

Robot hình 1 là robot có cơ cấu tay dạng cầu vớ i 2 khớ p quay và 1 khớ p tịnh

tiến. Trục của khớp quay θ1 vuông góc vớ i trục của khớp quay θ2 , đồng thờ i trục

của khớp quay θ2 cũng vuông góc vớ i trục của khớ p tịnh tiến .

Xây dựng hệ toạ độ cho các thanh nối theo phương pháp Denavit_Hartenberg:

Chọn khung tọa độ gốc (O) và các khung toạ độ thanh nối như hình vẽ

Hình 1.2 : Các khung tọa độ thanh nối của Robot.

Trong đó:

+ Gốc khung toạ độ thanh i đặt ở trục i+1 (cuối thanh i).

+ Trục zi trùng trục khớ p i+1.

+ Trục xi trùng phương ai nhưng đi từ i đến i+1.

+ Khi hai trục cắt nhau ta chọn x, y sao cho khi quay từ zi – 1 thì đến trùng vớ i zi.

y

z

d

d

O

2

3

z 3

z 2

z 1

x1 x2

x3

O 1

O 3

O 2

x



3

Câu 2. Ma trận T biểu diễn hệ tọa độ tay Robot

Các khung tọa độ các thanh nối của Robot đã đượ c thiết k ế ở hình 1.2. Từ đó, ta

xác định đượ c các tham số theo biểu diễn D – H trong bảng sau :

Bảng 1.1 : Bảng D – H của Robot.

Ma trận biểu diễn quan hệ giữa hai khung tọa độ i-1 và i

1i

i A

( 1.2 )

Trong đó :

{ .

Ta sử dụng ma trận ( 1.2 ) và bảng 1.1 thì ta xác định đượ c các ma trận thành

phần1i

i A

biểu diễn quan hệ giữa hai khung tọa độ của hai khớ p i-1, i là :

=

; =

= ;

Thanh ai 1 0 -90o 0

2 0 90o d2

3 0 0 0 d3

A1

0

A2

1

A3

2



4

Như vậy, ta xác định đượ c ma trận T biểu diễn hệ trục tọa độ tay Robot :

0 1 2

3 1 2 3. .T A A A

= . .

=

=

; ( 1.3 )

Trong đó :

{

Câu 3. Ý nghĩa của ma trận T3 Ma trận T3 có 4 hàng 4 cột.

Biểu diễn một khung tọa độ tay Robot trong khung tọa chuẩn .

Biểu diễn vị trí và hướ ng của tay Robot.

3 cột đầu tiên biểu diễn hướ ng của tay Robot, cột thứ tư biểu diễn vị trí của

tay Robot.

+ Cột thứ nhất biểu diễn vectơ pháp tuyến n ( normal ).

+ Cột thứ hai biểu diễn vectơ có hướ ng mà theo đó ngón tay của bàntay nắm vào nhau khi cần nắm đối tượ ng, gọi là vectơ o

(occupation).

+ Cột thứ ba biểu diễn vectơ có hướng mà theo đó bàn tay tiếp cận

với đối tượ ng, gọi là vectơ a ( approach ).

T3



5

Câu 4. Vị trí của tay Robot trong hệ tọa độ gốc

Khi ; ; d2 = 0,3 m ; d3 = 0,1 m

Vị trí tay Robot được xác định bở i ma trận T3 như sau:

= ; ( 1.3 )

=

[

√ √ √

√

√ √

√ (√ )√

]

=

Vậy vị trí tay robot trong hệ tọa độ gốc là: [-0,107 0,285 0,087 1]T

T3



6

Bài 2

Câu 1. 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

Hình 2.1 : Cấu trúc Robot có hai thanh nối.

Dạng tổng quát của phương trình động lực học:

2J

2

vmK

PP

KK

PKL

q

L

q

L

dt

d)F(M

2

ii

2

iii

2

1ii

2

1ii

i

ii

Trong đó :

Mi và Fi lần lượ t là momen của khớ p thứ i và lực tác dụng lên thanhnối thứ i.

q là biến của khớ p quay hoặc khớ p tịnh tiến.

L là một hàm của Lagrange.

Ki và Pi lần lược là động năng và thế năng của thanh nối thứ i.

mi và lần lượ t là khối lượ ng và vận tốc dài của thanh nối thứ i.

1

2

r

r1

m2

m1

x1 x2

y2

y1

y

1

2

r

r1

m2

m1

x1 x2

y2

y1

y

x



7

Ji và lần lượ t là momen quán tính của thanh nối thứ i đối vớ i trục

qua tâm khối của thanh nối thứ i và khớ p quay của thanh nối thứ i.

Ta lần lượt xác định các giá trị Ki và Pi:

1. Xác định động năng và thế năng củ a thanh nố i thứ 1

Động năng của thanh nố i thứ nhấ t là :

K1 = m1 + J1

Vì J1 = 0 nên ta có :

K1 = m1

Theo hình 2.1, ta có :

2

1

2

1

2

1 yxv

1 1 1 1

1 1 1 1

cos sin

sin cos

x r x r

y r y r

Do đó: 2 2 2

1 1

2 21

1 1

.

(2.1)2

v r

r K m

Thế năng của thanh nố i thứ nhấ t là :

P1 = m1gr1 sinθ ( 2.2 )

2. Xác định động năng và thế năng củ a thanh nố i thứ 2

Động năng của thanh nố i thứ 2 là :

K2 = m2 + J2

Vì J1 = 0 nên ta có :

K2 = m2

Tương tự thanh nối thứ 1:



8

2

2

2

2

2

2 yxv

2 2

2 2

cos cos sin

sin sin cos

x r x r r

y r y r r

Do đó:

2 2 2 2

2

2 2 2

2 2

.

2.32

v r r

r r K m

Thế năng của thanh nố i thứ 2 là :

P2 = m2gr sinθ (2.4)

Từ (2.1) và (2.3), ta có tổng động năng của hai thanh nối là :

K = K1 + K2 = )rr(m2

1rm

2

1 222

2

22

11 ( 2.5 )

Từ (2.2) và (2.4), ta có tổng thế năng của hai thanh nối là :

P = P1 + P2 = m1gr1 sinθ + m2gr sinθ ( 2.6 )

Do vậy, từ (2.5) và (2.6), ta có :

singrmsingrm)rr(m2

1rm

2

1PKL 211

222

2

22

11

3. Xác đị nh momen ở khớ p quay của Robot θ - r

Theo phương trình động lực học tổng quát :

ii

ii

q

L

q

L

dt

d)F(M

Đối vớ i khớ p quay q1 = θ, ta có :

LL

dt

dM1



9

Vớ i :

singrmsingrm)rr(m2

1rm

2

1PKL 211

222

2

22

11

cosgrmcosgrmL

rrm2rmrmLdtd

rmrmL

211

22

22

11

2

2

2

11

Do đó :

cosg)rmrm(rrm2)rmrm(

cosgrmcosgrmrrm2rmrmM

2112

2

2

2

11

2112

2

2

2

111

Xác định t ốc độ chuyển động của khớ p t ịnh tiế n

Theo đầu bài, ta có :

= 18o = ( rad/s ) dθ =

dt .

∫

Mà = 90o

= ( rad/s )

Thờ i gian chuyển động của khớ p quay là : Tốc độ chuyển động của khớ p tịnh tiến là :

Theo đề bài, ta đã biết ở cuối hành trình chuyển động :

rmax = 1.1 m

(rad/s)



10

Do khớ p tịnh tiến chuyển động vớ i tốc độ không đổi 0r .

Do khớ p quay chuyển động vớ i tốc độ không đổi

⁄

Thay số vào M1 , ta thu đượ c momen ở khớ p quay Robot θ – r :

M1 = 0 + 2*2*1.1*0.11* + (2*0.55+2*1.1)*9.81*

= 0,152 ( N.m )

4. Xác đị nh l ự c tổ ng ở khớ p t ị nh tiế n của Robot θ - r

Theo phương trình động lực học tổng quát :

ii

iiq

L

q

L

dt

d)F(M

Đối vớ i khớ p tịnh tiến q2 = r, ta có :

r

L

r

L

dt

dF2

Vớ i :

singrmsingrm)rr(m2

1rm

2

1PKL 211

222

2

22

11

Ta có:

singmrm

rmdtd

rm

2

2

2

2

2

r

L

r

L

r

L

=> singmrmrmF 2

2

222



11

Thay số vào F2 , ta thu đượ c lực tổng ở khớ p tịnh tiến khi Robot ở cuối

hành trình chuyển động :

F2 = = 19,4 (N) .

Vậy momen ở 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 là:

M = 0,152 N.m

F = 19,4 N

Câu 2. Thiết kế bộ điều khiển bù trọng lượ ng

Mục đích của bộ điều khiển bù trọng lượ ng (sử dụng cấu trúc PD) là khử ảnh hưở ng

của G(m) đến độ chính xác tĩnh. Sơ đồ cấu trúc hệ thống điều khiển Robot vớ i bộ điều khiển PD có dạng như sau :

Hình 2.2 : Sơ đồ cấu trúc hệ thống điều khiển Robot có bù trọng lượ ng

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

Trong đó :

: sai số vị trí của khớ p Robot .

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

là biến khớ p .



12

( ) - ma trận đườ ng chéo các hệ số khuếch

đại .

Ta chọn :

* +

- ma trận đườ ng chéo các hệ số đạo

hàm.

Ta chọn : * + là momen điều khiển .

là thành phần momen trọng lực .

Vớ i robot r thì ta có:

Phương trình động lực học dạng ma trận của Robot : ( 2.7 )

Mà :

( 2.8 )

Từ ( 2.7 ) và ( 2.8 ) , ta có các ma trận thành phần của phương trình động lực

học Robot :



13

Bộ điều khiển PD bù trọng lượ ng vớ i robot r :

Khớ p quay: 1 1 2( ) os

dk p d M k k m r m r gc

Khớ p tịnh tiến: 2sin

dkr pr r dr M k k r m g

Vớ i :

*

*

r r r

Ở đây để đơ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:

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



14

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ết0 0

0c

q

q

. Mặt khác ta có:

0

11 1 1

1

2

1 0 1 1

; .2 2

.

1.

2

c cm m

m

m

t q qt q

q qq q t

t t

q q q t

2

1 1 1 1 0. . . 0

c cq t q t t q q

2

01

12 4

c c ct t q qt q

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

2

01

12 4

c c ct t q q

t q

.(*)

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:

2

0

14

c ct q q

q

> 0

0

1 1 2

4.c

cp

c

q qq q

t

.(**)

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 1q nằm trong khoảng theo biểu thức (**).

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

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



15

2

1 0 1

11 2 0 1 1

2

2 3 1 1

10 : ( ) .

2

: ( ) ( )2

1: ( ) ( )2

c c

t q t q q t

t t t q t q q t t

t t q t q q t t t

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ề truyền

động đó là thời gian quá độ tqđ và độ quá điều chỉnh % .

Câu 3. 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 vớ i

giao diện đồ họa nhằm tăng khả năng tương tác với người dùng. Chương trình có các

tính năng chính sau:

1. Cho phép điều chỉnh bằng tay các tham số của bộ điều khiển PD bù trọng lượ ng

để 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.

2. Đồ thị đặc tính của các tham số theo thời gian cũng đượ c thể hiện ngay trên

chương trình, ngườ i dùng 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.55 0)

Vị trí cuối (0 1.1)

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

50 0

0 50

Kp

10 0

0 10Kd

Sau đó ta có thể điều chỉnh lại tham số các bộ Kp, Kd nhờ các thanh slider. Sau

khi điều chỉnh ấn nút RUN để thực hiện xem xét các đáp ứng theo thờ i gian.



16

Sau đây sẽ là giao diện chính của chương trình:

Chương trình bao gồm 6 file vớ i các chức năng chính như sau: 1. MoPhongDKRB_PD.m: File chương trình chính, dùng để điều khiển việc tạo

GUI và các thao tác tương tác với ngườ i dùng.

2. RobotThetaR.m:

% File name: RobotThetaR.m %% Purpose : Main Program for ThetaR Robot Control. Using PD. %% Author : Nguyen Duc Nghia_Tu Dong Hoa 4_K50_HUT %%--------------------------------------------------------------------- % 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 = 2 ; %Khoi luong thanh 1 m2 = 2 ; %Khoi luong thanh 2 r1 = 0.55; %Chieu dai thanh noi 1 %Vi tri cua tay ban dau` va` cuoi S0 = [0.55 0]; Sc = [0 1.1];

%Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop q0 = DHN(S0); qc = DHN(Sc);



17

%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; %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]; %Tinh momen can thiet de thuc hien chuyen dong

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

%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



18

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

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

3. RobotModel.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] = 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 = 2 ; %Khoi luong thanh 1 m2 = 2 ; %Khoi luong thanh 2 r1 = 0.55; %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; 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);



19

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

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

5. quiDaoKhopThetaR.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 %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



20

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

function[M,errorTheta, errordTheta] = ControllerPD(sd,dsd,theta,dtheta,Kp,Kd,G)

%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; j = 1;

bai tap nghia

Documents