Luận văn Điều khiển robot hai bánh tự cân bằng sử dụng kỹ thuật điều khiển trượt

HU
TE
CH
BỘ GIÁO DỤC VÀ ĐÀO TẠO 
TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHỆ TP.HCM 
HOÀNG NGUYÊN PHƯỚC 
ĐIỀU KHIỂN 
ROBOT HAI BÁNH TỰ CÂN BẰNG 
SỬ DỤNG KỸ THUẬT ĐIỀU KHIỂN TRƯỢT 
LUẬN VĂN THẠC SỸ 
CHUYÊN NGÀNH: THIẾT BỊ, MẠNG VÀ NHÀ MÁY ĐIỆN 
MÃ SỐ : 60.52.50 
Tp. Hồ Chí Minh, tháng 06 năm 2012 
HU
TE
CH
BỘ GIÁO DỤC VÀ ĐÀO TẠO 
TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHỆ TP.HCM 
-------------------------------------- 
HOÀNG NGUYÊN PHƯỚC 
ĐIỀU KHIỂN 
ROBOT HAI BÁNH TỰ CÂN BẰNG 
SỬ DỤNG KỸ THUẬT ĐIỀU KHIỂN TRƯỢT 
LUẬN VĂN THẠC SỸ 
CHUYÊN NGÀNH: THIẾT BỊ, MẠNG VÀ NHÀ MÁY ĐIỆN 
MÃ SỐ : 60.52.50 
 HƯỚNG DẪN KHOA HỌC: PGS. TSKH. HỒ ĐẮC LỘC 
 TS. NGUYỄN THANH PHƯƠNG 
Tp. Hồ Chí Minh, tháng 06 năm 2012 
HU
TE
CH
CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI 
TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHỆ TP. HCM 
Hướng dẫn khoa học : 
 PGS. TSKH. HỒ ĐẮC LỘC 
 TS. NGUYỄN THANH PHƯƠNG 
Cán bộ chấm nhận xét 1: PGS. TS. BÙI XUÂN LÂM 
Cán bộ chấm nhận xét 2: TS. ĐỒNG VĂN HƯỚNG 
Luận văn Thạc sỹ được bảo vệ tại Trường Đại học Kỹ thuật Công nghệ thành phố 
Hồ Chí Minh ngày 14 tháng 07 năm 2012. 
Thành phần Hội đồng đánh giá luận văn Thạc sỹ gồm: 
1. TS. NGUYỄN HÙNG Chủ tịch Hội đồng 
2. PGS.TS. BÙI XUÂN LÂM Phản biện 1 
3. TS. ĐỒNG VĂN HƯỚNG Phản biện 2 
4. TS. TRƯƠNG VIỆT ANH Ủy viên 
5. TS. HỒ VĂN HIẾN Thư ký 
Xác nhận của Chủ tịch Hội đồng đánh giá luận văn và Khoa quản lý chuyên ngành 
sau khi luận văn đã được sửa chữa (nếu có). 
 Chủ tịch Hội đồng đánh giá Luận văn Quản lý chuyên ngành 
 TS. NGUYỄN HÙNG TS. NGUYỄN THANH PHƯƠNG 
