JP3098618B2 - 産業用ロボットシステムの制御方法及び制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ワークを把持しワーク
の姿勢を変化するためのワーク取扱装置と、ツールを設
けツール先端の位置およびツール姿勢を制御するための
ツール移動装置とからなる産業用ロボットシステムのた
めの制御方法及び制御装置に関し、特にティーチングプ
レイバック方法を用いて協調動作を行う産業用ロボット
システムのための制御方法及び制御装置に関する。

【０００２】

【従来の技術及び発明が解決しようとする課題】従来、
ツール移動装置とワーク取扱装置を同時に動作させなが
ら１つの作業を行う協調動作に関する再生動作におい
て、ワークに対するツール先端の相対速度を、「教示し
た一定の速度」に保持することは困難であった。この理
由を、例えば、図２８の（ａ）から（ｂ）に示すよう
に、徐々にワーク１の姿勢を変化させながら、溶接トー
チであるツール２をＰ１点からＰ２点まで楕円を直線近
似することにより移動させる従来例の３つの方法を引用
して説明する。

【０００３】第１の従来例の方法においては、図２９の
（ａ）乃至（ｅ）で示す状態を各々の目標点とした多数
の教示点を作業プログラム内に記憶して再生する。この
再生動作では、溶接トーチであるツール２の開始時刻と
終了時刻はそれぞれ、ワーク１のそれらと同期する。し
かしながら、ツール移動装置とワーク取扱装置はそれぞ
れ異なる制御装置を有し、別々に動作指令の演算を行っ
ているため、両者の動作中の相対速度は変化するという
問題点があった。従って、当該方法を例えば溶接プロセ
スに適用した場合、ツール移動装置とワーク取扱装置は
協調動作を行うことができないので、所望のシームに沿
って溶接作業を行うことができず、溶接の品質が大幅に
低下するという問題点があった。

【０００４】また、第２の従来例の方法として、特開昭
６３−２１６１０５号公報には、ツール移動装置とワー
ク取扱装置の各々の指令目標値を計算するとともに、動
作実行中におけるツールおよびワークの位置を検出して
相対的な位置を計算することによって、所望の相対位置
となるように個々の指令目標値を修正する制御方法が提
案されている。しかしながら、ツール移動装置とワーク
取扱装置はそれぞれ、第１の従来例の方法と同様に、異
なる制御装置を有し、別々に動作指令の演算を実行する
ので、両者の動作中の相対速度は変化する。

【０００５】さらに、第３の従来例の方法として、ワー
クに対するツールの位置をセンサによって検出し、ツー
ルの位置を示すセンサの検出値を補正する方法が提案さ
れている。この方法を用いた装置において、ワークとツ
ールが正確にかつ一定の相対速度で協調動作を行うため
には、補正演算中において、ワーク取扱装置の制御装置
とツール移動装置の制御装置との間で、情報交換を行う
ための高速な通信を実行することが必要となる。しかし
ながら、従来例の産業用ロボットシステムにおいては、
実用上高速な応答性を実現することは非常に困難であっ
た。

【０００６】本発明の目的は以上の問題点を解決し、ワ
ーク取扱装置の制御装置とツール移動装置の制御装置と
の間で通信を行う必要がなく、実質的に一定の相対速度
でワークとツールとを協調動作させることができる産業
用ロボットシステムの制御方法及び制御装置を提供する
ことにある。

【０００７】

【課題を解決するための手段】本発明に係る請求項１記
載の産業用ロボットシステムの制御方法は、ワークと上
記ワークを移動させる第１の関節手段とを有するワーク
取扱装置と、ツールと上記ツールを移動させる第２の関
節手段とを有するツール移動装置とを備えた産業用ロボ
ットシステムにおいて、連続する２個の教示点間の軌道
上で上記ワークと上記ツールを移動させるための制御方
法であって、（ａ）上記２個の教示点についての、所定
のワーク基準点を基準とした上記ツールの位置姿勢と、
所定の基準座標系を基準とした上記ワークの位置姿勢
と、上記２個の教示点間の軌道を上記ツールが並進する
ときの上記ワーク基準点を基準とした上記ツールの所定
の基準点の並進速度とに基づいて、上記２個の教示点間
の軌道を所定数に分割して得られる複数の補間点におけ
る、上記基準座標系を基準とした上記ワークの位置姿勢
と、上記ワーク基準点を基準とした上記ツールの位置姿
勢とを所定の補間方法によって計算するステップと、
（ｂ）上記計算された上記基準座標系を基準とした上記
ワークの位置姿勢と、上記ワーク基準点を基準とした上
記ツールの位置姿勢とに基づいて、上記ワーク取扱装置
の設置基準面の座標系を基準とした上記ワークの所定の
取付基準点の位置姿勢と、上記ツール取扱装置の設置基
準面の座標系を基準とした上記ツールの所定の取付基準
点の位置姿勢とを所定の座標変換によって計算するステ
ップと、（ｃ）上記計算された上記ワークの所定の取付
基準点の位置姿勢と、上記ツールの所定の取付基準点の
位置姿勢とに基づいて、上記軌道上で上記ワークと上記
ツールを移動させるための上記第１の関節手段の関節変
数と上記第２の関節手段の関節変数とを所定の逆変換に
よって計算するステップと、（ｄ）上記計算された上記
第１の関節手段の関節変数と上記第２の関節手段の関節
変数とに基づいて、上記第１と第２の関節手段を同時に
駆動するステップとを含むことを特徴とする。

【０００８】また、請求項２記載の産業用ロボットシス
テムの制御方法は、請求項１記載の制御方法において、
上記基準座標系は、所定の絶対座標系又は上記ワーク取
扱装置の設置基準面の座標系であることを特徴とする。

【０００９】さらに、本発明に係る請求項３記載の産業
用ロボットシステムの制御装置は、ワークと上記ワーク
を移動させる第１の関節手段とを有するワーク取扱装置
と、ツールと上記ツールを移動させる第２の関節手段と
を有するツール移動装置とを備えた産業用ロボットシス
テムにおいて、連続する２個の教示点間の軌道上で上記
ワークと上記ツールを移動させるための制御装置であっ
て、（ａ）上記２個の教示点についての、所定のワーク
基準点を基準とした上記ツールの位置姿勢と、所定の基
準座標系を基準とした上記ワークの位置姿勢と、上記２
個の教示点間の軌道を上記ツールが並進するときの上記
ワーク基準点を基準とした上記ツールの所定の基準点の
並進速度とに基づいて、上記２個の教示点間の軌道を所
定数に分割して得られる複数の補間点における、上記基
準座標系を基準とした上記ワークの位置姿勢と、上記ワ
ーク基準点を基準とした上記ツールの位置姿勢とを所定
の補間方法によって計算する第１の計算手段と、（ｂ）
上記第１の計算手段によって計算された上記基準座標系
を基準とした上記ワークの位置姿勢と、上記ワーク基準
点を基準とした上記ツールの位置姿勢とに基づいて、上
記ワーク取扱装置の設置基準面の座標系を基準とした上
記ワークの所定の取付基準点の位置姿勢と、上記ツール
取扱装置の設置基準面の座標系を基準とした上記ツール
の所定の取付基準点の位置姿勢とを所定の座標変換によ
って計算する第２の計算手段と、（ｃ）上記第２の計算
手段によって計算された上記ワークの所定の取付基準点
の位置姿勢と、上記ツールの所定の取付基準点の位置姿
勢とに基づいて、上記軌道上で上記ワークと上記ツール
を移動させるための上記第１の関節手段の関節変数と上
記第２の関節手段の関節変数とを所定の逆変換によって
計算する第３の計算手段と、（ｄ）上記第３の計算手段
によって計算された上記第１の関節手段の関節変数と上
記第２の関節手段の関節変数とに基づいて、上記第１と
第２の関節手段を同時に駆動する駆動手段とを備えたこ
とを特徴とする。

【００１０】また、請求項４記載の産業用ロボットシス
テムの制御装置は、請求項３記載の制御装置において、
上記駆動手段は、（ａ）所定の周期を有する同期信号を
発生する信号発生手段と、（ｂ）上記第３の計算手段に
よって計算された上記第１の関節手段の関節変数と上記
第２の関節手段の関節変数とを、上記信号発生手段によ
って発生された同期信号に同期して同時に記憶して出力
する同期出力手段と、（ｃ）上記同期出力手段から出力
される上記第１の関節手段の関節変数と上記第２の関節
手段の関節変数とに基づいて上記第１と第２の関節手段
を駆動する駆動制御手段とを備えたことを特徴とする。

【００１１】さらに、請求項５記載の産業用ロボットシ
ステムの制御装置は、請求項３又は４記載の制御装置に
おいて、上記基準座標系は、所定の絶対座標系又は上記
ワーク取扱装置の設置基準面の座標系であることを特徴
とする。

【００１２】

【作用】請求項１記載の産業用ロボットシステムの制御
方法においては、まず、上記２個の教示点についての、
所定のワーク基準点を基準とした上記ツールの位置姿勢
と、所定の基準座標系を基準とした上記ワークの位置姿
勢と、上記２個の教示点間の軌道を上記ツールが並進す
るときの上記ワーク基準点を基準とした上記ツールの所
定の基準点の並進速度とに基づいて、上記２個の教示点
間の軌道を所定数に分割して得られる複数の補間点にお
ける、上記基準座標系を基準とした上記ワークの位置姿
勢と、上記ワーク基準点を基準とした上記ツールの位置
姿勢とを所定の補間方法によって計算する。次いで、上
記計算された上記基準座標系を基準とした上記ワークの
位置姿勢と、上記ワーク基準点を基準とした上記ツール
の位置姿勢とに基づいて、上記ワーク取扱装置の設置基
準面の座標系を基準とした上記ワークの所定の取付基準
点の位置姿勢と、上記ツール取扱装置の設置基準面の座
標系を基準とした上記ツールの所定の取付基準点の位置
姿勢とを所定の座標変換によって計算する。さらに、上
記計算された上記ワークの所定の取付基準点の位置姿勢
と、上記ツールの所定の取付基準点の位置姿勢とに基づ
いて、上記軌道上で上記ワークと上記ツールを移動させ
るための上記第１の関節手段の関節変数と上記第２の関
節手段の関節変数とを所定の逆変換によって計算する。
最後に、上記計算された上記第１の関節手段の関節変数
と上記第２の関節手段の関節変数とに基づいて、上記第
１と第２の関節手段を同時に駆動する。

