CHƯƠNG 3 : XÂY DỰNG MƠ HÌNH ROBOT TỰ HÀNH
3.5. Xây dựng thuật toán tránh vật cản
3.5.1. Môi trường di chuyển của robot
Robot di chuyển trong mơi trường tĩnh có vật cản, vật cản cố định hay xuất hiện ngẫu nhiên trong quá trình robot di chuyển. Hình dạng và kích thước của vật cản là tùy ý nhưng chúng ta ưu tiên các vật cản hình hộp với kích thước tương đối lớn để thể hiện rõ được biên dạng vật cản.
3.5.2. Giải thuật phát hiện và tránh vật cản Yêu cầu của giải thuật: Yêu cầu của giải thuật:
Phát hiện được vật cản từ vị trí robot đến vật cản trong khoảng cách an toàn, tránh va chạm xảy ra cho robot.
Rẽ tránh vật cản và trở lại hướng di chuyển ban đầu.
Trong quá trình rẽ tránh vật cản phải vẽ lại được biên dạng của vật cản. Từ yêu cầu trên chúng ta tiến hành mơ phỏng như sau:
Hình 3.19. Mơi trường di chuyển của robot
Robot được trang bị 3 cảm biến siêu âm để đo khoảng cách từ nó đến vật cản. Các cảm biến được bố trí như hình 3.20 nhằm thực hiện các chức năng khác nhau.
60
Hình 3.20. Vị trí các cảm biến trên robot
Cảm biến trung tâm có nhiệm vụ phát hiện vật cản trong khoảng cách 10cm.
Cảm biến trái và cảm biến phải có nhiệm vụ thu thập khoảng cách từ vật cản đến robot để thể hiện lại biên dạng của vật cản.
Khoảng cách từ vật cản đến robot tương ứng với khoảng cách từ vật cản đến cảm biến: cảm biến trung tâm (d1), cảm biến trái (d2) và cảm biến phải (d3).
Quá trình di chuyển của robot:
Đầu tiên các cảm biến siêu âm sẽ đo và trả về các khoảng cách từ robot đến môi trường xung quay theo các hướng tương ứng. Khoảng cách từ cảm biến trung tâm (d1) quyết định hướng di chuyển của robot.
Nếu (d1>=10cm): khơng có vật cản trong phạm vi 10cm, robot di chuyển về phía trước.
61
Nếu (d1<10cm): có vật cản trong phạm vi 10cm, robot dừng lại.
Hình 3.22. Cảm biến trung tâm phát hiện vật cản
Trong lúc dừng lại, các cảm biến trái và phải của robot sẽ quyết định hướng rẽ tránh vật cản của robot theo các trường hợp sau sau:
o Nếu d2<d3: khoảng cách bên trái lớn hơn bên phải, robot rẽ trái 900 tránh vật cản.
62
o Nếu d2>d3: khoảng cách bên trái lớn hơn bên phải, robot rẽ phải 900 tránh vật cản.
Hình 3.24. Cảm biến phải trả về d3 từ đó vẽ lại biên dạng của vật cản.
Robot được quy định góc xoay là 900 để thuận lợi cho việc di chuyển cũng như thu thập các số liệu cần thiết từ các cảm biến. Robot di chuyển đến khi hết biên dạng mặt phẳng trước của vật cản thì dừng lại và rẽ về hướng di chuyển ban đầu.
63
Trong khi robot rẽ hướng và trở lại hướng di chuyển ban đầu sẽ có 2 trường hợp:
Nếu robot rẽ phải tránh vật cản (d2<d3) thì rẽ trái để trở lại hướng di chuyển ban đầu.
Nếu robot rẽ trái tránh vật cản (d2>d3) thì rẽ phải để trở lại hướng di chuyển ban đầu.
Trong quá trình di chuyển, cảm biến trái và cảm biến phải thu thập số liệu cho việc vẽ lại biên dạng vật cản, cảm biến trung tâm tiếp tục phát hiện vật cản trong phạm vi 10cm.
Hình 3.26. Cảm biến thu thập dữ liệu của biên dạng
64