JPH02284883A - マスタスレーブ型マニピュレータ - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔発明の目的〕 （産業上の利用分野） 本発明は、マスタ側アーム機構及びスレーブ側アーム機
構の各アームをロープ部材を介して駆動するマスタスレ
ーブ型マニピュレータに関するものである。

（従来の技術） 第４図は従来のマスタスレーブ型マニピュレータのスレ
ーブ側アーム機構を示す正面図である。

この図において、ドライブユニット１は、サーボモータ
２、減速機３、さらに、図示を省略しであるトルク検出
器、回転角検出器等により構成されている。

サーボモータ２の出力は、減速機３、かさ歯車４．５、
ロープ部材６を介して、中間アーム７及び先端アーム８
の関節に取付けられているプーリ９．１０に伝達される
ようになっている。そして、これらプーリ９，１０の回
転により、中間アーム７及び先端アーム８は各関節を中
心にして所定角度の回転動作を行う。なお、マスタ側ア
ーム機構も、第４図のスレーブ側アーム機構と略同−構
成となっている。

次に、上記のように構成されるマスタスレーブ型マニピ
ュレータの動作を第５図のブロック図を参照しつつ説明
する。

まず、オペレータが、マスタ側先端アーム１１を所望の
角度だけ回転させると、サーボモータ１２の出力軸も回
転する。回転角検出器１３は、このときの回転角を検出
して回転角信号θ□を出力する。そして、この回転角信
号θ８と、スレーブ側の回転角検出器１４からの回転角
信号θ８との間の回転角偏差信号ε。が位置調整器１５
に送られる。

位置調整器１５は、この回転角偏差信号ε、に対応した
位置調整信号を、増幅器１６を介して、サーボモータ２
に送る。サーボモータ２は、この位置調整信号により、
回転角偏差信号εσがゼロになる方向に回転する。これ
によって、スレーブ側先端アーム８は、マスタ側先端ア
ーム１１の動作に追従するように駆動制御されることに
なる。

このとき、サーボモータ２，１２の出力軸に加わるトル
クＴ　ｓ　、Ｔ　ｓは、それぞれトルク検出器１７．１
８により検出され、トルク偏差信号ε１がトルク調整器
１９に送られる。トルク調整器１９は、このトルク偏差
信号ε、ｒに対応したトルク調整信号を、増幅器２０を
介して、サーボモータ１２に送る。サーボモータ１２は
、このトルク調整信号により、トルク偏差信号εＴがゼ
ロになるように回転駆動される。そして、このトルク調
整信号に基づくサーボモータ１２の駆動力の大きさによ
り、オペレータは、スレーブ側先端アーム８に加わる負
荷の大きさを感じることができるようになっている。つ
まり、このマスタスレーブ型マニピュレータは、パイラ
テラルサーボ機構を採用した構成となっている。

（発明が解決しようとする課題） しかしながら、上述したマスタスレーブ型マニピュレー
タでは、サーボモータ２の駆動力はロープ部材６を介し
て、スレーブ側先端アーム８に伝達されるようになって
いる。そのため、スレーブ側先端アーチ８に作業用工具
等が取付けられたときには、この作業用工具の重量に起
因してロープ部材６に大きな引張り力が加わり、ロープ
部材６に伸びが発生することになる。

ロープ部材６にこのような伸びが発生すると、作業領域
内におけるスレーブ側先端アーム８の姿勢に関し、回転
角検出器１４で検出される回転角θ８に対応する姿勢と
、実際の姿勢との間にずれが生じることになる。このた
め、オペレータがマスタ側先端アーム１１を操作して、
スレーブ側先端アーム８に所望の作業を行なわせる場合
に、マスタ側先端アーム１１の姿勢とスレーブ側先端ア
ーム８の姿勢とが一致しなくなり、作業を円滑に行うこ
とができない場合があった。

本発明は上記事情に鑑みなされたものであり、スレーブ
側先端アームにロープ部材の伸びが発生した場合にも、
常に、マスタ側先端アームの姿勢とスレーブ側先端アー
ムの姿勢とを一致させることができるマスタスレーブ型
マニピュレータを提供することを目的としている。

〔発明の構成〕

