Đồ án Xây dựng robot di động tránh vật cản dựa trên các sensor siêu âm và sensor địa bàn

 luận tốt nghiệp 
ĐẠI HỌC QUỐC GIA HÀ NỘI 
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ 
Bạch Hoàng Giang 
XÂY DỰNG ROBOT DI ĐỘNG TRÁNH VẬT CẢN 
DỰA TRÊN CÁC SENSOR SIÊU ÂM VÀ 
SENSOR ĐỊA BÀN 
KHOÁ LUẬN TỐT NGHIỆP ĐẠI HỌC HỆ CHÍNH QUY 
Ngành: Điện tử - Viễn thông 
 Cán bộ hướng dẫn: TS. Trần Quang Vinh 
Đồng hướng dẫn: CN. Phạm Duy Hưng 
HÀ NỘI – 2005 
BẠCH HOÀNG GIANG K46ĐC 
1 
 luận tốt nghiệp 
Lời cảm ơn 
Trước tiên tôi xin chân thành cám ơn Thầy giáo TS.Trần Quang Vinh, 
cùng Người đồng hướng dẫn cử nhân Phạm Duy Hưng thuộc Khoa Điện Tử- 
Viễn Thông đã tận tình hướng dẫn tôi tìm hiểu và nghiên cứu khóa luận này. 
Tôi cũng tỏ lòng biết ơn tới các Thầy, cô giáo trong Trường Đại Học 
Công Nghệ - Đại Học Quốc Gia Hà Nội đã có công đào tạo, quan tâm, giúp đỡ 
tôi trong suốt quá trình học tập và hoàn thành khóa luận này. Đồng thời tôi 
cũng chân cảm ơn các cán bộ trong phòng Robot đã tạo mọi điều kiện cho tôi 
làm tốt phần thực nghiệm của mình. 
 Cuối cùng, tôi muốn cám ơn tới tất cả bạn bè, gia đình và người thân đã 
đông viên và giúp đỡ tôi trong quá trình học tập và hoàn thành khóa luận này. 
 Hà nội,ngày 1 tháng 6 năm 2005 
 Sinh viên 
BẠCH HOÀNG GIANG K46ĐC 
2 
 luận tốt nghiệp 
TÓM TẮT NỘI DUNG 
 Khóa luận này tập trung nghiên cứu các vấn đề liên quan đến xây dựng 
một robot di động tránh vật cản dựa trên các sensor siêu âm và sensor địa 
bàn.Trên cơ sở đó thực hiện nhiệm vụ chính là xây dựng một robot di động giải 
quyết 3 bài toán điều khiển cơ bản. Robot được xây dựng trong khóa luận này 
sử dụng vi điều khiển Basic Stamp 2sx, với một hệ sensor bao gồm: Sensor 
siêu âm và sensor địa bàn. Robot sử dụng motor một chiều được điều khiển 
thông qua mạch công suất. 
BẠCH HOÀNG GIANG K46ĐC 
3 
 luận tốt nghiệp 
Mục lục 
Giới thiệu 1 
Phần 1: Tổng quan về một số vấn đề liên quan đến xây dựng một 
robot di động thông minh tự động tránh vật...2 
Chương 1: Sơ lược về lịch sử ra đời và phát triển của robot.3 
 1.1. Lịch sử ra đời và phát triển3 
 1.2. Các thế hệ robot.4 
 1.3. Xu hướng phát triển của robot hiện đại 5 
Chương 2: Hệ sensor trong robot..7 
 2.1. Phân loại sensor.7 
 2.1.1. Sensor nội ..7 
 2.1.2. Sensor ngoại ..7 
 2.1.3. Sensor khoá 7 
 2.2. Một số loại sensor thường gặp...7 
 2.2.1. Sensor xác định khoảng cách .8 
 2.2.1.1. Phương pháp xác định khoảng cách bằng lượng giác.8 
 2.2.1.2. Phương pháp xác định khoảng cách bằng đo khoảng 
cách thời gian truyền song8
 2.2.2. Sensor phát hiện vật thể gần.10 
 2.2.2.1. Sensor siêu âm...10 
Chương 3: Cơ cấu chuyển động trong robot di động12 
 3.1. Phân loại motor điện12 
 3.1.1. Motor một chiều...12 
 3.1.2. Motor bước...12 
 3.2. Các đặc tính của motor13 
 3.2.1. Điện áp sử dụng và dòng tiêu thụ của motor...13 
 3.2.2. Tốc độ cực đại của motor....13 
BẠCH HOÀNG GIANG K46ĐC 
4 
 luận tốt nghiệp 
 3.2.3. Lực xoắn củamotor13 
 3.3. Điều khiển chuyển động của motor .14 
 3.3.1. Điều khiển motor DC.14 
 3.3.2. Điều khiển motor bước15 
Chương 4: Khảo sát một số loại sensor sử dụng trong robot di 
động.. 16 
 4.1. Sensor hồng ngoại..16 
 4.1.1. Sensor hồng ngoại sử dụng phát hiện màu đen trắng..16 
 4.1.2. Nghiên cứu các sensor hồng ngoại có mặt trên thị trường 
tích hợp cả bộ thu và phát trên cùng một vỏ...17 
 4.2. Sensor siêu âm18 
 4.2.1. Sensor siêu âm MUST01d...18 
 4.2.2. Sensor siêu âm Devantech SRF04...20 
 4.3. Giới thiệu về sensor địa bàn CMPS03 ...22 
Phần 2: Thực nghiệm thiết kế, xây dựng một robot di động tự động 
 tránh vật...25 
