JPH0631525Y2 - マニピユレ−タの制御装置 - Google Patents

Description

【考案の詳細な説明】 Ａ産業上の利用分野 本考案は、マニピュレータ、例えば電動両腕型マスター
・スレーブサーボマニピュレータをはじめ、原子力関連
施設等で遠隔保守作業等を行う場合に作業者の手に代っ
て使用されるマニピュレータの力帰還型制御ループの改
良に関する。

Ｂ考案の概要 本考案を簡単に説明すると、マスター・スレーブ式マニ
ピュレータの力帰還型制御装置において、スレーブ側モ
ータ駆動用アンプにスレーブ側トルク信号を負帰還する
ことにより非線形要素の影響をフィードバックループ内
で吸収し、且つ上記アンプにマスター側トルク信号を加
えることによりスレーブ側トルク信号の負帰還によるゲ
イン低下を補償する。これらにより、リミットサイクル
を防止しながら系の剛性を上げることができるようにし
たものである。

Ｃ従来の技術 第６図に、マスター・スレーブ式マニピュレータに用い
られている従来の力帰還型制御ループを示す。同図中、
１はマスター軸、２はギヤ、３はマスター側軸角度セン
サ（Ｐ_Ｍ）、４はマスター側モータ（Ｍ_Ｍ）、５はマス
ター側モータ駆動用アンプ（ＡＰ_Ｍ）、６はマスター側
トルクセンサ（Ｔ_Ｍ）である。また、１１はスレーブ
軸、１２はギヤ、１３はスレーブ側軸角度センサ
（Ｐ_Ｓ）、１４はスレーブ側モータ（Ｍ_Ｓ）、１５はス
レーブ側モータ駆動用アンプ（ＡＰ_Ｓ）、１６はスレー
ブ側トルクセンサ（Ｔ_Ｓ）である。２１は軸角度の差分
を求める回路、２２は操作力（トルク）の差分を求める
回路である。

動作としては、オペレータがマスター側装置を操作する
と、その操作はマスター軸１の軸角度変化としてとらえ
られ、ギヤ２を介した軸角度センサ３によりθ_Ｍとして
検出される。また同時に、トルクセンサ６がオペレータ
の発生している操作力（トルク）をＦ_Ｍとして検出して
いる。一方、スレーブ側についても、スレーブ軸１１の
軸角度を軸角度センサ１３がθ_Ｓとして検出し、またス
レーブ軸１１の操作力をトルクセンサ１６がＦ_Ｓとして
検出している。そこで、 (a)マスター軸１の軸角度θ_Ｍが変化すると、軸角度の
突き合わせ点Ａにおいて回路２１の軸角度差分の出力Δ
θが変化する。この差分出力Δθはスレーブ側へ駆動力
として伝達されるため、アンプ１５はΔθを少なくする
方向にスレーブ側モータ１４を回転させる。これにより
スレーブ軸１１の軸角度θ_Ｓがマスター軸１の軸角度θ
_Ｍに追従する。

(b)一方、スレーブ側トルクＦ_Ｓとマスター側トルクＦ
_ＭとがＢ点で突き合わされているため、トルク差分ΔＦ
が回路２２から出力される。この差分出力ΔＦはマスタ
ー側へ駆動力として伝達され、アンプ５はオペレータの
操作力Ｆ_Ｍとは反対方向にトルクを発生するようにマス
ター側モータ４を駆動する。これが力帰還型制御であ
る。

(C)以上の動作によって、オペレータはスレーブをマス
ターに追従させると共に、スレーブ側の負荷を反力とし
て感じながら操作をすることができる。即ち、マニピュ
レータの力帰還型制御装置では、スレーブ軸１１を動か
すスレーブ側モータ１４をマスター軸１とスレーブ軸１
１間の軸角度偏差に基づいてスレーブ側モータ駆動用ア
ンプ１５で駆動することによりスレーブ軸１１をマスタ
ー軸１の動きに追従させ、マスター軸１を動かすマスタ
ー側モータ４をスレーブ軸１１のトルクを検出するスレ
ーブ側トルクセンサ１６から得たスレーブ側トルク信号
に基づいてマスター側モータ駆動用アンプ５で駆動する
ことによりスレーブ側の負荷の反力をマスター軸１に与
える。

Ｄ考案が解決しようとする問題点 上述した如く力帰還型制御ループはオペレータがスレー
ブ側の負荷を反力として感じることができるので優れた
制御方式であるが、その反面下記（ｉ）〜（iv）の問題
点がある。

