"相対的に移動する対象物をロボットで把持する移動体把持装置であって、対象物の位置又は姿勢を計測する計測装置と、対象物を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ精度指標値から対象物の把持の可否を判定する状態推定装置と、把持可能の場合、内部状態の推定結果に基づいて対象物の移動先を予測して把持動作するようにロボットを制御するロボット制御装置と、を備えることを特徴とする移動体把持装置。相対的に移動する対象物をロボットで把持する移動体把持方法であって、A計測装置により、対象物の位置又は姿勢を計測し、B計測した計測結果から、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつC精度指標値から対象物の把持の可否を判定し、D把持可能の場合、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、E把持不可の場合、前記A~Dを繰り返す、ことを特徴とする移動体把持方法。前記精度指標値は、対象物の位置と姿勢を予測した予測値とその実測値との誤差と、状態遷移モデルと観測モデルに基づいて前記誤差の分散を予想した共分散行列とからなる、ことを特徴とする請求項2に記載の移動体把持方法。前記Cにおいて、前記誤差を前記共分散行列で割ったマハラノビス距離の自乗が第1閾値より小さく、かつ、共分散行列の行列式が第2閾値より小さい時に、把持可能と判定する、ことを特徴とする請求項3に記載の移動体把持方法。前記Cにおいて、前記誤差を前記共分散行列で割ったマハラノビス距離の自乗が第3閾値より小さく、かつ、共分散行列のトレースが第4閾値より小さい時に、把持可能と判定する、ことを特徴とする請求項3に記載の移動体把持方法。上述した特許文献2によれば、対象物例えばワークを計測した観測履歴から、対象物の運動状態を推定することで、計測値が得られていない間のデータを補完したり、対象物をロボットで把持する際の把持地点を予測したりすることができる。運動状態の推定には、カルマンフィルタなどが一般的に用いられる。特許文献2、3の状態推定手段では、対象物の速度、加速度などを内部状態とし、内部状態が時間と共にどのように変化するかを表す状態遷移モデルを事前に定義する。特許文献1、2のように移動先を予測して把持する装置では、予測の精度が低いと、対象物が予測した位置から外れ、把持に失敗することがある。特許文献1では、把持動作に移行する前に、アームと対象物の偏差が一定になっている状態完全追従状態、不完全追従状態であることを判定する。この判定によって、アームと対象物の運動状態が高い精度で一致している時のみ把持動作をするため、把持精度が向上する。しかし、この判定条件は、等速直線運動、等速円運動、等加速度直線運動には適用できるが、それ以外の運動に対して適用できない。対象物の予測位置の精度が低くなるのは、主に以下のような状況の時である。1計測開始直後。2対象物を計測できない時間がしばらく続いた時。3対象物が、事前に定めた状態遷移モデルと違った動きをしている時。本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物例えばワークの内部状態例えば位置、姿勢、速度、角速度に基づいて対象物の運動予測をすることができ、予測精度の低下の影響を受けることなく、ロボットを対象物に追従させて確実に把持することができる移動体把持装置と方法を提供することにある。本発明によれば、相対的に移動する対象物をロボットで把持する移動体把持装置であって、対象物の位置又は姿勢を計測する計測装置と、対象物を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ精度指標値から対象物の把持の可否を判定する状態推定装置と、把持可能の場合、内部状態の推定結果に基づいて対象物の移動先を予測して把持動作するようにロボットを制御するロボット制御装置と、を備えることを特徴とする移動体把持装置が提供される。また本発明によれば、相対的に移動する対象物をロボットで把持する移動体把持方法であって、A計測装置により、対象物の位置又は姿勢を計測し、B計測した計測結果から、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつC精度指標値から対象物の把持の可否を判定し、D把持可能の場合、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、E把持不可の場合、前記A~Dを繰り返す、ことを特徴とする移動体把持方法が提供される。上記本発明の装置と方法によれば、計測装置及び状態推定装置により、対象物の位置又は姿勢を計測し、対象物の内部状態を推定するので、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物例えばワークの内部状態例えば位置、姿勢、速度、角速度に基づいて対象物の運動予測をすることができる。また、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、精度指標値から対象物の把持の可否を判定するので、把持不可の場合、ロボット制御装置により、予測精度が低下する場合でも把持の失敗を未然に回避することができる。さらに、把持可能の場合に、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作するので、ロボットを対象物に追従させて確実に把持することができる。以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。図1は、本発明による移動体把持装置を有するロボットシステムの実施形態図である。この図において、1は対象物ワーク、2はロボット、3はロボットアーム、4はハンド、5aはハンド4に固定された第1カメラ、5bは外部の定位置に固定された第2カメラ、10は移動体把持装置である。このロボットシステムは、振り子運動しながら移動するワーク1をカメラ5a,5bで計測し、ロボット2をワーク1に追従させて制御し、ハンド4によりワーク1を把持するようになっている。図2は、本発明による移動体把持装置の実施形態図である。この図において、移動体把持装置10は、計測装置12、状態推定装置14、及びロボット制御装置20を備える。状態推定装置14は、データ記憶装置16を備える。計測装置12は、対象物1の位置と姿勢を計測する。計測装置12は、この例では第1カメラ5aと第2カメラ5bにそれぞれ接続された2台の計測装置第1計測装置12aと第2計測装置12bからなる。すなわち、この例では、カメラと計測装置をそれぞれ2台用い、第1カメラ5aと第2カメラ5bから対象物1の画像を取り込み、画像処理によって、対象物1の位置と姿勢を求める。求められた対象物1の位置と姿勢は、データ記憶装置16に記憶される。なお、計測装置12は、この構成に限定されず、対象物1の位置を1台で3次元計測することができる計測装置、例えばレーザレーダを用いてもよい。状態推定装置14は、対象物1を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値E後述するを含む対象物1の内部状態を推定し、かつ精度指標値Eから対象物1の把持の可否を判定する機能を有する。状態推定装置14には、この例ではカルマンフィルタのアルゴリズムを基にした状態推定アルゴリズムが実装されている。この状態推定アルゴリズムでは、状態遷移モデルとして、以下の4つを定義する。時刻t=0の時の内部状態:X0初期条件X0の共分散:CovX0状態遷移方程式:XtΔt=fXt,Δt1単位時間当たりのプロセスノイズ共分散:Qこのアルゴリズムでは、対象物1が吊るされている振り子の振れ角θや、角速度Δθ、支点位置x,y,zなどの状態変数をまとめて、内部状態Xとする。モデルを定義する際、初期条件が必ずしも正確にわかるわけではない。したがって、初期条件と実物の差がどの程度大きくなるかを予想した共分散CovX0を定義する。状態遷移方程式1は、時刻が進むと内部状態Xがどのように変化するかを示す。この例の場合、振り子の運動方程式や、支点位置が等速直線運動することなどから、状態遷移方程式を定義する。定義した状態遷移方程式1は、必ずしも実際の対象物1の運動と一致しない。そこで、単位時間Δt=1の時間幅だけ状態遷移計算をした時に、実際の運動とどの程度差が生まれるかを示す共分散Qを定義する。また、カルマンフィルタは、計測装置12で算出される対象物1の位置と姿勢を計測値Yとし、式23の観測モデル観測方程式と観測ノイズを定義する。観測方程式:Yt=gXt2観測ノイズ共分散:CovSt3観測方程式2は、内部状態Xと計測値Yを対応づける式である。また、観測ノイズ3は、時刻tの時の計測にどの程度の計測誤差が含まれるかを表す共分散であり、カメラの分解能や視点の向きによって決まる。観測ノイズ3は固定値ではなく、画像ごとに計測装置12が算出し、計測値と共に状態推定装置14に渡される。ロボット制御装置20は、把持可能の場合、内部状態の推定結果に基づいて対象物1の移動先を予測して把持動作するようにロボットを制御する機能を有する。ロボット制御装置20は、ロボット2から手先位置情報などを受信し、手先速度指令値やハンド開閉指令値を送信する。これらの送受信を、一定の制御周期4ms周期で行う。以下、図2に基づき本発明の移動体把持方法を説明する。本発明の方法では、A初めに、計測装置12により、対象物1の位置と姿勢を計測する。B次いで、計測した計測結果から、状態推定装置14により、状態遷移モデルに基づいて、精度指標値Eを含む対象物1の内部状態を推定し、かつC精度指標値Eから対象物1の把持の可否を判定する。D次に、把持可能の場合、ロボット制御装置20により、内部状態の推定結果に基づいて対象物1の移動先を予測してロボット2を把持動作し、E把持不可の場合、ロボット制御装置20により、対象物1の位置と姿勢の計測を継続できるようにロボット2を移動させる。精度指標値Eは、対象物1の位置と姿勢を予測した予測値とその実測値との誤差と、状態遷移モデルと観測モデルに基づいて前記誤差の分散を予想した共分散行列とからなる。前記Cにおいて、前記誤差を前記共分散行列で割ったマハラノビス距離Mtの自乗が第1閾値より小さく、かつ、共分散行列の行列式が第2閾値より小さい時に、把持可能と判定する、ことが好ましい。第1閾値と第2閾値は、予め設定した任意の閾値である。また前記Cにおいて、前記誤差を前記共分散行列で割ったマハラノビス距離Mtの自乗が第3閾値より小さく、かつ、共分散行列のトレースが第4閾値より小さい時に、把持可能と判定してもよい。第3閾値と第4閾値は、予め設定した任意の閾値である。以下、発明の移動体把持装置の動作を説明する。a1計測装置12第1計測装置12aと第2計測装置12bは任意のタイミングで2台のカメラ第1カメラ5aと第2カメラ5bにそれぞれシャッター信号を送信し、撮像された画像を取得する。a2計測装置12により、得られた画像から画像処理によって対象物1の位置と姿勢の情報計測値Yを取得する。例えば、画像内の白色領域を抽出し、重心を求める。a3計測装置12からシャッター信号を送信した時刻をtyとする。また、カメラ5a,5bの向きや分解能などから、観測ノイズCovSを算出する。a4状態推定装置14は、計測値Yと、状態遷移モデルと観測モデルによって予測したYの予測値Ymを比較して、内部状態Xを修正する。また、対象物1の位置を予測する精度を表す精度指標値Eを算出する。a5ロボット制御装置20は、制御周期ごとに状態推定装置14の結果を参照し、精度指標値Eを評価して、把持動作を開始するかどうかの判定を行う。a6把持動作を開始する場合は、対象物1の未来の位置を予測し、予測した把持位置を目指して手先速度指令値を算出する。把持動作しない場合は、カメラ5a,5bが対象物1を捕捉し続けられるように、手先速度指令値を算出する。上記a4における状態推定装置14の処理内容の詳細を以下に説明する。以下の処理内容は、カルマンフィルタを利用した一般的な状態推定手段である。b1計測時刻tyと現在のモデル時刻tから、状態遷移させる時間幅Δtを算出する。モデル時刻tの初期値は、計測を開始した時刻とする。また、計測時刻を、開始時刻を基点とした経過時間として定義し、モデル時刻の初期値を0としても良い。状態遷移させる時間幅Δtは、Δt=ty-t4で表される。b2計測時刻tyにおける内部状態を予測する。内部状態の予測値XtΔtは、上述した状態遷移モデルで定義した、XtΔt=fXt,Δt1で表される。b3計測時刻における、内部状態予測値の共分散を算出する。ここで、行列Aは状態遷移方程式fを偏微分した行列である。更新b9前の内部状態の共分散CovXtΔt|tは、式5で表される。CovXtΔt|t=AtΔtCovXtAtΔtTQ|Δt|5状態遷移方程式の偏微分行列AtΔtは、式6で表される。AtΔt=∂fXt、Δt/∂Xt6b4内部状態予測値と観測方程式を用いて、式7で計測値Yの予測値YmtΔtを予測する。YmtΔt=gXtΔt7b5内部状態予測値の共分散に基づいて、予測値Ymの共分散CovYtΔtを式8で求める。ここで、行列Hは観測方程式gを偏微分した行列である。CovYtΔt=HtCovXtΔt|tHtT8観測方程式の偏微分行列Htは、式9で表される。Ht=∂gXt/∂Xt9b6予測値Ymの共分散CovYに加えて、カメラの計測誤差を考慮した共分散VtΔtを、式10で求める。CovStΔtは時刻ty=tΔtの時の観測ノイズであり、計測装置12が算出する。共分散VtΔtは、予測値Ymの共分散に、カメラの観測ノイズを加えているため、予測値Ymと観測値Yの差の大きさを示す共分散となる。VtΔt=CovYtΔtCovStΔt10b7カルマンゲインKtΔtを式11で算出する。KtΔt=CovXtΔt|tHtT/VtΔt11b8内部状態XtΔtを式1213で更新する。なお、更新前の内部状態をXtΔt|tと表記する。なおYはセンサで計測された観測値である。eYtΔt=YmtΔt|t-YtΔt12XtΔt=XtΔt|t-KtΔteYtΔt13b9更新後の内部状態の共分散CovXtΔtを式14で算出する。CovXtΔt=I-KtΔtHtCovXtΔt|t14上述したb1からb9までの処理によって、時刻tyの観測値に基づいて、内部状態が修正される。したがって、システム全体が上記a1~a6の処理を繰り返すことで、内部状態が徐々に真の値対象物の実速度などに近づく。内部状態がどの程度、真の値に近づいているか、を判断するには、上記b5、b6で求めた予測値の共分散CovY、Vを評価する。これらの共分散は、現在の推定結果を使って対象物の位置と姿勢を予測した場合の、予測誤差の大きさを表す。事前に定義した状態遷移モデルと観測モデルが正確な場合、観測値と予測値の差eYは、共分散Vにしたがった分布となる。したがって、実際の差eYが、Vに比べて大きな値となった場合は、事前に定めたモデルが正しくないことを意味する。このような評価には、式15に示すマハラノビス距離Mtを用いる。Mt=eYtTVt-1eYt0.515そこで状態推定装置14では、b1からb9の処理後に、現在の状態推定の精度を表す精度指標値Eを、以下のように算出する。b10以下のc1c2c3のいずれかにより精度指標値Eを算出する。c1E=exp-MtΔt2/2/2πm|VtΔt|0.516ここでmは、eYの次数である。c2E=exp-MtΔt2/2/traceCovYtΔt0.517c3式1617のEを、過去の計測結果全てについて総和した式18。ここでpは、内部状態の変数の数である。なお、c1の式16は、正規分布のモデル適合度の評価式であり、c3の式18は、情報量基準AICである。c2の式17は、分母に共分散CovYのトレースを用いる点に特徴がある。式16において共分散の行列式の平方根は、eYの分布の体積を表す。一方、式17においてトレースの平方根は、分布を包み込む最小の球の半径を表す。次に上記a5a6におけるロボット制御装置20の処理内容を以下に示す。図3は、本発明による移動体把持方法の全体フロー図である。この図に示すように、移動体把持方法は、S1~S12の各ステップ工程からなる。S1では、ロボット2から手先位置情報などを受信する。ロボット2との通信は、例えば4msの制御周期で行なわれるようにロボット2が管理する。したがって、ロボット制御装置20は、データを受信するまで待機し、データを受信してから例えば4msの制御周期以内でS12の送信処理を完了しなければならない。S2では、把持動作中を示すフラグFを判定する。フラグFはプログラム開始時にfalseに初期化されている。S3では、把持動作中でない場合、上記a1からa4の処理を行う。この処理は、別の処理系が任意の実行周期で実施し、ロボット制御装置20は最新の推定結果と精度指標値Eを参照する形式でも良い。S4では、現在の状態推定の精度指標値Eから、把持動作を行うか、追従動作を継続するかを判定する。判定手段は以下のd1~d3のいずれかを用いる。d1上述したc1の精度指標値Eがある閾値第1閾値を超えるかどうかを判定する。式16では、マハラノビス距離Mtが大きい場合、指数関数部が0に近づく。また、共分散Vの体積が大きい場合、分母が大きくなる。したがって、マハラノビス距離Mtが小さく、かつ、共分散Vの体積が小さい時のみ、把持動作が開始される。この例のように2台のカメラ第1カメラ5aと第2カメラ5bを用いる場合、対象物1が片方のカメラの視野から外れ、1台のカメラのみで計測を続ける状況が起こりうる。この時、片方の視点でしか計測されないため、共分散CovY、Vはカメラの視線方向に細長い分布を示す共分散行列となる。d1の手段の場合、Vが細長い場合でも体積は小さくなるので、把持動作が開始される可能性がある。d2上述したc2の精度指標値Eがある閾値第3閾値を超えるかどうかを判定する。式17の精度指標値Eは、CovYが細長い分布を表す場合、小さな値となる。したがって、2台のカメラが両方とも対象物1を捉えている時のみ、把持動作が開始される。d3上述したd1d2は、共に、最新の計測結果で判定するが、過去数点分の精度指標値を考慮して判定しても良い。例えば、上述したc3のように、過去に算出された精度指標値の対数をとって、総和する方法や、過去一定時間分の平均値をとる方法などが挙げられる。S5では、把持動作をしない場合、把持動作中フラグFはfalseとする。S6では、把持動作をしない場合、例えば以下の式192021ようにアーム速度Vx、Vy、Vzを設定する。Vx=Kpxmx-rxKdxmx-rx-pr_x19Vy=Kpymy-ryKdymy-ry-pr_y20Vz=Kpzmz-rzKdzmz-rz-pr_z21この制御は、PD制御であり、mx、my、mzは、計測された対象物の現在位置[mm]、rx、ry、rzは追従制御点カメラ1の視線上の点の現在位置[mm]、pr_x、pr_y、pr_zは、前回のステップで算出された位置偏差my-ry、mz-rzなど、Kpは位置制御ゲイン、Kdは微分制御ゲインである。S7では、S4で把持動作を行うと判定された場合は、状態遷移モデルと観測モデルを用いて、未来の対象物1の位置と姿勢を予測し、把持位置Aとする。S8では、把持動作開始時に、把持動作中フラグFをtrueにする。S9では、把持動作中は、ロボットアーム3が把持位置Aに移動するように、ロボットアーム3の目標速度を算出する。S10では、把持動作が終了したかどうかを判定する。例えば、ロボットアーム3が把持位置Aに到達して、ハンド4を閉じる動作が終了している場合に把持動作終了と判定する。S11では、把持動作を終了する場合、把持動作フラグFをfalseにする。把持動作を終了しない場合は、把持動作フラグFはtrueのままなので、S2の分岐により、次回以降も把持動作が継続される。S12では、算出された目標速度やハンド開閉指令値をロボット2に送信する。なお上述の例では、対象物1の運動状態を推定する方法として、カルマンフィルタを用いたが、以下のような条件を満たす状態推定手法であれば、他の手法を用いても良い。例えば、パーティクルフィルタ、最小自乗法などを適用できる。e1対象物1の運動状態を推定して、未来の位置を予測できること。e2未来の位置を予測した時の予測誤差の分散を表す共分散行列を算出できること。上述のc1では、Vの行列式を使った判定の例を示しているが、V,CovYのどちらの行列式を使っても構わない。また、上述のc2では、CovYのtraceを使った判定の例を示しているが、V,CovYのどちらのtraceを使っても構わない。すなわち誤差の分散を予想した共分散行列が、V,CovYのどちらもあり得るようになっていればよい。図4は、実施例における経過時間と精度指標値Eとの関係を示す図である。この例は、上述したc2の式17で、精度指標値Eを算出した結果である。この図から計測開始時から精度指標値Eは徐々に大きくなり、計測開始から経過時間が2秒後以降は80以上で安定することがわかる。したがって、式17の精度指標値Eが80以上であることを把持動作開始条件とすれば、把持失敗しにくくなるといえる。また、この図で経過時間が1秒付近に見られるように、一時的に精度指標値が高くなる場合がある。したがって、上述したc3のように、過去数点分の精度指標値を考慮して判定することが好ましい。例えば、過去数点の平均値が80を超えている、一定時間以上連続して80を超えているなどを考慮する。例えば、120ms以上連続して、精度指標値が80を超えていることを把持動作開始条件とするのがよい。上述した本発明の装置と方法によれば、計測装置12及び状態推定装置14により、対象物1の位置又は姿勢を計測し、対象物1の内部状態を推定するので、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物1例えばワークの内部状態例えば位置、姿勢、速度、角速度に基づいて対象物1の運動予測をすることができる。また、状態推定装置14により、状態遷移モデルと観測モデルに基づいて、精度指標値Eを含む対象物1の内部状態を推定し、精度指標値Eから対象物1の把持の可否を判定するので、把持不可の場合、ロボット制御装置20により、例えば対象物1の位置と姿勢の計測を継続できるようにロボット2を移動させることで、予測精度が低下する場合でも把持の失敗を未然に回避することができる。さらに、把持可能の場合に、ロボット制御装置20により、内部状態の推定結果に基づいて対象物1の移動先を予測してロボット2を把持動作するので、ロボット2を対象物1に追従させて確実に把持することができる。上述した例では、対象物1が移動しロボット2が固定されているが、本発明はこの例に限定されず、対象物が動かずにロボットが移動する場合でも、対象物とロボットの両方が移動する場合でもよい。したがって、本発明における対象物の移動は、ロボットから見た対象物の相対的な移動を意味する。なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。"