JPH06138945A - マスタスレーブマニピュレータの制御装置 - Google Patents

Abstract

(57)【要約】 【目的】 少ない制御プログラム量で複数の操作モード
を実現することができるマスタスレーブマニピュレータ
の制御装置を提供する。 【構成】 内部モデルを用いるマスタスレーブマニピュ
レータのバイラテラル制御アルゴリズムを基本とし、こ
の内部モデルとこの内部モデルが出力する目標位置にア
ームを追従させる位置追従制御手段との間に、上記目標
運動値を操作モードに対応してオフセット補正して上記
位置追従制御手段に与えるオフセット補正手段を配置し
ている。 【効果】 マスタスレーブの制御処理アルゴリズムを各
操作モード毎に設ける必要がなく全体の処理プログラム
量を減らすことができる。また、目標値を一旦保持して
いるオフセット補正手段を介して他の操作モードへ移行
するので、モードの移行に伴うアームの急な動きを防止
して滑らかな動きを実現可能である。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、遠隔作業用のロボット
であるマスタスレーブマニピュレータ装の制御装置に関
する。

【０００２】

【従来の技術】近年、マスタスレーブマニピュレータの
多機能化が進み、従来の「バイラテラルモード」に加
え、「マスタシフト（原点移動）モード」、「速度操作
モード」等の機能が要求され、準備されるようになっ
た。

【０００３】「バイラテラルモード」とは、マスタアー
ム（操縦装置）の位置を追従するように、作業環境側の
スレーブアームを制御するモードであり、通常、マスタ
スレーブマニピュレータは、バイラテラルモードで使用
される。

【０００４】「マスタシフトモード」とは、異構造マス
タスレーブにおけるマスタアームとスレーブアームの動
作範囲の違いや、オペレータの作業し易さを考慮して準
備されたモードである。バイラテラルモードで作業して
いる場合に、マスタアームの動作範囲限界に達したとき
や、操作者が作業しにくい姿勢になったときに、このマ
スタシフトモードで任意の位置にマスタだけを移動する
ことができる。このモードの間、スレーブアームは停止
状態となり、マスタアームは自在に操作可能となる。そ
して、バイラテラルモードに復帰すると同時に、マスタ
シフトモードで決められたマスタスレーブ間のオフセッ
ト関係を維持したままバイラテラル作業に移行する。

【０００５】「速度操作モード」とは、ジョイスティッ
クでロボットを操作する感覚で、スレーブアームを速度
指令操作するモードである。このモードにより、スレー
ブアーム対応点（通常手先）の大きな移動を行う場合等
に、上記マスタシフトモードによるマスタ原点移動を繰
り返してスレーブアームを操作する煩雑さを避けること
ができる。この速度指令モードでは、通常、マスタアー
ムはサーボロックされ、操作者がマスタアームに加えた
力によって生じた微少変位や操作力が検出される。この
検出量に比例した速度でスレーブアーム先端を動作させ
る。

【０００６】上述した機能を、マスタスレーブで実現す
る場合、各機能毎にアルゴリズムを準備し、要求された
操作モードに応じて適切な制御アルゴリズムを選択・切
替える、分岐処理が一般的に採用されている。

【０００７】複数のモードを選択して異なる機能を実現
する分岐処理は、操作モード毎に制御アルゴリズムを切
替える当然の結果として、全体の制御プログラムが巨大
化し複雑になる欠点がある。更に、異なるアルゴリズム
間を移行する際に、連続的なマスタスレーブ操作を実現
するため、操作モードをどのように切替えても、各アー
ムを滑らかに動作させるマスタスレーブ間オフセット処
理及び同期処理がかなり複雑である。

【０００８】一方、バイラテラルモード及びマスタ原点
移動機能に限定した異構造マスタスレーブでは、分岐処
理をしなくとも、リレーを利用してバイラテラルモード
用の単一アルゴリズムでモードを切替えできる方式（例
えば、特開平３−１８４７７９号公報）も提案されてい
る。

【０００９】この方式によれば、複数のアルゴリズムを
用意しなくても良いため、プログラムがコンパクト、モ
ード切替による同期処理が不要、更にオフセット処理が
滑らかになる等の利点がある。しかしながら、この発明
は「バイラテラルモード」及び「マスタシフト（原点移
動）モード」の２つの機能の実現を前提として考慮され
たもので、この方式を拡張して、上記した「速度操作モ
ード」を実現することは出来ない。

