JP2012236254A - 移動体把持装置と方法 - Google Patents

Abstract

【課題】対象物の運動予測し、予測精度の低下の影響を受けることなく、ロボットを対象物に追従させて確実に把持することができる移動体把持装置と方法を提供する。
【解決手段】対象物の位置又は姿勢を計測する計測装置１２と、対象物を計測した計測結果から、対象物の内部状態を推定し把持の可否を判定する状態推定装置１４と、ロボットを制御するロボット制御装置２０とを備える。（Ａ）計測装置１２により、対象物の位置又は姿勢を計測し、（Ｂ）計測結果から、状態推定装置１４により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ（Ｃ）精度指標値から対象物の把持の可否を判定し、（Ｄ）把持可能の場合、ロボット制御装置２０により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、（Ｅ）把持不可の場合、前記（Ａ）〜（Ｄ）を繰り返す。
【選択図】図２

Description

本発明は、ロボットなどを使った自動装置のうち、移動する対象物を把持する移動体把持装置と方法に関する。

移動する対象物をロボットでハンドリングする自動装置では、ロボット上やロボットの外部に設置したカメラなどの視覚センサで対象物の位置を計測し、その計測結果に基づいてロボットを追従制御する必要がある。

上述したロボットの追従制御では、時々刻々と変化する対象物（例えばワーク）の位置を一定周期（例：３０ｆｐｓのカメラ）で計測し、計測結果に基づいて、例えば対象物にロボットを近づけるような、移動指令がロボットに出力される。

しかし、計測したワークの位置を目標としてロボットを動作させても、センサの計測遅れ、データの取得遅れ、ロボットの動作遅れなどの制御遅れにより、ロボットが移動するワークに追従できないことがある。
また、視覚センサの場合、センサの計測周期はロボットの制御周期（例：４ｍｓ）よりも一般に長いため、ロボットは制御周期ごとに最新の計測結果を得られるわけではなく、その分が計測遅れとなる。特に、画像処理に時間がかかったり、ワークがカメラの視野から外れたりすると、計測結果が更新される周期はさらに長くなり、かつ一定ではなくなる。このように、移動するワークを扱う装置では、制御遅れによりロボットの追従性能が低下する問題点があった。

上述した問題を解決するために、種々の制御手段が既に提案されている（例えば、特許文献１〜４）。

特許文献１の「ロボット装置及びその制御方法」では、まずアームを対象物に追従させ、追従時のアームの運動状態と、対象物の運動状態が一致していることを利用して、対象物の移動先を予測する方法が提案されている。このように、アームの動作時間を考慮して、対象物の移動先を予測することで、移動体の把持が実現される。
特許文献２の「３次元運動予測装置」は、単振動する被計測体の位置データからその運動パラメータを推定し、その将来の位置を予測し、その位置情報を基にマニュピュレータにより被計測体を把持するものである。
特許文献３の状態推定手段は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するものである。内部状態とは、対象物の位置、姿勢、振れ角などの状態変数を意味する。
特許文献４の「運動予測装置」は、バックグラウンド処理とフォアグラウンド処理を併用して自動追尾と運動予測を行うものである。

特許第４２６５０８８号公報、「ロボット装置及びその制御方法」 特開平０７−０１９８１８号公報、「３次元運動予測装置」 特許第４０７２０１７号公報、「状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置」 特許第４１５３６２５号公報、「運動予測装置」

上述した特許文献２によれば、対象物（例えばワーク）を計測した観測履歴から、対象物の運動状態を推定することで、計測値が得られていない間のデータを補完したり、対象物をロボットで把持する際の把持地点を予測したりすることができる。

運動状態の推定には、カルマンフィルタなどが一般的に用いられる。特許文献２、３の状態推定手段では、対象物の速度、加速度などを「内部状態」とし、内部状態が時間と共にどのように変化するかを表す「状態遷移モデル」を事前に定義する。

