JPS59152079A - マスタ・スレ−ブ形マニプレ−タの制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の技術分野〕 本発明は、実際の作業をおこなうスレーブ側マニプレー
タと、このスレーブ側マニブレータヲ操縦するマスク側
マニプレータとχ有′ｆるマスタ・スレーブ形マニプレ
ータに関で６０ 〔発明の技術的背景とその問題点〕 原子力発電所等において、遠隔操縦により複雑かつ細か
な作業７行なう場合、力感覚によって対象物や環境の情
報？オペレータに帰還ｆる必要がある◇もし力感覚の帰
還がない場合、：Ａ′ペレータハスレーブ側マニプレー
タの状態火工ａ用−ｙ−ｖビ（ＩＴＶ）等の視覚情報に
より推定ｆろことになり、正確な操縦ができないばかり
でなく、操縦するに際しての疲労感も強いことが指摘さ
れている。

そこで、従来はスレーブ側マニプレータに作用したカン
検出してマスク側マニプレータに帰還でるカサーボ機構
ケ、マスク側マニプレータからの位置指令によりスレー
ブ１１１１１マニプレータ２制例丁す位１１サーボ機構
に加えたパイラテラル（反動型）サーボ機構ン用いてい
る。第１図に、このノ（イラテラルサーボ機構Ｗ用いた
マスク・スレーブ形マニプレータ％’　示ｉｏマスク・
スレーフ形−マニブレータはスレーブ側マニプレータ１
とマスク側マニプレータ８とχ備えている。スレーフ゛
１則マニプレータ ム３にとりつけられた作業工具２により実際の作業？お
こなうものである。スレーブ側位置検出器５は，スレー
ブ１則マニプレータアーム３の姿勢を検出し、スレーブ
側トルク検出器６は、関節４に作用てろ全トルクな検出
てる。スレーブ側モータ７はスレー７’ｌｌｌｌマニプ
レータアーム３乞駆＃ｂでる。

マスク側マニプレータ８は，関４１２とマスク側マニプ
レータアーム１１にとりつけられ１こ）１ン′ドル１０
ケオペレータ９が操縦でることによって、スレーフ゛１
則マニプレータアーム３Ｙ駆＄１１ｆろものである。

マスク側位置検出器１３は、マスク側マニプレータアー
ム１１の姿勢を検出し、マスク側トルク検出器１４は、
関節１２に作用′ｆる全トルク欠検出ｊ　７）ｏマスタ
側モータ１５は，マスタ側マニプレータアーム１１を駆
動ｆる。さらに、位置調節器１７は、マスク側位置検出
器１３の出力である目標値にスレーブ側位置検出器５の
出力？一致させるように制御するものであり，サーボ増
巾器Ｉ８は，位置調節器１７の指令に基づきスレーブ側
モータ７ケ駆動’ｆるものである。トルク調節器潤は，
スレーブ側トルク検出器６の出力である目標値にマスク
側トルク検出器【４の出カケ一致させるように制御ｆ石
ものであり、サーボ増巾器２６は，トルク調節器囚の指
令に基づきマスタ側モータ１５Ｖ駆弛−ｃ−６。

かかるパイラテラル機構は力帰逮形パイラテラルサーボ
機構と呼ばれるものである。その位１４ｆーポループは
，マスタ側位置検出器１３と，スレーブ側位置検出器５
と、位置調節器１７，スレーブ側すーボ増ｌコ器１８と
，スレーブ側モータ７とで構成され、カサーボルーブは
、スレーブ側トルク検出器６と、マスタ１（１１１　）
ルク検出器１４と、トルク調節器囚と、マスタｉｌ＋ｌ
サーボｊ曽巾器２６と、マスタｉｌｌｌｌモータ１５と
で構成されてい６つ しかし１．、ｃがら，このような１ｉｔ来のマスク・ス
レフ形マニプレータにおいて，スレーブ側トルりの検出
器６で検出されるトルクは，実際に作業工具２が対象物
に穴あけ等の作業ン行なうことで発生でるトルクに加え
て、スレーブ側マニプレータアーム３の１直と作業工具
２の自重により発生するトルクが重畳したものである。

またマスク側トルク検出器１４で検出されるトルクは、
実際にスレーブ側−ｒニブレータ１に作用した力帰還量
に加えて、マスク側マニプレータアーム１１の自重によ
り発生−ｆ６トルクが重畳したものである。したがって
オペレータ９は、作業工具２やスレーブ側マニ′ブレー
タ３の自重ン含めて操縦時に感することになり。

