JPH08309680A

JPH08309680A - マニピュレータ制御システム

Info

Publication number: JPH08309680A
Application number: JP17087796A
Authority: JP
Inventors: Akira Dobashi; 亮土橋; Shinichi Takarada; 真一宝田; Taro Iwamoto; 太郎岩本; Yoshio Nakajima; 吉男中島; Ritsu Aoki; 立青木; Hiroshi Yamamoto; 広志山本
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1996-07-01
Filing date: 1996-07-01
Publication date: 1996-11-26

Abstract

(57)【要約】【課題】マスターアームの位置情報をスレーブアームの
指令位置を表わす位置情報に対応した第一一般化座標に
変換后行なわれていた追従動作の中断を指示する信号が
受信され、マスタアームと、画像装置によりコネクトさ
れたスレーブアームの位置関係が更新されると、第一一
般化座標はこの新たな位置関係に基づく第二一般化座標
に変換されて後両者間の追従動作が再開される。【解決手段】画像装置の交替、マスタアームの移動可能
領域内の再設定、追従動作の大きさの比の変更に対応し
た行列演算によりマニピュレータの最適制御が実現され
る。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明はオペレータが操作するマ
ニピュレータに係り、特に人間にとって耐え難い環境お
よび宇宙空間内での作業を確実に行なうに好適なマニピ
ュレータの制御方法に関するものである。

【０００２】

【従来の技術】従来のマニピュレータは、複雑なマニピ
ュレータの動作を行なうのをオペレータの技倆に頼る傾
向があった。

【０００３】例えば、マスタースレーブ形においては、
マスターアームとスレーブアームとが同一形状又は相似
形になっているため、マスターアームが人間の操作に不
適な構造であったり、オペレータ自身がマスターアーム
の操作の邪魔になってしまうほど大きかったり、また細
かい操作ができないことがあった。

【０００４】また、遠隔操作においては、マニピュレー
タ自身がオペレータの視野を遮ぎり、オペレータは顔を
左右に動かしながら操作しなければならなかった。この
ため、インダストリアルＴＶ（ＩＴＶ）などの利用も行
なわれたが、１つの画像では充分な視野が得られず、ま
たテレビカメラを多数配置すると画像とマニピュレータ
との関係が変わり、オペレータが操作を誤まることがあ
った。

【０００５】最近では、コンピュータ技術の急速な進歩
により、マニピュレータやロボットの制御に計算機を取
り入れ、高速化して来ている。その一例として、Ｔ．Ar
ai et ol.,“BILATERAL CONTROL FOR MANIPULATORS WIT
H DIFFERENT CONFIGURATORS”，１９８４，ＩＥＣＯＮ
’８４，第４０頁〜第４５頁に示されるように、形状
の異なるマスター腕とスレーブ腕との手先の運動を計算
機を用いた高速座標変換演算により１対１に対応させて
いるものがある。ヤコビ行列を用いた速度や力の変換も
同文献に示されている。

【０００６】また特開昭６１−２５２０８１号公報では
マスター腕とスレーブ腕との間に位置・姿勢のずれがあ
って共通の原点を持つように投影変換しても１対１の対
応関係がとれない場合でも座標系対応付装置によって対
応することのできる方式が示されている。

【０００７】さらに、ＩＴＶにロボットのアニメーショ
ンを出映して、ロボットのティーチングをアニメーショ
ン上で行なわせ、ロボットを実際に動作させてティーチ
ングしたのと同じ結果を便利に得られる技術も発表され
ている。

【０００８】

【発明が解決しようとする課題】上述した従来技術は、
それぞれ個々の狭い技術分野にとどまり、マニピュレー
タを総合的な見地から検討して人間を含めた望ましいシ
ステム技術として完成していないといえる。

【０００９】例えば、上述の高速座標変換演算技術の場
合では、マスタ腕とスレーブ腕とにおいて、それぞれの
基準座標と各腕手先の位置との２点を一致させているた
め各アームの運動の比率は一定となっている。

【００１０】このため、スレーブ腕によって精密な作業
を実現する場合には、マスター腕をスレーブ腕に要求さ
れる動作と同様に細かな微小動作を行なわねばならず、
またこれと逆にスレーブ腕が大きな動作を必要とする場
合には、同様にマスタ腕を大きく動作しなければならな
い。

【００１１】このように、スレーブアームの作業は、オ
ペレータに大きな負担をかけ、その技倆により作業の実
現性が決められることになる。

