WO2018092860A1 - 干渉回避装置 - Google Patents

Abstract

ツール部が周辺機器に干渉することを回避させる干渉回避装置（２）は、周辺機器の状態を計測して得られた計測データ（１ａ）とロボットおよび周辺機器のモデル情報（３ａ）とに基づき、ツール部の把持対象をツール部が把持可能な複数の把持点候補（１０ａ）を算出する把持点候補生成部（１０）と、複数の把持点候補（１０ａ）に基づき、ツール部が周辺機器に干渉することなく把持対象に接近する干渉回避軌道（２ａ）を算出する軌道算出部（１１）とを備える。

Description

干渉回避装置

　本発明は、ロボットが周辺機器に干渉することを回避する干渉回避装置に関する。

　干渉回避装置は、ロボットおよび周辺機器の各々が動作する占有空間の重複の有無により、ロボットと周辺機器とが干渉する可能性があるか否かの干渉チェックを実行する。そして動作計画時または動作実行時において干渉チェックにより、ロボットが周辺機器に干渉する可能性があると判断したとき、干渉回避装置は、プログラムで予め計画された動作軌道を異なる動作軌道に修正し、修正した動作軌道を目標軌道としてロボットを制御する。修正した動作軌道に合わせてロボットを制御することを干渉回避動作と称する。これにより、ロボットが当該ロボットの周辺に配置された他のロボットまたは周辺機器に干渉し、ロボットまたは周辺機器が故障することを未然に防止でき、またロボットによる作業が失敗することを未然に防止できる。

　従来の干渉回避装置は、デカルト座標系において干渉チェックを行う。または、従来の干渉回避装置は、コンフィギュレーション空間において干渉チェックを行い、干渉回避動作を実現する回避軌道を生成する。具体的には、干渉回避装置は、デカルト座標系上の空間において干渉が見つかった場合、中間軌道を修正して回避軌道を算出する。または干渉回避装置は、関節角度で表現されたコンフィギュレーション空間において干渉が見つかった場合、コンフィギュレーション空間中で回避軌道を探索する。これらの手法は、干渉が発生する経路をロボットが通ると判定されたとき、これを避けて通るように修正することを基本とした手法である。干渉回避動作を実現するための計算処理では、計算処理量が増加し、計算時間が一定以上必要となる代わりに確実に干渉しない経路を生成できる。

　一方、従来の干渉回避装置では、拘束条件が明示的に設定されていないため、単にロボットが干渉しないことが満たされても、ロボットによる作業成功率またはロボットの動作速度といった作業品質に影響を与える条件への考慮が不十分である。すなわち従来の干渉回避装置では、作業を実行するのに最適な軌道であるかという点が十分に検討されていない。最適な軌道とは、ロボットによる作業を短い時間で実行できる、特異点を通過しない軌道を取ることができる、作業成功率を最も高めることができる、ということを総合的に判断した目的位置および中間軌道を定めた作業軌道のことである。前述した回避軌道は、この作業軌道のサブセットに位置付けることができ、干渉が生じる場合を含む。

　近年、特にロボットによる組立作業の自動化の分野では、部品供給箱の中から部品を取り出すビンピッキングが盛んに行われている。ビンピッキングとは、ランダムに積み重ねられた複数の部品の中から特定の部品を把持して、特定の場所に運ぶことである。ビンピッキングでは目的位置が定まっておらず、部品の位置および姿勢に対応したロボットの動作制御が必要である。ビンピッキングにおいても前述した干渉チェックおよび干渉回避動作が必要である。

　特許文献１にはビンピッキング作業において干渉チェックおよび干渉回避動作を実現する演算方法が開示されている。特許文献１に開示される干渉回避装置は、収納箱内に配置された複数の部品の中からロボットが１つの部品を取り出す際、３次元計測器で部品の把持点候補を検出し、その把持点候補に対して部品を把持する指先を含めたロボットのツール部の位置姿勢を算出し、収納箱の内、部品に最も近い部分とツール部との干渉チェックを行う。また特許文献１に開示される干渉回避装置は、干渉チェックの結果、干渉していると判断されたとき、許容される範囲内でツール部が干渉しなくなるように目標位置姿勢を変更する。

特開２００２－３３１４８０号公報

　しかしながら特許文献１に開示される干渉回避装置では、ロボットが周辺機器に干渉しない把持姿勢に修正されるとき、予め設定された把持姿勢とは異なる姿勢に補正されるため、干渉を回避できるものの、予め設定された把持姿勢に近い姿勢に補正するだけでは部品の把持に失敗することがあり、干渉を回避しながら作業の成功率の低下を抑制することが困難であった。

　本発明は、上記に鑑みてなされたものであって、干渉を回避しながら作業の成功率の低下を抑制できる干渉回避装置を得ることを目的とする。

　上述した課題を解決し、目的を達成するために、本発明の干渉回避装置は、少なくとも周辺機器の状態を計測した計測データが入力され、ロボットのアーム部に設けられたツール部が周辺機器に干渉することを回避させる干渉回避装置であって、計測データと、ロボットおよび周辺機器のモデル情報とに基づき、ツール部の把持対象をツール部が把持可能な複数の把持点候補を算出する把持点候補生成部と、複数の把持点候補に基づき、ツール部が周辺機器に干渉することなく把持対象に接近する干渉回避軌道を算出する軌道算出部とを備える。軌道算出部は、複数の把持点候補に対し、ツール部が周辺機器に干渉することなく、かつ許容姿勢範囲内に存在する複数の把持姿勢候補を生成する把持姿勢生成部と、生成された把持姿勢候補の中で把持対象をずらすことなくツール部が把持対象に接近できる把持姿勢を抽出し、抽出された把持姿勢となるための複数の把持点候補を生成し、把持点候補のそれぞれを端点に持つ複数の軌道を第１の接近軌道として生成する第１の接近軌道生成部と、第１の接近軌道の端点のうち把持対象から遠い位置にある第１の端点に、ロボットのアーム部およびツール部が周辺機器に干渉することなく接近する第２の接近軌道を生成する第２の接近軌道生成部と、第１の接近軌道と第２の接近軌道とを接続した干渉回避軌道を生成する回避軌道生成部とを備えることを特徴とする。

　本発明によれば、干渉を回避しながら作業の成功率の低下を抑制できるという効果を奏する。

本発明の実施の形態１に係る干渉回避装置を備えたロボットシステムの構成図 図１に示す干渉回避装置と干渉回避装置の周辺機器とを備えた生産システムの構成図 図１に示す部品箱の上方に位置するロボットハンドが把持対象の物体に接近するときの把持姿勢と第１の接近軌道とを説明するための図 本発明の実施の形態１に係る干渉回避装置の構成図 本発明の実施の形態１に係る干渉回避装置において第１の接近軌道および把持接近開始点の定義方法の例を示す図 図４に示す軌道算出部の構成図 図６に示す干渉チェック部の動作を説明するための図 図６に示す把持姿勢生成部の動作を説明するための図 図６に示す第１の接近軌道生成部の動作を説明するための図 図６に示す第２の接近軌道生成部の動作を説明するための第１の図 図６に示す第２の接近軌道生成部の動作を説明するための第２の図 図６に示す第２の接近軌道生成部の動作を説明するための第３の図 図６に示す第２の接近軌道生成部の動作を説明するための第４の図 図６に示す回避軌道生成部の動作を説明するための図 本発明の実施の形態１に係る干渉回避装置の動作を説明するためのフローチャート 本発明の実施の形態２に係る干渉回避装置が備える軌道算出部の構成図 図１６に示す干渉リスク評価部の動作を説明するための第１の図 図１６に示す干渉リスク評価部の動作を説明するための第２の図 本発明の実施の形態３に係る干渉回避装置よって動作するロボットハンドの動作を示す図 本発明の実施の形態４に係る干渉回避装置が備える軌道算出部の構成図 本発明の実施の形態１から４に係る干渉回避装置のハードウェア構成図 図２に示す生産システムの変形例を示す図 図２２に示す干渉回避装置の構成例を示す図 図２２に示す生産システムの第１の変形例を示す図 図２２に示す生産システムの第２の変形例を示す図 図２２に示す生産システムの第３の変形例を示す図 本発明の実施の形態５に係る干渉回避装置が備える軌道算出部の構成図 本発明の実施の形態６に係る干渉回避装置が備える軌道算出部の構成図 図２８に示す軌道算出部による動作を説明するための図 図２８に示す軌道算出部による動作を説明するためのフローチャート 本発明の実施の形態７に係る干渉回避装置が備える軌道算出部の構成図

　以下に、本発明の実施の形態に係る干渉回避装置を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。

実施の形態１．
　図１は本発明の実施の形態１に係る干渉回避装置を備えたロボットシステムの構成図であり、図１に示すロボットシステム１００は、計測部１、干渉回避装置２、ロボット制御装置３およびロボット４を備える。図２は図１に示す干渉回避装置と干渉回避装置の周辺機器とを備えた生産システムの構成図であり、図２に示す生産システム２００は、図１に示すロボットシステム１００に加えて、情報統合端末５、コンベア制御装置６、加工機制御装置７およびロボット制御装置８を備える。

　図２に示すようにロボット制御装置３には情報統合端末５が接続される。情報統合端末５は、不図示のクラウド上のサーバ、エッジサーバまたはＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ　Ｌｏｇｉｃ　Ｃｏｎｔｒｏｌｌｅｒ）といった機器から情報を集めて配信する機器である。情報統合端末５には複数の周辺機器が接続される。複数の周辺機器としては、コンベア制御装置６、加工機制御装置７およびロボット制御装置８を例示できる。情報統合端末５には、コンベア制御装置６、加工機制御装置７およびロボット制御装置８のそれぞれから出力される情報が伝達され、情報統合端末５に伝達された情報は、ロボット制御装置３を介して干渉回避装置２に伝達される。干渉回避装置２では、後述する干渉チェックの際、当該周辺機器から伝達される最新の情報と、計測部１で計測された計測データ１ａとが用いられる。なお図２では、情報統合端末５がロボット制御装置３を介して干渉回避装置２に接続されているが、情報統合端末５は干渉回避装置２に直接接続されている構成でもよい。

　図２に示すロボット４は６自由度の多関節ロボットである。ロボット４は、ロボットメーカが製作するロボット本体部を構成するアーム部４１と、アーム部４１の先端に設けられたツール部４２とを備える。ツール部４２は、ハンド部４３と、ハンド部４３に取り付けられたジグ４４と、ジグ４４に取り付けられた計測部１とを備える。

　計測部１は、部品箱９内の部品９ａの位置および姿勢を計測するための計測機器であればよく、計測部１としては、部品箱９内の部品９ａを撮像するカメラと、ハンド部４３とその周辺機器とを撮像する画像センサであるビジョンセンサと、レーザ変位計と、非接触センサとを例示できる。ジグ４４を介してハンド部４３に設けられた計測部１は、部品箱９と干渉しないようにハンド部４３と共に移動する。ここで計測部１はツール部に備える「ハンドアイ」の構成で説明するが、周辺機器を計測する手段として、周辺機器を俯瞰的に観測できる位置にジグ等で固定する方式も同様に実施することができる。

　図２では、ロボット４による作業の一例として、部品箱９内に配置された複数の部品９ａの中からロボットが１つの部品９ａを取り出すビンピッキング作業の様子が示される。ただし実施の形態１に係る干渉回避装置２を用いたロボット４による作業は、ビンピッキング作業に限定されず、マテハン作業、キッティング作業または組立作業でもよい。このようにロボット４による作業が限定されない理由は、部品９ａの状態に応じてロボット４の動作を変化させるとき、基本的にロボット４が同様の処理を行うためである。以下ではビンピッキング作業時における干渉回避装置２、ロボット制御装置３およびロボット４の動作を説明する。

　ロボット４は、まずロボット制御装置３で実行されるプログラムに従い、ビンピッキング動作前の位置まで移動する。次に干渉回避装置２は、計測部１により部品９ａの３次元位置および３次元姿勢を計測する。以下では、部品９ａの３次元位置および３次元姿勢を、部品９ａの３次元位置姿勢と称する場合がある。

　干渉回避装置２は、計測部１で計測された計測データ１ａをそのまま使うのではなく、３次元の点群、線、面、および色の情報を保持した３次元位置姿勢に変換してから用いる。計測データ１ａから部品９ａの３次元位置姿勢を計測する方法としては、レーザ、赤外線または音波を用いて計測された対象点までの距離に基づき３次元位置姿勢を計測する方式と、ステレオカメラで取得された３次元位置姿勢を計測する方法とを例示できる。なお計測部１は、干渉回避装置２においてピッキング対象の３次元位置姿勢情報を再現できる計測機器であればよく、前述したカメラ、ビジョンセンサ、レーザ変位計および非接触センサに限定されるものではない。また計測対象は部品９ａだけでなく部品箱９および周辺機器を含めてもよく、干渉回避装置２は部品９ａ、部品箱９および周辺機器を計測して得られた計測データ１ａを用いて干渉チェックに活用してもよい。

　次に計測データ１ａを用いて、ピッキング対象を決定する方法について説明する。ピッキング対象の決定では、計測データ１ａから候補点を複数選出するプロセスと、複数選出された候補点の中から１つの候補点を選定するプロセスとを実行することが一般的である。前者のプロセス、すなわち計測データ１ａから候補点を複数選出する手段には大きく分けて２種類の手段ある。

　候補点を複数選出する１つ目の手段は、予め図面モデルまたは３Ｄコンピュータ支援設計（ＣＡＤ：Ｃｏｍｐｕｔｅｒ　Ａｉｄｅｄ　Ｄｅｓｉｇｎ）といった部品モデルを干渉回避装置２に格納しておき、またはピッキング対象の３次元位置姿勢情報の参照データを干渉回避装置２に格納しておき、計測された計測データ１ａを部品モデルまたは参照データに照合することで、最も確からしいピッキング対象の姿勢を推定する方法である。以下では、この推定方法をモデルマッチングと称する。

　干渉回避装置２は、推定したピッキング対象の姿勢を用いて、予め定められたピッキング対象とツール部４２との位置関係とから、ハンド部４３が部品９ａを把持する把持点を決定する。ここで、予め定められたピッキング対象とツール部４２との位置関係については、事前にユーザによって教示作業で与える場合と、ピッキング対象の重心位置および持ち易さからオフラインでユーザが決定する場合とがある。

