背景

• ROS2の概念を理解するためのメモでTF2・SLAM・Nav2・Gazeboの概念をざっと整理したが、「ホイールのオドメトリとLiDARが、具体的にAMCL・SLAM・Nav2のどこに入り、どう使われるのか」という駆動系（ドライブトレイン）側の繋がりが曖昧だった
• そこで、Gazebo上での差動二輪駆動（Differential Drive）のシミュレーションを起点に、オドメトリ・LiDARがAMCL・SLAM・Nav2にどう流れていくのかを整理する

全体像: 駆動系からナビゲーションまでのデータの流れ

• 大まかな処理の流れは以下の通り

• つまり、駆動系（1・5）とセンサー（2）がGazebo側の入出力、自己位置推定（3）とナビゲーション（4）がROS 2側の処理という役割分担になっており、この2つがオドメトリ・LiDARデータ・cmd_velを通じて閉じたループを作っている

TFツリー全体像

• このループを理解する上で軸になるのが、TF2が管理する座標フレームのツリー
• 代表的な構成は以下の通り

1

map -> odom -> base_footprint -> base_link -> base_laser（LiDARなど各センサーのフレーム）

• 重要なのは、AMCL・SLAMはodom->base_link系の変換を書き換えるのではなく、それとは別にmap->odomの変換を発行するという点
• odom->base_link系は駆動系が出し続けるオドメトリそのもの（ドリフトを含む、短期的には滑らかだが長期的にはズレていく値）であり、AMCL・SLAMはそのズレを、別のレイヤーで補正する役割分担になっている

TF2の仕組み: 分散システム前提の座標変換ライブラリ

• TF2は、複数のノードに分かれて動く分散システムであることを前提にした、座標変換のためのライブラリ
• 各ノードが、自分の知っている2つのフレーム間の位置・姿勢関係をtransform_broadcasterで発行し、それらを集約することで、ロボット全体の全フレームが繋がった1つのツリーが構築される
• 位置・姿勢を知りたい側は、transform_listenerでこのツリーをたどり、任意の2フレーム間の変換を取得する
• 位置は$x, y, z$（メートル単位）、姿勢はクォータニオン（四元数）で表現される

ROS座標フレームの一覧

• ここまで出てきたmap・odom・base_footprint・base_link・base_laserは、ROSのコミュニティで広く使われている座標フレームの命名規約（移動ロボット向けの標準規約REP-105ほか）に沿ったもので、それぞれの役割は以下の通り

• mapとodomはどちらも世界座標だが、mapは長期的に正確な代わりに不連続、odomは連続的だが長期的にはズレていくという、正反対の性質を持つ
• base_footprint・base_link・base_laserはどれもロボット本体を基準にしたローカル座標で、odom->base_footprintの変換さえ分かれば、そこから先は固定されたロボットの構造（URDF）だけで計算できる

Gazeboでの駆動系シミュレーション

差動二輪駆動の仕組み

• 差動二輪駆動（Differential Drive）は、左右2つの独立した駆動輪の回転速度の差で、前進・後退・旋回を実現する機構
• 左右輪の角速度を$\omega_L$、$\omega_R$、ホイール半径を$r$、ホイール間距離（トレッド）を$L$とすると、ロボット本体の並進速度$v$と角速度$\omega$は以下の式で計算される

• Nav2やteleopノードがcmd_vel（並進速度$v$・角速度$\omega$）を指令すると、駆動系はこの式を逆に解いて、左右輪それぞれの目標角速度を求める

gz-simのDiffDriveシステムプラグイン

• 現行のGazebo（gz-sim、旧Ignition Gazebo）では、DiffDriveというシステムプラグインが、この差動二輪駆動のシミュレーションとオドメトリ計算をまとめて担う
• 実際にソースを確認したところ、役割は以下の通り

• つまり、SDF（Gazeboのロボット記述形式）やURDFの中でこのプラグインに左右ホイールの関節名と、ホイール半径・間距離を指定するだけで、cmd_velを受け取って物理的にホイールを回転させ、その回転量から上の式を使ってオドメトリを逆算し、odom->base_link相当のTFとトピックを配信してくれる
• ROSとの橋渡し（ros_gz_bridge）を使うことで、これらのGazebo側トピックを、ROS 2標準の/cmd_vel・/odomトピックに変換して繋ぎ込む

オドメトリのドリフトという前提

