JP3351601B2 - 産業用ロボットの手動教示操作における速度制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は産業用ロボットの手動教
示操作における速度制御装置に係り、詳しくは、二基の
ロボットを用いたシステムにおいて、その両ロボットに
「連動する手動操作」によりワークとツールの再現すべ
き動きを教示する際に、ワークやツールの連動教示中の
予測外の速度による動きの発生を防止することができる
ようにした産業用ロボットの教示速度制御装置に関する
ものである。

【０００２】

【従来の技術】一基の産業用ロボットを手動操作によっ
て教示するとき、ロボットの動作としては、関節手動操
作と直角手動操作とが可能である。関節手動操作のとき
には入力データを許容値以内に制限すれば各軸の最高速
度を超えないように制限することができるが、直角手動
操作の場合の動作指定の対象は溶接トーチなどにあっ
て、それを取り付けたエンドエフェクタが挙動する合成
された動きの速度を制限するために、各関節の速度を換
算もしくは逆算しなければならない。そこで、教示中に
制御装置で算出した各関節の速度が許容値を超えるとき
には全ての軸を一様な割合で減少させる方法が、特開平
３−１０８００８号公報や特開平１−２９０００７号公
報に記載されている。これは、ロボットが機械的に許容
されている速度以上で回転して損傷するのを防ぐために
有効である。また、産業用ロボットの動作を教示する作
業においては、通常作業者がロボットアームの動作範囲
内にいるので、上記の許容値を小さく設定することによ
り安心感を作業者に与えることも重要なことである。

【０００３】ところで、近年、複数台の機械による協調
制御が実用化されてきた。そして、協調した動作を教示
するための手動運転の機能として、ワークに対するツー
ルの位置姿勢を変えないように、あたかも追従するかの
ように連動動作させる新しい技術が、特開平５−１６０
８４号公報に提案されている。これは、連動手動教示操
作において、ワークを動かしてもツールがワークから離
反しなく、次の教示点を得る際にワークの位置や姿勢を
変えても、ツールをワークから遠ざける必要がなくなる
利点がある。ツールを次の教示点へもっていくときに、
従来は、単独手動教示操作によってワークの位置姿勢を
変える場合、ワークがツールにぶつからないようにツー
ルを予め単独手動教示操作によって退避させなければな
らないことがあった。その退避動作によってすでに教示
した点を見失ったり、次に教示すべき点がわからなくな
ったりすることがあるが、そのような問題が解決され
る。さらには、次の教示点を得る際にワークの位置や姿
勢を変えても、ワークがツールにぶつかるといった事態
も回避される。

【０００４】ところで、上記の連動関係とは異なるが、
ツールが溶接トーチであるような場合には、溶接トーチ
の大地に対する姿勢が重要である。すなわち、ワークの
上り坂部位を溶接するときや下り坂部位を溶接すると
き、厚い溶接物や薄い溶接物における溶融池の流れの大
小が溶接の品質を左右するからである。それゆえ、溶接
品質を高くするためにツールとワークの姿勢を大地に保
っておきたい場合には、「ワーク取扱装置により位置姿
勢を変化させながらも教示点における姿勢を大地に対し
て一定にしておくワークと、そのワーク取扱装置に協調
動作するツール取扱装置により位置姿勢が変化するツー
ルとの間の動作」の関連づけにおいて、「大地に対する
ツールの姿勢」を一定に保ち、かつ、「ツールのワーク
に対する相対的な位置」を維持するようにしておくこと
が要求される。そのような連動関係を遂行させると、上
記の場合と同じく、ワークを動かしてもツールはワーク
から離反しなく、次の教示点を得る際にワークの位置や
姿勢を変えてもツールをワークから遠ざける必要がな
い。ツールを次の教示点へもっていくときに、その教示
すべきワーク上の位置を見失うこともない。さらには、
次の教示点を得る際にワークの位置や姿勢を変えても、
ツールの大地に対する姿勢が一定であり、溶接トーチを
溶接品質の向上に好適な姿勢に常時保持しておくことが
できる。このようなことを可能にした発明は、特開平６
−１９８５８４号公報に提案されている。

【０００５】上記のように複数の機械を協調させながら
手動運転させる方法は、特開平２−２７１４０２号公報
や特開平４−９２９０５号公報などにも記載されてい
る。こうした協調の手動教示運転では、一基の機械を動
作の対象として速度と動作方向を指定するだけで、他の
機械も従動するので教示作業の効率が向上する。しか
し、こうした技術では、ワークとツールの相対位置が大
きく離れている状態で一方の姿勢を変え、かつ、ワーク
とツールの相対的な位置姿勢を保持する協調動作を行わ
せると、図４４に示すように、主動作機械であるワーク
取扱装置のワーク１ｗが少ししか動かなくても、従動作
機械すなわち他方のツール取扱装置のツール２ｔが思い
のほか高速で動作することになるという問題がある。

【０００６】

【発明が解決しようとする課題】前述したように、協調
動作の教示作業を行う場合に、作業者は機械の動作範囲
内におり、操作の対象とした主動作機械に注意を集中し
て作業を行っている。それゆえに、従動作機械が予期し
ない速度で動作することは作業者の不意をつくことにな
る。従来では、一基の動作機械の速度を制限値までに抑
制することは行われていたが、こうした主動作機械の動
きに関連して生じる従動作機械の速度の制限はなされて
いない。したがって、従動作機械を主動作機械に追従さ
せたり、連動させながらそれぞれを同時に独立して動作
させる場合には、従動作機械の動きも注視しながら主動
作機械を教示操作している。その結果、作業者の注意が
主動作機械と従動作機械のそれぞれに払われることにな
り、作業能率の低下をきたしている問題がある。また、
作業者は不安を感じながら操作しなければならないとい
ったこともあった。さらに、甚だしくは、その不安を避
けるために教示すべき点へそれぞれのエンドエフェクタ
を移動させるとき、予め指令速度を大きく落とすか、安
全と思われる経路をたどるように余分な配慮をしなけれ
ばならないといった問題があった。

【０００７】ところで、連動の教示操作をすることがで
きるロボットシステムには、当然のことながらいずれか
一方のロボットのみを教示する単独操作もできるように
なっている。もちろん、教示された動作を再現して、実
際に製品を製作したり加工することもできるようになっ
ている。このようなシステムを図面をもとに以下に説明
する。図４５および図４６には制御装置の動きの全体が
示されている。まず、教示の指令があり〔Ｓ１０１〕、
所望するロボットの動きを制限すべくそれぞれの許容速
度を入力できる状態にあると〔Ｓ１０２〕、ボタンやキ
ーを叩くなどして許容速度が入力される〔Ｓ１０３〕。
操作ボックスなどにおいて「連動手動操作モード」が指
定されていると〔Ｓ１０４〕、主動作機械として選択さ
れた取扱装置がいずれであるかが判別される〔Ｓ１１
３〕。ワーク取扱装置が選択されていると、次に「直角
手動操作モード」となっているか判別される〔Ｓ１１
４〕。このようにして教示操作のモード特定がなされる
と、ワーク取扱装置が「連動の直角手動操作モード」に
よって教示処理される〔Ｓ１１５〕。ステップ１１４で
ＮＯであれば「関節手動操作モード」であり、ワーク取
扱装置が「連動の関節手動操作モード」によって教示処
理される〔Ｓ１１６〕。ステップ１１３においてワーク
取扱装置でなければ、ステップ１１７を経て、ツール取
扱装置が「連動の直角手動操作モード」によって教示処
理されたり〔Ｓ１１８〕、「連動の関節手動操作モー
ド」によって教示処理される〔Ｓ１１９〕。ステップ１
０４においてＮＯであれば「単独手動操作モード」であ
り、ワーク取扱装置が選択されていると〔Ｓ１０５〕、
モードが「直角手動操作モード」であるかの判別がなさ
れ〔Ｓ１０６〕、ワーク取扱装置が「単独の直角手動操
作モード」によって教示処理されたり〔Ｓ１０７〕、
「単独の関節手動操作モード」で教示処理される〔Ｓ１
０９〕。ステップ１０５でＮＯであれば、ステップ１１
０を経て、ツール取扱装置が「単独の直角手動操作モー
ド」によって教示処理されたり〔Ｓ１１１〕、「単独の
関節手動操作モード」で教示処理される〔Ｓ１１２〕。
いずれにしても、教示の終了命令がなければ該当する教
示処理が繰り返され、終了命令があれば〔Ｓ１０８〕、
その教示は終了されるか一旦停止された状態となる。所
望する教示が全て完了していれば再現自動運転指令を受
けることが可能であり、その指令を受けると〔Ｓ１２
０〕、以後教示に従って再現運転され〔Ｓ１２１〕、ワ
ークがツールによって所定のように加工される。その再
現が何回か繰り返されるなどして、次々とワークが運び
こまれると教示に従って同じことを繰り返す。所定個数
の加工が完了して停止指令があったりすると〔Ｓ１２
２〕、ロボットも停止される。以後に別の教示を行え
ば、それに従って上記した動作が実現される。

【０００８】上記のフローチャート中のＳ１０７，Ｓ１
０９，Ｓ１１１およびＳ１１２の教示処理過程は、一基
のロボットのみを対象にして教示することを意味してい
る。その際に、そのロボットの動く速度を抑制する制御
の例が、図４７ないし図４８のフローチャートと図４９
のタイムチャートや、図５０ないし図５２のフローチャ
ートと図５３のタイムチャートに示される。前者は図４
５のＳ１０７の「ワーク取扱装置の単独直角手動操作モ
ード」における詳細であり、後者は図４６のＳ１１２の
「ツール取扱装置の単独関節手動操作モード」における
詳細である。他のＳ１０９やＳ１１１のものは割愛され
ている。これらのフローチャートの説明は詳しくしない
が、それからも読み取れるように、ワークもしくはツー
ルの速度は許容範囲にとどめられ、教示している作業者
にワークやツールが高速でぶつからないように配慮され
ている。しかし、図４５のフローチャート中のＳ１１
５，Ｓ１１６，Ｓ１１８およびＳ１１９の連動する教示
処理過程においては、一基のロボットの動き速度を制限
しただけでは、必ずしも他方のロボットの速度を許容範
囲におさめることができないので、図４４に示す従動作
機械のツール２ｔが不意に高速で動作するという問題が
生じる。

【０００９】本発明は上記の問題に鑑みなされたもの
で、その目的は、教示操作を協調の手動運転によって行
う場合、操作の対象とした主動作機械に指定した動作速
度と、ツールとワークの相対的な位置姿勢を保持するな
どの連動動作の関係を演算する手段にもとづいて従動作
機械の動作速度を予測し、この値が所定の制限値を超え
る場合には、従動作機械の動作速度が制限値以内に抑制
されるように主動作機械の動作速度を抑制し、これによ
ってロボットを操作する作業者の安全を高く確保するこ
とができる産業用ロボットの手動教示操作における速度
制御装置を提供することである。

【００１０】

【課題を解決するための手段】本発明は、主制御対象物
の位置姿勢を変化させる主取扱装置と従制御対象物の位
置姿勢を変化させる従取扱装置とを有し、主取扱装置を
教示手動操作すると、６自由度を有する従取扱装置が主
取扱装置の動作に関連づけられて協調動作する産業用ロ
ボットの手動教示操作による制御装置に適用される。そ
の特徴とするところは、図１を参照して記述する。な
お、理解を容易にするため図１には示されていないが、
速度などの記号も実施例において表示されたものを掲げ
ておくことにする。教示作業中に位置姿勢が変化する従
制御対象物２ｓの直角座標系における許容移動速度Ｖ
_{s AL}と許容回転速度ω_{s AL}とを設定する従許容速度設定
手段２２や、主制御対象物１ｍの位置姿勢を変化させる
ための動作指示信号Ｓｘ，Ｓｙ，Ｓｚ，Ｓα，Ｓβ，Ｓ
γを出力する動作指示スイッチ手段１２が設けられる。
直角手動操作モードにおいて、主取扱装置１により位置
姿勢が変化する主制御対象物１ｍと、その主取扱装置１
に協調動作する従取扱装置２により位置姿勢が変化する
従制御対象物２ｓとの間の動作の関連づけを記憶すると
共に、動作指示スイッチ手段１２からの動作指示信号Ｓ
ｘ，Ｓｙ，Ｓｚ，Ｓα，Ｓβ，Ｓγにもとづき、主制御
対象物１ｍの直角座標系における移動速度成分指示値Ｖ
ｘ_m ，Ｖｙ_m ，Ｖｚ_m と回転速度成分指示値ωｘ_m ，ω
ｙ_m ，ωｚ_m を演算する速度成分指示値演算部２３を備
えて、主制御対象物１ｍの位置姿勢を変化させる主取扱
装置１の動作を制御すると共に、従制御対象物２ｓの位
置姿勢を変化させる従取扱装置２の動作を制御する直角
手動連動関係制御手段４Ａc がある。その直角手動連動
関係制御手段４Ａc に従い、主取扱装置１と協調動作す
る従取扱装置２によって位置姿勢が変化される従制御対
象物２ｓの直角座標系における合成移動速度換算値Ｖ_S
と合成回転速度換算値ω_S とを求める従速度演算手段２
６と、その従速度演算手段２６によって求められた合成
移動速度換算値Ｖ_S もしくは合成回転速度換算値ω
_S が、従許容速度設定手段２２により設定された許容移
動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}を超えると
き、従速度演算手段２６によって求められた合成移動速
度換算値Ｖ_S もしくは合成回転速度換算値ω_Sが、許容
移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下となる
ように、速度成分指示値演算部２３で演算された移動速
度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m もしくは回転速度
成分指示値ωｘ_m ，ωｙ_m ，ωｚ_m を修正する補正速度
成分指示値従演算手段２７とを備える。その補正速度成
分指示値従演算手段２７は、「従許容速度設定手段２２
により設定された許容移動速度Ｖ_{s AL}」を「従速度演算
手段２６で求められた合成移動速度換算値Ｖ_S 」で除し
た従制御対象物２ｓの合成移動速度比Ｖ_sR、もしくは
「従許容速度設定手段２２により設定された許容回転速
度ω_{s AL}」を「従速度演算手段２６で求められた合成回
転速度換算値ω_S 」で除した従制御対象物２ｓの合成回
転速度比ω_sRを演算し、その合成速度比Ｖ_sR，ω_sRを速
度成分指示値演算部２３で演算した移動速度成分指示値
Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m や回転速度成分指示値ωｘ_m ，
ωｙ_m ，ωｚ_m に乗じて、その速度成分指示値Ｖｘ_m ，
Ｖｙ_m ，Ｖｚ_m ，ωｘ_m ，ωｙ_m ，ωｚ_m を修正する。
その修正された移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖ
ｚ_m もしくは回転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ
_m でもって主制御対象物１ｍの位置姿勢を主取扱装置１
により変化させると共に、従制御対象物２ｓの許容移動
速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下でもって、
その従制御対象物２ｓの位置姿勢を従取扱装置２により
変化させるようにしたことである。

【００１１】

【００１２】また、異なる発明においては、図３を参照
して、教示作業中に位置姿勢が変化する従制御対象物２
ｓの直角座標系における許容移動速度Ｖ_{s AL}と許容回転
速度ω_{s AL}とを設定する従許容速度設定手段２２が設け
られる。関節手動操作モードにおいて、主取扱装置１に
より位置姿勢が変化する主制御対象物１ｍと、その主取
扱装置１に協調動作する従取扱装置２により位置姿勢が
変化する従制御対象物２ｓとの間の動作の関連づけを記
憶すると共に、動作指示スイッチ手段１２からの動作指
示信号Ｓ１，Ｓ２，Ｓ３，Ｓ４，Ｓ５，Ｓ６にもとづ
き、主制御対象物１ｍの各関節の回転速度指示値Ｖ
１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を演算
する関節速度指示値演算部３３を備えて、主制御対象物
１ｍの位置姿勢を変化させる主取扱装置１の動作を制御
すると共に、従制御対象物２ｓの位置姿勢を変化させる
従取扱装置２の動作を制御する関節手動連動関係制御手
段４Ａj がある。その関節手動連動関係制御手段４Ａj
に従い、主取扱装置１と協調動作する従取扱装置２によ
って位置姿勢が変化される従制御対象物２ｓの直角座標
系における合成移動速度換算値Ｖ_S と合成回転速度換算
値ω_S とを求める従速度演算手段２６と、その従速度演
算手段２６によって求められた合成移動速度換算値Ｖ_S
もしくは合成回転速度換算値ω_S が、従許容速度設定手
段２２により設定された許容移動速度Ｖ_{s AL}もしくは許
容回転速度ω_{s AL}を超えるとき、従速度演算手段２６に
よって求められた合成移動速度換算値Ｖ_S もしくは合成
回転速度換算値ω_Sが、許容移動速度Ｖ_{s AL}もしくは許
容回転速度ω_{s AL}以下となるように、関節速度指示値演
算部３３で演算された各関節の回転速度指示値Ｖ１_m ，
Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を修正する補
正速度指示値従演算手段３７とを備える。その補正速度
指示値従演算手段３７は、「従許容速度設定手段２２に
より設定された許容移動速度Ｖ_{s AL}」を「従速度演算手
段２６で求められた合成移動速度換算値Ｖ_S 」で除した
従制御対象物２ｓの合成移動速度比Ｖ_sR、もしくは、
「従許容速度設定手段２２により設定された許容回転速
度ω_{s AL}」を「従速度演算手段２６で求められた合成回
転速度換算値ω_S 」で除した従制御対象物２ｓの合成回
転速度比ω_sRを演算し、その合成速度比Ｖ_sR，ω_sRを関
節速度指示値演算部３３で演算した各関節の回転速度指
示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m，Ｖ６_m
に乗じて、その回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ
３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を修正する。そして、そ
の修正された各関節の回転速度指示値Ｖ１_m ，Ｖ２_m ，
Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m でもって主制御対象物
１ｍの位置姿勢を主取扱装置１により変化させると共
に、従制御対象物２ｓの許容移動速度Ｖ_{s AL}もしくは許
容回転速度ω_{s AL}以下でもって、その従制御対象物２ｓ
の位置姿勢を従取扱装置２により変化させることができ
るようになっている。

【００１３】

【００１４】なお、前記の主取扱装置１は主制御対象物
１ｍであるワーク１ｗの位置姿勢を変化させるワーク取
扱装置である一方、従取扱装置２はワーク１ｗに作業を
施す従制御対象物２ｓであるツール２ｔの位置姿勢を変
化させるツール取扱装置であり、前記した連動関係制御
手段４Ａに記憶されているワーク取扱装置１により位置
姿勢が変化するワーク１ｗとそのワーク取扱装置１に協
調動作するツール取扱装置２により位置姿勢が変化する
ツール２ｔとの間の動作の関連づけは、「ツール２ｔの
ワーク１ｗに対する相対的な位置姿勢」を維持するよう
したものとしておくことができる。

【００１５】また、連動関係制御手段４Ａに記憶されて
いるワーク取扱装置１により位置姿勢が変化するワーク
１ｗとそのワーク取扱装置１に協調動作するツール取扱
装置２により位置姿勢が変化するツール２ｔとの間の動
作の関連づけは、「大地に対するツール２ｔの姿勢」を
一定に保ち、かつ、「ツール２ｔのワーク１ｗに対する
相対的な位置」を維持するようにしたものとすることも
できる。

【００１６】

【作用】まず、直角手動操作モードにより連動教示操作
するとき、従許容速度設定手段２２において、６自由度
を有する従取扱装置２が位置姿勢を変化させる従制御対
象物２ｓの直角座標系における許容移動速度Ｖ_{s AL}と許
容回転速度ω_{s AL}とを設定しておく。動作指示スイッチ
手段１２から動作指示信号Ｓｘ，Ｓｙ，Ｓｚ，Ｓα，Ｓ
β，Ｓγが発せられると、直角手動連動関係制御手段４
Ａc の速度成分指示値演算部２３が主制御対象物１ｍの
直角座標系における移動速度成分指示値Ｖｘ_m ，Ｖ
ｙ_m ，Ｖｚ_m と回転速度成分指示値ωｘ_m ，ωｙ_m ，ω
ｚ_m を演算する。直角手動連動関係制御手段４Ａc が記
憶する直角手動操作モードにおける主取扱装置１により
位置姿勢が変化する主制御対象物１ｍと、その主取扱装
置１に協調動作する従取扱装置２により位置姿勢が変化
する従制御対象物２ｓとの間の動作の関連づけをもとに
して、従速度演算手段２６により、従制御対象物２ｓの
直角座標系における合成移動速度換算値Ｖ_S と合成回転
速度換算値ω_S とが求められる。従速度演算手段２６に
よって求められた合成移動速度換算値Ｖ_S もしくは合成
回転速度換算値ω_S が、従許容速度設定手段２２により
設定された許容移動速度Ｖ_{s AL}もしくは許容回転速度ω
_{s AL}と比較される。許容移動速度Ｖ_{s AL}もしくは許容回
転速度ω_{s AL}を超えていると、補正速度成分指示値従演
算手段２７は従速度演算手段２６によって求められた合
成移動速度換算値Ｖ_S もしくは合成回転速度換算値ω_S
が、許容移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以
下となるように、速度成分指示値演算部２３で演算され
た移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m もしくは
回転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ_m を修正す
る。その修正された移動速度成分指示値Ｖｘ_m ，Ｖ
ｙ_m ，Ｖｚ_m もしくは回転速度成分指示値ωｘ_m ，ωｙ
_m ，ωｚ_m でもって主制御対象物１ｍの位置姿勢が主取
扱装置１により変化されると共に、従制御対象物２ｓの
許容移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下で
もって、その従制御対象物２ｓの位置姿勢が従取扱装置
２により変化される。従速度演算手段２６によって求め
られた合成移動速度換算値Ｖ_S もしくは合成回転速度換
算値ω_S が、従許容速度設定手段２２により設定された
許容移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}を超え
ていなければ、速度成分指示値演算部２３で演算された
移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m もしくは回
転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ_m がそのまま採
用される。その移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖ
ｚ_m もしくは回転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ
_m でもって、主制御対象物１ｍの位置姿勢が主取扱装置
１により変化されると共に、従制御対象物２ｓの許容移
動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下でもっ
て、その従制御対象物２ｓの位置姿勢が従取扱装置２に
より変化される。