Chương 5: Khảo sát thiết kế bộ điều khiển robot..26 
 5.1. Chíp vi điều khiển Basic Stamp 2sx...26 
 5.2. Giới thiệu về ngôn ngữ BASIC STAMP28 
 5.2.1 Một số câu lệnh chính trong Basic Stamp30 
 5.3. Mạch điều khiển công suất động cơ...34 
 5.4. Kết cấu cơ khí của robot36 
 5.4.1. Sơ đồ thiết kế...36 
 5.4.2 Các thành phần khác của Robot37 
Chương 6: Robot di động tránh vật cản.38 
 6.1. Bài toán điều khiển robot tránh vật cản.38 
 6.2. Các trường hợp của vật cản trên đường đi của robot38 
BẠCH HOÀNG GIANG K46ĐC 
5 
 luận tốt nghiệp 
 6.2.1 Có một vật cản nằm trên đường đi của robot38 
 6.2.2. Trường hợp đặt 2 vật so le nhau..40 
 6.2.3. Trường hợp robot lách giữa 2 vật cản.41 
BẠCH HOÀNG GIANG K46ĐC 
6 
 luận tốt nghiệp 
GIỚI THIỆU 
Ngành khoa học công nghệ mới, tạo ra các sản phẩm robot và nghiên 
cứu ứng dụng chính hình thành trong những thập kỷ gần đây được gọi là 
Robotics. Trong Robotics có hầu hết các vấn để của “cơ-điện tử”. Thuật ngữ 
“cơ - điện tử “( mechatronics) thể hiện sự kết hợp giữa cơ học máy và điều 
khiển điện tử. Đồng thời sự phát triển của mechatronics cũng được phản ánh 
trong khoa học công nghệ robot. 
Một trong những tiêu chí đặc trưng cho robot là khả năng “lập trình 
được “(programable). Muốn có khả năng đó robot phải dùng đến máy tính hoặc 
các thiết bị khác có chức năng tương tự. Máy tính có vai trò như bộ não của 
robot. 
Ngày nay sự cải thiện của tốc độ máy tính đã tạo ra những bước trưởng 
thành đáng kể cho robot trong từng giai đoạn phát triển và đang mở ra những 
triển vọng trở thành những robot thông minh nhờ khả năng xử lý rất nhanh của 
bộ điều khiển dùng máy tính.Tuy nhiên khả năng hiện thực của robot này 
không chỉ phụ thuộc vào bộ điều khiển mà còn ở khả năng nhận biết của các 
“cơ cấu cảm nhận” và khả năng phản ứng kịp thời của cơ cấu “chân tay” chấp 
hành. 
Robot ngày càng đóng vai trò quan trọng trong công nghiệp và cuộc 
sống hàng ngày. Chúng sẽ thực hiện những công việc rất nhàm chán hoặc nguy 
hiểm, những công việc mà tốc độ và độ chính xác vượt quá khả năng của con 
người. 
Bản khoá luận này đề cập đến vấn đề xây dựng một robot di động tránh 
vật cản dựa trên các sensor siêu âm và sensor địa bàn. 
BẠCH HOÀNG GIANG K46ĐC 
7 
 luận tốt nghiệp 
PHẦN MỘT 
TỔNG QUAN MỘT SỐ VẤN ĐỂ 
LIÊN QUAN ĐẾN 
XÂY DỰNGMỘT ROBOT DI ĐỘNG 
TỰ ĐỘNG TRÁNH VẬT 
BẠCH HOÀNG GIANG K46ĐC 
8 
 luận tốt nghiệp 
CHƯƠNG 1 
SƠ LƯỢC VỀ LỊCH SỬ RA ĐỜI VÀ XU HƯỚNG 
PHÁT TRIỂN CỦA ROBOT 
Robot là loại máy có thể lập trình điều khiển tự động bằng máy tính 
(máy tính ở đây có thể được hiểu là một bộ vi điều khiển hoặc một máy tính 
PC) để thực hiện các di chuyển, cầm nắm các vật, hoàn thành các công việc 
dưới tác động của môi trường. Thường robot được sử dụng để thực hiện các 
công việc lặp đi lặp lại, các công việc dễ gây nhàm chán, nó cho các kết quả 
chính xác, nhanh hơn, rẻ hơn nếu được thực hiện bởi con người. 
1.1 Lịch sử ra đời và phát triển 
Thuật ngữ Robot lần đầu tiên xuất hiện năm 1921 ở Tiệp Khắc trong tác 
phẩm R.U.R (Rossum’s Universal Robot) của nhà soạn kịch Karel Capek mang 
ý nghĩa người làm tạp dịch. Kể từ đó thuật ngữ này được sử dụng rộng rãi. 
Khái niệm máy tự động xuất hiện từ lâu với những viễn tưởng về người 
máy trong cuộc sống. Ngay sau chiến tranh thế giới lần thứ hai, nhiều công 
trình được bắt đầu tại các phòng thí nghiệm OakRidge và Argome để phát triển 
các máy cơ khí được điều khiển từ xa nhằm phục vụ trong các phòng thí 
nghiệm về vật liệu phóng xạ. Các cánh tay này được thiết kết mô phỏng một 
cách chính xác sự chuyển động của bàn tay và cánh tay con người. 
Trong giữa những năm 1950, bên cạnh các cánh tay cơ khí đó đã xuất 
hiện các cánh tay thuỷ lực và điện từ. Cũng trong những năm này, George 
C.Devol đã thiết kế một thiết bị có tên là thiết bị vận chuyền có khớp nối được 
lập trình (programmed artculated transfer device). Đây là một cánh tay máy mà 
hoạt động của nó có thể được lập trình thực hiện một chuỗi các bước chuyển 
động được xác định trong các câu lệnh trong chương trình. Phát triển xa hơn ý 
tưởng trên, Devol và Joseph F.Engelberger đã dẫn đường cho các robot công 
nghiệp đầu tiên được giới thiệu năm 1959 ở công ty Unimation. Thiết bị này sử 
dụng máy tính liên kết với tay máy nhằm dạy cho nó thực hiện các công việc 
khác nhau một cách tự động . 
Khi robot được lập trình đã tạo một sự kỳ lạ và tạo ra sức mạnh trong 
sản xuất, vào năm 1960 như một sự tất yếu, sự linh hoạt của hệ thống robot 
được nâng cao đáng kể thông qua hệ thống phản hồi từ các sensor. Tiếp đó 
BẠCH HOÀNG GIANG K46ĐC 
9 
 luận tốt nghiệp 
