JP3030565B2 - ロボットの駆動制御装置 - Google Patents
ロボットの駆動制御装置Info
- Publication number
- JP3030565B2 JP3030565B2 JP2331247A JP33124790A JP3030565B2 JP 3030565 B2 JP3030565 B2 JP 3030565B2 JP 2331247 A JP2331247 A JP 2331247A JP 33124790 A JP33124790 A JP 33124790A JP 3030565 B2 JP3030565 B2 JP 3030565B2
- Authority
- JP
- Japan
- Prior art keywords
- axis
- joint angle
- value
- joint
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、産業用ロボット、特に第5軸にオフセッ
トのある6軸多関節ロボットの駆動制御装置に関するも
のである。
トのある6軸多関節ロボットの駆動制御装置に関するも
のである。
産業用ロボットに所望の動作を行わせる方法として、
ロボットのエンドエフェクタの先端がたどるべき経路上
の点を順次ティーチングし、これらのティーチング点を
滑らかに結ぶ曲線又は直線で補間する方法が広く知られ
ている(ティーチングプレイバック)。従って、この方
法を用いてロボットの駆動を制御するには、ロボットの
エンドエフェクタの先端の位置と姿勢を表現するXYZ絶
対座標系(X系)からロボットの関節角を表現する関節
座標系(θ系)への座標変換、即ち逆変換が必要とな
る。
ロボットのエンドエフェクタの先端がたどるべき経路上
の点を順次ティーチングし、これらのティーチング点を
滑らかに結ぶ曲線又は直線で補間する方法が広く知られ
ている(ティーチングプレイバック)。従って、この方
法を用いてロボットの駆動を制御するには、ロボットの
エンドエフェクタの先端の位置と姿勢を表現するXYZ絶
対座標系(X系)からロボットの関節角を表現する関節
座標系(θ系)への座標変換、即ち逆変換が必要とな
る。
一般にロボットの座標変換には、θ系からX系への順
変換と、前述の逆変換がある。前者は、ロボットの軸構
成がどのようなものであっても解析的に厳密解を求める
ことができることが知られている。同様に、後者に関し
ても解析的に厳密解を求める方法が試みられており、例
えばR.C.Paul,“Robot Manipulators;Mathematics,Prog
ramming and Control"MIT Press(1981)にその具体例
が示されている。即ち、順変換の変換式から代数学的に
逆変換の変換式を求めるわけである。
変換と、前述の逆変換がある。前者は、ロボットの軸構
成がどのようなものであっても解析的に厳密解を求める
ことができることが知られている。同様に、後者に関し
ても解析的に厳密解を求める方法が試みられており、例
えばR.C.Paul,“Robot Manipulators;Mathematics,Prog
ramming and Control"MIT Press(1981)にその具体例
が示されている。即ち、順変換の変換式から代数学的に
逆変換の変換式を求めるわけである。
しかし、ロボットの軸構成いかんによっては厳密な解
析解の求まる場合と求まらない場合があり、特にオフセ
ットのある多関節型ロボットにおいては、どの軸にオフ
セットがあるかによっても解析解が求まる場合と求まら
ない場合とが生じる。
析解の求まる場合と求まらない場合があり、特にオフセ
ットのある多関節型ロボットにおいては、どの軸にオフ
セットがあるかによっても解析解が求まる場合と求まら
ない場合とが生じる。
第5軸にオフセットのある6軸関節ロボットは逆変換
の厳密解を求めることができないケースとして知られて
おり、その様な6軸関節ロボットのアームの各駆動量を
制御する研究が従来より活発に行われてきた。例えば、
特開昭62−193786号公報や特開昭64−8407号公報等に示
された従来技術が知られている。
の厳密解を求めることができないケースとして知られて
おり、その様な6軸関節ロボットのアームの各駆動量を
制御する研究が従来より活発に行われてきた。例えば、
特開昭62−193786号公報や特開昭64−8407号公報等に示
された従来技術が知られている。
前者は、まずオフセットのある手首部は動作しないも
の(第4〜第6軸の関節角θ4〜θ6が一定値)として
ロボットのエンドエフェクタの先端の位置データより、
第1〜第3軸の関節角θ1〜θ3の値を近似的に求め
る。
の(第4〜第6軸の関節角θ4〜θ6が一定値)として
ロボットのエンドエフェクタの先端の位置データより、
第1〜第3軸の関節角θ1〜θ3の値を近似的に求め
る。
次に、これらの近似解(θ1〜θ3)を用いてエンド
エフェクタの先端の姿勢データより(この際位置データ
は0として扱う)、第4〜第6軸の関節角θ4〜θ6の
値を近似的に求めた後、位置データを補正する。更に、
補正後の位置データを用いて最初に関節角θ4〜θ6を
求めた近似的方法により、再度関節角θ4〜θ6を求め
る。そして、以上の演算を所望の精度に応じて繰り返し
行なうこととしたものである。
エフェクタの先端の姿勢データより(この際位置データ
は0として扱う)、第4〜第6軸の関節角θ4〜θ6の
値を近似的に求めた後、位置データを補正する。更に、
補正後の位置データを用いて最初に関節角θ4〜θ6を
求めた近似的方法により、再度関節角θ4〜θ6を求め
る。そして、以上の演算を所望の精度に応じて繰り返し
行なうこととしたものである。
一方、後者はロボットの6個の関節角の中から少なく
とも一つの関節角について、次式に示す様にヤコビ行列
(ヤコビアン)を用いてその近似解を求める。即ち、ロ
ボットのエンドエフェクタ先端の現在の位置と姿勢及び
次の移動すべき位置とその位置での姿勢を表わすベクト
ルをそれぞれ0及び1とし、ベクトル0及び1
に対応するロボットの関節角を示すベクトルをそれぞれ
0及び1、ヤコビアンをJ()として表わすとす
れば、上述の近似解1は、1 ≒0+J-1()・(1−0) …(1) として求められる。そして、この様にして求めた関節角
の一部の近似解1を用いて、残りの関節角の解を求め
るものである。この場合、逆変換の変換式から近似解と
して求めた関節角を除外することによりその分の数だけ
逆変換の変換式の未知数を減少させることができ、多く
の場合残りの関節角について解析的に解を求めることが
できるようになる。
とも一つの関節角について、次式に示す様にヤコビ行列
(ヤコビアン)を用いてその近似解を求める。即ち、ロ
ボットのエンドエフェクタ先端の現在の位置と姿勢及び
次の移動すべき位置とその位置での姿勢を表わすベクト
ルをそれぞれ0及び1とし、ベクトル0及び1
に対応するロボットの関節角を示すベクトルをそれぞれ
0及び1、ヤコビアンをJ()として表わすとす
れば、上述の近似解1は、1 ≒0+J-1()・(1−0) …(1) として求められる。そして、この様にして求めた関節角
の一部の近似解1を用いて、残りの関節角の解を求め
るものである。この場合、逆変換の変換式から近似解と
して求めた関節角を除外することによりその分の数だけ
逆変換の変換式の未知数を減少させることができ、多く
の場合残りの関節角について解析的に解を求めることが
できるようになる。
従来の技術は以上の様に構成されているので、次の様
な問題点があった。
な問題点があった。
特開昭62−193786号公報で開示される従来技術は、全
ての関節角θ1〜θ6について収束演算を行わなければ
ならず且つ位置データの補正が必要とされるので、計算
が複雑でありアームの駆動量決定に時間がかかるという
問題点があった。
ての関節角θ1〜θ6について収束演算を行わなければ
ならず且つ位置データの補正が必要とされるので、計算
が複雑でありアームの駆動量決定に時間がかかるという
問題点があった。
一方、特開昭64−8407号公報で開示される従来技術は
収束演算を用いないという点では演算速度を早める利点
を有するが、関節角θ1〜θ6の連立方程式の少なくと
も1つの関節角を近似計算し連立方程式の未知数を減少
させる方法であるため、関節角θ3がゼロに近い領域あ
るいは関節角θ5がゼロに近い領域等のいわゆる臨界領
域においては十分な精度で計算できないという問題点が
あった。
収束演算を用いないという点では演算速度を早める利点
を有するが、関節角θ1〜θ6の連立方程式の少なくと
も1つの関節角を近似計算し連立方程式の未知数を減少
させる方法であるため、関節角θ3がゼロに近い領域あ
るいは関節角θ5がゼロに近い領域等のいわゆる臨界領
域においては十分な精度で計算できないという問題点が
あった。
この発明は以上の様な問題点に鑑みなされたもので、
第5軸にオフセットのある6軸多関節ロボットに関し、
アームの駆動量決定の手順が簡単であって、且ついわゆ
る臨界領域においてもアームの駆動量の精度を一定のレ
ベル以上に保持できる駆動制御装置を提供することを目
的としている。
第5軸にオフセットのある6軸多関節ロボットに関し、
アームの駆動量決定の手順が簡単であって、且ついわゆ
る臨界領域においてもアームの駆動量の精度を一定のレ
ベル以上に保持できる駆動制御装置を提供することを目
的としている。
(1) 請求項1に係る発明は、アーム先端にエンドエ
フェクタが取付けられてなり、第5軸にオフセットのあ
る第6軸関節ロボットの駆動制御装置であって、(a)
前記ロボットの設置面に平行な面をXY平面とするXYZ絶
対座標系において、所定の移動経路上の各ティーチング
点に前記エンドエフェクタの先端を順次に移動してその
位置及び姿勢決めを行うことにより得られる各ティーチ
ング点における各軸の関節角の値のデータを電気的に順
変換することにより各ティーチング点のティーチングデ
ータ信号を求めると共に、互いに隣り合うティーチング
点間を両ティーチング点に関するティーチングデータ信
号より補間して、補間点に於ける前記エンドエフェクタ
の先端の位置及び姿勢に関する補間点データ信号を電気
的に求める手段と、(b)前記移動経路上の前記補間点
における前記エンドエフェクタの先端の位置及び姿勢の
補間点データ信号から、前記補間点における前記ロボッ
トの各軸の関節角を与える信号を、順変換方程式を変形
することにより与えられる、第1軸ないし第4軸の各関
節角を変数とする第1逆変換方程式を電気的に解くこと
によって決定する手段とを備え、前記手段(b)は、
(b−1)第4軸の関節角の値に初期値を電気的に与え
る手段と、(b−2)前記手段(b−1)により定めら
れた前記第4軸の関節角の値の信号を用いて前記第1逆
変換方程式より第1軸の関節角から第3軸の関節角まで
の値の信号を順次に電気的に求める手段と、(b−3)
前記手段(b−2)により求められた前記第1軸の関節
角から前記第3軸の関節角までの値の信号を用いて、前
記順変換方程式を変形することにより得られる、前記第
1軸ないし第3軸の各関節角を変数とする前記第4軸の
関節角を与える第2逆変換方程式より改めて第4軸の関
節角の値の信号を電気的に求める手段と、(b−4)前
記手段(b−3)により求められた第4軸の関節角の値
の信号と前記手段(b−2)に与えられた前記初期値の
信号との差分データ信号を求め、当該差分データ信号が
許容範囲内か否かを判定する手段と、(b−5)前記手
段(b−4)が許容範囲内にないと判定したときには、
前記手段(b−3)が求めた前記第4軸の関節角の値の
信号を電気的に補正して、新たな初期値を電気的に決定
する手段と、(b−6)前記手段(b−5)によって求
められた補正後の初期値の信号を前記第4軸の関節角の
値の信号として前記手段(b−2)に入力して前記手段
(b−2)を再実行させる手段と、(b−7)前記手段
(b−4)が、前記差分データ信号が前記許容範囲内に
なったと判定したときには、そのときの第1軸の関節角
の値から第4軸の関節角の値までの信号を前記第1逆変
換方程式の解として電気的に決定する手段と、(b−
8)前記手段(b−7)により決定された前記第1軸の
関節角から前記第4軸の関節角までの値の信号を用い
て、前記順変換方程式を変形して得られる第3及び第4
逆変換方程式よりそれぞれ第5軸及び第6軸の関節角の
値の信号を電気的に求める手段とを有し、前記駆動制御
装置は、(c)前記手段(b)で求めた各軸の関節角の
値の信号に応じた駆動出力を前記アームの対応する駆動
機構に与えて、前記6軸関節ロボットの駆動制御を行う
手段を更に備えることを特徴とする。
フェクタが取付けられてなり、第5軸にオフセットのあ
る第6軸関節ロボットの駆動制御装置であって、(a)
前記ロボットの設置面に平行な面をXY平面とするXYZ絶
対座標系において、所定の移動経路上の各ティーチング
点に前記エンドエフェクタの先端を順次に移動してその
位置及び姿勢決めを行うことにより得られる各ティーチ
ング点における各軸の関節角の値のデータを電気的に順
変換することにより各ティーチング点のティーチングデ
ータ信号を求めると共に、互いに隣り合うティーチング
点間を両ティーチング点に関するティーチングデータ信
号より補間して、補間点に於ける前記エンドエフェクタ
の先端の位置及び姿勢に関する補間点データ信号を電気
的に求める手段と、(b)前記移動経路上の前記補間点
における前記エンドエフェクタの先端の位置及び姿勢の
補間点データ信号から、前記補間点における前記ロボッ
トの各軸の関節角を与える信号を、順変換方程式を変形
することにより与えられる、第1軸ないし第4軸の各関
節角を変数とする第1逆変換方程式を電気的に解くこと
によって決定する手段とを備え、前記手段(b)は、
(b−1)第4軸の関節角の値に初期値を電気的に与え
る手段と、(b−2)前記手段(b−1)により定めら
れた前記第4軸の関節角の値の信号を用いて前記第1逆
変換方程式より第1軸の関節角から第3軸の関節角まで
の値の信号を順次に電気的に求める手段と、(b−3)
前記手段(b−2)により求められた前記第1軸の関節
角から前記第3軸の関節角までの値の信号を用いて、前
記順変換方程式を変形することにより得られる、前記第
1軸ないし第3軸の各関節角を変数とする前記第4軸の
関節角を与える第2逆変換方程式より改めて第4軸の関
節角の値の信号を電気的に求める手段と、(b−4)前
記手段(b−3)により求められた第4軸の関節角の値
の信号と前記手段(b−2)に与えられた前記初期値の
信号との差分データ信号を求め、当該差分データ信号が
許容範囲内か否かを判定する手段と、(b−5)前記手
段(b−4)が許容範囲内にないと判定したときには、
前記手段(b−3)が求めた前記第4軸の関節角の値の
信号を電気的に補正して、新たな初期値を電気的に決定
する手段と、(b−6)前記手段(b−5)によって求
められた補正後の初期値の信号を前記第4軸の関節角の
値の信号として前記手段(b−2)に入力して前記手段
(b−2)を再実行させる手段と、(b−7)前記手段
(b−4)が、前記差分データ信号が前記許容範囲内に
なったと判定したときには、そのときの第1軸の関節角
の値から第4軸の関節角の値までの信号を前記第1逆変
換方程式の解として電気的に決定する手段と、(b−
8)前記手段(b−7)により決定された前記第1軸の
関節角から前記第4軸の関節角までの値の信号を用い
て、前記順変換方程式を変形して得られる第3及び第4
逆変換方程式よりそれぞれ第5軸及び第6軸の関節角の
値の信号を電気的に求める手段とを有し、前記駆動制御
装置は、(c)前記手段(b)で求めた各軸の関節角の
値の信号に応じた駆動出力を前記アームの対応する駆動
機構に与えて、前記6軸関節ロボットの駆動制御を行う
手段を更に備えることを特徴とする。
(2) 請求項2に係る発明は、請求項1記載の6軸関
節ロボットの駆動制御装置であって、前記移動経路上の
任意のk番目の点をPkとして表すと、(k+1)番目の
補間点Pk+1における前記第4軸の関節角の初期値とし
て、(i)前記k番目の点Pk及び前記点Pk+1における前
記エンドエフェクタの先端のそれぞれの位置及び姿勢
と、(ii)前記点Pkにおける前記エンドエフェクタの先
端の位置及び姿勢のヤコビアンの値と、(iii)前記点P
kにおいて既に決定されている前記第4軸の関節角の値
とを用いて近似計算により電気的に推定される前記補間
点Pk+1における第4軸の関節角の値を用いることを特徴
とする。
節ロボットの駆動制御装置であって、前記移動経路上の
任意のk番目の点をPkとして表すと、(k+1)番目の
補間点Pk+1における前記第4軸の関節角の初期値とし
て、(i)前記k番目の点Pk及び前記点Pk+1における前
記エンドエフェクタの先端のそれぞれの位置及び姿勢
と、(ii)前記点Pkにおける前記エンドエフェクタの先
端の位置及び姿勢のヤコビアンの値と、(iii)前記点P
kにおいて既に決定されている前記第4軸の関節角の値
とを用いて近似計算により電気的に推定される前記補間
点Pk+1における第4軸の関節角の値を用いることを特徴
とする。
(3) 請求項3に係る発明は、請求項1記載の6軸関
節ロボットの駆動制御装置であって、前記移動経路上の
k番目の点である補間点Pkにおける前記第4軸の関節角
の初期値として、(k−)番目の点Pk−〔1≦
≦(k−1)〕から(k−1)番目の点Pk-1それぞれに
おいて既に決定されている第4軸の関節角の値の線形結
合によって与えられる値を用いることを特徴とする。
節ロボットの駆動制御装置であって、前記移動経路上の
k番目の点である補間点Pkにおける前記第4軸の関節角
の初期値として、(k−)番目の点Pk−〔1≦
≦(k−1)〕から(k−1)番目の点Pk-1それぞれに
おいて既に決定されている第4軸の関節角の値の線形結
合によって与えられる値を用いることを特徴とする。
この発明の第1の構成では、第4軸の関節角の値に初
期値を与え、順変換の方程式を変換して得られる第1軸
の関節角から第4軸の関節角を変数とする第1逆変換方
程式より、第1軸の関節角から第3軸の関節角の値を求
める。そして、得られた第1軸の関節角から第3軸の関
節角の値を順変換の方程式を変換して新たに得られる第
2逆変換方程式へ代入して改めて第4軸の関節角の値を
求めた後、得られた第4軸の関節角の値と初期値との差
が許容範囲内であるかを比較する。許容範囲外である場
合には、前記得られた第4軸の関節角の値を新たな初期
値として以上の演算処理が前記差が許容範囲内に収束す
るまで繰り返されるので、精度の高い第1軸の関節角か
ら第4軸の関節角の値が求められる。その後、第5軸及
び第6軸の関節角を求めるとともに、以上求められた各
関節角の値に応じた駆動出力がアームの各軸の駆動機構
に与えられ、エンドエフェクタの先端が所定の位置へ正
確に移動し、所定の姿勢が正確に保たれることになる。
期値を与え、順変換の方程式を変換して得られる第1軸
の関節角から第4軸の関節角を変数とする第1逆変換方
程式より、第1軸の関節角から第3軸の関節角の値を求
める。そして、得られた第1軸の関節角から第3軸の関
節角の値を順変換の方程式を変換して新たに得られる第
2逆変換方程式へ代入して改めて第4軸の関節角の値を
求めた後、得られた第4軸の関節角の値と初期値との差
が許容範囲内であるかを比較する。許容範囲外である場
合には、前記得られた第4軸の関節角の値を新たな初期
値として以上の演算処理が前記差が許容範囲内に収束す
るまで繰り返されるので、精度の高い第1軸の関節角か
ら第4軸の関節角の値が求められる。その後、第5軸及
び第6軸の関節角を求めるとともに、以上求められた各
関節角の値に応じた駆動出力がアームの各軸の駆動機構
に与えられ、エンドエフェクタの先端が所定の位置へ正
確に移動し、所定の姿勢が正確に保たれることになる。
又、この発明の第2の構成では、第4軸の関節角の初
期値が、移動経路上の補間点Pk+1と点Pk+1の一つ前の点
Pkにおけるエンドエフェクタの先端の位置及び姿勢のデ
ータ、点Pkにおいてすでに求められた第4軸の関節角の
値及び点Pkにおけるエンドエフェクタの先端の位置・姿
勢より求まるヤコビアンの値より推定されるので、第4
軸の関節角の初期値として適切な値を与える。
期値が、移動経路上の補間点Pk+1と点Pk+1の一つ前の点
Pkにおけるエンドエフェクタの先端の位置及び姿勢のデ
ータ、点Pkにおいてすでに求められた第4軸の関節角の
値及び点Pkにおけるエンドエフェクタの先端の位置・姿
勢より求まるヤコビアンの値より推定されるので、第4
軸の関節角の初期値として適切な値を与える。
更に、この発明の第3の構成においても、第4軸の関
節角の初期値が、エンドエフェクタの先端が移動すべき
補間点Pkよりも以前にエンドエフェクタの先端が移動し
た点Pl〔1≦l≦(k−1)〕において求められた第4
軸の関節角の値の線形結合により与えられるので、第4
軸の関節角の初期値として適切な値を与える。
節角の初期値が、エンドエフェクタの先端が移動すべき
補間点Pkよりも以前にエンドエフェクタの先端が移動し
た点Pl〔1≦l≦(k−1)〕において求められた第4
軸の関節角の値の線形結合により与えられるので、第4
軸の関節角の初期値として適切な値を与える。
A.装置の機械的構成 第1図は、この発明の一実施例である第5軸にオフセ
ットを有する6軸多関節ロボットRBの機械的構成を模式
的に示す側面図であり、第1軸から第6軸それぞれの軸
における関節変位(回転方向)もまた、第1図に模式的
に示されている。
ットを有する6軸多関節ロボットRBの機械的構成を模式
的に示す側面図であり、第1軸から第6軸それぞれの軸
における関節変位(回転方向)もまた、第1図に模式的
に示されている。
図において、ロボットRBの設置面10上に固定された絶
対座標系としてX系を定義するとともに、ロボットRBの
基台11よりZ軸に沿って延びた支柱20の先端部に当る第
1の関節部21の略中心(設置面10より高さd1の位置)に
固定された座標系(x1,y1,z1)を、第1軸の座標系とし
て定義する。即ち、z1軸はZ軸に平行な座標軸、x1,y1
軸はXY平面に平行な平面内であって、且つz1軸に直交す
る座標軸である。そして第1の関節部21は、第1の関節
部21の内部に備えつけられた第1の駆動機構によって、
z1軸の周りを第1の関節角θ1方向に回転することがで
きる。
対座標系としてX系を定義するとともに、ロボットRBの
基台11よりZ軸に沿って延びた支柱20の先端部に当る第
1の関節部21の略中心(設置面10より高さd1の位置)に
固定された座標系(x1,y1,z1)を、第1軸の座標系とし
て定義する。即ち、z1軸はZ軸に平行な座標軸、x1,y1
軸はXY平面に平行な平面内であって、且つz1軸に直交す
る座標軸である。そして第1の関節部21は、第1の関節
部21の内部に備えつけられた第1の駆動機構によって、
z1軸の周りを第1の関節角θ1方向に回転することがで
きる。
又、第1のアーム30が図に示す様に支柱20の第1の関
節部21に取付けられており、その接合部に当る第2の関
節部31の中心の固定された座標系(x2,y2,z2)を、第2
軸の座標系として定義する。即ち、第1のアーム30の軸
に平行にz2軸を、y1軸に平行にy2軸を、y2軸,z2軸に直
交する方向にx2軸をとるものとする。そして第1のアー
ム30は、第2の関節部31の内部に備えつけられた第2の
駆動機構によって、y2軸の周りを第2の関節角θ2方向
に回転することができる。
節部21に取付けられており、その接合部に当る第2の関
節部31の中心の固定された座標系(x2,y2,z2)を、第2
軸の座標系として定義する。即ち、第1のアーム30の軸
に平行にz2軸を、y1軸に平行にy2軸を、y2軸,z2軸に直
交する方向にx2軸をとるものとする。そして第1のアー
ム30は、第2の関節部31の内部に備えつけられた第2の
駆動機構によって、y2軸の周りを第2の関節角θ2方向
に回転することができる。
更に、第1のアーム30の先端部に当る第3の関節部32
(第2の関節部31の中心と第3の関節部32の中心との距
離をd2とする。)にはオフセットa2を介して第2のアー
ム40が取り付けられており、第3の関節部32の中心に固
定された座標系(x3,y3,z3)を第3軸の座標系として定
義する。即ち、y2軸に平行にy3軸を、第2のアーム40の
軸に平行にz3軸を、y3軸,z3軸に直交する方向にx3軸を
とるものとする。そして第2のアーム40は、第3の関節
部32の内部に備えつけられた第3の駆動機構によって、
y3軸の周りを第3の関節角θ3方向に回転することがで
きる。
(第2の関節部31の中心と第3の関節部32の中心との距
離をd2とする。)にはオフセットa2を介して第2のアー
ム40が取り付けられており、第3の関節部32の中心に固
定された座標系(x3,y3,z3)を第3軸の座標系として定
義する。即ち、y2軸に平行にy3軸を、第2のアーム40の
軸に平行にz3軸を、y3軸,z3軸に直交する方向にx3軸を
とるものとする。そして第2のアーム40は、第3の関節
部32の内部に備えつけられた第3の駆動機構によって、
y3軸の周りを第3の関節角θ3方向に回転することがで
きる。
尚、一般的には第2軸及び第3軸にオフセットがある
ケースが多く、第1図の構成においても実際には第3軸
のみならず第2軸にもオフセットが生じているが、その
ような第2軸及び第3軸にオフセットがある場合の逆変
換は容易に厳密解を解析的に求めることができる場合で
あるので、第2軸及び第3軸にオフセットがあるか否か
はこの発明においては本質的事項ではない。従って、第
1図においては、便宜上第2軸にオフセットがないもの
として取り扱っている。
ケースが多く、第1図の構成においても実際には第3軸
のみならず第2軸にもオフセットが生じているが、その
ような第2軸及び第3軸にオフセットがある場合の逆変
換は容易に厳密解を解析的に求めることができる場合で
あるので、第2軸及び第3軸にオフセットがあるか否か
はこの発明においては本質的事項ではない。従って、第
1図においては、便宜上第2軸にオフセットがないもの
として取り扱っている。
又、第2のアーム40の先端部に当る第4の関節部41
(第3の関節部32の中心と第4の関節部41の中心との距
離をd3とする。)にはオフセット部を介して第3のアー
ム50が取り付けられているが(尚、第4の関節部41は第
5の関節部を兼ね備えている)、この第2のアーム40と
第3のアーム50との接続関係については、第2図をも参
照しながら説明する。ここで第2図は、第2のアーム40
と第3のアーム50の接続部分の平面図を示している。
(第3の関節部32の中心と第4の関節部41の中心との距
離をd3とする。)にはオフセット部を介して第3のアー
ム50が取り付けられているが(尚、第4の関節部41は第
5の関節部を兼ね備えている)、この第2のアーム40と
第3のアーム50との接続関係については、第2図をも参
照しながら説明する。ここで第2図は、第2のアーム40
と第3のアーム50の接続部分の平面図を示している。
第1図及び第2図において、第4の関節部41にはオフ
セットa3のオフセット部42が取り付けられており、オフ
セット部42の先端部に当る第6の関節部43には第3のア
ーム50が取り付けられている。更に、第3のアーム50の
先端にはL字状のエンドエフェクタ60が取り付けられて
いる。
セットa3のオフセット部42が取り付けられており、オフ
セット部42の先端部に当る第6の関節部43には第3のア
ーム50が取り付けられている。更に、第3のアーム50の
先端にはL字状のエンドエフェクタ60が取り付けられて
いる。
そして、第4の関節部41の中心に固定された2つの座
標系(x4,y4,z4)及び(x5,y5,z5)を、それぞれ第4軸
の座標系,第5軸の座標系として定義する。即ち、第4
軸についてはオフセット部42の軸に平行にy4軸を、第2
のアーム40の軸に平行にz4軸を、y4軸及びz4軸に直交す
る方向にx4軸をとるとともに、第5軸に関してはオフセ
ット部42の軸に平行にy5軸を、第3のアーム50の軸に平
行にz5軸を、y5軸及びz5軸に直交する方向にx5軸をと
る。
標系(x4,y4,z4)及び(x5,y5,z5)を、それぞれ第4軸
の座標系,第5軸の座標系として定義する。即ち、第4
軸についてはオフセット部42の軸に平行にy4軸を、第2
のアーム40の軸に平行にz4軸を、y4軸及びz4軸に直交す
る方向にx4軸をとるとともに、第5軸に関してはオフセ
ット部42の軸に平行にy5軸を、第3のアーム50の軸に平
行にz5軸を、y5軸及びz5軸に直交する方向にx5軸をと
る。
又、第3のアーム50はオフセット部42とともに、第4
の関節部41の内部に備えつけられた第4の駆動機構によ
って、z4軸の周りを第4の関節角θ4方向に回転するこ
とができる。
の関節部41の内部に備えつけられた第4の駆動機構によ
って、z4軸の周りを第4の関節角θ4方向に回転するこ
とができる。
更に、第3のアーム50は第4の関節部41の内部に備え
つけられた第5の駆動機構によって、y5軸の周りを第5
の関節角θ5方向に回転することもできる。
つけられた第5の駆動機構によって、y5軸の周りを第5
の関節角θ5方向に回転することもできる。
そして、第6の関節部43の中心に固定された座標系
(x6,y6,z6)を、第6軸の座標系として定義する。即
ち、オフセット部42の軸に平行にy6軸を、第3のアーム
50の軸に平行にz6軸を、y6軸及びz6軸に直交する方向に
x6軸をとる。
(x6,y6,z6)を、第6軸の座標系として定義する。即
ち、オフセット部42の軸に平行にy6軸を、第3のアーム
50の軸に平行にz6軸を、y6軸及びz6軸に直交する方向に
x6軸をとる。
又、エンドエフェクタ60は第6の関節部43の内部に備
えつけられた第6の駆動機構によって、z6軸の周りを第
6の関節角θ6方向に回転することができる。
えつけられた第6の駆動機構によって、z6軸の周りを第
6の関節角θ6方向に回転することができる。
以上説明したロボットRBの各軸の関節角の可変範囲
を、表1に例示する。
を、表1に例示する。
B.装置の電気的構成 第3図は、第1図に示したロボットRBの制御系の電気
的構成を示すブロック図である。
的構成を示すブロック図である。
この制御系はCPU71やメモリ72等からなるコントロー
ラ70を有しており、ソフト的に制御される。又、ティー
チングボックス80を操作することによって、ティーチン
グデータの取込みが行われ、ティーチングデータがメモ
リ72に書き込まれる。
ラ70を有しており、ソフト的に制御される。又、ティー
チングボックス80を操作することによって、ティーチン
グデータの取込みが行われ、ティーチングデータがメモ
リ72に書き込まれる。
一方、第1の駆動機構91を構成するモータM1に駆動信
号D1が送信され、モータM1に取付けられたエンコーダE1
が、第1軸の関節角θ1の値を検出する。同様に、第2
〜第6の駆動機構92〜96にそれぞれ駆動信号D2〜D5が送
信され、各モータM2〜M6が動作し、第2軸の関節角θ2
から第6軸の関節角θ6までがそれぞれエンコーダE2〜
E6によって検出される。
号D1が送信され、モータM1に取付けられたエンコーダE1
が、第1軸の関節角θ1の値を検出する。同様に、第2
〜第6の駆動機構92〜96にそれぞれ駆動信号D2〜D5が送
信され、各モータM2〜M6が動作し、第2軸の関節角θ2
から第6軸の関節角θ6までがそれぞれエンコーダE2〜
E6によって検出される。
C.駆動制御方法 第4図は、ロボットRBを駆動制御するためのステップ
を示すフローチャートである。
を示すフローチャートである。
ステップS1はティーチング工程であり、ティーチング
データiの作成と記憶を行う。即ち、オペレータはマ
ニュアルでエンドエフェクタ60の先端の位置決めを行
い、エンコーダE1〜E6により各軸の関節角の値(θ1〜
θ6)を検出し、メモリ72に格納する。そしてCPU71に
おいて、検出された各軸の関節角の値を順変換すること
により各ティーチング点のティーチングデータiが計
算され、i=1,2,…についてのティーチングデータi
がメモリ72に格納される。
データiの作成と記憶を行う。即ち、オペレータはマ
ニュアルでエンドエフェクタ60の先端の位置決めを行
い、エンコーダE1〜E6により各軸の関節角の値(θ1〜
θ6)を検出し、メモリ72に格納する。そしてCPU71に
おいて、検出された各軸の関節角の値を順変換すること
により各ティーチング点のティーチングデータiが計
算され、i=1,2,…についてのティーチングデータi
がメモリ72に格納される。
次のステップ群10は再生動作に対応し、そのうちのス
テップS2ではひとつのティーチング点についてのティー
チングデータiをメモリ72より読出す。
テップS2ではひとつのティーチング点についてのティー
チングデータiをメモリ72より読出す。
そして、ステップS3ではCPU71においてそのティーチ
ング点とそれに隣接するティーチング点との間を補間
し、補間点のデータ▲▼を計算する。補間点のデ
ータ▲▼はX系による値であるため、エンドエフ
ェクタ60を補間点へ移動するための適切な各軸の関節角
の値を決定する必要がある。
ング点とそれに隣接するティーチング点との間を補間
し、補間点のデータ▲▼を計算する。補間点のデ
ータ▲▼はX系による値であるため、エンドエフ
ェクタ60を補間点へ移動するための適切な各軸の関節角
の値を決定する必要がある。
そこで、ステップS4ではX系からθ系への逆変換が行
われる。ここで、第5図はステップS4を詳細に説明する
フローチャートであり、以下第5図に基づいてステップ
S4を説明することとする。尚、以下の解析による逆変換
アルゴリズムの流れを第6図に示す。この第6図におい
て、かっこ付きの数字は以下の説明中における数式の番
号を示しており、(*)印はティーチングデータから直
ちに得られるX系の量を、(**)印は逆変換によって
求めるべきθ系の量を示している。また、ティーチング
点▲▼および補間点▲▼のすべてを順次に配
列した連鎖を点Pk(k=1,2,…)で表現する。以下で
は、特に、任意のkの値に対し、k番目の補間点Pkにつ
いての逆変換を考える。
われる。ここで、第5図はステップS4を詳細に説明する
フローチャートであり、以下第5図に基づいてステップ
S4を説明することとする。尚、以下の解析による逆変換
アルゴリズムの流れを第6図に示す。この第6図におい
て、かっこ付きの数字は以下の説明中における数式の番
号を示しており、(*)印はティーチングデータから直
ちに得られるX系の量を、(**)印は逆変換によって
求めるべきθ系の量を示している。また、ティーチング
点▲▼および補間点▲▼のすべてを順次に配
列した連鎖を点Pk(k=1,2,…)で表現する。以下で
は、特に、任意のkの値に対し、k番目の補間点Pkにつ
いての逆変換を考える。
まずステップS41では、各補間点Pkごとに第4軸の関
節角θ4の初期値θ40を設定する。そして設定された初
期値θ40は、メモリ72に格納される。この初期値θ40の
設定という手順は、後述する様に逆変換式の内、エンド
エフェクタ60の先端の位置ベクトルに関する3元連立方
程式が第1軸の関節角θ1から第4軸の関節角θ4によ
り表わされることから、第4軸の関節角θ4を既知の量
として扱えば、第1軸の関節角θ1から第3軸の関節角
θ3の解を求めることができる点に着眼したことによる
ものである。
節角θ4の初期値θ40を設定する。そして設定された初
期値θ40は、メモリ72に格納される。この初期値θ40の
設定という手順は、後述する様に逆変換式の内、エンド
エフェクタ60の先端の位置ベクトルに関する3元連立方
程式が第1軸の関節角θ1から第4軸の関節角θ4によ
り表わされることから、第4軸の関節角θ4を既知の量
として扱えば、第1軸の関節角θ1から第3軸の関節角
θ3の解を求めることができる点に着眼したことによる
ものである。
ここで初期値θ40として、以下のものを用いることも
できる。
できる。
その一つは、現在逆変換を行うとしている補間点Pkに
対して、既に逆変換を行った(k−)番目の点P
k−から(k−1)番目の点Pk-1までのl個(1≦
≦(k−1))の点(これらの点は補間点又はティーチ
ング点(ティーチング点では逆変換は不要)である)の
それぞれにおける第4軸の関節角の値(θ4)k−〜
(θ4)k-1の線形結合によって与えられる量を、初期
値θ40として用いるものである。即ち、初期値θ40は、
係数cmを用いて(2)式により表わされる。
対して、既に逆変換を行った(k−)番目の点P
k−から(k−1)番目の点Pk-1までのl個(1≦
≦(k−1))の点(これらの点は補間点又はティーチ
ング点(ティーチング点では逆変換は不要)である)の
それぞれにおける第4軸の関節角の値(θ4)k−〜
(θ4)k-1の線形結合によって与えられる量を、初期
値θ40として用いるものである。即ち、初期値θ40は、
係数cmを用いて(2)式により表わされる。
従って、上記(2)式の例として例えば、以下の
(3)式〜(5)式により表わされるものを初期値θ40
として用いてもよい。
(3)式〜(5)式により表わされるものを初期値θ40
として用いてもよい。
θ40=(θ4)k-1 …(3) θ40=2(θ4)k-1−(θ4)k-2 …(4) θ40=3(θ4)k-1−3(θ4)k-2+(θ4)k-3 …(5) (3)式〜(5)式により与えられる初期値θ40は、
補間点Pkに対して前回計算した点Pk-1,前々回計算した
点Pk-2等において既に求めた第4軸の関節角θ4の値を
用いるものであるから、補間点Pkにおける第4軸の関節
角の真値(θ4)kに対して比較的良い近似値となりえ
るものである。特に、再生動作において補間点(▲
▼)を細かくとる場合には、十分良い近似値となりえ
る。
補間点Pkに対して前回計算した点Pk-1,前々回計算した
点Pk-2等において既に求めた第4軸の関節角θ4の値を
用いるものであるから、補間点Pkにおける第4軸の関節
角の真値(θ4)kに対して比較的良い近似値となりえ
るものである。特に、再生動作において補間点(▲
▼)を細かくとる場合には、十分良い近似値となりえ
る。
尚、(2)式等により初期値θ40を設定する場合に
は、あらかじめ所定のプログラムを作成しておけば、後
はCPU71がメモリ72に格納された各点における第4軸の
関節角θ4のデータを読み出して自動的に初期値θ40を
決定してしまうので、初期値設定の自動化を図れる利点
もある。
は、あらかじめ所定のプログラムを作成しておけば、後
はCPU71がメモリ72に格納された各点における第4軸の
関節角θ4のデータを読み出して自動的に初期値θ40を
決定してしまうので、初期値設定の自動化を図れる利点
もある。
更に補間点Pkにおける初期値θ40の他の設定法として
は、前回の点Pk-1(この点は補間点又はティーチング点
である)におけるエンドエフェクタ60の先端の位置及び
姿勢のヤコビアンを求めることにより、補間点Pkにおけ
る第4軸の関節角(θ4)kを電気的に推定することも
できる。その方法を、以下に詳述する。
は、前回の点Pk-1(この点は補間点又はティーチング点
である)におけるエンドエフェクタ60の先端の位置及び
姿勢のヤコビアンを求めることにより、補間点Pkにおけ
る第4軸の関節角(θ4)kを電気的に推定することも
できる。その方法を、以下に詳述する。
今、点Pk-1及び点Pkにおけるエンドエフェクタ60の先
端の位置及び姿勢を表わすベクトルをそれぞれベクトル 点Pk-1及び点Pkにおける各軸の関節角を表わすベクトル
をそれぞれ とする。
端の位置及び姿勢を表わすベクトルをそれぞれベクトル 点Pk-1及び点Pkにおける各軸の関節角を表わすベクトル
をそれぞれ とする。
=(X1,X2,…,X6) …(6) =(θ1,θ2,…,θ6) …(7) そして、∂Xi/∂θjを第i行第j列の行列要素とす
るヤコビアンをJ()とし、ヤコビアン(J)の逆
行列J-1()を用いて、点Pkにおける関節角ベクトル
内を(8)式〜(10)式によって近似的に表わすこと
ができる。
るヤコビアンをJ()とし、ヤコビアン(J)の逆
行列J-1()を用いて、点Pkにおける関節角ベクトル
内を(8)式〜(10)式によって近似的に表わすこと
ができる。
従って、(8)式〜(10)式を用いて補間点Pkにおけ
る第4軸の関節角θ4の値を電気的に推定することがで
き、(8)式によって計算した第4軸の関節角θ4の値
を補間点Pkにおける第4軸の関節角(θ4)kの初期値
θ40に用いることができる。この方法による初期値θ40
の設定もまた良い近似値を与えるものであり、前述した
設定例と同様に、メモリ72より点Pk-1及び補間点Pkにお
けるエンドエフェクタ60の先端の位置及び姿勢データを
読み出して(8)式から(10)式の一連の演算をCPU71
に行わせることにより、初期値θ40の設定をソフト的に
実行できる利点をも合わせもつものである。
る第4軸の関節角θ4の値を電気的に推定することがで
き、(8)式によって計算した第4軸の関節角θ4の値
を補間点Pkにおける第4軸の関節角(θ4)kの初期値
θ40に用いることができる。この方法による初期値θ40
の設定もまた良い近似値を与えるものであり、前述した
設定例と同様に、メモリ72より点Pk-1及び補間点Pkにお
けるエンドエフェクタ60の先端の位置及び姿勢データを
読み出して(8)式から(10)式の一連の演算をCPU71
に行わせることにより、初期値θ40の設定をソフト的に
実行できる利点をも合わせもつものである。
以上の様にして初期値θ40の設定を行なった後、ステ
ップS42に示す一連の演算処理を行うこととなる。
ップS42に示す一連の演算処理を行うこととなる。
ステップS42は、補間点Pkにおける第4軸の関節角
(θ4)kについての収束演算を行うステップであり、
5つのステップS42A〜S42Eより構成される。以下、補間
点Pkにおける第4軸の関節角(θ4)kを単に第4軸の
関節角θ4と記すこととして、各ステップ(S42A〜S42
E)について詳述する。
(θ4)kについての収束演算を行うステップであり、
5つのステップS42A〜S42Eより構成される。以下、補間
点Pkにおける第4軸の関節角(θ4)kを単に第4軸の
関節角θ4と記すこととして、各ステップ(S42A〜S42
E)について詳述する。
ステップS42Aでは、初期値θ40を用いて逆変換の方程
式より第1軸の関節角θ1の仮値θ11から第3軸の関節
角θ3の仮値θ31までを求める。ここで、一般化して以
下の手順を説明すべく、第1軸の関節角θ1の仮値θ11
から第3軸の関節角θ3の仮値θ31までを、(11)式〜
(13)式で示すように整数i(i≧1)をもって表現す
る。
式より第1軸の関節角θ1の仮値θ11から第3軸の関節
角θ3の仮値θ31までを求める。ここで、一般化して以
下の手順を説明すべく、第1軸の関節角θ1の仮値θ11
から第3軸の関節角θ3の仮値θ31までを、(11)式〜
(13)式で示すように整数i(i≧1)をもって表現す
る。
θ11→θ1i …(11) θ21→θ2i …(12) θ31→θ3i …(13) ここで、仮値θ1i〜θ3iの導出式を導き出すために、
順変換及び逆変換の方程式の導出について説明する。
順変換及び逆変換の方程式の導出について説明する。
今、エンドエフェクタ60の先端の位置ベクトルをp、
姿勢ベクトルを(,,)で表わすとすれば、θ系
からX系への座標変換マトリックスT、即ち順変換の方
程式は、次式(14)式及び(15)式により表わされる。
姿勢ベクトルを(,,)で表わすとすれば、θ系
からX系への座標変換マトリックスT、即ち順変換の方
程式は、次式(14)式及び(15)式により表わされる。
T=A1A2A3A4A5A6 …(15) ここで、マトリックスA1〜A6はロボットRBの各関節部
の回転に伴う座標変換マトリックスであり、(16)式〜
(21)式により表わされる。
の回転に伴う座標変換マトリックスであり、(16)式〜
(21)式により表わされる。
尚、(16)式〜(21)式においては、余弦及び正弦の
記載方法として(22)式〜(24)式により示される簡略
形を用いており、以後の説明においても同様に(22)式
〜(24)式の簡略形を用いることとする。
記載方法として(22)式〜(24)式により示される簡略
形を用いており、以後の説明においても同様に(22)式
〜(24)式の簡略形を用いることとする。
Ci=cosθi …(22) Si=sinθi …(23) i=1〜6 …(24) そこで、(16)式〜(21)式を用いて(15)式の順変
換方程式を展開すれば、座標変換マトリックスTの各要
素を求めることができるが、そのうちベクトルのz成
分nz,ベクトルのz成分az及びベクトルの各成分に
ついて成立する順変換の方程式のみを(25)式〜(29)
式へ示す。
換方程式を展開すれば、座標変換マトリックスTの各要
素を求めることができるが、そのうちベクトルのz成
分nz,ベクトルのz成分az及びベクトルの各成分に
ついて成立する順変換の方程式のみを(25)式〜(29)
式へ示す。
nz=−(S23C4C5+C23S5)C6+S23S4S6 …(25) az=−S23C4S5+C23C5 …(26) px=d3C1S23−a2S1+d2C1S2−a3(C1C23S4+S1C4) …(27) py=d3S1S23+a2C1+d2S1S2−a3(S1C23S4−C1C4) …(28) pz=d3C23+d2C2+d1+a3S23S4 …(29) 尚、(25)式〜(29)式においては、(30)式及び
(31)式により定義される簡略形C23及びS23を用いてい
る。
(31)式により定義される簡略形C23及びS23を用いてい
る。
C23=C2C3−S2S3 …(30) S23=C2S3+S2C3 …(31) 以後、以上の様にして得られた順変換の方程式(15)
式及び(25)式〜(29)式により、逆変換の解θ1〜θ
6を求めることになる。
式及び(25)式〜(29)式により、逆変換の解θ1〜θ
6を求めることになる。
そこで、(15)式をマトリックスA1〜A3の逆マトリッ
クスA1 -1〜A3 -1を用いて以下の変換を行う。
クスA1 -1〜A3 -1を用いて以下の変換を行う。
まず(15)式の両辺に逆マトリックスA-1を乗ずれ
ば、 A-1T=A2A3A4A5A6 …(32) となる。この(32)式の両辺を展開し比較すると、第1
軸の関節角θ1から第4軸の関節角θ4を変数とする3
元連立方程式が得られる。即ち、 d3S23−a3S4C23+d2S2=pxC1+pyS1 …(33) a2+a3C4=−pxS1+pyC1 …(34) d3C23−a3S4S23+d2C2=pz−d1 …(35) となる。これらの(33)式〜(35)式は、第1軸の関節
角θ1ないし第4軸の関節角θ4を変数とする、順変換
方程式(15)式を変形して得られる「第1逆変換方程
式」である。
ば、 A-1T=A2A3A4A5A6 …(32) となる。この(32)式の両辺を展開し比較すると、第1
軸の関節角θ1から第4軸の関節角θ4を変数とする3
元連立方程式が得られる。即ち、 d3S23−a3S4C23+d2S2=pxC1+pyS1 …(33) a2+a3C4=−pxS1+pyC1 …(34) d3C23−a3S4S23+d2C2=pz−d1 …(35) となる。これらの(33)式〜(35)式は、第1軸の関節
角θ1ないし第4軸の関節角θ4を変数とする、順変換
方程式(15)式を変形して得られる「第1逆変換方程
式」である。
更に、(15)式の両辺に逆マトリックスの積A3 -1A2 -1
A1 -1を乗ずれば、 A3 -1A2 -1A1 -1T=A4A5A6 …(36) となる。この(36)式の両辺を展開して比較すると、 S5S4=−axS1+ayC1 …(37) S5C4=C23(axC1+ayS1)−azS23 …(38) C5=S23(axC1+ayS1)+azC23 …(38−1) S5S6=S23(oxC1+oyS1)+ozC23 …(38−2) −S5C6=S23(nxC1+nyS1)+nzC23 …(38−3) となる。従って、第4軸の関節角θ4について、(37)
式及び(38)式より、 が得られる。この(39)式は、第1軸の関節角θ1ない
し第3軸の関節角θ3を変数として与えられる、順変換
方程式(15)式を変形して得られる「第2逆変換方程
式」である。
A1 -1を乗ずれば、 A3 -1A2 -1A1 -1T=A4A5A6 …(36) となる。この(36)式の両辺を展開して比較すると、 S5S4=−axS1+ayC1 …(37) S5C4=C23(axC1+ayS1)−azS23 …(38) C5=S23(axC1+ayS1)+azC23 …(38−1) S5S6=S23(oxC1+oyS1)+ozC23 …(38−2) −S5C6=S23(nxC1+nyS1)+nzC23 …(38−3) となる。従って、第4軸の関節角θ4について、(37)
式及び(38)式より、 が得られる。この(39)式は、第1軸の関節角θ1ない
し第3軸の関節角θ3を変数として与えられる、順変換
方程式(15)式を変形して得られる「第2逆変換方程
式」である。
ここで、(33)式〜(35)式及び(39)式を概観すれ
ば、第4軸の関節角θ4を定数として(33)式〜(35)
式を取扱うとするならば、(34)式より第1軸の関節角
θ1を求めることができ、後述する様に(33)式及び
(35)式を変形して第2軸の関節角θ2及び第3軸の関
節角θ3を求めることができることが判る。従って、第
4軸の関節角θ4に初期値を設定して(33)式〜(35)
式へ代入し、得られた第1軸から第3軸の関節角θ1〜
θ3を(39)式へ代入すれば、第4軸の関節角θ4を求
めることができる。そして、この第4軸の関節角θ4の
値と初期値とを比較し、両者の差(差分データ信号に相
当)が許容できる範囲内にない場合は、初期値を修正し
たうえで以上の手順を繰り返し続行すれば、最終的には
十分精度の高い第1軸から第4軸の関節角θ1〜θ4の
値が得られるであろうと考えることができる。
ば、第4軸の関節角θ4を定数として(33)式〜(35)
式を取扱うとするならば、(34)式より第1軸の関節角
θ1を求めることができ、後述する様に(33)式及び
(35)式を変形して第2軸の関節角θ2及び第3軸の関
節角θ3を求めることができることが判る。従って、第
4軸の関節角θ4に初期値を設定して(33)式〜(35)
式へ代入し、得られた第1軸から第3軸の関節角θ1〜
θ3を(39)式へ代入すれば、第4軸の関節角θ4を求
めることができる。そして、この第4軸の関節角θ4の
値と初期値とを比較し、両者の差(差分データ信号に相
当)が許容できる範囲内にない場合は、初期値を修正し
たうえで以上の手順を繰り返し続行すれば、最終的には
十分精度の高い第1軸から第4軸の関節角θ1〜θ4の
値が得られるであろうと考えることができる。
逆変換の方程式については以上述べた通りであり、再
びステップS42Aへ話を戻すこととする。
びステップS42Aへ話を戻すこととする。
そこで、第4軸の関節角θ4の初期値θ40及び(11)
式〜(13)式を(33)式〜(35)式へ代入すれば、仮値
θ1i,θ2i,θ3iに関して d2S2i+d3S2i3i−a3′C2i3i=pxC1i+pyS1i …(40) d2C2i+d3C2i3i+a3′C2i3i=pz−d1 …(41) −pxS1i+pyC1i=a2′ …(42) が成立する(第1軸ないし第3軸の各関節角の仮値及び
第4軸の関節角の初期値に関する第1逆変換方程式に該
当)。
式〜(13)式を(33)式〜(35)式へ代入すれば、仮値
θ1i,θ2i,θ3iに関して d2S2i+d3S2i3i−a3′C2i3i=pxC1i+pyS1i …(40) d2C2i+d3C2i3i+a3′C2i3i=pz−d1 …(41) −pxS1i+pyC1i=a2′ …(42) が成立する(第1軸ないし第3軸の各関節角の仮値及び
第4軸の関節角の初期値に関する第1逆変換方程式に該
当)。
ここで、定数a2′及びa3′は a2′=a2+a3cosθ40 …(42A) a3′=a3sinθ40 …(40A) により定義される量である。
従って、(42)式より仮値θ1iは、(43)式〜(45)
式によって求められる。
式によって求められる。
Ф1=atan2(py,px) …(44) ここで、関数atan2(X,Y)は、 X=Sφ,Y=Cφ …(46) を満足する角度φを一意に与える関数である。
次に、仮値θ2i及びθ3iについては次の様にして求め
られる。(40)式及び(41)式をそれぞれ辺々自乗して
加算すると、 2d2(αS2+βC2)=α2+β2+d2 2−d3 2−a3′2
…(47) が得られる。ここで、α,βは α=pxC1i+pyS1i …(48) β=pz−d1 …(49) によって与えられる定数である。
られる。(40)式及び(41)式をそれぞれ辺々自乗して
加算すると、 2d2(αS2+βC2)=α2+β2+d2 2−d3 2−a3′2
…(47) が得られる。ここで、α,βは α=pxC1i+pyS1i …(48) β=pz−d1 …(49) によって与えられる定数である。
従って、仮値θ2iは(50)式〜(52)式によって求め
られる。
られる。
Φ2=atan2(β,α) …(51) 又、仮値θ3iは(50)式で求められた仮値θ2iを(4
0)式及び(41)式へ代入して、(53)式により求めら
れる。
0)式及び(41)式へ代入して、(53)式により求めら
れる。
θ3i=atan2(S2i3i,C2i3i)−θ2i …(53) S2i3i=〔d3(α-d2S2)+a3′(β-d2c2)〕/(d3 2+
a3′2) …(54) C2i3i=〔d3(β-d2c2)-a3′(α-d2S2)〕/(d3 2+
a3′2 …(55) 以上の結果より、CPU71は(43)式,(50)式及び(5
3)式を用いて仮値θ1i〜θ3iを算出する。
a3′2) …(54) C2i3i=〔d3(β-d2c2)-a3′(α-d2S2)〕/(d3 2+
a3′2 …(55) 以上の結果より、CPU71は(43)式,(50)式及び(5
3)式を用いて仮値θ1i〜θ3iを算出する。
次にステップS42Bでは、ステップS42Aで算出した仮値
θ1i〜θ3iを用いて、第4軸の関節角θ4の仮値θ4iを
算出する。即ち、(39)式より仮値θ4iは、(56)式又
は(57)式(仮値θ4iを与える第2逆変換方程式に該
当)によって求められる。
θ1i〜θ3iを用いて、第4軸の関節角θ4の仮値θ4iを
算出する。即ち、(39)式より仮値θ4iは、(56)式又
は(57)式(仮値θ4iを与える第2逆変換方程式に該
当)によって求められる。
θ4i=atan2(−axS1i+ayC1i,axC1iC2i3i +ayS1iC2i3i−azS2i3i)(θ5>0のとき) …(56) θ4i=atan2(axS1i−ayC1i,−axC1iC2i3i −ayS1iC2i3i+azS2i3i)(θ5<0のとき) …(57) ステップS42Cでは、仮値θ4iと初期値θ40との比較を
行う。即ち、許容値δをあらかじめ設定しておきメモリ
72内に蓄えておく。そしてCPU71はメモリ72により許容
値δを読み出し、仮値θ4iと初期値θ40との差(θ4i−
θ40)と許容値δとを比較する。
行う。即ち、許容値δをあらかじめ設定しておきメモリ
72内に蓄えておく。そしてCPU71はメモリ72により許容
値δを読み出し、仮値θ4iと初期値θ40との差(θ4i−
θ40)と許容値δとを比較する。
もし、差(θ4i−θ40)が許容値δより大きい場合、
即ち (θ4i−θ40)>δ …(58) の場合には、ステップS42Dへ移る。
即ち (θ4i−θ40)>δ …(58) の場合には、ステップS42Dへ移る。
ステップS42D及びS42Eは、初期値θ40の修正を行うス
テップである。
テップである。
即ち、ステップS42Dでは、(60)式〜(62)式で与え
られる量θ4i′を新たな初期値θ40として用いる。
られる量θ4i′を新たな初期値θ40として用いる。
θ4i′→θ40 …(59) f=θ4i−θ40 …(61) ここで、関数fB及び記号θ40Bは、それぞれ関数f及び
初期値θ40の1つ前の計算段階における値である。尚、
(62)式の≒で結ばれた関数式を用いると、微分式df/d
θ40を直接計算しなくても前回の計算段階で求めた値を
そのまま使用できる利点がある。
初期値θ40の1つ前の計算段階における値である。尚、
(62)式の≒で結ばれた関数式を用いると、微分式df/d
θ40を直接計算しなくても前回の計算段階で求めた値を
そのまま使用できる利点がある。
そして、(59)(60)式で与えられる初期値θ40を用
いて、ステップS42E(ステップS42Eは、新たな仮値θ
1(i+1),θ2(i+1),θ3(i+1)を求める意である。)を介し
て、再び一連のステップS42A〜S42Cが繰り返される。
いて、ステップS42E(ステップS42Eは、新たな仮値θ
1(i+1),θ2(i+1),θ3(i+1)を求める意である。)を介し
て、再び一連のステップS42A〜S42Cが繰り返される。
一方、差(θ4i−θ40)が許容値δの範囲内となる場
合には、一連のステップS42A〜S42Eの繰り返しは終了
し、ステップS43へ移る。
合には、一連のステップS42A〜S42Eの繰り返しは終了
し、ステップS43へ移る。
ステップS43では、ステップS42で求められた仮値、即
ち (θ4i−θ40)≦δ …(63) を満足させる仮値θ1i〜θ4iをそれぞれ当該補間点Pkに
関して求めるべき第1軸の関節角θ1から第4軸の関節
角θ4の値として決定する。
ち (θ4i−θ40)≦δ …(63) を満足させる仮値θ1i〜θ4iをそれぞれ当該補間点Pkに
関して求めるべき第1軸の関節角θ1から第4軸の関節
角θ4の値として決定する。
θ1=θ1i …(64) θ2=θ2i …(65) θ3=θ3i …(66) θ4=θ4i …(67) ステップS44では、(64)式〜(67)式で与えられる
第1軸から第4軸の関節角の値θ1〜θ4を用いて、当
該補間点Pkにおける第5軸及び第6軸の関節角の値θ5,
θ6を求める。即ち、(37)式と(38−1)式より、第
5軸の関節角θ5は(68)〜(70)式により決定される
(これらの式は「第3逆変換方程式」に相当する)。
第1軸から第4軸の関節角の値θ1〜θ4を用いて、当
該補間点Pkにおける第5軸及び第6軸の関節角の値θ5,
θ6を求める。即ち、(37)式と(38−1)式より、第
5軸の関節角θ5は(68)〜(70)式により決定される
(これらの式は「第3逆変換方程式」に相当する)。
θ5=atan2(ξ1,ξ2) …(68) ξ1=(−axS1+ayC1)/S4 …(69) ξ2=S23(axC1+ayS1)+azC23 …(70) 又、第6軸の関節角θ6は(38−2)式と(38−3)式
より、 θ6=atan2(ε1,ε2) …(71) ε1=(oxC1S23+oyS1S23+ozC23)/S5 …(72) ε2=−(nxC1S23+nyS1S23+nzC23)/S5 …(73) によって決定される(これらの式は「第4逆変換方程
式」に相当する)。
より、 θ6=atan2(ε1,ε2) …(71) ε1=(oxC1S23+oyS1S23+ozC23)/S5 …(72) ε2=−(nxC1S23+nyS1S23+nzC23)/S5 …(73) によって決定される(これらの式は「第4逆変換方程
式」に相当する)。
以上の手順によってステップS4は終了する。
再び第4図に戻り、次のステップS5では、ステップS4
で求められた各軸の関節角(θ1〜θ6)がそれぞれ駆
動信号D1〜D6に変換された後、各駆動機構91〜96に伝達
される(第3図参照)。そして、各駆動機構91〜96は、
各駆動信号D1〜D6に応じた量だけ各アームを駆動し、エ
ンドエフェクタ60の先端は点Pk、つまりステップS3にて
計算により求めた補間点(▲▼)へ向けて移動す
る。そして、補間点Pkにて所定の処理が行われる。
で求められた各軸の関節角(θ1〜θ6)がそれぞれ駆
動信号D1〜D6に変換された後、各駆動機構91〜96に伝達
される(第3図参照)。そして、各駆動機構91〜96は、
各駆動信号D1〜D6に応じた量だけ各アームを駆動し、エ
ンドエフェクタ60の先端は点Pk、つまりステップS3にて
計算により求めた補間点(▲▼)へ向けて移動す
る。そして、補間点Pkにて所定の処理が行われる。
次に、ステップS6では、その補間点(▲▼)が
最後の補間点か否かを判断する。尚、その判断はソフト
的にコントローラ70によって行われる。そして、補間点
(▲▼)が最後の補間点でないと判断したときに
はステップS3へ戻って次の補間点 が計算され、一連のステップS4〜S5が同様に行われる。
最後の補間点か否かを判断する。尚、その判断はソフト
的にコントローラ70によって行われる。そして、補間点
(▲▼)が最後の補間点でないと判断したときに
はステップS3へ戻って次の補間点 が計算され、一連のステップS4〜S5が同様に行われる。
それに対して、補間点(▲▼)が最後の補間点
であると判断したときには、ステップS7へ移る。
であると判断したときには、ステップS7へ移る。
ステップS7では、ティーチング点(▲▼)が最後
のティーチング点か否かを判断する。この判断も又、コ
ントローラ70によって行われる。そして、ティーチング
点(▲▼)が最後のティーチング点でないと判断し
たときには、ステップS2へ戻り次のティーチング点のデ
ータ がメモリ72より読出され、一連のステップS3〜S6即ちテ
ィーチングプレイバックが再び行われる。
のティーチング点か否かを判断する。この判断も又、コ
ントローラ70によって行われる。そして、ティーチング
点(▲▼)が最後のティーチング点でないと判断し
たときには、ステップS2へ戻り次のティーチング点のデ
ータ がメモリ72より読出され、一連のステップS3〜S6即ちテ
ィーチングプレイバックが再び行われる。
それに対して、ティーチング点(▲▼)が最後の
ティーチング点であると判断したときには、ロボットRB
の全ての動作が終了する。
ティーチング点であると判断したときには、ロボットRB
の全ての動作が終了する。
D.エンドエフェクタ先端の位置・姿勢の制御精度 この発明を用いてエンドエフェクタ60の先端の位置・
姿勢を決定した場合の位置・姿勢の精度について、シミ
ュレーションを行なった結果について、簡単に説明す
る。
姿勢を決定した場合の位置・姿勢の精度について、シミ
ュレーションを行なった結果について、簡単に説明す
る。
位置・姿勢の精度の評価は、次の様な手順で行われ
る。
る。
まず、エンドエフェクタ60の先端の位置・姿勢の目
標値のデータを定める。
標値のデータを定める。
次に、目標値のデータにこの発明を適用して第1
軸〜第6軸の関節角θ1〜θ6の各値を求める。即ち、
逆変換を行う。
軸〜第6軸の関節角θ1〜θ6の各値を求める。即ち、
逆変換を行う。
そして、得られた第1軸〜第6軸の関節角θ1〜θ
6の値を順変換して、エンドエフェクタ60の先端の位置
・姿勢を求める。このときの位置・姿勢のデータは+
Δとなり、データΔが求める位置・姿勢の誤差とな
る。
6の値を順変換して、エンドエフェクタ60の先端の位置
・姿勢を求める。このときの位置・姿勢のデータは+
Δとなり、データΔが求める位置・姿勢の誤差とな
る。
以上の手順によりエンドエフェクタ60の先端の位置・
姿勢の誤差を求めた結果は、次の通りである。
姿勢の誤差を求めた結果は、次の通りである。
例えば、エンドエフェクタ60の先端が10mm〜40mmの間
隔で2点間(点Pkと点Pk+1)を移動するものとすれば、
(59)式〜(61)式で与えられる収束演算を1〜2回繰
返す程度でも、誤差Δはエンドエフェクタ60の先端の
作動範囲の10-6程度(位置誤差±0.001mm程度)内に抑
制することができる。この誤差値は殆ど無視できうるも
のであり、エンドエフェクタ60の先端の位置・姿勢の精
度は十分良好であるといえる。
隔で2点間(点Pkと点Pk+1)を移動するものとすれば、
(59)式〜(61)式で与えられる収束演算を1〜2回繰
返す程度でも、誤差Δはエンドエフェクタ60の先端の
作動範囲の10-6程度(位置誤差±0.001mm程度)内に抑
制することができる。この誤差値は殆ど無視できうるも
のであり、エンドエフェクタ60の先端の位置・姿勢の精
度は十分良好であるといえる。
又、第5軸の関節角θ5がゼロに十分近い領域(θ5
≦数deg.)では逆変換の変換式の解は一般に求まりにく
いと考えられるが、この発明によれば、(56)式と(5
7)式間の変更を行うことにより容易に解を求めること
ができる。更に収束演算の回数を増やせばエンドエフェ
クタ60の先端の位置・姿勢の精度を高めることができ、
シミュレーション結果によれば、上述の誤差と同程度の
誤差範囲内でエンドエフェクタ60の先端の位置・姿勢を
決定できることが確認されている。
≦数deg.)では逆変換の変換式の解は一般に求まりにく
いと考えられるが、この発明によれば、(56)式と(5
7)式間の変更を行うことにより容易に解を求めること
ができる。更に収束演算の回数を増やせばエンドエフェ
クタ60の先端の位置・姿勢の精度を高めることができ、
シミュレーション結果によれば、上述の誤差と同程度の
誤差範囲内でエンドエフェクタ60の先端の位置・姿勢を
決定できることが確認されている。
又、第4軸の関節角θ4の初期値θ40として、(8)
式や(4)式又は(5)式で与えられるものを用いて逆
変換の変換式の解を求める場合には、上述初期値θ40と
して(3)式で与えられるものを用いて逆変換の変換式
を求める場合よりも、収束演算の繰返し数が同一である
にもかかわらず、エンドエフェクタ60の先端の位置・姿
勢の精度が大巾に向上されることも、シミュレーション
により確認されている。
式や(4)式又は(5)式で与えられるものを用いて逆
変換の変換式の解を求める場合には、上述初期値θ40と
して(3)式で与えられるものを用いて逆変換の変換式
を求める場合よりも、収束演算の繰返し数が同一である
にもかかわらず、エンドエフェクタ60の先端の位置・姿
勢の精度が大巾に向上されることも、シミュレーション
により確認されている。
E.その他 この発明は以上説明した第5軸にオフセットのある6
軸多関節ロボットの駆動制御に限られるものでなく、自
由度が6のロボットであってそのうちの1の自由度に関
してオフセットを有するため、逆変換の変換式を解析的
に解くことができない構成のロボットの駆動制御方法全
般について適用できるものである。
軸多関節ロボットの駆動制御に限られるものでなく、自
由度が6のロボットであってそのうちの1の自由度に関
してオフセットを有するため、逆変換の変換式を解析的
に解くことができない構成のロボットの駆動制御方法全
般について適用できるものである。
請求項1の発明によれば、6つの関節角θ1〜θ6の
内第4軸の関節角θ4のみ収束演算を行なえば他の関節
角については一意的に求められるため、アームの各駆動
量決定のプロセスを簡単化できる効果があるとともに、
第5軸の関節角θ5がゼロに近い様な臨界領域において
も高精度にアームの各駆動量を決定できるので、常にエ
ンドエフェクタの先端の位置及び姿勢を高精度で制御で
きる効果がある。
内第4軸の関節角θ4のみ収束演算を行なえば他の関節
角については一意的に求められるため、アームの各駆動
量決定のプロセスを簡単化できる効果があるとともに、
第5軸の関節角θ5がゼロに近い様な臨界領域において
も高精度にアームの各駆動量を決定できるので、常にエ
ンドエフェクタの先端の位置及び姿勢を高精度で制御で
きる効果がある。
又、請求項2の発明によれば、第4軸の関節角θ4の
収束演算の繰返し回数を少なくしても高精度にアームの
各駆動量を決定できるので、エンドエフェクタの先端の
位置及び姿勢を高精度で且つ短時間で制御できる効果が
ある。
収束演算の繰返し回数を少なくしても高精度にアームの
各駆動量を決定できるので、エンドエフェクタの先端の
位置及び姿勢を高精度で且つ短時間で制御できる効果が
ある。
更に、請求項3の発明においても、請求項2の発明と
同様の効果を有するものである。
同様の効果を有するものである。
第1図はこの発明の一実施例である6軸多関節ロボット
の機械的構成を示す側面図、 第2図は第1図の6軸多関節ロボットの第2のアーム,
オフセット部及び第3のアームの機械的構成を拡大して
示す平面図、 第3図は6軸多関節ロボットの制御系の電気的構成を示
すブロック図、 第4図は6軸多関節ロボットを駆動制御するためのステ
ップを示すフローチャート、 第5図は第4図におけるステップS4を詳細に説明するフ
ローチャート、第6図は逆変換アルゴリズムの流れを示
す説明図である。 RB……6軸多関節ロボット 10……ロボットの設置面 20……ロボットの支柱 21……第1の関節部、30……第1のアーム 31……第2の関節部、32……第3の関節部 40……第2のアーム、41……第4の関節部 42……オフセット部、43……第6の関節部 50……第3のアーム、60……エンドエフェクタ 91……第1の駆動機構 92……第2の駆動機構 93……第3の駆動機構 94……第4の駆動機構 95……第5の駆動機構 96……第6の駆動機構 θ1……第1軸の関節角 θ2……第2軸の関節角 θ3……第3軸の関節角 θ4……第4軸の関節角 θ5……第5軸の関節角 θ6……第6軸の関節角
の機械的構成を示す側面図、 第2図は第1図の6軸多関節ロボットの第2のアーム,
オフセット部及び第3のアームの機械的構成を拡大して
示す平面図、 第3図は6軸多関節ロボットの制御系の電気的構成を示
すブロック図、 第4図は6軸多関節ロボットを駆動制御するためのステ
ップを示すフローチャート、 第5図は第4図におけるステップS4を詳細に説明するフ
ローチャート、第6図は逆変換アルゴリズムの流れを示
す説明図である。 RB……6軸多関節ロボット 10……ロボットの設置面 20……ロボットの支柱 21……第1の関節部、30……第1のアーム 31……第2の関節部、32……第3の関節部 40……第2のアーム、41……第4の関節部 42……オフセット部、43……第6の関節部 50……第3のアーム、60……エンドエフェクタ 91……第1の駆動機構 92……第2の駆動機構 93……第3の駆動機構 94……第4の駆動機構 95……第5の駆動機構 96……第6の駆動機構 θ1……第1軸の関節角 θ2……第2軸の関節角 θ3……第3軸の関節角 θ4……第4軸の関節角 θ5……第5軸の関節角 θ6……第6軸の関節角
Claims (3)
- 【請求項1】アーム先端にエンドエフェクタが取付けら
れてなり、第5軸にオフセットのある6軸関節ロボット
の駆動制御装置であって、 (a)前記ロボットの設置面に平行な面をXY平面とする
XYZ絶対座標系において、所定の移動経路上の各ティー
チング点に前記エンドエフェクタの先端を順次に移動し
てその位置及び姿勢決めを行うことにより得られる各テ
ィーチング点における各軸の関節角の値のデータを電気
的に順変換することにより各ティーチング点のティーチ
ングデータ信号を求めると共に、 互いに隣り合うティーチング点間を両ティーチング点に
関するティーチングデータ信号より補間して、補間点に
於ける前記エンドエフェクタの先端の位置及び姿勢に関
する補間点データ信号を電気的に求める手段と、 (b)前記移動経路上の前記補間点における前記エンド
エフェクタの先端の位置及び姿勢の補間点データ信号か
ら、前記補間点における前記ロボットの各軸の関節角を
与える信号を、順変換方程式を変形することにより与え
られる、第1軸ないし第4軸の各関節角を変数とする第
1を変換方程式を電気的に解くことによって決定する手
段とを備え、 前記手段(b)は、 (b−1)第4軸の関節角の値に初期値を電気的に与え
る手段と、 (b−2)前記手段(b−1)により定められた前記第
4軸の関節角の値の信号を用いて前記第1逆変換方程式
より第1軸の関節角から第3軸の関節角までの値の信号
を順次に電気的に求める手段と、 (b−3)前記手段(b−2)により求められた前記第
1軸の関節角から前記第3軸の関節角までの値の信号を
用いて、前記順変換方程式を変形することにより得られ
る、前記第1軸ないし第3軸の各関節角を変数とする前
記第4軸の関節角を与える第2逆変換方程式より改めて
第4軸の関節角の値の信号を電気的に求める手段と、 (b−4)前記手段(b−3)により求められた第4軸
の関節角の値の信号と前記手段(b−2)に与えられた
前記初期値の信号との差分データ信号を求め、当該差分
データ信号が許容範囲内か否かを判定する手段と、 (b−5)前記手段(b−4)が許容範囲内にないと判
定したときには、前記手段(b−3)が求めた前記第4
軸の関節角の値の信号を電気的に補正して、新たな初期
値を電気的に決定する手段と、 (b−6)前記手段(b−5)によって求められた補正
後の初期値の信号を前記第4軸の関節角の値の信号とし
て前記手段(b−2)に入力して前記手段(b−2)を
再実行させる手段と、 (b−7)前記手段(b−4)が、前記差分データ信号
が前記許容範囲内になったと判定したときには、そのと
きの第1軸の関節角の値から第4軸の関節角の値までの
信号を前記第1逆変換方程式の解として電気的に決定す
る手段と、 (b−8)前記手段(b−7)により決定された前記第
1軸の関節角から前記第4軸の関節角までの値の信号を
用いて、前記順変換方程式を変形して得られる第3及び
第4逆変換方程式よりそれぞれ第5軸及び第6軸の関節
角の値の信号を電気的に求める手段とを有し、 前記駆動制御装置は、 (c)前記手段(b)で求めた各軸の関節角の値の信号
に応じた駆動出力を前記アームの対応する駆動機構に与
えて、前記6軸関節ロボットの駆動制御を行う手段を更
に備えることを特徴とする、 6軸関節ロボットの駆動制御装置。 - 【請求項2】請求項1記載の6軸関節ロボットの駆動制
御装置であって、 前記移動経路上の任意のk番目の点をPkとして表すと、 (k+1)番目の補間点Pk+1における前記第4軸の関節
角の初期値として、 (i)前記k番目の点Pk及び前記点Pk+1における前記エ
ンドエフェクタの先端のそれぞれの位置及び姿勢と、 (ii)前記点Pkにおける前記エンドエフェクタの先端の
位置及び姿勢のヤコビアンの値と、 (iii)前記点Pkにおいて既に決定されている前記第4
軸の関節角の値 とを用いて近似計算により電気的に推定される前記補間
点Pk+1における第4軸の関節角の値を用いることを特徴
とする、 6軸関節ロボットの駆動制御装置。 - 【請求項3】請求項1記載の6軸関節ロボットの駆動制
御装置であって、 前記移動経路上のk番目の点である補間点Pkにおける前
記第4軸の関節角の初期値として、 (k−)番目の点Pk−〔1≦≦(k−1)〕か
ら(k−1)番目の点Pk-1それぞれにおいて既に決定さ
れている第4軸の関節角の値の線形結合によって与えら
れる値を用いることを特徴とする、 6軸関節ロボットの駆動制御装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2331247A JP3030565B2 (ja) | 1990-11-28 | 1990-11-28 | ロボットの駆動制御装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2331247A JP3030565B2 (ja) | 1990-11-28 | 1990-11-28 | ロボットの駆動制御装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH04313105A JPH04313105A (ja) | 1992-11-05 |
JP3030565B2 true JP3030565B2 (ja) | 2000-04-10 |
Family
ID=18241548
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2331247A Expired - Fee Related JP3030565B2 (ja) | 1990-11-28 | 1990-11-28 | ロボットの駆動制御装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3030565B2 (ja) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6897520B2 (ja) * | 2017-11-24 | 2021-06-30 | 株式会社デンソーウェーブ | ロボットの制御装置及びロボットの逆変換処理方法 |
JP2019177436A (ja) * | 2018-03-30 | 2019-10-17 | 日本電産株式会社 | ロボット制御装置、ロボットの関節の角度を求める方法、プログラム |
-
1990
- 1990-11-28 JP JP2331247A patent/JP3030565B2/ja not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JPH04313105A (ja) | 1992-11-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US4680519A (en) | Recursive methods for world-to-joint transformation for a robot manipulator | |
EP0086950B1 (en) | Method of controlling an industrial robot | |
US4887222A (en) | Method for controlling operation of industrial robot | |
JP4735795B2 (ja) | 冗長マニピュレータの制御方法 | |
JP6700669B2 (ja) | 制御方法、ロボット装置、プログラム、記録媒体、及び物品の製造方法 | |
KR100262382B1 (ko) | 로봇의속도연산장치및로봇의속도연산방법 | |
JP2512099B2 (ja) | ロボットの動作教示方法および制御装置 | |
JPH09109072A (ja) | 冗長マニピュレータの制御方法 | |
GB2188754A (en) | A method for identifying kinematic parameters of robotic manipulators | |
JP3349652B2 (ja) | オフラインティーチング方法 | |
JPH04233602A (ja) | ロボットのたわみ補正方法及びたわみ認識方法 | |
EP0465661B1 (en) | Method and apparatus for controlling an articulated robot having redundancy | |
CN115008476B (zh) | 基于等效运动学模型的机器人标定方法及设备 | |
JP3030565B2 (ja) | ロボットの駆動制御装置 | |
JP2703767B2 (ja) | ロボットの教示データ作成方法 | |
JPH06304893A (ja) | 位置決め機構のキャリブレーション方式 | |
JP3206765B2 (ja) | マスタスレーブマニピュレータの制御方法 | |
JP4290312B2 (ja) | パラレル機構と、その制御方法及び制御装置 | |
JPH0411339B2 (ja) | ||
JPH08174453A (ja) | ロボット装置における位置決め誤差の計測装置および位置決め誤差の補正方法 | |
JPH0583922B2 (ja) | ||
JPH05303418A (ja) | 円筒座標ロボットの駆動制御方法 | |
Tatte et al. | Reduction of positional error in a path of a 2 DOF serial planar manipulator | |
Legnani et al. | Calibration of a SCARA Robot By Force-Controlled Contour Tracking of an Object of Known Geometry. | |
JP3507032B2 (ja) | ロボットの機差導出方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
LAPS | Cancellation because of no payment of annual fees |