đề tài mobile robot AGV bám theo line

Mo b il e R o b ot

201 4

Mục Lục
PHẦN 1:
1.1

XÂY DỰNG ĐỀ BÀI ................................................................................3

Tổng quan ..........................................................................................................3

1.1.1

Mobile robot và AGV mobile robot ............................................................3

1.1.2

Lịch sử phát triển của AGV mobile robot ...................................................3

1.1.3

Phân loại AGV mobile robot.......................................................................4

1.1.4

Ứng dụng của Mobile robot và AGV mobile robot ....................................5

1.2

Xác định đầu bài ................................................................................................5

1.3

Chọn phương án sơ bộ .......................................................................................6

1.3.1

Cấu hình xe .................................................................................................6

1.3.2

Chọn loại động cơ .......................................................................................7

1.3.3

Chọn cảm biến sơ bộ ...................................................................................8

1.3.4

Chọn giải thuật điều khiển ..........................................................................8

1.4

Phân công công việc ..........................................................................................9

1.5

Biểu đồ Gainn ....................................................................................................9

PHẦN 2: TIẾN HÀNH THỰC HIỆN ĐỀ TÀI ...........................................................10

2.1

Thiết kế phần cơ ...............................................................................................10

2.1.1

Cấu hình xe ...............................................................................................10

2.1.2

Động cơ (motor) ........................................................................................10

2.2

Mô hình hóa .....................................................................................................13

2.2.1

Mô hình hóa robot bám line ......................................................................13

2.2.2

Tính toán các thông số để thiết kế bộ điều khiển ......................................14

2.2.3

Kết quả mô phỏng .....................................................................................15

2.3

Thiết kế phần điện và cảm biến .......................................................................18

2.3.1

Thiết kế hệ thống điện ...............................................................................18

2.3.2

Cảm biến ...................................................................................................18

2.4

Giải thuật lập trình ...........................................................................................22

2.4.1

Sơ đồ truyền tính hiệu ...............................................................................22

2.4.2

Sơ đồ giải thuật .........................................................................................23

2.4.3

Vi điều khiển trung tâm.............................................................................23

1


Mo b il e R o b ot

201 4

2.4.4

Giao tiếp Driver .........................................................................................23

2.4.5

Giao tiếp Pixy CMUcam5 .........................................................................24

2.5

Thực nghiệm và đánh giá kết quả ....................................................................25

2.5.1

Giai đoạn 1 ................................................................................................25

2.5.2

Giai đoạn 2 ................................................................................................26

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

2


Mo b il e R o b ot

201 4

PHẦN 1:

XÂY DỰNG ĐỀ BÀI

1.1 Tổng quan
1.1.1 Mobile robot và AGV mobile robot
Cùng với sự phát triển không ngừng của các ngành khoa học kỹ
thuật, các ngành công nghiệp cũng phát triển nhanh chóng. Việc áp dụng
các máy móc hiện đại vào sản suất là một yêu cầu không thể thiếu trong
các nhà máy nhằm tăng năng suất, tăng chất lượng và giảm giá thành sản
phẩm. Nằm trong sự phát triển đó, công nghệ chế tạo các Mobile robot
trên thế giới cũng đang phát triển nhanh chóng nhằm đáp các nhu cầu về
sản xuất, quốc phòng, nghiêng cứu… Mobile robot có thể thực hiện
những công việc với năng suất cao, hiệu quả và có thể ở trong những môi
trường mà con người không thể thực hiện được.
Trong các dạng Mobile robot, thì AGV (Automatic guided vehicle)
mobile robot có nhiều ứng dụng trong thực tế, phổ biến ở các nhà máy, bến
cảng, hệ thống giao thông…những nơi cần sự vận chuyển liên tục theo một
tuyến đường cố định, năng suất cao và làm việc được trong thời gian dài.
1.1.2 Lịch sử phát triển của AGV mobile robot
AGV mobile robot đầu tiên được phát minh bởi Barrett Electronics
vào năm 1953 ở Mỹ . Lúc đầu nó chỉ là một chiếc xe kéo nhỏ chạy theo một
đường dẫn, dùng các cảm biến dò theo từ trường. AGV tồn tại ở mức này
cho đến giữa những năm 70. Công nghệ lúc này cho phép mở rộng khả
năng và tính linh hoạt của AVG. Xe không chỉ còn được dùng để kéo rơ
moóc trong kho, mà còn được sử dụng trong quá trình sản xuất, làm việc,
và các hệ thống lắp ráp ô tô.
Ngày nay, AGV đã trở nên phổ biến và có nhiều ứng dụng hiệu quả,

