JP2009107043A - 把持装置および把持装置制御方法 - Google Patents

Abstract

【課題】 本発明は、複雑な把持動作が必要になる把持装置の制御を、円滑に行うことを目的とする。
【解決手段】 本発明は、把持手段に把持される被把持部材の位置および姿勢に関する情報を取得する情報取得手段と、前記取得された情報に基づき前記被把持部材の把持に適した状態である把持目標状態を取得し、当該把持目標状態に向かうように前記把持手段の移動経路を設定する移動経路設定手段と、前記設定された移動経路に沿って、前記把持手段を移動させ、前記被把持部材を把持させる制御手段とを有することを特徴とする。
【選択図】 図２

Description

本発明は、把持対象を把持するための把持装置および把持装置制御方法に関する。

工業製品の生産には様々な工程があり、各工程の作業を自動的に行うために様々な作業用ロボットが用いられている。しかし、作業が複雑であるほど、作業用ロボットによる各工程の自動化は困難である。

特許文献１には、作業工程が比較的複雑になるフレキシブルケーブルを把持し、把持したフレキシブルケーブルをコネクタに接続する作業を行う把持装置が提案されている。

特許文献１における作業方法は、まず、位置変位の少ないフレキシブルケーブルの途中部位を第一のハンドで緩めに把持し、第一のハンドでフレキシブルケーブルをしごいてケーブル先端部を所定位置に固定する。そして、ケーブル先端部を所定位置に固定した後、固定された先端部を第二のハンドで把持するというものである。
特開２００５−１１５８０

特許文献１における把持装置は、把持の際に、第一のハンドと第二のハンドとによる二工程が必要になり、把持作業が複雑になってしまっていた。

本発明は、上記課題を鑑み、複雑な把持動作が必要になる把持装置の制御を、円滑に行うことを目的とする。

本発明は、把持手段に把持される被把持部材の位置および姿勢に関する情報を取得する情報取得手段と、前記取得された情報に基づき前記被把持部材の把持に適した状態である把持目標状態を取得し、当該把持目標状態に向かうように前記把持手段の移動経路を設定する移動経路設定手段と、前記設定された移動経路に沿って、前記把持手段を移動させ、前記被把持部材を把持させる制御手段とを有することを特徴とする。

本発明によれば、複雑な把持動作が必要になる把持装置の制御を、円滑に行うことが出来る。

（第一の実施形態）
図１は本実施形態における把持装置の機能構成を示す図である。

Ａ１０は、被把持部材に対して、把持などの作業を行う把持手段である。把持手段Ａ１０は、把持などを行うためのロボットアーム、ロボットハンドなどから構成される。

Ａ４０は、把持手段Ａ１０の動作を制御する制御手段である。制御手段Ａ４０は、把持手段Ａ１０の動作量を算出するパーソナルコンピュータなどから構成される。

Ａ３１０は、把持手段Ａ１０が被把持部材を把持する際の把持手段Ａ１０、被把持部材の位置を取得する第一の三次元位置取得手段である。第一の三次元位置取得手段は、カメラなどの撮像手段から構成される。

Ａ３２０も、把持手段Ａ１０が被把持部材を接合部材に接合する際の把持手段Ａ１０、被把持部材の位置を取得する第二の三次元位置取得手段である。第二の三次元位置取得手段も、カメラなどの撮像手段から構成される。

Ａ５１０は、情報取得手段の一部として機能し、第一の三次元位置取得手段Ａ３１０で取得された三次元位置情報に基づき、把持手段Ａ１０、被把持部材の位置、姿勢状態などの三次元情報を推定する第一の三次元状態推定手段である。第一の三次元状態推定手段Ａ５１０は、推定処理のための演算を行うパーソナルコンピュータなどから構成される。

Ａ５２０も、情報取得手段の一部として機能し、第一の三次元位置取得手段Ａ３１０で取得された三次元位置情報に基づき、把持手段Ａ１０、被把持部材の位置、姿勢状態などの三次元情報を推定する第二の三次元状態推定手段である。第二の三次元状態推定手段Ａ５２０も、推定処理のための演算を行うパーソナルコンピュータなどから構成される。

Ａ６１０は、第一の三次元状態推定手段Ａ５１０の三次元情報に基づき、把持手段Ａ１０の把持動作を行う際の移動経路（第一の情報）を生成する第一の移動経路設定手段である。第一の移動経路設定手段演算Ａ６１０は、経路生成のための演算を行うパーソナルコンピュータなどから構成される。

Ａ６２０は、第二の三次元状態推定手段Ａ５２０の三次元情報に基づき、把持手段Ａ１０の接合作業を行う際の移動経路（第二の情報）を生成する第二の移動経路設定手段である。第二の移動経路設定手段Ａ６２０は、経路生成のための演算を行うパーソナルコンピュータなどから構成される。

制御手段Ａ４０は、第一の移動経路設定手段演算Ａ６１０および第二の移動経路設定手段Ａ６２０からの移動経路に基づき、把持手段Ａ１０の移動を制御し、被把持部材に対する把持および接合作業を行う。

図２は、図１で示した機能構成の具体的な構成を示す図である。

１０は図１における制御手段Ａ４０に対応し、把持装置に指示を送るロボットコントローラである。

１１は把持手段Ａ１０を構成し、被把持部材を把持するロボットアームである。

１２は把持手段Ａ１０を構成し、ロボットアーム１１のエンドエフェクタとして取り付けられたグリッパ形式のロボットハンドである。

