JPH0271979A - マニピュレータの制御方法および装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明はマニピュレータの制御方法および装置に係り、
特に遠隔操作で作業を行うのに好適なマスク・スレーブ
マニピュレータの制御方法および装置に関する。

〔従来の技術〕

マスク・スレーブマニピュレータは放射性物質を遠隔操
作で取り扱うために生まれた技術である。

従来は同じ軸構成または相似形状の２本のマニピュレー
タアームを用い、対応する各軸の値が常に等しくなるよ
うにサーボ制御することによって２本のアーム全体の運
動を常に等しくさせ、運動を伝達するものであった。こ
のようなサーボ制御方式マスク・スレーブマニピュレー
タはスレーブアームがマスタアームの動作を逐一模倣す
る機能しか無く、種々の使用条件に対し柔軟に対応する
ことができない、これに対し、マスタアームとスレーブ
アームを計算機を介して結合し、異なる軸構成や構造の
アームの手先の運動を一致させて運動伝達を行う計算機
を用いた演算制御方式マニピュレータが開発されている
。この−例は、コンピュータズ・イン・メカニカルエン
ジニアリング。

１９８３年７月号第４８頁から第６０頁（Ｃｏｍｐｕｔ
ｅｒｓ　ｉｎ　Ｍｅｃｈａｎｉｃａｌ　Ｅｎｇｉｎｅｅ
ｒｉｎｇ、　Ｊ　ｕＬｙ　１９８３、ρＰ、４８〜６０
）に論じられている。

このような演算制御方式のマニピュレータでは、マスタ
アームとスレーブアームの長さ比には無関係に、伝達さ
れる運動の大きさの比率を設定することができる。この
点については第４回日本ロボット学会学術講演会予稿集
の第８３頁から第８４頁に論じられている。

〔発明が解決しようとする課題〕 上記の従来技術はオペレータの手の動きをマスタアーム
を介してスレーブアームに正確に伝達することか目標で
あり、いかにしてオペレータの手の動きを忠実にスレー
ブアームに再現できるかが技術開発の最大の課題であっ
た。しかしながら、人間の腕とは構造も機械特性も異な
るマニピュレータアームを用いて人間と同じ動作を正確
に再現することには自ら限界があり、操作しやすいマス
ク・スレーブマニピュレータの実現はなかなか困難であ
った。

ところで、マニピュレータで行う作業は作業内容によっ
て特定の異方性を持つ場合がある。例えば、第１４図に
示すようにハンド１で棒状部材２を把持し穴３へ挿入す
る作業では、穴への挿入方向の運動に対し、挿入方向に
垂直の方向は微細な位置！ｉｔ整が必要である。すなわ
ち、穴の挿入方向とそれに垂直な方向では必要な位置決
め精度や速度が異なる。従来の方法では全方向に等質な
運動伝達であるため、穴に垂直な方向の位置調整操作を
マスタアームで操作しやすくするためにマスタアームの
手先の運動を縮小してスレーブアームに伝達するように
スケール変換すれば、穴の挿入方向の運動も縮小されて
しまうので作業の能率が悪い。逆に挿入方向の運動に適
するようにスケール変換すれば、穴に垂直方向の位置調
整が難しくなる。

また、第１５図に示すようなスプレーガン４を用いた塗
装作業では例えば横方向の移動距離は大きいが、縦方向
の移動距離は少さいというような運動の異方性がある。

このような場合にも横方向の動きに合せてスケール変換
を行えば、縦方向の位置の調整が難しくなり、塗装のラ
インが乱れる恐れがある。逆に縦方向の動きに合せてス
ケール変換すれば、横方向の動作範囲が不足する恐れが
あり、問題である。

マスク・スレーブマニピュレータは一般にスレーブアー
ムの先端にがかる反力をマスタアームで再現してオペレ
ータに伝達するパイラテラル性を持っている。第１４図
に示すような挿入作業では棒状部材２が穴に斜めに挿入
されることにより生ずるかじりを防止しなければならな
い。このため、棒状部材に横方向に働く微妙な力やモー
メントをオペレータに伝達する必要がある。このために
はスレープアームシこ働く力を力センサを用いて検知し
、これに力のスケール変換を施して拡大して力を伝達す
ることが有効と思われるが、従来のように等方向にスケ
ール変換を行えば、挿入方向の反力に対しても過大な力
がマスタアームに返ってくることになるので、少しの摩
擦抵抗があっても棒状部材１を穴に挿入することができ
なくなり１問題である。

