Định tuyến hay lập kế hoạch đường đi cho robot từ vị trí hiện tại tới vị trí đích sử dụng giải thuật tìm đường đi ngắn nhất Dijkstra như trong hệ thống định vị, định tuyến sử dụng IPS.
Từ tọa độ vị trí của robot đã xác định trên bản đồ Costmap2D, chương trình định tuyến tạo đường đi từ vị trí hiện tại đến vị trí đích. Thuật toán Dijkstra được sử dụng để toán tìm đường đi ngắn nhất tới vị trí đích.
● Gọi nút mà chúng ta đang bắt đầu được gọi là nút ban đầu. Gọi khoảng cách của nút Y là khoảng cách từ nút ban đầu đến Y. Thuật toán Dijkstra sẽ gán một số giá trị khoảng cách ban đầu và sẽ cố gắng cải thiện chúng từng bước. ● Đánh dấu tất cả các nút không được sử dụng. Tạo một tập hợp tất cả các nút
chưa được duyệt được gọi là tập hợp chưa được duyệt.
● Gán cho mọi nút một giá trị khoảng cách dự kiến: đặt nó thành 0 cho nút ban đầu của chúng ta và thành vô cùng cho tất cả các nút khác. Đặt nút ban đầu là nút hiện tại.
● Đối với nút hiện tại, hãy xem xét tất cả các hàng xóm không được truy cập của nó và tính toán khoảng cách dự kiến của chúng qua nút hiện tại. So sánh khoảng cách dự kiến mới được tính toán với giá trị được gán hiện tại và chỉ định giá trị nhỏ hơn. Ví dụ: nếu nút hiện tại A được đánh dấu với khoảng cách
là 6 và cạnh nối nó với nút lân cận B có độ dài 2, thì khoảng cách từ B đến A sẽ là 6 + 2 = 8. Nếu trước đó B được đánh dấu bằng một khoảng cách lớn hơn 8 sau đó thay đổi nó thành 8. Nếu không, giá trị hiện tại sẽ được giữ nguyên. ● Khi chúng ta đã xem xét xong tất cả các lân cận chưa được truy cập của nút hiện tại, hãy đánh dấu nút hiện tại là đã được truy cập và xóa nó khỏi tập hợp chưa được truy cập. Một nút đã truy cập sẽ không bao giờ được kiểm tra lại. ● Nếu nút đích đã được đánh dấu là đã truy cập (khi lập kế hoạch một tuyến
đường giữa hai nút cụ thể) hoặc nếu khoảng cách dự kiến nhỏ nhất giữa các nút trong tập hợp chưa truy cập là vô cùng (khi lập kế hoạch truyền tải hoàn chỉnh; xảy ra khi không có kết nối giữa nút ban đầu và các nút chưa được truy cập còn lại), sau đó dừng lại. Thuật toán đã kết thúc.
● Nếu không, hãy chọn nút không mong đợi được đánh dấu bằng khoảng cách dự kiến nhỏ nhất, đặt nó làm "nút hiện tại" mới và quay lại bước 3.
Khi lập kế hoạch cho một tuyến đường, thực ra không cần phải đợi cho đến khi nút đích được "truy cập" như trên: thuật toán có thể dừng khi nút đích có khoảng cách dự kiến nhỏ nhất trong số tất cả các nút "chưa được thăm" (và do đó có thể được chọn làm tiếp theo "hiện tại"). Lưu đồ thuật toán được mô tả như hình dưới đây.
Thuật toán tìm đường Dijsktra function dijkstra(G, S) for each vertex V in G distance[V] ← infinite previous[V] ← NULL
if V != S, add V to Priority Queue Q distance[S] ← 0
while Q is not empty U ← Extract MIN from Q
for each unvisited neighbour V of U
tempDistance ← distance[U] + edge_weight(U, V) if tempDistance < distance[V]
distance[V] ← tempDistance previous[V] ← U