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

HỒ TRẦN TRỌNG NHÂN

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

ROBOT CÂN BẰNG BÁM THEO VẬT CÓ MÀU SẮC

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

HỒ TRẦN TRỌNG NHÂN – 1712426

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

ROBOT CÂN BẰNG BÁM THEO VẬT CÓ MÀU SẮC

XÁC ĐỊNH
A TWO-WHEEL BALANCED ROBOT TRACKING OBJECT BY COLOR

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

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

PGS.TS. HUỲNH THÁI HOÀNG

TP. HỒ CHÍ MINH, 2021

TRƯỜNG ĐẠI HỌC BÁCH KHOA TP. HỒ CHÍ CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
MINH
Độc lập - Tự do - Hạnh phúc
KHOA ĐIỆN – ĐIỆN TỬ
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:

ROBOT CÂN BẰNG BÁM THEO VẬT CÓ MÀU SẮC XÁC ĐỊNH/ A TWO-
WHEEL BALANCED ROBOT TRACKING OBJECT BY COLOR
Nhóm Sinh viên thực hiện: Cán bộ hướng dẫn:
Hồ Trần Trọng Nhân 1712426 PGS.TS. Huỳnh Thái Hoà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:

<nhận xét về kiến thức, phương pháp mà sinh viên đã tìm hiểu, nghiên cứu, nhận xét ưu
điểm và hạn chế>

3. Về tính ứng dụng:

<nhận xét về việc xây dựng ứng dụng demo, nhận xét ưu điểm và hạn chế>

4. Về thái độ làm việc của sinh viên:

<nhận xét về thái độ, ưu khuyết điểm của từng sinh viên tham gia>
Đánh giá chung: Luận văn đạt/không đạt yêu cầu của một luận văn tốt nghiệp kỹ sư, xếp
loại Giỏi/ Khá/ Trung bình
Điểm từng sinh viên:
Hồ Trần Trọng Nhân:………../10

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

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

4
TRƯỜNG ĐẠI HỌC BÁCH KHOA TP. HỒ CHÍ CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
MINH
Độc lập - Tự do - Hạnh phúc
KHOA ĐIỆN – ĐIỆN TỬ
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:

ROBOT CÂN BẰNG BÁM THEO VẬT CÓ MÀU SẮC XÁC ĐỊNH/ A TWO-
WHEEL BALANCED ROBOT TRACKING OBJECT BY COLOR
Nhóm Sinh viên thực hiện: Cán bộ phản biện:
Hồ Trần Trọng Nhân 1712426 <Họ tên>

Đá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:
<nhận xét về định dạng, cách thức viết báo cáo, phân bố nội dung, chương mục có hợp lý
không, …>

6. Về nội dung luận văn:

<nhận xét về kiến thức, phương pháp mà sinh viên đã tìm hiểu, nghiên cứu, nhận xét ưu
điểm và hạn chế>

7. Về tính ứng dụng:

<nhận xét về việc xây dựng ứng dụng demo, nhận xét ưu điểm và hạn chế>
8. Về thái độ làm việc của sinh viên:
<nhận xét về thái độ, ưu khuyết điểm của từng sinh viên tham gia>

5
Đánh giá chung: Luận văn đạt/không đạt yêu cầu của một luận văn tốt nghiệp kỹ sư, xếp
loại Giỏi/ Khá/ Trung bình
Điểm từng sinh viên:
Hồ Trần Trọng Nhân: ………../10

Người nhận xét

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

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

ĐỀ CƯƠNG CHI TIẾT

TÊN LUẬN VĂN: ROBOT CÂN BẰNG BÁM THEO VẬT CÓ MÀU SẮC XÁC
ĐỊNH

Cán bộ hướng dẫn: PGS.TS. Huỳnh Thái Hoàng

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

Sinh viên thực hiện:

Hồ Trần Trọng Nhân – 1712426

Nội dung đề tài: Robot cân bằng bám theo vật có màu sắc 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)

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

8
 MỤC LỤC
DANH MỤC TỪ VIẾT TẮT ..............................................................................................................6

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

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

ABSTRACT ............................................................................................................................................9

Chương 1. GIỚI THIỆU .............................................................................................................. 10

1.1. Giới thiệu bài toán .......................................................................................................... 10

1.2. Ý nghĩa khoa học và thực tiễn .................................................................................... 11

1.3. Các nghiên cứu trước đây ............................................................................................ 11

1.3.1. Điều khiển xe cân bằng 2 bánh dùng DSP TMS320F28335 ............... 11

1.3.2. Điều khiển xe cân bằng hai bánh dùng bộ điều khiển PID tự chỉnh 12

1.4. Mục tiêu đề tài.................................................................................................................. 13

1.5. Sơ lược nội dung luận văn ........................................................................................... 13

Chương 2. CƠ SỞ LÝ THUYẾT ................................................................................................ 14

2.1. Bộ điều khiển giữ cân bằng cho robot ..................................................................... 14

2.1.1. Nguyên lý hoạt động của robot 2 bánh tự cân bằng ............................... 14

2.1.2. Bộ điều khiển PID .............................................................................................. 15

2.1.3. Mô hình toán học của robot 2 bánh tự cân bằng...................................... 17

2.2. Phần xử lý ảnh.................................................................................................................. 19

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

3.1. Phần cứng .......................................................................................................................... 21

3.1.1. Phần điện ............................................................................................................... 21

3.1.1.1. Sơ đồ khối tổng quát ..................................................................................... 21

3.1.1.2. Danh sách chi tiết linh kiện ........................................................................ 22

9
 3.1.1.3. Vẽ và thiết kế mạch in .................................................................................. 24

3.1.2. Phần cơ ................................................................................................................... 26

3.2. Phần mềm .......................................................................................................................... 27

3.2.1. Các phần mềm hỗ trợ ........................................................................................ 27

3.2.2. Sử dụng Visual Studio để tạo GUI ............................................................... 29

3.3. Giải thuật ............................................................................................................................ 30

3.3.1. Sơ đồ giải thuật của toàn hệ thống ............................................................... 30

3.3.2. Sơ đồ bộ điều khiển robot................................................................................ 32

3.4. Chi tiết quá trình thực hiện đề tài .............................................................................. 34

3.4.1. Đọc giá trị các cảm biến ................................................................................... 34

3.4.2. Điều khiển động cơ ............................................................................................ 40

3.4.3. Thiết kế bộ điều khiển ...................................................................................... 44

3.4.4. Xử lý ảnh ............................................................................................................... 53

3.4.4.1. Chọn đối tượng phù hợp để robot có thể dễ dàng bám theo. .......... 53

3.4.4.2. Xử lý ảnh bị nhiễu ......................................................................................... 55

3.4.4.3. Xử lý ảnh trong các điều kiện môi trường khác nhau và chọn độ
phân giải ảnh phù hợp ..................................................................................................... 55

3.4.4.4. Đo khoảng cách đến đối tượng bằng camera đơn. ............................. 62

3.4.4.5. Kết quả đo khoảng cách khi dùng camera............................................. 64

3.4.4.6. Tốc độ xử lý ảnh............................................................................................. 69

3.4.4.7. Kết luận về việc sử dụng camera .............................................................. 70

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

4.1. Phần điều khiển cân bằng............................................................................................. 71

4.1.1. Điều khiển góc nghiêng của xe...................................................................... 71

10
 4.1.1.1. Góc nghiêng tại 0 ........................................................................................... 71

4.1.1.2. Góc nghiêng bất kỳ........................................................................................ 72

3.2. Điều khiển vị trí của xe .................................................................................................... 73

4.1.1.3. Điều khiển vị trí dựa theo giá trị đặt từ người điều khiển ............... 73

4.1.1.4. Điều khiển vị trí của xe dựa theo vị trí của đối tượng....................... 77

Chương 5. KẾT LUẬN ................................................................................................................. 79

5.1. Đánh giá kết quả đạt được ........................................................................................... 79

5.1.1. Ưu điểm ................................................................................................................. 79

5.1.2. Hạn chế .................................................................................................................. 79

5.2. Phương hướng phát triển trong tương lai ............................................................... 80

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

11
 DANH MỤC HÌNH VẼ
Hình 1: Segway ............................................................................................................................... 10
Hình 2: Robot cân bằng trong logictics ................................................................................ 10
Hình 3: Robot 2 bánh cân bằng dùng TMS320F28335 ................................................... 11
Hình 4: Bộ điều khiển của đề tài robot 2 bánh cân bằng dùng TMS320F28335 .. 12
Hình 5: Mô hình và bộ điều khiển robot 2 bánh tự cân bằng trên Science Direct12
Hình 6: Robot đứng yên ............................................................................................................... 14
Hình 7: Quán tính của robot ...................................................................................................... 14
Hình 8: Cách điều khiển robot .................................................................................................. 14
Hình 9: Điều khiển robot trong các điều kiện khác nhau ................................................ 15
Hình 10: Tổng quan mô hình .................................................................................................... 17
Hình 11: Mô hình nhìn từ cạnh bên ........................................................................................ 17
Hình 12: Mô hình nhìn từ trên xuống .................................................................................... 17
Hình 13: Kết quả tính toán bằng Matlab ............................................................................... 18
Hình 14: Hệ màu RGB ................................................................................................................... 19
Hình 15: Hệ màu HSV ................................................................................................................... 19
Hình 16: Sơ đồ khối của hệ thống ............................................................................................ 21
Hình 17: Sơ đồ nguyên lý ............................................................................................................ 24
Hình 18: Mạch in thiết kế trên Altium ................................................................................... 25
Hình 19: Mô phỏng lắp đặt linh kiện lên mạch in .............................................................. 25
Hình 20: Kết quả mạch in thực tế ............................................................................................. 26
Hình 21: Các bộ phận của robot .............................................................................................. 27
Hình 22: Robot được lắp ghép hoàn chỉnh .......................................................................... 27
Hình 23: Giao diện GUI để quan sát tín hiệu robot .......................................................... 29
Hình 24: Giải thuật toàn hệ thống ........................................................................................... 30
Hình 25: Giải thuật phần chọn chức năng ............................................................................ 31
Hình 26: Sơ đồ điều khiển của hệ thống ............................................................................... 32
Hình 27: Kết quả điều khiển bằng matlab ............................................................................. 33

