JPH06301411A - 産業用ロボットシステムの設置誤差較正方法及び較正制御装置 - Google Patents

Abstract

(57)【要約】 【目的】 取り付けなどの設置誤差を較正することがで
きる産業用ロボットシステムの設置誤差較正方法及び較
正制御装置を提供する。 【構成】 ツールを移動させるロボットを含むツール移
動装置と、被作業物であるワークを含むワーク取扱装置
とを備え、ツールの位置がワークに設定されたワークベ
ース座標系の原点となるようにロボットを制御して、ロ
ボットの制御の前後におけるツールの先端と、上記ワー
クベース座標系の原点にその１頂点が一致するように載
置された直方体のブロックとの間の各距離を測定し、上
記測定された各距離に基づいて、所定の絶対座標系を基
準としたワークベース座標系の位置の誤差を演算し、上
記演算した誤差に基づいて絶対座標系を基準としたワー
クベース座標系の位置を較正する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、産業用ロボットシステ
ムの設置誤差較正方法及び較正制御装置に関する。

【０００２】

【従来の技術】従来、作業用ツールを把持し当該ツール
の位置と姿勢（以下、位置姿勢という。）を制御するロ
ボットマニピュレータ（以下、ロボットという。）と、
ロボットを搭載する移動機構装置である１軸スライダ
と、被作業ワークを搭載し当該ワークの姿勢を制御する
ワーク取扱装置であるポジショナと、それらを制御する
制御装置とを用いて産業用ロボットシステムを構成し、
ストアード・コンピュータプログラム方式を用いて、ロ
ボットとスライダの両方を動作させる協調動作によりツ
ールの位置姿勢を制御することが知られている。ここ
で、制御装置内部に設定する各機械毎の座標系は、一般
にスライダの駆動軸方向とロボットの機械座標系の座標
軸の方向とが一致するように設定される。

【０００３】

【発明が解決しようとする課題】しかしながら、実際の
ロボットシステムでは、スライダ上にロボットを搭載す
る場合の据え付け誤差により、しばしば、スライダの駆
動軸方向とロボットの機械座標系の座標軸の方向が正確
に一致しない場合が生じる。当該据え付け誤差がある実
際のロボットシステムにおいては、ロボットとスライダ
の両方を協調動作させた場合に、ワークに対するツール
の相対速度を一定にすることができず、作業中のツール
の軌跡が予め決められた所望のツールの軌跡からずれ
て、例えば溶接作業などに適用した場合、溶接の品質が
大幅に低下するなど、所望の溶接結果を得ることができ
ないという問題点がった。

【０００４】以上の問題点を解決するために、各機械の
座標を校正する方法（以下、従来例の方法という。）
が、例えば、特開昭６４−８４０８号公報において開示
されている。この方法では、複数個の座標系の各々によ
って表現されたデータ組み合わせてロボットの動作制御
を行うロボットシステムと視覚装置の各々に固有の同次
変換行列を設定し、視覚装置によりワークの位置姿勢の
ずれ量を検出して、ロボットと視覚装置との間で座標系
間の較正を行う。

【０００５】しかしながら、従来例の方法では、視覚装
置を用いるとシステム全体のコストが高価となるという
問題点があった。また、当該方向の明細書の発明の詳細
な説明の欄の最後部に、「ロボットとポジショナ等の周
辺機器、ロボットと他のロボット、更にはそれらを包含
した多数の機器により構成される産業用ロボットシステ
ムなどにも、ロボットと視覚装置との間の座標系間を較
正する方法が同様に適用しうる」と記載しているが、そ
の具体的な実現手段は記載されておらず、その方法は不
明である。言い換えれば、この従来例の方法によれば、
ロボット単体のみへの適用は可能であるが、複数の機械
で構成されるロボットシステムへの適用はできない。

【０００６】本発明の目的は以上の課題を解決し、ツー
ルを把持するツール移動装置と、ワークを搭載するワー
ク取扱装置とからなる産業用ロボットシステムにおい
て、取り付けなどの設置誤差を較正することができる産
業用ロボットシステムの設置誤差較正方法及びその較正
制御装置を提供することにある。

【０００７】

【課題を解決するための手段】本発明に係る請求項１記
載の産業用ロボットシステムの設置誤差較正方法は、所
定の作業を行うツールと、上記ツールを移動させる第１
の関節手段とを含むツール移動装置と、被作業物である
ワークを含むワーク取扱装置とを備えた産業用ロボット
システムの設置誤差較正方法であって、上記ツールの位
置が上記ワークに設定されたワークベース座標系の原点
となるように上記第１の関節手段を制御して、上記第１
の関節手段の制御の前後における上記ツールの先端と、
上記ワークベース座標系の原点にその１頂点が一致する
ように載置された直方体のブロックとの間の各距離を測
定する第１のステップと、上記第１のステップにおいて
測定された各距離に基づいて、所定の絶対座標系を基準
とした上記ワークベース座標系の位置の誤差を演算し、
上記演算した誤差に基づいて絶対座標系を基準とした上
記ワークベース座標系の位置を較正する第２のステップ
とを含むことを特徴とする。

【０００８】また、請求項２記載の産業用ロボットシス
テムの設置誤差較正方法は、請求項１記載の方法におい
て、上記ワーク取扱装置は、上記ワークを移動させる第
２の関節手段をさらに含み、上記設置誤差較正方法はさ
らに、上記ツールの位置を所定の第１の位置から所定の
第２の位置に移動させるように上記第２の関節手段を制
御して、上記第２の関節手段の制御の前後における上記
ツールの先端と上記第２の関節手段との間の各距離を測
定する第３のステップと、上記第３のステップにおいて
測定された各距離に基づいて、上記ツールのずれ量を演
算しかつ上記絶対座標系を基準とした上記ワークベース
座標系の位置姿勢の誤差を演算し、上記演算した誤差に
基づいて上記絶対座標系を基準とした上記ワークベース
座標系の位置姿勢を較正する第４のステップとを含むこ
とを特徴とする。

【０００９】さらに、請求項３記載の産業用ロボットシ
ステムの設置誤差較正方法は、請求項１又は２記載の方
法において、上記ツール移動装置は、上記第１の関節手
段を所定の軸方向に沿って移動させる第３の関節手段を
さらに含み、上記設置誤差較正方法はさらに、上記第１
の関節手段を上記所定の軸方向に沿って所定の第３の位
置から所定の第４の位置に移動させるように上記第３の
関節手段を制御して、上記ツールの先端に治具を介して
設けられた距離測定手段を用いて、上記第３の関節手段
の制御の前後におけるその各面が上記絶対座標系の各軸
方向に平行となるように載置された直方体のブロックの
各面との各距離を測定する第５のステップと、上記第５
のステップにおいて測定された各距離に基づいて、上記
絶対座標系を基準とした上記ツールに設定されたツール
ベース座標系の位置姿勢のねじれ角度を演算し、上記演
算したねじれ角度に基づいて上記絶対座標系を基準とし
た上記ツールに設定されたツールベース座標系の位置姿
勢を較正する第６のステップと、上記ワークを基準とし
た上記ツールの位置姿勢を一定とする条件のもとで、上
記第１の関節手段を上記所定の軸方向に沿って所定の第
５の位置から所定の第６の位置に移動させるように上記
第１の関節手段と上記第３の関節手段とを制御して、上
記第１の関節手段と上記第３の関節手段の制御の前後に
おける上記ツールの先端と所定の固体位置との間の各距
離を測定する第７のステップと、上記第７のステップに
おいて測定された各距離に基づいて、上記第３の関節手
段の上記所定の軸方向に対する上記ツールベース座標系
のねじれ角度を演算し、上記演算したねじれ角度に基づ
いて上記第３の関節手段の上記所定の軸方向に対する上
記ツールベース座標系のねじれ角度を較正する第８のス
テップとを含むことを特徴とする。

【００１０】本発明に係る請求項４記載の産業用ロボッ
トシステムの設置誤差較正制御装置は、所定の作業を行
うツールと、上記ツールを移動させる第１の関節手段と
を含むツール移動装置と、被作業物であるワークを含む
ワーク取扱装置とを備えた産業用ロボットシステムにお
いて、上記ツールの位置が上記ワークに設定されたワー
クベース座標系の原点となるように上記第１の関節手段
を制御する第１の制御手段と、上記第１の関節手段の先
端に設けられ、上記第１の制御手段の動作の前後におけ
る上記ツールの先端と上記ワークベース座標系の原点に
その１頂点が一致するように載置された直方体のブロッ
クとの間の各距離を測定する第１の測定手段と、上記第
１の測定手段によって測定された各距離に基づいて、所
定の絶対座標系を基準とした上記ワークベース座標系の
位置の誤差を演算し、上記演算した誤差に基づいて絶対
座標系を基準とした上記ワークベース座標系の位置を較
正する第１の較正手段とを備えたことを特徴とする。

【００１１】また、請求項５記載の産業用ロボットシス
テムの設置誤差較正制御装置は、請求項４記載の装置に
おいて、上記ワーク取扱装置は、上記ワークを移動させ
る第２の関節手段をさらに含み、上記設置誤差較正制御
装置はさらに、上記ツールの位置を所定の第１の位置か
ら所定の第２の位置に移動させるように上記第２の関節
手段を制御する第２の制御手段と、上記第１の関節手段
の先端に設けられ、上記第２の制御手段の動作の前後に
おける上記ツールの先端と上記第２の関節手段との間の
各距離を測定する第２の測定手段と、上記第２の測定手
段によって測定された各距離に基づいて、上記ツールの
ずれ量を演算しかつ所定の絶対座標系を基準とした上記
ワークベース座標系の位置姿勢の誤差を演算し、上記演
算した誤差に基づいて上記絶対座標系を基準とした上記
ワークベース座標系の位置姿勢を較正する第２の較正手
段とを備えたことを特徴とする。

【００１２】さらに、請求項６記載の産業用ロボットシ
ステムの設置誤差較正制御装置は、請求項４又は５記載
の装置において、上記ツール移動装置は、上記第１の関
節手段を所定の軸方向に沿って移動させる第３の関節手
段をさらに含み、上記設置誤差較正制御装置はさらに、
上記第１の関節手段を上記所定の軸方向に沿って所定の
第３の位置から所定の第４の位置に移動させるように上
記第３の関節手段を制御する第３の制御手段と、上記ツ
ールの先端に治具を介して設けられ、上記第３の関節手
段の制御の前後におけるその各面が上記絶対座標系の各
軸方向に平行となるように載置された直方体のブロック
の各面との各距離を測定する第３の測定手段と、上記第
３の測定手段によって測定された各距離に基づいて、上
記絶対座標系を基準とした上記ツールに設定されたツー
ルベース座標系の位置姿勢のねじれ角度を演算し、上記
演算したねじれ角度に基づいて上記絶対座標系を基準と
した上記ツールに設定されたツールベース座標系の位置
姿勢を較正する第３の較正手段と、上記ワークを基準と
した上記ツールの位置姿勢を一定とする条件のもとで、
上記第１の関節手段を上記所定の軸方向に沿って所定の
第５の位置から所定の第６の位置に移動させるように上
記第１の関節手段と上記第３の関節手段とを制御する第
４の制御手段と、上記ツールの先端と所定の固体位置と
の間の各距離を測定する第４の測定手段と、上記第４の
測定手段によって測定された各距離に基づいて、上記第
３の関節手段の上記所定の軸方向に対する上記ツールベ
ース座標系のねじれ角度を演算し、上記演算したねじれ
角度に基づいて上記第３の関節手段の上記所定の軸方向
に対する上記ツールベース座標系のねじれ角度を較正す
る第４の較正手段とを備えたことを特徴とする。

【００１３】

【作用】請求項１記載の方法及び請求項４記載の装置に
おいては、上記ツールの位置が上記ワークに設定された
ワークベース座標系の原点となるように上記第１の関節
手段を制御して、上記第１の関節手段の制御の前後にお
ける上記ツールの先端と、上記ワークベース座標系の原
点にその１頂点が一致するように載置された直方体のブ
ロックとの間の各距離を測定する。次いで、上記測定さ
れた各距離に基づいて、所定の絶対座標系を基準とした
上記ワークベース座標系の位置の誤差を演算し、上記演
算した誤差に基づいて絶対座標系を基準とした上記ワー
クベース座標系の位置を較正する。

【００１４】また、請求項２記載の方法及び請求項５記
載の装置においては、上記ツールの位置を所定の第１の
位置から所定の第２の位置に移動させるように上記第２
の関節手段を制御して、上記第２の関節手段の制御の前
後における上記ツールの先端と上記第２の関節手段との
間の各距離を測定する。次いで、上記測定された各距離
に基づいて、上記ツールのずれ量を演算しかつ上記絶対
座標系を基準とした上記ワークベース座標系の位置姿勢
の誤差を演算し、上記演算した誤差に基づいて上記絶対
座標系を基準とした上記ワークベース座標系の位置姿勢
を較正する。

【００１５】さらに、請求項３記載の方法及び請求項６
記載の装置においては、上記第１の関節手段を上記所定
の軸方向に沿って所定の第３の位置から所定の第４の位
置に移動させるように上記第３の関節手段を制御して、
上記ツールの先端に治具を介して設けられた距離測定手
段を用いて、上記第３の関節手段の制御の前後における
その各面が上記絶対座標系の各軸方向に平行となるよう
に載置された直方体のブロックの各面との各距離を測定
する。次いで、上記測定された各距離に基づいて、上記
絶対座標系を基準とした上記ツールに設定されたツール
ベース座標系の位置姿勢のねじれ角度を演算し、上記演
算したねじれ角度に基づいて上記絶対座標系を基準とし
た上記ツールに設定されたツールベース座標系の位置姿
勢を較正する。さらには、上記ワークを基準とした上記
ツールの位置姿勢を一定とする条件のもとで、上記第１
の関節手段を上記所定の軸方向に沿って所定の第５の位
置から所定の第６の位置に移動させるように上記第１の
関節手段と上記第３の関節手段とを制御して、上記第１
の関節手段と上記第３の関節手段の制御の前後における
上記ツールの先端と所定の固体位置との間の各距離を測
定する。次いで、上記測定された各距離に基づいて、上
記第３の関節手段の上記所定の軸方向に対する上記ツー
ルベース座標系のねじれ角度を演算し、上記演算したね
じれ角度に基づいて上記第３の関節手段の上記所定の軸
方向に対する上記ツールベース座標系のねじれ角度を較
正する。

【００１６】

【実施例】以下、図面を参照して、本発明に係る一実施
例である産業用ロボットシステムのための設置誤差較正
制御装置について、以下の項目の順序で説明する。 （１）産業用ロボットシステムとその設置誤差 （２）産業用ロボットシステムとその設置誤差較正制御
装置の構成 （３）設置誤差較正制御装置の処理 （３−１）メインルーチン （３−２）第１の較正処理 （３−３）第２の較正処理 （３−４）第３の較正処理 （３−５）第４の較正処理 （４）教示作業と再生動作 （５）変形例

【００１７】（１）産業用ロボットシステムとその設置
誤差 図１に本実施例の産業用ロボットシステムとその設置誤
差較正制御装置（以下、制御装置という。）５の全体を
示し、図２にその機構図を示す。

【００１８】本実施例の産業用ロボットシステムにおい
ては、図１に示すように、ツール移動装置１１とワーク
取扱装置１２と制御装置５とに大きく分けられ、（ａ）
ツール移動装置１１は、例えば溶接トーチであるツール
４を把持しその位置姿勢を制御する６自由度のロボット
１と、それを搭載した移動機構装置であるスライダ２と
からなり、（ｂ）ワーク取扱装置１２は、被作業対象物
であるワーク（図示せず。）を載置するポシショナ３を
備え、（ｃ）制御装置５には、教示作業時において上記
２個の装置１１，１２を動作させて作業プログラムを作
成するために操作者がデータの入力を行うティーチング
ボックス２０と、教示作業と自動運転作業とを切り換え
たり自動運転作業時における起動信号を出力するために
操作者が手動操作する操作ボックス２１と、詳細後述す
るように所定の距離をそれぞれ測定する３つの距離セン
サＳＳ１−ＳＳ３とが接続され、本実施例は、これら機
械を設置したときに生ずる各機械間の座標系の位置を、
当該ロボットシステムを動作させる前、すなわち教示作
業や再生動作の前に、較正して正確にこれらの機械を協
調動作させる制御装置を備えたことを特徴とする。

【００１９】また、ツール移動装置１１において、ロボ
ット１を載置するスライダ２は、図１と図２とに示すよ
うに、その機械原点Ｏｓとロボット１の機械原点Ｏｍと
の間に互いに縦続して連結された４個のリンクＬ１０乃
至Ｌ１３からなり、リンクＬ１１と平行な方向である第
１軸（当該第１軸はリンクＬ１１と平行なので符号Ｌ１
１とする。）に沿って移動するとともに、リンクＬ１１
と直交するリンクＬ１２と平行な方向である第２軸（当
該第２軸はリンクＬ１２と平行なので符号Ｌ１２とす
る。）に沿って移動して、搭載したロボット１を、絶対
座標系ΣｗｏｒｌｄのＸ−Ｙ平面上で水平方向に移動さ
せる。ここで、スライダ２の各リンクＬ１０乃至Ｌ１３
を連結する各関節ＲＪ１１乃至ＲＪ１３を備え、このう
ち各関節ＲＪ１１，ＲＪ１２はそれぞれ、第１軸Ｌ１１
に沿って移動させる移動機構と、第２軸Ｌ１２に沿って
移動させる移動機構に対応する。

【００２０】さらに、ロボット１は、図１と図２とに示
すように、６個の回転可能な関節ＲＪ１乃至ＲＪ６と、
各関節間を連結する７個のリンクＬ０乃至Ｌ６とを備
え、ロボット１の機械原点Ｏｍは、第０リンクＬ０を介
して第１関節ＲＪ１に連結され、第１関節ＲＪ１は第１
リンクＬ１を介して第２関節ＲＪ２に連結され、第２関
節ＲＪ２は第２リンクＬ２を介して第３関節ＲＪ３に連
結される。また、第３関節ＲＪ３は第３リンクＬ３を介
して第４関節ＲＪ４に連結され、第４関節ＲＪ４は第４
リンクＬ４を介して第５関節ＲＪ５に連結され、第５関
節ＲＪ５は第５リンクＬ５を介して第６関節ＲＪ６に連
結される。さらに、第６関節ＲＪ６は第６リンクＬ６を
介してツール４に連結される。ロボット１の各関節ＲＪ
ｋ（ｋ＝１，２，…，６）は図４に示すように、制御装
置３によってその回転角度が制御されサーボステッピン
グモータにてなるモータＭＯｋと、モータＭＯｋの回転
を所定の減速比λｋで減速する減速機ＲＥｋと、減速後
の回転軸の位置を検出するセンサＳＮｋとから構成さ
れ、その軸がそれぞれ回転される。

【００２１】上記スライダ２の各関節ＲＪ１１，ＲＪ１
２に対応する移動機構はそれぞれ、ロボット１と同様
に、第１軸Ｌ１１に沿って移動させるモータＭＯ９と移
動後の位置を検出するセンサＳＮ９と、第２軸Ｌ１２に
沿って移動させるモータＭＯ１０と移動後の位置を検出
するセンサＳＮ１０とを備える。

【００２２】さらに、ポシショナ３は、その機械原点Ｏ
ｐとワークの制御点Ｏｗｏｒｋとの間に互いに縦続して
連結された４本のリンクＬ２０乃至Ｌ２３を備え、リン
クＬ２１，Ｌ２２間に回転可能な関節ＲＪ２１を備える
とともに、リンクＬ２２，Ｌ２３間に回転可能な関節Ｒ
Ｊ２２を備える。各関節ＲＪ２１，ＲＪ２２はそれぞ
れ、ロボット１と同様に、図４に示すように、制御装置
５によってその回転角度が制御されサーボモータにてな
るモータＭＯｋと、モータＭＯｋの回転を所定の減速比
λｋで減速する減速機ＲＥｋと、減速後の回転軸の位置
を検出するセンサＳＮｋとから構成され、その軸がそれ
ぞれ回転され、ここで、ｋ＝７，８である。

【００２３】本実施例のロボットシステムの各々の機械
要素の機械原点およびシステム原点を定義づけるため
に、図２及び図７乃至図９に示すように座標系を設定
し、各座標系の名称と、座標系の記号と、その設定位置
を以下に示す。 （ａ）絶対座標系Σｗｏｒｌｄ：ロボットシステムの原
点Ｏｗｏｒｌｄに設定する。 （ｂ）ツールベース座標系Σｓ：スライダ２の機械原点
Ｏｓに設定する。 （ｃ）ツール機械座標系Σｍ：ロボット１の機械原点Ｏ
ｍに設定する。 （ｄ）ツール側メカニカルインターフェイス座標系Σｔ
ｍ：ロボット１の出力軸原点Ｏｔｍに設定する。 （ｅ）ツール座標系Σｔｏｏｌ：ツール４の先端位置Ｏ
ｔｏｏｌに設定する。 （ｆ）ワークベース座標系Σｐ：ポジショナ３の機械原
点Ｏｐに設定する。 （ｇ）ワーク側メカニカルインターフェイス座標系Σｗ
ｍ：ポジショナ２の出力軸原点Ｏｗｍに設定する。 （ｈ）ワーク座標系Σｗｏｒｋ：ワーク制御原点（以
下、ワーク原点という。）Ｏｗｏｒｋに設定する。 なお、各座標系は、Ｘ軸，Ｙ軸，Ｚ軸の３軸でかつそれ
ぞれの付属符号を付して表される。

