Proprioceptive sensor

ロボットのセンサは2つの系統に分かれる。Proprioceptive sensors(内部センサ) はロボット自身の内部状態 — 自身の運動 — を計測し、一方Exteroceptive sensors(外部センサ)(カメラ、LiDAR、RADAR、ソナー)は外部の環境を計測する。SLAMで常に出会う2つの内部センサは以下の通り。

• IMU (Inertial Measurement Unit、慣性計測装置): 線形加速度 $\mathbf{a}_m$ (3軸加速度計)と角速度 $\boldsymbol{\omega}_m$ (3軸ジャイロスコープ)を高レート(100〜1000 Hz)で計測する。計測値は白色雑音と、オンラインで推定する必要がある緩やかに変化するバイアス $\mathbf{b}^a, \mathbf{b}^g$ によって汚染される。加速度計は重力も検知するため、これを補正する必要がある。
• ホイールエンコーダ: 差動駆動またはアッカーマン式の車両でホイールの回転数を数え、車両の運動学モデルを通じて線速度と角速度を得る。清潔な屋内フロアでは高精度だが、屋外ではホイールのスリップ、不均一な地面、タイヤの変形によって劣化する。

内部センサの計測値は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),$$

これは現在の状態から次の状態を予測する。それらの決定的な強みと弱みは以下の通り。

内部センサはドリフトするが決して故障せず、外部センサは高精度だが故障しうる(ブラー、低テクスチャ、遮蔽)ため、この2つの系統は自然な補完関係にある — これはvisual-inertial odometryの全体の前提であり、IMUの事前積分がキーフレーム間の数百件の慣性計測を1つの相対運動制約にまとめる。

計測モデル

標準的なIMU計測モデルは、雑音構造を明示的に表す。ボディフレーム $B$、ワールドフレーム $W$、ボディの姿勢を $R_{WB}$ とすると、

$$\boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}^g + \mathbf{n}^g, \qquad \mathbf{a}_m = R_{WB}^T(\mathbf{a}_W - \mathbf{g}) + \mathbf{b}^a + \mathbf{n}^a$$

注目すべき点が3つある。加速度計は**比力(specific force)**を計測しており、加速度そのものではない。重力 $\mathbf{g}$ がモデル内に現れるため、静止しているIMUは上向きに約 $9.81\,\text{m/s}^2$ を示し、姿勢誤差は重力を推定加速度に漏れ込ませる — $1°$ の傾き誤差は $9.81 \sin(1°) \approx 0.17\,\text{m/s}^2$ の見かけの水平加速度を注入し、これが数秒以内に二重積分されてメートル単位の誤差になる。雑音 $\mathbf{n}$ はホワイトガウス過程としてモデル化され、バイアス $\mathbf{b}$ は緩やかなランダムウォークとしてモデル化される。これが推定器がバイアス状態を保持し、連続的に更新する理由である。そして全てはボディフレームで表現されるため、それを使うにはまさに推定しようとしている姿勢そのものが必要になる — これが慣性ナビゲーションを1つの結合推定問題に結びつける要因である。

差動駆動ベースのホイールエンコーダ(ホイール半径 $r$、ホイール間隔 $L$、計測されたホイール角速度 $\omega_l, \omega_r$)については、

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

雑音は(優れている)エンコーダの分解能によってではなく、モデルの違反、つまりスリップ、スキッド、不均一な地面、タイヤの圧縮によって支配される — これらはホワイトノイズではなく、系統的かつ地形依存の誤差である。

どれくらい速くドリフトするか

概算のヒエラルキーとして有用なのは以下の通り。ジャイロスコープのみの方位は時間に対して線形にドリフトする。加速度計からの速度は線形にドリフトする(加えて姿勢誤差による重力の漏れ込み)。位置は加速度計誤差から二次的に、そしてジャイロによる傾き誤差からは三次的にドリフトする。これが、コンシューマー向けMEMS IMUが数秒以上単独で位置を提供できない理由であり、一方で(光ファイバージャイロを用いた)タクティカル/ナビゲーショングレードの装置は、はるかに高いコスト・サイズ・電力消費で数分から数時間にわたって保持できる理由である。ホイールオドメトリは時間ではなく移動距離に応じてドリフトするため、静止しているホイールロボットは全くドリフトしない — これがホイール+IMU融合が屋内で非常に効果的である理由の一つである。

よくある落とし穴

• バイアスを無視する — $\mathbf{a}_m, \boldsymbol{\omega}_m$ をバイアスなしの計測値として扱うのは、データシートの上ではうまくいくが、実機のロボットでは失敗する。バイアスは温度によって変化し、電源の入れ直しごとにも変化するため、定数ではなく状態として扱う必要がある。
• 重力初期化の失敗 — VIOシステムは静止中に加速度計の重力から初期姿勢を推定する。運動中(または振動するプラットフォーム上)での初期化は、傾き推定を破壊し、以降すべての処理に悪影響を及ぼす。
• 振動とエイリアシング — モーターやプロペラは、IMUのサンプリング仮定を超える振動を注入する。機械的な減衰や適切なローパスフィルタリングは、アルゴリズムと同じくらい重要である。
• 運動学パラメータもキャリブレーションの対象である — ホイール半径やホイールベースの誤差は「直進」軌跡に系統的な曲率を生じさせる。これらをキャリブレーションすること(タイヤ交換時には再キャリブレーションすること)。

SLAMにおける意義

デプロイされているSLAMシステムのほぼすべてが、内部センサと外部センサの融合である。スマートフォンやヘッドセット上のVIO(カメラ+IMU)、倉庫ロボットのホイール-慣性オドメトリ、自動車のLiDAR-慣性オドメトリなどである。内部センサが何を計測するか、どれくらい速くドリフトするか、その雑音がどのようにモデル化されるかを知ることは、レベル6でのカルマンフィルタとファクターグラフ融合を理解するための前提条件である。

関連ノート

• Exteroceptive sensor
• IMU
• Odometry
• IMU noise model