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-17 Đồ thị gia tốc xe 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-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-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 3-1: Sơ đồ điều khiển PID bằng Simulink ..................................................... 28
0 
Hình 3-2: Đồ thị dịch chuyển thân xe tại x0 0, x0 0, 0 0.1,  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 ......................... 30
0 
Hình 3-5: Đồ thị vận tốc thanh tại x0 0, x0 0, 0 0.1,  0 ............................... 30
0 
Hình 3-6: Đồ thị lực điều khiển tại x0 0, x0 0, 0 0.1,  0 .............................. 31
0 
Hình 3-7: Đồ thị dịch chuyển thân xe tại x0 0, x0 0, 0  0 ........................ 31
0 
1, 
0 
Hình 3-8: Đồ thị vận tốc xe tại x0 0, x0 0, 0 1,  0 ....................................... 32
Hình 3-9: Đồ thị góc lắc của thanh tại x0 0, x0 0, 0  0 
1,  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 ............................... 33
0 
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
1
T1  m 0 x2 (1.2)
2
T2 là động năng của thanh
1 1
T2  mv c2  I 2 (1.3)
2 2
Ở đây ta có
xc  x lc sin  x x  lc cos 

  c
yc lc cos  yc  lc sin 

 vc2  xc2 y c
2
2lc2 
 x22x l c cos 

Do đó
5
1 1 2
T2  m 2l c2 x2 2x  c cos   I 
l
2 2
1 1
 mx2  mlc2 I  2  mlc x 

cos 
2 2
Từ đó ta có động năng của hệ
T T1 T2
1 1 (1.4)
 x2  mlc2 
m0 m  
2 ml
I  
c x cos 
2 2
Thế năng của hệ
  mglc cos  (1.5)
Tính các đạo hàm
T
m0 m  x mlccos 
x
T
ml2c I  mlc xcos 

d T 
  m0 m  xmlc cos  
 mlc 2 
sin 
dt x 
T
ml2c I 
mlc 
x cos  
mlc x 
  
sin

T 
T
0;  sin 
mlc x
x 

 
0; mgl c sin 

x 

Q *x u (t )
Q*1  0
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  T   *
 
   Qi
dt qi  qi q1 1
q
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 
(1.6)
mlc2 I 
mlc cosx  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 
mlc2 I 
mlc cosx  mlc gsin 

Giải hệ này ta có
m0 m mlc cos 

mlc cos  mlc2  I
m0  mlc2
m  I m2lc2 cos 2 
u( t) mlc2 sin  mlc cos 

1 
mlc gsin mlc2 I
u( t) mlc 
2 sin  
mlc2 I 2lc2 gsin cos 

m
m m u(t) mlc 2 sin 
2  0
mlc cos mlc g sin 
 c gsin  mlc cos u (t ) 
m0 m ml mlc 
2 sin  
Từ đó tính được:
7
1 u( t) mlc sin    lc gsin cos 

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

2 m0 m ml 2 sin 

  
 m0 m  mlc2 I m2lc2 cos2 
, x4
Đưa về phương trình trạng thái ta đặt các biến như sau x1 x, x2 x, x 3  


x 
u (t ) ml x 2
c 4 sinx 3 
mlc2 I 2 lc2 gsinx 3 cosx 3

m
2
m0 m  mlc2 I m 2lc2 cos 2 x 3
 c gsinx 3 mlc cosx3 u(t ) mlc x42 sinx3
m0 m ml 
x
4 
m0 m  mlc2 I m2 lc2 cos 2 x 3
Mặt khác ta lại có
x 
1  x 2 , x 3 x 4
Từ đó ta có phương trình trạng thái của hệ
x
1 x2
x 
u (t ) ml x 2
c 4 sinx 3 
mlc2 I 2 lc2 gsinx 3 cosx 3

m
2
m0 m 
mlc2 I m l
2 2
c cos2 x 3
(1.7)
x
3 x4
mlc gsinx 3 mlc cosx 3 u (t ) 

m0 m  mlc x42 sinx 3 
x 
4
m0 m  mlc2 I m2 lc2 cos2 x3
Hay viết dưới dạng
xf ( x, u )
Trong đó :
8
x=[x 1,x 2,x 3,x 4 ] T

x2

u (t ) mlc x4 sinx3  2 lc2 gsinx3 cosx3
2
mlc2 I  m

 m0 m  mlc2 I m 2lc2 cos 2 x 3
f 
x
4
m0 m ml  c gsinx 3 mlc cosx 3 u (t ) ml c x 24 sinx 3 


 m0 m  mlc2 I m 2lc2 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 
ml c2 I 2l c2 gsin x3 cos x3

m
0
m0 m 
mlc2 I m l
2 2
c cos 2 x3
x 4 0
mlc gsin x3 mlc cos x3 u (t ) mlc x42 sin x3
m0 m  0
m0 m  mlc2 I m 2lc2 cos2 x3
Từ đó ta dễ dàng tính được
x2 0

sin x3 cos x3 0

x4 0

sin x3 0
Hệ trên có hai tập nghiệm là { x1 tùy ý; x2 0; x 3 0; x4 

0 } và
{ x1 tùy ý; x 2  0; x 3 ; x 4 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 đó
f
A
x 0 ,0
10
0 1 0 0

0 gl 2c m2
0 0
 mlc2 I
m0 m  l m
2
c
2
A 
0 0 0 1

0 gl cm m0 m 
0 0

 mlc2
m0 m  I l m
2
c
2
 0 
 
 mlc2 I 
B
f


m0 m mlc2 I m2 lc2 

u 0,0  0 
 l cm 
 
 mlc2 I m2l c2
 m0 m  

0 1 0 0
0 0 a 0
Tổng quát như sau A  
0 0 0 1

0 0 b 0
glc2 m2 glc mm0 m 

Trong đó a  0, b  0.

mlc2 I
m0 m  l m2
c
2
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 
0 s a 0  s a 0
s 1
det( sI A) det  s 0 s 1 s2 2
s ( s 2 b)
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 có trị
 A
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 | A2 B | A3 B)
0 1 0 0 
 2 2 
0 gl m
0 c
0 
 m0 m 
ml I c
2
l m
2
c
2

Với A  
0 0 0 1 
 
0 gl cm m0 m 
0 0 

 m0 m 
ml 2
c I l m
2
c
2 

 0 
 
 ml c2 I 
c  c 
 m 0 m ml 2
I 
m 2 2 
l
B 
 0 
 l cm 
 
 m0 m 
  c  c 
ml 2
 I m 2 2
l
Bằng sự trợ giúp của matlab ta tính được
g 2lc4m 4
det(B | AB | A 2B | A3B )  0
m0mlc2 Im Im0 
4
Do vậy ta có: rank (B | AB | A 2 B | A 3 B )  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 1 0 0 
 2 2 
0 gl m
0 c
0 
 m0 m 
ml I
c
2
l m
2
c
2

A  
0 0 0 1 
 
0 glc m m0 m 
0 0 

 m0 m 
ml 2
c I l m
2
c
2 

 0 
 2 
 mlc I 

m 0 m ml c2 I 
m 2l c2 
B  
 0 
 
 lc m 
 m0 m ml 2
I m
 2 2
l 
 c c 
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

1
J ( x, u) 
20( xT Qx uT Ru) dt min
(2.1)
Q Q T , x TQx 0
, x
Trong đó:
R R T , x TRx 0 
, x 0
Ta chọn các ma trận Q,R
Từ đó nhờ phương trình Ricati
 KB R-1 BT K + KA + ATK = Q
    (2.2)
Giải ra ta tìm được ma trận K

13
Từ đó thông qua ma trận K ta tìm được luật điều khiển
u t 
R 1B TKx (t )
(2.3)
2.2 Mô phỏng số
Với bộ số liệu g 10m / s 2 ;m 0 10kg ;m 

1kg ;l c 
0.3m ; I  kgm 2
0.06
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 0 0 
 
0 1 0 0
Q  
; R 1
0 0 1 0 
 
0 0 0 1 
5.2 13.2 52.7 11.8 

 
13.2 63.7 264.0 59.1 
K ; (2.4)
52.7 264 6361 1389 
 
11.8 59.1 1389 303.7 
1  4.69

-1 T  2  4.51
A BR B K E 
0 
3  0.22 
0.21i

4  0.22 
0.21i
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:
100 0 0 0 
0 100 0 0 
Q  
; R 10
0 0 100 0 
 
0 0 0 100 
326 481 1812 405 

481 1385 5500 1228 
K  
; (2.5)
1812 5500 74849 16375 
 
405 1228 16375 3595 
16
1  4.91
  
-1 T 2 4.31
A BR B K E 
0 
3  0.41 0.35 i
4  0.41 0.35i

