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

Description

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

〔従来の技術〕

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

一般にロボットの座標変換には、α系からｘ系への順
変換と、前述の逆変換がある。前者は、ロボットの軸構
成がどのうようなものであっても解析的に厳密解を求め
ることができることが知られている。同様に、後者に関
しても解析的に厳密解を求める方法が試みられており、
例えばR.C.Paul,“Robot Manipulators;Mathematics,Pr
ogramming and Control"MIT Press（1981）にその具体
例が示されている。即ち、順変換の変換式から代数学的
に逆変換の変換式を求めるわけである。

しかし、ロボットの軸構成いかんによっては厳密な解
析解の求まる場合と求まらない場合があり、一般的には
解析的に厳密解を求めることができない場合が大部分で
ある。そのため、数値解析による手法たとえば収束演算
法を用いて逆変換の解を近似的に求めることが行われて
いる。

また、逆変換の解を近似的に求める別の試みも行われ
ている。例えば、特開昭64−8407号公報に示された方法
がある。この方法では、ロボットのエンドエフェクタの
先端の位置及び姿勢を示すパラメータ群を、ロボットの
作業内容から判断して正確に制御することを必要とする
第１のパラメータ群と必要としない第２のパラメータ群
に分類し、第１のパラメータ群に影響を与えないロボッ
トの関節変位の中から少なくとも一つの変位について、
次式に示す用にヤコビ行列（ヤコビアン）を用いてその
近似解を求める。即ち、ロボットのエンドエフェクタ先
端の現在の位置と姿勢及び次の移動すべき位置とその位
置での姿勢を表わすベクトルをそれぞれ▲▼及び▲
▼とし、ベクトル▲▼及び▲▼に対応する
ロボットの関節変位を示すベクトルをそれぞれ▲▼
及び▲▼、ヤコビアンをＪ（）として表わとすれ
ば、上述の近似解▲▼は、 ▲▼≒▲▼＋J^-1（）・（▲▼−▲
▼） …（１） として求められる。そして、この様にして求めた関節変
位の一部の近似解▲▼を用いて、前記第１のパラメ
ータの値を満たすように残りの関節変位の解を求めるも
のである。この場合、逆変換の変換式から近似解として
求めた関節変位を除外することによりその分の数だけ逆
変換の変換式の未知数を減少させることができ、多くの
場合残りの関節変位について解析的に解を求めることが
できるようになる。

〔発明が解決しようとする課題〕

前記第１の方法、即ち収束演算法による数値解析値解
を求める方法は、位置誤差，姿勢誤差の小さい精度の高
い解を求め得る利点を有するが、かなり長い処理時間を
必要とするため、実時間でのロボットの駆動・制御に
は、適していないという問題点がある。又、ロボットの
軸構成によっては、解が収束しない範囲が存在するとい
う問題点もある。実際、後述するような構成からなる円
筒座標ロボットについて収束演算法により解を求めよう
としても、収束しない範囲がある。

又、第２の方法、即ち、ヤコビアンを用いて関節変位
のうち一部の変位の解のみ別個に近似解を求める方法で
は、残りの関節変位については、逆変換の変換式の近似
解を容易に求めることができる利点を有する。しかしな
がら、近似解を求めるにはエンドエフェクタが移動する
ごとにヤコビアンの逆行列を求める必要があるので、計
算プロセスが複雑となり必ずしも簡便な計算方法とは言
い難い。又、ロボットのエンドエフェクタ先端の位置，
姿勢共に誤差を持つという問題点がある。

尚、以上の様な問題点は産業用ロボット全般における
問題点であって、当然ながらこの発明が対象とする円筒
座標ロボットについても同様に解決すべき問題点となっ
ている。

〔発明の目的〕

この発明は、円筒座標ロボットについて、上述した問
題点を解決するためになされたものであり、ロボットの
エンドエフェクタ先端を指定された位置及び姿勢に正確
に且つ短時間で移動させることがてきる円筒座標ロボッ
トの駆動制御方法を提供することを目的とする。

〔課題を解決するための手段〕

