JP2007319970A

JP2007319970A - 産業用ロボットのツールの位置・姿勢制御方法及び制御システム

Info

Publication number: JP2007319970A
Application number: JP2006152202A
Authority: JP
Inventors: Yasuhiro Kamishina; 泰宏神品; Shigehiro Sawada; 成弘澤田; Mitsuharu Hamahata; 光晴浜畑
Original assignee: Daihen Corp; 株式会社ダイヘン; Nachi Fujikoshi Corp; 株式会社不二越
Priority date: 2006-05-31
Filing date: 2006-05-31
Publication date: 2007-12-13
Anticipated expiration: 2026-05-31
Also published as: JP5144035B2

Abstract

【課題】産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行うことなく、簡易に産業用ロボットのツールを手動制御する方法及び制御システムを提供すること。
【解決手段】産業用ロボット１の位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボット１に備えられたツール１１の並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行うことを解決手段とする。
【選択図】図１

Description

本発明は、産業用ロボットのツールの位置・姿勢制御方法及び制御システムに関し、詳しくは、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する方法、及び、当該方法に適する位置・姿勢制御システムに関する。

一般に産業用ロボットの手動制御は、多関節を用いた手動制御（以下、「関節手動制御」という。）と直交座標系を用いた手動制御（以下、「直角手動制御」という。）の２種に大別される。関節手動制御では、多関節を有する所謂多関節型ロボットに備えられた各関節を個別に回転動作させる。一方、直角手動制御では、産業用ロボットが把持するツール（ロボットの先端に取り付けられ、ワーク（被溶接物等）に作用させる道具）を、予め決められた、手動制御の際に基準とする座標系（以下、「基準座標系」という。）に従って並進・回転動作させる。これらの手動制御は、通常、産業用ロボットにその姿勢や位置での動作を教え込む所謂教示作業の際に使用される。尚、産業用ロボットでアーク溶接作業を行う場合には、前記ツールは、アーク溶接トーチ（以下、単に「トーチ」という。）となる。

通常、関節手動制御は、産業用ロボットの姿勢を大きく変化させるときだけ使用され、その他の大多数の教示作業では、トーチ等が教示により移動される教示点へのツールの位置決めやツールの姿勢変更には、直角手動制御が使用される。このように産業用ロボットの手動制御において、直角手動制御が多用されるのは、関節手動制御では、トーチの位置と姿勢とを分離して手動制御することができないが、直角手動制御ではこれが可能であり、産業用ロボットのツールを所望の位置や姿勢とし易いためである。

また、産業用ロボットの直角手動制御では、ツールの位置や姿勢の変化の自由度を増やすべく、通常、基準座標系を、複数の座標系から選択できるようになっている。これら基準座標系には、ベース座標系、ツール座標系、ワーク座標系、ショップフロア座標系、任意座標系等がある。これら基準座標系が変われば、産業ロボットに手動制御の動作内容を教示する手段（手動運転命令手段）であるティーチングペンダント（以下、「ＴＰ」と略す。）上で同じキー操作を行っても、ツールの並進動作や回転動作が変わるようになる。

ところで、前記直角手動制御では、産業用ロボットに行わせる作業や動作の種類に応じて適切な座標系が存在する。例えば、ワークに沿ってツールの位置を並進動作により変更したいときはワーク座標系を基準とし、ロボットの設置位置を基準としてツールを並進動作させたいときはベース座標系を基準とすると、それぞれの場合にロボットの手動制御が容易化される。また、トーチの姿勢を回転動作により変更したいときは、ツールの取り付け部位に基づくツール座標系を基準とすると、ロボットの手動制御が容易化される。これは、通常、ツール座標系では、ツールの形状やツールに接続されるワイヤの送給方向に一致するように座標系が設定されているためである。

ところが、従来は、直角手動制御において産業用ロボットのツールの位置や姿勢を変えるにあたり、並進動作及び回転動作の各動作には、常に同一の基準座標系が予め設定され、用いられている（例えば、特許文献１参照）。
特開平１１−１８８６７６号公報

このため、作業者は、主としてツールの並進動作を手動制御するときと、その回転動作を手動制御するときとで、その都度、各動作に適した基準座標系に切り替えることが必要となり、手動制御が著しく煩雑化することで産業用ロボットの教示作業に長時間を要し、作業効率を大きく低下させていた。

本発明は、上記問題点を解決するためになされたものであって、その目的は、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行うことなく、簡易に産業用ロボットのツールを手動制御することができる産業用ロボットのツールの位置・姿勢制御方法及び当該方法を実現する位置・姿勢制御システムを提供することにある。

上記問題点を解決するために、請求項１に記載の発明は、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行うこと、を要旨とする。

上記請求項１の構成によれば、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する前に、該ツールの並進動作及び回転動作の各動作に対してそれぞれ適した基準座標系を設定することができる。これにより、前記手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行う必要がなくなり、作業者が簡易に産業用ロボットのツールを手動制御することができるようになる。その結果、産業用ロボットへ作業プログラムを教示するために要する教示時間が低減され、産業用ロボットの教示作業が簡略化されるようになる。

請求項２に記載の発明は、請求項１に記載の位置・姿勢制御方法において、前記並進動作の基準座標系としてワールド座標系、ベース座標系、又はワーク座標系のいずれか一の座標系を用いるとともに、前記回転動作の基準座標系としてツール座標系を用いること、を要旨とする。

上記請求項２の構成によれば、ツールの並進動作については、作業者が直接的にその動作を認知できるベース座標系において所望の並進動作が実現される一方、ツールの回転動作についても、作業者が直接的にその動作を認知できるツール座標系において所望の回転動作が実現されるようになり、産業用ロボットのツールの手動制御が容易化されるようになる。

請求項３に記載の発明は、請求項２に記載の位置・姿勢制御方法において、前記産業用ロボットが６軸の多関節型ロボットであって、前記並進動作の基準座標系として、前記ワーク座標系を用いるとともに、該ワーク座標系が、前記ロボットに対してワークの相対位置を変更するポジショナに備えられたワーク載置面を基準とすること、を要旨とする。

上記請求項３の構成によれば、ワークをロボットが作業し易いポジションに位置決めするポジショナのワーク載置面を基準として基準座標系を設定できるので、ワークに対するツールの並進動作を、作業者にとって更に認知しやすい状態で手動制御することができるようになる。特に産業用ロボットが溶接ロボットの場合に、例えば、溶接部を常に溶接し易い状態に置くことができるポジショナの姿勢に倣ってトーチ（ツール）を容易に並進移動させることができるようになる。

請求項４に記載の発明は、多関節を有する産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、（１）ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成ステップと、（２）前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算ステップと、（３）前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算ステップと、（４）前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成ステップと、（５）前記指令を前記産業用ロボットの各関節を動かすサーボ機構に与えることで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御ステップと、を備え、前記開始点座標計算ステップ及び累積移動量計算ステップの各ステップにおいて、前記ロボットのツールの並進動作及び回転動作の各動作に対して、それぞれ個別の基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたこと、を要旨とする。

上記請求項４の構成によれば、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行う方法を、簡略的且つ系統だったプロセスで実現することができる。

