BÁO C ÁO MÔ N H Ọ C :

Nhận dạng và điều khiển hệ

thống
Đề tài: THIẾT KẾ BỘ ĐIỀU KHIỂN LQG CHO HỆ CON LẮC NGƯỢC
Giáo viên hướng dẫn : TS. Trần Đức Thiện Sinh viên thực hiện:
• Huỳnh Đặng Việt Hà - 20151464
• Phùng Minh Đức - 20151462
• Nguyễn Văn Trường An - 20151597
I. MỞ ĐẦU
MỤC TIÊU:
Xây dựng hệ xe con lắc ngược, sử
dụng bộ điều khiển phi tuyến hồi
tiếp tuyến tính hóa và hệ điều khiển
tối ưu LQG.
PHƯƠNG PHÁP NGHIÊN
CỨU:
+ Sử dụng các kiến thức đã học.
+ Dùng phần mềm Matlab để mô
phỏng, viết chương trình.
+ Tham khảo các tài liệu liên quan
đến đề tài.
II. NỘI DUNG:
• Giới thiệu
• Hình vẽ cấu trúc các bộ điều khiển
• Phân tích và trình bày các phương trình vi phân
• Bộ điều khiển phi tuyến ( ĐK hồi tiếp tuyến tính)
• Điều khiển tối ưu (ĐK LQG)
1.GIỚI THIỆU:
1.1 Hệ con lắc ngược:
Mô hình điều khiển con lắc ngược là một mô hình thí
nghiệm hệ thống Cơ – điện tử, lý tưởng cho việc ứng
dụng thuật toán điều khiển hiện đại và kỹ thuật điều
khiển máy tính.
1.2. Hệ phi tuyến:
Là hệ thống trong đó quan hệ vào ra không thể mô tả
bằng phương trình vi phân/ sai phân tuyến tính
2. CÁC PHƯƠNG TRÌNH VI PHÂN ĐƯỢC SỬ DỤNG ĐỂ
XÂY DỰNG HỆ THỐNG

• Phương trình động lực học:

+ Động năng của con lắc theo hai phương xy
2 2
1 1 1 1
Tp = m x + m y = m( x + l  cos  ) + m(−l  sin  ) 2 (2.1)
2

2 2 2 2
1 1 2 2
=> Tp = m x + ml x  cos  + ml 
2
(2.2)
2 2
1 (2.3)
+ Động năng của xe: TC = M x
2
Hình 2.1. Mô hình hệ con lắc ngược
2
M: Khối lượng (kg)
+ Động năng của hệ thống: T = TC + TP (2.4)
m: Khối lượng con lắc (kg)
+ Thế năng của hệ thống chính là thế năng của vật nặng l: Chiều dài con lắc (m)
đầu con lắc: (2.5)
u: Lực tác động vào xe (N)
g: gia tốc trọng trường (m/s^2)
x: vị trí xe con lắc (m)
θ: góc lệch giữa con lắc và
phương thẳng đứng (rad)
2. CÁC PHƯƠNG TRÌNH VI PHÂN ĐƯỢC SỬ DỤNG ĐỂ
XÂY DỰNG HỆ THỐNG
+ Toán tử Lagrangian: L = 1 ( M + m) x 2 + ml x  cos  + 1 ml 2  2 − mgl cos  (3.6)
2 2
+ Áp dụng công thức Euler-lagrange ta có:

d L L
 − =U => ( M + m ) x + ml (cos  )  − ml (sin  )  2
=U (2.7)
dt   x   x

d L  L
 −
dt     
=0 => m x cos  + ml  − mg sin  = 0 (2.8)

Từ phương trình (3.7) và (3.8) có phương trình vi phân:

U + ml (sin  )  2 − mg cos  sin  (2.9)

x=
M + m − m(cos  ) 2

U cos  − ( M + m) g (sin  ) + ml (cos  sin  ) 

= (2.10)
ml (cos  ) − ( M + m)l
2
2. CÁC PHƯƠNG TRÌNH VI PHÂN ĐƯỢC *PTTT tuyến tính hóa quanh điểm cân bằng
SỬ DỤNG ĐỂ XÂY DỰNG HỆ THỐNG: thẳng đứng (góc lệch 𝜃 nhỏ hơn 10).
*Chuyển đổi về dạng chuẩn:
 x1     f1     0 1 0 0  0 