作業工具２が対象物から受ける反力を正しく力感覚とし
て把握して帰還てｂことができないという問題があった
。

この問題に対して、従来は，０屯による影響フ補償ｆる
ため、■マスク測量＃１２とスレーブ側関節４に自重に
見合うカウンタウェイトケ収りつけてバランス？とる方
法，■バネ等の弾性力によりバランスをとる方法，■油
圧、空気圧、′亀動力等のアクチュエータ？新たに取り
つけて自重に見合う反カケ発生させてパランスケとる方
法が知られている。

しかしながら、上述した方法では、マスタ測量′Ｎ６１
２とスレーブ測量頗４に、駆動用モータとは別にカウン
タウェイト（■の方法）や、バネ等の弾性部材（■の方
法）や、アクチュエータ（■の方法）ヲ嘔付ける必要が
あるため、マニプレータ全体の重量が重くなり、取付け
たものの摩擦力や慣性力により大きな操縦力が必要とな
る欠点があった。また、大きな操縦力のために、スレー
ブ側マニプレータに加えられた小さな外力Ｙマスク側マ
ニプレータでオペレータが力感覚として得ろことができ
ないという欠点があった。更に、作業工具ケ交換した場
合に、カウンタウェイトの重量を調整したＩＸ（Ｄの方
法）、バネ等の弾性力を調整したり（■の方法）、自重
補償用アクチュエータの所要ドルクン演算しなおしたり
（■の方法）ｆろ必要があり問題となっていたり 〔発明の目的〕 本発明は、上記事情を考慮してなされたものであり、マ
ニプレータアームや作業工具の自重により発生−ｒるト
ルクを補償し、作業工具が対象物から受ける反力のみＹ
正しく力感覚として把握できるようにして、良好な操縦
性が得られるマスク・スレーブ形マニプレータの制御装
置ｒｔ提Ｌｆｂことを目的とｆる。

〔発明の概要〕

上記目的乞達成−ｆ′ろために、本発明によるマスク・
スレーブ形マニプレータの制御装置は、スレーブ側マニ
プレータの自重トルクと荷重ドルクン分Ａ［Ｉし、マス
ク側マニプレータの荷重トルクに前記スレーブ側マニプ
レータの自重トルフケ加えて。

前記マスク側マニプレータの関節部Ｖ駆動ｆるこトニよ
り、前記スレーブ側マニプレータに加えられた外カン前
記マスク側マニプレータｙｒ操縦ｆるときの外力として
帰還ｆることケ特徴とｆ′／ｂ０〔発明の実施例〕 以下１本発明？図示の一実施例に基づいて説明でる。第
２図に１本発明の第１の実施例によるマスタ・スレーブ
形マニプレータの制御装置ン示すこの制御装置は、マス
ク・スレーブ形マニプレータ１７’）各１４１節４．１
２％’制御でるものである。スレーブ側マニプレータの
関節４には、モータ７と位置検出器５とトルク検出器６
とが設けられ、実際の工作は工具２によりおこなわれる
。！ｌ：　ｒ、ニーマスク側マニプレータの関節１２に
は、モータ１５と位置検出器１３とトルク検出器１３と
が設けられ、オペレータ９により操縦される。１６で示
されろ位１斤サーボループは、マスク側位置検出器１３
と、スレーブ側位置検出器５と、位置調節器と、スレー
ブ側サーボ増巾器Ｉ８と、スレーブ（ｌｌモータ７とで
構成される。

１９で示されるカザーボルーブは、スレーブ側トルク検
出器６と、マスク側トルク検出器１４と、スレーブ側荷
重トルク演算器２１と、スレーブ側自重トルク演算器２
〕と、マスタ側荷重トルク検出器Ｚと。

マスタ側自重トルク演算器ｎと、トルクＨｊｌｌＪＷ６
器別と、自重トルク加算４乙と１．マスタ１則サーボ増
巾器がと、マスタ側モータ１５どで構成される。