【００１０】すなわち、「速度操作モード」を実現する
には、依然として速度操作モード専用の別途に用意され
た制御アルゴリズムを必要とし、従来の分岐処理と同様
の問題点が生ずる。

【００１１】

【発明が解決しようとする課題】以上のように、従来の
多機能を備えたマスタスレーブの制御装置では、各機能
毎に処理アルゴリズムを設けて分岐処理しるため、プロ
グラムが巨大化して複雑となる欠点があった。また、各
操作モードをどのように切り替えても、滑らかにマスタ
スレーブを動作させるためのオフセット処理が複雑にな
る欠点があった。

【００１２】よって、本発明は、少ない制御アルゴリズ
ムで各操作モードを実現でき、どのように操作モードを
切替えてもスレーブアームが滑らかで連続的に動作する
マスタスレーブの制御装置を提供することである。

【００１３】

【課題を解決するための手段】上記目的を達成するため
本発明のマスタスレーブマニピュレータの制御装置は、
マスタアーム及びスレーブアーム各々において検出され
た力検出力を入力として上記マスタアーム及び上記スレ
ーブアーム各々の目標運動値を生成する内部モデルと、
与えられる目標運動値を追従するように上記マスタアー
ム及び上記スレーブアームを夫々動作させる第１及び第
２の位置追従手段と、上記マスタアームの目標運動値を
アームの操作モードに対応してオフセット補正して上記
第１の位置追従手段に与える第１オフセット補正手段
と、上記スレーブアームの目標運動値をアームの操作モ
ードに対応してオフセット補正して上記第２の位置追従
手段に与える第２オフセット補正手段と、を備えたこと
を特徴とする。

【００１４】

【作用】内部モデルを用いるマスタスレーブマニピュレ
ータのバイラテラル制御アルゴリズムを基本とし、その
内部モデル出力とこの出力にアームを追従させる位置追
従制御手段の入力との間に、マスタスレーブマニピュレ
ータの操作モードに対応してオフセット量を設定するオ
フセット補正手段を配置する。

【００１５】この結果、バイラテラル制御アルゴリズム
を活用して他の操作モードを実現でき、同じ数の操作モ
ードを実現する制御プログラムに比べて全体の制御プロ
グラム量を少なくすることが可能となる。

【００１６】

【実施例】まず、内部モデルによるバイラテラル制御の
概念を説明する。図４は、本発明の基礎となる内部モデ
ル（仮想モデル）及びオフセット補正概念を示してい
る。内部モデルは、例えば、マスタスレーブマニピュレ
ータの力学的な仮想モデルであり、計算機上に構築され
る。図示の内部モデルはマス・ダンパからなる単純化さ
れた仮想モデルの例を示しており、マスタ側・スプレー
ブ側の内部モデル位置Ｐｖｍ，Ｐｖｓは一致している
（Ｐｖｍ＝Ｐｖｓ＝Ｐｖ）。質量Ｍｖの質点は固定ベー
スと粘性係数Ｃｖのダンパで接合され、マスタ側力検出
手段により検出されたマスタ操作力Ｆｍとスレーブ側力
検出手段により検出されたスレーブ反力Ｆｓとの合力が
加わり運動する。マスタ操作力Ｆｍ及びスレーブ反力Ｆ
ｓは共に力検出器に作用する方向で図示されている。操
作力Ｆｍと反力Ｆｓが釣り合えば、すなわち、Ｆｍ＝−
Ｆｓであればその運動を停止し、反力Ｆｓがなければ、
すなわち、スレーブが外部環境と干渉していなければ操
作力Ｆｍにより自在に運動する。このとき、実マスタア
ーム・実スレーブアームの設定点（通常手先）を、この
内部モデルの運動に一致させれば、操作者はスレーブア
ームを自在に操作していることになり、かつ、スレーブ
に加わる作業時反力Ｆｓを感じ取れることになる。そこ
で、内部モデル追従型バイラテラル制御は、マスタ側と
スレーブ側のアームが、夫々のモデル位置を追従するよ
うに実アームを制御し、「バイラテラルモード」を実現
している。