• このDiffDriveプラグインが計算するオドメトリは、ホイールの回転量だけから並進・回転を積分しているため、実機ではスリップ（ホイールの空転・横滑り）やエンコーダのノイズによって、時間が経つほど実際の位置とズレていく（ドリフトする）
• Gazeboでも、タイヤと地面の摩擦パラメータ次第でこのスリップが再現されるため、オドメトリ単体では長時間・長距離の自己位置推定には使えない、という実機と同じ制約がシミュレーション上でも成り立つ
• このドリフトを補正するのが、後述のAMCL・SLAMの役割になる

GazeboでのLiDARシミュレーション

• GazeboのLiDARセンサープラグインは、仮想空間内でレイを飛ばして周囲のオブジェクトとの交差点を計算することで、実際のLiDARと同じ形式の距離測定データ（sensor_msgs/LaserScan、各角度ごとの距離値の配列）を生成する
• このセンサーも、URDF上でbase_linkからの相対位置・向きを指定したリンク（例えばlaser_link）に取り付ける形で定義し、robot_state_publisherがそのリンクのTFフレームを自動的に配信する
• 出力は、ROS 2標準では/scanトピックとして配信され、このトピックを後述のAMCL・SLAM・Nav2のコストマップが共通して利用する

AMCL: オドメトリとLiDARで自己位置を補正する

パーティクルフィルタの基本

• AMCL（Adaptive Monte Carlo Localization）は、既知の地図上でのロボットの自己位置を、パーティクルフィルタで推定する手法
• 「ロボットがここにいるかもしれない」という仮説を、数百〜数千個の粒子（パーティクル、それぞれが位置・向きの候補）として保持し、センサー情報が入るたびに、尤度の高い粒子を生き残らせ、低い粒子を淘汰する、というリサンプリングを繰り返す

動作モデル（オドメトリ）と観測モデル（LiDAR）

• 実際にリポジトリで確認したところ、AMCLはodom（オドメトリ）・scan（LiDARのスキャン）・map（既知の地図）・initialpose（初期位置の手動指定）をsubscribeし、particlecloud（粒子群の可視化用）・amcl_pose（推定された自己位置）をpublishする
• 動作モデルは、直前のオドメトリの変化量を使って、各粒子を同じだけ移動させる。ただし、ドリフトを考慮して、移動量にはランダムなノイズを加える
• 観測モデルは、各粒子の位置から見たときに、今回のLiDARスキャンがどれだけ「地図と一致していそうか」を計算し、一致度が高い粒子ほど尤度を高くする
• この2つを組み合わせることで、「オドメトリでおおよその動きを予測し、LiDARと地図の照合でその予測を補正する」というベイズフィルタ的な推定を、観測が入るたびに繰り返す

map -> odom変換の補正という役割

• AMCLは、odom->base_linkのTFを書き換えるのではなく、map->odomのTFを配信することで、地図座標系での自己位置を表現する
• つまり、駆動系（DiffDriveプラグイン）が出し続ける生のオドメトリはそのまま残しつつ、AMCLがその上に「このオドメトリ座標系は、実際の地図上ではこれだけズレている」という補正値を重ねる、というレイヤー構造になっている
• このため、AMCLが何らかの理由で推定に失敗しても、odom->base_link自体は駆動系から配信され続けるため、システム全体が完全に座標情報を失うことはない

SLAM: 地図がない場合はslam_toolboxが代わりを担う

• AMCLは「既知の地図」が前提になっているため、未知の環境ではそもそも使えない
• そこで使われるのが、2D LiDARベースのSLAMとしてよく使われるslam_toolbox
• AMCLとの違いは以下の通り

• slam_toolboxも、AMCLと同様にオドメトリとLiDARスキャンを使ってスキャンマッチングを行い、その結果をもとにmap->odomのTFを配信する、という同じレイヤー構造を取る
• つまり、AMCLとSLAMは「地図を使うか作るか」の違いであり、駆動系（オドメトリ）・LiDARからの入力の受け方、map->odomという出力の形は共通している
• 作成した地図を保存しておけば、以降はAMCLで既知の地図として使い回せる、という使い分けになる

SLAMとAMCLは同時に動かさない

• AMCLとslam_toolboxは、どちらもmap->odomのTFを配信する役割を持つため、両者を同時に動かすとこの変換が競合してしまう
• 通常の運用では、地図作成中はslam_toolboxが地図と自己位置の両方を提供し、地図が完成した後はAMCLに切り替えて、既知の地図上での自己位置推定に専念するという使い分けが基本になる
• つまり「地図作成フェーズ（SLAM）」と「運用フェーズ（AMCL）」を明確に切り分け、同じタイミングでは片方しか動かさないのが前提

