JPH03104572A - マスタスレーブマニピユレータの制御方法及び装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は，マスタスレーブマニビュレー夕の制御方法及
び装置に関するものである。

〔従来の技術〕

マスタスレーブマニピュレータは、オペレータがマスタ
マニピュレータを操作すると、スレーブマニピュレータ
がそれに追随して動き、その動きによって物体の移動や
加工などを行うもので、原子力プラントなどの危険な場
所での作業に適した装置である． 従来のマスタスレーブマニピュレータには、スレーブマ
ニピュレータが単純にマスタマニピュレータに追随する
だけのものの他に、操作支援機構または装置を設け、マ
スタマニピュレータの動きと合わせてその動きとは無関
係な特定の動きをも行えるようにしたものがある．例え
ば特開昭５８−１３２４７１号に示されたものでは、ス
レーブマニピュレータの先端をマスタマニピュレータの
移動速度や方向に追随させずに、所定の速度及び方向に
駆動してグラインダ作業を行わせる．このように特定の
動作を別の機構で行えば、例えば直線に沿っての研磨、
溶接等の作業を正確に行え，操作性、作業性の向上がは
かれる． 〔発明が解決しようとするＲ題〕 マスタスレーブマニピュレータの操作を行うときには、
スレーブマニピュレータがその周囲にある物体に衝突や
接触等の干渉を起こさないようにする必要がある．しか
し，スレーブマニピュレータの手首（先端部に近い部分
）については、マスタマニピュレータの同部分と同様に
その動きを容易に判断できるが，先端から離れたアーム
部の動きを知るのは困難である．従来の操作支援機構は
このような干渉を回避する機能を持っていないので，障
害物がスレーブマニピュレータの周辺に存在する場合に
は操作性が良くないという問題があった。

本発明の目的は、スレーブマニピュレータと障害物が干
渉するのを効率よく回避できるようにするための、マス
タスレーブマニピュレータの制御方法及び装置を提供す
るにある． 〔課題を解決するための手段〕 上記の目的は、障害物が存在するときにその位置を検出
する機能を持った操作支援装置を設け、マスタマニピュ
レータの動きに応じたスレーブマニヒュレータの動作時
に，スレーブマニピュレータの手首の動きについてはオ
ペレータが目視により障害物回避を行うようにマスタマ
ニピュレータを操作してこれにスレーブマニピュレータ
の手首を追随させ、スレーブマニピュレータのアーム部
の動きについては、マスタマニピュレータの動きあるい
は操作支援装置により求めた動きをしたときの障害物と
の干渉の有無を操作支援装置により判定し、もし干渉す
ると判定したときには，操作支援装置によって上記マス
タマニピュレータの動きあるいは操作支援装置により求
めた動きを修正した動きをするように上記アーム部を制
御することにより達威される。

〔作用〕

オペレータが干渉の予測とその回避動作が困難なアーム
部は，操作支援装置により干渉回避の制御が行われるか
ら，オペレータの操作の負担が大幅に減り，操作性が向
上する．操作支援装置による障害物の検出は，マニピュ
レータの作業環境を予め記憶しておく方法、作業場に設
けたカメラやアーム部に設けた近接センサ出力を処理し
て判断する等の種々の方法で可能である。

〔実施例〕

以下、本発明を実施例により説明する。第１図は本発明
の第一の実施例を示すもので，マスタマニピュレータ（
以下単にマスクと呼ぶ）及びスレーブマニピュレータ（
以下単にスレーブと呼ぶ）はともに８自由度を持ち、ま
た操作支援装置は予め作成された作業環境の三次元情報
によってスレーブのアーム部衝突回避制御を行う例であ
る．第１図において、マスタ１は手首１１ｍの３自由度
とエンドエフェクタ１０ｍ（対象物の移動、加工，ボル
ト取付等の作業を施す部分）の工自由度及びアーム部１
２ｍの４自由度を有し、架台２ｏより伸びている．マス
タ１の架台２０の根元側より順に関節をＪｍｌ〜Ｊｍ４
（アーム部）　、Ｊｍ５〜Ｊｍ７（手首〉とし，その関
節角をそれぞれθｍ１〜θｍ７とする。各関節Ｊｍｉ　
　（ｉ＝１〜７）とエンドエフェクタ１０ｍは関節を回
転させるモータ及び回転角を検出するエンコーダから成
る。関節間はリンクで連結されている。オペレータ３は
マスタ１のエンドエフェクタ１０ｍを把持して、マスタ
１を操作する。

