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

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

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

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

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

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

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

 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.1: 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.2: Đ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.3: Tổng quan mô hình xe hai bánh cân bằng

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

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𝑚 + 𝑀)𝑅 2 + 2𝐽𝑤 + 2𝑛2 𝐽𝑚 ]Ӫ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝐿𝑅𝜓̇ 2 𝑠𝑖𝑛𝜓 =
𝛼(𝑣𝑙 + 𝑣𝑟 ) − 2(𝛽 + 𝑓𝑤 )Ө̇ + 2𝛽𝜓̇ (2.1)

(𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )Ӫ + (𝑀𝐿2 + 𝐽𝑤 + 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝑔𝐿𝑠𝑖𝑛𝜓 −

𝑀𝐿2 ɸ̇2 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = −𝛼 (𝑣𝑙 + 𝑣𝑟 ) + 2𝛽Ө̇ − 2𝛽𝜓̇ (2.2)

1 𝑊2
[ 𝑚𝑊 2 + 𝐽ɸ + (𝐽𝑤 + 𝑛2 𝐽𝑚 ) + 𝑀𝐿2 𝑠𝑖𝑛2 𝜓] ɸ̈2 + 2𝑀𝐿2 𝜓̇ɸ̇𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 =
2 2𝑅2
𝑊 𝑊2
𝛼(𝑣𝑙 − 𝑣𝑟 ) − (𝛽 + 𝑓𝑤 )ɸ̇ (2.3)
2𝑅 2𝑅2

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

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
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ơ 𝑣𝑙 , 𝑣𝑟 tác động lên các thông số đó dưới
dạng tổng 𝑣𝑙 + 𝑣𝑟 còn với góc xoay, giá trị điện áp hai động cơ 𝑣𝑙 , 𝑣𝑟 tác động
lên thông số này dưới dạng hiệu 𝑣𝑟 − 𝑣𝑙 . 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 𝑉𝜓 , 𝑉𝜙
1
𝑉𝜓 = 𝑣𝑙 + 𝑣𝑟 𝑣𝑙 = (𝑉𝜓 −𝑉𝜙 )
2
{ => { 1 (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.8: 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

13
thích hợp. Do tính thông dụng của nó nên nhiều hãng sản xuất 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.9: 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:

T z 1 KD z 1
G (z)  K P  K I .  . (2.6)
2 z 1 T z

Đ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 kết hợp

14
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 vong 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ó được
sử dụng chủ yếu với 2 mục đích chính là: Estimating the state of dynamic

15
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, 𝑥⃗ – vector chứa các biến trạng thái của hệ thống.
 Measurement values, 𝑧⃗𝑘 – vector chứa các giá trị trạng thái đo được tại
thời điểm k.
 Control input, 𝑢
⃗⃗𝑘 – vector chứa giá trị input của hệ thống tại thời điểm
k;
 Posteriori state, 𝑥̂𝑘|𝑘 – 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 𝑧⃗𝑘 .
 Priori state, 𝑥̂𝑘|𝑘−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 𝑥̂𝑘−1|𝑘−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)

𝑥̂𝑘|𝑘−1 = 𝐹𝑥̂𝑘−1|𝑘−1 + 𝐵𝑢
⃗⃗𝑘 (2.7)

𝑃𝑘|𝑘−1 = 𝐹𝑃𝑘−1|𝑘−1 𝐹 𝑇 + 𝑄𝑘 (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 𝑥̂𝑘|𝑘−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 𝑃𝑘|𝑘−1

Phương trình hiệu chỉnh trạng thái ( cập nhật giá trị):

𝐾𝑘 = 𝑃𝑘|𝑘−1 𝐻 𝑇 (𝐻𝑃𝑘|𝑘−1 𝐻 𝑇 + 𝑅)−1 (2.9)

𝑥̂𝑘|𝑘 = 𝑥̂𝑘|𝑘−1 + 𝐾𝑘 ( 𝑧⃗𝑘 − 𝐻𝑥̂𝑘|𝑘−1 ) (2.10)

𝑃𝑘|𝑘 = (𝐼 − 𝐾𝑘 𝐻)𝑃𝑘|𝑘−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 𝐾𝑘 theo công
thức (2.9). Sau đó chúng ta tính phép đo thực 𝑧⃗𝑘 . Dựa vào công thức (2.10) ta
tính được trạng thái ước lượng posteriori 𝑥̂𝑘|𝑘 . Cuối cùng, ta tính được giá trị
sai số ước lượng tương quan posteriori 𝑃𝑘|𝑘 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.10: 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.11: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.1: 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.2: 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.3: Sạc dự phòng Xmobile 5V-3A

3.1.2.3. Kit STM32F407VGT6 Discovery

Hình 3.4: 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.5: 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.6: 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.7: Độ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.8: 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.9: Sơ đồ chân Module TB6612FNG

25
 3.1.2.8. Cảm biến MPU6050

Hình 3.10: 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.11: 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.12: 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.13: 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.14: Thuật toán xử lý ảnh của đề tài

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

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.

31
 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.15: 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ẽ 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.

32
3.3. Mô phỏng Matlab Simulink

Hình 3.16: 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.

Hình 3.17: Cấu trúc chung của ba bộ điều khiển PID.

33
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.18: 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.19: 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.20: 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.21: 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.22: 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.23: Cấu hình chân Timer2.

Hình 3.24: 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.25: 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.26: 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.27: 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.28: 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.29: 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.30: Cấu hình chân UART2.

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

Hình 3.31: 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.32: Cấu hình chân UART3.

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

Hình 3.33: 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.34: 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.35: Power Management Register.

39
 Hình 3.36: Các chế độ chọn xung nguồn clock.
Sample Rate Divider Register: Chọn tốc độ lấy mẫu là 1kHz.
8 𝑘𝐻𝑧
Ta có MPU = , suy ra SMPLRT_DIV= 0x07.
1+SMPLRT_DIV

Hình 3.37: 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.38: Accelerometer Configuration Register

Hình 3.39: 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.40: Gyroscope Configuration Register.

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

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

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

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.

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

𝑥𝑘 = 𝐹𝑥𝑘−1 + 𝐵𝑢𝑘 (3.1)

Ө
Ma trận trạng thái 𝑥𝑘 là: 𝑥𝑘 = [ ̇ ] (3.2)
Ө𝑏 𝑘

Với Ө là góc nghiêng và Ө̇𝑏 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ó: Ө𝑘 = Ө𝑘−1 − Ө̇𝑏(𝑘−1) ∆𝑡 (3.3)

Và Ө̇𝑏 = Ө̇𝑏(𝑘−1) (3.4)

1 −∆𝑡
Suy ra 𝑥𝑘 = [ ] 𝑥𝑘−1 = 𝐹𝑥𝑘−1 . (3.5)
0 1
1 −∆𝑡
Do đó, F =[ ].
0 1

Ta có 𝑢𝑘 = Ө̇𝑘 , vì tín hiệu điều khiển 𝑢𝑘 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 𝑥𝑘 được viết lại là:

42
𝑥𝑘 = 𝐹𝑥𝑘−1 + 𝐵Ө̇𝑘 (3.6)

Vì ta không thể đo trực tiếp được Ө , chỉ đo được Ө̇ trong thời gian lấy mẫu ∆𝑡
∆𝑡
nên ma trận 𝐵 = [ ].
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.
𝑄Ө 0
𝑄𝑘 = [ 0 𝑄Ө̇𝑏 ] ∆𝑡 (3.7)

Tìm phép đo zk:

𝑧𝑘 = 𝐻𝑥𝑘 + 𝑣𝑘 (3.8)

Với 𝑣𝑘 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 𝐻 = [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:

𝑥̂𝑘|𝑘−1 = 𝐹𝑥̂𝑘−1|𝑘−1 + 𝐵Ө̇𝑘
Ө Ө + ∆𝑡(Ө̇ − Ө̇𝑏 )
 [ ̇ ]=[ ] (3.9)
Ө𝑏 Ө̇𝑏
𝑃𝑘|𝑘−1 = 𝐹𝑃𝑘−1|𝑘−1 𝐹 𝑇 + 𝑄𝑘
𝑃00 𝑃01 𝑃00 + ∆𝑡(∆𝑡𝑃11 − 𝑃01 − 𝑃10 + 𝑄Ө ) 𝑃01 − ∆𝑡𝑃11
 [ ]=[ ] (3.10)
𝑃10 𝑃11 𝑃10 − ∆𝑡𝑃11 𝑃11 + 𝑄Ө̇ ∆𝑡
𝑏

43
  Giai đoạn khởi tạo hiệu chỉnh:
𝑦𝑘 = 𝑧𝑘 − 𝐻𝑥̂𝑘−1|𝑘−1 = 𝑧𝑘 − Ө𝑘|𝑘−1 (3.11)

𝑆𝑘 = 𝐻𝑃𝑘|𝑘−1 𝐻𝑇 + 𝑅 = 𝑃00 𝑘|𝑘−1 + 𝑅 (3.12)

 Giai đoạn hiệu chỉnh:

𝐾𝑘 = 𝑃𝑘|𝑘−1 𝐻𝑇 𝑆𝑘 −1
𝑃00
[ ]
𝐾0 𝑃10 𝑘|𝑘−1
 [ ] = (3.13)
𝐾1 𝑘 𝑆𝑘

𝑥̂𝑘|𝑘 = 𝑥̂𝑘|𝑘−1 + 𝐾𝑘 𝑦𝑘
Ө Ө 𝐾𝑦
 [ ̇ ] =[ ̇ ] +[ 0 ] (3.14)
Ө𝑏 𝑘 Ө𝑏 𝑘|𝑘−1 𝐾1 𝑦 𝑘

𝑃𝑘|𝑘 = (𝐼 − 𝐾𝑘 𝐻)𝑃𝑘|𝑘−1
𝑃00 𝑃01 𝑃 𝑃01 𝐾 𝑃 𝐾0 𝑃01
 [ ] = [ 00 ] − [ 0 00 ] (3.15)
𝑃10 𝑃11 𝑘 𝑃10 𝑃11 𝑘|𝑘−1 𝐾1 𝑃00 𝐾1 𝑃01 𝑘|𝑘−1
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.46: 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/c(cm) 10 20 30 40 50 60 70 80 90 100

Fcam 345.2 389 408.9 418.6 422.8 429.6 436.2 442.8 445.2 447.8

Bảng 3.1: 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.7 30.9 40 49.5 59.3 68.5 77.4 86.8 96.6

Bảng 3.2: 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.47: 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.1: Đá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.2: Đá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.3: Đá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.4: 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.5: Đá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.6: Đá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.7: Đá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.8: Đá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.9: Đá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.10: Đá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.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.

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

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
  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.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.
 Trường hợp 4: Giá trị góc xoay ban đầu là -250, vật nặng 1.5kg đặt lên
xe.

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
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 đó:

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
  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 đó:

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.

 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.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
  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 đó:

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.

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

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

- 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

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

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

62