JPS6244382A - マスタ・スレ−ブマニプレ−タの制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〈産業上の利用分野〉 本発明はマスタ・スレーブマニプレータの制御方式に関
し、原子力、宇宙、海洋等の遠隔操作システムにおける
マスタ・スレーブマニプレータの制御全般に適用できる
ものである。

〈従来の技術〉 マニプレータの制御方式上しては、マスタ・スＬ／−プ
方式と、ＲＭＲＣ（Ｒｅ５ｏｌｖｅｄ　ＭｏｔｉｏｎＲ
ａｔｅ　Ｃｏｎｔｒｏｌ：分離速度制御）方式上が艮〈
知らｎている。

第７図に示す６軸マニプレータで最も簡単なマスタ・ス
レーブ制御アルゴリズムは、各軸１〜６のゲインをＫｉ
（ｉ＝ｔ、　２．・０６）、スレーブマニプレータへの
モータ出力電圧（操作指令〕をＶｉとすｎば、次式とな
る。ｌ〜３はアームの軸、４〜６は手首の軸であるｂＶ
ｉ＝（θ對−θ５ｉ）ＸＫ：　　　（ｉ＝１．２．・・
・６）但し、θＭｉはマスタ各軸の角度、θｓｊはスレ
ーブも軸の角度である。第７図（ａ）はマスタマニプレ
ータを示し、各関節には位置検出装置が’ＡＲ８ｎてい
る。７はマスタの手先である。

第７図の）はスレーブマニプレータを示し、各関節には
位置検出装置とモータが装着されている。８はスレーブ
の手先である。

このようなマスタ・スレーブマニプレータによシ、第６
図に示すように、水の入ったコツプ９の運搬作業をする
場合を例として従来の全軸マスタ・スレーブ制御方式を
考えてみる。第６図はスレーブのｅｓｓ軸を実線の角度
から点線への角度へ移動させた時の図である。

この場合オペレータは、θ３ｓ軸のみを操作すれば、第
６図のようにコツプ９の姿勢もかわってしまい水はこぼ
れてしまう。したがってオペレータはコツプ９の水をこ
ぼさないように、手首の姿勢をつねに最初の姿勢と等し
く保ちながらコツプを運搬しなけｎばならない。

この動作では、θｓｓ軸を補正すわばよいことがわかる
。したがってオペレータは、手首の姿勢につねに注意を
払いながら、コツプ９を運搬しなければならない。

これに対して、アーム３軸をマスタ・スレーブ方式で制
御し、手首３軸＠　ＲＭＲＣ方式で制御して、第６図と
同様のコツプ運搬作業を行わせると、アームのθＳｌｐ
θｓ２．θＳ、の各軸の動きに対して、手首の姿勢は常
に最初の姿勢を自動的に保持するように制御される。そ
のため、オペレータは手首の姿勢を全く気にすることな
く、手首の位置換言すればアーム３軸の操作を行うだけ
で、水をこぼすことなくコツプを運搬するこｄができる
。

ＲＭＲＣ方式とは、運動分解行列（ヤコビ逆行列）８用
いて、地球座標系（手首座標系でも良い）での運動をマ
ニプレータの各関節の運動に変換し、マニプレータを駆
動するものである。ｆｔＭＲｃ方式をマニプレータ４こ
適用すれば、アームの動きに対し、手首の姿勢を一定に
保つことが可能である。

ここで第７図を参照してＲＭＲＣ方式について簡単に説
明しておく。

■：マニプレータの手先の位置と方向 とＴると。

Ｋ＝ｆ（（？）　　　　　　　　　　　　　・・・（１
）なる関係がある。この（１）式を両辺微分することに
よ〕（２）式が得られる。

ｒｘ＝、ｒ（ｅ）ｉ　　　　　　　　　　　　−・・（
２）となっておシ、ム、　［Ｂ、　　ｃ、　ＩＤをそｎ
ぞれ（３，３）型の行列とすると と表わすことができる。

（２）式よシＲＭＲＣを用いたマニプレータの制御則は
（４）式で示される。

ｅ＝Ｊ　（Φ斥　　　　　　　　　　・・・（４）（４
）式において良は操作指令であり、アームの前後、左右
、上下動作のためのアーム位置操作指令ＩＸＡ　　と手
首の左右、上下振り及びねじフ動作のための手首方向操
作指令ｉｗであるとなっている。また０は各軸ｌ〜６へ
の回転駆動速度指令であシ、各軸１〜６の回転駆動速度
指令を０１〜θ６　とＴると次式（６）で示される。

