JP3931933B2 - アームの制御装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、遠隔操縦される物品の端末処理機の制御装置に関するもので、特に、制御点を変更する手段を有する制御装置に関する。
【０００２】
【従来の技術】
産業用物品の端末処理機が直交座標系で動作する場合には設定された制御点を基準にして動作する。例えば、図８のように、アーム先端の姿勢を軸Ｘ周りに姿勢１から姿勢２に変更しようとすると、アーム先端の姿勢が設定された制御点を回転中心として変化するように物品の端末処理機アームの各関節の目標角度が計算される。
また、産業用物品の端末処理機の先端には様々なツールが取り付けられるようになっていて、取り付けられたツールに応じて制御点の設定を変更する。ツール毎の制御点は通常ツールファイルとして物品の端末処理機制御装置の記憶装置に記憶されており、ジョブと呼ばれる作業プログラム（以下、ジョブと記す）を作成して教示する際に、同時にその時点での制御点情報が動作命令に記憶されるようになっている。
また、遠隔操縦型物品の端末処理機ではジョイスティックやマスタアームで手動操作する場合があるが、その場合は最後に動かしたジョブで設定されていた制御点を基準に動作するようになっている。特に遠隔操縦型物品の端末処理機を人間がジョイスティックやマスタアームなどで手動で操作する場合は、適切な制御点を設定しておくことが作業性を向上させることになる。
ここで、危険な場所においてケーブル等の設備の補修を行う遠隔操縦物品の端末処理機を例にとって説明する。図９は遠隔操縦ケーブル端末処理機システムの基本構成図を示している。このケーブル端末処理機には、７自由度双腕物品の端末処理機アームと数台の監視カメラが搭載されている。このケーブル端末処理機が作業する場所とは離れた安全な場所に数台のＴＶモニタとジョイスティック、各種スイッチが取り付けられた操縦室がある。オペレータは操縦室にて監視カメラが捉えた映像をＴＶモニタで作業状況を見ながらジョイスティックで手動操作したり、あらかじめ教示しておいた動作を再現させたりすることで作業を進める。また、ケーブル端末処理機システムには自動工具交換装置（ＡＴＣ）や自動治具授受装置（ＡＭＣ）が装備されており、ツール交換や治具取り出し動作等は、ケーブル端末処理機アームの自動運転によってすべて自動で行われる。さらに３次元レーザ計測器が装備されており、作業点までの距離を３次元的に測定することができ、その結果を基に作業点までの自動アプローチ等が可能となっている。
【０００３】
これらケーブル端末処理機システムの動作を「ケーブル切断処理作業」への適用例で具体的に説明する。図１０のように左アームはグリッパによって切断されたケーブルを把持している。右アームはツールをグリッパに交換し、その後そのグリッパによって「ケーブル端末処理治具」と呼ばれる治具をつかんで切断したケーブルの先に挿入する。このとき、右アームのツールの交換と治具（ケーブル端末処理治具）取り出しの動作はあらかじめ教示されており、自動的にケーブル端末処理機アームの動作が行われる。
その後、レーザー計測器によりケーブルの位置を計測後、その結果を元にケーブルの手前付近まで右アームの先端を自動的にアプローチさせる。ここから手動操作に切り替わり、オペレータがジョイスティックを操作してケーブル端末処理治具を切断されたケーブルへ挿入し、その後、グリッパを開いてケーブル端末処理治具を離し、右アームをケーブルから退避させるという作業を行う。この時、左アームは切断されたケーブルを把持したままであり、切断されたケーブルの先を、ケーブル端末処理治具を挿入しやすいような姿勢にしておく。
この場合、左右アームの制御点はツールを交換する前はケーブル端末処理機アーム先端のフランジ部、あるいは今まで取り付けられていたツールに応じた位置にある。そして、グリッパが取り付けられた時、グリッパの把持部中心に制御点が変更される。この制御点は次にツールが交換されるまで変更されない。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記従来技術においては、制御点はツール交換後次にツールを交換するまで不変であり、実際に治具をつかんで手動で動作させるときに、治具に応じた制御点ではないので操作しづらいという問題があった。
この問題を「ケーブル切断処理作業」で説明すると次のようになる。すなわち、図１１のように、ツール交換後の制御点はグリッパの把持中心点に設定されており、オペレータはジョイスティック操作によって自動アプローチ位置から挿入位置まで手動で右アームの姿勢を変更する。ここで、ケーブル端末処理機アームの制御点が図１１（ａ）の作業点のように、ケーブル端末処理治具の先端の作業点に設定されていれば、オペレータはケーブル端末処理治具を図のように作業点を中心に姿勢変化させることができるので、ケーブルの芯の延長線上にケーブル端末処理治具がくることとなり、スムーズに治具をケーブルに挿入することができる。
しかしながら、従来のものは図１１（ｂ）のようにグリッパの把持部中心にケーブル端末処理機アームの制御点が設定されているので、ジョイスティック操作により、回転指令を出力すると、図のように挿入位置がずれてしまい、ケーブル端末処理治具を挿入位置にもってくるにはさらにジョイスティック操作により並進移動指令を数回出力せねばならず、オペレータにとってはジョイスティックの操作回数が多くなって負担となり、作業時間も増加して作業効率が悪いという問題があった。
また、図１２のように左アームで把持したケーブルをケーブル端末処理治具が挿入しやすいような姿勢に持ってくる場合を考えると、制御点が図１２（ａ）のようにケーブル固定治具の根元にあれば、左アームはこの制御点を中心に回転するので、切断されたケーブルの姿勢を姿勢１から姿勢２へスムーズに変化させることが可能となる。
しかしながら、従来のものは、左アームの制御点はグリッパ把持中心に設定されているので、オペレータがジョイスティックによって回転指令だけを与えると、図１２（ｂ）のように左アームはこのグリッパ把持中心部を中心にして回転するので、ケーブルやケーブル端末処理機アームに無理な力がかかってしまい、ケーブルが変形してしまうこともあるために回転指令と並進移動指令を交互にあるいは同時に与えながらケーブルの先を適切な姿勢にするような操作を行っており、このような操作自体難しく、操作の時間もかかるため、作業性の悪化の一因となっているという問題があった。
そこで、本発明は、ツールや治具、あるいは３次元位置計測器のデータに応じて、常に作業に適した制御点を与えておくことで作業性を向上させる遠隔操縦される物品の端末処理機の制御装置を提供することを目的としている。
【０００５】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載の発明は、ロボットのアームの先端にツールが取り付けられ、当該ツールが治具を保持して成るロボットのアームの制御装置であって、少なくとも前記アームの関節の目標角度を計算する制御手段と、前記ツールを交換するツール交換手段と、前記治具を交換する治具交換手段とを備え、前記ツールと前記治具上に設定された前記アームの制御点の位置および姿勢が、前記アームの先端座標から見た位置および姿勢から算出され、算出された前記位置および前記姿勢に基づいて前記制御手段で前記アームの目標角度が計算され、前記ツールにより保持される治具を操作するアームの制御装置において、前記ツールが保持している前記治具を離したときには、前記アームの制御点を前記ツールの制御点に戻すように演算され制御されることを特徴としている。
【０００６】
上記構成によれば、遠隔操縦される物品の端末処理機において、物品の端末処理機先端に取付けられたツールの制御点データと、ツールによって把持している治具の制御点データからツールと治具をまとめて１つのエンドエフェクタとして新たに制御点データを計算する手段は、ツールの制御点データの同次変換行列と物品の端末処理機座標原点から見たフランジ座標系の現在の位置および姿勢の同次変換行列から、物品の端末処理機座標原点から見たツール先端の座標系の現在の位置および姿勢を次の（式１）より求められる。
【数１】