本発明の目的は、その時々の作業内容に応じてその作業
を最もやりやすいように、マスタアームの運動や力を異
方性にスケール変換してスレーブアームに伝達すること
にある。このとき、スケール変換はマニピュレータの基
準座標に対し行う場合と１手先座標系に対し行う場合と
作業対象物の座標系に対し行う場合がある。

〔課題を解決するための手段〕

上記の課題はマスタアームの基部の座標系に対するマス
タアーム手先の位置関係を示す座標変換マトリクスを求
める手順と、得られた座標変換マトリクスにその左側も
しくは右側から異方性のスケール変換マトリクスを掛け
、得られたマトリクスを、スレーブアームの基部の座標
系に対するスレーブアーム手先の座標系の位置関係を示
す座標変換マトリクスとする手順と、前記演算で得られ
たスレーブアームの基部の座標系に対するスレーブアー
ム手先の座標系の位置関係を示す座標変換マトリクスか
ら、スレーブアームの各関節軸の回転位置を演算して求
める手順と、前記異方性のスケール変換マトリクスの要
素の値を変更する手順と、を備えたマニピュレータの制
御方法により達成される。

また、マスタアームの基部の座標系に対するマスタアー
ム手先の位置関係を示す座標変換マトリクスの微小変化
を示すマトリクスを求める手順と、得られた微小変化を
示す座標変換マトリクスにその左側もしくは右側から異
方性のスケール変換マトリクスを掛け、得られたマトリ
クスを、スレーブアームの基部の座標系に対するスレー
ブアーム手先の座標系の位置関係を示す座標変換マトリ
クスの微小変化を示すマトリクスとする手順と、前記演
算で得られたスレーブアームの基部の座標系に対するス
レーブアーム手先の座標系の位置関係を示す座標変換マ
トリクスの微小変化を示すマトリクスから、スレーブア
ームの各関節軸の回転位置の微小変化を演算して求める
手順と、前記異方性のスケール変換マトリクスの要素の
値を変更する手順と、を備えたマニピュレータの制御方
法としてもよい。

さらに、マスタアームとスレーブアームの各関節軸の回
転位置を検出する手順と、スレーブアームの先端が受け
る反力を各座標軸方向の成分のマトリクスとして検出す
る手順と、該マトリクスにスケール変換マトリクスを掛
けてマスタアーム先端に加わる力の各座標軸方向の成分
のマトリクスを得る手順と、前記得られたマスタアーム
先端に加わる力のマトリクスからマスタアーム各関節軸
に加わるべきトルクを算出する手順と、を備えているマ
ニピュレータの制御方法とすることもできる。

本発明は、また上記の課題を達成する手段として、マス
タアームの各関節軸の回転位置を検出し信号として出力
する位置検出手段と、４該位置検出手段の出力信号に基
づいてマスタアームの基部の座標系に対するマスタアー
ム手先の位置関係を示す座標変換マトリクスを演算して
求め、該座標変換マトリクスに異方性のスケール変換マ
トリクスを掛けて得られたマトリクスに基づいてスレー
ブアーム各関節軸の回転位置の目標値を演算出力する演
算回路と、該演算回路が出力する前記目標値とスレーブ
アームの各関節軸の回転位置を検出する位置検出手段の
出力とに基づいてスレーブアームのアクチュエータを制
御するサーボ制御回路と。

前記演算回路に接続して設けられ前記異方性スケール変
換マトリクスの要素の値を記憶する記憶回路と、該記憶
回路に接続して設けられ該記憶回路の記憶内容を書き換
えるデータ入力装置とを備えたマニピュレータの制御装
置およびマスタアームの各関節軸の回転位置を検出し信
号として出力する位置検出手段と、該位置検出手段に接
続して設けられマスタアームの関節軸の回転位置の初期
位置からの変化量を検出する差分回路と、該変化量に基
づいてマスタアーム手先位置を示すマトリクスを演算し
、該マトリクスにスケール変換マトリクスを掛けてスレ
ーブアーム手先位置を示すマトリクスを求め、該マトリ
クスに基づいてスレーブアーム各関節軸回転位置の変化
分を演算する演算回路と、演算回路が出力する前記変化
分をスレーブアームの関節軸回転位置の現在目標値に加
算して出力する増分回路と、該増分回路が出力する値と
スレーブアームの各関節軸の回転位置を検出する位置検
出手段の出力とに基づいてスレーブアームのアクチュエ
ータを制御するサーボ制御回路と、前記演算回路に接続
して設けられ前記スケール変換マトリクスの要素の値を
記憶する記憶回路と、該記憶回路に接続して設けられ該
記憶回路の記憶内容を書き換えるデータ入力装置とを備
えたマニピュレータの制御装置を提供する。

