ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA CƠ KHÍ
BỘ MÔN CƠ ĐIỆN TỬ
BÁO CÁO
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Đề tài:
THIẾT KẾ, CHẾ TẠO VÀ ĐIỀU KHIỂN
ROBOT DÒ LINE (LINE FOLLOWING ROBOT)
GVHD: TS. Ngô Hà Quang Thịnh
Sinh viên thực hiện:
Phan Hữu Thanh Tú
21304661
Lê Tấn Sang
21303338
Huỳnh Văn Ngọc Sơn
21303402
Nguyễn Trọng Trân
21304292
Thành phố Hồ Chí Minh, tháng 5 năm 2017
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
MỤC LỤC
Nội dung Trang
MỤC LỤC ................................................................................................................................. ii
DANH SÁCH HÌNH ẢNH ...................................................................................................... iv
DANH MỤC BẢNG BIỂU ..................................................................................................... vi
MỤC TIÊU THIẾT KẾ ........................................................................................................... 1
CHƯƠNG 1: TỔNG QUAN .................................................................................................... 2
1.1 Sơ đồ nguyên lý ................................................................................................................ 2
1.2 Cảm biến ........................................................................................................................... 5
1.3 Động Cơ ............................................................................................................................ 7
1.4 Cấu trúc điều khiển ........................................................................................................... 7
1.5 Giải thuật điều khiển ......................................................................................................... 8
CHƯƠNG 2: PHƯƠNG ÁN THIẾT KẾ ............................................................................... 9
2.1 Đề xuất sơ đồ nguyên lý ................................................................................................... 9
2.2 Đề xuất cảm biến .............................................................................................................. 9
2.3 Đề xuất cấu trúc điều khiển ............................................................................................ 10
2.4 Đề xuất giải thuật điều khiển .......................................................................................... 10
2.5 Phương án thiết kế .......................................................................................................... 11
CHƯƠNG 3: CƠ KHÍ ............................................................................................................ 12
3.1 Lựa chọn bánh xe ............................................................................................................ 12
3.1.1 Lựa chọn bánh chủ động .......................................................................................... 12
3.1.2 Lựa chọn bánh bị động ............................................................................................. 12
3.2 Tính toán lựa chọn động cơ ............................................................................................ 12
3.3 Kích thước thân xe .......................................................................................................... 14
3.3.1 Chiều cao trọng tâm xe ............................................................................................ 14
3.3.2 Các kích thước cơ sở của xe ..................................................................................... 15
3.4 Hình ảnh mô hình 3D xe ................................................................................................. 17
3.5 Tính toán dung sai độ đồng trục hai động cơ .................................................................. 17
3.5.1 Tính chuỗi kích thước .............................................................................................. 17
3.5.2 Sơ đồ và phương pháp kiểm tra độ đồng trục của 2 động cơ ................................... 21
3.6 Bản vẽ robot xe dò line ................................................................................................... 22
CHƯƠNG 4: MÔ HÌNH HÓA .............................................................................................. 23
4.1 Mô hình động học ........................................................................................................... 23
4.2 Cách xác định vị trí của robot ......................................................................................... 24
ii
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
4.3 Bộ điều khiển tracking, tìm khoảng cách d .................................................................... 25
4.4 Mô phỏng sa bàn ............................................................................................................. 26
CHƯƠNG 5: CẢM BIẾN VÀ LẬP TRÌNH ........................................................................ 33
5.1 Thiết kế cảm biến ............................................................................................................ 33
5.1.1 Chọn cảm biến .......................................................................................................... 33
5.1.2 Tính toán giá trị điện trờ .......................................................................................... 33
5.2 Lựa chọn vi điều khiển ................................................................................................... 34
5.3 Cách bộ trí cảm biến ....................................................................................................... 35
5.4 Chọn khoảng cách hợp lý giữa cảm biến với sàn ........................................................... 36
5.5 Kiểm tra khoảng cách thực tế ......................................................................................... 37
5.6 Tính toán khoảng cách giữa 2 led ................................................................................... 37
5.7 Tính toán số lượng cảm biến ........................................................................................... 39
5.8 Ca-líp cảm biến ............................................................................................................... 39
5.9 Xác định vị trí tâm đường line ........................................................................................ 40
5.10 Mô hình thí nghiệm ....................................................................................................... 41
5.11 Giải thuật điều khiển xe ................................................................................................ 42
5.11.1 Sơ đồ khối của bộ điều khiển ................................................................................. 42
5.11.2 Giải thuật điều khiển .............................................................................................. 42
5.12 Lập trình chương trình .................................................................................................. 47
CHƯƠNG 6: ĐIỆN – MẠCH ĐIỆN ..................................................................................... 62
6.1 Sơ đồ khối hệ thống điện ................................................................................................ 62
6.2 Nguồn điện ...................................................................................................................... 62
6.3 Mạch nguồn .................................................................................................................... 63
6.4 Mạch driver động cơ ....................................................................................................... 64
6.5 Thiết kế board mạch tổng kết nối các thiết bị ................................................................. 67
6.6 Thiết kế mạch cảm biến .................................................................................................. 67
6.7 Giải thuật PID để nâng cao đáp ứng của bộ driver và motor .......................................... 68
6.8 Kết quả thực hiện phần điện cho xe dò line .................................................................... 71
CHƯƠNG 7: THỰC NGHIỆM VÀ ĐÁNH GIÁ ................................................................ 72
TÀI LIỆU THAM KHẢO ...................................................................................................... 74
iii
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
DANH SÁCH HÌNH ẢNH
Hình Trang
Hình 0.1 Sa bàn di chuyển của robot ......................................................................................... 1
Hình 1.1 Sơ đồ nguyên lý RC cars sử dụng trục truyền động ................................................... 2
Hình 1.2 Sơ đồ nguyên lý RC racing cars sử dụng trục truyền động ........................................ 3
Hình 1.3 Sơ đồ nguyên lý hai cặp chủ động vi sai loại 2 bánh ................................................. 4
Hình 1.4 Sơ đồ nguyên lý hai cặp chủ động vi sai loại 4 bánh ................................................. 4
Hình 1.5 Nguyên lý của cảm biến quang.................................................................................... 5
Hình 1.6 Vùng giao thoa của cực phát và cực thu ...................................................................... 5
Hình 1.7 Giải thuật xử lí tín hiệu bằng phương pháp so sánh .................................................... 6
Hình 1.8 Giải thuật xử lý tín hiệu cảm biến bằng phương pháp xấp xỉ ...................................... 6
Hình 1.9 Cấu trúc điều khiển thường dùng ................................................................................ 7
Hình 2.1 Phương án sơ đồ nguyên lý ......................................................................................... 9
Hình 2.2 Phương án cấu trúc điều khiển .................................................................................. 10
Hình 3.1 Bánh dẫn động ........................................................................................................... 12
Hình 3.2 Mô hình toán của bánh xe ......................................................................................... 12
Hình 3.3 Mô hình toán khi xe chuyển hướng ........................................................................... 14
Hình 3.4 Sơ đồ tính khoảng cách giữa hai tâm bánh xe dẫn động .......................................... 15
Hình 3.5 Sơ đồ phân bố linh kiện cho xe ................................................................................. 16
Hình 3.6 Mô hình 3D robot xe dò line ..................................................................................... 17
Hình 3.7 Sai lệch tâm trục 2 động cơ ....................................................................................... 17
Hình 3.8 Các khâu hình thành chuỗi kích thước ...................................................................... 18
Hình 3.9 Các khâu hình thành chuỗi kích thước ...................................................................... 20
Hình 3.10 Sơ đồ kiểm tra độ đồng trục của 2 động cơ ............................................................. 21
Hình 4.1 Mô hình động học của mobile platform .................................................................... 23
Hình 4.2 Mô hình động học được sử dụng cho robot dò line ................................................... 24
Hình 4.3 Cách xác định e3 ........................................................................................................ 25
Hình 4.4 Quan hệ giữa khoảng cách d và sai số lớn nhất ......................................................... 26
Hình 4.5 Mô phỏng chuyển động robot trong đoạn ABC ........................................................ 27
Hình 4.6 Mô phỏng chuyển động robot trong đoạn CDE ........................................................ 28
Hình 4.7 Mô phỏng chuyển động robot trong đoạn EFC ......................................................... 29
Hình 4.8 Mô phỏng chuyển động robot trong đoạn CGA ........................................................ 30
Hình 4.9 Mô phỏng chuyển động robot trong đoạn GAE ........................................................ 31
Hình 4.10 Mô phỏng chuyển động robot trong sa bàn ............................................................. 32
Hình 5.1 Sơ đồ nguyên lý của 1 cặp LED trong mạch cảm biến ............................................. 33
Hình 5.2 Đồ thị thể hiện giữa dòng và áp qua LED ................................................................. 34
iv
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 5.3 Kit Tiva C chip Tm4c123gh6pm ............................................................................... 34
Hình 5.4 Ảnh hưởng của cách đặt cảm biến đến switching distance Xd .................................. 36
Hình 5.5 Vùng hoạt động của cảm biến ................................................................................... 36
Hình 5.6 Tính toán giá trị h ...................................................................................................... 36
Hình 5.7 Test độ cao và đáp ứng của LED ............................................................................... 37
Hình 5.8 Phạm vi quét của led thu và led phát ở 2 cảm biến đặt liền kề nhau ......................... 38
Hình 5.9. Vùng bất định của cảm biến ..................................................................................... 38
Hình 5.10 sơ đồ bố trí sử dụng 5 cảm biến ............................................................................... 39
Hình 5.11 Sơ đồ bố trí sử dụng 7 cảm biến .............................................................................. 39
Hình 5.12 Giải thuật xấp xỉ bậc 2 ............................................................................................. 40
Hình 5.13 Giải thuật xấp xỉ bậc 2 ............................................................................................. 41
Hình 5.14 Đồ thị quan hệ giá trị tính toán và giá trị thực tâm đường line ............................... 41
Hình 5.15 Sơ đồ cấu trúc tập trung ........................................................................................... 42
Hình 5.16 Sơ đồ giải thuật điểu khiển xe ................................................................................. 46
Hình 6.1 Sơ đồ khối hệ thống điện ........................................................................................... 62
Hình 6.2 Module LM2596 ........................................................................................................ 63
Hình 6.3 Mạch in lắp nguồn ..................................................................................................... 64
Hình 6.4 Mạch driver TB6612 ................................................................................................. 64
Hình 6.5 Đồ thị giữa PWM và vận tốc motor (không tải) ........................................................ 65
Hình 6.6 Đồ thị đáp ứng với PWM = 40% ............................................................................... 66
Hình 6.7 Đồ thị đáp ứng với PWM = 70% ............................................................................... 66
Hình 6.8 Sơ đồ nguyên lý và sơ đồ mạch in board tổng........................................................... 67
Hình 6.9 Mô hình hóa 3D board tổng ....................................................................................... 67
Hình 6.10 Sơ đồ nguyên lý mạch cảm biến .............................................................................. 67
Hình 6.11 Bảng vẽ mạch in mạch cảm biến ............................................................................. 68
Hình 6.12 Mô hình hóa 3D mạch cảm biến .............................................................................. 68
Hình 6.13 Đồ thị giữa PWM và vận tốc motor (có tải) ............................................................ 68
Hình 6.14 Mô hình tuyến tính động cơ trái .............................................................................. 69
Hình 6.15 Mô hình tuyến tính động cơ phải ............................................................................. 69
Hình 6.16 Đồ thị đáp của động cơ trái tại PWM = 55% (có tải) .............................................. 69
Hình 6.17 Đồ thị đường cong đáp của hệ bậc nhất .................................................................. 70
Hình 6.18 PID Tuner Matlab .................................................................................................... 70
Hình 6.19 Bản vẽ sơ đồ điện .................................................................................................... 71
Hình 7.1 Mô hình thực tế xe dò line ......................................................................................... 72
Hình 7.2 Kết quả chạy trên sa bàn trích từ video ..................................................................... 72
v
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
DANH MỤC BẢNG BIỂU
Bảng Trang
Bảng 3.1 Các thông số đầu vào của xe ..................................................................................... 13
Bảng 3.2 Các thông số yêu cầu của động cơ ............................................................................ 14
Bảng 3.3 Các thiết bị trên xe .................................................................................................... 17
Bảng 4.1 Thông số đầu vào mô phỏng ..................................................................................... 25
Bảng 5.1 Bảng trạng thái và số xung cần ................................................................................. 45
Bảng 6.1 Công suất điện cần cung cấp cho các thiết bị............................................................ 62
Bảng 6.3 Các ngõ ra/vào module TB6612 ............................................................................... 64
vi
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
MỤC TIÊU THIẾT KẾ
Thiết kế và chế tạo xe dò line di chuyển tốc độ cao trên sa bàn có các đặc điểm :
- Màu sắc đường line: đen.
- Màu nền: trắng.
- Bề rộng đường line: 26mm.
- Bề mặt địa hình di chuyển: phẳng.
- Sa bàn được thể hiện trên Hình 0.1.
- Mỗi robot mang trên người một vật nặng hình hộp chữ nhật có trọng lượng 2 Kg.
Hình 0.1 Sa bàn di chuyển của robot
- Khi bắt đầu, robot được đặt tại vị trí START (điểm A), sau đó robot chạy theo thứ tự
đi qua các điểm nút quy định lần lượt :
(START) A → B → C → D → E → F → C → G → A → C → E (END)
Trang 1
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 1: TỔNG QUAN
Robot dò line là một trường hợp đặc biệt của mobile robot, trong đó robot sẽ nhận biết vị trí tương đối của robot và bám theo đường line (line từ, line màu) đã có. Hiện nay robot dò line được ứng dụng rộng rãi trong các quá trình chế tạo trong công nghiệp, mang vác vật đến các địa điểm đặc biệt,…
Để thiết kế và vận hành một robot dò line, tất cả các yếu tố kỹ thuật cấu thành của robot đều cần được quan tâm: sơ đồ nguyên lý, loại cảm biến, động cơ, cấu trúc điều khiển và giải thuật điều khiển được sử dụng.
1.1 Sơ đồ nguyên lý
Rất nhiều sơ đồ nguyên lý có thể được ứng dụng cho việc chế tạo robot dò line. Để đạt
được tốc độ và khả năng bám đường, sơ đồ nguyên lý của các loại xe đua điều khiển từ xa (RC
racing cars) có thể được sử dụng. Có hai loại sơ đồ nguyên lý chung cho các loại xe đua chuyên
chạy trên mặt đường phẳng:
- Loại 1(Hình 1.1) sử dụng trục truyền động cho trục trước và sau xe. (Khung xe của
hãng Awesomatrix, TAMIYA TT01, Overdose Divall…).
- Loại 2(Hình 1.2) sử dụng đai răng truyền động cho trục trước và sau xe. (Khung xe
của hãng Sakura D3 CS, Serpent VETEQ 02, TA04 EPRO…).
a) Khung xe hãng Overdose Divall b) Sơ đồ nguyên lý sử dụng trục truyền động
Hình 1.1 Sơ đồ nguyên lý RC cars sử dụng trục truyền động
Trang 2
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Khung xe hãng TA04 EPRO b) Sơ đồ nguyên lý sử dụng đai răng truyền động
Hình 1.2 Sơ đồ nguyên lý RC racing cars sử dụng trục truyền động
Những sơ đồ nguyên lý này có chung các ưu điểm và nhược điểm sau:
Ưu điểm: Hạn chế được hiện tượng trượt giữa các bánh khi xe thực hiện đổi hướng.
Nhược điểm:
- Thiết kế cơ khí phức tạp và bán kính cong nhỏ nhất của xe sẽ bị giới hạn bởi kết
cấu của xe.
- Giá thành cao đối với nhu cầu của sinh viên.
- Thích hợp cho việc đua trên mặt đường phẳng.
Bên cạnh các sơ đồ nguyên lý mà các xe RC car sử dụng, hiện nay rất nhiều xe đua dò
line như đội HBFS (Robot RobotChallenge) và Sylvestre (COSMOBOT 2012), Flash Robot
RobotChallenge 2016), Johnny-5 (IGVC),Thunderbolt (Robot Challenge 2014)… sử dụng hai
bánh chủ động được điều khiển độc lập kết hợp với bánh đa hướng (Hình 1.3).
Ưu – nhược điểm chung của sơ đồ nguyên lý này:
Ưu điểm: Bám đường tốt do tiếp xúc 3 điểm mới mặt đường, mô hình động học đơn giản,
dễ hiệu chỉnh sai số hệ thống và cho phép xe di chuyển được theo bán kính rất nhỏ, kể cả việc
quay tại chỗ.Giá thành chế tạo phù hợp với sinh viên.
Nhược điểm: Dễ bị trượt theo phương pháp tuyến khi thực hiện việc bám theo các đoạn
đường bán kính nhỏ ở tốc độ cao.
Trang 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
(a) Mẫu xe đội Sylvestre (b) Sơ đồ nguyên lý loại 2 bánh
Hình 1.3 Sơ đồ nguyên lý hai cặp chủ động vi sai loại 2 bánh
a) Mẫu xe đội CartisX04 (b) Sơ đồ nguyên lý loại 4 bánh
Hình 1.4 Sơ đồ nguyên lý hai cặp chủ động vi sai loại 4 bánh
Ngoài ra, một dạng khác của sơ đồ nguyên lý này cũng được các xe đua như CartisX04
(All Japan Micromouse 2015), Mouse (RobotChallenge 2014)… sử dụng (Hình 1.4). Ở sơ đồ
này, mỗi bánh xe vi sai chủ động được thay bằng một cặp bánh. Điều này tạo ra các ưu – nhược
điểm chung của sơ đồ nguyên lý này:
Ưu điểm: Dễ cân bằng, ưu thế chạy trên đường bằng phẳng
Nhược điểm: Kết cấu cơ khí phức tạp hơn, xuất hiện hiện tượng trượt bánh khi xe đổi
hướng.
Trang 4
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
1.2 Cảm biến
Về cảm biến, phần lớn các robot dò line hiện nay sử dụng các loại cảm biến quang để
nhận biết vị trí tương đối của đường line so với xe, từ đó xử lí để đưa ra tín hiệu điều khiển. Có
hai phương pháp thường được sử dụng cho robot dò line là phương pháp sử dụng camera và
các loại cảm biến quang dẫn:
- Phương pháp sử dụng camera. Hình ảnh đường đua được lấy từ camera, thông qua
xử lí và đưa ra tín hiệu điều khiển.
Ưu điểm: Độ chính xác cao, ít bị nhiễu.
Nhược điểm: Yêu cầu xử lí nhiều, do đó đòi hỏi tốc độ xử lí phải nhanh, nếu không sẽ
làm giảm tốc độ của xe.
- Phương pháp xử dụng cảm biến quang được sử dụng trong hầu hết các loại xe đua
hiện nay. Một số loại cảm biến có thể được sử dụng như quang điện trở (robot ALF trong cuộc
thi ROBOCON Malaysia 2006) hoặc Photo-transistor kết hợp với LED. Hai loại cảm biến này
có nguyên lí hoạt động như nhau (Hình 1.5). Hai led phát và thu phải bố trí khoảng cách với
mặt đường sao cho vùng hoạt động của chúng giao thoa với nhau và không trùng với vùng giao
thoa của bộ liền kề (Hình 1.6).
Hình 1.5 Nguyên lý của cảm biến quang
Hình 1.6 Vùng giao thoa của cực phát và cực thu
Trang 5
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Đối với các loại cảm biến quang, tín hiệu tương tự từ cảm biến sẽ được hiệu chuẩn và xử
lí bằng các giải thuật so sánh hoặc xấp xỉ để tìm ra vị trí tương đối của robot dò line với tâm
đường line.
- Phương pháp thứ nhất: dùng bộ so sánh để xác định trạng thái đóng/ngắt của các
sensor, sau đó suy ra vị trí xe theo một bảng trạng thái đã định sẵn (Hình 1.7). Với phương pháp
này, sai số dò line sẽ phụ thuộc vào thông số của sensor được sử dụng, hay khoảng cách giữa
các sensor. Phương pháp này có đặc điểm phụ thuộc chủ yếu vào mức ngưỡng so sánh của các
sensor, do đó tốc độ xử lý rất nhanh.
- Phương pháp thứ hai: xấp xỉ ra vị trí của xe so với tâm đường line từ các tín hiệu tương
tự từ cảm biến. Có 3 giải thuật xấp xỉ được giới thiệu đó là xấp xỉ theo bậc 2, tuyến tính và theo
trọng số (Hình 1.8) với sai số dò line lần lượt là 5.4mm, 2.8mm và 2.6mm trong thí nghiệm
được thực hiện . Đặc điểm của phương pháp này là phụ thuộc chủ yếu vào thời gian đọc ADC
tất cả các sensor của vi điều khiển, do đó thời gian xử lý sẽ lâu hơn phương pháp 1. Tuy nhiên
độ phân giải cao hơn đáng kể so với phương án đầu.
Hình 1.7 Giải thuật xử lí tín hiệu bằng phương pháp so sánh
a) Xấp xỉ bậc 2; (b) Xấp xỉ theo trọng số
Hình 1.8 Giải thuật xử lý tín hiệu cảm biến bằng phương pháp xấp xỉ
Trang 6
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
1.3 Động Cơ
Về động cơ, các xe đua dò line như Pika, HFBS-2, CartisX04, Thunderstorm, Impact…
đều sử dụng động cơ DC có gắn encoder làm cơ cấu chấp hành. Đặc điểm của động cơ DC đa
dạng về kích thước, momen, chủng loại driver, dễ dàng lắp đặt và điều khiển chính xác do có
thể kết hợp thêm encoder và được ứng dụng thêm bộ điều khiển PID để có thể điều chỉnh tốc
độ hoặc vị trí chính xác theo yêu cầu.
1.4 Cấu trúc điều khiển
Về cấu trúc điều khiển, robot dò line có các module chính bao gồm module sensor, module
điều khiển và module điều khiển động cơ. Trong đó có hai phương pháp chủ yếu để kết nối các
module đó với nhau là phương pháp điều khiển tập trung và phân cấp:
- Trong phương pháp điều khiển tâp trung (Hình 1.9a), một MCU nhận tín hiệu từ cảm
biến, xử lí dữ liệu rồi truyền tín hiệu điều khiển cho cơ cấu tác động.Đây là cấu trúc được sử
dụng khá nhiều trong các xe đua dò line thực tế như xe CartisX04, Le’Mua (Robot Challenge
2015), Pika. Cấu trúc điều khiển tập trung có đặc điểm phần cứng đơn giản, tuy nhiên MCU
phải xử lý tất cả thông tin trước khi cập nhật thông tin mới.
Hình 1.9 Cấu trúc điều khiển thường dùng
Trang 7
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
- Trong phương pháp điều khiển phân cấp (Hình 1.9b) nhiều hơn một MCU sẽ được sử
dụng trong hệ thống. Bên cạnh MCU master đảm nhiệm việc tính toán tổng thể, một số robot
còn có thêm 1 Slave MCU chuyên xử lí tín hiệu encoder hoặc 1 slave MCU để xử lí tín hiệu từ
sensor (RobotALF). Ngoài ra, các robot dò line dùng camera thường có một MCU slave chuyên
xử lí hình ảnh, rồi chuyển dữ liệu về MCU master. Cấu trúc này giúp giảm nhẹ khối lượng tính
toán cho master và cho phép robot thực hiện nhiều tác vụ cùng lúc. Cấu trúc điều khiển phân
cấp có đặc điểm phần cứng phức tạp hơn, phải quan tâm đến vấn đề giao tiếp giữa các MCU,
tuy nhiên có khả năng xử lý nhiều tác vụ cùng lúc, giúp cho thời gian lấy mẫu của hệ thống
nhanh hơn khi sử dụng cấu trúc tập trung.
1.5 Giải thuật điều khiển
Giải thuật điều khiển được dùng phổ biến cho các xe đua dò line là bộ điều khiển PD,
PID, FIC cho hệ thống lái của xe kết hợp với PID cho từng động cơ như xe Bolt, Pika, Major
(Robocomp 2014), Thunderstorm… Ngoài ra, một bộ điều khiển phổ biến khác thường được
ứng dụng cho mobile robot là bộ điều khiển tracking. Thực nghiệm từ cho thấy bộ điều khiển
này có thể giúp sai số bám line của robot trên đoạn đường thẳng và cong; đạt sai số tối đa
150mm khi gặp các đoạn line gấp khúc và tối đa 250mm khi robot thực hiện đổi hướng 900.
Về vận tốc tối đa, vận tốc cực đại trung bình của của các robot như Pika, HBFS-2,
Sylvestre, Thunderbolt, Thunderstorm, Impact… tại các cuộc thi đều đạt từ 1-3m/s.
Về sai số tối đa của robot trong suốt quá trình, sai số trong quá trình xe di chuyển trên
đường thẳng hay cong sẽ phụ thuộc vào sai số xác định vị trí của xe do hệ thống sensor (tối đa
5.4mm) và sai số do bộ điều khiển. Đối với sai số khi xe bám theo các vị trí đổi hướng đột ngột,
sai số phụ thuộc phần lớn vào giải thuật điều khiển.
Từ các thông số của các xe đã thực hiện và yêu cầu của đề bài mỗi robot mang vật nặng
2 Kg, lựa chọn các thông số:
- Tốc độ tối đa: vmax = 1 m/s
- Bán kính cong tối thiểu: Rmin = 500mm
Dựa trên kết quả mô phỏng và thực nghiệm của các đồ án đi trước lựa chọn sai số dò
line trên đoạn đường thẳng và cong: emax = ±15 mm.
Trang 8
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 2: PHƯƠNG ÁN THIẾT KẾ
Chương này, ta đưa ra đề xuất và lựa chọn phương án thiết kế phù hợp với mục tiêu thiết
kế đã đặt ra.Đề xuất gồm có: sơ đồ nguyên lý, cảm biến, cấu trúc điều khiển, giãi thuật điều
khiển.
2.1 Đề xuất sơ đồ nguyên lý
Do robot chỉ cần bám theo đường cong bán kính lớn (R = 500mm), khả năng đổi hướng
độ ngột tại các vị trí line gãy khúc đồng thời kết cấu xe phải đơn giản, giá thành chế tạo phù
hợp. Vì thế sơ đồ nguyên lý loại 2 bánh sử dụng bánh đa hướng được đề xuất (Hình 2.1).
Hình 2.1 Phương án sơ đồ nguyên lý
2.2 Đề xuất cảm biến
Từ yêu cầu đề bài về sai số bám line tối đa của robot (±15 mm) và khả năng giúp xe có
thể bám line ở các đoạn đường gấp khúc đột ngột, các phương án sau về loại cảm biến và giải
thuật xử lý sẽ được cân nhắc.
Về loại cảm biến:
Để thỏa mãn yêu cầu về việc đáp ứng được với các đường gãy khúc đột ngột của sa bàn,
phương án cảm biến phải có độ nhạy thích hợp. Dựa trên đặc tính độ nhạy cao của
phototransistor so với các loại cảm biến quang khác hai phương án sử dụng loại sensor này
được đề xuất:
- Phototransistor kết hợp với LED thường
- Phototransistor kết hợp với LED hồng ngoại
Đối với đường line màu có độ tương phản cao vì thế LED hồng ngoại cho độ nhạy cao
hơn nhưng cần phải che chắn để chống nhiễu. Đối với đường đua mà màu line với màu của nền
có độ tương phản thấp, sử dụng LED thường sẽ hiệu quả hơn.
Về giải thuật xử lí tín hiệu:
Trang 9
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Với sai số yêu cầu ±15mm như đầu bài, các phương pháp có độ phân cao nên được lựa
chọn. Có hai giải thuật xử lý được đề xuất:
- So sánh
- Xấp xỉ
Với phương pháp so sánh, vị trí của robot so với đường line chỉ có thể thuộc vào một số
trường hợp đã được quy định sẵn. Số trường hợp này phụ thuộc số lượng cảm biến, sai số ảnh
hưởng bởi khoảng cách tối thiểu giữa các cảm biến. Khoảng cách giữa các cảm biến này phụ
thuộc nhiều vào góc chiếu của LED, góc thu của sensor và độ cao so với mặt đất.
Với phương pháp xấp xỉ, sai số phụ thuộc vào số lượng cảm biến và cách chọn độ cao
của chúng so với mặt đất. Tuy nhiên, độ phân giải của phương pháp này cao hơn đáng kể so
với phương pháp so sánh, giúp cho hệ thống sensor có thể đạt được sai số tốt hơn. Tuy nhiên,
thời gian đáp ứng của phương pháp này sẽ lâu hơn phương án trên do vi điều khiển cần thực
hiện chuyển đổi ADC cho tất cả các cảm biến.
2.3 Đề xuất cấu trúc điều khiển
Để tránh tình trạng bị nhiễu hoặc rớt dữ liệu trong quá trình truyền dữ liệu phương án cấu
trúc điều khiển tập trung (Hình 2.2).
Hình 2.2 Phương án cấu trúc điều khiển
Phương án này sử dụng một MCU cho các hoạt động của robot do đó MCU phải thực
hiện nhiều tác vụ hơn, do đó việc tính toán thời gian để đảm bảo các tác vụ diên ra được thống
nhất và không bị trùng lặp là vấn đề quan trọng, cần tính toán kỹ.
2.4 Đề xuất giải thuật điều khiển
Dựa vào yêu cầu robot phải bám được trên các đoạn đường thẳng, cong và sai số vị trí
line bị gấp khúc (2mm), hai phương án cho bộ điều khiển được đề xuất:
- Phương án 1: Bộ điều khiển PD kết hợp ghi nhớ đường đi
- Phương án 2: Bộ điều khiển tracking
Trang 10
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Phương án 1 có đặc điểm giúp robot có khả năng cải thiện được khả năng bám đường line
sau mỗi lần chạy. Tuy nhiên giải thuật tự học phức tạp và cần phải kết hợp thêm cảm biến
accelometer và gyrometer để bộ điều khiển có thể nhớ được trạng thái gia tốc và gốc của robot
trong suốt đường đua.
Phương án 2 là một bộ điều khiển thông dụng trong các nghiên cứu về khả năng bám theo
quỹ đạo cho trước của mobile robot. Lý thuyết và thực nghiệm đã chứng minh bộ điều khiển
có khả năng di chuyển robot tới các tọa độ cho trước với vận tốc mong muốn.
2.5 Phương án thiết kế
Từ các đề xuất trên, ta tiến hành lựa chọn phương án phù hợp:
- Sơ đồ nguyên lý: robot 2 bánh chủ động vi sai có bánh đa hướng bị động (Hình 2.1)
- Cảm biến: bộ LED hồng ngoại-Phototransistor và sử dụng giải thuật xấp xỉ để tìm ra
vị trí của robot so với đường line.
- Động cơ: động cơ DC servo.
- Cấu trúc điều khiển: bộ điều khiển tập trung (Hình 2.2)
- Giải thuật điều khiển: bộ điều khiển tracking.
Trang 11
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 3: CƠ KHÍ
3.1 Lựa chọn bánh xe
3.1.1 Lựa chọn bánh chủ động
Từ giá trị vận tốc lớn nhất ở đầu bài là 1 (m/s), cũng như tham khảo những thiết kế của các đội thi các cuộc thi mobile robot trên thế giới thì ta chọn đường kính bánh xe dẫn động là 80 mm.
Bánh xe được chọn để làm mô hình là V2 80mm khớp lục giác 12mm.
Hình 3.1 Bánh dẫn động
3.1.2 Lựa chọn bánh bị động
Hai loại bánh bị động thường được sử dụng cho mobile robot là bánh mắt trâu và bánh
caster. Tuy nhiên đặc điểm của bánh caster là khoảng cách giữa trục quay và trục bánh gây ra
hiện tượng shopping-cart làm ảnh hưởng đến phương trình động học của xe. Để tránh hiện
tượng này và tận dụng được đặc điểm của sơ đồ nguyên lý, bánh mắt trâu được lựa chọn làm
bánh bị động.
3.2 Tính toán lựa chọn động cơ
Để xe chuyển động, động cơ có vai trò cung cấp moment cho các bánh. Quá trình chuyển
động này chịu ảnh hưởng đáng kể của khối lượng xe và ma sát giữa bánh xe và mặt đường. Mô
hình toán cho một bánh xe được thể hiện ở Hình 3.2
Hình 3.2 Mô hình toán của bánh xe
Trang 12
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Moment bánh xe có thể tính gần đúng:
(3.1) 𝐼 = 𝑚𝑅2 1 2
Cân bằng moment quanh tâm bánh xe, ta có:
(3.2) 𝜏 − 𝐹𝑚𝑠𝑅 = 𝐼𝛾
𝜏 = 𝐼𝛾 + 𝐹𝑚𝑠𝑅
Điều kiện để bánh xe không bị trượt khi động cơ quay, monent 𝜏 phải thỏa điều kiện sau :
𝜏 ≤ 𝐼𝛾 + 𝜇 ( 𝑀 + 𝑚) 𝑔𝑅 1 2
𝜏 ≤ 𝑚𝑅2𝛾 + 𝜇 ( 𝑀 + 𝑚) 𝑔𝑅 1 2 1 2
Phương trình định luật 2 Newton theo phương ngang :
(3.3)
𝐹𝑚𝑠 = 2𝐹𝑚𝑠 = (2𝑚 + 𝑀)𝑎 (2𝑚 + 𝑀)𝑎 2
Thay 𝐹𝑚𝑠 vào phương trình moment ở trên ta được:
𝜏 = 𝛾 + 𝑚𝑅2 2 (2𝑚 + 𝑀)𝑎𝑅 2
Công suất mỗi động cơ cần cung cấp:
(3.4) 𝑃 = 𝜏𝜔
Trong đó:
(kg.m2): moment quán tính của bánh xe. (N.m): moment.
(kg): khối lượng của bánh xe. (m/s2): gia tốc dài mong muốn.
(kg): khối lượng thân xe. (m/s2): gia tốc trọng trường.
(m): bán kính bánh xe. (W): công suất mỗi động cơ.
(rad/s): vận tốc góc.
𝐹𝑚𝑠 (N): lực ma sát. (rad/s2): gia tốc góc.
Ta dựa vào các thông số đầu vào có sẵn cũng như ước lượng, để có thể tính toán được các
thông số cần thiết của động cơ để thỏa mãn được các yêu cầu đặt ra.
Bảng 3.1 Các thông số đầu vào của xe
Vận tốc lớn nhất của xe 1 (m/s)
Thời gian tăng tốc mong muốn 1 (s)
Bán kính bánh xe 40 (mm)
Khối lượng bánh xe 0,025 (kg)
Khối lượng tải ước lượng 2,5 (kg)
Trang 13
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hệ số ma sát 0,8
Hệ số an toàn 2
Bảng 3.2 Các thông số yêu cầu của động cơ
Vận tốc lớn nhất 238,73 vg/ph
Moment cần thiết để xe chuyển động 41,02 Nmm
Moment trượt 321,79 Nmm
Công suất mỗi động cơ 2,05 (w)
Với thông số động cơ đã được tính như trên, động cơ GA25 V1 (𝜔 = 320 vòng/phút, 𝑃
= 13,2W) được lựa chọn.
Tính toán lại vận tốc:
V = (320/60).0,08.π = 1,34 (m/s)
Với động cơ lựa chọn vẫn thỏa mãn vận tốc đề bài đặt ra.
3.3 Kích thước thân xe
3.3.1 Chiều cao trọng tâm xe
Hình 3.3 Mô hình toán khi xe chuyển hướng
Trong đó:
𝑇: trọng tâm xe. 𝐹𝑙𝑡: lực li tâm.
𝑃: trọng lực. 𝐹𝑚𝑠: lực ma sát.
𝐶: tâm quay khi xe lật. ℎ: chiều cao trọng tâm xe.
Để tránh lật, moment sinh ra do trọng lực quanh tâm quay C phải lớn hơn moment của lực li tâm:
(3.5) ≤ 0 𝐹𝑙𝑡ℎ − 𝑃 𝑏 2
Trang 14
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
ℎ − 𝑚𝑔 ≤ 0 𝑚𝑣2 𝑅 𝑏 2
ℎ ≤ 𝑔𝑏𝑅 2𝑣2
Trong đó:
Gia tốc trọng trường 𝑔 = 9,81m/s2
Bề rộng xe 𝑏 = 0,17𝑚
Bán kính cong của đường đua 𝑅 = 0,5𝑚
Vận tốc dài tối đa 𝑣 = 1𝑚/𝑠
Do đó: ℎ ≤ 41,69𝑐𝑚
3.3.2 Các kích thước cơ sở của xe
Kích thước bao của xe phụ thuộc vào kích thước và việc sắp xếp các linh kiện điện, điện
tử, khoảng cách được mô phỏng giữa sensor và các bánh chủ động. Ngoài ra tỉ lệ kích thước
dài-rộng của xe nên được chọn theo tỉ số √5 nhằm giảm thiểu tối đa ảnh hưởng của các yếu tố
động lực học lên xe.
Ta tính toán khoảng cách giữa 2 bánh chủ động: Đặt bài toán xe đang chạy qua khúc
cong có bán kính R = 500mm, với vận tốc 1 m/s. Tìm khoảng cách nhỏ nhất giữa 2 bánh xe chủ
động để xe không bị lật.
𝐹𝑙𝑡⃗⃗⃗⃗
𝑃⃗
Hình 3.4 Sơ đồ tính khoảng cách giữa hai tâm bánh xe dẫn động
Từ việc ước lượng được các kích thước của các thành phần trên xe, Ước lượng được chiều
cao xe là 80 mm. Ước lượng điểm G (trọng tâm xe) nằm ở vị trí cách mặt đất h = 30 mm.
b: là khoảng các giữa tâm 2 bánh xe dẫn động.
Trang 15
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
𝐹𝑙𝑡⃗⃗⃗⃗ là lực ly tâm khi xe qua khúc cong.
Ta có:
= = 5(𝑁) 𝐹𝑙𝑡 = 𝑚𝑣2 𝑅 2,5. 12 0,5
Để xe không lật thì
(3.6)
𝑏 2
↔ 𝑏 ≥ = 30𝑚𝑚 𝐹𝑙𝑡. ℎ ≤ 𝑃. 2. 𝐹𝑙𝑡. ℎ 𝑃
Kết hợp với các kích thước phân bố linh kiện ta chọn b = 170 mm và chiều dài cơ sở của xe là 190 mm.
Sơ đồ phân bố các phần tử trên xe
Hình 3.5 Sơ đồ phân bố linh kiện cho xe
Trang 16
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Bảng 3.3 Các thiết bị trên xe
Tên thiết bị
STT 1 2 3 4 5 6 Bánh xe dẫn động Động cơ escap Board mạch tích hợp Mạch 7 cảm biến Hộp pin Bánh đa hướng Số lượng 2 2 1 1 1 1 3.4 Hình ảnh mô hình 3D xe
Hình 3.6 Mô hình 3D robot xe dò line
3.5 Tính toán dung sai độ đồng trục hai động cơ
3.5.1 Tính chuỗi kích thước
Dung sai độ đồng tâm của trục 2 động cơ: 𝑒 ≤ 0,5𝑚𝑚
Hình 3.7 Sai lệch tâm trục 2 động cơ
Trang 17
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Do đó:
𝑒2 = Δ𝑥2 + Δ𝑦2 ≤ 0,025
Giả sử Δ𝑥 = Δ𝑦 ⇒ Δ𝑥 = Δ𝑦 ≤ 0,112𝑚𝑚
Xét trên mặt phẳng chứa Δ𝑦 và song song với mặt phẳng cắt dọc trục động cơ, ta có:
a)
b) Hình 3.8 Các khâu hình thành chuỗi kích thước
Chuỗi trên Hình 3.8a với khâu khép kín 𝐴Σ = 𝐴3. Từ đó ta có được chuỗi kích thước
Hình 3.8b
Với:
𝐴1: là khâu thành phần giảm
𝐴2: là khâu thành phần tăng
𝐴Σ: là khâu khép kín
Theo điều kiện khép kín, ta có quan hệ kích thước:
𝐴2 − 𝐴1 = 𝐴Σ
Vì Δ𝑦 ≤ 0,112 và 2 chi tiết đồ gá có thể đổi lẫn cho nhau nên dung sai khau khép kín 𝐴Σ
từ −0,056 đến +0,056. Từ đó ta cần phải xác định lại dung sai của các khâu thành phần 𝐴𝑖
Giả sử các khâu thành phần có cùng một cấp chính xác, ta có:
𝑎𝑚 = ∑ 𝑖𝑖 𝐴Σ 𝑚+𝑛 𝑖=1
Dựa vào bảng 9.1[27] ta tìm được 𝑖𝑖 = 1,08
Trang 18
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
≈ 51 ⇒ 𝑎𝑚 = 112 2 × 1,08
Dựa vào bảng 4.1[27] ta chọn cấp chính xác 9 làm cấp chính xác chung cho các khâu thành phần
Sai lệch giới hạn và dung sai khâu 𝐴2
𝐴2 = 14𝐻9 = 14+0,043
Tính khâu 𝐴1 (khâu giảm):
Khâu tăng:
𝐴2 = 14𝐻9 = 14+0,043
𝐸𝑆 = +0,043
𝐸𝐼 = 0
Khâu để tính:
𝐴1 = 𝐴𝑘
𝑚
𝑇1 = 𝑇Σ − 𝑇2 = 112 − 43 = 69(𝑚𝑚)
𝑖=1
𝑛−1 − ∑ 𝑒𝑚𝑗 𝑗=1
𝑒𝑚1 = ∑ 𝐸𝑚𝑖 − 𝐸𝑚Σ
= − 0 − 0 = 21.5(𝑚𝑚) 43 2
= 21,5 + = 56(𝑚𝑚) 𝑒𝑠1 = 𝑒𝑚1 + 𝑇1 2 69 2
= 21,5 − = −13(𝑚𝑚) 𝑒𝑖1 = 𝑒𝑚1 − 𝑇1 2 69 2
+0,056(𝑚𝑚) và 𝐴2 = 14+0,043(𝑚𝑚)
Do đó
𝐴1 = 14+0,013
Ta thấy miền dung sai của khâu 𝐴2 và 𝐴1 sai khác không quá lớn và 2 chi tiết gá động cơ
có thể thay thế lẫn nhau. Do đó ta chọn dung sai của 2 khâu 𝐴1 và 𝐴2 là 14+0,04
Xét trên mặt phẳng chứa Δ𝑥 và vuông góc với mặt cắt dọc trục động cơ, ta có:
Trang 19
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a)
b)
Hình 3.9 Các khâu hình thành chuỗi kích thước
Chuỗi trên Hình 3.9b với khâu khép kín 𝐴Σ = 𝐴3. Từ đó ta có được chuỗi kích thước Hình 3.9b
Với:
𝐴1: là khâu thành phần giảm
𝐴2: là khâu thành phần tăng
𝐴Σ: là khâu khép kín
Theo điều kiện khép kín, ta có quan hệ kích thước:
𝐴2 − 𝐴1 = 𝐴Σ
Vì Δ𝑥 ≤ 0,112 và 2 chi tiết đồ gá có thể đổi lẫn cho nhau nên dung sai khau khép kín 𝐴Σ
Trang 20
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
từ −0,056 đến +0,056. Từ đó ta cần phải xác định lại dung sai của các khâu thành phần 𝐴𝑖
Kích thước danh nghĩa của khâu khép kín:
𝐷𝐴Σ = 𝐷𝐴2 − 𝐷𝐴1 = 33 − 21 = 12
Giả sử các khâu thành phần có cùng một cấp chính xác, ta có:
𝑎𝑚 = ∑ 𝑖𝑖 𝐴Σ 𝑚+𝑛 𝑖=1
Tra bảng 9.1[27] ta có 𝑖1 = 1,31; 𝑖2 = 1,56
≈ 39 ⇒ 𝑎𝑚 = 112 1.31 + 1.56
Dựa vào bảng 4.1[27] ta chọn cấp chính xác 9 làm cấp chính xác chung cho các khâu thành phần
Sai lệch giới hạn và cung sai khâu 𝐴2
𝐴2 = 33𝐻9 = 33+0,062
Khâu 𝐴1:
𝑚
𝑇1 = 𝑇Σ − 𝑇2 = 112 − 62 = 50(𝑚𝑚)
𝑖=1
𝑛−1 − ∑ 𝑒𝑚𝑗 𝑗=1
𝑒𝑚1 = ∑ 𝐸𝑚𝑖 − 𝐸𝑚Σ
= − 0 − 0 = 31(𝑚𝑚) 62 2
= 31 + = 56(𝑚𝑚) 𝑒𝑠1 = 𝑒𝑚1 + 𝑇1 2 50 2
= 31 − = 6(𝑚𝑚) 𝑒𝑖1 = 𝑒𝑚1 − 𝑇1 2 50 2
+0,056(𝑚𝑚) và 𝐴2 = 33+0,062(𝑚𝑚)
Do đó
𝐴1 = 21+0,006
3.5.2 Sơ đồ và phương pháp kiểm tra độ đồng trục của 2 động cơ
Hình 3.10 Sơ đồ kiểm tra độ đồng trục của 2 động cơ
Trang 21
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Phương pháp kiểm tra: Ta lồng hai trục chuẩn A và B vào hai lỗ, đầu đo rà liên tục trên một tiết diện vuông góc với trục B. Ta sẽ đọc được khoảng cách lớn nhất và nhỏ nhất.
Ta có công thức tính độ sai lệch như sau:
∆đ𝑡= 𝑥𝑚𝑎𝑥 − 𝑥𝑚𝑖𝑛 2
Để đảm bảo chính xác cần đo trên 2 tiết diện xa nhất có thể nhờ dịch chuyển tì chuẩn bằng vòng hãm C.
3.6 Bản vẽ robot xe dò line
(Đính kèm thuyết minh)
Trang 22
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 4: MÔ HÌNH HÓA
4.1 Mô hình động học
Để thực hiện việc điều khiển cho xe bám line tốt hơn, ta tiến hành thiết lập mô hình động
học của hệ thống.Mô hình này bao gồm các điểm quan trọng: Điểm R: điểm tham chiếu cho
robot; Điểm M: trung điểm của hai bánh chủ động; Điểm C: Điểm tracking của robot. (Hình
4.1).
Hình 4.1 Mô hình động học của mobile platform
Phương trình động học tại điểm M
(4.1)
Trong đó v và là vận tốc dài và vận tốc góc của xe.
Phương trình động học tại điểm C
(4.2)
Với d là khoảng cách từ M đến C.
Phương trình động học tại R
Trang 23
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
(4.3)
Trong đó vR là vận tốc mong muốn của xe đua tại điểm tham chiếu.
4.2 Cách xác định vị trí của robot
Sai số của xe so với điểm tham chiếu được xác định như sau:
(4.4)
Sai số động lực học được xác định như sau:
(4.5)
Do hệ thống phototransistor chỉ xác định được sai số theo phương pháp tuyến với phương
chuyển động của xe. Do đó mô hình động học của xe cần được giới thiệu lại với điểm C là tâm
của dãy sensor, M là trung điểm hai bánh chủ động và là điểm tracking của xe. Giả sử xe sẽ
chạy với vận tốc bằng vận tốc tham chiếu nên ta có e1 = 0. Mô hình này được thể hiện trên Hình
4.2. Như vậy, để xác định được đầy đủ thông tin về vị trí của điểm tracking so với tham chiếu,
sai số e2 và e3 cần được xác định.
xs
Hình 4.2 Mô hình động học được sử dụng cho robot dò line
Trang 24
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 4.3 Cách xác định e3
Trên thực tế, e2 được xác định trực tiếp từ hệ thống sensor. Đối với e3, phương án xác
định được đề xuất là cho robot di chuyển theo phương trước đó một đoạn ds đủ nhỏ để khi nối
2 điểm RR’ tạo thành tiếp tuyến của đường cong (Hình 4.3). Khi đó, sai số e3 được xác định
theo công thức.
(4.6) ) 𝑒3 = arctan ( 𝑒2 − 𝑒2′ 𝑑𝑠
4.3 Bộ điều khiển tracking, tìm khoảng cách d
Sau khi có được các sai số e1, e2, e3, bộ điều khiển tracking cho phép xác định giá trị v và
cần thiết để điểm tracking có thể bám theo điểm tham chiếu:
(4.7)
Tiến hành mô phỏng quá trình bám sa bàn của robot, ta sử dụng bảng thông số đầu vào:
Bảng 4.1 Thông số đầu vào mô phỏng
Đại lượng
Bán kính cong Vận tốc cần đạt Khoảng cách 2 bánh xe Đường kính bánh xe Thời gian lấy mẫu Thời di chuyển đoạn nhỏ (tìm e2’) Giá trị Đơn vị 500 1 170 80 0.02 0.002 mm m/s mm mm s s
Trang 25
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Với sự lựa chọn ban đầu các hệ số [k1 k2 k3] lần lượt là [1 500 1] từ đồ thị Hình
4.4 ta chọn khoảng cách d là 96 mm
Hình 4.4 Quan hệ giữa khoảng cách d và sai số lớn nhất
4.4 Mô phỏng sa bàn
Với khoảng cách từ tâm cảm biến C đến tâm 2 bánh chủ động M được chọn là 96
mm. Bên cạnh đó, giá trị e2 phụ thuộc vào bộ số [k1 k2 k3] được chọn. Tiến hành mô
phỏng trên đoạn đường đua với các hệ số [k1 k2 k3] lần lượt là [1 520 0.8]
Các thông số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận
tốc xe và số vòng quay của bánh xe trong quá trình chuyển động đoạn ABC được mô tả
trong Hình 4.5.
Trong đoạn ABC, sai số lớn e2 lớn nhất là khúc giao tại B, tại đây có sự chuyển
hướng đột ngột dẫn đến sai số lớn, tuy nhiên sai số trong đoạn này là 14,26 mm nhỏ hơn
sai số yêu cầu của đề bài. Trên đoạn AB và BC sai số tương đối ổn định (Hình 4.5 b).
Khi vào cua, vận tốc xe cần giảm xuống 0,66 m/s (Hình 4.5 c). Nhận thấy trong quá
trình chạy, tốc độ góc 2 bánh xe không vượt quá giới hạn cho phép của động cơ.
Quy ước: đường vẽ màu xanh lá thể hiện vận tốc gốc bánh xe phải, đường màu đỏ thể hiện vận tốc gốc bánh xe trái.
Trang 26
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Sa bàn di chuyển đoạn ABC b) Sai số trong quá trình di chuyển
c) Vận tốc robot đoạn ABC d) Tốc độ 2 bánh đoạn ABC
Các thông số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận
tốc xe và số vòng quay của bánh xe trong quá trình chuyển động đoạn CDE được mô tả
trong Hình 4.6
Tương tự đoạn ABC, trên đoạn CDE điểm có sai số lớn nhất là tại giao điểm D –
nơi chuyển giao giữa đường thẳng và đường cong, sai số e2 lớn nhất đạt gần 9 mm. Trên
đoạn thẳng và đoạn cong, sai số e2 tương đối ổn định (Hình 4.6 b)
Để vào cua mượt, tốc độ 2 bánh cần đảm bảo như Hình 4.6 d.
Sự thay đổi vận tốc liên tục trong đoạn CDE (Hình 4.6 c) giúp cho quá trình di
chuyển của xe đảm bảo được sự ổn định về sai số, tránh hiện tượng trượt xe ra khỏi
đường line.
Hình 4.5 Mô phỏng chuyển động robot trong đoạn ABC
Trang 27
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Sa bàn di chuyển đoạn CDE b) Sai số trong quá trình di chuyển
c) Vận tốc robot đoạn CDE d) Tốc độ 2 bánh đoạn CDE
Các thông số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận
tốc xe và số vòng quay của bánh xe trong quá trình chuyển động đoạn EFC được mô tả
trong Hình 4.7
Trong đoạn EFC, sai số lớn e2 lớn nhất là khúc giao tại F với 10 mm, tại đây có
sự chuyển hướng đột ngột dẫn đến sai số lớn. Trên đoạn EF và FC sai số tương đối ổn
định (Hình 4.7 b). Trong quá trình di chuyển, vận tốc thường xuyên thay đổi ( Hình 4.7
c) do sai số e3 (Hình 4.7 b) cũng hay thay đổi, để có thể đáp ứng cho sai số này, vận tốc
cũng cần thay đổi để tránh hiện tượng trượt, làm sai số càng lớn hơn.
Hình 4.6 Mô phỏng chuyển động robot trong đoạn CDE
Trang 28
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Sa bàn di chuyển đoạn EFC b) Sai số trong quá trình di chuyển
c) Vận tốc robot đoạn EFC d) Tốc độ 2 bánh đoạn EFC
Các thông số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận
tốc xe và số vòng quay của bánh xe trong quá trình chuyển động đoạn CGA được mô tả
trong Hình 4.8
Trong đoạn CGA, sai số lớn e2 lớn nhất là khúc giao tại G với giá trị -14,66 mm,
tại đây có sự chuyển hướng đột ngột dẫn đến sai số lớn. Trên đoạn CG và GA sai số
tương đối ổn định (Hình 4.8 b).
Trong quá trình di chuyển, vận tốc thường xuyên thay đổi (Hình 4.8 c) do sai số
e3 (Hình 4.8 b) thay đổi, để đáp ứng cho sai số này, vận tốc thay đổi để tránh hiện tượng
trượt, làm sai số càng lớn hơn.
.
Hình 4.7 Mô phỏng chuyển động robot trong đoạn EFC
Trang 29
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Sa bàn di chuyển đoạn CGA b) Sai số trong quá trình di chuyển
c) Vận tốc robot đoạn CGA d) Tốc độ 2 bánh đoạn CGA
Các thông số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận
tốc xe và số vòng quay của bánh xe trong quá trình chuyển động đoạn GAE được mô tả
trong Hình 4.9
Trong đoạn GAE, có khúc cua 90o làm cho sai số e2 tại vị trí này khá lớn 30 mm.
Tuy nhiên sai số này là có thể chấp nhận trong quá trình chạy.
Trong quá trình di chuyển, vận tốc xe thay đổi (Hình 4.9 c) và tại vị trí bẻ cua 90o
vận tốc xe giảm nhiều nhất, đây cũng là điều cần lưu ý khi áp dụng giải thuật cho mô
hình xe thật.
Hình 4.8 Mô phỏng chuyển động robot trong đoạn CGA
Trang 30
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Sa bàn di chuyển đoạn GAE b) Sai số trong quá trình di chuyển
c) Vận tốc robot đoạn GAE d) Tốc độ 2 bánh đoạn GAE
Dựa trên các hệ số đã lựa chọn và kiểm nghiệm trong từng đoạn, Hình 4.10 mô
phỏng toàn bộ sa bàn di chuyển của robot.
Trong quá trình di chuyển, sai số e2 thỏa mãn yêu cầu đề bài trên đoạn đường
thẳng sai số dao động từ 0,9 đến -8,6 mm và đường cong với sai số nhỏ hơn 15 mm,
riêng tại khúc cua 90o sai số e2 là lớn nhất với giá trị 21,14 mm.
Tốc độ hai bánh nhỏ hơn 320 vòng trên phút và thỏa mãn đáp ứng của động cơ đã
lựa chọn ban đầu.
Hình 4.9 Mô phỏng chuyển động robot trong đoạn GAE
Trang 31
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) Sa bàn di chuyển xe dò line b) Sai số trong quá trình di chuyển
c) Vận tốc robot d) Tốc độ 2 bánh
Hình 4.10 Mô phỏng chuyển động robot trong sa bàn
Trang 32
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 5: CẢM BIẾN VÀ LẬP TRÌNH
5.1 Thiết kế cảm biến
5.1.1 Chọn cảm biến
Dựa vào chương phương án thiết kế, ta nhận thấy đường line được sử dụng với 2 màu
trắng và đen. Nên ta sử dụng phototransistor kết hợp với LED hồng ngoại để nhận biết bên cạnh
đó ta đảm bản việc che chắn tốt đến tránh ảnh hưởng của nhiễu. Thông qua tìm hiểu thông tin,
ta nhận thấy trên thị trường hiện tại đã có nhiều sản phẩm kết hợp phototransistor với LED hồng
ngoài với giá thành tương đối phù hợp. Vì thế ta chọn sản phẩm TCRT5000 với giá 5000 VNĐ/1
Cặp LED.
Sơ đồ nguyên lý ta sử dụng cho 1 cặp LED trong mạch cảm biến.
Hình 5.1 Sơ đồ nguyên lý của 1 cặp LED trong mạch cảm biến
5.1.2 Tính toán giá trị điện trờ
Với sơ đồ nguyên lý trên, ta kết hợp với các giá trị được cho trong datasheet của TCRT5000:
{ 𝑉𝐹 = 1.25𝑉 𝐼𝐹 ≤ 60𝑚𝐴 Ta có:
= 𝑅1 = → 𝑅1 ≥ 5 − 1.25 60. 10−3 = 62.5Ω 𝑉𝑐𝑐 − 𝑉𝐹 𝐼𝐹 5 − 𝑉𝐹 𝐼𝐹
Ta chọn: 𝑅1 = 330 Ω Xác định 𝑅2. Từ đồ thị Hình 5.2 ta có:
≈ 17𝑚𝐴 𝐼𝐹 = 5 − 1.25 220
= 𝑅2 = Từ Hình 5.2 a), ta được 𝐼𝐶 = 1.2 mA. Và từ đồ thị hình 2 b) ta được 𝑉𝐶𝐸 = 0.6V 5 − 0.6 1,2. 10−3 = 3666,67Ω 𝑉𝑐𝑐 − 𝑉𝐶𝐸 𝐼𝐶
Chọn 𝑅2 = 4k7 Ω
Trang 33
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
a) b)
Hình 5.2 Đồ thị thể hiện giữa dòng và áp qua LED
a) Mối quan hệ giữa IF và IC b) Mối quan hệ giữa điện áp vào và dòng IC qua LED thu
5.2 Lựa chọn vi điều khiển
Yêu cầu đặt ra cho việc lựa chọn vi điều khiển để đảm bảo robot hoạt động tốt cần thỏa
mãn các yêu cầu đặt ra như sau:
Cần có ít nhất 7 kênh chuyển đổi ADC tương tứng với 7 cặp led hồng ngoại thu –
phát.
Cần có ít nhất 3 bộ timer/counter để thực việc tính toán vị trí xe cũng như đọc
encoder trả về từ động cơ.
Cần có ít nhất 2 khối tạo xung PWM
Cần có thời gian đáp ứng thỏa mãn yêu cầu của đã bài toán đặt ra.
Sau quá trình tìm hiểu các vi điều khiển, người thực hiện đã chọn kit tiva C với chip
Tm4c123gh6pm.
Hình 5.3 Kit Tiva C chip Tm4c123gh6pm
Trang 34
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Các đặc điểm của kit:
Xung clock tối đa 80MHz.
Có 16 khối tạo xung PWM
Có 12 kênh chuyển đổi ADC
5 bộ timer/counter 16/32 bit
6 chân ngắt ngoài đọc encoder
Tính toán thời gian đáp ứng của vi điều khiển. Từ các đặc điểm kit tiva C ta có các tính
toán sau:
Thời gian chuyển đổi ADC.
+ Chu kì lệnh vi điều khiển
2 𝑇 = 80. 106 = 2,5. 10−8𝑠
=> Thời gian lấy mẫu 𝑡𝑠= 250 ns
+ Thời gian chuyển đổi 𝑡𝑐 = 1𝜇𝑠
tADC = 1.25 𝜇𝑠 << 0.02s vì thế vi điều khiển đáp ứng được yêu cầu thời gian
lấy mẫu 0.02s.
5.3 Cách bộ trí cảm biến
Có 2 cách đă ̣t cảm biến trên mảng các cảm biến : đă ̣t do ̣c (position 2 Hình 5.4) và đă ̣t
ngang (position 1 Hình 5.4).
Khi đi từ nền trắng vào nền đen, cảm biến phải di chuyển một khoảng Xd thì giá trị analog
của nó mới thật sự xác định. Ta chọn cách bố trí cảm biến sao cho giá trị Xd nhỏ để đảm bảm
cảm biến ổn định và sẽ cho kết quả chính xác hơn
Dựa vào đồ thị Hình 5.4 ta sẽ chọn cảm biến bố trí nằm ngang (position 1)
Trang 35
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 5.4 Ảnh hưởng của cách đặt cảm biến đến switching distance Xd
5.4 Chọn khoảng cách hợp lý giữa cảm biến với sàn
Khoảng cách cảm biến so với mặt đường cần đảm bảo thu được tín hiệu tại các vị trí nền
trắng là như nhau.
Xuất hiện vùng giao thao giữa 2 cực phát và cực thu
Phạm vi hoạt động của cảm biến được mô hình lại như sau:
Hình 5.5 Vùng hoạt động của cảm biến
Hình 5.6 Tính toán giá trị h
Dựa vào các tính toán hình học cơ bản, để xuất hiện vùng giao thoa thì h > 8,57mm.
Cảm biến có độ phân giải tốt nhất trong khoảng 2-10cm.
Trang 36
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Qua phân tích và tham khảo thí nghiệm của các nhóm trước, chọn h = 10mm.
5.5 Kiểm tra khoảng cách thực tế
Mục đích: Nhằm kiểm tra lại phạm vi thu phát của một cặp led so với đường line. Từ đó, ta
có thể xác định được khoảng cách tối đa giữa led so với mặt đường.
Quá trình test led được thực hiện như Hình 5.7
Kết quả test:
- Khi ta che chắn nền trắng thì tín hiệu ADC trả về giảm, khi ta che chắn nền đèn thì
tín hiệu ADC trả về tăng.
- Khoảng cách ta nhận được trong quá trình kiểm tra phạm vi ảnh hưởng và hoạt
động tốt của led từ 8 đến 12 mm.
Hình 5.7 Test độ cao và đáp ứng của LED
5.6 Tính toán khoảng cách giữa 2 led
Yêu cầu:
- Phạm vi hoạt động của 2 led không chồng lên nhau
- Tối thiểu sai số
Để cảm biến hoạt động tốt thì các led phải tách biệt nhau, các led không được giao thoa
với nhau, vì sẽ gây nên sai số khi hoạt động. Phạm vi quét của 2 led liền kề được mô tả lại như
sau
Trang 37
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 5.8 Phạm vi quét của led thu và led phát ở 2 cảm biến đặt liền kề nhau
Khoảng cách giữa 2 led phát vầ thu liền kề phải đảm bảo:
l ≥ ℎ ∗ tan 16 + ℎ ∗ 𝑡𝑎𝑛30 = 8.64mm
Khoảng cách giữa 2 led trong 1 cảm biến là 5 mm. Do đó khoảng cách tối thiểu giữa 2
cảm biến sẽ là:
d = l+5 ≥ 13.64mm
Ngoài ra khi hoạt động sẽ có trường hợp cảm biến nằm trong vùng bất định thì giá trị
analog của cảm biến đưa về sẽ như nhau. Do đó sẽ không xác định được chính xác vị trí cảm
biến so với tâm đường line. Vùng bất định của cảm biến được mô tả lại như sau:
Hình 5.9. Vùng bất định của cảm biến
Trên Hình 5.8 ta thấy khi cảm biến dịch chuyển sang phải một đoạn 26-d thì luôn có 2 led
nằm trong đường line, và do đó tín hiệu analog đo được sẽ như nhau, tương tự khi cảm biến di
chuyển sang trái đoạn 2d-26 thì chỉ có 1 led nằm trong đường line. Ta phải chọn giá trị của d
sao cho các khoảng cách này là nhỏ nhất.
𝑒 = min (max (2𝑑 − 26,26 − 𝑑))
⟹ 𝑑 = 17𝑚𝑚
Trang 38
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
⟹ 𝑒 = 9 𝑚𝑚
Vậy ta chọn d = 17 mm.
5.7 Tính toán số lượng cảm biến
Ta nhận thấy khi sử dụng giải thuật xấp xỉ bậc 2 thì cần ít 3 cặp cảm biến để nhận dạng
được tâm đường line. Vì thế giả sử khi tâm đường line trùng với tâm nội suy từ cảm biến thì cần
3 cặp cảm biến, đồng thời khi xe lệch về bên trái/phải ta cần thêm 1 cặp cảm biến ở mỗi bên để
nhận dạng được phía lệch của xe. Từ lập luận trên ta nhận thấy Hình 5.10 với việc sử dụng 5
cảm biến ta hoàn toàn có thể xác định được phía lệch của xe. Nhưng khi gặp các khúc cua thì
việc sử dụng 5 cặp cảm biến sẽ dẫn đến việc ôm cua của xe trở nên khó khăn hơn do xe không
nhận được tín hiệu cần thiết. Từ đó, ta tăng thêm mỗi bên của mạch cảm biến một cặp LED nên
mạch cảm biến của ta sẽ sử dụng 7 cặp LED như Hình 5.11.
Hình 5.10 sơ đồ bố trí sử dụng 5 cảm biến
Hình 5.11 Sơ đồ bố trí sử dụng 7 cảm biến
5.8 Ca-líp cảm biến
Tín hiê ̣u analog cho mỗi cảm biến là khác nhau, khi làm viê ̣c trong cù ng điều kiê ̣n như
nhau sẽ cho ra giá tri ̣ khác nhau. Do đó ta phải tiến hành calip cảm biến
Công thức calib
𝑦𝑗0 = 𝑦𝑚𝑖𝑛 + (𝑥𝑗,𝑖 − 𝑥𝑚𝑖𝑛,𝑖) 𝑦𝑚𝑎𝑥 − 𝑦𝑚𝑖𝑛 𝑥𝑚𝑎𝑥,𝑖 − 𝑥𝑚𝑖𝑛,𝑖
Với:
𝑥𝑚𝑖𝑛,𝑖 và 𝑥𝑚𝑎𝑥,𝑖 là giá trị nhỏ nhất và lớn nhất từ cảm biến thứ 𝑖
𝑦𝑚𝑎𝑥 và 𝑦𝑚𝑖𝑛 là giá trị lớn nhất và nhỏ nhất mà ta mong muốn cho tất cả cảm biến
𝑥𝑗,𝑖 là giá trị đọc được lần thứ j của cảm biến thứ 𝑖
𝑦𝑗0 giá trị sau khi điều chỉnh 𝑥𝑗,𝑖
Trang 39
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Từ giá trị thực nghiệm ta thu được công thức calip cho 7 cảm biến
n
𝑥𝑚𝑎𝑥 x 2698 2834 3264 3261 2506 3412 2814 𝑦𝑗0 𝑦𝑗0 = 184 + 0779(𝑥𝑗,0– 184) 𝑦𝑗0 = 184 + 0.818(𝑥𝑗,1 – 194) 𝑦𝑗0 = 184 + 0.948(𝑥𝑗,2 – 204) 𝑦𝑗0 = 184 + 0.942(𝑥𝑗,3 – 220) 𝑦𝑗0 = 184 + 0.713(𝑥𝑗,4 – 204) 𝑦𝑗0 = 184 + 0.995(𝑥𝑗,5 – 202 𝑦𝑗0 = 184 + 0.806(𝑥𝑗,6 – 212)
𝑥𝑚𝑖𝑛 Cảm biến 184 𝑥0 194 𝑥1 204 𝑥2 220 𝑥3 204 𝑥4 202 𝑥5 𝑥6 212 Chọn 𝑦𝑚𝑖𝑛= 184, 𝑦𝑚𝑎𝑥 = 3412
5.9 Xác định vị trí tâm đường line
Sử dụng giải thuật xấp xỉ bậc 2
Hình 5.12 Giải thuật xấp xỉ bậc 2
2 + 𝑏𝑥1 + 𝑐
Lấy 3 cảm biến cho giá trị cao nhất. Ta xấp xỉ tín hiệu đầu ra
𝑦1 = 𝑎𝑥1
𝑦2 = 𝑎(𝑥1 + 1)2 + 𝑏(𝑥1 + 1) + 𝑐
𝑦3 = 𝑎(𝑥1 + 2)2 + 𝑏(𝑥1 + 1) + 𝑐
Vị trí tâm đường line sẽ là vị trí mà hàm xấp xỉ đạt giá trị cực đại
𝑥 = −𝑏 2𝑎
𝑎 = 𝑦1 + 𝑦2 − 2𝑦3 2
𝑏 = 𝑦2 − 𝑦2 − 2𝑎𝑥1 − 1
Trang 40
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
5.10 Mô hình thí nghiệm
Ta tiến hành kiểm tra khả năng đáp ứng của LED với các nhận định đã nên ra bên trên. Sử dụng mô hình bên dưới. Kết quả mối quan hệ giữa giá trị tính toán và giá trị thực của vị trí tâm đường line so với
tâm cảm biến được thể hiện ở hình bên dưới (Hình 5.14). Với giá trị sai số e2max là 1,7 mm, sai
số e2min là 0,8 mm, sai số trung bình tính toán so với sai số lý tưởng là 1,2 mm. Từ kết quả trả
về cũng như sai số từ Hình 5.13, ta thu được đường tuyến tính xấp xỉ như sau: y = 0,51x - 0,31.
Với đường xấp xỉ này, thì sai số e2 thỏa mãn yêu cầu đề bài đặt ra.
)
m m
(
y
f o
e u l a v
y r o e h T
Hình 5.13 Giải thuật xấp xỉ bậc 2
Position x (mm)
Hình 5.14 Đồ thị quan hệ giá trị tính toán và giá trị thực tâm đường line
Trang 41
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
5.11 Giải thuật điều khiển xe
Ở phần này trình bày sơ đồ khối của bộ điều khiển, lưu đồ giải thuật, giải thuật nhận biết vị trí xe trên sa bàn.
5.11.1 Sơ đồ khối của bộ điều khiển
Robot sử dụng giải thuật điều khiển tập trung để điều khiển các chức năng riêng biệt.
Trong đó vi điều khiển (MCU) Tiva Tm4c123 đảm nhiệm tất cả nhiệm vụ gồm có:
- Đọc và xử lí tín hiệu sensor.
- Điều khiển 2 động cơ thông qua driver và encoder.
Sau khi tín hiệu trả về từ sensor dưới dạng số sẽ được vi điều khiển xử lý đưa ra sai số e2
và e3, kế đến sẽ áp dụng tiêu chuẩn ổn định lyaponouv để đưa ra vận tốc động cơ trái và động
cơ phải. Vận tốc động cơ trái và phải sẽ được vi điều khiển (MCU) xử lý và truyền tín hiệu điều
khiển dưới dạng xung pwm cho driver tương ứng. Vi điều khiển (MCU) nhận xung encoder trả
về và áp dụng giải thuật PID để đảm bảo vận tốc xe đáp ứng được giá trị đã được tính toán.
Hình 5.15 Sơ đồ cấu trúc tập trung
5.11.2 Giải thuật điều khiển
Theo sa bàn đề bài Hình 0.1, trong quá trình xe di chuyển sẽ có các trạng thái sau:
- Trạng thái 1: trên đoạn đường AB
- Trạng thái 2: trên đoạn đường BC
- Trạng thái 3: trên đoạn đường CD
- Trạng thái 4: trên đoạn đường DF
- Trạng thái 5: trên đoạn đường FC
Trang 42
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
- Trạng thái 6: trên đoạn đường CG
- Trạng thái 7: trên đoạn đường GA
- Trạng thái 8: trên đoạn đường AC
- Trạng thái 9: trên đoạn đường CE
Đối với mỗi trạng thái cần có các tác vụ xử lý khác nhau.
Tác vụ 1:
- Trạng thái áp dụng: trạng thái 1, 2, 3, 4, 5, 6.
- Nhiệm vụ: giúp xe bám theo đường line.
- Giải thuật: do đã biết trước quỹ đạo đường line nên ta sử dụng luật điều khiển sau:
𝑣 = 𝑣𝑅 cos 𝑒3 + 𝑘1𝑒1 { 𝜔 = 𝑘2𝑣𝑅𝑒2 + 𝑘3𝑠𝑖𝑛𝑒3
Trong đó:
v là vận tốc dài của xe.
ω là vận tốc góc của xe.
vR là vận tốc dài mong muốn của xe.
ωR là vận tốc góc mong muốn của xe.
e1, e2, e3 là các sai số về vị trí và góc của Tracking Point.
k1, k2, k3 là các hệ số dương.
Trên đoạn đường thẳng ta có:
𝑣 = 1 (𝑚 𝑠⁄ ) { 𝜔 = 0 (𝑟𝑎𝑑 𝑠⁄ )
Trên đoạn đường cong ta có:
𝑣 = 1 (𝑚 𝑠⁄ ) { 𝜔 = 2,4 (𝑟𝑎𝑑 𝑠⁄ )
Bộ điều khiển này thỏa mãn điều kiện ổn định theo tiêu chuẩn Lyapunov.
Ta có mối liên hệ sau:
𝑣 = (5.1) { 𝜔 = 𝑣𝑟 + 𝑣𝑙 2 𝑣𝑟 − 𝑣𝑙 𝐿
Trong đó:
- lần lượt là vận tốc dài bánh trái và bánh phải.
- L là khoảng cách giữa tâm 2 bánh.
Từ đó ta có:
Trang 43
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
𝑣𝑙 = (5.2) {
𝑣𝑅 = 2𝑣 − 𝜔𝐿 2 2𝑣 + 𝜔𝐿 2
Tác vụ 2:
- Áp dụng cho việc chuyển động giữa các trạng thái: 2 →3, 5 →6, 8 →9
- Nhiệm vụ: giúp xe đi thẳng qua chỗ giao C.
- Giải thuật: giữ vận tốc bánh trái và bánh phải cố định bằng nhau cho đến khi dãy cảm biến
vượt ra ngoài vùng line giao nhau tại C:
𝑣𝑙 = 1 (𝑚 𝑠⁄ ) { 𝑣𝑟 = 1 (𝑚 𝑠⁄ )
Tác vụ 3:
- Trạng thái áp dụng: khi trạng thái 5 giao trạng thái 9 tại E lần 1.
- Nhiệm vụ: giúp xe đi qua chỗ giao E.
- Giải thuật: giữ vận tốc bánh trái và bánh phải cố định cho đến khi dãy cảm biến vượt ra
ngoài vùng line giao nhau tại E, giá trị vận tốc bánh trái và bánh phải xác định như sau:
= = 0,85(𝑚 𝑠⁄ ) 𝑣𝑙 = {
= = 1,15(𝑚 𝑠⁄ ) 𝑣𝑟 = 2𝑣 − 𝜔𝐿 2 2𝑣 + 𝜔𝐿 2 2.1 − 2.0,15 2 2.1 + 2.0,15 2
Tác vụ 4:
- Áp dụng cho việc chuyển động giữa các trạng thái: 7 →8
- Nhiệm vụ: giúp xe bẻ cua tại A.
- Giải thuật: giữ vận tốc bánh trái bằng 0, giữ vận tốc bánh phải cố định cho đến khi dãy cảm
biến trở lại đường line, giá trị vận tốc bánh phải xác định như sau:
𝑣𝑙 = 0(𝑚 𝑠⁄ ) { 𝑣𝑟 = 1(𝑚 𝑠⁄ )
Tác vụ 5:
- Áp dụng khi kết thúc trạng thái 9.
- Nhiệm vụ: dừng xe.
- Giải thuật: giữ vận tốc bánh trái và bánh phải bằng 0.
Cách nhận biết chỗ các đường line giao nhau:
Sử dụng 1 biến đếm để điểm số xung counter từ encoder trả về từ bánh trái của xe (nhìn
từ sau xe - 3456 xung ứng với 1 vòng). để nhận biết được vị trí của xe hiện tại trên đường đua
cũng như khi xe hoàn thành các trang thái.
Ta có công thức như sau:
Trang 44
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
𝑠 = 𝜋𝑑 = 𝜋 ∗ 80 = 251,3 𝑚𝑚
Trong đó:
- S : là quãng đường xe đi được khi bánh trái quay được 1 vòng.
- D : là đường kính bánh xe trái.
Ta có quãng đường đi lí thuyết của bánh trái xe khi xe bám line tốt:
Từ tính toán lý thuyết kết hợp thực tế ta đưa ra được giá trị encoder trả về ứng với các
trạng thái như sau:
Bảng 5.1 Bảng trạng thái và số xung cần
Trạng thái
1 – 2 tại B 2 – 3 tại C Vượt qua C lần 1 3 – 4 tại D 4– 5 tại F 5 – 6 tại C Vượt qua C lần 2 6 –7 tại G 7 – 8 tại A Bẻ cua tại A 8 – 9 tại C Vượt qua C lần 3 Chạy đến E Dừng lại Số xung cần 1200 2800 3650 4200 7600 8200 8800 10000 11400 12300 13700 14500 16400 16500
Trang 45
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 5.16 Sơ đồ giải thuật điểu khiển xe
Trang 46
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
5.12 Lập trình chương trình
/***************************************************************************
* Tool : CCS 6.1
* Library : TivaWare v1.0
* Chip : TM4C123
* History : 18-3-2017
* Author : Huynh Van Ngoc Son - BKNI
*
***************************************************************************/
#include
#include
#include
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_timer.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/adc.h"
#include "driverlib/interrupt.h"
#include "driverlib/pwm.h"
#include "driverlib/timer.h"
#include "driverlib/uart.h"
#include "inc/tm4c123gh6pm.h"
#include "driverlib/rom.h"
uint32_t period = 2000;
volatile uint32_t counterA = 0;
volatile uint32_t counterB = 0;
volatile uint32_t counterA_1 = 0;
uint32_t velocitytrai = 0;
uint32_t velocityphai = 0;
Trang 47
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
float kp_dc_trai = 0.1721;
float ki_dc_trai = 4.523;
float ki_dc_set_trai = 4.523;
float kd_dc_trai = 0.00024;
float error_trai = 0.0;
float error_1_trai = 0.0;
float error_2_trai = 0.0;
float u_k_trai = 0.0;
float u_k_1_trai = 0.0;
float kp_dc_phai = 0.1532;
float ki_dc_phai = 4.321;
float ki_dc_set_phai = 4.321;
float kd_dc_phai = 0.00001;
float error_phai = 0.0;
float error_1_phai = 0.0;
float error_2_phai = 0.0;
float u_k_phai = 0.0;
float u_k_1_phai = 0.0;
float e2 = 0.0;
float e2_1 = 0.0;
float e3 = 0.0;
float x,x1,a,b;
float ds = 0.0;
void ConfigInitial()
{
SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
Trang 48
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
GPIOPinTypeGPIOInput(GPIO_PORTB_BASE, GPIO_PIN_0|GPIO_PIN_1);
GPIOIntTypeSet(GPIO_PORTB_BASE, GPIO_PIN_0|GPIO_PIN_1,
GPIO_RISING_EDGE);
GPIOPinTypeGPIOInput(GPIO_PORTA_BASE, GPIO_PIN_2|GPIO_PIN_3);
GPIOIntTypeSet(GPIO_PORTA_BASE, GPIO_PIN_2|GPIO_PIN_3,
GPIO_RISING_EDGE);
GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE,
GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7);
GPIOPinTypeADC(GPIO_PORTE_BASE,
GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5); // CB1-4
GPIOPinTypeADC(GPIO_PORTD_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3); //
CB5-7//
}
void ConFigPWM()
{
SysCtlPWMClockSet(SYSCTL_PWMDIV_64);
GPIOPinConfigure(GPIO_PB4_M0PWM2);
GPIOPinConfigure(GPIO_PB6_M0PWM0);
GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_6|GPIO_PIN_4);
PWMGenConfigure(PWM0_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN |
PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, period);
PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN |
PWM_GEN_MODE_NO_SYNC);
Trang 49
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, period);
PWMGenEnable(PWM0_BASE, PWM_GEN_0);
PWMGenEnable(PWM0_BASE, PWM_GEN_1);
PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
PWMOutputState(PWM0_BASE, PWM_OUT_2_BIT, true);
}
void ConfigADC()
{
ADCSequenceConfigure(ADC0_BASE,0,ADC_TRIGGER_PROCESSOR,0);
ADCSequenceStepConfigure(ADC0_BASE,0,0,ADC_CTL_CH0);
ADCSequenceStepConfigure(ADC0_BASE,0,1,ADC_CTL_CH1);
ADCSequenceStepConfigure(ADC0_BASE,0,2,ADC_CTL_CH2);
ADCSequenceStepConfigure(ADC0_BASE,0,3,ADC_CTL_CH3);
ADCSequenceStepConfigure(ADC0_BASE,0,4,ADC_CTL_CH4);
ADCSequenceStepConfigure(ADC0_BASE,0,5,ADC_CTL_CH5);
ADCSequenceStepConfigure(ADC0_BASE,0,6,ADC_CTL_CH6);
ADCSequenceStepConfigure(ADC0_BASE,0,7,ADC_CTL_CH9 | ADC_CTL_IE |
ADC_CTL_END);
ADCSequenceEnable(ADC0_BASE, 0);
}
void ConFigTimer0()
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
TimerConfigure(TIMER0_BASE,TIMER_CFG_PERIODIC);
TimerLoadSet(TIMER0_BASE, TIMER_A,400000 - 1);
IntMasterEnable();
IntEnable(INT_TIMER0A);
TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
TimerEnable(TIMER0_BASE, TIMER_A);
}
void ConFigTimer1()
{
Trang 50
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
TimerConfigure(TIMER1_BASE,TIMER_CFG_PERIODIC);
TimerLoadSet(TIMER1_BASE, TIMER_A,80000 - 1);
IntMasterEnable();
IntEnable(INT_TIMER1A);
TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
TimerEnable(TIMER1_BASE, TIMER_A);
}
void ConFigCounter1()
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
GPIOPinTypeTimer(GPIO_PORTD_BASE, GPIO_PIN_6);
GPIOPinConfigure(GPIO_PD6_WT5CCP0);
GPIOPinTypeTimer(GPIO_PORTD_BASE, GPIO_PIN_7);
GPIOPinConfigure(GPIO_PD7_WT5CCP1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_WTIMER5);
TimerConfigure(WTIMER5_BASE, TIMER_CFG_SPLIT_PAIR |
TIMER_CFG_B_CAP_COUNT_UP|TIMER_CFG_A_CAP_COUNT_UP;
TimerControlEvent(WTIMER5_BASE, TIMER_BOTH, TIMER_EVENT_POS_EDGE);
TimerIntEnable(WTIMER5_BASE, TIMER_CAPB_EVENT);
TimerIntEnable(WTIMER5_BASE, TIMER_CAPA_EVENT);
TimerEnable(WTIMER5_BASE, TIMER_BOTH);
TimerSynchronize(WTIMER5_BASE,TIMER_2B_SYNC);
TimerSynchronize(WTIMER5_BASE,TIMER_2A_SYNC);
}
void ConFigCounter()
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_1);
GPIOPinConfigure(GPIO_PB1_T2CCP1);
Trang 51
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_0);
GPIOPinConfigure(GPIO_PB0_T2CCP0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);
TimerConfigure(TIMER2_BASE, TIMER_CFG_SPLIT_PAIR |
TIMER_CFG_B_CAP_COUNT_UP|TIMER_CFG_A_CAP_COUNT_UP);
TimerControlEvent(TIMER2_BASE, TIMER_BOTH, TIMER_EVENT_POS_EDGE);
TimerIntEnable(TIMER2_BASE, TIMER_CAPB_EVENT);
TimerEnable(TIMER2_BASE, TIMER_BOTH);
TimerSynchronize(TIMER2_BASE,TIMER_2B_SYNC);
}
void ConfigUart(void)
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 9600,
(UART_CONFIG_WLEN_8 |
UART_CONFIG_STOP_ONE |
UART_CONFIG_PAR_NONE));
}
void
UARTSend(uint8_t *pui8Buffer, uint32_t ui32Count)
{
while(ui32Count--)
{
UARTCharPutNonBlocking(UART0_BASE, *pui8Buffer++);
}
}
Trang 52
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
void Timer0IntHandler(void)
{
velocitytrai = 6000*counterA/374 ;
velocityphai = 6000*counterB/374 ;
uint8_t buf[5];
buf[0] = velocitytrai / 100 + 0x30;
buf[1] = (velocitytrai % 100) / 10 + 0x30;
buf[2] = velocitytrai % 10 + 0x30;
buf[3] = '\r';
buf[4] = '\n';
UARTSend(buf, 5);
HWREG(TIMER2_BASE+TIMER_O_TAV) = 0;
HWREG(TIMER2_BASE+TIMER_O_TBV) = 0;
TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
}
void Timer1IntHandler(void)
{
e2 = x - 51;
e3 = atan((e2 - e2_1)/ds);
e2_1 = e2;
TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
}
int PIDtrai(float set_point,float velocity)
{
error_trai = set_point - velocity;
u_k_trai = u_k_1_trai + error_trai*(kp_dc_trai + kd_dc_trai/0.01 + ki_dc_trai*0.01/2)
+ error_1_trai*(ki_dc_trai*0.01/2 - kp_dc_trai - 2*kd_dc_trai/0.01) +
(kd_dc_trai/0.01)*error_2_trai;
Trang 53
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
error_2_trai = error_1_trai;
error_1_trai = error_trai;
u_k_1_trai = u_k_trai;
if (u_k_trai > 95)
{
u_k_trai = 95;
ki_dc_trai = 0;
}
else if (u_k_trai <= 15)
{
u_k_trai = 15;
ki_dc_trai = 0;
}
else ki_dc_trai = ki_dc_set_trai;
return u_k_trai;
}
int PIDphai(float set_point,float velocity)
{
error_phai = set_point - velocity;
u_k_phai = u_k_1_phai + error_phai*(kp_dc_phai + kd_dc_phai/0.01 +
ki_dc_phai*0.01/2)
+ error_1_phai*(ki_dc_phai*0.01/2 - kp_dc_phai - 2*kd_dc_phai/0.01)
+ (kd_dc_phai/0.01)*error_2_phai;
error_2_phai = error_1_phai;
error_1_phai = error_phai;
u_k_1_phai = u_k_phai;
if (u_k_phai > 95)
{
u_k_phai = 95;
ki_dc_phai = 0;
}
Trang 54
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
else if (u_k_phai <= 15)
{
u_k_phai = 15;
ki_dc_phai = 0;
}
else ki_dc_phai = ki_dc_set_phai;
return u_k_phai;
}
float main(void)
{
uint32_t ui32ADC0Value[8];
int8_t i,j;
int16_t chiso[3];
int8_t signal = 0;
float calib[7],y[3],max;
float l = 0.15;
float velocity1 = 0.0;
float velocity2 = 0.0;
float vr = 0.0;
float wr = 0.0;
float duty1_1 =0.0;
float duty2_1 = 0.0;
float v = 0.0,w = 0.0;
float k2 = 500;
float k3 = 1;
uint8_t giaolo = 0;
volatile uint16_t ADCresult[7];
ConfigInitial();
ConfigADC();
ConFigPWM();
Trang 55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
ConFigTimer0();
ConFigTimer1();
ConFigCounter1();
ConFigCounter();
ConfigUart();
while(1)
{
GPIOPinWrite(GPIO_PORTC_BASE,
GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7,0x50);
counterA = TimerValueGet(TIMER2_BASE, TIMER_B);
counterA_1 = TimerValueGet(WTIMER5_BASE, TIMER_A);
counterB = TimerValueGet(TIMER2_BASE, TIMER_A);
ADCIntClear(ADC0_BASE, 0);
ADCProcessorTrigger(ADC0_BASE, 0);
while (!ADCIntStatus(ADC0_BASE,0, false))
{
}
ADCSequenceDataGet(ADC0_BASE,0 , ui32ADC0Value);
ADCresult[0] = ui32ADC0Value[2];
ADCresult[1] = ui32ADC0Value[1];
ADCresult[2] = ui32ADC0Value[0];
ADCresult[3] = ui32ADC0Value[7];
ADCresult[4] = ui32ADC0Value[6];//
ADCresult[5] = ui32ADC0Value[5];
ADCresult[6] = ui32ADC0Value[4];
calib[0] = 184 + 0.779*(ADCresult[0]-184) ;
calib[1] = 184 + 0.818*(ADCresult[1]-194) ;
calib[2] = 184 + 0.948*(ADCresult[2]-204) ;
calib[3] = 184 + 0.942*(ADCresult[3]-220) ;
calib[4] = 184 + 0.713*(ADCresult[4]-204) ;
calib[5] = 184 + 0.995*(ADCresult[5]-202) ;
calib[6] = 184 + 0.806*(ADCresult[6]-212) ;
Trang 56
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
max = calib[0];
for(i = 0 ;i <= 6;i++)
{
if (calib[i]>= max)
{
max = calib[i];
j = i; // 0 -> 6
}
}
if (j == 0)
{
y[0] = max;
y[1] = calib[j+1];
y[2] = calib[j+2];
chiso[0] = j;
chiso[1] = j+1;
chiso[2] = j+2;
a = (y[0]+y[1]-2*y[2])/2;
b = y[2] - y[1] -2*a*chiso[0]- a;
}
else if (j == 6)
{
y[0] = calib[j-2];
y[1] = calib[j-1];
y[2] = max;
chiso[0] = j-2;
chiso[1] = j-1;
chiso[2] = j;
a = (y[0]+y[1]-2*y[2])/2;
Trang 57
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
b = y[2] - y[1] -2*a*chiso[0] - a;
}
else
{
y[0] = calib[j-1];
y[1] = max;
y[2] = calib[j+1];
chiso[0] = j-1;
chiso[1] = j;
chiso[2] = j+1;
a = (y[0]+y[1]-2*y[2])/2;
b = y[2] - y[1] -2*a*chiso[0] - a;
}
if ((abs(y[1]- y[0]) < 450)&&(abs(y[1]- y[2]) < 450))
signal = 1;
else signal = 0;
x1 = -b/(2*a);
x = 17*(x1-0.5);
if (signal == 0)
{
ds = 0.002*1.5*1000;
if (counterA_1 < 1200)
{ // AB
vr = 1;
wr = 2;
giaolo = 0;
}
else if ((counterA_1 > 1200)&&(counterA_1 <= 2800))
{ //BC
vr = 1;
wr = 0;
Trang 58
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
giaolo = 0;
}
else if ((counterA_1 > 2800)&&(counterA_1 <= 3200))
{ //C
giaolo = 1;
}
else if ((counterA_1 > 3200)&&(counterA_1 <= 3650))
{ //CD
vr = 1;
wr = 0;
giaolo = 0;
}
else if ((counterA_1 > 3650)&&(counterA_1 <= 8200))
{ //DE
vr = 1;
wr = 2;
giaolo = 0;
}
else if ((counterA_1 > = 8200)&&(counterA_1 <= 8800))
{ //EC
vr = 1;
wr = 0;
giaolo = 0;
}
else if ((counterA_1 > 8800)&&(counterA_1 <= 11400))
{ //C
giaolo = 1;
}
else if ((counterA_1 > 11400)&&(counterA_1 <= 12300))
{ //CG
vr = 1;
wr = 0;
giaolo = 0;
}
Trang 59
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
else if ((counterA_1 > 12300)&&(counterA_1 <= 13700))
{ // GA
vr = 1;
wr =2;
giaolo = 0;
}
else if ((counterA_1 > 13700)&&(counterA_1 <= 14500))
{ // A
giaolo = 2;
}
else if ((counterA_1 > 14500)&&(counterA_1 <= 16400))
{ // A
vr = 1;
wr = 0;
giaolo = 0;
}
else if ((counterA_1 > 16400)&&(counterA_1 <= 16500))
{ // A
giaolo = 1;
}
else if ((counterA_1 > 16500)&&(counterA_1 <= 175000))
{ // A
vr = 1;
wr = 0;
giaolo = 0;
}
else if ((counterA_1 > 175000)&&(counterA_1 <= 176000))
{
giaolo = 1;
}
else if (counterA_1 > 176000)
{
giaolo = 3;
}
Trang 60
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
if (giaolo == 0)
{
v = vr*cos(e3);
w = k2*vr*e2/1000 + wr + k3*sin(e3);
velocity1 = 0.5*(2*v - w*l)*293.8;
velocity2 = 0.5*(2*v + w*l)*293.8;
}
else if (giaolo == 1)
{
velocity1 = 350;
velocity2 = 350;
}
else if (giaolo == 2)
{
velocity1 = 50;
velocity2 = 350;
}
else if (giaolo == 3)
{
velocity1 = 0;
velocity2 = 0;
}
}
duty1_1 = PIDtrai(velocity1,velocitytrai);
duty2_1 = PIDphai(velocity2,velocityphai);
PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0,duty1_1*period/100 PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,duty2_1*period/100); } }
Trang 61
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 6: ĐIỆN – MẠCH ĐIỆN
6.1 Sơ đồ khối hệ thống điện
Hình 6.1 Sơ đồ khối hệ thống điện
6.2 Nguồn điện
Bảng 6.1 Công suất điện cần cung cấp cho các thiết bị
Thiết bị Số lượng Dòng Điện áp Tổng cộng
Động cơ Cảm biến Driver Tiva C LaunchPad 2 1 1 1 600 mA 40 mA 2,2 mA 100 mA 12V 5V 5V 5V
1200 mA – 12V 40 mA – 5V 2,2 mA – 5V 100 mA – 5V Tổng cộng 1200 mA – 12V 142,2 mA – 5V Vậy nguồn điện cung cấp cần có công suất tối thiểu:
𝑃 = 0,6.12 + 0,1422.5 = 15,11𝑊
Trang 62
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Chọn 4 pin sạc 18650 Panasonic 3,7V (sạc đầy là 4,2V), dung lượng 3700mAh – Imax là
5A , sử dụng khay lắp nối tiếp 4 pin để được điện thế cấp cho mạch nguồn là 14,8V. Khi đó
công suất bộ nguồn tối đa đạt 74W, đủ cho hệ thống.
Dòng xả là 5A đủ cho động cơ khởi động.
6.3 Mạch nguồn
Mạch nguồn có vai trò cung cấp nguồn điện ổn định 12V và 5V cho hệ thống, đảm bảo đủ dòng theo yêu cầu từ bảng 6.1. Mạch ổn áp có chức năng tạo ra điện áp nhỏ hơn điện áp đầu vào và luôn duy trì mức áp này mặc dù áp đầu vào tăng/giảm.
Nhóm sử dụng 2 module mạch giảm áp DC LM2596 có sẵn trên thị trường dùng để biến đổi điện áp từ nguồn cấp thành 12V và 5V ổn định. Áp được điều chỉnh bằng biến trở.
Thông số kỹ thuật:
- Điện áp đầu vào: 3 – 40V DC
- Điện áp đầu ra: điềuu chỉnh được
trong khoảng 1,5 – 35V DC
- Dòng đáp ứng tối đa: 3A
- Hiêu suất: 92%
- Công suất: 15W
Hình 6.2 Module LM2596
Bảng 6.2: Thông số về mức ổn áp của LM2596 [31]
Mức ổn áp Điên áp ngõ vào Điện áp ngõ ra Hiệu suất
5V 8 – 40V 4,8 – 5,2V 80%
12V 15 – 40V 11,52 – 12,48V 90%
Trang 63
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Thiết kế mạch PCB để lắp ráp 2 mạch giảm áp LM2596 theo dạng tầng
Hình 6.3 Mạch in lắp nguồn
6.4 Mạch driver động cơ
Nhóm lựa chọn mạch driver module TB6612 cho DC servo motor, với các yêu cầu kỹ
thuật đảm bảo cho hệ thống.
Thông số kỹ thuật:
- Điện áp nuôi và mức logic: 2,7 – 5,5VDC - Điện áp cấp cho motor tối đa 15VDC - 2 mạch cầu H cho 2 động cơ - 1,2A cho mỗi cầu H - Bảo vệ quá nhiệt, quá áp, chống dòng ngược.
Hình 6.4 Mạch driver TB6612
Bảng 6.2 Các ngõ ra/vào module TB6612
Chức năng
Chân GND, VCC VM PWMA, PWM2 AIN1, BIN1 AIN2, BIN2 STB Cấp nguồn nuôi Điện áp cấp cho motor Cấp xung PWM điều khiển động cơ Chiều quay 1 Chiều quay 2 Bật/tắt module
Tiến hành khảo sát không tải driver TB6612 và động cơ GA25 mà nhóm sử dụng, điều
khiển và đo vận tốc bằng board ARM (STM32F4 Discovery).
Trang 64
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 6.5 Đồ thị giữa PWM và vận tốc motor (không tải)
Đồ thị (Hình 6.5) cho thấy hệ driver và động cơ có độ tuyến tính cao, có thể sử dụng luật điều khiển vòng hở để điều khiển vận tốc.
Đáp ứng vận tốc của 2 động cơ theo PWM cấp cho driver không có sai lệch nhiều (± 0,1 vòng/phút), do đó việc mô hình hóa và điều khiển 2 động cơ hoàn toàn tương tự như nhau.
Đo thời gian đáp ứng vận tốc theo thời gian (Hình 6.6 và Hình 6.7). Khi không có tải, vận tốc động cơ ổn định (± 2 vòng/phút) và đáp ứng 2 động cơ gần như nhau.
Trang 65
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 6.6 Đồ thị đáp ứng với PWM = 40%
Hình 6.7 Đồ thị đáp ứng với PWM = 70%
Trang 66
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Nhận xét: Hệ thống driver TB6612 và motor có độ tuyến tính cao, thuận lợi cho việc điều
khiển vận tốc, thời gian đáp ứng không tải nhanh khoảng 0,1 giây.
6.5 Thiết kế board mạch tổng kết nối các thiết bị
Việc thiết kế board này nhằm mục đích lắp ráp kit vi điều khiển với board driver motor,
đồng thời đặt các ngõ ra vào phù hợp với kết cấu xe.
Hình 6.8 Sơ đồ nguyên lý và sơ đồ mạch in board tổng
Hình 6.9 Mô hình hóa 3D board tổng
6.6 Thiết kế mạch cảm biến
Hình 6.10 Sơ đồ nguyên lý mạch cảm biến
Trang 67
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 6.11 Bảng vẽ mạch in mạch cảm biến
Hình 6.12 Mô hình hóa 3D mạch cảm biến
6.7 Giải thuật PID để nâng cao đáp ứng của bộ driver và motor Khi có tải, đáp ứng vận tốc của động cơ vào khoảng gần 1s, điều này không đảm bảo thời
gian đáp ứng của hệ thống là 0,1s. Do đó cần phải có bộ PID điều khiển vận tốc động cơ, khi
đó vận tốc có thể điều khiển là nhỏ hơn 2/3 vận tốc max động cơ (khoảng 70% PWM).
Tiến hành khảo sát đáp ứng driver khi có tải và tuyến tính hóa bằng công cụ Curve Fitting
Tool của Matlab.
Hình 6.13 Đồ thị giữa PWM và vận tốc motor (có tải)
Trang 68
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 6.14 Mô hình tuyến tính động cơ trái
Hình 6.15 Mô hình tuyến tính động cơ phải
Tiến hành khảo sát đáp ứng khi đầy đủ tải.
Hình 6.16 Đồ thị đáp của động cơ trái tại PWM = 55% (có tải)
Trang 69
Hệ thống có dạng bậc nhất 𝐺(𝑠) = 𝐾 𝜏𝑠+1
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Hình 6.17 Đồ thị đường cong đáp của hệ bậc nhất
𝐺(𝑠) =
142 0,23𝑠 + 1
Sử dụng PID Tuner của Matlab để tìm bộ PID giảm ts < 0,1s
Xét với đồ thị hàm truyền PWM = 55%, Hàm truyền của động cơ 1 có dạng:
Hình 6.18 PID Tuner Matlab
Ta tìm được KP = 0.16, KI = 4,7, KD = 0.0001334
Trang 70
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
6.8 Kết quả thực hiện phần điện cho xe dò line
Bản vẽ sơ đồ điện (Bản vẽ A3 đính kèm)
Hình 6.19 Bản vẽ sơ đồ điện
Trang 71
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
CHƯƠNG 7: THỰC NGHIỆM VÀ ĐÁNH GIÁ
Chương này đánh giá kết quả thực nghiệm của mô hình so với mô phỏng và đưa ra kết
luận.
Hình ảnh xe thực tế:
Hình 7.1 Mô hình thực tế xe dò line
Hình 7.2 Kết quả chạy trên sa bàn trích từ video
Kết quả thực tế của của xe trên đường là 0.71 m/s đáp ứng được 71% yêu cầu của đề bài
đặt ra 1 m/s. Thời gian thực hiện hết một vòng chạy là 15s. Xe mô phỏng và chạy thực tế thể
hiện dạng bám line giống nhau: trên các đoạn đường thẳng (B→D, F→G,A→E), xe thể hiện
dao động hai bên đường line; trên các đoạn đường cong (A→B, D→F, G→A), xe đều nằm ở
một bên so với đường line. Trong khi sai số của mô phỏng đạt được yêu cầu đề bài về sai số
(trên đường thẳng và cong: emax = ±15mm và trên đoạn đổi hướng: emax = 21,14 mm, thể hiện
trên Hình 4.5b, Hình 4.6b, Hình 4.7b, Hình 4.8b, Hình 4.9b. Sai số thực tế của xe lại không
Trang 72
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
thỏa được yêu cầu đề ra về sai số trên đoạn thẳng và cong. Các lý do dẫn đến việc tăng sai số
này bao gồm:
- Sai số lắp đặt ảnh hưởng đến độ đồng trục của hai động cơ.
- Sai số tốc độ của hai động cơ.
- Ảnh hướng của hệ số ma sát của bánh mắt trâu.
- Quán tính của xe trong quá trình quẹo cua.
- Sai số của hệ thống cảm biến cảm biến do sự khác biệt giữa môi trường thí nghiệm và
thực nghiệm.
- Sai số của hệ thống cảm biến do chiều cao gá đặt và độ song song với mặt phẳng xe.
Để khắc phục các sai số này, các giải pháp được đề ra bao gồm:
- Khung xe và đồ gá được chế tạo bằng nhôm, thay cho mica trong mô hình thực nghiệm,
để có thể kiểm soát được các sai số trong quá trình gia công và lắp đặt.
- Thực hiện thí nghiệm nhằm đánh giá sai số vận tốc của động cơ để có thể đưa vào mô
phỏng.
- Thực nghiệm thí nghiệm để đánh giá ảnh hưởng của môi trường thực nghiệm lên giá
trị đọc của hệ thống cảm biến để có thể thực hiện các biện pháp xử lý phù hợp.
Trang 73
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
TÀI LIỆU THAM KHẢO
[1] DAIFUKU, SMARTCART, Automatic Guided Cart
Installed Systems Richard T. Vannoy II, M.S.I.T., B.S.E.E.T. Designing and Building a Line Following Robot.
[2] Andrew Reed Bacha, Line Detection and Lane Following for an Autonomous Mobile Robot, MS diss., Virginia Polytechnic Institute and State University, 2005. [3] Ramiro Velázquez et. al., A Review of Models and Structures for Wheeled Mobile Robots: Four Case Studies, The 15th International Conference on Advanced Robotics, Estonia, June 20-23, 2011.
[4] G. H. Lee et. al., Line Tracking Control of a Two-Wheeled Mobile Robot Using Visual Feedback,International Journal of Advanced Robotic Systems, DOI: 10.5772/53729, received 4 Apr 2012; Accepted 24 Sep 2012.
[5] Huu Danh Lam et. al., Smooth tracking controller for AGV through junction using
CMU camera,Hội nghị Toàn quốc lần thứ 7 về Cơ điện tử - VCM-2014.
[6] A. H. Ismail et. al.,Vision-based System for Line Following Mobile Robot,IEEE Symposium on Industrial Electronics and Applications (ISIEA 2009), October 4- 6, 2009, Kuala Lumpur, Malaysia.
[7] Mustafa Engin, Dilúad Engin, Path Planing of Line Follower Robot,Proceedings
of the 5th European DSP Education and Research Conference, 2012.
[8] F. Kaiser et.al., Line Follower Robot: Fabrication and accuracy measurement by data acquisition,International Conference on Electrical Engineering and Information & Communication Technology (ICEEICT) 2014.
[9] Hsin-Hsiung Huang et. al., Hands-on intelligent mobile robot laboratory with support from the industry, IEEE EDUCON Education Engineering 2010 – The future of Global Learing Engineering Education.
[10] Deepak Punetha, et. al. Development and Applications of Line Following Robot Based Health Care Management System. International Journal of Advanced Research in Computer Engineering & Technology (IJARCET) Volume 2, Issue 8, pp.2446-2449, August 2013.
[11] Umar Farooq et. al., Fuzzy Logic Reasoning System for Line Following Robot, IACSIT International Journal of Engineering and Technology, Vol. 6, No. 4, August 2014
[12] Subhash P. Rasal. Development of Intelligent Line Follower’s Robot. International Journal of Emerging Trends in Electrical and Electronics (IJETEE – ISSN: 2320- 9569) Vol. 7, Issue. 2, pp.15-16, Sep-2013.
[13] Juing-Huei Suet. al., An intelligent line-following robot project for introductory robot courses,World Transactions on Engineering and Technology Education, Vol.8, No.4, 2010.
[14] M. Zafri Baharuddin et. al., Analysis of Line Sensor Configuration for the
Advanced Line Follower Robot, Universiti Tenaga Nasional, Malaysia.
Trang 74
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Ngô Hà Quang Thịnh
Institute, Carnegie – Mellon University.
[16] Dirman Hanafi et. al., Wall Follower Autonomous Robot Development Applying Fuzzy Incremental Controller, Intelligent Control and Automation, 2013, 4, 18-25. [17] Takanori Fukao et. al., Adaptive Tracking Control of a Nonholonomic Mobile Robot, IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 16, NO. 5, OCTOBER 2000.
[18] R. Fierro et. al., Control of a Nonholonomic Mobile Robot Using Neural Networks,IEEE TRANSACTIONS ON NEURAL NETWORKS, VOL. 9, NO. 4, JULY 1998.
[19] Xiaoling Wu et.al., An Improved Hardware Design and Navigation Optimization Algorithm for Line Following Robot, Journal of Convergence Information Technology (JCIT) Volume8, Number5,Mar 2013
[20] Yutaka Kanayama et. al., A Stable Tracking Control Method for an Autonomous
Mobile Robot, IEEE 1990
[21] Iman Anvari, Non-holonomic Differential Drive Mobile Robot Control & Design: Critical Dynamics and Coupling Constraints, Master Thesis, ARIZONA STATE UNIVERSITY, December 2013
[22] Joseph R.Davis, Coeffiction of Friction. In Concise Metals Engineering Data
Book, 190-197. United State of America: ASM International, 1997
[23] Joseph L.Jones, Anita M.Flynn & Bruce A.Seiger. Mobile Robot Inspirational to Implementation. 2nd ed. Cambridge, MA: MIT Artificial Intelligence Laboratory Publication Office, 1998
[24] Portescap Motion Company, DC Motor Catalogue [25] Iman Anvari, Non-holonomic Differential Drive Mobile Robot Control & Design: Critical Dynamics and Coupling Constraints, A Thesis Presented in Partial Fulfillment of the Requirement for the Degree Master of Science, Arizona State [26] Trịnh Chất & Lê Văn Uyển. Tính Toán Thiết Kế Hệ Dẫn Động Cơ Khí Tập 2, 116-
117. Việt Nam, Công ty In Công Đoàn Việt Nam, 2006
[27] Ninh Đức Tốn, Dung Sai và Lắp Ghép, Việt Nam, Công ty Cổ Phần In Thái
Nguyên, 2010
[28] VISHAY, TCRT5000, TCRT5000L Datasheet, Document number: 80112, Rev.
1.1, 02-Jul-09
[29] VISHAY, Application of Optical Reflex SensorsTCRT1000, TCRT5000, CNY70,
Document number: 80107, Rev. 1.1, 02-02
[30] LM2586 Datasheet
[15] Richard Wallace et. al., First Results in Robot Road – Following, Robotics
Trang 75