次に、今、遠隔操作のジョイスティックから出力された並進移動量をｄｘ、ｄｙ、ｄｚ、回転移動量をｒｘ、ｒｙ、ｒｚ，とすると、この時の回転移動量を３×３の行列Ｒとして計算し、（１）式で求めたツール先端の位置および姿勢を表す同次変換行列の上３×３の小行列をＥとすれば、回転後の姿勢Ｍは（式２）となる。
Ｍ＝Ｒ・Ｅ ・・・（式２）
更に、現在の制御点の位置をｘ、ｙ、ｚとすると、並進移動後の位置は（式３）となる。
ｘ＝ｘ＋ｄｘ、
ｙ＝ｙ＋ｄｙ、
ｚ＝ｚ＋ｄｚ、 ・・・（式３）
そして、同次変換行列としてまとめて表すと、ツールに設定された制御点の移動後の位置および姿勢行列は（式４）となる。
【数２】

この（式４）で示されるツールに設定された制御点の位置および姿勢行列から、逆運動学演算による解を求め各関節の動作量に変換することによって、ケーブル端末処理機アームを動作させる。以上がツール単独の場合の基本的なアームの運動制御である。
次に、それにツールで治具を保持する動作が加わると、治具に対する制御点データを、治具がツールによって把持されるべき点からの位置および／又は姿勢で表し、ツールの制御点が治具を把持する位置に設定されていれば、フランジから見たツール＋治具の新しい制御点の位置および姿勢は（式５）となる。
【数３】