この発明の第１の構成では、ロボット設置面をXY平面
とするXYZ絶対座標系に関し、Ｚ軸まわりの旋回自由度
を持つ第１のアームと、前記第１のアームに対して前記
XY平面内及び前記Ｚ軸に平行な並進自由度を持つ第２の
アームと、前記第２のアームの先端部に略直角に結合し
て前記第２のアームの軸方向まわりの回転自由度とそれ
自身の軸まわりの回転自由度とを持つ第３のアームと、
前記第３のアームの先端部に略直角に結合したエンドエ
フェクタとを有する円筒座標ロボットの駆動制御方法で
あって、（ａ）前記絶対座標系において前記エンドエフ
ェクタの先端の位置及び姿勢を指定し、（ｂ）前記エン
ドエフェクタの軸を含み且つ前記XYZ平面に垂直な平面
が前記エンドエフェクタの軸方向を法線方向とする平面
と交差する際の光線と、前記第３のアームの軸方向との
なす角度を正弦又は余弦を変数とする４次方程式を解く
ことによって、前記指定を受けた位置及び姿勢に対応す
る前記正弦又は余弦の値を求めるとともに、（ｃ）前記
正弦または余弦の値から得られる角度値を用いて前記第
２のアームと前記第３のアームとの連結点に対応する座
標値を特定し、当該連結点に対応する座標値を用いて、
前記第１のアームの旋回角度を求め、さらに前記角度値
および前記旋回角度に基づいて、前記第２のアームの並
進駆動量、前記第２の軸方向まわりの前記第３のアーム
の駆動量、および前記第３のアーム自身の軸回りの回転
駆動量を算出するとともに、前記角度値に基づいて、前
記第２のアームのＺ方向の昇降量を求めることにより、
前記円筒座標ロボットの各アームの駆動量を決定する。
そして、（ｄ）これらの決定した駆動量に応じた駆動能
力を各アームの駆動機構に与えて前記ロボットにおける
駆動制御を行うようにしたものである。

また、第２の構成では、第１の構成におけるステップ
（ｂ）において、前記指定された位置及び姿勢に対応す
る各アームの状態を基準として、前記第１のアームを前
記Ｚ軸のまわりに仮想的に旋回させて前記エンドエフェ
クタの軸の前記XY平面への射影がＸ軸またはＹ軸に平行
な状態を想定し、前記想定された状態について前記４次
方程式を解くことにより前記正弦又は余弦の値を求め、
前記ステップ（ｃ）において、（ｃ−１）前記正弦又は
余弦の値を用いて前記想定された状態での前記第１のア
ームの前記Ｚ軸のまわりの旋回角度を求め、（ｃ−２）
前記ステップ（ｃ−１）で求められた旋回角度から、前
記第１のアームを前記ステップ（ｂ）において仮想的に
旋回させる前の状態における前記第１のアームの前記Ｚ
軸のまわりの旋回角度へ変換するとともに、（ｃ−３）
前記ステップ（ｃ−２）で求められた前記第１のアーム
の前記Ｚ軸のまわりの旋回角度の値を用いて、前記円筒
座標ロボットの各アームの他の駆動量を決定することと
したものである。

〔作用〕

この発明の第１の構成では、エンドエフェクタの軸を
含みXY平面に垂直な平面がエンドエフェクタの軸方向を
法線方向とする平面と交差する際の交線と第３のアーム
の軸方向とのなす仮想的な角度と、第１のアームのＺ軸
まわりの旋回角度との幾何学的関係に着眼する。即ち、
前記仮想的な角度の正弦又は余弦について成立する４次
方程式を直接解析的に解き、得られた解より前記旋回角
度を決定する。そして、この旋回角度値をエンドエフェ
クタの位置と姿勢に関する逆変換式に代入することによ
り、各アームの駆動量を厳密に求めることができる。従
って、以上得られた各アームの駆動量に基づいて各アー
ムを駆動すれば、エンドエフェクタの先端は誤差なく正
確に指定された位置へ移動し、指定された姿勢を保つこ
とができる。

又、第２の構成では、第１の構成において、第１のア
ームを更にＺ軸のまわりに仮想的に旋回させ、エンドエ
フェクタの軸のXY平面への射影がＸ軸に平行になる状態
を想定する。そして、この仮想状態において前記４次方
程式を解析的に解き、得られた解より決定されるこの仮
想状態での第１のアームの旋回角度を、仮想的に旋回さ
せる前の本来の旋回角度へ変換する。このステップによ
って容易に第１のアームの旋回角度を決定することがで
き、以下第１の構成と同様のステップにより、各アーム
の駆動量を厳密に求めることが可能となる。

〔実施例〕

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

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

又、第２のアーム22が第１のアーム21に略直角に交差
して備えつけられており、第２のアーム22は第１のアー
ム21内部に備えつけられた第２の駆動機構によってＺ軸
に平行に昇降することができ、更に第２のアーム22内部
に備えつけられた第３の駆動機構によって、XY平面内を
並進運動することができる。尚、第１のアーム21の軸A1
と第２のアーム22の軸A2との交点ＭのＸ系に対する位置
座標を点Ｍ（0,0,Z_S）とする。