　候補点を複数選出する２つ目の手段は、モデルマッチングを不要とする方法であり、以下ではモデルレスマッチングと称する。モデルレスマッチングでは、ピッキング対象の部品モデルが不要であり、計測された点群、線および面といった特徴量から、ハンド部４３が部品９ａを把持し易い条件を満たす空間が探索され、把持点が決定される。把持し易い条件とは、ハンド部４３が備える指の開閉時に指先間で挟み込みが可能な空間を把持領域としたとき、点群の中で把持領域に包含可能な部位である。把持領域に包含可能な部位であれば全て生成することができるため、把持点の個数を制限する場合、把持領域に関して、指の開閉方向に対して部品９ａを安定して把持できるように、把持領域に包含される点群の形状について、制約条件を付けて数を減らすことができる。制約条件としては、ハンド部４３が備える指と対向し、かつ、平行である場合と、ハンド部４３が備える指をさしこむ方向に深さＤ［ｍｍ］にわたり障害物が無い場合とを例示できる。

　モデルレスマッチングでは、モデルマッチングと異なり、複数の把持対象の物体のそれぞれの境目が曖昧となる問題がある。従って点群情報は、クラスタリングという処理によって、把持対象の物体が複数の物体のそれぞれの境目でグルーピングされることが多く、これにより複数の物体を区別して重心位置の目安を算出することが可能となる。

　次に、前述した「複数選出された候補点の中から１つの候補点を選定するプロセス」について説明する。一般的には、候補点を１つ選定するルールとしては、把持し易い指標を設けて、これに対して最も把持し易い部品を選定することを例示できる。把持し易い指標としては、過去の実績から統計的にデータベース化された指標と、重力と力のつりあいと重心位置といった物理法則を考慮して導き出されるルールに基づいて決定される指標とを例示できる。

　複数選出された候補点の中から１つの候補点を選定する簡単な指標としては、部品箱９に配置された部品９ａ群の中から１つの部品９ａを取り出すとき、部品９ａ群の上側に配置されているものを候補点にすることが考えられる。これは、部品９ａ群の下側に配置されている部品９ａよりも部品９ａ群の上側に配置されている部品９ａを把持する方が、部品９ａ群を崩すことが少なく、結果としてピッキング対象とした部品９ａを把持する際に部品９ａを取り損ねる要因を低減できるためである。また大量の実行データから統計的に優先順位を決定することもできる。すなわち把持が成功し易い高さおよび方向が分かっている場合、その把持点を優先的に選定することもできる。

　干渉回避装置２は、選定された１つの候補点について、ツール部４２が部品箱９または周辺機器に干渉する可能性があるか否かの干渉チェックを実施する。ここで候補点とは「ツール部が部品を把持する座標点情報」を示しており、例えばツール部の位置（Ｘ，Ｙ，Ｚ）および姿勢（例えば姿勢を表す３つの角度の組で表されるオイラー角表現）の情報を含む。すなわち干渉回避装置２は、選定された候補点にツール部４２が移動したとき、ツール部４２が部品箱９に干渉するか否か、またツール部４２が前述した周辺機器に干渉するか否かを、ビンピッキング動作前に確認する。具体的には、干渉回避装置２は、把持点におけるツール部４２が占める領域において、ツール部４２が部品箱９または周辺機器に干渉するか否かを確認し、または、ツール部４２の移動方向に延長された軌道を含めた領域において、ツール部４２が部品箱９または周辺機器に干渉するか否かを確認する。

　一般的に部品箱９、部品９ａ、アーム部４１およびツール部４２はモデル化される。モデルは、３次元空間上の占有体積と２次元平面上の占有面積とで表現されるものである。モデル化としては、部品箱９、部品９ａ、アーム部４１およびツール部４２のそれぞれの詳細な形状を３Ｄ－ＣＡＤ通りにモデル化する場合と、多面体、直方体、円筒または球といったプリミティブと呼ばれる簡易なモデルでモデル化する場合とを例示できる。前者は、干渉チェックの判定精度が向上するものの、計算コストが高いためリアルタイム計算に向いていない。後者は、計算コストが低いためリアルタイム計算に向いているものの、モデル判定数が増えると計算コストが増加し、判定精度が低いため誤判定される可能性があり、また判定精度が低いため過度に安全な判定がなされる可能性がある。

　干渉回避装置２は、干渉チェック判定において干渉が生じる可能性がある場合、予め設定されたロボット姿勢の許容範囲である許容姿勢範囲に基づいて、ツール部４２が部品箱９から遠ざかる方向に指先姿勢を変更する。これによりツール部４２が部品箱９に干渉することを回避しながら把持点を決定することができる。図２に示すように部品箱９に配置される複数の部品９ａは、それぞれの位置と向きが異なるため、前述した許容姿勢範囲はピッキング対象毎に変化するが、一般には一定角度分の姿勢が変化してもよいという制約下においては、干渉しない姿勢が探索される。

　このように干渉回避装置２は、ピッキング候補を生成する機能と干渉チェック機能とを備え、ツール部４２が部品箱９または周辺機器に干渉する可能性が無い場合、ピッキング動作を実行する。一方、干渉する可能性がある場合、干渉回避装置２は、前述した許容姿勢範囲に基づいて、把持点における姿勢を修正し、これに対して再度干渉チェックを行う。

　ここで、把持が成功し易い把持点から干渉が生じない姿勢を探索し、その姿勢を新しい把持点として定義して、その把持点にツール部４２を移動させて把持させる場合、従来の干渉回避装置では、以下の２つの課題が生じる。

　１つ目の課題は、従来の干渉回避装置では把持時の接近方向が必要軌道として考慮されていないため、従来の干渉回避装置は把持成功率を下げている点である。

　ピッキングする部品９ａが突起を有する場合、部品９ａの把持点へツール部４２が接近する方向と、部品９ａに対するツール部４２の姿勢、すなわちツール部４２の相対位置とに対して、点としての合致するときの位置関係だけではなく、把持に至るまでの軌道を考慮しなければ、把持成功率をいたずらに下げてしまうことがある。すなわち、ツール部４２を動かす方向に制約が無い場合には把持成功率が低下する可能性は低いが、ツール部４２を動かす方向において、ツール部４２が部品箱９または周辺機器に干渉する可能性があるとき、または特定の方向から把持点に至らなければ部品９ａが動く可能性があるときには、把持成功率が低下する可能性が高い。

　把持成功率が低下する可能性が高い場合としては、部品箱９の形状が非対称であり、または周辺機器の形状が非対称であるためツール部４２が特定の位置で干渉し易い場合と、アーム部４１の指形状が平板形状ではなく折れ曲がったＬ字形状である場合と、部品９ａの端面が折れ曲がっている場合と、部品９ａの形状が非対称であり許容姿勢範囲が狭い場合とを例示できる。

　このとき、部品９ａの把持点におけるツール部４２の姿勢と、部品９ａの把持点に接近するツール部４２の姿勢とが異なる場合がある。把持姿勢に関してツール部４２を一定方向に移動する方向に対して干渉チェックすることは、把持軌道に対する考慮が無いため汎用的ではない。一方、干渉を避けるためツール部４２の姿勢を許容範囲内で回転させた場合、干渉チェックを行う際に把持成功率がどの程度悪化するかの評価は、把持点に対しては実行できるが、把持軌道に対しては実行できない。

　２つ目の課題は、把持の成功率を高めながら干渉を回避する軌道を考える上で、干渉しない経路が存在するか否かを含めた形で、把持点を１つ決めるための方法が従来技術に開示されていない点である。従来の干渉チェックでは、把持点のみに着目している。このため、把持の成功率が高くかつ干渉の可能性が無い把持点を１つの位置として算出するところまでが課題であり、把持点に至るまでの移動は、部品９ａを把持しているときのツール部４２の姿勢と同じ姿勢でツール部４２を部品箱９の上方から下降させるものである。しかしながら、モデルを詳細化するに従い、把持点における単純な干渉の有無だけをチェックする方法では、把持に至る軌道で見た場合、制約が多く、解が少なくなってしまうばかりか、把持成功率を著しく低下させる可能性がある。

　図３は図１に示す部品箱の上方に位置するロボットハンドが把持対象の物体に接近するときの把持姿勢と第１の接近軌道とを説明するための図である。なお、第１の接近軌道とは、生成された把持姿勢で、ロボットハンドのツール部が、把持対象をずらすことなく把持対象に接近できる軌道であり、把持接近軌道とも呼ぶ。図３に示す部品箱９は、床面５０に接する平坦な底面部９１と、水平方向における底面部９１の端部の内、一方の端部から鉛直方向に延びる一方の測面部９２ａと、水平方向における底面部９１の端部の内、他方の端部から鉛直方向に延びる他方の測面部９２ｂと、鉛直方向における測面部９２ｂの端部の内、上側の端部から水平方向に延びる上面部９３とを備える。

　上面部９３の水平方向における長さは、底面部９１の水平方向における長さよりも短い。従って、部品箱９を構成する底面部９１の上方には、開口部９４が形成される。開口部９４は、上面部９３の測面部９２ａ側の端部と、測面部９２ａの上端の端部との間に設けられている。このように図３に示す部品箱９は、その左側上端部分が部品箱９の内側に突き出た形状である。

　図３には、ハンド部４３の把持姿勢を説明するために便宜上３つのハンド部４３Ａ，４３Ｂ，４３Ｃが示されている。ハンド部４３Ａ，４３Ｂ，４３Ｃは何れも図１に示すハンド部４３に対応する。ハンド部４３Ａは把持姿勢Ａの状態にあるハンド部４３に対応し、ハンド部４３Ｂは把持姿勢Ｂの状態にあるハンド部４３に対応し、ハンド部４３Ｃは把持姿勢Ｃの状態にあるハンド部４３に対応する。

　部品箱９には複数の部品９ａが配置され、複数の部品９ａの中で最も上に配置された部品９ａ１は、ハンド部４３Ａ，４３Ｂ，４３Ｃの把持対象となる物体である。部品９ａ１は、長手片９ａ１１と、長手片９ａ１１の端部から直角方向に延びる短手片９ａ１２とを備えるＬ字形状である。部品９ａ１以外の複数の部品９ａも部品９ａ１と同様の形状である。部品９ａ１は、長手片９ａ１１の端部が、測面部９２ａと対向し、長手片９ａ１１の底部が、部品９ａ１の下方に存在する部品９ａと接している。また部品９ａ１は、短手片９ａ１２の端部が、部品９ａ１の下方に存在する部品９ａ側とは反対側に位置している。

　把持姿勢Ａのハンド部４３Ａは、把持姿勢Ｂ，Ｃのハンド部４３Ｂ，４３Ｃよりも上面部９３の近くに位置している。実線の移動軌跡Ａ１は、ハンド部４３Ａの先端部の移動軌跡を表す。図３では、移動軌跡Ａ１で移動するハンド部４３Ａが部品９ａ１の短手片９ａ１２を把持するように部品９ａ１に接近する。ところが移動軌跡Ａ１で移動するハンド部４３Ａおよび計測部１は、上面部９３に干渉するおそれがある。

　ハンド部４３Ｃは部品９ａ１の長手片９ａ１１を把持することができるため、把持姿勢Ｃは部品９ａ１の把持成功率を高める上では望ましい姿勢であるが、測面部９２ａと重なる位置に配置されているため、測面部９２ａに干渉する。

　点線の移動軌跡Ｂ１は、ハンド部４３Ｂの先端部の移動軌跡を表す。図３では、移動軌跡Ｂ１で移動するハンド部４３Ｂが部品９ａ１の短手片９ａ１２を把持するように部品９ａ１に接近する。移動軌跡Ｂ１で部品９ａ１に接近するハンド部４３Ｂおよび計測部１は、上面部９３および測面部９２ａに干渉するおそれがない。ところが移動軌跡Ｂ１で移動するハンド部４３Ｂが部品９ａ１を取り出すときの把持成功率は、移動軌跡Ａ１で移動するハンド部４３Ａが部品９ａ１を取り出すときの把持成功率よりも低い。これは、部品９ａ１の短手片９ａ１２の一部を把持するハンド部４３Ｂに比べて、ハンド部４３Ａは、部品９ａ１の短手片９ａ１２と長手片９ａ１１とを同時に把持するためである。

　このように、部品箱９に配置された複数の部品９ａの中から部品９ａ１を取り出すときの把持成功率を高めるためには、把持姿勢Ｂよりも把持姿勢Ａおよび把持姿勢Ｃが望ましい。ただし把持姿勢Ｃは干渉チェックにより干渉有りと判定されるため把持点候補から除外され、把持点候補には把持姿勢Ａおよび把持姿勢Ｂが残る。干渉の有無においては、第１の接近軌道を把持点候補と同じ姿勢とすることを考えると、把持姿勢Ｂを把持点候補として選定することが望ましい。ところが把持姿勢Ｂは干渉しないことに重きが置かれた姿勢であるため、把持成功率を高める上では、把持成功率と干渉の有無とをバランスよく考慮する事が重要である。従って干渉をしないという条件をどのようにとらえるかが重要な点になる。

　実施の形態１に係る干渉回避装置２は、図３に示す把持姿勢Ａが選ばれるように、把持に用いるツール部４２毎の第１の接近軌道を定義し、ツール部４２が干渉する可能性が無い第１の接近軌道を生成し、生成した第１の接近軌道の起点とピッキング動作開始点とを結ぶことで、干渉回避できる第２の接近軌道を探索し、この解を有する場合の有効把持姿勢である把持点候補を選定して、選定した軌道を接続して干渉回避軌道Ａ２とする。なお、第２の接近軌道とは、第１の接近軌道の端点（起点）から、ロボットのアーム部およびツール部が周辺機器に干渉することなく把持対象に接近する軌道である。

　従来の干渉回避装置では、部品の把持点に至るツール部の移動軌跡が考慮されずに把持点のみが決定されるため、図３に示す把持姿勢Ａが選ばれない。これに対して実施の形態１に係る干渉回避装置２では把持姿勢Ａが選ばれる。

