Tính toán, chế tạo và điều khiển mô hình con lắc ngược

i

LỜI CAM ĐOAN
Tên tôi là Phạm Công Dũng, học viên cao học khóa 2013B.CĐT.KH chuyên ngành
Cơ Điện Tử. Sau hơn 2 năm học tập, nghiên cứu tại trường Đại Học Bách Khoa Hà
Nội, được sự giúp đỡ của các thầy cô giáo, đặc biệt là GS.TSKH. Nguyễn Văn
Khang, tôi đã hoàn thành xong luận văn tốt nghiệp thạc sĩ.
Với đề tài luận văn là: "Tính toán, chế tạo và điều khiển mô hình con lắc
ngược", tôi xin cam đoan đây là công trình nghiên cứu của cá nhân tôi dưới sự
hướng dẫn của GS. TSKH. Nguyễn Văn Khang và chỉ tham khảo các tài liệu được
liệt kê. Tôi không sao chép công trình của cá nhân khác dưới bất kỳ hình thức nào.
Nếu có, tôi xin hoàn thành chịu trách nhiệm

Hà Nội, ngày 20/03/2016
Học viên

Phạm Công Dũng


ii

LỜI CẢM ƠN

Trong suốt quá trình thực hiện đề tài "Tính toán , chế tạo và điều khiển mô hình
con lắc ngược", em đã đạt được một số kết quả nhất định: Nghiên cứu các phương
pháp điều khiển con lắc ngược, các phương thức lập trình, điều khiển hoạt động của
hệ thống, cũng như hiểu thêm về cách thiết kế, chế tạo cơ khí. Em xin trân trọng
cảm ơn sự giúp đỡ của GS. TSKH. Nguyễn Văn Khang và các thầy trong bộ môn
Cơ học ứng dụng, Viện Cơ khí đã tạo điều kiện để em hoàn thành đề tài này. Tuy
nhiên, với kinh nghiệm thực tế còn hạn chế nên luận văn không thể tránh được
những thiết sót và chưa thể hoàn thiện một cách hoàn hảo như mong đợi. Kính

mong quý thầy cô đóng góp những ý kiến để đề tài được hoàn thành tốt hơn.

Hà Nội, ngày 20/03/2016
Học viên

Phạm Công Dũng


iii

MỤC LỤC
Trang
LỜI CAM ĐOAN............................................................................................................... i
LỜI CẢM ƠN ................................................................................................................... ii
HỆ THỐNG KÝ HIỆU, CHỮ VIẾT TẮT ......................................................................... v
HỆ THỐNG DANH MỤC BẢNG BIỂU........................................................................... v
HỆ THỐNG DANH MỤC HÌNH VẼ ............................................................................... vi
MỞ ĐẦU .......................................................................................................................... 1
CHƯƠNG 1 THIẾT LẬP PHƯƠNG TRÌNH VI PHÂN MÔ TẢ CHUYỂN ĐỘNG CON
LẮC NGƯỢC ................................................................................................................... 3
1.1 Thiết lập phương trình vi phân chuyển động ............................................................ 3
1.2 Biến đổi phương trình chuyển động về phương trình trạng thái ................................ 6
1.3 Vị trí cân bằng của hệ .............................................................................................. 8
CHƯƠNG 2 ĐIỀU KHIỂN CON LẮC NGƯỢC BẰNG PHƯƠNG PHÁP LQR .............. 9
2.1 Điều khiển ổn định của điểm cân bằng không ổn định ............................................. 9
2.2 Mô phỏng số ......................................................................................................... 13
CHƯƠNG 3 ĐIỀU KHIỂN CON LẮC NGƯỢC BẰNG PHƯƠNG PHÁP PID.............. 27
3.1 Thiết kế bộ điều khiển PID cho con lắc ngược ....................................................... 27
3.2 Mô phỏng số ......................................................................................................... 28
CHƯƠNG 4 CHẾ TẠO VÀ ĐIỀU KHIỂN THỬ NGHIỆM MÔ HÌNH CON LẮC

