JP2635776B2 - マスタ・スレーブ形マニピュレータ - Google Patents

Description

【発明の詳細な説明】 〔発明の目的〕 （産業上の利用分野） 本発明は、スレーブアームを所望の方向に動作させる
ことのできるマスタ・スレーブ形マニピュレータに関す
る。

（従来の技術） マスタ・スレーブ形マニピュレータは、対象物に対し
て行動するマニピュレータ（スレーブアーム）と、その
動きを指示するマニピュレータ（マスタアーム）とで構
成されるミニピュレータ系で、マスタアームとスレーブ
アームは、通常複数の軸を有するが、寸法は異なってい
ても構造・機構は同一である。すなわち、スレーブアー
ムは操作者が動かすマスタアームと同一の姿勢をとりな
がら（同期的に）駆動される。

第３図は、このマスタ・スレーブ形マニピュレータの
うち、力帰還形バイラテラル・サーボ・マニピュレータ
のマスタアームとスレーブアームの各軸についての自動
制御系を示すブロック線図である。

このマスタ・スレーブ形マニピュレータは、以下の10
個のブロックと２つの加算点を有する。すなわち、マス
タアーム軸１とこのマスタアーム軸１に対応するスレー
ブアーム軸２をそれぞれ駆動させるサーボモータ3,4、
サーボモータ3,4の駆動に係る信号を増幅する増幅器5,
6、マスタアーム軸１とスレーブアーム軸２のそれぞれ
に働くトルクを検出するマスタアームトルク検出器（以
下「Ｍトルク検出器」という）７とスレーブアームトル
ク検出器（以下「Ｓトルク検出器」という）８、マスタ
アーム軸１とスレーブアーム軸２の位置（アーム角度）
を検出するマスタアーム位置検出器（以下「Ｍ位置検出
器」という）９とスレーブアーム位置検出器（以下「Ｓ
位置検出器」という）10、マスタアーム軸１とスレーブ
アーム軸２のアーム角度が同じになるようにスレーブア
ーム軸２のアーム角度を調整する位置調整器11、マスタ
アーム軸１とスレーブアーム軸２のトルクが同じになる
ようにマスタアーム軸１のトルクを調整するトルク調整
器12、および位置の加算器13とトルクの加算器14であ
る。

Ｍ位置検出器９とＳ位置検出器10は、それぞれＭトル
ク検出器７とＳトルク検出器８を介してマスタアーム軸
１とスレーブアーム軸２に接続し、また位置の加算器13
と接続する。位置調整器11とトルク調整器12は、それぞ
れ位置の加算器13と増幅器６、およびトルクの加算器14
と増幅器５に接続する。サーボモータ３と４は、それぞ
れマスタアーム軸１と増幅器５、およびスレーブアーム
軸２と増幅器６に接続する。

この自動制御系において、マスタアーム軸１とスレー
ブアーム軸２のトルクと位置に関する信号は、それぞれ
Ｍトルク検出器７とＳトルク検出器８、およびＭ位置検
出器９とＳ位置検出器10に送られる。Ｍ位置検出器９と
Ｓ位置検出器10はこの信号を受けると、それぞれマスタ
アーム軸１とスレーブアーム軸２の位置を検出してその
検出値を位置の加算器13へ送る。加算器13は両位置検出
器9,10が検出した値から偏差ε_Θを算出し、位置調整器
11へ送る。位置調整器11はマスタアーム軸１とスレーブ
アーム軸２の間に位置の偏差があると、この偏差を解消
すべき旨増幅器６へ位置偏差解消指令を送る。増幅器６
はこの位置偏差解消指令を受けると、これを増幅してサ
ーボモータ４へ送る。サーボモータ４は、増幅された位
置偏差解消指令を受けると、スレーブアーム軸２をマス
タアーム軸１と同じアーム角度をとるように駆動させ
る。

他方、Ｍトルク検出器７とＳトルク検出器８は上述の
トルクに関する信号を受けると、それぞれマスタアーム
軸１とスレーブアーム軸２のトルクを検出してその検出
値をトルクの加算器14へ送る。加算器14は両トルク検出
器7,8が検出した値から偏差ε_Ｔを算出し、トルク調整
器12へ送る。トルク調整器12はマスタアーム軸１とスレ
ーブアーム軸２の間にトルクの偏差があると、この偏差
を解消すべき旨増幅器５へトルク偏差解消指令を送る。
増幅器６はこのトルク偏差解消指令を受けると、これを
増幅してサーボモータ３へ送る。サーボモータ３は、増
幅されたトルク偏差解消指令を受けると、マスタアーム
軸１にスレーブアーム軸２と同じトルクを与える。する
とマスタアーム軸１を握っている操作者は、スレーブア
ーム軸２に働くトルクの大きさを感じとることができ
る。