Hình 2-7: Đồ thị gia tốc xe đẩy 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
Hình 2-10: Đồ thị lực điều khiển với bộ giá trị tại công thức 2.5
18
Với bộ giá trị mới:
100 0 0 0 
 
0 50 0 0 
Q  ; R 10
0 0 100 0 
 
0 0 0 50 
317 477 1802 403 

477 1330 5305 1186 
K  
; (2.6)
1802 5305 74035 16211 
 
403 1186 16211 3556 
1  4.81
  
 4.40
0  2
A BR-1 BT K E 
3  0.39 0.37i

4  0.39 0.37i
19
Hình 2-12: Đồ thị gia tốc xe với bộ giá trị tại công thức 2.6
20
Với bộ giá trị:
50 0 0 0 
 
0 100 0 0
Q  
; R 10
0 0 50 0 
 
0 0 0 100 
194 326 1250 279 

 
326 1139 4569 1020 
K ; (2.7)
1250 4569 71298 15584 
 
279 1020 15584 3418 
21
1  4.91
  
-1 T 2 4.31
A BR B K E 0 
3  0.50 
0.28i
4  0.50 
0.28i

Hình 2-17 Đồ thị gia tốc xe với bộ giá trị tại công thức 2.7
22
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
Với bộ giá trị:
50 0 0 0 
0 100 0 0 
Q  
;R 200
0 0 50 0 
 
0 0 0 100 
400 1300 5100 1100 

1300 8500 35800 8000 
K  
; (2.8)
5100 35800 1203400 262600 
 
1100 8000 262600 57300 
1  4.68
  
2 4.53
A BR B K E 0 
-1 T
3  0.15 
0.15i
4  0.15 
0.15i

23
24
Hình 2-24: Đồ thị vận tốc góc với bộ giá trị tại công thức 2.8
50 0 0 0 
0 100 0 0 
Q  
; R 1
0 0 50 0 
 
0 0 0 100 
135 133 469 103 

133 313 1160 256 
K  
; (2.9)
469 1160 9852 2129 
 
103 256 2129 471 
1  5.67
  
2 3.73
A BR-1 BT K E 0 
3  0.73 
0.34i

4  0.73 
0.34i
25
26
27
CHƯƠNG 3
ĐIỀU KHIỂN CON LẮC NGƯỢC BẰNG PHƯƠNG PHÁP PID
Trong chương này trình bày việc điều khiển vị trí cân bằng thẳng đứng của con lắc ngược
bằng luật điều khiển PID. Cơ sở lý thuyết về điều khiển PD và PID đã được trình bày
tương đối kỹ trong tài liệu [10]. Ở đây ta áp dụng lý thuyết, thiết kế bộ điều khiển vị trí cân
bằng của con lắc ngược.
3.1 Thiết kế bộ điều khiển PID cho con lắc ngược
Ở đây ta sẽ sử dụng 2 bộ điều khiển PID:
Bộ điều khiển PID1: Dùng cho việc ổn định vị trí của xe.
Bộ điều khiển PID2: Dùng cho việc ổn định của con lắc so với phương thẳng đứng.
Ký hiệu:
e1 t 
x xd với x d là giá trị đặt trước vị trí của xe
e2 t 
 d với d là giá trị đặt trước góc con lắc so với phương thẳng đứng
Chọn luật điều khiển PID1 như sau:


d
u1 K P1 e1 t K I1 
e1 d 
 K D1 e1 t 
dt
0 (3.1)
Chọn luật điều khiển PID2 như sau:


d
u 2 K P2 e 2 t  e 2 d 
K I2  K D2 e t 
dt 2
0 (3.2)
Tín hiệu điểu khiển u u 1 u 2 (3.3)
Bằng Matlab, thiết lập sơ đồ như sau:

28
Hình 3-1: Sơ đồ điều khiển PID bằng Simulink
3.2 Mô phỏng số
Ta có góc đặt d 0 , vị trí đặt xd 0.2 (m)
Các hệ số PID của hai bộ điều khiển được chọn như sau:
K P1 1; KI1 0.1; K D1 

1; KP2 40; KI 2
 1;KD2 8;
Trường hợp 1: Các giá trị đầu được chọn như sau
x0 0, x
0 0, 0 

0.1, 
0 
0
29
Hình 3-2: Đồ thị dịch chuyển thân xe tại x0 0, x0 0, 0 0.1, 
0 
0
Hình 3-3: Đồ thị vận tốc xe tại x0 0, x0 0, 0 0.1, 
0 
0
30
0 
Hình 3-4: Đồ thị góc lắc của thanh tại x0 0, x0 0, 0 0.1,  0
Hình 3-5: Đồ thị vận tốc thanh tại x0 0, x0 0, 0 0.1, 
0 
0
31
Hình 3-6: Đồ thị lực điều khiển tại x 0 0, x0 0, 0 0.1, 
0 
0
x0 0, x
0 0, 0  0 
1,  0
Hình 3-7: Đồ thị dịch chuyển thân xe tại x0 0, x0 0,0  
1, 0 
0
32
Hình 3-8: Đồ thị vận tốc xe tại x 0 0, x0 0, 0  0 

1,  0
Hình 3-9: Đồ thị góc lắc của thanh tại x0 0, x0 0, 0 
1, 
0 
0
33
Hình 3-10: Đồ thị vận tốc của thanh tại x0 0, x0 0, 0  0 
1,  0
Hình 3-11: Đồ thị lực điều khiển tại x0 0, x0 0, 0  
1, 
0 
0
34
0 0, 0 
x0 0, x , 

0 
0
0 0, 0 
Hình 3-12: Đồ thị dịch chuyển thân xe tại x0 0, x , 

0 
0
Hình 3-13: Đồ thị vận tốc xe tại x0 0, x0 0, 0 , 

0 
0
35
Hình 3-14: Đồ thị góc lắc của thanh tại x0 0, x0 0, 0 , 

0 
0
Hình 3-15: Đồ thị vận tốc thanh tại x0 0, x0 0, 0 , 

0 
0
36
Hình 3-16: Đồ thị lực điều khiển tại x0 0, x0 0, 0 , 

0 
0
Từ 3 trường hợp điều khiển ta có kết luận sau:
- Khi góc ban đầu càng gần góc đặt thì biên độ u hay lực điều khiển ban đầu nhỏ, hệ
dao động ít
- Khi góc ban đầu càng xa góc đặt thì biên độ u hay lực điều khiển ban đầu lớn, hệ
dao động nhiều
37
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
Xây dựng mô hình con lắc ngược phục vụ cho sinh viên học tập các môn học Robot
công nghiệp, Mô hình hóa các hệ cơ điện tử, Kỹ thuật tự động là cần thiết. Trong
chương trình bầy việc tính toán thiết kế con lắc và bộ điều khiển dựa trên lý thuyết
Truyền động điện[13], các luật điều khiển PD và PID [10] và lý thuyết Cơ học kỹ
thuật.
4.1 Các vấn đề điều khiển trên mô hình thật
Hình 4-1: Mô hình cơ khí con lắc ngược
Mô hình thử nghiệm có kết cấu cơ khí bao gồm 1 động cơ gắn dẫn động, hệ con lắc
gồm thanh trượt và xe được bố trí di chuyển trên 1 ray trượt bi. Con lắc và xe liên
kết với động cơ thông qua đai. Có 2 encoder tương đối(bộ mã hóa vòng quay tương
đối) dùng để đo dịch chuyển của xe theo phương ngang và góc lệch của thanh. Độ
phân giải của encoder là 0,3125o .
38
4.1.1 Các giai đoạn điều khiển
Trên mô hình thử nghiệm, quá trình điều khiển hệ con lắc được chia là 2 giai đoạn
là giai đoạn lật ngược(Swingup) và giai đoạn điều khiển ổn định.
Góc ổn định
Góc lật ngược
Giai đoạn ổn định Giai đoạn lật ngược
Hình 4-2: Vị trí của thanh khi thực hiện các giai đoạn điều khiển
Hình 4-3: Các giai đoạn điều khiển
4.1.1.1 Giai đoạn lật ngược
Giai đoạn lật ngược là giai đoạn con lắc được điều khiển sao cho thanh từ vị trí
thẳng đứng hướng xuống chuyển sang thẳng đứng hướng lên. Ở đây ta sử dụng bộ
điều khiển PD
39
 ,  xd Ux x, x, ,
x, x
Hình 4-4: Bộ điều khiển lật ngược
Bộ điều khiển lật ngược gồm 2 vòng điều khiển, vòng ngoài được sử dụng để điều
khiển quỹ đạo của xe nhằm dao động thanh tới vị trí cân bằng. Bằng cách di chuyển
xe tiến hoặc lùi có thể làm thanh vung lên được vị trí cân bằng hướng lên trên. Thiết
kế của bộ điều khiển PD vòng ngoài như sau:
 =  + ̇ (4.1)
