JPS62102980A

JPS62102980A - マスタスレ−ブマニピユレ−タの制御方式

Info

Publication number: JPS62102980A
Application number: JP24097385A
Authority: JP
Inventors: 康一鈴森
Original assignee: Toshiba Corp
Current assignee: Toshiba Corp
Priority date: 1985-10-28
Filing date: 1985-10-28
Publication date: 1987-05-13

Abstract

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

Description

【発明の詳細な説明】〔発明の技術分野〕本発明は１機構干渉が存在するマニピュレータを用い、
かつ力帰還形で制御されるマスタスレーブマニピュレー
タの制御方式の改良に関する。

〔発明の技術的背景とその問題点〕

ロボット技術の発達に伴って種々の多関節マニピュレー
タが考えられている。このようなマニピュレータの中に
はドライブユニットをアー″ムの根元部分に集中配置し
、これらドライブユニットで得られた動力をギア、チェ
ーン、ワイヤ等で伝達１−で関節を動作させるようにし
たものがある。第３図は、このようなタイプの代表的な
多関節マニピュレータを示している。

この図は１人間の腕を模した７自由度のものを示してい
る。すなわち、このマニピュレータは。

大きく分けて肩部Ａと、上腕部Ｂと、材部Ｃと。

下腕部りと１手首部Ｅと、ハンド部Ｆと、駆動部Ｇとで
構成されている。駆動部Ｇは自由度の数と同数、この場
合には７個のドライブユニット１〜７によって構成され
ており、これらドライブユニット１〜７は肩部Ａ側に集
中して配置されている。

そして、各ドライブユニット１〜７から各関節に次のよ
うにして動力を伝達している。すなわち。

肘関節Ｃを動かす場合を例にとると、この場合にはドラ
イブユニット３を対応させている。ドライブユニット３
で発生した力は、スプロケット８゜チェーン９．スプロ
ケット１０に伝えられる。そして、スプロケット１０に
伝えられた力は１円筒１１に固着された傘歯車１２に伝
えられ、さらに肩部Ａに支持された軸１３に回転自在に
装着された傘歯車１４に伝えられ、続いて傘歯車１５９
円筒１６．傘歯車１７．材部Ｃに支持された軸１８に回
転自在に装着された傘歯車１９に伝えられる。

傘歯車１９は下腕部りを支持する支持筒２０に連結され
ている。したがって、ドライブユニット３を駆動すると
、下腕部りが軸１８を支点にして回動することになり、
材部Ｃを動かすことが可能となる。

このように構成された多関節マニピュレータは。

関節部にドライブユニットを有していないのでアーム全
体の軽量化およびアーム全体の単純化を図れる等の利点
を備えている。しかし２反面、このように構成された多
関節マニピュレータではドライブユニットの回転が対応
する関節の回転に直接対応しない不具合もある。すなわ
ち、あるドライブユニットの回転により複数の関節が回
転してしまう。また、逆にある関節のみを回転させるた
めには複数のドライブユニットとの協調回転が必要とな
る。これは一般に機構干渉と呼ばれている。

この機構干渉を数式で表現すると次のようになる。すな
わち、各ドライブユニットの回転速度を成分とするベク
トルをωｄ、各関節の回転速度を成分とするベクトルを
ａｔ　ａ　、　　ωｄとａｔａとを結びつける行列をＭ
とすると。

ωａｍＭωｄ　　　　　　　　　　　・（１）ωｄ−Ｍ
−１　　ωａ　　　　　　　　・・・（２）で表わされ
る。一方、各ドライブユニットのトルクを成分とするベ
クトルをｔｄ、各関節のトルクを成分とするベクトルを
ｔａとすると、ｔｄとｔａとの関係は上述の行列Ｍを使
って次のように表わせることが知られている。

ｔ　ａ　−（Ｍｔ）　’　　ｔ　ｄ　　　　・（３）ｔ
ｄ　−Ｍ　　ｔａ　　　　　　−（４）ただし９行列Ｍ
の右肩の−１，ｔは２それぞれ逆行列および転置行列を
表わしている。

ところで、マニピュレータを使って実際に作業を行なう
場合、マニピュレータのハンド部に加わる力の感覚が操
作者に伝われば遠隔操作性を大幅に向上させることがで
きる。このようなことから従来、パイラテラルサーボ機
構を用いたマスタスレーブマニピュレータが種々考えら
れている。そして、第３図に示したマニピュレータ、つ
まりアーム関節とドライブユニットとの間に機構干渉が
存在するマニピュレータをマスク側とスレーブ側とに用
いた力帰還形のマスタスレーブマニピュレータも出現し
ている。第４図は、第３図に示したマニピュレータをマ
スク側およびスレーブ側に組み込んだ従来の力帰還形マ
スタスレーブマニピュレータの概略構成を示すものであ
る。

