トップ :: B 処理操作 運輸 :: B25 手工具;可搬型動力工具;手工具用の柄;作業場設備;マニプレ−タ




【発明の名称】 溶接ロボットの制御方法
【発明者】 【氏名】西村 利彦

【氏名】友近 信行
【要約】 【課題】溶接ロボットにおいて比較的簡易な計算で第４及び第５の軸の２軸のみによるウィビング動作を実現する。

【構成】ウィビング成分加算前のトーチ先端１６ａの位置Ｐｃ（ｔ）にウィビング成分を加算してウィビング加算後の仮のトーチ先端１６ａの位置Ｐ（ｔ）を求める。アームの先端位置Ｐａｒｍを中心とし、仮のトーチ先端１６ａまでの距離を半径とする球面Ｑを求める。少なくとも仮のツール先端位置Ｐ（ｔ）を使用して、球面Ｑ上にウィビング成分加算後の真のトーチ先端１６ａの位置となる交点ＰＱを求める。交点ＰＱから第４及び第５の軸の関節角度Ｊ４’，Ｊ５’を算出する。
【特許請求の範囲】
【請求項１】
６軸の垂直多関節型の溶接ロボットにウィビング動作を実行させるための制御方法であって、
前記溶接ロボットは、基本軸として第１から第３の軸を備え、ツールを保持する手首軸として第４から第６の軸を備えると共に、前記第１から第４の軸を含むアームの先端位置に第５の軸が位置し、
制御サイクル毎に、
ウィビング成分加算前のツール先端位置の逆キネマティクスを解いて少なくとも前記第１から第３及び前記第６の軸の角度を算出し、
前記ウィビング成分加算前のツール先端位置にウィビング成分を加算してウィビング加算後の仮のツール先端位置を求め、
前記アームの先端位置を中心とし、前記アームの先端位置からツール先端までの距離を半径とする球面を求め、
少なくとも前記仮のツール先端位置を使用して、前記球面上にウィビング成分加算後の真のツール先端位置を求め、
前記前記ウィビング成分加算後の真のツール先端位置から前記第４及び第５の軸の関節角度を算出し、
前記第１から第６の軸の関節角度を前記マニピュレータに指令として出力して前記第１から第６の軸を駆動する
ことを特徴とする溶接ロボットの制御方法。
【請求項２】
前記ウィビング成分加算後の真のツール先端位置は、前記ウィビング成分加算後の仮のツール先端位置と前記アームの先端位置とを通る直線が前記球面と交差する交点であることを特徴とする、請求項１に記載の溶接ロボットの制御方法。
【請求項３】
前記ウィビング成分加算後の真のツール先端位置は、前記ウィビングの方向と、溶接の進行方向とにより決まる平面と前記球面との交円のうち前記ウィビング成分加算後の仮のツール先端の位置に最も近い点であることを特徴とする、請求項１に記載の溶接ロボットの制御方法。
【請求項４】
前記ウィビング成分加算後の真のツール先端位置は、前記ウィビングの方向と、重力の方向とにより決まる平面と前記球面との交円のうち前記ウィビング成分加算後の仮のツール先端の位置に最も近い点であることを特徴とする、請求項１に記載の溶接ロボットの制御方法。
【発明の詳細な説明】【技術分野】
【０００１】
本発明は溶接ロボットの制御方法に関する。特に、本発明は、６軸の垂直多関節型の溶接ロボットにツールを左右に振動させながら溶接するウィビング動作を実行させるための制御方法に関する。
【背景技術】
【０００２】
図９を参照すると、アーク溶接ロボットのウィビング動作は、溶接線に対してトーチ１を左右に振動させながら溶接を行うもので、教示によって指定された直線・円弧等の移動軌跡上のＰｃ（ｔ）に対してウィビング成分Ｗ（ｔ）を加算して実現されるのが一般的である。即ち、ウィビング成分加算後のトーチ１の先端位置Ｐ（ｔ）は以下の式（１）で表される。
【０００３】
【数１】


【０００４】
ウィビング成分Ｗ（ｔ）としては、図１０に示す正弦波の他、三角波等が使用され、教示データ等で指定されたパラメータ（周波数や振動等）に従って、時系列で変化するウィビング並進成分（Ｗｘ，Ｗｙ，Ｗｚ）が算出される。ロボットに対して設定された直交座標系（ベース座標系）において、位置Ｐｃ（ｔ）におけるロボット先端の位置及び姿勢は、以下の式（２）で表される。
【０００５】
【数２】


【０００６】
ここでＰｃｘ、Ｐｃｙ、ＰＣＺはベース座標系の直交軸上の位置、Ｐｃα、Ｐｃβ、Ｐｃγは直交軸に対する回転角度である。
【０００７】
同様に、ベース座標系におけるウィビング成分加算後のロボット先端位置Ｐ（ｔ）は以下の式（３）で表される。
【０００８】
【数３】