【００１３】すなわち、上記計算された上記第１の関節
手段の関節変数と上記第２の関節手段の関節変数とに基
づいて、上記第１と第２の関節手段を同時に駆動するの
で、上記連続する２個の教示点間の所望の軌道上で、上
記ツールと上記ワークを一定の相対速度で協調動作をさ
せて移動させることができる。

【００１４】また、請求項２記載の産業用ロボットシス
テムの制御方法において、上記基準座標系は、好ましく
は、所定の絶対座標系又は上記ワーク取扱装置の設置基
準面の座標系である。

【００１５】さらに、請求項３記載の産業用ロボットシ
ステムの制御装置においては、上記第１の計算手段は、
上記２個の教示点についての、所定のワーク基準点を基
準とした上記ツールの位置姿勢と、所定の基準座標系を
基準とした上記ワークの位置姿勢と、上記２個の教示点
間の軌道を上記ツールが並進するときの上記ワーク基準
点を基準とした上記ツールの所定の基準点の並進速度と
に基づいて、上記２個の教示点間の軌道を所定数に分割
して得られる複数の補間点における、上記基準座標系を
基準とした上記ワークの位置姿勢と、上記ワーク基準点
を基準とした上記ツールの位置姿勢とを所定の補間方法
によって計算する。次いで、上記第２の計算手段は、上
記第１の計算手段によって計算された上記基準座標系を
基準とした上記ワークの位置姿勢と、上記ワーク基準点
を基準とした上記ツールの位置姿勢とに基づいて、上記
ワーク取扱装置の設置基準面の座標系を基準とした上記
ワークの所定の取付基準点の位置姿勢と、上記ツール取
扱装置の設置基準面の座標系を基準とした上記ツールの
所定の取付基準点の位置姿勢とを所定の座標変換によっ
て計算する。さらに、上記第３の計算手段は、上記第２
の計算手段によって計算された上記ワークの所定の取付
基準点の位置姿勢と、上記ツールの所定の取付基準点の
位置姿勢とに基づいて、上記軌道上で上記ワークと上記
ツールを移動させるための上記第１の関節手段の関節変
数と上記第２の関節手段の関節変数とを所定の逆変換に
よって計算する。最後に、上記駆動手段は、上記第３の
計算手段によって計算された上記第１の関節手段の関節
変数と上記第２の関節手段の関節変数とに基づいて、上
記第１と第２の関節手段を同時に駆動する。

【００１６】すなわち、上記計算された上記第１の関節
手段の関節変数と上記第２の関節手段の関節変数とに基
づいて、上記第１と第２の関節手段を同時に駆動するの
で、上記連続する２個の教示点間の所望の軌道上で、上
記ツールと上記ワークを一定の相対速度で協調動作をさ
せて移動させることができる。

【００１７】また、請求項４記載の産業用ロボットシス
テムの制御装置の駆動手段においては、上記信号発生手
段は、所定の周期を有する同期信号を発生した後、上記
同期出力手段は、上記第３の計算手段によって計算され
た上記第１の関節手段の関節変数と上記第２の関節手段
の関節変数とを、上記信号発生手段によって発生された
同期信号に同期して同時に記憶して出力する。さらに、
上記駆動制御手段は、上記同期出力手段から出力される
上記第１の関節手段の関節変数と上記第２の関節手段の
関節変数とに基づいて上記第１と第２の関節手段を駆動
する。これによって、上記計算された上記第１の関節手
段の関節変数と上記第２の関節手段の関節変数とに基づ
いて、上記第１と第２の関節手段を同時に駆動すること
ができる。

【００１８】さらに、請求項５記載の産業用ロボットシ
ステムの制御装置において、上記基準座標系は、好まし
くは、所定の絶対座標系又は上記ワーク取扱装置の設置
基準面の座標系である。

【００１９】

【実施例】以下、添付の図面を参照して本発明に係る一
実施例の産業用ロボットシステムについて、以下の項目
の順序で説明する。 （１）産業用ロボットシステムの構成 （２）制御装置の構成 （３）制御方法の原理 （３−１）教示作業 （３−２）再生動作 （４）制御装置の処理 （４−１）メインルーチン （４−２）手動操作処理 （４−３）教示データ計算処理 （４−４）再生動作処理 （４−５）増分量計算処理 （４−６）モータ制御処理 （５）変形例

【００２０】なお、本明細書において、「位置姿勢」と
は位置及び姿勢を意味し、例えば「Ａ点の位置姿勢」と
は、Ａ点に設けられた座標系の位置姿勢を意味する。本
実施例において、「ワーク基準点の位置姿勢」はワーク
基準点に設けられた座標系の位置姿勢であって、当該座
標系はワーク１の位置姿勢を示すために設けられ、従っ
て、「ワーク基準点の位置姿勢」はワーク基準点の位置
及びワーク１本体の位置姿勢を表す。ツール２について
の同様である。さらに、「設置基準面を基準とした」と
いうことは「設置基準面の座標系を基準とした」という
ことを意味する。

【００２１】（１）産業用ロボットシステムの構成 図１に本実施例の産業用ロボットシステムの全体を示
す。本実施例の産業用ロボットシステムは、図１に示す
ように、溶接トーチであるツール２を移動するための６
自由度マニピュレータからなるツール移動装置１１と、
被作業対象物であるワーク１を把持するグリッパＧＲを
有する６自由度マニピュレータからなるワーク取扱装置
１２とからなるロボットシステムにおいて、特に上記各
装置１１，１２を制御するための１個の制御装置３を備
えたことを特徴としている。ここで、制御装置３には、
教示作業時において上記２個の装置１１，１２を動作さ
せて作業プログラムを作成するために操作者がデータの
入力を行うティーチングボックス２０と、教示作業と自
動運転作業とを切り換えたり自動運転作業時における起
動信号を出力するために操作者が手動操作する操作ボッ
クス２１とが接続される。

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

【００２３】ツール移動装置１１においては、図８乃至
図１２に示すように、次の３次元の座標系が設定され
る。 （ａ）各リンクＬ０乃至Ｌ５の先端である各関節ＲＪ１
乃至ＲＪ６並びにリンクＬ６の先端にそれぞれ、それら
の各関節軸Ｚ０乃至Ｚ６の中心ＯＴ０，ＯＴ１，ＯＴ
２，ＯＴ３，ＯＴ４，ＯＴ５，ＯＴ６を中心として設定
される７個の座標系（Ｘｋ，Ｙｋ，Ｚｋ）（ｋ＝０，
１，２，…，６）； （ｂ）台座１１Ｂに、中心ＯＴｂを中心として設定され
る基準座標系（Ｘｂａｓｅ，Ｙｂａｓｅ，Ｚｂａｓ
ｅ）；並びに、 （ｃ）ツール２の先端に、中心ＯＴＴを中心として設定
されるツール座標系（Ｘｔ，Ｙｔ，Ｚｔ）。

【００２４】ここで、リンクＬ０，Ｌ１はそれぞれ、関
節ＲＪ１の関節軸Ｚ０の回りに回転可能であって、かつ
互いに同一の中心軸Ｚｂａｓｅ，Ｚ０を有し、関節変数
θ１はＸ０−Ｙ０平面上で軸Ｘｂａｓｅに平行でかつ中
心ＯＴ０から延在する軸Ｘｂａｓｅ’と軸Ｘ０との間の
角度で定義される。また、リンクＬ０の共通法線距離ａ
０ｔは０であり、そのリンク間距離ｄ０ｔは各中心ＯＴ
ｂ，ＯＴ０間の距離である。さらに、リンクＬ１の共通
法線距離はａ１ｔで示され、そのリンク間距離はｄ１ｔ
で示される。リンクＬ２は関節ＲＪ２の関節軸Ｚ１の回
りに回転可能であって、関節変数θ２はＸ１−Ｙ１平面
上で軸Ｘ０に平行でかつ中心ＯＴ１から延在する軸Ｘ
０’と軸Ｘ１との間の角度で定義される。以下同様にし
て、リンクＬ３乃至Ｌ６はそれぞれ、関節ＲＪ３乃至Ｒ
Ｊ６の関節軸Ｚ２乃至Ｚ５の回りに回転可能であって、
関節変数θ３乃至θ６が同様に定義される。さらに、以
下同様にして、各リンクＬ２乃至Ｌ６の共通法線距離は
それぞれａ２ｔ乃至ａ６ｔで示され、それらのリンク間
距離はｄ２ｔ乃至ｄ６ｔで示される。

【００２５】表１に、ツール移動装置１１の各リンクＬ
０乃至Ｌ６のパラメータを示す。ここで、関節変数θｎ
は第ｎ番目のリンクＬｎの回転角度を示し、共通法線距
離ａｎｔは第ｎ番目のリンクＬｎの長さに対応し、リン
ク間距離ｄｎｔは第ｎ−１番目のリンクＬｎ−１と第ｎ
番目のリンクＬｎとの間の距離に対応し、ねじれ角度α
ｎｔは第ｎ−１番目のリンクＬｎ−１と第ｎ番目のリン
クＬｎとの間の角度に対応する関節軸間の角度である。

【００２６】

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

【００２７】ツール移動装置１１の各リンクＬｎ（ｎ＝
０，１，２，…，６）における座標変換を、公知のデナ
ビット−ハルテンべルグ（Ｄｅｎａｖｉｔ−Ｈａｒｔｅ
ｎｂｅｒｇ）の表現によるリンクパラメータを用いて、
次の数１の同次変換行列Ａｎｔで表すことができる。

【数１】