またＪ　（Φ）は回転角変換行列であシ（６゜６）型の
行列となっておシ次式（７）で示される。

・・・（７） （７）式において■は（３，３）型の単位行列、Ｐ、　
Ｑｌ、　Ｒ，Ｓは（３，３）型の行列であり次のような
機能を果す。

Ｐ：アーム位置操作指令ＩＸＡ　をアーム軸ｌ。

２．３への主速度指令”ＡＭに変換する行列。

＄二手前方向操作指令ｉｗを手首軸４．５゜６への主速
度指令”ＷＭに、変換する行列。

ＦＬ：アーム位置操作指令ＴＸＡ　に対して手首の軸４
，５．６に補正速度指令ａＷＣを与えるための行列。

Ｑ：手首の方向操作の際にアームの位置がずれることを
防ぐためにアーム駆動軸 １．２．３に補正速度指令ｅＡＣを与えるための行列。

これまでのところをまとめると（４）式は次式（８）こ
こでマニプレータの制御を股間する。操作者がマスタを
操作Ｔるとアーム位置操作指令Ｄ（Ａ　が出力され、こ
の操作指令ＩＸＡ　は、行列Ｐによシ主速度指令ＪＡＭ
に変換されるとともに行列正によ〕補助速度指令ｅＷｃ
に変換される。また手首方向操作指令ｉｗが出力さｎ。

この操作指令ｉｗは、行列Ｓによシ主速度指令ｊｗＭに
変換されるとともに行列Ｑによシ補助速度指令６９ＡＣ
に変換される。スレーブのアーム位ｉ１３軸サーボ系は
、主速度指令”ＡＭと補助速度指令（ｉ’ＡＣとを加え
た指令Φムを受けてこｎを積分することによファーム軸
回転角ΦＡを求める。そしてこのアーム軸回転角のＡを
基にスレーブのアーム軸１，２．３を駆動する。一方ス
レープの手首方向３軸サーボ系は、主速度指令（ｊｖｔ
ｙｘと補助速度指令ｅｗｃ　七を加えた指令ｔＢｗを受
けてこｎを積分することによシ手首軸回転角〜を求める
。そしてこの手首軸回転角ａｗを基にスレーブの手首軸
４，５゜、６を駆動する。

〈発明が解決しようとする問題点〉 本発明は上記従来技術に鑑み、適宜、全軸７　ス１・ス
レーブ方式と、アーム３軸マスタ・スレーブ方式及び手
首３軸ＲＭＲＣ方式の組合方式とを切替えながらマスタ
マニプレータの操作を可能とし、よってマスタースレー
ブマニプレータシステムの高性能化を達成しようとする
ものである。

く問題点を解決するための手段〉 上記目的を達成した本発明による制御方式は、上下、左
右及び前後の動作が可能なアーム３自由度と、上下の振
り、左右の撮り及びねじりの動作が可能な手首３自由度
とを有する２本のマニプレータを用いてマスタースレー
ブ制御を行うに際し、第１図に示す如く、全６軸マスタ
・スレーブ動作の第１モード１００と、アーム３軸はマ
スタ・スレーブ動作２０１とし、手首３軸はスレーブア
ームの動きに拘らず基準座標に対して常に同じ姿勢を保
つ動作２０２を行う第２モード２０〇七の２つの動作モ
ードを切替えながら操作を行うもの心し。

第１モードから第２モードへの切替時には、切替瞬間の
マスタの手首の姿勢をそれ以後のスレーブ手首の姿勢の
目標値として処理３０１し、第２モードから第１モード
への切替時には、マスタの手首の姿勢がスレーブ手首の
姿勢に一致してはじめて第１モードに復帰させる処理３
０２をすることを特徴とする。

く作　　　用〉 第２図（ａ）、　（ｂ）により、コツプ９をＡ点からＢ
Ａまで運搬する作業を例にとって、作用を説明する。以
下、モードｌは全軸マスタースレーブ方式、モード２は
アーム３軸マスターユ、−ブ方式と手首３軸ＲＭＲＣ方
式の組合方式とする。

（１）　　モードｌでオペレータはまずコツプ９をつか
む（第２図（ａ））。

（２）　　コツプ９をつかむ動作終了後、オペレータは
モード２へ切替える。このモード切替のためのスイッチ
は第４図に３０で示す如くマスタ・マニプレータのオペ
レータ操作端３１に設けると扱いやすい。

（３）　　モード２１こて動作が進行する。この時の動
作状況を次に示す（第２図０）参照）。

中　マスタからの指令は、アーム３軸のみ有効で、手首
３軸を無視される。同様に手先の開閉動作も無視される
。