【００１７】更に、図４に示される第１スイッチＳＷ１
は、マスタアームのオフセット位置Ｐｏｍが内部モデル
と連動するか否かを決める。第２スイッチＳＷ２は、ス
レーブアームのオフセット位置Ｐｏｓが内部モデルと連
動するか否かを決める。スイッチがオフのときのオフセ
ット位置は、内部モデルの運動とは無関係に一定値を取
る。また、スイッチがオンのときのオフセット位置は、
内部モデルの運動に合わせて変化する。

【００１８】このモデルの従来例との相違点は、従来例
が内部モデル位置Ｐｖそのものを目標位置Ｒとしていた
のに対し、本発明では、オフセット位置Ｐｏから見た内
部モデル位置Ｐｖが、実アームの追従すべき目標位置Ｒ
となる点にある。

【００１９】２つのスイッチＳＷ１及びＳＷ２が共にオ
フであれば、マスタ及びスレーブアームの初期位置が異
なるだけで、従来の内部モデルを使ったバイラテラル制
御と変らないが、どちらかのスイッチがオンとなると、
状態は大きく異なってくる。

【００２０】スイッチＳＷ１がオンのとき、マスタ側オ
フセット位置Ｐｏｍと内部モデルＰｖは連動するので、
マスタアームの目標位置Ｒｍは、スイッチＳＷ１がオン
になる直前の目標位置を保持し続け、その結果、マスタ
アームはサーボロックされて動かない。同様に、スイッ
チＳＷ２がオンのときは、スレーブアームの目標位置Ｒ
ｓがスイッチＳＷ２がオンになる直前の目標位置に保持
され、スレーブアームが動かない。

【００２１】しかし、内部モデルは、常に、マスタ操作
力Ｆｍとスレーブ反力Ｆｓの合力で運動しているため、
スイッチがオフ状態である側のアームは内部モデルの運
動に合わせて動く。例えば、スイッチＳＷ１がオン、ス
イッチＳＷ２がオフのとき、マスタアームはサーボロッ
クされて動かないが、内部モデルは合力により運動して
おり、スレーブアームは内部モデルの運動を追従するよ
うに動作する。操作者がマスタに操作力Ｆｍを与えてお
り、スレーブが拘束されていない（スレーブ反力Ｆｓが
“０”）状態であれば、内部モデルは操作力Ｆｍにより
モデル粘性摩擦力と釣り合った速度で運動するので、結
果として、スレーブアームのみが、操作力に比例した速
度で動く「速度操作モード」を実現できる。

【００２２】表１に示されるように動作モードとスイッ
チの関係を選定すると、「待機モード」で、マスタアー
ム及びスレーブアーム共にサーボロック状態となる。
「マスタシフトモード」で、マスタアームだけを自在に
動かせる。「速度操作モード」で、マスタアームをジョ
イスティック操作しているかのようにしてスレーブアー
ムを動かせる。

【００２３】以上のように、基本として「バイラテラル
モード」を実現するアルゴリズムだけ調整・準備等すれ
ば、操作者の要求する操作モードに連動するスイッチ手
段を備えたオフセット補正手段により、各種多機能を実
現することが可能となる。

【００２４】次に、本発明の理解を容易にするために、
特公平４−４５３０４号公報により提案されているマニ
ピュレータの制御装置について説明する。この装置の構
成は、図６に示されるように、マスタアーム１、スレー
ブアーム２、内部モデル３、マスタ側追従装置１０ａ及
びスレーブ側追従装置１０ｂによって示される。

