Thietkemophong utc bai tap lon, bao cao

THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI
KHOA CƠ KHÍ
BỘ MƠN CƠ ĐIỆN TỬ
--------🙟🙟🙟--------

THIẾT KẾ MƠN HỌC
ĐỀ TÀI : THIẾT KẾ MƠ HÌNH MƠ PHỎNG HỆ ĐIỀU KHIỂN
CON LẮC NGƯỢC
Sinh viên thực hiện:
Lớp
:
Khoa
:
Mã sinh viên
:

Đặng Xuân Long
Cơ điện tử 2 K60
Cơ khí

191313722

Người hướng dẫn : Giảng viên Phạm Xuân Hiển.

HÀ NỘI,10/2022

1


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

MỤC LỤC
LỜI CẢM ƠN........................................................................................................3
Phần 1 : Thành lập phương trình vi phân chuyển động..................................4
Phần 2 : Tìm hàm truyền của hệ thống.............................................................7
Phần 3: Thiết kế bộ điều khiển Fuzzy ( bộ điều khiển mờ )................................11
Phần 3: Thiết kế bộ điều khiển LQR cho con lắc ngược..............................15
Phần 4: Thiết kế bộ điều khiển LQR kết hợp bộ lọc Kalman.......................22
4.1. Tính tốn thơng số cho bộ điều khiển LQR.............................................22
4.2. Khảo sát bộ lọc Kalman...........................................................................23
Phụ lục................................................................................................................26
Tài liệu tham khảo........................................................................................27

2

THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

LỜI CẢM ƠN
Em xin chân thành cảm ơn quý thầy, cô trường Đại Học Giao Thơng
Vận Tải đã tận tình dạy dỗ trong suốt những năm qua. Trong đó phải kể đến
quý thầy cơ trong Khoa Cơ Khí đã tạo điều kiện cho chúng em thực hiện đề
tài nghiên cứu khoa học này.
Đặc biệt, em xin chân thành cảm ơn thầy – giảng viên hướng dẫn
Phạm Xuân Hiển đã tận tình giúp đỡ chúng em trong quá trình lựa chọn đề tài
và hỗ trợ em trong quá trình thực hiện đề tài. Cung cấp cho em những kiến
thức quý báu cũng như những lời khuyên cực kỳ hữu ích. Tạo động lực cho
em hồn thành tốt nhiệm vụ của mình. Bên cạnh đó, em cũng xin cảm ơn các
bạn sinh viên lớp Cơ Điện Tử K60 đã đóng góp ý kiến cho em thực hiện đề
tài đạt hiệu quả hơn. Với thời gian thực hiện đề tài ngắn, kiến thức còn hạn
hẹp, dù em đã rất cố gắng nhưng vẫn không tránh khỏi những sai sót, em rất
mong nhận được lời chỉ dẫn thêm của quý thầy cô và bạn bè.
Hà Nội, ngày 24 tháng 10 năm 2022
Sinh viên
Long
ĐặngXuân Long

Đề bài :
3

THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN
Cho mơ hình con lắc ngược như hình vẽ trong đó xe đẩy di chuyển
trong mặt phẳng có khối lượng , con lắc có khối lượng , thanh nối có
chiều dài (bỏ qua khối lượng và quán tính). Tác dụng lên xe đẩy một
lực điều khiển u(t ) với mục đích điều khiển là ổn định con lắc tại vị trí
thẳng đứng ( θ = 0°). Yêu cầu:

Phần 1 : Thành lập phương trình vi
phân chuyển động
Yêu cầu : Viết phương trình vi phân chuyển động của mơ hình con lắc
với hai tọa độ suy rộng là θ (rad , góc nghiêng của con lắc) và x ( m ,
vị trí của xe đẩy).
Với thông số như sau :
M
m
l

Khối lượng xe
đẩy
Khối lượng con
lắc
Chiều dài dây
của con lắc

4

5.6 kg
0.8 kg
0.5 m


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN
Vị trí tọa độ
của con lắc
Vị trí tọa độ
của xe đẩy
Lực điều khiển
tác dụng xe đẩy

x
u(t )

Rad
m
N

Chọn hệ tọa độ gắn liền hệ như hình. Ta sử dụng hệ tọa độ suy rộng ở
đây là q 1=x và q 2=θ.