２１は、把持手段Ａ１０による被作業物体の一つであるプリント基板である。

２２は、把持手段Ａ１０による被作業物体の一つである接合部材であり、例えば、コネクタである。

２３は、把持手段Ａ１０による被作業物体の一つである被把持部材であり、例えば、フレキシブルケーブルである。プリント基板２１、接合部材２２、被把持部材２３は一括してワークとも呼ばれる。

４１は、計算機であり、ＣＰＵ、ＲＡＭなどを有するパーソナルコンピュータなどにより構成される。計算機４１は、図１における第一の三次元状態推定手段Ａ５１０、第二の三次元状態推定手段Ａ５２０、第一の移動経路設定手段Ａ６１０、第二の移動経路設定手段Ａ６２０に対応する。計算機４１のＲＡＭには、第一の三次元状態推定手段Ａ５１０、第二の三次元状態推定手段Ａ５２０、第一の移動経路設定手段Ａ６１０、第二の移動経路設定手段Ａ６２０それぞれを動作させるためのコンピュータプログラムが格納されている。計算機４１による演算結果は、ロボットコントローラ１０に送信され、ロボットコントローラ１０はロボットアーム１１、ロボットハンド１２を制御する。

Ａ３１１は、図１における第一の三次元位置取得手段Ａ３１０に対応し、情報取得手段の一部として機能する第一のステレオカメラである。

Ａ３２１は、図１における第二の三次元位置取得手段Ａ３２０に対応し、情報取得手段の一部として機能する第二のステレオカメラである。第一のステレオカメラＡ３１１、第二のステレオカメラＡ３２１は、例えば被把持部材２３が変形したとしても、ワークを撮像可能な位置に配置する。

図３は、図２におけるロボットハンド１２の先端部の詳細な構成を示す図である。

１３は、ロボットハンド１２が被把持部材２３を把持する際に被把持部材２３に接触するフィンガーであり、被把持部材２３を把持するために十分な長さを有する。

１４は、被把持部材２３を把持する際に被把持部材２３と接触するフィンガー１３の把持面である。

１５は、被把持部材２３を把持する際の把持中心位置である。

１６は、フィンガー１３の中心を示すフィンガー中心基準線である。

１７は、ロボットハンド１２の中心軸であり、フィンガー１３は中心軸を挟んで配置されている。

なお、把持作業の際には様々な誤差が蓄積する可能性があるため、ロボットハンド１２は、後述する作業において、微小量の倣い機構が備わっていることが望ましい。

図４は、図２における把持装置による把持作業処理フローである。尚、図４の各処理は、計算機４１の計算結果がロボットコントローラ１０に送信され、ロボットコントローラ１０がロボットアーム１１、ロボットハンド１２の動作を制御することにより、行われる。

（Ｓ１００）Ｓ１００で、本実施形態における把持装置の作業を開始する。

（Ｓ２００）Ｓ２００では、ロボットコントローラ１０により、初期動作としてロボットアーム１１およびロボットハンド１２を第一の待機状態へと移動させる。図７（ａ）は、本実施形態におけるロボットアーム１１およびロボットハンド１２の第一待機状態を示す図である。第一の待機状態とは、ロボットアーム１１およびロボットハンド１２が作業の開始位置に位置している状態である。第一の待機状態では、ロボットアーム１１およびロボットハンド１２によって、第一のステレオカメラＡ３１１による被把持部材２３の計測は遮られない。

（Ｓ３００）Ｓ３００では、ロボットコントローラ１０が、本実施形態の把持装置による作業が終了したか否かを判定する。作業が終了したと判定された場合、後述するＳ７００に処理を進め、把持装置による作業を終了させる。作業が終了していないと判定された場合、後述するＳ４００に処理を進める。尚、把持作業が終了したか否かの判断は、ユーザー指示により決定してもよい。

（Ｓ４００）Ｓ４００では、第一のステレオカメラＡ３１１により、ワーク（プリント基板２１、接合部材２２、被把持部材２３）を撮像する。

図８は、第一のステレオカメラＡ３１１撮像されたカメラ画像である。図８（ａ）は、第一のステレオカメラＡ３１１の左カメラによる画像である。図８（ｂ）は、第一のステレオカメラＡ３２１の右カメラにより撮像されたカメラ画像である。Ｓ４００では、左カメラ画像もしくは右カメラ画像から、把持装置が作業を行うことが出来る位置にワークが配置されているか否かを検出する。左カメラ画像もしくは右カメラ画像にワークが撮像されていた場合、ワークは作業を行うことが出来る位置に位置していると判定し、Ｓ５００に処理を進める。

（Ｓ５００）Ｓ５００では、本実施形態の把持装置による把持動作を行う。ここでは、ロボットアーム１１により被把持部材２３を把持する。Ｓ５００における処理の詳細は、後述する。Ｓ５００における把持動作の終了後は、Ｓ６００へ処理を進める。

（Ｓ６００）Ｓ６００では、本実施形態の把持装置による接合作業を行う。ここでは、Ｓ５００で把持された被把持部材２３を接合部材２２に接合する。Ｓ６００における接合作業の詳細は、後述する。Ｓ６００における接合作業の終了後は、他の作業を行うためにＳ２００へ進む。

（Ｓ７００）Ｓ７００では、一連の作業が終了後、把持装置の処理を終了させる。

以上が、本実施形態における把持装置の作業フローである。

