物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法およびシステム
【課題】物体に対するプローブの姿勢を、プローブを用いて物体をプロービングすることによって求めるためのシステム及び方法を提供する。
【解決手段】ラオ・ブラックウェル化パーティクルフィルタリングを使用して姿勢の確率を求めるステップであって、姿勢のロケーションの確率が前記パーティクルのロケーション２１２によって表され、姿勢の配向の確率が、各パーティクルのロケーションを条件とするパーティクルの配向にわたるガウス分布２１５によって表され、姿勢の確率が特定の姿勢の周りに集中するまで、後続のプロービングごとに実行されるステップと、特定の姿勢に基づいて物体に対するプローブの姿勢を推定するステップとを含む。
【解決手段】ラオ・ブラックウェル化パーティクルフィルタリングを使用して姿勢の確率を求めるステップであって、姿勢のロケーションの確率が前記パーティクルのロケーション２１２によって表され、姿勢の配向の確率が、各パーティクルのロケーションを条件とするパーティクルの配向にわたるガウス分布２１５によって表され、姿勢の確率が特定の姿勢の周りに集中するまで、後続のプロービングごとに実行されるステップと、特定の姿勢に基づいて物体に対するプローブの姿勢を推定するステップとを含む。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、包括的には産業用ロボット工学に関し、より詳細には、産業用ロボット工学における確率的位置同定のための、プロービングに基づく方法に関する。
【背景技術】
【０００２】
自動ロボット組立てを含む、多数の産業用ロボット工学用途は、ロボットのアームを、物体又は環境に対して所望の姿勢にナビゲートすることを必要とする。この問題は、多くの場合に、針状のプローブが孔を含み得る物体と単一の接点を有すると共に物体に対するプローブの初期姿勢が未知であるペグインホール問題として定式化される。物体に対するプローブの姿勢は、６自由度（６−ＤＯＦ）を有し、プローブの位置における３−ＤＯＦの不確定性、及び物体に対するプローブの配向の３−ＤＯＦの不確定性を含む。初期姿勢不確定性は、６−ＤＯＦ全てにおいて所望の正確度を大幅に超える場合がある。
【０００３】
いくつかの従来の方法は、プロービング及びパーティクルに基づくモンテカルロ位置同定を使用して、ロボットのペグインホール問題を解決する。たとえば、１つの方法は、ロックアンドキーアセンブリにプロービング及びパーティクルフィルタリングを使用する位置同定を説明している。この方法は、まず、全ての（ｘ，ｙ）においてプローブを用いて物体を高密度にプロービングして接触位置（ｘ，ｙ，ｚ）を取得し、接触配置空間マップを生成する。この接触配置空間マップは、プローブが物体と接触する全ての取り得る姿勢を記述する。この高密度プロービングの前処理ステップの後、この方法は、プローブを用いて物体を連続してプロービングすることによって、該連続的なプロービングから取得される接触位置を観測値として使用して、パーティクルフィルタリングを行う。観測尤度は、接触配置空間マップを使用して評価される。
【０００４】
別の方法は、同様に、網羅的な前処理ステップを説明しており、物体と複数の接点を有し得るプローブを用いて物体を高密度にプロービングし、プローブに接続された力トルクセンサーを使用して力トルクマップを生成する。力トルクマップは、プローブと物体との間に接触が存在する全ての取り得る姿勢における接触力及び接触トルクを含む。この方法は、コンピューター支援設計（ＣＡＤ）モデルから直接力トルクマップを推定することも説明している。しかしながら、推定される力トルクマップは、プロービングによって得られるマップほど正確ではない。高密度プロービングによって事前に得られた力トルクマップを使用して、パーティクルフィルタリングを連続プロービングと共に使用して力トルク観測値をそのマップにマッチングする。この方法は、カメラからの観測値もオプションで組み込む。
【０００５】
しかしながら、上述した方法は、６−ＤＯＦ全部の位置同定にはあまり適していない。なぜなら、標準的なパーティクルフィルタリングに必要とされるパーティクルの数は、探索空間の次元数と共にほぼ指数関数的に増大するためである。これらの方法は、最大６自由度（６−ＤＯＦ）の不確定性に関して説明されているが、実際にはこれらの方法は、不確定性がｘ、ｙ並進においてしか存在しない２−ＤＯＦ空間のような、より低い次元の探索空間における位置同定にしか使用されない。
【０００６】
したがって、プローブと物体との間の姿勢が最大６自由度（６−ＤＯＦ）を有する、該姿勢を効率的に求めることができる方法を有することが望ましい。また、高密度プロービングの前処理ステップなしで物体のＣＡＤモデルから直接求めることができると共に、マップ内の不確定性の変動するレベル、及びＣＡＤモデルと物体との間の不一致を考慮することができるマップ表現を有することが望ましい。
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明の目的は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法を提供することである。
【０００８】
本発明のさらなる目的は、たとえば製造及び測定の誤差から生じる、物体のモデルと物体の測定値との間の差異を扱うことができる方法を提供することである。
【課題を解決するための手段】
【０００９】
本発明の実施の形態は、プローブの６−ＤＯＦ姿勢の推定に適用されるパーティクルフィルタリングが、計算的に困難であるが、６−ＤＯＦ姿勢を２つの部分、すなわち３−ＤＯＦロケーション及び３−ＤＯＦ配向に因数分解することができ、それによってプローブの推定される配向が該プローブの推定されるロケーションによって決まると共に、該配向をそのロケーションに基づいてカルマンフィルタリングを使用して更新することができるるという認識に基づく。この認識に基づいて、６−ＤＯＦパーティクルフィルタリングは、各パーティクルのロケーションを条件とする３−ＤＯＦパーティクルフィルタリング及び３−ＤＯＦカルマンフィルタリングに低減される。この結果、より効率的な計算となる。そのような因数分解の理論的枠組みは、ラオ・ブラックウェル化パーティクルフィルタリングとして既知である。
【００１０】
本発明の１つの実施の形態は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングの結果、前記プローブと前記物体との間の接点がもたらされ、それによって各該接点において前記姿勢が推定され、該方法は、前記プローブと前記物体との間の前記姿勢の確率をパーティクルのセットによって表すステップであって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表すステップと、前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置するステップであって、該動きは前記プロービングによって生じる、再配置するステップと、各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求めるステップであって、該重要度重みは、前記パーティクルが前記接点を表す確率である、求めるステップと、各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新するステップと、前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返すステップと、前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求めるステップとを含み、該方法の前記ステップはプロセッサによって実行される、方法を開示する。
【００１１】
また、この実施の形態の１つの変形形態は、物体モデル及び不確定性モデルに基づいてマップモデルを求め、不確定性モデルは、前記物体モデルの各特徴に不確定性関数を割り当てることによって求められ、該特徴は面、エッジ、又は頂点である。
【００１２】
別の実施の形態は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、ラオ・ブラックウェル化パーティクルフィルタリングを使用して前記姿勢の確率を求めるステップであって、該姿勢のロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの前記配向にわたるガウス分布によって表され、該求めることは、前記姿勢の前記確率が特定の姿勢の周りに集中するまで、後続のプロービングごとに実行される、求めるステップと、前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を推定するステップと、を含む、方法を開示する。
【００１３】
さらに別の実施の形態は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるように構成されるシステムであって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングの結果、前記プローブと前記物体との間の接点がもたらされ、それによって各該接点において前記姿勢が推定され、該システムは、前記物体に対する前記姿勢の確率をパーティクルのセットによって表す手段であって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表す手段と、前記プロビーングの接点から次のプロービングの接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置する手段であって、該動きは前記プロービングによって生じる、再配置する手段と、各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求める手段であって、該重要度重みは、前記パーティクルが前記物体に対する前記プローブの前記姿勢を表す確率である、求める手段と、各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新する手段と、前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返す手段と、前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求める手段とを含み、前記方法の前記ステップはプロセッサによって実行される、システムを開示する。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態による、プローブと物体との間の姿勢を求めるためのシステム及び方法の概略図である。
【図２】本発明の実施形態による、プローブと物体との間の姿勢を求めるための方法のブロック図である。
【図３】本発明の実施形態による、確率的依存関係の概略図である。
【図４】本発明の実施形態による、物体座標系とベース座標系との間の座標変換の概略図である。
【図５】本発明の実施形態によるマップモデルの概略図である。
【図６】本発明の実施形態による、ラオ・ブラックウェル化パーティクルフィルタリングを用いた、プローブと物体との間の姿勢の確率を求めるための擬似コードである。
【発明を実施するための形態】
【００１５】
図１は、物体１２０に対するプローブ１１０の姿勢１５０を、該プローブを用いて該物体をプロービングすることによってを求めるためのシステム１００及び方法２００を示している。本発明の様々な実施形態において、プローブは、タッチプローブ又はタッチレスプローブであり、たとえば電気プローブ、機械プローブ、光プローブ、又はレーザープローブである。各プロービングによってプローブと物体との間の接点１１５が求められ、各接点において姿勢が推定される。たとえば、１つの実施形態では、プローブは針状の形状を有する機械プローブであり、各接点は、プローブが物体に触れる点である。代替的な実施形態では、プローブはタッチレスプローブであり、各接点は、該タッチレスプローブによって求められる物体の表面上のロケーションである。
【００１６】
物体に対するプローブの姿勢が求められると、産業プロセスにおいてその姿勢を使用することができる。産業プロセスには、物体を持ち上げること、物体を位置決めすること、部品を物体と嵌合すること、物体に対するロボットの姿勢を求めること、ロボット座標系を較正すること、又は物体に対し、溶射ガン、アーク溶接機、ドリル、若しくは研磨機のような道具を使用することが含まれ得る。
【００１７】
物体は、ＣＡＤモデルのような、物体の３次元（３Ｄ）モデル１２５に基づいて求められるマップモデル５００、及び以下でより詳細に説明される不確定性モデル５１０によって表される。本システムは、本発明の実施形態に従って方法２００のステップを実行するプロセッサ１４０も備える。プロセッサは、当該技術分野において既知の入力／出力インターフェース及びメモリを備える。
【００１８】
プローブは、物体の近傍に位置決めされるが、物体に対する該プローブの姿勢は未知であり、姿勢のロケーション、及び姿勢の配向を含む、最大６自由度（６−ＤＯＦ）の不確定性を有する。通常、姿勢のロケーション及び配向はそれぞれ３次元であるが、いくつかの実施形態では、姿勢のロケーション及び／又は配向は、３−ＤＯＦ未満の不確定性を有する。
【００１９】
１つの実施形態では、プロービングは、プローブを保持するロボットアーム１６０を再位置決めし、該プローブを、該プローブと物体との間の接触を検出するまで物体の一般方向に動かし、ロボット座標系における接点のロケーション１３０を測定することによって達成される。様々な実施形態において、ロボットのアームの内蔵機能及びエンコーダーによって、ロボットアームのコンプライアンス制御によって、かつ／又は、カメラ、タッチセンサー、若しくは力センサーのような外部センサーによって、接触のロケーションが検出及び測定される。
【００２０】
本発明の実施形態は、プローブの６−ＤＯＦ姿勢の推定に適用されるパーティクルフィルタリングは計算的に困難であるが、該６−ＤＯＦ姿勢を２つの部分、すなわち３−ＤＯＦロケーション及び３−ＤＯＦ配向に因数分解することができ、それによってプローブの配向が該プローブのロケーションに依存すると共に、該配向をそのロケーションに基づいてカルマンフィルタリングを使用して更新することができるという認識に基づく。この認識に基づいて、６−ＤＯＦパーティクルフィルタリングは３−ＤＯＦパーティクルフィルタリング及び３−ＤＯＦカルマンフィルタリングに変形される。この結果、より効率的な計算となる。そのような因数分解の理論的枠組みは、ラオ・ブラックウェル化パーティクルフィルタリングとして既知である。
【００２１】
図２は、本発明の実施形態による方法２００のブロック図を示している。上述した認識によれば、プローブと物体との間の相対姿勢は、ラオ・ブラックウェル化パーティクルフィルターを使用して求められる。ラオ・ブラックウェル化パーティクルフィルターにおいて、ロケーション（３−ＤＯＦ並進）にわたる確率分布はパーティクルを用いて表され、すなわち各パーティクルはロケーションに関する特定の仮説を表し、配向（３−ＤＯＦ回転）にわたる分布は、パーティクルのロケーションを条件とする、パーティクルごとのガウス分布によって近似される。この因数分解は、フィルタリングの効率を大幅に改善し、結果として、従来のパーティクルフィルタリングと比較して、必要とされるパーティクルの数が数桁削減される。
【００２２】
したがって、物体に対するプローブの姿勢の確率はパーティクルのセットによって表され（２１０）、姿勢のロケーションの確率は、各パーティクルのロケーション２１２によって表され、姿勢の配向の確率は、パーティクルのロケーションを条件とする、パーティクルごとの配向にわたるガウス分布２１５によって表される。
【００２３】
後続のプロービング２２０ごとに、プローブの動き２２５が得られ、該動きに従って各パーティクルのロケーションが次のロケーション２３５に更新され（２３０）、該パーティクルの次のロケーション、各パーティクルの配向にわたるガウス分布、及び物体のマップモデル５００に基づいて各パーティクルの重要度重み２５５が更新される（２５０）。ここで、重要度重みは、パーティクルのロケーションが接点１１５を表し、パーティクルの配向分布が物体に対するプローブの配向を表す確率である。また、パーティクルの次のロケーションを条件とするカルマンフィルタリング２４５、たとえば拡張カルマンフィルタリング（ＥＫＦ）を使用して、各パーティクルの配向にわたるガウス分布２１５が更新される（２４０）。ＥＫＦは、パーティクルの次のロケーションを条件とする配向にわたる確率分布の次の推定値２５８を生成する。
【００２４】
パーティクルのセット２１０内のパーティクルが特定の姿勢の周りに集中している（２７０）とき、物体に対するプローブの姿勢１５０は、その特定の姿勢に基づいて、たとえばパーティクルのロケーション及び配向の平均をとることによって求められる。そうでない場合、方法２００の再位置決めステップ、プロービングステップ、及び更新ステップが繰り返される。
【００２５】
１つの実施形態では、パーティクルの更新されたセット及び重要度重みを後続のプロービングに使用するか、又は各パーティクルの重要度重みに比例する確率を用いてパーティクルのセットを再サンプリングする（２６０）かの決定２６５が行われる。パーティクルのセットが再サンプリングされる場合、高い確率で、低い重要度重みを有するパーティクルがセット２１０から除去され、高い重要度重みを有するパーティクルがセットにおいて複数回表される。
【００２６】
１つの実施形態では、物体のマップモデルは、プロービングされている物体のモデル、たとえばコンピューター支援設計（ＣＡＤ）モデル、及び該ＣＡＤモデルの不確定性に関する任意の事前の知識を表す不確定性モデルに基づいて生成される。不確定性モデルは、物体の異なる特徴に関する不確定性を組み込む。したがって、マップモデルは確率マップモデルである。
【００２７】
本発明の実施形態によるマップモデルの使用は、実際の測定がＣＡＤモデルに一致しない状況において有利である。そのような差異は、スリッピング及びセンサーの不正確性のような様々な要因に起因する測定誤差、並びに物体のＣＡＤモデルと該物体の実際の形状との間の差異から生じ得る。
【００２８】
確率的依存関係
物体に対するプローブの姿勢は、（ｓ，θ）として表される。ここで、ｓ＝（ｘ，ｙ，ｚ）Ｔはプローブと物体との間の相対位置であり、θ＝（α，β，γ）Ｔはプローブと物体との間の相対配向である。角度（α，β，γ）は、（ｘ，ｙ，ｚ）軸のそれぞれを中心とした回転の角度として規定される。
【００２９】
図３は、方法２００において使用される確率的依存関係のグラフィックモデルを示している。各接点から次の接点までのプローブの動きのシーケンス３１０（該動きは時刻１からｔまで、各プロービングによって生じる）は、ｕ１：ｔ＝｛ｕ１，．．．，ｕｔ｝として規定され、ここで、ｕｉは、時刻ｉ−１における接点から時刻ｉにおける接点までの動きを表す。接触イベントのシーケンス３３０はｚ０：ｔ＝｛ｚ１，．．．，ｚｔ｝として規定される。本実施形態は、物体に対するプローブの姿勢の確率を、
【００３０】
【数１】
【００３１】
に従って因数分解する。
【００３２】
したがって、各パーティクルは、接点の位置のシーケンスＳ０：ｔ３２０及びプローブと物体との間の配向にわたるガウス分布３４０を表す。ガウス分布は平均μｔ及び共分散行列Σｔによって表される。
【００３３】
通常、接触イベントはバイナリ、すなわちＺｔ∈｛１，０｝であり、ここで、時点ｔにおいてプローブが位置ｓｔにおいて物体と接触している場合、Ｚｔ＝１であり、そうでない場合、Ｚｔ＝０である。しかしながら、接点は、プロービングごとに、該接点が感知されるとき、すなわちＺｔ＝１である時点でしか測定されない。したがって、明確にするために、ｐ（ｚｔ＝１）を省略表現ｐ（ｚｔ）を使用して表すことによって、確率の表記を簡略化する。
【００３４】
座標変換
図４は、物体に対して固定した物体座標系４１０と、ロボット座標系に対して固定したベース座標系４２０、すなわちロボットのアーム（図示せず）の基準系との間の座標変換を示している。いくつかの実施形態では、ロボット座標系とベース座標系との間の変換は、回転を伴わない並進であり、それによって、プローブの動きは、ベース座標系において、ロボット座標系における並進と同じ並進によって表される。
【００３５】
ベース座標系における、時刻ｔにおけるプローブの位置は、Ｓｔとして規定される。物体座標系における、時刻ｔにおけるプローブの位置は、Ｓｔ０として規定される。ベース座標系及び物体座標系は、プローブの初期位置Ｓ０４４０を中心とした３Ｄ回転４３０θによって関係付けられる。このため、プローブの初期位置は、ベース座標系及び物体座標系の双方において同じである。しかしながら、２つの座標系間の配向差異θは未知である。
【００３６】
マップモデル４６０及び測定確率は、物体座標系において規定される。時刻ｔ＝０において、ベース座標系及び物体座標系は同一である。時刻ｔ＝１において、プローブの動き４４５の後、ベース座標系に対する物体座標系の回転に対応して、各パーティクルの配向にわたるガウス分布が更新される。
【００３７】
動き
後続の各プロービングは、プローブを１つの接点から別の接点まで動かす。各接点について、ロボットアームの内部エンコーダーから、ロボット座標系におけるプローブの位置を取得することができる。時刻ｔ−１から時刻ｔまでのプローブの動きは、ロボット座標系及びベース座標系において同一である。動きｕｔを使用して、ベース座標系における各パーティクルの位置が、
【００３８】
【数２】
【００３９】
に従って位置Ｓｔ−１から次の位置ｓｔに更新される。ここで、Ｎ（μ，Σ）は、平均μ及び共分散行列Σを有する多変量正規（ガウス）分布を表し、σｘ、σｙ、σｚは、ロボットの動きの誤差又はロボットのアームのエンコーダーから取得された値における誤差に起因する雑音をモデル化している。
【００４０】
いくつかの実施形態は、実際のロボット動き誤差及びエンコーダー誤差よりも大きいσｘ、σｙ、σｚの値を使用して、パーティクル欠乏（deprivation）を考慮する。ここで、パーティクルのうちのいずれも初期接触位置の完全に正確な推定を有しないため、動きモデルにおける雑音によって、パーティクルフィルターがこの不正確性を経時的に補償することが可能になる。
【００４１】
物体座標系における時刻ｔにおけるプローブの位置Ｓｔ０は、配向θ＝（α，β，γ）Ｔを使用する回転行列Ｒに基づいて求められる。回転行列Ｒは、
【００４２】
【数３】
【００４３】
に従って求められる。ここで、Ｓαはｓｉｎαの省略表現であり、Ｃαはｃｏｓαの省略表現である。
【００４４】
したがって、物体座標における時刻ｔにおけるプローブの位置は、
【００４５】
【数４】
【００４６】
に従って求められる。
【００４７】
マップモデル
本発明のいくつかの実施形態は、物体の測定モデルが、物体モデル設計及び測定の不確定性、たとえば物体モデルと物体の実際の形状との間の差異、並びに測定ツール及び測定手順の不完全性を反映するべきであるという認識に基づく。たとえば、プローブの先端は、とがった針状である代わりに、丸みを有するか又は幅広である場合があり、それによって測定誤差がもたらされる。したがって、本発明者らは、面（平面）、エッジ（線）、及び頂点（点）のような、モデルの全ての特徴について求められる不確定性のパラメーターを求めると共に、該パラメーターをマップモデル内に含める。
【００４８】
図５は、本発明の実施形態による、マップモデル５００の１つの例を示している。物体のマップは、物体モデル１２５及び不確定性モデル５１０から求められる。いくつかの実施形態では、物体モデルは物体のＣＡＤモデルから生成される。物体モデルは、物体の形状の特徴ｆｉ５２０、たとえば、頂点、エッジ、及び三角形の面を含む。たとえば、図５に示すように、特徴ｆ１、ｆ３、ｆ５は頂点又はエッジを表し、特徴ｆ２及びｆ４はエッジ又は面を表す。
【００４９】
異なる実施形態では、不確定性モデルは、広範にわたる確率フィールド又はエネルギー関数を使用して定式化される。たとえば、１つの実施形態では、不確定性モデルは、接点１１５と物体モデルの特徴５２０との間の距離５４５のガウス確率密度関数を使用して定式化される。マップモデルは、物体モデルの異なる特徴に、異なる不確定性値を割り当てることができる。これによって、ユーザーが特定の産業的設定又は製造プロセスの知識をマップモデルに組み込むことが可能になる。
【００５０】
測定不確定性は、不確定性モデル５１０を形成する。特徴ｆｋごとに（ここでｋ＝｛１，．．．，Ｋ｝）、特徴の測定不確定性の標準偏差がσｋ５４０として規定される。１つの実施形態では、平面の交差部におけるエッジ及び頂点のような、その法線が互いに大きな角度を形成する特徴には、より大きな不確定性が割り当てられる。たとえば、この実施形態の１つの変形形態では、各エッジ／頂点の境界となる面の全ての可能な対の法線間の最大角度が求められ、最大角度の値に基づいて不確定性が割り当てられる。
【００５１】
測定確率は、物体モデルと、物体座標における接点Ｓ０５３０との間の距離５４５に基づいて求められる。接点から各特徴ｆｋへの距離がｄ（Ｓ０，ｆｋ）として規定される。この実施形態の１つの変形形態において、関数ｄは、接点Ｓ０と特徴ｆｋとの間のユークリッド距離を計算する。対応する平面又は線に対する接点の垂直射影が面又はエッジ内にある場合、該面又は該エッジへの距離が求められる。
【００５２】
接点ごとに、全ての特徴に対する距離が求められ、対応する標準偏差で正規化された最小の距離を有する特徴が接触特徴として選択される。このように、接触特徴のインデックスｋｃが
【００５３】
【数５】
【００５４】
に従って求められる。
【００５５】
１つの実施形態では、距離測定値ｈは、物体座標においてＳ０の関数として表されるか、又はベース座標において位置ｓ及び配向θの関数として表され、以下となる。
【００５６】
【数６】
【００５７】
パーティクルごとの時刻ｔにおける測定確率が、距離測定値に基づいて
【００５８】
【数７】
【００５９】
に従って求められる。
【００６０】
表記Ｎ（ｘ；μ，Σ）は、変数ｘにわたる、平均μ及び共分散行列Σの多変量正規（ガウス）分布を表す。
【００６１】
ラオ・ブラックウェル化パーティクルフィルタリング
図６は、ラオ・ブラックウェル化パーティクルフィルタリングを使用して、物体に対するプローブの姿勢の確率を求めるための擬似コードを示している。ここで、姿勢のロケーションの確率は、各パーティクルのロケーションによって表され、姿勢の配向の確率は、パーティクルのロケーションを条件とする、各パーティクルの配向にわたるガウス分布によって表される。
【００６２】
プロービングごとに、時刻ｔにおいて、各パーティクルｊがプローブと物体との間の接点の位置Ｓｔ［ｊ］、及び配向にわたる正規分布
【００６３】
【数８】
【００６４】
に関連付けられる。
【００６５】
初期化
方法２００は、最大初期（ｘ，ｙ）不確定性によって境されるベース座標における位置（ｘ，ｙ）にわたる一様分布からＪ個のパーティクルをサンプリングする。１つの実施形態では、初期パーティクル位置Ｓ０［ｊ］＝（ｘ，ｙ，ｚ）がマップモデルの表面上に設定されるように、パーティクルｊごとのｚ座標の初期値が求められる。パーティクルごとに、配向θにわたるガウス分布が平均μ０［ｊ］＝０及び初期不確定性を表す対角共分散行列Σ０［ｊ］で初期化される。
【００６６】
プロービングごとのパーティクル更新
時刻ｔにおけるパーティクルのセットはＸｔとして規定される。ロボットアームがプローブを１つの接点から次の接点に動かすとき、各パーティクルの位置ｊ＝｛１，．．．，Ｊ｝は、ロボットアームエンコーダーから取得されるベース座標における動きｕｔ、及びロボットが接触を検出するという観測ｚｔに基づいて更新される。各パーティクルの次の位置に基づいて、パーティクルごとの配向θにわたる分布が更新される。
【００６７】
位置更新
時刻ｔにおけるパーティクルｊの次の位置Ｓｔ［ｊ］は、式（２）を使用して、パーティクルの位置Ｓｔ−１［ｊ］及び動きｕｔに基づいて
【００６８】
【数９】
【００６９】
に従って求められる。
【００７０】
配向更新
ベース座標におけるパーティクルの次の位置Ｓｔ［ｊ］、及びプローブと物体との間に接触が存在したという観測ｚｔに基づいて、配向θにわたる、パーティクルごとの事後分布が、ＥＫＦを使用して求められる。１つの実施形態は、測定確率を、配向の関数として表し、その測定確率を、θ＝μｔ−１［ｊ］、すなわち前回のプロービング後に求められたパーティクルの配向の分布の平均を中心に線形化する。
【００７１】
測定確率を、θ＝μｔ−１［ｊ］を中心に線形化するために、平均値及び式（４）に基づいて、
【００７２】
【数１０】
【００７３】
に従って、物体座標系における各パーティクルの予測位置
【００７４】
【数１１】
【００７５】
が求められる。
【００７６】
予測パーティクル位置
【００７７】
【数１２】
【００７８】
を使用して、式（５）に従って接触特徴のインデックスｋｃが求められる。
【００７９】
接触特徴
【００８０】
【数１３】
【００８１】
に対して規定された測定確率（７）を使用して、角度にわたって事後確率を更新する。これは（１）の右辺の第２の因数である。
【００８２】
したがって、事後確率は以下のようにさらに因数分解される。
【００８３】
【数１４】
【００８４】
式（１０）を使用して、配向の事後確率をガウス分布として近似する。１つの実施形態は、
【００８５】
【数１５】
【００８６】
に従って、θ＝μｔ−１［ｊ］を中心に測定関数を線形化することによってＥＫＦを使用する。ここで、
【００８７】
【数１６】
【００８８】
は予測位置
【００８９】
【数１７】
【００９０】
において求められる距離測定値であり、Ｈｔ［ｊ］は、
【００９１】
【数１８】
【００９２】
に従って求められる、配向θに対する距離測定のヤコビ行列式である。
【００９３】
次に、標準ＥＫＦ測定更新規則を使用して、
【００９４】
【数１９】
【００９５】
に従って、配向にわたる事後分布が計算される。
【００９６】
重要度重み更新及び再サンプリング
各時刻ｔにおいて、各パーティクルｊの重要度重みｗｔ［ｊ］が、該パーティクルの前回の重要度重みｗｔ−１［ｊ］及び測定確率から求められる。本発明の１つの実施形態では、重要度重みは、
【００９７】
【数２０】
【００９８】
に従って、パーティクルの配向にわたって測定確率を周辺化することによって更新される。
【００９９】
したがって、
【０１００】
【数２１】
【０１０１】
であり、ここで、
【０１０２】
【数２２】
【０１０３】
である。
【０１０４】
パーティクルの多様性を妥当なレベルに保つために、１つの実施形態は、プロービングごとに、
【０１０５】
【数２３】
【０１０６】
に従って、パーティクルの有効数Ｊｅｆｆを推定する。
【０１０７】
Ｊｅｆｆがパーティクルのセット内のパーティクルの数に基づいて求められる閾値未満である、たとえばＪｅｆｆ＜Ｊ／２である場合、パーティクルは、パーティクルの重みｗｔ［ｊ］に比例する確率を用いて再サンプリングされる（２６０）。再サンプリング後、全てのパティクル重みはｗｔ［ｊ］＝１／Ｊに再設定される。
【０１０８】
時刻ｔにおいてパーティクルが再サンプリングされない場合、たとえばＪｅｆｆ≧Ｊ／２である場合、式（１７）に従って求められた重要度重みを有する全てのパーティクルがパーティクルのセット２１０内に保持される。
【０１０９】
１つの実施形態では、初期状態において、比較的多くの数のパーティクルセットがセット２１０内で使用される。しかしながら、方法２００を数回反復した後、位置同定に必要とされるパーティクルの数が減少する。パーティクルの数は、所定の数Ｊｍｉｎに達するまで各再サンプルにおいて低減される。この実施形態の１つの変形形態は、再サンプリングの前にパーティクルの数ＪがＪｍｉｎより大きい場合、パーティクルの現在の数の半分のみを再サンプリングすることによって、セット内のパーティクルの数を半分に低減する。
【０１１０】
集中
本発明の実施形態は、パーティクルのセット内のパーティクルが、特定の姿勢の周りに集中するまで、該パーティクルのセットのプロービング及び再サンプリングを反復する。１つの実施形態では、物体座標系におけるパーティクルの位置の重み付けされた平均値及び重み付けされた共分散に基づいて、
【０１１１】
【数２４】
【０１１２】
及び
【０１１３】
【数２５】
【０１１４】
に従って集中が求められる。ここで、Ｓｔ０［ｊ］は式（４）を使用して
【０１１５】
【数２６】
【０１１６】
に従って求められる。
【０１１７】
プロービングは、共分散行列Ψｔ０のトレースが所定の閾値未満になり、パーティクル位置間の不確定性が小さいことを示すまで繰り返される。この収束状態が達成されると、姿勢の推定位置が、セット内に残っている全てのパーティクルの位置の重み付けされた平均に基づいて求められる。姿勢の推定配向は、パーティクルの平均配向の重み付けされた平均に基づいて求められる。
【０１１８】
求められた姿勢を、後続の産業プロセスに直接使用することができる。求められた姿勢を、反復型最近接点のような、全ての接点を同時に使用することによってコスト関数を最小にする姿勢推定アルゴリズムの初期推定として使用することによって、姿勢をさらに精緻化することができる。
【０１１９】
本発明を好ましい実施形態の例として説明してきたが、本発明の精神及び範囲内で様々な他の適応及び変更を行うことができることは理解されたい。したがって、添付の特許請求の範囲の目的は、本発明の真の精神及び範囲内に入るすべての変形及び変更を包含することである。
【技術分野】
【０００１】
本発明は、包括的には産業用ロボット工学に関し、より詳細には、産業用ロボット工学における確率的位置同定のための、プロービングに基づく方法に関する。
【背景技術】
【０００２】
自動ロボット組立てを含む、多数の産業用ロボット工学用途は、ロボットのアームを、物体又は環境に対して所望の姿勢にナビゲートすることを必要とする。この問題は、多くの場合に、針状のプローブが孔を含み得る物体と単一の接点を有すると共に物体に対するプローブの初期姿勢が未知であるペグインホール問題として定式化される。物体に対するプローブの姿勢は、６自由度（６−ＤＯＦ）を有し、プローブの位置における３−ＤＯＦの不確定性、及び物体に対するプローブの配向の３−ＤＯＦの不確定性を含む。初期姿勢不確定性は、６−ＤＯＦ全てにおいて所望の正確度を大幅に超える場合がある。
【０００３】
いくつかの従来の方法は、プロービング及びパーティクルに基づくモンテカルロ位置同定を使用して、ロボットのペグインホール問題を解決する。たとえば、１つの方法は、ロックアンドキーアセンブリにプロービング及びパーティクルフィルタリングを使用する位置同定を説明している。この方法は、まず、全ての（ｘ，ｙ）においてプローブを用いて物体を高密度にプロービングして接触位置（ｘ，ｙ，ｚ）を取得し、接触配置空間マップを生成する。この接触配置空間マップは、プローブが物体と接触する全ての取り得る姿勢を記述する。この高密度プロービングの前処理ステップの後、この方法は、プローブを用いて物体を連続してプロービングすることによって、該連続的なプロービングから取得される接触位置を観測値として使用して、パーティクルフィルタリングを行う。観測尤度は、接触配置空間マップを使用して評価される。
【０００４】
別の方法は、同様に、網羅的な前処理ステップを説明しており、物体と複数の接点を有し得るプローブを用いて物体を高密度にプロービングし、プローブに接続された力トルクセンサーを使用して力トルクマップを生成する。力トルクマップは、プローブと物体との間に接触が存在する全ての取り得る姿勢における接触力及び接触トルクを含む。この方法は、コンピューター支援設計（ＣＡＤ）モデルから直接力トルクマップを推定することも説明している。しかしながら、推定される力トルクマップは、プロービングによって得られるマップほど正確ではない。高密度プロービングによって事前に得られた力トルクマップを使用して、パーティクルフィルタリングを連続プロービングと共に使用して力トルク観測値をそのマップにマッチングする。この方法は、カメラからの観測値もオプションで組み込む。
【０００５】
しかしながら、上述した方法は、６−ＤＯＦ全部の位置同定にはあまり適していない。なぜなら、標準的なパーティクルフィルタリングに必要とされるパーティクルの数は、探索空間の次元数と共にほぼ指数関数的に増大するためである。これらの方法は、最大６自由度（６−ＤＯＦ）の不確定性に関して説明されているが、実際にはこれらの方法は、不確定性がｘ、ｙ並進においてしか存在しない２−ＤＯＦ空間のような、より低い次元の探索空間における位置同定にしか使用されない。
【０００６】
したがって、プローブと物体との間の姿勢が最大６自由度（６−ＤＯＦ）を有する、該姿勢を効率的に求めることができる方法を有することが望ましい。また、高密度プロービングの前処理ステップなしで物体のＣＡＤモデルから直接求めることができると共に、マップ内の不確定性の変動するレベル、及びＣＡＤモデルと物体との間の不一致を考慮することができるマップ表現を有することが望ましい。
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明の目的は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法を提供することである。
【０００８】
本発明のさらなる目的は、たとえば製造及び測定の誤差から生じる、物体のモデルと物体の測定値との間の差異を扱うことができる方法を提供することである。
【課題を解決するための手段】
【０００９】
本発明の実施の形態は、プローブの６−ＤＯＦ姿勢の推定に適用されるパーティクルフィルタリングが、計算的に困難であるが、６−ＤＯＦ姿勢を２つの部分、すなわち３−ＤＯＦロケーション及び３−ＤＯＦ配向に因数分解することができ、それによってプローブの推定される配向が該プローブの推定されるロケーションによって決まると共に、該配向をそのロケーションに基づいてカルマンフィルタリングを使用して更新することができるるという認識に基づく。この認識に基づいて、６−ＤＯＦパーティクルフィルタリングは、各パーティクルのロケーションを条件とする３−ＤＯＦパーティクルフィルタリング及び３−ＤＯＦカルマンフィルタリングに低減される。この結果、より効率的な計算となる。そのような因数分解の理論的枠組みは、ラオ・ブラックウェル化パーティクルフィルタリングとして既知である。
【００１０】
本発明の１つの実施の形態は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングの結果、前記プローブと前記物体との間の接点がもたらされ、それによって各該接点において前記姿勢が推定され、該方法は、前記プローブと前記物体との間の前記姿勢の確率をパーティクルのセットによって表すステップであって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表すステップと、前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置するステップであって、該動きは前記プロービングによって生じる、再配置するステップと、各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求めるステップであって、該重要度重みは、前記パーティクルが前記接点を表す確率である、求めるステップと、各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新するステップと、前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返すステップと、前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求めるステップとを含み、該方法の前記ステップはプロセッサによって実行される、方法を開示する。
【００１１】
また、この実施の形態の１つの変形形態は、物体モデル及び不確定性モデルに基づいてマップモデルを求め、不確定性モデルは、前記物体モデルの各特徴に不確定性関数を割り当てることによって求められ、該特徴は面、エッジ、又は頂点である。
【００１２】
別の実施の形態は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、ラオ・ブラックウェル化パーティクルフィルタリングを使用して前記姿勢の確率を求めるステップであって、該姿勢のロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの前記配向にわたるガウス分布によって表され、該求めることは、前記姿勢の前記確率が特定の姿勢の周りに集中するまで、後続のプロービングごとに実行される、求めるステップと、前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を推定するステップと、を含む、方法を開示する。
【００１３】
さらに別の実施の形態は、物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるように構成されるシステムであって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングの結果、前記プローブと前記物体との間の接点がもたらされ、それによって各該接点において前記姿勢が推定され、該システムは、前記物体に対する前記姿勢の確率をパーティクルのセットによって表す手段であって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表す手段と、前記プロビーングの接点から次のプロービングの接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置する手段であって、該動きは前記プロービングによって生じる、再配置する手段と、各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求める手段であって、該重要度重みは、前記パーティクルが前記物体に対する前記プローブの前記姿勢を表す確率である、求める手段と、各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新する手段と、前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返す手段と、前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求める手段とを含み、前記方法の前記ステップはプロセッサによって実行される、システムを開示する。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態による、プローブと物体との間の姿勢を求めるためのシステム及び方法の概略図である。
【図２】本発明の実施形態による、プローブと物体との間の姿勢を求めるための方法のブロック図である。
【図３】本発明の実施形態による、確率的依存関係の概略図である。
【図４】本発明の実施形態による、物体座標系とベース座標系との間の座標変換の概略図である。
【図５】本発明の実施形態によるマップモデルの概略図である。
【図６】本発明の実施形態による、ラオ・ブラックウェル化パーティクルフィルタリングを用いた、プローブと物体との間の姿勢の確率を求めるための擬似コードである。
【発明を実施するための形態】
【００１５】
図１は、物体１２０に対するプローブ１１０の姿勢１５０を、該プローブを用いて該物体をプロービングすることによってを求めるためのシステム１００及び方法２００を示している。本発明の様々な実施形態において、プローブは、タッチプローブ又はタッチレスプローブであり、たとえば電気プローブ、機械プローブ、光プローブ、又はレーザープローブである。各プロービングによってプローブと物体との間の接点１１５が求められ、各接点において姿勢が推定される。たとえば、１つの実施形態では、プローブは針状の形状を有する機械プローブであり、各接点は、プローブが物体に触れる点である。代替的な実施形態では、プローブはタッチレスプローブであり、各接点は、該タッチレスプローブによって求められる物体の表面上のロケーションである。
【００１６】
物体に対するプローブの姿勢が求められると、産業プロセスにおいてその姿勢を使用することができる。産業プロセスには、物体を持ち上げること、物体を位置決めすること、部品を物体と嵌合すること、物体に対するロボットの姿勢を求めること、ロボット座標系を較正すること、又は物体に対し、溶射ガン、アーク溶接機、ドリル、若しくは研磨機のような道具を使用することが含まれ得る。
【００１７】
物体は、ＣＡＤモデルのような、物体の３次元（３Ｄ）モデル１２５に基づいて求められるマップモデル５００、及び以下でより詳細に説明される不確定性モデル５１０によって表される。本システムは、本発明の実施形態に従って方法２００のステップを実行するプロセッサ１４０も備える。プロセッサは、当該技術分野において既知の入力／出力インターフェース及びメモリを備える。
【００１８】
プローブは、物体の近傍に位置決めされるが、物体に対する該プローブの姿勢は未知であり、姿勢のロケーション、及び姿勢の配向を含む、最大６自由度（６−ＤＯＦ）の不確定性を有する。通常、姿勢のロケーション及び配向はそれぞれ３次元であるが、いくつかの実施形態では、姿勢のロケーション及び／又は配向は、３−ＤＯＦ未満の不確定性を有する。
【００１９】
１つの実施形態では、プロービングは、プローブを保持するロボットアーム１６０を再位置決めし、該プローブを、該プローブと物体との間の接触を検出するまで物体の一般方向に動かし、ロボット座標系における接点のロケーション１３０を測定することによって達成される。様々な実施形態において、ロボットのアームの内蔵機能及びエンコーダーによって、ロボットアームのコンプライアンス制御によって、かつ／又は、カメラ、タッチセンサー、若しくは力センサーのような外部センサーによって、接触のロケーションが検出及び測定される。
【００２０】
本発明の実施形態は、プローブの６−ＤＯＦ姿勢の推定に適用されるパーティクルフィルタリングは計算的に困難であるが、該６−ＤＯＦ姿勢を２つの部分、すなわち３−ＤＯＦロケーション及び３−ＤＯＦ配向に因数分解することができ、それによってプローブの配向が該プローブのロケーションに依存すると共に、該配向をそのロケーションに基づいてカルマンフィルタリングを使用して更新することができるという認識に基づく。この認識に基づいて、６−ＤＯＦパーティクルフィルタリングは３−ＤＯＦパーティクルフィルタリング及び３−ＤＯＦカルマンフィルタリングに変形される。この結果、より効率的な計算となる。そのような因数分解の理論的枠組みは、ラオ・ブラックウェル化パーティクルフィルタリングとして既知である。
【００２１】
図２は、本発明の実施形態による方法２００のブロック図を示している。上述した認識によれば、プローブと物体との間の相対姿勢は、ラオ・ブラックウェル化パーティクルフィルターを使用して求められる。ラオ・ブラックウェル化パーティクルフィルターにおいて、ロケーション（３−ＤＯＦ並進）にわたる確率分布はパーティクルを用いて表され、すなわち各パーティクルはロケーションに関する特定の仮説を表し、配向（３−ＤＯＦ回転）にわたる分布は、パーティクルのロケーションを条件とする、パーティクルごとのガウス分布によって近似される。この因数分解は、フィルタリングの効率を大幅に改善し、結果として、従来のパーティクルフィルタリングと比較して、必要とされるパーティクルの数が数桁削減される。
【００２２】
したがって、物体に対するプローブの姿勢の確率はパーティクルのセットによって表され（２１０）、姿勢のロケーションの確率は、各パーティクルのロケーション２１２によって表され、姿勢の配向の確率は、パーティクルのロケーションを条件とする、パーティクルごとの配向にわたるガウス分布２１５によって表される。
【００２３】
後続のプロービング２２０ごとに、プローブの動き２２５が得られ、該動きに従って各パーティクルのロケーションが次のロケーション２３５に更新され（２３０）、該パーティクルの次のロケーション、各パーティクルの配向にわたるガウス分布、及び物体のマップモデル５００に基づいて各パーティクルの重要度重み２５５が更新される（２５０）。ここで、重要度重みは、パーティクルのロケーションが接点１１５を表し、パーティクルの配向分布が物体に対するプローブの配向を表す確率である。また、パーティクルの次のロケーションを条件とするカルマンフィルタリング２４５、たとえば拡張カルマンフィルタリング（ＥＫＦ）を使用して、各パーティクルの配向にわたるガウス分布２１５が更新される（２４０）。ＥＫＦは、パーティクルの次のロケーションを条件とする配向にわたる確率分布の次の推定値２５８を生成する。
【００２４】
パーティクルのセット２１０内のパーティクルが特定の姿勢の周りに集中している（２７０）とき、物体に対するプローブの姿勢１５０は、その特定の姿勢に基づいて、たとえばパーティクルのロケーション及び配向の平均をとることによって求められる。そうでない場合、方法２００の再位置決めステップ、プロービングステップ、及び更新ステップが繰り返される。
【００２５】
１つの実施形態では、パーティクルの更新されたセット及び重要度重みを後続のプロービングに使用するか、又は各パーティクルの重要度重みに比例する確率を用いてパーティクルのセットを再サンプリングする（２６０）かの決定２６５が行われる。パーティクルのセットが再サンプリングされる場合、高い確率で、低い重要度重みを有するパーティクルがセット２１０から除去され、高い重要度重みを有するパーティクルがセットにおいて複数回表される。
【００２６】
１つの実施形態では、物体のマップモデルは、プロービングされている物体のモデル、たとえばコンピューター支援設計（ＣＡＤ）モデル、及び該ＣＡＤモデルの不確定性に関する任意の事前の知識を表す不確定性モデルに基づいて生成される。不確定性モデルは、物体の異なる特徴に関する不確定性を組み込む。したがって、マップモデルは確率マップモデルである。
【００２７】
本発明の実施形態によるマップモデルの使用は、実際の測定がＣＡＤモデルに一致しない状況において有利である。そのような差異は、スリッピング及びセンサーの不正確性のような様々な要因に起因する測定誤差、並びに物体のＣＡＤモデルと該物体の実際の形状との間の差異から生じ得る。
【００２８】
確率的依存関係
物体に対するプローブの姿勢は、（ｓ，θ）として表される。ここで、ｓ＝（ｘ，ｙ，ｚ）Ｔはプローブと物体との間の相対位置であり、θ＝（α，β，γ）Ｔはプローブと物体との間の相対配向である。角度（α，β，γ）は、（ｘ，ｙ，ｚ）軸のそれぞれを中心とした回転の角度として規定される。
【００２９】
図３は、方法２００において使用される確率的依存関係のグラフィックモデルを示している。各接点から次の接点までのプローブの動きのシーケンス３１０（該動きは時刻１からｔまで、各プロービングによって生じる）は、ｕ１：ｔ＝｛ｕ１，．．．，ｕｔ｝として規定され、ここで、ｕｉは、時刻ｉ−１における接点から時刻ｉにおける接点までの動きを表す。接触イベントのシーケンス３３０はｚ０：ｔ＝｛ｚ１，．．．，ｚｔ｝として規定される。本実施形態は、物体に対するプローブの姿勢の確率を、
【００３０】
【数１】
【００３１】
に従って因数分解する。
【００３２】
したがって、各パーティクルは、接点の位置のシーケンスＳ０：ｔ３２０及びプローブと物体との間の配向にわたるガウス分布３４０を表す。ガウス分布は平均μｔ及び共分散行列Σｔによって表される。
【００３３】
通常、接触イベントはバイナリ、すなわちＺｔ∈｛１，０｝であり、ここで、時点ｔにおいてプローブが位置ｓｔにおいて物体と接触している場合、Ｚｔ＝１であり、そうでない場合、Ｚｔ＝０である。しかしながら、接点は、プロービングごとに、該接点が感知されるとき、すなわちＺｔ＝１である時点でしか測定されない。したがって、明確にするために、ｐ（ｚｔ＝１）を省略表現ｐ（ｚｔ）を使用して表すことによって、確率の表記を簡略化する。
【００３４】
座標変換
図４は、物体に対して固定した物体座標系４１０と、ロボット座標系に対して固定したベース座標系４２０、すなわちロボットのアーム（図示せず）の基準系との間の座標変換を示している。いくつかの実施形態では、ロボット座標系とベース座標系との間の変換は、回転を伴わない並進であり、それによって、プローブの動きは、ベース座標系において、ロボット座標系における並進と同じ並進によって表される。
【００３５】
ベース座標系における、時刻ｔにおけるプローブの位置は、Ｓｔとして規定される。物体座標系における、時刻ｔにおけるプローブの位置は、Ｓｔ０として規定される。ベース座標系及び物体座標系は、プローブの初期位置Ｓ０４４０を中心とした３Ｄ回転４３０θによって関係付けられる。このため、プローブの初期位置は、ベース座標系及び物体座標系の双方において同じである。しかしながら、２つの座標系間の配向差異θは未知である。
【００３６】
マップモデル４６０及び測定確率は、物体座標系において規定される。時刻ｔ＝０において、ベース座標系及び物体座標系は同一である。時刻ｔ＝１において、プローブの動き４４５の後、ベース座標系に対する物体座標系の回転に対応して、各パーティクルの配向にわたるガウス分布が更新される。
【００３７】
動き
後続の各プロービングは、プローブを１つの接点から別の接点まで動かす。各接点について、ロボットアームの内部エンコーダーから、ロボット座標系におけるプローブの位置を取得することができる。時刻ｔ−１から時刻ｔまでのプローブの動きは、ロボット座標系及びベース座標系において同一である。動きｕｔを使用して、ベース座標系における各パーティクルの位置が、
【００３８】
【数２】
【００３９】
に従って位置Ｓｔ−１から次の位置ｓｔに更新される。ここで、Ｎ（μ，Σ）は、平均μ及び共分散行列Σを有する多変量正規（ガウス）分布を表し、σｘ、σｙ、σｚは、ロボットの動きの誤差又はロボットのアームのエンコーダーから取得された値における誤差に起因する雑音をモデル化している。
【００４０】
いくつかの実施形態は、実際のロボット動き誤差及びエンコーダー誤差よりも大きいσｘ、σｙ、σｚの値を使用して、パーティクル欠乏（deprivation）を考慮する。ここで、パーティクルのうちのいずれも初期接触位置の完全に正確な推定を有しないため、動きモデルにおける雑音によって、パーティクルフィルターがこの不正確性を経時的に補償することが可能になる。
【００４１】
物体座標系における時刻ｔにおけるプローブの位置Ｓｔ０は、配向θ＝（α，β，γ）Ｔを使用する回転行列Ｒに基づいて求められる。回転行列Ｒは、
【００４２】
【数３】
【００４３】
に従って求められる。ここで、Ｓαはｓｉｎαの省略表現であり、Ｃαはｃｏｓαの省略表現である。
【００４４】
したがって、物体座標における時刻ｔにおけるプローブの位置は、
【００４５】
【数４】
【００４６】
に従って求められる。
【００４７】
マップモデル
本発明のいくつかの実施形態は、物体の測定モデルが、物体モデル設計及び測定の不確定性、たとえば物体モデルと物体の実際の形状との間の差異、並びに測定ツール及び測定手順の不完全性を反映するべきであるという認識に基づく。たとえば、プローブの先端は、とがった針状である代わりに、丸みを有するか又は幅広である場合があり、それによって測定誤差がもたらされる。したがって、本発明者らは、面（平面）、エッジ（線）、及び頂点（点）のような、モデルの全ての特徴について求められる不確定性のパラメーターを求めると共に、該パラメーターをマップモデル内に含める。
【００４８】
図５は、本発明の実施形態による、マップモデル５００の１つの例を示している。物体のマップは、物体モデル１２５及び不確定性モデル５１０から求められる。いくつかの実施形態では、物体モデルは物体のＣＡＤモデルから生成される。物体モデルは、物体の形状の特徴ｆｉ５２０、たとえば、頂点、エッジ、及び三角形の面を含む。たとえば、図５に示すように、特徴ｆ１、ｆ３、ｆ５は頂点又はエッジを表し、特徴ｆ２及びｆ４はエッジ又は面を表す。
【００４９】
異なる実施形態では、不確定性モデルは、広範にわたる確率フィールド又はエネルギー関数を使用して定式化される。たとえば、１つの実施形態では、不確定性モデルは、接点１１５と物体モデルの特徴５２０との間の距離５４５のガウス確率密度関数を使用して定式化される。マップモデルは、物体モデルの異なる特徴に、異なる不確定性値を割り当てることができる。これによって、ユーザーが特定の産業的設定又は製造プロセスの知識をマップモデルに組み込むことが可能になる。
【００５０】
測定不確定性は、不確定性モデル５１０を形成する。特徴ｆｋごとに（ここでｋ＝｛１，．．．，Ｋ｝）、特徴の測定不確定性の標準偏差がσｋ５４０として規定される。１つの実施形態では、平面の交差部におけるエッジ及び頂点のような、その法線が互いに大きな角度を形成する特徴には、より大きな不確定性が割り当てられる。たとえば、この実施形態の１つの変形形態では、各エッジ／頂点の境界となる面の全ての可能な対の法線間の最大角度が求められ、最大角度の値に基づいて不確定性が割り当てられる。
【００５１】
測定確率は、物体モデルと、物体座標における接点Ｓ０５３０との間の距離５４５に基づいて求められる。接点から各特徴ｆｋへの距離がｄ（Ｓ０，ｆｋ）として規定される。この実施形態の１つの変形形態において、関数ｄは、接点Ｓ０と特徴ｆｋとの間のユークリッド距離を計算する。対応する平面又は線に対する接点の垂直射影が面又はエッジ内にある場合、該面又は該エッジへの距離が求められる。
【００５２】
接点ごとに、全ての特徴に対する距離が求められ、対応する標準偏差で正規化された最小の距離を有する特徴が接触特徴として選択される。このように、接触特徴のインデックスｋｃが
【００５３】
【数５】
【００５４】
に従って求められる。
【００５５】
１つの実施形態では、距離測定値ｈは、物体座標においてＳ０の関数として表されるか、又はベース座標において位置ｓ及び配向θの関数として表され、以下となる。
【００５６】
【数６】
【００５７】
パーティクルごとの時刻ｔにおける測定確率が、距離測定値に基づいて
【００５８】
【数７】
【００５９】
に従って求められる。
【００６０】
表記Ｎ（ｘ；μ，Σ）は、変数ｘにわたる、平均μ及び共分散行列Σの多変量正規（ガウス）分布を表す。
【００６１】
ラオ・ブラックウェル化パーティクルフィルタリング
図６は、ラオ・ブラックウェル化パーティクルフィルタリングを使用して、物体に対するプローブの姿勢の確率を求めるための擬似コードを示している。ここで、姿勢のロケーションの確率は、各パーティクルのロケーションによって表され、姿勢の配向の確率は、パーティクルのロケーションを条件とする、各パーティクルの配向にわたるガウス分布によって表される。
【００６２】
プロービングごとに、時刻ｔにおいて、各パーティクルｊがプローブと物体との間の接点の位置Ｓｔ［ｊ］、及び配向にわたる正規分布
【００６３】
【数８】
【００６４】
に関連付けられる。
【００６５】
初期化
方法２００は、最大初期（ｘ，ｙ）不確定性によって境されるベース座標における位置（ｘ，ｙ）にわたる一様分布からＪ個のパーティクルをサンプリングする。１つの実施形態では、初期パーティクル位置Ｓ０［ｊ］＝（ｘ，ｙ，ｚ）がマップモデルの表面上に設定されるように、パーティクルｊごとのｚ座標の初期値が求められる。パーティクルごとに、配向θにわたるガウス分布が平均μ０［ｊ］＝０及び初期不確定性を表す対角共分散行列Σ０［ｊ］で初期化される。
【００６６】
プロービングごとのパーティクル更新
時刻ｔにおけるパーティクルのセットはＸｔとして規定される。ロボットアームがプローブを１つの接点から次の接点に動かすとき、各パーティクルの位置ｊ＝｛１，．．．，Ｊ｝は、ロボットアームエンコーダーから取得されるベース座標における動きｕｔ、及びロボットが接触を検出するという観測ｚｔに基づいて更新される。各パーティクルの次の位置に基づいて、パーティクルごとの配向θにわたる分布が更新される。
【００６７】
位置更新
時刻ｔにおけるパーティクルｊの次の位置Ｓｔ［ｊ］は、式（２）を使用して、パーティクルの位置Ｓｔ−１［ｊ］及び動きｕｔに基づいて
【００６８】
【数９】
【００６９】
に従って求められる。
【００７０】
配向更新
ベース座標におけるパーティクルの次の位置Ｓｔ［ｊ］、及びプローブと物体との間に接触が存在したという観測ｚｔに基づいて、配向θにわたる、パーティクルごとの事後分布が、ＥＫＦを使用して求められる。１つの実施形態は、測定確率を、配向の関数として表し、その測定確率を、θ＝μｔ−１［ｊ］、すなわち前回のプロービング後に求められたパーティクルの配向の分布の平均を中心に線形化する。
【００７１】
測定確率を、θ＝μｔ−１［ｊ］を中心に線形化するために、平均値及び式（４）に基づいて、
【００７２】
【数１０】
【００７３】
に従って、物体座標系における各パーティクルの予測位置
【００７４】
【数１１】
【００７５】
が求められる。
【００７６】
予測パーティクル位置
【００７７】
【数１２】
【００７８】
を使用して、式（５）に従って接触特徴のインデックスｋｃが求められる。
【００７９】
接触特徴
【００８０】
【数１３】
【００８１】
に対して規定された測定確率（７）を使用して、角度にわたって事後確率を更新する。これは（１）の右辺の第２の因数である。
【００８２】
したがって、事後確率は以下のようにさらに因数分解される。
【００８３】
【数１４】
【００８４】
式（１０）を使用して、配向の事後確率をガウス分布として近似する。１つの実施形態は、
【００８５】
【数１５】
【００８６】
に従って、θ＝μｔ−１［ｊ］を中心に測定関数を線形化することによってＥＫＦを使用する。ここで、
【００８７】
【数１６】
【００８８】
は予測位置
【００８９】
【数１７】
【００９０】
において求められる距離測定値であり、Ｈｔ［ｊ］は、
【００９１】
【数１８】
【００９２】
に従って求められる、配向θに対する距離測定のヤコビ行列式である。
【００９３】
次に、標準ＥＫＦ測定更新規則を使用して、
【００９４】
【数１９】
【００９５】
に従って、配向にわたる事後分布が計算される。
【００９６】
重要度重み更新及び再サンプリング
各時刻ｔにおいて、各パーティクルｊの重要度重みｗｔ［ｊ］が、該パーティクルの前回の重要度重みｗｔ−１［ｊ］及び測定確率から求められる。本発明の１つの実施形態では、重要度重みは、
【００９７】
【数２０】
【００９８】
に従って、パーティクルの配向にわたって測定確率を周辺化することによって更新される。
【００９９】
したがって、
【０１００】
【数２１】
【０１０１】
であり、ここで、
【０１０２】
【数２２】
【０１０３】
である。
【０１０４】
パーティクルの多様性を妥当なレベルに保つために、１つの実施形態は、プロービングごとに、
【０１０５】
【数２３】
【０１０６】
に従って、パーティクルの有効数Ｊｅｆｆを推定する。
【０１０７】
Ｊｅｆｆがパーティクルのセット内のパーティクルの数に基づいて求められる閾値未満である、たとえばＪｅｆｆ＜Ｊ／２である場合、パーティクルは、パーティクルの重みｗｔ［ｊ］に比例する確率を用いて再サンプリングされる（２６０）。再サンプリング後、全てのパティクル重みはｗｔ［ｊ］＝１／Ｊに再設定される。
【０１０８】
時刻ｔにおいてパーティクルが再サンプリングされない場合、たとえばＪｅｆｆ≧Ｊ／２である場合、式（１７）に従って求められた重要度重みを有する全てのパーティクルがパーティクルのセット２１０内に保持される。
【０１０９】
１つの実施形態では、初期状態において、比較的多くの数のパーティクルセットがセット２１０内で使用される。しかしながら、方法２００を数回反復した後、位置同定に必要とされるパーティクルの数が減少する。パーティクルの数は、所定の数Ｊｍｉｎに達するまで各再サンプルにおいて低減される。この実施形態の１つの変形形態は、再サンプリングの前にパーティクルの数ＪがＪｍｉｎより大きい場合、パーティクルの現在の数の半分のみを再サンプリングすることによって、セット内のパーティクルの数を半分に低減する。
【０１１０】
集中
本発明の実施形態は、パーティクルのセット内のパーティクルが、特定の姿勢の周りに集中するまで、該パーティクルのセットのプロービング及び再サンプリングを反復する。１つの実施形態では、物体座標系におけるパーティクルの位置の重み付けされた平均値及び重み付けされた共分散に基づいて、
【０１１１】
【数２４】
【０１１２】
及び
【０１１３】
【数２５】
【０１１４】
に従って集中が求められる。ここで、Ｓｔ０［ｊ］は式（４）を使用して
【０１１５】
【数２６】
【０１１６】
に従って求められる。
【０１１７】
プロービングは、共分散行列Ψｔ０のトレースが所定の閾値未満になり、パーティクル位置間の不確定性が小さいことを示すまで繰り返される。この収束状態が達成されると、姿勢の推定位置が、セット内に残っている全てのパーティクルの位置の重み付けされた平均に基づいて求められる。姿勢の推定配向は、パーティクルの平均配向の重み付けされた平均に基づいて求められる。
【０１１８】
求められた姿勢を、後続の産業プロセスに直接使用することができる。求められた姿勢を、反復型最近接点のような、全ての接点を同時に使用することによってコスト関数を最小にする姿勢推定アルゴリズムの初期推定として使用することによって、姿勢をさらに精緻化することができる。
【０１１９】
本発明を好ましい実施形態の例として説明してきたが、本発明の精神及び範囲内で様々な他の適応及び変更を行うことができることは理解されたい。したがって、添付の特許請求の範囲の目的は、本発明の真の精神及び範囲内に入るすべての変形及び変更を包含することである。
【特許請求の範囲】
【請求項１】
物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングは、前記プローブと前記物体との間の接点を求め、それによって各該接点において前記姿勢が推定され、該方法は、
前記プローブと前記物体との間の前記姿勢の確率をパーティクルのセットによって表すステップであって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表すステップと、
前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置するステップであって、該動きは前記プロービングによって生じる、再配置するステップと、
各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求めるステップであって、該重要度重みは、前記パーティクルが前記物体に対する前記プローブの前記姿勢を表す確率である、求めるステップと、
各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新するステップと、
前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返すステップと、
前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求めるステップと、
を含み、該方法の前記ステップはプロセッサによって実行される、方法。
【請求項２】
物体モデル及び不確定性モデルに基づいて前記マップモデルを求めることをさらに含む、請求項１に記載の方法。
【請求項３】
前記物体モデルの各特徴に不確定性関数を割り当てることによって前記不確定性モデルを求めることであって、該特徴は面、エッジ、又は頂点である、求めることをさらに含む、請求項２に記載の方法。
【請求項４】
前記物体モデルの各前記特徴に関して、前記接点と前記特徴との間の距離のガウス確率密度関数として不確定性関数を定義することをさらに含む、請求項３に記載の方法。
【請求項５】
時刻ｔにおける接触イベントの観測確率を、
【数１】
に従って決定することをさらに含み、ここで、ｐ（・）は確率関数であり、Ｓｔは前記パーティクルの位置を表し、θは前記パーティクルの配向を表し、ｚｔは時刻ｔにおける前記プローブと前記物体との間の接触の観測値であり、ｈは距離測定値であり、ｋｃは接触特徴
【数２】
のインデックスであり、
【数３】
は前記接触特徴の前記不確定性関数の分散であり、Ｎ（ｘ；μ，σ２）は、変数ｘにわたる、平均μ及び分散σ２を有するガウス分布である確率密度関数を表す、請求項４に記載の方法。
【請求項６】
前記物体モデルの近傍の特徴の対間の角度に基づいて各前記特徴の前記不確定性関数を求めることをさらに含む、請求項４に記載の方法。
【請求項７】
前記更新の後、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングすることをさらに含む、請求項１に記載の方法。
【請求項８】
前記パーティクルのセット内の前記パーティクルの全てを単一のベース座標系に表すこと、
前記パーティクルのセット内の前記パーティクルごとの物体座標系を定義することであって、前記ベース座標系と各前記パーティクルの前記物体座標系との間の回転が各該パーティクルの前記姿勢の前記配向に対応し、前記回転の中心が該パーティクルの該姿勢の初期ロケーションである、定義すること、
前記ベース座標系において前記プローブの前記動きを表現すること、及び
前記物体座標系において前記マップモデルを表現すること、
をさらに含む、請求項１に記載の方法。
【請求項９】
前記プロービングは、ロボットのアームによって前記プローブを再位置決めし、該プローブを、該プローブと前記物体との間の接触を検出するまで前記物体の一般方向に動かすことによって達成され、前記方法は、
前記ロボットの前記アームのエンコーダーから前記接点の位置を受信することをさらに含む、請求項１に記載の方法。
【請求項１０】
前記再位置決めは、ランダムに、又はロケーションの所定のシーケンスに基づいて、又は前記パーティクルのセットにおける前記パーティクルの前記ロケーション及び前記配向に基づいて実行される、請求項９に記載の方法。
【請求項１１】
前記パーティクルのセット内のパーティクルの有効数を求めること、及び
前記有効数が閾値未満の場合、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングすることであって、前記閾値は前記パーティクルのセット内のパーティクルの数に基づいて求められる、再サンプリングすることをさらに含む、請求項７に記載の方法。
【請求項１２】
前記パーティクルの前記次のロケーションを、
【数４】
に従って求めることをさらに含み、ここで、Ｓｔ−１［ｊ］は時刻ｔ−１におけるパーティクルｊの前記ロケーションであり、Ｓｔ［ｊ］はパーティクルｊの前記次のロケーションであり、ｕｔは、時刻ｔ−１における前記プロービングの前記接点から、時刻ｔにおける前記プロービングの前記接点への前記動きを表す、請求項１に記載の方法。
【請求項１３】
パーティクルｊの前記姿勢の前記配向にわたる前記ガウス分布の前記パラメーターを、
【数５】
に従って更新することをさらに含み、ここで、μｔ−１［ｊ］は時刻ｔ−１におけるパーティクルｊの前記ガウス分布の平均であり、μｔ［ｊ］はパーティクルｊの次のガウス分布の平均であり、Σｔ−１［ｊ］は時刻ｔ−１におけるパーティクルｊの前記ガウス分布の共分散行列であり、Σｔ［ｊ］はパーティクルｊの前記次のガウス分布の共分散行列であり、
【数６】
は、接触特徴のインデックスｋｃを使用した、パーティクルｊの予測位置における距離測定値であり、
【数７】
であり、ここで
【数８】
は前記接触特徴のインデックスｋｃの不確定性測定値である、請求項１に記載の方法。
【請求項１４】
パーティクルｊの時刻ｔにおける前記重要度重みｗｔ［ｊ］を、
【数９】
に従って求めることをさらに含み、ここで、
【数１０】
である、請求項１３に記載の方法。
【請求項１５】
前記パーティクルのセット内の前記パーティクルが前記特定の姿勢の周りに集中するか否かを、前記物体座標系における前記パーティクルの前記ロケーションの共分散行列のトレースを所定の閾値と比較することによって求めることをさらに含む、請求項１に記載の方法。
【請求項１６】
前記接点を前記検出することは、前記ロボットアームの内蔵の衝撃検出機能、該ロボットアームのコンプライアンス制御、及び外部センサーのうちの少なくとも１つを使用して達成される、請求項９に記載の方法。
【請求項１７】
前記物体に対する前記プローブの前記姿勢が産業プロセスにおいて使用される、請求項１に記載の方法。
【請求項１８】
前記産業プロセスは、前記物体を持ち上げること、前記物体を位置決めすること、部品を前記物体と嵌合すること、前記物体に対するロボットの姿勢を求めること、前記物体に対し道具を使用すること、又はロボット座標系を較正することのうちの少なくとも１つを含む、請求項１７に記載の方法。
【請求項１９】
物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、
ラオ・ブラックウェル化パーティクルフィルタリングを使用して前記姿勢の確率を求めるステップであって、該姿勢のロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの前記配向にわたるガウス分布によって表され、該求めることは、前記姿勢の前記確率が特定の姿勢の周りに集中するまで、後続のプロービングごとに実行される、求めるステップと、
前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を推定するステップと、
を含む、方法。
【請求項２０】
前記求めることは、
前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置することであって、該動きは前記プロービングによって生じる、再配置すること、
各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求めることであって、該重要度重みは、前記パーティクルが前記接点を表す確率である、求めること、及び
各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新すること、
をさらに含む、請求項１９に記載の方法。
【請求項２１】
物体モデル及び不確定性モデルに基づいて前記マップモデルを求めることをさらに含む、請求項２０に記載の方法。
【請求項２２】
前記更新することの後に、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングすることをさらに含む、請求項２０に記載の方法。
【請求項２３】
物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるためのシステムであって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングの結果、前記プローブと前記物体との間の接点がもたらされ、それによって各該接点において前記姿勢が推定され、該システムは、
前記物体に対する前記姿勢の確率をパーティクルのセットによって表す手段であって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表す手段と、
前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置する手段であって、該動きは前記プロービングによって生じる、再配置する手段と、
各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求める手段であって、該重要度重みは、前記パーティクルが前記接点を表す確率である、求める手段と、
各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新する手段と、
前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返す手段と、
前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求める手段と、
を含み、前記方法の前記ステップはプロセッサによって実行される、システム。
【請求項２４】
物体モデル及び不確定性モデルに基づいて前記マップモデルを求める手段をさらに備える、請求項２３に記載のシステム。
【請求項２５】
前記更新すること及び前記反復することの間に、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングする手段をさらに備える、請求項２３に記載のシステム。
【請求項１】
物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングは、前記プローブと前記物体との間の接点を求め、それによって各該接点において前記姿勢が推定され、該方法は、
前記プローブと前記物体との間の前記姿勢の確率をパーティクルのセットによって表すステップであって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表すステップと、
前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置するステップであって、該動きは前記プロービングによって生じる、再配置するステップと、
各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求めるステップであって、該重要度重みは、前記パーティクルが前記物体に対する前記プローブの前記姿勢を表す確率である、求めるステップと、
各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新するステップと、
前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返すステップと、
前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求めるステップと、
を含み、該方法の前記ステップはプロセッサによって実行される、方法。
【請求項２】
物体モデル及び不確定性モデルに基づいて前記マップモデルを求めることをさらに含む、請求項１に記載の方法。
【請求項３】
前記物体モデルの各特徴に不確定性関数を割り当てることによって前記不確定性モデルを求めることであって、該特徴は面、エッジ、又は頂点である、求めることをさらに含む、請求項２に記載の方法。
【請求項４】
前記物体モデルの各前記特徴に関して、前記接点と前記特徴との間の距離のガウス確率密度関数として不確定性関数を定義することをさらに含む、請求項３に記載の方法。
【請求項５】
時刻ｔにおける接触イベントの観測確率を、
【数１】
に従って決定することをさらに含み、ここで、ｐ（・）は確率関数であり、Ｓｔは前記パーティクルの位置を表し、θは前記パーティクルの配向を表し、ｚｔは時刻ｔにおける前記プローブと前記物体との間の接触の観測値であり、ｈは距離測定値であり、ｋｃは接触特徴
【数２】
のインデックスであり、
【数３】
は前記接触特徴の前記不確定性関数の分散であり、Ｎ（ｘ；μ，σ２）は、変数ｘにわたる、平均μ及び分散σ２を有するガウス分布である確率密度関数を表す、請求項４に記載の方法。
【請求項６】
前記物体モデルの近傍の特徴の対間の角度に基づいて各前記特徴の前記不確定性関数を求めることをさらに含む、請求項４に記載の方法。
【請求項７】
前記更新の後、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングすることをさらに含む、請求項１に記載の方法。
【請求項８】
前記パーティクルのセット内の前記パーティクルの全てを単一のベース座標系に表すこと、
前記パーティクルのセット内の前記パーティクルごとの物体座標系を定義することであって、前記ベース座標系と各前記パーティクルの前記物体座標系との間の回転が各該パーティクルの前記姿勢の前記配向に対応し、前記回転の中心が該パーティクルの該姿勢の初期ロケーションである、定義すること、
前記ベース座標系において前記プローブの前記動きを表現すること、及び
前記物体座標系において前記マップモデルを表現すること、
をさらに含む、請求項１に記載の方法。
【請求項９】
前記プロービングは、ロボットのアームによって前記プローブを再位置決めし、該プローブを、該プローブと前記物体との間の接触を検出するまで前記物体の一般方向に動かすことによって達成され、前記方法は、
前記ロボットの前記アームのエンコーダーから前記接点の位置を受信することをさらに含む、請求項１に記載の方法。
【請求項１０】
前記再位置決めは、ランダムに、又はロケーションの所定のシーケンスに基づいて、又は前記パーティクルのセットにおける前記パーティクルの前記ロケーション及び前記配向に基づいて実行される、請求項９に記載の方法。
【請求項１１】
前記パーティクルのセット内のパーティクルの有効数を求めること、及び
前記有効数が閾値未満の場合、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングすることであって、前記閾値は前記パーティクルのセット内のパーティクルの数に基づいて求められる、再サンプリングすることをさらに含む、請求項７に記載の方法。
【請求項１２】
前記パーティクルの前記次のロケーションを、
【数４】
に従って求めることをさらに含み、ここで、Ｓｔ−１［ｊ］は時刻ｔ−１におけるパーティクルｊの前記ロケーションであり、Ｓｔ［ｊ］はパーティクルｊの前記次のロケーションであり、ｕｔは、時刻ｔ−１における前記プロービングの前記接点から、時刻ｔにおける前記プロービングの前記接点への前記動きを表す、請求項１に記載の方法。
【請求項１３】
パーティクルｊの前記姿勢の前記配向にわたる前記ガウス分布の前記パラメーターを、
【数５】
に従って更新することをさらに含み、ここで、μｔ−１［ｊ］は時刻ｔ−１におけるパーティクルｊの前記ガウス分布の平均であり、μｔ［ｊ］はパーティクルｊの次のガウス分布の平均であり、Σｔ−１［ｊ］は時刻ｔ−１におけるパーティクルｊの前記ガウス分布の共分散行列であり、Σｔ［ｊ］はパーティクルｊの前記次のガウス分布の共分散行列であり、
【数６】
は、接触特徴のインデックスｋｃを使用した、パーティクルｊの予測位置における距離測定値であり、
【数７】
であり、ここで
【数８】
は前記接触特徴のインデックスｋｃの不確定性測定値である、請求項１に記載の方法。
【請求項１４】
パーティクルｊの時刻ｔにおける前記重要度重みｗｔ［ｊ］を、
【数９】
に従って求めることをさらに含み、ここで、
【数１０】
である、請求項１３に記載の方法。
【請求項１５】
前記パーティクルのセット内の前記パーティクルが前記特定の姿勢の周りに集中するか否かを、前記物体座標系における前記パーティクルの前記ロケーションの共分散行列のトレースを所定の閾値と比較することによって求めることをさらに含む、請求項１に記載の方法。
【請求項１６】
前記接点を前記検出することは、前記ロボットアームの内蔵の衝撃検出機能、該ロボットアームのコンプライアンス制御、及び外部センサーのうちの少なくとも１つを使用して達成される、請求項９に記載の方法。
【請求項１７】
前記物体に対する前記プローブの前記姿勢が産業プロセスにおいて使用される、請求項１に記載の方法。
【請求項１８】
前記産業プロセスは、前記物体を持ち上げること、前記物体を位置決めすること、部品を前記物体と嵌合すること、前記物体に対するロボットの姿勢を求めること、前記物体に対し道具を使用すること、又はロボット座標系を較正することのうちの少なくとも１つを含む、請求項１７に記載の方法。
【請求項１９】
物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるための方法であって、
ラオ・ブラックウェル化パーティクルフィルタリングを使用して前記姿勢の確率を求めるステップであって、該姿勢のロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの前記配向にわたるガウス分布によって表され、該求めることは、前記姿勢の前記確率が特定の姿勢の周りに集中するまで、後続のプロービングごとに実行される、求めるステップと、
前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を推定するステップと、
を含む、方法。
【請求項２０】
前記求めることは、
前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置することであって、該動きは前記プロービングによって生じる、再配置すること、
各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求めることであって、該重要度重みは、前記パーティクルが前記接点を表す確率である、求めること、及び
各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新すること、
をさらに含む、請求項１９に記載の方法。
【請求項２１】
物体モデル及び不確定性モデルに基づいて前記マップモデルを求めることをさらに含む、請求項２０に記載の方法。
【請求項２２】
前記更新することの後に、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングすることをさらに含む、請求項２０に記載の方法。
【請求項２３】
物体に対するプローブの姿勢を、該プローブを用いて該物体をプロービングすることによって求めるためのシステムであって、前記姿勢は該姿勢のロケーション及び該姿勢の配向を含み、各前記プロービングの結果、前記プローブと前記物体との間の接点がもたらされ、それによって各該接点において前記姿勢が推定され、該システムは、
前記物体に対する前記姿勢の確率をパーティクルのセットによって表す手段であって、前記姿勢の前記ロケーションの確率が各前記パーティクルのロケーションによって表され、前記姿勢の前記配向の確率が、各前記パーティクルの前記ロケーションを条件とする、該パーティクルの配向にわたるガウス分布によって表される、表す手段と、
前記接点から次の接点への前記プローブの動きに従って各前記パーティクルを次のロケーションに再配置する手段であって、該動きは前記プロービングによって生じる、再配置する手段と、
各前記パーティクルの重要度重みを、該パーティクルの前記次のロケーション、該パーティクルの前記配向の前記確率、及び前記物体のマップモデルに基づいて求める手段であって、該重要度重みは、前記パーティクルが前記接点を表す確率である、求める手段と、
各前記パーティクルの前記次のロケーションを条件とするカルマンフィルタリングを使用して、前記パーティクルのセット内の各前記パーティクルの前記配向の前記確率を更新する手段と、
前記パーティクルのセット内の前記パーティクルが特定の姿勢の周りに集中するまで前記再配置すること、前記求めること、及び前記更新することを繰り返す手段と、
前記特定の姿勢に基づいて前記物体に対する前記プローブの前記姿勢を求める手段と、
を含み、前記方法の前記ステップはプロセッサによって実行される、システム。
【請求項２４】
物体モデル及び不確定性モデルに基づいて前記マップモデルを求める手段をさらに備える、請求項２３に記載のシステム。
【請求項２５】
前記更新すること及び前記反復することの間に、各前記パーティクルの前記重要度重みに基づいて前記パーティクルのセットを再サンプリングする手段をさらに備える、請求項２３に記載のシステム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−２２１００８（Ｐ２０１１−２２１００８Ａ）
【公開日】平成２３年１１月４日（２０１１．１１．４）
【国際特許分類】
【外国語出願】
【出願番号】特願２０１１−５２７９４（Ｐ２０１１−５２７９４）
【出願日】平成２３年３月１０日（２０１１．３．１０）
【出願人】（５９７０６７５７４）ミツビシ・エレクトリック・リサーチ・ラボラトリーズ・インコーポレイテッド (484)
【住所又は居所原語表記】２０１ ＢＲＯＡＤＷＡＹ， ＣＡＭＢＲＩＤＧＥ， ＭＡＳＳＡＣＨＵＳＥＴＴＳ ０２１３９， Ｕ．Ｓ．Ａ．
【Ｆターム（参考）】
【公開日】平成２３年１１月４日（２０１１．１１．４）
【国際特許分類】
【出願番号】特願２０１１−５２７９４（Ｐ２０１１−５２７９４）
【出願日】平成２３年３月１０日（２０１１．３．１０）
【出願人】（５９７０６７５７４）ミツビシ・エレクトリック・リサーチ・ラボラトリーズ・インコーポレイテッド (484)
【住所又は居所原語表記】２０１ ＢＲＯＡＤＷＡＹ， ＣＡＭＢＲＩＤＧＥ， ＭＡＳＳＡＣＨＵＳＥＴＴＳ ０２１３９， Ｕ．Ｓ．Ａ．
【Ｆターム（参考）】
[ Back to top ]