BỘ CÔNG THƯƠNG

TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP HÀ NỘI

KHOA CƠ KHÍ
----------------------------------

ĐỒ ÁN TỐT NGHIỆP ĐẠI HỌC

NGÀNH CÔNG NGHỆ KỸ THUẬT CƠ ĐIỆN TỬ

ĐỀ TÀI:
NGHIÊN CỨU, THIẾT KẾ ROBOT ĐIỀU KHIỂN THEO QUỸ ĐẠO
ỨNG DỤNG LA BÀN SỐ VÀ CẢM BIẾN KHOẢNG CÁCH

Giáo viên hướng dẫn : Th.s Lê Ngọc Duy

Nhóm sinh viên thực hiện: Lương Đình Phúc
Nguyễn Văn Quân
Trịnh Văn Thắng
Lớp : Cơ điện tử 1 – K10

Hà Nội – 2019
 NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN

.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
Hà Nội, ngày tháng năm 2018
Giáo viên hướng dẫn
 NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
Hà Nội, ngày tháng năm 2018
Giáo viên phản biện
 LỜI NÓI ĐẦU
Kỹ thuật robot là một lĩnh vực phát triển hết sức nhanh chóng, là một
trong những công nghệ mũi nhọn của thế kỉ 21. Cùng với sự phát triển của xã
hội, kỹ thuật robot ngày càng được ứng dụng rộng rãi vào cuộc sống con người.
Ngày nay, chúng ta có thể thấy robot đã làm thay con người rất nhiều công việc.
Đó là những công việc mà con người cảm thấy nhàm chán, khó thực hiện, yêu
cầu độ chính xác hoặc có mức độ nguy hiểm cao. Ví dụ như robot lau nhà, robot
lắp ráp trong công nghiệp, robot trong ngành năng lượng hạt nhân, robot để
thám hiểm vũ trụ….

Để góp phần giúp sinh viên hiểu hơn về cấu tạo và nguyên lí hoạt động
của Mobile robot nhóm em đã chọn đề tài Mobile robot 3 bánh di chuyển theo
quỹ đạo, ứng dụng la bàn số và cảm biến khoảng cách. Dựa trên các kiến thức
đã tìm hiểu ở loại Mobile robot truyền thống, nhóm em ứng dụng thêm la bàn
số để tăng tính chính xác và điều khiển tối ưu quỹ đạo của robot hơn.

Em xin chân thành cám ơn thầy Lê Ngọc Duy đã hướng dẫn, nhiệt tình
giúp đỡ chúng em trong quá trình thực hiện đồ án. Chúng em cũng gửi lời cảm
ơn tới nhà trường, đặc biệt là quý thầy cô trong bộ môn cơ khí, cơ điện tử,
những người đã trang bị kiến thức nền tảng và giúp đỡ chúng em trong thời
gian qua.

Do kiến thức còn hạn chế nên đồ án còn nhiều sai sót. Chúng em mong
được sự giúp đỡ và đóng góp của các thầy và các bạn để đồ án được hoàn thiện
hơn. Chúng em xin chân thành cảm ơn!

Sinh viên thực hiện đồ án

Lương Đình Phúc

Nguyễn Văn Quân

Trịnh Văn Thắng

 MỤC LỤC

NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN ........................................... 2

CHƯƠNG 1. TỔNG QUAN VỀ MOBILE ROBOT ĐIỀU KHIỂN THEO

QUỸ ĐẠO ........................................................................................................ 1

1.1. Lịch sử nghiên cứu ................................................................................. 1

1.1.1. Giới thiệu chung về Robot .............................................................. 1

1.1.2. Giới thiệu chung về robot omni ...................................................... 3

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

1.3. Phương pháp thực hiện đề tài................................................................. 8

CHƯƠNG 2. MÔ HÌNH TOÁN HỌC MOBILE ROBOT ĐIỀU KHIỂN

THEO QUỸ ĐẠO ........................................................................................... 9

2.1. Tính toán, lựa chọn phương pháp điều khiển ........................................ 9

2.2. Mô hình toán học hệ thống cơ khí ......................................................... 9

2.2.1. Mô hình hóa robot ........................................................................... 9

2.2.2. Tính toán động học, động lực học cho robot ................................ 10

2.2.3. Tính toán động học cho robot cho một số chuyển động cơ bản ... 13

2.3. Mô hình toán học hệ thống điều khiển................................................. 15

2.3.1. Bộ điều khiển động cơ PID ........................................................... 15

2.3.2. Xây dựng mô hình động cơ điện một chiều dưới dạng hàm truyền
................................................................................................................. 17

CHƯƠNG 3. TÍNH TOÁN, THIẾT KẾ ROBOT ĐIỀU KHIỂN THEO

QUỸ ĐẠO ...................................................................................................... 19

3.1. Tính toán thiết kế hệ thống cơ khí ....................................................... 19

3.1.1. Thiết kế phần đế và nắp robot omi................................................ 19

3.1.2. Thiết kế khớp nối U ...................................................................... 20

 3.1.3. Lựa chọn bánh xe omni ................................................................. 21

3.1.4. Khớp nối bánh xe với động cơ ...................................................... 22

3.1.5. Tính toán chọn động cơ................................................................. 23

3.2. Tính toán, thiết kế hệ thống điện, điện tử ............................................ 26

3.2.1. Lựa chọn vi điều khiển trung tâm ................................................. 27

3.2.2. Vi điều khiển Arduino Nano ......................................................... 30

3.2.3. Lựa chọn module bluetooth .......................................................... 31

3.2.4. Lựa chọn driver ............................................................................. 33

3.2.5. Cảm biến gia tốc............................................................................ 35

3.2.6. La bàn số ....................................................................................... 36

3.2.7. Hệ thống nguồn ............................................................................. 38

3.2.8. Mạch giảm áp ................................................................................ 39

3.2.9. Sơ đồ kết nối mạch điện ................................................................ 40

3.3. Tính toán, thiết kế hệ thống điều khiển................................................ 40

3.3.1. Lưu đồ thuật toán điều khiển Mobile Robot ................................. 40

3.3.2. Thiết kế giao diện điều khiển ........................................................ 40

CHƯƠNG 4. CHẾ TẠO, LẮP RÁP VÀ THỬ NGHIỆM MÔ HÌNH ..... 44

4.1. Chế tạo hệ thống cơ khí. ...................................................................... 44

4.2. Hệ thống điện, điện tử. ......................................................................... 45

4.3. Mô hình hoàn chỉnh ............................................................................. 47

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

PHỤ LỤC ....................................................................................................... 49

Chương trình điều khiển trên Arduino: ....................................................... 49

Chương trình lập trình ứng dụng điều khiển trên điện thoại: ..................... 57
 DANH MỤC HÌNH ẢNH
Hình 1-1: Robot vận chuyển hàng hóa tự động trong kho hàng Amazon
........................................................................................................................... 2

Hình 1-2: Robot hút bụi Samsung POWERbot ...................................... 3

Hình 1-3: Robot thám hiểm Sao Hỏa Curiosity của NASA ................... 3

Hình 1-4: Xe lăn alexis- 1982 ................................................................. 4

Hình 1-5: Robot KUKA - omniMove..................................................... 5

Hình 1-6: Robot Omni do Trường ĐHBK Đà Nẵng nghiên cứu ........... 6

Hình 1-7: Robot Omni tránh vật cản sử dụng hồng ngoại...................... 7

Hình 2-1: Mô hình động học và động lực học cho Omni Robot ............ 9

Hình 2-2: Quan hệ giữa hệ số ma sát khô và hệ số ma sát nhớt ........... 13

Hình 2-3: Chuyển động tịnh tiến .......................................................... 14

Hình 2-4: Chuyển động quay ................................................................ 14

Hình 2-5 Mô hình bộ điều khiển PID ................................................... 16

Hình 2-6: Mô hình hóa động cơ DC ..................................................... 17

Hình 3-1: Hình ảnh thiết kế của đế và nắp .......................................... 20

Hình 3-2: Khớp nối U ........................................................................... 20

Hình 3-3: Ví dụ các loại bánh xe trên thực tế ....................................... 21

Hình 3-4: Bánh xe omni mô hình ......................................................... 22

Hình 3-5: Khớp nối trục động cơ và bánh. ........................................... 22

Hình 3-6: Các lực tác dụng lên bánh xe ............................................... 23

Hình 3-7: Các chân kết nối của Encoder .............................................. 25

Hình 3-8: Hình ảnh mô hình của động cơ ............................................ 26

Hình 3-9: Mô hình thiết kế trên phần mềm Solidworks ....................... 26

Hình 3-10: Sơ đồ khối hệ thống............................................................ 27

Hình 3-11: Hình ảnh mô hình của Arduino .......................................... 29

Hình 3-12: Module bluetooth HC-06 ................................................... 32

Hình 3-13: Hình ảnh thực tế của Driver L298...................................... 33

Hình 3-14: Vị trí các chân của driver L298 .......................................... 34

Hình 3-15: Cảm biến gia tốc và la bàn số MPU-6050 ......................... 35

Hình 3-16: Cảm biến la bàn số HMC5883L......................................... 37

Hình 3-17: Sơ đồ nguyên lí la bàn số HC8553L .................................. 38

Hình 3-18: Hình ảnh thực tế của Pin 12V ............................................ 39

Hình 3-19: Mạch giảm áp ..................................................................... 39

Hình 3-20: Giao diện Project MIT App Inventor ................................. 42

Hình 3-21: Giao diện thiết kế ứng dụng ............................................... 42

Hình 3-22: Hình ảnh giao diện lập trình ứng dụng............................... 43

Hình 3-23: Giao diện điều khiển bằng điện thoại ................................. 43

Hình 4-1: Khung máy sau khi được lắp ráp.......................................... 44

Hình 4-2: Bánh xe omni 58mm ............................................................ 44

Hình 4-3: khớp nối trục động cơ và bánh omni .................................... 45

Hình 4-4: Mô hình hoàn chỉnh.............................................................. 47

 DANH MỤC BẢNG