発展: 意味情報を加えるOWL/RDFとSemantic SLAM

• ここまでのSLAM・AMCLは、あくまで「空間のどこに壁や障害物があるか」という幾何学的な情報を扱う技術で、「その場所が何をする場所なのか」という意味までは扱わない
• OWL（Web Ontology Language）・RDF（Resource Description Framework）は、場所やオブジェクトに「101号室は病室」「薬品庫はスタッフ専用エリア」といった意味・関係をオントロジー・知識グラフとして記述するための標準規格で、SLAM・AMCLとは独立した、意味情報を載せるためのレイヤーにあたる
• 通常のSLAMが「そこに壁や物体がある」という幾何学的な地図しか作らないのに対し、Semantic SLAM（意味的SLAM）は「それはドアなのか机なのか部屋なのか」といった意味ラベルも地図に結びつける研究分野で、OWL/RDFはこの意味情報を整理する手段の1つとして使われることがあるが、必須というわけではない
• ROS4HRIの仕組みのLLM対応チュートリアルで触れたknowledge_coreも、OWL/RDFベースの記号的な知識ベースであり、位置情報そのものではなく、その位置・場所が持つ意味を扱う用途で使われている
• 整理すると、SLAM（地図を作る）→AMCL（地図上で位置を知る）→OWL/RDF（その位置・場所に意味を与える）という積み重ねで理解すると分かりやすいが、OWL/RDFはSLAM・AMCLの必須要素ではなく、必要に応じて追加する別レイヤー

Nav2との接続: コストマップと速度指令

• AMCL・SLAMがamcl_pose（またはそれに相当するTF）として自己位置を確定させると、Nav2はそれをもとにグローバルプランナーで大まかな経路を計画し、コントローラー（ローカルプランナー）が直近の障害物を避けながら実際の速度指令を計算する
• この「直近の障害物」を表すのがコストマップで、/scanのLiDARデータがリアルタイムに反映される。地図上の静的な障害物（グローバルコストマップ）と、今まさにLiDARが検出している動的な障害物（ローカルコストマップ）の両方に、同じ/scanデータが使われる
• 最終的にコントローラーが計算した速度指令がcmd_velとして配信され、それが駆動系シミュレーション（DiffDriveプラグイン）に渡って実際にホイールが回り、ロボットが移動する。この移動がまた新しいオドメトリ・LiDARスキャンを生み、ループが続いていく

まとめ

• Gazebo上の駆動系シミュレーション（差動二輪駆動の場合はgz-simのDiffDriveプラグイン）は、cmd_velを受けてホイールを回転させ、その回転量からオドメトリを逆算し、odom->base_linkのTFとトピックを配信する
• このオドメトリは、ホイールの回転だけから積分した値のため、実機・シミュレーション問わずドリフトする。この誤差を補正するのが、既知の地図があるときのAMCL、地図がないときのslam_toolboxの役割
• AMCL・slam_toolboxはどちらも、オドメトリ（動作モデル／スキャンマッチングの初期値）とLiDARスキャン（観測モデル／地図とのマッチング）を使い、odom->base_link自体は書き換えず、別レイヤーのmap->odomというTFを配信することで、地図座標系での自己位置を補正する
• 両者ともmap->odomを配信するため、同時には動かさず、地図作成フェーズ（SLAM）と運用フェーズ（AMCL）を切り分けて使う
• Nav2は、この自己位置と、LiDARスキャンから作られるコストマップをもとに経路計画・障害物回避を行い、最終的な速度指令cmd_velを駆動系に戻すことで、駆動系→センサー→自己位置推定→ナビゲーション→駆動系という1つの閉じたループが成立する
• SLAM・AMCLが扱うのはあくまで幾何学的な位置情報で、「その場所が何をする場所か」という意味はOWL/RDFのような別レイヤー（Semantic SLAMという研究分野も含む）で扱う、必須ではない拡張領域になる

参考文献

• ROS2の概念を理解するためのメモ
• gazebosim/gz-sim - DiffDrive system plugin - GitHub
• ros-navigation/navigation2 - nav2_amcl - GitHub
• SteveMacenski/slam_toolbox - GitHub
• ros-navigation/navigation2 - GitHub
• ROSの座標変換TFについて