【００２５】マスタアーム１及びスレーブアーム２は、
共にバイラテラル制御を行うのに必要な公知の構成要素
を含んでいる。マスタアーム１は、マスタの位置追従制
御手段１０ａの出力である各軸駆動指令Ｔｍを入力とし
て動作し、各軸関節角度情報θｍ及び共通座標に変換し
た外力情報Ｆｍを出力する。スレーブアーム２はスレー
ブの位置追従制御手段１０ｂの出力である各軸駆動指令
Ｔｓを入力として動作し、各軸関節角度情報θｓ及び共
通座標に変換した外力情報Ｆｓを出力する。位置追従制
御手段１０ａは、運動学に基づいた座標変換を備えた公
知の位置制御手段で、共通座標系表記したアーム設定点
（例えば手先）の位置・姿勢が、共通座標系で夫々与え
られた目標位置情報Ｒm （位置・姿勢）と一致するよう
に、フィードバック制御する。位置追従制御手段１０ｂ
も、運動学に基づいた座標変換を備えた公知の位置制御
手段で、共通座標系表記したアーム設定点（例えば手
先）の位置・姿勢が、共通座標系で夫々与えられた目標
位置情報Ｒｓ（位置・姿勢）と一致するように、フィー
ドバック制御する。

【００２６】内部モデル３は種々のものが考えられ、例
えば、図７乃至図９に示される公知の構成要素を含むブ
ロックで表現される。図７は、マス・ダンパ力学モデル
を示している。図８は、ＰＤ動作型力制御モデルを示し
ている。図９は、マス・ダンパからなるマスタ側・スレ
ーブ側の力学モデルをバネ・ダンパ接合した力学モデル
を示している。各図において、ｓはラプラス演算子、Ｍ
ｖは仮想質量、Ｃｖは粘性抵抗、ｋｆは力反射比、ｋｐ
は変位拡大係数を表している。図８において、ｇｐは比
例成分のゲインを、ｇｉは積分成分のゲインを表す。図
９において、Ｋｃは２つのマス・ダンパモデルを接続す
るばね定数を、Ｄｃは２つのマス・ダンパモデルを接続
するダンピング係数を表す。

【００２７】上記の如くに選定された内部モデル３は、
マスタ１の力情報Ｆｍ及びスレーブ２の力情報Ｆｓの２
つが入力されると、内部モデルによる演算が実行されて
マスタ側内部モデル出力Ｐｖｍ及びスレーブ側内部モデ
ルＰｖｓの２つを出力する。モデル出力Ｐｖｍは出力Ｒ
ｍとして位置追従制御手段１０ａに供給され、モデル出
力Ｐｖｓは出力Ｒs として位置追従制御手段１０ｂに供
給される。

【００２８】このような内部モデルを用いる構成とする
ことの利点は、例えば、力帰還型制御における力制御時
の位相差に起因する発振不安定の問題を解決できること
である。

【００２９】本発明の実施例を図１を参照して説明す
る。同図において図６と対応する部分には同一符号を付
し、かかる部分の説明を省略する。図１に示される構成
では、内部モデルのマスタ側位置情報Ｐｖｍは、操作モ
ードによって制御される第１オフセット補正手段２０ａ
を介して出力Ｒｍとなり、位置追従制御手段１０ａに入
力される。また、内部モデルのスレーブ側位置情報Ｐｖ
ｓは、操作モードによって制御される第２オフセット補
正手段２０ｂを介して出力Ｒｓとなり、位置追従制御手
段１０ｂに入力される。他の構成は同様である。

【００３０】前述した図６に示される仮想内部モデル追
従型バイラテラル制御では、内部モデルの位置情報Ｐｖ
（Ｐｖｍ及びＰｖｓ) が、そのままマスタ及びスレーブ
夫々の位置制御手段への目標位置情報Ｒ（Ｒｍ及びＲ
ｓ）となっているが、本発明の図１に示される構成では
オフセット補正手段が間に介在するために、これらの内
部モデル位置情報Ｐｖ（Ｐｖｍ及びＰｖｓ) と、位置制
御手段への目標位置情報Ｒ（Ｒｍ及びＲｓ）とは分けて
定義されていることに注意すべきである。

【００３１】図１において、内部モデルのマスタ側位置
情報Ｐｖｍは、第１のオフセット補正手段２０ａに入力
される。また、内部モデルのスレーブ側位置情報Ｐｖｓ
は、第２のオフセット補正手段２０ｂに入力される。オ
フセット補正手段２０ａ及び２０ｂは、オフセット補正
装置２０によって同様に構成される。図２は，オフセッ
ト補正装置２０の構成例を示しており、差分器２１、ス
イッチ２２、オフセット量更新回路２３及び目標位置補
正回路２４によって構成されている。オフセット補正装
置２０に入力された内部モデル位置情報Ｐｖは、オフセ
ット補正装置の目標位置補正回路２４及び差分器２１に
入力される。