Trong đó xd là vị trí mong muốn của xe,  là góc lệch của thanh, giá trị lớn nhất của
góc lệch là ±180° . Hai tham số P=0.55 và D=0.001 được lựa chọn bằng thực
nghiệm nhằm làm mượt bộ điều khiển.
Bộ điều khiển PD vòng trong được sử dụng để tạo ra điện áp động cơ nhằm làm
thay đổi vị trí của xe theo .
 = ( − ) + ̇ (4.2)
Các tham số Kp và Kd cũng được dò bằng thực nghiệm.
4.1.1.2 Giai đoạn điều khiển ổn định
Hệ thống con lắc ngược là một ví dụ điển hình của hệ thống dạng under-actuated,
trong đó: số lượng cơ cấu chấp hành (xe) nhỏ hơn số bậc tự do của hệ thống
(chuyển động của xe và chuyển động quay của con lắc). Vì vậy, để có thể điều
khiển cân bằng con lắc đồng thời cả vị trí xe, ta cần thiết kế hai bộ điều khiển riêng
biệt. Tuy nhiên, chúng ta chỉ có một điện áp điều khiển chung cho cả hai đối tượng
này. Vì vậy, điện áp điều khiển góc lệch con lắc và điện áp điều khiển vị trí xe phải
40
được kết hợp chung thành một điện áp điều khiển cho cả hệ thống. Thực tế thấy
rằng, nếu ta cấp một điện áp để xe dịch chuyển về phía trái sẽ làm con lắc ngã về
phía ngược lại. Điều này chứng minh điện áp điều khiển hai đối tượng này sẽ trái
dấu trên cùng một hệ quy chiếu. Thông qua mối quan hệ này, ta xây dựng được sơ
đồ điều khiển cho hệ thống như Hình 4-5
Ux
U
Hình 4-5: Mô hình kết hợp 2 bộ điều khiển PID
Do có 2 bộ điều khiển PID nên ta phải thực hiện việc chỉnh định lần lượt:
Ban đầu bộ điều khiển vị trí xe theo x được tắt đi, ta tiến hành điều chỉnh tham số
cho bộ PID dùng cho góc lệch đến khi nào hệ ổn định. Lúc này xe sẽ di chuyển tự
do trên ray trượt.
Khi hệ đã ổn định thì ta tiến hành chỉnh định bộ PID dành cho vị trí xe. Bộ tham số
cho bộ PID này yêu cầu phải tạo ra đáp ứng chậm hơn so với bộ PID dùng cho góc
lệch.
41
4.1.2 Phương pháp đo đạc vị trí xe và góc lệch của thanh
Để đo đạc được vị trí xe và góc lệch của thanh chúng ta có thể sử dụng nhiều
phương pháp khác nhau sử dụng nguyên lí quang, điện.
4.1.2.1 Phương pháp dùng thước đo kiểu điện trở
Đây là cảm biến dạng điện kế gồm một điện trở cố định, trên đó có 1 tiếp xúc điện
có thể di chuyển gọi là con chạy. Điện trở này có dạng cuộn dây kim loại hoặc dạng
băng dẫn được chế tạo bằng chất dẻo trộn bột dẫn điện là carbon hoặc kim loại.
Nguyên lý đo của loại cảm biến này dựa trên công thức tính điện trở:

 = . (4.3)

Trong đó:
R: điện trở dây
ρ: điện trở suất của dây
l: chiều dài dây
S: tiết diện dây
Với ρ và S không đổi, điện trở phụ thuộc và chiều dài l. Hay:
R = K.l (4.4)
Quan hệ giữa đầu ra của cảm biến và đại lượng đo được với thước đo thẳng:

 = . (4.5)

Trong đó:
x: khoảng di chuyển
l: chiều dài biến trở
R: điện trở toàn phần
Vậy thông qua đo Rx ta có thể xác định được dịch chuyển của con chạy.
42
4.1.2.2 Phương pháp dùng thước đo laser
Thước đo laser sử dụng một chùm tia laser để xác định khoảng cách đến một đối
tượng. Các hình thức phổ biến nhất của máy đo khoảng cách laser hoạt động dựa
trên nguyên tắc thời gian bay bằng cách gửi một xung laser trong một chùm tia hẹp
hướng thẳng đến đối tượng và đo thời gian được thực hiện bởi xung phản xạ từ mục
tiêu và quay trở lại bộ phát xung. Khoảng cách giữa 2 điểm A và B được tính theo
công thức:

= (4.6)

Trong đó:
c : Tốc độ của ánh sáng trong không khí
t : Khoảng thời gian ánh sáng di chuyển từ A tới B và quay trở lại A

=

Trong đó φ là sự trễ pha gây ra do dịch chuyển của ánh sáng và ω là tần số
góc của sóng quang.
Với mô hình của luận văn, thước đo laser có thể gắn trên thân cố định và liên tục đo
khoảng cách tới phần xe dịch chuyển.
4.1.2.3 Phương pháp dùng thước đo bằng siêu âm
Đây là phương pháp đo khoảng cách dựa trên nguyên lý thu phát siêu âm. Cảm biến
gồm một bộ phát và một bộ thu sóng siêu âm. Sóng siêu âm từ đầu phát truyền đi
trong không khí, gặp vật cản (vật cần đo khoảng cách tới) sẽ phản xạ ngược trở lại
và được đầu thu ghi lại. Khoảng cách đo được của một số cảm biến loại này nằm
trong phạm vi từ 4cm đến 300cm.
43
Hình 4-6: Nguyên lý đo khoảng cách dùng cảm biến siêu âm
Tuy nhiên phương pháp này có nhược điểm là độ chính xác không cao và dễ bị
nhiễu do môi trường nên sẽ không được xét đến.
4.1.2.4 Phương pháp dùng bộ mã hóa vòng quay encoder tương đối
Đây là một dụng cụ đo bao gồm một thước mã hoá tương đối và bộ thu phát tia
quang học:
- Bộ thu phát quang học:
Bộ phát: là các diode phát tia hồng ngoại(Led) hoặc đèn phát quang, được sử dụng
như một nguồn phát sáng. Bộ thu: có thể là một đi ốt quang cho phép dòng điện
chạy theo một hướng khi có tia hồng ngoại chiếu tới, tia hồng ngoại tới như một
dòng điện base của tranzito. Dòng trên cực C sẽ tỷ lệ với cường độ ánh sáng tới.
44
Hình 44-7: Nguyên lý một bộ thu phát điển hình

- Thước mã hoá: Bao gồm 2 loại: Loại thước thẳng và dạng đĩa tròn.
Loại thước thẳng: gồm ccác vạch chia độ. Vạch chia độ có thể là vvạch đen, hoặc
trắng, ánh sáng xuyên qu
ua hoặc phản xạ trên các vạch khắc độ.
Hình 4-8: Nguyên lý hoạt động thước encoder thẳng

Mỗi lần ánh sáng xuyên qua một vạch, tế bào quang điện nhận đượcc một xung ánh
sáng và tạo ra một xungg điện. Bằng cách đếm số xung này ta có thể
t suy ra được
dịch chuyển.
D x = Nd0 (4.7)
Trong đó:
D x: Khoảng dịch ch
huyển
N: Số xung đếm đư
ược
45
d0 : Giá trị của vạch chia độ
Loại đĩa tròn: đĩa có thể làm bằng nhựa trong hoặc kính với các vạch hướng tâm
hoặc nhựa mờ với các khe nhỏ, vì vậy khi đĩa quay thì các vạch này sẽ ngăn hoặc
cho tia sáng đi qua tới thiết bị dò quang học.
Hình 4-9: Bộ mã hóa encoder tương đối và nguyên lý hoạt động

Với bộ mã hóa encoder tương tự như Hình 4-9 sẽ có 2 kênh A và B tương ứng với 2
cảm biến. Hai kênh A và B này tạo thành 2 chuỗi xung vuông góc với nhau. Dựa
vào thứ tự của 2 chuỗi xung này ta có thể xác định được chiều quay của trục. Công
thức xác định góc quay theo radian và độ:

 = 2 (4.8)


 = 360 

Trong đó:
n: Số xung đếm được
k: Tổng số vạch
Do có hai kênh A và B độc lập nên sẽ có 2 phương pháp giải mã 1X và 4X. Phương
pháp 1X giải mã dựa theo mức của 2 kênh, đĩa có bao nhiêu vạch sẽ tương ứng bấy
nhiêu xung. Phương pháp 4X giải mã dựa theo sườn của 2 kênh nên số xung đếm
46
được sẽ bằng 4 lần số vạch trên đĩa. Nói cách khác độ chính xác của phương pháp
4X gấp 4 lần phương pháp 1X.
Hình 4-10: Giản đồ xung của encoder tương đối
4.1.2.5 So sánh và lựa chọn phương pháp đo cho mô hình
Tổng kết lại ta có sự so sánh giữa các phương pháp như sau:
STT Phương pháp Ưu điểm Nhược điểm
Độ chính xác chấp nhận Chế tạo phức tạp.