Bảng 2-1: : Nhận xét về các thông số Kp, Ki, Kd ................................ 16

Bảng 3-1: Thông số yêu cầu robot ........................................................ 19

Bảng 3-2: Thông số kỹ thuật động cơ................................................... 24

Bảng 3-3: Thông số kỹ thuật Arduino Mega 2560 ............................... 29

Bảng 3-4: Bảng thông số của Arduino Nano ........................................ 30

Bảng 3-5: Thông số kỹ thuật Driver L298 ........................................... 33

Bảng 3-6: Thông số kỹ thuật PIN 12V ................................................. 38

Bảng 4-1 :Các linh kiện điện, điện tử. .................................................. 45

 1

CHƯƠNG 1. TỔNG QUAN VỀ MOBILE ROBOT ĐIỀU KHIỂN THEO

QUỸ ĐẠO

1.1. Lịch sử nghiên cứu

1.1.1. Giới thiệu chung về Robot

Robot là một từ chỉ người lao động trong hệ ngôn ngữ Sla-vơ. Cho đến
nay có rất nhiều định nghĩa khác nhau về robot. Những định nghĩa đó có nội
dung tương tự như nhau.

Robot hay người máy là một loại máy có thể thực hiện những công việc
một cách tự động bằng sự điều khiển của máy tính hoặc các vi mạch điện tử
được lập trình.

Robot là một tác nhân cơ khí, nhân tạo, thường là một hệ thống cơ khí-
điện tử. Với sự xuất hiện và chuyển động của mình, robot gây cho người ta cảm
giác rằng nó giác quan giống như con người. Từ "robot" (người máy) thường
được hiểu với hai nghĩa: robot cơ khí và phần mềm tự hoạt động.

Ngày nay, người ta vẫn còn đang tranh cãi về vấn đề: “Một loại máy như
thế nào thì đủ tiêu chuẩn để được gọi là một robot?” Một cách gần chính xác,
robot phải có một vài (không nhất thiết phải đầy đủ) các đặc điểm sau đây:

- Không phải là tự nhiên, tức là do con người sáng tạo ra.

- Có khả năng nhận biết môi trường xung quanh.
- Có thể tương tác với những vật thể trong môi trường.
- Có sự thông minh, có khả năng đưa ra các lựa chọn dựa trên môi
trường và được điều khiển một cách tự động theo những trình tự đã
được lập trình trước.
- Có khả năng điều khiển được bằng các lệnh để có thể thay đổi tùy
theo yêu cầu của người sử dụng.
- Có thể quay hoặc tịnh tiến theo một hay nhiều chiều.
- Có sự khéo léo trong vận động.
 2

Phân loại Mobile Robot: Có thể phân loại Mobile robot theo phương
pháp di chuyển.
- Robot có chân, di chuyển giống con người hay động vật.
- Robot di chuyển bằng xích, đai.
- Robot di chuyển bằng bánh xe.
Các dạng điều khiển:
- Robot điều khiển từ xa bằng tay qua sóng RF, wifi, hồng ngoại hay
Bluetoot…Robot điều khiển từ xa giúp con người tránh khỏi nguy hiểm,
không phải di chuyển theo robot.
- Thực thi theo lộ trình: dạng robot thường thấy đó là robot dò line, phát
hiện đường đi theo vạch chỉ sẵn; hay lập trình sẵn quỹ đạo và robot di chuyển
theo quỹ đạo đó.
- Robot hoạt động độc lập với những chuyển động ngẫu nhiên: Về cơ bản
đó là chuyển động nhảy bật lên tường, hay di chuyển trong khoảng không gian
nhất định, có sử dụng cảm biến tránh vật cản như robot hút bụi, máy cắt cỏ…
Ứng dụng của Mobile robot:
Mobile robot được dùng trong các nhà máy vận chuyển hàng hóa.

Hình 1-1: Robot vận chuyển hàng hóa tự động trong kho hàng Amazon
Ứng dụng trong đời sống, hỗ trợ con người làm việc nhà như hút bụi, lau
sạch bể bơi, giám sát an ninh, cắt cỏ, đồ chơi…
 3

Hình 1-2: Robot hút bụi Samsung POWERbot

Ứng dụng trong những môi trường độc hại, những nơi con người không
thể đi tới, hay đi tới một cách khó khăn, nguy hiểm như sao hỏa, đáy biển, núi
lửa…

Hình 1-3: Robot thám hiểm Sao Hỏa Curiosity của NASA
1.1.2. Giới thiệu chung về robot omni

Robot Omni là một loại mobile robot di chuyển bằng những bánh xe đã
được ứng dụng nhiều trong thực tế do quỹ đạo chuyển động của nó rất đa dạng.
Đặc biệt là Omni có kết cấu lạ với 3 bánh xe, nhưng chính do sự phối hợp hoạt
động của 3 bánh xe lại cho ta khả năng điều khiển tốt quỹ đạo của robot.
 4

Omni robot thuộc hệ robot di động tự hành, tự định hướng và tự tránh

được vật cản. Ứng dụng của nó là phục vụ trong công tác dịch vụ chăm sóc y
tế, đặc biệt là trong điều kiện môi trường lây nhiễm cao hay môi trường có
cường độ phóng xạ cao thay con người...
Robot Omni có đặc điểm là kết cấu đơn giản, có quỹ đạo di chuyển khá
linh hoạt nên được phát triển để thay thế cho các loại mobile robot truyền thống.
Trên thế giới, Mobile robot đã được phát triển từ lâu và ngày càng có nhiều
ứng dụng đa dạng do được tích hợp các công nghệ mới nhất.
Một số mô hình Omni robot trong ứng dụng thực tế trên thế giới:
- Alexis – xe lăn đa hướng bánh xe omni (năm 1982)

Hình 1-4: Xe lăn alexis- 1982

Xe lăn Alexis sử dụng bánh xe omni được nghiên cứu và phát triển bởi
nhóm nghiên cứu đại học Stanford (Hoa Kỳ) vào năm 1982. Vào thời gian đó,
đây là một cuộc cách mạng về cách di chuyển mới của robot là sử dụng bánh
 5

xe omni-directional (gồm 1 bánh lớn và nhiều bánh nhỏ có trục vuông góc với
bánh lớn). Alexis có thể di chuyển trực tiếp đến bất kỳ vị trí nào trên đường đi
bằng thao tác của người lái thông qua một núm điều khiển nó sẽ gửi tín hiệu
đến vi xử lý để điều tốc độ của ba bánh xe. Xe được tạo ra để phục vụ người
tàn tật và các cựu chiến binh không có khả năng đi lại bình thường.

Đặc điểm của xe lăn Alexis:

- Hiệu quả cao.

- Tốc độ cao.
- Tải trọng lớn.
- Thao tác đơn giản.
- Khả năng cơ động cao.
- Hoạt động ổn định.
- Giá thành phù hợp.
- Khả năng leo dốc kém.

Thật không may, Alexis không bao giờ được sản xuất ra thị trường vì Jon
King, giám đốc điều hành Intex đã tham ô hàng triệu đô la vốn đầu tư. Ông này
sau đó đã bị kết án 10 năm tù giam trong nhà tù liên bang. Nhưng Alexis đã đi
tiên phong trong công nghệ ứng dụng của loại xe sử dụng bánh xe Omni giúp
đỡ người tàn tật.

Hình 1-5: Robot KUKA - omniMove

Robot KUKA omniMove của hãng sản xuất robot công nghiệp KUKA,
Robot omniMove là sản phẩm điển hình cho giải pháp vận chuyển mới trong
nhà máy.
 6

Điểm mạnh:
- Có khả năng vận hành tự động hoặc điều khiển từ xa
- Tính linh hoạt cao
- Độ chính xác cực kì cao
- Trọng tải siêu lớn
- Có thể lắp ghép mô-đun.
Các nghiên cứu trong nước
- Omni Robot cân bằng trên quả cầu.

Hình 1-6: Robot Omni do Trường ĐHBK Đà Nẵng nghiên cứu

Robot cân bằng trên quả cầu (Ball Balancing Robot – BBR) lấy cảm hứng
từ Rezero của đại học ETH Zurich (Anh), nguyên mẫu BBR được thiết kế và
chế tạo tại khoa cơ khí, Trường đại học bách khoa Đà Nẵng gồm ba bánh xe đa
hướng được truyền động bởi ba động cơ DC được bố trí lệch nhau 120 độ. Điểm
đặc biệt là phần thân nguyên mẫu có hình lăng trụ lục giác đều, phù hợp cho
việc chế tạo lớp vỏ bảo vệ. Nguyên mẫu BBR bao gồm 3 phần chính: phần
thân, hệ thống động lực và quả cầu.

Khối điều khiển trung tâm là một mạch Arduino Mega2560, nhận dữ liệu
trạng thái từ IMU và tính toán, truyền đến khối điều khiển động cơ, các giá trị
đặt cho mỗi động cơ. Mạch này cũng đảm bảo các kết nối không dây để điều
khiển và thu thập dữ liệu BBR từ xa. Hệ thống động lực robot bao gồm ba động
cơ DC dẫn động ba bánh xe đa hướng và các mạch driver tương ứng. Vị trí trục
và dòng điện của các động cơ được đo bằng encoder và cảm biến dòng. Các
 7

thông tin này được khối điều khiển động cơ sử dụng để điều chỉnh tốc độ và
momen xoắn của mỗi động cơ theo phương pháp điều chế độ rộng xung (PWM
- Pulse Width Modulation).
- Omni Robot tránh vật cản:
Điều khiển phối hợp 3 động cơ RC Servo và sử dụng 3 cảm biến hồng
ngoại đi theo những quy đạo đơn giản và tránh chướng ngại vật do nhóm sinh
viên đại học bách khoa Hà Nội chế tạo.

Hình 1-7: Robot Omni tránh vật cản sử dụng hồng ngoại
1.2. Mục tiêu của đề tài