特許文献１、２のように移動先を予測して把持する装置では、予測の精度が低いと、対象物が予測した位置から外れ、把持に失敗することがある。
特許文献１では、把持動作に移行する前に、アームと対象物の偏差が一定になっている状態（完全追従状態、不完全追従状態）であることを判定する。この判定によって、アームと対象物の運動状態が高い精度で一致している時のみ把持動作をするため、把持精度が向上する。しかし、この判定条件は、等速直線運動、等速円運動、等加速度直線運動には適用できるが、それ以外の運動に対して適用できない。

対象物の予測位置の精度が低くなるのは、主に以下のような状況の時である。
（１）計測開始直後。
（２）対象物を計測できない時間がしばらく続いた時。
（３）対象物が、事前に定めた状態遷移モデルと違った動きをしている時。

本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物（例えばワーク）の内部状態（例えば位置、姿勢、速度、角速度）に基づいて対象物の運動予測をすることができ、予測精度の低下の影響を受けることなく、ロボットを対象物に追従させて確実に把持することができる移動体把持装置と方法を提供することにある。

本発明によれば、相対的に移動する対象物をロボットで把持する移動体把持装置であって、
対象物の位置又は姿勢を計測する計測装置と、
対象物を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ精度指標値から対象物の把持の可否を判定する状態推定装置と、
把持可能の場合、内部状態の推定結果に基づいて対象物の移動先を予測して把持動作するようにロボットを制御するロボット制御装置と、を備えることを特徴とする移動体把持装置が提供される。

また本発明によれば、相対的に移動する対象物をロボットで把持する移動体把持方法であって、
（Ａ）計測装置により、対象物の位置又は姿勢を計測し、
（Ｂ）計測した計測結果から、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ
（Ｃ）精度指標値から対象物の把持の可否を判定し、
（Ｄ）把持可能の場合、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、
（Ｅ）把持不可の場合、前記（Ａ）〜（Ｄ）を繰り返す、ことを特徴とする移動体把持方法が提供される。

上記本発明の装置と方法によれば、計測装置及び状態推定装置により、対象物の位置又は姿勢を計測し、対象物の内部状態を推定するので、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物（例えばワーク）の内部状態（例えば位置、姿勢、速度、角速度）に基づいて対象物の運動予測をすることができる。

また、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、精度指標値から対象物の把持の可否を判定するので、把持不可の場合、ロボット制御装置により、予測精度が低下する場合でも把持の失敗を未然に回避することができる。

さらに、把持可能の場合に、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作するので、ロボットを対象物に追従させて確実に把持することができる。

本発明による移動体把持装置を有するロボットシステムの実施形態図である。 本発明による移動体把持装置の実施形態図である。 本発明による移動体把持方法の全体フロー図である。 実施例における経過時間と精度指標値Ｅとの関係を示す図である。

以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明による移動体把持装置を有するロボットシステムの実施形態図である。
この図において、１は対象物（ワーク）、２はロボット、３はロボットアーム、４はハンド、５ａはハンド４に固定された第１カメラ、５ｂは外部の定位置に固定された第２カメラ、１０は移動体把持装置である。
このロボットシステムは、振り子運動しながら移動するワーク１をカメラ５ａ，５ｂで計測し、ロボット２をワーク１に追従させて制御し、ハンド４によりワーク１を把持するようになっている。

図２は、本発明による移動体把持装置の実施形態図である。
この図において、移動体把持装置１０は、計測装置１２、状態推定装置１４、及びロボット制御装置２０を備える。状態推定装置１４は、データ記憶装置１６を備える。

計測装置１２は、対象物１の位置と姿勢を計測する。
計測装置１２は、この例では第１カメラ５ａと第２カメラ５ｂにそれぞれ接続された２台の計測装置（第１計測装置１２ａと第２計測装置１２ｂ）からなる。
すなわち、この例では、カメラと計測装置をそれぞれ２台用い、第１カメラ５ａと第２カメラ５ｂから対象物１の画像を取り込み、画像処理によって、対象物１の位置と姿勢を求める。求められた対象物１の位置と姿勢は、データ記憶装置１６に記憶される。
なお、計測装置１２は、この構成に限定されず、対象物１の位置を１台で３次元計測することができる計測装置、例えばレーザレーダを用いてもよい。