【０００９】
従って、ベース座標系におけるウィビング並進成分Ｗ（ｔ）を（Ｗｘ，Ｗｙ，Ｗｚ）とすると、ロボット先端位置Ｐ（ｔ）は以下の式（４）で表される。
【００１０】
【数４】


【００１１】
かかるウィビング動作を図１１に示すような一般的な垂直多関節型ロボットに行わせると、全ての関節Ｊ１〜Ｊ６がウィビング周波数で揺動することになり、ウィビング周波数が高い場合（一般的には５〜１０kHz以上）、アームを駆動する比較的低剛性の３軸（関節Ｊ１〜Ｊ３）に発振が生じやすく、その結果トーチ先端位置の精度確保が困難となり、正確な溶接作業を行うことができなるという問題があった。
【００１２】
この問題を解決してより高周波数でのウィビングを可能とする方法としては、低剛性の基本３軸（図１１の関節Ｊ１〜Ｊ３）ではウィビング動作を実行せずに、より高剛性の手首軸（図１１の関節Ｊ４〜Ｊ６）のみでウィビング動作を実行することが知られている。この方法は、例えば特許文献１及び２に開示されている。
【００１３】
特許文献１の方法では、トーチの長手方向の延長線が手首軸の関節Ｊ４〜Ｊ６の回転中心（図１１に示すアーム先端位置Ｐａｒｍ）と交差するように、３軸（関節Ｊ４〜Ｊ６）でウィビングを行う。しかし、特殊な形状のトーチが必要であり、かつ使用できるアームの姿勢も限定されることとから、作業領域が極めて狭くなり実用的でない。
【００１４】
これに対し、特許文献２の方法では、トーチの形状は限定されず、手首軸のうち最先端の軸（関節Ｊ６）を除く、２軸（関節Ｊ４，Ｊ５）でウィビング動作を実行している。演算手法は概ね以下の通りである。
【００１５】
１）ウィビング加算前の位置Ｐｃ（ｔ）の逆キネマティクスを解き、全関節Ｊ１〜Ｊ６の関節角度を求め。
２）ウィビング動作に使用する２軸（関節Ｊ４，Ｊ５）が微小角度回転した際の方向ベクトルａ４，ａ５を求める。
３）ウィビング成分Ｗを方向ベクトルａ４，ａ５で分解し、関節Ｊ４，Ｊ５の微小角度変化ΔＪ４，ΔＪ５を求める。
４）微小角度変化ΔＪ４，ΔＪ５を１）で求めた関節Ｊ４，Ｊ５の関節角度に加算する。
【００１６】
しかしながら、特許文献２の方法は、手順２）でウィビング成分を該当２軸（関節Ｊ４，Ｊ５）に分解する際に、複雑な座標変換やベクトル演算が必要となり、実時間性確保のためには高価な演算装置（ＣＰＵ）が必要となる。また、ウィビング動作パターンとしては、正弦波や三角波等の単純な式に限定され、任意のパターンを操作者がプログラムによって設定する場合や、溶接開先やギャップの変動に応じて周波数や振幅をリアルタイムに変化させる場合もあり、これら種々のケース毎に演算手段を用意する必要がある。
【００１７】
【特許文献１】特開１−２７３６７４号公報
【特許文献２】特開１１−５８０１４号公報
【発明の開示】
【発明が解決しようとする課題】
【００１８】
前記従来の溶接ロボットの制御方法の問題に鑑み、本発明は、特殊な形状のトーチを使用することなく第４及び第５の軸の２軸のみによるウィビング動作を実現でき、かつ演算が比較的な演算で種々のウィビング動作パターンに対応可能できる溶接ロボットの制御方法を提供することを目的とする。
【課題を解決するための手段】
【００１９】
本発明は、６軸の垂直多関節型の溶接ロボットにウィビング動作を実行させるための制御方法であって、前記溶接ロボットは、基本軸として第１から第３の軸を備え、ツールを保持する手首軸として第４から第６の軸を備えると共に、前記第１から第４の軸を含むアームの先端位置に第５の軸が位置し、制御サイクル毎に以下の手順を実行する。
・ウィビング成分加算前のツール先端位置の逆キネマティクスを解いて少なくとも前記第１から第３及び前記第６の軸の角度を算出する。
・前記ウィビング成分加算前のツール先端位置にウィビング成分を加算してウィビング加算後の仮のツール先端位置を求める。
・前記アームの先端位置を中心とし、前記アームの先端位置からツール先端までの距離を半径とする球面を求める。
・少なくとも前記仮のツール先端位置を使用して、前記球面上にウィビング成分加算後の真のツール先端位置を求める。
・前記前記ウィビング成分加算後の真のツール先端位置から前記第４及び第５の軸の関節角度を算出する。
・前記第１から第６の軸の関節角度を前記マニピュレータに指令として出力して前記第１から第６の軸を駆動する。
【００２０】
具体的には、前記ウィビング成分加算後の真のツール先端位置は、前記ウィビング成分加算後の仮のツール先端位置と前記アームの先端位置とを通る直線が前記球面と交差する交点である。
【００２１】
代案としては、前記ウィビング成分加算後の真のツール先端位置は、前記ウィビングの方向と、溶接の進行方向とにより決まる平面と前記球面との交円のうち前記ウィビング成分加算後の仮のツール先端の位置に最も近い点であること。
【００２２】
他の代案としては、前記ウィビング成分加算後の真のツール先端位置は、前記ウィビングの方向と、重力の方向とにより決まる平面と前記球面との交円のうち前記ウィビング成分加算後の仮のツール先端の位置に最も近い点である。
【発明の効果】
【００２３】
本発明の溶接ロボットの制御方法では、アームの先端位置を中心としてツール先端までの距離を半径とする球面を求め、この球面上にウィビング成分加算後の真のツール先端位置を求め、さらにこの真のツール先端位置から第４及び第５の軸の関節角度を算出する。そのため、ツールの延長線が第５の軸と交差する必要はなく、特殊な形状のツールを使用することなく第４及び第５の軸の２軸のみによるウィビング動作を実現できる。また、従来の第１から第６の軸でウィビング動作を実現する制御において算出するツール先端位置を仮のツール先端位置として算出し、それを真の先端位置の算出に使用する点で、演算が比較的な容易で種々のウィビング動作パターンに対応可能でき、前述の従来の制御との切り換えも簡単に行うことができる。
【発明を実施するための最良の形態】
【００２４】
次に、添付図面を参照して本発明の実施形態を詳細に説明する。
【００２５】
（第１実施形態）
図１を参照すると、溶接ロボットは、マニピュレータ１１、制御装置１２、及び教示装置１３を備える。
【００２６】
マニピュレータ１１は先端にツールとしてトーチ１６を備え、その位置及び姿勢を三次元空間内で変化させる。マニピュレータ１１は６軸の垂直多関節型であり、基本軸として第１から第３の軸（回転関節ＲＪｍ１，ＲＪｍ２，ＲＪｍ３）を備え、トーチ１６を保持する手首軸として第４から第６の軸（回転関節ＲＪｍ４，ＲＪｍ５，ＲＪｍ６）を備えると共に、第１から第４の軸（回転関節ＲＪｍ１〜ＲＪｍ５）を含むアームの先端位置Ｐａｒｍに第５の軸（回転関節ＲＪｍ５）が位置している。第１の軸（回転関節ＲＪｍ１）に対して第２及び第３の軸（回転関節ＲＪｍ２，ＲＪｍ３）が直交している。また、第４及び第６の軸（回転関節ＲＪｍ４，ＲＪｍ６）は第５の軸（回転関節ＲＪｍ５）に対して直交している。
【００２７】
制御装置１２は、ＣＰＵ等からなる処理部１４、記憶部１５、及び入出力ポート等を備える。処理部１４は、記憶部１５に記憶されたプログラムに従って、教示装置１３及びマニピュレータ１１の回転関節ＲＪｍ１〜ＲＪｍ６が備える角度センサ（図示せず）等からの入力に基づいて、回転関節ＲＪｍ１〜ＲＪｍ６を駆動する。
【００２８】
教示装置１３を使用して制御装置１２に対して制御に必要な条件が入力される。この条件には、溶接の始点と終点の位置、始点と終点間の補間方法、及びウィビング条件（波形の種類と周波数、振幅等）が含まれる。
【００２９】
次に、制御に使用する座標について説明する。マニピュレータ１１に関し、２つの座標系を設定する。まず、６個の関節角度Ｊ１〜Ｊ６で与えられる回転関節座標系（マニピュレータ座標系）を設定する。マニピュレータ座標系でのマニピュレータ１１の位置及び姿勢（トーチ先端１６ａの位置及び姿勢）を、こられの関節角度Ｊ１〜Ｊ６で表記する。また、三次元空間に対して固定された直交座標系Ｘ−Ｙ−Ｚ（ベース座標系Σbase）を設定する。ベース座標系Σbaseにおけるマニピュレータ１１の位置及び姿勢（トーチ先端１６ａの位置及び姿勢）は、並進成分（Ｐｘ，Ｐｙ，Ｐｚ）とロール・ピッチ・ヨー姿勢角（Ｐα，Ｐβ，Ｐγ）で表記する。さらに、ワーク上でのトーチ先端１６ａの軌跡に関し、三次元空間に対して固定された直交座標系Ｘline−Ｙline−Ｚline（溶接線座標系Σlime）を設定する。溶接座標系Σlineの計算については後に詳述する。
【００３０】
次に、本実施形態と後述する第２及び第３実施形態に共通する本発明の原理について説明する。図２を参照すると、前述のように第４の軸（回転関節ＲＪｍ４）と第５の軸（回転関節ＲＪｍ５）は互いに直交しているので、これらの軸を変化させて移動できるトーチ先端１６ａの位置は球面Ｑとなる（地球儀でいえば「緯度」と「経度」に相当する。）。この球面Ｑの中心はアーム先端位置Ｐａｒｍであり、半径はアーム先端位置Ｐａｒｍからトーチ先端１６ａまでの距離（図１の符号Ｌｗ）である。この距離Ｌｗは第５及び第６の軸（回転関節ＲＪｍ５，ＲＪｍ６）間のリンク長さとツール１６の形状で決定される。第１から第６の軸のすべてをウィビング動作に使用する従来の方法では、個々の時刻ｔについてベース座標系における溶接線の中心軌跡のＰｃ（ｔ）に対してベース座標系におけるウィビング成分Ｗ（ｔ）を加算することでトーチ先端１６ａの位置を求めている。この場合ウィビング成分加算前のトーチ先端１６ａの位置Ｐｃ（ｔ）は必ず球面Ｑ上に存在するが、ウィビング成分加算後のトーチ先端１６ａの位置（Ｐ（ｔ）＝Ｐｃ（ｔ）＋Ｗ（ｔ））は一般に球面Ｑ上には存在しない。そこで、本発明ではこの位置Ｐ（ｔ）、すなわち仮のトーチ先端１６ａの位置から球面Ｑ上の点の位置ＰＱ、すなわち真のトーチ先端１６ａの位置を求め、この位置ＰＱから第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）の角度を算出している。そして、第１から第３の軸及び第６の軸（回転関節ＲＪｍ１〜ＲＪｍ３，ＲＪｍ６）については位置Ｐ（ｔ）から求めた関節角度Ｊ１〜Ｊ３，Ｊ６をそのまま指令値として使用し、第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）については位置ＰＱから求めた関節角度Ｊ４，Ｊ５を指令値として使用している。これにより第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）のみにより高周波ウィビングを可能としている。球面Ｑ上の位置を決定する方法は各実施形態毎で異なっている。
【００３１】
なお、前述の特許文献２に記載の方法では、第４及び第５の軸が微小角度回転した際の方向ベクトルを用いて第４及び第５の軸の関節角度の近似解を求めている。これに対して本発明では、アーム先端位置Ｐａｒｍを中心として球面Ｑをもとに順幾何学的に第４及び第５の軸の関節角度の厳密解を求めている。この点で、本発明は特許文献２に記載のものとは計算原理そのものが全く異なっている。
【００３２】
次に、図３Ａ，３Ｂを参照しつつ、図４Ａ，４Ｂのフローチャートに従って本実施形態の制御方法（制御装置１２の処理部１４の動作）を具体的に説明する。
【００３３】
まず、ステップＳ１において記憶部１５から教示された始点及び終点位置Ｐ１，Ｐ２、これらの教示位置Ｐ１，Ｐ２間の補間条件、及びウィビング条件（例えば正弦波、三角波等の動作方式、振幅、周波数等）を読み出す。本実施形態では教示位置Ｐ１，Ｐ２はベース座標系Σbaseにおける並進成分（Ｐｘ，Ｐｙ，Ｐｚ）とロール・ピッチ・ヨー姿勢角（Ｐα，Ｐβ，Ｐγ）で与えられる。また、補間条件としては、ウィビング成分加算前のトーチ先端１６ａの位置Ｐｃ（ｔ）が等速直線移動する。さらに、ウィビング条件としては、並進成分の振幅がΔＷで周波数ｆの正弦波でウィビング動作を行う。
【００３４】
ステップＳ２において、ベース座標系における教示位置Ｐ１，Ｐ２間の経路方程式（下記の式（５））を求める。
【００３５】
【数５】