được. Độ chính xác phụ
thuộc kết cấu cơ khí.
1 Thước đo kiểu điện trở Có sự mài mòn theo
thời gian sử dụng.
Chịu ảnh hưởng bởi

bụi, bẩn
Độ chính xác cao. Giá thành rất cao.

2 Thước đo laser
Hầu như không nhiễu.
Giá rẻ. Bị ảnh hưởng bởi

3 Thước đo siêu âm nhiễu môi trường.
Độ chính xác thấp.
4 Bộ mã hóa encoder tương Giá rẻ, dễ lắp đặt. Không cho biết vị trí
47
đối Độ chính xác cao tuyệt đối.
Hầu như không nhiễu Không đo được ở môi

trường nhiệt độ cao.
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
Từ kết quả so sánh ở Bảng 4-1 ta sẽ lựa chọn phương pháp đo số 4 dùng bộ mã hóa
encoder tương đối. Tuy nhiên, do loại encoder thẳng dùng cho trục x khó chế tạo
nên ta sẽ dùng encoder quay cho cả 2 vị trí cần đo. Với trục x thì dịch chuyển sẽ
được biến từ tịnh tiến sang quay bằng các kết cấu cơ khí (đai, bánh đai). Với cách
làm này thì sẽ phát sinh thêm sai số tích lũy do phần cơ khí nhưng hầu như không
đáng kể, giá trị đo được vẫn đủ đáp ứng yêu cầu của bài toán (Coi yêu cầu ban đầu
là lấy sai số trục x < 0.05mm).
Nhược điểm không cho biết được vị trí chính xác của xe sẽ được xử lí bằng phần
mềm trong mỗi lần khởi động. Sai số tích lũy do tín hiệu điện sẽ được xử lí bằng
cách sử dụng encoder có thêm kênh tham chiếu Z (Mỗi vòng quay sẽ tạo ra một
xung).
4.1.3 Phương pháp điều khiển khối dẫn động
4.1.3.1 Lựa chọn động cơ
Với yêu cầu về điều khiển, kích thước và công suất khối dẫn động của mô hình
chúng ta sẽ xem xét lựa chọn các loại động cơ điện như sau:
- Động cơ một chiều DC

- Động cơ một chiều không chổi than BLDC
- Động cơ xoay chiều 1 pha
Nguyên lý của từng loại động cơ được tham khảo trong [13]. Dưới đây là bảng mô
tả ưu nhược điểm của từng loại động cơ:
48
STT Động cơ Ưu điểm Nhược điểm
Điều chỉnh tốc độ tốt. Giá thành vừa phải.
Khả năng chịu quá tải. Cấu tạo phức tạp, khó
Mạch điều khiển rẻ, dễ bảo dưỡng do sử dụng

điều khiển cổ góp.
Đáp ứng động nhanh do Gây nhiễu khi điều

1 Một chiều DC quán tính nhỏ. khiển.
Hiệu suất cao do sử

dụng rôto nam châm
vĩnh cửu.
Dải tốc độ rộng.
Dải công suất rộng.
Điều chỉnh tốc độ tốt. Giá thành rất cao.
Khả năng chịu quá tải Mạch điều khiển phức
Đáp ứng động nhanh do tạp, khó điều khiển.

quán tính nhỏ.
Hiệu suất cao do sử

Một chiều không chổi
2 dụng rôto nam châm
than BLDC
vĩnh cửu.
Tuổi thọ cao do không

có chuyển mạch cơ khí.
Không gây nhiễu khi

hoạt động.
49
Dải tốc độ rộng.
Dải công suất rộng.
Giá thành rẻ. Dải tốc độ không
Hiệu suất cao. rộng.

3 Xoay chiều 1 pha
Dễ dàng lắp đặt. Dải công suất hẹp.
Cấu tạo đơn giản, bền
Bảng 4-2: Bảng so sánh các loại động cơ

Từ kết quả so sánh của Bảng 4-2 ta sẽ lựa chọn động cơ một chiều DC cho mô hình
4.1.3.2 Phương pháp điều khiển điện áp
Như đã trình bày trong CHƯƠNG 3 lực điều khiển được tính theo (N) tuy nhiên do
luận văn không xét đến phương án sử dụng các cảm biến lực nên lực điều khiển sẽ
được quy đổi sang lực điện (thông qua điện áp). Ngoài ra việc quy đổi sang lực điện
cũng hoàn toàn thỏa mãn phương pháp điều khiển PID.
Trong phạm vi của luận văn này chúng ta sẽ chỉ xét đến phương pháp điều khiển
điện áp động cơ điện một chiều sử dụng PWM.
Phương pháp PWM (Pulse Width Modulation) là phương pháp điều chỉnh điện áp
ra tải, hay nói cách khác, là phương pháp điều chế dựa trên sự thay đổi độ rộng của
chuỗi xung vuông, dẫn đến sự thay đổi điện áp ra. Các PWM khi biến đổi thì có
cùng 1 tần số và khác nhau về độ rộng của sườn dương hay sườn âm. PWM được
ứng dụng nhiều trong điều khiển. Điển hình nhất mà chúng ta thường hay gặp là
điều khiển động cơ và các bộ xung áp, điều áp... Sử dụng PWM điều khiển độ
nhanh chậm của động cơ hay cao hơn nữa, nó còn được dùng để điều khiển sự ổn
định tốc độ động cơ.
50
Ngoài lĩnh vực điều khiển hay ổn định tải thì PWM còn tham gia và điều chế các
mạch nguồn như : boot, buck, nghịch lưu 1 pha và 3 pha... PWM còn gặp nhiều
trong thực tế ở các mạch điện điều khiển. Điều đặc biệt là PWM chuyên dùng để
điều khiển các phần tử điện tử công suất có đường đặc tính là tuyến tính khi có sẵn
1 nguồn 1 chiều cố định.
Nguyên lý hoạt động của PWM là phương pháp được thực hiện theo nguyên tắc
đóng ngắt nguồn của tải một cách có chu kì theo luật điều chỉnh thời gian đóng cắt.
Phần tử thực hiện nhiệm vụ đó trong mạch các van bán dẫn. Chúng ta tham khảo
Hình 4-15 để xem xét cách thức thực hiện phương pháp PWM.
Trong khoảng thời gian 0 - t0 , ta cho van G mở, toàn bộ điện áp nguồn Ud được đưa
ra tải. Còn trong khoảng thời gian t 0 – T (T là chu kì điều khiển van), cho van G
khóa, cắt nguồn cung cấp cho tải. Vì vậy với t0 thay đổi từ 0 cho đến T, ta sẽ cung
cấp toàn bộ, một phần hay khóa hoàn toàn điện áp cung cấp cho tải. Giá trị điện áp
trung bình cho tải được tính theo công thức:

U = U  (V) (4.9)

Trong đó:
U max là điện áp nguồn cung cấp cho tải.
Hình 4-11: Sơ đồ quan hệ giữa xung kích MOSFET và đầu ra

Vậy tỉ số t0 /T quy định u(t) của động cơ, muốn điều khiển điện áp cho động cơ ta
chỉ cần điều chỉnh tỉ số này.
51
4.1.3.1 Lựa chọn mạch công suất của động cơ
* Mạch dùng cầu H
Xét một cách tổng quát mạch cầu H là mạch gồm 4 công tắc đượcc mắc theo hình
chữ H.
1 2
3 4
Hình 4-12: Mạch cầu H tổng quát
1 2 1 2
3 4 3 4
(a) (b)
Hình 4-1
13: Nguyên lí đảo chiều động cơ của cầu H
Bằng cách điều khiển ch
ho các công tắc này đóng mở mà ta có thể đđiều khiển được
động cơ một chiều dừng hay chạy theo chiều mong muốn. Thực tế c ác công tắc này
52
sẽ được thay thế bằng MOSFET, Transistor hay rờle. Bảng dưới đấy mô tả trạng
thái động cơ ứng với trạng thái các công tắc. Các trường hợp chỉ 1 công tắc đóng (
có dòng điện chạy qua) không được xét đến bởi động cơ lúc đó sẽ dừng lại. Ngoài
ra các cặp công tắc cùng phía (1,3) hoặc (2,4) không được phép đóng đồng thời do
sẽ dẫn đến hiện tượng ngắn mạch.
STT Công tắc 1 Công tắc 2 Công tắc 3 Công tắc 4 Chiều động cơ
Chạy như Hình

1 Đóng Mở Mở Đóng
4-13(a)
2 Đóng Đóng Mở Mở Phanh động cơ
3 Mở Mở Đóng Đóng Phanh động cơ
Chạy như Hình

