đề tài Con lắc ngược

1.

Mô hình toán học của hệ thống vật lý.
Hệ thống con lắc ngược tịnh tiến có sơ đồ như hình vẽ:

Trong đó:
-

M : khối lượng của xe (kg).

-

m : khối lượng của con lắc (kg).

-

l : khoảng cách từ trục quay đến trọng tâm của con lắc (m).

-

F : ngoại lực tác dụng vào xe theo phương x (N).

-

θ : góc lệch của con lắc so với phương thẳng đứng hướng lên (rad).

-

b : hệ số ma sát mặt đường

-

d: hệ số ma sát trục quay


-

(x,0) : tọa độ trục quay.

-

(x1,y1) : tọa độ trong tâm con lắc.

Tọa độ trọng tâm con lắc:
x1 = x + l sinθ
y1 = l cos θ

Vận tốc theo hai trục của con lắc :
•

•

•

x1 = x + l θ cos θ
•

•

y1 = −l θ sinθ

Vận tốc tổng hợp của con lắc:
•

•

•

•

•

•

V12 = x12 + y12 = x 2 + 2 x l θ cos θ + l 2 θ 2

Động năng của con lắc:
•
• •
1
1 2 1  •2
1 •2
2
2
2
K 1 = mV1 + Iω = m x + 2 x l θ cos θ + l θ  + I θ
2
2
2 
 2

Động năng của xe:
•
1
K2 = M x2
2

Thế năng của con lắc :
P1 = mgl cos θ

Thế năng của xe :
P2 = 0

Phương trình Lagrange :

L = ( K 1 + K 2 ) − ( P1 + P2 ) =

•
•
• •
 1 • 1
1  •2
m x + 2 x l θ cos θ + l 2 θ 2  + I θ 2 + M x 2 − mgl cos θ
2 
2
 2


•

•

d  ∂L  ∂L
−
+
d
θ
=0
dt  ∂ θ•  ∂θ



∂L
•

∂θ

•

(1)

•

•

= m x l cos θ + ml 2 θ + I θ

• •
•
••
d  ∂L 
 ••


2
=
ml
x
cos
θ
−
x
θ
sin
θ
+
ml
θ
+
I
θ


dt  ∂ θ• 




• •
∂L
= −ml x θ sinθ + mgl sinθ
∂θ

(1)

•

=>

( I + ml ) θ + ml x cosθ − mgl sinθ + d θ = 0
2

••

••

•

•
d  ∂L  ∂L
−
+
b
x
=F
dt  ∂ x•  ∂x
 

∂L
•

∂x

•

(2)

•

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

•
••
••
d  ∂L 
2
=
(
M
+
m
)
x
+
ml
(
θ
cos
θ
−
θ
sinθ )
dt  ∂ x• 




∂L
=0
∂x
••

••

•

•

2
(2) => ( M + m) x + ml θ cos θ − ml θ sinθ + b x = F

Mô hình toán học phi tuyến của hệ thống con lắc ngược được thể hiện qua hai
phương trình sau :

(

)

••
••
•

2
I

+
ml
θ
+
ml
x
cos
θ
−
mgl
sin
θ
+
d
θ
=0


•
••
••
•
( M + m) x + ml θ cos θ − ml θ 2 sinθ + b x = F



Đặt: M t = M + m
L=

I + ml 2

ml

•
−1 
2


ml cos θ
ml θ 2 sinθ cos θ cos θ
  g sinθ −
θ =  L −
−

M
M
Mt
t
t

 


••


ml cos 2 θ 

x =  M t −
L




••

−1


•

 d •
F
−
b
x
−
θ



 ml 


•
•

mgl sinθ cos θ d cos θ • 
 F − b x + ml θ 2 sinθ −
+
θ 
L

L



(3)

(4)


Mô phỏng trên MATLAB/Simulink:

Hình 1. Mô hình phi tuyến con lắc ngược