次に、図４における把持動作Ｓ５００の処理の詳細を説明する。

図５は、把持動作Ｓ５００の詳細な処理を示すフローである。
（Ｓ５１０）Ｓ５１０では、第一のステレオカメラＡ３１１が、ワーク（プリント基板２１、接合部材２２、被把持部材２３）の位置を取得する第一の三次元位置取得工程を行う。Ｓ４００では、ワークが撮像画像中に存在するか否かで、把持装置が作業を行うことが出来る位置にワークが配置されているか否かを検出した。本ステップＳ５１０では把持手段Ａ１０およびワークの位置座標を算出する。接合部材２２、被把持部材２３それぞれの位置の自由度は高く、設計値から正確な位置座標を予想することは困難であるため、本ステップＳ５１０で位置座標を検出する必要がある。本ステップＳ５１０では、図８（ａ）の左カメラ画像と図８（ｂ）の右カメラ画像とから、三角測量の手法などを用いてワークの位置座標を算出する。

（Ｓ５２０）Ｓ５２０では、計算機４１が、左カメラ画像と右カメラ画像とから視差画像を算出し、ワークの三次元位置状態を推定する第一の三次元状態推定工程を行う。本実施形態では、ワークの三次元位置状態とはワークの位置および姿勢を示す。また、視差画像とは、左カメラ画像と右カメラ画像とのズレの大きさを表した画像である。ズレはステレオカメラＡ３１１もしくはステレオカメラＡ３２１から遠いほど大きくなるため、視差画像から遠近情報を得ることが出来る。視差画像におけるズレの大きさは、色分けなどで表現される。得られた視差画像を解析することにより、ワークの三次元位置状態を推定することが出来る。

図９（ａ）は、視差画像に対して、ワークの三次元位置状態を推定するための処理の一つである閾値処理を行った画像を示す図である。図９（ａ）は、閾値処理により、被把持部材２３を抽出した図である。図９（ａ）のような画像を取得するためには、例えば、あらかじめ被把持部材２３が位置する位置範囲を取得しておき、Ｓ５１０で算出された位置情報を用いて、位置範囲外の部分を閾値処理により消去すればよい。

図９（ｂ）は、図８（ａ）、（ｂ）における左カメラ画像もしくは右カメラ画像に対して、ワークの三次元位置状態を推定するための処理の一つであるエッジ処理を行った画像である。エッジ処理を行うことによって、プリント基板２１、接合部材２２、被把持部材２３それぞれのエッジが抽出され、境界を把握しやすくなる。

図９（ｃ）は、被把持部材２３に対してエッジ処理を行った画像である。図９（ｂ）の画像を用いることにより、図９（ａ）の画像から、被把持部材２３のエッジを抽出することが出来る。

図９（ｄ）は、被把持部材２３のエッジを世界座標系に適用した状態を示す図である。

図１０は、図９（ｄ）の状態に対して、三次元位置状態を推定するための処理の一つであるフィッティング処理を行った状態を示す図である。フィッティング処理とは、所定形状のモデルを三次元座標空間上の適切な位置に配置する処理である。本実施形態では、被把持部材２３の形状である矩形形状モデルを、図９（ｄ）のエッジに当てはめるように配置する。フィッティング処理を行うことによって、配置された矩形形状モデルから被把持部材２３の位置姿勢を算出することが出来る。例えば、矩形形状モデルの端点１０２の位置座標から平面の方程式を算出し、平面の方程式から法線ベクトル１０１を算出することにより、被把持部材２３の姿勢を定量化することが出来る。また、図１０の１０３は、被把持部材２３の把持位置を示し、１０４は被把持部材２３の中心軸を示している。把持位置１０３は、被把持部材２３の材質などを考慮して、矩形形状モデル上にあらかじめ設定されている。また、把持位置１０３と中心軸１０４との交点１０５が、把持中心目標位置となる。

（Ｓ５３０）Ｓ５３０では、計算機４１が、Ｓ５２０で推定した被把持部材２３の三次元位置状態を用いて、第一の移動経路設定処理を行う。第一の移動経路とは、被把持部材２３を把持するまでにロボットアーム１１およびロボットハンド１２が移動する移動経路である。

本実施形態では、被把持部材２３の把持を行う場合、フィンガー１３と被把持部材２３が重なり合う位置まで、ロボットアーム１１およびロボットハンド１２を移動させる。

図１１は、フィンガー１３と被把持部材２３とが重なり合う位置まで、ロボットアーム１１およびロボットハンド１２を移動させた状態を示す図である。

図１２は、図１１と同様の位置までロボットアーム１１およびロボットハンド１２を移動させた状態を斜視図で示した図である。図１１および図１２に示すように、フィンガー１３の把持面１４の法線ベクトルと、Ｓ５１０で算出された法線ベクトルとが平行になるようにさせた後、フィンガー１３を閉じれば、被把持部材２３をスムーズに把持することが出来る。

本ステップで生成する第一の移動経路は、図１１もしくは図１２の状態までロボットアーム１１およびロボットハンド１２を移動させる経路である。第一の移動経路を生成するために、第二待機状態、アプローチ状態、把持目標状態を設定する。

図７（ｂ）では第二待機状態、図７（ｃ）ではアプローチ状態、図７（ｄ）では把持目標状態を示している。本実施形態では把持を行う際に、ロボットアーム１１およびロボットハンド１２を、第二の待機状態からアプローチ状態へ移動させ、更に、アプローチ状態から把持目標状態へと移動させる。以下にそれぞれの状態を説明する。