（課題を解決するための手段） 本発明は上記課題を解決するための手段として、マスタ
側アーム機構及びスレーブ側アーム機構の各アームを、
ロープ部材を介してマスタ側駆動モータ及びスレーブ側
駆動モータにより駆動するようにし、前記マスタ側アー
ム機構を操作したときに、マスタ側駆動モータ出力軸の
回転角とスレーブ側駆動モータ出力軸の回転角とを検出
し、これら両回転角についての回転角偏差をゼロにする
方向に前記スレーブ側駆動モータを駆動して、前記マス
タ側アーム機構の動作に前記スレーブ側アーム機構を追
従させるようにしたマスタスレーブ型マニピューレタに
おいて、前記スレーブ側駆動モータ出力軸の回転角を検
出するスレーブ側回転角検出手段と、前記スレーブ側回
転角検出手段の検出に基づいて、前記スレーブ側アーム
機構の先端アームが被作業対象物を保持したときの、該
先端アームの姿勢を演算する姿勢演算手段と、前記姿勢
演算手段の演算に基づいて、特定平面に対する前記先端
アームの傾斜角を演算する傾斜角演算手段と、前記傾斜
角演算手段の演算結果に基づいて、前記ロープ部材に加
わる荷重を演算するロープ荷重演算手段と、前記ロープ
荷重演算手段の演算結果に基づいて、前記ロープ部材の
伸び量に対応する、前記先端アームの伸び対応角度を演
算する伸び対応角度演算手段と、前記伸び対応角度と前
記傾斜角との角度比率を演算する角度比率演算手段と、
前記角度比率演算手段で演算された角度比率に基づいて
、前記スレーブ側回転角検出手段が検出した検出値を補
正する回転角補正手段と、を備えた構成としである。

（作　用） スレーブ側アーム機構の先端アームが被作業対象物を保
持したとき、スレーブ側回転角検出手段はスレーブ側駆
動モータ出力軸の回転角を検出し、この検出に基づいて
、姿勢演算手段は被作業対象物を保持したときのスレー
ブ側先端アームの姿勢を演算する。傾斜角演算手段はこ
の演算に基づいて、特定平面に対するスレーブ側先端ア
ームの傾斜角を演算する。

ロープ荷重演算手段は、この演算された傾斜角に基づい
て、ロープ部材に加わる荷重を演算し、伸び対応角度演
算手段は、この演算された荷重に基づいて、ロープ部材
の伸び量に対応する伸び対応角度を演算する。角度比率
演算手段は、演算された伸び対応角度と先端アームの傾
斜角との角度比率を演算する。

そして、回転角補正手段は、演算された角度比率に基づ
いて、スレーブ側駆動モータ出力軸の回転角についてス
レーブ側回転角検出手段により検出された検出値を補正
する。

（実施例） 以下、本発明の実施例を第１図乃至第３図に基づき説明
する。但し、第４図及び第５図と同様の構成要素には同
一符号を付して重複した説明を省略する。

第２図は作業用工具２１を保持したときの、スレーブ側
先端アーム８の姿勢を示す説明図である。

いま、このマニピュレータの自由度がｎであるとして、
スレーブ側先端アーム８の軸が第ｎ軸であるとする。そ
して、第１軸から第ｎ軸までにおける各軸の回転角度を
θ１・・・θ。とすると、第ｎ軸の姿勢はこの回転角度
θ１・・・θ□をトータルしたちのとして求めることが
できる。そして、θ１・・・θ　によるトータルの座標
変換行列を、ｃ　１−・・Ｃ，＝［ｃｉｊコ　　（ｉ、
　　　ｊ−１，２，３）とすると、座標系（Ｘ、　Ｙ、
　　Ｚ）における第ｎ軸の姿勢は の式により置き換えることができる。ここで、αは第ｎ
軸がＸ軸となす角度であり、βは第ｎ軸がＸ軸及びＹ軸
を含む水平面となす角度であり、γは第ｎ軸の軸回りの
回転角度である。

