u – K oa ệ - ệ tử - H ác K oa PHCM

BÀI THÍ NGHIỆM 1

ĐIỀU KHIỂN HỆ PHI TUYẾN

CON LẮC NGƯỢC QUAY
I. MỤC ĐÍCH
Trong bài thí nghiệm này, sinh viên sẽ xây dựng mô hình phi tuyến của hệ con lắc
ngược quay bằng phương pháp Euler-Lagrange. Sinh viên được gợi ý sử dụng
Symbolic Toolbox của MATLAB để kiểm tra lại quá trình tính toán. Dựa trên mô
hình toán đưa ra, sinh viên xây dựng mô hình mô phỏng trên MATLAB Simulink và
thực hiện kiểm chứng lại mô hình đã tìm ra.

Ngoài ra, sinh viên còn sử dụng 3 phương pháp thiết kế bộ điều khiển cân bằng
con lắc ngược quay: tuyến tính hóa tại điểm làm việc tĩnh kết hợp thiết kế bộ điều
khiển LQR, thiết kế bộ điều khiển hồi tiếp tuyến tính hóa và thiết kế bộ điều khiển
trượt. Bộ ước lượng trạng thái được giới thiệu trong trường hợp tín hiệu hồi tiếp
không được quan sát đầy đủ, kết hợp với bộ điều khiển LQR để điều khiển cân bằng
con lắc. Giải thuật swingup đưa con lắc từ vị trí cân bằng dưới (cân bằng bền) đến vị
trí cân bằng trên dựa trên phương pháp năng lượng cũng được giới thiệu trong bài thí
nghiệm này.

Mục tiêu sau khi hoàn thành bài thí nghiệm

 Biết cách xây dựng mô hình toán học mô tả hệ thống cơ học bằng phương pháp
Euler-Lagrange.
 Biết cách sử dụng Symbolic Toolbox của MATLAB hỗ trợ cho việc thành lập mô
hình toán của hệ thống.
 Xây dựng và kiểm chứng mô hình hệ phi tuyến sử dụng MATLAB Simulink.
 Sử dụng phương pháp tuyến tính hóa tại điểm làm việc tĩnh để thiết kế bộ điều
khiển tuyến tính cho hệ phi tuyến. Nhận xét được ảnh hưởng của ma trận trong số Q
và R trong luật điều khiển LQR.
 Thiết kế bộ điều khiển hồi tiếp tuyến tính hóa, bộ điều khiển trượt cho hệ phi
tuyến, đánh giá chất lượng điều khiển.
 Thiết kế bộ ước lượng trạng thái trong trường hợp hồi tiếp tín hiệu không đầy đủ
và có nhiễu.
 Sử dụng phương pháp năng lượng để xây dựng giải thuật swingup cho con lắc và
lựa chọn điều kiện để chuyển đổi giữa điều khiển swingup và điều khiển cân bằng.

1/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

II. CƠ SỞ LÝ THUYẾT
2.1 Mô hình toán học mô tả hệ thốngcon lắc ngược quay

z m2 ,J2

θ2 ,T2
O y l2

θ1 ,T1 m1,J1
x l1
DC MOTOR

Hình 1. Mô hình con lắc ược quay và các thông số

Moment của thanh quay đối với
2.48*10-2
trục quay O (J1)
Moment của thanh lắc đối với
3.86*10-3
trục quay đi qua trọng tâm (J2)
Chiều dài của thanh lắc (l1) 0.15
Khoảng các từ trục quay của thanh lắc
0.148
đến trọng tâm của thanh lắc (l2)
Khối lượng của thanh lắc (m2) 0.075
Hệ số ma sát trên trục quay O (c1) 10-4
Hệ số ma sát trên trục quay
2.8*10-4
của thanh lắc (c2)
Hằng số moment (động cơ có hộp số 1:5) (Kt) 0.09*5
Hằng số điện từ (Ke) 0.09
Điện trở phần ứng (Ra) 7.8
Gia tốc trọng trường (g) 9.8

