JP3884928B2

JP3884928B2 - 多関節ロボットの姿勢算出方法および姿勢算出装置

Info

Publication number: JP3884928B2
Application number: JP2001208539A
Authority: JP
Inventors: 俊之近藤
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2001-07-09
Filing date: 2001-07-09
Publication date: 2007-02-21
Anticipated expiration: 2021-07-09
Also published as: JP2003019682A

Description

【０００１】
【発明の属する技術分野】
本発明は、多関節ロボットの姿勢算出方法および姿勢算出装置に関し、特に、多関節ロボットの先端部の位置および傾斜角度が決まっているときに、各関節の姿勢値を算出する多関節ロボットの姿勢算出方法および姿勢算出装置に関する。
【０００２】
【従来の技術】
工場の製造ラインでは、溶接、塗装および組み立て作業等に多関節ロボットが採用されている。
【０００３】
通常、多関節ロボットの先端部には作業に応じたエンドエフェクタ（作業ツール）が取り付けられている。エンドエフェクタが空間上の任意の位置において任意の姿勢をとることを可能にするためには空間上の位置を示すＸ、Ｙ、Ｚ座標で表される３つの自由度と、傾斜角度を示すロール、ピッチ、ヨーで表される３つの自由度との合計６つの自由度が必要であり、これを実現するために、多関節ロボットは６つの関節を有しているものが多い。
【０００４】
また、多関節ロボットが広範に採用されるのに従い、生産ラインに対しての高密度配置によるライン長あたりの生産性の向上が望まれ、１つの工程内で複数の多関節ロボットが作業を分担しながら動作する事例や、ワークや治具、搬送装置等の混在する中で多関節ロボットが作業を行うといった事例が増えている。この場合、多関節ロボット同士やその他の干渉物が、特に関節部分で接触することがないように、十分に注意を払って動作のプログラムまたはティーチングを行う必要がある。
【０００５】
【発明が解決しようとする課題】
ところで、多関節ロボットの動作をプログラミングまたはティーチングする際には、エンドエフェクタの位置および傾斜角度は予め与えられていることが多い。例えば、溶接作業であれば溶接のポイントと被溶接板が設定されている向きとにより、エンドエフェクタの位置および傾斜角度も決定される。
【０００６】
このような場合、多関節ロボットの各関節の姿勢を決定するためには、エンドエフェクタの位置および傾斜角度を与えられたパラメータとして、各関節の姿勢を求める方程式を導出してこれを解くこととなる。
【０００７】
ところが、多関節ロボットの構造が、図１１に示すようなオフセット量Ｗを持つときには関節の姿勢を求める方程式を解析的に解くことはできないということが知られている。このように、解析的に姿勢を求めることができない構造の多関節ロボットについては、数値解法を適用することになり、各関節について適当な姿勢値を与えて、その姿勢値を初期値としてニュートン・ラフソン（Newton-Raphson）法などの収束計算を行う方法がある。この方法により、所望のエンドエフェクタの位置および傾斜角度が得られるように収束させて各関節についての姿勢値を求めることができる。
【０００８】
また、高密度配置による多関節ロボット同士や、その他の干渉物との接触を回避する観点からは、なるべく多くの姿勢を求めておけば選択肢が多くなるので望ましいが、収束計算においては、１つの初期値から１つの姿勢しか求めることができない。例えば、適当なエンドエフェクタの位置および傾斜角度に対して、理論上、１６通りの姿勢をとり得る多関節ロボットがあるとして、この１６通りの姿勢を収束計算によって求めるためには１６組の初期値が必要である。しかし、この１６組の初期値をどのように選定すればよいのかを示す具体的な手法がない。
【０００９】
つまり、初期値の選び方によっては、異なる初期値であっても同一解である姿勢に収束してしまったり、収束速度が遅い、または収束しないなどの不都合が発生することがある。
【００１０】
本発明はこのような課題を考慮してなされたものであり、先端部の位置および傾斜角度から他の部分の姿勢を解析的に特定することのできない構造の多関節ロボットに対して、また、高密度に配置された多関節ロボット同士やその他の干渉物に対して、他の部分の姿勢を収束計算により求めようとするとき、その収束計算を実行するための初期値として、適切な近似姿勢を算出することを可能にする多関節ロボットの姿勢算出方法および姿勢算出装置を提供することを目的とする。
【００１１】
【課題を解決するための手段】
本発明に係る多関節ロボットの姿勢算出方法は、６つの関節を有する多関節ロボットにおける先端部の位置および傾斜角度から前記関節の姿勢値を算出する多関節ロボットの姿勢算出方法において、前記関節から選択した２または３の関節の姿勢変数と、前記先端部の位置または傾斜角度とを関係付ける第１方程式を導出する第１方程式導出過程と、前記第１方程式が近似的に成立するための前記選択した２または３の関節の姿勢変数の近似姿勢値を算出する第１算出過程と、近似姿勢値が既に求められている関節の姿勢変数の少なくとも１つと、さらにその他の関節のうち１つの姿勢変数とを関係付ける第２方程式を導出する第２方程式導出過程と、前記第２方程式を用いて、前記その他の関節のうち１つの近似姿勢値を求める第２算出過程と、前記第２方程式導出過程と前記第２算出過程を繰り返し、６つの関節の近似姿勢値を算出する第３算出過程と、前記第２算出過程と前記第３算出過程にて求めた６つの関節の近似姿勢値を初期値として、前記先端部の位置および傾斜角度を満たす前記関節の姿勢値を収束計算により算出する第４算出過程とを有し、前記第１算出過程では、前記選択した２または３の関節について可動範囲を所定の微小区分に分割し、前記微小区分ごとの組み合わせについて前記第１方程式が近似的に成立するか否かを調査し、誤差の小さい組み合わせを理論的に存在する解の数だけ選択することにより、前記選択した２または３の関節の姿勢変数についての前記近似姿勢値を算出することを特徴とする。
【００１２】
これにより、多関節ロボットの先端部に取り付けられたエンドエフェクタ等の位置および傾斜角度から他の関節部分の姿勢を解析的に特定することのできない構造の多関節ロボットに対して、収束計算により他の関節部分の姿勢を求めようとするとき、その収束計算を実行するための初期値として、適切な近似姿勢を算出することができる。
【００１９】
また、本発明に係る姿勢算出装置は、６つの関節を有する多関節ロボットにおける先端部の位置および傾斜角度から前記関節の姿勢値を算出する多関節ロボットの姿勢算出装置において、前記関節から選択した２または３の関節の姿勢変数と、前記先端部の位置または傾斜角度とを関係付ける第１方程式と、近似姿勢値が既に求められている関節の姿勢変数の少なくとも１つと６つの関節の姿勢変数のうち前記第１方程式に含まれない姿勢変数との関係をそれぞれ表す複数の第２方程式とを記憶する記憶部と、前記第１方程式が近似的に成立するための前記選択した２または３の関節の姿勢変数の近似姿勢値を算出する第１算出部と、前記第２方程式を用いて、前記その他の関節のうち１つの近似姿勢値を求める第２算出部と、前記第２算出部を繰り返し実行させ、６つの関節の近似姿勢値を算出する第３算出部と、前記第３算出部にて求めた６つの関節の近似姿勢値を初期値として、前記先端部の位置および傾斜角度を満たす前記関節の姿勢値を収束計算により算出する手段とを有し、前記第１算出部は、前記選択した２または３の関節について可動範囲を所定の微小区分に分割し、前記微小区分ごとの組み合わせについて前記第１方程式が近似的に成立するか否かを調査し、誤差の小さい組み合わせを理論的に存在する解の数だけ選択することにより、前記選択した２または３の関節の姿勢変数についての前記近似姿勢値を算出することを特徴とする。
【００２０】
【発明の実施の形態】
以下、本発明に係る多関節ロボットの姿勢算出方法および姿勢算出装置に適用した実施の形態を図１〜図１０を参照しながら説明する。
【００２１】
本実施の形態は、基本的に、６軸（６つの関節）をもつ多関節ロボットから２つの関節を選択し、この２つの関節とエンドエフェクタの位置またはエンドエフェクタの傾斜角度との関係を示す方程式を導出して、この方程式を満たす近似姿勢値を検索するものである。求まった近似姿勢値に基づいて、他の関節の近似姿勢値も算出した後、これらの近似姿勢値を初期値として収束計算を実施することにより多関節ロボットの姿勢を求める。
【００２２】
図１に示すように、本実施の形態において用いるロボットシステム１０は、機枠１２に高密度に配置された４台の多関節ロボット１４からなるロボット群１６と、ロボット群１６の動作、姿勢等を算出するティーチング装置（多関節ロボットの姿勢算出装置）１８と、ティーチング装置１８で作成したデータに基づいてロボット群１６を制御するロボット制御部２０とを有する。
【００２３】
４台の多関節ロボット１４は全て同じ構造であり、図２に示すように、機枠１２に設定された基台４０の中心軸である軸Ｊ１に対して回転可能なベース部４２と、ベース部４２の先端部横方向の軸Ｊ２を中心にして揺動可能な第１アーム部４４と、第１アーム部４４に対して伸縮可能な第２アーム部４６と、第２アーム部４６の軸芯である軸Ｊ４を中心にして回転可能な第１手首部４８と、第１手首部４８の先端部横方向の軸Ｊ５を中心にして揺動可能な第２手首部５０と、第２手首部５０の軸芯である軸Ｊ６を中心にして回転可能な先端部５２と、先端部５２に取り付けられたエンドエフェクタ５４とから構成される。なお、以下の説明では、多関節ロボット１４のうち機枠１２側を後端側と規定し、後端からエンドエフェクタ５４へ向かう側を先端側と規定する。
【００２４】
エンドエフェクタ５４は、溶接を行うための、所謂、Ｃ型ガンユニットであり、開閉する一対の溶接電極５６ａ、５６ｂを有する。この溶接電極５６ａ、５６ｂが閉じて接するポイントが溶接打点ＴＣＰ（Tool Center Point）であり、軸Ｊ６上に設定されている。また、溶接打点ＴＣＰを基準にして、軸Ｊ６上の先端側を向く単位ベクトルをベクトルＶとして規定する。このベクトルＶは、エンドエフェクタ５４の傾斜角度を示す。
【００２５】
多関節ロボット１４の各部の寸法を次のように規定する。すなわち、基台４０の後端部から軸Ｊ２までの距離をＬ₁、軸Ｊ１と軸Ｊ４との幅方向の距離をＬ₂、第２アーム部４６が最も縮退した状態における軸Ｊ２と軸Ｊ５との距離をＬ₃、軸Ｊ４と軸Ｊ６との幅方向の距離をＬ₄、軸Ｊ５と溶接打点ＴＣＰとの距離をＬ₅とする。
【００２６】
さらに、軸Ｊ１、Ｊ２、Ｊ４、Ｊ５およびＪ６におけるベース部４２、第１アーム部４４、第１手首部４８、第２手首部５０および先端部５２の回転、揺動角度（姿勢変数）をθ₁、θ₂、θ₄、θ₅およびθ₆とする。また、第２アーム部４６の第１アーム部４４に対する伸縮量をλ₃とする。
【００２７】
基台４０の端部で軸芯（軸Ｊ１）と交差する点を原点Ｏと規定し、この原点Ｏを基準として先端側をＺ軸、多関節ロボット１４の幅方向をＹ軸、Ｙ軸およびＺ軸に垂直で図２の紙面の手前方向をＸ軸（図３参照）と規定する。
【００２８】
図３に示すように、ベクトルＶを原点Ｏに平行移動すると仮定したとき、ベクトルＶがＸ−Ｙ平面、Ｙ−Ｚ平面、Ｚ−Ｘ平面に映し出される写像ベクトルをそれぞれ、α₆（α_6X、α_6Y、α_6Z）、β₆（β_6X、β_6Y、β_6Z）およびγ₆（γ_6X、γ_6Y、γ_6Z）とする。ここで、括弧内の３つのパラメータはそれぞれの写像ベクトルのＸ座標成分、Ｙ座標成分、Ｚ座標成分である。
【００２９】
図４に示すように、ティーチング装置１８は、制御部７０と、表示装置としてのモニタ７２と、入出力装置としてのキーボード７４およびマウス７６とを備える。制御部７０は、制御手段としてのＣＰＵ７８と、記憶部であるＲＯＭ８０およびＲＡＭ８２と、ハードディスク８４と、外部機器との入出力制御を行うインターフェース８６と、外部記録媒体８８ａを制御する記録媒体ドライブ８８と、ティーチングデータを算出および作成するデータ算出部９０と、ティーチングデータに基づきモニタ７２の画面上でシミュレーションを行うシミュレーション部９２とを有する。このシミュレーション部９２は、３次元ＣＡＤをベースにしており多関節ロボット１４のモデルを作成したり、該モデル相互の干渉（接触等）を調査する機能を持つ。
【００３０】
図５に示すように、データ算出部９０は、エンドエフェクタ５４の姿勢および傾斜角度とから、姿勢変数θ₁およびθ₄の１６組の近似値を算出する第１算出部９０ａと、姿勢変数θ₁およびθ₄に基づいて他の姿勢変数について近似値を算出する第２算出部９０ｂと、第１および第２算出部９０ａ、９０ｂで求めた姿勢変数の近似値を初期値として収束計算を行う収束計算部９０ｃを有する。
【００３１】
多関節ロボット１４の姿勢を算出する方法の説明に先立ち、溶接打点ＴＣＰの位置を算出する方法について説明する。
【００３２】
溶接打点ＴＣＰの位置座標をＰ₆（ｐ_6X、ｐ_6Y、ｐ_6Z）としたとき、この位置座標Ｐ₆は、姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆および距離Ｌ₁〜Ｌ₅によって一意的に決まるものであり、各揺動軸および伸縮軸における同時変換行列を乗算することによって求めることができる。
【００３３】
軸Ｊ１、Ｊ２、第２アーム部４６の伸縮軸、軸Ｊ４、Ｊ５およびＪ６における同時変換行列Ａ₁、Ａ₂、Ａ₃、Ａ₄、Ａ₅、Ａ₆はそれぞれ以下のように表される。
【００３４】
【数１】

