センサーフュージョン on Physical AI Hub

ロボット自己位置推定のセンサーフュージョン設計：カメラ・LiDAR・IMUの役割分担

Thu, 13 Aug 2026 01:47:13 +0900

センサーを増やすほど自己位置推定が安定するのか、環境条件から考え直せる点がこのテーマの面白さです。

導入

ロボットの自己位置推定で、カメラ・LiDAR・IMUを常にすべて使えばよいとは限りません。選択の基本は、IMUを短時間の運動伝播に使い、カメラは視覚特徴、LiDARは距離と幾何形状の拘束に使うことです。明るく特徴の多い環境ではカメラ+IMU、暗所や照明変動下で幾何が豊富な環境ではLiDAR+IMUが有力です。視覚と幾何のどちらか一方が失敗しやすく、冗長性も必要なら3センサー構成を検討します。

ただし、センサー数の多さだけで精度が決まるわけではありません。時刻同期、外部パラメータ、座標系、IMUバイアスを整合でき、追加される計算量と実装負荷を許容できることが前提です。

なぜ組み合わせの判断が必要か

自己位置推定は、各センサーの観測を同じ状態推定問題へ取り込む処理です。センサーには得意な時間スケールと失敗条件があります。IMUは高レートですが、バイアスやノイズを積分するため、単独では時間とともにドリフトします。カメラは再観測による相対的な拘束を与えられますが、照明やテクスチャに左右されます。LiDARは照明そのものではなく形状を観測できますが、幾何的に特徴の少ない場所や点群が疎になる条件では位置合わせが退化します。

したがって設計の問いは「どのセンサーが最も高性能か」ではなく、「対象環境でどの観測が残り、どの失敗を別の観測で補えるか」です。

各センサーが提供する情報

IMU：高レートの短期伝播

IMUは角速度と比力を取得し、姿勢・速度・位置の短時間の伝播を支えます。カメラやLiDARの観測間をつなぎ、高速運動時にも状態を更新できる点が役割です。一方、慣性積分だけでは誤差が蓄積するため、カメラやLiDARからの拘束でバイアスと状態を補正します。

カメラ：特徴と外観による相対拘束

カメラは画像特徴の追跡と、外観情報の再観測を利用します。十分な照明、視覚テクスチャ、適度なモーションがある環境では、IMUと組み合わせたVisual-Inertial Odometry（VIO）が機能します。画像特徴を一時的に追跡できない場合でも、IMUが短時間の運動拘束を提供しますが、視覚観測が長く失われれば慣性ドリフトを抑えにくくなります。

LiDAR：距離と幾何による拘束

LiDARは点群の距離・幾何形状を使い、スキャン間またはスキャンと地図の位置合わせを行います。暗所や照明変動の影響を受けにくい環境では、IMUと組み合わせたLiDAR-Inertial Odometry（LIO）が候補になります。IMUは短時間の運動推定だけでなく、点群のモーションデスクューにも利用できます。ただし、広く平坦な場所など幾何的な手掛かりが少ない環境では、LiDARの拘束が弱くなる可能性があります。

flowchart LR
  I["IMU\n角速度・比力"] --> P["高レートの短時間伝播\n姿勢・速度・位置"]
  C["カメラ\n画像特徴・再観測"] --> V["視覚拘束"]
  L["LiDAR\n距離・点群・幾何形状"] --> G["幾何拘束\nスキャン間または地図との位置合わせ"]
  I --> D["点群のモーションデスクュー"]
  D --> G
  P --> E["状態推定器"]
  V --> E
  G --> E
  E --> S["状態・IMUバイアスの補正"]
  E -.-> Q["品質指標の監視\n残差・イノベーションなど"]
  E --> M["推定状態"]
  K["EKF系または因子グラフ・平滑化系"] --> E

IMUによる短時間伝播に、カメラの視覚拘束とLiDARの幾何拘束を組み合わせる概念図。実際の性能や処理順序は実装に依存します。

代表的な構成の比較

