JP2016132086A - ワーク供給装置およびワーク把持姿勢計算方法 - Google Patents

Abstract

【課題】装置を大型化させることなく、バラ積みされたワークを把持する確率を向上させ、後工程における作業の成功率を向上させる。【解決手段】センサ１の計測データと、予め蓄積されたワーク５の形状データとを照合し、ワーク５の位置および姿勢を算出するワーク認識部４３と、算出したワーク５の位置から把持するワーク５を選択し、選択したワーク５の位置および姿勢と、ワーク５を組み付け対象物１００に供給可能に算出されたワーク５と把持機構３との位置関係を示す複数の把持姿勢との対応付けを行い、ワーク５の把持姿勢を決定するワーク把持姿勢計算部４４と、ワーク把持姿勢計算部４４が決定した把持姿勢に基づいて、移動機構２および把持機構３を制御する制御部４５とを備える。【選択図】図１

Description

この発明は、多数のワークからワークを取り出すワーク供給装置およびワークを取り出す際の姿勢を計算するワーク把持姿勢計算方法に関するものである。

従来のワーク供給装置は、センサにより得られたデータからワークの姿勢を認識し、認識したワークの姿勢に基づいて把持するワークを選択し、ロボットなどの移動機構を制御して、当該移動機構に取り付けられた把持機構で選択されたワークを把持する。ワークの選択では、認識したワークの姿勢の評価指標を参照して当該評価指標が最も高く、周辺のワークや壁などにぶつかることなく把持することができるワークが選択される。選択されたワークは、予め一意に定められた把持姿勢を有する把持機構により把持される。ここで、把持姿勢とは、把持機構に対してワークがどのような姿勢で把持されるかを示す、把持機構とワークとの相対的な位置および姿勢の関係である。例えば、把持機構の座標系と、ワークの座標系間の並進３自由度、回転３自由度の６自由度の変換行列で表すことができる。

しかし、予め一意に定められた把持姿勢でワークを把持する場合、選択されたワークを把持する場合に、把持機構が周辺のワークや壁などにぶつかり、把持位置まで到達することができず、ワークを把持できない場合が発生する。
この問題を解決するため、特許文献１では把持機構の形状と計測データとから、ワークのどこを把持するかを動的に決定し、複数の把持位置姿勢候補を作成し、作成した把持位置姿勢候補から最も掴みやすい把持位置姿勢を選択するワーク取り出し装置が開示されている。

さらに、上述した特許文献１のように任意の把持位置姿勢で把持されたワークを、後工程に整列させて供給する技術として例えば特許文献２に開示されたものがある。特許文献２の部品供給装置は、把持機構が把持したワークをカメラで撮像された仮置き台の上で一度解放してワークを台上に置く。撮像画像を参照しながら垂直多関節型の把持機構が仮置き台上のワークを把持し、複数の垂直多関節型の把持機構でワークの裏表などを反転させながら、ワークをパレット上に整列配置させる。

国際公開２０１２／６６８１９号公報 国際公開２０１３／２０９９号公報

しかしながら、上述した特許文献１および特許文献２では、把持可能な場所を把持してワークを取り出すことができるが、ワークを取り出した後、取り出したワークを持ち替えて一意の姿勢で配置させる必要があり、装置サイズが大きくなるという課題があった。

この発明は、上記のような課題を解決するためになされたもので、装置を大型化させることなく、バラ積みされたワークを把持する確率を向上させ、後工程における作業の成功率を向上させることを目的とする。

この発明に係るワーク供給装置は、センサの計測データと、予め蓄積されたワークの形状データとを照合し、ワークの位置および姿勢を算出するワーク認識部と、ワーク認識部が算出したワークの位置から把持するワークを選択し、選択したワークの位置および姿勢と、ワークを供給対象物に供給可能に算出されたワークと把持機構との位置関係を示す複数の把持姿勢との対応付けを行いワークの把持姿勢を決定するワーク把持姿勢計算部と、ワーク把持姿勢計算部が決定した把持姿勢に基づいて、移動機構および把持機構を制御する制御部とを備えるものである。

この発明によれば、後工程のためにワークを持ち替える必要のない把持姿勢で、バラ積みされたワークを把持する確率を向上させることができる。これにより、後工程における作業の成功率も向上させることができる。また、後工程のためにワーク持ち替える必要がなく、装置サイズの大型化を抑制することができる。

実施の形態１に係るワーク供給装置の構成を示す図である。 実施の形態１に係るワーク供給装置の作業工程を示す図である。 実施の形態１に係るワーク供給装置の情報処理部のハードウェア構成を示すブロック図である。 実施の形態１に係るワーク供給装置の把持機構の把持姿勢のパターンを示す図である。 実施の形態１に係るワーク供給装置の動作を示すフローチャートである。 実施の形態１に係るワーク供給装置のワーク把持姿勢計算部の動作を示すフローチャートである。 実施の形態１によるワーク供給装置のワーク把持姿勢計算部の処理を示す説明図である。 実施の形態２に係るワーク供給装置の構成を示す図である。 実施の形態２に係るワーク供給装置の後工程を実現する把持姿勢の計算を示す図である。 実施の形態２に係るワーク供給装置の動作を示すフローチャートである。 実施の形態２に係るワーク供給装置のワーク把持姿勢計算部および可操作把持姿勢計算部の動作を示すフローチャートである。 実施の形態２に係るワーク供給装置のワーク把持姿勢計算部および可操作把持姿勢計算部の動作を示すフローチャートである。

実施の形態１．
図１は、実施の形態１に係るワーク供給装置の構成を示す図である。
実施の形態１のワーク供給装置１０は、センサ１、移動機構２、把持機構３および情報処理部４で構成されている。
図２は、実施の形態１に係るワーク供給装置１０の作業工程を示す図である。
図２（ａ）では、当該ワーク供給装置１０は、供給箱６内のワーク５を把持機構３が任意の把持姿勢で把持して取り出す（以下、把持工程と称する）。取り出したワーク５は、把持機構３で把持されたまま移動機構２により次の工程に運ばれ、図２（ｂ）に示すように組み付け対象物（供給対象物）１００へ組み付けが行われる（以下、後工程と称する）。このように、ワーク供給装置１０はワーク５の把持と組み付け対象物への組み付けを継続して行う。

