Do An 2 Balance Cart 19161127 19151121

lOMoARcPSD|15618161
Đồ Án 2 Balance CART 19161127 19151121
Điều khiển tự động (Trường Đại học Sư phạm Kỹ thuật Thành phố Hồ Chí Minh)
Studocu is not sponsored or endorsed by any college or university

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)
lOMoARcPSD|15618161
121Equation Chapter 1 Section 2
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH

KHOA ĐIỆN – ĐIỆN TỬ
ĐỒ ÁN HỌC PHẦN 2
ĐỀ TÀI: “NGHIÊN CỨU VÀ THIẾT KẾ HỆ

XE HAI BÁNH TỰ CÂN BẰNG”
GVHD: TS. Nguyễn Trần Minh Nguyệt

Nhóm sinh viên thực hiện:
1 Trương Trí Kiệt 19161127
2 Lê Văn Hà 19151121
TP. HỒ CHÍ MINH – 7/2021

lOMoARcPSD|15618161
LỜI CẢM ƠN
Chúng em chân thành cảm ơn, quý thầy cô trong khoa Điện – Điện tử trường
Đại học Sư Phạm Kỹ Thuật TP.HCM nói chung và quý thầy cô bộ môn Điều khiển tự
động nói riêng, đã trang bị kiến thức và giúp đỡ chúng em, giải quyết những khó khăn,
hỗ trợ phương tiện thí nghiệm trong suốt quá trình tìm hiểu và nghiên cứu đề tài.
Đặc biệt chúng em xin chân thành cảm ơn giảng viên hướng dẫn là ThS.
Nguyễn Trần Minh Nguyệt đã tận tình giúp đỡ trong quá trình lựa chọn đề tài và hỗ trợ
sinh viên trong quá trình thực hiện.
Tuy nhiên do lần đầu tiên thực hiện đồ án với đề tài “Nghiên cứu và thiết kế hệ
xe hai bánh tự cân bằng” và làm quen với các giải thuật điều khiển mới, chuyên môn
còn hạn chế, nên không tránh khỏi những sai sót. Chúng em rất mong nhận được sự
thông cảm, góp ý và hướng dẫn của quý thầy cô, các anh chị cùng các bạn.
Xin chúc quý thầy cô nhiều sức khoẻ, thành công trong công việc, cuộc sống và
tiếp tục đào tạo các sinh viên giỏi góp phần đóng góp cho nhà trường, đất nước. Chúc
các anh chị, cùng các bạn trong khoá sức khoẻ, học tập thật tốt để chuẩn bị kiến thức
vững vàng trong tương lai.
TP Hồ Chí Minh, ngày … tháng … năm 2021
Nhóm Sinh viên thực hiện
Trương Trí Kiệt – Lê Văn Hà

lOMoARcPSD|15618161
NHẬN XÉT CỦA GIÁO VIÊN
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
…………………………………………………………………………………………
TP Hồ Chí Minh, ngày … tháng … năm 2021
Giáo viên hướng dẫn
Ths. Nguyễn Trần Minh Nguyệt
ii

lOMoARcPSD|15618161
TÓM TẮT ĐỀ TÀI
Đề tài “Nghiên cứu và thiết kế hệ xe hai bánh tự cân bằng” được nhóm thực
hiện dựa trên lý thuyết mô hình con lắc ngược. Qua so sánh và phân tích ưu nhược
điểm của một số bộ điều khiển có thể sử dụng cho hệ xe hai bánh tự cân bằng, nhóm
đã quyết định sử dụng giải thuật điều khiển toàn phương tuyến tính LQR (Linear
Quadratic Regulator) để điều khiển và tiến hành mô phỏng trên MATLAB. Thông qua
đó để nghiên cứu giải thuật LQR và ứng dụng để điều khiển hệ xe hai bánh tự cân
bằng.
Trong đề tài nhóm thực hiện so sánh các trường hợp sử dụng bộ điều khiển toàn
phương tuyến tính LQR cho hệ thống đối với trường hợp có nhiễu hệ thống, nhiễu đo
lường từ cảm biến và trường hợp hệ không chịu tác động của nhiễu. Nhóm sẽ kiểm
nghiệm những điểm giống và khác nhau trong các trường hợp đồng thời ứng dụng bộ
lọc Kalman để quan sát, ước lượng trạng thái, lọc nhiễu cho hệ thống. Qua đó, đưa ra
ưu nhược điểm và cách khắc phục để hệ thống điều khiển xe cân bằng đạt trạng thái ổn
định nhất có thể.
Đồ án nãy cũng sẽ trình bày về thiết kế hệ thống. Về phần cứng bao gồm lựa
chọn các thiết bị phù hợp cho hệ thống, thiết kế mô hình trên SolidWorks,… Về phần
mềm bao gồm giải thuật điều khiển và lưu đồ hệ thống: điều khiển toàn phương tuyến
tính LQR cho động cơ, xử lý tín hiệu từ cảm biến,… đồng thời thiết kế giao diện giám
sát hệ thống lập trình bằng ngôn ngữ C#.
iii

lOMoARcPSD|15618161
MỤC LỤC
LỜI CẢM ƠN...............................................................................................................i
NHẬN XÉT CỦA GIÁO VIÊN..................................................................................ii
TÓM TẮT ĐỀ TÀI.....................................................................................................iii
MỤC LỤC...................................................................................................................iv
DANH MỤC BẢNG VÀ HÌNH ẢNH......................................................................vii
DANH MỤC CÁC TỪ VIẾT TẮT............................................................................ix
CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI.........................................................................1
1.1 Đặt vấn đề............................................................................................................... 1
1.2 Mục tiêu đề tài........................................................................................................3
1.3 Giới hạn đề tài........................................................................................................4
1.4 Phương pháp nghiên cứu......................................................................................4
1.5 Nội dung đề tài.......................................................................................................5
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT...........................................................................6
2.1 Đặc tính động lực học............................................................................................6
2.2 Một số phương pháp điều khiển có thể sử dụng cho hệ thống..........................11
2.2.1 Bộ điều khiển vi tích phân tỉ lệ...................................................................11
2.2.2 Bộ điều khiển vi tích phân tỉ lệ kết hợp với giải thuật di truyền.................11
2.2.3 Bộ điều khiển mờ.......................................................................................12
2.2.4 Bộ điều khiển tối ưu...................................................................................13
2.3 Điều khiển toàn phương tuyến tính....................................................................14
2.3.1 Bộ lọc Kalman............................................................................................14
2.3.2 Bài toán điều khiển toàn phương tuyến tính liên tục..................................15
2.3.3 Tuyến tính hoá hệ thống.............................................................................16
2.3.4 Khảo sát tính điều khiển được và quan sát được của hệ thống...................19
2.3.5 Hàm chỉ tiêu chất lượng.............................................................................20
iv

lOMoARcPSD|15618161
2.4 Sơ đồ mô phỏng hệ thống trên MATLAB Simulink..........................................20
CHƯƠNG 3: THIẾT KẾ PHẦN CỨNG..................................................................25
3.1 Yêu cầu thiết kế....................................................................................................25
3.2 Sơ đồ khối hệ thống.............................................................................................25
3.3 Lựa chọn thiết bị phần cứng...............................................................................27
3.3.1 Khối điều khiển trung tâm..........................................................................27
3.3.2 Khối cảm biến đo góc nghiêng...................................................................29
3.3.3 Khối điều khiển động cơ............................................................................31
3.3.4 Khối động cơ và cảm biến đọc xung của động cơ......................................33
3.3.5 Khối cấp nguồn..........................................................................................35
3.3.6 Bộ khung xe...............................................................................................38
3.4 Bảng đi dây hệ thống...........................................................................................39
3.4.1 Phần nguồn.................................................................................................39
3.4.2 Phần tín hiệu điều khiển.............................................................................39
CHƯƠNG 4: THIẾT KẾ PHẦN MỀM...................................................................40
4.1 Yêu cầu thiết kế....................................................................................................40
4.2 Xây dựng thuật toán điều khiển.........................................................................40
4.2.1 Thuật toán chương trình chính....................................................................41
4.2.2 Thuật toán xử lý xung trả về từ encoder và tính toán giá trị góc tới, góc
xoay..................................................................................................................... 42
4.2.3 Thuật toán đọc và xử lý giá trị từ cảm biến góc nghiêng............................45
4.2.4 Thuật toán tính xung băm PWM mỗi bánh với giải thuật LQR và điều khiển
động cơ................................................................................................................ 46
4.2.5 Xuất và vẽ biểu đồ lên máy tính.................................................................48
4.3 Phần mềm giám sát..............................................................................................48
CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI...........................50
5.1 Những kết quả đã đạt được.................................................................................50

v

lOMoARcPSD|15618161
5.1.1 Kết quả thiết kế mô hình và mô phỏng.......................................................50
5.1.2 Kết quả thực tế...........................................................................................56
5.2 Kết luận................................................................................................................59
5.3 Hạn chế của đề tài................................................................................................59
5.4 Hướng phát triển.................................................................................................60
TÀI LIỆU THAM KHẢO.........................................................................................61
PHỤ LỤC...................................................................................................................62
vi

lOMoARcPSD|15618161
DANH MỤC BẢNG VÀ HÌNH Ả
Bảng 2.1 Kí hiệu và ý nghĩa các đại lượng....................................................................7

Bảng 2.2 Thông số mô hình.........................................................................................21
Bảng 3.1 Bảng kết nối chân của cảm biến MPU6050..................................................31
Bảng 3.2 Bảng kết nối chân của mạch cầu H L298N...................................................33
Bảng 3.3 Bảng kết nối chân của động cơ và encoder...................................................35
Bảng 3.4 Bảng đi dây phần nguồn...............................................................................39
Bảng 3.5 Bảng đi dây phần tín hiệu điều khiển............................................................39
Y
Hình 1.1 Xe dạng ba bánh khi di chuyển trên địa hình bằng phẳng...............................2
Hình 1.2 Xe dạng ba bánh khi đi lên dốc.......................................................................2
Hình 1.3 Xe dạng ba bánh khi đi xuống dốc..................................................................2
Hình 1.4 Xe hai bánh di chuyển trên các địa hình khác nhau theo hướng bảo toàn sự
thăng bằng.....................................................................................................................3
Hình 2.1 Mô hình xe hai bánh tự cân bằng trên mặt phẳng............................................6
Hình 2.2 Cấu trúc cơ bản của bộ điều khiển mờ..........................................................12
Hình 2.3 Sơ đồ khối của bộ lọc Kalman......................................................................14
Hình 2.4 Sơ đồ khối của bộ điều khiển toàn phương tuyến tính (LQR).......................15
Hình 2.5 Khối Two-wheeled Self Balancing Robot trong MATLAB Function............22
Hình 2.6 Sơ đồ mô phỏng hệ thống trong trường hợp 1...............................................22
Hình 3.1 Sơ đồ khối hệ thống......................................................................................26
Hình 3.2 Sơ đồ chân kết nối của Arduino Mega 2560 Rev3........................................29
Hình 3.3 Cảm biến góc nghiêng GY-521 6DOF IMU MPU6050................................31
Hình 3.4 Sơ đồ nối dây cảm biến với Arduino Mega 2560..........................................31
Hình 3.5 Mạch cầu H điều khiển động cơ DC L298N (Dual Full Bridge Driver)........33
Hình 3.6 Động cơ DC Servo GM25-370 DC Geared Motor........................................35
Hình 3.7 Các thiết bị trong khối cấp nguồn.................................................................37
Hình 3.8 Sơ đồ cấp nguồn cho hệ thống......................................................................37
Hình 3.9 Các thành phần để xây dựng khung xe..........................................................38
vii

lOMoARcPSD|15618161
Hình 4.1 Lưu đồ thuật toán chương trình chính...........................................................41

Hình 4.2 Lưu đồ chương trình ngắt xử lý xung trả về từ encoder................................42
Hình 4.3 Lưu đồ tính toán giá trị góc tới và góc xoay..................................................43
Hình 4.4 Lưu đồ thuật toán đọc và xử lý giá trị từ cảm biến góc nghiêng...................45
Hình 4.5 Lưu đồ thuật toán tính xung băm PWM mỗi bánh với giải thuật LQR.........46
Hình 4.6 Lưu đồ thuật toán điều khiển động cơ...........................................................47
Hình 4.7 Lưu đồ đọc và biểu diễn đồ thị......................................................................48
Hình 4.8 Giao diện giám sát hệ thống..........................................................................49
Hình 5.1 Mô hình hệ xe hai bánh tự cân bằng.............................................................50
Hình 5.2 Đáp ứng của góc tới, góc nghiêng, góc xoay của xe trong trường hợp 1.......52
Hình 5.4 Tín hiệu điều khiển của xe trong trường hợp 2.............................................53
Hình 5.7 Đáp ứng của góc tới, góc nghiêng, góc xoay của xe khi chịu tác động của
nhiễu trong trường hợp 4.............................................................................................55
Hình 5.8 Đáp ứng của góc tới, góc nghiêng, góc xoay của xe sau khi qua bộ lọc
Kalman trong trường hợp 4..........................................................................................55
Hình 5.10 Mô hình phần cứng thi công thực tế hướng trực diện..................................56
Hình 5.11 Mô hình phần cứng thi công thực tế hướng nằm ngang..............................57
Hình 5.12 Mô hình phần cứng thi công thực tế hướng chéo........................................57
Hình 5.13 Khảo sát góc nghiêng của xe khi qua bộ lọc Kalman..................................58
Hình 5.14 Khảo sát góc nghiêng của xe trả về khi xe hoạt động..................................58
viii

lOMoARcPSD|15618161
DANH MỤC CÁC TỪ VIẾT TẮT
Từ viết Tên tiếng Anh Tên tiếng Việt

