THIẾT KẾ VÀ MÔ PHỎNG BỘ ĐIỀU KHIỂN LQG CHO HỆ CON LẮC NGƯỢC

111Equation Chapter 1 Section 1BỘ GIÁO
DỤC & ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH
KHOA ĐIỆN – ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
-----------------⸙∆⸙-----------------

BÀI KIỂM TRA QUÁ TRÌNH 2
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG
ĐỀ TÀI: THIẾT KẾ VÀ MÔ PHỎNG BỘ ĐIỀU
KHIỂN LQG CHO HỆ CON LẮC NGƯỢC

GVHD:

TS.

TRẦN

ĐỨC

THIỆN
SVTH: PHAN MINH ĐIỀN
MSSV: 19151937


Tp. Hồ Chí Minh tháng 6 năm 2022


Lời cảm ơn

Mục lục
Lời cảm ơn.....................................................................................................................i
Mục lục.........................................................................................................................ii
Danh sách hình ảnh.....................................................................................................iii
Danh sách bảng............................................................................................................iv
Chương 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG........1
1.1

THƠNG SỐ VÀ MƠ TẢ TỐN HỌC CỦA HỆ THỐNG..........................1

1.1.1 Thơng số hệ con lắc ngược...............................................................................1
1.1.2 Mơ hình tốn học của hệ con lắc ngược...........................................................1
1.2

THÀNH LẬP HỆ PHƯƠNG TRÌNH CỦA HỆ THỐNG............................3

Chương 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR........................................................6
2.1

THIẾT KẾ BỘ LQR.....................................................................................6

2.1.1 Mơ hình hố hệ con lắc ngược bằng phương trình tốn học............................6
2.1.2 Cơ sở lý thuyết..................................................................................................7
2.1.3 Xây dựng và chọn thông số Q R theo các trường hợp và thiết kế bộ LQR......7
2.2

THIẾT KẾ BỘ LỌC KALMAN................................................................13

2.2.1 Cơ sở lý thuyết................................................................................................13
2.2.2 Xây dựng bộ lọc Kalman................................................................................14

2.3

THIẾT KẾ BỘ LQG..................................................................................15

2.3.1 Cơ sở lý thuyết................................................................................................15
2.3.2 Xây dựng bộ LQG..........................................................................................18
Chương 3. NHẬN XÉT...........................................................................................21
TÀI LIỆU THAM KHẢO..........................................................................................22


Danh sách hình ảnh
Hình 1.1: Mơ hình tốn học hệ con lắc ngược trên xe..................................................1
Hình 2.1:Mơ hình hố hệ con lắc ngược trên Simulink................................................6
Hình 2.2 Bộ điều khiển LQR cho hệ con lắc ngược trên phần mềm mơ phỏng
Simulink.................................................................................................................................9
Hình 2.3: Biểu đồ góc quay vận tốc góc hệ thống trong trường hợp 1.9
Hình 2.4: Đồ thị vị trí và vận tốc của hệ thống trong trường hợp 1...10
Hình 2.5: Đồ thị lực tác dụng vào hệ thống trong trường hợp 1...........10
Hình 2.6: Biểu đồ góc quay vận tốc góc hệ thống trong trường hợp 2
..............................................................................................................................................12
Hình 2.7: Đồ thị vị trí và vận tốc của hệ thống trong trường hợp 2....12
Hình 2.8: Đồ thị lực tác dụng vào hệ thống trong trường hợp 2...........13
Hình 2.9: Sơ đồ khối bộ lọc Kalman..........................................................................13
Hình 2.10: Xây dụng bộ lọc Kalman trên Simulink...................................................14
Hình 2.11: Biểu đồ góc quay hệ thống ngẫu nhiêu khi có bộ lọc..........14
Hình 2.12: Biểu đồ vị trí hệ thống ngẫu nhiêu khi có bộ lọc...................15
Hình 2.13: Bộ điều khiển LQG cho hệ con lắc ngược...............................................18
Hình 2.14: Thơng số của khối nhiễu góc quay...........................................................18
Hình 2.15: Thơng số của khối nhiễu vị trí..................................................................19
Hình 2.16: Biểu đồ góc quay và vận tốc gốc của hệ con lắc ngược sử