スレープ２は手首ＩＩｓの３自由度とエンドエフェクタ
ＬＯｓのｌ自由度及びアーム部１２ｓの４自由度を有し
，架台２１より伸びている。スレーブ２の架台２ｌの根
元側より順に関節をＪｓｌ”Ｊｓ７とし、その関節角を
θｓｌ〜θＳ７とする。各関節Ｊｓｉ（ｉ＝１〜７）と
エンドエフエクタ１０ｇは関節を回転させるモータ及び
回転角を検出するエンコーダから成る．関節間はリンク
で連結されている。一般にスレーブ２のアーム部１２ｓ
と手首１１ｓの自由度の和が６あれば、スレーブ２のエ
ンドエフェクタ１０ｇの位置及び姿勢を任意に選択でき
るが，ここでは，スレーブ２が７自由度（エンドエフェ
クタ１０ｓの自由度を除く）あり，冗長自由度を有する
構造となっている。エンドエフエクタ１０ｓは，ボルト
の締緩作業を行うインパクトレンチ形，平行グリッパ形
、多指ハンド形等のエンドエフエクタであり、目的に応
じて手首１１ｓに取付けられて作業対象物６に対する作
業を行う。障害物７は作業環境に存在する配管等であり
，スレーブ２の動作範囲，姿勢を制限するものである。

マスタスレーブ制御装置４はマスタ信号８によりマスタ
１と、スレーブ信号９によりスレーブ２と接続されてい
る。

第２図はマスタスレーブ制御装置４の入出力信号の説明
図で、マスタ１の各関節の回転角θｍｉ（ｉ＝１〜８．
ここでｉ＝８はエンドエフエクタ１０ｓの回転角を意味
するとする。また以下ではこの（ｉ＝１〜７），（ｉ＝
１〜８）等の記述を、特に指定しなくても意味が明確な
ときは省略する）を信号８として入力し、各関節へのモ
ータ電流■ｍｉを出力する。またスレーブ２の各関節の
回転角θｓｉを信号９として入力し、各関節へのモータ
電流Ｉｓｉを出力する。また、操作支援装置から出力さ
れるマスク目標関節角θｒｉをｉ．．Ｂする。

第３図は操作支援装置５の構或を示すもので、環境形威
装置１３，干渉判定装置１４及び座標変換装置１５より
或る。環境形或装置ｌ３はＣＡＤデータから作成された
作業環境の三次元情報Ｐ、例えば障害物７の三次元情報
が蓄えられている．干渉判定装置１４は、上記の三次元
情報Ｐと座標変換装置ｌ５からの目標関節角θｃｉとか
ら干渉の有無を調べ、その結果を判定信号Ａとして出力
する．座標変換装置１５は，マスタｌ及びスレーブ２の
関節角θｍｉ及びθｓｉを入力し、スレーブ目標関節角
θｒｉを出力する． 次に本実施例の動作を説明する。オペレータ３はマスタ
１のエンドエフェクタ１０ｍを操作し，スレーブ２を動
かして目的の作業を行うが，この作業は通常スレーブ２
の手先部を観察しながらマスタ１を操作する。このため
スレーブ２の手先部の障害部７への衝突回避はオペレー
タ操作で容易に行える。そこでこの手先部としてのスレ
ーブ２の手首１１ｓとエンドエフェクタ１０ｓをマスタ
スレーブ方式テ制御し、オペレータによる衝突回避動作
の難しいアーム部１２ｓを操作支援装置５を用いて制御
するものとする。