カサーボルーブ１９ｖｃおいて、マスク側内ボトルク演
算器２ｏは、スレーブ側位置検出器５の出カケ受ケて、
スレーブ側マニプレータアーム３と１’Ｆ＝　−５１ｉ
ｊ工具２のそれぞれの日頃により発生″ｆｂ）ルクＹ演
１１ｆるもσ）であり、スレーブ側荷重トルク演算器２
１は、スレーブ側トル久：・笑出器６の出力からスレー
ブ側自ＩＮ　）ルク演真器加の出力Ｙ減算し、作業工具
２が対象物に加えたトルクのみ？演算器るものであり、
マスク測自車トルク演算器乙は、マスク側位置検出器１
３の出力、？受けてマスタ仙１マニプレータアーム１１
の自重により発生でろトルフケ演＠、ｆろものであり、
マスク側荷重トルク演算器路は、マスク側トルク検出器
１４の出力からマスク側自重トルク演算器四の出力Ｙ減
算し、スレーブ側マニプレータから帰還されたトルク？
演算するものであり、トルク調節４別は、スレーブ側荷
重トルク演算器２１の出力である目標値にマスク側荷重
トルク演算器器の出力Ｗ一致させるようにトルクを制御
丁すものであり、自重トルク加算器５は、トルク調節器
ムの出力とマスク側自重トルク演算器ｎの出力を加算ｆ
るものであり、マスク匍１サーボ１曽巾器２６は、自車
トルク加算器部の出方ン・受けてマスター則１列節１２
に収りつけられたマスク側モータ１５ン駆動ｆろもので
あ石。位置サーボループ１６において、位１ｔＷＦＡ顛
器１７は、マスタ側位置検出器１３の出力である目標値
にスレーブ側位前検出器５の出カケ一致させるように位
置馨制御ｆろものであり、スレーブ側す−ボ増中器１８
は、位置調節器１７の出カケ受けてスレーブ側関節４に
敗りつけたスレーブ１則モータ７ン鳴夷１１ｆるもので
ある。

次にこのような構成の制＃Ｊ′装置によって、マスタ側
マニフ“レータアーム１１と、スレーフ゛１則マニプレ
ータアーム３と、作業工具２との自重が補償できること
ン示す。マニプレータが第３図（ａ）に示すような１関
節形マニプレータであれば、ｘｙｚ座標系でのマニプレ
ータアームの角度ケθ、アーム重ＷｒｖＷ、関節からア
ーム重心までの距離Ｙ／と′ｒあと、関節に加わる自重
トルクＴは次式で求めろことができる。

Ｔ　＝　ＷｌｏＯｓ（−−θ）　　　　　　　・・・　
■自重トルク演算２加、２２では上記０式に従って自重
トルクを求め、この自重トルクＹ用いて荷車トルク演算
器２１．田において正しい荷重トルフケ検出できるので
、トルク調節器〃で正確な力帰還指令ン作ることができ
ろまた、マスク側自重トルク演算器ｎで求めた自重トル
クにより、マスク側マニプレータ８の自重により発生’
ｆるトルク？打ち消すトルクチトル２調節器別の出力に
加算できるので、良好な操縦性２得ろことができろ。

また、マニプレータが第３図（ｂ）に示すような４関節
形マニプレータの場合も、各関節１０１．１０２゜１０
３．１０４にカロわろ自重トルク’ｒｌ、　Ｔｌｌ　Ｔ
１．Ｔ４は次のように求めることができる。まずアーム
２０３の姿勢変化ケ、アーム２０２に設定した座標系に
おける角度θ４による極座標変化とし、アーム２０２の
姿勢変化ケ、アーム２０１に設定した座標系におけろ角
度θ３と角度θりによる極座標変化とし。

アーム２０１の姿勢変化ｖ、ｘｙｚ座標系における角度
θｌによる極座標変化とｆる。またアーム□２０１　、
２０２．２０３ｆｆ’１重量Ｙ　ＷＩ＋　Ｗ！　＋　Ｗ
ｓとし、各関節１０１．１０２．１０４から各アーム２
０１．２０２．２０３の重心までの距離ケそれぞれＩＪ
Ｉ＋／ｓ、７ｇとし、各アーム２０１．２０％展から距
離１１ｔ、１ｔｓ７￥引いたもの’１４．１４とｆ　７
ｂ　ｏ　ｆ　、Ｂと関節１０４．１０３゜１０２、１０
１での自重トルクＴａ　、　Ｔｓ　、　Ｔ！　、　Ｔ＋
は次式％式％ ただし、　Ａ＝ｃｏｓ（θ、−〇＋）＋　Ｂ＝ｃｏｓ（
θ、＋θ、）Ｃ＝ｃｏｓθ４ＳＩＩｌθ！　、　Ｄ　＝
ｓｉｎθ４（！０１ｌｆｊｌＥ＝ｓｉｎθ４ｓｊｎθ３ したがって、上記０式に従って、１関節形マニプレータ
の場合と同様にして良好な操縦性？得ることができる。

