JPH05303418A

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

Info

Publication number: JPH05303418A
Application number: JP9886092A
Authority: JP
Inventors: Masakazu Kobayashi; 正和小林; Jiyunko Momozaki; 潤子桃崎; Shigeki Ochi; 重貴越智; Shunei Kuroda; 俊英黒田; Shigeki Fujinaga; 茂樹藤長; Masuo Kasai; 増雄笠井; Kiyoshi Takeuchi; 清武内; Ryuhei Mifuji; 龍平美藤
Original assignee: Shin Meiva Industry Ltd
Current assignee: Shinmaywa Industries Ltd
Priority date: 1992-02-27
Filing date: 1992-03-24
Publication date: 1993-11-16
Anticipated expiration: 2014-07-26
Also published as: JP2923713B2

Abstract

(57)【要約】【目的】円筒座標ロボットの駆動出力を全領域にわた
って高確度で決定することができる制御方法を提供す
る。【構成】ＣＰＵ５１１は、ティーチング点又は補間点
の姿勢（天頂角）が特定領域内か特定領域外かを判断す
る。特定領域外の場合には、ＣＰＵ５１１は交差角の余
弦の４次方程式の厳密解を算出し、当該解より旋回角を
算出する。特定領域内の場合には、交差角の余弦の逆数
の４次方程式の厳密解より旋回角を算出する。更にＣＰ
Ｕ５１１は旋回角より他の駆動量を決定し、駆動信号Ｄ
₁〜Ｄ₅を算出するとともに、駆動信号Ｄ₁〜Ｄ₅をデ
ータバス５７を介して駆動機構５３１〜５３５へ送信す
る。その結果、トーチ先端はティーチング点又は補間点
へ正確に移動する。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

【０００５】また、逆変換の解を近似的に求める別の試
みも行われている。例えば、特開昭６４−８４０７号公
報に示された方法がある。この方法では、ロボットのエ
ンドエフェクタの先端の位置及び姿勢を示すパラメータ
群を、ロボットの作業内容から判断して正確に制御する
ことを必要とする第１のパラメータ群と必要としない第
２のパラメータ群に分類し、第１のパラメータ群に影響
を与えないロボットの関節変位の中から少なくとも一つ
の変位について、次式に示す用にヤコビ行列（ヤコビア
ン）を用いてその近似解を求める。即ち、ロボットのエ
ンドエフェクタ先端の現在の位置と姿勢及び次の移動す
べき位置とその位置での姿勢を表わすベクトルをそれぞ
れベクトルＸ₀及びベクトルＸ₁とし、ベクトルＸ₀及
びベクトルＸ₁に対応するロボットの関節変位を示すベ
クトルをそれぞれベクトルθ₀及びベクトルθ₁、ヤコ
ビアンをＪ（ベクトルθ）として表わすとすれば、上述
の近似解ベクトルθ₁は、

【０００６】

【数１】

【０００７】として求められる。そして、この様にして
求めた関節変位の一部の近似解ベクトルθ₁を用いて、
前記第１のパラメータの値を満たすように残りの関節変
位の解を求めるものである。この場合、逆変換の変換式
から近似解として求めた関節変位を除外することにより
その分の数だけ逆変換の変換式の未知数を減少させるこ
とができ、多くの場合残りの関節変位について解析的に
解を求めることができるようになる。

【０００８】

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

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

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

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

【００１２】

【課題を解決するための手段】この発明の第１の方法で
は、ロボット設置面に略垂直に設置された第１アーム
と、第１アームに結合してロボット設置面に略平行な平
面内に並進自由度を有するとともに、第１アームに対し
て定まる所定の平面内に直接的又は間接的に回転自由度
を有する第２アームと、第２アームの先端部に略直角に
結合して第２アームの軸方向まわりの回転自由度とそれ
自身の軸まわりの回転自由度とを有する第３アームと、
第３アームの先端部に略直角に結合したエンドエフェク
タとを有する円筒座標ロボットの駆動制御方法に関し、
（ａ）エンドエフェクタの先端の位置及び姿勢を指定す
るステップと、（ｂ）エンドエフェクタの軸を含み且つ
所定の平面に垂直な平面がエンドエフェクタの軸方向を
法線方向とする平面と交差する際の交線と第３アームの
軸方向とのなす角度の余弦の逆数、又は当該角度の正弦
の逆数のうち、いずれかを選択するステップと、（ｃ）
ステップ（ｂ）で選択された逆数を変数とする４次方程
式の解を算出するステップと、（ｄ）４次方程式の解よ
り位置及び姿勢に対応する当該角度の値を求めるステッ
プと、（ｅ）当該角度の値を用いて第１、第２及び第３
アームの各駆動量を決定するステップと、（ｆ）駆動量
に応じた駆動出力をそれぞれ第１、第２及び第３アーム
の各駆動機構に与えることにより第１、第２及び第３ア
ームの駆動を行うステップとを備える様にしたものであ
る。

【００１３】また、第２の方法では、第１の方法に於け
るロボットと同様の第１、第２及び第３アームとエンド
エフェクタとを有する円筒座標ロボットの駆動制御方法
に関し、（ａ）エンドエフェクタの先端の位置及び姿勢
を複数指定するステップと、（ｂ）姿勢が予め定められ
た特定領域内に有るか否かを判断するステップと、
（ｃ）姿勢が前記特定領域内に有る場合には、エンドエ
フェクタの軸を含み且つ所定の平面に垂直な平面がエン
ドエフェクタの軸方向を法線方向とする平面と交差する
際の交線と第３のアームの軸方向とのなす角度の余弦の
逆数、又は角度の正弦の逆数のうち、いずれかを選択
し、選択された逆数を変数とする４次方程式の解を算出
するとともに、当該４次方程式の解より位置及び姿勢に
対応する角度の値を求めるステップと、（ｄ）姿勢が前
記特定領域外に有る場合には、当該角度の余弦、又は前
記角度の正弦のうち、いずれかを変数に選択し、変数に
関する４次方程式の解を算出するとともに、当該４次方
程式の解より位置及び姿勢に対応する当該角度の値を求
めるステップと、（ｅ）当該角度の値を用いて第１、第
２及び第３アームの各駆動量を決定するステップと、
（ｆ）駆動量に応じた駆動出力をそれぞれ第１、第２及
び第３アームの各駆動機構に与えることにより第１、第
２及び第３アームの駆動を行うステップとを備える様に
したものである。

【００１４】

【作用】この発明に係る駆動制御方法では、エンドエフ
ェクタの軸を含み第１アームに対して定まる所定の平面
に垂直な平面がエンドエフェクタの軸方向を法線方向と
する平面と交差する際の交線と第３アームの軸方向との
なす仮想的な角度と、第２アームの当該所定の平面内に
於ける旋回角度との幾何学的関係に着眼している。

【００１５】即ち、第１の方法では、仮想的な角度の正
弦の逆数又は余弦の逆数について成立する４次方程式を
直接解析的に解き、得られた解より旋回角度を決定す
る。そして、この旋回角度値をエンドエフェクタの位置
と姿勢とに関する逆変換式に代入することにより、各ア
ームの駆動量を厳密に決定している。更に、得られた各
アームの駆動量に基づいて各アームを駆動する。その結
果、エンドエフェクタの先端は、指定された位置へ誤差
なく正確に移動し、しかも指定された姿勢を保ってい
る。

【００１６】又、第２の方法では、指定された複数の姿
勢の内、姿勢が所定の範囲内にあると判断された場合に
は、仮想的な角度の正弦の逆数又は余弦の逆数について
成立する４次方程式を直接解析的に解き、得られた解よ
り旋回角度が決定される。一方、姿勢が所定の範囲外に
あると判断された場合には、仮想的な角度の正弦又は余
弦について成立する４次方程式を直接解析的に解き、得
られた解より旋回角度が決定される。従って、指定され
た位置が絶対座標系内のいずれの位置であっても、エン
ドエフェクタの先端は、その指定された位置へ誤差なく
正確に移動し、しかも指定された姿勢を保っている。

