把持計画 on Physical AI Hub

キャリブレーション誤差を計画へ伝える：ロボットの位置不確実性を扱う方法

Fri, 23 Oct 2026 13:32:17 +0900

校正値を高精度な固定値として扱うだけで、計画時の安全性まで十分に表せるのかが本稿の出発点です。

先に結論

キャリブレーション誤差は、単一の安全マージンやXYZ各軸の独立した誤差として扱うのではなく、校正パラメータ、関節状態、認識結果などの誤差源を分けた確率分布として保持し、最終的な手先・対象物・障害物の座標へ伝播させるべきです。姿勢にはSE(3)上の局所摂動と6次元共分散を使い、局所的な非線形性であればヤコビアンによる共分散伝播を適用します。非線形性や分布の歪みが強い場合は、シグマ点法やモンテカルロ法へ切り替えます。

経路計画では、衝突確率を制約する方法、または不確実性に応じてリンクや掃引体積を保守的に膨張させる方法を使います。把持計画では、エンドエフェクタと対象物の姿勢を同じ基準座標系へ変換し、相対姿勢の分布から接触成立、衝突回避、力閉包、把持成功のロバスト性を評価します。

なぜ校正誤差を計画へ伝えるのか

校正後の変換値を固定パラメータとして扱うと、計画器は「推定された平均姿勢が正しい」という前提で動作します。しかし実際には、校正データの測定誤差、関節角度の誤差、対象物認識の誤差などが残ります。校正パラメータの推定時に測定値の不確実性を重み付けし、パラメータ間の相関を含む推定共分散を保存できれば、その不確実性を後段の計画に入力できます。

ここで重要なのは、校正残差の共分散だけで実機の誤差全体を説明できるとは限らないことです。温度、荷重によるたわみ、バックラッシュ、摩擦、未モデル化の機構誤差などは、校正時と運用時で変化する可能性があります。したがって、検証用の姿勢、荷重、温度条件で残差を確認し、必要なら計画側の不確実性モデルへ反映します。

前提となる姿勢表現

位置と回転を別々のスカラー誤差として扱うのではなく、ロボットの姿勢をSE(3)上の状態として表します。基準姿勢の近傍にある小さな変動を6次元の局所摂動で表し、その平均と6×6共分散を保持する形が基本です。共分散には、位置と回転の相関や、回転軸間の相関も含められます。

ただし、共分散行列だけを保存しても意味は完全には決まりません。左摂動か右摂動か、摂動をbody frameで定義するかworld frameで定義するか、回転摂動をどのように定義するかによって、同じ6×6行列の解釈が変わります。実装では、共分散とともに摂動の定義、基準座標系、姿勢の変換規約を記録します。

共分散を座標変換へ伝播する仕組み

関節状態、校正パラメータ、対象物姿勢などを入力x、変換後の手先姿勢や相対姿勢を出力yとし、y=f(x)と表します。平均状態の近傍で線形化できるなら、ヤコビアンJを用いて次のように出力共分散を近似します。

Σ_y ≈ JΣ_xJᵀ

校正パラメータと関節状態など複数の誤差源を同時に扱う場合、それぞれの共分散を足すだけでは不十分です。誤差源の間に相関があるなら、交差共分散も含めて計算します。たとえば、校正パラメータの不確実性を順運動学へ入力し、手先姿勢の分布を得た後、対象物姿勢の分布と同じ座標系へ変換して相対姿勢を構成します。

一次のヤコビアン近似は、摂動が小さく、変換が局所的に線形とみなせる場合に適しています。大きな回転誤差、接触境界付近の判定、強い非線形性、多峰性のある分布では、平均と共分散だけでは形状を十分に表現できません。その場合はシグマ点法、モンテカルロ法、粒子表現を検討します。

flowchart LR
  A["校正データ"] --> B["校正パラメータ推定"]
  A --> C["測定誤差のモデル化"]
  B --> D["パラメータ平均と共分散"]
  E["関節状態"] --> F["誤差源の統合"]
  G["対象物認識結果"] --> F
  D --> F
  C --> F
  F --> H["摂動規約と基準座標系を統一"]
  H --> I["SE(3)姿勢変換"]
  I --> J["ヤコビアンによる共分散伝播"]
  J --> K["手先・対象物・障害物の姿勢分布"]
  K --> L{ "非線形性や分布の歪みが強いか" }
  L -- "いいえ" --> M["共分散を計画へ入力"]
  L -- "はい" --> N["シグマ点法またはモンテカルロ法"]
  N --> M
  M --> O["確率制約または保守的形状膨張"]
  M --> P["相対姿勢分布による把持評価"]

校正パラメータ、関節状態、認識結果の不確実性を姿勢分布へ伝播し、経路計画と把持計画へ入力する処理の全体像。

伝播手法の比較

手法	非線形性への対応	計算量の目安	相関の扱い	向いている場面	注意点
ヤコビアンによる共分散伝播	局所近似	小	共分散に含めて扱う	連続的な姿勢変換、計画の反復	大きな摂動や多峰性に弱い
シグマ点法	一次近似より広い非線形性に対応	中	入力分布から反映	姿勢変換や状態推定	分布の多峰性を完全には表せない
モンテカルロ法	強い非線形性に対応	大	サンプル間の関係として扱う	接触・衝突・把持成功の評価	サンプル数と計算時間が必要
保守的な形状膨張	分布形状を単純化	小	相関を別途考慮	衝突判定のベースライン	安全性と保守性の調整が必要

経路計画と衝突判定