HU
TE
CH
TRƯỜNG ĐH KỸ THUẬT CÔNG NGHỆ TP. HCM 
PHÒNG QLKH – ĐTSĐH 
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM 
Độc lập - Tự do - Hạnh phúc 
Tp. Hồ Chí Minh, ngày 15 tháng 09 năm 2011 
NHIỆM VỤ LUẬN VĂN THẠC SỸ 
Họ tên học viên: HOÀNG NGUYÊN PHƯỚC Giới tính: Nam. 
Ngày, tháng, năm sinh: 12/04/1970 Nơi sinh: Gia định. 
Chuyên ngành: Thiết bị, mạng và nhà máy điện MSHV:1081031020. 
I. TÊN ĐỀ TÀI. 
Điều khiển robot hai bánh tự cân bằng sử dụng kỹ thuật điều khiển trượt. 
II. NHIỆM VỤ VÀ NỘI DUNG. 
1. Nhiệm vụ của đề tài: 
- Nghiên cứu phương pháp điều khiển trượt. 
- Xác định mô hình toán học của hệ thống robot hai bánh tự cân bằng. 
- Thiết kế bộ điều khiển sử dụng phương pháp trượt. 
- Mô phỏng bộ điều khiển sử dụng Matlab đối với hệ thống robot hai 
bánh tự cân bằng. 
- Nhận xét kết quả đạt được và so sánh với các kết quả đã được nghiên 
cứu trong và ngoài nước. 
2. Nội dung của đề tài: 
Nội dung luận văn gồm 3 phần: 
Phần 1: Nghiên cứu phương pháp điều khiển trượt, hệ thống robot hai bánh 
tự cân bằng, từ đó nhận dạng các vấn đề cần giải quyết để phục vụ cho việc thiết kế 
bộ điều khiển. 
HU
TE
CH
Phần 2: Xác định mô hình toán học của hệ thống robot hai bánh tự cân bằng, 
xây dựng mô hình con lắc ngược trên xe hai bánh bằng phần mềm mô phỏng 
Matlab. 
Phần 3: Thiết kế bộ điều khiển trượt điều khiển ổn định tại điểm cân bằng 
cho hệ thống robot hai bánh tự cân bằng và mô phỏng hệ thống trên Matlab . Phân 
tích và nhận xét kết quả mô phỏng. 
III. NGÀY GIAO NHIỆM VỤ: ngày 15 tháng 09 năm 2011. 
IV. NGÀY HOÀN THÀNH NHIỆM VỤ: ngày 15 tháng 06 năm 2012. 
V. HƯỚNG DẪN KHOA HỌC: PGS. TSKH. Hồ Đắc Lộc. 
 TS. Nguyễn Thanh Phương. 
 Hướng dẫn khoa học Quản lý chuyên ngành 
 PGS. TSKH. HỒ ĐẮC LỘC TS. NGUYỄN THANH PHƯƠNG 
HU
TE
CH
LỜI CAM ĐOAN 
Tôi xin cam đoan rằng luận văn với nội dung: 
“Điều khiển robot hai bánh tự cân bằng sử dụng kỹ thuật điều khiển trượt” 
là công trình nghiên cứu của riêng tôi, dưới sự hướng dẫn của PGS.TSKH. Hồ Đắc 
Lộc và TS. Nguyễn Thanh Phương. 
Các số liệu, kết quả mô phỏng nêu trong luận văn là trung thực, có nguồn 
trích dẫn và chưa được công bố trong các công trình nghiên cứu khác. 
 Tp. Hồ Chí Minh, ngày 15 tháng 06 năm 2012 
 Thực hiện 
 HOÀNG NGUYÊN PHƯỚC 
HU
TE
CH
LỜI CẢM ƠN 
May mắn thay, khi tôi được bước chân đến Đại học Kỹ Thuật Công Nghệ 
thành phố Hồ Chí Minh, nơi tôi đã được dìu dắt đến bến bờ tri thức bởi những 
người Thầy, Cô, Anh, Chị với tấm lòng hy sinh, tận tụy và bao dung. 
Xin cảm ơn trường Đại học Kỹ thuật Công nghệ thành phố Hồ Chí Minh, 
cùng quý Thầy, Cô đã tận tình truyền đạt kiến thức và tạo mọi điều kiện tốt nhất cho 
lớp chúng tôi và cá nhân tôi trong suốt học trình Cao học. 
Với lòng tri ân sâu sắc, tôi muốn nói lời cảm ơn đến PGS.TSKH. Hồ Đắc 
Lộc và TS. Nguyễn Thanh Phương_ những người Thầy đã nhiệt tình hướng dẫn và 
chỉ bảo cho tôi trong suốt thời gian thực hiện nghiên cứu này. 
Cám ơn tất cả các bạn trong khóa học, những người cùng chung chí hướng 
trong con đường tri thức để tất cả chúng ta có được kết quả ngày hôm nay. 
Cảm ơn gia đình và những người thân đã động viên, hỗ trợ tôi trong suốt 
thời gian thực hiện nghiên cứu này. 
Xin trân trọng và chân thành gửi lại tất cả nơi đây lòng tri ân sâu sắc nhất. 
 Tp. Hồ Chí Minh, ngày 15 tháng 06 năm 2012 
 Thực hiện 
 HOÀNG NGUYÊN PHƯỚC 