状態推定装置１４は、対象物１を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値Ｅ（後述する）を含む対象物１の内部状態を推定し、かつ精度指標値Ｅから対象物１の把持の可否を判定する機能を有する。

状態推定装置１４には、この例ではカルマンフィルタのアルゴリズムを基にした状態推定アルゴリズムが実装されている。
この状態推定アルゴリズムでは、「状態遷移モデル」として、以下の４つを定義する。
時刻ｔ＝０の時の内部状態：Ｘ_０
初期条件Ｘ_０の共分散：Ｃ_ｏｖＸ_０
状態遷移方程式：Ｘ_ｔ＋Δｔ＝ｆ（Ｘ_ｔ，Δｔ）・・・（１）
単位時間当たりのプロセスノイズ（共分散）：Ｑ

このアルゴリズムでは、対象物１が吊るされている振り子の振れ角θや、角速度Δθ、支点位置ｘ，ｙ，ｚなどの状態変数をまとめて、内部状態Ｘとする。モデルを定義する際、初期条件が必ずしも正確にわかるわけではない。したがって、初期条件と実物の差がどの程度大きくなるかを予想した共分散Ｃ_ｏｖＸ_０を定義する。
状態遷移方程式（１）は、時刻が進むと内部状態Ｘがどのように変化するかを示す。この例の場合、振り子の運動方程式や、支点位置が等速直線運動することなどから、状態遷移方程式を定義する。

定義した状態遷移方程式（１）は、必ずしも実際の対象物１の運動と一致しない。そこで、単位時間（Δｔ＝１）の時間幅だけ状態遷移計算をした時に、実際の運動とどの程度差が生まれるかを示す共分散Ｑを定義する。
また、カルマンフィルタは、計測装置１２で算出される対象物１の位置と姿勢を計測値Ｙとし、式（２）（３）の「観測モデル」（観測方程式と観測ノイズ）を定義する。
観測方程式：Ｙ_ｔ＝ｇ（Ｘ_ｔ）・・・（２）
観測ノイズ（共分散）：Ｃ_ｏｖＳ_ｔ・・・（３）

観測方程式（２）は、内部状態Ｘと計測値Ｙを対応づける式である。また、観測ノイズ（３）は、時刻ｔの時の計測にどの程度の計測誤差が含まれるかを表す共分散であり、カメラの分解能や視点の向きによって決まる。観測ノイズ（３）は固定値ではなく、画像ごとに計測装置１２が算出し、計測値と共に状態推定装置１４に渡される。

ロボット制御装置２０は、把持可能の場合、内部状態の推定結果に基づいて対象物１の移動先を予測して把持動作するようにロボットを制御する機能を有する。

ロボット制御装置２０は、ロボット２から手先位置情報などを受信し、手先速度指令値やハンド開閉指令値を送信する。これらの送受信を、一定の制御周期（４ｍｓ周期）で行う。

以下、図２に基づき本発明の移動体把持方法を説明する。
本発明の方法では、（Ａ）初めに、計測装置１２により、対象物１の位置と姿勢を計測する。
（Ｂ）次いで、計測した計測結果から、状態推定装置１４により、状態遷移モデルに基づいて、精度指標値Ｅを含む対象物１の内部状態を推定し、かつ（Ｃ）精度指標値Ｅから対象物１の把持の可否を判定する。
（Ｄ）次に、把持可能の場合、ロボット制御装置２０により、内部状態の推定結果に基づいて対象物１の移動先を予測してロボット２を把持動作し、（Ｅ）把持不可の場合、ロボット制御装置２０により、対象物１の位置と姿勢の計測を継続できるようにロボット２を移動させる。

精度指標値Ｅは、対象物１の位置と姿勢を予測した予測値とその実測値との誤差と、状態遷移モデルと観測モデルに基づいて前記誤差の分散を予想した共分散行列とからなる。

前記（Ｃ）において、前記誤差を前記共分散行列で割ったマハラノビス距離Ｍ_ｔの自乗が第１閾値より小さく、かつ、共分散行列の行列式が第２閾値より小さい時に、把持可能と判定する、ことが好ましい。
第１閾値と第２閾値は、予め設定した任意の閾値である。