図１を参照しながら、各構成の説明を行う。
センサ１は、移動機構２に設置された視覚センサであり、図１ではハンドアイと呼ばれる方式の設置形態を採用した場合を示している。ハンドアイ方式を採用した設置形態においては、視覚センサを用いてワーク５の位置を計測する。計測した位置は移動機構２の動作または位置の補正に用いられる。

センサ１は、供給箱６内にバラ積みされたワーク５の形状を判断するための距離値、あるいは輝度値、または距離値および輝度値の双方を計測する機能を有する手段であれば適宜、適用可能である。例えば、画像を撮像するカメラ、撮影したワーク５までの距離を計測するステレオ視を行うステレオカメラ、アクティブステレオを実施する投光手段とカメラの組み合わせ、超音波センサ、静電容量センサなどで構成可能である。
図１では、センサ１を移動機構２に設置するハンドアイ方式の例を示したが、移動機構２以外の場所に固定設置してもよいし、移動機構２とは異なる独立した移動機構に固定設置してもよい。

移動機構２は、把持機構３を所定の位置まで搬送する機構であり、図１に示した例では６自由度のロボットアームの形状を有している。なお、移動機構２として４自由度のスカラ型ロボット、７自由度のロボットアーム、複数の一軸移動ステージの組み合わせなど、どのような自由度の構成を適用してもよい。このように、自由度の高い構成とすることにより、バラ積みされたワーク５を把持機構３が把持する確率を高めることができる。

図１で示した把持機構３は、ワーク５を挟んで掴む形状（以下、グリッパ形状と称する）を有している。なお、把持機構３は、バラ積みされたワーク５を任意の把持姿勢で拾い上げることが可能であればどのような構成を適用してもよい。例えば、バキューム機構によりワーク５に吸い付いて把持する吸着ハンド、あるいは電磁力で金属製のワーク５に吸い付くハンドなどを適用することができる。把持機構３の把持姿勢とは、把持機構３に対してワーク５がどのような姿勢で把持されるかを示す、把持機構３とワーク５との相対的な位置および姿勢の関係である。当該把持姿勢は、例えば把持機構３の座標系と、ワーク５の座標系間の並進３自由度、回転３自由度の６自由度の変換行列で表すことができる。

情報処理部４は、データベース４１、計測部４２、ワーク認識部４３、ワーク把持姿勢計算部４４および制御部４５で構成され、センサ１から入力された情報に基づいて、把持機構３がワーク５を把持する際の把持姿勢を計算し、計算結果に基づいて移動機構２および把持機構３を制御する。
図３は、実施の形態１に係るワーク供給装置１０の情報処理部４のハードウェア構成を示すブロック図であり、プロセッサ４ａとメモリ４ｂで構成されている。計測部４２、ワーク認識部４３、ワーク把持姿勢計算部４４および制御部４５は、プロセッサ４ａがメモリ４ｂに記憶されたプログラムを実行することにより、実現される。データベース４１は、メモリ４ｂ内に記憶されているものとする。

データベース４１は、後工程を実現するために予め学習した把持機構３の一意の把持姿勢、当該一意の把持姿勢から計算された後工程を実現する把持機構３の複数の把持姿勢、予め学習した移動機構２および把持機構３の動作シーケンス、移動機構２の運動学モデル、把持機構３の形状モデル、ワーク５の形状モデル、把持工程が行われる作業環境の形状モデル、および後工程が行われる作業環境の形状モデルそれぞれを示すデータを蓄積している。
ここで、一意の把持姿勢とは、後工程を開始する際にユーザがワーク供給装置１０に対して入力する把持姿勢であって、後工程を実現することができる把持姿勢である。また、複数の把持姿勢は、当該ユーザによって入力された一意の把持姿勢から計算によって導かれる、同様に後工程を実現することができる複数の把持姿勢である。データベース４１に蓄積されたデータの詳細については後述する。

計測部４２は、センサ１が計測した距離値、あるいは輝度値、または距離値および輝度値の双方から、センサ１からバラ積みされたワーク５までの距離を計測し、距離データとして出力する。また、ワーク認識部４３がワーク５までの距離を必要とせず、センサ１が取得した画像データのみを必要とする場合、画像データの補正を行う。例えば、センサ１が計測した輝度値を補正する、あるいは画像データのノイズ除去を行い、ワーク認識部４３がワーク５の位置および姿勢を特定可能な画像データに補正し、出力する。また、計測部４２は、センサ１の計測データから、ワーク５および供給箱６の周囲に存在する構造物までの距離を計測し、周辺距離データとして出力する。

ワーク認識部４３は、計測部４２が計測したセンサ１からワーク５までの距離を示す距離データと、データベース４１に蓄積されたワーク５の形状データを照合し、センサ１の計測データ上でのワーク５の位置および姿勢を算出する。ここで、ワーク５の姿勢とは、ワーク５がバラ積みされた際の向きや傾きなどである。また、ワーク認識部４３は、位置および姿勢を算出できたワーク５の数を計測し、バラ積みされたワーク５の数として特定してもよい。

計測部４２が計測したワーク５までの距離データとワーク５の形状データとの照合には、例えば、計測部４２が計測した計測データがポイントクラウドなどの距離データの場合、点対点の距離の二乗和を評価値としてそれを最小にするＩｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ（ＩＣＰ）法を利用することができる。また、ワーク５の形状データから得られる幾何学的な特徴（形状データ上の円の半径や、ワーク５の輪郭にあたる形状データのエッジ・コーナー位置、そのエッジ・コーナー間の距離や角度）と、距離データから得られる幾何学的な特徴との類似性を各特徴量の差分の総和で評価し、類似度が高い位置および姿勢を探索するなどの方法を利用してもよい。

