特許 6908908 ロボットアームの経路生成装置および経路生成プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6908908

(24)【登録日】2021年7月6日

(45)【発行日】2021年7月28日

(54)【発明の名称】ロボットアームの経路生成装置および経路生成プログラム

(51)【国際特許分類】

   B25J 9/10 20060101AFI20210715BHJP

   G05B 19/4155 20060101ALI20210715BHJP

【ＦＩ】

   B25J9/10 A

   G05B19/4155 T

【請求項の数】8

【全頁数】14

(21)【出願番号】特願2019-186643(P2019-186643)

(22)【出願日】2019年10月10日

(65)【公開番号】特開2021-62416(P2021-62416A)

(43)【公開日】2021年4月22日

【審査請求日】2019年10月18日

(73)【特許権者】

【識別番号】518312725

【氏名又は名称】株式会社トキワシステムテクノロジーズ

(74)【代理人】

【識別番号】100136630

【弁理士】

【氏名又は名称】水野 祐啓

(74)【代理人】

【識別番号】100201514

【弁理士】

【氏名又は名称】玉井 悦

(72)【発明者】

【氏名】今井 倫太郎

【審査官】 篠原 将之

(56)【参考文献】

【文献】 特開２０１９−１５５５３５（ＪＰ，Ａ）

【文献】 特開２０１６−１３２０８６（ＪＰ，Ａ）

【文献】 特開２０１３−０４３２７１（ＪＰ，Ａ）

【文献】 特開２０１３−１３２７４２（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００ − ２１／０２

Ｇ０５Ｂ １９／４１５５

(57)【特許請求の範囲】

【請求項1】

ピッキングロボットの動作経路を生成する装置であって、
ピッキングロボットを制御する制御手段を備え、
ピッキングロボットが、ハンドと、ハンドを支持するアームと、を含み、
前記制御手段が、ハンドの位置情報および姿勢情報を含むポーズを生成するポーズ生成手段を含み、
前記ポーズが、ハンドがワークを把持する把持ポーズの候補と、ハンドが前記把持ポーズの前に形成する接近ポーズと、ハンドが前記把持ポーズの後に形成する離脱ポーズと、を含み、
前記ポーズ生成手段は、所定の干渉条件に基づいて物体同士の干渉を検出し、
前記干渉条件が、ハンドを仮想的に表すＨ領域と、アームを仮想的に表すＡ領域と、ワークの収容容器を仮想的に表すＹ領域と、ワークおよびワークを把持したハンドを仮想的に表すＨＷ領域と、Ｈ，Ａ，Ｙ，ＨＷ領域に各々所定のマージンを付加したＨ，Ａ，Ｙ，ＨＷ拡張領域と、ハンド先端から所定距離に含まれるＪ領域と、について、
（条件１）Ｈ，Ａ，Ｙ領域が相互に干渉しないこと、
（条件２）Ｈ，Ａ，Ｙ，ＨＷ領域が相互に干渉しないこと、
（条件３）Ｈ，Ａ，Ｙ領域が相互に干渉しないこと、または、干渉した場合であっても、干渉位置がＪ領域に該当すること、
（条件４）Ｈ，Ａ，Ｙ拡張領域が相互に干渉しないこと、
（条件５）Ｈ，Ａ，Ｙ，ＨＷ拡張領域が相互に干渉しないこと、
のいずれか一つ以上を含み、
前記制御手段は、前記干渉条件を含む所定の条件を満たす前記ポーズに基づいて前記動作経路を生成することを特徴とする経路生成装置。

【請求項2】

前記ポーズ生成手段が、前記候補を生成する候補生成手段を含み、
前記候補生成手段は、ワークの位置と、ワークの軸方向と、ワークの大きさと、に基づいて一の前記候補を生成し、該一の候補を所定の回転軸周りに回転させて他の複数の候補を生成する請求項１に記載の経路生成装置。

【請求項3】

前記ポーズ生成手段は、前記候補に基づいて前記接近ポーズおよび前記離脱ポーズを生成する請求項１または２に記載の経路生成装置。

【請求項4】

前記ポーズ生成手段は、生成したポーズについて逆運動学の解を算出し、解が存在しないポーズに紐づく前記候補を除外する請求項１〜３のいずれか一項に記載の経路生成装置。

【請求項5】