1
Hình 28: Sơ đồ điều khiển hệ thống hoàn chỉnh ................................................................. 33
Hình 29: Qui ước đặt tên cho các góc đo .............................................................................. 34
Hình 30: Giá trị Accel và Gyro khi chưa mở động cơ ...................................................... 35
Hình 31: Giá trị Accel và Gyro khi mở động cơ ................................................................. 36
Hình 32: Kết quả Accel và Gyro khi dùng bộ lọc thông thấp ......................................... 36
Hình 33: Giá trị offset của số liệu đo gyro ............................................................................ 37
Hình 34: So sánh giá trị đo góc từ MPU6050 và Encoder trường hợp 1 .................. 38
Hình 35: So sánh giá trị đo góc từ MPU6050 và Encoder trường hợp 2 .................. 39
Hình 36: Chế độ đọc x4 của Encoder ...................................................................................... 39
Hình 37: Tần số điều khiển F = 1Khz, vận tốc đặt V = 1 rad/s ..................................... 40
Hình 38: Tần số điều khiển F = 1Khz, vận tốc đặt V = 2 rad/s ..................................... 41
Hình 39: Tần số điều khiển F = 1Khz, vận tốc đặt V = 15 rad/s .................................. 41
Hình 40: Tần số điều khiển F = 10Khz, vận tốc đặt V = 1 rad/s .................................. 42
Hình 41: Tần số điều khiển F = 1Khz, vận tốc đặt V = 2 rad/s ..................................... 42
Hình 42: Tần số điều khiển F = 1Khz, vận tốc đặt V = 15 rad/s .................................. 43
Hình 43: Biểu diễn “dead zone” của động cơ ..................................................................... 44
Hình 44: Phản hồi vận tốc của 2 động cơ khi được cấp cùng mức điện áp .............. 45
Hình 45: Đáp ứng động cơ khi Kp = 1.2 ............................................................................... 45
Hình 46: Đáp ứng động cơ khi Ki = 0.1 ................................................................................ 46
Hình 47: Đáp ứng động cơ khi Kd = 0.001 .......................................................................... 46
Hình 48: Đáp ứng góc của robot khi Kp = 1.35 ................................................................. 47
Hình 49: Đáp ứng góc của robot khi Kp = 1.8 .................................................................... 47
Hình 50: Đáp ứng góc của robot khi Ki = 3.4 ..................................................................... 48
Hình 51: Đáp ứng góc của robot khi Ki = 5.43 .................................................................. 48
Hình 52: Đáp ứng góc của robot khi Kd = 0.01 ................................................................. 48
Hình 53: Đáp ứng góc của robot khi Kd = 0.04 ................................................................. 49
Hình 54: Đáp ứng góc của robot khi góc đặt là 2° ............................................................ 49
Hình 55: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, trường hợp Kp = 0.009
................................................................................................................................................................ 50

2
Hình 56: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, trường hợp Ki = 0.002 50
Hình 57: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, trường hợp Kp = 0.009,
Ki = 0.002 .......................................................................................................................................... 51
Hình 58: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, Kp = 0.015, Ki = 0.0023
................................................................................................................................................................ 52
Hình 59: Đáp ứng vận tốc của robot khi bị đẩy, Kp =0.2, Ki = 0.1, Kd = 0.0001. 52
Hình 60: Màu HSV của các loại màu sắc .............................................................................. 53
Hình 61: So sánh màu sắc trong môi trường ánh sáng mạnh ..................................... 54
Hình 62: Đối tượng cần theo dõi ............................................................................................. 54
Hình 63: So sánh ảnh trước và sau khi lọc Gauss ............................................................. 55
Hình 64: Phương pháp cân bằng Histogram ..................................................................... 56
Hình 65: Dùng phương pháp tự động điều chỉnh độ sáng và tương phản................. 56
Hình 66: Hình ảnh tạo bởi camera độ phân giải thấp .................................................... 57
Hình 67: Hình ảnh tạo bởi camera độ phân giải cao ........................................................ 57
Hình 68: Phát hiện vật trong ánh sáng bình thường với độ phân giải 320x240 . 58
Hình 69: Phát hiện vật dưới ánh sáng đèn huỳnh quang với độ phân giải
320x240 .............................................................................................................................................. 59
Hình 70: Phát hiện vật trong môi trường thiếu sáng với độ phân giải 320x240 59
Hình 71: Phát hiện vật trong ánh sáng bình thường với độ phân giải 1280x1024. 60
Hình 72: Phát hiện vật trong môi trường ánh sáng yếu với độ phân giải 1280x1040
................................................................................................................................................................ 60
Hình 73: Phát hiện vật trong môi trường đèn huỳnh quang với độ phân giải
1280x1040 .......................................................................................................................................... 61
Hình 74: Phát hiện vật trong môi trường ánh sáng yếu với độ phân giải 1280x1040
................................................................................................................................................................ 61
Hình 75: Đồ thị mối quan hệ giữa khoảng cách thực tế và độ cao của ảnh thu được
................................................................................................................................................................ 63
Hình 76: FPS của quá trình xử lý ảnh .................................................................................... 69
Hình 77: Đồ thị dao động của góc nghiêng tại 0° ............................................................. 71

3
Hình 78: Đồ thị dao động của góc nghiêng tại 5° .............................................................. 72
Hình 79: Đồ thị dao động của góc nghiêng tại 10° ........................................................... 72
Hình 80: Đồ thị đáp ứng của robot khi vị trí đặt là 300mm ........................................... 73
Hình 81: Đồ thị đáp ứng của robot khi vị trí đặt là 600mm ........................................... 74
Hình 82: Đồ thị đáp ứng của robot khi vị trí đặt là -300mm .......................................... 75
Hình 83: Đồ thị đáp ứng của robot khi vị trí đặt là -600mm .......................................... 76
Hình 84: Đáp ứng góc nghiêng của robot khi bám theo vật ........................................... 77
Hình 85: Đáp ứng vị trí của robot khi bám theo vật .......................................................... 77
Hình 86: Đáp ứng góc xoay của robot khi bám theo vật ................................................. 78

4
 DANH MỤC BẢNG
Bảng 2.1 Ảnh hưởng của các hệ số KP, KI, KD đến chất lượng điều khiển ................. 16

Bảng 2.2 Giải thích các ký hiệu được sử dụng .................................................................... 17

Bảng 3.1 Danh sách linh kiện điện tử ..................................................................................... 22

Bảng 3.2 Danh sách các phần mềm sử dụng ....................................................................... 27

Bảng 3.3 Chọn điện áp cho từng động cơ ............................................................................. 44

Bảng 3.4 Mối quan hệ giữa khoảng cách thật và độ cao của ảnh............................... 62

Bảng 3.5 Công thức nội suy......................................................................................................... 63

5
 DANH MỤC TỪ VIẾT TẮT

PID : Bộ điều khiển vi tích phân tỉ lệ

LQR: Bộ điều khiển toàn phương tuyến tính

RGB: Hệ màu RGB

HSV: Hệ màu HSV

FPS: Số khung hình trong một giây

LPF: Bộ lọc thông thấp

PWM: Bộ điều chế độ rộng xung

PCB: Mạch in

PPR: Số xung mỗi vòng quay

GUI: Giao diện giao tiếp người dùng

6
 LỜI CẢM ƠN
Lời đầu tiên, em xin chân thành cảm ơn PGS.TS. Huỳnh Thái Hoàng đã tận tình
hướng dẫn, giúp đỡ và truyền đạt nhiều kinh nghiệm cũng như kiến thức quý báu giúp
em hoàn thành tốt luận văn này.

Tiếp theo em xin gửi lời cảm ơn sâu sắc đến các Thầy/Cô trong bộ môn Điều khiển
tự động thuộc Khoa Điện-Điện tử, Trường Đại học Bách Khoa – ĐHQG TP. HCM
đã tạo điều kiện cho em được học tập, nghiên cứu và trau dồi kiến thức trong suốt
thời gian học tập và thực hiện luận văn.

Em xin cảm ơn Quý Thầy/Cô Hội đồng chấm luận văn đã dành thời gian góp
ý nhận xét để luận văn được hoàn thiện.

Cuối cùng, em xin cảm ơn gia đình, các anh chị và bạn bè, những người luôn động
viên, giúp đỡ em trong quá trình học tập và cuộc sống.

Trân trọng./.

Hồ Trần Trọng Nhân

7
 TÓM TẮT LUẬN VĂN
Điều khiển robot 2 bánh tự cân bằng là đề tài phổ biến trong những năm gần đây. Có
rất nhiều bộ điều khiển đã được áp dụng để giữ cho robot cân bằng như điều khiển
mờ Fuzzy, điều khiển LQR, điều khiển trượt,... Do mô hình được tự tay sinh viên làm
nên cấu trúc vật lý không đạt chuẩn, khó có thể áp dụng các bộ điều khiển vừa kể ở
trên. Thay vào đó, đề tài sử dụng bộ điều khiển PID – bộ điều khiển tuy đơn giản
nhưng dễ ứng dụng và khả năng điều khiển đạt chất lượng khá cao, rất phù hợp với
nhiều đối tượng trong thực tế. Bài luận văn này trình bày chi tiết các quá trình, từ
thiết kế, điều khiển đến khi hoàn chỉnh sản phẩm. Robot cân bằng cũng được trang
bị thêm camera để có thể bám theo vật bằng phương pháp xử lý ảnh đơn thuần.