NGƯỢC BẰNG BỘ ĐIỀU KHIỂN PID ......................................................................... 37
4.1 Các vấn đề điều khiển trên mô hình thật ................................................................ 37
4.1.1 Các giai đoạn điều khiển ................................................................................ 38
4.1.2 Phương pháp đo đạc vị trí xe và góc lệch của thanh ........................................ 41
4.1.3 Phương pháp điều khiển khối dẫn động .......................................................... 47
4.2 Thiết kế cơ khí ...................................................................................................... 56
4.2.1 Các thành phần cơ khí .................................................................................... 56
4.2.2 Hình ảnh sau lắp ráp ....................................................................................... 59
4.3 Thiết kế mạch ........................................................................................................ 60
4.3.1 Mạch điều khiển ............................................................................................. 60


iv

4.3.2 Mạch công suất .............................................................................................. 64
4.4 Thiết kế phần mềm ................................................................................................ 67
4.4.1 Ngôn ngữ và trình biên dịch ........................................................................... 67
4.4.2 Kiến trúc và thiết kế ....................................................................................... 68
4.4.3 Thiết lập PWM điều khiển điện áp cho động cơ .............................................. 70
4.4.4 Mã giả cho thuật toán PID/PD ........................................................................ 72
4.5 Kết quả đạt được và các vấn đề gặp phải ............................................................... 74
KẾT LUẬN..................................................................................................................... 76
TÀI LIỆU THAM KHẢO ............................................................................................... 77


v

HỆ THỐNG KÝ HIỆU, CHỮ VIẾT TẮT
Vgs: Điện áp chân G và S của Mosfet.
PWM: Điều chế độ rộng xung.

GPIO: Chân vào ra đa chức năng.
AFIO: Chức năng đặc biệt của các chân vào ra.
BLDC: Động cơ một chiều không chổi than
DC: Động cơ một chiều
IC: Vi mạch tích hợp

HỆ THỐNG DANH MỤC BẢNG BIỂU
Trang
Bảng 4-1: Bảng so sánh các phương pháp đo vị trí xe và góc lệch thanh................ 47
Bảng 4-2: Bảng so sánh các loại động cơ ............................................................... 49
Bảng 4-3: Trạng thái động cơ theo các công tắc cầu H .......................................... 52
Bảng 4-4: So sánh các mạch công suất cho động cơ DC ........................................ 55


vi

HỆ THỐNG DANH MỤC HÌNH VẼ
Trang
Hình 1-1: Mô hình con lắc ngược ............................................................................ 3
Hình 2-1: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.4 .................... 14
Hình 2-2: Đồ thị vận tốc xe với bộ giá trị tại công thức 2.4.................................... 14
Hình 2-3: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.4 ........................ 14
Hình 2-4: Đồ thị vận tốc của thanh với bộ giá trị tại công thức 2.4 ........................ 15
Hình 2-5: Đồ thị lực điều khiển u(t) với bộ giá trị tại công thức 2.4 ....................... 15
Hình 2-6: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.5 .................... 16
Hình 2-7: Đồ thị gia tốc xe đẩy với bộ giá trị tại công thức 2.5 .............................. 16
Hình 2-8: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.5 ........................ 17
Hình 2-9: Đồ thị vận tốc góc của thanh với bộ giá trị tại công thức 2.5 .................. 17
Hình 2-10: Đồ thị lực điều khiển với bộ giá trị tại công thức 2.5 ........................... 17
Hình 2-11: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.6 .................. 19

Hình 2-12: Đồ thị gia tốc xe với bộ giá trị tại công thức 2.6 .................................. 19
Hình 2-13: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.6....................... 19
Hình 2-14: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.6 .................. 20
Hình 2-15: Đồ thị lực điều khiển với bộ giá trị tại công thức 2.6 ........................... 20
Hình 2-16: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.7 .................. 21
Hình 2-17 Đồ thị gia tốc xe với bộ giá trị tại công thức 2.7.................................... 21
Hình 2-18: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.7....................... 21
Hình 2-19: Đồ thị vận tốc góc của thanh với bộ giá trị tại công thức 2.7 ................ 22


vii