又、第２のアーム22の先端部31には、第３のアーム23
が第２のアーム22に略直角に結合されており、第３のア
ーム23は第２のアーム22の先端部31の内部に備えつけら
れた第４の駆動機構によって、第２のアーム22の軸A2の
まわりのα方向に回転することができる。ここで、軸A2
と第２のアーム22の先端の点Ｐと点Ｍとの相対距離を変
数x_Sで表わす。更に、第３のアーム23は、それ自身の先
端部32の内部に備えつけられた第５の駆動機構によっ
て、それ自身の軸A3のまわりのβ方向に回転することも
できる。尚、第３のアーム23の長さは一定値d₁で固定さ
れている。

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

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

一方、第３図はトーチ40の先端（点Ｎ）の位置及び姿
勢を示す説明図であり、点Ｑを原点としたｘ系により表
わされる。即ち、点Ｎの位置は位置座標（x,y,z）によ
り決まり、又、その姿勢はベクトル▲▼とＺ軸との
なす角度（天頂角）θ及びベクトル▲▼のxy平面へ
の射影ベクトルとｘ軸とのなす角度（方位角）φにより
決まる。尚、ｘ系の各軸方向は、Ｘ系の各軸方向に対応
する様に設定されている。即ち、ｘ軸,z軸及びｚ軸は、
それぞれＸ軸,Y軸及びＺ軸に平行である。

B.装置の電気的構成 第４図は、レーザ溶接ロボットRBの制御系の電気的構
成を示すブロック図である。この制御系は、CPUやメモ
リ等からなるコントローラ51を有しており、ソフト的に
制御される。又、ティーチングボックス52を操作するこ
とによってティーチングデータの取込みなどが行なわれ
る。

一方、第１の駆動機構531を構成するモータM₁に駆動
信号D₁が送信され、モータM₁に取付けられたエンコーダ
E₁が第２のアーム22の軸A1のまわりの旋回角Θを検出す
る。同様に、第２〜第５の駆動機構532〜535にそれぞれ
駆動信号D₂〜D₅が送信され、各モータM₂〜M₅が動作し、
変位z_S,x_S及び回転角度α，βがそれぞれエンコーダE₂
〜E₅によって検出される。

又、レーザ電源54はレーザ発振器55に電力を供給する
ための電源である。レーザ発振器55で発生したレーザビ
ームLBは、ロボットRBの各アーム内に設けたミラーによ
って順次に反射されつつ、トーチ40の先端からワークに
向けて照射される。

C.駆動制御方法 第５図は、ロボットRBを駆動制御するためのステップ
を示すフローチャートであり、第６図は第５図における
ステップS4を詳細に説明するフローチャートである。

ステップ１はティーチング工程であり、ティーチング
データ▲▼の作成と記憶とを行う。即ち、オペレー
タはマニュアルでトーチ40の先端の位置決めを行い、エ
ンコーダE₁〜E₅により各アームの駆動量（α系の値）が
読込まれる。そして、順変換することにより各ティーチ
ング点のティーチングデータ▲▼が計算され、ｉ＝
1,2,…についてのティーチングデータ▲▼はメモリ
に格納される。

次のステップ群10は再生動作に対応し、そのうちのス
テップS2ではひとつのティーチング点についてのティー
チングデータ▲▼を読出す。そして、ステップS3で
はそのティーチング点とそれに隣接するティーチング点
との間を補間し、補間点のデータ▲▼を計算す
る。補間点のデータ▲▼はＸ系による値であるた
め、トーチ40を補間点へ移動するための適切な各アーム
の駆動量を決定する必要がある。

そこで、ステップS4ではＸ系からα系への逆変換が行
われる。

この逆変換のプロセスを説明する前に、トーチ40と第
２のアーム22及び第３のアーム23との関係を、幾何学的
に考察する。第７図は、第２図で示した４点M,N,P,Qの
位置関係を、XYZ空間において示した立体図である。

今、第２のアーム22が軸A1のまわりに角度Θで旋回
し、第３のアーム23が軸A2のまわりに角度αで回転し、
更に軸A3のまわりに角度βで回転した状態にある場合を
考える。

第７図において、トーチ40の先端の位置Ｎと姿勢が与
えられると、自動的に点Ｑの位置も定まる。

ここで次の（ｉ）〜（iii）の条件を満足する円Ｃを
定義する。

（ｉ） その中心が点Ｑであること。

（ii） 半径がｄであること。

（iii） ベクトル▲▼を法線とする平面内に存在
すること。

すると、点Ｐは円Ｃの円周上に存在し、ベクトル▲
▼は円Ｃの半径ベクトルとなっている。そして、ロボ
ットRBの各軸構成から明らかなように、点Ｐについて、 ▲▼ ⊥ ▲▼ …（２） ▲▼ ⊥ ▲▼ …（３） の関係が成立する。