構成	主な観測	向く条件	主な失敗条件・負荷
IMU単独	角速度・比力の積分	高レートの短期伝播	バイアスとノイズによるドリフト
カメラ+IMU（VIO）	画像特徴、再観測、慣性	明るく視覚特徴が豊富な屋内外	暗所、照明変動、テクスチャ不足、視覚追跡喪失
LiDAR+IMU（LIO）	点群の距離・幾何、慣性	暗所、照明変動下、幾何が豊富な場所	幾何的特徴の不足、点群が疎な条件
カメラ+LiDAR+IMU	視覚、幾何、慣性	視覚と幾何の一方が失敗しやすい環境、冗長性が必要な長時間運用	計算量、実装・校正負荷、同期や座標整合の複雑化

3センサー構成は、カメラとLiDARの観測が相互補完になるときに意味があります。たとえば照明条件が不安定でも形状が豊富ならLiDARが支え、幾何が一時的に弱くても視覚特徴が残っていればカメラが補います。ただし、3センサーの優位性をあらゆる条件で示す一般的な比較結果があるわけではなく、採用判断は環境、実装、計算資源に依存します。

カメラとLiDARの外部キャリブレーション：点群と画像を正しく重ねる方法

Sat, 25 Apr 2026 00:51:08 +0900

画像と点群のずれは、外部変換だけでなく時刻やレンズ歪みにも現れます。切り分けを手順に組み込めるかが気になったテーマです。

結論

カメラとLiDARの融合で最初に固定すべきなのは、校正アルゴリズムよりも座標系、変換方向、単位、時刻基準です。そのうえで、カメラの内部パラメータとレンズ歪みを先に校正し、同期した観測からLiDAR座標系をカメラ座標系へ写す剛体変換を推定します。最後に、校正に使っていない姿勢・距離・視野・速度のデータへ点群を再投影し、誤差の大きさと分布を確認します。

校正時の残差が小さいだけでは不十分です。画像上のずれが内部パラメータ、外部変換、時間オフセット、LiDARの走査遅延のどれに由来するかを条件別に切り分け、用途に応じた合格基準を定める必要があります。

なぜ外部キャリブレーションが重要か

カメラは画素、LiDARは3次元点として同じ対象を観測します。しかし、それぞれの座標原点や軸方向は異なります。外部パラメータに誤りがあると、LiDAR点を画像へ重ねたときに、物体の輪郭から一定方向へずれたり、距離によってずれが変化したりします。物体検出への深度付与、障害物の位置推定、画像と点群の対応付けでは、この空間的な不整合が直接結果に影響します。

前提となる用語

外部パラメータは、LiDAR座標系の点をカメラ座標系へ変換する回転と並進、つまり剛体変換として管理します。実装では、変換をどちら向きに適用するのか、フレーム名、長さの単位、時刻基準を明示します。フレーム間の関係は一貫したフレームツリーで管理し、同じ数値を別の座標軸定義へそのまま移植しないことが重要です。

内部パラメータは焦点距離や主点など、カメラ内部の投影特性を表します。レンズ歪みモデルも画像投影に関係するため、外部変換とは分けて推定し、外部キャリブレーションの前に妥当性を確認します。内部行列と歪みモデルが不正確なままでは、外部変換を調整しても再投影誤差を正しく解釈できません。

推奨する校正手順

1. センサーを剛体として固定する

カメラとLiDARの取り付けを固定し、校正中に相対位置が変わらない状態を作ります。振動や取り付け部の変形が疑われる場合、得られた変換はその時点の状態に限られる可能性があります。

2. 座標系と時刻を確認する

各センサーのフレーム名、軸方向、単位、変換の保存方向を記録します。さらに、ハードウェア同期の有無、LiDAR点ごとのタイムスタンプ、走査中の運動補償の有無を確認します。移動中に時間同期が不足すると、時間オフセットが空間的な外部パラメータ誤差として現れます。必要なら時間オフセットを別パラメータとして推定・検証します。

3. カメラ内部を校正する

平面パターンなどを複数の視点から撮影し、内部パラメータ、歪み、各撮影姿勢を推定します。単一画像だけに依存せず、複数視点の観測で結果を確認します。ここで得た投影モデルを固定してから、カメラとLiDARの外部変換へ進みます。

4. 対応を集めて初期値を求める

既知形状のターゲットを使う場合、LiDAR側の3次元特徴と画像側の2次元特徴、またはターゲット平面を対応付けます。十分な姿勢や視点の違いを含む観測から、まず外部変換の初期値を求めます。初期値の品質が低いと、後段の非線形最適化が適切な解へ到達しない場合があります。

5. SE(3)上で最適化する