HU
TE
CH
TÓM TẮT 
Vấn đề điều khiển con lắc ngược di động đã được nghiên cứu mạnh từ những 
yêu cầu đặt ra là thực hiện nhanh và chính xác. Cấu trúc cơ khí ở đây được chia làm 
ba phần chính, một là con lắc với một bậc tự do được đặt trên đế quay quanh trực 
thẳng đứng hoặc chuyển động trượt tuyến tính. 
Để đạt được tính chính xác cao, hệ thống điều khiển con lắc ngược di động 
được thiết kế thông qua kỹ thuật điều khiển trượt. Kỹ thuật này là phương pháp điều 
khiển bền vững, nó tạo ra ngõ vào để mang lại quỹ đạo mong muốn cho hệ thống. 
Trình tự thiết kế của nó là đầu tiên chọn ra mặt trượt thỏa đáp ứng vòng kín trong 
không gian trạng thái và sau đó thiết kế bộ điều khiển sao cho quỹ đạo hệ thống 
trạng thái theo mặt trượt và ở trên nó. 
Luận văn trình bày ba vấn đề. 
Thứ nhất, mô hình toán học tuyến tính, làm tiền đề phát triển hệ thống điều 
khiển. 
Thứ hai, kỹ thuật điều khiển trượt được phát triển để ổn định và bám theo hệ 
thống. 
Sau cùng, mô phỏng và dựa vào kết quả để kiểm chứng giải thuật đã điều 
khiển. 
HU
TE
CH
ABSTRACT 
Inverted pendulum control problems have been intensively studied due to the 
challenging demand of fast and precise performance, where the mechanical 
structure can be divided into three main types: One is a pendulum with one degree 
of freedom (DOF) mounted on a base rotating around the vertical axis or actuated 
by a linear slide. 
For achieving high-precision performance the control system for the mobile 
inverted pendulum is designed via sliding mode control (SMC) in this thesis. SMC 
is a robust control method which generates an input to yield a desired trajectory for 
a given system. Its design procedure is to first select a sliding surface that satisfies 
the desired closed-loop performance in the state space, and then to design a 
controller such that the system state trajectories are forced toward the sliding 
surface and stay on it. 
This thesis considers the control system design for a mobile inverted 
pendulum via sliding mode technique. 
Firstly, a linear mathematical model was used in order to facilitate the 
development of an efficient control system. 
Secondly, sliding mode controllers were developed for stabilizing and 
tracking this system. 
Lastly, the simulations were given to validate of the proposed controllers. 
HU
TE
CH
i 
MỤC LỤC 
DANH MỤC CÁC TỪ VIẾT TẮT .................................................................................................................................... iii
DANH MỤC CÁC BẢNG .............................................................................................................................................................. iv
DANH MỤC CÁC HÌNH ẢNH ................................................................................................................................................ v
MỞ ĐẦU ................................................................................................................................................................................................................... 1
Chương 1: TỔNG QUAN ................................................................................................................................................................. 2
1.1 Thế nào là xe hai bánh tự cân bằng ............................................................................................................................ 2
1.2 Tại sao phải thiết kế robot hai bánh tự cân bằng ....................................................................................... 4
1.3 Ưu, khuyết điểm ................................................................................................................................................................................ 6
1.3.1 Ưu điểm .............................................................................................................................................................................. 6
1.3.2 Khuyết điểm ................................................................................................................................................................... 6
1.4 Khả năng ứng dụng ....................................................................................................................................................................... 6
1.5 Tình hình nghiên cứu trong và ngoài nước ...................................................................................................... 6
1.5.1 Một số dạng xe hai bánh tự cân bằng dùng trên robot ............................................ 6
1.5.2 Tình hình nghiên cứu trong và ngoài nước ........................................................................ 10
1.6 Nhu cầu thực tế ............................................................................................................................................................................... 11
1.7 Mục đích của đề tài .................................................................................................................................................................... 11
1.8 Nhiệm vụ và giới hạn của đề tài ................................................................................................................................ 12
1.8.1 Nhiệm vụ của đề tài ......................................................................................................................................... 12
1.8.2 Giới hạn của đề tài ........................................................................................................................................... 12
1.9 Phương pháp nghiên cứu .................................................................................................................................................... 12
HU
TE
CH
ii 
Chương 2: CƠ SỞ LÝ THUYẾT ....................................................................................................................................... 13
2.1 Tổng quan về phi tuyến ..................................... ... 
51 
nh 5.2: Trạng thái tốc độ và tốc độ nghiêng của hệ thống. 
5.3: Trạng thái góc nghiêng của hệ thống. 
0 1 2 3 4 5 6 7 8 9 10
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
Thoi gian (giay)
Tr
an
g 
th
ai
 c