8
 ABSTRACT
Controlling self-balancing 2-wheeled robots is a popular topic in recent years. There
are many controllers that have been applied to keep the robot balanced such as Fuzzy
control, LQR control, sliding control, etc. Because the model is hand-made by
students, the physical structure is not up to standard.So , it is difficult to apply the
above-mentioned controllers. Instead, the topic uses a PID controller - a simple but
easy to apply and has high controllability, which is very suitable for many objects in
practice. This thesis details the process, from design, control to finished product. The
balancing robot is also equipped with a camera so that it can track objects using
simple image processing.

9
Chương 1. GIỚI THIỆU

1.1. Giới thiệu bài toán

Robot di chuyển bằng bánh xe (Wheeled mobile robots) có ưu điểm vượt trội hơn so
với các robot mô phỏng theo hình dáng con người (Humanoid type robots) bởi vì
chúng có thể di chuyển nhanh và linh động trong việc thay đổi hướng đi, điều này
mở ra rất nhiều ứng dụng trong thực tế. Trong các loại robot di chuyển bằng bánh xe
thì robot 2 bánh tự cân bằng đã trở nên phổ biến và được sử dụng để đi lại (segway)
hoặc vận chuyển hàng hóa .

Hình 1: Segway Hình 2: Robot cân bằng trong

logictics

Hệ xe hai bánh tự cân bằng là một đối tượng phi tuyến quen thuộc trong các bài toán
lý thuyết điều khiển. Không giống như các loại xe 3 và 4 bánh, việc thăng bằng và ổn
định của chúng là nhờ trọng tâm nằm trong mặt phẳng chân đế do các bánh xe tạo ra.
Còn đối với xe 2 bánh tự cân bằng thì trọng tâm của xe phải nằm ngay giữa các bánh
xe thì mới giữ thăng bằng được. Để có thể giữ trọng tâm của xe ở vị trí mong muốn
thì cần phải có bộ điều khiển đặc biệt để làm điều đó. Các bộ điều khiển thường được
sử dụng là điều khiển LQR, Fuzzy, điều khiển tự chỉnh,...

10
1.2. Ý nghĩa khoa học và thực tiễn

Hệ thống robot tự cân bằng hai bánh có đặc điểm là đa biến, bậc cao, phi tuyến và
không ổn định, vì vậy nó được nhiều nhà nghiên cứu lý thuyết điều khiển hiện đại coi
là mục tiêu nghiên cứu, và nhiều khái niệm điều khiển trừu tượng như độ ổn định của
hệ thống, khả năng điều khiển và đặc tính chống nhiễu của hệ thống có thể được hiển
thị thông qua các thí nghiệm hệ thống robot tự cân bằng hai bánh. Việc nghiên cứu
và áp dụng thành công các giải thuật điều khiển lên robot 2 bánh tự cân bằng sẽ mở
ra nhiều hướng phát triển mới cho các mô hình robot theo kiểu con lắc ngược cũng
như các hệ thống phi tuyến và kém ổn định.

1.3. Các nghiên cứu trước đây

1.3.1. Điều khiển xe cân bằng 2 bánh dùng DSP TMS320F28335

Hình 3: Robot 2 bánh cân bằng dùng TMS320F28335

Đây là luận văn thạc sĩ được thực hiện vào năm 2010 tại trường Đại học Bách Khoa
TP.HCM. Luận văn tập trung khai thác sức mạnh của vi điều khiển DSP
LMS320F28335 và trình bày một giải thuật điều khiển mới mang thần kinh suy diễn
mờ thích nghi dùng để cập nhật trực tiếp các trọng số mô hình để đối tượng thích nghi
tốt với các tác động bên ngoài và sai số của mô hình toán.

11
Giải thuật điều khiển: Hệ thống điều khiển hai vòng thích nghi dựa trên Back Stepping
và mạng neuron mờ (FNN). Vòng trong dùng bộ điều khiển PI để điều khiển gia tốc
góc bánh xe, vòng ngoài dùng điều khiển Back Stepping đảm bảo cho sự ổn định và
sai số tiến về 0. Mạng neuron mờ dùng để xấp xỉ hàm phi tuyến không biết trước
trong mỗi bước thiết kế.

Hình 4: Bộ điều khiển của đề tài robot 2 bánh cân bằng dùng TMS320F28335

1.3.2. Điều khiển xe cân bằng hai bánh dùng bộ điều khiển PID tự chỉnh

Bài nghiên cứu được đăng tải trên Science Direct. Hệ thống điều khiển bao gồm 3
khối PID điều khiển 3 thông số chính là góc cân bằng robot, vận tốc bánh xe, góc
xoay giữa bánh xe trái và bánh xe phải. Do hệ thống còn bị tác động do nhiễu bên
ngoài nên mạng neuron được sử dụng để đảm bảo sự cân bằng và ổn định của robot.

Hình 5: Mô hình và bộ điều khiển robot 2 bánh tự cân bằng trên Science Direct

12
1.4. Mục tiêu đề tài

Nhiệm vụ của luận văn đặt ra là thiết kế và điều khiển được robot 2 bánh tự cân bằng
bằng bộ điều khiển PID và có khả năng bám theo vật có màu sắc xác định. Mặc dù
đây là đề tài không mới và đã có nhiều bộ điều khiển khác tốt hơn nhưng với mô hình
sinh viên tự làm và nhiều linh kiện điện tử, động cơ không có thông số đầy đủ nên
việc lấy đúng mô hình đối tượng để phục vụ cho các bộ điều khiển nâng cao là rất
khó nên sinh viên chọn bộ điều khiển PID, thông số bộ điều khiển được chọn thông
qua quá trình vận hành thực tế. Bên cạnh đó, đề tài muốn kiểm tra khả năng nhận diện
đối tượng xung quanh mà không cần sử dụng mạng thần kinh bằng phương pháp sử
dụng đơn giản là xử lý ảnh, lọc lấy đối tượng có màu sắc cần theo dõi.
Quá trình này bắt đầu bằng việc chọn lựa phần cứng, xây dựng khung cho robot, thiết
kế PCB để kết nối các module điện tử lại với nhau, mô phỏng mô hình trên Matlab,
đọc và xử lý tín hiệu cảm biến, sau đó lập trình và điều khiển robot giữ cân bằng, và
cuối cùng là các thao tác xử lý ảnh để gửi trả về dữ liệu cần thiết để điều khiển robot.

1.5. Sơ lược nội dung luận văn

Luận văn bao gồm các chương chính như sau:

 Chương 1: Giới thiệu tổng quan về luận văn

 Chương 2: Trình bày các cơ sở lý thuyết được áp dụng vào đề tài
 Chương 3: Mô tả chi tiết quá trình từ khâu thiết kế, lập trình, đọc và xử lý tín
hiệu cảm biến đến chọn thông số bộ điều khiển
 Chương 4: Trình bày kết quả mà sinh viên đạt được
 Chương 5: Nêu ra nhận xét ưu nhược điểm của đề tài và phương hướng phát
triển tương lai

13
Chương 2. CƠ SỞ LÝ THUYẾT

2.1. Bộ điều khiển giữ cân bằng cho robot

2.1.1. Nguyên lý hoạt động của robot 2 bánh tự cân bằng

Robot 2 bánh tự cân bằng hoạt động dựa vào mô hình 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 6: Robot Hình 7: Quán tính Hình 8: Cách điều

đứng yên của robot khiển robot

Khi Robot đứng cân bằng thì góc nghiêng θ của thân Robot và trục thẳng đứng bằng
0 (Hình 1).

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 hướng mà Robot đang nghiêng),
dẫn đến Robot bị ngã (Hình 2).

Do đó, trong trường hợp Robot nghiêng về phía trước, ta sẽ điều khiển cho 2 bánh xe
chạy về phía trước (về phía mà Robot đang nghiêng) để góc lệch θ = 0, từ đó Robot
sẽ cân bằng trở lại (Hình 3).

Tương tự như vậy cho trường hợp Robot nghiêng về phía sau.

14
Hình ảnh minh họa cho các trường hợp điều khiển cân bằng cho Robot:[1]

Hình 9: Điều khiển robot trong các điều kiện khác nhau

2.1.2. 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 toán 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 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. [2]

Công thức bộ điều khiển PID trong miền thời gian liên tục:

15
 𝑡 𝑑𝑒(𝑡)
𝑢(𝑡 ) = 𝐾𝑃 𝑒(𝑡 ) + 𝐾𝐼 ∫0 𝑒(𝑡)𝑑𝑡 + 𝐾𝐷 (2.1)
𝑑𝑡

Vì tín hiệu nhận được là không liên tục nên trong điều khiển, người ta hay sử dụng
bộ điều khiển PID rời rạc như sau:

𝐾𝐼 𝑇 𝑧+1 𝐾𝐷 𝑧−1
𝐺 (𝑧) = 𝐾𝑃 + + (2.2)
2 𝑧−1 𝑇 𝑧

Đ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.
Bảng 2.1 Ảnh hưởng của các hệ số KP, KI, KD đến chất lượng điều khiển

Đáp ứng Sai số xác

Thời gian lên Vọt lố Thời gian xác lập
vòng kín lập

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

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

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

16
2.1.3. Mô hình toán học của robot 2 bánh tự cân bằng

Sơ đồ và hệ quy chiếu hệ xe hai bánh tự cân bằng: [3]

Hình 10: Tổng quan Hình 11: Mô hình nhìn Hình 12: Mô hình nhìn từ
mô hình từ cạnh bên trên xuống

Bảng 2.2 Giải thích các ký hiệu được sử dụng

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

 [(2𝑚 + 𝑀)𝑅2 + 2𝐽𝑤 + 2𝑛2 𝐽𝑚 ]𝜃̈ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜓̈ −

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

