Xe hai bánh tự cân bằng giải thuật LQR, ứng dụng xử lý ảnh để bám theo quả bóng

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT
THÀNH PHỐ HỒ CHÍ MINH

ĐỒ ÁN TỐT NGHIỆP
NGÀNH ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA

XE HAI BÁNH TỰ CÂN BẰNG GIẢI THUẬT LQR, ỨNG
DỤNG XỬ LÝ ẢNH ĐỂ BÁM THEO QUẢ BĨNG

GVHD:VŨ VĂN PHONG
SVTT:NGUYỄN HUY HỒNG MSSV:16151304
SVTH:NGUYỄN HUY HỒNG MSSV:16151092

SKL 0 0 7 3 0 6

Tp. Hồ Chí Minh, tháng 07/2020


BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH
KHOA ĐÀO TẠO CHẤT LƯỢNG CAO


ĐỒ ÁN TỐT NGHIỆP
Đề Tài:
XE HAI BÁNH TỰ CÂN BẰNG GIẢI THUẬT LQR, ỨNG DỤNG XỬ LÝ ẢNH
ĐỂ BÁM THEO QUẢ BĨNG
GVHD

: TS Vũ Văn Phong

SVTH

: Nguyễn Huy Hồng

MSSV:16151304

Phạm Văn Trân

MSSV:16151092

Chuyên ngành

: Điều khiển và Tự động hóa

Hệ đào tạo

: Chính Quy

Khóa đào tạo

: 2016 – 2020

TP. Hồ Chí Minh, ngày 27 tháng 07 năm 2020


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

tháng
năm 2020

Họ và tên Sinh Viên :
:

NHIỆM VỤ ĐỒ ÁN
Nguyễn Huy Hoàng
Phạm Văn Trân

MSSV: 16151304
MSSV: 16151092

Giảng viên hướng dẫn:
Vũ Văn Phong
SĐT: 0981479507
1. Tên đề tài: Xe hai bánh tự cân bằng giải thuật LQR, ứng dụng xử lý ảnh để bám theo
quả bóng
2. Các số liệu, tài liệu ban đầu:……………………………………………………………..
………………………………………………………………………………………………
………………………………………………………………………………………………
3. Nội dung thực hiện đề tài:………………………………………………………………..
………………………………………………………………………………………………
………………………………………………………………………………………………
4. Sản phẩm:………………………………………………………………………………...
………………………………………………………………………………………………
………………………………………………………………………………………………
TRƯỞNG NGÀNH
GIẢNG VIÊN HƯỚNG DẪN

Khoa ĐT CLC – ĐH SPKT TP.HCM


CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh phúc
*******
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN
Họ và tên Sinh viên:
Nguyễn Huy Hoàng
MSSV:16151304
Phạm Văn Trân
MSSV:16151092
Ngành: CNKT Điều Khiển và Tự Động Hóa
Tên đề tài: Xe hai bánh tự cân bằng giải thuật LQR, ứng dụng xử lý ảnh để bám theo quả
bóng
Họ và tên Giáo viên hướng dẫn: Vũ Văn Phong
NHẬN XÉT
1. Về nội dụng đề tài và khối lượng thực hiện:
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
2. Ưu điểm:
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
3. Khuyết điểm:
………………………………………………………………………………………………
………………………………………………………………………………………………

………………………………………………………………………………………………
4. Đề nghị cho bảo vệ hay không?
………………………………………………………………………………………………
5. Đánh giá loại:
………………………………………………………………………………………………
6. Điểm ………………(Bằng chữ:………………………………………………………..)
Tp. Hồ Chí Minh, ngày
tháng
năm 2020
Khoa ĐT CLC – ĐH SPKT TP.HCM


CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh phúc
*******
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
Họ và tên Sinh viên :…………………………………………..MSSV:………………….
:……………….………………………….MSSV:………………….
Ngành……………………………………………………………………………………….
Tên đề tài:…………………………………………………………………………………..
………………………………………………………………………………………………
Họ và tên Giáo viên phản biện:……………………………………………………………..
………………………………………………………………………………………………
NHẬN XÉT
1. Về nội dụng đề tài và khối lượng thực hiện:
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
2. Ưu điểm:

………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
3. Khuyết điểm:
………………………………………………………………………………………………
………………………………………………………………………………………………
………………………………………………………………………………………………
4. Đề nghị cho bảo vệ hay không?
………………………………………………………………………………………………
5. Đánh giá loại:
………………………………………………………………………………………………
6. Điểm ………………(Bằng chữ:………………………………………………………..)
Tp. Hồ Chí Minh, ngày
tháng
năm 2020
Khoa ĐT CLC – ĐH SPKT TP.HCM