4 Mở Đóng Đóng Mở
4-13(b)
Mở: Không có dòng điện chạy qua. Đóng: Có dòng điện chạy qua
Bảng 4-3: Trạng thái động cơ theo các công tắc cầu H
Trong thực tế các công tắc được thay thế bằng các MOSFET để đảm bảo công suất
và tốc độ cho mạch.
MOSFET là viết tắt của cụm Meta Oxide Semiconductor Field-Effect Transistor tức
Transisor hiệu ứng trường có dùng kim loại và oxit bán dẫn. Hình 4-14 mô tả cấu
tạo của MOSFET kênh N và kí hiệu của nó.
53
Hình 4-14: MOSFET kênh N

MOSFET có 3 chân gọi là Gate (G), Drain (D) và Source (S). Đối với MOSFET
kênh N, nếu điện áp chân G lớn hơn chân S khoảng từ 3V thì MOSFET bão hòa hay
dẫn. Khi đó điện trở giữa 2 chân D và S rất nhỏ (gọi là điện trở dẫn DS), MOSFET
tương đương với một khóa đóng. Vì tính dẫn của MOSFET phụ thuộc vào điện áp
chân G, MOSFET được gọi là linh kiện điều khiển bằng điện áp, rất lý tưởng cho
các mạch số nơi mà điện áp được dùng làm mức logic (ví dụ 0V là mức 0, 5V là
mức 1).
MOSFET thường được dùng trong các mạch cầu H vì dòng mà linh kiện bán dẫn
này có thể dẫn rất cao, thích hợp cho các mạch công suất lớn. Hình 4-15 mô tả ví dụ
một MOSFET kênh N được dùng điều khiển motor DC.
Hình 4-15: Ví dụ dùng MOSFET điều khiển động cơ

54
Q1 Q2
IRF540N/TO IRF540N/TO
24V
MOTOR_OUT1 MOTOR_OUT2
C6
Q3 Q4
470uF/50V
GNDP
Hình 4-16: Mạch cầu H dùng MOSFET
* Mạch dùng MOSFET và Rơle
Xét ví dụ ở Hình 4-15 có sử dụng MOSFET để điều khiển động cơ, có thể thấy cách
làm này rõ ràng chỉ có thể điều khiển động cơ quay một chiều. Muốn đảo chiều thì
phải lắp tới 4 MOSFET theo Hình 4-16, cách này sẽ khá tốn kém. Chúng ta sẽ xét
tới một thiết kế khác sử dụng một rờ le và một MOSFET.
BX1 12V
R57
1
VCC 1 2 BX1 ROLE1

3 BX2 ISO10
4 BX3
5 D36
BX4 R55 100 Q18
2
PG3
A331 C1815
R56
LED
10K
GNDP D32
BX2 12V 12V 8 ROLE1 12V

IRF540N/TO 1
5 DIODE
1
D34 D33 6 J102

ISO9 Q19 Q17 24V 7 C136 1
R53 100 D468 4 2
1N5408 3 104
2
2
D35
Q20 R52 DCCN1
PG4 B562 R
R54 10K LS5
GNDP
LED
Hình 4-17: Mạch điều khiển động cơ dùng MOSFET và Rờle

Với thiết kế này việc điều khiển điện áp cho động cơ vẫn phụ thuộc vào Vgs của
MOSFET giống như cầu H. Tuy nhiên quá trình đảo chiều lại được thực hiện bởi
rờle LS5. Mạch có thiết kế khá đơn giản nhưng có nhược điểm là tốc độ đảo chiều
55
thấp (đảo chiều nhanh sẽ dẫn tới hiện tượng trùng dẫn làm cháy MOSFET), không
tạo ra được trạng thái hãm như cầu H. Ngoài ra công suất của mạch không cao do
phụ thuộc vào khả năng chịu tải của rờ le.
* So sánh 2 phương pháp
STT Mạch điều khiển Ưu điểm Nhược điểm
Công suất lớn Mạch công suất phức
Tốc độ đảo chiều tạp.

1 Cầu H nhanh.
Điều khiển dễ dàng.
Có trạng thái hãm.
Giá rẻ hơn, mạch đơn Không có trạng thái

giản hãm
Công suất phụ thuộc

vào khả năng chịu tải
2 MOSFET và rờ le
của rờ le.
Tốc độ đảo chiều thấp

nên điều khiển khó
khăn hơn.
Bảng 4-4: So sánh các mạch công suất cho động cơ DC

Với so sánh kể trên kết hợp yếu tố cần đảo chiều nhanh của mô hình (dao động
quanh vị trí cân bằng) chúng ta lựa chọn mạch cầu H làm mạch công suất cho động
cơ DC.
56
4.2 Thiết kế cơ khí
4.2.1 Các thành phần cơ khí
STT Tên thiết bị Chú thích
Động cơ sử dụng là loại động

cơ 1 chiều (DC) 2 cặp chổi
1 than, công suất 60W, điện áp
24V
Bộ truyền đai sử dụng đai

2
thang
Ray trượt bi đi kèm thanh dẫn

hướng
Encoder gắn với thanh được lấy

từ 1 động cơ nhỏ có sẵn
4
57
Thanh con lắc dài 30 cm bằng

nhôm đồng chất.
Bệ đỡ con lắc bằng sắt, khối

lượng 1,2kg
Ổ bi
Bộ đỡ thanh trượt
9 Bệ máy
58
Hình 4-18: Thiết kế cơ khí 3D của hệ thống

59
4.2.2 Hình ảnh sau lắp ráp
Hình 4-19: Hệ thống lắp ráp hoàn thiện

60
Hình 4-20: Thanh ở vị trí tự do
4.3 Thiết kế mạch
4.3.1 Mạch điều khiển
Mạch điều khiển sử dụng vi điều khiển STM32F103RCT6 là 1 vi điều khiển 32 bit
thuộc họ ARM, đây là một chíp xử lí tương đối mạnh phù hợp cho nhu cầu tính toán
và thu thập dữ liệu của luận văn.
Trên mạch điều khiển có 2 ngõ ra điều khiển động cơ một chiều, 4 ngõ vào cho các
encoder, một cổng kết nối truyền thông nối tiếp với máy tính. Ngoài ra còn cho các
ngoại vi khác.
Phần mềm thiết kế mạch là Cadence Allegro. Các cụm hình bên dưới mô tả các
khối chức năng của mạch điều khiển.
61
U2 STM32F103_HD_64
uIn_EnA2 14 13 3V3A
uIn_EnB2 15 PA0/WKUP/USART2_CTS/ADC123_IN0/TIM2_CH1_ETR/TIM5_CH1/TIM8_ETR VDD_A 19
uIn_EnC2 16 PA1/USART2_RTS/ADC123_IN1/TIM2_CH2/TIM5_CH2 VDD_4 64
3V3
uIn_EnD2 17 PA2/USART2_TX/ADC123_IN2/TIM2_CH3/TIM5_CH3 VDD_3 48
LED2 20 PA3/USART2_RX/ADC123_IN3/TIM2_CH4/TIM5_CH4 VDD_2 32
EN1 21 PA4/SPI1_NSS/USART2_CK/ADC12_IN4/DAC_OUT1 VDD_1
PWM1 22 PA5/SPI1_SCK/ADC12_IN5/DAC_OUT2
DIR1 23 PA6/SPI1_MISO/ADC12_IN6/TIM3_CH1/TIM8_BKIN 1 R5 0R
41 PA7/SPI1_MOSI/ADC12_IN7/TIM3_CH2/TIM8_CH1N VBAT
TXD 42 PA8/USART1_CK/TIM1_CH1/MCO
RXD 43 PA9/USART1_TX/TIM1_CH2 8 M1_ENABLE
CANRX 44 PA10/USART1_RX/TIM1_CH3 ADC123_IN10/PC0 9 M2_ENABLE
CANTX 45 PA11/USART1_CTS/USBDM/CANRX/TIM1_CH4 ADC123_IN11/PC1 10 SW1
JTMS 46 PA12/USART1_RTS/USBDP/CANTX/TIM1_ETR ADC123_IN12/PC2 11 SW2
JTCK 49 PA13/JTMS-SWDIO/(PA13) ADC123_IN13/PC3 24 SW3
uIn_EnA1 R6 0R JTDI 50 PA14/JTCK-SWCLK/(PA14) ADC12_IN14/PC4 25 SW4
PA15/JTDI/SPI3_NSS/I2S3_WS/(TIM2_CH1_ETR/SPI1_NSS/PA15) ADC12_IN15/PC5 37 uIn_EnA1
R7 0R
(TIM3_CH1)/I2S2_MCK/TIM8_CH1/SDIO_D6/PC6 38 R8 0R uIn_EnB1
CURR_FEED1 26 (TIM3_CH2)/I2S3_MCK/TIM8_CH2/SDIO_D7/PC7 39 LimSW1
CURR_FEED2 27 PB0/ADC12_IN8/TIM3_CH3/TIM8_CH2N/(TIM1_CH2N) (TIM3_CH3)/TIM8_CH3/SDIO_D0/PC8 40 LimSW2
BOOT1 28 PB1/ADC12_IN9/TIM3_CH4/TIM8_CH3N/(TIM1_CH3N) (TIM3_CH4)/TIM8_CH4/SDIO_D1/PC9 51 LimSW3
uIn_EnB1 R9 55 PB2/BOOT_1 (USART3_TX)/UART4_TX/SDIO_D2/PC10 52
0R JTDO LimSW4
NJTRST 56 PB3/JTDO/SPI3_SCK/I2S3_CK/(TRACESWO/TIM2_CH2/SPI1_SCK/PB3) (USART3_RX)/UART4_RX/SDIO_D3/PC11 53 LimSW5
EN2 57 PB4/NJTRST/SPI3_MISO/(TIM3_CH1/SPI1_MISO/PB4) (USART3_CK)/UART5_TX/SDIO_CK/PC12 2 LimSW6
PWM2 58 PB5/I2C1_SMBA/SPI3_MOSI/I2S3_SD/(TIM3_CH2/SPI1_MOSI) TAMPER_RTC/PC13 3 M1_MODE
DIR2 59 PB6/I2C1_SCL/TIM4_CH1/(USART1_TX) OSC32_IN/PC14 4 M2_MODE
LED3 61 PB7/I2C1_SDA/TIM4_CH2/(USART1_RX) OSC32_OUT/PC15
LED4 62 PB8/TIM4_CH3/SDIO_D4/(I2C1_SCL/CAN_RX)
uIn_EnC1 29 PB9/TIM4_CH4/SDIO_D5/(I2C1_SDA/CAN_TX) 5 OSC_IN
uIn_EnD1 30 PB10/I2C2_SCL/USART3_ TX/(TIM2_CH3) (CAN_RX)/OSC_IN/PD0 6 OSC_OUT
SPI_NSS 33 PB11/I2C2_SDA/USART3_RX/(TIM2_CH4) (CAN_TX)/OSC_OUT/PD1 54 LED1
SPI_SCK 34 PB12/SPI2_NSS/I2S2_SMBA/USART3_CK/TIM1_BKIN TIM3_ETR/UART5_RX/SDIO_CMD/PD2
SPI_MISO 35 PB13/SPI2_SCK/I2S2_CK/USART3_CTS/TIM1_CH1N
SPI_MOSI 36 PB14/SPI2_MISO/USART3_RTS/TIM1_CH2N 31
PB15/SPI2_MOSI/I2S2_SD/TIM1_CH3N VSS_1 47
VSS_2 63 R14 R15
RESET 7 VSS_3 18 GND C6 C7
3V3 R16 10K BOOT0 60 NRST VSS_4 12 AGND
BOOT_0 VSS_A 104 104
GND 0R GND 0R
2
1
J8 GNDR17 10K BOOT0