Hình 2-20: Đồ thị dịch lực điều khiển với bộ giá trị tại công thức 2.7 .................... 22
Hình 2-21: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.8 .................. 23
Hình 2-22: Đồ thị gia tốc xe với bộ giá trị tại công thức 2.8 .................................. 23
Hình 2-23: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.8....................... 23
Hình 2-24: Đồ thị vận tốc góc với bộ giá trị tại công thức 2.8 ................................ 24
Hình 2-25: Đồ thị lực điều khiển với bộ giá trị tại công thức 2.8 ........................... 24
Hình 2-26: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.9 .................. 25
Hình 2-27: Đồ thị gia tốc xe với bộ giá trị tại công thức 2.9 .................................. 25
Hình 2-28: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.9....................... 25
Hình 2-29: Đồ thị vận tốc góc của thanh với bộ giá trị tại công thức 2.9 ................ 26
Hình 2-30: Đồ thị lực điều khiển với bộ giá trị tại công thức 2.9 ........................... 26
Hình 3-1: Sơ đồ điều khiển PID bằng Simulink ..................................................... 28
Hình 3-2: Đồ thị dịch chuyển thân xe tại x0  0, x0  0, 0  0.1, 0  0 ..................... 29
Hình 3-3: Đồ thị vận tốc xe tại x0  0, x0  0, 0  0.1, 0  0 .................................... 29
Hình 3-4: Đồ thị góc lắc của thanh tại x0  0, x0  0, 0  0.1, 0  0 ......................... 30
Hình 3-5: Đồ thị vận tốc thanh tại x0  0, x0  0, 0  0.1, 0  0 ............................... 30
Hình 3-6: Đồ thị lực điều khiển tại x0  0, x0  0, 0  0.1, 0  0 .............................. 31
Hình 3-7: Đồ thị dịch chuyển thân xe tại x0  0, x0  0, 0  1, 0  0 ........................ 31

Hình 3-8: Đồ thị vận tốc xe tại x0  0, x0  0, 0  1, 0  0 ....................................... 32
Hình 3-9: Đồ thị góc lắc của thanh tại x0  0, x0  0, 0  1, 0  0 ............................ 32
Hình 3-10: Đồ thị vận tốc của thanh tại x0  0, x0  0, 0  1, 0  0 .......................... 33


viii

Hình 3-11: Đồ thị lực điều khiển tại x0  0, x0  0, 0  1, 0  0 ............................... 33
Hình 3-12: Đồ thị dịch chuyển thân xe tại x0  0, x0  0, 0   , 0  0 ..................... 34
Hình 3-13: Đồ thị vận tốc xe tại x0  0, x0  0, 0   , 0  0 .................................... 34
Hình 3-14: Đồ thị góc lắc của thanh tại x0  0, x0  0, 0   , 0  0 ......................... 35
Hình 3-15: Đồ thị vận tốc thanh tại x0  0, x0  0, 0   , 0  0 ............................... 35
Hình 3-16: Đồ thị lực điều khiển tại x0  0, x0  0, 0   , 0  0 ............................. 36
Hình 4-1: Mô hình cơ khí con lắc ngược ............................................................... 37
Hình 4-2: Vị trí của thanh khi thực hiện các giai đoạn điều khiển .......................... 38
Hình 4-3: Các giai đoạn điều khiển........................................................................ 38
Hình 4-4: Bộ điều khiển lật ngược ......................................................................... 39
Hình 4-5: Mô hình kết hợp 2 bộ điều khiển PID .................................................... 40
Hình 4-6: Nguyên lý đo khoảng cách dùng cảm biến siêu âm ................................ 43
Hình 4-7: Nguyên lý một bộ thu phát điển hình ..................................................... 44
Hình 4-8: Nguyên lý hoạt động thước encoder thẳng ............................................. 44
Hình 4-9: Bộ mã hóa encoder tương đối và nguyên lý hoạt động ........................... 45
Hình 4-10: Giản đồ xung của encoder tương đối.................................................... 46
Hình 4-11: Sơ đồ quan hệ giữa xung kích MOSFET và đầu ra............................... 50
Hình 4-12: Mạch cầu H tổng quát .......................................................................... 51
Hình 4-13: Nguyên lí đảo chiều động cơ của cầu H ............................................... 51
Hình 4-14: MOSFET kênh N................................................................................. 53
Hình 4-15: Ví dụ dùng MOSFET điều khiển động cơ ............................................ 53
Hình 4-16: Mạch cầu H dùng MOSFET ................................................................ 54

ix

