JP2507449B2 - 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 - Google Patents
冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法Info
- Publication number
- JP2507449B2 JP2507449B2 JP62174641A JP17464187A JP2507449B2 JP 2507449 B2 JP2507449 B2 JP 2507449B2 JP 62174641 A JP62174641 A JP 62174641A JP 17464187 A JP17464187 A JP 17464187A JP 2507449 B2 JP2507449 B2 JP 2507449B2
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- joint
- master
- slave
- joints
- 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
- Control Of Position Or Direction (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、マスタ・スレーブマニピュレータに係り、
特に原子力プラント内の弁などの保守作業を遠隔で操作
するのに好適な冗長関節を有するマニピュレータに関す
る。
特に原子力プラント内の弁などの保守作業を遠隔で操作
するのに好適な冗長関節を有するマニピュレータに関す
る。
従来の装置は、マスタとスレーブのアーム間の関節数
及び関節配置が等しいものがほとんである。これは、ア
ームの関節数が６関節以下であるからであり、障害物の
回避や動作範囲の拡大等は考慮されなかつたからであ
る。冗長関節を有するアームの制御方法として、新井，
中野：異自由度マニピュレータ間におけるバイラテラル
マスタスレイブ制御；日本ロボツト学会誌,Vol.4,No5,4
80/487（1986）に記載されたものがある。これは、構造
の異なるマスタ，スレーブアームに関して、マスタアー
ムを用いてスレーブアームを制御するというものであ
る。この方法は、３関節以下のアームについて、マスタ
の関節角に対するスレーブの関節角の解を陽関数の形で
直接求めている。また、スレーブからマスタへのカフイ
ードバツク量は、マスタ関節からスレーブ関節角への変
換行列の偏微分行列を用いて、この変換行列とスレーブ
のトルクを表わすベクトルとの積によつて与えている。
及び関節配置が等しいものがほとんである。これは、ア
ームの関節数が６関節以下であるからであり、障害物の
回避や動作範囲の拡大等は考慮されなかつたからであ
る。冗長関節を有するアームの制御方法として、新井，
中野：異自由度マニピュレータ間におけるバイラテラル
マスタスレイブ制御；日本ロボツト学会誌,Vol.4,No5,4
80/487（1986）に記載されたものがある。これは、構造
の異なるマスタ，スレーブアームに関して、マスタアー
ムを用いてスレーブアームを制御するというものであ
る。この方法は、３関節以下のアームについて、マスタ
の関節角に対するスレーブの関節角の解を陽関数の形で
直接求めている。また、スレーブからマスタへのカフイ
ードバツク量は、マスタ関節からスレーブ関節角への変
換行列の偏微分行列を用いて、この変換行列とスレーブ
のトルクを表わすベクトルとの積によつて与えている。
従来の冗長関節を有するスレーブアームの制御方法
は、マスタアームによりスレーブアームの全て関節を制
御するように構成されていない。このため、スレーブア
ームを容易に且つスムーズに動作させることができな
い。
は、マスタアームによりスレーブアームの全て関節を制
御するように構成されていない。このため、スレーブア
ームを容易に且つスムーズに動作させることができな
い。
また、上記従来技術が、３関節以下のアームについて
検討されているのみで、６関節以上のアームに適用した
ときの問題点に触れていなかつた。その問題点とは、ス
レーブの位置指令は、冗長関節のあるアームでは、解析
的に陽関数の形で求めることは不可能なことであり、冗
長関節を有するアームには適用困難であつた。また、３
関節のアームの場合で、速度制御をする場合でも数10ms
の計算時間を要し、６関節以上のアームでは実時間制御
が困難であつた。
検討されているのみで、６関節以上のアームに適用した
ときの問題点に触れていなかつた。その問題点とは、ス
レーブの位置指令は、冗長関節のあるアームでは、解析
的に陽関数の形で求めることは不可能なことであり、冗
長関節を有するアームには適用困難であつた。また、３
関節のアームの場合で、速度制御をする場合でも数10ms
の計算時間を要し、６関節以上のアームでは実時間制御
が困難であつた。
本発明の目的は、マスタ側よりもスレーブ側の関節数
が多い冗長関節を有するマスタ・スレーブマニピュレー
タの動きをスムーズに且つ実時間制御できるようにする
ことにある。
が多い冗長関節を有するマスタ・スレーブマニピュレー
タの動きをスムーズに且つ実時間制御できるようにする
ことにある。
上記目的を達成する手段は、６個以上の関節を有する
マスタマニピュレータと、該マスタマニピュレータより
多い関節を有するスレーブマニピュレータと、前記マス
タマニピュレータの各関節の位置に関する前記スレーブ
マニピュレータの関節の目標値を該スレーブマニピュレ
ータの各関節に分配するように演算する手段と、該演算
手段の演算結果に基づいてスレーブマニピュレータの各
関節を駆動する手段とを備え、前記演算手段は、マスタ
マニピュレータによる目標値をスレーブマニピュレータ
の関節の角速度に変換するために用いる行列を低速周期
で計算し、更新する長周期演算部と、前記低速周期で演
算され、且つ更新される行列を用いてマスタマニピュレ
ータにより目標値をスレーブマニピュレタの各関節の角
速度に高速周期で計算して変換する短周期演算部とを有
するマスタ・スレーブマニピュレータであり、他の手段
は、マスタマニピュレータによる目標値をスレーブマニ
ピュレータの関節の角速度に変換するために用いる行列
をスレーブマニピュレータの関節の角速度の２乗和が最
小となるように低速周期で計算して更新するとともに、
前記低速周期で計算され、且つ更新される行列を用いて
マスタマニピュレータによる目標値をスレーブマニピュ
レータの各関節の角速度に高速周期で計算して変換し、
該計算された角速度に基づいてスレーブマニピュレータ
の各関節を駆動するマスタ・スレーブマニピュレータの
制御方法である。
マスタマニピュレータと、該マスタマニピュレータより
多い関節を有するスレーブマニピュレータと、前記マス
タマニピュレータの各関節の位置に関する前記スレーブ
マニピュレータの関節の目標値を該スレーブマニピュレ
ータの各関節に分配するように演算する手段と、該演算
手段の演算結果に基づいてスレーブマニピュレータの各
関節を駆動する手段とを備え、前記演算手段は、マスタ
マニピュレータによる目標値をスレーブマニピュレータ
の関節の角速度に変換するために用いる行列を低速周期
で計算し、更新する長周期演算部と、前記低速周期で演
算され、且つ更新される行列を用いてマスタマニピュレ
ータにより目標値をスレーブマニピュレタの各関節の角
速度に高速周期で計算して変換する短周期演算部とを有
するマスタ・スレーブマニピュレータであり、他の手段
は、マスタマニピュレータによる目標値をスレーブマニ
ピュレータの関節の角速度に変換するために用いる行列
をスレーブマニピュレータの関節の角速度の２乗和が最
小となるように低速周期で計算して更新するとともに、
前記低速周期で計算され、且つ更新される行列を用いて
マスタマニピュレータによる目標値をスレーブマニピュ
レータの各関節の角速度に高速周期で計算して変換し、
該計算された角速度に基づいてスレーブマニピュレータ
の各関節を駆動するマスタ・スレーブマニピュレータの
制御方法である。
マスタアームの各関節の位置に関する目標値をスレー
ブアームの各関節に分配するよう駆動制御すれば、スレ
ーブアームの全関節を制御できるので、スレーブアーム
をスムーズに動かすことが可能となる。即ち、目標値を
スレーブアームの全関節に分担させることができるの
で、各関節の移動量を少なくすることができ、スレーブ
アーム全体としてスムーズな動きとなる。そして、冗長
関節を有するスレーブアームの全関節を容易に制御でき
きるので、動作範囲を拡大でき、最適姿勢への移動が容
易となり操作性が向上する。
ブアームの各関節に分配するよう駆動制御すれば、スレ
ーブアームの全関節を制御できるので、スレーブアーム
をスムーズに動かすことが可能となる。即ち、目標値を
スレーブアームの全関節に分担させることができるの
で、各関節の移動量を少なくすることができ、スレーブ
アーム全体としてスムーズな動きとなる。そして、冗長
関節を有するスレーブアームの全関節を容易に制御でき
きるので、動作範囲を拡大でき、最適姿勢への移動が容
易となり操作性が向上する。
また、長周期演算部では、マスタマニピュレータの位
置情報であるマスタ関節角とスレーブマニピュレータの
関節角をある長いサンプリング周期で入力し、前記変換
行列の各要素を計算する。この計算に要する時間より大
きくなるように長サンプリング周期を設定する。この計
算収量毎に、短周期演算部に格納してある前記変換行列
の各要素を、更新する。短周期演算部では、短いサンプ
リング周期（制御対象を安定に制御できる周期）で、マ
スタの関節角または関節角速度と、スレーブのそれらと
を入力し、それらの値から前記変換行列を用いて、スレ
ーブの各関節駆動モータの出力指令値を計算して、その
出力指令値でスレブの関節を駆動する作用を得る。
置情報であるマスタ関節角とスレーブマニピュレータの
関節角をある長いサンプリング周期で入力し、前記変換
行列の各要素を計算する。この計算に要する時間より大
きくなるように長サンプリング周期を設定する。この計
算収量毎に、短周期演算部に格納してある前記変換行列
の各要素を、更新する。短周期演算部では、短いサンプ
リング周期（制御対象を安定に制御できる周期）で、マ
スタの関節角または関節角速度と、スレーブのそれらと
を入力し、それらの値から前記変換行列を用いて、スレ
ーブの各関節駆動モータの出力指令値を計算して、その
出力指令値でスレブの関節を駆動する作用を得る。
以下、本発明の一実施例を第１図〜第３図を用いて説
明する。マスタマニピュレータ１は、第２図に示すよう
な関節配置を有する。それぞれの関節は第２図に示す方
向に回転する。すなわち、アームの根元から第１関節，
第２関節，……，第６関節と呼ぶと、第１関節はアーム
の中心軸回りの回転、すなわちロール関節であり、第２
関節はアーム中心軸に垂直な軸回りの回転、すなわちピ
ツチ関節である。以下、第３関節はピツチ関節、第４関
節はロール関節、第５関節はピツチ関節、第６関節はロ
ール関節である。それぞれの関節角を根元から順に
１、２，……，６と表わし、これらのマスタ関節
角を ここに、Ｔは転置行列を表わす。
明する。マスタマニピュレータ１は、第２図に示すよう
な関節配置を有する。それぞれの関節は第２図に示す方
向に回転する。すなわち、アームの根元から第１関節，
第２関節，……，第６関節と呼ぶと、第１関節はアーム
の中心軸回りの回転、すなわちロール関節であり、第２
関節はアーム中心軸に垂直な軸回りの回転、すなわちピ
ツチ関節である。以下、第３関節はピツチ関節、第４関
節はロール関節、第５関節はピツチ関節、第６関節はロ
ール関節である。それぞれの関節角を根元から順に
１、２，……，６と表わし、これらのマスタ関節
角を ここに、Ｔは転置行列を表わす。
とベクトルで表わす。同様にスレーブマニピュレータ２
は第２図に示すような関節配置を有し、根元から順に第
１関節，第２関節，……，第９関節と呼ぶことにする。
これらの関節は、第１関節がロール関節，第２関節はピ
ツチ関節，第３関節はピツチ関節，第４関節はロール関
節，第５関節はピツチ関節，第６関節はピツチ関節，第
７関節はロール関節、第８関節はピツチ関節，第９関節
はロール関節である。スレーブ関節角を根元から順にθ
１，θ２，……，θ９と表わし、スレーブ関節角ベクト
ルｓを、ｓ ＝（θ１，θ２，……、θ９）Ｔ …（２） と表わす。
は第２図に示すような関節配置を有し、根元から順に第
１関節，第２関節，……，第９関節と呼ぶことにする。
これらの関節は、第１関節がロール関節，第２関節はピ
ツチ関節，第３関節はピツチ関節，第４関節はロール関
節，第５関節はピツチ関節，第６関節はピツチ関節，第
７関節はロール関節、第８関節はピツチ関節，第９関節
はロール関節である。スレーブ関節角を根元から順にθ
１，θ２，……，θ９と表わし、スレーブ関節角ベクト
ルｓを、ｓ ＝（θ１，θ２，……、θ９）Ｔ …（２） と表わす。
スレーブの関節には、第３図に示すようにモータ10,
減速機11、トルクセンサ12,回転角検出器13とが装着さ
れており、それぞれの入力軸がモータ10の軸の回転と同
期して回転するように構成されている。モータ10の軸の
回転に伴い、関節軸14の回りにリンク15が回転し、その
回転角が回転角検出器13で検出される。回転角13の出力
は、位置カウンタ16と、F/Vコンバータ17とに入力され
る。位置カウンタ16は、関節角ベクトルｓを出力す
る。一方、F/Vコンバータ17は角速度ベクトルｓ を出力し、自動速度制御系（AVR）４に入力される。自
動速度制御系４は、角速度目標ベクトルsr sr ＝（ωsr1，ωsr2，……，ωsr9）Ｔ …（４） と、角速度ベクトルｓを入力し、モータ指令ベクトル
＝V1,V2,……,V9)Tを出力する。自動速度制御系４
は、減算器18,アンプ19とから構成され、減算器18で実
行されたベクトルsr−ｓに比例した出力がアンプ19
を介してモータに与えられるように構成されている。す
なわち、角速度目標ベクトルsrに等しくなるように、
関節の角速度ｓが自動制御される。尚、第３図では図
式的に各要素は１ケずつ描かれているが、実際の系では
関節の数、すなわち９組ある。
減速機11、トルクセンサ12,回転角検出器13とが装着さ
れており、それぞれの入力軸がモータ10の軸の回転と同
期して回転するように構成されている。モータ10の軸の
回転に伴い、関節軸14の回りにリンク15が回転し、その
回転角が回転角検出器13で検出される。回転角13の出力
は、位置カウンタ16と、F/Vコンバータ17とに入力され
る。位置カウンタ16は、関節角ベクトルｓを出力す
る。一方、F/Vコンバータ17は角速度ベクトルｓ を出力し、自動速度制御系（AVR）４に入力される。自
動速度制御系４は、角速度目標ベクトルsr sr ＝（ωsr1，ωsr2，……，ωsr9）Ｔ …（４） と、角速度ベクトルｓを入力し、モータ指令ベクトル
＝V1,V2,……,V9)Tを出力する。自動速度制御系４
は、減算器18,アンプ19とから構成され、減算器18で実
行されたベクトルsr−ｓに比例した出力がアンプ19
を介してモータに与えられるように構成されている。す
なわち、角速度目標ベクトルsrに等しくなるように、
関節の角速度ｓが自動制御される。尚、第３図では図
式的に各要素は１ケずつ描かれているが、実際の系では
関節の数、すなわち９組ある。
マスタの関節の構成についても、スレーブと同様であ
る。但し、関節の数が６ケであり、また、モータ等の要
素の仕様についた、馬力等の点で異なつている。
る。但し、関節の数が６ケであり、また、モータ等の要
素の仕様についた、馬力等の点で異なつている。
また、第１図中に10で示されるモータ部のＪはスレー
ブアームのそれぞれの関節を動すためのモータの軸まわ
りの慣性モーメントを表わす行列である。
ブアームのそれぞれの関節を動すためのモータの軸まわ
りの慣性モーメントを表わす行列である。
マニピュレータの制御装置は、その自動速度制御系４
を下位のハードウエアとして、上位に自動制御系４への
指令値srを与える制御系３を有する。制御系３は、短
周期演算部5,長周期演算部6,インタフエース７とから構
成される。インタフエース７は、キーボード20,デイス
プレイ装置21,処理装置22とから 構成される（第４図
参照）。長周期演算部６は、マスタ関節角ベクトル スレーブ関節角ベクトルｓ、インタフエース７の出力
行列ｕとをサンプリング周期TLで入力し、マスタからス
レーブへの角度ベクトル変換行列Ｇ、または角速度ベク
トル変換行列 を計算し、同じサンプリング周期TLで短周期演算部５
に、Ｇまたは を出力するように構成されている。長周期演算部６は、
角度ベクトル変換行列Ｇの演算部35と角速度ベクトル変
換行列 の演算部36とから構成される。
を下位のハードウエアとして、上位に自動制御系４への
指令値srを与える制御系３を有する。制御系３は、短
周期演算部5,長周期演算部6,インタフエース７とから構
成される。インタフエース７は、キーボード20,デイス
プレイ装置21,処理装置22とから 構成される（第４図
参照）。長周期演算部６は、マスタ関節角ベクトル スレーブ関節角ベクトルｓ、インタフエース７の出力
行列ｕとをサンプリング周期TLで入力し、マスタからス
レーブへの角度ベクトル変換行列Ｇ、または角速度ベク
トル変換行列 を計算し、同じサンプリング周期TLで短周期演算部５
に、Ｇまたは を出力するように構成されている。長周期演算部６は、
角度ベクトル変換行列Ｇの演算部35と角速度ベクトル変
換行列 の演算部36とから構成される。
短周期演算部５は、上記２つの変換行列に応じた２つ
の演算手段37,38を有し、内部にあるスイツチS1，S2、
及び加算器30とによつて、いずれか一方の演算手段が洗
濯されて切替わるように構成されている。１つの演算手
段37は変換行列Ｇに関するもので、乗算器31、ホルダ3
9,減算器32,乗算器33とから構成される。乗算器31は長
周期演算部６の演算部35の出力Ｇとマスタ角度ベクトル との積を出力する。この出力Ｇ ∈R9x1はホルダ39の入力される。ここにRixjはｉ行ｊ列
の次元を有する行列の集合を表わす。ｉかｊのいずれか
一方が１のき、ベクトルの集合を表わし、１でないｉ又
はｊがその次元を表わす。
の演算手段37,38を有し、内部にあるスイツチS1，S2、
及び加算器30とによつて、いずれか一方の演算手段が洗
濯されて切替わるように構成されている。１つの演算手
段37は変換行列Ｇに関するもので、乗算器31、ホルダ3
9,減算器32,乗算器33とから構成される。乗算器31は長
周期演算部６の演算部35の出力Ｇとマスタ角度ベクトル との積を出力する。この出力Ｇ ∈R9x1はホルダ39の入力される。ここにRixjはｉ行ｊ列
の次元を有する行列の集合を表わす。ｉかｊのいずれか
一方が１のき、ベクトルの集合を表わし、１でないｉ又
はｊがその次元を表わす。
ホルダ39では、次式に示す演算が実行され、関節角指
令ベクトルを出力する。
令ベクトルを出力する。
＝（ξ１，ξ２，……，ξ９）Ｔ ∈R9x1 …（５） ただし、ξ0iは、Ｇの要素が変更される直前のスレー
ブ関節角θi0と変更直後の の第ｉ成分である i0との差で与えられる。
ブ関節角θi0と変更直後の の第ｉ成分である i0との差で与えられる。
減算器32では関節角指令ベクトルとスレーブ関節角
ベクトルｓとの減算が実行される。減算器32の出力
は、係数器33の係数行列Kp が乗算される。この乗算結果
は、演算手段37の出力（スレーブ第２角速度指令ベクト
ルs2）であり、スイツチS2を介して加算器30に入力さ
れるように構成されている。演算手段37の入出力関係を
数式で表わすと次のようになる。
ベクトルｓとの減算が実行される。減算器32の出力
は、係数器33の係数行列Kp が乗算される。この乗算結果
は、演算手段37の出力（スレーブ第２角速度指令ベクト
ルs2）であり、スイツチS2を介して加算器30に入力さ
れるように構成されている。演算手段37の入出力関係を
数式で表わすと次のようになる。
ここに、 s2＝（ωs21，ωs22，……，ωs29）Ｔ∈R9x1 …
（９） ０＝（ξ01，ξ02，……，ξ09）Ｔ ∈R9x1 …（10） Kp ＝diag（Kp1，Kp2，……，Kps）∈R9x1 …（12） Kpi（ｉ＝1,……,9）は、スレーブ関節角制御系の応
答仕様で定まる定数である。diag（ ）は（ ）内の成
分が対角要素である対角行列である。
（９） ０＝（ξ01，ξ02，……，ξ09）Ｔ ∈R9x1 …（10） Kp ＝diag（Kp1，Kp2，……，Kps）∈R9x1 …（12） Kpi（ｉ＝1,……,9）は、スレーブ関節角制御系の応
答仕様で定まる定数である。diag（ ）は（ ）内の成
分が対角要素である対角行列である。
他の演算手段38は、第１図，第５図に示すように変換
行列 に関するもので、マスタ座標変換部23,スレーブ座標変
換部24,減算器25,マスタ逆座標変換部26,マスタ・スレ
ーブ角速度ベクトル変換部27,係数器28とから構成され
る。演算手段38は、マスタ角度ベクトル スレーブ角度ベクトルｓを入力とし、スレーブ第１角
速度指令ベクトルs1を出力する。演算手段38では次に
示す演算が実行される。
行列 に関するもので、マスタ座標変換部23,スレーブ座標変
換部24,減算器25,マスタ逆座標変換部26,マスタ・スレ
ーブ角速度ベクトル変換部27,係数器28とから構成され
る。演算手段38は、マスタ角度ベクトル スレーブ角度ベクトルｓを入力とし、スレーブ第１角
速度指令ベクトルs1を出力する。演算手段38では次に
示す演算が実行される。
ｍ＝Jm -1・Δ∈R6x1 …（14） s1＝Kv ・s0 ∈R9x1 …（16） ここに、０はマスタ，スレーブ間の位置ベクトルの
オフセツト量である。ｍ ∈R6x1は、マスタ座標変換部23によつて実行される計算
結果を示し、物理的には、マスタ・アームの関節角ベク
トルが であるときのアーム先端の位置と姿勢を表わすベクトル
である。
オフセツト量である。ｍ ∈R6x1は、マスタ座標変換部23によつて実行される計算
結果を示し、物理的には、マスタ・アームの関節角ベク
トルが であるときのアーム先端の位置と姿勢を表わすベクトル
である。
同様に、ｓ（ｓ）∈R6x1はスレーブ座標変換部24
によつて実行される計算結果を示し、物理的意味は、ス
レーブアームの関節角ベクトルがｓであるときのスレ
ーブアーム先端の位置と姿勢を表わすベクトルである。
従つて、Δは、（（13）式によれば）マスタとスレー
ブのアーム先端の位置誤差及び姿勢誤差を表わしてい
る。（13）式の演算は減算器25によつて実行される。
（14）式の演算はマスタ逆座標変換部26で実行される。
ここに、Jm -1は、ｍ を角度ベクトル で偏微分したヤコビアン行列の逆行列を表わし、次式で
与えられる。
によつて実行される計算結果を示し、物理的意味は、ス
レーブアームの関節角ベクトルがｓであるときのスレ
ーブアーム先端の位置と姿勢を表わすベクトルである。
従つて、Δは、（（13）式によれば）マスタとスレー
ブのアーム先端の位置誤差及び姿勢誤差を表わしてい
る。（13）式の演算は減算器25によつて実行される。
（14）式の演算はマスタ逆座標変換部26で実行される。
ここに、Jm -1は、ｍ を角度ベクトル で偏微分したヤコビアン行列の逆行列を表わし、次式で
与えられる。
ｍはマスタアームの関節角速度ベクトルに換算し
た、マスタ・スレーブの先端の位置，姿勢の誤差であ
る。（15）式は、マスタ・スレーブ角速度ベクトル変換
部27で実行される。ここに、 は長期演算部６の角速度変換行列演算部36によつて与え
られ、周期TLで更新される。ただし、 である。
た、マスタ・スレーブの先端の位置，姿勢の誤差であ
る。（15）式は、マスタ・スレーブ角速度ベクトル変換
部27で実行される。ここに、 は長期演算部６の角速度変換行列演算部36によつて与え
られ、周期TLで更新される。ただし、 である。
（16）式の演算は、係数器28で実行される。ここに、
Kv は（12）式と同様に次式で与えられる速度ゲイン行列
であり、対角行列である。Kv ＝diag（Kv1，KV2，……，Kv9）∈R9x9 …（19） ここに、Kvi（ｉ＝1,……,9）はスレーブ関節に関す
る角速度制御系の応答仕様によつて定まる定数である。
Kv は（12）式と同様に次式で与えられる速度ゲイン行列
であり、対角行列である。Kv ＝diag（Kv1，KV2，……，Kv9）∈R9x9 …（19） ここに、Kvi（ｉ＝1,……,9）はスレーブ関節に関す
る角速度制御系の応答仕様によつて定まる定数である。
（13）式〜（16）式の演算は制御周期Ts以下に実行さ
れる。但し、 Ts＜TL …（20） である。
れる。但し、 Ts＜TL …（20） である。
次に、演算部35のＧの計算方法について説明する。
（11）式で示す変換行列Ｇの要素Gijを次式で与える。
（11）式で示す変換行列Ｇの要素Gijを次式で与える。
Gij＝δvi・δiw・Kw …（21） ここに、δijはクロネツカのデルタであり、 で表わされる関数であり、Kw（ｗ＝1,……,6）は、マス
タの関節角をスレーブの関節角に伝達する比例定数であ
る。v,wはそれぞれスレーブ，マスタの関節を表わす番
号である。このv,w,Kwは、インタフエース７から、入力
ｕとして、 で与えられる。
タの関節角をスレーブの関節角に伝達する比例定数であ
る。v,wはそれぞれスレーブ，マスタの関節を表わす番
号である。このv,w,Kwは、インタフエース７から、入力
ｕとして、 で与えられる。
次に入力ｕの与え方について説明するマスタアーム１
を操作するオペレータは、マスタアーム１の関節角ｗ
と一対一で対応させて動かしたいスレーブアーム２の関
節角θｖを決め、キーボード20から、ｗ及びｖを入力す
る。同時に、マスタからスレーブに伝達する比例定数Kw
を入力する。この比例定数Kwは、通常１である。このと
きマスタ関節角の変化量Δｗとスレーブ関節角の変位
量Δθｖとが等しくなる。Ｋｗ＞１では、マスタの変位
量Δｗは、スレーブに拡大して伝えられ、少しのマス
タアームの移動に対してスレーブアームを大きく移動さ
せる事に好適である。逆にKw＜１では、マスタの変位量
Δｗは縮小してスレーブに伝達され、スレーブを小さ
く移動させる事に好適である。
を操作するオペレータは、マスタアーム１の関節角ｗ
と一対一で対応させて動かしたいスレーブアーム２の関
節角θｖを決め、キーボード20から、ｗ及びｖを入力す
る。同時に、マスタからスレーブに伝達する比例定数Kw
を入力する。この比例定数Kwは、通常１である。このと
きマスタ関節角の変化量Δｗとスレーブ関節角の変位
量Δθｖとが等しくなる。Ｋｗ＞１では、マスタの変位
量Δｗは、スレーブに拡大して伝えられ、少しのマス
タアームの移動に対してスレーブアームを大きく移動さ
せる事に好適である。逆にKw＜１では、マスタの変位量
Δｗは縮小してスレーブに伝達され、スレーブを小さ
く移動させる事に好適である。
このようにして、マスタの６関節に対して、（w1，
v1，Kw1），（w2，v2，Kw2），……，（w6，v6，Kw6）
を入力する。処理装置22では、キーボード20から入力さ
れたデータが更新されるたびに、入力ｕの出力を更新し
て出力する。同時に、デイスプレイ装置21に、出力す
る。デイスプレイ装置21では、第４図に示すように、マ
スタ，スレーブアームの線画が画面上に表示される。マ
スタとスレーブの対応する関節viとwiとが同じ色に表示
され、また、異なる関節viとvjは異なつた色に表示され
る。これによつて、オペレータは入力した関節の対応関
係が誤つているか否かを即座に判断できる。尚、線画の
表示は、マスタ、スレーブの関節情報 ｓに応じて関節の回転角を移動させてもよい。また、
関節の対応関係付けは色によるものの他に引き出し線等
のものでも適用可能である。
v1，Kw1），（w2，v2，Kw2），……，（w6，v6，Kw6）
を入力する。処理装置22では、キーボード20から入力さ
れたデータが更新されるたびに、入力ｕの出力を更新し
て出力する。同時に、デイスプレイ装置21に、出力す
る。デイスプレイ装置21では、第４図に示すように、マ
スタ，スレーブアームの線画が画面上に表示される。マ
スタとスレーブの対応する関節viとwiとが同じ色に表示
され、また、異なる関節viとvjは異なつた色に表示され
る。これによつて、オペレータは入力した関節の対応関
係が誤つているか否かを即座に判断できる。尚、線画の
表示は、マスタ、スレーブの関節情報 ｓに応じて関節の回転角を移動させてもよい。また、
関節の対応関係付けは色によるものの他に引き出し線等
のものでも適用可能である。
以上演算を実行すると演算手段37における計算結果
s2の第ｉ関節要素ωs2iは次式で表わされる。
s2の第ｉ関節要素ωs2iは次式で表わされる。
ωs2i＝Kpi（ξ01＋Kj ｊ−θｉ） …（24） （（24）式中の記号i,jはｉ＝vl,j＝wl（ｌ＝１〜
６）で、vl，wlは上述のオペレータが定めたものであ
る。） すなわち、スレーブの関節角θｉは、オペレータが指
定したマスタ関節角ｊによつて位置に関する自動制御
が実行される。
６）で、vl，wlは上述のオペレータが定めたものであ
る。） すなわち、スレーブの関節角θｉは、オペレータが指
定したマスタ関節角ｊによつて位置に関する自動制御
が実行される。
次に、ξ0iの働きについて説明する。ξ0iがないと、
オペレータがスレーブ関節角に対応するマスタの関節角
の指定を変更すると、それまで指定されていたスレーブ
関節角の指令値が急に０になる場合（関節角が指定され
ない場合）が生じ、スレーブ関節が０の値になるように
自動制御され、スレーブアームが動く。また、新たに指
定されたマスタ関節角とスレーブの間に大きな差が生
じ、その差がなくなるようにスレーブ関節角が自動制御
され、同様にスレーブアームが動く。このように、オペ
レータが指定する関節の対応関係を変更する毎に、スレ
ーブアームが動いてしまうので、スレーブアームの有効
な位置決めが困難になる。これを防ぐため、オペレータ
がマスタ・スレーブの対応関係を変更する直前のスレー
ブ関節角の値をスレーブの関節角にオフセツト量として
加算し、変更直後のマスタ関節角の値をスレーブ関節目
標角のオフセツト量として減算してある（（７）式参
照）。
オペレータがスレーブ関節角に対応するマスタの関節角
の指定を変更すると、それまで指定されていたスレーブ
関節角の指令値が急に０になる場合（関節角が指定され
ない場合）が生じ、スレーブ関節が０の値になるように
自動制御され、スレーブアームが動く。また、新たに指
定されたマスタ関節角とスレーブの間に大きな差が生
じ、その差がなくなるようにスレーブ関節角が自動制御
され、同様にスレーブアームが動く。このように、オペ
レータが指定する関節の対応関係を変更する毎に、スレ
ーブアームが動いてしまうので、スレーブアームの有効
な位置決めが困難になる。これを防ぐため、オペレータ
がマスタ・スレーブの対応関係を変更する直前のスレー
ブ関節角の値をスレーブの関節角にオフセツト量として
加算し、変更直後のマスタ関節角の値をスレーブ関節目
標角のオフセツト量として減算してある（（７）式参
照）。
次に演算部36の の計算方法について説明する。前述のように、（13）式
から、 は、マスタとスレーブの位置誤差ベクトルを表わす。こ
の演算は減算器25と加算器40とで行なわれる演算を示し
ている。（13）式を時間ｔで微分したベクトルは次式で
与えられる。
から、 は、マスタとスレーブの位置誤差ベクトルを表わす。こ
の演算は減算器25と加算器40とで行なわれる演算を示し
ている。（13）式を時間ｔで微分したベクトルは次式で
与えられる。
ここに、 で でそれぞれ関節角 ｓに関するｍ，ｓのヤコビアン行列である。
制約条件 のもとに評価関数 を極小化する を求める。評価関数Ωの意味は、スレーブ関節の角速度
の２乗和を最小化する条件である。ただしＭ∈P9x9の対
角行列（重み行列）である。この をLagrangeの未定乗数法で求めると、 となる。すなわち、 である。Ｍ＝Ｅ（単位行列）とすると、 となる。演算部36では（29式の演算が実行される。
の２乗和を最小化する条件である。ただしＭ∈P9x9の対
角行列（重み行列）である。この をLagrangeの未定乗数法で求めると、 となる。すなわち、 である。Ｍ＝Ｅ（単位行列）とすると、 となる。演算部36では（29式の演算が実行される。
ヤコビアン行列 の計算は、ミニコンピユータで数10msを要する演算であ
るので、乗算が数十倍多い（299式は数100ms以上を必要
とする。一方、関節を安定に制御するには関節の角速度
データを数10ms以内に計算する必要がある。そこで、演
算部36の計算がサンプリング周期TL＝数100msに選び、
このサンプリング周期の間、 の行列の各要素を一定に保つことで、関節の速度データ
を数10ms以下のサンプリング周期Tsで計算できる。従つ
て、関節の安定な制御が可能になる。
るので、乗算が数十倍多い（299式は数100ms以上を必要
とする。一方、関節を安定に制御するには関節の角速度
データを数10ms以内に計算する必要がある。そこで、演
算部36の計算がサンプリング周期TL＝数100msに選び、
このサンプリング周期の間、 の行列の各要素を一定に保つことで、関節の速度データ
を数10ms以下のサンプリング周期Tsで計算できる。従つ
て、関節の安定な制御が可能になる。
尚、本実施例では９関節のスレーブアームについて述
べたが他の冗長関節をもつスレーブアームについても同
様の方法で適用できる。
べたが他の冗長関節をもつスレーブアームについても同
様の方法で適用できる。
本発明のマスタ・スレーブマニピュレータによれば、
冗長関節を有するスレーブマニピュレータをスムーズに
動作させることができ、操作性が向上する。
冗長関節を有するスレーブマニピュレータをスムーズに
動作させることができ、操作性が向上する。
また、本発明によれば、長周期、短周期の２つの制御
周期を有する演算手段を有し、長周期の演算手段に於
て、マスタアームの６関節から６関節以上のスレーブア
ーム関節を決定する演算を関節の自動制御系が必要とす
る制御周期に無関係に実行できるので、マスタからスレ
ーブに伝達する目標値の複雑な計算や、不定期にオペレ
ータから投入するコマンドに対して柔軟な対応がとれ、
冗長関節を有するスレーブアームを円滑に実時間で制御
でき、マスタスレーブマニピュレータの操作性が向上す
る効果がある。本発明のマスタ・スレーブマニピュレー
タの制御方法によれば、演算部での行列計算に際してそ
の行列の関節角速度に係る特定要素を一定に保つて演算
を高速化できるから、マスタ・スレーブマニピュレータ
を円滑に実時間で制御できる方法が提供できる。
周期を有する演算手段を有し、長周期の演算手段に於
て、マスタアームの６関節から６関節以上のスレーブア
ーム関節を決定する演算を関節の自動制御系が必要とす
る制御周期に無関係に実行できるので、マスタからスレ
ーブに伝達する目標値の複雑な計算や、不定期にオペレ
ータから投入するコマンドに対して柔軟な対応がとれ、
冗長関節を有するスレーブアームを円滑に実時間で制御
でき、マスタスレーブマニピュレータの操作性が向上す
る効果がある。本発明のマスタ・スレーブマニピュレー
タの制御方法によれば、演算部での行列計算に際してそ
の行列の関節角速度に係る特定要素を一定に保つて演算
を高速化できるから、マスタ・スレーブマニピュレータ
を円滑に実時間で制御できる方法が提供できる。
第１図は本発明の一実施例の制御ブロツク図、第２図
は、本発明の一実施例を適用するアームの記号図、第３
図は第１図の部分詳細図、第４図は第１図の部分詳細
図、第５図は第１図のうち一つの演算手段の説明する制
御ブロツク図である。 １…マスタアーム、２…スレーブアーム、５…短周期演
算部、６…長周期演算部、７…インタフエース、21…デ
イスプレイ装置、35…角度変換行列演算部、36…角速度
変換行列演算部、39…ホルダ。
は、本発明の一実施例を適用するアームの記号図、第３
図は第１図の部分詳細図、第４図は第１図の部分詳細
図、第５図は第１図のうち一つの演算手段の説明する制
御ブロツク図である。 １…マスタアーム、２…スレーブアーム、５…短周期演
算部、６…長周期演算部、７…インタフエース、21…デ
イスプレイ装置、35…角度変換行列演算部、36…角速度
変換行列演算部、39…ホルダ。
Claims (8)
- 【請求項１】６個以上の関節を有するマスタマニピュレ
ータと、 該マスタマニピュレータより多い関節を有するスレーブ
マニピュレータと、 前記マスタマニピュレータの各関節の位置に関する前記
スレーブマニピュレータの関節の目標値を該スレーブマ
ニピュレータの各関節に分配するように演算する手段
と、該演算手段の演算結果に基づいてスレーブマニピュ
レータの各関節を駆動する手段とを備え、 前記演算手段は、マスタマニピュレータによる目標値を
スレーブマニピュレータの関節の角速度に変換するため
に用いる行列を低速周期で計算し、更新する長周期演算
部と、 前記低速周期で演算され、且つ更新される行列を用いて
マスタマニピュレータにより目標値をスレーブマニピュ
レタの各関節の角速度に高速周期で計算して変換する短
周期演算部と を有するマスタ・スレーブマニピュレータ。 - 【請求項２】特許請求の範囲第１項において、前記長周
期演算部は、スレーブマニピュレータの関節の角速度の
２乗和が最小となるように前記変換のための行列を計算
するマスタ・スレーブマニピュレータ。 - 【請求項３】特許請求の範囲第２項において、前記演算
手段は、 マスタマニピュレータの各関節を、スレーブマニピュレ
ータの関節から任意に選ばれたマスタマニピュレータと
同数の関節に各々一対一で対応させるとともに、この対
応関係を動的に変更させる手段と、 該対応関係の変更手段の指令に基づきマスタマニピュレ
ータによる目標値をスレーブマニピュレータの関節角に
一対一で対応させて変換するために用いる行列を計算
し、更新する手段と、 前記行列を用いてマスタマニピュレータによる目標値を
スレーブマニピュレータの関節角に計算して変換する演
算部とを有するマスタ・スレーブマニピュレータ。 - 【請求項４】特許請求の範囲第３項において、前記関節
の対応関係変更する手段は、関節の対応関係を人により
指令する手段を含んで構成されているマスタ・スレーブ
マニピュレータ。 - 【請求項５】特許請求の範囲第３項において、前記行列
を用いてマスタマニピュレータによる目標値をスレーブ
マニピュレータの関節角に変換する演算部は、 関節の対応関係を変更する直前のスレーブマニピュレー
タの関節角と、変更直後の行列を用いて計算された目標
値に基づくスレーブマニピュレータの関節角との差を、
切り換え後のオフセット量として、変更後の行列を用い
て計算される目標値に基づくスレーブマニピュレータの
関節角に加算し、スレーブマニピュレータの関節角を計
算するマスタ・スレーブマニピュレータ。 - 【請求項６】特許請求の範囲第１項において、前記マス
タマニピュレータの各関節の位置に関する前記スレーブ
マニピュレータの関節の目標値を該スレーブマニピュレ
ータの各関節に分配するように演算する手段であって、 人間の指令により、マスタマニピュレータの各関節を、
スレーブマニピュレータの関節から任意に選ばれたマス
タマニピュレータと同数の関節に各々一対一で対応させ
るとともに、この対応関係を動的に変更させる手段と、 該対応関係の変更手段の指令に基づきマスタマニピュレ
ータによる目標値をスレーブマニピュレータの関節角に
一対一で対応させて変換するために用いる行列を計算
し、更新する手段と、 前記行列を用いてマスタマニピュレータによる目標値を
スレーブマニピュレータの関節角に変換する演算部とか
ら構成された演算手段と； 前記人間が指令した関節の対応関係を、対応する関節同
志を関連付けて表示する手段と； 前記演算手段の演算結果に基づいてスレーブマニピュレ
ータの各関節を駆動する手段と； を有するマスタ・スレーブマニピュレータ。 - 【請求項７】特許請求の範囲第６項において、前記関節
の対応関係を表示する手段は、対応する関節同志を同色
で、対応しない関節は異なった色で表示するものである
マスタ・スレーブマニピュレータ。 - 【請求項８】マスタマニピュレータによる目標値をスレ
ーブマニピュレータの関節の角速度に変換するために用
いる行列をスレーブマニピュレータの関節の角速度の２
乗和が最小となるように低速周期で計算して更新すると
ともに、 前記低速周期で計算され、且つ更新される行列を用いて
マスタマニピュレータによる目標値をスレーブマニピュ
レータの各関節の角速度に高速周期で計算して変換し、 該計算された角速度に基づいてスレーブマニピュレータ
の各関節を駆動するマスタ・スレーブマニピュレータの
制御方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP62174641A JP2507449B2 (ja) | 1987-07-15 | 1987-07-15 | 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP62174641A JP2507449B2 (ja) | 1987-07-15 | 1987-07-15 | 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS6420983A JPS6420983A (en) | 1989-01-24 |
| JP2507449B2 true JP2507449B2 (ja) | 1996-06-12 |
Family
ID=15982146
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP62174641A Expired - Lifetime JP2507449B2 (ja) | 1987-07-15 | 1987-07-15 | 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2507449B2 (ja) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5363149A (en) * | 1992-04-23 | 1994-11-08 | Pioneer Electronic Corporation | Projection television set |
| JP2007019033A (ja) * | 2001-01-10 | 2007-01-25 | Ebara Corp | 電子線による検査装置、検査方法、及びその検査装置を用いたデバイス製造方法 |
| JP5612971B2 (ja) * | 2010-09-07 | 2014-10-22 | オリンパス株式会社 | マスタスレーブマニピュレータ |
| JP5669590B2 (ja) * | 2011-01-20 | 2015-02-12 | オリンパス株式会社 | マスタスレーブマニピュレータ及び医療用マスタスレーブマニピュレータ |
| US10959795B2 (en) * | 2015-08-25 | 2021-03-30 | Kawasaki Jukogyo Kabushiki Kaisha | Remote-control manipulator system and method of operating the same |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61146482A (ja) * | 1984-12-20 | 1986-07-04 | 工業技術院長 | 異構造異自由度バイラテラル・マスタスレイブ・マニピユレ−タの制御装置 |
| JPS61252081A (ja) * | 1985-04-30 | 1986-11-10 | 工業技術院長 | マスタ・スレ−ブ・マニピユレ−タ |
-
1987
- 1987-07-15 JP JP62174641A patent/JP2507449B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JPS6420983A (en) | 1989-01-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| US9149931B2 (en) | Robot system, robot control device and method for controlling robot | |
| JP2664205B2 (ja) | マニピュレータ制御システム | |
| US4594671A (en) | Velocity method of controlling industrial robot actuators | |
| Zhu et al. | Motion/force/image control of a diagnostic ultrasound robot | |
| KR20180059888A (ko) | 로봇교시방법 및 로봇 암 제어장치 | |
| JP2507449B2 (ja) | 冗長関節を有するマスタ・スレ―ブマニピュレ―タ及び該マニピュレ―タの制御方法 | |
| JPH0720941A (ja) | 作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法 | |
| JPS63278777A (ja) | ロボツトの教示運転方法 | |
| JP2629291B2 (ja) | マニピュレータ学習制御方法 | |
| JPH0386484A (ja) | ロボットの遠隔操作装置 | |
| JPH0630012B2 (ja) | 産業用ロボツトの制御方法 | |
| JP2645866B2 (ja) | マニピュレータの制御方法および装置 | |
| JP3212491B2 (ja) | ダイレクトティーチング制御装置 | |
| JP3146463B2 (ja) | 冗長軸マニピュレータの肘移動制御装置 | |
| JP2021115654A (ja) | 主従ロボットの制御装置 | |
| JP3431026B2 (ja) | 冗長軸を有する７軸以上のマニピュレータの制御方法 | |
| JPH06114766A (ja) | マニピュレータの遠隔制御装置 | |
| JP2737325B2 (ja) | ロボットの軌道生成方式 | |
| JP2909074B2 (ja) | ロボットアームの動作シミュレータ装置 | |
| JPS62127905A (ja) | ロボツトア−ムの動作シミユレ−タ装置 | |
| JPH01237703A (ja) | ロボツト教示装置 | |
| WO2023157137A1 (ja) | ロボット制御システム、ロボット制御方法、ロボット制御プログラム、および推定システム | |
| JPH04300173A (ja) | マスタスレーブマニピュレータ | |
| JPH08309680A (ja) | マニピュレータ制御システム |