【００１７】補正速度成分指示値従演算手段２７では、
まず、「従許容速度設定手段２２により設定された許容
移動速度Ｖ_{s AL}」を「従速度演算手段２６で求められた
合成移動速度換算値Ｖ_S 」で除した従制御対象物２ｓの
合成移動速度比Ｖ_sR、もしくは、「従許容速度設定手段
２２により設定された許容回転速度ω_{s AL}」を「従速度
演算手段２６で求められた合成回転速度換算値ω_S 」で
除した従制御対象物２ｓの合成回転速度比ω_sRを演算す
る。その合成速度比Ｖ_sR，ω_sRが速度成分指示値演算部
２３で演算した移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖ
ｚ_m や回転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ_mに乗
じられ、その速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m ，
ωｘ_m ，ωｙ_m ，ωｚ_m を修正する。この速度成分指示
値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m ，ωｘ_m ，ωｙ_m ，ωｚ_m に
よって、主制御対象物１ｍの位置姿勢が主取扱装置１に
より変化されると共に、従制御対象物２ｓの許容移動速
度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下でもって、そ
の従制御対象物２ｓの位置姿勢が従取扱装置２により変
化される。

【００１８】異なる発明においては、まず、関節手動操
作モードにより連動教示操作するとき、従許容速度設定
手段２２において、６自由度を有する従取扱装置２が位
置姿勢を変化させる従制御対象物２ｓの直角座標系にお
ける許容移動速度Ｖ_{s AL}と許容回転速度ω_{s AL}とを設定
しておく。動作指示スイッチ手段１２から動作指示信号
Ｓ１，Ｓ２，Ｓ３，Ｓ４，Ｓ５，Ｓ６が発せられると、
関節手動連動関係制御手段４Ａj の関節速度指示値演算
部３３が主制御対象物１ｍの各関節の回転速度指示値Ｖ
１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を演算
する。関節手動連動関係制御手段４Ａj が記憶する関節
手動操作モードにおける主取扱装置１により位置姿勢が
変化する主制御対象物１ｍと、その主取扱装置１に協調
動作する従取扱装置２により位置姿勢が変化する従制御
対象物２ｓとの間の動作の関連づけをもとにして、従速
度演算手段２６により、従制御対象物２ｓの直角座標系
における合成移動速度換算値Ｖ_S と合成回転速度換算値
ω_S とが求められる。従速度演算手段２６によって求め
られた合成移動速度換算値Ｖ_S もしくは合成回転速度換
算値ω_S が、従許容速度設定手段２２により設定された
許容移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}と比較
される。許容移動速度Ｖ_{s AL}もしくは許容回転速度ω
_{s AL}を超えていると、補正速度指示値従演算手段３７
は、従速度演算手段２６によって求められた合成移動速
度換算値Ｖ_S もしくは合成回転速度換算値ω_S が、許容
移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下となる
ように、関節速度指示値演算部３３で演算された各関節
の回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ
５_m ，Ｖ６_m を修正する。その修正された各関節の回転
速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，
Ｖ６_m でもって主制御対象物１ｍの位置姿勢が主取扱装
置１により変化されると共に、従制御対象物２ｓの許容
移動速度Ｖ_{s AL}もしくは許容回転速度ω_sAL以下でもっ
て、その従制御対象物２ｓの位置姿勢が従取扱装置２に
より変化される。従速度演算手段２６によって求められ
た合成移動速度換算値Ｖ_S もしくは合成回転速度換算値
ω_S が、従許容速度設定手段２２により設定された許容
移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}を超えてい
なければ、関節速度指示値演算部３３で演算された各関
節の回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，
Ｖ５_m ，Ｖ６_m がそのまま採用される。その各関節の回
転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ
５_m ，Ｖ６_m でもって、主制御対象物１ｍの位置姿勢が
主取扱装置１により変化されると共に、従制御対象物２
ｓの許容移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以
下でもって、その従制御対象物２ｓの位置姿勢が従取扱
装置２により変化される。

【００１９】補正速度指示値従演算手段３７では、ま
ず、「従許容速度設定手段２２により設定された許容移
動速度Ｖ_{s AL}」を「従速度演算手段２６で求められた合
成移動速度換算値Ｖ_S 」で除した従制御対象物２ｓの合
成移動速度比Ｖ_sR、もしくは、「従許容速度設定手段２
２により設定された許容回転速度ω_{s AL}」を「従速度演
算手段２６で求められた合成回転速度換算値ω_S 」で除
した従制御対象物２ｓの合成回転速度比ω_sRを演算す
る。その合成速度比Ｖ_sR，ω_sRが関節速度指示値演算部
３３で演算した各関節の回転速度指示値Ｖ１_m ，Ｖ
２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m に乗じられ、そ
の各関節の回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ
４_m ，Ｖ５_m ，Ｖ６_m を修正する。この各関節の回転速
度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ
６_m によって、主制御対象物１ｍの位置姿勢が主取扱装
置１により変化されると共に、従制御対象物２ｓの許容
移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下でもっ
て、その従制御対象物２ｓの位置姿勢が従取扱装置２に
より変化される。

【００２０】主取扱装置１を主制御対象物１ｍであるワ
ーク１ｗの位置姿勢を変化させるワーク取扱装置とする
一方、従取扱装置２をワーク１ｗに作業を施す従制御対
象物２ｓであるツール２ｔの位置姿勢を変化させるツー
ル取扱装置としておく。そして、直角手動連動関係制御
手段４Ａc や関節手動連動関係制御手段４Ａj の連動関
係制御手段４Ａに記憶されている「ワーク取扱装置１に
より位置姿勢が変化するワーク１ｗと、そのワーク取扱
装置１に協調動作するツール取扱装置２により位置姿勢
が変化するツール２ｔとの間の動作」の関連づけを、
「ツール２ｔのワーク１ｗに対する相対的な位置姿勢」
を維持するようにしておくと、ワーク１ｗが動作して
も、ツール２ｔはそのワーク１ｗに対して常に一定の姿
勢を維持しながらワーク１ｗ上の位置を保つ。したがっ
て、連動手動教示操作において、ワーク１ｗを動かして
もツール２ｔはワーク１ｗから離反しなく、次の教示点
を得る際にワーク１ｗの位置や姿勢を変えても、ツール
２ｔをワーク１ｗから遠ざける必要がない。ツール２ｔ
を次の教示点へもっていくときに、その教示すべきワー
ク１ｗ上の位置を見失うこともない。さらには、次の教
示点を得る際にワーク１ｗの位置や姿勢を変えても、ワ
ーク１ｗがツール２ｔにぶつかるといった事態も回避さ
れる。

【００２１】また、連動関係制御手段４Ａに記憶されて
いる「ワーク取扱装置１により位置姿勢が変化するワー
ク１ｗとそのワーク取扱装置１に協調動作するツール取
扱装置２により位置姿勢が変化するツール２ｔとの間の
動作」の関連づけを、「大地に対するツール２ｔの姿
勢」を一定に保ち、かつ、「ツール２ｔのワーク１ｗに
対する相対的な位置」を維持するようにしておくと、ワ
ーク１ｗが動作しても、ツール２ｔはその姿勢を常に大
地に対して一定にしたままワーク１ｗ上の位置を保つ。
したがって、連動手動教示操作において、ワーク１ｗを
動かしてもツール２ｔはワーク１ｗから離反しなく、次
の教示点を得る際にワーク１ｗの位置や姿勢を変えて
も、ツール２ｔをワーク１ｗから遠ざける必要がない。
ツール２ｔを次の教示点へもっていくときに、その教示
すべきワーク１ｗ上の位置を見失うこともない。さらに
は、次の教示点を得る際にワーク１ｗの位置や姿勢を変
えても、ツール２ｔの大地に対する姿勢が一定であり、
溶接トーチをツール２ｔとする場合などでは、その溶接
の品質向上に好適なトーチの姿勢を常時保持しておくこ
とができる。

【００２２】

【発明の効果】本発明によれば、従制御対象物の合成移
動速度換算値もしくは合成回転速度換算値が、予め記憶
されている許容移動速度もしくは許容回転速度を超える
とき、従制御対象物の合成移動速度換算値もしくは合成
回転速度換算値が許容移動速度もしくは許容回転速度以
下となるように、主制御対象物の移動速度成分指示値も
しくは回転速度成分指示値が修正され、その修正された
移動速度成分指示値もしくは回転速度成分指示値でもっ
て主制御対象物の位置姿勢を主取扱装置により変化させ
ると共に、従制御対象物の許容移動速度もしくは許容回
転速度以下でもって、従制御対象物の位置姿勢を従取扱
装置により変化させることができる。

【００２３】補正速度成分指示値従演算手段によると、
従制御対象物の合成移動速度比もしくは合成回転速度比
を演算して、主制御対象物の移動速度成分指示値や回転
速度成分指示値を修正することができる。その修正され
た主制御対象物の移動速度成分指示値や回転速度成分指
示値をもとにすると共に、主制御対象物と従制御対象物
との連動関係の関連づけにより、従制御対象物を許容移
動速度もしくは許容回転速度以下で従取扱装置により変
化させることができるようになる。

【００２４】異なる発明においては、従制御対象物の合
成移動速度換算値もしくは合成回転速度換算値が、予め
記憶されている許容移動速度もしくは許容回転速度を超
えるとき、従制御対象物の合成移動速度換算値もしくは
合成回転速度換算値が許容移動速度もしくは許容回転速
度以下となるように、主制御対象物の各関節の回転速度
指示値が修正され、その修正された各関節の回転速度指
示値でもって主制御対象物の位置姿勢を主取扱装置によ
り変化させると共に、従制御対象物の許容移動速度もし
くは許容回転速度以下でもって、従制御対象物の位置姿
勢を従取扱装置により変化させることができる。

【００２５】補正速度指示値従演算手段によると、従制
御対象物の合成移動速度比もしくは合成回転速度比を演
算して、主制御対象物の各関節の回転速度指示値を修正
することができる。その修正された主制御対象物の各関
節の回転速度指示値をもとにすると共に、主制御対象物
と従制御対象物との連動関係の関連づけにより、従制御
対象物を許容移動速度もしくは許容回転速度以下で従取
扱装置により変化させることができるようになる。

【００２６】主取扱装置をワークの位置姿勢を変化させ
るワーク取扱装置し、従取扱装置をワークに作業を施す
ツールの位置姿勢を変化させるツール取扱装置してお
き、連動関係制御手段に記憶されているワークとツール
の関連づけを、「ツールのワークに対する相対的な位置
姿勢」を一定に保つようにしておいても、ワークの速度
に制限をかけることによってツール取扱装置によって変
化されるツールの速度を許容範囲に留めておくことがで
きる。

【００２７】連動関係制御手段に記憶されているワーク
とツールの関連づけを、「大地に対するツールの姿勢」
を一定に保ち、かつ、「ツールのワークに対する相対的
な位置」を維持するようにしておいても、ワークの速度
に制限をかけることによってツール取扱装置によって変
化されるツールの速度を許容範囲内にすることができ
る。

【００２８】

【実施例】以下に、ワーク取扱装置とツール取扱装置の
二基のロボットからなる溶接ロボットシステムに適用し
た実施例にもとづいて、本発明に係る産業用ロボットの
手動教示操作における速度制御装置を、詳細に説明す
る。本例のロボットシステムには、図４に示すように、
ワーク１ｗの「位置姿勢」を変化させることができるワ
ーク取扱装置１と、ワークに作業を施すツール２ｔ例え
ば溶接用のトーチの「位置姿勢」を変化させることがで
きるツール取扱装置２とが備えられている。そして、ワ
ーク取扱装置１とツール取扱装置２の二基のロボットは
対向して設置され、溶接などの実稼働中に必要とされる
「ワーク１ｗの位置姿勢」と「ツール２ｔの位置姿勢」
とを、各取扱装置の「単独の手動操作」によって教示し
たり、両取扱装置の「連動する手動操作」によって教示
することができるようになっている。エンドエフェクタ
１Ａに取り付けたワーク１ｗを動かすワーク取扱装置１
は、図５に示すように、例えば六つの関節１ａないし１
ｆにおいてφ₁ ないしφ₆ の「関節変数」を有する６自
由度のマニプレータである。このワーク取扱装置１は後
述する主動作機械としての主取扱装置とする場合には６
自由度のマニプレータに限られることなく、３自由度以
下のポジショナを使用することもできる。一方、エンド
エフェクタ２Ａ（図４参照）に取り付けたツール２ｔを
動かすツール取扱装置２は、図５に示す六つの関節２ａ
ないし２ｆにおいてθ₁ ないしθ₆ の「関節変数」を有
する６自由度のマニプレータである。そして、例えば、
関節の幾つかを組み合わせて動作させれば、ワーク１ｗ
とツール２ｔは所望する任意の方向へそれぞれ動かすこ
とができる。例えばＸ軸方向へのみ動かせたり、Ｙ軸回
りのみに回転させることもできるようになっている。な
お、このツール取扱装置２が後述する従動作機械として
の従取扱装置とされる場合には、その追従動作を実現す
るため必ず６自由度のマニプレータが採用されるが、こ
れを逆に主取扱装置とする場合には３自由度以下のポジ
ショナであってもよい。

【００２９】このワーク取扱装置１とツール取扱装置２
とを操作するための操作ボックスやティーチングペンダ
ントさらには制御装置が設けられる。図８は操作ボック
ス１７であり、ロボットシステムにおいて教示するのか
再現動作させるのかを選定する動作選定スイッチ１７ａ
と、教示に従ってロボットを再現動作させるための開始
指令をする自動運転起動スイッチ１７ｂが設けられてい
る。図６はティーチングペンダント３の操作パネル面で
あり、上部に表示盤５が下部に停止指令ボタン６が設け
られる。操作パネルの中段の左には手動操作の対象とな
るロボットを選択するロボット切替スイッチ７があり、
上方へ傾けると「ワーク取扱装置」が指定され、下方に
傾けると「ツール取扱装置」が指定されるようになって
いる。これは、後述する動作切替スイッチ９によって、
「単独の手動操作」が選択されている場合に、その手動
操作の対象となる取扱装置を指定し、また、「連動する
手動操作」が選択されている場合に、その手動操作の直
接の対象となるワーク取扱装置を指定する取扱装置指定
手段として機能する。なお、操作ボックス１７やティー
チングペンダント３は、図４に示すように制御装置４と
共にロボット１，２の近傍などに設置され、そのロボッ
トシステムの制御や動作指令の全てを操作することがで
きるようになっている。

【００３０】図６において、操作パネルの中央には手動
操作のモードを切り替えるモード切替スイッチ８が設置
され、下方へ傾けると「関節手動操作モード」が選定さ
れ、上方へ傾けると「直角手動操作モード」が選定され
るようになっている。右端には動作切替スイッチ９が設
けられ、上方へ傾けると各ロボットを「単独の手動操
作」で動作させることが選択され、下方へ傾けると二基
のロボットを「連動する手動操作」で動作させることが
選択されるようになっている。これは、ワーク取扱装置
１による「ワーク１ｗの位置姿勢」の変化とツール取扱
装置２による「ツール２ｔの位置姿勢」の変化とをそれ
ぞれ「単独の手動操作」によって行わせるか、ワーク取
扱装置１による「ワーク１ｗの位置姿勢」の変化とツー
ル取扱装置２による「ツール２ｔの位置」の変化とを
「連動する手動操作」によって行わせるかを選択する単
独連動操作選択手段を構成している。なお、本発明にお
ける「連動する手動操作」とは、「ワーク１ｗの位置姿
勢」を変化させるとき、「ツール２ｔのワーク１ｗに対
する相対的な位置姿勢」を一定に保つようにする場合
や、「大地に対するツール２ｔの姿勢」を一定に保ち、
かつ「ツール２ｔのワーク１ｗに対する相対的な位置」
を維持させるようにする場合において、ツール取扱装置
２による「ツール２ｔの位置姿勢」を制御させることで
ある。

【００３１】ちなみに、ワーク取扱装置１の動きに従っ
てツール取扱装置２を動かすように教示する場合、もし
くはその逆の場合でも、例えばツール２ｔに凹凸がある
と連動する際にワーク１ｗがツール２ｔの突起にぶつか
るといったことがあり、それを回避して教示を円滑に進
めるためには、上記した前者の連動形態を採用すること
に意義がある一方、溶接作業においては、溶接線が大地
に対して所望する傾きとなるようにワーク１ｗを配置す
ることが重要である。そこで、溶接線の傾きが大地に対
して常に一定となるように「ワーク１ｗの姿勢」が保持
される。一方、「トーチ２ｔの姿勢」は溶接線の傾きに
よって決定されるものであるので、溶接線の傾きを大地
に対して常に一定にしておくならば、「トーチ２ｔの姿
勢」も大地に対して常に一定としておく必要がある。こ
のようなことから、上記した連動関係の後者を採用する
ことにも意味がある。なお、このような連動関係におい
てワーク取扱装置１をマスターとしての主取扱装置と、
ツール取扱装置２をスレーブとしての従取扱装置と名づ
け、ワーク１ｗを主制御対象物と、ツール２ｔを従制御
対象物２ｓと称することにする。もちろん、マスターと
スレーブとを入れ替えても連動させることができる場合
のあることは言うまでもない。

【００３２】パネル中央左端の目盛のある部分は速度率
切替スイッチ１０であり、教示時にワーク１ｗが動く速
度を、予め制御装置４の最大速度規定部に設定されてい
る速度テーブルの最高値よりも遅くできるようにするた
めの「速度率」を選定するものである。例えば目盛の
０．５を選定すると、作業員によって教示された位置
へ、速度テーブルに記憶された最高値の１／２の速度で
移動させることができる。早く移動させるときは１．０
を選定すればよい。その「速度率」はいずれを選定して
も差し支えないが、次の教示位置までの距離が長い場合
には、その８０％までの距離を例えば１．０の速度率と
し、残りの２０％の距離を０．１の速度率とすると、目
標位置近傍までは迅速に、目標位置に到達させるときに
はゆっくりとした移動を実現でき、目標位置に正確に停
止させやすくなる。高い速度率の選定に起因して目標位
置をオーバーランさせた場合には引き返せばよいが、目
標位置に停止させるためには速度の遅い方が教示作業の
緊張感を軽減することができ好都合である。そのため
に、「速度率」として例えば０．１ ,０．２ ,０．５ ,
１．０の四つが準備されている。パネルの中央下には、
教示されたデータのための編集ボックス１４が確保され
ている。その左側には、操作ボタン群１２が配置され
る。いずれの操作ボタン１２ａ〜１２ｆ，１２Ａ〜１２
Ｆも押されると１の信号を出力し、押されてないときは
常に０を出力している。この１２個の押しボタンから構
成される操作ボタン群１２は、教示作業のための「動作
指示信号」を出力する動作指示スイッチ手段となってい
る。

【００３３】この操作ボタン群１２を以下に説明する。
ロボット切替スイッチ７により「ワーク取扱装置」が指
定され、モード切替スイッチ８により「関節手動操作モ
ード」が選定されているとき、左側の列の操作ボタン１
２ａないし１２ｆを押せば、動作指示信号Ｓ１，Ｓ２，
Ｓ３，Ｓ４，Ｓ５，Ｓ６を出力して、ワーク取扱装置１
の関節が正転されることになる。また、右側の列の操作
ボタン１２Ａないし１２Ｆを押せば、その関節を逆転さ
せることができる。モード切替スイッチ８で「直角手動
操作モード」が選定されているときに、上から三段の操
作ボタンを押せば、図５中の絶対座標系５１のＸ，Ｙ，
Ｚの各軸方向の移動を手動操作する動作指示信号Ｓｘ，
Ｓｙ，Ｓｚを出力することになり、左側の操作ボタン１
２ａないし１２ｃを押せばプラス方向へ、右側の１２Ａ
ないし１２Ｃを押せばマイナス方向へ進むようになって
いる。下の三段の操作ボタンを押せば動作指示信号Ｓ
α，Ｓβ，Ｓγを出力して、絶対座標系５１に対する図
５中のオイラー角α，β，γの方向への回転を与えるこ
とができ、左側の操作ボタン１２ｄないし１２ｆを押せ
ばプラス方向に、右側の１２Ｄないし１２Ｆを押せばマ
イナス方向になる。図６の操作ボタン群１２の右下に
は、「単独の手動操作」によって教示する場合のみなら
ず、連動関係制御手段による「連動する手動操作」によ
って教示する場合に使用される教示ボタン１３が設けら
れている。これは、ワーク１ｗとツール２ｔの位置決め
が達成された状態で、その「ワーク１ｗの位置姿勢」と
「ツール２ｔの位置姿勢」を記憶させる指令をするもの
である。

【００３４】このような装置からなる産業用ロボットの
手動操作による教示制御装置としては、動作切替スイッ
チ９からの「連動する手動操作の信号」、ロボット切替
スイッチ７からの「ワーク取扱装置を指定する信号」お
よび操作ボタン群１２からの「動作指示信号」を受けた
ときに制御作動する教示操作用の連動関係制御手段とし
てのコンピュータ４Ａが、図４に示すように制御装置４
に内蔵されている。この制御装置４は、本発明に係る連
動関係制御手段４Ａのみならず、後述する「単独の手動
操作」において制御動作するワーク手動操作教示制御手
段４Ｗと、ツール手動操作教示制御手段４Ｔをも含んで
いる。その全体構成は図２に示すごとくであり、図７に
示すＣＰＵ，ＲＯＭやＲＡＭなどからなるマイクロプロ
セシングユニット１６と、それから出力された「アクチ
ュエータ動作指令値」を駆動信号に変換するサーボドラ
イバ１５Ｗ，１５Ｔなどを有している。本例は、ワーク
取扱装置１およびツール取扱装置２が６自由度のマニプ
レータの例であるので、それぞれにサーボドライバ１５
Ｗ₁ 〜１５Ｗ₆ ，１５Ｔ₁ 〜１５Ｔ₆ が存在する。そし
て、ワーク取扱装置１の関節におけるリンクを回動させ
るためのサーボモータなどのアクチュエータＭ_1w〜Ｍ_6w
（図１７参照）がサーボドライバ１５Ｗ₁ 〜１５Ｗ₆ に
対応して設けられ、ツール取扱装置２の関節におけるリ
ンクを回動させるためのアクチュエータＭ_1t〜Ｍ_6tがサ
ーボドライバ１５Ｔ₁ 〜１５Ｔ₆ に対応して設けられて
いる。図４中の１８Ｗは、サーボドライバ１５Ｗとワー
ク取扱装置１のアクチュエータＭ_w とを結ぶ制御ケーブ
ルであり、１８Ｔもサーボドライバ１５Ｔとツール取扱
装置２のアクチュエータＭ_t とを結ぶ制御ケーブルであ
る。そして、各アクチュエータ動作指令値の信号を出力
するためのインターフェースが設けられ、出力フェース
やバッファも存在する。なお、図５に示した「ワーク取
扱装置１のその時点での関節変数」φ₁ , φ₂, φ₃ ,
φ₄ , φ₅ , φ₆ と、「ツール取扱装置２のその時点で
の関節変数」θ₁ , θ₂ , θ₃ , θ₄ , θ₅ , θ₆ は、
後述するセンサＳ_1w〜Ｓ_6w，Ｓ_1t〜Ｓ_6t（図１７参照）
によって検出することができ、それを記憶させておく場
合のため図２中に１９ｂw や１９ｂt で示されるワーク
１ｗの関節変数記憶部やツール２ｔの関節変数記憶部が
設けられる。