〔作用〕

マスタアームの手先位置は、常に座標変換マトリクスと
して求められ、求められた座標変換マトリクスに対して
異方性のスケール変換演算が行われ、この演算結果にス
レーブアームの手先位置が一致するように、スレーブア
ーム各関節軸の回転位置が演算され、指示される。異方
性のスケール変換がなされるので、マスタアーム手先の
動きとスレーブアーム手先の動きは、方向によって動き
の比率が異なる。スケール変換演算に用いられるスケー
ル変換マトリクスの要素の値が変えられるから、マスタ
アーム手先の動きとスレーブアーム手先の動きの比率を
変えることができる。

また、スレーブアーム先端に加ねわる反力を各座標軸方
向の成分のマトリクスとして検出し、該マトリクスにス
ケール変換マトリクスを掛けると、マスタアーム先端に
加わる反力の各座標軸方向の成分のマトリクスが求めら
れる。マスタアーム先端に加わわる力は、マスタアーム
各関節軸に加ねねるトルクの関数であるから、マスタア
ーム先端に加わわる反力の成分が算出されたら、この反
力に対抗してバランスするために、各関節軸が発生すべ
きトルクが逆に演算により求められる。

〔実施例〕

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

まず、本発明の第１の実施例を説明するに先立って本発
明の動作原理を第２図を用いて説明する。

ただし、第２図では３自由度アームとして表現されてい
るが、実際には６自由度である。この例ではスレーブア
ーム６はマスタアーム５と異なる構造に形成されている
。いま、マスタアーム５の基準座標系をＭ、この基準座
標系から手先座標系ＭＣまでの座標変換マトリクスをＴ
２、スレーブアーム６の基準座標系をＳ、この基準座標
系Ｓから手先座標系ＳＣまでの座標変換マトリクスをＴ
６ｓとし、前述したスケール変換マトリクスをＫとする
と、座標変換演算の手厘は次のようになる。

よく知られているように、マニピュレータアームを構成
する各リンクの特性を表すリンクツペラメタはアームの
構造及び寸法関係より各アーム固有の情報として設計デ
ータから得られる。この１ノンクパラメヌと各関節軸回
転位置の現在位置情報から座標変換マ：〜リクスＴ６′
″及びＴｉ”が得られることはボールの「ロボットマニ
ピュレ〒り」で詳しく論じられている。次にマスタアー
ム手先までの座標変換マトリクスＴ♂とスレーブアーム
手先までの座標変換マトリクスＴ−との間でのスケール
変換を含んだ座標変換演算は次の（１）式で表される。

Ｔ６Ｓ＝ＫＴｓｌＩ　　　　　　　　　　　・・・・・
・（１）ここでＫはスケール変換マトリクスであり、で
表される４×４のマトリクスである。ここで、となるよ
うに係数を定めたときは、マスタアームの基準座標系Ｍ
のＸ軸方向にａ倍、ｙ軸方向にｂ倍、Ｚ軸方向に０倍さ
れた座標変換演算マトリクスがスレーブアームの座標変
換マトリクスとなる。

ａ　＝　ｂ　：　ｃとすれば３軸方向に均等にスケール
変換することになる。

マスタアームの基準座標系の３軸方向と異なる座標軸に
ついてスケール変換を行う場合は１例えば手先座標系に
対するスケール変換マトリクスは次のようになる。例え
ば、第３図に示すように、スレーブマニピュレータの基
準座標系Ｓに対し。

手先座標系ＳＣまでの座標変換マトリクスをＴ−とし、
手先座標系ＳＣのＸｈ軸方向にａ倍、　ｙｈ軸方向にｂ
倍、Ｚｈ軸方向に０倍すると仮定する。

ポールのロボットマニピュレータ（吉川恒夫訳）の第３
９頁に論じられているように、基準座標系に対してスケ
ール変換する場合はスケール変換マトリクスを座標変換
マトリクスの左から掛け１手先座標系に対してスケール
変換する場合はスケール変換マトリクスを右から掛けれ
ば良νＡ。したがって１手先座標系でスケール変換する
場合は（１）式の代りに次式を用いる。