　以下では実施の形態１に係る干渉回避装置２の構成および処理内容を具体的に説明する。まず図４を用いて実施の形態１に係る干渉回避装置２の構成の概要を説明する。図４は本発明の実施の形態１に係る干渉回避装置の構成図である。

　図４に示す干渉回避装置２は、計測部１から出力される計測データ１ａと、ロボット制御装置３を介して入力されるモデル情報３ａと、ロボット制御装置３から出力される把持前後のロボットプログラム３ｂと、ロボット制御装置３を介して入力される設定情報３ｃとに基づき、干渉回避軌道２ａを生成して出力する。

　モデル情報３ａは、アーム部４１、ツール部４２、周辺機器および部品箱９に関するデータである。具体的には、モデル情報３ａは、３次元空間上におけるアーム部４１、ツール部４２、周辺機器および部品箱９の占有領域を定義するための情報として、３Ｄ－ＣＡＤで表現された各幾何情報であり、またはアーム部４１、ツール部４２、周辺機器および部品箱９のそれぞれを包含するプリミティブとして表現されたものである。プリミティブとしては、球体とその中心位置とを例示できる。モデル情報３ａを用いることで、アーム部４１、ツール部４２、周辺機器および部品箱９の相互間の干渉の有無の確認が可能となり、またアーム部４１、ツール部４２、周辺機器および部品箱９の相互間の相対距離演算とが可能となる。

　把持前後のロボットプログラム３ｂは、図１に示すロボット制御装置３に記憶されたロボット言語プログラムである。ロボットプログラム３ｂは、ロボット動作に関する位置を規定する情報であればよく、ロボット制御装置３に記憶されたものに限定されず、不図示のパソコンに記憶されたロボット動作に関する位置を規定する情報でもよい。

　設定情報３ｃは、予め設定されたロボット姿勢の許容範囲である許容姿勢範囲を設定する情報である。

　干渉回避装置２は、計測データ１ａ、モデル情報３ａおよびロボットプログラム３ｂに基づき複数の把持点候補１０ａを算出する把持点候補生成部１０と、複数の把持点候補１０ａと設定情報３ｃとに基づき干渉回避軌道２ａを算出する軌道算出部１１とを備える。

　把持点候補１０ａは把持点候補情報および把持拘束情報を含む。把持拘束情報は、把持を成功させるためにツール部４２と把持対象の物体との位置関係を予め決定しておき、位置関係を達成する点を目標地点として把持対象の物体から一定距離離れた位置からツール部４２を接近させて把持対象の物体を把持する場合の拘束条件を示す。把持対象の物体から一定距離離れた位置は把持接近を開始する位置であり、把持点候補からツール座標系のＺ軸方向に１０ｍｍ離れた位置を例示できる。ツール座標系は、ツール部４２に設定する座標系を示す。ツール座標系のＺ軸方向は、ハンド部４３の指先の開閉を行う部分のストロークの中心点を通り、かつ、ハンド部４３の指先方向と平行な軸上に設定する。ツール座標系のＸ軸方向およびＹ軸方向は、ユーザが使い易いように決定される。

　把持拘束では、まずツール部４２と把持対象の物体とに関して目標となる位置姿勢関係が決定される。物体の把持を実施する場合、最も望ましい把持姿勢を、予めツール部４２と把持対象の物体との位置関係として定義しておく。最も望ましい把持姿勢は、ツール部４２を用いて把持対象の物体を把持する操作のデータベースに基づいて決定できる。データベースには、実際の実験結果で導出された結果を用いてもよいし、物理シミュレーションまたはオフラインシミュレーションの結果を用いてもよい。

　ツール部４２と把持対象の物体との位置姿勢関係には、許容される姿勢誤差が存在し、把持が成功した位置姿勢関係の統計的な平均値および中央値を定義できる。すなわちツール部４２と把持対象の物体との予め定められた望ましい位置姿勢関係は、把持が成功する代表的な位置姿勢関係として予め決定されたものである。

　以下に干渉回避装置２の動作を説明する。ロボット４が物体を把持するための位置または軌道を計算するため、計測部１では把持対象の物体の状態が計測される。物体の状態としては、部品９ａの位置および姿勢を例示できる。物体の状態を計測して得られた情報から、ロボット座標系に対する把持対象の物体の位置姿勢情報が得られる。ロボット座標系とは、ロボット４を基準とした座標系であり、ロボット４を位置制御する際に用いる座標系のことである。通常、ロボット４の据付面に鉛直な方向にＺ軸が定義され、第１関節の軸と据付面とが交わる点に原点を持ち、第１関節の可動範囲の中心位置にＸ軸が定義される。

　把持点候補生成部１０では、把持対象の物体の位置姿勢情報を取得するため、一般に画像処理技術および位置推定処理が行われる。把持点候補生成部１０では、計測部１と計測対象との間の距離情報と、当該距離情報を３次元的にプロットした点群情報と、計測対象の表面のテクスチャ情報とを入力として、予め用意されたモデルとのマッチング、または時刻の違う取得情報間の関係から、位置推定処理としてＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　Ａｎｄ　Ｍａｐｐｉｎｇ）を適用し、また画像処理技術としてモデルマッチング技術を用いて、把持対象の物体の位置姿勢情報が算出される。

　把持点候補生成部１０は、算出した把持対象の物体のロボット座標系からみた位置姿勢ｒｏｂｏｔ＿Ｔ＿ｗｏｒｋと、予め決定したツール部４２と把持対象の物体との目標位置姿勢ｔｏｏｌ＿Ｔ＿ｗｏｒｋとを入力として、ロボットが把持を実現するためのツール部４２の目標位置姿勢ｒｏｂｏｔ＿Ｔ＿ｔｏｏｌを決定する。

　ここで、ｉ＿Ｔ＿ｊという表現は、ｉ座標系からみたｊ座標系への同次変換行列Ｔを示す。同次変換行列Ｔとは、ｉ座標系とｊ座標系との間のＸ軸，Ｙ軸，Ｚ軸の関係を表す３行３列の回転行列Ｒと、ｉ座標系からみたｊ座標系原点の位置関係を表す３要素の並進ベクトルｐ（＝（ｘ，ｙ，ｚ））から下記（１）式で表現される４行４列の行列である。ただし下記（１）式の０は１行３列で０からなるゼロ行列を表している。
　Ｔ＝（Ｒ　　　　ｐ　
０　　　　１）・・・（１）
　回転行列Ｒは一般にＲ＝（Ｒ１１，Ｒ１２，Ｒ１３；Ｒ２１，Ｒ２２，Ｒ２３；Ｒ３１，Ｒ３２，Ｒ３３）からなる３行３列の行列で、相対的な回転を行列で表現している。また、ｐは３行１列のベクトルであり、ある参照座標系からみた対象となる点の位置ベクトルを示している。

　把持点候補生成部１０は、同次変換行列ｉ＿Ｔ＿ｊを用いて目標位置姿勢ｒｏｂｏｔ＿Ｔ＿ｔｏｏｌを計算することで、把持動作のためのロボット指先位置の目標位置Ｐｇｇを決定する。目標位置Ｐｇｇはツール部４２の目標位置姿勢ｒｏｂｏｔ＿Ｔ＿ｔｏｏｌをロボット指令値の表現に置き代えたものである。ロボット指令値の表現としては、並進位置Ｘ，Ｙ，Ｚと、Ｘ，Ｙ，Ｚ各軸周りの回転量Ａ，Ｂ，Ｃとを例示できる。目標位置姿勢ｒｏｂｏｔ＿Ｔ＿ｔｏｏｌは下記（２）式で求めることができる。
　ｒｏｂｏｔ＿Ｔ＿ｔｏｏｌ＝ｒｏｂｏｔ＿Ｔ＿ｗｏｒｋ＊（ｔｏｏｌ＿Ｔ＿ｗｏｒｋ）＾－１・・・（２）

　ロボット指先位置の目標位置Ｐｇｇは、把持対象の物体とツール部４２との間の望ましい関係が多様である場合、複数点候補が存在する。このため以下では複数の目標位置Ｐｇｇの候補点を、把持点候補Ｐｇｇ（ｉ）として表現する。

　把持点候補生成部１０は、ツール部４２と把持対象の物体との目標位置姿勢関係に基づいて、把持点候補Ｐｇｇ（ｉ）から一定距離離れた位置を算出し、その位置を把持接近開始点Ｐｓｇ（ｉ）としたとき、把持点候補毎の把持接近開始点Ｐｓｇ（ｉ）から把持点候補Ｐｇｇ（ｉ）までの補間方法と、それぞれの位置姿勢とを記憶する。すなわち、把持のみに着目した場合、把持を成功させるために把持対象の物体とツール部４２との位置関係の変化（軌道）を記述した軌道が第１の接近軌道である。当該第１の接近軌道は、把持接近開始点Ｐｓｇ（ｉ）から把持点候補Ｐｇｇ（ｉ）までの軌道である。把持接近開始点Ｐｓｇ（ｉ）は、把持動作を開始する点である。

　把持動作には、ツール部４２と把持対象の物体の目標位置姿勢に到達する前に、ツール部４２が把持対象の物体または周辺機器に触れることで把持の失敗を避ける目的で、移動方向および移動量に制約が与えられたツール部４２を使った把持のみを含むものとする。ここでは、ツール部４２が把持対象の物体の目標位置姿勢に到達するまでの軌道において、把持対象の物体の目標位置姿勢から一定距離離れたツール部４２の軌道は、把持の成功または把持の失敗に寄与しないとし、把持動作以外の移動として、第２の接近軌道として区別して取り扱われる。すなわち、第２の接近軌道は、第１の接近軌道と違う制御方法あるいは動作設定を選択することができる。例えば動作速度を上げるために、第２の接近軌道は速度設定を第１の接近軌道よりも高速に設定することができる。あるいは第２の接近軌道は、第２の接近軌道の間のみ関節補間を選択して動作速度を向上させる事もできる。ここで関節補間とは、モータ軸毎に目標地点の角度を目標角度として設定して制御する補間方法である。関節補間では、手先軌道を設計した軌道に制御できない代わりに、各軸モータを制御する制約が少なくなり高速に動かすことができる。第２の接近軌道については後述する。

　第１の接近軌道に関連する位置関係、距離および姿勢変更は、把持の成功または把持の失敗に関するデータベースを用いて、学習的に獲得された位置関係から求めることもでき、また幾何的な拘束関係から同様の関係を解析的に導いてもよい。すなわち、把持接近開始点Ｐｓｇを決定する方法がツール部４２のアーム方向以外の場合もある。具体的には、把持点候補Ｐｇｇ（ｉ）を入力として、把持点候補Ｐｇｇ（ｉ）を円弧の端点とする半径Ｒの円弧を描きながら、当該端点から一定距離離れた位置を算出し、その位置を把持接近開始点Ｐｓｇと呼ぶこともできる。また、把持対象の物体の表面上に凹凸がある場合、ハンド部４３の指先と把持対象の物体を把持する目標位置姿勢関係に到達するまでに、物体表面上の凹凸部を避けるようにハンド部４３を動かす一定の軌道が与えられる場合もある。

　図５を用いて把持点候補Ｐｇｇ（ｉ）、把持接近開始点Ｐｓｇおよび第１の接近軌道を説明する。図５は本発明の実施の形態１に係る干渉回避装置において第１の接近軌道および把持接近開始点の定義方法の例を示す図である。図５に示すように把持接近開始点Ｐｓｇは、ハンド部４３の指先の通過すべき動作点に対応している。把持点候補Ｐｇｇ（ｉ）を第１の接近軌道の終端点として、ハンド部４３の指先が部品９ａの凸部１２との干渉を避けながら部品９ａから離れる第１の接近軌道１３の起点として求めることができる。ここで、第１の接近軌道は、接近して把持した後に取り出すように移動する軌道にも用いることができる。特に、取り出す時のことを考える場合は、起点として最短距離の移動でワークを取り出すことのできる位置で把持姿勢から離れた点を指定した第１の接近軌道を指定することができる。例えば、部品の把持後に他の部品から十分離れて持ち上げが成功するのに最低限必要な移動量を最短距離の移動として定義することができる。

　把持の成功または失敗に関するデータベースは、物理シミュレーションを用いて試行を行う場合と、実機を用いて実際に実験を行う場合との何れでも実現可能である。先に述べたような優先順位に基づいて作業の把持点候補の優先順位を予め決めておく。優先順位としては、過去のデータベースに基づいて成功率の高い把持姿勢を選定し、または部品箱９の上側に配置された部品９ａを優先的に選定することを例示できる。

　軌道算出部１１は、設定情報３ｃと複数の把持点候補１０ａとを用いて、把持姿勢生成部で把持点を選び、選ばれた把持点に対して把持拘束を満たす第１の接近軌道を生成する。さらに第１の接近軌道の起点である把持接近開始点に接続する形で、箱等に干渉しない軌道を第２の接近軌道として生成する。最後にこれら第１の接近軌道と第２の接近軌道を繋いだ形として干渉回避軌道を演算し、干渉回避軌道２ａとして出力する。従来の干渉回避装置は、干渉回避軌道を演算せずに把持点候補の演算のみ行うが、実施の形態１に係る干渉回避装置２では、把持点候補以外に、把持点候補１０ａと第２の接近軌道とを用いて、これらを合わせた形で干渉回避軌道全体を演算する。

　以下では干渉回避軌道２ａを生成するための軌道算出部１１の構成および動作を説明する。

　図６は図４に示す軌道算出部の構成図であり、図７は図６に示す干渉チェック部の動作を説明するための図であり、図８は図６に示す把持姿勢生成部の動作を説明するための図であり、図９は図６に示す第１の接近軌道生成部の動作を説明するための図であり、図１０から図１３は図６に示す第２の接近軌道生成部の動作を説明するための第１から第４の図である。図１４は図６に示す回避軌道生成部の動作を説明するための図である。

　図６に示す軌道算出部１１は、把持姿勢生成部２１、第１の接近軌道生成部２２、第２の接近軌道生成部２３、回避軌道生成部２４および干渉チェック部２５を備える。