【００３５】このような装置において、本発明に関連す
る部分を、図１を参照して以下に説明する。その図は、
主制御対象物を例えばワーク１ｗとして、そのワーク１
ｗの位置姿勢を変化させる主取扱装置としてのワーク取
扱装置１と、従制御対象物２ｓをツール２ｔとして、そ
のツール２ｔの位置姿勢を変化させる従取扱装置として
のツール取扱装置２とを有して、ワーク取扱装置１を教
示手動操作すると、６自由度を有するツール取扱装置２
がワーク取扱装置１の動作に関連づけられて協調動作す
るようになっている産業用ロボットの手動教示操作によ
る制御装置のブロック図である。すなわち、図６におい
て、動作切替スイッチ９が下へ、モード切替スイッチ８
が上へ、ロボット切替スイッチ７も上へ操作されている
状態での教示における場合の例である。

【００３６】順を追って、個々に説明する。まず、各取
扱装置１，２におけるワーク１ｗやツール２ｔの動作速
度に制限をかけて、予測外の早い動作とならないよう
に、ワーク取扱装置１のための主許容速度設定手段２１
と、ツール取扱装置２のための従許容速度設定手段２２
が設けられる。これは、図６のティーチングペンダント
３の右下において許容速度を設定することができる幾つ
かのボタンからなっていて、前者の場合にはロボット切
替スイッチ７を上げておいた状態で、また、後者はロボ
ット切替スイッチ７を下げておいた状態で、許容速度設
定ボタンの０から９などの数字で速度を入力するように
している。前者は、教示作業中に位置姿勢が変化するワ
ーク１ｗの直角座標系における許容移動速度Ｖ_{m AL}と許
容回転速度ω_{m AL}とを設定するものであり、後者は、教
示作業中に位置姿勢が変化するツール２ｔの直角座標系
における許容移動速度Ｖ_sALと許容回転速度ω_{s AL}とを
設定するものである。いずれにおいても、移動速度と回
転速度との区別をするために、ティーチングペンダント
３の中央に位置する速種切替スイッチ２０が使用され
る。上げておけば許容移動速度Ｖ_{m AL}もしくはＶ_{s AL}を
入力でき、下げておけば許容回転速度ω_{m AL}もしくはω
_{s AL}を入力することができる。

【００３７】二つのロボット１，２を連動して教示する
際に機能する連動関係制御手段４Ａは、前述した連動関
係を記憶するが、この場合の直角手動連動関係制御手段
４Ａc は、直角手動操作モードにおいて、主取扱装置１
により位置姿勢が変化する主制御対象物１ｍと、主取扱
装置１に協調動作する従取扱装置２により位置姿勢が変
化する従制御対象物２ｓとの間の動作の関連づけを記憶
し、ワーク１ｗの位置姿勢を変化させるワーク取扱装置
１の動作を制御すると共に、ツール２ｔの位置姿勢を変
化させるツール取扱装置２の動作を制御するようになっ
ている。これは、単独連動操作選択手段９，取扱装置指
定手段７および動作指示スイッチ手段１２からの信号を
受けた図１に示す連動手動操作開始指令部４o からの伝
達信号を受けて動作をするようになっている。この制御
手段４Ａc の構成は複雑であるが、以下にその要素を羅
列して簡単に説明する。ワーク取扱装置１の関節変数記
憶部１９ｂw から現在の関節変数を得て、ワーク１ｗの
初期もしくは現在の状態を演算するワークの初期状態演
算部４₁₁、ツール取扱装置２の関節変数記憶部１９ｂt
から現在の関節変数を得て、ツール２ｔの初期もしくは
現在の状態を演算するツールの初期状態演算部４₁₂、そ
のツールの初期状態演算部４₁₂と上記のワークの初期状
態演算部４₁₁とからの演算結果を受けてワーク１ｗとツ
ール２ｔ間の初期もしくは現在の状態関係を演算する制
御対象物間の初期状態演算部４₁₃がある。これらに加え
て、後述する主速度演算手段２４からの演算結果を得
て、ワーク１ｗのＸ，Ｙ，Ｚ軸方向の速度およびオイラ
ー角α，β，γの回転速度を演算する速度成分指示値演
算部２３が設けられている。これは、前述した動作指示
スイッチ手段１２からの動作指示信号Ｓｘ，Ｓｙ，Ｓ
ｚ，Ｓα，Ｓβ，Ｓγにもとづき、ワーク１ｗの直角座
標系における移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ
_m と回転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ_m を演算
するものである。

【００３８】それ以外に、以下のものが設けられるが、
その機能は後述する図２４ないし図３０で説明する制御
例において把握することができる。４₂ は動作指示信号
の変更や消滅を検出する動作指示信号変更検出部であ
り、信号が消失していると終了指令部４ｆにその旨の信
号を出力する。４₃₁と４₃₂は制御対象物の後述する時間
ΔＴ経過後の位置姿勢を演算するもので、前者はワーク
１ｗの位置姿勢を求める演算部、後者はツール２ｔの位
置姿勢を求める演算部である。４₄₁と４₄₂は上記の演算
部４₃₁，４₃₂の演算結果を受けて取扱装置の時間ΔＴ経
過後の関節変数を演算するもので、前者はワーク取扱装
置１の関節変数を求める演算部、後者はツール取扱装置
２の関節変数を求める演算部である。なお、演算部
４₄₁，４₄₂の演算結果は次の演算部４₅₁，４₅₂に出力さ
れると共に、各取扱装置の関節変数記憶部１９ｂw ，１
９ｂt へも送られる。なお、関節変数記憶部１９ｂw ，
１９ｂt では、新しい信号がくるたびにその記憶データ
が置き換えられるようになっている。４₅₁，４₅₂は上記
の演算部４₄₁，４₄₂の演算結果を受けて取扱装置のアク
チュエータの動作指令値を演算するもので、前者はワー
ク取扱装置１のサーボモータＭ_1w〜Ｍ_6wの動作指令値を
求める演算部、後者はツール取扱装置２のサーボモータ
Ｍ_1t〜Ｍ_6tの動作指令値を求める演算部である。その信
号はそれぞれ取扱装置のアクチュエータ動作指令部４w
，４t へ出力され、サーボモータＭ_1w〜Ｍ_6w，Ｍ_1t〜
Ｍ_6tを動作させるように指令する。

【００３９】連動関係制御手段４Ａc には、以下のもの
も含まれている。主取扱装置１に関連するものとして
は、主速度演算手段２４と補正速度成分指示値主演算手
段２５とがある。前者は、速度成分指示値演算部２３に
よって演算されたワーク１ｗの直角座標系における移動
速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m と回転速度成分
指示値ωｘ_m ，ωｙ_m ，ωｚ_m とから、ワーク１ｗの直
角座標系における合成移動速度換算値Ｖ_m および合成回
転速度換算値ω_m とを求めるものである。後者は、主速
度演算手段２４によって求められた合成移動速度換算値
Ｖ_m もしくは合成回転速度換算値ω_s が主許容速度設定
手段２１により設定された許容移動速度Ｖ_{m AL}もしくは
許容回転速度ω_{m AL}を超えるとき、主速度演算手段２４
によって求められた合成移動速度換算値Ｖ_m もしくは合
成回転速度換算値ω_m が許容移動速度Ｖ_{m AL}もしくは許
容回転速度ω_{m AL}以下となるように、速度成分指示値演
算部２３で演算された移動速度成分指示値Ｖｘ_m ，Ｖｙ
_m ，Ｖｚ_m もしくは回転速度成分指示値ωｘ_m ，ω
ｙ_m ，ωｚ_m を修正するものである。なお、それらに付
随して、主許容速度設定手段２１によって設定された主
許容速度を記憶する主許容速度記憶部２１ａや、この主
許容速度記憶部２１ａから取り出される記憶値と主速度
演算手段２４で求められた演算値との大小を比較する主
速度比較部２１ｂも設けられている。それゆえに、補正
速度成分指示値主演算手段２５は、「主許容速度設定手
段２１により設定した許容移動速度Ｖ_{m AL}」を「主速度
演算手段２４で求めた合成移動速度換算値Ｖ_m 」で除し
たワーク１ｗの合成移動速度比Ｖ_mR、もしくは、「主許
容速度設定手段２１により設定した許容回転速度
ω_{m AL}」を「主速度演算手段２４で求めた合成回転速度
換算値ω_m 」で除したワーク１ｗの合成回転速度比ω_mR
を演算し、それらの合成速度比Ｖ_mR，ω_mRを速度成分指
示値演算部２３で演算した移動速度成分指示値Ｖｘ_m ，
Ｖｙ_m ，Ｖｚ_m や回転速度成分指示値ωｘ_m ，ωｙ_m ，
ωｚ_m に乗じて、各速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖ
ｚ_m ，ωｘ_m ，ωｙ_m ，ωｚ_m を修正することができる
ようになっている。したがって、修正された移動速度成
分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m もしくは回転速度成分
指示値ωｘ_m ，ωｙ_m ，ωｚ_m でもってワーク１ｗの位
置姿勢をワーク取扱装置１により変化させることができ
る。

【００４０】さらに、従取扱装置２に関連するものとし
ては、従速度演算手段２６と補正速度成分指示値従演算
手段２７とがある。前者は、直角手動連動関係制御手段
４Ａc に従い、ワーク取扱装置１と協調動作するツール
取扱装置２によって位置姿勢が変化されるツール２ｔの
直角座標系における合成移動速度換算値Ｖ_S と合成回転
速度換算値ω_S とを求めるものである。これは、時間Δ
Ｔ経過後のツール２ｔの位置姿勢演算部４₃₂からの演算
結果を受けて動作する。後者は、従速度演算手段２６に
よって求められた合成移動速度換算値Ｖ_S もしくは合成
回転速度換算値ω_S が従許容速度設定手段２２により設
定された許容移動速度Ｖ_{s AL}もしくは許容回転速度ω
_{s AL}を超えるとき、従速度演算手段２６によって求めら
れた合成移動速度換算値Ｖ_S もしくは合成回転速度換算
値ω_S が許容移動速度Ｖ_{s AL}もしくは許容回転速度ω
_{s AL}以下となるように、速度成分指示値演算部２３で演
算された移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m も
しくは回転速度成分指示値ωｘ_m ，ωｙ_m ，ωｚ_m を修
正するものである。なお、それらに付随して、従許容速
度設定手段２２によって設定された従許容速度を記憶す
る主許容速度記憶部２２ａや、この従許容速度記憶部２
２ａから取り出される記憶値と従速度演算手段２６で求
められた演算値との大小を比較する従速度比較部２２ｂ
も設けられている。それゆえに、補正速度成分指示値従
演算手段２７は、従許容速度設定手段２２により設定し
た許容移動速度Ｖ_{s AL}」を「従速度演算手段２６で求め
た合成移動速度換算値Ｖ_S 」で除したツール２ｔの合成
移動速度比Ｖ_sR、もしくは、「従許容速度設定手段２２
により設定した許容回転速度ω_{s AL}」を「従速度演算手
段２６で求めた合成回転速度換算値ω_S 」で除したツー
ル２ｔの合成回転速度比ω_sRを演算し、それらの合成速
度比Ｖ_sR，ω_sRを速度成分指示値演算部２３で演算した
移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m や回転速度
成分指示値ωｘ_m ，ωｙ_m ，ωｚ_m に乗じて、各速度成
分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m ，ωｘ_m ，ωｙ_m ，ω
ｚ_m を修正することができるようになっている。したが
って、修正された移動速度成分指示値Ｖｘ_m ，Ｖｙ_m ，
Ｖｚ_m もしくは回転速度成分指示値ωｘ_m ，ωｙ_m ，ω
ｚ_m でもってワーク１ｗの位置姿勢をワーク取扱装置１
により変化させると共に、ツール２ｔの許容移動速度Ｖ
_{s AL}もしくは許容回転速度ω_{s AL}以下でもって、ツール
２ｔの位置姿勢をツール取扱装置２により変化させるこ
とができる。これから分かるように、ツール２ｔの動作
速度の制限は、ワーク１ｗの動作速度に制限をかけるこ
とによって間接的に可能となる。なぜなら連動する教示
制御となっているからである。

【００４１】ところで、ロボットシステムにおいて、教
示操作をするとき各ロボットを単独で教示操作すること
もある。したがって、制御装置４には、上記した連動関
係制御手段４Ａのほかに、図６に示すロボット切替スイ
ッチ７によって「ワーク取扱装置」が指定され、かつ、
動作切替スイッチ９により「単独の手動操作」が選択さ
れている場合に、ワーク取扱装置１の「単独の手動操
作」を実行するワーク手動操作教示制御手段４Ｗが図２
に示すように設けられる。さらには、ロボット切替スイ
ッチ７によって「ツール取扱装置」が指定され、かつ、
動作切替スイッチ９により「単独の手動操作」が選択さ
れている場合に、ツール取扱装置２の「単独の手動操
作」を実行するツール手動操作教示制御手段４Ｔも設け
られる。なお、ワーク取扱装置１の関節変数記憶部１９
ｂw とツール取扱装置２の関節変数記憶部１９ｂt と
は、「単独の手動操作」の場合の「関節変数」も「連動
する手動操作」の場合の「関節変数」も記憶される。し
たがって、関節変数記憶部１９ｂw と関節変数記憶部１
９ｂt とは、制御装置４にそれぞれが一つずつ設けられ
て共用され、終了指令部４ｆも同様に一つだけとなって
いる。このような単独手動教示操作においては、上記し
た従速度演算手段２６や補正速度成分指示値従演算手段
２７などを機能させる必要がなく、ワーク取扱装置１に
ついては図２に示されないが、主速度演算手段２４に相
当する速度演算部４₆₁や補正速度成分指示値主演算手段
２５に相当する速度修正処理手段４₆₂が機能するだけと
なっている。一方、ツール取扱装置２については主速度
演算手段２４に相当する速度演算部４₇₁や補正速度成分
指示値主演算手段２５に相当する速度修正処理手段４₇₂
が設けられている。なお、それぞれの教示制御のトリガ
を与えるものとして、単独手動操作開始指令部４₆₀，４
₇₀が設けられている。ちなみに、その図２に記載された
連動関係制御手段４Ａの中の速度修正処理手段４₈ は、
補正速度成分指示値従演算手段２７などを一括表示した
ものである。

【００４２】以下に、ワーク取扱装置１により位置姿勢
が変化するワーク１ｗとそのワーク取扱装置１に協調動
作するツール取扱装置２により位置姿勢が変化するツー
ル２ｔとの間の動作の関連づけを、「ツール２ｔのワー
ク１ｗに対する相対的な位置姿勢」を維持するようした
ものについて、図１のブロック図で示される制御装置全
体の構成および図９ないし図１２のフローチャートを用
いて説明する。まず、動作指示スイッチ手段１２からの
動作指示信号があると〔ステップ４０１、以下Ｓ４０１
などと記す〕、その信号Ｓｘ，Ｓｙ，Ｓｚ，Ｓα，Ｓ
β，Ｓγが取り込まれる〔Ｓ４０２〕。そして、速度成
分指示値演算部２３において、ワーク１ｗの速度成分指
示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m ，ωｘ_m ，ωｙ_m ，ωｚ_m
が演算される〔Ｓ４０３〕。それを受けて主速度演算手
段２４においてワーク１ｗの合成速度換算値Ｖ_m ，ω_m
を算出する〔Ｓ４０４〕。次に、主許容速度設定手段２
１で設定された許容速度Ｖ_{m AL}，ω_{m AL}を主許容速度記
憶部２１ａから読み取る〔Ｓ４０５〕。主速度比較部２
１ｂにおいて合成速度換算値Ｖ_m ，ω_m が許容速度Ｖ
_{m AL}，ω_{m AL}と対比される〔Ｓ４０６〕。合成速度換算
値Ｖ_m ，ω_m が許容速度Ｖ_{m AL}，ω_{m AL}以下であれば、
ワーク１ｗは所望速度で動作していることが分かり、Ｓ
４０７へ進められる。合成速度換算値Ｖ_m ，ω_m が許容
速度Ｖ_{m AL}，ω_{m AL}より大きければ、ワーク１ｗは所望
速度を超えているので制限がかけられる。Ｓ４３０にお
いてワーク１ｗの合成速度比Ｖ_mR，ω_mRが演算される。
その合成速度比Ｖ_mR，ω_mRは、ワーク１ｗの許容速度を
ワーク１ｗの合成速度換算値で除したＶ_{m AL}／Ｖ_m ，ω
_{m AL}／ω_m が採用される。その合成速度比Ｖ_mR，ω_mRを
用いてワーク１ｗの速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖ
ｚ_m ，ωｘ_m ，ωｙ_m ，ωｚ_m が、補正速度成分指示値
主演算手段２５において修正される〔Ｓ４３１〕。

【００４３】次にワーク取扱装置１の関節変数記憶部１
９ｂw から現在の関節変数が取り込まれ〔Ｓ４０７〕、
それを用いてワーク１ｗの現在の位置姿勢を演算する
〔Ｓ４０８〕。同時に、ツール取扱装置２の現在の関節
変数も関節変数記憶部１９ｂtから取り込まれ〔Ｓ４０
９〕、それを用いてツール２ｔの現在の位置姿勢が演算
される〔Ｓ４１０〕。直角手動連動関係制御手段４Ａc
においてこのような演算が行われるが、次に上記のそれ
ぞれの位置姿勢から、ツール２ｔのワーク１ｗに対する
現在の相対的な位置姿勢が演算される〔Ｓ４１１〕。こ
のような演算の後には、速度成分指示値演算部２３で求
められているワーク１ｗの速度成分指示値Ｖｘ_m ，Ｖｙ
_m ，Ｖｚ_m ，ωｘ_m ，ωｙ_m ，ωｚ_m を用いて、ワーク
１ｗの時間ΔＴ経過後の位置姿勢が演算される〔Ｓ４１
２〕。その時間ΔＴとは例えば１／２０秒といった短い
時間であるが、その時間が経過したときの位置姿勢が予
測されることになる。現在の速度成分指示値が分かって
いるから時間ΔＴを乗ずれば求まる（後述する式(22)を
参照）。これをもとにすれば、ツール２ｔとワーク１ｗ
との連動関係から、ツール２ｔの時間ΔＴ経過後の位置
姿勢を演算することができる〔Ｓ４１３〕。従速度演算
手段２６において、ツール２ｔの時間ΔＴ経過後の位置
姿勢とツール２ｔの現在の位置姿勢との差分をとって、
ツール２ｔの合成速度換算値Ｖ_s ，ω_s が求められる
〔Ｓ４１４〕。そこで、従許容速度設定手段２２で設定
されている許容速度Ｖ_{s AL}，ω_{s AL}を従許容速度記憶部
２２ａから読み取る〔Ｓ４１５〕。上記のツール２ｔの
合成速度換算値Ｖ_s ，ω_s と許容速度Ｖ_{s AL}，ω_{s AL}と
が従速度比較部２２ｂにおいて対比される〔Ｓ４１
６〕。合成速度換算値Ｖ_s ，ω_s が許容速度Ｖ_{s AL}，ω
_{s AL}以下であれば、ツール２ｔは許容速度以下の所望速
度で動作することになり、Ｓ４１７へ進められる。合成
速度換算値Ｖ_s ，ω_s が許容速度Ｖ_{s AL}，ω_{s AL}より大
きければ、ツール２ｔは許容速度を超えているので制限
がかけられる。Ｓ４３２においてツール２ｔの合成速度
比Ｖ_sR，ω_sRが演算される。その合成速度比Ｖ_sR，ω_sR
は、ツール２ｔの許容速度Ｖ_{s AL}，ω_{s AL}をツール２ｔ
の合成速度換算値Ｖ_s ，ω_s で除したものが採用され
る。その合成速度比Ｖ_sR，ω_sRを用いてワーク１ｗの速
度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m ，ωｘ_m ，ω
ｙ_m ，ωｚ_m が補正速度成分指示値従演算手段２７にお
いて修正される〔Ｓ４３３〕。その修正されたワーク１
ｗの速度成分指示値Ｖｘ_m ，Ｖｙ_m ，Ｖｚ_m ，ωｘ_m ，
ωｙ_m ，ωｚ_m はＳ４１２へ戻され、以後Ｓ４１６まで
再度演算される。このように修正された値で換算したツ
ール２ｔの合成速度換算値はＳ４１６に再度来た時点で
許容速度以下になっているはずである。しかし、もし、
許容速度Ｖ_{s AL}，ω_{s AL}を超えているようであれば、当
然フローチャートの流れに沿ってＳ４１２へ再度戻され
る。