Ｔ、Ｓ　＝Ｔｓ”Ｋｓ　　　　　　　　　−−（２）（
１）式と（２）式より ＫＴ、”＝Ｔ、”Ｋｓ ゆえに Ｋ　＝　Ｔ、’Ｋ　ｓ　（Ｔ６”）″１　　　　　・・
・・・・（３）（３）式よりＫの各項を求めれば（１）
式により手先座標系に基づく異方性スケール変換ができ
る。ただしく２）式を直接用いて演算することも可能で
ある。

作業対象物に固有の座標系に対しスケール変換を行う場
合は次のようになる。第４図に示すように１作業対象物
に固有の座標系をＳＯとし、スレーブアームのベース座
標系Ｓから作業対象物の座標系Ｓｏへの座標変換マトリ
クスをＴ６゜とすると、作業対象物の座標系からスレー
ブハンドの座標系ＳＣへの座標変換マトリクスＴ、。ｃ
は次のようになる。

Ｔ、Ｓ＝　Ｔ、ＯＴ、ＱＣ ゆえに Ｔ、Ｏｃ＝　（ＴＳ’）　−’Ｔ６５　　　　　　−・
−−−−（４）ベース座標系Ｓと作業対象物の座標系Ｓ
。との間をスレーブアームの１つのリンクと仮定し、Ｓ
ｏをこのリンクが１つ増えたスレーブアームのベース座
標系と仮定すれば、（４）式のＴ、。ｃを（１）式のＴ
、′−に置き換えれば対象物座標系でのスケール変換が
できることになる。

スケール変換して得られたＴＧｓに対し、よく知られて
いるように逆座標変換を行うことにより、スレーブアー
ムの各関節軸回転位置の目標値が得られ、得られた目標
値に従ってスレーブアーム各軸毎にサーボ制御すればマ
スタアームの手先の位置がスケール変換されてスレーブ
アームに伝達される。

一方、力については次のようになる。スレーブアームの
手首部に取り付けられた力センサは手先座標系に基づい
て手先にかかる力とモーメントを座標軸方向の成分とし
て検出する。この力成分のマトリクスを７ＧＦ５とし、
マスタアーム先端に発生する力とモーメントの手先座標
系の各軸方向成分のマトリクスを７１Ｆｍとすると Ｔ　’　Ｆ　Ｍ　：＝　Ｋ　ｌ　７６　Ｆ！−・・・・
・・（５）である。ここにに′は位置のスケール変換マ
トリクスと同様の力のスケール変換マトリクスであり、
一般には と表される。ここで、 となるように係数を定めたときは、スレーブアームの基
準座標系ＳのＸ軸方向にａ′倍、ｙ軸方向にｂ′倍、Ｚ
軸方向に０７倍された力のマトリクスがマスタアームの
力のマトリクスとなる。また、Ｔ＠Ｆｌｌよ”ＦＳＸ’
ｓ　　　　　　　　　・・・・・・（６）とすれば、力
のスケール変換はスレーブアームの手先座標系に対して
行われる。

（５）（６）式より に′　＝ＴＧＦＳＫ′　Ｓ　（Ｔ’Ｆ５）−’　　　・
・・・・・（７）力センサの検出軸が手先座標系に一致
しているときはＴＧＦＳはＴ６Ｓに置き換えることがで
きる。

次に上述した原理に基づいて、本発明の一実施例を第１
図を参照して説明する。マニピュレータは一般に６軸の
アームを備えているが１図では説明を分りやすくするた
め、３軸のアームで表して、ある。マスタアーム５は、
各関節に位置検出センサ７Ａ〜７Ｃを備え、該センサ７
八〜７Ｃの出力線はセンサ入力処理回路８に接続されて
いる。センサ入力処理回路８は前述の変換演算を行う演
算回路９に接続され、該演算回路９はサーボ制御回路１
０に接続されている。サーボ制御回路１ｏは、マスタア
ーム５の形と異なる形状のスレーブアーム６の各関節に
設けられたアクチュエータ１２Ａ〜１２Ｇおよび位置検
出センサＩＩＡ〜１１Ｃに接続されており、前記演算回
路９から出力されるスレーブアーム６の各関節軸の回転
位置目標値と、位置検出センサＩＬＡ〜１１Ｃから出力
される信号に基づいてアクチュエータ１２Ａ〜１２Ｃの
動作を制御する。演算回路９には、スケール変換マトリ
クスの要素のデータを記憶している記憶回路１３が接続
され、演算回路９は記憶回路１３からスケール変換マト
リクスの要素のデータを読み出して、スケール変換演算
を行う。記憶回路１３には該記憶回路１３のデータを書
きかえるデータ入力装置１４が接続されており、データ
入力装置１４は人間が操作できる入力器として、例えば
キーボード、ジョイスティック、マウス、可変抵抗器。