（発明が解決しようとする課題） ところがこのようなマスタ・スレーブ形マニピュレー
タは、スレーブアーム全体を最終的に所望の地点に移動
させて、作業をさせることはできるが、多軸・多関節と
いう制約から、マスタアームを手動で動かす操作者が、
スレーブアームの最終地点に至る道筋までを制御するの
はむずかしい。このため、例えば棒状の物体をスレーブ
アームで把持し、これを環または穴へ挿入する作業で
は、スレーブアームを環または穴の入口から奥まで直線
的に移動させることができず、棒状の物体が環または穴
の端に衝突して破損事故が生ずるおそれがあった。

本発明は上記事情に鑑みてなされたもので、スレーブ
アーム軸の移動の道筋をコントロールすることのできる
マスタ・スレーブ形マニピュレータを提供することを目
的とする。

〔発明の構成〕

（課題を解決するための手段） 本発明は上記課題を解決するために、対象物に対して
行動するスレーブアームと、このスレーブアームの動き
を指示するマスタアームを備えるマスタ・スレーブ形マ
ニピュレータにおいて、マスタアームに被拘束点を設定
する手段と、この被拘束点の動作方向を設定する手段
と、上記被拘束点が上記設定された動作方向に動作する
ようにマスタアームを駆動する手段とを有することを特
徴とするマスタ・スレーブ形マニピュレータを提供す
る。

（作用） 本発明のマスタ・スレーブ形マニピュレータは、マス
タアームに被拘束点を設定する手段と、この被拘束点の
動作方向を設定する手段と、上記被拘束点が上記設定さ
れた動作方向に動作するようにマスタアームを駆動する
手段とを有する。このため、スレーブアームにおいて移
動の道筋をコントロールしたい箇所がある場合は、マス
タアーム上のこれに対応する箇所を被拘束点として設定
する。そしてスレーブアームについてコントロールした
い道筋を、この被拘束点の動作方向として設定する。こ
の後は、被拘束点がこの設定された動作方向に動作する
ようマスタアームを駆動すれば、スレーブアームの移動
の道筋を制御することができる。

（実施例） 以下第１図と第２図を参照して本発明の実施例を説明
する。

第１図は、本発明のマスタ・スレーブ形マニピュレー
タのマスタアームとスレーブアームの各軸についての自
動制御系を示すブロック線図である。この制御系のうち
第３図と対応する箇所には同一の符号を付して、その説
明は省略する。

本発明のマスタ・スレーブ形マニピュレータの自動制
御系は、第３図に示した従来のそれに加え、さらにマス
タアーム上に被拘束点を設定する手段としての被拘束点
設定器20と、被拘束点位置算出器21、被拘束点の動作方
向を設定する手段としての拘束条件設定器22、拘束選択
スイッチ23およびトルク調整器12とともに被拘束点を拘
束条件設定器22で設定された動作方向に動作するように
マスタアームを駆動する手段としての拘束位置の加算器
24を備える。

被拘束点位置算出器21は、被拘束点設定器20と拘束位
置の加算器24、およびＭ位置検出器９と接続する。拘束
位置の加算器24は、さらに拘束条件設定器22および拘束
選択スイッチ23と接続し、拘束選択スイッチ23はその上
トルク調整器２と接続する。

スレーブアームについて移動の道筋を制御したい箇所
がある場合は、これに対応するマスタアーム上の箇所
（被拘束点）を被拘束点設定器20で設定し、その被拘束
点を被拘束点位置算出器21に知らせる。被拘束点位置算
出器21は、被拘束点について、この被拘束点が設けられ
たマスタアーム軸１に接続したＭ位置検出器９から送ら
れる位置に関する信号を基にその位置を算出し、その値
を拘束位置の加算器24へ送る。拘束位置の加算器24は、
被拘束点の現在位置について拘束条件設定器22から送ら
れた拘束条件との偏差を算出し、トルク調整器12へ送
る。