【００２８】ここで、Ｒｏｔ（ａ，ｂ）はａの軸を中心
として角度ｂ（ｒａｄ）だけ回転することを示す回転変
換であり、Ｔｒａｎｓ（ａ，ｂ，ｃ）はＸ軸方向に距離
ａだけ並進させ、Ｙ軸方向に距離ｂだけ並進させ、Ｚ軸
方向に距離ｃだけ並進させることを示す並進変換であ
る。これらの同次変換は、リチャード・ピー・ポール
（Ｒｉｃｈａｒｄ Ｐ Ｐａｕｌ）著，“ロボット・マ
ニピュレータズ；数学，プログラミングと制御（ＲＯＢ
ＯＴ ＭＡＮＩＰＵＬＡＴＯＲＳ；ＭＡＴＨＥＭＡＴＩ
ＣＳ，ＰＲＯＧＲＡＭＭＩＮＧ，ＡＮＤ ＣＯＮＴＲＯ
Ｌ）”，ザ・エムアイティー・プレス（Ｔｈｅ ＭＩＴ
Ｐｒｅｓｓ），１９８１年（以下、参照文献１とい
う。）において用いられており、上記数１は上記参照文
献１の式（２．３５）に対応する。

【００２９】本実施例において、関節ＲＪ６の座標系
（Ｘ６，Ｙ６，Ｚ６）をメカニカルインターフェース座
標系と呼び、その座標系の中心ＯＴ６をツール取付基準
点と呼ぶ。ツール２においては、図１２に示すように、
関節軸Ｚ６上にツール２の溶接トーチの先端が位置し、
当該先端をツール座標系（Ｘｔ，Ｙｔ，Ｚｔ）の中心Ｏ
ＴＴ（以下、ツール基準点という。）に設定している。
ここで、中心ＯＴ６と中心ＯＴＴ間の距離をｄｔとし、
関節軸Ｚ６とツール２の中心軸である軸Ｚｔとの間の角
度をβｔとすると、ツール座標系（Ｘｔ，Ｙｔ，Ｚｔ）
の位置姿勢Ｅｔは上記メカニカルインターフェース座標
系（Ｘ６，Ｙ６，Ｚ６）を基準にして次の数２で示すよ
うに４行４列の同次変換行列で表される。

【数２】

【００３０】一方、ワーク取扱装置１２は、台座１２Ｂ
上に載置され、６個の回転可能な関節ＲＪ１１乃至ＲＪ
１６と、各関節間を連結する７個のリンクＬ１０乃至Ｌ
１６とを備える。ここで、台座１２Ｂは、第０リンクＬ
１０を介して第１関節ＲＪ１１に連結され、第１関節Ｒ
Ｊ１１は第１リンクＬ１１を介して第２関節ＲＪ１２に
連結され、第２関節ＲＪ１２は第２リンクＬ１２を介し
て第３関節ＲＪ１３に連結される。また、第３関節ＲＪ
１３は第３リンクＬ１３を介して第４関節ＲＪ１４に連
結され、第４関節ＲＪ１４は第４リンクＬ１４を介して
第５関節ＲＪ１５に連結され、第５関節ＲＪ１５は第５
リンクＬ１５を介して第６関節ＲＪ１６に連結される。
さらに、第６関節ＲＪ１６は第６リンクＬ１６とグリッ
パＧＲを介してワーク１に連結される。各関節ＲＪｋ
（ｋ＝１１，１２，…，１６）は図７に示すように、制
御装置３によってその回転角度が制御されサーボモータ
にてなるモータＭＯｋと、モータＭＯｋの回転を所定の
減速比λｋで減速する減速機ＲＥｋと、減速前のモータ
ＭＯｋの回転軸の位置を検出するセンサＳＮｋとから構
成され、その軸がそれぞれ矢印３０１乃至３０６の方向
で回転される。

【００３１】ワーク取扱装置１２においても、ツール移
動装置１１と同様に、次のように３次元の座標系が設定
される。図１３に示すように、台座１２Ｂに、ワーク基
準座標系（Ｘｗｂａｓｅ，Ｙｗｂａｓｅ，Ｚｗｂａｓ
ｅ）が設定され、リンクＬ１６の先端であるグリッパＧ
Ｒの取付位置にワーク取付基準点ＯＷ６が設定され、ワ
ーク１の先端に、中心ＯＷ（以下、ワーク基準点とい
う。）を中心として設定されるワーク座標系（Ｘｗ，Ｙ
ｗ，Ｚｗ）が設定される。さらに、ワーク取扱装置１２
の各関節ＲＪ１１乃至ＲＪ１６において、ツール移動装
置１１と同様に関節変数φ１乃至φ６が定義される。さ
らに、ワーク取扱装置１２の各リンクＬｎ＋１０（ｎ＝
０，１，２，…，６）において、ツール移動装置１１と
同様に、共通法線距離ａｎｗと、リンク間距離ｄｎｗ
と、ねじれ角度αｎｗが定義される。

【００３２】従って、ワーク取扱装置１２の各リンクＬ
ｎ＋１０（ｎ＝０，１，２，…，６）における座標変換
を、ツール移動装置１１と同様に、公知のデナビット−
ハルテンべルグ（Ｄｅｎａｖｉｔ−Ｈａｒｔｅｎｂｅｒ
ｇ）の表現によるリンクパラメータを用いて、次の数３
の同次変換行列Ａｎｗで表すことができる。

【数３】

【００３３】さらに、ワーク取付基準点ＯＷ６に対する
ワーク基準点ＯＷの位置姿勢は、ツール移動装置１１の
数２と同様に、４行４列の同次変換行列Ｅｗで表され
る。

【００３４】（２）制御装置の構成 図２に制御装置２の構成を示す。

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

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

【００３７】なお、ＣＰＵ１００は、後述のように計算
される関節変数を減速比λｋで除算することによってモ
ータ指令値ＤＰｋ，ＤＰｋ＋１０のデータを計算し、デ
ュアルポートＲＡＭ１０４に書き込む。

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

【００３９】図４に操作ボックス２１の正面図を示す。
図４に示すように、操作ボックス２１上に、教示作業処
理を行うか（Ａ側）又は自動運転による再生動作処理を
行うか（Ｂ側）を選択的に切り換える処理切り換えスイ
ッチＳＷ１と、再生動作処理の実行を起動させるための
起動スイッチＳＷ２が設けられる。

【００４０】図５に教示作業処理を行うときに操作者が
操作するためのティーチングボックス２０の正面図を示
す。図５に示すように、当該ティーチングボックス２０
上に、制御すべき装置１０，１１を指定する装置指定ス
イッチＳＷ３と、装置指定スイッチＳＷ３によって指定
された装置１０又は１１における第１乃至第６関節のモ
ータＭＯｋを関節軸Ｚｋ−１の上方から見て左回りに回
転させる＋スイッチと右回りに回転させる−スイッチか
らなる１２個の回転指示スイッチＳＷ４と、教示データ
計算処理を実行することを指示する記憶スイッチＳＷ５
と、教示作業を終了し作業プログラム生成処理を実行す
ることを指示する終了スイッチＳＷ６と、教示データ計
算処理においてツール２の並進速度を入力するためのテ
ンキーＳＷ７とが設けられる。

【００４１】（３）制御方法の原理 本実施例の産業用ロボットシステムにおいて用いられる
制御方法について、教示作業と再生動作に分けて、以下
に説明する。

【００４２】（３−１）教示作業 以下においては、複数回の教示のうちのｊ番目の教示に
おいて、第ｊ教示データを作成する場合について説明す
る。

【００４３】ツール移動装置１１の設置基準面（Ｘｂａ
ｓｅ−Ｙｂａｓｅ平面）に設けられた基準座標系（Ｘｂ
ａｓｅ，Ｙｂａｓｅ，Ｚｂａｓｅ）を基準とした、ツー
ル取付基準点ＯＴ６に設けられた座標系（Ｘ６，Ｙ６，
Ｚ６）の位置姿勢（以下、ツール移動装置１１の設置基
準面（Ｘｂａｓｅ−Ｙｂａｓｅ平面）を基準としたツー
ル取付基準点ＯＴ６の位置姿勢といい、他の位置姿勢に
ついても同様に表現する。）を示す同次変換行列Ｔｔ
（ｊ）は、参照文献１の式（２．４０）に対応して、次
の数４のように各リンクＬｎ（ｎ＝０，１，２，…，
６）の同次変換行列Ａｎｔの積で表される。

【数４】Ｔｔ（ｊ）＝Ａ０ｔ・Ａ１ｔ・Ａ２ｔ・Ａ３ｔ
・Ａ４ｔ・Ａ５ｔ・Ａ６ｔ

【００４４】一方、ワーク取扱装置１２の設置基準面
（Ｘｗｂａｓｅ−Ｙｗｂａｓｅ平面）を基準としたワー
ク取付基準点ＯＷ６の位置姿勢を示す同次変換行列Ｔｗ
（ｊ）は、数４と同様に、次の数５のように各リンクＬ
ｎ＋１０（ｎ＝０，１，２，…，６）の同次変換行列Ａ
ｎｗの積で表される。

【数５】Ｔｗ（ｊ）＝Ａ０ｗ・Ａ１ｗ・Ａ２ｗ・Ａ３ｗ
・Ａ４ｗ・Ａ５ｗ・Ａ６ｗ

【００４５】次に、図１３に図示された所定の絶対座標
系Σｗｏｒｌｄ（Ｘｗｏｒｌｄ，Ｙｗｏｒｌｄ，Ｚｗｏ
ｒｌｄ）を基準としたワーク基準点ＯＷの位置姿勢を示
す同次変換行列ｗｏｒｌｄＸｗ（ｊ）は、数５で表され
た同次変換行列Ｔｗ（ｊ）を用いて、次の数６で表され
る。

【数６】 ｗｏｒｌｄＸｗ（ｊ）＝Ｚｗ・Ｔｗ（ｊ）・Ｅｗ ここで、Ｚｗは絶対座標系Σｗｏｒｌｄを基準としたワ
ーク取扱装置１２の設置基準面（Ｘｗｂａｓｅ−Ｙｗｂ
ａｓｅ平面）の位置姿勢を示す同次変換行列であり、同
次変換行列Ｚｗ，Ｅｗのデータは予めＲＯＭ１０１に格
納される。

【００４６】また、図１３に図示された所定の絶対座標
系Σｗｏｒｌｄ（Ｘｗｏｒｌｄ，Ｙｗｏｒｌｄ，Ｚｗｏ
ｒｌｄ）を基準としたツール基準点ＯＴＴの位置姿勢を
示す同次変換行列ｗｏｒｌｄＸｔ（ｊ）は、数４で表さ
れた同次変換行列Ｔｔ（ｊ）を用いて、次の数７で表さ
れる。