今、ベクトル▲▼を含みXY平面に垂直な平面Ｓが
円Ｃと交差している場合を考え、平面Ｓと円Ｃとの交点
をそれぞれ点Ｒ及び点Ｔとする。そして、ベクトル▲
▼とベクトル▲▼とのなす角をδと定義すること
とする。

又、ベクトル▲▼及び円ＣをXY平面へ射影する場
合を考える。即ち、ベクトル▲▼のXY平面への射影
ベクトルを 円ＣをXY平面へ射影した場合にできる楕円を楕円Ｅ′と
し、各点P,R,Tに対応するXY平面上の各点をそれぞれ点
Ｐ′,R′,T′とする。

以上述べた幾何学的関係を基にして、逆変換の厳密解
を求めることとなるのであるが、ここでは第６図に示す
フローチャートに従って、第７図とともに以後の手続を
説明することとする。

まず、ステップS41では、第２のアーム22をＺ軸をま
わりに仮想的に旋回することを考え、第２のアーム22の
先端の点が点Ｐにある状態から仮想的な角度φ（第７図
の例においては、角度Θとは逆まわりとなっている。）
だけ旋回することによって、トーチ40の軸A4方向のベク
トル▲▼のXY平面への射影ベクトル がＸ軸に平行になったものとする。この状態における円
Ｃに対応する円を円C₁、円C₁のXY平面への射影により生
じる楕円を楕円E₁′とする。又、ベクトル▲▼
を含みXY平面に垂直な平行面S₁が円C₁と交差する点を点
R₁,T₁とし、円C₁上の３点P₁,P₁,T₁の楕円E₁′上の対応
する点をそれぞれ点P₁′,R₁′,T₁′とする。

以上の様に仮想的にベクトル▲▼を角度φだけ旋
回することにより、点Ｐと点Ｑの幾何学的関係式を導出
することが容易となる。この点について、以下に説明す
る。なお、以下の解析による逆変換アルゴリズムの流れ
を第９図に示してある。この第９図においてカッコ付き
の数字は以下の説明中における数式の番号を示してい
る。また、（＊）印はティーチングデータから直ちに得
られるＸ系の量であり、（＊＊）印は逆変換によって求
めるべきα系の量を示している。

第８図は、ベクトル▲▼及び円C₁をXZ平面へ
射影した場合を示す説明図であり、ベクトル がベクトル▲▼のXZ平面への射影ベクトル、楕
円E₁″が円C₁をXZ平面へ射影した結果生ずる楕円，点
P₁″,R₁″,T₁″が点P₁,R₁,T₁のXZ平面への射影点を示し
ている。第３図に示す様にトーチ40即ちベクトル▲
▼の姿勢のうち、ベクトル▲▼のＺ軸との
なす角度が角度θであるから、第８図において、ベクト
ル と軸Ｚとのなす角度もまた角θで表わされる。従って、
直線R₁″T₁″とＸ軸とのなす角もまた角θである。

そこで、話を第７図に戻し平面S₁と円C₁を含む平面と
の交線を交線CLとすると、ベクトル▲▼の交線
CL方向の成分ベクトル（大きさは、d₁′cosδ）はXZ平
面に平行であり、ベクトル▲▼の面S₁の法線方
向の成分ベクトル（大きさは、d₁′sinδ）はＹ軸に平
行である。従って、点Q₁及び点P₁の位置座標を点Q
₁（x₀,y₀,z₀）及び点P₁（x,y,z）とすると、点P₁と点Q₁
との関係は、第７図においては余弦cosδが負、正弦sin
δが正であることを考慮して次式（４）〜（６）により
表わされる。

ｘ＝x₀＋d₁C_δＣ_θ …（４） ｙ＝y₀−d₁S_δ …（５） ｚ＝z₀−d₁C_δＳ_θ …（６） ここでは、余弦，正弦の記載に（７）式，（８）式の
様な簡略形を用いており、以後の説明においてもこれら
の簡略形を使用する。

Ｃ_δ＝cosδ …（７） Ｓ_δ＝sinδ …（８） 次に、第６図に示すステップS42では、cosδの４次方
程式を解くステップを行う。cosδの４次方程式は、第
７図の位置関係より容易に導き出される。即ち、ベクト
ル▲▼とベクトル▲▼は直交しているの
で、 ▲▼^２＋▲▼^２＝▲▼^２ …（９） 又、 ▲▼^２＝x₀ ²＋y₀ ²＋（d₁C_δＳ_θ）^２ …（10） となる。従って、（９）式に（４）〜（６）式及び（1
0）式を代入し、x₀及びy₀をd₁により正規化すると、
（９）式は Ｓ_δ＝（−Ｓ_θ ²C_δ ^２＋x₀′Ｃ_θＣ_δ＋１）/y₀′ …（11） x₀′＝x₀/d₁ …（12） y₀′＝y₀/d₁ …（13） となる。そして、（11）式の両辺を２乗し、 Ｃ_δ ^２＋Ｓ_δ ^２＝１ …（14） の関係を用いて展開すると、次式（15）に示す通りＣ_δ
に関する４次方程式が得られる。