CON2 R19 10K BOOT1
Hình 4-21: Thiết kế vi điều khiển trung tâm
Vi điều khiển hoạt động ở tần số 72MHz với kiến trúc thực thi lệnh kiểu đường ống
6 tầng nên tốc độ nhanh gấp 6 lần vi điều khiển thông thường ở cùng tần số. Thiết
kế hiện nay cho phép điều khiển tối đa 2 động cơ DC hoặc 1 động cơ BLDC.
R29 0R 3V3
1
J12 R30 0R 5V
2 GND
3 TXD
4 RXD
DEBUG UART1
Hình 4-22: Thiết kế cổng kết nối máy tính
Tuy STM32F103RCT6 có hỗ trợ các kết nối mới như USB, CAN, I2S nhưng để
tương thích tốt với máy tính thì kết nối UART-RS232 vẫn được lựa chọn để làm
cổng kết nối với phần mềm trên máy tính. Tốc độ của kết nối này là 115200 bps (bit
per second)
62
5V
R31
R32
R33
R34
R35R36
R37
R38
330330330330330330330330
U4
uIn_EnB2 3 18 In_EnB2 J17 J18
uIn_EnA2 4 SL1 DH1 17 In_EnA2
uIn_EnC2 5 SL2 DH2 16 In_EnC2 5V 1 5V 1
uIn_EnD2 6 SL3 DH3 15 In_EnD2 GND 2 GND 2
uIn_EnD1 7 SL4 DH4 14 In_EnD1 In_EnA1 3 In_EnA2 3
uIn_EnC1 8 SL5 DH5 13 In_EnC1 In_EnB1 4 In_EnB2 4
uIn_EnA1 9 SL6 DH6 12 In_EnA1 In_EnC1 5 In_EnC2 5
uIn_EnB1 10 SL7 DH7 11 In_EnB1 In_EnD1 6 In_EnD2 6
SL8 DH8
3V3 2 19 R45 200K 5V
GND 1 SREF DREF 20 CON6 CON6
GND GREF GND C30 104
GTL2003
Hình 4-23: Thiết kế đầu vào bộ chuyển đổi điện áp cho encoder
Do điện áp của encoder là 0-5V trong khi của vi điều khiển là 0-3.3V nên cần một
IC GTL2003 làm nhiệm vụ chuyển đổi mức điện áp này.
FB1
1 2 3V3A
U3 R28
3 2 Ferrit Bead FB2
5V 3V3
GND 1 V IN V OUT C14 C15 C16 C17 C18 C19 C20 GND 1 2 AGND
GND
4
LM1117-3V3 GND 104 104 104 104 104 104 1K

D5
104 LED Ferrit Bead
T
3V3
C26 3V3A
12V C21 C11 C12
GND 470uF C24 C25 C27
104 AGND 104 104
104 GND12 104 104
U5
D6 L1
12V 1 2 5V
VIN OUT
D7
R43 C28 4 INDUCTOR C29 R44
J16 1 DIODE 5 FB 1K
NC
4K7 47uF ON/OFF

2 RU3A 470uF
D8
HO
LM2576/TO_5V
HEADER 2 FB3
GND12 LED 1 2 GND D9 LED
Ferrit Bead
Hình 4-24: Thiết kế nguồn cho mạch điều khiển
Đây là khối nguồn dành riêng cho cụm mạch điều khiển, nó chuyển đổi điện áp 1
chiều từ 12 đến 24V thành các mức điện áp 3.3V và 5V-3A.
63
Hình 4-25: Thiết kế PCB của mạch điều khiển
Mạch PCB(printed circuit board) gồm 2 lớp TOP(màu xanh) và BOTTOM(màu đỏ)
64
4.3.2 Mạch công suất
Như đã phân tích ở Mục 4.1.3.1 ta sẽ thiết kế mạch cầu H làm mạch lực cho động
cơ. Mạch công suất hiện tại có thể chạy được động cơ có dòng đỉnh lên tới 10A.
U1 ACS712
1 8 5V_MCU
2 IP1+ VDD
IP2+ 7 R1 1K CURR _FEED1
VIOUT 6
3 FILTER C1 1nF R2
24V 4 IP1- 5 GND_MCU
IP2- VSS 1K
GND_MCU
D1 RU RU D2
12V 12V
C2 C3 C5
Q1 Q2 C4
4u7/100V 104 104 4u7/100
U2 U3
5
5
IR2184 8 IRF540N/TO IRF540N/TO 8 IR2184
VCC
VCC
R5 VB VB R6
R3 4.7 R4 4.7
IN1 1 7 7 1 IN2
IN HO 24V HO IN
6 MOTOR_OUT1 MOTOR_OUT2 6
4K7 VS VS 4K7
SD1 2 4 4 2 SD1
COM
COM
SD LO C6 LO SD
3
3
Q3 Q4
R7 4.7 470uF/50V R8 4.7
GNDP
ISO1
1
180
PULSE_1+ R9 2 8 5VP
R10 3K3 5VP
7 5VP
6 PW M1 U4A
14
PULSE_1- 3 U4B
14
1
6N137 3 5
2 4 IN1
GNDP 6
5
R11
7
EN_1+ 180 5VP 4001
7
1
R12 1K 5VP GNDP 4001