dụng bộ điều khiển LQG trường hợp có nhiễu....................................................19
Hình 2.17: Biểu đồ vị trí và vận tốc của hệ con lắc ngược sử dụng bộ
điều khiển LQG trường hợp có nhiễu.....................................................................20
Hình 2.18: Biểu đồ lực tác đôngj của hệ con lắc ngược sử dụng bộ
điều khiển LQG trường hợp có nhiễu.....................................................................20


Danh sách bảng
No table of figures entries found.


CHƯƠNG 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG

Chương 1.PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ
HỆ THỐNG
1.1 THƠNG SỐ VÀ MƠ TẢ TỐN HỌC CỦA HỆ THỐNG
1.1.1

Thơng số hệ con lắc ngược

Hình 1.1: Mơ hình tốn học hệ con lắc ngược trên xe
Thông số hệ con lắc ngược

M  1.0kg : Trọng lượng xe
m  0.1kg : Trọng lượng con lắc
l  1.0m : Chiều dài con lắc
u : Lực tác động vào xe  N 
2
g : Gia tốc trọng trường  m / s 

x : Vị trí xe  m 

 : Góc giữ con lắc và phương thẳng đứng  rad 
1.1.2

Mô hình tốn học của hệ con lắc ngược
Cách 1: Dùng định luật Newton:
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

1


CHƯƠNG 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG
Gọi (

xp , yp

) là toạ độ của vật nặng m ở đầu con lắc, ta có:
x p  x  l sin 
y p  x  l cos 

212\* MERGEFORMAT (.)

Áp dụng định luật II Newton cho cho chuyển động theo phương , ta có:
d 2 xp
d 2x
M 2 m 2 u
dt
dt
Thay

xp

313\* MERGEFORMAT (.)

ở biểu thức 12 vào biểu thức 13 suy ra:
M

d 2x
d 2 ( x  l sin  )

m
u
dt 2
dt 2

414\* MERGEFORMAT (.)

Khai triển các đạo hàm ở biểu thức 14 và rút gọn ta được:

 M  m  x&& ml  sin   &2  ml (cos  )&& u

515\*

MERGEFORMAT (.)
Mặt khác, áp dụng định luật II Newton cho chuyển động quay của con lắc quanh trục
ta được:
m

d 2 xp

dt 2

l cos   m

d 2 yp
dt 2

l sin   mgl sin 

616\*

MERGEFORMAT (.)
Thay 12 vào 16 suy ra:
 d 2  x p  l sin   
 d 2  l cos   
m

l
cos


m
 l sin   mgl sin 
dt 2
dt 2





717\*

MERGEFORMAT (.)
Khai triển các đạo hàm ở biểu thức 17 và rút gọn ta ta được:
& mg sin 
&cos   ml&
mx&

818\* MERGEFORMAT (.)

Từ 15 và 18, ta có thể tính được:
g

u  ml (sin  )  2  mg cos sin 
x
M  m  m(cos  ) 2
gg

g

u cos   ( M  m) g sin   ml (cos  sin  )  2

ml (cos  ) 2  ( M  m)l
gg

919\*

MERGEFORMAT (.)
Cách 1: Dùng công thức Euler – Lagrange:
Động năng của vật nặng đầu con lắc:

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

2


CHƯƠNG 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG
g
g
g
1
1
1 g g
1
m xP 2  m yP 2  m( x  l  cos  ) 2  m(l  sin  ) 2
2
2
2
2
2
g
g
g
g
1
1
 TP  m xP 2  ml x  cos   ml 2 
2
2

TP 

10110\*

MERGEFORMAT (.)
Động năng của xe:
g
1
Tc  M x 2
2

11111\* MERGEFORMAT (.)

Động năng của hệ thống:
T  TP  TC 

g
g
g g
1
1
( M  m) x 2  ml x  cos   ml 2  2
2
2

12112\*

MERGEFORMAT (.)
Thế năng của hệ thống chính là thế năng vật nặng đầu con lắc:
U  mgl cos 

13113\* MERGEFORMAT (.)

Toán tử Lagramgian:
L  T U 

g
g
g g
1
1
( M  m) x 2  ml x  cos   ml 2  2  mgl cos 
2
2

14114\*

MERGEFORMAT (.)
Phương trình Euler – Lagrange :
d  L  L
F