一方、計測部４２が計測した計測データが画像データの場合には、ワーク５の形状データを仮想的な視点から撮影したと想定した場合に得られる、ワーク５の仮想画像データをワーク５の形状データとしてデータベース４１に蓄積しておく。計測部４２が計測した画像データと、蓄積されたワーク５の形状データとを照合し、センサ１の計測データ上でのワーク５の位置および姿勢を算出する。画像データと形状データとの照合には、正規化相互相関法などのテンプレートマッチング手法や、距離変換画像を利用するチャンファーマッチングなどの形状データのエッジを特徴とする照合方法を利用してもよい。

上述した方法を用いて、距離データまたは画像データとワーク５の形状データとを照合することにより、センサ１の計測データに対してワーク５の形状データを一致させ、計測データ上でのワーク５の位置および姿勢を算出する。

ワーク把持姿勢計算部４４は、ワーク認識部４３が算出したワーク５の位置および姿勢を示す情報から、把持機構３が当該ワーク５を把持するための把持位置および把持姿勢を計算する。
従来では、ワーク認識部４３がワーク５の位置および姿勢を特定すれば、特定されたワーク５の位置および姿勢から把持機構３の把持位置および把持姿勢が一意に決定されていた。しかし、上述したように、当該把持位置および把持姿勢は予め学習された一意の把持姿勢から計算されるものであって、移動機構２および把持機構３が決定された把持姿勢を取ることができない場合が多々存在していた。

そこで、本願発明のワーク供給装置１０のワーク把持姿勢計算部４４は、データベース４１に蓄積された一意の把持姿勢、および当該一意の把持姿勢から計算された後工程を実現する複数の把持姿勢と、センサ１の計測データにおけるワーク５の位置および姿勢とを対応付ける処理を行う。当該対応付けを行うことにより、把持機構３がワーク５を把持する確率を向上させ、後工程の作業を実現可能な姿勢でワーク５を把持する。なお、データベース４１に蓄積された把持姿勢とセンサ１の計測データにおけるワーク５の位置および姿勢との対応付けの詳細については後述する。

制御部４５は、ワーク把持姿勢計算部４４が計算した把持位置および把持姿勢に基づいて、移動機構２および把持機構３に対して制御を行い、ワーク５の把持を実行させる。計測部４２がワーク５の数を算出している場合には、制御部４５が当該ワーク５の数を参照し、ワーク５の供給処理を継続するか終了させるか判断し、制御する。また、ワーク５の数を算出していない場合には、計測部４２がワーク５を計測できない場合にワーク５の供給処理を終了させる判断を行う。
また、ワーク認識部４３がワーク５の位置および姿勢を１度認識すれば、その後認識処理を繰り返すことなく、移動機構２および把持機構３がワーク５を把持する動作を繰り返すように構成することも可能である。この場合、ワーク認識部４３の認識回数に対するワーク５の把持個数を判定する場合に、ワーク認識部４３が計測したワーク５の数を参照する。

次に、データベース４１に蓄積されたデータの詳細について説明する。上述のように、データベース４１は以下のデータを蓄積している。
（１）後工程を実現するために与えられた一意の把持姿勢
（２）一意の把持姿勢から計算された後工程を実現する複数の把持姿勢
（３）予め学習した移動機構２および把持機構３の動作シーケンス
（４）移動機構２の運動学モデル
（５）把持機構３の形状モデル
（６）ワーク５の形状モデル
（７）把持工程が行われる作業環境の形状モデル、
（８）後工程が行われる作業環境の形状モデル

以下では、（１）から（３）のデータの蓄積について詳細に説明を行う。
まず、「後工程を実現するために与えられた一意の把持姿勢」について、バラ積みされたワーク５を一つ把持して取り出し、そのまま組み付け対象物に組み付ける場合、組み付け作業を行う工程が後工程である。当該後工程を実現するためには、ワーク５を同一の組み付け姿勢で組み付け対象物に組み付ける必要がある。当該組み付け姿勢を取るために、把持機構３がワーク５をどのような姿勢で掴むかを示すのが把持機構３の把持姿勢である。

これらに基づけば、「後工程を実現するための一意の把持姿勢」とは、ワーク５を組み付け姿勢で把持した際の把持機構３の一意の姿勢であり、組み付け作業を開始する際の把持機構３の一意の姿勢である。組み付け姿勢を満たす把持機構３の把持姿勢は通常複数存在するが、その中の１つの姿勢が一意の把持姿勢として与えられ、データベース４１に蓄積される。

上述のように、「後工程を実現するための一意の把持姿勢」とは、組み付け作業を開始する際の把持機構３の一意の把持姿勢であるが、後工程の前にはバラ積みされたワーク５を取り出す把持工程が存在する。そのため、組み付け作業開始時の把持姿勢であり、後工程に移行する直前の把持工程の最終的な姿勢であると言える。さらに、把持工程において、把持機構３がワーク５を持ち替える操作を行わない場合には、把持機構３がバラ積みされたワーク５を掴む際の把持姿勢も「後工程を実現するための一意の把持姿勢」となる。

与えられた「後工程を実現するための一意の把持姿勢」から、さらに後工程を実現する複数の把持姿勢を計算し、データベース４１に蓄積する。
ここで、後工程を実現する複数の把持姿勢について、図４を参照しながら説明する。図４は、実施の形態１に係るワーク供給装置１０の把持機構３の把持姿勢のパターンを示す図である。

図４では、ワーク５を組み付け対象物１００の穴部１００ａに挿入する組み付けを行う場合を示している。ワーク５の組み付け姿勢Ａを満たす、把持機構３の４つの把持姿勢２０ａ，２０ｂ，２０ｃ，２０ｄを示している。組み付け姿勢Ａを満たす、即ちワーク５の挿入位置（穴部１００ａ）および挿入方向（矢印Ｂ方向）が同一であれば、把持機構３はどのような把持姿勢でワーク５を把持してもよい。そこで、例えば把持姿勢２０ａが一意の把持姿勢としてユーザから与えられ、与えられた把持姿勢２０ａから、組み付け姿勢Ａを満たす把持姿勢２０ｂ，２０ｃ，２０ｄを、後工程を実現する複数の把持姿勢として計算し、データベース４１に蓄積しておく。