マスタスレーブ制御装置４による上記手先部の制御は、
手首１１ｍの関節角θｍ５〜θｍ７と手首ＩＩｓの関節
角θＳ５〜θＳ７とを入力して，その差に比例した電流
Ｉｓｉを出力する； Ｉｓｉ＝Ｋｉ　（θｍｉ一θＳｉ）・・・（ｌ〉（ｉ＝
５．６．７） Ｋｉ：比例定数 またエンドエフエクタ１０ｓがハインパクトレンチ形の
ときはエンドエフエクタ１０ｍの操作によってオンまた
はオフし、エンドエフエクタ１０ｓが平行形グリッパの
場合は式（１）に従ってマスタスレーブ制御する． 一方、アーム部１２ｓは次のように制御される。

座標変換装置ｌ５はスレーブ２の関節角Ｏｓｉとマスタ
｛の関節角θｍｉとが入力されると、まず最初に干渉判
定装［１４にマスクの関節角Ｑｍｉを目標関節角Ｏｃｉ
として出力する。干渉判定装［１１４は，環境形或装置
ｌ３から障害物７の位置等の作業環境の三次元情報を取
り込み，これと座標変換劉置１５からの目標関節角θｃ
ｉの値から，スレーブ２のアーム部１２ｓが障害物７に
衝突するかどうかを判定し、その結果を座標変換装置１
５に判定信号Ａとして出力する．座標変換装置１５は，
判定信号Ａからアーム部１２ｇと作業環境が干渉しない
ことが分かると、干渉判定装置ｌ４に出力した目標関節
角Ｏｃｉをスレーブ目標関節角θｒｉとしてマろタスレ
ーブ制御装ｔ！４に出力する．また、座標変換装置１５
は，判定信号Ａから干渉することが分かると、目標関節
角θｃｉを次のように変更する。

即ちアーム部１２ｍ及びアーム部１２ｓの自由度はとも
に４なので、マスタ｛のアーム部１２ｍの先端に相当す
る位置（姿勢は含まず）にスレーブ２のアーム部１２ｓ
の先端を移動させる目標関節角θｃｉは一意には決まら
ない。このため例えばスレーブ２の関節角θｓｉとの差
の二乗和； Ｓ＝Σ（θｃｉ−θＳｉ）２・・・（２）が最小となる
ように目標関節角θｃｉを定める．こうして定めた値θ
ｃｉを再び干渉判定装置ｌ４に送って干渉の有無を調べ
、干渉するならば、そのときの目標関節角θｃｉを、障
害物７に衝突しない方向に少しづつ変更して干渉の有無
を調べるという動作を繰り返す。こうして干渉のないこ
とが確認されたときの値θｃｉが目標関節角θｒｉとし
てマスタスレーブ制御装置４へ出力される。マスタスレ
ーブ制御装置４は次式に従ってスレーブ２に電流Ｉｓｉ
を出力し，関節Ｊｓｉを制御する；Ｉｓｉ＝Ｋｉ（θｒ
ｉ−θＳｉ）・・・（３）（ｉ−１〜４） Ｋｉ：比例定数 以上の動作においては，マスタスレーブ制御装置４から
マスタ１へ出力される電流Ｉｍｉは全てＯであるが、マ
スタｌの自由度が６の場合にはマスタ１のアーム部１２
ｍの１つの関節をある一定の関節角に保持するために，
その関節Ｊｍｊのみに電流Ｉ　ｍ　ｊを流す（ｊは１つ
の固定値）．第１の実施例においては、オペレータ３は
手先の作業だけ意識してマスタ１を操作すれば、スレー
ブ２のアーム部１２ｓが障害物７に衝突することなく作
業が行える。

第４図は本発明の第２の実施例を示すもので、第１図の
場合と異なるのは，マスタ１が手首１１ｍの３自由度と
エンドエフェクタ１０ｍの１自由度の４自由度しか持た
ず、アーム部かない点と，アクセル１６が設けられてい
る点である。このマスタ１の関節を根元側よりＪ　ｍ　
１　＝　Ｊ　ｍ　３、その関節角をθｍ｛〜θｍ３とす
る。オペレータ３はマスタ１のエンドエフェクタｌｏｍ
を把持して，マスタ１を操作する。一方、スレーブ２は
第１図と同じ８自由度のもので、エンドエフエクタ１０
ｓには物体を把持できる平行グリッパ形が装着されてい
る。