{
{

x =x

Ta có : yM =0
M

(1)

x m=x+ lsin θ
y m=l cos θ

(2)

Đạo hàm hai vế của hệ phương trình (1) và (2) ta lần lượt xác định
được vận tốc của m và M :

{

x´ M =´x
´y M =0

{

´x m= ´x + θ´ lcos θ
´y m=−θ´ lsin θ

Động năng của hệ là :

1

1

K = 2 M( ´x M 2 + ´y M 2) + 2 m( ´x m2 + ´y m2)
1

1

1

1

= 2 M ´x 2 + 2 m¿ + θ´ 2 l2 sin θ2)
= 2 M ´x 2 + 2 m¿)
Thế năng của hệ là :
P = mglcos θ
Hàm Lagrange được xác định như sau :
1

1

L = K – P = 2 M ´x 2 + 2 m¿) - mglcos θ
Ta có :
∂L
´x
´x +l θ´ cos θ )
∂ ´x = M + m(
d ∂L
(

dt ∂ x´

) = ( M + m ) ´x + mlθ´ cos θ - mlθ´

∂L
=0
∂x

5

2

sin θ


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN
Lại có :
∂L
∂ θ´

= ml θ´ + ml ´x cos θ
2

d ∂L

dt ( ∂ θ´
∂L
∂θ

) = ml θ´ + ml ´x cos θ - ml ´x θ´ sin θ
2

= - ml ´x θ´ sin θ +

mglsin θ

Trong trường hợp này có một lực đẩy u(t) tác dụng lên vật M.
Phương trình Lagrange loại 2 có dạng :
d ∂L
(
dt ∂ q´ i

)-

∂L
∂ qi

=Q

i

Thay các giá trị vừa tính được vào phương trình Lagrange :
(M +m) ´x +ml θ´ cos θ−ml θ´ 2 sin θ=u(t)
ml 2 ´θ+ml ´x cos θ−ml ´x θ´ sin θ+ml x´ θ´ sin θ−mgl sinθ=0

{

(M +m) ´x +ml θ´ cos θ−ml θ´ 2 sin θ=u(t)
´
m x´ cos θ+ ml θ=mg
sin θ(3)

{

Nhân cả 2 vế của (3) với cos θ ta được hệ :
(M +m) ´x +ml θ´ cos θ−ml θ´ 2 sin θ=u(t)(4)
2
m ´x (cos θ) +m l θ´ cos θ=mg sin θ cos θ(5)

{

Lấy (4) trừ (5) ta được :
⌊( M +m)−m¿ ¿ = u(t) + ml θ´ 2 sin θ - mg sin θ cos θ

Suy ra : ´x =
Từ đó : θ´

u(t)+ml θ´ 2 sin θ−mg sinθ cos θ
( M + m)−m¿ ¿ ¿

= −(M + m) g sin θ+u (t)cos θ+ml θ´

6

(6)

2

¿ ¿ ¿ (7)


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

Phần 2 : Tìm hàm truyền của hệ thống
u cầu : Tuyến tính hóa mơ hình bằng việc xấp xỉ các đại lượng , khi
góc là θ bé. Viết hệ phương trình vi phân chuyển động của hệ dưới
dạng mơ hình khơng gian trạng thái.
´ ) rất nhỏ (
Do ta phải giữ con lắc ngược thẳng đứng nghĩa là θ(t ), θ(t
θ<10 ° ¿. Đồng thời bỏ qua khối lượng cũng như momen quán tính dây

Bởi vậy θ → 0 => sin θ ≈ θ & cos θ = 1 & θ θ´ 2 = 0
Từ đó hệ phương trình vi phân chuyển động được viết lại như sau :

´
(M +m)´x +ml θ=u(t)
2´
ml θ+ml x´ =mgl θ

{

 Đây là mơ hình tuyến tính hóa của con lắc đảo ngược.
Từ mơ hình tuyến tính hóa ta suy ra được hệ sau :

{

M ´x =u(t )−mgθ
´
Ml θ=( M + m)gθ−u(t )

Đặt biến z 1, z 2, z 3, z 4 là các biến trạng thái

{

z 1=θ
´ z´1
z 2=θ=
z´2=θ´

{

z 3=x
z 4=´x = z´3
z´4= x´



{

M z´4=u (t)−mg z 1

Ml z´2=( M +m) g z1 −u(t)

Mơ hình trạng thái được mô tả dưới dạng ma trận sau :

7


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

z´1
z´2
z´3
z´4

[]

[

=

0
(M + m) g
Ml
0

−mg
M

1 0 0
0 0 0
0 0 1
0 0 0

0
¿1
Ml
0
1
M

][ ] [ ]
z1
z2
z3
z4

+

u(t)

z1
0
z
1
0

0
0
θ
0
2
x = 0 0 1 0 z 3 + 0 u(t)
0
z4

[] [

]

[ ] []

Thay các gias trị M = 5,6 ; m = 0,8 ; l = 0,5 ; g = 9,8 ta được :
z´1
z´2
z´3
z´4

[] [
=

0
22.4
0
−1,4

1

0
0
0

0
0
0
0

0
0
1
0

z1
z2
z3
z4

0
−0,357
0
0,179

][ ] [ ]
+

u(t)

z1

0
1 0 0 0 z2
θ
0
=
+
u(t)
0 0 1 0 z3
x
0
0
z4

[] [
3.

]

[ ] []

Xem góc nghiêng θ (t) của con lắc là đầu ra (Output), lực điều
θ( s)

khiển u(t) là đầu vào (Input). Tìm hàm truyền G(s)= u( s) của hệ.
Xét hệ thống tuyến tính có phương trình trạng thái tổng qt :
= Ax+ Bu
{ ´xy=Cx+
Du

Biến đổi Laplace 2 vế của hệ ta được :

sX (s)=AX (s)+ BU (s )
Y (s)=CX (s)+ DU ( s)

{

8


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

( sI− A) X (s )=BU (s)
Y ( s)=CX (s )+ DU ( s)

{

 Y(s) = C( sI− A)−1BU(s) + DU(s)
Y (s)

 Hàm truyền của hệ thống : G(s) = U ( s) = C( sI− A)−1B + D

Để đơn giản hóa ta dùng Matlab tính tốn. Với Matlab ta có 2 phương
pháp để tính hàm truyền từ mơ hình trạng thái ( state space model) và
được trình bày như sau :
Cách 1

Cách 2:

9


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

Kết quả :

θ( s)

Vậy hàm truyền của hệ G(s)= u( s)

=

Đồ thị hàm truyền :

10

−0.357
.
s 2−22.4

THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

Phần 3: Thiết kế bộ điều khiển Fuzzy ( bộ điều
khiển mờ )
Để điều khiển cân bằng con lắc, ta sử dụng bộ điều khiển Fuzzy bằng
phương pháp Sugeno.
1. Chọn các biến vào/ra : Ta chọn bốn biến vào ra và góc lệch của con
lắc, vận tốc của con lắc, vị trí xe và tốc độ xe, biến ra là lực tác dụng
vào xe. Tập cơ sở của các biến phụ thuộc chủ yếu vào đối tượng ta
chọn như sau :
Góc lệch : [ -0.3, 0.3] (rad)
Vận tốc góc : [ -1.1] (rad/sec)
Vị trí xe : [ -3,3] (m)
Vận tốc xe : [ -3, 3] (m/s)
Lực tác dụng : [ -40, 40] (N)
11


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG

: Đặng Xn
Long ĐIỀU KHIỂN
2. Chuẩn hóa tập cơ sở của các biến vào/ra về miền [-1,1], do đó các
ma trận hệ số khuếch đại của các khối tiền xử lý và hậu xử lý như sau :
Các hệ số tiền xử lý : K1 = 1/0.3 ; K2 = 1 ; K3 = 1/3 ; K4 = 1/3
Hệ số hậu xử lý : K5 = 100;
3. Định nghĩa các tập mờ mô tả các giá trị ngôn ngữ của các biến vào
và biến ra của bộ điều khiển mờ. Chọn 3 tập mờ ( giá trị ngôn ngữ ) cho
mỗi biến vào, các tập mờ này được phân hoạch mờ trên cơ sở chuẩn
hóa và hàm liên thuộc có dạng tam giác. Số tập mờ cho biến ở ngõ ra
được chọn bằng 9. Với càng nhiều liên thuộc đầu ra, đầu vào hệ thống
sẽ được ổn định và hoạt động mượt mà hơn.
Membership Function
NB ( Negative Big)
NM ( Negative Middle )
NE ( Negative )
NS ( Negative Small )
ZE ( Zero)
PS ( Positive Small )
PO ( Positive )
PM ( Positive Middle )
PB ( Positive Big )

Value
-1
-0.75
-0.5
-0.25
0
0.25

0.5
0.75
1

4. Đưa ra hệ quy tắc mờ bằng phương pháp thử sai, dựa vào kinh
nghiệm. Với ba tập mờ ở ngõ vào cho mỗi biến, để thỏa mãn tính hồn
chỉnh thì hệ quy tắc phải gồm 81 quy tắc ( phần Phụ lục ).
5. Vì ta ứng dụng quy tắc mờ trong điều khiển và khối hậu xử lý
khơng có khâu tích phân nên chọn phương pháp giải mờ trọng tâm
( COA).

12


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN

Hình 1 : Tập mờ chuẩn hóa biến ngõ vào

Các hàm liên thuộc được xác định như sau :
μ NE (x ) = tramf(x, [ -1000, -1000 , -1, 0 ])
μZE ( x ) = trimf(x, [ -1, 0 , 1 ])
μ PO (x) = tramf(x, [ 0, 1, 1000, 1000 ])

13

THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN

Hình 2 : Sơ đồ SIMULINK mô phỏng hệ thống điều khiển hệ con lắc ngược dùng
logic mờ

Hình 3: Sơ đồ khối Pendulum
0.15

0.1

0.05

0

-0.05

-0.1

0

10

20

30

40

50

Thời gian ( giây)

14

60

70

80

90

100


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG
: Đặng Xuân
Long ĐIỀU KHIỂN

0.5
0.4
0.3
0.2
0.1
0
-0.1
-0.2
-0.3
-0.4

0

10

20

30

40

50

60

70

80

90

100

60

70

80

90

100

Thời gian (giây)

15

10

5

0

-5

-10

0

10

20

30

40

50

Thời gian ( giây )

Hình 4: Kết quả mơ phỏng hệ thống điều khiển giữ cân bằng con lắc ngược

Kết quả mô phỏng trên cho thấy bộ điều khiển đã thiết kế có thể điều
khiển vị trí xe đúng vị trí đặt động thời giữ được trạng thái con lắc cân
bằng quanh vị trí thẳng đứng. Tuy vậy thời gian ổn định tương đối lâu.

Phần 3: Thiết kế bộ điều khiển LQR cho
con lắc ngược.
Từ mơ hình trạng thái đã xây dựng ở chương trên. Ta áp dụng để xây
dựng mô hình LQR.
Yêu cầu :

15


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG

thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN
1. Đặ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ỏ.
2. 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)
3. Khơng có nhiễu tác động vào hệ thống.
Từ hệ phương trình :
¿

Ta xây dựng được mơ hình con lắc ngược :

Hình 5 : Khối mơ phỏng Pendulum

Trong đó khối FCN có thơng số là các giá trị các biểu thức của hệ trên
được mô tả như sau :

16


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN

Hình 6: Khối Fcn
Chương trình Matlab tìm thơng số của bộ điều khiển LQR:

17


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN

Hình 7: Thơng số bộ điều khiển LQR

Ta lần lượt khảo sát các giá trị khác nhau của ma trận Q và R:
- Trường hợp 1: q11 = 1; q33 = 1; r =1
Ta được độ lợi hồi tiếp trạng thái: K = [-182,2531 -30.2385 -1 -4,125]
Luật điều khiển tối ưu :
u(t) = −Kx(t) → u(t) = [-182,2531 -30.2385 -1 -4,125]x(t)
18


THIẾT KẾ MÔNSv
HỌC
HỆ THỐNG
thựcMÔ
hiệnPHỎNG

: Đặng Xuân
Long ĐIỀU KHIỂN
Đáp ứng quá độ và năng lượng tiêu tốn của hệ thống:
Time Series Plot:

0.6

Góc lệch
Vận tốc góc
Vị trí cart
Vận tốc cart

0.5
0.4
0.3
0.2
0.1
0
-0.1
-0.2
-0.3

0

10

20

30

40

50

60

70

80

90

100

Time (seconds)

Time Series Plot:

16

u

14
12
10
8
6
4
2
0

-2

0

10

20

30

40

50

60

70

80

90

100

Time (seconds)

Hình 8: Đáp ứng quá độ và năng lượng tiêu tốn của hệ thống trường hợp 1

- Trường hợp 2: q11 = 1; q33 = 100; r =1
Ta được độ lợi hồi tiếp trạng thái:

K = [-182,2531 -39,3647 -10 -15,6242]
Luật điều khiển tối ưu :
u(t) = −Kx(t) → u(t) = [-182,2531 -39,3647 -10 -15,6242]x(t)
Đáp ứng quá độ và năng lượng tiêu tốn của hệ thống:
Time Series Plot:

0.4

Góc lệch
Vận tóc góc
Vị trí xe
Vận tốc xe

0.3
0.2
0.1
0
-0.1
-0.2
-0.3
-0.4

0

10

20

30

40

50

Time (seconds)

19

60

70

80

90

100


THIẾT KẾ MƠNSv
HỌC
HỆ THỐNG
thựcMƠ
hiệnPHỎNG
: Đặng Xn
Long ĐIỀU KHIỂN

Time Series Plot:

20

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

15

10

5

0

-5

0

10

20

30

40

50

60

70

80

90

100

Time (seconds)

Hình 9: Đáp ứng quá độ và năng lượng tiêu tốn của hệ thống trường hợp 2

- Trường hợp 3: q11 = 1; q33 = 100; r =10
Ta được độ lợi hồi tiếp trạng thái:
K = [-154.932 -33.176 -3.162 -7.70]
Luật điều khiển tối ưu :
u(t) = −Kx(t) → u(t) = [-154.932 -33.176 -3.162 -7.70]x(t)
Đáp ứng quá độ và năng lượng tiêu tốn của hệ thống:
Time Series Plot:

0.4

Góc lệch
Vận tốc góc
Vị trí xe
Vân tốc xe

0.3

0.2

0.1

0

-0.1

-0.2

-0.3

0

10

20

30

40

50

60

70

80

90

100

Time (seconds)

Time Series Plot:

16

u

14
12
10
8
6
4
2
0
-2

0

10

20

30

40

50

Time (seconds)

20

60

70

80

90

100