（ｉ）スレーブからの反力をマスターに負帰還している
ため、マスターとスレーブ間の剛性が低下し、マスター
とスレーブとがバランスしたとき、軸角度差分Δθが大
きい。

（ii）また、剛性低下のため、操作中はマスターとスレ
ーブとが軟らかいバネでつながったような状態となり、
操作性、操作感覚が低下する。

（iii）上記（ｉ），（ii）の欠点を解決するには系の
剛性を向上させれば良いが、そのためには一般にスレー
ブ側モータ駆動用アンプ１５のゲインを大として電気的
剛性を大きくしなければならない。

（iv）しかし、アンプ１５のゲインを大とすると、系の
信号レベルが大きくなるため、電気的非線形要素（電気
回路の飽和など）並びに機械的非線形要素（摩擦、ガタ
など）によってリミットサイクル（周期振動）が発生し
てしまう。これによる振動現象は操作性を極めて悪化さ
せる。

本考案は上記従来技術の問題点に鑑み、リミットサイク
ルを防止しながらも系の剛性を上げることができる、マ
ニピュレータの力帰還型制御装置を提供することを目的
とする。

Ｅ問題点を解決するための手段 上述した目的を達成するため本考案の制御装置は、スレ
ーブ側モータ駆動用アンプにスレーブ側トルク信号を負
帰還する手段と、上記アンプにマスター側トルク信号を
比例ゲインを乗じて加える手段とを備えることを特徴と
する。

Ｆ作用 スレーブ側トルク信号をスレーブ側モータ駆動用アンプ
に負帰還することにより、フィードバック補償が行われ
て電気的・機械的非線形要素がループ内で吸収されるた
め、リミットサイクルが防止される。一方、上記のフィ
ードバック補償によってアンプのゲインが低下するが、
マスター側トルク信号をそのアンプに加えることにより
フィードフォワード補償が行われ系の剛性が上がる。

この場合、スレーブ側トルク信号の負帰還量の調整と、
マスター側トルク信号に乗じる比例ゲインの調整によ
り、系が安定に動作する。

Ｇ実施例 第１図〜第５図、及び第７図〜第１０図により本考案の
一実施例を説明する。第１図は一実施例装置のブロック
図であり、二点鎖線で囲んだ部分だけが第６図の従来の
ものと異なる。

即ち、スレーブ側トルクセンサ１６の出力を負帰還ゲイ
ン調整部（Ｋ_Ｓ）３２及び回路３１を通してスレーブ側
モータ駆動アンプ１５に負帰還し、一方マスター側トル
クセンサ６の出力をフィード・フォワード・ゲイン調整
部（Ｋ_Ｍ）３３及び回路３１を通してスレーブ側モータ
駆動アンプ１５に加えている。従来からの軸角度差分信
号Δθは回路３１を通してアンプ１５に与えられてい
る。

なお、力帰還型制御本来の動作は従来と同じであり、ス
レーブ軸１１を動かすスレーブ側モータ１４をマスター
軸１とスレーブ軸１１間の軸角度偏差に基づいてスレー
ブ側モータ駆動用アンプ１５で駆動することによりスレ
ーブ軸１１をマスター軸１の動きに追従させ、マスター
軸１を動かすマスター側モータ４をスレーブ軸１１のト
ルクを検出するスレーブ側トルクセンサ１６から得たス
レーブ側トルク信号に基づいてマスター側モータ駆動用
アンプ５で駆動することによりスレーブ側の負荷の反力
をマスター軸１に与える。但し、マスター側モータ駆動
用アンプ５にはマスター側トルクセンサ６から得たマス
ター側トルク信号と前記スレーブ側トルク信号との差を
入力している。

第１図に示す制御装置においては、電気的・機械的非線
形要素の影響はスレーブ側トルク負帰還のループによっ
て吸収されるため、リミットサイクルの発生が防げる。
またスレーブ側トルク負帰還ループの存在により、スレ
ーブ側トルク特性の周波数帯域が広がり、よって分解能
が向上する。

一方、スレーブ側トルク負帰還によってゲインが低下す
るが、その代りにマスター側トルク信号をアンプ１５に
加算してフィードフォワード補償しているので結果的に
ゲイン低下にならず、従来よりも大きなゲインが実現さ
れる。これによって非線形要素の影響を受けない状態で
系の剛性が上がる。またマスター側トルクとフィードフ
ォワード制御することにより、スレーブ側トルクがマス
ター側へ戻るまでの時間遅れが補償されることになり、
速応性が向上する。