Đặt: 𝑥1 = 𝜃, 𝑥2 = 𝑥̇ 1 , 𝑥3 = 𝜓, 𝑥4 = 𝑥̇ 3 , 𝑥5 = 𝜑, 𝑥6 = 𝑥̇ 5 , ta được hệ phương trình

trạng thái của hệ xe cân bằng:

𝑥̇ 1 = 𝑥2
𝑥̇ 2 = 𝑓1 (𝑥1 , 𝑥2 , 𝑥3 , 𝑥4 , 𝑥5 , 𝑥6 , 𝑣𝑙 , 𝑣𝑟 )
𝑥̇ 3 = 𝑥4
(2.6)
𝑥̇ 4 = 𝑓2 (𝑥1 , 𝑥2 , 𝑥3 , 𝑥4 , 𝑥5 , 𝑥6 , 𝑣𝑙 , 𝑣𝑟 )
𝑥̇ 5 = 𝑥6
{𝑥̇ 6 = 𝑓3 (𝑥1 , 𝑥2 , 𝑥3 , 𝑥4 , 𝑥5 , 𝑥6 , 𝑣𝑙 , 𝑣𝑟 )
Với f1, f2, f3 được suy ra từ các phương trình động học của hệ thống. Sử dụng thư
viện Symbolic Math Toolbox của MATLAB để tìm biểu thức f1, f2, f3:

Hình 13: Kết quả tính toán bằng Matlab

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

Hệ màu RGB:

RGB là một mô hình màu có thể pha trộn, có nghĩa là các tỷ lệ ánh sáng màu đỏ,
xanh lam và xanh lục khác nhau có thể được sử dụng để tạo ra bất kỳ màu nào. Mô
hình màu RGB được tạo ra đặc biệt cho mục đích hiển thị (màn hình hiển thị, máy
chiếu,...).

Hình 14: Hệ màu RGB

Hệ màu HSV

Không gian màu này biến đổi không gian RGB thành một kiểu biểu diễn thuận tiện
hơn, giúp màu sắc có thể được biểu diễn chính xác hơn, trông tự nhiên hơn.

Hình 15: Hệ màu HSV

Saturation: là sự thể hiện sự thay đổi giữa các pixel, đại diện cho sự động của một
màu sắc.

19
Value: Là độ sáng tối đa mà một màu có thể có. Cụ thể hơn, nó tỷ lệ chính xác với
độ sáng của pixel sáng nhất trong màu RGB.

Hue: Cho chúng ta biết mọi thứ chúng ta cần biết về màu tinh khiết mà chúng ta đang
cố gắng biểu diễn.

Lý do chọn hệ màu HSV để nhận dạng màu sắc:

Lý do chọn sử dụng không gian màu HSV để phát hiện/tạo ngưỡng màu là vì HSV
tỏ ra ưu thế mạnh mẽ hơn đối với các thay đổi ánh sáng bên ngoài. Có nghĩa là trong
trường hợp ánh sáng bên ngoài thay đổi nhỏ (chẳng hạn như bóng nhạt,...), giá trị Hue
thay đổi tương đối ít hơn giá trị RGB.

Ví dụ: hai sắc thái của màu đỏ có thể có các giá trị Hue tương tự nhau, nhưng các giá
trị RGB khác nhau rất nhiều. Trong các tình huống thực tế, chẳng hạn như theo dõi
đối tượng dựa trên màu sắc, chúng ta cần đảm bảo rằng chương trình của chúng ta
chạy tốt bất kể những thay đổi của môi trường càng nhiều càng tốt. Vì vậy, chúng ta
chọn ngưỡng màu HSV.

Ảnh được chụp có rất nhiều nhiễu, để giảm nhiễu, ta dùng bộ lọc Gaussian. Lọc
Gaussian có tác dụng giống như bộ lọc thông thấp (loại bỏ tần số cao), giúp làm mịn
ảnh và làm mờ các chi tiết (không quan trọng).

Khái niệm về contour:

Contour có thể được giải thích đơn giản là một đường cong nối tất cả các điểm liên
tục (dọc theo đường biên), có cùng màu hoặc cường độ. Contour là một công cụ hữu
ích để phân tích hình dạng, phát hiện 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 ở xử lý ảnh cơ bản có thể liệt kê đến là đặt ngưỡng, hoặc candy edge.

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

3.1. Phần cứng

3.1.1. Phần điện

3.1.1.1. Sơ đồ khối tổng quát

Hình 16: Sơ đồ khối của hệ thống

Giới thiệu tổng quan các thành phần trong sơ đồ khối:

- Khối nguồn: Source_1 để cung cấp điện áp chính cho động cơ, source_2 để
cung cấp năng lượng cho khối điều khiển, vì source_2 cho ra điện áp ổn định,
đảm bảo cho vi điều khiển hoạt động tốt.
- Các khối cảm biến: Bao gồm encoder, cảm biến góc MPU6050, Raspberry
Camera

21
 - Khối điều khiển: Bao gồm vi điều khiển STM32F411VET để điều khiển hoạt
động của robot và máy tính nhúng Raspberry Pi 3B+ xử lý các vấn đề liên
quan đến ảnh.
- Khối hiển thị: Màn hình Oled
- Khối truyền thông: Module Bluetooth để truyền nhận dữ liệu giữa máy tính/
điện thoại và robot
- Khối công suất: Mạch cầu H để điều khiển tốc độ và hướng quay của động cơ.

3.1.1.2. Danh sách chi tiết linh kiện

Bảng 3.1 Danh sách linh kiện điện tử

Số
STT Tên Đặc tính kỹ thuật Hình ảnh
lượng
512 KB Flash
Mạch phát
128 KB RAM
triển
1 1 Nguồn cung cấp:
STM32F411
3VDC/5VDC
Discovery
Hỗ trợ I2C, UART, …
Nguồn cung cấp: 3-
12VDC
Công suất: 2W
Động cơ DC
Tỉ số truyền: 30:1
2 có hộp số 2
Cảm biến trường: 11 ppr
JGB37-520
Tốc độ: 333 rpm
Moment xoắn: 3.5
KG.CM
Nguồn cung cấp: 5-
Module cầu H 50VDC
3 2
L298 V2 Tín hiệu điều khiển 5-
7VDC

22
 Dòng cao nhất mỗi cầu:
2A
Công suất: 20W
Nguồn cung cấp: 3-
5VDC
Module
4 1 Giao thức: I2C
MPU6050
Dữ liệu 16 bit

Nguồn cung cấp: 5VDC

Tín hiệu giao tiếp: TTL
Module 3.3-5VDC
5 Bluetooth 2 Khoảng cách truyền lớn
HM10 nhất: 100m trong điều
kiện lý tưởng
Sử dụng công nghệ BLE
Nguồn cung cấp: 3-
5VDC
6 Màn hình Oled 1
Độ phân giải: 128x64
Giao thức: I2C
Điện áp sạc đầy:
4.2VDC
7 Pin 18650 5
Dung lượng: 2100mAh
Dòng xả: 12.5A
Nguồn cung cấp:
5V/2.5A
Raspberry Pi Broadcom
8 1
3B+ BCM2837B0, Cortex-
A53 (ARMv8) 64-bit
SoC @ 1.4GHz

23
 1GB LPDDR2 SDRAM
Cảm biến Sony IMX219
8-megapixel
Raspberry Pi
9 1 Hỗ trợ quay video
Camera V2.1
1080p30, 720p60 và
640x480p90

3.1.1.3. Vẽ và thiết kế mạch in

Vẽ sơ đồ nguyên lý và sắp xếp mạch in bằng phần mềm Altium.

Mạch in được tạo ra nhằm mục đích kết nối các module điện tử rời rạc lại với nhau
thông qua các đường đồng trên mạch để các tín hiệu được ổn định và giảm nhiễu.
Mạch in được thực hiện là mạch in 1 lớp và được làm thủ công

Hình 17: Sơ đồ nguyên lý

24
 Hình 18: Mạch in thiết kế trên Altium

Hình 19: Mô phỏng lắp đặt linh kiện lên mạch in

25
 Hình 20: Kết quả mạch in thực tế

3.1.2. Phần cơ

Một số lưu ý khi thiết kế phần cơ khí cho robot:

- Robot hoạt động theo nguyên lý cân bằng con lắc ngược nên trọng tâm của
Robot càng cao thì càng dễ điều khiển. Vì vậy cần đặt các vật nặng lên phần
đỉnh của robot (pin,...)
- Để robot có khả năng giữ cân bằng tốt thì động cơ được chọn phải có moment
xoắn phù hợp với khối lượng và kích thước của robot. Ngoài vấn đề chọn động
cơ thì chọn bánh xe có kích thước lớn, vỏ bánh xe cứng cũng rất quan trọng.
- Phần đế của robot (nơi kết nối giữa động cơ và thân của robot) phải thật chắc
chắn vì đây là nơi chịu rung lắc rất nhiều (do động cơ gây nên).
Kích thước của Robot: Cao 30cm, rộng 35cm, cân nặng 4kg

26
Dưới đây là chi tiết robot khi đã được lắp ráp hoàn chỉnh:

Hình 22: Các bộ phận của robot Hình 21: Robot được lắp ghép hoàn
chỉnh

3.2. Phần mềm

3.2.1. Các phần mềm hỗ trợ

Bảng 3.2 Danh sách các phần mềm sử dụng

Hệ điều hành làm

STT. Biểu tượng Tên Mục đích sử dụng
việc

Lập trình và gỡ lỗi

1 Keil uVision5 Windows
(debug) robot

Quan sát và vẽ đồ thị

giá trị các biến trong
2 STM Studio Windows
chương trình điều
khiển

27
 Mô phỏng mô hình
3 Matlab Windows đối tượng và bộ điều
khiển

Lập trình GUI để thu

4 Visual Studio Windows thập dữ liệu và điều
khiển robot

Điều khiển RaspBerry

4 VNC Viewer Windows
từ xa

Thiết kế mạch in
5 Altium Windows
(PCB)

Quan sát tín hiệu xung

6 Logic Windows
từ vi điều khiển

Lập trình và gỡ lỗi

7 PyCharm Linux
phần xử lý ảnh

Xây dựng GUI trên

Bluetooth
8 Android điện thoại để điều
Electronics
khiển robot

28
3.2.2. Sử dụng Visual Studio để tạo GUI

GUI được tạo ra mục đích chính là để thu thập dữ liệu khi robot di chuyển. Dữ liệu
được thu thập thông qua bluetooth. Có thể điều khiển robot hoạt động từ GUI nhưng
thao tác khó nên việc điều khiển sẽ do ứng dụng trên điện thoại.

Đây là giao diện của GUI:

Hình 23: Giao diện GUI để quan sát tín hiệu robot

GUI gồm 3 khu vực chính:

Khu vực 1 là nơi vẽ đồ thị đáp ứng góc nghiêng robot, khoảng cách của robot khi
dùng encoder hoặc camera để đo và góc xoay của robot. Dữ liệu trên đồ thị sẽ được
cập nhật mỗi khi có thông tin gửi về từ robot.

Khu vực 2 dùng để điều khiển chuyển động của robot.

Khu vực 3 quản lý chức năng liên quan đến kết nối robot để lấy dữ liệu và điều khiển.

29
3.3. Giải thuật

3.3.1. Sơ đồ giải thuật của toàn hệ thống

Hình 24: Giải thuật toàn hệ thống

Chi tiết các khối chức năng trong sơ đồ khối:

- Khởi động các ngoại vi: Cấu hình và khởi tạo các ngoại vi phục vụ cho hoạt
động của Robot như: Timer để điều chế xung PWM; Timer để tạo chu kỳ lấy
mẫu điều khiển Robot; Ngoại vi UART để giao tiếp với module Bluetooth và
Raspberry; Ngoại vi I2C để giao tiếp với Oled LCD và cảm biến góc nghiêng;
Các GPIO phục vụ cho điều khiển ON/OFF, ngắt nút nhấn.

30
- Chọn chế độ hoạt động cho Robot: Chọn 1 trong 3 chế độ hoạt động của Robot:
Calib góc đo của robot; điều khiển theo vị trí; tự động bám theo vật.

Hình 25: Giải thuật phần chọn chức năng

31
3.3.2. Sơ đồ bộ điều khiển robot

- Sơ đồ khối điều khiển robot cân bằng, mô phỏng bằng Matlab:

Hình 26: Sơ đồ điều khiển của hệ thống

Trong đó:

Theta: Góc bánh xe

Psi: Góc nghiêng Robot

Phi: Góc xoay Robot

Bộ điều khiển mô phỏng trên Matlab bao gồm 3 bộ PID: 2 bộ PID mắc nối tiếp nhau
để điều khiển vị trí xe và 1 bộ PID dùng để điều khiển góc xoay của xe. Cụ thể hoạt
động của từng bộ PID như sau:

- Bộ điều khiển “PID theta” chịu trách nhiệm điều khiển góc tới, lui của bánh
xe (tức là vị trí của xe). Khi người điều khiển đưa ra một giá trị đặt, bộ PID
này sẽ dựa vào sai số giữa giá trị đặt đó và vị trí hiện tại của xe để tính ra được
góc nghiêng cần thiết của xe để xe đến được vị trí mong muốn.
- Bộ điều khiển “PID Psi” nhận giá trị góc nghiêng mong muốn từ bộ PID theta
trước đó, dựa vào sai số giữa giá trị góc đặt và giá trị góc thực tế để tính ra
được vận tốc của bánh xe để đạt được góc nghiêng mong muốn.

32
- Bộ điều khiển “PID phi” hoạt động độc lập, dựa vào sai số giữa góc xoay mong
muốn và góc xoay hiện tại để tăng/giảm vận tốc của từng bánh xe để robot có
thể xoay đến góc mong muốn.
Dưới đây là kết quả mô phỏng khi điều khiển robot với góc bánh xe là 5 rad,
góc xoay của Robot là –pi/2:

Hình 27: Kết quả điều khiển bằng matlab

Nhưng trong thực tế, 2 động cơ của robot khó có thể đáp ứng cùng lúc nên làm
cho robot bị xoay, hay thậm chí là giữ cân bằng không được. Để giải quyết
vấn đề này, ta ghép thêm bộ PID điều khiển vận tốc cho từng động cơ.
Vì mô hình toán học của robot tham khảo [3] không hỗ trợ đọc giá trị vận tốc
của từng bánh xe nên không thể thêm vào matlab để chạy kết quả mô phỏng.
Bộ điều khiển cân bằng hoàn chỉnh sẽ có sơ đồ như sau:

Hình 28: Sơ đồ điều khiển hệ thống hoàn chỉnh

33
3.4. Chi tiết quá trình thực hiện đề tài

3.4.1. Đọc giá trị các cảm biến

a) Cảm biến góc nghiêng GY-521