【００４４】このようにツール２ｔが許容速度以下とな
るまで演算はコンピュータによって高速処理される。そ
して、Ｓ４０２から時間ΔＴが経過すれば〔Ｓ４１
７〕、動作指示スイッチ手段１２での入力信号Ｓｘ，Ｓ
ｙ，Ｓｚ，Ｓα，Ｓβ，Ｓγに変化がないかどうか確認
される。動作指示信号が同じであれば上記の演算結果が
以後の処理に使用される。上記のＳ４１３で演算したツ
ール２ｔの時間ΔＴ経過後の位置姿勢を用いてツール取
扱装置２の時間ΔＴ経過後の関節変数が演算される〔Ｓ
４１９〕。また、Ｓ４１２で演算されたワーク１ｗの時
間ΔＴ経過後の位置姿勢を用いてワーク取扱装置１の時
間ΔＴ経過後の関節変数が演算される〔Ｓ４２０〕。こ
の関節変数は、その時点でそれぞれの関節変数記憶部１
９ｂw ，１９ｂt に入力され、従前の記憶値と置き換え
られる。ツール取扱装置２の時間ΔＴ経過後の関節変数
からツール取扱装置２のアクチュエータ動作指令値が演
算され〔Ｓ４２１〕、ワーク取扱装置１の時間ΔＴ経過
後の関節変数からワーク取扱装置１のアクチュエータ動
作指令値が演算される〔Ｓ４２２〕。

【００４５】Ｓ４１８からさらに時間ΔＴが経過すると
〔Ｓ４２３〕、上記のワーク取扱装置１のアクチュエー
タ動作指令値がワーク取扱装置１のアクチュエータ動作
指令部４w へ出力され〔Ｓ４２４〕、ツール取扱装置２
のアクチュエータ動作指令値がツール取扱装置２のアク
チュエータ動作指令部４t へ出力され〔Ｓ４２５〕。そ
の結果、ワーク取扱装置１が動作して〔Ｓ４２６〕、ワ
ーク１ｗが所望する位置姿勢となるように動かされる
〔Ｓ４２８〕。同時に、ツール取扱装置２も動作して
〔Ｓ４２７〕、ツール２ｔが所望する位置姿勢となるよ
うに動かされる〔Ｓ４２９〕。このようにして、連動手
動操作においては、ワーク取扱装置１を主取扱装置とし
て動作させると、ツール取扱装置２が従取扱装置として
追従するかのようにして動作する。もちろん、上記の説
明から分かるようにツール取扱装置２はワーク取扱装置
１の動きを検出してそれに追従して動くというのではな
いが、両者が互いに相手の動きに関係なく演算に従って
同時に独立して動きながら、見た目にはツール２ｔがワ
ーク１ｗの動きにあわせて動いたように見えることにな
る。その際に、ワーク１ｗもツール２ｔもそれぞれの許
容速度以内の速度で動くことになる。したがって、ワー
ク１ｗの少しの動きに対してツール２ｔの動く範囲が大
きくても（図４４参照）、ツール２ｔの動作が許容速度
以下に抑えられるように、ワーク１ｗの動きに制限がか
けられたことになる。例えば、ワーク１ｗの動く範囲が
小さくても、ツール２ｔをゆっくりと動かすためにワー
ク１ｗは低速度で動かされることになる。これによって
教示操作する作業者がワーク１ｗの動きのみに着目して
連動教示操作しても、ツール２ｔが不意にもしくは思い
のほかに急速な動きをするというようなことはなくな
る。

【００４６】Ｓ４２３の直後には、Ｓ４２４，Ｓ４２５
へ進むと同時に、Ｓ４１８へ戻される。動作指示信号Ｓ
ｘ，Ｓｙ，Ｓｚ，Ｓα，Ｓβ，Ｓγが同じであれば次の
時間ΔＴにおいて同じことが繰り返される。動作指示信
号に変化があれば、動作指示信号の全てが消失していな
いか確認される〔Ｓ４３４〕。変更されているだけであ
れば、Ｓ４０２へ戻ってそれ以降のステップが続けられ
る。再度Ｓ４１８に至り、動作指示信号の全部が消滅し
ていれば、すなわち、動作指示スイッチ手段１２での操
作ボタンの全部が手放されていれば〔Ｓ４３４〕、ひと
まず教示操作は停止する〔Ｓ４３５〕。この状態でワー
ク１ｗもツール２ｔも停止し、ロボットを再現動作させ
たときにたどらせる位置と姿勢が確定する。その位置姿
勢が作業者の所望するものとなっていれば、教示ボタン
１３を押してそれを記憶させる。同様にして、次々と他
の位置と姿勢を教示すればよいことが分かる。この一連
の動作は、図３６のタイミングチャートで把握される
が、それについては図２４ないし図３０のフローチャー
トを用いて、実際の演算をする場合にあてはめて説明す
ることにする。なお、「ツール２ｔのワーク１ｗに対す
る位置姿勢」を一定に保つことに代えて、「ワーク１ｗ
のツール２ｔに対する位置姿勢」を一定に保つ場合も同
じであり、その場合には、主取扱装置がツール取扱装置
２で、従取扱装置がワーク取扱装置１となる。したがっ
て、主制御対象物はツール２ｔであり、従制御対象物は
ワーク１ｗとなる。このように入れ替えたものであって
も、上記の制御は可能である。しかし、従取扱装置は常
に６自由度のマニプレータとしておく必要のあることは
前述した。

【００４７】次に、異なる本発明に関連する部分を、図
３を参照して以下に説明する。これは、図６において、
動作切替スイッチ９が下へ、モード切替スイッチ８も下
へ、ロボット切替スイッチ７が上へ操作されている状態
での教示である。なお、図１と異なるところだけを述べ
る。二つのロボット１，２を連動して教示する際に機能
する連動関係制御手段４Ａは、前述した連動関係を記憶
するが、この間接手動連動関係制御手段４Ａj は、関節
手動操作モードにおいて、主取扱装置１により位置姿勢
が変化する主制御対象物１ｍと、主取扱装置１に協調動
作する従取扱装置２により位置姿勢が変化する従制御対
象物２ｓとの間の動作の関連づけを記憶し、ワーク１ｗ
の位置姿勢を変化させるワーク取扱装置１の動作を制御
すると共に、ツール２ｔの位置姿勢を変化させるツール
取扱装置２の動作を制御する。これの構成において、速
度成分指示値演算部２３に代わるものとして関節速度指
示値演算部３３が設けられる。これは、動作指示スイッ
チ手段１２からの動作指示信号Ｓ１，Ｓ２，Ｓ３，Ｓ
４，Ｓ５，Ｓ６にもとづき、ワーク１ｗの各関節の回転
速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，
Ｖ６_m を演算するものである。

【００４８】それ以外に、主取扱装置１に関連するもの
としては、主速度演算手段２４と補正速度指示値主演算
手段３５とがある。前者は、関節速度指示値演算部３３
によって演算された各関節の回転速度指示値Ｖ１_m ，Ｖ
２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m から、ワーク１
ｗの直角座標系における合成移動速度換算値Ｖ_m および
合成回転速度換算値ω_s とを求めるものである。後者
は、主速度演算手段２４によって求められた合成移動速
度換算値Ｖ_m もしくは合成回転速度換算値ω_s が主許容
速度設定手段２１により設定された許容移動速度Ｖ_{m AL}
もしくは許容回転速度ω_{m AL}を超えるとき、主速度演算
手段２４によって求められた合成移動速度換算値Ｖ_m も
しくは合成回転速度換算値ω_m が許容移動速度Ｖ_{m AL}も
しくは許容回転速度ω_{m AL}以下となるように、関節速度
指示値演算部３３で演算された各関節の回転速度指示値
Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を修
正するものである。それゆえに、補正速度指示値主演算
手段３５は、「主許容速度設定手段２１により設定した
許容移動速度Ｖ_{m AL}」を「主速度演算手段２４で求めた
合成移動速度換算値Ｖ_m 」で除したワーク１ｗの合成移
動速度比Ｖ_mR、もしくは、「主許容速度設定手段２１に
より設定した許容回転速度ω_{m AL}」を「主速度演算手段
２４で求めた合成回転速度換算値ω_m 」で除したワーク
１ｗの合成回転速度比ω_mRを演算し、それらの合成速度
比Ｖ_mR，ω_mRを関節速度指示値演算部３３で演算した各
関節の回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ
４_m ，Ｖ５_m ，Ｖ６_m に乗じて、回転速度指示値Ｖ
１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を修正
することができるようになっている。したがって、修正
された回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ
４_m ，Ｖ５_m ，Ｖ６_m でもって、ワーク１ｗの位置姿勢
をワーク取扱装置１により変化させることができる。

【００４９】さらに、従取扱装置２に関連するものとし
ては、従速度演算手段２６と補正速度指示値従演算手段
３７とがある。前者は、関節手動連動関係制御手段４Ａ
j に従い、ワーク取扱装置１と協調動作するツール取扱
装置２によって位置姿勢が変化されるツール２ｔの直角
座標系における合成移動速度換算値Ｖ_S と合成回転速度
換算値ω_S とを求めるものである。後者は、従速度演算
手段２６によって求められた合成移動速度換算値Ｖ_S も
しくは合成回転速度換算値ω_S が従許容速度設定手段２
２により設定された許容移動速度Ｖ_{s AL}もしくは許容回
転速度ω_{s AL}を超えるとき、従速度演算手段２６によっ
て求められた合成移動速度換算値Ｖ_S もしくは合成回転
速度換算値ω_S が許容移動速度Ｖ_{s AL}もしくは許容回転
速度ω_{s AL}以下となるように、関節速度指示値演算部３
３で演算された各関節の回転速度指示値Ｖ１_m ，Ｖ
２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を修正するもの
である。それゆえに、補正速度指示値従演算手段３７
は、「従許容速度設定手段２２により設定した許容移動
速度Ｖ_{s AL}」を「従速度演算手段２６で求めた合成移動
速度換算値Ｖ_S 」で除した従制御対象物２ｓの合成移動
速度比Ｖ_sR、もしくは、「従許容速度設定手段２２によ
り設定した許容回転速度ω_{s AL}」を「従速度演算手段２
６で求めた合成回転速度換算値ω_S 」で除した従制御対
象物２ｓの合成回転速度比ω_sRを演算し、それらの合成
速度比Ｖ_sR，ω_sRを関節速度指示値演算部３３で演算し
た各関節の回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ
４_m ，Ｖ５_m，Ｖ６_m に乗じて、その回転速度指示値Ｖ
１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ５_m ，Ｖ６_m を修正
することができるようになっている。したがって、修正
された回転速度指示値Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ
４_m ，Ｖ５_m ，Ｖ６_m でもってワーク１ｗの位置姿勢を
ワーク取扱装置１により変化させると共に、ツール２ｔ
の許容移動速度Ｖ_{s AL}もしくは許容回転速度ω_{s AL}以下
でもって、ツール２ｔの位置姿勢をツール取扱装置２に
より変化させることができる。これから分かるように、
ツール２ｔの動作速度の制限は、ワーク１ｗの動作速度
に制限をかけることによって可能となり、前述の例と同
じである。

【００５０】なお、関節手動連動関係制御手段４Ａj は
直角手動連動関係制御手段４Ａc に代わるものである
が、図２においては、その上下の中央に配置されるブロ
ックとして表される。もちろん、制御装置４の中にその
両方が設けられていてもよいし一方だけでもよい。両方
が設けられている制御装置４においては、その選択が図
６中のモード切替スイッチ８によってなされることにな
る。もちろん、この連動関係制御手段４Ａj において
も、ワーク取扱装置１により位置姿勢が変化するワーク
１ｗとワーク取扱装置１に協調動作するツール取扱装置
２により位置姿勢が変化するツール２ｔとの間の動作の
関連づけは、「ツール２ｔのワーク１ｗに対する相対的
な位置姿勢」を一定に保つ場合だけでなく、「大地に対
するツール２ｔの姿勢」を一定に保ち、かつ「ツール２
ｔのワーク１ｗに対する相対的な位置」を維持するよう
な場合にも適用することができる。そして、前者におい
ては、主取扱装置と従取扱装置とを入れ替えることもで
きる。

【００５１】この場合の制御のフローチャートは図１３
から図１６に表される。その説明の詳細は図３７ないし
図４２でなされているので省くことにするが、図９から
図１２までのフローチャートの場合と少し手順が異なっ
ているもののほとんど同じ思想である。すなわち、Ｓ５
１０の後のＳ５３０，Ｓ５３１（図１４参照）における
主取扱装置１の速度低減操作や、Ｓ５１７の後のＳ５３
２，Ｓ５３３（図１５参照）における従取扱装置２の速
度低減操作にもとづく主取扱装置１の速度制限操作の演
算は、図９のＳ４３０，Ａ４３１や図１１のＳ４３２，
Ａ４３３と同様に理解される。

【００５２】次に、上記のシステムにおけるワーク取扱
装置１とツール取扱装置２との本発明に係る手動教示操
作について、図２４ないし図３０のフローチャートをも
参照しながら説明する。なお、前述した主取扱装置に関
連して符号の添え字に「マスター」の「ｍ」を採用し、
従取扱装置に関連して符号の添え字に「スレーブ」の
「ｓ」を採用しているものについては、以後の説明にお
いて、主取扱装置をワーク取扱装置とし、従取扱装置を
ツール取扱装置とするので、該当する符号の添え字に
「ワーク」の「ｗ」を、「ツール」の「ｔ」を付して表
記することにする。図６のモード切替スイッチ８により
「直角手動操作モード」が選定され、動作切替スイッチ
９により「連動する手動操作」が選択されている場合の
「連動する直角手動操作モード」について述べる。これ
は、「ツール２ｔの姿勢」については絶対座標系５１す
なわち大地６４に対して一定に維持されるように、「ツ
ール２ｔの位置」の変化については「ワーク１ｗの位置
姿勢」の変化に連動して「ワーク１ｗに対するツール２
ｔの相対的な位置」が維持されるように、図５の絶対座
標系５１においてワーク取扱装置１およびツール取扱装
置２のそれぞれの関節を動作させ、ワーク基準点５６を
所望する位置姿勢へ変更するときの動作である。

【００５３】「連動する手動操作」による教示作動に先
だち、まずは、ワーク１ｗとツール２ｔの位置決めをし
おく。これは、一般的には「単独の手動操作」によって
行われる。例えば、図３５の（ａ）を参照して、ワーク
１ｗ上の「着目点」Ｂ₁ の接線Ｂ_1Sが「大地６４に対し
て所望する傾き」δ_w となるように、ワーク取扱装置１
によって「ワーク１ｗの位置姿勢」を与える。次に、ワ
ーク１ｗ上のその「着目点」Ｂ₁ において所望する「大
地６４に対するツール２ｔの姿勢」がツール取扱装置２
によって与えられる。これによって得られるワーク１ｗ
とツール２ｔの位置決め状態を得る。この位置決め状態
を「第一の教示点」Ａ₁ として教示する。なお、教示す
る必要がなければ、位置決めだけを行っておけばよい。
いずれの場合も、与えられた「ワーク１ｗの位置姿勢」
にもとづく「ワーク取扱装置１の関節変数」はφ_1-0，
φ_2-0 ，φ_3-0 ，φ_4-0 ，φ_5-0 ，φ_6-0 として関節変
数記憶部１９ｂw （図１参照）に記憶され、各センサＳ
_1w ,Ｓ_2w・・・Ｓ_6w（図１７参照）はその値を検出して
いる。一方、与えられた「ツール２ｔの位置姿勢」にも
とづく「ツール取扱装置２の関節変数」はθ_1-0 , θ
_2-0 , θ_3-0 , θ_4-0 , θ_5-0 , θ_6-0 として関節変数
記憶部１９ｂt に記憶され、各センサＳ_1t ,Ｓ_2t・・・
Ｓ_6tはその値を検出している。

【００５４】ティーチングペンダント３（図６参照）に
おいて、速度率切替スイッチ１０で所望する「速度率」
を、例えば０．５である「ｒ_b 」に選定する。そこで、
所望する操作ボタン例えば１２ｂ，１２ｃ，１２Ｅが押
されると〔図２４のフローチャートのステップ１、以下
ST01などと記す〕、それらが手放されるまでの処理は次
のようになる。操作ボタン１２ｂ，１２ｃ，１２Ｅを押
すことによって、「動作指示信号」が取り込まれる〔ST
02〕。ワーク１ｗ上に定められたワーク取付点Ｈ_0w（図
５参照）の各軸方向の移動速度成分指示値および各軸回
りの回転速度成分指示値が、以下のようにして求められ
る。操作ボタン１２ｂ，１２ｃ，１２Ｅにより生成され
た「動作指示信号」Ｓｘ，Ｓｙ，Ｓｚ，Ｓα，Ｓβ，Ｓ
γと、選定された「速度率」ｒ_b と、ワーク１ｗの直角
手動操作のための速度テーブルに記憶されている最高値
Ｖｘ_w0 ,Ｖｙ_w0 ,Ｖｚ_w0 ,ωｘ_w0 ,ωｙ_w0 ,ωｚ_w0とを
用いて、図２０に示す移動速度成分指示値Ｖｘ_w , Ｖｙ
_w ，Ｖｚ_w 、および図２１に示す回転速度成分指示値ω
ｘ_w ，ωｙ_w，ωｚ_w が、式(1) から演算される〔ST0
3〕。

【数１】

【００５５】ＳＴ０３で演算した移動速度成分指示値Ｖ
ｘ_w , Ｖｙ_w ，Ｖｚ_w から主動作機械としてのワーク取
扱装置１におけるワーク１ｗの合成移動速度換算値Ｖ_w
（図２２参照）と、回転速度成分指示値ωｘ_w ，ω
ｙ_w ，ωｚ_w からワーク１ｗの合成回転速度換算値ω_w
（図２３参照）とを求める。まず、合成移動速度換算値
Ｖ_w はＸ，Ｙ，Ｚ各方向の移動速度成分指示値の合成に
より求めることができる〔ST04〕。すなわち、

【数２】 また、回転に関してはＸ，Ｙ，Ｚの三つの軸に関する回
転速度成分指示値の合成を求めて、その回転をある一つ
の軸Ｋ_w に関するω_w である合成回転速度換算値として
計算する。以下に、その手法を述べる。今、Ｘ軸回りに
ωｘ_w 回転する場合のワーク取付点Ｈ_0wの回転量は、次
のような同時変換行列で記述できることが知られてい
る。なお、下記のRot はRotationの略である。

【数３】 三つの軸の回転の合成は、上記の式(3) ないし式(5) を
順次乗じて、以下のように表現できる。

【数４】 そして、上式の計算結果を、以下のように定める。

【数５】 上式の軸Ｋ_w およびワーク１ｗの合成回転速度換算値ω
_w は、以下の式によって与えられる〔ST05〕。

【数６】

【００５６】既に記憶されているワーク１ｗの許容移動
速度Ｖ_{w AL}と許容回転速度ω_{w AL}とを読み取る〔ST0
6〕。ＳＴ０４で求めたワーク１ｗの合成移動速度換算
値Ｖ_w と許容移動速度Ｖ_{w AL}とを比較する〔ST07〕。合
成移動速度換算値Ｖ_w が許容移動速度Ｖ_{w AL}以下の場合
には、ＳＴ０５で求めたワーク１ｗの合成回転速度換算
値ω_w と許容回転速度ω_{w AL}とを比較する〔ST08〕。合
成回転速度換算値ω_w が許容回転速度ω_{w AL}以下の場合
にはＳＴ１６（図２５参照）へ進む。合成回転速度換算
値ω_w が許容回転速度ω_{w AL}よりも大きい場合には〔ST
08〕、ワーク１ｗの合成回転速度比ω_wRをω_{w AL}／ω_w
により求める〔ST09〕。ワーク１ｗの合成回転速度換算
値がω_{w AL}となるように設定し、同時に移動速度成分指
示値も減少させる。そのために、ＳＴ０３で演算された
移動速度成分指示値Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w と、回転速
度成分指示値ωｘ_w ，ωｙ_w ，ωｚ_w とをそれぞれω_wR
倍する〔ST10〕。すなわち、

【数７】 一方、ＳＴ０７で合成移動速度換算値Ｖ_w が許容移動速
度Ｖ_{w AL}よりも大きい場合は、ワーク１ｗの合成移動速
度比Ｖ_wRをＶ_{w AL}／Ｖ_w により求める〔ST11〕。そし
て、ＳＴ０５で求めたワーク１ｗの合成回転速度換算値
ω_w と許容回転速度ω_{w AL}とを比較する〔ST12〕。合成
回転速度換算値ω_w が許容回転速度ω_{w AL}以下の場合に
は、ワーク１ｗの合成移動速度換算値がＶ_{w AL}となるよ
うに設定し、同時に回転速度成分指示値も減少させる。
そのために、ＳＴ０３で演算された移動速度成分指示値
Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w と、回転速度成分指示値ω
ｘ_w ，ωｙ_w ，ωｚ_w とをそれぞれＶ_wR倍する〔ST1
3〕。すなわち、

【数８】 一方、ＳＴ１２で、合成回転速度換算値ω_w が許容回転
速度ω_{w AL}より大きい場合は、ワーク１ｗの合成回転速
度比ω_wRをω_{w AL}／ω_w により求める〔ST14〕。そし
て、ＳＴ１１で求めたワーク１ｗの合成移動速度比Ｖ_wR
とＳＴ１４で求めた合成回転速度比ω_wRとを比較する
〔ST15〕。合成移動速度比Ｖ_wRが合成回転速度比ω_wR以
下であればＳＴ１３へ進み、合成移動速度比Ｖ_wRが合成
回転速度比ω_wRより大きければＳＴ１０へ進む。このよ
うに、たどる経路は４通り存在するが、いずれにして
も、ワーク取付点Ｈ_0wの移動速度成分指示値Ｖｘ_w , Ｖ
ｙ_w ，Ｖｚ_w と回転速度成分指示値ωｘ_w，ωｙ_w ，ω
ｚ_w との合成値が、読み取られた許容移動速度Ｖ_{w AL}や
許容回転速度ω_{w AL}以下の次の表の値とされる。

【表１】 このようにして、ワーク１ｗの移動速度成分指示値Ｖｘ
_w , Ｖｙ_w ，Ｖｚ_w と回転速度成分指示値ωｘ_w ，ωｙ
_w ，ωｚ_w とが補正された場合には、その補正値を用い
てワーク１ｗの合成速度換算値が許容速度以下に調整さ
れる。したがって、ワーク１ｗの速度が許容された範囲
内におさまるように低減され、ワーク１ｗの過大な動き
の生じるのが防止される。

