JPH05127725A - 円筒座標ロボツトの駆動制御方法 - Google Patents

Abstract

(57)【要約】 【目的】 連続的且つ滑らかに変化する駆動出力を各ア
ームの駆動機構に与えることができる円筒座標ロボット
の駆動制御方法を提供する。 【構成】 交差角度δ_S1，δ_S2が既知である２点を抽出
し、それらの交差角度δ_S1，δ_S2より、ロボットのトー
チの先端が移動する点に於ける交差角度の近似値δ_a を
求める。更に、交差角度の近似値δ_a ，位置ｐ及び天頂
角θより方位角の近似値φ_a を求める。そして、位置
ｐ，天頂角θ，交差角度の近似値δ_a 及び方位角の近似
値φ_a を用いて、ロボットのトーチの先端が移動する点
において各アームの駆動機構に与えるべき駆動出力
（Θ，ｘ_S ，ｚ_S ，α，β）を求める。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は、産業用ロボット、特
に円筒座標ロボットの駆動制御方法に関するものであ
る。

【０００２】

【従来の技術】産業用ロボットに所望の動作を行わせる
方法として、ロボットのエンドエフェクタの先端がたど
るべき経路上の点を順次ティーチングし、これらのティ
ーチング点を滑らかに結ぶ曲線又は直線で補間する方法
が広く知られている（ティーチングプレイバック）。従
って、この方法を用いてロボットの駆動を制御するに
は、ロボットのエンドエフェクタの先端の位置と姿勢を
表現するＸＹＺ絶対座標系（Ｘ系）からロボットの関節
変位（並進自由度と回転自由度を含む。）を表現する関
節座標系（α系）への座標変換が、即ち逆変換が必要と
なる。しかも、この逆変換をロボットの動作に同期して
実時間で行う必要性があるため、効率の良い逆変換の方
法が求められる。

【０００３】一般にロボットの座標変換には、α系から
ｘ系への順変換と、前述の逆変換がある。前者は、ロボ
ットの軸構成がどのようなものであっても解析的に厳密
解を求めることができることが知られている。同様に、
後者に関しても解析的に厳密解を求める方法が試みられ
ており、例えばＲ．Ｃ．Ｐａｕｌ，“Ｒｏｂｏｔ Ｍａ
ｎｉｐｕｌａｔｏｒｓ；Ｍａｔｈｅｍａｔｉｃｓ，Ｐｒ
ｏｇｒａｍｍｉｎｇａｎｄ Ｃｏｎｔｒｏｌ”ＭＩＴ
Ｐｒｅｓｓ（１９８１）にその具体例が示されている。
即ち、順変換の変換式から代数学的に逆変換の変換式を
求めるわけである。

【０００４】しかし、ロボットの軸構成いかんによって
は厳密な解析解の求まる場合と求まらない場合があり、
一般的には解析的に厳密解を求めることができない場合
が大部分であって、この発明が対象とする円筒座標ロボ
ットにおいても例にもれることなく、逆変換の変換式よ
り代数学的に直接厳密解を求めることができなかった。
そのため、数値解析による手法を用いて逆変換の解を近
似的に求めることが行われていた。

【０００５】そこでかかる問題点を解決すべくなされた
発明が、本出願人の出願に係る特願平２−２６１１８４
号に開示されたものである。この発明の詳細については
後述する「実施例」において述べることとし、ここでは
この発明の概要について触れることとする。

【０００６】即ち、この発明は、まず円筒座標ロボット
の逆変換を求めるに際して「交差角度」なる概念を導入
する。この交差角度とは、エンドエフェクタの軸を含
み、しかもロボット設置面に平行な平面に垂直な平面
が、エンドエフェクタの軸方向を法線方向とする平面と
交差した際に生ずる交線と、エンドエフェクタが取付け
られた円筒座標ロボットの第３のアームの軸とがなす角
度である。そして、逆変換の変換式を交差角度を変数と
する方程式に変換し、この方程式を解くことにより交差
角度を求め、得られた交差角度の値から円筒座標ロボッ
トの各アームの駆動量を求めようとするものであり、こ
れらの駆動量は逆変換の変換式の厳密解となっている。

【０００７】以上の制御方法を、出願人は「幾何学的方
法」又は「ｇ法」と呼んでいる。

【０００８】

【発明が解決しようとする課題】上述した通りｇ法によ
れば、円筒座標ロボットの逆変換の厳密解を容易に求め
ることができ、その結果、エンドエフェクタの先端を指
定した姿勢にて指定位置へ厳密に移動させることができ
る。

【０００９】しかし、本出願人は更に上記ｇ法について
検討した結果、エンドエフェクタの先端の姿勢のうち、
ある限られた範囲内での姿勢にてエンドエフェクタの先
端を移動させる場合には、前述した交差角度δについて
の方程式の解が不連続解となることが判明した。例え
ば、エンドエフェクタの先端の姿勢のうち、天頂角θが
θ＝９０ｄｅｇ．付近の姿勢においては、交差角度δの
値が不連続値となる。

【００１０】もし、この様な不連続解を交差角度δの値
として採用し、なお円筒座標ロボットの駆動制御を続行
するならば、交差角度δが不連続解となる２点間をエン
ドエフェクタの先端が移動する際には、各アームの駆動
量がステップ関数的に急激に大きく変化するため、作業
者が安全にロボットを動作させることができなくなるの
みならず、各アームを駆動すべき移動機構に大きな負荷
が加わる結果、移動機構が破壊されるという事態が生ず
るおそれもある。この様な動作は、ロボットにとっては
好ましくないものである。従って、ｇ法によるロボット
の駆動制御を行ないつつエンドエフェクタ先端を移動さ
せるためには、上記交差角度δの解が不連続解となる領
域を避けて移動させなければロボットを使用できないと
いう制限が課される結果となっていた。そのため、エン
ドエフェクタの先端の姿勢のいかんにかかわらず、常に
逆変換の変換式の厳密解を求めることができ、各アーム
の駆動機構を連続的に且つ滑らかに駆動することができ
る様な駆動制御方法の実現が強く要望されていた。

【００１１】この発明はこの様な要望に応えるべくなさ
れたものであって、エンドエフェクタ先端の姿勢のいか
んにかかわらず、常に連続且つ滑らかに変化する駆動出
力を各アームの駆動機構に与え続けることができる高機
能な円筒座標ロボットの駆動制御方法を提供することを
目的としている。

【００１２】

【課題を解決するための手段】この発明は、円筒座標ロ
ボットでは、エンドエフェクタの先端の位置については
常に正確であることが求められるが、姿勢については或
程度の範囲内なら指定値からずれていても実用上は何ら
問題にはならないであろうという考え方を基礎としてい
る。即ち、エンドエフェクタの先端の姿勢の一方につい
ては指定値通りの値を用い、姿勢の他方には許容範囲内
で積極的に誤差を導入して、各アームの駆動量の値を連
続値とするという思想を用いている。その具体的手段と
しては、次の通りである。

【００１３】この発明の第１の構成では、ロボット設置
面に略垂直に設置された第１アームと、第１アームに結
合してロボット設置面に略平行な平面内に並進自由度を
有するとともに、第１アームに対して定まる所定の平面
内に直接的又は間接的に旋回自由度を有する第２アーム
と、第２アームの先端部に略直角に結合して第２アーム
の軸方向まわりの回転自由度とそれ自身の軸まわりの回
転自由度とを有する第３アームと、第３アームの先端部
に略直角に結合したエンドエフェクタとを有する円筒座
標ロボットの駆動制御方法であって、（ａ）絶対座標系
で表される空間内の所定の２点におけるエンドエフェク
タの先端の位置及び姿勢を指定し、（ｂ）所定の２点そ
れぞれにおいて、エンドエフェクタの軸を含み且つ所定
の平面に垂直な平面がエンドエフェクタの軸方向を法線
方向とする平面と交差する際に生ずる交線と、第３アー
ムの軸方向とがなす交差角度の値を、交差角度を変数と
する方程式を解くことにより求める。次に、（ｃ）ステ
ップ（ｂ）で求めた所定の２点における交差角度の値よ
り、所定の２点間の点における交差角度の値を近似的に
求め、（ｄ）所定の２点間の点におけるエンドエフェク
タの先端の位置と姿勢の一方とを指定するとともに、
（ｅ）ステップ（ｃ）で決定した交差角度の値及びステ
ップ（ｄ）で指定したエンドエフェクタの先端の位置と
姿勢の一方とを用いて、エンドエフェクタの先端の姿勢
の他方を求めるとともに、更に所定の２点間の点におけ
る第１，第２及び第３アームそれぞれの駆動量を決定す
る。そして、（ｆ）各駆動量に応じた駆動出力をそれぞ
れ対応する第１，第２及び第３アームの駆動機構に与え
ることにより、第１、第２及び第３アームの駆動を行う
様にしたものである。

【００１４】また、この発明の第２の構成では、第１の
構成の係る円筒座標ロボットと同一の構成を有するロボ
ットの駆動制御方法において、（ａ）絶対座標系で表さ
れる空間内の各点におけるエンドエフェクタの先端の位
置及び姿勢を指定し、（ｂ）空間内の各点において、エ
ンドエフェクタの軸を含み且つ所定の平面に垂直な平面
がエンドエフェクタの軸方向を法線方向とする平面と交
差する際に生ずる交線と、第３アームの軸方向とがなす
交差角度の値を、交差角度を変数とする方程式を解くこ
とにより求める。次に、（ｃ）ステップ（ｂ）で求めた
空間内の各点における交差角度の値の内、その値が連続
的となる空間内の点については、当該点における交差角
度の値並びにステップ（ａ）で指定した当該点における
エンドエフェクタの先端の位置及び姿勢を用いて、第
１，第２及び第３アームそれぞれの駆動量を決定する。
一方、（ｄ）ステップ（ｂ）で求めた空間内の各点にお
ける交差角度の値の内、その値が不連続となる空間内の
点については、当該点における交差角度の値を、その
値が連続的である空間内の他の２点における交差角度の
値を用いて近似的に求め、その近似的に求めた交差角
度の値並びにステップ（ａ）で指定した当該点における
エンドエフェクタの先端の位置及び姿勢の一方を用い
て、エンドエフェクタの先端の姿勢の他方を新たに求め
るとともに、更に当該点における第１，第２及び第３ア
ームそれぞれの駆動量を決定する様にする。そして、
（ｅ）ステップ（ｃ）又は（ｄ）により決定された各駆
動量に応じた駆動出力を、それぞれ対応する第１，第２
及び第３アームの駆動機構に与えることにより、第１、
第２及び第３アームの駆動を行うこととしたものであ
る。