Tính toán góc từ dữ liệu Accel: [4]

Hình 29: Qui ước đặt tên cho các góc đo

𝐴𝑋
𝜌 = 𝑎𝑟𝑐𝑡𝑎𝑛 ( ) (3.1)
√𝐴2𝑌 +𝐴2𝑍

𝐴𝑌
𝜑 = 𝑎𝑟𝑐𝑡𝑎𝑛 ( ) (3.2)
√𝐴2𝑋 +𝐴2𝑍

√𝐴2𝑋 +𝐴2𝑌
𝜃 = 𝑎𝑟𝑐𝑡𝑎𝑛 ( ) (3.3)
𝐴𝑍

Tính toán góc từ dữ liệu Gyro:

Con quay hồi chuyển cho ta tốc độ thay đổi của vị trí góc theo thời gian (vận tốc góc)
với đơn vị là [deg./s]. Điều này có nghĩa là chúng ta nhận được đạo hàm của vị trí
góc theo thời gian.
dθ
θ̇ = (3.4)
dt

Để có được vị trí góc, chúng ta chỉ cần tích phân vận tốc góc. Vì vậy, giả sử rằng tại
t = 0 theta = 0, chúng ta có thể tìm thấy vị trí góc tại bất kỳ thời điểm t nào bằng
phương trình sau:

34
 𝑡
𝜃(𝑡 ) = ∫0 𝜃̇(𝑡)𝑑𝑡 ≈ ∑𝑡0 𝜃̇(𝑡)𝑇𝑆 (3.5)
Trong thực tế, giá trị đo của Accel và Gyro rất nhạy với nhiễu, đặc biệt là khi động
cơ trong quá trình hoạt động:
Ví dụ đồ thị biểu diễn giá trị Accel và Gyro theo trục X:
Khi để xe đứng yên:

Hình 30: Giá trị Accel và Gyro khi chưa mở động cơ

35
Khi động cơ xe hoạt động:

Hình 31: Giá trị Accel và Gyro khi mở động cơ

Dùng bộ lọc thông thấp với băng thông = 10 Hz

Hình 32: Kết quả Accel và Gyro khi dùng bộ lọc thông thấp

36
Vậy là ta đã xử lý xong nhiễu ảnh hưởng đến Accel và Gyro.

Calib lại giá trị đo Gyro: Khi xe đứng yên nhưng giá trị đo Gyro không dao động
quanh giá trị 0 nên cần calib lại:

Hình 33: Giá trị offset của số liệu đo gyro

Việc tính giá trị offset cho Gyro và calib giá trị đo góc sẽ được thực hiện lúc robot
bắt đầu khởi động.

Tính toán góc nghiêng từ các dữ liệu thô đo được trên MPU6050

Dữ liệu từ Accel: Vì gia tốc kế đo tất cả các lực tác động lên vật thể nên nó cũng sẽ
đo nhiều hơn một vectơ trọng lực. Mọi lực nhỏ tác dụng lên vật thể sẽ làm xáo trộn
hoàn toàn phép đo. Nếu chúng ta đang làm việc trên một hệ thống được truyền động
(như quadrocopter), thì các lực điều khiển hệ thống cũng sẽ hiển thị trên cảm biến.
Dữ liệu gia tốc kế chỉ đáng tin cậy về lâu dài, vì vậy phải sử dụng bộ lọc thông thấp.

Dữ liệu từ Gyro: Vấn đề thường gặp phải nhất của gyro là drift = độ trôi, nó thay đổi
chậm theo thời gian. Nguyên nhân bởi vì do các tác động cơ khí, rung động tác động
lên gyro, sau một thời gian sử dụng thì giá trị trôi này tích lũy lên đáng kể, làm giá trị
đo góc không còn chính xác. Dù vậy, điểm mạnh của gyro là ít bị nhiễu hơn
accelemeter, nghĩa là giá trị tức thời của nó đáng tin cậy.

37
Giải pháp khắc phục: Sử dụng bộ lọc để khử nhiễu hoặc tính toán góc bằng việc kết
hợp 2 giá trị đo của Accel và Gyro: Bộ lọc Complementary cung cấp sự kết hợp tốt
nhất của dữ liệu Accel và Gyro. Về ngắn hạn, ta sử dụng dữ liệu từ con quay hồi
chuyển, vì nó rất chính xác và không dễ bị tác động bởi các lực bên ngoài. Về lâu dài,
ta sử dụng dữ liệu từ gia tốc kế, vì nó không bị trôi. Ở dạng đơn giản nhất, bộ lọc
trông như sau:

Angle = α*(Angle + gyrData*Ts) + (1- α)*(accData) (3.6)