【００５７】ところで、この速度減少操作は、図３１に
示すようにすることもできる。ワーク１ｗの許容移動速
度Ｖ_{w AL}と許容回転速度ω_{w AL}とを読み取った〔ST06〕
後に、ＳＴ０４で求めたワーク１ｗの合成移動速度換算
値Ｖ_w と許容移動速度Ｖ_{w AL}とを比較する〔ST07Ａ〕。
合成移動速度換算値Ｖ_w が許容移動速度Ｖ_{w AL}より大き
い場合にはＳＴ１１Ａへ進む。合成移動速度換算値Ｖ_w
が許容移動速度Ｖ_{w AL}以下の場合にはＳＴ０８Ａへ進
み、合成回転速度換算値ω_w と許容回転速度ω_{w AL}とを
比較する。合成回転速度換算値ω_w が許容回転速度ω
_{w AL}以下の場合はＳＴ１６へ進み、合成回転速度換算値
ω_w が許容回転速度ω_{w AL}より大きい場合には、ワーク
１ｗの合成回転速度比ω_wRをω_{w AL}／ω_w により求める
〔ST09Ａ〕。これによって、ワーク１ｗの合成回転速度
換算値がω_{w AL}となるように設定する。そのために、Ｓ
Ｔ０３で演算された回転速度成分指示値ωｘ_w , ω
ｙ_w ，ωｚ_w をω_wR倍する〔ST10Ａ〕。すなわち、

【数９】 ＳＴ０７Ａで合成移動速度換算値Ｖ_w が許容移動速度Ｖ
_{w AL}よりも大きい場合には、ワーク１ｗの合成移動速度
比Ｖ_wRをＶ_{w AL}／Ｖ_w により求める〔ST11Ａ〕。さら
に、ＳＴ０５で求められたワーク１ｗの合成回転速度換
算値ω_w と許容回転速度ω_{w AL}とを比較する〔ST12
Ａ〕。合成回転速度換算値ω_w が許容回転速度ω_{w AL}以
下の場合には、ワーク１ｗの合成移動速度換算値がＶ
_{w AL}となるように設定する。そのために、ＳＴ０３で演
算された移動速度成分指示値Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w を
Ｖ_wR倍する〔ST13Ａ〕。すなわち、

【数１０】 一方、合成回転速度換算値ω_w が許容回転速度ω_{w AL}よ
り大きい場合には、ワーク１ｗの合成回転速度比ω_wRを
ω_{w AL}／ω_w により求め〔ST14Ａ〕、ワーク１ｗの合成
移動速度換算値がＶ_{w AL}となるように、また、合成回転
速度換算値がω_{w AL}となるように設定する〔ST15Ａ〕。
すなわち、

【数１１】 このような過程を採る場合には、ワーク取付点Ｈ_0wの速
度の選定は、以下のようになることが理解される。

【表２】

【００５８】また、上記の速度減少操作を、図３２に示
すようにすることもできる。ワーク１ｗの許容移動速度
Ｖ_{w AL}と許容回転速度ω_{w AL}とを読み取った〔ST06〕後
に、ＳＴ０４で求めたワーク１ｗの合成移動速度換算値
Ｖ_w と許容移動速度Ｖ_{w AL}とを比較する〔ST07Ｂ〕。合
成移動速度換算値Ｖ_w が許容移動速度Ｖ_{w AL}以下の場合
にはＳＴ１０Ｂへ進む。合成移動速度換算値Ｖ_w が許容
移動速度Ｖ_{w AL}よりも大きい場合には、ワーク１ｗの合
成移動速度比Ｖ_wRをＶ_{w AL}／Ｖ_w により求め〔ST08
Ｂ〕、ワーク１ｗの合成移動速度換算値をＶ_{w AL}となる
ように設定する。そのために、ＳＴ０３で演算された移
動速度成分指示値Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w をＶ_wR倍する
〔ST09Ｂ〕。すなわち、

【数１２】 次に、ＳＴ０５で求めたワーク１ｗの合成回転速度換算
値ω_w と許容回転速度ω_{w AL}とを比較する〔ST10Ｂ〕。
合成回転速度換算値ω_w が許容回転速度ω_{w AL}以下の場
合にはＳＴ１６へ進む。合成回転速度換算値ω_w が許容
回転速度ω_{w AL}よりも大きい場合には、ワーク１ｗの合
成回転速度比ω_wRをω_{w AL}／ω_w により求め〔ST11
Ｂ〕、ワーク１ｗの合成回転速度換算値がω_{w AL}となる
ように設定する。そのために、ＳＴ０３で演算された回
転速度成分指示値ωｘ_w , ωｙ_w ，ωｚ_w をω_wR倍する
〔ST12Ｂ〕。すなわち、

【数１３】 このような過程を採る場合には、ワーク取付点Ｈ_0wの速
度の選定は以下のようになる。

【表３】

【００５９】次に、「動作指示信号」を受ける直前の
「ワーク取扱装置１の関節変数」および「ツール取扱装
置２の関節変数」から、「絶対座標系５１におけるワー
ク基準点５６の現在の位置姿勢」を演算する。まず、
「動作指示信号」を受ける前にすでにワーク取扱装置１
の各可動部分に取り付けられたセンサＳ_1w ,Ｓ_2w ,・・
・ ,Ｓ_6w（図１７参照）から、「ワーク取扱装置１の現
在の関節変数」φ_1-0 ，φ_2-0 ，φ_3-0 ，φ_4-0 ，φ
_5-0 ，φ_6-0 を取り込む〔図２５のST16〕。そして、後
述する式(10)を用いて「ワーク取扱装置１の設置基準点
Ｂ_0w（図５参照）に対するワーク取付点Ｈ_0wの位置姿
勢」を表す同次変換行列Ｗ_2-0 を算出する。そのため
に、一般にＤｅｎａｖｉｔ−Ｈａｒｔｅｎｂｅｒｇの表
現を用いて計算を行う。まず、この表現について説明す
る。図１８は、ワーク取扱装置１の設置基準点Ｂ_0wを原
点とする基準座標系（Ｂ_0w−ＸＹＺ）と、ワーク取付点
Ｈ_0wに設けたメカニカルインターフェース座標系（Ｈ_0w
−Ｘ’Ｙ’Ｚ’）とを示している。さらに、設置基準点
Ｂ_0wから順に各関節に対して系統づけて定めた座標原点
Ｏ_{0w ,}Ｏ_{1w ,}・・・，Ｏ_5wによる座標系が、（Ｏ_0w−Ｘ
₀ Ｙ₀ Ｚ₀ ），（Ｏ_1w−Ｘ₁ Ｙ₁ Ｚ₁ ），・・，（Ｏ_5w
−Ｘ₅ Ｙ₅ Ｚ₅ ）によって示されている。第一関節の座
標原点はＯ_0wであり、その座標系はＯ_0w−Ｘ₀ Ｙ₀ Ｚ₀
である。前記の系統づけられた座標系ごとの関係は、Ｄ
ｅｎａｖｉｔ−Ｈａｒｔｅｎｂｅｒｇの表現によると、
隣接するリンク間の回転の関係を示す次式として表現で
きる。

【数１４】 上記の式(9) 中の

【数１５】

【００６０】そこで、これを説明するために、ワーク取
扱装置１のリンクを分けたものを図１８に示す。設置基
準点Ｂ_0wと第一関節の間の符号１１Ａを台座リンクと呼
び、第一関節と第二関節の間を第一リンクと呼ぶことに
する。なお、いま対象としているのはワーク取扱装置で
あるので、それぞれには、添え字の「_w 」を付してい
る。先ず、台座リンク１１Ａと第一リンク１１Ｂとの関
係を示す。台座リンク１１ＡのＺ軸に対して第一リンク
のＺ軸（Ｚ_0w）は同一方向であるため、Ｘを中心とした
回転角度α_0wは０である。そして、台座リンクの原点Ｏ
をＺ軸方向にＤ₀ 、Ｘ軸方向に０移動したところに第一
リンクの原点Ｏ_0wがある。台座リンク１１と第一リンク
１１ＢのＺ軸回りの回転φ_0wは０である。よって、式
(9) で示されるリンクパラメータは、次のようになる。

【数１６】 次に、第一リンク１１Ｂと第二リンク１１Ｃの関係を示
す。第一リンク１１ＢのＺ軸であるＺ_0wに対して第二リ
ンク１１ＣのＺ軸であるＺ_1wは、第一リンク１１ＢのＺ
軸を中心として＋π／２回転した位置にある。そして、
第一リンク１１Ｂの原点Ｏ_0wのＺ軸方向にＤ₁ 、Ｘ軸方
向にＬ₁ 離れたところに第二リンク１１Ｃの原点Ｏ_1wが
ある。さらに、第一リンク１１Ｂが台座リンク１１Ａに
対してＺ_0w軸回りにΦ₁ 回転した位置にある。このΦ₁
は、図１７におけるサーボモータＭ_1wと減速機Ｕ_1wとに
よって回転可能なものであり、関節変数φ₁ と呼ぶ。こ
れは、センサＳ_1wによって検出される。よって、式(9)
で示されるリンクパラメータは、次のようになる。

【数１７】 さらに、第二リンク１１Ｃと第三リンク１１Ｄの関係を
示す。第二リンク１１ＣのＺ軸（Ｚ_1w）に対し、第三リ
ンク１１ＤのＺ軸（Ｚ_2w）は同一方向であるため、第二
リンク１１Ｃの原点Ｏ_1wのＺ軸方向に０、Ｘ軸方向にＬ
₂ 離れたところに第三リンク１１Ｄの原点Ｏ_2wがある。
よってｄ_2w＝０、ａ_2w＝Ｌ₂ である。さらに、第二リン
ク１１Ｃが第一リンク１１Ｂに対してＺ_1w軸回りにΦ₂
回転した位置にある。このΦ₂ はサーボモータＭ_2wと減
速機Ｕ_2wとによって回転可能なものであり、回転変数φ
₂ と呼ぶ。これは、センサＳ_2wによって検出される。よ
って、式(9) で示されるリンクパラメータは次のように
なる。

【数１８】 以下、同様にして、ワーク移動装置１のリンクパラメー
タを纏めると、表４のようになる。

【表４】 上記のＤｅｎａｖｉｔ−Ｈａｒｔｅｎｂｅｒｇの表現を
用いると、ワーク取扱装置１の設置基準点Ｂ_0wに対する
ワーク取付点Ｈ_0wの現在の位置姿勢Ｗ_2-0 は、式(9) で
表されるＡ_0wからＡ_6wまでを掛けあわせることにより算
出することができる〔ST17〕。すなわち、

【数１９】 「絶対座標系５１におけるワーク基準点５６の現在の位
置姿勢」を表す同次変換行列world Ｘ_w-o が、式(10)を
用いて次式(11)のように表される〔ST18〕。

【数２０】 ただし、図５から分かるように、Ｚ_w は「絶対座標系５
１におけるワーク取扱装置１の設置基準点Ｂ_0wの位置姿
勢」を表す同次変換行列であり、Ｅ_w は「ワーク取付点
Ｈ_0wに対するワーク基準点５６の位置姿勢」を表す同次
変換行列である。Ｚ_w のデータとＥ_w のデータとは既知
値であり、制御装置４に予め記憶されている。式(11)
は、下式のようにおくことができる。

【数２１】 式(12)を変換すると、「絶対座標系５１におけるワーク
基準点５６の現在の位置姿勢」のデータ、すなわち「動
作指示信号を受ける直前での絶対座標系５１におけるワ
ーク基準点５６の位置」のデータ（Ｘ_w-o , Ｙ_w-o , Ｚ
_w-o ）が得られる。加えて、「動作指示信号を受ける直
前での絶対座標系５１におけるワーク基準点５６の姿
勢」のデータ（α_w-o ，β_w-o ，γ_w-o ）も得られる。
これらは、下式で等価となる６つのパラメータとして求
められる〔ST19〕。

【数２２】

【００６１】「動作指示信号」を受ける直前の「ワーク
取扱装置１の関節変数」および「ツール取扱装置２の関
節変数」から、「大地６４に対するツール先端点５７の
姿勢」と、「ワーク基準点５６に対するツール先端点５
７の相対的な位置」とを求める。すなわち、「絶対座標
系５１におけるツール先端点５７の連動手動操作開始前
の姿勢」と、「ワーク基準点５６に対するツール先端点
５７の連動手動操作開始前の相対的な位置」とが演算さ
れる。次に、従動作機械であるツール取扱装置２の各可
動部分に取り付けられているセンサＳ_1t ,Ｓ_2t ,・・・
,Ｓ_6t（図１７参照）から、「ツール取扱装置２の現在
の関節変数」θ_1-0 , θ_2-0 , θ_3-0 , θ_4-0 ,
θ_5-0 , θ_6-0 を取り込む〔ST20〕。そして、以下のよ
うにして、「ツール取扱装置２の設置基準点Ｂ_0tに対す
るツール取付点Ｈ_0tの現在の位置姿勢」を表す同次変換
行列Ｔ_2-0 を算出する〔ST21〕。同次変換行列Ｔ
_2-0 は、式(10)のＷ_2-0 の場合と同様に、Ｄｅｎａｖｉ
ｔ−Ｈａｒｔｅｎｂｅｒｇの表現によるリンクパラメー
タを使った式(17)の同次変換行列の積として得られる式
(18)のように表すことができる。

【数２３】 上記の式(17)中の

【数２４】

【００６２】「ツール先端点５７の姿勢」は、「大地６
４に対するツール先端点５７の連動手動操作開始前の姿
勢」を維持する必要があるため、「動作指示信号」を受
ける直前での「絶対座標系５１におけるツール先端点５
７の姿勢」を求める。そのためには、まず、「絶対座標
系５１におけるツール先端点５７の連動手動操作開始前
の位置姿勢」を表す同次変換行列world Ｘ_t-o が、式(1
8)を代入して次式(19)によって算出される〔ST22〕。

【数２５】 ただし、図５から分かるように、Ｚ_t は「絶対座標系５
１におけるツール取扱装置２の設置基準点Ｂ_0tの位置姿
勢」を表す同次変換行列であり、Ｅ_t は「ツール取付点
Ｈ_0tに対するツール先端点５７の位置姿勢」を表す同次
変換行列である。Ｚ_t のデータとＥ_t のデータとは既知
値であり、制御装置４に予め記憶されている。上記の式
(19)の各要素を、下式(19a) ないし式(19c) に代入する
ことにより、「絶対座標系５１におけるツール先端点５
７の現在の姿勢」のデータ（α_t-0 ，β_t-0 ，γ_t-0 ）
が得られる〔ST23〕。

【数２６】 ここで、「ワーク基準点５６に対するツール先端点５７
の連動手動操作開始前の相対的な位置」を維持させる必
要のあることから、「動作指示信号を受ける直前でのワ
ーク基準点５６に対するツール先端点５７の相対的な位
置」を求める。「ワーク基準点５６に対するツール先端
点５７の連動手動操作開始前の相対的な位置姿勢」を表
す同次変換行列を wＸ_t とすると、「絶対座標系５１に
おけるツール先端点５７の連動手動操作開始前の位置姿
勢」を表す同次変換行列world Ｘ_t-o は、図５から分か
るように次式(20)のごとく表される。

【数２７】 したがって、上記の同次変換行列 wＸ_t は、式(12)と式
(19)とから次式(21)で表される〔ST24〕。

【数２８】 上記の式(21)で表現される wＸ_t は、「ワーク基準点５
６に対するツール先端点５７の連動手動操作開始前の相
対的な位置姿勢」を表す同次変換行列であり、「ツール
先端点５７の位置」が「ワーク基準点５６の位置」の変
化に連動すればよいから、必要となるのは「動作指示信
号を受ける直前でのワーク基準点５６に対するツール先
端点５７の相対的な位置」のデータ（Ｘ _wｘ_t ，Ｙ _wｘ
_t ，Ｚ _wｘ_t ）である。それらは、上記の式(21)の４列
目の要素より、次式のように表現することができる。

【数２９】 式(21a) から「ワーク基準点５６に対するツール先端点
５７の連動手動操作開始前の相対的な位置」を表す同次
変換行列 wＴ_t は、次式により求めることができる〔ST
25〕。

【数３０】

【００６３】「絶対座標系５１におけるワーク基準点５
６の時間ΔＴ経過後の位置」のデータ（Ｘ_w-i ，
Ｙ_w-i ，Ｚ_w-i ）と、「絶対座標系５１におけるワーク
基準点５６の時間ΔＴ経過後の姿勢」のデータ
（α_w-i ，β_w-i ，γ_w-i ）とが、既に演算されている
移動速度成分指示値Ｖｘ_w , Ｖｙ_w , Ｖｚ_w および回転
速度成分指示値ωｘ_w , ωｙ_w , ωｚ_w を用いて、次式
(22)によって求められる〔図２７のST26〕。

【数３１】 すなわち、時間ΔＴ経過後のＸ_w-i は、式(13)で得られ
ている「絶対座標系５１におけるワーク基準点５６の現
在の位置」のデータＸ_w-o に、表１ないし表３のいずれ
かで得られている移動速度成分指示値Ｖｘ_w に経過時間
ΔＴを乗じたものを加えることにより求められる。Ｙ
_w-i ないしγ_w-i も同様である。「絶対座標系５１にお
けるワーク基準点５６の時間ΔＴ経過後の位置姿勢」を
表す同次変換行列 worldＸ_w-i は、次式(23)によって求
められる〔ST27〕。

【数３２】 この式(23)は、上記した式(11)と同様にして、次式(24)
のように表される。

【数３３】 「ワーク取扱装置１の設置基準点Ｂ_0wに対するワーク取
付点Ｈ_0wの時間ΔＴ経過後の位置姿勢」を表す同次変換
行列Ｗ_2-i は式(25)によって表され、式(23)を代入して
演算することができる〔ST28〕。

【数３４】

【００６４】一方、ワーク基準点５６が式(23)に従って
動作しても、「ツール先端点５７の姿勢」は変化させ
ず、「絶対座標系５１におけるツール先端点５７の位
置」の変化のみを、「ワーク基準点５６の位置姿勢」の
変化に連動させる必要がある。したがって、そのときの
「ワーク基準点５６に対するツール先端点５７の相対的
な位置」を変えないようにするための「ツール取扱装置
２の設置基準点Ｂ_0tに対するツール取付点Ｈ_0tの時間Δ
Ｔ経過後の位置姿勢」を表す同次変換行列Ｔ_2-i が求め
られる。まず、ワーク基準点５６の動きに追従するため
に、「絶対座標系５１におけるツール先端点５７の位
置」は、ＳＴ２５で式(21b) から求められた「手動操作
開始前のワーク基準点５６に対するツール先端点５７の
相対的な位置」を表す同次変換行列 wＴ_t と式(23)とを
用いて、次式で算出される。

【数３５】 「絶対座標系５１におけるツール先端点５７の位置」
は、上記の式(26)の４列目の要素より、次式で表され
る。

【数３６】 ワーク基準点５６が式(23)に従って動作したときの「絶
対座標系５１におけるツール先端点５７の時間ΔＴ経過
後の位置姿勢」を表す同次変換行列world Ｘ_t-i を、上
記の式(26a) と、式(19a) ないし(19c) によって算出さ
れた「絶対座標系５１におけるツール先端点５７の現在
の姿勢」のデータ（α_t-o ，β_t-o ，γ_t-o ）から、次
のように求めることができる。

【数３７】 これは、式(19)と同様に、式(28)のように表現される
〔ST29〕。

【数３８】 この式(28)を変形することによって、「ツール取扱装置
２の設置基準点Ｂ_0tに対するツール取付点Ｈ_0tの時間Δ
Ｔ経過後の位置姿勢」を表す同次変換行列Ｔ_2-iは、以
下のように関連づけることができる。

【数３９】 ワーク基準点５６が式(23)に従って動作しても、「ワー
ク基準点５６に対するツール先端点５７の相対的な位
置」を変えないようにするための「ツール取扱装置２の
設置基準点Ｂ_0tに対するツール取付点Ｈ_0tの時間ΔＴ経
過後の位置姿勢」を表す同次変換行列Ｔ_2-i は、上記の
式(29)に式(27)を代入することにより得られる〔ST3
0〕。

【００６５】ここで、ツール２ｔの平均速度である合成
移動速度換算値Ｖ_t と合成回転速度換算値ω_t とを算出
する。すなわち、ツール取付点Ｈ_0tの時間ΔＴが経過す
るまでの合成速度を求める。まず、ツール２ｔの合成移
動速度換算値Ｖ_t は、以下のようにして演算される。な
お、ここで上記の同次変換行列Ｔ_2-0 および時間ΔＴ経
過後の同次変換行列Ｔ_2-i について図１９をもとにして
説明する。ツール取扱装置２を例にした座標系Ｂ_0t−Ｘ
ＹＺは、ツール取扱装置２の設置基準点Ｂ_0tに固定され
た直交座標系である。点Ｈ_0tはＢ_0t−ＸＹＺ座標系にお
けるツール取付点の位置を表す。これを次式のようにお
く。

【数４０】 また、Ｂ_0t−ＸＹＺ座標空間におけるツール取付点Ｈ_0t
の姿勢を表現するために三つの単位ベクトル；ｐ_1t，ｐ
_2t，ｐ_3tがツール取付点Ｈ_0tに固定してとられている。
これらの単位ベクトルの成分を次式で定義する。

【数４１】 以上より、「ツール取扱装置２の設置基準点Ｂ_0tに対す
るツール取付点Ｈ_0tの現在の位置姿勢」を表す前述した
同次変換行列Ｔ_2-0 の式(18)は、次式によって表現され
る。

【数４２】 これは次式のように表される。

【数４３】 式(18b) において、左上の３行３列の部分行列は姿勢を
表し、右上の３行１列の部分行列は位置ベクトルを表す
ものである。したがって、前述した式(18)を式(18b) の
ように置き換えれば、ツール取扱装置２の設置基準点Ｂ
_0tに対するツール取付点Ｈ_0tの位置は、上式の右上の行
列Ｑ₀ 、すなわち、行列（Ｈ_0xt0 ,Ｈ_0yt0 ,Ｈ_0zt0) ^T
によって表される。同様に、「ツール取扱装置２の設置
基準点Ｂ_0tに対するツール取付点Ｈ_0tの時間ΔＴ経過後
の位置姿勢」を表す前述した同次変換行列Ｔ_2-i も、以
下のように表すことができる。

