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.
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.
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
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.
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 α T X β Hình 6.3: 2 vật cản sắp xếp so le với l < AB (adsbygoogle = window.adsbygoogle || []).push({});
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.
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ó.
START ĐI THẲNG KC<Dmin S Đ RẼ PHẢI KC<Dmin Đ S RẼ TRÁI
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
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 (adsbygoogle = window.adsbygoogle || []).push({});
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:
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 (adsbygoogle = window.adsbygoogle || []).push({});
LOW 15 'banh trai tien HIGH 14
'****************************** '*CHUONG TRINH CON DO GOC LECH* '****************************** Dogoclech:
PULSIN 9,1,temp
temp = (temp-1250)/125
IF temp<>514 THEN gocquay=temp RETURN
Tài liệu tham khảo
[1] Parallax Inc. www.parallaxinc.com.
[2]Scott Edwards “programming and Customizing the Basic Stamp omputer”.
[3]http://www.robot-electronics.co.uk/
[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”.