MỤC LỤC
LỜI CẢM ƠN .................................................................................................................. i
LỜI MỞ ĐẦU ................................................................................................................. ii
TÓM TẮT ...................................................................................................................... iii
ABSTRACT ................................................................................................................... iv
DANH SÁCH HÌNH ẢNH .............................................................................................. v
DANH MỤC TỪ VIẾT TẮT.......................................................................................... ix
TÀI LIỆU THAM KHẢO ............................................................................................... x
CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI .............................................................................. 1
1.1. Đặt vấn đề ............................................................................................................. 1
1.2. Mục tiêu ................................................................................................................ 1

1.3. Giới hạn................................................................................................................. 1
1.4. Phương pháp nghiên cứu ....................................................................................... 1
1.5. Nội dung................................................................................................................ 2
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT................................................................................. 3
2.1. Nguyên lý xe hai bánh tự cân bằng ....................................................................... 3
2.2. Xây dựng phương trình tốn học ........................................................................... 4
2.3. Bộ điều khiển LQR.............................................................................................. 10
2.3.1. LQR liên tục.................................................................................................. 10
2.3.2. LQR rời rạc ................................................................................................... 11
2.3.3. Xây dựng bộ điều khiển cho hệ xe cân bằng .................................................. 11
2.4. Bộ lọc Kalman..................................................................................................... 14
2.5. Giao tiếp I2C ....................................................................................................... 17
2.5.1. Giới thiệu về giao tiếp I2C ............................................................................ 17
2.5.2. Phương thức giao tiếp I2C ............................................................................. 18
2.5.3. Hoạt động của giao thức I2C ......................................................................... 21
2.5.4. Các chế độ hoạt động của I2C ....................................................................... 22
2.6. Giao tiếp UART .................................................................................................. 24
2.7. Ngơn ngữ lập trình Python và thư viện Opencv ................................................... 26
2.7.1. Ngơn ngữ lập trình Pthon .............................................................................. 26


2.7.2. Thư viện Opencv ........................................................................................... 28
2.8. Phương pháp nhận dạng vật theo màu sắc............................................................ 29
2.8.1. Không gian màu RGB và HSV ...................................................................... 29
2.9.2. Nhận dạng màu vật bằng không gian màu HSV............................................. 30
2.9. Tìm vị trí đường trịn ........................................................................................... 33
2.9.1. Tích chập và Non – Maximum Suppression .................................................. 33
2.9.3. Theo dõi đường biên đối tượng...................................................................... 38
2.9.2. Tìm tâm đường trịn....................................................................................... 42
2.10. Bộ điều khiển PID ............................................................................................. 44

2.11. Phần mềm lập trình Arduino ............................................................................. 47
CHƯƠNG 3: TÍNH TỐN VÀ THIẾT KẾ HỆ THỐNG .............................................. 49
3.1. Tính tốn và thiết kế hệ thống.............................................................................. 49
3.2. Phần cứng xứ lý ảnh ............................................................................................ 60
CHƯƠNG 4: GIẢI THUẬT ĐIỀU KHIỂN ................................................................... 65
4.1. Quy trình cơng nghệ ............................................................................................ 65
4.2. Lưu đồ giải thuật ................................................................................................. 65
CHƯƠNG 5: KẾT QUẢ THỰC NGHIỆM ................................................................... 70
5.1. Kết quả xử lý ảnh ................................................................................................ 70
5.2. Mơ phỏng trên matlab.......................................................................................... 72
5.3. Mơ hình thực tế ................................................................................................... 74
CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ................................................ 78
6.1. Kết luận ............................................................................................................... 78
6.2. Hướng phát triển.................................................................................................. 78


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

LỜI CẢM ƠN
Trước hết chúng em xin cảm ơn TS. Vũ Văn Phong đã có những sự chỉ dẫn, đóng
góp ý kiến vơ cùng quý báu và sự động viên khích lệ tinh thần của thầy đã truyền cảm
hứng và nguồn động lực lớn để nhóm hồn thành đồ án mơn học.
Chúng em xin gửi lời cảm ơn đến các Thầy, Cô Khoa Điện đã dạy chúng em trong
những năm tháng đại học để em có những kiến thức vững chắc và là tiền đề để em hồn
thành đồ án mơn học ngày hơm nay. Lời cảm ơn tiếp theo là đến những người bạn, đã giúp
đỡ và có những đóng góp để em hồn thành đồ án mơn học tốt hơn.

i

ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