tắt
LQR Linear Quadratic Điều khiển toàn phương
Regulator tuyến tính
PID Proportional Integral Điều khiển vi tích phân tỉ
Derivative lệ
GA Genetic Algorithm Giải thuật di truyền
MIMO Multiple Input Hệ thống nhiều đầu vào
Multiple Output nhiều đầu ra
RAM Ramdom Access Memory Bộ nhớ truy cập ngẫu nhiên
ROM Read Only Memory Bộ nhớ chỉ đọc
EPROM Erasable Programmable Bộ nhớ chỉ đọc có thể
Read Only Memory lập trình xoá được
I/O Input/Ouput Đầu vào/ Đầu ra
PWM Pulse Width Modulation Điều chế độ rộng xung
UART Universal Asynchronous Bộ truyền nhận nối tiếp
Receiver - Transmitter bất đồng bộ
I2C Inter – Intergrated Circuit Giao thức giao tiếp nối tiếp
đồng bộ
IMU Inertial Measurement Khối đo lường quán tính
Unit
SRAM Static Ramdom Bộ nhớ truy cập ngẫu nhiên
Access Memory tĩnh
ICSP In Circuit Serial Lập trình nối tiếp mạch
Programming điện tử
SPI Serial Peripheral Interface Giao diện ngoại vi nối tiếp
DMP Digital Motion Processor Bộ xử lý chuyển động số
ADC Analog to Digital Chuyển đổi tương tự sang
Converter số
DOF Degrees Of Freedom Bậc tự do
DC Direct Current Dòng điện một chiều
ix

lOMoARcPSD|15618161
CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI
1.1 Đặt vấn đề
Trong ngành tự động hóa – điều khiển tự động nói chung và điều khiển học nói
riêng, mô hình con lắc ngược là một trong những đối tượng nghiên cứu điển hình và
đặc thù bởi đặc tính động không ổn định của mô hình nên việc điều khiển được đối
tượng này trên thực tế đặt ra như một thử thách.
Kết quả nghiên cứu mô hình con lắc ngược cơ bản, ví dụ như mô hình xe con
lắc, con lắc ngược quay… có thể ứng dụng và kế thừa sang các mô hình tương tự khác
nhưng có tính ứng dụng thực tiễn hơn, chẳng hạn như mô hình tên lửa, mô hình xe hai
bánh tự cân bằng,…
Như vậy, để cân đối giữa tính cơ bản với tính ứng dụng thực tiễn nhưng vẫn
đảm bảo quy mô nghiên cứu nằm trong khả năng cho phép, xe hai bánh tự cân bằng
được chọn làm xuất phát điểm cho ý tưởng về đề tài nghiên cứu.
Xe hai bánh tự cân bằng được xem như cầu nối kinh nghiệm giữa mô hình con
lắc ngược với robot hai chân và robot giống người. Đây là dạng xe có hai bánh đồng
trục, do đó khắc phục được những nhược điểm vốn có của các robot hai hoặc ba bánh
kinh điển. Các xe ba hoặc bốn bánh kinh điển, theo đó có cấu tạo gồm hai bánh dẫn
động và môt hoặc hai bánh tự do (hay bất kì cái gì khác) để đỡ trọng lượng xe.
Thiết kế của xe ba hoặc bốn bánh có thể di chuyển tốt trên địa hình phẳng
nhưng không thể di chuyển lên xuống trên địa hình lồi lõm hoặc mặt phẳng nghiêng.
Khi di chuyển lên đồi, trọng lượng robot dồn vào đuôi xe làm mất khả năng bám và
trượt ngã. Khi di chuyển xuống đồi, trọng tâm thay đổi về phía trước làm xe bị lật úp.
Xe dạng ba bánh xe di chuyển trên địa hình bằng phẳng trọng lượng được chia
đều cho bánh lái và bánh dẫn nhỏ.

lOMoARcPSD|15618161
Hình 1.1 Xe dạng ba bánh khi di chuyển trên địa hình bằng phẳng
Xe dạng ba bánh khi lên dốc, trọng lượng dồn vào bánh trước khiến lực ma sát
giúp xe bám trên mặt đường không được đảm bảo.
Hình 1.2 Xe dạng ba bánh khi đi lên dốc

Xe dạng ba bánh khi xuống dốc, trọng lực dồn vào bánh sau khiến xe có thể bị
lật úp.
Hình 1.3 Xe dạng ba bánh khi đi xuống dốc

Ngược lại, các xe dạng hai bánh đồng trục lại thăng bằng rất linh động khi di
chuyển trên địa hình phức tạp, mặc dù bản thân robot là một hệ thống không ổn định.
Khi xe di chuyển trên địa hình dốc, nó tự động nghiêng ra trước và giữ cho trọng
lượng dồn về hai bánh chính. Tương tự, khi di chuyển xuống dốc, nó nghiêng ra sau và

lOMoARcPSD|15618161
giữ trọng tâm rơi vào bánh chính. Vì vậy, không bao giờ có hiện tượng trọng tâm xe
rơi ngoài vùng đỡ bánh xe để có thể gây ra lật úp.
Hình 1.4 Xe hai bánh di chuyển trên các địa hình khác nhau theo hướng bảo toàn sự
thăng bằng
Đối với những địa hình lồi lõm, ưu điểm về khả năng thăng bằng và di chuyển
linh hoạt của xe hai bánh sẽ mang lại được ý nghĩa thực tiễn trong giới hạn ổn định
hơn là đối với xe ba bánh truyền thống. Qua đó, xe hai bánh tự cân bằng cũng nhận
được sự quan tâm từ nhiều nhà nghiên cứu và các hãng sản xuất robot trên toàn thế
giới. Nhiều ứng dụng của xe hai bánh tự cân bằng như về phương tiện di chuyển và
bảo vệ môi trường có xe điện hai bánh balancing scooter hay segway; về khả năng tự
hành, phục vụ vận chuyển hàng hoá vật phẩm hay vận chuyển trong môi trường khắc
nghiệt; về nghiên cứu vũ trụ có xe phục vụ do thám các địa hình không phẳng ngoài
không gian,… và những ứng dụng thực tế khác.
1.2 Mục tiêu đề tài
Mục tiêu của đề tài là xây dựng mô hình xe hai bánh tự cân bằng dựa trên nền
tảng lý thuyết mô hình con lắc ngược. Khả năng di chuyển cân bằng trên hai bánh làm
phương tiện di chuyển hiệu quả và linh động hơn, dễ dàng xoay xở trong các không

lOMoARcPSD|15618161
gian chật hẹp. Trong thời gian làm đề tài, những mục tiêu của đề tài được đặt ra như
sau:
- Tìm hiểu các mô hình xe hai bánh tự cân bằng và các nguyên lý cơ bản về cân
bằng.
- Ứng dụng các phương pháp đã học để xây dựng mô hình toán cho hệ xe hai
bánh tự cân bằng tổng quát.
- Tìm hiểu và áp dụng Bộ lọc Kalman để lọc nhiễu cho cảm biến, xây dựng các
thuật toán bù trừ để có giá trị góc chính xác.
- Xây dựng mô hình và mô phỏng trên MATLAB Simulink.
- Xây dựng thuật toán điều khiển động cơ, giữ thăng bằng cho robot.
- Thiết kế, thi công và lập trình điểu khiển cho mô hình xe hai bánh tự cân bằng.
- Thiết kế giao diện để quan sát tín hiệu trả về từ cảm biến của xe.
1.3 Giới hạn đề tài
Trong khuôn khổ đề tài này, chỉ xét và nghiên cứu:
- Mô hình xe hai bánh tự cân bằng sử dụng bộ điều khiển LQR để hệ thống có
chỉ tiêu chất lượng trong miền thời gian thoả yêu cầu chất lượng hệ thống (độ vọt lố,
thời gian quá độ). Tuy nhiên việc lựa chọn bộ tham số LQR để hệ thống đạt được chất
lượng tối ưu nhất hay các kỹ thuật điều khiển khác nằm ngoài phạm vi đề tài.
- Mô hình xe hai bánh tự cân bằng chỉ đứng cân bằng chưa thể di chuyển tiến
lùi, quay vòng hay lên xuống dốc được.
- Mô hình xe kích thước nhỏ ,vùng hoạt động trong không gian hẹp không đặt
lên được các vật thể để xét khả năng di chuyển cân bằng.
- Mô hình xe thực hiện trong đề tài chỉ giao tiếp đọc tín hiệu trả về thông qua
truyền thông kết nối USB trực tiếp với máy tính, chưa thể xét tới việc điều khiển và
truyền nhận tín hiệu từ xa.
1.4 Phương pháp nghiên cứu
Dựa trên những kiến thức thực tế, mô hình toán học của hệ xe hai bánh cân
bằng đã được chỉ rõ. Nhóm tiến hành sử dụng bộ điều khiển phù hợp giúp xe giữ thăng
bằng theo những yêu cầu chất lượng được đặt ra.

lOMoARcPSD|15618161
Xây dựng mô hình lý thuyết xe hai bánh tự cân bằng, mô phỏng trên MATLAB
Simulink và thực hiện tính toán các thông số cho bộ điều khiển LQR.
Xây dựng mô hình thực gồm:
- Thiết kế cơ khí: khung sườn của mô hình.
- Ứng dụng các mạch điện tử, mạch công suất (điều khiển bánh xe).
- Mạch cảm biến (góc, vận tốc, gia tốc) và phương pháp bù trừ giá trị cảm biến
để ước lượng lọc nhiễu.
- Bộ điều khiển trung tâm.
- Lập trình vi xử lý.
1.5 Nội dung đề tài
Phần còn lại của đề tài có nội dung như sau:
Chương 2. Cơ sở lý thuyết
Ở chương 2, mô tả về mô hình toán học của hệ xe hai bánh tự cân bằng

trên địa hình phẳng. Đồng thời nêu cơ sở lý thuyết, kiến thức nền tảng một bộ điều
khiển có thể sử dụng để điều khiển đối tượng.
Chương 3. Thiết kế phần cứng
Trong chương này các giải pháp về phần cứng cho mô hình được xem
xét và lựa chọn phù hợp với yêu cầu hệ thống được đề ra, tiến hành thi công và thực
hiện hoá mô hình hệ xe hai bánh tự cân bằng.
Chương 4. Thiết kế phần mềm
Chương này thực hiện xây dựng thuật toán để lập trình điều khiển và lập
trình giao diện giám sát hệ thống.
Chương 5. Kết quả thực hiện
Trong chương này trình bày các kết quả thực hiện được thông qua mô
phỏng trên MATLAB Simulink và thực nghiệm trên phần mềm giám sát. Đồng thời
đưa ra hạn chế còn gặp của đề tài và hướng phát triển nghiên cứu đề tài trong tương
lại.

lOMoARcPSD|15618161
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT
2.1 Đặc tính động lực học
Xây dựng hệ phương trình trạng thái mô tả hệ thống xe hai bánh tự cân bằng
Hình 2.5 Mô hình xe hai bánh tự cân bằng trên mặt phẳng
Kí hiệu – [đơn vị] Ý nghĩa
m  [kg ] Khối lượng bánh xe
M  [ kg ] Khối lượng robot
R  [ m] Bán kính bánh xe
W  [m] Chiều rộng robot
D  [ m] Chiều sâu robot
H  [ m] Chiều cao robot
L  [ m] Khoảng cách từ trọng tâm robot

đến trục bánh xe
fw Hệ số ma sát giữa bánh xe và mặt
phẳng di chuyển
fm Hệ số ma sát giữa robot và động cơ
DC

lOMoARcPSD|15618161
J m  [kg .m 2 ] Moment quán tính của động cơ DC
Rm  [Ohm] Điện trở của động cơ DC
Kb  [V sec/ rad ] Hệ số EMF của động cơ DC.
K t  [ Nm / A] Moment xoắn của động cơ DC
n Tỉ số giảm tốc
g  [m / s 2 ] Gia tốc trong trường.
  [rad ] Góc trung bình của bánh trái và

phải
l , r  [rad ] Góc của bánh trái và phải
  [ rad ] Góc nghiêng của phần thân robot
  [rad ] Góc xoay của robot
xl , yl , zl  [m] Toạ độ bánh trái
xr , yr , zr  [ m] Toạ độ bánh phải
xm , ym , zm  [m] Toạ độ trung bình
F , F , F  [ Nm] Moment phát động theo các

phương khác nhau
Fl , Fr  [ Nm] Moment phát động của động cơ
bánh trái và phải
il , ir  [ A] Dòng điện động cơ bánh trái và
phải
vl , vr  [V ] Điện áp động cơ bánh trái và phải
Bảng 2.1 Kí hiệu và ý nghĩa các đại lượng
Sử dụng phương pháp Euler-Lagrange để xây dựng mô hình động học. Giả sử
tại thời điểm t = 0, robot di chuyển theo chiều dương trục x, ta có các phương trình
sau:
Góc tịnh tiến trung bình của hai bánh xe và góc xoay của robot được xác định
như sau:

lOMoARcPSD|15618161
1 
 (l   r ) 

  2
    R 
   (   ) 
 W 
l r
222\* MERGEFORMAT (.)
Trong đó toạ độ trung bình của xe trong hệ quy chiếu:
 x 
 xm    m 
 y   y 
 m  m
 zm   R 
  323\* MERGEFORMAT (.)
 x m   R cos  
 y    
 m 
 R sin  
Và 424\* MERGEFORMAT (.)
Toạ độ bánh trái trong hệ quy chiếu:
 W 
 xm  2 sin  
 xl   
y  W
 l  ym  sin  

 2 
 zl   
 zm 
  525\* MERGEFORMAT (.)
Toạ độ bánh phải trong hệ quy chiếu:
 W 
 xm  2 sin  
 xr   
y   W
 ym  sin  
 r  2 
 zr   
 zm 
  626\* MERGEFORMAT (.)
Toạ độ tâm đối xứng giữa hai động cơ trong hệ quy chiếu:
 xb   xm  L sin cos  
 y   y  L sin sin  
 b  m 
 zb   zm  L cos 
Phương trình động năng của chuyển động tịnh tiến:

1 1 1
T1  m( x l 2  y l 2  z l 2 )  m( x r 2  y r 2  z r 2 )  M ( x b 2  y b 2  z b 2 )
2 2 2 828\*
MERGEFORMAT (.)

lOMoARcPSD|15618161
Phương trình động năng của chuyển động quay:

1 1 1 1 1 1
T2  J wl 2  J wr 2  J 2  J  2  n 2 J m (l   ) 2  n2 J m (r   ) 2
2 2 2 2 2 2 929\*
MERGEFORMAT (.)
1 2 1
n J m (l   ) 2 ; n 2 J m (r   ) 2
Với 2 2 10210\* MERGEFORMAT (.)
là động năng quay của phần ứng động cơ trái và phải
Phương trình thế năng:
U mgzl  mgzr  mgzb