前記ポーズ生成手段は、干渉を検出したポーズに紐づく前記候補を除外する請求項１〜４のいずれか一項に記載の経路生成装置。

【請求項6】

前記ポーズ生成手段は、生成したポーズについて、
（イ）接近ポーズの関節位置で（条件３）を満たすこと、
（ロ）候補の関節位置で（条件３）を満たすこと、
（ハ）離脱ポーズの関節位置で（条件３）および（条件５）を満たすこと、
を検査し、（イ）〜（ハ）のいずれか一つを満たさない場合に、該ポーズに紐づく候補を除外する請求項１〜５のいずれか一項に記載の経路生成装置。

【請求項7】

前記ポーズ生成手段は、前記候補に基づいて、ハンドが前記接近ポーズから把持ポーズに向かう接近動作およびハンドが前記把持ポーズから離脱ポーズに向かう離脱動作と、を生成し、
前記ポーズ生成手段は、生成した各動作について、
（二）接近動作を時系列に細分し、各時間毎の関節位置において（条件３）を満たすこと、
（ホ）離脱動作を時系列に細分し、各時間毎の関節位置において（条件３）を満たすこと、
を検査し、（二）または（ホ）のいずれか一方を満たさない場合に、該動作に紐づく候補を除外する請求項１〜６のいずれか一項に記載の経路生成装置。

【請求項8】

コンピュータにおいて実行されることにより、当該コンピュータを、請求項１〜７のいずれか一項に記載の経路生成装置として機能させる経路生成プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ランダムに山積みされたワークをロボットを用いてピッキングする際に、ロボットの動作経路を生成する経路生成装置および経路生成プログラムに関する。

【背景技術】

【0002】

従来より、ランダムに山積みされたワークを撮影し、撮影画像を解析してワークを認識し、認識結果に基づいてロボットアームの移動経路を生成するランダムピッキングの技術が知られている。例えば、特許文献１には、ワークの形状モデルを用いてロボットハンドの把持位置の候補を選出し、選出した候補に基づいてロボットハンドの動作経路を生成する技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００８−２７２８８６号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかし、特許文献１の技術によれば、ピッキングを開始する前に、ロボットにワークを登録してワークの形態を記憶させる必要があるため、手間と工数がかかるという問題があった。また、現場にワークを登録できる技術者がいない場合、作業進められないという問題もあった。

【0005】

そこで、本発明の目的は、ワークを登録する手間を排し、自動的にロボットの動作経路を生成できる経路生成装置および経路生成プログラムを提供することにある。

【課題を解決するための手段】

【0006】

上記課題を解決するために、本発明の経路生成装置は、以下の特徴を備える。
（１）ピッキングロボットの動作経路を生成する装置であって、ピッキングロボットを制御する制御手段を備え、ピッキングロボットが、ハンドと、ハンドを支持するアームと、を含み、制御手段が、ハンドの位置情報および姿勢情報を含むポーズを生成するポーズ生成手段を含み、制御手段は、所定の条件を満たすポーズに基づいて動作経路を生成する。

【0007】

（２）ポーズには、ハンドがワークを把持する把持ポーズの候補が含まれ、ポーズ生成手段が、候補を生成する候補生成手段を含み、候補生成手段は、ワークのポーズおよびワークの大きさに基づいて一の候補を生成し、該一の候補を所定の回転軸周りに回転させて他の複数の候補を生成する。

【0008】

（３）ポーズには、ハンドが把持ポーズの前に形成する接近ポーズと、ハンドが把持ポーズの後に形成する離脱ポーズと、が含まれ、ポーズ生成手段は、候補に基づいて接近ポーズおよび離脱ポーズを生成する。

【0009】

（４）ポーズ生成手段は、生成したポーズについて逆運動学の解を算出し、解が存在しないポーズに紐づく候補を除外する。

【0010】

（５）ポーズ生成手段は、所定の干渉条件に基づいて物体同士の干渉を検出し、干渉を検出したポーズに紐づく候補を除外する。

【0011】

