TRƯỜNG ĐẠI HỌC BÁCH KHOA TP.

HỒ CHÍ MINH

KHOA ĐIỆN – ĐIỆN TỬ

BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

TRẦN MẠNH KHẢI

LUẬN VĂN TỐT NGHIỆP

XE HAI BÁNH TỰ CÂN BẰNG BÁM THEO VẬT

XÁC ĐỊNH

KỸ SƯ NGÀNH KỸ THUẬT ĐIỀU KHIỂN & TỰ ĐỘNG HÓA

TP. HỒ CHÍ MINH, 2021

 TRƯỜNG ĐẠI HỌC BÁCH KHOA TP. HỒ CHÍ MINH

KHOA ĐIỆN – ĐIỆN TỬ

BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

TRẦN MẠNH KHẢI - 1711740

LUẬN VĂN TỐT NGHIỆP

XE HAI BÁNH TỰ CÂN BẰNG BÁM THEO VẬT

XÁC ĐỊNH
TWO-WHEEL SELF-BALANCING ROBOT FOR OBJECT TRACKING

KỸ SƯ NGÀNH KỸ THUẬT ĐIỀU KHIỂN & TỰ ĐỘNG HÓA

GIẢNG VIÊN HƯỚNG DẪN

ThS.HUỲNH HỮU PHƯƠNG

TP. HỒ CHÍ MINH, 2021

TRƯỜ NG ĐẠ I HỌ C BÁ CH KHOA TP. HỒ CHÍ MINH CỘ NG HÒ A XÃ HỘ I CHỦ NGHĨA VIỆ T NAM
KHOA ĐIỆN – ĐIỆN TỬ Độ c lậ p - Tự do - Hạ nh phú c
BỘ MÔN: ĐIỀU KHIỂN TỰ ĐỘNG
TP. HCM, ngày….tháng…..năm……..

NHẬN XÉT LUẬN VĂN TỐT NGHIỆP

CỦA CÁN BỘ HƯỚNG DẪN

Tên luận văn:

XE HAI BÁNH TỰ CÂN BẰNG BÁM THEO VẬT XÁC ĐỊNH

Nhóm Sinh viên thực hiện: Cán bộ hướng dẫn:

Trầ n Mạ nh Khả i 1711740 ThS. Huỳnh Hưu Phương

Đánh giá Luận văn

1. Về cuố n bá o cá o:
Số trang ____________ Số chương ____________
Số bả ng số liệu ____________ Số hình vẽ ____________
Số tà i liệu tham khả o ____________ Sả n phẩ m ____________
- Mộ t số nhậ n xét về hình thứ c cuố n bá o cá o:
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
2. Về nộ i dung luậ n vă n:
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
3. Về tính ứ ng dụ ng:
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
4. Về thá i độ là m việc củ a sinh viên:
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
..................................................................................................................................................................
Đánh giá chung:

Điểm từng sinh viên:

Trầ n Mạ nh Khả i :………../10

Cán bộ hướng dẫn

(Ký tên và ghi rõ họ tên)

TRƯỜ NG ĐẠ I HỌ C BÁ CH KHOA TP. HỒ CHÍ MINH CỘ NG HÒ A XÃ HỘ I CHỦ NGHĨA VIỆ T NAM
KHOA ĐIỆN – ĐIỆN TỬ Độ c lậ p - Tự do - Hạ nh phú c
BỘ MÔN: ĐIỀU KHIỂN TỰ ĐỘNG
TP. HCM, ngày….tháng…..năm……..

NHẬN XÉT LUẬN VĂN TỐT NGHIỆP

CỦA CÁN BỘ PHẢN BIỆN

Tên luận văn:

XE HAI BÁNH TỰ CÂN BẰNG BÁM THEO VẬT XÁC ĐỊNH

Nhóm Sinh viên thực hiện: Cán bộ phản biện:

Trầ n Mạ nh Khả i 1711740 ThS. Huỳnh Hữ u Phương

Đánh giá Luận văn

5. Về cuố n bá o cá o:
Số trang ____________ Số chương ____________
Số bả ng số liệu ____________ Số hình vẽ ____________
Số tà i liệu tham khả o ____________ Sả n phẩ m ____________
- Mộ t số nhậ n xét về hình thứ c cuố n bá o cá o:
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
6. Về nộ i dung luậ n vă n:
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
7. Về tính ứ ng dụ ng:
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
8. Về thá i độ là m việc củ a sinh viên:
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
.....................................................................................................................................................................
Đánh giá chung:

Điểm từng sinh viên:

Trầ n Mạ nh Khả i :………../10

Người nhận xét

(Ký tên và ghi rõ họ tên)

 Lời cảm ơn

Lờ i đầ u tiên, em xin gử i lờ i cả m ơn châ n thà nh đến quý thầ y cô trườ ng Đạ i

họ c
Bá ch Khoa TP.HCM nó i chung, và bộ mô n Điều khiển Tự độ ng nó i riêng, đã truyền
dạ y
cho em nhữ ng kiến thứ c và kinh nghiệm quý bá u để hoà n thà nh đề tà i luậ n vă n nà y.
Để hoà n thà nh đượ c đề tà i luậ n vă n thì sự giú p đỡ củ a bạ n bè, cá c anh khó a
trên và đặ c biệt là sự chỉ bả o tậ n tình củ a thầ y hướ ng dẫ n Huỳnh Hữ u Phương trong
quá trình là m luậ n văn.
Cả m ơn đến quý Thầ y/Cô Hộ i đồ ng chấ m luậ n vă n đã dà nh thờ i gian nhậ n xét,
gó p ý để cho em có thêm kiến thứ c, kinh nghiệm.
Lờ i cuố i cù ng, em xin cả m ơn gia đình, ngườ i thâ n, bạ n bè đã luô n quan tâ m,
tiếp độ ng lự c cho em trong quá trình là m luậ n vă n.
Trâ n trọ ng.

Sinh viên
Trầ n Mạ nh Khả i
TRƯỜ NG ĐẠ I HỌ C BÁ CH KHOA TP. HỒ CHÍ MINH CỘ NG HÒ A XÃ HỘ I CHỦ NGHĨA VIỆ T NAM
KHOA ĐIỆN – ĐIỆN TỬ Độ c lậ p - Tự do - Hạ nh phú c
BỘ MÔN: ĐIỀU KHIỂN TỰ ĐỘNG
TP. HCM, ngày….tháng…..năm……..

ĐỀ CƯƠNG CHI TIẾT

TÊN ĐỀ TÀI: XE HAI BÁNH TỰ CÂN BẰNG BÁM THEO VẬT XÁC ĐỊNH

Cán bộ hướng dẫn: Ths. Huỳnh Hữu Phương

Thời gian thực hiện: Từ ngà y………………..đến ngà y……………

Sinh viên thực hiện:

Trần Mạnh Khải - 1711740

Nội dung đề tài: Xe hai bá nh tự câ n bằ ng bá m theo vậ t xá c định.

Kế hoạch thực hiện: (Mô tả kế hoạch làm việc và phân công công việc cho từng sinh
viên tham gia)

Xác nhận của Cán bộ hướng dẫn TP. HCM, ngà y….thá ng …..nă m…….

(Ký tên và ghi rõ họ tên) Sinh viên

(Ký tên và ghi rõ họ tên)

 DANH SÁCH HỘI ĐỒNG BẢO VỆ LUẬN VĂN

Hộ i đồ ng chấ m luậ n vă n tố t nghiệp, thà nh lậ p theo Quyết định số ……………………

ngà y ………………….. củ a Hiệu trưở ng Trườ ng Đạ i họ c Bá ch khoa TP.HCM.

1. …………………………………………. – Chủ tịch

2. …………………………………………. – Thư ký
3. …………………………………………. – Ủ y viên
4. …………………………………………. – Ủ y viên
5. …………………………………………. – Ủ y viên
 MỤC LỤC

TÓ M TẮ T LUẬ N VĂ N..............................................................................................................................

Chương 1. GIỚ I THIỆ U ĐỀ TÀ I.........................................................................................................

1.1. Lý do chọ n đề tà i............................................................................................................2

1.2. Mụ c tiêu củ a đề tà i.........................................................................................................2

1.3. Tình hình nghiên cứ u xe hai bá nh hiện nay........................................................3

1.3.1. Trong nướ c...........................................................................................................3

1.3.2. Ngoà i nướ c...........................................................................................................4

Chương 2. CƠ SỞ LÝ THUYẾ T VÀ PHƯƠNG PHÁ P THỰ C HIỆ N.........................................

2.1. Nguyên lý hoạ t độ ng củ a xe hai bá nh tự câ n bằ ng...........................................6

2.2. Đặ c tính độ ng lự c họ c...................................................................................................7

2.2.1. Mô hình hó a robot 2 bá nh tự câ n bằ ng trên địa hình phẳ ng...........7

2.2.2. Mô hình hó a robot trong Matlab Simulink.........................................11

2.3. Bộ điều khiển PID........................................................................................................13

2.4. Bộ lọ c Kalman...............................................................................................................15

2.4.1. Giớ i thiệu về bộ lọ c Kalman........................................................................15

2.4.2. Thuậ t toá n bộ lọ c Kalman rờ i rạ c.............................................................16

2.5. Phầ n xử lý ả nh..............................................................................................................18

Chương 3. THIẾ T KẾ VÀ THỰ C HIỆ N..........................................................................................

3.1. Thiết kế phầ n cứ ng.....................................................................................................20

3.1.1. Tổ ng quan về mô hình..................................................................................20

3.1.2. Cá c thà nh phầ n chính củ a mô hình..........................................................21

3.2. Thiết kế giả i thuậ t.......................................................................................................28

 3.2.1. Sơ đồ khố i hệ thố ng.......................................................................................28

3.2.2. Lưu đồ giả i thuậ t điều khiển củ a hệ thố ng...........................................29

3.2.3. Lưu đồ giả i thuậ t xử lý ả nh.........................................................................31

3.3. Mô phỏ ng Matlab Simulink.....................................................................................33

3.4. Cấ u hình cá c châ n và chế độ hoạ t độ ng trên STM32CubeMx....................35

3.5. Cấ u hình thanh ghi cả m biến MPU6050.............................................................39

3.6. Xử lý tín hiệu củ a cả m biến MPU6050 qua bộ lọ c Kalman.........................42

3.7. Tính toá n tiêu cự và khoả ng cá ch trên Raspberry Pi...................................45

3.8. Thiết kế giao diện C# giá m sá t thô ng số ............................................................47

Chương 4. KẾ T QUẢ THỰ C HIỆ N..................................................................................................

4.1. Đá p ứ ng cá c thô ng số mô phỏ ng trên Matlab Simulink...............................48

4.1.1. Gó c nghiêng Angle..........................................................................................48

4.1.2. Gó c xoay Rotation...........................................................................................48

4.1.3. Khoả ng cá ch Distance...................................................................................49

4.2. Kết quả thự c hiện xử lý ả nh trên Raspberry Pi...............................................50

4.3. Đá p ứ ng cá c thô ng số củ a mô hình thự c............................................................50

4.3.1. Kiểm chứ ng sự hiệu quả củ a bộ lọ c Kalman và tìm giá trị nhiễu đo
lườ ng R. 50