【００１５】更にこの発明の第３の構成では、ロボット
設置面に略垂直に設置された第１アームと、第１アーム
に結合してロボット設置面に略平行な平面内に並進自由
度を有するとともに、第１アームに対して定まる所定の
平面内に直接的又は間接的に旋回自由度を有する第２ア
ームと、第２アームの先端部に略直角に結合して第２ア
ームの軸方向まわりの回転自由度とそれ自身の軸まわり
の回転自由度とを有する第３アームと、第３アームの先
端部に略直角に結合したエンドエフェクタとを有する円
筒座標ロボットにおいて、エンドエフェクタの先端が絶
対座標系で表される空間内の焦点を通って空間内を移動
する場合の駆動制御方法を対象とする。

【００１６】まず、（ａ）空間内の各点におけるエンド
エフェクタの先端の位置及び姿勢を指定し、（ｂ）エン
ドエフェクタの軸を含み且つ所定の平面に垂直な平面が
エンドエフェクタの軸方向を法線方向とする平面と交差
する際に生ずる交線と第３アームの軸方向とがなす交差
角度の値を、交差角度を変数とする方程式を解くことに
より求める。そして（ｃ）ステップ（ｂ）で求めた交差
角度の値より、エンドエフェクタの先端が焦点を通過し
たか否かを判断する。今、（ｄ）焦点を通過していない
と判断した場合には、ステップ（ａ）で指定したエンド
エフェクタの先端の位置及び姿勢並びにステップ（ｂ）
で求めた交差角度の値を用いて、空間内の点における第
１，第２及び第３アームそれぞれの駆動量を決定する。
一方、（ｅ）焦点を通過したと判断した場合には、ステ
ップ（ａ）で指定した焦点における姿勢の内の天頂角の
値を所定の値だけ変化させた値を、焦点通過後のエンド
エフェクタの先端の位置する点における天頂角の指定値
とし、当該天頂角の指定値並びにステップ（ａ）で指定
した当該点におけるエンドエフェクタの先端の位置及び
姿勢の内の方位角の値を用いて、当該点における交差角
度の値を求めた後、第１，第２及び第３アームそれぞれ
の駆動量を決定する。（ｆ）そしてこの様にして決定し
た各駆動量に応じた駆動出力を、それぞれ対応する第
１，第２及び第３アームの駆動機構に与えることによ
り、第１、第２及び第３アームの駆動を行う。

【００１７】

【作用】この発明の第１の構成では、まず所定の２点に
おけるエンドエフェクタ先端の交差角度の値をそれぞ
れ、それらの点におけるエンドエフェクタ先端の予め指
定された位置及び姿勢に基づいて求める。そして、それ
らの交差角度の値より、エンドエフェクタの先端が移動
すべき点における交差角度の値を近似的に求め、この交
差角度の近似値と予め指定したエンドエフェクタの先端
の位置及び姿勢の一方とを用いて、逆にエンドエフェク
タの先端の姿勢の他方を求める。これにより、エンドエ
フェクタの先端の姿勢の他方には所定の範囲内で誤差が
生ずる。しかし、これらのエンドエフェクタの先端の位
置，姿勢及び交差角度を用いて各アームの駆動量を求め
れば、得られる各駆動量は常に連続的な値となる。

【００１８】又、この発明の第２の構成では、まずエン
ドエフェクタの先端が移動しうる全ての点におけるエン
ドエフェクタの先端の位置及び姿勢を指定し、各点にお
ける交差角度の値が求められる。そのうち、求めた交差
角度の値が連続的となり得る領域内の点へエンドエフェ
クタの先端を移動させる場合には、求められた交差角度
の値及び指定されたエンドエフェクタの先端の位置等か
ら求められた各アームの駆動量を、各アームの移動機構
へ与えて各アームの駆動を制御する。一方、交差角度の
値が不連続的となりうる領域内の点へエンドエフェクタ
の先端を移動させる場合には、当該点における交差角度
の値を、交差角度の値が連続的となりうる領域内の２点
を抽出し、この抽出した２点における交差角度の値よ
り、近似的に求める。そして、この近似値と予め指定さ
れた当該点におけるエンドエフェクタの先端の位置及び
姿勢の一方とを用いて、逆にその点におけるエンドエフ
ェクタの先端の姿勢の他方が求められる。更にこれらの
求められた値を用いて、エンドエフェクタの先端を当該
点へ移動させるために必要な各アームの移動機構に与え
るべき駆動量が決定される。従って、各アームの駆動量
は、常に連続的な値となる。

【００１９】更に、この発明の第３の構成では、エンド
エフェクタの先端が焦点を通過した後の点においては、
エンドエフェクタの先端の位置及び姿勢内の方位角には
その点における指定値をそのまま用い、姿勢の内の天頂
角には焦点における天頂角の指定値を所定の量だけステ
ップ関数的に変えて得られる値を用いることとして、そ
の点における交差角度の値を求め、これらの値より各ア
ームの駆動量を決定する。エンドエフェクタの先端が焦
点を通過する際、第１及び第２アームの駆動量は、エン
ドエフェクタ先端の天頂角の値の変化に対して影響を受
けない。従って、求められる第２及び第３のアームそれ
ぞれの駆動量は常に連続的な値となる。

【００２０】

【実施例】

（Ａ） 装置の機械的構成 図１及び図２は、それぞれこの発明の一実施例である円
筒座標ロボットとしてのレーザ溶接ロボットＲＢの機械
的構成を示す斜視図及び模式的側面図であり、図２には
レーザ溶接ロボットＲＢの各自由度が模式的に表わされ
ている。

【００２１】両図において、ロボットＲＢの設置面１０
上に固定された絶対座標系としてＸ系が定義され、ロボ
ットＲＢの基台１１より第１アーム２１がＺ軸にそって
延びている。この第１アーム２１は、Ｚ軸まわりに、即
ち第１アーム２１の軸Ａ１のまわりに、第１アーム２１
内部に備えつけられた第１の駆動機構によってΘ方向に
旋回することができる。

【００２２】又、第２アーム２２が第１アーム２１に略
直角に交差して備えつけられており、第１アーム２１の
旋回と共にΘ方向に旋回する。従って、第２アーム２２
は、第１アーム２１の旋回を通じて間接的にＸＹ平面内
に旋回自由度を有しているものと言うことができる。し
かも第２アーム２２は、第１アーム２１内部に備えつけ
られた第２の駆動機構によってＺ軸に平行に昇降するこ
とができ、更に第２アーム２２内部に備えつけられた第
３の駆動機構によって、ＸＹ平面内を並進運動すること
ができる。尚、第１アーム２１の軸Ａ１と第２アーム２
２の軸Ａ２との交点ＭのＸ系に対する位置座標を点Ｍ
（０，０，Ｚ_s ）とする。

【００２３】又、第２アーム２２の先端部３１には、第
３アーム２３が第２アーム２２に略直角に結合されてお
り、第３アーム２３は第２アーム２２の先端部３１の内
部に備えつけられた第４の駆動機構によって、第２アー
ム２２の軸Ａ２のまわりのα方向に回転することができ
る。ここで、軸Ａ２と第２アーム２２の先端の点Ｐと点
Ｍとの相対距離を変数ｘ_s で表わす。

【００２４】更に、第３アーム２３は、それ自身の先端
部３２の内部に備えつけられた第５の駆動機構によっ
て、それ自身の軸Ａ３のまわりのβ方向に回転すること
もできる。尚、第３のアーム２３の長さは一定値ｄ₁ で
固定されている。

【００２５】また、エンドエフェクタとしてのトーチ４
０が第３アーム２３の先端部３２に略直角に結合されて
おり、トーチ４０の長さ、即ちトーチ４０の先端の点Ｎ
とトーチ４０の軸Ａ４と第３アーム２３の先端の点Ｑの
距離は、一定値ｄ₂ である。尚、トーチ４０にはワーク
検知用センサ（図示せず）が備えつけられており、トー
チ４０自身は、自身の軸Ａ４を回転軸としてγ方向に回
転することができる。トーチ４０自身はその軸Ａ４まわ
りに完全回転対称性を有しており、そのロール角に相当
する角度γは上記センサの位置を定めるためにのみ意味
を持つ。このため、角度γは座標変換によって決定され
る値ではなく、別個独立に指定すべき角度である。後述
する座標変換において角度γを決定する式がないのはこ
のような事情による。

【００２６】以上述べた各アームの自由度の内、トーチ
４０自身の回転自由度を除いた全ての自由度の範囲を表
１に例示する。尚、表１における記号ｚ₀ は、第２アー
ム２２が基台１１と接した際の点Ｍの高さである。

【００２７】

【表１】