Ｇｉ）　　手首の姿勢は、モードｌからモード２へ切替
った瞬間の姿勢が保持される。

（＋ｉｉ）　　手先の開閉位置は、モードｌからモード
２へ切替った瞬間のマスタの位置が目標値として保持さ
れる。すなわちスレーブ手先の把持力は、モード切替時
の力が保持される。

（ψ　オペレータがマスターマニプレータのアーム３ｍ
を操作し、Ａ点からＢ点に運搬する。この時、手首の姿
勢がかわらないように自動的ｌこ制御される。図の場合
は水平Ｉこ保たｎる。また手先の把持力もそのまま保た
ｎる。

０　コツプ９がＢ点に運ばｎ１オペレータはモード２よ
りモードｌへ切替える。この時、モード２よりモードｌ
へ切替わった時、マスタ側とスレーブ側では、手首の姿
勢が大きく異なっている可能性がある。したがって、モ
ード２からモードｌへ切替直後いきなフモード１すなわ
ち全軸マスタ・スレーブ動作が開始すわば、スレーブの
手首の姿勢がマスタの手首の姿勢となるため、両者の姿
勢が異っていわば急にスレーブの手首の姿勢が変化し・
　てコツプ９の水をこげずことになる。いいかえれば、
とｎまで手首の姿勢を一定に保ってきたことが無駄にな
る。

そこで、モード２よシモードｌへ切替 った直後は、いきなり全軸マスタ・スレーブ動作とせプ
′、スレーブの手首の姿勢はそのまま保だ、ｎ、、オペ
レータがマスタの手首の姿勢を操作してマスタの手首の
姿勢がスレーブの手首の姿勢ｌこ一致して始めて、全軸
マスタ・スレーブ動作が開始するようにする。これｌこ
より、手首姿勢がスムーズｌこ継がる。

（４）　　再び（１）ｔこもどる。即ち上記（υ→（２
）→（３）→（４）→（１）・・拳の一連の動作を必要
に応じてくり返Ｔ０ く実　施　例〉 第３図〜第５図により、本発明方式の実施例を説明Ｔる
。

第３図は本発明制御方式を実現する装置のブロック図で
あり、第４図はモードスイッチ３０の設置例を示し、第
５図はフローチャートである。動作を以下に述べる。な
お、第３図中、二点鎖線内は電子計算機（ＣＰＵ）を用
いて処理が行わｎている。第３図と第５図で同一符号部
分は同一機能を示す。第５図中、ステップ１０．１１に
よりマニプレータの各関節角の入力および手先の位置の
ａｒ算を行い、ステップ１２で制御に必要な座標変換行
列、運動分解行列の各要素の演：Ｊｌ：を、各関節角の
値をもとに求める。ステップ１３はアーム３軸（位置３
自由度）の位置制御ブロックである。ここまでは、アー
ムの位置の制御であるので、モードｌ、モード２のどち
らの場合も同一演算である。このモードの切替は、マス
タ側の手元のモードスイッチ３０等により行い、たとえ
ば右に倒した時は、読込み値がモード１％左に倒した時
はモード２となるようになっている。

モードスイッチ３０が最初モードｌ即ち、全軸マスタ・
スレーブ動作の時は、１５→１９→２１→２４→２５の
手順となり、マスタとスレーブの手首の姿勢が一致する
まで、手′首は動きださない。　この時は、手首３軸Ｒ
ＭＲＣモードである。　そして、オペレータはマスタの
手首を操作し、マスタとスレーブの手首の姿勢が一致し
てはじめて１９→２１→２３となシ、次のサンプリング
以降は、１９→２３となるため全軸マスタ・スレーブ動
作モードとなる。

モードスイッチ３０をモードｌからモード２に切替えた
直後のサンプリン・グでは、１４→１７→２２→２４→
２５の手順となり、手首ｆ１ＭＲｃのために必要な初期
値をとり込む。

その次のサンプリングからは１４→１８→２４→２５と
なり、手首１（Ｍ　Ｒｅモードとなる。２２でマスタか
らの情報をとり込むが、そｎ以降２２は通過しないので
マスタの手首情報は一切無視さｎる。また２４の演算で
は、実際のスレーブ・マニプレータの移動量ΔＸｓＡを
基に補正ｉを計算する。これは本来、マスタ・スレーブ
システムであるからマスタ・マニプレータの移動量を基
に行っても数学的ｌこはかわらないが、マスタの情報を
用いるより、実際のスレーブの動きに基づき演算するほ
うがより積度が向上するためである。