Hình 4-17: Mạch điều khiển động cơ dùng MOSFET và Rờle ............................... 54
Hình 4-18: Thiết kế cơ khí 3D của hệ thống .......................................................... 58
Hình 4-19: Hệ thống lắp ráp hoàn thiện ................................................................. 59
Hình 4-20: Thanh ở vị trí tự do .............................................................................. 60
Hình 4-21: Thiết kế vi điều khiển trung tâm .......................................................... 61
Hình 4-22: Thiết kế cổng kết nối máy tính ............................................................. 61
Hình 4-23: Thiết kế đầu vào bộ chuyển đổi điện áp cho encoder ........................... 62
Hình 4-24: Thiết kế nguồn cho mạch điều khiển.................................................... 62
Hình 4-25: Thiết kế PCB của mạch điều khiển ...................................................... 63
Hình 4-26: Thiết kế mạch cầu H cho động cơ ........................................................ 64
Hình 4-27: Thiết kế PCB mạch công suất .............................................................. 66
Hình 4-28: Hình ảnh mạch hoàn thiện ................................................................... 66
Hình 4-29: Trình biên dịch và mạch debug cho STM32F103................................. 67
Hình 4-30: Kiến trúc phần mềm ............................................................................ 68
Hình 4-31: Lưu đồ thuật toán chương trình điều khiển .......................................... 69
Hình 4-32: Thiết lập chế độ PWM ......................................................................... 71
Hình 4-33: Đáp ứng vị trí của xe (mm) theo thời gian (s) ...................................... 74
Hình 4-34: Đồ thị đáp ứng góc lệch của thanh (rad) theo thời gian (s) ................... 74


1

MỞ ĐẦU
I - ĐẶT VẤN ĐỀ
Động lực học và điều khiển dựa trên mô hình là một lĩnh vực đang được quan tâm
nghiên cứu của chuyên ngành cơ điện tử và động lực học. Bài toán điều khiển con
lắc ngược là một bài toán điển hình của động lực học và điều khiển đã và đang được

tính toán, thiết kế.
II - MỤC ĐÍCH CỦA LUẬN VĂN
Trong luận văn này thiết lập phương trình vi phân chuyển động của mô hình con lắc
ngược. Sau đó thiết kế bộ điều khiển PID để ổn định vị trí cân bằng không ổn định
của con lắc ngược. Qua đó xây dựng một mô hình minh họa lý thuyết cho sinh viên
ngành cơ điện tử học tập và nghiên cứu.
III - Ý NGHĨA KHOA HỌC VÀ Ý NGHĨA THỰC TIỄN CỦA LUẬN VĂN
Con lắc ngược là một thí dụ sinh động về một hệ cơ học mà có thể chuyển vị trí cân
bằng không ổn định thành vị trí cân bằng ổn định nhờ lực điều khiển. Con lắc
ngược là một mô hình đơn giản hóa của mô hình tên lửa khi bắt đầu được phóng
vào không gian. Nghiên cứu ổn định hóa con lắc ngược là một bài toán vừa có ý
nghĩa khoa học vừa có ý nghĩa thực tế.
IV - NỘI DUNG CỦA LUẬN VĂN
Luận văn này gồm 4 chương với các nội dung như sau:
Chương 1: Thiết lập phương trình vi phân mô tả chuyển động con lắc ngược. Nội
dung của chương này tập trung vào việc thiết lập phương trình vi phân cho con lắc
ngược.
Chương 2: Điều khiển con lắc ngược bằng bộ điều khiển LQR. Ở chương này áp
dụng lý phương pháp điều khiển LQR, điều khiển mô hình con lắc ngược.


2

Chương 3: Điều khiển con lắc ngược bằng bộ điều khiển PID. Ở chương này áp
dụng phương pháp điều khiển PID vào điều khiển mô hình con lắc ngược.
Chương 4: Chế tạo và điều khiển thử nghiệm mô hình con lắc ngược bằng bộ điều
khiển PID. Áp dụng phương pháp điều khiển PID và lý thuyết truyền động điện vào
điều khiển mô hình con lắc ngược chế tạo ở phòng thí nghiệm cơ điện tử, Trường
đại học Kinh doanh và Công nghệ Hà Nội.

3

CHƯƠNG 1
THIẾT LẬP PHƯƠNG TRÌNH VI PHÂN MÔ TẢ CHUYỂN ĐỘNG CON
LẮC NGƯỢC
Việc xây dựng mô hình cơ học là điều kiện cần thiết để điều khiển dựa trên mô
hình. Trong chương này trình bày việc thiết lập phương trình vi phân chuyển động
của mô hình con lắc ngược.
1.1 Thiết lập phương trình vi phân chuyển động