Về mặt thiết kế cơ khí: Mobile robot có thiết kế đơn giản, nhỏ gọn, đảm
bảo độ cứng vững và đủ không gian lắp ráp các linh kiện. Các bánh được lắp
đặt theo góc đều nhau và đảm bảo trọng tâm của mô hình trong quá trình di
chuyển.
Hệ thống điều khiển: Sử dụng các linh kiện điện tử phổ biến trên thị
trường, dễ dàng trong quá trình thiết kế hệ thống điều khiển, lập trình. Các linh
kiện, đường đi dây được bố trí hợp lí, gọn gàng, không bị lỏng lẻo, ngắt kết nối
trong quá trình hoạt động.
Mục tiêu hoạt động của mô hình: Mobile robot có khả năng di chuyển đa
hướng, di chuyển theo quỹ đạo đơn giản được thiết kế trên giao diện điện thoại.
 8

Mô hình có thể sử dụng chế độ điều khiển bằng tay và chế độ điều khiển tự
động được kết nối thông qua modul bluetooth với độ chính xác trên 95%.
1.3. Phương pháp thực hiện đề tài

- Tham khảo tài liệu và đánh giá:

Đọc tài liệu về động lực học Robot, Robot đa hướng, tìm hiểu về nguyên
lí điều khiển, các phương trình động lực học, bộ điều khiển PID vận dụng kiến
thức các môn học đưa vào thực tế.
Tìm hiểu tài liệu vi điều khiển, cụ thể là Arduino để tạo ra chương trình
điều khiển cho Robot, các tài liệu về kĩ thuật điện tử, vi mạch, để hiểu hơn về
nguyên lí các linh kiện.
Sử dụng các phần mềm hỗ trợ thiết kế cơ khí như Solidworks để tạo ra các
mẫu thử, btừ đó đánh giá mô hình thích hợp nhất. Phần mềm Proteus dùng để
vẽ sơ đồ mạch điện, đường đi dây.
Ngoài ra chúng em con tham khảo các tài liệu trên Internet cũng như các
đồ án tương tự của khóa trước để hiểu rõ hơn về lĩnh vực Robot.
- Quan sát: Hoạt động Robot đa hướng trong các video hay các cuộc thi
Robocon xem khả năng hoạt động của Mobile robot và đưa ra các yêu cầu cho
mô hình.
- Thực nghiệm: Từ mục tiêu đề tài và đánh giá trên mô hình thiết kế trên phần
mềm chúng em thiết kế, chế tạo thân robot, lắp ráp các Board mạch, cảm biến,
kết nối với máy tính, viết chương trình chạy thử nghiệm và hiệu chỉnh.
 9

CHƯƠNG 2. MÔ HÌNH TOÁN HỌC MOBILE ROBOT ĐIỀU KHIỂN

THEO QUỸ ĐẠO

2.1. Tính toán, lựa chọn phương pháp điều khiển

Trong đồ án này, chúng em sẽ sử dụng Aduino mega 2560 và ứng dụng la

bàn số để điều khiển phối hợp 3 động cơ 12V-DC Encoder cho robot di chuyển
đa hướng. Sử dụng phương pháp điều khiển trên máy tính (thiết kế quỹ đạo)
kết hợp với Board mạch điều khiển để điều khiển Robot di chuyển theo quỹ
đạo có sẵn. Từ đó có thể ứng dụng trong công việc vận chuyển, giám sát, xác
định được quỹ đạo tối ưu giúp Robot di chuyển một cách dễ dàng và linh hoạt.
2.2. Mô hình toán học hệ thống cơ khí

2.2.1. Mô hình hóa robot

Chuyển động của robot là do chuyển động của các bánh xe. Các bánh xe
chuyển động do động cơ lắp ở mỗi trục bánh quay và sinh ra lực kéo. Vậy ta
xét đến mô hình robot, trong đó các lực kéo đặt ở các bánh xe. Vận tốc của
robot được tính dựa vào vận tốc của các bánh xe. Từ đó ta có mô hình robot
như sau:

Hình 2-1: Mô hình động học và động lực học cho Omni Robot
Robot gồm 3 bánh đặt lệch nhau 1200, tiếp tuyến với một đường tròn
đường kính 350mm. Gọi các bánh xe lần lượt là 1,2,3. Chọn 2 hệ trục tọa độ
như sau: hệ trục tọa độ Oxy gắn cố định với mặt phẳng sàn, hệ trục tọa độ Ox1y1
gắn với robot như trên hình vẽ. Tại thời điểm ban đầu giả thiết rằng hai trục tọa
 10

độ trùng với nhau. Tại thời điểm t, hai hệ trục tọa độ lệch nhau 1 góc 𝜃 chính
là góc quay của robot so với vị trí ban đầu.
Trong đó:
- x: vị trí của robot trong hệ trục tọa độ Oxy theo phương Ox
- y: vị trí của robot trong hệ trục tọa độ Oxy theo phương Oy
- d: khoảng cách giữa các bánh và tâm robot [m]
- v1, v2, v3: vận tốc dài của các bánh xe [m/s]
- 𝜔1 , 𝜔2 , 𝜔3 : vận tốc góc của các bánh [rad/s]
- f1, f2, f3: lực do động cơ sinh ra trên các bánh [N]
- T1, T2, T3 : Momen kéo do động cơ sinh ra đặt lên các bánh [Nm]
- v, vn : Vận tốc theo 2 phương trên hệ trục Ox1y1 của robot [m/s]
-  : vận tốc góc của robot: [rad/s]
- Fv, Fvn: lực tác dụng lên robot dọc theo phương 0x1 và 0y1 [N]
- T: Momen robot (theo  ) [Nm]
2.2.2. Tính toán động học, động lực học cho robot

Gọi vx(t), vy(t),   t  là vận tốc tức thời của robot trên hệ trục tọa độ Oxy. Ta
có:

𝑑𝑥(𝑡)
𝑣𝑥 (𝑡) =
𝑑𝑡 (2.1)
𝑑𝑦(𝑡)
𝑣𝑦 (𝑡) =
𝑑𝑡
𝑑𝜃(𝑡)
𝜔(𝑡) =
𝑑𝑡
Chiếu các vận tốc trên lên hệ trục tọa độ Ox1y1 gắn với robot, ta có:

𝑣(𝑡) = 𝑣𝑥 (𝑡)𝑐os(𝜃(𝑡)) + 𝑣𝑦 (𝑡) 𝑠𝑖𝑛(𝜃(𝑡))

𝑣𝑛(𝑡) = −𝑣𝑥 (𝑡) 𝑠𝑖𝑛(𝜃(𝑡)) + 𝑣𝑦 (𝑡)𝑐os(𝜃(𝑡)) (2.2)

𝜔𝑂𝑥1𝑦1 (𝑡) = 𝜔(𝑡)
 11

Viết lại các công thức trên dưới dạng ma trận, ta được công thức liên hệ giữa
vận tốc theo 2 hệ trục tọa độ khác nhau:

𝑣(𝑡) 𝑐os(𝜃(𝑡)) 𝑠𝑖𝑛(𝜃(𝑡)) 0 𝑣𝑥 (𝑡)

(2.3)
[𝑣𝑛(𝑡) ] = [− 𝑠𝑖𝑛(𝜃(𝑡)) 𝑐os(𝜃(𝑡)) 0] [𝑣𝑦 (𝑡)]
𝜔𝑂𝑥1𝑦1 (𝑡) 0 0 1 𝜔(𝑡)
Xét trong hệ trục gắn với robot Ox1y1, giả sử robot chuyển động với các vận
tốc là v(t), vn(t) và 𝜔(𝑡), ta có công thức tính vận tốc của các bánh là:
𝑣1 (𝑡) = −𝑣(𝑡) 𝑠𝑖𝑛(𝜋/3) + 𝑣𝑛(𝑡)𝑐os(𝜋/3) + 𝜔(𝑡)𝑑
(2.4)
𝑣2 (𝑡) = −𝑣𝑛(𝑡) + 𝜔(𝑡)𝑑
𝑣3 (𝑡) = 𝑣(𝑡) 𝑠𝑖𝑛(𝜋/3) + 𝑣𝑛(𝑡)𝑐os(𝜋/3) + 𝜔(𝑡)𝑑
Viết lại hệ phương trình (2.4) dưới dạng ma trận ta thu được:
𝑣1 (𝑡) − 𝑠𝑖𝑛(𝜋/3) 𝑐os(𝜋/3) 𝑑 𝑣(𝑡)
[𝑣2 (𝑡)] = [ 0 −1 𝑑 ] [𝑣𝑛(𝑡)] (2.5)
𝑣3 (𝑡) 𝑠𝑖𝑛(𝜋/3) 𝑐os(𝜋/3) 𝑑 𝜔(𝑡)
Giải hệ phương trình trên ta có:
√3
𝑣(𝑡) =
3
(𝑣3 (𝑡) − 𝑣1 (𝑡))
1 2 (2.6)
𝑣𝑛(𝑡) = (𝑣3 (𝑡) + 𝑣1 (𝑡)) − 𝑣2 (𝑡)
3 3
1
𝜔(𝑡) = (𝑣 (𝑡) + 𝑣2 (𝑡) + 𝑣3 (𝑡))
4𝑑 1
Các công thức trên cho phép ta tính các vận tốc của robot (vận tốc theo
các phương của trục tọa độ và vận tốc của robot) khi đã biết vận tốc của các
bánh xe.
Mô hình động lực học cho robot
Theo phương trình các phương trình tính gia tốc và gia tốc góc cho một vật:

∑ 𝐹 = 𝑀𝑣̇

∑ 𝑇 = 𝐽𝜔̇

Ta có:
 12