第二待機状態とは、把持を行う際の移動開始位置である。第二待機状態の算出の際には、あらかじめ把持に適した複数の第二待機状態をＬＵＴ（ルックアップテーブル）などに記録しておき、適切な第二待機状態を選択する。第二待機状態の選択の際には、アプローチ状態とそれぞれの第二待機状態をスプライン曲線などで結んだ移動曲線上に特異点が存在するか否かで判定する。特異点上では速度ベクトルがゼロになってしまうため、ロボットアーム１１およびロボットハンド１２の移動に際に、不具合が生じる可能性がある。よって、算出した移動曲線上に特異点が存在しない第二待機状態を選択する。従って、第二の待機状態を選択するためには、まずアプローチ状態を算出する必要がある。

アプローチ状態とは、フィンガー１３が被把持部材１３と重なり合う直前の状態である。

図１３は、本実施形態におけるアプローチ状態を示す図である。本実施形態におけるアプローチ状態は、把持目標状態から、フィンガー１３を所定距離だけ引き抜く方向に移動させた状態である。このときの所定距離は、例えば、図１３に示すように、把持中心基準位置１５からフィンガー１３の先端位置までの距離ｄ_ｆと被把持部材２３の半分の幅すなわち長方形モデルの横エッジと中心軸１０４の距離ｄ_ｍの和にマージンεを加えたものとする。図１３のように所定距離を設定することにより、フィンガー１３と被把持部材２３とが干渉することを防止することが出来る。

把持目標状態とは、被把持部材２３を把持するためのロボットアーム１１およびロボットハンド１２の最終的な位置および姿勢である。最終的な位置姿勢は、前述したように、フィンガー１３の把持面１４の法線ベクトルと、Ｓ５１０で算出された法線ベクトルとが平行になる位置姿勢である。また、把持時には図１０に示した把持位置１０３と図３に示したフィンガー中心基準線１６が一致すべきであり、同様に把持中心目標位置１０５と把持中心基準位置１５も一致しているべきである。これらの条件を満たすために、把持目標状態は、把持手段中心線１７と把持位置１０３が一致し、両フィンガー１３の把持中心基準位置１５を結ぶ直線１８が把持中心目標位置１０５を通るような位置に設定される。

以上の第二待機状態、アプローチ状態、把持目標状態の位置姿勢を、スプライン関数などを用いて結ぶことにより、第一の移動経路を生成する。

図１４は、第一の移動経路を示す概念図である。図１４に示すように、把持部材２３の位置姿勢に応じて、ロボットアーム１１およびロボットハンド１２の移動経路はさまざまに変化する。

（Ｓ５４０）Ｓ５４０では、生成された第一の移動経路に基づき、ロボットコントローラ１０が移動指示を送信することにより、ロボットアーム１１およびロボットハンド１２が移動し、把持動作を行う。Ｓ５４０は、以下、Ｓ５４１〜Ｓ５４４から構成される。

（Ｓ５４１）Ｓ５４１では、初期状態である第一待機状態から第二待機状態へ、ロボットアーム１１およびロボットハンド１２を移動させる。

（Ｓ５４２）Ｓ５４２では、第二待機状態からアプローチ状態へ、ロボットアーム１１およびロボットハンド１２を移動させる。

（Ｓ５４３）Ｓ５４３では、アプローチ状態から把持目標状態へ、ロボットアーム１１およびロボットハンド１２を移動させる。

（Ｓ５４４）Ｓ５４４では、把持目標状態でフィンガー１３を閉じ、把持部材２３をフィンガー１３で把持する。

次に、図４における接合作業Ｓ６００の処理の詳細を説明する。Ｓ６００は、Ｓ６１１〜Ｓ６４７から構成される。Ｓ６００の接合作業における移動経路も、Ｓ５００の把持作業と同様に、複数のロボットアーム１１およびロボットハンド１２状態をつなぐことにより、生成される。

図１５は、本実施形態におけるＳ６００の各ステップにおけるロボットアーム１１およびロボットハンド１２の状態を示す図である。以下にそれぞれの状態の詳細および算出方法を説明する。

図１５（ａ）は、被把持部材２３が接合部材２２に接合される前の状態である接合前状態を示す図である。接合前状態とは、接合の準備のために、被把持部材２３が接合部材２２の手前に位置している状態である。

図１６は、本実施形態における接合前状態を示す図である。

図１６において、２５は接合部材２２の接合部分である。

図１６に示すように、本実施形態の接合前状態は、接合部材２２の接合部分２５手前で、被把持部材２３が接合部材２２と平行を保ちつつ所定の距離δ（例えば、δ＝５ｍｍ）を開けた状態であるとする。ただし、接合部材２２の取り付け公差や被把持部材の把持ずれなどの要因により、接合部材２２と被把持部材２３の位置関係は必ずしも所望の状態にはなっていない。

図１５（ｂ）は、接合部材２２と被把持部材２３を接合するにあたって、被把持部材の一端点の挿入を開始する直前の接合待機状態を示す図である。接合待機状態は、後述する端部挿入状態から挿入方向の逆方向に所定距離だけ移動させた状態である。ただし、接合前状態から接合待機状態を経由して端部挿入状態へとスムーズに移行させるため、接合前状態よりも被把持部材２３が接合部材２２に近づき、かつ被把持部材２３が接合部分２５内部に入り込まない状態であることが望ましい。