LỜI MỞ ĐẦU
Đề tài “Xe hai bánh tự cân bằng giải thuật LQR, ứng dụng xử lý ảnh để bám theo
quả bóng”, được nhóm phát triển từ lý thuyết con lắc ngược. Nhóm quyết định phát triển
từ bộ điều khiển PID sang bộ điều khiển LQR. Đồng thời, nhóm nâng cấp cho hệ xe cân
bằng này có khả năng theo dõi và bám sát đối tượng bằng xử lý ảnh trên máy tính Raspberry
Pi. Qua nghiên cứu, nhóm sẽ xử lý ảnh trên Raspberry và gửi dữ liệu về Arduino bằng giao
thức I2C để tính tốn khoảng cách giữa robot và đối tượng.
Để robot có thể theo dõi quả bóng tennis và duy trì khoảng cách mong muốn, thì
cần có sự kết hợp giữa bộ điều khiển PID và bộ điều khiển LQR để robot có thể cân bằng
và theo dõi bóng cùng một lúc.

ii


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

TÓM TẮT
Đề tài “Xe hai bánh tự cân bằng giải thuật LQR, ứng dụng xử lý ảnh để bám theo
quả bóng” là cầu nối kinh nghiệm từ mơ hình thăng bằng con lắc ngược đến việc nghiên
cứu và chế tạo xe hai bánh nằm song song với nhau. Nhóm đã nghiên cứu và thực hiện xe
hai bánh tự cân bằng bằng bộ điều khiển LQR với sử dụng phần mềm hỗ trợ việc giải thuật
bộ điều khiển.

Đồng thời, nhóm tiếp tục phát triển hệ xe có khả năng nhận dạng mục tiêu theo màu
sắc và bám sát mục tiêu đó bằng máy tính Raspberry Pi. Để robot bám theo quả bóng tennis
và duy trì khoảng cách mong muốn, cần kết hợp với bộ điều khiển PID vào bộ điều khiển
LQR để robot vừa bám vừa bám theo trái banh.
Mơ hình gồm một thân mang hai động cơ Nisca NFS5475E, Arduino Mega 2560,
cảm biến góc nghiêng MPU 6050, module cầu H XY-160D L298 7A 160W, giảm áp
LM2596, module Bluetooth HC-05.

iii


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

ABSTRACT
The topic "Seft-balancing two wheelers LQR algoritm, image processing
application to tracking the ball" is developed by our team from “Two-wheel balancing
robot” based on “Inverted Pendubot”. The team has researched and implemented a selfbalancing two-wheeled vehicle with an LQR controller using software that supports the
driver algorithm.
At the same time, the team continues to develop a vehicle system capable of
recognizing targets by color and sticking to that target using a Raspberry Pi computer. order
for the robot to follow the tennis ball and maintain the desired distance, it is necessary to
combine the PID controller with the LQR controller so that the robot can both follow and
follow the ball.
Model includes a body with two Nisca NFS5475E engines, Arduino Mega 2560,
MPU 6050 tilt angle sensor, H XY-160D bridge module L298 7A 160W, LM2596 pressure
relief, Bluetooth HC-05 module.

iv

ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

DANH SÁCH HÌNH ẢNH
Hình 2.1: Nguyên lý xe hai bánh tự cân bằng................................................................... 3
Hình 2.2: Trạng thái cân bằng và trạng thái di chuyển ..................................................... 4
Hình 2.3: Xe hai bánh đi lên và đi xuống ......................................................................... 4
Hình 2.4: Mơ tả thơng số của xe hai bánh tự cân bằng ..................................................... 5
Hình 2.5: Mơ tả xe hai bánh tự cân bằng nhìn nghiêng và từ trên xuống .......................... 5
Hình 2.6:: Tổng quan của bộ điều khiển LQR ................................................................ 10
Hình 2.7: Sơ đồ điều khiển LQR liên tục ....................................................................... 11
Hình 2.8: Sơ đồ điều khiển xe hai bánh tự cân bằng....................................................... 14
Hình 2.9: Thuật tốn bộ lọc Kalman .............................................................................. 17
Hình 2.10: Kết nối thiết bị trong giao tiếp I2C ............................................................... 18
Hình 2.11: Địa chỉ thiết bị trong I2C.............................................................................. 19
Hình 2.12: Cấu trúc truyền dữ liệu của I2C.................................................................... 20
Hình 2.13: Truyền nhận dữ liệu giữa Master/Slave. ....................................................... 21
Hình 2.14: Bit dữ liệu trên SDA khi Master gửi dữ liệu từ Slave ................................... 22
Hình 2.15: Bit dữ liệu trên SDA khi Master gửi dữ liệu từ Slave ................................... 22
Hình 2.16: Một Master nhiều Slave ............................................................................... 23
Hình 2.17: Nhiều Master nhiều Salve ............................................................................ 24
Hình 2.18: Giao tiếp UART ........................................................................................... 25
Hình 2.19: Cấu trúc gói dữ liệu trong giao tiếp UART ................................................... 25
Hình 2.20: Logo của Python .......................................................................................... 26
Hình 2.21: Logo của Opencv ......................................................................................... 28
Hình 2.22: Khơng gian màu HSV .................................................................................. 30
Hình 2.23: Hình thể hiên phép giãn ............................................................................... 31