次に、予め学習した移動機構２および把持機構３の動作シーケンスの蓄積について説明する。まず、一意の把持姿勢および一意の把持姿勢から計算された複数の把持姿勢を実現するための、移動機構２および把持機構３の動作シーケンスを学習する。ここで、動作シーケンスの学習とは、把持機構３がある把持姿勢を取るための移動機構２および把持機構３の位置および姿勢の関係、移動速度や移動軌跡などをプログラムとして設定し、記憶することである。

具体的には、ユーザがコントローラなどを用いて移動機構２および把持機構３を動かし、ある位置であるワーク５を掴む姿勢とする。動かした後の移動機構２および把持機構３の位置および姿勢をプログラムとして記憶する。さらに、次に作業すべき位置および姿勢に移動機構２および把持機構３を動かし、動かした後の移動機構２および把持機構３の位置および姿勢をプログラムとして記憶する。さらに、記憶した２つの位置の間を補間する軌跡を作成し、作成した軌跡に沿って移動機構２および把持機構３を動かす動作をプログラミングする。

上述した位置および姿勢をプログラムとして記憶する処理、および２つの位置の間を補間する軌跡に沿って移動機構２および把持機構３を動かす動作をプログラミングする処理を繰り返し、移動機構２および把持機構３の動作シーケンスを作成する。これにより、一意の把持姿勢、および一意の把持姿勢から計算される複数の把持姿勢を実現するための移動機構２および把持機構３の動作シーケンスがデータベース４１に蓄積されている。

同様に、把持工程の後に続く後工程において、ワーク５を組み付け対象物１００に組み付けるための移動機構２および把持機構３の動作シーケンスも学習され、データベース４１に蓄積されている。

次に、実施の形態１に係るワーク供給装置１０の動作について説明する。
図５は、実施の形態１に係るワーク供給装置１０の動作を示すフローチャートである。以下では、計測部４２が計測した距離データに基づいてワーク５の認識を行う場合を例に説明を行う。
まず、センサ１は、供給箱６内にバラ積みされたワーク５の計測を行い、計測データを取得する（ステップＳＴ１）。計測部４２は、ステップＳＴ１で取得された計測データから、センサ１からワーク５までの距離データ、およびワーク５の周囲に存在する構造物までの周辺距離データを取得する（ステップＳＴ２）。ワーク認識部４３は、ステップＳＴ２で取得された距離データと、データベース４１に蓄積されたワーク５の形状データとを照合し（ステップＳＴ３）、ステップＳＴ１でセンサ１が取得した計測データ上でのワーク５の位置、姿勢、および数を算出する（ステップＳＴ４）。

ワーク把持姿勢計算部４４は、ステップＳＴ４で算出したワーク５の位置および姿勢に基づいて、把持機構３が当該ワーク５を把持するための把持姿勢を計算する（ステップＳＴ５）。制御部４５は、ステップＳＴ５で計算された把持姿勢に基づいて、移動機構２および把持機構３を制御する（ステップＳＴ６）。移動機構２および把持機構３はステップＳＴ６での制御に基づいて動作し、供給箱６内のワーク５を把持して後工程に供給する（ステップＳＴ７）。

制御部４５は、ステップＳＴ４で取得されたワーク数を参照し、残りのワーク数が０であるか判定を行う。具体的には、ステップＳＴ４で取得したワーク数をＮ個とした場合、Ｎ−１＝０であるか否か判定を行う（ステップＳＴ８）。Ｎ−１＝０でない場合（ステップＳＴ８；ＮＯ）、ステップＳＴ１の処理に戻り、上述した処理を繰り返す。一方、Ｎ−１＝０の場合（ステップＳＴ８；ＹＥＳ）、処理を終了する。

上述した図５のフローチャートでは、１つのワーク５を認識して把持姿勢を選択し、選択した把持姿勢でワーク５を後工程に供給した後、次のワーク５の認識を行う構成を示した。しかし、当該構成に限定されることなく、最初のステップＳＴ４の処理において全てのワーク５の位置、姿勢、および数を算出し、ステップＳＴ５からステップＳＴ８の処理を繰り返すように構成してもよい。その場合、ステップＳＴ８の処理では、後工程に供給したワーク５の数と、ステップＳＴ４で計測したワーク５の数を用いて残りのワーク数を判定するものとする。

次に、上述したステップＳＴ５のワーク把持姿勢計算部４４の詳細な動作を、図６のフローチャートを参照しながら説明する。
図６は、実施の形態１に係るワーク供給装置１０のワーク把持姿勢計算部４４の動作を示すフローチャートである。
まず、ワーク把持姿勢計算部４４は、ステップＳＴ４で算出されたワーク５の位置および姿勢を参照し、バラ積みされたワーク５（以下、選択候補と称する）の中からセンサ１との距離が最も短いワーク５を選択する（ステップＳＴ５１）。ここで、センサ１との距離が最も短いワーク５とは、選択候補のうち最も高い位置に積まれたワーク５である。

ワーク把持姿勢計算部４４は、データベース４１を参照し、蓄積された把持姿勢からある一つの把持姿勢を判定対象に設定する（ステップＳＴ５２）。ここで、蓄積された把持姿勢とは、与えられた一意の把持姿勢、および当該一意の把持姿勢から学習した複数の把持姿勢から成る。ワーク把持姿勢計算部４４は、ステップＳＴ５１で選択したワーク５を、ステップＳＴ５２で設定した把持姿勢で把持する際に、把持機構３が周囲のワーク５と衝突するか否か判定を行う（ステップＳＴ５３）。把持機構３が周囲のワーク５と衝突しない場合、ワーク把持姿勢計算部４４はさらに、把持機構３が周囲の構造物と衝突するか否か判定を行う（ステップＳＴ５４）。

ステップＳＴ５４の衝突判定では、把持機構３がセンサ１が取得した計測データ内で把持姿勢を取った際に、データベース４１に蓄積された把持機構３の形状モデルが、ステップＳＴ２で計測部４２が取得した周辺距離データと重なる場合に把持機構３が周囲の構造物と衝突すると判定する。一方、把持機構３の形状モデルが周辺距離データと重ならない場合には把持機構３が周囲の構造物と衝突しないと判定する。