X⁴＋4aX³＋6bX²＋4cX＋ｄ＝０ …（15） ここで、 Ｘ＝Ｃ_δ …（16） ａ＝−x₀′Ｃ_θ/2S_θ ^２ …（17） ｂ＝（x₀′²C_θ ^２−2S_θ ^２＋y₀′^２）/6S_θ ^４ …（18） ｃ＝x₀′Ｃ_θ/2S_θ ^４ …（19） ｄ＝（１−y₀′^２）/S_θ ^４ …（20） 以上の説明では余弦Ｃ_δに関して４次方程式を導出し
たが、もちろん正弦Ｓ_δに関しても同様に４次方程式を
導出することができる。即ち、（11）式を（14）式の関
係を用いて変形すれば、 x₀′Ｃ_θＣ_δ＝−Ｓ_θ ²S_δ ^２＋y₀′Ｓ_δ−Ｃ_θ ^２ …（21） となり、（21）式の両辺を自乗し、 Ｘ＝Ｓ_δ …（22） とおけば、 X⁴＋4a′X³＋6b′X²＋4c′Ｘ＋ｄ′＝０ …（23） となる。ここで、 ａ′＝−y₀′/2S_θ ^２ …（24） ｂ′＝（y₀′^２＋2C_θ ²S_θ ^２＋x₀′²C₀ ²）/6S_θ ^４ …（25） ｃ′＝−Ｃ_θ ²y₀′/2S_θ ^４ …（26） ｄ′＝Ｃ_θ ^２（Ｃ_θ ^２−x₀′^２）Ｓ_θ ^４ …（27） である。

尚、Ｃ_δおよびＳ_δのうちの一方がわかれば他方は
（14）式によって求められるため、少なくとも角δの正
弦又は余弦の一方について４次方程式を解けばよく、以
後は（15）式に戻り、角δの余弦を求めるステップを説
明する。

（15）式の４次方程式の解は、よく知られたデカルト
の方法によって解析的に求めることができる。即ち、解
X₁〜X₄は、以下に示す（28）式〜（31）式により与えら
れるので、これらの解は、予めプログラミングしておけ
ば第４図に示すコントローラ51によって容易に計算する
ことができる。

ここで、（28）式〜（31）式の右辺第２項の符号はａ
≧０のとき上側の符号を、ａ＜０のとき下側の符号をと
ると約束する。また、定数U,V,Wは、（17）式〜（20）
式で与えられる係数ａ〜ｄによって与えられるものであ
り、ここではそれらの具体的記載は省略する。

次に、第６図のステップS43においては、ステップS42
で求めた余弦Ｃ_δよりベクトル とＸ軸とのなす角度Θ′（第７図参照）を求めた後、角
度Θ′から角度Θへ変換することにより、第２のアーム
22の本来の旋回角Θを求める。尚、角度Θ′を求めるた
めには点P₁′（x,y,0），従って点Q₁′（x₀,y₀,0）の位
置座標を求める必要があり、この点Q₁′の位置座標は次
のようにしてトーチ40の先端の位置より求めることがで
きる。

そこで、今、α系からｘ系への順変換を考えることと
する。即ち、トーチ40の先端の位置ベクトルを、姿勢
を（，，）で表わすとすれば、α系からｘ系への
座標変換マトリックスＴは（32）式，（33）式により表
わされる。

Ｔ＝Ａ_ΘＡ_zxαＡ_βＡ_γ …（33） ここで、各マトリックスＡ_Θ,A_zxα,A_β及びＡ_γは、
ロボットRBの各アームの自由度に伴う座標変換マトリッ
クスであり、次式（34）式〜（37）式により表わされ
る。

従って、（34）式〜（37）式を用いて（33）式を展開
すれば、座標変換マトリックスＴの各要素を求めること
ができるが、そのうちベクトルとベクトルの各要素
のみを以下に示す。