【００３６】
式（５）においてΔＰは単位時間当たりの移動量である。この式（５）で与えられる位置Ｐｃ（ｔ）は個々の時刻ｔにおけるウィビング成分加算前のトーチ先端１６ａの位置である。
【００３７】
次に、ステップＳ３において溶接線座標系Σlineを求める。本発明では、Ｘ座標の方向を溶接進行方向への単位ベクトルＸｌｉｎｅ、Ｙ座標の方向を単位ベクトルＸｌｉｎｅと重力方向単位ベクトルｇの外積として得られる単位ベクトルＹｌｉｎｅ、Ｚ座標の方向を単位ベクトルＸｌｉｎｅと単位ベクトルＹｌｉｎｅの外積として得られる単位ベクトルＺｌｉｎｅとする。これらの単位ベクトルＸｌｉｎｅ，Ｙｌｉｎｅ，Ｚｌｉｎｅは以下の式（６）で表される。
【００３８】
【数６】


【００３９】
また、ステップＳ４においてベース座標系Σbaseから見た溶接線座標系Σlineの３×３の回転行列baseＲｌｉｎｅを求める。
【００４０】
次に、ステップＳ５において、時刻ｔを初期化する（ｔ＝０）。
【００４１】
ステップＳ６からステップＳ１７は、溶接ロボットの制御サイクルΔｔ（通常１０〜２０ｍｓ）毎に繰り返される。まず、ステップＳ６において時刻ｔを時刻ｔ＋Δｔに更新する。
【００４２】
ステップＳ７において経路方程式Ｐｃ（ｔ）に更新した時刻ｔを代入してウィビング加算前の位置Ｐｃ（ｔ）を計算する。
【００４３】
ステップＳ８では、溶接線座標系Σlineでのウィビング並進成分lineＷ（ｔ）（＝（ｗｘ，ｗｙ，ｗｚ））を算出する。本実施形態では前述した正弦波のウィビングパターンを採用しているので、ウィビング並進成分lineｗ（ｔ）は以下の式（７）で表される。
【００４４】
【数７】