　把持姿勢生成部２１は、複数の把持点候補１０ａの中から部品箱９を含む周辺機器との干渉チェックをした後、干渉する可能性が無い有効把持姿勢２１ａを把持点候補として生成する。複数の把持点候補１０ａには図８に示すように複数の把持姿勢Ａ，Ｂ，Ｃが含まれ、ツール部４２が取り得る姿勢は、これらの複数の把持姿勢Ａ，Ｂ，Ｃの中から、干渉チェック部２５における処理を経て生成される。なお、把持点候補は、ロボットツール部の位置あるいは姿勢の何れか違うものに対して候補点と呼ぶ。

　図６に示す干渉チェック部２５は、例えば、図７に示すロボットハンドモデル１０１と周辺機器モデル１０２を、干渉チェックを行う対象として、ロボットハンドモデル１０１と周辺機器モデル１０２との間で相対距離を計算して、相対距離が基準値以下であるか否かを判定する。また、干渉チェック部２５は、例えば、図７に示すロボットハンドモデル１０１と周辺機器モデル１０２を、干渉チェックを行う対象として、ツールモデル１０３と周辺機器モデル１０２との間で相対距離を計算して、相対距離が基準値以下であるか否かを判定する。干渉チェックを行う際には、干渉チェック部２５は、モデル情報として、モデルの幾何形状情報および位置姿勢情報を入力し、干渉しているか否かの判定結果と、計算されたモデル間の相対距離とを含んだ、干渉チェック判定情報を出力する。幾何形状情報は、属性、寸法などに関する情報であり。属性に関する情報は、形状特性、プリミティブ形状、多面体、自由形状などの情報である。プリミティブ形状は、球、直方体、円筒、円錐などの形状である。多面体は、３Ｄ－ＣＡＤ（ｔｈｒｅｅ　Ｄｉｍｅｎｓｉｏｎａｌ　Ｃｏｍｐｕｔｅｒ　Ａｉｄｅｄ　Ｄｅｓｉｇｎ）でフォーマットされた三角パッチ面で囲まれた多面体である。自由形状は、点群で定義された点群集合である。寸法は、球であれば半径Ｒであり、円筒であれば半径Ｒと長さＬとである。なお、干渉チェック部２５は、生成軌道の各時刻におけるチェックを行う必要がある。干渉チェック部２５は、軌道に対して時刻毎のモデル情報を入力し、軌道全体で干渉がないか演算する。計算コストを下げるために、起点と終点の内、代表的な姿勢数点のみを評価する方法がある。他にも、計算コストを下げるために、軌道起点のロボットアームおよびロボットハンドモデルと、軌道終点のロボットアームおよびロボットハンドモデルとを、空間的に内挿した幾何モデル、例えば多面体などを生成し、干渉の有無を評価する方法などがある。

　図６に示す第１の接近軌道生成部２２は、把持姿勢生成部２１で生成された有効把持姿勢２１ａに対応する把持制約を当てはめて有効第１の接近軌道２２ａを生成する。干渉チェック部２５では有効第１の接近軌道２２ａに対して干渉チェックが行われる。図９には、把持姿勢生成部２１で生成された有効把持姿勢Ａ’，Ｂ’と、有効把持姿勢Ａ’に接近制約を考慮した第１の接近軌道Ａ１’と、有効把持姿勢Ｂ’に接近制約を考慮した第１の接近軌道Ｂ１’とが示される。この場合、把持対象である部品９ａ１を確実に把持するためには、部品９ａ１から一定距離離れた位置からツール部４２の接近動作を開始させて、ツール部４２の先端が部品９ａ１の下に潜り込むような動作を実施する必要がある。

　図９に示すように、第１の接近軌道Ａ１’で動作するツール部４２は部品箱９に干渉する可能性がある。そのため、有効把持姿勢Ａ’に対応する第１の接近軌道Ａ１’を生成せず、有効把持姿勢Ｂ’に対応する第１の接近軌道Ｂ１’を生成する方が望ましい。また第１の接近軌道生成部２２の前段に把持姿勢生成部２１が設けられているため、第１の接近軌道生成部２２は全ての把持点候補に対して有効第１の接近軌道２２ａを演算する必要が無く、計算効率が向上する。

　以下では、把持点候補に対するツール部４２側の把持制約について説明する。把持制約とは、作業の成功率を上げるために導入する把持点候補までのツール部４２の移動軌道を規定するものである。把持点に関するツール部４２の目標位置姿勢を変更する場合、ツール部４２の目標位置姿勢は、ハンド部４３の指先の把持形態、機構および材質といった変動条件の影響も受けるため、生産システム２００を構成する機構毎に把持成功率を上げる方法が異なる。第２の接近軌道生成部２３は、干渉回避軌道を生成する際、ハンド部４３の指先に関する制約条件を考慮し、把持姿勢だけではなく、把持に関する中間軌道を拘束条件として考慮する。中間軌道は、有効第１の接近軌道の起点である把持接近開始点Ｐｓｇとピッキング動作の開始点とを結ぶ軌道である。ピッキング動作の開始点には、対象となる部品の入った棚や箱（周辺環境の一部）の上空か、アームやハンドと周辺環境が干渉が生じない程度に十分離れた点が選ばれる。

　図１０に示すように開閉グリッパ式のハンド部４３が用いられている場合、ハンド部４３を閉じ切る方法がエアー式であれば、把持速度は空気圧力で決まり、ハンド部４３が設置される工場で用いられるエアーの圧力は一定値であることが多いため、把持速度は一定値となる。このとき、把持速度が速すぎる場合、ハンド部４３から部品が落ちてしまうリスクがある。そのようなリスクを防ぐために、ハンド部４３が部品９ａ１の下側に触れてから部品９ａ１の上側に触れるといった把持点候補の考え方は、ハンド部４３の構成により変わる。なお図１０では、ハンド部４３が部品９ａ１に接近する理想アプローチ方向１４と、ハンド部４３が部品９ａ１を把持可能な許容アプローチ方向１５とが示される。

　図１１では、図１０に示す開閉グリッパ式のハンド部４３の代わりに、吸着式のハンド部４３－１が用いられている。ハンド部４３－１には吸着パッド４３－１ａが設けられ、吸着パッド４３－１ａの硬さおよび吸着力によって、ハンド部４３－１の理想アプローチ方向１４が変化する。特に吸着パッド４３－１ａは、部品９ａ１の表面に対して鉛直方向にアプローチした方が望ましいため、図１０に示すハンド部４３を用いる場合に比べて、許容アプローチ方向１５が狭い。これを考慮すると、把持成功率に寄与する把持前後の接近軌道を、干渉チェックと共に不用意に変化させることは望ましくない。

　以上のように、ここまでは把持点候補に対するツール部４２側の制約条件について述べた。次に把持点候補に対する部品９ａ１側の制約条件について述べる。

　モデルがある場合、モデル情報３ａを用いたモデルレスマッチングにより複数の部品９ａの各々が軽量であることを事前に把握できる。このとき、図１２に示す把持点候補１７に対して、偶力が発生し、また不図示のハンド部４３の指先で部品９ａを弾いてしまわないようにするため、把持点候補１７に対して、距離を含む許容進行ベクトル１８が定義される。許容進行ベクトル１８は、ベクトルの大きさが幾何的に把持点候補１７よりも進んでよい距離と、把持点候補１７にかけてよい力の大きさとを示す。すなわち把持点候補１７では、把持現象が生じるためにかけてはいけない負荷の上限を規定している。

　退避方向については、図１３に示すように、距離を含む許容退避ベクトル１９が定義される。許容退避ベクトル１９は、把持位置にツール部４２を移動させて把持を行った場合、どの程度ならばハンド部４３を引き上げてよいかを示しており、ベクトルの幅は、何れの方向であれば他の物体への影響を少なくして引き上げられるかを指標化したものである。これらの把持拘束を満たす最も移動量が少ない軌道が、有効第１の接近軌道２２ａとして定義される。

　第２の接近軌道生成部２３は、第１の接近軌道生成部２２で生成された有効第１の接近軌道２２ａを通過する干渉回避軌道を検討する上で、有効第１の接近軌道２２ａの開始点Ｐｓｇとピッキング動作の開始点とを結ぶ中間軌道を生成する。ここで第２の接近軌道２３ａとは、有効第１の接近軌道２２ａに対して、把持動作の成功率に直接関わりが無く、自由空間の移動について区別するために定義された軌道である。そのため、始点は様々であるが、終点は必ず有効第１の接近軌道２２ａの開始点になる。

　第２の接近軌道生成部２３では、有効第１の接近軌道２２ａの起点の位置姿勢に対して、できるだけ姿勢変化の少ないピッキング動作開始点姿勢を指定することで、ツール部４２を高速に動作させることができる。また干渉回避軌道の生成については、各軌道を生成する場合の干渉を避ける軌道探索は、デカルト座標で実施してもコンフィギュレーション空間で実施してもよい。オフラインで探索的に軌道を探す場合、一般に知られる探索方法を活用することができる。当該探索方法としてはＲＲＴ（Ｒａｐｉｄ　Ｒａｎｄｏｍ　Ｔｒｅｅ）を例示できる。

　図１４に示す第２の接近軌道２３ａは、ロボットハンドモデル１０１およびツールモデル１０３が部品箱９の周辺機器モデル１０２と干渉しないように、ロボットハンドモデル１０１およびツールモデル１０３が有効第１の接近軌道である第１の接近軌道Ｂ１’の開始点に移動するような干渉回避軌道である。

　図６に示す回避軌道生成部２４は、把持姿勢生成部２１で得られたツール部４２の有効把持姿勢２１ａと、第１の接近軌道生成部２２で得られたツール部４２の有効第１の接近軌道２２ａと、第２の接近軌道生成部２３で得られたツール部４２の第２の接近軌道２３ａとを繋ぎ、干渉回避軌道２ａとしてロボット制御装置３に出力する。ロボット制御装置３は、干渉回避軌道２ａに基づきビンピッキング動作を実行する。なお、軌道算出部１１では、有効把持姿勢２１ａ、有効第１の接近軌道２２ａおよび第２の接近軌道２３ａがそれぞれ複数出力され、組み合わせが複数存在する場合には、評価関数を定義して、評点が閾値を超え最も点数が高いものを実行するような方法をとることが出来る。評価関数としては、干渉回避軌道２ａ全体の移動に要する時間、すなわちロボットの総作業時間が最小化するような軌道を選択することができる。また、評価関数としては、ロボット動作に必要な電流の値に基づいて算出されるエネルギー消費量などを入力として、エネルギー消費量が最小化するよう評価値を算出することが出来る。評価関数の方法は、これらに限定するものではない。

　図１５は本発明の実施の形態１に係る干渉回避装置の動作を説明するためのフローチャートである。干渉回避装置２は、計測データ１ａに基づき複数の把持点候補１０ａを算出し、算出した複数の把持点候補１０ａの中から干渉する可能性が無い有効把持姿勢２１ａを把持点候補として生成する（Ｓ１）。干渉回避装置２は、生成した把持点候補に対応する有効第１の接近軌道２２ａを生成し（Ｓ２）、有効把持姿勢２１ａおよび有効第１の接近軌道２２ａに対して干渉チェックを実施する（Ｓ３）。

　干渉チェックの結果、干渉する可能性が有る場合（Ｓ４，Ｎｏ）、干渉チェック部２５は、部品箱９と干渉する可能性のあるツール部４２の一部を生成し、許容姿勢範囲において干渉をなくすために最も小さい姿勢変化をさせた場合の修正した把持点候補を探索し、干渉の可能性の有無を判定する（Ｓ５）。

　許容姿勢範囲に他の解が無い、すなわち許容姿勢範囲において干渉の可能性が無くなる演算結果が得られない場合（Ｓ５，Ｎｏ）、干渉回避装置２ではＳ２からＳ５までの処理が繰り返される。干渉の可能性が無くなる演算結果が得られない場合、把持姿勢生成部２１より、優先順位が次に高い新しい把持点候補を選び、この候補点に関する評価を行う。

　許容姿勢範囲に他の解が有る、すなわち許容姿勢範囲において干渉の可能性が無くなる演算結果が得られる場合（Ｓ５，Ｙｅｓ）、干渉回避装置２では有効把持姿勢２１ａが修正され（Ｓ６）、Ｓ３からＳ６までの処理が繰り返される。

　Ｓ４において干渉する可能性が無い場合（Ｓ４，Ｙｅｓ）、第２の接近軌道生成部２３が第２の接近軌道２３ａを生成し（Ｓ７）、干渉チェック部２５では再び干渉チェックが行われる（Ｓ８）。

　第２の接近軌道２３ａに対して干渉の可能性が有る場合（Ｓ８，Ｎｏ）、Ｓ７およびＳ８の処理が繰り返される。第２の接近軌道２３ａに対して干渉の可能性が無い場合（Ｓ８，Ｙｅｓ）、回避軌道生成部２４はロボット制御装置３に対する干渉回避軌道を生成する（Ｓ９）。

　以上に説明したように、実施の形態１に係る干渉回避装置２は、有効第１の接近軌道２２ａと第２の接近軌道２３ａという２つの領域に分け、先に把持成功率が高い有効第１の接近軌道２２ａに干渉の可能性が無いことを確認し、次に第２の接近軌道２３ａが有効な解をもつ場合、その起点となる把持点を有効把持点として選定し、それぞれの軌道を接続して干渉回避軌道とする。この構成により、干渉しない把持点を選定しながら把持成功率を高めることができ、従来の干渉回避装置を用いた場合に比べてロボットシステム１００の作業効率を向上できる。

実施の形態２．
　図１６は本発明の実施の形態２に係る干渉回避装置が備える軌道算出部の構成図である。以下、実施の形態１と同一部分には同一符号を付してその説明を省略し、ここでは異なる部分についてのみ述べる。実施の形態１と実施の形態２との相違点は以下の通りである。
　（１）実施の形態２では、実施の形態１の軌道算出部１１の代わりに軌道算出部１１Ａが用いられている。
　（２）軌道算出部１１Ａは、軌道算出部１１が備える把持姿勢生成部２１、第１の接近軌道生成部２２、第２の接近軌道生成部２３、回避軌道生成部２４および干渉チェック部２５に加えて、干渉リスク評価部２６を備える。