同図において、添字ｍを付しである側がマスク側を示し
、添字Ｓを付しである側がスレーブ側を示している。マ
スク側のドライブユニット１ｍ〜７ｍは、それぞれモー
タ３１ｍと１位置センサ３２ｍと、トルクセンサ３３ｍ
とで構成されている。同様に、スレーブ側のドライブユ
ニット１ｓ〜７ｓもモータ３１ｓと２位置センサ３２ｓ
と。

トルクセンサ３３ｓとで構成されている。そして。

マスク側とスレーブ側との間に制御装置３４を介在させ
、この制御装置３４で対応するドライブユニット毎に次
のような制御を独立的に行なわせるようにしている。す
なわち、対応する２つのドライブユニット毎にスレーブ
側のトルクセンサ３３Ｓの出力とマスク側のトルクセン
サ３３ｍの出力とを信号加算点３５に導入して偏差をと
り、この偏差信号をマスク側のモータ３１ｍへ駆動信号
として与え、また、マスク側の位置センサ３２ｍの出力
とスレーブ側の位置センサ３２ｓの出力とを信号加算点
３δに導入して偏差をとり、この偏差信号をスレーブ側
のモータ３１ｓの駆動信号として与えている。

しかしながら、上記のように機構干渉が存在するマニピ
ュレータを用いて構成された従来の力帰還形マスタスレ
ーブマニピュレータにあっては次のような問題があった
。すなわち＋　ｍ４図に示すように構成されたサーボ系
ではマスク側の各ドライブユニット１ｍ〜７　ｍ’は、
前述した符号で示すとｔｄ　　とｔｄ　　との偏差で駆
動されることになる。なお、右肩の添字ｓ、ｍはスレー
ブ側、マスク側をそれぞれ示す。したがって、マスク側
の各ドライブユニットの駆動信号をＩＩｄ′Ｉとすると
。

ａｄ　　は。

ｕｄ　　＝Ａ　（ｔｄ”−ｔｄ　　）　　　　・・・（
５）で表わされる。なお９人は対角行列である。一方。

各ドライブユニットは駆動信号ｕ　ｄｌｌｌに応じて回
転する。これを式で表わすと。

ｖｄ”　Ｂｕｄ’　−ＢＡ　（ｔｄ”−ｔｄＩｌｌ）・
・・（６）となる。ただし、Ｂは対角行列である。

しかし１機構干渉が存在するマニピュレータには、前記
（１）〜（４）式で示す特性があるため、（２）式およ
び（４）式を（６）式に代入すると、関節にトルクが作
用しているときの各関節の動きは次式のようになる。

ａｈａ”　−Ｍ［３人Ｍｔ　（ｔａ８−ｔａ″１）・・
・（７）機構干渉が存在するマニピュレータでは１Ｍが対角行列
ではないので、ＭＢＡＭｔも対角行列にはならない。し
たがって、たとえば、ある１つの関節にトルクが作用し
た場合、っまりｔａＳ−ｔａ　　の−成分が発生した場
合にはωａのその関節に対応しない成分にも影響を与え
ることになる。

すなわち、第４図において操作者がマスク側の関節Ｃｍ
ａを曲げようとして関節ｃＩｌｌａに力を加えると。

関節Ｃｍｂも回転すると言った現象が発生する。このた
め、操作者は関節ＣｌｌＩｂの回転を止めるために関節
ＣＩＩＩａ以外にも力を加えなければならず、操作性が
著しく悪いと言う問題があった。

〔発明の目的〕

本発明は、このような事情に鑑みてなされたもので、そ
の目的とするところは９機構干渉が存在するマニピュレ
ータを用いて力帰還パイラテラル制御を行なうに際し１
機構干渉に起因する操作性の低下を防止できるマスタス
レーブマニピュレータの制御方式を提供することにある
。

〔発明の概要〕

本発明は、マスク側の各ドライブユニットの駆動信号ａ
ｄ　　を次式で表わされるように与えて目的を達成して
いる。

ａｄ−Ｄ−１１Ｍ−’　（Ｃ（Ｍｌ）−’　　（ｔｄ”
−ｔｄｍ）・・・（７）゛たたし、のはそれぞれの関節を駆動する制御ゲインを対
角要素に持つ対角行列、Ｄは（８）式で定義される対角
行列である。

このような駆動信号供給方式であると、各関節をｔ　ａ
　”’　−’ｔ　ａ　１１の成分に対応させて動かすこ
とができる。すなわち、各ドライブユニ、ントは駆動信
号ｕｄＩＩｌに対応して回転する。

ｔｖ　ｄ　　＝Ｄ　ｕ　ｄ’　　　　　　　　・”（８
）この（８）式に（７）式を代入すると。

