JP2008055518A - 溶接ロボットの制御方法 - Google Patents

Abstract

【課題】溶接ロボットにおいて比較的簡易な計算で第４及び第５の軸の２軸のみによるウィビング動作を実現する。
【解決手段】ウィビング成分加算前のトーチ先端１６ａの位置Ｐ_ｃ（ｔ）にウィビング成分を加算してウィビング加算後の仮のトーチ先端１６ａの位置Ｐ（ｔ）を求める。アームの先端位置Ｐ_ａｒｍを中心とし、仮のトーチ先端１６ａまでの距離を半径とする球面Ｑを求める。少なくとも仮のツール先端位置Ｐ（ｔ）を使用して、球面Ｑ上にウィビング成分加算後の真のトーチ先端１６ａの位置となる交点Ｐ_Ｑを求める。交点Ｐ_Ｑから第４及び第５の軸の関節角度Ｊ４’，Ｊ５’を算出する。
【選択図】図２

Description

本発明は溶接ロボットの制御方法に関する。特に、本発明は、６軸の垂直多関節型の溶接ロボットにツールを左右に振動させながら溶接するウィビング動作を実行させるための制御方法に関する。

図９を参照すると、アーク溶接ロボットのウィビング動作は、溶接線に対してトーチ１を左右に振動させながら溶接を行うもので、教示によって指定された直線・円弧等の移動軌跡上のＰ_ｃ（ｔ）に対してウィビング成分Ｗ（ｔ）を加算して実現されるのが一般的である。即ち、ウィビング成分加算後のトーチ１の先端位置Ｐ（ｔ）は以下の式（１）で表される。

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

ここでＰ_ｃｘ、Ｐ_ｃｙ、Ｐ_ＣＺはベース座標系の直交軸上の位置、Ｐ_ｃα、Ｐ_ｃβ、Ｐ_ｃγは直交軸に対する回転角度である。

同様に、ベース座標系におけるウィビング成分加算後のロボット先端位置Ｐ（ｔ）は以下の式（３）で表される。

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

かかるウィビング動作を図１１に示すような一般的な垂直多関節型ロボットに行わせると、全ての関節Ｊ１〜Ｊ６がウィビング周波数で揺動することになり、ウィビング周波数が高い場合（一般的には５〜１０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のＹ座標の方向Ｙ_ｌｉｎｅ（ウィビングの幅方向）と重力ｇの逆向きのベクトルＧ_ｉｎｖで決定される平面Ｐｗ’を考える。そして、この平面Ｐｗ’と球面Ｑの交円Ｃ上で最も仮のトーチ先端１６ａの位置Ｐ（ｔ）に近い点を交点Ｐ_Ｑに決定し、この交点Ｐ_Ｑを使用して関節角度Ｊ_４’，Ｊ_５’を計算する。

本実施形態の方法では、従来の第１から第６の軸のすべてを使用してウィビング動作を実現する場合と比較した誤差は第１実施形態の場合よりも大きくなるが、ウィビング動作は重力ｇに対して常に直角となる。

第３実施形態のその他の構成及び作用は第１実施形態と同様である（図１、図４Ａ，４Ｂ参照）。