ua
 h
e 
th
on
g
Toc do
Toc do nghieng
0 1 2 3 4 5 6 7 8 9 10
-0.04
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
0.04
Thoi gian (giay)
Tr
an
g 
th
ai
 g
oc
 n
gh
ie
ng
 c
ua
 h
e 
th
on
g
Goc nghieng (rad)
HU
TE
CH
52 
5.4: Trạng thái tốc độ góc nghiêng của hệ thống. 
5.5: Tín hiệu ngõ vào. 
0 1 2 3 4 5 6 7 8 9 10
-0.05
0
0.05
0.1
0.15
Thoi gian (giay)
To
c 
do
 g
oc
 n
gh
ie
ng
 c
ua
 h
e 
th
on
g
Toc do goc nghieng (rad/s)
0 1 2 3 4 5 6 7 8 9 10
-50
-40
-30
-20
-10
0
10
20
30
40
50
Thoi gian (giay)
Ti
n 
hi
eu
 d
ie
u 
kh
ie
n
HU
TE
CH
53 
5.6: Bề mặt trượt S. 
5.3 Kết luận 
Điều khiển hệ thống con lắc ngược di động thông qua kỹ thuật điều khiển trượt 
được sử dụng trong luận văn này được thể hiện qua kết quả mô phỏng. 
So sánh với các kỹ thuật điều khiển khác, kỹ thuật điều khiển trượt là điều khiển 
bền vững, không nhạy với các nhiễu loạn bên ngoài. Đối với xe hai bánh tự cân bằng sử 
dụng kỹ thuật điều khiển trượt, nó ổn định nhanh khi di chuyển trong những địa hình 
phức tạp hoặc khi lên dốc, xuống dốc. 
Tương lai cần phải nghiên cứu hơn nữa nhằm tìm hiểu những ảnh hưởng của hệ 
thống. Các phương pháp phi tuyến đề nghị dùng cho các nghiên cứu sau này để có 
những cải tiến hơn nữa về tính ổn định và bền vững. 
0 1 2 3 4 5 6 7 8 9 10
-0.1
-0.05
0
0.05
0.1
0.15
Thoi gian (giay)
M
at
 tr