Hình 2. Mô hình phi tuyến con lắc ngược
với lực tác dụng F = 0 N


Hình 3. Đáp ứng góc theta của con lắc ngược khi lực F = 0 N


2. Mối quan hệ giữa điện áp động cơ và lực tác dụng lên xe
Hệ thống cơ khí được trình bày như hình vẽ

Trong đó:
-

V: điện áp đặt vào phần ứng động cơ (V)

-

Tm: moment đầu trục động cơ (N.m)

-

r: bán kính pulley (m)

Lực điều khiển F của hệ thống được truyền động bởi động cơ DC qua pulley. Tín
hiệu điều khiển động cơ là điện áp V có đơn vị là volt, trong khi đó lực điều khiển
F được đo bằng Newton. Từ đó ta phải suy ra mối quan hệ giữa lực điều khiển F và
điện áp V đặt vào phần ứng động cơ.


Giả sử bỏ qua điện cảm phần ứng động cơ, mô hình toán học của động cơ được
cho bởi phương trình sau:
V = R A I A + k mω m

(5)

trong đó V là điện áp đầu vào động cơ, R A là điện tở phần ứng động cơ, I A là dòng
điện chạy trong phần ứng động cơ, k m là hằng số moment của động cơ, và ω m là tốc
độ động cơ.
Phương trình cơ khí thể hiện mối liện hệ giữa moment động cơ và dòng điện phần
ứng của động cơ là :
Tm = k m k g I a

(6)

trong đó Tm là moment đầu trục động cơ, k g là tỷ số hộp giảm tốc của hộp giảm tốc
gắn vào đầu trục động cơ (nếu có).
Lực tác dụng vào xe do động cơ truyền động qua pulley được thể hiện qua biểu

thức :
F=

Tm
r

(7)

trong đó r là bán kính pulley.
Vận tốc góc đầu trục động cơ liên hệ với vận tốc dài của xe theo biểu thức :
•

x
ωm = k g
r

(9)

•

trong đó x là vận tốc dài của xe.
Từ (7) và (9) ta suy ra được :
IA =

Fr
km k g

(10)

Cuối cùng, thay thế (10) và (9) vào (5) ta thu được phương trình thể hiện mối quan

hệ giữa điện áp đầu vào V và lực tác dụng lên xe F :


F=

kmk g
Rr

V−

k m2 k g2
Rr 2

•

x

(11)

Thay (11) vào (3) và (4) ta được :

•
−1 

k m2 k g2

ml cos 2 θ  
ml θ 2 sinθ cos θ cos θ  k m k g




θ = L −
−
V − b+

  g sinθ −

Mt
Mt
M t  Rr
Rr 2

 


••

 •  d • 
 x − θ 
  ml

 

(12)



ml cos 2 θ 

x =  M t −

L



••

−1

•
 km k g
•

k m2 k g2  •

 x + ml θ 2 sinθ − mgl sinθ cos θ + d cos θ θ 
V − b+


L
L
Rr 2 
 Rr



(13)
Mô phỏng trên MATLAB/Simulink

Hình 4. Mô hình phi tuyến hệ thống động cơ – xe – con lắc ngược

Hìn
h 5. Mô hình phi tuyến hệ thống động cơ – xe – con lắc ngược
khi điện áp V = 0 V

Hình 6. Đáp ứng góc theta của con lắc khi điện áp


phần ứng động cơ V = 0 V