また前記（Ｃ）において、前記誤差を前記共分散行列で割ったマハラノビス距離Ｍ_ｔの自乗が第３閾値より小さく、かつ、共分散行列のトレースが第４閾値より小さい時に、把持可能と判定してもよい。
第３閾値と第４閾値は、予め設定した任意の閾値である。

以下、発明の移動体把持装置の動作を説明する。
（ａ１）計測装置１２（第１計測装置１２ａと第２計測装置１２ｂ）は任意のタイミングで２台のカメラ（第１カメラ５ａと第２カメラ５ｂ）にそれぞれシャッター信号を送信し、撮像された画像を取得する。
（ａ２）計測装置１２により、得られた画像から画像処理によって対象物１の位置と姿勢の情報（計測値Ｙ）を取得する。例えば、画像内の白色領域を抽出し、重心を求める。
（ａ３）計測装置１２からシャッター信号を送信した時刻をｔｙとする。また、カメラ５ａ，５ｂの向きや分解能などから、観測ノイズＣ_ｏｖＳを算出する。
（ａ４）状態推定装置１４は、計測値Ｙと、状態遷移モデルと観測モデルによって予測したＹの予測値Ｙｍを比較して、内部状態Ｘを修正する。また、対象物１の位置を予測する精度を表す精度指標値Ｅを算出する。
（ａ５）ロボット制御装置２０は、制御周期ごとに状態推定装置１４の結果を参照し、精度指標値Ｅを評価して、把持動作を開始するかどうかの判定を行う。
（ａ６）把持動作を開始する場合は、対象物１の未来の位置を予測し、予測した把持位置を目指して手先速度指令値を算出する。把持動作しない場合は、カメラ５ａ，５ｂが対象物１を捕捉し続けられるように、手先速度指令値を算出する。

上記（ａ４）における状態推定装置１４の処理内容の詳細を以下に説明する。以下の処理内容は、カルマンフィルタを利用した一般的な状態推定手段である。
（ｂ１）計測時刻ｔｙと現在のモデル時刻ｔから、状態遷移させる時間幅Δｔを算出する。モデル時刻ｔの初期値は、計測を開始した時刻とする。また、計測時刻を、開始時刻を基点とした経過時間として定義し、モデル時刻の初期値を０としても良い。
状態遷移させる時間幅Δｔは、Δｔ＝ｔｙ−ｔ・・・（４）で表される。

（ｂ２）計測時刻ｔｙにおける内部状態を予測する。
内部状態の予測値Ｘ_ｔ＋Δｔは、上述した状態遷移モデルで定義した、Ｘ_ｔ＋Δｔ＝ｆ（Ｘ_ｔ，Δｔ）・・・（１）で表される。

（ｂ３）計測時刻における、内部状態予測値の共分散を算出する。ここで、行列Ａは状態遷移方程式ｆを偏微分した行列である。
更新（ｂ９）前の内部状態の共分散Ｃ_ｏｖＸ_ｔ＋Δｔ｜_ｔは、式（５）で表される。
Ｃ_ｏｖＸ_ｔ＋Δｔ｜_ｔ＝Ａ_ｔ（Δｔ）・Ｃ_ｏｖＸ_ｔ・Ａ_ｔ（Δｔ）^Ｔ＋Ｑ・｜Δｔ｜
・・・（５）
状態遷移方程式の偏微分行列Ａ_ｔ（Δｔ）は、式（６）で表される。
Ａ_ｔ（Δｔ）＝（∂ｆ（Ｘ_ｔ、Δｔ）／∂Ｘ_ｔ）・・・（６）

（ｂ４）内部状態予測値と観測方程式を用いて、式（７）で計測値Ｙの予測値Ｙｍ_ｔ＋Δｔを予測する。
Ｙｍ_ｔ＋Δｔ＝ｇ（Ｘ_ｔ＋Δｔ）・・・（７）