また、スレーブ側が無負荷時はスレーブトルク負帰還が
ないため大きな剛性を持つことになる。その結果、マス
ターとスレーブは定値制御に近くなり、軸角度差分がΔ
θ≒０（ゼロ）となって操作性が向上する。

ここで、系の安定性について第７図〜第１０図を参照し
て説明する。

まず、第７図により負帰還ゲイン調整部３２における負
帰還ゲインＫ_Ｓと系の安定性との関係を説明する。な
お、第７図は第１図と同じ内容の第８図で二点鎖線で囲
んだVII部分を伝達関数で表わしたものであり、Ｋはス
レーブ側モータ駆動用アンプ１５のゲイン、Ｆ_Ｓはスレ
ーブ側トルク、Ｋ_Ｓは負帰還ゲイン、１／ＳＪはスレー
ブ側慣性モーメントをＪとした場合のマニピュレータの
伝達関数、ｖ_Ｓはスレーブ速度、Ｆは摩擦損失係数、１
／Ｓはスレーブ速度ｖ_Ｓからスレーブ角度θ_Ｓへの変
換、θ_Ｍはマスタ角度、θ_Ｓはスレーブ角度を示してい
る。

第７図から判るように、スレーブ側トルクセンサ１６か
らの信号Ｆ_Ｓを負帰還することは、スレーブ側モータ駆
動用アンプ１５のゲインが本来Ｋであったものが、等価
的にＫ／（１＋Ｋ×Ｋ_Ｓ）になることであり、１／（１
＋Ｋ×Ｋ_Ｓ）＜１であることから、等価的なゲインはア
ンプ本来のゲインＫよりも小さくなって、系は安定方向
に動作する。

次に、第９図によりフィード・フォワード・ゲイン調整
部３３におけるフィード・フォワード・ゲインＫ_Ｍと系
の安定性との関係を説明する。なお、第９図は第１図と
同じ内容の第１０図で二点鎖線で囲んだIX部分を伝達関
数で表わしたものであり、θ_Ｍ，θ_Ｓ，Ｋ，Ｆ_Ｓ，１／
ＳＪ，Ｖ_Ｓ，Ｆ，１／Ｓは第７図のものと同記号である
が、Ｆ_Ｍはマスター側トルク、Ｋ_Ｍはフィード・フォワ
ード・ゲイン、２つのＳはマスター角度θ_Ｍからマスタ
ー側トルクＴ_Ｍへの変換を示している。即ち、マスター
側トルクセンサ６はＦ_Ｍ＝θ_Ｍ×Ｓ×Ｓ＝θ_Ｍ×Ｓ^２を
検出している。

第９図において、系全体の伝達関数はＳ^２×Ｋ_Ｍ×Ｋ×
（１／ＳＪ）×（１／Ｓ）であるから、これを１とすれ
ば、即ちＫ_Ｍ×Ｋ＝Ｊとすれば、スレーブ側慣性モーメ
ントＪによる遅れをキャンセルすることができる。即
ち、摩擦損失Ｆを無視すれば、瞬時にθ_Ｍ＝θ_Ｓとする
ことができる。

従って、Ｋ_Ｍ×Ｋ＜Ｊとなるように、フィード・フォワ
ード・ゲインＫ_Ｍを設定することにより、系の安定性を
損うことなく追従性を高めることができる。

なお、第１図の実施例ではスレーブ側トルクの負帰還量
とマスター側トルクのフィードフォワード量とは、それ
ぞれの調整部３２，３３のゲインを変化させることによ
り調整可能にしてある。これにより、安定性の確保、マ
スター・スレーブの剛性の設定等を種々行えるようにな
っている。

また第１図の実施例は駆動モータと関節軸とが１対１に
対応するマニピュレータについてのものであるが、例え
ばトルクチューブ方式のマニピュレータなど対応のない
もの、またマスターとスレーブの形状が異なる非相似型
マニピュレータにも本考案を適用することができる。

ところで本考案の制御装置では第１図の回路構成のう
ち、２１，２２及び３１の回路並びに３２及び３３のゲ
イン調整部がＣＰＵ（マイクロコンピュータなどの中央
処理装置）で実現される。このようにＣＰＵを用いる構
成には第２図〜第５図に示す各種のものが考えられる。

