(57)【要約】【課題】  把持対象物の近くに他の物体が存在する場合において対象物の把持動作を適切に制御することを可能としたロボットハンドによる把持制御方法を提供する。【解決手段】  初期位置ｑ１から到達目標位置ｑＧ，ＴＯまでの経路を設定する際に、ランダム探索により経路設定が不能な場合には、さらに、経路候補の終端を処理し、処理した終端位置ｑＡから到達目標位置ｑＧ，ＴＯへと至る経路を設定する。この経路上の物体Ｏ１０を除去すべき障害物とする。同様に、Ｏ１０を除去するための把持位置ｑＧ１０に到達する経路の障害物Ｏ２０やＯ２０を除去するための把持位置ｑＧ２０に到達する経路の障害物Ｏ３０、Ｏ３１についても同様の処理を行うことで、障害物の除去を含めた経路を設定する。【選択図】  図４
2007237334.tif 000002
【特許請求の範囲】
【請求項１】
視覚センサを用いて把持対象物およびその周囲の位置関係を把握し、対象物を把持させるロボットハンドによる把持制御方法において、
経路探索アルゴリズムによって初期位置から対象物の把持位置までのハンドの移動経路である第１種の経路を設定する工程と、
障害物が存在するため、前記第１種の経路設定が不能な場合に、経路を再設定する工程であって、
前記第１種の経路設定時に障害物の存在やロボットハンドの拘束条件により候補外とされた経路の最終地点から把持位置までを接続することで第２種の経路を設定する工程と、
前記第２種の経路上に存在する障害物を抽出する工程と、
抽出した障害物を除去する経路を含めた把持位置までのハンドの移動経路を再設定する工程と、を有する工程と、
を備えていることを特徴とするロボットハンドによる把持制御方法。
【請求項２】
前記第２種の経路を複数設定し、経路上に存在する障害物に応じて経路の優先順位を設定して、経路の再設定を行うことを特徴とするロボットハンドによる把持制御方法。
【請求項３】
視覚センサを用いて把持対象物およびその周囲の位置関係を把握し、対象物を把持させるロボットハンドによる把持制御方法において、
経路探索アルゴリズムによって初期位置から対象物の把持位置までのハンドの移動経路である第１種の経路を設定する工程と、
障害物が存在するため、前記第１種の経路設定が不能な場合に、経路を再設定する工程であって、
把持対象物に近接し、把持動作を妨害する近接障害物を検出する工程と、
検出した近接障害物がない場合の把持対象物へと至る第２種の経路を設定する工程と、
前記第２種の経路設定が不能な場合に、ロボットハンドの動きを限定した条件で、初期位置から把持位置へと至る第３種の経路を設定する工程と、
前記第３種の経路上に存在する他の障害物を抽出する工程と、
障害物を除去する経路を含めた把持位置までのハンドの移動経路を再設定する工程と、を有する工程と、
を備えていることを特徴とするロボットハンドによる把持制御方法。
【請求項４】
前記第３種の経路を複数設定するとともに、経路上に存在する障害物に応じて経路に優先順位を設定して、経路の再設定を実行することを特徴とする請求項３記載のロボットハンドによる把持制御方法。
【請求項５】
障害物を検出する際に、前提とした把持動作では経路の再設定が不能な場合には、前提とする把持動作を変更して再設定工程を実施することを特徴とする請求項３または４に記載のロボットハンドによる把持制御方法。
【請求項６】
経路の優先順位は、ロボットハンドに近い障害物がある経路を優先し、ほぼ同じ距離であればロボットハンドから見て外側に障害物が位置する経路を優先し、それらが近接している場合には、背の高い障害物が存在する経路を優先することを特徴とする請求項２または４のいずれかに記載のロボットハンドによる把持制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットハンドによって対象物を把持する際の把持制御方法に関し、特に、把持対象物の周囲に物体が存在し、物体把持動作の障害となる場合の把持制御方法に関する。
【背景技術】
【０００２】
視覚センサを用いて外界の情報を取得し、複雑な作業を実行するロボットの開発が進められている。例えば、特許文献１に開示されている技術は、知能ロボット等において能動的に外部環境を認識する環境認識システムである。この技術の基本的な考え方は、ある点から見ただけでは、物体を認識することができなければ、もっとよく見える位置に視点を移動したり、複雑に隠蔽されて認識が困難な物体は、障害物を隠蔽しない場所に移動させる、あるいは、照明条件を適切に変更するなど、積極的に自己および外部環境を変化させることにより、視覚系が機能しやすい環境を自ら作り出そうとするものである、と記載されている。
【特許文献１】特開平８−３０３２７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
この技術においては、例えば、別のロボットを視覚により認識する場合に、自らが動く、他のロボットの移動を促す、間にある障害物を移動させる、等の例が開示されている。ところで、人の生活環境内で動作するロボットにおいては、ある物体を探索して、これを把持し、所定の動作を行わせるような用途が考えられるが、上記技術においては、把持対象物の近くに他の物体が存在し、それが物体把持の障害になっているような場合に、除去する障害物の選択を含めた把持動作の設定をどのように行うべきかについては何ら開示されていない。
【０００４】
そこで本発明は、把持対象物の近くに他の物体が存在する場合において対象物の把持動作を適切に制御することを可能としたロボットハンドによる把持制御方法を提供することを課題とする。
【課題を解決するための手段】
【０００５】
上記課題を解決するため、本発明にかかるロボットハンドによる把持制御方法は、視覚センサを用いて把持対象物およびその周囲の位置関係を把握し、対象物を把持させるロボットハンドによる把持制御方法において、（１）経路探索アルゴリズムによって初期位置から対象物の把持位置までのハンドの移動経路である第１種の経路を設定する工程と、（２）障害物が存在するため、この第１種の経路設定が不能な場合に、経路を再設定する工程であって、（ａ）第１種の経路設定時に障害物の存在やロボットハンドの拘束条件により候補外とされた経路の最終地点から把持位置までを接続することで第２種の経路を設定する工程と、（ｂ）第２種の経路上に存在する障害物を抽出する工程と、（ｃ）抽出した障害物を除去する経路を含めた把持位置までのハンドの移動経路を再設定する工程と、を有する工程と、を備えていることを特徴とする。
【０００６】
（１）の工程における経路探索アルゴリズムにおいては、実施の一形態として初期位置からランダムな方向に経路を伸ばしていく探索動作を実行し、ロボットハンドの拘束条件や障害物にぶつかった場合には、その位置からの探索の続行を止め、最終的に把持位置へと到達した経路を経路として選択する。このようにして経路が見つかった場合の経路を第１種の経路と称する。本発明では、この第１種の経路が見つからない場合には、探索の続行を止めた位置、最終地点から把持位置までを接続することにより、経路を設定する（上記（２）の（ａ）工程）。この経路（第２種の経路）算出においては、経路探索アルゴリズムを用いるとよく、この探索においては、ロボットハンドの拘束条件は考慮されるが、障害物の条件は無視する。そして、続く（ｂ）工程において、設定した経路上の障害物を抽出し、（ｃ）工程において、各障害物を除去するための経路を含めて経路を求めることで、把持位置へと至る経路の再設定を行う。
【０００７】
なお、上記の経路探索アルゴリズムとしては、上述したようなランダム探索アルゴリズムである必要はなく、例えば、初期地点から最終地点に至るポテンシャル場を設定し、最急降下法で経路を求める方法や、他の経路探索アルゴリズム等いずれを用いてもよい。
【０００８】
この第２種の経路を複数設定し、経路上に存在する障害物に応じて経路の優先順位を設定して、経路の再設定を行うとよい。第２種の経路を設定する際に、複数の経路を設定しておくことで、経路の選択肢が増える。
【０００９】
あるいは、本発明にかかるロボットハンドによる把持制御方法は、上記の（２）に代えて、（２’）障害物が存在するため、第１種の経路設定が不能な場合に、経路を再設定する工程であって、（ａ）把持対象物に近接し、把持動作を妨害する近接障害物を検出する工程と、（ｂ）検出した近接障害物がない場合の把持対象物へと至る第２種の経路を設定する工程と、（ｃ）第２種の経路設定が不能な場合に、ロボットハンドの動きを限定した条件で、初期位置から把持位置へと至る第３種の経路を設定する工程と、（ｄ）第３種の経路上に存在する障害物を抽出する工程と、（ｅ）これらの障害物を除去する経路を含めた把持位置までのハンドの移動経路を再設定する工程と、を有する工程と、を備えているものでもよい。
【００１０】
この発明においては、（１）の工程における第１種の経路が存在しない場合には、把持対象物の周囲において、把持動作自体を妨害する可能性のある近接障害物をまず検出する（（ａ）工程）。この障害物が存在しなければ把持対象物へと至る経路（第２種の経路）の設定が可能な場合には、この第２種の経路を経路候補とする（（ｂ）工程）。この第２種の経路の設定手法としては、例えば、ランダム探索アルゴリズムを用いるとよい。この経路の設定が不能な場合には、さらに、ロボットハンドの動きを限定した条件で初期位置から把持位置へと至る第３種の経路を設定する（（ｃ）工程）。ロボットハンドの動きを限定することにより、探索すべき経路範囲が限定される。この第３種の経路の設定において存在する障害物は無視される。（ｄ）工程では、設定した経路上の障害物を抽出し、（ｅ）工程において、各障害物・障害物を除去するための経路を含めて経路を求めることで、把持位置へと至る経路の再設定を行う。
【００１１】
この第３種の経路を複数設定し、経路上に存在する障害物に応じて経路に優先順位を設定して、経路の再設定を実行するとよい。第３種の経路を設定する際に、複数の経路を設定しておくことで、経路の選択肢が増える。
【００１２】
また、上記（ａ）工程において、障害物を検出する際に、前提とした把持動作では経路の再設定が不能な場合には、前提とする把持動作を変更して再設定工程を実施するとよい。具体的には、把持姿勢や把持位置を変更することにより、再設定工程をやり直すことで、経路の選択肢を増やす。
【００１３】
経路の優先順位は、ロボットハンドに近い障害物がある経路を優先し、ほぼ同じ距離であればロボットハンドから見て外側に障害物が位置する経路を優先し、それらが近接している場合には、背の高い障害物が存在する経路を優先するとよい。これは、ロボットハンドに近いほど、また外側に位置する場合ほど、背の高い障害物ほど、物体を把持する場合に障害となりやすいからである。
【発明の効果】
【００１４】
上記（２）の工程によれば、把持対象物の周囲に物体が存在しているため、そのままでは把持対象物に直接触れることができない場合に、障害となる物体を取り除いて把持対象物を把持するまでの経路を確実に設定することが可能となる。このとき、例えば、ランダム探索アルゴリズムの結果を利用することで、探索に要する演算量を少なくすることができ、より短時間で経路を探索することができる。
【００１５】
また、（２’）の工程によっても、把持対象物の周囲に物体が存在しているため、そのままでは把持対象物に直接触れることができない場合に、障害となる物体を取り除いて把持対象物を把持するまでの経路を確実に設定することが可能となる。この場合は、把持動作の障害となる物体を優先して除去するとともに、ロボットハンドの動きを限定した条件で経路探索を行うことで、探索に要する演算量を少なくすることができ、より短時間で経路を探索することができる。
【００１６】
（２）の工程の第２種の経路、（３）の工程の第３種の経路を複数設定しておくことで、経路の選択肢が増える。設定した複数の経路からいずれを選択するかについては、例えば、障害物が少なく、除去しやすい経路を選択するとよい。この順序としては、ロボットハンドからの距離、外側への位置、高さにより判断を行うことで、判断基準が簡明になり、処理が容易で、高速で選別を行うことができる。
【発明を実施するための最良の形態】
【００１７】
以下、添付図面を参照して本発明の好適な実施の形態について詳細に説明する。説明の理解を容易にするため、各図面において同一の構成要素に対しては可能な限り同一の参照番号を附し、重複する説明は省略する。
【００１８】
図１は、本発明にかかる把持制御方法を実行するロボットのブロック構成図である。このロボットは、多指多関節のハンドを左右の多関節アームの先端に配置しており、各関節に配置されるアクチュエータ３１を駆動することで、物体を把持する等の所定の動作を行わせることができる。ロボットの頭部にはステレオカメラ２０が配置されており、対象物や周辺環境のステレオ画像を取得し、制御部１へ転送する。
【００１９】
制御部１は、ＣＰＵ、ＲＯＭ、ＲＡＭ等によって構成されており、内蔵したプログラムに応じてロボットハンド・アームに所定の動作を実行させる。図示は、省略しているが、入力手段（キーボードや音声入力、有線・無線ＬＡＮ等）からの指示に応じて所定の動作を行ったり、ステレオカメラ２０で取得した画像中から探索した対象物に応じた動作を行うようにすればよい。所定の動作としては、物体を把持して予め定められた位置に移動させる動作や、物体を把持したままプログラムされている一連の動作を実行する動作等が含まれる。
【００２０】
制御部１は、ステレオカメラ２０で取得した画像を処理する画像処理部１０と、処理結果からロボットの周辺環境における物体の形状や位置情報を取得する周辺環境認識部１１、ロボットの各関節等に配置された角度センサ３０の出力からロボットの位置姿勢を認識する位置姿勢認識部１２、所望の動作を行わせるためのロボットハンドの移動経路を探索する経路探索部１３、アクチュエータ３１を駆動して探索した経路上の移動を支援する駆動制御部１４を備えている。これらの各部１０〜１４は、別個のハードウェアによって構成されていてもよいが、一部または全部が、一部または全部のハードウェアを共有していてもよく、その場合でもソフトウェアが完全に独立している必要はなく、その一部を共有する構成となっていてもよい。
【００２１】
このロボットによる物体の把持制御について説明する前に、対象物の被操作部という概念について簡単に説明する。図２は、把持経路設定における対象物、被操作部、操作姿勢の関係を示す図である。把持の対象となる物体（対象物）には、把持可能な場所である被操作部（EfM: Effective for Manipulation part）が１ないし複数存在する。そして、各被操作部に対して、１ないし複数の操作姿勢が存在する。この操作姿勢とは把持を行う場合を例にとれば、把持するハンドの位置・姿勢に対応する。
【００２２】
対象物が立円筒の場合であれば把持する位置である被操作部としては少なくとも側壁部と上部がありうる。そして、例えば上部を把持する場合でも把持する姿勢には様々な姿勢が考えられる。そして、姿勢が異なれば対象物の周辺にある物体とロボットとの位置関係にも影響を及ぼし、把持操作の障害となる物体＝障害物も変わりうる。本発明にかかる把持制御方法においては、これら対象物、被操作部、操作姿勢を考慮して、経路設定を行う。
【００２３】
図３は、本発明にかかる把持制御方法の第１の実施形態の処理を示すフローチャートである。この処理は、制御部１において、外部からまたは、内部のプログラムから把持指令が発せられた場合に実行されるものである。
【００２４】
最初に、ランダム探索アルゴリズムによって対象物までの経路探索処理を行う（ステップＳ１）。このランダム探索アルゴリズムとは、各種の経路設定手法としてよく知られている手法である。例えば、図３に示されるように、開始位置ｑ１から到達目標位置ｑＧ，ＴＯまでの経路を探索する場合に、経路を短いノードの組み合わせとして設定するとともに、開始位置からランダムにノードの方向を設定し、さらに各ノードからの分岐についても同じようにランダムにノードを設定していくことで、正しい経路を探索する手法である。ノードが他の物体へ接触・衝突する場合や、ノードの終端位置がロボットハンドの拘束条件から設定される境界位置を越える場合には、当該ノードの設定や、ノードからの分岐は停止される。また、ノードが重複するような分岐も制限される。
【００２５】
具体的には、ステレオカメラ２０で取得した画像に基づいて画像処理部１０、周辺環境認識部１１において、対象物とその周囲の障害物となりうる物体の形状、位置情報を取得しておく。一方、位置姿勢認識部１２は、角度センサ３０の出力から各関節の状態を把握し、これを基にしてハンドの位置を把握しておく。経路探索部１３は、各位置情報、姿勢情報を基にして対象物の被操作部、その操作姿勢を設定することにより、最終的なハンドの到達目標位置を設定し、現在位置から到達目標位置へと至る経路のランダム探索を実行する。
【００２６】
ステップＳ２では、このランダム探索アルゴリズムによって目標位置へと至る経路（第１種の経路）の設定に成功したか否かを評価する。設定に成功した場合には、他の障害物等に接触することなく把持動作が可能な場合であるから、ステップＳ３へと移行して設定した探索経路を出力し、処理を終了する。その後、駆動制御部１４が、アクチュエータ３１を駆動して設定した経路に沿ってハンドを移動させる。このとき、角度センサ３０の出力を位置姿勢認識部１２で読み込んで、経路に沿った移動が実施されているか、フィードバック制御を行うとよい。この制御はフィードバック制御に限られるものではなく、各種の制御手法を適宜組み合わせるとよい。
【００２７】
第１種の経路設定に失敗した場合、つまり、目的位置までの間に障害物が存在している場合には障害物除去判定に移行する。具体的には、まず、ステップＳ４では、ステップＳ２のランダム探索アルゴリズムで求めた経路候補を抽出し、その終端位置を処理する。ここでいう経路候補は、上述したノード連結において、障害物に接触・衝突したり、拘束条件によりノード設定・分岐が停止された経路である。最初の経路候補としては、最も目標位置へ近い位置でノード設定・分岐が停止された経路が選択される。そして、終端処理とは、抽出した経路候補を構成するノードのうち、最も目標位置に物理的に近い位置を終端位置とする処理である。例えば、図４に示される経路では、ｑＢ、ｑＣまでノードが延びていたとしても、ｑＢ、ｑＣより目標位置にｑＧ，ＴＯ近いｑＡが終端位置に設定される。
【００２８】
次に経路候補の終端位置から目標位置までの接続経路を導出する（ステップＳ５）。ここでは、経路上の障害物は無視し、ロボットハンドの動力学的特性を基にして終端位置と目標位置とを接続する経路を求める。具体的には、終端位置からは、単純な関節動作の組み合わせで到達できる経路（第２種の経路）を設定する。
【００２９】
経路を設定したら、当該経路上に存在する物体＝障害物を検出する（ステップＳ６）。図４の例では、Ｏ１０がこれに対応する。ここで、検出する障害物は、単にロボットハンド上の特定の一点の軌跡上に存在する物体を指すのではなく、ハンドを設定した経路に沿って移動させる際に、ロボットハンド・アームのいずれかの部分が接触ないし衝突する可能性のある物体全てが検出される。
【００３０】
次に障害物の順序づけを行う（ステップＳ７）。具体的には、検出された障害物が複数存在する場合には、予め設定された基準に基づいて障害物の除去優先順位を設定する。例えば、基準としては、まず、障害物のうちロボットに近い方を優先する。両者が略同じ距離にある場合には、ロボットの手の届く境界位置に近いもの、左手で把持する場合は、ロボットハンドの到達範囲内で最も左側にある物体、右手で把持する場合は、ロボットハンドの到達範囲内で最も右側にある物体を優先する。これらは、障害物同士がある一定距離以上離れている場合に用いられ、障害物間の距離が近い場合には、それらのうち、被操作部が高い位置にあるものを優先して除去する。
【００３１】
障害物の除去優先順位を決定したら、障害物自体を除去する経路を設定する。これは、障害物を対象物としてステップＳ１の処理と同様の手法により経路を設定することにより行われる。この際に、当該障害物を除去するためには、さらに別の障害物を除去する必要がある場合、当該障害物の除去に先立ってこの別の障害物を除去する経路の検索を行う。
【００３２】
例えば、図４に示される経路では、障害物Ｏ１０を除去するための到達位置ｑＧ１０がＯ２０により遮られているため、Ｏ２０を先行して除去する必要がある。このＯ２０を除去するための到達位置ｑＧ２０は、障害物Ｏ３０、Ｏ３１によって遮られているので、これらＯ３０、Ｏ３１を除去する必要がある。これらの障害物の関係をツリー図として表したのが、図５である。このツリー図においては、最上位が最終到達目標位置（本図ではｑＧ，ＴＯ）を表し、それぞれの到達位置ないし物体に対する障害物がその下にそれぞれ分岐する構成となる。Ｏ３０とＯ３１が並列に記されているのは、いずれもＯ２０に対する独立した障害物であるからである。そして、ロボットに近いＯ３０が優先して除去される。
【００３３】
次に、ステップＳ９において経路探索に成功したか否かをチェックする。経路を設定できなかった場合には、ステップＳ１０へと移行して、別経路候補を選択し、ステップＳ４からの経路設定処理をやり直す。なお、別経路候補には、把持対象物の被操作部、把持姿勢を異ならせた経路も含まれる。経路探索に成功した場合には、探索経路と障害物を出力し（ステップＳ１１）、処理を終了する。この後、前述したように、駆動制御部１４が、アクチュエータ３１を駆動して設定した経路に沿ってハンドを移動させる制御を実施する。
【００３４】
経路上に複数の障害物が存在する場合に、ごく稀にではあるが、物体Ａを除去するには、物体Ｂが障害物となり、物体Ｂを除去するには物体Ａが障害物となるというデッドロック関係が成立することがある。本制御においては、このような事態が発生しうる環境においても、ランダム探索によって設定された複数の経路候補から経路を選択することで、これらの物体と干渉しない経路の選択可能性が高まるので対象物へ至る経路を設定する角度が高まり、経路生成が容易になる。
【００３５】
また、ランダム探索アルゴリズムによって障害物に接触・衝突することなく目標到達位置へと到達する経路を優先して設定し、その経路の設定が不能な場合には、このランダム探索で用いた経路候補を利用して障害物を除去する必要のある経路の設定を行うため、障害物検出時の経路設定処理が簡便化されるので、処理時間の高速化が図れるという利点も有している。
【００３６】
なお、ステップＳ４〜Ｓ８の処理を複数の経路候補について同時並行的に実行し、除去障害物の少ない経路や経路が単純な経路を優先して設定するようにしてもよい。この場合には、さらに適切な経路設定が可能となるという利点を有する。
【００３７】
本発明にかかる把持制御方法は上記の実施形態に限られるものではない。図６は、本発明にかかる把持制御方法の第２の実施形態の処理を示すフローチャートである。この処理も第１の実施形態と同様に、制御部１において、外部からまたは、内部のプログラムから把持指令が発せられた場合に実行されるものである。
【００３８】
最初にランダム探索アルゴリズムによって対象物までの経路探索処理を行い（ステップＳ２１）、探索の成否を判定して（ステップＳ２２）、成功した場合には、探索経路（第１種の経路）を出力して（ステップＳ２３）処理を終了する点については、第１の実施形態におけるステップＳ１〜Ｓ３の処理と同一である。
【００３９】
ステップＳ２２において、経路の設定ができなかったと判定した場合には、ステップＳ２４へと移行して、対象物に近接する障害物を判定する。ここで、対象物に近接する障害物とは、対象物近くで、把持姿勢に影響を及ぼしうる障害物を指す。図７は、このような障害物と対象物、被操作部、操作姿勢の関係を示している。ここで検出される「対象物に近接する障害物」（以下、近接障害物と称する。）は、操作姿勢によって定まる。そして、操作姿勢は、被操作部ＥｆＭによって１つあるいは複数定まりうる。さらに被操作部ＥｆＭも対象物により１つないし複数設定されうる。したがって、１つの対象物に対して、１つあるいは複数の被操作部ＥｆＭがありえ、これらは選択的な関係にある。そして各被操作部ＥｆＭに対しても１つあるいは複数の把持姿勢がありえ、これらも選択的な関係にある。さらに、各把持姿勢に対して１つないし複数の近接障害物がありうる。特定の把持姿勢に対する複数の近接障害物は選択的な関係ではなく、その全てが同等の障害物である。
【００４０】
この近接障害物の判定処理についてさらに具体的に説明する。図８は、周辺環境の一例を示している。ここでは、ロボット４０により、テーブル７０上の急須５０を把持する場合を例にとる。そして、急須５０の周囲の障害物を符号６０で表す。図９、図１０に示されるように、急須５０の把持部である把手５１を円柱で近似し、その長さをｗｈ、直径をｄｃとする。実際に把持を行うための操作用領域５５は、把手５１を取り囲む直径（ｄｃ＋２×ｄｈ）で長さ（ｗｈ＋ｗｍ）の空間となる。
【００４１】
図１１は、この近接障害物の判定処理を示すフローチャートである。最初に、各物体形状を水平面に投影する（ステップＳ４１）。例えば、図８に示されるような環境下では、テーブル７０上に投影を行う。この投影は、ロボットからみて前後方向をｘ軸、左右方向をｙ軸とするｘｙ平面に対して行われる。次に、把持位置近傍の所定の点を原点とする所定の座標系へと変換を行う（ステップＳ４２）。図１０に示される例では、把手５１の中心軸上の把手の根元の投影点を原点Ｏ’とし、Ｘ’軸を把手５１の中心軸、Ｙ’軸をこれに直角な方向に設定した座標系にＸ’Ｙ’座標系へ変換する。
【００４２】
座標変換後、操作用領域５５と投影面の重なる物体を障害物候補として抽出する（ステップＳ４３）。上述したように座標を変換することで、操作用領域５５は、Ｘ’が０以上（ｗｈ＋ｗｍ）以下、Ｙ’の絶対値が（ｒｃ＋ｄｈ）となり、この操作用領域５５との重なり判定が容易になる。図１０に示される例では、横円筒６０３，１、立方体６０１，２、立円筒６０２，１、球６０４，１が投影面の重なる障害物候補として認識される。
【００４３】
次に、抽出した障害物候補と対象物の３次元形状、位置関係から実際に近接関係にあるか否かを判定し、近接関係を満たす候補を障害物として抽出する（ステップＳ４４）。例えば、図１２に示されるように、障害物候補６０ｉ、６０ｊと操作用領域５５の高さ位置関係から接触の有無を判定するとよい。同図に示される条件下では、障害物候補６０ｉは障害物ではないと判定され、障害物候補６０ｊが障害物と判定される。最後に、抽出した障害物についてその除去の優先順位である順序づけを行う（ステップＳ４５）。
【００４４】
障害物の判定処理が終了したら、図６に示されるフローチャートのステップＳ２５へと移行し、条件を満たす近接障害物が存在したか否かを判定する。近接障害物が存在していると判定した場合には、ステップＳ２６へと移行して抽出した近接障害物がないと仮定して対象物までの経路（第２種の経路）を探索する。この経路探索はステップＳ２１と同様のランダム探索に基づいて行ってもよいし、当該障害物によってノード接続が中止された経路候補を延長する形で探索を行ってもよい。
【００４５】
経路探索後、その成否を判定する（ステップＳ２７）。経路設定に成功した場合には、後述するステップＳ３０へと移行する。一方、経路設定が不能と判定した場合、および、前述したステップＳ２５において、近接障害物は存在していないと判定した場合には、ステップＳ２８へと移行し、ロボットの関節の動きを単純化することで初期位置から目標到達位置へと至る経路（単純経路＝第３種の経路）を設定し、この単純経路上における障害物を検出する。
【００４６】
ここでは、図８に示されるような環境下におけるこの単純経路の例を２つ述べる。第１の例を図１３に示す。まず、ロボットの右腕を右側にまっすぐ肩の高さまで伸ばす。その位置からハンドを対象物５０へ向かって弧を描くように左下方へと下ろしていく。このとき、肩関節４０ｓと肘関節４０ｅを同時に曲げる。それぞれの関節は、等角速度運動をなす。このとき手首４０ｗの描く軌跡は、アルキメデス螺旋となり、肘関節４０ｅの描く軌跡は、アルキメデスの螺旋面の形成する円錐の斜め横断面の弧に対応する。これらは、ロボットの形状から幾何学的に求めうる。そして、このときロボットハンド・アームが通過する領域にその一部が位置する物体を障害物と認定する。
【００４７】
第２の例を図１４、図１５に示す。この場合は、まず、肘関節４０ｅを駆動し、前腕部を上腕部側へ折り曲げる。次に、上腕部を右側へ伸ばして、肩の高さまであげる。このとき、前腕部は上腕部の上に折り曲げられた体勢となる。ここから、肩関節４０ｓを回して前腕部を左方向に回転させ、前腕部と上腕部の中心軸のなす平面が対象物５０上に位置する位置まで移動させる。次に、ハンドの先端をまっすぐに対象物まで移動させる。このとき、肘関節４０ｅは、右側へと移動させる形となる。この場合にもおいても、各関節等の移動軌跡は幾何学的に求めることができる。そのため、ロボットハンド・アームが通過する領域、そこに位置する障害物についても幾何計算により求めることができる。
【００４８】
障害物を検出したら、検出した全障害物が存在しないものと仮定して対象物までの経路を検索する（ステップＳ２９）。この場合、少なくとも上述した単純経路の設定は可能であることから、上述した単純経路を経路としてもよいが、さらに、それ以外の経路を探索してもよい。こうした経路は、駆動系の動作としては複雑になるものの、経路自体の軌跡は単純経路に比べて短くなりうる。
【００４９】
ステップＳ３０では、障害物・近接障害物が複数ある場合に、その除去の順位づけを行う。この順位設定は、上述した第１の実施形態と同様の基準により行えばよい。順位づけが終了したら、障害物除去のための経路を探索する（ステップＳ３１）。この経路探索は、個々の障害物を対象物としてステップＳ２１と同様のランダム探索アルゴリズムによって経路設定を行えばよい。
【００５０】
経路設定を終えたらステップＳ３２へと移行して、探索に成功したか否かを判定する。経路を設定できなかった場合には、ステップＳ３３へと移行して、別経路ないし別の近接障害物を選択し、ステップＳ２４からの経路設定処理をやり直す。なお、別経路候補には、把持対象物の被操作部、把持姿勢を異ならせた経路も含まれる。経路探索に成功した場合には、探索経路と障害物を出力し（ステップＳ３４）、処理を終了する。この後、前述したように、駆動制御部１４が、アクチュエータ３１を駆動して設定した経路に沿ってハンドを移動させる制御を実施する。
【００５１】
本実施形態の制御においては、近接障害物を優先的に除去する経路を設定するため、確実に把持体勢へと至ることができる。また、障害物を検出する際にハンド・アームの動きを限定した単純経路を用いて判定を行うので、障害物の判定処理が容易かつ簡便なものとなり、処理を高速化することが可能となる。また、デッドロック関係に対しても第１の実施形態と同様に他の経路を設定できる可能性が高まる。
【００５２】
上述した単純経路はこれに限られるものではなく、各種の経路設定を行うことが可能である。また、本実施形態においても、複数の把持部・把持姿勢に対してステップＳ２４〜Ｓ３２の処理を同時並行的に処理するようにしてもよい。
【図面の簡単な説明】
【００５３】
【図１】本発明にかかる把持制御方法を実行するロボットのブロック構成図である。
【図２】把持経路設定における対象物、被操作部、操作姿勢の関係を示す図である。
【図３】本発明にかかる把持制御方法の第１の実施形態の処理を示すフローチャートである。
【図４】図３の処理における物体と経路候補の一例を示す図である。
【図５】図４の状態における経路設定結果の構成を示す図である。
【図６】本発明にかかる把持制御方法の第２の実施形態の処理を示すフローチャートである。
【図７】図６の処理において検出対象となる対象物に近接する障害物と対象物、被操作部、操作姿勢の関係を示す図である。
【図８】近接障害物の判定処理を説明する周辺環境の一例を示す図である。
【図９】図８の環境中での把持対象物である急須を示す図である。
【図１０】図９の急須に近接する障害物を示している。
【図１１】図６の処理における近接障害物の判定処理を示すフローチャートである。
【図１２】図１０に示す急須を対象に、図１１の処理で対象となる障害物との位置関係を示す図である。
【図１３】図６の処理における単純経路の第１の例を示す図である。
【図１４】図６の処理における単純経路の第２の例を示す図である。
【図１５】図１４の単純経路を上から見た図である。
【符号の説明】
【００５４】
１…制御部、１０…画像処理部、１１…周辺環境認識部、１２…位置姿勢認識部、１３…経路探索部、１４…駆動制御部、２０…ステレオカメラ、３０…角度センサ、３１…アクチュエータ、４０…ロボット、４０ｓ…肩関節、４０ｗ…手首、４０ｅ…肘関節、５０…急須（対象物）、５１…把手、５５…操作用領域、６０ｉ、６０ｊ…障害物候補、７０…テーブル、ＥｆＭ…被操作部。
【図１】
2007237334.tif 000003
【図２】
2007237334.tif 000004
【図３】
2007237334.tif 000005
【図４】
2007237334.tif 000006
【図５】
2007237334.tif 000007
【図６】
2007237334.tif 000008
【図７】
2007237334.tif 000009
【図８】
2007237334.tif 000010
【図９】
2007237334.tif 000011
【図１０】
2007237334.tif 000012
【図１１】
2007237334.tif 000013
【図１２】
2007237334.tif 000014
【図１３】
2007237334.tif 000015
【図１４】
2007237334.tif 000016
【図１５】
2007237334.tif 000017