（ｂ５）内部状態予測値の共分散に基づいて、予測値Ｙｍの共分散Ｃ_ｏｖＹ_ｔ＋Δｔを式（８）で求める。ここで、行列Ｈは観測方程式ｇを偏微分した行列である。
Ｃ_ｏｖＹ_ｔ＋Δｔ＝Ｈｔ・Ｃ_ｏｖＸ_ｔ＋Δｔ｜_ｔ・Ｈｔ^Ｔ・・・（８）
観測方程式の偏微分行列Ｈ_ｔは、式（９）で表される。
Ｈ_ｔ＝（∂ｇ（Ｘ_ｔ）／∂Ｘ_ｔ）・・・（９）

（ｂ６）予測値Ｙｍの共分散Ｃ_ｏｖＹに加えて、カメラの計測誤差を考慮した共分散Ｖ_ｔ＋Δｔを、式（１０）で求める。Ｃ_ｏｖＳ_ｔ＋Δｔは時刻ｔｙ（＝ｔ＋Δｔ）の時の観測ノイズであり、計測装置１２が算出する。
共分散Ｖ_ｔ＋Δｔは、予測値Ｙｍの共分散に、カメラの観測ノイズを加えているため、予測値Ｙｍと観測値Ｙの差の大きさを示す共分散となる。
Ｖ_ｔ＋Δｔ＝Ｃ_ｏｖＹ_ｔ＋Δｔ＋Ｃ_ｏｖＳ_ｔ＋Δｔ・・・（１０）

（ｂ７）カルマンゲインＫ_ｔ＋Δｔを式（１１）で算出する。
Ｋ_ｔ＋Δｔ＝（Ｃ_ｏｖＸ_ｔ＋Δｔ｜_ｔ・Ｈｔ^Ｔ）／（Ｖ_ｔ＋Δｔ）・・・（１１）

（ｂ８）内部状態Ｘ_ｔ＋Δｔを式（１２）（１３）で更新する。なお、更新前の内部状態をＸ_ｔ＋Δｔ｜_ｔと表記する。なおＹはセンサで計測された観測値である。
ｅＹ_ｔ＋Δｔ＝Ｙｍ_ｔ＋Δｔ｜_ｔ−Ｙ_ｔ＋Δｔ・・・（１２）
Ｘ_ｔ＋Δｔ＝Ｘ_ｔ＋Δｔ｜_ｔ−Ｋ_ｔ＋Δｔ・ｅＹ_ｔ＋Δｔ・・・（１３）

（ｂ９）更新後の内部状態の共分散Ｃ_ｏｖＸ_ｔ＋Δｔを式（１４）で算出する。
Ｃ_ｏｖＸ_ｔ＋Δｔ＝（Ｉ−Ｋ_ｔ＋ΔｔＨ_ｔ）Ｃ_ｏｖＸ_ｔ＋Δｔ｜_ｔ・・・（１４）

上述した（ｂ１）から（ｂ９）までの処理によって、時刻ｔｙの観測値に基づいて、内部状態が修正される。したがって、システム全体が上記（ａ１）〜（ａ６）の処理を繰り返すことで、内部状態が徐々に真の値（対象物の実速度など）に近づく。

内部状態がどの程度、真の値に近づいているか、を判断するには、上記（ｂ５）、（ｂ６）で求めた予測値の共分散Ｃ_ｏｖＹ、Ｖを評価する。これらの共分散は、現在の推定結果を使って対象物の位置と姿勢を予測した場合の、予測誤差の大きさを表す。

事前に定義した状態遷移モデルと観測モデルが正確な場合、観測値と予測値の差ｅＹは、共分散Ｖにしたがった分布となる。したがって、実際の差ｅＹが、Ｖに比べて大きな値となった場合は、事前に定めたモデルが正しくないことを意味する。このような評価には、式（１５）に示すマハラノビス距離Ｍ_ｔを用いる。
Ｍ_ｔ＝（ｅＹ_ｔ ^Ｔ・Ｖ_ｔ ^−１・ｅＹ_ｔ）^０．５・・・（１５）

そこで状態推定装置１４では、（ｂ１）から（ｂ９）の処理後に、現在の状態推定の精度を表す精度指標値Ｅを、以下のように算出する。
（ｂ１０）以下の（ｃ１）（ｃ２）（ｃ３）のいずれかにより精度指標値Ｅを算出する。