ωｄ”　−Ｍ−’　（Ｃ（Ｍｔ）−’　　（ｔｃｌｓ−
ｔ　ｄ”　）・・・（８）′ となる。さらに（８）′式に前記（２）式および（４）
式を代入して整理すると。

ｔｖａ［ｌｌ−０１：　（ｔａ”−ｔａ”　）　　　　
　−＝（９）となる。Ｃ゛は対角行列であるから、外力
によってｔａ８−ｔａｌｌｌので成分が発生すると、そ
の成分に対応した関節のみが回転することになる。

〔発明の効果〕

本発明の制御方式であると、複雑な機構干渉が存在する
マニピュレータを用゛いた場合でも機構干渉に起因する
操作性の低下を防止することができる。また、その制御
も簡単でアナログ式、ディジタル式のどちらの演算装置
を使用しても容易に実現することができる。

〔発明の実施例〕

以下２図面を参照しながら本発明の詳細な説明する。

第１図は９本発明の一実施例に係るマスタスレーブマニ
ピュレータの制御系を示している。この図は第３図に示
した７自由度を有するマニピュレータをマスク側および
スレーブ側に使用した例を示すもので、第４図と同一部
分は同一符号で示しである。したがって２重複する部分
の説明は省略する。また１本発明はマスク側の各ドライ
ブユニットの駆動信号形成手段に特徴を有している。し
たがって、この図ではスレーブ側の駆動系が省略されて
いる。スレーブ側の各ドライブユニットは。

従来と同様に、マスク側のドライブユニットとこれに対
応するスレーブ側のドライブユニットととの位−置偏差
に応じてそれぞれ独立的に駆動される。

しかして、この実施例では、マスク側マニピュレータと
スレーブ側マニピュレータとの間に次のような制御装置
４１を介在させている。すなわち。

スレーブ側のドライブユニット１８〜７ｓにそれぞれ設
けられたトルクセンサ３３ｓの出力とマスク側のドライ
ブユニット１　ｍ、〜７ｍにそれぞれ設けられたトルク
センサ３３ｍの出力と、の偏差を対応するドライブユニ
ット毎に信号加算点４２〜４８で算出し、これら偏差信
号を後述する（ｌｌｂ）式の行列の各要素に対応して設
けられた係数器４９ａ〜４９ｇ、５０ａ〜５０ｇ、’５
１ａ〜５１ｇ、・・・・・・・・・５５ａ〜５５ｇを介
して各ドライブユニット１ｍ〜７ｍに対応した信号加算
点５６゜５７．５８．・・・・・・６２に導入し、これ
ら信号加算点５６，５７，５８．・・・・・・６２を出
た信号を各ドライブユニット１ｍ〜７ｍの対応するモー
タ３１ｍの駆動信号として導入するようにしている。

これを式で表わすと次のようになる。すなわち。

肩部Ａの回転、肩部Ａの曲げ、材部Ｃの曲げ、下腕部り
の回転２手首部Ｅの曲げ、ハンド部Ｆの開閉を司る駆動
軸の回転速度をそれぞれωａＩ＋ωａ２＋　　ωａ３．
　　・・・・・・ωａ７．トルクをｔａｌ。

ｔ　ａ２　、　　ｔ　ａ３　、　・・・・・・ｔａ７　
とし、ドライブユニット１〜７の回転速度およびトルク
をそれぞれωｄ１　、　ωｄ２＋　　ωｄ３．　　・・
・・・・ωｄ７．ｔｄ１　。

ｔ　ｄ２　、　　ｔ　ｄ３　、・・・・・・ｔｄ１７と
すると１機構干渉は（１）式に相当する次式であられさ
れる。

・・・（１０）・・・（ｌｌｂ）ｋ２．に３．・・・・・・ｋ７は各関節を駆動する制御
ゲインを示している。また、上記式では簡単化するため
にＤを単位行列としている。第１図に示す実施例では（
ｌｌｂ）式に示す行列の各要素に対応した定数に各係数
器４９ａ　〜４９ｇ、５０ａ　〜５０ｇ、・・・・・・
５５ａ〜５５ｇを設定しているのである。

したがって、上記のように構成された力帰還形マスタス
レーブマニピュレータではマニプレータの機構干渉が完
全に補償されることになり２機構干渉による操作性の低
下を防止することができる。

第２図は本発明の他のｉ施例に係る制御方式を採用した
マスタスレーブマニピュレータの制御系における要部だ
けを示すものである。

第１図に示した実施例では（ｌｌｂ）式の形で演算を行
なっているが、第２図に示すものは（ｌｌａ）式の形で
右から演算を行なう制御装置４１ａを介在させたもので
ある。すなわち、（ｌｌａ）式を。

ａｄＩＩｌ−ＤＭ’　Ｃ（Ｍ”　）−’　　（ｔｄ８−
ｔｄｆｆｌ）と書くと、（１）弐〜（４）式の関係から
。