【００１２】このような、オペレータの負担に加え、Ｉ
ＴＶを見ながら操作するときには、画像を見てテレビカ
メラの方向を考えマスターアームを操作することになり
精神的な負担も大きくなる。

【００１３】特開昭６１−２５２０８１号公報は上記の
問題点の解決に有効な手段を提供している。この方法は
マスター腕とスレーブ腕とを独立の制御が可能にしてお
き後で座標系の対応付けを行なうやり方である。このた
め広範囲の対応付けができる反面予想外の対応付けによ
って危険を生じる恐れがある。またＩＴＶカメラに応じ
た対応付けはマスター腕とスレーブ腕とが独立に扱って
いるためカメラとマスタ腕、カメラとスレーブ腕を別々
にそれぞれ対応付けする必要が生じて操作が複雑で実用
的でない恐れがある。

【００１４】本発明の目的は簡単な操作で安全にオペレ
ータの操作する操作腕の作業性を向上させることのでき
るマニピュレータ制御方法を提供することである。

【００１５】本発明の別の目的はマニピュレータの操作
腕の操作性が簡単な操作で向上し、その結果安全にマニ
ピュレータ自体の操作性も向上するマスタスレーブ制御
方法を提供することである。

【００１６】本発明の更に別の目的は画像入力装置のズ
ーム比を任意に変えたときに、マスタアームの手先の運
動とテレビモニタ上のスレーブアームの手先の映像の運
動との大きさの比率を常に一定に保持することのできる
マニピュレータ制御方法を提供することである。

【００１７】

【課題を解決するための手段】上記目的は、操作腕と、
該操作腕の動作に応じて作動するマニピュレータとを有
し前記操作腕と該マニピュレータとに１対１の対応をと
るための原点を設けて該操作腕の位置情報を一般化座標
に変換するステップ、該マニピュレータの原点を一般化
座標の原点に一致させ、該一般化座標を該マニピュレー
タの一般化座標となし該マニピュレータを作動させるた
めの各軸指令値に座標変換する第一座標変換ステップ、
該一般化座標を第２一般化座標に変換するための第二座
標変換ステップを含むことで達成される。

【００１８】

【作用】オペレータが操作する操作腕は、人間が操作し
やすい形状と大きさを持っている。マニピュレータは必
要な作動空間を動作できる大きさを持ち、相手部品を取
扱うのに適した構造を持っている。オペレータは、最も
楽な姿勢をとり、テレビカメラはオペレータの視野を補
うに必要なだけの台数があり、多くの部品のそれぞれが
見やすくなるような方向に設置されている。テレビカメ
ラ上には光の状態によっては見にくい部分があるため、
アニメーションを利用して、実画像と重ね合わせをする
ことができるようになっている。

【００１９】このようなシステムをオペレータが充分に
使いこなして作業するために、操作腕とマニピュレータ
及びアニメーションの動作比を自由に変えられるよう
に、操作腕が動作限界を越さないよう基点のシフトでき
るように、および方向の違うテレビカメラの画像に切換
えても混乱が生じないよう操作腕と画像表示装置の方向
が一致するように、第２の一般化座標変換を行なうもの
である。

【００２０】

【実施例】以下、本発明の実施例を図面を参照して説明
する。

【００２１】まず、本発明の装置の一実施例を説明する
に先立って、本発明の動作原理を図５を用いて説明す
る。この図において、１０１はマスタ腕、１０２はスレ
ーブ腕で、この例ではスレーブ腕１０２はマスタ腕１０
１と異なる構造に形成されている。いま、マスタ腕１０
１の基準座標系を基部Ｑmを原点にとってＭ、この基準
座標系Ｍからマスタ腕１０１の手先までの座標変換マト
リクスをＴ₅ ^m、スレーブ腕１０２の基準座標系を基部Ｏ
sを原点にとってＳ、この基準座標系Ｓからスレーブ腕
１０２の手先までの座標変換マトリクスをＴ₆ ^Sとし、前
述した座標変換マトリクスＴ₆ ^m，Ｔ₆ ^s間でのスケール変
換マトリクスをＫとする。このとき、ＯmとＯsは一般化
座標の原点Ｏgに一致させて演算を行ない、Ｔ₆ ^mとＴ₆ ^s
とをＫを介して一対一に対応づける。これによって一般
座標上でマスタ腕１０１の手先とスレーブ腕１０２の手
先は一致する。これを順を追って説明すると、マスタ腕
１０１を構成する各リンクのパラメータと各リンク連結
軸の位置とから前述したマスタ腕手先までの座標変換マ
トリクスＴ₆ ^mを求めることができる。次にマスタ腕手先
までの座標変換マトリクスＴ₆ ^mとスレーブ腕手先までの
座標変換マトリクスＴ₆ ^sとの間での（スケール変換を含
んだ）座標変換演算は次の数１で表わされる。