thay thế được sức lao động của con người, đặc biệt là ở các nước tiên tiến.
Ở nước ta, công nghệ chế tạo AGV mobile robot và việc ứng dụng
chúng vào sản xuất mới chỉ ở giai đoạn đầu và đây cũng là một lĩnh vực
hứa hẹn nhiều tiềm năng trong tương lai. Muốn phát triển công nghiệp
hóa, hiện đại hóa thì việc ứng dụng robot vào sản xuất và đời sống là yếu
tố cần thiết.

3


Mo b il e R o b ot

201 4

1.1.3 Phân loại AGV mobile robot
Theo chức năng: có 4 kiểu AGV:


Xe kéo (Towing machine): xe kéo xuất hiện đầu tiên và bay giờ vẫn
còn thịnh hành. Loại này có thể kéo nhiều loại toa hàng có khối lượng
khác nhau.



Xe chở (Unit Load Machine): xe chở được trang bị các tầng khay
chứa để chuyên chở.



Xe đẩy (Cart Vehicle): xe đẩy có tính linh hoạt cao và rẻ tiền. Chúng

được sử dụng để chuyên chở vật liệu và các hệ thống lắp ráp.



Xe nâng (Fork Vehicle): có khả năng nâng các tải trọng đặt trên sàn
hoặc trên các bục cao hay các khối hàng đặt trên giá.

Theo dạng đường đi : theo dạng đường đi thì AGV chia làm các loại như
sau:


Loại không chạy theo đường dẫn (Free path navigation): có thể di
chuyển đến các vị trí bất kỳ trong không gian hoạt động. Đây là loại
xe AGV có tính linh hoạt cao được định vị vị trí nhờ các cảm biến
con quay hồi chuyển (Gyroscop sensor) để xác định hướng di chuyển,
cảm biến laser để xác định vị trí các vật thể xung quanh trong quá
trình di chuyển, hệ thống định vị cục bộ (Local navigation Location)
để xác định tọa độ tức thời,…Việc thiết kế loại xe này đòi hỏi công
nghệ cao và phức tạp hơn so với các loại AGV khác.



Loại chạy theo đường dẫn (Fixed path navigation) :Xe AGV thuộc
loại này được thiết kế chạy theo các đường dẫn định sẵn gồm các loại
đường dẫn như sau:
-

Đường dẫn từ: là loại đường dẫn có cấu tạo là dây từ (Magnetic
wire) chôn ngầm dưới nền sàn. Khi di chuyển, nhờ có các cảm
biến cảm ứng từ mà xe có thể di chuyển theo đường dây dẫn.

Loại đường dẫn này không nằm bên trên mặt sàn nên có mỹ
quan tốt, không ảnh hưởng đến các công việc vận hành khác.
Tuy nhiên khi sử dụng phải tiêu tốn năng lượng cho việc tạo từ

4


Mo b il e R o b ot

201 4

tính trong dây, đồng thời đường dẫn là cố định và không thể thay
đổi được.
-

Đường ray dẫn: xe AGV được chạy trên các ray định trước trên
mặt sàn. Loại này chỉ sử dụng đối với những hệ thống chuyên
dụng. Nó cho phép thiết kế xe đơn giản hơn và có thể di chuyển
với tốc độ cao nhưng tính linh hoạt thấp.

-

Đường băng kẻ trên sàn: xe AGV di chuyển theo các đường
băng kẻ sẵn trên sàn nhờ các loại cảm biến nhận dạng vạch kẻ.
Loại này có tính linh hoạt cao vì trong quá trình sử dụng người ta
có thể thay đổi đường đi một cách dễ dàng nhờ kẻ lại các vạch
dẫn. Tuy nhiên khi sử dụng, các vạch dẫn có thể bị bẩn hay hư
hại gây khó khăn cho việc điều khiển chính xác xe.