Hình 2.24: Hình thể hiện phép co .................................................................................. 32
Hình 2.25: Hình thể hiện phép mở ................................................................................. 32
Hình 2.26: Hình thể hiện phép đóng .............................................................................. 33
Hình 2.27: Các giai đoạn phát hiện vị trí của vịng trịn ................................................. 33
v


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Hình 2.28: (a) Thay đổi cường độ ảnh, (b) Cực đại của cường dộ ảnh............................ 34
Hình 2.29: Nhân tích chập tìm sự thay đổi theo phương ngang ...................................... 34
Hình 2.30: Nhân tích chập tìm sự thay đổi theo phương ngang. ..................................... 34
Hình 2.32: Phạm vi của góc hướng pixel ....................................................................... 38
Hình 2.33: Mức độ ưu tiên của các điển cần quét dựa trên hướng pixel 2 ...................... 41
Hình 2.34: Mức độ ưu tiên của các điểm ảnh cần quét dựa trên hướng .......................... 41
Hình 2.35: Mức độ ưu tiên của các điểm ảnh cần quét dựa trên hướng pixel 1, 2, 3, 4, 7, 8
...................................................................................................................................... 42
Hình 2.31: Tìm tâm đường trịn ..................................................................................... 43
Hình 2.36: Sơ đồ khối của bộ điều khiển PID ................................................................ 45
Hình 2.37: Giao diện Arduino IDE ............................................................................... 48
Hình 3.1: Sơ đồ khối hệ thống ....................................................................................... 49
Hình 3.2: Sơ đồ nối dây của hệ thống ............................................................................ 49
Hình 3.3: Arduino Mega 2560 R3 .................................................................................. 50
Hình 3.4: Sơ đồi mạch của module MPU 6050. ............................................................. 52
Hình 3.5: Động cơ Nisca NF5475E ............................................................................... 55
Hình 3.6: Sơ đồ mạch điện nguyên lý chung cho cầu H ................................................. 56
Hình 3.7: Điều khiển cầu thuận...................................................................................... 56
Hình 3.8: Điều khiển cầu nghịch.................................................................................... 57

Hình 3.9: Driver điều khiển động cơ XY-160D ............................................................. 58
Hình 3.10: Module HC-05 ............................................................................................. 58
Hình 3.11: Sơ đồ mạch của module HC-05 .................................................................... 60
Hình 3.12: Raspberry pi ................................................................................................. 61
Hình 3.13: Sơ đồ khối cấu trúc Raspberry Pi ................................................................. 62
Hình 3.14: Module Camera Raspberry ........................................................................... 63
Hình 4.1: Quy trình cơng nghệ điều khiển xe hai bánh tự cân bằng................................ 63
Hình 4.2: Quy trình cơng nghệ xử lý, tính tốn, xuất ra dữ liệu ảnh ............................... 63
Hình 4.3: Lưu đồ giải thuật xe hai bánh tự cân bằng ...................................................... 64
vi


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Hình 4.4: Lưu đồ giải thuật nhận dạng quả bóng............................................................ 66
Hình 5.1: Chuyển ảnh sang hệ màu HSV sau khi đã giảm nhiễu hạt .............................. 68
Hình 5.2: Hình đã nhận dạng được quả bóng ................................................................. 69
Hình 5.3: Nhận dạng được quả bóng.............................................................................. 69
Hình 5.4: Mơ phỏng xe hai bánh tự cân bằng ................................................................. 70
Hình 5.5: Sơ đồ khối mô phỏng hệ xe hai bánh giải thuật LQR...................................... 70
Hình 5.6: Đáp ứng ngõ ra mơ phỏng xe hai bánh tự cân bằng LQR ............................... 71
Hình 5.7: Mơ hình thực tế của nhóm.............................................................................. 72
Hình 5.8: Thay đổi góc nghêng (psi) .............................................................................. 72
Hình 5.9: Thay đổi góc tới (theta) .................................................................................. 73
Hình 5.10: Thay đổi góc xoay (phi) ............................................................................... 73
Hình 5.11: Ảnh hưởng của góc tới đến hệ. ..................................................................... 74
Hình 5.12: Ảnh hưởng của góc phi đến hệ. .................................................................... 75

vii


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