この（式５）で新たに求めたツール＋治具の制御点の位置および姿勢を、（式１）のツールの制御点データの代わりに用いて、（式１）〜（式４）の演算を行えば、ジョイスティックによる並進・回転移動量が逆運動学演算の結果により関節動作量に変換され、動作指令生成手段により新しい制御点に応じたアームの動作を正確に行うことができる。
あるいは、計測した３次元位置データを現在の制御点から見た３次元計測位置と現在の制御点データから、新たな制御点データを生成して、この新たな制御点データを用いて（式１）〜（式４）の演算を行えば、３次元位置計測により得られた位置を制御点として動作指令を生成し、新しい制御点に応じてアームを正確に動作させることができる。
【０００７】
【発明の実施の形態】
以下、本発明の第１の実施の形態について図を参照して説明する。
図１〜図５は第１の実施の形態に係る図である。図１は本発明の実施の形態に係る遠隔操縦ケーブル端末処理機の制御装置のブロック図であり、図２は図１に示すジョブの中のツール交換用ジョブの１例を示す図である。また、図３は図１に示すケーブル端末処理機のケーブル端末処理機アームに配置される座標系を表す図である。図４は図１に示すジョブの治具取出し用ジョブの１例を示す図であり、図５は図３に示すケーブル端末処理機アームに把持される治具に配置される座標系を表す図である。
図１において、フラッシュメモリ２にはツールや治具に応じた制御点データ８を記憶させており、また、そのフラッシュメモリ２にはケーブル端末処理機４の先端の制御点データに関する計算を行ったり、ケーブル端末処理機アームの動作を行うコマンドが記述されたジョブ７が記憶されている。中央演算処理装置（ＣＰＵ）１では、ジョブ７を解釈・実行するジョブインタープリタ９や、計算された制御点データ８とジョイスティック３の操作量やジョブの中の移動コマンドに基づいてケーブル端末処理機４へ動作指令を出力する、動作指令部１０を備えている。 又、作業点の３次元位置座標を計測する３次元レーザー計測器５とその制御装置６を備えており、その位置データがジョブインタープリタ９へ取り込まれる。作業点の３次元位置を計測する手段としてはレーザー計測器に限定されるものでなく、ステレオカメラや超音波等の非接触センサ、または力センサによる接触した際の力の作用点、もう一方のアーム先端位置を用いるなどさまざまな手段が考えられる。以上の本発明の遠隔操縦ケーブル端末処理機の制御装置は、外形は図９の従来例と同一構成である遠隔操縦ケーブル端末処理機を制御してケーブルの切断作業等を改善するものである。
【０００８】
つぎに動作について説明する。
先ず、フラッシュメモリ２に記憶されたジョブ７が呼び出されると、ジョブインタープリタ９によって解釈、実行される。ジョブ７の命令には図２のように、移動命令や演算命令、ツール選択命令などが含まれる。図２はツール交換用のジョブの例である。
このジョブでは、
▲１▼ 自動工具交換装置（ＡＴＣ）へケーブル端末処理機アームを移動（Ｓ２１）し、
▲２▼ ツールを交換して（Ｓ２２）、
▲３▼ ケーブル端末処理機アームを元の位置へ戻し（Ｓ２３）、
▲４▼ 交換したツールの制御点データを設定（Ｓ２４，Ｓ２５）している。
ジョブで制御点データ８を設定した後、動作指令部１０はその制御点データ８とジョイスティック操作量に基づいて動作指令を生成する。
制御点データ８は３×３の姿勢行列と、３次元の位置ベクトルで記述されており、その行列は図３のようにケーブル端末処理機アーム先端のフランジに固定されたフランジ座標系ｆからみた制御点（ツール先端座標系ｅ）の位置および姿勢を表している。その制御点データの同次変換行列と、ケーブル端末処理機座標原点ｏから見たフランジ座標系ｆの現在の位置および姿勢の同次変換行列より、ケーブル端末処理機座標原点ｏから見たツール先端座標系ｅの現在の位置および姿勢は（式１）となる。
【数４】