アクセルｌ６は、ペダルとその回転角を検出するための
エンコーダとから或り，その検出信号であるアクセル信
号１７はマスタスレーブ制御装置４へ入力される。他の
部分は第工図と同じ構或であるが、マスタスレーブ制御
装置４及び操作支援装置５の動作は，後述するように第
１図の場合と同一ではない。

次に動作を説明する．マスタ１の手首１１ｍの動作に応
じて，マスタスレーブ制御装Ｗ４がスレーブ２の手首Ｉ
Ｉｓをマスタスレーブ制御する。この制御は、第１図の
場合と同様に、マスク↓の手首１１ｍの関節角をθｍｌ
〜θｍ３とスレーブ手首ＩＩｓの関節角θｓ５〜θｓ７
との各々の差に比例した電流で関節Ｊｍｌ〜Ｊｍ３を駆
動する。エンドエフェクタ１０ｓの制御も同様である． スレーブ２のアーム部１２ｓの制御は以下の通りである
．本実施例では、マスタ１にアーム部がないから，スレ
ーブ２のアーム部の最初の目標関節角θｓ１〜θｓ４を
マスタｌのアーム部から得ることはできない．そこで操
作支援装置５は、スレーブ２の関節角θｓｉを入力して
、まずスレーブ２の手首１１ｓの方向ベクトルｅを次式
で求める．８＝Ｃ，Ｃ，Ｃ，Ｃ４Ｃ，Ｃ．Ｃｌｅ７・−
　−　（４）Ｃ，；スレーブ２のｉ番目の関節の４×４
の座標変換マトリクスの中の始めの３×３マトリクス ｅ７；第７関節の基準姿勢での関節中心軸のべクトル＝
　（０，０．１） ただし基準姿勢というのは、第５図に示したように、ス
レーブ２がまっすぐ直立した姿勢である。

ここで，第６図に示すＸ軸，ｙ軸，Ｚ軸回りの，関節の
３Ｘ３７トリクスをｃｘ（θ），ｃＹ（θ），ＣＺ（θ
）とすると であり、これらから式（４）は次となる；ｃ，＝ｃ２（
θｓ１）＋　Ｃｔ＝Ｃｙ　（θｓ２），Ｃ．＝ｃＹ（θ
ｓ　３　）　＋　ＣＩ　＝　Ｃｙ　（θｓ４），ｃｓ”
ｃｘ（θｓ　５）　，　Ｃ．＝Ｃｙ　（θｓ６），Ｃ７
＝０２（θｓ７）， 座標変換装置１５は、目標関節角θｒｉを計算するため
に以下の動作をする．スレーブ２の目標速度Ｖをアクセ
ル信号１７の値Ｋｖを用いてｖ＝ＫＶ　０　ｅ　　　　
　　　　　　０　Ｉ　−（８）により求める。ＫｖがＯ
のときスレーブ２は手首ＩＩｓだけ動き、アーム部１２
ｓは動かない。この速度でスレーブ２の手先を動かすた
めに、スレーブ２の関節Ｊｓｌ〜Ｊｓ４の関節角速度θ
ｓｉ（ｉ＝１〜４）の一般解を次式で求める； ７　ｓ　＝　Ｊ＋・マ＋（Ｉ−Ｊ”Ｊ）言・・・（９）
ただし Ｊ：ヤコビ行列（ｖ＝Ｊ・θｓｉ） Ｊ”；　Ｊの疑似逆行列 Ｉ；単位行列 Ｕ；任意ベクトル １ｓ＝（θｓｌ，θｓ２，θｓ３，θｓ４）７ｓは一意
には決まらないのでｕ７ｓｎ　　（７ｓのユークリッド
ノルム）を最小にする解を１０＝（θｏｌ，　　・・・
θｏ４）とし，目標関節角θＣを次式で計算する； １ｃ＝（θｃｌ，θｃ２，θｃ３，θｃ４）＝’７ｓ＋
ｔ　”７ｏ　　　　　６　６　−（ｔｏ）ただしては操
作支援装置５のサンプリング時間である， 第１の実施例においては、マスタｌのアーム部より入力
される関節角θｍ１〜θｍ４が最初の目標関節角１ｃと
して干渉判定装置へ入カされたが、本実施例では、式（
ｉｏ）の７０がこれに相当する値である。干渉判定装置
ｌ４は座標変換装置１５で計算された目標関節角θｃｉ
と環境形成装置１３がらの環境データＰより干渉の有無
の判定を行う．ここで干渉がない場合は干渉判定装置ｌ
４は判定信号Ａとして干渉がないことを送り、座標変換
装［１５はマスタスレーブ制御装置４にスレーブ目標関
節角Ｏｒｊ＝θｃｉを出カする。干渉がある場合には、
干渉判定装置１４は座標変換装置ｌ５に判定信号Ａとし
て干渉していることを送る。このとき干渉判定装置ｌ４
は次の手順で１０を変更する。スレーブ２の根元側の関
節Ｊｓｌより手先側の関ｊｌＪｓ４へ向かって干渉の有
無を判定し、最初に干渉した関節をＪｓｊとする．この
関節Ｊｓｊの関節角θｓｊをこの値に最も近い値で干渉
しない関節角θｓｊに変更する．以上の処理を関節Ｊｓ
ｊが干渉しなくなるまで順に行い、干渉がなくなると、
座標変換装［１５から関節角θｓｊを目標関節角θＣｊ
としてマスタスレーブ制御装置４へ出力する。