次に本発明の第２の実施例によるマスタスレーブ形マニ
プレータの側倒装置の一部Ｖ第４図に示で。本実施例は
１作業工具２．７敗り換えても同様な１助１乍７Ｊ′−
おこなえるものである。自車トルク演算器２０＆１スレ
ーフ゛＋１ｌｌｌマニプレータアーム３の自重によろト
ルクを演算−１７６スレ一ブ側マニプレータアーム自重
トルク演算器２０ａと、各作業工具の自重によるトルフ
ケ各々演算ｆ６作業工具自重トルク演算器２０ｂとで構
成されろ。作業工具自重トルク演算器２ｏｂには１作業
工具２０種＝ｇＡ　？判別して装着した作業工具２に相
当ｆろ自重トルクを演算させる指令が作業工具切換２四
から出力されろ。

そこで１作業工具２ケ嘔り侯え１こ場合スレーブ側−ｑ
ニブレータの自軍が、各作≠工具の自重だけ変化′ｆろ
ので、第３図（ａ）に示ｆ１関節形マニプレータでは、
アーム重着Ｗと重心までの距離ｌが変化し１こと考えて
■式Ｖ適用ｆればよい。同様に第３図（ｂ）に示ｆ４関
節形マニプレータでは、アーム２０３の重瞬Ｗ３と重心
までの距離ｌ＠が変化したと考えての弐？適用ｆればよ
い。あらかじめ各作業工具ごとに識別情＠を付けておき
、この識別情報ン作業工具切同器あで識別し、各作業工
具に対応でろ作業工具トルク演算器２０ｂ？用いろ。こ
の作業工具自重トルク演算器２０ｂの出力にスレーブ側
マニプレータアーム自重トルク演算器２０ｂの出力Ｙ加
算し、スレーブＩ＋４１１自重トルク演算器１０の出力
と−ｆる。また作業工具２が収りつけられていない場合
も、識別情報により作業工具切換器側の出力を切換え１
作業工具自重トルク検出器２０ｂの出力をゼロとし、ス
レーフ゛１則マニフ“レータアーム自重トルク演算器２
０ａのみの出力をスレーブ側自重トルク演券、器Δ〕の
出力とｆる〇 このように作業工具ケ何棟頬も用いろ場合でも、自重ト
ルク演算−「ることで、オペレータ９９マ、各作業工具
が対象物に作用した力のみの力帰還により良好な操縦性
ケ確保できる。

次に本発明の第３の実施例によるマスタスレーブ形マニ
プレータの制御装ａｒｔ第５図に示ｆ０本実施例は１作
猶工具としてマニプレータハンドゲ用いＴこ場合である
。マニプレータハンドル種々のワーク２７ケつかむもの
と−１石。第２図に示ｆ第１の実施例とは異なり、スレ
ーブ側トルク検出５６が不要である。新ｆこに設けられ
たトルク演算器側は、位置調節器１７の出力からスレー
ブ１則マニプレータの関μｉ］４に作用しているトルフ
ケ検出するものであり、荷重トルク演算器２９は、トル
ク演算器側の出力から自重トルク演算器加の出カン減算
てろことで、スレーブ側マニプレータにワークｄにより
作用しているトルフケ算出ｆるものである。

今、スレーブ側マニプレータ１のマニプレータハンドに
よりワークｎがつかまれると、スレーブ側マニプレータ
アームはワーク４の重積により位置が変化する。′ｒる
と２位置−節器１７にはサーボ偏差が生じ、位置偏差ｌ
是正ｆｂ向きに出力が増加ｆ７１）。この増加した位置
調節器１７の出力は、ワーク２７の自ｔによるものなの
で、位置調節器１７　ノ出力から自重トルク演算器側の
出力Ｙ引けば、第１の実施例の荷重トルク演算器２１と
同様の機能ケ得ろことができる。