【数７】 ｗｏｒｌｄＸｔ（ｊ）＝Ｚｔ・Ｔｔ（ｊ）・Ｅｔ ここで、Ｚｔは絶対座標系Σｗｏｒｌｄを基準としたツ
ール移動装置１１の設置基準面（Ｘｂａｓｅ−Ｙｂａｓ
ｅ平面）の位置姿勢を示す同次変換行列であり、同次変
換行列Ｚｔ，Ｅｔのデータは予めＲＯＭ１０１に格納さ
れる。

【００４７】ところで、ワーク基準点ＯＷを基準とした
ツール基準点ＯＴＴの位置姿勢を示す同次変換行列をｗ
Ｘｔ（ｊ）とすると、絶対座標系Σｗｏｒｌｄを基準と
したツール基準点ＯＴＴの位置姿勢を示す同次変換行列
ｗｏｒｌｄＸｔ（ｊ）は、次の数８で表される。

【数８】 ｗｏｒｌｄＸｔ（ｊ）＝ｗｏｒｌｄＸｗ（ｊ）・ｗＸｔ（ｊ）

【００４８】従って、上記数８から、同次変換行列ｗＸ
ｔ（ｊ）は次の数９で表される。

【数９】 ｗＸｔ（ｊ）＝ｗｏｒｌｄＸｗ（ｊ）^-1・ｗｏｒｌｄＸｔ（ｊ）

【００４９】さらに、教示作業処理において、前の第
（ｊ−１）教示点から今回教示する第ｊ教示点に向って
動くときの、ワーク１に対するツール２の並進速度Ｖ
（ｊ）が操作者によってテンキーＳＷ７を用いて入力さ
れる。そして、上記数６によって計算された同次変換行
列ｗｏｒｌｄＸｗ（ｊ）と、上記数９によって計算され
た同次変換行列ｗＸｔ（ｊ）と、上記入力された並進速
度Ｖ（ｊ）が、１組の第ｊ教示データＴＤ（ｊ）として
ＲＡＭ１０２に格納される。以上の操作を必要な教示点
の数だけ繰り返して、作業プログラムが生成されて、１
５図に示すように、ＲＡＭ１０２に格納される。

【００５０】（３−２）再生動作 以下、第ｊ教示点の位置まで再生動作処理が完了した
後、これから第ｊ＋１教示点の位置に向って、ワーク１
に対してツール２が移動する場合について説明する。

【００５１】まず、ワーク基準点ＯＷに対するツール基
準点ＯＴＴの運動については、｛ｗＸｔ（ｊ），ｗｏｒ
ｌｄＸｗ（ｊ），Ｖ（ｊ）｝からなる第ｊ教示データＴ
Ｄ（ｊ）と、｛ｗＸｔ（ｊ＋１）、ｗｏｒｌｄＸｗ（ｊ
＋１）、Ｖ（ｊ＋１）｝からなる第ｊ＋１教示データＴ
Ｄ（ｊ＋１）のうち、同次変換行列ｗＸｔ（ｊ）と、同
次変換行列ｗＸｔ（ｊ＋１）と、並進速度Ｖ（ｊ＋１）
を使って下記の計算処理を行う。

【００５２】ここで、第ｊ教示点における同次変換行列
ｗＸｔ（ｊ）は次の数１０で表される。

【数１０】

【００５３】数１０の右辺の行列において、左上に位置
する１行１列から３行３列までの９個のデータは、第ｊ
教示点におけるワーク基準点ＯＷを基準としたツール１
の姿勢、すなわちツール基準点ＯＴＴに設けられた座標
系の姿勢を表わす。また４列の１行から３行までのデー
タは、第ｊ教示点におけるワーク基準点ＯＷに対するツ
ール１のツール基準点ＯＴＴの位置を表わす。

【００５４】また、第ｊ＋１教示点における同次変換行
列ｗＸｔ（ｊ＋１）は、数１０と同様に、次の数１１で
表される。

【数１１】

【００５５】従って、数１０と数１１より、第ｊ教示点
から第ｊ＋１教示点へのツール基準点ＯＴＴの並進移動
の増分量を示す同次変換行列（以下、並進増分量行列と
いう。）δｐｔ（ｊ＋１）は、次の数１２で表される。

【数１２】

【００５６】ここで、第ｊ教示点から第ｊ＋１教示点へ
のツール基準点ＯＴＴの並進移動における並進速度はデ
ータＶ（ｊ＋１）として入力されて格納されているの
で、当該並進移動の所要時間Ｔ（ｊ＋１）と、単位時間
当たりのツール基準点ＯＴＴの並進増分量行列△ｐｔ
（ｊ＋１）はそれぞれ、次の数１３および数１４で表さ
れる。

【数１３】 Ｔ（ｊ＋１）＝｜δｐｔ（ｊ＋１）｜／Ｖ（ｊ＋１）

【数１４】

【００５７】一方、ワーク１とツール２の姿勢の変化に
ついては第ｊ教示点から第ｊ＋１教示点に向う、溶接ト
ーチであるツール２の回転運動を次のように仮定して計
算処理を行う。すなわち、ツール２が、所定の回転中心
ベクトルＫｒｔ（ｊ）の回りに、角度ψｔ（ｊ）だけ回
転することによって、第ｊ教示点におけるツール２の姿
勢が第ｊ＋１教示点におけるツール２の姿勢となると仮
定する。この場合、当該回転運動を示す同次変換行列Ｒ
ｏｔ（Ｋｒｔ（ｊ），ψｔ（ｊ））は次の数１５で表さ
れる。

【数１５】

【００５８】ここで、数１５の右辺の計算結果を次の数
１６のようにおく。

【数１６】

【００５９】次いで、上記所定の回転中心ベクトルＫｒ
ｔ（ｊ）のＸ成分Ｋｒｔｘ（ｊ）、Ｙ成分Ｋｒｔｙ
（ｊ）及びＺ成分Ｋｒｔｚ（ｊ）と、上記角度ψｔ
（ｊ）はそれぞれ、次の数１７乃至数１９によって計算
される。

【数１７】 Ｋｒｔｘ（ｊ）＝（Ｏｚｊ−Ｃｚｊ）／［２・ｓｉｎ(ψｔ（ｊ）)］

【数１８】 Ｋｒｔｙ（ｊ）＝（Ｃｘｊ−Ｎｚｊ）／［２・ｓｉｎ(ψｔ（ｊ）)］

【数１９】 Ｋｒｔｚ（ｊ）＝（Ｎｙｊ−Ｏｘｊ）／［２・ｓｉｎ(ψｔ（ｊ）)］ ここで、ψｔ（ｊ）は次の数２０で表される。

【数２０】 ψｔ（ｊ）＝ｃｏｓ^-1［（Ｎｘｊ＋Ｏｙｊ＋Ｃｚｊ−１）／２］， ただし、０≦ψｔ（ｊ）≦πである。

【００６０】従って、ツール基準点の単位時間当たりの
回転増分量を示す同次変換行列（以下、回転増分量行列
という。）はψｔ（ｊ）／Ｔ（ｊ＋１）となる。これに
よって、単位時間当たりのツール基準点ＯＴＴの姿勢変
化を示す同次変換行列ΔＲｔ（ｊ）は、上記数１７乃至
数１９によって計算される回転中心ベクトルＫｒｔ
（ｊ）の回りに、上記回転運動が生じるので、次の数２
１で表される。

【数２１】 △Ｒｔ（ｊ）＝Ｒｏｔ（Ｋｒｔ（ｊ），ψｔ（ｊ）／Ｔ(ｊ＋１)）

【００６１】次に、絶対座標系Σｗｏｒｌｄを基準とし
たワーク基準点ＯＷの運動について説明する。

【００６２】同次変換行列ｗｏｒｌｄＸｗ（ｊ）と同次
変換行列ｗｏｒｌｄＸｗ（ｊ＋１）とから、ワーク基準
点ＯＷの単位時間当たりの並進増分量行列△ｐｗ（ｊ＋
１）と回転増分量行列△Ｒｗ（ｊ）を、次のように求め
ることができる。この場合、ワーク１の並進速度が与え
られていないので、数１３を用いて計算された所要時間
Ｔ（ｊ＋１）を使って求める。すなわち、同次変換行列
ｗｏｒｌｄＸｗ（ｊ）と同次変換行列ｗｏｒｌｄＸｗ
（ｊ＋１）はそれぞれ、次の数２２と数２３で表すこと
ができる。

【数２２】

【数２３】

【００６３】従って、ワーク１についての単位時間当た
りの並進増分量行列Δｐｗ（ｊ＋１）と、回転増分量行
列ΔＲｗ（ｊ）は、数１４と数２１と同様に、次の数２
４と数２５で表される。

【数２４】

【数２５】 △Ｒｗ（ｊ）＝Ｒｏｔ（Ｋｒｗ（ｊ），ψｗ（ｊ）／Ｔ（ｊ＋１））

【００６４】ここで、ワーク１の回転中心ベクトルＫｒ
ｗ（ｊ）と回転角ψｗ（ｊ）は、ツール２の場合におけ
る数１５と同様に、次の数２６で表される。

【数２６】

【００６５】以上の処理で、第ｊ教示点から第ｊ＋１教
示点に向うワーク１とツール２の軌道の計画処理は終了
する。

【００６６】次いで、第ｊ教示点から第ｊ＋１教示点に
向うワーク１とツール２の軌道を分割された各補間点の
所要時間が互いに同一となるように所定数に分割して、
各分割された補間点を以下に示す補間の方法で求める。
以下、第ｊ教示点から第ｊ＋１教示点に向う軌道におい
て第ｉ−１補間点から第ｉ補間点に向かうときの第ｉ補
間点を求めるための処理について説明する。

【００６７】まず、分割された１補間期間の時間間隔を
△ｔとすれば、第ｉ補間点までの経過時間Ｔｐｉは、次
の数２７で表される。

【数２７】Ｔｐｉ＝△ｔ×ｉ

【００６８】そして、第ｉ補間点での絶対座標系Σｗｏ
ｒｌｄを基準としたワーク基準点ＯＷの位置姿勢の同次
変換行列ｗｏｒｌｄＸｗ（ｊ，ｉ）は次の数２８で表さ
れる。

