Costmap là "bức tranh" Nav2 dùng để quyết định đi đâu: mỗi ô lưới mang một giá trị chi phí 0–254. Cấu hình costmap tốt thì xe đi giữa lối, tránh người mượt mà; cấu hình kém thì xe hoặc đi sát kệ, hoặc không dám qua lối hẹp, hoặc "nhớ" mãi vật cản đã đi khỏi.

Sau bài này bạn sẽ:
  • Hiểu ý nghĩa các mức cost và các lớp của costmap.
  • Chọn tham số inflation hợp với lối đi trong kho.
  • Cấu hình nguồn quan sát để vật cản động xuất hiện và biến mất đúng lúc.

1. Các mức cost và các lớp

Giá trịTênÝ nghĩa
254LETHALÔ có vật cản
253INSCRIBEDTâm xe ở đây chắc chắn va chạm (trong bán kính nội tiếp)
1–252—Gần vật cản, càng gần càng "đắt"
0FREETự do
255NO_INFORMATIONChưa biết

Costmap được xếp chồng từ các lớp (plugin): static_layer lấy từ bản đồ, obstacle_layer (2D) hoặc voxel_layer (3D) đánh dấu vật cản từ cảm biến, inflation_layer "thổi phồng" vật cản để giữ khoảng cách an toàn. Global costmap thường gồm cả ba lớp trên toàn bản đồ; local costmap là cửa sổ cuốn vài mét quanh xe, thường chỉ cần obstacle + inflation.

2. Inflation: giữ khoảng cách bao xa?

Hình 1. Cost giảm theo hàm mũ khi xa vật cản; cost_scaling_factor quyết định độ dốc.
Hình 1. Cost giảm theo hàm mũ khi xa vật cản; cost_scaling_factor quyết định độ dốc.

Với khoảng cách d từ ô tới vật cản gần nhất và bán kính nội tiếp ri của xe, inflation layer gán: cost = 252 · e−k·(d − ri) khi ri < d ≤ inflation_radius, với k là cost_scaling_factor.

  • inflation_radius nên lớn hơn bán kính ngoại tiếp của xe, thường bằng một nửa bề rộng lối đi: lối 2 m → khoảng 0,8–1,0 m, để vùng giữa lối có cost thấp nhất và planner tự đi giữa.
  • cost_scaling_factor nhỏ (2–4): cost giảm chậm, xe có xu hướng đi xa vật; lớn (8–10): cost tụt nhanh, xe dám đi sát hơn.
  • Lối đi hẹp mà xe "không dám qua": inflation_radius quá lớn làm hai bên lối chồng lên nhau thành vùng cost cao liên tục.

3. Cấu hình mẫu cho kho hàng

Hình 2. Costmap gộp lớp tĩnh (tường, kệ), lớp vật cản (người từ lidar) và lớp inflation.
Hình 2. Costmap gộp lớp tĩnh (tường, kệ), lớp vật cản (người từ lidar) và lớp inflation.
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_footprint
      rolling_window: true
      width: 4
      height: 4
      resolution: 0.05
      footprint: "[[0.40, 0.28], [0.40, -0.28], [-0.40, -0.28], [-0.40, 0.28]]"
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          data_type: "LaserScan"
          marking: true
          clearing: true
          obstacle_max_range: 4.0
          raytrace_max_range: 6.0
          max_obstacle_height: 2.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.80
        cost_scaling_factor: 4.0

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 2.0
      global_frame: map
      robot_base_frame: base_footprint
      resolution: 0.05
      track_unknown_space: true
      footprint: "[[0.40, 0.28], [0.40, -0.28], [-0.40, -0.28], [-0.40, 0.28]]"
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: scan
        scan: {topic: /scan, data_type: "LaserScan", marking: true, clearing: true,
               obstacle_max_range: 4.0, raytrace_max_range: 6.0}
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.90
        cost_scaling_factor: 3.0

Footprint đa giác (đúng hình chữ nhật của xe, cộng thêm vài cm cho hàng nhô ra) chính xác hơn robot_radius cho AGV dài; xe kéo kệ có thể đổi footprint lúc chạy bằng cách phát lên topic footprint của costmap khi nhận/thả hàng.

4. Vật cản động: hiện ra và biến mất đúng lúc

  • marking: điểm lidar đánh dấu ô là vật cản (trong obstacle_max_range).
  • clearing: tia lidar đi qua ô nào thì xóa vật cản ở ô đó (trong raytrace_max_range). Khi người đi khỏi, tia lidar xuyên qua chỗ cũ và xóa nó. raytrace_max_range phải lớn hơn obstacle_max_range.
  • Vùng lidar không quét tới (điểm mù sau xe, dưới bàn nâng) sẽ giữ vật cản cũ – cần thêm cảm biến hoặc gọi xóa costmap khi bắt đầu nhiệm vụ.
  • Lidar 2D không thấy càng xe nâng thấp hay mặt bàn cao: dùng camera độ sâu hoặc lidar 3D với voxel_layer (bài #16).
Costmap filters: Nav2 có keepout filter (vẽ vùng cấm trên một bản đồ mask riêng – lối thoát hiểm, khu cấm xe) và speed filter (vùng giới hạn tốc độ – gần cửa, khu có người). Không cần sửa bản đồ gốc, quản lý vùng rất linh hoạt.

5. Quy trình hiệu chỉnh

  1. Trong RViz, hiển thị global và local costmap (màu "costmap").
  2. Kiểm tra footprint đúng kích thước xe thật (đặt xe sát tường, so viền).
  3. Cho người đi ngang trước xe: vật cản phải hiện trong < 0,2 s và biến mất sau khi người đi khỏi.
  4. Thử lối hẹp nhất trong kho; tinh chỉnh inflation cho đến khi xe đi giữa lối và qua được.

6. Lỗi thường gặp

  • Vết vật cản "ma" không mất: clearing: false hoặc raytrace_max_range nhỏ hơn khoảng cách vật.
  • Sàn bị đánh dấu là vật cản: lidar lắp nghiêng thấy sàn – chỉnh lắp đặt hoặc dùng min_obstacle_height với dữ liệu 3D.
  • Costmap không có lớp static: QoS map không phải transient local – xem bài #4.
  • CPU cao: local costmap quá lớn, resolution 0,02, update_frequency quá cao.

Tài liệu tham khảo