Khi gắn con lắc trên vào một xe đẩy với lực điều khiển theo phương ngang, ta tìm
điều kiện của lực điều khiển để vị trí cân bằng tại gốc tọa độ từ không ổn định trở
nên ổn định với điều kiện thêm lực vào là tối ưu về năng lượng điều khiển.

u(t)

Hình 1-1: Mô hình con lắc ngược

Hệ xe cần trục có dạng như trên Hình 1-1


4

Các thông số của mô hình gồm:

m0 : Khối lượng của xe
m : Khối lượng của thanh

l : Chiều dài của thanh

I : Mô men quán tính đối với tâm của thanh
 : Là góc hợp bởi thanh với mặt phẳng thắng đứng
u (t ) : Lực điều khiển
lc : là khoảng cách từ O đến trọng tâm C của thanh

Tính động năng của hệ

T  T1  T2

(1.1)

Trong đó

T1 là động năng của xe
T1 

1
m0 x 2
2

(1.2)

T2 là động năng của thanh
T2 

1 2 1 2
mvc  I 
2
2

Ở đây ta có

 xc  x  lc sin   xc  x  lc cos 


y

l
cos

 c c
 y c  lc sin 
 vc2  xc2  y c2   2lc2  x 2  2 xlc cos 
Do đó

(1.3)


5

1
1
m  2lc2  x 2  2 x lc cos    I 2
2
2
1
1
 mx 2   mlc2  I   2  mlc x cos 
2

2

T2 

Từ đó ta có động năng của hệ

T  T1  T2


1
1
 m0  m  x 2   mlc2  I  2  mlc x cos
2
2

(1.4)

Thế năng của hệ

  mglc cos 
Tính các đạo hàm

T
  m0  m  x  mlc cos 
x
T
  mlc2  I    mlc x cos 


d  T 

x  mlc cos   mlc 2 sin 
     m0  m  
dt  x 
T
  mlc2  I   mlc 
x cos   mlc x sin 


T
 0;
x

T
 mlc x sin 



 0;
x


 mglc sin 


Qx*  u (t )
Q*1  0

(1.5)

6

Từ đó thay các đạo hàm tính được trên vào phương trình Lagrange loại II
d  T

dt  qi

 T
 


 Qi*



q

q

q
i
1
1


Ta có phương trình vi phân chuyển động của hệ

 m0  m x  mlc cos  u(t )  mlc 2 sin 
mlc cos 
x   mlc2  I    mlc gsin 

1.2 Biến đổi phương trình chuyển động về phương trình trạng thái

Đầu tiên ta xét hệ phương trình (1.6)

 m0  m x  mlc cos  u(t )  mlc 2 sin 
mlc cos 
x   mlc2  I    mlc gsin 
Giải hệ này ta có

mlc cos 
  m0  m   mlc2  I   m 2lc2 cos 2 
2
mlc  I



m0  m
mlc cos 

1 

u (t )  mlc 2 sin 
mlc g sin 

mlc cos 
mlc2  I

  u (t )  mlc 2 sin   mlc2  I   m 2lc2 gsin  cos 
2 

m0  m

u (t )  mlc 2 sin 

mlc cos 

mlc g sin 

  m0  m  mlc g sin   mlc cos   u (t )  mlc 2 sin  
Từ đó tính được:

(1.6)


7

2
2
2 2
1  u (t )  mlc sin   mlc  I   m lc gsin  cos 

x


 m0  m   mlc2  I   m2lc2 cos2 
2
 2  m0  m  mlc g sin   mlc cos   u (t )  mlc sin  
 



 m0  m   mlc2  I   m2lc2 cos2 

Đưa về phương trình trạng thái ta đặt các biến như sau x1  x, x2  x , x3   , x4  

x2 

 u (t )  ml x

2
c 4

sinx 3  mlc2  I   m 2lc2 gsinx 3 cosx 3

 m0  m   mlc2  I   m2lc2 cos2 x 3
 m0  m  mlc g sinx 3  mlc cosx 3  u(t )  mlc x42 sinx 3 
x4 
 m0  m   mlc2  I   m2lc2 cos2 x 3
Mặt khác ta lại có

x1  x2 , x3  x4
Từ đó ta có phương trình trạng thái của hệ

x1  x2
x2

 u (t )  ml x


2

c 4

sinx 3  mlc2  I   m 2lc2 gsinx 3 cosx 3

 m0  m   mlc2  I   m2lc2 cos2 x 3