【数４４】 式(18c) において、左上の３行３列の部分行列は姿勢を
表し、右上の３行１列の部分行列は位置ベクトルを表す
ことも同じである。したがって、前述した式(29)を式(1
8c) のように置き換えれば、ツール取扱装置２の設置基
準点Ｂ_0tに対するツール取付点Ｈ_0tの時間ΔＴ経過後の
位置は、上式の右上の行列Ｑ_i 、すなわち、行列（Ｈ
_0xti ,Ｈ_0yti ,Ｈ_0zti ) ^Tによって表される。それゆえ
に、時間ΔＴが経過するまでのツール取付点Ｈ_0tの平均
的な合成移動速度換算値Ｖ_t は、上記二つのベクトルの
差のノルムをΔＴで除すことにより算出できる〔ST3
1〕。すなわち、

【数４５】 次に、ツール２ｔの平均した合成回転速度換算値ω
_t が、上記したＴ_2-0 からＴ_2-i への姿勢変化量Ω_t を
ΔＴで除して求められる。Ｔ_2-0 およびＴ_2-i の姿勢成
分のみを取り出した同次変換行列をＰ_2-0 とＰ_2-i とす
ると、以下の関係が成立する。

【数４６】 上式のＫ_t は等価回転軸である。上式を変形することに
より、

【数４７】 となる。上式において、式(8) と同様にして等価回転軸
Ｋ_t および姿勢変化量Ω_t を計算する。以上より、合成
回転速度換算値ω_t は下式によって求められる〔ST31〕

【数４８】

【００６６】既に記憶されているツール２ｔの許容移動
速度Ｖ_{t AL}と許容回転速度ω_{t AL}とを読み取る〔ST3
2〕。そこで、ＳＴ３１で求めたツール２ｔの合成移動
速度換算値Ｖ_t と許容移動速度Ｖ_{t AL}とを比較する〔ST
33〕。合成移動速度換算値Ｖ_t が許容移動速度Ｖ_{t AL}以
下の場合にはＳＴ３４へ進む。そして、ＳＴ３１で求め
たツール２ｔの合成回転速度換算値ω_t とツール２ｔの
許容回転速度ω_{t AL}とを比較する。合成回転速度換算値
ω_t が許容回転速度ω_{t AL}以下の場合にはＳＴ４２へ進
む。合成回転速度換算値ω_t が許容回転速度ω_{t AL}より
も大きい場合には〔ST34〕、ツール２ｔの合成回転速度
比ω_tRをω_{t AL}／ω_t により求める〔ST35〕。ツール２
ｔの合成回転速度換算値がω_{t AL}となるように、ワーク
１ｗの回転速度成分指示値を減少させ、同時に、ワーク
１ｗの移動速度成分指示値を減少させる。そのために、
ＳＴ１６の直前で得られている移動速度成分指示値Ｖｘ
_w , Ｖｙ_w ，Ｖｚ_w と回転速度成分指示値ωｘ_w ，ωｙ
_w ，ωｚ_w とを、それぞれω_tR倍する〔ST36〕。すなわ
ち、

【数４９】 一方、ＳＴ３３で、合成移動速度換算値Ｖ_t が許容移動
速度Ｖ_{t AL}よりも大きい場合には、ツール２ｔの合成移
動速度比Ｖ_tRをＶ_{t AL}／Ｖ_t により求める〔ST37〕。そ
して、ＳＴ３１で求めたツール２ｔの合成回転速度換算
値ω_t と許容回転速度ω_{t AL}とを比較する〔ST38〕。合
成回転速度換算値ω_t が許容回転速度ω_{t AL}以下の場合
には、ツール２ｔの合成移動速度換算値がＶ_{t AL}となる
か近づくようにワーク１ｗの移動速度成分指示値を減少
させ、同時に、ワーク１ｗの回転速度成分指示値も減少
させる。そのために、ＳＴ１６の直前で得られている移
動速度成分指示値Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w と回転速度成
分指示値ωｘ_w ，ωｙ_w ，ωｚ_w とを、それぞれＶ_tR倍
する〔ST39〕。すなわち、

【数５０】 一方、ＳＴ３８で、合成回転速度換算値ω_t が許容回転
速度ω_{t AL}より大きい場合には、ツール２ｔの合成回転
速度比ω_tRをω_{t AL}／ω_t によって求める〔ST40〕。そ
して、ＳＴ３７で求めたツール２ｔの合成移動速度比Ｖ
_tRとツール２ｔの合成回転速度比ω_tRとを比較する〔ST
41〕。合成移動速度比Ｖ_tRが合成回転速度比ω_tR以下で
あればＳＴ３６へ進み、合成移動速度比Ｖ_tRが合成回転
速度比ω_tRより大きければＳＴ３９へ進む。いずれにし
ても、ワーク取付点Ｈ_0wの移動速度成分指示値Ｖｘ_w ,
Ｖｙ_w ，Ｖｚ_w と、回転速度成分指示値ωｘ_w ，ω
ｙ_w ，ωｚ_w とが、下記の表の値とされる。

【表５】 このようにして、ワーク１ｗの移動速度成分指示値Ｖｘ
_w , Ｖｙ_w ，Ｖｚ_w と回転速度成分指示値ωｘ_w ，ωｙ
_w ，ωｚ_w とが補正された場合には、その補正値を用い
て再度ＳＴ２６から繰り返され、ツール２ｔの合成速度
換算値が許容速度以下となるように調整される。すなわ
ち、ワーク１ｗの速度が抑制され、ひいては、ツール２
ｔの速度も許容された範囲内におさまるように低減され
る。

【００６７】ところで、この速度減少操作は、図３３に
示すようにすることもできる。ツール２ｔの許容移動速
度Ｖ_{t AL}と許容回転速度ω_{t AL}とを読み取った〔ST32〕
後、ＳＴ３１で求めたツール２ｔの合成移動速度換算値
Ｖ_t と許容移動速度Ｖ_{t AL}とを比較する〔ST33Ａ〕。合
成移動速度換算値Ｖ_t が許容移動速度Ｖ_{t AL}より大きい
場合にはＳＴ３７Ａへ進められる。合成移動速度換算値
Ｖ_t が許容移動速度Ｖ_{t AL}以下の場合にはＳＴ３４Ａへ
進み、合成回転速度換算値ω_t と許容回転速度ω_{t AL}と
を比較する。合成回転速度換算値ω_t が許容回転速度ω
_{t AL}以下の場合はＳＴ４２へ進み、合成回転速度換算値
ω_t が許容回転速度ω_{t AL}より大きい場合には、ツール
２ｔの合成回転速度比ω_tRをω_{t AL}／ω_t により求める
〔ST35Ａ〕。そして、ツール２ｔの合成回転速度換算値
がω_{t AL}となるか近づくようにワーク１ｗの回転速度成
分指示値を減少させる。そのために、ＳＴ１６の直前で
得られている回転速度成分指示値ωｘ_w , ωｙ_w ，ωｚ
_w をω_tR倍する〔ST36Ａ〕。すなわち、

【数５１】 ＳＴ３３Ａで合成移動速度換算値Ｖ_t が許容移動速度Ｖ
_{t AL}よりも大きい場合には、ツール２ｔの合成移動速度
比Ｖ_tRをＶ_{t AL}／Ｖ_t により求め〔ST37Ａ〕、さらに、
ＳＴ３１で求めたツール２ｔの合成回転速度換算値ω_t
と許容移動速度ω_{t AL}とを比較する〔ST38Ａ〕。合成回
転速度換算値ω_t が許容回転速度ω_{t AL}以下の場合に
は、ツール２ｔの合成移動速度換算値がＶ_{t AL}となるか
近づくようにワーク１ｗの移動速度成分指示値を減少さ
せる。そのために、ＳＴ１６の直前で得られている移動
速度成分指示値Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w をＶ_tR倍する
〔ST39Ａ〕。すなわち、

【数５２】 一方、合成回転速度換算値ω_t が許容回転速度ω_{t AL}よ
り大きい場合には〔ST38Ａ〕、ツール２ｔの合成回転速
度比ω_tRをω_{t AL}／ω_t により求め〔ST40Ａ〕、ツール
２ｔの合成移動速度換算値がＶ_{t AL}となるように、ま
た、合成回転速度換算値がω_{t AL}となるように、ワーク
１ｗの移動速度成分指示値と回転速度成分指示値を減少
させる〔ST41Ａ〕。すなわち、

【数５３】 このような過程を採る場合には、ワーク取付点Ｈ_0wの速
度の選定は、以下のようになることが理解される。

【表６】

【００６８】また、上記の速度減少操作を、図３４に示
すようにすることもできる。ツール２ｔの許容移動速度
Ｖ_{t AL}と許容回転速度ω_{t AL}とを読み取った〔ST32〕
後、ＳＴ３１で求めたツール２ｔの合成移動速度換算値
Ｖ_t と許容移動速度Ｖ_{t AL}とを比較する〔ST33Ｂ〕。合
成移動速度換算値Ｖ_t が許容移動速度Ｖ_{t AL}以下の場合
にはＳＴ３４Ｂへ進む。合成移動速度換算値Ｖ_t が許容
移動速度Ｖ_{t AL}よりも大きい場合には、ツール２ｔの合
成移動速度比Ｖ_tRをＶ_{t AL}／Ｖ_t により求め〔ST35
Ｂ〕、ツール２ｔの合成移動速度換算値がＶ_{t AL}となる
か近づくようにワーク１ｗの移動速度成分指示値を減少
させる。そのために、ＳＴ１６の直前で得られている移
動速度成分指示値Ｖｘ_w , Ｖｙ_w ，Ｖｚ_w をＶ_tR倍する
〔ST36Ｂ〕。すなわち、

【数５４】 合成移動速度換算値Ｖ_t が許容移動速度Ｖ_{t AL}以下の場
合には〔ST33Ｂ〕、ＳＴ３１で求めたツール２ｔの合成
回転速度換算値ω_t と許容移動速度ω_{t AL}とを比較する
〔ST34Ｂ〕。合成回転速度換算値ω_t が許容回転速度ω
_{t AL}以下の場合にはＳＴ４２へ進む。一方、合成回転速
度換算値ω_t が許容回転速度ω_{t AL}よりも大きい場合に
は〔ST34Ｂ〕、ツール２ｔの合成回転速度比ω_tRをω
_{t AL}／ω_t により求め〔ST38Ｂ〕、ツール２ｔの合成回
転速度換算値がω_{t AL}となるか近づくようにワーク１ｗ
の回転速度成分指示値ωｘ_w , ωｙ_w ，ωｚ_w を減少さ
せる。そのために、ＳＴ１６の直前で得られている回転
速度成分指示値ωｘ_w , ωｙ_w ，ωｚ_w をω_tR倍する
〔ST39Ｂ〕。すなわち、

【数５５】 ＳＴ３７で合成回転速度換算値ω_t が許容回転速度ω
_{t AL}よりも大きい場合には、ＳＴ３８へ進みＳＴ３８で
置き換えを行って、ＳＴ２６へ戻る。このような過程を
採る場合には、ワーク取付点Ｈ_0wの速度の選定は、以下
のようになることが理解される。

【表７】

【００６９】最初に「動作指示信号」を得てから、設定
された微小時間ΔＴ例えば前述した１／２０秒が経過す
るまで待機される。設定微小時間ΔＴが経過すれば〔図
２８のST42〕、ｉ＝０にして、経過時間Δｔ_-iを０に設
定する〔ST43〕。なお、１／２０秒は、ＳＴ０２からＳ
Ｔ４２に至るまでの演算を完了するに十分な時間が選定
されている。それのみならず、これから述べる制御動作
を完了させるに十分な長さともなっている（後述する図
３６のΔＴを参照）。最初に押された操作ボタン１２
ｂ，１２ｃ，１２Ｅが、設定微小時間ΔＴの経過した時
点まで押され続けていたか、すなわち「動作指示信号」
が同じかどうかを確認する〔ST44〕。「最初の動作指示
信号」が「新しい動作指示信号」に変化してなければ、
経過時間Δｔ_-iに１／２０秒という微小時間ΔＴを加算
し〔ST45〕、経過時間がΔｔ_-i+1に置き換えられる〔ST
46〕。そして、次の制御動作の開始が指示される。も
し、操作ボタン１２ｂ，１２ｃ，１２Ｅの全てが手放さ
れているならば〔ST47〕、すなわち、「動作指示信号」
がなくなっているならば、終了指令部４ｆ（図１参照）
に制御動作の終了を指示する〔ST48〕。異なった操作ボ
タンが押されているならば、すなわち操作ボタン１２
ｂ，１２ｃ，１２Ｅに代えて操作ボタン１２ａ，１２Ｅ
が押されていると〔ST47〕、その「新しい動作指示信
号」を「最初の動作指示信号」に置き換えて、ＳＴ０２
から繰り返される。このように「動作指示信号」に変化
があった場合や消失した場合については、後で説明す
る。

【００７０】式(29)で得た「ツール取扱装置２の設置基
準点Ｂ_0tに対するツール取付点Ｈ_0tの時間ΔＴ経過後の
位置姿勢」を表す同次変換行列Ｔ_2-i を逆変換して、
「ツール取扱装置２の時間ΔＴ経過後の関節変数」θ
_1-i ，θ_2-i ，θ_3-i ，θ_4-i ，θ_5-i ，θ_6-i が求め
られる〔ST49〕。一方、式(25)で得た「ワーク取扱装置
１の設置基準点Ｂ_0wに対するワーク取付点Ｈ_0wの時間Δ
Ｔ経過後の位置姿勢」を表す同次変換行列Ｗ_2-i を逆変
換して、「ワーク取扱装置１の時間ΔＴ経過後の関節変
数」φ_1-i ，φ_2-i ，φ_3-i ，φ_4-i ，φ_5-i ，φ_6-i
が求められる〔ST50〕。なお、求められた「ツール取扱
装置２の時間ΔＴ経過後の関節変数」θ_1-i ，θ_2-i ，
θ_3-i ，θ_4-i ，θ_5-i ，θ_6-i は、直ちにツール取扱
装置２の関節変数記憶部１９ｂt （図１参照）に記憶さ
れ、「ワーク取扱装置１の時間ΔＴ経過後の関節変数」
φ_1-i ，φ_2-i ，φ_3-i ，φ_4-i ，φ_5-i ，φ_6-i も、
直ちに、ワーク取扱装置１の関節変数記憶部１９ｂw に
記憶される。

【００７１】ツール取扱装置２における「アクチュエー
タ動作指令値」およびワーク取扱装置１における「アク
チュエータ動作指令値」の単位当たりの「関節変数」の
動作量を、Ｒ_t1 ,Ｒ_t2 ,Ｒ_t3 ,Ｒ_t4 ,Ｒ_t5 ,Ｒ_t6 ,Ｒ_w1
,Ｒ_w2 ,Ｒ_w3 ,Ｒ_w4 ,Ｒ_w5 ,Ｒ_w6とする。いずれもロボ
ットごとに決まった値であり、制御装置４に予め記憶さ
れている。ＳＴ４９で求めた「ツール取扱装置２の時間
ΔＴ経過後の関節変数」θ_1-i ，θ_2-i ，θ_3-i ，θ
_4-i ，θ_5-i ，θ_6-i から、ツール取扱装置２の「アク
チュエータＭ_t の動作指令値」ａ_t1-i ,ａ_t2-i ,ａ_t3-i
,ａ_t4-i ,ａ_t5-i ,ａ_t6-iが、下記の式(38)によって演
算される〔ST51〕。また、ＳＴ５０で求めた「ワーク取
扱装置１の時間ΔＴ経過後の関節変数」φ_1-i ，
φ_2-i ，φ_3-i ，φ_4-i ，φ_5-i ，φ_6-i から、ワーク
取扱装置１の「アクチュエータＭ_w の動作指令値」ａ
_w1-i ,ａ_w2-i ,ａ_w3-i ,ａ_w4-i ,ａ_w5-i ,ａ_w6-iが、下
記の式(39)によって演算される〔ST52〕。

【数５６】 こうして得られた「ツール取扱装置２のアクチュエータ
動作指令値」ａ_t1-i ,ａ_t2-i ,ａ_t3-i ,ａ_t4-i ,ａ_t5-i
,ａ_t6-iと、「ワーク取扱装置１のアクチュエータ動作
指令値」ａ_w1-i ,ａ_w2-i ,ａ_w3-i ,ａ_w4-i ,ａ_w5-i ,ａ
_w6-iとは、経過時間ΔＴ×ｉとなった直後に〔ST53〕、
ワーク取扱装置１とツール取扱装置２とを独立して作動
させるために個別に、ほとんど同時にワーク取扱装置１
のアクチュエータ動作指令部４w （図１参照）とツール
取扱装置２のアクチュエータ動作指令部４t へ出力され
る〔ST54, ST55〕。その後、ＳＴ４４へ戻される。ＳＴ
５４，ＳＴ５５の後に、ワーク取扱装置１のアクチュエ
ータＭ_1w ,Ｍ_2w ,Ｍ_3w ,Ｍ_4w ,Ｍ_5w ,Ｍ_6w（図１７参
照）が作動し〔ST56〕、ワーク取扱装置１により「ワー
ク１ｗの位置姿勢」が変化される〔ST58〕。また、ツー
ル取扱装置２のアクチュエータＭ_1t ,Ｍ_2t ,Ｍ_3t ,Ｍ_4t
,Ｍ_5t ,Ｍ_6tも同時に作動し〔ST57〕、ツール取扱装置
２により「ツール２ｔの位置姿勢」が変化される〔ST5
9〕。

【００７２】以上の制御動作は、ワーク取扱装置１とツ
ール取扱装置２の「連動する手動操作」による図３５の
（ａ）から図３５の（ｂ）までの動きである。図３５の
（ｂ）においては、（ａ）の「着目点」Ｂ₁ における
「大地６４に対するツール先端点５７の姿勢」と「ワー
ク基準点５６に対するツール先端点５７の相対的な位
置」とを維持しながら、ワーク１ｗ上の「次の着目点」
Ｂ₂ の接線Ｂ_2Sが「大地６４に対して所望する傾き」δ
_w となるように、「ワーク１ｗの位置姿勢」が変更され
ている。なお、ワーク１ｗ上の「次の着目点」Ｂ₂ の接
線Ｂ_2Sが「大地６４に対して所望する傾き」δ_w となっ
ているかどうかは、作業員が操作ボタンの全てを手放し
た後に、「着目点」Ｂ₂ に角度測定具を当てるなどして
測定することによって確認される。接線Ｂ_2Sが「大地６
４に対して所望する傾き」δ_w となっていなければ、再
度「連動する手動操作」が実行され、その後に、再び角
度測定具によって接線Ｂ_2Sの傾きが計測される。その後
は、図３５の（ｃ）に示すように、ツール取扱装置２の
「単独の手動操作」により「第一の教示点」Ａ₁ におけ
る「大地６４に対するツール先端点５７の姿勢」を維持
して、「第一の教示点」Ａ₁ に位置するツール２ｔをワ
ーク１ｗ上の「次の着目点」Ｂ₂ へ移動させる。これに
よって得られるワーク１ｗとツール２ｔの位置決め状態
を「第二の教示点」Ａ₂ とするならば、ティーチングペ
ンダント３の教示ボタン１３を押して、その「第二の教
示点」Ａ₂ が教示される。このツール取扱装置２の「単
独の手動操作」は、公知であるので説明を省く。引き続
いて、「連動する手動操作」もしくは「単独の手動操
作」を実行することができる。すなわち、「第三の教示
点」Ａ₃ は、「第一の教示点」Ａ₁ を出発してから「第
二の教示点」Ａ₂ を教示するまでの手順を繰り返すこと
によって教示される。このときも、その「第三の教示
点」Ａ₃ において「大地６４に対するツール２ｔの姿
勢」が維持される。ちなみに、図３５の（ａ）のよう
に、「連動する手動操作」を開始する前のワーク１ｗと
ツール２ｔの位置決め状態を得る場合、上記した説明で
は、ワーク取扱装置１を「単独の手動操作」によって、
ツール取扱装置２も「単独の手動操作」によって行って
いる。しかし、以上の説明からも推測できるように、
「連動する手動操作」を開始する前のワーク１ｗとツー
ル２ｔの位置決め状態を得るために、「連動する手動操
作」を使用することもできる。

【００７３】上記のことから、「連動する手動操作」
は、ワーク取扱装置１による「ワーク１ｗの位置姿勢」
の変化と、ツール取扱装置２による「大地に対するツー
ル先端点５７の姿勢」および「ワーク基準点５６に対す
るツール先端点５７の相対的な位置」の維持とが、それ
ぞれ予め設定された微小時間ΔＴの経過後に、独立して
ほとんど同時に実行される。したがって、「ワーク１ｗ
の位置姿勢」の変化と「ツール２ｔの位置姿勢」の変化
とは迅速となる。「ツール２ｔの位置姿勢」の変化が
「ワーク１ｗの位置姿勢」の変化に追従するのではない
ので、「ワーク１ｗの位置姿勢」の変化において発生す
る演算上の誤差が「ツール２ｔの位置姿勢」の変化に影
響することはない。それゆえに、「ツール２ｔの位置」
の変化も精度よく行われる。このように、二基のロボッ
トを「連動する手動操作」によって教示すれば、教示操
作の途中で、ツール２ｔをワーク１ｗからその都度退避
させる必要がなくなり、「次の教示点」を教示するまで
の間に、前の「教示点」もしくは前の「着目点」を見失
うことがなく、手動教示作業の簡素化および迅速化が実
現される。また、「連動する手動操作」において「大地
６４に対するツール先端点５７の姿勢」と「ワーク基準
点５６に対するツール先端点５７の相対的な位置」が維
持されることになり、トーチを用いて溶接する場合な
ど、重力の影響を受ける溶融池の所望外の流動や無用な
停滞が回避され、溶接品質の向上が図られる。もちろ
ん、本発明の目的であるワーク１ｗとツール２ｔの許容
範囲での速度制御が達成され、主動作機械であるワーク
取扱装置１を動作させても、従動作機械であるツール取
扱装置２のツール２ｔの動きを許容速度以内で動作させ
ることができる。したがって、教示操作するオペレータ
がワーク１ｗの動きのみに着目していても、ワーク１ｗ
に連動してツール２ｔが予測外の速度で動作することが
なくなり、たとえ、ツール２ｔの動作量が大きくなる場
合でも、ゆっくりした動きによって教示操作の安全が図
られる。