Bảng 1. Các thông số của mô hình con lắc ược quay

Góc lệch của thanh quay θ1

Góc lệch của thanh lắc θ2
Moment của thanh quay T1
Moment tác động vào thanh lắc T2
Điện áp cấp cho động cơ DC u

Bảng 2. Các tín hiệu qua sát và u khi n của mô hình con lắc ược quay

2/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Tọa độ của chất điểm m2 trong hệ tọa độ Oxyz gắn với trục quay của động cơ.

 x2  l1 cos1  l2 sin 1 sin  2

 (1)
 y2  l1 sin 1  l2 cos 1 sin  2
 z  l cos 
 2 2 2

Vận tốc của chất điểm m2 trong hệ tọa độ Oxyz

 x2  l11 sin 1  l21 cos1 sin  2  l2 2 sin 1 cos 2


 y2  l11 cos 1  l21 sin 1 sin  2  l2 2 cos1 cos 2 (2)

 z2  l2 2 sin  2

 v2 2  x2 2  y2 2  z2 2  l1212  l2 22 2  l2 21 sin 2 2  2l1l212 cos2 (3)

Động năng và thế năng của thanh quay

1
K1  J112
2
(4)
P1  0

Động năng và thế năng của thanh lắc

1 1
K2  J 2 2 2  m2v2 2
2 2
 m2l1212   J 2  m2l2 2  2 2  m2l2 21 sin 2  2  m2l1l21 2 cos 2
1 1 1 (5)
2 2 2

P2  m2 gz2  m2 gl2 cos2

Toán tử Lagrange L = ∑K - ∑P

J112  m2l1212   J 2  m2l2 2  2 2  m2l2 21 sin 2  2

1 1 1 1
L
2 2 2 2
(6)
 m2l1l21 2 cos 2  m2 gl2 cos 2

Phương trình Euler – Lagrange cho chuyển động của con lắc

d  L  L
   Ti
dt  i  i (7)

Trong đó, θi và Ti lần lượt là góc quay và tổng moment tác động lên con lắc đối với
trục quay tương ứng.

3/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Xét phương trình Euler-Lagrange với θ1 và θ2 ta có phương trình toán học mô tả hệ

thống (Tm là moment tạo bởi động cơ DC, T1 được tính như công thức (30), T2 chỉ có
moment ma sát cản do xét trường hợp không có ngoại lực tác động)


 J1  m2l1 m2l2 sin  2  m2l1l2 2 cos 2 m2l2 1 2 sin(2 2 )
2 2 2 2


m2l1l2 2 sin  2  Tm  c11
2


m2l1l21 cos 2   J 2 m2l2 2  2  m2l2 2 sin(2 2 )12  m2 gl2 sin  2  0  c2 2
1 (8)
 2

Xét động cơ DC, ta có các mối quan hệ

Ra La

Tm θ
u
Armature
current
e .
J
ia
Rotor + load

Hình 2. Sơ đồ tương đương động cơ DC

E  Ke1 Ke là hệ số điện áp cảm ứng (Back-EMF)

Tm  Kt ia Kt là hằng số moment của động cơ DC

Bỏ qua sự ảnh hưởng của điện cảm cuộn dây rotor, ta có mối quan hệ

u  E Kt KK
Tm  Kt  u  t e 1
Ra Ra Ra (9)

Thay (9) vào (8) ta được mô hình toán mô tả hệ thống con lắc ngược quay với đầu vào
là điện áp cấp cho động cơ u. Trong hệ đơn vị SI thì Kt = Ke.

2.2 Tuyến tính hóa tại điểm cân bằng

Hệ thống phi tuyến mô tả bởi phương trình trạng thái

 x  f ( x(t ), u (t ))

 y  h( x(t ), u (t )) (10)