初期変換を出発点に、3次元点をカメラ座標系へ変換し、内部パラメータと歪みモデルを適用して画像へ投影します。3D-2Dの再投影残差、点と平面の残差など、利用できる対応に応じた目的関数を最適化します。

flowchart LR
  A["センサーを剛体として固定"] --> B["座標系・単位・変換方向・時刻を確認"]
  B --> C["カメラ内部パラメータと歪みを校正"]
  C --> D["同期した観測データを収集"]
  D --> E["3D-2Dまたは特徴・平面の対応を作成"]
  E --> F["外部変換の初期値を推定"]
  F --> G["SE(3)上で再投影・幾何残差を最適化"]
  G --> H["未使用データで再投影・幾何・時間整合を検証"]
  H --> I["条件別の誤差を診断して合否基準を判断"]
  I -. "運動時のみ誤差が増える場合" .-> B

外部キャリブレーションは、内部校正と座標・時刻の確認を分けたうえで、初期推定、SE(3)最適化、独立データによる検証の順に進めます。

ターゲット方式とターゲットレス方式

既知ターゲット方式は、対応する幾何が明確で、初期推定と残差の意味を説明しやすい方法です。一方、設置や撮影の準備が必要で、ターゲットの見え方や配置が観測条件を制限します。

ターゲットを設置しない方式では、LiDAR点群の反射強度や投影情報と、画像の明るさ・外観との相互情報量を最大化する方法が提案されています。また、画像エッジなどの視覚特徴とレーザー点の幾何を使い、運用中に外部パラメータを調整する方式もあります。ただし、シーンのテクスチャ、反射率、天候、動的物体による安定性は個別データで確認する必要があります。

方式	準備	強み	注意点
既知ターゲット	ターゲット設置と対応抽出	幾何的に解釈しやすく、初期推定を作りやすい	設置条件と視点の設計が必要
相互情報量によるターゲットレス	専用ターゲット不要	実環境の観測を利用できる	外観や反射特性への依存を確認する必要がある
画像エッジとレーザー特徴	運用中の特徴観測	固定ターゲットに依存せず更新を目指せる	特徴の量と動的環境の影響を受ける
時空間キャリブレーション	同期状態を含む観測	空間変換と時間オフセットを同時に扱える	固定・同期済みのシステムでは必要な自由度を見極める必要がある

融合結果の検証方法

推定した変換でLiDAR点をカメラ座標系へ移し、深度が正の点だけを残します。その後、内部行列と歪みモデルで画像へ再投影し、次の項目を記録します。

ロボットセンサーの時刻同期設計：カメラ・LiDAR・IMUのデータを正しく重ねる方法

Fri, 06 Mar 2026 00:23:05 +0900

センサー融合では、値の精度だけでなく「いつ取得した値か」の定義が結果を左右します。

先に結論

カメラ・LiDAR・IMUの時刻同期では、近いタイムスタンプのメッセージを組み合わせるだけでは不十分です。まず、各センサーがどのクロックを使い、タイムスタンプが取得時刻、露光時刻、スキャン時刻、受信時刻のどれを表すのかを確認します。そのうえで、可能な範囲ではPTP、PPS、外部トリガーなどを使って共通の時間基準を構築し、残った固定オフセットやドリフトをログから推定します。

補正では、センサー時刻を基準時刻へ変換し、IMUの状態補間、LiDARのデスキュー、カメラの露光時刻モデルを適用します。最後に、共通イベントの時刻差、運動信号の相関、推定残差、再投影誤差を確認します。ROS 2のApproximateTimeはメッセージの対応付けには使えますが、センサー内部の時計を直す機能ではありません。

なぜ時刻同期が問題になるのか

センサーフュージョンでは、同じ「時刻」のデータを重ねているつもりでも、実際には異なる時間基準、単位、エポックを参照していることがあります。センサーが生成した時刻をドライバがホスト受信時刻に置き換えている場合もあります。

ずれは、主に四つに分けて考えます。一定に近い固定オフセット、クロック周波数の差によるドリフト、通信・ドライバの可変遅延、タイムスタンプが示す物理的な時刻の違いです。最後の問題は、数値に固定値を加えるだけでは解消できません。角速度をω、時刻誤差をΔtとすると、短時間近似では角度誤差は概ね|ω|×|Δt|に比例します。高速回転や急加速、動体追跡、点群の画像投影ほど同期ずれの影響が大きくなります。

