UBND TỈNH BÌNH DƯƠNG

TRƯỜNG ĐẠI HỌC THỦ DẦU MỘT

VIỆN KỸ THUẬT – CÔNG NGHỆ

BÁO CÁO

THỰC HÀNH ĐIỀU KHIỂN TỰ ĐỘNG NÂNG CAO (0+1)

GIẢNG VIÊN GIẢNG DẠY: Th.s Nguyễn Văn Bình

SINH VIÊN: Nguyễn Đình Khánh Hoàn
MSSV: 2025202160120

Buổi 3, ngày 8 tháng 03 năm 2024

 ĐIỀU KHIỂN LQR (TỐI ƯU) CHO HỆ PENDUBOT

1.Nhập thông số
g=9.81;
l1=0.3;
l2=0.4;
lc1=0.17;
lc2=0.21;
m1=0.1;
m2=0.08;
I1=0.04;
I2=0.06;

q1_init=pi/2+0.001;
q1_dot_init=0.002;
q2_init=0.003;
q2_dot_init=0.004;

teta1=m1*lc1^2+m2*l1^2+I1;
teta2=m2*lc2^2+I2;
teta3=m2*l1*lc2;
teta4=m1*lc1+m2*l1;
teta5=m2*lc2;
2.Tìm A B ( Top )
syms x1 x2 x3 x4 to1
M1=1/(teta1*teta2-
teta3^2*(cos(x3))^2)*(teta2*teta3*sin(x3)*(x2+x4)^2+teta3^2*cos(x3)*sin(x3)*x3^2-
teta2*teta4*g*cos(x1+pi/2)+teta3*teta5*g*cos(x3)*cos(x1+pi/2+x3)+teta2*to1)

M2=1/(teta1*teta2-teta3^2*(cos(x3))^2)*(-
teta3*(teta2+teta3*cos(x3))*sin(x3)*(x2+x4)^2-
(teta1+teta3*cos(x3))*teta3*sin(x3)*(x2)^2+(teta2+teta3*cos(x3))*(teta4*g*cos(x1+pi/
2)-to1)-(teta1+teta3*cos(x3))*teta5*g*cos(x1+pi/2+x3))

A=[0 1 0 0;...
diff(M1,x1) diff(M1,x2) diff(M1,x3) diff(M1,x4);...
0 0 0 1;...
diff(M2,x1) diff(M2,x2) diff(M2,x3) diff(M2,x4)]
B=[0;diff(M1,to1);0;diff(M2,to1)]
3.Tìm K ( Top )