【００３２】目標位置補正回路２４は、オフセット位置
Ｐｏから見た内部モデル位置Ｐｖを位置制御手段１０へ
の入力である目標位置情報Ｒとして生成するものであ
り、次式の減算処理を行う。 Ｒ [k]＝Ｐｖ [k]−Ｐｏ [k] …（１） ここで、オフセット位置Ｐｏは、差分器２１、スイッチ
２２及びオフセット量更新回路２３を経由して生成され
る。

【００３３】差分器２１は、減算器とレベル保持器によ
って構成されて、１制御周期以前の内部モデル位置Ｐｖ
［ｋ−１］と現時点での内部モデル位置Ｐｖ［ｋ］との
変化量を算出し、その差分量Ｄｖを操作モードに対応し
て導通するスイッチング回路２２に送る。差分量Ｄｖ
は、 Ｄｖ[k] ＝Ｐｖ[k] −Ｐｖ[k-1] …（２） となる。

【００３４】オフセット量更新回路２３は、加算器とレ
ベル保持器によって構成される積算器であり、スイッチ
２２から送られた値Ｄｖと１制御周期以前のオフセット
量Ｐｏ[k-1] とを加算する。スイッチの状態により、次
の出力を発生する。 オフの時、 Ｐｏ[k] ＝Ｐｏ[k-1] オンの時、 Ｐｏ[k] ＝Ｐｏ[k-1] ＋Ｄｖ[k] …（３） と、オフセット位置を更新する。

【００３５】すなわち、スイッチ２２がオフであれば、
オフセット量は更新されないが、スイッチがオンになる
と、Ｐｏ[k] −Ｐｏ[k-1] ＝Ｐｖ[k] −Ｐｖ[k-1] とな
るように更新される。

【００３６】式（１）乃至（３）より、スイッチがオフ
であれば、 Ｒ[k] ＝Ｐｖ[k] ＋ｃｏｎｓｔ． となり、目標位置は内部モデルの位置を一定量シフトし
ただけで、内部モデルの運動と一致し目標運動値を逐次
生成する。スイッチがオンであれば、 Ｒ[k] ＝Ｐｖ[k] −（Ｐｏ[k-1] ＋Ｐｖ[k] −Ｐｖ[k-
1] ） ＝Ｐｖ[k-1] −Ｐｏ[k-1] ＝Ｒ[k-1] ＝ｃｏｎｓｔ． となって、目標位置Ｒは変化せず、スイッチがオンに切
替えられた時点での目標位置情報を保持し続ける。

【００３７】すなわち、スイッチ２２がオン状態である
限り、オフセット補正装置２０の出力である目標位置情
報Ｒは変化せず、内部モデル位置Ｐｖと目標位置Ｒとの
差が常にオフセット量Ｐｏとなるように処理されるので
ある。

【００３８】次に、マニピュレータの操作モードとスイ
ッチ２２のオン・オフ状態との対応関係について説明す
る。

【００３９】既述したように、上記オフセット補正回路
２０は、マスタ側のオフセット補正手段２０ａ及びスレ
ーブ側のオフセット補正手段２０ｂとして用いられる。
従って、図１に示される構成では２つのスイッチが用い
られている。オフセット補正手段２０ａのスイッチ２２
を第１スイッチ、オフセット補正手段２０ｂのスイッチ
２２を第２スイッチとしたとき、操作モードと各スイッ
チのオン／オフ状態の関係を表１に示す。各スイッチ
は、図示しない制御回路によって制御される。

【００４０】同表に示されるように、待機モードでは、
第１及び第２スイッチは共にオンに設定される。このモ
ードでは、操作者はマスタスレーブ操作を要求していな
いので、マスタスレーブとも動作しないようにすること
が必要である。このモードで第１スイッチはオンである
ため、先述した第１オフセット補正手段は目標位置情報
を変更しない。すなわち、マスタ側の目標位置は常に一
定値を保持し続け、マスタはサーボロック状態となる。
また第２スイッチもオンであるため、スレーブ側も一定
位置を保持するように位置制御される。