【００２４】ここで、２つの座標系間の位置姿勢の関係
示す同次変換行列の符号を以下に示す。これらの同次変
換行列の各要素と、これら同次変換行列を演算するとき
に用いられる詳細後述の位置データと姿勢データはＲＡ
Ｍ１０２に格納される。 （ａ）［Ｚｔ］：絶対座標系Σｗｏｒｌｄを基準とした
ツールベース座標系Σｓの位置姿勢。 （ｂ）［Ｔｓ］：ツールベース座標系Σｓを基準とした
ツール機械座標系Σｍの位置姿勢。当該位置姿勢は、ス
ライダ２の位置姿勢である。 （ｃ）［Ｔｍａ］：ツール機械座標系Σｍを基準とした
ツール側メカニカルインターフェイス座標系Σｔｍの位
置姿勢。当該位置姿勢は、ロボット１の位置姿勢であ
る。 （ｄ）［Ｅｔ］：ツール側メカニカルインターフェイス
座標系Σｔｍを基準としたツール座標系Σｔｏｏｌの位
置姿勢。 （ｅ）［Ｚｗ］：絶対座標系Σｗｏｒｌｄを基準とした
ワークベース座標系ΣＯｐの位置姿勢。 （ｆ）［Ｔｐ］：ワークベース座標系Σｐを基準とした
ワーク側メカニカルインターフェイス座標系Σｗｍの位
置姿勢。当該位置姿勢は、ポジショナ３の位置姿勢であ
る。 （ｇ）［Ｅｗ］：ワーク側メカニカルインターフェイス
座標系Σｗｍを基準としたワーク座標系Σｗｏｒｋの位
置姿勢。

【００２５】まず、ツール側メカニカルインターフェイ
ス座標系Σｗｍを基準としたツール座標系Σｔｏｏｌの
位置姿勢を示す同次変換行列［Ｅｔ］は、３つの位置デ
ータＡｔｅ、Ｂｔｅ、Ｃｔｅと、３つの姿勢データαｔ
ｅ、βｔｅ、γｔｅを用いて、次の数１によって表すこ
とができる。

【数１】 ［Ｅｔ］＝Trans(Ａte,Ｂte,Ｃte)・Rot(Ｘ,αte)・Rot(Ｙ,βte)・Rot(Ｚ,γte)

【００２６】ここで、Ｒｏｔ（ａ，ｂ）はａの軸を中心
として角度ｂ（ｒａｄ）だけ回転することを示す回転変
換であり、Ｔｒａｎｓ（ａ，ｂ，ｃ）はＸ軸方向に距離
ａだけ並進させ、Ｙ軸方向に距離ｂだけ並進させ、Ｚ軸
方向に距離ｃだけ並進させることを示す並進変換であ
り、以下同様である。これらの同次変換は、リチャード
・ピー・ポール（Ｒｉｃｈａｒｄ Ｐ Ｐａｕｌ）著，
“ロボット・マニピュレータズ；数学，プログラミング
と制御（ＲＯＢＯＴ ＭＡＮＩＰＵＬＡＴＯＲＳ；ＭＡ
ＴＨＥＭＡＴＩＣＳ，ＰＲＯＧＲＡＭＭＩＮＧ，ＡＮＤ
ＣＯＮＴＲＯＬ）”，ザ・エムアイティー・プレス
（Ｔｈｅ ＭＩＴ Ｐｒｅｓｓ），１９８１年において
用いられている。なお、上記数１から、ツール側メカニ
カルインターフェイス座標系Σｔｍを基準としたツール
座標系Σｔｏｏｌの位置姿勢を示す同次変換行列［Ｅ
ｔ］は、Ｔｒａｎｓ（Ａｔｅ，Ｂｔｅ，Ｃｔｅ）なる並
進変換を行い、Ｘ軸を中心にαｔｅ［ｒａｄ］だけ回転
変換し、Ｙ軸を中心にβｔｅ［ｒａｄ］だけ回転変換
し、かつＺ軸を中心にγｔｅ［ｒａｄ］だけ回転変換す
る同次変換であることを示す。

【００２７】上記数１の結果は、次の数２で示す４行×
４列の同次変換行列の形式で表すことができる。

【数２】

【００２８】また、ワーク側メカニカルインターフェイ
ス座標系Σｗｍを基準としたワーク座標系の位置姿勢を
示す同次変換行列［Ｅｗ］は、３つの位置データＡｗ
ｅ、Ｂｗｅ、Ｃｗｅと、３つの姿勢データαｗｅ、βｗ
ｅ、γｗｅとを用いて、次の数３で表すことができる。

【数３】 ［Ｅｗ］＝Trans(Ａwe,Ｂwe,Ｃwe)・Rot(Ｘ,αwe)・Rot(Ｙ,βwe)・Rot(Ｚ,γwe)

【００２９】さらに、絶対座標系Σｗｏｒｌｄを基準と
したツールベース座標系Σｓの位置姿勢を示す同次変換
行列［Ｚｔ］は、３つの位置データＡｔｚ、Ｂｔｚ、Ｃ
ｔｚと、３つの姿勢データαｔｚ、βｔｚ、γｔｚとを
用いて、次の数４で表すことができる。

【数４】 ［Ｚｔ］＝Trans(Ａtz,Ｂtz,Ｃtz)・Rot(Ｘ,αtz)・Rot(Ｙ,βtz)・Rot(Ｚ,γtz)

【００３０】またさらに、絶対座標系Σｗｏｒｌｄを基
準としたワークベース座標系Σｐの位置姿勢を示す同次
変換行列［Ｚｗ］は、３つの位置データＡｗｚ、Ｂｗ
ｚ、Ｃｗｚと、３つの姿勢データαｗｚ、βｗｚ、γｗ
ｚを用いて、次の数５で表すことができる。

【数５】 ［Ｚｗ］＝Trans(Ａwz,Ｂwz,Ｃwz)・Rot(Ｘ,αwz)・Rot(Ｙ,βwz)・Rot(Ｚ,γwz)

【００３１】また、ロボット１、スライダ２及びポジシ
ョナ３などの複数の関節とそれを連結する複数のリンク
を有する機械における各リンクＬｉにおける座標変換
を、公知のディナビット−ハルテンブルグ（Ｄｅｎａｖ
ｉｔ−Ｈａｒｔｅｎｂｅｒｇ）のリンクパラメータ（以
下、Ｄ−Ｈのリンクパラメータという。）の表現による
Ａ行列を用いて、数６のように表すことができる。

【数６】 Ａｉ＝Ｒｏｔ(Ｚ,θｉ)・Ｔｒａｎｓ(ａｉ,０,ｄｉ)・Ｒｏｔ(Ｘ,αｉ)， ここで、ｉ＝０，１，２，…，６である。

【００３２】このＡ行列を用いると、当該ロボットシス
テムのロボット１、スライダ２及びポジショナ３の位置
姿勢を示す同次変換行列は、次の数７乃至数９で表すこ
とができる。

【数７】 ［Ｔｍａ］＝Ａma０・Ａma１・Ａma２・Ａma３・Ａma４・Ａma５・Ａma６

【数８】［Ｔｓ］＝Ａｓ０・Ａｓ１・Ａｓ２・Ａｓ３

【数９】［Ｔｐ］＝Ａｐ０・Ａｐ１・Ａｐ２・Ａｐ３ ここで、Ａｍａ０、Ａｓ０およびＡｐ０は各機械の台座
リンクに相当する。

【００３３】さらに、ワーク座標系Σｗｏｒｋを基準と
したツール座標系Σｔｏｏｌの位置姿勢を示す同次変換
行列［ｗＸｔ］を、上記数２を用いることにより、次の
数１０で定義する。

【数１０】

【００３４】このとき、ロボット１、スライダ２及びポ
ジショナ３並びにツール４で構成されるロボットシステ
ムにおいて設定した各機械構成要素の位置姿勢に対し
て、次の数１１が成立する。

【数１１】 [Ｚｔ]・[Ｔｓ]・[Ｔｍａ]・[Ｅｔ]＝[Ｚｗ]・[Ｔｐ]・[Ｅｗ]・[ｗＸｔ]

【００３５】上記数１１により、ツール４とワークの相
対的な位置姿勢の関係は、同次変換行列［ｗＸｔ］で定
義づけられ、当該同次変換行列［ｗＸｔ］を変更するこ
とにより、具体的には当該同次変換行列［ｗＸｔ］の各
要素を変更することにより、ロボット１、スライダ２及
びポジショナ３を協調動作させることができる。

【００３６】本実施例において、図１のロボットシステ
ムの各機械間の設置関係を設定するために必要な位置姿
勢データを、具体的に以下のように設定する。まず、絶
対座標系Σｗｏｒｌｄの原点Ｏｗｏｒｌｄを基準にした
スライダ２の機械原点Ｏｓの位置姿勢を示す同次変換行
列［Ｚｔ］を演算する位置姿勢データを、次の数１２乃
至数１７に示すように設定する。

【数１２】Ａｔｚ＝０．０

【数１３】Ｂｔｚ＝０．０

【数１４】Ｃｔｚ＝０．０

【数１５】αｔｚ＝αｔｚ

【数１６】βｔｚ＝βｔｚ

【数１７】γｔｚ＝γｔｚ

【００３７】次いで、絶対座標系Σｗｏｒｌｄの原点Ｏ
ｗｏｒｌｄを基準にしたポジショナ３の機械原点Ｏｐの
位置姿勢を示す同次変換行列［Ｚｗ］を演算する位置姿
勢データを、次の数１８乃至数２３に示すように設定す
る。

【数１８】Ａｗｚ＝ａｗｚ

【数１９】Ｂｗｚ＝ｂｗｚ

【数２０】Ｃｗｚ＝ｃｗｚ

【数２１】αｗｚ＝αｗｚ

【数２２】βｗｚ＝βｗｚ

【数２３】γｗｚ＝γｗｚ

【００３８】同様に、同次変換行列［Ｅｔ］及び［Ｅ
ｗ］を演算する位置姿勢データを、次の数２４乃至数３
５に示すように設定する。

【数２４】Ａｔｅ＝０．０

【数２５】Ｂｔｅ＝０．０

【数２６】Ｃｔｅ＝Ｃｔｅ

【数２７】αｔｅ＝０．０

【数２８】βｔｅ＝βｔｅ

【数２９】γｔｅ＝０．０

【数３０】Ａｗｅ＝０．０

【数３１】Ｂｗｅ＝０．０

【数３２】Ｃｗｅ＝０．０

【数３３】αｗｅ＝０．０

【数３４】βｗｅ＝０．０

【数３５】γｗｅ＝０．０

【００３９】また、ロボット１、スライダ２及びポジシ
ョナ３の位置姿勢を演算するために、次の表１から表３
にそれぞれ示すロボット１、スカイダ２及びポシショナ
３のＤ−Ｈのリンクパラメータを用いる。ここで、例え
ばロボット１においては、関節変数θｍａｉは第ｎ番目
のリンクＬｉの回転角度を示し、共通法線距離ａｍａｉ
は第ｉ番目のリンクＬｉの長さに対応し、リンク間距離
ｄｍａｉは第ｉ−１番目のリンクＬｉ−１と第ｉ番目の
リンクＬｉとの間の距離に対応し、ねじれ角度αｍａｉ
は第ｉ−１番目のリンクＬｉ−１と第ｎ番目のリンクＬ
ｉとの間の角度に対応する関節軸間の角度であり、ここ
で、ｉ＝０，１，２，３，…，６である。スライダ２及
びポジショナ３についても同様である。また、表１乃至
表３において、ロボット１の関節変数θｍａｉ（ｉ＝
１，２，…，６）と、スライダ２のリンク間距離である
動作位置ｄｓ１，ｄｓ２と、スライダ２の関節変数θｓ
３と、ポジショナ３の関節変数θｐｋ（ｋ＝１，２）は
それぞれ変数であってＲＡＭ１０２に格納されるが、こ
れ以外のパラメータはＲＯＭ１０１に格納された定数値
である。

【００４０】

【表１】 リンク 関節変数θmai 距離ａmai 距離ｄmai ねじれ角度αmai ────────────────────────────────── Ｌ０ ０．０ ０．０ ｄｍａ０ ０．０ Ｌ１ θｍａ１ ａｍａ１ ｄｍａ１ ＋π／２ Ｌ２ θｍａ２ ａｍａ２ ０．０ ０．０ Ｌ３ θｍａ３ ａｍａ３ ０．０ ＋π／２ Ｌ４ θｍａ４ ０．０ ｄｍａ４ ＋π／２ Ｌ５ θｍａ５ ０．０ ０．０ −π／２ Ｌ６ θｍａ６ ０．０ ｄｍａ６ ０．０

【００４１】

【表２】 リンク 関節変数θｓｊ 距離ａｓｊ 距離ｄｓｊ ねじれ角度αｓｊ ────────────────────────────────── Ｌ１０ ＋π／２ ０．０ ｄｓ０ ＋π／２ Ｌ１１ ＋π／２ ０．０ ｄｓ１ ＋π／２ Ｌ１２ ＋π／２ ０．０ ｄｓ２ ＋π／２ Ｌ１３ θｓ３ ０．０ ０．０ ０．０

【００４２】

【表３】 リンク 関節変数θｐｋ 距離ａｐｋ 距離ｄｐｋ ねじれ角度αｐｋ ────────────────────────────────── Ｌ２０ ０．０ ０．０ ０．０ ０．０ Ｌ２１ θｐ１ ０．０ ｄｐ１ ＋π／２ Ｌ２２ θｐ２ ０．０ ０．０ −π／２ Ｌ２３ ０．０ ０．０ ｄｐ３ ０．０

【００４３】上記各機械１，２，３及び機械１，２，３
間の位置姿勢のデータが与えられると、各座標系間の位
置姿勢の関係を示す同次変換行列の各要素が演算され、
制御装置５内のＲＡＭ１０２（図３参照。）に格納され
る。

【００４４】しかしながら、制御装置５に格納された各
機械１，２，３間の位置姿勢のデータは、しばしば実際
の位置姿勢の数値と同一とならず、図１０乃至図１２に
示すように種々の設置誤差が存在する。そこで、本発明
に係る本実施例においてはこれらの設置誤差を較正する
装置構成を以下に示す。

【００４５】本実施例では、制御装置５が実行する較正
処理は、以下の４つの較正処理からなる。

【００４６】（ａ）第１の較正処理（ステップＳ１，図
３０及び図３１参照。）：絶対座標系Σｗｏｒｌｄとス
ライダ２の原点Ｏｓに設定したツールベース座標系Σｓ
との間の相互の座標系の回転に関する姿勢成分αｔｚ、
βｔｚ、γｔｚを較正する処理であり、設置誤差パラメ
ータは、絶対座標系Σｗｏｒｌｄとツールベース座標系
Σｓの姿勢成分αｔｚ、βｔｚ、γｔｚのねじれ角度△
αｔｚ、△βｔｚ、△γｔｚからなる設置誤差ε３であ
る。

【００４７】（ｂ）第２の較正処理（ステップＳ２，図
３２参照。）：スライダ２の駆動第１軸Ｌ１１方向と、
ロボット１の設置基準面に設けたツール機械座標系Σｍ
との間の交互の座標系の回転に関する姿勢成分θｓ３を
較正する処理であり、誤差パラメータは、スライダ２の
駆動第１軸Ｌ１１方向と、ロボット１の設置基準面に設
けたツール機械座標系Σｍとのねじれ角度△θｓ３から
なる設置誤差ε４である。

【００４８】（ｃ）第３の較正処理（ステップＳ３，図
３３乃至図３６参照。）：絶対座標系Σｗｏｒｌｄと、
ポジショナ３の原点Ｏｐに設定したワークベース座標系
Σｐとの相互の座標系の回転に関する、絶対座標系Σｗ
ｏｒｌｄのＺ軸、Ｙ軸及びＸ軸の姿勢成分γｗｚ，βｗ
ｚ，αｗｚを較正する処理であり、誤差パラメータは、
絶対座標系Σｗｏｒｌｄと、ポジショナ３の原点Ｏｐに
設定したワークベース座標系Σｐとの間の絶対座標系Σ
ｗｏｒｌｄのＺ軸、Ｙ軸及びＸ軸の回りのねじれ角度△
γｗｚ，Δβｗｚ，Δαｗｚからなる設置誤差ε３であ
る。ここで、当該第３の較正処理は、図３３に示すよう
に、角度γｗｚ較正処理（ステップＳ３１，図３４参
照。）と、角度βｗｚ較正処理（ステップＳ３２，図３
５参照。）と、角度αｗｚ較正処理（ステップＳ３３，
図３６参照。）とからなる。

【００４９】（ｄ）第４の較正処理（ステップＳ４，図
３７参照。）：ポジショナ３の出力軸原点Ｏｗｍに設け
たワーク座標系Σｗｏｒｋに関して、制御装置５におけ
る演算上のワーク座標系Σｗｏｒｋの原点Ｏｗｏｒｋの
位置と、実際のワークの原点の位置との差を較正する処
理であり、誤差パラメータは、絶対座標系Σｗｏｒｌｄ
の原点Ｏｗｏｒｌｄからポジショナ３の機械原点Ｏｐま
での位置成分ａｗｚ、ｂｗｚ、ｃｗｚのずれ量△ａｗ
ｚ、△ｂｗｚ、△ｃｗｚである。

【００５０】従って、上述した４つの設置誤差を図示す
ると図５のようになる。すなわち、設置誤差ε１とε２
は絶対座標系Σｗｏｒｌｄとポシショナ３との間の設置
誤差であって、さらに、設置誤差ε３は絶対座標系Σｗ
ｏｒｌｄとスライダ２との設置誤差であり、設置誤差ε
４はロボット１とスライダ２との間の設置誤差である。 （２）産業用ロボットシステムとその設置誤差較正制御
装置の構成 図３に制御装置５の構成を示す。

【００５１】図３に示すように、制御装置５は、（ａ）
クロック発生器１１０によって発生されるクロック信号
と当該クロック信号を所定の分周比で分周する分周器１
１１によって発生される同期信号ＳＹＮＣに基づいてＲ
ＯＭ１０１に格納されたシステムプログラムに従って、
ロボット１とスライダ２とポジショナ３とを制御するＣ
ＰＵ１００と、（ｂ）ロボット１とスライダ２とポジシ
ョナ３の各動作を制御するためのシステムプログラム
と、当該システムプログラムを実行するために必要なデ
ータを格納するＲＯＭ１０１と、（ｃ）ＣＰＵ１００の
ワーキングエリアとして用いられるとともに、作業プロ
グラム生成処理において生成された作業プログラムを格
納するためのＲＡＭ１０２と、（ｄ）ティーチングボッ
クス２０と操作ボックス２１とが接続されたインターフ
ェース１０３と、（ｅ）各モータＭＯ１乃至ＭＯ１０に
対する駆動制御値であるモータ指令位置ＤＰｋ（ｋ＝
１，２，…，１０）のデータを格納するデュアルポート
ＲＡＭ１０４とを備え、これらの各回路１００乃至１０
３とデュアルポートＲＡＭ１０４の第１のポートがバス
１０９を介して接続される。なお、ティーチングボック
ス２０と操作ボックス２１の各操作スイッチについての
処理はＣＰＵ１００の割り込み処理によって実行され
る。

【００５２】デュアルポートＲＡＭ１０４の第２のポー
トはラッチ回路１０５に接続され、ラッチ回路１０５は
同期信号ＳＹＮＣに同期して、デュアルポートＲＡＭ１
０４に格納された各モータＭＯ１乃至ＭＯ１０に対する
１０個のモータ指令位置ＤＰｋのデータを同時にラッチ
した後、以下の３つの動作を同時に実行する。 （ａ）ロボット１の関節制御に関するモータ指令位置Ｄ
Ｐ１乃至ＤＰ６のデータをサーボ制御回路１０６を介し
てモータＭＯ１乃至ＭＯ６に出力して駆動する。 （ｂ）スライダ２の関節制御に関するモータ指令位置Ｄ
Ｐ９乃至ＤＰ１０のデータをサーボ制御回路１０８を介
してモータＭＯ９乃至ＭＯ１０に出力して駆動する。 （ｃ）ポジショナ３の関節制御に関するモータ指令位置
ＤＰ７乃至ＤＰ８のデータをサーボ制御回路１０７を介
してモータＭＯ７乃至ＭＯ８に出力して駆動する。ここ
で、同期信号ＳＹＮＣの発生周期は、教示作業における
モータＭＯ１乃至ＭＯ１０の駆動周期と、再生動作処理
における各補間点毎にモータＭＯ１乃至ＭＯ１０を駆動
する周期に対応している。当該発生周期は、制御装置２
において教示作業時及び再生動作時における各補間毎に
要する処理、すなわち演算に要する時間に、若干の余裕
値を加算した値とする。また、当該発生周期は短い方が
好ましいが、このためには処理時間の速いＣＰＵによる
処理が必要となり、高価となる。実用上は、好ましくは
１０乃至７０ｍｓｅｃに設定される。

