オドメトリ

**オドメトリ(odometry)**とは、時間をかけて増分的な運動計測を積分することでロボットの姿勢を推定することである——デッドレコニング。既知の初期姿勢から出発し、新たな計測ごとに小さな相対運動 $\Delta T_k$ が得られ、現在の姿勢はすべての増分の合成として表される。

$$T_k = T_0 \cdot \Delta T_1 \cdot \Delta T_2 \cdots \Delta T_k$$

「オドメトリ」という語は元々**ホイールオドメトリ(wheel odometry)**を指していた。エンコーダがホイールの回転数を数え、キネマティックモデル(差動駆動、アッカーマン)がそれを線速度と角速度に変換する。同じ考え方は、相対的な運動を計測できる任意のセンサに一般化できる。

• ホイールオドメトリ — 車輪型ロボットでは安価で常に利用可能だが、特に屋外ではホイールスリップによって精度が損なわれる。
• 慣性オドメトリ — IMUの加速度と角速度を積分する。加速度を二重積分するため、単独では非常に速く発散(ドリフト)する。
• 視覚オドメトリ(VO) — 画像の対応関係からフレーム間のカメラ運動を推定する。
• LiDARオドメトリ — 連続する点群を位置合わせする(例:ICPや特徴ベースのレジストレーション)。

オドメトリの決定的な特性はドリフトである。各増分は小さな誤差を持ち、増分は乗法的に連結されるため、誤差は無制限に蓄積される。1メートルあたり0.5%の並進誤差は、デスクサイズの動きでは目に見えないが、1キロメートルでは致命的になる。したがって、オドメトリだけでは大域的に一貫した地図を生成することは決してできない——それにはループ閉じ込みと大域的最適化が必要であり、これこそがオドメトリとSLAMを分ける点である。

現代のSLAMバックエンドでは、オドメトリは相対姿勢制約として現れる。ポーズグラフにおける連続する姿勢ノード間のエッジ、あるいはファクターグラフにおける動きモデルファクターとして、

$$\mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) + \mathbf{w}_t, \qquad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t)$$

ここで制御/計測 $\mathbf{u}_t$ はエンコーダ、IMU、あるいはVOフロントエンドから得られ、$Q_t$ はその不確実性をモデル化する。

演習例:差動駆動オドメトリ

ホイール半径 $r$、エンコーダで計測されたホイール角速度 $\omega_l, \omega_r$、ホイール間距離 $L$ を持つ2輪差動駆動ロボットについて、機体速度は次のようになる。

$$v = \frac{r(\omega_r + \omega_l)}{2}, \qquad \omega = \frac{r(\omega_r - \omega_l)}{L}.$$

平面上の姿勢 $(x, y, \theta)$ をタイムステップ $\Delta t$ にわたって積分する(最も単純なオイラー形式)。

$$x_{k+1} = x_k + v\cos\theta_k\,\Delta t, \qquad y_{k+1} = y_k + v\sin\theta_k\,\Delta t, \qquad \theta_{k+1} = \theta_k + \omega\,\Delta t.$$

この十行程度の積分器は、完全なオドメトリシステムであり——典型的な失敗モードすべてを示す。位置の更新が $\theta_k$ に依存していることに注目してほしい。方位の誤差があると、それ以降の並進はすべて誤った方向に回転してしまう。距離 $d$ を方位誤差 $\delta\theta$ を持ったまま走行した後の横方向の位置誤差はおおよそ $d \cdot \delta\theta$ である——これがなぜ方位(ジャイロの品質、ホイールベースのキャリブレーション)がオドメトリ精度を支配するのか、そして安価なジャイロをホイールオドメトリに追加するだけで大きく改善する理由である。

ドリフトの振る舞い

• ランダム誤差はランダムウォークとして蓄積する:各増分が独立なノイズを持つ場合、積分された姿勢の分散は移動距離とともに増大する——不確かさの楕円は着実に膨張し、外部からの補正なしには決して縮小しない。
• 系統誤差は線形に増大する:キャリブレーションが不正確なホイール半径(例えばタイヤの空気圧による)やホイールベースは、すべての増分を同じ方向に偏らせる——直線通路を走行しても、毎回同じように少し曲がって戻ってくる。系統誤差はキャリブレーションによって取り除くことができ、取り除くべきものである。
• 方位誤差が支配的である:演習例が示すように、角度誤差は移動距離に比例した位置誤差に変換される。並進のみの誤差はただ加算されるだけである。
• 誤差の合成は $SE(3)$ 上で乗法的である:増分は変換として合成されるため、誤差は単純には加算されない——これが、オドメトリの共分散伝播が合成のヤコビアン(あるいはより整然とはリー群の機構)を用いる理由である。

実用上、オドメトリの品質は相対誤差として——移動距離に対する割合や1メートルあたりの角度で——表現される。これはまさにKITTIオドメトリベンチマークと相対姿勢誤差(RPE)指標が計測している対象である。絶対軌跡誤差(ATE)はオドメトリシステムには誤ったレンズである。なぜなら絶対誤差は設計上無限に増大するからである。

よくある落とし穴

• オドメトリの出力をそのまま下流でグラウンドトゥルースとして扱う — 常にその共分散を伝播させるべきである。オドメトリの不確実性を無視するプランナーや融合スタックは、確信を持って間違い続ける。
• フレームのセマンティクスの誤り(ROS) — オドメトリは連続的でドリフトするodom -> base_link変換として公開されるべきであり、ループを閉じるSLAM補正はmap -> odomに存在する。補正が到着したときにオドメトリを飛躍させると、それを利用するすべてのコントローラが破綻する。
• タイムスタンプの取り扱いの粗さ — 誤った $\Delta t$ に対してエンコーダのティックを積分したり、速度を誤った画像とペアにすると、センサノイズと全く同じように見える誤差を注入するが、それは純粋な帳簿上のバグである。
• スリップとキネマティックの不整合を無視する — 差動駆動の方程式は滑りのない転がり運動を仮定している。カーペット、砂利、あるいは急な旋回時には、これは静かに破綻する。それを平均化するのではなく検出すべきである(例:ジャイロとエンコーダが示唆するヨーの不一致)。

SLAMにおける意義

オドメトリはすべてのSLAMシステムの背骨である。それは、トラッキング、マッピング、ループクロージングモジュールが構築の基盤とする高頻度かつ局所的に正確な運動推定を提供する。その誤差特性——局所的には滑らかだが大域的にはドリフトする——を理解することは、現代のSLAMのアーキテクチャを説明する。短い時間軸ではオドメトリを信頼し、ループ閉じ込みや絶対計測が利用可能になったときには常に大域的に補正する。

関連ノート

• 固有受容センサ
• 視覚オドメトリ
• VO対SLAM
• ポーズグラフ最適化