(57)【要約】      （修正有）【課題】ロボットアームの手先部にハンドアイセンサを取り付けたビンピッキングロボットにおいて、ハンドアイセンサの存在を考慮した周辺との干渉防止を図ること。【解決手段】ロボットのフランジ部９に取り付けたハンドアイセンサ１１の干渉候補点の位置ベクトルＲcp1,Ｒcp2を、ツール座標系揮毫Σtoolからロボット座標系Σbaseを介してステージ座標系Σsatageに座標変換する。そして、ステージ座標系Σsatageにおけるハンドアイセンサ１１の干渉候補点の位置ベクトルＲcp1，Ｒcp2のＺ座標軸の値が正の値であれば、ハンドアイセンサ１１がロボットの設置面Ａ（ステージ）に干渉しないものと判断する。【選択図】図５
2011093014.tif 000002
【特許請求の範囲】
【請求項１】
ロボットアームの手先部に取り付けたエンドエフェクタにより、前記ロボットアームのベースが設置されたステージ上のワークをピッキングする際に、前記手先部のハンドアイセンサのセンシング結果によって認識した前記ワークの位置及び姿勢に基づいて、該ワークのピッキングに適した前記エンドエフェクタの姿勢を決定し、前記エンドエフェクタが前記ワークのピッキング箇所において前記決定した姿勢となるように前記ロボットアームを駆動させるハンドアイ式ビンピッキングロボットの制御装置において、
前記ステージ上における前記ピッキング箇所を原点とするピッキング座標系を、前記ステージの所定箇所を原点とするステージ座標系に座標変換するピッキング座標変換手段と、
前記エンドエフェクタを前記決定した姿勢として前記ピッキング箇所に位置させた場合における、前記ツール座標系における前記ハンドアイセンサの干渉候補点の座標値を、前記ピッキング座標変換手段による座標変換によって、前記ステージ座標系における前記干渉候補点の座標値に変換する移動先座標値変換手段と、
前記エンドエフェクタが前記ピッキング箇所において前記決定した姿勢となるように前記ロボットアームを駆動させた場合に、前記干渉候補点が前記ステージと干渉するか否かを、前記移動先座標値変換手段が変換した前記ステージ座標系における前記干渉候補点の座標値のうち、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって判定する移動先干渉判定手段と、
前記移動先干渉判定手段が干渉しないと判定した場合に、前記エンドエフェクタが前記ピッキング箇所において前記移動先干渉判定手段が干渉しないと判定した姿勢となるように、停止中の前記ロボットアームを駆動させる駆動手段と、
前記移動先干渉判定手段が干渉すると判定した場合に、前記ワークのピッキングに適した前記エンドエフェクタの姿勢を新たに決定する姿勢再決定手段と、
を備えることを特徴とするハンドアイ式ビンピッキングロボットの制御装置。
【請求項２】
前記手先部は、該手先部からの前記エンドエフェクタの立設方向と平行に延在する回転軸の周りに前記エンドエフェクタ及び前記ハンドアイセンサを回転させる前記自由軸を有しており、前記移動先干渉判定手段は、前記エンドエフェクタ及び前記ハンドアイセンサの前記手先部に対する所定角度の回転の前後について前記移動先座標値変換手段が変換した、前記ステージ座標系における前記干渉候補点の座標値において、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって、前記干渉候補点が前記ステージと干渉するか否かを判定することを特徴とする請求項１記載のハンドアイ式ビンピッキングロボットの制御装置。
【請求項３】
前記ベースを原点とするベース座標系を前記ステージ座標系に座標変換するベース座標変換手段と、
前記駆動手段により前記ロボットアームを駆動させている間、前記ロボットアームの各自由軸における回転角度を検出する回転角度検出手段と、
前記回転角度検出手段により検出された前記各自由軸における回転角度から検出される、前記ベース座標系における前記エンドエフェクタの座標値に基づき、前記駆動手段による前記ロボットアームの駆動中における、前記ツール座標系における前記干渉候補点の座標値を、前記ベース座標変換手段による座標変換によって、前記ステージ座標系における前記干渉候補点の座標値に変換する駆動中座標値変換手段と、
前記駆動手段による前記ロボットアームの駆動中に、前記干渉候補点が前記ステージと干渉するか否かを、前記駆動中座標値変換手段が変換した前記ステージ座標系における前記干渉候補点の座標値のうち、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって判定する駆動中干渉判定手段と、
前記駆動中干渉判定手段が干渉すると判定した場合に、前記ロボットアームの駆動を停止させる停止手段とをさらに備えており、
前記姿勢再決定手段は、前記駆動中干渉判定手段が干渉すると判定した場合に、前記停止手段が駆動を停止させた前記ロボットアームの前記駆動手段による再駆動以降の前記エンドエフェクタの姿勢を新たに決定する、
ことを特徴とする請求項１又は２記載のハンドアイ式ビンピッキングロボットの制御装置。
【請求項４】
ロボットアームの手先部に取り付けたエンドエフェクタにより、前記ロボットアームのベースが設置されたステージ上のワークをピッキングする際に、前記手先部のハンドアイセンサのセンシング結果によって認識した前記ワークの位置及び姿勢に基づいて、該ワークのピッキングに適した前記エンドエフェクタの姿勢を決定し、前記エンドエフェクタが前記ワークのピッキング箇所において前記決定した姿勢となるように前記ロボットアームを駆動させるハンドアイ式ビンピッキングロボットの制御装置において、
前記ベースを原点とするベース座標系を前記ステージ座標系に座標変換するベース座標変換手段と、
前記エンドエフェクタが前記ワークの位置において、前記ハンドアイセンサの干渉候補点が前記ステージと干渉しないと姿勢となるように、前記ロボットアームを駆動させる駆動手段と、
前記駆動手段により前記ロボットアームを駆動させている間、前記ロボットアームの各自由軸における回転角度を検出する回転角度検出手段と、
前記回転角度検出手段により検出された前記各自由軸における回転角度から検出される、前記ベース座標系における前記エンドエフェクタの座標値に基づき、前記駆動手段による前記ロボットアームの駆動中における、前記ツール座標系における前記干渉候補点の座標値を、前記ベース座標変換手段による座標変換によって、前記ステージ座標系における前記干渉候補点の座標値に変換する駆動中座標値変換手段と、
前記駆動手段による前記ロボットアームの駆動中に、前記干渉候補点が前記ステージと干渉するか否かを、前記駆動中座標値変換手段が変換した前記ステージ座標系における前記干渉候補点の座標値のうち、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって判定する駆動中干渉判定手段と、
前記駆動中干渉判定手段が干渉すると判定した場合に、前記ロボットアームの駆動を停止させる停止手段と、
前記駆動中干渉判定手段が干渉すると判定した場合に、前記停止手段が駆動を停止させた前記ロボットアームの前記駆動手段による再駆動以降の前記エンドエフェクタの姿勢を新たに決定する姿勢再決定手段と、
を備えることを特徴とするハンドアイ式ビンピッキングロボットの制御装置。
【請求項５】
前記姿勢再決定手段は、前記停止手段が駆動を停止させた前記ロボットアームの前記駆動手段による再駆動時の前記エンドエフェクタの姿勢を、前記停止手段による前記ロボットアームの駆動停止時とは異なる姿勢に、新たに決定することを特徴とする請求項３又は４記載のハンドアイ式ビンピッキングロボットの制御装置。
【請求項６】
前記手先部は、該手先部からの前記エンドエフェクタの立設方向と平行に延在する回転軸の周りに前記エンドエフェクタ及び前記ハンドアイセンサを回転させる前記自由軸を有しており、前記駆動中干渉判定手段は、前記エンドエフェクタ及び前記ハンドアイセンサの前記手先部に対する所定角度の回転の前後について前記駆動中座標値変換手段が変換した、前記ステージ座標系における前記干渉候補点の座標値において、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって、前記干渉候補点が前記ステージと干渉するか否かを判定することを特徴とする請求項３、４又は５記載のハンドアイ式ビンピッキングロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ビンピッキングロボットの制御装置に係り、特に、ピッキングするワークの位置や姿勢を検出するハンドアイセンサをロボットアームの手先部に有するハンドアイ式ビンピッキングロボットの制御装置に関するものである。
【背景技術】
【０００２】
ロボットアームの手先部に取り付けたエンドエフェクタによってトレイのワークをピッキングするビンピッキングロボットにおいては、ワークの位置や姿勢を検出するセンサをワークのトレイの付近に配置する代わりに、手先部に取り付けたハンドアイセンサによりワークの位置や姿勢を検出するハンドアイ方式を採用する場合が増えている。それは、主に以下に示すような理由による。
【０００３】
まず、ピッキングするワーク（例えば部品）を収容したトレイが複数ある場合は、それらのトレイに沿ってセンサを広範囲に移動させる必要がある。また、ワークがトレイに山積みされた部品等である場合、各ワークの位置や姿勢を正確に検出するためには、様々な角度からワークを測定する必要がある。さらに、ワークが小物部品等である場合のように、ショートレンジのセンサを使用しなければならない場合は、ワークに近接してセンサが配置されることから、ロボットアームのアプローチ経路と干渉する恐れがある。
【０００４】
そこで、上述した事情に対応するために、シリンダレール等を用いてセンサを高速移動できるようにすることも考えられる。しかし、構成が大がかりになってしまい、また、レイアウト変更や部品トレイの追加等によってセンサの移動範囲が変わったときに、構成を全面的に改めなければならず汎用性に欠けるという弱点がある。その点、ハンドアイ方式であれば上述した点にも柔軟に対応できるので、近年採用するケースが増えている。
【０００５】
ところで、ビンピッキングロボットにおいては、ピッキングするワークの位置及び姿勢の情報に基づいて、その都度ロボットアームや手先部のエンドエフェクタの移動パターンが変わるので、ティーチングによって予め決められたパターンで移動するロボットとは異なり、ロボットアームやエンドエフェクタが他の物体と干渉することを防ぐための対策が必要になる。
【０００６】
ロボット自身の干渉に対する対策としては、周辺の物体との干渉時にその衝撃を減らす技術や（例えば、特許文献１）、周辺の移動体の移動経路を推定してそれらとの干渉を未然に防止する技術（例えば、特許文献２）等が多数提案されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００２−２８３２７８号公報
【特許文献２】特開２００４−１２１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかし、ハンドアイ方式の場合は、ロボットアーム自身だけでなくそれに付随するハンドアイセンサの存在を考慮して干渉防止を図る必要があるので、既存の技術をそのまま利用することができない。
【０００９】
本発明は前記事情に鑑みなされたもので、本発明の目的は、ロボットアームの手先部にハンドアイセンサを取り付けたハンドアイ方式ビンピッキングロボットにおいて、ハンドアイセンサの存在を考慮した周辺との干渉防止を図ることができる制御装置を提供することにある。
【課題を解決するための手段】
【００１０】
上記目的を達成するため、請求項１に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置は、
ロボットアームの手先部に取り付けたエンドエフェクタにより、前記ロボットアームのベースが設置されたステージ上のワークをピッキングする際に、前記手先部のハンドアイセンサのセンシング結果によって認識した前記ワークの位置及び姿勢に基づいて、該ワークのピッキングに適した前記エンドエフェクタの姿勢を決定し、前記エンドエフェクタが前記ワークのピッキング箇所において前記決定した姿勢となるように前記ロボットアームを駆動させるハンドアイ式ビンピッキングロボットの制御装置において、
前記ステージ上における前記ピッキング箇所を原点とするピッキング座標系を、前記ステージの所定箇所を原点とするステージ座標系に座標変換するピッキング座標変換手段と、
前記エンドエフェクタを前記決定した姿勢として前記ピッキング箇所に位置させた場合における、前記ツール座標系における前記ハンドアイセンサの干渉候補点の座標値を、前記ピッキング座標変換手段による座標変換によって、前記ステージ座標系における前記干渉候補点の座標値に変換する移動先座標値変換手段と、
前記エンドエフェクタが前記ピッキング箇所において前記決定した姿勢となるように前記ロボットアームを駆動させた場合に、前記干渉候補点が前記ステージと干渉するか否かを、前記移動先座標値変換手段が変換した前記ステージ座標系における前記干渉候補点の座標値のうち、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって判定する移動先干渉判定手段と、
前記移動先干渉判定手段が干渉しないと判定した場合に、前記エンドエフェクタが前記ピッキング箇所において前記移動先干渉判定手段が干渉しないと判定した姿勢となるように、停止中の前記ロボットアームを駆動させる駆動手段と、
前記移動先干渉判定手段が干渉すると判定した場合に、前記ワークのピッキングに適した前記エンドエフェクタの姿勢を新たに決定する姿勢再決定手段と、
を備えることを特徴とする。
【００１１】
請求項１に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、ワークのピッキングに適した姿勢が決定されると、その姿勢でピッキング箇所にエンドエフェクタを位置させた場合に、ハンドアイセンサの干渉候補点がステージに衝突して干渉するか否かが判定される。干渉すると判定されると、ワークのピッキングに適した新たな姿勢が決定される。干渉しないと判定されると、ロボットアームの駆動によりエンドエフェクタがピッキング箇所に向けて移動される。
【００１２】
以上の動作を、ハンドアイセンサの干渉候補点のステージ座標系における座標値の内容に基づいて決定し行うことができるので、手先部に取り付けたハンドアイセンサの存在を考慮した周辺との干渉防止を的確に図ることができる。
【００１３】
また、請求項２に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置は、請求項１に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、前記手先部が、該手先部からの前記エンドエフェクタの立設方向と平行に延在する回転軸の周りに前記エンドエフェクタ及び前記ハンドアイセンサを回転させる前記自由軸を有しており、前記移動先干渉判定手段が、前記エンドエフェクタ及び前記ハンドアイセンサの前記手先部に対する所定角度の回転の前後について前記移動先座標値変換手段が変換した、前記ステージ座標系における前記干渉候補点の座標値において、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって、前記干渉候補点が前記ステージと干渉するか否かを判定することを特徴とする。
【００１４】
請求項２に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、請求項１に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、ハンドアイセンサがエンドエフェクタと共に手先部に対して通常の位置から所定角度回転すれば、ハンドアイセンサの干渉候補点がエンドエフェクタのピッキング箇所への移動先においてステージに衝突しない場合は、干渉候補点がステージと干渉しないと判定される。そして、手先部に対するエンドエフェクタ及びハンドアイセンサの所定角度の回転を交えながら、エンドエフェクタがピッキング箇所に移動される。
【００１５】
したがって、ハンドアイセンサがステージに衝突、干渉しないエンドエフェクタの移動パターン（ロボットアームの駆動パターン）を増やし、ハンドアイセンサの干渉防止のためにエンドエフェクタのピッキング箇所への移動が頻繁に停止、変更されるのを防ぐことができる。
【００１６】
なお、特にピッキング箇所においてエンドエフェクタ及びハンドアイセンサを手先部に対して所定角度回転させる場合は、所定角度回転してもエンドエフェクタがワークのピッキングに支障を生じない姿勢となるような（例えば、所定角度回転しても、エンドエフェクタがワークに対して回転前の姿勢と同じ姿勢になるような）角度に設定するのが好ましい。
【００１７】
さらに、請求項３に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置は、請求項１又は２に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、
前記ベースを原点とするベース座標系を前記ステージ座標系に座標変換するベース座標変換手段と、
前記駆動手段により前記ロボットアームを駆動させている間、前記ロボットアームの各自由軸における回転角度を検出する回転角度検出手段と、
前記回転角度検出手段により検出された前記各自由軸における回転角度から検出される、前記ベース座標系における前記エンドエフェクタの座標値に基づき、前記駆動手段による前記ロボットアームの駆動中における、前記ツール座標系における前記干渉候補点の座標値を、前記ベース座標変換手段による座標変換によって、前記ステージ座標系における前記干渉候補点の座標値に変換する駆動中座標値変換手段と、
前記駆動手段による前記ロボットアームの駆動中に、前記干渉候補点が前記ステージと干渉するか否かを、前記駆動中座標値変換手段が変換した前記ステージ座標系における前記干渉候補点の座標値のうち、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって判定する駆動中干渉判定手段と、
前記駆動中干渉判定手段が干渉すると判定した場合に、前記ロボットアームの駆動を停止させる停止手段とをさらに備えており、
前記姿勢再決定手段は、前記駆動中干渉判定手段が干渉すると判定した場合に、前記停止手段が駆動を停止させた前記ロボットアームの前記駆動手段による再駆動以降の前記エンドエフェクタの姿勢を新たに決定する、
ことを特徴とする。
【００１８】
請求項３に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、請求項１又は２に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、ピッキング箇所に向けたエンドエフェクタの移動中にも、ハンドアイセンサの干渉候補点がステージに衝突して干渉するか否かが随時判定される。干渉すると判定されると、エンドエフェクタのピッキング箇所に向けた移動が停止され、ワークのピッキングに適した新たな姿勢が決定されて、ロボットアームの駆動によるエンドエフェクタのピッキング箇所に向けた移動が再開される。干渉しないと判定されると、エンドエフェクタのピッキング箇所に向けた移動が継続される。
【００１９】
以上の動作も、ハンドアイセンサの干渉候補点のステージ座標系における座標値の内容に基づいて決定し行うことができるので、手先部に取り付けたハンドアイセンサの存在を考慮した周辺との干渉防止を的確に図ることができる。
【００２０】
また、上記目的を達成するため、請求項４に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置は、
ロボットアームの手先部に取り付けたエンドエフェクタにより、前記ロボットアームのベースが設置されたステージ上のワークをピッキングする際に、前記手先部のハンドアイセンサのセンシング結果によって認識した前記ワークの位置及び姿勢に基づいて、該ワークのピッキングに適した前記エンドエフェクタの姿勢を決定し、前記エンドエフェクタが前記ワークのピッキング箇所において前記決定した姿勢となるように前記ロボットアームを駆動させるハンドアイ式ビンピッキングロボットの制御装置において、
前記ベースを原点とするベース座標系を前記ステージ座標系に座標変換するベース座標変換手段と、
前記エンドエフェクタが前記ワークの位置において、前記ハンドアイセンサの干渉候補点が前記ステージと干渉しないと姿勢となるように、前記ロボットアームを駆動させる駆動手段と、
前記駆動手段により前記ロボットアームを駆動させている間、前記ロボットアームの各自由軸における回転角度を検出する回転角度検出手段と、
前記回転角度検出手段により検出された前記各自由軸における回転角度から検出される、前記ベース座標系における前記エンドエフェクタの座標値に基づき、前記駆動手段による前記ロボットアームの駆動中における、前記ツール座標系における前記干渉候補点の座標値を、前記ベース座標変換手段による座標変換によって、前記ステージ座標系における前記干渉候補点の座標値に変換する駆動中座標値変換手段と、
前記駆動手段による前記ロボットアームの駆動中に、前記干渉候補点が前記ステージと干渉するか否かを、前記駆動中座標値変換手段が変換した前記ステージ座標系における前記干渉候補点の座標値のうち、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって判定する駆動中干渉判定手段と、
前記駆動中干渉判定手段が干渉すると判定した場合に、前記ロボットアームの駆動を停止させる停止手段と、
前記駆動中干渉判定手段が干渉すると判定した場合に、前記停止手段が駆動を停止させた前記ロボットアームの前記駆動手段による再駆動以降の前記エンドエフェクタの姿勢を新たに決定する姿勢再決定手段と、
を備えることを特徴とする。
【００２１】
請求項４に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、ハンドアイセンサの干渉候補点とロボットアームのベースが設置されるステージとが衝突して干渉することがないような、ワークのピッキングに適した姿勢で、エンドエフェクタがピッキング箇所に位置するように、ロボットアームの駆動によりエンドエフェクタがピッキング箇所に向けて移動される間、ハンドアイセンサの干渉候補点がステージに衝突して干渉するか否かが随時判定される。干渉すると判定されると、エンドエフェクタのピッキング箇所に向けた移動が停止され、ワークのピッキングに適した新たな姿勢が決定されて、ロボットアームの駆動によるエンドエフェクタのピッキング箇所に向けた移動が再開される。干渉しないと判定されると、エンドエフェクタのピッキング箇所に向けた移動が継続される。
【００２２】
以上の動作を、ハンドアイセンサの干渉候補点のステージ座標系における座標値の内容に基づいて決定し行うことができるので、手先部に取り付けたハンドアイセンサの存在を考慮した周辺との干渉防止を的確に図ることができる。
【００２３】
さらに、請求項５に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置は、請求項３又は４に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、前記姿勢再決定手段が、前記停止手段が駆動を停止させた前記ロボットアームの前記駆動手段による再駆動時の前記エンドエフェクタの姿勢を、前記停止手段による前記ロボットアームの駆動停止時とは異なる姿勢に、新たに決定することを特徴とする。
【００２４】
請求項５に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、請求項３又は４に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、ハンドアイセンサの干渉候補点がステージに衝突して干渉すると判定されて、エンドエフェクタのピッキング箇所に向けた移動が停止されると、その時点と異なるエンドエフェクタの姿勢が新たに決定されて、その決定されたエンドエフェクタの姿勢から、ロボットアームの駆動によるエンドエフェクタのピッキング箇所に向けた移動が再開される。
【００２５】
したがって、ワークのピッキングに適した新たなエンドエフェクタの姿勢を、ロボットアームの駆動停止時とは異なる姿勢として容易に決定することができ、手先部に取り付けたハンドアイセンサの存在を考慮した周辺との干渉防止を、より簡素なロジックで図ることができる。
【００２６】
また、請求項６に記載した本発明の請求項５に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置は、請求項３、４又は５に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、前記手先部が、該手先部からの前記エンドエフェクタの立設方向と平行に延在する回転軸の周りに前記エンドエフェクタ及び前記ハンドアイセンサを回転させる前記自由軸を有しており、前記駆動中干渉判定手段が、前記エンドエフェクタ及び前記ハンドアイセンサの前記手先部に対する所定角度の回転の前後について前記駆動中座標値変換手段が変換した、前記ステージ座標系における前記干渉候補点の座標値において、前記ステージの法線方向に延在する座標軸に関する座標値が、正の値であるか否かによって、前記干渉候補点が前記ステージと干渉するか否かを判定することを特徴とする。
【００２７】
請求項６に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、請求項３、４又は５に記載した本発明のハンドアイ式ビンピッキングロボットの制御装置において、ハンドアイセンサがエンドエフェクタと共に手先部に対して通常の位置から所定角度回転すれば、ハンドアイセンサの干渉候補点がエンドエフェクタのピッキング箇所への移動中においてステージに衝突しない場合は、干渉候補点がステージと干渉しないと判定される。そして、手先部に対するエンドエフェクタ及びハンドアイセンサの所定角度の回転を交えながら、エンドエフェクタがピッキング箇所に移動される。
【００２８】
したがって、ハンドアイセンサがステージに衝突、干渉しないエンドエフェクタの移動パターン（ロボットアームの駆動パターン）を増やし、ハンドアイセンサの干渉防止のためにエンドエフェクタのピッキング箇所への移動が頻繁に停止、変更されるのを防ぐことができる。
【発明の効果】
【００２９】
本発明のハンドアイ式ビンピッキングロボットの制御装置によれば、ロボットアームの手先部にハンドアイセンサを取り付けたハンドアイ方式ビンピッキングロボットにおいて、ロボットアームが設置されるステージに対するハンドアイセンサの干渉防止を図ることができる。
【図面の簡単な説明】
【００３０】
【図１】本発明の一実施形態に係る制御装置を用いたハンドアイ式ビンピッキングロボットの概略構成を示す説明図である。
【図２】図１のハンドアイ式ビンピッキングロボットの電気的な概略構成を示すブロック図である。
【図３】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図４】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図５】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図６】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図７】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図８】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図９】図１のロボットコントローラがハンドアイセンサの設置面に対する干渉の有無を判定する手順の説明図である。
【図１０】図２のロボットコントローラのＣＰＵがＲＯＭに格納されたプログラムにしたがって行う制御を示すフローチャートである。
【図１１】図２のロボットコントローラのＣＰＵがＲＯＭに格納されたプログラムにしたがって行う制御の変形例を示すフローチャートである。
【発明を実施するための形態】
【００３１】
以下、本発明の実施形態について図面を参照しながら説明する。
【００３２】
図１は本発明の一実施形態に係る制御装置を用いたハンドアイ式ビンピッキングロボットの概略構成を示す説明図である。
【００３３】
図１中引用符号１で示す本実施形態のハンドアイ式ビンピッキングロボット（以下、「ロボット」と略記する。）は、ロボット１の設置面Ａ（請求項中のステージに相当）に設置されたトレー１３内にランダムに積み上げ収容されたワーク１５のビンピッキングを行うものである。そして、ロボット１は、円柱状の基台３（請求項中のベースに相当）上に設置された多関節型アーム５（請求項中のロボットアームに相当）の先端にフランジ部９（請求項中の手先部に相当）を有している。
【００３４】
前記多関節型アーム５は、基台３側から順に第１乃至第４の４つのアーム部６ａ〜６ｄを有している。基台３と第１アーム部６ａはＡ１軸６ｅで回転可能に連結されている。第１アーム部６ａと第２アーム部６ｂはＡ２軸６ｆで回転可能に連結されている。第２アーム部６ｂと第３アーム部６ｃはＡ３軸６ｇで回転可能に連結されている。第３アーム部６ｃの先端側（第４アーム部６ｄ側）はＡ４軸６ｈにより、基端側（第２アーム部６ｂ側）に対して回転可能とされている。第３アーム部６ｃと第４アーム部６ｄはＡ５軸６ｉで回転可能に連結されている。第４アーム部６ｄの先端には、Ａ６軸６ｊを介してフランジ部９が回転可能に連結されている。Ａ１〜Ａ６の各軸６ｅ〜６ｊは、請求項中の自由軸にそれぞれ相当している。
【００３５】
このように構成された本実施形態のロボット１においては、多関節型アーム５に内蔵したＡ１〜Ａ６の各軸６ｅ〜６ｊ用のロータリアクチュエータ５Ａ〜５Ｆ（図２参照）に、ロボットコントローラ３１から適切な制御値を与えることで、基台３に対して多関節型アーム５を適切な姿勢とする。
【００３６】
このような構成のロボット１は、位置ベクトルによって各部の位置及び姿勢を管理する。この位置及び姿勢の管理にはロボット座標系（請求項中のベース座標系に相当）が用いられる。ロボット座標系は、ロボット原点Ｏを原点とする座標系である。このロボット原点Ｏは、ロボット１の設置面Ａに対向する基台３の設置面３ａの中心点に設定される。
【００３７】
なお、フランジ部９にはエンドエフェクタ１７が取り付けられる。但し、このエンドエフェクタ１７は、ロボット１の出荷段階ではフランジ部９に取り付けられていない。エンドエフェクタ１７は、ロボット１の受け入れ先において、ロボット１を使用するシステム上の作業に適したものが適宜取り付けられる。本実施形態では、ワークをピッキングするためのグリッパタイプのエンドエフェクタ１７がフランジ部９に取り付けられている。
【００３８】
また、フランジ部９には、ステー９ａを介してハンドアイセンサ１１が取り付けられる。ハンドアイセンサ１１は、エンドエフェクタ１７によってピッキングするワーク１５の位置及び姿勢を検出するためのものである。ハンドアイセンサ１１としては、例えばレーザスキャナを用いることができる。レーザスキャナでは、レーザ光等の測定光をポリゴンミラー及びガルバノミラーによって２次元方向に走査して測定対象物に照射し、その反射光を受光する。ハンドアイセンサ１１で受光した測定対象物からの反射光を、後述するハンドアイコントローラ１９で解析することで、測定対象物の３次元形状を検出することができる。本実施形態では、ハンドアイセンサ１１は矩形の箱状を呈している。
【００３９】
本実施形態においてロボット１の設置面Ａに対するハンドアイセンサ１１の干渉の有無を判定するために、ハンドアイセンサ１１には干渉候補点が設定される。本実施形態の場合は、ハンドアイセンサ１１の先端がエンドエフェクタ１７の先端よりも出っ張らないので、ハンドアイセンサ１１の先端側の外側に位置する２箇所の角部が干渉候補点に設定される。ハンドアイセンサ１１の先端がエンドエフェクタ１７の先端よりも出っ張る場合は、ハンドアイセンサ１１の先端側の内側２箇所の角部も干渉候補点となる。
【００４０】
エンドエフェクタ１７やハンドアイセンサ１１は、ロボット１の出荷段階ではフランジ部９に取り付けられていない。したがって、後述するロボットコントローラ３１（図２参照）がロボット１の位置及び姿勢を制御するために有しているロボット座標系における位置ベクトルのデータは、フランジ部９までの分だけである。フランジ部９に取り付けたエンドエフェクタ１７やハンドアイセンサ１１の位置及び姿勢の管理には、エンドエフェクタ１７の代表点であるツールセンターポイントＴＣＰを原点とするツール座標系が用いられる。本実施形態では、ツールセンターポイントＴＣＰは、グリッパの両チャックの先端中央の空間に設定される。そして、上述したハンドアイセンサ１１の先端側の外側に位置する２箇所の角部に設定される干渉候補点は、ツール座標系における原点からの位置ベクトルによってｒｃｐ１  ，ｒｃｐ２  と表される。
【００４１】
ツール座標系は、ロボット１の受け入れ側において行う較正作業によって、後からロボットコントローラ３１に設定される。ツール座標系の較正は、ロボット１のフランジ部９に対するエンドエフェクタ１７の取付点を原点とするロボット手先座標系を用いて行う。具体的には、ロボット手先座標系から見たツール座標系の相対的位置・姿勢を較正することで行う。そして、ロボット手先座標系から見たツール座標系の相対的位置・姿勢を記述する行列やベクトルを表すデータを、ロボットコントローラ３１に与える。
【００４２】
したがって、ロボットコントローラ３１では、ロボット座標系におけるロボット手先座標系の原点の位置ベクトルに基づいて、エンドエフェクタ１７のツールセンターポイントＴＣＰやハンドアイセンサ１１の干渉候補点のロボット座標系における位置ベクトルを特定することができる。また、ロボットコントローラ３１では、ハンドアイセンサ１１によるワーク１５からの測定光の反射光の解析でハンドアイコントローラ１９が検出した、ハンドアイセンサ１１から見たワーク１５の位置及び姿勢に基づいて、ワーク１５のロボット座標系における位置ベクトルを特定することができる。
【００４３】
また、本実施形態では、ロボットコントローラ３１が、ハンドアイセンサ１１の干渉候補点が設置面Ａと干渉（衝突）するか否かを、設置面Ａの代表点Ａ１を原点とするステージ座標系上における干渉候補点の座標値に基づいて判定する。したがって、ロボットコントローラ３１は、ロボット座標系から見たステージ座標系の相対的位置・姿勢を記述する行列やベクトルを表すデータを有している。
【００４４】
図２は、図１のハンドアイ式ビンピッキングロボットの電気的な概略構成を示すブロック図である。図２に示すように、ハンドアイコントローラ１９は、ＣＰＵ、ＲＡＭ、及び、ＲＯＭを有している。ＣＰＵは、ＲＯＭに格納された解析プログラムを実行することにより、ハンドアイセンサ１１によるワーク１５からの測定光の反射光を解析して、ハンドアイセンサ１１から見たワーク１５の位置及び姿勢を検出する。そして、ＣＰＵは、検出した結果をロボットコントローラ３１に出力する。なお、ＲＡＭは、ＣＰＵが上記の処理を実行する際の作業エリア等として利用される。
【００４５】
一方、図２に示すロボットコントローラ３１は、ハンドアイコントローラ１９において検出されたハンドアイセンサ１１に対するワーク１５の位置及び姿勢に基づいて、そのワーク１５のピッキングに適した姿勢で、そのワーク１５をピッキングするピッキング箇所にエンドエフェクタ１７が移動するように、図１に示すロボット１の多関節型アーム５を動作させる制御を行う。
【００４６】
また、ロボットコントローラ３１は、エンドエフェクタ１７の移動中や移動先でハンドアイセンサ１１がロボット１の設置面Ａと干渉（衝突）しないようにするために、多関節型アーム５やエンドエフェクタ１７、ハンドアイセンサ１１の位置と姿勢に関する制御を行う。
【００４７】
そのために、ロボットコントローラ３１は、ロボット座標系におけるフランジ部９の座標値と、ツール座標系におけるエンドエフェクタ１７のツールセンターポイントＴＣＰ及びハンドアイセンサ１１の干渉候補点の座標値をと管理する。
【００４８】
そして、上述した管理を行うために、図２に示すロボットコントローラ３１は、ＣＰＵやＲＡＭ及びＲＯＭを有するコンピュータによって構成される。このロボットコントローラ３１には、ロボット１の多関節型アーム５のＡ１軸乃至Ａ６軸６ｅ〜６ｊを回転させるためのＡ１軸乃至Ａ６軸６ｅ〜６ｊ用のロータリアクチュエータ５Ａ〜５Ｆのドライブ回路５Ｇ〜５Ｌと、Ａ１軸乃至Ａ６軸６ｅ〜６ｊ用のロータリアクチュエータ５Ａ〜５Ｆによる各軸の回転方向及び回転角度（又は回転量）を検出するロータリセンサ５Ｍ〜５Ｒ（請求項中の回転角度検出手段に相当）とが接続される。
【００４９】
次に、ロボットコントローラ３１が行う、ハンドアイセンサ１１の干渉候補点が設置面Ａと干渉するか否かの判定手順について、図３乃至図９を参照して説明する。
【００５０】
上述したように、ハンドアイセンサ１１はエンドエフェクタ１７と共にフランジ部９に取り付けられているので、エンドエフェクタ１７の位置及び姿勢を表すツール座標系ΣＴＯＯＬ内で、ハンドアイセンサ１１はエンドエフェクタ１７に対して常に同じ相対位置にあることになる。なお、本実施形態では、図３に示すように、ツール座標系ΣＴＯＯＬのＺ軸周りの回転角度が小さい順に、ハンドアイセンサ１１の干渉候補点の位置ベクトルｒｃｐ１  ，ｒｃｐ２  を定義している。
【００５１】
ロボットコントローラ３１のＲＡＭには、図４に示すように、設置面Ａ上のトレー１３内のワーク１５の位置及び姿勢を表すピッキング座標系ΣＧＲＡＳＰ  と、先に説明したステージ座標系ΣＳＴＡＧＥ  との、ロボット座標系ΣＢＡＳＥからの相対位置姿勢が記憶されている。なお、ステージ座標系ΣＳＴＡＧＥ  のＸ，Ｙ，Ｚの各座標軸と、ロボット座標系ΣＢＡＳＥのＸ，Ｙ，Ｚの各座標軸とは、それぞれ同じ方向を向いている。そして、管理の便宜上、ロボット座標系ΣＢＡＳＥ（の原点）から見たステージ座標系ΣＳＴＡＧＥ  （の原点）の位置ベクトルを、ｒｓｔと定義している。
【００５２】
なお、各座標系における位置ベクトルは、Ｘ，Ｙ，Ｚの各座標値と、ロールａ（Ｚ軸周り）角度、ピッチｂ（Ｙ軸周り）角度、ヨーｃ（Ｘ軸周り）角度とで表現する。一般型は、Σ＝（ｘ，ｙ，ｚ，ａ，ｂ，ｃ）となる。
【００５３】
また、以下の説明において、ある位置ベクトルがどの座標系から見たものであるかを明らかにすることが重要である場合は、その座標系を左肩のプリフィックスによって表す。例えば、ツール座標系ΣＴＯＯＬから見た干渉候補点の位置ベクトルｒｃｐはＴＯＯＬｒｃｐ、ステージ座標系ΣＳＴＡＧＥ  から見た干渉候補点の位置ベクトルｒｃｐはＳＴＡＧＥ  ｒｃｐと表す。
【００５４】
そして、ピッキング座標系ΣＧＲＡＳＰ  上にあるワーク１５のピッキング箇所まで、ツール座標系ΣＴＯＯＬの位置ベクトルで示されるエンドエフェクタ１７を移動し、ピッキングに適した姿勢とした場合、図５に示すように、ハンドアイセンサ１１の干渉候補点の位置ベクトルｒｃｐ１  ，ｒｃｐ２  が共に、設置面Ａよりも上方（ステージ座標系ΣＳＴＡＧＥ  におけるＺ軸の正側の領域）に存在すれば、ハンドアイセンサ１１の設置面Ａに対する干渉（衝突）は発生しない。
【００５５】
そのことを判定するためには、ツール座標系ΣＴＯＯＬから見た干渉候補点の位置ベクトルＴＯＯＬｒｃｐ１  ，ＴＯＯＬｒｃｐ２  を、ステージ座標系ΣＳＴＡＧＥ  における位置ベクトルに座標変換する必要がある。
【００５６】
そこで、まず、ピッキング座標系ΣＧＲＡＳＰ  上にあるワーク１５のピッキング箇所に一致するエンドエフェクタ１７のツールセンターポイントＴＣＰから見た、ツール座標系ΣＴＯＯＬ上の干渉候補点の位置ベクトルＴＯＯＬｒｃｐを、ロボット座標系ΣＢＡＳＥに座標変換する。この座標変換は、ピッキング座標系ΣＧＲＡＳＰ  からロボット座標系ΣＢＡＳＥへの同次変換行列ＢＡＳＥＴＧＲＡＳＰ  によって行う。座標変換後のロボット座標系ΣＢＡＳＥ上の干渉候補点の位置ベクトルは、ＢＡＳＥＴＧＲＡＳＰ  ・ＴＯＯＬｒｃｐによって表される。
【００５７】
次に、ロボット座標系ΣＢＡＳＥに座標変換された干渉候補点の位置ベクトルＢＡＳＥＴＧＲＡＳＰ  ・ＴＯＯＬｒｃｐを、ステージ座標系ΣＳＴＡＧＥ  に座標変換する。この座標変換は、ロボット座標系ΣＢＡＳＥからステージ座標系ΣＳＴＡＧＥ  への同次変換行列ＳＴＡＧＥ  ＴＢＡＳＥによって行う。座標変換後のステージ座標系ΣＳＴＡＧＥ  上の干渉候補点の位置ベクトルＳＴＡＧＥ  ｒＣＰは、
ＳＴＡＧＥ  ｒＣＰ＝ＳＴＡＧＥ  ＴＢＡＳＥ・ＢＡＳＥＴＧＲＡＳＰ  ・ＴＯＯＬｒｃｐ
によって表される。
【００５８】
なお、先に説明したように、ステージ座標系ΣＳＴＡＧＥ  のＸ，Ｙ，Ｚの各座標軸と、ロボット座標系ΣＢＡＳＥのＸ，Ｙ，Ｚの各座標軸とは、それぞれ同じ方向を向いている。このため、ロボット座標系ΣＢＡＳＥからステージ座標系ΣＳＴＡＧＥ  への同次変換行列ＳＴＡＧＥ  ＴＢＡＳＥを用いる代わりに、ロボット座標系ΣＢＡＳＥ（の原点）から見たステージ座標系ΣＳＴＡＧＥ  （の原点）の位置ベクトルｒｓｔを用いて、ステージ座標系ΣＳＴＡＧＥ  上の干渉候補点の位置ベクトルＳＴＡＧＥ  ｒＣＰを、
ＳＴＡＧＥ  ｒＣＰ＝ＢＡＳＥＴＧＲＡＳＰ  ・ＴＯＯＬｒｃｐ−ＢＡＳＥｒｓｔ
によって表すこともできる。
【００５９】
以上のようにして、ハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  を求め、それぞれのＺ軸位置（ベクトルの３番目の要素の値）が両方とも正の値であれば、ハンドアイセンサ１１と設置面Ａとの干渉（衝突）は発生しない。
【００６０】
ちなみに、エンドエフェクタ１７が、本実施形態のようなグリッパタイプのものである場合には、図６及び図７に示すように、多関節型アーム５のＡ６軸６ｊよりも先の部分が、フランジ部９に対するエンドエフェクタ１７の立設方向と平行に延在する回転軸（ツールセンターポイントＴＣＰを通る軸）の周りに１８０゜回転しても、同じ姿勢でエンドエフェクタ１７がワーク１５をピッキングすることができる。
【００６１】
そして、ある姿勢ではハンドアイセンサ１１が設置面Ａと干渉（衝突）してしまう場合であっても、多関節型アーム５のＡ６軸６ｊよりも先の部分が１８０゜回転した姿勢では、ハンドアイセンサ１１が設置面Ａと干渉（衝突）しないことがある。逆に、ある姿勢ではハンドアイセンサ１１が設置面Ａと干渉（衝突）しない場合であっても、多関節型アーム５のＡ６軸６ｊよりも先の部分が１８０゜回転した姿勢では、ハンドアイセンサ１１が設置面Ａと干渉（衝突）してしまうことがある。
【００６２】
そこで、本実施形態では、エンドエフェクタ１７をある姿勢でピッキング箇所に位置させた場合に、ハンドアイセンサ１１が設置面Ａと干渉（衝突）するか否かの判定は、多関節型アーム５のＡ６軸６ｊよりも先の部分を１８０゜回転させる前の姿勢と、１８０゜回転させた後の姿勢との両方について行う。
【００６３】
多関節型アーム５のＡ６軸６ｊよりも先の部分を１８０゜回転させた後の姿勢については、次のように表すことができる。つまり、ピッキング座標系ΣＧＲＡＳＰ  を１８０゜反転させた座標系ＧＲＡＳＰ  Σ１８０  ＝ＧＲＡＳＰ  （０，０，０，１８０゜，０，０）から、ピッキング座標系ΣＧＲＡＳＰ  への同次変換行列を、ＧＲＡＳＰ  Ｔ１８０  とすると、ステージ座標系ΣＳＴＡＧＥ  上の干渉候補点の位置ベクトルＳＴＡＧＥ  ｒＣＰは、
ＳＴＡＧＥ  ｒＣＰ＝ＳＴＡＧＥ  ＴＢＡＳＥ・ＢＡＳＥＴＧＲＡＳＰ  ・ＧＲＡＳＰ  Ｔ１８０  ・ＴＯＯＬｒｃｐ
、又は、
ＳＴＡＧＥ  ｒＣＰ＝ＢＡＳＥＴＧＲＡＳＰ  ・ＧＲＡＳＰ  Ｔ１８０  ・ＴＯＯＬｒｃｐ−ＢＡＳＥｒｓｔ
によって表すことができる。
【００６４】
以上のようにして、多関節型アーム５のＡ６軸６ｊよりも先の部分を１８０゜回転させた後の、ハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  を求め、それぞれのＺ軸位置（ベクトルの３番目の要素の値）が両方とも正の値であれば、ハンドアイセンサ１１と設置面Ａとの干渉（衝突）は発生しない。
【００６５】
なお、ハンドアイセンサ１１がエンドエフェクタ１７と同じような形状、即ち、ハンドアイセンサ１１がエンドエフェクタ１７の一側面のみと対向し、他の三側面とは対向しない形状である場合には、ハンドアイセンサ１１が設置面Ａと干渉（衝突）してしまう姿勢から、多関節型アーム５のＡ６軸６ｊよりも先の部分が１８０゜回転すると、その姿勢ではハンドアイセンサ１１が設置面Ａと干渉（衝突）することは、構造上起こりえない。したがって、ハンドアイセンサ１１がエンドエフェクタ１７と同じような形状である場合、ある姿勢でハンドアイセンサ１１が設置面Ａと干渉（衝突）してしまうと判定された場合には、多関節型アーム５のＡ６軸６ｊよりも先の部分が１８０゜回転した姿勢について、ハンドアイセンサ１１が設置面Ａと干渉（衝突）するか否かを、改めて上記の手順により判定するには及ばない。
【００６６】
また、エンドエフェクタ１７が、１個の吸着パッドのように１点でワーク１５をピッキング（保持）するものである場合には、エンドエフェクタ１７を吸着中心軸の周り（ツールセンターポイントＴＣＰを通る軸）に、任意の角度回転させても、同じ姿勢でエンドエフェクタ１７がワーク１５をピッキングすることができる。
【００６７】
そして、ある姿勢ではハンドアイセンサ１１が設置面Ａと干渉（衝突）してしまう場合であっても、ハンドアイセンサ１１が設置面Ａから離れるように、多関節型アーム５のＡ６軸６ｊよりも先の部分を適切な角度回転させれは、ハンドアイセンサ１１が設置面Ａと干渉（衝突）しないことがある。
【００６８】
その場合には、多関節型アーム５のＡ６軸６ｊよりも先の部分をピッキング座標系ΣＧＲＡＳＰ  のＺ軸周りに回転させるので、ピッキング座標系ΣＧＲＡＳＰ  のＸ−Ｙ平面において、エンドエフェクタ１７とハンドアイセンサ１１の適切な回転角度を考える。
【００６９】
つまり、図８に示すように、ピッキング座標系ΣＧＲＡＳＰ  のＸ−Ｙ平面をステージ座標系ΣＳＴＡＧＥ  に投影し、ステージ座標系ΣＳＴＡＧＥ  のＺ軸におけるハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  の座標値が、より高い値となるようにすることが、ステージ座標系ΣＳＴＡＧＥ  のＺ軸方向の座標値＝０で示される設置面Ａからハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  を最も遠ざけて、両者の干渉（衝突）を防ぐことになる。
【００７０】
そこで、ステージ座標系ΣＳＴＡＧＥ  に投影したハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  が、ステージ座標系ΣＳＴＡＧＥ  のＺ軸上に位置するように、多関節型アーム５のＡ６軸６ｊよりも先の部分を、図９に示すように、ピッキング座標系ΣＧＲＡＳＰ  のＺ軸周りに回転させる角度（適切な角度）を求める。
【００７１】
このことを式によって表すと、求める角度θＺＧＲＡＳＰは、ハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  の中間点を、ピッキング座標系ΣＧＲＡＳＰ  のＸ−Ｙ平面を投影したステージ座標系ΣＳＴＡＧＥ  におけるＺ軸上に位置させる回転角度である。したがって、求める角度θＺＧＲＡＳＰは、ハンドアイセンサ１１の２つの干渉候補点ＳＴＡＧＥ  ｒＣＰ１  ，ＳＴＡＧＥ  ｒＣＰ２  と、ピッキング座標系ΣＧＲＡＳＰ  のＸ軸とがそれぞれなす角度θＣＰ１  ，θＣＰ２  の半分の角度（θＣＰ１  ＋θＣＰ２  ）／２に、ピッキング座標系ΣＧＲＡＳＰ  のＸ−Ｙ平面を投影したステージ座標系ΣＳＴＡＧＥ  におけるＺ軸の、ピッキング座標系ΣＧＲＡＳＰ  のＸ軸に対してなす角度θＺＳＴＡＧＥを加えた角度、即ち、θＺＧＲＡＳＰ＝θＺＳＴＡＧＥ−｛（θＣＰ１  ＋θＣＰ２  ）／２｝によって表すことができる。
【００７２】
上式を解くと、
θＣＰ１  ＝ｔａｎ−１（ｙＣＰ１  ／ｘＣＰ１  ）
θＣＰ２  ＝ｔａｎ−１（ｙＣＰ２  ／ｘＣＰ２  ）
θＺＳＴＡＧＥ＝ｔａｎ−１（ｙＺＳＴＡＧＥ／ｘＺＳＴＡＧＥ）
となる。
【００７３】
なお、上式を解いて、多関節型アーム５のＡ６軸６ｊよりも先の部分をピッキング座標系ΣＧＲＡＳＰ  のＺ軸周りに回転させる適切な角度θＺＧＲＡＳＰを求めるには、ステージ座標系ΣＳＴＡＧＥ  からピッキング座標系ΣＧＲＡＳＰ  に座標変換するための同次変換行列が必要になる。しかし、ハンドアイセンサ１１を用いた解析によりワーク１５の位置及び姿勢を検出しなければ、ピッキング座標系ΣＧＲＡＳＰ  は定まらないので、これに対してステージ座標系ΣＳＴＡＧＥ  を座標変換する同次変換行列を予め定義することは、現実には無理である。
【００７４】
そこで、ピッキング座標系ΣＧＲＡＳＰ  のＸ軸ベクトルとＹ軸ベクトルとを（ツール座標系ΣＴＯＯＬ及びロボット座標系ΣＢＡＳＥを介して）ステージ座標系ΣＳＴＡＧＥ  に座標変換し、ステージ座標系ΣＳＴＡＧＥ  上に座標変換したピッキング座標系ΣＧＲＡＳＰ  のＸ軸ベクトルとＹ軸ベクトルとを含む平面に、ステージ座標系ΣＳＴＡＧＥ  のＺ軸ベクトルを投影する。
【００７５】
以上の投影は、次の計算式によって示すとことができる。ピッキング座標系ΣＧＲＡＳＰ  の原点ＧＲＡＳＰ  ＯＧＲＡＳＰ  を、
ＧＲＡＳＰ  ＯＧＲＡＳＰ  ＝ＧＲＡＳＰ  （０，０，０，０，０，０）
、ピッキング座標系ΣＧＲＡＳＰ  のＸ軸単位ベクトルＧＲＡＳＰ  ｘＧＲＡＳＰ  を、
ＧＲＡＳＰ  ｘＧＲＡＳＰ  ＝ＧＲＡＳＰ  （１，０，０，０，０，０）
、ピッキング座標系ΣＧＲＡＳＰ  のＹ軸単位ベクトルＧＲＡＳＰ  ｙＧＲＡＳＰ  を、
ＧＲＡＳＰ  ｙＧＲＡＳＰ  ＝ＧＲＡＳＰ  （０，１，０，０，０，０）
とした場合、これらをステージ座標系ΣＳＴＡＧＥ  に座標変換する。
【００７６】
ここで、先に説明したように、ステージ座標系ΣＳＴＡＧＥ  のＸ，Ｙ，Ｚの各座標軸と、ロボット座標系ΣＢＡＳＥのＸ，Ｙ，Ｚの各座標軸とは、それぞれ同じ方向を向いている。そこで、上記した投影の計算は、ステージ座標系ΣＳＴＡＧＥ  に座標変換する代わりに、ロボット座標系ΣＢＡＳＥに座標変換して行うことができる。そして、そのように計算する場合の計算式は、次のように表すことができる。
【００７７】
つまり、ロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＸ軸単位ベクトルＢＡＳＥｘＧＲＡＳＰ  とＹ軸単位ベクトルＢＡＳＥｙＧＲＡＳＰ  は、
ＢＡＳＥｘＧＲＡＳＰ  ＝ＢＡＳＥＴＧＲＡＳＰ  ・ＧＲＡＳＰ  ｘＧＲＡＳＰ  −ＢＡＳＥＴＧＲＡＳＰ  ・ＧＲＡＳＰ  ＯＧＲＡＳＰ  
ＢＡＳＥｙＧＲＡＳＰ  ＝ＢＡＳＥＴＧＲＡＳＰ  ・ＧＲＡＳＰ  ｙＧＲＡＳＰ  −ＢＡＳＥＴＧＲＡＳＰ  ・ＧＲＡＳＰ  ＯＧＲＡＳＰ  
と表すことができる。
【００７８】
このロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＸ軸単位ベクトルＢＡＳＥｘＧＲＡＳＰ  とＹ軸単位ベクトルＢＡＳＥｙＧＲＡＳＰ  とを含む平面に、ロボット座標系ΣＢＡＳＥのＺ軸単位ベクトルＢＡＳＥｚＢＡＳＥ＝ＢＡＳＥ（０，０，１，０，０，０）を投影すると、それぞれのベクトル方向の成分は内積をとることで求まる。
【００７９】
つまり、ロボット座標系ΣＢＡＳＥのＺ軸単位ベクトルＢＡＳＥｚＢＡＳＥ＝のうち、ロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＸ軸単位ベクトルＢＡＳＥｘＧＲＡＳＰ  方向の成分ｘＺＳＴＡＧＥは、
ｘＺＳＴＡＧＥ＝ＢＡＳＥｚＢＡＳＥ・ＢＡＳＥｘＧＲＡＳＰ  
すなわち、ロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＸ軸単位ベクトルＢＡＳＥｘＧＲＡＳＰ  のうち３番目の要素の値で示される。
【００８０】
また、ロボット座標系ΣＢＡＳＥのＺ軸単位ベクトルＢＡＳＥｚＢＡＳＥ＝のうち、ロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＹ軸単位ベクトルＢＡＳＥｙＧＲＡＳＰ  方向の成分ｙＺＳＴＡＧＥは、
ｙＺＳＴＡＧＥ＝ＢＡＳＥｚＢＡＳＥ・ＢＡＳＥｙＧＲＡＳＰ  
すなわち、ロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＹ軸単位ベクトルＢＡＳＥｙＧＲＡＳＰ  のうち３番目の要素の値で示される。
【００８１】
そこで、上記のようにして求めたロボット座標系ΣＢＡＳＥのＺ軸単位ベクトルＢＡＳＥｚＢＡＳＥ＝の各成分ｘＺＳＴＡＧＥとｙＺＳＴＡＧＥを使って、多関節型アーム５のＡ６軸６ｊよりも先の部分をピッキング座標系ΣＧＲＡＳＰ  のＺ軸周りに回転させる適切な角度θＺＧＲＡＳＰ＝ｔａｎ−１（ｙＺＳＴＡＧＥ／ｘＺＳＴＡＧＥ）を求めることができる。
【００８２】
なお、ピッキング座標系ΣＧＲＡＳＰ  のＺ軸とステージ座標系ΣＳＴＡＧＥ  のＺ軸とが平行であり場合、つまり、ピッキング箇所のエンドエフェクタ１７が設置面Ａの法線方向からワーク１５をピッキングする姿勢である場合は、多関節型アーム５のＡ６軸６ｊよりも先の部分をピッキング座標系ΣＧＲＡＳＰ  のＺ軸周りに回転させても、ステージ座標系ΣＳＴＡＧＥ  のＺ軸方向におけるハンドアイセンサ１１の干渉候補点の位置ベクトルｒｃｐ１  ，ｒｃｐ２  の座標値は変わらない。つまり、ハンドアイセンサ１１の干渉候補点は設置面Ａから遠ざからない。
【００８３】
このように、エンドエフェクタ１７がこのような姿勢であると、ロボット座標系ΣＢＡＳＥのＺ軸単位ベクトルＢＡＳＥｚＢＡＳＥ＝のうち、ロボット座標系ΣＢＡＳＥで見たステージ座標系ΣＳＴＡＧＥ  のＸ軸単位ベクトルＢＡＳＥｘＧＲＡＳＰ  方向の成分ｘＺＳＴＡＧＥやＹ軸単位ベクトルＢＡＳＥｙＧＲＡＳＰ  方向の成分ｙＺＳＴＡＧＥは、いずれもゼロ（ｘＺＳＴＡＧＥ＝ｙＺＳＴＡＧＥ＝０）となる。そのため、この場合は、多関節型アーム５のＡ６軸６ｊよりも先の部分をピッキング座標系ΣＧＲＡＳＰ  のＺ軸周りに回転させる適切な角度θＺＧＲＡＳＰも、θＺＧＲＡＳＰ＝０となる。
【００８４】
また、理論的には、ハンドアイセンサ１１の干渉候補点の位置ベクトルｒｃｐが、ピッキング座標系ΣＧＲＡＳＰ  （つまり、ツール座標系ΣＴＯＯＬ）のＺ軸と平行である場合にも、上記と同様の現象が起こる。しかし、実際にはそのような向きでハンドアイセンサ１１をフランジ部９に取り付けることはない。
【００８５】
以上が、ハンドアイセンサ１１の干渉候補点が設置面Ａと干渉するか否かのロボットコントローラ３１による判定手順である。
【００８６】
次に、ロボットコントローラ３１のＣＰＵがＲＯＭに格納されたプログラムにしたがって行う制御を、図１０のフローチャートを参照して説明する。
【００８７】
まず、ロボットコントローラ３１のＣＰＵは、トレー１３内のワーク１５の位置及び姿勢をハンドアイコントローラ１９に検出させるために、多関節型アーム５を駆動させてハンドアイセンサ１１をトレー１３に接近させる（ステップＳ１）。なお、このとき、ハンドアイセンサ１１はトレー１３の若干上方に位置して測定光をワーク１５に照射する。そのため、ここでの多関節型アーム５の駆動によってハンドアイセンサ１１が設置面Ａに干渉することはない。
【００８８】
次に、ロボットコントローラ３１のＣＰＵは、ハンドアイコントローラ１９がハンドアイセンサ１１からの測定光の反射光に基づいて検出した、ハンドアイセンサ１１から見たワーク１５の位置及び姿勢を示すデータを受信する（ステップＳ３）。そして、受信したデータに基づいて、ピッキングするワーク１５を特定し（ステップＳ５）、特定したピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７のピッキング箇所及び姿勢を決定する（ステップＳ７）。
【００８９】
続いて、多関節型アーム５を駆動してエンドエフェクタ１７をステップＳ７で決定した姿勢でピッキング箇所に位置させた場合に、ハンドアイセンサ１１が設置面Ａと干渉（衝突）するか否かを判定する（ステップＳ９）。この判定は、先に説明した手順によって行われる。干渉すると判定された場合は（ステップＳ９でＹＥＳ）、ステップＳ７にリターンして、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７のピッキング箇所及び姿勢を新たに決定する。
【００９０】
なお、ステップＳ９からリターンしたステップＳ７で新たに決定するのは、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７の姿勢のみとしてもよい。また、図１０中の破線で示すように、ステップＳ９からのリターン先をステップＳ７ではなくステップＳ５として、ピッキングするワーク１５を特定し直してから、次のステップＳ７で、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７のピッキング箇所及び姿勢を新たに決定してもよい。
【００９１】
一方、多関節型アーム５を駆動してエンドエフェクタ１７をステップＳ７で決定した姿勢でピッキング箇所に位置させた場合に、干渉しないと判定された場合は（ステップＳ９でＮＯ）、直前のステップＳ７で決定した姿勢でエンドエフェクタ１７がピッキング箇所に移動するように、多関節型アーム５を駆動させる（ステップＳ１１）。そして、エンドエフェクタ１７がピッキング箇所に移動するまで多関節型アーム５を駆動させている間（ステップＳ１３でＮＯの間）、ロータリセンサ５Ｍ〜５Ｒからの入力信号に基づいて、ハンドアイセンサ１１が設置面Ａと干渉（衝突）する（しそうになっている）か否かを判定する（ステップＳ１５）。
【００９２】
なお、ステップＳ１５の判定は、ロータリセンサ５Ｍ〜５Ｒからの入力信号によって示されるＡ１軸乃至Ａ６軸６ｅ〜６ｊ用の各回転角度を用いて、キネマティクス方程式を解くことで、多関節型アーム５の姿勢を検出し、これに基づいて、ステージ座標系ΣＳＴＡＧＥ  上のハンドアイセンサ１１の干渉候補点の位置ベクトルＳＴＡＧＥ  ｒＣＰを求めることによって、実行することができる。
【００９３】
ハンドアイセンサ１１が設置面Ａと干渉（衝突）しない（しそうになっていない）場合は（ステップＳ１５でＮＯ）、ステップＳ１１にリターンして、多関節型アーム５の駆動を継続する。ハンドアイセンサ１１が設置面Ａと干渉（衝突）する（しそうになっている）場合は（ステップＳ１５でＹＥＳ）、多関節型アーム５の駆動を停止させ（ステップＳ１７）、ステップＳ７にリターンして、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７のピッキング箇所及び姿勢を新たに決定する。
【００９４】
なお、ここでも、ステップＳ１５からリターンしたステップＳ７で新たに決定するのは、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７の姿勢のみとしてもよい。また、図１０中の破線で示すように、ステップＳ１５からのリターン先をステップＳ７ではなくステップＳ５として、ピッキングするワーク１５を特定し直してから、次のステップＳ７で、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７のピッキング箇所及び姿勢を新たに決定してもよい。
【００９５】
そして、エンドエフェクタ１７がピッキング箇所に移動するまで多関節型アーム５を駆動させたならば（ステップＳ１３でＹＥＳ）、エンドエフェクタ１７によりピッキング対象のワーク１５をピッキングさせて、所定の移動先に搬送させる動作を、エンドエフェクタ１７及び多関節型アーム５に行わせる（ステップＳ１９）。以上で、一連の制御が終了する。その後、必要なワーク１５のピッキングが終了するまで、図１０のフローチャートの制御が繰り返し実行される。
【００９６】
以上の説明からも明らかなように、本実施形態では、ステップＳ９が、請求項中のピッキング座標変換手段、移動先座標値変換手段、及び、移動先干渉判定手段に対応する処理となっている。また、本実施形態では、ステップＳ１１が、請求項中の駆動手段に対応する処理となっている。さらに、本実施形態では、ステップＳ１７が、請求項中の停止手段に対応する処理となっている。
【００９７】
また、本実施形態では、ステップＳ９やステップＳ１５からリターンして実行されるステップＳ７が、請求項中の姿勢再決定手段に対応する処理となっている。さらに、本実施形態では、ステップＳ１５が、請求項中のベース座標変換手段、駆動中座標値変換手段、及び、駆動中干渉判定手段に対応する処理となっている。
【００９８】
このように構成された本実施形態のロボットコントローラ３１が行うロボット１の制御によれば、エンドエフェクタ１７をピッキング対象のワーク１５のピッキング箇所に適切な姿勢で移動させる経路や移動先において、ハンドアイセンサ１１が設置面Ａと干渉しそうな場合には、エンドエフェクタ１７の移動が停止され、新たな姿勢を決定した上でエンドエフェクタ１７の移動が再開される。
【００９９】
そのため、ロボット１の多関節型アーム５やエンドエフェクタ１７だけでなく、フランジ部９に取り付けられたハンドアイセンサ１１についても、設置面Ａとの干渉を確実に防止することができる。
【０１００】
なお、本実施形態では、図１０のフローチャートに示すように、ハンドアイセンサ１１が設置面Ａと干渉（衝突）する（しそうになっている）場合（ステップＳ１５でＹＥＳ）に、多関節型アーム５の駆動を停止させた後（ステップＳ１７）、ステップＳ７にリターンして、ピッキング対象のワーク１５のピッキングに適したエンドエフェクタ１７のピッキング箇所及び姿勢を新たに決定する構成とした。
【０１０１】
しかし、図１１のフローチャートに示すように、多関節型アーム５の駆動を停止させた後（ステップＳ１７）、ステップＳ１７における多関節型アーム５の停止時とは異なるエンドエフェクタ１７の姿勢を新たに決定し（ステップＳ１９）、エンドエフェクタ１７が決定した姿勢となるように多関節型アーム５を駆動させた後（ステップＳ２１）、ステップＳ１１にリターンして、ステップＳ７で決定した姿勢でエンドエフェクタ１７がピッキング箇所に移動するように、多関節型アーム５を駆動させるようにしてもよい。このように構成する場合には、ステップＳ１９が、請求項中の姿勢再決定手段に対応する処理となる。
【０１０２】
このように構成した場合には、ワーク１５のピッキングに適した新たなエンドエフェクタ１７の姿勢を、多関節型アーム５の駆動停止時とは異なる姿勢として容易に決定することができ、ロボット１のフランジ部９に取り付けられたハンドアイセンサ１１の設置面Ａとの干渉防止を、より簡素な姿勢再決定のロジックで図ることができる。
【０１０３】
また、本実施形態では、ある姿勢ではハンドアイセンサ１１が設置面Ａと干渉（衝突）してしまう場合に、多関節型アーム５のＡ６軸６ｊよりも先の部分が適切な角度（例えば１８０゜）回転した姿勢で、ハンドアイセンサ１１が設置面Ａと干渉（衝突）するか否かを、合わせて判定するものとした。しかし、そのための構成は省略しても良い。
【０１０４】
また、上述した実施形態では、グリッパタイプのエンドエフェクタ１７や吸着パッドによるエンドエフェクタ１７をフランジ部９に取り付けたロボット１を例に取って説明した。しかし、本発明は、それら以外のエンドエフェクタをフランジ部９に取り付けたロボット１であっても、ワーク１５のビンピッキングに適したハンドアイ方式のロボットの制御に広く適用可能である。
【符号の説明】
【０１０５】
１  ハンドアイ式ビンピッキングロボット
３  基台（ベース）
３ａ  設置面
５  多関節型アーム（ロボットアーム）
５Ａ〜５Ｆ  ロータリアクチュエータ
５Ｇ〜５Ｌ  ドライブ回路
５Ｍ〜５Ｒ  ロータリセンサ（回転角度検出手段）
６ａ  第１アーム部
６ｂ  第２アーム部
６ｃ  第３アーム部
６ｄ  第４アーム部
６ｅ  Ａ１軸（自由軸）
６ｆ  Ａ２軸（自由軸）
６ｇ  Ａ３軸（自由軸）
６ｈ  Ａ４軸（自由軸）
６ｉ  Ａ５軸（自由軸）
６ｊ  Ａ６軸（自由軸）
９  フランジ部（手先部）
９ａ  ステー
１１  ハンドアイセンサ
１３  トレー
１５  ワーク
１７  エンドエフェクタ
１９  ハンドアイコントローラ
３１  ロボットコントローラ（ピッキング座標変換手段、ベース座標変換手段、移動先座標値変換手段、移動先干渉判定手段、駆動手段、駆動中座標値変換手段、駆動中干渉判定手段、停止手段、姿勢再決定手段）
Ａ  設置面（ステージ）
Ａ１  代表点
【図１】
2011093014.tif 000003
【図２】
2011093014.tif 000004
【図３】
2011093014.tif 000005
【図４】
2011093014.tif 000006
【図５】
2011093014.tif 000007
【図６】
2011093014.tif 000008
【図７】
2011093014.tif 000009
【図８】
2011093014.tif 000010
【図９】
2011093014.tif 000011
【図１０】
2011093014.tif 000012
【図１１】
2011093014.tif 000013