a_x＝Ｃ_ΘＣ_β＋Ｓ_ΘＣ_αＳ_β …（38） a_y＝Ｓ_ΘＣ_β−Ｃ_ΘＣ_αＳ_β …（39） a_z＝−Ｓ_αＳ_β …（40） p_x＝（Ｃ_ΘＣ_β＋Ｓ_ΘＣ_αＳ_β）d₂ −Ｓ_ΘＳ_αd₁＋x_sC_Θ …（41） p_y＝（Ｓ_ΘＣ_β−Ｃ_ΘＣ_αＳ_β）d₂ ＋Ｃ_ΘＳ_αd₁＋x_sS_Θ …（42） p_z＝−Ｓ_αＳ_βd₂−Ｃ_αd₁＋z_s …（43） このようにして得られた順変換の式（38）〜（43）よ
り、逆変換の解（Θ,x_s,z_s,α，β）を求めることにな
る。

そこで、（38）〜（40）式を（41）〜（43）式に代入
すると、 p_xm＝−d₁S_ΘＳ_α＋x_sC_Θ …（44） p_ym＝ d₁C_ΘＳ_α＋x_sS_Θ …（45） p_zm＝−d₁C_α＋z_s …（46） となる。ここで、位置ベクトル▲▼を、 p_xm＝p_x−d₂a_x …（47） p_ym＝p_y−d₂a_y …（48） p_zm＝p_z−d₂a_z …（49） と定義した。即ち、位置ベクトル▲▼の各成分p_xm,
p_ym,p_zmは、第７図における点Ｑの位置座標を与える。

再び、第７図に目を向ければ、（50）式〜（52）式で
与えられる幾何学的関係が成立していることがわかる。

x₀＝▲▼・Ｃ_ψ …（50） y₀＝▲▼・Ｓ_ψ …（51） ▲▼＝▲▼ …（52） ここで、角度ψはベクトル とＸ軸とのなす角度である。又、点Ｑの位置座標（p_xm,
p_ym,p_zm）を用いれば、 となる。従って、 ψ＝atan2（p_ym,p_xm）−φ …（54） z₀＝p_zm …（57） となり、角度ψ，点Q₁（したがってQ₁′）の位置座標が
トーチ40の先端の位置より求められる。

ここで atan2（p_ym,p_xm）は、 p_ym＝Ｓ_ξ …（58） p_xm＝Ｃ_ξ …（59） となる角度ξを一意に与える関数である。

一方、角度δは次のようにして求めることができる。
即ち、余弦Ｃ_δは（28）式〜（31）式によって表現され
ており、正弦Ｓ_δは（11）式によって表現されているの
で、（55），（56）式で求めたx₀,y₀の値を用いつつ、
角度δは δ＝atan2（Ｓ_δ,C_δ） …（60） より決定される。

次に、以上求められた点Q₁の位置座標Q₁（x₀,y₀,z₀）
及び角度δを用いて、角度Θ′を求める。即ち、（55）
式〜（57）式を（４）式〜（６）式へ代入すれば、点P₁
の位置座標P₁（x,y,z）ないしは点P₁′の位置座標（x,
y,0）が求められるので、角度Θ′は（61）式により決
定される。

Θ′＝atan2（y,x） …（61） これより、角度Θはベクトル▲▼を角度−φだ
けＺ軸のまわりに旋回することにより求めることができ
る（第７図参照）。故に、角度Θは、 Θ＝atan2（y,x）＋φ …（62） により求められる。

次に、第６図のステップS44では、ステップS43で角度
Θの厳密解が求められたので、これを座標変換式（44）
〜（46）式へ代入することにより、第２のアーム22のXY
平面内の並進量x_s及び軸A₂まわりの回転角αを求める。
このステップは、次の様にして行われる。

p_zm＝z₀であるから、 （46）式及び（57）式より、 z_s＝z₀＋d₁C_α …（63） であり、又、（６）式は ｚ＝z_s＝z₀−d₁C_δＳ_θ …（64） で表わされる。従って、（63）式及び（64）式より Ｃ_α＝Ｃ_δＳ_θ …（65） となる。

一方、（44）式及び（45）式より並進自由度x_sは x_s＝p_xmC_Θ＋p_ymS_Θ …（66） により求められ、又（66）式を（44）式に代入すれば、 Ｓ_α＝（−p_xmS_Θ＋p_ymC_Θ）/d₁ …（67） となる。よって、角度αは（65）式及び（67）式より α＝atan2（Ｓ_α,C_α） …（68） により決定される。

次に、第６図のステップS45では、第２のアーム22の
Ｚ軸方向への並進自由度z_sと軸A3のまわりの回転角βが
決定される。即ち、並進自由度Z_sは（57）式及び（63）
式により決定され、回転角βは（38）式〜（40）式より β＝atan2｛（ayC_Θ−a_xS_Θ）/C_α,a_xC_Θ＋a_yS_Θ｝ …（69） 又は、 β＝atan2（−a_z/S_α,a_xC_Θ＋a_yS_Θ） …（70） により決定される。

