BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HCM



MÔN HỌC: THỰC TẬP ĐIỀU KHIỂN TỰ ĐỘNG

ĐIỀU KHIỂN CON LẮC NGƯỢC SỬ

DỤNG BỘ ĐIỀU KHIỂN LQR KẾT
HỢP SWING UP

GVHD: thầy Nguyễn Văn Đông Hải

Nhóm 4
SVTH: MSSV

Dương Trường Giang 19151116

Dương Thành Huy 19151023
Lớp thứ 4 – Tiết 1-5

Tp. Hồ Chí Minh, tháng 12 năm 2021

 MỤC LỤC

BÌA........................................................................................................................................................1
MỤC LỤC.............................................................................................................................................2
CHƯƠNG 1. MÔ HÌNH TOÁN..........................................................................................................1
1.1.Phương trình động học:.............................................................................................................1
1.2.Bộ điều khiển LQR:....................................................................................................................2
1.3. Bộ điều khiển swing up:............................................................................................................4
1.4.Xây dựng hệ thống trên Matlab Simulink................................................................................5
CHƯƠNG 2. KẾT QUẢ MÔ PHỎNG...............................................................................................7
2.1. Mô phỏng điều khiển hệ thống với bộ điều khiển LQR..........................................................7
2.2. Mô phỏng điều khiển hệ thống với bộ điều khiển LQR kết hợp swing up............................9
TÀI LIỆU THAM KHẢO..................................................................................................................10
 CHƯƠNG 1. MÔ HÌNH TOÁN
1.1. Phương trình động học:

Tiến hành tổng hợp các lực tác động vào xe con lắc theo phương ngang ta được
các phương trình về chuyển động:
M ẍ +b ẋ + N=F (1.1)
Chúng ta có thể tổng hợp các lực theo phương thẳng đứng nhưng không hữu ích vì
chuyển động của hệ thống con lắc ngược không chuyển động theo hướng này và trọng
lực của Trái Đất cân bằng với tất cả lực thẳng đứng.
m ẍ +ml θ̈ cos θ−ml θ̇2 sinθ=N (1.2)
L
Trong đó là chiều dài từ tâm con lắc tới điểm gốc là: l= Từ phương trình (1.2) ta
2
thay vào phương trình (1.1) được:
( M +m ) ẍ +b ẋ +ml θ̈ cos θ−ml θ̇ 2 sin θ=F (1.3)
Tổng hợp các lực vuông góc với thanh con lắc:
P sin θ+ N cos θ−mgsin θ=ml θ̈+ ml ẍ cos θ (1.4)
Để làm triệt tiêu hai điều kiện P và N ta tiến hành tổng hợp moment tại trọng tâm
thanh con lắc:
−Pl sin θ−N cos θ=I θ̈(1.5)
Thay phương trình 1.4 vào phương trình 1.5 ta được:
( I +ml 2 ) θ̈+mgl sin θ=−ml ẍ cos θ(1.6)

1
 Từ phương trình 1.3 và 1.6, ta biến đổi ra được:
F−b ẋ−ml θ̈ cos θ+ ml θ̇2 sin θ
ẍ= (1.7)
M +m
−ml ẍ cos θ−mlg sin θ
θ̈= (1.8)
I + ml 2

Thay các phương trình (1.7) và (1.8) vào các phương trình (1.3) và (1.6) ta được
phương trình toán của hệ con lắc ngược phi tuyến và đơn giản hóa như sau:
2
F+ml (sin θ) θ̇ −mg sin θ cos θ
ẍ= 2
(1.9)
M +m−mcos θ
θ̈=F cos θ−( M + m ) g sin θ+ml ¿ ¿

Vì để mô phỏng sát với thực tế nên ngõ ra của bộ điều khiển LQR là tín hiệu
điện áp. Lúc này, ta sẽ chuyển từ tín hiệu u điều khiển (điện áp) thành lực tác dụng vào
hệ con lắc ngược:
Km K m2
F= v− ẋ (2.1)
R m∗r Rm∗r 2

Từ đó ta có được mô hình toán của hệ con lắc ngược với ngõ vào là điện áp như
sau:
Km K m2 2
v− ẋ +ml( sinθ) θ̇ −mgsin θ cos θ
R m∗r Rm∗r 2

ẍ= 2
(2.2)
M +m−m cos θ
2
Km Km
θ̈= v− 2
ẋ cos θ−( M +m ) g sin θ+ ml ¿ ¿
R m∗r Rm∗r

1.2.Bộ điều khiển LQR:

Để có thể thành lập bộ điều khiển LQR, ta đặt lại biến cho phương trình về dạng
bậc 1:

