差動二輪ロボットの駆動系とAMCL・SLAM・Nav2

Wed, 14 Oct 2026 11:00:00 +0900

背景

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

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

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

順序	段階	内容
1	駆動系シミュレーション	Gazeboが、ホイールの関節の回転からロボットの移動をシミュレートし、オドメトリを計算する
2	センサーシミュレーション	Gazeboが、LiDARセンサーをシミュレートし、距離測定データを生成する
3	自己位置推定	AMCL（既知の地図がある場合）またはSLAM（地図がない場合）が、オドメトリとLiDARデータから、地図上での自己位置を推定する
4	経路計画・制御	Nav2が、推定された自己位置とLiDARベースのコストマップを使って、経路を計画し、速度指令（`cmd_vel`）を出す
5	駆動系への反映	`cmd_vel`がGazeboの駆動系シミュレーションに戻され、ロボットが実際に動く

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

TFツリー全体像

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

1

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

変換	発行元	内容
`odom` -> `base_footprint`（実質的に`base_link`）	駆動系シミュレーション（Gazeboのプラグイン、実機ではモータードライバ側）	ホイールの回転から計算した、ロボット自身の移動量（オドメトリ）
`map` -> `odom`	AMCLまたはSLAM	オドメトリの誤差を、LiDARと地図の照合で補正した分のズレ
`base_footprint` -> `base_link`、`base_link` -> `base_laser`など	`robot_state_publisher`（URDFから自動計算）	ロボット本体の構造に対する、各フレーム・センサーの固定された取り付け位置

重要なのは、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`	ワールド座標	ロボットが位置する世界座標系。REP-105によれば、Z軸が上向きで、地図に対する位置は長期的にほとんどドリフトしないが、再位置推定のタイミングで不連続にジャンプすることがある
`odom`	ワールド座標（ローカルに連続）	オドメトリ（車輪エンコーダなどの距離計）を積分した座標系。REP-105によれば連続的でなめらかな一方、実際の世界座標（`map`）とは長期的にズレ（ドリフト）ていく。このズレを補正するのが、前述の`map`->`odom`変換
`base_footprint`	ローカル座標	`base_link`を地面に投影したフレーム。XY座標・向きは`base_link`と連動するが、Z座標は常に地面の高さ（0）に固定される。2Dのナビゲーションアルゴリズムが、ロボットを平面上の点として扱えるようにするためのフレーム
`base_link`	ローカル座標	ロボット本体に固定された基準フレーム。車体の実際の高さ・姿勢を含む、ロボットそのものの位置
`base_laser`（本記事では`laser_link`と表記）	ローカル座標	LiDARなどのレーザースキャナが取り付けられている位置を表すフレーム。具体的な名前はロボットごとに異なる

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$は以下の式で計算される

$$ v = \frac{r(\omega_R + \omega_L)}{2}, \quad \omega = \frac{r(\omega_R - \omega_L)}{L} $$

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

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

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

項目	内容
入力トピック	`/model/<モデル名>/cmd_vel`（`geometry_msgs/Twist`、トピック名は`<topic>`タグで変更可能）
出力トピック（オドメトリ）	`/model/<モデル名>/odometry`（`<odom_topic>`タグで変更可能）
出力トピック（TF）	`/model/<モデル名>/tf`（`<tf_topic>`タグで変更可能）
主なパラメータ	`<left_joint>`・`<right_joint>`（ホイールの関節名）、`<wheel_separation>`（ホイール間距離、デフォルト1.0m）、`<wheel_radius>`（ホイール半径、デフォルト0.2m）、`<odom_publish_frequency>`（オドメトリ配信周波数、デフォルト50Hz）
発行するTF	`frame_id`は`<モデル名>/odom`、`child_frame_id`は`<モデル名>/<ホイールが接続されたリンク名>`がデフォルト

つまり、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との違いは以下の通り

項目	AMCL	slam_toolbox
前提	既知の地図が必要	地図は不要（走行しながら作る）
役割	自己位置推定のみ	自己位置推定＋地図作成（SLAM）を同時に行う
使う情報	オドメトリ＋LiDARスキャン＋既存の地図	オドメトリ＋LiDARスキャンのスキャンマッチングで地図を構築・更新
内部構造	パーティクルフィルタ	ポーズグラフ（過去の位置同士の関係をグラフ化）とループクロージャ（同じ場所に戻ったことを検出して誤差を補正）の最適化
発行するTF・トピック	`map`->`odom`	`map`->`odom`、および`/map`（占有格子地図）、`/slam_toolbox/pose`

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という研究分野も含む）で扱う、必須ではない拡張領域になる

AMCL on M1KE BL0G

差動二輪ロボットの駆動系とAMCL・SLAM・Nav2

背景

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

TFツリー全体像

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

ROS座標フレームの一覧

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

差動二輪駆動の仕組み

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

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

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

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

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

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

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

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

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

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

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

まとめ

参考文献