JP2645866B2 - マニピュレータの制御方法および装置 - Google Patents
マニピュレータの制御方法および装置Info
- Publication number
- JP2645866B2 JP2645866B2 JP63221852A JP22185288A JP2645866B2 JP 2645866 B2 JP2645866 B2 JP 2645866B2 JP 63221852 A JP63221852 A JP 63221852A JP 22185288 A JP22185288 A JP 22185288A JP 2645866 B2 JP2645866 B2 JP 2645866B2
- Authority
- JP
- Japan
- Prior art keywords
- matrix
- arm
- slave arm
- circuit
- master arm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Description
【発明の詳細な説明】 〔産業上の利用分野〕 本発明はマニピュレータの制御方法および装置に係
り、特に遠隔操作で作業を行うのに好適なマスタ・スレ
ーブマニピュレータの制御方法および装置に関する。
り、特に遠隔操作で作業を行うのに好適なマスタ・スレ
ーブマニピュレータの制御方法および装置に関する。
マスタ・スレーブマニピュレータは放射性物質を遠隔
操作で取り扱うために生まれた技術である。従来は同じ
軸構成または相似形状の２本のマニピュレータアームを
用い、対応する各軸の値が常に等しくなるようにサーボ
制御することによって２本のアーム全体の運動を常に等
しくさせ、運動を伝達するものであった。このようなサ
ーボ制御方式マスタ・スレーブマニピュレータはスレー
ブアームがマスタアームの動作を逐一模倣する機能しか
無く、種々の使用条件に対し柔軟に対応することができ
ない。これに対し、マスタアームとスレーブアームを計
算機を介して結合し、異なる軸構成や構造のアームの手
先の運動を一致させて運動伝達を行う計算機を用いた演
算制御方式マニピュレータが開発されている。この一例
は、コンピュータズ・イン・メカニカルエンジニアリン
グ、1983年７月号第48頁から第60頁（Computers in Mec
hanical Engineering, July 1983, pp.48〜60）に論じ
られている。
操作で取り扱うために生まれた技術である。従来は同じ
軸構成または相似形状の２本のマニピュレータアームを
用い、対応する各軸の値が常に等しくなるようにサーボ
制御することによって２本のアーム全体の運動を常に等
しくさせ、運動を伝達するものであった。このようなサ
ーボ制御方式マスタ・スレーブマニピュレータはスレー
ブアームがマスタアームの動作を逐一模倣する機能しか
無く、種々の使用条件に対し柔軟に対応することができ
ない。これに対し、マスタアームとスレーブアームを計
算機を介して結合し、異なる軸構成や構造のアームの手
先の運動を一致させて運動伝達を行う計算機を用いた演
算制御方式マニピュレータが開発されている。この一例
は、コンピュータズ・イン・メカニカルエンジニアリン
グ、1983年７月号第48頁から第60頁（Computers in Mec
hanical Engineering, July 1983, pp.48〜60）に論じ
られている。
このような演算制御方式のマニピュレータでは、マス
タアームとスレーブアームの長さ比には無関係に、伝達
される運動の大きさの比率を設定することができる。こ
の点については第４回日本ロボット学会学術講演会予稿
集の第83頁から第84頁に論じられている。
タアームとスレーブアームの長さ比には無関係に、伝達
される運動の大きさの比率を設定することができる。こ
の点については第４回日本ロボット学会学術講演会予稿
集の第83頁から第84頁に論じられている。
上記の従来技術はオペレータの手の動きをマスタアー
ムを介してスレーブアームに正確に伝達することが目標
であり、いかにしてオペレータの手の動きを忠実にスレ
ーブアームに再現できるかが技術開発の最大の課題であ
った。しかしながら、人間の腕とは構造も機械特性も異
なるマニピュレータアームを用いて人間と同じ動作を正
確に再現することには自ら限界があり、操作しやすいマ
スタ・スレーブマニピュレータの実現はなかなか困難で
あった。
ムを介してスレーブアームに正確に伝達することが目標
であり、いかにしてオペレータの手の動きを忠実にスレ
ーブアームに再現できるかが技術開発の最大の課題であ
った。しかしながら、人間の腕とは構造も機械特性も異
なるマニピュレータアームを用いて人間と同じ動作を正
確に再現することには自ら限界があり、操作しやすいマ
スタ・スレーブマニピュレータの実現はなかなか困難で
あった。
ところで、マニピュレータで行う作業は作業内容にら
って特定の異方性を持つ場合がある。例えば、第14図に
示すようにハンド１で棒状部材２を把持し穴３へ挿入す
る作業では、穴への挿入方向の運動に対し、挿入方向に
垂直の方向は微細な位置調製が必要である。すなわち、
穴の挿入方向とそれに垂直な方向では必要な位置決め精
度や速度が異なる。従来の方法では全方向に等質な運動
伝達であるため、穴に垂直な方向の位置調製操作をマス
タアームで操作しやすくするためにマスタアームの手先
の運動を縮小してスレーブアームに伝達するようスケー
ル変換すれば、穴の挿入方向の運動も縮小されてしまう
ので作業の能率が悪い。逆に挿入方向の運動に適するよ
うにスケール変換すれば、穴に垂直方向の位置調整が難
しくなる。
って特定の異方性を持つ場合がある。例えば、第14図に
示すようにハンド１で棒状部材２を把持し穴３へ挿入す
る作業では、穴への挿入方向の運動に対し、挿入方向に
垂直の方向は微細な位置調製が必要である。すなわち、
穴の挿入方向とそれに垂直な方向では必要な位置決め精
度や速度が異なる。従来の方法では全方向に等質な運動
伝達であるため、穴に垂直な方向の位置調製操作をマス
タアームで操作しやすくするためにマスタアームの手先
の運動を縮小してスレーブアームに伝達するようスケー
ル変換すれば、穴の挿入方向の運動も縮小されてしまう
ので作業の能率が悪い。逆に挿入方向の運動に適するよ
うにスケール変換すれば、穴に垂直方向の位置調整が難
しくなる。
また、第15図に示すようなスプレーガン４を用いた塗
装作業では例えば横方向の移動距離は大きいが、縦方向
の移動距離は少さいというような運動の異方性がある。
このような場合にも横方向の動きに合せてスケール変換
を行えば、縦方向の位置の調整が難しくなり、塗装のラ
インが乱れる恐れがある。逆に縦方向の動きに合せてス
ケール変換すれば、横方向の動作範囲が不足する恐れが
あり、問題である。
装作業では例えば横方向の移動距離は大きいが、縦方向
の移動距離は少さいというような運動の異方性がある。
このような場合にも横方向の動きに合せてスケール変換
を行えば、縦方向の位置の調整が難しくなり、塗装のラ
インが乱れる恐れがある。逆に縦方向の動きに合せてス
ケール変換すれば、横方向の動作範囲が不足する恐れが
あり、問題である。
マスタ・スレーブマニピュレータは一般にスレーブア
ームの先端にかかる反力をマスタアームで再現してオペ
レータに伝達するバイラテラル性を持っている。第14図
に示すような挿入作業では棒状部材２が穴に斜めに挿入
されることにより生ずるかじりを防止しなければならな
い。このため、棒状部材に横方向に働く微妙な力やモー
メントをオペレータに伝達する必要がある。このために
はスレーブアームに働く力を力センサを用いて検知し、
これに力のスケール変換を施して拡大して力を伝達する
ことが有効と思われるが、従来のように等方的にスケー
ル変換を行えば、挿入方向の反力に対しても過大な力が
マスタアームに返ってくることになるので、少しの摩擦
抵抗があっても棒状部材１を穴に挿入することができな
くなり、問題である。
ームの先端にかかる反力をマスタアームで再現してオペ
レータに伝達するバイラテラル性を持っている。第14図
に示すような挿入作業では棒状部材２が穴に斜めに挿入
されることにより生ずるかじりを防止しなければならな
い。このため、棒状部材に横方向に働く微妙な力やモー
メントをオペレータに伝達する必要がある。このために
はスレーブアームに働く力を力センサを用いて検知し、
これに力のスケール変換を施して拡大して力を伝達する
ことが有効と思われるが、従来のように等方的にスケー
ル変換を行えば、挿入方向の反力に対しても過大な力が
マスタアームに返ってくることになるので、少しの摩擦
抵抗があっても棒状部材１を穴に挿入することができな
くなり、問題である。
本発明の目的は、その時々の作業内容に応じてその作
業を最もやりやすいように、マスタアームの運動や力を
異方性にスケール変換してスレーブアームに伝達するこ
とにある。このとき、スケール変換はマニピュレータの
規準座標に対し行う場合と、手先座標系に対し行う場合
と作業対象物の座標系に対し行う場合がある。
業を最もやりやすいように、マスタアームの運動や力を
異方性にスケール変換してスレーブアームに伝達するこ
とにある。このとき、スケール変換はマニピュレータの
規準座標に対し行う場合と、手先座標系に対し行う場合
と作業対象物の座標系に対し行う場合がある。
上記の課題は、マスタアームの基部の座標系に対する
マスタアーム手先の位置関係を示す座標変換マトリクス
の微小変化を示すマトリクスを求める手順と、得られた
微小変化を示す座標変換マトリクスにその左側もしくは
右側から異方性のスケール変換マトリクスを掛け、得ら
れたマトリクスを、スレーブアームの基部の座標系に対
するスレーブアーム手先の座標系の位置関係を示す座標
変換マトリクスの微小変化を示すマトリクスとする手順
と、前記演算で得られたスレーブアームの基部の座標系
に対するスレーブアーム手先の座標系の位置関係を示す
座標変換マトリクスの微小変化を示すマトリクスから、
スレーブアームの各関節軸の回転位置の微小変化を演算
して求める手順と、前記異方性のスケール変換マトリク
スの要素の値を変更する手順と、を備えたマニピュレー
タの制御方法により達成される。
マスタアーム手先の位置関係を示す座標変換マトリクス
の微小変化を示すマトリクスを求める手順と、得られた
微小変化を示す座標変換マトリクスにその左側もしくは
右側から異方性のスケール変換マトリクスを掛け、得ら
れたマトリクスを、スレーブアームの基部の座標系に対
するスレーブアーム手先の座標系の位置関係を示す座標
変換マトリクスの微小変化を示すマトリクスとする手順
と、前記演算で得られたスレーブアームの基部の座標系
に対するスレーブアーム手先の座標系の位置関係を示す
座標変換マトリクスの微小変化を示すマトリクスから、
スレーブアームの各関節軸の回転位置の微小変化を演算
して求める手順と、前記異方性のスケール変換マトリク
スの要素の値を変更する手順と、を備えたマニピュレー
タの制御方法により達成される。
さらに、マスタアームとスレーブアームの各関節軸の
回転位置を検出する手順と、スレーブアームの先端が受
ける反力を各座標軸方向の成分のマトリクスとして検出
する手順と、該マトリクスにスケール変換マトリクスを
掛けてマスタアーム先端に加わる力の各座標軸方向の成
分のマトリクスをえる手順と、前記得られたマスタアー
ム先端に加わる力のマトリクスからマスタアーム各関節
軸に加わるべきトルクを算出する手順と、を備えている
マニピュレータの制御方法とすることもできる。
回転位置を検出する手順と、スレーブアームの先端が受
ける反力を各座標軸方向の成分のマトリクスとして検出
する手順と、該マトリクスにスケール変換マトリクスを
掛けてマスタアーム先端に加わる力の各座標軸方向の成
分のマトリクスをえる手順と、前記得られたマスタアー
ム先端に加わる力のマトリクスからマスタアーム各関節
軸に加わるべきトルクを算出する手順と、を備えている
マニピュレータの制御方法とすることもできる。
本発明は、また上記の課題を達成する手段として、マ
スタアームの各関節軸の回転位置を検出し信号として出
力する位置検出手段と、該位置検出手段の出力信号に基
づいてマスタアームの基部の座標系に対するマスタアー
ム手先の位置関係を示す座標変換マトリクスを演算して
求め、該座標変換マトリクスに異方性のスケール変換マ
トリクスを掛けて得られたマトリクスに基づいてスレー
ブアーム各関節軸の回転位置の目標値を演算出力する演
算回路と、該演算回路が出力する前記目標値とスレーブ
アームの各関節軸の回転位置を検出する位置検出手段の
出力とに基づいてスレーブアームのアクチュエータを制
御するサーボ制御回路と、前記演算回路に接続して設け
られた前記異方性スケール変換マトリクスの要素の値を
記憶する記憶回路と、該記憶回路に接続して設けられ該
記憶回路の記憶内容を書き換えるデータ入力装置とを備
えたマニピュレータの制御装置およびマスタアームの各
関節軸の回転位置を検出し信号として出力する位置検出
手段と、該位置検出手段に接続して設けられマスタアー
ムの関節軸の回転位置の初期位置からの変化量を検出す
る差分回路と、該変化量に基づいてマスタアーム手先位
置を示すマトリクスを演算し、該マトリクスにスケール
変換マトリクスを掛けてスレーブアーム手先位置を示す
マトリクスを求め、該マトリクスに基づいてスレーブア
ーム各関節軸回転位置の変化分を演算する演算回路と、
演算回路が出力する前記変化分をスレーブアームの関節
軸回転位置の現在目標値に加算して出力する増分回路
と、該増分回路が出力する値とスレーブアームの各関節
軸の回転位置を検出する位置検出手段の出力とに基づい
てスレーブアームのアクチュエータを制御するサーボ制
御回路と、前記演算回路に接続して設けられ前記スケー
ル変換マトリクスの要素の値を記憶する記憶回路と、該
記憶回路に接続して設けられ該記憶回路の記憶内容を書
き換えるデータ入力装置とを備えたマニピュレータの制
御装置を提供する。
スタアームの各関節軸の回転位置を検出し信号として出
力する位置検出手段と、該位置検出手段の出力信号に基
づいてマスタアームの基部の座標系に対するマスタアー
ム手先の位置関係を示す座標変換マトリクスを演算して
求め、該座標変換マトリクスに異方性のスケール変換マ
トリクスを掛けて得られたマトリクスに基づいてスレー
ブアーム各関節軸の回転位置の目標値を演算出力する演
算回路と、該演算回路が出力する前記目標値とスレーブ
アームの各関節軸の回転位置を検出する位置検出手段の
出力とに基づいてスレーブアームのアクチュエータを制
御するサーボ制御回路と、前記演算回路に接続して設け
られた前記異方性スケール変換マトリクスの要素の値を
記憶する記憶回路と、該記憶回路に接続して設けられ該
記憶回路の記憶内容を書き換えるデータ入力装置とを備
えたマニピュレータの制御装置およびマスタアームの各
関節軸の回転位置を検出し信号として出力する位置検出
手段と、該位置検出手段に接続して設けられマスタアー
ムの関節軸の回転位置の初期位置からの変化量を検出す
る差分回路と、該変化量に基づいてマスタアーム手先位
置を示すマトリクスを演算し、該マトリクスにスケール
変換マトリクスを掛けてスレーブアーム手先位置を示す
マトリクスを求め、該マトリクスに基づいてスレーブア
ーム各関節軸回転位置の変化分を演算する演算回路と、
演算回路が出力する前記変化分をスレーブアームの関節
軸回転位置の現在目標値に加算して出力する増分回路
と、該増分回路が出力する値とスレーブアームの各関節
軸の回転位置を検出する位置検出手段の出力とに基づい
てスレーブアームのアクチュエータを制御するサーボ制
御回路と、前記演算回路に接続して設けられ前記スケー
ル変換マトリクスの要素の値を記憶する記憶回路と、該
記憶回路に接続して設けられ該記憶回路の記憶内容を書
き換えるデータ入力装置とを備えたマニピュレータの制
御装置を提供する。
マスタアームの手先位置は、常に座標変換マトリクス
として求められ、求められた座標変換マトリクスに対し
ての異方性のスケール変換演算が行われ、この演算結果
にスレーブアームの手先位置が一致するように、スレー
ブアーム各関節軸の回転位置が演算され、指示される。
異方性スケール変換がなされるので、マスタアーム手先
の動きとスレーブアーム手先の動きは、方向によって動
きの比率が異なる。スケール変換演算に用いられるスケ
ール変換マトリクスの要素の値が変えられるから、マス
タアーム手先の動きとスレーブアーム手先の動きの比率
を変えることができる。
として求められ、求められた座標変換マトリクスに対し
ての異方性のスケール変換演算が行われ、この演算結果
にスレーブアームの手先位置が一致するように、スレー
ブアーム各関節軸の回転位置が演算され、指示される。
異方性スケール変換がなされるので、マスタアーム手先
の動きとスレーブアーム手先の動きは、方向によって動
きの比率が異なる。スケール変換演算に用いられるスケ
ール変換マトリクスの要素の値が変えられるから、マス
タアーム手先の動きとスレーブアーム手先の動きの比率
を変えることができる。
また、スレーブアーム先端に加わわる反力を各座標軸
方向の成分のマトリクスとして検出し、該マトリクスに
スケール変換マトリクスを掛けると、マスタアーム先端
に加わわる反力の各座標軸方向の成分のマトリクスが求
められる。マスタアーム先端に加わわる力は、マスタア
ーム各関節軸に加わわるトルクの関数であるから、マス
タアーム先端に加わわる反力の成分が算出されたら、こ
の反力に対向してバランスするために、各関節軸が発生
すべきトルクが逆に演算により求められる。
方向の成分のマトリクスとして検出し、該マトリクスに
スケール変換マトリクスを掛けると、マスタアーム先端
に加わわる反力の各座標軸方向の成分のマトリクスが求
められる。マスタアーム先端に加わわる力は、マスタア
ーム各関節軸に加わわるトルクの関数であるから、マス
タアーム先端に加わわる反力の成分が算出されたら、こ
の反力に対向してバランスするために、各関節軸が発生
すべきトルクが逆に演算により求められる。
以下本発明の第１の実施例を図面を参照して説明す
る。
る。
まず、本発明の第１の実施例を説明するに先立って本
発明の動作原理を第２図を用いて説明する。ただし、第
２図では３自由度アームとして表現されているが、実際
には６自由度である。この例ではスレーブアーム６はマ
スタアーム５と異なる構造に形成されている。いま、マ
スタアーム５の基準座標系をＭ、この基準座標系から手
先座標系MCまでの座標変換マトリクスをT6 m、スレーブ
アーム６の基準座標系をＳ、この基準座標系Ｓから手先
座標系SCまでの座標変換マトリクスをT6 Sとし、前述し
たスケール変換マトリクスをＫとすると、座標変換演算
の手順は次のようになる。
発明の動作原理を第２図を用いて説明する。ただし、第
２図では３自由度アームとして表現されているが、実際
には６自由度である。この例ではスレーブアーム６はマ
スタアーム５と異なる構造に形成されている。いま、マ
スタアーム５の基準座標系をＭ、この基準座標系から手
先座標系MCまでの座標変換マトリクスをT6 m、スレーブ
アーム６の基準座標系をＳ、この基準座標系Ｓから手先
座標系SCまでの座標変換マトリクスをT6 Sとし、前述し
たスケール変換マトリクスをＫとすると、座標変換演算
の手順は次のようになる。
よく知られているように、マニピュレータアームを構
成する各リンクの特性を表すリンクパラメータはアーム
の構造及び寸法関係より各アーム固有の情報として設計
データから得られる。このリンクパラメタと各関節軸回
転位置の現在位置情報から座標変換マトリクスT6 m及びT
6 Sが得られることはポールの「ロボットマニピュレー
タ」で詳しく論じられている。次にマスタアーム手先ま
での座標変換マトリクスT6 mとスレーブアーム手先まで
の座標変換マトリクスT6 Sとの間でのスケール変換を含
んだ座標変換演算は次の（１）式で表される。
成する各リンクの特性を表すリンクパラメータはアーム
の構造及び寸法関係より各アーム固有の情報として設計
データから得られる。このリンクパラメタと各関節軸回
転位置の現在位置情報から座標変換マトリクスT6 m及びT
6 Sが得られることはポールの「ロボットマニピュレー
タ」で詳しく論じられている。次にマスタアーム手先ま
での座標変換マトリクスT6 mとスレーブアーム手先まで
の座標変換マトリクスT6 Sとの間でのスケール変換を含
んだ座標変換演算は次の（１）式で表される。
T6 S＝KT6 m ……（１） ここでＫはスケール変換マトリクスであり、 で表される４×４のマトリクスである。ここで、 となるように係数を定めたときは、マスタアームの基準
座標系Ｍのｘ軸方向にａ倍、ｙ軸方向にｂ倍、ｚ軸方向
にｃ倍された座標変換演算マトリクスがスレーブアーム
の座標変換マトリクスとなる。ａ＝ｂ＝ｃとすれば３軸
方向に均等にスケール変換することになる。
座標系Ｍのｘ軸方向にａ倍、ｙ軸方向にｂ倍、ｚ軸方向
にｃ倍された座標変換演算マトリクスがスレーブアーム
の座標変換マトリクスとなる。ａ＝ｂ＝ｃとすれば３軸
方向に均等にスケール変換することになる。
マスタアームの基準座標系の３軸方向と異なる座標軸
についてスケール変換を行う場合は、例えば手先座標系
に対するスケール変換マトリクスは次のようになる。例
えば、第３図に示すように、スレーブアームマニピュレ
ータの基準座標系Ｓに対し、手先座標系SCまでの座標変
換マトリクスをT6 Sとし、手先座標系SCのxh軸方向にａ
倍、yh軸方向にｂ倍、zh軸方向にｃ倍すると仮定する。
ポールのロボットマニピュレータ（吉川恒夫訳）の第39
頁に論じられているように、基準座標系に対してスケー
ル変換する場合はスケール変換マトリクスを座標変換マ
トリクスの左から掛け、手先座標系に対してスケール変
換する場合はスケール変換マトリクスを右から掛ければ
良い。したがって、手先座標系でスケール変換する場合
は（１）式の代りに次式を用いる。
についてスケール変換を行う場合は、例えば手先座標系
に対するスケール変換マトリクスは次のようになる。例
えば、第３図に示すように、スレーブアームマニピュレ
ータの基準座標系Ｓに対し、手先座標系SCまでの座標変
換マトリクスをT6 Sとし、手先座標系SCのxh軸方向にａ
倍、yh軸方向にｂ倍、zh軸方向にｃ倍すると仮定する。
ポールのロボットマニピュレータ（吉川恒夫訳）の第39
頁に論じられているように、基準座標系に対してスケー
ル変換する場合はスケール変換マトリクスを座標変換マ
トリクスの左から掛け、手先座標系に対してスケール変
換する場合はスケール変換マトリクスを右から掛ければ
良い。したがって、手先座標系でスケール変換する場合
は（１）式の代りに次式を用いる。
T6 S＝T6 mKs ……（２） （１）式と（２）式より KT6 m＝T6 mKs ゆえに Ｋ＝T6 mKs（T6 m）-1 ……（３） （３）式よりＫの各項を求めれば（１）式により手先座
標系に基づく異方性スケール変換ができる。ただし
（２）式を直接用いて演算することも可能である。
標系に基づく異方性スケール変換ができる。ただし
（２）式を直接用いて演算することも可能である。
作業対象物に固有の座標系に対しスケール変換を行う
場合は次のようになる。第４図に示すように、作業対象
物に固有の座標系をSoとし、スレーブアームのベース座
標系Ｓから作業対象物の座標系Soへの座標変換マトリク
スをT6 Oとすると、作業対象物の座標系からスレーブア
ームの座標系SCへの座標変換マトリクスT6 OCは次のよう
になる。
場合は次のようになる。第４図に示すように、作業対象
物に固有の座標系をSoとし、スレーブアームのベース座
標系Ｓから作業対象物の座標系Soへの座標変換マトリク
スをT6 Oとすると、作業対象物の座標系からスレーブア
ームの座標系SCへの座標変換マトリクスT6 OCは次のよう
になる。
T6 S＝T6 OT6 OC ゆえに T6 OC＝（T6 O）-1T6 S ……（４） ベース座標系Ｓと作業対象物の座標系Soとの間をスレ
ーブアームの１つのリンクと仮定し、Soをこのリンクが
１つ増えたスレーブアームのベース座標系と仮定すれ
ば、（４）式のT6 OCを（１）式のT6 Sに置き換えれば対
象物座標系でのスケール変換ができることになる。
ーブアームの１つのリンクと仮定し、Soをこのリンクが
１つ増えたスレーブアームのベース座標系と仮定すれ
ば、（４）式のT6 OCを（１）式のT6 Sに置き換えれば対
象物座標系でのスケール変換ができることになる。
スケール変換して得られたT6 Sに対し、よく知られて
いるように逆座標変化を行うことにより、スレーブアー
ムの各関節軸回転位置の目標値が得られ、得られた目標
値に従ってスレーブアーム各軸毎にサーボ制御すればマ
スタアームの手先の位置がスケール変換されてスレーブ
アームに伝達される。
いるように逆座標変化を行うことにより、スレーブアー
ムの各関節軸回転位置の目標値が得られ、得られた目標
値に従ってスレーブアーム各軸毎にサーボ制御すればマ
スタアームの手先の位置がスケール変換されてスレーブ
アームに伝達される。
一方、力については次のようになる。スレーブアーム
の手首部に取り付けられた力センサは手先座標系に基づ
いて手先にかかる力とモーメントを座標軸方向の成分と
して検出する。この力成分のマトリクスをT6FSとし、マ
スタアーム先端に発生する力とモーメントの手先座標系
の各軸方向成分のマトリクスをT6FmとするとT6 Fm＝Ｋ′T6FS ……（５） である。ここにＫ′は位置スケール変換マトリクスと同
様の力のスケール変換マトリクスであり、一般には と表される。ここで、 となるように係数を定めたときは、スレーブアームの基
準座標系Ｓのｘ軸方向にａ′倍、ｙ軸方向にｂ′倍、ｚ
軸方向にｃ′倍された力のマトリクスがマスタアームの
力のマトリクスとなる。また、T6 Fm＝T6FSＫ′ｓ ……（６） とすれば、力のスケール変換はスレーブアームの手先座
標系に対して行われる。
の手首部に取り付けられた力センサは手先座標系に基づ
いて手先にかかる力とモーメントを座標軸方向の成分と
して検出する。この力成分のマトリクスをT6FSとし、マ
スタアーム先端に発生する力とモーメントの手先座標系
の各軸方向成分のマトリクスをT6FmとするとT6 Fm＝Ｋ′T6FS ……（５） である。ここにＫ′は位置スケール変換マトリクスと同
様の力のスケール変換マトリクスであり、一般には と表される。ここで、 となるように係数を定めたときは、スレーブアームの基
準座標系Ｓのｘ軸方向にａ′倍、ｙ軸方向にｂ′倍、ｚ
軸方向にｃ′倍された力のマトリクスがマスタアームの
力のマトリクスとなる。また、T6 Fm＝T6FSＫ′ｓ ……（６） とすれば、力のスケール変換はスレーブアームの手先座
標系に対して行われる。
（５）（６）式より Ｋ′＝T6FSＫ′ｓ（T6FS）-1 ……（７） 力センサの検出軸が手先座標系に一致しているときは
T6FSはT6 Sに置き換えることができる。
T6FSはT6 Sに置き換えることができる。
次に上述した原理に基づいて、本発明の位置実施例を
第１図を参照して説明する。マニピュレータは一般に６
軸のアームを備えているが、図では説明を分りやすくす
るため、３軸のアームで表してある。マスタアーム５
は、各関節に位置検出センサ7A〜7Cを備え、該センサ7A
〜7Cの出力線はセンサ入力処理回路８に接続されてい
る。センサ入力処理回路８は前述の変換演算を行う演算
回路９に接続され、該演算回路９はサーボ制御回路10に
接続されている。サーボ制御回路10は、マスタアーム５
の形と異なる形状のスレーブアーム６の各関節に設けら
れたアクチュエータ12A〜12Cおよび位置検出センサ11A
〜11Cに接続されており、前記演算回路９から出力され
るスレーブアーム６の各関節軸の回転位置目標値と、位
置検出センサ11A〜11Cから出力される信号に基づいてア
クチュエータ12A〜12Cの動作を制御する。演算回路９に
は、スケール変換マトリクスの要素のデータを記憶して
いる記憶回路13が接続され、演算回路９は記憶回路13か
らスケール変換マトリクスの要素のデータを読み出し
て、スケール変換演算を行う。記憶回路13には該記憶回
路13のデータを書きかえるデータ入力装置14が接続され
ており、データ入力装置14は人間が操作できる入力器と
して、例えばキーボード，ジョイスティック，マウス，
可変抵抗器，タッチスクリーン，ディジタイザ，スイッ
チ，音声入力装置などが適用できる。記憶回路13には、
また、計算機15が接続され、該計算機15の出力により、
記憶回路13にデータを書きこみ、あるいは書き換えるこ
とができる。演算回路９には、さらに表示装置16が接続
されており、座標変換のデータや、手先座標系の位置や
姿勢、スケール変換係数の値などが文字や画像情報とし
て表示される。マニピュレータのオペレータは表示され
た情報を見ながら、作業内容に適したスケール変換係数
をデータ入力装置を介して設定する。なお、作業内容と
その作業に適したスケール変換係数をあらかじめ外部の
計算機15や記憶回路13に設定しておき、これをデータ入
力装置14で選定しても良い。
第１図を参照して説明する。マニピュレータは一般に６
軸のアームを備えているが、図では説明を分りやすくす
るため、３軸のアームで表してある。マスタアーム５
は、各関節に位置検出センサ7A〜7Cを備え、該センサ7A
〜7Cの出力線はセンサ入力処理回路８に接続されてい
る。センサ入力処理回路８は前述の変換演算を行う演算
回路９に接続され、該演算回路９はサーボ制御回路10に
接続されている。サーボ制御回路10は、マスタアーム５
の形と異なる形状のスレーブアーム６の各関節に設けら
れたアクチュエータ12A〜12Cおよび位置検出センサ11A
〜11Cに接続されており、前記演算回路９から出力され
るスレーブアーム６の各関節軸の回転位置目標値と、位
置検出センサ11A〜11Cから出力される信号に基づいてア
クチュエータ12A〜12Cの動作を制御する。演算回路９に
は、スケール変換マトリクスの要素のデータを記憶して
いる記憶回路13が接続され、演算回路９は記憶回路13か
らスケール変換マトリクスの要素のデータを読み出し
て、スケール変換演算を行う。記憶回路13には該記憶回
路13のデータを書きかえるデータ入力装置14が接続され
ており、データ入力装置14は人間が操作できる入力器と
して、例えばキーボード，ジョイスティック，マウス，
可変抵抗器，タッチスクリーン，ディジタイザ，スイッ
チ，音声入力装置などが適用できる。記憶回路13には、
また、計算機15が接続され、該計算機15の出力により、
記憶回路13にデータを書きこみ、あるいは書き換えるこ
とができる。演算回路９には、さらに表示装置16が接続
されており、座標変換のデータや、手先座標系の位置や
姿勢、スケール変換係数の値などが文字や画像情報とし
て表示される。マニピュレータのオペレータは表示され
た情報を見ながら、作業内容に適したスケール変換係数
をデータ入力装置を介して設定する。なお、作業内容と
その作業に適したスケール変換係数をあらかじめ外部の
計算機15や記憶回路13に設定しておき、これをデータ入
力装置14で選定しても良い。
次に前述したセンサ入力処理回路８の具体的な構成を
第５図に示す。この図において位置検出センサ7A〜7Cと
しては回転式パルス発生器を用いている。この位置検出
センサ7A〜7Cからは90℃位相のずれた１組のパルス信号
すなわちＡ相及びＢ相が回転角に応じて発生する。この
信号は方向判別回路8Aに入力されて回転角の方向判別を
行う。一方Ａ相またはＢ相の信号がカウンタ8Bに入力さ
れ、パルス数をカウントする。前記の方向判別回路8Aよ
り出力される方向信号8CはカウンタＢに入力され、パル
ス数の増減の切換え行う。したがって、カウンタ8Bの値
は回転角の増減に対応して増減するので、カウンタ8Bの
出力8Dを外部から読み込むことによって回転角を検知す
ることができる。
第５図に示す。この図において位置検出センサ7A〜7Cと
しては回転式パルス発生器を用いている。この位置検出
センサ7A〜7Cからは90℃位相のずれた１組のパルス信号
すなわちＡ相及びＢ相が回転角に応じて発生する。この
信号は方向判別回路8Aに入力されて回転角の方向判別を
行う。一方Ａ相またはＢ相の信号がカウンタ8Bに入力さ
れ、パルス数をカウントする。前記の方向判別回路8Aよ
り出力される方向信号8CはカウンタＢに入力され、パル
ス数の増減の切換え行う。したがって、カウンタ8Bの値
は回転角の増減に対応して増減するので、カウンタ8Bの
出力8Dを外部から読み込むことによって回転角を検知す
ることができる。
第６図は演算回路９の具体的な構成例を示す。
この中にはデータの入出力制御及び加減算等を行うプロ
セッサ9A、三角関数表やマニピュレータのリンクパラメ
ータなどのデータを格納するメモリ9B、掛算器9C及び割
算器9Dがバス回路9Eで接続されている。さらにこのバス
回路9Eにはシリアルまたはパラレルのインタフェイス回
路9F,9Gが接続されている。インタフェイス回路9Gには
位置検出センサのセンサ入力処理回路８およびサーボ制
御回路10が接続されている。プロセッサ9Aはバス回路9E
を介してバス回路9Eに接続されている全ての機器にアク
セスし、データを処理することができる。
セッサ9A、三角関数表やマニピュレータのリンクパラメ
ータなどのデータを格納するメモリ9B、掛算器9C及び割
算器9Dがバス回路9Eで接続されている。さらにこのバス
回路9Eにはシリアルまたはパラレルのインタフェイス回
路9F,9Gが接続されている。インタフェイス回路9Gには
位置検出センサのセンサ入力処理回路８およびサーボ制
御回路10が接続されている。プロセッサ9Aはバス回路9E
を介してバス回路9Eに接続されている全ての機器にアク
セスし、データを処理することができる。
第７図はサーボ制御回路10の具体例を示す。演算回路
９からの出力は減算器10Aを通過した後、デジタルアナ
ログ変換回路10Bを介してアナログ信号に変換され、ア
クチュエータ12A〜12Cに入力される。アクチュエータ12
A〜12Cはアームの関節を駆動するとともに位置検出セン
サ11A〜11Cを回転させる。位置検出センサ11A〜11Cから
の出力信号はインタフェース回路10Cに入力され、角度
信号を発生し、第６図に示すパラレルインタフェース回
路9Gを介してプロセッサ9Aに読み取られ、また、減算器
に入力される。減算器10Aの出力は演算回路９から出力
される目標値信号と位置検出センサ11A〜11Cとによりイ
ンタフェイス回路10Cを介して入力される角度信号の差
となるので、この差が零になるようにアクチュエータ12
A〜12Cが駆動される。これによってスレーブアーム６の
関節角を目標値に一致させることができる。
９からの出力は減算器10Aを通過した後、デジタルアナ
ログ変換回路10Bを介してアナログ信号に変換され、ア
クチュエータ12A〜12Cに入力される。アクチュエータ12
A〜12Cはアームの関節を駆動するとともに位置検出セン
サ11A〜11Cを回転させる。位置検出センサ11A〜11Cから
の出力信号はインタフェース回路10Cに入力され、角度
信号を発生し、第６図に示すパラレルインタフェース回
路9Gを介してプロセッサ9Aに読み取られ、また、減算器
に入力される。減算器10Aの出力は演算回路９から出力
される目標値信号と位置検出センサ11A〜11Cとによりイ
ンタフェイス回路10Cを介して入力される角度信号の差
となるので、この差が零になるようにアクチュエータ12
A〜12Cが駆動される。これによってスレーブアーム６の
関節角を目標値に一致させることができる。
次に上述した本発明の装置の一実施例の動作を説明す
る。
る。
マスタアーム５を動作させると、マスタアーム５の各
関節角は位置検出センサ7A〜7Cにより検出される。この
検出信号はセンサ入力処理回路８を介して演算回路９に
入力される。演算回路９はマスタアーム５の手先座標系
MCのマスタアーム基準座標系Ｍに対する相対位置関係を
座標変換マトリクスT6 mとして演算し、またマスタアー
ム５の手先運動に対するスレーブアーム６の手先の運動
の寸法比率すなわちスケール変換マトリクスＫのデータ
を記憶回路から読み出す。そして、演算回路９はマスタ
アーム５の座標変換マトリクスT6 mに対しスケール変換
演算を行ない、スレーブアーム座標変換マトリクスT6 S
を得る。次に、スレーブアーム６の手先座標系SCのスレ
ーブアーム基準座標系Ｓに対する相対位置がスレーブア
ーム座標変換マトリクスT6 Sに一致するようにしたとき
のスレーブアーム６の各関節軸目標値を逆座標変換演算
により求め、これをサーボ制御回路10に出力する。サー
ボ制御回路10はアクチュエータ12A〜12Cを駆動する。こ
れにより、マスタアーム５の手先の運動をスケール変換
してスレーブアーム６の手先の運動に伝達することがで
きる。この結果、各アームの可動領域内では、アームの
形状に係りなく、マスタアーム５の運動を、等方性ある
いは異方性に任意に拡大または縮小してスレーブアーム
６に伝えることができ、マスタアーム５の操作に対して
スレーブアーム６をある方向では細かく運動させ、他の
方向では粗雑であるが大きく動作を与えることができ
る。演算手順を第８図に示す。
関節角は位置検出センサ7A〜7Cにより検出される。この
検出信号はセンサ入力処理回路８を介して演算回路９に
入力される。演算回路９はマスタアーム５の手先座標系
MCのマスタアーム基準座標系Ｍに対する相対位置関係を
座標変換マトリクスT6 mとして演算し、またマスタアー
ム５の手先運動に対するスレーブアーム６の手先の運動
の寸法比率すなわちスケール変換マトリクスＫのデータ
を記憶回路から読み出す。そして、演算回路９はマスタ
アーム５の座標変換マトリクスT6 mに対しスケール変換
演算を行ない、スレーブアーム座標変換マトリクスT6 S
を得る。次に、スレーブアーム６の手先座標系SCのスレ
ーブアーム基準座標系Ｓに対する相対位置がスレーブア
ーム座標変換マトリクスT6 Sに一致するようにしたとき
のスレーブアーム６の各関節軸目標値を逆座標変換演算
により求め、これをサーボ制御回路10に出力する。サー
ボ制御回路10はアクチュエータ12A〜12Cを駆動する。こ
れにより、マスタアーム５の手先の運動をスケール変換
してスレーブアーム６の手先の運動に伝達することがで
きる。この結果、各アームの可動領域内では、アームの
形状に係りなく、マスタアーム５の運動を、等方性ある
いは異方性に任意に拡大または縮小してスレーブアーム
６に伝えることができ、マスタアーム５の操作に対して
スレーブアーム６をある方向では細かく運動させ、他の
方向では粗雑であるが大きく動作を与えることができ
る。演算手順を第８図に示す。
第９図にスケール変換データの入力方法の一例を示
す。表示装置16には３次元グラフィクス装置を用い、マ
ニピュレータの手先の略図と手先座標系17が表示されて
いる。手先座標系17の各軸の長さはスケール変換比率を
示しており、さらにスケール変換比率を数値でも表示し
ている。データ入力装置としてはマウス18とキーボード
19を用いている。キーボードでデータ入力コマンドをキ
ーインし、マウスを用いて各軸のスケール変換比率を調
整し、設定する。マウスの代りにポテンショメータやジ
ョイスティックを用いたり、キーボードから直接数値を
キーインしても良い。
す。表示装置16には３次元グラフィクス装置を用い、マ
ニピュレータの手先の略図と手先座標系17が表示されて
いる。手先座標系17の各軸の長さはスケール変換比率を
示しており、さらにスケール変換比率を数値でも表示し
ている。データ入力装置としてはマウス18とキーボード
19を用いている。キーボードでデータ入力コマンドをキ
ーインし、マウスを用いて各軸のスケール変換比率を調
整し、設定する。マウスの代りにポテンショメータやジ
ョイスティックを用いたり、キーボードから直接数値を
キーインしても良い。
次に本発明の装置の第２の実施例を説明する。
この実施例を説明するに先立って、この実施例の原理
を第10図を用いて説明する。第１図と同様、第10図でも
３軸アームとして表現されているが、実際には６軸であ
る。この実施例はマスタアーム５の手先の位置の微少変
位に対し、スケール変換演算を行ったものを、スレーブ
アーム６の手先の微少変位としてスレーブアーム６に伝
えるようにしたものである。そして、第２図に示す実施
例と同様にマスタアーム５の基準座標系をＭ、基準座標
系Ｍからマスタアーム５の手先までの座標変換マトリク
スをT6 m、スケール変換演算のためのスケール変換マト
リクスをＫとする。またスレーブアーム６の基準座標変
換マトリクスをT6 Sとすると、座標変換演算の手順は次
のようになる。すなわち、マスタアーム５の各リンクパ
ラメタとその各関節軸の回転位置から座標変換マトリク
スT6 mが求められる。またスレーブアーム６の各リンク
パラメタとその手先の位置を示す座標変換マトリクスT6
Sが与えられれば、スレーブアーム６の各関節軸回転位
置の目標値が求められる。
を第10図を用いて説明する。第１図と同様、第10図でも
３軸アームとして表現されているが、実際には６軸であ
る。この実施例はマスタアーム５の手先の位置の微少変
位に対し、スケール変換演算を行ったものを、スレーブ
アーム６の手先の微少変位としてスレーブアーム６に伝
えるようにしたものである。そして、第２図に示す実施
例と同様にマスタアーム５の基準座標系をＭ、基準座標
系Ｍからマスタアーム５の手先までの座標変換マトリク
スをT6 m、スケール変換演算のためのスケール変換マト
リクスをＫとする。またスレーブアーム６の基準座標変
換マトリクスをT6 Sとすると、座標変換演算の手順は次
のようになる。すなわち、マスタアーム５の各リンクパ
ラメタとその各関節軸の回転位置から座標変換マトリク
スT6 mが求められる。またスレーブアーム６の各リンク
パラメタとその手先の位置を示す座標変換マトリクスT6
Sが与えられれば、スレーブアーム６の各関節軸回転位
置の目標値が求められる。
今ある時点でマスタアーム５とスレーブアーム６の運
動を同期させるものとすると、手先の位置の微少変位dT
6とマニピュレータ各軸回転位置の微少変位dQとの間に
は次の関係がある。
動を同期させるものとすると、手先の位置の微少変位dT
6とマニピュレータ各軸回転位置の微少変位dQとの間に
は次の関係がある。
dT6＝JdQ ……（８） （J:ヤコビ行列） 今、マスタアーム５を微少変位させたとき、その各関
節軸の回転位置の変化をdQmとし、マスタアーム５のヤ
コビ行列をJmとしたとき、マスタアーム５の手先の微少
変位dT6 mは次の式から得られる。
節軸の回転位置の変化をdQmとし、マスタアーム５のヤ
コビ行列をJmとしたとき、マスタアーム５の手先の微少
変位dT6 mは次の式から得られる。
dT6 m＝Jm・dQm ……（９） ここで、dT6 mをスケール変換してスレーブアーム６の手
先の微少変位dT6 Sを次式から得る。
先の微少変位dT6 Sを次式から得る。
dT6 S＝KdT6 m ……（10） ここで、手先座標系でスケール変換を行うときは（３）
の式のＫを用いる。
の式のＫを用いる。
次にスレーブアーム６の各関節軸回転位置の微少変位
dQSをスレーブアーム６のヤコビ行列JSの逆行列（JS）
-1を解くことにより求める。すなわち、 dQS＝（JS）-1dT6 S ……（11） 上式で得られたスレーブアーム６各関節軸回転位置の
微少変位dQSをスレーブアーム６の各関節軸回転位置に
加え、これをスレーブアーム６の各関節軸回転位置のサ
ーボ制御回路の目標値とする。
dQSをスレーブアーム６のヤコビ行列JSの逆行列（JS）
-1を解くことにより求める。すなわち、 dQS＝（JS）-1dT6 S ……（11） 上式で得られたスレーブアーム６各関節軸回転位置の
微少変位dQSをスレーブアーム６の各関節軸回転位置に
加え、これをスレーブアーム６の各関節軸回転位置のサ
ーボ制御回路の目標値とする。
次に上述した原理にもとづいて本発明の制御装置の第
２の実施例を第11図を用いて説明する。この図において
第１図と同符号のものは同一部分または相当する部分で
ある。20は差分回路、21は増分回路である。前述した差
分回路20はサンプリングタイムにおけるセンサ7A〜7Cの
センサ信号の変化分を検出する。演算回路９は前述した
（９）式〜（11）式で示された計算を行い、スレーブア
ーム６の各関節軸の変化分を求め、この変化分を増分回
路21に出力する。増分回路21はスレーブアーム６の各関
節軸毎に現在目標値に演算回路９で求めた変化分を加算
し、サーボ制御回路10に入力する。サーボ制御回路10は
スレーブアームの各関節軸に設けたアクチュエータ12A
〜12Cを駆動する。これにより、スレーブアーム６は駆
動されるが、その移動量は位置検出センサ11A〜11Cによ
って検出され、サーボ制御回路10にフィードバックされ
る。その結果、マスタアーム５の手先の運動を異方性ま
たは等方性にスケール変換してスレーブアームの手先に
伝達することができる。
２の実施例を第11図を用いて説明する。この図において
第１図と同符号のものは同一部分または相当する部分で
ある。20は差分回路、21は増分回路である。前述した差
分回路20はサンプリングタイムにおけるセンサ7A〜7Cの
センサ信号の変化分を検出する。演算回路９は前述した
（９）式〜（11）式で示された計算を行い、スレーブア
ーム６の各関節軸の変化分を求め、この変化分を増分回
路21に出力する。増分回路21はスレーブアーム６の各関
節軸毎に現在目標値に演算回路９で求めた変化分を加算
し、サーボ制御回路10に入力する。サーボ制御回路10は
スレーブアームの各関節軸に設けたアクチュエータ12A
〜12Cを駆動する。これにより、スレーブアーム６は駆
動されるが、その移動量は位置検出センサ11A〜11Cによ
って検出され、サーボ制御回路10にフィードバックされ
る。その結果、マスタアーム５の手先の運動を異方性ま
たは等方性にスケール変換してスレーブアームの手先に
伝達することができる。
上述した本発明の制御装置の第２の実施例における演
算回路の演算処理動作を第12図を用いて説明する。
算回路の演算処理動作を第12図を用いて説明する。
最初に、初期位置でスタートすると、マスタアーム５
の各関節の初期値を読み込んでおく。次にマスタアーム
５とスレーブアーム６の関節角を入力し、前回のデータ
との差から関節角（関節軸の回転位置、以下関節角とい
う）の変化量dQmを求める。次にテーブルを参照して三
角関数を求め、マスタアーム５のヤコビ行列Jmを計算す
る。関節角変化量dQmとヤコビ行列Jmからマスタアーム
５の手先変位dT6 mを求める。スケール変換定数Ｋを入力
する。スケール変換を手先座標系で行う場合は（３）式
よりＫの値を求めておく。マスタアームの手先変位dT6 m
にＫを乗じてスレーブアームの手先変位dT6 Sを求める。
次にスレーブアームのヤコビ逆行列（JS）-1を求める。
このdT6 Sに（JS）-1を乗じることによりスレーブアーム
の各関節角変位dQSを求めスレーブアームの関節角QSとd
QSの和をとってスレーブアームの各サーボ系に結果を出
力する。上記手続きを運転終了まで繰返し実行する。
の各関節の初期値を読み込んでおく。次にマスタアーム
５とスレーブアーム６の関節角を入力し、前回のデータ
との差から関節角（関節軸の回転位置、以下関節角とい
う）の変化量dQmを求める。次にテーブルを参照して三
角関数を求め、マスタアーム５のヤコビ行列Jmを計算す
る。関節角変化量dQmとヤコビ行列Jmからマスタアーム
５の手先変位dT6 mを求める。スケール変換定数Ｋを入力
する。スケール変換を手先座標系で行う場合は（３）式
よりＫの値を求めておく。マスタアームの手先変位dT6 m
にＫを乗じてスレーブアームの手先変位dT6 Sを求める。
次にスレーブアームのヤコビ逆行列（JS）-1を求める。
このdT6 Sに（JS）-1を乗じることによりスレーブアーム
の各関節角変位dQSを求めスレーブアームの関節角QSとd
QSの和をとってスレーブアームの各サーボ系に結果を出
力する。上記手続きを運転終了まで繰返し実行する。
力の異方性スケール変換の演算手順を第13図に示す。
まず、マスタとスレーブの関節角を入力し、次にスレ
ーブの力センサよりデータを入力する。さらに、第９図
に示すようなデータ入力手段により設定されたスケール
変換データを入力する。スケール変換を手先座標系にて
行う場合はT6 S，（T6 S）-1を求めてＫ′を計算する。基
準座標系の場合はＫ′を設定する。さらに、T6Fmを演算
する。マスタアーム先端の力T6Fmとマスタアームの各関
節軸のトルクτとの関係はよく知られているようにヤコ
ビアンの転置行列により次式で求まる。
ーブの力センサよりデータを入力する。さらに、第９図
に示すようなデータ入力手段により設定されたスケール
変換データを入力する。スケール変換を手先座標系にて
行う場合はT6 S，（T6 S）-1を求めてＫ′を計算する。基
準座標系の場合はＫ′を設定する。さらに、T6Fmを演算
する。マスタアーム先端の力T6Fmとマスタアームの各関
節軸のトルクτとの関係はよく知られているようにヤコ
ビアンの転置行列により次式で求まる。
τ＝Jm T T6Fm ……（12） （12）式で求めたτをマスタアーム５の各関節軸の駆
動制御回路に出力すればマスタアーム５の先端に力T6Fm
が加わる。以上の計算を繰返し実行すれば、異方性スケ
ール変換をほどこした力のフィードバックが得られる。
動制御回路に出力すればマスタアーム５の先端に力T6Fm
が加わる。以上の計算を繰返し実行すれば、異方性スケ
ール変換をほどこした力のフィードバックが得られる。
請求項１に記載の本発明によれば、マスタアーム手先
の変位量に応じたスレーブアーム手先の変位量を得るこ
とができ、マニピュレータ起動時に、マスタアームとス
レーブアームの位置が基準座標に対して対応する位置に
ないとき、スレーブアームが暴走するのを防ぐ効果があ
る。
の変位量に応じたスレーブアーム手先の変位量を得るこ
とができ、マニピュレータ起動時に、マスタアームとス
レーブアームの位置が基準座標に対して対応する位置に
ないとき、スレーブアームが暴走するのを防ぐ効果があ
る。
請求項２に記載の本発明によれば、スレーブアーム先
端に加わる反力をスケール変換してマスタアームにフィ
ードバックすることが可能になり、オペレータの操作感
覚を向上させる効果がある。
端に加わる反力をスケール変換してマスタアームにフィ
ードバックすることが可能になり、オペレータの操作感
覚を向上させる効果がある。
請求項３に記載の本発明によれば、マスタアームの動
きに対してスレーブアームの動きを異方性のスケール変
換を行って制御するので、微細な操作を要する方向のス
レーブアームの動きをマスタアームの動きよりも小さく
できるとともに、異方性の方向、大きさを作業の内容に
よって任意に変化させることが可能となり、作業の内容
に応じたマニピュレータの操作特性とする効果がある。
きに対してスレーブアームの動きを異方性のスケール変
換を行って制御するので、微細な操作を要する方向のス
レーブアームの動きをマスタアームの動きよりも小さく
できるとともに、異方性の方向、大きさを作業の内容に
よって任意に変化させることが可能となり、作業の内容
に応じたマニピュレータの操作特性とする効果がある。
請求項４に記載の本発明によれば、請求項１に記載の
発明と同様の効果が得られる。
発明と同様の効果が得られる。
第１図は本発明の第１の実施例の構成を示すブロック
図、第２図は本発明の動作原理を示す概念図、第３図は
本発明の第１の実施例の原理を示す概念図、第４図は本
発明の応用例を示す説明図、第５図、第６図、第７図は
本発明の実施例の制御回路のブロック図、第８図は本発
明の実施例の演算手順を示すフロー図、第９図は本発明
の実施例を示す斜視図、第10図は本発明の第２の実施例
の原理を示す概念図、第11図は本発明の第２の実施例を
示すブロック図、第12図は本発明の第２の実施例の演算
手順を示すフロー図、第13図は本発明の実施例の演算手
順を示すフロー図、第14図は従来技術の例を示す側面図
であり、第15図は従来技術の例を示す斜視図である。 ５……マスタアーム、６……スレーブアーム、7a〜7c,1
1a〜11c……位置検出手段（位置検出センサ）、９……
演算回路、10……サーボ制御回路、12a〜12c……アクチ
ュエータ、13……記憶回路、14……データ入力装置、20
……差分回路、21……増分回路。
図、第２図は本発明の動作原理を示す概念図、第３図は
本発明の第１の実施例の原理を示す概念図、第４図は本
発明の応用例を示す説明図、第５図、第６図、第７図は
本発明の実施例の制御回路のブロック図、第８図は本発
明の実施例の演算手順を示すフロー図、第９図は本発明
の実施例を示す斜視図、第10図は本発明の第２の実施例
の原理を示す概念図、第11図は本発明の第２の実施例を
示すブロック図、第12図は本発明の第２の実施例の演算
手順を示すフロー図、第13図は本発明の実施例の演算手
順を示すフロー図、第14図は従来技術の例を示す側面図
であり、第15図は従来技術の例を示す斜視図である。 ５……マスタアーム、６……スレーブアーム、7a〜7c,1
1a〜11c……位置検出手段（位置検出センサ）、９……
演算回路、10……サーボ制御回路、12a〜12c……アクチ
ュエータ、13……記憶回路、14……データ入力装置、20
……差分回路、21……増分回路。
───────────────────────────────────────────────────── フロントページの続き (72)発明者 青木 立 茨城県土浦市神立町502番地 株式会社 日立製作所機械研究所内 (56)参考文献 特開 昭61−252081（ＪＰ，Ａ)
Claims (4)
- 【請求項１】マスタアームの基部の座標系に対するマス
タアーム手先の位置関係を示す座標変換マトリクスの微
小変化を示すマトリクスを求める手順と、得られた微小
変化を示す座標変換マトリクスにその左側もしくは右側
から異方性のスケール変換マトリクスを掛け、得られた
マトリクスを、スレーブアームの基部の座標系に対する
スレーブアーム手先の座標系の位置関係を示す座標変換
マトリクスの微小変化を示すマトリクスとする手順と、
前記演算で得られたスレーブアームの基部の座標系に対
するスレーブアーム手先の座標系の位置関係を示す座標
変換マトリクスの微小変化を示すマトリクスから、スレ
ーブアームの各関節軸の回転位置の微小変化を演算して
求める手順と、前記異方性のスケール変換マトリクスの
要素の値を変更する手順と、を備えたマニピュレータの
制御方法。 - 【請求項２】マスタアームとスレーブアームの各関節軸
の回転位置を検出する手順と、スレーブアームの先端が
受ける反力を各座標軸方向の成分のマトリクスとして検
出する手順と、該マトリクスにスケール変換マトリクス
を掛けてマスタアーム先端に加わる力の各座標軸方向の
成分のマトリクスを得る手順と、前記得られたマスタア
ーム先端に加わる力のマトリクスからマスタアーム各関
節軸に加わるべきトルクを算出する手順と、を備えてい
るマニピュレータの制御方法。 - 【請求項３】マスタアームの各関節軸の回転位置を検出
し信号として出力する位置検出手段と、該位置検出手段
の出力信号に基づいてマスタアームの基部の座標系に対
するマスタアーム手先の位置関係を示す座標変換マトリ
クスを演算して求め、該座標変換マトリクスに異方性の
スケール変換マトリクスを掛けて得られたマトリクスに
基づいてスレーブアーム各関節軸の回転位置の目標値を
演算出力する演算回路と、該演算回路が出力する前記目
標値とスレーブアームの各関節軸の回転位置を検出する
位置検出手段の出力とに基づいてスレーブアームのアク
チュエータを制御するサーボ制御回路と、前記演算回路
に接続して設けられ前記異方性スケール変換マトリクス
の要素の値を記憶する記憶回路と、該記憶回路に接続し
て設けられ該記憶回路の記憶内容を書き換えるデータ入
力装置とを備えたマニピュレータの制御装置。 - 【請求項４】マスタアームの各関節軸の回転位置を検出
し信号として出力する位置検出手段と、該位置検出手段
に接続して設けられマスタアームの関節軸の回転位置の
初期位置からの変化量を検出する差分回路と、該変化量
に基づいてマスタアーム手先位置を示すマトリクスを演
算し、該マトリクスにスケール変換マトリクスを掛けて
スレーブアーム手先位置を示すマトリクスを求め、該マ
トリクスに基づいてスレーブアーム各関節軸回転位置の
変化分を演算する演算回路と、演算回路が出力する前記
変化分をスレーブアームの関節軸回転位置の現在目標値
に加算して出力する増分回路と、該増分回路が出力する
値とスレーブアームの各関節軸の回転位置を検出する位
置検出手段の出力とに基づいてスレーブアームのアクチ
ュエータを制御するサーボ制御回路と、前記演算回路に
接続して設けられ前記スケール変換マトリクスの要素の
値を記憶する記憶回路と、該記憶回路に接続して設けら
れ該記憶回路の記憶内容を書き換えるデータ入力装置と
を備えたマニピュレータの制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP63221852A JP2645866B2 (ja) | 1988-09-05 | 1988-09-05 | マニピュレータの制御方法および装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP63221852A JP2645866B2 (ja) | 1988-09-05 | 1988-09-05 | マニピュレータの制御方法および装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH0271979A JPH0271979A (ja) | 1990-03-12 |
| JP2645866B2 true JP2645866B2 (ja) | 1997-08-25 |
Family
ID=16773198
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP63221852A Expired - Lifetime JP2645866B2 (ja) | 1988-09-05 | 1988-09-05 | マニピュレータの制御方法および装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2645866B2 (ja) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO1995002487A1 (fr) * | 1993-07-13 | 1995-01-26 | Komatsu Ltd. | Manipulateur |
| EP0816020A4 (en) * | 1994-09-21 | 1999-04-21 | Komatsu Mfg Co Ltd | MASTER / SLAVE MANIPULATOR AND ITS CONTROL METHOD |
| JP6297784B2 (ja) * | 2013-03-21 | 2018-03-20 | 株式会社椿本チエイン | マニピュレータ装置 |
| WO2022024238A1 (ja) * | 2020-07-29 | 2022-02-03 | 日本電信電話株式会社 | 力触覚制御システム、マスタ装置、スレーブ装置、力触覚制御方法、及び力触覚制御プログラム |
| CN112659120A (zh) * | 2020-12-11 | 2021-04-16 | 山东大学 | 一种液压作业机械臂的主从遥操作及力反馈控制方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61252081A (ja) * | 1985-04-30 | 1986-11-10 | 工業技術院長 | マスタ・スレ−ブ・マニピユレ−タ |
-
1988
- 1988-09-05 JP JP63221852A patent/JP2645866B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0271979A (ja) | 1990-03-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0273273B1 (en) | Controlling apparatus of manipulator | |
| JP2664205B2 (ja) | マニピュレータ制御システム | |
| Handlykken et al. | Control system analysis and synthesis for a six degree-of-freedom universal force-reflecting hand controller | |
| US5459382A (en) | Method and system for providing a tactile virtual reality and manipulator defining an interface device therefor | |
| US5629594A (en) | Force feedback system | |
| Ueberle et al. | Design, control, and evaluation of a new 6 DOF haptic device | |
| JP2645866B2 (ja) | マニピュレータの制御方法および装置 | |
| JPH0443744B2 (ja) | ||
| JPS6179589A (ja) | ロボツト運転装置 | |
| JP2685602B2 (ja) | マスタースレーブマニピュレータシステム | |
| Peer et al. | Towards a mobile haptic interface for bimanual manipulations | |
| Peer et al. | Haptic telemanipulation with dissimilar kinematics | |
| JP3240817B2 (ja) | 速度指令による三次元座標入力方法 | |
| JP4230196B2 (ja) | 位置決め演算方法および位置決め演算装置 | |
| JP2552977B2 (ja) | 力フィードバック型多軸操作装置 | |
| JP2507449B2 (ja) | 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 | |
| JPH01222311A (ja) | 多自由度作業機械の曲面倣い制御装置 | |
| Stanczyk et al. | Development of a high-performance haptic telemanipulation system with dissimilar kinematics | |
| JP3431026B2 (ja) | 冗長軸を有する７軸以上のマニピュレータの制御方法 | |
| JPH08309680A (ja) | マニピュレータ制御システム | |
| JPH02205494A (ja) | マニピュレータ画像追従方法及び装置並びに該装置を備えたマニピュレータ装置 | |
| JPH046434A (ja) | 六軸動作モニタ装置 | |
| Lan et al. | Virtual reality application for direct-drive robot with force feedback | |
| Handlykken | 7034 Trondheim, Norway | |
| Ferreira et al. | Development of a teleoperated micromanipulation system with visual and haptic feedback |