【００１７】

【実施例】

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

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

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

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

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

【００２２】また、エンドエフェクタとしてのトーチ４
０が第３アーム２３の先端部３２に略直角に結合されて
おり、トーチ４０の長さ、即ちトーチ４０の先端の点Ｎ
とトーチ４０の軸Ａ４と第３アーム２３の先端の点Ｑの
距離は、一定値ｄ₂である。

【００２３】尚、トーチ４０にはワーク検知用センサ
（図示せず）が備えつけられており、トーチ４０自身
は、自身の軸Ａ４を回転軸としてγ方向に回転すること
ができる。トーチ４０自身はその軸Ａ４まわりに完全回
転対称性を有しており、そのロール角に相当する角度γ
は上記センサの位置を定めるためにのみ意味を持つ。こ
のため、角度γは座標変換によって決定される値ではな
く、別個独立に指定すべき角度である。後述する座標変
換において角度γを決定する式がないのはこのような事
情による。

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

【００２５】

【００２６】一方、図３はトーチ４０の先端（点Ｎ）の
位置及び姿勢を示す説明図であり、ここでは点Ｑを原点
としたｘ’系により表わされている。同図に於いて、
ｘ’系の各軸方向は、Ｘ系の各軸方向に対応する様に設
定されている。即ち、ｘ’軸，ｙ’軸及びｚ’軸は、そ
れぞれＸ軸，Ｙ軸及びＺ軸に平行である。ここで原点Ｑ
の位置は、Ｘ系表示ではＱ（ｘ_Q，ｙ_Q，ｚ_Q）として
表される。

【００２７】先ず、点Ｎの姿勢は、ベクトルＱＮとｚ’
軸とのなす角度（天頂角）θ及びベクトルＱＮのｘ’
ｙ’平面への射影ベクトルとｘ’軸とのなす角度（方位
角）φにより決定される。

【００２８】又、その位置は、位置座標（ｘ’，ｙ’，
ｚ’）により決定される。従って、Ｘ系表示の点Ｎの位
置座標（ｘ，ｙ，ｚ）とｘ’系表示の位置座標（ｘ’，
ｙ’，ｚ’）との間には、ｘ＝ｘ_Q＋ｘ’，ｙ＝ｙ_Q＋
ｙ’及びｚ＝ｚ_Q＋ｚ’の関係が成立する。

【００２９】Ｂ．装置の電気的構成図４は、レーザ溶接ロボットＲＢの制御系の電気的構成
を示すブロック図である。この制御系は、ＣＰＵ５１１
やメモリ５１２等からなるコントローラ５１を有してお
り、ソフト的に制御される。又、ティーチングボックス
５２を操作することによってティーチングデータの取込
みなどが行なわれる。コントローラ５１からの各種指令
信号（駆動信号Ｄ₁等）は、データバス５７を介して各
構成部分（第１の駆動機構５３１等）に与えられる。

【００３０】又、オペレータは、キーボード等の入出力
装置５６を用いて、コントローラ５１のソフト制御に必
要な各種データを設定することができる。

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

【００３２】又、レーザー電源５４はレーザー発振器５
５に電力を供給するための電源であり、コントローラ５
１からの指令に応じて作動する。そして、レーザー発振
器５５で発生したレーザービームＬＢは、ロボットＲＢ
の各アーム内に設けたミラーによって順次に反射されつ
つトーチ４０の先端からワークに向けて照射される。

【００３３】Ｃ．駆動制御方法図５及び図６は、ロボットＲＢを駆動制御するためのス
テップを示すフローチャートであり、図７〜図９は図５
におけるステップＳ６を詳細に説明するフローチャート
である。

【００３４】（Ｃ−１）ステップＳ１先ず、特定領域の設定が行われる。即ち、オペレータ
は、入出力装置５６を用いて、予めコントローラ５１に
特定領域のデータを与える。この特定領域とは姿勢の
内，天頂角θに関して特別に定められた領域であり、テ
ィーチング点又は補間点の天頂角θがこの領域内に該当
する場合には、後述する改良ｇ法が適用されることとな
る。尚、この様な特定領域を設定する理由については後
述する。本実施例では、特定領域は０°〜（０°＋Δ
θ）又は（１８０°−Δθ）〜１８０°である。

【００３５】（Ｃ−２）ステップＳ２（ティーチング
工程）ティーチングデータＸ_i（ｉ＝１，２，…，ｎ）の作成
と記憶とが行われる。尚、ティーチングデータＸ_iと
は、ティーチング点Ｐ_iの位置（ｘ，ｙ，ｚ）及び姿勢
（θ，φ）と、それらの位置等を与える各アームの駆動
量（実測値）とを含む概念として用いられている。

【００３６】先ず、オペレータは、各ティーチング点Ｐ
_i毎にマニュアルでトーチ４０の先端の位置決めを行
う。その際、各ティーチング点Ｐ_i毎の各アームの駆動
量（α系の値）が、エンコーダＥ₁〜Ｅ₅により読込ま
れる。

【００３７】そして、各アームの駆動量を順変換するこ
とにより、各ティーチング点Ｐ_iの位置（ｘ_i，ｙ_i，
ｚ_i）と姿勢（θ_i，φ_i）とが計算される。ここで順
変換に関しては、既述した通り厳密解を解析的に求める
ことが可能であり、順変換の手順は予めコントローラ５
１内にプログラミングされている。そして計算された位
置（ｘ_i，ｙ_i，ｚ_i）と姿勢（θ_i，φ_i）とは、テ
ィーチングデータＸ_iとしてメモリに格納される。

【００３８】（Ｃ−３）ステップＳ３以上の準備ステップが終了すると、ＣＰＵ５１１は発振
指令信号をレーザー電源５４に対して発する。これによ
り、レーザー電源５４はレーザー発振器５５をオン状態
にする。

【００３９】次のステップ群Ｓ１０は、いわゆる再生動
作に対応している。

【００４０】（Ｃ−４）ステップＳ４ティーチング点Ｐ_iに関するティーチングデータＸ_iの
読出しが行われる。そして、読出された各アームの駆動
量に基づき、コントローラ５１は各駆動機構５３１〜５
３５に対して駆動信号Ｄ₁〜Ｄ₅を発する。その結果、
トーチ４０の先端（点Ｎ）は、ティーチング点Ｐ_iに移
動する。

【００４１】（Ｃ−５）ステップＳ５ティーチング点Ｐ_iと該点Ｐ_iに隣接するティーチング
点Ｐ_i+1との間を補間し、補間点Ｐ_ijのデータＸ_ij〔位
置（ｘ_ij，ｙ_ij，ｚ_ij），姿勢（θ_ij，φ_ij）〕が計算
される。このような補間方法としては、よく知られてい
る通り、線型補間や二次曲線で補間する方法等がある。
これらの補間方法の説明については、省略する。尚、補
間点Ｐ_ijのデータＸ_ijはＸ系の値である。従って、トー
チ４０の先端を補間点Ｐ_ijへ移動するための適切な各ア
ームの駆動量（α系）を決定する必要がある。

【００４２】（Ｃ−６）ステップＳ６Ｘ系からα系への逆変換が行われる。

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

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

【００４５】図１８において、トーチ４０の先端の位置
Ｎと姿勢とが与えられると、自動的に点Ｑの位置も定ま
る。

【００４６】ここで、次の（ｉ）〜（ｉｉｉ）の条件を
満足する円Ｃを定義する。

【００４７】（ｉ）その中心が点Ｑであること。

【００４８】（ｉｉ）半径がｄであること。

【００４９】（ｉｉｉ）ベクトルＱＮを法線とする平
面内に存在すること。