請求項５に記載の発明は、請求項４に記載のツールの位置・姿勢制御方法において、前記ツール位置指令生成ステップには、（ｉ）前記位置及び姿勢の累積移動量にそれぞれ対応する等価並進行列及び等価回転行列を計算する等価並進・回転行列計算ステップと、（ii）前記開始点座標と、前記等価並進行列及び等価回転行列とに基づいて、前記ツールを前記開始点から目標点に到達させる目標並進行列及び目標回転行列を計算する目標並進・回転行列計算ステップと、（iii）前記目標並進行列及び目標回転行列に基づいて前記産業用ロボットの各関節角度を計算するロボット関節角度計算ステップと、が含まれること、を要旨とする。

上記請求項５の構成によれば、産業用ロボットのツールの並進動作及び回転動作の累積移動量を各関節の関節角度に逆変換することで、ツールを開始点から目標点に到達させる位置指令を生成する方法を、簡略的且つ系統だったプロセスで実現することができる。

請求項６に記載の発明は、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたって使用される制御システムであって、ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成手段と、前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算手段と、前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算手段と、前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成手段と、前記位置指令を前記産業用ロボットの各関節を動かすサーボ機構に送信することで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御手段と、を備え、前記開始点座標計算手段及び累積移動量計算手段の各手段において、前記ロボットのツールの並進動作及び回転動作の各動作に対して、それぞれ個別の基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたこと、を要旨とする。

上記請求項６の構成によれば、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行う方法を、簡略的且つ系統だった制御システムで実現することができる。

本発明によれば、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行うことなく、簡易に産業用ロボットのツールを手動制御することができる産業用ロボットのツールの位置・姿勢制御方法及び当該方法を実現する位置・姿勢制御システムを提供することができる。

以下、本発明の実施形態について図面を参照しながら説明する。
図１に本実施形態で用いる多関節型ロボット装置の制御システムを示す。該制御システムには、多関節型ロボット１、ポジショナ２、前記多関節型ロボット１とポジショナ２とを制御するロボット制御装置３、及び前記多関節型ロボット１とポジショナ２とに駆動電力を供給する溶接電源装置４が設けられている。

前記多関節型ロボット１は、トーチ（ツール）１１と、５個（複数個）のアーム１２ａ，１２ｂ，１２ｃ，１２ｄ，１２ｅと、各アーム間に存在する各関節の関節角度を変えるための６個（複数個）のサーボモータ（駆動モータ）１３ａ，１３ｂ，１３ｃ，１３ｄ，１３ｅ、１３ｆ（但し、サーボモータ１３ｂとサーボモータ１３ｅとは図示省略）とを備えている（トーチ１１側よりロボット１が設置される大地面に向けてアルファベット順で示す。）。これらサーボモータ１３ａ〜１３ｆには、ロボット制御装置３のサーボドライバより回転駆動電力が供給されるともに、ロボット制御装置３から送信される制御信号によって制御される。

また、前記多関節型ロボット１は、トーチ１１を把持するためのフランジ部（面）（符号省略）を有するエンドエフェクタ１４を更に備えており、このトーチ１１は、アーム１２ａを介して前記フランジ部に取り付けられている。この多関節型ロボット１では、トーチ１１を用いてワーク（被溶接物）に対してアーク溶接（溶接）が行われる。尚、図１に示すトーチ１１の直棒状の先端部１１ａは、その基準位置において大地面に対して略直交するようにされている。

このように、多関節型ロボット１は、複数個のサーボモータ１３ａ〜１３ｆがロボット制御装置３により制御され、回転駆動されることによって、多関節型ロボット１の各関節が回転し、且つアーム１２ａ〜１２ｅが移動する。これにより、エンドエフェクタ１４のフランジ部に取り付けられたトーチ１１が上下前後左右に移動可能とされ、所望の位置とされるとともに、その姿勢も所望の状態とされる。

前記多関節型ロボット１のアーム１２ｃ近傍には、トーチ１１に対して溶接ワイヤ（符号省略）を送給するワイヤ送給装置１５が設けられている。該ワイヤ送給装置１５は、溶接ワイヤが巻回されたリールと、該リールを回転させる送給モータ（いずれも図示省略）とを備え、この送給モータは、溶接電源装置４からの電力によって所定のタイミングで回転駆動される。前記溶接ワイヤを介して溶接電源装置４からトーチ１１に高電圧の溶接電圧が供給されることで溶接ワイヤの先端とワークとの間にアークが発生し、その熱で溶接ワイヤが溶融することで、ワークが溶接される。

本実施形態の制御システムにおいて、前記ポジショナ２は、多関節型ロボット１のトーチ１１と対向し、ワークを回転又は傾斜させて当該ワークをトーチ１１による溶接に適する状態とする。詳述すると、このポジショナ２は、溶接時にワークが載置される略円形のワーク載置面２１ａを有するワーク載置台２１と、該ワーク載置台２１を所定方向に回転又は傾斜自在に支持する支持台２１ｂとを備えている。そして、前記トーチ１１は、その先端部１１ａがワーク載置面２１ａに対向するように多関節型ロボット１に設けられている。このようにして、ポジショナ２がワークを自在に回転又は傾斜させるとともに、前述したように複数個のサーボモータ１３ａ〜１３ｆによってアーム１２ａ〜１２ｅを変位させることで、ワークがトーチ１１に対して溶接に適する所要の位置及び姿勢関係に設定される。尚、ポジショナ２のワーク載置台２１は、支持台２１ｂの側部に設けられたサーボモータ２２によって大地面に対して傾斜可能とされている。このサーボモータ２２には、その下方位置にて支持台２１ｂに設けられた電源ボックス２３を介して溶接電源装置４から回転駆動電力が供給される。

また、図１に示すように、本実施形態の制御システムにおいては、多関節型ロボット１の手動制御にあたり、複数の基準座標系が用いられる。これら基準座標系としては、多関節型ロボット１側には、大地を基準に設定され、本制御システムで絶対的な基準となるワールド座標系（Ｏ-Ｘ０，Ｙ０，Ｚ０）、多関節型ロボット１のアーム１２ｅを基準に設定されたベース座標系（Ｏ１-Ｘ１，Ｙ１，Ｚ１）、トーチ１１が取り付けられるフランジ面（取り付け面）を基準に設定されたエンドエフェクタ座標系（Ｏ２-Ｘ２，Ｙ２，Ｚ２）、トーチ１１を基準に設定されたツール座標系（Ｏ３-Ｘ３，Ｙ３，Ｚ３）が定義される。また、ポジショナ２側には、ポジショナ２の設置面を基準に設定されたポジショナ座標系（Ｏ４-Ｘ４，Ｙ４，Ｚ４）、及び、ポジショナ２のワーク載置面２１ａを基準に設定されたワーク座標系（Ｏ５-Ｘ５，Ｙ５，Ｚ５）が定義される。本実施形態における多関節型ロボット１の直角手動制御では、このように定義された複数の基準座標系を用いて、トーチ１１の位置及び姿勢が表現される。

