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

Abstract

(57)【要約】 【課題】先端部の位置および傾斜角度から他の部分の姿
勢を解析的に特定することのできない構造の多関節ロボ
ットに対して、また、高密度に配置された多関節ロボッ
ト同士やその他の干渉物に対して、他の部分の姿勢を収
束計算により求めるための初期値として、適切な近似姿
勢を算出する。 【解決手段】多関節ロボットの関節のうち２つを選択
し、この２つの関節の姿勢変数の関係を表す第１方程式
を導出する（ステップＳ２）。第１方程式に数値を走査
的に代入して誤差を記録し（ステップＳ３）、このうち
極小の誤差を示す姿勢変数の組を調査、算出する（ステ
ップＳ４）。さらに、他の姿勢変数との関係をも表す第
２方程式を導出し（ステップＳ５）、この第２方程式か
ら別の姿勢変数を算出する（ステップＳ６）。求まった
姿勢変数の組を初期値として収束計算を行い真の姿勢値
を求める（ステップＳ７）。

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＝Ｃ₅Ｃ₆（Ｃ₁Ｃ₂Ｃ₄−Ｓ₁Ｓ₄）−Ｓ₅Ｃ₁Ｓ₂Ｃ₆−Ｓ₆（Ｓ₄Ｃ₁Ｃ₂＋Ｓ₁ Ｃ₄） …（１２） β_6X＝−Ｓ₆Ｃ₅（Ｃ₁Ｃ₂Ｃ₄−Ｓ₁Ｓ₄）＋Ｓ₅Ｓ₆Ｃ₁Ｓ₂−Ｃ₆（Ｓ₄Ｃ₁Ｃ₂＋ Ｓ₁Ｃ₄） …（１３） γ_6X＝Ｓ₅（Ｃ₁Ｃ₂Ｃ₄−Ｓ₁Ｓ₄）＋Ｓ₂Ｃ₁Ｃ₅ …（１４） ｐ_6X＝Ｌ₅Ｓ₅（Ｃ₁Ｃ₂Ｃ₄−Ｓ₁Ｓ₄）＋Ｌ₅Ｃ₁Ｓ₂Ｃ₅−Ｌ₄（Ｓ₄Ｃ₁Ｃ₂＋Ｓ₁ Ｃ₄）＋（Ｌ₃＋λ₃）Ｓ₂Ｃ₁＋Ｌ₂Ｓ₁ …（１５） α_6Y＝（Ｓ₁Ｃ₂Ｃ₄＋Ｓ₄Ｃ₁）Ｃ₅Ｃ₆−Ｓ₁Ｓ₂Ｓ₅Ｃ₆−Ｓ₆（Ｓ₁Ｓ₄Ｃ₂−Ｃ₁ Ｃ₄） …（１６） β_6Y＝−（Ｓ₁Ｃ₂Ｃ₄＋Ｓ₄Ｃ₁）Ｓ₆Ｃ₅＋Ｓ₁Ｓ₂Ｓ₅Ｓ₆−Ｃ₆（Ｓ₁Ｓ₄Ｃ₂− Ｃ₁Ｃ₄） …（１７） γ_6Y＝（Ｓ₁Ｃ₂Ｃ₄＋Ｃ₁Ｓ₄）Ｓ₅＋Ｓ₁Ｓ₂Ｃ₅ …（１８） ｐ_6Y＝Ｌ₅Ｓ₅（Ｓ₁Ｃ₂Ｃ₄＋Ｃ₁Ｓ₄）＋Ｌ₅Ｓ₁Ｓ₂Ｃ₅−Ｌ₄（Ｓ₁Ｓ₄Ｃ₂−Ｃ₁ Ｃ₄）＋（Ｌ₃＋λ₃）Ｓ₁Ｓ₂−Ｌ₂Ｃ₁ …（１９） α_6Z＝−Ｓ₂Ｃ₄Ｃ₅Ｃ₆−Ｓ₅Ｃ₂Ｃ₆＋Ｓ₂Ｓ₄Ｓ₆ …（２０） β_6Z＝Ｓ₂Ｓ₆Ｃ₄Ｃ₅＋Ｓ₅Ｓ₆Ｃ₂＋Ｓ₂Ｓ₄Ｃ₆ …（２１） γ_6Z＝−Ｓ₂Ｓ₅Ｃ₄＋Ｃ₂Ｃ …（２２） ｐ_6Z＝−Ｌ₅Ｓ₂Ｓ₅Ｃ₄＋Ｌ₅Ｃ₂Ｃ₅＋Ｌ₄Ｓ₂Ｓ₄＋（Ｌ₃＋λ₃）Ｃ₂＋Ｌ₁ …（２３） これらの（１２）式〜（２３）式は、Ｌ₄の、所謂、オ
フセット量が「０」であれば、θ₁、θ₂、λ₃、θ₄〜θ
₆について解析的に解くことができるが、本実施の形態
ではオフセット量に相当するＬ₄が、Ｌ₄≠０であること
から、解析的に解くことはできない。よって、次のステ
ップＳ２以降の数値解法を行ってθ₁、θ₂、λ₃、θ₄〜
θ₆を解く。