前提となる時刻の意味

ROS 2ではSystem Time、Steady Time、ROS Timeが区別されます。ROSの時刻とセンサーのハードウェア時刻が自動的に同一になるわけではありません。IMUメッセージのHeader stampは測定時刻として扱われますが、メッセージ定義だけでは、センサーのクロックやドライバが時刻を付与した位置までは保証されません。

カメラでは、フレーム時刻が露光開始、露光中心、露光終了、転送完了のどれを示すかを確認します。ローリングシャッターでは行ごとに露光時刻が異なるため、フレーム全体を単一時刻の観測として扱えるとは限りません。

LiDARの1スキャンも瞬間的な観測とは限りません。ビーム、列、パケットなどで取得時刻が異なる場合があるため、点ごとの時刻、またはセンサー固有のスキャン時刻モデルを使って運動補正します。時刻の提供粒度は機種、ファームウェア、データ形式、ドライバによって異なります。

同期の仕組みと検出方法

PTP（IEEE 1588）は、ネットワーク上のクロック間のオフセットと遅延を測定して同期する標準です。ハードウェアタイムスタンプを使える実装では、ソフトウェアだけで時刻を取得する場合より不確実性を抑えやすくなります。ただし、精度はネットワーク遅延の対称性、タイムスタンプ方式、プロファイル、実装に依存します。ptp4lでネットワーク上のクロックを同期しても、LinuxのPHC、システムクロック、センサーのタイムスタンプ経路まで自動的に同一になるわけではありません。

PPSは時刻基準をそろえる手段、外部トリガーは取得イベントをそろえる手段です。ただし、対応機種、ファームウェア、ドライバによって意味が異なります。同期パルスを入力できても、センサー内部の取得からアプリケーション到着までの遅延は別途検証します。

flowchart LR
  subgraph S1["センサー側の時刻"]
    C["Camera Clock\n露光時刻モデル"]
    L["LiDAR Clock\n点・パケット・スキャン時刻"]
    I["IMU Clock\n測定時刻"]
  end

  P["PTP・PPS・外部トリガー\n利用可能な同期手段"]
  R["Reference Clock\n共通の基準時刻"]
  T["時刻変換\nt_ref = α_i × t_i + β_i"]
  X["共通タイムライン\n対応付け・補間・融合"]

  P -. "クロックまたは取得イベントを同期" .-> C
  P -. "クロックまたは取得イベントを同期" .-> L
  P -. "クロックまたは取得イベントを同期" .-> I
  C --> T
  L --> T
  I --> T
  R --> T
  T --> X

  D["固定オフセット β_i\nクロックレート差 α_i"]
  D -. "ログから推定" .-> T

各センサーの時計とタイムスタンプを共通の基準時刻へ変換し、共通タイムラインでデータを対応付ける概念図。PTPなどを有効にしても、センサーのタイムスタンプ経路や取得時刻の意味は別途確認が必要です。

検出の第一歩は、複数センサーで共通イベントを観測することです。同期パルスや外部イベントについて、各センサーが記録した時刻差を複数回測定し、平均値と分散を求めます。平均的な差は固定オフセット、分散はイベント検出や通信のばらつきを考える材料になります。

ロボットに触覚と力覚を加える設計判断：視覚との役割分担

Sun, 25 Jan 2026 00:05:01 +0900

視覚に情報を集約する設計は分かりやすい一方、接触した瞬間から別の観測が必要になる点が気になります。

先に結論

ロボットに触覚・力覚センサーを追加する価値は、視覚を置き換えることではなく、接触後に増える観測の曖昧さを減らし、制御を切り替えられる点にある。視覚は対象の大域的な位置や形状を非接触で捉える。一方、指先などの触覚センサーは接触部の圧力、変形、局所形状を捉え、手首の力覚センサーは手先に作用する合力・合トルクを捉える。

したがって、対象への接近と大域的位置決めには視覚、接触検出と荷重監視には力覚、把持面の状態や滑り兆候の推定には触覚を使い分ける構成が基本になる。ただし、視覚だけで接触状態を間接推定できる場合もあるため、採用は作業の接触性、必要精度、損傷リスク、実装負荷を比較して決めるべきである。

なぜ視覚だけでは不十分になりやすいのか

