SLAM Toolbox 建圖基礎

2026-07-18
  • ros2
  • slam
  • slam-toolbox
  • mapping
  • lidar

問題定義

上一節提到 Nav2 依賴 map 這個座標系做全域路徑規劃，但這張地圖從哪裡來？如果機器人是第一次進入一個沒有地圖的環境，需要一邊探索、一邊建立這個環境的地圖——這正是 SLAM（Simultaneous Localization and Mapping，同時定位與建圖）要解決的問題。這一節用 slam_toolbox 實際建立一張地圖，作為下一節代價地圖與 Nav2 定位的輸入。

核心概念說明

SLAM 為什麼「同時」定位與建圖是一個難題

如果已經有一張準確的地圖，用感測器資料比對地圖來定位自己的位置並不難；反過來，如果已經確切知道機器人每一刻的精準位置，把感測器資料疊加起來畫出地圖也不難。SLAM 困難的地方在於這兩者互相依賴、卻同時都是未知的——你需要地圖來定位，也需要準確的位置資訊來畫出正確的地圖，兩個問題互為前提。slam_toolbox 底層採用基於圖優化（graph-based）的方法解這個問題：把每次移動與每次掃描比對的結果都當作圖上的節點與邊，持續優化整張圖，讓地圖跟軌跡估計互相修正、逐漸收斂到一致的結果。

掃描比對：SLAM 的基本運算單位

slam_toolbox 持續執行的核心運算是掃描比對（scan matching）：拿當前這一幀的雷射掃描資料，跟已經建立的地圖（或前一幀掃描）比對，找出「機器人實際移動了多少」能讓這幀新資料跟既有地圖疊合得最好。這也是為什麼上面 FAQ 提到移動太快會影響品質——相鄰掃描的重疊區域太少，比對演算法就沒有足夠的共同特徵可以依靠，估計出來的位移容易出錯。

實作範例：用第 5 章的模擬環境建圖

延續第 5 章加了雷射雷達的模擬機器人，這裡示範完整的建圖流程。

1. 啟動模擬環境（沿用第 5.3 節加了雷射雷達的模型）

bash
gz sim empty.sdf &
ros2 run ros_gz_sim create -file /tmp/dual_arm_expanded.urdf -name dual_arm -z 0.5
ros2 run ros_gz_bridge parameter_bridge /scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan

實務上建圖需要機器人能移動，這裡假設模擬環境裡的機器人底盤已經橋接好 /cmd_vel（速度指令）與里程計，這部分會在後面控制相關章節深入，這一節聚焦在建圖本身的操作流程。

2. 啟動 slam_toolbox

bash
ros2 run slam_toolbox async_slam_toolbox_node --ros-args \
  -p use_sim_time:=true \
  -p odom_frame:=odom \
  -p base_frame:=base_link \
  -p scan_topic:=/scan

預期輸出

text
[INFO] [slam_toolbox]: Node using stack size 40000000
[INFO] [slam_toolbox]: Message Filter dropping message: frame 'lidar_link' at time ... for reason 'discarding message because the queue is full'

第二行這種訊息在剛啟動的頭幾秒很常見，是因為 TF 樹跟第一批掃描資料到達的時間點沒有完全對齊（呼應第 4.3 節提到的 ExtrapolationException 概念），只要不是持續大量出現，通常會隨系統穩定運作而消失。

3. 用鍵盤或手動發布指令讓機器人移動，同時觀察建圖過程

在 RViz2 裡加入 Map 這個 Display（Topic 選 /map），Fixed Frame 設成 map，隨著機器人移動，畫面上會逐漸浮現黑白灰三色構成的地圖：黑色代表偵測到障礙物（牆面），白色代表確認為空曠可通行區域，灰色代表尚未探索、資訊不足的未知區域。

4. 儲存建好的地圖

bash
ros2 run nav2_map_server map_saver_cli -f ~/ros2_ws/maps/my_map

預期輸出