DANH SÁCH BẢNG
Bảng 2.1: Kí hiệu và ý nghĩa của các đại lượng ............................................................... 7
Bảng 2.2: Các điều kiện sử dụng xác định hương pixel .................................................. 36
Bảng 2.3: Hình ảnh với cửa sổ tuyến tính màu tương ứng với hướng pixel và độ dốc và
kết quả của NMS ........................................................................................................... 36
Bảng 2.4: Quan hệ của pixel .......................................................................................... 38
Bảng 2.5: Tọa độ của pixel tiếp theo được tính dựa trên mã chuỗi cho pixel hiện tại (5, 5)
...................................................................................................................................... 39
Bảng 3.1: Bảng thông số kỹ thuật Arduino Mega 3560 R3 ............................................ 50
Bảng 3.2 thông số kỹ thuật MPU 6050 .......................................................................... 51
Bảng 3.3: Thông số kỹ thuật Nisca NF5475E ............................................................... 54
Bảng 3.4: Thông số kỹ thuật Driver XY-160D............................................................... 57
Bảng 3.5: Thông số kỹ thuật của Module HC-05 ........................................................... 58
Bảng 3.6: Tham số truyền Baud Rate tương ứng ............................................................ 58
Bảng 3.7: Thông số của Module Camera Raspberry ...................................................... 62

viii


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

DANH MỤC TỪ VIẾT TẮT
ACK : Acknowledged
CPU : Central Processing Unit
IDE

: Integrated Development Environment

I2C

: Inter-Integrated Circuit

GPU : Graphics Processing Unit
HSV : Hue Saturation Value
LQR : Linear Quadratic Regulator
NACK: Non- Acknowledged
NMS : Non – Maximum Suppression
PID

: Proportional Integral Derivative

RGB : Red – Green – Blue
SCL : Serial Clock Line
SDA : Serial Data Line
UART : Universal Asynchronous Receiver and Transmitter

ix


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

TÀI LIỆU THAM KHẢO
Tiếng Việt
[1]
Nguyễn Minh Tâm, Nguyễn Văn Đông Hải, Nguyễn Phong Lưu, Lê Văn Tuấn,
“MƠ HÌNH HỐ VÀ ĐIỀU KHIỂN TỐI ƯU CHO HỆ XE HAI BÁNH TỰ
CÂN BẰNG”
[2]
Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng (2005), “LÝ THUYẾT ĐIỀU KHIỂN
TỰ ĐỘNG” , trang 281-301.
Tiếng Anh
[3]
Grag Welch and Gary Bishop (2006), “A Introduction to the Kalman Filter”, UNCChapel Hill, pp.1-16.
[4]
H. Freeman (1974), “Computer Processing of Line Drawing Images”, Computing
Surveys, pp. 57-97
[5]
M. Z. Zhang and H. R. Cao (2008), “A New Method of Circle’s Center and Radius
Detection in Image Processing”, IEEE Int. Conf. Automation and Logistics, pp. 2239-2242.
[6]
Nguyen Dang Khoa, Nguyen Hoang Hai (2017), “RESEARCH, DESIGN AND
CONSTRUCT TWO-WHEEL BALANCING ROBOT FOR OBJECT TRACKING” 84
Page.

x


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI
1.1. Đặt vấn đề
Robot hai bánh tự bằng là hệ thống phi tuyến và bất ổn định ở mức cao, nên nó ln
là đề tài mn thuở cho nhiều nhà nghiên cứu thử nghiệm sự hiệu quả của nhiều giải thuật
điều khiển khác nhau (PID, LQR, sliding mode control, điều khiển mờ,…).
Trong thực tế, robot được dùng làm phương tiện cá nhân vì khả năng di chuyển linh
hoạt và chiếm ít khơng gian so với phương tiện nhiều bánh xe khác. Đối với mơi trường
nguy hiểm, robot có khả năng di chuyển trong môi trường nguy hiểm hoặc không gian chật
hẹp. Trong vai trị lính cứa hỏa, robot với một camera trong mơt vụ hỏa hoạn, ta có thể
đánh giá mức độ nghiêm trọng, số người mắc kẹt, từ đó giúp lính cứu hỏa giải quyết cơng
việc một cách tốt nhất. Hiểu được nhu cầu của robot, nên nhóm xin chọn đề tài “Xe hai
bánh tự cân bằng giải thuật LQR, ứng dụng xử lý ảnh để bám theo quả bóng”.
1.2. Mục tiêu
Mục tiêu của đề tài là xây dựng phương tiện xe hai bánh cân bằng di chuyển trên địa
hình phẳng, dựa trên nền tảng lý thuyết mơ hình con lắc ngược. Trong khn khổ 16 tuần
thực hiện, luận văn tốt nghiệp đại học, những mục tiêu của đề tài được đề ra như sau:
• Thiết kế bản vẽ phần cứng robot bằng solidworks
• Nghiên cứu và sử dụng các thành phần thiết bị như cảm biến, vi xử lý, động cơ,
bluetooth,…
• Sử dụng bộ lọc Kalman đọc giá trị cảm biến MPU6050.
• Tính tốn động học và xây dựng phương trình robot dùng Euler Lagrange.
• Nghiên cứu và xây dựng bộ điều khiển LQR, PID.
• Kết nối module bluetooth vs bộ điều khiển trung tâm.
• Xử lý ảnh, nhận diện vật trên Raspberry.
• Giải thuật điều khiển vi điều khiển kết hợp Raspberry và tính tốn khoảng cách cho
robot tránh vật.
1.3. Giới hạn
• Xây dựng hệ phương trình động học cho Robot hai bánh tự cân bằng.

