Unitree Go2 EDUで現在位置を推定してみた: AMCLで「自分の場所を知る」

こんにちは。atsu です。

Unitree Go2 EDU のフロア巡回機能を 9 つに分解した全体像は、別記事 で整理しています。本記事では、そのうち 自分の場所を知る 機能を扱います。

https://dev.classmethod.jp/articles/go2-patrol/

slam_toolbox などでフロアの 2D マップを作っても、地図があるだけではロボットは動けません。地図上のどこに自分がいるのかを継続的に推定する必要があります。この役割を担うのが Nav2 の AMCL です。

この記事では、AMCL の数式を深追いするのではなく、AMCL が何をしているのかをざっくり理解したうえで、Go2 EDU の LiDAR / odom / TF を AMCL が読める形に揃え、巡回開始時に位置推定を安定させることに焦点を当てます。

この記事で分かることは、以下です。

全体像記事では、この機能を「周囲の壁の形と地図上の壁の形を照らし合わせて、確率的に位置を当てていく」と説明しました。この記事は、その説明をもう少しだけ内側から見ていく内容です。

先に用語をゆるく整理する

AMCL の話では、ROS topic、odom、TF という言葉が出てきます。ROS に詳しくない方は、ここだけ先に押さえると読みやすくなります。

ROS topic は「ロボット内の情報チャンネル」

ROS topic は、ロボットの中を流れる情報のチャンネルです。

たとえば、LiDAR が見ている距離情報、ロボットの現在位置、速度指令などが、それぞれ別の topic として流れます。テレビのチャンネルのように、見たい情報ごとに名前が付いていると考えると分かりやすいです。

AMCL は、これらの情報を別々に受け取って、「地図上のどこにいるのが一番それっぽいか」を推定します。

odom は「ロボット自身が思っている移動量」

odom は odometry の略です。ロボットが自分の動きから推定した移動量のことです。

人間で例えると、「目をつぶって 3 歩進んだから、たぶん 2m くらい前に進んだはず」という感覚に近いです。ロボットの場合は、足の動き、関節、IMU、内部センサーなどから「これくらい動いたはず」と推定します。

ただし odom は少しずつズレます。床で滑る、足の接地がずれる、センサーに誤差がある、といった理由で、実際の位置とロボット自身の思い込みが離れていきます。

AMCL は odom を「前回の位置からどれくらい動いたか」の参考にします。ただし odom だけを信じるのではなく、LiDAR と地図でズレを直します。

TF は「座標のものさしをつなぐ仕組み」

TF は、ROS で座標同士の関係を扱う仕組みです。

ロボット開発では、同じ物体でも「どの座標で見るか」によって位置の表し方が変わります。地図から見たロボットの位置、ロボット本体から見た LiDAR の位置、odom から見たロボットの位置は、それぞれ基準が違います。

AMCL は最終的に map → odom という TF を出します。これは「odom で積み上げた移動量を、地図上ではどこに置けばよいか」をつなぐ補正のようなものです。

ここが切れていると、地図、ロボット、LiDAR が同じ画面にいても、互いの位置関係が分からなくなります。Nav2 が動けなくなる典型的な原因です。

AMCL とは

今回使った AMCL は、ROS 2 Foxy の Nav2 に含まれる nav2_amcl パッケージです。上流のソースコードは ros-navigation/navigation2 の nav2_amcl にあります。

nav2_amcl は、既知の 2D マップ上で、2D LiDAR の観測を使ってロボットの位置と向きを推定する localization パッケージです。ROS Index でも、nav2_amcl は navigation2 リポジトリ内のパッケージとして公開されています。実際に使うバージョンは ROS 2 の distro や Debian package に依存するため、実機の厳密なバージョンを確認したい場合は apt show ros-foxy-nav2-amcl や container image の build log を見るのが確実です。

AMCL は Adaptive Monte Carlo Localization の略で、粒子フィルタを使った自己位置推定です。ざっくり言うと、地図上に「ロボットはこのあたりにいるかも」という候補をたくさんばらまき、LiDAR の見え方と地図の形が合う候補を残していく方式です。

名前を分解すると、少し分かりやすくなります。

つまり AMCL は、「地図上にたくさんの位置候補を置き、センサー情報に合う候補へだんだん寄せていく自己位置推定」と考えると入りやすいです。

AMCL が出す map → odom の TF が、後段の Planner / Controller にとって重要になります。Nav2 はこの TF を使って「地図上の現在位置」を理解します。

ロボットに詳しくない方向けに言うと、AMCL は「地図を見ながら、自分の現在地を当て続ける機能」です。GPS が使えない屋内では、壁の形や柱の位置を手がかりにします。人間でいうと、「この廊下の曲がり方なら、いま会議室の前にいるはず」と判断する感覚に近いです。