ところで拘束位置の加算器24とトルク調整器12の間に
は拘束選択スイッチ23があるため、スレーブアームの道
筋を制御する必要があるときだけ、この拘束選択スイッ
チ23を投入し、拘束位置の加算器24で算出された偏差を
トルク調整器12へ送る。するとトルク調整器12は、マス
タアームとスレーブアームのトルク偏差、および被拘束
点の位置の拘束条件との偏差を解消するトルク値を算出
して増幅器５へ送る。すると増幅器５は、このトルク値
を増幅してサーボモータ３へ送る。サーボモータ３はこ
の増幅されたトルク値に従ってマスタアーム軸１を駆動
させる。このようにして、マスタアームと同期的に移動
するスレーブアームについて、被拘束点と対応する箇所
の動きを制御し、直線や平面に沿った動作をさせること
ができる。

第２図は、７つの軸と６つの関節25,26,27,28,29,30
を有するマスタアーム31について、その軸の先端（手首
32の根元）に設けられた被拘束点Ｐ（座標は（p_x,p_y,
p_z））の移動の様子を示す。以下第１図と第２図を参照
して被拘束点Ｐの設定からこの移動までの手順を示す。

この実施例においては、被拘束点Ｐを、点Ｑ（座標は
（q_x,q_y,q_z））と法線ベクトルNpで表される平面33（点
Ｑを通って法線ベクトルNpに垂直）に沿って移動させ、
点Ｑに到達させることを目的とする。

この場合は、第１図に示した被拘束点設定器20で被拘
束点Ｐを定め、非拘束点位置算出器21でその位置を算出
する。そして拘束条件設定器22で拘束条件（平面33）を
定めた後、拘束位置の加算器24で、非拘束点Ｐと平面33
の間の距離（偏差）を算出する。偏差が求まったらこれ
を拘束選択スイッチ23を介してトルク調整器12へ送る。

トルク調整器12では、まずこの偏差に比例する力Ｆ＝
ｋ（Ｐ−Ｑ）×Np（ｋは比例定数）を求める。そしてこ
の力Ｆをさらに各関節ごとに、式T_i＝（L_i×Ｆ）×e
_i（ｉは６個の関節の番号（ｉ＝１〜６）、T_iは各関節
のトルク、L_iは各関節から非拘束点Ｐまでの距離、e_iは
各関節の回転方向を示す単位ベクトル）に従ってトルク
T_iに算出し直し、増幅器５を介して各軸（関節）を駆動
するサーボモータ３に入力する。

こうしてトルクT_iを各関節に発生させることにより、
非拘束点Ｐに拘束力Ｆを働かせ、前述の位置調整機構に
よって非拘束点Ｐに対応するスレーブアームの手先を、
平面33に対応する平面に沿って、点Ｑに対応する点に到
達させることができる。

〔発明の効果〕

以上説明したように、本発明のマスタ・スレーブ形マ
ニピュレータは、マスタアームに被拘束点を設定する手
段と、この被拘束点の動作方向を設定する手段と、上記
被拘束点が上記設定された動作方向に動作するようにマ
スタアームを駆動する手段とを有する。このため、スレ
ーブアーム軸に移動の道筋をコントロールしたい箇所が
ある場合は、マスタアーム軸のこれに対応する箇所を被
拘束点として設定する。そしてスレーブアーム軸につい
てコントロールしたい道筋を、この被拘束点の動作方向
として設定する。この後は、被拘束点がこの設定された
動作方向に動作するようマスタアーム軸を駆動すれば、
スレーブアーム軸の移動の道筋を制御することができ
る。

【図面の簡単な説明】

第１図は本発明の一実施例に係るマスタ・スレーブ形マ
ニピュレータの自動制御系を示すブロック線図、第２図
は被拘束点の移動の様子を示す模式図、第３図は従来の
マスタ・スレーブ形マニピュレータの自動制御系を示す
ブロック線図である。 12……トルク調整器、20……被拘束点設定器、22……拘
束条件設定器22。

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】対象物に対して行動するスレーブアーム
   と、このスレーブアームの動きを指示するマスタアーム
   を備えるマスタ・スレーブ形マニピュレータにおいて、
   マスタアームに被拘束点を設定する手段と、この被拘束
   点の動作方向を設定する手段と、上記被拘束点が上記設
   定された動作方向に動作するようにマスタアームを駆動
   する手段とを有することを特徴とするマスタ・スレーブ
   形マニピュレータ。