Tuyến tính hóa hệ thống tại điểm cân bằng ta được phương trình trạng thái tuyến tính
như sau

4/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

 x  Ax  Bu

 y  Cx (11)

Với các ma trận

 f1 
 f1   u 
f1  x  x0 ,u u0 
 
 x1 x  x0 ,u u0 xn x  x0 ,u u0 
 f 
 2 
A   ... ... 

B   u x  x0 ,u u0 

 f   
f n  
 n 
 x1 x  x0 ,u u0 xn x  x ,u u   f n 
0 
0
  (12)
 u 
x  x0 ,u u0 

 h h h 
C 
 x1 x  x0 ,u u0
x2 x  x0 ,u u0
xn x  x0 ,u u0 


2.3 Điều khiển hồi tiếp tuyến tính hóa

Điều khiển hồi tiếp tuyến tính hóa trước hết chuyển hệ thống phi tuyến về dạng một
hệ thống tuyến tính, và sau đó sử dụng các kỹ thuật điều khiển tuyến tính đã biết để
hoàn chỉnh bộ điều khiển của hệ thống. Phương pháp này vẫn mô tả chính xác sự thay
đổi trạng thái của hệ thống, không giống như phương pháp xấp xỉ tuyến tính ở phần
2.2

Hệ thống phi tuyến biểu diễn bởi phương trình trạng thái sau

 x  f ( x)  g ( x).u

 y  h( x ) (13)

Trong đó u, y là tín hiệu vào và tín hiệu ra của hệ thống và vector trạng thái là

x   x1 xn 
T
x2

Nếu đối tượng có bậc tương đối bằng n thì có thể biểu diễn mối quan hệ vào ra của đối
tượng như sau

y ( n)  a( x)  b( x).u (14)

Trong đó:

5/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

a( x)  Lnf h( x)
 n 1
b(x)  Lg L f h( x)  0 (15)

v  a( x)
Chọn đầu vào u 
b( x ) (16)

Hệ thống trở thành y (n)  v

(17)

Chọn luật điều khiển bám

v  yd (n)  k1e(n 1)  k2e(n 2)   kn e (18)

Đa thức đặc trưng sai số Δs = s(n) + k1s(n-1) + … + kn, chọn các giá trị k sao cho đa thức
này là đa thức Hurwitz, tức là tất cả các nghiệm Δs nằm bên trái mặt phẳng phức và vị
trí các cực thỏa mãn yêu cầu quá độ.

2.4 Điều khiển trượt

Hệ thống phi tuyến biểu diễn bởi phương trình trạng thái sau

 x  f ( x)  g ( x).u

 y  h( x ) (19)

Trong đó u, y là tín hiệu vào và tín hiệu ra của hệ thống và vector trạng thái là

x   x1 xn 
T
x2

Nếu đối tượng có bậc tương đối bằng n thì có thể biểu diễn mối quan hệ vào ra của đối
tượng như sau

y ( n)  a( x)  b( x).u (20)

Trong đó:

a( x)  Lnf h( x)
 n 1
b(x)  Lg L f h( x)  0 (21)

Định nghĩa mặt trượt:

  e( n1)  k1e( n2)   kn2e  kn1e (22)

6/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Δs = s(n) + k1s(n-1) + … + kn là đa thức đặc trưng của mặt trượt, và ki được chọn sao cho
Δs là đa thức Hurwitz (tất cả các nghiệm nằm bên trái mặt phẳng phức) và thỏa mãn
yêu cầu về đáp ứng quá độ.

Tín hiệu u(t) được chọn như sau thì sai số e tiến về 0, hay tín hiệu đầu ra y bám theo
tín hiệu đặt yd.

1
u  a( x)  yd( n )  k1e( n1)   kn2e  kn1e  Ksign( ) 
b( x ) (23)

Để giảm hiện tượng chattering, có thể thay thế hàm sign(x) bằng hàm sat(x).