【００２８】一方、図３はトーチ４０の先端（点Ｎ）の
位置及び姿勢を示す説明図であり、ここでは点Ｑを原点
としたｘ系により表わされている。即ち、点Ｎの姿勢は
ベクトルＱＮと、ｚ′軸とのなす角度（天頂角）θ及び
ベクトルＱＮのｘ′ｙ′平面への射影ベクトルとｘ′軸
とのなす角度（方位角）φにより決まる。尚、ｘ′系の
各軸方向は、Ｘ系の各軸方向に対応する様に設定されて
いる。即ち、ｘ′軸，ｙ′軸及びｚ′軸は、それぞれＸ
軸，Ｙ軸及びＺ軸に平行である。又、点Ｎの位置は、Ｘ
系で現わした位置座標（ｘ，ｙ，ｚ）により決まる。 （Ｂ） 装置の電気的構成 図４は、レーザ溶接ロボットＲＢの制御系の電気的構成
を示すブロック図である。この制御系は、ＣＰＵやメモ
リ等からなるコントローラ５１を有しており、ソフト的
に制御される。又、ティーチングボックス５２を操作す
ることによってティーチングデータの取込みなどが行な
われる。

【００２９】一方、第１の駆動機構５３１を構成するモ
ータＭ₁ に駆動信号Ｄ₁ が送信され、モータＭ₁ に取付
けられたエンコーダＥ₁ が第２のアーム２２の軸Ａ１の
まわりの旋回角Θを検出する。同様に、第２〜第５の駆
動機構５３２〜５３５にそれぞれ駆動信号Ｄ₂ 〜Ｄ₅ が
送信され、各モータＭ₂ 〜Ｍ₅ が動作し、変位ｚ_s ，ｘ
_s 及び回転角度α，βがそれぞれエンコーダＥ₂ 〜Ｅ₅
によって検出される。

【００３０】又、レーザ電源５４はレーザ発振器５５に
電力を供給するための電源である。レーザ発振器５５で
発生したレーザビームＬＢは、ロボットＲＢの各アーム
内に設けたミラーによって順次に反射されつつトーチ４
０の先端からワークに向けて照射される。 （Ｃ） ｇ法について 本発明においては、基本的には「従来の技術」の欄で簡
単に述べたｇ法を用いて逆変換を行う。そこで、まず以
下においては、この基本となるｇ法の詳細について述べ
ることとする。尚、このｇ法自身は、新規な発明であ
る。

【００３１】まず、トーチ４０と第２アーム２２及び第
３アーム２３との幾何学的関係を考察する。ここに図７
は、図２で示した４点Ｍ，Ｎ，Ｐ，Ｑの位置関係を、Ｘ
ＹＺ空間において示した立体図である。

【００３２】今、第２アーム２２が軸Ａ１のまわりに角
度Θで旋回し、第３アーム２３が軸Ａ２のまわりに角度
αで回転し、更に軸Ａ３のまわりに角度βで回転した状
態にある場合を考える。

【００３３】同図において、トーチ４０の先端の位置Ｎ
（ｘ，ｙ，ｚ）と姿勢（θ，φ）が与えられると、自動
的に点Ｑの位置も定まる。

【００３４】ここで次の（ｉ）〜（ｉｉｉ）の条件を満
足する円Ｃを定義する。即ち、（ｉ）その中心が点Ｑで
あること。（ｉｉ）半径がｄであること。（ｉｉｉ）ベ
クトルＱＮを法線とする平面内に存在すること。このと
き点Ｐは円Ｃの円周上に存在し、ベクトルＱＰは円Ｃの
半径ベクトルとなっている。そしてロボットＲＢの各軸
構成から明らかなように、点Ｐについて、ベクトルＱＰ
とベクトルＱＮとの間及びベクトルＱＰとベクトルＭＰ
との間には、それぞれ直交関係が成立する。

【００３５】今、ベクトルＱＮを含みＸＹ平面に垂直な
平面Ｓが円Ｃと交差している場合を考え、平面Ｓと円Ｃ
との交点をそれぞれ点Ｒ及び点Ｔとする。そしてベクト
ルＱＲとベクトルＱＰとのなす角度を考え、この角度を
交差角度δと定義する。

【００３６】又、ベクトルＱＮ及び円ＣをＸＹ平面へ射
影する場合を考える。即ち、ベクトルＱＮのＸＹ平面へ
の射影ベクトルをベクトルＱ′Ｎ′、円ＣをＸＹ平面へ
射影した場合にできる楕円を楕円Ｅ′とし、各点Ｐ，
Ｒ，Ｔに対応するＸＹ平面上の各点をそれぞれ点Ｐ′，
Ｒ′，Ｔ′とする。

【００３７】そこで以上の幾何学的関係を基にして、逆
変換の厳密解を求めることとする。まず、第２アーム２
２をＺ軸のまわりに仮想的に旋回することを考え、第２
アーム２２の先端の点が点Ｐにある状態から仮想的な角
度φ（すなわち、トーチ４０の先端の点Ｎの方位角に相
当。尚、図７の例においては、角度Θとは逆まわりとな
っている。）だけ旋回することによって、トーチ４０の
軸Ａ４方向のベクトルＱ₁ Ｎ₁ のＸＹ平面への射影ベク
トルＱ₁′Ｎ₁ ′がＸ軸に平行になったものとする。こ
の状態における円Ｃに対応する円を円Ｃ₁ 、円Ｃ₁ のＸ
Ｙ平面への射影により生じる楕円を楕円Ｅ₁ ′とする。
又、ベクトルＱ₁ Ｎ₁ を含みＸＹ平面に垂直な平面Ｓ₁
が円Ｃ₁ と交差する点を点Ｒ₁ ，Ｔ₁ とし、円Ｃ₁ 上の
３点Ｐ₁ ，Ｒ₁ ，Ｔ₁ の楕円Ｅ₁ ′上の対応する点をそ
れぞれ点Ｐ₁ ′，Ｒ₁ ′，Ｔ₁ ′とする。

【００３８】今、平面Ｓ₁ と円Ｃ₁ を含む平面との交線
を交線ＣＬとすると、ベクトルＰ₁ Ｑ₁ の交線ＣＬ方向
の成分ベクトル（大きさは、ｄ₁ ′ｃｏｓδ）はＸＺ平
面に平行であり、ベクトルＰ₁ Ｑ₁ の面Ｓ₁ の法線方向
の成分ベクトル（大きさは、ｄ₁ ′ｓｉｎδ）はＹ軸に
平行である。従って、点Ｑ₁ 及び点Ｐ₁ の位置座標を点
Ｑ₁ （ｘ₀ ．ｙ₀ ，ｚ₀ ）及び点Ｐ₁ （ｘ．ｙ，ｚ）と
すると、点Ｐ₁ と点Ｑ₁ との関係は、図７においては余
弦ｃｏｓδが負、正弦ｓｉｎδが正であることを考慮し
て数１〜数３により表わされる。

【００３９】

【数１】

【００４０】

【数２】

【００４１】

【数３】

【００４２】尚、余弦，正弦の記載方法としてＣδ＝ｃ
ｏｓδ及びＳδ＝ｓｉｎδの様な簡略形を用いたが、以
後の説明においてもこれらの簡略形を使用する。

【００４３】更に、ベクトルＱ₁ Ｐ₁ とベクトルＭＰ₁
は直交しているので、

【００４４】

【数４】

【００４５】

【数５】

【００４６】の関係式が成立する。従って、数４に数１
〜数３及び数５を代入し、ｘ₀ 及びｙ₀ をｄ₁ により正
規化すれば、数４は

【００４７】

【数６】

【００４８】となる。

【００４９】この数６で表わされる関係式はｇ法におけ
る基本式であって、ｇ法による逆変換の出発点となる式
である。即ち、この数６を変形すれば、交差角度のδの
余弦Ｃδを変数とする解析可能な４次方程式が得られ
る。従って、この４次方程式を解くことにより、交差角
度δの余弦Ｃδが求められる。又、数６を別途変形すれ
ば、交差角度の正弦Ｓδを変数とする解析可能な４次方
程式も得られ、この４次方程式を解いて、交差角度δの
正弦Ｓδが求められる。従って、交差角度δは、数７に
より与えられることとなる。

【００５０】

【数７】

【００５１】ここでａｔａｎ２（ｘ，ｙ）とは、ｘ＝Ｓ
ξ，ｙ＝Ｃξとなる角度ξを一意に与える関数である。

【００５２】以上の考察によって交差角度δの値が求め
られたので、次にこの交差角度δの値を用いて、逆関数
の解（Θ，ｘ_s ，ｚ_s ，α，β）を与える式を導出する
ことを考えるのであるが、その導出の説明については、
前述の特許出願（特願平２−２６１１８４号）に委ねる
として、ここではそれらの結果のみを示すこととする。
即ち、トーチ４０の先端（点Ｎ）の位置ベクトルｐ及び
姿勢ベクトルの一つａが、それぞれベクトルｐ（Ｘ，
Ｙ，Ｚ）及びベクトルａ（ａ_x ，ａ_y ，ａ_z ）として与
えられているものとすれば、旋回角度Θは、

【００５３】

【数８】

【００５４】のように与えられる。尚、数８中の点Ｐ₁
のｘ，ｙ座標値は、それぞれ数１及び数２に数９で与え
られるｘ₀ 及び数１０で与えられるｙを代入することに
より求められる。

【００５５】

【数９】

【００５６】

【数１０】

【００５７】又、数９及び数１０中のｐ_xm，ｐ_ym及びψ
は、それぞれ数１１，数１２及び数１３により与えられ
る量である。

【００５８】

【数１１】

【００５９】

【数１２】

【００６０】

【数１３】

【００６１】又、第２アーム２２のＸＹ平面内の並進自
由度ｘ_s は、

【００６２】

【数１４】

【００６３】により与えられ、回転角度αは、数１５で
与えられる角度αの正弦及び数１６で与えられる角度α
の余弦を用いて、数１７により与えられる。

