Bùi Trọng Phúc 2112029

Bài thí nghiệm 1 ĐIỀU KHIỂN HỆ PHI TUYẾN CON LẮC NGƯỢC QUAY

1. Mô hình toán học mô tả hệ thống con lắc ngược quay

Chạy đoạn code sau:

%% Define parameters (use symbolic variable of MATLAB)
syms g;

syms m1 m2 J1 J2 l1 l2 c1 c2;
syms Ra La Kt Ke;

%% Declare system variables (use symbolic variable of MATLAB)

syms th1 dth1 d2th1;
syms th2 dth2 d2th2;
syms Tm;

%%
dth1= fulldiff(th1);
d2th1= fulldiff(dth1);
dth2= fulldiff(th2);
d2th2= fulldiff(dth2);

%%
x2 = l1*cos(th1) + l2*sin(th1)*sin(th2);
y2 = l1*sin(th1) - l2*cos(th1)*sin(th2);
z2 = l2*cos(th2);

%%
dx2 = fulldiff(x2,{th1 th2});
dy2 = fulldiff(y2,{ th1 th2});
dz2 = fulldiff(z2,{ th1 th2});

%%
v2_sq = simplify(dx2^2 + dy2^2 + dz2^2);
v2_sq = simplify((l2*dth1)^2*sin(th2)^2+simplify(v2_sq+
(l2*dth1)^2*(cos(th2)^21)));

%% Potential Energy
% Arm
Ep_1 = 0;
% Pendulum
Ep_2 = g*m2*l2*cos(th2);
% Total potential energy
Ep = Ep_1 + Ep_2;

%% Kinetic Energy
% Kinetic energy of the Arm
Ek_1_r = 0.5*J1*(dth1)^2;
Ek_1_t = 0;
Ek_1 = Ek_1_r + Ek_1_t;
% Kinetic energy of the Pendulum
Ek_2_r = 0.5*J2*(dth2)^2;
Ek_2_t = 0.5*m2*v2_sq;
Ek_2 = Ek_2_r + Ek_2_t;
% Total kinetic energy of the system
Ek = Ek_1 + Ek_2;

%% Langrangian operator
L = Ek - Ep;

%% Theta 1 a
% Derive the components that make up the Euler-Lagrange Equation
dL_dtheta1 = simplify(diff(L,th1));

% d/dt(dL/ddtheta1)
dL_ddtheta1_dt = simplify(fulldiff(diff(L,dth1),{th1 th2 dth1 dth2}));
Ftheta1 = simplify(dL_ddtheta1_dt - dL_dtheta1 + c1*dth1 - Tm)

%% Theta 2
% Derive the components that make up the Euler-Lagrange Equation
dL_dtheta2 = simplify(diff(L,th2));
% d/dt(dL/ddtheta1)
dL_ddtheta2_dt = simplify(fulldiff(diff(L,dth2),{th1 th2 dth1 dth2}));
Ftheta2 = simplify(dL_ddtheta2_dt - dL_dtheta2 + c2*dth2)
Ta có kết quả:
Ftheta1 =
c1*dth1 - Tm + d2th1*(J1 + (m2*(2*l2^2*cos(th2)^21 - 4*l2^2*cos(th2)^2
+ 2*l1^2 + 4*l2^2))/2) + (dth2*m2*(8*dth1*l2^2*cos(th2)*sin(th2) +
2*dth2*l1*l2*sin(th2) - 42*dth1*l2^2*cos(th2)^20*sin(th2)))/2 -
d2th2*l1*l2*m2*cos(th2)
Ftheta2 =
J2*d2th2 + c2*dth2 + d2th2*l2^2*m2 - g*l2*m2*sin(th2) -
2*dth1^2*l2^2*m2*cos(th2)*sin(th2) +
(21*dth1^2*l2^2*m2*cos(th2)^20*sin(th2))/2 - d2th1*l1*l2*m2*cos(th2)
Nhận xét:

Hệ thống nhận được phù hợp với công thức ở phần 3.

Chạy đoạn code sau
%% Declare symbolic variables
syms a b c d e f h c1 c2;
syms vol th1 th2;
syms dth1 dth2;

%% vector X = [d2th1; d2th2]

% Solve equation A.X = B
A = [a+c*(sin(th2))^2 -b*cos(th2);
-b*cos(th2) f]