以上のステップにより、逆変換の解即ち、各アームの
駆動量Θ,x_s,z_s,α，βが全て厳密に決定されたことに
なり、第５図におけるステップS4は終了する。

第５図ステップS5では、ステップS4で決定された各ア
ームの駆動量Θ,x_s,z_s,α，βがそれぞれ駆動信号D₁,
D₂,D₃,D₄,D₅に変換された後、各駆動機構531〜535に伝
達される。そして、各駆動機構531〜535は各駆動信号D₁
〜D₅に応じた量だけ各アームを駆動し、トーチ40の先端
はステップS3にて計算により求めた補間点（▲
▼）へ移動する。その際のトーチ40の先端の位置及び姿
勢は、もちろんその補間点におけるデータ▲▼と
厳密に一致する。そして、レーザ電源54より電力の供給
を受けたレーザ発振器55はレーザビームLBを発振し、レ
ーザビームLBはトーチ40の先端より溶接部に向けて発射
され補間点（▲▼）に向けて移動しつつワークの
レーザ溶接が行われる。

ステップS6においては、その補間点（▲▼）が
最後の補間点か否かを判断する。尚、その判断はソフト
的にコントローラ51によって行われる。そして、補間点
（▲▼）が最後の補間点でないと判断したときに
は、ステップS3へ戻り次の補間点（▲▼が計
算され一連のステップS4〜S5が同様に行われる。

それに対して、補間点（▲▼）が最後の補間点
であると判断したときには、ステップS7へ移る。

ステップS7では、ティーチング点（▲▼）が最後
のティーチング点か否かを判断する。この判断も又、コ
ントローラ51によって行われる。そして、ティーチング
点（▲▼）が最後のティーチング点でないと判断し
たときには、ステップS2へ戻り次のティーチング点のデ
ータ▲▼が読出され、一連のステップS3〜S6が
再び行われる。

それに対して、ティーチング点（▲▼）が最後の
ティーチング点であると判定したときには、レーザ溶接
ロボットRBの全ての動作が終了する。

以上の説明から明らかな通り、この実施例は幾何学的
に想定した角度δの簡単な４次方形式をソフト的に処理
することにより、逆変換を容易にかつ近似することなく
行えるようにしたものであることがわかる。

ところで、上記実施例ではXY平面へのトーチ40の射影
がＸ軸に平行となるような状態を想定したが、Ｘ軸とＸ
軸とは相互に入れ換えて定義したときには、上記射影が
Ｙ軸に平行となるような仮想的状態を想定してもよいこ
とになる、 また、第９図からわかるように仮想的φ回転は演算プ
ロセスの途中に現われるのみであり、最終的にはその仮
想的φ回転を行なっていない現実の状態についてのα系
の値を求めている。したがって、上記各式を組合せるこ
とによって、仮想的φ回転に対応して現われてくる量
（たとえばΘ′）を消去した式を作れば、その中には仮
想的φ回転に関する量は出てこない。つまり、この場合
には途中のプロセスとして仮想的φ回転を行わない演算
式が得られるのであり、このような変形もこの発明の範
囲に含まれる。角度δはスカラー量であって座標軸の回
転の影響を受けないため、このような場合にもＣ_δまた
はＳ_δについての４次方程式の解となる。

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

〔発明の効果〕

以上の様にこの発明の第１の構成によれば、角度δの
正弦又は余弦の４次方程式を解き、角度δと旋回角Θの
位置関係により旋回角Θを厳密に求め、この旋回角Θの
値を用いて順次残りの各アームの駆動量も厳密に求める
ことができるので、エンドエフェクタ先端の位置決めを
誤差なく正確に行うことができる効果がある。

更に、角度δの正弦又は余弦の４次方程式の解の形式
は定まっているので、上記４次方程式を解くステップを
系統的に処理することができる。従って、各アームの駆
動制御を短時間で行える効果もある。

また、第２の構成によれば、第１の構成において第１
のアームをＺ軸のまわりに更に角度φだけ旋回すること
により、エンドエフェクタのXY平面への射影がＸ軸と平
行になる様な仮想的な状態を取り扱うので、角度δの正
弦又は余弦の４次方程式が簡単な形になり、解を容易に
求めることができる効果もある。

【図面の簡単な説明】