（ｃ１）Ｅ＝ｅｘｐ（−Ｍ_ｔ＋Δｔ ^２／２）／（（２π）^ｍ｜Ｖ_ｔ＋Δｔ｜）^０．５
・・・（１６）
ここでｍは、ｅＹの次数である。
（ｃ２）Ｅ＝ｅｘｐ（−Ｍ_ｔ＋Δｔ ^２／２）／（ｔｒａｃｅ（Ｃ_ｏｖＹ_ｔ＋Δｔ））^０．５
・・・（１７）
（ｃ３）式（１６）（１７）のＥを、過去の計測結果全てについて総和した式（１８）。ここでｐは、内部状態の変数の数である。

なお、（ｃ１）の式（１６）は、正規分布のモデル適合度の評価式であり、（ｃ３）の式（１８）は、情報量基準（ＡＩＣ）である。
（ｃ２）の式（１７）は、分母に共分散Ｃ_ｏｖＹのトレースを用いる点に特徴がある。
式（１６）において共分散の行列式の平方根は、ｅＹの分布の体積を表す。一方、式（１７）においてトレースの平方根は、分布を包み込む最小の球の半径を表す。

次に上記（ａ５）（ａ６）におけるロボット制御装置２０の処理内容を以下に示す。
図３は、本発明による移動体把持方法の全体フロー図である。この図に示すように、移動体把持方法は、Ｓ１〜Ｓ１２の各ステップ（工程）からなる。

Ｓ１では、ロボット２から手先位置情報などを受信する。ロボット２との通信は、例えば４ｍｓの制御周期で行なわれるようにロボット２が管理する。したがって、ロボット制御装置２０は、データを受信するまで待機し、データを受信してから例えば４ｍｓの制御周期以内でＳ１２の送信処理を完了しなければならない。
Ｓ２では、把持動作中を示すフラグＦを判定する。フラグＦはプログラム開始時にｆａｌｓｅに初期化されている。

Ｓ３では、把持動作中でない場合、上記（ａ１）から（ａ４）の処理を行う。この処理は、別の処理系が任意の実行周期で実施し、ロボット制御装置２０は最新の推定結果と精度指標値Ｅを参照する形式でも良い。

Ｓ４では、現在の状態推定の精度指標値Ｅから、把持動作を行うか、追従動作を継続するかを判定する。判定手段は以下の（ｄ１）〜（ｄ３）のいずれかを用いる。

（ｄ１）上述した（ｃ１）の精度指標値Ｅがある閾値（第１閾値）を超えるかどうかを判定する。式（１６）では、マハラノビス距離Ｍ_ｔが大きい場合、指数関数部が０に近づく。また、共分散Ｖの体積が大きい場合、分母が大きくなる。したがって、マハラノビス距離Ｍ_ｔが小さく、かつ、共分散Ｖの体積が小さい時のみ、把持動作が開始される。

この例のように２台のカメラ（第１カメラ５ａと第２カメラ５ｂ）を用いる場合、対象物１が片方のカメラの視野から外れ、１台のカメラのみで計測を続ける状況が起こりうる。この時、片方の視点でしか計測されないため、共分散Ｃ_ｏｖＹ、Ｖはカメラの視線方向に細長い分布を示す共分散行列となる。（ｄ１）の手段の場合、Ｖが細長い場合でも体積は小さくなるので、把持動作が開始される可能性がある。

（ｄ２）上述した（ｃ２）の精度指標値Ｅがある閾値（第３閾値）を超えるかどうかを判定する。式（１７）の精度指標値Ｅは、Ｃ_ｏｖＹが細長い分布を表す場合、小さな値となる。したがって、２台のカメラが両方とも対象物１を捉えている時のみ、把持動作が開始される。

（ｄ３）上述した（ｄ１）（ｄ２）は、共に、最新の計測結果で判定するが、過去数点分の精度指標値を考慮して判定しても良い。例えば、上述した（ｃ３）のように、過去に算出された精度指標値の対数をとって、総和する方法や、過去一定時間分の平均値をとる方法などが挙げられる。