【００２２】

【数１】Ｔ₆ ^s＝Ｋ・Ｔ₆ ^m ここでＫは

【００２３】

【数２】

【００２４】とすると、ｄ＝ｅ＝ｆ＝０のときＴ₆ ^sはＴ
₆ ^mをマスタアーム１０１の基準座標系Ｍのｘ軸方向にａ
倍、ｙ軸方向にｂ倍、ｚ軸方向にｃ倍にしたものとな
る。そして３軸方向に均等に拡大（又は縮少）する場合
にはａ＝ｂ＝ｃとすればよいことになる。そして、前述
した数１により得られた変換マトリクスＴ₆ ^sに対し、ス
レーブアーム１０２のリンクパラメータを与え、スレー
ブアームの各軸の位置の目標値を逆座標変換演算により
求める。このようにして得られたスレーブアーム１０２
の各軸の目標値に対し、各軸ごとにサーボ制御すれば、
各アームの可動領域内では、アームの形状に係りなくマ
スタアーム１０１の運動を任意に拡大または縮少してス
レーブアーム１０２を操作することができる。

【００２５】次に、ａ＝ｂ＝ｃ＝１のとき、Ｔ₆ ^sはＴ₆ ^m
をマスタアーム１の基準座標系Ｍのｘ軸方向にｄ，ｙ方
向にｅ，ｚ方向にｆ平行移動したものとなる。そして、
前述した数１により得られた変換マトリクスＴ₆ ^sに対
し、スレーブアーム１０２のリンクパラメータを与え、
スレーブアーム１０２の各軸の目標値逆変換演算により
求める。このようにして得られたスレーブアーム１０２
の各軸の目標値に対し、各軸ごとにサーボ制御すれば、
マスタアームの基準点に対するスレーブアームの基準点
を任意にずらして操作することができる。

【００２６】次に本発明の装置の一実施例を図２により
説明する。

【００２７】この図において、１０１はマスタアーム、
１０２はスレーブアームで、このスレーブアーム１０２
はマスタアーム１０１とは異なる形状に構成されてい
る。１０５は中央処理装置で、座標変換演算および両ア
ームの制御等を行う。

【００２８】１１８は座標変換追加装置で、キーボード
からの入力に基づき数２のＫを作り、中央処理装置１０
１に数１に示す座標変換を追加する指示を与える。

【００２９】１０４はマスタアーム１０１の入出力装置
で、マスタアーム１０１の各関節角度等を中央処理装置
１０５へ入力し、また中央処理装置１０５からマスタア
ーム１０１への指示値をサーボ増幅し、マスタアーム１
０１の各関節のアクチュエータを駆動する働きをする。
１０６はスレーブアーム１０２の入出力装置で、スレー
ブアーム１０２の各関節角度等を中央処理装置１０５へ
入力し、また中央処理装置１０５からスレーブアーム１
０２への指令値をサーボ増幅し、スレーブアーム１０２
の各関節のアクチュエータを駆動する働きをする。

【００３０】１０７はジョイスティックで、中央処理装
置１０５内部の切換えにより、マスタアーム１０１の代
りにスレーブアーム１０２、及び後述するアニメーショ
ン画像に対する指令値を出す。１０８はジョイスティッ
ク１０７の入出力装置で、ジョイスティック１０７の傾
き角の信号等を中央処理装置１０５へ入力し、また、中
央処理装置１０５からジョイステック１０７への帰還信
号をサーボ増幅し、ジョイスティック１０７のアクチュ
エータを駆動する働きをもつ。

【００３１】１０９は画像入力装置で、スレーブアーム
１０２の作業を種々の方向から監視する。１１０は画像
処理装置で、画像入力装置１０９で入力した画像にファ
ストフーリエトランスフォーム（ＦＦＴ）等の演算を処
し、パターン同定等を行う。