Phương trình Lagrange:
L T1  T2  U
d L L
( ) F
dt   13213\* MERGEFORMAT (.)
d L L
( ) F
dt   14214\* MERGEFORMAT (.)
d L L
( ) F
dt   15215\* MERGEFORMAT (.)
Lấy đạo hàm L theo các biến  , ,  ta được:
 (2m  M ) R 2  2 J w  2n 2 J m    ( MLR cos  2n 2 J m )  MLR 2 sin F

16216\*
MERGEFORMAT (.)
 MLR cos  2n J     ML

2
m
2
 J  2n 2 J m    MgL sin
 ML2 2 sin cos F
17217\*
MERGEFORMAT (.)
1 W2 
2 mW 2
 J   2
( J w  n 2 J m )  ML2 sin 2     2 ML2
  sin cos F
 2 R  18218\*
MERGEFORMAT (.)
Moment động lực do động cơ DC sinh ra:

lOMoARcPSD|15618161
 
 F   Fl  Fr 
   
 F   F 
 F   W 
   ( Fr  Fl ) 
 2R  19219\* MERGEFORMAT (.)
Và:
Fl nK t il  f m (  l )  f wl

Fr nK t ir  f m (  r )  f wr

F  nKt il  nKt ir  f m (  l )  f m (  r )

Sử dụng phương pháp PWM để điều khiển động cơ nên chuyển từ dòng điện
sang điện áp động cơ:
Lmil ,r vl , r  K b (  l , r )  Rmil ,r

Xem điện cảm phần ứng tương đối nhỏ (gần bằng 0), có thể bỏ qua, suy ra:
vl ,r  K b (  l ,r )
il ,r 
Rm 24224\* MERGEFORMAT (.)
Từ đó, các moment lực sinh ra:
F a(vl  vr )  2(   f w )  2 

F  a(vl  vr )  2  2

nKt nK K
 ;   t b  fm
Với Rm Rm 27227\* MERGEFORMAT (.)
W W2
F   (vr  vl )   (   f w )
2R 2R2 28228\* MERGEFORMAT (.)
Thu được phương trình động lực học mô tả chuyển động của xe như sau:
 (2m  M ) R 2  2 J w  2n 2 J m    ( MLR cos  2n 2 J m )  MLR 2 sin

 (vl  vr )  2(   f w )  2 29229\*
MERGEFORMAT (.)
10

lOMoARcPSD|15618161
 MLR cos  2n J     ML

2
m
2
 J  2n 2 J m    MgL sin  ML2 2 sin cos
  (vl  vr )  2   2 
1 W2  
2 2 2 2 2
  sin cos
 2 mW  J  2 R 2 ( J w  n J m )  ML sin     2ML 
 
2
W W
  (vr  vl )  (   f w )
2R 2R2 31231\*
MERGEFORMAT (.)
2.2 Một số phương pháp điều khiển có thể sử dụng cho hệ thống
2.2.1 Bộ điều khiển vi tích phân tỉ lệ
Bộ điều khiển PID là bộ điều khiển vòng kín, có sự kết hợp của ba bộ điều
khiển: tỉ lệ, tích phân và vi phân có khả năng điều chỉnh sai số thấp nhất có thể, tăng
tốc độ đáp ứng, giảm độ vọt lố, hạn chế sự dao động. Bộ điều khiển dễ dàng thiết kế
với việc ta chỉ cần chỉnh 3 thông số của bộ PID là Kp, Ki, Kd.
Hàm truyền của bộ điều khiển PID:

KI 1
GPID ( s) K P   K D s K P (1   TDs )
s T1s 32232\* MERGEFORMAT (.)
Về mặt ưu điểm:
- Dễ dàng thiết kế vì ta chỉ điều chỉnh ba thông số Kp, Ki, Kd.
Về mặt nhược điểm:
- Việc ta thiết kế bộ điều khiển PID ta phải tìm ba thông số của PID với hệ
thống MIMO thì ta cần nhiều bộ PID để khiều khiển, với việc ta dùng nhiều bộ PID thì
ta cần phải tìm nhiều thông số. Trong thực tế thì 3 thông số Kp, Ki, Kd ta khó tìm chính
xác được mà phải dựa vào kinh nghiệm.
- Bộ điều khiển PID khi ta thay đổi các thông số cơ bản của hệ thống thì 3
thông số ta tìm được nó không chính xác và độ ổn định của hệ thống bị thay đổi theo.
2.2.2 Bộ điều khiển vi tích phân tỉ lệ kết hợp với giải thuật di truyền
Với bộ điều khiển PID đã giới thiệu trước đó, nó có nhiều ưu điểm như việc tìm
các thống số của bộ điều khiển ta phải dựa vào kinh nghiệm hoặc khi ta thay đổi khối
lượng của vật thì làm cho tính ổn định của hệ thống không còn ổn định.
11

lOMoARcPSD|15618161
Thuật toán giải thuật di truyền GA là thuật toán tối ưu ngẫu nhiên dựa trên cơ
chế chọn lục tự nhiên và tiến hóa di truyền. Được ứng dụng đầu tiền trong 2 lĩnh vực
chính: tối ưu hóa và học tập của máy. Nó được áp dụng vào bộ điều khiển PID nhầm
cải thiện việc khi ta thay đổi các giá trị của hệ thống thì GA sẽ tìm và đưa ra giá trị tối
ưu cho hệ thống.
Về mặt ưu điểm: Giúp cho hệ thống có thể hoạt động được khi ta thay đổi các
giá trị của hệ thống như: khối lượng, kích thước,…
Về mặt nhược điểm: Thời gian tính toán các số liệu dựa nhiều về phần cứng,
kết quả tìm được có thể làm cho RAM của vi điều khiển tràn và không tính được.
2.2.3 Bộ điều khiển mờ
Điều khiển mờ là phương pháp điều khiển bắt chước quá trình xử lý thông tin
không rõ ràng và ra quyết định điều khiển của con người.
Cơ sở toán học của bộ điều khiển mờ được Zadeh đưa ra từ năm 1965. Ngày
nay, nó được sử dụng rộng rãi hơn trong lĩnh vực điều khiển tự động. Các suy luận mờ
được xây dựng nhờ vào lối suy luận logic của con người.
Một bộ điều khiển mờ cơ bản có cấu trúc như sau:
Hình 2.6 Cấu trúc cơ bản của bộ điều khiển mờ

- Tiền xử lý: chuẩn hoá hoặc tỷ lệ giá trị đo vào tầm giá trị chuẩn, lọc nhiễu.
- Mờ hóa: chuyển giá trị rõ phản hồi từ ngõ ra của đối tượng thành giá trị mờ để
hệ quy tắc có thể suy luận được.
- Hệ quy tắc mờ: rút ra từ kinh nghiệm chuyên gia trong việc điều khiển đối
tượng.
- Phương pháp suy diễn: từ hệ quy tắc được xây dựng sẽ có các phương pháp để
tìm ngõ ra tương ứng qua khối giải mờ .
12

lOMoARcPSD|15618161
- Giải mờ: chuyển giá trị mờ suy luận được ở ngõ ra của hệ quy tắc thành các
giá trị rõ để điều khiển đối tượng.
- Hậu xử lý: tỷ lệ giá trị sau khi giải mờ thành giá trị vật lý để điều khiển hệ
thống.
- Dễ thiết kế vì tập mờ và các mệnh đề mờ được xây dựng dựa trên lối suy luận
của con người.
- Tính linh hoạt cao.
- Vì được ra đời lâu nên các luật mờ có thể dễ tìm được trên mạng và thay đổi
chúng cho phù hợp với hệ thống thực tế.
- Có thể liên kết với bộ điều khiển PID để tăng tính ổn định của hệ thống.
Nhược điểm:
- Nếu lối suy luận của ta sai thì kết quả cho ra có thể sai.
- Quá trình giải mờ tùy vào việc ta lựa chọn mô hình nào có thể khiến độ chính
xác của kết quả cho được không đúng.
- Với hệ thống phức tạp nhiều đầu vào nhiều đầu ra thì ta cần nhiều tập cơ sở
khác nhau và các luật mờ khác nhau. Điều này có thể khiến cho hệ thống rắc rối hơn
để thiết kế.
- Do suy luận để thiết kế nên không nắm rõ được bản chất, mô hình hệ thống.
- Đòi hỏi vi điều khiển có tốc độ xử lý nhanh, tốt và bộ nhớ cao.
2.2.4 Bộ điều khiển tối ưu
Một hệ điều khiển được thiết kế ở chế độ làm việc tốt nhất là hệ thống luôn ở
trạng thái tối ưu theo một tiêu chuẩn chất lượng nào đó (đạt được giá trị cực trị).
Có nhiều bài toán điều khiển tối ưu, tuỳ theo:
- Loại đối tượng điều khiển.
- Miền thời gian liên tục hay rời rạc.
- Chỉ tiêu chất lượng.
- Bài toán tối ưu có ràng buộc hay không.
13

lOMoARcPSD|15618161
Có hai phương pháp điều khiển tối ưu chính:
- Điều khiển tối ưu tĩnh: chỉ tiêu chất lượng không phụ thuộc thời gian.
- Điều khiển tối ưu động: chỉ tiêu chất lượng phụ thuộc thời gian, có các
phương pháp sau:
+ Bài toán chỉnh toàn phương tuyến tính (Linear Quadratic Regulator – LQR).
+ Bài toán điều khiển tối ưu H2.
+…
Khảo sát ưu và nhược điểm của bộ điều khiển tối ưu sử dụng phương pháp toàn
phương tuyến tính (LQR) như sau:
- Hệ thống vẫn ổn định ngay cả khi ta thay đổi đặc tính trên hệ thống.
- Có thể thiết kế với các hệ thống phi tuyến phức tạp.
- Kết quả cho ra tính ổn định cao.
Về mặt nhược điểm:
- Tính toán ma trận của bộ điều khiển LQR rất phức tạp.
- Việc lựa chọn ma trận trọng lượng thích hợp ở chỉ tiêu chất lượng rất quan
trọng vì nó ảnh hưởng rất nhiều đến kết quả tính hoá.
- Nếu chịu ảnh hưởng của nhiễu hệ thống và nhiễu đo lường tác động vào hệ
thống thì sẽ gây khó khăn khi cân bằng, tín hiệu điều khiển không ổn định nhiều gai
gây ảnh hưởng tới động cơ.
Qua phân tích về ưu nhược điểm của từng bộ điều khiển trình bày ở trên thì
nhóm quyết định xây dựng và mô phỏng hệ xe hai bánh tự cân bằng sử dụng bộ điều
khiển toàn phương tuyến tính (LQR) để thực hiện khảo sát trong đề tài.
14

lOMoARcPSD|15618161
2.3 Điều khiển toàn phương tuyến tính
2.3.1 Bộ lọc Kalman
Hình 2.7 Sơ đồ khối của bộ lọc Kalman

Xét hệ tuyến tính liên tục:
 x (t )  Ax(t )  Bu (t )  w(t )

 y (t ) Cx(t )  v(t ) 33233\* MERGEFORMAT (.)
Trong đó: w(t ) là nhiễu hệ thống; v(t ) là nhiễu đo lường.
Giả sử nhiễu hệ thống và nhiễu đo lường có phân bố Gauss, không tương quan,
có trung bình bằng 0 và phương sai là:
E[ wwT ] QN
E[vvT ] RN
Bộ lọc Kalman liên tục:
 xˆ (t ) [Axˆ (t )  Bu (t )]  L[ y (t )  yˆ (t )]


 yˆ (t ) Cxˆ (t )
 36236\* MERGEFORMAT (.)
Trong đó L là độ lợi của bộ lọc Kalman:
L  C T RN 1
Với  là nghiệm của phương trình Ricatti:
A   AT   C T RN 1C QN 0
Lời giải bộ lọc Kalman liên tục trên MATLAB: (với G là ma trận đơn vị)
L lqe( A, G, C , QN , RN ) 39239\* MERGEFORMAT (.)
15

lOMoARcPSD|15618161
2.3.2 Bài toán điều khiển toàn phương tuyến tính liên tục
Hình 2.8 Sơ đồ khối của bộ điều khiển toàn phương tuyến tính (LQR)
Trong đề tài này, chỉ xét bài toán LQR liên tục thời gian vô hạn.
Đối tượng tuyến tính mô tả bởi phương trình trạng thái:

x (t )  Ax(t )  Bu (t ) 40240\* MERGEFORMAT (.)
Chỉ tiêu chất lượng dạng toàn phương, trong đó thời điểm cuối tiến đến vô
cùng:

1
J (u )  [xT (t )Qx(t )  uT (t ) Ru (t )]dt
20
Tín hiệu điều khiển tối ưu:
u * (t )  Kx(t ) 42242\* MERGEFORMAT (.)
Trong đó:
K R  1 B T P 43243\* MERGEFORMAT (.)
Và P là nghiệm bán xác định dương của phương trình đại số Ricatti:
PA  AT P  Q  PBR  1BT P 0 44244\* MERGEFORMAT (.)
Chú ý: trong trường hợp này K và P là không phụ thuộc vào thời gian
Giá trị cực tiểu của chỉ tiêu chất lượng:
J min xT (0) Px (0)

Lời giải bộ điều khiển toàn phương tuyến tính liên tục:
K lqr ( A, B, Q, R) 46246\* MERGEFORMAT (.)
16

lOMoARcPSD|15618161
2.3.3 Tuyến tính hoá hệ thống
Từ 229, 230, 231 ta có hệ phương trình mô tả hệ thống xe hai bánh tự cân bằng
có dạng như sau:
 x1  f1 ( x1 , x2 , x3 , x4 , x5 , x6 )  x2
 x  f ( x , x , x , x , x , x )
 2 2 1 2 3 4 5 6
 x 3  f3 ( x1 , x2 , x3 , x4 , x5 , x6 ) x4

 x 4  f 4 ( x1 , x2 , x3 , x4 , x5 , x6 )
 x 5  f5 ( x1 , x2 , x3 , x4 , x5 , x6 )  x6

 x 6  f 6 ( x1 , x2 , x3 , x4 , x5 , x6 ) 47247\* MERGEFORMAT (.)
Với:
 x1    
 x    
 2  
 x   