また、ジョイスティックから出力された並進移動量をｄｘ, ｄｙ, ｄｚ、回転移動量をｒｘ, ｒｙ, ｒｚとする。このとき、回転移動量を３×３の行列Ｒとして計算し、現在のツール先端の位置および姿勢を表す同次変換行列の上３×３の小行列をＥとすると、回転後の姿勢Ｍは（式２）となる。
Ｍ＝Ｒ・Ｅ ・・・（式２）
さらに、現在の制御点の位置をｘ、ｙ、ｚとすると、並進移動後の位置は、
ｘ＝ｘ＋ｄｘ、
ｙ＝ｙ＋ｄｙ、
ｚ＝ｚ＋ｄｚ ・・・（式３）
となり、同次変換行列としてまとめると、ツールに設定された制御点の移動後の位置および姿勢行列は（式４）となる。
【数５】

従って、逆運動学演算により各関節の動作量に変換することで、設定された制御点を基準にしてケーブル端末処理機アームが動作することになる。
次に治具を把持した時の作用を説明する。図４は治具取り出し用のジョブの例を示している。
▲１▼ ここではまず、自動治具授受装置（ＡＭＣ）へケーブル端末処理機アームを移動させた後（Ｓ４３）、
▲２▼ 現在装着しているツールで対象となる治具を受け取り（Ｓ４４）、
▲３▼ また元の位置へ戻る（Ｓ４５）。
▲４▼ 移動が終わった後にツールに対する制御点データ（Ｓ４１）と治具に対する制御点データ（Ｓ４２）から新たな制御点データを作成し（Ｓ４６）、
▲５▼ この制御点データをこれから用いる制御点データとして設定する（Ｓ４７，Ｓ４８）。
ここで、治具に対する制御点データは図５のようにその治具がツールによって把持されるべき点からの位置および姿勢で表されている。更に、ツールの制御点が治具を把持する位置に設定されていれば、ケーブル端末処理機アームのフランジｆからみた治具の制御点ｍの位置および姿勢は、ツールの制御点データと、治具の制御点データより、（式５）となる。
【数６】

この（式５）で計算した治具の制御点ｍの位置および姿勢を、ツールの制御点データの代わりに用いて、（式１）〜（式４）のように計算すれば、ケーブル端末処理機アームが治具を把持した後、治具に設定された制御点に応じてアームが動作する。
又、把持している治具を離したときには、グリッパ等の開閉動作命令と共に、制御点をツールの制御点に戻す計算を行えば、再びケーブル端末処理機アームに装着しているツールに応じた制御点を設定することが可能となる。これによって、図１１、図１２に示したような場合の操作も改善され作業効率が向上する。
【０００９】
次に、本発明の第２の実施の形態について図を参照して説明する。
図６は本発明の第２の実施の形態に係る遠隔操縦ケーブル端末処理機制御装置のジョブの１例を示す図である。この第２の実施の形態は、図１に示したような３次元レーザ計測器５で測定した３次元位置データから制御点データを直接設定するもので、その場合のジョブの例を第２の実施の形態として図６に示している。
図６においては、
▲１▼ 計測した３次元位置データ（Ｓ６３）とケーブル端末処理機アームの現在の制御点位置および姿勢データ（Ｓ６２）から新たな制御点データを作成する（Ｓ６５）。
▲２▼ ここで、計測された３次元位置データと、現在のケーブル端末処理機アームの制御点の位置および姿勢から、現在の制御点から見た３次元計測位置（Ｓ６４）を求めると（式６）となる。
【数７】

この３次元計測位置と現在の制御点データから、あらたな制御点データが（式７）により求められる。
【数８】

