Định tuyến hay tạo 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.
Từ tọa độ vị trí của robot đã xác định trên bản đồ, chương trình định tuyến tạo đường đi từ vị trí hiện tại đến vị trí đích. Thuật tốn Dijkstra được sử dụng để tốn tìm đường đi ngắn nhất tới vị trí đích:
1) 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.
2) Đá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.
3) 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.
4) Đố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 tố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 tố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 25 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.
5) 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.
6) 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 hồ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 tốn kết thúc.
22
7) 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 tố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 tố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
23