BÀI CHUẨN BỊ THÍ NGHIỆM 1

ĐIỀU KHIỂN HỆ PHI TUYẾN CON LẮC NGƯỢC QUAY

Họ và tên: Trần Quang Huy
MSSV:1711561
NHÓM: A06
Trình tự:

4.1 Xây dựng mô hình toán mô tả hệ thống con lắc ngược quay
1/Tạo m-file “pend_dynamics1.m”: để có phương trình toán mô tả hệ thống với các biến trạng thái đã
cho, tín hiệu điều khiển là moment động cơ DC Tm.

CODE:
%% 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)^2-1)));
%% 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
% 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)
Kết quả:
Ftheta1 =

J1*d2th1 - Tm + c1*dth1 + d2th1*l1^2*m2 + (d2th1*l2^2*m2)/2 +

(d2th1*l2^2*m2*(2*sin(th2)^2 - 1))/2 + dth2^2*l1*l2*m2*sin(th2) +
d2th2*l1*l2*m2*(2*sin(th2/2)^2 - 1) + dth1*dth2*l2^2*m2*sin(2*th2)

Ftheta2 =

J2*d2th2 + c2*dth2 + d2th2*l2^2*m2 - g*l2*m2*sin(th2) -

dth1^2*l2^2*m2*cos(th2)*sin(th2) - d2th1*l1*l2*m2*cos(th2)

-Ftheta1 =J1*d2th1 - Tm + c1*dth1 + d2th1*l1^2*m2 + (d2th1*l2^2*m2)/2 +

(d2th1*l2^2*m2*(2*sin(th2)^2 - 1))/2 + dth2^2*l1*l2*m2*sin(th2) +
d2th2*l1*l2*m2*(2*sin(th2/2)^2 - 1) + dth1*dth2*l2^2*m2*sin(2*th2)
 -Ftheta2 =J2*d2th2 + c2*dth2 + d2th2*l2^2*m2 - g*l2*m2*sin(th2) -
dth1^2*l2^2*m2*cos(th2)*sin(th2) - d2th1*l1*l2*m2*cos(th2)

2/Tạo một m-file “pend_dynamics2.m” Để tìm vector X là phương tình toán mô tả hệ thống

%% 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ả:
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=

(- b*c*sin(2*th2)*cos(th2)*dth1^2 +
2*c*f*sin(2*th2)*dth1*dth2 + 2*d*f*dth1 + 2*b*f*sin(th2)*dth2^2 +
2*b*c2*cos(th2)*dth2 - 2*e*f*vol - 2*b*h*cos(th2)*sin(th2))/(b^2*cos(2*th2) - c*f -
2*a*f + b^2 + c*f*cos(2*th2))
(2*b*cos(th2)*(b*sin(th2)*dth2^2 + c*dth1*sin(2*th2)*dth2 + d*dth1 -
e*vol))/(b^2*cos(2*th2) - c*f - 2*a*f + b^2 + c*f*cos(2*th2)) -
(((c*sin(2*th2)*dth1^2)/2 - c2*dth2 + h*sin(th2))*(2*a + c -
c*cos(2*th2)))/(b^2*cos(2*th2) - c*f - 2*a*f + b^2 + c*f*cos(2*th2))
X = (- b*c*sin(2*th2)*cos(th2)*dth1^2 + 2*c*f*sin(2*th2)*dth1*dth2 +
2*d*f*dth1 + 2*b*f*sin(th2)*dth2^2 + 2*b*c2*cos(th2)*dth2 - 2*e*f*vol -
2*b*h*cos(th2)*sin(th2))/(b^2*cos(2*th2) - c*f - 2*a*f + b^2 +
c*f*cos(2*th2))

(2*b*cos(th2)*(b*sin(th2)*dth2^2 + c*dth1*sin(2*th2)*dth2 + d*dth1 -

e*vol))/(b^2*cos(2*th2) - c*f - 2*a*f + b^2 + c*f*cos(2*th2)) -
(((c*sin(2*th2)*dth1^2)/2 - c2*dth2 + h*sin(th2))*(2*a + c -
c*cos(2*th2)))/(b^2*cos(2*th2) - c*f - 2*a*f + b^2 + c*f*cos(2*th2))
4.2 Xây dựng mô hình mô phỏng hệ thống với MATLAB Simulink
1/Tạo m-file “pend_model_init.m”
CODE:
%% 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;
2/Xây dựng mô hình mô phỏng Simulink “pend_model.mdl”, Thay u(1)= th1, u(2)= dth1,
u(3)=th2, u(4)= dth2, u(5)= vol=1V
Lần 1: θ1=0, θ2=0

Lần 2: θ1=90, θ2=0

Lần 3: θ1=180, θ2=0
Lần 4: θ1=0, θ2=10
Lần 5: θ1=90, θ2=45

Lần 6: θ1=180, θ2=90

4.3 Tuyến tính hóa tại điểm cân bằng trên
Tạo m-file “pend_linear.m” có nội dung như sau
%%
clear;clc;
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]);
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);
Kết quả thu dc:
A=

[ (7576077791072511*sin(th2)^2)/4611686018427387904 + 2119/80000, -
(59987947036575*cos(th2))/36028797018963968]
[ -(59987947036575*cos(th2))/36028797018963968,
6344296455550557/1152921504606846976]

B=

- (59987947036575*sin(th2)*dth2^2)/36028797018963968 -
(7576077791072511*dth1*sin(2*th2)*dth2)/4611686018427387904 - (43*dth1)/8125 +
(3*vol)/52
(7576077791072511*sin(2*th2)*dth1^2)/9223372036854775808
- (7*dth2)/2500 + (5439*sin(th2))/50000

ans =

ans =

-0.2037

ans =

1.2667

ans =

0
ans =

-0.0616

ans =

20.1514

>>