本発明の第１実施形態に係るロボットを示す模式図。 本発明の原理を説明するためのマニピュレータの手首部分の模式的な斜視図。 本発明の第１実施形態の制御方法を説明するためのマニピュレータの手首部分の模式的な斜視図。 本発明の第１実施形態の制御方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。 本発明の第１実施形態の制御方法を説明するためのフローチャート。 本発明の第１実施形態の制御方法を説明するためのフローチャート。 本発明の第２実施形態の制御方法を説明するためのマニピュレータの手首部分の模式的な斜視図。 本発明の第２実施形態の制御方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。 第２実施形態における交点Ｐ_Ｑの座標の算出方法を説明するためのマニピュレータの手首部分の模式的な斜視図。 第２実施形態における交点Ｐ_Ｑの座標の算出方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。 第２実施形態における交点Ｐ_Ｑの座標の算出方法を説明するためのマニピュレータの手首部分を線分ＨＧ方向から見た模式図（位置Ｐ（ｔ）が交円Ｃの外側に位置する場合）。 交点Ｐ_Ｑの座標の算出方法を説明するためのマニピュレータの手首部分を線分ＨＧ方向から見た模式図（位置Ｐ（ｔ）が交円Ｃの内側に位置する場合）。 交点Ｐ_Ｑから第４及び第５の軸の関節角度を算出する方法を説明するためのマニピュレータの手首部分の模式的な斜視図。 交点Ｐ_Ｑから第４及び第５の軸の関節角度を算出する方法を説明するためのマニピュレータの手首部分を第５の軸の回転方向から見た模式図。 交点Ｐ_Ｑから第４及び第５の軸の関節角度を算出する方法を説明するためのマニピュレータの手首部分を線分ＨＧ方向に見た模式図。 本発明の第３実施形態の制御方法を説明するためのマニピュレータの手首部分の模式的な斜視図。 本発明の第３実施形態の制御方法を説明するためのマニピュレータの手首部分を第５の関節の回転方向から見た模式図。 従来のウィビング動作の制御方法を説明するための模式図。 ウィビング動作パターンの一例を示すグラフ。 一般的な垂直多関節型ロボットを示す模式図。

符号の説明

１１ マニピュレータ
１２ 制御装置
１３ 教示装置
１４ 処理部
１５ 記憶部
１６ トーチ
１６ａ トーチ先端
ＲＪ_ｍ１，ＲＪ_ｍ２，ＲＪ_ｍ３，ＲＪ_ｍ４，ＲＪ_ｍ５，ＲＪ_ｍ６ 回転関節

Claims (4)

1. ６軸の垂直多関節型の溶接ロボットにウィビング動作を実行させるための制御方法であって、
   前記溶接ロボットは、基本軸として第１から第３の軸を備え、ツールを保持する手首軸として第４から第６の軸を備えると共に、前記第１から第４の軸を含むアームの先端位置に第５の軸が位置し、
   制御サイクル毎に、
   ウィビング成分加算前のツール先端位置の逆キネマティクスを解いて少なくとも前記第１から第３及び前記第６の軸の角度を算出し、
   前記ウィビング成分加算前のツール先端位置にウィビング成分を加算してウィビング加算後の仮のツール先端位置を求め、
   前記アームの先端位置を中心とし、前記アームの先端位置からツール先端までの距離を半径とする球面を求め、
   少なくとも前記仮のツール先端位置を使用して、前記球面上にウィビング成分加算後の真のツール先端位置を求め、
   前記前記ウィビング成分加算後の真のツール先端位置から前記第４及び第５の軸の関節角度を算出し、
   前記第１から第６の軸の関節角度を前記マニピュレータに指令として出力して前記第１から第６の軸を駆動する
   ことを特徴とする溶接ロボットの制御方法。
2. 前記ウィビング成分加算後の真のツール先端位置は、前記ウィビング成分加算後の仮のツール先端位置と前記アームの先端位置とを通る直線が前記球面と交差する交点であることを特徴とする、請求項１に記載の溶接ロボットの制御方法。
3. 前記ウィビング成分加算後の真のツール先端位置は、前記ウィビングの方向と、溶接の進行方向とにより決まる平面と前記球面との交円のうち前記ウィビング成分加算後の仮のツール先端の位置に最も近い点であることを特徴とする、請求項１に記載の溶接ロボットの制御方法。
4. 前記ウィビング成分加算後の真のツール先端位置は、前記ウィビングの方向と、重力の方向とにより決まる平面と前記球面との交円のうち前記ウィビング成分加算後の仮のツール先端の位置に最も近い点であることを特徴とする、請求項１に記載の溶接ロボットの制御方法。

Images