text
[INFO] [map_saver]: Waiting for the map
[INFO] [map_saver]: Received a 384 X 384 map at 10 m/s
[INFO] [map_saver]: Writing map occupancy data to my_map.pgm
[INFO] [map_saver]: Writing map metadata to my_map.yaml
[INFO] [map_saver]: Map saved successfully

這會產生 my_map.pgm（地圖影像本身）跟 my_map.yaml（地圖中繼資料，包含解析度、原點座標），下一節設定代價地圖時會直接載入這份 .yaml 檔案。

常見錯誤與除錯技巧

錯誤一：RViz2 的 Map Display 完全沒有畫面

原因：最常見的是 use_sim_time 設定不一致——如果 slam_toolbox 用模擬時間，但橋接時鐘資料（第 5.1 節提到的 /clock 橋接）沒有正確設定，或是 RViz2 本身沒有同步使用模擬時間，會導致訊息時間戳記對不上，Map Display 遲遲拿不到能正確顯示的資料。

排除方式：確認整條資料鏈上所有相關節點的 use_sim_time 設定一致（slam_toolbox、robot_state_publisher 等），並確認 /clock 主題確實在正常發布：

bash
ros2 topic hz /clock

如果這裡沒有穩定的輸出，代表時鐘橋接沒有正常運作，是問題的根源。

錯誤二：建好的地圖出現明顯的「雙重牆」或扭曲

現象：地圖裡同一面牆看起來變成兩條平行但略微錯開的黑線，而不是一條清晰的線。

原因：這是掃描比對失敗、累積誤差的典型徵兆——移動過快、或環境裡特徵過於稀疏單調（例如長走廊，缺乏足夠的幾何特徵讓掃描比對準確估計位移），導致演算法對「機器人到底移動了多少」的估計出現偏差。

排除方式：建圖時放慢移動速度，尤其是轉彎處；如果環境特徵確實稀疏，可以規劃來回經過同一區域的路徑，讓 slam_toolbox 的圖優化機制有機會透過「迴路閉合」（loop closure，偵測到回到了先前去過的區域）重新修正累積誤差。

小結

SLAM 解決「同時建圖與定位互為前提」的難題，slam_toolbox 靠掃描比對持續估計機器人移動量並疊加建構地圖，建圖品質高度依賴移動速度與環境特徵豐富程度。建好的地圖存成 .pgm + .yaml 這組標準格式，下一節要用這張地圖，搭配代價地圖與路徑規劃演算法，真正做到自主導航。

延伸閱讀

常見問題

faq_01.log
SLAM 建圖跟 Nav2 定位是同一件事嗎？
不是，是兩個階段。SLAM（同時定位與建圖）是在『還沒有地圖』的情況下，一邊移動一邊建立地圖並估計自己的位置，通常是初次進入新環境時做一次；定位（localization）則是『已經有地圖』的前提下，只需要估計機器人在這張既有地圖裡的位置，不需要持續更新地圖本身。slam_toolbox 可以做 SLAM，也可以切換到純定位模式使用已經建好的地圖，兩種模式對應不同的使用情境。
faq_02.log
建圖時機器人移動速度會影響地圖品質嗎？
會，移動太快會導致相鄰兩次雷射掃描之間的重疊區域太少，讓掃描比對（scan matching）演算法難以準確估計出兩次掃描之間機器人實際移動了多少，容易累積定位誤差、進而讓地圖出現扭曲或重複的牆面。實務上建圖階段通常會刻意放慢移動速度，尤其是在轉彎或狹窄空間。
faq_03.log
地圖檔案（.pgm + .yaml）裡的 .yaml 是存什麼？
存的是地圖的中繼資料：地圖影像檔案路徑、解析度（每個像素代表多少公尺）、地圖原點在世界座標系裡的位置、以及佔用機率的判斷門檻值。.pgm 檔案本身只是一張灰階影像，單獨看不出這些資訊怎麼對應到真實世界座標，必須靠 .yaml 一起讀取才有意義。