前記ロボット制御装置３は、図２に示すように、本ロボット制御装置３の制御中枢となるＣＰＵ３１、多関節型ロボット１の作業や動作を規定する作業プログラムの各バッファとして機能するＲＡＭ３２、作業プログラムが格納されるＲＯＭ３３、及び所定の定期時刻毎に同期信号をＣＰＵ３１へ発生するタイマー（ＴＩＭＥＲ）３４を備えている。さらに、該ロボット制御装置３は、多関節型ロボット１に対して手動制御によって教示された教示データが格納されるハードディスク３５、及び、手動制御時に使用し、ＴＰ３６ａが接続されるティーチングペンダントＩ／Ｆ（インターフェース）３６を備えている。このＴＰ３６ａは、多関節型ロボット１に前記作業プログラムを教示する教示手段として用いる。さらに、該ロボット制御装置３は、操作ボックスＩ／Ｆ３７及びサーボドライバＩ／Ｆ３８を備えている。この操作ボックスＩ／Ｆ３７には、教示した作業プログラムのタイミングをＣＰＵ３１に与える操作ボックス３７ａが接続されている。

また、サーボドライバＩ／Ｆ３８には、多関節型ロボット１の各関節を駆動する前記した複数のサーボドライバ（サーボ機構）３８ａ，…が接続されている。そして、サーボドライバ３８ａ，…には、各サーボドライバ３８ａによって制御可能な状態で、前記したサーボモータ１３ａ〜１３ｆ（図１参照）が接続されている。詳述すると、各サーボドライバ３８ａの制御は、ＣＰＵ３１からの動作指令信号に基づいて行われ、図２に示す多関節型ロボット１を、６個のサーボモータ１３ａ〜１３ｆによってそれぞれ各アーム１２ａ〜１２ｅ間の関節角度θ１，θ２，θ３，θ４，θ５，θ６で回転するようにされている。

以上説明したＣＰＵ３１、ＲＡＭ３２、ＲＯＭ３３、タイマー３４、ハードディスク３５、ＴＰ３６ａ、操作ボックスＩ／Ｆ３７、及びサーボドライバＩ／Ｆ３８は、バス（ＢＵＳ）３９を介して相互に接続され、上述した各機能を実現すべく必要な信号をやりとりするように構成されている。

本実施形態の制御システムを構成する主要部について詳述すると、前記ＣＰＵ３１は、前記作業プログラム、ＴＰ３６ａや操作ボックス３７ａから送信される操作信号、又は、各サーボモータ１３ａ〜１３ｆに設けられた図示しないエンコーダからの現在位置情報等に基づいて、所定のデータ演算処理を行い、バス３９及びサーボドライバＩ／Ｆ３８を介して各サーボドライバ３８ａに動作指令を与える。これにより、サーボモータ１３ａ〜１３ｆが制御され回転駆動され、前述したように、トーチ１１がワークに対して溶接に適した所要の位置及び姿勢関係とされる。また、前記ＲＯＭ３３には、前記作業プログラムの他、この作業プログラムの実行条件を示すデータ、及び制御定数を示すデータ等が格納される。

また、前記ＴＰ３６ａは、多関節型ロボット１を手動制御するときに作業者によって操作され、所定の操作情報が表示される表示装置３６ｂと所定の情報の入力に用いるキーボード３６ｃとを備えている。該キーボード３６ｃには、手動制御操作用のスイッチ（以下、「軸キー」と呼ぶ。）が１２個設けられている（図示省略）。このようにキーボード３６ｃに１２個の軸キーが設けられているのは、本実施形態のように多関節型ロボット１を直角手動制御で操作する場合では、位置（３自由度）及び姿勢（３自由度）からなる計６自由度に対して、それぞれ正（＋）方向及び負（−）方向の２つの方向に対応する数の軸キーが使用される（６自由度×２つ＝１２個）ためである。

前記キーボード３６ｃには、多関節型ロボット１を直角手動制御で操作するにあたり、作業者がトーチ１１の並進動作及び回転動作の各動作に対してそれぞれ個別の基準座標系を設定するための不図示のスイッチ（以下、「座標系スイッチ」という。）が備えられている。本実施形態において、並進動作及び回転動作の各動作の基準座標系としては、前述したワールド座標系、ベース座標系、ワーク座標系、及びツール座標系の４個の座標系が使用でき、それぞれの座標系について、１から４の間の整数で表される座標系番号が識別のために割り当てられる。ここで、並進動作の座標系番号が入力されるパラメータ変数には、CoorParaを使用し、回転動作の座標系番号が入力されるパラメータ変数には、CoorRotを使用する。作業者は、前記した座標系番号から所望の数字を選択し、前記座標系スイッチを使用して、CoorPara及びCoorRotにそれぞれ個別に座標系番号を入力する。

また、前記キーボード３６ｃには、多関節型ロボット１を直角手動制御で操作するにあたり、作業者がトーチ１１の並進動作及び回転動作の各動作に対してそれぞれ個別の動作速度を設定するための不図示のスイッチ（以下、「速度スイッチ」という。）も備えられている。本実施形態において、並進動作の速度（単位：［mm/sec］）が入力されるパラメータ変数には、ManuParaVelを使用し、回転動作の速度（単位：［rad/sec］）が入力されるパラメータ変数には、ManuRotVelを使用する。作業者は、前記速度スイッチを使用して、ManuParaVel及びManuRotVelにそれぞれ個別に速度を入力する。

さらに、前記キーボード３６ｃには、多関節型ロボット１を直角手動制御で操作するにあたり、作業者がトーチ１１の並進動作及び回転動作の各動作に対してそれぞれ個別の動作方向を指示するための不図示のスイッチ（以下、「動作方向指示スイッチ」という。）も備えられている。本実施形態において、例えば、Ｘ軸方向の並進動作については、正（＋）方向への並進動作の指示の有無（０：なし、１：有り）が入力されるパラメータ変数には、AxesKey_Xpulsを使用し、負（−）方向への並進動作の指示の有無（０：なし、１：有り）が入力されるパラメータ変数には、AxesKey_XMinusを使用する。作業者は、前記動作方向指示スイッチを使用して、AxesKey_Xpuls及びAxesKey_XMinus等にそれぞれ個別に動作指示の有無を入力する。

下表１に、トーチ１１の並進動作及び回転動作に用いる基準座標系のパラメータ変数（CoorPar及びCoorRot）、当該並進動作及び回転動作の各動作の速度のパラメータ変数（ManuParaVel及びManuRotVel）、及び当該並進動作及び回転動作の各動作における２つの動作方向、即ち、正（＋）方向及び負（−）方向のパラメータ変数（AxesKey_Xpuls及びAxesKey_XMinus等）、から構成される動作指示の一覧を示す。

前記ＣＰＵ３１は、これらの各種のパラメータ変数で表される操作信号を受け取ることによって所定のデータ処理を行うとともに、ＴＰ３６ａに表示データを送信して前記表示装置３６ｂに操作情報を表示させる。さらに、ＣＰＵ３１は、これら操作信号を、後述する手動運転命令生成部５１（図４参照）によって教示データの形で読み出し可能な状態でハードディスク３５に格納する。