1.1.4 Ứng dụng của Mobile robot và AGV mobile robot

AGV đã được sử dụng rộng rãi trong nhiều lĩnh vực công nghiệp và
dịch vụ như giao thông vận tải, quân sự, an ninh, không gian, hộ gia đình,
văn phòng tự động hóa và các hệ thống phòng thí nghiệm khoa học…
Ví dụ như trong môi trường độc hại, nguy hiểm, Robot Pioneer được
thiết kế để dò tìm và kiểm tra nồng độ phóng xạ trong thảm họa Chernobyl.
Hay những nơi mà con người không thể tới được, Robot MBARI’s ALTEX
AUV hoạt động ở dưới đáy biển.
Trong quân sự: Robot TALON dùng trong quân sự được trang bị
súng, rađa dò tìm, di chuyển linh hoạt trên các loại địa hình.
Trong công nghiệp: AGV Mobile robot được sử dụng chủ yếu để di
chuyển các thiết bị, mang vác nguyên liệu và phụ kiện cần thiết.
Trong đời sống : Robot lau nhà RC 3000 có khả năng tự nhận biết
khu vực chưa được quét dọn dựa trên sensor trang bị trên nó. Các sensor
quang trang bị trên robot sẽ tự động đo độ bẩn của sàn nhà và đưa ra chế độ
làm việc cho robot…

1.2 Xác định đầu bài
5


Mo b il e R o b ot

201 4

Trong phạm vi đồ án Thiết kế hệ thống Cơ điện tử, chúng em thực hiện chế
tạo và điều khiển Mobile Platform to Track a Reference Line, một dạng khá phổ
biến và có nhiều ứng dụng cụ thể. Mục đích của đề tài là bước đầu tìm hiểu, tiếp
xúc với dạng Mobile robot này, áp dụng và kiểm chứng những kiến thức đã học
trong thực tiễn.

Thiết kế mobile robot dò line.
Đường line màu xanh trên nền đỏ, độ rộng line 3,5mm.
Bán kính cong 200mm.
Độ chính xác của robot (khả năng bám line) là ± 2mm.
Tốc độ tối đa của robot có thể đạt được là 0,5m/s.
Thời gian thực hiện 15 tuần.
Đường line dự kiến:

1.3 Chọn phương án sơ bộ
1.3.1 Cấu hình xe
Để có thể di chuyển theo line thì có rất nhiều phương án có thể được
lựa chọn cho mobile robot.

-

Wheeled robot: Robot di chuyển bằng bánh xe.

-

Tracked robot: Robot di chuyển bằng xích.

-

Legged robot: Robot di chuyển bằng chân.

-

Air – based robot: Robot di chuyển trên không.

6

Mo b il e R o b ot

201 4

-

Water – based robot: Robot di chuyển trong môi trường nước.

-

Combination/hybrid robot: Robot có cấu tạo hỗn hợp các kiểu
trên.

Mỗi phương án trên đều có những ưu và nhược điểm riêng. Vậy, ta
phải lựa chọn phương án hợp lí nhất với điều kiện thực tế, mục đích của đề
bài cũng như trình độ của các thành viên trong nhóm:

-

Trong thực tế hiện nay, nhất là tại Việt Nam, để đi từ A đến B thì
phương án di chuyển bằng bánh xe vẫn là lựa chọn tối ưu nhất,
với những ưu điểm như gọn nhẹ, ít ma sát (so với bánh xích), giữ
thăng bằng tốt (so với di chuyển bằng chân), đơn giản và dễ điều
khiển (so với Air – based robot và Combination robot), có nhiều
lựa chọn (2 bánh, 3 bánh hay 4 bánh…) và giá thành rẻ.

-

Mục đích của đề bài là thiết kế một mobile robot di chuyển trên
cạn (Water – based robot bị loại bỏ vì yếu tố này).

-

Các thành viên trong nhóm đều là sinh viên nên trình độ có giới
hạn, các phương án quá phức tạp sẽ khó có thể thực hiện được.