把持機構３が周囲の構造物と衝突しない場合（ステップＳＴ５４；ＮＯ）、ワーク把持姿勢計算部４４は、ステップＳＴ５２で設定した把持姿勢に向けて、移動機構２が把持機構３を搬送可能か否か判定を行う。移動機構２が把持機構３を搬送可能か否かの判定では、まず、把持操作を行う前の待機状態の把持機構３の位置および姿勢と、選択したワーク５を把持する把持機構３の位置および姿勢とをつなぐ動作経路を算出する（ステップＳＴ５５）。

次に、データベース４１に蓄積された移動機構２の運動学モデルを参照し、移動機構２がステップＳＴ５５で算出した動作経路上を動作可能か否か判定する（ステップＳＴ５６）。移動機構２が動作経路上を動作可能な場合（ステップＳＴ５６；ＹＥＳ）、ワーク把持姿勢計算部４４は、ステップＳＴ５２で設定した把持姿勢をワーク５を把持する姿勢として選択する（ステップＳＴ５７）。その後、フローは図３のステップＳＴ６の処理に進む。

一方、把持機構３が周囲のワーク５と衝突する場合（ステップＳＴ５３；ＹＥＳ）、把持機構３が周囲の構造物と衝突する場合（ステップＳＴ５４；ＹＥＳ）、および移動機構２が動作経路上を動作できない場合（ステップＳＴ５６；ＮＯ）、ワーク把持姿勢計算部４４は、ステップＳＴ５１で選択したワーク５に対応する全ての把持姿勢について処理を行ったか否か判定を行う（ステップＳＴ５８）。全ての把持姿勢について処理を行っていない場合（ステップＳＴ５８；ＮＯ）、新たな把持姿勢を判定対象に設定し（ステップＳＴ５９）、ステップＳＴ５３の処理に戻り、上述した処理を繰り返す。

一方、全ての把持姿勢について処理を行った場合（ステップＳＴ５８；ＹＥＳ）、ワーク把持姿勢計算部４４は現在選択しているワーク５を選択候補から除外する（ステップＳＴ６０）。そして、除外後の選択候補が１以上存在するか否か判定を行う（ステップＳＴ６１）。１以上存在しない場合（ステップＳＴ６１；ＮＯ）には、図５のフローチャートに戻り、処理を終了する。一方、除外後の選択候補が１以上存在する場合（ステップＳＴ６１；ＹＥＳ）、除外後の選択候補からセンサ１との距離が最も短いワーク５を選択する（ステップＳＴ６２）。その後、ステップＳＴ５２の処理に戻り、上述した処理を繰り返す。

上述した図５および図６のフローチャートで示した処理に加えて、以下に示す処理を追加して行ってもよい。
まず、ワーク認識部４３が認識した全てのワーク５に対して、ワーク把持姿勢計算部４４が選択した全ての把持姿勢を、バラ積みされたワーク５の取り出しにおける把持姿勢の解とする。次に、把持機構３の中心座標からワーク５に対して所定方向に延びるベクトル（対応関係を示す情報）を作成し、作成したベクトルがワーク５の表面と交わる交点を算出する。この処理を複数のワーク５に対して行い、ワーク５に対する交点の分布を取得し、交点が最も密集する位置を算出する。次に、バラ積みされた各ワーク５それぞれについて、交点が最も密集した位置に最も近い位置に交点を有するベクトルを特定し、特定したベクトルを延ばす把持機構３の把持姿勢を、優先的に設定する把持姿勢とする。

ワーク５に対する交点の分布の取得について図７に具体例を示す。図７は、実施の形態１に係るワーク供給装置１０のワーク把持姿勢計算部４４の処理を示す説明図である。
図７（ａ）は、上述したベクトルとワーク５の表面との交点の算出の一例を示す図である。
把持機構３の中心座標Ｐからワーク５に対してベクトルＱを作成し、作成したベクトルＱがワーク５の表面において交点Ｒで交わることを示している。図７の例では、ワーク５の表面を三角形状の平面の集合と見なし、交点Ｒが算出された三角形状Ｓが特定される。次にベクトルＱと新たなワーク５の表面との交点を算出する際に、当該三角形状Ｓの周囲に位置する三角形状について交差判定を行い、交点を算出する。
図７（ｂ）は、ワーク５における交点の密集を示す図である。算出された交点の分布をワーク５の表面に示した場合に、図７（ｂ）に示すように交点の分布Ｔが得られる。

上述のように算出された優先的に設定する把持姿勢を、データベース４１に蓄積しておくことにより、図６のフローチャートのステップＳＴ５２の処理において優先的に判定対象を設定することができる。これにより、ステップＳＴ５７においてワークを把持する姿勢として選択される可能性が高まり、ワーク把持姿勢計算部４４の処理時間を短縮することができる。また、バラ積みされたワーク５の把持確率を向上させることができる。これにより、安定した短いサイクルタイムでワーク５を供給することができる。

以上のように、この実施の形態１によれば、後工程を実現するためにユーザから与えられた一意の把持姿勢と、当該一意の把持姿勢から計算された後工程を実現する複数の把持姿勢を蓄積するデータベース４１と、蓄積された一意の把持姿勢および複数の把持姿勢から判定対象に設定した把持姿勢を取った際に、把持機構３が周囲の構造物と衝突するか、あるいは、移動機構２が動作経路上を動作可能か判定し、判定結果に基づいて把持姿勢を選択するワーク把持姿勢計算部４４を備えるように構成したので、ワークを持ち替える動作を行うことなく、後工程の作業を実現可能な把持姿勢でワークを把持することができる。また、設定された把持姿勢を、移動機構２および把持機構３が確実に実行することができる。

また、実施の形態１によれば、ワーク把持姿勢計算部４４が後工程を実現可能な把持姿勢を選択するように構成したので、後工程における作業の成功率を向上させることができる。さらに、把持工程から後工程に移行する際に、把持機構３がワーク５を持ち替える必要がなく、ワーク供給装置１０の装置サイズの大型化を抑制することができる。

