Dieu khien robot final

Bài tập lớn điều khiển robot UTC Đại Học Giao Thông Vận Tải tai liệu mang tính chất tham khảo mọi người có thể tải về xem và tham khảo cảm ơn mọi người nhiều ạ. mọi ý kiến có thể gửi về email vuvipvai182gmail.com de duocj giai dap em, minh cảm ơn mọi người rất nhiều ạ

TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI

KHOA CƠ KHÍ

BỘ MÔN KỸ THUẬT MÁY

BÀI TẬP LỚN ĐIỀU KHIỂN ROBOT Giáo viên hướng dẫn: PGS.TS Đinh Thị Thanh Huyền

Nhóm sinh viên thực hiện:

Hà Nội, Tháng 05 Năm 2022

CHƯƠNG 1: ĐỘNG LỰC HỌC ROBOT

Hình 1.1 Mô hình cánh tay robot

Giới thiệu về rô bốt.

● Khối lương và chiều dài khâu:

1.1 Đế Robot

Vật liệu : gang xám

Khối lượng: 76,11 kg

Hình 1.2 Đế của robot.

1.2 Khâu 1 :

● Vật liệu : thép cacbon.

● Khối lượng: 18,61 Kg

Hình 1.3 khâu 1 của robot.

1.3 khâu 2:

● vật liệu: thép cacbon.

● Khối lượng: 14,99 Kg

Hình 1.4 khâu 2 của robot.

1.4 khâu 3:

● vật liệu: thép cacbon

● Khối lượng: 14,83 Kg

Hình 1.5 khâu 3 của robot.

1.5 khâu 4:

● vật liệu: thép cabon

● Khối lượng: 5,14 Kg

Hình 1.6 Khâu 4 của robot.

1.6 Chọn hệ quy chiếu, gắn hệ trục tọa độ suy rộng lên các khâu và lập bảng DH.

Hình 1.7 Hệ trục của cánh tay Robot.

● Từ bảng DH ta có ma trận thuần nhất của các khâu:

Do cách chọn trục sai so với quy tắc của bảng ĐH nên tính lại ta có:

=Rot(X,90o) trans(0,L1,0)Rot(Z,2)

● Vecto tọa độ trọng tâm (bỏ qua khâu 0)

● Vecto gia tốc trọng trường

● Momen quán tính tại trọng tâm (kg.m2)

Iyz2= 0,0021

Izz2= 0,429Khâu 3:

Ixx3= 0,0283

Ixy3= - 0,0016

Iyy3= 0,2099

Izz3= 0,2003Khâu 4:

● Code Matlab thiết lập hệ phương trình vi phân chuyển động của Robot:

clc;

syms q1 q2 q3 q4 qd1 qd2 qd3 qd4 qdd1 qdd2 qdd3 qdd4 m1 m2 m3 m4 L1 L2 L3 L4 g dx1 dy1 dz1 dx2 dy2 dz2 dx3 dy3 dz3 dx4 dy4 dz4

%Vector gia toc trong truong

-Iyz1+m1*dy1*dz1 -m1*dy1; Ixz1+m1*dx1*dz1 -Iyz1+m1*dy1*dz1 Izz1)/2+m1*dz1*dz1 -m1*dz1; -m1*dx1 -m1*dy1 -m1*dz1 m1];

(Ixx1+Iyy1J2=[(Ixx2+Iyy2+Izz2)/2+m2*dx2*dx2 Ixy2+m2*dx2*dy2 Ixz2*m2*dx2*dz2 m2*dx2; Ixy2+m2*dx2*dy2 (Ixx2-Iyy2+Izz2)/2+m2*dy2*dy2

-Iyz2+m2*dy2*dz2 -m2*dy2; Ixz2+m2*dx2*dz2 -Iyz2+m2*dy2*dz2 Izz2)/2+m2*dz2*dz2 -m2*dz2; -m2*dx2 -m2*dy2 -m2*dz2 m2];