x
 1 M + m   x1   1 
x     
d d  2  d    f 2  (2.11)    0 0 0   − 
x = f ( x, u , t ) = = = g
dt dt  x3  dt  x   f3   x2  =  Ml   x2  +  Ml  u
         0 0 0 1   x3   0  (2.13)
 x4   x   f 4   x3      
*Đặt các biến trạng thái 𝑥1 = 𝜃, 𝑥2 = 𝑥, 𝑥3 = 𝜃,ሶ 𝑥4 = 𝑥,ሶ 𝑓 = 𝑢    −m g 0 0 0  x4   1 
 x4   M   M 
f1 = x2 u cos( x1 ) − ( M + m) g sin x1 + ml (cos x1 sin x1 ) x 2 2
f2 =
ml (cos x1 ) 2 − ( M + m)l
f3 = x4 *Thay cụ thể thông số của hệ con lắc
u + ml (sin x1 ) x 2 − mg cos x1 sin x1
2
f4= ngược: M=1(kg); m=(0.1); l=1(m); g=9.81
M + m − ml (cos x1 ) 2
 
   x2   x1   0 1 0 0   x1   0 
 x1   u cos x1 − ( M + m) g sin x1 + ml (cos x1 sin x1 ) x2    
   
 x2  = 10.78 0 0 0   x2   −1
=>  2 =
x  ml (cos x ) 2
− ( M + m)l  + u

1
 (2.12)    0 0 0 1   x3   0 
   x3   (2.14)
 x3   x4     
   −0.98 0 0 0   x4   1 
   u + ml (sin x1 ) x2 2 − mg cos x1 sin x1 
 x4 
 x4   M + m − m(cos x1 ) 2 
 
2. CÁC PHƯƠNG TRÌNH VI PHÂN 3. CẤU TRÚC CÁC BỘ ĐIỀU
ĐƯỢC SỬ DỤNG ĐỂ XÂY DỰNG KHIỂN
HỆ THỐNG 3.1.Bộ điều khiển phi tuyến hồi tiếp tuyến tính hóa

Phương trình tuyến tính có dạng:

d
 x = A x + B u (3.15)
dt
Suy ra:
 0 1 0 0 0
10.78   −1
B= 
0 0 0 Hình 3.1. Cấu trúc bộ điều khiển hồi tiếp tuyến tính hóa
A=
 0 0 0 1 0
   
 −0.98 0 0 0 1 Hai vòng điều khiển:
• Vòng điều khiển trong: Bộ điều khiển hồi tiếp
tuyến tính hóa, biến đổi hệ phi tuyến thành hệ
tuyến tính
• Vòng điều khiển ngoài: Bộ điều khiển bám, thiết
kế dựa vào lý thuyết điều khiển tuyến tính thông
thường.
3. CẤU TRÚC CÁC BỘ ĐIỀU KHIỂN
3.1.Bộ điều khiển tối ưu LQG

• A,B,C: ma trận hệ số của

hệ phương trình trạng thái.
• L : độ lợi của bộ lọc
Kalman.
• K: độ lọc hồi tiếp trạng thái.

Hình 3.2. Cấu trúc bộ điều khiển LQG

4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:
Từ phương trình (3.12), ta có:
 y1     x1 
Tín hiệu ngõ ra : y =   =   =  
 y2   x   x3 
Mục tiêu điều khiển là đảm bảo tín hiệu ngõ ra bám theo tín hiệu đặt với POT<5% và tqd  0.2 ( s )
• Bước 1: Tính đạo hàm của các sai số ngõ ra
Đặt sai số bám của hệ phi tuyến là: e1 = y1d − y1 (4.1) và e2 = y2 d − y2 (4.2)

Đạo hàm bậc 1 của sai số bám: e1 = y1d − y1 = y1d − x2 (4.3) và e 2 = y2 d − y2 = y2 d − x4 (4.4)

Đạo hàm bậc 2 của sai số bám: e1 = y1d − y1 = y1d − x 2 = y1d − f1 ( x ) − g1 ( x ) u (4.5)

e 2 = y2 d − y2 = y2 d − x 4 = y 2 d − f 2 ( x ) − g 2 ( x ) u (4.6)