【数２８】 ｗｏｒｌｄＸｗ（ｊ，ｉ） ＝ｗｏｒｌｄＸｗ（ｊ）・△ｐｗ（Ｔｐｉ）・Ｒｏｔ
（Ｋｒｗ（ｊ），Ｔｐｉ・ψｗ（ｊ）／Ｔ（ｊ＋１））

【００６９】ここで、経過時間Ｔｐｉにおける単位時間
当たりのワーク基準点ＯＷの並進増分量行列Δｐｗ（Ｔ
ｐｉ）は次の数２９で表される。

【数２９】

【００７０】同様にして、第ｉ補間点でのワーク基準点
ＯＷを基準としたツール基準点ＯＴＴの位置姿勢を示す
同次変換行列ｗＸｔ（ｊ，ｉ）は、

【数３０】 ｗＸｔ（ｊ，ｉ） ＝ｗＸｔ（ｊ）・△ｐｔ（Ｔｐｉ） ・Ｒｏｔ（Ｋｒｔ（ｊ），Ｔｐｉ・ψｔ（ｊ）／Ｔ（ｊ＋１））

【００７１】ここで、経過時間Ｔｐｉにおける単位時間
当たりのツール基準点ＯＴＴの並進増分量行列Δｐｔ
（Ｔｐｉ）は次の数３１で表される。

【数３１】

【００７２】ところで、第ｊ教示点から第ｊ＋１教示点
に向う軌道における第ｉ補間点における経過時間Ｔｐｉ
に対応する時刻Ｔ（ｊ，ｉ）において、同次変換行列ｗ
ｏｒｌｄＸｗ（ｊ，ｉ）は数５と同様にして、次の数３
２で表される。

【数３２】 ｗｏｒｌｄＸｗ（ｊ，ｉ）＝Ｚｗ・Ｔｗ（ｊ，ｉ）・Ｅｗ

【００７３】上記数３２から、ワーク取扱装置１２の設
置基準面を基準としたワーク取付基準点ＯＷ６の位置姿
勢を示す同次変換行列である次の数３３を得ることがで
きる。

【数３３】 Ｔｗ（ｊ，ｉ）＝Ｚｗ^-1・ｗｏｒｌｄＸｗ（ｊ，ｉ）・Ｅｗ^-1

【００７４】また、絶対座標系Σｗｏｒｌｄを基準とし
たツール基準点ＯＴＴの位置姿勢を示す数７と数８に対
応してそれぞれ、次の数３４と数３５を得ることができ
る。

【数３４】 ｗｏｒｌｄＸｔ（ｊ，ｉ）＝Ｚｔ・Ｔｔ（ｊ，ｉ）・Ｅｔ

【数３５】 ｗｏｒｌｄＸｔ（ｊ，ｉ）＝ｗｏｒｌｄＸｗ（ｊ，ｉ）・ｗＸｔ（ｊ，ｉ）

【００７５】上記数３４と数３５の各右辺は等しくなる
ので、次の数３６を得ることができる。

【数３６】 Ｚｔ・Ｔｔ（ｊ，ｉ）・Ｅｔ＝ｗｏｒｌｄＸｗ（ｊ，ｉ）・ｗＸｔ（ｊ，ｉ）

【００７６】従って、上記数３６から、ツール移動装置
１１の設置基準面を基準としたツール取付基準点ＯＴ６
の位置姿勢を示す同次変換行列である次の数３７を得る
ことができる。

【数３７】 Ｔｔ(ｊ，ｉ)＝Ｚｔ^-1・Ｚｗ・Ｔｗ(ｊ，ｉ)・Ｅｗ・ｗＸｔ(ｊ，ｉ)・Ｅｔ^-1

【００７７】上記数３７によって表される同次変換行列
Ｔｔ（ｊ，ｉ）と、上記数３３によって表される同次変
換行列Ｔｗ（ｊ，ｉ）をそれぞれ逆変換することによっ
て、ツール移動装置１１とワーク取扱装置１２の各関節
変数θ１乃至θ６，φ１乃至φ６を計算することができ
る。上記計算された各関節変数θ１乃至θ６，φ１乃至
φ６に対応するモータ指令値ＤＰｋを計算して、そのデ
ータがデュアルポートＲＡＭ１０４に格納される。次い
で、デュアルポートＲＡＭ１０４に格納された各データ
が、当該データを上記経過時間Ｔｐｉに達した時点で、
上記同期信号ＳＹＮＣに同期してラッチ回路１０５によ
ってラッチされて、同時にサーボ制御回路１０６，１０
７を介してモータＭＯ１乃至ＭＯ６，ＭＯ１１乃至ＭＯ
１６に出力されて駆動される。以上の処理は、経過時間
Ｔｐｉが経過時間Ｔ（ｊ＋１）と等しくなるまで、すな
わち第ｊ＋１教示点に達するまで繰り返される。

【００７８】なお、第１教示点に向かう運動をするとき
に必要となる第０教示データＴＤ（０）は、ＲＡＭ１０
２内の作業プログラム中には存在しないので、再生動作
処理の開始時における位置姿勢を第０教示データＴＤ
（０）としてＲＡＭ１０２内に格納する。すなわち、デ
ュアルポートＲＡＭ１０４には、現在設定されている各
関節変数θ１乃至θ６，φ１乃至φ６に対応するモータ
指令値ＤＰｋが格納されているので、上述の教示データ
計算処理と同様の方法で、当該処理の開始時において上
記モータ指令値ＤＰｋからツール移動装置１１とワーク
取扱装置１２の各関節変数θ１乃至θ６，φ１乃至φ６
を逆算し、当該関節変数のデータを上記数６と数９に代
入して、同次変換行列ｗｏｒｌｄＸｗ（０）と同次変換
行列ｗＸｔ（０）のデータを計算して、第０教示点のデ
ータである第０教示データＴＤ（０）としてＲＡＭ１０
２に格納される。

【００７９】（４）制御装置の処理 以下、制御装置３によって実行される当該ロボットシス
テムの制御処理について説明する。

【００８０】（４−１）メインルーチン 図１６は制御装置３によって実行されるメインルーチン
のフローチャートである。

【００８１】図１６に示すように、当該制御装置３の電
源がオンされたとき当該メインルーチンの処理が開始さ
れ、まず、ステップＳ１において操作ボックス２１の処
理切り換えスイッチＳＷ１がＢ側（自動運転）に切り換
えられているか否かが判断され、Ｂ側に切り換えられて
いるとき（ステップＳ１においてＹＥＳ）ステップＳ２
に進み、一方、Ａ側に切り換えられているとき（ステッ
プＳ１においてＮＯ）ステップＳ４に進む。ステップＳ
２において、起動スイッチＳＷ２がオンされたか否かが
判断され、オンされたとき（ステップＳ２においてＹＥ
Ｓ）ステップＳ３において図１９の再生動作処理を実行
した後ステップＳ１に戻る。一方、起動スイッチＳＷ２
がオンされていないとき（ステップＳ２においてＮＯ）
そのままステップＳ１に戻る。

【００８２】ステップＳ４、Ｓ５及びＳ６においてはそ
れぞれ、スイッチＳＷ４，ＳＷ５，ＳＷ６がオンされた
か否かが順次判断された後、ステップＳ１に戻る。回転
指示スイッチＳＷ４のいずれか１つのスイッチがオンさ
れたとき（ステップＳ４においてＹＥＳ）ステップＳ７
において図１７の手動操作処理を実行した後、ステップ
Ｓ１に戻る。また、記憶スイッチＳＷ５がオンされたと
き（ステップＳ５においてＹＥＳ）ステップＳ８におい
て図１８の教示データ計算処理を実行した後、ステップ
Ｓ１に戻る。さらに、終了スイッチＳＷ６がオンされた
とき（ステップＳ６においてＹＥＳ）ステップＳ９にお
いて作業プログラム生成処理をを実行した後、ステップ
Ｓ１に戻る。この作業プログラム生成処理においては、
図１５に示すように、ステップＳ８の教示データ計算処
理で計算された一連の作業を示す複数の教示点に関する
教示データＴＤ（ｊ）（ｊ＝１，２，…，ｋ）に対して
１つの作業プログラムを付してＲＡＭ１０２に格納す
る。

【００８３】従って、操作者は次の手順に従って、ま
ず、教示作業を行う。 （ａ）スイッチＳＷ１をＡ側に切り換えた後、ティーチ
ングボックス２０の装置指定スイッチＳＷ３を制御した
い所望の装置側に切り換え、制御する関節に対応したス
イッチＳＷ４の少なくとも１個をオンする。この後、図
１７の手動操作処理が実行される。すなわち、図２３に
示すように、スイッチＳＷ４がオンされた後オフされる
まで、同期信号ＳＹＮＣのパルスに同期して上記指定さ
れた関節が所定の角度だけ回転する。 （ｂ）所望の教示点に達したとき、スイッチＳＷ４をオ
フした後、当該教示点に関する教示データを計算するた
め、記憶スイッチＳＷ５をオンして教示データ計算処理
を実行させる。 （ｃ）上記（ａ）及び（ｂ）の作業を繰り返して複数の
教示点に関する教示データが計算されたとき、終了スイ
ッチＳＷ６をオンして、作業プログラム生成処理を行っ
て、１回の教示作業が終了する。

【００８４】次いで、上記教示作業の後に生成された作
業プログラムに基づいて自動運転を行うときは、スイッ
チＳＷ１をＢ側に切り換えた後、起動スイッチＳＷ２を
オンして再生動作処理を実行する。

【００８５】（４−２）手動操作処理 図１７は図１６の手動操作処理のフローチャートであ
り、図２３及び図２４は手動操作処理の一例を示すタイ
ミングチャートである。これらタイミングチャートにお
いて各処理のパルスは当該処理を実行するタイミングを
示す。