【００４７】ステップＳ２において、（１２）式〜（２
３）式のうち適当な式を選択して、互いの式の関係から
６つの姿勢変数θ₁、θ₂、λ₃、θ₄〜θ₆のうち４つを
消去し、２つの姿勢変数の式を導出する。

【００４８】例えば、（１４）式、（１５）式、（１
８）式および（１９）式を選択すると、Ｓ₂、Ｓ₄、
Ｓ₅、Ｃ₂、Ｃ₅が消去され、２つの姿勢変数θ₁およびθ
₄で定まるＳ₁、Ｃ₁およびＣ₄を含む次の（２４）式（第
１方程式）を得る。この（２４）式はＲＡＭ８２に記憶
する。

【００４９】 ｐ_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 (8)

【特許請求の範囲】
1. 【請求項１】少なくとも３つの関節を有する多関節ロボ
   ットにおける先端部の位置および傾斜角度から前記関節
   の姿勢値または近似姿勢値を算出する多関節ロボットの
   姿勢算出方法において、 前記関節から選択した複数の関節の姿勢変数と、前記先
   端部の位置または傾斜角度とを関係付ける第１方程式を
   導出する第１方程式導出過程と、 前記第１方程式が近似的に成立するための前記選択した
   関節の姿勢変数の近似姿勢値を算出する第１算出過程
   と、 近似姿勢値を求めることのできる関節の姿勢変数と、さ
   らにその他の関節の姿勢変数とを関係付ける第２方程式
   を導出する第２方程式導出過程と、 前記第１算出過程で算出した近似姿勢値と前記第２方程
   式を用いて、前記その他の関節の近似姿勢値を求める第
   ２算出過程とを有することを特徴とする多関節ロボット
   の姿勢算出方法。
2. 【請求項２】請求項１記載の姿勢算出方法において、 前記第２方程式導出過程と前記第２算出過程を繰り返
   し、前記関節の全ての近似姿勢値を算出することを特徴
   とする多関節ロボットの姿勢算出方法。
3. 【請求項３】請求項２記載の姿勢算出方法において、 前記第２方程式導出過程と前記第２算出過程を繰り返し
   て求めた前記関節の近似姿勢値を初期値として、前記先
   端部の位置および傾斜角度を満たす前記関節の姿勢値を
   収束計算により算出することを特徴とする多関節ロボッ
   トの姿勢算出方法。
4. 【請求項４】請求項３記載の姿勢算出方法において、 前記収束計算には、ニュートン・ラフソン法もしくはそ
   の拡張法を適用して、前記関節の姿勢値を算出すること
   を特徴とする多関節ロボットの姿勢算出方法。
5. 【請求項５】請求項１〜４のいずれか１項に記載の姿勢
   算出方法において、 前記第１方程式導出過程で選択する関節は２つの関節で
   あることを特徴とする多関節ロボットの姿勢算出方法。
6. 【請求項６】請求項１〜５のいずれか１項に記載の姿勢
   算出方法において、 前記第１算出過程では、前記第１方程式が近似的に成立
   するための前記選択した関節の近似姿勢値を、前記多関
   節ロボットが理論的に取りうる姿勢の数だけ算出するこ
   とを特徴とする多関節ロボットの姿勢算出方法。
7. 【請求項７】請求項１〜６のいずれか１項に記載の姿勢
   算出方法において、 前記第１算出過程では、前記選択した関節について可動
   範囲を所定の微小区分に分割し、前記微小区分ごとの組
   み合わせについて前記第１方程式が近似的に成立するか
   否かを調査することにより、前記近似姿勢値を算出する
   ことを特徴とする多関節ロボットの姿勢算出方法。
8. 【請求項８】少なくとも３つの関節を有する多関節ロボ
   ットにおける先端部の位置および傾斜角度から前記関節
   の姿勢値または近似姿勢値を算出する多関節ロボットの
   姿勢算出装置において、 前記関節から選択した複数の関節の姿勢変数と、前記先
   端部の位置または傾斜角度とを関係付ける第１方程式
   と、前記第１方程式から近似値を求めることができる関
   節とその他の関節との関係を表す第２方程式とを記憶す
   る記憶部と、 前記第１方程式が近似的に成立するための前記選択した
   関節の姿勢変数の近似姿勢値を算出する第１算出部と、 前記第１算出部で算出した近似姿勢値を前記第２方程式
   を用いて、前記その他の関節の近似姿勢値を求める第２
   算出部とを有することを特徴とする多関節ロボットの姿
   勢算出装置。