x3  x4

 m0  m  mlc g sinx 3  mlc cosx 3  u (t )  mlc x42 sinx 3 
x4 
 m0  m   mlc2  I   m 2lc2 cos 2 x 3

Hay viết dưới dạng
x  f ( x, u )

Trong đó :

(1.7)


8

x =[x1 ,x 2 ,x 3 ,x 4 ]T
 x2



2
2
2 2

  u (t )  mlc x4 sinx 3  mlc  I   m lc gsinx 3 cosx 3 


 m0  m   mlc2  I   m2lc2 cos 2 x 3


f 

x
 4

  m0  m  mlc g sinx 3  mlc cosx 3  u (t )  mlc x42 sinx 3  


 m0  m   mlc2  I   m2lc2 cos 2 x 3



1.3 Vị trí cân bằng của hệ

Điểm cân bằng của hệ được xác định từ hệ phương trình
x  f ( x, 0)  0

Hay

x2  0

 u (t )  ml x

2

c 4

sin x3  mlc2  I   m 2lc2 g sin x3 cos x3

 m0  m   mlc2  I   m2lc2 cos2 x3

0

x4  0

 m0  m  mlc g sin x3  mlc cos x3  u (t )  mlc x42 sin x3 
0
 m0  m   mlc2  I   m 2lc2 cos 2 x3
Từ đó ta dễ dàng tính được
 x2  0
sin x cos x  0

3
3

 x4  0
sin x3  0

Hệ trên có hai tập nghiệm là { x1 tùy ý; x2  0; x3  0; x4  0 } và
{ x1 tùy ý; x2  0; x3   ; x4  0 }


9

Từ đó ta có hai tập điểm cân bằng cua hệ là:

 x, x,1,1   *,0,0,0 và  x, x,1 ,1   *,0,  ,0

CHƯƠNG 2
ĐIỀU KHIỂN CON LẮC NGƯỢC BẰNG PHƯƠNG PHÁP LQR
Phương pháp điều khiển LQR ( linear-quadratic regulator ) là một áp dụng của
nguyên lý cực đại Pontryagin để điều khiển tối ưu các hệ tuyến tính [9,11]. Trong
phần này sử dụng các kết quả đã trình bày trong [11] để điều khiển vị trí cân bằng
thẳng đứng của con lắc ngược.
2.1 Điều khiển ổn định của điểm cân bằng không ổn định

 x, x,1,1   *,0,0,0
Ta nhận thấy rằng điểm cân bằng  x, x,1 ,1   *,0,0,0  là không ổn định.
Thực vậy để xem xét tính không ổn định của điểm cân bằng  x, x,1 ,1   *,0,0,0 
ta làm như sau:
Thực hiện tuyến tính hóa hệ quanh điểm cân bằng về dạng x = Ax + Bu trong đó

A

f
x 0,0


10

0

0

A

0

0



B

1

0

0

 glc2 m 2
 m0  m   mlc2  I   lc2 m 2

0

0

0

f
u 0,0

glc m  m0  m 

 m0  m   mlc2  I   lc2 m 2

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

0




mlc2  I


  m0  m   mlc2  I   m 2lc2 


0




lc m



  m0  m   mlc2  I   m2lc2 



0
0

A

Tổng quát như sau
0

0

1 0 0
0 a 0 

0 0 1

0 b 0

glc m  m0  m 
 glc2 m2

0,
b

 0.
Trong đó a 
 m0  m   mlc2  I   lc2 m2

 m0  m   mlc2  I   lc2 m2
Để nghiên cứu tính ổn định ta tính các trị riêng của ma trận A , ta có:

 s 1 0 0 
s a 0
0 s a 0 
  s 0 s 1  s 2 s 1  s 2 ( s 2  b)
det( sI  A)  det 
0 0 s 1
b s
0 b s


0 0 b s 
s  0

Do đó det( sI  A )  0  s 2 ( s 2  b)  0   s  b . Ta thấy rằng ma trận A có trị

s   b

riêng bên phải trục ảo, do đó điểm cân bằng là không ổn định.


11

Xét tính điều khiển được của hệ quanh vị trí cân bằng không ổn định

 x, x,1 ,1   *,0, 0, 0
Xét det(B | AB | A 2B | A3B)
0


0

Với A  
0

0



1

0