【００４５】
次に、ステップＳ９において、回転行列baseＲｌｉｎｅを使用して溶接線座標系Σlineでのウィビング並進成分lineＷ（ｔ）を回転変換し、ベース座標系Σbaseにおけるウィビング並進成分Ｗ（ｔ）（＝（Ｗｘ，Ｗｙ，Ｗｚ））を算出する。ベース座標系Σbaseにおけるウィビング並進成分Ｗ（ｔ）は以下の式（８）で表される。
【００４６】
【数８】


【００４７】
続いて、ステップＳ１０において、以下の式（９）に示すようにウィビング成分加算前のトーチ先端１６ａの位置Ｐｃ（ｔ）にウィビング並進成分Ｗ（ｔ）を加算し、ウィビング成分加算後のトーチ先端１６ａの位置Ｐ（ｔ）を求める。このトーチ先端１６ａの位置Ｐ（ｔ）は本発明における「仮のツール先端位置」に相当する。
【００４８】
【数９】


【００４９】
ステップＳ１１では、ウィビング加算前の位置Ｐｃ（ｔ）の逆キネマティクスを計算して位置Ｐｃ（ｔ）での各軸の関節角度Ｊｃ（Ｊｃ１，Ｊｃ２，Ｊｃ３，Ｊｃ４，Ｊｃ５，Ｊｃ６）を計算する。
【００５０】
ステップＳ１２では、基本３軸（回転関節ＲＪｍ１〜ＲＪｍ３）の回転角度Ｊｃ１，Ｊｃ２，Ｊｃ３の順キネマティクスを計算し、アーム先端位置Ｐａｒｍとアーム方向を示す回転行列baseＲａｒｍを求める。Ｊｃ２３＝Ｊｃ２＋Ｊｃ３、回転関節ＲＪｍ２，ＲＪｍ３間のリンク長をＬ２、回転関節ＲＪｍ３，ＲＪｍ５間のリンク長をＬ３とすると、アーム先端位置Ｐａｒｍは以下の式（１０）で表され、アーム方向を示す回転行列baseＲａｒｍは以下の式（１１）で表される。
【００５１】
【数１０】