• Tính tốn trên Matlab để tìm ra hệ số K cho bộ điều khiển LQR.
• Giao tiếp Bluetooth để điều khiển xe đi tới, đi lui.
• Sử dụng máy tính nhúng Raspberry Pi xử lý ảnh bằng phương pháp nhận dạng màu và
tìm ra bán kính và tọa độ tâm.
1.4. Phương pháp nghiên cứu
1


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Dự án nghiên cứu được chia thành các chương, mỗi chương là một bước tuần tự trong
quá trình phát triển và chế tạo robot cân bằng hai bánh. Cách tiếp cận này được sử dụng
trong nỗ lực tiến triển dự án từ nhiệm vụ này sang nhiệm vụ tiếp theo khi nó được thực
hiện.
• Bước đầu tiên là xác định điểm chính và mục tiêu để hiểu sâu sắc về robot cân bằng hai
bánh thực sự là gì. Bên cạnh đó, điều quan trọng là xác định kế hoạch tiến hành nghiên
cứu của bạn và thực hiện thiết kế và lập trình phù hợp.
• Bước thứ hai là tham khảo các dự án trước đó. Nó thực sự hữu ích để có cách tiếp cận
phù hợp. Điều này thiết lập nền tảng cho việc đưa ra quyết định dựa trên kinh nghiệm
trước đó để tránh những sai lầm và thiết kế lỗi thời.
• Bước thứ ba trong phương pháp lý thuyết là áp dụng kiến thức nghiên cứu vào hệ thống
điều khiển. Cách tiếp cận này cung cấp dữ liệu có giá trị về khả năng ổn định có khả
năng của hệ thống điều khiển bằng cách tìm các thơng số phù hợp trong nhiều ngày.
1.5. Nội dung
Đề án “Xe hai bánh tự cân bằng theo dõi và bám sát mục tiêu ứng dụng xử lý ảnh” gồm
các chương sau:
• Chương 1: Tổng quan hệ thống: Chương này giới thiệu tổng quan về luận văn gồm đặt
vấn đề, mục tiêu, giới hạn và nội dung.

• Chương 2: Cơ sở lý thuyết: Chương này giới thiệu kiến thức cơ bản như nguyên lý cân
bằng của robot, mơ tả phương trình động học của robot, bộ điều khiển LQR và PID, bộ
lọc Kalman và quy trình xử lý ảnh.
• Chương 3: Tính tốn và thiết kế hệ thống: Chương này trình bày việc chọn thiết bị điều
khiển và sơ đồ mạch điện của robot.
• Chương 4: Giải thuật điều khiển: Đây là chương mà những gì nhóm đã làm, sử dụng
ngơn ngữ lập trình, giải thuật để điều khiển cho hệ Robot hai bánh tự cân bằng
• Chương 5: Kết quả và nhận xét: Chương này trình bày kết quả q trình thực hiện robot.
• Chương 6: Kết luận và hướng phát triển: Chương này trình bày vấn đề mà robot thực
hiện và chưa thực hiện được. Đồng thời, đưa ra hướng phát triển, ý tưởng mới nhằm
phát triển robot.

2


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT
2.1. Nguyên lý xe hai bánh tự cân bằng
Đối với các xe ba hay bốn bánh, việc thăng bằng và ổn định của chúng là nhờ trọng
tâm của chúng nằm trong bề mặt chân đế do các bánh xe tạo ra. Đối với các xe 2 bánh có
cấu trúc như xe đạp, việc thăng bằng khi không di chuyển là hồn tồn khơng thể, vì việc
thăng bằng của xe dựa trên tính chất con quay hồi chuyển ở hai bánh xe khi đang quay.
Còn đối với xe hai bánh tự cân bằng, là loại xe chỉ có hai bánh với trục của hai bánh xe
trùng nhau, để cho xe cân bằng, trọng tâm của xe (bao gồm cả người sử dụng chúng) cần
được giữ nằm ngay giữa các bánh xe. Điều này giống như ta giữ một cây gậy dựng thẳng
đứng cân bằng trong lòng bàn tay.
Thực ra, trọng tâm của tồn bộ xe hai bánh khơng được biết nằm ở vị trí nào, cũng