以下、図３を参照しつつ、前記したサーボドライバ３８ａにおけるサーボモータ１３ａ〜１３ｆのサーボ制御系について簡単に説明する。このサーボ制御系には、位置制御ブロック３００、速度制御ブロック３０１、及び電流制御ブロック３０２が設けられており、各ブロックによって、後述する多関節型ロボット１の各関節に対する位置指令にサーボモータ１３ａ〜１３ｆの動作を追従させることが可能となるとともに、教示データの再生が実行される。詳述すると、ＣＰＵ３１から送信される前記位置指令は、先ず、位置制御ブロック３００に入力され、該位置制御ブロック３００からの出力は、速度制御ブロック３０１に入力される。そして、この速度制御ブロック３０１からの出力は、電流制御ブロック３０２に入力され、電流制御ブロック３０２の出力は、サーボモータ１３ａ〜１３ｆに入力される。サーボモータ１３ａ〜１３ｆからの出力は、ばね等の減速機構３０４を介して、負荷となるアーム１２ａ〜１２ｅに伝えられる。

各サーボモータ１３ａ〜１３ｆには、それぞれ電流検出ブロック３０３が接続され、各電流検出ブロック３０３において各サーボモータ１３ａ〜１３ｆに流れる電流が検出され、該電流値は電流制御ブロック３０２にフィードバックされる。また、各サーボモータ１３ａ〜１３ｆには、それぞれエンコーダ３０５が接続され、各エンコーダ３０５によって現在のアームの移動速度に対応する各サーボモータ１３ａ〜１３ｆの回転速度に関するデータが取得され、該取得された回転速度データは、位置制御ブロック３００と速度制御ブロック３０１とにフィードバックされる。

前記位置制御ブロック３００では、電流制御ブロック３０２にフィードバックされた前記回転速度データが積分ブロック３０６によって積分されることにより位置データに変換されて減算器３０７に入力され、この減算器３０７により多関節型ロボット１の各関節に対する位置指令に対する現在の各関節の位置（関節角度）の誤差データが演算される。この誤差データは増幅器３０８により所定のゲインＫｐｐ（以下、「位置ＦＢゲイン」という。）でレベル補正が行なわれた後、加算器３０９に入力される。

また、位置制御ブロック３００では、多関節型ロボット１の各関節に対する位置指令のデータは、微分ブロック３１０によって微分されることにより速度データに変換され、さらに増幅器３１１により所定のゲインＫｆｆ（以下、「速度ＦＦゲイン」という。）でレベル補正が行なわれた後、加算器３０９に入力される。該加算器３０９では速度ＦＦゲインＫｆｆで出力される速度データと、位置ＦＢゲインＫｐｐで出力される誤差データとが加算され、前記した速度制御ブロック３０１に出力される。

以上のとおり、本実施形態の制御システムでは、多関節型ロボット１の各関節に対する位置指令に対して各サーボモータ１３ａ〜１３ｆの動作をフィードバック（ＦＢ）制御することにより、予め教示された教示データの再生が正確に行われるようになっている。

以下、図４を参照しつつ、このプログラムの構成について説明する。このプログラムは、多関節型ロボット１のトーチ１１（ツール）の位置及び姿勢を直角手動制御によって制御するためのものである。

前述したＲＯＭ３３（図２参照）には、多関節型ロボット１の動作制御を実行するプログラム、そのプログラムの実行条件、及び制御定数等が格納される。このプログラムは、図４に示すように、手動運転命令生成部５１、開始点座標生成部５２、補間点生成部５３、関節補間点生成部５４、及びサーボ出力部５５を備えており、本プログラムに従ってＣＰＵ３１にて演算処理が行われる。また、ＲＡＭ３２は、ＣＰＵ３１のワーキングエリアとして用いられ、図４に示す動作命令バッファ５１ａ、軌道バッファ５２ａ、補間点バッファ５３ａ、及び関節補間点バッファ５４ａの各機能が実現される。これら各バッファは、ＦＩＦＯ（first-in first-out）バッファとして構成されており、先入れ及び先出しでデータが処理され、上記プログラムの各部５１〜５５で生成するデータが一時的に格納されるようになっている。

以下、図５〜図９を参照しつつ、本実施形態の多関節型ロボット装置の制御システムを使用し、産業用ロボットの位置及び姿勢を直角手動制御によって制御する方法について説明する。

図５を参照して、先ず、多関節型ロボット１の動作制御を実行するプログラムにおける手動運転命令生成部５１（図４参照）が、ＴＰ３６ａからの手動制御のための操作（以下、「手動制御操作」という。）の有無を待機する（ステップＳ０）。この手動制御操作が実行されない場合は、この待機の状態を継続する。一方、手動制御操作を実行したことが手動運転命令生成部５１によって検知されると、当該手動運転命令生成部５１において、前記検知を契機として多関節型ロボット１の手動運転命令が生成されるとともに、該手動運転命令が動作命令バッファ５１ａに格納される（ステップＳ１）。詳述すると、ステップＳ０にて、手動運転命令生成部５１において、ＴＰ３６ａの１２個の軸キーの内の少なくとも１個が操作されたこと、または、操作中の前記軸キーの数や種類が変更されたことが検知されると、ステップＳ１にて、前記手動運転命令生成部５１において、新たな手動運転命令が生成される。この手動運転命令には、表１に示したトーチ１１の並進動作及び回転動作の基準座標系のパラメータ変数（CoorPar及びCoorRot）、当該並進動作及び回転動作の各動作の速度［mm/sec，rad/sec］のパラメータ変数（ManuParaVel及びManuRotVel）、及び当該並進動作及び回転動作の各動作における２つの動作方向、即ち、正（＋）方向及び負（−）方向のパラメータ変数（AxesKey_Xpuls及びAxesKey_XMinus等）、から構成される動作指示データが含まれる。

本実施形態では、ＴＰ３６ａを用い、パラメータ変数CoorParaに２を入力し（即ち、並進動作の基準座標系をベース座標系とする。）、パラメータ変数CoorRotに４を入力する（即ち、回転動作の基準座標系をツール座標系とする。）。また、パラメータ変数ManuParaVel及びManuRotVelには、ぞれぞれ所定値［mm/sec，rad/sec］を入力するとともに、軸キーを操作し、AxesKey_Xpuls及びAxesKey_XMinus等にも、所要の動作方向に応じた数値が入力されるようにする。

ここで、並進動作の座標系は、図６（ａ）に示すように、ベース座標系を基準座標系に用いて表現され（同図では並進動作の移動距離＝Ｄ［mm］とする。）、回転動作の座標系は、図６（ｂ）に示すように、ツール座標系を基準座標系に用いて表現される（同図では回転動作の回転角度＝α［°］とする。）。このように、本実施形態では、並進動作及び回転動作の座標系はそれぞれ個別の基準座標系を用いて表現されるが、後述する同次変換行列を使用すれば、図７に示すように、並進動作及び回転動作の座標系は、まるで同一の基準座標系を用いる場合のように、統一して表現されるようになる。