タッチスクリーン、ディジタイザ、スイッチ、音声入力
装置などが適用できる。記憶回路１３には。

また、計算機１５が接続され、該計算機１５の出力によ
り、記憶回路１３にデータを書きこみ、あるいは書き換
えることができる。演算回路９には、さらに表示装置１
６が接続されており、座標変換のデータや、手先座標系
の位置や姿勢、スケール変換係数の値などが文字や画像
情報として表示される。マニピュレータのオペレータは
表示された情報を見ながら１作業内容に適したスケール
変換係数をデータ入力装置を介して設定する。なお、作
業内容とその作業に適したスケール変換係数をあらかじ
め外部の計算機１５や記憶回路１３に設定しておき、こ
れをデータ入力装置１４で選定しても良い。

次に前述したセンサ人力処理回路８の具体的な構成を第
５図に示す。この図において位置検出センサ７Ａ〜７Ｃ
としては回転式パルス発生器を用いている。この位置検
出センサ７Ａ〜７Ｃからは９０’位相のずれた［１のパ
ルス信号すなわちＡ相及びＢ相が回転角に応じて発生す
る。この信号は方向判別回路８Ａに入力されて回転角の
方向判別を行う。一方Ａ相またはＢ相の信号がカウンタ
８Ｂに入力され、パルス数をカラン１〜する。前記の方
向判別回路８Ａより出力される方向信号８Ｃはカウンタ
８Ｂに入力され、パルス数の増減の切換えを行う。した
がって、カウンタ８Ｂの値は回転角の増減に対応して増
減するので、カウンタ８Ｂの出力８Ｄを外部から読み込
むことによって回転角を検知することができる。

第６図は演算回路９の具体的な構成例を示す。

この中にはデータの入出力制御及び加減算等を行うプロ
セッサ９Ａ、三角関数表やマニピュレータのリンクパラ
メータなどのデータを格納するメモリ９Ｂ、掛算器９Ｃ
及び割算器９Ｄがバス回路９Ｅで接続されている。さら
にこのバス回路９Ｅにはシリアルまたはパラレルのイン
タフェイス回路９Ｆ、９Ｇが接続されている。インタフ
ェイス回路９Ｇには位置検出センサのセンサ入力処理回
路８およびサーボ制御回路１ｏが接続されている。

プロセッサ９Ａはバス回路９Ｅを介してバス回路９Ｅに
接続されている全ての機器にアクセスし、データを処理
することができる。

第７図はサーボ制御回路１０の具体例を示す。

演算回路９からの出力は減算器１０Ａを通過した後、デ
ジタルアナログ変換回路１０Ｂを介してアナログ信号に
変換され、アクチュエータ１２Ａ〜１２Ｃに入力される
。アクチュエータ１２Ａ〜１２Ｃはアームの関節を駆動
するとともに位置検出センサＩＩＡ〜ＩＩＣを回転させ
る６位置検出センサ１１Ａ〜ＩＩＣからの出力信号はイ
ンタフェース回路１０Ｃに人力され、角度信号を発生し
、第６図に示すパラレルインタフェース回路９Ｇを介し
てプロセッサ９Ａに読み取られ、また、減算器に入力さ
れる。減算器１０Ａの出力は演算回路９から出力される
目標値信号と位置検出センサ１１Ａ〜１１Ｃとによりイ
ンタフェイス回路１０Ｃを介して入力される角度信号の
差となるので、この差が零になるようにアクチュエータ
１２Ａ〜１２Ｃが駆動される。これによってスレーブア
ーム６の関節角を目標値に一致させることができる。

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