【００３２】１１１はアニメータシミュレータで、コン
ピュータグラフィックによるスレーブアームをリアルタ
イムで表示するアニメーション用計算機である。１１２
はグラフィックディスプレイで、アニメータシミュレー
タ１１１によるアニメーション画像を表示する他、アニ
メーション画像と画像入力装置１０９で入力した実画像
を重ねて表示することもでき、さらにモード切換等のメ
ニュー表示を始め、中央処理装置１０５及びアニメータ
シミュレータ１１１とのマンマシンインタフェースの対
話用出力にも用いる。１１３はテレビモニタで、画像入
力装置１０９の映像を表示する。１１７はキーボードで
スケール変換定数、基点シフト指示メニューの入力を始
め、中央処理装置１０５及びアニメータシミュレータ１
１１とのマンマシンインタフェースの対話用入力に用い
る。

【００３３】２０１は作業対象物で、スレーブアーム１
０２は広い範囲において作業対象物２０１に対し作業を
行わねばならない。

【００３４】次に本実施例の信号の流れの主なものを図
１を用いて説明する。なお、実際のものは、マスタアー
ム、スレーブアーム、ジョイスティック共に６自由度を
持つが、以下、簡単のため３自由度として説明を行う。

【００３５】１１４Ａ〜１１４Ｃはマスタアーム１０１
の各関節軸に設けた位置検出センサであり、この信号は
マスタアーム入出力装置１０４で処理し、中央処理装置
１０５へ入力する。１１５Ａ〜１１５Ｃはスレーブアー
ム１０２の各関節軸に設けた位置検出センサであり、こ
の信号はスレーブアーム入出力装置１０６で処理し、中
央処理装置１０５へ入力する。以上の信号より、中央処
理装置１０５では一般化座標への座標変換演算を行い、
さらに座標変換追加装置１１８の指示に基づき、第２一
般化座標への座標変換を行うことにより、スレーブアー
ム１０２への指定値を決定する。この信号はスレーブア
ーム入出力装置１０６を経てスレーブアーム１０２の各
関節軸に設けたアクチュエータ１１６Ａ〜１１６Ｃを駆
動する。又同時に、中央処理装置１０５からアニメータ
シミュレータ１１１へ信号を送り、グラフィックディス
プレイ１１２上にスレーブアームのアニメーション画像
を表示する。

【００３６】次に、１１７Ａ〜１１７Ｃはジョイスティ
ックの角度を検出する位置検出センサであり、この信号
はジョイスティック入出力装置１０８で処理し、中央処
理装置１０５へ入力する。中央処理装置１０５ではスレ
ーブアームへの指令を決定する際に、外部からの切換信
号に基づき、マスタアームとジョイスティックのいずれ
からの信号を参照するかを決定する。

【００３７】また、画像入力装置１０９で入力した画像
は、画像処理装置１１０でＦＦＴ、パターン同定を行っ
た後、実画像をテレビモニタ１１３で表示すると同時に
アニメータシミュレータ１１１へ送り、必要に応じてグ
ラフィックディスプレイ１１２において、アニメーショ
ン画像に重ねて表示する。

【００３８】次に、位置検出センサ１１４Ａ〜１１４Ｃ
からの信号をマスタアーム入出力装置１０４で処理する
際の具体例を図３に示す。これはジョイスティック、ス
レーブアームのものに関しても同様である。図３におい
て位置検出センサ１１４Ａ〜１１４Ｃとしては回転式パ
ルス発生器を用いている。この位置検出センサ１１４Ａ
〜１１４Ｃからは９０°位相のずれた１組のパルス信号
すなわちＡ相及びＢ相が回転角に応じて発生する。この
信号は方向判別回路３０１に入力されて回転角の方向判
別を行う。一方Ａ相またはＢ相の信号がカウンタ３０２
に入力され、パルス数をカウントする。前記の方向判別
回路３０１より出力される方向信号３０３はカウンタ３
０２に入力され、パルス数の増減の切換えを行う。した
がって、カウンタ３０２の値は回転角の増減に対応して
増減するので、カウンタ３０２の出力３０４を外部から
読み込むことによって回転角を検知することができる。

【００３９】図４は中央処理装置１０５の具体的な構成
例を示す。この中にはデータの入出力制御及び加減算等
を行うプロセッサ４０１、三角関数表やマニピュレータ
のリンクパラメタなどのデータを格納するメモリ４０
２、掛算器４０３及び割算器４０４がバス回路４０５で
接続されている。さらにこのバス回路４０５にはシリア
ルまたはパラレルのインタフェイス回路４０６Ａ〜４０
６Ｅが接続されている。インタフェイス回路４０６Ａ〜
４０６Ｅには各アーム入出力装置およびアニメータシミ
ュレータ、座標変換追加装置が接続されている。プロセ
ッサ４０１はバス回路４０５を介してバス回路４０５に
接続されている全ての機器にアクセスし、データを処理
することができる。