(Ixx2+Iyy2J3=[(Ixx3+Iyy3+Izz3)/2+m3*dx3*dx3 Ixy3+m3*dx3*dy3 Ixz3*m3*dx3*dz3 m3*dx3; Ixy3+m3*dx3*dy3 (Ixx3-Iyy3+Izz3)/2+m3*dy3*dy3

-Iyz3+m3*dy3*dz3 -m3*dy3; Ixz3+m3*dx3*dz3 -Iyz3+m3*dy3*dz3 Izz3)/2+m3*dz3*dz3 -m3*dz3; -m3*dx3 -m3*dy3 -m3*dz3 m3];

(Ixx3+Iyy3J4=[(Ixx4+Iyy4+Izz4)/2+m4*dx4*dx4 Ixy4+m4*dx4*dy4 Ixz4*m4*dx4*dz4 m4*dx4; Ixy4+m4*dx4*dy4 (Ixx4-Iyy4+Izz4)/2+m4*dy4*dy4

-Iyz4+m4*dy4*dz4 -m4*dy4; Ixz4+m4*dx4*dz4 -Iyz4+m4*dy4*dz4 Izz4)/2+m4*dz4*dz4 -m4*dz4; -m4*dx4 -m4*dy4 -m4*dz4 m4];

(Ixx4+Iyy4-T01=[cos(q1) -sin(q1) 0 0; sin(q1) cos(q1) 0 0; 0 0 1 0; 0 0 0 1];

T12=[cos(q2) -sin(q2) 0 0; 0 0 -1 L1; sin(q2) cos(q2) 0 0; 0 0 0 1];

T23=[cos(q3) -sin(q3) 0 L2; sin(q3) cos(q3) 0 0; 0 0 1 0; 0 0 0 1];

T34=[sin(q4) cos(q4) 0 L3; cos(q4) -sin(q4) 0 0; 0 0 1 0; 0 0 0 1];

10000 + (5906705098477029*qdd1*sin(2*q2))/144115188075855872 +

(810107500971404828791*qd2^2*sin(q2))/36028797018963968000000 +

(5693934785881723169*qdd1*cos(2*q2 + 2*q3))/22517998136852480000 - (3341*qdd2*sin(q2 + q3 - q4))/50000 - (3341*qdd3*sin(q2 + q3 - q4))/50000 + (3341*qdd4*sin(q2 + q3 - q4))/50000 + (307151*qdd2*sin(q2 + q3))/1000000 +

(307151*qdd3*sin(q2 + q3))/1000000 + (70881*qdd1*cos(q3))/100000 -

(810107500971404828791*qdd2*cos(q2))/36028797018963968000000 -

(257*qdd1*cos(q4))/2500 - (771*qdd1*cos(2*q2 + q3 - q4))/5000 -

(3341*qd2^2*cos(q2 + q3 q4))/50000 (3341*qd3^2*cos(q2 + q3 q4))/50000 (3341*qd4^2*cos(q2 + q3 - q4))/50000 +

-(37829124300660220784889*qdd2*sin(q2))/36028797018963968000000 + (70881*qdd1*cos(2*q2 + q3))/100000 - (771*qdd1*cos(q3 - q4))/5000 +

(257*qd1*qd2*sin(2*q2 + 2*q3 - q4))/1250 - (657*qd1*qd2*sin(2*q2 + 2*q3 - 2*q4))/5000 + (257*qd1*qd3*sin(2*q2 + 2*q3 - q4))/1250 -

(657*qd1*qd3*sin(2*q2 + 2*q3 2*q4))/5000 (257*qd1*qd4*sin(2*q2 + 2*q3 q4))/2500 + (657*qd1*qd4*sin(2*q2 + 2*q3 - 2*q4))/5000 -

-(5693934785881723169*qd1*qd2*sin(2*q2 + 2*q3))/11258999068426240000 - (5693934785881723169*qd1*qd3*sin(2*q2 + 2*q3))/11258999068426240000 - (3341*qd2*qd3*cos(q2 + q3 - q4))/25000 + (3341*qd3*qd4*cos(q2 + q3 -

q4))/25000 + (307151*qd2*qd3*cos(q2 + q3))/500000

F2 =(71601064143701390989*qdd2)/22517998136852480000 +

(1479709448968291181*qdd3)/2251799813685248000 - (657*qdd4)/5000 - (70881*qd3^2*sin(q3))/100000 - (257*qd4^2*sin(q4))/2500 -

(257*qd2*qd3*sin(q4))/1250

● Phương trình động lực học của robot là :

F  J q q C q q q G q     

F1 =´q1[2.365+1.25 cos cos2 q2 −0.103 coscos(2q2 +2 q3 −q4)+ 0.0657 cos cos(2 q2 +2 q 3 −2 q 4)+0.04 sin sin 2 q2 +0.253 cos cos(2 q2 +2 q 3)+0.709cos cosq3 −0.709 coscosq 4 −0.15 coscos(2 q2 +q3 −q4)+ 0.709 cos cos(2 q2 +q3)−0.15 coscos(q3 +q4)]+ ´q2[−0.067 sin sin(q2 +q3 −q4)+ 0.3072sin sin(q2 +q3)−0.023 cos(q2)+ 1.05 sin sin(q2)]+ ´q3[−0.067 sin sin(q2 +q3 −q4)+ 0.3072sin sin(q2 +q3)]+ ´q4 0.067 sin sin(q2 +q3 −q4)+ ´q22[1.05cos cosq2 −0.022 sin ⁡(q 2 )−0.067 coscos(q2 +q3 −q4)+0.307 cos cos(q2 +q3)]+ ´q32[−0.067 coscos(q2 +q3 −q4)+0307 coscos(q2 +q3)]−0.067 ´q 42cos cos(q2 +q3 −q4)−0.15 ´q1q´ 3sin sin q3 +0.1 ´q1q´ 4sin sin q4 +0.3 ´q1q´ 2 sin sin(2 q2 +q3 −q4)+0.15 ´q 1q´ 3 sin sin(2 q2 +q3 −q4)−0.15 ´q 1q´ 4 sin sin(2 q2 +q3 −q4)−1.4 ´q1q´ 2 sin sin(2 q2 +q3)−0.78 ´q 1q´ 3 sinsin(2 q2 +q3)+0.15 ´q1 ´q3 sin sin(q3 −q4)−0.15 ´q 1q´ 4 sin sin(q3 −q4)+0.08 ´q1q´ 2 cos cos(2 q2)−2.5 ´q 1q´ 2 sin sin(2 q2)+0.2 ´q1q´ 2 sin sin(2 q2 +2 q 3 −q4)−0.13 ´q1q´ 2 sin sin(2q2 +2 q 3 −2 q 4)+0.2 ´q1q´ 3 sin sin(2 q2 +2 q 3 −q4)−0.13 ´q 1q´ 3 sin sin(2 q2 +2 q 3 −2 q 4)−0.1 ´q 1q´ 4 sin sin(2q2 +2 q3 −q4)+1.3 ´q1 ´q4 sin sin(2 q2 +2 q 3 −2 q 4)−0.5 ´q 1q´ 2 sin sin(2 q2 +2 q3)−0.5 ´q 1q´ 3 sinsin(2q2 +2q3)−0.13 ´q 2q´ 3 coscos(q2 +q3 −q4)+0.13 ´q 3q´ 4 coscos(q2 +q3 −q4)+0.6 ´q 2q´ 3 coscos(q2 +q3)

F2=´q1[−0,067 sin sin(q2+q3−q4)+0,3 sin sin(q2+q3)−0.02 coscos(q2)+1,133∈(q2)]+ ´q2[3.18+1.4 coscos q3−0.2cos cos q4−0.3 cos cos(q3−q4)]+ ´q3[0.66+0.7 cos cos q3−0.2 cos cosq4− 0.154 coscos(q3−q4)]+ ´q4[−0.1314+ 0.1cos cos q4−0.154 coscos(q3−q4)]+ ´q12[−0.154 sin sin(2 q2+q3−q4)+ 0.7 sin sin(2 q2+q3)+2 q2−0,04 cos cos2 q2−0.1 sinsin(2 q2+2q3−q4)+ 0.066 sin sin(2q2+2q3−2 q4)+ 0.25 cos cos(2 q2+2 q3)]+ ´q32[−0.7 sin sin q3+ 0.154 sin sin(q3−q4)]+ ´q42[−0.1 sin sin(q4)+0.15 sin sin(q3−q4)]+0.5 g cos cos(q2+q3−q4)−3.1 g cos cos(q2+q3)−8.4 g coscos(q2)−0.15 gsin q2−0.15 ´q2q´3sin sin(q3)+0.2 ´q2q´4sin sin(q4)+0.2 ´q3q´4sin sin(q4)+0.3 ´q2´q3sin sin(q3−q4)−0.3 ´q2q´4sin sin(q3−q4)−0.3 ´q3q´4sinsin(q3−q4)

F3=´q1[−0.067 sin sin(q2+q3−q4)+0.307 sin sin(q2+q3)]+ ´q2[0.657+0.7 coscos q3−0.2 cos cosq4−0.15 coscos(q3−q4)]+ ´q3[0.657−0.2 cos cosq4]+ ´q4[−0.13+0.1 cos cos q4]+ ´q12

[0.35 sin sin q3−0.077 sin sin(2 q2+q3−q4)+0.35 sin sin(2 q2+q3)−0.077 sin sin(q3−q4)+ 0.1sin sin(2 q2+2q3−q4)+ 0.0657 sinsin(2 q2+q3−2q4)+ 0.25sin sin(2 q2+2q3)]+ ´q22

[0.708sin sin q3−0.15 sin sin(q3−q4)]−0.1 ´q 24sin sin(q4)+0.51 g coscos(q2+q3−q4)−3.1 g cos cos(q2+q3)+0.2 ´q3q´4sin sin(q4)

F4=0.066 ´q1sin sin(q2+q3−q4)+ ´q2[−0.13+0.1 coscos q4+0.15 cos cos(q3−q4)]+ ´q3[−0.13+0.1 cos cosq4]+0.13 ´q4+ ´q12[−0.05 sin sin(q4)+0.077 sin sin(2 q2+q3−q4)+0.077 sin sin(q3−q4)+ 0.5 sin sin(2q2+2 q3−q4)−0.066 sin sin(2 q2+2 q3−2 q4)]+ ´q22[−0.1 sin sin(q4)+0.15 sin sin(q3−q4)]−0.1 ´q32sin sin(q4)+0.5 g coscos(q2+q3−q4)−1.03 g cos cos(q2+q3)−0.2 ´q2q´3sin sin(q4)

CHƯƠNG 2:THIẾT KẾ BỘ ĐIỀU KHIỂN

2.1 Chọn động cơ:

Hình 2.1 Bảng thông số động cơ.

Động cơ khâu 1: ST130 M15015

Khối lượng khâu m1 ¿ 18,61 kg

Chiều dài khâu l1 ¿ 0.13 m

Điện trở phần ứng động cơ: R1 ¿2,41Ω

Moment quán tính phần ứng: Jm1 ¿ 0.0054kgm2

Điện áp phần ứng : V1 ¿ 220(V)

Tỉ số truyền từ động cơ đến các khớp: 𝑟1 ¿ 20 rad/m

Hệ số cản của cơ cấu dẫn động: Bm 1= ¿ 0.0634 Nms/rad

Hệ số sức phản điện động: Kb1 ¿ 1,25Vs/rad

Hệ số moment: K m 1=1.14 Nm/A

Gia tốc trọng trường g ¿9.8 m/s2

Động cơ khâu 2: ST130 M15015

Khối lượng khâu m2 ¿ 14.99 kg

Chiều dài khâu l2 ¿ 0,3 m

Điện trở phần ứng động cơ: R1 ¿1.1Ω

Moment quán tính phần ứng: Jm1 ¿ 0.00277kgm2

Điện áp phần ứng : V1 ¿ 220(V)

Tỉ số truyền từ động cơ đến các khớp: 𝑟1 ¿ 16 rad/m

Hệ số cản của cơ cấu dẫn động: Bm 1= ¿ 0.634 Nms/rad

Hệ số sức phản điện động: Kb1 ¿ 1.25 Vs/rad

Hệ số moment: K m 1=1.58 Nm/A

Gia tốc trọng trường g ¿9.8 m/s2

Động cơ khâu 3: 130ST M15015

Khối lượng khâu m3 ¿ 14.83 kg

Chiều dài khâu l3 ¿ 0,2 m

Điện trở phần ứng động cơ: R1 ¿1.1Ω

Moment quán tính phần ứng: Jm1 ¿ 0.0054kgm2

Điện áp phần ứng : V1 ¿ 220(V)

Tỉ số truyền từ động cơ đến các khớp: 𝑟1 ¿ 15 rad/m

Hệ số cản của cơ cấu dẫn động: Bm 1= ¿ 0.0634 Nms/rad

Hệ số sức phản điện động: Kb1 ¿ 1.25 Vs/rad

Hệ số moment: K m 1=1.14 Nm/A

Gia tốc trọng trường g ¿9.8 m/s2

Động cơ khâu 4: 130ST M15015

Khối lượng khâu m3 ¿ 5,14 kg

Chiều dài khâu l3 = ¿ 0,16m

Điện trở phần ứng động cơ: R1 ¿1.1Ω

Moment quán tính phần ứng: Jm1 ¿ 0.00277kgm2

Điện áp phần ứng : V1 ¿ 220(V)

Tỉ số truyền từ động cơ đến các khớp: 𝑟1 ¿ 10 rad/m

Hệ số cản của cơ cấu dẫn động: Bm 1= ¿ 0.634 Nms/rad

Hệ số sức phản điện động: Kb1 ¿ 1.25 Vs/rad

Hệ số moment: K m 1=1.58 Nm/A

Gia tốc trọng trường g ¿9.8 m/s2

Ta có :

K1=K m 1

R1 =

1,14 2,41=0,47

K2=K m 2

R2

= 1,14 2,41=0,47

d2= 1

16[ ´q1[−0.06 sin sin(q2+q3−q4)+0.3 sin sin(q2+q3)−0.02cos ⁡(q2)]+ ´q2[1.5 coscosq3−0.2 cos cos q4−0.3 cos cos(q3−q4)]+ ´q3[0.6+0.7 coscos q3−0.2 coscos q4−0.15 coscos(q3−q4)]+ ´q4[−0.7+ 0.1cos cosq4−0.15 cos cos(q3−q4)]+ ´q12[−0.15 sin sin(2 q2+q3−q4)+0.7 sin sin(2 q2+q3)+2 q2−0,04 coscos 2 q2−0.1 sin sin(2 q2+2 q3−q4)+ 0.66 sin sin(2 q2+2 q3−2q4)+ 0.25 cos cos(2 q2+2 q3)+ 0.89 sin sin(2q2+2 q3)]+ ´q32[−0.7 sin sin q3+0.15 sin sin(q3−q4)]+ ´q42[− 0.1sin sin(q4)+ 0.15 sin sin(q3−q4)]+0.5 g cos cos(q2+q3−q4)−3.1 g cos cos(q2+q3)−8.4 g cos cos(q2)−0.15 gsinq2−0.15 ´q2q´3sin sin(q3)+0.2 ´q2q´4sin sin(q4)+0.2 ´q3q´4sin sin(q4)+0.3 ´q2q´3sin sin(q3−q4)−0.3 ´q2q´4sin sin(q3−q4)−0.3 ´q3q´4sin sin(q3−q4)]

Khâu 3:

d3= 1

15[ ´q1[−0.067 sin sin(q2+q3−q4)+0.307 sin sin(q2+q3)]+ ´q2[0.657+0.7 cos cosq3−0.2 cos cosq4−0.15cos cos(q3−q4)]+ ´q3[−0.2 coscosq4]+ ´q4[−0.13+0.1 coscos q4]+ ´q12

[0.35 sin sin q3− 0.077 sin sin(2 q2+q3−q4)+ 0.35 sin sin(2 q2+q3)−0.077 sin sin(q3−q4)+ 0.1sin sin(2 q2+2q3−q4)+ 0.0657 sinsin(2 q2+q3−2q4)+ 0.25sin sin(2 q2+2q3)+0.2 sin sin(2 q2+2 q3)]+ ´q22

[0.708 sin sin q3−0.15sin sin(q3−q4)]−0.1 ´q42 sin sin(q4)+0.51 g cos cos(q2+q3−q4)−3.1 g cos cos(q2+q3)+0.2 ´q3q´4sin sin(q4)]

K4=K m 4

R4 =

1,14 2,41=0,47

d4= 1

10[0.066 ´q1 sin sin(q2+q3−q4)+ ´q2[−0.13+ 0.1 coscos q4+0.15 cos cos(q3−q4)]+ ´q3[−0.13+ 0.1cos cosq4]+ ´q12

[−0.05 sin sin(q4)+ 0.077 sin sin(2q2+q3−q4)+0.077 sin sin(q3−q4)+0.5 sin sin(2 q2+2 q3−q4)−0.066 sin sin(2 q2+2 q3−2q4)]+ ´q22

[−0.1 sin sin(q4)+0.15 sin sin(q3−q4)]−0.1 ´q32 sin sin(q4)+0.5 g coscos(q2+q3−q4)−1.03 g cos cos(q2+q3)−0.2 ´q2q´3sin sin(q4)]

t s =w1

n[1,589ξ3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿≤1(4)

Thử chọn ξ=1 thì (3) :ξ w n ≥4,6

5 (4): w n ≥ 0,92

¿ > ¿ chọn w n = 40

Khi đó:

↔ [0,0195S2 + (K1K d+ 0,654).s + K1K p].θ m (s) = K1K d θ m d

(s) - D(s) Đặt Ω(s) = 0,0195S2 + (K1K d+ 0,654).s + K1K p

Đa thức đặc trưng của hệ:

Độ quá điều chỉnh:

t s =w1

n[1,589ξ3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿≤1(4)

Thử chọn ξ=1 thì (3) :ξ w n ≥4,6

5 (4): w n ≥ 0,92

t s= ξ w4,6

n

≤5=¿ξ w n ≥4,6

5 (3)Thời gian tang:

t s =w1

n[1,589ξ3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿≤1(4)Thử chọn ξ=1 thì (3) :ξ w n ≥4,6

5 (4): w n ≥ 0,92

Đa thức đặc trưng của hệ:

t s =w1

n[1,589ξ3−0,156 ξ2+0,924 ξ+1,0141 ¿≤1(4)

Thử chọn ξ=1 thì (3) :ξ w n ≥4,6

5 (4): w n ≥ 0,92

Hình 2.2 sơ đồ Simulink bộ điều khiển PD.

Hình 2.3

● Nhận xét: hệ thống ổn định sau 0,8522(s) và đã bám theo đường mong muốn ban đầu là 10(rad) đảm bảo hệ thống khâu 1 hoạt động ổn định

Hình 2.4

● Nhận xét: hệ thống ổn định sau 0,2(s) nhưng lại có sai số tương đối là 2.67% do

hệ số chọn chưa tối ưu

Hình 2.5

● Nhận xét: hệ thống ổn đinh sau 0,3(s) nhưng vẫn xuât hiện sai số E=0,62% tương đối nhỏ nên hệ thống có thể coi là hoạt động ổn định

Hình 2.7 Sơ đồ Simulink bộ điều khiển PID.

Hình 2.8

● Nhận xét: hệ thống ổn định sau 0,27(s) và đã bám theo đường mong muốn ban đầu là 10(rad) đảm bảo hệ thống khâu 1 hoạt động ổn định

Hình 2.9

● Nhận xét: hệ thống ổn định sau 0,33(s) và chưa bám theo đường mong muốn ban đầu là 7,5(rad) sai số E=2,5% không đảm bảo hệ thống khâu 2 hoạt động ổn định

Hình 2.10

● Nhận xét:hệ thống ổn định sau 0,61(s) và chưa bám theo đường mong muốn ban đầu là 7,5(rad) sai số E=0,3% đảm bảo hệ thống khâu 3 hoạt động ổn định

Hình 2.11

● Nhận xét: hệ thống ổn định sau 0,2(s) và chưa bám theo đường mong muốn ban đầu là 5(rad) sai số E=0,06% đảm bảo hệ thống khâu 4 hoạt động ổn định

2.3 thiết kế bộ điều khiển PD bão hòa.

Hình 2.12

Hình 2.13

❖ Nhận xét: hệ thống ổn định sau 0,5522(s) và đã bám theo đường mong muốn ban

đầu là 10 (rad) đảm bảo hệ thống khâu 1 hoạt động ổn định

Hình 2.14

❖ Nhận xét: hệ thống ổn định sau 0,62(s) và chưabám theo đường mong muốn ban

đầu là 8(rad) sai số E=6.25% hệ thống khâu 2 hoạt động không ổn định

Hình 2.15

❖ Nhận xét: hệ thống ổn định sau 0,61(s) và chưa bám theo đường mong muốn ban

đầu là 7,5(rad) sai số E=0,3% đảm bảo hệ thống khâu 3 hoạt động ổn định

Hình 2.16

❖ Nhận xét: hệ thống ổn định sau 0,2(s) và chưa bám theo đường mong muốn ban

đầu là 5(rad) sai số E=0,06% đảm bảo hệ thống khâu 4 hoạt động ổn định

2.4 Thiết kế bộ điều khiển PD tiếp điểm thuận.

Hình 2.17 Sơ đồ Simulink bộ điều khiển PD gán điểm cực.

Hình 2.18

❖ Nhận xét: Chọn hàm đầu vào Theta1 và Theta4 là hàm cos nên quỹ đạo đầu ra của

khâu 1 ổn định theo mong muốn là hình cos Ở khâu 1 mất một khoảng thời gian rất nhỏ ban đầu để ổn định

Hình 2.19

❖ Nhận xét: Chọn hàm đầu vào Theta2 và Theta5 là hàm sin nên quỹ đạo đầu ra của

khâu 2 ổn định theo mong muốn là hình sin Ở khâu 2 mất một khoảng thời gian ban đầu khoảng 0.17(s) để ổn định

Hình 2.20

❖ Nhận xét: Chọn hàm đầu vào Theta3 và Theta6 là hàm cos nên quỹ đạo đầu ra của

khâu 3 ổn định theo mong muốn là hình cos Ở khâu 3 mất một khoảng thời gian ban đầu khoảng 0.36(s) để ổn định và từ biên dương về

Hình 2.21

❖ Nhận xét: Chọn hàm đầu vào Theta7 và Theta8 là hàm cos nên quỹ đạo đầu ra của

khâu 4 ổn định theo mong muốn là hình cos Ở khâu 4 mất một khoảng thời gian ban đầu khoảng 0.31(s) để ổn định và ở biên dương về

2.5 Thiết kế bộ điều khiển gán điểm cực phản hồi trạng thái.

Ta có được các phương trình như sau:

- Đối với khâu 1:

- Đối với khâu 2:

P = [B A*B A^2*B A^3*B A^4*B];

rank(P);

K=place(A,B,miu);

times=0:0.5:10;

% xac dinh cac bien trang thai cua he kin tuyen tinh

figure(1);

subplot(2,2,1);

grid on ;

subplot(2,2,1);

grid on ;

Hình 2.23

Khâu 3:

Hình 2.24

Khâu 4:

Hình 2.25

2.6 Thiết kế bộ điều khiển toàn phương tuyến tính sử dụng phản hồi trạng thái.

Tín hiệu điều khiển thiết kế của bộ điều khiển toàn phương tuyến tính với mục tiêu cực tiểu hóa hàm chất lượng:

𝐾 = R−1 B T𝑆Với S là ma trận xác định dương xác định từ phương trình Ricaccti:

A T𝑆 + 𝑆𝐴 − 𝑆𝐵R−1B T𝑆 + 𝑄 = 0

Sử dụng phần mềm Matlab có thể tính ma trận K như sau: >> K = lqr(A,B,Q,R)

Các kết quả mô phỏng sau được thực hiện với các ma trận tham số điểu khiển:

𝐾 = 1000 ∗ 𝑜𝑛𝑒𝑠(8); R=[0.3 0 0 0;0 0.5 0 0;0 0 0.5 0;0 0 0 0.5];

Với nhiễu bằng 0;

Điều kiện đầu X0=[0;0;0;0;0.01;0.01;0.01;0.01];

% xac dinh cac bien trang thai cua he kin tuyen tinh

trang thai theo thoi gian

% Tính giá tri cua bien trang thai tai tung thoi diem

xlabel( 'Thoi gian

xlabel( 'Thoi gian