なお、前述した実施例において、スレーブ側マニプレー
タＩＫ作用し１０トルクとマスク側マニプレータ８に滞
磁されるトルクとの比率ｍ、　ｆなわちバ？涜うル比ケ
大きくして操縦力を軽減′ｆ＝−ことができろ。

マタマニブレータの関節数については１関節型と４関即
型に限らす、Ｎ関節形マニプレータについても通用でき
６゜マニプレータ自重に関与ｆろＮ関節以後の関節に収
りつけ１こ位置検出器の出カン、各自重トルク演算器に
入力して、同様に計算−ｒ−ｔｔ、ハ任意の関節Ｐで発
生ｆろマニプレータ自重によるトルクがわかる。

また、前述した実施例においては、アクチュエータとし
てモータゲ用いたが、他の油圧、空気圧等のアクチュエ
ータでもよい。

〔発明の効果〕

以上の通り、本発明によれば、スレーブ側マニプレータ
に作用した力のみンオペレータに帰還させろことができ
、良好な操縦性ケ得ることができ石。また１作業工具が
対象物に作業を行なう場合以外２作業工具およびマニプ
レータの自重をさサエロカが不要であり、良好な操縦性
が確保できる。

【図面の簡単な説明】

＠１図は従来のマスタスレーブ形マニプレータの制御装
置馨示す図、第２図は本発明の第１の実施例によろマス
タスレーブ形マニプレータのｆｔＩＵ　ａ　装［１のブ
ロック図、第３図は同装置による自重トルク演算のため
の説明図、第４図は本発明の第２の実施例によるマスタ
スレーブ形マニプレータの制？ｉｌ装置のブロック図、
第５図は本発明の第３の実施例によるマスタスレーブ形
マニプレータの制御装置のブロック図である。 １・・・スレーブ側マニプレータ、２・・・作業工具、
３．１１・・・マニプレータアーム、、、　４　、１２
°“°関節・５．１３・・・位置検出器、６．１４・・
・トルク検出器。 ７、．１５・・・モータ％８・・・マスタ側マニプレー
タ、９・・・オペレータ、１０・・・マニプレータハン
ドル、１７・・・位置調節暢、１８．屓・・・ザーボ増
巾器、加。 ２２・・・自重トルク演算器、　２１　、２：（・・・
荷重トルク演算器、２４・・・トルり調節器、２５・・
・自重トルク検出器。

Claims (1)

1. 【特許請求の範囲】 関節部により結合されたマニプレータアームの先端部に
   収りつけられ作業工具により実際の作業ケおこ１ようス
   レーブ側マニプレータと、関節部により結合されたマニ
   プレータアームの先端部に敗・１つつけられたオペレー
   タハンドルによりスレーブ＋１１１１マニプレータｖ操
   ｇｆ；ｂマスク側マニプレータとを備エタマスク・スレ
   ーブ形マニプレータの制御装置において、 …１記スレーブ［１マニプレータの前記関節部の位置に
   基づいて、前記スレーブ側マニプレータの１市によるス
   レーブ１ｄす自車トルク請求め、前記スレーブ側マニプ
   レータの前記関節部に作用ｆ石トルクから減算して、前
   記スレーブ側自重トルク以外の純粋なスレーブ側荷重ト
   ルク請求めるスレーブ側荷重トルク演算手段と。 前記マスク側マニプレータの前記関節部の位１行に基づ
   いて、前記マスク側マニプレータの自軍によるマスク側
   自重トルクを求め、前記マスク側マニプレータの前記関
   節部に作用するトルクから減算して、前記マスク側自重
   トルク以外の純粋なマスタ側荷重トルｉｙｔ求めるマス
   ク側荷重トルク演算手段と、 前記スレーブ側荷重トルク？目標値として前記マスク側
   荷重トルクとの差？演算し、この差に前記マスタ側自電
   ドルクン加算して駆動トルクを求めろ駆動トルク演算手
   段と、 この駆動トルク演算手段により求められた駆動トルクで
   前記マスク測量節部Ｖ駆動ｆる駆動部と２備え。 前記スレーブ側マニプレータに前記作業工具に加、ｔ　
   ラｔｌ−た外力乞、前記マスタ側マニプレータのオペレ
   ータハンドルケ操縦ｆ７−＋ときの外力としてｅｔｉ−
   ｒることケ特徴とｆろマスク・スレーブ形マニプレータ
   の制御装置。