ＲＭ　ＲＣ動作終了後、モードスイッチ３０を切替えた
直後のサンプリングでは、１４→１６→２０→２１→２
４の手順となり、マスタとスレーブの手首の姿勢が一致
するまで手首は動き出さない。またモード切替後も、１
５→１９→２１→２４となシ、同様に手首は働かない。

いいかえれば１手首３軸ＲＭ　Ｒｅモードのままである
。そして姿勢が一致した時のサンプリングでは１５→１
９→２１→２３の手順ごな９、以降は１５→１９→２３
となシ全軸マスタ・スレーブのモード１が再び開始する
。

ここで第り図の制御フローチャートの説明をして２〈。

ゴ　は地球座標系（鉛直上下、水平左右・前後）から手
首座標系（手首に注目した前後・左右・上下）への３Ｘ
３行列の変換行列、Ａ　は運動分解行列Ｊｒ　　（＃）
のうち、アーム３軸の動きを各関節角の動きに変換する
部分３×３行列、 −ＩＤ　　（１”Ａ　　は運動分解行列、！’　　ＣＩ
）のうち、アームの動きに対してつねに手首の姿勢を一
定に保つための手首姿勢補正量を求める部分３Ｘ３行列
。

ＫＡはスレーブマニプレータのアーム３＠の位置制御ゲ
インの３×３行列、 ＩＫｗはスレーブマニプレータの手首３軸（上下、左右
のふり、ねじり）の位置制御ゲインの３Ｘ３行列、 ＫＨはスレーブマニプレータの手先開閉のための位置制
御ゲイン。

■ＳＡはスレーブマニプレータの１サンプリング間の地
球座標系におけるアーム位置移動量、 ΔＴはサンプリングタイム１ 、０口はマスタの手首３軸の各関節角で、ｅｓｗはスレ
ーブの手首３軸の各関節角で、＃５Ｗｒｅｆ　　はスレ
ーブの手首３軸の各関節角θＭＨはマスタ手先の開閉位
置、 θＳＲはスレーブ手先の開閉位置、 θ５Ｈｒｅｆ　　はスレーブ手先の開閉位置目標値。

モードＳＷ　＝１は全軸マスタ・スレーブ動作モード、 モード５Ｗ＝＝２はアームがマスタ・スレーブで手首が
ＲＭＲＣの動作モード、 Ｌはマスタとスレーブの手首の角度が一致したかどうか
を判定するための閾値である。

〈発明の効果〉 勢がアームの動きに対してそのまま自動的に保持ざｎる
モードと、全軸マスタ−スレーブモード８適時切替えな
がら作業を行うことができるのでオペレータの疲労軽減
に貢献する。

第：

【図面の簡単な説明】

第１図は本発明による制御方式を示すフローチャート、
第２図（ａ）、　（ｂ）はコツプ運搬を例としたその作
用の説明図、第３図は本発明を実現する装置例のブロッ
ク図、第４図はモードスイッチ設置例の説明図、第５図
は第３図に示すもの゛のフローチャート、第６図は単な
る全軸マスタ・スレーブ方式によるコツプ運搬作業の説
明図、第７図（ａ）　、　（ｂ）は一般的なマスタ・ス
レーブマニプレータシステムの説明図である。 図面中、１〜３はアームの３軸、４〜６は手首の３軸、
１００は全軸マスタ・スレーブ動作、２００はアーム３
軸マスタ・スレーブ動作と手首３軸ＲＭ　）ｔｃ動作の
組合せ、３０１はマスタ・スレーブ→ＲＭＲＣモード変
更に伴う処理、３ｏ２はＲＭＲＣ→マスタースレーブモ
ード変更に伴う処理である。

Claims (1)

1. 【特許請求の範囲】 上下、左右及び前後の動作が可能なアーム３自由度と、
   上下の振り、左右の振り及びねじりの動作が可能な手首
   ３自由度とを有する２本のマニプレータを用いてマスタ
   ・スレーブ制御を行うに際し、 全６軸マスタ・スレーブ動作の第１モードと、アーム３
   軸はマスタ・スレーブ動作とし、手首３軸はスレーブア
   ームの動きに拘らず基準座標に対して常に同じ姿勢を保
   つ動作を行う第２モードとの２つの動作モードを切替え
   ながら操作を行うものとし、 第１モードから第２モードへの切替時には、切替瞬間の
   マスタの手首の姿勢をそれ以後のスレーブ手首の姿勢の
   目標値とし、第２モードから第１モードへの切替時には
   、マスタの手首の姿勢がスレーブ手首の姿勢に一致して
   はじめて第１モードに復帰することを特徴とするマスタ
   ・スレーブマニプレータの制御方式。