2.5 Điều khiển swing-up dựa trên phương pháp năng lượng
Định luật 2 Newton cho chuyển động quay của thanh lắc (bỏ qua ma sát)

J 2  m2 gl2 cos2  T1l1l2 cos2 (24)

Trong đó, T1 là moment do thanh quay tác động vào thanh lắc.

Năng lượng của thanh lắc (chọn gốc thế năng tại điểm cân bằng trên)

1
E J 2 2 2  m2 gl2  cos 2  1
2 (25)

Thay vì điều khiển góc lệch của thanh lắc hay vận tốc góc của thanh lắc, phương pháp
năng lượng dựa vào việc điều khiển năng lượng của thanh lắc đạt đến giá trị E0 = 0
(năng lượng tại vị trí cân bằng trên). Xét đạo hàm của năng lượng.

dE
 J 2 2 2  m2 gl2 sin  2 2  T1l1l2 2 cos 2
dt (26)

Như vậy, để năng lượng của thanh lắc tăng, ta cần cung cấp moment cho thanh quay
T1 ngược hướng với dấu của  2 cos 2 (để đạo hàm dương). Tiếp tục ta xét hàm
Lyapunov

1
 E  E0 
2
V
2 (27)

dV
  E  E0  E    E  E0  T1l1l2 2 cos 2
dt (28)

Nếu ta chọn


T1   K sw  E  E0  sign 2 cos2  (K sw > 0)
(29)

7/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

dV  3
  K sw l1l2  E  E0   2 cos 2  0 (    2  , 2 
2
thì )
dt 2 2
Kết luận với tín hiệu điều khiển T1 đã chọn thì năng lượng của hệ thống ổn định và
đạo hàm năng lượng dương, tức là E tăng đến giá trị E0 và ổn định tại E0. Giá trị Ksw
đặc trưng cho tốc độ swing-up, Ksw càng lớn thì tốc độ swing-up càng nhanh.

Mối quan hệ giữa moment thanh lắc và đầu vào điện áp u

T1  Tm  c11

 Kt Kt Ke
Tm  R u  R 1
 a a

K KK 
 T1  t u   t e  c1 1
(30)
Ra  Ra 

Vậy luật điều khiển swing-up cho con lắc ngược quay
KK 
 
 K sw  E  E0  sign  2 cos  2   t e  c1 1
u  Ra 
Kt
Ra
(31)

2.6 Giới thiệu thư viện Symbolic Math Toolbox của MATLAB
Các biến thông thường được sử dụng trong MATLAB là các biến kiểu dữ liệu số
(double precision numeric variables), thư viện Symbolic Math Toolbox cho phép khai
báo các biến chữ (symbolic variables) và thực hiện các phép toán: đạo hàm, tích phân,
giải phương trình, …

 Khai báo các biến kiểu symbolic

syms x y z; % x, y, z are symbolic variables

 Kiểm tra kiểu các biến trong workspace (kiểu double hay symbolic)
whos;

 Khai báo một hàm : f(x,y) = 2x + 3y + cos(xy)

f = 2*x + 3*y + cos(x*y); % f is now a function of x and y

 Đạo hàm df/dx

df_dx=diff(f, x); %the result is the derivative of f with respect to the
variable x

 Đạo hàm hàm hợp theo thời gian

8/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

d_x = fulldiff(x); % declare that d_x is the derivative of x with

respect to the variable t

Lưu ý: ùy t eo phiên bản MATLAB mà hàm fulldiff có th tồn tại hoặc không, sinh
viên có th dow load và ặt ở workspace khi chạy:
https://www.mathworks.com/matlabcentral/fileexchange/7174-fulldiff-m

df df dx df dy
 Đạo hàm hàm hợp nhiều biến theo thời gian dt  dx dt  dy dt

df_dt = fulldiff(f, {x y}); % df_dt is the derivative of f with respect

to the variable t

 Tối giản hàm số