【００８６】図１７に示すように、まず、ステップＳ１
１においてデュアルポートＲＡＭ１０４に格納されてい
る各関節に関するモータ指令値ＤＰｋ，ＤＰｋ＋１０
（ｋ＝１，２，…，６）のデータを取り込み、当該デー
タに対応する関節変数θｋ，φｋを計算する。次いで、
ステップＳ１２において先にオンされた回転指示スイッ
チＳＷ４がオフされたか否かが判断され、オフされたと
き（ステップＳ１２においてＹＥＳ）元のメインルーチ
ンに戻る。一方、回転指示スイッチＳＷ４がオフされて
いないときは（ステップＳ１２においてＮＯ）ステップ
Ｓ１３において、スイッチＳＷ４の１２個のキーのうち
オンされたキーに対応する回転方向と関節に基づいて、
関節変数の変化量Δθｋ，Δφｋを計算する。なお、当
該ステップＳ１３の処理においては、スイッチＳＷ４の
１２個のキーのうちオンされたキー以外のキーに対応す
る関節の関節変数の変化量はゼロに設定される。

【００８７】次いで、ステップＳ１４において、元の関
節変数θｋ，φｋにそれぞれ上記計算された変化量Δθ
ｋ，Δφｋを加算して、関節変数θｋ，φｋとする。さ
らに、ステップＳ１５において、ステップＳ１４で計算
された関節変数θｋ，φｋに対応するモータ指令値ＤＰ
ｋ，ＤＰｋ＋１０を計算してデュアルポートＲＡＭ１０
４に書き込む。さらに、ステップＳ１６において同期信
号ＳＹＮＣが発生されたか否かが判断され、発生されて
いないとき（ステップＳ１６においてＮＯ）ステップＳ
１６の処理を繰り返す。同期信号ＳＹＮＣが発生された
とき（ステップＳ１６においてＹＥＳ）ステップＳ１２
に戻る。このとき、デュアルポートＲＡＭ１０４にセッ
トされたモータ指令値ＤＰｋ，ＤＰｋ＋１０のデータが
同時にラッチ回路１０５によって同時にラッチされた
後、サーボ制御回路１０６，１０７を介して各関節のモ
ータＭＯ１乃至ＭＯ６，ＭＯ１１乃至ＭＯ１６に出力さ
れて同時に駆動される。

【００８８】（４−３）教示データ計算処理 図１８は図１６の教示データ計算処理を示すフローチャ
ートである。

【００８９】図１８に示すように、まず、ステップＳ２
１においてツール移動装置１１の関節変数θｋに基づい
て、ツール移動装置１１の設置基準面（Ｘｂａｓｅ−Ｙ
ｂａｓｅ平面）を基準としたツール取付基準点ＯＴ６の
位置姿勢を示す同次変換行列Ｔｔ（ｊ）を数１と数４を
用いて計算した後、ステップＳ２２においてワーク取扱
装置１２の関節変数φｋに基づいて、ワーク取扱装置１
２の設置基準面（Ｘｗｂａｓｅ−Ｙｗｂａｓｅ平面）を
基準としたワーク取付基準点ＯＷ６の位置姿勢を示す同
次変換行列Ｔｗ（ｊ）を数３と数５を用いて計算する。
次いで、ステップＳ２３において、絶対座標系Σｗｏｒ
ｌｄを基準としたワーク基準点ＯＷの位置姿勢を示す同
次変換行列ｗｏｒｌｄＸｗ（ｊ）を数６を用いて計算し
た後、ステップＳ２４において、絶対座標系Σｗｏｒｌ
ｄを基準としたツール基準点ＯＴＴの位置姿勢を示す同
次変換行列ｗｏｒｌｄＸｔ（ｊ）を数７を用いて計算す
る。さらに、ステップＳ２５においてツール基準点ＯＴ
Ｔの並進速度Ｖ（ｊ）がスイッチＳＷ７を用いて入力さ
れたか否かが判断され、入力されないとき（ステップＳ
２５においてＮＯ）ステップＳ２５の処理を繰り返す。
一方、並進速度Ｖ（ｊ）がテンキーＳＷ７を用いて入力
されたとき（ステップＳ２５においてＹＥＳ）ステップ
Ｓ２６において、上記数６によって計算された同次変換
行列ｗｏｒｌｄＸｗ（ｊ）と、上記数９によって計算さ
れた同次変換行列ｗＸｔ（ｊ）と、上記入力された並進
速度Ｖ（ｊ）が、図１５に示すように、１組の第ｊ教示
データＴＤ（ｊ）としてＲＡＭ１０２に格納される。以
上で、教示データ計算処理が終了し、メインルーチンに
戻る。

【００９０】（４−４）再生動作処理 図１９は図１６の再生動作処理のフローチャートであ
り、当該再生動作処理の一例のタイミングチャートを図
２５乃至図２７に示す。これらタイミングチャートにお
いて各処理のパルスは当該処理を実行するタイミングを
示す。

【００９１】図１９に示すように、まず、ステップＳ３
１においてパラメータｊをゼロにリセットした後、ステ
ップＳ３２においてパラメータｊが０であるか否かが判
断され、ゼロであるとき（ステップＳ３２においてＹＥ
Ｓ）ステップ３４に進み、一方、ゼロでないとき（ステ
ップＳ３２においてＮＯ）ステップ３３に進む。

【００９２】ステップＳ３４においては、再生動作処理
の開始時における位置姿勢を第０教示データＴＤ（０）
としてＲＡＭ１０２内に格納する。すなわち、デュアル
ポートＲＡＭ１０４には、現在設定されている各関節変
数θ１乃至θ６，φ１乃至φ６に対応するモータ指令値
ＤＰｋが格納されているので、上述の教示データ計算処
理と同様の方法で、当該処理の開始時において上記モー
タ指令値ＤＰｋからツール移動装置１１とワーク取扱装
置１２の各関節変数θ１乃至θ６，φ１乃至φ６を逆算
し、当該関節変数のデータを上記数６と数９に代入し
て、同次変換行列ｗｏｒｌｄＸｗ（０）と同次変換行列
ｗＸｔ（０）のデータを計算して、当該データを第０教
示点のデータである第０教示データＴＤ（０）としてＲ
ＡＭ１０２に格納される。次いで、ステップＳ３５にお
いて、ＲＡＭ１０２に格納された作業プログラムから第
１教示データＴＤ（１）を取り込んだ後、ステップＳ３
６に進む。

【００９３】一方、ステップＳ３３においては、ＲＡＭ
１０２に格納された作業プログラムから第ｊ教示データ
ＴＤ（ｊ）と第ｊ＋１教示データＴＤ（ｊ＋１）を取り
込んだ後、ステップＳ３６に進む。

【００９４】次いで、ステップＳ３６において図２０の
増分量計算処理を実行した後、ステップＳ３７において
図２１のモータ制御処理を実行して各関節のモータを駆
動する。さらに、ステップＳ３８においてパラメータｊ
を１だけインクリメントした後、ステップＳ３９におい
てＲＡＭ１０２において第ｊ＋１教示データが存在する
か否かが判断され、存在するときは（ステップＳ３９に
おいてＹＥＳ）、さらに次の教示点への移動処理を実行
するため、ステップＳ３２に戻る。一方、ＲＡＭ１０２
において第ｊ＋１教示データが存在しないとき（ステッ
プＳ３９においてＮＯ）、当該再生動作処理が完了した
と判断してメインルーチンに戻る。

【００９５】（４−５）増分量計算処理 図２０は図１６の増分量計算処理のフローチャートであ
る。図２０に示すように、まず、ステップＳ４１におい
て、第ｊ教示点から第ｊ＋１教示点まで移動するとき
の、ワーク基準点ＯＷを基準としたツール基準点ＯＴＴ
の並進移動の所要時間Ｔ（ｊ＋１）と、ワーク基準点Ｏ
Ｗを基準とした単位時間当たりのツール基準点ＯＴＴの
並進増分量行列△ｐｔ（ｊ＋１）をそれぞれ、数１３と
数１４を用いて計算する。次いで、ステップＳ４２にお
いて、単位時間当たりのツール基準点ＯＴＴの回転増分
量行列△Ｒｔ（ｊ）を数２１を用いて計算する。さら
に、ステップＳ４３において、絶対座標系Σｗｏｒｌｄ
を基準としたワーク基準点ＯＷの単位時間当たりの並進
増分量行列△ｐｗ（ｊ＋１）を数２４を用いて計算した
後、ステップＳ４４において絶対座標系Σｗｏｒｌｄを
基準としたワーク基準点ＯＷの単位時間当たりの回転増
分量行列△Ｒｗ（ｊ）を数２５を用いて計算する。以上
で増分量計算処理が終了し、すなわち第ｊ教示点から第
ｊ＋１教示点までの軌道の計画処理が終了し、メインル
ーチンに戻る。

【００９６】（４−６）モータ制御処理 図２１は図１６のモータ制御処理のフローチャートであ
る。図２１に示すように、まず、ステップＳ５１におい
て補間点の通し番号を示すパラメータｉをゼロにリセッ
トした後、ステップＳ５２においてパラメータｉを１だ
けインクリメントした後、ステップＳ５３において経過
時間Ｔｐｉを数２７を用いて計算する。次いで、以下の
ステップＳ５４乃至Ｓ５９において、当該経過時間Ｔｐ
ｉだけ経過した第ｉ補間点における各行列、関節変数及
びモータ指令値を計算する。

【００９７】すなわち、ステップＳ５４において、絶対
座標系Σｗｏｒｌｄを基準としたワーク基準点ＯＷ６の
位置姿勢の同次変換行列ｗｏｒｌｄＸｗ（ｊ，ｉ）を数
２８を用いて計算した後、ステップＳ５５においてワー
ク基準点ＯＷを基準としたツール基準点ＯＴＴの位置姿
勢を示す同次変換行列ｗＸｔ（ｊ，ｉ）を数３０を用い
て計算する。次いで、ステップＳ５６においてワーク取
扱装置１２の設置基準面を基準としたワーク取付基準点
ＯＷ６の位置姿勢を示す同次変換行列Ｔｗ（ｊ，ｉ）を
数３３を用いて計算した後、ステップＳ５７において、
ツール移動装置１１の設置基準面を基準としたツール取
付基準点ＯＴ６の位置姿勢を示す同次変換行列Ｔｔ
(ｊ，ｉ)を数３７を用いて計算する。さらに、ステップ
Ｓ５８において、上記計算された同次変換行列Ｔｗ
（ｊ，ｉ），Ｔｔ(ｊ，ｉ)をそれぞれ逆変換することに
よって、ツール移動装置１１とワーク取扱装置１２の各
関節変数θ１乃至θ６，φ１乃至φ６を計算した後、ス
テップＳ５９において、上記計算された各関節変数θ１
乃至θ６，φ１乃至φ６に対応するモータ指令値ＤＰｋ
を計算して、そのデータがデュアルポートＲＡＭ１０４
に格納される。