H.A.Ernst đã công bố sự ra đời và phát triển của bàn tay cơ khí được điều khiển 
bằng máy tính sử dụng các sensor xúc giác. Đây là sự xuất hiện đầu tiên về 
robot có khả năng thích ứng với môi trường. 
Vào cuối những năm 1960, Mc Carthy cùng bạn đồng nghiệp đã công bố 
sự phát triển của máy tính cùng với camera vô tuyến và microphone. Năm 1968 
Pieper đã nghiên cứu những vấn đề động học trong điều khiền robot bằng máy 
tính, trong khi đó năm 1971 Kanh và Roth đã phân tích về động lực học và giới 
hạn điều khiển tay máy. 
Trong suốt những năm 1970, một số lượng lớn các công trình nghiên 
cứu đã tập trung vào việc sử dụng các sensor ngoại để tăng sự tiện lợi và linh 
hoạt cho robot. Vào thời gian này công ty máy tính IBM đã chế tạo ra loại 
robot có các sensor xúc giác và sensor lực để lắp ráp các máy in gồm 20 cụm 
chi tiết . 
Một lĩnh vực được nhiều phòng thí nghiệm quan tâm là robot tự hành, 
robot di động. Nhiều công trình nghiên cứu đã thiết kế, xây dựng tạo ra các 
robot tự hành bắt chước chân người hoặc súc vật. 
Trong những thập kỷ 80 - 90 do sự phát triển mạnh mẽ của khoa học kỹ 
thuật, đặc biệt trong lĩnh vực kỹ thuật vi xử lý và công nghệ thông tin, số lượng 
robot đã gia tăng, giá thành giảm đi rõ rệt, tính năng có nhiều bước tiến vượt 
bậc. 
Ngày nay, chuyên ngành khoa học robot (robotics) đã trở thành một lĩnh 
vực rộng trong khoa học, bao gồm việc giải quyết các vấn đề về cấu trúc cơ cấu 
động học, động lực học, lập trình quỹ đạo, cảm biến tín hiệu, điều khiển chuyển 
động.v.v. 
1.2 Các thế hệ robot. 
Kể từ khi khái niệm robot ra đời, việc thiết kế và chế tạo robot đã trải 
qua nhiều giai đoạn với nhiều thế hệ khác nhau. Có 5 thế hệ robot ra đời kể từ 
năm 1960 . 
Thế hệ thứ nhất: Bao gồm các loại robot hoạt động lặp lại theo một chu 
trình không thay đổi. Chương trình điều khiển có hai dạng: 
- Chương trình “cứng”, nghĩa là không thay đổi hoặc không sửa được 
trừ khi thay đổi phần cứng. 
BẠCH HOÀNG GIANG K46ĐC 
10 
 luận tốt nghiệp 
- Chương trình có thể thay đổi được thông qua các panel điều khiển 
hoặc thông qua máy tính. 
Các robot thế hệ này sử dụng cơ cấu điều khiển servo vòng hở (open-
loop nonservo controlled system ). Đây là hệ thống không sử dụng thông tin 
phản hồi từ môi trường về để điều khiển robot. 
Thế hệ thứ hai: Robot được trang bị các sensor cho phép robot giao tiếp 
với môi trường bên ngoài. Các thiết bị này thực chất là các bộ biến đổi năng 
lượng. Nó chuyển các đại lượng không điện thành đại lượng điện mà qua đó bộ 
điều khiển robot có thể biết được trạng thái của môi trường xung quanh nó. 
Nhờ các sensor này robot có thể chọn các phương án khác nhau một cách linh 
hoạt nhằm thích nghi với môi trường bên ngoài. Dạng robot với trình độ điều 
khiển này còn được gọi là robot điều khiển thích nghi cấp thấp. Đây gọi là cơ 
cấu điều khiển servo vòng kín (closed-loop servo controller system). 
Thế hệ thứ ba: các bộ điều khiển logic khả trình PLC (Programmable 
Logic Controller) được sử dụng trong robot với nhiều chức năng chuyên biệt . 
Thế hệ thứ bốn: Khác với PLC bị giới hạn trong chương trình của chúng, 
thế hệ robot này sử dụng các máy tính được trang bị các ngôn ngữ lập trình đặc 
biệt hoặc ngôn ngữ chuẩn như Basic, C, C++...., để tạo ra nhiều ứng dụng 
CAD/CAM và CIM hoặc chương trình không trực tuyến. 
Thế hệ thứ năm: Các bộ điều khiển của robot sử dụng trí tuệ nhân tạo 
(artificial intelligence). Robot được trang bị các kỹ thuật như nhận dạng tiếng 
nói, hình ảnh, xác định khoảng cách, cảm nhận đối tượng tiếp xúc (da nhân tạo) 
để xử lý, ra những quyết định hợp lý. Ngoài ra robot được trang bị mạng 
Neuron giúp nó có khả năng tự học, tự xây dựng kiến thức . 
1.3 Những xu hướng phát triển của robot hiện đại 
Các robot hiện đại sẽ có xu hướng tăng trong tương lai giúp con người 
có thể tạo ra các sản phẩm mới, bảo vệ cơ sở hạ tầng của thế giới, chăm sóc nhà 
cửa, mua bán. 
Một xu hướng quan trọng trong nghiên cứu và phát triển robot là phát 
triển các hệ thống máy vi cơ điện tử (MEMS) có kích thước nhỏ từ vài cm tới 
mm thậm chí nhỏ hơn µ m. Các robot rất nhỏ này có thể di chuyển vào trong 
mạch máu để phân phối thuốc hoặc làm vệ sinh mạch máu; chúng có thể di 
chuyển trong một máy tính lớn để chuẩn đoán các vấn đề xảy ra với máy tính. 
BẠCH HOÀNG GIANG K46ĐC 
11 
 luận tốt nghiệp 