【００４１】バイラテラルモードでは、第１スイッチ及
び第２スイッチは共にオフに設定される。内部モデル
は、マスタの力情報Ｆｍとスレーブの力情報Ｆｓとを受
け、夫々の内部モデルによる位置出力値Ｐｖｍ、Ｐｖｓ
を更新する。オフセット補正手段２０ａ及び２０ｂは、
第１及び第２スイッチがオフであるため、オフセット量
更新回路２３に保持しているオフセット量は更新され
ず、内部モデルの出力Ｐｖの変化量と目標位置情報Ｒの
変化量は全く同一となる。

【００４２】従って、内部モデルの運動と実アームの運
動とが一致するように制御され、バイラテラルモードを
実現できる。また、マスタ側及びスレーブ側の各オフセ
ット量は、動作モードを切替えた時点での、内部モデル
位置と目標位置（位置制御手段により実アームの設定点
位置と一致している）との差に相当した一定値であり、
その結果、切替えた時点でのマスタスレーブ位置からバ
イラテラルモードが実現されることになる。

【００４３】マスタシフトモードは、スレーブを現在位
置に保持しつつ、マスタを任意位置に移動することがで
きるモードである。マスタシフトモードでは、第１スイ
ッチはオフ、第２スイッチはオンに設定される。第１ス
イッチがオフであるため、マスタに加えられる操作力
（マスタの力情報）により、内部モデルの出力が変化
し、それに伴いマスタは移動する。しかし、第２スイッ
チがオンであるため、スレーブは目標位置情報が更新さ
れないため動かない。

【００４４】速度操作モードは、ジョイスティックのよ
うにスレーブを指令速度で指令方向に運動させるモード
である。ここでは、マスタに加えた力情報の大きさを指
令速度とみなし、加えた力の方向にスレーブを運動させ
ることを考えている。この速度操作モードでは、第１ス
イッチがオンに、第２スイッチがオフに設定される。第
１スイッチはオンであるため、操作力に関わらずマスタ
は移動しない。

【００４５】しかし、内部モデルには、マスタの力情報
及びスレーブの力情報が入力され、その合力の方向に移
動する。通常、スレーブアームは外部環境に拘束されて
いないので、スレーブ反力は“０”であり、内部モデル
は操作力により、自在に運動することになる。また、ス
レーブのオフセット補正手段２０ｂの第２スイッチはオ
フであるため、内部のオフセット位置は変わらず、スレ
ーブアームのみが操作モードを切り替えた時点のスレー
ブ位置から内部モデルの運動に合わせ動くことになる。
すなわち、操作者が動かした方向に加えた力に比例した
速度で、スレーブアームを自在に動かせる速度操作モー
ドが実現できる。

【００４６】以上のように、操作モードに対応してマス
タ側・スレーブ側の２つのオフセット補正装置２０の動
作を切替えるだけで、制御アルゴリズムを分岐選択しな
くとも、マスタスレーブに要求される多機能を容易に実
現できる。

【００４７】次に、本発明の第２の実施例について図３
を参照して説明する。同図において図１の構成と対応す
る部分には同一符号を付し、かかる部分の説明は省略す
る。この実施例では、内部モデルへの入力である力情報
を操作モードに応じて補正することができる。

【００４８】図３において、マスタアーム１及び内部モ
デル３間の力情報Ｆｍの経路にマスタ側力情報補正手段
４０ａが設けられている。また、スレーブアーム２及び
内部モデル３間の力情報Ｆｓの経路にスレーブ側力情報
補正手段４０ｂが設けられている。他の構成は、図１の
構成と同様である。

【００４９】マスタ側力情報補正手段４０ａ及びスレー
ブ側力情報補正手段４０ｂは、力情報補正装置４０とし
て構成される。力情報補正装置４０は、例えば、図５に
示されるように処理切替スイッチ４１、不感帯処理回路
４２及び力情報増幅回路４３によって形成される。処理
切替スイッチ４１のＢ側選択動作によって不感帯処理回
路４２を経由した力情報が力情報増幅回路４３に供給さ
れ、Ａ側選択動作によって不感帯処理回路４２を経由し
ない力情報が力情報増幅回路４３に供給される。不感帯
処理回路４２は、力検出時に混入する外乱の影響を減少
する。力情報増幅回路４３は、操作モードに対応して増
幅率Ｇｆを設定し、この増幅率Ｇｆで力情報を増幅して
内部モデル３に供給する。本実施例における操作モード
と力情報補正手段の動作状態との対応関係を表２に示
す。