Ｓ５では、把持動作をしない場合、把持動作中フラグＦはｆａｌｓｅとする。
Ｓ６では、把持動作をしない場合、例えば以下の式（１９）（２０）（２１）ようにアーム速度Ｖｘ、Ｖｙ、Ｖｚを設定する。
Ｖｘ＝Ｋｐｘ・（ｍｘ−ｒｘ）＋Ｋｄｘ・（ｍｘ−ｒｘ−ｐｒ＿ｘ）・・・（１９）
Ｖｙ＝Ｋｐｙ・（ｍｙ−ｒｙ）＋Ｋｄｙ・（ｍｙ−ｒｙ−ｐｒ＿ｙ）・・・（２０）
Ｖｚ＝Ｋｐｚ・（ｍｚ−ｒｚ）＋Ｋｄｚ・（ｍｚ−ｒｚ−ｐｒ＿ｚ）・・・（２１）

この制御は、ＰＤ制御であり、ｍｘ、ｍｙ、ｍｚは、計測された対象物の現在位置［ｍｍ］、ｒｘ、ｒｙ、ｒｚは追従制御点（カメラ１の視線上の点）の現在位置［ｍｍ］、ｐｒ＿ｘ、ｐｒ＿ｙ、ｐｒ＿ｚは、前回のステップで算出された位置偏差（ｍｙ−ｒｙ、ｍｚ−ｒｚなど）、Ｋｐは位置制御ゲイン、Ｋｄは微分制御ゲインである。

Ｓ７では、Ｓ４で把持動作を行うと判定された場合は、状態遷移モデルと観測モデルを用いて、未来の対象物１の位置と姿勢を予測し、把持位置Ａとする。
Ｓ８では、把持動作開始時に、把持動作中フラグＦをｔｒｕｅにする。
Ｓ９では、把持動作中は、ロボットアーム３が把持位置Ａに移動するように、ロボットアーム３の目標速度を算出する。
Ｓ１０では、把持動作が終了したかどうかを判定する。例えば、ロボットアーム３が把持位置Ａに到達して、ハンド４を閉じる動作が終了している場合に把持動作終了と判定する。
Ｓ１１では、把持動作を終了する場合、把持動作フラグＦをｆａｌｓｅにする。把持動作を終了しない場合は、把持動作フラグＦはｔｒｕｅのままなので、Ｓ２の分岐により、次回以降も把持動作が継続される。
Ｓ１２では、算出された目標速度やハンド開閉指令値をロボット２に送信する。

なお上述の例では、対象物１の運動状態を推定する方法として、カルマンフィルタを用いたが、以下のような条件を満たす状態推定手法であれば、他の手法を用いても良い。例えば、パーティクルフィルタ、最小自乗法などを適用できる。
（ｅ１）対象物１の運動状態を推定して、未来の位置を予測できること。
（ｅ２）未来の位置を予測した時の予測誤差の分散を表す共分散行列を算出できること。

上述のｃ１では、Ｖの行列式を使った判定の例を示しているが、Ｖ，Ｃ_ｏｖＹのどちらの行列式を使っても構わない。また、上述のｃ２では、Ｃ_ｏｖＹのｔｒａｃｅを使った判定の例を示しているが、Ｖ，Ｃ_ｏｖＹのどちらのｔｒａｃｅを使っても構わない。
すなわち「誤差の分散を予想した共分散行列」が、Ｖ，Ｃ_ｏｖＹのどちらもあり得るようになっていればよい。

図４は、実施例における経過時間と精度指標値Ｅとの関係を示す図である。
この例は、上述した（ｃ２）の式（１７）で、精度指標値Ｅを算出した結果である。

この図から計測開始時から精度指標値Ｅは徐々に大きくなり、計測開始から経過時間が２秒後以降は８０以上で安定することがわかる。
したがって、式（１７）の精度指標値Ｅが８０以上であることを把持動作開始条件とすれば、把持失敗しにくくなるといえる。

