(57)【特許請求の範囲】
【請求項１】
複数の姿勢においてプローブを用いて物体をプロービングすることによって、前記物体に対する前記プローブの姿勢を求める方法であって、前記プローブの前記各姿勢は、ロケーションおよび向きを含み、前記方法は、
粒子のセットによって前記プローブの現在のロケーションの確率分布を表し、なお、前記各粒子は、前記粒子のロケーションと前記粒子の確率に対応する重みとを有し、前記各粒子について、前記粒子の前記ロケーションを条件とするガウス分布によって前記プローブの現在の向きの確率分布を表すステップと、
候補動作のセットを選ぶステップと、
前記各候補動作について、前記粒子のセットに基づいて期待不確実性を求めるステップと、
最小の期待不確実性を有する前記候補動作を前記プローブの次の動作として選択するステップと、
前記次の動作に従って前記プローブを移動させるステップと、
前記プローブの次の姿勢を用いて前記粒子のセットを更新するステップと、
を含み、前記物体に対する前記プローブの姿勢を求めることは、ＲＢＰＦ（Ｒａｏ  Ｂｌａｃｋｗｅｌｌｉｚｅｄ  ｐａｒｔｉｃｌｅ  ｆｉｌｔｅｒｉｎｇ）フレームワークにおいて実行され、
前記姿勢は、（ｓ，θ）であり、ここで、ｓは相対的な３Ｄ並進ロケーション（ｘ，ｙ，ｚ）Ｔであり、θは相対的な３Ｄ角度の向き（α，β，γ）Ｔであり、Ｔは転置演算子あり、
姿勢のシーケンスが時間０〜ｔのｓ０：ｔであり、動作のシーケンスが時間１〜ｔのｕ１：ｔであり、測定値のシーケンスが時間０〜ｔのｚ０：ｔであり、
前記ロケーションおよび前記向きにわたる事後確率分布ｐ（ｓ０：ｔ，θ｜ｚ０：ｔ，ｕ１：ｔ）を推測することを更に含み、
前記ロケーションおよび前記向きにわたる前記事後確率分布を
【数１】
0005812940.tif 000016
として因数分解すること、
を更に含み、
前記事後確率分布の前記不確実性は、エントロピー
【数２】
0005812940.tif 000017
として求められ、ここで、
【数３】
0005812940.tif 000018
は、ｓ０：ｔにわたる期待値を示し、
【数４】
0005812940.tif 000019
は、前記ロケーションにわたる前記確率分布の前記エントロピーを表し、
【数５】
0005812940.tif 000020
は、前記向きにわたる前記確率分布の前記エントロピーを表し、
かつ各ステップは、プロセッサにより実行される、複数の姿勢においてプローブを用いて物体をプロービングすることによって、前記物体に対する前記プローブの姿勢を求める方法。
【請求項２】
前記更新するステップは、
動作モデルを用いて前記粒子のセット内の前記各粒子の前記ロケーションを求めることと、
前記物体の３Ｄモデルおよび不確実性モデルから導出されたマップに基づく尤度を用いて前記粒子のセット内の前記各粒子の前記重みを求めることであって、前記不確実性モデルは、前記プローブによって取得された測定値に内在する不確実性を記述する、求めることと、
拡張カルマンフィルタリングを用いて前記粒子のセット内の前記各粒子の前記ガウス分布を更新することと、
を更に含む、請求項１に記載の方法。
【請求項３】
前記向きにわたる前記確率分布の前記エントロピーは、前記粒子のセット内の前記粒子の前記重みおよび前記粒子のセット内の前記粒子の前記ガウス分布から推定される、請求項１に記載の方法。
【請求項４】
前記ロケーションにわたる前記確率分布の前記エントロピーは、単一のガウス分布を用いて前記粒子のセット内の前記粒子の前記ロケーションおよび前記重みを近似することによって推定される、請求項１に記載の方法。
【請求項５】
前記ロケーションにわたる前記確率分布の前記エントロピーは、前記粒子のセット内の前記粒子の前記ロケーションおよび前記重みからのカーネル密度推定を用いることによって推定される、請求項１に記載の方法。
【請求項６】
前記ロケーションにわたる前記確率分布の前記エントロピーは、前記粒子のセット内の前記粒子の前記重みのみを用いることによって推定される、請求項１に記載の方法。
【請求項７】
前記プローブは、ロボットアームに取り付けられ、前記プローブは、前記次の動作に従って前記ロボットアームによって自動的に移動される、請求項１に記載の方法。
【請求項８】
前記プローブは、オペレーターによって手動で移動され、前記次の動作は、前記プローブを移動させるように前記オペレーターをガイドするのに用いられる、請求項１に記載の方法。
【請求項９】
前記候補動作は、ランダムに選択される、請求項１に記載の方法。
【請求項１０】
前記ステップは、終了条件が満たされるまで繰り返される、請求項１に記載の方法。
【請求項１１】
前記終了条件は、所定のプロービング回数である、請求項１０に記載の方法。
【請求項１２】
前記終了条件は、前記粒子のセット内の前記粒子の前記ロケーションの収束である、請求項１０に記載の方法。
【請求項１３】
前記複数の姿勢は、前記プローブの座標系で測定される、請求項１に記載の方法。
【請求項１４】
前記ロケーションおよび前記向きは、物体座標系とプローブ座標系との間の変換を示す、請求項１に記載の方法。
【請求項１５】
前記候補動作は、前記プローブの座標系で表される、請求項１に記載の方法。
【請求項１６】
複数の姿勢においてプローブを用いて物体をプロービングすることによって、前記物体に対する前記プローブの姿勢を求めるシステムであって、前記プローブの前記各姿勢は、ロケーションおよび向きを含み、前記システムは、
粒子のセットによって前記プローブの現在のロケーションの確率分布を表し、なお、前記各粒子は、前記粒子のロケーションを有し、前記粒子の確率に対応する重みを有し、前記各粒子について、前記粒子の前記ロケーションを条件とするガウス分布によって前記プローブの現在の向きの確率分布を表す手段と、
候補動作のセットを選ぶ手段と、
前記各候補動作について、前記粒子のセットに基づいて期待不確実性を求める手段と、
最小の期待不確実性を有する前記候補動作を前記プローブの次の動作として選択する手段と、
前記次の動作に従って前記プローブを移動させるように構成されるロボットアームと、
前記プローブの次の姿勢を用いて前記粒子のセットを更新する手段と、
を備え、前記物体に対する前記プローブの姿勢を求めることは、ＲＢＰＦ（Ｒａｏ  Ｂｌａｃｋｗｅｌｌｉｚｅｄ  ｐａｒｔｉｃｌｅ  ｆｉｌｔｅｒｉｎｇ）フレームワークにおいて実行され、
前記姿勢は、（ｓ，θ）であり、ここで、ｓは相対的な３Ｄ並進ロケーション（ｘ，ｙ，ｚ）Ｔであり、θは相対的な３Ｄ角度の向き（α，β，γ）Ｔであり、Ｔは転置演算子あり、
姿勢のシーケンスが時間０〜ｔのｓ０：ｔであり、動作のシーケンスが時間１〜ｔのｕ１：ｔであり、測定値のシーケンスが時間０〜ｔのｚ０：ｔであり、
前記ロケーションおよび前記向きにわたる事後確率分布ｐ（ｓ０：ｔ，θ｜ｚ０：ｔ，ｕ１：ｔ）を推測することを更に含み、
前記ロケーションおよび前記向きにわたる前記事後確率分布を
【数６】
0005812940.tif 000021
として因数分解すること、
を更に含み、
前記事後確率分布の前記不確実性は、エントロピー
【数７】
0005812940.tif 000022
として求められ、ここで、
【数８】
0005812940.tif 000023
は、ｓ０：ｔにわたる期待値を示し、
【数９】
0005812940.tif 000024
は、前記ロケーションにわたる前記確率分布の前記エントロピーを表し、
【数１０】
0005812940.tif 000025
は、前記向きにわたる前記確率分布の前記エントロピーを表す、
複数の姿勢においてプローブを用いて物体をプロービングすることによって、前記物体に対する前記プローブの姿勢を求めるシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、包括的には、産業ロボットに関し、より詳細には、産業ロボットにおける確率的ロケーション特定（ｐｒｏｂａｂｉｌｉｓｔｉｃ  ｌｏｃａｌｉｚａｔｉｏｎ）のためのプロービングベースの方法およびシステムに関する。
【背景技術】
【０００２】
ロボットに対して物体をレジストレーションすることは、把持および挿入等の多くの産業アセンブリの作業において行われる。「レジストレーションする」とは、物体とロボットとの間の相対的な姿勢（ｐｏｓｅ）を決める（ｆｉｎｄ）ことをいう。プロービングベースのレジストレーション方法は、ロボットに取り付けられたプローブを用いて、物体と接触するロケーションを測定し、それらのロケーションを用いて、物体に対してロボットをレジストレーションする。
【０００３】
いくつかの従来の方法は、プロービングおよび粒子ベースのモンテカルロロケーション特定を用いて、レジストレーション問題を解いている。例えば、１つの方法は、鍵および鍵穴のアセンブリ（ｌｏｃｋ  ａｎｄ  ｋｅｙ  ａｓｓｅｍｂｌｙ）にプロービングおよび粒子フィルタリングを用いるロケーション特定を記載している。前処理ステップとして、この方法は、プローブを用いて物体上の（ｘ，ｙ）ロケーションのグリッドを高密度にプロービングし、接触ロケーション（ｘ，ｙ，ｚ）の高密度なセットを取得し、接触構成−空間マップ（ｃｏｎｔａｃｔ  ｃｏｎｆｉｇｕｒａｔｉｏｎ−ｓｐａｃｅ  ｍａｐ）を生成する。この接触構成−空間マップは、プローブが物体と接触する可能な姿勢を示している。次に、この方法は、プロービングから取得された接触ロケーションのシーケンスを用いて、物体を順次プロービングすることによって粒子フィルタリングを行う。接触構成−空間マップを用いて尤度が評価される。
【０００４】
物体を高密度にプロービングする前処理ステップを用いる別の方法は、プローブに接続された力およびトルクセンサーを用いて、力トルクマップを生成する。力トルクマップは、プローブと物体との間に接触が存在するあらゆる可能な姿勢における接触力およびトルクを含む。この方法は、コンピューター支援設計（ＣＡＤ：ｃｏｍｐｕｔｅｒ  ａｉｄｅｄ  ｄｅｓｉｇｎ）モデルから直接、力トルクマップを推定することも記載している。高密度プロービングによって事前に取得された力トルクマップを用いて、力トルク測定値をマップと照合するために、粒子フィルタリングが順次プロービングとともに用いられる。この方法は、カメラからの観測値も任意選択で組み込む。
【０００５】
６自由度（６−ＤＯＦ：６−ｄｅｇｒｅｅ  ｏｆ  ｆｒｅｅｄｏｍ）レジストレーションに必要とされる粒子数を削減するために、別の方法は、各粒子に、単一の点の代わりに領域を割り当てることによる粗密（ｃｏａｒｓｅ  ｔｏ  ｆｉｎｅ）手法を用いる。この方法は、最初に、測定不確実性を増大させることによって粗い解像度で高い尤度の領域を求め、その後、不確実性を低減させながら、領域内の粒子を反復的に再サンプリングする。
【０００６】
物体の点測定値のセットおよび平面のセットとしての物体の３Ｄモデルが与えられると、いくつかの方法は、３Ｄ平面と３Ｄ点との間の３つの対応関係を６−ＤＯＦレジストレーションの法線とともに用いる。１つの方法は、６つの３Ｄ点対３Ｄ平面の対応関係を用いる。これらの方法は、それぞれ、最小数の対応関係を用い、ランダムサンプルコンセンサス（ＲＡＮＳＡＣ：ＲＡＮｄｏｍ  ＳＡｍｐｌｅ  Ｃｏｎｓｅｎｓｕｓ）等の仮説検定フレームワークにおける仮説生成器として適している。点対平面の対応関係を用いる別の方法は、分岐限定最適化を利用して、大域的に最適なレジストレーションを達成する。反復最近傍点（ＩＣＰ：ｉｔｅｒａｔｉｖｅ  ｃｌｏｓｅｓｔ  ｐｏｉｎｔ）法は、多くの場合、レジストレーションの良好な初期推定値を有しない限り、大域的最適解に達しない。これらのオフライン法は、点−平面レジストレーション問題に対する解を提供するが、接触測定値の完全なセットが既に収集されていることを前提とする。これらの方法は、接触測定値の既に収集されたサブセットに基づいて次の良好な動作を選択する方法も提供していないし、十分な測定値がいつ収集されたのかを判断する終了条件も提供していない。
【０００７】
この発明の発明者らの知る限りでは、動作選択、すなわち、次にプロービングする箇所を計画する一般的な問題は、プロービングベースのレジストレーションにおいてこれまで対処されてこなかった。しかしながら、動作選択は、或る方法が移動ロボットの次の動作を計画するアクティブな同時のロケーション特定およびマッピング（アクティブＳＬＡＭ：ａｃｔｉｖｅ  ｓｉｍｕｌｔａｎｅｏｕｓ  ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  ｍａｐｐｉｎｇ）の用途等の移動ロボット工学においては既知である。
【０００８】
移動ロボット工学では、次の動作を選択することは、多くの場合、不確実性の低減量の尺度を示す期待情報利得またはエントロピーの低減量等のエントロピーベースの尺度を用いて対処される。しかしながら、ガウス近似を用いて粒子分布のエントロピーを求める等、通常は、従来の移動ロボットにおいて用いられるエントロピー推定方法は、プロービングベースのレジストレーションにあまり適していない。
【発明の概要】
【発明が解決しようとする課題】
【０００９】
ロボットに対して物体をレジストレーションすることは、把持および挿入等の多くの産業アセンブリの作業において行われる。プロービングベースのレジストレーション方法は、ロボットに取り付けられたプローブを用いて、物体との複数の接触ロケーションを測定し、それらのロケーションを用いて、ロボットを物体のモデルにレジストレーションする。このレジストレーション手法では、接触ロケーションの選択は、通常、議論されていない。
【課題を解決するための手段】
【００１０】
この発明の実施の形態は、次のロボットの動作を選択し、期待情報として定量化され、結果として行われる物体との接触によって得られる、不確実性の期待低減量を最大にする方法を提供する。本方法は、以下のように、ラオブラックウェル化粒子フィルタリング（ＲＢＰＦ：Ｒａｏ  Ｂｌａｃｋｗｅｌｌｉｚｅｄ  ｐａｒｔｉｃｌｅ  ｆｉｌｔｅｒｉｎｇ）フレームワークにおいて６−ＤＯＦレジストレーションを実行する。
【００１１】
プローブのロケーションは、粒子のセットによって表される。３Ｄガウス角度分布が各粒子について規定される。
【００１２】
物体の３Ｄモデルおよび不確実性モデルから導出されたマップ、並びに現在の粒子分布を用いると、次のロボット動作からの期待情報利得が、動作によって更新された粒子のエントロピーを用いて求められる。
【００１３】
最大情報利得、すなわち最小エントロピーを提供する動作が選択され、次の測定に用いられる。このプロセスは、粒子分布の収束または所定のプロービング回数等の終了条件に達するまで繰り返される。
【発明の効果】
【００１４】
この発明の発明者らのＲＢＰＦベースの方法は、各動作をランダムに選ぶ従来技術の方法とは異なり、いくつかの候補動作を選び、次に、期待エントロピーの形態の期待不確実性の推定値を用いて、最良の候補動作を選択する。この発明の発明者らは、この発明の発明者らの方法を完全自動レジストレーションだけでなく、人間がガイドするレジストレーションにも適用する。この発明の発明者らのエントロピーベースの動作選択方法は、性能がランダムな動作選択よりも大幅に優れている。
【図面の簡単な説明】
【００１５】
【図１】この発明の実施の形態によるロボットに取り付けられるかまたはロボット内に組み込まれるプローブに対して物体をレジストレーションするシステムの概略図である。
【図２】この発明の実施の形態によるプローブに対して物体をレジストレーションする方法のブロック図である。
【発明を実施するための形態】
【００１６】
この発明の実施の形態は、ロボットに取り付けられるかまたはロボット内に組み込まれるプローブに対して物体をレジストレーションする方法およびシステムを提供する。プローブの姿勢は、（並進）ロケーションおよび（角度の）向きを含む。各プロービングは、物体とプローブとの間の接触点を求め、それによって、各接触点における姿勢を推定することができる。物体は、関連付けられた物体座標系を有し、プローブは、関連付けられたプローブ座標系を有する。プローブは、ロボットの座標系に対してレジストレーションされる。したがって、ロボットと物体との間のレジストレーションは、プローブと物体との間のレジストレーションによって求められる。
【００１７】
米国出願第１２／７５１３６６号において、この発明の発明者らは、６−ＤＯＦレジストレーション問題にラオブラックウェル化粒子フィルタリング（ＲＢＰＦ）を用いた。粒子は、確率分布を、粒子の３−ＤＯＦロケーションと、各粒子のロケーションを条件とするガウス分布によって近似された３−ＤＯＦ角度分布とに因数分解する。
【００１８】
この米国出願では、ロボットアームに取り付けられたプローブを用いて物体にタッチすることによって物体をプローブとレジストレーションする。タッチのシーケンスを用いてこのレジストレーションを行った。このシーケンスにおける次のタッチ点を求める各動作は、ランダムに選択された。
【００１９】
次に、前に開示したＲＢＰＦフレームワークに基づく動作選択方法を説明する。この方法は、エントロピーに基づいて最適な次の動作を求める。また、自動レジストレーションだけでなく、人がガイドするレジストレーションについても、この方法の新規な適用を説明する。
【００２０】
ラオブラックウェル化粒子フィルタリングを用いたプロービングベースの６−ＤＯＦレジストレーション
図１は、物体１２０をプローブ１１０とレジストレーションするシステム１００および方法２００を示している。この発明のさまざまな実施の形態において、プローブは、タッチプローブ、例えば、力およびトルクの測定値を用いるプローブであるか、またはタッチレスプローブ、例えば、電気的プローブ、機械的プローブ、光学的プローブ、若しくはレーザープローブである。各プロービングによって、物体との接触点１１５が求められる。
【００２１】
物体に対するプローブの姿勢（ロケーションおよび向き）が求められる。姿勢は、その後、産業プロセスにおいて用いることができる。産業プロセスは、物体をピックアップすること、物体を位置決めすること、物体に部品を接合すること、物体に対するロボットの姿勢を求めること、ロボット座標系に対して物体若しくはワールドの座標系を較正すること、またはスプレーガン、アーク溶接機、ドリル、若しくはグラインダー等の道具を物体に対して用いることを含むことができる。
【００２２】
物体は、ＣＡＤモデル等の３Ｄモデル１２５によって表される。３つまでの自由度（３−ＤＯＦ）の不確実性モデル１２６は、プローブによって取得される測定値に内在する不確実性を記述する。３Ｄモデルおよび不確実性モデルを組み合わせて、プロセッサ１４０において実行される方法２００によって用いられるマップ１２７を生成し、姿勢１５０を求めることができる。プロセッサは、当該技術分野で知られているように入出力インターフェースおよびメモリ１４５を含む。３Ｄモデル、不確実性モデル、およびマップは、メモリ１４５に記憶することができる。代替的に、本方法は、ディスクリート回路、フィールドプログラマブルゲートアレイ（ＦＰＧＡ：ｆｉｅｌｄ  ｐｒｏｇｒａｍｍａｂｌｅ  ｇａｔｅ  ａｒｒａｙ）、または専用デジタル信号プロセッサ等の他の手段によって実施することができる。
【００２３】
１つの実施の形態では、プロービングは、プローブと物体との間の接触を検出するまで、プローブが配置されているロボットアーム１６０に動作を適用することによって達成される。
【００２４】
一般的な方法
この発明の実施の形態は、プローブの６−ＤＯＦ姿勢の推定に適用される標準的な粒子フィルタリングが、必要とされる粒子の数が自由度の数の概ね指数関数となるので極めて非効率的であるという認識に基づいている。しかしながら、６−ＤＯＦ姿勢は、２つの部分、すなわち、粒子のセットによって表される３−ＤＯＦロケーションと、各粒子がプローブの向きについてそれ自身の分布を保持するようにガウス分布によって表される３−ＤＯＦ向きとに因数分解することができる。各粒子の向きは、カルマンフィルタリングを用いて更新することができる。この認識に基づくと、６−ＤＯＦ粒子フィルタリングは、３−ＤＯＦ粒子フィルタリングおよび３−ＤＯＦカルマンフィルタリングに縮約される。このような因数分解のフレームワークは、ラオブラックウェル化粒子フィルタリング（ＲＢＰＦ）として知られている。
【００２５】
一般的に、問題は、物体をプローブとレジストレーションすることである。これは、３Ｄ接触ロケーションを測定することによって、物体に対するプローブの姿勢を求めることによって行われる。
【００２６】
姿勢における６−ＤＯＦの不確実性は、（ｓ，θ）である。ここで、ｓは３Ｄ並進ロケーション（ｘ，ｙ，ｚ）Ｔであり、θは３Ｄ角度の向き（α，β，γ）Ｔであり、Ｔは転置演算子ある。
【００２７】
時間１〜ｔの動作のシーケンス（ｕ１：ｔで示す）および時間０〜ｔの測定値のシーケンス（ｚ０：ｔで示す）が与えられると、この発明の発明者らの目標は、ロケーションおよび向きにわたる事後確率分布
【００２８】
【数１】
0005812940.tif 000002
【００２９】
を推測することである。
【００３０】
この発明の発明者らの手法は、以下のように、事後確率分布をロケーションおよび向きに因数分解することである。
【００３１】
【数２】
0005812940.tif 000003
【００３２】
ロケーションにわたる確率分布（式（１）の右側の第１因数）は、粒子によって表され、これによって、ロケーションの多モード分布を表すことが可能になる。向きにわたる確率分布（式（１）の右側の第２因数）は、各粒子について、この粒子の現在のロケーションおよび前のロケーションを条件とするガウス分布として表される。
【００３３】
向きにわたる分布ｐ（θ｜ｓ０：ｔ，ｚ０：ｔ）は、各粒子のロケーションｓ０：ｔが粒子フィルタリングによって求められた後、拡張カルマンフィルタリング（ＥＫＦ：ｅｘｔｅｎｄｅｎ  Ｋａｌｍａｎ  ｆｉｌｔｅｒｉｎｇ）を用いて解析的に求めることができるので、式（１）は、効率的に解くことができる。
【００３４】
より具体的には、まず、動作モデルを用いて各粒子のロケーションを更新し、マップ１２７および向きにわたる粒子の前のガウス分布に基づいて尤度を計算することによって粒子の重みを求める。次に、ＥＫＦを用いて粒子の向きのガウス分布を更新する。
【００３５】
このプロセスは、終了条件が満たされるまで繰り返すことができる。可能な終了条件には、所定のプロービング回数に達すること、または粒子ロケーションの収束が含まれる。
【００３６】
動作選択のためのエントロピー推定
マップおよびロケーションの双方が経時的に推定されるＳＬＡＭとは異なり、この発明の発明者らのプロービングベースのレジストレーションでは、マップ１２７が既知であり、物体とプローブとの間の相対的な姿勢１５０のみが未知である。
【００３７】
この発明の発明者らの出願である米国出願第１２／７５１３６６号により詳細に記載されているこの発明の発明者らの前のＲＢＰＦレジストレーションフレームワークでは、ランダムな次の動作を用いて次の各接触点を得ていた。この出願では、不確実性を最小にすることに基づいて最適な次の動作を選択する。これは、エントロピーを最小にすることによって達成される。
【００３８】
図２に示すように、物体１２０に対するプローブ１１０の現在のロケーションは、粒子のセット２０１によって表される。プローブの向きは、粒子のロケーションを条件とする、各粒子に関連付けられたガウス分布によって表される。この発明の発明者らの目標は、式（１）におけるこの発明の発明者らのＲＢＰＦフィルタリング分布の期待エントロピーを最小にする次の動作を選ぶことである。この期待エントロピーを最小にすることは、物体とプローブとの間の相対的なロケーションおよび相対的な向きの双方にわたる不確実性を最小にすることとして見ることができる。それを行うために、Ｍ個の候補動作のセット２２２を選び、各候補動作を用いて、粒子の現在のセット２０１を更新し（２１０）、更新された粒子のＭ個のセット２１５を取得する。次に、更新された粒子のこれらのセットそれぞれの期待エントロピー２２１を推定し（２２０）、最小エントロピー２２１になる動作を選択する（２３０）。
【００３９】
１つの実施の形態では、単一の多変量ガウス分布を用いて粒子分布を近似することによってエントロピーを推定する。このガウス近似手法を他のエントロピー推定方法と対比する。この発明の発明者らの実施の形態のうちの１つは、粒子重み（ｐａｒｔｉｃｌｅ  ｗｅｉｇｈｔ：粒子量）のみに基づき、他の実施の形態は、パルツェン窓（Ｐａｒｚｅｎ  ｗｉｎｄｏｗ）に基づくノンパラメトリックエントロピー推定値を用いる。
【００４０】
ラオブラックウェル化粒子フィルターのエントロピー
ラオブラックウェル化粒子フィルター（ＲＢＰＦ）の事後確率分布（式（１））のエントロピーは、
【００４１】
【数３】
0005812940.tif 000004
【００４２】
と表すことができる。ここで、
【００４３】
【数４】
0005812940.tif 000005
【００４４】
は、確率変数ｓ０：ｔにわたる期待値を示す。式（２）の右側の第１項は、粒子を用いて表されるロケーション分布のエントロピーを表し、以下で説明する第２項は、角度分布のエントロピーを表す。
【００４５】
第１項を求めるために、重み付きサンプルの集合体に基づいて連続分布のエントロピーを推定するが、これは簡単ではない。このエントロピーを求めるさまざまな代替形態を以下で説明する。
【００４６】
角度分布のエントロピー
式（２）の右側の第２項は、次のように求めることができる。ロケーションｓ０：ｔにわたる分布は、粒子によって表されるので、期待値は、
【００４７】
【数５】
0005812940.tif 000006
【００４８】
として近似される。ここで、ｓ［ｊ］０：ｔは、ｊ番目の粒子の時間ｔまでのロケーションのシーケンスであり、ｗ［ｊ］ｔは、時間ｔにおけるｊ番目の粒子の重み（Σｊｗ［ｊ］ｔ＝１となるように正規化されている）であり、Ｊは粒子数である。ｐ（θ｜ｓ［ｊ］０：ｔ，ｚ０：ｔ）はガウス分布であるので、ｐ（θ｜ｓ［ｊ］０：ｔ，ｚ０：ｔ）＝Ｎ（θ；μ，Σ）と書くことができ、これは、平均μおよび共分散行列Σを有する、θの多変量ガウス確率密度関数（ｐｄｆ：ｐｒｏｂａｂｉｌｉｔｙ  ｄｅｎｓｉｔｙ  ｆｕｎｃｔｉｏｎ）を表す。
【００４９】
ガウス分布のエントロピーは、ｄ次元ガウス分布のエントロピーの公式
【００５０】
【数６】
0005812940.tif 000007
【００５１】
を用いて求められる。ここで、｜Σ｜は共分散行列Σの行列式を示す。
【００５２】
ロケーション分布のエントロピー
重み付き粒子の集合体によって表されるロケーション分布のエントロピーを求めることは、結局、有限個の重み付きサンプルに基づいて未知の確率密度関数（ｐｄｆ）のエントロピーを求めることになる。これは、有限のサンプルが任意の密度分布を求めるのに十分ではないので、本来的に不良設定問題である。したがって、分布の関数形式またはその平滑性のいずれかについていくつかの仮定を行わなければならない。
【００５３】
式（２）の第１項であるロケーション分布のエントロピーを
【００５４】
【数７】
0005812940.tif 000008
【００５５】
として近似する。すなわち、粒子の現在のロケーションのみを考慮し、粒子の前のロケーションを考慮しない。推定したいエントロピーの分布は、
【００５６】
【数８】
0005812940.tif 000009
【００５７】
となる。
【００５８】
この表記は、変数ｓおよびｗから下付き文字ｔを省くことによって簡単化することができる。例えば、式（５）の関数をｆ（ｓ）と示し、ｓ［ｊ］およびｗ［ｊ］を用いて、時間ｔにおける粒子ｊのロケーションおよび重みをそれぞれ表す。
【００５９】
Ｊ個の粒子のロケーションおよび重みを用いて、未知のｐｄｆであるｆ（ｓ）のエントロピーを推定する。
【００６０】
ロケーション分布のエントロピーの推定
このエントロピーを推定するいくつかの方法を比較する。
【００６１】
まず、パラメトリック手法を用いてこの分布のエントロピーを近似する。パラメトリック手法では、エントロピーは、分布がガウスであると仮定することによって求められる。分布がガウスでない場合、このパラメトリック手法は、不正確なものとなる。したがって、さまざまなカーネルを有する分布のパルツェン窓ベースのカーネル密度推定値を用いてエントロピーを求める、エントロピー推定の非パラメトリック手法も考慮する。また、重みのセットを離散確率分布における確率質量のセットとして扱うことによって、粒子重みのみを用いる一方、粒子の空間ロケーションを無視してエントロピーを推定することも考慮する。
【００６２】
粒子分布のガウス近似
時間ｔにおける粒子分布にガウス関数を当てはめるために、重み付き平均ｓ（バー）および重み付き共分散行列Σを求める。
【００６３】
【数９】
0005812940.tif 000010
【００６４】
ここで、ｗ［ｊ］ｔは、時間ｔにおける粒子ｊの重み（Σｊｗ［ｊ］ｔ＝１である）であり、ｓ［ｊ］は、物体座標系における粒子のロケーションである。このように、ｆ（ｓ）を多変量  ガウスｆ（ハット）（ｓ）＝Ｎ（ｓ；ｓ（バー），Σ）として近似している。そのエントロピーは、式（４）を用いて求めることができる。
【００６５】
粒子分布のカーネル密度推定
ガウスエントロピー近似では、粒子は、特定の関数形式（ガウス）を有する分布からの重み付きサンプルであると仮定し、それらのサンプルを用いて、関数のパラメーターを推定した。サンプルの集合体から分布を推定する代替的な手法は、カーネル密度推定等の非パラメトリック方法を用いることである。ここでは、分布のパルツェン窓ベースの近似を用いて、分布のエントロピーを推定する。ロケーションサンプルの集合体および対応する非負の重みから、重み付きパルツェン密度推定値を用いて未知の確率密度ｆ（ｓ）を近似することができる。
【００６６】
従来のパルツェン窓は、通常、非重み付きサンプルまたは等しい重みを有するサンプルを指す。これとは対照的に、この発明の発明者らは、パルツェン密度推定の重み付きのもの
【００６７】
【数１０】
0005812940.tif 000011
【００６８】
を用いる。ここで、ｋ（）はカーネル関数である。ｆ（ハット）（ｓ）のエントロピー
【００６９】
【数１１】
0005812940.tif 000012
【００７０】
によってｆ（ｓ）のエントロピーを推定する。ここで、Ｅｐ（ｘ）［ｇ（ｘ）］は、確率変数ｘがｐｄｆ  ｐ（ｘ）に従って分布しているときの関数ｇ（ｘ）の期待値を表す。
【００７１】
不都合なことに、単純なガウスカーネルを含めて、カーネル関数ｋ（）の多種多様な選択肢のために、式（８）における積分を解析的に求めることは、困難であるかまたは不可能である。しかしながら、確率分布ｆ（ハット）（ｓ）にわたる期待値を、重み付きサンプルｓ［ｊ］にわたる期待値として近似することができる。この発明の発明者らのエントロピー推定値は、それゆえ、
【００７２】
【数１２】
0005812940.tif 000013
【００７３】
に簡単化される。
【００７４】
さまざまなカーネル関数を用いることができる。形式
【００７５】
【数１３】
0005812940.tif 000014
【００７６】
のゼロ平均放射対称性カーネル関数族を考える。この形式において、パラメーターσはカーネル幅を制御し、指数ｎはカーネル関数がどのような重い裾を有するのかを制御する。ｎ＝２である場合、式（１０）におけるカーネル関数は、各次元においてσ２の周辺分散（ｍａｒｇｉｎａｌ  ｖａｒｉａｎｃｅ）を有する球対称ガウスカーネルである。式（１０）におけるカーネルの比例定数を無視することができる。これは、その定数が、ｎおよびσの値のどの特定の選択肢についても不変であるからであり、式（９）におけるカーネル関数ｋに定数因数を乗算することが、定数（定数因数の負の対数）をエントロピー推定値に単に加算することであるからである。
【００７７】
粒子重みのみを用いたエントロピーの推定
粒子重みのみを用いるエントロピー推定は、粒子ロケーションを完全に無視する。この推定は、重みのセット全体を、離散確率分布における確率質量のセットとして扱い、この離散分布のエントロピー
【００７８】
【数１４】
0005812940.tif 000015
【００７９】
を求める。
【００８０】
動作選択方法
上述したＲＢＰＦのエントロピー尺度を用いて最適な次の動作を選択する方法を説明する。本方法は、ランダムな動作選択と比較すると、より少ない数の測定値を用いて同じ正確度を達成することを可能にするか、または代替的に、同じ数の測定値を用いてより高い正確度を達成することを可能にする。
【００８１】
この発明の発明者らのエントロピーベースの動作選択方法の概要を説明し、次に、本方法を２つの異なる用途、すなわち、自動レジストレーションおよび人間がガイドするレジストレーションに適用する方法を説明する。
【００８２】
方法の概要
時間ｔにおける粒子のセット２０１  Ｘｔが与えられると、Ｍ個の候補動作２２２  Γｍ（ｍ＝｛１，．．．，Ｍ｝）から最適な次の動作２０２を選択する（２３０）欲張り手法を用いる。各候補動作２２２の期待エントロピー２２１を求め（２２０）、次の動作について最小期待エントロピーを有する動作を選択する（２３０）。エントロピーの計算を効率的にするために、粒子セットＸｔをそのサブセットＹｔ（最も大きな重みを有するＫ個の粒子）によって近似する。ここで、Ｋ≦Ｊである。
【００８３】
各動作候補Γｍは、ランダムに選ばれる。各動作候補Γｍについて、Ｙｔから粒子の重みに比例する確率を有するランダムな粒子ｑ［ｃ］を選択するごとに、測定プロセスをＣ回シミュレートする。各ｑ［ｃ］について、次の質問、すなわち「ｑ［ｃ］が物体とロボットとの間の相対的な姿勢について正しい場合において、Γｍによって指示される動作を実行するとき、どのような接触位置測定値が結果として得られるのか？」に対する答えを計算する。この答えは、推定された動作ｕ（ハット）ｔ［ｃ］を与える。この推定された動作は、粒子セットＹｔを更新するのに用いられる。この結果、更新された粒子セットＹ［ｃ］ｔ＋１が得られ、そのエントロピーが計算される。このシミュレーション手順をＣ回繰り返し、動作候補Γｍの期待エントロピーをＣ回のシミュレーションにわたる平均として計算する。
【００８４】
この発明の発明者らの用途では、ロボットを測定位置間で移動させるのに必要な時間は、単一の接触ロケーションを精密に測定するのに必要な時間よりもはるかに短いので、プローブを次の位置に移動させるコストは、位置とは無関係であると仮定する。異なる動作候補が異なるコストを有する用途では、例えば、より遠くの位置に移動することがより多くの時間を要する場合、その移動をエントロピーに加えてコスト関数に組み込むことができる。
【００８５】
自動レジストレーションへの適用
自動レジストレーションの場合、米国出願第１２／７５１３６６号における方法は、ロボット座標系に対するｘ方向およびｙ方向にランダムな動作を適用し、次に、接触するまで、物体に向かって、ロボット座標系のｚ軸に平行にプローブを移動させることによってｚ位置を測定する。
【００８６】
このフレームワークにおいてこの発明の発明者らのエントロピーベースの動作選択方法を適用するために、各動作候補Γｍは、ロボット座標系におけるｘ方向およびｙ方向におけるランダムな変位を示し、ｚ位置の測定が各粒子ｑ［ｃ］についてシミュレートされる。粒子ｑ［ｃ］は、ロボットと物体との間の相対的な姿勢の推定値を保持し、この推定値は、３Ｄ位置ｓｔ［ｃ］と、平均μθ［ｃ］を有する、角度にわたるガウス分布とを含む。粒子の位置ｓｔ［ｃ］および平均角度μθ［ｃ］を、物体のモデルと併せて用い、プローブが物体に接触することになるロケーションを推定する。ｚ方向における推定された動作は、Γｍによって与えられた（ｘ，ｙ）変位と組み合わされて、期待動作ｕ（ハット）ｔ［ｃ］を構成する。
【００８７】
自動レジストレーション中、この発明の発明者らのシステムおよび方法は、物体に向かってロボットを移動させながら、プローブに印加される力／トルクの値を連続的に測定する。この動作は、測定が所定のしきい値を超えると終了する。この発明の発明者らのシステムは、まず、大きな動作ステップサイズを用いて接触ロケーションを粗く検出し、次に、小さな動作ステップサイズを用いて接触ロケーションを精緻化して、精密なロケーション測定を達成する。
【００８８】
人間がガイドするレジストレーションへの適用
いくつかの場合には、上述した自動レジストレーションプロセスは、実用的でない場合がある。例えば、ロボットの可能な動作を制限する他の物体が近くに存在する場合がある。別の例は、物体が、単一の方向からプロービングすることができる十分に有用な特徴を有していない場合に、複数の方向から物体をプロービングする場合があることである。そのような場合、人間のオペレーターにロボットを移動させて接触位置を測定することが、多くの場合、より安全であり、より実用的である。
【００８９】
視覚化システムは、物体上の最適なタッチロケーションを人間のオペレーターに提案して、高速かつ正確なレジストレーションを達成することができる。
【００９０】
この発明の発明者らのシステムは、人間のオペレーターによって始動される。オペレーターは、プローブを物体表面上の任意の位置に移動させ、物体表面上の近似の位置をシステムに指定する。システムは、その粒子の全てを、オペレーターによって示されたロケーションの近くに初期化し、角度分布が、プローブと表面とのあらゆる可能な初期接触角度を含む範囲から選択される。この人間がガイドする場合には、各候補動作Γｍは、物体座標における物体表面上のランダムに選ばれた点である。各粒子ｑ［ｃ］について、物体表面上の選択された点に粒子を移動させるのに必要な動作を計算することによって期待動作ｕ（ハット）ｔ［ｃ］を求める。この期待動作は、その後、粒子Ｙｔを更新するのに用いられる。
【００９１】
本方法は、期待エントロピーを最小にする動作候補Γｍを選択し、物体上のそのロケーションで次にプロービングを行うようにオペレーターに指示する。また、この発明の発明者らのシステムは、現在の力／トルク測定値をオペレーターに視覚化し、オペレーターが、精密な接触ロケーションを求めるようロボットの動作を制御できるようにする。
【００９２】
オペレーターは、提案された位置にプローブを精密に移動させる必要はないことに留意されたい。ＲＢＰＦフレームワークは、次の接触が物体の表面上のいずれかの場所である限り、適切に機能する。一方、提案された位置は、動作候補の全ての最小エントロピーを達成するので、本方法の提案したロケーションを用いることによって、より少ない測定で正確なレジストレーションを行うことが可能になる。
【００９３】
発明の効果
その３Ｄモデルが既知である物体へのロボットのプロービングベースのレジストレーションにおけるオンラインの動作選択のための方法およびシステムを説明した。この発明の発明者らのＲＢＰＦベースの方法は、各動作をランダムに選ぶ従来技術の方法とは異なり、いくつかの候補動作を選び、次に、期待エントロピーの形態の期待不確実性の推定値を用いて、最良の候補動作を選択する。この発明の発明者らは、この発明の発明者らの方法を完全自動レジストレーションだけでなく、人間がガイドするレジストレーションにも適用する。この発明の発明者らのエントロピーベースの動作選択方法は、性能がランダムな動作選択よりも大幅に優れている。
【図１】
0005812940.tif 000026
【図２】
0005812940.tif 000027