Dữ liệu con quay hồi chuyển được tích phân lên rồi cộng dồn vào giá trị góc hiện tại.
Sau đó, nó được kết hợp với dữ liệu thông thấp từ gia tốc kế (đã được xử lý với atan2).
Hệ số α có thể được thay đổi để điều chỉnh bộ lọc phù hợp.

Chọn thông số α phù hợp cho bộ lọc. Hệ số α được chọn sao cho tốc độ gốc đáp ứng
thật nhanh nhưng cũng phải tránh vọt lố.

α = 0.98: Xuất hiện một ít lọt vố

Hình 34: So sánh giá trị đo góc từ MPU6050 và Encoder trường hợp 1

38
α = 0.99: Số đo góc từ MPU và Encoder tương đối tương đồng nhau, ít vọt lố hơn
trường hợp α = 0.98.

Hình 35: So sánh giá trị đo góc từ MPU6050 và Encoder trường hợp 2

b) Encoder
Mô hình sử dụng động cơ có gắn Hall Encoder với độ phân giải là 11 xung/vòng.
Động cơ cũng được gắn với 1 bộ giảm tốc với tỉ lệ là 1/30. Để tăng độ phân giải cho
encoder, ta dùng chế độ đọc encoder x4

Hình 36: Chế độ đọc x4 của Encoder

Vậy tổng cộng encoder sẽ có độ phân giải là 30 * 11 * 4 = 1320 (xung/vòng)

39
Tính toán quãng đường robot di chuyển từ giá trị encoder:
(𝑠ố 𝑥𝑢𝑛𝑔 đọ𝑐 đượ𝑐)∗(𝐶ℎ𝑢 𝑣𝑖 𝑏á𝑛ℎ 𝑥𝑒)
𝑑(𝑚𝑚) = (3.7)
𝑠ố 𝑥𝑢𝑛𝑔 1 𝑣ò𝑛𝑔

Tính toán góc xoay robot dựa vào độ chênh lệch giá trị góc giữa 2 bánh xe. Với R là
bán kính bánh xe, W là khoảng cách giữa 2 bánh xe, ta có:
𝑅
𝑦𝑎𝑤 = ∗ (𝐺ó𝑐 𝑏á𝑛ℎ 𝑥𝑒 1 − 𝐺ó𝑐 𝑏á𝑛ℎ 𝑥𝑒 2) (3.8)
𝑊

3.4.2. Điều khiển động cơ

Dùng mạch cầu H để điều khiển tốc độ và chiều quay của động cơ.

a) Khảo sát và chọn tần số PWM để điều khiển động cơ

Tần số PWM có ảnh hưởng lớn đến chất lượng điều khiển của động cơ.

F = 1 Khz, v_sp = 1 rad/s

Hình 37: Tần số điều khiển F = 1Khz, vận tốc đặt V = 1 rad/s

40
F = 1 Khz, v_sp = 2 rad/s

Hình 38: Tần số điều khiển F = 1Khz, vận tốc đặt V = 2 rad/s

F = 1 Khz, v_sp = 15 rad/s

Hình 39: Tần số điều khiển F = 1Khz, vận tốc đặt V = 15 rad/s

41
F = 10 Khz, v_sp = 1 rad/s

Hình 40: Tần số điều khiển F = 10Khz, vận tốc đặt V = 1 rad/s

F = 10Khz, v_sp = 2 rad/s

Hình 41: Tần số điều khiển F = 1Khz, vận tốc đặt V = 2 rad/s

42
F = 10 Khz, v_sp = 15 rad/s

Hình 42: Tần số điều khiển F = 1Khz, vận tốc đặt V = 15 rad/s

Nhận xét: Giá trị tần số PWM càng lớn thì chất lượng điều khiển càng tốt. Chọn tần
số PWM = 10Khz.

43
 b) Tìm giá trị bù cho điện áp của động cơ
Vì mỗi động cơ đều có 1 “vùng chết” (dead zone). Nếu điện áp đặt vào nằm ngoài
“dead zone” đó thì động cơ mới hoạt động.

Hình 43: Biểu diễn “dead zone” của động cơ

Việc tìm giá trị dead zone này phụ thuộc vào thực nghiệm: Tăng dần điện áp cấp vào
động cơ đến khi nó bắt đầu hoạt động. Giá trị tìm được là:

Bảng 3.3 Chọn điện áp cho từng động cơ

DC Motor 1 DC Motor 2

U1 U2 U1 U2

-4.83V 5.27V -4.88V 5.27V

3.4.3. Thiết kế bộ điều khiển

a) Bộ PID điều khiển vận tốc bánh xe

44
Vì khi được cấp cùng một mức điện áp, 2 động cơ sẽ không đáp ứng cùng lúc, không
quay cùng tốc độ nên phải thiết kế bộ PID vận tốc cho từng động cơ.

Hình 44: Phản hồi vận tốc của 2 động cơ khi được cấp cùng mức điện áp

Đặt V_sp = 2 rad/s

Tăng dần Kp đến khi vận tốc phản hồi có vọt lố

Hình 45: Đáp ứng động cơ khi Kp = 1.2

45
Tăng dần Ki để sai số xác lập tiến về 0.

Hình 46: Đáp ứng động cơ khi Ki = 0.1

Tăng dần Kd để giảm lọt vố, làm mịn vận tốc của bánh xe:

Hình 47: Đáp ứng động cơ khi Kd = 0.001

Thông số PID cuối cùng là: Kp = 1.2, Ki = 0.1, Kd = 0.001

46
b) Bộ PID điều khiển góc nghiêng robot
Đặt góc nghiêng bằng 0.

Kp = 1.35: Xe có thể giữ cân bằng trong 1 khoảng thời gian ngắn

Hình 48: Đáp ứng góc của robot khi Kp = 1.35

Kp = 1.8: Xe có thể giữ cân bằng nhưng dao động lớn, sau 1 khoảng thời gian sẽ chạy
về phía trước như các trường hợp trước.

Nguyên nhân robot chạy về phía trước: Do mô hình thiết kế không cân đối nên khi
dao động, nếu robot nghiêng về phía có khối lượng lớn hơn thì sẽ có quán tính chạy
về trước, không thể vực dậy để trở về góc 0 được. Cách khắc phục:

Cách 1: Điều chỉnh lại mô hình cho cân đối, điều này khó vì phải tính toán lại mô thật
cơ khí thật chi tiết.

Cách 2: Chấp nhận mô hình không cân xứng, tiếp tục chỉnh thông số Ki để hỗ trợ
robot có thể trở về góc 0 (bù thêm phần năng lượng mà robot còn thiếu)

Hình 49: Đáp ứng góc của robot khi Kp = 1.8

47
Ki = 3.4: Giá trị nhỏ nhất để robot có thể đứng yên 1 chỗ, nhưng thỉnh thoảng vẫn xê
dịch về phía trước

Hình 50: Đáp ứng góc của robot khi Ki = 3.4

Ki = 5.43: Giá trị phù hợp, robot đứng yên trong hầu hết trường hợp

Hình 51: Đáp ứng góc của robot khi Ki = 5.43

Vấn đề tiếp theo là làm mịn phản hồi của robot: tinh chỉnh hệ số Kd để đáp ứng góc
của robot bớt dao động.

Kd = 0.01: Robot giảm dao động nhưng không thể dừng lại khi bị đẩy đi

Hình 52: Đáp ứng góc của robot khi Kd = 0.01

48
Kd = 0.04: Robot có thể dừng lại khi bị đẩy đi

Hình 53: Đáp ứng góc của robot khi Kd = 0.04

Thông số bộ PID Kp = 1.35, Ki = 5.45, Kd = 0.04

Kiểm tra lại thông số PID vừa chọn với góc nghiêng đặt là 2°

Hình 54: Đáp ứng góc của robot khi góc đặt là 2°

49
c) Bộ PID điều khiển vị trí robot
Điều khiển PID cho vị trí: Vì điều khiển theo vị trí đã có khâu tích phân lý tưởng rồi
nên chỉ cần thay đổi các hệ số Kp và Kd.

Kp= 0.009: Robot có thể trở về vị trí ban đầu khi bị đẩy đi, nhưng lọt vố còn lớn

Hình 55: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, trường hợp Kp = 0.009

Kp = 0.009, Ki = 0.002: Robot có thể trở về vị trí ban đầu khi bị đẩy đi, lọt vố giảm

Hình 56: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, trường hợp Ki = 0.002

50
Kiểm tra đáp ứng Robot với bộ PID vừa tìm được:

Hình 57: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, trường hợp Kp = 0.009,
Ki = 0.002

Nhận xét: Thời gian đáp ứng 20cm chậm (>1.5s), sai số xác lập lớn nhất là 7cm. Cần
phải cải thiện thêm.

51
Kp = 0.015, Kd = 0.0023, thời gian đáp ứng 20cm dưới 1s, sai số xác lập lớn nhất là
5cm

Hình 58: Đáp ứng vị trí của robot khi bị đẩy khỏi vị trí 0, Kp = 0.015, Ki = 0.0023

d) Bộ PID điều khiển vận tốc robot

Bộ điều khiển này được tạo ra để giữ robot đứng yên (v_sp = 0) trong chế độ bám
theo vật, không dùng để điều khiển robot.

Kp =0.2, Ki = 0.1, Kd = 0.0001. Đồ thị biểu diễn quá trình từ khi robot đứng yên,
chịu tác động lực và đứng yên trở lại.

Hình 59: Đáp ứng vận tốc của robot khi bị đẩy, Kp =0.2, Ki = 0.1, Kd = 0.0001

52
3.4.4. Xử lý ảnh

3.4.4.1. Chọn đối tượng phù hợp để robot có thể dễ dàng bám theo.