Ta nhận thấy khi con lắc ở vị trí ngược mà không có sự điều khiển thì con lắc sẽ rơi
và dao động ở vị trí cân bằng hướng xuống. Vấn đề đặt ra là cần phải thiết kế bộ
điều khiển để giữ cân bằng cho con lắc ở vị trí cân bằng không ổn định hướng lên.
Ta sử dụng phương pháp điều khiển LQR để thiết kế bộ điều khiển.
3. Phương pháp tuyến tính hóa dạng toàn phương LQR.
Hệ thống con lắc ngược là hệ thống có tính phi tuyến cao, tuy nhiên ở vị trí cân
bằng với góc lệch nhỏ, ta có thể tuyến tính hóa mô hình phi tuyến để được mô hình
tuyến tính, từ đó áp dụng bộ đều khiển LQR vào việc điều khiển cân bằng con lắc
ở vị trí thẳng đứng hướng lên.
Ở lân cận vị trí cân bằng không ổn định hướng lên thì θ ≈ 0 ; sinθ ≈ θ ; cos θ ≈ 1 ;
•

θ2 ≈ 0.

Các biểu thức (12), (13) lần lượt trở thành:
••

θ=


•

k2k2
g
d
1
b + m g
θ−
θ + 0x +
ml
Rr 2


ml 
ml  
L−


ml L −
M t  L −
Mt
M t 
M t 



•
km k g
1
 x−

V

Rr


ml


M t  L −
M t 


(14)
••

x=



•

k2k2
− mgl
d
1
b + m g
θ+
θ + 0x −
ml 
ml 

ml 
Rr 2



M
−
L M t −
L
M
−

 t

t
L
L 
L 



•
kmk g
1
 x+
V

ml Rr

Mt −

L

(15)
Ta suy ra được hệ phương trình biến trạng thái của hệ con lắc ngược :
=+
+v


Y=
Bỏ qua ma sát trục quay và ma sát mặt đường, sau khi thay số ta thu được :
=+V



Thiết kế bộ điều khiển LQR:



Giả thiết:

-

Đặc tính động học của học của hệ con lắc ngược đượ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ỏ (dưới
15  ).

-

Hệ thống phản hồi toàn bộ trạng thái, nghĩa là có thể đo được 4 biến trạng

-

thái (góc lệch θ , tốc độ góc θ , tọa độ xe x , tốc độ xe x )
Không có nhiễu tác động vào hệ thống.



Thiết kế dùng MATLAB:

•

- Chọn R = 1;
- Chọn ma trận Q =
- K = lqr(A,B,Q,R)
K=
Mô phỏng trên MATLAB/Simulink:

•


Hình 7. Bộ điều khiển LQR.

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


Hình 9. Đáp ứng góc lệch của con lắc và tọa độ của xe.

4. Giải thuật điều khiển swing-up con lắc ngược.
Bộ điều khiển swing-up có nhiệm vụ đưa con lắc từ vị trí cân bằng ổn định hướng
xuống lên vị trí cân bằng không ổn định hướng lên. Khi con lắc tiến đến vùng lân

cận hướng lên trên, bộ điều khiển cân bằng LQR sẽ ‘bắt’ con lắc để giữ cho con lắc
cân bằng.
Bộ điều khiển sử dụng được đề xuất bởi K.J.Astrom và K.Furuta trong “Swinging
up a pendulum by energy control”,1996.
Phương trình chuyển động của con lắc là:
••

J θ − mgl sinθ + mlu cos θ = 0

(16)
••

2
trong đó J = I + ml là momen quán tính của con lắc đối với trục quay, u = x là gia

tốc của xe.
Năng lượng của con lắc là:


1 •2
E = J θ + mgl (cos θ − 1)
2

(17)
•

Khi con lắc ở vị trí thẳng đứng hướng lên, θ = 0 và θ = 0 , thì E = 0 và khi con lắc ở
•

vị trí thẳng đứng hướng xuống dưới θ = π và θ = 0 thì E = −2mgl . Vậy để swing up

con lắc ta phải cung cấp cho con lắc một lượng năng lượng đúng bằng 2mgl .
Đạo hàm năng lượng của con lắc theo thời gian :
• ••
•
•
dE
= J θ θ − mgl θ sinθ = −mul θ cos θ
dt

(18)

Phương trình (16) cho thấy rằng dễ dàng điều khiển năng lượng của con lắc. Để
dE
•
>0
u
θ
dt
tăng năng lượng cho con lắc tức là
thì phải trái dấu với cos θ .