ISO7 GNDP
D3 R13 Opto_4Pin PW M1 5VP
LED 3K3 SD1
2
EN_1- U4C
14
DIR_1+ R14 180 DIR1 R15
1
10K 8
R16 10 IN2
ISO8
Opto_4Pin GNDP DIR1 9
DIR_1- GNDP
2
4K7
7
4001
GNDP
Hình 4-26: Thiết kế mạch cầu H cho động cơ
Ở đây chúng ta sử dụng MOSFET IRF540 của hãng International Rectifier. Tuy
nhiên nếu chúng ta dùng vi điều khiển để kích MOSFET, rất dễ gây ra hiện tượng
nóng, trùng dẫn làm cháy MOSFET hoặc MOSFET không đạt được công suất mong
muốn. Vậy nên ở thiết kế của Hình 4-26 chúng ta sẽ phải sử dụng IC IR2184 kết
hợp thêm các IC logic làm IC kích MOSFET.
IR2184 là IC lái MOSFET bằng Boottrap. Như ta đã biết muốn kích MOSFET dẫn
thì Vgs >3. Trên Hình 4-26, đối với Q3 và Q4 thì đơn giản vì chân S đã nối xuống
GND, muốn kích ta chỉ cần cấp xung kích vào chân G là được. Nhưng với Q1 và
Q2, chân S có thế không cố định. Ví dụ: khi Q3 dẫn thì S của Q1 là 0V, nhưng khi
Q1 dẫn thì S của Q1 là 24V. Vì vậy muốn kích phải cấp nguồn kích độc lập vào 2
65
chân G và S. Vậy ta phải sử dụng 4 nguồn độc lập để kích 4 MOSFET. Kỹ thuật
Boottrap giúp ta chỉ cần sử dụng 1 nguồn để kích 4 MOSFET. Và sử dụng IC2184,
2 tụ C2 và C5 đóng vai trò làm 2 nguồn kích cho Q1 và Q2. Khi Q3 được kích đóng
thì tụ C2 sẽ được nạp. Khi Q3 kích ngắt thì 2 đầu tụ này sẽ được nối vào G và S của
Q1 để kích Q1 đóng.
Khi sử dụng ta chỉ cần dùng 1 xung PWM, xung còn lại cho bằng 0. Hai xung này
được nối với IN1 và IN2. Ví dụ như PWM IN1 được sử dụng , IN2 tắt, khi đó Q4
luôn tắt và Q2 luôn đóng và ta chỉ cần kích Q1 và Q3. Dòng tải qua Q2, qua động
cơ rồi qua Q3 về đất. Lúc này nếu MOSFET không tốt thì Q2 sẽ bị nóng do dòng
qua nó liên tục. Muốn đổi chiều động cơ ta tắt PWM IN1 và cấp xung PWM IN2.
Hình 4-26 cũng mô tả khối cách ly (các IC kí hiệu ISO hay còn gọi là cách ly
quang), khối này có công dụng cách ly mạch cầu H với vi điều khiển, vì khi điều
khiển động cơ bằng chân PWM khả năng có những xung không mong muốn dội từ
mạch công suất về vi điều khiển thường làm hỏng chân PWM. Cấu tạo của cách ly
quang bao gồm một đèn led và một transistor quang. Khi có xung PWM mức cao
cấp vào cách ly quang để led phát quang làm transistor dẫn và đưa mức cao ở đầu
ra. Khi có xung từ khối công suất dội về tới transistor của cách ly quang thì không
qua được, không làm ảnh hưởng tới vi điều khiển.
IC ACS712 là cảm biến dòng điện giúp vi điều khiển kiểm soát dòng điện chạy qua
động cơ. Việc này giúp bảo vệ mạch công suất do quá tải gây ra.
66
Hình 4-27: Thiết kế PCB mạch công suất
Hình 4-28: Hình ảnh mạch hoàn thiện

67
4.4 Thiết kế phần mềm
4.4.1 Ngôn ngữ và trình biên dịch
Vi điều khiển STM32F103RCT6 được lập trình bằng ngôn ngữ C thông qua trình
biên dịch KeilC ARM. Phần mềm KeilC ARM hỗ trợ Debug(Gỡ rối) thông qua
mạch JTAG Ulink2 nên rất thuận tiện cho quá trình phát triển.
Hình 4-29: Trình biên dịch và mạch debug cho STM32F103
Phần mềm Matlab và QT5 được dùng để thu thập dữ liệu và thiết lập tham số điều
khiển cho hệ thống.
68
4.4.2 Kiến trúc và thiết kế
Hình 4-30: Kiến trúc phần mềm

Phần mềm điểu khiển được thiết kế theo dạng một vòng lặp vô hạn. Cứ mỗi 1 mili
giây sẽ cập nhật lại giá trị các encoder và dữ liệu điều khiển một lần. Ngoài ra còn
có 2 luồng ngắt chạy gần như song song dùng để đọc trạng thái encoder và giao tiếp
với máy tính. Lưu đồ thuật toán hệ thống như sau:
69
Hình 4-31: Lưu đồ thuật toán chương trình điều khiển
Chi tiết các bước chạy như sau:
- Khởi tạo hệ thống: Khởi tạo phần cứng, ngoại vi

- Khởi tạo các bộ điều khiển: Thiết lập các biến trạng thái, tham số của các bộ
điều khiển
70
- Khởi tạo quỹ tạo: Khởi tạo chương trình tính toán quỹ đạo hình thang phục vụ
việc điều chỉnh vị trí tham chiếu của xe khi thanh ở vị trí cân bằng
- Tính sai số vị trí từ các encoder: Vị trí của encoder được lưu bởi chương trình
ngắt, sau đó được đọc ra bởi vòng lặp chính. Sai số được lấy bằng hiệu giá trị
encoder hiện tại và giá trị đặt.
- Kiểm tra vị trí encoder ở miền làm việc nào. Nếu thanh thuộc vùng lật ngược
Hình 4-2 thì chương trình sẽ thực hiện việc lật ngược thanh và ngược lại
- Thực hiện các bộ điều khiển PD/PID: Tham khảo Mục 4.4.4
- Khâu tỉ lệ chuyển đổi điện áp: Từ giá trị PID tính được kết hợp với phương
pháp PWM để tính ra độ rộng xung nhằm điều chỉnh điện áp cho động cơ.
- Giải thuật tinh chỉnh sai số do khối tâm: Do thanh chế tạo và lắp ráp có sai số
nên khối tâm của thanh có thể không đúng nên cần điều chỉnh vị trí sai số
này.
4.4.3 Thiết lập PWM điều khiển điện áp cho động cơ
Vi điều khiển STM32F103RCT6 hỗ trợ việc điều khiển PWM bằng bộ định thời
cứng (Timer) thông qua bộ thư viện CMSIS (Cortex Microcontroller Software
Interface Standard). Dưới đây là là lưu đồ quá trình thiết lập và mã giả cho nó:
71
Hình 4-32: Thiết lập chế độ PWM

Mã giả:
BEGIN
Enable Clock for GPIO, AFIO, TIMER3
GPIOA Configuration:Pin6, 7 as alternate function push-pull
A.6 PWM
A.7 DIRECTION
Compute the prescaler value

/* Time base configuration */
TIM_Period = 7999
TIM_Prescaler = PrescalerValue
/* PWM1 Mode configuration: Channel1 */

TIM_OCPolarity = TIM_OCPolarity_Low
TIM_OCMode = TIM_OCMode_PWM1
TIM_OutputState = TIM_OutputState_Enable;
TIM_Pulse =(t0/T)* TIM_Period/100;
Reload mode config

Enable TIMER3
END
72
Trong đoạn mã trên, TIM_Pulse và TIM_Period chính là các biến quy địn tỷ số t0/T. TIM_Pulse
đại diện cho t0, TIM_Period đại diện cho T.
4.4.4 Mã giả cho thuật toán PID/PD
* Mã giả cho PID:
BEGIN
/* Caculate PID */
pos_err_prev = pos_err;
input_err = _desired - _position;
if (input_err > 32767)