khơng có cách nào tìm ra nó, và có thể khơng có khả năng di chuyển bánh xe đủ nhanh để
giữ nó ln ở dưới tồn bộ trọng tâm.
Về mặt kỹ thuật, góc giữa sàn xe hai bánh và chiều trọng lực có thể biết được. Do
vậy, thay vì tìm cách xác định trọng tâm nằm giữa các bánh xe, tay lái cần được giữ thẳng
đứng, vng góc với sàn xe (góc cân bằng khi ấy là zero).

Hình 2.1: Nguyên lý xe hai bánh tự cân bằng

3


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Hình 2.2: Trạng thái cân bằng và trạng thái di chuyển
Nếu tay lái được đẩy hơi nghiêng tới trước, xe hai bánh sẽ chạy tới trước và khi nó
được đẩy nghiêng ra sau, xe hai bánh sẽ chạy lùi. Đây là một phân tích lý tính. Hầu hết mọi
người đều có thể kiểm sốt tay lái trong vịng vài giây để giữ lấy nó.
Để dừng lại, chỉ cần kéo trọng tâm xe nghiêng ngược hướng đang di chuyển thì tốc
độ xe giảm xuống. Do tốc độ cảm nhận và phản ứng thăng bằng của mỗi người là khác
nhau, nên xe hai bánh tự cân bằng chỉ được thiết kế cho một người sử dụng

Hình 2.3: Xe hai bánh đi lên và đi xuống
2.2. Xây dựng phương trình tốn học [1]
Sơ đồ và hệ quy chiếu hệ xe hai bánh tự cân bằng được thể hiện như hình:

4

ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Hình 2.4: Mơ tả thơng số của xe hai bánh tự cân bằng

Hình 2.5: Mơ tả xe hai bánh tự cân bằng nhìn nghiêng và từ trên xuống
❖ Trong đó, cách thành phần của hệ được liệt kê ở bảng dưới.

5


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Ý nghĩa

Ký hiệu
m (kg)

Khối lượng 2 bánh xe

M (kg)

Khối lượng thân xe

R (m)

Đường kính bánh xe

W (m)

Chiều rộng của thân xe

D (m)

Chiều dài của thân xe

H (m)

Chiều cao của thân xe

L (m)

Khoảng cách giữa trọng tâm thân xe đến trục bánh xe

𝑓𝑤

Hệ số ma sát giữa bánh xe với mặt đường

𝑓𝑚

Hệ số ma sát giữa thân xe với động cơ DC

𝐽𝑚 (𝑘𝑔. 𝑚2 )
𝑅𝑚 (𝛺)
𝐾𝑏 (𝑉

𝑠𝑒𝑐

)
𝑟𝑎𝑑

𝑁𝑚
𝐾𝑡 (
)
𝐴
N
𝑚

g ( 2)
𝑠

𝜃 (𝑟𝑎𝑑)
𝜃𝑙,𝑟 (𝑟𝑎𝑑)

Mơ men qn tính động cơ DC
Trở kháng động cơ DC
Sức điện động cảm ứng của động cơ DC
Mô men lực của động cơ DC
Tỉ số giảm tốc
Gia tốc trọng trường
Góc xoay trung bình của bánh xe trái và phải
Góc xoay của bánh xe trái và phải

ѱ (𝑟𝑎𝑑)

Góc nghiêng khung xe

𝛷 (𝑟𝑎𝑑)

Góc quay của xe

6


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

𝑥𝑙 , 𝑦𝑙 , 𝑧𝑙 (𝑚)

Trục tọa độ của bánh xe trái

𝑥𝑟 , 𝑦𝑟 , 𝑧𝑟 (𝑚)

Trục tọa độ của bánh xe phải

𝑥𝑚 , 𝑦𝑚 , 𝑧𝑚 (𝑚)

Trục tọa độ trung bình

𝐹𝜃 , 𝐹ѱ , 𝐹𝛷 (𝑁𝑚)

Momen phát động theo các hướng khác nhau

𝐹𝑙,𝑟 (𝑁𝑚)

Momen phát động của động bánh trái, phải

𝑖𝑙 , 𝑖𝑟 (𝐴)

Dòng điện động cơ trái và phải

𝑣𝑙 , 𝑣𝑟 (𝑉)

Điện áp động cơ trái và phải

Bảng 2.1: Kí hiệu và ý nghĩa của các đại lượng
Ta 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, xe di chuyển theo chiều dương trục x, ta có góc tịnh tiến trung bình của
hai bánh xe và góc xoay của xe được xác định như sau:
1