Với những lí do trên, phương án di chuyển bằng bánh xe trở nên thiết
thực nhất với những yêu cầu như đơn giản, gọn nhẹ, dễ điều khiển, giữ
thăng bằng tốt, di chuyển được trên nhiều dạng địa hình, an toàn và nhất là
giá thành rẻ.
Trong phương án di chuyển bằng bánh xe, ta cũng có nhiều sự lựa
chọn như hai bánh, ba bánh hay bốn bánh.
-

Hai bánh: Đơn giản về cơ cấu tuy nhiên phức tạp về mặt điều
khiển vì phải thêm yếu tố cân bằng.

-

Bốn bánh: Dễ điều khiển tuy nhiên cơ cấu lái phức tạp.

-

Ba bánh: Đơn giản về cả cơ cấu lẫn điều khiển.

Phương án lựa chọn cuối cùng: 3 - Wheeled robot.
1.3.2 Chọn loại động cơ

7


Mo b il e R o b ot

201 4

Có rất nhiều loại động cơ như DC, DC servo, AC servo, step motor,
BLDC. Mỗi động cơ có những ưu khuyết điểm riêng. Nhưng xét theo yêu
cầu của đề bài cũng như điều kiện thực tế, nhóm lựa chọn động cơ DC
servo với những ưu điểm dùng nguồn một chiều, điều khiển với độ tin cậy
cao hơn Step motor, moment tốt ở tốc độ cao và giá thành hợp lý.
1.3.3 Chọn cảm biến sơ bộ
Xét đến đòi hỏi về độ chính xác ± 2mm thì camera có thể đáp ứng
được, tùy giá thành cao nhưng camera là lựa chọn phù hợp với nhu cầu cần
tìm hiểu, học hỏi của nhóm. Vậy cảm biến được sử dụng là Pixy CMUCam5.
1.3.4 Chọn giải thuật điều khiển

Để điều khiển Mobile robot ta có các luật điều khiển sau:
- Fuzzy
- Adaptive control
- Lyapunov
- Neural Network
…
Từ việc sử dụng Camera xử lý ảnh, có thể xác định được 3 sai số
e1, e2, e3 với giá trị cụ thể. Điều khiển Mobile robot cũng không quá
phức tạp, với 2 động cơ độc lập, hệ robot không thay đổi và đường
line là cố định. Vậy nên nhóm sử dụng luật điều khiển Lyapunov để
tính toán từ 3 sai số thành vận tốc cần thiết của 2 bánh xe chủ động.

8


Mo b il e R o b ot

201 4

1.4 Phân công công việc
Vũ Quang Huy (trưởng nhóm): thiết kế phần điều khiển và tiến hành thực
nghiệm.
Trần Vũ Quang Trung: thiết kế phần điện và lấy tín hiệu cảm biến.
Trương Quang Bảo: thiết kế phần cơ.
Lê Khắc Trình: mô hình hóa và mô phỏng.

1.5 Biểu đồ Gainn

9


Mo b il e R o b ot

201 4

PHẦN 2: TIẾN HÀNH THỰC HIỆN ĐỀ TÀI
2.1 Thiết kế phần cơ
2.1.1 Cấu hình xe
Wheeled robot sẽ có 3 bánh, với 2 bánh chủ động, điều khiển độc lập
ở trục sau và bánh xe tự lựa ở phía trước. Ưu điểm giải quyết vấn đề cân
bằng của xe, có thể chuyển hướng bằng cách thay đổi tốc 2 động cơ, cơ cấu
đơn giản, dễ chế tạo.

Kích thước xe: 260 x 260 x 84mm.

Bánh chủ động phải

Bánh tự lựa

ω1

ω2

Bánh chủ động trái

Sơ đồ nguyên lý

Chú thích

2.1.2 Động cơ (motor)
Công suất động cơ được tính theo công thức: P = M.ω
Trong đó:

M: momen xoắn của động cơ
ω: tốc độ góc

Với vận tốc tối đa là 0.5m/s, đường kính bánh là 90mm, khối lượng xe
dự tính là 2kg ta tính được tốc độ góc của động cơ:
𝜔=

60. 𝑣
60.0,5
=

= 106 (𝑣ò𝑛𝑔/𝑝ℎú𝑡)
𝜋𝑑
𝜋. 90. 10−3