が生成される。
▲３▼ この制御点データをこれから用いる制御点として設定して（Ｓ６６，Ｓ６７）、現在の制御点データの代わりに（式１）〜（式４）の計算を行えば、ケーブル端末処理機アームは３次元レーザ計測器５で計測した位置を制御点として、動作指令が生成される。
又、ここでは３次元レーザ計測器５で計測した位置を制御点としたが、計測した位置からあるオフセットを持たせた位置を制御点としてもよい。あるいは３次元計測した位置でなく、直接キーボードなどから適当な座標系における位置を入力してもよく、外部記憶装置に記憶しておいたものを呼び出して用いてもよい。
さらには、ケーブル端末処理機アーム手首部に装着した力センサによって作業対象物との接触時に計測された力・モーメントの値から算出した３次元位置座標を用いてもよい。あるいはもう片方のアーム先端の現在位置を制御点としてもよい。いずれにしても（式５）、または（式６）〜（式７）の計算によって制御点データが計算され、（式１）〜（式４）によって動作指令が生成されることになる。
【００１０】
以上は遠隔操縦ケーブル端末処理機においてジョイスティックやマスタアームなどによる手動操作時について論じてきたが、ケーブル端末処理機単体のティーチングプレイバック時にもこの制御点変更手段が有効になる。例えば、図７（ａ）のように、ケーブル端末処理機アームの手先に装着されたグリッパによって切断されたケーブルが把持されており、このケーブルを姿勢１から姿勢２へ上方に回避させる動作がある。このとき制御点はグリッパによって把持されるはずの点から距離Ｌだけオフセットした点に設定され、この制御点データがジョブのなかの移動命令とともに記憶されている。しかしながら、図７（ｂ）のようにケーブルを把持する場所が毎回同じとは限らず、作業毎に把持する場所が異なりケーブル根元までの距離も違うので、そのままプレイバック動作を行うと、ケーブルやケーブル端末処理機アームに無理な力がかかってしまうことがある。この場合に、本実施の形態の適用によって、ケーブル根元の点の位置を３次元計測し、その位置データに基づいて、（式６）、（式７）により制御点データを算出し、この制御点データによってツール選択コマンド（Ｓ６７など）を移動命令の前に記述しておく。この記述を有効にして、移動命令に含まれる制御点データを無効にしておくと、直前に発行されたツール選択コマンドで設定された制御点を基準にして動作することになり、安全が保証され作業効率が向上する。
また、以上はケーブルの端末を遠隔操作で処理する例について述べてきたが、これは１例であり、本発明はケーブルの端末を遠隔操作で処理するものに限定されるものではなく、例えば部品の端末どうしを溶接したり固着したりする場合や、鋳物部品の周辺部のバリ取りなどのロボット作業にも当然活用できるものである。
【００１１】
【発明の効果】
以上説明したように、本発明によれば、ケーブル端末処理機アームの動作の基準となる制御点を、治具を把持したとき、あるいは、３次元位置計測器で計測したときなどに、所望の新たな制御点に変更されるので、オペレータがジョイスティックなどで手動で動作させるときに、作業がしやすく、作業時間も短縮され、オペレータの負担が軽減されるという効果がある。
更に、治具に対する制御点データは治具が把持される点からの相対的なものとなるので、ツールと治具の組み合わせによる制御点データの増加を防ぐことができるという効果もある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係る遠隔操縦ケーブル端末処理機の制御装置のブロック図である
【図２】図１に示すジョブの中のツール交換用ジョブの１例を示す図である。
【図３】図１に示すケーブル端末処理機のケーブル端末処理機アームに配置される座標系を表す図である。
【図４】図１に示すジョブの中の治具取出し用ジョブの１例を示す図である。
【図５】図３に示すケーブル端末処理機アームに把持される治具に配置される座標系（制御点）を表す図である。
【図６】本発明の第２の実施の形態に係る遠隔操縦ケーブル端末処理機の制御装置のジョブの１例を示す図である。
【図７】図６に示すジョブによるプレイバック動作の１を示す図である。
【図８】従来のケーブル端末処理機の制御点を基準にしたアームの動作を示した図である。
【図９】従来の遠隔操縦ケーブル端末処理機の外形図である。
【図１０】図９に示すケーブル端末処理機によるケーブル切断処理作業を示す図である。
【図１１】図１０に示す切断作業において右アームが把持したケーブル端末処理治具の動きを示す図である。
【図１２】図１０に示す切断作業において左アームが把持したケーブルの動きを示す図である。
【符号の説明】
１ 中央演算処理装置
２ フラッシュメモリ
３ ジョイスティック
４ ケーブル端末処理機
５ ３次元レーザ計測器
６ レーザ計測制御装置
７ ジョブ
８ 制御点データ
９ ジョブインタープリタ
１０ 動作指令部

Claims (1)

1. ロボットのアームの先端にツールが取り付けられ、当該ツールが治具を保持して成るロボットのアームの制御装置であって、少なくとも前記アームの関節の目標角度を計算する制御手段と、前記ツールを交換するツール交換手段と、前記治具を交換する治具交換手段とを備え、前記ツールと前記治具上に設定された前記アームの制御点の位置および姿勢が、前記アームの先端座標から見た位置および姿勢から算出され、算出された前記位置および前記姿勢に基づいて前記制御手段で前記アームの目標角度が計算され、前記ツールにより保持される治具を操作するアームの制御装置において、
   前記ツールが保持している前記治具を離したときには、前記アームの制御点を前記ツールの制御点に戻すように演算され制御されることを特徴とするアームの制御装置。

Images