【００４０】次に上述した本発明の装置の一実施例の動
作を説明する。

【００４１】マスタアーム１０１を動作させると、マス
タアーム１０１の各関節角は位置検出センサ１１４Ａ〜
１１４Ｃにより検出される。この検出信号はマスタアー
ム入出力装置１０４を介して中央処理装置１０５に入力
される。中央処理装置１０５はマスタアーム１０１の手
先座標系ＭＣのマスタアーム基準座標系Ｍに対する相対
位置関係を座標変換マトリクスＴ₆ ^mとして記憶してお
り、一般化座標（第１）への座標変換演算を行う。ま
た、キーボード１１７からの入力に基づき座標変換追加
装置１１８はマスタアーム１０１の手先運動に対するス
レーブアーム１０２の手先の運動の寸法比率すなわちス
ケール変換定数、およびマスタアーム１０１の先端の位
置とスレーブアーム１０２の先端の位置の基準点のシフ
ト量を示すマトリクスＫを記憶しており、中央処理装置
１０５に座標変換の追加を指示する。すると、中央処理
装置１０５はマスタアーム座標変換マトリクスＴ₆ ^mに対
しＫを作用させる演算を行い、スレーブアーム座標変換
マトリクスＴ₆ ^sを得る。次に、スレーブアーム１０２の
手先座標系ＳＣのスレーブアーム基準座標系Ｓに対する
相対位置がスレーブアーム座標変換マトリクスＴ₆ ^sに一
致するようにしたときのスレーブアーム１０２の各関節
軸目標値を逆座標変換演算により求め、これをスレーブ
アーム入出力装置１０６に出力する。スレーブアーム入
出力装置１０６はアクチュエータ１１６Ａ〜１１６Ｃを
駆動する。これにより、マスタアーム１０１の手先の運
動をスケール変換、基点シフトあるいは両方を行ってス
レーブアーム１０２の手先の運動に変化することができ
る。この結果、各アームの可動領域内では、アームの形
状に係りなく、マスタアーム１０１の運動を、任意に拡
大または縮少してスレーブアーム１０２に伝えることが
でき、マスタアーム１０１の操作に対してスレーブアー
ム１０２を細かく運動させたり、粗雑であるが大きく動
作を与えることができる。

【００４２】また、キーボード１１７の入力により座標
変換追加装置１１８の指令で、中央処理装置１０５は、
スレーブアーム１０２を１時的にマスタアーム１０１か
ら切離して静止させておき、操作者はマスタアーム１０
１のみを任意の位置まで動かす。この状態で、座標変換
追加装置１１８は、マスタアーム１０１とスレーブアー
ム１０２の位置のずれ量を数２のｄ，ｅ，ｆの部分に記
憶し直して、再びキーボード１１７の入力によってマス
タアーム１０１とスレーブアーム１０２を連動するよう
にすることにより、マスタアーム１０１にもとづいたス
レーブアーム１０２の動作基準点を自由に設定し直すこ
とができ、常にマスタアーム１０１を操縦者にとって操
作し易い位置で動かすことができる。

【００４３】一方、ジョイスティックの位置検出センサ
１１７Ａ〜１１７Ｃからの信号は、ショイスティック入
出力装置１０８を経て中央処理装置１０５に入力する。
中央処理装置１０５はこの信号を時間的に積分し、ジョ
イスティック１０７の仮想的な手先座標系ＪＣのジョイ
スティック基準座標系Ｊに対する相対位置関係を座標変
換マトリクスＴ₆ ^jとして記憶する。そして、前述のＴ₆ ^s
を求める際に、外部からの切換信号によりＴ₆ ^mの代りに
Ｔ₆ ^jを用いることにより、ジョイスティック１０７を操
縦することによりスレーブアーム１０２を動かすことが
できる。

【００４４】マスタアーム１０１はスレーブアーム１０
２への位置指令を出し、ジョイスティックは速度指令と
してスレーブアーム１０２への指令を出すため、操作者
は状況に応じて使い易い方を選択することができる。