（６）このとき、干渉条件には、ハンドを仮想的に表すＨ領域と、アームを仮想的に表すＡ領域と、ワークの収容容器を仮想的に表すＹ領域と、ワークおよびワークを把持したハンドを仮想的に表すＨＷ領域と、Ｈ，Ａ，Ｙ，ＨＷ領域に各々所定のマージンを付加したＨ，Ａ，Ｙ，ＨＷ拡張領域と、ハンド先端から所定距離に含まれるＪ領域と、について、
（条件１）Ｈ，Ａ，Ｙ領域が相互に干渉しないこと、
（条件２）Ｈ，Ａ，Ｙ，ＨＷ領域が相互に干渉しないこと、
（条件３）Ｈ，Ａ，Ｙ領域が相互に干渉しないこと、または、干渉した場合であっても、干渉位置がＪ領域に該当すること、
（条件４）Ｈ，Ａ，Ｙ拡張領域が相互に干渉しないこと、
（条件５）Ｈ，Ａ，Ｙ，ＨＷ拡張領域が相互に干渉しないこと、が含まれる。

【0012】

（７）ポーズ生成手段は、生成したポーズについて、
（イ）接近ポーズの関節位置で（条件３）を満たすこと、
（ロ）候補の関節位置で（条件３）を満たすこと、
（ハ）離脱ポーズの関節位置で（条件３）および（条件５）を満たすこと、
を検査し、（イ）〜（ハ）のいずれか一つを満たさない場合に、該ポーズに紐づく候補を除外する。

【0013】

ポーズ生成手段は、候補に基づいて、ハンドが接近ポーズから把持ポーズに向かう接近動作およびハンドが把持ポーズから離脱ポーズに向かう離脱動作と、を生成し、ポーズ生成手段は、生成した各動作について、
（二）接近動作を時系列に細分し、各時間毎の関節位置において（条件３）を満たすこと、
（ホ）離脱動作を時系列に細分し、各時間毎の関節位置において（条件３）を満たすこと、
を検査し、（二）または（ホ）のいずれか一方を満たさない場合に、該動作に紐づく候補を除外する。

【0014】

また、上記課題を解決するために、本発明の経路生成プログラムは、上記（１）〜（８）の何れか一つに記載の制御手段を備える。

【発明の効果】

【0015】

本発明の経路生成装置および経路生成プログラムによれば、ワークのポーズおよびワークのサイズに基づいて把持ポーズを生成し、生成した把持ポーズに基づいて自動的にピッキングロボットの動作経路を生成するため、あらかじめワークを登録する必要がなく、手間と工数を削減できるという優れた効果を有する。

【図面の簡単な説明】

【0016】

【図1】本発明の一実施形態を示す経路生成装置を利用したピッキングシステムの概略図である。

【図2】図１のピッキングシステムのブロック図である。

【図3】点群データの法線ベクトルから軸候補点を推定する処理を示す模式図である。

【図4】（ａ）仮半径ｒ_i（ｒ_min≦ｒ_i≦ｒ_max：ｉ＝０〜１００）から生成されたモデルを、軸候補点を原点Ｏとするｚ軸周りに回転させる様子を示す模式図である。（ｂ）仮半径ｒ_i毎に一致度の平均値をプロットしたグラフである。

【図5】ワークの把持位置を示す（ａ）斜視図、（ｂ）側面図、（ｃ）正面図である。

【図6】把持ポーズの候補の生成についての説明図である。

【図7】接近ポーズおよび離脱ポーズの生成についての説明図である。

【図8】（ａ）条件１に関するＨ，Ａ，Ｙ領域、（ｂ）条件４に関するＨ，Ａ，Ｙ拡張領域を示す模式図である。

【図9】（ａ）条件２に関するＨ，Ａ，Ｙ，ＨＷ領域、（ｂ）条件５に関するＨ，Ａ，Ｙ，ＨＷ拡張領域を示す模式図である。

【図10】（ａ）条件３に関するＨ，Ａ，Ｙ，Ｊ領域、（ｂ）Ｊ領域の拡大図を示す模式図である。

【図11】経路生成装置、経路生成プログラムの処理の流れを示すフローチャートである。

【図12】ワークのポーズとサイズを算出する処理の流れを示すフローチャートである。

【発明を実施するための形態】

【0017】