図５に戻り、次に、開始点座標生成部５２（図４参照）において、動作命令バッファ５１ａから手動運転命令が読み出され、この手動運転命令に基づいて、トーチ１１の手動制御の開始点座標（開始点の位置及び姿勢に対応する。）が三次元空間の直交座標上で計算（生成）されるとともに該開始点座標及び手動運転命令に関するデータ（以下、「軌道データ」という。）が軌道バッファ５２ａに格納される（ステップＳ２）。このステップＳ２では、多関節型ロボット１のトーチ１１の並進動作及び回転動作の各動作に対して、それぞれ個別に基準座標系を用いて開始点座標が計算（生成）される。詳述すると、本実施形態では、前述したように、並進動作（位置に対応）、回転動作（姿勢に対応）の各動作に対して、それぞれベース座標系、ツール座標系を基準座標系に用いるので、トーチ１１の並進動作及び回転動作の各動作用にそれぞれ個別に計２個の開始点座標が計算（生成）される。尚、本実施形態では、前記開始点座標の目標座標系（基準座標系によって表現する目標（目的）となる座標系）については、並進動作及び回転動作共に、トーチ１１が実際（最終的）に動作する座標系であるツール座標系を用いて表現する。

このように、本実施形態では、並進動作、回転動作の各動作に対して、それぞれベース座標系、ツール座標系を基準座標系に用いる一方、両動作共にツール座標系を目標座標系に用いてその位置及び姿勢を表現する。このため、前記開始点座標は、トーチ１１の並進動作については、ベース座標系から見たツール座標系の位置が表現され、トーチ１１の回転動作については、ツール座標系から見たツール座標系の姿勢が表現される。後者のように回転動作の目標座標系及び基準座標系が共にツール座標系の場合は、ツール座標系から見たツール座標系の姿勢を求めることとなって意味をなさないので、ベース座標系を基準座標系とする。以上のようにして、本実施形態では、開始点座標の並進動作及び回転動作は、ベース座標系から見たツール座標系の位置及び姿勢を表す４行４列の同次変換行列として計算される。

ここで、前記同次変換行列の計算方法について説明する。下表２に、本計算で用いる同次変換行列を一覧して示す。表２に示す同次変換行列は、一の基準座標系から見た隣接する他の座標系（目標座標系）の位置及び姿勢を表現するものであり、多関節型ロボット１及びポジショナ２のアームの寸法、各関節の関節角度、設置（据付）位置姿勢等のリンクパラメータを用いて従来周知の方法で計算される。

注）上表２において、「時不変」とは、同次変換行列の特性が時間に依存せずに不変であることをいう。

以下、用いる基準座標系別に、該基準座標系から見た、トーチ１１の並進動作及び回転動作の各開始点座標を表す同次変換行列の計算式を示す。ここでは、トーチ１１の並進動作の開始点座標を表す同次変換行列をMStartPara、トーチ１１の回転動作の開始点座標を表す同次変換行列をMStartRotでそれぞれ表記する。尚、各同次変換行列において、右上の添え字「−１」がある行列は、同表記の添え字のない行列に対する逆行列を示す。

１）並進動作のパラメータ変数CoorPara＝１（ワールド座標系基準）のとき、
MStartPara＝XWorld2Base×XBase2End×XEnd2Tool…式(１−１)
２）回転動作のパラメータ変数CoorRot＝１（ワールド座標系基準）のとき、
MStartRot＝XWorld2Base×XBase2End×XEnd2Tool…式(１−２)
３）並進動作のパラメータ変数CoorPara＝２（ベース座標系基準）のとき、
MStartPara＝XBase2End×XEnd2Tool…式(１−３)
４）回転動作のパラメータ変数CoorRot＝２（ベース座標系基準）のとき、
MStartRot＝XBase2End×XEnd2Tool…式(１−４)
５）並進動作のパラメータ変数CoorPara＝３（ワーク座標系基準）のとき、
MStartPara＝XPo2Work^-1×XWorld2Po^-1×XWorld2Base×XBase2End×XEnd2Tool…式(１−５)
６）回転動作のパラメータ変数CoorRot＝３（ワーク座標系基準）のとき、
MStartRot＝XPo2Work^-1×XWorld2Po^-1×XWorld2Base×XBase2End×XEnd2Tool…式(１−６)
７）並進動作のパラメータ変数CoorPara＝４（ツール座標系基準）のとき、
MStartPara ＝XBase2End×XEnd2Tool…式(１−７)
８）回転動作のパラメータ変数CoorRot＝４（ツール座標系基準）のとき、
MStartRot＝XBase2End×XEnd2Tool…式(１−８)

前述したように、本実施形態では、パラメータ変数CoorPara＝２、パラメータ変数CoorRot＝４であるので、以下の計算プロセスでは、上式(１−３)及び式(１−８)が使用される。

次いで、図５を参照して、補間点生成部５３（図４参照）において、軌道バッファ５２ａから並進動作及び回転動作についての軌道データが各動作について個別に読み出される。そして、この軌道データに基づいて、トーチ１１の位置及び姿勢のそれぞれについて、補間周期（以降の計算式で「Sampling」と表記する。単位：[sec]）と呼ぶ所定の時間毎にトーチ１１が到達すべき位置及び姿勢に関するデータが補間点データとして計算されるとともに、該補間点データは、トーチ１１が前記開始点座標で表される開始点から目標点（手動制御の終了点）に至る全補間周期の間で累積計算される。こうして得られた各補間点データ及び累積移動量は、補間点バッファ５３ａに一時的に格納される（ステップＳ３）。これら累積計算の内、位置（並進動作）の累積計算について図８を参照して説明すると、開始点Ｐ０から目標点Ｐｎ間をｎ等分する各点Ｐ１〜Ｐｎ−１についてそれぞれサイクリックに補間点データが計算され、各補間点データが開始点Ｐ０から目標点Ｐｎまで累積計算される。即ち、本ステップＳ３では、多関節型ロボット１のトーチ１１の並進動作及び回転動作の各動作に対して、それぞれベース座標系及びツール座標系を基準座標系として補間点データ及び累積移動量が計算されることになる。具体的には、これら補間点データ及び累積移動量は、前述した直角手動制御のパラメータ変数を用いて計算される。つまり、軸キーの操作状態（AxesKey_XPuls―AxesKey_Xminus等）に対して、直角手動制御の並進動作又は回転動作の速度（ManuParaVel［mm/sec］又はManuRotVel［rad/sec］）に補間周期（Sampling[sec]）を乗じた値が、一の補間周期の間で計算される位置データ（SampParaX、並進移動量）又は姿勢データ（SampRotA、回転移動量）となる。具体的には、下に示す計算式で表される。

SampParaX＝(AxesKey_XPuls―AxesKey_XMinus)×ManuParaVel×Sampling…式(２−１)
SampParaY＝(AxesKey_Ypuls―AxesKey_YMinus)×ManuParaVel×Sampling…式(２−２)
SampParaZ＝(AxesKey_Zpuls―AxesKey_ZMinus)×ManuParaVel×Sampling…式(２−３)
SampRotA＝(AxesKey_Apuls―AxesKey_AMinus)×ManuRotVel×Sampling…式(２−４)
SampRotB＝(AxesKey_Bpuls―AxesKey_BMinus)×ManuRotVel×Sampling…式(２−５)
SampRotC＝(AxesKey_Cpuls―AxesKey_CMinus)×ManuRotVel×Sampling…式(２−６)
ここで、
SampParaX：一の補間周期の間におけるトーチ１１のX軸方向の並進移動量[mm]
SampParaY：一の補間周期の間におけるトーチ１１のY軸方向の並進移動量[mm]
SampParaZ：一の補間周期の間におけるトーチ１１のZ軸方向の並進移動量[mm]
SampRotA：一の補間周期の間におけるトーチ１１のX軸周りの回転移動量[rad]
SampRotB：一の補間周期の間におけるトーチ１１のY軸周りの回転移動量[rad]
SampRotC：一の補間周期の間におけるトーチ１１のZ軸周りの回転移動量[rad]