第１図は、この発明の一実施例であるレーザ溶接ロボッ
トの機械的構成を示す斜視図、 第２図は、この発明の一実施例であるレーザ溶接ロボッ
トの機械的構成を示す模式的側面図、 第３図は、トーチの先端の位置及び姿勢を示す説明図、 第４図は、レーザ溶接ロボットの制御系の電気的構成を
示すブロック図、 第５図は、レーザ溶接ロボットを駆動制御するためのス
テップを示すフローチャート、 第６図は、第５図におけるステップS4を詳細に説明する
フローチャート、 第７図は、第２図で示した４点M,N,P,Qの位置関係をXYZ
空間において示した立体図、 第８図は、ベクトル▲▼及び円C₁をXZ平面へ射
影した場合を示す説明図、 第９図は、実施例における逆変換アルゴリズムの流れを
示す図である。 RB……レーザ溶接ロボット、 10……ロボットの設置面、21……第１のアーム、 22……第２のアーム、23……第３のアーム、 31……第２のアームの先端部、 32……第３のアームの先端部、 40……トーチ、 A1……第１のアームの軸、 A2……第２のアームの軸、 A3……第３のアームの軸、 A4……トーチの軸

フロントページの続き (72)発明者 越智 重貴 兵庫県西宮市田近野町６番107号 新明 和工業株式会社開発技術本部内 (72)発明者 藤長 茂樹 兵庫県西宮市田近野町６番107号 新明 和工業株式会社開発技術本部内 (72)発明者 渡辺 寛 兵庫県宝塚市新明和町１番１号 新明和 工業株式会社産業機械事業部内

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】ロボット設置面に平行な面をXY平面とする
   XYZ絶対座標系において、Ｚ軸まわりの旋回自由度を持
   つ第１のアームと、前記第１のアームに対して前記XY平
   面内及び前記Ｚ軸に平行な並進自由度を持つ第２のアー
   ムと、前記第２のアームの先端部に略直角に結合して前
   記第２のアームの軸方向まわりの回転自由度とそれ自身
   の軸まわりの回転自由度とを持つ第３のアームと、前記
   第３のアームの先端部に略直角に結合したエンドエフェ
   クタとを有する円筒座標ロボットの駆動制御方法であっ
   て、 （ａ） 前記絶対座標系において前記エンドエフェクタ
   の先端の位置及び姿勢を指定するステップと、 （ｂ） 前記エンドエフェクタの軸を含み且つ前記XY平
   面に垂直な平面が前記エンドエフェクタの軸方向を法線
   方向とする平面と交差する際の交線と、前記第３のアー
   ムの軸方向とのなす角度の正弦又は余弦を変数とする４
   次方程式を解くことによって、前記指定を受けた位置及
   び姿勢に対応する前記正弦又は余弦の値を求めるステッ
   プと、 （ｃ） 前記正弦または余弦の値から得られる角度値を
   用いて前記第２のアームと前記第３のアームとの連結点
   に対応する座標値を特定し、当該連結点に対応する座標
   値を用いて、前記第１のアームの旋回角度を求め、さら
   に前記角度値および前記旋回角度に基づいて、前記第２
   のアームの並進駆動量、前記第２の軸方向まわりの前記
   第３のアームの駆動量、および前記第３のアーム自身の
   軸回りの回転駆動量を算出するとともに、前記角度値に
   基づいて、前記第２のアームのＺ方向の昇降量を求める
   ことにより、前記円筒座標ロボットの各アームの駆動量
   を決定するステップと、 （ｄ） 前記駆動量に応じた駆動出力を各アームの駆動
   機構に与えて前記ロボットにおける駆動制御を行うステ
   ップ とを備えることを特徴とする円筒座標ロボットの駆動制
   御方法。
2. 【請求項２】請求項１記載の方法において、 前記ステップ（ｂ）が前記指定された位置及び姿勢に対
   応する各アームの状態を基準として、前記第１のアーム
   を前記Ｚ軸のまわりに仮想的に旋回させて前記エンドエ
   フェクタの軸の前記XY平面への射影がＸ軸またはＹ軸に
   平行な状態を想定し、前記想定された状態について前記
   ４次方程式を解くことにより前記正弦又は余弦の値を求
   めるステップであり、 前記ステップ（ｃ）が、 （ｃ−１） 前記正弦又は余弦の値を用いて前記想定さ
   れた状態での前記第１のアームの前記Ｚ軸のまわりの旋
   回角度を求めるステップと、 （ｃ−２） 前記ステップ（ｃ−１）で求められた旋回
   角度から、前記第１のアームを前記ステップ（ｂ）にお
   いて仮想的に旋回させる前の状態における前記第１のア
   ームの前記Ｚ軸のまわりの旋回角度へ変換するステップ
   と、 （ｃ−３） 前記ステップ（ｃ−２）で求められた前記
   第１のアームの前記Ｚ軸のまわりの旋回角度の値を用い
   て、前記円筒座標ロボットの各アームの他の駆動量を決
   定するステップ とを有することを特徴とする円筒座標ロボットの駆動制
   御方法。