Unitree Go2 EDUで巡回用の2Dマップを作ってみた
こんにちは、atsuです。
Unitree Go2 EDU で slam_toolbox を使い、1フロア分の2D LiDAR SLAM で地図を作成してみました。
SLAM (Simultaneous Localization And Mapping) は、ロボットが自分の位置を推定しながら同時に周囲の地図を作る技術です。今回は四足歩行ロボット Go2 EDU を歩かせて、フロアの 2D 地図 (上から見た図面のような OccupancyGrid) を自動生成しました。slam_toolbox はこの SLAM を実現する OSS ライブラリの1つで、ROS 2 (ロボット開発用フレームワーク) の Nav2 (自律ナビゲーションのスタック) と組み合わせて使うのが一般的です。
SLAM の仕組みは記事の前半で軽くおさらいし、ROS 2 の細かい作法は前提としつつ、Go2 EDU 上での実装と気をつけた点の共有に重心を置いて書きます。本記事は Go2 巡回ロボットの機能を機能ごとに深掘りする記事の1つで、巡回プロジェクト全体像は別記事 (Unitree Go2 EDUで巡回ロボットを作っている話) を参照してください。
環境
本記事は、Go2 EDU 出荷構成 (Foxy + JetPack 5.1.1) を前提としています。
- ロボット: Unitree Go2 EDU (Jetson Orin NX 16GB、Ubuntu 20.04、JetPack 5.1.1)
- ROS 2: Foxy (Jetson にプリインストール)
- SLAM:
slam_toolbox(Steve Macenski 氏メンテ、Foxy 以降は apt 配布) - 可視化: Foxglove Studio (Mac 側で起動し
ws://<GO2_IP>:9090で接続)
ROS 2 Foxy は 2023-05 に EOL を迎えていますが、Go2 EDU プリインストール環境をそのまま使うため Foxy で進めます。
SLAM の基本的な仕組み
SLAM (Simultaneous Localization And Mapping) は名前のとおり、ロボットが「自己位置推定 (Localization)」と「マッピング (Mapping)」を 同時並行で やる技術です。地図がない状態でロボットを動かしながら、自分がどこにいるかと、周囲にどんな壁・障害物があるかを少しずつ把握していきます。
ロボット視点で動きを追うと、こんな流れになります。
この流れの中で、特に重要な3つの処理があります。
- LiDAR スキャン — レーザーで周囲の壁までの距離を 360° 全方位で取得します (1秒間に何回も)。SLAM はこの「点の集まり」を比較しながら、ロボットの動きと地図を組み立てていきます。
- スキャンマッチング — 前回のスキャンと、少し動いた後の新しいスキャンを照合し、ロボットがどれだけ動いたかを逆算します。オドメトリ (関節や車輪エンコーダから推定する移動量) だけでは誤差が大きくなりがちなので、スキャン同士の照合で補正します。
- ループクロージャ — 一周して出発点付近に戻ったとき、過去に作った地図と今のスキャンが「同じ場所」だと判定できると、それまで蓄積した位置誤差 (累積ドリフト) を一気に補正できます。これが SLAM の品質を大きく左右するポイントで、地図を綺麗に閉じるか歪んだまま残すかの分かれ目になります。
この3つの処理をどう実装するかで SLAM ライブラリの個性が出ます。今回採用した slam_toolbox は、過去の姿勢をグラフのノード・姿勢間の相対変換をエッジとして保持し、ループクロージャ時にグラフ全体を最適化する Pose Graph SLAM という手法を採っています。
ROS 2 における SLAM
ROS 2 (Robot Operating System 2) はロボットアプリケーション開発向けのミドルウェアで、SLAM もこの上で動く1つの機能として整理されています。ノード同士が トピック (publish/subscribe で流れるメッセージ) と TF (座標系の親子関係) でつながる構造になっており、SLAM のライブラリは以下のトピックを基本にやり取りします。
| 役割 | トピック | メッセージ型 |
|---|---|---|
| 入力: 2D LiDAR スキャン | /scan |
sensor_msgs/LaserScan |
| 入力: 座標系の親子関係 | /tf, /tf_static |
tf2_msgs/TFMessage |
| 入力 (補助): オドメトリ | /odom |
nav_msgs/Odometry |
| 出力: 2D 占有格子地図 | /map |
nav_msgs/OccupancyGrid |
出力: map → odom 補正 |
/tf |
tf2_msgs/TFMessage |
ROS 2 の慣例として、TF ツリーは map → odom → base_link → 各センサーのフレーム という階層が標準です。SLAM は map → odom の補正を担当し、odom → base_link はオドメトリ生成側 (今回の tf_bridge のような役割) が担当する分業になっています。
代表的な OSS SLAM ライブラリ
ROS 2 で使える主要な 2D SLAM ライブラリには次のようなものがあります。今回はこの中から slam_toolbox を採用しました (理由は次節で書きます)。
slam_toolbox— Steve Macenski 氏メンテ、Pose Graph SLAM ベース、Nav2 と同じ開発系列cartographer_ros— Google 製、3D LiDAR や IMU 統合にも対応gmapping(ROS 2 移植) — ROS 1 時代から使われている古典的なパーティクルフィルタ系
これらはいずれも apt パッケージ (ros-foxy-slam-toolbox のような形) で配布されており、ros2 launch で起動するのが標準です。パラメータは YAML で渡します。
Nav2 との関係
SLAM とよくセットで語られるのが Nav2 (ROS 2 の自律ナビゲーションスタック) です。役割分担は次のようになります。
- SLAM は地図を「作る」側 (write):
slam_toolboxのようなライブラリが/mapを生成 - AMCL は地図上で「位置推定する」側 (read): 既存の
/mapと/scanを使って粒子フィルタで自己位置を推定 - Nav2 の経路計画 (Planner / Controller) は両者の出力 (マップ + 自己位置) を入力に経路を生成
つまり、巡回ロボットのライフサイクルは「SLAM でマップを作る → 保存する → AMCL で自己位置推定 → Nav2 で巡回」と進みます。今回の記事はこのうち最初の マップを作る 部分です。
なぜ slam_toolbox を選んだか
前節で挙げた候補のうち、今回の用途では slam_toolbox の特性が合致したので採用しました。選んだ理由は3つあります。
ひとつは Pose Graph SLAM ベースで、ループクロージャと Pose Graph Optimization が組み込みで動くこと。
ここで、ループクロージャは「出発地点や過去通った場所に戻ったときに、それまで歩く中で少しずつ蓄積した位置誤差 (累積ドリフト) を一気に補正する処理」を指します。Pose Graph Optimization は「過去の自己位置をグラフのノード、姿勢間の相対変換をエッジとして保持し、ループが閉じた瞬間にグラフ全体を再計算して全姿勢を整える処理」です。出発点に戻る巡回用途では、これらが自動で走ってくれるのは相性が良いと判断しました。
もうひとつは Nav2 との接続性です。Nav2 は ROS 2 の自律ナビゲーション用スタックで、slam_toolbox の開発元 (Steve Macenski 氏) と同じです。Nav2 内のマップ配信ノード (map_server) や障害物コストマップ (costmap_2d) への連携手順がそのまま使えます。
加えて、ロボットを歩かせながら作るオンラインモードと、録画した bag (ROS 2 のトピックを丸ごと録画したファイル) を後から再生して作るオフラインモードの両方に対応しているため、検証フェーズで bag 再生が必要になっても困らない見通しが立ちました。.posegraph 形式でマッピング途中状態を保存できるので、大規模空間で分割マッピングする選択肢も残せます。
システム構成
Go2 EDUで slam_toolbox を動かすにあたって自分で実装する必要があった部分が2つあります。順に説明します (pc2_to_scan tf_bridge は本プロジェクトでの内製ノード名で、ROS 2 標準の枠組みに沿って実装したものです)。
4D LiDAR L1 を 2D に潰す: pc2_to_scan
LiDAR は赤外光のレーザーで距離を測るセンサーで、Go2 EDU 搭載の Unitree 4D LiDAR L1 は周囲を3D点群 (空間上の点の集合、sensor_msgs/PointCloud2) として 15 Hz で出します。トピック名は /utlidar/cloud_deskewed などです。一方 slam_toolbox の入力は sensor_msgs/LaserScan (1次元の距離配列で、平面 360° のスキャンを表現するメッセージ型) なので、3D点群を2Dに変換する処理が必要になります。これを担うのが pc2_to_scan で、ROS 2 公式の pointcloud_to_laserscan 相当の役割をプロジェクトの要件に合わせて実装したものです。
このノードでは、複数のフィルタを組み合わせて Go2 自身の体反射や歩行中のノイズを除外し、SLAM や後段の Nav2 が安定して読めるスキャンに整える処理を入れています。特に LiDAR の単発フレームだと視野が狭く Nav2 の経路計画が引けないため、複数フレームを蓄積して占有空間を広げる工夫を入れています。
TF チェーンを自前で作る: tf_bridge
ROS 2 では座標系の親子関係 (例: map → odom → base_link → utlidar_lidar のように、地図座標系からロボット中心、さらに各センサーまでをツリー状に繋ぐ階層) を /tf /tf_static というトピックで配信し、各ノードが必要に応じて変換 (TF lookup) する仕組みになっています。SLAM や AMCL (粒子フィルタによる自己位置推定) は、この TF ツリーが揃っていることを前提に動作します。
ところが Go2 EDU は標準で /tf と /tf_static を一切 publish しないため、自分で TF ツリーを構築する必要があります。幸い /utlidar/robot_odom (nav_msgs/Odometry、ロボット中心の累積移動量を表すメッセージ) には位置情報が乗っているので、これを購読して odom → base_link の TF に変換します。あわせて base_link → utlidar_lidar の static TF (時間で変化しない固定の取り付け関係) を配信し、map → odom の TF は slam_toolbox 側が担当する分業構成です。
マッピング手順
Step 1. slam_toolbox 起動
Jetson 上の Docker コンテナ内で、/scan トピックが流れている状態で slam_toolbox を async モードで起動します。読者環境では Docker でなくホスト直接実行でも構いません。
ros2 launch slam_toolbox online_async_launch.py \
slam_params_file:=/path/to/slam_toolbox_params.yaml \
use_sim_time:=false
slam_toolbox_params.yaml は後述の「実装で気をつけた点」で四足歩行向けに調整した中身を載せています。/map (nav_msgs/OccupancyGrid 型のメッセージで、グリッド状の地図を「壁/通路/未観測」の値で持つもの) が 1 Hz で更新されはじめれば準備完了です。
slam_toolbox には async / sync / lifelong / localization のモードがあり、マッピング用途では async が標準です。処理が間に合わない場合に古いスキャンをドロップして最新スキャンを優先する設計のため、Go2 のように LiDAR レートが高めのロボットでも詰まりにくい挙動になっています。
Step 2. リモコンで歩かせる
Mac側で Foxglove Studio を ws://<GO2_IP>:9090 に接続します。3D パネルに /map と /scan を追加して、Go2 をリモコンでフロア全体に歩かせます。
歩かせ方には少しコツがあります。急旋回すると振動でスキャンマッチングが外れるので、目安として 0.2 m/s 程度のゆっくりしたペースで歩かせること。ここでスキャンマッチングは「前回のレーザースキャンと今回のレーザースキャンを照合し、ロボットがどれだけ動いたかを逆算する処理」で、SLAM の自己位置推定の根幹です。
歩くときは直線中心で緩やかなカーブを描くと崩れにくく、最後に出発点に戻ると ループクロージャ が発動して累積ドリフトが一気に補正されます。Foxglove で見ているとこの瞬間にマップ全体の補正が一度に反映される挙動が観察できます。
Step 3. マップ保存
別端末で nav2_map_server の map_saver_cli を実行します。
ros2 run nav2_map_server map_saver_cli \
-t /map \
-f maps/patrol \
--ros-args -p map_subscribe_transient_local:=true
maps/patrol.pgm (画像) と maps/patrol.yaml (メタデータ) が出力されます。Step 1 の端末で Ctrl+C を押して slam_toolbox を止めれば終わりです。
map_subscribe_transient_local:=true を忘れると、Foxy では QoS (Quality of Service) 不一致で /map を取りこぼして timeout する挙動があるので、明示しておくと安全です。
実装で気をつけた点
四足歩行ロボット + Go2 固有で気をつけた点があります。
歩行振動と関節オドメトリのドリフト
オドメトリは「センサー (車輪エンコーダや関節エンコーダ) の動きから累積で移動量を推定する仕組み」のことで、ROS 2 では nav_msgs/Odometry 型で配信されます。SLAM はこのオドメトリを補助情報にスキャンマッチングを行い、自己位置を推定します。
ところが四足歩行は構造上、歩行中に上下動が乗りやすく LiDAR スキャンに揺れが現れます。さらに Go2 は車輪エンコーダではなく関節エンコーダから移動量を逆算しているため、オドメトリのドリフト (累積誤差) も大きめになりがちです。
対処として config/slam_toolbox_params.yaml でパラメータを次のように調整しました。
slam_toolbox:
ros__parameters:
# スキャン採用のフィルタ
throttle_scans: 3 # 3 スキャンに 1 回採用 (振動でブレた瞬間を捨てる)
minimum_travel_distance: 0.3 # 30 cm 動いてからスキャンを採用
minimum_time_interval: 1.0 # 1 秒以上の間隔
# スキャンマッチング (前回スキャンと今回スキャンの照合) の探索範囲
correlation_search_space_dimension: 0.5 # ドリフトに対応するため広げる (デフォルト 0.3)
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.03
# ループクロージャの探索範囲と判定閾値
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_match_minimum_response_coarse: 0.4 # 判定を少し緩めてループを検出しやすく
loop_match_minimum_response_fine: 0.45
考え方としては「振動が収まった安定スキャンだけ採用」「ドリフトが大きくてもマッチングできる広い探索範囲」「ループクロージャ判定を少し緩く」の組み合わせです。
ホイール型ロボットを前提にしたデフォルト値だと Go2 ではマッピングが崩れがちなので、四足歩行で試す方は調整推奨です。ただしループクロージャの判定閾値を緩めすぎると誤発動でマップが歪むので、Foxglove でループクロージャの瞬間を観察しながら詰めるのが現実的です。
出力ファイル
patrol.pgm (約 100 KB)
PGM は Portable Gray Map というグレースケール画像のフォーマットで、ROS 2 ではこの画像形式に2D地図を保存します。今回出力されるのは天井から見た2D図面で、1ピクセルが現実の 5 cm に対応します(resolution: 0.05 m/pix)。negate: 0 の慣例で、白(254)が free (通れる空間)、黒(0)が occupied (壁・障害物)、灰色(205)が unknown (未観測) を表します。
patrol.yaml
image: patrol.pgm
mode: trinary
resolution: 0.05
origin: [-8.04, -6.29, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
origin はマップ左下端の現実座標 (x, y, yaw) です。ロボット起動位置を (0, 0) とすると、そこから左に 8.04 m、下に 6.29 m の場所が PGM 左下に対応します。REP-103 (x=forward, y=left) に従い、PGM の右上方向が現実の +x +y です。
なお、PGM ファイル自体は一般的な画像と同じく左上が原点ですが、ROS の map_server が読み込むときに Y 軸を反転して左下を原点として扱う、という慣例になっています。
occupied_thresh / free_thresh は「壁」「通れる空間」を判定するための閾値で、Nav2 の costmap での障害物余裕距離 (inflation_radius) と合わせて巡回品質が決まります。
実際の運用では、生の PGM に加えてノイズ除去版も別途生成しています。歩行中の通行人や Go2 自身の影響で残るノイズを後処理で消しておかないと、Nav2 の global planner が孤立した障害物を避けようとして経路がぎこちなくなる傾向があったためです。
結果
1フロア分のマップをマッピングできました。
- マップサイズ: 369 × 294 px (= 約 18 m × 14 m)
- 解像度: 0.05 m/pix
- 出力: PGM 約 100 KB + YAML 7 行
- ループクロージャ発動を確認、sha256 検算で保存も確認済み
このマップを AMCL の入力にして自己位置推定 → Nav2 で経路計画、というのが次のステップです。
まとめ
Go2 EDU + slam_toolbox で1フロア分の 2D LiDAR SLAM (地図作成) を実装し、四足歩行ロボット特有の注意点と対処を共有しました。
歩行振動と関節オドメトリのドリフトは slam_toolbox のパラメータ調整 (探索範囲拡大とループクロージャ閾値の調整) で吸収できました。四足歩行ロボットで 2D LiDAR SLAM を試される方の参考になれば嬉しいです。