dt   xg 

x

d   L  L
0



dt   g 




15115\* MERGEFORMAT (.)

Thay 114 vào 115
gg

gg

g

( M  m) x  ml cos    ml (sin  )  2  u
gg

gg

m x cos   ml   mg sin   0

16116\*

MERGEFORMAT (.)
Suy ra:
g

u  ml (sin  )  2  mg cos  sin 
x

M  m  m(cos  ) 2
gg

g

u cos   ( M  m) g sin   ml (cos  sin  )  2

ml (cos  ) 2  ( M  m)l
gg

17117\*

MERGEFORMAT (.)
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

3


CHƯƠNG 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG
Như vậy cả 2 phương trình tốn học suy ra từ 2 cách khác nhau là giống nhau.

1.2 THÀNH LẬP HỆ PHƯƠNG TRÌNH CỦA HỆ THỐNG
Tiến hành chuyển phương trình tìm được ở 117 về dạng chuẩn:
 x1 
   f1 
 
  f 
d
d x
d &

x  f ( x, u, t )   2       2 
dt
dt  x3  dt  x   f 3 
 
   
 x&  f 4 
 x4 

18118\*

MERGEFORMAT (.)
Đặt các biến trạng thái
g

x1   , x2  

g

x3  x, x4  x

19119\* MERGEFORMAT (.)

Trong đó:
f1  x2
f2 

u cos x1  ( M  m) g sin x1  ml (cos x1 sin x1 ) x2 2
ml (cos x1 ) 2  ( M  m)l

f 3  x4

f4 

u  ml (sin x1 ) x2 2  mg cos x1 sin x1
M  m  m(cos x1 )2

20120\*

MERGEFORMAT (.)
Phương trình trạng thái phi tuyển
x2


 xg  
2
 1   u cos x1   M  m  g sin x1  ml  cos x1 sin x1  x2 
2
 g 

ml  cos x1    M  m  l
 x2   

 g 
x4

 x3  

u  ml  sin x1  x2 2  mg cos x1 sin x1
 g 

 x4  

2

M  m  m  cos x1 



21121\*

MERGEFORMAT (.)
Phương trình tuyến tính hóa quanh điểm cân bằng thẳng đứng (góc lệch nhỏ hơn 10 o,

x0  0, u0  0 )

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

4


CHƯƠNG 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG
 g
 x1 
 x1 
 g
 
 x2   J x , u  x2   J x , u u


x 0
0
 g

 x3  u 0 0
 x3 
 
 g
 x4 
 x4 

22122\*

MERGEFORMAT (.)
Với:

J x  x0 , u0 

 f1
 x
 1
 f 2
 x
1

 f 3

 x1
 f 4

 x1

f1
x2

f1
x3

f 2
x2

f 2
x3

f 3
x2

f 3
x3

f 4
x2

f 4
x3

f1 
x4  
0



f 2
  M  m g


x4  
Ml

f3  
0
 
x4    mg

f 4  
M

x4 

1 0 0

0 0 0

0 0 1


0 0 0

23123\*

MERGEFORMAT (.)
Và:

J u  x0 , u0 

 f1 
 u   0 

  
 f 2   1 
 u   M 

  
 f3   0 
 u   1 
 f   
 4 M 
 u 
24124\* MERGEFORMAT (.)

Như vậy, thay 124 và 123 vào 122 và các thơng số của bài tốn ta thành lập được
phương trình tuyến tính hố quanh điểm cân bằng thẳng đứng (góc lệch nhỏ hơn 10o,

x0  0, u0  0 )

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

5


CHƯƠNG 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG
0
 g 
 x1   M  m g


 g  
x

 2 
Ml
 g 
0
 x3  
 g    mg
 x4  
M
1
 0
10.78 0

 0
0

 0.98 0

1 0 0
0
  x1   
1
0 0 0  x   
  2 M 

u
0 0 1   x3   0 
   

 x
1
0 0 0  4   
M 

0 0   x1   0 
0 0   x2   1

u
0 1   x3   0 
   
0 0   x4   1 

25125\*

MERGEFORMAT (.)

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

6


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

Chương 2.THIẾT KẾ BỘ ĐIỀU KHIỂN LQR
2.1 THIẾT KẾ BỘ LQR
2.1.1