【００５０】待機モードでは、マスタ側及びスレーブ側
共に、処理切替スイッチ４１はＡ側、力情報増幅回路４
３の利得はＧｆ＝０に設定される。第１の実施例で述べ
たオフセット補正手段の採用でアームはサーボロックさ
れているため、力情報補正手段を用いる必要はない。し
かし、力センサ出力に有色性の外乱が混入すると、内部
モデルはその外乱により運動し続け、長時間放置してい
ると、計算機のオーバーフローを生ずる可能性が考えら
れる。このような事態を回避する目的で、待機モード時
の力情報増幅率Ｇｆを強制的に“０”にして、待機モー
ド時には内部モデルに力を加えないようにしている。

【００５１】バイラテラルモードでは、マスタ側及びス
レーブ側共に、処理切替スイッチ４１はＡ側、力情報増
幅回路４３の利得はＧｆ＝１に設定される。このモード
では、マスタ側及びスレーブ側共に力補正の必要がない
ため、力情報補正装置４０は検出操作力をそのまま位置
検出手段に送る。

【００５２】マスタシフトモードでは、マスタ側の処理
切替スイッチ４１はＡ側、力情報増幅回路４３の利得は
Ｇｆ＝１、スレーブ側の処理切替スイッチ４１はＡ側、
力情報増幅回路４３の利得はＧｆ＝０に設定される。こ
のモードでは、マスタ側力情報補正手段は検出操作力を
そのまま位置検出手段に送る。スレーブ側力情報補正手
段は、バイラテラルモードと同じで良いが、マスタを自
在に動かせるようにするために、スレーブ側反力を除去
するのが適当である。そこで、力情報増幅手段の増幅率
を０にしている。

【００５３】速度操作モードでは、マスタ側の処理切替
スイッチ４１はＢ側、力情報増幅回路４３の利得は、図
７に示される内部モデルを使用した場合、Ｇｆ＝ｋｖＣ
ｖ／ｋｐ、スレーブ側の処理切替スイッチ４１はＡ側、
力情報増幅回路４３の利得はＧｆ＝０に設定される。こ
の場合の動作について説明する。このモードにおける力
情報補正手段の主目的は、良好な速度操作モードの実現
にある。上述したようにオフセット補正手段を内部モデ
ルを使うバイラテラル制御方式に付加することにより、
速度操作モードを実現できる。しかしながら、内部モデ
ルを使うバイラテラル制御方式は、もともと良好な力制
御を実現することを指向しており、速度操作を行うこと
を前提にしたものではない。内部モデルのパラメータ調
整はバイラテラルモードにおける良好な力制御を実現す
る見地から行われる。このため、良好な速度操作を実現
する重要なファクタの一つである操作力と指令速度の関
係は上記パラメータ調整で決まり、速度操作モードにお
いて、操作力と指令速度の関係を任意に設定する自由度
がない。第２の実施例で付加された力情報補正手段は、
以下に述べるようにかかる点の改善を図ることをも可能
とする。

【００５４】例えば、図７の内部モデルが用いられ、ス
レーブアームが物体に接触していない状態（スレーブ反
力Ｆｓが０である）で、操作力Ｆｍと指令速度Ｖの関係
を考えると、内部モデルのスレーブ側速度出力は、 Ｖ＝ｋｐ・（１／Ｃｖ）・Ｆｍ により、スレーブアームの指令速度Ｖが決定される。そ
こで、力情報補正手段では、マスタ側力情報増幅回路４
３の増幅率をＣｖ・ｋｖ／ｋｐに設定し、 Ｖ＝ｋｐ・（１／Ｃｖ）・（Ｃｖ・ｋｖ／ｋｐ）・Ｆｍ ＝ｋｖ・Ｆｍ の関係が得られるようにする。係数Ｋｖをマスタ側の力
情報増幅回路４３の利得で調整することによって、操作
力Ｆｍと指令速度Ｖの関係を設定することが可能とな
る。