【００５０】その結果、点Ｐは円Ｃの円周上に存在し、
ベクトルＱＰは円Ｃの半径ベクトルとなっている。そし
て、ロボットＲＢの各軸構成から明らかなように、点Ｐ
について、

【００５１】

【数２】

【００５２】

【数３】

【００５３】の関係が成立する。

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

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

【００５６】次に、以上述べた幾何学的関係を基にし
て、逆変換の厳密解を求めることとなるのであるが、こ
こでは図７及び図８に示すフローチャートに従って、図
１８とともに以後のステップを説明することとする。

【００５７】（Ｃ−６−１）ステップＳ６１補間点Ｐ_ijの天頂角θ_ijが特定領域内に有るか否かが判
断される。その際、天頂角θ_ijが特定領域内に有ると判
断された場合には、ステップＳ６Ｂで詳述する改良ｇ法
が適用されることとなる。ここでは、先ず、天頂角θ_ij
が特定領域外である場合について詳述する。この場合に
は、ステップＳ６２〜Ｓ６９及びＳ６Ａで示されるｇ法
が適用されることとなる。

【００５８】（Ｃ−６−２）ｇ法（Ｃ−６−２−１）ｇ− cosδ法ステップＳ６２パラメータｙ₀が−Δｙ₀から＋Δｙ₀の範囲内の値で
あるか否か（０近傍値であるか否か）が判断される。こ
の判断が必要とされる理由については後述説明で明らか
となるが、本ステップ自身は後述するｇ法の形式を定め
る点で意義がある。尚、Δｙ₀は予めオペレータにより
定められた一定値（１より十分に小さな値）であり、そ
の値は使用するＣＰＵ５１１の計算容量を考慮して決定
される。

【００５９】ここでは、先ず、パラメータｙ₀が０近傍
値でない一般的な場合（ステップＳ６３〜Ｓ６９）につ
いて詳述することにする。本出願人は、この様なケース
に対する制御方法をｇ法乃至はｇ− cosδ法と呼んでい
る。

【００６０】尚、パラメータｙ₀は後述する数４９によ
り決定されるので、本ステップは判断の前段階としてパ
ラメータｙ₀の算出ステップを含んでいる（図示せ
ず）。

【００６１】ステップＳ６３第２アーム２２をＺ軸のまわりに仮想的に旋回すること
を考える。即ち、第２アーム２２の先端の点が点Ｐにあ
る状態から仮想的な角度φ（図１８の例においては、角
度Θとは逆まわりとなっている。）だけ旋回することに
よって、トーチ４０の軸Ａ４方向のベクトルＱ₁Ｎ₁の
ＸＹ平面への射影ベクトルＱ₁′Ｎ₁がＸ軸に平行にな
ったものとする。この状態における円Ｃに対応する円を
円Ｃ₁、円Ｃ₁のＸＹ平面への射影により生じる楕円を
楕円Ｅ₁′とする。又、ベクトルＱ₁Ｎ₁を含みＸＹ平
面に垂直な平面Ｓ₁が円Ｃ₁と交差する点を点Ｒ₁，Ｔ
₁とし、円Ｃ₁上の３点Ｐ₁，Ｐ₁，Ｔ₁の楕円Ｅ₁′
上の対応する点を、それぞれ点Ｐ₁′，Ｒ₁′，Ｔ₁′
とする。

【００６２】以上の様に仮想的にベクトルＭＰを角度φ
だけ旋回することにより、点Ｐと点Ｑとの幾何学的関係
式を導出することが容易となる。この点について、以下
に説明する。

【００６３】図１９は、ベクトルＱ₁Ｎ₁及び円Ｃ₁を
ＸＺ平面へ射影した場合を示す説明図である。同図に於
いて、ベクトルＱ₁″Ｎ₁″がベクトルＱ₁Ｎ₁のＸＺ
平面への射影ベクトルを、楕円Ｅ₁″が円Ｃ₁をＸＺ平
面へ射影した結果生ずる楕円を，点Ｐ₁″，Ｒ₁″，Ｔ
₁″が点Ｐ₁，Ｒ₁，Ｔ₁のＸＺ平面への射影点を示し
ている。ベクトルＱＮについて図３に示した様に、トー
チ４０、即ちベクトルＱ₁Ｎ₁の姿勢のうち、ベクトル
Ｑ₁Ｎ₁とＺ軸とのなす角度が角度θであるから、図１
９において、ベクトルＱ₁″Ｎ₁″とＺ軸とのなす角度
もまた角度θで表わされる。従って、直線Ｒ₁″Ｔ₁″
とＸ軸とのなす角もまた角度θである。

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

【００６５】

【数４】

【００６６】

【数５】

【００６７】

【数６】

【００６８】ここでは、余弦，正弦の記載に関して、数
７，数８の様な簡略形が用いられている。以後の説明に
おいてもこれらの簡略形が使用されている。

【００６９】

【数７】

【００７０】

【数８】

【００７１】ステップＳ６４ cosδの４次方程式の解が導出される。その様な cosδ
の４次方程式は、図１８の位置関係より容易に導き出さ
れる。

【００７２】即ち、ベクトルＱ₁Ｐ₁とベクトルＭＰ
₁とは直交しているので、

【００７３】

【数９】

【００７４】及び

【００７５】

【数１０】

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

【００７７】

【数１１】

【００７８】

【数１２】

【００７９】

【数１３】

【００８０】に変形される。更に数１１の両辺を２乗
し、２乗後の関係式を

【００８１】

【数１４】

【００８２】の関係を用いて展開すると、数１５に示す
様なＣδに関する４次方程式が得られる。

【００８３】

【数１５】

【００８４】ここで、変数Ｘ_C，係数ａ，ｂ，ｃ及びｄ
は、それぞれ

【００８５】

【数１６】

【００８６】

【数１７】

【００８７】

【数１８】

【００８８】

【数１９】

【００８９】

【数２０】

【００９０】によって表される。

【００９１】数１５の４次方程式は、よく知られたデカ
ルトの方法によって解析的に求められる。即ち、解Ｘ_C1
〜Ｘ_C4は、以下に示す数２１〜数２４により与えられ
る。従って、これらの関係式（数２１〜数２４）を予め
プログラミングしておけば、図４に示すコントローラ５
１によって厳密解Ｘ_C1〜Ｘ_C4を容易に算出することがで
きる。

【００９２】

【数２１】

【００９３】

【数２２】

【００９４】

【数２３】

【００９５】

【数２４】

【００９６】尚、数２１〜数２４の右辺第２項の符号に
関しては、ａ≧０のとき上側の符号が採用され、ａ＜０
のとき下側の符号が採用されるものと約束されている。
また、定数Ｕ，Ｖ，Ｗは、数１７〜数２０で与えられる
係数ａ〜ｄによって与えられるものであり、ここではそ
れらの具体的記載は省略されている。

【００９７】ステップＳ６５〜Ｓ６８ステップＳ６４で求められた余弦Ｃδの解Ｘ_C1〜Ｘ_C4に
基づき正弦Ｓδが求められる（ステップＳ６５）。そし
て、これらの値（Ｃδ，Ｓδ）より、交差角δが算出さ
れる（ステップＳ６６）。

【００９８】更にステップＳ６７及びＳ６８では、ベク
トルＯＰ₁′とＸ軸とのなす角度Θ′（図１８参照）が
求められた後、角度Θ′（仮想的な旋回角）から角度Θ
（旋回角）への変換が行われる。この変換により、第２
アーム２２の本来の旋回角Θが求められる。尚、角度
Θ′を求めるためには点Ｐ₁′（ｘ，ｙ，０），従って
点Ｑ₁′（ｘ₀，ｙ₀，０）の位置座標を求める必要が
ある。この点Ｑ₁′の位置座標は、以下で述べる様に、
トーチ４０の先端の位置より求められる。そこで、以下
においては、ステップＳ６５〜Ｓ６８に於ける原理の説
明及び当該ステップで必要な諸式の導出を行う。