𝑑𝑣(𝑡)
𝑀 = ∑ 𝐹𝑣 (𝑡) − 𝐹𝐵𝑣 (𝑡) − 𝐹𝐶𝑣 (𝑡)
𝑑𝑡
𝑑𝑣𝑛(𝑡)
𝑀 = ∑ 𝐹𝑣𝑛 (𝑡) − 𝐹𝐵𝑣𝑛 (𝑡) − 𝐹𝐶𝑣𝑛 (𝑡)
𝑑𝑡
𝑑𝜔(𝑡)
𝐽. = ∑ 𝑇(𝑡) − 𝑇𝐵𝜔 (𝑡) − 𝑇𝐶𝜔 (𝑡)
𝑑𝑡
Trong đó:
M [kg]: khối lượng robot
J [kgm2]: Mô men quán tính của robot
𝐹𝐵𝑣 ,𝐹𝐵𝑣𝑛 [N]: Lực ma sát nhớt dọc theo phương Ox1 và Oy1 tác dụng lên robot
𝑇𝐵𝜔 [Nm]: Mô men ma sát nhớt tác dụng lên robot
𝐹𝐶𝑣 ,𝐹𝐶𝑣𝑛 [N]: Lực ma sát khô dọc theo phương Ox1 và Oy1 tác dụng lên robot
𝑇𝐶𝜔 [Nm]: Mô men ma sát khô tác dụng lên robot
Lực ma sát nhớt tỉ lệ với vận tốc của robot:
𝐹𝐵𝑣 (𝑡) = 𝐵𝑣 . 𝑣(𝑡)
𝐹𝐵𝑣𝑛 (𝑡) = 𝐵𝑣𝑛 . 𝑣𝑛(𝑡)
𝐹𝐵𝑣𝑛 (𝑡) = 𝐵𝜔 . 𝜔(𝑡) (2.7)
𝐵𝑣 ,𝐵𝑣𝑛 [N/(m/s)]: Hệ số ma sát nhớt
𝐵𝜔 [Nm/(rad/s)]: Hệ số ma sát nhớt do vận tốc quay
Độ lớn của các lực ma sát khô là hằng số:
𝐹𝐶𝑣 (𝑡) = 𝐶𝑣 . 𝑠𝑖𝑛(𝑣(𝑡))
𝐹𝐶𝑣𝑛 (𝑡) = 𝐶𝑣𝑛 . 𝑠𝑖𝑛(𝑣𝑛(𝑡))
(2.8)
𝑇𝐶𝜔 (𝑡) = 𝐶𝜔 . 𝑠𝑖𝑛(𝜔(𝑡))
𝐶𝑣 ,𝐶𝑣𝑛 [N]: hệ số ma sát khô dọc theo phương Ox1 và Oy1
𝐶𝜔 [Nm]: hệ số ma sát khô theo w
 13

Hình 2-2: Quan hệ giữa hệ số ma sát khô và hệ số ma sát nhớt

Mối quan hệ giữa lực kéo của robot, mô men quay và lực kéo trên mỗi
bánh được đưa vào công thức sau:
𝜋
∑ 𝐹𝑣 (𝑡) = (𝑓3 (𝑡) − 𝑓1 (𝑡)) 𝑠𝑖𝑛
3
𝜋
∑ 𝐹𝑣𝑛 (𝑡) = −𝑓2 (𝑡) + (𝑓3 (𝑡) + 𝑓1 (𝑡))𝑐os
3
(2.9)
∑ 𝑇(𝑡) = (𝑓1 (𝑡) + 𝑓2 (𝑡) + 𝑓3 (𝑡)). 𝑑

Lực kéo trên mỗi bánh được ước lượng theo mô men kéo có thể xác định
được bằng cách sử dụng dòng điện của động cơ, được miêu tả bằng công thức
dưới đây:
𝑇𝑗 (𝑡)
𝑓𝑗 (𝑡) =
𝑟
𝑇𝑗 (𝑡) = 𝑙. 𝐾𝑡 . 𝑖𝑗 (𝑡) (2.10)
l: hệ số của hộp tốc độ
r [m]: bán kính bánh xe
Kt [Nm/A]: hằng số mô men động cơ
ij: dòng động cơ [A]: j= động cơ thứ j

2.2.3. Tính toán động học cho robot cho một số chuyển động cơ bản

Các chuyển động dùng để lập trình cho robot gồm có:
 14

Chuyển động tịnh tiến: Khi cho 2 bánh bất kỳ trong 3 bánh quay cùng lúc
theo 1 phương nào đó (2 bánh đó quay ngược chiều nhau) thì sẽ làm cho robot
chuyển động tịnh tiến (hình 2.1b)

Hình 2-3: Chuyển động tịnh tiến

Ví dụ: Khi cho 2 bánh xe V1 và V2 theo phương như hình vẽ 2.4 với r là bán
kính của bánh xe Omni. Giả sử 2 bánh xe cùng quay với vận tốc ω (quay
ngược chiều nhau) có vector vận tốc dài là ⃗⃗⃗
𝑉1 và ⃗⃗⃗
𝑉2 .

Tổng hợp 2 chuyển động trên ta có vector chuyển động tịnh tiến của
robot: (2.11)

𝑉𝑡𝑡 = 2𝑉1 . 𝑐os𝜑=2V2 . 𝑐os𝜑 = 2𝜔𝑟. 𝑐os300 = √3𝜔𝑟

Chuyển động quay: Khi cho 3 bánh xe quay cùng chiều thì sẽ làm cho
robot quay quanh tâm của nó (hình 2.5). Tùy vào góc quay mà độ lớn vận tốc
và hướng quay của các bánh sẽ khác nhau. Căn cứ vào vận tốc các bánh mà ta
có thể điều khiển góc quay và vận tốc của robot.

Hình 2-4: Chuyển động quay

 15

2.3. Mô hình toán học hệ thống điều khiển

Dựa vào các tính toán động học cơ cấu Omni, góc chuyển động và vận tốc
của robot phụ thuộc vào tốc độ các bánh. Vì vậy bằng cách thiết lập tốc độ cho
trước ta có thể tính toán được tốc độ của các động cơ cơ cấu bánh omni.

Do đó bài toán được đưa về điều khiển tốc độ dùng phương pháp PID
cho từng động cơ.

2.3.1. Bộ điều khiển động cơ PID

Giới thiệu về bộ điều khiển PID

Một bộ điều khiển vi tích phân tỉ lệ (bộ điều khiển PID) 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
bộ điều khiển công nghiệp- bộ điều khiển PID được sử dụng phổ biến nhất
trong số các bộ điều khiển có phản hồi. Một bộ điều khiển PID tính toán một
giá trị ‘sai số’ là hiệu số giữa giá trị thông số đo được và giá trị mong muốn đạt
được. 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. Trong trường hợp không có kiến thức cơ bản về quá trình,
bộ điều khiển PID là sự lựa chọn tốt nhất. Tuy nhiên, để đạt được kết quả tốt
nhất, các thông số PID sử dụng trong tính toán phải điều chỉnh theo tính chất
của hệ thống- trong khi kiểu điều khiển là giống nhau, các thông số phải phụ
thuộc vào đặc thù của hệ thống.

Một bộ điều khiển PID bao gồm 3 khâu: khâu tỷ lệ (P), khâu tích phân (I),
khâu vi phân (D). Trong miền thời gian, bộ điều khiển PID được mô tả bằng
mô hình vào ra:
𝑡
𝑑𝑒(𝑡)
𝑢(𝑡) = 𝐾𝑝 𝑒(𝑡) + 𝐾1 ∫ 𝑒(𝑡) 𝑑𝑡 + 𝐾𝐷
0 𝑑𝑡

Trong đó u(t) là tín hiệu ngõ vào, e(t) là tín hiệu ngõ ra của bộ điều khiển.

Tín hiệu u sẽ được gửi tới cơ cấu chấp hành, và cho ra đầu ra y. Đầu ra y
sẽ được gửi đến cảm biến đo và ta lại có giá trị sai số e mới. Bộ điều khiển sẽ
 16

lại lấy giá trị sai số mới, tính toán tích phân, vi phân và lại đưa ra tín hiệu điều
khiển. Quá trình này sẽ được lặp đi lặp lại.

Hình 2-5 Mô hình bộ điều khiển PID

Đặc tính của các luật điều khiển P, I, D:

Luật điều khiển P có tác dụng làm giảm thời gian xác lập nhưng không thể
khử được sai số trạng thái xác lập.

Luật điều khiển I có tác dụng khử được sai số trạng thái, nhưng nó làm
giảm khả năng đáp ứng tức thời của hệ thống.

Luật điều khiển D có tác dụng tăng độ ổn định của hệ thống và giảm độ
quá điều chỉnh, và cải thiện khả năng đáp ứng tức thời.

Hiệu quả của từng luật điều khiển P, I và D lên hệ thống vòng kín được
tổng kết qua bảng sau:
Bảng 2-1: : Nhận xét về các thông số Kp, Ki, Kd

Thời gian Độ quá điều Thời gian

Thông số Sai số tĩnh
quá độ chỉnh đến ổn định

Kp Giảm Tăng Ít thay đổi Giảm

Ki Giảm Tăng Tăng Khử được

Kd Ít thay đổi Giảm Giảm Ít thay đổi

 17

2.3.2. Xây dựng mô hình động cơ điện một chiều dưới dạng hàm truyền

Động cơ điện một chiều có thể được mô hình thành mạch điện phần ứng
và rotor như hình vẽ:

Hình 2-6: Mô hình hóa động cơ DC

Trong đó:

J: Mô men quán tính của rotor

b: hệ số giảm chấn cơ khí

K=Kt=Ke: hằng số điện cơ

R: điện trở phần ứng

L: điện cảm phần ứng

V: điện áp vào

 : Vị trí trục quay

Ta có phương trình cân bằng điện áp phần ứng:

𝑑𝑖
𝑉 = 𝑒 + 𝑅𝑖 + 𝐿 (3.1)
𝑑𝑡

Trong đó e là sức điện động phần ứng

𝑒 = 𝐾𝑒 𝜃̇ = 𝐾𝜃̇

Phương trình (4.1) tương đương

𝑑𝑖
𝑅𝑖 + 𝐿 = 𝑉 − 𝐾𝜃̇ (3.2)
𝑑𝑡
 18

Với phần rotor, theo định luật II Newton ta có:

𝐽𝜃̈ + 𝑏𝜃̇ = 𝑇 (3.3)