(𝜃𝑙 +𝜃𝑟 )
𝜃
[ ] = [ 𝑅2
]
𝜙
(𝜃 −𝜃 )
𝑊

𝑙

(2.1)

𝑟

Tọa độ trung bình của robot trong hệ quy chiếu:
𝑥𝑚

∫ 𝑥̇ 𝑚
[𝑦𝑚 ] = [∫ 𝑦̇𝑚 ]
𝑧𝑚
𝑅
và

𝑥̇
𝑅𝜃̇𝑐𝑜𝑠𝜙
[ 𝑚] = [
]
𝑦̇𝑚
𝑅𝜃̇𝑠𝑖𝑛𝜙

(2.2)

(2.3)

Tọa độ của bánh xe bên trái trong hệ quy chiếu:
𝑊

𝑥𝑚 − 𝑠𝑖𝑛𝜙
𝑥𝑙
2
[𝑦𝑙 ] = [𝑦 + 𝑊 𝑐𝑜𝑠𝜙]
𝑚
2
𝑧𝑙
𝑧𝑚

(2.4)

Tọa độ của bánh xe bên phải trong hệ quy chiếu:
𝑊

𝑥𝑚 + 𝑠𝑖𝑛𝜙
𝑥𝑟
2
[𝑦𝑟 ] = [𝑦 − 𝑊 𝑐𝑜𝑠𝜙]
𝑚
2
𝑧𝑟
𝑧𝑚

(2.5)

7


ĐỒ ÁN TỐT NGHIỆP

GVHD: TS.VŨ VĂN PHONG

Tọa độ tâm giữa hai trục động cơ:
𝑥𝑏
𝑥𝑚 + 𝐿𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜙
𝑦
[ 𝑏 ] = [ 𝑦𝑚 𝐿𝑠𝑖𝑛𝜓𝑠𝑖𝑛𝜙 ]
𝑧𝑏
𝑧𝑚 + 𝐿𝑐𝑜𝑠𝜓

(2.6)

Phương trình động năng của chuyển động tịnh tiến:
1

1

1

2

2

2

𝑇1 = 𝑚(𝑥̇ 𝑙 2 + 𝑦̇ 𝑙 2 + 𝑧̇𝑙 2 ) + 𝑚(𝑥̇ 𝑟 2 + 𝑦̇𝑟 2 + 𝑧̇𝑟 2 ) + 𝑚(𝑥̇ 𝑏 2 + 𝑦̇ 𝑏 2 + 𝑧̇𝑏 2 )

(2.7)

Phương trình động năng của chuyển động quay:
2
2
1
1
1
1
1
1
𝑇2 = 𝐽𝑤 𝜃̇𝑙 + 𝐽𝑤 𝜃̇𝑟 + 𝐽𝜓 𝜓̇ 2 + 𝐽𝜙 𝜙̇ 2 + 𝑛2 𝐽𝑚 (𝜃̇𝑙 − 𝜓̇)2 + 𝑛2 𝐽𝑚 (𝜃𝑟̇ − 𝜓̇)2
2

2

2

2

2

(2.8)

2

1
𝑛2 𝐽𝑚 (𝜃̇𝑙 − 𝜓̇)2 ; 𝑛2 𝐽𝑚 (𝜃𝑟̇ − 𝜓̇)2 là động năng quay của phần ứng động cơ trái

và phải.

(2.9)

Với

1

2

2

Phương trình thế năng:
𝑈 = 𝑚𝑔𝑧𝑙 + 𝑚𝑔𝑧𝑟 + 𝑚𝑔𝑧𝑏

(2.10)

Phương trình Lagrange:
𝐿 = 𝑇1 + 𝑇2 − 𝑈
𝑑

𝜕𝐿

𝜕𝐿

𝑑𝑡 𝜕𝜃

𝜕𝜃

= 𝐹𝜃

(2.12)

𝜕𝜓

= 𝐹𝜓

(2 13)

𝜕𝐿
𝜕𝐿
)−
̇
𝑑𝑡 𝜕𝜙

𝜕𝜙

= 𝐹𝜙

(2.14)

𝑑

( ̇)−

(2.11)

𝜕𝐿
)
𝑑𝑡 𝜕𝜓̇
𝑑

(
(

−

𝜕𝐿

Lấy đạo hàm L theo các biến ta được:
[(2𝑚 + 𝑀)𝑅2 + 2𝐽𝑤 + 2𝑛2 𝐽𝑚 ]𝜃̈ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝐿𝑅𝜓̇ 2 𝑠𝑖𝑛𝜓 = 𝐹𝜃
(2.15)

8