【００９９】今、α系からｘ系への順変換を考えること
とする。即ち、トーチ４０の先端の位置ベクトルをベク
トルｐ、姿勢を（ベクトルｎ，ベクトルｏ，ベクトル
ａ）で表わすとすれば、α系からｘ系への座標変換マト
リックスＴは数２５，数２６により表わされる。

【０１００】

【数２５】

【０１０１】

【数２６】

【０１０２】ここで、各マトリックスＡΘ，Ａ_zxα，Ａ
β及びＡγは、ロボットＲＢの各アームの自由度に伴う
座標変換マトリックスであり、数２７〜数３０により表
わされる。

【０１０３】

【数２７】

【０１０４】

【数２８】

【０１０５】

【数２９】

【０１０６】

【数３０】

【０１０７】従って、数２７〜数３０を用いて数２６を
展開すれば、座標変換マトリックスＴの各要素を求める
ことができる。ここでは、ベクトルａとベクトルｐとの
各要素のみを以下に示す。

【０１０８】

【数３１】

【０１０９】

【数３２】

【０１１０】

【数３３】

【０１１１】

【数３４】

【０１１２】

【数３５】

【０１１３】

【数３６】

【０１１４】このようにして得られた順変換の式（数３
１〜数３６）より、逆変換の解（Θ，ｘ_s，ｚ_s，α，
β）が求められる。

【０１１５】そこで、数３１〜数３３を数３４〜数３６
に代入すると、

【０１１６】

【数３７】

【０１１７】

【数３８】

【０１１８】

【数３９】

【０１１９】が得られる。ここで、位置ベクトルｐ
_mは、

【０１２０】

【数４０】

【０１２１】

【数４１】

【０１２２】

【数４２】

【０１２３】と定義されている。即ち、位置ベクトルｐ
_mの各成分ｐ_xm，ｐ_ym，ｐ_zmは、図１８における点Ｑの
位置座標を与える。

【０１２４】再び、図１８に目を向ければ、数４３〜数
４５で与えられる幾何学的関係が成立していることが理
解される。

【０１２５】

【数４３】

【０１２６】

【数４４】

【０１２７】

【数４５】

【０１２８】ここで、角度ψはベクトルＯＱ₁′とＸ軸
とのなす角度である。又、点Ｑの位置座標（ｐ_xm，
ｐ_ym，ｐ_zm）を用いれば、

【０１２９】

【数４６】

【０１３０】が得られる。従って、

【０１３１】

【数４７】

【０１３２】

【数４８】

【０１３３】

【数４９】

【０１３４】

【数５０】

【０１３５】で与えられる関係式が成立し、角度ψ，点
Ｑ₁（したがって点Ｑ₁′）の位置座標がトーチ４０の
先端の位置より求められる。

【０１３６】ここでatan２（ｐ_ym，ｐ_xm）は、

【０１３７】

【数５１】

【０１３８】

【数５２】

【０１３９】となる角度ξを一意に与える関数である。

【０１４０】一方、交差角δは次の手順により求められ
る。即ち、余弦Ｃδは既にステップＳ６４に於いて算出
されており（数２１〜数２４）、正弦Ｓδは数１１によ
り算出される（ステップＳ６５）。従って、数４８，数
４９で求めたｘ₀，ｙ₀の値を用いることより、交差角
δは、

【０１４１】

【数５３】

【０１４２】より決定される（ステップＳ６６）。

【０１４３】次に、以上求められた点Ｑ₁の位置座標Ｑ
₁（ｘ₀，ｙ₀，ｚ₀）及び交差角δに基づき、角度
Θ′が求められる。即ち、数４８〜数５０を数４〜数６
へ代入すれば、点Ｐ₁の位置座標Ｐ₁（ｘ，ｙ，ｚ）な
いしは点Ｐ₁′の位置座標（ｘ，ｙ，０）が求められ
る。その結果、角度Θ′は数５４により決定される（ス
テップＳ６７）。

【０１４４】

【数５４】

【０１４５】そして、ベクトルＭＰ₁を角度−φだけＺ
軸のまわりに旋回することにより、角度Θが求められる
こととなる（図１８参照）。故に、角度Θは、

【０１４６】

【数５５】

【０１４７】により決定される（ステップＳ６８）。

【０１４８】（Ｃ−６−２−１）ｇ− sinδ法ステップＳ６２パラメータｙ₀が０近傍の値である場合には、ステップ
Ｓ６Ａのｇ− sinδ法の適用により旋回角Θが求められ
る。これは、次の様な理由によるものである。

【０１４９】即ち、ｇ− cosδ法では、先ず余弦Ｃδを
求め、次に正弦Ｓδを求めた上で交差角δの算出を行っ
ていた。しかし、数１１より明らかな様に、パラメータ
ｙ₀が極めて０に近い値のときにはＣＰＵ５１１内の計
算過程で桁落ちが大きくなり、各アームの駆動制御が困
難となる問題が生じる。実際、パラメータｙ₀の値が１
０^-3のオーダーのときには、逆変換による駆動誤差は数
mmにもなっていた。

【０１５０】そこで、この様な問題点の解決手段として
考えだされたのが本ｇ− sinδ法であり、本方法は、先
に正弦Ｓδを求めておき、次に余弦Ｃδを求めることに
より交差角δを求めようとするものである。この方法に
よれば、前述の桁落ちという問題は発生せず、正確な駆
動制御が可能となる。以下、図９に示す各ステップに基
づき、本ｇ− sinδ法の詳細を説明する。

【０１５１】ステップＳ６Ａ１〜６Ａ２ステップＳ６Ａ１は図７のステップＳ６３と同一である
ため、本ステップの詳細な記述を省略する。

【０１５２】ステップＳ６Ａ２では、正弦Ｓδに関する
４次方程式の解が算出される。この正弦Ｓδに関する４
次方程式は、余弦Ｃδの場合と同様の考察により導出さ
れる。即ち、数１４を用いて数１１を変形すれば、

【０１５３】

【数５６】

【０１５４】が得られる。そして、数５６の両辺を自乗
し、

【０１５５】

【数５７】

【０１５６】とおけば、次の関係式

【０１５７】

【数５８】

【０１５８】が得られる。ここで、各係数ａ’，ｂ’，
ｃ’及びｄ’は、それぞれ

【０１５９】

【数５９】

【０１６０】

【数６０】

【０１６１】

【数６１】

【０１６２】

【数６２】

【０１６３】で表される。

【０１６４】数５８の解Ｘ_S1〜Ｘ_S4は、数１５と同様
に、デカルトの方法により求められる。即ち、数２１〜
数２４に基づいて解Ｘ_S1〜Ｘ_S4を与えるプログラムを同
様に作成し、このプログラムをメモリ５１２に記憶して
おくことにより、解Ｘ_S1〜Ｘ_S4をソフト処理により求め
ることが可能となる。しかも、数５９〜数６２はパラメ
ータｙ₀をその分母に含んでいないため、パラメータｙ
₀が０近傍値であっても、解Ｘ_S1〜Ｘ_S4の算出に際して
十分良好な計算精度が得られる。

【０１６５】ステップＳ６Ａ３前ステップにより算出された正弦Ｓδの値を用いて、余
弦Ｃδの値が算出される。余弦Ｃδの導出は数１１及び
数１４より得られる。即ち、余弦Ｃδは、

【０１６６】

【数６３】として表され、その分母にパラメータｙ₀を含まない。
従って、パラメータｙ₀の値如何に係わらず、正確に余
弦Ｃδを算出することが可能となる。尚、数６３は、そ
の分母に（ｘ_oＣθ）を含んでいる。しかし、パラメー
タｙ₀が０近傍（−Δｙ₀〜＋Δｙ₀）となる範囲にお
いては（ｘ_oＣθ）が０近傍となる様なケースが発生し
ないことが、シミュレーションや実験等により確認され
ている。従って、（ｘ_oＣθ）を考慮する必要はない。
この点は、後述するパラメータｙ₀が１近傍又は−１近
傍の値となる場合についても成立する。