Một xu hướng phát triển hiện nay là việc nghiên cứu phát triển trí tuệ 
nhân tạo, mạng neuron vào trong robot nhằm tạo ra các robot thông minh, có 
khả năng thích nghi với môi trường xung quanh như con người. 
BẠCH HOÀNG GIANG K46ĐC 
12 
 luận tốt nghiệp 
CHƯƠNG 2 
HỆ SENSOR TRONG ROBOT 
Khi được trang bị một hệ thống sensor robot sẽ có khả năng thích ứng 
với sự thay đổi của môt trường xung quanh. Đây cũng là một chỉ tiêu quan 
trọng để đánh giá một robot có thông minh hay không. Các sensor đều là bộ 
biến đổi năng lượng, nó biến năng lượng từ dạng này sang dạng khác, thường 
là biến đổi tín hiệu từ các đại lượng không điện thành các đại lượng điện. Tín 
hiệu này được phản hồi về bộ điều khiển, thông qua nó bộ điều khiển có các 
quyết định cho phù hợp. 
2.1. Phân loại sensor 
Các sensor sử dụng trong robot được chia thành 3 loại: Sensor nội 
(internal sensor), sensor ngoại (external sensor), sensor khoá (interlock sensor). 
2.1.1. Sensor nội 
Được đặt trong bản thân robot, nó sử dụng các thiết bị về cơ khí, điện, 
điện tử hoặc thuỷ lực để nhận các thông tin phản hồi về vị trí của các bộ phận 
trên robot. Sensor nội có thể đơn giản là các công tắc giới hạn nhưng cũng có 
thể phức tạp như đĩa lập mã quang dùng để điều khiển motor. 
2.1.1 Sensor ngoại 
Sensor ngoại là loại sensor giúp robot giao tiếp với môi trường bên 
ngoài như sensor thị giác giúp robot quan sát, sensor xác định khoảng cách 
giúp robot ước lượng được khoảng cách tới đối tượng, sensor xúc giác giúp 
robot có cảm giác khi cầm nắm các vật. Các sensor này sẽ phản ánh các thông 
tin môi trường xung quanh tới robot. 
2.1.3 Sensor khoá 
Đây là loại sensor được dùng trong các tình huống khẩn cấp để bảo vệ 
robot . 
Sau đây xin được đề cập đến một số loại sensor thường được sử dụng . 
2.2. Một số sensor thường gặp 
Trong thực tế nghiên cứu robot, có rất nhiều loại sensor được sử dụng 
cho các mục đích khác nhau. Sau đây, xin giới thiệu một số sensor thường được 
sử dụng trong kĩ thuật robot. 
BẠCH HOÀNG GIANG K46ĐC 
13 
 luận tốt nghiệp 
2.2.1 Sensor xác định khoảng cách (Range sensor) 
Đây là loại sensor được sử dụng giúp robot có thể ước lượng khoảng 
cách từ vị trí đặt sensor (nằm trên robot ) tới đối tượng. Có nhiều cách xác định 
khoảng cách tới đối tượng. Sau đây trình bày hai cách xác định thông dụng 
nhất đó là: 
2.2.1.1 Phương pháp xác định khoảng cách bằng lượng giác 
Phương pháp này sử dụng một nguồn phát sáng phát ra một chùm tia 
hẹp tới đập vào bề mặt vật thể. Do phản xạ, tia sáng chuyển hướng tới bộ thu 
được đặt cách bộ phát một khoảng B, góc phát tia sáng so với phương ngang là 
α ( hình 2.1). 
Đối 
tượng 
D 
Nguồn phát α
Nguồn 
thu 
 B 
Hình 2.1 Sensor đo khoảng cách sử dụng phương pháp lượng gi ... oops = b0 
FOR loops=1 to 3 
BẠCH HOÀNG GIANG K46ĐC 
37 
 luận tốt nghiệp 