0

 glc2 m 2
 m0  m   mlc2  I   lc2 m 2

0

0
glc m  m0  m 

0

 m0  m   mlc2  I   lc2 m 2

0


0


1

0



0




mlc2  I


  m0  m   mlc2  I   m2lc2 

B
0




lc m


  m0  m   mlc2  I   m 2lc2 



Bằng sự trợ giúp của matlab ta tính được
2

g 2lc4 m 4

3

det(B | AB | A B | A B) 

 m ml
0

2
c

 Im  Im0 

4

0

Do vậy ta có: rank ( B | AB | A 2 B | A 3B )  4 . Áp dụng định lý Kalman1 ở chương 1
của [11] ta có hệ là điều khiển được trong lân cận điểm cân bằng.
Với mô hình con lắc ngược gắn với xe như ở Mục 1.1. Vị trí cân bằng thẳng đứng
không ổn định. Bài toán đặt ra, ta đặt thêm vào một lực nằm ngang u(t). Thông qua
hàm u(t) vị trí cân bằng thẳng đứng của hệ không ổn định trở nên ổn định. Hàm u(t)
được chọn sao năng lượng tiêu hao là cực tiểu trong quá trình điều khiển con lắc
ngược .

12

Xét hệ thống có mô hình trạng thái

x  Ax  Bu
Với
0

0

A
0

0



1

0

0

 glc2 m 2
 m0  m   mlc2  I   lc2 m 2

0

0

0

glc m  m0  m 

 m0  m   mlc2  I   lc2 m 2

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

0




mlc2  I


  m0  m   mlc2  I   m2lc2 

B
0





lc m


  m0  m   mlc2  I   m 2lc2 


Khi đó với yêu cầu là giảm chi phí về hao tổn năng lượng trong quá trình ổn định hệ
điều khiển trên ta chọn phiếm hàm mục tiêu xác định bởi công thức


J ( x, u ) 

1
( xT Qx  u T Ru ) dt  min
2 0

Trong đó:

(2.1)

Q  QT , xT Qx  0, x
R  RT , xT Rx  0, x  0

Ta chọn các ma trận Q,R
Từ đó nhờ phương trình Ricati

  KB  R -1  BTK  + KA + ATK = Q
Giải ra ta tìm được ma trận K

(2.2)


13

Từ đó thông qua ma trận K ta tìm được luật điều khiển
u  t   R 1BTKx(t )

(2.3)

2.2 Mô phỏng số

Với bộ số liệu g  10m / s 2 ; m0  10kg; m  1kg ; lc  0.3m; I  0.06kgm2
Thực hiện mô phỏng số bằng phần mềm Matlab 2012a ta tính được ma trận K nhờ
lệnh lqr sau đó tính được luật điều khiển u  t   R 1BTKx(t ) , từ đó thay vào
phương trình vi phân chuyển động của hệ cho ta các kết quả sau
1
0
Q
0

0

0
1
0
0

0
0
1
0

0
0 
;R 1
0

1

 5.2 13.2 52.7 11.8 
13.2 63.7 264.0 59.1 
;
K
52.7 264 6361 1389 


11.8 59.1 1389 303.7 

1  4.69
  4.51
 2
A  BR -1B TK   E  0  
3  0.22  0.21i
4  0.22  0.21i

(2.4)

14

Hình 2-1: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.4

Hình 2-2: Đồ thị vận tốc xe với bộ giá trị tại công thức 2.4

Hình 2-3: Đồ thị góc lắc của thanh với bộ giá trị tại công thức 2.4


15

Hình 2-4: Đồ thị vận tốc của thanh với bộ giá trị tại công thức 2.4

Hình 2-5: Đồ thị lực điều khiển u(t) với bộ giá trị tại công thức 2.4

Với bộ giá trị mới:
0
0 
100 0
 0 100 0
0 

Q
; R  10
 0
0 100 0 



0
0 100
 0

405 
 326 481 1812
 481 1385 5500 1228 
;
K
1812 5500 74849 16375 


 405 1228 16375 3595 

(2.5)


16

1  4.91
  4.31
 2
-1 T
A  BR B K   E  0  
3  0.41  0.35i
4  0.41  0.35i

Hình 2-6: Đồ thị dịch chuyển thân xe với bộ giá trị tại công thức 2.5

Hình 2-7: Đồ thị gia tốc xe đẩy với bộ giá trị tại công thức 2.5