uo
t
HU
TE
CH
54 
Chương 6 
KẾT LUẬN 
Luận văn này giới thiệu phần mô phỏng bộ điều khiển độ ổn định con lắc 
ngược di động thông qua kỹ thuật điều khiển trượt. Các kết quả đã thực hiện được 
trong luận văn này là: 
- Thiết lập mô hình toán và hàm tr ạng thái cho động cơ và con lắc ngược. 
- Thiết kế bộ điều khiển thông qua kỹ thuật điều khiển trượt để ổn định 
con lắc ngược. 
- Xây dựng chương trình mô phỏng của hệ thống bằng Matlab. 
- Phân tích và nhận xét kết quả mô phỏng để chứng minh hiệu quả của kỹ 
thuật điều khiển trượt. 
HU
TE
CH
TÀI LIỆU THAM KHẢO 
[1] F. Grasser, A. D. Arrigo, S. Colombi and A. C. Rufer, “JOE: A Mobile, 
Inverted Pendulum”, IEEE Trans. Indus. Elec., Vol. 49, No. 1, 107-114, Feb. 
2002. 
[2] V. Williams and K. Matsuoka, “Learning to Balance the Inverted Pendulum 
Using Neural Networks”, IEEE Conference on Neural Networks, Vol. 1, pp. 
214-219, 2002. 
[3] R. J. Lee, K. C. Chou, S. H. Liu and J. Y. Yen, “Solid Modeling Based Servo 
System Design for a High Speed Micro Grinding Machine”, Machine Tools 
& Manufacture, Vol. 46, pp.208-217, 2006. 
[4] F. Esfandiari and H. K. Khalil, “Output Feedback Stabilization of Fully 
Control System”, International Journal of Control, Vol. 56, pp. 1007-1037, 
1992. 
[5] R. J. Wai and L. J. Chang, “Adaptive Stabilizing and Tracking Control for a 
Nonlinear Inverted-Pendulum System via Sliding-Mode Technique”, IEEE 
Trans. Indus. Elec., Vol. 53, No. 2, Apr. 2006. 
[6] R. N. Gasimov, A. Karamancioglu and A. Yazici, “A Nonlinear 
Programming Approach for the Sliding Mode Control Design”, Applied 
Mathematical Modeling, Vol. 29, pp. 1135-1148, 2005. 
[7] C. Edwards, “A Practical Method for the Design of Sliding Mode Controllers 
Using Linear Matrix Inequalities”, Automatica, Vol. 40, pp. 1761-1769, 
2004. 
[8] J. H. Wu, D. L. Pu and H. Ding, “Adaptive Robust Motion Control of SISO 
Nonlinear Systems with Implementation on Linear Motors”, Mechatronics, 
2007. 
HU
TE
CH
[9] R. J. Lee, K. C. Chou, S. H. Liu and J. Y. Yen, “Solid Modeling Based Servo 
System Design for a High Speed Micro Grinding Machine”, Machine Tools 
& Manufacture, Vol. 46, pp. 208-217, 2006. 
[10] C. Bonivento, L. Marconi and R. Zanasi, “Output Regulation of Nonlinear 
System by Sliding Mode”, Automatica, Vol. 37, pp. 535-542, 2001. 
[11] J. J. E. Slotine, J. K. Hedrick and E. A. Misawa, “On Sliding Observer for 
Nonlinear System”, Journal of Dynamics System, Measurement and Control, 
Vol. 109, pp. 245-252, 1987. 
[12] R. Sreedhar, B. Fernandez and J. Y. Masawa, “Robust Fault Detection in 
Nonlinear System Using Sliding Mode Observer”, Proceedings of IEEE 
Conference on Control and Applications, Vol. 2, pp. 715-721, Sept. 1993. 
[13] Y. Yao and M. Tomizuka, “Adaptive and Robust Control of Robot 
Manipulators: Theory and Comparative Experiment”, Proceedings IEEE 
Conference on Decision and Control, pp. 1290-1295, 1994. 
[14] Y. Huang and J. Han, “Analysis and Design for Nonlinear Continuous 
Extended State Observer”, Chinese Science Bulletin, pp. 1373-1379, 2000. 
[15] Z. Gao, S. Hu and F. Jiang, “A Novel Motion Control Design Approach 
Based on Active Disturbance Projection”, Proceedings the 40th IEEE 
Conference on Decision and Control, Vol. 5, pp. 4877-4882, Dec. 2001. 
[16] Z. Gao, Y. Huang and J. Han, “An Alternative Paradigm for Control System 
Design”, Proceedings the 40th IEEE Conference on Decision and Control, 
Orlando, Florida USA, Vol. 5, pp. 4578-4585, Dec. 2001. 
[17] J. Han, “Auto-Disturbance-Rejection Control and Its Application”, Control 
and Decision, Vol. 13, No. 1, pp. 19-23, 1998. 
[18] J. Ackermann and V. Utkin, “Sliding Mode Control Design Based on 
Ackermann’s Formula”, IEEE Trans. Auto. Con., Vol.43, No.2, 234-237, 
Feb. 1998. 
HU
TE
CH
[19] P. J. McKerrow, Introduction to Robotics, Addivision Wesley Longman 
China Ltd., VVP, 1998. 
[20] G. F. Franklin, J. D. Powell and A. Emami-Naeini, Feedback Control of 
Dynamic Systems, London: Prentice Hall, Inc., New Jersey, 2002. 
[21] J. J. E. Slotine and W. Li, Applied Nonlinear Control, New Jersey: Prentice-
Hall International, Inc., Englewood Cliffs, 1991. 
[22] D. Necsulescu, Mechatronics, Prentice Hall, 2002. 
[23] V. I. Utkin, Sliding-modes in Control Optimization, Springer-Verlag, 1992. 
[24] C. Edwards and S. K. Spurgeon, Sliding Mode Control: Theory and 
Applications, Taylor & Francis, Mar. 1998. 
[25] T. L. Chung, A Nonlinear Feedback Control of Wall-Following Robot di 
động, Master Thesis, The Graduate School of Pukyong National University, 
Feb. 2004. 
[26] Dương Hoài Nghĩa, Điều khiển hệ thống đa biến, NXB ĐHQG TP.HCM, 
2007. 
[27] Nguyễn Thị Phương Hà : Lý Thuyết điều khiển Hiện Đại , NXB ĐHQG 
TP.HCM, 2009. 
[28] Nguyễn Doãn Phước, Phan Xuân Minh, Hán Thành Trung, Lý thuyết điều 
khiển phi tuyến, NXB Khoa học kỹ thuật, 2006. 
HU
TE
CH
PHỤ LỤC 