debug “Go!”,cr 
NEXT 
debug “Done” 
Chúng ta sẽ có 
Go! Go! Go! Done. 
Cặp lện For ...Nẽt sử dụng biến để đếm số vòng lặp. Lệnh For đặt giá trị 
ban đầu của biến: 
FOR loops=1 ... 
Phần sau câu lệnh For là ...to 3, thực chất là để thông báo cho lệnh Next. 
Lệnh Next thêm 1 vào biến (vòng lặp) và so sánh kết quả với giá trị cuối cùng 
được gửi từ lện For (trong trường hợp này bằng 3). Nếu giá trị biến nhỏ hơn 
hoặc bằng với kết quả cuối cùng, lệnh Nẽt làm cho vòng lặp của chương trình 
quay trở lại lệnh For. Nếu giá trị biến lớn hơn giá trị cuối cùng thì lệnh Next 
kết thúc vòng lặp và để chương trình chạy tiếp các câu lệnh khác. 
For...Next sẽ chạy các câu lệnh nằm giữa chúng và vẫn tiếp tục cho đến 
khi giá trị biến vượt quá giá trị cuối cùng. 
c) Cấu điều kiện If...Then 
Trong BASIC sử dụng câu điều kiện If...Then. Ví dụ: 
BS2 
x varbyte 
x=99 
IF x<100 then saySo 
debug “x is 100 or more” 
End 
saySo 
debug “x is less than 900” 
chương trình đưa ra x=99 nhỏ hơn 100. Thay đổi x=99 gán giá trị x bằng 
100 hoặc nhiều hơn (nhưng không lớn hơn 255, giá trị lớn nhất của byte) và trở 
lại chương trình. Bây giờ chương trình sẽ hiển thị thông điệp “x is 100 or 
more”. (x lớn hơn hoặc bằng 100). 
BẠCH HOÀNG GIANG K46ĐC 
38 
 luận tốt nghiệp 
Có hai phần trong câu lệnh If...Then. phần đầu là điều kiện- câu lệnh 
này là mối quan hệ giữa hai giá trị, như là x<100. Nếu câu lệnh If đúng thì vế 
thứ hai sẽ được thực hiện. Nếu câu điều kiện sai thì câu lẹnh If...then sẽ không 
lam gì cả. nó cho phép chương trình tiếp tục chạy các câu lệnh khác. 
Biểu 
tượng 
ý nghĩa 
= 
< 
> 
<= 
>= 
Bằng 
Nhỏ hơn 
Lớn hơn 
Không bằng 
Nhỏ hơn hoặc bằng 
Lớn hơn hoặc bằng 
Ví dụ: 
BS2 
Loops var byte 
Loops=0 
Repeat: 
Debug “looping:”, DEC loops,cr 
Loops=loops +1 
If loops <=5 then repeat 
Debug “Done:”, DEC loops 
d) Lệnh Gosub 
Các lệnh trong ngôn ngữ Stamp đều sử dụng hòm công cụ để xây dựng 
chương trình. Và thùng công cụ này nhỏ, chỉ có 36 lệnh trong BS2. trong một 
số trường hợp, nhóm lệnh sẽ rất hữu ích vì nó sẽ sử dụng nhiều hơn một lần 
trong chương trình đơn, ở đó ta sử dụng câu lệnh Gosub...Return. 
BẠCH HOÀNG GIANG K46ĐC 
39 
 luận tốt nghiệp 
Gosub là một biến thể của lệnh Goto, nó gọi chương trình con được bắt 
đầu bằng một nhãn và bao giờ cũng kết thúc bằng lệnh Return. Sau đây là một 
ví dụ có sử dụng chương trình con: 
BS2 
Loops var byte 
Debug “Subroutine Example” 
GOSUB mySub 
For loops=0 to 5 
Debug DEC loops 
GOSUB mySub 
Next 
end 
mySub: 
pause 500 
debug cls,”>” 
RETURN 
Chương trình con mySub sẽ làm tạm dừng chương trình Stamp một nửa 
giây (500/1000ths/sec), xóa màn hình Debug bằng lệnh cls, và ghi ra màn hình 
ký tự >. Khi chạy chương trình, chương trình con được đưa vào đầu chương 
trình và được gọi một lần trong mỗi bước của vòng lặp. 
5.3 Mạch điều khiển công suất động cơ. 
Mạch được thiết kế theo nguyên lý mạch cầu H để đảo chiều của động 
cơ DC. ở đây em sử dụng cặp transistor trường kênh P là IRF9540 và kênh N là 
IRF640. 
IRF9540: Nếu thế mở VGS = -10V thì dòng điện cực đại đi qua liên tục 
là 25 Ampe, dòng điện xung là 75 Ampe. Thế đánh thủng VGS là 20V. 
IRF640: Nếu thế mở VGS = +10V thì dòng điện cực đại đi qua liên tục 
là 18 Ampe, dòng điện xung là 75 Ampe. Thế đánh thủng VGS là 20V. 
Nguyên lý hoạt động của mạch. Ở cầu H thứ nhất khi không có tín hiệu 
điều khiển vào, hai transistor Q1 và Q2 đóng, lúc đó thế vào cực Gate của các 
BẠCH HOÀNG GIANG K46ĐC 
40 
 luận tốt nghiệp 