df = simplify(df_dt); % simplify function

Trong một số trường hợp MATLAB không tự động tối giản hàm số được, trường hợp
này sinh viên kiểm tra lại và thực hiện bằng tay.

• Thay thế giá trị cho một biến trong hàm

subs(df_dt, x, 0); % replace var x by its value 0 in function dx_dt

• Thay thế giá trị cho một vector (nhiều biến) trong hàm
subs(dx_dt, [x, y], [0, 0]); % replace vector [x, y] by their values [0,
0] in function dx_dt

• Chuyển đổi giá trị symbolic sang giá trị số (double) sau khi đã thay tất cả các giá trị
cho các biến symbolic.
double(df); % convert symbolic value to numeric value of object A.

III. MÔ TẢ THÍ NGHIỆM

Phần mềm sử dụng trong các bài thí nghiệm này là bộ phần mềm Matlab Simulink.
Sinh viên tạo mô hình mô phỏng Simulink như hình vẽ của từng thí nghiệm.

Sinh viên thực hiện cài đặt các thông số mô phỏng như sau: vào menu Simulation -
Configuration Parameters, chọn mục Solver để cài đặt các thông số về thời gian mô
phỏng (Simulation time) và phương pháp mô phỏng (Solver Options) như ở ình 4.
Thời gian mô phỏng (khoanh tròn) thay đổi tùy theo thí nghiệm.

9/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Hình 3. Mô hình mô phỏng Simulink

Hình 4. Cài đặt các thông số mô phỏng

IV. CHUẨN BỊ TRƯỚC KHI THÍ NGHIỆM

4.1 Xây dựng mô hình toán mô tả hệ thống con lắc ngược quay
Để hiểu rõ về phương pháp Euler-Lagrange, sinh viên thực hiện lại đầy đủ các bước
để thành lập phương trình toán mô tả hệ thống con lắc ngược quay như công thức
(32). Đầu vào hệ thống là u (điện áp cấp cho động cơ DC). Các biến trạng thái: góc
quay của thanh quay θ1, góc lệch của thanh lắc θ2, vận tốc góc tương ứng của thanh
quay và thanh lắc ̇ ̇

10/23
 u – K oa ệ - ệ tử - H ác K oa PHCM


1  f1 (1 , 2 ,1 , 2 , u)

 2  f 2 (1 , 2 ,1 , 2 , u)
 (32)

- Sinh viên chạy thử để hiểu ý nghĩa các câu lệnh trong thư viện Symbolic Math
Toolbox đã giới thiệu ở phần 2.6.

- Tạo m-file “pend_dynamics1.m” với nội dung như sau. Lưu ý: ếu lệnh fulldiff
không có sẵn thì cầ ặt t ư v ệ tro t ư ục làm việc.
%% 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;

11/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

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)

- Kiểm tra lại kết quả thu được từ MATLAB và công thức (8). Kết quả chính là
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.

- Thay thế các moment động cơ (Tm) bởi công thức (9) và đặt các hằng số để thu gọn
mô hình, ta tiếp tục giải hệ phương trình (8) với 2 ẩn d2θ1/dt2 và d2θ2/dt2.

a  J1  m2l12 , b  m2l1l2 , c  m2l2 2

Kt K e K
d  c1 , e  t
Ra Ra
f  J 2  m2l2 2 , h  m2 gl2

- Tạo một m-file “pend_dynamics2.m” để giải phương trình với nội dung như 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)

12/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

- Vector X vừa tìm được chính là phương trình toán mô tả hệ thống (công thức (32))
với các biến trạng thái đã cho, tín hiệu điều khiển là điện áp cấp cho động cơ DC u.

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

- Sinh viên tạo m-file “pend_model_init.m” để khởi tạo thông số mô hình với số liệu
được cho như bảng 1.
%% 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;

- Xây dựng mô hình mô phỏng Simulink “pend_model.mdl” như hình 5 và 6. Thay