【０１６７】ステップＳ６６〜Ｓ６８ステップＳ６Ａにより正弦Ｓδと余弦Ｃδとが正確に算
出されたので、交差角δをも正確に算出することが可能
となる。従って、本ｇ− sinδ法についても同様に、数
４７に基づき交差角δが算出された後（ステップＳ６
６）、ステップＳ６７及びＳ６８により旋回角Θが算出
される。

【０１６８】（Ｃ−６−３）改良ｇ法ステップＳ６１天頂角θが特定領域内にあると判断された場合には、ス
テップＳ６Ｂの改良ｇ法が適用される。この理由は次の
通りである。

【０１６９】即ち、ｇ− cosδ法によれば、余弦Ｃδの
４次方程式（数１５）を解くことによって交差角δが算
出される。

【０１７０】しかし、上記４次方程式の各係数ａ，ｂ，
ｃ及びｄ（数１７〜数２０）は、その分母に正弦Ｓδを
含んでいる。従って、天頂角θが特定領域内（０≦θ≦
Δθ，１８０−Δθ≦θ≦１８０）にある場合には、係
数ａ〜ｄの全てが大きな値となる。しかも数１５の解
（数２１〜数２４）の算出に際しては、数６４〜数７０
に示される通り、各係数ａ〜ｄについて２乗、３乗等
（ａ²〜ｄ²等）の複雑な計算が必要とされる。尚、数
６８〜数７０の量ｕ，ｖ及びｗは、それぞれ数２１〜数
２４で示された量ｕ，ｖ及びｗである。

【０１７１】

【数６４】

【０１７２】

【数６５】

【０１７３】

【数６６】

【０１７４】

【数６７】

【０１７５】

【数６８】

【０１７６】

【数６９】

【０１７７】

【数７０】

【０１７８】その結果、特定領域内において数１５の解
Ｘ_c1〜Ｘ_c4を求めようとする場合には、ＣＰＵ５１１に
於ける演算がオーバーフローするという問題点が生ず
る。即ち、解Ｘ_c1〜Ｘ_c4の精度が極端に低下することと
なる。この点に関しては、ｇ−sinδ法についても同様
である。

【０１７９】勿論、この様な問題点はＣＰＵ５１１の計
算容量とも密接に関係しているため、計算容量を大きく
することによって係る問題点の発生を抑制することも可
能である。しかし、汎用的なＣＰＵ５１１を用いても該
問題点を生じさせない様なフレシキブルな解決方法が求
められるのである。

【０１８０】そこで本発明では、係る解決方法として、
改良ｇ法が適用される。以下、改良ｇ法に於ける各ステ
ップを、図１０〜図１２に基づき説明する。

【０１８１】尚、本実施例では、特定領域を定めるΔθ
の値は３ｄｅｇ．〜１０ｄｅｇ．の範囲内で選択されて
いる。勿論、このΔθの設定可能範囲は、上記範囲に限
定されるものではない。即ち、設計者は、ＣＰＵ５１１
の計算容量や量ｘ₀，ｙ₀の値等を考慮しながら、Δθ
の値を任意に定めることができる。

【０１８２】ステップＳ６Ｂ１パラメータｙ₀が０近傍値か１近傍値かそれとも−１近
傍値か否かが判断される。即ち、パラメータｙ₀が、−
Δｙ₀₁≦ｙ₀≦Δｙ₀₁，１−Δｙ₀₂≦ｙ₀≦１＋Δｙ₀₂
又は−１−Δｙ₀₃≦ｙ₀≦−１＋Δｙ₀₃（Δｙ₀₁，Δｙ
₀₂，Δｙ₀₃は１よりも十分に小さな値）の範囲内にある
か否かが判断される。本ステップもまた、計算精度の低
下による駆動量の誤差発生を回避するために適用可能な
改良ｇ法のモードを選択するという意義を有する。尚、
Δｙ₀₁，Δｙ₀₂，Δｙ₀₃もまた、ＣＰＵ５１１の計算容
量を考慮して任意に定められる。

【０１８３】即ち、上記範囲内にパラメータｙ₀が存在
しないと判断された場合には、ステップＳ６Ｂ２の改良
ｇ− cosδ法が適用される。しかし、パラメータｙ₀が
上記範囲内にあると判断された場合には、改良ｇ− cos
δ法ではパラメータｙ₀の値による計算精度の低下が発
生するため、ステップＳ６Ｂ３の改良ｇ− sinδが適用
される。以下、ステップＳ６Ｂ２の改良ｇ− cosδ法か
ら説明することにする。

【０１８４】改良ｇ− cosδ法図１１は改良ｇ− cosδ法の主要部分を示すフローチャ
ートであり、本図に基づき説明する。

【０１８５】−１ステップＳ６Ｂ２１第１アーム２２がＺ軸のまわりに仮想的に旋回される。
このステップ自体はステップＳ６３と同一である。

【０１８６】−２ステップＳ６Ｂ２２（ cosδ）^-1の４次方程式の解が算出される。即ち、特
定領域では、 cosδの逆数を変数とする４次方程式を導
出し、この４次方程式を解くことによって、天頂角θの
値による上記問題点が克服される。その様な４次方程式
は、

【０１８７】

【数７１】で表される新たな変数Ｘ_c’について数１５を変形する
ことにより得られる。即ち、導出すべき４次方程式は、

【０１８８】

【数７２】として与えられる。ここで、各係数Ａ〜Ｄは、それぞれ

【０１８９】

【数７３】

【０１９０】

【数７４】

【０１９１】

【数７５】

【０１９２】

【数７６】として表される。

【０１９３】数７３〜数７６より明白な通り、各係数Ａ
〜Ｄの分母には正弦Ｓδが含まれていない。従って、天
頂角θが特定領域内の値であっても、ＣＰＵ５１１の動
作がオーバーフローすることはない。即ち、天頂角θが
０ｄｅｇ．又は１８０ｄｅｇ．に限りなく近くなるケー
スにおいても、正確に解Ｘ_c’及び解Ｘ_cを算出するこ
とが可能となる。

【０１９４】尚、数７２の解Ｘ_c’の算出に当たって
も、数１５と同様の方法（デカルト法）が適用される。
即ち、解Ｘ_c’は、数２１〜数２４の関係式に基づき定
められるＣＰＵ５１１内のソフト処理により算出され
る。

【０１９５】−３ステップＳ６Ｂ２３本ステップでは、前記ステップＳ６Ｂ２２で求められた
解Ｘ_c’に基づき、余弦Ｃδの値が求められる。余弦Ｃ
δは、数７７により算出される。

【０１９６】

【数７７】

【０１９７】−４ステップＳ６Ｂ２４改良ｇ法においても同様に、余弦Ｃδの値を用いて数１
１より正弦Ｓδが算出される。

【０１９８】改良ｇ− sinδ法パラメータｙ₀が０近傍値の場合には、数１１より明ら
かな様に正弦Ｓδを正確に算出することができない。
又、パラメータｙ₀が１近傍値か又は−１近傍値の場合
には、数７３〜数７６より明らかな様に、余弦Ｃδを正
確に算出することができない。従って、本ケースでは、
改良ｇ− sinδ法を適用することにより、天頂角θによ
る上記問題点とパラメータｙ₀による上記問題点とを同
時に解決するものである。以下、図１２に基づき、改良
ｇ− sinδ法の主要部を説明する。