また、この実施の形態１によれば、ワーク把持姿勢計算部４４が選択した把持姿勢を用いて、把持機構３の中心からワーク５に延びるベクトルを作成し、作成したベクトルがワーク５の表面と交わる交点の分布から、後工程を実現するために適した把持姿勢を優先的に設定する構成としたので、ワークの把持姿勢を決定する処理時間を短縮することができる。また、バラ積みされたワークの把持確率を向上させることができる。さらに、安定した短いサイクルタイムでワークを供給することができる。

なお、上述した実施の形態１では、後工程を実現するためにユーザから与えられた一意の把持姿勢と、当該一意の把持姿勢から計算された後工程を実現する複数の把持姿勢との両者をデータベース４１に蓄積する構成を示したが、後工程を実現する複数の把持姿勢に一意の把持姿勢が含まれている場合には、当該一意の把持姿勢をデータベース４１に蓄積しなくてもよい。

実施の形態２．
上述した実施の形態１では、後工程を実現するためにユーザから与えられた一意の把持姿勢から計算された後工程を実現する複数の把持姿勢をデータベース４１に蓄積しておく構成を示したが、この実施の形態２では、当該後工程を実現するための複数の把持姿勢を情報処理部４内で計算する構成を示す。さらに、この実施の形態２のワーク供給装置では、計算した把持姿勢に基づいて行った後工程の作業が成功したか否かの判定も行う構成も備える。

図８は、実施の形態２に係るワーク供給装置１０ａの構成を示す図である。なお、以下では、実施の形態１に係るワーク供給装置１０の構成要素と同一または相当する部分には、実施の形態１で使用した符号と同一の符号を付して説明を省略または簡略化する。
実施の形態２に係るワーク供給装置１０ａは、情報処理部４に可操作把持姿勢計算部４６を追加して設け、把持機構３のグリッパ形状の部分にセンサ７を追加して設けている。また、実施の形態２のデータベース４１には、実施の形態１で示した一意の把持姿勢から計算された後工程を実現する複数の把持姿勢は蓄積されていないものとする。

可操作把持姿勢計算部４６は、データベース４１に蓄積された一意の把持姿勢に基づいて、後工程を実現する複数の把持姿勢を計算する。図９は、実施の形態２に係るワーク供給装置１０ａの後工程を実現する把持姿勢の計算を示す図である。
図９（ａ），図９（ｂ）は、データベース４１に蓄積された一意の把持姿勢を示し、図９（ｃ）は一意の把持姿勢の動作経路を示す説明図である。図９（ｄ）は一意の把持姿勢から計算されるその他の把持姿勢およびその動作経路を示す図である。

図９（ａ）は、移動機構２、把持機構３、把持機構３に把持されたワーク５のそれぞれの位置および姿勢の相対的な関係が、実施の形態１で示した「後工程を実現するために与えられた一意の把持姿勢」を満たしている。取り付け操作のため、図９（ａ）の状態から図９（ｂ）の状態に移動し、ワーク５が組み付け姿勢Ａ（図４と同一の姿勢）を取る。
図９（ｃ）に示すように、把持機構３は、組み付けを待機している位置（以下、待機位置と称する）から、組み付け対象物１００に組み付ける位置（以下、組み付け位置と称する）に移動する。待機位置の把持機構３の位置・姿勢３ａと、組み付け位置の把持機構３の位置・姿勢３ｂとの間を補間する動作経路８は予め学習により与えられ、データベース４１に蓄積されているものとする。

可操作把持姿勢計算部４６は、図９（ｃ）で示した一意の把持姿勢に対して、後工程を実現する把持工程を計算する。実施の形態１の図４で示したよう、ワーク５が組み付け姿勢Ａを取るための把持姿勢は複数計算される。例えば、図９（ｄ）に示す把持姿勢が計算される場合、可操作把持姿勢計算部４６は、待機位置の把持機構３の位置・姿勢３ｃと、組み付け位置の把持機構３の位置・姿勢３ｄとの間を補間する動作経路９を推定する。さらに、可操作把持姿勢計算部４６は、データベース４１に蓄積された移動機構２の運動学モデル、把持機構３の形状モデル、ワーク５の形状モデルおよび後工程が行われる作業環境の形状モデルを参照し、動作経路９を経由して把持機構３が位置・姿勢３ｄに到達することができると判定した場合に、後工程を実現する把持姿勢とする。判定では、移動機構２が把持機構３を移動経路９を経由させて移動可能か否か、移動経路９を経由して移動する際に把持機構３、移動機構２およびワーク５がその他の構造物と衝突しないか否かが判断される。

センサ７は、後工程においてワーク５の組み付け作業が成功したか、失敗したかを判定するための情報を収集する検知手段である。例えば、ワーク５を把持する把持機構３のグリッパ形状のハンド部分の開閉量を検知するセンサ、吸着ハンド部分の吸い付き状態を検知するセンサ、ハンド部分あるいは吸着ハンド部分のワーク５の把持状態を撮像するカメラなどで構成される。センサ７が収集した情報は、後工程の作業の成功あるいは失敗を判定するための後工程成否情報としてデータベース４１に蓄積される。

可操作把持姿勢計算部４６はデータベース４１に蓄積された後工程成否情報を参照することにより、各把持姿勢について後工程の成功率を算出可能となる。可操作把持姿勢計算部４６は一意の把持姿勢から計算された複数の把持姿勢に対して、後工程の実際の成功率に基づいて重み付けを行う。重み付けに用いる値は、後工程の成功率そのものを用いてもよいし、後工程の成功率が任意の閾値以上であるか未満であるかに応じて１か０の値に正規化した値を用いてもよく、適宜構成可能である。

次に、可操作把持姿勢計算部４６の動作を中心に、実施の形態２に係るワーク供給装置１０ａの処理動作について説明する。図１０は、実施の形態２に係るワーク供給装置１０ａの動作を示すフローチャートである。実施の形態１と同様に、計測部４２が計測した距離データに基づいてワーク５の認識を行う場合を例に説明を行う。また、図９で示した組み付け姿勢Ａを実現する把持姿勢を算出するものとして説明を行う。

