Chương 2. CƠ SỞ LÍ THUYET VÀ THUC NGHIEM
2.4. Nguyên lý xác định khoảng cách đến vật thé từ cảm biến RPLiIDAR
Al.
Thông số kỹ thuật cảm biến RPLIDAR A1(Hinh 2.11):
Khoảng cách quét tối đa là 12 mét.
Nguồn sử dụng là 5V.
Tan số lay mẫu từ 2-10Hz.
24
Bằng cách phát đi một chùm tia laser rồi thu nhận lại tín hiệu phản hồi. Tốc độ ánh sáng đã biết trước, độ trễ phản hồi được ghi nhận, từ đó tinh được khoảng cách giữa bộ cảm biến và vật thê một cách tương đối chính xác. Sự chênh lệch về thời gian
và bước sóng laser sau đó được sử dụng dé mô hình s6 2 chiều của đối tượng, Khoảng cách (d) tính bằng 1⁄2 tích giá trị tốc độ ánh sáng (c) và thời gian (t):
d=stXxc (2.1)
Trong đó: d là khoảng cách từ cảm bộ cảm biến đến vật
t là thời gian từ lúc phát tín hiệu đi đến khi nhận tín hiệu trả về
c là tốc độ ánh sáng (299.792.458 m/s)
2.5. Thuật toán Dijkstra cho global planner
Trong điều hướng cho robot dé robot đến được vị trí mong muốn, ta phải đưa ra một hoạch định đường đi từ điểm bắt đầu (vị trí hiện tại của robot) đến điểm kết thúc (vị trí đích mong muốn). Nhằm tối ưu hóa cho đường di, ta cần phải lựa chọn đường
đi ngắn nhất giữa hai điểm ấy. Với yêu cầu này, thuật toán Dijkstra là một trong những thuật toán đơn giản đề tìm được đường đi tối ưu nhất.
25
Bài toán đặt ra cho thuật toán Dijkstra là có một đồ thị G = (V,E) (với V là tập hợp các đỉnh và E là các cạnh vô hướng hoặc có hướng), mỗi cạnh sẽ có trọng số luôn dương và một đỉnh nguồn s. Chúng ta cần tính toán được đường đi ngắn nhất từ đỉnh
nguồn s đến mỗi đỉnh trên đô thị. Dé hiểu rõ hơn, ta hãy xem hình sau:
=oằ
Tá
Hình 2.12: Sơ đồ nút và trọng số trên mỗi cạnh trong đồ thị [8]
Trên Hình 2.12 ta có một đồ thị gồm 6 nút, 9 cạnh, các trọng số trên mỗi cạnh
và có điểm nguồn a, điểm đích b. Vậy chúng ta cần tìm đường đi ngắn nhất từ a đến
b. Dé thực hiện được thuật toán này, ta sẽ có phương pháp giải như sau:
1. Trước khi bắt đầu, ta cần gán mỗi nút một giá trị khởi tạo: bằng 0 đối với
nút bat dau va băng vô cùng cho tat cả các nút còn lại.
2. Dua tat cả các nút vào trong một tập dữ liệu Q dé ta tính khoảng cách từ điểm khởi tạo đến điểm đó.
3. Đối với điểm nút hiện tại, lần lượt xét các điểm nút xung quanh chưa từng xét trước đó và tính khoảng cách dự kiến của nó từ trọng số của nút hiện tại. Nếu giá trị dự kiến nhỏ hơn giá trị hiện tại của nút đó thì gán lại giá trị của nút đó bằng giá tri
dự kiến.
4. Đối với mỗi nút đã xét, ta loại bỏ nút này khỏi tập Q dé không cần phải xét
lại.
26
5. Chúng ta lần lượt kiểm tra các nút trong tập Q. Nếu nút đích đã được kiểm tra thì dừng lại và kết thúc thuật toán.
Từ phương pháp giải trên, chúng ta sẽ thành lập đoạn mã giả (pseudo code) sau
dé có thé tìm được đường đi ngắn nhất (Error! Reference source not found.).
Bảng 2.6: Thuật toán Dijkstra [10]
function Dijkstra(Graph, source):
for each vertex v in Graph: // Initialization
dist[v] := infinity // initial distance from source to vertex v
// is set to infinite previous[v] := undefined // Previous node in optimal path
// from source dist[source] := 0 // Distance from source to source
Q := the set of all nodes in Graph // all nodes in the graph are
// unoptimized - thus are in Q while Q is not empty: // main loop
u := node in Q with smallest dist[ ] remove u from Q
for each neighbor v of u: _// where v has not yet been
// removed from Q.
alt := dist[u] + dist_between(u, v)
if alt < dist[v]// Relax (u,v)
dist[v] := alt previous[v] :=u
return previous[]
Qua thuật toán trên, ta có thé dé dang tim được đường di ngăn nhất từ điểm bắt
đâu đên điêm đích.
27
Hình 2.13: Hoạch định đường đi dùng thuật toán Dijkstra [10]
Hình 2.13 biểu diễn thuật toán Dijkstra khi được sử dung trong package
ros_navigation của ROS.
2.6. Thuật toán DWA để tránh vật can cho local planner
Thuật toán DWA là dùng dé tìm ra một tín hiệu điều khién hop lý gửi xuống robot nhằm mục đích điều khiến nó đến đích an toàn, nhanh chóng dựa trên global planner
đã hoạch định từ trước và được mô phỏng như Hình 2.14. Thuật toán này gồm hai bước chính là cắt giảm không gian tìm kiếm (search space) của vận tốc và tìm được vận tốc tối ưu trong không gian tìm kiếm đó.
Hình 2.14: Mô phỏng quỹ đạo với thuật toán DWA
28
2.6.1. Không gian tìm kiếm
Các vận toc có thê điêu khiên được trong không gian tìm kiêm được cat giảm theo ba bước sau:
© Quỹ đạo tròn: thuật toán DWA chỉ xét đến quỹ đạo là hình tròn (đường
cong) được xác định duy nhất bởi một cặp vận tốc thắng và vận tốc xoay (0,
@)).
e Vận tốc cho phép: nhằm tao ra một quỹ đạo an toàn cho robot dé tránh vật
cản. Một cặp vận tốc (v, w) được cho phép là khi robot có thể dừng trước vật cản gần nhất mà không có sự va chạm trên đường cong tương ứng với vận tốc đó. Vận tốc cho phép được định nghĩa như sau
Va={(v, w)lv < VJ2.dist(V,o).Ủp A œ < VJ2.dist(v,o).@p } (2.2)
Trong đú: - Va (Hỡnh 2.15) là chuỗi cỏc giỏ tri vận tốc ( 0, ứ ) cho
phép robot dừng trước vật cản mà không có sự va chạm.
- dist(v, w) là khoảng cách nhỏ nhất mà robot dừng trước vật can dé không có sự va chạm.
- Vp, ôyp là gia tốc của vận tốc thăng và vận tốc xoay tối đa nếu
robot di chuyên sẽ gây va chạm với vật cản.
dcnlal velocity
Va
Hình 2.15: Vận tốc cho phép Va trong DWA [11]
e Dynamic window: nhằm hạn chế vận tốc cho phép đối với những vận tốc có
thé đạt được trong khoảng chu kỳ cho trước với gia tốc tối đa của robot. Dé At
là khoảng thời gian mà trong đú gia tốc ứ, ử sẽ được thực thi dộ (va, wa) là
29
vận tốc thực được gửi xuống robot. Từ đó, vận tốc Va (Hình 2.16) sẽ được
định nghĩa như sau:
Va = {(0, w)lv € [UaT— 0. At, va +0. At] Aw € [Wa-@ .At, @a+ (0. At]} (2.3)
SỐ CitiSec
/
A i
— ị
left wall ~~ corridor | right wall I
X——
“OD degsec 90 dep/sec
Hinh 2.16: Van téc trong cura số động Va trong DWA [11]
e©_ Kết thúc 3 bước trên thì ta tìm được không gian tìm kiếm
Vr=VsNVaN Va (2.4)
2.6.2. Tối ưu
Ta có hàm mục tiêu được định nghĩa như sau:
G(v, w) = a. heading(0, w) + B. dist(v, w) + y. vel(v, w) (2.5)
Dé có thé tôi ưu hóa van toc ngõ ra thi ham mục tiêu phải có giá tri tôi da. Dé thực hiện được điêu này, ta thực hiện các bước sau:
e Target heading: heading (Hình 2.17) là giá trị đo tiến độ hướng đến đích của
robot. Giá trị sẽ mang giá trị tối đa khi robot đi chuyên trực tiếp về phía đích.
Giỏ tri của heading(v, w) được tớnh bởi cụng thức 180 — ỉ, với ỉ là gúc giữa hướng của robot và diém đích.
30
Hình 2.17: Heading của robot trong DWA [11]
e Không gian trống (clearance): hàm dist(v, w) thể hiện khoảng cách tính từ
robot đến vật cản gần nhất nằm trên quỹ đạo cong của nó. Giá trị này sẽ rất lớn nếu không có vật can năm trên quỹ đạo cong di chuyền của nó. Giá tri này càng nhỏ thì việc nó đối mặt với vật cản cảng cao, khi đó nó sẽ di chuyển xung quanh vật can ấy.
e Vận tốc: hàm vel(v, w) là vận tốc di chuyên thang của robot và hỗ trợ di
chuyên nhanh hơn.
e_ Cỏc hệ số ứ, B, y được chọn sao cho phự hợp với đặc tớnh của robot và mụi
trường hoạt động. Khi hàm mục tiêu có giá trị lớn nhất thì quỹ đạo tối ưu sẽ được chọn với vận tốc (0, w) tốt nhất và là kết quả của thuật toán.
31