【００９８】次いで、ステップＳ６０において同期信号
ＳＹＮＣが発生されたか否かが判断され、発生されてい
ないとき（ステップＳ６０においてＮＯ）ステップＳ６
０の処理を繰り返す。一方、同期信号ＳＹＮＣが発生さ
れたとき（ステップＳ６０においてＹＥＳ）、ステップ
Ｓ６１に進む。このとき、上記経過時間Ｔｐｉに達して
おり、デュアルポートＲＡＭ１０４に格納された各デー
タが上記同期信号ＳＹＮＣに同期してラッチ回路１０５
によって同時にラッチされて、同時にサーボ制御回路１
０６，１０７を介してモータＭＯ１乃至ＭＯ６，ＭＯ１
１乃至ＭＯ１６に同時に出力されて駆動される。

【００９９】さらに、ステップＳ６１において経過時間
Ｔｐｉが所要時間Ｔ（ｊ＋１）よりも小さいか否かが判
断され、Ｔｐｉ＜Ｔ（ｊ＋１）のとき（ステップＳ６１
においてＹＥＳ）、さらに、ステップＳ５２からＳ６０
までの処理を行うため、ステップＳ５２に戻る。一方、
Ｔｐｉ≧Ｔ（ｊ＋１）のとき（ステップＳ６１において
ＮＯ）、ツール基準点ＯＴＴが第ｊ＋１教示点に達した
と判断し、当該モータ制御処理を終了し、メインルーチ
ンに戻る。

【０１００】上記ステップＳ５９の処理においてＣＰＵ
１００による各モータ指令値ＤＰｋ，ＤＰｋ＋１０の計
算処理は、時間的にずれて行われるが、上記計算されて
デュアルポートＲＡＭ１０４に格納されたこれらのデー
タは、上述のように、クロック発生器１１０と分周器１
１１によって発生される同期信号ＳＹＮＣに同期してラ
ッチ回路１０５によって同時にラッチされた後、同時に
サーボ制御回路１０６，１０７を介してモータＭＯ１乃
至ＭＯ６，ＭＯ１１乃至ＭＯ１６に出力されて駆動され
る。従って、各関節のモータを同時に駆動することがで
き、ツール移動装置１１とワーク取扱装置１２とを協調
動作をさせながら上記ツール２とワーク１とを一定の相
対速度で、連続する２個の教示点間の所望の軌道上で移
動させることができる。

【０１０１】以上説明したように、本実施例において
は、１つの制御装置３を用いてツール移動装置１１とワ
ーク取扱装置１２の各関節を、同一の同期信号ＳＹＮＣ
を用いて制御しているので、再生動作時のツール２とワ
ーク１間の相対速度を一定にすることができる。例え
ば、当該ロボットシステムを溶接作業に用いる場合、片
方のマニピュレータに溶接トーチを、他方のマニピュレ
ータまたはポジショナにはワークを把持させて協調動作
を行わせることにより、複雑な部材に対しても、ワーク
の姿勢をかえながら溶接トーチの位置と姿勢を制御しか
つ溶接速度を一定に保つことが可能となり、理想的な溶
接が実現できる。なお、ワークとツールの相対的関係を
必要とする作業、例えば塗布作業等にも有効に利用でき
る。

【０１０２】（５）変形例 以上の実施例においてワーク１を移動させるための教示
データとして、絶対座標系Σｗｏｒｌｄを基準としたワ
ーク基準点ＯＷの位置姿勢を示す同次変換行列ｗｏｒｌ
ｄＸｗを用いているが、本発明はこれに限らず、これに
代えて、図１４に示すように、ワーク取扱装置１２の設
置基準面（Ｘｗｂａｓｅ−Ｙｗｂａｓｅ平面）を基準と
したワーク基準点ＯＷの位置姿勢を示す同次変換行列ｗ
ｂＸｗを用いてもよい。以下、この場合において用いら
れる数式について説明する。

【０１０３】この場合、ワーク基準点ＯＷの位置姿勢を
表す数５に対応する同次変換行列は、次の数３８で表さ
れる。

【数３８】ｗｂＸｗ（ｊ）＝Ｔｗ（ｊ）・Ｅｗ

【０１０４】次に、絶対座標系Σｗｏｒｌｄを基準とし
たワーク基準点ＯＷの位置姿勢は次の数３９で表され
る。

【数３９】 ｗｏｒｌｄＸｗ（ｊ）＝Ｚｗ・ｗｂＸｗ（ｊ）

【０１０５】さらに、ワーク基準点ＯＷを基準としたツ
ール基準点ＯＴＴの位置姿勢を示す同次変換行列ｗＸｔ
（ｊ）は上記数３９の右辺を数９に代入することによっ
て次の数４０を得る。

【数４０】 ｗＸｔ（ｊ）＝ｗｂＸｗ（ｊ）^-1・Ｚｗ^-1・ｗｏｒｌｄＸｔ（ｊ）

【０１０６】そして、数６の右辺を数４０に代入するこ
とによって次の数４１を得る。

【数４１】 ｗＸｔ（ｊ）＝（ｗｂＸｗ（ｊ））^-1・Ｚｗ^-1・Ｚｔ・Ｔｔ（ｊ）・Ｅｔ ここで、図１４に示されたｔｂＸｔ（ｊ）は次の数４２
によって表される。

【数４２】ｔｂＸｔ（ｊ）＝Ｔｔ（ｊ）・Ｅｔ

【０１０７】上述の実施例において教示データとして用
いられている同次変換行列ｗｏｒｌｄＸｗは、当該変形
例において、ワーク取扱装置１２の設置基準面を基準と
したワーク基準点ＯＷの位置姿勢を示す同次変換行列ｗ
ｂＸｗに置き換えられる。すなわち、数２２の左辺はｗ
ｂＸｗ（ｊ）となり、数２３の左辺はｗｂＸｗ（ｊ＋
１）となる。従って、数２２に対応する同次変換行列ｗ
ｂＸｗ（ｊ，ｉ）は次の数４３で表される。

【数４３】 ｗｂＸｗ（ｊ，ｉ） ＝ｗｂＸｗ（ｊ）・△ｐｗ（Ｔｐｉ） ・Ｒｏｔ（Ｋｒｗ（ｊ），Ｔｐｉ・ψｗ（ｊ）／Ｔ（ｊ＋１））

【０１０８】さらに、ワーク取付基準点ＯＷ６の位置姿
勢を示す数３３は次の数４４の同次変換行列Ｔｗ（ｊ，
ｉ）に置き換えられる。

【数４４】 Ｔｗ（ｊ，ｉ）＝ｗｂＸｗ（ｊ，ｉ）・Ｅｗ^-1

【０１０９】以上の実施例において、ツール移動装置１
１とワーク取扱装置１２はそれぞれ、６自由度のマニピ
ュレータにてなるが、本発明はこれに限らず、ワーク取
扱装置１２は少なくとも１自由度のマニピュレータを有
すればよく、また、ツール移動装置１１は少なくとも６
自由度のマニピュレータを有すればよい。

【０１１０】以上の実施例において、ラッチ回路１０５
を用いているが、本発明はこれに限らず、ＭＰＵで構成
してもよい。当該ＭＰＵはデュアルポートＲＡＭ１０４
から高速でモータ指令値のデータを取り込んだのち、サ
ーボ制御回路１０６，１０７に高速で出力する。

【０１１１】

【発明の効果】以上詳述したように本発明に係る産業用
ロボットシステムの制御方法及び制御装置によれば、ワ
ークと上記ワークを移動させる第１の関節手段とを有す
るワーク取扱装置と、ツールと上記ツールを移動させる
第２の関節手段とを有するツール移動装置とを備えた産
業用ロボットシステムにおいて、連続する２個の教示点
間の軌道上で上記ワークと上記ツールを移動させるため
の制御方法及び制御装置であって、上記２個の教示点に
ついての、所定のワーク基準点を基準とした上記ツール
の位置姿勢と、所定の基準座標系を基準とした上記ワー
クの位置姿勢と、上記２個の教示点間の軌道を上記ツー
ルが並進するときの上記ワーク基準点を基準とした上記
ツールの所定の基準点の並進速度とに基づいて、上記２
個の教示点間の軌道を所定数に分割して得られる複数の
補間点における、上記基準座標系を基準とした上記ワー
クの位置姿勢と、上記ワーク基準点を基準とした上記ツ
ールの位置姿勢とを所定の補間方法によって計算し、上
記ワーク取扱装置の設置基準面を基準とした上記ワーク
の所定の取付基準点の位置姿勢と、上記ツール取扱装置
の設置基準面を基準とした上記ツールの所定の取付基準
点の位置姿勢とを所定の座標変換によって計算し、上記
軌道上で上記ワークと上記ツールを移動させるための上
記第１の関節手段の関節変数と上記第２の関節手段の関
節変数とを所定の逆変換によって計算し、上記計算され
た上記第１の関節手段の関節変数と上記第２の関節手段
の関節変数とに基づいて、上記第１と第２の関節手段を
同時に駆動する。すなわち、上記計算された上記第１の
関節手段の関節変数と上記第２の関節手段の関節変数と
に基づいて、上記第１と第２の関節手段を同時に駆動す
るので、上記連続する２個の教示点間の所望の軌道上
で、上記ツールと上記ワークを一定の相対速度で協調動
作をさせて移動させることができる。