【００５５】また、力検出時の外乱影響でスレーブアー
ム２が動作するのは好ましくないため、処理切り替えス
イッチ４３を不感帯処理手段４２のＢ側に切替え、力情
報増幅手段４３の全般で不感帯処理している。このモー
ドでは、スレーブ側力情報補正手段４０ｂは意味を持た
ないため、この実施例では増幅率を“０”としている。

【００５６】しかしながら、通常の速度操作モードで
は、作業対象に過大な力を加える危険があるため、スレ
ーブがコンプライアンスを持つ速度操作モードが望まれ
る場合がある。表２には示していないが、この速度操作
モードにおいて、スレーブ側の力情報補正手段４０ｂの
処理切替スイッチ４３を不感帯処理のＢ側とし、力情報
増幅手段４３の増幅率を図７のモデルに対応してＣｖ・
ｋｖ／ｋｐに設定する。こうすると、スレーブアームが
作業対象に接触した際に反力が発生し、操作力が作業対
象への作用力の力反射比ｋｆ倍した値と一致するため、
速度操作モードのままで、かなりのマスタスレーブ作業
を複合的に行える。つまり、４０ｂの処理を若干変更す
るだけで、コンプライアンスを持つ速度操作モードを容
易に実現できる。

【００５７】

【表１】

【００５８】

【表２】

【００５９】

【発明の効果】以上説明したように本発明のマスタスレ
ーブ制御装置は、オフセット補正手段を備えて、バイラ
テラルモードのアルゴリズムによる出力を要求される操
作モードに対応して適宜に補正して出力を得るようにし
ているので、従来のように、マスタスレーブの複数の操
作モード毎に制御処理アルゴリズムを設ける必要がな
く、全体の処理手段（制御プログラム）をコンパクトに
することが可能である。また、制御処理の流れが各操作
モードにおいて共通にオフセット補正手段を経由するた
め、従来のような各操作モード相互間の切替の際に必要
とされた処理アルゴリズム間の同期プロセスが不要で、
滑らかな操作モード切替を実現可能である。更に、力情
報補正手段を含む速度操作モードでは、スレーブが作業
対象に過負荷を与えないコンプライアンスをもつ動作を
容易に実現することが可能になる。

【図面の簡単な説明】

【図１】本発明の第１の実施例を示すブロック図。

【図２】図１に示されるオフセット補正手段の構成例を
示すブロック図。

【図３】本発明の第２の実施例を示すブロック図。

【図４】本発明の概念のモデルを示す説明図。

【図５】図３に示される力補正手段の構成例を示すブロ
ック図。

【図６】内部モデル追従型制御の構成例を示すブロック
図。

【図７】内部モデルの第１の構成例を示すブロック図。

【図８】内部モデルの第２の構成例を示すブロック図。

【図９】内部モデルの第３の構成例を示すブロック図。

【符号の説明】

１ マスタアーム ２ スレーブアーム ３ 内部モデル ２１ 差分器 ２２ スイッチ ２３ オフセット量更新回路 ４１ 増幅器 ４２ 不感帯処理回路 ４３ 処理切替スイッチ ５１ 力反射比用増幅器 ５２ 変位拡大比用増幅器 １０ａ マスタ側追従制御手段 １０ｂ スレーブ側追従制御手段 ２０ａ マスタ側オフセット補正手段 ２０ｂ スレーブ側オフセット補正手段 ４０ａ マスタ側力補正手段 ４０ｂ スレーブ側力補正手段

Claims (1)

【特許請求の範囲】
1. 【請求項１】マスタアーム及びスレーブアーム各々にお
   いて検出された力検出力を入力として前記マスタアーム
   及び前記スレーブアーム各々の目標運動値を生成する内
   部モデルと、 与えられる目標運動値を追従するように前記マスタアー
   ム及び前記スレーブアームを夫々動作させる第１及び第
   ２の位置追従手段と、 前記マスタアームの目標運動値をアームの操作モードに
   対応してオフセット補正して前記第１の位置追従手段に
   与える第１オフセット補正手段と、 前記スレーブアームの目標運動値をアームの操作モード
   に対応してオフセット補正して前記第２の位置追従手段
   に与える第２オフセット補正手段と、 を備えたことを特徴とするマスタスレーブマニピュレー
   タの制御装置。