そして、スレーブ側先端アーム８に作業用工具２１が取
付けられ、スレーブ側先端アーム８が第・・・・・・・
・・（１） ２図の状態にあるとすると、第１図における姿勢検出手
段２２は、回転角検出器１４で検出した第１輔から第ｎ
軸までの回転角θ１・・・θ。をトータルすることによ
りスレーブ側先端アーム８の姿勢を演算する。

傾斜角演算手段２３は、この演算に基づいて、スレーブ
側先端アーム８の軸がＸ輔及びＹ軸を含む水平面となす
角度βを、（１）式により演算する。

次いで、ロープ荷重演算手段２４は、この演算された傾
斜角βに基づいて、スレーブ側先端アーム８のロープ部
材６に加わる荷重を演算する。すなわち、第２図におい
て、傾斜角βが求まれば、作業用工具２１の重ｆｆ１Ｗ
Ｔ及びその重心位置、並びにスレーブ側先端アーム８の
重量ＷＡ及びその重心位置は既知であるから、関節２５
に作用するトルクを求めることができる。したがって、
この関節に直結されているプーリ（図示せず）に巻回さ
れたロープ部材６（第２図における図示は略）について
、その引張り荷重を求めることができる。

ロープ部材６に引張り荷重が加わると、その伸び量は第
３図に示す特性曲線２５ａにより決まるが、伸び対応角
度演算手段２６は、この伸び二に対応する伸び対応角度
Δβを演算する。

次いで、角度比率演算手段２７は、この伸び対応角度Δ
βと、傾斜角演算手段２３で演算されたβとの角度比率
Δβ／βを演算する。

そして、回転角補正手段２８は、この演算された角度比
率Δβ／βに基づいて回転角検出器１４で検出された回
転角θ８の補正を行う。つまり、θ８は上記したロープ
部材６の伸びを考慮しない場合の回転角であるため、ス
レーブ側先端アーム８の実際の姿勢に対応した回転角を
求めるためには、このθ　からθｓ傘Δβ／βを減算し
、θ８　（１−Δβ／β）とする必要がある。回転角補
正手段２８は、回転角検出器１４で検出された回転角θ
　を、このように０８　（１−Δβ／β）に補正を行う
ものであるる 上記のように、回転角検出器１４で検出された回転角θ
８が、回転角補正手段２８によりθｓ　（１−Δβ／β
）に補正されるので、スレーブ側先端アーム８のロープ
部材６に伸びが生じても、このスレーブ側先端アーム８
の姿勢と、マスタ側先端アーム１１との姿勢を常に一致
させることができる。

ところで、このようなマニピュレータの使用を継続して
いくうちに、ロープ部材６の特性が経年変化あるいは使
用頻度によって次第に変化するという問題が生ずる。つ
まり、ロープ荷重からロープ伸び量を算出する場合、当
初は第３図の特性曲線２５ａに従ってロープ荷重を算出
することになるが、この算出の基礎となる特性曲線を次
第に２５ｂ、２５ｃに移行していく必要がある。

そこで、本実施例では、スレーブ側先端アーム８に水平
検出器２９を取付けて、一定期間経過毎に、第３図の特
性曲線２５ａ、２５ｂ、２５ｃのうちいずれの特性曲線
を選択するのが妥当かをチエツクできるようにしである
。

すなわち、第２図において、当初、水平面Ｈに位置して
いたスレーブ側先端アーム８を角度βだけ上方へ立上げ
るようにする。この上方へ立上げた状態でのロープ荷重
は、前述のように、ロープ荷重演算手段により求めるこ
とができる。そして、この立上げた状態から、今度は次
第に立下げていくと、実際にはロープ部材に伸びが発生
しているために、立上げた状態から下方に向けてβ′−
β−Δβだけ回転した時点で、水平検出器２つから水平
状態にあることを示す水平信号が発せられることになる
。したがって、β−β′−Δβにより、伸び対応角度Δ
βを求めることができ、この伸び対応角度Δβに対応す
るロープ伸び量を求めることができる。

これによって、Ｔ＆３図における、縦軸のロープ荷重の
値と、横軸のロープ伸び量の値とが求められたので、両
者の交点の位置を確認することによって、特性曲線２５
ａ、２５ｂ、２５ｃのうちいずれを選択するのが妥当か
を判断することができる。

