(57)【特許請求の範囲】
【請求項１】
相対的に移動する対象物をロボットで把持する移動体把持装置であって、
対象物の位置又は姿勢を計測する計測装置と、
対象物を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、状態推定を行って対象物の内部状態の推定値を取得し、取得した前記推定値を用いて対象物の位置と姿勢を予測した予測値とその実測値との誤差と、前記誤差の分散を予想した共分散行列とを用いて計算される精度指標値を算出し、かつ前記精度指標値に基づいて対象物の把持の可否を判定する状態推定装置と、
把持可能の場合、内部状態の推定結果に基づいて対象物の移動先を予測して把持動作するようにロボットを制御するロボット制御装置と、を備えることを特徴とする移動体把持装置。
【請求項２】
前記状態推定に、カルマンフィルタを用いる、ことを特徴とする請求項１に記載の移動体把持装置。
【請求項３】
相対的に移動する対象物をロボットで把持する移動体把持方法であって、
（Ａ）計測装置により、対象物の位置又は姿勢を計測し、
（Ｂ）計測した計測結果から、状態推定装置により、状態遷移モデルと観測モデルに基づいて、状態推定を行って対象物の内部状態の推定値を取得し、取得した前記推定値を用いて対象物の位置と姿勢を予測した予測値とその実測値との誤差と、前記誤差の分散を予想した共分散行列とを用いて計算される精度指標値を算出し、かつ
（Ｃ）前記精度指標値に基づいて対象物の把持の可否を判定し、
（Ｄ）把持可能の場合、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、
（Ｅ）把持不可の場合、前記（Ａ）〜（Ｃ）を繰り返す、ことを特徴とする移動体把持方法。
【請求項４】
前記（Ｃ）において、前記実測値と前記予測値の差と前記予測値の共分散にセンサの観測ノイズを加えた共分散から求まるマハラノビス距離の自乗が第１閾値より小さく、かつ、共分散行列の行列式が第２閾値より小さい時に、把持可能と判定する、ことを特徴とする請求項３に記載の移動体把持方法。
【請求項５】
前記（Ｃ）において、前記実測値と前記予測値の差と前記予測値の共分散にセンサの観測ノイズを加えた共分散から求まるマハラノビス距離の自乗が第３閾値より小さく、かつ、共分散行列のトレースが第４閾値より小さい時に、把持可能と判定する、ことを特徴とする請求項３に記載の移動体把持方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットなどを使った自動装置のうち、移動する対象物を把持する移動体把持装置と方法に関する。
【背景技術】
【０００２】
移動する対象物をロボットでハンドリングする自動装置では、ロボット上やロボットの外部に設置したカメラなどの視覚センサで対象物の位置を計測し、その計測結果に基づいてロボットを追従制御する必要がある。
【０００３】
上述したロボットの追従制御では、時々刻々と変化する対象物（例えばワーク）の位置を一定周期（例：３０ｆｐｓのカメラ）で計測し、計測結果に基づいて、例えば対象物にロボットを近づけるような、移動指令がロボットに出力される。
【０００４】
しかし、計測したワークの位置を目標としてロボットを動作させても、センサの計測遅れ、データの取得遅れ、ロボットの動作遅れなどの制御遅れにより、ロボットが移動するワークに追従できないことがある。
また、視覚センサの場合、センサの計測周期はロボットの制御周期（例：４ｍｓ）よりも一般に長いため、ロボットは制御周期ごとに最新の計測結果を得られるわけではなく、その分が計測遅れとなる。特に、画像処理に時間がかかったり、ワークがカメラの視野から外れたりすると、計測結果が更新される周期はさらに長くなり、かつ一定ではなくなる。このように、移動するワークを扱う装置では、制御遅れによりロボットの追従性能が低下する問題点があった。
【０００５】
上述した問題を解決するために、種々の制御手段が既に提案されている（例えば、特許文献１〜４）。
【０００６】
特許文献１の「ロボット装置及びその制御方法」では、まずアームを対象物に追従させ、追従時のアームの運動状態と、対象物の運動状態が一致していることを利用して、対象物の移動先を予測する方法が提案されている。このように、アームの動作時間を考慮して、対象物の移動先を予測することで、移動体の把持が実現される。
特許文献２の「３次元運動予測装置」は、単振動する被計測体の位置データからその運動パラメータを推定し、その将来の位置を予測し、その位置情報を基にマニュピュレータにより被計測体を把持するものである。
特許文献３の状態推定手段は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するものである。内部状態とは、対象物の位置、姿勢、振れ角などの状態変数を意味する。
特許文献４の「運動予測装置」は、バックグラウンド処理とフォアグラウンド処理を併用して自動追尾と運動予測を行うものである。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第４２６５０８８号公報、「ロボット装置及びその制御方法」
【特許文献２】特開平０７−０１９８１８号公報、「３次元運動予測装置」
【特許文献３】特許第４０７２０１７号公報、「状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置」
【特許文献４】特許第４１５３６２５号公報、「運動予測装置」
【発明の概要】
【発明が解決しようとする課題】
【０００８】
上述した特許文献２によれば、対象物（例えばワーク）を計測した観測履歴から、対象物の運動状態を推定することで、計測値が得られていない間のデータを補完したり、対象物をロボットで把持する際の把持地点を予測したりすることができる。
【０００９】
運動状態の推定には、カルマンフィルタなどが一般的に用いられる。特許文献２、３の状態推定手段では、対象物の速度、加速度などを「内部状態」とし、内部状態が時間と共にどのように変化するかを表す「状態遷移モデル」を事前に定義する。
【００１０】
特許文献１、２のように移動先を予測して把持する装置では、予測の精度が低いと、対象物が予測した位置から外れ、把持に失敗することがある。
特許文献１では、把持動作に移行する前に、アームと対象物の偏差が一定になっている状態（完全追従状態、不完全追従状態）であることを判定する。この判定によって、アームと対象物の運動状態が高い精度で一致している時のみ把持動作をするため、把持精度が向上する。しかし、この判定条件は、等速直線運動、等速円運動、等加速度直線運動には適用できるが、それ以外の運動に対して適用できない。
【００１１】
対象物の予測位置の精度が低くなるのは、主に以下のような状況の時である。
（１）計測開始直後。
（２）対象物を計測できない時間がしばらく続いた時。
（３）対象物が、事前に定めた状態遷移モデルと違った動きをしている時。
【００１２】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物（例えばワーク）の内部状態（例えば位置、姿勢、速度、角速度）に基づいて対象物の運動予測をすることができ、予測精度の低下の影響を受けることなく、ロボットを対象物に追従させて確実に把持することができる移動体把持装置と方法を提供することにある。
【課題を解決するための手段】
【００１３】
本発明によれば、相対的に移動する対象物をロボットで把持する移動体把持装置であって、
対象物の位置又は姿勢を計測する計測装置と、
対象物を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、状態推定を行って対象物の内部状態の推定値を取得し、取得した前記推定値を用いて対象物の位置と姿勢を予測した予測値とその実測値との誤差と、前記誤差の分散を予想した共分散行列とを用いて計算される精度指標値を算出し、かつ前記精度指標値に基づいて対象物の把持の可否を判定する状態推定装置と、
把持可能の場合、内部状態の推定結果に基づいて対象物の移動先を予測して把持動作するようにロボットを制御するロボット制御装置と、を備えることを特徴とする移動体把持装置が提供される。
【００１４】
また本発明によれば、相対的に移動する対象物をロボットで把持する移動体把持方法であって、
（Ａ）計測装置により、対象物の位置又は姿勢を計測し、
（Ｂ）計測した計測結果から、状態推定装置により、状態遷移モデルと観測モデルに基づいて、状態推定を行って対象物の内部状態の推定値を取得し、取得した前記推定値を用いて対象物の位置と姿勢を予測した予測値とその実測値との誤差と、前記誤差の分散を予想した共分散行列とを用いて計算される精度指標値を算出し、かつ
（Ｃ）前記精度指標値に基づいて対象物の把持の可否を判定し、
（Ｄ）把持可能の場合、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、
（Ｅ）把持不可の場合、前記（Ａ）〜（Ｃ）を繰り返す、ことを特徴とする移動体把持方法が提供される。
【発明の効果】
【００１５】
上記本発明の装置と方法によれば、計測装置及び状態推定装置により、対象物の位置又は姿勢を計測し、対象物の内部状態を推定するので、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物（例えばワーク）の内部状態（例えば位置、姿勢、速度、角速度）に基づいて対象物の運動予測をすることができる。
【００１６】
また、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、精度指標値から対象物の把持の可否を判定するので、把持不可の場合、ロボット制御装置により、予測精度が低下する場合でも把持の失敗を未然に回避することができる。
【００１７】
さらに、把持可能の場合に、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作するので、ロボットを対象物に追従させて確実に把持することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明による移動体把持装置を有するロボットシステムの実施形態図である。
【図２】本発明による移動体把持装置の実施形態図である。
【図３】本発明による移動体把持方法の全体フロー図である。
【図４】実施例における経過時間と精度指標値Ｅとの関係を示す図である。
【発明を実施するための形態】
【００１９】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２０】
図１は、本発明による移動体把持装置を有するロボットシステムの実施形態図である。
この図において、１は対象物（ワーク）、２はロボット、３はロボットアーム、４はハンド、５ａはハンド４に固定された第１カメラ、５ｂは外部の定位置に固定された第２カメラ、１０は移動体把持装置である。
このロボットシステムは、振り子運動しながら移動するワーク１をカメラ５ａ，５ｂで計測し、ロボット２をワーク１に追従させて制御し、ハンド４によりワーク１を把持するようになっている。
【００２１】
図２は、本発明による移動体把持装置の実施形態図である。
この図において、移動体把持装置１０は、計測装置１２、状態推定装置１４、及びロボット制御装置２０を備える。状態推定装置１４は、データ記憶装置１６を備える。
【００２２】
計測装置１２は、対象物１の位置と姿勢を計測する。
計測装置１２は、この例では第１カメラ５ａと第２カメラ５ｂにそれぞれ接続された２台の計測装置（第１計測装置１２ａと第２計測装置１２ｂ）からなる。
すなわち、この例では、カメラと計測装置をそれぞれ２台用い、第１カメラ５ａと第２カメラ５ｂから対象物１の画像を取り込み、画像処理によって、対象物１の位置と姿勢を求める。求められた対象物１の位置と姿勢は、データ記憶装置１６に記憶される。
なお、計測装置１２は、この構成に限定されず、対象物１の位置を１台で３次元計測することができる計測装置、例えばレーザレーダを用いてもよい。
【００２３】
状態推定装置１４は、対象物１を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値Ｅ（後述する）を含む対象物１の内部状態を推定し、かつ精度指標値Ｅから対象物１の把持の可否を判定する機能を有する。
【００２４】
状態推定装置１４には、この例ではカルマンフィルタのアルゴリズムを基にした状態推定アルゴリズムが実装されている。
この状態推定アルゴリズムでは、「状態遷移モデル」として、以下の４つを定義する。
時刻ｔ＝０の時の内部状態：Ｘ０ 
初期条件Ｘ０の共分散：ＣｏｖＸ０ 
状態遷移方程式：Ｘｔ＋Δｔ＝ｆ（Ｘｔ，Δｔ）・・・（１）
単位時間当たりのプロセスノイズ（共分散）：Ｑ
【００２５】
このアルゴリズムでは、対象物１が吊るされている振り子の振れ角θや、角速度Δθ、支点位置ｘ，ｙ，ｚなどの状態変数をまとめて、内部状態Ｘとする。モデルを定義する際、初期条件が必ずしも正確にわかるわけではない。したがって、初期条件と実物の差がどの程度大きくなるかを予想した共分散ＣｏｖＸ０を定義する。
状態遷移方程式（１）は、時刻が進むと内部状態Ｘがどのように変化するかを示す。この例の場合、振り子の運動方程式や、支点位置が等速直線運動することなどから、状態遷移方程式を定義する。
【００２６】
定義した状態遷移方程式（１）は、必ずしも実際の対象物１の運動と一致しない。そこで、単位時間（Δｔ＝１）の時間幅だけ状態遷移計算をした時に、実際の運動とどの程度差が生まれるかを示す共分散Ｑを定義する。
また、カルマンフィルタは、計測装置１２で算出される対象物１の位置と姿勢を計測値Ｙとし、式（２）（３）の「観測モデル」（観測方程式と観測ノイズ）を定義する。
観測方程式：Ｙｔ＝ｇ（Ｘｔ）・・・（２）
観測ノイズ（共分散）：ＣｏｖＳｔ・・・（３）
【００２７】
観測方程式（２）は、内部状態Ｘと計測値Ｙを対応づける式である。また、観測ノイズ（３）は、時刻ｔの時の計測にどの程度の計測誤差が含まれるかを表す共分散であり、カメラの分解能や視点の向きによって決まる。観測ノイズ（３）は固定値ではなく、画像ごとに計測装置１２が算出し、計測値と共に状態推定装置１４に渡される。
【００２８】
ロボット制御装置２０は、把持可能の場合、内部状態の推定結果に基づいて対象物１の移動先を予測して把持動作するようにロボットを制御する機能を有する。
【００２９】
ロボット制御装置２０は、ロボット２から手先位置情報などを受信し、手先速度指令値やハンド開閉指令値を送信する。これらの送受信を、一定の制御周期（４ｍｓ周期）で行う。
【００３０】
以下、図２に基づき本発明の移動体把持方法を説明する。
本発明の方法では、（Ａ）初めに、計測装置１２により、対象物１の位置と姿勢を計測する。
（Ｂ）次いで、計測した計測結果から、状態推定装置１４により、状態遷移モデルに基づいて、精度指標値Ｅを含む対象物１の内部状態を推定し、かつ（Ｃ）精度指標値Ｅから対象物１の把持の可否を判定する。
（Ｄ）次に、把持可能の場合、ロボット制御装置２０により、内部状態の推定結果に基づいて対象物１の移動先を予測してロボット２を把持動作し、（Ｅ）把持不可の場合、ロボット制御装置２０により、対象物１の位置と姿勢の計測を継続できるようにロボット２を移動させる。
【００３１】
精度指標値Ｅは、対象物１の位置と姿勢を予測した予測値とその実測値との誤差と、状態遷移モデルと観測モデルに基づいて前記誤差の分散を予想した共分散行列とからなる。
【００３２】
前記（Ｃ）において、前記誤差を前記共分散行列で割ったマハラノビス距離Ｍｔの自乗が第１閾値より小さく、かつ、共分散行列の行列式が第２閾値より小さい時に、把持可能と判定する、ことが好ましい。
第１閾値と第２閾値は、予め設定した任意の閾値である。
【００３３】
また前記（Ｃ）において、前記誤差を前記共分散行列で割ったマハラノビス距離Ｍｔの自乗が第３閾値より小さく、かつ、共分散行列のトレースが第４閾値より小さい時に、把持可能と判定してもよい。
第３閾値と第４閾値は、予め設定した任意の閾値である。
【００３４】
以下、発明の移動体把持装置の動作を説明する。
（ａ１）計測装置１２（第１計測装置１２ａと第２計測装置１２ｂ）は任意のタイミングで２台のカメラ（第１カメラ５ａと第２カメラ５ｂ）にそれぞれシャッター信号を送信し、撮像された画像を取得する。
（ａ２）計測装置１２により、得られた画像から画像処理によって対象物１の位置と姿勢の情報（計測値Ｙ）を取得する。例えば、画像内の白色領域を抽出し、重心を求める。
（ａ３）計測装置１２からシャッター信号を送信した時刻をｔｙとする。また、カメラ５ａ，５ｂの向きや分解能などから、観測ノイズＣｏｖＳを算出する。
（ａ４）状態推定装置１４は、計測値Ｙと、状態遷移モデルと観測モデルによって予測したＹの予測値Ｙｍを比較して、内部状態Ｘを修正する。また、対象物１の位置を予測する精度を表す精度指標値Ｅを算出する。
（ａ５）ロボット制御装置２０は、制御周期ごとに状態推定装置１４の結果を参照し、精度指標値Ｅを評価して、把持動作を開始するかどうかの判定を行う。
（ａ６）把持動作を開始する場合は、対象物１の未来の位置を予測し、予測した把持位置を目指して手先速度指令値を算出する。把持動作しない場合は、カメラ５ａ，５ｂが対象物１を捕捉し続けられるように、手先速度指令値を算出する。
【００３５】
上記（ａ４）における状態推定装置１４の処理内容の詳細を以下に説明する。以下の処理内容は、カルマンフィルタを利用した一般的な状態推定手段である。
（ｂ１）計測時刻ｔｙと現在のモデル時刻ｔから、状態遷移させる時間幅Δｔを算出する。モデル時刻ｔの初期値は、計測を開始した時刻とする。また、計測時刻を、開始時刻を基点とした経過時間として定義し、モデル時刻の初期値を０としても良い。
状態遷移させる時間幅Δｔは、Δｔ＝ｔｙ−ｔ・・・（４）で表される。
【００３６】
（ｂ２）計測時刻ｔｙにおける内部状態を予測する。
内部状態の予測値Ｘｔ＋Δｔは、上述した状態遷移モデルで定義した、Ｘｔ＋Δｔ＝ｆ（Ｘｔ，Δｔ）・・・（１）で表される。
【００３７】
（ｂ３）計測時刻における、内部状態予測値の共分散を算出する。ここで、行列Ａは状態遷移方程式ｆを偏微分した行列である。
更新（ｂ９）前の内部状態の共分散ＣｏｖＸｔ＋Δｔ｜ｔは、式（５）で表される。
ＣｏｖＸｔ＋Δｔ｜ｔ＝Ａｔ（Δｔ）・ＣｏｖＸｔ・Ａｔ（Δｔ）Ｔ＋Ｑ・｜Δｔ｜
・・・（５）
状態遷移方程式の偏微分行列Ａｔ（Δｔ）は、式（６）で表される。
Ａｔ（Δｔ）＝（∂ｆ（Ｘｔ、Δｔ）／∂Ｘｔ）・・・（６）
【００３８】
（ｂ４）内部状態予測値と観測方程式を用いて、式（７）で計測値Ｙの予測値Ｙｍｔ＋Δｔを予測する。
Ｙｍｔ＋Δｔ＝ｇ（Ｘｔ＋Δｔ）・・・（７）
【００３９】
（ｂ５）内部状態予測値の共分散に基づいて、予測値Ｙｍの共分散ＣｏｖＹｔ＋Δｔを式（８）で求める。ここで、行列Ｈは観測方程式ｇを偏微分した行列である。
ＣｏｖＹｔ＋Δｔ＝Ｈｔ・ＣｏｖＸｔ＋Δｔ｜ｔ・ＨｔＴ・・・（８）
観測方程式の偏微分行列Ｈｔは、式（９）で表される。
Ｈｔ＝（∂ｇ（Ｘｔ）／∂Ｘｔ）・・・（９）
【００４０】
（ｂ６）予測値Ｙｍの共分散ＣｏｖＹに加えて、カメラの計測誤差を考慮した共分散Ｖｔ＋Δｔを、式（１０）で求める。ＣｏｖＳｔ＋Δｔは時刻ｔｙ（＝ｔ＋Δｔ）の時の観測ノイズであり、計測装置１２が算出する。
共分散Ｖｔ＋Δｔは、予測値Ｙｍの共分散に、カメラの観測ノイズを加えているため、予測値Ｙｍと観測値Ｙの差の大きさを示す共分散となる。
Ｖｔ＋Δｔ＝ＣｏｖＹｔ＋Δｔ＋ＣｏｖＳｔ＋Δｔ・・・（１０）
【００４１】
（ｂ７）カルマンゲインＫｔ＋Δｔを式（１１）で算出する。
Ｋｔ＋Δｔ＝（ＣｏｖＸｔ＋Δｔ｜ｔ・ＨｔＴ）／（Ｖｔ＋Δｔ）・・・（１１）
【００４２】
（ｂ８）内部状態Ｘｔ＋Δｔを式（１２）（１３）で更新する。なお、更新前の内部状態をＸｔ＋Δｔ｜ｔと表記する。なおＹはセンサで計測された観測値である。
ｅＹｔ＋Δｔ＝Ｙｍｔ＋Δｔ｜ｔ−Ｙｔ＋Δｔ・・・（１２）
Ｘｔ＋Δｔ＝Ｘｔ＋Δｔ｜ｔ−Ｋｔ＋Δｔ・ｅＹｔ＋Δｔ・・・（１３）
【００４３】
（ｂ９）更新後の内部状態の共分散ＣｏｖＸｔ＋Δｔを式（１４）で算出する。
ＣｏｖＸｔ＋Δｔ＝（Ｉ−Ｋｔ＋ΔｔＨｔ）ＣｏｖＸｔ＋Δｔ｜ｔ・・・（１４）
【００４４】
上述した（ｂ１）から（ｂ９）までの処理によって、時刻ｔｙの観測値に基づいて、内部状態が修正される。したがって、システム全体が上記（ａ１）〜（ａ６）の処理を繰り返すことで、内部状態が徐々に真の値（対象物の実速度など）に近づく。
【００４５】
内部状態がどの程度、真の値に近づいているか、を判断するには、上記（ｂ５）、（ｂ６）で求めた予測値の共分散ＣｏｖＹ、Ｖを評価する。これらの共分散は、現在の推定結果を使って対象物の位置と姿勢を予測した場合の、予測誤差の大きさを表す。
【００４６】
事前に定義した状態遷移モデルと観測モデルが正確な場合、観測値と予測値の差ｅＹは、共分散Ｖにしたがった分布となる。したがって、実際の差ｅＹが、Ｖに比べて大きな値となった場合は、事前に定めたモデルが正しくないことを意味する。このような評価には、式（１５）に示すマハラノビス距離Ｍｔを用いる。
Ｍｔ＝（ｅＹｔＴ・Ｖｔ−１・ｅＹｔ）０．５・・・（１５）
【００４７】
そこで状態推定装置１４では、（ｂ１）から（ｂ９）の処理後に、現在の状態推定の精度を表す精度指標値Ｅを、以下のように算出する。
（ｂ１０）以下の（ｃ１）（ｃ２）（ｃ３）のいずれかにより精度指標値Ｅを算出する。
【００４８】
（ｃ１）Ｅ＝ｅｘｐ（−Ｍｔ＋Δｔ２／２）／（（２π）ｍ｜Ｖｔ＋Δｔ｜）０．５
・・・（１６）
ここでｍは、ｅＹの次数である。
（ｃ２）Ｅ＝ｅｘｐ（−Ｍｔ＋Δｔ２／２）／（ｔｒａｃｅ（ＣｏｖＹｔ＋Δｔ））０．５
・・・（１７）
（ｃ３）式（１６）（１７）のＥを、過去の計測結果全てについて総和した式（１８）。ここでｐは、内部状態の変数の数である。
【００４９】
【数１】
0005733516.tif 000002
【００５０】
なお、（ｃ１）の式（１６）は、正規分布のモデル適合度の評価式であり、（ｃ３）の式（１８）は、情報量基準（ＡＩＣ）である。
（ｃ２）の式（１７）は、分母に共分散ＣｏｖＹのトレースを用いる点に特徴がある。
式（１６）において共分散の行列式の平方根は、ｅＹの分布の体積を表す。一方、式（１７）においてトレースの平方根は、分布を包み込む最小の球の半径を表す。
【００５１】
次に上記（ａ５）（ａ６）におけるロボット制御装置２０の処理内容を以下に示す。
図３は、本発明による移動体把持方法の全体フロー図である。この図に示すように、移動体把持方法は、Ｓ１〜Ｓ１２の各ステップ（工程）からなる。
【００５２】
Ｓ１では、ロボット２から手先位置情報などを受信する。ロボット２との通信は、例えば４ｍｓの制御周期で行なわれるようにロボット２が管理する。したがって、ロボット制御装置２０は、データを受信するまで待機し、データを受信してから例えば４ｍｓの制御周期以内でＳ１２の送信処理を完了しなければならない。
Ｓ２では、把持動作中を示すフラグＦを判定する。フラグＦはプログラム開始時にｆａｌｓｅに初期化されている。
【００５３】
Ｓ３では、把持動作中でない場合、上記（ａ１）から（ａ４）の処理を行う。この処理は、別の処理系が任意の実行周期で実施し、ロボット制御装置２０は最新の推定結果と精度指標値Ｅを参照する形式でも良い。
【００５４】
Ｓ４では、現在の状態推定の精度指標値Ｅから、把持動作を行うか、追従動作を継続するかを判定する。判定手段は以下の（ｄ１）〜（ｄ３）のいずれかを用いる。
【００５５】
（ｄ１）上述した（ｃ１）の精度指標値Ｅがある閾値（第１閾値）を超えるかどうかを判定する。式（１６）では、マハラノビス距離Ｍｔが大きい場合、指数関数部が０に近づく。また、共分散Ｖの体積が大きい場合、分母が大きくなる。したがって、マハラノビス距離Ｍｔが小さく、かつ、共分散Ｖの体積が小さい時のみ、把持動作が開始される。
【００５６】
この例のように２台のカメラ（第１カメラ５ａと第２カメラ５ｂ）を用いる場合、対象物１が片方のカメラの視野から外れ、１台のカメラのみで計測を続ける状況が起こりうる。この時、片方の視点でしか計測されないため、共分散ＣｏｖＹ、Ｖはカメラの視線方向に細長い分布を示す共分散行列となる。（ｄ１）の手段の場合、Ｖが細長い場合でも体積は小さくなるので、把持動作が開始される可能性がある。
【００５７】
（ｄ２）上述した（ｃ２）の精度指標値Ｅがある閾値（第３閾値）を超えるかどうかを判定する。式（１７）の精度指標値Ｅは、ＣｏｖＹが細長い分布を表す場合、小さな値となる。したがって、２台のカメラが両方とも対象物１を捉えている時のみ、把持動作が開始される。
【００５８】
（ｄ３）上述した（ｄ１）（ｄ２）は、共に、最新の計測結果で判定するが、過去数点分の精度指標値を考慮して判定しても良い。例えば、上述した（ｃ３）のように、過去に算出された精度指標値の対数をとって、総和する方法や、過去一定時間分の平均値をとる方法などが挙げられる。
【００５９】
Ｓ５では、把持動作をしない場合、把持動作中フラグＦはｆａｌｓｅとする。
Ｓ６では、把持動作をしない場合、例えば以下の式（１９）（２０）（２１）ようにアーム速度Ｖｘ、Ｖｙ、Ｖｚを設定する。
Ｖｘ＝Ｋｐｘ・（ｍｘ−ｒｘ）＋Ｋｄｘ・（ｍｘ−ｒｘ−ｐｒ＿ｘ）・・・（１９）
Ｖｙ＝Ｋｐｙ・（ｍｙ−ｒｙ）＋Ｋｄｙ・（ｍｙ−ｒｙ−ｐｒ＿ｙ）・・・（２０）
Ｖｚ＝Ｋｐｚ・（ｍｚ−ｒｚ）＋Ｋｄｚ・（ｍｚ−ｒｚ−ｐｒ＿ｚ）・・・（２１）
【００６０】
この制御は、ＰＤ制御であり、ｍｘ、ｍｙ、ｍｚは、計測された対象物の現在位置［ｍｍ］、ｒｘ、ｒｙ、ｒｚは追従制御点（カメラ１の視線上の点）の現在位置［ｍｍ］、ｐｒ＿ｘ、ｐｒ＿ｙ、ｐｒ＿ｚは、前回のステップで算出された位置偏差（ｍｙ−ｒｙ、ｍｚ−ｒｚなど）、Ｋｐは位置制御ゲイン、Ｋｄは微分制御ゲインである。
【００６１】
Ｓ７では、Ｓ４で把持動作を行うと判定された場合は、状態遷移モデルと観測モデルを用いて、未来の対象物１の位置と姿勢を予測し、把持位置Ａとする。
Ｓ８では、把持動作開始時に、把持動作中フラグＦをｔｒｕｅにする。
Ｓ９では、把持動作中は、ロボットアーム３が把持位置Ａに移動するように、ロボットアーム３の目標速度を算出する。
Ｓ１０では、把持動作が終了したかどうかを判定する。例えば、ロボットアーム３が把持位置Ａに到達して、ハンド４を閉じる動作が終了している場合に把持動作終了と判定する。
Ｓ１１では、把持動作を終了する場合、把持動作フラグＦをｆａｌｓｅにする。把持動作を終了しない場合は、把持動作フラグＦはｔｒｕｅのままなので、Ｓ２の分岐により、次回以降も把持動作が継続される。
Ｓ１２では、算出された目標速度やハンド開閉指令値をロボット２に送信する。
【００６２】
なお上述の例では、対象物１の運動状態を推定する方法として、カルマンフィルタを用いたが、以下のような条件を満たす状態推定手法であれば、他の手法を用いても良い。例えば、パーティクルフィルタ、最小自乗法などを適用できる。
（ｅ１）対象物１の運動状態を推定して、未来の位置を予測できること。
（ｅ２）未来の位置を予測した時の予測誤差の分散を表す共分散行列を算出できること。
【００６３】
上述のｃ１では、Ｖの行列式を使った判定の例を示しているが、Ｖ，ＣｏｖＹのどちらの行列式を使っても構わない。また、上述のｃ２では、ＣｏｖＹのｔｒａｃｅを使った判定の例を示しているが、Ｖ，ＣｏｖＹのどちらのｔｒａｃｅを使っても構わない。
すなわち「誤差の分散を予想した共分散行列」が、Ｖ，ＣｏｖＹのどちらもあり得るようになっていればよい。
【実施例１】
【００６４】
図４は、実施例における経過時間と精度指標値Ｅとの関係を示す図である。
この例は、上述した（ｃ２）の式（１７）で、精度指標値Ｅを算出した結果である。
【００６５】
この図から計測開始時から精度指標値Ｅは徐々に大きくなり、計測開始から経過時間が２秒後以降は８０以上で安定することがわかる。
したがって、式（１７）の精度指標値Ｅが８０以上であることを把持動作開始条件とすれば、把持失敗しにくくなるといえる。
【００６６】
また、この図で経過時間が１秒付近に見られるように、一時的に精度指標値が高くなる場合がある。したがって、上述した（ｃ３）のように、過去数点分の精度指標値を考慮して判定することが好ましい。
【００６７】
例えば、過去数点の平均値が８０を超えている、一定時間以上連続して８０を超えているなどを考慮する。
例えば、「１２０ｍｓ以上連続して、精度指標値が８０を超えている」ことを把持動作開始条件とするのがよい。
【００６８】
上述した本発明の装置と方法によれば、計測装置１２及び状態推定装置１４により、対象物１の位置又は姿勢を計測し、対象物１の内部状態を推定するので、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物１（例えばワーク）の内部状態（例えば位置、姿勢、速度、角速度）に基づいて対象物１の運動予測をすることができる。
【００６９】
また、状態推定装置１４により、状態遷移モデルと観測モデルに基づいて、精度指標値Ｅを含む対象物１の内部状態を推定し、精度指標値Ｅから対象物１の把持の可否を判定するので、把持不可の場合、ロボット制御装置２０により、例えば対象物１の位置と姿勢の計測を継続できるようにロボット２を移動させることで、予測精度が低下する場合でも把持の失敗を未然に回避することができる。
【００７０】
さらに、把持可能の場合に、ロボット制御装置２０により、内部状態の推定結果に基づいて対象物１の移動先を予測してロボット２を把持動作するので、ロボット２を対象物１に追従させて確実に把持することができる。
【００７１】
上述した例では、対象物１が移動しロボット２が固定されているが、本発明はこの例に限定されず、対象物が動かずにロボットが移動する場合でも、対象物とロボットの両方が移動する場合でもよい。
したがって、本発明における「対象物の移動」は、ロボットから見た対象物の相対的な移動を意味する。
【００７２】
なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。
【符号の説明】
【００７３】
１  ワーク（対象物）、２  ロボット、
３  ロボットアーム、４  ハンド、
５ａ  第１カメラ、５ｂ  第２カメラ、
１０  移動体把持装置、１２  計測装置、
１２ａ  第１計測装置、１２ｂ  第２計測装置、
１４  状態推定装置、１６  データ記憶装置、
２０  ロボット制御装置
【図１】
0005733516.tif 000003
【図２】
0005733516.tif 000004
【図３】
0005733516.tif 000005
【図４】
0005733516.tif 000006