4.3.2. Gó c nghiêng Angle..........................................................................................53

4.3.3. Gó c xoay Rotation...........................................................................................54

4.3.4. Khoả ng cá ch Distance...................................................................................56

Chương 5. KẾ T LUẬ N VÀ HƯỚ NG PHÁ T TRIỂ N.....................................................................

5.1. Kết quả đạ t đượ c.........................................................................................................60

5.2. Nhữ ng hạ n chế.............................................................................................................60

 5.3. Hướ ng phá t triển.........................................................................................................61

TÀ I LIỆ U THAM KHẢ O.........................................................................................................................

 DANH MỤC HÌNH ẢNH
Hình 1.1: HomeSheel HP.............................................................................................................3
Hình 1.2: Homesheel MI..............................................................................................................4
Hình 1.3: SEGWAY (Hoa Kì).......................................................................................................4
Hình 1.4: Robot EMIEW (Nhậ t Bả n).......................................................................................5
Hình 1.5: Toyota I-SWING (Nhậ t Bả n)...................................................................................5
Hình 2.1: Nguyên lý hoạ t độ ng củ a robot.............................................................................6
Hình 2.2: Điều khiển robot trong cá c điều kiện bề mặ t khá c nhau............................7
Hình 2.3: Tổ ng quan mô hình xe hai bá nh câ n bằ ng........................................................8
Hình 2.4: Mô hình xe hai bá nh câ n bằ ng nhìn tờ trên xuố ng.......................................8
Hình 2.5: Mô hình xe hai bá nh câ n bằ ng nhìn từ cạ nh bên...........................................9
Hình 2.6: Mô hình xe hai bá nh câ n bằ ng trong Matlab Simulink.............................11
Hình 2.7: Bên trong khố i Two-wheel balancing robot.................................................12
Hình 2.8: Bên trong khố i decoupling...................................................................................13
Hình 2.9: Sơ đồ khố i bộ điều khiển PID..............................................................................14
Hình 2.10: Cá c quá trình trong bộ lọ c Kalman.................................................................18
Hình 2.11:Khô ng gian mà u HSV............................................................................................18
Hình 3.1: Mô hình thự c tế xe hai bá nh tự câ n bằ ng.......................................................20
Hình 3.2: Pin Lipo 2200mAh...................................................................................................21
Hình 3.3: Sạ c dự phò ng Xmobile 5V-3A.............................................................................21
Hình 3.4: Board STM32F407VGT6.......................................................................................21
Hình 3.5: Raspberry Pi 3B+.....................................................................................................22
Hình 3.6: Raspberry Camera V2.1........................................................................................23
Hình 3.7: Độ ng cơ JGB37-520.................................................................................................23
Hình 3.8: Module điều khiển độ ng cơ TB6612FNG.......................................................25
Hình 3.9: Sơ đồ châ n Module TB6612FNG........................................................................25
Hình 3.10: MPU6050..................................................................................................................26
Hình 3.11: Module Bluetooth HC-05...................................................................................27
Hình 3.12: Sơ đồ khố i chứ c nă ng củ a toà n bộ hệ thố ng...............................................28
Hình 3.13: Lưu đồ giả i thuậ t toà n bộ hệ thố ng................................................................29
Hình 3.14: Thuậ t toá n xử lý ả nh củ a đề tà i.......................................................................31
Hình 3.15: Ngưỡ ng mà u HSV vớ i V=255............................................................................32
Hình 3.16: Mô hình điều khiển xe hai bá nh tự câ n bằ ng trên Simulink................33
Hình 3.17: Cấ u trú c chung củ a ba bộ điều khiển PID....................................................33
Hình 3.18: Thô ng số củ a mô hình xe hai bá nh câ n bằ ng trên Simulink Matlab. 34
Hình 3.19: Pinout view on STM32CubeMX.......................................................................35
Hình 3.20: System view on STM32CubeMX......................................................................36
Hình 3.21: Cấ u hình DMA.........................................................................................................36
Hình 3.22: Cấ u hình châ n GPIO Output..............................................................................36
Hình 3.23: Cấ u hình châ n Timer2........................................................................................37
Hình 3.24: Cấ u hình châ n Timer5.........................................................................................37
Hình 3.25: Cấ u hình châ n Timer3.........................................................................................37
Hình 3.26: Cấ u hình counter cho Timer3..........................................................................37
Hình 3.27: Cấ u hình chế độ ngắ t cho Timer4...................................................................37
Hình 3.28: Cấ u hình counter cho Timer4..........................................................................38
Hình 3.29: Cấ u hình châ n I2C3...............................................................................................38
Hình 3.30: Cấ u hình châ n UART2..........................................................................................38
Hình 3.31: Cấ u hình chế độ UART2......................................................................................38
Hình 3.32: Cấ u hình châ n UART3..........................................................................................39
Hình 3.33: Cấ u hình chế độ UART3......................................................................................39
Hình 3.34: WHO_AM_I Register.............................................................................................39
Hình 3.35: Power Management Register...........................................................................39
Hình 3.36: Cá c chế độ chọ n xung nguồ n clock.................................................................40
Hình 3.37: Sample Rate Divider Register..........................................................................40
Hình 3.38: Accelerometer Configuration Register.........................................................40
Hình 3.39: Cá c tầ m đo củ a Accelerometer........................................................................40
Hình 3.40: Gyroscope Configuration Register.................................................................41
Hình 3.41: Cá c tầ m đo củ a Gyroscope.................................................................................41
Hình 3.42: Thanh ghi 59 đến 64 – Accelerometer Measurements..........................41
Hình 3.43: Tầ m đo củ a thanh ghi 59 đến 64 – Accelerometer Measurements.. 41
Hình 3.44: Thanh ghi 67 đến 72 – Gyroscope Measurements..................................41
Hình 3.45: Tầ m đo củ a thanh ghi 67 đến 72 – Gyroscope Measurements...........42
Hình 3.46: Mộ t số cô ng thứ c tính qua ố ng kính camera..............................................45
Hình 3.47: Giao diện C# theo dõ i cá c thô ng số đá p ứ ng củ a mô hình....................47
Hình 4.1: Đá p ứ ng gó c nghiêng Angle trên Matlab Simulink.....................................48
Hình 4.2: Đá p ứ ng gó c xoay Rotation trên Matlab Simulink......................................48
Hình 4.3: Đá p ứ ng khoả ng cá ch Distance trên Matlab Simulink..............................49
Hình 4.4: Kết quả thưc hiện xử lý ả nh trên Raspberry Pi...........................................50
Hình 4.5: Đá p ứ ng gó c nghiêng củ a hệ thố ng vớ i nhiễu đo lườ ng R=0.05...........51
Hình 4.6: Đá p ứ ng gó c nghiêng củ a hệ thố ng vớ i nhiễu đo lườ ng R=0.2..............51
Hình 4.7: Đá p ứ ng gó c nghiêng củ a hệ thố ng vớ i nhiễu đo lườ ng R=0.5..............52
Hình 4.8: Đá p ứ ng gó c nghiêng củ a hệ thố ng vớ i nhiễu đo lườ ng R=0.8..............52
Hình 4.9: Đá p ứ ng gó c nghiêng củ a hệ thố ng khi khô ng có vậ t nặ ng.....................53
Hình 4.10: Đá p ứ ng gó c nghiêng củ a hệ thố ng khi đặ t vậ t 1.5kg lên xe................53
Hình 4.11: Đá p ứ ng gó c xoay củ a hệ thố ng vớ i gó c xoay ban đầ u là 250, khô ng
có vậ t nặ ng đặ t lên xe................................................................................................................ 54
Hình 4.12: Đá p ứ ng gó c xoay củ a hệ thố ng vớ i gó c xoay ban đầ u là = 250, đặ t
vậ t 1,5kg lên xe............................................................................................................................. 54
Hình 4.13: Đá p ứ ng gó c xoay củ a hệ thố ng vớ i gó c xoay ban đầ u là -300, khô ng
có vậ t nặ ng đặ t lên xe................................................................................................................ 55
Hình 4.14: Đá p ứ ng gó c xoay củ a hệ thố ng vớ i gó c xoay ban đầ u là - 250, đặ t
vậ t 1,5kg lên xe............................................................................................................................. 55
Hình 4.15: Đá p ứ ng khoả ng cá ch củ a hệ thố ng khi khoả ng cá ch đặ t distance_ref
=20 cm và khoả ng cá ch ban đầ u là 50 cm.........................................................................56
Hình 4.16: Đá p ứ ng khoả ng cá ch củ a hệ thố ng khi khoả ng cá ch đặ t distance_ref
=50 cm và khoả ng cá ch ban đầ u là 20 cm.........................................................................57
Hình 4.17: Đá p ứ ng khoả ng cá ch củ a hệ thố ng khi khoả ng cá ch đặ t distance_ref
=50 cm và khoả ng cá ch ban đầ u là 79 cm.........................................................................57
Hình 4.18: Đá p ứ ng khoả ng cá ch củ a hệ thố ng khi khoả ng cá ch đặ t distance_ref
=60 cm và khoả ng cá ch ban đầ u là 35 cm.........................................................................58
Hình 4.19: Đá p ứ ng khoả ng cá ch củ a hệ thố ng khi khoả ng cá ch đặ t distance_ref
=60 cm và khoả ng cá ch ban đầ u là 80 cm.........................................................................58

DANH MỤC BẢNG

Bả ng 2.1: Ký hiệu và ý nghĩa củ a cá c đạ i lượ ng...............................................................10
Bả ng 2.2: Ả nh hưở ng củ a cá c hệ số Kp, Ki, Kd lên chấ t lượ ng điều khiển...............15
Bả ng 3.1: Tiêu cự củ a camera trong 1m.............................................................................46
Bả ng 3.2: Khoả ng cá ch đo qua camera so vớ i khoả ng cá ch thự c.............................46
 TÓM TẮT LUẬN VĂN

Đề tà i về điều khiển xe hai bá nh tự câ n bằ ng là đề tà i đã phổ biến đố i vớ i cá c

bạ n sinh viên trong nhữ ng nă m gầ n đâ y. Nó phổ biến như vậ y bở i lẻ vì nhu
cầ u thự c tế củ a robot câ n bằ ng ngà y cà ng cao trong nhiều lĩnh vự c, vì thế việc
nghiên cứ u về đề tà i nà y là rấ t có ích cho việc nghiên cứ u họ c tậ p cũ ng như có
thể đó ng gó p cho ngà nh cô ng nghiệp sả n xuấ t ra cá c sả n phẩ m liên quan củ a
Việt Nam sau này.

Để điều khiển đượ c robot hai bá nh câ n bằ ng có thể á p dụ ng nhiều phương