transistor công suất là mức cao, do đó Q5, Q6 mở, không có dòng điện đi qua 
động cơ. Khi có tín hiệu điều khiển vào, chẳng hạn chân 1, lúc đó Q1 mở, nó sẽ 
làm cho Q6 và Q9 cùng mở, Q5 và Q10 cùng đóng, dòng điện sẽ đi qua động 
cơ theo chiều thuận. Khi tín hiệu vào chân số 2, Q2 mở, dẫn đến Q5, Q10 mở 
còn Q6, Q9 đóng. Dòng điện sẽ đi qua động cơ theo chiều nghịch. 
Hình 5.4: Sơ đồ nguyên lý mạch điều khiển động cơ 
Tương tự khi không có tín hiệu điều khiển vào, hai transistor Q3 và Q4 
đóng, lúc đó thế vào cực Gate của các transistor công suất là mức cao, do đó 
Q7, Q8 mở, không có dòng điện đi qua động cơ. Khi có tín hiệu điều khiển vào, 
chẳng hạn chân 3, lúc đó Q3 mở, nó sẽ làm cho Q8 và Q11 cùng mở, Q7 và 
Q12 cùng đóng, dòng điện sẽ đi qua động cơ theo chiều thuận. Khi tín hiệu vào 
chân số 4, Q4 mở, dẫn đến Q7, Q12 mở còn Q8, Q11 đóng. Dòng điện sẽ đi 
qua động cơ theo chiều nghịch. 
Hình 5.5: Mạch công suất điều khiển động cơ 
BẠCH HOÀNG GIANG K46ĐC 
41 
 luận tốt nghiệp 
Điểm đặc biệt của mạch cầu này là có chế độ “phanh động “ (có nghĩa là 
khi ta ngắt không điều khiển động cơ nữa thì hai transistor ở nửa trên sẽ cùng 
mở, tức là hai cực của động cơ sẽ chập vào nhau, chống lại quán tính quay của 
động cơ ). Mạch này cũng có cơ chế tự bảo vệ, khi ta cung cấp tín hiệu điều 
khiển vào cả hai chân chiều tiến và lùi vẫn không xảy ra xung đột, chập cháy 
mạch mà vẫn là ở chế độ phanh động. 
5.4 Kết cấu cơ khí của robot 
5.4.1 Sơ đồ thiết kế 
Robot được liên kết bởi các thanh nhôm, có kích thước và kết cấu được 
chỉ ra ở hình 5.6 
 ắcqui 
Sensor 
địa bàn 
45cm 
39cm 
 sensor 
siêu âm 
Hình 5.6: Hình dạng và kích thước của robot 
Khung đế của robot hình chữ nhật, chiều dài 36cm, chiều rộng là 31cm, 
chiều cao tính từ mặt sàn tới sonar là 30cm. Bao gồm 3 bánh xe, 2 bánh phát 
động và một bánh bị động. 
BẠCH HOÀNG GIANG K46ĐC 
42 
 luận tốt nghiệp 
 Hình 5.7: Hình dạng robot nhìn từ phía trên 
Hình 5.8: Sơ đồ đế và các bánh của robot 
5.4.2 Các thành phần khác của Robot 
Robot sử dụng một nguồn nuôi là acqui 12V-5Ah để cung cấp nguồn 
điện cho các mạch điện. 
- Động cơ sử dụng cho robot là động cơ 12V DC. 
- Sensor siêu âm dùng để đo khoảng cách. 
- Compass Sensor 
BẠCH HOÀNG GIANG K46ĐC 
43 
 luận tốt nghiệp 
CHƯƠNG 6 
ROBOT DI ĐỘNG TRÁNH VẬT CẢN 
6.1 Yêu cầu bài toán đặt ra 
Khoá luận này nghiên cứu, xây dựng chương trình điều khiển robot thực 
hiện 3 bài toán : 
- Điều khiển robot đi thẳng. 
- Nếu gặp vật cản thì tránh. 
- Điều khiển robot trở về hướng đi trùng với hướng đi ban đầu. 
6.2 Các trường hợp của vật cản trên đường đi của robot 
Có 3 trường hợp chính khi gặp vật cản : 
6.2.1 Có một vật cản nằm trên đường đi của robot: 
Để biết được có vật cản trên đường đi của robot hay không ta dùng một 
sensor siêu âm xác định khoảng cách. Sensor siêu âm dùng ở đây là sensor 
must01d đã được nhắc đến ở trên. Nhờ việc chuẩn hoá khoảng cách ta có thể 
xác định được khoảng cách từ robot tới vật cản là bao nhiêu để có thể tránh vật. 
Vì robot có kích thước xác định nên ta có thể xác định được khoảng cách tối 
thiểu giữa robot và vật cản mà robot bắt đầu tránh. Để đơn giản cho bài toán ta 
giả sử vật cản là một khối hộp hình chữ nhật như hình 6.1. Ta cần xác định 
khoảng cách nhỏ nhất (Dmin) để robot bắt đầu tránh vật. 
Robot Vật cản 
α
αα 
L 
P 
Q R 
M
A 
H
O 
T
α
Hình 6.1: Trường hợp có 1 vật cản 
BẠCH HOÀNG GIANG K46ĐC 
44 
 luận tốt nghiệp 