マスタアーム５を動作させると、マスタアーム５の各関
節角は位置検出センサ７Ａ〜７Ｃにより検出される。こ
の検出信号はセンサ入力処理回路８を介して演算回路９
に入力される。演算回路９はマスタアーム５の手先座標
系ＭＣのマスタアーム基準座標系Ｍに対する相対位置関
係を座標変換マトリクスＴｇ”として演算し、またマス
グアー４５０手先運動に対するスレーブアーム６の手先
の運動の寸法比率すなわちスケール変換マトリクスにの
データを記憶回路から読み出す。そして、演算回路９は
マスタアーム５の座標変換マトリクスＴ６′に対しスケ
ール変換演算を行ない、スレーブアーム座標変換マトリ
クスＴ−を得る。次に、スレーブアーム６の手先座標系
ＳＣのスレーブアーム基準座標系Ｓに対する相対位置が
スレーブアーム座標変換マトリクスＴ−に一致するよう
にしたときのスレーブアーム６の各関節軸目標値を逆座
標変換演算により求め、これをサーボ制御回路１０に出
力する。サーボ制御回路１０はアクチュエータ１２Ａ〜
１２Ｇを駆動する。これにより、マスタアーム５の手先
の運動をスケール変換してスレーブアーム６の手先の運
動に伝達することができる。この結果、各アームの可動
領域内では、アームの形状に係りなく、マスタアーム５
の運動を、等方性あるいは異方性に任意に拡大または縮
少してスレーブアーム６に伝えることができ、マスタア
ーム５の操作に対してスレーブアーム６をある方向では
細かく運動させ、他の方向では粗雑であるが大きく動作
を与えることができる。演算手順を第８図に示す。

第９図にスケール変換データの入力方法の一例を示す６
表示装置１６には３次元グラフィクス装置を用い、マニ
ピュレータの手先の略図と手先座標系１７が表示されて
いる。手先座標系１７の各軸の長さはスケール変換比率
を示しており、さらにスケール変換比率を数値でも表示
している。データ入力装置としてはマウス１８とキーボ
ード１９を用いている。キーボードでデータ入力コマン
ドをキーインし、マウスを用いて各軸のスケール変換比
率を調整し、設定する。マウスの代りにポテンショメー
タやジョイスティックを用いたり、キーボードから直接
数値をキーインしても良い。

次に本発明の装置の第２の実施例を説明する。

この実施例を説明するに先立って、この実施例の原理を
第１０図を用いて説明する。第１図と同様、第１０図で
も３軸アームとして表現されているが、実際には６軸で
ある。この実施例はマスタアーム５の手先の位■の微少
変位に対し、スケール変換演算を行ったものを、スレー
ブアーム６の手先の微少変位としてスレーブアーム６に
伝えるようにしたものである。そして、第２図に示す実
施例と同様にマスタアーム５の基準座標系をＭ、基準座
標系Ｍからマスタアーム５の手先までの座標変換マトリ
クスをＴ６′、スケール変換演算のためのスケール変換
７１ヘリクスをＫとする。またスレーブアーム６の基準
座標変換マトリクスをＴ６ｓとすると、座標変換演算の
手順は次のようになる。

すなわち、マスタアーム５の各リンクパラメタとその各
関節軸の回転位置から座標変換マトリクスＴ♂が求めら
れる。またスレーブアーム６の各リンクパラメタとその
手先の位置を示す座標変換マトリクスＴ６Ｓが与えられ
れば、スレーブアーム６の各関節軸回転位置の目標値が
求められる。

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

ｄＴ、＝ＪｄＱ　　　　　　　　　　　　　・・・・・
・（８）（Ｊニヤコピ行列） 今、マスタアーム５を微少変位させたとき、その各関節
軸の回転位置の変化をｄＱ’″とし、マスタアーム５の
ヤコビ行列をＪ”としたとき、マスタアーム５の手先の
微少変位ｄＴ♂は次の式から得られる。

ｄＴ♂＝Ｊ”・ｄＱ’″　　　　　　　　・・・・・・
（９）ここで、ｄ　ＴＧ”をスケール変換してスレーブ
アーム６の手先の微少変位ｄＴｔを次式から得る。

ｄ　Ｔ−＝　Ｋ　ｄ　Ｔ＠”　　　　　　　　　・・・
・・・（１０）ここで、手先座標系でスケール変換を行
うときは（３）式のＫを用いる。

次にスレーブアーム６の各関節軸回転位置の微少変位ｄ
Ｑ３をスレーブアーム６のヤコビ行列ＪＳの逆行列（Ｊ
ｓ）−”を解くことにより求める。すなわち、 ｄＱ”＝（ＪＳ）″１ｄＴ−・・・・・・（１１）上式
で得られたスレーブアーム６各関節軸回転位置の微少変
位ｄＱｓをスレーブアーム６の各関節軸回転位置に加え
、これをスレーブアーム６の各関節軸回転位置のサーボ
制御回路の目標値とする。

次に上述した原理にもとづいて本発明の制御装置の第２
の実施例を第１１図を用いて説明する。

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

上述した本発明の制御装置の第２の実施例における演算
回路の演算処理動作を第１２図を用％Ｘて説明する。