図１５（ｃ）は、被把持部材２３を接合部材２２に接合するために、被把持部材２３の一端を接合部材２２に挿入した端部挿入状態を示す図である。被把持部材２３の一端を接合部材２２に挿入する理由は、接合の失敗を防止するためである。例えば、被把持部材２３を接合部材２２に挿入する場合、機械誤差などにより被把持部材２３が接合部材２２に対して平行でないために、挿入が失敗する場合がある。よって、最初に、被把持部材２３の一端を接合部材２２に挿入することにより、挿入が失敗する可能性を低くすることが出来る。

以下に、端部挿入状態の算出方法について説明する。

図１７（ａ）は、被把持部材２３が接合部分２５の近傍に位置している状態を示した図である。図１７（ａ）のような画像は、図１６の状態を撮像することにより、取得することが出来る。取得した画像に対して、Ｓ５２０の処理と同様のフィッティング処理を行うことにより、被把持部材２３、接合部材２２の三次元位置状態を推定し、端部挿入状態を算出することが出来る。

図１７（ｂ）は、図１７（ａ）に対して、Ｓ５２０の場合と同様に、閾値処理およびエッジ処理を行った状態を示す図である。

図１８は、図１７（ｂ）のような画像データに対して、フィッティング処理を行った状態を示す図である。図１８に示すように、接合部材２２に対して矩形形状モデル２００、把持部材２３に対してコの字形状モデル３００を用いている。図１８のように、フィッティング処理を行うことによって、Ｓ５２０の場合と同様に、把持部材２３の位置姿勢を算出することが出来る。また、線分形状モデル２２０を用いてフィッティング処理することにより、接合部分２５の位置姿勢を算出することが出来る。尚、２０１は、矩形形状モデル２００に対する法線ベクトル、３０１は、コの字形状モデル３００に対する法線ベクトル、を示している。３１１、３１２はコの字形状モデル３００の端点、２２１、２２２は線分形状モデル２２の端点、２１１、２１２は矩形形状モデルの端点である。

以下に、図１８の情報に基づいて、端部挿入状態の位置姿勢を算出する方法を説明する。

本実施形態では、把持部材２３の挿入が失敗する可能性を低くするために、把持部材２３を接合部分２５に対して斜めに挿入する。仮に、把持部材２３を接合部分２５に対して平行に挿入すると、わずかな誤差で把持部材２３と接合部材２２とが接触し、挿入が失敗する可能性がある。しかし、斜めに挿入することによって、わずかな誤差が生じていても、把持部材２３と接合部材２２との接触後、把持部材２３と接合部材２２との接触位置がずれて接合部材２２への挿入を成功させることが出来る。

図１９、図２０、図２１は、端部挿入状態の位置姿勢を算出する際の概念図である。図１９に示すように、まず、線分形状モデル２２０に対して、仰角θ_Ｉ（例えばθ_Ｉ＝１５°）を持ち、かつ線分形状２２０を通る平面Ｐ_１を設定する。そして、平面Ｐ_１に平行で、かつ線分２１０を通る平面Ｐ_２を設定する。平面Ｐ_１と平面Ｐ_２とに挟まれていて、かつ両平面に平行な平面Ｐ_Ｉを設定する。Ｐ_１とＰ_Ｉの距離は微小量で、前述した把持手段Ａ１０の持つ倣い機構の許容範囲内の値であるとし、接合部材２３の寸法や被把持部材２３の弾性、ロボットアーム１１の微小移動に関する誤差を考慮して設定する。例えば平面Ｐ_１と平面Ｐ_２の距離が０。５ｍｍであったとき、Ｐ_１とＰ_Ｉの距離を０。１ｍｍと設定する。また、平面Ｐ_Ｉは接合部２５の幅方向に対して線分２２０の両端点２２１および２２２から推定される接合部の幅で切り取られた、幅方向に有限な平面であるとする。

尚、端部挿入動作の動作方向ベクトルｄ_Ｉは、線分２１０もしくは２２０の方向ベクトルに直交し、かつ平面Ｐ_Ｉの法線ベクトル４０１に直交するベクトルで定義される。動作方向ベクトルｄ_Ｉは、前述した接合待機状態を、端部挿入状態から算出する際に用いられる。

ここで、接合部分２５のエッジである線分２２０を通り、接合部材２２の法線ベクトル２０１と同じ方向の法線ベクトル５０１を持つ平面Ｐ_Ｊを考える。平面Ｐ_Ｊは、平面Ｐ_Ｉと同様、接合部２５の幅方向に対して線分２２０の両端点２２１および２２２から推定される接合部の幅で切り取られた、幅方向に有限な平面である。線分２２０から前述の微小挿入量βの距離だけ離れた平面Ｐ_Ｊ上の直線５１０を引き、直線５１０上で平面Ｐ_Ｊの幅方向端から距離ρにある点５２０を、端部挿入状態における被把持部材２３の端点３１２の目標位置とする。ここでのρは微小量で、把持手段の持つ倣い機構の許容範囲内であるとする。端部挿入状態における被把持部材２３の状態は、端点３１２が端点目標位置５２０に一致し、線分３１０が直線５１０の上に乗り、被把持部材２３の法線ベクトル３０１が平面Ｐ_Ｊの法線ベクトル５０１と一致した状態である。

以上の処理により、把持部材２３の端部挿入状態における位置姿勢を算出することが出来る。

図１５（ｄ）は、図１５（ｃ）の端部挿入状態では挿入されていない他端を接合部材２２に挿入した先端部挿入状態を示す図である。