【００７４】ところで、ＳＴ５３からＳＴ４４に戻った
とき、最初の押された操作ボタン１２ｂ，１２ｃ，１２
Ｅが継続していれば、ＳＴ４５からＳＴ５９までが、同
じ「動作指示信号」にもとづいて制御動作される。これ
がｉ×ΔＴの時間持続されていると、同じ「動作指示信
号」にもとづく制御動作がｉ回繰り返される。なお、Ｓ
Ｔ４９で求められた「ツール取扱装置の関節変数」θ
_1-i ，θ_2-i ，θ_3-i ，θ_4-i ，θ_5-i ，θ_6-i は、ツ
ール取扱装置２の関節変数記憶部１９ｂt に記憶されて
いる「従前のツール取扱装置の関節変数」θ_1-(i-1) ，
θ_2-(i-1) ，θ_3-(i-1) ，θ_4-(i-1) ，θ_5-(i-1) ，θ
_6-(i-1) と置き換えられる。同様に、ＳＴ５１で求めら
れた「ワーク取扱装置の関節変数」φ_1-i ，φ_2-i ，φ
_3-i ，φ_4-i ，φ_5-i ，φ_6-i も、ワーク取扱装置１の
関節変数記憶部１９ｂw に記憶されている「従前のツー
ル取扱装置の関節変数」φ_1-(i-1) ，φ_2-(i-1) ，φ
_3-(i-1) ，φ_4-(i-1) ，φ_5-(i-1) ，φ_6-(i-1) と置き
換えられる。ＳＴ４４に戻った（ｉ＋１）回目に「新し
い動作指示信号」が現れていれば、すなわち、操作ボタ
ン１２ｂ，１２ｃ，１２Ｅに代えて操作ボタン１２ａ，
１２Ｅが押されていると〔ST47〕、ＳＴ０２へ戻され
る。そして、その「新しい動作指示信号」にもとづき、
ワーク基準点５６の絶対座標系５１の各軸方向の移動速
度および各軸回りの回転速度が計算される〔ST03〕。Ｓ
Ｔ４４に戻った（ｉ＋ｎ）回目に「動作指示信号」がな
くなっていれば、すなわち、操作ボタン１２ａ，１２Ｅ
の全てが手放されているならば〔ST47〕、終了指令部４
ｆ（図１参照）に制御動作の終了を指示する〔ST48〕。
この時点で、一連の「連動する手動操作」はひとまず終
了する。

【００７５】以上は、連動手動操作教示制御手段４Ａに
おけるアルゴリズムを含めて、制御作動を説明した。上
記したプロセスは、図３６のタイミングチャートにおけ
るΔＴごとにΔＴ×ｉまでの間繰り返され、ΔＴごとに
ＳＴ５９までの動作を繰り返すことが理解される。ＳＴ
２４において「動作指示信号」の全てが０となれば、も
はやワーク１ｗを移動させる必要のない状態であり、
「連動する手動操作」の制御作動はタイムチャートの終
了点ＥＥで終わる。この後は、さらに「連動する手動操
作」が繰り返されるか、ＳＴ２から従来技術のところで
少し触れた「単独の手動操作」が実行される。制御動作
がＳＴ５３からＳＴ４４に戻ったとき、操作ボタン１２
ｂ，１２ｃ，１２Ｅに代えて、操作ボタン１２ａ，１２
Ｅに押し変えられているといった場合がある。これは、
図３６のタイミングチャート中の上段に太い凹凸線で示
したように、「動作指示信号」に変化があった場合であ
る。このように「動作指示信号」が変化しても、全ての
信号が０でないかぎり、ＳＴ４７からＳＴ０２へ戻さ
れ、新しい「動作指示信号」にもとづいてＳＴ５９まで
実行される。

【００７６】なお、上記の「連動する手動操作」の制御
動作において、「ワーク１ｗの位置姿勢」を変化させる
「動作指示信号」は、ワーク基準点５６の「直角座標系
の各軸方向の移動および各軸回りの回転を指示する信
号」を採用してもよいし、ワーク取扱装置１の各関節に
関する「回転を指示する信号」とすることもできる。以
上は主動作機械がワーク取扱装置１として、従動作機械
がツール取扱装置２として、動作指示を受けた場合の速
度抑制方法について説明した。もちろん、６自由度のマ
ニプレータと３自由度以下のポジショナとによる連動に
も、本発明を適用することができる。その場合には、ポ
ジショナが主動作機械でありマニプレータが従動作機械
とされた場合に限られることになる。追従性を発揮する
ために高い自由度が必要となることから当然である。ま
た、ワークとツールの連動関係が次に述べる「ツール２
ｔのワーク１ｗに対する位置姿勢を一定に保つ」という
ような場合には、ツール取扱装置を主動作機械とし、ワ
ーク取扱装置を従動作機械とすることができる。

【００７７】次に、モード切替スイッチ８（図６参照）
によって、「関節手動操作モード」が選定され、かつ、
動作切替スイッチ９により「連動する手動操作」が選択
されている場合の「連動する関節手動操作モード」につ
いて述べる。前述の例は、「ツール２ｔの姿勢」につい
ては絶対座標系５１に対して一定に維持されるように、
「ツール２ｔの位置」の変化については「ワーク１ｗの
位置姿勢」の変化に連動して「ワーク１ｗに対するツー
ル２ｔの相対的な位置」が維持されるように、ワーク取
扱装置１およびツール取扱装置２のそれぞれの各関節を
動作させ、ワーク基準点５６を所望する位置姿勢に変更
するときの動作であった。しかし、ここでは、連動関係
の異なる例を採用して説明する。すなわち、「ワーク１
ｗの位置姿勢」の変化に連動して「ワーク１ｗに対する
ツール２ｔの相対的な位置姿勢」が維持されるように、
ワーク取扱装置１およびツール移動装置２の関節を動作
させ、ワーク基準点５６を所望する位置姿勢に変更する
ときの動作とする。これによれば、「ツール２ｔの姿
勢」も逐一変更されるが、ワーク１ｗに対しては常に同
じ姿勢をとるので、ワーク１ｗに突起箇所があるなどの
場合には、ツール２ｔの姿勢を大地に一定とした場合に
生じるであろうツール２ｔの突起箇所との衝突を回避さ
せることができる。

【００７８】図４３の（ａ）においてワーク１ｗとツー
ル２ｔとの最初の位置決め状態を得ておくことは、前述
した「連動する直角手動操作モード」を実行する前の状
態、すなわち、図３５の（ａ）の場合と同じである。テ
ィーチングペンダント３（図６参照）において、速度率
切替スイッチ１０で所望する「速度率」を、例えば１．
０である「ｒ_c 」に選定する。図３に示すブロック構成
をなす制御装置であり、これは、図３７ないし図４２の
フローチャートを参照して、所望する操作ボタン例えば
１２ｂ，１２Ｄが押されていると〔ST101 〕、それらが
手放されるまでの処理は、次のようになる。まず、操作
ボタン１２ｂ，１２Ｄとを押すことによって、「動作指
示信号」が取り込まれる〔ST102 〕。この「関節手動操
作モード」においては、ワーク取扱装置１が６自由度の
マニプレータか３自由度以下のポジショナでもよい。ポ
ジショナが使用されていると、操作ボタン群１２によっ
て生成される「動作指示信号」はＳ１ないしＳ３だけと
なる。ワーク取扱装置１の各関節１ａ，１ｂ，１ｃ，１
ｄ，１ｅ，１ｆの関節回転速度指示値が、次のようにし
て求められる。操作ボタン１２ｂ，１２Ｄにより生成さ
れた「動作指示信号」Ｓ１ _,Ｓ２ _,Ｓ３ _,Ｓ４ _,Ｓ５ _,
Ｓ６と、選定された「速度率」ｒ_c と、ワーク取扱装置
１の関節手動操作のための速度テーブルに記憶された最
高値Ｖ１_w0 ,Ｖ２_w0 ,Ｖ３_w0 ,Ｖ４_w0 ,Ｖ５_w0 ,Ｖ６_w0
とを用いて、速度指示値Ｖ１_w , Ｖ２_w ，Ｖ３_w ，Ｖ４
_w ，Ｖ５_w ，Ｖ６_w が、式(1A)から演算される〔ST103
〕。

【数５７】

【００７９】「動作指示信号」を受ける直前の「ワーク
取扱装置１の関節変数」および「ツール取扱装置２の関
節変数」から、「絶対座標系５１におけるワーク基準点
５６の現在の位置姿勢」を演算する。「動作指示信号」
を受ける前にすでにワーク取扱装置１の各可動部分に取
り付けられたセンサＳ_1w ,Ｓ_2w ・・・Ｓ_6w（図１７参
照）によって検出されているか関節変数記憶部１９ｂw
に記憶されている「ワーク取扱装置１の現在の関節変
数」φ_1-0 , φ_2-0 , φ_3-0 , φ_4-0 , φ_5-0 , φ_6-0
を取り込む〔ST104 〕。図５に示す「ワーク取扱装置１
の設置基準点Ｂ_0wに対するワーク取付点Ｈ_0wの現在の位
置姿勢」を表す同次変換行列Ｗ_2-0 は、Ｄｅｎａｖｉｔ
−Ｈａｒｔｅｎｂｅｒｇの表現によるリンクパラメータ
を使った式(9A)の同次変換行列の積として得られる式(1
0A) のように表すことができる〔ST105 〕。

【数５８】 この式(9A) , (10A)は、前述した式(9) , (10)と同じで
あり、φ_j , ａ_jw ,ｄ_jw,α_jwも、式(9) の後で説明し
たものと同じである。

【００８０】次に、時間ΔＴ経過後の「ワーク取扱装置
１の関節変数」φ_1-i ，φ_2-i ，φ_3-i ，φ_4-i ，φ
_5-i ，φ_6-i を、次式(11A) によって求める〔ST106
〕。

【数５９】 すなわち、「ワーク取扱装置１の時間ΔＴ経過後の関節
変数」φ_1-i は、「ワーク取扱装置１の現在の関節変
数」φ_1-0 に、式(1A)で得られている速度指示値Ｖ１_w
に経過時間ΔＴを乗じたものを加えることにより得られ
る。φ_2-i ないしφ_6-i も同様である。

【００８１】「ワーク取扱装置１の設置基準点Ｂ_0wに対
するワーク取付点Ｈ_0wの時間ΔＴ経過後の位置姿勢」を
表す同次変換行列Ｗ_2-i は、Ｄｅｎａｖｉｔ−Ｈａｒｔ
ｅｎｂｅｒｇの表現によるリンクパラメータを使った式
(12A) の同次変換行列の積として得られる式(13A) のよ
うに表すことができる〔ST107 〕。

【数６０】 上記の式(12A) , (13A) 中のφ_j , ａ_jw ,α_jwも、式
(9) の後で説明したものと同じである。そこで、前述し
た式(34)と式(37)と同様にして、「ワーク取扱装置１の
設置基準点Ｂ_0wを基準としたワーク取付点Ｈ_0wの時間Δ
Ｔ経過後の位置姿勢」を表す同次変換行列Ｗ_2-i と「ワ
ーク取扱装置１の設置基準点Ｂ_0wに対するワーク取付点
Ｈ_0wの現在の位置姿勢」を表す同次変換行列Ｗ_2-0 とか
ら、ワーク１ｗの平均的な合成移動速度換算値Ｖ_w と合
成回転速度換算値ω_w を演算する〔ST108 〕。

【００８２】既に記憶されているワーク１ｗの許容移動
速度Ｖ_{w AL}と許容回転速度ω_{w AL}とを読み取る〔ST109
〕。そこで、ＳＴ１０８で求めたワーク１ｗの合成移
動速度換算値Ｖ_w と許容移動速度Ｖ_{w AL}とを比較する
〔ST110 〕。合成移動速度換算値Ｖ_w が許容移動速度Ｖ
_{w AL}以下の場合にはＳＴ１１１へ進む。そして、ＳＴ１
０８で求めたワーク１ｗの合成回転速度換算値ω_w とワ
ーク１ｗの許容回転速度ω_{w AL}とを比較する。合成回転
速度換算値ω_w が許容回転速度ω_{w AL}以下の場合にはＳ
Ｔ１１９へ進む。合成回転速度換算値ω_w が許容回転速
度ω_{w AL}よりも大きい場合〔ST111 〕、ワーク１ｗの合
成回転速度比ω_wRをω_{w AL}／ω_w により求める〔ST112
〕。ワーク１ｗの合成回転速度換算値がω_{w AL}となる
ようにワーク１ｗの関節回転速度指示値を減少させる。
そのために、ＳＴ１０３で得られている各関節の関節回
転速度指示値Ｖ１_w ,Ｖ２_w ，Ｖ３_w ，Ｖ４_w ，Ｖ
５_w ，Ｖ６_w をそれぞれω_wR倍する〔ST113 〕。すなわ
ち、

【数６１】 一方、ＳＴ１１０で、合成移動速度換算値Ｖ_w が許容移
動速度Ｖ_{w AL}よりも大きい場合には、ワーク１ｗの合成
移動速度比Ｖ_wRをＶ_{w AL}／Ｖ_w により求める〔ST114
〕。そして、ＳＴ１０８で求めたワーク１ｗの合成回
転速度換算値ω_w と許容回転速度ω_{w AL}とを比較する
〔ST115 〕。合成回転速度換算値ω_w が許容回転速度ω
_{w AL}以下の場合には、ワーク１ｗの合成移動速度換算値
がＶ_{w AL}となるか近づくようにワーク１ｗの関節回転速
度指示値を減少させる。そのために、ＳＴ１０３で得ら
れている関節回転速度指示値Ｖ１_w , Ｖ２_w ，Ｖ３_w ，
Ｖ４_w ，Ｖ５_w ，Ｖ６_w をそれぞれＶ_wR倍する〔ST116
〕。すなわち

【数６２】 一方、ＳＴ１１５で、合成回転速度換算値ω_w が許容回
転速度ω_{w AL}より大きい場合には、ワーク１ｗの合成回
転速度比ω_wRをω_{w AL}／ω_w により求める〔ST117 〕。
そして、ＳＴ１１４で求めたワーク１ｗの合成移動速度
比Ｖ_wRとワーク１ｗの合成回転速度比ω_wRとを比較する
〔ST118 〕。合成移動速度比Ｖ_wRが合成回転速度比ω_wR
以下であればＳＴ１１６へ進み、合成移動速度比Ｖ_wRが
合成回転速度比ω_wRより大きければＳＴ１１３へ進む。
いずれにしても、関節回転速度指示値Ｖ１_w , Ｖ２_w ，
Ｖ３_w ，Ｖ４_w ，Ｖ５_w ，Ｖ６_w が下記の表の値とされ
る。

【表８】 このようにして、ワーク１ｗの関節回転速度指示値Ｖ１
_w , Ｖ２_w ，Ｖ３_w ，Ｖ４_w ，Ｖ５_w ，Ｖ６_w が補正さ
れた場合には、その補正値を用いてワーク１ｗの合成速
度換算値が許容速度以下に調整される。したがって、ワ
ーク１ｗの速度が許容された範囲内におさまるように低
減され、ワーク１ｗの過大な動きの生じるのが防止され
る。なお、以上の手順に代えて、前述した図３１や図３
２と同じ手順を採用することができるが、容易に類推で
きるので説明は省く。

【００８３】主動作機械であるワーク取扱装置１が安全
な速度で動作するようにした後は、「絶対座標系５１に
おけるワーク基準点５６の現在の位置姿勢」を演算す
る。すなわち、「絶対座標系５１におけるワーク基準点
５６の現在の位置姿勢」を表す同次変換行列 worldＸ
_w-0 が、式(10A) を用いて、次式(14A) のように表され
る〔ST119 〕。

【数６３】 この式(14A) は、前述した式(11)と同じである。また、
Ｚ_w やＥ_w も、式(11)の後のところで説明したものと同
じである。

【００８４】次に、「動作指示信号」を受ける直前、す
なわち、従前の「ワーク取扱装置１の関節変数」および
「ツール移動装置２の関節変数」から、「ワーク基準点
５６に対するツール先端点５７の相対的な位置姿勢」を
求める。「動作指示信号」を受ける前にすでに従動作機
械であるツール移動装置２の各可動部分に取り付けられ
ているセンサＳ_1t ,Ｓ_2t ,・・・ ,Ｓ_6tから「ツール移
動装置２の現在の関節変数」θ_1-0 , θ_2-0 , θ_3-0 ,
θ_4-0 , θ_5-0 , θ_6-0を取り込む〔ST120 〕。図５に
示す「ツール取扱装置２の設置基準点Ｂ_0tに対するツー
ル取付点Ｈ_0tの現在の位置姿勢」を表す同次変換行列Ｔ
_2-0 を演算する〔ST121 〕。これは、Ｄｅｎａｖｉｔ−
Ｈａｒｔｅｎｂｅｒｇの表現によるリンクパラメータを
使った式(15A) の同次変換行列の積として得られる式(1
6A) のように表すことができる。

【数６４】 この式(15A) , (16A) は前述した式(17) , (18) と同じ
であり、φ_j , ａ_jt ,ｄ_jt ,α_jtは、式(17) , (18) の
後で説明したものと同じである。

【００８５】「絶対座標系５１におけるツール先端点５
７の現在の位置姿勢」を表す同次変換行列 worldＸ_t-0
は、式(16A) を代入して、式(17A) によって算出される
〔ST122 〕。

【数６５】 この式(17A) も前述した式(19)と同じである。また、Ｚ
_t やＥ_t も式(19)の後のところで説明したものと同じで
ある。ここで、「動作指示信号を受ける直前でのワーク
基準点５６に対するツール先端点５７の相対的な位置姿
勢」を求める。「ワーク基準点５６に対するツール先端
点５７の相対的な現在の位置姿勢」を表す同次変換行列
を wＸ_t とすると、「絶対座標系５１におけるツール先
端点５７の位置姿勢」を表す同次変換行列world Ｘ_t-o
は、式(14A) を用いて、次式(18A) のように表される。

【数６６】 従って、上記の同次変換行列 wＸ_t は次式(19A) で算出
される〔ST123 〕。

【数６７】 なお、上式(18A), (19A)は、それぞれ前述した式(20),
(21) と同じである。ワーク取付点Ｈ_0wが式(10A) に従
って動作しても、「ワーク１ｗに対するツール２ｔの相
対的な位置姿勢」を変化させずに、「絶対座標系５１に
おけるツール先端点５７の位置姿勢」を、「ワーク取付
点Ｈ_0wの位置姿勢」の変化に連動させる必要がある。し
たがって、そのときの「ワーク取付点Ｈ_0wに対するツー
ル先端点５７の相対的な位置姿勢」を変えないようにす
るための「ツール移動装置２の設置基準点Ｂ_0tに対する
ツール取付点Ｈ_0tの時間ΔＴ経過後の位置姿勢」を表す
同次変換行列Ｔ_2-i が求められる。前記した式(20)と同
様にして、式(20A) が成立する。

【数６８】 これは、式(28)と同様に、式(21A) のように表現され
る。

【数６９】 この式(21A) を変形することによって、「ツール移動装
置２の設置基準点Ｂ_0tに対するツール取付点Ｈ_0tの時間
ΔＴ経過後の位置姿勢」を表す同次変換行列Ｔ_2-i は、
以下のように関連づけることができる〔ST124 〕。

【数７０】 そこで、前述した式(34)と式(37)と同様にして、「ツー
ル取扱装置２の設置基準点Ｂ_0tに対するツール取付点Ｈ
_0tの時間ΔＴ経過後の位置姿勢」を表す同次変換行列Ｔ
_2-i と「ツール取扱装置２の設置基準点Ｂ_0tに対するツ
ール取付点Ｈ_0tの現在の位置姿勢」を表す同次変換行列
Ｔ_2-0 とから、ツール２ｔの平均的な合成移動速度換算
値Ｖ_t と合成回転速度換算値ω_t とを演算する〔ST125
〕。

【００８６】既に記憶されているツール２ｔの許容移動
速度Ｖ_{t AL}と許容回転速度ω_{t AL}とを読み取る〔ST126
〕。そこで、ＳＴ１２５で求めたツール２ｔの合成移
動速度換算値Ｖ_t と許容移動速度Ｖ_{t AL}とを比較する
〔ST127 〕。合成移動速度換算値Ｖ_t が許容移動速度Ｖ
_{t AL}以下の場合にはＳＴ１２８へ進む。そして、ＳＴ１
２５で求めたツール２ｔの合成回転速度換算値ω_t とツ
ール２ｔの許容回転速度ω_{t AL}とを比較する。合成回転
速度換算値ω_t が許容回転速度ω_{t AL}以下の場合にはＳ
Ｔ１３６へ進む。一方、合成回転速度換算値ω_t が許容
回転速度ω_{t AL}よりも大きい場合には〔ST128 〕、ツー
ル２ｔの合成回転速度比ω_tRを、ω_{t AL}／ω_t により求
める〔ST129 〕。ツール２ｔの合成回転速度換算値がω
_{t AL}となるか近づくようにワーク１ｗの関節回転速度指
示値を減少させる。そのために、ＳＴ１０３で得られて
いる関節回転速度指示値Ｖ１_w , Ｖ２_w ，Ｖ３_w ，Ｖ４
_w ，Ｖ５_w ，Ｖ６_w をそれぞれω_tR倍する〔ST130 〕。
すなわち

【数７１】 一方、ＳＴ１２７で、合成移動速度換算値Ｖ_t が許容移
動速度Ｖ_{t AL}よりも大きい場合には、ツール２ｔの合成
移動速度比Ｖ_tRをＶ_{t AL}／Ｖ_t により求める〔ST131
〕。そして、ＳＴ１２５で求めたツール２ｔの合成回
転速度換算値ω_t と許容回転速度ω_{t AL}とを比較する
〔ST132 〕。合成回転速度換算値ω_t が許容回転速度ω
_{t AL}以下の場合には、ツール２ｔの合成移動速度換算値
がＶ_{t AL}となるか近づくようにワーク１ｗの関節回転速
度指示値を減少させる。そのために、ＳＴ１０３で得ら
れている関節回転速度指示値Ｖ１_w , Ｖ２_w ，Ｖ３_w ，
Ｖ４_w ，Ｖ５_w ，Ｖ６_w をそれぞれＶ_tR倍する〔ST133
〕。すなわち