　図１７は図１６に示す干渉リスク評価部の動作を説明するための第１の図である。距離情報Ｌｉｊは、互いに干渉を避けたい物体であるロボットハンドモデル１０１と部品箱９との間の距離、または互いに干渉を避けたい物体であるツールモデル１０３と部品箱９との間の距離である。通常これらが一定以上の距離に保たれることで、干渉が避けられている。よって、計測部の高精度な計測結果や、計測結果に基づいて算出される周辺機器とロボットの間の正確な相対距離関係が常に分かっている場合は、これらが一定以上の距離に保たれていることを管理することが重要で、これらが一定以上になる軌道を第２の接近軌道生成部は選択する。しかし、キャリブレーション誤差やノイズ・相対距離のモデル化誤差の影響により、正確な相対距離関係が得られないことがある。この場合、第２の接近軌道生成部内では、正確な相対距離関係が得られない場合には、第２の接近軌道の中の干渉リスクを接近距離Ｌｉｊに基づいて評価して、リスクが高いと考えられる部分については、移動速度を調整することで実際にぶつかる場合のダメージを低減させるように、第２の接近軌道の実行速度を調整することが望ましい。

　図１６に示す干渉リスク評価部２６は、有効第１の接近軌道２２ａの開始点情報とモデル情報３ａとを入力として、第２の接近軌道生成部２３に対して、物体ｉと物体ｊの間の距離情報Ｌｉｊを演算する。物体ｉおよび物体ｊは、図１７に示すロボットハンドモデル１０１、ツールモデル１０３および部品箱９といった干渉を避けたいもの同士であればよい。干渉リスク評価部２６は、モデル情報３ａに含まれるアーム部４１、ツール部４２、周辺機器および部品箱９といった各モデルの位置情報と計測データ１ａとに基づき、物体ｉと物体ｊの間の距離情報Ｌｉｊを演算する。

　図１８は図１６に示す干渉リスク評価部の動作を説明するための第２の図である。図１８の上側に示す図では縦軸が距離情報Ｌｉｊの差分を時間で割ったことで算出された接近速度Ｌｉｊ’であり横軸が時間である。図１８の下側に示す図は縦軸が距離情報Ｌｉｊであり横軸が時間である。図１６に示す第２の接近軌道生成部２３は、図１８に示すように距離情報Ｌｉｊが接近距離の閾値３１ｂに達しているか否かを確認し、または接近距離の変化率、すなわち接近速度Ｌｉｊ’が距離情報Ｌｉｊに応じて変化する接近速度の速度上限値３１ａを超えているか否かを確認する。図１８に示す例では接近距離Ｌｉｊが干渉チェックの閾値３１ｂに近づく場合に、速度上限値３１ａを低減させる設定がされている。すなわち、干渉リスクが高まる評価の場合は、許容される移動速度を低下させる。仮に、低下させた速度よりも大きい移動速度が設定されていた場合は、移動速度を速度上限値３１ａまで下げられるよう、ロボットの移動速度が調整される。

　干渉チェックにおいて干渉の可能性が無いと判定されても、センシング情報およびモデル化誤差が生じている場合、干渉が生じることがある。通常は、これらを見越してすべての干渉チェックの接近距離Ｌｉｊに対して干渉していると判定する閾値を大きく設定する「判定マージン」をとることで、ツール部と周辺環境が衝突するリスクを低減する。しかし、この判定マージンをとることで、判定が過剰となり必要以上に「干渉する」と誤判定することがある。これにより把持可能であるとする軌道の数が極端に減ってしまう。実施の形態２に係る干渉回避装置２によれば、前記過剰な誤判定を避ける目的で判定マージンを小さくしたい場合で、かつ、要求作業時間が短く動作が高速なために干渉リスクが高い場合でも、接近距離Ｌｉｊと接近速度Ｌｉｊ’に基づいて物体の破損レベルが低い速度に指令値を調整してハンド部４３を移動させることができる。

　前記のように周辺機器とロボットが接近する場合に破損レベルが低いということを保った上で、軌道全体に対しては必要に応じて動作速度を高速化することもできる。例えば、各モデル間の接近速度Ｌｉｊ’の速度上限値３１ａを満たす場合、特に実際の接近速度３０ａと速度上限値３１ａの差が大きい部分を探索し、それらの部分におけるロボットの移動速度を大きく調整することができる。この場合、図１８上側に示した接近速度３０ａが大きくなり、速度上限値３１ａに近づき、図１８下側に示した接近距離３０ｂについては時間軸（横軸）に対して圧縮し、縦軸方向の値は変化しない状況となる。ある時刻のロボット動作速度を上げていくことを繰り返すことで、第２の接近軌道生成部で生成された第２の接近軌道の動作速度を最大化することができるようになる。実際の調整方法としては、第２の接近軌道におけるロボットアームの姿勢によってロボットアームの各軸が動くことのできる最大加速度が変化することを考慮する必要がある。まず、ロボットアームの各姿勢において最大速度で動かした場合の接近速度および接近距離の関係を先に求める。そして、次に接近速度３０ａが速度上限値３１ａを超えている部分を見つけ、これらを修正するように調整することで速度を最大化することができる。

　実施の形態２に係る干渉リスク評価部２６は、干渉リスクとなる物体間の距離として、特にハンド部４３と周辺機器との相対距離と、ハンド部４３と周辺機器との相対接近速度に着目し、各構成部品が許容する干渉速度を超えるリスクがある場合には速度を低下させることで、作業効率の低下を抑制しながらハンド部４３および部品９ａの損傷を防止でき、ハンド部４３および部品９ａの交換に要する時間ロスがなく、ロボットシステム１００の更なる作業効率の向上を図ることができる。

実施の形態３．
　実施の形態３に係る干渉回避装置２は、実施の形態２に係る干渉回避装置２と同様に把持姿勢生成部２１、第１の接近軌道生成部２２、第２の接近軌道生成部２３、回避軌道生成部２４、干渉チェック部２５および干渉リスク評価部２６を備える。実施の形態３では、実施の形態２に係る第２の接近軌道生成部２３にポテンシャル法を適用した干渉回避装置２の動作例を説明する。ポテンシャル法とは、物体間に仮想的なエネルギー場を定義して、２つの物体間が近づくほど斥力（力の次元）が作用するものとして仮想外力を定義するものである。この仮想外力が作用したとして、この仮想外力をロボットアームに作用させ運動方程式に基づいてアームの変位を求めて、変位量を軌道の各地点での補正量として補正して動作させるのがポテンシャル法での軌道補正の考え方である。他にも、仮想外力によって物体が押しのけられる動きをするものとして位置や姿勢の軌道補正量（位置の次元）を表すベクトル量を演算する。ロボットモデルを物体の片方であるとして、各地点における指令値生成をする際に、演算された斥力あるいは軌道補正量ベクトルに基づいて元の指令値に対して一定量押しのけられた位置に働く定義とする。ポテンシャル法を用いた軌道修正とは、以上の様な手順に基づき仮想的に手先制御軌道を修正する方法である。実施の形態３に係る第２の接近軌道生成部２３は、干渉リスク評価部２６で得られた距離情報Ｌｉｊに応じた斥力である仮想外力をツール部４２に作用させて、第２の接近軌道を参照軌道としてポテンシャル法を用いて参照軌道を修正するものである。

　図１９は実施の形態３に係る干渉回避装置よって動作するロボットハンドの動作を示す図である。ハンド部４３には、干渉リスク評価部２６で得られた距離情報Ｌｉｊに応じた斥力である仮想外力３２が作用する。また干渉回避装置２は、図２に示すロボット制御装置３に対して、図１６に示される第２の接近軌道２３ａを含む干渉回避軌道２ａに加えて、仮想外力３２の示す仮想外力情報を出力する。

　ロボット制御装置３は、第２の接近軌道２３ａを第２の接近軌道Ｐとしてハンド部４３を追従させて、仮想外力情報に応じてインピーダンス制御を実施する。ポテンシャル法では、力の合力がそのまま移動速度として解釈され、適当な係数を掛けて位置補正量ΔＰとされ、第２の接近軌道Ｐ＋位置補正量ΔＰを目標値とした位置制御を行うことができる。ここで、第２の接近軌道Ｐ＋位置補正量ΔＰの求め方の例を示す。図１９では、ロボットのツール部先端が、第２の接近軌道Ｐに沿って速度ベクトルＶで方向に移動している場合、ツール部先端と箱の距離に応じて仮想外力３２が２か所作用している様子を示している。この場合、ロボット制御装置３は、仮想外力３２に応じて速度ベクトルＶの大きさと方向を補正し、補正された速度ベクトルをＶ’とする。ロボット制御装置３は、補正された速度ベクトルＶ’および制御周期ΔＴｃを掛けた量ΔＴｃ＊Ｖ’を、位置補正量Ｐ＋位置補正量ΔＰとして、目標値を補正する。速度ベクトルではなく、仮想外力Ｆと位置補正量ΔＰの関係で表現すると、インピーダンス制御の仮想バネ剛性をＫとして、外力作用Ｆに対して、Ｋ＊ΔＰが釣り合うような位置補正量ΔＰを選択することもできる。これにより、実施の形態３では、実施の形態２では得られなかったオンラインでの状況の変化への対応を実現し、急な環境変化が生じてもロボットシステム１００を破損させることなく動作させてシステム運用上の効率を維持できる。

実施の形態４．
　図２０は本発明の実施の形態４に係る干渉回避装置が備える軌道算出部の構成図である。以下、実施の形態１と同一部分には同一符号を付してその説明を省略し、ここでは異なる部分についてのみ述べる。実施の形態４に係る干渉回避装置２が備える軌道算出部１１Ｂは、実施の形態１に係る干渉回避装置２が備える軌道算出部１１と同様に、把持姿勢生成部２１、第１の接近軌道生成部２２、第２の接近軌道生成部２３、回避軌道生成部２４および干渉チェック部２５を備える。実施の形態４では、有効把持姿勢２１ａが第１の接近軌道生成部２２に入力されると共に、ハンド開閉速度３３とハンド開閉時刻３４とが把持接近制約として第１の接近軌道生成部２２に入力される。

　サーボガンを代表とする溶接技術の分野では溶接時間を短縮するためにアームの静定を見越してサーボガンのトリガを自動的に設定する技術がある。これと関連した事実として、把持においても、ハンド開閉速度および把持開始時間は、重要であり把持成功率に関連する。特に、第１の接近軌道を細かに規定する以上にロボットハンドに備え付けられたサーボガン毎にその軌道にあったハンド開閉動作を実行することが成功率を上げるために必要となる。

　実施の形態４に係る第１の接近軌道生成部２２は、把持接近拘束、ハンド開閉速度３３およびハンド開閉時刻３４を考慮した軌道生成およびハンド開閉指示を合わせて評価した有効第１の接近軌道２２ａを生成する。ハンド部４３が空気圧を用いたハンドである場合、当該ハンドの応答時間を部品毎に変えることができない。有効第１の接近軌道２２ａにおいてハンド開閉が間に合わない場合、回避軌道生成部２４にハンド開閉時刻を設定することで無駄な待ち時間を低減することができる。このようなハンド開閉時間の決定については、回避軌道生成部２４において有効第１の接近軌道２２ａに要する時間から判断する。

　以上に説明したように実施の形態４では、ハンド開閉時刻３４を考慮したピッキング動作を決定することができるため、ハンド開閉に係る１回分の動作の遅延を低減し、タクトタイムを短縮することができる。また実施の形態４では、把持接近制約として軌道に加えてハンド開閉速度を合わせて考慮することで、実施の形態１の効果に加えて、軌道上の把持動作をより細やかに制御でき、把持成功率を向上させることができる。

　図２１は本発明の実施の形態１から４に係る干渉回避装置のハードウェア構成図である。干渉回避装置２は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）６１、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）６２、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）６３、入力部６５を備える。干渉回避装置２では、ＣＰＵ６１、ＲＯＭ６２、ＲＡＭ６３および入力部６５がバスライン６６を介して接続されている。

　入力部６５は、マウスおよびキーボードを備え、使用者により入力される情報を入力する。入力部６５へ入力された情報は、ＣＰＵ６１へ送られる。ＲＯＭ６２には干渉回避装置２用のプログラム６０が格納されている。実施の形態１から４に係る干渉回避装置２を実現する場合、ＣＰＵ６１はＲＡＭ６３内にロードされたプログラム６０を実行することにより、干渉回避装置２の把持点候補生成部１０および軌道算出部１１が実現される。

　図２２は図２に示す生産システムの変形例を示す図である。図２２に示す生産システム２００は、図２に示す生産システム２００の構成要素に加えて画像処理装置１００１を備える。画像処理装置１００１は計測部１および干渉回避装置２の間に設けられている。画像処理装置１００１は、干渉回避装置２から抜き出された把持点候補生成部１０の機能を有する装置である。すなわち画像処理装置１００１は、計測データ１ａに画像処理を行い把持点候補１０ａを算出する図４の把持点候補生成部１０を備えた装置である。

　図２３は図２２に示す干渉回避装置の構成例を示す図である。図２２に画像処理装置１００１を用いる場合、図２３に示すように干渉回避装置２では図４の把持点候補生成部１０が除かれ、干渉回避装置２は、把持点候補１０ａ、モデル情報３ａ、ロボットプログラム３ｂおよび設定情報３ｃを入力情報とする軌道算出部１１を備える。

　また図２２の生産システム２００は、画像処理装置１００１から画像処理結果である把持点候補１０ａを伝送するように構成されているため、イーサネット（登録商標）等のネットワークを介して処理結果を共有することができる。

　図２４は図２２に示す生産システムの第１の変形例を示す図である。図２４に示す生産システム２００は、画像処理の結果得られた把持点候補１０ａがロボット制御装置３で受信されるように構成されている。なお生産システム２００は、把持点候補１０ａがロボット制御装置３および干渉回避装置２の双方で受信されるように構成してもよい。

　図２５は図２２に示す生産システムの第２の変形例を示す図である。図２５に示す生産システム２００は、ＰＬＣやパーソナルコンピュータ等を用いた情報統合端末５を備え、全ての情報をネットワークを介して情報統合端末５で一括で管理して、状態に応じて全体に動作変更の指示あるいは指令を出力するように構成されている。情報統合端末５には把持点候補１０ａが入力され、情報統合端末５に入力された把持点候補１０ａは、ロボット制御装置３を介して干渉回避装置２に入力される。