以下、本発明をピッキングシステムに用いる演算用ＰＣに具体化した一実施形態を図面に基づいて説明する。以下の説明において、ピッキングシステム全体を基準とするワールド座標系、各々のワークＷを基準とするワーク座標系、ピッキングロボット３のハンド３２を基準とするハンド座標系を用いる。詳しくは、ワールド座標系は、ピッキングロボット３の背面から前方へ向かう方向をｘ軸正方向、正面視左側から右側へ向かう向きをｙ軸正方向、鉛直方向下から上に向かう方向をｚ軸正方向とし、ワーク座標系は、円柱形ワークＷの中心Ｏを通過する中心軸に沿って紙面奥から手前に向かう向きをｘ軸正方向、円柱の紙面向かって左側面から右側面に向かう方向をｙ軸正方向、鉛直方向下から上に向かう方向をｚ軸正方向とし（図５，６参照）、ハンド座標系は、ハンド３２の基端から先端に向かう方向をｚ軸正方向とする（図７参照）。また、「ポーズ」は、所定のｘｙｚ空間のある一点を示す位置と、ｘｙｚ軸を回転軸とする回転量を示す姿勢の二つの要素を含むものとする。

【0018】

図１は、ピッキングシステム１の概略図である。ピッキングシステム１は、ランダムに積まれたワークＷをピッキングするピッキングロボット３と、ワークＷ表面の三次元形状をスキャンする３Ｄスキャナ４と、３Ｄスキャナ４から受信した計測中間データに基づいてピッキングロボット３が動作する経路を演算する経路生成装置として機能する演算用ＰＣ２から構成される。このとき、演算用ＰＣ２に経路生成プログラムをインストールして機能を実装しても良い。また、３Ｄスキャナ４は、ワークＷの収容容器としてのパレット７をスキャンし、パレット７の２Ｄデータおよび３Ｄデータを取得するためにも用いられる。

【0019】

ピッキングロボット３は、ワークＷを把持するハンド３２とハンド３２を所定の姿勢に支持するアーム３１から構成される。ハンド３２を制御するハンドコントローラ６と、ピッキングロボット３を制御するロボットコントローラ５は、演算用ＰＣ２に接続され、演算用ＰＣ２から受信した制御信号に基づいて、ハンド３２またはピッキングロボット３を作動させる。このとき、経路生成装置をロボットコントローラ５またはハンドコントローラ６に搭載すること、経路生成プログラムをロボットコントローラ５またはハンドコントローラ６にインストールすることも採用できる。

【0020】

図２に示すように、演算用ＰＣ２は、ピッキングロボット３を制御する制御部１１と、制御部１１が算出したデータを保存する記憶部１２と、３Ｄスキャナ４、ロボットコントローラ５、ハンドコントローラ６等の外部装置との間で信号やデータをやりとりするインターフェース（図示なし）から構成される。

【0021】

制御部１１は、パレット７の３Ｄデータに基づいてパレット７を検出するパレット検出部２１と、把持する対象となるワークＷの位置、軸方向（姿勢）、大きさを認識するワーク認識部１３と、ピッキングロボット３の動作経路を生成する経路生成装置または経路生成プログラムとして機能する経路生成部２２から構成される。なお、このときの動作経路とは、ピッキングロボット３がワークＷを把持する動作経路を示す。

【0022】

パレット検出部２１は、記憶部１２から、パレット縁部を抽出した点群データを読み出し、スキャンされた３Ｄデータと照合して、パレット７を検出し、パレット７の位置決めを実施する。なお、記憶部１２には、当該画像データのほか、パレット７の深さ情報があらかじめ登録されている。

【0023】

ワーク認識部１３は、３Ｄスキャナ４から受信した計測中間データを点群データＰに変換する点群データ生成部１４と、点群データＰに基づいてワークＷのサイズを推定するサイズ推定部１５と、推定したサイズをパラメタとしてモデルＭを生成するモデル生成部１６と、生成したモデルＭを用いて軸方向を推定する軸方向推定部１７から構成される。

【0024】

経路生成部２２は、ピッキングロボット３がワークＷを把持するポーズを生成する把持ポーズ生成部２４から構成され、把持ポーズ生成部２４は、把持ポーズの候補となる複数のポーズを生成する候補生成部２５を含む。

【0025】

ここで、点群データＰとは、ワールド座標系における三次元点情報の配列（ｘ，ｙ，ｚ）のことを言う。点群データＰの取得の流れは、以下の通りである。
（１）ＰＣ２が、３Ｄスキャナ４へ計測トリガを発行する。
（２）３Ｄスキャナ４が、計測対象物の表面の三次元形状をスキャンする。
（３）３Ｄスキャナ４が、計測中間データを演算用ＰＣ２へ送信する。
（４）演算用ＰＣ２が、計測中間データを点群データＰに変換する。
なお、３Ｄスキャナ４が、点群データＰへの変換を実施する場合もある。しかし、点群データＰの生成は計算量が多いため、一般的には演算用ＰＣ２にて、専用ライブラリを用いて点群データＰに変換される。また、三次元形状のスキャン手法としては、例えば、プロジェクタ方式、ステレオカメラ方式等が挙げられる。