{
x 1=θ
x =θ̇
Đặt: 2 → Hệ phương trình toán trở thành: ¿
x 3=x
x 4= ẋ

Tín hiệu điều khiển:

2
 []
x1
x
u=−Kx Với: x= x2
3
x4

Với K là ma trận điều khiển tính từ K=lqr ( A , B , Q , R )

Trong đó: Ma trận Q và R là ma trận dương cần chọn lựa (ma trận trọng số). A
và B là ma trận tuyến tính hóa tại điểm làm việc.

Điểm làm việc: ( x 0 u0 ) ≡ {x = x =xu=∅=x =0

1 2 3 4

Hệ phương trình trạng thái được viết lại:

T
ẋ=f ( x , u ) với x=[ x 1 x 2 x 3 x 4 ]

[ ]
x2
f ( x , u)
f= 1
x4
f 2 ( x , u)

Tại ( x 0 ,u 0 ), ta có thể xem như:

ẋ=f ( x , u ) ≅ ⏟
⏟ ˙ Bu
x= Ax+
Hệ phituyến Hệ tuyếntính

[ ]
0 1 0 0
∂f1 ∂f1 ∂f1 ∂f1
∂x ∂ x2 ∂ x3 ∂ x4
Với A= 0 1 0 0 1
∂f2 ∂f2 ∂f2 ∂f2
∂ x1 ∂ x2 ∂ x3 ∂ x4

[ ]
T
∂f1 ∂f 2
B= 0 0
∂u ∂u

[ ]
0 1 0 0
2
−g( M +m) −K m
0 0
lm−l (M + m) Rm r 2 (lm−l ( M +m ))
A= 0 0 0 1
2
−mg −K m
0 0
M Rm r M
2

3
 [ ]
0
Km
R r (lm−l ( M +m ) )
B= m
0
Km
Rm rM

Sau khi thay các thông số của hệ thống vào thì ta tính được giá trị của ma trận
A và B như sau:

[ ]
0 1 0 0
58.86 0 0 0.1436
A= 0 0 0 1
−3.7278 0 0 −0.0330

[ ]
0
B= −0.812
0
0.1869

Sau đó ta chọn ma trận Q và R tính được K như sau:

[ ]
100 0 0 0
0 1 0 0
Q= 0 0 150 0
0 0 0 1

R=1
K = [ -184.9854 -25.0596 -12.2474 -16.9729]
1.3. Bộ điều khiển swing up:

Đối với bộ điều khiển swing up, mục tiêu của ta chính là đưa con lắc từ vị trí
ban đầu hướng xuống thông qua quá trình dao động để con lắc đạt được vị trí cân
bằng. Để làm được điều đó ta cần thêm năng lượng vào hệ thống.

4
 Năng lượng của hệ thống bằng tổng của động năng quay cộng cho thế năng của
con lắc. Và được biểu diễn như sau:
1
E= Jθ̇2 +mglcosθ
2
Gia tốc sẽ được tính theo công thức sau:
ẍ=k ( E−Eup ) sgn ¿

1.4. Xây dựng hệ thống trên Matlab Simulink

Khối mô phỏng bộ điều khiển swing up

Khối mô phỏng hệ con lắc ngược

Khối điều khiển

5
Toàn cảnh hệ thống

6
 CHƯƠNG 2. KẾT QUẢ MÔ PHỎNG
2.1. Mô phỏng điều khiển hệ thống với bộ điều khiển LQR
Điều khiển con lắc với góc lệch ban đầu là 0.2 rad với các ma trận như sau:

[ ]
1 0 0 0
0 1 0 0
Q=
0 0 1 0
0 0 0 1

R=1
K = [ -184.9854 -25.0596 -12.2474 -16.9729]

Nhận xét: Với ma trận K như trên, ta có thể điều khiển con lắc về trạng thái cân
bằng. Thời gian để góc con lắc vị trí cân bằng là khoảng 4 giây. Thời gian để xe về vị
trí cân bằng còn khá chậm. khoảng 12 giây.
Sau đó nhóm tiến hành tăng giá trị ma trận Q như sau để thời gian xe về vị trí câ
bằng nhanh hơn.

[ ]
1 0 0 0
0 1 0 0
Q= 0 0 100 0
0 0 0 1

R=1
K = [ -180.1570 -24.3616 -10.0000 -15.0323]

7
 Nhận xét: Sau khi Q(3,3) tăng lên 100 thì bộ điều khiển LQR đã điều khiển vị
trí xe tốt hơn. Thời gian để xe về vị trí cân bằng là khoảng 4 giây.
Đồng thời, nhóm cũng tiến hành khảo sát nếu tăng giá trị Q(3,3) lên quá nhiều
để bộ điều khiển quan trọng việc điều khiển vị trí xe nhiều hơn thì các trạng thái khác
sẽ được điều khiển chậm hơn và thời gian đáp ứng lâu hơn.Với thông số:

[ ]
1 0 0 0
0 1 0 0
Q=
0 0 1500 0
0 0 0 1

R=1

8
2.2. Mô phỏng điều khiển hệ thống với bộ điều khiển LQR kết hợp swing up
Đáp ứng của hệ thống với góc lệch ban đầu là π.

Nhận xét: Ta có thể thấy sau khoảng 11 giây thì hệ thống đã swing up thành
công và kết hợp được với bộ điều khiển lqr để điều khiển con lắc ổn định ở vị trí cân
bằng. Nhưng vị trí xe tăng cao nên sẽ khó đáp ứng được trong thực tế. Còn cần phải
tinh chỉnh bộ điều khiển nhiều hơn nữa.

9
 TÀI LIỆU THAM KHẢO
1. Cao Xuân Cường, Trần Đình Khôi Quốc, ĐIỀU KHIỂN MÔ HÌNH CON LẮC NGƯỢC
SỬ DỤNG BỘ ĐIỀU KHIỂN LQR VỚI HAI VÒNG PHẢN HỒI, Tạp chí khoa học trường
Đại học Cao Đẳng Tây Nguyên.
2. Debasish Chatterjeea, Amit Patraa, Harish K. Joglekarb, Swing-up and stabilization of a
cart–pendulum system under restricted cart track length Debasish Chatterjeea, Amit Patraa,
Harish K. Joglekarb, Elsevier.
3. Control Tutorial for Matlab and Simulink, Control Tutorials for MATLAB and Simulink -
Inverted Pendulum: System Modeling (umich.edu).

10