また、この図で経過時間が１秒付近に見られるように、一時的に精度指標値が高くなる場合がある。したがって、上述した（ｃ３）のように、過去数点分の精度指標値を考慮して判定することが好ましい。

例えば、過去数点の平均値が８０を超えている、一定時間以上連続して８０を超えているなどを考慮する。
例えば、「１２０ｍｓ以上連続して、精度指標値が８０を超えている」ことを把持動作開始条件とするのがよい。

上述した本発明の装置と方法によれば、計測装置１２及び状態推定装置１４により、対象物１の位置又は姿勢を計測し、対象物１の内部状態を推定するので、等速直線運動、等速円運動、等加速度直線運動以外の運動であっても対象物１（例えばワーク）の内部状態（例えば位置、姿勢、速度、角速度）に基づいて対象物１の運動予測をすることができる。

また、状態推定装置１４により、状態遷移モデルと観測モデルに基づいて、精度指標値Ｅを含む対象物１の内部状態を推定し、精度指標値Ｅから対象物１の把持の可否を判定するので、把持不可の場合、ロボット制御装置２０により、例えば対象物１の位置と姿勢の計測を継続できるようにロボット２を移動させることで、予測精度が低下する場合でも把持の失敗を未然に回避することができる。

さらに、把持可能の場合に、ロボット制御装置２０により、内部状態の推定結果に基づいて対象物１の移動先を予測してロボット２を把持動作するので、ロボット２を対象物１に追従させて確実に把持することができる。

上述した例では、対象物１が移動しロボット２が固定されているが、本発明はこの例に限定されず、対象物が動かずにロボットが移動する場合でも、対象物とロボットの両方が移動する場合でもよい。
したがって、本発明における「対象物の移動」は、ロボットから見た対象物の相対的な移動を意味する。

なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。

１ ワーク（対象物）、２ ロボット、
３ ロボットアーム、４ ハンド、
５ａ 第１カメラ、５ｂ 第２カメラ、
１０ 移動体把持装置、１２ 計測装置、
１２ａ 第１計測装置、１２ｂ 第２計測装置、
１４ 状態推定装置、１６ データ記憶装置、
２０ ロボット制御装置

Claims (5)

1. 相対的に移動する対象物をロボットで把持する移動体把持装置であって、
   対象物の位置又は姿勢を計測する計測装置と、
   対象物を計測した計測結果から、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ精度指標値から対象物の把持の可否を判定する状態推定装置と、
   把持可能の場合、内部状態の推定結果に基づいて対象物の移動先を予測して把持動作するようにロボットを制御するロボット制御装置と、を備えることを特徴とする移動体把持装置。
2. 相対的に移動する対象物をロボットで把持する移動体把持方法であって、
   （Ａ）計測装置により、対象物の位置又は姿勢を計測し、
   （Ｂ）計測した計測結果から、状態推定装置により、状態遷移モデルと観測モデルに基づいて、精度指標値を含む対象物の内部状態を推定し、かつ
   （Ｃ）精度指標値から対象物の把持の可否を判定し、
   （Ｄ）把持可能の場合、ロボット制御装置により、内部状態の推定結果に基づいて対象物の移動先を予測してロボットを把持動作し、
   （Ｅ）把持不可の場合、前記（Ａ）〜（Ｄ）を繰り返す、ことを特徴とする移動体把持方法。
3. 前記精度指標値は、対象物の位置と姿勢を予測した予測値とその実測値との誤差と、状態遷移モデルと観測モデルに基づいて前記誤差の分散を予想した共分散行列とからなる、ことを特徴とする請求項２に記載の移動体把持方法。
4. 前記（Ｃ）において、前記誤差を前記共分散行列で割ったマハラノビス距離の自乗が第１閾値より小さく、かつ、共分散行列の行列式が第２閾値より小さい時に、把持可能と判定する、ことを特徴とする請求項３に記載の移動体把持方法。
5. 前記（Ｃ）において、前記誤差を前記共分散行列で割ったマハラノビス距離の自乗が第３閾値より小さく、かつ、共分散行列のトレースが第４閾値より小さい時に、把持可能と判定する、ことを特徴とする請求項３に記載の移動体把持方法。