Để cho xe có thể chuyển động được thì lực kéo tiếp tuyến sinh ra ở
vùng tiếp xúc giữa bánh xe chủ động và mặt đường phải lớn hơn hoặc bằng
tổng các lực cản chuyển động, nghĩa là:
Ff + Fj ≤ F
Trong đó:

10


Mo b il e R o b ot

201 4

Lực cản lăn: Ff = f x G
Với: Trọng lượng phân bố trên hai bánh xe: G = 20N
Hệ số cản lăn: f = 0,015
[ Bảng II.1 – Lý thuyết ô tô máy kéo (Nguyễn Hữu Cẩn) ]
 Ff = 0,015.20 = 0,3 (N)
Lực cản quán tính: Fj = m.j
Với: Khối lượng của xe: m = 2kg
Gia tốc của xe: j = 0,5m/s2
 Fj = 2.0,5 = 1 (N)
Vậy ta suy ra:

0.3 + 1 ≤ F
1,3 ≤ F

1,3 x 0,045 ≤ M
0,06 ≤ M


6,36 ≤ P

Vậy ta phải chọn động cơ có công suất lớn hơn 6,36W
Căn cứ vào công suất sơ bộ và vận tốc tối đa của xe, chọn động cơ DC
servo encoder + hộp giảm tốc:
-

Tỷ số truyền 1: 34

-

Điện áp động cơ 12V

-

Dòng qua động cơ 1A

-

Vận tốc 4300prm

-

Encoder 334 xung

Đặt điện áp đầu vào cho động cơ 0 – 12V (xung từ 0 – 255), ghi lại

giá trị tốc độ quay của động cơ ở đồ thị bên dưới.
Dựa vào đồ thị, ta có nhận xét:
-

Khoảng điều khiển tuyến tính của động cơ là 50 – 200 xung.

-

Tốc độ 2 động cơ không bằng nhau khi cho cùng một xung điều
khiển, động cơ trái nhanh hơn động cơ phải.

11


Mo b il e R o b ot

201 4



Khoảng điều khiển của động cơ 50 – 200 xung, sử dụng điều
khiển vòng kín để loại bỏ sai số của động cơ.

Động cơ trái
70.00
60.00

Vòng/giây

50.00

40.00
30.00
20.00
10.00

10
20
30
40
50
60
70
80
90
100
110
120
130
140
150
160
170
180
190
200
210
220
230
240
250

0.00

Xung điều khiển

Động cơ phải
70.00
60.00

40.00
30.00
20.00
10.00
0.00

10
20
30
40
50
60
70
80
90
100
110
120
130
140
150

160
170
180
190
200
210
220
230
240
250

Vòng/giây

50.00

Xung điều khiển

12


Mo b il e R o b ot

201 4

2.2 Mô hình hóa
2.2.1 Mô hình hóa robot bám line



(𝑥,𝑦) : hệ tọa độ cố định.



(𝑋,𝑌) :hệ tọa độ gắn với mobile robot.



𝑥 ,𝑦 : tọa độ của tâm mobile robot so với hệ tọa độ (𝑥,𝑦).



𝑟 : bán kính bánh xe.



𝑏 :khoảng cách tâm quay của mobile robot đến bánh xe.



𝑀 điểm tham chiếu trên đường line.



𝑡 − 𝑡 đường tiếp tuyến của line tại điểm 𝑊.



𝜙𝑤 góc của tiếp tuyến t-t tại điểm tham chiếu 𝑊.



𝑣𝑟 vận tốc cố định của điểm tham chiếu 𝑊 trên đường line.



𝜙 góc di chuyển của robot so với phương ngang 𝑂𝑥.



𝑣 vận tốc dài của tâm robot (điểm cảm biến).



𝜔 vận tốc góc xoay của tâm mobile robot (điểm cảm biến).



𝜔𝑙𝑤 , 𝜔𝑟𝑤 : vận tốc góc của bánh trái và bánh phải.



𝑣𝑤 vận tốc dài của điểm tham chiếu 𝑊.

13


Mo b il e R o b ot

201 4



𝜔𝑤 vận tốc góc của điểm tham chiếu 𝑊.



𝑒1 ,𝑒2 ,𝑒3 các sai số.

2.2.2 Tính toán các thông số để thiết kế bộ điều khiển