phá p điều khiển như PID, fuzzy, LQR, điều khiển trượ t…Mỗ i phương phá p
điều khiển sẽ có ưu, nhượ c điểm riêng ứ ng vớ i từ ng mẫ u robot câ n bằ ng. Ở đề
tà i luậ n vă n tố t nghiệp nà y, mụ c tiêu củ a đề tà i sẽ là điều khiển xe hai bá nh
câ n bằ ng bá m theo vậ t. Vì mô hình đượ c thiết kế bằ ng thủ cô ng nên chỉ có
điều khiển bằ ng bộ PID là phù hợ p nhấ t, khô ng yêu cầ u tính toá n chính xá c
cá c thô ng số vậ t lý củ a mô hình mà chỉ cầ n điều chỉnh cá c hệ số Kp, Ki, Kd là
có thể đá p ứ ng cá c ngõ ra vớ i cá c chỉ tiêu mong muố n trong phạ m vi họ c tậ p,
nghiên cứ u. Luậ n vă n này sẽ chỉ ra từ ng bướ c cá ch á p dụ ng cá c cơ sở lý
thuyết điều khiển cũ ng như xử lý ả nh và o mô hình thự c tế củ a xe hai bá nh tự
câ n bằ ng bá m theo vậ t xá c định.

1
Chương 1. GIỚI THIỆU ĐỀ TÀI

1.1. Lý do chọn đề tài

Xe hai bá nh tự câ n bằ ng là ứ ng dụ ng củ a hệ con lắ c ngượ c rấ t quen thuộ c vớ i

tấ t cả cá c bạ n sinh viên họ c về điều khiển tự độ ng. Đâ y là đố i tượ ng phi tuyến
MIMO mà chú ng ta thườ ng dù ng trong cá c phò ng thí nghiệm để kiếm chứ ng
thự c nghiệm cá c giả i thuậ t điển khiển. Xe hai bá nh tự câ n bằ ng là hệ MIMO chỉ
có hai và o là hai độ ng cơ nhưng lạ i có ba ra là tớ i lui, quay trá i phả i, đứ ng yên
khô ng ngã nên để điều khiển hệ này khô ng phả i là chuyện dễ dà ng. Ở trên thế
giớ i cá c nướ c phá t triển đã sả n xuấ t ra rấ t nhiều sả n phẩ m đượ c ứ ng dụ ng từ
mô hình nà y và đã thương mạ i thà nh cô ng nhưng ở Việt Nam thì cá c cô ng ty
sả n xuấ t cá c sản phẩ m tương tự là vẫ n cò n khá ít. Vớ i cá c lý do đó thì em xin
chọ n đề tà i nà y để có thể nghiên cứ u, kiểm chứ ng cá c kiến thứ c điều khiển cơ
bả n đượ c họ c và o trong mô hình và để có cá i nhìn thự c tế về cá c sả n phẩ m xe
hai bá nh tự câ n bằ ng ngoà i thự c tế.

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

Mụ c tiêu củ a đề tà i này là phả i thự c hiện đượ c xe hai bá nh tự câ n bằ ng bá m

theo vậ t xá c định bằ ng mà u sắ c vớ i mộ t khoả ng cá ch nhấ t định. Xe luô n câ n
bằ ng ngay cả khi chịu tá c độ ng củ a ngoạ i lự c, hướ ng bá m và khoả ng cá ch đến
vậ t phả i chính xá c và đá p ứ ng nhanh nhấ t có thể. Để thự c hiện mụ c tiêu đó thì
trong thờ i gian thự c hiện cầ n đặ t ra cá c mụ c tiêu cụ thể như sau:

- Tìm hiểu mô hình xe hai bá nh tự câ n bằ ng và cá c cơ sở lý thuyết về

nguyên lý câ n bằ ng xe.
- Tìm hiểu cá c phương phá p điều khiển xe hai bá nh tự câ n bằ ng và chọ n
ra phương phá p điều khiển phù hợ p là PID.
- Thiết kế và mô phỏ ng điều khiển mô hình trên MatLab Simulink.
- Tìm hiểu cá ch dù ng STM32CubeMX để cấ u hình cá c châ n, chế độ cho
mạ ch điều khiển.

2
 - Tìm hiểu về cả m biến MPU6050 về cá c cấ u hình và cá c dù ng bộ lọ c
Kalman để lọ c nhiễu cá c tín hiệu đo đượ c.
- Tìm hiểu cá ch cầ u hình hệ điều hà nh cho Raspberry Pi và cá ch kết nố i
Raspberry vớ i mà n hình má y tính.
- Tìm hiểu về thư viện OpenCv, lậ p trình python để xử lý ả nh vậ t như
phá t hiện vậ t, đo khoả ng cá ch qua Camera Raspberry.
- Nghiên cứ u giả i thuậ t để kết hợ p truyền nhậ n cá c tín hiệu giữ a
Raspberry vớ i kit STM32F407.
- Chạ y thử và chọ n ra cá c thô ng số điểu khiển Kp, Ki, Kd để xe câ n bằ ng
và bá m theo vậ t tố t nhấ t.
- Tìm hiểu về lậ p trình C# để vẽ biểu đồ quan sá t cá c thô ng số đặ c trưng
cầ n quan tâ m củ a mô hình.

1.3. Tình hình nghiên cứu xe hai bánh hiện nay

1.3.1. Trong nước

Cá ch đâ y và i nă m thì khô ng có cô ng ty nà o sả n xuấ t về sả n phẩ m này

chỉ chủ yếu là cá c mô hình cơ bả n củ a cá c sinh viên kỹ thuậ t nhưng hiện nay
đã có mộ t và i cô ng ty đã sả n xuấ t và đã đạ t nhiều thà nh cô ng trong việc
thương mạ i như cô ng ty TNHH TMDV Homesheel. Cá c sả n phẩ m về xe điện
hai bá nh câ n bằ ng củ a hãng Homesheel như hình dướ i:

Hình 1.1: HomeSheel HP

3
 Hình 1.2: Homesheel MI
1.3.2. Ngoài nước

Cá c sả n phẩ m robot hai bá nh tự câ n bằ ng đã rấ t phổ biến rấ t nhiều ở

cá c cá ch cô ng nghiệp củ a cá c nướ c phá t triển như: Mỹ, Nhậ t Bả n, Hà n Quố c…
Vớ i nhiều loạ i hình biến tấ u theo nhiều kiểu dá ng và cô ng dụ ng khá c nhau,
phù hợ p vớ i đa dạ ng mô i trườ ng là m việc như robot thể thao, robot phụ c vụ ,
robot trong nhà má y sả n xuấ t. Sau đâ y là mộ t số hã ng robot câ n bằ ng nổ i
tiếng:

Hình 1.3: SEGWAY (Hoa Kì)

4
 Hình 1.4: Robot EMIEW (Nhật Bản)

Hình 1.5: Toyota I-SWING (Nhật Bản)

5
Chương 2. CƠ SỞ LÝ THUYẾT VÀ PHƯƠNG PHÁP THỰC HIỆN

2.1. Nguyên lý hoạt động của xe hai bánh tự cân bằng

Xe hai bá nh tự câ n bằ ng hoạ t độ ng dự a và o mô hình con lắ c ngượ c gắ n vớ i hệ

hai bá nh xe đượ c điều khiển độ c lậ p vớ i nhau:

Hình 2.6: Nguyên lý hoạt động của robot

Khi robot đứ ng câ n bằ ng thì gó c nghiêng θ củ a thâ n robot và trụ c thẳ ng đứ ng
(trụ c củ a lự c trọ ng trườ ng) bằ ng 0.

Khi robot nghiêng về phía trướ c θ > 0, nếu khô ng có điều khiển thì theo quá n
tính, hai bá nh xe sẽ chạ y về phía sau (phía ngượ c lạ i củ a thâ n robot đang
nghiêng), dẫ n đến robot sẽ bị ngã . Nên trong trườ ng hợ p nà y, chú ng ta sẽ
điều khiển cho 2 bá nh xe chạ y về phía trướ c (phía mà robot đang nghiêng)
nhằ m cho gó c lệch θ = 0, robot sẽ câ n bằ ng trở lạ i. Tương tự , trong trườ ng
hợ p khi robot nghiêng về phía sau θ < 0, chú ng ta sẽ điều khiển cho 2 bá nh xe
chạ y về phía sau (phía mà robot đang nghiêng) nhằ m cho gó c lệch θ = 0, robot
sẽ thă ng bằ ng trở lạ i.

6
Dướ i đâ y là hình ả nh minh họ a điều khiển robot trong cá c điều kiện về mặ t
khá c nhau:

Hình 2.7: Điều khiển robot trong các điều kiện bề mặt khác nhau

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

2.2.1. Mô hình hóa robot 2 bánh tự cân bằng trên địa hình phẳng

Khả o sá t mô hình xe hai bá nh tự câ n bằ ng:

7
 Hình 2.8: Tổng quan mô hình xe hai bánh cân bằng

Hình 2.9: Mô hình xe hai bánh cân bằng nhìn tờ trên xuống

8
 Hình 2.10: Mô hình xe hai bánh cân bằng nhìn từ cạnh bên

Kí hiệu Đơn vị Ý nghĩa

m kg Khố i lượ ng củ a bá nh xe

M kg Khố i lượ ng củ a robot

R m Bá n kính bá nh xe

W m Chiều rộ ng củ a robot

D m Chiều ngang củ a robot

H m Chiều cao củ a robot

L m Khoả ng cá ch từ trọ ng tâ m củ a 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

9
 fm Hệ số ma sá t giữ a robot và độ ng cơ DC

Jm kgm2 Moment quá n tính độ ng cơ DC

Rm Ω Điện trở độ ng cơ DC

Kb Vs/rad Hệ số EFM củ a độ ng cơ DC

Kt Nm/A Moment xoắ n củ a độ ng cơ DC

N Tỉ số giả m tố c

G m/s2 Gia tố c trọ ng 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 Tọ a độ bá nh trá i

xr, yr, zr m Tọ a độ bá nh phả i

xm, ym, zm m Tọ a độ trung bình

FӨ, Fψ, Fɸ Nm Moment phá t độ ng theo cá c phương khá c nhau

Fl,r Nm Moment phá t độ ng củ a độ ng cơ bá nh trá i, phả i

il, ir A Dò ng điện độ ng cơ bá nh trá i, phả i

vl, vr V Điện á p độ ng cơ bá nh trá i, phả i

Bảng 2.1: Ký hiệu và ý nghĩa của các đại lượng

10
 Phương trình động lực học mô tả chuyển động của robot như sau:

[ ( 2 m+ M ) R2 +2 J w +2 n2 J m ] Ӫ+( MLRcosψ −2 n2 J m ) ψ̈−MLR ψ̇ 2 sinψ=α ( v l +v r )−2 ( β +f w) Ө̇+2 β ψ̇

(2.1)

( MLRcosψ −2 n2 J m )Ӫ+ ( M L2 + J w +2 n2 J m ) ψ̈−MgLsinψ−M L2 ɸ̇2 sinψcosψ=−α ( v l+ v r ) + 2 β Ө̇−2 β ψ̇

(2.2)

[ 1
2
W2
2R
2 2
]
mW + J ɸ + 2 ( J w +n J m ) + M L sin ψ ɸ̈ + 2 M L ψ̇ ɸ̇ sinψcosψ=
2 2 2 2 W
2R
W2
α ( v l−v r ) − 2 ( β + f w ) ɸ̇
2R

(2.3)

2.2.2. Mô hình hóa robot trong Matlab Simulink

Hình 2.11: Mô hình xe hai bánh cân bằng trong Matlab Simulink

11
 Hình 2.12: Bên trong khối Two-wheel balancing robot