先端部挿入状態は、端部挿入状態と同様に、把持部材２３の端点３１１の端点目標位置を求めることにより、算出することが出来る。

図１５（ｅ）は、図１５（ｃ）の先端部挿入状態から接合方向に挿入することで、被把持部材２３と接合部材２２とが接合された接合状態を示す図である。

図２２は、接合状態を算出する際の概念を示す概念図である。端部挿入状態を算出する際に得られた平面Ｐ_Ｊ上において、線分２２０から距離Ｌ＋ξにある直線６１０を設定する。ここでＬは接続部材２２および被把持部材２３の設計値から与えられる被把持部材２３の接合に必要な押し込み量であり、ξは微小量で、ロボットハンド１２の持つ倣い機構の許容範囲内であるとする。平面Ｐ_Ｊの中心線６０１と直線６１０の交点を、被把持部材２３先端部の中心位置に関する目標位置６２０とする。接合状態における被把持部材２３の状態は、両端点３１１および３１２の中点が目標位置６２０に一致し、線分３１０が直線６１０の上に乗り、被把持部材２３の法線ベクトル３０１が平面Ｐ_Ｊの法線ベクトル５０１と一致した状態である。

以上の処理により、接合状態における把持部材２３の位置姿勢を算出することが出来る。

図１５（ｆ）は、図１５（ｅ）の接合状態からフィンガー１３を開いて、被把持部材２３と干渉しないようにロボットハンド１２を引き抜いた接合終了状態を示す図である。

接合終了状態は、Ｓ５２０におけるアプローチ状態と同様にして算出する。すなわち、接合状態から把持手段中心線１７に沿ってロボットハンド１２を引き抜く方向へと移動させる。このときの移動距離は把持中心基準位置１５からフィンガー１３先端位置の距離ｄ_ｆと被把持部材２３の半分の幅すなわち長方形モデルの横エッジと中心軸１０４の距離ｄ_ｍの和にマージンεを加えたものとする。

図６は、Ｓ６００における各ステップの流れを示す図である。図６を用いて、Ｓ６００における各ステップを説明する。Ｓ６００における各ステップは、ロボットコントローラ１０から指示が送信され、ロボットアーム１１およびロボットハンド１２が駆動することにより、処理される。

（Ｓ６１１）Ｓ６１１では、ロボットコントローラ１０が、ロボットアーム１１およびロボットハンド１２を接合前状態に移動させる。

（Ｓ６１２）Ｓ６１２では、第二のステレオカメラＡ３２１が、ワークを撮像することにより、ワークの位置を取得する三次元位置取得工程を行う。本工程の処理は、Ｓ５１０の処理とほぼ同様である。

（Ｓ６２０）Ｓ６２０では、Ｓ５１０の処理とほぼ同様に、計算機４１が、ステレオカメラＡ３２１の左カメラ画像と右カメラ画像とから視差画像を算出し、ワークの三次元位置状態を推定する第二の三次元状態推定工程を行う。本工程における三次元状態推定は、前述した図１５の各状態の算出の際に説明したワークの三次元位置および姿勢の算出である。

（Ｓ６３０）Ｓ６３０では、計算機４１が、接合作業の際に用いる第二の経路を生成する。第二の経路生成の際には、前述した図１５の各状態を用いる。図１５の各状態をスプライン関数などを用いて結び、第二の経路を生成する。ロボットアーム１１およびロボットハンド１２を第二の経路に従い移動させれば、接合作業を行うことが出来る。

Ｓ６４０以降は、把持部材２３を接合部材２２に接合する工程である。
接合作業工程では、第二の経路に従い、Ｓ６４１（接合待機状態）、Ｓ６４２（端部挿入状態）Ｓ６４３（先端部挿入状態）、Ｓ６４４（接合状態）、の間を移動する。

（Ｓ６４５）Ｓ６４５では、ロボットコントローラ１０がフィンガー１３を把持部材２３に対して開放方向に駆動させ、把持終了動作させる。

（Ｓ６４６）Ｓ６４６では、ロボットコントローラ１０が、ロボットアーム１１およびロボットハンド１２を接合終了状態に移動させる。

（Ｓ６４７）Ｓ６４７では、次の把持動作のために、ロボットコントローラ１０がロボットアーム１１およびロボットハンド１２を第二の待機状態に移動させる。このときの第二の待機状態は、記憶されたＬＵＴもしくは識別関数を呼び出すことで、計算機４１が接合終了状態に対応する第二待機状態を選択する。

以上のＳ２００からＳ６００までの処理後、再びＳ２００へ処理を戻し、作業待ち状態となる。

以上の処理を行うことによって、フレキシブルケーブルとコネクタを嵌合させるような複雑な把持動作作業を円滑に行うことが出来る。尚、図４における各処理は、把持装置を制御する制御プログラムが格納された記憶装置、ＣＰＵなどから構成されるコンピュータで代替してもよい。

（第二の実施形態）
図２３は、本実施形態における機能構成を示す図である。図２３から分かるように、基本的な構成は第一の実施形態の図１と同様である。第一の実施形態と異なる点は、３Ｄ−ＣＡＤデータＡ５０を備えることである。３Ｄ−ＣＡＤデータＡ５０には、把持部材２３、接合部材２２などの正確な外形形状データが格納されている。これらのデータを矩形形状モデルやコの字形状モデルなどのパラメトリックなモデルデータの替わりに用いることによって、本実施形態では、より正確なフィッティング処理を行うことが出来る。正確なフィッティング処理を行うことにより、被把持部材２３、接合部材２２の位置姿勢の推定精度が向上し、ロボットハンド１２の倣い機構に依存する設定値β、γおよびρの値をより小さな値に設定することができ、作業の正確性が向上する。