【0026】

モデル生成部１６は、パラメタとして半径ｒを入力し、半径ｒ、高さｒの半円柱の側面部からなるワークＷのモデルを生成する。

【0027】

以下、ワーク認識部１３の構成について説明する。
軸方向推定部１７は、図３（ａ）に示すように、まず、点群データＰに含まれる各々の点ｐについて法線ベクトルｎを推定する。そして、点ｐを法線ベクトルｎと逆方向に半径ｒ分進めた点ｑを、法線ベクトルｎの曲面を側面に含む円柱の軸候補点とし、軸候補点ｑを記憶部１２に登録する。なお、点ｑは、下記の数式１により求められる。

【0028】

【数1】

【0029】

次に、軸方向推定部１７は、軸候補点ｑの集積度を算出し、集積度の高い点ｑを特定する。そして、あらかじめ生成したモデルＭの原点Ｏを、点ｑに配置し、モデルＭと点群データＰとの一致度Ｄを算出する。一致度Ｄは、「モデルＭ付近に配置された点ｐの数」を、「モデルＭを側面とする半径ｒ、高さｒの半円柱に含まれる点ｐの総数」で除算することにより求められる。軸方向推定部１７は、モデルＭを、ｚ軸周りに０度から１８０度まで回転させ、ｚ角度５度毎に一致度Ｄを算出して保持する。そして、最も一致度Ｄの高かったｚ角度をモデルＭの軸方向として記憶部１２に登録する。

【0030】

サイズ推定部１５は、図４（ａ）に示すように、最小半径ｒ_minと最大半径ｒ_maxを仮定し、下記の数式２により求められるｒ_i（ｒ_min≦ｒ_i≦ｒ_max：ｉ＝０〜１００）を半径とするモデルＭ_iをそれぞれ生成し、モデルＭ_i毎に軸方向を推定する。モデルＭ_iの生成および軸方向の推定は、モデル生成部１６および軸方向推定部１７による処理と同じであるため説明を省略する。

【0031】

【数2】

【0032】

その後、サイズ推定部２５は、各半径ｒ_iおよび集積度の高い点ｑについて、モデルＭ_iと点群データＰとの一致度Ｄ_iを算出する。一致度Ｄ_iは、一致度Ｄと同様に、「モデルＭ_i付近に配置された点ｐの数」を、「モデルＭ_iを側面とする半径ｒ_i、高さｒ_iの半円柱に含まれる点ｐの総数」で除算することにより求められる。

【0033】

サイズ推定部２５は、モデルＭ_iを、ｚ軸周りに０度から１８０度まで回転させ、ｚ角度５度毎および集積度の高い軸候補点ｑ毎に一致度Ｄ_i（ｒ_i，ｑ）を求める。そして、サイズ推定部２５は、一致度Ｄ_i（ｒ_i，ｑ）の最大値maxＤ_i（ｒ_i，ｑ）を求め、さらに、maxＤ_i（ｒ_i，ｑ）をｑに関して平均して、平均値maxＤ_i（ｒ_i）を求める。その後、サイズ推定部２５は、maxＤ_i（ｒ_i）が最大になる半径ｒ_iを実半径ｒとして出力する。

【0034】

図４（ｂ）に、maxＤ_i（ｒ_i）をプロットしたグラフを示す。ここでは、ｒ_min＝２．０mm、ｒ_max＝８．０mmとし、半径ｒ_iを０．０６mm増加させた毎のmaxＤ_iをプロットしている。この例では、maxＤ_iが最も大きいｒ_iである４．６ｍｍ付近が、ワークＷの実半径ｒと推定される。

【0035】

記憶部１２は、一致度の高い順にモデルＭを記憶している。詳しくは、点群データＰには複数のワークＷの表面形状が含まれているため、記憶部１２では、それぞれのワークＷと同じ軸方向を向くモデルＭを算出し、一致度の高い順にモデルＭを記憶する。

【0036】