【０１９９】同図に示す様に、第１アームの仮想的旋回
の想定（ステップＳ６Ｂ３１）に基づき、正弦Ｓδの逆
数に関する４次方程式の解が求められる（ステップＳ６
Ｂ３２）。この４次方程式は、数７８で表される関係式
を用いて正弦Ｓδに関する４次方程式（数５８）を変形
することにより導出される。即ち、正弦Ｓδの逆数に関
する４次方程式は数７９として与えられる。

【０２００】

【数７８】

【０２０１】

【数７９】

【０２０２】ここで各係数Ａ’〜Ｄ’は、それぞれ数８
０〜数８３により表される。

【０２０３】

【数８０】

【０２０４】

【数８１】

【０２０５】

【数８２】

【０２０６】

【数８３】

【０２０７】本４次方程式についても同様にデカルトの
方法を適用することができ、解Ｘ_S1’〜Ｘ_S4’をソフト
処理により求めることが可能となる。しかも、各係数
Ａ’〜Ｄ’はパラメータｙ₀をその分母に含んでいない
ため、本ソフト処理はパラメータｙ₀の値による影響を
受けることはない。

【０２０８】次にステップＳ６Ｂ３３では、本来求める
べき正弦Ｓδの値が数８４に基づき算出される。

【０２０９】

【数８４】

【０２１０】更にステップＳ６Ｂ３４では、数６３に基
づき余弦Ｃδの値が算出されるが、同様に、本ステップ
もパラメータｙ₀の値による影響を受けることはない。

【０２１１】ステップＳ６６〜Ｓ６８改良ｇ− cosδ法又は改良ｇ− sinδ法により余弦Ｃδ
及び正弦Ｓδの値が求められた後は、同様に交差角δが
算出され（ステップＳ６６）、更に旋回角Θが算出され
る（ステップＳ６７〜Ｓ６８）。

【０２１２】（Ｃ−６−４）ステップＳ６９ｉ）旋回角Θの厳密解が求められたので、該厳密解を
座標変換式（数３７〜数３９）に代入することにより、
第２のアーム２２のＸＹ平面内の並進量ｘ_s及び軸Ａ₂
まわりの回転角αが求められる。これらの算出は、次の
様にして行われる。

【０２１３】即ち、ｐ_zm＝ｚ₀であるから、数３９及び
数５０より、

【０２１４】

【数８５】

【０２１５】であり、又、数６は

【０２１６】

【数８６】

【０２１７】で表わされる。従って、数８５及び数８６
より

【０２１８】

【数８７】

【０２１９】となる。

【０２２０】一方、数３７及び数３８より並進自由度ｘ
_sは、

【０２２１】

【数８８】

【０２２２】により求められる。又、数８８を数３７に
代入すれば、

【０２２３】

【数８９】

【０２２４】が得られる。よって、角度αは、数８７及
び数８９より導かれる数９０により決定される。

【０２２５】

【数９０】

【０２２６】ｉｉ）次に、第２アーム２２のＺ軸方向
への並進自由度ｚ_sと軸Ａ３のまわりの回転角βとが決
定される。即ち、並進自由度ｚ_sは数５０及び数８５に
より決定され、回転角βは、数３１〜数３３より導かれ
る数９１又は数９２により決定される。

【０２２７】

【数９１】

【０２２８】

【数９２】

【０２２９】以上のステップにより、逆変換の解、即
ち、各アームの駆動量Θ，ｘ_s，ｚ_s，α，βが全て厳
密に決定されたことになり、ｇ− cosδ法によるステッ
プＳ６は終了する。

【０２３０】（Ｃ−７）ステップＳ７ステップＳ６で決定された各アームの駆動量Θ，ｘ_s，
ｚ_s，α，βはそれぞれ、駆動信号Ｄ₁，Ｄ₂，Ｄ₃，
Ｄ₄，Ｄ₅に変換される。この変換処理は、ＣＰＵ５１
１により行われる。そして、各駆動信号Ｄ₁〜Ｄ₅は、
データバス５７を介して各駆動機構５３１〜５３５に伝
達される。

【０２３１】一方、各駆動機構５３１〜５３５は各駆動
信号Ｄ₁〜Ｄ₅に応じた量だけ各アームを駆動する。そ
の結果、トーチ４０の先端はステップＳ５の計算により
求められた補間点Ｐ_ijへ移動する。その際のトーチ４０
の先端の位置及び姿勢は、勿論その補間点Ｐ_ijにおける
補間データＸ_ijと厳密に一致する。従って、トーチ４０
の先端より溶接部に向けて発射されたレーザビームＬＢ
は、その前の補間点Ｐ_ij-1と目標の補間点Ｐ_ij間をワー
ク線に沿って正確に移動することとなる。

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

【０２３３】それに対して、補間点Ｐ_ijが最後の補間点
であると判断されたときには、ステップＳ９へと進めら
れる。

【０２３４】（Ｃ−９）ステップＳ９本ステップでは、ティーチング点Ｐ_iが最後のティーチ
ング点か否かが判断される。この判断も又、コントロー
ラ５１によって行われる。そして、ティーチング点Ｐ_i
が最後のティーチング点でないと判断されたときにはス
テップＳ４へ戻り、次のティーチング点Ｐ_i+1のティー
チングデータＸ_i+1が読出され、一連のステップＳ５〜
Ｓ９が再び行われる。

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

【０２３６】以上の説明から明らかな通り、本実施例の
ポイントは、次の様に理解される。即ち、幾何学的に想
定された角度δ（交差角）という概念を導入し、特定領
域外では余弦Ｃδの４次方程式の厳密解をソフト的に処
理する。但し、バラメータｙ₀の値によっては余弦Ｃδ
に代えて正弦Ｓδを選択し、正弦Ｓδの４次方程式の厳
密解をソフト的に処理する。一方、特定領域内では余弦
Ｃδの逆数の４次方程式の厳密解をソフト的に処理す
る。但し、バラメータｙ₀の値によっては同様に正弦Ｓ
δを選択し、正弦Ｓδの逆数の４次方程式の厳密解をソ
フト的に処理することとする。これらの方法により、ト
ーチの移動点の全てについて、近似することなく厳密に
しかも簡単に逆変換を行えるようにした点である。

【０２３７】なお、参考として、既述解析による逆変換
アルゴリズムの流れを図２０（ｇ法）及び図２１（改良
ｇ法）に示す。両図において、カッコ付きの数字は上記
説明中において用いられた数式の番号を示している。ま
た、（＊）印はティーチングデータから直ちに得られる
Ｘ系の量であり、（＊＊）印は逆変換によって求めるべ
きα系の量を示している。

【０２３８】Ｄ．変形例（Ｄ−１）その１以上のレーザー溶接ロボットＲＢでは、基本的には、特
定領域外では余弦Ｃδに関する４次方程式の解を算出す
ることにより（ｇ− cosδ法）、特定領域内では余弦Ｃ
δの逆数に関する４次方程式の解を算出することにより
（改良ｇ− cosδ法）、各アームの駆動信号Ｄ₁〜Ｄ₅
が決定されていた。但し、パラメータｙ₀が０近傍値，
１近傍値乃至は−１近傍値の場合には、ｇ− sinδ法や
改良ｇ−sinδ法を選択的に用いていた。

【０２３９】しかし、正弦Ｓδを基本としてｇ法及び改
良ｇ法を展開することも可能である。即ち、特定領域外
では正弦Ｓδに関する４次方程式の解より駆動信号Ｄ₁
〜Ｄ₅を決定し、特定領域外では正弦Ｓδの逆数に関す
る４次方程式の解より駆動信号Ｄ₁〜Ｄ₅を決定するこ
とができる。この場合には、パラメータｙ₀の値を考慮
する必要がなく、天頂角θが特定領域内か否かを考慮す
れば足りるという利点がある。尚、この場合の特定領域
もまた、０deg.≦θ≦（０deg.＋Δθ），（１８０deg.
−Δθ）≦θ≦１８０deg.である。