Do kích thước của robot ta có thể xác định ngay từ ban đầu nên việc xác 
định Dmin sẽ trở nên đơn giản hơn. Khi robot quay thì 1 bánh tiến và 1 bánh 
lùi nên coi như tâm quay (O) ở giữa 2 bánh, và khi robot quay thì quay quanh 
đường tròn tâm O. Nên để cho cạnh của robot không chạm vào vật cản thì 
Dmin OA – OH.. Ta có thể lấy Dmin = OA - OH +≥ ∆d (∆d là một sai số). 
Robot 
Vật cản 
O 
Â
Hình 6.2 Xác định Dmin 
Sau khi xác định được khoảng cách ngắn nhất để robot bắt đầu quay thì 
điều khiển cho robot quay tránh vật. Nhưng còn 1 vấn để đặt ra là làm thế nào 
để robot biết được nó đã quay 1 góc là bao nhiêu để điểm A trên robot không 
chạm vào vật. Do sensor siêu âm dùng ở đây có góc mở bé nên khi robot quay 
được 1 góc α nào đó thì sensor không còn nhìn thấy vật cản nữa trong khi 
robot chưa thực sự thoát hẳn khỏi vật cản. Để xác định được điểm A trên robot 
đã thực sự thoát khỏi vật cản chưa ta có thể dùng 1 sensor siêu âm hoặc sensor 
hồng ngoại phát hiện màu đen trắng đặt tại điểm A. 
Nhưng trong khoá luận này em đã giải quyết vấn đề trên bằng cách tạo 
trễ. Tức là khi robot quay đến 1 góc mà sensor siêu âm không còn nhìn thấy vật 
nữa thì em cho robot quay thêm một góc nhỏ nữa (qua thực nghiệm có thể xác 
định được góc này). Sau khi robot đã quay 1 góc để tránh vật thì nó còn phải đo 
xem nó đã quay góc α là bao nhiêu. Việc xác định độ lớn của góc quay dựa 
vào sensor la bàn. Xác định độ lớn của góc quay này là rất cần thiết vì sau khi 
tránh vật ta dùng nó để điều khiển robot quay trở về hướng đi ban đầu. 
BẠCH HOÀNG GIANG K46ĐC 
45 
 luận tốt nghiệp 
Tiến trình của robot tránh vật là: 
- Tại điểm P quay bên phải 1 góc α để điểm A của robot ra khỏi vật cản. 
- Tiến 1 đoạn L tới điểm Q để điểm D của robot ra khỏi hướng chắn của 
vật cản. 
- Tại điểm D quay lại bên trái 1 góc α để trở lại hướng đi song song với 
hướng ban đầu. 
- Tiến 1 đoạn M đến điểm R để robot thực sự thoát khỏi vật. 
- Tại điểm R lại quay bên trái 1 góc α . 
- Tiến 1 đoạn L để robot tới điểm T. 
Tại điểm T robot quay bên phải 1 góc α để robot trở lại hướng đi ban 
đầu sau khi đã tránh được vật cản. 
6.2.2 Trường hợp đặt 2 vật so le nhau. 
Vật cản 1 βP 
Y 
Vật 
cản 2α
Robot 
L 
m
Q R 2L 
Lα X βT
Hình 6.3: 2 vật cản sắp xếp so le với l < AB 
Trong trường hợp này khi robot tránh vật cản thứ nhất rồi lại gặp luôn 
vật cản thứ 2 thì nó sẽ tránh tiếp vật cản thứ 2, sau đó nó quay trở về hướng đi 
ban đầu. Vấn đề đặt ra ở đây là điều kiện khi nào thì nó coi vật 2 là vật cản. 
Do kích thước của robot ta đã xác định được trước nên ta có thể xác đinh 
được khi nào nó coi vật 2 là vật cản. 
BẠCH HOÀNG GIANG K46ĐC 
46 
 luận tốt nghiệp 
Như ở trên đã trình tại điểm Q robot tiến 1 đoạn QR để thoát khỏi vật 1, 
nếu khe hở giữa 2 vật không đủ để robot lách qua thì nó sẽ tránh vật thứ 2. Sau 
đó với quay trở về hướng đi ban đầu. 
Giả sử khi robot đi tới điểm R mà khoảng cách do sensor siêu âm đo 
được so với vật 2 là m < Dmin tức là khe hở giữa 2 vật không đủ để robot có 
thể lách qua được thì robot sẽ tiếp tục tránh vật 2. Việc xử lý tránh vật 2 cũng 
tương tự như việc xử lý tránh vật 1. Sau khi tránh vật 2 và robot tiến tiếp tới 
điểm Y thì tại điểm Y nó sẽ quay trái 1 góc β . Sau khi quay xong thì không 
giống với trường hợp 1 là nó tiến 1 đoạn L, mà nó phải đi 1 đoạn là 2L để về 
điểm T. Tại T nó quay phải 1 góc β để về hướng đi ban đầu. 
6.2.3 Trường hợp robot lách giữa 2 vật cản. 
Robot Vật 
cản 1
T
P 
R
m
 x 
 M 
Vật cản 2
Q 
Hình 6.4: Robot lách qua 2 vật 
Trong trường hợp này thì tại điểm R sensor siêu âm đo được khoảng 
cách từ robot tới vật cản 2 là m > Dmin, tức là khoảng cách giữa vật đủ để 
robot lách qua. Thì nó sẽ không coi vật 2 là vật cản và nó sẽ tiếp tục công việc 
quay trở về hướng đi ban đầu của nó. 
BẠCH HOÀNG GIANG K46ĐC 
47 
 luận tốt nghiệp 
 START
ĐI THẲNG
KC<Dmin
S
Đ
 RẼ PHẢI
KC<Dmin
Đ
S 
 RẼ TRÁI
Hình 6.1: Lưu đồ thuật toán chương trình tránh vật cản 
BẠCH HOÀNG GIANG K46ĐC 
48 
 luận tốt nghiệp 
Chương trình điều khiển robot 
' {$STAMP BS2sx} 
' {$PBASIC 2.5} 
' {$PORT COM1} 
'**************** 
'*KHAI BAO BIEN * 
'**************** 
gocquay VAR Word 'bien luu so xung cua goc lech 
gocbd VAR Word 
gocsau VAR Word 
acc VAR Word 'bien luu so xung khoang cach tu vat toi sensor 
kc VAR Word 'bien luu khoang cach tu vat toi sensor 
chuankc VAR Word 
Dmin VAR Word 'Bien luu khoang cach de quay tranh vat 
loops VAR Word 
temp VAR Word 
delta VAR Word 
'********************* 
'* CHUONG TRINH CHINH* 
'********************* 
main: 
Batdau: 
 HIGH 5 
 IF IN5 =0 THEN Tien 'Neu chan 5 dc an thi bat dau chay 