Mơ hình hố hệ con lắc ngược bằng phương trình tốn học
Dựa vào phương trình tốn học đã xây dựng ở phần trước ta tiến hành xây dựng mơ

hình hệ con lắc ngược như sau:
Code Matlab:
% Design by Phan Minh Dien
% Date: 07/06/2022
function [theta_2d,x_2d] =
Inverted_Pendulum(u,theta,theta_d,x,x_d)
%% Declaration
M = 1; m = 0.1; l = 1; g = 9.81;
%% Phuonng trinh he con lac nguoc
x_2d = (u+m*l*sin(theta)*(theta_d)^2m*g*cos(theta)*sin(theta))/(M+m-m*(cos(theta))^2);
theta_2d = ((u*cos(theta)-(M+m)*g*sin(theta)
+m*l*cos(theta)*sin(theta)*(theta_d)^2))/(m*l*(cos(theta))^2(M+m)*l);

Hình 2.2:Mơ hình hố hệ con lắc ngược trên Simulink

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

7


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

Cơ sở lý thuyết
Đặc tính động của hệ con lắc ngược có thể được mơ tả bởi hệ phương trình biến
trạng thái tuyến tính. Điều này chỉ đúng khi góc lệch nhỏ.
Hệ thống phản hồi trạng thái đầy đủ, nghĩa là có thể đo được 4 biến trạng thái
(góc lệch, vận tốc góc, vị trí xe, vận tốc xe). Và khơng có nhiễu tác động vào hệ
thống.

2.1.3

Xây dựng và chọn thông số Q R theo các trường hợp và thiết kế bộ LQR
Tính độ lợi hồi tiếp trạng thái K của bộ điều khiển LQR:
Trường hợp 1:
Ta sẽ tiến hành chọn các thông số Q R như sau
1
0
Q
0

0

0 0 0
1 0 0 
0 1 0

0 0 1

R 1

26126\* MERGEFORMAT (.)

Tín hiệu điều khiển tối ưu LQR
u& t    Kxˆ  t 

27127\* MERGEFORMAT (.)

K  R 1 BT P

28128\* MERGEFORMAT (.)

Với độ lợi hồi tiếp trạng thái

Trong đó P là nghiệm bán xác định dương của phương trình đại số Ricatta

PA  AT P  Q  PBR 1BT P  0

29129\* MERGEFORMAT

(.)
Code matlab
By Phan Minh Dien
%%Tim thong so P va K
clc;
%% Khai bao cac thong so thong so mo hinh
M = 1;
m = 0.1;
l = 1;
g = 9.81;
q11 = 1;
q33 =1;
r1 = 1;
%% cai dat gia tri khoi tao
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

8

CHƯƠNG 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR
theta_d = 0 ;
theta = 0.1;
x_d = 0;
x = 0;
%% Ma Tran so lieu tuyen tinh hoa
A = [ 0 1 0 0; 10.78 0 0 0; 0 0 0 1; -0.98 0 0 0];
B = [0; -1 ; 0; 1];
C = [1 0 0 0; 0 0 1 0];
%% Ma tran trong so
Q = [q11 0 0 0; 0 1 0 0; 0 0 q33 0; 0 0 0 1];
R = r1;
%% Tinh toan ma tran LQR
P = care(A,B,Q,R);
K = lqr(A,B,Q,R);
Tiến hành biên dịch chương trình trên ta tìm được nghiệm của Ricatti liên tục:
182.5688 56.7551 10.7009   22.3931
 56.7551  17.8215 3.4063
7.1205 

P
 10.7009 3.4063 2.4109
2.4063 


2.4063 4.7096 
 22.3931 7.1205
K   34.3620  10.7009  1.0000  2.4109

30130\*

MERGEFORMAT (.)
Xây dựng trên Simulink

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

9


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

Hình 2.3 Bộ điều khiển LQR cho hệ con lắc ngược trên phần mềm mơ phỏng Simulink

Hình 2.4: Biểu đồ góc quay vận tốc góc hệ thống trong trường hợp 1

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

10


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

Hình 2.5: Đồ thị vị trí và vận tốc của hệ thống trong trường hợp 1

Hình 2.6: Đồ thị lực tác dụng vào hệ thống trong trường hợp 1
Trường hợp 2:
Ta sẽ tiến hành chọn các thông số Q R như sau

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