【０２４０】その様な正弦Ｓδを基本としたｇ法及び改
良ｇ法を用いた制御方法を、図１３及び図１４に示す。
但し、本制御方法と図５及び図６で示された制御方法と
の相違点は、Ｘ系からα系への逆変換のステップ（Ｓ
６）の部分だけのみである。従って、図１３及び図１４
には、図５のステップＳ６に対応するステップのみが示
されている。

【０２４１】尚、本変形例に於ける電気的及び機械的構
成もまた、上述レーザー溶接ロボットＲＢにおける構成
と同一である。

【０２４２】ステップＳ６Ｃ１天頂角θが特定領域内に有るか否かが判断される。

【０２４３】ステップＳ６Ｃ２〜Ｓ６Ｃ３天頂角θが特定領域内であると判断された場合には、ｇ
− sinδ法が適用され、正弦Ｓδが先ず算出される。

【０２４４】ステップＳ６Ｃ４特定領域内においては、改良ｇ− sinδ法が適用され
る。改良ｇ− sinδ法の詳細を図１５に示すが、内容は
既述した通りである。

【０２４５】ステップＳ６Ｃ５〜Ｓ６Ｃ９ｇ− sinδ法または改良ｇ− sinδ法により算出された
正弦Ｓδの値より、余弦Ｃδの値が算出される。更に、
交差角δが算出される。その後、旋回角Θを始めとして
各駆動量が算出されるが、これらのソフト処理も既述し
た通りである。

【０２４６】以上述べた様に、本変形例では、特定領域
内では正弦Ｓδの逆数を算出することにより、特定領域
外では正弦Ｓδを算出することにより、全ての点Ｐ_i，
点Ｐ_ijにおいて各駆動量を正確に決定することができ
る。

【０２４７】（Ｄ−２）その２前述した各実施例では、トーチを移動すべき点が特定領
域内にあると判断された場合にのみ、改良ｇ法が適用さ
れていた。しかし、トーチを移動すべき点が特定領域内
のみならず特定領域外にある場合にも、改良ｇ法を適用
することも可能である。

【０２４８】改良ｇ− sinδ法を適用する場合天頂角θの全範囲にわたって改良ｇ− sinδ法を適用す
る。この場合には、天頂角θが９０°近傍となる場合及
びＣθ₃ ²＝ｘ₀ ²を満足する角度近傍となる場合を除
いて、既述した特定領域という概念が存在しないことと
なる。この点に関しては、数６５〜数６８の各係数Ａ〜
Ｄの分母を参照することによって理解することができ
る。

【０２４９】その様なステップＳ６Ｄを例示したのが、
図１６のフローチャートである。図１６もまた、図５の
ステップＳ６に相当するステップのみを示しており、本
変形例は図７のステップＳ６からステップＳ６１及びＳ
６２を除いた制御方法に相当している。本変形例では、
天頂角θの値によってｇ法と改良ｇ法とを使い分ける必
要がなく（上記例外を除く。）、しかもパラメータｙ₀
の値をも考慮する必要がないため、駆動量決定の手順を
簡易化できる効果がある。

【０２５０】改良ｇ− cosδ法を主として適用する
場合改良ｇ− cosδ法を主として適用する場合を図１７のス
テップＳ６Ｅに示す。本変形例でも既述した特定領域と
いう概念はなく、天頂角θの値によってｇ法と改良ｇ法
とを使い分ける必要はない。

【０２５１】しかし、パラメータｙ₀の値により改良ｇ
− cosδ法と改良ｇ− sinδ法とを使い分ける必要が生
じ、本変形例は改良ｇ− sinδ法と改良ｇ− cosδ法と
の併用型となる。即ち、パラメータｙ₀が０近傍値か１
近傍値か、又は−１近傍値となる場合には改良ｇ− sin
δ法を適用することとし（ステップＳ６Ｅ２）、パラメ
ータｙ₀が上記範囲外である限りは、改良ｇ− cosδ法
を適用して交差角δを算出するものである（ステップＳ
６Ｅ３）。

【０２５２】（Ｄ−３）その３特定領域内では改良ｇ− cosδ法を基本的に用い、
特定領域外ではｇ− sinδ法を基本的に用いることもで
きる。

【０２５３】逆に、特定領域内では改良ｇ− sinδ
法を基本的に用い、特定領域外ではｇ− cosδ法を基本
的に用いることもできる。

【０２５４】尚、既述制御方法に於いて用いられた改良
ｇ法とｇ法との組合せ及び改良ｇ法同士の組合せを、図
２２に示す。同図に於いて、Ｎｏ．１は図７〜図１２に
示した方法であり、Ｎｏ．２は変形例（Ｄ−１）であ
り、Ｎｏ．２〜Ｎｏ．４はそれぞれ変形例（Ｄ−２）
、を示している。又、Ｎｏ．５〜Ｎｏ．６はそれぞ
れ変形例（Ｄ−３）、を示している。

【０２５５】（Ｄ−４）その４ところで、上記実施例ではＸＹ平面へのトーチ４０
の射影がＸ軸に平行となるような状態を想定した。しか
し、Ｘ軸とＹ軸とを相互に入れ換えて定義したときに
は、上記射影がＹ軸に平行となるような仮想的状態を想
定してもよいことになる。

【０２５６】また、図２０及び図２１から理解され
るように、仮想的φ回転は演算プロセスの途中に現われ
るのみであって、最終的にはその仮想的φ回転を行なっ
ていない現実の状態についてのα系の値が、コントロー
ラ内のＣＰＵによって求めらる。したがって、上記各式
を組合せることによって、仮想的φ回転に対応して現わ
れてくる量（例えば仮想的な旋回角Θ′）を消去した式
を作れば、その式中には仮想的φ回転に関する量は現れ
ないこととなる。つまり、この場合には途中のプロセス
として仮想的φ回転を行わない演算式が得られるのであ
り、その様な演算式に基づいてα系の各値を求めること
が可能となる。即ち、このような変形もこの発明の範囲
に含まれる。しかも角度δはスカラー量であって座標軸
の回転の影響を受けない。従ってこの様な場合に於いて
も、Ｃδ若しくはＳδについての４次方程式の解、又は
Ｃδの逆数若しくはＳδの逆数についての４次方程式の
解より、角度δを決定することが可能となる。

【０２５７】上記実施例では余弦Ｃδと正弦Ｓδと
を求めた上で、交差角δの値を求めていた。しかし、余
弦Ｃδから、又は正弦Ｓδから直接的に交差角δの値を
求めても良い。

【０２５８】（Ｄ−５）その５この発明は、以上説明したレーザー溶接ロボットＲＢ等
に限らず、図２に示す座標系と等価な構成を有する円筒
座標ロボット全般について適用できるものである。即
ち、図２に示したＸＹＺ座標系を座標変換して得られる
新たなＸＹＺ座標系を有する円筒座標ロボットに関して
も、本制御方法を適用することが出来る。

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

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

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

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

【０２６３】以上の構成より、本ロボットＲＢ１は、図
１のロボットＲＢを図２のＸ軸のまわりに９０度回転し
た構成に対応していることが理解される。その様な座標
変換を図２５に示す。尚、図２５（ａ）は図２のＸＹＺ
座標系のＸ軸まわりの回転を示しており、同図（ｂ）は
回転後の座標系を本ロボットＲＢ１のデカルト座標系と
して適用した場合を示している。本図に示される通り、
本ロボットＲＢ１のＸ軸、Ｙ軸、Ｚ軸はそれぞれ、図２
のＺ軸、Ｘ軸、Ｙ軸に対応している。従って、本ロボッ
トＲＢ１の旋回角Θは図２の旋回角Θに対応しており、
本ロボットＲＢ１にｇ法又は改良ｇ法を適用した場合に
は、第２アーム２２’をＸ軸の周りに仮想的に旋回する
こととなる。この点は、図１のロボットＲＢと異なる点
である。

