BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
TRƯỜNG ĐẠI ĐẠI HỌ HỌC BÁCH KHOA HÀ NỘ N ỘI VIỆN VIỆN ĐIỆN ĐIỆN ==============
BỘ MÔN TỰ TỰ ĐỘ ĐỘNG NG HÓA
BÀI TẬ TẬP LỚ LỚ N K Ỹ THUẬ THUẬTROBOT
Sinh viên: H ọ và tên: Lớ p : SHSV :
1|Page
Ngô Văn Đứ c ĐK&TĐH 4-K54 20090793
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
ĐỀ 12 Bài 1: Cho Robot có cơ cấu c ấu như hình vẽ a. b. c. d.
Xây dựng hệ tọa độ cho các thanh nối. Xác định ma tr ận T biểu diễn hệ tọa độ tay Robot Giải thích ý nghĩa củ a ma tr ận T. 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ó r 1=0.6 m; m1 = m2 = 1Kg . Khớ p t ịnh tiến chuyển động 0 vớ i tốc độ r = 0,1 m/s t ừ r 1 đến r max max = 1.2 m. Robot quay t ừ góc ban đầu 0 đến góc 600 a.
b. c.
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 cuối hành trình chuyển động. Thiết k ế bộ điều khiển phản hồi PD cho từng khớ p. Mô phỏng hệ thống
2|Page
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Bài 1: ựng g hệ t ọa ọa độ cho các thanh nố i.i. a. Xây d ự n 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|Page
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
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 ọ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Đ K1 K2 K3
θi θ1 0 0
αi 0 - 900 0
di 0 d2 d3
ai 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 Ai. 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. i-i-1
Dạng tổng quát của ma tr ận i-1Ai như sau: Ci S i i 1 Ai 0 0 4|Page
Si C i
S i S i
C iC i
C i S i
S i
C i
0
0
aiC i
(1.2) d i 1
ai S i
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Áp dụng (1.2) để tính toán các ma trân i-1Ai vớ i tham số cho trong bảng DH như sau: C1 S 1 0 S C 1 0 1 0 A1 0 0 1 0 0 0 1 0 0 0 0 0 1 0 1 A2 0 1 0 d 2 0 0 0 1 1 0 0 0 0 1 0 0 2 A3 0 0 1 d 3 0 0 0 1
0
0 1 0
Từ đó thu đượ c ma tr ận ận T như sau: T 0 A11 A2 2 A3
C1 S 1 0 S C 1 0 1 0 0 1 0 0 0 C1 0 S1 S 0 C1 1 0 1 0 0 0 0
0 1
0
0
0
1
0 0
0 1
0 0 1
1 0 d 2 0 1 0 0 0 1 0 d 3 S 1 d3C 1 d 2 1 0 0
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: n X n y T n z 0
5|Page
oX
aX
p X
oy
ay
py
oz
az
0
0
p z 1
0
0
0
0
0
0
1
d 3
0
0
1
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Trong đó: - Véc tơ n , o, a xác định hướ ng 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 ậ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 ậ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 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: c: Khi biết vị trí và hướ ng 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. 0
d. Xác định vị trí của tahy Robot trong hệ t ọa ọa độ g ốc ốc khi θ 1=60 , d 2 = 0,1m , d 3 = 0,1m.
Khi robot quay một góc θ1 = 600 và chuyển động t ịnh tiến các đoạn d 2= 0,1m và d3 = 0,1 m thì ta có ma tr ận T có giá tr ị như sau: C 600 0 S 600 0,1.S 600 S 600 0 C 60 0 0,1.C 60 0 T 0 1 0 0,1 0 0 1 0 1 3 3 0 2 20 2 3 1 1 0 2 2 20 1 0 0 1 10 0 0 1 0
6|Page
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Vậy vị trí của gốc tọa độ gắn lên tay có v ị trí như sau: 3 20 1 p 20 1 10 1
7|Page
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Bài 2 ực t ổ ổng g hợ p ở kh a. Hãy xác định momen ở kh khớ p quay và l ự n khớ p t ịnh tiế n khi Robot ở cu cuố i hành trình chuy ển động. Ta có mô hình của Robot θ– r θ– r như sau : Y Y2
r
B
Y1
A
r
θ X1
X2
X
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 -r để đơn giản chúng ta giả thuyết: - Khối lượ ng 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 ng m2 c ủa pittong tậ p trung ở bàn bàn tay Robot tức là tậ p trung tại điểm B. - Mô mem quán tính ở các các khớ p Ji = 0. Trình tự tự xây xây dựng dựng phương trình động động lự lự c học học như sau: 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 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|Page
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Vớ i khớp quay ta có phương trình độ ng lực học như sau:
̇ Vớ i khớ p tịnh tiến ta có phương trình độ ng lực học như sau:
̇
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: X 1 r1.cos Y1 r 1.sin
Đạo hàm hai vế theo thời gian thu đượ c thành phần vận tốc theo các tr ục:
X 1 r 1. si sin .
Y1 r1.cos os .
Vận tốc điểm A:
Động năng củ a khớ p 1:
̇ ̇ ̇ ̇
9|Page
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Động năng của khớ p 2: Vị trí điểm B trong hệ tọa độ như sau:
ục như sau: Vận tốc của điểm B theo các tr ục
̇ ̇ ̇ ̇ ̇ ̇ Vận tốc của B : ̇ ̇ ̇ ̇
Động năng của khớ p 2:
( ̇ ̇ )
Xác định thế năng ế năng của hệ thố ng ng - Thế năng của khớ p 1:
- Thế năng của khớ p 2:
Hàm lagrange
Từ các tính toán trên ta có hàm lagrange như sau:
̇ ( ̇ ̇ )
10 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Tính Mô men khớ p quay (khớ p 1)
Mô men quán tính đượ c tính theo công thức :
̇
Ta có :
̇ ̇ ̇ ̈ ̈ ̇ ̇ ̇ Và Từ đó mô men khớp quay như sau: ̈ ̈ ̇ ̇ ̈ ̇ ̇
ự c khớ p t ịnh tiế n (khớ p 2 Tính l ự
Lực tác động lên khớ p tịnh tiến được tính như sau:
̇
Trong đó:
̇ ̇ ̇ ̈ ̇
Vậy lực cần tìm là :
̈ ̇ 11 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Tím toán vớ i tham số c cụ thể
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ừ r 1 = 0,6 [m] tớ i vị trí r max max = 1.2 [m] vớ i vận tốc r = 0,1 [m/s] là: t
rmax r 1 1.2 1.2 0.6 r
0.1
6[s]
Mặt khác, cơ cấu Robot lại quay từ 00 đến 600 vậy ở cu cuối hành trình thì kh ớp đã 0 quay một góc θ = 60 = π/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:
θ ̇ θ ̇ ⁄
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:
̈ θ ̈ Mặt khác ở cu cuối hành trình chuyển động thì : r = r max max = 1.2 [m] và θ = π/3 Thay vào các công thức đã xây dựng ở trên ở trên đượ c - Mô mem của khơp quay:
θ ̈ θ ̈ ̇θ ̇ θ θ ̈ ̇θ ̇ θ - Lực tổng của khớ t tịnh tiến: ̈ θ ̇ θ 12 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
b. Thiế t k ế bộ điề u khiể n phản hồi PD cho t ừ n ế b ừng g khớ p
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 :
̅ ̇ Trong đó : -
̅ : sai số vị trí của khớ p Robot . ̇ là tốc độ của khớ p Robot . là biến khớ p . ( ( ) - ma tr ậnận đườ ng ng chéo các hệ số khuếch đại . Ta chọn :
* + -
13 | P a g e
- ma tr ậnận đườ ng ng chéo các hệ số đạo hàm. Ta chọn : * +
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
-
là momen điều khiển .
Vớ i robot r thì ta có:
Phương trình độ ng lực học dạng ma tr ận của Rob
̈ ̇ 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 r : Khớ p quay: Khớ p tịnh tiến: Vớ i :
*
r r
*
r
Để đơ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
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
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. q0 0 Ta giả thiết . Mặt khác ta có: 0 q c tc qc q 0 t ; q . m 2 m 2 qm q1 q q . t q1.t12 q1.tc .t1 qc q0 0 1 1 1 tm t 1 1 2 q1 q0 .q1t1 2
15 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
t 1
tc
tc2
2
4
qc q0 q1
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ó: t 1
tc 2
tc2 4
qc q0 q1
.(*)
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:
tc2 4
qc q0 q1
> 0 q1cp q1
4. qc q0 2 c
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
q1 nằm trong khoảng theo biểu thức (**).
Biết q , tc, qc, q0 ta tính đượ c t1 theo (*). 1
Khi đó quĩ đạo trong các khoảng thời gian như sau: 1 2 0 : ( ) t q t q q1t . 1 0 2 t 1 : ( ) ( ) t t q t q q t t 1 2 0 11 2 1 2 : ( ) t t q t q q1t1 (t tc ) c 2 3 2
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 ng truyền động như là thời gian quá độ và độ quá điều chỉnh. c. Mô phỏng hệ thố ng ng bằ ng 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
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Có thể điều chỉnh các tham số của bộ điều khiển phản hồi PD ở chương ở 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 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)
* + * +
17 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
m ô ph ỏng thu đượ c. c. Đồ th ị mô
18 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
Code cá các file chương trình m ô ph ỏng. trình mô
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 PD function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] function [At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd) tc = 6; %Dat thoi gian di chuyen cua tay Robot m1 = 1 ; %Khoi luong thanh 1 m2 = 1 ; %Khoi luong thanh 2 r1 = 0.6; %Chieu dai thanh noi 1 Kp=8000;% Kp=8000;% chon he so khuyech dai Kd=100;%chon Kd=100;%chon he so dao ham %Vi tri cua tay ban dau va cuoi S0 = [0.6 0]; Sc = [0 1.2]; %Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop q0 = DHN(S0); qc = DHN(Sc); %Khac phuc truong hop chia 0 if (qc(2)==q0(2)); if (qc(2)==q0(2)); qc(2) = qc(2)-0.001; end if (qc(1)==q0(1)); 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 dongkhoi dong, deu va ham 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('text.txt' fopen('text.txt', ,'w' 'w'); ); i = 0; %Bien dung de dem for t for 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
19 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT [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]; %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 dq(1);%Toc do khop quay dq2(i) = dq(2);%Toc 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 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)); %3.4f\n' ,t,qd(1),q(1),qd(2),q(2),M(1),M(2)); end %ve do thi mo phong subplot(3,2,1) plot(j,qdd1);hold on on; ;%ve do thi gia tri dat khop quay plot(j,q1,'r--' plot(j,q1,'r--'); );%ve %ve do thi gia tri thuc khop quay grid on on; ; title('Hinh1.goc title('Hinh1.goc quay cua khop 1(rad)') 1(rad)') xlabel('t(s)' xlabel('t(s)') ) %-------------------subplot(3,2,2) plot(j,qdd2);hold on on; ;% ve do thi gia tri dat khop tinh tien plot(j,q2,'r--' plot(j,q2,'r--'); );%ve %ve do thi gia tri thuc khop tinh tien grid on on; ; title('Hinh2.quang title('Hinh2.quang duong cua khop tinh tien 2(m)') 2(m)') xlabel('t(s)' xlabel('t(s)') ) ylabel('r(m)' ylabel('r(m)') ) %--------------subplot(3,2,3) plot(j,eTheta);hold on on; ;% do thi bieu dien sai lech giua tin hieu thuc %va tin hieu dat khop quay plot(j,er,'r--' plot(j,er,'r--'); );% % do thi bieu dien sai lech giua tin hieu thuc %va tin hieu dat khop tinh tien
20 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT grid on on; ; title(' title(' Hinh3.sailechgiuatinhieuthucvadat') Hinh3.sailechgiuatinhieuthucvadat') xlabel('t(s)' xlabel('t(s)') ) ylabel('e' ylabel('e') ) %--------------subplot(3,2,4) plot(j,M1);hold on on; ;%ve di thi momen khop quay plot(j,M2,'r--' plot(j,M2,'r--'); );%ve %ve di thi luc khop tinh tien grid on on; ; title(' title(' Hinh4.momen(N.m)va luc(N)' luc(N)') ) xlabel('t(s)' xlabel('t(s)') ) ylabel('M' ylabel('M') ) %--------------------subplot(3,2,5) plot(j,dq1);%do plot(j,dq1);%do thi bieu dien toc do khop quay grid on on; ; title('Hinh5.toc title('Hinh5.toc do khop quay(rad/s)') quay(rad/s)') xlabel('t(s)' xlabel('t(s)') ) %-------------------subplot(3,2,6) plot(j,dq2);%do plot(j,dq2);%do thi bieu dien toc do khop quay grid on on; ; title(' title(' Hinh6.toc do khoptinh tien (m/s)' (m/s)') ) xlabel('t(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. trên. function[q,dq] = Robot(M,X0,Tk) function[q,dq] %------------------------------------------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];
21 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
%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); %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 x11 = x21 = x12 = x22 =
gan x11 x21 x12 x22
dung + Tk + Tk + Tk + Tk
phuong trinh vi phan * x11p; * x21p; * x12p; * x22p;
X = [x11;x12;x21;x22]; %Tinh vi tri tay Robot s = [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] 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];
22 | P a g e
BÀI TẬP LỚ N K Ỹ THUẬT ROBOT
4. quiDaoKhop.m: 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] function [q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t) if t<=t1 if t<=t1 %Khoi dong q = q0 + ddq*t^2/2; dq = ddq * t; elseif((t>t1) elseif ((t>t1) & (t<=t2)) %Chuyen dong deu q = q0 + ddq*t1*(t-t1/2); dq = ddq * t1; elseif((t>t2)&(t<=tc)) 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
5. Controller.m: Controller.m: thực hiện luật điều khiển PD kinh điể n. function[M,errorTheta, errordTheta] = function[M,errorTheta, Controller(sd,dsd,theta,dtheta,Kp,Kd) %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;
23 | P a g e