【００６４】

【数１５】

【００６５】

【数１６】

【００６６】

【数１７】

【００６７】又、並進自由度ｚ_s は数１８により与えら
れ、回転角度βは数１９により与えられる。

【００６８】

【数１８】

【００６９】

【数１９】

【００７０】以上の説明のより明らかな様に、ｇ法と
は、交差角度δなる量を導入し、トーチ４０と各アーム
との幾何学的関係を考察することにより数６で表わされ
る基本式を与え、この基本式を解くことにより交差角度
δの値を算出するとともに、更に、交差角度δと各逆変
換の解との関係を明らかとして、逆変換の厳密解を算出
しようとする方式である。尚、上記一連の手順を整理す
ると、図８に示される様なフローとなる。同図において
（ ）の中の数字は、上述した数式の番号を表示してい
る。

【００７１】（Ｄ） Ｄゾーンについて 上述したｇ法を用いれば、トーチ４０の位置ｐ（ｘ，
ｙ，ｚ）及び姿勢（θ，φ）がいかなる値であっても、
最終的に厳密な逆変換の解を求めることができる様にも
思える。実際、トーチ４０が取り得る姿勢領域内の殆ど
の領域において、逆変換の厳密解が求められる。しか
し、ｇ法を用いて様々な条件でシミュレーションを行な
ってみると、トーチ４０の姿勢のうち方位角θが９０ｄ
ｅｇ．付近では、交差角度δの値が不連続に変化する領
域が存在することが判明した。その様な交差角度δの不
連続領域を示したのが図９である。即ち、図９は、ｇ法
の基本式である数６を、交差角度δを変数とするｙ₀ の
関数形に変形した数２０を、方位角θをパラメータとし
てｘ＝１５００ｍｍ，ｚ＝１０００ｍｍ及びφ＝０の条
件の下で図示した説明図である。

【００７２】

【数２０】

【００７３】同図において、曲線ｆ₁ 〜ｆ₆ は、それぞ
れ方位角θが８６．５ｄｅｇ．，８８．０ｄｅｇ．，８
９ｄｅｇ．，９１ｄｅｇ．，９２．２５ｄｅｇ．および
９３．５ｄｅｇ．である場合のｙ₀ の曲線である。又、
曲線ｆ₀ は方位角θが９０ｄｅｇ．の場合のｙ₀の曲線
である。これらの曲線を概観すると、これらの関数は次
の様な特徴を有していることがわかる。

【００７４】 交差角度δ＞０の範囲では、図中全て
の曲線は点Ｆ₀ （δ＝９０ｄｅｇ．，ｙ₀ ＝１）を通
り、交差角度δ＜０の範囲では、同様に全ての曲線が点
Ｆ₀ ′（δ＝−９０ｄｅｇ．，ｙ₀ ＝−１）を通る。そ
こで、点Ｆ₀ 及び点Ｆ₀ ′を以後、焦点と呼ぶこととす
る。

【００７５】 全ての曲線は、原点に対して点対象と
なっている。

【００７６】 図中、太わくで囲まれた領域（ＤＺ１
〜ＤＺ４）では、ｙ₀ の曲線は単調関数とならず、極小
点又は変曲点を有する様な関数となる。従って、この領
域内では、以下に一例を示す様に交差角度δの値が不連
続に変化することとなる。そこで、以後この４つの不連
続領域ＤＺ１〜ＤＺ４を、総称して「Ｄゾーン」と呼ぶ
こととする。又、各不連続領域ＤＺ１〜ＤＺ４を、それ
ぞれ第１Ｄゾーン〜第４Ｄゾーンと呼んで区別すること
とする。尚、図９における各Ｄゾーンの範囲を具体的に
示すと、表２に示す通りとなる。

【００７７】

【表２】

【００７８】 より、曲線ｆ₀ は丁度Ｄゾーンの漸
近線となる。

【００７９】図９における曲線群の性質は以上の通りで
あるが、以下に上記性質で述べた交差角度δの不連続
値の発生について、具体的に考察する。

【００８０】図１０は、図９に示した曲線群の内、θ＝
８９ｄｅｇ．のｙ₀の曲線，即ち関数ｆ₃ を抜き出して
示した図であり、交差角度δの値に不連続が発生するこ
とを明らかにするための説明図である。今トーチ４０の
先端（点Ｎ）を、その位置ｘ及びｚをそれぞれ１５００
ｍｍ，１０００ｍｍに保ち、且つ天頂角θ＝８９ｄｅ
ｇ．，方位角φ＝０ｄｅｇ．で定まる姿勢を保ちなが
ら、移動させる場合を考える。即ち、トーチ４０の先端
を、ｙ₀₁で与えられる位置からｙ₀₄で与えられる位置ま
で移動させるものとする（点Ｍ₁ →点Ｍ₅ ）。図１０に
示す様に、ｙ₀ ＝ｙ₀₁では交差角度δの解は２個存在
し、ｙ₀ ＝ｙ₀₂では交差角度δの解は３個存在する。
又、ｙ₀ ＝ｙ₀₃及びｙ₀ ＝ｙ₀₄ではそれぞれ交差角度δ
の解は４個及び３個存在する。この様に交差角度δの解
の数は２個〜４個そして３個へと変化するが、この場合
には、交差角度δの値として、δ₁ からδ₄ までの連続
的な値を採用することができるので、交差角度δに不連
続値が生ずることはない。

【００８１】しかし、ｙ₀ ＝１からｙ₀ ＝ｙ₀₄へとトー
チ４０の先端が移動する場合には、交差角度δの値に不
連続的変化が生じる。即ち、ｙ₀ ＝ｙ₀₄では、交差角度
δの解は２個存在するが、そのうち交差角度δ₄ に最も
近い解であるδ₅ においてさえも、その値はδ₅ ＝８ｄ
ｅｇ．であり、δ₄ とδ₅ との差は８２ｄｅｇ．とな
る。この様な交差角度δの変化を示したのが、図１１で
あり、交差角度δのステップ関数的な変化が顕著に現わ
れていることか理解できる。

【００８２】以上説明した様にＤゾーン内の点にトーチ
４０の先端が移動する場合には、交差角度δの値に不連
続的な変化が生じるのであるが、交差角度δの値に急激
な不連続的変化が生じるということは、その値δより導
き出される各逆変換の解にも当然不連続的な変化が生ず
るということであり、この様な急激な各アームの動き
は、アームの駆動部の破壊を引き起こす等、安全性の面
からロボットの動作としては好ましくない。この様な問
題点の解決策として考え出されたのが、ｇ法の改良に係
る「δ法」と本出願人が呼ぶ方法であり、本発明の核心
部をなす部分である。

【００８３】（Ｅ） 動作原理（δ法）について トーチ４０の先端の位置（ｘ，ｙ，ｚ）及び姿勢（θ，
φ）はいずれも厳密でなければならないという考え方に
固執するならば、前述した様にある条件下では各アーム
の駆動量（Θ，ｘ_s ，ｚ_s ，α，β）に必ず不連続が発
生してしまう。

【００８４】しかし、実際の溶接ロボット等において
は、位置（Ｘ，Ｙ，Ｚ）については正確であることが要
求されるが、姿勢（θ，φ）については１０ｄｅｇ．程
度指定値からシフトしたとしても実際上は問題とはなら
ないと考えられる。一方、姿勢θ，φの値のいずれか又
は両方共その値を変えると、数２０で与えられる方程式
の曲線形は、様々に変化する。従って、姿勢（θ，φ）
に許容される範囲内で姿勢（θ，φ）のいずれか一方に
意図的に誤差を導入することにより、交差角度δの解の
不連続性を解消することができるものと考えられる。こ
の様な考え方を前提としたのがδ法であり、δ法には、
方位角φには指定値通りの値を用い、天頂角θには誤差
を導入するというδ−φ法と、δ−φ法とは逆のδ−θ
法がある。まずδ−φ法について詳細を述べることとす
る。

【００８５】（Ｅ−１） δ−φ法について 上述した基本的な考え方に基づけば、δ法では交差角度
δの変化が不連続とならない様に、方位角φの誤差Δφ
を決定するという問題に帰着する。この問題は一般的に
は、数６の微分形式を用いて定式化されるが、この方法
では微分係数の変化が大きいときには解析的に解くこと
ができない。

【００８６】そこで本発明では、逆にＤゾーン内の点に
おける交差角度δの値自身をＤゾーン外の点の既知の交
差角度δの値より近似的に求め、それらの値，即ち交差
角度δの近似値δ_a 並びに天頂角θ及び位置（ｘ，ｙ，
ｚ）の指定値を満足する様な方位角φの近似値φ_a を求
めることとする。

【００８７】ここに図１２は、その様な近似値δ_a の求
め方を示す説明図である。同図は、次の様なケースを取
り扱っている。即ち、トーチ４０の先端は、曲線ｆ₀ 及
び曲線ｆ₆ により形成される第４のＤゾーンＤＺ４を横
切って、第４のＤゾーン外の点Ｐ_s1から同じく第４のＤ
ゾーン外の点Ｐ_s2へ移動するものとする。

【００８８】そこで、第４のＤゾーンＤＺ４内の点Ｐ_i
における交差角度δ_i の値を、ｇ法により連続的な厳密
解として求められた２点Ｐ_s1，Ｐ_s2における交差角度δ
_S1，δ_S2の値を用いて近似的に与えることを考える。即
ち、２点Ｐ_s1，Ｐ_s2を結ぶ線上の点Ｐ_i における交差角
度δ_i の値を、線形近似により求めることとする。この
とき、交差角度δ_i は、一般的には数２１により決定さ
れる。

【００８９】即ち、係数ｋ_1i，ｋ_2iの値を各点Ｐ_i ごと
に定めることにより、交差角度δ_i の値は決定される。