【０２６４】ここで、本ロボットＲＢ１に於ける逆変換
の方法の一例を図２７及び図２８に示す。このステップ
Ｓ６Ｆは、図７，図８のステップＳ６に対応している。
尚、図７，図８で示した方法（プログラム）をそのまま
使用して先ず図１、図２に適応した各駆動量を決定して
おき、その後、これらの駆動量を図２５の座標変換に従
って変換することにより、本ロボットＲＢ１に適応した
駆動量（ｘ_s，ｙ_s，Θ，α，β）を決定することもで
きる。この場合には、ロボットの軸構成が変わる毎にプ
ログラムを改めて構築する必要がなくなり、一つのプロ
グラムに簡単な座標変換用のプログラムを付加するだけ
で足りるという利点がある。

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

【０２６６】

【発明の効果】

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

【０２６７】請求項２記載の発明によれば、エンドエフ
ェクタの先端の姿勢に応じて旋回角Θを厳密に求めるこ
とができ、順次残りの各アームの駆動量をも厳密に求め
ることができる。従って、本発明もまた、エンドエフェ
クタ先端の位置決めを誤差なく正確に行うことができ
る。

【０２６８】（２）更に請求項１及び請求項２記載の
発明によれば、角度δに関して成立する４次方程式の解
の形式は定まっているので、上記４次方程式を解くステ
ップを系統的に処理することができる。従って、各アー
ムの駆動制御を短時間で行える効果もある。

【０２６９】（３）尚、次の様な実施例特有の効果が
ある。即ち、第１のアームをＺ軸のまわりに角度φだけ
旋回することによりエンドエフェクタのＸＹ平面への射
影がＸ軸と平行になる様な仮想的な状態が導入されてい
るので、角度δの正弦又は余弦の４次方程式及び角度δ
の正弦又は余弦の逆数の４次方程式を簡単な形にするこ
とができ、厳密な旋回角Θの値を容易に決定することが
できる効果がある。

【図面の簡単な説明】

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

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

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

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

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

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

【図７】Ｘ系からα系への逆変換のステップを詳細に説
明するフローチャートである。

【図８】Ｘ系からα系への逆変換のステップを詳細に説
明するフローチャートである。

【図９】逆変換に於けるｇ− sinδ法のステップを詳細
に説明するフローチャートである。

【図１０】逆変換に於ける改良ｇ法のステップを説明す
るフローチャートである。

【図１１】逆変換に於ける改良ｇ− cosδ法のステップ
を詳細に説明するフローチャートである。

【図１２】逆変換に於ける改良ｇ− sinδ法のステップ
を詳細に説明するフローチャートである。

【図１３】この発明の他の実施例に於ける逆変換のステ
ップを詳細に説明するフローチャートである。

【図１４】この発明の他の実施例に於ける逆変換のステ
ップを詳細に説明するフローチャートである。

【図１５】この発明の他の実施例に於ける改良ｇ− sin
δ法のステップを詳細に説明するフローチャートであ
る。

【図１６】この発明の更に他の実施例に於ける逆変換の
ステップを詳細に説明するフローチャートである。

【図１７】この発明の更に他の実施例に於ける逆変換の
ステップを詳細に説明するフローチャートである。

【図１８】位置関係を明確化するための立体図である。

【図１９】ＸＺ平面への射影を示す説明図である。

【図２０】ｇ法における逆変換アルゴリズムの流れを示
す説明図である。

【図２１】改良ｇ法における逆変換アルゴリズムの流れ
を示す説明図である。

【図２２】ｇ法と改良ｇ法との組合せ及び改良ｇ法同士
の組合せを示す説明図である。

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

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

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

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

【図２７】この発明の更に他の実施例であるレーザ溶接
ロボットの逆変換のステップを詳細に説明するフローチ
ャートである。

【図２８】この発明の更に他の実施例であるレーザ溶接
ロボットの逆変換のステップを詳細に説明するフローチ
ャートである。

【符号の説明】

ＲＢレーザ溶接ロボット１０ロボットの設置面２１第１のアーム２２第２のアーム２３第３のアーム３１第２のアームの先端部３２第３のアームの先端部４０トーチＡ１第１のアームの軸Ａ２第２のアームの軸Ａ３第３のアームの軸Ａ４トーチの軸

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

Claims

【特許請求の範囲】

【請求項１】ロボット設置面に略垂直に設置された第
１アームと、前記第１アームに結合して前記ロボット設
置面に略平行な平面内に並進自由度を有するとともに、
前記第１アームに対して定まる所定の平面内に直接的又
は間接的に旋回自由度を有する第２アームと、前記第２
アームの先端部に略直角に結合して前記第２アームの軸
方向まわりの回転自由度とそれ自身の軸まわりの回転自
由度とを有する第３アームと、前記第３アームの先端部
に略直角に結合したエンドエフェクタとを有する円筒座
標ロボットの駆動制御方法であって、（ａ）前記エンドエフェクタの先端の位置及び姿勢を
指定するステップと、（ｂ）前記エンドエフェクタの軸を含み且つ前記所定
の平面に垂直な平面が前記エンドエフェクタの軸方向を
法線方向とする平面と交差する際の交線と前記第３アー
ムの軸方向とのなす角度の余弦の逆数、又は前記角度の
正弦の逆数のうち、いずれかを選択するステップと、（ｃ）前記ステップ（ｂ）で選択された逆数を変数と
する４次方程式の解を算出するステップと、（ｄ）前記４次方程式の解より前記位置及び姿勢に対
応する前記角度の値を求めるステップと、（ｅ）前記角度の値を用いて前記第１、第２及び第３
アームの各駆動量を決定するステップと、（ｆ）前記駆動量に応じた駆動出力をそれぞれ前記第
１、第２及び第３アームの各駆動機構に与えることによ
り前記第１、第２及び第３アームの駆動を行うステップ
とを備えた円筒座標ロボットの駆動制御方法。
【請求項２】ロボット設置面に略垂直に設置された第
１アームと、前記第１アームに結合して前記ロボット設
置面に略平行な平面内に並進自由度を有するとともに、
前記第１アームに対して定まる所定の平面内に直接的又
は間接的に旋回自由度を有する第２アームと、前記第２
アームの先端部に略直角に結合して前記第２アームの軸
方向まわりの回転自由度とそれ自身の軸まわりの回転自
由度とを有する第３アームと、前記第３アームの先端部
に略直角に結合したエンドエフェクタとを有する円筒座
標ロボットの駆動制御方法であって、（ａ）前記エンドエフェクタの先端の位置及び姿勢を
複数指定するステップと、（ｂ）前記姿勢が予め定められた特定領域内に有るか
否かを判断するステップと、（ｃ）前記姿勢が前記特定領域内に有る場合には、前記エンドエフェクタの軸を含み且つ前記所定の平面に
垂直な平面が前記エンドエフェクタの軸方向を法線方向
とする平面と交差する際の交線と前記第３のアームの軸
方向とのなす角度の余弦の逆数、又は前記角度の正弦の
逆数のうち、いずれかを選択し、選択された逆数を変数とする４次方程式の解を算出する
とともに、当該４次方程式の解より前記位置及び姿勢に対応する前
記角度の値を求めるステップと、（ｄ）前記姿勢が前記特定領域外に有る場合には、前記角度の余弦、又は前記角度の正弦のうち、いずれか
を変数に選択し、前記変数に関する４次方程式の解を算出するとともに、当該４次方程式の解より前記位置及び姿勢に対応する前
記角度の値を求めるステップと、（ｅ）前記角度の値を用いて前記第１、第２及び第３
アームの各駆動量を決定するステップと、（ｆ）前記駆動量に応じた駆動出力をそれぞれ前記第
１、第２及び第３アームの各駆動機構に与えることによ
り前記第１、第２及び第３アームの駆動を行うステップ
とを備えた円筒座標ロボットの駆動制御方法。