AMCL が苦手なのは、似た形の場所が多いフロアです。まっすぐな廊下が長く続く、同じ形の部屋が並ぶ、といった環境では候補が絞りにくくなります。逆に、角、柱、曲がり角など特徴がある場所では収束しやすいです。

全体像記事で書いた通り、起動直後は位置が定まらず、数秒〜数十秒かけて 1 点に収束します。この「収束する」という表現は、AMCL の particle が地図上の正しそうな場所へ集まっていくことを指しています。

AMCL の原理をざっくり理解する

AMCL を理解するうえで大事なのは、「ロボットは 1 つの現在地だけを信じているわけではない」という点です。

AMCL は地図上にたくさんの仮説を持ちます。この仮説を particle と呼びます。particle は「もしロボットがここにいて、この向きを向いていたら」という候補です。

たとえば、ロボットが廊下にいるとします。地図上の候補 A では、正面 2m に壁があるはずです。実際の LiDAR でも正面 2m に壁が見えていれば、その候補は「それっぽい」と評価されます。逆に、候補 B では右側に壁があるはずなのに、実際の LiDAR では何も見えていなければ、その候補は「違いそう」と評価されます。

この「地図から予想される LiDAR の見え方」と「実際の LiDAR の見え方」を比べるのが、AMCL の中心です。

AMCL が毎回やっていること

AMCL の処理は、大きく 4 つの繰り返しとして見ると分かりやすいです。

odom は「ロボットが自分ではこれくらい動いたと思っている」という情報です。ただし、odom だけでは少しずつズレます。床の滑り、歩行の揺れ、センサー誤差があるためです。

LiDAR は「今まわりに何が見えているか」を教えてくれます。ただし、LiDAR だけでは、似た形の場所を区別できないことがあります。

AMCL は、この 2 つを組み合わせます。

• odom で「前回の位置からこれくらい動いたはず」と予測する
• LiDAR と地図で「その場所にいると見え方が合うか」を確認する
• 合っている候補を残して、間違っていそうな候補を減らす

このため、AMCL は「LiDAR だけで位置を当てる機能」でも「odom だけで位置を積分する機能」でもありません。地図、LiDAR、odom、TF が揃ってはじめて安定します。

なぜ初期位置が大事なのか

AMCL は強力ですが、最初から何も知らない状態で完璧に場所を当てる魔法ではありません。

初期位置を与えると、「このあたりにいるはず」という範囲に particle を集めて始められます。すると、少ない候補でも早く収束します。逆に初期位置が大きく外れていると、間違った候補群の中で頑張ることになり、収束が遅くなったり、違う場所に自信を持ってしまうことがあります。

巡回ロボットでは、ここを運用で助けられます。毎回ドック前や決まったスタート地点から始めるなら、初期位置を固定できます。これは「ロボットが賢くない」のではなく、実運用で使える前提を使って安定性を上げる設計です。

AMCL が間違えると何が起きるか

AMCL の出力が間違うと、後段の Nav2 はその間違いを前提に動きます。ここが怖いところです。

特に「別の場所に誤収束」は、見た目には /amcl_pose が出ているため気づきにくいです。pose が出ていることと、pose が正しいことは別です。Foxglove で /scan が地図の壁に重なっているかを見るのが重要です。

検証環境

ロボットと開発 PC は、全体像記事と同じ環境です。ROS 2 Foxy は EOL ですが、Go2 EDU のプリインストール環境に合わせています。

Go2 で AMCL を動かすための役割分担

AMCL は単体で魔法のように位置を当てるわけではありません。入力が揃って初めて動きます。

Go2 固有で大事なのは、AMCL より手前の /scan と TF です。ここがズレていると、AMCL のパラメータをいくら触っても収束しません。

AMCL は「地図」「現在のスキャン」「ロボットのだいたいの動き」を照合します。どれか 1 つでもズレると、粒子が散ったままになったり、間違った場所に自信満々で収束したりします。実機で怖いのは後者です。見た目上は pose が出ているのに、実際の場所と違うと、Nav2 は誤った前提で経路を引きます。

初心者向けに言うと、AMCL の失敗はだいたい「見えている壁」「保存済み地図」「ロボット自身の思い込み」のどれかが噛み合っていない状態です。いきなり AMCL の難しいパラメータを見るより、まずこの 3 つが同じ現実を指しているかを確認する方が近道です。

代表的な AMCL パラメータの見方

AMCL の設定ファイルには多くのパラメータがあります。全部を最初から理解しようとすると大変なので、まずは「何の考え方に関係するか」で見ると楽です。

正確なパラメータ一覧は Nav2 の AMCL Configuration Guide を見るのが確実です。この記事では、実機で調整するときに最初に意味を知っておきたいものだけを抜き出しています。

パラメータ調整で最初にやりがちなのは、いきなり粒子数や laser model を触ることです。ただ、実機ではその前に /scan と TF の確認を優先した方がよいです。入力がズレたまま AMCL を調整すると、間違ったデータに合わせてパラメータを曲げることになり、別の場所で破綻しやすくなります。