x1=0;x2=0;x3=0;x4=0;to1=0;
A=[
0,
1,
0,
0;
-((7364746379999305*sin(pi/2 + x1))/288230376151711744
- (239413466037369*sin(pi/2 + x1 +
x3)*cos(x3))/288230376151711744)/((7497229028203849*cos(x3)^2)/29514790517935
2825856 - 7337463437988417/2305843009213693952),
-(5906299177873687*sin(x3)*(2*x2 +
2*x4))/(18446744073709551616*((7497229028203849*cos(x3)^2)/2951479051793528
25856 - 7337463437988417/2305843009213693952)),
- ((7497229028203849*x3^2*cos(x3)^2)/295147905179352825856 -
(7497229028203849*x3^2*sin(x3)^2)/295147905179352825856 -
(239413466037369*cos(pi/2 + x1 + x3)*sin(x3))/288230376151711744 -
(239413466037369*sin(pi/2 + x1 + x3)*cos(x3))/288230376151711744 +
(5906299177873687*cos(x3)*(x2 + x4)^2)/18446744073709551616 +
(7497229028203849*x3*cos(x3)*sin(x3))/147573952589676412928)/((7497229028203
849*cos(x3)^2)/295147905179352825856 -
7337463437988417/2305843009213693952) -
(7497229028203849*cos(x3)*sin(x3)*((2288837417020743*to1)/36028797018963968 -
(7364746379999305*cos(pi/2 + x1))/288230376151711744 +
(239413466037369*cos(pi/2 + x1 + x3)*cos(x3))/288230376151711744 +
(5906299177873687*sin(x3)*(x2 + x4)^2)/18446744073709551616 +
(7497229028203849*x3^2*cos(x3)*sin(x3))/295147905179352825856))/(14757395258
9676412928*((7497229028203849*cos(x3)^2)/295147905179352825856 -
7337463437988417/2305843009213693952)^2), -
(5906299177873687*sin(x3)*(2*x2 +
2*x4))/(18446744073709551616*((7497229028203849*cos(x3)^2)/2951479051793528
25856 - 7337463437988417/2305843009213693952));

0,
0,
0,
1;
((40221*sin(pi/2 + x1)*((63*cos(x3))/12500 +
2288837417020743/36028797018963968))/100000 - sin(pi/2 + x1 +
x3)*((1297863*cos(x3))/1562500000 +
103190409/12500000000))/((7497229028203849*cos(x3)^2)/295147905179352825856
- 7337463437988417/2305843009213693952),
(2*x2*sin(x3)*((3969*cos(x3))/156250000 + 315567/1250000000) + sin(x3)*(2*x2 +
2*x4)*((3969*cos(x3))/156250000 +
144196757272306809/450359962737049600000))/((7497229028203849*cos(x3)^2)/
295147905179352825856 - 7337463437988417/2305843009213693952),
(7497229028203849*cos(x3)*sin(x3)*(((63*cos(x3))/12500 +
2288837417020743/36028797018963968)*(to1 - (40221*cos(pi/2 + x1))/100000) +
cos(pi/2 + x1 + x3)*((1297863*cos(x3))/1562500000 + 103190409/12500000000) +
x2^2*sin(x3)*((3969*cos(x3))/156250000 + 315567/1250000000) + sin(x3)*(x2 +
x4)^2*((3969*cos(x3))/156250000 +
144196757272306809/450359962737049600000)))/(147573952589676412928*((74972
29028203849*cos(x3)^2)/295147905179352825856 -
7337463437988417/2305843009213693952)^2) - ((3969*sin(x3)^2*(x2 +
x4)^2)/156250000 + (63*sin(x3)*(to1 - (40221*cos(pi/2 + x1))/100000))/12500 +
(3969*x2^2*sin(x3)^2)/156250000 + (1297863*cos(pi/2 + x1 +
x3)*sin(x3))/1562500000 + sin(pi/2 + x1 + x3)*((1297863*cos(x3))/1562500000 +
103190409/12500000000) - x2^2*cos(x3)*((3969*cos(x3))/156250000 +
315567/1250000000) - cos(x3)*(x2 + x4)^2*((3969*cos(x3))/156250000 +
144196757272306809/450359962737049600000))/((7497229028203849*cos(x3)^2)/
295147905179352825856 - 7337463437988417/2305843009213693952), (sin(x3)*(2*x2
+ 2*x4)*((3969*cos(x3))/156250000 +
144196757272306809/450359962737049600000))/((7497229028203849*cos(x3)^2)/
295147905179352825856 - 7337463437988417/2305843009213693952)];

B=[
0;

-2288837417020743/(36028797018963968*((7497229028203849*cos(x3)^2)/
295147905179352825856 - 7337463437988417/2305843009213693952));

0;((63*cos(x3))/12500 +
2288837417020743/36028797018963968)/((7497229028203849*cos(x3)^2)/
295147905179352825856 - 7337463437988417/2305843009213693952)];
Q=[1 0 0 0;0 1 0 0;0 0 1 0; 0 0 0 1];
R=1;
[Ad,Bd]=c2d(A,B,0.01);
K=dlqr(Ad,Bd,Q,R);
%K=lqr(A,B,Q,R)

4.Chạy mô phỏng Simulink