【００４５】ジョイスティック１０７を用いた場合も、
スケール変換によってジョイスティックを倒した際のス
レーブアームの移動速度を自由に設定できる等の利点が
生じる。

【００４６】さらに、第２一般化座標、すなわち数２の
ＫＴ₆ ^mをスレーブアーム入出力装置１０６へ出力する代
りに、あるいは同時に、アニメータシミュレータ１１１
へ出力することにより、グラフィックディスプレイ１１
２において、アニメーション画像として見ることができ
る。スレーブアーム１０２を動かさずにアニメーション
画像を表示することにより、スケール変換等の効果によ
る動きを確認できるので、危険な状況に陥いることがな
い。

【００４７】さらに、地上から宇宙にあるスレーブアー
ムを操作するときには、通信の遅れ時間を利用し、実際
のスレーブアームより早く画像を見ることができ、マス
タアームの誤操作をチェックして修正することにも使用
できる。もし、実際のスレーブアームの動きを知りたい
ときには、アニメーション画像を通信の遅れ時間分だけ
おくらして表示すればよい。

【００４８】また、スレーブアーム１０２を動かすのと
同時にアニメーションを表示した場合、アニメーション
は任意の方向から見た場合の表示ができるので、操作性
は向上する。

【００４９】また、画像入力装置１０９により入力した
画像はテレビモニタ１１３で表示する他、グラフィック
ディスプレイ１１２のアニメーション画像に重ね表示す
ることができる。これにより、より現実的な画像情報が
得られると共に、実画像においては手前の物体の影にな
って死角となる部分もアニメーション画像では表示でき
るので、実画像とアニメーションがお互いの欠点を補い
合うことができる。

【００５０】なお、この際座標変換追加装置１１８は第
２一般化座標への変換マトリックスＫをアニメーション
画像が実画像にずれることなく重なるように決定する必
要がある。

【００５１】さらに、図２に示したように、画像入力装
置１０９は複数あり、種々の方向からスレーブアームを
見ているため、操作者がテレビモニタ１１３を見ながら
操縦する際には、マスタアーム１０１との方向との対応
ずけを常に頭においておかねばならず、基点シフト等を
行ううえでも困難となる。従って、スレーブアーム座標
変換マトリクスＴ₆ ^sを得る際に、ｉ番目の画像入力装置
１０９に対してスレーブアーム１０２の方向を補正する
座標変換マトリクスをＲｉとすると、Ｔ₆ ^s＝Ｒｉ・Ｋ・Ｔ₆ ^m として、Ｔ₆ ^sを得て画像装置に映ることにより、操作者
は方向の差を意識することなく、マスタアーム１０１を
操作することができる。（実際のスレーブの動きはＫ・
Ｔ₆ ^mで行なわれる。）Ｒｉ・Ｋは座標変換追加装置１１
８により与える。

【００５２】ここで、ｉ番目の画像入力装置１０９の視
線の方向のロール角、ピッチ角、ヨー角をα_i，β_i，γ
_iとすると、Ｒｉは次式で与えられる。

【００５３】

【００５４】ようような座標変換マトリックスＲを導入
すると宇宙におけるスレーブアームの熱変形、慣性変形
に対応した信号をスレーブアームに与えることができ、
操作性が向上する。上記の目的の場合アーム変形による
先端の姿勢のロール角、ピッチ角、ヨー角をδ，ε，ζ
とし、ひずみによるｘ，ｙ，ｚ方向へのずれ位置をｐｑ
ｒとすると

【００５５】と表すことができる。

【００５６】次に、本発明のもう一つの実施例を図５を
用いて説明する。この実施例はマスタアーム１０１の手
先の位置の微少変位に対し、スケール変換演算を行った
ものを、スレーブアーム１０２の手先の微少変位として
スレーブアーム１０２に伝えるようにしたものである。
そして、図１に示す実施例と同様にマスタアーム１０１
の基準座標系をＭ、基準座標系Ｍからマスタアーム１０
１の手先までの座標変換マトリクスＴ₆ ^m、スケール変換
演算等のための変換マトリクスをＫとする。

【００５７】またスレーブアーム１０２の基準座標変換
マトリクスをＴ₆ ^sとすると、座標変換演算の手順は次の
ようになる。すなわち、マスタアーム１０１の各リンク
パラメタとその各関節軸の位置から座標変換マトリクス
Ｔ₆ ^mが求められる。またスレーブアーム１０２の各リン
クパラメタとその手先の位置を示す座標変換マトリクス
Ｔ₆ ^sが与えられれば、スレーブアーム１０２の各軸の目
標値が求められる。