マスタスレーブ制御装置４は、スレーブ２のアーム部１
２ｓの関節角θｓｌ〜θｓ４とスレーブ目標関節角ｏｒ
１〜θｒ４を入力して、その差に比例した電流を出力し
、関節Ｊｓｉを制御する．第２の実施例によれば、マス
タ１を手首の３自由度とエンドエフェクタのみの簡単な
構造にしても，オペレータ３はスレーブ２の手先の環境
だけを意識してマスタｌを操作すれば，スレーブ２のア
ーム部１２ｓが障害物に衝突することなく作業を行える
． 第７図は本発明の第３の実施例を示すもので、第４図に
示した第２の実施例に、スレーブ２の手首１．１ｓをＩ
ｉＩ！察できる画像入力装置ｌ７と、その画像を表示す
る画像表示装置ｌ８と、これらを接続する画像信号１９
が付加されたものである。

本実施例において、オペレータ３がマスタ１を操作する
方法及びスレーブ２の動作は第２の実施例と同じである
が、画像入力装置１７はスレーブ２の手首ＩＩｇ、エン
ドエフェクタ１０ｓ及び作業対象物６の画像を取り込み
、画像信号ｌ９を通して画像表示装置１８に送信し、画
像表示装置１８はこの送信された画像を表示する。従っ
てオペレータ３はこの画像を観察しながらマスタｌを操
作することができるので、オペレータ３の視覚範囲に障
害物７が入り，スレーブ２の手先が直接観察できない場
合でも、画像入力装置１７をこの手先が観察可能な位置
に設けておけば、その画像を画像表示装置１８でｗ４察
できるので，スレーブ２を障害物７に衝突させることな
くマスタ１を操作することができる。

なお、本実施例の付加部分、即ち画像入力装置及び画像
表示装置を第１図に示した第１の実施例に付加しても、
同じ効果を得ることができる．第８図は本発明の第４の
実施例を示すもので、第４図に示した第２の実施例にお
いて、スレーブ２のアーム部１２ｓの各リンクに近接セ
ンサ２２を付加し、さらに操作支援装置５（第３図）の
環境形成装置１３に代わって第９図のようにセンサ検出
装置２４を設けたものである．近接センサ２２は各リン
クの前面に取り付けることが望ましいが、本実施例では
リンク代表点としてリンクの中間点に設けた。近接セン
サ２２には光，超音波、電磁波を用いたものがあるが，
ここでは光を用いた例について説明する。第１０図はリ
ンクに近接センサを取り付けたときの断面図で、図の斜
線部（センサ先端から距離１の点を通る円内の一部）に
物体が接近したときセンサ信号２３を出力し，これはセ
ンサ検出装置２４に取り込まれる６センサ検出装置２４
は干渉判定装１ｉ１１４に信号Ｑを出力する。