【００５２】
【数１１】


【００５３】
ステップＳ１３では、アーム先端位置Ｐａｒｍ（＝（Ｘａｒｍ，Ｙａｒｍ，Ｚａｒｍ））を中心としてアーム先端位置Ｐａｒｍからトーチ先端１６ａまでの距離Ｌｗを半径とする球面Ｑの方程式を求める。球面Ｑの方程式は以下の式（１２）の通りである。
【００５４】
【数１２】


【００５５】
次に、ステップＳ１４において、アーム先端位置Ｐａｒｍ及びウィビング加算後の位置Ｐ（ｔ）を通る直線と球面Ｑとの交点ＰＱを求める。この交点ＰＱは本発明における「ウィビング成分加算後の真のツール先端位置」に相当する。
【００５６】
次に、ステップＳ１５において、交点ＰＱ、アーム先端位置Ｐａｒｍ、及びアーム方向を示す回転行列baseＲａｒｍから関節ＲＪｍ４，ＲＪｍ５の関節角度Ｊ４’，Ｊ５’を計算する。この関節角度Ｊ４’，Ｊ５’の算出方法は後述の第２実施形態に関して詳述する。
【００５７】
ステップＳ１６ではすべての軸（回転関節ＲＪｍ１〜ＲＪｍ６）に対して関節角度を指令値として出力し、マニピュレータ１１を動作させる。具体的には、基本３軸（回転関節ＲＪｍ１〜ＲＪｍ３）及び手首の最先端の軸（回転関節ＲＪｍ６）に対しては、ステップＳ１１において算出したウィビング加算前の位置Ｐｃ（ｔ）の逆キネマティクスにより算出した関節角度Ｊｃ１〜Ｊｃ３，Ｊｃ６を出力する。これに対して第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）に対しては、ステップＳ１５において交点ＰＱから算出した関節角度Ｊ４’，Ｊ５’を出力する。
【００５８】
ステップＳ１７において教示された終点位置Ｐ２に到着するまで、ステップＳ６からステップＳ１６の処理が繰り返される。
【００５９】
このように、本実施形態では比較的低剛性の基本３軸（回転関節ＲＪｍ１〜ＲＪｍ３）を高いウィビング周波数で揺動させることなく、手首の第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）のみでウィビング動作を行うことができ、より高周波なウィビング動作が可能である。また、トーチ１６の延長線が第５の軸と交差する必要はなく、特殊な形状のトーチ１６を使用することなく第４及び第５の軸の２軸のみによるウィビング動作を実現できる。さらに、従来の第１から第６の軸でウィビング動作を実現する制御において算出するツール先端位置を仮のトーチ先端１６ａの位置として算出し（ステップＳ１０）、それをウィビング動作の対象となる第４及び第５の軸の関節角度を得るための真の先端位置の算出（ステップＳ１４）に使用しており、この点で、演算が比較的な容易で種々のウィビング動作パターンに対応可能でき、従来の第１から第６の軸のすべてを使用してウィビング動作を実現する制御との切り換えも簡単に行うことができる。例えば、ウィビング動作の周波数が低い場合には従来の方法を使用し、ウィビング動作の周波数が高い場合には本実施形態の方法を採用することができる。
【００６０】
本実施形態では球面Ｑ上において仮のトーチ先端１６ａの位置と最も近接した位置である交点ＰＱによりウィビング動作を行う第４及び第５の軸の関節角度Ｊ４’，Ｊ５’を決定しているので、従来の第１から第６の軸のすべてを使用してウィビング動作を実現する場合と比較した誤差が最小となる。
【００６１】
（第２実施形態）
図５Ａ，５Ｂに示す本発明の第２実施形態は、第４及び第５の関節角度Ｊ４’，Ｊ５’の算出に使用する球面Ｑ上の交点ＰＱの決定方法（図４ＢのステップＳ１４参照）のみが第１実施形態と異なる。すなわち、第２実施形態では、溶接線座標系ΣlineのＹ座標の方向Ｙｌｉｎｅ（ウィビングの幅方向）とＺ座標の方向Ｚlineとを含む平面Ｐｗを考える。そして、この平面Ｐｗと球面Ｑの交円Ｃ上で最も仮のトーチ先端１６ａの位置Ｐ（ｔ）に近い点を交点ＰＱに決定し、この交点ＰＱを使用して関節角度Ｊ４’，Ｊ５’を計算する。
【００６２】
本実施形態の方法では、従来の第１から第６の軸のすべてを使用してウィビング動作を実現する場合と比較した誤差は第１実施形態の場合よりも大きくなるが、溶接進行方向（溶接線座標系ΣlineのＸｌｉｎｅ方向）に対してウィビング動作の向きが必ず直角となる。
【００６３】
次に、図６Ａ〜６Ｄを参照して交点ＰＱの算出について詳述する。以下の説明では、特に言及しない限りベース座標系Σbaseを基準とする。
【００６４】
前述のように第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）のみでウィビング動作を実行すると、トーチ先端１６ａの描く軌跡はアーム先端位置Ｐarm(以下の説明では点Ｇと称する。)を中心とし、点Ｇとトーチ先端１６ａを結ぶ線分を半径とする球面Ｑ上に位置する。ここで溶接線座標系ΣlineのＸ座標の方向（溶接進行方向）ＸｌｉｎｅとＺ座標の方向Ｚｌｉｎｅのなす平面をＳとし、球面Ｑと平面Ｓの交円をＣとする。
【００６５】
Ｇ点から平面Ｓ上に降ろした垂線の足を点Ｈとする。このとき線分ＧＨの長さｈは、点Ｇから点Ｅ（平面Ｓ上で点ＨからＺ座標の方向Ｚｌｉｎｅの方向に延びる直線と交円Ｃの交点）へのベクトル（ベクトルＧＥ）と溶接線方向の単位ベクトルＸｌｉｎｅ（＝（ｖｘ，ｖｙ，ｖｚ））の内積である。従って、点Ｇの座標を（ｘ５，ｙ５，ｚ５）、点Ｅの座標を（ｘｅ，ｙｅ，ｚｅ）とすると、長さｈは以下の式（１３）で表される。
【００６６】
【数１３】