（第三の実施形態）
図２４は、本実施形態における把持装置の構成を示す図である。図２４から分かるように、基本的な構成は第一の実施形態における図２と同様である。第一の実施形態と異なる点は、第一のステレオカメラＡ３１１と第二のステレオカメラＡ３２１とを別の手段に置き換えていることである。第一のステレオカメラＡ３１１の替わりに、第一のカメラＡ３１３と第一の投光器Ａ３１４を備えている。また、第二のステレオカメラＡ３２１の替わりに、第二のカメラＡ３２３と第二の投光器Ａ３２４とを備えている。投光器としては、２本以上のスリット光を照射することができるマルチスリット光型であることが望ましい。また、ガルバノミラーもしくはポリゴンミラーによる走査型であってもよい。

本実施形態では、撮像を行う際に、まずカメラ光軸から所定角度をもって、投光器からスリット光を投光する。そして、カメラが撮像した画像からスリット光による照射位置を検出し、三角測量の原理を用いて撮像対象の奥行き情報を取得することが出来る。この手法は、一般的に光切断法と呼ばれている。本実施形態では、ステレオカメラの替わりに、投光器およびカメラを用いた光切断法によって、撮像対象の三次元位置および姿勢を検出することが出来る。

（第四の実施形態）
図２５は、本実施形態における把持装置の構成を示す図である。図２５から分かるように、基本的な構成は第一の実施形態における図２と同様である。第一の実施形態と異なる点は、第一のステレオカメラＡ３１１と第二のステレオカメラＡ３２１とを別の手段に置き換えていることである。第一のステレオカメラＡ３１１の替わりに、第一のカメラＡ３１５と第一のプロジェクタＡ３１５とを備えている。また、第二のステレオカメラＡ３２１の替わりに、第二のカメラＡ３２５と第二のプロジェクタＡ３２４とを備えている。本実施形態では、ステレオカメラの替わりに、カメラとプロジェクタを用いることにより、空間コード化法による撮像対象の位置計測が可能である。空間コード化法とは、まず、プロジェクタから撮像対象に対してコード化パターンを投影する。そして、カメラが撮像した画像からコード化パターンを検出し、三角測量の原理を用いて撮像対象の奥行き情報を取得する方法である。

本実施形態では、ステレオカメラの替わりに、プロジェクタおよびカメラを用いることよって、撮像対象の三次元位置および姿勢を検出することが出来る。

（第五の実施形態）
図２６は、本実施形態における把持装置の構成を示す図である。図２６から分かるように、基本的な構成は第一の実施形態における図２と同様である。第一の実施形態と異なる点は、第二のステレオカメラＡ３２１とを別の手段に置き換えていることである。第二のステレオカメラＡ３２１の替わりに、第一のカメラＡ３２３と投光器Ａ３２４とを備えている。本実施形態では、把持時における撮像にはステレオカメラを用いて、把持部材２３の接合時における撮像に対しては、カメラと投光器とを用いている。カメラと投光器によって撮像された画像に対する処理は、第三の実施形態と同様である。

尚、本実施形態の変形例として、第一のステレオカメラＡ３１１の替わりに、カメラと投光器を用いても良い。また、カメラと投光器の替わりに、プロジェクタとカメラを用いて、第四の実施形態と同様の方法により、撮像対象の位置および姿勢を検出してもよい。

（第六の実施形態）
図２８は、本実施形態における把持装置の構成を示す図である。図２８から分かるように、基本的な構成は第一の実施形態における図２と同様である。第一の実施形態と異なる点は、第二のステレオカメラＡ３２１を備えていないことである。本実施形態では、第一のステレオカメラＡ３０１を、把持および接合時に被把持部材２３を撮像可能な位置に配置する。そして、第一のステレオカメラＡ３０１で、把持および接合時の撮像を行う。

図２７は、本実施形態における把持装置の機能構成を示す図である。図２７から分かるように、基本的な構成は第一の実施形態における図１と同様である。第一の実施形態と異なる点は、第一の三次元位置取得手段Ａ３１０および第二の三次元位置取得手段Ａ３２０の機能を、三次元位置取得手段Ａ３００が担っていることである。三次元位置取得手段Ａ３００は、図２８における第一のステレオカメラＡ３０１に対応している。

以上のように、第一のステレオカメラＡ３０１を適切な位置に配置することにより、第二のステレオカメラを配置せずに把持および接合動作が可能であり、装置の製造コストを削減することが出来る。