- Chọn chất liệu của đối tượng: Để Camera dễ phát hiện ra đối tượng cần theo
dõi, đối tượng đó phải có màu sắc dễ phân biệt và chất liệu ít phản chiếu lại ánh sáng
(bề mặt nhám, chất liệu bằng vải, len,... ). Do không tìm được vật có chất liệu phù
hợp với yêu cầu, nên đề tài sẽ bám theo vật có chất liệu từ nhựa bóng, dẫn đến chất
lượng theo dõi đối tượng bị giảm.

- Chọn hình dạng của đối tượng: Đề tài chọn đối tượng có hình cầu, vì khi
camera thu ảnh 2D từ hình cầu, dù lấy ảnh từ góc độ nào thì ảnh 2D thu được vẫn là
hình tròn, dễ dàng cho việc phát hiện và đo khoảng cách.

- Chọn kích thước đối tượng: Đối tượng có kích thước càng to, càng dễ theo dõi.

- Chọn màu sắc cho đối tượng: Vì ý tưởng ban đầu là dựa vào hệ màu HSV để
phát hiện màu, nên ta thu ảnh tất cả các màu và chuyển về hệ HSV để dễ lựa chọn:

Hình 60: Màu HSV của các loại màu sắc

53
Kết quả quan sát được: Chỉ có màu hồng và màu trắng là nổi bật so với những màu
còn lại. Nhưng màu trắng rất dễ gây nhầm lẫn, nhất là khi các vật trong khung hình
gặp nguồn sáng lớn, các vật có bề mặt bóng (nền gạch, ...) sẽ gây hiểu làm là màu
trắng, như ví dụ sau:

Hình 61: So sánh màu sắc trong môi trường ánh sáng mạnh

Kết luận: Sau khi chọn lựa đối tượng phù hợp với các yêu cầu trên, vật được chọn để
robot bám theo là quả bóng nhựa màu hồng, đường kính 80 mm.

Hình 62: Đối tượng

cần theo dõi

54
3.4.4.2. Xử lý ảnh bị nhiễu

Phương hướng giải quyết: Dùng các bộ lọc thông thấp, thông cao, lọc Gauss để giảm
nhiễu cho ảnh.

Đề tài chọn bộ lọc Gauss để lọc nhiễu cho ảnh vì bộ lọc Gauss có thể loại bỏ nhiễu
bởi các tần số cao tốt hơn những bộ lọc khác, điều này giúp cho ảnh nhận được sẽ
mịn và giảm đi đáng kể các thành phần có tần số cao ( nhiễu)

Hình 63: So sánh ảnh trước và sau khi lọc Gauss

3.4.4.3. Xử lý ảnh trong các điều kiện môi trường khác nhau và chọn độ phân
giải ảnh phù hợp

Do ảnh hưởng bởi ánh sáng là rất lớn nên trong nhiều trường hợp, camera sẽ không
nhận dạng được màu sắc của đối tượng (Ánh sáng quá yếu, ánh sáng quá mạnh, ánh
sáng từ đèn huỳnh quang hoặc các nguồn sáng khác). Có 2 phương hướng giải quyết:

- Phương hướng 1: Dùng các phương pháp tăng cường ảnh để cải thiện chất
lượng ảnh trong các điều kiện khác nhau. Cách này có thể cải thiện được chất lượng
ảnh, nhưng qua quá trình thực nghiệm, nếu ảnh khác quá xa so với chất lượng mong
muốn, khối lượng tính toán sẽ rất lớn, làm giảm FPS, dẫn đến chất lượng điều khiển
robot không được tốt; Đồng thời, các nhiễu cũng được khuếch đại theo nên gây khó
khăn cho việc xác định màu ảnh. Các cách cải thiện chất lượng ảnh đã thực hiện: Cân
bằng Histogram, tự động điều chỉnh độ sáng và tương phản

55
Kết quả của 1 số phương pháp cải thiện ảnh: (Vấn đề này gặp phải khi sử dụng camera
cũ)

- Dùng phương pháp cân bằng Histogram: (Camera cũ)

Hình 64: Phương pháp cân bằng Histogram

- Dùng phương pháp tự động điều chỉnh độ sáng và độ tương phản: (Camera
cũ)

Hình 65: Dùng phương pháp tự động điều chỉnh độ sáng và tương phản

56
- Phương hướng 2: Sử dụng phần cứng camera tốt hơn, chấp nhận Robot không
hoạt động được trong các điều kiện quá sáng hoặc quá tối, hạn chế xử lý ảnh
bằng phần mềm vì sẽ làm giảm FPS.
So sánh ảnh và Histogram được lấy từ camera cũ 5MP và camera mới 8MB:

Cũ

Hình 66: Hình ảnh tạo bởi camera độ phân giải thấp

Mới

Hình 67: Hình ảnh tạo bởi camera độ phân giải cao

57
Kết quả sau khi lọc màu trong nhiều điều kiện ánh sáng với 2 độ phân giải khác nhau
(320x240 và 1280x1024):

 Ghi ảnh với độ phân giải (320x240)

Môi trường ánh sáng bình thường

Hình 68: Phát hiện vật trong ánh sáng bình thường với
độ phân giải 320x240
Ảnh 1: Ảnh hệ màu RGB

Ảnh 2:Ảnh hệ màu HSV

Ảnh 3: Mặt nạ (Mask) dùng để lọc màu quả bóng (màu hồng)

Ảnh 4: Dùng mặt nạ để lọc quả bóng (màu hồng)

58
Môi trường ánh sáng đèn huỳnh quang

Hình 69: Phát hiện vật dưới ánh sáng đèn huỳnh quang với
độ phân giải 320x240

Môi trường thiếu sáng

Hình 70: Phát hiện vật trong môi trường thiếu sáng với độ
phân giải 320x240

59
  Ghi ảnh với độ phân giải (1280x1024)

Trong môi trường ánh sáng bình thường:

Hình 71: Phát hiện vật trong ánh sáng bình thường với độ phân giải 1280x1024

Trong môi trường ánh sáng yếu:

Hình 72: Phát hiện vật trong môi trường ánh sáng yếu với độ phân giải 1280x1040

60
Trong môi trường ánh sáng đèn huỳnh quang:

Hình 73: Phát hiện vật trong môi trường đèn huỳnh quang với độ phân giải
1280x1040
Trong môi trường tối

Hình 74: Phát hiện vật trong môi trường ánh sáng yếu với độ phân giải 1280x1040

61
Nhận xét:

- Do sử dụng Camera tốt hơn nên đã giảm được sự ảnh hưởng ánh sáng của môi
trường xung quanh. (Kết quả từ camera cũ khá tệ nên không đề cập trong bài
báo cáo này)

- Nhận thấy độ phân giải 320x240 và 1280x1024 đều cho ra kết quả khá tương
đồng (mặc dù 1280x1024 cho ra chi tiết tốt hơn, nhưng 320x240 đã đủ đáp
ứng nhu cầu của đề tài, chọn độ phân giải cao trong trường hợp này sẽ tăng
khối tượng tính toán và giảm FPS)

3.4.4.4. Đo khoảng cách đến đối tượng bằng camera đơn.

Việc xác định khoảng cách phụ thuộc rất nhiều vào quá trình phát hiện đối tượng ở
các bước ở trên.

Ý tưởng thực hiện đo khoảng cách bằng Camera: Khi vật ở gần Camera thì ảnh thu
được sẽ lớn, khi vật ở xa thì kích thước ảnh của vật sẽ nhỏ.

Sau quá trình đo đạc mối quan hệ giữa khoảng cách thật từ vật đến camera và độ cao
của vật thu được trong ảnh, ta được bảng sau:

Bảng 3.4 Mối quan hệ giữa khoảng cách thật và độ cao của ảnh

Khoảng cách (cm) Độ cao trung bình của vật sau 3 lần đo (pixel)

60 32

70 29

80 25

90 23

100 21.5

110 19.5

120 17

62
Từ bảng 3.4, ta biểu diễn mối quan hệ đó lên đồ thị, được kết quả như hình 75:

Hình 75: Đồ thị mối quan hệ giữa khoảng cách thực tế và độ cao của ảnh thu được
Nhận xét: Mối quan hệ giữa khoảng cách và độ lớn của ảnh không phải quan hệ tuyến
tính. Từ dạng đồ thị vẽ được, ta dùng phương pháp nội suy để tính khoảng cách trong
từng khoảng cố định.

Bảng 3.5 Công thức nội suy

Chiều cao ảnh (pixel) Khoảng cách (cm)

(19.5 − ℎ) ∗ 10
17.0 ≤ h ≤ 19.5 𝑑 = 110 +
2.5
(21.5 − ℎ) ∗ 10
19.5 ≤ h ≤ 21.5 𝑑 = 100 +
2.0
(23 − ℎ) ∗ 10
21.5 ≤ h ≤ 23.0 𝑑 = 90 +
1.5
(25 − ℎ) ∗ 10
23.0 ≤ h ≤ 25.0 𝑑 = 80 +
2.0
(29 − ℎ) ∗ 10
25.0 ≤ h ≤ 29.0 𝑑 = 70 +
4.0

63
 (32 − ℎ) ∗ 10
29.0 ≤ h ≤ 32.0 𝑑 = 60 +
3.0

Bên cạnh đó, khoảng cách gần nhất mà robot có thể thấy được là 60cm, khoảng cách
xa nhất mà robot còn xác định được là 120cm.

3.4.4.5. Kết quả đo khoảng cách khi dùng camera

Khi đặt xe đứng yên (Tắt động cơ xe, tựa xe vào vị trí cố định để nhìn “vật”)

Khoảng cách thực = 60 cm

Trung bình: 59.78 cm

Phương sai: 1.35 cm2

Khoảng cách thực = 60 cm

Trung bình: 59.72 cm

Phương sai: 2.32 cm2

64
Khoảng cách thực = 90 cm

Trung bình: 90.29 cm

Phương sai: 1.84 cm2

Khoảng cách thực = 90 cm

Trung bình: 90.24 cm