【数７２】 一方、ＳＴ１３２で、合成回転速度換算値ω_t が許容回
転速度ω_{t AL}より大きい場合には、ツール２ｔの合成回
転速度比ω_tRをω_{t AL}／ω_t により求める〔ST134 〕。
そして、ＳＴ１３１で求めたツール２ｔの合成移動速度
比Ｖ_tRとツール２ｔの合成回転速度比ω_tRとを比較する
〔ST135 〕。合成移動速度比Ｖ_tRが合成回転速度比ω_tR
以下であればＳＴ１３３へ進み、合成移動速度比Ｖ_tRが
合成回転速度比ω_tRより大きければＳＴ１３０へ進む。
いずれにしても、関節回転速度指示値Ｖ１_w , Ｖ２_w ，
Ｖ３_w ，Ｖ４_w ，Ｖ５_w ，Ｖ６_w が下記の表の値とされ
る。

【表９】 このようにしてワーク１ｗの関節回転速度指示値Ｖ
１_w , Ｖ２_w ，Ｖ３_w ，Ｖ４_w ，Ｖ５_w ，Ｖ６_w が補正
された場合には、その補正値を用いて再度ＳＴ１０５か
ら繰り返され、ツール２ｔの合成速度換算値が許容速度
以下に調整される。したがって、ワーク１ｗの速度が抑
制され、ひいては、ツール２ｔの速度も許容された範囲
内におさまるように低減される。なお、以上の手順に代
えて、前述した図３３や図３４と同じ手順を採用するこ
とができるが、容易に類推できるので説明は省く。

【００８７】最初に「動作指示信号」を得てから、設定
された微小時間ΔＴ例えば前述した１／２０秒が経過す
るまで待機される。設定微小時間ΔＴが経過すれば〔ST
136〕ｉ＝０にして、経過時間Δｔ_-iを０に設定する〔S
T137 〕。なお、１／２０秒は、ＳＴ１０２からＳＴ１
３６に至るまでの演算を完了するに十分な時間である。
それのみならず、これから述べる制御動作を完了させる
に十分な長さともなっている。最初に押された操作ボタ
ン１２ｂ，１２Ｄが、設定微小時間ΔＴの経過した時点
まで押され続けていたか、すなわち、「動作指示信号」
が同じかどうかを確認する〔ST138 〕。「最初の動作指
示信号」が「新しい動作指示信号」に変化してなけれ
ば、経過時間Δｔ_-iに１／２０秒という微小時間ΔＴを
加算し〔ST139 〕、経過時間がΔｔ_-i+1に置き換えられ
る〔ST140 〕。そして、次の制御動作の開始が指示され
る。もし、操作ボタン１２ｂ，１２Ｄの全てが手放され
ているならば〔ST141 〕、すなわち、「動作指示信号」
がなくなっているならば、終了指令部４ｆ（図３参照）
に制御動作の終了を指示する〔ST142 〕。異なった操作
ボタンが押されているならば、すなわち、操作ボタン１
２ｂ，１２Ｄに代えて操作ボタン１２ｃ，１２Ｅが押さ
れていると〔ST141 〕、その「新しい動作指示信号」を
「最初の動作指示信号」に置き換えて、ＳＴ０２から繰
り返される。このように「動作指示信号」に変化があっ
た場合や消失した場合については、前述と同じであり説
明を省く。

【００８８】式(22A) で得た「ツール取扱装置２の設置
基準点Ｂ_0tに対するツール取付点Ｈ_0tの時間ΔＴ経過後
の位置姿勢」を表す同次変換行列Ｔ_2-i を逆変換して、
「ツール取扱装置２の時間ΔＴ経過後の関節変数」θ
_1-i ，θ_2-i ，θ_3-i ，θ_4-i，θ_5-i ，θ_6-i が求め
られる〔ST143 〕。一方、「ワーク取扱装置１の時間Δ
Ｔ経過後の関節変数」φ_1-i ，φ_2-i ，φ_3-i ，
φ_4-i ，φ_5-i ，φ_6-i は既にＳＴ１０６で求められて
いる。なお、求められた「ツール取扱装置２の時間ΔＴ
経過後の関節変数」θ_1-i ，θ_2-i ，θ_3-i ，θ_4-i ，
θ_5-i ，θ_6-i は、直ちにツール取扱装置２の関節変数
記憶部１９ｂt （図３参照）に記憶され、「ワーク取扱
装置１の時間ΔＴ経過後の関節変数」φ_1-i ，φ_2-i ，
φ_3-i ，φ_4-i ，φ_5-i ，φ_6-i は、ＳＴ１０６の時点
で、ワーク取扱装置１の関節変数記憶部１９ｂw に記憶
されている。ツール取扱装置２における「アクチュエー
タ動作指令値」およびワーク取扱装置１における「アク
チュエータ動作指令値」の単位当たりの「関節変数」の
動作量を、Ｒ_t1 ,Ｒ_t2 ,Ｒ_t3 ,Ｒ_t4 ,Ｒ_t5 ,Ｒ_t6 ,Ｒ_w1
,Ｒ_w2 ,Ｒ_w3 ,Ｒ_w4 ,Ｒ_w5 ,Ｒ_w6とする。いずれもロボ
ットごとに決まった値であり、制御装置４に予め記憶さ
れている。

【００８９】ＳＴ１４３で求めた「ツール取扱装置２の
時間ΔＴ経過後の関節変数」θ_1-i，θ_2-i ，θ_3-i ，
θ_4-i ，θ_5-i ，θ_6-i から、ツール取扱装置２の「ア
クチュエータＭ_t の動作指令値」ａ_t1-i ,ａ_t2-i ,ａ
_t3-i ,ａ_t4-i ,ａ_t5-i ,ａ_t6-iが、前記した式(38)と同
じ式によって演算される〔ST144 〕。また、ＳＴ１０６
で求めた「ワーク取扱装置１の時間ΔＴ経過後の関節変
数」φ_1-i ，φ_2-i ，φ_3-i ，φ_4-i ，φ_5-i ，φ_6-i
から、ワーク取扱装置１の「アクチュエータＭ_wの動作
指令値」ａ_w1-i ,ａ_w2-i ,ａ_w3-i ,ａ_w4-i ,ａ_w5-i ,ａ
_w6-iが、前述と同じ式(39)によって演算される〔ST145
〕。こうして得られた「ツール取扱装置２のアクチュ
エータ動作指令値」ａ_t1-i ,ａ_t2-i ,ａ_t3-i ,ａ_t4-i ,
ａ_t5-i ,ａ_t6-iと「ワーク取扱装置１のアクチュエータ
動作指令値」ａ_w1-i ,ａ_w2-i ,ａ_w3-i ,ａ_w4-i ,ａ_w5-i
,ａ_w6-iとは、経過時間ΔＴ×ｉとなった直後に〔ST14
6 〕、ワーク取扱装置１とツール取扱装置２とを独立し
て作動させるために個別に、ほとんど同時に、ワーク取
扱装置１のアクチュエータ動作指令部４w （図３参照）
と、ツール取扱装置２のアクチュエータ動作指令部４t
へ出力される〔ST147, ST148〕。その後は、ＳＴ１３８
へ戻される。ＳＴ１４７，ＳＴ１４８の後に、ワーク取
扱装置１のアクチュエータＭ_1w ,Ｍ_2w ,Ｍ_3w ,Ｍ_4w ,Ｍ
_5w ,Ｍ_6w（図１７参照）が作動して〔ST149 〕、ワーク
取扱装置１により「ワーク１ｗの位置姿勢」が変化され
る〔ST151 〕。また、ツール取扱装置２のアクチュエー
タＭ_1t ,Ｍ_2t ,Ｍ_3t ,Ｍ_4t ,Ｍ_5t ,Ｍ_6tも同時に作動し
〔ST150 〕、ツール取扱装置２により「ツール２ｔの位
置姿勢」が変化される〔ST152 〕。これ以後は、前述し
た「連動する直角手動操作モード」の場合と変わるとこ
ろがない。

【００９０】このような動作は、図４３の（ａ）から
（ｂ）の変化であり、必要に応じて図４３の（ｂ）から
（ｃ）やさらには図４３の（ｄ）へ単独の手動操作によ
って移行される。なお、主動作機械がツール取扱装置、
従動作機械がワーク取扱装置という動作指示を受けた場
合は、ワーク取扱装置のワークの許容移動速度Ｖ_{w AL}お
よび許容回転速度ω_{w AL}と、ツール取扱装置のツールの
許容移動速度Ｖ_{t AL}および許容回転速度ω_{t AL}のそれぞ
れの扱いが逆となり、各々の許容値の扱いを入れ替える
ことによって、ＳＴ１０１ないしＳＴ１５２と同様の処
理を行い、「ワークのツールに対する位置姿勢を一定に
保つ」ことができる。

【図面の簡単な説明】

【図１】 本発明の一つである「連動手動教示操作」を
実現する制御構成のブロック図。

【図２】 「連動手動教示操作」の制御と「単独手動教
示操作」の制御を実現するロボットの制御構成の全体ブ
ロック図。

【図３】 本発明の他の「連動手動教示操作」を実現す
る制御構成のブロック図。

【図４】 本発明が適用される二基のロボットからなる
ロボット配置図。

【図５】 各ロボットに採用された種々な座標系と、各
座標系の「位置姿勢」の関係を表す同次変換行列の関連
説明図。

【図６】 ティーチングペンダントの操作パネルの正面
図。

【図７】 制御装置の内部構成図。

【図８】 ロボットの教示・再現の運転を切り換える操
作ボックス図。

【図９】 本発明における一つの制御の進展を説明する
フローチャート。

【図１０】 図９に続くフローチャート。

【図１１】 図１０に続くフローチャート。

【図１２】 図１１に続くフローチャート。

【図１３】 本発明における他の制御の進展を説明する
フローチャート。

【図１４】 図１３に続くフローチャート。

【図１５】 図１４に続くフローチャート。

【図１６】 図１５に続くフローチャート。

【図１７】 各ロボットの駆動系統の説明図。

【図１８】 ロボットの各リンクにおける座標系の説明
図。

【図１９】 ロボットの各関節における座標系の説明
図。

【図２０】 ワークの各軸方向の移動成分の説明図。

【図２１】 ワークの各軸回りの回転成分の説明図。

【図２２】 ワークの合成移動方向の説明図。

【図２３】 ワークの合成回転方向の説明図。

【図２４】 実施例における一つの制御を説明するフロ
ーチャート。

【図２５】 図２４に続くフローチャート。

【図２６】 図２５に続くフローチャート。

【図２７】 図２６に続くフローチャート。

【図２８】 図２７に続くフローチャート。

【図２９】 図２８に続くフローチャート。

【図３０】 図２９に続くフローチャート。

【図３１】 ワークにおける異なる速度制御を説明する
フローチャート。

【図３２】 ワークにおけるさらに異なる速度制御を説
明するフローチャート。

【図３３】 ツールの速度を考慮したワークにおける異
なる速度制御を説明するフローチャート。

【図３４】 ツールの速度を考慮したワークにおけるさ
らに異なる速度制御を説明するフローチャート。

【図３５】 ツールを大地に対して一定にし、かつ、ワ
ークに対するツールの位置を一定に保つ教示操作時の教
示手順図。

【図３６】 実施例における一つの制御を説明するタイ
ミングチャート。

【図３７】 実施例における他の制御を説明するフロー
チャート。

【図３８】 図３７に続くフローチャート。

【図３９】 図３８に続くフローチャート。

【図４０】 図３９に続くフローチャート。

【図４１】 図４０に続くフローチャート。

【図４２】 図４１に続くフローチャート。

【図４３】 ワークに対するツールの位置姿勢を一定に
保つ教示操作時の教示手順図。

【図４４】 主動作機械の動きにもとづき従動作機械が
大きく動き、その間の従制御対象物の速度が大きくなる
ことを説明する概略図。

【図４５】 ロボットシステムにおける制御の全体の進
展を説明するフローチャート。

【図４６】 図４５に続くフローチャート。

【図４７】 ワーク取扱装置を「単独の直角手動操作」
によって教示する場合の速度制御を含むフローチャー
ト。

【図４８】 図４７に続くフローチャート。

【図４９】 ワーク取扱装置を「単独の直角手動操作」
によって教示する場合の速度制御を含むタイミングチャ
ート。

【図５０】 ツール取扱装置を「単独の関節手動操作」
によって教示する場合の速度制御を含むフローチャー
ト。

【図５１】 図５０に続くフローチャート。

【図５２】 図５１に続くフローチャート。

【図５３】 ツール取扱装置を「単独の関節手動操作」
によって教示する場合の速度制御を含むタイミングチャ
ート。

【符号の説明】

１…主取扱装置（ワーク取扱装置）、１ｍ…主制御対象
物、１ｗ…ワーク、２…従取扱装置（ツール取扱装
置）、２ｓ…従制御対象物、２ｔ…ツール、４…連動関
係制御手段、４Ａc …直角手動連動関係制御手段、４Ａ
j …関節手動連動関係制御手段、１２…動作指示スイッ
チ手段、２２…従許容速度設定手段、２３…速度成分指
示値演算部、２６…従速度演算手段、２７…補正速度成
分指示値従演算手段、３３…関節速度指示値演算部、３
７…補正速度指示値従演算手段、６４…大地、Ｓｘ，Ｓ
ｙ，Ｓｚ，Ｓα，Ｓβ，Ｓγ…動作指示信号、Ｓ１，Ｓ
２，Ｓ３，Ｓ４，Ｓ５，Ｓ６…動作指示信号、Ｖ_{s AL}…
従制御対象物の直角座標系における許容移動速度、Ｖｘ
_m ，Ｖｙ_m ，Ｖｚ_m …主制御対象物の直角座標系におけ
る移動速度成分指示値、Ｖ_S …従制御対象物の直角座標
系における合成移動速度換算値、Ｖ_sR…従制御対象物の
合成移動速度比、Ｖ１_m ，Ｖ２_m ，Ｖ３_m ，Ｖ４_m ，Ｖ
５_m ，Ｖ６_m …主制御対象物の各関節の回転速度指示
値、ω_{s AL}…従制御対象物の直角座標系における許容回
転速度、ωｘ_m ，ωｙ_m，ωｚ_m …主制御対象物の直角
座標系における回転速度成分指示値、ω_S …従制御対象
物の直角座標系における合成回転速度換算値、ω_sR…従
制御対象物の合成回転速度比。

フロントページの続き (56)参考文献 特開 平５−16084（ＪＰ，Ａ) 特開 昭58−146910（ＪＰ，Ａ) 特開 昭61−244472（ＪＰ，Ａ) 特開 昭63−80307（ＪＰ，Ａ) 特開 平３−108008（ＪＰ，Ａ) 特開 平５−233052（ＪＰ，Ａ) 特開 平５−84678（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 9/22 G05B 19/42

Claims (4)

(57)【特許請求の範囲】
1. 【請求項１】 主制御対象物の位置姿勢を変化させる主
   取扱装置と、従制御対象物の位置姿勢を変化させる従取
   扱装置とを有し、前記主取扱装置を教示手動操作する
   と、６自由度を有する従取扱装置が前記主取扱装置の動
   作に関連づけられて協調動作するようになっている産業
   用ロボットの手動教示操作による制御装置において、 教示作業中に位置姿勢が変化する従制御対象物の直角座
   標系における許容移動速度と許容回転速度とを設定する
   従許容速度設定手段と、 前記主制御対象物の位置姿勢を変化させるための動作指
   示信号を出力する動作指示スイッチ手段と、 直角手動操作モードにおいて、前記主取扱装置により位
   置姿勢が変化する主制御対象物と、該主取扱装置に協調
   動作する従取扱装置により位置姿勢が変化する従制御対
   象物との間の動作の関連づけを記憶すると共に、上記動
   作指示スイッチ手段からの動作指示信号にもとづき、前
   記主制御対象物の直角座標系における移動速度成分指示
   値と回転速度成分指示値を演算する速度成分指示値演算
   部を備えて、前記主制御対象物の位置姿勢を変化させる
   主取扱装置の動作を制御すると共に、前記従制御対象物
   の位置姿勢を変化させる従取扱装置の動作を制御する直
   角手動連動関係制御手段と、 上記直角手動連動関係制御手段に従い、上記主取扱装置
   と協調動作する従取扱装置によって位置姿勢が変化され
   る従制御対象物の直角座標系における合成移動速度換算
   値と合成回転速度換算値とを求める従速度演算手段と、 上記従速度演算手段によって求められた合成移動速度換
   算値もしくは合成回転速度換算値が、前記従許容速度設
   定手段により設定された許容移動速度もしくは許容回転
   速度を超えるとき、前記従速度演算手段によって求めら
   れた合成移動速度換算値もしくは合成回転速度換算値
   が、前記許容移動速度もしくは許容回転速度以下となる
   ように、前記速度成分指示値演算部で演算された移動速
   度成分指示値もしくは回転速度成分指示値を修正する補
   正速度成分指示値従演算手段とを備え、 該補正速度成分指示値従演算手段は、「前記従許容速度
   設定手段により設定された許容移動速度」を「前記従速
   度演算手段で求められた合成移動速度換算値」で除した
   従制御対象物の合成移動速度比、もしくは、「前記従許
   容速度設定手段により設定された許容回転速度」を「前
   記従速度演算手段で求められた合成回転速度換算値」で
   除した従制御対象物の合成回転速度比を演算し、該合成
   速度比を前記速度成分指示値演算部で演算した移動速度
   成分指示値や回転速度成分指示値に乗じて、該速度成分
   指示値を修正するようにし、 上記修正された移動速度成分指示値もしくは回転速度成
   分指示値でもって主制御対象物の位置姿勢を前記主取扱
   装置により変化させると共に、前記従制御対象物の許容
   移動速度もしくは許容回転速度以下でもって、該従制御
   対象物の位置姿勢を前記従取扱装置により変化させるよ
   うにしたことを特徴とする産業用ロボットの手動教示操
   作における速度制御装置。
2. 【請求項２】 主制御対象物の位置姿勢を変化させる主
   取扱装置と、従制御対象物の位置姿勢を変化させる従取
   扱装置とを有し、前記主取扱装置を教示手動操作する
   と、６自由度を有する従取扱装置が前記主取扱装置の動
   作に関連づけられて協調動作するようになっている産業
   用ロボットの手動教示操作による制御装置において、 教示作業中に位置姿勢が変化する従制御対象物の直角座
   標系における許容移動速度と許容回転速度とを設定する
   従許容速度設定手段と、 前記主制御対象物の位置姿勢を変化させるための動作指
   示信号を出力する動作指示スイッチ手段と、 関節手動操作モードにおいて、前記主取扱装置により位
   置姿勢が変化する主制御対象物と、該主取扱装置に協調
   動作する従取扱装置により位置姿勢が変化する従制御対
   象物との間の動作の関連づけを記憶すると共に、上記動
   作指示スイッチ手段からの動作指示信号にもとづき、前
   記主取扱装置の各関節の回転速度指示値を演算する関節
   速度指示値演算部を備えて、前記主制御対象物の位置姿
   勢を変化させる主取扱装置の動作を制御すると共に、前
   記従制御対象物の位置姿勢を変化させる従取扱装置の動
   作を制御する関節手動連動関係制御手段と、 上記関節手動連動関係制御手段に従い、上記主取扱装置
   と協調動作する従取扱装置によって位置姿勢が変化され
   る従制御対象物の直角座標系における合成移動速度換算
   値と合成回転速度換算値とを求める従速度演算手段と、 上記従速度演算手段によって求められた合成移動速度換
   算値もしくは合成回転速度換算値が、前記従許容速度設
   定手段により設定された許容移動速度もしくは許容回転
   速度を超えるとき、前記従速度演算手段によって求めら
   れた合成移動速度換算値もしくは合成回転速度換算値
   が、前記許容移動速度もしくは許容回転速度以下となる
   ように、前記関節速度指示値演算部で演算された各関節
   の回転速度指示値を修正する補正速度指示値従演算手段
   とを備え、 該補正速度指示値従演算手段は、「前記従許容速度設定
   手段により設定された許容移動速度」を「前記従速度演
   算手段で求められた合成移動速度換算値」で除した従制
   御対象物の合成移動速度比、もしくは、「前記従許容速
   度設定手段により設定された許容回転速度」を「前記従
   速度演算手段で求められた合成回転速度換算値」で除し
   た従制御対象物の合成回転速度比を演算し、該合成速度
   比を前記関節速度指示値演算部で演算した各関節の回転
   速度指示値に乗じて、該回転速度指示値を修正するよう
   にし、 上記修正された各関節の回転速度指示値でもって主制御
   対象物の位置姿勢を前記主取扱装置により変化させると
   共に、前記従制御対象物の許容移動速度もしくは許容回
   転速度以下でもって、該従制御対象物の位置姿勢を前記
   従取扱装置により変化させるようにしたことを特徴とす
   る産業用ロボットの手動教示操作における速度制御装
   置。
3. 【請求項３】 前記主取扱装置は主制御対象物であるワ
   ークの位置姿勢を変化させるワーク取扱装置である一
   方、前記従取扱装置は前記ワークに作業を施す従制御対
   象物であるツールの位置姿勢を変化させるツール取扱装
   置であり、 前記連動関係制御手段に記憶されている前記ワーク取扱
   装置により位置姿勢が変化するワークと該ワーク取扱装
   置に協調動作するツール取扱装置により位置姿勢が変化
   するツールとの間の動作の関連づけは、「前記ツールの
   前記ワークに対する相対的な位置姿勢」を一定に保つよ
   うになっていることを特徴とする請求項１または請求項
   ２に記載された産業用ロボットの手動教示操作における
   速度制御装置。
4. 【請求項４】 前記主取扱装置は主制御対象物であるワ
   ークの位置姿勢を変化させるワーク取扱装置である一
   方、前記従取扱装置は前記ワークに作業を施す従制御対
   象物であるツールの位置姿勢を変化させるツール取扱装
   置であり、 前記連動関係制御手段に記憶されている前記ワーク取扱
   装置により位置姿勢が変化するワークと該ワーク取扱装
   置に協調動作するツール取扱装置により位置姿勢が変化
   するツールとの間の動作の関連づけは、「大地に対する
   ツールの姿勢」を一定に保ち、かつ、「ツールのワーク
   に対する相対的な位置」を維持するようになっているこ
   とを特徴とする請求項１または請求項２に記載された産
   業用ロボットの手動教示操作における速度制御装置。