【００９０】

【数２１】

【００９１】但し、本実施例では簡単化のために、２点
Ｐ_s1，Ｐ_s2における交差角度δ_s1，δ_s2の差（δ_s1−δ
_s2）をｎ等分するものとして、点Ｐ_i における交差角度
δ_i の値を求めることとする。従って、第４のＤゾーン
ＤＺ４内には、トーチ４０の先端が移動すべき点がｎ−
１個存在するものとしている。〔ｉ≦（ｎ−１）〕。こ
のとき、交差角度δ_i は、

【００９２】

【数２２】

【００９３】によって与えられる。

【００９４】尚、交差角度δ_i の値を近似する方法とし
ては、上述の線形近似によるのではなく、既知の交差角
度δ_s1，δ_s2の値を与える曲線δ＝ｆ（ｙ₀ ）を適当に
定め、この曲線によって各交差角度δ_i の値を近似して
もよい。又第４のＤゾーンＤＺ４中にティーチング点Ｐ
_s3を取り、３点Ｐ_s1，Ｐ_s2，Ｐ_s3における交差角度
δ_s1，δ_s2，δ_s3より交差角度δ_i の近似値を線形近似
等により求める様にしてもよい。この場合には、交差角
度δ_i の近似値としてより適した近似値が得られ、方位
角φの誤差もより小さくすることができる。

【００９５】更に、２点Ｐ_s1，Ｐ_s2は、必ずしもＤゾー
ンの外にある点でなければならないわけではなく、Ｄゾ
ーンの境界上の点であってもよい。

【００９６】次に、Ｄゾーン内の点Ｐ_i における交差角
度δ_i の近似値δ_a が数２２により求められたものとし
て、点Ｐ_iにおける方位角φ_i の近似値φ_a を与える式
を導出する。今、点Ｐ_i におけるトーチ４０の先端の位
置がＰ_i （ｘ，ｙ，ｚ）、天頂角がθ、交差角度がδ_a
であるとし、数２３及び数２４により定義される量Ｌ_D
及びＬ_E を用いて数２０を表わすならば、数２０は、数
２５で表わされる。

【００９７】

【数２３】

【００９８】

【数２４】

【００９９】

【数２５】

【０１００】ここで、点Ｐ_i における姿勢ベクトルａの
ｘ成分ａ_x 及びｙ成分ａ_y は、それぞれａ_x ＝ＣφＳθ
及びａ_y ＝ＳφＳθで与えられるので、数１１，数１２
より明らかな通り、ｐ_xm及びｐ_ymはともに方位角φを変
数とする関数としてみることができる。従って、数９，
数１０よりｘ₀ 及びｙ₀ も方位角φを変数とする関数と
なる。そこで、数２６により与えられる関数Ｓ（φ）を
考え、関数Ｓ（φ）がＳ（φ′）＝０となる様なφ′を
決定することによって、求めるべき数２５の解、即ち点
Ｐ_i における方位角φ_aを求めることができる。

【０１０１】

【数２６】

【０１０２】尚、この様な解φ′を求めるには、Ｎｅｗ
ｔｏｎ法による収束演算を用いることができる。

【０１０３】以上述べた方法により、交差角度δ_a 及び
方位角φ_a が求められたならば、以後は、ｇ法と同一の
手順によって、点Ｐ_iにおける逆変換の解（Θ，ｘ_s ，
ｚ_s ，α，β）を求めることになる。即ち、数８〜数１
９に交差角度δ_a ，方位角φ_a 、位置（ｘ，ｙ，ｚ）及
び天頂角θを代入することにより、逆変換の解が求めら
れる。

【０１０４】以上述べたδ−φ法における一連の手順を
整理すれば、図１３におけるフローの様になる。同図に
おいて（ ）中の数字は、上記説明中において導出した
式の番号を表わしている。

【０１０５】このδ−φ法を用いれば、方位角φの誤差
を許容範囲内におさえつつ、逆変換の解（Θ，ｘ_s ，ｚ
_s ，α，β）をＤゾーン内においても連続的に変化する
値とすることができる。その様な一例を示したのが、図
１４〜図１６である。即ち、図１４（ａ），（ｂ）〜図
１６（ａ）はそれぞれ、位置のｘ，ｚ座標をそれぞれ１
５００ｍｍ，１０００ｍｍと固定し、且つトーチ４０の
方位角φを常に０ｄｅｇ．に指定した条件の下で、トー
チ４０の先端を点Ａ₂ （ｙ＝３７．５ｍｍ，θ＝８６．
５ｄｅｇ．）から点Ａ₂ ′（ｙ＝２３８ｍｍ，θ＝９４
ｄｅｇ．）へ第４のＤゾーンＤＺ４を横切って移動する
場合にδ−φ法に基づいて求められた逆変換の解Θ，ｘ
_s ，ｚ_s ，α及びβを（実線）、ｇ法に基づいて求めた
場合の逆変換の解（破線）とともに示したシミュレーシ
ョン結果である。また、図１６（ｂ）は、点Ａ₂ から点
Ａ₂ ′へのトーチ４０の先端の移動に際してδ−φ法に
より方位角φを求めた結果生じた方位角φの誤差Δφ
（Δφ＝φ_a ）を示している。図１４（ａ），（ｂ）、
図１５（ａ），（ｂ）及び図１６（ａ）より明らかな通
り、点Ａ₂ から点Ａ₂ ′へトーチ４０の先端を移動する
ために各駆動量をｇ法で決定すれば、点Ａ₂から点
Ａ₂ ′への移動中に各アームの駆動量が急激に大きく変
化する現象が発生するのに対して、本発明のδ−φ法に
よれば、その様な各アームの駆動量の不連続的変化は一
切生じない。即ち、トーチ４０の先端を第４のＤゾーン
ＤＺ４を移動中、常にアームを滑らかに連続的に駆動し
続けることができる。しかも、方位角φの誤差Δφは図
１６（ｂ）に示す通り、最大１．５ｄｅｇ．程度であ
り、十分許容範囲内（Δφ≦１０ｄｅｇ．）に入ってい
る。

【０１０６】以上より、δ−φ法によって各アームの駆
動量を決定するならば、Ｄゾーン内においても実用に的
したロボットＲＢの動作を実現することができ、又、そ
の際生じるトーチ４０の方位角φの誤差も実用上問題と
ならない値である。尚、δ−φ法によっても、トーチ４
０の先端の位置及び天頂角θの値は、誤差のない正確な
値となる。

【０１０７】（Ｅ−２） δ−θ法について δ−φ法については、天頂角θを指定値に等しいとし
て、方位角φに誤差を与えることとしたが、その逆、即
ち、方位角φを指定値に保ったまま、天頂角θに誤差を
与える様にすることもできる。その様な方法を、δ−θ
法と呼ぶ。

【０１０８】今、Ｄゾーン内の点Ｐ_i における交差角度
δ_i の近似値δ_a が、δ−φ法において述べたδ_a の決
定方法と同様にして求められたものとする。即ち、δ_a
は、数２２により与えられたものとする。このとき、各
量Ｌ_D ，Ｌ_E ，ｘ₀ ，ｙ₀ は天頂角θを変数とする関数
としてみることができるので、δ−θ法においても数２
７で与えられる関数Ｓ（θ）を考え、Ｓ（θ′）＝０と
なる解を求めることにより、数６を満足する天頂角θ_a
を求めることができる。

【０１０９】

【数２３】

【０１１０】その後の手順は、δ−φ法と同様であり、
求めた交差角度δ_a及び天頂角θ_a 並びに位置及び方位
角φの指定値を用いて、逆関数の解（Θ，ｘ_s ，ｚ_s ，
α，β）求めることとなる。

【０１１１】同様に、δ−θ法における手順を、図１７
に示す。

【０１１２】ここに図１８及び図１９は、図９に示すあ
るＤゾーン境界上の点から他のＤゾーン境界上の点へト
ーチ４０の先端を移動させる際に各アームへ与えられる
べき駆動量をδ−θ法及びδ−φ法を用いて求めた場合
に生ずる姿勢誤差のシミュレーション結果を示す説明図
である。即ち、図１８及び図１９においては、実線が天
頂角θの誤差Δθ（δ−θ法による結果）を示してお
り、破線が方位角φの誤差Δφ（δ−φ法による結果）
を示している。又、図１８（ａ）、（ｂ）及び図１９
（ａ）、（ｂ）はそれぞれ、トーチ４０の先端が図９に
示した点Ａ″から点Ａ₁ ′へ，点Ａから点Ａ′へ，点Ｂ
から点Ａ′へ及び点Ｃから点Ｃ′へ移動した場合の姿勢
誤差を表わした図である。

【０１１３】図１８及び図１９より明らかな通り、δ−
θ法及びδ−φ法いずれの方法を用いても姿勢誤差（Δ
θ，Δφ）は許容範囲内となりうるのであるが、δ−θ
法を用いるほうが常に、姿勢誤差Δθを小さくすること
ができることがわかる。この点について鑑みるならば、
δ−φ法よりもδ−θ法のほうがより好ましい方法であ
ると言うことができる。そこで、以後の説明において
は、δ−θ法を基に、話を進めることとする。

【０１１４】（Ｆ） 駆動制御の手順 次に、上記δ−θ法を用いてロボットＲＢを駆動する場
合の制御手順について述べる。ここで簡単化のため、テ
ィーチング点は全てＤゾーン外又は境界上の点であると
し、Ｄゾーン内にある点は全ての補間点であるものとす
る。又、トーチ４０の先端は、連続領域及びＤゾーンの
両領域内を移動し得るのとする。

【０１１５】図５は、ロボットＲＢを駆動制御するため
のステップを示すフローチャートであり、図６は図５に
おけるステップＳ４を詳細に説明するフローチャートで
ある。

