Path Planning là gì? Dijkstra và A* tìm đường trên lưới
Atlasbot · 17/8/2026 · 12 phút đọc
Bài Navigation là gì? đặt Path Planning là bước tính "đường đi nào từ đây tới đích". Đây là bài toán kinh điển của khoa học máy tính, có từ trước robot học rất lâu — coi bản đồ (bài Occupancy Grid) như một đồ thị, mỗi ô trống là một đỉnh, các ô liền kề nối với nhau bằng cạnh.
Khái niệm chính
Hai thuật toán nền tảng nhất cho bài toán tìm đường ngắn nhất trên đồ thị:
- Dijkstra — mở rộng đều theo mọi hướng, không quan tâm hướng đích
- A* — Dijkstra cộng thêm "linh cảm" (heuristic) hướng về đích
Kết quả của cả hai chỉ là một path tĩnh, chuỗi điểm không kèm thông tin thời gian (đã nói ở bài Trajectory, chuyên mục Điều khiển Robot) — việc biến path này thành chuyển động thực tế theo thời gian là việc của Path Following (bài tiếp theo).
Nguyên lý hoạt động
Từ điểm xuất phát, Dijkstra khám phá dần các ô lân cận theo thứ tự chi phí tăng dần — luôn xử lý ô có tổng chi phí thấp nhất tiếp theo, không quan tâm ô đó có gần đích hay không:
1. Đặt chi phí điểm xuất phát = 0, mọi ô khác = vô cực
2. Lặp lại: chọn ô CHƯA xử lý có chi phí thấp nhất
3. Cập nhật chi phí các ô lân cận nếu đi qua ô hiện tại rẻ hơn
4. Dừng khi đã xử lý tới ô đích
Đảm bảo tìm được đường đi ngắn nhất tuyệt đối — nhưng lãng phí thời gian khám phá cả những vùng không liên quan gì tới đích, vì không có khái niệm "hướng về đích".
A* (đọc "A sao") thêm một thành phần heuristic — ước lượng khoảng cách còn lại tới đích (thường dùng khoảng cách đường thẳng) — vào công thức chọn ô xử lý tiếp theo. Ô có f(ô) nhỏ nhất được xử lý trước — kết quả là A* "thiên vị" khám phá về phía đích thay vì mở rộng đều mọi hướng như Dijkstra, tìm ra đường đi nhanh hơn nhiều trong hầu hết trường hợp thực tế.
Tóm lại: Dijkstra = A* với heuristic luôn bằng 0 (không có "linh cảm" gì về hướng đích) — về bản chất, A* là bản tổng quát hơn, Dijkstra chỉ là một trường hợp đặc biệt của nó.
Kiến thức kỹ thuật
Công thức
f(ô) = g(ô) + h(ô)
g(ô) = chi phí thực tế đã đi từ điểm xuất phát tới ô này
h(ô) = ước lượng chi phí còn lại từ ô này tới đích (heuristic)
A* vẫn đảm bảo đường đi ngắn nhất nếu heuristic không bao giờ ước lượng quá cao thực tế (gọi là heuristic "admissible") — khoảng cách đường thẳng (Euclidean distance) tới đích luôn là một heuristic admissible hợp lệ trên grid, vì không đường đi thực tế nào có thể ngắn hơn đường thẳng.
VD: ô hiện tại cách đích 10 ô theo trục x, 5 ô theo trục y
h(ô) = √(10² + 5²) ≈ 11.18 (khoảng cách đường thẳng, admissible)
So với: h(ô) = 10 + 5 = 15 (khoảng cách Manhattan — chỉ admissible
nếu robot chỉ di chuyển ngang/dọc, không di chuyển chéo được)
Thông số
| Dijkstra | A* | |
|---|---|---|
| Heuristic | Không có | Có (ước lượng khoảng cách tới đích) |
| Tốc độ | Chậm hơn (khám phá đều mọi hướng) | Nhanh hơn (thiên vị về đích) |
| Đảm bảo tối ưu | Có | Có (nếu heuristic admissible) |
| Dùng khi | Cần đường đi tối ưu tuyệt đối, không quan tâm tốc độ tính | Cần cân bằng tốc độ và chất lượng — phổ biến hơn trong thực tế |
Ứng dụng thực tế
Trong thực tế Nav2, các planner phổ biến (NavFn, SmacPlanner — bài Planner trong Nav2) đều dựa trên nền tảng A*/Dijkstra này, chỉ khác nhau ở cách tối ưu tốc độ tính toán và chất lượng đường đi sinh ra — ví dụ SmacPlanner hỗ trợ thêm ràng buộc động học (robot không thể xoay tại chỗ như Ackermann) mà Dijkstra/A* thuần tuý trên grid không tính tới. Với AMR trong nhà kho, chọn heuristic phù hợp (Euclidean cho robot di chuyển tự do mọi hướng, hoặc heuristic có tính tới ràng buộc động học cho robot kiểu xe) ảnh hưởng trực tiếp chất lượng đường đi thực tế sinh ra, không chỉ tốc độ tính toán.
Ví dụ thực tế
import heapq
def a_star(grid, start, goal):
def heuristic(a, b):
return ((a[0]-b[0])**2 + (a[1]-b[1])**2) ** 0.5
open_set = [(0, start)]
g_score = {start: 0}
came_from = {}
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
return path[::-1]
for dx, dy in [(-1,0),(1,0),(0,-1),(0,1)]:
neighbor = (current[0]+dx, current[1]+dy)
if neighbor not in grid or grid[neighbor] == 1: # 1 = occupied
continue
tentative_g = g_score[current] + 1
if neighbor not in g_score or tentative_g < g_score[neighbor]:
g_score[neighbor] = tentative_g
f = tentative_g + heuristic(neighbor, goal)
heapq.heappush(open_set, (f, neighbor))
came_from[neighbor] = current
return None
Với grid 5x5 free, start=(0,0), goal=(4,4):
Output path: [(0,0), (1,1), (2,2), (3,3), (4,4)]
Đoạn code trên là bản A* tối giản trên grid không có vật cản — Nav2 thực tế phức tạp hơn nhiều (tính cả chi phí costmap, ràng buộc động học) nhưng nguyên lý lõi (mở rộng ưu tiên theo f(ô) = g(ô) + h(ô)) hoàn toàn giống nhau.
Lỗi thường gặp
- Dùng heuristic không admissible (ước lượng quá cao thực tế) — phá vỡ đảm bảo tìm đường ngắn nhất của A*, có thể trả về đường đi không tối ưu mà không có cảnh báo lỗi rõ ràng.
- Dùng khoảng cách Manhattan cho robot di chuyển được theo đường chéo — như ở mục Công thức, Manhattan distance không còn admissible khi robot có thể đi chéo, cần dùng Euclidean distance thay thế.
- Nhầm lẫn Path Planning (chạy 1 lần khi có goal mới) với Path Following (chạy liên tục) — kỳ vọng sai rằng planner sẽ tự động né vật cản động xuất hiện giữa đường, trong khi đó là nhiệm vụ của Path Following/local costmap.
- Không tính chi phí costmap khi mở rộng ô lân cận — A*/Dijkstra thuần tuý trên grid free/occupied 2 trạng thái bỏ qua vùng đệm an toàn (inflation, đã học ở bài Navigation Layer trong AMR) quanh vật cản, dẫn tới đường đi sát vật cản hơn mức an toàn mong muốn.
Kết luận
Dijkstra và A* là nền tảng toán học của mọi bộ tìm đường trong robot học — A* chỉ là Dijkstra được trang bị thêm "linh cảm" hướng đích qua heuristic, đánh đổi lấy tốc độ tính toán nhanh hơn nhiều mà vẫn giữ được đảm bảo tối ưu nếu heuristic hợp lệ. Bài tiếp theo trong chuyên mục đi vào Path Following — bước biến chuỗi điểm tĩnh này thành chuyển động thực tế theo thời gian.