thế các biến của hệ thống trong phương trình tìm được ở phần 4.1 bằng các tín hiệu
tương ứng: u(1) th1, u(2) dth1, u(3)th2, u(4) dth2, u(5) vol và nhập vào
function block của MATLAB trong hình 6.

- Khối Voltage Limit giới hạn tín hiệu áp của động cơ DC [-25, 25].

Hình 5. Mô hình khảo sát hệ con lắc ngược quay

13/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Hình 6. Khối mô phỏng con lắc ngược quay

Khảo sát đáp ứng đầu ra của các trạng thái trong các trường hợp góc lệch ban đầu
được cho như bên dưới và kiểm chứng tính đúng đắn của mô hình dựa vào suy luận
vật lý cơ bản.

Giá trị khởi tạo Giá trị khởi tạo

Lần chạy
của θ1 của θ2
1 0 0
2 900 0
3 1800 0
4 0 100
5 900 450
6 1800 900

4.3 Tuyến tính hóa tại điểm cân bằng trên

Sinh viên tiếp tục sử dụng thư viện Symbolic Toolbox để thành lập phương trình trạng
thái tuyến tính của hệ thống con lắc ngược cân bằng tại điểm cân bằng trên, tìm ra ma
trận A, B.

- Sinh viê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]

14/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

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);

- Kết quả thu được là ma trận A của công thức (11). Sinh viên thực hiện tương tự để
tìm được ma trận B.

4.4 Điều khiển hồi tiếp tuyến tính hóa và điều khiển trượt.
Dựa vào phương trình toán mô tả hệ thống tìm được ở phần chuẩn bị 4.1, sinh viên
tìm các biểu thức a(x) và b(x) trong công thức (14) hoặc (20) của bộ điều khiển hồi
tiếp tuyến tính hóa và điều khiển trượt. Xét trường hợp tín hiệu đầu ra cần điều khiển
là góc lệch θ2

V. THỰC HIỆN THÍ NGHIỆM

5.1 Tuyến tính hóa tại điểm cân bằng trên và thiết kế bộ điều khiển LQR
- Sử dụng mô hình tuyến tính hóa tại điểm cân bằng trên đã chuẩn bị ở nhà trong phần
4.3 (các ma trận As, Bs đã tìm được), sinh viên khảo sát bộ điều khiển LQR như sau

- Xây dựng mô hình mô phỏng Simulink “pend_lqr.mdl” như hình 7. Lưu ý c ọn chế
Multiplication là Matrix(K*u) cho khối Gain.

15/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

- Viết m-file “pend_lqr_init.m” sử dụng ma trận As, Bs và lệnh lqr để tìm vector hồi
tiếp trạng thái K.

Hình 7. Điều khiển cân bằng con lắc ngược bằng LQR

a. Khảo sát ảnh hưởng của ma trận Q và ma trận R

Giá trị khởi tạo của các trạng thái (θ1 = dθ1/dt=dθ2/dt = 0, θ2 = 100), sinh viên thay đổi
giá trị các ma trận Q và R tương ứng với các trường hợp và tính lại vector K, vẽ đáp
ứng ngõ ra.

Lần chạy Giá trị của ma trận Q Giá trị của ma trận R
1 Q = diag([1, 1, 0.001, 1]) R=1
2 Q = diag([1, 1, 1, 1]) R=1
3 Q = diag([1, 1, 10^4, 1]) R=1
4 Q = diag([1, 1, 1, 1]) R = 0.01
5 Q = diag([1, 1, 1, 1]) R=1
6 Q = diag([1, 1, 1, 1]) R = 100

- Nhận xét về ảnh hưởng của việc thay đổi ma trận Q đến đáp ứng của góc lệch θ2?
- Nhận xét về ảnh hưởng của việc thay đổi ma trận R đến đáp ứng của góc lệch θ2 ?