x  3   
 x4   
 x5    
   
 x6     48248\* MERGEFORMAT (.)
v 
u  l 
và  vr  49249\* MERGEFORMAT (.)
Nếu chọn điểm cân bằng là:
 0
 0
 
 0
x0  
 0
 0
 
 0 50250\* MERGEFORMAT (.)
 0
u0   
 0 51251\* MERGEFORMAT (.)
Ta có thể tuyến tính hoá hệ thống 247 về dạng:

x  Ax  Bu 52252\* MERGEFORMAT (.)
Suy ra:
17

lOMoARcPSD|15618161
 f1 f1 f1 f1 f1 f1 

 
 x1 ux ux00 x2 x x0
u u0
x3 x x0
u u0
x4 x x0
u u0
x5 x x0
u u0
x6 x x0 
u u0
 
 f 2 f 2 f 2 f 2 f 2 f 2 
 
 x1 ux ux00 x2 x x0
u u0
x3 x x0
u u0
x4 x x0
u u0
x5 x x0
u u0
x6
u u0 
x x0
 
 f 3 f 3 f 3 f3 f3 f3 
 x x2 x3 x4 x5 x6 
 1 ux ux00 x x0
u u0
x x0
u u0
x x0
u u0
x x0
u u0
x x0
u u0 
A  
 f 4 f 4 f 4 f 4 f 4 f 4 
 x x x x2 x x0 x3 x x0 x4 x x0 x5 x x0 x6 x x0

 1 u u00 u u0 u u0 u u0 u u0 u u0 
 
 f 5 f 5 f 5 f 5 f 5 f 5 
 x x x0 x2 x x0 x3 x x0 x4 x x0 x5 x x0 x6 x x0

 1 u u0 u u0 u u0 u u0 u u0 u u0 
 
 f 6 f 6 f 6 f 6 f 6 f 6 
 x1 x x0 x2 x x0 x3 x x0 x4 x x0 x5 x x0 x6 x x0 
 u u0 u u0 u u0 u u0 u u0 u u0 
0 1 0 0 0 0
0 A A2 A3 0 0 
 1
0 0 0 1 0 0
A  
 0 A4 A5 A6 0 0
0 0 0 0 0 1
 
 0 0 0 0 0 A7 
53253\*
MERGEFORMAT (.)
trong đó:
2 J   2 J f w  2 L2 M   2 L2 Mf w  4 J m f w n 2  2 LMR 
A1 
Am 54254\*
MERGEFORMAT (.)
RgL2 M 2  2 J m gLMn 2
A2 
Am 55255\* MERGEFORMAT (.)
2 M  L2  2 MR L  2 J 
A3 
4 J w   2MR 2   4 J m fwn 2  4 R 2  m  2 LMR   2 LMRf w

A4 
Am 57257\*
MERGEFORMAT (.)
18

lOMoARcPSD|15618161
LgM 2 R 2  2 LgmMR 2  2 J m LgMn 2  2 J w LgM

A5 
4 J w   2MR 2   4 R 2  m  2 LMR 
A6 
W 2 W 2 fw
A7 
mR 2W 2  2 J  R 2  J mW 2 n 2  J wW 2
Am 2 J J w  2 J w L2 M  J MR 2  2 J J m n 2  4 J m J w n 2  2 J R 2 m  2 J m L2 Mn 2
2 J m MR 2 n 2  2 L2 MR 2 m  4 J m R 2 mn 2  4 J m LMRn 2 61261\*
MERGEFORMAT (.)
Và
 f1 f1 
 
 vl x x0
u u0
vr x x0 
u u0
 
 f 2 f 2 
 
 vl x x0
u u0
vr
u u0 
x x0
   0 0
 f 3 f 3   B B1 
 v vr   1
x x0 x x0
 l u u0 u u0   0 0
B    
 f 4 f 4   B2 B2 
 v x x0 vr x x0
  0 0
 l u u0 u u0   
    B3 B3 
 f 5 f5 
 v x x0 vr x x0

 l u u0 u u0 
 
 f 6 f 6 
 vl x x0 vr x x0 
 u u0 u u0 
trong đó:
 ( ML2  MRL  J )
B1 
Bm 63263\* MERGEFORMAT (.)
  (2 J w  MR 2  2 R 2 m  LMR)
B2 
Bm 64264\* MERGEFORMAT (.)
RW 
B3 
mR W  2 J  R 2  J mW 2 n 2  J wW 2
2 2
19

lOMoARcPSD|15618161
Bm 2 J J w  2 J w L2 M  J MR 2  2 J J m n 2  4 J m J w n 2  2 J R 2 m  2 J m L2 Mn 2
2 J m MR 2 n2  2 L2 MR 2 m  4 J m R 2 mn 2  4 J m LMRn 2 66266\*
MERGEFORMAT (.)
2.3.4 Khảo sát tính điều khiển được và quan sát được của hệ thống
Ma trận C quan sát được ba biến trạng thái là góc tịnh tiến trung bình của hai
bánh xe  , góc nghiêng của robot  , góc xoay của robot  được thể hiện như sau:
 1 0 0 0 0 0
C  0 0 1 0 0 0 
 0 0 0 0 1 0 
2 3 4 5
Ma trận điều khiển M c [ B, AB, A B, A B, A B, A B ]
2 3 4 5 T
Ma trận quan sát M o [C , CA, CA , CA , CA , CA ]
rank ( M c ) rank (M o ) 6
Kiểm tra, ta được .
Suy ra hệ thống điều khiển được và quan sát được.
2.3.5 Hàm chỉ tiêu chất lượng
Mục tiêu điều khiển là tìm vector điều khiển tối ưu K thoả mãn chỉ tiêu chất
lượng J đạt cực tiểu:

J ( xT Qx  uT Ru )dt
0 67267\* MERGEFORMAT (.)
Lúc này ta có các ma trận trọng số như sau:
 Q1 0 0 0 0 0
0 Q 0 0 0 0
 2 
0 0 Q3 0 0 0
Q  
0 0 0 Q4 0 0
0 0 0 0 Q5 0
 
 0 0 0 0 0 Q6 
R 0
R  1
0 R2 
20

lOMoARcPSD|15618161
Q1 , Q2 , Q3 , Q4 , Q5 , Q6 , R1 , R2
Với các thông số để tinh chỉnh cho bộ điều khiển.
Q1 , Q2 , Q3 , Q4 , Q5 , Q6
Trong đó, thông số lần lượt là các trọng số tối ưu cho các giá trị
góc trung bình của bánh trái và bánh phải  , vận tốc góc trung bình của bánh trái và
bánh phải  , góc nghiêng của robot  , vận tốc góc nghiêng của robot  , góc xoay
 R ,R
của robot  , vận tốc góc xoay của robot  và thông số 1 2 là trọng số tối ưu cho tín
vl vr
hiệu điều khiển cho bánh trái và tín hiệu điều khiển cho bánh phải . Nếu muốn
biến trạng thái nào được ưu tiên trong việc điều khiển thì ta cho thông số trọng số
tương ứng đó lớn hơn tương đối so với các trọng số còn lại trong hai ma trận Q và R .
Sau khi chọn được thông số điều khiển tương ứng thì ta tiến hành xây dựng
thông số hồi tiếp K với tín hiệu điều khiển u  Kx(t ) . Thông số K được tính dựa vào
phương trình đại số 243.
2.4 Sơ đồ mô phỏng hệ thống trên MATLAB Simulink
Mô phỏng hệ thống với thông số mô hình như sau:
Kí hiệu - [đơn Giá trị

vị]
m  [kg ] 0.044
M  [kg ] 0.92
R  [ m] 0.0267
W  [ m] 0.145
D  [ m] 0.055
H  [ m] 0.185
L  [ m] 0.1
fw 0
fm 0.0022
J m  [kg .m 2 ] 1.4762×10-5
Rm  [Ohm] 40
K b  [V sec/ rad ] 0.0218
K t  [ Nm / A] 0.0197
n 30
g  [m / s ] 2
9.81
Bảng 2.2 Thông số mô hình
21

lOMoARcPSD|15618161
Sau khi tuyến tính hoá hệ thống ta được hệ phương trình trạng thái dạng
x  Ax  B với:
0 1 0 0 0 0 
 0  0.1549 45.3824 0.1549 0 0 

0 0 0 1 0 0 
A  
 0 0.0337 51.4164  0.0337 0 0 
0 0 0 0 0 1 
 
0 0 0 0 0  0.1874  70270\*
MERGEFORMAT (.)
 0 0 
 0.4536 0.4536 
 
 0 0 
B  
  0.0988  0.0988
 0 0 
 
  0.2022 0.2022  71271\*
MERGEFORMAT (.)
Xây dựng khối mô tả hệ thống với tham số mô hình như trên trong MATLAB
Function:
Hình 2.9 Khối Two-wheeled Self Balancing Robot trong MATLAB Function
Trường hợp 1: Khi không điều khiển và hệ thống chịu tác động của nhiễu:
22

lOMoARcPSD|15618161
Hình 2.10 Sơ đồ mô phỏng hệ thống trong trường hợp 1

Trường hợp 2: Khi sử dụng bộ điều khiển LQR và hệ thống không chịu tác
động của nhiễu:

Trường hợp 3: Khi sử dụng bộ điều khiển LQR và hệ thống chịu tác động của
nhiễu:
23

lOMoARcPSD|15618161

Trường hợp 4: Khi sử dụng bộ điều khiển LQR kết hợp với bộ lọc Kalman để
lọc nhiễu cho hệ thống:

723Equation Section (Next)
24

lOMoARcPSD|15618161
CHƯƠNG 3: THIẾT KẾ PHẦN CỨNG
3.1 Yêu cầu thiết kế
Để thiết kế phần cứng đảm bảo cho xe hoạt động cần lựa chọn các thiết bị phù
hợp với các yêu cầu sau:
- Đảm bảo tổng khổi lượng của xe để động cơ có thể kéo cân bằng được.
- Nguồn cung cấp đủ cho hệ thống hoạt động.
- Khung xe chắc chắn.
- Cảm biến trả tín hiệu về tránh tối đa nhiễu và sai số.
- Khảo sát hoạt động trong thời gian ngắn.
- Động cơ moment đủ để kéo tải.
- Vi điều khiển đầy đủ chuẩn giao tiếp với máy tính và các cảm biến, đảm bảo
xử lý tín hiệu nhanh.
3.2 Sơ đồ khối hệ thống
Sơ đồ khối mô tả hệ thống được chia thành các thành phần sau:
- Bộ khung xe.
- Tầng đáy: Khối động cơ và cảm biến đọc xung của động cơ.
- Tầng thứ nhất: Khối điều khiển động cơ.
- Tầng thứ hai: Khối điều khiển trung tâm và Khối cảm biến đo góc nghiêng.
- Tầng thứ ba: Khối nguồn.
25

lOMoARcPSD|15618161
Hình 3.14 Sơ đồ khối hệ thống

Chức năng của từng khối từng khối:
- Bộ khung xe: cố định từng khối thành phần của xe với nhau, lựa chọn kích
thước chiều dài chiều rộng phù hợp để có thể chứa vừa các khối thành phần đã được
nêu và dễ dàng cân bằng được.
- Khối cảm biến đo góc nghiêng có nhiệm vụ đọc dữ liệu trả về và gửi tín hiệu
đó về Khối điều khiển trung tâm xử lý ra giá trị góc nghiêng của xe.
- Khối điều khiển trung tâm là một vi điều khiển, có nhiệm vụ chính là xử lý tất
cả dữ liệu đọc về từ Khối đo góc nghiêng và Khối cảm biến đọc xung của động cơ, xử
lý các thuật toán điều khiển đưa ra tín hiệu để điều khiển Khối điều khiển động cơ.
- Khối điều khiển động cơ: nhiệm vụ là nhận tín hiệu từ Khối điều khiểu trung
tâm để cấp điện áp ra phù hợp để điều khiển Khối động cơ.
- Khối động cơ và cảm biến đọc xung của động cơ: nhận điện áp cấp từ Khối
điều khiển động cơ điều khiển xe di chuyển qua lại, nhanh, chậm, dừng để cho xe có
thể đạt được trạng thái cân bằng. Ngoài ra còn có một bộ mã hoá quay encoder để trả
về tín hiệu số xung đọc được, từ đó qua Khối điều khiển trung tâm xử lý có thể xác
định chính xác được vị trí, tốc độ quay và chiều quay động cơ.
26

lOMoARcPSD|15618161
- Khối nguồn: cấp nguồn ổn định cho Khối điều khiển trung tâm, Khối điều
khiển động cơ.
3.3 Lựa chọn thiết bị phần cứng
3.3.1 Khối điều khiển trung tâm
Khối điều khiển trung tâm là một vi điều khiển có nhiệm vụ xử lý dữ liệu đọc
về từ cảm biến đo góc nghiêng và bộ mã hoá quay encoder từ đó xử lý các thuật toán
về điều khiển xuất ra tín hiệu đến Khối điều khiển động cơ.
Cấu tạo chung của vi điều khiển bao gồm: Vi xử lý được kết nối với các port
ngoại vi, các chân I/O, chân Analog đo từ giá trị 0-5V, port truyền thông bằng giao
thức RS232, các chức năng ngắt ngoại/nội để kết nối đọc chân encoder, timer/counter,
giao thức I2C để truyền/nhận dữ liệu nối tiếp đến vi điều khiển xử lý, chân cấp nguồn,
ROM, RAM, EPROM,…
Khối điều khiển trung tâm phải có tốc độ xử lý nhanh với hiệu suất làm việc
cao. Một số Board vi điều khiển có thể xử dụng như Arduino Uno R3, STM32F4,
Rasberry, họ vi điều khiển PIC,…
Để thực hiện đề tài này chúng tôi sử dụng Board Arduino Mega 2560 Rev3 với
các ưu điểm sau:
- Có số lượng chân I/O kết nối nhiều hơn so với Arduino Uno R3 có thể thoải
mái trong việc đi dây hệ thống.
- Tốc độ xử lý phù hợp, có nhiều chân ngắt ngoại để đọc tín hiệu từ bộ mã hoá
quay encoder trả về để điều khiển chính xác tốc độ, vị trí và chiều quay của động cơ.
- Có giao tiếp I2C để đọc tín hiệu trả về giá trị góc nghiêng từ cảm biến góc
nghiêng IMU.
- Giá thành rẻ, thư viện hỗ trợ nhiều và cộng đồng người sử dụng lớn nên có thể
dễ dàng lập trình và tìm kiếm thông tin nhanh chóng.
- Có nhiều chân hỗ trợ xuất xung PWM.
- Có nhiều tính năng nổi bật, chứa mọi thứ để hỗ trợ vi điều khiển.
- Có nguồn cấp tương thích với các loại board mạch ngoại vi hỗ trợ khác nhau
như tín hiệu mức cao (5V) hoặc tín hiệu mức thấp (3.3V).
27