pos_err = 32767;
else if (input_err < -32768){
pos_err = -32768;
else
pos_err = input_err;
/* Proportional */
ProportionalPortion = (pos_err) * (kp);
if (ProportionalPortion>=0)
ProportionalPortion = ProportionalPortion ;
else
ProportionalPortion = -(-ProportionalPortion);
/* Derivative */
DerivativePortion = ( (pos_err - pos_err_prev)) * (kd);
if (DerivativePortion>=0)
DerivativePortion = DerivativePortion;
else
DerivativePortion = -(-DerivativePortion);
/* Integral */
IntegralError = (pos_err) * (ki );
integral = integral + IntegralError;
IntegralPortion = integral;
/* Accumulator saturation */
if (IntegralPortion >= integral_limit)
integral = IntegralPortion = integral_limit;
else if (IntegralPortion < - (integral_limit))
integral = IntegralPortion = - (integral_limit);
pd = ProportionalPortion + DerivativePortion;
pi = IntegralPortion;
pid = ProportionalPortion + DerivativePortion + IntegralPortion;
73
END
* Mã giả cho PD:
BEGIN
pos_err_prev = pos_err;
input_err = _desired - _position;
if (input_err > 32767)

pos_err = 32767;
else if (input_err < -32768){
pos_err = -32768;
else
pos_err = input_err;
/* Proportional */
ProportionalPortion = (pos_err) * (kp);
if (ProportionalPortion>=0)
ProportionalPortion = ProportionalPortion ;
else
ProportionalPortion = -(-ProportionalPortion);
/* Derivative */
DerivativePortion = ( (pos_err - pos_err_prev)) * (kd);
if (DerivativePortion>=0)
DerivativePortion = DerivativePortion;
else
DerivativePortion = -(-DerivativePortion);
pd = ProportionalPortion + DerivativePortion;
END
* Mã giả kết hợp 2 bộ điều khiển PID điều khiển vị trí xe và góc lệch thanh:
BEGIN
int current_position = curent_pos_encoder1;
int current_angle = curent_pos_encoder2;
int angle_control = PID1(ANGLE_SETPOINT, current_position);
int position_control = PID2(POSITION_SETPOINT, current_angle);
int pid = angle_control + position_control;
END
74
4.5 Kết quả đạt được và các vấn đề gặp phải
Bằng cách thu thập dữ liệu từ góc lệch thanh và vị trí xe khi chạy và sử dụng máy
tính để vẽ lại ta có đồ thị mô tả đáp ứng như sau:
Hình 4-33: Đáp ứng vị trí của xe (mm) theo thời gian (s)
Hình 4-34: Đồ thị đáp ứng góc lệch của thanh (rad) theo thời gian (s)
75
Từ Hình 4-33 và Hình 4-34 ta có thể thấy sau 6s xe sẽ dao động ổn định xung quanh
vị trí x mong muốn và sau 2s thanh dao động quanh vị trí đặt.
Kết quả này bị ảnh hưởng khá lớn bởi việc dò các tham số cho bộ điều khiển và sai
số cơ khí của hệ thống, rung động của hệ thống khi làm việc. Để khắc phục rung
động xảy ra thì cần cải tiến phần kết cấu cơ khí cho con lắc chạy trên ray trượt
chuyên dụng.
76
KẾT LUẬN
Mô hình con lắc ngược là một hệ cơ học đơn giản nhưng điển hình về việc ổn định
hóa vị trí cân bằng không ổn định bằng lực điều khiển. Mô hình này được xây dựng
phục vụ cho học tập, nghiên cứu điều khiển các hệ cơ điện tử.
Trong luận văn này đã sử dụng hai bộ điều khiển khác nhau (bộ điều khiển PID và
bộ điều khiển LQR) để điều khiển vị trí cân bằng thẳng đứng của con lắc ngược.
Mô hình con lắc ngược và bộ điều khiển đã được chế tạo, lắp ráp và vận hành phục
vụ cho luận án và cho các bài thực hành của sinh viên cơ điện tử Trường đại học
Kinh doanh và Công nghệ Hà Nội. Trong tương lai mô hình thí nghiệm này sẽ được
hoàn thiện hơn để phục vụ cho việc đào tạo sinh viên cơ điện tử và tự động hóa của
nhiều trường đại học khác.
77
TÀI LIỆU THAM KHẢO
Tiếng Việt
1. Nguyễn Văn Khang: Cơ học kỹ thuật (tái bản). NXB giáo dục Hà Nội 2012
2. Nguyễn Văn Khang: Động lực học hệ nhiều vật. NXB Khoa học và Kỹ thuật, Hà
Nội 2007.
3. Nguyễn Văn Khang, Chu Anh Mỳ: Cơ sở robot công nghiệp. NXB giáo dục, Hà
Nội 2011.
4. Nguyễn Văn Khang: Dao động kỹ thuật. NXB Khoa học và Kỹ thuật, Hà Nội
2004.
5. Nguyễn Doãn Phước: Lý thuyết điều khiển tuyến tính. NXB Khoa học và Kỹ
thuật, Hà Nội 2004.
6. Nguyễn Doãn Phước: Lý thuyết điều khiển phi tuyến. NXB Khoa học và Kỹ
thuật, Hà Nội 2003.
7. Nguyễn Doãn Phước: Lý thuyết điều khiển nâng cao. NXB Khoa học vs Kỹ thuật,
Hà Nội 2005.
8. Đỗ Sanh: Ổn định của hệ động lực và các áp dụng kỹ thuật. NXB Bách Khoa, Hà
Nội 2010.
9. Đỗ Sanh,Đỗ Đăng Khoa: Điều khiển các hệ động lực, điều khiển chuyển dộng
chương trình, điều khiển tối ưu chuyển động.NXB Bách Khoa, Hà Nội 2014.
10. Phạm Thượng Cát: Một số phương pháp điều khiển hiện đại cho robot công
nghiệp. NXB Đại học Thái Nguyên, Thái Nguyên 2009.
11. Hoàng Kim Đức: Điều khiển tối ưu hệ động lực tuyến tính bằng phương pháp
LQR. Đồ án tốt nghiệp, Trường Đại học Bách Khoa Hà Nội, Hà Nội 2015.
12. Nguyễn Phong Điền: Kỹ thuật đo và phân tích dao động cơ học. NXB Giáo dục
Việt Nam, Hà Nội 2015.
78
13. Bùi Quốc Khánh, Nguyễn Văn Liễn, Nguyễn Thị Hiền: Truyền động điện. NXB
Khoa học và kỹ thuật, Hà Nội 2010.
14. Mai Trong Dũng: Tính toán động học, động lực học và
thiết kế chế tạo mô hình robot song song delta Rostock. Luận văn Thạc sĩ khoa học,
Trường Đại học Bách Khoa Hà Nội, Hà Nội 2015.
15. Nguyễn Nhật Lệ: Các bài toán cơ bản của tối ưu hóa và điều khiển tối ưu. NXB
Khoa học và kỹ thuật, Hà Nội 2009.
16. Đinh Văn Phong: Mô phỏng số và điều khiển các hệ cơ học. NXB Giáo dục việt
nam, Hà Nội 2010.
17. Vũ Ngọc Phát: Nhập môn lý thuyết điều khiển toán học.NXB đại học quốc gia
Hà Nội 2010
18. Thái Hữu Nguyên: Điều khiển thích nghi phi tuyến cho robot công nghiệp trên
cơ sở mạng nơ ron nhân tạo. Luận văn Tiến sĩ, Trường Đại Học Bách Khoa Hà
Nội 2014, Hà Nội 2014.
Tiếng Anh
19. P. C. Muller and W. Schiehlen: Linear Vibrations. Martinus Nijhoff Publishers

Dordrech 1985
20. Lee, E. B. ; Markus, L: Foundations of Optimal Control Theory. John Wiley

and Sons, NewYork 1967
21. Brian D.O. Anderson, John B. Moore: Optimal Control Linear quadratic
methods. Prentice-Hall International, Englewood Cliffs, New Jersey, 1989.
22. H. Wener: Lecture Notes Optimal and Robust Control. TU Hamburg-Harburg,

2009.
23. A.Sinha: Linear Systems: Optimal and Robust Control.CRC press, Taylor and
Robust Group, Boca Raton 2007.
79
24. W.S.Levine, M.Athans: On the determination of the optimal constant output

feedback gains for linear multivariable systems. IEEE Transactions on Automatic
Control, Vol AC-15, N1, 1970, pp 44-48.
25. Nguyen Van Dao: Stability of Dynamic Systems. Vietnam National University
Publishing House, Hanoi 1998.
26. Zhongmin Wang, YangQuan Chen, Ning Fang: Minimum-Time Swing-up of A

Rotary Inverted Pendulum by Iterative Impulsive Control. The 2004 American
control conference, Boston, Massachusetts June 30 – July 2, 2004

Tính Toán, Chế Tạo Và Điều Khiển Mô Hình Con Lắc Ngược

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Tính Toán, Chế Tạo Và Điều Khiển Mô Hình Con Lắc Ngược

Uploaded by

Copyright:

Available Formats

i

LỜI CAM ĐOAN

Hà Nội, ngày 20/03/2016

Phạm Công Dũng

Hà Nội, ngày 20/03/2016

Phạm Công Dũng

LỜI CAM ĐOAN............................................................................................................... i

4.3.2 Mạch công suất .............................................................................................. 64

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.

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

Bảng 4-2: Bảng so sánh các loại động cơ ............................................................... 49

HỆ THỐNG DANH MỤC HÌNH VẼ

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

Hình 3-1: Sơ đồ điều khiển PID bằng Simulink ..................................................... 28

Hình 4-1: Mô hình cơ khí con lắc ngược ............................................................... 37

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-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-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

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-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-32: Thiết lập chế độ PWM ......................................................................... 71

II - MỤC ĐÍCH CỦA LUẬN VĂN

IV - NỘI DUNG CỦA LUẬN VĂN

1.1 Thiết lập phương trình vi phân chuyển động

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

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

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)

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

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

xc  x lc sin  x x  lc cos 

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