Phương trình độ ng lự c họ c củ a robot như trên thể hiện mố i quan hệ
giữ a giá trị điện á p điều khiển hai độ ng cơ; bao gồ m độ nghiêng, vị trí, vậ n tố c
củ a hệ robot, giá trị điện á p hai độ ng cơ vl , v r tá c độ ng lên cá c thô ng số đó dướ i
dạ ng tổ ng vl + v r cò n vớ i gó c xoay, giá trị điện á p hai độ ng cơ vl , v r tá c độ ng lên
thô ng số nà y dướ i dạ ng hiệu v r−v l. Khi đó , tá ch bà i toà n hệ robot thà nh hai
bà i toá n nhỏ hơn vớ i hai tín hiệu điều khiển V ψ ,V ϕ

{
1
v l = (V ψ −V ϕ )
{
V ψ =v l +v r
V ϕ =v r −v l
=¿ 2
1
v r = (V ψ +V ϕ )
(2.4)
2

Khố i thự c hiện chứ c nă ng này gọ i là khố i phâ n tá ch (decoupling)

12
 Hình 2.13: Bên trong khối decoupling
2.3. Bộ điều khiển PID

PID – bộ điều khiển vi tích phâ n tỉ lệ, là mộ t cơ chế phả n hồ i vò ng điều khiển
tổ ng quá t đượ c sử dụ ng rộ ng rã i trong cá c hệ thố ng điều khiển cô ng nghiệp –
bộ điều khiển PID là bộ điều khiển đượ c sử dụ ng nhiều nhấ t trong cá c bộ điều
khiển phả n hồ i. Bộ điều khiển PID sẽ tính giá trị “sai số ” là hiệu số giữ a giá trị
đo thô ng số biến đổ i và giá trị đặ t mong muố n. Bộ điều khiển sẽ thự c hiện
giả m tố i đa sai số bằ ng cá ch điều chỉnh giá trị điều khiển đầ u và o.

Bộ điều khiển PID đượ c sử dụ ng rấ t rộ ng rã i trong thự c tế để điều khiển

nhiều loạ i đố i tượ ng khá c nhau như nhiệt độ lò nhiệt, tố c độ độ ng cơ, mự c
chấ t lỏ ng trong bồ n chứ a... do nó có khả năng là m triệt tiêu sai số xá c lậ p, tă ng
tố c độ đá p ứ ng quá độ , giả m độ vọ t lố nếu cá c thô ng số củ a bộ điều khiển
đượ c chọ n lự a thích hợ p. Do tính thô ng dụ ng củ a nó nên nhiều hã ng sả n xuấ t

13
thiết bị điều khiển đã cho ra đờ i cá c bộ điều khiển PID thương mạ i rấ t tiện
dụ ng. [1]

Hình 2.14: Sơ đồ khối bộ điều khiển PID.

Luậ t điều khiển PID trong miền thờ i gian liên tụ c:

u(t) = KP*e(t) + KD*(de(t)/dt) + KI*ʃe(t)dt (2.5)

Luậ t điều khiển PID trong miền rờ i rạ c:

(2.6)

Điều chỉnh tỉ lệ (P): là phương phá p điều chỉnh tạ o ra tín hiệu điều chỉnh tỉ lệ
vớ i sai lệch đầ u và o.

Điều chỉnh tích phâ n (I): là phương phá p điều chỉnh tỉ lệ để lạ i mộ t độ lệch
(offset) sau điều chỉnh rấ t lớ n. Để khắ c phụ c ta sử dụ ng kết hợ p điều chỉnh tỉ
lệ vớ i điều chỉnh tích phâ n. Điều chỉnh tích phâ n là phương phá p điều chỉnh
tạ o ra tín hiệu điều chỉnh sao cho độ lệch giả m tớ i 0. Thờ i gian cà ng nhỏ thể
hiện tá c độ ng điều chỉnh tích phâ n cà ng mạ nh, ứ ng vớ i độ lệch cà ng bé.

 Điều chỉnh vi phâ n (D): khi hằ ng số thờ i gian hoặ c thờ i gian chết củ a hệ
thố ng rấ t lớ n điều chỉnh theo P hoặ c PI có đá p ứ ng quá chậ m thì ta sử dụ ng

14
kết hợ p vớ i điều chỉnh vi phâ n. Điều chỉnh vi phâ n tạ o ra tín hiệu điều chỉnh
sao cho tỉ lệ vớ i tố c độ  thay đổ i sai lệch đầ u và o. Thờ i gian cà ng lớ n thì điều
chỉnh vi phâ n cà ng mạ nh, ứ ng vớ i bộ điều chỉnh đá p ứ ng vớ i thay đổ i đầ u và o
cà ng nhanh.

Điều khiển tỉ lệ (KP) có ả nh hưở ng là m giả m thờ i gian lên và sẽ là m giả m

nhưng khô ng loạ i bỏ sai số xá c lậ p. Điều khiển tích phâ n (KI) sẽ loạ i bỏ sai số
xá c lậ p nhưng có thể là m đá p ứ ng quá độ xấ u đi. Điều khiển vi phâ n (KD) có
tá c dụ ng là m tă ng sự ổ n định củ a hệ thố ng, giả m vọ t lố và cả i thiện đá p ứ ng
quá độ . Ả nh hưở ng củ a mỗ i bộ điều khiển KP, KI và KD lên hệ thố ng vò ng kín
đượ c tó m tắ t ở bả ng bên dướ i.

Thông Thời gian lên Độ vọt lố Thời gian xác Sai số xác lập
số lập

Kp Giảm Tăng Thay đổi nhỏ Giảm

Ki Giảm Tăng Tăng Loại bỏ

Kd Thay đổi nhỏ Giảm Giảm Thay đổi nhỏ

Bảng 2.2: Ảnh hưởng của các hệ số Kp, Ki, Kd lên chất lượng điều khiển
2.4. Bộ lọc Kalman

2.4.1. Giới thiệu về bộ lọc Kalman

Bộ lọ c Kalman đượ c giớ i thiệu lầ n đầ u tiên và o nă m 1960 bở i Rudolf E.

Kalman (1930 – 2016), mộ t kỹ sư điện, nhà toá n họ c, nhà phá t minh ngườ i
Mỹ gố c Hungary. Thự c tế đã chứ ng tỏ bộ lọ c Kalman là mộ t khá m phá tuyệt
vờ i trong lĩnh vự c “Statistical Estimation Theory”, cũ ng như là mộ t trong
nhữ ng khá m phá quan trọ ng nhấ t thế kỷ 20.

Mặ c dù Bộ lọ c Kalman đượ c ứ ng dụ ng trong nhiều lĩnh vự c, chẳ ng hạ n

như Process control, Tracking, Location & Navigation system,… nhưng nó

15
đượ c sử dụ ng chủ yếu vớ i 2 mụ c đích chính là : Estimating the state of
dynamic system (Ướ c tính trạ ng thá i củ a hệ thố ng độ ng) và the Analysis of
Estimation Systems. Ở đề tà i này thì bộ lọ c Kalman dù ng cho mụ c đích ướ c
tính trạ ng thá i củ a hệ thố ng độ ng củ a hai tín hiệu củ a cả m biến MPU6050 là
gia tố c gó c (accel) và vậ n tố c gó c (gyro).

2.4.2. Thuật toán bộ lọc Kalman rời rạc

Các khái niệm và ký hiệu:

 System state, ⃗x – vector chứ a cá c biến trạ ng thá i củ a hệ thố ng.

 Measurement values, ⃗z k – vector chứ a cá c giá trị trạ ng thá i đo đượ c tạ i
thờ i điểm k.
 Control input,u⃗ k – vector chứ a giá trị input củ a hệ thố ng tạ i thờ i điểm k;
 Posteriori state, ^x k∨k – giá trị ướ c tính củ a cá c biến trạ ng thá i ở thờ i
điểm k sau khi thự c hiện phép đo ⃗z k .
 Priori state, ^x k∨k−1 – giá trị củ a cá c biến trạ ng thá i ở thờ i điểm k đượ c
dự đoá n từ previous state ^x k−1∨k−1.
 Dynamic Model (có khi đượ c gọ i là  State Transition Model) – mô hình
biểu diễn mố i quan hệ giữ a cá c trạ ng thá i củ a hệ thố ng tạ i thờ i
điểm k và  k-1, đượ c dù ng để dự đoá n Priori state;
 Measurement Noise, R – nhiễu trong quá trình đo đạ c (sai số củ a cả m
biến, sai số củ a dụ ng cụ đo…);
 Process Noise, Qk – nhiễu trong quá trình hoạ t độ ng củ a hệ thố ng (tá c
độ ng củ a mô i trườ ng).

Thuật toán Kalman rời rạc:

Phương trình dự đoán trạng thái ( cập nhật thời gian):

^x k∨k−1=F x^ k−1∨k−1 +B u⃗ k (2.7)

Pk∨k−1=F P k−1∨k−1 FT +Qk (2.8)

16
 Phương trình trên có đượ c khi chú ng ta thự c hiện trong quá trình dự
đoá n trạ ng thá i. Để dự đoá n priori state ^x k∨k−1, chú ng ta cầ n có dynamic
model (chính là ma trậ n F) dự a và o cá c phương trình vậ t lý liên hệ cá c trạ ng
thá i theo thờ i gian lấ y mẫ u.

Sau khi cậ p nhậ t cá c giá trị trạ ng thá i mớ i chú ng ta cầ n cậ p nhậ t là giá
trị ma trậ n hiệp phương sai Pk∨k−1

Phương trình hiệu chỉnh trạng thái ( cập nhật giá trị):
T T −1
K k =P k∨k−1 H ( H P k∨k−1 H + R) (2.9)

^x k∨k = x^ k∨k−1 + K k ( ⃗z k −H ^x k∨k−1) (2.10)

Pk∨k =(I −K k H ) Pk∨k−1 (2.11)

Vớ i H là observation model, là ma trậ n đượ c ướ c tính theo giá trị trạ ng

thá i đo đượ c từ cả m biến. Chú ng ta sẽ tính đượ c độ lợ i Kalman K k theo cô ng
thứ c (2.9). Sau đó chú ng ta tính phép đo thự c z⃗ k . Dự a và o cô ng thứ c (2.10) ta
tính đượ c trạ ng thá i ướ c lượ ng posteriori ^x k∨k . Cuố i cù ng, ta tính đượ c giá trị
sai số ướ c lượ ng tương quan posteriori Pk∨k theo cô ng thứ c (2.11). Cá c giá
trị trên sẽ đượ c cậ p nhậ t lạ i sau mỗ i chu trình cậ p nhậ t thờ i gian và giá trị
(trạ ng thá i đệ quy tự nhiên).

17
 Hình 2.15: Các quá trình trong bộ lọc Kalman
2.5. Phần xử lý ảnh

Không gian màu HSV:

Hình 2.16:Không gian màu HSV

18
HSV là khô ng gian mà u đượ c xá c định bở i ba số liệu:

H (Hue): Vù ng mà u.

S (Saturation): Độ bã o hò a mà u

V (Value): Độ sá ng - tỷ lệ vớ i độ sáng củ a pixel sá ng nhấ t trong mà u RGB.

Khô ng giố ng như RGB, HSV tá ch biệt độ  sá ng, hoặ c cườ ng độ hình ả nh, từ  sắ c
độ  hoặ c thô ng tin mà u sắ c. Điều nà y rấ t hữ u ích trong nhiều ứ ng dụ ng. Ví dụ ,
ở đề tà i nà y thì mà u sắ c củ a ả nh sẽ bị thay đổ i nhiều về độ sá ng, độ bã o hò a và
vù ng mà u ở cá c mô i trườ ng khá c nhau. Chú ng ta chuyển ả nh sang khô ng gian
mà u HSV để có thể dễ dà ng lấ y ngưỡ ng trên và dướ i củ a để phù hợ p vớ i sự
thay đổ i đó .

Contour:

Contour là tậ p cá c điểm liên tụ c tạ o thà nh mộ t đườ ng cong (curve)

(boundary), và khô ng có khoả ng hở trong đườ ng cong đó . Đặ c điểm chung
trong mộ t contour là cá c cá c điểm có cù ng hoặ c gầ n xấ u xỉ mộ t giá trị mà u,
hoặ c cù ng mậ t độ . Contour là mộ t cô ng cụ hữ u ích đượ c dù ng để phâ n tích
hình dạ ng đố i tượ ng, phá t hiện đố i tượ ng và nhậ n dạ ng đố i tượ ng.

Để tìm contour chính xá c, chú ng ta cầ n phả i nhị phâ n hó a bứ c ả nh. Cá c kỹ

thuậ t nhị phâ n hó a như đặ t ngưỡ ng, hoặ c candy edge detection.

19
Chương 3. THIẾT KẾ VÀ THỰC HIỆN

3.1. Thiết kế phần cứng

3.1.1. Tổng quan về mô hình

Hình 3.17: Mô hình thực tế xe hai bánh tự cân bằng.

Kích thướ c mô hình là : chiều ngang dướ i bá nh xe là 240mm, chiều
ngang phầ n thâ n xe là 160mm, bề dà y củ a thâ n xe là 90mm, chiều cao củ a xe
là 175mm.

Khung sườ n có chấ t liệu từ nhự a mica và trụ vít bằ ng đồ ng.

Có mạ ch in 1 lớ p để nố i cá ch châ n củ a cá c module, thiết bị cả m biến lạ i

vớ i nhau.

Hai độ ng cơ gắ n vớ i hai bá nh xe có đườ ng kính 62mm bằ ng trụ c lụ c

giá c 12mm.

20
 3.1.2. Các thành phần chính của mô hình

3.1.2.1. Nguồn cho động cơ

 Dù ng Pin Lipo 12v 2200mAh

Hình 3.18: Pin Lipo 2200mAh

3.1.2.2. Nguồn cho máy tính nhúng Raspberry và STM32F407

 Dù ng sạ c dự phò ng Xmobile 5V-3A

Hình 3.19: Sạc dự phòng Xmobile 5V-3A

3.1.2.3. Kit STM32F407VGT6 Discovery

Hình 3.20: Board STM32F407VGT6

21
 Thông số kỹ thuật:

 Vi điều khiển 32-bit ARM Cortex®-M4 STM32F407VGT6 vớ i lõ i FPU hỗ

trợ xử lý tính toá n dấ u phẩ y độ ng, 1-MB bộ nhớ Flash, 192 Kbyte RAM.
 On-board ST-LINK/V2 trên STM32F4-DISCOVERY giú p nạ p chương
trình, gỡ lỗ i.
 Nguồ n điện cung cấ p cho bả ng mạ ch: thô ng qua cổ ng USB hoặ c từ mộ t
nguồ n cung cấ p điện á p 5V bên ngoà i.
 Từ bả ng mạ ch, có thể cấ p nguồ n 3.3 V và 5 V cho cá c ứ ng dụ ng.

3.1.2.4. Máy tính nhúng Raspberry Pi 3B+

Hình 3.21: Raspberry Pi 3B+

Thông số kĩ thuật:
 Vi xử lý: Broadcom BCM2837B0Quad-core A53 (ARMv8). 64-bit SoC
@1.4GHz
 RAM: 1GB LPDDR2 SDRAM
 Kết nố i: 2.4GHz and 5GHz (IEEE 802.11 b/g/n/ac wireless
LAN)Bluetooth 4.2 , Gigabit Ethernet over USB 2.0 (Tố i đa 300Mbps)
 Nguồ n: 5V/2.5A DC micro USB5V DC
 Lưu trữ : MicroSD
 Mở rộ ng: 40-pin GPIO

22
 3.1.2.5. Raspberry Camera V2.1

Hình 3.22: Raspberry Camera V2.1

Thông số kĩ thuật:

 Camera Raspberry Pi V2 8MP dù ng cho má y tính nhú ng Raspberry Pi.

 Cả m biến IMX219 từ Sony.
 Số điểm ả nh: 8MP.
 CCD size : 1/4inch 
 Focal Length : 3.04mm 
 Angle of View (diagonal) : 62.2 degree
 Camera Resolution: 3280 x 2464 pixel stills
 Video Resolution: HD 1080p30, 720p60 and 640x480p90 video
 Dimensions: 25mm x 23mm x 9mm
 Connector: ribbon connector
 Interface: CSI

3.1.2.6. Động cơ DC servo JGB37-520

Hình 3.23: Động cơ JGB37-520

23
 Thông số động cơ dc servo giảm tốc JGB37:

 Điện á p định mứ c: 12VDC

 Dò ng tố i đa: 3A
 Tố c độ độ ng cơ trướ c hộ p số giả m tố c: 10.000RPM
 Tỉ số truyền: 1:56
 Tố c độ sau hộ p số giả m tố c (trụ c chính): 10.000 / 56 = 178RPM
 Đườ ng kính trụ c: 6mm
 Momen xoắ n định mứ c: 6.5 Kg.cm
 Momen xoắ n cự c đạ i: 9.0 Kg.cm
 Trọ ng lượ ng: 155g

Thông số Encoder:

 Điện á p cấ p: 3.3VDC
 Encoder: Đĩa từ 11 xung, 2 kênh xung AB.
 Số xung Encoder sau giả m tố c: 11×56 = 616 xung / 1 vò ng.
Sơ đồ chân Encoder:
 Port 1 - M1: châ n cấ p nguồ n cho độ ng cơ, tố i đa 12VDC.
 Port 2 - GND: Châ n Mass 0v củ a Encoder.
 Port 3 - C1: Kênh A củ a Encoder, xuấ t xung mứ c tín hiệu 3.3VDC
 Port 4 - C2: Kênh B củ a Encoder, xuấ t xung mứ c tín hiệu 3.3VDC
 Port 5 - VCC: Nguồ n dương 3.3VDC cấ p cho Encoder hoạ t độ ng.
 Port 6 - M2: châ n cấ p nguồ n cho độ ng cơ, tố i đa 12VDC.

24
 3.1.2.7. Mạch điều khiển động cơ TB6612FNG

Hình 3.24: Module điều khiển động cơ TB6612FNG

Thông số kỹ thuật:

 Điện á p cung cấ p: 2.7 ~ 5.5V DC.

 Driver hỗ trợ 2 cầ u H có thể dù ng cho hai độ ng cơ DC hoặ c mộ t độ ng cơ
bướ c.
 Điện á p cung cấ p cho motor: giớ i hạ n ở mứ c 15V DC.
 Tầ n số hoạ t độ ng lên đến 100 KHz.
 Dò ng điện ngõ ra max: 3.2A cho mỗ i cầ u.
 Dò ng ngõ ra liên tụ c: 1.2A cho mỗ i cầ u (có thể mắ c song song để lên
đến 2.4A).
 4 chế độ điều khiển: quay thuậ n, quay ngượ c, hã m, stop.
 Bả o vệ quá nhiệt và quá á p. 
 Bả o vệ chố ng ngượ c nguồ n cấ p cho motor.

Sơ đồ chân:

Hình 3.25: Sơ đồ chân Module TB6612FNG

25
 3.1.2.8. Cảm biến MPU6050

Hình 3.26: MPU6050

MPU-6050 tích hợ p 6 trụ c cả m biến bao gồ m:

 Con quay hồ i chuyển 3 trụ c (3-axis MEMS gyroscope)

 Cả m biến gia tố c 3 chiều (3-axis MEMS accelerometer)

Cá c cả m biến bên trong MPU-6050 sử dụ ng bộ chuyển đổ i tương tự - số

(Anolog to Digital Converter - ADC) 16-bit cho ra kết quả chi tiết về gó c quay,
tọ a độ ... Vớ i 16-bit sẽ có 2^16 = 65536 giá trị cho 1 cả m biến.

MPU-6050 có khả nă ng đo ở phạ m vi:

 Con quay hồ i chuyển: ± 250 500 1000 2000 dps

 Gia tố c: ± 2 ± 4 ± 8 ± 16g
Các thông số khác:
 Điện á p sử dụ ng: 3-5VDC
 Điện á p giao tiếp: 3-5VDC
 Chuẩ n giao tiếp: I2C

26
 3.1.2.9. Module Bluetooth HC-05

Hình 3.27: Module Bluetooth HC-05

Module thu phá t Bluetooth HC-05 dù ng để kết nố i Serial giữ a 2 thiết bị
bằ ng só ng bluetooth. HC-05 là module có thể hoạ t độ ng ở 2 chế độ : MASTER
hoặ c SLAVE.

Các thông số kỹ thuật:

 Điện áp hoạt động: 3.2 - 6VDC.

 Mức điện áp giao tiếp: TTL tương thích 3.3VDC hoặc 5VDC.
 Baudrate UART có thể chọn được: 1200, 2400, 4800, 9600, 19200, 38400,
57600, 115200.
 Frequency: 2.4 GHz ISM band
 Truyền khoảng cách tối đa là 10 m

27
3.2. Thiết kế giải thuật

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

Hình 3.28: Sơ đồ khối chức năng của toàn bộ hệ thống

28
 3.2.2. Lưu đồ giải thuật điều khiển của hệ thống

Hình 3.29: Lưu đồ giải thuật toàn bộ hệ thống

Mô tả chi tiết giải thuật:

Khi xe hoạ t độ ng thì STM32 luô n lấ y về giá trị gó c nghiêng từ cả m biến

MPU6050 thô ng qua giao thứ c I2C để xử lý tín hiệu vớ i bộ lọ c Kalman.

29
 Khi camera khô ng phá t hiện vậ t thì mặ c định khoả ng cá ch và gó c xoay
củ a vậ t vớ i xe là 0. Khi đó chỉ có bộ PID gó c nghiêng hoạ t độ ng giú p xe câ n
bằ ng tạ i chỗ . Để xe câ n bằ ng tạ i chỗ thì giá trị gó c nghiêng đặ t là bằ ng 0, bộ
PID gó c nghiêng sẽ cho ra tín hiệu điều khiển là giá trị tầ n số xung cấ p cho
driver để điều khiển hai độ ng cơ.

Khi camera phá t hiện đượ c vậ t thì thô ng qua thuậ t toá n xử lý ả nh sẽ
cho ra hai thô ng số gó c xoay và khoả ng cá ch đượ c truyền UART từ Raspberry
xuố ng STM32F4. Hai giá trị này sẽ là ngõ và o (giá trị đặ t) cho bộ điều khiển
PID gó c xoay và PID khoả ng cá ch:

 Bộ điều khiển PID gó c xoay sẽ xuấ t ra tín hiệu tầ n số xung cho driver để
cho xe quay trá i hoặ c quay phả i sao cho ả nh củ a vậ t nằ m ở trọ ng tâ m củ a
khung hình camera.
 Bộ điều khiển PID khoả ng cá ch sẽ có đầ u và o là khoả ng cá ch đặ t và ngõ
ra sẽ xuấ t ra tín hiệu điều khiển là gó c đặ t. Gó c đặ t này là ngõ và o củ a bộ điều
khiển PID gó c nghiêng. Vớ i mỗ i khoả ng cá ch khá c nhau thì PID khoả ng cá ch
sẽ xuấ t ra giá trị gó c đặ t tương ứ ng cho đến đến khi nà o khoả ng cá ch từ vậ t
đến xe bằ ng đú ng giá trị khoả ng cá ch đặ t thì PID khoả ng cá ch sẽ xuấ t ra giá trị
gó c đặ t là 0. Lú c đó xe đứ ng yên và câ n bằ ng tạ i chỗ vớ i khoả ng cá ch đặ t
trướ c đó .

30
3.2.3. Lưu đồ giải thuật xử lý ảnh

Hình 3.30: Thuật toán xử lý ảnh của đề tài

Mô tả chi tiêt giải thuật:

31
 Sau khi bậ t camera, chú ng ta đọ c frame ả nh về liên tụ c. Sau đó , chú ng
ta thay đổ i kích thướ c ả nh xuố ng vớ i độ rộ ng width = 600 pixel để tă ng khố i
lượ ng tính toá n và tă ng FPS.

Sau khi thay đổ i kích thướ c ả nh thì chú ng ta cầ n là m mờ ả nh để giả m

nhiễu noise và là m trơn ả nh.

Tiếp theo, chú ng ta chuyển qua khô ng gian mà u HSV để có thể lấ y

ngưỡ ng mà u sắ c cầ n xá c định củ a vậ t. Vớ i mỗ i mà u sắ c thì chú ng ta cầ n lấ y
ngưỡ ng trên và ngưỡ ng dướ i ứ ng vớ i dà i mà u đó .

Cách lấy ngưỡng màu sắc:

Hình 3.31: Ngưỡng màu HSV với V=255

Dự a và o hình 3.14, để lấ y ngưỡ ng mà u cầ n xá c định thì:

- Chọ n mà u sắ c cầ n lấ y ngưỡ ng (ví dụ mà u xanh lá câ y)

- Chọ n H:0-180 và S: 0-255. Mà u xanh lá câ y thì H: 45-70, V: 140-255

- Chọ n V từ 25-255.

Sau khi xá c định xong mà u sắ c bằ ng cá ch lấ y ngưỡ ng thì chú ng ta cầ n

vẽ đườ ng bao cho vậ t. Nếu có ít nhấ t đườ ng bao cho vậ t thì ta xá c định đượ c
tọ a độ (x, y) và độ rộ ng W củ a đườ ng bao lớ n nhấ t, cá c đườ ng bao nhỏ hơn sẽ

32
bị là m ẩ n đi. Nếu khô ng có đườ ng bao thì trở lạ i bướ c đầ u, tiếp tụ c đọ c frame
ả nh.

Ta dự a và o tọ a độ (x, y) và độ rộ ng W củ a đườ ng bao để tính khoả ng

cá ch đến vậ t và gó c xoay củ a robot so vớ i vậ t. Cuố i cù ng là ta truyền UART giá
trị khoả ng cá ch và gó c xoay này đến STM32F407 để điều khiển.

3.3. Mô phỏng Matlab Simulink

Hình 3.32: Mô hình điều khiển xe hai bánh tự cân bằng trên Simulink.
Tiếp nố i phầ n mô hình hó a xe hai bá nh câ n bằ ng trên matlab Simulink ở phầ n
2.2.2, thì ta sẽ thiết kế cá c bộ điều khiển để điều khiển xe như hình 3.15. Bộ
điều khiển bao gồ m ba bộ PID là DISTANCE PID, ANGLE PID và ROTATION
PID:

Ngõ và o củ a DISTANE PID là gó c theta, ngõ ra sẽ là gó c đặ t psi_ref cho bộ

ANGLE PID. Ngõ ra củ a ANGLE PID đặ t sẽ là điện á p Vpsi.

Ngõ và o củ a ROTATION PID là gó c phi, ngõ ra sẽ là điện á p Vphi.

Như đã nó i ở phầ n 2.2.2 thì Vpsi và Vphi là ngõ và o củ a bộ decoupling, ngõ ra

là ra hai tín hiệu điều khiển hai độ ng cơ vr và vl để điều khiển xe câ n bằ ng.

33
 Hình 3.33: Cấu trúc chung của ba bộ điều khiển PID.
Dướ i đâ y là thô ng số củ a mô hình củ a bộ TWBR1 trên Simulink. Cá c thô ng số
đượ c chọ n và tính toá n mộ t cá ch dự trù . Vì mô hình thự c đượ c là m thủ cô ng
bằ ng tay nên cá c khô ng thể tính toá n chính xá c cá c thô ng số so vớ i mô hình
thự c.

Hình 3.34: Thông số của mô hình xe hai bánh cân bằng trên Simulink Matlab

34
3.4. Cấu hình các chân và chế độ hoạt động trên STM32CubeMx

Hình 3.35: Pinout view on STM32CubeMX.

- Chip STM32F407VGTx sử dụ ng thạ ch xung clock Thạ ch Anh bên ngoà i

(8MHz), sau khi qua cá c bộ chuyển đổ i thì xung clock sử dụ ng cho chip có tầ n
số 168MHz.

- Cấ u hình tổ ng thể cá c chứ c nă ng như hình 3.20:

35
 Hình 3.36: System view on STM32CubeMX.

 Cấ u hình DMA:

Sử dụ ng DMA1 cho UART2 Tx,Rx và UART3_Rx như hình 3.21.

Hình 3.37: Cấu hình DMA.

 Cấ u hình cho cá c châ n GPIO Output:

Cá c châ n GPIO Output gồ m PE8, PE10 cho Motor bên trá i và PE12, PE14 cho
Motor bên phả i.

Hình 3.38: Cấu hình chân GPIO Output.

 Cấ u hình Timer2 và Timer5:

Timer2 dù ng để nhậ n xung ENC củ a motor bên trá i.

Timer5 dù ng để nhậ n xung ENC củ a motor bên phả i.

36
 Hình 3.39: Cấu hình chân Timer2.

Hình 3.40: Cấu hình chân Timer5.

 Cấ u hình Timer3:

Timer3 dù ng để xuấ t xung PWM xuố ng Module điều khiển độ ng cơ

TB6612FNG.

Hình 3.41: Cấu hình chân Timer3.

Tầ n số sử dụ ng cho độ ng cơ là 20kHz vớ i độ rộ ng xung là 280, prescaler là 15.

Hình 3.42: Cấu hình counter cho Timer3.

 Cấ u hình Timer4:

Timer4 để tạ o ngắ t vớ i thờ i gian ngắ t là 0.01s= 10ms, ta sử dụ ng global

interupt.

Hình 3.43: Cấu hình chế độ ngắt cho Timer4.

Ta thiết lậ p Counter cho Timer4 như hình 3.28:

37
 Hình 3.44: Cấu hình counter cho Timer4.

 Cấ u hình I2C:

Ta sử dụ ng I2C3 vớ i cá c châ n PA8, PC9 tương ứ ng vớ i chế độ SCL và SDA để

truyền nhậ n tín hiệu giữ a MPU6050 và STM32 như hình 3.29.

Hình 3.45: Cấu hình chân I2C3.

 Cấ u hình UART2:

UART2 là giao thứ c truyền nhậ n giữ a STM32 và module bluetooth HC-05 để
truyền nhậ n dữ liệu lên má y tính.

Tx ứ ng vớ i châ n PA2 để truyền dữ liệu từ STM32 sang HC-05 để dữ liệu lên

má y tính.

Rx ứ ng vớ i châ n PA2 để nhậ n dữ liệu từ má y tính truyền đến HC-05.

Hình 3.46: Cấu hình chân UART2.

Tố c độ Baud củ a HC-05 mặ c định là 9600 Bit/s.

Hình 3.47: Cấu hình chế độ UART2.

38
 Cấ u hình UART3:

UART3 để nhậ n dữ liệu khoả ng cá ch và gó c xoay từ Raspberry Pi về STM32.

Hình 3.48: Cấu hình chân UART3.

Tố c độ Baud đượ c chọ n là 115200 Bits/s.

Hình 3.49: Cấu hình chế độ UART3.

3.5. Cấu hình thanh ghi cảm biến MPU6050

Dự a và o Datasheet củ a cả m biến MPU6050, ta có thể chọ n cá c chế độ phù vớ i

yêu cầ u củ a mô hình thự c và tiến hà nh cấ u hình cá c thanh ghi như sau:

WHO_AM_I Register: dù ng để xá c định danh tính củ a thiết bị. Cả m biến

MPU6050 có giá trị mặ c định là 0x68.

Hình 3.50: WHO_AM_I Register.

Power Management Register: dù ng để cấ u hình xung nguồ n clock. Ta dù ng
nguồ n clock thạ ch anh nộ i là 8MHz nên có giá trị thanh ghi là 0x00.

Hình 3.51: Power Management Register.

39
 Hình 3.52: Các chế độ chọn xung nguồn clock.
Sample Rate Divider Register: Chọ n tố c độ lấ y mẫ u là 1kHz.

8 kHz
Ta có MPU = 1+ SMPLRT , suy ra SMPLRT_DIV= 0x07.
¿

Hình 3.53: Sample Rate Divider Register.

Accelerometer Configuration Register: Chọ n tầ m đo củ a Accelerometer là

+/-2g, suy ra chọ n giá trị thanh ghi là 0x00.

Hình 3.54: Accelerometer Configuration Register

Hình 3.55: Các tầm đo của Accelerometer.

Gyroscope Configuration Register: Chọ n tầ m đo củ a Gyroscope là +/- 250 0/s,
suy ra chọ n giá trị thanh ghi là 0x00.

40
 Hình 3.56: Gyroscope Configuration Register.

Hình 3.57: Các tầm đo của Gyroscope.

Hình 3.58: Thanh ghi 59 đến 64 – Accelerometer Measurements.

Hình 3.59: Tầm đo của thanh ghi 59 đến 64 – Accelerometer Measurements.

Hình 3.60: Thanh ghi 67 đến 72 – Gyroscope Measurements.

41
 Hình 3.61: Tầm đo của thanh ghi 67 đến 72 – Gyroscope Measurements.

3.6. Xử lý tín hiệu của cảm biến MPU6050 qua bộ lọc Kalman

Dự a và o cơ sở lý thuyết củ a thuậ t toá n bộ lọ c Kalman rờ i rạ c ở mụ c 2.4.2,

chú ng ta thiết lậ p đượ c phương trình trạ ng thá i củ a hệ thố ng tạ i thờ i điểm k
là :

x k =F x k−1 + Bu k (3.1)

[ ]
Ө
Ma trậ n trạ ng thá i x k là : x k = Ө̇
b k
(3.2)

Vớ i Ө là gó c nghiêng và Ө̇b là độ lệch bias dự a trên cá c phép đo từ giá tố c kế

và con quay hồ i chuyển củ a cả m biến MPU6050. Độ lệch bias là số lượ ng con
quay hồ i chuyển đã trô i sau mộ t thờ i gian. Để tìm ra độ lệch thì chú ng ta dự a
và o tỉ lệ thự c trừ đi độ lệch củ a phép đo con quay hồ i chuyển.

Ta có : Өk =Ө k−1 −Ө̇b(k−1) ∆ t (3.3)

Và Ө̇b=Ө̇ b(k−1) (3.4)

Suy ra x k = [0
1 −∆ t
]
x k−1=F x k−1 .
1
(3.5)

Do đó , F = [ 10 −∆ t
1
.]
Ta có uk =Ө̇k , vì tín hiệu điều khiển uk cũ ng chính là phép đo con quay hồ i
chuyển vớ i đơn vị (°/giâ y).

Phương trình trạ ng thá i x k đượ c viết lạ i là :

42
x k =F x k−1 + B Ө̇ k (3.6)

Vì ta khô ng thể đo trự c tiếp đượ c Ө , chỉ đo đượ c Ө̇ trong thờ i gian lấ y mẫ u

∆ t nên ma trậ n B=
∆t
0 . [ ]
Tìm ma trận Qk của nhiễu hệ thống:

Qk là ma trậ n hiệp phương sai ướ c lượ ng trạ ng thá i củ a gia tố c kế và giá trị độ
lệch bias.

Qk =
[ QӨ 0
0 QӨ̇
∆t
b
] (3.7)

Tìm phép đo zk:

z k =H x k +v k (3.8)

Vớ i v k là nhiễu đo lườ ng (chính là R), ma trậ n H là ma trậ n đượ c ướ c tính theo

giá trị trạ ng thá i đo đượ c từ cả m biến. Ở đâ y, chú ng ta chỉ đo đượ c Ө̇ từ gia
tố c kế mà khô ng đo đượ c trự c tiếp Ө nên H = [ 1 0 ] .

Vậ y là chú ng ra đã hoà n thà nh bướ c khở i tạ o, bâ y giờ chú ng ta sẽ triển khai

cá c ma trậ n củ a bộ lọ c Kalman:

 Giai đoạn dự đoán:

^x k∨k−1=F x^ k−1∨k−1 +B Ө̇k

Ө
 Ө̇ =
b
[ ][ Ө+ ∆ t ( Ө̇−Ө̇b )
Ө̇ b ]
(3.9)
Pk∨k−1=F P k−1∨k−1 FT +Qk

 P P =
00

10
01

11
[ P P
][ P10−∆ t P 11 ]
P 00+ ∆ t( ∆ t P11 −P01−P10+ QӨ ) P01−∆ t P11
P11 +QӨ̇ ∆ t
b
(3.10)

43
  Giai đoạn khởi tạo hiệu chỉnh:
y k =z k −H x^ k−1∨k−1=z k −Өk∨k−1 (3.11)

Sk =H Pk ∨k−1 H T + R=P00k∨k−1+ R (3.12)

 Giai đoạn hiệu chỉnh:
T −1
K k =P k∨k−1 H S k

[ ]
P00

[ ]
 K0 P10 k∨k−1
=
K1 k
Sk

(3.13)

^x k∨k = x^ k∨k−1 + K k y k

[ ] [ ] [ ]
b k
Ө
 Ө̇ = Ө̇
b
Ө
k ∨k−1
+
K0 y
K1 y k
(3.14)

Pk∨k =(I −K k H ) Pk∨k−1

 [ P00 P01 P
= 00
][
P01
P10 P11 k P 10 P11 ] k∨k−1
−
[ K 0 P00 K 0 P01
K 1 P00 K 1 P01 ]
k ∨k−1
(3.15)

Sau khi ta triển khai xong cá ch phương trình củ a bộ lọ c Kalman thì chú ng ta
sẽ dự a và o đó và thự c hiện tạ o hà m bộ lọ c Kalman trên KeilC.

44
3.7. Tính toán tiêu cự và khoảng cách trên Raspberry Pi

Hình 3.62: Một số công thức tính qua ống kính camera
Dự a và o cá c cô ng thứ c cơ bả n ở trên, muố n đo đượ c khoả ng cá ch bấ t kì củ a
mộ t vậ t đến camera thì ta cầ n tính tiêu cự F củ a camera bằ ng cá ch như sau:

Đo chiều rộ ng W củ a vậ t bấ t kì, đặ t vậ t trướ c camera vậ t đó và đo

khoả ng cá ch D từ vậ t đến camera. Tiếp đến là tính độ rộ ng P củ a vậ t trên mà n
hình camera vớ i đơn vị Pixel. Ta tính đượ c tiêu cự F qua cô ng thứ c:

F=(P x D)/W (3.16)

Khi có tiêu cự F ta tính đượ c khoả ng cá ch D’ củ a vậ t thể đến camera

thô ng qua cô ng thứ c:

D ’=(W x F )/P (3.17)

Vì vớ i mỗ i camera có mỗ i tiêu cự riêng nên khoả ng cá ch tính đượ c củ a

mộ t vậ t đến camera thô ng qua tiêu cự củ a camera là có giớ i hạ n. Vì vậ y,
khô ng nên để ả nh vượ t quá giớ i hạ n đó .

Tính toán tiêu cự của camera:

Ta tính toá n đượ c tiêu cự củ a vậ t theo cá c khoả ng cá ch thự c củ a vậ t đến

camera từ 10cm đến 100 cm như bả ng sau:

45
 K/ 10 20 30 40 50 60 70 80 90 100
c(cm)

Fcam 345.2 389 408.9 418.6 422.8 429.6 436.2 442.8 445.2 447.8

Bảng 3.3: Tiêu cự của camera trong 1m

Vì giớ i hạ n củ a độ phâ n giả i và sự rung củ a xe nên ta chỉ đo trong 1m để xe ổ n
định nhấ t có thể.

Dự a và o bả ng trên ta đo đượ c giá trị tiêu cự trung bình trong 1m là F = 418.6.

Khi đã có F ta tính đượ c khoả ng cá ch qua camera D_cam. Bả ng dướ i đâ y là

khoả ng cá ch thự c D_real và khoả ng cá ch đo đượ c qua camera:

D_real(cm) 10 20 30 40 50 60 70 80 90 100

D_cam(cm) 12.1 21. 30.9 40 49.5 59.3 68.5 77.4 86.8 96.6
7

Bảng 3.4: Khoảng cách đo qua camera so với khoảng cách thực.

46
3.8. Thiết kế giao diện C# giám sát thông số

Hình 3.63: Giao diện C# theo dõi các thông số đáp ứng của mô hình
Giao diện C# dù ng để theo dõ i cá c thô ng số đá p ứ ng củ a mô hình là gó c
nghiêng Angle, gó c xoay Rotation và khoả ng cá ch Distance

Cá c thô ng số đượ c truyền UART từ STM32 đến má y tính thô ng qua module
Bluetooth HC-05.

Để xem đá p ứ ng cá c thô ng số theo thờ i gian thự c thì phả i chọ n cổ ng Com củ a
Module Bluetooth HC-05 và nhấn Connect. Nếu khô ng đú ng cổ ng Com sẽ
khô ng kết nố i đượ c.

Bấ m Start để chạ y, bấ m thêm lầ n nữ a sẽ Stop. Bấ m Exit để thoá t khỏ i theo

dõ i.

47
Chương 4. KẾT QUẢ THỰC HIỆN

4.1. Đáp ứng các thông số mô phỏng trên Matlab Simulink

4.1.1. Góc nghiêng Angle

Hình 4.64: Đáp ứng góc nghiêng Angle trên Matlab Simulink.
Nhậ n xét: Độ vọ t lố POT= 28.6%.

Thờ i gian quá độ = 6.8s (tiêu chuẩ n 5%)

Sai số xá c lậ p = 0.

4.1.2. Góc xoay Rotation

Hình 4.65: Đáp ứng góc xoay Rotation trên Matlab Simulink.

48
Nhậ n xét: Khô ng có độ vọ t lố

Thờ i gian quá độ = 7.5s (tiêu chuẩ n 5%)

Sai số xá c lậ p = 0.

4.1.3. Khoảng cách Distance

Hình 4.66: Đáp ứng khoảng cách Distance trên Matlab Simulink.
Nhậ n xét: Như đã nó i ở phầ n thiết kế 3.3 Mô phỏ ng trên Matlab Simulink. Do
thô ng số mô phỏ ng củ a mô hình trên Simulink chỉ là dự trù , khô ng thể tính
toá n chính xá c cá c thô ng số đó ứ ng vớ i mô hình thự c do mô hình đượ c thự c
hiện thủ cô ng. Nên việc điều chỉnh cá c thô ng số Kp,Ki, Kd rấ t khó có thể để
đá p ứ ng ngõ ra cho cả 3 bộ điều khiển Angle PID, Rotation PID và Distance
PID. Ở trườ ng hợ p nà y thì ngõ ra củ a bộ Distance PID khô ng thể đá p ứ ng.

49
4.2. Kết quả thực hiện xử lý ảnh trên Raspberry Pi

Hình 4.67: Kết quả thưc hiện xử lý ảnh trên Raspberry Pi

Kết quả xử lý ả nh trên Raspberry Pi như hình trên. Vớ i nú t Start, Stop
để cho phép bắ t đầ u hoặ c dừ ng phá t hiện vậ t. Thô ng số 21.9 và 396.0 lầ n lượ t
là khoả ng cá ch và gó c xoay hiện tạ i củ a vậ t so vớ i camera.

4.3. Đáp ứng các thông số của mô hình thực.

4.3.1. Kiểm chứng sự hiệu quả của bộ lọc Kalman và tìm giá trị
nhiễu đo lường R.

Để tìm nhiễu đo lườ ng R cho ngõ và o bộ lọ c Kalman thì ta xá c định

bằ ng cá ch thử cá c giá trị R khá c nhau và chọ n ra R để ngõ ra củ a bộ lọ c
Kalman là tố i ưu nhấ t, đồ ng thờ i kiểm chứ ng sự hiệu quả củ a bộ lọ c Kalman.

Sau đâ y là cá c trườ ng hợ p đo đượ c củ a cá c giá trị gó c nghiêng

Angle_raw đo đượ c từ cả m biến MPU6050 và giá trị gó c nghiêng sau khi đượ c
xử lý bằ ng bộ lọ c Kalman:

50
  Trườ ng hợ p 1: R=0.05

Hình 4.68: Đáp ứng góc nghiêng của hệ thống với nhiễu đo lường R=0.05

 Trườ ng hợ p 2: R=0.2

Hình 4.69: Đáp ứng góc nghiêng của hệ thống với nhiễu đo lường R=0.2

51
  Trườ ng hợ p 3: R=0.5

Hình 4.70: Đáp ứng góc nghiêng của hệ thống với nhiễu đo lường R=0.5

 Trườ ng hợ p 4: R= 0.8

Hình 4.71: Đáp ứng góc nghiêng của hệ thống với nhiễu đo lường R=0.8
Nhận xét: Vớ i cá c trườ ng hợ p trên thì ta có thể thấ y vớ i R=0.05 thì bộ lọ c
Kalman cho ra ngõ ra ít bị nhiễu nhấ t. Nó i chung thì ta thấ y bộ Kalman đã lọ c
cá c tín hiệu đo đượ c từ cả m biến MPU6050 rấ t hiệu quả , cá c nhiễu đo lườ ng
đã đượ c loạ i bỏ rấ t nhiều. Nhờ thế mà mô hình đượ c hoạ t độ ng ổ n định hơn.

52
 4.3.2. Góc nghiêng Angle

Đá p ứ ng ngõ ra củ a bộ Angle PID vớ i tín hiệu đặ t là Psi_ref=0o:

 Trườ ng hợ p 1: Khô ng có vậ t nặ ng đặ t lên xe.

Hình 4.72: Đáp ứng góc nghiêng của hệ thống khi không có vật nặng.

 Trườ ng hợ p 2: Có vậ t nặ ng 1.5kg đặ t lên xe.

Hình 4.73: Đáp ứng góc nghiêng của hệ thống khi đặt vật 1.5kg lên xe.
Nhận xét: Ở cả hai trườ ng hợ p có vậ t nặ ng và khô ng có vậ t nặ ng đặ t lên xe thì
robot vẫn câ ng bằ ng tố t vớ i độ giao độ ng nhỏ trong khoả ng [-0.50, 0.50].

53
 4.3.3. Góc xoay Rotation

Sau đâ y là đá p ứ ng ngõ ra củ a bộ Rotation PID, vớ i cá c trườ ng hợ p có

vậ t nặ ng và khô ng có vậ t nặ ng đặ t lên xe:

 Trườ ng hợ p 1: Giá trị gó c xoay ban đầ u là 250, khô ng có vậ t đặ t lên xe.

Hình 4.74: Đáp ứng góc xoay của hệ thống với góc xoay ban đầu là 250, không
có vật nặng đặt lên xe.
 Trườ ng hợ p 2: Giá trị gó c xoay ban đầ u là 250, vậ t nặ ng 1.5kg đặ t lên
xe.

54
Hình 4.75: Đáp ứng góc xoay của hệ thống với góc xoay ban đầu là = 250, đặt
vật 1,5kg lên xe.
 Trườ ng hợ p 3: Giá trị gó c xoay ban đầ u là -300, khô ng có vậ t nặ ng đặ t
lên xe.

Hình 4.76: Đáp ứng góc xoay của hệ thống với góc xoay ban đầu là -300, không
có vật nặng đặt lên xe.

 Trườ ng hợ p 4: Giá trị gó c xoay ban đầ u là -250, vậ t nặ ng 1.5kg đặ t lên

xe.

55
 Hình 4.77: Đáp ứng góc xoay của hệ thống với góc xoay ban đầu là - 250, đặt
vật 1,5kg lên xe
Nhận xét: Chú ng ta có thể thấ y ở tấ t cả cá c trườ ng hợ p thì ngõ ra gó c xoay
củ a hệ thố ng đá p ứ ng tố t vớ i sai số xá c lậ p nhỏ , độ vọ t lố nhỏ . Tuy nhiên, ở
trườ ng hợ p có vậ t nặ ng lên xe thì thờ i gian đá p ứ ng chậ m hơn so vớ i trườ ng
hợ p khô ng có vậ t nặ ng đặ t lên xe.

Nguyên nhâ n dẫ n đến điều nà y là do trọ ng lự c củ a vậ t nặ ng tá c độ ng lên xe

lớ n lớ n là m cho độ ng cơ khô ng đá p ứ ng nhanh đượ c như khi khô ng có vậ t
nặ ng.

4.3.4. Khoảng cách Distance

 Trườ ng hợ p 1: Vậ t xe ban đầ u cá ch vậ t 50 cm và xe sẽ bá m theo vậ t

đến khi cò n cá ch vậ t 20cm và duy trì vớ i khoả ng cá ch đó :

56
 Hình 4.78: Đáp ứng khoảng cách của hệ thống khi khoảng cách đặt
distance_ref =20 cm và khoảng cách ban đầu là 50 cm.

 Trườ ng hợ p 2: Vậ t xe ban đầ u cá ch vậ t 20cm và xe sẽ bá m theo vậ t đến

khi cò n cá ch vậ t 50cm và duy trì vớ i khoả ng cá ch đó :

57
 Hình 4.79: Đáp ứng khoảng cách của hệ thống khi khoảng cách đặt
distance_ref =50 cm và khoảng cách ban đầu là 20 cm.
 Trườ ng hợ p 3: Vậ t xe ban đầ u cá ch vậ t 79cm và xe sẽ bá m theo vậ t đến
khi cò n cá ch vậ t 50cm và duy trì vớ i khoả ng cá ch đó :

Hình 4.80: Đáp ứng khoảng cách của hệ thống khi khoảng cách đặt
distance_ref =50 cm và khoảng cách ban đầu là 79 cm.

 Trườ ng hợ p 4: Vậ t xe ban đầ u cá ch vậ t 35 cm và xe sẽ bá m theo vậ t

đến khi cò n cá ch vậ t 60 cm và duy trì vớ i khoả ng cá ch đó :

58
 Hình 4.81: Đáp ứng khoảng cách của hệ thống khi khoảng cách đặt
distance_ref =60 cm và khoảng cách ban đầu là 35 cm.

 Trườ ng hợ p 5: Vậ t xe ban đầ u cá ch vậ t 80cm và xe sẽ bá m theo vậ t đến

khi cò n cá ch vậ t 60cm và duy trì vớ i khoả ng cá ch đó :

Hình 4.82: Đáp ứng khoảng cách của hệ thống khi khoảng cách đặt
distance_ref =60 cm và khoảng cách ban đầu là 80 cm.
Nhận xét: Thờ i gian quá độ củ a khoả ng cá ch là khoả ng 5-6 giâ y và sai số xá c
lậ p tương đố i nhỏ trong tấ t cả cá c trườ ng hợ p khoả ng cá ch ban đầ u bé hơn

59
70cm. Nhưng vớ i khoả ng cá ch ban đầ u là 79 cm và 80 cm như ở trườ ng hợ p 3
và 5 thì độ vọ t lố tă ng rõ rệt, hệ thố ng lâ u xá c lậ p hơn.

60
Chương 5. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN

5.1. Kết quả đạt được

- Thiết kế xe hai bá nh câ n bằ ng bá m theo vậ t vớ i mà u sắ c xá c định vớ i mộ t

khoả ng cá ch cố định.

- Hiểu đượ c lý thuyết cơ bả n về điều khiển xe hai bá nh câ n bằ ng và á p dụ ng

thà nh cô ng bộ điều khiển PID lên mô hình.

- Á p dụ ng thà nh cô ng lý thuyết củ a bộ lọ c Kalman và o thự c tế để xử lý tín hiệu

cả m biến gó c nghiêng MPU6050.

- Mô phỏ ng đượ c xe hai bá nh tự câ n bằ ng trên Matlab Simulink.

- Hiểu rõ và thự c hiện đượ c cá ch phá t hiện vậ t bằ ng mà u sắ c đồ ng thờ i việc

thự c hiện tính khoả ng cá ch và gó c xoay qua camera trên Raspberry.

- Thiết kế giao diện C# để theo dõ i đá p ứ ng cá c thô ng số củ a cá c bộ điều

khiển.

5.2. Những hạn chế

Về đáp ứng cân bằng và bám theo vật:

- Mô hình điều khiển theo bộ điều khiển PID nên cá c chấ t lượ ng củ a đá p ứ ng
ngõ ra trong quá trình câ n bằ ng tạ i chỗ cũ ng như bá m vậ t thể sẽ bị thay đổ i.
Nguyên nhâ n là do bộ điều khiển PID chỉ phụ thuộ c và o ngõ và o và cá c hệ số
Kp ,Ki, Kd nên khi pin cấ p cho độ ng cơ yếu thì tố c độ độ ng cơ cũ ng giả m theo
là m cho mô hình đá p ứ ng chậ m hơn.

- Khi đặ t vậ t nặ ng 1.5kg lên xe, mô hình có thể câ n bằ ng và xoay trá i, xoay

phả i tạ i chỗ đượ c nhưng khô ng thể bá m theo vậ t vớ i khoả ng cá ch xa đượ c.
Nguyên nhân là do bá nh xe nhỏ và độ ng cơ khá yếu.

Về phần phát hiện vật và tính toán khoảng cách:

61
- Mô hình chỉ phá t hiện vậ t bằ ng mà u sắ c nên bị nhiễu trong mô i trườ ng á nh
sá ng khô ng tố t. Nguyên nhâ n là vì mô hình sử dụ ng má y tính nhú ng
Raspberry 3B+ có cấ u hình khá yếu, khô ng thể thự c hiện cá c thuậ t toá n phá t
hiện vậ t tố t hơn như deep learning, machine learning…

- Mô hình khô ng thể đo khoả ng cá ch đến vậ t mộ t cá ch chính xá c khi vậ t thể ở

quá xa. Nguyên nhâ n do camera có độ phâ n giả i thấ p và tiêu cự ngắ n.

5.3. Hướng phát triển

- Nếu có kinh phí tố t hơn sẽ tiến hà nh thay má y tính nhú ng và camera tố t hơn
để thự c hiện cá c thuậ t toá n phá t hiện vậ t tố i ưu hơn như deep learning,
machine learning…

- Thiết kế phầ n cơ khí mô hình tố t hơn như in 3D cho khung sườ n xe. Sử dụ ng
độ ng cơ tố t hơn và bá nh xe có kích thướ c phù hợ p để có thể câ n bằ ng ổ n định
và bá m theo vậ t vớ i khoả ng cá ch xa hơn kể cả khi đặ t vậ t nặ ng lên xe.

- Sử dụ ng cá c phương phá p điều khiển tố i ưu hơn như: LQR, Fuzzy, Nơ-ron,

điều khiển trượ t… để xe câ n bằ ng và bá m vậ t thể ổ n định hơn.

62
 TÀI LIỆU THAM KHẢO

[1] Nguyễn Thị Phương Hà , Huỳnh Thá i Hoà ng, Lý thuyết điều khiển tự động,
nhà xuấ t bả n đạ i họ c Quố c gia Thà nh phố Hồ Chí Minh, 2010.

[2] Nguyễn Thá i Vin, Robot hai bánh tự cân bằng, Luậ n vă n tố t nghiệp, Đạ i họ c
Bá ch khoa Tp. Hồ Chí Minh, 2016.

[3] Ntppro, “Lý thuyết cơ bả n bộ lọ c Kalman”, Internet, 2019:

https://codelungtung.wordpress.com/2019/05/29/bo-loc-kalman/

[4] OpenCV, “Drawing Functions in OpenCV”, Internet:

https://docs.opencv.org/4.5.2/dc/da5/tutorial_py_drawing_functions.ht
ml

[4] Trầ n Anh Tứ , Nghiên cứu, thiết kế, thử nghiệm xe hai bánh cân bằng, Luậ n
vă n thạ c sĩ, Trườ ng đạ i họ c Cô ng nghệ Tp. Hồ Chí Minh, 2014.

[5] Tkjelectronics, “A practical approach to Kalman filter and how to

implement it”, Internet, 2012:

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-
filter-and-how-to-implement-it/

63