lOMoARcPSD|15618161
- Hỗ trợ nhiều chuẩn giao tiếp (đặc biệt là UART có giao tiếp với máy tính
thông qua USB hoặc thông qua bluetooth và I2C để giao tiếp với cảm biến góc
nghiêng IMU).
- Dễ dàng cấp nguồn từ USB từ máy tính hoặc từ các nguồn ngoài như pin,
adapter AC-DC,…
Thông số kỹ thuật:
- Vi điều khiển chính: ATmega2560.
- IC nạp và giao tiếp UART: ATmega16U2.
- Nguồn cấp: 5VDC từ cổng USB hoặc nguồn ngoài cắm từ giắc tròn DC
(9VDC).
- Số chân đọc và ghi tín hiệu số (Digital I/O): 54 (trong đó 15 chân có khả năng
xuất xung PWM).
- Số chân đọc tín hiệu tương tự (Analog Input) : 16.
- Cường độ dòng điện dòng điện trên mỗi chân I/O: 20mA.
- Cường độ dòng điện trên mỗi chân 3.3V: 50mA.
- Flash Memory: 256 KB trong đó 8 KB sử dụng cho bootloader.
- SRAM: 8 KB.
- EEPROM: 4 KB.
- Số cổng UARTs giao tiếp nối tiếp phần cứng: 4.
- Một thạch anh dao động với tần số 16 MHz.
- Một cổng kết nối USB, một jack nguồn, một đầu ICSP, một nút Reset, một
đèn LED tích hợp ở chân 13.
- Giao tiếp: USB, ICSP, SPI, I2C và UART.
- Kích thước: 101.52 x 53.3 mm.
- Khối lượng: 37g.
28

lOMoARcPSD|15618161
Hình 3.15 Sơ đồ chân kết nối của Arduino Mega 2560 Rev3
3.3.2 Khối cảm biến đo góc nghiêng
Khối cảm biến đo góc nghiêng là một module gồm hai cảm biến đó là:
- Accelerometer: cảm biến đo gia tốc của module (bao gồm cả gia tốc trọng
lực), có ba trục xyz tương ứng với ba chiều không gian.
- Gyroscope: cảm biến đo tốc độ quay của module quanh một trục, có ba trục
xyz tương ứng với ba chiều không gian.
Khối điều khiển trung tâm sẽ nhận tín hiệu trả về từ hai cảm biến này và sẽ xử
lý tính toán ra được giá trị góc nghiêng của xe.
Đối với yêu cầu hệ thống, chỉ để xác định giá trị góc nghiêng của xe thì chúng
tôi lựa chọn ra được loại cảm biến GY-521 6DOF IMU MPU6050 với các ưu điểm và
nhược điểm sau:
- Về ưu điểm:
+ Là loại cảm biến phổ biến nhất thị trường với giá thành hợp lý.
29