b. Khảo sát ảnh hưởng của giá trị khởi tạo góc θ2
- Đặt giá trị cho ma trận Q = diag([1, 1, 1, 1]) và ma trận R = 1 và tìm lại vector hồi
tiếp trạng thái K.

- Giá trị khởi tạo của các trạng thái (θ1 = dθ1/dt=dθ2/dt = 0), thay đổi giá trị khởi tạo
góc θ2 tương ứng như các trường hợp sau và vẽ đáp ứng ngõ ra.

16/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Lần chạy Giá trị khởi tạo góc θ2

1 θ2 = 100
2 θ2 = 150
3 θ2 = 200
4 θ2 = 250

Nhận xét sự ảnh hưởng của giá trị khởi tạo θ2 đến đáp ứng của hệ thống ?

5.2. Bộ ước lượng trạng thái

- Xây dựng mô hình Simulink bộ ước lượng trạng thái “pend_state_estimator.mdl”
như hình 8 và hình 9. Các giá trị nhiễu được cộng vào các tín hiệu đó tương ứng như
sau: nhiễu đối với tín hiệu đo θ1 là Noise1 với variance là N1, tương tự cho θ2 là
Noise2 với variance là N2. Tín hiệu hồi tiếp là góc lệch của thanh quay và thanh lắc
(θ1 và θ2).

- Thực hiện tính lại vector hồi tiếp trạng thái K với ma trận Q = diag ([1 1 1 1]) và ma
trận R = 1.

- Giá trị khởi tạo của các trạng thái (θ1 = dθ1/dt=dθ2/dt = 0, θ2 = 100)

- Tính độ lợi Kalman cho bộ ước lượng thái bằng đoạn code sau.
%% Kalman gain
G = eye(4);
Cs = [1 0 0 0;0 0 1 0];
Cm = [1 0;0 1];
QN = 10^-6*eye(4); %variance of process noise
RN = [10^-3 0;
0 10^-3]; %variance of measurement noise
Lo = lqe(As,G,Cs,QN,RN)

Hình 8. Ước lượng trạng thái và điều khiển LQR cân bằng con lắc ngược quay

17/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Hình 9. Cấu trúc bộ ước lượng trạng thái

Sinh viên đánh giá sai số ước lượng đối với tín hiệu θ1 và θ2 trong trường hợp sau

Giá trị phương sai của nhiễu Giá trị ma trận QN và RN

QN =10-6*eye(4)
N1 = 0.001, N2 = 0.001
RN =10-3*eye(2)

5.3 Điều khiển swing-up con lắc bằng phương pháp năng lượng
Sinh viên thực hiện mô hình mô phỏng Simulink “pend_swingup.mdl” như hình 10,
các khối chức năng trong hệ thống hoạt động như sau

Hình 10. Sơ đồ khối điều khiển swing-up và điều khiển cân bằng dùng LQR
Khối swing-up thực hiện luật điều khiển swing-up để đưa con lắc đến vị trí cân bằng
trên, tốc độ swing-up được đặc trưng bởi giá trị Ksw và thay đổi được. Sinh viên thực
hiện luật điều khiển swing-up như công thức (31) vào khối swing-up như hình 11.

18/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Hình 11. Khối điều khiển swing-up con lắc

Bộ điều khiển LQR để giữ cân bằng cho con lắc ở vùng làm việc tuyến tính đã thực
hiện ở thí nghiệm 5.1

Khối switchC (khối Embedded Matlab Function trong Simulink) thực hiện việc xem
xét điều kiện về góc quay và vận tốc góc để chuyển đổi giữa luật điều khiển swing-up
và luật điều khiển cân bằng LQR khi con lắc đi vào vùng tuyến tính hóa.

Lưu ý: ối signal specification chỉ cần thiết nếu MATLAB báo lỗi thiếu ịnh ng ĩa
ki u dữ liệu c o ầu vào cho khối switchC.
function y = switchC(ang, vel)