続いて、経路生成部２２の構成について説明する。
まず、図５に示すように、ポーズ生成部２４の候補生成部２５は、モデルＭの原点Ｏの座標（ｘ_o，ｙ_o，ｚ_o）、軸方向のベクトル、半径ｒに基づいて、把持ポーズの候補を一つ生成する。また、把持位置は、ハンド３２がワークＷを支持する支持方法により定まる。例えば、吸着式ハンドの場合は、把持位置は、位置Ｐａ（ｘ_o，ｙ_o，ｚ_o＋ｒ）となり、挟み込み式ハンドの場合は、把持位置は位置Ｐｂ（ｘ_o，ｙ_o，ｚ_o）となり、すくい上げ式ハンドの場合は、位置Ｐｃ（ｘ_o，ｙ_o，ｚ_o−ｒ）となる。このとき、把持位置Ｐａ，Ｐｂ，Ｐｃは、実際のハンド３２との接触点ではなく、例えば、挟み込み式ハンドの場合は左右の爪の中点となり、「し」型のすくい上げ式ハンドの場合は「し」の下端を示す。候補生成部２５は、これらの位置Ｐａ，Ｐｂ，Ｐｃのいずれかに基づいて一つの把持ポーズの候補５６を生成する。

【0037】

図６に示すように、候補生成部２５は、ワークＷのポーズおよび大きさに基づいて一の候補５６を生成し、生成した一の候補５６を所定の軸周りに回転させ、所定の刻み角度毎に他の候補５６を生成する。具体的には、一の候補５６を、位置はモデルＭの原点Ｏ、姿勢はワーク座標系のｘ軸（モデルＭの軸方向）、大きさはモデルＭの半径ｒに基づいて生成する（図６（ａ））。そして、一の候補５６を、中心軸（ワーク座標系のｘ軸、＋方向）周りに−９０〜＋９０度の範囲で５度刻みに回転させ、他の複数の候補５６を生成する。次に、先程の一の候補５６を、ワーク座標系のＷｚを中心軸として１８０度回転させ、さらに新たな一の候補５６を生成する（図６（ｂ）。そして、該新たな一の候補５６を、中心軸（ワーク座標系のｘ軸、−方向）周りに−９０〜＋９０度の範囲で５度刻みに回転させ、さらなる他の複数の候補５６を生成し、最終的に、７３個の候補５６を生成する。なお、当該候補生成方法は、図６（ｃ）に示すように、面状のワークおよびリング状のワークに応用できる。例えば、面状のワークを吸着したり、リング状のワークに差し込むポーズの一の候補を生成し、ワーク座標系におけるｚ軸を中心軸として、−１８０度〜＋１８０度の範囲を５度刻みに回転させ、７３個の候補５６を生成できる。なお、刻み角度は、適宜変更可能である。

【0038】

このようにして生成された各々の候補に基づいて、図７に示すように、ポーズ生成部２４は、ハンド３２が候補５６まで接近する接近ポーズ５２と、ハンド３２が候補５６から離脱する離脱ポーズ５４を生成する。具体的には、接近ポーズ５２は、候補５６のポーズを基点としてハンド３２の先端部から基端部に向かう方向（ハンド座標系ｚ軸、−方向）に、所定距離移動させたものであり、離脱ポーズ５４は、候補５６のポーズを基点として鉛直上方向（ワールド座標系ｚ軸、＋方向５８）に所定距離移動したものである。

【0039】

また、このとき、ポーズ生成部２４は、候補５６と接近ポーズ５２との間に生成した直線補間動作を接近動作５７とし、候補５６と離脱ポーズ５４との間に生成した直線補間動作を離脱動作５８とする。

【0040】

ここで、図８〜１０に示すように、記憶部１２では、物体同士の干渉を検出するための所定の領域情報が保存されている。領域情報には、ハンドを仮想的に表すＨ領域と、アームを仮想的に表すＡ領域と、ワークの収容容器を仮想的に表すＹ領域と、ワークＷおよびワークＷを把持したハンド３２を仮想的に表すＨＷ領域と、Ｈ，Ａ，Ｙ，ＨＷ領域に所定のマージンを付加したＨ，Ａ，Ｙ，ＨＷ拡張領域と、ハンド３２先端から所定距離ｄに含まれるＪ領域が含まれる。

【0041】