【００５３】なお、ＣＰＵ１００は、後述のように計算
される関節変数を減速比λｋで除算することによってモ
ータ指令位置ＤＰｋのデータを計算し、デュアルポート
ＲＡＭ１０４に書き込み、ＣＰＵ１００は、計算されて
現在位置する関節変数の値と、上述した各座標系間の関
係を示す同次変換行列の各要素の値をＲＡＭ１０２に格
納している。

【００５４】図４に、ロボット１とポジショナ３の各関
節ＲＪｋの構造とサーボ制御回路１０６，１０７のブロ
ック図を示す。図４に示すように、モータＭＯｋの回転
軸は減速機ＲＥｋの入力軸に連結され、減速機ＲＥｋは
モータＭＯｋの回転速度を所定の減速比λｋだけ減速す
ることによって、モータＭＯｋによって発生されるトル
クを上記減速比の逆数倍だけ増大させて、減速機ＲＥｋ
の出力軸に連結されたリンクＬｋに出力する。ここで、
モータＭＯｋの回転軸にはセンサＳＮｋの軸が連結さ
れ、当該センサＳＮｋからの出力信号がサーボ制御回路
１０６，１０７内の加算器１２０の減算入力端子に入力
される。一方、ラッチ回路１０５からのモータ指令位置
ＤＰｋのデータが加算器１２０の加算入力端子に入力さ
れる。加算器１２０から出力されるデータは、Ｄ／Ａ変
換器（図示せず。）によってアナログ電圧信号に変換さ
れた後、所定の増幅度を有する増幅器１２１を介してモ
ータＭＯｋの駆動端子に印加される。以上のように構成
されたサーボ制御のフィードバック系においては、セン
サＳＮｋからの出力信号に基づいて、１段前の関節のＸ
軸Ｘｋ−１に平行であって関節軸Ｚｋ−１から延在する
軸Ｘｋ−２’に対応しかつモータＭＯｋの回転軸から延
在する軸Ｘｋ−２’’からの回転角度がモータ指令位置
ＤＰｋとなるように制御され、これによって、リンクＬ
ｋが軸Ｘｋ−２’から関節変数θｋだけ回転される。な
お、スライダ２の関節の構造とそのためのサーボ制御回
路１０８は、２つの駆動軸Ｌ１１，Ｌ１２で並進移動さ
せることを除いて同様に構成される。

【００５５】（３）設置誤差較正制御装置の処理 （３−１）メインルーチン 図２６に当該制御装置５によって実行される設置誤差較
正処理のメインルーチンのフローチャートを示す。

【００５６】図２６において、まず、ステップＳ１にお
いて設置誤差ε３を較正する第１の較正処理を実行し、
次いで、ステップＳ２において設置誤差ε４を較正する
第２の較正処理を実行する。さらに、ステップＳ３にお
いて設置誤差ε２を較正する第３の較正処理を実行し、
次いで、ステップＳ４において設置誤差ε１を較正する
第４の較正処理を実行する。

【００５７】（３−２）第１の較正処理 図３０及び図３１は、図２６及び図２８の第１の較正処
理（ステップＳ１）を示すフローチャートである。

【００５８】当該第１の較正処理においては、絶対座標
系Σｗｏｒｌｄを基準にしたスライダ２の機械原点Ｏｓ
に設定したツールベース座標系Σｓの姿勢成分αｔｚ、
βｔｚ、γｔｚを較正対象とし、これらのパラメータ
は、制御装置５のＲＡＭ１０２に格納している。しかし
ながら、実際の当該ロボットシステムにおいては、前記
格納したパラメータ値に対して、図１０乃至図１２に示
すように、設置誤差ε３であるねじれ角度△αｔｚ、△
βｔｚ、△γｔｚを有する。当該第１の較正処理におい
ては、制御装置５内で格納する位置姿勢データと実際の
ロボットシステムの位置姿勢データとの相違であるねじ
れ角度△αｔｚ、△βｔｚ、△γｔｚを検出し、制御装
置５内の位置姿勢データを実際のロボットシステムの位
置姿勢データに較正する。

【００５９】まず、図１３と図１４に示すように、直方
体のブロックＢＫ１の各表面が絶対座標系Σｗｏｒｌｄ
の各軸方向に対して平行となるような予め決められた位
置に、表面が平担となるように機械加工されている直方
体のブロックＢＫ１を載置する。また、Ｚｗｏｒｌｄ軸
に対して平行なブロックＢＫ１の上面との距離を測定す
る距離センサＳＳ１と、Ｘｗｏｒｌｄ軸に対して平行な
ブロックＢＫ２の側面との距離を測定する距離センサＳ
Ｓ２とを、距離センサＳＳ１，ＳＳ２のセンサ面が直交
しかつ距離センサＳＳ１のセンサ軸はブロックＢＫ１の
上面に対して垂直となり距離センサＳＳ２のセンサ軸は
ブロックＢＫ１の側面に対して垂直になるように、これ
ら距離センサＳＳ１，ＳＳ２を把持する治具Ｊ１を介し
てロボット１のツール４に操作者が取り付ける。この準
備作業が完了したことを、操作者がティーチングボック
ス２０を用いてＣＰＵ１００に知らしめることにより、
制御装置５はスライダ５がポジションＡ１にあるものと
して、以下の第１の較正処理を実行する。

【００６０】ステップＳ１１０において、制御装置５の
ＲＡＭ１０２に記憶した、現在のポジションＡ１でのス
ライダ２の動作位置ｄｓ１をＣＰＵ１００内に取り込
む。以下、各処理において「データを取り込む」とは
「データをＣＰＵ１００内に取り込む」ことを意味す
る。次いで、ステップＳ１１２において、制御装置５の
ＲＡＭ１０２に格納されている、絶対座標系Σｗｏｒｌ
ｄを基準としたツールベース座標系Σｓの位置姿勢を示
す同次変換行列［Ｚｔ］を取り込む。さらに、ステップ
Ｓ１１６において、図１３及び図１４に示すスライダ２
がポジションＡ１に位置するときの距離センサＳＳ１，
ＳＳ２が検出したセンサ検出距離ｌｘ１、ｌｚ１を図３
に示すインターフェイス１０３を介して取り込む。ここ
で、各センサ検出距離ｌｘ１とｌｚ１はそれぞれ、絶対
座標系ΣｗｏｒｌｄのＸ軸方向とＺ軸方向の各距離であ
る。

【００６１】次いで、ステップＳ１１８において、ロボ
ット１が載置されたスライダ２をその第１軸Ｌ１１に沿
って予め決められた距離ＤＬ１だけ移動させて、スライ
ダ２をポシションＢ１に位置させて固定させる。当該処
理において、スライダ２上のロボット１は動作させな
い。そして、ステップＳ１１９において、図１３及び図
１４に示すように、スライダ２がポジションＢ１に位置
するときに、ステップＳ１１６と同様に、距離センサＳ
Ｓ１，ＳＳ２がそれぞれ検出したセンサ検出距離をｌｚ
２、ｌｘ２を取り込む。さらに、ステップＳ１２０にお
いて、ステップＳ１１６，Ｓ１１９において得られたセ
ンサ検出距離ｌｘ１、ｌｘ２及びｌｚ１、ｌｚ２と、上
記移動距離ＤＬ１とに基づいて、次の数３６，数３７を
用いて、絶対座標系Σｗｏｒｌｄを基準としたツールベ
ース座標系Σｓの姿勢成分αｅ及びγｅを演算する。

【００６２】

【数３６】

【数３７】

【００６３】さらに、ステップＳ１２１において、ステ
ップＳ１２０で演算された絶対座標系Σｗｏｒｌｄを基
準としたツールベース座標系ΣｓのＸ軸回りの姿勢成分
αｅが、ＲＡＭ１０２に格納されているαｔｚに等しい
か否かが判断され、次いで、ステップＳ１２２におい
て、ステップＳ１２０で演算された絶対座標系Σｗｏｒ
ｌｄを基準としたツールベース座標系ΣｓのＺ軸回りの
姿勢成分γｅが、ＲＡＭ１０２に格納されているγｔｚ
に等しいか否かが判断される。ここで、もしαｅ＝αｔ
ｚでありかつγｅ＝γｔｚであるとき、ステップＳ１３
０乃至Ｓ１３２の較正処理を行う必要がないと判断して
図３１の処理に進む。一方、もし、αｅ≠αｔｚであり
かつγｅ≠γｔｚであるときステップＳ１３０に進む。

【００６４】ステップＳ１３０において、ステップＳ１
２０で演算した絶対座標系Σｗｏｒｌｄを基準としたツ
ールベース座標系Σｓの姿勢成分αｅ、γｅに基づい
て、次の数３８と数３９とを用いて、制御装置５のＲＡ
Ｍ１０２に格納している姿勢成分αｔｚ、γｔｚに対す
る、ねじれ角度△αｔｚ、△γｔｚを演算する。

【数３８】△αｔｚ＝αｅ−αｔｚ

【数３９】△γｔｚ＝γｅ−γｔｚ

【００６５】次いで、ステップＳ１３１において、ステ
ップＳ１３０において演算された、ねじれ角度△αｔ
ｚ、△γｔｚを補正値として用いて、ロボット制御装置
のＲＡＭに格納している姿勢成分αｔｚ、γｔｚに対し
て補正する。すなわち、ねじれ角度△αｔｚ、△γｔｚ
を用いて較正された後の、絶対座標系Σｗｏｒｌｄを基
準としたツールベース座標系Σｓの姿勢成分の較正値α
ｔｚ’、γｔｚ’は次の数４０と数４１のように表わさ
れる。

【数４０】αｔｚ’＝αｔｚ＋△αｔｚ

【数４１】γｔｚ’＝γｔｚ＋△γｔｚ

【００６６】次いで、上記演算された較正値αｔｚ’、
γｔｚ’に対する、絶対座標系Σｗｏｒｌｄを基準とし
たツールベース座標系Σｓの位置姿勢を示す較正値の同
次変換行列［Ｚｔ’］を、数４２を用いて演算する。