【００６７】
この長さｈを用いて、点Ｈの座標（ｘｈ，ｙｈ，ｚｈ）は以下の式（１４）で表される。
【００６８】
【数１４】


【００６９】
点Ｈは交円Ｃの中心座標であり、交円Ｃの半径は線分ＨＥである。ここで線分ＧＥの長さをｌ（定数）とすると、三平方の定理より線分ＨＥの長さは以下の式（１５）で表される。
【００７０】
【数１５】


【００７１】
仮のトーチ先端１６ａの先端の位置Ｐ（ｔ）がウィビング幅方向の目標値として与えられ、線分ＥＰ（ｔ）の長さをｕとする。位置Ｐ（ｔ）の座標（ｘｒ，ｙｒ，ｚr）は、ウィビング幅方向の単位ベクトルＹline（＝（ｗｘ，ｗｙ，ｗｚ））を用いて以下の式（１６）により表される。
【００７２】
【数１６】


【００７３】
直線ＧＰ（ｔ）と球面Ｑの交点が求める交点ＰＱでる。交点ＰＱは点Ｈから見てベクトルＨＰ（ｔ）の方向にあるので、交点ＰＱの座標は未知数ｄ（ｄ＞０）を使用して以下の式（１７）のように表される。
【００７４】
【数１７】