ワーク供給装置１０ａは、実施の形態１と同様にステップＳＴ１からステップＳＴ４の処理を行う。次に、ワーク把持姿勢計算部４４および可操作把持姿勢計算部４６は、ステップＳＴ４で算出したワーク５の位置および姿勢に基づいて、把持機構３が当該ワーク５を把持するための把持姿勢を計算する（ステップＳＴ５ａ）。その後、ステップＳＴ６からステップＳＴ８の処理を行い、ワーク５を供給する処理を行う。

次に、上述したステップＳＴ５ａのワーク把持姿勢計算部４４および可操作把持姿勢計算部４６の詳細な動作を、図１１Ａおよび図１１Ｂのフローチャートを参照しながら説明する。図１１Ａおよび図１１Ｂは、実施の形態２に係るワーク供給装置１０ａのワーク把持姿勢計算部４４および可操作把持姿勢計算部４６の動作を示すフローチャートである。
まず、ワーク把持姿勢計算部４４は、ステップＳＴ４で算出されたワーク５の位置および姿勢を参照し、選択候補のワーク５の中からセンサ１との距離が最も短いワーク５を選択する（ステップＳＴ５１）。ここで、センサ１との距離が最も短いワーク５とは、供給箱６内にバラ積みされたワーク５のうち最も高い位置に積まれたワーク５である。

可操作把持姿勢計算部４６は、ステップＳＴ５１で選択されたワーク５の組み付け姿勢Ａを計算し（ステップＳＴ７１）、当該組み付け姿勢Ａを実現する把持機構３の一意の把持姿勢をデータベース４１から読み出す（ステップＳＴ７２）。ステップＳＴ７２で読み出した一意の把持姿勢に基づいて、組み付け姿勢Ａを実現するその他の把持姿勢候補を計算する（ステップＳＴ７３）。ステップＳＴ７３で計算した各把持姿勢候補から、１つの把持姿勢候補を選択し（ステップＳＴ７４）、選択した把持姿勢候補によって、ワーク５を組み付け対象物に組み付ける後工程を行った場合に、組み付け待機地点と、組み付け地点との２点の間を補間する動作経路を推定する（ステップＳＴ７５）。

可操作把持姿勢計算部４６は、データベース４１に蓄積された移動機構２の運動学モデルを参照し、移動機構２がステップＳＴ７４で推定した動作経路を実現可能か否か判定する（ステップＳＴ７６）。動作経路が実現可能である場合（ステップＳＴ７６；ＹＥＳ）、可操作把持姿勢計算部４６は、動作経路の各点において、データベース４１に蓄積された移動機構２の形状モデル、把持機構３の形状モデル、把持しているワーク５の形状モデルそれぞれが、後工程が行われる作業環境の形状モデルと重ならないか否か判定を行う（ステップＳＴ７７）。なお、判定を行う各点の位置は適宜設定可能である。

後工程が行われる作業環境の形状モデルと重なる場合（ステップＳＴ７７；ＮＯ）、および上述したステップＳＴ７６の判定処理において動作経路を実現できない場合（ステップＳＴ７６；ＮＯ）、当該把持姿勢候補を候補から除外する（ステップＳＴ７８）。その後、ステップＳＴ７９の処理に進む。一方、後工程が行われる作業環境の形状モデルと重ならない場合（ステップＳＴ７７；ＹＥＳ）、対応する把持姿勢を、後工程を実現する把持姿勢に設定する（ステップＳＴ７９）。可操作把持姿勢計算部４６は、ステップＳＴ７３で計算した全ての把持姿勢候補について処理を行ったか否か判定を行う（ステップＳＴ８０）。全ての把持姿勢候補について処理を行っていない場合（ステップＳＴ８０；ＮＯ）、新たな把持姿勢候補を選択し（ステップＳＴ８１）、ステップＳＴ７５の処理に戻る。

一方、全ての把持姿勢候補について処理が終了した場合（ステップＳＴ８０；ＹＥＳ）、可操作把持姿勢計算部４６は、ステップＳＴ７９で設定した把持姿勢、およびステップＳＴ７２で読み出した一意の把持姿勢について、データベース４１を参照して後工程の成功率に基づく重み付けを行う（ステップＳＴ８２）。可操作把持姿勢計算部４６は、ステップＳＴ８２で重み付けを行った各把持姿勢を、ワーク把持姿勢計算部４４に出力する（ステップＳＴ８３）。

ワーク把持姿勢計算部４４は、ステップＳＴ８３で出力された各把持姿勢に付された重みを参照し、後工程の成功率が最も高い把持姿勢を判定対象に設定する（ステップＳＴ８４）。その後、ワーク把持姿勢計算部４４は、ステップＳＴ５３からステップＳＴ６２の処理を経てワークを把持する姿勢を選択すると（ステップＳＴ５７）、ステップＳＴ６の処理に進む。その後、実施の形態１と同様の処理動作を行う。なお、ステップＳＴ５９の処理において、付された重みを参照し、後工程の成功率が高い順に判定対象に設定する。

なお、図１１Ａのフローチャートでは、ステップＳＴ７７の処理において、移動機構２の形状モデル、把持機構３の形状モデル、把持しているワーク５の形状モデルそれぞれが、後工程が行われる作業環境の形状モデルと重なると判定された場合に（ステップＳＴ７７；ＮＯ）、ステップＳＴ７８において当該把持姿勢候補を候補から除外する処理を行ったが、以下のように構成してもよい。

後工程が行われる作業環境の形状モデルと重なると判定された場合、可操作把持姿勢計算部４６は重なると判定された地点の動作経路を修正し、再度ステップＳＴ７６およびステップＳＴ７７の判定を行う。さらに、ステップＳＴ７７の判定処理において、後工程が行われる作業環境の形状モデルと重ならないと判定されるまで処理を繰り返す、あるいは設定された回数判定処理を繰り返し行い、後工程を実現するための正確な動作経路を探索する。このように、後工程が行われる作業環境の形状モデルと重ならない動作経路を探索することにより、組み付け姿勢Ａを実現する把持姿勢をより多く設定することができる。