temp = mod(ang, 2*pi);

if(temp>pi)
temp=temp-2*pi;
end

if((abs(temp)<pi*20/180)&&(abs(vel)<2*pi))
y=-1;
else
y = 1;
end

Thay đổi tốc độ swing-up

- Chức năng chuyển đổi bộ điều khiển: bộ điều khiển LQR được sử dụng nếu (θ2 <
200, dθ2/dt < 2), bộ điều khiển swing-up được sử dụng nếu điều kiện trên không thỏa

- Thực hiện tính lại vector hồi tiếp trạng thái LQR với ma trận Q = diag ([0.01 0.01 1
1]) và ma trận R = 1.

- Giá trị khởi tạo của các trạng thái (θ1 = dθ1/dt=0, θ2 = 1800, dθ2/dt = 0.1).

- Thay đổi các giá trị Ksw và nhận xét về tốc độ swing-up

19/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Lần chạy Giá trị Ksw

1 10
2 15
3 50
4 100

5.4 Thiết kế bộ điều khiển hồi tiếp tuyến tính hóa

- Xây dựng bộ điều khiển hồi tiếp tuyến tính hóa để cân bằng con lắc ngược quay
“pend_fblinearization.mdl” như sơ đồ hình 12, hình 13 và hình 14.

- Sinh viên nhập các hàm a(x) và b(x) của khối hồi tiếp tuyến tính hóa đã chuẩn bị ở
nhà vào khối hồi tiếp tuyến tính hóa.

Lưu ý: ro t số phiên bản MATLAB có th báo lỗi nếu tín hiệu hồi tiếp không có
giá trị ở lần chạy ầu t ê , trường hợp này có th thêm khối Transport Delay với thời
gian delay bằng thời gian lấy mẫu ã cà ặt, ví dụ: 0.01s

- Chọn các cực của bộ điều khiển bám để đáp ứng thỏa mãn yêu cầu:

POT < 10% và tqđ < 0.3s

Hình 12. Điều khiển hồi tiếp tuyến tính hóa cân bằng con lắc ngược

20/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Hình 13. Cấu trúc khối hồi tiếp tuyến tính hóa

Hình 14. Cấu trúc khối điều khiển bám

5.5 Thiết kế bộ điều khiển trượt
- Xây dựng mô hình điều khiển trượt để cân bằng con lắc ngược quay
“pend_slidingmode.mdl” như sơ đồ hình 15 và hình 16.

- Chọn mặt trượt và vị trí nghiệm của đa thức đặc trưng và giá trị của hệ số K để đáp
ứng hệ thống thỏa mãn điều kiện POT < 10% và tqđ < 0.3s

21/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Hình 15. Điều khiển trượt cân bằng con lắc ngược

Hình 16. Cấu trúc bộ điều khiển trượt

Nhận xét khi về tín hiệu điều khiển khi thay thế hàm sign(x) bởi hàm sat(x) ?

22/23
 u – K oa ệ - ệ tử - H ác K oa PHCM

Báo cáo thí nghiệm

Họ và tên: Nhóm: Ngày:

1. Dựa vào đáp ứng ngõ ra của các trường hợp trong thí nghiệm 5.1, nêu ý nghĩa của
ma trận Q và ma trận R ? Ta có thể điều khiển θ1 đến góc 300 được không ?
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
2. Trong thí nghiệm 5.2, tại sao khi góc khởi tạo của θ2 lớn (θ2 = 250) thì hệ thống
không thể cân bằng ?
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
3. Trong thí nghiệm 5.3, giải thích vì sao khi tăng Ksw lên rất lớn thì tốc độ swing-up
không tăng được nữa ?
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
4. Trong thí nghiệm 5.4 và 5.5, quan sát giá trị ngõ ra θ1 và dθ1/dt và giải thích hiện
tượng ?
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................
...............................................................................................................................................

23/23