【００７５】
式（１６），（１７）により線分ＨＷの長さの２乗は以下の式（１８）のように表される。
【００７６】
【数１８】


【００７７】
一方、式（１５）より線分ＨＥの長さ（線分ＨＷの長さと等しい）の２乗は以下の式（１９）であらわされる。
【００７８】
【数１９】


【００７９】
従って、式（１８），（１９）の右辺どうしが等しいことを利用すると、変数ｄは以下の式（２０）のように表される。
【００８０】
【数２０】


【００８１】
一方、式（１４）と式（１６）を式（１７）に代入すると、交点ＰＱの座標（ｘｗ，ｙｗ，ｚｗ）は以下の式（２１）のように表される。
【００８２】
【数２１】


【００８３】
式（２０）を式（２１）に代入すれば、交点ＰＱ、すなわち真のトーチ先端１６ａの位置が求まる。
【００８４】
なお、以上の計算は図６Ｃのように仮のトーチ先端１６ａの位置Ｐ（ｔ）が交円Ｃの外側に位置する場合に限定されず、図６Ｄに示すように位置Ｐ（ｔ）が交円Ｃの内側に位置する場合にも成立する。
【００８５】
次に、式（２１）で得られた交点ＰＱから第４及び第５の軸（回転関節ＲＪｍ４，ＲＪｍ５）の関節角度Ｊ４’，Ｊ５’を計算する方法（ステップＳ１５）について、図７Ａ〜７Ｃを参照して詳述する。図７Ｂは図７Ａを矢印Ａで示すように第５の軸（回転関節ＲＪｍ５）の回転軸方向から見た図である。ここで第３の軸（回転関節ＲＪｍ３）の座標系（直交座標系）をΣ３とし、方向余弦Ｘ３（ｅｘ，ｅｙ，ｅｚ）、Ｙ３（ｆｘ，ｆｙ，ｆｚ）、Ｚ３（ｇｘ，ｇｙ，ｇｚ）を図７Ｂに示すように設定する。
【００８６】
点ＧからΣ３座標系のＸ軸方向に直線を延ばし、この直線に対して交点ＰＱから降ろした垂線の足を点Ｎとする。第５の軸（回転関節ＲＪｍ５）の関節角度Ｊ５’は線分ＧＮと線分ＧＰＱのなす角であるので、以下の式（２２）が成立する。
【００８７】
【数２２】


【００８８】
この式（２２）より関節角度Ｊ５’が求められるが、第４の軸（回転関節ＲＪｍ４）の関節角度Ｊ４’によって関節角度Ｊ５’は正にも負にもなり得る。しかし、トーチ先端１６ａが特異点（関節角度Ｊ５’が０となる点）を通らないと仮定し、前回の制御サイクル時に検出した回転関節ＲＪｍ５の関節角度が正であれば、今回の制御サイクルにおける関節角度Ｊ５’を正とし、逆に前回の制御サイクル時に検出した回転関節ＲＪｍ５の関節角度が負であれば今回の制御サイクルにおける関節角度Ｊ５’も負とすることにより、符号を決定できる。
【００８９】
次に、第４の軸（回転関節ＲＪｍ４）の関節角度Ｊ４’を求める。線分ＧＮの長さをｎとすると以下の式（２３）が成立する。
【００９０】
【数２３】


【００９１】
この長さｎにより点Ｎの座標（ｘＮ，ｙＮ，ｚＮ）は以下の式（２４）で表される。
【００９２】
【数２４】


【００９３】
また、線分ＮＷの長さは三平方の定理より以下の式（２５）で表される。
【００９４】
【数２５】


【００９５】
図７Ｂを点Ｎから方向余弦Ｘ３に沿って点Ｇを見た図を図７Ｃに示す。
【００９６】
ここで図７Ｃに示すように角度αと角度βを定めると、以下の式（２６），（２７）が成立する。
【００９７】
【数２６】


【００９８】
【数２７】


【００９９】
式（２６），（２７）の右辺のパラメータはすべて既知であるので、これらの式から角度α，βを算出できる。また、第５の軸の関節角度Ｊ５’の正負に対して以下の式（２８）〜（３１）に関係が成立し、これらから第４の軸の関節角度Ｊ４’が得られる。
【０１００】
【数２８】


【０１０１】
【数２９】