Hàm Lyapunov :

V =

( E − E0 ) 2
2

(19)

Đạo hàm hàm Lyapunov theo thời gian :
•
dV
= − mlk (( E − E 0 ) θ cos θ ) 2
dt

(20)

Theo K.J.Astrom và K.Furuta ta tìm được luật điều khiển :
•

•

u = ngsign (( E − E 0 ) θ cos θ ) = ksign (( E − E 0 ) θ cos θ )

(21)

Giải thuật điều khiển swing up con lắc được thể hiện qua biểu thức (21), tuy nhiên
theo phương pháp của K.J.Astrom và K.Furuta thì tọa độ của xe không được đề
cập đến. Điều này sẽ dẫn đến xe vượt ra ngoài đường ray.
Vì vậy một giải thuật mới được đưa ra dựa trên phương pháp điều khiển năng
lượng của K.J.Astrom và K.Furuta kết hợp với điều khiển tọa độ của xe. Giải thuật
này được giới thiệu trong bài báo ‘Swing-up Control of a Serial Double Inverted
Pendulum’ của T.Henmi, M.Deng và A.Inoue.


Nội dung giải thuật là kết hợp phương pháp điều khiển năng lượng được đưa ra ở
•

biểu thức (21) và phản hồi toàn bộ trạng thái của xe (tọa độ x và vận tốc x ).

Đầu vào điều khiển phản hồi u x để điều khiển xe vào trung tâm đường ray.
u x = − k1 x1

( k1 > 0)

(22)

T

 •
x1 =  x, x 
  và k1 là ma trận hệ số phản hồi điều khiển x1 về 0.
trong đó

Giải thuật điều khiển:
•

ksign (( E − E 0 ) θ cos θ + u x __________( sign (u ) = sign (u x )or | u |>| u x |)
ut = 
•
ksign (( E − E 0 ) θ cos θ ______________( sign (u ) ≠ sign (u x )and | u |≤| u x |)

Mô phỏng trên MATLAB/Simulink:

(23)


Hình 10. Năng lượng của con lắc phỏng theo biểu thức (17)

Hình 11. Luật điều khiển phỏng theo biểu thức (21)

Hình 12. Luật điều khiển phỏng theo biểu thức (22)


Hình 13. Luật điều khiển phỏng theo biểu thức (23)

Vì đầu ra của luật điều khiển là gia tốc a của xe, trong khi đó đầu vào của đối
tượng điều khiển là điện áp V nên ta phải tìm mối quan hệ giữa gia tốc a và điện áp
V.
F = Mta =
V=


kmkg
Rr

V−

k m2 k g2
Rr 2

km k g •
M t Rr
a+
x
km k g
r

Mô phỏng trên MATLAB/Simulink:

•

x

(24)


Hình 14. Mối quan hệ giữa a và V.

Kết hợp hình 10 và 11 ta được thuật toán swing up con lắc.

Hình 15. Bộ điều khiển Swing Up


Khi thực hiện kết hợp 2 bộ điều khiển: Bộ điều khiển cân bằng LQR và bộ điều
khiển Swing Up yêu cầu phải có 1 công tắc chuyển đổi giữa hai bộ điều khiển.
Ban đầu con lắc ở vị trí treo, bộ điều khiển Swing Up hoạt động, khi góc θ < 15
hay cos θ ≥ 0.96 thì công tắc chuyển qua bộ điều khiển LQR đón lấy con lắc và giữ
cân bằng cho con lắc.


Hình 16. Công tắc chuyển đổi giữa 2 bộ điều khiển.


Hình 17. Mô hình toàn bộ hệ thống


Hình 18. Đáp ứng góc lệch con lắc và tọa độ xe
trong toàn bộ qua trình.

Hình 19. Năng lượng của con lắc


trong toàn bộ quá trình