第一の実施形態における把持装置の機能構成を示す図。 第一の実施形態における把持装置の具体的な構成を示す図。 ロボットハンド１２の先端部の詳細な構成を示す図。 第一の実施形態における把持装置による把持作業処理フローを示す図。 把持動作Ｓ５００の詳細な処理フローを示す図。 接合作業Ｓ６００の各ステップの流れを示す図。 第一の実施形態における把持作業中の把持手段の状態を示す図。 第一のステレオカメラにより撮像されたカメラ画像を示す図。 視差画像に対して閾値処理を行った画像を示す図。 閾値処理を行った画像に対してフィッティング処理を行った画像を示す図。 フィンガーと被把持部材とが重なりあっている状態を示す図。 フィンガーと被把持部材とが重なりあっている状態を斜視図で示す図。 第一の実施形態におけるアプローチ状態と把持目標状態の関係を説明した図。 第一の実施形態における第一の移動経路を示す概念図。 第一の実施形態におけるＳ６００の各ステップにおける把持手段の状態を示す図。 第一の実施形態における接合前状態を示す図。 被把持部材が接合部分の近傍に位置している状態を示す図。 フィッティング処理を行った状態を示す図。 端部挿入状態の位置姿勢を算出する際の概念図。 端部挿入状態の位置姿勢を算出する際の概念図。 端部挿入状態の位置姿勢を算出する際の概念図。 接合状態を算出する際の概念を示す概念図。 第二の実施形態における把持装置の機能構成を示す図。 第三の実施形態における把持装置の構成を示す図。 第四の実施形態における把持装置の構成を示す図。 第五の実施形態における把持装置の構成を示す図。 第六の実施形態における把持装置の機能構成を示す図。 第六の実施形態における把持装置の構成を示す図。

Claims (14)

1. 把持手段に把持される被把持部材の位置および姿勢に関する情報を取得する情報取得手段と、
   前記取得された情報に基づき前記被把持部材の把持に適した状態である把持目標状態を取得し、当該把持目標状態に向かうように前記把持手段の移動経路を設定する移動経路設定手段と、
   前記設定された移動経路に沿って、前記把持手段を移動させ、前記被把持部材を把持させる制御手段とを有することを特徴とする把持装置。
2. 前記移動経路設定手段は、
   前記被把持部材が前記接合部材に接合された状態とは異なる角度で、前記把持手段により被把持部材の端部が前記接合部材に挿入されている挿入状態を取得し、
   前記挿入状態を経由するように、前記第一の移動経路を設定することを特徴とすることを特徴とする請求項１に記載の把持装置。
3. 前記制御手段は、前記設定された移動経路に基づき、前記被把持部材を把持した前記把持手段を移動させ、前記被把持部材を接合部材に接合させることを特徴とする請求項１もしくは２のいずれかに記載の把持装置。
4. 前記情報取得手段は、前記把持手段が前記被把持部材を把持する際の前記被把持部材の位置および姿勢に関する第一の情報と、
   前記把持手段が前記被把持部材を前記接合部材に接合する際の前記被把持部材の位置および姿勢に関する第二の情報とを取得し、
   前記移動経路設定手段は、前記第一の情報に基づき、前記把持手段が前記被把持部材を把持する際の前記把持手段の第一の移動経路を設定し、
   前記第二の情報に基づき、前記第二の情報に基づき、前記把持手段が前記被把持手段を前記接合部材に接合する際の前記把持手段の第二の経路を設定し、
   前記制御手段は、前記設定された第一の移動経路に基づき、前記把持手段を移動させ、前記被把持部材を把持させ、
   前記設定された第二の移動経路に基づき、前記被把持部材を把持した前記把持手段を移動させ、前記被把持部材を接合部材に接合させることを特徴とする請求項３に記載の把持装置。
5. 前記移動経路設定手段は、前記把持目標状態につながるような曲線を算出し、当該曲線を前記第一の移動経路として設定することを特徴とする請求項１乃至４のいずれかに記載の把持装置。
6. 前記情報取得手段は、前記被把持部材の位置および姿勢をパラメトリックなモデルデータで取得することを特徴とする請求項１乃至５のいずれかに記載の把持装置。
7. 前記パラメトリックなモデルデータは、矩形形状のモデルデータであることを特徴とする請求項６に記載の把持装置。
8. 前記移動経路設定手段は、前記把持手段の複数の把持に必要な姿勢状態を設定し、
   当該複数の姿勢状態を経由するように、前記移動経路を設定することを特徴とする請求項１に記載の把持装置。
9. 前記パラメトリックなモデルデータと、ＣＡＤデータから得られた前記被把持部材の外形を元にしたモデルであることを特徴とする請求項６に記載の把持装置。
10. 前記情報取得手段は、複数のカメラを有することを特徴とする請求項１乃至９のいずれかに記載の把持装置。
11. 前記情報取得手段は、カメラと投光器とを有することを特徴とする請求項１乃至９のいずれかに記載の把持装置。
12. 前記情報取得手段は、カメラとプロジェクタとを有することを特徴とする請求項１乃至９のいずれかに記載の把持装置。
13. 情報取得手段が、把持手段に把持される被把持部材の位置および姿勢に関する情報を取得する情報取得工程と、
    移動経路設定手段が、前記取得された情報に基づき前記被把持部材の把持に適した状態である把持目標状態を取得し、当該把持目標状態に向かうように前記把持手段の移動経路を設定する移動経路設定工程と、
    制御手段が、前記設定された移動経路に沿って、前記把持手段を移動させ、前記被把持部材を把持させる制御工程とを有することを特徴とする把持装置制御方法。
14. コンピュータを、
    把持手段に把持される被把持部材の位置および姿勢に関する情報を取得する情報取得手段と、
    前記取得された情報に基づき前記被把持部材の把持に適した状態である把持目標状態を取得し、当該把持目標状態に向かうように前記把持手段の移動経路を設定する移動経路設定手段と、
    前記設定された移動経路に沿って、前記把持手段を移動させ、前記被把持部材を把持させる制御手段として機能させるためのコンピュータプログラム。

Images