【０１０２】
第２実施形態のその他の構成及び作用は第１実施形態と同様である（図１、図４Ａ，４Ｂ参照）。
【０１０３】
（第３実施形態）
図８Ａ，８Ｂに示す本発明の第３実施形も、第４及び第５の関節角度Ｊ４’，Ｊ５’の算出に使用する球面Ｑ上の交点ＰＱの決定方法（図４ＢのステップＳ１４参照）のみが第１実施形態と異なる。すなわち第３実施形態では、溶接線座標系ΣlineのＹ座標の方向Ｙｌｉｎｅ（ウィビングの幅方向）と重力ｇの逆向きのベクトルＧｉｎｖで決定される平面Ｐｗ’を考える。そして、この平面Ｐｗ’と球面Ｑの交円Ｃ上で最も仮のトーチ先端１６ａの位置Ｐ（ｔ）に近い点を交点ＰＱに決定し、この交点ＰＱを使用して関節角度Ｊ４’，Ｊ５’を計算する。
【０１０４】
本実施形態の方法では、従来の第１から第６の軸のすべてを使用してウィビング動作を実現する場合と比較した誤差は第１実施形態の場合よりも大きくなるが、ウィビング動作は重力ｇに対して常に直角となる。
【０１０５】
第３実施形態のその他の構成及び作用は第１実施形態と同様である（図１、図４Ａ，４Ｂ参照）。
【図面の簡単な説明】
【０１０６】
【図１】本発明の第１実施形態に係るロボットを示す模式図。
【図２】本発明の原理を説明するためのマニピュレータの手首部分の模式的な斜視図。
【図３Ａ】本発明の第１実施形態の制御方法を説明するためのマニピュレータの手首部分の模式的な斜視図。
【図３Ｂ】本発明の第１実施形態の制御方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。
【図４Ａ】本発明の第１実施形態の制御方法を説明するためのフローチャート。
【図４Ｂ】本発明の第１実施形態の制御方法を説明するためのフローチャート。
【図５Ａ】本発明の第２実施形態の制御方法を説明するためのマニピュレータの手首部分の模式的な斜視図。
【図５Ｂ】本発明の第２実施形態の制御方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。
【図６Ａ】第２実施形態における交点ＰＱの座標の算出方法を説明するためのマニピュレータの手首部分の模式的な斜視図。
【図６Ｂ】第２実施形態における交点ＰＱの座標の算出方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。
【図６Ｃ】第２実施形態における交点ＰＱの座標の算出方法を説明するためのマニピュレータの手首部分を線分ＨＧ方向から見た模式図（位置Ｐ（ｔ）が交円Ｃの外側に位置する場合）。
【図６Ｄ】交点ＰＱの座標の算出方法を説明するためのマニピュレータの手首部分を線分ＨＧ方向から見た模式図（位置Ｐ（ｔ）が交円Ｃの内側に位置する場合）。
【図７Ａ】交点ＰＱから第４及び第５の軸の関節角度を算出する方法を説明するためのマニピュレータの手首部分の模式的な斜視図。
【図７Ｂ】交点ＰＱから第４及び第５の軸の関節角度を算出する方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。
【図７Ｃ】交点ＰＱから第４及び第５の軸の関節角度を算出する方法を説明するためのマニピュレータの手首部分を線分ＨＧ方向に見た模式図。
【図８Ａ】本発明の第３実施形態の制御方法を説明するためのマニピュレータの手首部分の模式的な斜視図。
【図８Ｂ】本発明の第３実施形態の制御方法を説明するためのマニピュレータの手首部分を第５の関節の回転方向から見た模式図。
【図９】従来のウィビング動作の制御方法を説明するための模式図。
【図１０】ウィビング動作パターンの一例を示すグラフ。
【図１１】一般的な垂直多関節型ロボットを示す模式図。
【符号の説明】
【０１０７】
１１ マニピュレータ
１２ 制御装置
１３ 教示装置
１４ 処理部
１５ 記憶部
１６ トーチ
１６ａ トーチ先端
ＲＪｍ１，ＲＪｍ２，ＲＪｍ３，ＲＪｍ４，ＲＪｍ５，ＲＪｍ６ 回転関節
【出願人】 【識別番号】０００００１１９９
【氏名又は名称】株式会社神戸製鋼所
【出願日】 平成１８年８月２９日（２００６．８．２９）
【代理人】 【識別番号】１０００６２１４４
【弁理士】
【氏名又は名称】青山 葆

【識別番号】１０００７３５７５
【弁理士】
【氏名又は名称】古川 泰通

【識別番号】１００１００１７０
【弁理士】
【氏名又は名称】前田 厚司

【識別番号】１００１１１０３９
【弁理士】
【氏名又は名称】前堀 義之
【公開番号】 特開２００８−５５５１８（Ｐ２００８−５５５１８Ａ）
【公開日】 平成２０年３月１３日（２００８．３．１３）
【出願番号】 特願２００６−２３２１４９（Ｐ２００６−２３２１４９）