　図２６は図２２に示す生産システムの第３の変形例を示す図である。図２６に示す生産システム２００は、複数の画像処理装置１００１，１００２と、複数の計測部１，１Ａとを備える。計測部１Ａは、固定台７０によりロボット４を撮影可能な位置に設置されている。計測部１Ａは、ロボット４の動作を撮像して得られた撮像情報を計測データ１ａとして画像処理装置１００２に送信する。

　画像処理装置１００２は、画像処理装置１００１と同様に構成されおり、計測データ１ｂに基づき把持点候補１０ｂを算出する。算出された把持点候補１０ｂは情報統合端末５に入力される。情報統合端末５は把持点候補１０ａ，１０ｂを集約して、状態に応じて全体に動作変更の指示あるいは指令を出力する。

　なお本実施の形態では、ロボット４に固定された計測部１およびロボット４以外の場所に設置された計測部１Ａを用いた例が示されるが、本実施の形態に係る生産システム２００は、計測部１および計測部１Ａの何れか一方のみ用いた構成としてもよい。また図示しないが、干渉回避装置２がロボット４以外の対象全体の干渉回避軌道を計算する場合、情報統合端末５の中に干渉回避装置２を含む構成にして、情報統合端末５がコンベア制御装置６、加工機制御装置７およびロボット制御装置８のそれぞれの干渉回避装置２として機能する構成としてもよい。

実施の形態５．
　図２７は本発明の実施の形態５に係る干渉回避装置が備える軌道算出部の構成図である。図２７に示す軌道算出部１１Ｃは、図６に示す第１の接近軌道生成部２２および第２の接近軌道生成部２３の代わりに、第１の接近軌道生成部２２Ａおよび第２の接近軌道生成部２３Ａを備える。軌道算出部１１Ｃは、第１の接近軌道生成部２２Ａおよび第２の接近軌道生成部２３Ａ以外にも、図６に示す把持姿勢生成部２１、回避軌道生成部２４および干渉チェック部２５を備える。図２７では、図６の把持姿勢生成部２１、回避軌道生成部２４および干渉チェック部２５の図示が省略されている。以下、実施の形態１と同一部分には同一符号を付してその説明を省略し、ここでは異なる部分についてのみ述べる。

　第１の接近軌道生成部２２Ａは、軌道要素生成部８３および接近軌道生成部８４を備える。

　軌道要素生成部８３は、第１の接近軌道に関するＭ個の中間ゴール情報８１と有効把持姿勢２１ａとに基づき、Ｍ＋１個の第１の接近軌道要素１３ａを演算する。Ｍは１以上の整数である。ｋは１からＭ＋１までの変数である。軌道要素生成部８３は、ｋ＝１から、ｋ＝Ｍ＋１まで順に、第１の接近軌道要素１３ａを演算する。第１の接近軌道要素は、２点の位置(把持点、経由点、動作開始点、動作完了点のうちの２つ)を両端点として、両端点を補間して定義される。補間方法については、スプライン補間を例示できる。中間ゴールとは、ツール部の目標位置である把持点候補Ｐｇｇと、ツール部の目標位置Ｐｇｇから一定距離離れた位置を示す第１の接近軌道の起点であるツール部の把持接近開始点Ｐｓｇとの間に定義される軌道の経由点のことである。中間ゴール情報８１は、中間ゴールの内容を示す情報である。

　接近軌道生成部８４は、ｋ＝１から、ｋ＝Ｍ＋１まで順に演算された第１の接近軌道要素１３ａをそれぞれ記憶し、記憶した順に、複数の第１の接近軌道要素１３ａのそれぞれを接続することによって、有効第１の接近軌道２２ａを生成する。例えば、１番目に演算された第１の接近軌道要素１３ａに、２番目に演算された第１の接近軌道要素１３ａが接続され、２番目に演算された第１の接近軌道要素１３ａに、３番目に演算された第１の接近軌道要素１３ａが接続される。このような処理が、ｋ＝１に演算された第１の接近軌道要素１３ａから、ｋ＝Ｍ＋１に演算された第１の接近軌道要素１３ａまで、順に実行されることによって、有効第１の接近軌道２２ａが生成される。ｋ＝１のときは、把持点候補Ｐｇｇと第１の接近軌道に関する１番目の中間ゴールとを接続する接近軌道要素を演算することになる。ｋ＝Ｍ＋１のときは、第１の接近軌道に関するＭ番目の中間ゴールと第１の接近軌道の起点である把持接近開始点Ｐｓｇとを接続する接近軌道要素を演算することになる。２つの接近軌道要素の接続の仕方として、ロボット動作が滑らかに遷移する目的で次のような接近軌道要素の接続方法を適用することができる。１つ目の接続の仕方は、速度を考慮した接続方法である。第１の接近軌道生成部は、接続点である中間ゴール付近で生じる慣性力や遠心力の影響を考慮し、低速かつ加減速が緩やかになるように接近軌道を定義することができる。この際、２つの接近軌道要素の接続点が不連続にならない軌道として接続し、かつ接続点近傍の速度指令値が大きくならないように接続する。これは、接続点前後で動作軌道で定義される移動方向が速度の大きさが急激に変化する場合、適切に減速して滑らかなロボット動作を実現させるという、接続の仕方である。通常、軌道には必ずしも速度情報は必要無いが、ここではロボットのツール部が実現しうる通過速度である必要があるため、軌道上の任意の点においてロボットのツール部の通過速度が指定される場合として説明している。２つめの接続の仕方は、スプライン曲線を使って経由点を滑らかに接続する接続の仕方である。第１の接近軌道生成部は、第１の接近軌道の起点である把持接近開始点Ｐｓｇと終点である把持点候補Ｐｇｇと中間ゴールとに基づいて、スプライン補間して、滑らかな軌道を生成することも出来る。ここで、中間ゴールの役割を説明する。実施の形態１では、有効把持姿勢２１ａが決まると、ツール部の把持接近開始点Ｐｓｇとツール部の把持点候補Ｐｇｇとを用いて、軌道候補が計算されている。一方、本実施の形態では、ピッキングに必要な有効把持姿勢で表される把持点から算出される把持点候補Ｐｇｇ以外に、Ｍ個の中間ゴールを、必ず通過する経由点として設定している。これらの中間ゴールは、いくつかの方法で得られる。例示すると、ユーザが事前の作業試行をロボットに行わせた後に成功事例から自動的に中間ゴールを抽出する場合、または事前のシミュレーションで成功事例から学習し自動的に中間ゴールを抽出する場合がある。すなわち、把持点に対して法線方向から真っ直ぐ掴みにいくと把持し損ねるような場合には、把持位置直前の目標地点を与える必要が出てくる。この目標地点が中間ゴール８１となる。Ｍ＝１として中間ゴールを１つ設定する場合を説明する。対象物を把持する場合で、把持位置直前の接近ベクトル方向に制約がある場合を考える。ここで「制約」とは、接近ベクトル方向として指定できないベクトルが存在する場合に、その接近ベクトル方向を指す。指定できない接近ベクトル方向は、ロボットが当該接近ベクトル方向に移動すると次のような事象が生じる場合に設定される。前記事象としては、具体的にロボットのツール部やエンドエフェクタが障害物への干渉する事象、把持対象の作業に支障がある作用を及ぼす（把持対象の転倒・把持対象の損傷）事象、などが挙げられる。把持姿勢から１０ｍｍオフセットした位置から、姿勢を変えずに指先を把持点に接近させる場合は、把持点候補Ｐｇｇから１０ｍｍオフセットした位置に中間ゴールを設定する。ここで、「オフセットした位置」は、「ある基準となる位置から、ある方向に一定量の位置（ｍｍ）だけ平行移動した別の位置」を意味する。ここで示した「１０ｍｍオフセットした位置」は、制約として説明した「接近ベクトル方向として指定できないベクトル」を除いた、ロボットが把持成功できる方向に平行移動した位置となっている。一方で、中間ゴールではない点（把持点候補Ｐｇｇや把持接近開始点Ｐｓｇ）と中間ゴールの決め方の違いについて補足説明する。把持点候補は前述の通り、計測データに基づいて計算される。第１の接近軌道の把持点Ｐｇｇではない側の端点、すなわち把持接近開始点Ｐｓｇは、取り出す部品と周囲の部品との干渉が無くなる高さや位置まで、取り出す部品を把持したロボットが移動した位置として設定する。すなわち、中間ゴールとは違う位置を設定する。ただしここでは供給箱に入っていて把持点候補として選ばれている部品を「取り出す部品」と呼び、供給箱に入っているそれ以外の部品を「周囲の部品」と呼んでいる。また、中間ゴールはあくまで、把持位置直前の目標地点を定義することが必要となる場合に定義されるものであり、把持位置直前の接近ベクトル方向に制約が無ければ中間ゴールは設定されない。

　第２の接近軌道生成部２３Ａは、軌道要素生成部８５および接近軌道生成部８６を備える。

　軌道要素生成部８５は、有効第１の接近軌道２２ａの内、有効第１の接近軌道２２ａの起点である把持接近開始点Ｐｓｇと、ロボット動作開始点である第２の接近軌道２３ａの開始点であるピッキング動作の開始点と、第２の接近軌道に関するＮ個の中間ゴール情報８２とに基づき、Ｎ＋１個の第２の接近軌道要素１３ｂを演算する。Ｎは１以上の整数である。ｊは１からＮ＋１までの変数である。軌道要素生成部８５は、ｊ＝１から、ｊ＝Ｎ＋１まで順に、第２の接近軌道要素１３ｂを演算する。ｊ＝１のときは、軌道要素生成部８５は、有効第１の接近軌道２２ａの把持接近開始点Ｐｓｇと第２の接近軌道に関する１番目の中間ゴールとを接続する接近軌道要素を演算する。ｊ＝Ｎ＋１のときは、軌道要素生成部８５は、第２の接近軌道に関するＮ番目の中間ゴールとピッキング動作の開始点とを接続する接近軌道要素を演算する。中間ゴール情報８２は、ロボットツール部の、位置（Ｘ，Ｙ，Ｚ）および姿勢（回転角（Ａ，Ｂ，Ｃ）/回転行列Ｒ）で定義される情報である。ただし、第２の接近軌道２３ａの開始点であるピッキング動作の開始点については、軌道要素生成部８５内で定義されるものとする。例えば、ピッキング動作の開始点は、［００７６］に説明している通り、対象となる部品の入った棚や箱（周辺環境の一部）の上空か、アームやハンドと周辺環境が干渉を生じない程度に十分離れた点がユーザによって設計され指定される。また、それ以外の方法として、ピッキング動作の開始点は、［００８３］、［００８４］に説明している通り、探索的に獲得した点を定義しておくことが出来る。探索的に獲得する場合も探索の終了条件としては、箱からハンドが十分離れた点をユーザが定義して設定する。

　接近軌道生成部８６は、ｊ＝１から、ｊ＝Ｎ＋１まで順に演算された第２の接近軌道要素１３ｂをそれぞれ記憶し、記憶した順に、第２の接近軌道要素１３ｂのそれぞれを接続することによって、第２の接近軌道２３ａを生成する。

　ここで、図５に示したように、把持対象となる部品とハンドの幾何形状とを考慮して、ハンド部４３の指先が部品９ａの凸部１２に干渉しないように、有効第１の接近軌道２２ａが生成される。ところが、有効第１の接近軌道２２ａの生成において、干渉を避ける軌道探索を行う手法を適用する場合、中間ゴールを設定する必要が生じるときがある。例えば、把持ハンドの指先の開閉と連動して、把持姿勢の直前の中間ゴールにおいて把持ハンドの指をわずかに閉じる動作を、ロボット動作の１つとして実施することによって、把持対象を幾何的にハンドの指先の空間にトラップした状態に遷移させて、さらに把持対象を目標把持点まで移動してから把持する場合を例示できる。なお、把持対象を幾何的にハンドの指先の空間にトラップした状態とは、空間運動に制約を生じている状態、またはケージングの状態である。空間運動に制約を生じている状態は、例えば、把持ハンドの指先が部品よりわずかに大きな領域で近接している場合に、部品が把持ハンドの指先の中で回転しようとすると指先に接触してしまい、部品が回転できなくなるような状態を表している。指の間の隙間から外側に出るほど把持対象部品が大きく変位することがなく、把持不可能とはならない範囲の姿勢変化にとどめることが可能である。このような中間ゴールを使った把持動作により把持点候補Ｐｇｇに移動しようとした際に、転倒しやすい部品の転倒を防ぐことが出来る。ケージングとは、接触した幾何拘束はないが、いずれの方向にも抜け出ることが出来ないように周囲を囲う物体拘束である。この場合も、指先開閉による部品への作用力以外の外力により部品が変位する場合、部品が転倒あるいは把持不可能になってしまう事象を防ぐことが目的となる。

　同様の理由で、第２の接近軌道２３ａの生成時にも、中間ゴールを設けることによって、把持成功率を高める動作を設定することができる。例えば、物体の搬送中に、物体の重心位置がロボットハンドの把持点から離れて、モーメントが大きくなった場合、搬送中の物体が落下するおそれがある。このようなことを防ぐため第２の接近軌道における中間ゴールとして、ロボット手先姿勢を次のような方法で設定した中間ゴールを選定することができる。第２の接近軌道生成部２３Ａは、把持点と物体の重心位置に対して、鉛直方向に直交する方向に対する距離を最小化し、把持ハンドから把持対象がモーメントの作用で脱落を防ぐような安定把持姿勢を、中間ゴールのロボットの把持ハンドの姿勢として設定できる。鉛直方向に直交する方向に対する距離は、ロボットハンドの把持点から物体の重心位置までの距離に相当する。把持ハンドの姿勢に対応する中間ゴールの位置については次のように計算できる。第２の接近軌道生成部２３Ａは、部品箱９と把持物体が当該安定把持姿勢においても把持ハンドと周辺環境が干渉しない高さを演算し、干渉しない高さに到達するまでは前記安定把持姿勢を取らず、干渉しない高さ到達してから前記安定把持姿勢を取る点を中間ゴールの位置として設定できる。ここで、干渉しない高さは、例えば、事前に計測された対象物データから把持姿勢を算出しておき、算出された把持姿勢と事前に取得した物体の幾何データとから、部品箱等と干渉しない距離を演算することによって求めることができる。