Phương sai: 1.72 cm2

65
Khoảng cách thực = 120cm

Trung bình: 119.95 cm

Phương sai: 7.12 cm2

Khoảng cách thực = 120 cm

Trung bình: 120.72 cm

Phương sai: 13.24 cm2

66
Khi xe tự cân bằng tại vị trí cố định (Bật động cơ xe, cho xe tự đứng yên tại vị trí
cố định)

Lần 1: Khoảng cách thực = 60 cm

Trung bình: 59.403 cm

Phương sai: 7.064 cm2

Lần 2: Khoảng cách thực = 90 cm

Trung bình: 91.905 cm

Phương sai: 10.535 cm2

67
Lần 4: Khoảng cách thực = 60 cm

Trung bình: 61.099 cm

Phương sai: 10.103 cm2

Lần 5: Khoảng cách thực = 90 cm

Trung bình: 91.907 cm

Phương sai: 12.126 cm2

68
Đánh giá, nhận xét:

Quá trình xử lý ảnh rất nhạy cảm với môi trường xung quanh, cụ thể là ánh sáng. Nếu
đặt robot và vật cố định tại 1 vị trí, trong môi trường ánh sáng đèn huỳnh quang, mỗi
lần chạy đo khoảng cách lại ra 1 kết quả khác, có lúc kết quả không dao động quanh
vị trí chính xác nữa. Vấn đề này có thể bắt nguồn từ 2 phía: Một, có thể do môi trường
lúc đó có thay đổi nhỏ mà con người không cảm nhận được, điều này phải chấp nhận
vì đang không làm việc trong môi trường lý tưởng. Hai, có thể bắt nguồn từ cảm biến
ảnh trên camera, có thể mỗi lần khởi chạy, cảm biến camera có 1 thay đổi hay sai số
ngẫu nhiên nào đó. Cần phải có module máy ảnh chuyên nghiệp, có thể can thiệp sâu
vào phần cứng để đưa ra nhận xét.

3.4.4.6. Tốc độ xử lý ảnh

Đây là tốc độ xử lý ảnh khi dùng Raspberry Pi 3b+ khi trải qua các bước tiền xử lý
ảnh trong đề tài:

Hình 76: FPS của quá trình xử lý ảnh

Tốc độ FPS rơi vào khoảng 16 khung hình / s , tức là tốc độ lấy mẫu khoảng 62.5 ms,
lâu gấp 3 lần so vói tốc độ lấy mẫu để điều khiển robot.

69
3.4.4.7. Kết luận về việc sử dụng camera

Như kết quả phép đo và tốc độ FPS đã phân tích ở trên, ta nhận thấy việc dùng phần
cứng để xử lý ảnh như vậy là không được tốt:

- Về đo khoảng cách: Khoảng cách đo được dao động lớn, chưa tính đến sai số
giữa phép đo bằng camera và khoảng cách thực tế; và khoảng cách có thể nhìn
thấy vật ngắn (60 – 120 cm)
- Về vấn đề FPS: Thời gian lấy mẫu lâu gấp 3 lần tốc độ lấy mẫu để điều khiển
robot nên rất khó điều khiển, thêm vào đó là robot có quán tính lớn, nếu điều
khiển không kịp thì robot sẽ di chuyển xa khỏi tầm có thể phát hiện được vật.

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

4.1. Phần điều khiển cân bằng

4.1.1. Điều khiển góc nghiêng của xe

4.1.1.1. Góc nghiêng tại 0°

Hình 77: Đồ thị dao động của góc nghiêng tại 0°

Nhận xét: Dao động góc nghiêng quanh vị trí cân bằng với biên độ 0.5°, thỉnh
thoảng dao động lên tối đa là 1°.

71
4.1.1.2. Góc nghiêng bất kỳ

Khi để góc đặt của xe là 5°, đặt xe trên tấm phẳng nghiêng khoảng 7° so với phương
ngang.

Hình 78: Đồ thị dao động của góc nghiêng tại 5°

Khi đặt góc nghiêng của xe là 10°, đặt xe trên tấm phẳng nghiêng khoảng 12° so với
phương ngang.

Hình 79: Đồ thị dao động của góc nghiêng tại 10°

72
3.2. Điều khiển vị trí của xe

4.1.1.3. Điều khiển vị trí dựa theo giá trị đặt từ người điều khiển

Trường hợp 1: Vị trí đặt = |300mm|

Hình 80: Đồ thị đáp ứng của robot khi vị trí đặt là 300mm

Độ lọt vố POT = 10%

Thời gian đáp ứng T = 1.9s

Sai số xác lập e = 20mm

Khoảng cách thực tế: 290mm

73
Trường hợp 2: Vị trí đặt = |600mm|

Hình 81: Đồ thị đáp ứng của robot khi vị trí đặt là 600mm

Độ lọt vố POT = 50%

Thời gian đáp ứng T = 2.1s

Sai số xác lập e = 35mm

Khoảng cách thực tế: 618mm

74
Trường hợp 3: Vị trí đặt = |-300mm|

Hình 82: Đồ thị đáp ứng của robot khi vị trí đặt là -300mm

Độ lọt vố POT = 25%

Thời gian đáp ứng T = 1.6s

Sai số xác lập e = 15mm

Khoảng cách thực tế: 292mm

75
Trường hợp 4: Vị trí đặt = |-600mm|

Hình 83: Đồ thị đáp ứng của robot khi vị trí đặt là -600mm

Độ lọt vố POT = 53%

Thời gian đáp ứng T = 2.3s

Sai số xác lập e = 50mm

Khoảng cách thực tế: 622mm

76
4.1.1.4. Điều khiển vị trí của xe dựa theo vị trí của đối tượng

Phương thức kiểm tra: Đặt vật (quả bóng cố định), thay đổi khoảng cách đặt từ robot
đến vật.

Lúc đầu khoảng đặt vật cách robot là 80cm, giá trị đặt = 80cm, sau đó ta thay đổi
từng khoảng 20cm

Hình 84: Đáp ứng góc nghiêng của robot khi bám theo vật

Hình 85: Đáp ứng vị trí của robot khi bám theo vật

77
 Hình 86: Đáp ứng góc xoay của robot khi bám theo vật

Nhận xét chung: Vì như đã phân tích ở phần đánh giá chất lượng đo đạc khi sử dụng
camera đơn, kết quả phép đo là không tốt nên dẫn đến chất lượng điều khiển kém. Ta
phải giảm thông số Kp xuống đến mức nhỏ nhất có thể để tránh hiện tượng robot đáp
ứng quá nhanh, cộng thêm việc FPS thấp và kết quả đo không chính xác sẽ làm robot
bị mất kiểm soát.

Sai số phép đo là khoảng 5cm, sai số điều khiển cũng khoảng 3-5cm. Vậy sai số toàn
bộ hệ thống là xấp xỉ 10cm.

78
Chương 5. KẾT LUẬN

5.1. Đánh giá kết quả đạt được

5.1.1. Ưu điểm

- Robot có thể giữ cân bằng ổn định trong thời gian dài.
- Robot có khả năng phát hiện và bám theo vật.

5.1.2. Hạn chế

Về phần giữ cân bằng và điều khiển robot:

- Robot còn dao động khá nhiều, sai số điều khiển còn khá lớn.
- Nguyên nhân: Do mô hình thiết kế bằng tay nên còn tồn tại sai số do thiết kế
(các chi tiết bị lệch, một số linh kiện chọn không phù hợp); Do thiết kế nhiều
bộ PID nối tiếp nhau nên chất lượng điều khiển bị ảnh hưởng qua từng tầng
PID; Do các giá trị offset của cảm biến liên tục thay đổi theo thời gian, giá trị
deadzone của động cơ cũng hay bị thay đổi; Do ảnh hưởng bởi nguồn cung
cấp (Pin) không tạo ra điện áp DC lý tưởng, thường bị hụt điện áp khi động cơ
đột ngột thay đổi; Do động cơ DC bị rơ (trục động cơ bị rung, các khớp bánh
răng trong bộ giảm tốc không khít với nhau nên khi trục động cơ xoay nhẹ,
encoder không quay theo được)
Về phần xử lý ảnh:

- Chất lượng xử lý ảnh phụ thuộc rất nhiều vào môi trường, khi có thay đổi nhỏ
về ánh sáng, vật thể không được phát hiện bao quát sẽ dẫn đến sai số đo lường.
- Ảnh hưởng của phần cứng xử lý ảnh (raspberry pi 3b+) xử lý ảnh rất chậm nên
phải scale ảnh về 320x240, khi đó 1 pixel ảnh chứa trọng số thông tin về
khoảng cách rất lớn (1 pixel khoảng 4cm).

79
5.2. Phương hướng phát triển trong tương lai

Dùng phần cứng xử lý ảnh tốt hơn để có thể xử lý ảnh kích thước lớn, làm sai số
khoảng cách nhỏ đi.

Sử dụng trí tuệ nhân tạo để học vật trong nhiều điều kiện môi trường khác nhau, dẫn
đến giảm bớt sự phụ thuộc vào môi trường.

80
 TÀI LIỆU THAM KHẢO
[1] Nguyễn Gia Minh Thảo, “A PID backstepping controller for two-wheeled self-
balancing robot”, Luận văn thạc sĩ, Đại học Bách Khoa TP.HCM, 2010
[2] 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, 2011.
[3] Nguyễn Minh Tâm, Nguyễn Văn Đông Hải, “Mô hình hóa và điều khiển tối ưu
cho hệ xe hai bánh tự cân bằng”, Tạp chí Khoa học Giáo dục Kỹ thuật số 37, Đại
học Sư phạm Kỹ thuật TPHCM, 2016
[4] Kimberly Tuck. “Using the accelerometer.” Internet:
http://husstechlabs.com/projects/atb1/using-the-accelerometer, Oct.3,2009

81