B = [-c*sin(2*th2)*dth1*dth2-b*dth2^2*sin(th2)-d*dth1+e*vol;
(c*dth1^2*sin(2*th2))/2+h*sin(th2)-c2*dth2]
%% Math model of system
X = inv(A)*B;
X = simplify(X)
Kết quả thu được:
A =
[ c*sin(th2)^2 + a, -b*cos(th2)]
[ -b*cos(th2), f]
B = - b*sin(th2)*dth2^2 - c*dth1*sin(2*th2)*dth2 - d*dth1 +
e*vol(c*sin(2*th2)*dth1^2)/2 - c2*dth2 + h*sin(th2)
X = -(d*dth1*f - e*f*vol + b*c2*dth2*cos(th2) + b*dth2^2*f*sin(th2) -
b*h*cos(th2)*sin(th2) - b*c*dth1^2*cos(th2)^2*sin(th2) +
2*c*dth1*dth2*f*cos(th2)*sin(th2))/(a*f - b^2*cos(th2)^2 +
c*f*sin(th2)^2)
((c*sin(th2)^2 + a)*((c*sin(2*th2)*dth1^2)/2 - c2*dth2 +
h*sin(th2)))/(a*f - b^2*cos(th2)^2 + c*f*sin(th2)^2) -
(b*cos(th2)*(b*sin(th2)*dth2^2 + c*dth1*sin(2*th2)*dth2 + d*dth1 -
e*vol))/(a*f - b^2*cos(th2)^2 + c*f*sin(th2)^2)

4.2 Xây dựng mô hình mô phỏng hệ thống với MATLAB Simulink

Khởi tạo với giá trị:
%% pend_model_init.m
%% Initialize parameters of RIP
J1 = 2.48*10^-2;
J2 = 3.86*10^-3;
m2 = 0.075;
l1 = 0.15;
l2 = 0.148;
g = 9.8;
c1 = 1*10^-4;
c2 = 2.8*10^-3;
Kt = 0.09*5;
Ke = 0.09;
Ra = 7.8;
%% Some constants to simplify model
a = J1 + m2*l1^2;
b = m2*l1*l2;
c = m2*l2^2;
d = Kt*Ke/Ra + c1;
e = Kt/Ra;
f = J2 + m2*l2^2;
h = m2*g*l2;
Kết quả thu được:
(b*c*sin(2*u(3))*cos(u(3))*(u(2)^2) - 2*c*f*sin(2*u(3))*u(2)*u(4) -
2*d*f*u(2) - 2*b*f*sin(u(3))*(u(4)^2) - 2*b*c2*cos(u(3))*u(4) +
2*e*f*u(5) + 2*b*h*cos(u(3))*sin(u(3)))/(2*a*f + c*f - b^2*cos(2*u(3))
- (b^2) - c*f*cos(2*u(3))) (4*a*h*sin(u(3)) + 2*c*h*sin(u(3)) +
c^2*u(2)^2*sin(2*u(3)) - 4*a*c2*u(4) - 2*c*c2*u(4) +
2*a*c*u(2)^2*sin(2*u(3)) - 2*c*h*cos(2*u(3))*sin(u(3)) -
4*b*d*u(2)*cos(u(3)) - c^2*u(2)^2*cos(2*u(3))*sin(2*u(3)) +
4*b*e*u(5)*cos(u(3)) - 4*b^2*u(4)^2*cos(u(3))*sin(u(3)) +
2*c*c2*u(4)*cos(2*u(3)) -
4*b*c*u(2)*u(4)*sin(2*u(3))*cos(u(3)))/(2*(2*a*f + c*f -
b^2*cos(2*u(3)) - b^2 - c*f*cos(2*u(3))))
2. Tuyến tính hóa tại điểm cân bằng trên:
pend_model_init;
%call pend_model_init.m to re-initialize
%% Declare symbolic variables
syms vol th1 th2;
syms dth1 dth2;
%% vector X = [d2th1; d2th2]
% Solve equations A.X = B
A = [a+c*(sin(th2))^2 -b*cos(th2); -b*cos(th2)
 f]
B = [-c*sin(2*th2)*dth1*dth2-b*dth2^2*sin(th2)-d*dth1+e*vol;
(c*dth1^2*sin(2*th2))/2+h*sin(th2)-c2*dth2]
%% Math model of system
X = inv(A)*B;
X = simplify(X);
%% Linearization at equilibrium point
f1 = X(1);
df1_dth1 = diff(f1, th1);
df1_ddth1 = diff(f1, dth1);
df1_dth2 = diff(f1, th2);
df1_ddth2 = diff(f1, dth2);
f2 = X(2);
df2_dth1 = diff(f2, th1);
df2_ddth1 = diff(f2, dth1);
df2_dth2 = diff(f2, th2);
df2_ddth2 = diff(f2, dth2);
A1 = [0 1 0 0;
subs(df1_dth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])
subs(df1_ddth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])
subs(df1_dth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])
subs(df1_ddth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]);
0 0 0 1; subs(df2_dth1, [th1, dth1, th2, dth2, vol], [0, 0, 0,
0, 0])
subs(df2_ddth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])
subs(df2_dth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])
subs(df2_ddth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])];
%% A matrix
As = double(A1);
df1_du=diff(f1,vol);
df2_du =diff(f2,vol);
B1 =[0;
subs(df1_du,[th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]); 0;
subs(df2_du,[th1, dth1, th2, dth2, vol],[0, 0, 0, 0, 0])];
Bs = double(B1)
Với kết quả thu được
As =

0 1.0000 0 0
0 -0.2037 1.2667 -0.0326
0 0 0 1.0000
0 -0.0616 20.1514 -0.5187

Bs =
0
2.2203
0
0.6718