　以上のように、実施の形態５に係る干渉回避装置によれば、実施の形態１の効果に加えて、把持物体の把持状態からの脱落を防ぐことができ、結果としてシステムの生産性が向上するという効果が得られる。

実施の形態６．
　図２８は本発明の実施の形態６に係る干渉回避装置が備える軌道算出部の構成図である。図２８に示す軌道算出部１１Ｄは、図６に示す第１の接近軌道生成部２２および第２の接近軌道生成部２３の代わりに、第１の接近軌道生成部２２Ｂおよび第２の接近軌道生成部２３Ｂを備える。また軌道算出部１１Ｄは、図６に示す把持姿勢生成部２１の代わりに、組立姿勢候補生成部２７と組立位置候補生成部２８を備える。実施の形態６では、物体の把持では無く、把持した物体の組み立て作業に構成が変わるのに応じ、把持点候補を「組立姿勢候補」と置き換え、把持接近開始点（第１の接近軌道の起点）を「組立動作開始点」と置き換える。ロボット動作開始点であるピッキング動作の開始点を「組立動作全体の開始点」と置き換える。なお、組立位置候補生成部２８は、軌道算出部１１Ｄの外部に設けられていてもよい。以下、実施の形態１と同一部分には同一符号を付してその説明を省略し、ここでは異なる部分についてのみ述べる。

　まず、ハンド部４３は、カメラまたは画像センサを活用して、部品９ａの位置を計測し、部品を把持する。この際、供給形態がバラ積み供給で、部品がバラ積み状態である場合は、ロボットは部品９ａを把持した後に部品の上空である実施の形態１と同じような処理で、組立動作全体の開始点Ｐｓａに移動する。その後、ロボットは軌道算出部１１Ｄで生成された軌道に従い動作する。組立動作全体の開始点Ｐｓａは、第２の接近軌道生成部にて生成される。組立動作開始点に至るまで、または、組立動作開始点において、干渉回避装置２は、計測部であるビジョンセンサまたはカメラを活用して、把持している物体の位置姿勢を計測し、計測した計測情報（ＲＧＢ（Ｒｅｄ　Ｇｒｅｅｎ　Ｂｌｕｅ）画像情報、ポイントクラウドなどの距離画像情報、距離情報）画像認識処理を施すことによって、ハンドで把持した対象の位置姿勢を演算する。画像認識処理では、モデルマッチング処理等が利用される。これにより、部品がハンドに対してどのように把持されているかという、「部品の位置姿勢」が取得される。ただし、部品の位置姿勢については、部品把持のために取得した部品の位置姿勢に対し、把持ハンドが滑りおよび位置ずれの無い理想的な状態で部品の把持を成功したものとして、把持前に計測した部品の位置姿勢情報に基づいて演算することも出来る。組立位置候補生成部２８は、部品の位置姿勢に基づいてロボットのツール位置姿勢で表現された把持姿勢情報８７を演算する。組立位置候補生成部２８は、ロボットおよび周辺機器のモデル情報とに基づき、把持対象のツール部に対する把持姿勢情報８７を使って、ツール部が把持対象の組立を完了する複数の第１の組立姿勢候補８８を算出する。実施の形態１～実施の形態５では、把持姿勢生成部２１が生成できる姿勢候補は複数ある場合が多い。一方、実施の形態６においては、図２９に示すような部品９ａを部品９ｂに対して組み付ける場合のように完了時における候補姿勢が１つしかなく、第１の組立姿勢候補８８は１つのみ設定する場合がある。実施の形態６において、組立位置候補生成部２８が生成する第１の組立姿勢候補８８が複数ある場合の例として、部品形状が直方体、円柱または円盤である場合が挙げられる。部品形状が直方体、円柱または円盤である場合、部品の対称性により、組立位置候補生成部２８が第１の組立姿勢候補８８を複数個設定する。組立姿勢候補生成部２７は、許容される誤差を含めて組立て作業を実現することが可能な第２の組立姿勢候補を生成する。すなわち、組立姿勢候補生成部２７は、第２の組立姿勢候補に対し、ツール部が周辺機器に干渉することなく、かつ許容姿勢範囲内に存在する複数の有効組立姿勢候補２７ａを生成する。有効組立姿勢候補２７ａには、位置情報と姿勢情報（［Ｘ，Ｙ，Ｚ，Ａ，Ｂ，Ｃ］）とが含まれる。

　図２９は図２８に示す軌道算出部による動作を説明するための図である。図２９には、ロボットのツール部４２が部品供給箱である部品箱９に接近し、ハンド部４３が部品箱９内の把持対象物である部品９ａを把持してから、ハンド部４３が部品９ａを部品９ｂの上側に移動して、ハンド部４３が把持している部品９ａを部品９ｂに組み付けるまでの動作が示される。

　部品箱９から把持対象物を取り出すロボットは、物体の把持動作以外にも、工場において製品の組立て作業を実施することがある。図２９で示すように、部品９ｂに部品９ａを組み付けることによって製品が組立てられる場合、部品９ｂには、スナップフィットによって部品９ａが嵌め合わされる固定部８０、コの字形状の突起部９ｂ１等が存在する場合がある。スナップフィットとは、２以上の材料の結合に用いられる機械的接合法の一種であり、材料の弾性を利用して２以上の材料同士を嵌め合わせることにより、２以上の材料を互いに固定する方式のことである。そのため、ロボットは、製品の組立て作業の際、部品９ａの先端が部品９ｂの突起部９ｂ１に干渉することがなく、また部品９ａの水平方向の両端が固定部８０の上面に載ることがないように、部品９ｂに部品９ａを組み付ける必要がある。

　ただし、部品９ｂへの部品９ａの組立完了直前の位置および角度と、部品９ｂへの部品９ａの組立完了位置および角度とを比較した場合、部品９ｂの延伸方向に対する部品９ａの延伸方向の角度が異なるだけでなく、部品９ｂの固定部８０および突起部９ｂ１といった部材に対する部品９ａの先端部の位置が異なる。組立完了位置は、部品９ｂに存在する複数の部品９ａの内、最も底側に存在する部品９ａの位置である。このように、固定部８０および突起部９ｂ１といった部材を有する部品９ｂへ部品９ａを組み付ける場合、部品９ｂへの部品９ａの挿入方向、挿入角度等に制約が生じるときがある。また、例えば部品９ａの組立完了位置を部品９ｂの鉛直上方に延長した位置から、鉛直方向に部品９ｂへの部品９ａの組立動作を開始した場合、部品９ｂの突起部９ｂ１に部品９ａが干渉するおそれがある。従って、部品９ａの最終到達点である組立位置候補点以外に、中間ゴールである位置に部品９ａを経由させる等して、部品９ｂに対する部品９ａの相対位置、部品９ｂに対する部品９ａの相対角度等を、細かく変更しなければ、組立てを完了することができない。すなわち、実施の形態５の第１の接近軌道生成部２２Ａで生成される有効第１の接近軌道２２ａをさらに複数の軌道に分割し、また第２の接近軌道生成部２３Ａで生成される第２の接近軌道２３ａをさらに複数の軌道に分割しなければ、組立てが完了しない場合がある。

　以下では、把持された対象物体が別の対象物体に組み付けられるときにおける、実施の形態６に係る軌道算出部１１Ｄの動作を説明する。図３０は図２８に示す軌道算出部による動作を説明するためのフローチャートである。

　ハンド部４３は、供給された部品９ａを把持し（Ｓ１０）、干渉回避装置２は、ハンド部４３が把持した部品９ａの位置姿勢を演算する（Ｓ１１）。作業対象の組立姿勢候補点が生成される（Ｓ１２）。次に、Ｓ１３において、軌道算出部１１Ｄの第１の接近軌道生成部２２Ｂは、組立の中で組立方向あるいは挿入方向が制約されている部品移動、および組立対象と把持している部品９ａが接触を伴う部品移動からなる第１の接近軌道を生成する。軌道算出部１１Ｄの第１の接近軌道生成部２２Ｂは、生成された複数の有効組立姿勢候補２７ａの中で、把持している部品９ａおよび把持ハンドが組立対象をずらすことない状態を満たしつつツール部が組立対象に接近できる組立姿勢を抽出する。次に、第１の接近軌道生成部２２Ｂは、抽出された組立姿勢となるための複数の組立軌道候補を生成し、複数の組立姿勢候補のそれぞれを端点に持つ複数の軌道を第１の接近軌道として生成する。具体的には、第１の接近軌道生成部２２Ｂは、Ｍ個の中間ゴールに対し、ｋ番目（ここではｋ＝１）の第１の接近軌道要素１３ａ１を生成する。図２９には、このとき生成される第１の接近軌道要素１３ａ１が上向きの湾曲した矢印で示される。続いて、第１の接近軌道生成部２２Ｂは、固定部８０に押し付ける要素動作のための軌道を設け、その位置まで部材を挿入する要素動作として、ｋ＋１番目の第１の接近軌道要素１３ａ２を生成する。図２９には、このとき生成される第１の接近軌道要素１３ａ２が左上向きの矢印で示される。第１の接近軌道要素１３ａ２は、図２９の部品９ｂに形成される突起部９ｂ１へ部品９ａの先端を挿入するときの移動量と水平方向に対する傾きを含む。第１の接近軌道要素１３ａ１と第１の接近軌道要素１３ａ２とを組み合わせて形成される軌跡は、部品９ｂへの部品９ａの組立動作時における、有効第１の接近軌道２２ａとなる。図２９には、有効第１の接近軌道２２ａに関する中間ゴール情報８１の数が１つの例、すなわちＭ＝１の例が示される。従って、軌道算出部１１Ｄの第１の接近軌道生成部２２Ｂでは、第１の接近軌道要素１３ａがＭ＋１個、すなわちＭ＝１の例では２つ生成されるため、生成された２つの第１の接近軌道要素１３ａのそれぞれが接続される形で、有効第１の接近軌道２２ａが生成される。

　Ｓ１４において、干渉チェック部２５では、有効把持姿勢２１ａおよび有効第１の接近軌道２２ａに対して干渉チェックを実施する。干渉チェックの結果、干渉する可能性が有る場合（Ｓ１５，Ｎｏ）、干渉チェック部２５は、部品箱９と干渉する可能性のあるツール部４２の一部を生成し、許容姿勢範囲において干渉をなくすために最も小さい姿勢変化をさせた場合の修正した把持点候補を探索し、干渉の可能性の有無を判定する（Ｓ１６）。

　許容姿勢範囲に他の解が無い、すなわち許容姿勢範囲において干渉の可能性が無くなる演算結果が得られない場合（Ｓ１６，Ｎｏ）、Ｓ１３からＳ１５までの処理が繰り返される。干渉の可能性が無くなる演算結果が得られない場合、把持姿勢生成部２１より、優先順位が次に高い新しい把持点候補を選び、この候補点に関する評価を行う。

　許容姿勢範囲に他の解が有る、すなわち許容姿勢範囲において干渉の可能性が無くなる演算結果が得られる場合（Ｓ１６，Ｙｅｓ）、干渉回避装置２では有効把持姿勢２１ａが修正され（Ｓ１７）、Ｓ１４からＳ１５までの処理が繰り返される。

　Ｓ１５において干渉する可能性が無い場合（Ｓ１５，Ｙｅｓ）、軌道算出部１１Ｄの第２の接近軌道生成部２３Ｂは、有効第１の接近軌道２２ａの端点のうち組立対象から最も遠い位置にある第１の端点に、ロボットのアーム部およびツール部が周辺機器に干渉することなく接近する第２の接近軌道２３ａを生成する（Ｓ１８）。具体的には、第２の接近軌道生成部２３Ｂは、Ｎ個の中間ゴール情報８２に対し、ｊ番目（ここではｊ＝１）の第２の接近軌道要素１３ｂ１を生成する。なお、図２９に示す符号１３ｂ１はｊ＝1番目の第２の接近軌道要素を示す。ここでは、中間ゴール情報８２には、部品９ｂの上側の位置で、部品９ａの把持姿勢を、把持搬送に有利である脱落しにくい把持姿勢から組立に都合の良い把持姿勢に遷移させることを意図した経由点が設定される。このときｊ＝１番目の第２の接近軌道要素１３ｂ１となる軌道の端点１３ｂ１ａは、組立開始点に接続される。組立開始点は、Ｍ＋１番目の第１の接近軌道要素１３ａ２の組立位置よりも遠い方の端点１３ａ２ａである。図２９には、第２の接近軌道２３ａに関する中間ゴール情報８２の数が１つの例、すなわちＮ＝１の例が示される。第２の接近軌道要素１３ｂ２の端点は、組立動作全体の開始点Ｐｓａに接続する。なお、供給位置から部品９ａを取り出し部品９ｂの上空に対する移動は、搬送動作と呼ぶこととし区別している。従って、軌道算出部１１Ｄの第２の接近軌道生成部２３Ｂでは、第２の接近軌道要素１３ｂがＮ＋１個、すなわち２つ生成されるため、生成された２つの第２の接近軌道要素１３ｂのそれぞれが接続される形で、第２の接近軌道２３ａが生成される。

　干渉チェック部２５では再び干渉チェックが行われる（Ｓ１９）。第２の接近軌道２３ａに対して干渉の可能性が有る場合（Ｓ１９，Ｎｏ）、Ｓ１８およびＳ１９の処理が繰り返される。第２の接近軌道２３ａに対して干渉の可能性が無い場合（Ｓ１９，Ｙｅｓ）、回避軌道生成部２４は、有効第１の接近軌道２２ａと第２の接近軌道２３ａとを接続した干渉回避軌道２ａを生成する（Ｓ２０）。有効第１の接近軌道２２ａと第２の接近軌道２３ａとの接続方法について説明する。有効組立姿勢候補２７ａ、有効第１の接近軌道２２ａ、第２の接近軌道２３ａがそれぞれ複数出力され、組み合わせが複数存在する場合には、評価関数を用いて評価値が最も小さくなるあるいは最も大きくなるもの軌道を選択する。評価関数としては、干渉回避軌道全体の移動に相当する総作業時間を入力としてが時間に関する評価値が最小化するような軌道を選択することができる。また、評価関数としては、ロボット動作による電流値に基づくエネルギー消費量などを入力として、エネルギー消費量が最小化するよう評価値を算出することが出来る。評価関数の方法としては、これらに限定するものではない。