経路計画では、各時刻のロボット状態と障害物との衝突確率に上限を設ける確率制約付き計画を構成できます。実装しやすいベースラインは、障害物境界へ向かう距離方向に位置分散を投影し、信頼区間や分位点を計算してリンク形状や掃引体積を膨張させる方法です。平均姿勢だけで衝突なしと判定するより、不確実性が大きい方向へ余裕を持たせられます。

不確実性を扱うロボット把持計画：成功確率に応じて実行と再観測を切り替える

Sun, 23 Aug 2026 01:51:41 +0900

把持の「自信あり」という数値が、実際の成功確率と同じ意味を持つのかが気になるテーマです。

はじめに

ロボットが物体を正しく認識できたように見えても、その把持が成功するとは限りません。姿勢推定の誤差、隠れた形状、摩擦や接触状態のばらつき、照明や視点の変化が、実行結果を左右します。ここで必要なのは、認識モデルのスコアを単純なしきい値で分類することではなく、「いま動く価値があるか」「追加観測で判断が改善するか」「安全のために中止すべきか」を分けて決める層です。

先に結論

把持計画では、生の把持スコアを成功確率として扱わず、対象・センサー・把持条件に合わせて校正した確率と、別々に評価した不確実性・安全信号を使います。実行、再観測、中止の切り替えは、成功確率だけでなく、成功時の価値、失敗や破損の損失、追加観測の時間・移動リスク、ロボットや周辺設備への安全リスクを含む期待効用、または明示的なリスク制約で設計します。

さらに、実行後も視覚などの観測で把持状態を検証し、必要なら再計画します。固定しきい値は実装の出発点にはなりますが、対象の価値やサイクルタイム、安全条件が異なる環境にそのまま移植できる普遍的なしきい値は確認されていません。

なぜ単一の信頼度では足りないのか

把持モデルの出力は、訓練データ、センサー、把持品質の定義、対象分布に依存するモデル出力です。たとえば解析的な把持品質指標から学習した予測値と、行動選択や方策価値を直接学習した出力では、数値が似ていても意味は同じではありません。高いスコアを「実際の成功確率」と解釈するには、別途検証が必要です。

また、不確実性には少なくとも二つの側面があります。観測ノイズや接触ばらつきのように、データ自体に固有の aleatoric uncertainty と、データ不足やモデルの知識不足に由来する epistemic uncertainty です。前者は同じ条件でも結果が揺れる要因、後者は未知の対象や条件に出会ったときに予測が不安定になる要因と考えられます。対応策が異なるため、合計値だけでなく内訳を保持する設計が有用です。

belief として状態を保持する

部分的にしか観測できない物体姿勢、形状、接触状態、摩擦などを、単一の推定値だけで表すと、計画は誤った前提に固定されます。そこで、状態についての信念、すなわち belief として候補や確率分布を保持し、行動と観測によって更新します。把持計画と観測計画を同じ枠組みで扱えば、把持を急ぐべきか、別の視点から見直すべきかを比較できます。

ただし、完全な belief-space 計画は計算量やモデル化の負担が大きくなります。実機では、候補生成、校正、実行ゲート、監視、検証、再計画、中止を状態機械として分離し、必要な範囲で近似する構成が扱いやすいでしょう。

確率校正と分布ずれの確認

意思決定に確率を使うなら、検証用データで校正します。予測確率をいくつかの信頼度ビンに分け、各ビンの平均予測値と実際の成功率を比較する reliability diagram を確認します。ECE や Brier score も、確率の整合性を評価する指標になります。分類精度が高くても、予測確率が実際の成功率と一致するとは限らないためです。

校正データは、学習時と同じ分布だけでなく、実行時に想定される対象、照明、視点、センサー、把持条件を含めて評価します。分布外やデータシフト下では不確実性の品質が変化し、アンサンブル、ベイズ近似、校正手法のいずれかが常に優れるとは限りません。校正済みの値であっても、安全リスクを十分に表すとは限らない点にも注意が必要です。

flowchart TD
  A["観測・状態推定"] --> B["把持候補を生成"]
  B --> C["成功確率を校正し、不確実性を分離"]
  C --> D["期待効用と安全制約を評価"]
  D --> E{"安全制約を満たすか"}
  E -- "いいえ" --> X["中止・退避"]
  E -- "はい" --> F{"追加観測で判断が改善する見込みがあるか"}
  F -- "はい" --> G["再観測"]
  G --> A
  F -- "いいえ" --> H{"実行の期待効用が許容範囲か"}
  H -- "いいえ" --> X
  H -- "はい" --> I["把持を実行"]
  I --> J["実行中の観測と状態監視"]
  J --> K["把持状態を検証"]
  K --> L{"検証に成功したか"}
  L -- "はい" --> M["完了"]
  L -- "いいえ" --> N{"別候補や別経路に価値があるか"}
  N -- "はい" --> O["再計画"]
  O --> A
  N -- "いいえ" --> X

把持候補の評価から再観測、実行、実行後の検証、再計画、中止・退避までを状態機械として示す。成功予測と安全制約は別々の判断信号として扱う。

実行・再観測・中止の判断

候補ごとに、校正済み成功確率、二種類の不確実性、把持余裕、接触や衝突に関する安全信号をまとめます。そのうえで、候補を実行したときの成功時の価値から、失敗・破損・落下・設備損傷などの損失を差し引きます。再観測については、観測によって候補順位や安全判定が変わる見込みと、観測時間、移動リスク、処理コストを比較します。これは「信頼度が低ければ必ず見る」という規則ではありません。