【０１１２】従って、本発明は以下の特有の効果を有す
る。 （ａ）複数の補間点における制御データを計算して上記
第１と第２の関節手段をそれぞれ同時に同期させて駆動
するので、細かく教示する必要がなく、教示点の数を少
なくすることができ、教示データを入力する教示作業を
操作者にとって容易にかつ迅速に行うことができる。こ
れによって、教示データを記憶する記憶装置の記憶容量
も小さくでき、装置コストを低減することができる。 （ｂ）上記連続する２個の教示点間の所望の軌道上で、
上記ツールと上記ワークを一定の相対速度で移動させる
ことができるので、所定のワーク基準点に対する例えば
ツール先端の基準点の移動速度を指示通り再生すること
が可能となり、協調動作をより確実に実現できる。 （ｃ）１個の制御装置を用いてツール移動装置１１とワ
ーク取扱装置１２とを制御するので、２個の制御装置間
の通信は必要がなく、「高速な情報交換の通信」の問題
が回避することができる。

【図面の簡単な説明】

【図１】 本発明に係る一実施例の産業用ロボットシス
テムの全体を示す斜視図である。

【図２】 図１の産業用ロボットシステムの制御装置の
ブロック図である。

【図３】 図１の産業用ロボットシステムの各関節の構
成を示す斜視図及び制御装置のサーボ制御回路のブロッ
ク図である。

【図４】 図２の操作ボックスの正面図である。

【図５】 図２のティーチングボックスの正面図であ
る。

【図６】 図１のツール移動装置の構成を示す斜視図で
ある。

【図７】 図１のワーク取扱装置の構成を示す斜視図で
ある。

【図８】 図６のツール移動装置に設定された各座標系
を示す斜視図である。

【図９】 図８のツール移動装置の第１の部分に設定さ
れた各座標系の詳細を示す斜視図である。

【図１０】 図８のツール移動装置の第２の部分に設定
された各座標系の詳細を示す斜視図である。

【図１１】 図８のツール移動装置の台座と第１リンク
に設定された各座標系の詳細を示す側面図である。

【図１２】 図８のツールに設定された各座標系の詳細
を示す側面図である。

【図１３】 図１の産業用ロボットシステムに設定され
た各座標系の間の関係を示す模式側面図である。

【図１４】 変形例の産業用ロボットシステムに設定さ
れた各座標系の間の関係を示す模式側面図である。

【図１５】 図２のＲＡＭに格納される作業プログラム
の構成を示す図である。

【図１６】 図２の制御装置によって実行されるメイン
ルーチンを示すフローチャートである。

【図１７】 図１６のサブルーチンである手動操作処理
を示すフローチャートである。

【図１８】 図１６のサブルーチンである教示データ計
算処理を示すフローチャートである。

【図１９】 図１６のサブルーチンである再生動作処理
を示すフローチャートである。

【図２０】 図１９のサブルーチンである増分量計算処
理を示すフローチャートである。

【図２１】 図１９のサブルーチンであるモータ制御処
理の第１の部分を示すフローチャートである。

【図２２】 図１９のサブルーチンであるモータ制御処
理の第２の部分を示すフローチャートである。

【図２３】 図１７の手動操作処理の一例の第１の期間
における図２の制御装置の動作を示すタイミングチャー
トである。

【図２４】 図１７の手動操作処理の一例の第２の期間
における図２の制御装置の動作を示すタイミングチャー
トである。

【図２５】 図１９の再生動作処理の一例の第１の期間
における図２の制御装置の動作を示すタイミングチャー
トである。

【図２６】 図１９の再生動作処理の一例の第２の期間
における図２の制御装置の動作を示すタイミングチャー
トである。

【図２７】 図１９の再生動作処理の一例の第３の期間
における図２の制御装置の動作を示すタイミングチャー
トである。

【図２８】 従来例の産業用ロボットシステムにおいて
ワークの姿勢を変化させながら溶接トーチであるツール
を移動させる場合の一例を示す正面図である。

【図２９】 図２８の場合においてＰ１点からＰ２点ま
での楕円を直線近似することによってツールを移動させ
る動作を示す正面図である。

【符号の説明】

１…ワーク、 ２…ツール、 ３…制御装置、 １１…ツール移動装置、 １２…ワーク取扱装置、 ２０…ティーチングボックス、 ２１…操作ボックス、 １００…ＣＰＵ、 １０１…ＲＯＭ、 １０２…ＲＡＭ、 １０３…インターフェース、 １０４…デュアルポートＲＡＭ、 １０５…ラッチ回路、 １０６，１０７…サーボ制御回路、 １１０…クロック発生器、 １１１…分周器、 ＲＪ１乃至ＲＪ６，ＲＪ１１乃至ＲＪ１６…関節、 Ｌ０乃至Ｌ６，Ｌ１０乃至Ｌ１６…リンク、 ＭＯ１乃至ＭＯ６，ＭＯ１１乃至ＭＯ１６…モータ、 ＳＮ１乃至ＳＮ６，ＳＮ１１乃至ＳＮ１６…センサ、 ＲＥ１乃至ＲＥ６，ＲＥ１１乃至ＲＥ１６…減速機。

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05B 19/18 - 19/46 B25J 9/00 - 9/22

Claims (5)

(57)【特許請求の範囲】
1. 【請求項１】 ワークと上記ワークを移動させる第１の
   関節手段とを有するワーク取扱装置と、ツールと上記ツ
   ールを移動させる第２の関節手段とを有するツール移動
   装置とを備えた産業用ロボットシステムにおいて、連続
   する２個の教示点間の軌道上で上記ワークと上記ツール
   を移動させるための制御方法であって、（ａ）上記２個
   の教示点についての、所定のワーク基準点を基準とした
   上記ツールの位置姿勢と、所定の基準座標系を基準とし
   た上記ワークの位置姿勢と、上記２個の教示点間の軌道
   を上記ツールが並進するときの上記ワーク基準点を基準
   とした上記ツールの所定の基準点の並進速度とに基づい
   て、上記２個の教示点間の軌道を所定数に分割して得ら
   れる複数の補間点における、上記基準座標系を基準とし
   た上記ワークの位置姿勢と、上記ワーク基準点を基準と
   した上記ツールの位置姿勢とを所定の補間方法によって
   計算するステップと、（ｂ）上記計算された上記基準座
   標系を基準とした上記ワークの位置姿勢と、上記ワーク
   基準点を基準とした上記ツールの位置姿勢とに基づい
   て、上記ワーク取扱装置の設置基準面の座標系を基準と
   した上記ワークの所定の取付基準点の位置姿勢と、上記
   ツール取扱装置の設置基準面の座標系を基準とした上記
   ツールの所定の取付基準点の位置姿勢とを所定の座標変
   換によって計算するステップと、（ｃ）上記計算された
   上記ワークの所定の取付基準点の位置姿勢と、上記ツー
   ルの所定の取付基準点の位置姿勢とに基づいて、上記軌
   道上で上記ワークと上記ツールを移動させるための上記
   第１の関節手段の関節変数と上記第２の関節手段の関節
   変数とを所定の逆変換によって計算するステップと、
   （ｄ）上記計算された上記第１の関節手段の関節変数と
   上記第２の関節手段の関節変数とに基づいて、上記第１
   と第２の関節手段を同時に駆動するステップとを含むこ
   とを特徴とする産業用ロボットシステムの制御方法。
2. 【請求項２】 上記基準座標系は、所定の絶対座標系又
   は上記ワーク取扱装置の設置基準面の座標系であること
   を特徴とする請求項１記載の制御方法。
3. 【請求項３】 ワークと上記ワークを移動させる第１の
   関節手段とを有するワーク取扱装置と、ツールと上記ツ
   ールを移動させる第２の関節手段とを有するツール移動
   装置とを備えた産業用ロボットシステムにおいて、連続
   する２個の教示点間の軌道上で上記ワークと上記ツール
   を移動させるための制御装置であって、（ａ）上記２個
   の教示点についての、所定のワーク基準点を基準とした
   上記ツールの位置姿勢と、所定の基準座標系を基準とし
   た上記ワークの位置姿勢と、上記２個の教示点間の軌道
   を上記ツールが並進するときの上記ワーク基準点を基準
   とした上記ツールの所定の基準点の並進速度とに基づい
   て、上記２個の教示点間の軌道を所定数に分割して得ら
   れる複数の補間点における、上記基準座標系を基準とし
   た上記ワークの位置姿勢と、上記ワーク基準点を基準と
   した上記ツールの位置姿勢とを所定の補間方法によって
   計算する第１の計算手段と、（ｂ）上記第１の計算手段
   によって計算された上記基準座標系を基準とした上記ワ
   ークの位置姿勢と、上記ワーク基準点を基準とした上記
   ツールの位置姿勢とに基づいて、上記ワーク取扱装置の
   設置基準面の座標系を基準とした上記ワークの所定の取
   付基準点の位置姿勢と、上記ツール取扱装置の設置基準
   面の座標系を基準とした上記ツールの所定の取付基準点
   の位置姿勢とを所定の座標変換によって計算する第２の
   計算手段と、（ｃ）上記第２の計算手段によって計算さ
   れた上記ワークの所定の取付基準点の位置姿勢と、上記
   ツールの所定の取付基準点の位置姿勢とに基づいて、上
   記軌道上で上記ワークと上記ツールを移動させるための
   上記第１の関節手段の関節変数と上記第２の関節手段の
   関節変数とを所定の逆変換によって計算する第３の計算
   手段と、（ｄ）上記第３の計算手段によって計算された
   上記第１の関節手段の関節変数と上記第２の関節手段の
   関節変数とに基づいて、上記第１と第２の関節手段を同
   時に駆動する駆動手段とを備えたことを特徴とする産業
   用ロボットシステムの制御装置。
4. 【請求項４】 上記駆動手段は、（ａ）所定の周期を有
   する同期信号を発生する信号発生手段と、（ｂ）上記第
   ３の計算手段によって計算された上記第１の関節手段の
   関節変数と上記第２の関節手段の関節変数とを、上記信
   号発生手段によって発生された同期信号に同期して同時
   に記憶して出力する同期出力手段と、（ｃ）上記同期出
   力手段から出力される上記第１の関節手段の関節変数と
   上記第２の関節手段の関節変数とに基づいて上記第１と
   第２の関節手段を駆動する駆動制御手段とを備えたこと
   を特徴とする請求項３記載の制御装置。
5. 【請求項５】 上記基準座標系は、所定の絶対座標系又
   は上記ワーク取扱装置の設置基準面の座標系であること
   を特徴とする請求項３又は４記載の制御装置。