次に、ポーズ生成部２４は、生成した各々の候補５６と、接近ポーズ５２と、離脱ポーズ５４とについて、以下（条件１）〜（条件５）の干渉条件について、（イ）〜（ホ）の検査を実施する。ポーズ生成部２４は、（イ）〜（ホ）のいずれかの検査が不合格である場合は、検査対象の候補５６を除外する。
（条件１）Ｈ，Ａ，Ｙ領域が相互に干渉しないこと（図８（ａ）参照）。
（条件２）Ｈ，Ａ，Ｙ，ＨＷ領域が相互に干渉しないこと（図９（ａ）参照）。
（条件３）Ｈ，Ａ，Ｙ領域が相互に干渉しないこと、または、干渉した場合であっても、干渉位置がＪ領域に該当すること（図１０参照）。
（条件４）Ｈ，Ａ，Ｙ拡張領域が相互に干渉しないこと（図８（ｂ）参照）。
（条件５）Ｈ，Ａ，Ｙ，ＨＷ拡張領域が相互に干渉しないこと（図９（ｂ）参照）。
（イ）接近ポーズ５２について逆運動学の解が存在し、接近ポーズ５２の関節位置で、（条件３）を満たすこと。
（ロ）候補５６について逆運動学の解が存在し、候補５６の関節位置で、（条件３）を満たすこと。
（ハ）離脱ポーズ５５について逆運動学の解が存在し、離脱ポーズ５５の関節位置で、（条件３）および（条件５）を満たすこと。
（二）接近動作を時系列に細分し、各時間毎の関節位置において（条件３）を満たすこと。
（ホ）離脱動作を時系列に細分し、各時間毎の関節位置において（条件３）を満たすこと。
このとき、（ハ）の検査が不合格であった場合には、離脱ポーズ５４をパレット７の中央位置に向けて、ワールド座標系のxy方向に所定距離分、平行移動させ、（ハ）について再検査することが好ましい。パレット７の隅に配置されたワークＷをパレット７から引き離し、ワークＷとパレット７との干渉を回避するように調整することが可能である。また、時系列に細分化する場合には、ピッキングロボット３の動作の０．１％ずつ、つまり移動距離２〜３ｍｍごとに検査することが好適である。（イ）〜（ホ）の検査を実施する際のマージン量は、２ｍｍ以下であることが望ましい。

【0042】

最後に、制御部１１は、ポーズ生成部２４が生成し、ポーズ生成部２４により除外されなかった候補５６に基づいて、ピッキングロボット３の動作する経路を生成する。このとき、制御部１１は、あらかじめ記憶部１２に保存されている開始ポーズ５１と終了ポーズ５５を読み出し、開始ポーズ５１から接近ポーズ５２に向かう経路を、（条件４）の下で自動生成する。また、離脱ポーズ５４から終了ポーズ５５に向かう経路を、（条件５）の下で自動生成する。開始ポーズ５１から接近ポーズ５２に向かう経路および離脱ポーズ５４から終了ポーズ５５に向かう経路を自動生成する際のマージン量は、５ｍｍ程度であることが望ましい。

【0043】

経路の自動生成は、例えば、ＫＰＩＥＣＥアルゴリズムを用いることができる。また、除外されなかった候補５６が複数存在する場合には、これらの候補について並列的に経路を自動生成し、最も速く生成された経路を実際のピッキング経路として採用する。一方、候補５６が全て除外された場合や、所定の時間内に経路が生成されなかった場合には、経路の生成を中止し、次に一致度の高いモデルＭを取り出す。

【0044】

次に、上記構成の経路生成部２２の処理の流れについて、図１１に基づいて説明する。まず、３Ｄスキャナ４が、ワークＷがランダムに山積みされたパレット７をスキャンし、スキャンされた３Ｄデータに基づいてパレット７を検出する（Ｓ１）。次に、３Ｄスキャナ４は、ワークＷをスキャンし、ワーク認識部１３が、スキャン結果に基づいてワークＷを認識し、認識結果としてのモデルＭを一致度の高い順に記憶部１２に保存する（Ｓ２）。ワークＷの認識が終了すると、処理は経路生成部２２が引き継ぐ。

【0045】