なお、上記実施例では傾斜角演算手段２３は、Ｘ軸、Ｙ
軸の両軸を含む水平面に対する傾斜角を演算するように
なっているが、この水平面即ち特定平面は、水平面に対
して例えば４５度等の傾斜角を有する平面であってもよ
い。

〔発明の効果〕

以上のように、本発明によれば、スレーブ側回転角検出
手段によるスレーブ側駆動モータ出力軸の回転角に基づ
いて、被作業対象物を保持したときのスレーブ側先端ア
ームの姿勢を姿勢演算手段が演算し、この演算に基づい
て傾斜角演算手段が特定平面に対するスレーブ側先端ア
ームの傾斜角を演算し、この演算に基づいてロープ荷重
演算手段がロープ部材に加わる荷重を演算し、この演算
に基づいて伸び対応角度演算手段がロープ部材の伸び量
に対応するスレーブ側先端アームの伸び対応角度を演算
し、この伸び対応角度と傾斜角との角度比率を角度比率
演算手段が演算し、この演算された角度比率に基づいて
、スレーブ側回転角補正手段がスレーブ側回転角検出手
段の検出値を補正する構成としたので、スレーブ側先端
アームにロープ部材の伸びが発生した場合にも、常に、
マスタ側先端アームの姿勢とスレーブ側先端アームの姿
勢とを一致させることができる。

【図面の簡単な説明】

第１図は本発明の実施例の構成を示すブロック図、第２
図は第１図の動作を説明するための説明図、第３図は第
１図におけるスレーブ側ロープ部材の特性図、第４図は
スレーブ側アーム機構を示す正面図、第５図は従来例の
構成を示すブロック図である。 ２・・・スレーブ側駆動モータ（サーボモータ）、６・
・・ロープ部材、８・・・スレーブ側先端アーム、１１
・・・マスタ側先端アーム、１２・・・マスタ側駆動モ
ータ（サーボモータ）、１４・・・スレーブ側回転角検
出手段（回転角検出器）、２２・・・姿勢検出手段、２
３・・・傾斜角演算手段、２４・・・ロープ荷重演算手
段、２６・・・伸び対応角度演算手段、２７・・・角度
比率演算手段、２８・・・回転角補正手段。 出願人代理人　　佐　　藤　　−雄 ロート暉酬 第５ 図

Claims (1)

1. 【特許請求の範囲】 マスタ側アーム機構及びスレーブ側アーム機構の各アー
   ムを、ロープ部材を介してマスタ側駆動モータ及びスレ
   ーブ側駆動モータにより駆動するようにし、前記マスタ
   側アーム機構を操作したときに、マスタ側駆動モータ出
   力軸の回転角とスレーブ側駆動モータ出力軸の回転角と
   を検出し、これら両回転角についての回転角偏差をゼロ
   にする方向に前記スレーブ側駆動モータを駆動して、前
   記マスタ側アーム機構の動作に前記スレーブ側アーム機
   構を追従させるようにしたマスタスレーブ型マニピュー
   レタにおいて、 前記スレーブ側駆動モータ出力軸の回転角を検出するス
   レーブ側回転角検出手段と、 前記スレーブ側回転角検出手段の検出に基づいて、前記
   スレーブ側アーム機構の先端アームが被作業対象物を保
   持したときの、該先端アームの姿勢を演算する姿勢演算
   手段と、 前記姿勢演算手段の演算に基づいて、特定平面に対する
   前記先端アームの傾斜角を演算する傾斜角演算手段と、 前記傾斜角演算手段の演算結果に基づいて、前記ロープ
   部材に加わる荷重を演算するロープ荷重演算手段と、 前記ロープ荷重演算手段の演算結果に基づいて、前記ロ
   ープ部材の伸び量に対応する、前記先端アームの伸び対
   応角度を演算する伸び対応角度演算手段と、 前記伸び対応角度と前記傾斜角との角度比率を演算する
   角度比率演算手段と、 前記角度比率演算手段で演算された角度比率に基づいて
   、前記スレーブ側回転角検出手段が検出した検出値を補
   正する回転角補正手段と、 を備えたことを特徴とするマスタスレーブ型マニピュレ
   ータ。