Mà mô men trên trục T có quan hệ với dòng điện phần ứng I thông qua hằng
số Kt:

𝑇 = 𝐾𝑡 𝑖 = 𝐾𝑖

Do đó:

𝐽𝜃̈ + 𝑏𝜃̇ = 𝐾𝑖 (3.4)

Biến đổi Laplace cho phương trình (4.2) và (4.4) ta được:

𝑠(𝐽𝑠 + 𝑏)𝜙(𝑠) = 𝐾𝐼(𝑠)

(𝐿𝑠 + 𝑅)𝐼(𝑠) = 𝑉 − 𝐾𝑠𝜙(𝑠)

Khử I(s) ở cả 2 phương trình ta được

𝐾(𝑉 − 𝐾𝑠𝜙(𝑠))
𝑠(𝐽𝑠 + 𝑏)𝜙(𝑠) =
(𝐿𝑠 + 𝑅)

Từ đây ta có thể rút ra được hàm truyền của hệ thống với đầu vào là điện áp V,
đầu ra là vận tốc góc của rotor  :
𝜃 𝐾
= (𝐽𝑠+𝑏)(𝐿𝑠+𝑅)+𝐾2 (3.5)
𝑉
 19

CHƯƠNG 3. TÍNH TOÁN, THIẾT KẾ ROBOT ĐIỀU KHIỂN THEO

QUỸ ĐẠO

3.1. Tính toán thiết kế hệ thống cơ khí

Bảng 3-1: Thông số yêu cầu robot

Thông số Ký hiệu Giá trị
Bán kính robot R 175 mm
Chiều cao thân h 15 mm
Tốc độ trung bình Vtb 0.4 m/s
Tốc độ tối đa Vmax 0.7 m/s
Khối lượng m 3 kg

Yêu cầu: Di chuyển linh hoạt và chính xác theo quỹ đạo được thiết kế sẵn,
hoặc chế độ điều khiển bằng tay.
3.1.1. Thiết kế phần đế và nắp robot omi

Với nhiệm vụ vừa là phần đế, nắp giống nhau đồng thời là bộ khung thân
của robot, các linh kiện đều được bắt vít vào đế. Phần đế phải đảm bảo độ bền
chịu tải trọng của toàn bộ thân robot 3 bánh. Do vậy chúng em đã chọn vật liệu
thép tấm không gỉ (Inox), có sẻ các rảnh để dễ dàng tùy chỉnh bắt vít các linh
kiện và đồng thời giảm trọng lượng của đế mà vẫn đảm bảo độ bền. Phần bích
để bắt động cơ vào khung được thiết kế dạng chữ U gắn liền với đế bằng
bulong- đai ốc để tạo sự liền mạch và chắc chắn.

- Vật liệu: thép không gỉ (độ dày 1.5mm)

- Khối lượng: 0.3 kg (đo trên phần mềm Solidworks)

- Phương pháp gia công: gia công máy cắt plasma CNC

Phần nắp có nhiệm vụ bảo vệ các bộ phận bên trong robot, được thiết kế
giống như phần đế với các rãnh có thể linh động lắp ghép với các module (Giá
chứa phôi, tay máy robot…).
 20

Hình 3-1: Hình ảnh thiết kế của đế và nắp

3.1.2. Thiết kế khớp nối U

Khớp nối U nhiệm vụ chính là gá động cơ và là khớp nối phần đế và nắp

lại với nhau đảm bảo độ vững chắc cho kết cấu khung.

Hình 3-2: Khớp nối U

- Vật liệu: thép không gỉ (inox)
- Khối lượng: 70g
- Phương pháp gia công: máy cắt Plasma CNC và chấn tạo biên dạng chữ
U
 21

3.1.3. Lựa chọn bánh xe omni

Bánh xe omni là một loại bánh xe đặc biệt, gồm một bánh xe lớn và các
bánh xe nhỏ có trục theo phương tiếp tuyến với bánh lớn và vuông góc với trục
bánh. Nhờ đó bánh xe có thể di chuyển theo phương song song với trục bánh
xe. Ưu điểm của bánh xe Omni là sự kết hợp chuyển động của mỗi bánh xe sẽ
tạo nên chuyển động đa hướng mà không cần có cơ cấu quay của robot. Từ ưu
điểm đó, đã có rất nhiều loại hình dạng bánh xe Omni ra đời, mỗi loại phù hợp
với những điều kiện hoạt động khác nhau.

Hình 3-3: Ví dụ các loại bánh xe trên thực tế

Ở đây, ta chọn loại bánh xe như sau:
- Đường kính bánh xe 58 mm
- Bánh xe omni nhựa ghép đôi
- Đường kính trục 13mm
- Tải trọng: 3 Kg
- Số lượng con lăn: 10
- Khối lượng bánh xe: 60g
- Chất liệu: Nhựa ABS và cao su
 22

Hình 3-4: Bánh xe omni mô hình

3.1.4. Khớp nối bánh xe với động cơ

- Nhiệm vụ kết nối bánh xe với trục động cơ

- Vật liệu: Hợp kim nhôm
- Đường kính lỗ trục: 6mm
- Bu lông: M4x15mm
- Khớp nối được thiết kế đi kèm với bánh Omni 58mm

Hình 3-5: Khớp nối trục động cơ và bánh.

 23

3.1.5. Tính toán 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ục tiêu: tốc độ mong muốn 0.7 m/s.

Tốc độ lớn nhất của 1 động cơ chính bằng Vtt=0.7= 𝜔𝑟 (m/s), r=0.05m suy
ra tốc độ góc bánh xe (tốc độ góc động cơ) là 𝜔=14 rad/s <=> tốc độ quay
động cơ 133,69 vòng/phút. Vậy tốc độ tối thiểu của động cơ phải là 133.69
vòng /phút.

Hình 3-6: Các lực tác dụng lên bánh xe

Mô-men quán tính tâm bánh xe:
1 (3.1)
𝐼 = 𝑚. 𝑅2
2

Cân bằng mô-men quanh tâm bánh xe, ta được:

𝜏 − 𝐹𝑚𝑠 . 𝑟 = 𝐼. 𝛾
(3.2)
⟺ 𝜏 = 𝐹𝑚𝑠 . 𝑟 + 𝐼. 𝛾

Điều kiện bánh xe lăn không trượt khi động cơ quay:

𝐹𝑚𝑠 ≤ 𝜇𝑁

⇔ 𝜏 ≤ 𝜇𝑁𝑟 + 𝐼𝛾

1 1
⇔ 𝜏 ≤ 𝜇 ( 𝑀 + 𝑚) 𝑔𝑟 + 𝑚𝑟 2 𝛾
2 2
 24 (3.3)

Thay vào phương trình mô-men trên ta được:

1 1
𝜏 = 𝜇 ( 𝑀 + 𝑚) 𝑔𝑟 + 𝑚𝑟 2 𝛾 (3.4)
2 2
Công suốt động cơ: 𝑃 = 𝜔. 𝜏

Trong đó:

- M=3 kg: khối lượng robot omni

- m= 0.35 kg: khối lượng bánh xe omni
- 𝜇=0.7: hệ số ma sát
- r=0.05m: bán kính bánh xe
- 𝛾: gia tốc góc bánh xe
- 𝐹𝑚𝑠 : lực ma sát
- 𝑔 = 9.81 m/s2: gia tốc trọng trường
- 𝜔: tốc độ góc bánh xe
Từ đó momen lăn không trượt:

1 2
0,72 1
𝜏 = . 0,35. 0,05 . + ( . 3 + 0.35) . 0,7.9,81.0,05 = 0,639 N. m
2 0,05 2

Với hệ số an toàn là 1,2. Ta tính được:

0.7
𝑃 = 1,2.0,639. = 10.7352 𝑊
0.05
Dựa vào thông số tốc độ, mô-men và công suất chúng em chọn loại động
cơ có hộp giảm tốc 12V DC Motor GA37V1 w/Encoder.

Bảng 3-2: Thông số kỹ thuật động cơ

Thông số kỹ thuật động cơ Giá trị

Tỷ số truyền động 33:1

Điện áp định mức 12v

Tốc độ không có tải 303 RPM

 25

Dòng khi không tải trọng 0.35A

Tốc độ định mức 204prm

Mô-men xoắn định mức 1.765 N.m

Dòng khi bị giữ 7A

Loại trục kiểu D

Kích thước 37D x 53L mm

Trọng lượng 205 g

Đường kính trục 6 mm

Chiều dài trục 15 mm

Điện áp định mức encoder 3.3 VDC

Loại Encoder Hall

33 xung/vòng ở trục động cơ và

Độ phân giải
363 xung/vòng ở trục hộp số.

Hình 3-7: Các chân kết nối của Encoder

Sơ đồ chân Encoder:
- M1: Chân cấp nguồn cho động cơ, tối đa 12 VDC.
- GND: Chân Mass 0V của Encoder.
 26

- C1: Kênh A của Encoder, xuất xung mức tín hiệu 3.3 VDC.
- C2: Kênh B của Encoder, xuất xung mức tín hiệu 3.3 VDC.
- 3.3V: Nguồn dương 3.3VDC cho Encoder hoạt động.
- M2: Chân cấp nguồn cho động cơ, tối đa 12VDC.

Hình 3-8: Hình ảnh mô hình của động cơ

Hình 3-9: Mô hình thiết kế trên phần mềm Solidworks

3.2. Tính toán, thiết kế hệ thống điện, điện tử

Sơ đồ khối hệ thống điện tổng quan:

 27

Hình 3-10: Sơ đồ khối hệ thống

3.2.1. Lựa chọn vi điều khiển trung tâm

Arduino đã và đang được sử dụng rất rộng rãi trên thế giới, và ngày càng
chứng tỏ được sức mạnh của chúng thông qua vô số ứng dụng độc đáo của
người dùng cộng đồng nguồn mở (open-soure). Hiện nay ở Việt Nam arduino
cũng đang được phát triển và ứng dụng vào học tập nghiên cứu rất phổ biến.