cos x1
Với: ( M + m) g sin x1 + ml (cos x1 sin x1 ) x2 (4.7) g1 ( x ) = (4.8)
f1 ( x ) = ml (cos x1 ) 2 − ( M + m)l
ml (cos x1 ) 2 − ( M + m)l
1
ml (sin x1 ) x2 2 − mg cos x1 sin x1 (4.9) g2 ( x ) =
f2 ( x ) = M + m − m(cos x1 ) 2
(4.10)
M + m − m(cos x1 ) 2
4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:
• Bước 2: Bộ điều khiển hồi tiếp tuyến tính hóa

u1 =
1
(− f ( x) + v ) g2 ( x )
(4.11) u2 =
1
(− f ( x) + v ) (4.12)
g1 ( x )
1 1 2 2

Thay (4.11) (4.12) vào phương trình (4.5) (4.6) ta được: e1 = y1d − v1 (4.13) và e2 = y2 d − v2 (4.14)

• Bước 3: Bộ điều khiển bám tuyến tính v1 = y1d + k1 e1 + k2 e1 (4.15) và v2 = y2 d + k3 e2 + k4 e2 (4.16)

• Bước 4: Tính thông số bộ điều khiển
Thay (4.15) (4.16) vào (4.13) (4.14) ta được:
e1 = −k1 e1 − k2 e1 (4.17) và e2 = −k3 e2 − k4 e2 (4.18)

Phương trình đặc trưng động học sai số: s + k1s + k2 = 0

2
(4.19)

Các thông số dùng để chọn hệ số k dựa vào công thức sau:

 − 
POT = exp    5% (4.20) →   0.69 chọn  = 0.7
 1−  2 
4 
→ n  28.57 chọn n  30
tqd =  0.2 ( s ) (4.21)
n
Phương trình đặc trưng động học sai số mong muốn: s + 2n s +  = 0
2 2
n
(4.22)
4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:
Cân bằng hệ số ta được:
k1 = k3 = 2n = 2  0.7  30 = 42 k2 = k4 = n2 = 302 = 900
• Bước 5: Thiết kế bộ lọc tín hiệu vào yd ( t ) khả vi bị chặn đến đạo hàm bậc 2. Hàm truyền của bộ lọc:
Chọn bộ lọc thông thấp bậc 2 để tín hiệu
1
GLF ( s ) = (4.23)
( 0.1s + 1)
2

❖ Xây dựng bộ điều khiển hồi tiếp tính hóa bằng Matlab

Hình 4.1 Bộ điều khiển hồi tiếp tuyến tính hóa trong Matlab
4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:
- Bộ điều khiển hồi tiếp tuyến tính hóa bao gồm các thành phần cụ thể sau:
▪ Hệ xe con lắc ngược (Inverted Pendulum on a Cart)

Hình 4.2: Mô hình hệ xe con lắc ngược trong Matlab

4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:

▪ Khối tín hiệu đặt ▪ Khối điều khiển bám( Tracking control)

Hình 4.3: Mô phỏng khối tín hiệu đặt

4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:
▪ Các khối Add Mux và Scope để phục vụ tổng hợp
▪ Khối điều khiển tuyến tính hóa
và quan sát tín hiệu ngõ ra
(Linearization control)

Hình 4.4: Scope, Mux và Add

4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:

❖ Kết quả mô phỏng

▪ Ngõ ra bộ điều khiển:

Hình 4.5: Tín hiệu ngõ ra góc lệch của con lắc Hình 4.6: Ngõ ra vị trí và vận tốc của xe
4.BỘ ĐIỀU KHIỂN PHI TUYẾN HỒI TIẾP TUYẾN TÍNH HÓA:

❖ Kết quả mô phỏng

▪ Tín hiệu sai số: ▪ Tín hiệu điều khiển:

Hình 4.7: Sai số bộ điều khiển Hình 4.8: Tín hiệu điều khiển của hệ thống
5. XÂY DỰNG BỘ ĐIỀU KHIỂN TỐI ƯU LQG
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 :
𝐿𝑄𝐺 = 𝐿𝑄𝑅 + Kalman Filter

Tín hiệu điều khiển tối ưu LQR: u (t ) = − K x(t ) (4.1)