経路生成部２２において、ポーズ生成部２４の候補生成部２５は、記憶部１２から最も一致度の高いモデルＭを一つ取り出し（Ｓ３）、モデルＭに基づいて把持ポーズの候補５６を生成する（Ｓ４）。候補５６が生成されると、ポーズ生成部２４は、全ての候補５６について検査を実施し、検査に不合格であった候補５６を除外する（Ｓ５）。全ての候補５６が除外された場合は（Ｓ６：Ｎｏ）、次に一致度の高いモデルＭを一つ取り出す（Ｓ３）。候補５６が残った場合は、制御部１１は、全ての候補５６について並列的に経路の自動生成を実施する。全ての候補５６について、所定の時間内に経路が生成されなかった場合には（Ｓ７：Ｎｏ）、次に一致度の高いモデルＭを一つ取り出す（Ｓ３）。一方、所定の時間内に経路が生成された場合には、最も速く経路を生成できた経路を出力する。その後、ピッキングロボット３は出力された経路に従って動作するように制御され、ピッキングが実施される（Ｓ８）。

【0046】

ここで、ワーク認識部１３の処理の流れについて、図１２に基づいて説明する。３Ｄスキャナ４がワークＷをスキャンし、演算用ＰＣ２に計測中間データを送信する。計測中間データを受信した演算用ＰＣ２は、点群データＰを生成し（Ｓ２１）、半径をｒ_min〜ｒ_maxと仮定してサイズを推定する（Ｓ２２）。その後、実半径ｒを用いてモデルを生成し（Ｓ２３）、モデルＭを点群データＰにあてはめて軸方向を推定する（Ｓ２４）。ここで、ワーク認識は、実半径ｒを用いたモデルの生成および軸方向の推定を主要部とする。この主要部となる処理は、点群データＰに含まれるワークＷの全てについて実施され、記憶部１２では、一致度が高い順にモデルＭをソートして保存する（Ｓ２５）。

【0047】

以上の構成の経路生成装置および経路生成プログラムによれば、ワークＷのポーズである位置、軸方向（姿勢）、大きさに基づいて把持ポーズ５３を生成し、生成した把持ポーズ５３をワークＷの軸周りに回転させて自動的に候補５６を生成するため、ワークＷの登録が不要となり、手間と工数を削減できるという優れた効果を有する。また、生成した候補５６に基づき、Ｈ，Ａ，Ｙ，ＨＷ拡張領域に基づいて経路を自動生成するため、実際にピッキングロボット３を作動させた場合に、アーム３１、ハンド３２、ワークＷ、パレット７との間の接触や衝突をより良く防止することが可能である。さらに、（条件３）を設け、領域Ｊにおける干渉については無視することとしたため、過敏な検出を排除して、パレット７の隅に配置されたワークＷまでハンド３２の先端を届かせて、ピッキングすることができるという効果も有する。

【0048】

その他、本発明は、上記実施形態に限定されるものではなく、発明の趣旨を逸脱しない範囲で、各部の構成を任意に変更して実施することも可能である。例えば、マージンの大きさについて、領域毎に異なる値に設けることも可能である。

【符号の説明】

【0049】

１ ピッキングシステム
２ 演算用ＰＣ
３ ピッキングロボット
４ ３Ｄスキャナ
５ ロボットコントローラ
６ ハンドコントローラ
７ パレット
１１ 制御部
１２ 記憶部
１３ ワーク認識部
１４ 点群データ生成部
１５ サイズ推定部
１６ モデル生成部
１７ 軸方向推定部
２１ パレット検出部
２２ 経路生成部
２４ ポーズ生成部
２５ 候補生成部
３１ アーム
３２ ハンド
５１ 開始ポーズ
５２ 接近ポーズ
５３ 把持ポーズ
５４ 離脱ポーズ
５５ 終了ポーズ
５６ 把持ポーズの候補
５７ 接近動作
５８ 離脱動作
Ｓ 領域（Ｓｈ：Ｈ領域、Ｓｈ’：Ｈ拡張領域、Ｓａ：Ａ領域、Ｓａ：Ａ拡張領域、Ｓｙ：Ｙ領域、Ｓｙ：Ｙ拡張領域、Ｓｈｗ：ＨＷ領域、Ｓｈｗ’：ＨＷ拡張領域、Ｓｊ：Ｊ領域、Ｓｍ：マージン）
Ｏ 原点
Ｐ 点群データ
Ｍ モデル
Ｗ ワーク
ｑ 軸候補点
ｐ 点群データに含まれる各点
ｒ 半径
ｎ 法線ベクトル

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】