次に動作について説明する。第２の実施例と異なるのは
、スレーブ２のアーム部１２ｓの障害物回避の方法であ
り、他は同じである。センサ検出装ｗ２４は，センサ信
号２３より干渉しているリンクを検出し、干渉判定装置
ｌ４に信号Ｑでその結果を知らせる。干渉していない場
合は，式（４）〜（１ｏ）で述べた方法により関節Ｊｓ
ｌ〜Ｊｓ４を動かす。

センサ検出装置２４でアーム部１２ｓと障害物７が干渉
していることが検出されたときは、干渉判定装ｆｉｌ４
に干渉しているリンクＬｓｊと干渉している方向Ｗが信
号Ｑで送信される．干渉判定装置１４は、リンクＬｓｊ
から根元の関節までの関節中心ベクトルｕ　ｊと送られ
たベクトルＷとが垂直，即ちｃｏｓ　ｙ＝ｕ，）−ｗ／
　（ｌｕＪＩ　ａｗｌ）がＯとなる関節角θｓｊを求め
る．ここで例えばベクトルｕ４は次式となる； ｕ　４　＝　Ｃ，ＣＧＣｓＣ４　ｅ　４そして関節Ｊｓ
ｊの目標関節角θｃｊのみを上に求めた関節角θｓｊに
変更し、他はそのままにして座標変換装置１５に出力す
る。これにより障害物７にリンクＬｓｊが近づくことな
く、現在位置を保持する。干渉しているリンクより根元
側の関節が動いて、近接センサ２２の出カがら干渉して
ぃないことがわかるが、式（１０）で新たに計算した目
標関節角θｃｉで干渉の原因となった関節角の目標値が
干渉直前と反対になったときは、干渉判定装置ｌ４は干
渉がないと判断して、判定信号Ａを出力する。

第４の実施例によれば、スレーブ２のアーム部１２ｓと
障害物７との接近を近接センサ２２で検出しているため
、安価な装置で障害物回避ができる。

同様に第ｌの実施例においても、第４の実施例で付加し
た部分を加えることにより同じ効果を得ることができる
。また干渉検出は，近接センサによるとしたが、これは
接触センサに替えることもできる。

第１１図は本発明の第５の実施例を示すもので、第４図
に示した第２の実施例において、マスタ１の操作領域に
警告装置２５を付加し、操作支援装置５の干渉判定装置
１４は，第１２図のように警告装置２５へ警告信号２６
を出力するようにしたものである。

この構或では、干渉判定装置１４が、第２の実施例と同
様の計算によりスレーブ２のアーム部１２ｓが障害物７
と干渉していることを検出すると、警告信号２６を通し
て警告装置２５に干渉した関節またはリンクを知らせる
．警告装置２５は音または表示によりオペレータ３に干
渉している関節またはリンクを知らせる。

第５の実施例によれば、オペレータ３は干渉している関
節、リンクがわかるので、その情報を考慮してマスタ１
を操作でき，操作性を一層向上することができる。同様
に第ｌの実施例においても、第５の実施例で付加した部
分を加えることにより同じ効果を得ることができる。

第１３図は本発明の第６の実施例を示すもので，第４図
に示した第２の実施例において，スレーブ２の作業環境
に三次元画像入力装置２７を付加し、第１４図に示すよ
うに操作支援装置５を三次元環境認識装置２８，環境形
或装置１３、干渉判定装置１４及び座標変換装ｗｌ５で
構成したものである。三次元画像入力装置２７は三次元
画像信号２９を三次元環境認識装置２８に出力し、三次
元環境認識装置２８は環境形成装［１３に環境信号Ｄを
送信する．次に動作について説明する。三次元画像入力
装置２７はスレーブ２の作業環境の画像を三次元画像信
号２９として出力し，これを用いて三次元環境認識装置
２８は作業環境の三次元情報を得る．この情報は環境形
或装置ｌ３へ環境情報Ｄとして送られ、同装置１３にお
いてＣＡＤデータから作成された作業環境の三次元情報
の変更に用いられる。この変更した三次元情報は干渉判
定装置１４へ送られ、以下第２の実施例と同様な処理が
行われる。