第２図に示す例では、マスター及びスレーブの各軸毎に
ＣＰＵを別々に用いたものであり、マスター側のｉ軸の
所要のセンサ信号４１がＣＰＵ４２に入力され、アンプ
５を介してモータ４が駆動される。一方スレーブ側のｉ
軸の所要のセンサ信号４３がＣＰＵ４４に入力され、ア
ンプ１５を介してモータ１４が駆動される。なおＣＰＵ
４２，４４間で所要のデータが授受される。

第３図に示す例では、対応するマスター軸とスレーブ軸
の１組毎にＣＰＵを用いたものであり、各ｉ軸の所要の
センサ信号５１がＣＰＵ５２に入力され、アンプ５を介
してマスター側のモータ４が、またアンプ１５を介して
スレーブ側のモータ１４がそれぞれ駆動される。

第４図に示す例では、対応するマスター軸とスレーブ軸
の複数組毎にＣＰＵを用いたものであり、ｎ＋１個のマ
スター軸とスレーブ軸の所要のセンサ信号６１がＣＰＵ
６２に入力され、それぞれｎ＋１個のマスター側モータ
４とスレーブ側モータ１４とがアンプを介して駆動され
る。また他方のＣＰＵ６４には別のｍ＋１個のマスター
軸とスレーブ軸の所要のセンサ信号６３が入力されて同
様に、ｍ＋１個のモータ４とモータ１４がアンプを介し
て駆動される。なお、ＣＰＵ６２と６４間には、スレー
ブ軸間の干渉を防ぐため、所要のデータ授受が行われ
る。

第５図に示す例では、マスター及びスレーブの各ｎ個の
軸毎に別々にＣＰＵを用いたものであり、ｎ個のマスタ
ー軸の所要のセンサ信号７１がＣＰＵ７２に入力され、
ｎ個のモータ４がアンプ５を介して駆動される。一方、
ｎ個のスレーブ軸の所要のセンサ信号７３がＣＰＵ７４
に入力され、ｎ個のモータ１４がアンプ１５を介して駆
動される。またＣＰＵ７２，７４間で所要のデータが授
受される。

Ｈ考案の効果 本考案によれば、マニピュレータの剛性を上げて操作性
を良好にすると同時に、非線形要素の影響を取り除いて
リミットサイクル等を防止することができる。また、無
負荷時はスレーブトルクの負帰還がないため、定値制御
に近い状態となり、よってスレーブは軸角度差分Δθが
十分小さい状態でマスターに追従する。更に、マスター
トルクのフィードフォワード制御を行うことにより、マ
スターとスレーブの動作時間遅れが補償でき、速応性が
向上する。

【図面の簡単な説明】

第１図は本考案によるマニピュレータの制御装置の一実
施例のブロック図、第２図〜第５図はそれぞれＣＰＵを
用いた例の機器構成図、第６図は従来の制御装置のブロ
ック図である。第７図は第８図中の負帰還ループ部分VI
Iを伝達関数で示す図、第８図は第７図の負帰還ループ
部分を示す第１図と同内容のブロック図、第９図は第１
０図中のフィード・フォワード・ループ部分IXを伝達関
数で示す図、第１０図は第９図のフィード・フォワード
・ループ部分を示す第１図と同内容のブロック図であ
る。 図面中、 １はマスター軸、 ３と１３は軸角度センサ、 ４と１４はモータ、 ５と１５はアンプ、 ６と１６はトルクセンサ、 １１はスレーブ軸、 ３２は負帰還ゲイン調整部、 ３３はフィード・フォワード・ゲイン調整部である。

Claims (1)

【実用新案登録請求の範囲】
1. 【請求項１】(a)スレーブを動かすスレーブ側モータを
   マスターとスレーブ間の位置偏差に基づいてスレーブ側
   モータ駆動用アンプで駆動することによりスレーブをマ
   スターの動きに追従させ、マスターを動かすマスター側
   モータをスレーブのトルクを検出するスレーブ側トルク
   センサから得たスレーブ側トルク信号に基づいてマスタ
   ー側モータ駆動用アンプで駆動することによりスレーブ
   側の負荷の反力をマスターに与えるマニピュレータの力
   帰還型制御装置において、 (b)スレーブ側モータ駆動用アンプにスレーブ側トルク
   信号を負帰還する手段と、 (c)上記スレーブ側モータ駆動用アンプにマスターのト
   ルクを検出するマスター側トルクセンサから得たマスタ
   ー側トルク信号を比例ゲインを乗じて加える手段とを備
   えたことを特徴とするマニピュレータの制御装置。