−












=−
44342414
43332313
42322212
41312111
44434241
34333231
24232221
14131211
kbkbkbkb
kbkbkbkb
kbkbkbkb
kbkbkbkb
aaaa
aaaa
aaaa
aaaa
bkA T












=












−−−−
−−−−
−−−−
−−−−
=
44434241
34333231
24232221
14131211
4444344324421441
4334333323321331
4224322322221221
4114311321121111
AAAA
AAAA
AAAA
AAAA
kbakbakbakba
kbakbakbakba
kbakbakbakba
kbakbakbakba














−−−























=−= −
44
3
4
2
4
1
44434241
34333231
24232221
14131211
4321
1
1
0100
0010
0001
0100
0010
0001
)(
cc
c
c
c
c
c
AAAA
AAAA
AAAA
AAAA
cccc
TbkAA T






















−−−
−−−
−−−
−−−
=
4
4
4
3
43
4
2
42
4
1
41
4
34
4
3
3433
4
2
3432
4
1
3431
4
24
4
3
2423
4
2
2422
4
1
2421
4
14
4
3
1413
4
2
1412
4
1
1411
c
M
c
cMM
c
cMM
c
cMM
c
A
c
cAA
c
cAA
c
cAA
c
A
c
cAA
c
cAA
c
cAA
c
A
c
cAA
c
cAA
c
cAA
ở đây, 
4143132121111 AcAcAcAcM +++= 
4243232221212 AcAcAcAcM +++= 
4343332321313 AcAcAcAcM +++= 
4443432421414 AcAcAcAcM +++= 
HU
TE
CH
CHƯƠNG TRÌNH MÔ PHỎNG (m file) 
1. File điều khiển cân bằng: Balancing_controller.m 
clear all; 
close all; 
global C M0 F 
ts=0.02; 
T=30; 
TimeSet=[0:ts:T]; 
para=[]; 
options=odeset('RelTol',1e-3,'AbsTol',[1e-3 1e-3 1e-3 1e-3]); 
%options=[]; 
x0=[0.5,0.3,0,0]; 
[t,xout]=ode45('dynamical_equation',TimeSet,x0,options,para); 
x1=xout(:,1); 
x2=xout(:,2); 
x3=xout(:,3); 
x4=xout(:,4); 
s=C(1)*x1+C(2)*x2+C(3)*x3+C(4)*x4; 
if F==1 
 M0=40; 
 u=-M0*sign(s); 
elseif F==2 
 beta=30; 
 delta=0; 
 for k=1:1:T/ts+1 
u(k)=-
beta*(abs(x1(k))+abs(x2(k))+abs(x3(k))+abs(x4(k))+delta)*sign(s(k); 
 end 
end 
figure(1); 
plot(t,[x1 x2 x3 x4]); 
HU
TE
CH
xlabel('Thoi gian (giay)'); 
ylabel('Trang thai cua he thong'); 
legend('Khoang cach','Toc do','Goc','Toc do nghieng'); 
axis([0 10 -0.4 0.8]); 
figure(2); 
plot(t,s,'r'); 
xlabel('Thoi gian (giay)'); 
ylabel('Mat truot'); 
axis([0 10 -0.15 0.15]); 
figure(3); 
plot(t,u,'r'); 
xlabel('Thoi gian (giay)'); 
ylabel('Tin hieu dieu khien'); 
axis([0 10 -50 50]); 
figure(4); 
plot(t,[x2,x4]); 
xlabel('Thoi gian (giay)'); 
ylabel('Trang thai cua he thong') 
legend('Toc do','Toc do nghieng'); 
axis([0 10 -0.4 0.4]); 
figure(5); 
plot(t,x3); 
xlabel('Thoi gian (giay)'); 
ylabel('Trang thai goc nghieng cua he thong'); 
legend('Goc nghieng (rad)'); 
axis([0 10 -0.04 0.04]); 
figure(6); 
plot(t,x4); 
xlabel('Thoi gian (giay)'); 
ylabel('Toc do goc nghieng cua he thong'); 
legend('Toc do goc nghieng (rad/s)'); 
axis([0 10 -0.05 0.15]); 
HU
TE
CH
2. File phương trình động học: Dynamical_equation.m 
function dx=DynamicModel(t,x,flag,para) 
global C M0 F 
%Variable initialization 
g=9.81; %Gravity(m/s^2) 
r=0.051; %Radius of wheel(m) 
Mw=0.03; %Mass of wheel(kg) 
Mp=1.13; %Mass of body(kg) 
Iw=0.000039; %Inertia of the wheel(kg*m^2) 
Ip=0.0041; %Inertia of the body(kg*m^2) 
L=0.07; %Length to the body's center of mass(m) 
%Motor's variables 
Km=0.006123; %Motor torque constant(Nm/A) 
Ke=0.006087; %Back EMF constant(Vs/rad) 
R=3; %Nominal terminal resistance(Ohm) 
%Va=voltage applied to motors for controlling the pendulum 
%State Space Equation 
%Pre-calculated to simplify the matrix 
%Denominator for the A and B matrices 
beta=(2*Mw+(2*Iw/r^2)+Mp); 
alpha=(Ip*beta+2*Mp*L^2*(Mw+Iw/r^2)); 
a22=(2*Km*Ke*(Mp*L*r-Ip-Mp*L^2))/(R*r^2*alpha); 
a23=(Mp^2*g*L^2)/alpha; 
a42=(2*Km*Ke*(r*beta-Mp*L))/(R*r^2*alpha); 
a43=(Mp*g*L*beta)/alpha; 
b2=(2*Km*(Ip+Mp*L^2-Mp*L*r))/(R*r*alpha); 
b4=(2*Km*(Mp*L-r*beta)/(R*r*alpha)); 
A=[0,1,0,0; 
 0,a22,a23,0; 
 0,0,0,1; 
 0,a42,a43,0]; 
HU
TE
CH
b=[0;b2;0;b4]; 
Cont=[A A*b A^2*b A^3*b]; 
Rho=rank(Cont); 
%Ackermann's formula 
eig(A) 
n1=-1;n2=-2;n3=-3; 
C=[0,0,0,1]*inv([b,A*b,A^2*b,A^3*b])*(A-n1*eye(4))*(A-
n2*eye(4))*(A-n3*eye(4)); 
s=C*x; 
F=1; 
if F==1 
 M0=40; 
 u=-M0*sign(s); 
elseif F==2 
 beta=30; 
 delta=0; 
 u=-beta*(abs(x(1))+abs(x(2))+abs(x(3))+abs(x(4))+delta)*sign(s); 
end 
%State equation 
dx=zeros(4,1); 
f0=0.5; 
ft=f0*sin(3*t); 
dx=A*x+b*(u+ft);