第６の実施例によれば、スレーブ２の作業環境がスレー
ブ２の作業結果等により変化したときでも，その変化に
応じて変更された三次元情報をもとに干渉判定装置ｌ４
で干渉の有無の判定を行っているので，干渉の有無の判
定の信頼性が向上する。

同様に第工の実施例においても，第６図の実施例で付加
した部分を加えることにより同じ効果を得ることができ
る。

〔発明の効果〕

本発明によれば、スレープマニビュレータのアーム部の
障害物回避が操作支援装置により実施され、スレーブマ
ニピュレータの手首及びエンドエフェクタの動きはマス
タマニピュレータのうごきに追随するので，マスタスレ
ーブマニピュレータの操作性を良好にすることができる
．

【図面の簡単な説明】

第１図，第２図及び第３図は本発明の第１の実施例の全
体システム図、マスタスレーブ制御装置のブロック図及
び操作支援装置のブロック図、第４図、第５図及び第６
図は本発明の第２の実施例の全体システム図、スレーブ
マニピュレータの基準姿勢を示す図及び関節の回転軸を
示す図、第７図は本発明の第３の実施例の全体システム
図、第８図，第９図及び第１０図は本発明の第４の実施
例の全体システム図，操作支援装置のブロック図及び近
接センサの配置図，第１１図及び第１２図は本発明の第
５の実施例の全体システム図及び操作支援装置のブロッ
ク図、第ｌ３図及び第１４図は本発明の第６の実施例の
全体システム図及び操作支援装置のブロック図である。 １・・・マスタマニピュレータ，２・・・スレーブマニ
ピュレータ、３・・・オペレータ、４・・・マスタスレ
ーブ制御装置、５・・・操作支・援装置、６・・・作業
対象物，７・・・障害物、ｌＯｍ　，１０　ｓ・・・エ
ンドエフェクタ、ｌｌｍ，１１ｇ・・・手首．１２ｍ，
１２ｇ・・・アーム部、１９・・・アクセル，１７・・
・画像入力装置、ｌ８・・・画像表示装置，２２・・・
近接センサ、２５・・・警告装置，２７・・・三次元画
像入力装置．

Claims (1)