Phương trình chuyển động của robot tracing line có thể được viết như
sau:
𝑥̇
𝑐𝑜𝑠𝜙
𝑦̇
[ ] = [ 𝑠𝑖𝑛𝜙
𝜙̇
0



Mối quan hệ giữa 𝑣,𝜔 với vận tốc quay của 2 bánh xe:
1

𝑏

𝜔𝑙𝑤
[𝜔 ] = [𝑟1
𝑟𝑤

𝑣
𝑟
−𝑏] [𝜔 ]

𝑟



0
𝑣
0] [𝜔] (1)
1

(2)

𝑟

Điểm tham chiếu 𝑊(𝑥𝑤 , 𝑦𝑤 )di chuyển với vận tốc không đổi 𝑣𝑟 trên
đường tham chiếu ,có tọa độ (𝑥𝑤 ,𝑦𝑤 ) và góc quay 𝜙𝑤 thỏa mãn
phương trình động học:
𝑥𝑤̇
𝑐𝑜𝑠𝜙𝑤
𝑦
̇
[ 𝑤 ] = [ 𝑠𝑖𝑛𝜙𝑤
𝜙𝑤̇
0



Sai số được xác định như sau:
𝑒1
𝑐𝑜𝑠𝜙
𝑒
[ 2 ] = [−𝑠𝑖𝑛𝜙
𝑒3
0



0 𝑣
𝑤
0] [𝜔 ] (3)
𝑤
1

𝑠𝑖𝑛𝜙
𝑐𝑜𝑠𝜙
0

0 𝑥𝑟 - 𝑥𝑤
0] [ 𝑦𝑟 - 𝑦𝑤 ]
1 𝜙𝑟 -𝜙𝑤

Ta cần thiết kế bộ điều khiển sao cho 𝑒1 → 0 ; 𝑒2 → 0 ; 𝑒3 → 0 khi 𝑡
→∞
𝑣𝑟 cos 𝑒3
𝑒1̇
−1

𝑒
̇
[ 2 ] = [𝑣𝑟 sin 𝑒3 ] + [ 0
𝑒3̇
𝜔𝑟
0



𝑒2 + l
𝑣
- 𝑒1 ] [𝜔]
−1

Ta chọn hàm Lyapunov:
1 2 1 2 1 − 𝑒32
𝑉 (𝑥) = 𝑒1 + 𝑒2 +
2
2
𝑘2
Điểm cân bằng: 𝑒1 = 𝑒2 = 𝑒3 = 0
𝑉(𝑥) ≥ 0: hàm xác định dương

14


Mo b il e R o b ot

201 4

̇ siṅ 𝑒3
𝑑𝑉(𝑥)
= 𝑒1 𝑒1̇ + 𝑒2 𝑒2̇ +
𝑒̇
𝑑𝑡
𝑘2 3
= 𝑒1 (−𝑣 + 𝑙𝑤 + 𝑣𝑟 cos 𝑒3 ) +
Ta chọn luật điều khiển để

𝑑𝑉(𝑥)
𝑑𝑡

sin 𝑒3
𝑘2

(- 𝜔 + 𝜔r + 𝑘2 𝑒2 𝑣𝑟 )

≤0

Luật điều khiển:
v = 𝑣𝑟 cos 𝑒3 + 𝑘1 𝑒1
𝜔 = 𝜔𝑟 + 𝑘2 𝑒2 𝑣𝑟 + 𝑘3 sin 𝑒3
Trong đó 𝑘1 𝑘2 𝑘3 là 3 số dương.
2.2.3 Kết quả mô phỏng

Fig 1. Đường line tham chiếu.

15

Mo b il e R o b ot

201 4

Fig 2. Mô phỏng mobile robot chạy bám theo line.

Fig 3. Vận tốc bánh xe trái và vận tốc bánh xe phải khi bám theo đường
line như mô phỏng.

16


Mo b il e R o b ot

201 4

Fig 4. Các sai số 𝑒1 ,𝑒2 , 𝑒3

17


Mo b il e R o b ot

201 4

2.3 Thiết kế phần điện và cảm biến
2.3.1 Thiết kế hệ thống điện

5V
GND

12V
GND

7805

Lipo Battery
1500 mAh
35C
PWM