11


CHƯƠNG 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR
1
0
Q
0

0

0
1 0 0 
0 100 0 

0 0 1

0

0

R 1

31131\* MERGEFORMAT (.)

Code matlab
By Phan Minh Dien
%%Tim thong so P va K
clc;
%% Khai bao cac thong so thong so mo hinh

M = 1; m = 0.1; l = 1; g = 9.81;
q11 = 1;
q33 =100;
r1 = 1;
%% cai dat gia tri khoi tao
theta_d = 0 ; x_d = 0; x = 0;
theta = 0.1;
%% Tuyen tinh hoa
A = [ 0 1 0 0; 10.78 0 0 0; 0 0 0 1; -0.98 0 0 0];
B = [0; -1 ; 0; 1];
C = [1 0 0 0; 0 0 1 0];
%% Ma tran trong so
Q = [q11 0 0 0; 0 1 0 0; 0 0 q33 0; 0 0 0 1];
R = r1;
%% Tinh toan ma tran LQR
P = care(A,B,Q,R);
K = lqr(A,B,Q,R);
Tiến hành biên dịch chương trình trên ta tìm được nghiệm của Ricatti liên tục:
 769.1196
 243.9052  
P
 221.0906  

173.7696  
K   70.1356 

243.9052
77.5354 
70.5670 
55.4263  

22.1091 

221.0906  173.7696 
70.5670   55.4263 
110.5142   60.5670 

60.5670   44.3749 
10.0000  11.0514

32132\*

MERGEFORMAT (.)

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

12


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

Hình 2.7: Biểu đồ góc quay vận tốc góc hệ thống trong trường hợp 2

Hình 2.8: Đồ thị vị trí và vận tốc của hệ thống trong trường hợp 2

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

13


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

Hình 2.9: Đồ thị lực tác dụng vào hệ thống trong trường hợp 2

2.2 THIẾT KẾ BỘ LỌC KALMAN
2.2.1

Cơ sở lý thuyết
Bộ lọc Kalman thực hiện phương pháp truy hồi đối với chuỗi các giá trị đầu vào bị

nhiễu, nhằm tối ưu hóa giá trị ước đoán trạng thái của hệ thống. Bộ lọc Kalman được ứng
dụng rộng rãi trong kỹ thuật, phổ biến trong các ứng dụng định hướng, định vị và điều
khiển các phương tiện di chuyển.

Hình 2.10: Sơ đồ khối bộ lọc Kalman
Trong đó các giá trinh được thể hiện như sau

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

14


CHƯƠNG 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR
 xˆ& Axˆ  t   Bu  t   L  y  t   yˆ  t  

 yˆ  t   Cxˆ  t 

33133\*

MERGEFORMAT (.)
Với

L   C T RN1

A   AT   C T RN1C   QN  0

34134\*

MERGEFORMAT (.)
2.2.2

Xây dựng bộ lọc Kalman

Hình 2.11: Xây dụng bộ lọc Kalman trên Simulink
Kết quả thu được khi sử dụng bộ lọc Kalman

Hình 2.12: Biểu đồ góc quay hệ thống ngẫu nhiêu khi có bộ lọc

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

15


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

Hình 2.13: Biểu đồ vị trí hệ thống ngẫu nhiêu khi có bộ lọc

2.3 THIẾT KẾ BỘ LQG
2.3.1

Cơ sở lý thuyết
Bộ điều khiển LQG là sự kết hợp của một bộ lọc Kalman, tức là hàm ước lượng

tuyến tính bậc hai (LQE), với một bộ điều chỉnh tuyến tính bậc hai (LQR). Nguyên tắc tách
biệt đảm bảo rằng chúng có thể được thiết kế và tính tốn độc lập. Điều khiển LQG áp
dụng cho cả các hệ thống tuyến tính thời gian bất biến và các hệ thống tuyến tính thời gian
biến đổi. Việc áp dụng vào các hệ thống tuyến tính biến đổi theo thời gian là nổi tiếng. Việc
áp dụng đối với các hệ thống tuyến tính thời gian biến đổi cho phép thiết kế các bộ điều
khiển phản hồi tuyến tính cho các hệ thống phi tuyến khơng ổn định.
Bài tốn LQG đặt ra là tìm tín hiệu điều khiển
đầu