さらに、トーチ１１の並進動作の累積移動量（累積並進移動量：TotalParaX等）及びトーチ１１の回転動作の累積移動量（累積回転移動量：TotalRotA等）は、下に示す計算式で表される。
TotalParaX＝TotalParaX +SampParaX…式(３−１)
TotalParaY＝TotalParaY +SampParaY…式(３−２)
TotalParaZ＝TotalParaZ +SampParaZ…式(３−３)
TotalRotA＝TotalRotA +SampRotA…式(３−４)
TotalRotB＝TotalRotB +SampRotB…式(３−５)
TotalRotC＝TotalRotC +SampRotC…式(３−６)
ここで、
TotalParaX: 手動制御スタート後の基準座標系のＸ軸方向の累積並進移動量[mm]
TotalParaY: 手動制御スタート後の基準座標系のＹ軸方向の累積並進移動量[mm]
TotalParaZ: 手動制御スタート後の基準座標系のＺ軸方向の累積並進移動量[mm]
TotalRotA: 手動制御スタート後の基準座標系のＸ軸周りの累積回転移動量[rad]
TotalRotB: 手動制御スタート後の基準座標系のＹ軸周りの累積回転移動量[rad]
TotalRotC: 手動制御スタート後の基準座標系のＺ軸周りの累積回転移動量[rad]

尚、各累積移動量TotalParaX，TotalParaY，TotalParaZ，TotalRotA，TotalRotB，及びTotalRotCは、先ず、手動運転命令生成部５１において、作業者がＴＰ３６ａに対して手動制御操作を実行したことが検知され、さらに開始点座標生成部５２において、動作命令バッファ５１ａから手動運転命令が読み出された後、最初に補間点生成部５３において、補間点データが計算される直前にそれぞれ初期の累積移動量０［mm，rad］にリセットされる。

続いて、図５を参照して、関節補間点生成部５４（図４参照）において、上述したように計算され、直交座標系で表現された位置及び姿勢にそれぞれ対応する累積移動量が、多関節型ロボット１の各関節の関節角度に逆変換される。これにより、前記関節補間点生成部５４において、トーチ１１を開始点座標で表される開始点から目標点に到達させるためのサーボドライバ３８ａ，…への位置指令（多関節型ロボット１の各関節に対する位置指令）が計算される。この位置指令は、関節補間点バッファ５４ａに一時的に格納され、その後、サーボ出力部５５によって関節補間点バッファ５４ａから読み出され、サーボドライバ３８ａ（図２参照）に送信される（ステップＳ４）。

前記ステップＳ４について詳述すると、該ステップＳ４は、３つ（複数）の小ステップＳ４１，Ｓ４２，及びＳ４３によって構成される。各小ステップＳ４１，Ｓ４２，及びＳ４３について、図５を参照して説明すると、先ず、関節補間点生成部５４において、トーチ１１の位置（並進動作）及び姿勢（回転動作）の累積移動量にそれぞれ対応する等価並進行列及び等価回転行列が計算される（小ステップＳ４１）。

具体的には、上述したように計算した累積並進移動量及び累積回転移動量に基づいて、トーチ１１の並進動作と回転動作の各動作について個別に等価並進行列及び等価回転行列を計算する。即ち、先ず、等価並進行列MatXYZについては、累積並進移動量を用いて以下の４次元ベクトルの形で定義する。

また、等価回転行列MatRPYについては、累積回転移動量を用いて以下の４次元ベクトルの形で定義する。尚、剛体の回転動作（姿勢）には多種の表現が提案されているが、ここでは、多関節型ロボットの制御分野において一般的に使用されるロール・ピッチ・ヨー角φ，θ，ψによる表現を使用する。

次に、前述したように並進動作及び回転動作について個別に計算された各開始点座標と、上式（４−１）及び（４−４）によってそれぞれ計算された等価並進行列及び等価回転行列とに基づいて、関節補間点生成部５４において、各開始点座標で表される開始点からトーチ１１の目標点に至る目標並進行列及び目標回転行列がそれぞれ計算される（小ステップＳ４２）。

具体的には、目標並進行列MTargetParaについては、ＴＰ３６ａに入力されたパラメータ変数（CoorParａ）が１（ワールド座標系）、２（ベース座標系）、又は３（ワーク座標系）のいずれかの場合には、式（４−１）で示されるMatXYZを、並進動作の開始点座標を表す同次変換行列MStartParaに対して左からかけることで計算する。このように等価並進行列MatXYZを同次変換行列MStartParaに対して左からかけることは、並進動作をワールド、ベース、ワーク座標系のいずれか一の基準座標系に関して行うことに相当する。本実施形態では、前述したように、パラメータ変数CoorParａ＝２であって、並進動作の基準座標系はベース座標系であるので、MatXYZはベース座標系の並進動作として計算される。このように計算すると、作業者が直接的に並進動作を認知できるベース座標系において所望の並進動作が実現され、トーチ１１の操作が容易化されるため好ましい。この場合、目標並進行列MTargetParaは、関節補間点生成部５４において、下式(４−２)によって計算される。
MTargetPara＝MatXYZ×MStartPara…式(４−２)

これに対して、ＴＰ３６ａに入力されたパラメータ変数（CoorPar）が４（ツール座標系）の場合には、式（４−１）で示されるMatXYZを、並進動作の開始点座標を表す同次変換行列MStartParaに対して右からかけることで目標並進行列MTargetParaを計算する。このように等価並進行列MatXYZを同次変換行列MStartParaに対して右からかけることは、並進動作を目標座標系に関して行うことに相当する。即ち、この場合は、目標並進行列MTargetParaは、関節補間点生成部５４において、下式(４−３)によって計算される。
MTargetPara＝MStartPara×MatXYZ…式(４−３)

一方、目標回転行列MTargetRotについては、ＴＰ３６ａに入力されたパラメータ変数（CoorRot）が１（ワールド座標系）、２（ベース座標系）、又は３（ワーク座標系）のいずれかの場合には、式（４−４）で示されるMatRPYを、回転動作の開始点座標を表す同次変換行列MStartRotに対して左からかけることで計算する。このように等価回転行列MatRPYを同次変換行列MStartRotに対して左からかけることは、回転動作をワールド、ベース、ワーク座標系のいずれか一の基準座標系に関して行うことに相当する。即ち、この場合、目標回転行列MTargetRotは、関節補間点生成部５４において、下式(４−５)によって計算される。
MTargetRot＝MatRPY×MStartRot…式(４−５)

これに対して、ＴＰ３６ａに入力されたパラメータ変数（CoorRot）が４（ツール座標系）の場合には、式（４−４）で示されるMatRPYを、回転動作の開始点座標を表す同次変換行列MStartRotに対して右からかけることで目標回転行列MTargetRotを計算する。このように等価回転行列MatRPYを同次変換行列MStartRotに対して右からかけることは、回転動作を目標座標系に関して行うことに相当する。本実施形態では、前述したように、回転動作の目標座標系はツール座標系であるので、MatRPYはツール座標系の回転動作として計算される。このように計算すると、作業者が直接的に回転動作を認知できるツール座標系において所望の回転動作が実現され、トーチ１１の操作が容易化されるため好ましい。即ち、この場合は、目標回転行列MTargetRotは、関節補間点生成部５４において、下式(４−６)によって計算される。
MTargetRot＝MStartRot×MatRPY…式(４−６)