【０１１６】 ステップＳ１はティーチング工程であ
り、ティーチングデータＸ_i 、及び交差角度δ_i の作成
と記憶とを行なう。即ち、オペレータはマニュアルでト
ーチ４０の先端の位置決めを行ない、エンコーダＥ₁ 〜
Ｅ₅ により各アームの駆動量（α系の値）が読込まれ
る。そして、順変換することにより各ティーチング点の
ティーチングデータＸ_i が計算され、ひき続いて数７に
基づいて各ティーチング点における交差角度δ_i の値が
計算された後、ｉ＝１，２，…についてのティーチング
データＸ_i 及び交差角度δ_i の値がコントローラ５１内
のメモリに格納される。

【０１１７】 次のステップ群１０は再生動作に対応
し、そのうちのステップＳ２ではひとつのティーチング
点についてのティーチングデータＸ_i を読出す。そし
て、ステップＳ３ではそのティーチング点とそれに隣接
するティーチング点（Ｘ_i+1 ）との間を補間し、補間点
のデータＸ_ijを計算する。補間点のデータＸ_ijはＸ系に
よる値であるため、トーチ４０を補間点へ移動するため
の適切な各アームの駆動量を決定する必要がある。

【０１１８】 そこで、ステップＳ４ではＸ系からα
系への逆変換が行われるわけであるが、この逆変換につ
いては図６に基づいて説明を行なうこととする。

【０１１９】まずステップＳ４１においては、補間点
（Ｘ_ij）における交差角度δ_ijの算出が行われる。即
ち、コントローラ５１内のＣＰＵは、メモリより補間点
（Ｘ_ij）における位置ベクトルｐ（ｘ，ｙ，ｚ）及び姿
勢（θ，φ）の各値を読出し、数６及び数７より交差角
度δ_ijを計算する。そして、ひき続いて、ＣＰＵは、数
２０に基づきｙ_0ijを計算する（ステップＳ４２）。

【０１２０】その後、ステップＳ４３においては、補間
点（Ｘ_ij）がＤゾーン内に位置するのか否かを判定す
る。即ち、ＣＰＵは、交差角度δ_ij，ｙ_0ij 及び天頂角
θが表２に示された範囲内にあるか否かを判定する。そ
して、補間点（Ｘ_ij）がＤゾーン外の点であるとＣＰＵ
が判定した場合には、ＣＰＵはｇ法のプログラムに従っ
て各アームの駆動量Θ，ｘ_s ，ｚ_s ，α及びβを、数８
〜数１９に基づいて計算する（ステップＳ４８）。その
後、図５のステップＳ５へと移る。

【０１２１】一方、ＣＰＵが補間点（Ｘ_ij）がゾーン内
の点であると判断した場合には、ＣＰＵはδ−θ法に基
づくプログラムに従って、以後の一連のステップを続行
する。即ち、ステップＳ４４では、補間点（Ｘ_ij）にお
ける交差角度δ_ijの近似値δ_a を求めるための２つの基
準点Ｐ_s1，Ｐ_s2の抽出を行う。ここでは便宜上、基準点
Ｐ_s1，Ｐ_s2として、ｉ番目のティーチング点（Ｘ_i ）及
びｉ＋１番目のティーチング点（Ｘ_i+1 ）を用いること
とし、ティーチング点（Ｘ_i ）及びティーチング点（Ｘ
_i+1 ）における交差角度δ_i ，δ_i+1 をそれぞれ交差角
度δ_S1，δ_S2とすることにする。従って、ＣＰＵはメモ
リより交差角度δ_i ，δ_i+1 の値を読出し、数２２に基
づき補間点（Ｘ_ij）における交差角度δ_ijの近似値δ_a
を算出する（ステップＳ４５）。

【０１２２】そして、ステップＳ４６においては、ＣＰ
Ｕは数２７の関数Ｓ（θ）が０となる解を収束演算法
（Ｎｅｗｔｏｎ法）によって求める。この求めた解θ′
が既述した通り、補間点（Ｘ_ij）における天頂角θの近
似値θａである。

【０１２３】更に、ステップＳ４７においては、ステッ
プＳ４５及びＳ４６で求めた交差角度及び天頂角の近似
値δ_a およびθａ並びに、メモリより読出した補間点
（Ｘ_ij）における位置ｐ（ｘ，ｙ，ｚ）のデータ及び方
位角φのデータを用いて、ＣＰＵが各アームの駆動量
Θ，ｘ_s ，ｚ_s ，α及びβを順次計算することとなる。
そして、以上の手順によりステップＳ４の動作は終了
し、次にステップＳ５へと動作は移る。

【０１２４】 図５のステップＳ５では、ステップＳ
４で決定された各アームの駆動量Θ，ｘ_s ，ｚ_s ，α及
びβがそれぞれ駆動漬浸号Ｄ₁ ，Ｄ₂ ，Ｄ₃ ，Ｄ₄ ，Ｄ
₅に変換された後、各駆動機構５３１〜５３５に伝達さ
れる。そして、各駆動機構５３１〜５３５は各駆動機構
Ｄ₁ 〜Ｄ₅ に応じた量だけ各アームを駆動し、トーチ４
０の先端はステップＳ３にて計算により求めた補間点
（Ｘ_ij）へ移動する。その際のトーチ４０の先端の位置
及び方位角φは、もちろんその補間点におけるデータＸ
_ijと厳密に一致する。そして、レーザ電源５４により電
力の供給を受けたレーザ発振器５５はレーザビームＬＢ
を発振し、レーザービームＬＢはトーチ４０の先端より
溶接部に向けて発射されるとともに、補間点（Ｘ_ij）に
向けて移動しつつワークのレーザ溶接が行われる。

【０１２５】 ステップＳ６においては、その補間点
（Ｘ_ij）が最後の補間点か否かを判断する。尚、その判
断はソフト的にコントローラ５１によって行われる。そ
して、補間点（Ｘ_ij）が最後の補間点でないと判断した
ときには、ステップＳ３へ戻り次の補間点（Ｘ_ij+1）が
計算され一連のステップＳ４〜Ｓ５が同様に行われる。

【０１２６】それに対して、補間点（Ｘ_ij）が最後の補
間点であると判断したときには、ステップＳ７へ移る。

【０１２７】ステップＳ７では、ティーチング点
（Ｘ_i ）が最後のティーチング点か否かを判断する。こ
の判断も又、コントローラ５１によって行なわれる。

【０１２８】そして、ティーチング点（Ｘi ）が最後の
チィーチング点でないと判断したときには、ステップＳ
２へ戻り、次のティーチング点のデータＸ_i+1 が読出さ
れ、一連のステップＳ３〜Ｓ６が再び行われる。

【０１２９】それに対して、ティーチング点（Ｘ_i ）が
最後のティーチング点であると判定したときには、レー
ザー溶接ロボットＲＢの全ての動作が終了する。

【０１３０】（Ｇ） 他の実施例 図６に示した制御方法においては、交差角度δが連
続的に変化する領域内をトーチ４０の先端が移動する場
合には、各アームの駆動量をｇ法で厳密に求め、交差角
度δが不連続値となるＤゾーン内をトーチ４０の先端が
移動する場合には、各アームの駆動量をδ法で決定する
ことにより、全領域にわたって各アームが連続的にかつ
滑らかに駆動される様に駆動量を決定した。

【０１３１】ここでδ法においては、位置については常
に厳密解でなければならないが、姿勢については許容範
囲ならば誤差を与えてもよいという考え方に基いてい
た。従って、Ｄゾーン外に関係なく、全領域にわたって
このδ法を用いて各アームの駆動量を決定することもで
きる。その様な制御方法の逆変換に関する手順を示した
のが図２０であり、図２０は図５におけるステップＳ４
（逆変換）を詳細に示したステップＳ４Ａを示してい
る。尚、図２０に示した手順も、ティーチング点
（Ｘ_i ），（Ｘ_i+1 ）を基準点Ｐ_s1，Ｐ_s2として用い
て、補間点Ｘ_ijにおける交差角度δ_ijの近似値δ_a を求
める場合を示している。

【０１３２】 θ法について 数６を更に展開してゆくと、焦点Ｆ₀ ，Ｆ₀ ′において
は、数２８〜数３０で与えられる関係が成立する。

【０１３３】

【数２８】

【０１３４】

【数２９】

【０１３５】

【数３０】

【０１３６】即ち、旋回角度Θ，並進運動量ｘ_s 及び回
転角度αの天頂角θに対する微分係数は、焦点Ｆ₀ ，Ｆ
₀ ′においては０となる。又、全てのｙ₀ の曲線は、図
９に示す通り焦点Ｆ₀ ，Ｆ₀ ′を通る。これらの関係式
及び性質は、焦点Ｆ₀ ，Ｆ₀ ′においては、境界面にお
いて２つの関数が連続となるための連続の条件が、全て
のｙ₀ の曲線について成立することを意味している。従
って、トーチ４０の先端がＤゾーン外の領域を移動して
きた後、焦点Ｆ₀ 又はＦ₀ ′に到達し、焦点Ｆ₀ 又はＦ
₀′を通過後は、Ｄゾーン内の点へ移動するという様な
ケースの場合には、トーチ４０の先端が焦点Ｆ₀ ，
Ｆ₀ ′を通過するときに、焦点Ｆ₀ ，Ｆ₀ ′を通過後は
ｙ₀ の曲線がＤゾーン外となる様に天頂角θの値を所定
の量だけ変化させるならば、旋回角Θ，並進運動量ｘ_s
及び回転角αを連続的に変化させることができる。但
し、並進運動量ｚ_s 及び回転角βについては、不連続的
に変化する。この様な点に着眼して各アームの駆動量を
決定する方法を、本出願人はδ法と呼んでいる。