【００３５】
【数２】

【００３６】
【数３】

【００３７】
【数４】

【００３８】
【数５】

【００３９】
【数６】

【００４０】
また、溶接打点ＴＣＰの位置座標Ｐ₆および傾斜角度α₆、β₆、γ₆をそれぞれ転置行列で表すと、次の（７）式〜（１０）式を得る。
【００４１】
Ｐ₆＝［ｐ_6X、ｐ_6Y、ｐ_6Z］^T …（７）
α₆＝［α_6X、α_6Y、α_6Z］^T …（８）
β₆＝［β_6X、β_6Y、β_6Z］^T …（９）
γ₆＝［γ_6X、γ_6Y、γ_6Z］^T …（１０）
さらに、この表現方法を用いると、傾斜角度α₆、β₆、γ₆と同時変換行列Ａ₁、Ａ₂、Ａ₃、Ａ₄、Ａ₅およびＡ₆との関係は次の（１１）式のように表される。
【００４２】
【数７】

【００４３】
このように同時変換行列Ａ₁、Ａ₂、Ａ₃、Ａ₄、Ａ₅およびＡ₆を用いると、溶接打点ＴＣＰの位置座標Ｐ₆および傾斜角度α₆、β₆、γ₆を算出することができる。
【００４４】
次に、溶接打点ＴＣＰの位置座標Ｐ₆および傾斜角度α₆、β₆、γ₆が与えられているときに、多関節ロボット１４の姿勢変数、すなわち、θ₁、θ₂、λ₃、θ₄〜θ₆を算出する方法について図６〜図１０を参照しながら説明する。
【００４５】
まず、ステップＳ１において、（１１）式を展開して次の連立方程式である（１２）式〜（２３）式をたてる。なお、以下の式ではｓｉｎθ_NをＳ_N、ｃｏｓθ_NをＣ_N（ただし、Ｎ＝１、２、４、５、６とする。）と簡略化して表記する。
【００４６】