lOMoARcPSD|15618161
+ Có tích hợp sẵn phần cứng chuyên xử lý tín hiệu (Digital Motion Processor –
DMP) do cảm biến thu thập và thực hiện các tính toán cần thiết giúp cải thiện tốc độ
xử lý, phản hồi nhanh hơn và giảm bớt đáng kể phần xử lý tính toán của vi điều khiển.
+ Giao tiếp I2C phù hợp với Khối điều khiển trung tâm đã chọn.
+ Có nhiều nguồn thư viện để hỗ trợ trong việc xử lý tín hiệu và lập trình.
Về nhược điểm:
+ Tín hiệu trả về từ hai cảm biến có sai số trên mỗi trục làm lệch đi các giá trị
đo so với thực tế.
+ Cả hai cảm biến đo gia tốc và tốc độ quay đều chịu ảnh hưởng của nhiễu.
=> Để có thể khắc phục nhược điểm này thì chúng tôi dùng bộ lọc Kalman xử
lý các thuật toán để ước lượng trạng thái trả về từ hai cảm biến chính xác nhất. Bộ lọc
Kalman được chúng tôi sử dụng đã được viết sẵn dưới dạng thư viện cho vi điều khiển
(https://github.com/TKJElectronics/KalmanFilter).
- Gồm 6 trục cảm biến (6DOF): cảm biến gia tốc (3 trục), cảm biến con quay
hồi chuyển (3 trục).
- Bộ chuyển đổi tương tự sang số (ADC): 16 bit.
- Điện áp sử dụng: 3-5 VDC.
- Điện áp giao tiếp: 3-5 VDC.
- Chuẩn giao tiếp: I2C.
- Giá trị Gyroscope trong khoảng: ±250, ±500, ±1000, ±2000 o/sec.
- Giá trị Accelerometer trong khoảng: ±2, ±4, ±8, ±16 g.
30

lOMoARcPSD|15618161
Hình 3.16 Cảm biến góc nghiêng GY-521 6DOF IMU MPU6050
Hình 3.17 Sơ đồ nối dây cảm biến với Arduino Mega 2560
Chân Kết nối và chức năng
VCC Chân cấp nguồn dương.
GND Chân cấp GND.
SCL Serial Clock Line
SDA Serial Data Line
XDA Chân dữ liệu (kết nối với cảm biến
khác)
XCL Chân xung (kết nối với cảm biến
khác)
ADO Bit0 của địa chỉ I2C
INT Chân ngắt
Bảng 3.3 Bảng kết nối chân của cảm biến MPU6050
3.3.3 Khối điều khiển động cơ
Khối điều khiển động cơ nhận tín hiệu PWM (điều chế độ rộng xung) từ Khối
điều khiển trung tâm để dễ dàng điều khiển hai động cơ bánh trái và bánh phải quay
31

lOMoARcPSD|15618161
thuận, quay nghịch, dừng hoặc theo tốc độ nhanh, chậm để xe dễ dàng đạt trạng thái
cân bằng.
Điều chế độ rộng xung (PWM) là một kỹ thuật điều chế được sử dụng để mã
hóa một thông điệp thành một tín hiệu xung. Mặc dù kỹ thuật điều chế này có thể được
sử dụng để mã hóa thông tin để truyền tải, việc sử dụng chính của nó là cho phép điều
khiển nguồn điện cung cấp cho các thiết bị điện, đặc biệt là để tải quán tính như động
cơ.
Giá trị trung bình của điện áp (và dòng điện) cung cấp cho tải được kiểm soát
bằng cách thay đổi việc đóng cắt giữa nguồn và tải tắt với tốc độ rất nhanh. Thời gian
đóng càng lâu so với thời gian cắt, thì tổng công suất cung cấp cho tải càng cao.
Ở đây, chúng tôi lựa chọn mạch điều khiển động cơ DC L298N (Dual Full
Bridge Driver) với các ưu điểm sau:
- Dễ dàng sử dụng, dễ dàng lắp đặt, chi phi thấp, có hiệu năng cao.
- Một module có khả năng điều khiển hai động cơ.
- Tích hợp IC nguồn 7805 giúp cấp nguồn 5VDC cho các module khác (phù
hợp để cấp nguồn cho Khối cảm biến tốc độ động cơ sử dụng encoder).
Nhược điểm:
- Sử dụng trong một thời gian dài sẽ làm module nóng lên gây ra hao phí công
suất .
- Hiệu suất không đạt được tối đa.
=> Trong giới hạn điều khiển này thì chỉ cần điều khiển xe trong một thời gian
ngắn để quan sát được xác lập của xe.
- IC chính: L298 - Dual Full Bridge Driver.
- Điện áp đầu vào: 5 - 12VDC.
- Công suất tối đa mỗi cầu H: 25W.
- Dòng tối đa cho mỗi cầu H: 2A.
- Dòng tín hiệu điều khiển: 0 – 36mA.
- Điện áp của tín hiệu điều khiển: 5 – 7 VDC.

32

lOMoARcPSD|15618161
- Mức điện áp logic: Low -0.3V - 1.5V, High: 2.3V – Vss.
- Công suất hao phí: 20W khi nhiệt độ IC ở 75oC.
- Kích thước: 43x43x27mm.
Hình 3.18 Mạch cầu H điều khiển động cơ DC L298N (Dual Full Bridge Driver)
12V Chân cấp nguồn dương (9 – 12VDC)
GND Chân cấp GND (nối chung với GND Arduino)
5V Chân nguồn ra 5V
ENA Chân điều khiển mạch cầu H động cơ 1 (cấp xung PWM
cho động cơ 1)
IN1 Chân nhận tín hiệu điều khiển động cơ 1
ENB Chân điều khiển mạch cầu H động cơ 2 (cấp xung PWM
cho động cơ 2)
OUT1 Chân nối với chân dương M1 động cơ 1
OUT2 Chân nối với chân âm M2 động cơ 1
OUT3 Chân nối với chân dương M1 động cơ 2
OUT4 Chân nối với chân âm M2 động cơ 2
Bảng 3.4 Bảng kết nối chân của mạch cầu H L298N
3.3.4 Khối động cơ và cảm biến đọc xung của động cơ
Khối động cơ và cảm biến đọc xung của động cơ gồm:
- Hai động cơ bên phải và bên trái là cơ cấu chấp hành chính của xe để điều
khiển xe cân bằng.
- Hai bộ mã hoá quay encoder gắn liền với mỗi động cơ giúp hồi tiếp xung về
Khối điều khiển trung tâm từ đó xử lý tín hiệu giúp xác định chính xác vị trí, tốc độ
quay và chiều quay của từng động cơ.
33

lOMoARcPSD|15618161
Vì yêu cầu đặt ra là động cơ gắn sẵn bộ mã hoá quay encoder và động cơ kéo
tải tầm 700g nên chúng tôi đã lựa chọn động cơ DC Servo GM25-370 DC Geared
Motor loại 12VDC 250 vòng/phút với ưu điểm như sau:
- Động cơ tích hợp thêm Encoder hai kênh A,B.
- Cấu tạo bằng kim loại cho độ bền và ổn định cao.
- Hộp giảm tốc của động cơ có nhiều tỉ số truyền giúp dễ dàng lựa chọn giữa
lực kéo và tốc độ.
- Động cơ được sử dụng nguyên liệu chất lượng cao với lõi dây đồng nguyên
chất, lá thép 407, nam châm có độ từ tính mạnh,… cho sức mạnh và độ bền vượt trội
với các loại giá rẻ khác trên thị trường.
- Động cơ:
+ Điện áp cấp cho động cơ hoạt động: 3 - 12VDC.
+ Đường kính động cơ: 25mm.
+ Đường kính trục: 4mm.
+ Tỉ số truyền 34:1 (động cơ quay 34 vòng trục chính hộp giảm tốc quay 1
vòng).
+ Dòng không tải: 150mA.
+ Dòng chịu đựng tối đa khi có tải: 750mA.
+ Tốc độ không tải: 250 vòng/phút.
+ Tốc độ chịu đựng tối đa khi có tải: 140 vòng/phút.
+ Lực kéo Moment định mức: 4.3 kg.cm.
+ Lực léo Moment tối đa: 5.2 kg.cm.
+ Chiều dài hộp số L: 21mm.
- Encoder:
+ Điện áp cấp cho Encoder hoạt động: 3.3 – 5 VDC.
+ Cảm biến từ trường Hall, có 2 kênh AB lệch nhau.
+ Đĩa Encoder trả 11 xung/ 1 kênh/ 1 vòng.
34

lOMoARcPSD|15618161
+ Số xung Encoder mỗi kênh trên 1 vòng quay trục chính: 11 x 34 = 374 xung.
Hình 3.19 Động cơ DC Servo GM25-370 DC Geared Motor

M1 (đỏ) Dây cấp nguồn dương cho động cơ
GND (đen) Dây cấp nguồn âm cho động cơ
C1/A (vàng) Tín hiệu trả xung kênh A
C2/B (xanh lá) Tín hiệu trả xung kênh B
VCC (xanh Dây cấp nguồn cho Encoder
dương)
M2 (trắng) Dây cấp nguồn âm cho động cơ
Bảng 3.5 Bảng kết nối chân của động cơ và encoder
3.3.5 Khối cấp nguồn
Cung cấp nguồn ổn định cho các khối giúp hệ thống hoạt động tốt. Có thể cấp
nguồn cho hệ thống bằng nhiều cách như pin, nguồn xung tổ ong hoặc adapter chuyển
đổi AC-DC,…
Lựa chọn khối cấp nguồn gồm:
- Nguồn gồm 3 pin Panasonic Lithium ion IRC18650.
- Hộp đế pin 3 cell 18650.
- Mạch giảm áp chuyển đổi DC – DC LM2596 3A.
- Công tắc bập bênh 10x15 mm.
- Jack DC đực có dây.
Ưu điểm của khối cấp nguồn đã được chọn:
- Khi sử dụng pin kết hợp với mạch giảm áp sẽ giúp ổn định được một mức
điện áp cố định cấp cho hệ thống, phòng trường hợp sụt áp khi pin cạn.
- Sử dụng được linh động, gọn nhẹ hơn so với dùng nguồn tổ ong hay adapter.
35

lOMoARcPSD|15618161
- Lắp đặt, đi dây dễ dàng, không bị vướng dây nguồn khi xe chạy.
- Dễ dàng sử dụng, thay và sạc pin cho nguồn.
- Sử dụng công tắc giúp thuận tiện hơn trong việc đóng/tắt hệ thống.
- Sử dụng giắc DC đực thuận tiện trong việc cấp nguồn cho vi điều khiển.
- Một phần giúp tăng khối lượng của xe khiến xe dễ dàng cân bằng hơn.
- Mạch hạ áp LM2596 có kích thước nhỏ gọn, có khả năng giảm áp từ 30VDC
xuống 1.5VDC nhưng vẫn đạt được hiệu suất cao, dòng ra 3A đảm bảo dòng qua các
khối của hệ thống.
- Ba pin Panasonic Lithium ion IRC18650:
+ Lõi pin Li-on MH1350.
+ Điện áp cung cấp mỗi pin: 4.2 VDC (Ba pin thì tổng điện áp cung cấp là
4.2×3 = 12.6VDC).
+ Dung lượng mỗi pin: 4200 mAh.
- Mạch giảm áp LM2596 3A:
+ Điện áp đầu vào: Từ 3V đến 30V.
+ Điện áp đầu ra: có chiết áp điều chỉnh được trong khoảng 1.5V đến 30V.
+ Tần số xung: 65 KHz.
+ Dòng đáp ứng tối đa là 3A.
+ Hiệu suất: 92%.
+ Công suất: 15W.
+ Kích thước: 45×20×14 mm (dài×rộng×cao).
36

lOMoARcPSD|15618161
Hình 3.20 Các thiết bị trong khối cấp nguồn

Cấp nguồn cho hệ thống:
- Từ nguồn pin cấp cho mạch giảm áp chuyển đổi DC – DC ổn định mức điện
áp cấp ra là 9VDC ( là mức điện áp phù hợp để cấp cho Arduino Mega 2560 Rev3 và
mạch cầu H L298).
- Nguồn ra 9VDC qua jack cắm DC rồi cung cấp cho Arduino, từ ngõ ra nguồn
5VDC Arduino cấp nguồn cho cảm biến gia tốc góc nghiêng MPU6050.
- Nguồn ra 9VDC nối vào chân 12V và GND của L298.
- L298 có ngõ ra 5VDC để cấp nguồn trực tiếp cho hai encoder.
- Ngõ ra OUT1 OUT2 của mạch cầu H cấp nguồn cho động cơ 1, ngõ ra OUT3
OUT4 của mạch cầu H cấp nguồn cho động cơ 2.
Hình 3.21 Sơ đồ cấp nguồn cho hệ thống
37

lOMoARcPSD|15618161
3.3.6 Bộ khung xe
Xây dựng khung xe gồm 3 tầng chính và tầng đáy được miêu tả ở mục 3.2, mục
đích để kết nối các hệ thống đã được chọn tạo thành một khối hoàn chỉnh. Đảm bảo
các yêu cầu sau:
- Bề rộng, bề dài, bề cao và khối lượng phù hợp để xe dễ dàng cân bằng.
- Tối ưu không gian để thuận tiện trong việc lắp ráp các linh kiện và đi dây hệ
thống.
Các thiết bị được lựa chọn để xây dựng khung xe:
- Ba tấm mica kích thước 150×90 mm bề dày 3mm thiết kế có các lỗ 3 để có

thể cố định các linh kiện phần cứng và các lỗ để đi dây cố định dây phù hợp.
- Các trụ đồng 40 mm để cố định bốn góc khung xe, liên kết ba tấm mica lại với
nhau.
- Hai bánh xe đường kính 65 mm được thiết kế tối ưu để cho độ ma sát và bám
đường tốt nhất.
- Hai gá động cơ và trục đồng để liên kết động cơ với khung xe.
Hình 3.22 Các thành phần để xây dựng khung xe

38

lOMoARcPSD|15618161
3.4 Bảng đi dây hệ thống
3.4.1 Phần nguồn
Nguồn Ard MP L2 Động Động cơ

9VDC uino U6050 98 cơ 1 và 2 và Encoder
(LM2596) Mega Encoder 1 2
OUT + Pow 12
er Jack V
OUT - G
ND
5V VCC
GN GN GND GND
D D
5V VCC VCC
O M1
UT1
O M2
UT2
O M1
UT3
O M2
UT4
Bảng 3.6 Bảng đi dây phần nguồn
3.4.2 Phần tín hiệu điều khiển
Ardui MPU6 L29 Động cơ 1 Động cơ 2

no Mega 050 8 và Encoder 1 và encoder 2
9 EN
A
30 IN1
32 IN2
10 EN
B
36 INT
3
34 INT
4
21 SCL
(SCL)
20 SDA
(SDA)
2 C1/A
22 C2/B
3 C1/A
26 C2/B
39

lOMoARcPSD|15618161
Bảng 3.7 Bảng đi dây phần tín hiệu điều khiển

40

lOMoARcPSD|15618161
CHƯƠNG 4: THIẾT KẾ PHẦN MỀM
4.1 Yêu cầu thiết kế
Về phần mềm điều khiển hệ thống các yêu cầu thiết kế để xe đảm bảo cân bằng
như sau:
- Đọc tín hiệu cảm biến MPU6050 thông qua truyền thông I2C.
- Đọc giá trị xung encoder trả về.
- Xuất xung PWM điều khiển mạch cầu H điều khiển động cơ theo tốc độ, chạy
thuận, chạy nghịch, dừng động cơ.
- Giới hạn góc nghiêng để cân bằng xe từ -30o đến 30o.
- Giao tiếp với máy tính thông qua cồng USB.
- Thiết kế giao diện để vẽ biểu đồ đáp ứng khi điều khiển xe cân bằng theo thời
gian thực.
4.2 Xây dựng thuật toán điều khiển
Để sử dụng và lập trình Arduino Mega 2560 thì phần mềm sử dụng để lập trình
là Arduino IDE 1.8.15, thuận tiện trong việc kết nối Arduino với máy tính bằng dây
cáp USB.
Ngôn ngữ lập trình của Arduino là C/C++, lập trình đơn giản hơn nhiều so với
các loại vi điều khiển khác nhờ cộng đồng người dùng lớn và sẵn các thư viện hỗ trợ
lập trình đa dạng.
Chương trình điều khiển được đưa vào ở phần Phụ lục.
41

lOMoARcPSD|15618161
4.2.1 Thuật toán chương trình chính
Hình 4.23 Lưu đồ thuật toán chương trình chính

Phần khai báo và khởi tạo chương trình:
- Khai báo các thư viện sử dụng: Wire.h để giao tiếp I2C với cảm biến góc
nghiêng MPU6050, và Kalman.h để sử dụng bộ lọc Kalman lọc nhiễu cho cảm biến
góc nghiêng MPU6050.
- Định nghĩa các chân kết nối.
- Khai báo các biến và giá trị ban đầu.
- Khởi tạo Serial giao tiếp máy tính thông qua USB với baud rate là 115200.
- Khởi tạo sử dụng timer2 để xuất xung PWM ở chân 9 và chân 10.
- Khởi tạo các chân xuất tín hiệu ngõ ra là chân xuất PWM của mỗi bánh, chân
điều chỉnh chiều quay của mỗi bánh.
42

lOMoARcPSD|15618161
- Khởi tạo các chân nhận tín hiệu ngõ vào là chân nhận tín hiệu từ encoder kênh
A và kênh B của mỗi bánh.
- Khởi tạo ngắt 0 sử dụng để đọc tín hiệu trả về từ kênh A bánh trái tác động
cạnh lên, ngắt 1 sử dụng để đọc tín hiệu trả về từ kênh A bánh phải tác động cạnh lên.
- Khởi tạo hàm đọc giá trị cảm biến góc nghiêng từ giao tiếp I2C.
Phần vòng lặp:
- Đọc và xử lý giá trị góc nghiêng trả về từ MPU6050.
- Thời gian lấy mẫu là 6ms.
- Cập nhập các giá trị theta, psi, phi và tính toán giá trị thetadot, psidot, phidot.
- Tính toán điện áp cung cấp ở bánh trái và bánh phải.
- Quy đổi sang PWM để xuất xung điều khiển cho mạch cầu.
- Xuất giá trị đọc được qua giao tiếp UART với máy tính đưa lên phần mềm
giám sát.
4.2.2 Thuật toán xử lý xung trả về từ encoder và tính toán giá trị góc tới,
góc xoay
Sử dụng chức năng ngắt ngoại của vi điều khiển để đọc giá trị xung encoder mà
không làm ảnh hưởng đến chương trình chính.
Hình 4.24 Lưu đồ chương trình ngắt xử lý xung trả về từ encoder
43

lOMoARcPSD|15618161
Hình 4.25 Lưu đồ tính toán giá trị góc tới và góc xoay
Phần khởi tạo:
- PIN 2 nối với encoder kênh A động cơ trái (sử dụng ngắt 0).
- PIN 22 nối với encoder kênh B động cơ trái (đọc tín hiệu trả về).
- PIN 3 nối với encoder kênh A động cơ phải (sử dụng ngắt 1).
- PIN 26 nối với encoder kênh B động cơ trái (đọc tín hiệu trả về).
Thuật toán đọc giá trị động cơ trái:
- Sử dụng ngắt 0 cho kênh A.
- Khi xung cạnh lên tác động thì chương trình ngắt thực hiện.
- Lúc này đọc giá trị trả về từ kênh B: nếu kênh B đang ở mức cao thì động cơ đang
quay thuận, leftencoder + + và ngược lại kênh B đang ở mức thấp thì động cơ đang
quay theo chiều nghịch, leftencoder - -.
Thuật toán đọc giá trị động cơ phải:
- Sử dụng ngắt 1 cho kênh A.
- Khi xung cạnh lên tác động thì chương trình ngắt thực hiện.
- Lúc này đọc giá trị trả về từ kênh B: nếu kênh B đang ở mức cao thì động cơ đang
quay thuận, rightencoder + + và ngược lại kênh B đang ở mức thấp thì động cơ đang
quay theo chiều nghịch, rightencoder - -.
Sử dụng phương pháp này sẽ tối ưu được tốc độ xử lý của vi điều khiển thay vì
phương pháp sử dụng ngắt cho cả hai kênh A và B.
Thuật toán đọc giá trị góc tới  và góc xoay  :
44

lOMoARcPSD|15618161
Từ phần xây dựng mô hình toán đối tượng trong chương 2 ta có được công thức
tính góc tới trung bình của hai bánh xe từ phương trình 22.
Động cơ sử dụng hộp số với tỉ số truyền là 34:1, đĩa Encoder trả ra 11 xung/
vòng. Nên số xung của Encoder trên 1 vòng quay trục chính động cơ là 34 x 11 = 374
xung/vòng.
Góc Encoder được xác định theo công thức:
Suy ra:
  1 360  
    (leftencoder  rightencoder ) 
   2 317 
    

   R 360 
   (leftencoder  rightencoder ) 
  W 317   74474\*
MERGEFORMAT (.)
Với leftencoder là xung encoder đọc được ở bánh trái.

rightencoder là xung encoder đọc được ở bánh phải.
45

lOMoARcPSD|15618161
4.2.3 Thuật toán đọc và xử lý giá trị từ cảm biến góc nghiêng
Hình 4.26 Lưu đồ thuật toán đọc và xử lý giá trị từ cảm biến góc nghiêng
Phần khởi tạo:
- Thiết lập địa chỉ I2C là 0x68, 0x69 địa chỉ này giúp ta kết nối nhiều module,
cảm biến khác nhau trên cùng 1 đường dữ liệu.
- Thiết lập các giá trị và mảng lưu giá trị thanh ghi từ module I2C.
- Thiết lập tần số I2C đến 400 kHz TWBR = ((F_CPU / 400000UL) - 16) / 2.
Thuật toán đọc giá trị từ cảm biến:
- Kiểm tra khoảng thời gian Arduino hoạt động đến thời gian đặt thì lấy mẫu
cảm biến 1 lần.
- Kiểm tra có giá trị trả về từ module I2C về không ?
- Cập nhập giá trị trả về vào mảng i2cData và trích xuất các giá trị gia tốc theo
phương X biến AcX, gia tốc theo phương Y biến AcY, vận tốc con xoay hồi chuyển
46

lOMoARcPSD|15618161
trục Y biến Gyro. Ở đây chỉ cần quan tâm các giá trị cần thiết để tính góc pitch, đó
chính là góc nghiêng của xe.
- Cập nhập thời gian chu kì đọc dt là thời gian từ lúc đọc tới lúc nhận được kết
quả (quy ra s).
- Tính toán giá trị góc pitch và tỉ lệ vận tốc con xoay hồi chuyển Gyrorate .
- Sử dụng hàm kalman.getAngle(pitch, Gyrorate,dt) trong thư viện Kalman.h để

trả về giá trị góc nghiêng.
- Giới hạn vùng hoạt động của xe: Nếu góc nghiêng trả về trong khoảng từ -30 o
đến 30o thì xe sẽ điều khiển cân bằng, ngoài khoảng đó thì ngắt động cơ dừng điều
khiển xe.
Sử dụng hàm Kalman lọc nhiễu ước lượng giá trị góc nghiêng trả về phẳng, ít
gai hơn. Từ đó ổn định được tín hiệu điều khiển cho hệ thống, giúp hệ thống hoạt động
mượt mà hơn.
4.2.4 Thuật toán tính xung băm PWM mỗi bánh với giải thuật LQR và
điều khiển động cơ
Hình 4.27 Lưu đồ thuật toán tính xung băm PWM mỗi bánh với giải thuật LQR
Thuật toán tính xung băm PWM mỗi bánh:
Từ ma trận K tính toán được ở chương cơ sở lý thuyết, ma trận K sẽ có dạng:
47

lOMoARcPSD|15618161
K K2 K3 K4 K5 K6 
K  1
 K1 K2 K3 K4  K5  K 6 
75475\*
MERGEFORMAT (.)
Và tín hiệu điều khiển:
v 
u  l 
 vr  76476\* MERGEFORMAT (.)
Theo giải thuật LQR thì tín hiệu điều khiển cấp cho động cơ là:
u  Kx(t ) 77477\* MERGEFORMAT (.)
Suy ra tín hiệu điều khiển cho bánh trái và bánh phải lần lượt là:
vl K1   K 2   K3   K 4   K 5   K 6 

v K   K   K   K   K   K 
r 1 2 3 4 5 6 78478\*
MERGEFORMAT (.)
Từ tín hiệu điều khiển, chuẩn hoá và giới hạn giá trị xung băm PWM: khi động
cơ nghiêng 1 góc là 15 độ thì sẽ băm xung tối đa (tuỳ theo tần số băm xung cầu H sẽ
có một mức giới hạn riêng).
Thuật toán điều khiển động cơ được thể hiện rõ trong lưu đồ:
48

lOMoARcPSD|15618161
Hình 4.28 Lưu đồ thuật toán điều khiển động cơ

4.2.5 Xuất và vẽ biểu đồ lên máy tính
Thiết lập truyền thông nối tiếp RS232 thóng qua cổng nạp USB với baud rate là
115200 với cú pháp Serial.begin(115200).
Đọc giá trị theta psi phi của hệ thống thông qua cảm biến và encoder rồi lưu vào
các biến nhớ.
Xuất giá trị ra serial bằng hàm Serial.println((String)(psi * ToDeg) + ',' +

(String)(theta * ToDeg) + ',' + (String)(phi * ToDeg)); để gửi chuỗi ký tự lên máy tính
cách nhau bởi dấu phẩy.
Thiết kế giao diện bằng ngôn ngữ lập trình C#, giá trị đọc cách nhau dấy phẩy
để tách chuỗi và mã hóa về kiểu double khi ta gửi số liệu lên máy tính, sử dụng thư
viện ZedGraph để tạo khung vẽ biểu đồ đường.
49

lOMoARcPSD|15618161
Hình 4.29 Lưu đồ đọc và biểu diễn đồ thị

4.3 Phần mềm giám sát
Phần mềm giám sát được thiết kế giao diện gồm:
- Khu vực kết nối thiết bị Terminal: chọn Baudrate và Port của vi điều khiển,
nút Connect (kết nối), nút Disconnect (ngưng kết nối), Exit (thoát chương trình).
- Khu vựa Receivered (tín hiệu nhận được): trả về tín hiệu góc nghiêng  (Psi)
theo thời gian thực.
- Khu vực biểu đồ: vẽ biểu đồ quan sát góc nghiêng trả về với trục tung là góc
nghiêng  (độ), trục hoành là thời gian (s).
50

lOMoARcPSD|15618161
Hình 4.30 Giao diện giám sát hệ thống

51

lOMoARcPSD|15618161
CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI
Ở chương này, nhóm trình bày về kết quả mô phỏng, thực tế đã đạt được và đưa
ra nhận xét đối với từng trường hợp cụ thể. Dựa vào kết quả thực hiện đó đưa ra các
hạn chế mà nhóm còn thiếu sót trong quá trình thực hiện đề tài đồng thời đề xuất các
hướng phát triển có thể thực hiện cho đề tài trong tương lai.
5.1 Những kết quả đã đạt được
5.1.1 Kết quả thiết kế mô hình và mô phỏng
 Thiết kế mô hình trên SolidWorks:
Hình 5.31 Mô hình hệ xe hai bánh tự cân bằng

 Kết quả mô phỏng của hệ xe hai bánh tự cân bằng:
Sơ đồ mô phỏng và thông số mô hình đã được nêu ở Chương 2 mục 2.4.
Tuỳ theo độ tương đối giữa trọng số Q và R mà hệ thống có đáp ứng quá độ và
năng lượng tiêu tốn khác nhau. Qua quá trình thực hiện mô phỏng và điều chỉnh hệ
thống để đảm bảo chất lượng tương đối tốt thì lựa chọn được trọng số ma trận Q và R
phù hợp với hệ thống như sau:
52

lOMoARcPSD|15618161
 500 0 0 0 0 0
 0 1 0 0 0 0 

 0 0 100 0 0 0
Q  
 0 0 0 1 0 0
 0 0 0 0 1000 0 
 
 0 0 0 0 0 1 80580\*
MERGEFORMAT (.)
 0.1 0 
R  
 0 0.1 81581\* MERGEFORMAT (.)
Ma trận điều khiển toàn phương tuyến tính K tính toán được từ ma trận Q và R:
  50  23.4  1766.1  231.7  70.7  18.4 

K  
  50  23.4  1766.1  231.7 70.7 18.4  82582\*
MERGEFORMAT (.)
Lựa chọn ma trận QN và RN của hệ thống khi có nhiễu hệ thống và nhiễu đo

lường:
 0.001 0 0 0 0 0 
 0 0.001 0 0 0 0 

 0 0 0.001 0 0 0 
QN  
 0 0 0 0.001 0 0 
 0 0 0 0 0.001 0 
 
 0 0 0 0 0 0.001 83583\*
MERGEFORMAT (.)
 0.1 0 0 
RN  0 0.001 0 

 0 0 0.01
84584\*
MERGEFORMAT (.)
Giả sử các giá trị ban đầu của hệ thống là:
- Góc tới trung bình của hai bánh:  0.01 .


- Vận tốc góc tới trung bình của hai bánh:  0.01 .
- Góc nghiêng của xe: 0.01 .
- Vận tốc góc nghiêng của xe: 0.01 .

53

lOMoARcPSD|15618161
- Góc xoay của xe:  0.01 .


- Vận tốc góc xoay của xe:  0.01 .
 Trường hợp 1: Khi không điều khiển và hệ thống chịu tác động của nhiễu với
thời gian mô phỏng 1s
Hình 5.32 Đáp ứng của góc tới, góc nghiêng, góc xoay của xe trong trường hợp 1
Nhận xét: Khi đặt một tín hiệu điều khiển bất kì cho hai bánh xe, trong trường
hợp không điều khiển thì xe ngay lập tức ngã, không cân bằng được. Để có thể điều
khiển được xe di chuyển hay cân bằng thì cần xây dựng mô hình toán phù hợp cho hệ
và ứng dụng các kĩ thuật điều khiển như vi tích phân tỉ lệ (PID), điều khiển mờ (Fuzzy
Logic Controller), toàn phương tuyến tính (LQR),…
 Trường hợp 2: Khi sử dụng bộ điều khiển LQR và hệ thống không chịu tác
động của nhiễu
54

lOMoARcPSD|15618161
Hình 5.34 Tín hiệu điều khiển của xe trong trường hợp 2
Nhận xét: Trong trường hợp chưa có nhiễu tác động vào hệ thống thì chất lượng
hệ thống đáp ứng tương đối tốt, các biến trạng thái của xe cân bằng tại 0, không có sai
số xác lập, thời gian đạt xác lập nhanh, có xuất hiện vọt lố. Tuy nhiên mục đích vọt lố
để xe có thể đạt được trạng thái cân bằng nhanh hơn.
 Trường hợp 3: Khi sử dụng bộ điều khiển LQR và hệ thống chịu tác động của
nhiễu
55

lOMoARcPSD|15618161
Nhận xét: Khi có nhiễu tác động vào hệ thống thì chất lượng điều khiển của hệ
thống bị ảnh hưởng đáng kể, tín hiệu điều khiển dao động với tần số cao. Nếu trong
thời gian dài có thể gây hư hỏng động cơ. Đây cũng là nhược điểm của bộ điều khiển
LQR khi hệ thống chịu tác động của nhiễu hệ thống và nhiễu đo lường từ cảm biến.
Qua khảo sát bộ điều khiển LQR trong hai trường hợp trên, thì cho thấy được
rằng trọng số ma trận Q ảnh hưởng tới chất lượng hệ thống. Khi tăng trọng số ma trận
Q thì đáp ứng tín hiệu ra tốt hơn, giảm được đáng kể thời gian xác lập. Còn trọng số R
ảnh hưởng tới tín hiệu điều khiển, khi giảm trọng số R sẽ giúp tín hiệu điều khiển tăng
lên giúp xe cân bằng nhanh hơn.
56

lOMoARcPSD|15618161
 Trường hợp 4: Khi sử dụng bộ điều khiển LQR kết hợp với bộ lọc Kalman để
ước lượng trạng thái và lọc nhiễu cho hệ thống
Hình 5.37 Đáp ứng của góc tới, góc nghiêng, góc xoay của xe khi chịu tác động của
nhiễu trong trường hợp 4
Hình 5.38 Đáp ứng của góc tới, góc nghiêng, góc xoay của xe sau khi qua bộ lọc
Kalman trong trường hợp 4
57

lOMoARcPSD|15618161
Nhận xét: Tín hiệu đáp ứng của hệ thống sau khi qua bộ lọc nhiễu Kalman thì
tín hiệu được lọc nhiễu đáng kể. Qua đó giúp chất lượng hệ thống đáp ứng tốt hơn, tín
hiệu điểu khiển ít dao động, ổn định hơn, động cơ chạy mượt hơn khi điều khiển. Bộ
lọc Kalman ước lượng trạng thái hệ thống và lọc nhiễu, nhờ vậy đáp ứng của hệ thống
khi kết hợp với bộ lọc Kalman tốt hơn khi không sử dụng bộ lọc Kalman trong trường
hợp có nhiễu hệ thống và nhiễu đo lường từ cảm biến trả về.
5.1.2 Kết quả thực tế
 Mô hình phần cứng:
Hình 5.40 Mô hình phần cứng thi công thực tế hướng trực diện
58

lOMoARcPSD|15618161
Hình 5.41 Mô hình phần cứng thi công thực tế hướng nằm ngang
Hình 5.42 Mô hình phần cứng thi công thực tế hướng chéo
59

lOMoARcPSD|15618161
 Kết quả trả về từ phần mềm giám sát:

Kiểm tra giá trị góc nghiêng trả về của xe sau khi qua bộ lọc Kalman khi để lần
lượt tại vị trí cân bằng, vị trí hướng nghiêng 30 độ về phía sau, vị trí hướng nghiêng 25
độ về phía trước
Hình 5.43 Khảo sát góc nghiêng của xe khi qua bộ lọc Kalman
Khi xe cân bằng góc nghiêng của xe  được trả về chính xác hiển thi trên giao
diện giám sát
Hình 5.44 Khảo sát góc nghiêng của xe trả về khi xe hoạt động
60

lOMoARcPSD|15618161
5.2 Kết luận
Qua quá trình nghiên cứu và thực hiện đề tài nhóm xin tổng hợp lại những
thành quả đã đạt được:
- Thiết kế hoàn chỉnh và hoàn thiện mô hình xe hai bánh tự cân bằng.
- Tính toán mô hình toán học, hàm trạng thái cho hệ thống.
- Thực hiện mô phỏng hệ thống thành công trên MATLAB Simulink.
- Các thành phần điện và vi điều khiển hoạt động tốt:
+ Giá trị trả về từ cảm biến góc nghiêng tương đối chính xác, áp dụng bộ lọc
Kalman để lọc nhiễu tốt.
+ Điều chế độ rộng xung cho mạch cầu H tốt, ổn định không bị quá tải, quá
nhiệt trong quá trình hoạt động.
+ Đọc bộ mã hoá xoay encoder tương đối chính xác.
+ Vi điều khiển hoạt động ổn định, xử lý tín hiệu nhanh.
- Giao tiếp với máy tính qua UART và thiết kế phần mềm giám sát vẽ biểu đồ
hoạt động cho xe.
- Mô hình có thể cân bằng tại chỗ.
Tuy vậy do thời gian thực hiện có hạn nên đề tài vẫn còn nhiều hạn chế.
5.3 Hạn chế của đề tài
Các mặt hạn chế còn gặp phải trong quá trình nghiên cứu đề tài là:
- Nhiều địa hình khác nhau như: gồ ghề, nhấp nhô, nghiêng, bề mặt nhám hay
bám dính cao,… vẫn chưa được khảo sát cụ thể do tính phức tạp của địa hình.
- Mô hình xe chạy thực tế so với mô phỏng vẫn còn nhiều sai số như về
moment quán tính, độ rơ hộp số trên motor bánh xe,… và phần động cơ sử dụng vẫn
chưa nhận dạng được đầy đủ các thông số dẫn đến quá trình mô hình hoá và tính toán
ma trận LQR cho mô hình thực tế còn chưa chính xác. Vì vậy khi điều khiển thực tế so
với phần mô phỏng vẫn còn khoảng cách khá lớn.
- Robot chưa có khả năng tự kiểm soát hay đưa ra quyết định như khả năng tự
di chuyển, vượt chướng ngại vật, tránh vật cản, tự đứng dậy, khả năng quản lý năng
lượng để ổn định quá trình điều khiển trong thời gian dài,…
61

lOMoARcPSD|15618161
- Giữ cân bằng khá tốt, còn gặp phải tính trạng vọt lố cao của thành phần góc
nghiêng robot.
- Khả năng chịu tác động ngoại lực còn yếu.
- Chỉ có thể giữ cân bằng tại chỗ, chưa thể di chuyển được.
- Hệ thống chưa đáp ứng được tốt khi hệ số ma sát hay khối lượng M của xe
thay đổi.
5.4 Hướng phát triển
Qua các hạn chế được nêu ở trên, nhóm xin đề xuất một số hướng phát triển
trong tương lai của hệ xe hai bánh tự cân bằng như sau:
- Khảo sát và xây dựng hoàn chỉnh đặc tính động học của mô hình đối tượng
trong những điều kiện về địa hình khác nhau.
- Xây dựng giải thuật thông minh để nhận dạng những thông số chưa mô hình
hoá được như đã đề cập ở trên.
- Khảo sát và xây dựng khả năng tự đứng dậy, khả năng tự hành cho robot,…
giúp robot hoạt động độc lập hơn có nhiều ứng dụng trong thực tế hơn.
- Mô hình sẽ được điều khiển thông qua kết nối bluetooth kết nối với máy tính
hoạt điện thoại thuận tiện trong việc điều khiển từ xa robot.
- Giao diện sẽ được thiết kế thân thiện dễ dùng với nhiều chức năng hơn.
- Tối ưu hoá khối lượng, thuật toán điều khiển cho xe để xe có thể hoạt động
cân bằng trơn tru, linh hoạt trên mọi địa hình dễ điều khiển và tiết kiệm năng lượng.
- Phát triển thêm camera và gps ứng dụng định vị, ghi hình, xử lý ảnh để robot
có thể thực hiện nhiều ứng dụng hơn trong không gian làm việc lớn hơn.
- Mô hình này là tiền đề để phát triển thành xe hai bánh tự cân bằng có thể chở
được người hay đồ vật linh hoạt hơn trong việc di chuyển với tải trọng lớn.
62

lOMoARcPSD|15618161
TÀI LIỆU THAM KHẢO
[1] N. T. Hiếu, “Điều Khiển Robot Xe Hai Bánh Cân Băng Trên Địa Hình
Phẳng” Đại Học Bách Khoa TP. HCM.
[2] Nguyễn Trị Phương Hà; Huỳnh Thái Hoàng, “Lý Thuyết Điều Khiển Tự
Động” in Lý Thuyết Điều Khiển Tự Động, 1st ed., Nguyễn Trị Phương Hà, Ed. Hồ Chí
Minh City: Nhà Xuất Bản Đại Học Quốc Gia TP. HCM, 2005, pp. 311–314.
[3] Huỳnh Thái Hoàng, “Giải Thuật Di Truyền” in Điều Khiển Thông Minh,
1st ed., Huỳnh Thái Hoàng, Ed. Hồ Chí Minh City: Nhà Xuất Bản Đại Học Quốc Gia
TP. HCM, 2006, pp. 126–129.
[4] Huỳnh Thái Hoàng, “Bộ Điều Khiển Mờ” in Hệ Thống Điều Khiển
Thông Minh, Huỳnh Thái Hoàng, Ed. Hồ Chí Minh City: Đại Học Quốc Gia TP. Hồ
Chí Minh, 2006, pp. 25–60.
[5] Nguyễn Trị Phương Hà, “Điều Khiển Trượt” in Lý Thuyết Điều Khiển
Hiện Đại, Nguyễn Trị Phương Hà, Ed. Hồ Chí Minh City: Minh, Đại Học Quốc Gia
TP. Hồ Chí Minh, 2009, pp. 50–54.
[6] Nguyễn Trị Phương Hà, “Phương Pháp Toàn Phương Tuyến Tính LQR”
in Lý Thuyết Điều Khiển Hiện Đại, Nguyễn Trị Phương Hà, Ed. Hồ Chí Minh City:
Đại Học Quốc Gia TP.Hồ Chí Minh, 2009, pp. 154–170.
[7] R. S. A. A.-W. Murtadha Awoda, “Parameter Estimation of a Permanent

Magnetic DC Motor” Iraqi Journal for Electrical And Electronic Engineering, Iraq, pp.
28–36, Jun. 2019.
63

lOMoARcPSD|15618161
PHỤ LỤC
Phụ lục 1: Chương trình Code Arduino cho hệ xe hai bánh tự cân bằng
#include <Wire.h>
#include <Kalman.h> //https://github.com/TKJElectronics/KalmanFilter
#define ToDeg 180/PI
#define ToRad PI/180
Kalman kalman; //Kalman filter define: kalman.getAngle(pitch, gyrorate, dt);
int inChar;
uint32_t timerloop, timerold;
//Motor control Pin//

int leftpwm = 9; //Control PWM left motor
int leftdir = 32; //Control direction left motor
int leftdir2 = 30;//Control direction left motor
int righpwm = 10; //Control PWM right motor

int righdir = 34; //Control direction right motor
int righdir2 = 36;//Control direction right motor
volatile long leftencoder; //Read left encoder value

volatile long righencoder; //Read right encoder value
// INITIALIZE ENCODER PIN
int leftencoder_a = 2; //Read state encoder channel LOW or HIGH
int leftencoder_b = 22;
int righencoder_a = 3;
int righencoder_b = 26;
//MPU6050 Data//
double mpudata; //Save psi angle (Y axis)
double accX, accZ;
float Gyro;
uint32_t timer; //Timer for kalman filter psi angle;

uint8_t i2cData[14];
//LQR data//
long PWML, PWMR; //PWM output for H-Brigde
float K1, K2, K3, K4, K5, K6; //The factor of K maxtrix
bool falldown; //Run = true; Stop = false;
64

lOMoARcPSD|15618161
float theta, psi, phi;

float thetadot, psidot, phidot;
float thetaold = 0, psiold = 0, phiold = 0;
float leftvolt; //output volt left motor in LQR

float righvolt; //output volt right motor in LQR
/////////////////////////////////////////////////
/////////// SERIAL BEGIN ///////////////////
/////////////////////////////////////////////////
void setup() {
Serial.begin(115200);
/*
//Set factor of K matrix
K = [k1 k2 k3 k4 k5 k6
k1 k2 k3 k4 -k5 -k6]
*/
// Initialize Parameters LQR Controller.
// DDRB |= 1 << PB4;
// DDRH |= 1 << PH6;
K1 = 50;
K2 = 10;
K3 = 240;
K4 = 10;
K5 = 0;
K6 = 0;
pinMode(leftpwm, OUTPUT);
pinMode(righpwm, OUTPUT);
//Set PWM frequency 23429.4Hz f_PWM = 12e6/(510*N) = 23429.4Hz
// Phase Correct PWM
// Fast PWM
// TCCR2A |= (1 << WGM21) | (1 << WGM20);
TCCR2B = TCCR2B & B11111000 | B00000001; //Pin 9 & Pin 10
// Timer1.initialize(10000);//10us
// Timer1.attachInterrupt(NGAT);
//Output pin control motor left and right//
pinMode(leftdir, OUTPUT);
pinMode(righdir, OUTPUT);
pinMode(leftdir2, OUTPUT);
pinMode(righdir2, OUTPUT);
//Input pin read encorder//

pinMode(leftencoder_a, INPUT_PULLUP);
65

lOMoARcPSD|15618161
pinMode(leftencoder_b, INPUT_PULLUP);
pinMode(righencoder_a, INPUT_PULLUP);
pinMode(righencoder_b, INPUT_PULLUP);
//interrupt encoder//
attachInterrupt(0, left_isr, RISING);
attachInterrupt(1, righ_isr, RISING);
//Data MPU6050//
Wire.begin();
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz

i2cData[0] = 7;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while (i2cWrite(0x19, i2cData, 4, false));
while (i2cWrite(0x6B, 0x01, true));
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100);
while (i2cRead(0x3B, i2cData, 6));

accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;

kalman.setAngle(pitch);
kalman.setQangle(0.0000085);
kalman.setQbias(0.000005);
kalman.setRmeasure(0.0009);
timer = micros();
}
//////////////////////////////////
// MAIN PROGRAMMING //
//////////////////////////////////
void loop() {
readmpu();
66

lOMoARcPSD|15618161
if ((micros() - timerloop) > 6000) {//Set time loop update and control motor
theta = gettheta(leftencoder, righencoder) * ToRad; //Read theta value and convert
to Rad
psi = (mpudata) * ToRad; //Read psi value and convert to Rad
phi = getphi(leftencoder, righencoder) * ToRad; //Read phi value and convert to
Rad
//Update time compare with timeloop
double dt = (float)((micros() - timerloop)) / 1000000.0;
timerloop = micros();
//Update input angle value
thetadot = (theta - thetaold) / dt;
psidot = (psi) / dt;
phidot = (phi - phiold) / dt;
//Update old angle value
thetaold = theta;
psiold = psi;
phiold = phi;
getlqr(theta, thetadot, psi, psidot, phi, phidot);

motorcontrol(PWML, PWMR, (mpudata), falldown);
//Send data to serial
Serial.println((String)(psi * ToDeg));
}
}
//left motor encoder interrupt//