私のおすすめの順番は以下です。

1. /map が正しい
2. /scan が実際の壁と合っている
3. map → odom → base_footprint → lidar の TF がつながっている
4. 初期位置を入れると /scan が地図に重なる
5. それでも揺れる場合に AMCL パラメータを見る

AMCL は賢いアルゴリズムですが、入力の座標系ミスまでは賢く直してくれません。

起動時の初期位置

AMCL は起動直後、地図上のどこにいるかを知りません。そのため、巡回開始時に初期位置を与えます。

Foxglove からは /initialpose に PoseWithCovarianceStamped を publish できます。マップ上でロボットの位置と向きをクリックして指定する形です。

ただし、自動巡回では毎回手動でクリックしたくありません。そこで今回の構成では、環境変数で初期位置を渡せるようにしました。

INITIAL_POSE_X=2.0 \
INITIAL_POSE_Y=5.0 \
INITIAL_POSE_YAW=0.0 \
python3 /app/scripts/launch_nav2_nodes.py /app/maps/patrol.yaml

launcher 側で AMCL 用の override params yaml を動的生成し、set_initial_pose: true と initial pose を起動時に渡しています。

Foxy でハマったこと

/initialpose の race

最初は AMCL 起動後に別プロセスから /initialpose を publish していました。しかし Foxy の lifecycle node 初期化タイミングと噛み合わず、particle filter の初期化前に pose を投げてしまうような race が起きました。

そのため、現在は AMCL 起動時の params として initial pose を渡す方式に寄せています。巡回ロボットでは、初期位置は運用上ほぼ固定できるため、この方が安定しました。

ここは実機運用らしい判断でした。研究用途なら global localization 的に「どこに置いても勝手に当てる」を目指したくなりますが、巡回業務では開始位置を決められることが多いです。ドック前、充電場所、スタート地点などを運用で固定できるなら、初期位置を明示した方が堅いです。ロボットに賢さを求めすぎず、運用で簡単に固定できるものは固定する、という割り切りです。

CLI override が yaml より効かないように見えた

--ros-args --params-file <yaml> -p use_sim_time:=true のように CLI override を渡しても、期待通り反映されない挙動に遭遇しました。ROS 2 の設計上は CLI が優先されるはずですが、Foxy + 自作 launcher + lifecycle node の組み合わせで切り分けが必要になりました。

今回は nav2_params.yaml 側から use_sim_time を削除し、launcher 側で制御する方針にしています。

確認ポイント

AMCL が動いているかは、以下を見ます。

ros2 topic echo /amcl_pose --once
ros2 topic hz /amcl_pose
ros2 run tf2_ros tf2_echo map odom
ros2 topic hz /particlecloud

Foxglove では以下を表示すると分かりやすいです。

AMCL が安定すると、particle が 1 点に集まり、ロボットを少し動かしても /amcl_pose が地図上の通路に沿って追従します。全体像記事で書いた「数秒〜数十秒で 1 点に収束する」は、この状態を指しています。

Foxy では particle 表示用の topic 名に注意が必要です。新しめの資料では /particle_cloud と書かれていることがありますが、今回の Foxy 環境では /particlecloud を確認対象にしています。末尾にアンダースコアがない旧形式の topic です。

確認コマンドの意味も整理しておきます。

/amcl_pose だけ出ていても、TF がつながっていないと Nav2 全体は動きません。Nav2 のデバッグでは topic と TF をセットで見るのが大事です。

実装上の考え方

今回の自己位置推定では、AMCL を「位置推定の本体」として扱い、Go2 固有処理は前処理に閉じ込めました。

この分け方にしておくと、後から Nav2 側のパラメータを調整するときに、センサー問題と localization 問題を切り分けやすくなります。

残った課題

• 歩行振動が強い場面では /scan 側のノイズが増え、AMCL の pose が揺れやすい
• 初期位置が大きく外れると収束に時間がかかる
• 実機と bag replay で時刻同期条件が異なるため、use_sim_time の扱いに注意が必要

まとめ

Go2 EDU の巡回で「自分の場所を知る」機能は、Nav2 AMCL を中心に構成しました。

• map_server が保存済みマップを配信する
• pc2_to_scan が AMCL 向けの /scan を作る
• tf_bridge が Nav2 の期待する TF ツリーを作る
• 初期位置は起動時 params で渡して race を避ける

AMCL が安定すると、Nav2 の Planner / Controller が「地図上の現在位置」を前提に経路を計算できるようになります。

参考リンク

• Unitree Go2 EDU のフロア巡回機能を 9 つの機能に分解してみた
• Nav2 Documentation
• nav2_amcl source code
• nav2_amcl - ROS Index
• AMCL Configuration Guide
• ROS Command Line Arguments