Costmap: từ Occupancy Grid thô ra bản đồ chi phí nhiều lớp

Bài Occupancy Grid chỉ có 3 giá trị mỗi ô. Nav2 không dùng thẳng occupancy grid cho việc lập kế hoạch — nó biến đổi thành costmap, nơi mỗi ô mang một chi phí (cost) liên tục, không chỉ nhị phân "có/không vật cản".

Khái niệm chính

Hai ô đều "free" nhưng một ô cách tường 5cm, một ô cách tường 2m — về mặt an toàn, chúng không tương đương. Costmap biểu diễn sự khác biệt này bằng giá trị chi phí liên tục (0-255): xa vật cản chi phí thấp, gần vật cản chi phí cao dần — planner/controller ưu tiên đi qua vùng chi phí thấp khi có nhiều lựa chọn khả thi.

Ba lớp costmap phổ biến, chồng lên nhau: Static Layer, Obstacle Layer, Inflation Layer.

Nguyên lý hoạt động

Static Layer     — lấy trực tiếp từ occupancy grid (bản đồ SLAM đã dựng)
Obstacle Layer    — cập nhật thời gian thực từ cảm biến (LiDAR đang quét)
                     — vật cản KHÔNG có trong bản đồ tĩnh (người, xe đẩy mới xuất hiện)
Inflation Layer   — "phồng" chi phí ra xung quanh mỗi vật cản theo bán kính giảm dần

Các lớp chồng lên nhau, giá trị chi phí cuối cùng của mỗi ô là giá trị cao nhất trong số các lớp — một ô có thể "an toàn" theo Static Layer (không có trên bản đồ tĩnh) nhưng "nguy hiểm" theo Obstacle Layer (LiDAR vừa phát hiện vật cản mới ở đó ngay bây giờ).

Nếu chỉ đánh dấu đúng ô có vật cản là nguy hiểm, planner có thể tính ra đường đi sượt sát ngay mép vật cản — về lý thuyết "không chạm" nhưng không chừa biên độ an toàn cho sai số định vị/điều khiển thực tế. Inflation Layer giải quyết bằng cách lan toả chi phí ra các ô lân cận theo bán kính cấu hình được, giảm dần theo khoảng cách.

Tóm lại: Static Layer trả lời "bản đồ tĩnh nói gì" (chậm thay đổi), Obstacle Layer trả lời "cảm biến đang thấy gì ngay bây giờ" (thời gian thực), Inflation Layer biến "có/không vật cản" thành "nguy hiểm tới mức nào theo khoảng cách". Ba lớp bổ trợ, không thay thế nhau.

Kiến thức kỹ thuật

Công thức

inflation_layer:
  inflation_radius: 0.55      # bán kính lan toả (m)
  cost_scaling_factor: 3.0    # tốc độ giảm chi phí theo khoảng cách

Chi phí giảm dần theo hàm mũ tính từ khoảng cách tới vật cản gần nhất, đến ngưỡng inflation_radius thì bằng 0:

cost(d) = 253 × e^(−cost_scaling_factor × (d − bán_kính_robot))
(với d = khoảng cách tới vật cản, giới hạn trong [bán_kính_robot, inflation_radius])

cost_scaling_factor càng lớn → chi phí giảm càng nhanh theo khoảng cách
                              → robot "dám" đi gần vật cản hơn ở cùng inflation_radius

inflation_radius nên đặt tối thiểu bằng bán kính robot (bài Footprint — chuyên mục Tuning Nav2) cộng thêm biên độ an toàn — quá nhỏ, robot có thể tính đường sượt sát vật cản; quá lớn, robot né quá xa, không lách qua được không gian hẹp dù về lý thuyết đủ rộng để đi qua.

Thông số

Layer	Nguồn dữ liệu	Tần số cập nhật
Static	Occupancy grid từ SLAM	Gần như tĩnh, chỉ đổi khi nạp bản đồ mới
Obstacle	Cảm biến thời gian thực (LiDAR)	Cao, mỗi scan mới
Inflation	Tính lại từ 2 layer trên	Theo tần số layer nguồn thay đổi