最初に、初期位置でスタートすると、マスタアーム５の
各関節の初期値を読み込んでおく。次にマスタアーム５
とスレーブアーム６の関節角を人力し、前回のデータと
の差から関節角（関節軸の回転位置、以下関節角という
）の変化量ｄＱ”を求める。次にテーブルを参照して三
角関数を求め、マスタアーム５のヤコビ行列Ｊ′″を計
算する。関節角変化量ｃｌＱ″″とヤコビ行列Ｊ１から
マスタアーム５の手先変位ｄＴ２を求める。スケール変
換定数Ｋを入力する。スケール変換を手先座標系で行う
場合は（３）式よりＫの値を求めておく。マスタアーム
の手先変位ｄＴ２にＫを乗じてスレーブアームの手先変
位ｄ　ＴＧ５を求める。次にスレーブアームのヤコビ逆
行列（Ｊｓ）−１を求める。このｄＴ、Ｓに（Ｊｓ）−
１を乗じることによりスレーブアームの各関節角変位ｄ
Ｑ！′を求めスレーブアームの関節角ＱｓとｄＱｓの和
をとってスレーブアームの各サーボ系に結果を出力する
。上記手続きを運転終了まで繰返し実行する。

力の異方性スケール変換の演算手順を第１３図に示す。

まず、マスクとスレーブの関節角を入力し、次にスレー
ブの力センサよりデータを入力する。さらに、第９図に
示すようなデータ入力手段により設定されたスケール変
換データを入力する。スケール変換を手先座標系にて行
う場合はＴｔ（Ｔ、５）　−１を求めてに′を計算する
。基準座標系の場合はに′を設定する。さらに、Ｔ″Ｆ
１１を演算する。マスタアーム先端の力Ｔ’Ｆ”とマス
タアームの各関節軸のトルクでとの関係はよく知られて
いるようにヤコビアンの転置行列により次式で求まる。

τ：ＪｌＩＴ１６Ｆｌ′　　　　　　　　　・・・・・
・（１２）（１２）式で求めたでをマスタアーム５の各
関節軸の駆動制御回路に出力すればマスタアーム５の先
端に力Ｔ１Ｆ′″が加わる。以上の計算を繰返し実行す
れば、異方性スケール変換をほどこした力のフィードバ
ックが得られる。

〔発明の効果〕

請求項１に記載の本発明によれば、マスタアームの動き
に対してスレーブアームの動きを異方性のスケール変換
を行って制御するので、微細な操作を要する方向のスレ
ーブアームの動きをマスタアームの動きよりも小さくで
きるとともに、異方性の方向、大きさを作業の内容によ
って任意に変化させることが可能となり１作業の内容に
応じたマニピュレータの操作特性とする効果がある。

請求項２に記載の本発明によれば、マスタアーム手先の
変位量に応じたスレーブアーム手先の変位量を得ること
ができ、マニピュレータ起動時に、マスタアームとスレ
ーブアームの位置が基準座標に対して対応する位置にな
いとき、スレーブアームが暴走するのを防ぐ効果がある
。

請求項３に記載の本発明によれば、スレーブアーム先端
に加わる反力をスケール変換してマスタアームにフィー
ドバックすることが可能になり、オペレータの操作感覚
を向上させる効果がある。

請求項４に記載の本発明によれば請求項１に記載の発明
と同様の効果が得られる。

請求項５に記載の本発明によれば、請求項２に記載の発
明と同様の効果が得られる。

【図面の簡単な説明】

第１図は本発明の第１の実施例の構成を示すブロック図
、第２図は本発明の動作原理を示す概念図、第３図は本
発明の第１の実施例の原理を示す概念図、第４図は本発
明の応用例を示す説明図。 第５図、第６図、第７図は本発明の実施例の制御回路の
ブロック図、第８図は本発明の実施例の演算手順を示す
フロー図、第９図は本発明の実施例を示す斜視図、第１
０図は本発明の第２の実施例の原理を示す概念図、第１
１図は本発明の第２の実施例を示すブロック図、第１２
図は本発明の第２の実施例の演算手順を示すフロー図、
第１３図は本発明の実施例の演算手順を示すフロー図、
第１４図は従来技術の例を示す側面図であり、第１５図
は従来技術の例を示す斜視図である。 ５・・・マスタアーム、６・・・スレーブアーム、７ａ
〜７ｃ、ｌｌａ〜ｌｌｃ・・・位置検出手段（位置検出
センサ）、９・・・演算回路、１０・・・サーボ制御回
路。 １２ａ〜１２ｃ・・・アクチュエータ、１３・・・記憶
回路、１４・・・データ入力装置、２０・・・差分回路
、２１・・増分回路。