void left_isr() {
if (digitalRead(leftencoder_b) == HIGH) leftencoder++;
else leftencoder--;
}
//right motor encoder interrupt//

void righ_isr() {
if (digitalRead(righencoder_b) == HIGH) righencoder++;
else righencoder--;
}
//Read psi//
void readmpu() {
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
67

lOMoARcPSD|15618161
Gyro = (int16_t)((i2cData[10] << 8) | i2cData[11]);
double dt = (double)(micros() - timer) / 1000000;

timer = micros();
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
double Gyrorate = Gyro / 131.0;
mpudata = kalman.getAngle(pitch, Gyrorate, dt) - 0.5;
if (abs(mpudata) > 30)

{
falldown = true;
}
else
{
falldown = false;
}
//
}
//Read theta angle function

float gettheta(long lencoder, long rencoder) {
float angle = 0.5 * (360 / 374) * (lencoder + rencoder);
return angle;
}
//Read phi angle function//

float getphi(long lencoder, long rencoder) {
float angle = (3.4 / 14.7)* (360 / 374) * (lencoder - rencoder);
return angle;
}
//LQR function
void getlqr(float theta_, float thetadot_, float psi_, float psidot_, float phi_, float
phidot_) {
righvolt = K1 * theta_ + K2 * thetadot_ + K3 * psi_ + K4 * psidot_ + K5 * phi_ +
K6 * phidot_;
leftvolt = K1 * theta_ + K2 * thetadot_ + K3 * psi_ + K4 * psidot_ - K5 * phi_ - K6
* phidot_;
PWML = map(leftvolt, -K3 * PI / 15, K3 * PI / 15, -250, 250); //Limit 15 deg.

PWMR = map(righvolt, -K3 * PI / 15, K3 * PI / 15, -250, 250);
PWML = constrain(PWML, -200, 200); // SATURATION
68

lOMoARcPSD|15618161
PWMR = constrain(PWMR, -200, 200);

}
//Motor control function