カメラは対象全体の位置や形状を把握するのに向いている。しかし、手や対象に隠れた接触面、接触パッチの形状、接触圧力の分布は、視覚から直接観測しにくい。把持後に物体が正しい位置にあるように見えても、指先の接触が偏っていたり、滑り始めていたりする可能性がある。

また、挿入や嵌合では、目標位置が分かっていても、部品同士がどの方向で接触しているか、押し付けが強すぎないかを位置情報だけで判断しにくい。接触力やトルクを監視しながら、硬く位置を追従する状態から、環境になじむコンプライアンス制御へ切り替える必要がある。

「視覚では解けない」という表現は絶対的ではない。既知の対象、十分な視認性、適切な物理モデル、学習済みの推定器があれば、視覚から把持成否や力を間接的に推定できる場合もある。触覚・力覚は、視覚の全面的な代替ではなく、接触状態を観測する追加チャネルと考えるのが適切だ。

前提となるセンサーの違い

触覚センサーは、指先やハンド表面など、接触に近い場所で局所的な情報を取得する。方式によって、法線力、せん断力、圧力分布、接触形状など、得られる物理量は異なる。光学式触覚センサーの一例では、接触によるエラストマー表面の変形を画像として取得し、接触形状や力を推定する。

力覚センサーは、代表的には手首に配置され、Fx、Fy、FzとTx、Ty、Tzの6成分を扱う。複数の接触が手先に与える総合的な荷重を監視できる一方、センサー単独で接触点や圧力分布を一意に復元できるとは限らない。局所接触に強い触覚と、手先全体の荷重監視に強い力覚は代替関係ではない。

センサー	主な観測対象	強い場面	苦手・制約
視覚	大域的な位置・形状	接近、対象認識、初期位置決め	遮蔽された接触面や圧力分布を直接観測しにくい
指先触覚	局所圧力、変形、接触形状、接触状態	把持成否、接触位置、滑り兆候の推定	局所情報が中心で、手先全体の合力・合トルクは直接表さない
手首6軸力覚	手先に作用する合力・合トルク	接触検出、荷重監視、挿入・嵌合	接触点や圧力分布を単独では一意に決めにくい

内部の仕組みとデータフロー

実装では、センサーを常時同じ目的で使うのではなく、作業状態に応じて役割を切り替える。まず視覚で対象の位置と形状を推定し、ロボットを接触直前まで移動させる。次に力覚の変化から接触を検出し、位置制御からインピーダンス制御やハイブリッドな位置・力制御へ移行する。これにより、押し付け力や接触時の剛性を調整できる。

接触後は、指先触覚から局所的な接触分布や把持状態を確認する。把持が不十分、または滑りの兆候があると判断した場合は、把持力や手の姿勢を調整し、必要なら再把持する。視覚と触覚から把持成否を予測して再把持する方策は、研究上も提案・評価されている。最後に、視覚と接触情報を組み合わせ、作業完了、異常、再試行を判定する。

flowchart LR
  A["視覚で対象の位置・形状を推定"] --> B["接触直前まで接近・初期位置決め"]
  B --> C["力覚の変化を監視"]
  C --> D{"接触を検出したか"}
  D -- "いいえ" --> B
  D -- "はい" --> E["位置制御からインピーダンス制御・ハイブリッド位置力制御へ切り替え"]
  E --> F["押し付け力・接触時の剛性を調整"]
  F --> G["指先触覚で局所接触分布・把持状態を確認"]
  G --> H{"把持不十分または滑り兆候ありか"}
  H -- "はい" --> I["把持力・手の姿勢を調整"]
  I --> J{"再把持が必要か"}
  J -- "はい" --> I
  J -- "いいえ" --> K["力覚で荷重・嵌合異常を監視"]
  H -- "いいえ" --> K
  K --> L["視覚と接触情報を組み合わせて結果判定"]
  L --> M{"作業結果"}
  M -- "完了" --> N["作業完了"]
  M -- "異常・再試行" --> O["停止・退避または再試行"]

視覚は接近と初期位置決め、力覚は接触検出と荷重監視、触覚は局所的な把持状態の確認に使い、観測結果に応じて制御や再試行を切り替える。

この流れでは、センサー値を単に追加するだけでは足りない。どの観測をどの状態遷移の条件に使うか、また力の異常を停止、退避、再試行のどれに結び付けるかを制御仕様として定義する必要がある。