x(t0 )  x0

bất kỳ về trạng thái cuối

x (t f )  0

u( t )

điều chỉnh hệ thống từ trạng thái

sao cho chỉ tiêu hàm chất lượng là tối

thiểu.
Cách giải bài toán điều khiển LQG
Nguyên lý tách rời: Bài toán tối ưu LQG có thể giải bằng cách giải riêng bài toán
điều khiển tối ưu tiền định và bài toán ước lượng trạng thái tối ưu:
LQG  LQR  Kalman

35135\* MERGEFORMAT (.)

Giả thiết bài toán thiết kế bộ điều khiển LQG cho hệ con lắc ngược:
-

Hệ thống hoạt động trong miền tuyến tính.
Giả sử chỉ đo được góc lệch con lắc và vị trí xe.
Có nhiễu tác động vào hệ thống. Nhiễu đo vị trí xe có phương sai là 0.01, nhiễu
đo góc lệch con lắc có phương sai 0.001.

Dùngng bộ lọc Kalman để ước lượng trạng thái và lọc nhiễu.
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

16


CHƯƠNG 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR
Ta có Bộ lọc Kalman:
 xˆ& Axˆ  t   Bu  t   L  y  t   yˆ  t  
a
 yˆ  t   Cxˆ  t 
1 0 0 0 
C

0 0 1 0

36136\*

MERGEFORMAT (.)
Độ lợi ước lượng
L   C T RNT

37137\* MERGEFORMAT (.)

Phương trình Ricatti
A   AT   C T RN1C   QN  0

38138\*

MERGEFORMAT (.)
Dựa vào điều kiện đưa ra ở phần giả thuyết, ta chọn được các thông số a
1
0
QN  0.000001 
0

0

0
1
0
0

0
0
1
0

 0.01 0 
RN  
0.01
 0

0
0 
0

1
39139\* MERGEFORMAT (.)

Code Matlab:
%% By Phan Minh Dien
clc;
%% Khai bao bo thong so mo hinh
M = 1;
m = 0.1;
l = 1;
g = 9.81;
q11 = 1;
q33 =100;
r1 = 1;
%% Cai dat gia tri khoi tao
thetadot_init = 0;
theta_init = pi/18;
xdot_init = 0;
x_init = 0;
%% Tuyen so lieu tinh hoa
A = [ 0 1 0 0; 10.78 0 0 0; 0 0 0 1; -0.98 0 0 0];
B = [0; -1 ; 0; 1];
C = [1 0 0 0; 0 0 1 0];
%% Ma tran trong so
Q = [ q11 0 0 0; 0 1 0 0; 0 0 q33 0; 0 0 0 1];

R = r1;
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

17


CHƯƠNG 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR
%% Ma tran LQR
P = care(A,B,Q,R)
K = lqr(A,B,Q,R)
%% Bo loc Kalman
G=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
Qn=0.000001*G;
Rn=[0.001 0;0 0.01];
%% Ma tran LQG
L = lqe(A,G,C,Qn,Rn)
Tiến hành biên dịch ta thu được các thơng số của phương trình Ricatti như sau:
 0.0066 0.0215 0.0006 0.0020 
 0.0215 0.0707 0.0019 0.0064 

 0.0006 0.0019 0.0015 0.0013 


 0.0020 0.0064 0.0003 0.0006 

40140\*

MERGEFORMAT (.)
 6.5617 0.0517 
 21.5437 0.1876 


L
 0.5713 0.1470 


 1.9568 0.0271 
 769.1196
 243.9052  
P
 221.0906  

173.7696  

41141\* MERGEFORMAT (.)

173.7696 
55.4263 
60.5670 

44.3749 
K   70.1356  22.1091  10.0000  11.0514
243.9052 221.0906 
77.5354  70.5670  
70.5670  110.5142  
55.4263   60.5670  

42142\*

MERGEFORMAT (.)

NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

18


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

Xây dựng bộ LQG

Hình 2.14: Bộ điều khiển LQG cho hệ con lắc ngược
Thơng số nhiễu 

Hình 2.15: Thơng số của khối nhiễu góc quay
Thơng số nhiễu x
NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG

19