Ở đây, chúng em chọn Arduino Mega 2560 R3 làm bộ điều khiển cho hệ
thống, với các ưu điểm như mạnh mẽ với bộ nhớ flash lớn, số chân nhiều hơn
và cùng số lượng shield hỗ trợ không hề nhỏ. Arduino Mega đã được đưa vào
các dự án lớn hơn như xử lý thông tin nhiều luồng, điều khiền nhiều động cơ,
xe điều khiển từ xa, LED cube hay còn mở rộng cánh cửa với thế giới IOT.

Cấu tạo của Arduino Mega 2560 gồm các phần chính:

- Cổng USB: Đây là cổng giao tiếp dùng để nạp chương trình (Code) từ
máy tính lên bộ điều khiển Arduino, đồng thời nó cũng là giao tiếp serial
để truyền dữ liệu giữa vi điều khiển mà máy tính.
 28

- Jack nguồn: Để cung cấp nguồn cho Arduino ta có thể sử dụng nguồn từ
cổng USB máy tính, nhưng trong trường hợp đối tượng điều khiển di
chuyển, ta cần có nguồn riêng. Vì vậy, ta cần sử dụng nguồn riêng DC 9-
12V.
- Có 14 chân ra/vào đánh số thứ tự từ 0 đến 13, ngoài ra có một số chân
nối đất (GND) và một chân điện áp tham chiếu (AREF).
- Vi điều khiển AVR: đây là bộ xử lí trung tâm của bo mạch. Với mỗi mâu
Arduino khác nhau thì chip khác nhau. Riêng mạch Arduino Mega 2560
thì sử dụng ATMega 2560.
- Có một nút nhấn Reset và một đầu ICSP.
Ưu điểm của Arduino:
- Chạy trên đa nền tảng: Việc lập trình Arduino có thể thực hiện trên các
hệ điều hành khác nhau như Windows, Mac Os, Linux trên Desktop,
Android trên điện thoại.
- Ngôn ngữ lập trình đơn giản dễ hiểu.
- Nền tảng mở: Arduino được phát triển dựa trên nguồn mở nên phần
mềm chạy trên Arduino được chia sẻ dễ dàng và tích hợp vào các nền
tảng khác nhau.
- Mở rộng phần cứng: Arduino được thiết kế và sử dụng theo dạng
module nên việc mở rộng phần cứng cũng dễ dàng hơn.
- Đơn giản và nhanh: Mọi người dễ dàng chia sẻ mã nguồn với nhau mà
không lo lắng về ngôn ngữ hay hệ điều hành mình đang sử dụng.
- Có nhiều loại module dạng lắp ghép rất nhanh gọn, dễ sử dụng.
 29

Hình 3-11: Hình ảnh mô hình của Arduino

Thông số kỹ thuật của Arduino Mega 2560:

Bảng 3-3: Thông số kỹ thuật Arduino Mega 2560

Vi điều khiển Atmega 2560

5V DC (Từ cổng USB hoặc nguồn DC

Điện áp hoạt động
ngoài)

Tần số hoạt động 16 MHz

Dòng tiêu thụ Khoảng 30mA

Điện áp vào khuyên dùng 7-15V DC

Điện áp vào giới hạn 6-20 V DC

1 bộ (chân 50-53) dung với thư viện SPI

Giao tiếp SPI
của Arduino

Giao tiếp UART 4 bộ

Số chân Digital I/O 54 (15 chân hardware PWM)

Số chân Analog 16 (độ phân giải 10bit)

Dòng tối đa trên mỗi chân I/O 30 mA

Dòng ra tối đa (5V) 500 mA

 30

Dòng ra tối đa (3.3V) 50 mA

Bộ nhớ flash 256 KB, 8 KB sử dụng cho Bootloader

SRAM 8 KB (ATmega 16U2)

EEPROM 1 KB (ATmega 16U2)
Bề rộng 38 mm

Chiều dài 54.2 mm

3.2.2. Vi điều khiển Arduino Nano

Nhiệm vụ của Arduino Nano: Nhận và chuyển đổi tín hiệu đọc từ module
gia tốc MPU6050 đến bộ xử lý trung tâm là Arduino Mega 2560. Việc này giảm
gánh nặng tính toán và tránh nhiễu tín hiệu khi cùng đọc trên một bộ xử lý
Arduino Mega 2560.
Board Arduino Nano là một trong những phiên bản nhỏ gọn của board
Arduino. Arduino Nano có đầy đủ các chức năng và chương trình có trên
Arduino Uno do cùng sử dụng MCU ATmega328P. Nhờ việc sử dụng IC dán
của ATmega328P thay vì IC chân cắm nên Arduino Nano có thêm 2 chân
Analog so với Arduino Uno.
Bảng 3-4: Bảng thông số của Arduino Nano

IC chính: ATmega328P

IC nạp và giao tiếp UART: CH340

Điện áp hoạt động: 5V - DC

Điện áp đầu vào khuyên dùng: 7-12V - DC

Điện áp đầu vào giới hạn: 6-20V - DC

Số chân Digital I/O: 14 (trong đó có 6 chân PWM)

8 (độ phân giải 10bit, nhiều hơn

Số chân Analog:
Arduino Uno 2 chân)
 31

Dòng tối đa trên mỗi chân I/O: 40mA

Bộ nhớ flash: 32KB với 2KB dùng bởi bootloader

SRAM: 2KB

EEPROM: 1KB

Xung nhịp: 16MHz

Kích thước: 0.73" x 1.70"

Chiều dài: 45mm

Chiều rộng: 18mm

Khối lượng: 5g
3.2.3. Lựa chọn module bluetooth

Có nhiệm vụ nhận lệnh dạng dữ liệu ký tự text từ ứng dụng trên điện thoại
gửi tới bộ điều khiển robot.
Với ưu điểm thiết kế nhỏ gọn, tiện lợi, giao tiếp với vi điều khiển chỉ bằng
2 chân (Tx và Rx), vì vậy module bluetooth HC06 truyền dẫn và điều khiển từ
xa một cách dễ dàng.
Đặc điểm của module HC06 chỉ có thể chạy được 1 chế độ Slave (khác
với HC05 có thể hoạt động với chế độ Mater hoặc Slave). Điều này có nghĩa
là không thể chủ động kết nối từ vi điều khiển đến các thiết bị ngoại vi. Mà
cách kết nối là: phải sử dụng thiết bị ngoại vi (điện thoại thông minh, máy
tính laptop) để dò tín hiệu kêt nối Buletooth mà HC06 phát ra. Sau khi pair
thành công có thể gửi tín hiệu từ vi điều khiển đến các thiết bị ngoại vi này,
và ngược lại.
 32

Hình 3-12: Module bluetooth HC-06

Thông số kỹ thuật:
- Điện áp hoạt động: 3V3-5V DC
- Dòng điện tiêu thụ: 20-30mA
- Nhiệt độ hoạt động: -20~75°C
- Sử dụng chip: CSR Bluetooth V2.
- Cấu hình slave mặc định, không thay đổi được.
- Hỗ trợ tốc độ baud: 200, 2400, 4800, 9600, 19200, 38400, 57600,
115200.
- Kích thước: 28x15x2,35mm
- Giao tiếp: UART (TX, RX)
- Tốc độ:
• Bất đồng bộ: 2.1Mbps (Max)/160kbps
• Đồng bộ: 1Mbps/1Mbps
- Bảo mật: mã hóa và chứng thực
- Cấu hình mặc định:
• Tốc độ baud 9600, N, 8, 1
• Mật khẩu: 1234
 33

3.2.4. Lựa chọn driver

Hình 3-13: Hình ảnh thực tế của Driver L298

Yêu cầu:
- Dòng ra của driver phải lớn hơn dòng tải cực đại của động cơ, hoạt động ổn
định trong quá trình vận hành.
- Có khả năng đáp ứng tần số xung từ vi điều khiển.
- Kích thước nhỏ gọn, dễ lắp đặt.
- Giá thành rẻ.
Nhóm đã lựa chọn Driver L298: là một IC tích hợp nguyên khối gồm 2
mạch cầu H bên trong. Với điện áp làm tăng công suất nhỏ như động cơ DC
loại vừa và nhỏ.
Bảng 3-5: Thông số kỹ thuật Driver L298
Điện áp điều khiển 5 – 12V

Điện áp tín hiệu điều khiển 5 – 7V

Dòng điện tối đa cho mỗi mạch

2A (cho mỗi Motor)
cầu

Công suất tối đa 25W/cầu

 34

Mức điện áp Logic Low (0.3-1.5V), High (2.3-Vss)

Tần số điều khiển tối đa 20KHz

Kích thước 43x43x27 mm

Các chân của Driver L298:

Hình 3-14: Vị trí các chân của driver L298

- Chân 12V Power/Power GND: Chân cấp nguồn cho Driver L298.
- Chân 5V Power: chân cấp nguồn hoặc lấy nguồn 5V đầu ra biến đổi từ
chân 12V.
- A Enable/B Ennable: dung để điều khiển mạch cầu H trong L298. Nếu
ở mức - Logic “1” (nối với nguồn 5V) cho phép mạch cầu H hoạt động,
nếu ở mức - - Logic “0” thì mạch cầu H không hoạt động.
- Khi ENA=0: Động cơ không quay với mọi đầu vào
- Khi ENA=1: INT1=1; INT2=0: Động cơ quay thuận. INT1=0; INT2=1:
Động cơ quay nghịch.
- Với ENB tương tự với ENA với chân INT 3 và INT4.
- Input: In1, In2, In3, In4 dùng để Set logic hiht/low điều khiển chiều
quay động cơ.
 35