図１１Ａのフローチャートでは、後工程の成功率に基づいた重み付けを行う例を示したが、当該後工程の成功率に応じた重みに加えて、実施の形態１で示した把持機構３の中心からワーク５に延びるベクトルと、ワーク５の表面とが交わる交点の分布の密集度合いを用いて優先的な把持姿勢の設定を行うように構成してもよい。この場合、成功率に応じた重みに交点の分布の密集度合いを乗算する、あるいは成功率に応じた重みに交点の分布の密集度合いを加算し、高い値を示す把持姿勢について優先的な設定を行う。

以上のように、この実施の形態２によれば、データベース４１に蓄積された一意の把持姿勢に基づいて、後工程を実現する複数の把持姿勢を計算する可操作把持姿勢計算部４６と、後工程の成功率を算出するための情報を収集するセンサ７とを備え、可操作把持姿勢計算部４６は後工程の成功率に応じた重みに基づいて優先的に設定すべき把持姿勢を選択するように構成したので、後工程を実現する可能性の高い把持姿勢を選択することができる。また、蓄積した後工程の成功率を把持姿勢の選択に反映させることができ、後工程が実現する可能性を向上させることができる。

また、この実施の形態２によれば、可操作把持姿勢計算部４６は、後工程の成功率に応じた重みに加えて、把持機構３の中心からワーク５に延びるベクトルを作成し、作成したベクトルがワーク５の表面と交わる交点の分布を考慮して優先的に設定すべき把持姿勢を選択するように構成したので、後工程を実現する可能性の高い把持姿勢を選択することができる。

なお、上述した実施の形態１および実施の形態２では、一意の把持姿勢から計算された後工程を実現する複数の把持姿勢を用いる構成を示したが、一意の把持姿勢から計算される把持姿勢は１以上存在すれば上述した動作を行うことができる。

上記以外にも、本発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

１，７ センサ、２ 移動機構、３ 把持機構、４ 情報処理部、５ ワーク、６ 供給箱、１０，１０ａ ワーク供給装置、４１ データベース、４２ 計測部、４３ ワーク認識部、４４ ワーク把持姿勢計算部、４５ 制御部、４６ 可操作把持姿勢計算部。

Claims (7)

1. バラ積みされたワークを把持する把持機構と、前記把持機構を搬送する移動機構と、前記ワークがバラ積みされた領域を計測するセンサとを備え、前記把持機構が把持したワークを供給対象物に供給するワーク供給装置において、
   前記センサの計測データと、予め蓄積された前記ワークの形状データとを照合し、前記ワークの位置および姿勢を算出するワーク認識部と、
   前記ワーク認識部が算出した前記ワークの位置から把持するワークを選択し、前記選択したワークの位置および姿勢と、前記ワークを前記供給対象物に供給可能に算出された前記ワークと前記把持機構との位置関係を示す複数の把持姿勢との対応付けを行い前記ワークの把持姿勢を決定するワーク把持姿勢計算部と、
   前記ワーク把持姿勢計算部が決定した前記把持姿勢に基づいて、前記移動機構および前記把持機構を制御する制御部とを備えたことを特徴とするワーク供給装置。
2. 前記算出された複数の把持姿勢は、前記ワークを前記供給対象物に供給可能に学習された一意の把持姿勢と、当該一意の把持姿勢から算出された１以上の把持姿勢であることを特徴とする請求項１記載のワーク供給装置。
3. 前記ワーク把持姿勢計算部は、前記ワークを前記供給対象物に供給可能に算出された複数の把持姿勢から、ある一の把持姿勢を取った場合に、前記ワークがバラ積みされた領域周辺に位置する構造物と前記把持機構とが衝突するか否か、且つ前記移動機構が前記把持機構を前記ある一の把持姿勢まで搬送可能か否かに基づいて、前記ワークの把持姿勢を決定することを特徴とする請求項１または請求項２記載のワーク供給装置。
4. 前記ワーク把持姿勢計算部は、前記決定したワークの把持姿勢について、前記ワークと前記把持機構との対応関係を示す情報を取得し、取得した対応関係を示す情報を統合して得られる情報の分布を作成し、作成した分布を参照して前記ワークを前記供給対象物に供給可能に算出された複数の把持姿勢から、前記ある一の把持姿勢を決定することを特徴とする請求項３記載のワーク供給装置。
5. 前記学習された一意の把持姿勢から、前記ワーク把持姿勢計算部が選択した前記把持するワークを、前記供給対象物に供給可能な把持姿勢の候補である把持姿勢候補を算出し、前記算出した把持姿勢候補について、前記移動機構が前記把持機構を前記供給対象物まで搬送可能か否か、前記供給対象物までの搬送過程において前記把持機構、前記移動機構および前記ワークが、他の構造物と衝突しないか否かの判定を行い、前記１以上の把持姿勢を算出する可操作把持姿勢計算部を備えたことを特徴とする請求項２から請求項４のうちのいずれか１項記載のワーク供給装置。
6. 前記可操作把持姿勢計算部は、前記供給対象物に前記ワークを供給できたか否かを示す予め蓄積された値に基づいて、前記一意の把持姿勢および前記１以上の把持姿勢に重み付けを行い、
   前記ワーク把持姿勢計算部は、前記可操作把持姿勢計算部が付した重みを参照し、前記一意の把持姿勢および前記１以上の把持姿勢から前記ある一の把持姿勢を決定することを特徴とすることを特徴とする請求項５記載のワーク供給装置。
7. ワーク認識部が、バラ積みされたワークを計測した計測データと、予め蓄積された前記ワークの形状データとを照合し、前記ワークの位置および姿勢を算出するステップと、
   ワーク把持姿勢計算部が、前記算出したワークの位置から把持するワークを選択し、前記選択したワークの位置および姿勢と、前記ワークを前記供給対象物に供給可能に算出された前記ワークと前記把持機構との位置関係を示す複数の把持姿勢との対応付けを行い前記ワークの把持姿勢を決定するステップとを備えた把持姿勢計算方法。

Images