Ứng dụng thực tế

Local Costmap và Global Costmap khác nhau ở phạm vi: Global Costmap phủ toàn bộ bản đồ, dùng cho planner_server tính đường đi tổng thể; Local Costmap chỉ phủ một vùng nhỏ quanh robot (ví dụ 3x3m), cập nhật tần số cao, dùng cho controller_server né vật cản tức thời. Local costmap nhỏ và cập nhật nhanh vì controller cần phản ứng tức thời với vật cản mới xuất hiện ngay gần robot; global costmap lớn nhưng không cần cập nhật nhanh bằng vì chỉ dùng để lập kế hoạch tổng thể, không phải né tránh tức thời.

Ví dụ thực tế

So sánh 2 giá trị inflation_radius cho robot bán kính 0.3m,
hành lang rộng 1m (0.5m mỗi bên tính từ tâm hành lang):

inflation_radius = 0.55 (bán kính robot 0.3m + 0.25m biên an toàn):
  → vùng chi phí cao lan ra 0.55m từ mỗi bên tường
  → hành lang 1m rộng: 2 vùng inflation từ 2 bên tường CHỒNG LẤN nhau
    ở giữa hành lang (0.55×2 = 1.1m > 1m rộng hành lang)
  → planner có thể coi toàn bộ hành lang "chi phí cao", khó tìm đường qua

inflation_radius = 0.35 (bán kính robot 0.3m + 0.05m biên an toàn nhỏ hơn):
  → vùng chi phí cao 2 bên không chồng lấn (0.35×2 = 0.7m < 1m)
  → vẫn còn khoảng trống ở giữa hành lang với chi phí thấp
  → planner tìm được đường qua, robot đi giữa hành lang an toàn

Ví dụ này minh hoạ trực tiếp cảnh báo "quá lớn, robot né quá xa, không lách qua được không gian hẹp" đã nêu ở phần Nguyên lý hoạt động — cần cân bằng giữa an toàn và khả năng đi qua không gian hẹp thực tế của môi trường.

Lỗi thường gặp

• Đặt inflation_radius quá lớn cho môi trường có hành lang hẹp — như ví dụ trên, robot không tìm được đường qua dù về lý thuyết đủ rộng để đi lọt.
• Đặt inflation_radius nhỏ hơn bán kính robot — không chừa đủ biên an toàn, đường đi có thể sượt sát vật cản hơn mức chấp nhận được với sai số định vị/điều khiển thực tế.
• Nhầm lẫn Local Costmap và Global Costmap khi debug — kiểm tra sai costmap khi nghi ngờ vấn đề né vật cản tức thời (nên xem Local) hay lập kế hoạch tổng thể (nên xem Global).
• Không tính tới việc Obstacle Layer có thể "kẹt" dữ liệu cũ — nếu vật cản động đã di chuyển đi nhưng costmap chưa cập nhật kịp, robot có thể né một vị trí không còn vật cản thực tế — đây là lý do recovery behavior ClearEntireCostmap (chuyên mục Nav2, bài Recovery) tồn tại.

Kết luận

Costmap chuyển occupancy grid nhị phân thành bản đồ chi phí liên tục qua 3 lớp Static, Obstacle, Inflation — mỗi lớp trả lời một câu hỏi khác nhau (bản đồ tĩnh nói gì, cảm biến thấy gì ngay bây giờ, nguy hiểm tới mức nào theo khoảng cách). Cân bằng đúng inflation_radius giữa an toàn và khả năng đi qua không gian hẹp là quyết định tinh chỉnh quan trọng nhất của lớp này. Bài tiếp theo trong chuyên mục đi vào Behavior Tree — cách Nav2 điều phối Planner và Controller vừa học thành một luồng xử lý có logic rõ ràng.