L298N

PWM

Vcc
5V
GND
Pic
16F887
Pin
Encoder

DC
motor

Encoder

Pin
Pic

16F887

DC
motor

5V
GND
GND

2.3.2 Cảm biến
Thông số cảm biến:


Kích thước: 54 x 51 x 35 mm.



Trọng lượng: 27grams.



Phương thức giao tiếp có thể: UART, SPI, I2S, digital ouput, analog
ouput.



Độ phân giải cảm biến ảnh :720p HD(1280 x 800).



Tốc độ ghi ảnh 50/1s (640x400).



Ống kính loại : tiêu chuẩn M12.



Tầm quay : 75o theo phương ngang, 47o theo phương thẳng đứng.



Dòng : 140 mA.



Áp vào : 5V( dùng USB) hoặc từ 6V- 10V.
CMUcam5 có thể nhận dạng tốt với 7 màu: Đỏ, cam, vàng, xanh lá,

xanh nước biển đậm, xanh nước biển nhạt và tím.

18


Mo b il e R o b ot

201 4

Khoảng cách tối thiểu từ thấu kính tới vật để ảnh không bị nhòe là
2.8mm.

Ta kết nối sử dụng cổng giao tiếp ICSP trên chip vi xử lý Arduino nối
với port J2 trên cảm biến camera.
Để nhận diện màu ta sử dụng chương trình PixyMon. Sau khi khởi
động chương trình. Ta xác định màu cần nhận diện bằng cách vào Action
Set signature 1 và quét khối màu muốn nhận dạng (Đỏ, cam, vàng, xanh
lá, xanh nước biển đậm, xanh nước biển nhạt, và tím).
Ta được kết quả như hình dưới:

Sau khi kết nối với arduino, và nạp chương trình để xuất ra các giá trị:
-

x : tọa độ x của tâm block.

-

y : tọa độ y của tâm block.

-

W: bề rộng của block.

-

H : chiều cao của block.

19


Mo b il e R o b ot

201 4

Cách xác định tỷ lệ mm/pixel : ta dịch chuyển xe và đường line sao
cho hình ảnh đường line nằm gọn trong ô block. Sau đó ta đo độ rộng của
đường line đó và xác định độ rộng của ô block W. Lấy tỷ số hai số liệu ta
được tỷ lệ của mm/pixel.
Để xác định được dấu của góc quay của xe ta dùng cách chia đường
line hiển thị trên màn hình ra hai block. Bằng cách chia đổi thấu kính của
camera ra làm hai ta được kết quả như hình dưới:

Ta lấy được các thông số:
-

Block 0 (tính theo pixel)

W1,H1, X1, Y1.

-

Block 1 (tính theo pixel)

W2, H2, X2, Y2.

-

Tâm màn hình camera

I ( 160; 99 ) pixel.

-

Tỉ lệ (pixel/mm ) = 13.43.

20


Mo b il e R o b ot

201 4

-

Sai số e1 = 0.

-

Sai số e2 = ( 159 - XG )/13.43 (mm)
= (159 - (( X1 + W1/2 ) + ( X2 - W2/2 ))/2 )/13.43.

-

Sai số e3: tan(e3) = (X2 - X1) / (Y2 - Y1)
Nếu e3 rất bé  e3 = tan(e3) = ( X2 - X1 ) / ( Y2 - Y1 ) (rad).
Nếu X2 < X1  e3 < 0.
Nếu X2 > X1  e3 > 0.

-

Điều kiện chiếu sáng và chống nhiễu:
-

Sử dụng hệ thống đèn led để chiếu sáng cho camera.

-

Sử dụng đường line có nền là các màu tối để tránh hiện tượng
chói nền cho camera.

21


Mo b il e R o b ot

201 4

2.4 Giải thuật lập trình
2.4.1 Sơ đồ truyền tính hiệu

MCU

Block 0

Gearbox

Left wheel

Vận tốc trái

Aduino Mega 2560
Nhận tín hiệu MCU

Thuật toán
Lyapunov

PWM điều khiển

Xung PWM

Vận tốc phải

Pic 16F887
PID tốc độ

DC motor

Pic 16F887
PID tốc độ

Encoder

Block 1

PWM điều khiển