- Output A/ Output B: Đầu ra 2 động cơ A/B (đấu ngược chiều +/- sẽ đổi
chiều quay động cơ.
3.2.5. Cảm biến gia tốc

Yêu cầu: Sử dụng cảm biến, với độ chính xác và độ linh hoạt cao, dễ dàng
trong việc kết nối và viết chương trình cho Robot. Do vậy, nhóm đã chọn cảm
biến gia tốc và la bàn số MPU-6050.

Hình 3-15: Cảm biến gia tốc và la bàn số MPU-6050

MPU-6050 là cảm biến của hãng InvenSense, là một trong những giải
pháp cảm biến chuyển động đầu tiên trên thế giới có tới 6 trục (có thể mở rộng
tới 9) trục cảm biến tích hợp trong một chip duy nhất. MPU-6050 sử dụng công
nghệ độc quyền MotionFusion của InvenSense có thể chạy trên các thiết bị di
động, tay điều khiển…
MPU-6050 tích hợp 6 trục cảm biến bao gồm:
- Con quay hồi chuyển 3 trục (3-axis MEMS gyroscope)
- Cảm biến gia tốc 3 chiều (3-axis MEMS accelerometer)
Ngoài ra, MPU-6050 còn có 1 đơn vị tăng tốc phần cứng chuyên xử lý tín
hiệu (Digital Motion Processor - DSP) do cảm biến thu thập và thực hiện các
tính toán cần thiết. Điều này giúp giảm bớt đáng kể phần xử lý tính toán của vi
điều khiển, cải thiện tốc độ xử lý và cho ra phản hồi nhanh hơn. Đây chính là 1
điểm khác biệt đáng kể của MPU-6050 so với các cảm biến gia tốc và gyro
khác.
 36

MPU-6050 có thể kết hợp với cảm biến từ trường (bên ngoài) để tạo thành
bộ cảm biến 9 góc đầy đủ thông qua giao tiếp I2C.
Các cảm biến bên trong MPU-6050 sử dụng bộ chuyển đổi tương tự - số
(Anolog to Digital Converter - ADC) 16-bit cho ra kết quả chi tiết về góc quay,
tọa độ... Với 16-bit bạn sẽ có 2^16 = 65536 giá trị cho 1 cảm biến.
Tùy thuộc vào yêu cầu của bạn, cảm biến MPU-6050 có thể hoạt động ở
chế độ tốc độ xử lý cao hoặc chế độ đo góc quay chính xác (chậm hơn). MPU-
6050 có khả năng đo ở phạm vi:
- Con quay hồi chuyển: ± 250 500 1000 2000 dps
- Gia tốc: ± 2 ± 4 ± 8 ± 16g
Hơn nữa, MPU-6050 có sẵn bộ đệm dữ liệu 1024byte cho phép vi điều
khiển phát lệnh cho cảm biến, và nhận về dữ liệu sau khi MPU-6050 tính toán
xong.

Các thông số kĩ thuật khác của module MPU-6050

- Nguồn: 3-5V, trên module MPU-6050 đã có sẵn LDO chuyển nguồn
5V -> 3V
- Giao tiếp I2C ở mức 3V
- Khoảng cách chân cắm: 2.54mm
- Địa chỉ: 0x68, có thể cấp mức cao vào chân AD0 để chuyển địa chỉ
thành 0x69.

3.2.6. La bàn số

Loại la bàn: Cảm biến la bàn số HMC5883L.

Cảm biến la bàn số HMC5883L có kích thước nhỏ gọn sử dụng giao tiếp
I2C, được dùng để đo từ trường của trái đất nhằm xác định phương hướng với
độ chính xác lên đến 1 hoặc 2 độ, cảm biến có cách đo riêng biệt cho từng trục
và có thể kết hợp lại để tính toán 3D.
Cảm biến la bàn số HMC5883L còn có thể dùng để đo từ trường thô hoặc
các nguồn từ trường mạnh hơn gần nó, cảm biến có thể cảm nhận được nguồn
 37

từ trường xung quanh nó như của nam châm hoặc điện trường, khi phát hiện
được từ trường từ bên ngoài, nó có thể xác định được khoảng cách tương đối
hoặc chiều đến vật phát ra từ trường đó.

Hình 3-16: Cảm biến la bàn số HMC5883L

Thông số kĩ thuật:
- Cảm biến từ tính 3 trục.
- ADC 12bit kết hợp với các cảm biến AMR có độ ồn thấp đạt được độ
phân giải trường (giải đo) 2 milli-gauss -± 8 Gauss.
- Tích hợp tự kiểm tra.
- Điện áp hoạt động: 2.16-3.6V.
- Giao tiếp kĩ thuật số theo chuẩn I2C.
- Kích thước 14x13mm.
 38

Hình 3-17: Sơ đồ nguyên lí la bàn số HC8553L

3.2.7. Hệ thống nguồn

Yêu cầu: Cung cấp nguồn cho động cơ và hệ thống vi điều khiển, cảm
biến, la bàn số, các driver điều khiển động cơ. Nguồn đảm bảo cung cấp điện
có khả năng hoạt động liên tục 30 – 45 phút, nhỉ gọn và có thể tích hợp trên mô
hình.
Ở đây, chúng ta sẽ chọn nguồn cho hệ thống là loại Pin có thông số sau:

Bảng 3-6: Thông số kỹ thuật PIN 12V

Thông số Giá trị
Dung lượng 2200mAh
Điện áp 12V
Dòng xả 35c
Kích thước 105x32x23 mm
Khối lượng 170g
 39

Hình 3-18: Hình ảnh thực tế của Pin 12V

3.2.8. Mạch giảm áp

Để cấp nguồn cho các linh kiện điện tử như bộ điều khiển, mạch động lực,
la bàn số cần có một bộ chuyển đổi nguồn 12VDC từ Pin thành 5VDC, vì vậy
nhóm đã chọn mạch hạ áp để đáp ứng nguồn điện cho toàn hệ thống.

Hình 3-19: Mạch giảm áp

Tính năng: Mạch hạ áp DC-DC có Led hiển thị điện áp, dòng ra, trên mạch
có 2 biến trở điều chỉnh dòng và điện áp ra.
Thông số kĩ thuật:
- Điện áp vào: DC 5 – 36V
- Điện áp ra: DC 1.25 -32V
 40

3.2.9. Sơ đồ kết nối mạch điện

Hình 3.19: Sơ đồ đấu dây mạch điện hệ thống

3.3. Tính toán, thiết kế hệ thống điều khiển

3.3.1. Lưu đồ thuật toán điều khiển Mobile Robot

3.3.2. Thiết kế giao diện điều khiển

Thiết kế phần mềm điều khiển trên điện thoại Android

Chúng em chọn sử dụng công cụ phổ biến nhất để thiết kế phần mềm là
MIT App Inventor. Phương pháp thiết kế phần mềm là ngôn ngữ kéo và thả -
Drag and Drop rất phù hợp tạo ra các ứng dụng đơn giản trên điện thoại
Android.

a, Giới thiệu về MIT App Inventor:

MIT App Inventor dành cho Android là một ứng dụng web nguồn mở ban
đầu được cung cấp bởi Google và hiện tại được duy trì bởi Viện Công nghệ
 41

Massachusetts (MIT). Nền tảng cho phép nhà lập trình tạo ra các ứng dụng
phần mềm cho hệ điều hành Android (OS). Bằng cách sử dụng giao diện đồ
họa, nền tảng cho phép người dùng kéo và thả các khối mã (blocks) để tạo ra
các ứng dụng có thể chạy trên thiết bị Android.

Những tính năng của App Inventor là:

- Cho phép xây dựng nhanh chóng những thành phần cơ bản (components) của
một ứng dụng Android: Nút bấm, nút lựa chọn, chọn ngày giờ, ảnh, văn bản,
thông báo, kéo trượt, trình duyệt web.
- Sử dụng nhiều tính năng trên điện thoại: Chụp ảnh, quay phim, chọn ảnh, bật
video hoặc audio, thu âm, nhận diện giọng nói, chuyển lời thoại thành văn
bản, dịch.
- Hỗ trợ xây dựng game bằng các components: Ball, Canvas, ImageSprite.
- Cảm biến: đo gia tốc (AccelerometerSensor), đọc mã vạch, tính giờ, con quay
hồi chuyển (gyroscopeSensor), xác định địa điểm (locationSensor), NFC, đo
tốc độ (pedometer), đo khoảng cách xa gần với vật thể (proximitySensor).
- Kết nối: Danh bạ, email, gọi điện, chia sẻ thông qua các ứng dụng mạng xã
hội khác trên thiết bị, nhắn tin, sử dụng twitter qua API, bật ứng dụng khác,
bluetooth, bật trình duyệt.
- Lưu trữ: đọc hoặc lưu tệp txt, csv, sử dụng FusiontablesControl, tạo cơ sở dữ
liệu đơn giản trên điện thoại hoặc trên đám mây thông qua server tự tạo hoặc
Firebase.
- Điều khiển robot thông qua LegoMindstorms.

b, Thiết kế phần mềm

Giao diện quản lý Project: chúng ta có thể quản lý các project với giao
diện rất đơn giản dễ sử dụng.
 42

Hình 3-20: Giao diện Project MIT App Inventor

Giao diện thiết kế (design): Có rất nhiều công cụ để tạo và chỉnh sửa giao
diện của ứng dụng. Giao diện thiết kế gồm các phần chính sau:
- Palette: khối chức năng tạo giao diện cho ứng dụng.
- Viewer: màn hình giao diện ứng dụng.
- Components: các khối chức năng đã được thêm vào ứng dụng.
- Properties: khối chỉnh sửa thông số tương ứng.

Hình 3-21: Giao diện thiết kế ứng dụng

Giao diện lập trình (block): lập trình bằng ngôn ngữ lắp ghép các khối
block rất dễ hiểu và trực quan.
 43

Hình 3-22: Hình ảnh giao diện lập trình ứng dụng

Giao diện điện thoại: ứng dụng chụp màn hình trên điện thoại:

Hình 3-23: Giao diện điều khiển bằng điện thoại

 44

3.4. CHẾ TẠO, LẮP RÁP VÀ THỬ NGHIỆM MÔ HÌNH

3.4.1. Chế tạo hệ thống cơ khí.

Khung máy: là bộ phận để lắp ráp các chi tiết, linh kiện khác trong máy
nên được tính toán, chế tạo phù hợp với mô hình, như khả năng chịu tải, độ
vững chắc, đủ kích thước lắp ráp các linh kiện.
Khung bao gồm tấm đế và tấm bảo vệ ở phía trên, được kết nối với nhau
bằng khớp nối U. Các chi tiết được gia công, cắt bằng máy cắt Plasma CNC,
sau đó được gia công nguội để đảm bảo tính thẩm mĩ, an toàn cho người và
đảm bảo chính xác khi lắp ráp. Riếng khớp nối U, sau khi cắt Plasma còn được
chấn để tạo biên dạng.

Hình 3-24: Khung máy sau khi được lắp ráp

Bánh xe omni: mua tại cửa hàng hshop.vn

Hình 3-25: Bánh xe omni 58mm

Khớp nối trục động cơ: mua tại cửa hàng hshop.vn
 45

Hình 3-26: khớp nối trục động cơ và bánh omni

3.4.2. Hệ thống điện, điện tử.

Hệ thống điện, điện tử được chúng em sử dụng phụ thuộc vào yêu cầu,
công năng của Robot, chúng em tìm hiểu các loại linh kiện có trên thị trường
và tổng hợp để chọn sao cho tối ưu với mô hình cần đạt được.
Tổng hợp các linh kiện điện tử được sử dụng trong mô hình:
Bảng 3-7 :Các linh kiện điện, điện tử.
STT Tên linh kiện Hình ảnh Địa chỉ mua

Arduino
Banlinhkien.vn
1 Mega 2560

Arduino
2 Banlinhkien.vn
Nano

3 Driver L298 Hshop.vn

 46

Module
4 Banlinhkien.vn
bluetooth

Cảm biến
5 Banlinhkien.vn
MPU-6050

La bàn số
6 Banlinhkien.vn
HMC5883L

Banlinhkien.vn
7 Mạch hạ áp

Chotroihn.vn
8 Nguồn (Pin)
 47

3.4.3. Mô hình hoàn chỉnh

Hình 3-27: Mô hình hoàn chỉnh

Kết luận
 48

TÀI LIỆU THAM KHẢO

[1] A. Ashmore and N. Barnes, Omni-drive Robot Motion on Curved

Paths: The Fastest Path between Two Points Is Not a Straight-Line, AI 2002:
Advances in Artificial Intelligence, vol. 2557, 2002, pp. 225-236.

[2] T.A. Baede, Motion control of an omni directional mobile robot,

Traineeship report DCT 2006, 2006.

[3] P. Oliveira, Armando J. Sousa, A. Paulo Moreira and Paulo J. Costa:

Dynamical models for omni-directional robots with 3 and 4 wheels helder.

[4] Raul Rojas, Holonomic control of a robot with an omni directional

drive,
Kunstliche Intelligenz, 2006.

[5] Tham khảo từ nguồn youtuber Kris Temmerman, Omni wheel Robot,
2017.
 49

PHỤ LỤC

Chương trình điều khiển trên Arduino:

#include <PID_v1.h>
#include<SoftwareSerial.h>

int dir1 = 8;
int dir2 = 9;
int dir3 = 10;
int pwm1 = 5;
int pwm2 = 6;
int pwm3 = 7;

String tinhieu;

int goc;
float vx, vy;
int v1, v2, v3;
float Vtt = 137; //toc do dat 137 vong/phut
float Vquay = 137;

double kp = 5 , ki = 1 , kd = 0.01;//thông số PID

long temp;

volatile long encoderPos1 = 0;

volatile long encoderPos2 = 0;
volatile long encoderPos3 = 0;
 50

double input1 = 0, output1 = 0, setpoint1 = 0;

PID myPID1(&input1, &output1, &setpoint1, kp, ki, kd, DIRECT);
double input2 = 0, output2 = 0, setpoint2 = 0;
PID myPID2(&input2, &output2, &setpoint2, kp, ki, kd, DIRECT);
double input3 = 0, output3 = 0, setpoint3 = 0;
PID myPID3(&input3, &output3, &setpoint3, kp, ki, kd, DIRECT);

void setup() {
pinMode(dir1, OUTPUT);
pinMode(dir2, OUTPUT);
pinMode(dir3, OUTPUT);
pinMode(pwm1, OUTPUT);
pinMode(pwm2, OUTPUT);
pinMode(pwm3, OUTPUT);

pinMode(2, INPUT_PULLUP); // định dạng encoder cổng

A1
pinMode(11, INPUT_PULLUP); // định dạng encoder cổng
B1
pinMode(3, INPUT_PULLUP); // định dạng encoder cổng
A2
pinMode(12, INPUT_PULLUP); // định dạng encoder cổng
B2
pinMode(4, INPUT_PULLUP); // định dạng encoder cổng
A3
pinMode(13, INPUT_PULLUP); // định dạng encoder cổng
B3
 51

attachInterrupt(0, encoder1, FALLING); // cập nhật vị trí encoder1

attachInterrupt(0, encoder2, FALLING); // cập nhật vị trí encoder2
attachInterrupt(0, encoder3, FALLING); // cập nhật vị trí encoder3
TCCR1B = TCCR1B & 0b11111000 | 1; // cài đặt 31KHz PWM ngăn
nhiễu động cơ

myPID1.SetMode(AUTOMATIC);
myPID1.SetSampleTime(1);
myPID1.SetOutputLimits(-255, 255);

myPID2.SetMode(AUTOMATIC);
myPID2.SetSampleTime(1);
myPID2.SetOutputLimits(-255, 255);

myPID3.SetMode(AUTOMATIC);
myPID3.SetSampleTime(1);
myPID3.SetOutputLimits(-255, 255);

Serial.begin(9600); // Kết nối bluetooth module ở tốc độ 9600

}
void encoder1() //đọc tốc độ động cơ từ cổng encoder B1
{
if (digitalRead(11) == HIGH) encoderPos1++;
else encoderPos1--;
}
void encoder2() //đọc tốc độ động cơ từ cổng encoder B2
{
 52

if (digitalRead(12) == HIGH) encoderPos2++;

else encoderPos2--;
}
void encoder3() //đọc tốc độ động cơ từ cổng encoder B3
{
if (digitalRead(13) == HIGH) encoderPos2++;
else encoderPos2--;
}
void quaythuan()
{
analogWrite(pwm1, output1);
analogWrite(pwm2, output2);
analogWrite(pwm3, output3);

digitalWrite(dir1, HIGH);
digitalWrite(dir2, HIGH);
digitalWrite(dir3, HIGH);
}
void quaynguoc()
{
analogWrite(pwm1, output1);
analogWrite(pwm2, output2);
analogWrite(pwm3, output3);

digitalWrite(dir1, LOW);
digitalWrite(dir2, LOW);
digitalWrite(dir3, LOW);
 53

}
void ngung()
{
digitalWrite(pwm1, LOW);
digitalWrite(pwm2, LOW);
digitalWrite(pwm3, LOW);
}
void tinhtien()
{
vx = cos(goc * PI / 180);
vy = sin(goc * PI / 180);

v1 = vx;
v2 = sqrt(3) / 2 * vy - 0.5 * vx;
v3 = sqrt(3) / 2 * vy + 0.5 * vx;

boolean dir1 = v1 < 0 ? true : false;

boolean dir2 = v2 < 0 ? true : false;
boolean dir3 = v3 < 0 ? true : false;

v1 = Vtt * abs(v1);
v2 = Vtt * abs(v2);
v3 = Vtt * abs(v3);
}
void loop()
{
if (Serial.available() > 0) // nếu có kết nối cổng serial
 54

{
while (1)
{
tinhieu = Serial.readStringUntil('w'); // đọc chuỗi dc gửi tới
if (tinhieu.startsWith("s"))
{
tinhieu.replace("s", ""); // tách chuỗi lấy tín hiệu analog
goc = tinhieu.toInt(); // chuyển tính hiệu thành kiểu int

setpoint1 = v1; //cài đặt tốc độ động cơ

input1 = encoderPos1; // nhận dữ liệu từ encoder1
myPID1.Compute(); // tính toán đầu ra dc1

setpoint2 = v2; //cài đặt tốc độ động cơ

input2 = encoderPos2; // nhận dữ liệu từ encoder2
myPID2.Compute(); // tính toán đầu ra dc2

setpoint1 = v3; //cài đặt tốc độ động cơ

input3 = encoderPos3; // nhận dữ liệu từ encoder3
myPID3.Compute(); // tính toán đầu ra dc3

tinhtien();
analogWrite(pwm1, output1);
analogWrite(pwm2, output2);
analogWrite(pwm3, output3);

delay(10);
 55

}
if (tinhieu.startsWith("T"))
{
setpoint1 = Vquay; //cài đặt tốc độ động cơ
input1 = encoderPos1 ; // nhận dữ liệu từ encoder1
myPID1.Compute(); // tính toán đầu ra dc1

setpoint2 = Vquay; //cài đặt tốc độ động cơ

input2 = encoderPos2 ; // nhận dữ liệu từ encoder2
myPID2.Compute(); // tính toán đầu ra dc2

setpoint1 = Vquay; //cài đặt tốc độ động cơ

input3 = encoderPos3 ; // nhận dữ liệu từ encoder3
myPID3.Compute(); // tính toán đầu ra dc3

quaythuan();
}

if (tinhieu.startsWith("N"))
{
setpoint1 = Vquay; //cài đặt tốc độ động cơ
input1 = encoderPos1 ; // nhận dữ liệu từ encoder1
myPID1.Compute(); // tính toán đầu ra dc1

setpoint2 = Vquay; //cài đặt tốc độ động cơ

input2 = encoderPos2 ; // nhận dữ liệu từ encoder2
myPID2.Compute(); // tính toán đầu ra dc2
 56

setpoint1 = Vquay; //cài đặt tốc độ động cơ

input3 = encoderPos3 ; // nhận dữ liệu từ encoder3
myPID3.Compute(); // tính toán đầu ra dc3

quaynguoc();
}

if (tinhieu.startsWith("o"))
{
ngung();
}
delay(10);
}
}
}
 57

Chương trình lập trình ứng dụng điều khiển trên điện thoại:
58