　以上のように、実施の形態６に係る干渉回避装置によれば、部品９ｂに部品９ａを組み付けることによって製品が組立てられる場合でも、有効第１の接近軌道２２ａが複数の軌道に分割され、また第２の接近軌道２３ａが複数の軌道に分割されるため、部品９ｂに固定部８０等が存在する場合でも、部品９ｂに部品９ａが干渉することなく、部品９ｂに部品９ａを組み付けることができる。

実施の形態７．
　図３１は本発明の実施の形態７に係る干渉回避装置が備える軌道算出部の構成図である。図３１に示す軌道算出部１１Ｅは、図６に示す把持姿勢生成部２１、第１の接近軌道生成部２２、第２の接近軌道生成部２３、回避軌道生成部２４および干渉チェック部２５に加えて、シミュレーション部３００を備える。シミュレーション部３００は、３Ｄモデルシミュレーション部３０１、学習器３０２、第１の接近軌道候補記憶部３０３および第２の接近軌道候補記憶部３０４を備える。

　３Ｄモデルシミュレーション部３０１は、３Ｄ－ＣＡＤモデル、または３次元計測で得られた３Ｄモデルを元に、構築された仮想空間においてロボット動作をシミュレーションすることによって、動作に必要な駆動力と、ロボットハンドまたはロボットアームが通過する動作軌道と、動作起動中の周辺環境との干渉に関する干渉判定情報と、を含むシミュレーション結果３１１を出力する。

　学習器３０２は、これらのシミュレーション結果３１１を入力し、評価関数を基にして一定の基準を満たす効率的な動作と干渉の無い動作とを抽出して、それらの動作軌道と動作に必要な駆動力とに関する情報として、ロボット動作軌道候補および動作パラメータ制約に関する情報３１２を出力する。ここで動作パラメータ制約とは、ロボットの関節可動範囲、直交座標表現のロボットの可動範囲などの、許容する探索空間を狭めるための条件、または駆動軸の加減速度の上限等を狭めるための上限である。

　３Ｄモデルシミュレーション部３０１は、情報３１２と、実際の作業時にカメラ等で取得する対象物体の計測データを元にして得られる把持点候補とに基づいて、第１の接近軌道候補３１３と第２の接近軌道候補３１４とを生成して出力する。他に、３Ｄモデルシミュレーション部３０１は、情報３１２と、仮想環境でカメラモデルを用いてシミュレーション上で撮像処理し、仮想環境中で得られる把持点候補に基づいて、第１の接近軌道候補３１３と第２の接近軌道候補３１４とを生成して出力することもできる。第１の接近軌道候補３１３は第１の接近軌道候補記憶部３０３に記憶され、第２の接近軌道候補３１４は第２の接近軌道候補記憶部３０４に記憶される。シミュレーション部３０１では実際のロボットで想定される状況を想定した物体配置や把持点候補に基づいて、計算処理能力が許容可能な回数だけシミュレーションを事前実施しておくことができるため、第１の接近軌道候補３１３および第２の接近軌道候補３１４は、あらゆる把持点候補および周辺環境の位置関係に対応した形で事前に記憶しておくことが出来る。すなわち、実際にロボットが稼動する際には、出力された有効把持点候補２１ａの情報に基づいて、把持成功率と周辺との干渉を考慮した第１の接近軌道候補生成部は短時間に第１の接近軌道を出力できる。同様に第２の接近軌道生成部は短時間で第２の接近軌道を出力することが出来る。

　第１の接近軌道生成部２２は、有効把持姿勢２１ａに対応する第１の接近軌道候補３１３を、第１の接近軌道候補記憶部３０３から選択して、選択した第１の接近軌道候補３１３を、有効第１の接近軌道２２ａとして出力する。第２の接近軌道生成部２３は、有効第１の接近軌道２２ａに対応する第２の接近軌道候補３１４を、第２の接近軌道候補記憶部３０４から選択して、選択した第２の接近軌道候補３１４を、第２の接近軌道２３ａとして出力する。

　以上のように、実施の形態７に係る干渉回避装置によれば、実施の形態１の効果に加えて、動作計画を実施する処理時間が短縮され、生産時のタクトタイムが短くなるという効果が得られる。

　以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。

　１，１Ａ　計測部、１ａ　計測データ、２　干渉回避装置、２ａ　干渉回避軌道、３，８　ロボット制御装置、３ａ　モデル情報、３ｂ　ロボットプログラム、３ｃ　設定情報、４　ロボット、５　情報統合端末、６　コンベア制御装置、７　加工機制御装置、９　部品箱、９ａ，９ａ１，９ｂ　部品、９ａ１１　長手片、９ａ１２　短手片、９ｂ１　突起部、１０　把持点候補生成部、１０ａ，１０ｂ，１７　把持点候補、１１，１１Ａ，１１Ｂ，１１Ｃ，１１Ｄ，１１Ｅ　軌道算出部、１２　凸部、１３　第１の接近軌道、１３ａ，１３ａ１，１３ａ２　第１の接近軌道要素、１３ａ２ａ，１３ｂ１ａ　端点、１３ｂ，１３ｂ１，１３ｂ２　第２の接近軌道要素、１４　理想アプローチ方向、１５　許容アプローチ方向、１８　許容進行ベクトル、１９　許容退避ベクトル、２１　把持姿勢生成部、２１ａ　有効把持姿勢、２２，２２Ａ，２２Ｂ　第１の接近軌道生成部、２２ａ　有効第１の接近軌道、２３，２３Ａ，２３Ｂ　第２の接近軌道生成部、２３ａ　第２の接近軌道、２４　回避軌道生成部、２５　干渉チェック部、２６　干渉リスク評価部、２７　組立姿勢候補生成部、２７ａ　有効組立姿勢候補、２８　組立位置候補生成部、２８ａ　組立位置候補、３０ａ　接近速度、３０ｂ　接近距離、３１ａ　速度上限値、３１ｂ　閾値、３２　仮想外力、３３　ハンド開閉速度、３４　ハンド開閉時刻、４１　アーム部、４２　ツール部、４３，４３－１，４３Ａ，４３Ｂ，４３Ｃ　ハンド部、４３－１ａ　吸着パッド、４４　ジグ、５０　床面、６０　プログラム、６１　ＣＰＵ、６２　ＲＯＭ、６３　ＲＡＭ、６５　入力部、６６　バスライン、７０　固定台、８０　固定部、８１，８２　中間ゴール情報、８３，８５　軌道要素生成部、８４，８６　接近軌道生成部、８７　把持姿勢情報、８８　第１の組立姿勢候補、９１　底面部、９２ａ，９２ｂ　測面部、９３　上面部、９４　開口部、１００　ロボットシステム、１０１　ロボットハンドモデル、１０２　周辺機器モデル、１０３　ツールモデル、２００　生産システム、３００　シミュレーション部、３０１　３Ｄモデルシミュレーション部、３０２　学習器、３０３　第１の接近軌道候補記憶部、３０４　第２の接近軌道候補記憶部、３１１　シミュレーション結果、３１２　情報、３１３　第１の接近軌道候補、３１４　第２の接近軌道候補、１００１，１００２　画像処理装置。

Claims (9)

1. 　少なくとも周辺機器の状態を計測した計測データが入力され、ロボットのアーム部に設けられたツール部が前記周辺機器に干渉することを回避させる干渉回避装置であって、
   　前記計測データと、前記ロボットおよび前記周辺機器のモデル情報とに基づき、前記ツール部の把持対象を前記ツール部が把持可能な複数の把持点候補を算出する把持点候補生成部と、
   　前記複数の把持点候補に基づき、前記ツール部が前記周辺機器に干渉することなく前記把持対象に接近する干渉回避軌道を算出する軌道算出部と
   　を備え、
   　前記軌道算出部は、
   　前記複数の把持点候補に対し、前記ツール部が前記周辺機器に干渉することなく、かつ許容姿勢範囲内に存在する複数の把持姿勢候補を生成する把持姿勢生成部と、
   　生成された前記把持姿勢候補の中で前記把持対象をずらすことなく前記ツール部が前記把持対象に接近できる把持姿勢を抽出し、抽出された把持姿勢となるための複数の把持点候補を生成し、把持点候補のそれぞれを端点に持つ複数の軌道を第１の接近軌道として生成する第１の接近軌道生成部と、
   　前記第１の接近軌道の端点のうち前記把持対象から最も遠い位置にある第１の端点に、前記ロボットのアーム部および前記ツール部が周辺機器に干渉することなく接近する第２の接近軌道を生成する第２の接近軌道生成部と、
   　前記第１の接近軌道と前記第２の接近軌道とを接続した前記干渉回避軌道を生成する回避軌道生成部と
   　を備えることを特徴とする干渉回避装置。
2. 　前記ツール部と前記周辺機器との間の距離を前記計測データあるいは前記モデル情報に基づいて演算し、前記距離とその距離の変化を用いて前記周辺機器への前記ツール部の干渉のし易さを評価する干渉リスク評価部を備え、
   　前記第２の接近軌道生成部は、前記第１の接近軌道の第１の端点と前記ツール部の把持接近開始点とを結ぶ軌道の中から、前記評価が基準値となる距離より小さくならない、かつ距離の変化量より大きくならない前記第２の接近軌道を選択することを特徴とする請求項１に記載の干渉回避装置。
3. 　前記第２の接近軌道生成部は、前記距離に応じた斥力である仮想外力を前記ツール部に作用させて、前記第２の接近軌道を参照軌道としてポテンシャル法を用いて前記参照軌道を修正することを特徴とする請求項２に記載の干渉回避装置。
4. 　前記第２の接近軌道生成部は、前記距離に応じて変化する前記ツール部の前記把持対象への接近速度を、前記接近速度の上限値を超えない範囲で最大化することを特徴とする請求項２に記載の干渉回避装置。
5. 　前記第１の接近軌道生成部は、前記ツール部の開閉速度および開閉時間を、前記把持対象への接近を制約する把持接近制約として、第１の接近軌道を生成することを特徴とする請求項１から請求項３の何れか一項に記載の干渉回避装置。
6. 　前記第１の接近軌道生成部は、前記ツール部の目標位置と前記ツール部の目標位置から一定距離離れた位置を示す前記ツール部の把持接近開始点との間に定義される軌道の経由点であるＭ個の中間ゴールと、把持接近開始点と、把持点候補とを接続するＭ＋１個の第１の接近軌道要素を生成し、生成された複数の第１の接近軌道要素を接続し、
   　前記Ｍは１以上の整数であることを特徴とする請求項１から請求項３の何れか一項に記載の干渉回避装置。
7. 　前記第２の接近軌道生成部は、前記ツール部の目標位置と前記ツール部の目標位置から一定距離離れた位置を示す前記ツール部の把持接近開始点との間に定義される軌道の経由点であるＮ個の中間ゴールと、把持接近開始点と、ピッキング動作の開始点とを接続するＮ＋１個の第２の接近軌道要素を生成し、生成された複数の第２の接近軌道要素を接続し、
   　前記Ｎは１以上の整数であることを特徴とする請求項１から請求項３の何れか一項に記載の干渉回避装置。
8. 　少なくとも周辺機器の状態を計測した計測データが入力され、ロボットのアーム部に設けられたツール部が前記周辺機器に干渉することを回避させる干渉回避装置であって、
   　前記計測データと、前記ロボットおよび前記周辺機器のモデル情報とに基づき、前記ツール部の把持対象のツール部に対する把持姿勢情報を使って、前記ツール部が把持対象の組立を完了する複数の組立位置候補を算出し、算出した前記複数の組立位置候補に基づき、前記ツール部が前記周辺機器に干渉することなく組立対象に接近する干渉回避軌道を算出する軌道算出部を備え、
   　前記軌道算出部は、
   　前記組立位置候補に対し、前記ツール部が前記周辺機器に干渉することなく、かつ許容姿勢範囲内に存在する複数の組立姿勢候補を生成する組立姿勢候補生成部と、
   　生成された前記複数の組立姿勢候補の中で、前記組立対象をずらすことなく前記ツール部が前記組立対象に接近できる組立姿勢を抽出し、抽出された組立姿勢となるための複数の組立軌道候補を生成し、前記複数の組立姿勢候補のそれぞれを端点に持つ複数の軌道を第１の接近軌道として生成する第１の接近軌道生成部と、
   　前記第１の接近軌道の端点のうち前記組立対象から最も遠い位置にある第１の端点に、前記ロボットのアーム部および前記ツール部が周辺機器に干渉することなく接近する第２の接近軌道を生成する第２の接近軌道生成部と、
   　前記第１の接近軌道と前記第２の接近軌道とを接続した前記干渉回避軌道を生成する回避軌道生成部と、
   　を備え、
   　前記第１の接近軌道生成部は、前記ツール部の目標位置である組立姿勢候補と前記ツール部の目標位置から一定距離離れた位置を示す前記ツール部の把持接近開始点との間に定義される軌道の経由点であるＭ個の中間ゴールと、組立動作開始点と、把持点とを接続するＭ＋１個の第１の接近軌道要素を生成し、生成された複数の第１の接近軌道要素を接続し、
   　前記第２の接近軌道生成部は、前記ツール部の目標位置と前記ツール部の目標位置から一定距離離れた位置を示す前記ツール部の把持接近開始点との間に定義される軌道の経由点であるＮ個の中間ゴールと、組立動作開始点と、組立動作全体の開始点とを接続するＮ＋１個の第２の接近軌道要素を生成し、生成された複数の第２の接近軌道要素を接続し、
   　前記Ｍは１以上の整数であり、前記Ｎは１以上の整数であることを特徴とする干渉回避装置。
9. 　前記軌道算出部は、
   　複数の前記第１の接近軌道を記憶する第１の記憶部と、
   　複数の前記第２の接近軌道を記憶する第２の記憶部と、
   　をさらに備え、
   　前記第１の接近軌道生成部は、前記把持姿勢候補に対応する第１の接近軌道を、前記第１の記憶部から選択して出力し、
   　前記第２の接近軌道生成部は、前記第１の接近軌道生成部で選択された、第１の接近軌道に対応する第２の接近軌道を、前記第２の記憶部から選択して出力することを特徴とする請求項１に記載の干渉回避装置。

Images