＜イ＞　　（Ｍ’　）−＋　　（ｔｄ８−ｔｃｉＩＩｌ
）のａｍ結果が関節のトルク偏差となり。

（ｏ）　（Ｃ（ｔ　ｄ”　−ｔ　ｄｍ）　ノａ算結果１
）（関ｍの回転速度偏差ωａＳ−ωａＩｆｉとなり。

（ハ）Ｍ−’　　（ωａ”−ｅｒａＩＩ′）（Ｄ演算結
果がωｄとなる。

この実施例では信号加算点６１〜６７で（イ）の演算を
行なわせ、信号加算点７１〜７７で（ロ）の演算を行な
わせ、信号加算点８１〜８７で（ハ）の演算を行なわせ
るようにしている。なお２図中９１〜９７は各関節のゲ
インで行列Ｂの対角要素である。

この実施例では第１図に示した実施例と同じ演算結果が
得られる上、関節トルク偏差、関節回転速度偏差を知る
ことができる。また、各関節の駆動ゲインを独立して調
整できるため、特にアナログ演算装置を使用した場合に
調整の容易化を図ることができる。

なお２本発明は上述した実施例に限定されるものではな
い。すなわち９機構干渉が存在する全てのドライブユニ
ットとアーム関節間に適用する必要は必ずしもなく、操
作性に大きな悪影響を与えるドライブユニットとアーム
関節間だけに適用するようにしてもよい。

【図面の簡単な説明】

第１図は本発明の一実施例に係るマスタスレーブマニピ
ュレータの制御方式を説明するための概略構成図、第２
図は本発明の他の実施例に係る制御方式を説明するため
の概略構成図、第３図は機構−１′−渉が存在する代表
的なマニピュレータの構成図、第４図は第３図に示した
マニピュレータをマスク側およびスレーブ側に使用した
従来のマスタスレーブマニピュレータの制御系を説明す
るための図である。Ａｍ、Ａｓ・・−肩部、Ｂｍ、Ｂｓ−上腕部、Ｃｍ。Ｃｓ・・・材部、Ｄｍ、Ｄｓ・・・上腕部、Ｅｍ、Ｅｓ
・・・手首部、Ｆｍ、Ｆｓ・・・ハンド部、Ｇｍ、Ｇｓ
・・・駆動部、１ｍ〜７ｍ・・・マスク側ドライブユニ
ット。１５〜７Ｓ・・・スレーブ側ドライブユニット。３１ｍ、３１ｓ・・・モータ、３２ｍ、３２ｓ・・・位
置センサ、３３ｍ、３３ｓ・・・トルクセンサ、４１゜
４１ａ・・・制御装置。出願人代理人　弁理士　鈴江武彦第１図

Claims

【特許請求の範囲】

（１）自由度と同数のドライブユニットを有し、かつア
ーム関節とドライブユニット間に機構干渉が存在する多
関節マニピュレータをそれぞれマスタマニピュレータお
よびスレーブマニピュレータとするマスタスレーブマニ
ピュレータの制御方式において、前記マスタマニピュレ
ータと前記スレーブマニピュレータとの間に、上記各マ
ニピュレータの各ドライブユニットに装備されたトルク
センサの信号からアーム関節とドライブユニット間の変
位の干渉関係およびトルクの干渉関係を利用して上記マ
スタマニピュレータの上記各ドライブユニトを駆動する
制御装置を介在させ、この制御装置で上記マスタマニピ
ュレータと上記スレーブマニピュレータとの間の力帰還
ループを構成するようにしたことを特徴とするマスタス
レーブマニピュレータの制御方式。
（２）前記制御装置は、前記マスタマニピュレータの各
ドライブユニットとこれに対応する前記スレーブマニピ
ュレータの各ドライブユニットにそれぞれ働くトルクの
差をそれぞれ算出する第１の演算部と、この第１の演算
部の演算結果から前記トルクの干渉関係に基づいて前記
マスタマニピュレータの各アーム関節とこれに対応する
前記スレーブマニピュレータの各アーム関節にそれぞれ
働くトルクの差をそれぞれ算出する第２の演算部と、前
記マスタマニピュレータの各アーム関節を前記第２の演
算部で算出されたトルク差に対応させて動作させるべく
前記変位の干渉関係に基づいて上記マスタマニピュレー
タの前記各ドライブユニットの動きを決定する第３の演
算部と、この第３の演算部の演算結果に基づいて前記マ
スタマニピュレータの前記各ドライブユニットを駆動す
る制御部とで構成されることを特徴とする特許請求の範
囲第１項記載のマスタスレーブマニピュレータの制御方
式。
（３）前記制御装置は、前記第２および第３の演算部で
行われる一連の演算過程を等価変換して行なう演算部を
有していることを特徴とする特許請求の範囲第２項記載
のマスタスレーブマニピュレータの制御方式。