【０１３７】例えば、図９において、今、トーチ４０の
先端をθ＝８８ｄｅｇ．である関数ｆ₂ 上の点Ｂ′から
関数ｆ₂ に沿って点Ｂ″へ移動する場合を考える。既述
した通り、点Ｂから焦点Ｆ₀ までは、交差角度δの値は
連続となる。そこでトーチ４０の先端が焦点Ｆ₀ を通過
する場合に、天頂角θをθ＝８８ｄｅｇ．からθ＝８
５．５ｄｅｇ．へ変化すれば、トーチ４０の先端は、焦
点Ｆ₀ を通過後は、関数ｆ₇ （θ＝８５．５ｄｅｇ．の
場合）に沿って移動することとなる。従って、旋回角
Θ，並進運動量ｘ_s 及び回転角αは、連続的かつ滑らか
に変化することとなる。

【０１３８】図２２〜図２４は、その様なθ法によって
各アームの駆動量を決定する場合のシミュレーション結
果（実線）を、ｇ法により決定する場合のシミュレーシ
ョン結果（破線）とともに示した説明図である。即ち、
ｘ＝１５００ｍｍ，ｚ＝１０００ｍｍ，φ＝０ｄｅｇ．
という条件の下で、トーチ４０の先端を、θ＝８８ｄｅ
ｇ．として点Ｂ′（ｙ＝−１４２ｍｍ）より点Ｂ″（ｙ
＝２７８ｍｍ）へ向けて移動するとともに、焦点Ｆ
₀ （ｙ＝１５０ｍｍ）において天頂角θを８８ｄｅｇ．
から８６ｄｅｇ．へ変更した場合の各アームの駆動量
Θ，ｘ_s ，ｚ_s ，α及びβの計算結果である。これらの
結果より明らかな通り、旋回角Θ，並進運動量ｘ_s ，回
転角αの連続性が実現されている。又、並進運動量ｚ_s
についてもその不連続性は殆ど無視できる程度である。

【０１３９】ここに図２１は、θ法によるアームの駆動
制御を行う場合の逆変換に関する手順を示すフローチャ
ートであり、図５におけるステップＳ４を詳細に示した
ステップＳ４Ｂを示している。

【０１４０】同図に示す様に、ステップＳ４Ｂ３にて、
補間点（Ｘ_ij）が焦点Ｆ₀ 又はＦ₀ ′であるか否かが判
断される。そして補間点（Ｘ_ij）が焦点Ｆ₀ 又はＦ₀ ′
である場合には、天頂角θをΔθだけ変化させ、θ＋Δ
θで与えられる量を新たな天頂角θとすることにより、
以後のアームの駆動量をｇ法により求めてゆくこととな
る。（ステップＳ４Ｂ４〜Ｓ４Ｂ５）。

【０１４１】 尚、この発明は以上説明したレーザ溶
接ロボットに限らず、図２に示す座標系と同様な構成を
有する円筒座標ロボット全般について適用できるもので
ある。

【０１４２】即ち、図２に示したＸＹＺ座標系を座標変
換して得られる新たなＸＹＺ座標系を有する円筒座標ロ
ボットに関しても、本制御方法を適用することが出来
る。

【０１４３】その様な一例を示したのが、図２５及び図
２６に記載されたレーザー溶接ロボットＲＢ１である。
ここで図２６は、レーザー溶接ロボットＲＢ１のα系を
示す模式図であり、図２５に示したレーザー溶接ロボッ
トＲＢ１を同図の矢印ＡＲの方向から眺めた側面図に対
応している。本ロボットＲＢ１の構成を概観すれば、次
の通りである。

【０１４４】ＸＹ平面に平行なロボット設置面１０’に
は第１アーム２１’が垂直に設置されており、第１アー
ム２１’自身が有する駆動機構（図示せず）の駆動力を
受けて第１アーム２１’は、ガイド１２に沿ってＸ方向
に並進運動する（第１軸、駆動量ｘ_S ）。尚、第１アー
ム２１’にはジャバラ機構１３が設けられている。

【０１４５】又、第１アーム２１’の一面に取り付けら
れた第２アーム２２’は、自身が有する駆動機構（図示
せず）の駆動力を受けてＹ方向に並進運動するとともに
（第３軸、駆動量ｙ_S ）、点Ｍ’を旋回中心としてＹＺ
平面内を旋回運動する（第２軸、旋回角Θ）。従って、
第２アーム２２’は、第１アーム２１’に対して直接的
にＹＺ平面内に旋回自由度を有しているものと言える。

【０１４６】更に第３アーム２３’は、第４軸と第５軸
とによる回転運動を有している（回転角α、β）。

【０１４７】以上の構成より、本ロボットＲＢ１は、図
１のロボットＲＢを図２のＸ軸のまわりに９０度回転し
た構成に対応していることが理解される。その様な座標
変換を図２７に示す。尚、図２７（ａ）は図２のＸＹＺ
座標系のＸ軸まわりの回転を示しており、同図（ｂ）は
回転後の座標系を本ロボットＲＢ１のデカルト座標系と
して適用した場合を示している。本図に示される通り、
本ロボットＲＢ１のＸ軸、Ｙ軸、Ｚ軸はそれぞれ、図２
のＺ軸、Ｘ軸、Ｙ軸に対応している。従って、本ロボッ
トＲＢ１の旋回角Θは図２の旋回角Θに対応しており、
本ロボットＲＢ１では、第２アーム２２’をＸ軸の回り
に仮想的に旋回することとなる。この点は、図１のロボ
ットＲＢ１と異なる点である。尚、図６に示した方法
（プログラム）をそのまま使用して先ず図１、図２に適
応した各駆動量を決定しておき、その後、これらの駆動
量を図２７の座標変換に従って変換することにより、本
ロボットＲＢ１に適応した駆動量（ｘ_S ，ｙ_S ，Θ，
α，β）を決定することもできる。この場合には、ロボ
ットの軸構成が変わる毎にプログラムを改めて構築する
必要がなくなり、一つのプログラムに簡単な座標変換用
のプログラムを付加するだけで足りるという利点があ
る。

【０１４８】更に、本ロボットＲＢ１に於けるトーチ４
０の姿勢角を図２８に参考として示す。

【０１４９】

【発明の効果】請求項１に係る発明によれば、エンドエ
フェクタの姿勢いかんにかかわらず常に、第１，第２及
び第３アームそれぞれの駆動機構を連続的に且つ滑らか
に変化する駆動出力により駆動することができる効果が
ある。しかも、エンドエフェクタの先端の位置に対応す
る各アームの駆動量は厳密に決定することができるの
で、エンドエフェクタの先端の位置決めを誤差なく正確
に実現できる効果がある。この場合、エンドエフェクタ
の先端の姿勢決めの誤差については、実用上、十分許容
できる範囲内にあるほどに小さい。従って、作業上安全
で、しかも実用上作業に適合した円筒座標ロボットを提
供することができる。

【０１５０】又、請求項２に係る発明によれば、交差角
度の解が連続解となる場合にはいわゆるｇ法により各ア
ームの駆動量を決定し、交差角度の解がｇ法によれば不
連続解となる様な場合には、いわゆるδ法により各アー
ムの駆動量を決定するという様にして円筒座標ロボット
の駆動制御を行なうので、請求項１に係る発明と同様
に、エンドエフェクタの先端の姿勢のいかんによらず常
に各アームの駆動機構を連続的に且つ滑らかに変化する
駆動出力によって駆動できる効果があるとともに、交差
角度の解が不連続解となる場合においてはエンドエフェ
クタの先端の位置決めのみ正確に実現できるが、交差角
度の解が連続解となる場合にあっては、エンドエフェク
タの先端の位置決めと姿勢決めの両方を指定値に対して
誤差なく厳密に実現することができる効果がある。従っ
て、請求項１記載の発明に較べて、更に高精度の円筒座
標ロボットの駆動方法を提供することが可能である。

【０１５１】又、請求項３に係る発明によれば、エンド
エフェクタの姿勢いかんにかかわらず常に、第１及び第
２アームそれぞれの駆動機構を連続的に且つ滑らかに変
化する駆動出力により駆動できる効果がある。更に本発
明では、交差角度の値が不連続となる空間内の点におけ
るエンドエフェクタの先端の姿勢の天頂角の値を、焦点
における天頂角の指定値に対して所定の値だけステップ
関数的に変化させた値をその点における天頂角の指定値
とするだけでよいので、簡便な円筒座標ロボットの駆動
方法を提供できる効果もある。

【図面の簡単な説明】

【図１】この発明の一実施例であるレーザー溶接ロボッ
トの機械的構成を示す斜視図である。

【図２】この発明の一実施例であるレーザー溶接ロボッ
トの機械的構成を示す側面図である。

【図３】トーチの先端の位置及び姿勢を示す説明図であ
る。

【図４】この発明の一実施例であるレーザー溶接ロボッ
トの制御系の電気的構成を示すブロック図である。

【図５】レーザー溶接ロボットを駆動するための手順を
示すフローチャートである。

【図６】レーザー溶接ロボットを駆動するための手順を
示すフローチャートである。

【図７】図２で示した４点Ｍ，Ｎ，Ｐ，Ｑの位置関係を
ＸＹＺ空間で示した立体図である。

【図８】ｇ法における手順のフローを示す説明図であ
る。

【図９】交差角度の値が不連続に変化するＤゾーンを示
した説明図である。

【図１０】交差角度の値が不連続に変化することを明ら
かにするために示した説明図である。

【図１１】交差角度の値が不連続に変化することを示し
た説明図である。

【図１２】Ｄゾーン内の点における交差角度の値の決定
方法を示した説明図である。

【図１３】δ−φ法における手順のフローを示す説明図
である。

【図１４】δ−φ法を用いて逆変換を行った場合のシミ
ュレーション結果を示す説明図である。