これらの（１２）式〜（２３）式は、Ｌ₄の、所謂、オフセット量が「０」であれば、θ₁、θ₂、λ₃、θ₄〜θ₆について解析的に解くことができるが、本実施の形態ではオフセット量に相当するＬ₄が、Ｌ₄≠０であることから、解析的に解くことはできない。よって、次のステップＳ２以降の数値解法を行ってθ₁、θ₂、λ₃、θ₄〜θ₆を解く。
【００４７】
ステップＳ２において、（１２）式〜（２３）式のうち適当な式を選択して、互いの式の関係から６つの姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆のうち４つを消去し、２つの姿勢変数の式を導出する。
【００４８】
例えば、（１４）式、（１５）式、（１８）式および（１９）式を選択すると、Ｓ₂、Ｓ₄、Ｓ₅、Ｃ₂、Ｃ₅が消去され、２つの姿勢変数θ₁およびθ₄で定まるＳ₁、Ｃ₁およびＣ₄を含む次の（２４）式（第１方程式）を得る。この（２４）式はＲＡＭ８２に記憶する。
【００４９】
ｐ_6XＳ₁−ｐ_6YＣ₁＝Ｌ₅（γ_6X−γ_6YＣ₁）−Ｌ₄Ｃ₄＋Ｌ₂ …（２４）
この（２４）式に含まれる姿勢変数はθ₁およびθ₄の２つだけであり、しかも、簡易な関係式になっているので演算が容易である。
【００５０】
次に、ステップＳ３において、（２４）式の姿勢変数θ₁およびθ₄に走査的に数値を代入して誤差を調べる。
【００５１】
具体的には、姿勢変数θ₁およびθ₄の可動範囲をそれぞれ微小区分、例えば１００等分に分割し、これらの微小区分内の代表する数値を、ＲＡＭ８２に記憶した（２４）式に代入する。そして、右辺と左辺との誤差を記録する。図７は、この記録のための書式を模式的に表したものであり、姿勢変数θ₁、θ₄のそれぞれが１００分割され、１００×１００のマトリックス状に形成されている。
【００５２】
次に、ステップＳ４において、（２４）式が近似的に成立する姿勢変数θ₁およびθ₄の組をデータ算出部９０によって算出する。
【００５３】
具体的には、図７の書式に記録した誤差の絶対値が極小値をとる箇所を選択する。このとき、選択する箇所の数は多関節ロボット１４が取りうる姿勢の理論的組み合わせの数だけ選択する。すなわち、図８Ａおよび図８Ｂに示すように、溶接打点ＴＣＰの位置座標Ｐ₆および傾斜角度を示すベクトルＶが同一であっても、θ₁、θ₂、λ₃、θ₄〜θ₆の組み合わせによって多関節ロボット１４が取りうる姿勢は複数通り存在するものであり、本実施の形態の場合は論理的に１６通りの組み合わせが存在することが分かっている。
【００５４】
このことから、図７に示す書式においても、極小値を示す箇所が１６組存在することとなり、これらの１６組の姿勢変数θ₁、θ₄を選択すればよい。
【００５５】
なお、図７の書式で、極小値が連続している場合は、その連続している区画同士は同一の組み合わせとみなす。
【００５６】
多関節ロボット１４の姿勢を求める他の方法としては、前記の連立方程式である（１２）式〜（２３）式に対して６つの姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆の全ての組み合わせを走査するように検索を行う手法も考えられるが、それぞれの関節の可動範囲を１００等分として検索を行うと、その組み合わせは１００⁶通り存在する。これらの組み合わせを比較的複雑な式である（１２）式〜（２３）式に代入しながら検索を行うことは処理負担が非常に大きく、コンピュータを使用しても長時間を要する。
【００５７】
これに対し、前記ステップＳ３およびＳ４の処理は、（２４）式に対して、まずθ₁およびθ₄の２つの姿勢変数だけについて検索を行うもので、組み合わせの数は１００²通りだけで済み、非常に短時間に結果を得ることができる。
【００５８】
次に、ステップＳ５において、θ₁およびθ₄を含み、さらに別の姿勢変数を含む方程式（第２方程式）を導出する。具体的には、（１４）式および（１８）式から次の（２５）式を導出する。
【００５９】
γ_6YＣ₁−γ_6XＳ₁＝Ｓ₄Ｓ₅ …（２５）
この（２５）式を次の（２６）式に変形してＲＡＭ８２に記憶する。
【００６０】
θ₅＝Ｓｉｎ^-1（（γ_6YＣ₁−γ_6XＳ₁）／Ｓ４） …（２６）
次に、ステップＳ６において、データ算出部９０の機能により、ＲＡＭ８２に記憶した（２６）式にステップＳ４で算出した１６組の姿勢変数θ₁、θ₄をそれぞれ代入して解くことにより１６個の姿勢変数θ₅を算出する。
【００６１】
以下、同様にして他の姿勢変数を順次算出する。
【００６２】
具体的には、θ₁、θ₄、θ₅およびθ₆を含む次の（２７）式を（１２）式および（１６）式から導出し、この（１６）式にθ₁、θ₄、θ₅を代入することによりθ₆を算出する。
【００６３】
α_6YＣ₁−α_6XＳ₁＝Ｓ₄Ｃ₅Ｃ₆＋Ｃ₄Ｓ₆ …（２７）
次に、θ₄、θ₆およびθ₂を含む次の（２８）式を（２０）式および（２１）式から導出し、この（２８）式にθ₄、θ₆を代入することによりθ₂を算出する。
【００６４】
α_6Z＋β_6ZＳ₆＋β_6Z＝Ｓ₂Ｓ₄ …（２８）
さらに、θ₂、θ₄、および、λ₃を含む次の（２９）式を（２２）式および（２３）式から導出し、この（２９）式に姿勢変数θ₂、θ₄を代入してλ₃を算出する。
【００６５】
ｐ_6Z＝γ_6ZＬ₅＋Ｌ₄Ｓ₂Ｓ₄＋（Ｌ₃＋λ₃）Ｃ₂＋Ｌ₁ …（２９）
このようにして、（２４）式が略成立する姿勢変数θ₁とθ₄との組み合わせに基づいて、他の姿勢変数θ₂、λ₃、θ₄、θ₅およびθ₆についても１６組のすべてについて順次算出することができる。これらの１６組の姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆は、（２４）式が近似的に成立する条件において求めた近似姿勢値であり、多少の誤差を含んでいる。そこで、次のステップＳ７でこれらの誤差を「０」に収束させる。
【００６６】
ステップＳ７において、ステップＳ５およびＳ６で求めた多関節ロボット１４の姿勢を示す１６組の近似姿勢値としての姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆を初期値として設定し、前記収束計算部９０ｃにより所定の収束計算を適用する。
【００６７】
例えば、ニュートン・ラフソン法を適用するならば、６つの姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆をｑと表し、連立方程式である（１２）式〜（２３）式をｒ＝Ｒ（ｑ）と置いたとき、ヤコビ行列Ｊを次の（３０）式により求める。
【００６８】
Ｊ＝ｄＲ／ｄｑ＝［∂Ｒ_i／∂ｑ_i］ …（３０）
ここで、ｉは繰り返し計算の繰り返し回数を表すパラメータである。
【００６９】
ヤコビ行列Ｊを用いて次の（３１）式を所定回数だけ、ｉ＋１を次のｉに置き換えながら繰り返して行うことにより収束計算を行う。
【００７０】
ｑ_i+1＝ｑ_i＋ｋＪ^-1（ｑ_i）（ｒ−Ｒ（ｑ_i）） …（３１）
ここで、ｋは計算の収束性を改善するための定数である。
【００７１】
これにより、１６組の姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆の誤差をそれぞれ「０」に収束させることができるので、多関節ロボット１４の取りうる姿勢が１６通り求まったこととなる。
【００７２】
なお、ニュートン・ラフソン法は、比較的収束速度が速い数値解法であり、他の収束計算に比較すると高速に算出することが可能となり好適である。ニュートン・ラフソン法を拡張した方法を適用してもよいことはもちろんである。
【００７３】
次に、ステップＳ８において、求まった１６組の姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆、すなわち多関節ロボット１４の１６通りの姿勢をシミュレーション部９２を用いてモニタ７２の画面上に表示させ、４台の多関節ロボット１４同士が接触または干渉することがないかを確認する。そして、１６通りの姿勢のうちから動作上の前後の姿勢や安全性を勘案して最適である姿勢を選定する。
【００７４】
ステップＳ８のシミュレーションについて、図９を参照しながらより具体的に説明すると、操作者は入力装置のキーボード７４及びマウス７６を操作することによってエンドエフェクタ５４の位置座標Ｐ₆と傾斜角度を示すベクトルＶを入力する。入力した位置座標Ｐ₆およびベクトルＶはインターフェース８６を介してデータ算出部９０に伝えられる。そして、このデータ算出部９０では、上述のとおり第１算出部９０ａ、第２算出部９０ｂおよび収束計算部９０ｃの機能によって、多関節ロボット１４の姿勢を１６通り算出する。
【００７５】
次に、算出された多関節ロボット１４の１６通りの姿勢はシミュレーション部９２へ伝えられて、多関節ロボット１４をモデル化した画像をモニタ７２の画面上に表示する。
【００７６】
操作者は、このモデル化した１６通りの画像をモニタ７２の画面上で視認しながら、シミュレーション部９２が持つ干渉確認機能などを用いて、多関節ロボット１４同士および他の構造物との接触、干渉の有無を確認する。そして、１６通りの画像のうち最適であるものを選択する。選択した姿勢は、多関節ロボット１４の正式な姿勢として決定されたことを、入力装置であるキーボード７４，マウス７６により入力し、指定された姿勢の姿勢変数はＲＡＭ８２またはハードディスク８４に記憶されるとともに、ロボット制御部２０にも伝えられ、実機のロボット群１６の制御に利用される。
【００７７】
次に、シミュレーション機能を用いることなく、実際の多関節ロボット１４を直接的に動作させて姿勢の良否を判断する例について図１０を参照しながら説明する。
【００７８】
図１０のロボット制御部２０には、制御部７０のデータ算出部９０と同じ機能をもったデータ算出部９１が組み込まれており、入力装置７９を介して入力されたデータに基づいて多関節ロボット１４の姿勢を算出することができる。またロボット制御部２０は、ロボット群１６の図示しないアクチュエータや角度センサとの入出力処理を行いロボット群１６を制御するインターフェース８７を有する。
【００７９】
操作者は、入力装置７９により、エンドエフェクタ５４の位置座標Ｐ₆と傾斜角度を示すベクトルＶを入力する。入力した位置座標Ｐ₆およびベクトルＶはデータ算出部９１に伝えられ、上述の場合と同様に第１算出部９０ａ、第２算出部９０ｂおよび収束計算部９０ｃの機能によって、多関節ロボット１４の姿勢を１６通り算出する。
【００８０】
次に、算出された多関節ロボット１４の１６通りの姿勢変数はインターフェース８７に伝えられ、その姿勢変数に基づき、多関節ロボット１４を実際に動作させる。操作者は、多関節ロボット１４の実際の動作を直接的に確認し、姿勢の良否を判断する。このとき、多関節ロボット１４の動作は、低速で動作する設定にしておき、多関節ロボット１４同士が接触しそうになったときには、入力装置７９からの指令により停止させるようにするとよい。
【００８１】
このようにして、１６通りの姿勢を実際の動作によって確認して、そのうち最適であると判断したものを入力装置７９から入力指示し、所定の記憶部に記憶するようにする。
【００８２】
このように本実施の形態に係る多関節ロボットの姿勢算出方法および姿勢算出装置によれば、多関節ロボット１４の先端部に設けたエンドエフェクタ５４の位置座標Ｐ₆および傾斜角度を表すベクトルＶが与えられたとき、これらの値と多関節ロボット１４の各関節との関係を表す（１２）式〜（２３）式から、所定の２つの関節の姿勢変数θ₁、θ₄を含む第１方程式を導出する。そして、この第１方程式を近似的に充足する関節の姿勢値を調査、算出するようにしたので、多関節ロボット１４の構造が解析的に解くことができない形状であっても、まず近似的に２つの関節の姿勢値を調査、算出することができる。このとき、調査する組み合わせは各関節の分割数の２乗回だけで済むので、短時間で調査できる。
【００８３】
さらに、求まった２つの関節の姿勢値を用いて、他の関節との関係を表す第２方程式を導出して、この第２方程式から他の関節についても近似姿勢値を順次算出することができる。
【００８４】
また、このようにすると、多関節ロボット１４が理論上取りうる姿勢の数の全ての組み合わせについて各関節の近似姿勢値を求めることができる。
【００８５】
そして、求めた近似姿勢値を初期値としてニュートン・ラフソン法などの収束計算を適用すれば、誤差を「０」に収束させて真の姿勢値を算出することができる。
【００８６】
さらにまた、本実施の形態によれば、収束計算において、初期値としての近似姿勢値は当初から小さい誤差しか含まないので、少ない回数の繰り返し計算で誤差を「０」に収束させることができる。また、初期値としての近似姿勢値は１６通り用意されているが、これらの初期値はそれぞれ個別の姿勢値に収束し、特定の同一姿勢に収束してしまうことがない。従って、多関節ロボット１４が取りうる１６通りの姿勢を全て把握することができる。
【００８７】
上述の実施の形態では、近似計算を行うために最初に用いる第１方程式は、θ₁およびθ₄の２つの関節に関する姿勢変数値を含む形式としたが、選択する変数はθ₁およびθ₄以外であってもよく、また、式の導出が困難である場合は、３つ以上の関節に関する姿勢変数を含む式としてもよい。
【００８８】
また、多関節ロボット１４は、例えば、７軸構造やリンク機構を含む構造など異なる構造であってもよい。
【００８９】
さらに、計算条件として与えられるエンドエフェクタ５４の位置座標Ｐ₆と傾斜角度を示すベクトルＶとは、１通りである必要はなく、エンドエフェクタ５４が移動する経路を連続的に表記しておき、これらの経路に対して連続して多関節ロボット１４の姿勢を算出するようにしてもよい。またこの場合、姿勢の算出とロボット１４の動作をほぼ同時に行い、所謂、リアルタイム制御方式にしてもよい。
【００９０】
さらにまた、この発明に係る多関節ロボットの姿勢算出方法および姿勢算出装置は、上述の実施の形態例に限らず、この発明の要旨を逸脱することなく、種々の構成を採り得ることはもちろんである。
【００９１】
【発明の効果】
以上説明したように、本発明に係る多関節ロボットの姿勢算出方法および姿勢算出装置によれば、、先端部の位置および傾斜角度から他の部分の姿勢を解析的に特定することのできない構造の多関節ロボットに対して、また、高密度に配置された多関節ロボット同士やその他の干渉物に対して、他の部分の姿勢を収束計算により求めようとするとき、その収束計算を実行するための初期値として、適切な近似姿勢を算出することができるという効果が達成される。
【図面の簡単な説明】
【図１】本実施の形態に係るロボットシステムを示す説明図である。
【図２】多関節ロボットの構造を示す説明図である。
【図３】エンドエフェクタの傾斜角度を表すベクトルの説明図である。
【図４】ティーチング装置の構成を示すブロック図である。
【図５】データ算出部の構成を示すブロック図である。
【図６】多関節ロボットの姿勢算出方法の手順を示すフローチャートである。
【図７】２つの姿勢変数について、第１方程式の誤差を記録する書式例を示す説明図である。
【図８】図８Ａは、所定の姿勢をとる多関節ロボットを示す説明図であり、図８Ｂは、エンドエフェクタの姿勢のみが図８Ａの例に等しく、他の関節の姿勢が異なる状態を示す説明図である。
【図９】多関節ロボットの姿勢をシミュレーションによって確認する適用例を示す説明図である。
【図１０】多関節ロボットの姿勢を実際のロボットの動作によって確認する適用例を示す説明図である。
【図１１】多関節ロボットのオフセット量を示す説明図である。
【符号の説明】
１０…ロボットシステム１２…機枠
１４…多関節ロボット１６…ロボット群
１８…ティーチング装置２０…ロボット制御部
４０…基台４２…ベース部
４４…第１アーム部４６…第２アーム部
４８…第１手首部５０…第２手首部
５２…先端部５４…エンドエフェクタ
７０…制御部７８…ＣＰＵ
８２…ＲＡＭ８６、８７…インターフェース
９０、９１…データ算出部９２…シミュレーション部
Ｊ１、Ｊ２、Ｊ４〜Ｊ６…軸Ｏ…原点
ＴＣＰ…溶接打点Ｖ…ベクトル
λ₃…伸縮量