【００５８】今ある時点でマスタアーム１０１とスレー
ブアーム１０２の運動を同期させるものとすると、手先
の位置の微少変位ｄＴ₆とマニピュレータ各軸の微少変
位ｄＱとの間には次の関係がある。

【００５９】

【数３】ｄＴ₆＝ＪｄＱ（Ｊ：ヤコビ行列）今、マスタアーム１０１を微少運動ＭＤさせたとき、そ
の各関節軸の変位の変化をｄＱ^mとし、マスタアーム１
０１のヤコビ行列をＪ^mとしたとき、マスタアーム１０
１の手先の微少運動ｄＴ₆ ^mは数４から得られる。

【００６０】

【数４】ｄＴ₆ ^m＝Ｊ^m・ｄＱ^m ここで、ｄＴ₆ ^mをスケール変換してスレーブアーム１０
２の手先の微少運動ｄＴ₆ ^sを数５から得る。

【００６１】

【数５】ｄＴ₆ ^s＝ＫｄＴ₆ ^m 次にスレーブアーム１０２の各関節軸の微少変位ｄＱ^s
をスレーブアーム１０２のヤコビ行列Ｊ^sの逆行列
（Ｊ^s)~¹を解くことにより求める。すなわち、

【００６２】

【数６】ｄＱ^s＝（Ｊ^s)~¹・ｄＴ₆ ^s 数６で得られたスレーブアーム１０２各関節軸の微少変
位ｄＱ^sをスレーブアーム１０２の各関節軸の位置に加
え、これをスレーブアーム１０２の各関節軸のサーボ制
御回路の目標値とする。

【００６３】次に上述した原理にもとづいて本発明の制
御装置の他の実施例を図６を用いて説明する。なお、ジ
ョイスティック１０７、画像入力装置１０９、アニメー
タシミュレータ１１１等の取扱いについては、最初の例
に準じるので、ここではマスタアーム１０１とスレーブ
アーム１０２の関係のみについて説明する。

【００６４】図６において図１と同符号のものは同一部
分または相当する部分である。６０１は差分回路、６０
２は増分回路である。差分回路９はサンプリングタイム
におけるセンサ１１４Ａ〜１１４Ｃのセンサ信号の変化
分を検出する。中央処理装置１０５は前述した数３〜数
６で示された計算を行い、スレーブアーム１０２の各関
節軸の変化分を求め、この変化分を増分回路６０２に出
力する。増分回路６０２はスレーブアーム１０２の各関
節軸毎に現在目標値に中央処理装置で求めた変化分を加
算し、スレーブアーム入出力装置１０６に入力する。ス
レーブアーム入出力装置１０６はスレーブアーム１０２
の各関節軸に設けたアクチュエータ１１６Ａ〜１１６Ｃ
を駆動する。これにより、スレーブアーム１０２は駆動
されるが、その移動量は検出器１１５Ａ〜１１５Ｃによ
って検出され、スレーブアーム入出力装置１０６にフィ
ードバックされる。その結果、マスタアーム１０１の手
先の運動をスケール変換してスレーブアームの手先に伝
達することができる。

【００６５】上述した本発明の制御装置の他の実施例に
おける演算回路の演算処理動作を図７を用いて説明す
る。

【００６６】最初に、初期位置でスタートすると、マス
タアーム１の各関節の初期値を読み込んでおく（７０
２）。次にマスタアーム１とスレーブアームの関節角を
入力し（７０４）、前回のデータとの差から関節角の変
化量ｄＱ^m７０６を求める。次にテーブルを参照して参
画関数を求め７０８、マスタアーム１のヤコビ行列Ｊ^m
を計算する７１０。三角角変化量ｄＱ^mとヤコビ行列Ｊ^m
からマスタアーム１の手先変位ｄＴ₆ ^mを求める７１２。
スケール変換定数Ｋは入力されたデータを用いて得る７
１４。マスタアームの手先変位ｄＴ₆ ^mにＫを乗じてスレ
ーブアームの手先変位ｄＴ₆ ^sを求める７１６。次にスレ
ーブアームのヤコビ逆行列（Ｊ^s)~¹を求める７１８。こ
のｄＴ₆sに（Ｊs)~¹を乗じることによりスレーブアーム
の各関節角変位ｄＱ^sを求め７２０スレーブアームの関
節角Ｑ^sとｄＱ^sの和をとって７２２スレーブアームの各
サーボ系に結果を出力する７２４。上記手続きを運転終
了まで繰返し実行する。