【図１５】δ−φ法を用いて逆変換を行った場合のシミ
ュレーション結果を示す説明図である。

【図１６】δ−φ法を用いて逆変換を行った場合のシミ
ュレーション結果を示す説明図である。

【図１７】δ−θ法における手順のフローを示す説明図
である。

【図１８】δ−φ法とδ−θ法との比較結果を示す説明
図である。

【図１９】δ−φ法とδ−θ法との比較結果を示す説明
図である。

【図２０】この発明の他の実施例であるレーザー溶接ロ
ボットの駆動制御方法の逆変換に関する手順を示すフロ
ーチャートである。

【図２１】この発明の他の実施例であるレーザー溶接ロ
ボットの駆動制御方法の逆変換に関する手順を示すフロ
ーチャートである。

【図２２】θ法を用いて逆変換を行った場合のシミュレ
ーション結果を示す説明図である。

【図２３】θ法を用いて逆変換を行った場合のシミュレ
ーション結果を示す説明図である。

【図２４】θ法を用いて逆変換を行った場合のシミュレ
ーション結果を示す説明図である。

【図２５】この発明の更に他の実施例であるレーザ溶接
ロボットの機械的構成を示す斜視図である。

【図２６】この発明の更に他の実施例であるレーザ溶接
ロボットのα系を示す説明図である。

【図２７】座標変換を示す説明図である。

【図２８】トーチの姿勢角を示す説明図である。

【符号の説明】

１０ レーザー溶接ロボットの設置面 ２１ 第１アーム ２２ 第２アーム ２３ 第３アーム

【数２７】

フロントページの続き (72)発明者 黒田 俊英 兵庫県西宮市田近野町６番107号 新明和 工業株式会社開発技術本部内 (72)発明者 桃崎 潤子 兵庫県西宮市田近野町６番107号 新明和 工業株式会社開発技術本部内 (72)発明者 武内 清 兵庫県西宮市田近野町６番107号 新明和 工業株式会社開発技術本部内 (72)発明者 美藤 龍平 兵庫県宝塚市新明和町１番１号 新明和工 業株式会社産業機械事業部内

Claims (3)

【特許請求の範囲】
1. 【請求項１】 ロボット設置面に略垂直に設置された第
   １アームと、前記第１アームに結合して前記ロボット設
   置面に略平行な平面内に並進自由度を有するとともに、
   前記第１アームに対して定まる所定の平面内に直接的又
   は間接的に旋回自由度を有する第２アームと、前記第２
   アームの先端部に略直角に結合して前記第２アームの軸
   方向まわりの回転自由度とそれ自身の軸まわりの回転自
   由度とを有する第３アームと、前記第３アームの先端部
   に略直角に結合したエンドエフェクタとを有する円筒座
   標ロボットの駆動制御方法であって、 （ａ） 絶対座標系で表される空間内の所定の２点にお
   ける前記エンドエフェクタの先端の位置及び姿勢を指定
   するステップと、 （ｂ） 前記所定の２点それぞれにおいて、前記エンド
   エフェクタの軸を含み且つ前記所定の平面に垂直な平面
   が前記エンドエフェクタの軸方向を法線方向とする平面
   と交差する際に生ずる交線と、前記第３アームの軸方向
   とがなす交差角度の値を、前記交差角度を変数とする方
   程式を解くことにより求めるステップと、 （ｃ） 前記ステップ（ｂ）で求めた前記所定の２点に
   おける前記交差角度の値より、前記所定の２点間の点に
   おける前記交差角度の値を近似的に求めるステップと、 （ｄ） 前記所定の２点間の点における前記エンドエフ
   ェクタの先端の位置と姿勢の一方とを指定するステップ
   と、 （ｅ） 前記ステップ（ｃ）で決定した前記交差角度の
   値及び前記ステップ（ｄ）で指定した前記エンドエフェ
   クタの先端の位置と姿勢の一方とを用いて、前記エンド
   エフェクタの先端の姿勢の他方を求めるとともに、更に
   前記所定の２点間の点における前記第１，第２及び第３
   アームそれぞれの駆動量を決定するステップと、 （ｆ） 前記各駆動量に応じた駆動出力をそれぞれ対応
   する前記第１，第２及び第３アームの駆動機構に与える
   ことにより、前記第１、第２及び第３アームの駆動を行
   うステップとを備えたことを特徴とする円筒座標ロボッ
   トの駆動制御方法。
2. 【請求項２】 ロボット設置面に略垂直に設置された第
   １アームと、前記第１アームに結合して前記ロボット設
   置面に略平行な平面内に並進自由度を有するとともに、
   前記第１アームに対して定まる所定の平面内に直接的又
   は間接的に旋回自由度を有する第２アームと、前記第２
   アームの先端部に略直角に結合して前記第２アームの軸
   方向まわりの回転自由度とそれ自身の軸まわりの回転自
   由度とを有する第３アームと、前記第３アームの先端部
   に略直角に結合したエンドエフェクタとを有する円筒座
   標ロボットの駆動制御方法であって、 （ａ） 絶対座標系で表される空間内の各点における前
   記エンドエフェクタの先端の位置及び姿勢を指定するス
   テップと、 （ｂ） 前記空間内の各点において、前記エンドエフェ
   クタの軸を含み且つ前記所定の平面に垂直な平面が前記
   エンドエフェクタの軸方向を法線方向とする平面と交差
   する際に生ずる交線と、前記第３アームの軸方向とがな
   す交差角度の値を、前記交差角度を変数とする方程式を
   解くことにより求めるステップと、 （ｃ） 前記ステップ（ｂ）で求めた前記空間内の各点
   における前記交差角度の値の内、その値が連続的となる
   空間内の点については、当該点における前記交差角度の
   値並びに前記ステップ（ａ）で指定した当該点における
   前記エンドエフェクタの先端の位置及び姿勢を用いて、
   前記第１，第２及び第３アームそれぞれの駆動量を決定
   するステップと、 （ｄ） 前記ステップ（ｂ）で求めた前記空間内の各点
   における前記交差角度の値の内、その値が不連続となる
   空間内の点については、 当該点における前記交差角度の値を、その値が連続
   的である空間内の他の２点における前記交差角度の値を
   用いて近似的に求め、 その近似的に求めた前記交差角度の値並びに前記ス
   テップ（ａ）で指定した当該点における前記エンドエフ
   ェクタの先端の位置及び姿勢の一方を用いて、前記エン
   ドエフェクタの先端の姿勢の他方を新たに求めるととも
   に、更に当該点における前記第１，第２及び第３アーム
   それぞれの駆動量を決定するステップと、 （ｅ） 前記ステップ（ｃ）又は（ｄ）により決定され
   た前記各駆動量に応じた駆動出力を、それぞれ対応する
   前記第１，第２及び第３アームの駆動機構に与えること
   により、前記第１、第２及び第３アームの駆動を行うス
   テップとを備えたことを特徴とする円筒座標ロボットの
   駆動制御方法。
3. 【請求項３】 ロボット設置面に略垂直に設置された第
   １アームと、前記第１アームに結合して前記ロボット設
   置面に略平行な平面内に並進自由度を有するとともに、
   前記第１アームに対して定まる所定の平面内に直接的又
   は間接的に旋回自由度を有する第２アームと、前記第２
   アームの先端部に略直角に結合して前記第２アームの軸
   方向まわりの回転自由度とそれ自身の軸まわりの回転自
   由度とを有する第３アームと、前記第３アームの先端部
   に略直角に結合したエンドエフェクタとを有する円筒座
   標ロボットにおいて、前記エンドエフェクタの先端が絶
   対座標系で表される空間内の焦点を通って前記空間内を
   移動する場合の駆動制御方法であって、 （ａ） 前記空間内の各点における前記エンドエフェク
   タの先端の位置及び姿勢を指定するステップと、 （ｂ） 前記エンドエフェクタの軸を含み且つ前記所定
   の平面に垂直な平面が前記エンドエフェクタの軸方向を
   法線方向とする平面と交差する際に生ずる交線と前記第
   ３アームの軸方向とがなす交差角度の値を、前記交差角
   度を変数とする方程式を解くことにより求めるステップ
   と、 （ｃ） 前記ステップ（ｂ）で求めた前記交差角度の値
   より、前記エンドエフェクタの先端が焦点を通過したか
   否かを判断するステップと、 （ｄ） 前記ステップ（ｃ）で焦点を通過していないと
   判断した場合には、前記ステップ（ａ）で指定した前記
   エンドエフェクタの先端の位置及び姿勢並びに前記ステ
   ップ（ｂ）で求めた前記交差角度の値を用いて、前記空
   間内の点における前記第１，第２及び第３アームそれぞ
   れの駆動量を決定するステップと、 （ｅ） 前記ステップ（ｃ）で焦点を通過したと判断し
   た場合には、前記ステップ（ａ）で指定した前記焦点に
   おける前記姿勢の内の天頂角の値を所定の値だけ変化さ
   せた値を、前記焦点通過後の前記エンドエフェクタの先
   端の位置する点における前記天頂角の指定値とし、当該
   天頂角の指定値並びに前記ステップ（ａ）で指定した当
   該点における前記エンドエフェクタの先端の位置及び姿
   勢の内の方位角の値を用いて、当該点における前記交差
   角度の値を求めた後、前記第１，第２及び第３アームそ
   れぞれの駆動量を決定するステップと、 （ｆ） 前記ステップ（ｄ）又は（ｅ）で決定した前記
   各駆動量に応じた駆動出力を、それぞれ対応する前記第
   １，第２及び第３アームの駆動機構に与えることによ
   り、前記第１、第２及び第３アームの駆動を行うステッ
   プとを備えたことを特徴とする円筒座標ロボットの駆動
   制御方法。