Claims

６つの関節を有する多関節ロボットにおける先端部の位置および傾斜角度から前記関節の姿勢値を算出する多関節ロボットの姿勢算出方法において、
前記関節から選択した２または３の関節の姿勢変数と、前記先端部の位置または傾斜角度とを関係付ける第１方程式を導出する第１方程式導出過程と、
前記第１方程式が近似的に成立するための前記選択した２または３の関節の姿勢変数の近似姿勢値を算出する第１算出過程と、
近似姿勢値が既に求められている関節の姿勢変数の少なくとも１つと、さらにその他の関節のうち１つの姿勢変数とを関係付ける第２方程式を導出する第２方程式導出過程と、
前記第２方程式を用いて、前記その他の関節のうち１つの近似姿勢値を求める第２算出過程と、
前記第２方程式導出過程と前記第２算出過程を繰り返し、６つの関節の近似姿勢値を算出する第３算出過程と、
前記第２算出過程と前記第３算出過程にて求めた６つの関節の近似姿勢値を初期値として、前記先端部の位置および傾斜角度を満たす前記関節の姿勢値を収束計算により算出する第４算出過程と、
を有し、
前記第１算出過程では、前記選択した２または３の関節について可動範囲を所定の微小区分に分割し、前記微小区分ごとの組み合わせについて前記第１方程式が近似的に成立するか否かを調査し、誤差の小さい組み合わせを理論的に存在する解の数だけ選択することにより、前記選択した２または３の関節の姿勢変数についての前記近似姿勢値を算出することを特徴とする多関節ロボットの姿勢算出方法。
６つの関節を有する多関節ロボットにおける先端部の位置および傾斜角度から前記関節の姿勢値を算出する多関節ロボットの姿勢算出装置において、
前記関節から選択した２または３の関節の姿勢変数と、前記先端部の位置または傾斜角度とを関係付ける第１方程式と、近似姿勢値が既に求められている関節の姿勢変数の少なくとも１つと６つの関節の姿勢変数のうち前記第１方程式に含まれない姿勢変数との関係をそれぞれ表す複数の第２方程式とを記憶する記憶部と、
前記第１方程式が近似的に成立するための前記選択した２または３の関節の姿勢変数の近似姿勢値を算出する第１算出部と、
前記第２方程式を用いて、前記その他の関節のうち１つの近似姿勢値を求める第２算出部と、
前記第２算出部を繰り返し実行させ、６つの関節の近似姿勢値を算出する第３算出部と、
前記第３算出部にて求めた６つの関節の近似姿勢値を初期値として、前記先端部の位置および傾斜角度を満たす前記関節の姿勢値を収束計算により算出する手段と、
を有し、
前記第１算出部は、前記選択した２または３の関節について可動範囲を所定の微小区分に分割し、前記微小区分ごとの組み合わせについて前記第１方程式が近似的に成立するか否かを調査し、誤差の小さい組み合わせを理論的に存在する解の数だけ選択することにより、前記選択した２または３の関節の姿勢変数についての前記近似姿勢値を算出することを特徴とする多関節ロボットの姿勢算出装置。