void motorcontrol(long lpwm, long rpwm, float angle, bool stopstate) {
if (abs(angle) > 30) //angle psi > 30 motor will stop
{
stopandreset();
}
else
{
if (leftvolt > 0)
{
leftmotor(abs(lpwm), 1); //Forward
}
else if (leftvolt < 0)
{
leftmotor(abs(lpwm), 0); //Back
}
else
{
stopandreset();
}
//
if (righvolt > 0)
{
righmotor(abs(rpwm), 1);
}
else if (righvolt < 0)
{
righmotor(abs(rpwm), 0);
}
else
{
stopandreset();
}
}
}
//Stop motor and reset data

void stopandreset() //The data angle and encoder will be reset back to zero.
{
analogWrite(leftpwm, 0);
69

lOMoARcPSD|15618161
digitalWrite(leftdir, LOW);
digitalWrite(leftdir2, LOW);
analogWrite(righpwm, 0);
digitalWrite(righdir, LOW);
digitalWrite(righdir2, LOW);
//Reset default place
leftencoder = 0;
righencoder = 0;
}
//Control left motor
void leftmotor(uint8_t lpwm, int direct) {
analogWrite(leftpwm, lpwm);
if (direct == 1) { //angle > 0
digitalWrite(leftdir, LOW);
digitalWrite(leftdir2, HIGH);
}
else {
digitalWrite(leftdir, HIGH);
digitalWrite(leftdir2, LOW);
}
}
//Control right motor
void righmotor(uint8_t rpwm, int direct) {
analogWrite(righpwm, rpwm);
if (direct == 1) { //angle > 0
digitalWrite(righdir, LOW);
digitalWrite(righdir2, HIGH);
}
else {
digitalWrite(righdir, HIGH);
digitalWrite(righdir2, LOW);
}
}
70

Do An 2 Balance Cart 19161127 19151121

Uploaded by

Document Information

Original Title

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

Do An 2 Balance Cart 19161127 19151121

Uploaded by

Copyright:

Available Formats

lOMoARcPSD|15618161

Đồ Án 2 Balance CART 19161127 19151121

Studocu is not sponsored or endorsed by any college or university

121Equation Chapter 1 Section 2

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH

ĐỀ TÀI: “NGHIÊN CỨU VÀ THIẾT KẾ HỆ

GVHD: TS. Nguyễn Trần Minh Nguyệt

TP. HỒ CHÍ MINH – 7/2021

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)

TP Hồ Chí Minh, ngày … tháng … năm 2021

Nhóm Sinh viên thực hiện

Trương Trí Kiệt – Lê Văn Hà

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)

NHẬN XÉT CỦA GIÁO VIÊN

TP Hồ Chí Minh, ngày … tháng … năm 2021

Giáo viên hướng dẫn

Ths. Nguyễn Trần Minh Nguyệt

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)

TÓM TẮT ĐỀ TÀI

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)

LỜI CẢM ƠN...............................................................................................................i

NHẬN XÉT CỦA GIÁO VIÊN..................................................................................ii

TÓM TẮT ĐỀ TÀI.....................................................................................................iii

DANH MỤC BẢNG VÀ HÌNH ẢNH......................................................................vii

DANH MỤC CÁC TỪ VIẾT TẮT............................................................................ix

CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI.........................................................................1

1.1 Đặt vấn đề............................................................................................................... 1

1.2 Mục tiêu đề tài........................................................................................................3

1.3 Giới hạn đề tài........................................................................................................4

1.4 Phương pháp nghiên cứu......................................................................................4

1.5 Nội dung đề tài.......................................................................................................5

2.1 Đặc tính động lực học............................................................................................6

2.2.1 Bộ điều khiển vi tích phân tỉ lệ...................................................................11

2.2.3 Bộ điều khiển mờ.......................................................................................12

2.2.4 Bộ điều khiển tối ưu...................................................................................13

2.3 Điều khiển toàn phương tuyến tính....................................................................14

2.3.1 Bộ lọc Kalman............................................................................................14

2.3.3 Tuyến tính hoá hệ thống.............................................................................16

2.3.5 Hàm chỉ tiêu chất lượng.............................................................................20

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)

2.4 Sơ đồ mô phỏng hệ thống trên MATLAB Simulink..........................................20

CHƯƠNG 3: THIẾT KẾ PHẦN CỨNG..................................................................25

3.1 Yêu cầu thiết kế....................................................................................................25

3.2 Sơ đồ khối hệ thống.............................................................................................25

3.3 Lựa chọn thiết bị phần cứng...............................................................................27

3.3.1 Khối điều khiển trung tâm..........................................................................27

3.3.2 Khối cảm biến đo góc nghiêng...................................................................29

3.3.3 Khối điều khiển động cơ............................................................................31

3.3.5 Khối cấp nguồn..........................................................................................35

3.3.6 Bộ khung xe...............................................................................................38

3.4 Bảng đi dây hệ thống...........................................................................................39

3.4.1 Phần nguồn.................................................................................................39

3.4.2 Phần tín hiệu điều khiển.............................................................................39

CHƯƠNG 4: THIẾT KẾ PHẦN MỀM...................................................................40

4.1 Yêu cầu thiết kế....................................................................................................40

4.2 Xây dựng thuật toán điều khiển.........................................................................40

4.2.1 Thuật toán chương trình chính....................................................................41

4.2.5 Xuất và vẽ biểu đồ lên máy tính.................................................................48

4.3 Phần mềm giám sát..............................................................................................48

CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI...........................50

5.1 Những kết quả đã đạt được.................................................................................50

Downloaded by Trung Hi?u Tr?n Lê (trunghieutranle0910aa@gmail.com)

5.1.1 Kết quả thiết kế mô hình và mô phỏng.......................................................50