Encoder

DC motor

Xung PWM
Driver L298

Gearbox

Right wheel

22


Mo b il e R o b ot

201 4

2.4.2 Sơ đồ giải thuật
Arduino

Sai

Pic 16F887

CMU

Cổng giao tiếp
Uart

Có tín hiệu
Block

Tính sai số
Vận tốc

Đọc tín hiệu

Encoder

Đúng
Tính sai số
e1, e2, e3

Nhân hệ số
Kp, ki ,kd

Tính vận tốc
2 bánh

Tính xung PWM

Cổng giao tiếp
Uart

Điều khiển
Động cơ

2.4.3 Vi điều khiển trung tâm
Arduino Mega 2560 sử dụng chip ATmega2560. Nó bao gồm 54
digital input/output pins, 15 PWM output pins, 16 analog input pins. Điện
áp hoạt động 5V. Tốc độ xử lý 16MHz.
2.4.4 Giao tiếp Driver
Pin Input1: cấp xung PWM (C).
Pin Input2: cấp tín hiệu LOW (D).
Pin Enable1: tín hiệu tắt mở động cơ (Ven).

23

Mo b il e R o b ot

201 4

2.4.5 Giao tiếp Pixy CMUcam5


Thêm thư viện.
#include <Servo.h>
#include <Wire.h>
#include <PixyI2C.h>



Xác định tọa độ đất.
Pixy pixy; (khai báo phải được đặt ngoài hàm void setup() và void
loop())



Xác định đối tượng của CMU.
-

pixy.blocks[i].signature The signature number of the detected
object (1-7).

-

pixy.blocks[i].x The x location of the center of the detected
object (0 to 319).

-

pixy.blocks[i].y The y location of the center of the detected
object (0 to 199).

-

pixy.blocks[i].width The width of the detected object (1 to 320).

-

pixy.blocks[i].height The height of the detected object (1 to 200).

-

pixy.blocks[i].print() A member function that prints the detected
object information to the serial port.



Phương thức giao tiếp
-

Uart
Arduino TX kết nối pin 1 Pixy's I/O connector.
Arduino RX kết nối pin 4 of Pixy's I/O connector.
Arduino GND kết nối pin 6, 8 or 10 of Pixy's I/O

connector.

24


Mo b il e R o b ot

201 4

-

Pin 1 bằng 3.3V khi có vật thể được xác định, bằng 0V khi
không có vật thể nào được xác định (dòng 5mA).

-

Pin 3 xác định từ 0-3,3V, gồm 2 chế độ:
Mode 3 (x mode) trả về tọa độ x điểm trung tâm của vật thể
(0V – cạnh trái và 3,3V- cạnh phải).
Mode 4 (y mode) trả về tọa độ y điểm trung tâm của vật thể
(0V – cạnh dưới và 3,3V- cạnh trên).

2.5 Thực nghiệm và đánh giá kết quả
2.5.1 Giai đoạn 1
Trong giai đoạn đầu của quá trình chạy thử nghiệm, xe mobile robot
của nhóm không hoàn thành được vòng chạy, thường bị mất line một cách
đột ngột.
Phương hướng giải quyết: kiểm tra hoạt động của từng module một,
có thể thay thế bằng các module mới cùng loại để chạy thử.
Trình tự tiến hành kiểm tra: camera CMU => Arduino mega => Pic

16F887 => mạch driver => 2 động cơ => nguồn pin 12V => nguồn 5V
=> dây dẫn và jack cắm.
Nguyên nhân ghi nhận được:

-

Camera CMU nhận màu không chính xác, hệ thống chiếu sáng
không tốt

-

gây nhiễu tín hiệu.

Do nguồn 5V được chia sẽ từ nguồn pin 12V (nguồn công suất),
khi các bánh xe tăng tốc, nguồn pin bị sụt áp dẫn đến nguồn 5V
cũng sụt áp => vi điều khiển không nhận đủ mức điện áp cần
thiết => ngừng hoạt động => động cơ không đáp ứng và xe bị
mất bám line. Quá trình này xảy ra khá nhanh nên khó phát hiện,
trong khi phần lớn thời gian ta thấy nguồn 5V vẫn hoạt động
bình thường.

25