次いで、以上のようにして計算された目標並進行列及び目標回転行列に基づいて、関節補間点生成部５４において、多関節型ロボット１の各関節の関節角度が計算され、関節補間点バッファ５４ａに一時的に格納される（小ステップＳ４３）。

一般に、トーチ１１の位置及び姿勢を前記した目標並進行列及び目標回転行列で示される座標に一致させるには、多関節型ロボットの各関節を操作することが必要になる。本実施形態では、多関節型ロボット１の各関節の関節角度を決定するために、多関節型ロボット１のベース座標系から見たエンドエフェクタ座標系の位置及び姿勢をそれぞれ表す同次変換行列TargetParaXBase2End及びTargetRotXBase2Endを、それぞれ表２に示す同次変換行列を用いて目標並進行列MTargetPara及び目標回転行列MTargetRotから抽出する方法を用いる。この方法を実行する計算式について、以下、並進動作及び回転動作の各動作について基準座標系別に示す。尚、各同次変換行列において、右上の添え字「−１」がある行列は、同表記の添え字のない行列に対する逆行列を示す。

１）並進動作のパラメータ変数CoorPar＝１（ワールド座標系基準）のとき、
TargetPaｒaXBase2End＝XWorld2Base^-1×MTargetPara×XEnd2Tool^-1…式(５−１)
２）回転動作のパラメータ変数CoorRot＝１（ワールド座標系基準）のとき、
TargetRotXBase2End＝XWorld2Base^-1×MTargetRot×XEnd2Tool^-1…式(５−２)
３）並進動作のパラメータ変数CoorPar＝２（ベース座標系基準）のとき、
TargetPaｒaXBase2End＝MTargetPara×XEnd2Tool^―1…式(５−３)
４）回転動作のパラメータ変数CoorRot＝２（ベース座標系基準）のとき、
TargetRotXBase2End＝MTargetRot×XEnd2Tool^-1…式(５−４)
５）並進動作のパラメータ変数CoorPar＝３（ワーク座標系基準）のとき、
TargetPaｒaXBase2End＝XWorld2Base^-1×XWorld2Po×XPo2Work×MTargetPara×XEnd2Tool^-1・・・式(５−５)
６）回転動作のパラメータ変数CoorRot＝３（ワーク座標系基準）のとき、
TargetRotXBase2End＝XWorld2Base^-1×XWorld2Po×XPo2Work×MTargetRot×XEnd2Tool^-1・・・式(５−６)
７）並進動作のパラメータ変数CoorPar＝４（ツール座標系基準）のとき、
TargetPaｒaXBase2End＝MTargetPara×XEnd2Tool^-1…式(５−７)
８）回転動作のパラメータ変数CoorRot＝４（ツール座標系基準）のとき、
TargetRotXBase2End＝MTargetRot×XEnd2Tool^-1…式(５−８)

並進動作及び回転動作の各動作について、それぞれ個別の座標系を用いて上記のように計算されたTargetParaXBase2End及びTargetRotXBase2Endは、それぞれ、多関節型ロボットのベース座標系から見たエンドエフェクタ座標系の位置及び姿勢を表す同次変換行列である。したがって、同次変換行列TargetParaXBase2Endから位置成分を、同次変換行列TargetRotXBase2Endから姿勢成分を取り出して合成して得られる同次変換行列TargetMixXBase2Endが、トーチ１１の位置及び姿勢を共に所望の状態（目標点の状態）とする同次変換行列となる。ここで、上述したように計算されたTargetParaXBase2End及びTargetRotXBase2Endの各同次変換行列は、それぞれ下式(７−１)及び式（７−２）で表される。

この場合、同次変換行列TargetMixXBase2Endは、並進成分と回転成分とをそれぞれ上式(７−１)及び式（７−２）から分離し、再結合する従来周知の方法によって、下式(７−３)で表される。

上式（７−３）によって、多関節型ロボット１のベース座標系から見たエンドエフェクタ座標系の位置及び姿勢を表す同次変換行列が得られる。この変換プロセスについて、図９を参照して説明すると、前述したように、本実施形態では、開始点座標は、並進動作については、ベース座標系を基準とし、回転動作については、ツール座標系を基準とする。また、開始点Ｐ０から目標点Ｐｎまでの並進移動（距離Ｄ[mm]の移動）及び回転移動（角度α［°］の移動）は、ベース座標系を基準としたエンドエフェクタ座標系の同次変換行列で表現する。即ち、並進動作については、TargetParaXBase2Endと表現し、回転動作については、TargetRotXBase2Endと表現する。さらに、目標点Ｐｎは、両同次変換行列を合成して得られる同次変換行列TargetMixXBase2Endによって、ベース座標系を基準として統一的に扱われる。

以上のように計算された同次変換行列を、関節補間点生成部５４において、従来周知の方法で逆変換して多関節型ロボット１の各関節角度を算出することにより、当該多関節型ロボット１のトーチ１１が開始点から目標点（手動制御の終了点）に至る全補間周期で到達する関節角度が求められる。

その後、図５を参照して、サーボ出力部５５（図４参照）によって、関節補間点バッファ５４ａから多関節型ロボット１の各関節角度が読み出され、該各関節角度に基づいて、サーボドライバ３８ａ（図２参照）に多関節型ロボット１の各関節及びトーチ１１の位置及び姿勢を目標の状態とさせる指令（多関節型ロボット１の各関節に対する位置指令）が生成される。その後、サーボ出力部５５から、前記位置指令をサーボドライバＩ／Ｆ３８を介して各サーボドライバ３８ａに送信することでサーボモータ１３ａ〜１３ｆを制御し、これによりトーチ１１を開始点から目標点に到達させるようにする。尚、前記位置指令は、タイマー３４（図２参照）で発生する同期信号に同期して各サーボドライバ３８ａへ送信される（ステップＳ５）。

以上説明したように、本実施形態によれば、下記する作用・効果が奏される。
（１）本実施形態では、多関節型ロボット１の位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該多関節型ロボット１に備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行うようにしている。このため、多関節型ロボット１のトーチ１１の位置及び姿勢を直交座標系を用いて手動で制御する前に、該トーチ１１の並進動作及び回転動作の各動作に対してそれぞれ適した基準座標系を設定しておくことができる。これにより、前記手動制御の最中に、多関節型ロボット１のトーチ１１の並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行う必要がなくなり、作業者が簡易に多関節型ロボット１のトーチ１１を手動制御することができるようになる。その結果、多関節型ロボット１へ作業プログラムを教示するために要する教示時間が低減され、多関節型ロボット１の教示作業が簡略化されるようになる。

（２）本実施形態では、トーチ１１の並進動作の基準座標系としてベース座標系を用いるとともに、トーチ１１の回転動作の基準座標系としてツール座標系を用いている。このため、トーチ１１の並進動作については、作業者が直接的にその動作を認知できるベース座標系において所望の並進動作が実現される一方、トーチ１１の回転動作についても、作業者が直接的にその動作を認知できるツール座標系において所望の回転動作が実現されるようになり、トーチ１１の手動制御が容易化されるようになる。