【数４２】 [Ｚｔ']＝Trans(ａtz，ｂtz，ｃtz)・Rot(X,αtz')・Rot(Y,βtz)・Rot(Z,γtz')

【００６７】そして、ステップＳ１３２において、上記
演算された較正値の同次変換行列［Ｚｔ’］を、同次変
換行列［Ｚｔ］としてＲＡＭ１０２に格納され、これに
よって較正が完了したので、絶対座標系Σｗｏｒｌｄを
基準としたツールベース座標系Σｓの姿勢成分αｔｚ、
βｔｚ、γｔｚのうち姿勢成分αｔｚ、γｔｚを較正す
ることができ、残りの姿勢成分βｔｚを較正するため
に、図３１の以下の処理を行う。

【００６８】図１５及び図１６に示すように、ロボット
１を所定のポジションＡ２に位置させ、Ｚｗｏｒｌｄ軸
に対して平行なブロックＢＫ１の上面との距離を測定す
る距離センサＳＳ１と、Ｙｗｏｒｌｄ軸に対して平行な
ブロックＢＫ２の側面との距離を測定する距離センサＳ
Ｓ２とを、距離センサＳＳ１，ＳＳ２のセンサ面が直交
しかつ距離センサＳＳ１のセンサ軸はブロックＢＫ１の
上面に対して垂直となり距離センサＳＳ２のセンサ軸は
ブロックＢＫ１の側面に対して垂直になるように、これ
ら距離センサＳＳ１，ＳＳ２を把持する治具Ｊ１を介し
てロボット１のツール４に操作者が取り付ける。この準
備作業が完了したことを操作者がティーチングボックス
２０を用いてＣＰＵ１００に知らしめることにより、制
御装置５はスライダ２がポジションＡ２にあるものとし
て以下の処理を実行する。まず、図３１のステップＳ１
５０において、スライダ２の動作位置ｄｓ２を取り込
む。

【００６９】次いで、ステップＳ１５２において、制御
装置５のＲＡＭ１０２に格納されている、絶対座標系Σ
ｗｏｒｌｄを基準としたツールベース座標系Σｓの位置
姿勢を示す同次変換行列［Ｚｔ］を取り込む。さらに、
ステップＳ１５３において、図１５及び図１６に示すよ
うに、ロボット１が当該ポジションＡ２に位置するとき
の距離センサＳＳ１，ＳＳ２がそれぞれ検出したセンサ
検出距離ｌｚ１、ｌｙ１を取り込む。ここで、各センサ
検出距離ｌｙ１とｌｚ１はそれぞれ、絶対座標系Σｗｏ
ｒｌｄのＹ軸方向とＺ軸方向の各距離である。

【００７０】次いで、ステップＳ１５５においてロボッ
ト１が載置されたスライダ２をその第２軸Ｌ１２に沿っ
て予め決められた距離ＤＬ２だけ移動させて、スライダ
２をポジションＢ２に位置させて固定させる。当該処理
において、スライダ２上のロボット１は動作させない。
そして、ステップＳ１５６において、図１５及び図１６
に示すように、スライダ２がポジションＢ２に位置する
ときに、ステップＳ１５３と同様に、距離センサＳＳ
１，ＳＳ２がそれぞれ検出したセンサ検出距離をｌｚ
２、ｌｙ２を取り込む。さらに、ステップＳ１５７にお
いて、ステップＳ１５３，Ｓ１５６において得られたセ
ンサ検出距離ｌｙ１、ｌｙ２及びｌｚ１、ｌｚ２と、上
記移動距離ＤＬ２とに基づいて、次の数４３を用いて、
絶対座標系Σｗｏｒｌｄを基準としたツールベース座標
系Σｓの姿勢成分βｅを演算する。

【００７１】

【数４３】

【００７２】さらに、ステップＳ１５８において、ステ
ップＳ１５７で演算された絶対座標系Σｗｏｒｌｄを基
準としたツールベース座標系ΣｓのＹ軸回りの姿勢成分
βｅが、ＲＡＭ１０２に格納されているβｔｚに等しい
か否かが判断される。ここで、もしβｅ＝βｔｚである
とき、第１の較正処理が終了したとしてリターンする。
一方、もし、βｅ≠βｔｚであるときステップＳ１６０
に進む。

【００７３】ステップＳ１６０において、ステップＳ１
５７で演算した絶対座標系Σｗｏｒｌｄを基準としたツ
ールベース座標系Σｓの姿勢成分βｅに基づいて、次の
数４４を用いて、制御装置５のＲＡＭ１０２に格納して
いる姿勢成分βｔｚに対する、ねじれ角度△βｔｚを演
算する。

【数４４】△βｔｚ＝βｅ−βｔｚ

【００７４】次いで、ステップＳ１６１において、ステ
ップＳ１５７において演算された、ねじれ角度△βｔｚ
を補正値として用いて、ロボット制御装置のＲＡＭに格
納している姿勢成分βｔｚに対して補正する。すなわ
ち、ねじれ角度△βｔｚを用いて較正された後の、絶対
座標系Σｗｏｒｌｄを基準としたツールベース座標系Σ
ｓの姿勢成分の較正値βｔｚ’は次の数４５のように表
される。

【数４５】βｔｚ’＝βｔｚ＋△βｔｚ

【００７５】次いで、上記演算された較正値βｔｚ’に
対する、絶対座標系Σｗｏｒｌｄを基準としたツールベ
ース座標系Σｓの位置姿勢を示す較正値の同次変換行列
［Ｚｔ’’］を、次の数４６を用いて演算する。

【数４６】 [Ｚｔ'']＝Trans(ａtz，ｂtz，ｃtz)・Rot(X,αtz)・Rot(Y,βtz')・Rot(Z,γtz)

【００７６】そして、ステップＳ１６２において、上記
演算された較正値の同次変換行列［Ｚｔ’’］を、同次
変換行列［Ｚｔ］として、ＲＡＭ１０２に格納される。
これによって、第１の較正処理が完了したとしてリター
ンする。以上の図３０と図３１の第１の較正処理によっ
て、絶対座標系Σｗｏｒｌｄを基準としたツールベース
座標系Σｓのすべての姿勢成分αｔｚ、βｔｚ、γｔｚ
を較正することができる。

【００７７】（３−３）第２の較正処理 図３２に第２の較正処理のフロチャートを示す。

【００７８】第２の較正処理は、図１０乃至図１２に示
すように、スライダ２の第２軸Ｌ１２方向と、ロボット
１の設置基準面に設けたツール機械座標系ΣＯｍとの間
の姿勢成分θｓ３の設置誤差であるねじれ角度Δθｓ３
を検出して、姿勢成分θｓ３を較正する。

【００７９】まず、図１８及び図１９に示すように、ツ
ール４の先端基準点Ｏｔｏｏｌまでの距離ｌを測定する
ために、ツール４の先端に表面が平担となるように機械
加工されている直方体のブロックＢＫ１を取り付け、直
線距離を計測する距離センサＳＳ２をそのセンサ面がス
ライダ２の第２軸Ｌ１２の基準面Ｌ１Ｒに対して平行と
なるように、例えば台上に固定する。従って、ここで、
固定した距離センサＳＳ２のセンサ軸は、ブロックＢＫ
１の側面及びスライダ２の第２軸Ｌ１２の基準面Ｌ１Ｒ
に対して垂直であり、距離センサＳＳ２はセンサＳＳ２
のセンサ面からツール４の先端基準点Ｏｔｏｏｌまでの
距離ｌを測定する。

【００８０】ところで、図４に示すように、ロボット
１、スライダ２及びポジショナ３の各関節は、モータと
減速機とを備えて回転可能に構成されている。従って、
ロボット１の各関節ＲＪ１乃至ＲＪ６において、（ａ）
関節番号をｉ（ｉ＝１，２，３，…，６）とし、（ｂ）
モータ指令位置をＰｍａｉ［パルス］とし、（ｃ）モー
タの１回転当たりの指令位置増減量をＮｍａｉ［パルス
／回転］（ｄ）減速機の減速比をλｍａｉとし、（ｅ）
ロボット１の関節変数をθｍａｉ［ｒａｄ］とすると、
次の数４７が成立する。

【００８１】

【数４７】 θｍａｉ＝Ｐｍａｉ×（２π／Ｎｍａｉ）×λｍａｉ，
ｉ＝１，２，…，６

【００８２】また、スライダ２の第１および第２の移動
軸において、（ａ）軸番号をｊ（ｊ＝１、２）とし、
（ｂ）モータ指令位置をＰｓｊ［パルス］とし、（ｃ）
モータの１回転当たりのスライダ移動量をＤｓｊ［ｍ
ｍ］とし、（ｄ）モータの１回転当たりの指令位置増減
量をＮｓｊ［パルス／回転］とし、（ｅ）スライダ２の
動作位置（関節変数に対応する。）をｄｓｊ［ｍｍ］と
すると、次の数４８が成立する。

【００８３】

【数４８】ｄｓｊ＝Ｐｓｊ×（Ｄｓｊ／Ｎｓｊ），ｊ＝
１，２

【００８４】さらに、ポジショナ３の１軸及び２軸の関
節ＲＪ２１，ＲＪ２２において、（ａ）関節番号をｋ
（ｋ＝１、２）とし、（ｂ）モータ指令位置をＰｐｋ
［ｐｕｌｓｅ］とし、（ｃ）モータの１回転当たりの指
令位置増減量をＮｐｋ［パルス／回転］とし、（ｄ）減
速機の減速比をλｐｋとし、（ｅ）ポジショナ３の関節
変数をθｐｋ［ｒａｄ］とすると、次の数４９が成立す
る。

【００８５】

【数４９】 θｐｋ＝Ｐｐｋ×（２π／Ｎｐｋ）×λｐｋ，ｋ＝１，
２

【００８６】当該第２の較正処理のステップＳ２１０に
おいては、図３におけるデュアルポートＲＡＭ１０４か
ら各機械１，２，３のモータ指令位置Ｐｍａｉ，Ｐｓ
ｊ，Ｐｐｋを読み出す。

【００８７】次いで、数４７乃至数４９を用いて、ロボ
ット１を図１８と図１９に示すように位置させて固定し
たときのポジションＡ３でのロボット１、スライダ２及
びポジショナ３の関節変数θｍａｉ，θｐｋと動作位置
ｄｓｊを計算する。

【００８８】次いで、ステップＳ２１１において、ステ
ップＳ２１０で演算された、ロボット１の関節変数θｍ
ａｉ（ｉ＝１，２，…，６）、スライダ２の動作位置ｄ
ｓｊ（ｊ＝１，２）、及びポジショナ３の関節変数θｐ
ｋ（ｋ＝１，２）に基づいて、図１７のポジションＡ３
におけるロボット１の位置姿勢を示す同次変換行列［Ｔ
ｍａ］と、スライダ２の位置姿勢を示す同次変換行列
［Ｔｓ］と、ポジショナ３の位置姿勢を示す同次変換行
列［Ｔｐ］とをそれぞれ、以下のように演算してＲＡＭ
１０２に格納する。

【００８９】まず、上記演算されたロボット１の関節変
数θｍａｉ（ｉ＝１，２，…，６）の値を、数６に代入
することにより、ロボット１のＡ行列Ａｍａｉを次の数
５０を用いて演算する。

【数５０】 Ａmai＝Ｒｏｔ(Ｚ,θmai)・Ｔｒａｎｓ(ａmai,０,ｄmai)・Ｒｏｔ(Ｘ,αmai)， ここで、ｉ＝０，１，２，…，６である。

【００９０】なお、Ａｍａ０は上記表１のリンクＬ０の
パラメータ値に基づいて演算される。上記演算されたこ
のＡ行列Ａｍａｉ（ｉ＝０，１，２，…，６）を数７に
代入することにより、ロボット１の位置姿勢を示す同次
変換行列［Ｔｍａ］を演算する。

【００９１】また、スライダ２のＡ行列Ａｓｊは、数６
にスライダ２の動作位置ｄｓｊ（ｊ＝１，２）を代入す
ることにより次式で表すことができ、上記演算されたス
ライダ２の動作位置ｄｓｊ（ｊ＝１，２）の値を、数５
１に代入することにより、当該Ａ行列の各要素を演算す
る。

【数５１】 Ａsj＝Ｒｏｔ(Ｚ,θsj)・Ｔｒａｎｓ(ａsj,０,ｄsj)・Ｒｏｔ(Ｘ,αsj) ここで、ｊ＝１，２である。

【００９２】なお、Ａｓ０，Ａｓ３はそれぞれ上記表２
のリンクＬ１０，Ｌ１３のパラメータ値に基づいて演算
される。上記演算されたこのＡ行列Ａｓｊ（ｊ＝０，
１，２，３）を数８に代入することにより、スライダ２
の位置姿勢を示す同次変換行列［Ｔｓ］を演算する。

【００９３】さらに、ポジショナ３のＡ行列Ａｐｋは、
数６にポジショナ３の関節変数θｐｋ（ｋ＝１，２）を
代入することにより次式で表すことができ、上記演算さ
れたポジショナ３の動作位置θｐｋ（ｋ＝１，２）の値
を、数５２に代入することにより、当該Ａ行列の各要素
を演算する。

【数５２】 Ａｐｋ＝Ｒｏｔ(Ｚ,θｐｋ)・Ｔｒａｎｓ(０，０，０)・Ｒｏｔ(Ｘ,αｐｋ) ここで、ｋ＝１，２である。

【００９４】なお、Ａｐ０，Ａｐ３はそれぞれ上記表３
のリンクＬ２０，Ｌ２３のパラメータ値に基づいて演算
される。上記演算されたこのＡ行列Ａｐｋ（ｋ＝０，
１，２，３）を数９に代入することにより、ポジショナ
３の位置姿勢を示す同次変換行列［Ｔｐ］を演算する。

【００９５】上述のように演算されたポジションＡ３で
のロボット１とスライダ２とポジショナ３の位置姿勢を
示す同次変換行列［Ｔｍａ］，［Ｔｓ］，［Ｔｐ］を演
算してＲＡＭ１０２に格納する。

【００９６】次いで、ステップＳ２１３において、ステ
ップ２が上記ポジションＡ３に固定されたときの距離セ
ンサＳＳ２と、ツール４の先端Ｏｔｏｏｌに取り付けた
ブロックＢＫ１とのセンサ検出距離ｌを距離ｌ１とし
て、図３に示すインターフェイス１０３を介してＲＡＭ
１０２に取り込む。さらに、ステップＳ２１４におい
て、第１の較正処理によって較正されＲＡＭ１０２に同
次変換行列［Ｚｔ］として格納されている同次変換行列
［Ｚｔ’’］と、ＲＡＭ１０２に格納されている［Ｚ
ｗ］、［Ｅｔ］、［Ｅｗ］を取り込んだ後に、ステップ
Ｓ２１１において演算されたポジションＡ３におけるロ
ボット１、スライダ２及びポジショナ３の位置姿勢を示
す同次変換行列［Ｔｍａ］、［Ｔｓ］、［Ｔｐ］に基づ
いて、ワーク座標系Σｗｏｒｋを基準としたツール４の
先端Ｏｔｏｏｌの位置姿勢を示す同次変換行列［ｗＸ
ｔ］を演算する。

【００９７】

【数５３】 [ｗＸｔ]＝[Ｅｗ]^-1・[Ｔｐ]^-1・[Ｚｗ]^-1・[Ｚｔ'']・[Ｔｓ]・[Ｔｍａ]・[Ｅｔ]

【００９８】なお、上記数５３において、同次変換行列
［Ｚｗ］^-1、［Ｔｐ］^-1、［Ｅｗ］ ^-1はそれぞれ、ＲＡ
Ｍ１０２に格納されている同次変換行列［Ｚｗ］、［Ｔ
ｐ］、［Ｅｗ］をそれぞれ行列の逆変換を実行すること
により演算される。

【００９９】次いで、ステップＳ２１６において、以下
の処理を実行する。すなわち、ステップＳ２１０で演算
されたポジションＡ３でのスライダ２の第２軸Ｌ１２の
動作位置ｄｓ２と、予め決められたスライダ２の第２軸
Ｌ１２の移動量ＤＬ３に基づいて、次の数５４を用い
て、次のポジションＢ３でのスライダ２の動作位置ｄｓ
２’を演算する。

【０１００】

【数５４】ｄｓ２’＝ＤＬ３＋ｄｓ２

【０１０１】次いで、ポジションＢ３でのスライダ２の
第２軸Ｌ１２の動作位置ｄｓ２’を動作位置ｄｓ２とし
て、動作位置ｄｓ２におけるスライダ２の位置姿勢［Ｔ
ｓ’］を上記数８を用いて演算する。上記演算したポジ
ションＡ３でのワーク座標系Σｗｏｒｋを基準としたツ
ール４先端Ｏｔｏｏｌの位置姿勢を示す同次変換行列
［ｗＸｔ］を一定としかつポシショナ３を動作させずそ
の位置姿勢を示す同次変換行列［Ｔｐ］を一定とする条
件のもとで、スライダ２の第２軸Ｌ１２に沿って所定の
距離ＤＬ３だけ移動させたときのポジションＢ３のロボ
ット１の位置姿勢を示す同次変換行列［Ｔｍａ’］を次
の数５５を用いて演算して同次変換行列［Ｔｍａ’］を
演算する。

【０１０２】

【数５５】 [Ｔｍａ']＝[Ｔｓ']^-1・[Ｚｔ'']^-1・[Ｚｗ]・[Ｔｐ]・[Ｅｗ]・[ｗＸｔ]・[Ｅｔ]^-1 ここで、右辺の各同次変換行列は当該ステップＳ２１６
及び上記ステップＳ２１４において演算された同次変換
行列である。

【０１０３】次いで、ポジションＢ３でのロボット１の
位置姿勢を示す同次変換行列［Ｔｍａ’］を逆変換し
て、ロボット１の各関節変数θｍａｉ（ｉ＝１，２，
…，６）を演算する。さらに、ポジションＢ３における
ロボット１のモータ指令位置Ｐｍａｉ（ｉ＝１，２，
…，６）を、上記数４７を変形することにより得られた
次の数５６を用いて演算するとともに、スライダ２のモ
ータ指令位置Ｐｓｊ（ｊ＝１，２）を、上記数４８を変
形することにより得られた数５７を用いて演算する。

【０１０４】

【数５６】 Ｐｍａｉ＝θｍａｉ×（Ｎｍａｉ／２π）×（１／λｍａｉ）， ｉ＝１，２，…，６

【数５７】Ｐｓｊ＝ｄｓｊ×（Ｎｓｊ／Ｄｓｊ），ｊ＝
１，２

【０１０５】次いで、ステップＳ２１９において、ポジ
ションＢ３におけるロボット１及びスライダ２のモータ
指令位置Ｐｍａｉ（ｉ＝１，２，…，６），Ｐｓｊ（ｊ
＝１，２）のデータを、図３に示すようにデュアルポー
トＲＡＭ１０４、ラッチ回路１０５及びサーボ制御回路
１０６，１０８を介してモータＭＯ１乃至ＭＯ６，ＭＯ
９，ＭＯ１０に転送して、ロボット１の各関節ＲＪ１乃
至ＲＪ６を動作させるとともに、スライダ２をその第２
軸Ｌ１２に沿って距離ＤＬ３だけ移動させ図１７に示す
ポジションＢ３に移動させる。そして、ポジションＢ３
における距離センサＳＳ２とツール４の先端Ｏｔｏｏｌ
に取り付けたブロックＢＫ１とのセンサ検出距離ｌ２を
インターフェイス１０３を介してＲＡＭ１０２に取り込
む。

【０１０６】次いで、ステップＳ２２１において、ステ
ップＳ２１３において検出したポジションＡ３でのセン
サ検出距離ｌ１と、ステップＳ２２０において検出した
ポジションＢ３でのセンサ検出距離ｌ２とが等しいか否
かを判断し、もしｌ１＝ｌ２のとき、設置誤差であるね
じれ角度Δθｓ３は０となり、当該第２の較正処理は完
了していると判断して、リターンする。一方、もしｌ１
≠ｌ２のときはステップＳＳ２２２に進む。

【０１０７】ステップＳ２２２においては、スライダ２
の第２軸Ｌ１２の方向と、ロボット１の設置基準面に設
けたツール機械座標系Σｍとの間のねじれ角度△θｓ３
によるツール４の先端Ｏｔｏｏｌのずれ量△ＤＬ３を、
次の数５８を用いて演算する。

【数５８】△ＤＬ３＝ｌ２−ｌ１

【０１０８】次いで、ステップＳ２２３において、上記
ステップＳ２２２で検出したツール４の先端Ｏｔｏｏｌ
のずれ量△ＤＬ３と上記所定の移動量ＤＬ３とに基づい
て、スライダ２の第２軸Ｌ１２の方向とロボット設置基
準面に設けたツール機械座標系Σｍとのねじれ角度△θ
ｓ３を、次の数５９を用いて演算する。

【数５９】△θｓ３＝ｔａｎ^-1（△ＤＬ３／ＤＬ３）

【０１０９】そして、上記演算されたねじれ角度△θｓ
３を補正値として用いて、表２のスライダ２のＤ−Ｈの
リンクパラメータに記載の第３リンクのＺ軸回りの角度
回転量θｓ３に対して次の数６０を用いて補正して、較
正値θｓ３’を演算して回転量θｓ３としてＲＡＭ１０
２に格納する。

【数６０】θｓ３’＝θｓ３＋△θｓ３

【０１１０】以上の処理によって、第２の較正処理が完
了したとしてリターンする。

【０１１１】（３−４）第３の較正処理 第３の較正処理は、絶対座標系Σｗｏｒｌｄとポジショ
ナ３の原点Ｏｐに設定したワークベース座標系Σｐとの
間の絶対座標系ΣｗｏｒｌｄのＸ軸、Ｙ軸及びＺ軸回り
の各姿勢の角度成分αｗｚ，βｗｚ及びγｗｚを較正対
象とし、図３３に示すように、順次実行される （ａ）角度成分γｗｚを較正する角度γｗｚ較正処理
（ステップＳ３１）と、（ｂ）角度成分βｗｚを較正す
る角度βｗｚ較正処理（ステップＳ３２）と、（ｃ）角
度成分αｗｚを較正する角度αｗｚ較正処理（ステップ
Ｓ３３）とから構成される。以下各処理について説明す
る。なお、上記数３０乃至数３５によって、同次変換行
列［Ｅｗ］に関する位置姿勢データはすべて０と設定さ
れている。そこで、ワーク座標系Σｗｏｒｋは、ワーク
側メカニカルインターフェイス座標系Σｗｍと一致して
いるので、ワーク座標系Σｗｏｒｋの原点の位置はポジ
ショナ３のテーブル平面の中心Ｏｗｏｒｋである。

【０１１２】（３−４−１）角度γｗｚ較正処理（ステ
ップＳ３１） 図３４は角度γｗｚ較正処理（ステップＳ３１）を示す
フローチャートであち、当該処理では、設置誤差である
ねじれ角度△γｗｚを検出して角度γｗｚを較正する。

【０１１３】まず、ステップＳ３１０において、予めＲ
ＯＭ１０１に記憶されたポジショナ３の関節変数θｐ１
及びθｐ２の固定値データθｐ１＝−９０°、θｐ２＝
０°を取り込む。次いで、上記ポジショナ３の関節変数
θｐｋ（ｋ＝１，２）に対する、ポジショナ３のモータ
指令位置Ｐｐｋ（ｋ＝１，２）を、数４９を変形するこ
とによって得られた次の数６１を用いて演算した後、当
該演算されたデータをデユアルポートＲＡＭ１０４、ラ
ッチ回路１０５及びサーボ制御回路１０７を介してモー
タＭＯ７，ＭＯ８に転送してポジショナ３を初期設定の
位置に移動動作させる。

【数６１】 Ｐｐｋ＝θｐｋ×Ｎｐｋ／２π×（１／λｐｋ） ここで、ｋ＝１，２である。次いで、操作者が図２０に
示すように、ツール４の先端Ｏｔｏｏｌとポジショナ３
のテーブル平面との間の距離を測定するための距離セン
サＳＳ１をそのセンサ軸がポジショナ３のテーブル平面
に対して垂直となるように操作者が取り付ける。この準
備作業が完了したことを操作者がティーチングボックス
２０を用いてＣＰＵ１００に知らしめることにより、ス
テップＳ３１１における準備完了信号の受信の判断がＹ
ＳＥとなり、制御装置５は各機械１，２，３がポジショ
ンＡ４にあるものとして、以下の処理を実行する。

【０１１４】次いで、ステップＳ３１２においてポジシ
ョナ３の第１軸及び第２軸のＡ行列の値は、上記数６に
上記初期設定において取り込んだθｐ１及びθｐ２を代
入することによって次の数６２で演算される。

【数６２】 Ａｐｋ＝Ｒｏｔ(Ｚ，θｐｋ)・Ｔｒａｎｓ(０，０，０)・Ｒｏｔ(Ｘ，αｐｋ) ここで、ｋ＝１，２である。

【０１１５】さらに、Ａｐ０，Ａｐ３はそれぞれ上記表
３のリンクＬ２０，Ｌ２３のパラメータ値に基づいて演
算される。上記演算されたこのＡ行列Ａｐｋ（ｉ＝０，
１，２，３）を数９に代入することにより、ポジショナ
３の位置姿勢を示す同次変換行列［Ｔｐ］を演算する。

【０１１６】次いで、スライダ２の動作位置ｄｓｊ（ｊ
＝１，２）は図３におけるデュアルポートＲＡＭ１０４
からモータ指令位置Ｐｓｊ（ｊ＝１，２）を読み出し、
数４８によってスライダ２のＡ行列Ａｓｊ（ｊ＝１，
２）を演算する。

【０１１７】さらに、上記数５１を用いてスライダ２の
Ａ行列Ａｓｊ（ｊ＝１，２）を演算し、上述の第２の較
正処理によって既に較正されたスライダ２のＤ−Ｈリン
クパラメータθｓ３を用いてスライダ２のＡ行列Ａｓｊ
（ｊ＝０，３）を演算した後、数８を用いてスライダ２
の位置姿勢を示す同次変換行列［Ｔｓ］を演算する。

【０１１８】次いで、ロボット１の各関節変数θｍａｉ
（ｉ＝１，２，…，６）は図３におけるデュアルポート
ＲＡＭ１０４からモータ指令位置Ｐｍａｉ（ｉ＝１，
２，…，６）を読み出し、数４７を用いて演算する。さ
らに、数５０を用いてロボット１のＡ行列Ａｍａｉ（ｉ
＝０，１，２，…，６）を演算した後、数７を用いてロ
ボット１の位置姿勢を示す同次変換行列［Ｔｍａ］を演
算する。

【０１１９】以上でステップＳ３１２の処理が終了し、
次いで、ステップＳ３１６において、ポジションＡ４に
おける距離センサＳＳ１のセンサ検出距離ｌ１をインタ
ーフェイス１０３を介してＲＡＭ１０２に取り込む。

【０１２０】次いで、ステップＳ３１８において、第１
の較正処理によって較正されかつＲＡＭ１０２に同次変
換行列［Ｚｔ］として格納されている［Ｚｔ’’］と、
ＲＡＭ１０２に格納されている同次変換行列［Ｚｗ］、
［Ｅｔ］及び［Ｅｗ］とを取り込むとともに、上記演算
されたポジショナ３の位置姿勢を示す同次変換行列［Ｔ
ｐ］と、第２の較正処理によって得られた関節変数θｓ
３を用いて得られたスライダ２の位置姿勢を示す同次変
換行列［Ｔｓ］と、ロボット１の位置姿勢を示す同次変
換行列「Ｔｍａ」とに基づいて、次の数６３を用いて、
上記ポジションＡ４におけるワーク座標系Σｗｏｒｋを
基準としたツール４の先端Ｏｔｏｏｌの位置姿勢を示す
同次変換行列［ｗＸｔ］を演算する。

【０１２１】

【数６３】

【０１２２】また、ポジションＡ４のときの、ワーク座
標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏｏｌ
の位置姿勢を示す同次変換行列［ｗＸｔ］に基づいて、
ポジションＡ４におけるツール４の先端Ｏｔｏｏｌの位
置（ｘｔｏｏｌ１，ｙｔｏｏｌ１，ｚｔｏｏｌ１）は、
次の数６４を用いて表され、当該位置の座標値が演算さ
れる。

【数６４】ｘｔｏｏｌ１＝ｔｘ^ｗ ｙｔｏｏｌ１＝ｔｙ^ｗ ｚｔｏｏｌ１＝ｔｚ^ｗ

【０１２３】次いで、ステップＳ３２０において、ツー
ル４の先端Ｏｔｏｏｌを図２０に示すように、上記ポジ
ションＡ４からワーク座標系ΣｗｏｒｋのＹ軸方向に所
定の移動量ＤＬ４だけ移動させてポジションＢ４に位置
させたときのワーク座標系Σｗｏｒｋを基準としたツー
ル４の先端Ｏｔｏｏｌの位置姿勢を示す同次変換行列
［ｗＸｔ’］を以下のように演算する。

【０１２４】まず、上記数６４を用いて演算した上記ポ
ジションＡ４におけるツール４の先端Ｏｔｏｏｌの位置
成分に対して、ワーク座標系ΣｗｏｒｋのＹ軸方向のツ
ール４の先端移動量ＤＬ４を加算して、ポジションＢ４
におけるツール４の先端Ｏｔｏｏｌの位置（ｘｔｏｏｌ
２，ｙｔｏｏｌ２，ｚｔｏｏｌ２）を数６５を用いて演
算する。

【数６５】ｘｔｏｏｌ２＝ｔｘ^ｗ ｙｔｏｏｌ２＝ｔｙ^ｗ＋ＤＬ４ ｚｔｏｏｌ２＝ｔｚ^ｗ

【０１２５】次いで、上記ポジションＡ４におけるワー
ク座標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏ
ｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］の中の３
行×３列の姿勢行列と、上記数６５を用いて演算したポ
ジションＢ４におけるツール４の先端Ｏｔｏｏｌの位置
（ｘｔｏｏｌ２，ｙｔｏｏｌ２，ｚｔｏｏｌ２）に基づ
いて、当該ポジションＢ４におけるワーク座標系Σｗｏ
ｒｌｄを基準としたツール４の先端Ｏｔｏｏｌの位置姿
勢を示す同次変換行列［ｗＸｔ’］は次の数６６で表さ
れる。

【数６６】

【０１２６】ステップＳ３２２において、ポジションＡ
４からポジションＢ４まで移動させるときに、スライダ
２及びポジショナ３は動作させないので、各関節変数ｄ
ｓｊ（ｊ＝１，２）、θｐｋ（ｋ＝１，２）の値は変化
しない。すなわち、上記演算したポジショナ３の位置姿
勢を示す同次変換行列［Ｔｐ］と上記演算したスライダ
２の位置姿勢を示す同次変換行列［Ｔｓ］は変化しな
い。従って、上記ポジションＢ４におけるロボット１の
位置姿勢を示す同次変換行列［Ｔｍａ’］を、上記位置
姿勢を示す各同次変換行列［Ｔｓ］、［Ｔｐ］、［ｗＸ
ｔ’］、並びに第１の較正処理によって較正されかつＲ
ＡＭ１０２に同次変換行列［Ｚｔ］として格納されてい
る同次変換行列［Ｚｔ’’］と、ＲＡＭ１０２に格納さ
れている同次変換行列［Ｚｗ］、［Ｅｔ］、［Ｅｗ］と
に基づいて、次の数６７を用いて演算する。

【数６７】 [Ｔｍａ']＝[Ｔｓ]^-1・[Ｚｔ'']^-1・[Ｚｗ]・[Ｔｐ]・[Ｅｗ]・[ｗＸｔ']・[Ｅｔ]^-1

【０１２７】さらに、上記ポジションＢ４におけるロボ
ット１の位置姿勢を示す同次変換行列［Ｔｍａ’］を逆
変換して、ロボット１の各関節変数θｍａｉ（ｉ＝１，
２，…，６）を演算した後、上記数５６を用いて、ロボ
ット１のモータ指令位置Ｐｍａｉ（ｉ＝１，２，…，
６）を演算する。上記演算されたモータ指令位置Ｐｍａ
ｉのデータは、図３に示すようにデュアルポートＲＡＭ
１０４、ラッチ回路１０５及びサーボ制御回路１０６を
介してモータＭＯ１乃至ＭＯ６に転送され、これによっ
て、ロボット１の各関節を動かし、ツール４の先端Ｏｔ
ｏｏｌがポジションＢ４に位置する。

【０１２８】次いで、ステップＳ３２３において、ポジ
ションＢ４における距離センサＳＳ１のセンサ検出距離
ｌ２をＲＡＭ１０２に取り込む。そして、ステップＳ３
２４においてポジションＡ４におけるセンサ検出距離ｌ
１とポジションＢ４におけるセンサ検出距離ｌ２とが等
しいか否かが判断され、もしｌ１＝ｌ２のとき、当該角
度γｗｚ較正処理が完了したと判断して、リターンす
る。一方、もしｌ１≠ｌ２であるときステップＳ３２５
に進む。

【０１２９】ステップＳ３２５においては、上記検出さ
れたセンサ検出距離ｌ１，ｌ２に基づいて、ポジショナ
３のテーブル平面に対するツール４の先端Ｏｔｏｏｌの
ずれ量△ＤＬ４を、次の数６８を用いて演算する。

【数６８】△ＤＬ４＝ｌ２−ｌ１

【０１３０】次いで、ステップＳ３２６において、上記
演算されたずれ量△ＤＬ４と上記移動量ＤＬ４とに基づ
いて、絶対座標系Σｗｏｒｌｄを基準としたワークベー
ス座標系ΣＯｐのＺ軸回りのねじれ角度△γｗｚを、次
の数６９を用いて演算する。

【数６９】△γｗｚ＝ｓｉｎ^-1（△ＤＬ４／ＤＬ４）

【０１３１】そして、次の数７０に示すように、上記ね
じれ角度△γｗｚを補正値として用いて、ＲＡＭ１０２
に格納している絶対座標系Σｗｏｒｌｄを基準としたワ
ークベース座標系ΣｐのＺ軸回りの姿勢成分γｗｚに対
して補正して、較正値γｗｚ’を演算して姿勢成分γｗ
ｚとしてＲＡＭ１０２に格納する。

【数７０】γｗｚ’＝γｗｚ＋△γｗｚ となる。

【０１３２】次いで、上記姿勢成分の較正値γｗｚ’
を、数５に代入し、較正値γｗｚ’に対する絶対座標系
Σｗｏｒｌｄを基準としたワークベース座標系Σｐの位
置姿勢を示す同次変換行列［Ｚｗ’］を、次の数７１を
用いて演算する。

【数７１】 [Ｚw']＝Trans(ａwz,ｂwz,ｃwz)・Rot(Ｘ,αwz)・Rot(Ｙ,βwz)・Rot(Ｚ,γwz')

【０１３３】さらに、ステップＳ３２９において、上記
演算した位置姿勢を示す同次変換行列［Ｚｗ’］を、
［Ｚｗ］として、ＲＡＭ１０２に格納して更新する。こ
れによって、角度γｗｚに関する較正処理が完了したの
でリターンする。

【０１３４】（３−４−２）角度βｗｚ較正処理（ステ
ップＳ３２） 図３５は角度γｗｚ較正処理（ステップＳ３２）を示す
フローチャートであち、当該処理では、設置誤差である
ねじれ角度△βｗｚを検出して角度βｗｚを較正する。

【０１３５】まず、ステップＳ４１０において、予めＲ
ＯＭ１０１に記憶されたポジショナ３の関節変数θｐ１
及びθｐ２の固定値データθｐ１＝０°、θｐ２＝０°
を取り込む。次いで、上記ポジショナ３の関節変数θｐ
ｋ（ｋ＝１，２）に対する、ポジショナ３のモータ指令
位置Ｐｐｋ（ｋ＝１，２）を、数４９を変形することに
よって得られた数６２を用いて演算した後、当該演算さ
れたデータをデユアルポートＲＡＭ１０４、ラッチ回路
１０５及びサーボ制御回路１０７を介してモータＭＯ
７，ＭＯ８に転送してポジショナ３を初期設定の位置に
移動動作させる。次いで、図２１に示すように、操作者
がツール４の先端Ｏｔｏｏｌとポジショナ３のテーブル
平面との間の距離を測定するための距離センサＳＳ１
を、そのセンサ軸がポジショナ３のテーブル平面に対し
て垂直となるように操作者が取り付ける。この準備作業
が完了したことを、操作者がティーチングボックス２０
を用いてＣＰＵ１００に知らしめることによって、ステ
ップＳ４１１における準備完了信号の受信の判断はＹＥ
Ｓとなり、制御装置５は各機械１，２，３がポジション
Ａ５にあるものとして以下の処理を実行する。

【０１３６】次いで、ステップＳ４１２において、ポジ
ショナ３の第１軸及び第２軸のＡ行列の値は、上記数６
に上記初期設定において取り込んだ関節変数θｐ１及び
θｐ２を代入することによって数６３を用いて演算され
る。

【０１３７】さらに、Ａｐ０，Ａｐ３はそれぞれ上記表
３のリンクＬ２０，Ｌ２３のパラメータ値に基づいて演
算される。上記演算されたこのＡ行列Ａｐｋ（ｉ＝０，
１，２，３）を数９に代入することにより、ポジショナ
３の位置姿勢を示す同次変換行列［Ｔｐ］を演算する。

【０１３８】次いで、スライダ２の動作位置ｄｓｊ（ｊ
＝１，２）は、図３におけるデュアルポートＲＡＭ１０
４からモータ指令位置Ｐｓｊ（ｊ＝１，２）を読み出
し、上記数４８を用いて演算する。

【０１３９】さらに、上記数５１を用いてスライダ２の
Ａｓｊ（ｊ＝１，２）を演算し、第２の較正処理によっ
て既に較正されたスライダ２のＤ−Ｈリンクパラメータ
の関節変数θｓ３を用いてスライダ２のＡ行列Ａｓｊ
（ｊ＝０，３）を演算した後、上記数８を用いてスライ
ダ２の位置姿勢を示す同次変換行列［Ｔｓ］を演算す
る。

【０１４０】次いで、ロボット１の各関節変数θｍａｉ
（ｉ＝１，２，…，６）は図３におけるデュアルポート
ＲＡＭ１０４からモータ指令位置Ｐｍａｉ（ｉ＝１，
２，…，６）を読み出し、上記数４７を用いて演算され
る。さらに、上記数５０を用いてロボット１のＡ行列Ａ
ｍａｉ（ｉ＝０，１，２，…，６）を演算した後、上記
数７を用いてロボット１の位置姿勢を示す同次変換行列
［Ｔｍａ］を演算する。

【０１４１】以上でステップＳ４１２の処理が終了し、
次いで、ステップＳ４１６において、ポジションＡ５に
おける距離センサＳＳ１のセンサ検出距離ｌ１をＲＡＭ
１０２に取り込む。

【０１４２】次いで、ステップＳ４１８において、第１
の較正処理によって較正されかつＲＡＭ１０２に格納さ
れている同次変換行列［Ｚｔ’’］と、第３の較正処理
のうちの角度γｗｚ較正処理によって角度γｗｚが較正
されてＲＡＭ１０２に格納されている同次変換行列［Ｚ
ｗ’］と、ＲＡＭ１０２に格納されている同次変換行列
［Ｅｔ］、［Ｅｗ］とを取り込むとともに、上記演算さ
れたポジショナ３の位置姿勢を示す同次変換行列［Ｔ
ｐ］と、第２の較正処理によって得られた関節変数θｓ
３を用いて得られたスライダ２の位置姿勢を示す同次変
換行列［Ｔｓ］と、ロボット１の位置姿勢を示す同次変
換行列［Ｔｍａ］とに基づいて、次の数７７を用いて、
上記ポジションＡ５におけるワーク座標系Σｗｏｒｋを
基準としたツール４の先端Ｏｔｏｏｌの位置姿勢を示す
同次変換行列［ｗＸｔ］を演算する。

【０１４３】

【数７２】

【０１４４】また、ポジションＡ５のときの上記ワーク
座標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏｏ
ｌの位置姿勢を示す同次変換行列［ｗＸｔ］に基づい
て、ポジションＡ５におけるツール４の先端Ｏｔｏｏｌ
の位置（ｘｔｏｏｌ３，ｙｔｏｏｌ３１，ｚｔｏｏｌ
３）は、次の数７３を用いて表され、当該位置の座標値
が演算される。

【数７３】ｘｔｏｏｌ３＝ｔｘ^w’ ｙｔｏｏｌ３＝ｔｙ^w’ ｚｔｏｏｌ３＝ｔｚ^w’

【０１４５】次いで、ステップＳ４２０において、ツー
ル４の先端Ｏｔｏｏｌを図２１に示すように、上記ポジ
ションＡ５からワーク座標系ΣｗｏｒｋのＸ軸方向に所
定の移動量ＤＬ５だけ移動させてポジションＢ５に位置
させたときのワーク座標系Σｗｏｒｋを基準としたツー
ル４の先端Ｏｔｏｏｌの位置姿勢を示す同次変換行列
［ｗＸｔ’’］を以下のように演算する。

【０１４６】まず、上記数７４を用いて演算した上記ポ
ジションＡ５におけるツール４の先端Ｏｔｏｏｌの位置
成分に対して、ワーク座標系ΣｗｏｒｋのＸ軸方向のツ
ール４の先端移動量ＤＬ５を加算して、ポジションＢ５
におけるツール４の先端Ｏｔｏｏｌの位置（ｘｔｏｏｌ
４，ｙｔｏｏｌ４，ｚｔｏｏｌ４）を数７４を用いて演
算する。

【数７４】ｘｔｏｏｌ４＝ｔｘ^w’＋ＤＬ５ ｙｔｏｏｌ４＝ｔｙ^w’ ｚｔｏｏｌ４＝ｔｚ^w’

【０１４７】次いで、上記ポジションＡ５におけるワー
ク座標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏ
ｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］の中の３
行×３列の姿勢行列と、上記数７４を用いて演算したポ
ジションＢ４におけるツール４の先端Ｏｔｏｏｌの位置
（ｘｔｏｏｌ４，ｙｔｏｏｌ４，ｚｔｏｏｌ４）に基づ
いて、当該ポジションＢ５におけるワーク座標系Σｗｏ
ｒｌｄを基準としたツール４の先端Ｏｔｏｏｌの位置姿
勢を示す同次変換行列［ｗＸｔ’’］ｈａ次の数７５で
表される。

【０１４８】

【数７５】

【０１４９】ステップＳ４２２において、ポジションＡ
５からポジションＢ５まで移動させるときに、スライダ
２及びポジショナ３は動作させないので、各関節変数ｄ
ｓｊ（ｊ＝１，２）、θｐｋ（ｋ＝１，２）の値は変化
しない。すなわち、上記演算したポジショナ３の位置姿
勢を示す同次変換行列［Ｔｐ］と上記演算したスライダ
２の位置姿勢を示す同次変換行列［Ｔｓ］は変化しな
い。従って、上記ポジションＢ５におけるロボット１の
位置姿勢を示す同次変換行列［Ｔｍａ’］を、上記位置
姿勢を示す各同次変換行列［Ｔｓ］、［Ｔｐ］、［ｗＸ
ｔ’］、並びにＲＡＭ１０２に格納されている同次変換
行列［Ｚｔ’’］、［Ｚｗ’］、［Ｅｔ］、［Ｅｗ］に
基づいて、次の数７６を用いて演算する。

【０１５０】

【数７６】 [Ｔma']＝[Ｔｓ]^-1・[Ｚｔ'']^-1・[Ｚｗ']・[Ｔｐ]・[Ｅｗ]・[ｗＸｔ']・[Ｅｔ]^-1

【０１５１】さらに、上記ポジションＢ５におけるロボ
ット１の位置姿勢を示す同次変換行列［Ｔｍａ’］を逆
変換して、ロボット１の各関節変数θｍａｉ（ｉ＝１，
２，…，６）を演算した後、上記数５６を用いて、ロボ
ット１のモータ指令位置Ｐｍａｉ（ｉ＝１，２，…，
６）を演算する。上記演算されたモータ指令位置Ｐｍａ
ｉのデータは、図３に示すようにデュアルポートＲＡＭ
１０４、ラッチ回路１０５及びサーボ制御回路１０６を
介してモータＭＯ１乃至ＭＯ６に転送され、これによっ
て、ロボット１の各関節を動かし、ツール４の先端Ｏｔ
ｏｏｌの位置がポジションＢ５に位置する。

【０１５２】次いで、ステップＳ４２３において、ポジ
ションＢ５における距離センサＳＳ１のセンサ検出距離
ｌ２をＲＡＭ１０２に取り込む。そして、ステップＳ４
２４においてポジションＡ５におけるセンサ検出距離ｌ
１とポジションＢ５におけるセンサ検出距離ｌ２とが等
しいか否かが判断され、もしｌ１＝ｌ２のとき、当該角
度βｗｚ較正処理が完了したと判断して、リターンす
る。一方、もしｌ１≠ｌ２であるときステップＳ４２５
に進む。

【０１５３】ステップＳ４２５においては、上記検出さ
れたセンサ検出距離ｌ１，ｌ２に基づいて、ポジショナ
３のテーブル平面に対するツール４の先端Ｏｔｏｏｌの
ずれ量△ＤＬ５を、次の数７７を用いて演算する。

【数７７】△ＤＬ５＝ｌ２−ｌ１

【０１５４】次いで、ステップＳ４２６において、上記
演算されたずれ量△ＤＬ５と上記移動量ＤＬ５とに基づ
いて、絶対座標系Σｗｏｒｌｄを基準としたワークベー
ス座標系ΣＯｐのＹ軸回りのねじれ角度△βｗｚを、次
の数７８を用いて演算する。

【数７８】△βｗｚ＝ｓｉｎ^-1（△ＤＬ５／ＤＬ５）

【０１５５】そして、次の数７９に示すように、上記ね
じれ角度△βｗｚを補正値として用いて、ＲＡＭ１０２
に格納している絶対座標系Σｗｏｒｌｄを基準としたワ
ークベース座標系ΣｐのＺ軸回りの姿勢成分βｗｚに対
して補正して、較正値βｗｚ’を演算して姿勢成分βｗ
ｚとしてＲＡＭ１０２に格納する。

【数７９】βｗｚ’＝βｗｚ＋△βｗｚ となる。

【０１５６】次いで、上記姿勢成分の較正値βｗｚ’
を、数５に代入し、較正値βｗｚ’に対する絶対座標系
Σｗｏｒｌｄを基準としたワークベース座標系Σｐの位
置姿勢を示す同次変換行列［Ｚｗ’’］を、次の数８０
を用いて演算する。

【数８０】 [Ｚw'']＝Trans(ａwz,ｂwz,ｃwz)・Rot(Ｘ,αwz)・Rot(Ｙ,βwz')・Rot(Ｚ,γwz)

【０１５７】さらに、ステップＳ４２９において、上記
演算した位置姿勢を示す同次変換行列［Ｚｗ’’］を、
［Ｚｗ］として、ＲＡＭ１０２に格納して更新する。こ
れによって、角度βｗｚに関する較正処理が完了したの
で、リターンする。

【０１５８】（３−４−３）角度αｗｚ較正処理（ステ
ップＳ３３） 図３６は角度αｗｚ較正処理（ステップＳ３３）を示す
フローチャートであち、当該処理では、設置誤差である
ねじれ角度△αｗｚを検出して角度αｗｚを較正する。

【０１５９】まず、ステップＳ５１０において、予めＲ
ＡＭ１０１に記憶されたポジショナ３の関節変数θｐ１
及びθｐ２の固定値データθｐ１＝０°、θｐ２＝０°
を取り込む。次いで、上記ポジショナ３の関節変数θｐ
ｋ（ｋ＝１，２）に対する、ポジショナ３のモータ指令
位置Ｐｐｋ（ｋ＝１，２）を、数４９を変形することに
よって得られた数６１を用いて演算した後、当該演算さ
れたデータをデユアルポートＲＡＭ１０４、ラッチ回路
１０５及びサーボ制御回路１０７を介してモータＭＯ
７，ＭＯ８に転送してポジショナ３を初期設定の位置に
移動動作させる。次いで、図２２に示すように、操作者
がツール４の先端Ｏｔｏｏｌとポジショナ３のテーブル
平面との間後距離を測定するための距離センサＳＳ１を
そのセンサ軸がポジショナ３のテーブル平面に対して垂
直となるように操作者が取り付ける。この準備作業が完
了したことを、操作者がティーチングボックス２０を用
いてＣＰＵ１００に知らしめることによって、ステップ
Ｓ５１１の準備完了信号の判断はＹＥＳとなり、制御装
置５は各機械１，２，３がポジションＡ５にあるものと
として、以下の処理を行う。

【０１６０】次いで、ステップＳ５１２において、ポジ
ショナ３の第１軸及び第２軸のＡ行列の値は、上記数６
に上記初期設定において取り込んだ関節変数θｐ１及び
θｐ２を代入することによって数６２を用いて演算され
る。

【０１６１】さらに、Ａｐ０，Ａｐ３はそれぞれ上記表
３のリンクＬ２０，Ｌ２３のパラメータ値に基づいて演
算される。上記演算されたこのＡ行列Ａｐｋ（ｉ＝０，
１，２，３）を数９に代入することにより、ポジショナ
３の位置姿勢を示す同次変換行列［Ｔｐ］を演算する。

【０１６２】次いで、スライダ２の動作位置ｄｓｊ（ｊ
＝１，２）は、図３におけるデュアルポートＲＡＭ１０
４からモータ指令位置Ｐｓｊ（ｊ＝１，２）を読み出
し、数４８を用いて演算する。

【０１６３】さらに、数５１を用いてスライダ２のＡ行
列Ａｓｊ（ｊ＝１，２）を演算し、第２の較正処理によ
って既に較正されたスライダ２のＤ−Ｈリンクパラメー
タの関節変数θｓ３を用いて残りのＡ行列Ａｓｊ（ｊ＝
０，３）を演算した後、上記数８を用いてスライダ２の
位置姿勢を示す同次変換行列［Ｔｓ］を演算する。

【０１６４】次いで、ロボット１の各関節変数θｍａｉ
（ｉ＝１，２，…，６）は図３におけりデュアルポート
ＲＡＭ１０４からモータ指令位置Ｐｍａｉ（ｉ＝１，
２，…，６）を読み出し、数４７を用いて演算する。さ
らに、数５０を用いてロボット１のＡ行列Ａｍａｉ（ｉ
＝０，１，２，…，６）を演算した後、数７を用いてロ
ボット１の位置姿勢を示す同次変換行列［Ｔｍａ］を演
算する。

【０１６５】以上でステップＳ５１２の処理が終了し、
次いで、ステップＳ５１６において、ポジションＡ６に
おける距離センサＳＳ１のセンサ検出距離ｌ１をＲＡＭ
１０２に取り込む。

【０１６６】次いで、ステップＳ５１８において、第１
の較正処理によって較正されかつＲＡＭ１０２に格納さ
れている同次変換行列［Ｚｔ’’］と、第３の較正処理
のうちの角度γｗｚ較正処理と角度βｗｚ較正処理とに
よって角度γｗｚ，βｗｚｇａ較正されてかつ予めＲＡ
Ｍ１０２に格納されている［Ｚｗ’’］と、ＲＡＭ１０
２に格納されている同次変換行列［Ｅｔ］，［Ｅｗ］と
を取り込むとともに、上記演算されたポジショナ３の位
置姿勢を示す同次変換行列［Ｔｐ］と、第２の較正処理
によって得られた関節変数θｓ２を用いて得られたスラ
イダ２の位置姿勢を示す同次変換行列［Ｔｓ］と、ロボ
ット１の位置姿勢を示す同次変換行列［Ｔｍａ］とに基
づいて、次の数８１を用いて、上記ポジションＡ６にお
けるワーク座標系Σｗｏｒｋを基準としたツール４の先
端Ｏｔｏｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］
を演算する。

【０１６７】

【数８１】

【０１６８】また、ポジションＡ６のときの、ワーク座
標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏｏｌ
の位置姿勢を示す同次変換行列［ｗＸｔ］に基づいて、
ポジションＡ６におけるツール４の先端Ｏｔｏｏｌの位
置（ｘｔｏｏｌ５，ｙｔｏｏｌ５，ｚｔｏｏｌ５）は、
次の数８２を用いて表され、当該位置の座標値が演算さ
れる。

【数８２】ｘｔｏｏｌ５＝ｔｘ^w'’ ｙｔｏｏｌ５＝ｔｙ^w'’ ｚｔｏｏｌ５＝ｔｚ^w'’

【０１６９】次いで、ステップＳ５２０において、ツー
ル４の先端Ｏｔｏｏｌを図２２に示すように、上記ポジ
ションＡ６からワーク座標系ΣｗｏｒｋのＹ軸方向に所
定の移動量ＤＬ６だけ移動させてポジションＢ６に位置
させたときのワーク座標系Σｗｏｒｋを基準としたツー
ル４の先端Ｏｔｏｏｌの位置姿勢を示す同次変換行列
［ｗＸｔ’’’］を以下のように演算する。

【０１７０】まず、上記数８２を用いて演算した上記ポ
ジションＡ５におけるツール４の先端Ｏｔｏｏｌの位置
成分に対して、ワーク座標系ΣｗｏｒｋのＸ軸方向のツ
ール４の先端移動量ＤＬ６を加算して、ポジションＢ６
におけるツール４の先端Ｏｔｏｏｌの位置（ｘｔｏｏｌ
６，ｙｔｏｏｌ６，ｚｔｏｏｌ６）を次の数８３を用い
て演算する。

【数８３】ｘｔｏｏｌ６＝ｔｘ^ｗ'' ｙｔｏｏｌ６＝ｔｙ^ｗ''＋ＤＬ６ ｚｔｏｏｌ６＝ｔｚ^ｗ''

【０１７１】次いで、上記ポジションＡ６におけるワー
ク座標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏ
ｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］の中の３
行×３列の姿勢行列と、上記数８３を用いて演算したポ
ジションＢ６におけるツール４の先端Ｏｔｏｏｌの位置
（ｘｔｏｏｌ６，ｙｔｏｏｌ６，ｚｔｏｏｌ６）に基づ
いて、当該ポジションＢ６におけるワーク座標系Σｗｏ
ｒｌｄを基準としたツール４の先端Ｏｔｏｏｌの位置姿
勢を示す同次変換行列［ｗＸｔ’’’］は次の数８４で
表される。

【数８４】

【０１７２】ステップＳ５２２において、ポジションＡ
６からポジションＢ６まで移動させるときに、スライダ
２及びポジショナ３は動作させないので、各関節変数ｄ
ｓｊ（ｊ＝１，２）、θｐｋ（ｋ＝１，２）の値は変化
しない。すなわち、上記演算したポジショナ３の位置姿
勢を示す同次変換行列［Ｔｐ］と上記演算したスライダ
２の位置姿勢を示す同次変換行列［Ｔｓ］は変化しな
い。従って、上記ポジションＢ５におけるロボット１の
位置姿勢を示す同次変換行列［Ｔｍａ’］を、上記位置
姿勢を示す各同次変換行列［Ｔｓ］、［Ｔｐ］、［ｗＸ
ｔ’］、並びにＲＡＭ１０２に格納されている同次変換
行列［Ｚｔ’’］、［Ｚｗ’’］、［Ｅｔ］、［Ｅｗ］
に基づいて、次の数８５を用いて演算する。

【数８５】 [Ｔma']＝[Ｔｓ]^-1・[Ｚｔ'']^-1・[Ｚｗ'']・[Ｔｐ]・[Ｅｗ]・[ｗＸｔ']・[Ｅｔ]^-1

【０１７３】さらに、上記ポジションＢ６におけるロボ
ット１の位置姿勢を示す同次変換行列［Ｔｍａ’］を逆
変換して、ロボット１の各関節変数θｍａｉ（ｉ＝１，
２，…，６）を演算した後、上記数５６を用いて、ロボ
ット１のモータ指令位置Ｐｍａｉ（ｉ＝１，２，…，
６）を演算する。上記演算されたモータ指令位置Ｐｍａ
ｉのデータは、図３に示すようにデュアルポートＲＡＭ
１０４、ラッチ回路１０５及びサーボ制御回路１０６を
介してモータＭＯ１乃至ＭＯ６に転送され、これによっ
て、ロボット１の各関節を動かし、ツール４の先端Ｏｔ
ｏｏｌの位置がポジションＢ６に位置する。

【０１７４】次いで、ステップＳ５２３において、ポジ
ションＢ６における距離センサＳＳ１のセンサ検出距離
ｌ２をＲＡＭ１０２に取り込む。そして、ステップＳ５
２４においてポジションＡ６におけるセンサ検出距離ｌ
１とポジションＢ６におけるセンサ検出距離ｌ２とが等
しいか否かが判断され、もしｌ１＝ｌ２のとき、当該角
度αｗｚ較正処理が完了したと判断して、リターンす
る。一方、もしｌ１≠ｌ２であるときステップＳ５２５
に進む。

【０１７５】ステップＳ５２５においては、上記検出さ
れたセンサ検出距離ｌ１，ｌ２に基づいて、ポジショナ
３のテーブル平面に対するツール４の先端Ｏｔｏｏｌの
ずれ量△ＤＬ６を、次の数８６を用いて演算する。

【数８６】△ＤＬ６＝ｌ２−ｌ１

【０１７６】次いで、ステップＳ５２６において、上記
演算されたずれ量△ＤＬ６と上記移動量ＤＬ６とに基づ
いて、絶対座標系Σｗｏｒｌｄを基準としたワークベー
ス座標系ΣＯｐのＸ軸回りのねじれ角度△αｗｚを、次
の数８７を用いて演算する。

【数８７】△αｗｚ＝ｓｉｎ^-1（△ＤＬ６／ＤＬ６）

【０１７７】そして、次の数８８に示すように、上記ね
じれ角度△αｗｚを補正値として用いて、ＲＡＭ１０２
に格納している絶対座標系Σｗｏｒｌｄを基準としたワ
ークベース座標系ΣｐのＺ軸回りの姿勢成分αｗｚに対
して補正して、較正値αｗｚ’を演算して姿勢成分αｗ
ｚとしてＲＡＭ１０２に格納する。

【数８８】αｗｚ’＝αｗｚ＋△αｗｚ となる。

【０１７８】次いで、上記姿勢成分の較正値αｗｚ’
を、数５に代入し、較正値αｗｚ’に対する絶対座標系
Σｗｏｒｌｄを基準としたワークベース座標系Σｐの位
置姿勢を示す同次変換行列［Ｚｗ’’’］を、次の数８
９を用いて演算する。

【数８９】 [Ｚw''']＝Trans(ａwz,ｂwz,ｃwz)・Rot(X,αwz')・Rot(Y,βwz)・Rot(Z,γwz)

【０１７９】さらに、ステップＳ５２９において、上記
演算した位置姿勢を示す同次変換行列［Ｚｗ’’’］
を、［Ｚｗ］として、ＲＡＭ１０２に格納して更新す
る。そして、角度αＷＺに関する較正処理が完了したの
で、リターンする。

【０１８０】（３−５）第４の較正処理 図３７は第４の較正処理（ステップＳ３４）のフローチ
ャートである。当該第４の較正処理は、絶対座標系Σｗ
ｏｒｌｄとポジショナ３の原点Ｏｐに設定したワークベ
ース座標系Σｐとの位置成分ａｗｚ、ｂｗｚ、ｃｗｚを
較正対象とし、これらの誤差パラメータは、図１０乃至
図１２に示すように、位置の誤差△ａｗｚ、△ｂｗｚ、
△ｃｗｚである。当該第４の較正処理は、当該位置の誤
差△ａｗｚ、△ｂｗｚ、△ｃｗｚを検出して、当該誤差
が無くなるように較正する。

【０１８１】ワーク側メカニカルインターフェイス座標
系Σｗｍを基準としたワーク座標系Σｗｏｒｋの位置姿
勢を示す同次変換行列［Ｅｗ］に関するデータは、数３
０乃至数３５によって示したように、すべて０と設定さ
れている。そこで、ワーク座標系Σｗｏｒｋはポジショ
ナ３のテーブル平面の中心にワーク側メカニカルインタ
ーフェイス座標系Σｗｍと一致して設定されている。そ
こで、表面が平担になるように機械加工された直方体の
ブロックＢＫ１を、その１頂点をポジショナ３のテーブ
ツ平面の中心と一致するように載置する。すなわち、ブ
ロックＢＫ１の１頂点はワーク側メカニカルインターフ
ェイス座標系Σｗｍの原点と一致させているので、ワー
ク座標系Σｗｏｒｋの原点に一致する。さらに、上記ブ
ロックＢＫ１の１頂点を中心とし、上記ブロックＢＫ１
の１頂点から向かって右側にあるブロックＢＫ１の１辺
をワーク側メカニカルインターフェイス座標系Σｗｍの
Ｘ軸の正方向と一致するようにブロックＢＫ１を回転さ
せた後、ブロックＢＫ１をポジショナ３のテーブル平面
に固定する。そして、図２５に示すように、ロボット１
の先端に、３つの距離センサＳＳ１，ＳＳ２，ＳＳ３か
らなるセンサユニットを、上記各センサＳＳ１，ＳＳ
２，ＳＳ３のセンサ軸を互いに直交するように配設する
ための治具Ｊ１，Ｊ２，Ｊ３を介して、ツールとして取
り付ける。上記センサユニットにおける距離センサＳＳ
１とツール座標系Σｔｏｏｌの原点Ｏｔｏｏｌとの距離
は既知であり、それがｌｘ０として予めＲＯＭ１０１に
格納されている。上記距離センサＳＳ１は、ワーク座標
系ΣｗｏｒｋのＹ軸とＺ軸とによって構成されている面
と一致させて載置した直方体のブロックＢＫ１の側面と
の計測によって距離ｌｘを測定し、ワーク座標系Σｗｏ
ｒｋの原点Ｏｗｏｒｋとツール座標系Σｔｏｏｌの原点
Ｏｔｏｏｌとのワーク側メカニカルインターフェイス座
標系ΣｗｍのＸ軸に平行な距離を距離（ｌｘ−ｌｘ０）
として演算するために用いる。また、上記センサユニッ
トにおける距離センサＳＳ２とツール座標系Σｔｏｏｌ
の原点Ｏｔｏｏｌとの距離は既知であり、それがｌｙ０
として予めＲＯＭ１０１に格納されている。上記距離セ
ンサＳＳ２は、ワーク座標系ΣｗｏｒｋのＸ軸とＺ軸と
によって構成されている面と一致させて載置した直方体
のブロックＢＫ１の側面との計測によって距離ｌｙを測
定し、ワーク座標系Σｗｏｒｋの原点Ｏｗｏｒｋとツー
ル座標系Σｔｏｏｌの原点Ｏｔｏｏｌとのワーク側メカ
ニカルインターフェイス座標系ΣｗｍのＹ軸に平行な距
離を距離（ｌｙ−ｌｙ０）として演算するために用い
る。さらに、上記センサユニットにおける距離センサＳ
Ｓ３とツール座標系Σｔｏｏｌの原点Ｏｔｏｏｌとの距
離は既知であり、それがｌｚ０として予めＲＯＭ１０１
に格納されている。上記距離センサＳＳ３は、ワーク座
標系ΣｗｏｒｋのＸ軸とＹ軸とによって構成されている
面（すなわち、ポジショナ３の平面）との計測によって
距離ｌｚを測定し、ワーク座標系Σｗｏｒｋの原点Ｏｗ
ｏｒｋとツール座標系Σｔｏｏｌの原点Ｏｔｏｏｌとの
ワーク側メカニカルインターフェイス座標系ΣｗｍのＺ
軸に平行な距離を距離（ｌｚ−ｌｚ０）として演算する
ために用いる。

【０１８２】まず、ステップＳ６１０において、上記第
３の較正処理において較正した、絶対座標系Σｗｏｒｌ
ｄを基準としたワークベース座標系ΣｐのＸ軸、Ｙ軸お
よびＺ軸回りの姿勢成分の較正値αｗｚ’、βｗｚ’、
γｗｚ’を取り込む。次いで、ステップＳ６１１におい
て、直方体ブロックＢＫ１の各側面が、絶対座標系Σｗ
ｏｒｌｄのＸ軸及びＹ軸と平行になるように、絶対座標
系Σｗｏｒｌｄを基準としたワークベース座標系Σｐの
Ｚ軸回りの姿勢成分の較正値γｗｚ’だけ回転させるた
めに、次の数９０を用いてポジショナ３の第２軸Ｌ２２
の回りの回転角度（関節変数）θｐ２を演算する。

【０１８３】

【数９０】θｐ２＝（−１）×γｗｚ’ ここで、ポジショナ３の第１軸Ｌ２１の関節変数θｐ１
を０とし、数９０で算出したθｐ２と併せて、ＲＡＭ１
０２に格納される。

【０１８４】そして、上記関節変数θｐ１、θｐ２に対
するモータ指令位置Ｐｐｋ（ｋ＝１，２）を、数６１を
用いて演算する。

【０１８５】上記モータ指令位置Ｐｐｋのデータをデュ
アルポートＲＡＭ１０４、ラッチ回路１０５及びサーボ
制御回路１０７を介してモータＭＯ７，ＭＯ８に転送し
て、ポジショナ３をその第２軸Ｌ２２の回りに回転角度
θｐ２［ｒａｄ］だけ回転させる。

【０１８６】さらに、上記関節変数θｐ１，θｐ２に基
づいて、ポジショナ３の第１軸及び第２軸のＡ行列の値
を上記数６２を用いて演算する。

【０１８７】さらに、表３のポジショナ３のＤ−Ｈのリ
ンクパラメータの値に基づいて、Ａ行列Ａｐ０及びＡｐ
３も演算される。次いで、数９を用いて、ポジショナ３
の位置姿勢を示す同次変換行列［Ｔｐ］を演算する。

【０１８８】次いで、ステップＳ６１４において、予め
下記の条件のもとで作成されてＲＯＭ１０１に格納され
たワーク座標系Σｗｏｒｋを基準としたツール４の位置
姿勢を示す同次変換行列のデータ［ｗＸｔ］を取り込
む。このデータは、３個の距離センサＳＳ１−ＳＳ３を
有するセンサユニットであるツールが上記ブロックＢＫ
１とポジショナ３のテーブル面上に対して上述のように
所定の距離を測定するために設置されたときの、ワーク
座標系Σｗｏｒｋを基準としたツール４の姿勢と、ワー
ク座標系Σｗｏｒｋを基準としたツール４の位置とがす
べて０としたときの同次変換行列である。

【０１８９】ステップＳ６１５において、予めＲＯＭ１
０１に記憶されたスライダ２の所定の動作位置ｄｓｊ
（ｊ＝１，２）に対するモータ指令位置Ｐｓｊ（ｊ＝
１，２）を、数５７を用いて演算し、当該モータ指令位
置ＰｓｉのデータをデュアルポートＲＡＭ１０４、ラッ
チ回路１０５及びサーボ制御回路１０８を介してモータ
ＭＯ９，ＭＯ１０に転送され、スライダ２が動作され
る。

【０１９０】そして、上記スライダ２の動作位置ｄｓｊ
（ｊ＝１，２）に基づいて、数５１を用いてスライダ２
のＡ行列Ａｓｊ（ｊ＝１，２）を演算し、第２の較正処
理によって既に較正されたスライダ２のＤ−Ｈリンクパ
ラメータの関節変数θｓ３を用いてスライダ２のＡ行列
Ａｓｊ（ｊ＝０，３）を演算した後、スライダ２の位置
姿勢を示す同次変換行列［Ｔｓ］を、数８を用いて演算
してＲＡＭ１０２に格納する。

【０１９１】さらに、同様に、表２のスライダ２のＤ−
Ｈのリンクパラメータ値に基づいて、Ａ行列Ａｓ０及び
びＡｓ３も演算されるので、スライダ２の位置姿勢を示
す同次変換行列［Ｔｓ］を、数８を用いて、スライダ２
の動作位置ｄｓｊ（ｊ＝１，２）に対して演算する。

【０１９２】次いで、ステップＳ６１８において、ＲＡ
Ｍ１０２に格納されている同次変換行列［Ｚｔ’’］、
［Ｚｗ’’］、［Ｅｔ］、［Ｅｗ］を取り込む。そし
て、ツール４の先端の位置Ｏｔｏｏｌを、演算上のワー
ク座標系Σｗｏｒｋの原点Ｏｗｏｒｋに移動するように
ロボット１を動作させるロボット１の位置姿勢［Ｔｍ
ａ］を、上記演算されたポジショナ３の位置姿勢を示す
同次変換行列［Ｔｐ］と、上記演算されたスライダ２の
位置姿勢を示す同次変換行列［Ｔｓ］と、上記取り込ん
だ同次変換行列［Ｚｔ’’］、［Ｚｗ’’］、［Ｅ
ｔ］、［Ｅｗ］と、並びに上記ステップＳ６１４におい
て取り込んだワーク座標系Σｗｏｒｋを基準としたツー
ル４の位置姿勢を示す同次変換行列［ｗＸｔ］に基づい
て次の数９１を用いて演算する。

【０１９３】

【数９１】 [Ｔｍａ]＝[Ｔｓ]^-1・[Ｚｔ'']^-1・[Ｚｗ'']・[Ｔｐ]・[Ｅｗ]・[ｗＸｔ]・[Ｅｔ]^-1

【０１９４】さらに、ツール４の先端の位置Ｏｔｏｏｌ
が演算上のワーク座標系Σｗｏｒｋの原点Ｏｗｏｒｋに
おけるロボット１の位置姿勢を示す同次変換行列［Ｔｍ
ａ］を逆変換して、ロボット１の各関節変数θｍａｉ
（ｉ＝１，２，…，６）を演算し、上記数５６を用い
て、ロボット１のモータ指令位置Ｐｍａｉ（ｉ＝１，
２，…，６）が演算される。これらのモータ指令位置Ｐ
ｍａｉのデータはデュアルポートＲＡＭ１０４、ラッチ
回路１０５及びサーボ制御回路１０６を介してモータＭ
Ｏ１乃至ＭＯ６に転送され、これによって、ロボット１
の各関節ＲＪ１乃至ＲＪ６を回転させ、ツール４の先端
の位置Ｏｔｏｏｌが演算上のワーク座標系Σｗｏｒｋの
原点Ｏｗｏｒｋに位置させる。

【０１９５】次いで、ステップＳ６２０において、ツー
ル４の先端の位置Ｏｔｏｏｌが演算上のワーク座標系Σ
ｗｏｒｋの原点Ｏｗｏｒｋに位置する距離センサＳＳ
１，ＳＳ２，ＳＳ３のセンサ検出距離ｌｘ，ｌｙ，ｌｚ
を検出してＲＡＭ１０２に格納する。

【０１９６】上記格納された検出距離ｌｘ，ｌｙ，ｌｚ
は各距離センサＳＳ１−ＳＳ３からワーク座標系Σｗｏ
ｒｋの原点Ｏｗｏｒｋまでの距離である。そこで予めＲ
ＯＭ１０１に格納された各距離センサＳＳ１−ＳＳ３か
らＴＯＯＬ座標系Σｔｏｏｌの原点Ｏｔｏｏｌまでの距
離ｌｘ０，ｌｙ０，ｌｚ０とそれぞれ比較し、ワーク座
標系Σｗｏｒｋの原点Ｏｗｏｒｋの位置とツール座標系
Σｔｏｏｌの原点Ｏｔｏｏｌの位置とが一致しているか
がステップＳ６２１において判断される。機械１，２，
３間の誤差が無い場合は、両者が一致する。なぜなら
ば、ステップＳ６１４で取り出し、それ以降の演算の基
礎となったワーク座標系Σｗｏｒｋを基準としたツール
４の位置姿勢のデータのうち位置のデータはすべて上述
のように０としているからである。このように両者が一
致したとき、当該第４の較正処理が完了したと判断し
て、リターンする。一方、一致しないときには、ステッ
プＳ６２２に進む。

【０１９７】ステップＳ６２２において、ツール座標系
Σｔｏｏｌと各距離センサＳＳ１，ＳＳ２，ＳＳ３の端
面との最短距離ｌｘ０、ｌｙ０、ｌｚ０と、上記ステッ
プＳ６２０において検出されたセンサ検出距離ｌｘ，ｌ
ｙ，ｌｚと、上記ステップＳ６１０において取り込んだ
絶対座標系Σｗｏｒｌｄを基準としたワークベース座標
系ΣｐのＸ軸、Ｙ軸及びＺ軸回りの姿勢成分の較正値α
ｗｚ’、βｗｚ’、γｗｚ’とに基づいて、絶対座標系
Σｗｏｒｌｄを基準としたワークベース座標系Σｐの位
置成分の誤差△ａｗｚ、△ｂｗｚ、△ｃｗｚを、次の数
９２を用いて演算する。

【０１９８】

【数９２】 △ａｗｚ＝{(ｌｘ−ｌｘ０)−(ｌｚ−ｌｚ０)・ｔａｎβｗｚ'}・ｃｏｓβｗｚ' △ｂｗｚ＝{(ｌｙ−ｌｙ０)−(ｌｚ−ｌｚ０)・ｔａｎαｗｚ'}・ｃｏｓαｗｚ' △ｃｗｚ＝{(ｌｚ−ｌｚ０)・ｃｏｓαｗｚ’・ｃｏｓβｗｚ’

【０１９９】次いで、ステップＳ６２３においては、上
記演算した位置成分の誤差△ａｗｚ、△ｂｗｚ、△ｃｗ
ｚを補正値として用いて、ＲＡＭ１０２に格納されてい
る絶対座標系Σｗｏｒｌｄを基準としたワークベース座
標系Σｐの位置成分ａｗｚ、ｂｗｚ、ｃｗｚを次の数９
３を用いて補正して、その較正値ａｗｚ’，ｂｗｚ’，
ｃｗｚ’を演算する。

【０２００】

【数９３】ａｗｚ’＝ａｗｚ＋△ａｗｚ ｂｗｚ’＝ｂｗｚ＋△ｂｗｚ ｃｗｚ’＝ｃｗｚ＋△ｃｗｚ

【０２０１】さらに、上記演算した位置成分の較正値ａ
ｗｚ’、ｂｗｚ’、ｃｗｚ’を、上記数５に代入し、較
正値ａｗｚ’、ｂｗｚ’、ｃｗｚ’に対する、絶対座標
系Σｗｏｒｌｄを基準としたワークベース座標系Σｐの
位置姿勢を示す同次変換行列［Ｚｗ’’’’］を、次の
数９４を用いて演算する。

【数９４】 [Zw'''']＝Trans(ａwz',ｂwz',ｃwz')・Rot(X,αwz)・Rot(Y,βwz)・Rot(Z,γwz)

【０２０２】次いで、上記演算された較正値ａｗｚ’、
ｂｗｚ’、ｃｗｚ’に対する絶対座標系Σｗｏｒｌｄを
基準としたワークベース座標系Σｐの位置姿勢を示す同
次変換行列［Ｚｗ’’’’］を、［Ｚｗ］としてＲＡＭ
１０２に格納して更新する。これによって、第４の較正
処理が完了したとして、リターンする。

【０２０３】（４）教示作業と再生動作 以上の本発明に係る実施例の設置誤差の較正処理を当該
ロボットシステムの設置直後に実行することによって、
ロボット１とスライダ２及びポジショナ３の各々の間の
設置関係を示し、制御装置５内のＲＡＭ１０２に格納さ
れた同次変換行列［Ｚｗ］、［Ｚｔ］、［Ｔｓ］を実際
の設置状態に対応した値に修正することができる。この
後に、当該ロボットシステムに対して教示作業を行った
後に、再生動作である自動運転を行わせる。

【０２０４】もし当該較正処理を実行しない場合に、教
示作業と再生動作において発生する問題を、上記同次変
換行列［Ｚｗ］、［Ｚｔ］、［Ｔｓ］を対応し、当該較
正処理を実行しないときの設置関係を示す同次変換行列
［Ｚｗｑ］、［Ｚｔｑ］、［Ｔｓｑ］を用いて、以下に
説明する。なお、当該ロボットシステムによる作業内容
は、ワーク取扱装置１２とツール移動装置１１の両方を
動かし、ワークとツール４との相対的な動作を行わせる
ものとする。

【０２０５】本実施例のロボットシステムに対して制御
装置５が実行する計算のうち、絶対座標系Σｗｏｒｌｄ
を基準としたワーク座標系Σｐの位置姿勢を示す同次変
換行列［ｗｏｒｌｄＸｗｏｒｋ］は次の数９５で表すこ
とができ、絶対座標系Σｗｏｒｌｄを基準としたツール
座標系Σｔｏｏｌの位置姿勢を示す同次変換行列［ｗｏ
ｒｌｄＸｔｏｏｌ］は次の数９６で表すことができる。

【０２０６】

【数９５】 ［ｗｏｒｌｄＸｗｏｒｋ］＝［Ｚｗｑ］・［Ｔｐ］・［Ｅｗ］

【数９６】 ［ｗｏｒｌｄＸｔｏｏｌ］＝［Ｚｔｑ］・［Ｔｓｑ］・［Ｔｍａ］・［Ｅｔ］

【０２０７】従って、上記数９５と数９６とから、ワー
ク座標系Σｗｏｒｋを基準としたツール座標系Σｔｏｏ
ｌの位置姿勢を示す同次変換行列［ｗｏｒｋＸｔｏｏ
ｌ］は、次の数９７を用いて演算することができる。

【数９７】 [ｗｏｒｋＸｔｏｏｌ] ＝[ｗｏｒｌｄＸｗｏｒｋ]^-1・[ｗｏｒｌｄＸｔｏｏｌ] ＝[Ｅｗ]^-1・[Ｔｐ]^-1・[Ｚｗｑ]^-1・[Ｚｔｑ]・[Ｔｓｑ]・[Ｔｍａ]・[Ｅｔ]

【０２０８】教示作業において、例えば、ワーク座標系
Σｗｏｒｋを基準としたツール座標系Σｔｏｏｌの位置
姿勢を示す整数ｉ番目の同次変換行列［ｗｏｒｋＸｔｏ
ｏｌ］を上記数９７を用いて演算した後、［ｗｏｒｋＸ
ｔｏｏｌ］ｉとしてＲＡＭ１０２に格納し、次の整数
（ｉ＋１）番目のその同次変換行列を［ｗｏｒｋＸｔｏ
ｏｌ］ｉ＋１としてＲＡＭ１０２に記憶したとする。こ
こで、両方の同次変換行列の記憶データ［ｗｏｒｋＸｔ
ｏｏｌ］ｉ，［ｗｏｒｋＸｔｏｏｌ］ｉ＋１は大きな設
置誤差を含むと、以下同様に繰り返し、大きな誤差を含
んだ記憶データによる作業プログラムが作成される。

【０２０９】こうした作業プログラムに基づき再生動作
をさせると、ｉ番目からｉ＋１番目への動作は、それぞ
れ設置誤差を有する同次変換行列［ｗｏｒｋＸｔｏｏ
ｌ］ｉと同次変換行列［ｗｏｒｋＸｔｏｏｌ］ｉ＋１と
によって演算された、大きな設置誤差を有する移動距離
Ｌｑに基づいて動作を実行することになる。この間の動
作に要する所要時間Ｔｓｑは、指定されたｉ番目の相対
速度Ｖｉと、上記大きな設置誤差を有する移動距離Ｌｑ
とによって、次の数９８によって演算される。この結
果、実際のワーク上の移動距離Ｌと、ｉ番目の相対速度
Ｖｉによる期待する所要時間とが互いに大きく異なるこ
とになる。すなわち、指定した相対速度Ｖｉを実現でき
ないという問題点を生じる。

【数９８】Ｔｓｑ＝Ｌｑ÷Ｖｉ

【０２１０】従って、以上の本実施例の設置誤差の較正
処理を当該ロボットシステムの設置直後に実行すること
によって、設置誤差を無くし、これによって、教示作業
後に実行する再生作業を実行するときに、教示作業で指
示された作業内容と実質的に同一の作業内容を実現する
ことができ、例えば「ワークとツール４との間で所望の
相対速度を実現できない」という上述の問題点を解決す
ることができる。

【０２１１】（５）変形例 （５−１）第１の変形例 以上の実施例において、図６の（ａ）に示すように、産
業用ロボットシステムにおいてスライダ２が無い場合
は、上記設置誤差ε３及びε４が存在せず、図２７に示
すように、設置誤差ε２を較正する第３の較正処理（ス
テップＳ３）と、設置誤差ε１を較正する第４の較正処
理（ステップＳ４）のみを順次実行するように構成して
もよい。

【０２１２】この変形例において取り扱う数式は、基本
的には上記の実施例と同じであるが、上記スライダ２が
無い事による対応を以下に説明する。スライダ２が存在
しないことより、スライダ２の位置姿勢の同次変換行列
［Ｔｓ］は、不要となる。すなわち、数４は不要とな
る。また、スライダ２の機械原点に設定したツールベー
ス座標系Σｓ及びツール側メカニズムニカルインターフ
ェイス座標系Σｔｍは無い。そこで、絶対座標系Σｗｏ
ｒｌｄを基準としたツールベース座標系Σｓの位置姿勢
を表す同次変換行列［Ｚｔ］は不要であり、ロボット１
の機械原点Ｏｍに設定したツール機械座標系Σｍと絶対
座標系Σｗｏｒｌｄとは、一致しているとして取り扱
う。ここで、同次変換行列［Ｔｓ］および［Ｚｔ］は不
要となるから、ロボットシステムの各構成要素の位置姿
勢に関する数式、すなわち、上記実施例における数１１
は、次の数９９に置き換える。

【数９９】 ［Ｔｍａ］・［Ｅｔ］＝［Ｚｗ］・［Ｔｐ］・［Ｅｗ］・［ｗＸｔ］

【０２１３】設置誤差ε２を較正する第３の較正処理
（ステップＳ３）においては、次のように変更される。
まず、ステップＳ３１の角度γｗｚの較正処理におい
て、Ｓ３１１におけるスライダ２の動作位置ｄｓｊ（ｊ
＝１，２）の読み出しから同次変換行列［Ｔｓ］の演算
までが不要となる。さらに、ステップＳ３１８において
は同次変換行列［Ｚｔ］の読み出しが不要であり、ワー
ク座標系Σｗｏｒｋを基準としたツール４の先端Ｏｔｏ
ｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］は上記実
施例における数６３から、次の数１００に置き換える。

【数１００】 また、ステップＳ３２２において演算するポジションＢ
４におけるロボット１の位置姿勢［Ｔｍａ’］は上記実
施例における数６７から、次の数１０１に置き換える。

【数１０１】 ［Ｔｍａ'］＝［Ｚｗ］・［Ｔｐ］・［Ｅｗ］・［ｗＸｔ'］・［Ｅｔ］^-1

【０２１４】ステップ３２の角度βｗｚの較正処理にお
いて、ステップＳ４１１におけるスライダ２の動作位置
ｄｓｊ（ｊ＝１，２）の読み出しから同次変換行列［Ｔ
ｓ］の演算までが不要となる。さらに、ステップＳ４１
８においては同次変換行列［Ｚｔ］の読み出しが不要で
あり、ワーク座標系Σｗｏｒｋを基準としたツール４の
先端Ｏｔｏｏｌの位置姿勢を示す同次変換行列［ｗＸ
ｔ］は上記実施例における数７２から、次の数１０２に
置き換える。

【数１０２】 また、ステップＳ４２２において演算するポジションＢ
４におけるロボット１の位置姿勢［Ｔｍａ’］は上記実
施例における数７６から、次の数１０３に置き換える。

【数１０３】 ［Ｔｍａ'］＝［Ｚｗ'］・［Ｔｐ］・［Ｅｗ］・［ｗＸｔ'］・［Ｅｔ］^-1

【０２１５】ステップ３３の角度αｗｚの較正処理にお
いて、Ｓ５１１におけるスライダ２の動作位置ｄｓｊ
（ｊ＝１，２）の読み出しから同次変換行列［Ｔｓ］の
算出までが不要となる。さらに、ステップＳ５１８にお
いては同次変換行列［Ｚｔ］の読み出しが不要であり、
ワーク座標系Σｗｏｒｋを基準としたツール４の先端Ｏ
ｔｏｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］は上
記実施例における数８２から、次の数１０４に置き換え
る。

【数１０４】 また、ステップＳ５２２において算出するポジションＢ
４におけるロボット１の位置姿勢［Ｔｍａ’］は上記実
施例における数８５から、次の数１０５に置き換える。

【数１０５】 ［Ｔｍａ'］＝［Ｚｗ"］・［Ｔｐ］・［Ｅｗ］・［ｗＸｔ'］・［Ｅｔ］^-1

【０２１６】設置誤差ε１を較正する第４の較正処理
（ステップＳ４）においては、次のように変更される。
まず、ステップＳ６１５はスキップし実行しない。そし
て、ステップＳ６１８においては、同次変換行列［Ｚ
ｔ］の読み出しが不要であり、ステップＳ６１８におい
て算出するロボット１の位置姿勢［Ｔｍａ］は上記実施
例における数９１から、次の数１０６に置き換える。

【数１０６】 ［Ｔｍａ］＝［Ｚｗ"］・［Ｔｐ］・［Ｅｗ］・［ｗＸｔ］・［Ｅｔ］^-1

【０２１７】（５−２）第２の変形例 以上の実施例において、図６の（ｂ）に示すように、産
業用ロボットシステムにおいてポジショナ３が無い場合
は、上記設置誤差ε２が存在せず、図２８に示すよう
に、設置誤差ε３を較正する第１の較正処理（ステップ
Ｓ１）と、設置誤差ε４を較正する第２の較正処理（ス
テップＳ２）と、設置誤差ε１を較正する第４の較正処
理（ステップＳ４）のみを順次実行するように構成して
もよい。

【０２１８】この変形例において取り扱う数式は、基本
的には上記の実施例と同じであるが、前記構成要素が無
い事による対応を下記に説明する。ポジショナ３が存在
しないことより、ポジショナ３の位置姿勢の同次変換行
列［Ｔｐ］は、不要となる。すなわち、数９は不要とな
る。また、ポジショナ３の機械原点に設定したワークベ
ース座標系Σｐおよびワーク側メカニズムニカルインタ
ーフェイス座標系Σｗｍは無い。そこで、ワーク側メカ
ニズムニカルインターフェイス座標系Σｗｍを基準とし
たワーク座標系Σｗｏｒｋの位置姿勢を表す同次変換行
列［Ｅｗ］は不要である。そして、絶対座標系Σｗｏｒ
ｌｄを基準としたワーク座標系Σｗｏｒｋの位置姿勢を
示す同次変換行列を、同次変換行列［Ｚｗ］として取り
扱う。同次変換行列［Ｔｐ］及び同次変換行列［Ｅｗ］
は不要となるから、ロボットシステムの各構成要素の位
置姿勢に関する数式、すなわち、上記実施例における数
１１は、次の数１０７に置き換える。

【数１０７】 ［Ｚｔ］・［Ｔｓ］・［Ｔｍａ］・［Ｅｔ］＝［Ｚｗ］・［ｗＸｔ］

【０２１９】設置誤差ε４を較正する第２の較正処理
（ステップＳ２）においては、次のように変更される。
まず、ステップＳ２１０における、ポジショナ３の関節
変数θｐｋ（ｋ＝１，２）の計算は不要となり、ステッ
プＳ２１１における、ポジショナ３のＡ行列の計算およ
びポジショナ３の位置姿勢を示す同次変換行列［Ｔｐ］
の演算は不要となる。さらに、Ｓ２１４においては、ワ
ーク座標系Σｗｏｒｋを基準としたツール４の先端Ｏｔ
ｏｏｌの位置姿勢を示す同次変換行列［ｗＸｔ］は上記
実施例における数５３から、次の数１０８に置き換え
る。

【数１０８】 ［ｗＸｔ］＝［Ｚｗ］^-1・［Ｚｔ"］・［Ｔｓ］・［Ｔｍａ］・［Ｅｔ］ また、ステップＳ２１６において演算するポジショナＢ
３におけるロボット１の位置姿勢［Ｔｍａ’］は上記実
施例における数５５から、次の数１０９に置き換える。

【数１０９】 ［Ｔｍａ'］＝［Ｔｓ'］^-1・［Ｚｔ"］^-1・［Ｚｗ］・［ｗＸｔ］・［Ｅｔ］^-1

【０２２０】設置誤差ε１を較正する第４の較正処理
（ステップＳ４）においては、次の変更となる。まず、
較正処理を実施する準備作業として、ポジショナ３のテ
ーブル平面に相当する円板（図示せず）をワークの基準
点であるワーク座標系に設置する。そして、当該較正処
理においては、ステップＳ６１０及びステップＳ６１１
はスキップし実行しない。そして、ステップＳ６１８に
おいては、同次変換行列［Ｅｗ］の読み出しが不要であ
り、ステップＳ６１８において演算するロボット１の位
置姿勢［Ｔｍａ］は上記実施例における数９１から、次
の数１１０に置き換える。この変形例では、第３の較正
処理（ステップＳ３）は実行しないので同次変換行列
［Ｚｗ］は初期設定と同一である。

【数１１０】 ［Ｔｍａ］＝［Ｔｓ］^-1・［Ｚｔ"］^-1・［Ｚｗ］・［ｗＸｔ］・［Ｅｔ］^-1 また、ステップＳ６２２においては、数２１，数２２，
数２３によって、予め設定した絶対座標系Σｗｏｒｌｄ
を基準としたワーク座標系Σｗｏｒｋの位置姿勢を示す
同次変換行列［Ｚｗ］の姿勢成分αｗｚ，βｗｚ，γｗ
ｚを用い、これらをそれぞれαｗｚ’，βｗｚ’，γｗ
ｚ’として取り扱う。そして、絶対座標系Σｗｏｒｌｄ
を基準としたワーク座標系Σｗｏｒｋの位置姿勢を同次
変換行列［Ｚｗ］の位置成分の誤差Δａｗｚ、Δｂｗ
ｚ、Δｃｗｚを演算した後、位置成分の較正値ａｗ
ｚ’、ｂｗｚ’、ｃｗｚ’を演算する。

【０２２１】（５−３）第３の変形例 以上の実施例において、図６の（ｃ）に示すように、産
業用ロボットシステムにおいてスライダ２とポジショナ
３が無い場合は、上記設置誤差ε１のみしか存在せず、
図２９に示すように、設置誤差ε１を較正する第４の較
正処理（ステップＳ４）のみを実行するように構成して
もよい。

【０２２２】この変形例において取り扱う数式は、基本
的には上記の実施例と同じであるが、前記構成要素が無
い事による対応を下記に説明する。スライダ２が存在し
ないことより、スライダ２の位置姿勢の同次変換行列
［Ｔｓ］は、不要となる。すなわち、数４は不要とな
る。また、スライダ２の機械原点Ｏｓに設定したツール
ベース座標系Σｓ及びツール側メカニズムニカルインタ
ーフェイス座標系Σｔｍは無い。そこで、絶対座標系Σ
ｗｏｒｌｄを基準としたツールベース座標系Σｓの位置
姿勢を表す同次変換行列［Ｚｔ］は不要であり、ロボッ
ト１の機械原点Ｏｍに設定したツール機械座標系Σｍと
絶対座標系Σｗｏｒｌｄとは、一致しているとして取り
扱う。さらに、ポジショナ３が存在しないことにより、
ポジショナ３の位置姿勢の同次変換行列［Ｔｐ］は、不
要となる。すなわち、数９は不要となる。ポジショナ３
の機械原点に設定したワークベース座標系Σｐおよびワ
ーク側メカニズムニカルインターフェイス座標系Σｗｍ
は無い。そこで、ワーク側メカニズムニカルインターフ
ェイス座標系Σｗｍを基準としたワーク座標系Σｗｏｒ
ｋの位置姿勢を表す同次変換行列［Ｅｗ］は不要であ
る。そして、絶対座標系Σｗｏｒｌｄを基準としたワー
ク座標系Σｗｏｒｋの位置姿勢を示す同次変換行列を、
同次変換行列［Ｚｗ］として取り扱う。同次変換行列
［Ｔｓ］，［Ｚｔ］，［Ｔｐ］および［Ｅｗ］は不要と
なるから、ロボットシステムの各構成要素の位置姿勢に
関する数式、すなわち、上記実施例における数１１は、
次の数１１１に置き換える。

【数１１１】 ［Ｔｍａ］・［Ｅｔ］＝［Ｚｗ］・［ｗＸｔ］

【０２２３】設置誤差ε１を較正する第４の較正処理
（ステップＳ４）においては、次のように変更される。
まず、較正処理を実施する準備作業として、ポジショナ
３のテーブル平面に相当する円板（図示せず）をワーク
の基準点であるワーク座標系に設置する。そして、較正
処理においては、まず、ステップＳ６１０、ステップＳ
６１１、ステップＳ６１５はスキップし実行しない。そ
して、ステップＳ６１８においては、同次変換行列［Ｚ
ｔ］及び同次変換行列［Ｅｗ］の読み出しが不要であ
り、ステップＳ６１８において算出するロボット１の位
置姿勢を示す同次変換行列［Ｔｍａ］は上記実施例にお
ける数９１から、次の数１１２に置き換える。この変形
例では、第３の較正処理（ステップＳ３）は実行しない
ので同次変換行列［Ｚｗ］は初期設定と同一である。

【数１１２】 ［Ｔｍａ］＝［Ｚｗ］・［ｗＸｔ］・［Ｅｔ］^-1 また、ステップＳ６２２においては、数２１，数２２，
数２３によって、予め設定した絶対座標系Σｗｏｒｌｄ
を基準としたワーク座標系Σｗｏｒｋの位置姿勢を示す
同次変換行列［Ｚｗ］の姿勢成分αｗｚ，βｗｚ，γｗ
ｚを用いて、これらをそれぞれαｗｚ’，βｗｚ’，γ
ｗｚ’として取り扱う。そして、絶対座標系Σｗｏｒｌ
ｄを基準としたワーク座標系Σｗｏｒｋの位置姿勢を同
次変換行列［Ｚｗ］の位置成分の誤差Δａｗｚ，Δｂｗ
ｚ，Δｃｗｚを演算した後、位置成分の較正値ａｗ
ｚ’，ｂｗｚ’，ｃｗｚ’を演算する。

【０２２４】なお、上記第２又は第３の変形例における
ように第３の較正処理を実行せず第４の較正処理を実行
する場合は、ステップＳ６１０において入力データとし
て用いる、絶対座標系を基準としたワークベース座標系
の姿勢成分の較正値はＲＡＭ１０２に格納されているそ
れらの姿勢成分を用いる。

【０２２５】（５−４）第４の変形例 以上の実施例において、ワーク取扱装置１２にポジショ
ナ３を用いているが、本発明はこれに限らず、例えば６
自由度のロボット（図示せず。以下、第２のロボットと
いう。）を用いてもよい。この場合の処理における上記
実施例との相違点について以下に詳述する。ここで、相
違するのは、第１乃至第４の較正処理のうち第３と第４
の較正処理である。

【０２２６】（ａ）第２のロボットの位置姿勢を示す同
次変換行列［Ｔｐ］は次の数１１３で表される。

【数１１３】 ［Ｔｐ］＝Ａｐ０・Ａｐ１・Ａｐ２・Ａｐ３・Ａｐ４・Ａｐ５・Ａｐ６

【０２２７】（ｂ）第３の較正処理のうち角度γｗｚ較
正処理（図３４のステップＳ３１）において、ポジショ
ナ３のテーブル平面に相当する円板（図示せず。）を第
２のロボットの先端の把持させる。そして、第２のロボ
ットの設置基準面をワークベース座標系Σｐとする。こ
こで、ステップＳ３１０においては、上記円板が絶対座
標系ΣｗｏｒｌｄのＸ−Ｙ平面に対して９０°だけ傾斜
した状態、すなわちαｗｚ＝０，βｗｚ＝−９０°，γ
ｗｚ＝０に設定し、そのときの当該円板の位置姿勢を示
す同次変換行列［Ｔｐ］・［Ｅｗ］を予めＲＡＭ１０２
に格納する。次いで、当該同次変換行列［Ｔｐ］・［Ｅ
ｗ］に基づいて第２のロボットの位置姿勢を示す同次変
換行列［Ｔｐ］を演算し、さらに、当該同次変換行列
［Ｔｐ］を逆変換することによって第２のロボットの各
関節変数θｐｋ（ｋ＝１，２，…，６）を演算し、上記
実施例と同様に第２のロボットを動作させる。

【０２２８】（ｃ）第３の較正処理のうち角度βｗｚ較
正処理（図３５のステップＳ３２）において、ポジショ
ナ３のテーブル平面に相当する円板（図示せず。）を第
２のロボットの先端の把持させる。そして、第２のロボ
ットの設置基準面をワークベース座標系Σｐとする。こ
こで、ステップＳ４１０においては、上記円板が絶対座
標系ΣｗｏｒｌｄのＸ−Ｙ平面と平行となる状態、すな
わちαｗｚ＝０，βｗｚ＝０，γｗｚ＝０に設定し、そ
のときの当該円板の位置姿勢を示す同次変換行列［Ｔ
ｐ］・［Ｅｗ］を予めＲＡＭ１０２に格納する。次い
で、当該同次変換行列［Ｔｐ］・［Ｅｗ］に基づいて第
２のロボットの位置姿勢を示す同次変換行列［Ｔｐ］を
演算し、さらに、当該同次変換行列［Ｔｐ］を逆変換す
ることによって第２のロボットの各関節変数θｐｋ（ｋ
＝１，２，…，６）を演算し、上記実施例と同様に第２
のロボットを動作させる。

【０２２９】（ｄ）第３の較正処理のうち角度γｗｚ較
正処理（図３５のステップＳ３３）において、ポジショ
ナ３のテーブル平面に相当する円板（図示せず。）を第
２のロボットの先端の把持させる。そして、第２のロボ
ットの設置基準面をワークベース座標系Σｐとする。こ
こで、ステップＳ５１０においては、上記円板が絶対座
標系ΣｗｏｒｌｄのＸ−Ｙ平面と平行となる状態、すな
わちαｗｚ＝０，βｗｚ＝０，γｗｚ＝０に設定し、そ
のときの当該円板の位置姿勢を示す同次変換行列［Ｔ
ｐ］・［Ｅｗ］を予めＲＡＭ１０２に格納する。次い
で、当該同次変換行列［Ｔｐ］・［Ｅｗ］に基づいて第
２のロボットの位置姿勢を示す同次変換行列［Ｔｐ］を
演算し、さらに、当該同次変換行列［Ｔｐ］を逆変換す
ることによって第２のロボットの各関節変数θｐｋ（ｋ
＝１，２，…，６）を演算し、上記実施例と同様に第２
のロボットを動作させる。

【０２３０】（ｅ）第４の較正処理（図３７）におい
て、ポジショナ３のテーブル平面に相当する円板（図示
せず。）を第２のロボットの先端の把持させる。そし
て、第２のロボットの設置基準面をワークベース座標系
Σｐとする。ここで、ステップＳ６１１においては、上
記円板が絶対座標系ΣｗｏｒｌｄのＸ−Ｙ平面と平行と
なる状態に設定し、そのときの当該円板の位置姿勢を示
す同次変換行列［Ｔｐ］・［Ｅｗ］を予めＲＡＭ１０２
に格納する。次いで、当該同次変換行列［Ｔｐ］・［Ｅ
ｗ］に基づいて第２のロボットの位置姿勢を示す同次変
換行列［Ｔｐ］を演算し、さらに、当該同次変換行列
［Ｔｐ］を逆変換することによって第２のロボットの各
関節変数θｐｋ（ｋ＝１，２，…，６）を演算し、上記
実施例と同様に第２のロボットを動作させる。

【０２３１】（５−５）第５の変形例 以上の実施例において、図１に示す産業用ロボットシス
テムにおいてスライダ２が、図示した２軸のスライダで
はなく、１軸のスライダの場合には、設置誤差ε３を較
正する第１の較正処理（ステップＳ１）において、ステ
ップＳ１１０からステップＳ１３２を実行し、「αｅ」
と「γｅ」を算出する。そして、ステップ１５０以降は
スキップし実行しないことで較正処理が出来る。また、
３軸のスライダの場合には、本実施例に該当する２軸の
みを、本実施例に従って、「αｅ」と「βｅ」と「γ
ｅ」とを算出することによって、設置関係の較正ができ
る。

【０２３２】

【発明の効果】以上詳述したように本発明によれば、所
定の作業を行うツールと、上記ツールを移動させる第１
の関節手段とを含むツール移動装置と、被作業物である
ワークを含むワーク取扱装置とを備えた産業用ロボット
システムの設置誤差較正方法であって、上記ツールの位
置が上記ワークに設定されたワークベース座標系の原点
となるように上記第１の関節手段を制御して、上記第１
の関節手段の制御の前後における上記ツールの先端と、
上記ワークベース座標系の原点にその１頂点が一致する
ように載置された直方体のブロックとの間の各距離を測
定し、上記測定された各距離に基づいて、所定の絶対座
標系を基準とした上記ワークベース座標系の位置の誤差
を演算し、上記演算した誤差に基づいて絶対座標系を基
準とした上記ワークベース座標系の位置を較正する。従
って、当該較正処理を上記産業用ロボットシステムの設
置直後に実行することによって、設置誤差を無くし、こ
れによって、教示作業後に実行する再生作業を実行する
ときに、教示作業で指示された作業内容と実質的に同一
の作業内容を実現することができ、ツールとワークの相
対的な位置と速度の精度を向上させることができる。こ
れにより、例えば「ワークとツールとの間で所望の相対
速度を実現できない」という上述の問題点を解決するこ
とができる。

【図面の簡単な説明】

【図１】 本発明に係る一実施例である産業用ロボット
システムとその設置誤差較正制御装置を示す斜視図であ
る。

【図２】 図１の産業用ロボットシステムの各装置にお
けるリンク及び関節を示す機構図である。

【図３】 図１の較正制御装置のブロック図である。

【図４】 図１の産業用ロボットシステムの各装置（ス
ライダを除く。）における各関節の構成を示す斜視図、
並びに較正制御装置内のサーボ制御回路のブロック図で
ある。

【図５】 図１の実施例の産業用ロボットシステムの各
設置誤差を示す模式図である。

【図６】 （ａ）は第１の変形例の産業用ロボットシス
テムの各設置誤差を示す模式図であり、（ｂ）は第２の
変形例の産業用ロボットシステムの各設置誤差を示す模
式図であり、（ｃ）は第３の変形例の産業用ロボットシ
ステムの各設置誤差を示す模式図である。

【図７】 図１の産業用ロボットシステムに設けられた
各座標系の関連を示す平面図である。

【図８】 図１の産業用ロボットシステムに設けられた
各座標系の関連を示す正面図である。

【図９】 図１の産業用ロボットシステムに設けられた
各座標系の関連を示す側面図である。

【図１０】 図１の産業用ロボットシステムに設けられ
た各座標系において各設置誤差を示す平面図である。

【図１１】 図１の産業用ロボットシステムに設けられ
た各座標系において各設置誤差を示す正面図である。

【図１２】 図１の産業用ロボットシステムに設けられ
た各座標系において各設置誤差を示す側面図である。

【図１３】 図１の産業用ロボットシステムにおいて第
１の較正処理を実行するときにロボットをスライダの第
１軸Ｌ１１に沿って所定の距離ＤＬ１だけ移動させたと
きの装置構成を示す斜視図である。

【図１４】 図１の産業用ロボットシステムにおいて第
１の較正処理を実行するときにロボットをスライダの第
１軸Ｌ１１に沿って所定の距離ＤＬ１だけ移動させたと
きのブロックＢＫ１付近の装置構成を示す平面図であ
る。

【図１５】 図１の産業用ロボットシステムにおいて第
１の較正処理を実行するときにロボットをスライダの第
２軸Ｌ１２に沿って所定の距離ＤＬ２だけ移動させたと
きの装置構成を示す斜視図である。

【図１６】 図１の産業用ロボットシステムにおいて第
１の較正処理を実行するときにロボットをスライダの第
２軸Ｌ１２に沿って所定の距離ＤＬ２だけ移動させたと
きのブロックＢＫ１付近の装置構成を示す平面図であ
る。

【図１７】 図１の産業用ロボットシステムにおいて第
２の較正処理を実行するときの設置誤差を示す平面図で
ある。

【図１８】 図１の産業用ロボットシステムにおいて第
２の較正処理を実行するときの距離センサとブロックＢ
Ｋ１との位置関係を示す斜視図である。

【図１９】 図１の産業用ロボットシステムにおいて第
２の較正処理を実行するときの距離センサとブロックＢ
Ｋ１との位置関係を示す平面図である。

【図２０】 図１の産業用ロボットシステムにおいて第
３の較正処理のうちの角度γｗｚ較正処理を実行すると
きの設置誤差を示す正面図である。

【図２１】 図１の産業用ロボットシステムにおいて第
３の較正処理のうちの角度βｗｚ較正処理を実行すると
きの設置誤差を示す正面図である。

【図２２】 図１の産業用ロボットシステムにおいて第
３の較正処理のうちの角度αｗｚ較正処理を実行すると
きの設置誤差を示す正面図である。

【図２３】 （ａ）は図１の産業用ロボットシステムに
おいて第４の較正処理を実行するときの設置誤差を示す
平面図であり、（ｂ）はその正面図である。

【図２４】 図１の産業用ロボットシステムにおいて第
４の較正処理を実行するときの設置誤差を示す側面図で
ある。

【図２５】 （ａ）は図１の産業用ロボットシステムに
おいて第４の較正処理を実行するときの距離せンサの配
置を示す平面図であり、（ｂ）はその正面図であり、
（ｃ）はその側面図である。

【図２６】 図１の実施例の較正制御装置において実行
される設置誤差較正処理のメインルーチンである。

【図２７】 第１の変形例の較正制御装置において実行
される設置誤差較正処理のメインルーチンである。

【図２８】 第２の変形例の較正制御装置において実行
される設置誤差較正処理のメインルーチンである。

【図２９】 第３の変形例の較正制御装置において実行
される設置誤差較正処理のメインルーチンである。

【図３０】 図２６及び図２８の第１の較正処理（ステ
ップＳ１）の第１の部分を示すフローチャートである。

【図３１】 図２６及び図２８の第１の較正処理（ステ
ップＳ１）の第２の部分を示すフローチャートである。

【図３２】 図２６及び図２８の第２の較正処理（ステ
ップＳ２）を示すフローチャートである。

【図３３】 図２６及び図２７の第３の較正処理（ステ
ップＳ３）を示すフローチャートである。

【図３４】 図３３の角度γｗｚ較正処理（ステップＳ
３１）を示すフローチャートである。

【図３５】 図３３の角度βｗｚ較正処理（ステップＳ
３２）を示すフローチャートである。

【図３６】 図３３の角度αｗｚ較正処理（ステップＳ
３３）を示すフローチャートである。

【図３７】 図２６、図２７、図２８及び図２９の第４
の較正処理（ステップＳ４）を示すフローチャートであ
る。

【符号の説明】

１…ロボットマニピュレータ（ロボット）、 ２…スライダ、 ３…ポジショナ、 ４…ツール、 ５…較正制御装置、 １１…ツール移動装置、 １２…ワーク取扱装置、 ２０…ティーチングボックス、 ２１…操作ボックス、 １００…ＣＰＵ、 １０１…ＲＯＭ、 １０２…ＲＡＭ、 １０３…インターフェース、 １０４…デュアルポートＲＡＭ、 １０５…ラッチ回路、 １０６，１０７，１０８…サーボ制御回路、 １１０…クロック発生器、 １１１…分周器、 ＲＪ１乃至ＲＪ６、ＲＪ１１乃至ＲＪ１３，ＲＪ２１乃
至ＲＪ２３……関節、 Ｌ１乃至Ｌ６、Ｌ１１乃至Ｌ１３，Ｌ２１乃至Ｌ２３…
リンク、 ＭＯ１乃至Ｍ１０…モータ、 ＳＮ１乃至ＳＮ１０…センサ、 ＲＥ１乃至ＲＥ１０…減速機、 Ｊ１，Ｊ２，Ｊ３…治具、 ε１，ε２，ε３，ε４…設置誤差、 Δαｔｚ，Δβｔｚ，Δγｔｚ，Δθｓ３，Δαｗｚ，
Δβｗｚ，Δγｗｚ…ねじれ角度、 ［Ｚｔ］…絶対座標系を基準としたツールベース座標系
の位置姿勢を示す同次変換行列、 ［Ｚｔ’］，［Ｚｔ’’］…絶対座標系を基準としたツ
ールベース座標系の位置姿勢を示す同次変換行列の較正
値、 ［Ｚｗ］…絶対座標系を基準としたワークベース座標系
の位置姿勢を示す同次変換行列、 ［Ｚｗ’］，［Ｚｗ’’］，［Ｚｗ’’’］，［Ｚ
ｗ’’’’］…絶対座標系を基準としたワークベース座
標系の位置姿勢を示す同次変換行列の較正値、 ＢＫ１，ＢＫ２…直方体のブロック、 ＳＳ１，ＳＳ２，ＳＳ３…距離センサ、 Ｏｔｏｏｌ…ツールの先端。

Claims (6)

【特許請求の範囲】
1. 【請求項１】 所定の作業を行うツールと、上記ツール
   を移動させる第１の関節手段とを含むツール移動装置
   と、 被作業物であるワークを含むワーク取扱装置とを備えた
   産業用ロボットシステムの設置誤差較正方法であって、 上記ツールの位置が上記ワークに設定されたワークベー
   ス座標系の原点となるように上記第１の関節手段を制御
   して、上記第１の関節手段の制御の前後における上記ツ
   ールの先端と、上記ワークベース座標系の原点にその１
   頂点が一致するように載置された立方体のブロックとの
   間の各距離を測定する第１のステップと、 上記第１のステップにおいて測定された各距離に基づい
   て、所定の絶対座標系を基準とした上記ワークベース座
   標系の位置の誤差を演算し、上記演算した誤差に基づい
   て絶対座標系を基準とした上記ワークベース座標系の位
   置を較正する第２のステップとを含むことを特徴とする
   産業用ロボットシステムの設置誤差較正方法。
2. 【請求項２】 上記ワーク取扱装置は、上記ワークを移
   動させる第２の関節手段をさらに含み、上記設置誤差較
   正方法はさらに、 上記ツールの位置を所定の第１の位置から所定の第２の
   位置に移動させるように上記第２の関節手段を制御し
   て、上記第２の関節手段の制御の前後における上記ツー
   ルの先端と上記第２の関節手段との間の各距離を測定す
   る第３のステップと、 上記第３のステップにおいて測定された各距離に基づい
   て、上記ツールのずれ量を演算しかつ上記絶対座標系を
   基準とした上記ワークベース座標系の位置姿勢の誤差を
   演算し、上記演算した誤差に基づいて上記絶対座標系を
   基準とした上記ワークベース座標系の位置姿勢を較正す
   る第４のステップとを含むことを特徴とする請求項１記
   載の産業用ロボットシステムの設置誤差較正方法。
3. 【請求項３】 上記ツール移動装置は、上記第１の関節
   手段を所定の軸方向に沿って移動させる第３の関節手段
   をさらに含み、上記設置誤差較正方法はさらに、 上記第１の関節手段を上記所定の軸方向に沿って所定の
   第３の位置から所定の第４の位置に移動させるように上
   記第３の関節手段を制御して、上記ツールの先端に治具
   を介して設けられた距離測定手段を用いて、上記第３の
   関節手段の制御の前後におけるその各面が上記絶対座標
   系の各軸方向に平行となるように載置された直方体のブ
   ロックの各面との各距離を測定する第５のステップと、 上記第５のステップにおいて測定された各距離に基づい
   て、上記絶対座標系を基準とした上記ツールに設定され
   たツールベース座標系の位置姿勢のねじれ角度を演算
   し、上記演算したねじれ角度に基づいて上記絶対座標系
   を基準とした上記ツールに設定されたツールベース座標
   系の位置姿勢を較正する第６のステップと、 上記ワークを基準とした上記ツールの位置姿勢を一定と
   する条件のもとで、上記第１の関節手段を上記所定の軸
   方向に沿って所定の第５の位置から所定の第６の位置に
   移動させるように上記第１の関節手段と上記第３の関節
   手段とを制御して、上記第１の関節手段と上記第３の関
   節手段の制御の前後における上記ツールの先端と所定の
   固体位置との間の各距離を測定する第７のステップと、 上記第７のステップにおいて測定された各距離に基づい
   て、上記第３の関節手段の上記所定の軸方向に対する上
   記ツールベース座標系のねじれ角度を演算し、上記演算
   したねじれ角度に基づいて上記第３の関節手段の上記所
   定の軸方向に対する上記ツールベース座標系のねじれ角
   度を較正する第８のステップとを含むことを特徴とする
   請求項１又は２記載の産業用ロボットシステムの設置誤
   差較正方法。
4. 【請求項４】 所定の作業を行うツールと、上記ツール
   を移動させる第１の関節手段とを含むツール移動装置
   と、 被作業物であるワークを含むワーク取扱装置とを備えた
   産業用ロボットシステムにおいて、 上記ツールの位置が上記ワークに設定されたワークベー
   ス座標系の原点となるように上記第１の関節手段を制御
   する第１の制御手段と、 上記第１の関節手段の先端に設けられ、上記第１の制御
   手段の動作の前後における上記ツールの先端と上記ワー
   クベース座標系の原点にその１頂点が一致するように載
   置された直方体のブロックとの間の各距離を測定する第
   １の測定手段と、 上記第１の測定手段によって測定された各距離に基づい
   て、所定の絶対座標系を基準とした上記ワークベース座
   標系の位置の誤差を演算し、上記演算した誤差に基づい
   て絶対座標系を基準とした上記ワークベース座標系の位
   置を較正する第１の較正手段とを備えたことを特徴とす
   る産業用ロボットシステムの設置誤差較正制御装置。
5. 【請求項５】 上記ワーク取扱装置は、上記ワークを移
   動させる第２の関節手段をさらに含み、上記設置誤差較
   正制御装置はさらに、 上記ツールの位置を所定の第１の位置から所定の第２の
   位置に移動させるように上記第２の関節手段を制御する
   第２の制御手段と、 上記第１の関節手段の先端に設けられ、上記第２の制御
   手段の動作の前後における上記ツールの先端と上記第２
   の関節手段との間の各距離を測定する第２の測定手段
   と、 上記第２の測定手段によって測定された各距離に基づい
   て、上記ツールのずれ量を演算しかつ所定の絶対座標系
   を基準とした上記ワークベース座標系の位置姿勢の誤差
   を演算し、上記演算した誤差に基づいて上記絶対座標系
   を基準とした上記ワークベース座標系の位置姿勢を較正
   する第２の較正手段とを備えたことを特徴とする請求項
   ４記載の産業用ロボットシステムの設置誤差較正制御装
   置。
6. 【請求項６】 上記ツール移動装置は、上記第１の関節
   手段を所定の軸方向に沿って移動させる第３の関節手段
   をさらに含み、上記設置誤差較正制御装置はさらに、 上記第１の関節手段を上記所定の軸方向に沿って所定の
   第３の位置から所定の第４の位置に移動させるように上
   記第３の関節手段を制御する第３の制御手段と、 上記ツールの先端に治具を介して設けられ、上記第３の
   関節手段の制御の前後におけるその各面が上記絶対座標
   系の各軸方向に平行となるように載置された直方体のブ
   ロックの各面との各距離を測定する第３の測定手段と、 上記第３の測定手段によって測定された各距離に基づい
   て、上記絶対座標系を基準とした上記ツールに設定され
   たツールベース座標系の位置姿勢のねじれ角度を演算
   し、上記演算したねじれ角度に基づいて上記絶対座標系
   を基準とした上記ツールに設定されたツールベース座標
   系の位置姿勢を較正する第３の較正手段と、 上記ワークを基準とした上記ツールの位置姿勢を一定と
   する条件のもとで、上記第１の関節手段を上記所定の軸
   方向に沿って所定の第５の位置から所定の第６の位置に
   移動させるように上記第１の関節手段と上記第３の関節
   手段とを制御する第４の制御手段と、 上記ツールの先端と所定の固体位置との間の各距離を測
   定する第４の測定手段と、 上記第４の測定手段によって測定された各距離に基づい
   て、上記第３の関節手段の上記所定の軸方向に対する上
   記ツールベース座標系のねじれ角度を演算し、上記演算
   したねじれ角度に基づいて上記第３の関節手段の上記所
   定の軸方向に対する上記ツールベース座標系のねじれ角
   度を較正する第４の較正手段とを備えたことを特徴とす
   る請求項４又は５記載の産業用ロボットシステムの設置
   誤差較正制御装置。