【００６７】この実施例によれば、図１に示す実施例と
同様な効果が得られると共に、マスタアーム１０１とス
レーブアーム１０２の手先位置がどこにあっても同期ス
タートされることができ、かつ任意のスケール変換が可
能である。

【００６８】図８は本発明の装置のさらに他の実施例を
示すもので、この図において、図１と同符号のものは同
一部分である。この実施例は、スレーブアーム１０２の
手先を撮影する画像入力装置１０９のズーム比の変化に
対応して、マスタアーム１０１の運動に対するスレーブ
アーム１０２の運動のスケール変換の定数を変えるよう
にしたものであり、画像入力装置１０９にそのズームレ
ンズの動きを検出するセンサ９０１を設け、このセンサ
情報を座標変換追加装置１１８に入力する。座標変換追
加装置１１８はこのセンサ情報と予め求めておいて補正
演算を行うことにより、スケール変換マトリクスＫを決
定し、前述した実施例と同様に中央処理装置１０５に座
標変換の追加を指示し、中央処理装置１０５はスレーブ
アーム１０２の運動をマスタアーム１０１のそれに対し
てスケール変換動作させるための目標値を演算する。

【００６９】図８において、１１３はテレビモニタであ
る。

【００７０】

【発明の効果】本発明によれば、マニピュレータの操作
腕の操作性が向上し、その結果マニピュレータ自体の操
作性も向上する。

【図面の簡単な説明】

【図１】本発明の一実施例によるマニピュレータ制御シ
ステムのブロック図である。

【図２】同構成図である。

【図３】マスタアーム入出力装置１０４の具体的構成例
を示すブロック図である。

【図４】中央処理装置１０５の具体的構成例を示すブロ
ック図である。

【図５】本発明の動作原理を示す説明図である。

【図６】本発明の他の実施例によるマニピュレータ制御
システムのブロック図である。

【図７】演算処理動作を示すフローチャートである。

【図８】本発明の他の実施例によるマニピュレータ制御
システムのブロック図である。

【符号の説明】

１０１…マスタアーム、１０２…スレーブアーム、１０５…中央処理装置、１０９…画像入力装置１１０…画像処理装置、１１２…グラフィックディスプレイ、１１３…テレビモニタ、１１８…座標変換追加装置、２０１…作業対象物。

───────────────────────────────────────────────────── フロントページの続き (72)発明者中島吉男茨城県土浦市神立町502番地株式会社日立製作所機械研究所内 (72)発明者青木立茨城県土浦市神立町502番地株式会社日立製作所機械研究所内 (72)発明者山本広志茨城県土浦市神立町502番地株式会社日立製作所機械研究所内

Claims

【特許請求の範囲】

【請求項１】操作腕と、該操作腕の動作に応じて作動す
るマニピュレータとを有するマスタスレーブ装置におい
て、マニピュレータを制御する方法は前記操作腕と該マ
ニピュレータとに１対１の対応をとるための原点を設け
て該操作腕の位置情報を一般化座標に変換するステッ
プ、該マニピュレータの原点を一般化座標の原点に一致
させ、該一般化座標を該マニピュレータの一般化座標と
なし該マニピュレータを作動させるための各軸指令値に
座標変換する第一座標変換ステップ、該一般化座標を第
２一般化座標に変換するための第二座標変換ステップを
含むマニピュレータ制御システム。
【請求項２】請求項１記載の方法において、前記第二座
標変換ステップは、前記一般化座標を表示してアニメー
ションを作動させるステップを有する。
【請求項３】請求項２記載の方法において、前記アニメ
ーション作動ステップはアニメーションを画像入力装置
でとらえた実画像と重さね合わせるステップを有する。
【請求項４】請求項１記載の方法において、前記第二変
換ステップは２台以上の画像入力装置を切換えて画像表
示装置に表示するため、それぞれの画像入力装置の設置
方向に対応して該一般化座標を第２一般化座標変換に変
換するステップを有する。
【請求項５】請求項１記載の方法において、画像入力装
置のズーム比に応じて、該一般化座標を第２一般化座標
変換に変換する。
【請求項６】請求項１記載の方法において、前記操作腕
から出力される速度指令を時間的に積分して仮想的な位
置情報とするステップを更に有する。