−1
Với độ lợi hồi tiếp trạng thái: K = R B P
T
(4.2)
Trong đó 𝑃 là nghiệm bán xác định dương của phương trình đại số Ricatti:
−1
PA + A P + Q − PBR B P = 0 (4.3)
T T


 x(t ) = A x(t ) + Bu (t ) + L[ y (t ) − y (t )] (4.4)
Bộ lọc Kalman: 
 y (t ) = C x(t )

Với độ lợi ước lượng: L=C T R −1 (4.5)

N
−1
Với Π là nghiệm của phương trình Ricatti: A + A − C RN C + QN = 0
T T
(4.6)
5. XÂY DỰNG BỘ ĐIỀU KHIỂN TỐI ƯU LQG
*Mô hình xe con lắc ngược: Trong đó khối matlab function có thông số
như sau:

Hình 5.1: Xây dựng mô hình hệ xe con lắc ngược

5. XÂY DỰNG BỘ ĐIỀU KHIỂN TỐI ƯU LQG
* Thiết kế bộ điều khiển LQR cho hệ con lắc ngược có nhiễu

Với R = 1
Q=[1 0 0 0;
0 1 0 0;
0 0 100 0;
0 0 0 1 ];

Hình 5.2: Mô tả hệ con lắc ngược khi có nhiễu

[rad], [rad/s]
[m],[m/s^2]
[N]

Nhận xét: Nếu bộ LQR có nhiễu đo lường thì chất lượng điều khiển sẽ bị ảnh hưởng đáng kể.
Vì vậy cần phải lọc nhiễu thông qua bộ lọc Kalman.
4. XÂY DỰNG BỘ ĐIỀU KHIỂN TỐI ƯU LQG *Chương trình trong Matlab:
%Programmed by Nhom 15
*Thiết kế bộ lọc Kalman
%date 11/6/2023
clc;
clear all;
M=1;
m=0.1;
l=1;
g=9.81;
theta_init = 0.1;
theta_d_init = 0;
x_init = 0;
 6.5650 -0.0572  x_d_init = 0;
 21.5657 -0.1878 A = [0 1 0 0; ((M+m)*g/M*l) 0 0 0; 0 0 0 1; -(m*g/M) 0 0 0];
L= 
 -0.5716 0.1470  B = [0; -(1/(M*l)); 0; 1/M];
 
 -1.9588 0.0271  C = [1 0 0 0; 0 0 1 0];%ma trận c phải cùng số hàng với ma trận Rn
Q=[1 0 0 0; 0 1 0 0; 0 0 100 0; 0 0 0 1];
Hình 5.3. Mô hình bộ lọc Kalman R= [1];
Qn=0.000001*[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];%cùng số cột với ma trận A, ma trận vuông
Rn=[0.001 0;0 0.01]; %phương sai nhiễu vị trí và góc lệch
G=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
K= lqr(A,B,Q,R)
[L,P,E] = lqe(A,G,C,Qn,Rn)
4. XÂY DỰNG BỘ ĐIỀU KHIỂN TỐI ƯU LQG
1 0 0 0
Với 0 1 0 0
QN = 0.000001   
 0 1 0 0 0 0 1 0
10.78  
0 0 0 0 0 0 1
A= 
 0 0 0 1
  0.001 0 
 −0.98 0 0 0
RN =  
 0 0.01

0
 −1
B=  1000 
C=
0 
   0010 
1

1 0 0 0 K =  −70.1721 −22.1091 −10 −11.0482

0 1 0 0
R=1 Q=   6.565 −0.0572 
0 0 100 0  Hình 5.4. Hình xây dựng bộ điều khiển LQG  21.5657 −0.1878 
  L= 
0 0 0 1
 −0.5716 0.147 
 
 −1.9588 0.0271 
ad], [rad/s]
[m],[m/s^2]
[N]

III. KẾT LUẬN

Khi không có bộ lọc Kalman, hệ xe con lắc ngược bị nhiễu góc và nhiễu vị trí, dẫn đến hệ thống khó ổn
định
=> Bộ lọc Kalman có chức năng làm giảm độ nhiễu góc và nhiễu vị trí, giúp cho hệ thống dễ ổn định hơn
CẢM ƠN THẦY VÀ CÁC BẠN
ĐÃ LẮNG NGHE BÀI THUYẾT
TRÌNH CỦA NHÓM EM!