GOTO Batdau 
Tien: 'Tien thang cho toi khi gap vat 
 GOSUB Dogoclech 
 gocbd=gocquay 
 PULSIN 0,1,kc 
 IF kc=0 THEN tien 
 kc=20+((kc-1583)/70) 
IF kc<30 THEN 
 GOTO quaysangfai 
 ELSEIF kc>50 THEN 
 GOTO tienthang 
ENDIF 
tienthang: 
 GOSUB banhtraitien 
 GOSUB banhfaitien 
GOTO tien 
quaysangfai: 
 DO 
 GOSUB quayfai 
Tien1: 'Tien thang cho toi khi gap vat 
 PULSIN 0,1,kc 
 IF kc=0 THEN tien1 
 kc=(kc-1583)/70 +20 
 IF kc>50 THEN EXIT 
LOOP 
BẠCH HOÀNG GIANG K46ĐC 
49 
 luận tốt nghiệp 
quaysangtrai: 
 GOSUB quayfai 'quay tiep mot doan de tranh khoi vat truoc 
khi quay nguoc lai 
 PAUSE 500 
 GOSUB banhtraitien 'tien thang sau khi quay trai 
 GOSUB banhfaitien 
 PAUSE 3000 
 GOSUB Dogoclech 'do goc lech de quay nguoc lai 
 gocsau=gocquay 'goc lech khi tranh vat 
 delta = gocsau-gocbd 'luu goc lech 
DO 
 GOSUB quaytrai 'quay sang trai de co huong // huong ban 
dau 
 GOSUB dogoclech 
 IF gocquay<gocbd+10 THEN EXIT 'sai so goc quay la 10 do 
LOOP 
 GOSUB banhtraitien 'Tien 1 doan de thoat khoi vat 
 GOSUB banhfaitien 
 PAUSE 4000 
DO 'quay tiep ben trai de ve duong di ban dau 
 GOSUB quaytrai 
 GOSUB dogoclech 
 IF gocquay<gocbd-delta THEN EXIT 'quay ve huong ban dau - goc 
lech 
LOOP 
 GOSUB banhtraitien 
 GOSUB banhfaitien 
 PAUSE 3000 
DO 
 GOSUB quayfai 'quay ve huong ban dau 
 GOSUB dogoclech 
 IF gocquay>gocbd+1 THEN EXIT 
LOOP 
 GOSUB banhtraitien 
 GOSUB banhfaitien 
GOTO tien 
GOTO main 
'************************************ 
'* CHUONG TRINH CON DIEU KHIEN MOTOR* 
'************************************ 
banhtraitien: 
BẠCH HOÀNG GIANG K46ĐC 
50 
 luận tốt nghiệp 
LOW 15 'enable motor left 
HIGH 14 'derector motor left tien 
RETURN 
banhfaitien: 
LOW 13 'enable motor right 
HIGH 12 'derector motor right tien 
RETURN 
banhtrailui: 
HIGH 15 'enable motor left 
LOW 14 'derector motor left lui 
RETURN 
banhfailui: 
HIGH 13 'enable motor right 
LOW 12 'derector motor right lui 
RETURN 
stopbanhtrai: 
HIGH 15 'Stop motor left 
HIGH 14 
RETURN 
stopbanhfai: 
LOW 13 'Stop motor right 
LOW 12 
RETURN 
stop2banh: 
HIGH 13 'Stop motor right 
HIGH 12 
HIGH 15 'Stop motor left 
HIGH 14 
RETURN 
'************************************* 
'*CHUONG TRINH CON QUAY TRAI,QUAY FAI* 
'************************************* 
quaytrai: 
HIGH 15 'banh trai lui 
LOW 14 
LOW 13 'banh fai tien 
HIGH 12 
RETURN 
quayfai: 
HIGH 13 'banh fai lui 
LOW 12 
LOW 15 'banh trai tien 
HIGH 14 
RETURN 
BẠCH HOÀNG GIANG K46ĐC 
51 
 luận tốt nghiệp 
'****************************** 
'*CHUONG TRINH CON DO GOC LECH* 
'****************************** 
Dogoclech: 
PULSIN 9,1,temp 
temp = (temp-1250)/125 
IF temp514 THEN gocquay=temp 
RETURN 
BẠCH HOÀNG GIANG K46ĐC 
52 
 luận tốt nghiệp 
Tài liệu tham khảo 
 [1] Parallax Inc. www.parallaxinc.com. 
 [2] Scott Edwards “programming and Customizing the Basic Stamp omputer”. 
[3] 
[4] www.semiconductors.philips.com/ acrobat/datasheets/KMZ51_3.pdf 
[5]Trần Thị Thúy Hà “Nghiên cứu cấu trúc và phát triển chương trình điều 
khiển thông minh của các robot nhiều bậc tự do”. 
[6] Trần Duy Hưng “Nghiên cứu xây dựng một robot di động thông minh hoạt 
động tự quản trị”. 
[7]Phan Hữu Phú “Nghiên cứu thử nghiệm một mô hình thị giác máy tính dùng 
cho bám đối tượng và dẫn đường của robot di động thông minh”. 
BẠCH HOÀNG GIANG K46ĐC 
53