Claims (1)

1. 【特許請求の範囲】 １、マスタアームの基部の座標系に対するマスタアーム
   手先の位置関係を示す座標変換マトリクスを求める手順
   と、得られた座標変換マトリクスにその左側もしくは右
   側から異方性のスケール変換マトリクスを掛け、得られ
   たマトリクスを、スレーブアームの基部の座標系に対す
   るスレーブアーム手先の座標系の位置関係を示す座標変
   換マトリクスとする手順と、前記演算で得られたスレー
   ブアームの基部の座標系に対するスレーブアーム手先の
   座標系の位置関係を示す座標変換マトリクスから、スレ
   ーブアームの各関節軸の回転位置を演算して求める手順
   と、前記異方性のスケール変換マトリクスの要素の値を
   変更する手順と、を備えたマニピュレータの制御方法 ２、マスタアームの基部の座標系に対するマスタアーム
   手先の位置関係を示す座標変換マトリクスの微小変化を
   示すマトリクスを求める手順と、得られた微小変化を示
   す座標変換マトリクスにその左側もしくは右側から異方
   性のスケール変換マトリクスを掛け、得られたマトリク
   スを、スレーブアームの基部の座標系に対するスレーブ
   アーム手先の座標系の位置関係を示す座標変換マトリク
   スの微小変化を示すマトリクスとする手順と、前記演算
   で得られたスレーブアームの基部の座標系に対するスレ
   ーブアーム手先の座標系の位置関係を示す座標変換マト
   リクスの微小変化を示すマトリクスから、スレーブアー
   ムの各関節軸の回転位置の微小変化を演算して求める手
   順と、前記異方性のスケール変換マトリクスの要素の値
   を変更する手順と、を備えたマニピュレータの制御方法 ３、マスタアームとスレーブアームの各関節軸の回転位
   置を検出する手順と、スレーブアームの先端が受ける反
   力を各座標軸方向の成分のマトリクスとして検出する手
   順と、該マトリクスにスケール変換マトリクスを掛けて
   マスタアーム先端に加わる力の各座標軸方向の成分のマ
   トリクスを得る手順と、前記得られたマスタアーム先端
   に加わる力のマトリクスからマスタアーム各関節軸に加
   わるべきトルクを算出する手順と、を備えているマニピ
   ュレータの制御方法 ４、マスタアームの各関節軸の回転位置を検出し信号と
   して出力する位置検出手段と、該位置検出手段の出力信
   号に基づいてマスタアームの基部の座標系に対するマス
   タアーム手先の位置関係を示す座標変換マトリクスを演
   算して求め、該座標変換マトリクスに異方性のスケール
   変換マトリクスを掛けて得られたマトリクスに基づいて
   スレーブアーム各関節軸の回転位置の目標値を演算出力
   する演算回路と、該演算回路が出力する前記目標値とス
   レーブアームの各関節軸の回転位置を検出する位置検出
   手段の出力とに基づいてスレーブアームのアクチュエー
   タを制御するサーボ制御回路と、前記演算回路に接続し
   て設けられ前記異方性スケール変換マトリクスの要素の
   値を記憶する記憶回路と、該記憶回路に接続して設けら
   れ該記憶回路の記憶内容を書き換えるデータ入力装置と
   を備えたマニピュレータの制御装置。 ５、マスタアームの各関節軸の回転位置を検出し信号と
   して出力する位置検出手段と、該位置検出手段に接続し
   て設けられマスタアームの関節軸の回転位置の初期位置
   からの変化量を検出する差分回路と、該変化量に基づい
   てマスタアーム手先位置を示すマトリクスを演算し、該
   マトリクスにスケール変換マトリクスを掛けてスレーブ
   アーム手先位置を示すマトリクスを求め、該マトリクス
   に基づいてスレーブアーム各関節軸回転位置の変化分を
   演算する演算回路と、演算回路が出力する前記変化分を
   スレーブアームの関節軸回転位置の現在目標値に加算し
   て出力する増分回路と、該増分回路が出力する値とスレ
   ーブアームの各関節軸の回転位置を検出する位置検出手
   段の出力とに基づいてスレーブアームのアクチュエータ
   を制御するサーボ制御回路と、前記演算回路に接続して
   設けられ前記スケール変換マトリクスの要素の値を記憶
   する記憶回路と、該記憶回路に接続して設けられ該記憶
   回路の記憶内容を書き換えるデータ入力装置とを備えた
   マニピュレータの制御装置。