（３）本実施形態では、トーチ１１の並進動作の基準座標系を手動制御の状態に応じて変更したときでも、トーチ１１の回転動作の基準座標系は、トーチ１１の姿勢が変更された方向が作業者によって容易に認知できるツール座標系に固定されている。したがって、作業者に多関節型ロボット１の手動制御にあたっての混乱を生じさせることがなく、多関節型ロボット１の手動制御による教示時間を低減させることができる。

尚、上記実施形態は以下のように変形してもよい。
・上記実施形態では、多関節型ロボット１のトーチ１１の並進動作及び回転動作の基準座標系として、それぞれワールド座標系、ベース座標系、ワーク座標系、又はツール座標系のいずれか一の座標系を選択可能なようにしたが、トーチ１１の並進動作及び回転動作に対して個別に基準座標系を設定し得るようにする限り、これらの座標系に限るものではなく、多関節型ロボット１で一般に用いられるその他の座標系、例えば、エンドエフェクタ座標系やポジショナ座標系、ショップフロア座標系、又は、その他の任意座標系を選択可能としてもよい。

・また、上記実施形態では、多関節型ロボット１のトーチ１１の並進動作の基準座標系にベース座標系を用い、回転動作の基準座標系にツール座標系を用いるようにした。しかし、これに限らず、例えば、並進動作の基準座標系にワールド座標系を用い、回転動作の基準座標系にツール座標系を用いるようにしてもよい。

・また、上記実施形態では、直交座標系で表現された姿勢に対応する累積回転移動量を多関節型ロボット１の各関節の関節角度に逆変換するにあたり、該累積回転移動量を用いて等価回転行列を定義するためにロール・ピッチ・ヨー角による表現を用いたが、オイラー角による表現を用いることも可能である。

・また、上記実施形態の多関節型ロボット装置の制御システムにおいては、６軸の多関節型ロボット１及び２軸のポジショナ２を備えた構成としたが、当該制御システムにおいては、ロボットによる溶接作業が容易且つ確実に行える限り、ポジショナ２は必ずしも必要ではない。また、ポジショナ２の代わりに６軸の第二の多関節型ロボットを用いることもできる。このようにポジショナ２に代えて６軸の第二の多関節型ロボットを用いる場合には、ポジショナ２に設定したワーク座標系とポジショナ座標系の代わりに、該第二の多関節型ロボットに設定したロボットワールド座標系、ベース座標系、又はツール座標系のいずれかの座標系を基準座標系又は目標座標系に用いることができる。

・また、上記実施形態では、多関節型ロボット１（産業用ロボット）を自動車等の溶接に用いる溶接ロボット、即ち、ツールとしてトーチ１１（溶接トーチ）を備える溶接ロボットとしたが、本発明の技術的思想は、その他の産業用ロボット、例えば、組立ロボット、塗装ロボット、検査ロボット、研摩ロボット、洗浄ロボット等にも適用することができる。

さらに、前記した実施形態および変形例より把握できる技術的思想について以下に記載する。

・産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたって使用される制御システムであって、請求項４又は請求項５に記載の各ステップを実行することを特徴とする産業用ロボットのツールの位置・姿勢制御システム。このように構成した場合、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行う方法を、系統だった制御システムで実現することができる。

本発明に係る多関節型ロボットの制御システムの全体図。本発明に係るロボット制御装置の内部構成及びその周辺装置を示すブロック図。本発明に係るサーボドライバによるサーボ制御系を示すブロック図。本発明に係るロボット制御装置に用いる制御プログラムの構成を示す概念図。本発明に係るロボット制御装置に用いる制御プログラムのフローチャート図。本発明に係る多関節型ロボットに用いる基準座標系及び同基準座標系を基準とした目標座標系の動作を示す説明図であり、（ａ）は、ベース座標系を基準座標系とした目標座標系の並進動作を示す図、（ｂ）は、ツール座標系を基準座標系とした目標座標系の回転動作を示す図。本発明に係る多関節型ロボットにおけるベース座標系、ツール座標系をそれぞれ基準座標系とした目標座標系の並進動作、回転動作を示す説明図。本発明に係る多関節型ロボットにおける基準座標系を用いて目標座標系の並進動作の補間点データ及び累積移動量を計算するプロセスを示す説明図。ベース座標系から見たエンドエフェクタ座標系の位置及び姿勢を表す同次変換行列と目標座標系（ツール座標系）との対応関係を示す説明図。

符号の説明

１…多関節型ロボット，２…ポジショナ，３…ロボット制御装置，４…溶接制御装置，１１…トーチ。

Claims

産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行うことを特徴とする産業用ロボットのツールの位置・姿勢制御方法。
請求項１に記載の位置・姿勢制御方法において、
前記並進動作の基準座標系としてワールド座標系、ベース座標系、又はワーク座標系の内のいずれか一の座標系を用いるとともに、前記回転動作の基準座標系としてツール座標系を用いる産業用ロボットのツールの位置・姿勢制御方法。
請求項２に記載の位置・姿勢制御方法において、
前記産業用ロボットが６軸の多関節型ロボットであって、前記並進動作の基準座標系として、前記ワーク座標系を用いるとともに、該ワーク座標系が、前記ロボットに対してワークの相対位置を変更するポジショナに備えられたワーク載置面を基準とする産業用ロボットのツールの位置・姿勢制御方法。
多関節を有する産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、
（１）ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成ステップと、
（２）前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算ステップと、
（３）前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算ステップと、
（４）前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成ステップと、
（５）前記指令を前記産業用ロボットの各関節を動かすサーボ機構に与えることで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御ステップと、を備え、
前記開始点座標計算ステップ及び累積移動量計算ステップの各ステップにおいて、前記ロボットのツールの並進動作及び回転動作の各動作に対して、それぞれ個別の基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたことを特徴とする産業用ロボットのツールの位置・姿勢制御方法。
請求項４に記載のツールの位置・姿勢制御方法において、前記ツール位置指令生成ステップには、
（ｉ）前記位置及び姿勢の累積移動量にそれぞれ対応する等価並進行列及び等価回転行列を計算する等価並進・回転行列計算ステップと、
（ii）前記開始点座標と、前記等価並進行列及び等価回転行列とに基づいて、前記ツールを前記開始点から目標点に到達させる目標並進行列及び目標回転行列を計算する目標並進・回転行列計算ステップと、
（iii）前記目標並進行列及び目標回転行列に基づいて前記産業用ロボットの各関節角度を計算するロボット関節角度計算ステップと、
が含まれる産業用ロボットのツールの位置・姿勢制御方法。
産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたって使用される制御システムであって、
ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成手段と、
前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算手段と、
前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算手段と、
前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成手段と、
前記位置指令を前記産業用ロボットの各関節を動かすサーボ機構に送信することで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御手段と、を備え、
前記開始点座標計算手段及び累積移動量計算手段の各手段において、前記ロボットのツールの並進動作及び回転動作の各動作に対して、それぞれ個別の基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたことを特徴とする産業用ロボットのツールの位置・姿勢制御システム。