1. 【特許請求の範囲】 １、スレーブマニピュレータの先端部と該先端部を除い
   たアーム部のそれぞれに対応する先端部及びアーム部を
   有したマスタマニピュレータと、与えられた目標関節角
   となるように上記スレーブマニピュレータのアーム部各
   関節を動かしたときに該アーム部が障害物と干渉するか
   否かを検出する検出機能と該機能により上記干渉が検出
   されたときは該干渉を回避するように上記目標関節角を
   修正し上記干渉がなければ与えられた目標関節角をその
   まま出力する修正機能とを有した操作支援装置とを設け
   るとともに、上記スレーブマニピュレータの先端部の各
   関節角は上記マスタマニピュレータの先端部の対応する
   関節角に追随するように制御し、上記スレーブマニピュ
   レータのアーム部の各関節角は上記マスタマニピュレー
   タの対応する関節角を目標関節角としたときの上記操作
   支援装置から出力される目標関節角となるように制御す
   ることを特徴とするマスタスレーブマニピュレータの制
   御方法。 ２、スレーブマニピュレータの先端部に対応した先端部
   を有したマスタマニピュレータと、アクセルと、スレー
   ブマニピュレータの各関節角と上記アクセルからの信号
   に応じてスレーブマニピュレータの上記先端部を除いた
   アーム部の各関節角の目標関節角を算出し、該目標関節
   角となるように上記スレーブマニピュレータのアーム部
   各関節を動かしたときに該アーム部が障害物と干渉する
   か否かを検出する検出機能と該機能により上記干渉が検
   出されたときは該干渉を回避するように上記目標関節角
   を修正し上記干渉がなければ与えられた目標関節角をそ
   のまま出力する修正機能とを有した操作支援装置とを設
   けるとともに、上記スレーブマニピュレータの先端部の
   各関節角は上記マスタマニピュレータの先端部の対応す
   る関節角に追随するように制御し、上記スレーブマニピ
   ュレータのアーム部の各関節角は上記操作支援装置から
   出力される目標関節角となるように制御することを特徴
   とするマスタスレーブマニピュレータの制御方法。 ３、スレーブマニピュレータの先端部と該先端部を除い
   たアーム部のそれぞれに対応する先端部及びアーム部を
   有したマスタマニピュレータと、与えられた目標関節角
   となるように上記スレーブマニピュレータのアーム部各
   関節を動かしたときに該アーム部が障害物と干渉するか
   否かを検出する検出機能と該機能により上記干渉が検出
   されたときは該干渉を回避するように上記目標関節角を
   修正し上記干渉がなければ与えられた目標関節角をその
   まま出力する修正機能とを有した操作支援装置と、上記
   スレーブマニピュレータの先端部の各関節角を上記マス
   タマニピュレータの先端部の対応する関節角に追随する
   ように制御し、上記スレーブマニピュレータのアーム部
   の各関節角を上記マスタマニピュレータの対応する関節
   角を目標関節角としたときの上記操作支援装置から出力
   される目標関節角となるように制御するマスタスレーブ
   制御装置とを設けたことを特徴とするマスタスレーブマ
   ニピュレータの制御装置。 ４、スレーブマニピュレータの先端部に対応した先端部
   を有したマスタマニピュレータと、アクセルと、スレー
   ブマニピュレータの各関節角と上記アクセルからの信号
   に応じてスレーブマニピュレータの上記先端部を除いた
   アーム部の各関節角の目標関節角を算出し、該目標関節
   角となるように上記スレーブマニピュレータのアーム部
   各関節を動かしたときに該アーム部が障害物と干渉する
   か否かを検出する検出機能と該機能により上記干渉が検
   出されたときは該干渉を回避するように上記目標関節角
   を修正し上記干渉がなければ与えられた目標関節角をそ
   のまま出力する修正機能とを有した操作支援装置と、上
   記スレーブマニピュレータの先端部の各関節角を上記マ
   スタマニピュレータの先端部の対応する関節角に追随す
   るように制御し、上記スレーブマニピュレータのアーム
   部の各関節角を上記操作支援装置から出力される目標関
   節角となるように制御するマスタスレーブ制御装置とを
   設けたことを特徴とするマスタスレーブマニピュレータ
   の制御装置。 ５、前記操作支援装置は、予め設定されたスレーブマニ
   ピュレータの作業環境情報と前記目標関節角とから障害
   物との干渉の有無を検出することを特徴とする請求項３
   もしくは４記載のマスタスレーブマニピュレータの制御
   装置。 ６、スレーブマニピュレータの作業環境を撮像する撮像
   手段を設けるとともに、該手段により撮像された画像情
   報により前記作業環境情報を修正する機能を前記操作支
   援装置に設けたことを特徴とする請求項３もしくは４記
   載のマスタスレーブマニピュレータの制御装置。 ７、前記スレーブマニピュレータのアーム部の各関節角
   間を結合するリンクに物体が接近したことを検出する近
   接センサまたは物体と接触したことを検出する接触セン
   サを設け、上記近接センサまたは接触センサからの信号
   により前記操作支援装置は前記干渉の有無を検出するこ
   とを特徴とする請求項３もしくは４記載のマスタスレー
   ブマニピュレータの制御装置。 ８、オペレータへの警告手段を設け、前記操作支援装置
   は、前記干渉が有ると判断したときには該干渉のあるこ
   と及びその干渉を生じる関節あるいはリンクを上記警告
   手段によりオペレータへ知らせることを特徴とする請求
   項３もしくは４記載のマスタスレーブマニピュレータの
   制御装置。 ９、前記スレーブマニピュレータの先端部を撮像する三
   次元画像入力手段と、該手段により撮像された上記先端
   部の画像情報をオペレータに表示する表示手段とを設け
   たことを特徴とする請求項３もしくは４記載のマスタス
   レーブマニピュレータの制御装置。