JPH06138945A - Controller for master-slave manipulator - Google Patents

Abstract

PURPOSE:To obtain a compact processing means by correcting properly the offset by means of an offset correcting means in response to an operation mode that requires the output by the algorithm of a bilateral mode. CONSTITUTION:A master arm 1 and a slave arm 2 operate by using the shaft driving commands Tm and Ts, i.e., the outputs of the master and slave position follow-up control means 10a and 10b as the inputs. Then both arms 1 and 2 outputs the shaft articular angle information thetam and thetas and the outer force information Fm and Fs converted into the common coordinates. The master position informtion Pvm and the slave position information Pvs of an inside model are turned into the outputs Rm and Rs via the 1st and 2nd offset correcting means 20a and 20b which are controlled in each operation mode. These outputs Rm and Rs are supplied to both means 10a and 10b. Thus the inside model information Pv (Pvm and Pvs) and the target position information R(Rm and Rs) supplied to the position control means defined separately from each other.

Description

【発明の詳細な説明】Detailed Description of the Invention

【０００１】[0001]

【産業上の利用分野】本発明は、遠隔作業用のロボット
であるマスタスレーブマニピュレータ装の制御装置に関
する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for a master-slave manipulator device which is a robot for remote work.

【０００２】[0002]

【従来の技術】近年、マスタスレーブマニピュレータの
多機能化が進み、従来の「バイラテラルモード」に加
え、「マスタシフト（原点移動）モード」、「速度操作
モード」等の機能が要求され、準備されるようになっ
た。2. Description of the Related Art In recent years, master-slave manipulators have become multi-functional, and in addition to the conventional "bilateral mode", functions such as "master shift (origin movement) mode" and "speed operation mode" are required. Came to be.

【０００３】「バイラテラルモード」とは、マスタアー
ム（操縦装置）の位置を追従するように、作業環境側の
スレーブアームを制御するモードであり、通常、マスタ
スレーブマニピュレータは、バイラテラルモードで使用
される。The "bilateral mode" is a mode in which the slave arm on the working environment side is controlled so as to follow the position of the master arm (control device). Normally, the master-slave manipulator is used in the bilateral mode. To be done.

【０００４】「マスタシフトモード」とは、異構造マス
タスレーブにおけるマスタアームとスレーブアームの動
作範囲の違いや、オペレータの作業し易さを考慮して準
備されたモードである。バイラテラルモードで作業して
いる場合に、マスタアームの動作範囲限界に達したとき
や、操作者が作業しにくい姿勢になったときに、このマ
スタシフトモードで任意の位置にマスタだけを移動する
ことができる。このモードの間、スレーブアームは停止
状態となり、マスタアームは自在に操作可能となる。そ
して、バイラテラルモードに復帰すると同時に、マスタ
シフトモードで決められたマスタスレーブ間のオフセッ
ト関係を維持したままバイラテラル作業に移行する。The "master shift mode" is a mode prepared in consideration of the difference in the operating range of the master arm and the slave arm in the master-slave having a different structure and the ease of work of the operator. When working in the bilateral mode, when the operating range limit of the master arm is reached, or when the operator is in a position where it is difficult to work, the master shift mode moves only the master to an arbitrary position. be able to. During this mode, the slave arm is stopped and the master arm can be operated freely. Then, at the same time as returning to the bilateral mode, the bilateral work is started while maintaining the offset relationship between the master and slave determined in the master shift mode.

【０００５】「速度操作モード」とは、ジョイスティッ
クでロボットを操作する感覚で、スレーブアームを速度
指令操作するモードである。このモードにより、スレー
ブアーム対応点（通常手先）の大きな移動を行う場合等
に、上記マスタシフトモードによるマスタ原点移動を繰
り返してスレーブアームを操作する煩雑さを避けること
ができる。この速度指令モードでは、通常、マスタアー
ムはサーボロックされ、操作者がマスタアームに加えた
力によって生じた微少変位や操作力が検出される。この
検出量に比例した速度でスレーブアーム先端を動作させ
る。The "speed operation mode" is a mode in which the speed command operation is performed on the slave arm as if the robot was operated with a joystick. By this mode, when a large movement of the slave arm corresponding point (normal hand) is performed, it is possible to avoid the complexity of operating the slave arm by repeating the master origin movement in the master shift mode. In this speed command mode, the master arm is usually servo-locked, and a minute displacement or an operation force generated by a force applied to the master arm by the operator is detected. The tip of the slave arm is operated at a speed proportional to the detected amount.

【０００６】上述した機能を、マスタスレーブで実現す
る場合、各機能毎にアルゴリズムを準備し、要求された
操作モードに応じて適切な制御アルゴリズムを選択・切
替える、分岐処理が一般的に採用されている。When the above-mentioned functions are realized by a master / slave, a branching process is generally adopted in which an algorithm is prepared for each function and an appropriate control algorithm is selected / switched in accordance with a requested operation mode. There is.

【０００７】複数のモードを選択して異なる機能を実現
する分岐処理は、操作モード毎に制御アルゴリズムを切
替える当然の結果として、全体の制御プログラムが巨大
化し複雑になる欠点がある。更に、異なるアルゴリズム
間を移行する際に、連続的なマスタスレーブ操作を実現
するため、操作モードをどのように切替えても、各アー
ムを滑らかに動作させるマスタスレーブ間オフセット処
理及び同期処理がかなり複雑である。The branch processing for selecting a plurality of modes to realize different functions has a drawback that the entire control program becomes huge and complicated as a result of switching the control algorithm for each operation mode. Furthermore, in order to realize continuous master-slave operation when transitioning between different algorithms, the master-slave offset processing and synchronization processing that operate each arm smoothly regardless of how the operation mode is switched is quite complicated. Is.

【０００８】一方、バイラテラルモード及びマスタ原点
移動機能に限定した異構造マスタスレーブでは、分岐処
理をしなくとも、リレーを利用してバイラテラルモード
用の単一アルゴリズムでモードを切替えできる方式（例
えば、特開平３−１８４７７９号公報）も提案されてい
る。On the other hand, in the master-slave having a different structure limited to the bilateral mode and the master origin moving function, the mode can be switched by a single algorithm for the bilateral mode using a relay without branching (for example, Japanese Patent Laid-Open No. 3-184779) has also been proposed.

【０００９】この方式によれば、複数のアルゴリズムを
用意しなくても良いため、プログラムがコンパクト、モ
ード切替による同期処理が不要、更にオフセット処理が
滑らかになる等の利点がある。しかしながら、この発明
は「バイラテラルモード」及び「マスタシフト（原点移
動）モード」の２つの機能の実現を前提として考慮され
たもので、この方式を拡張して、上記した「速度操作モ
ード」を実現することは出来ない。According to this method, since it is not necessary to prepare a plurality of algorithms, there are advantages that the program is compact, synchronization processing by mode switching is unnecessary, and offset processing is smooth. However, the present invention has been considered on the premise that the two functions of the "bilateral mode" and the "master shift (origin movement) mode" are realized, and this method is expanded to include the "speed operation mode" described above. It cannot be realized.

【００１０】すなわち、「速度操作モード」を実現する
には、依然として速度操作モード専用の別途に用意され
た制御アルゴリズムを必要とし、従来の分岐処理と同様
の問題点が生ずる。That is, in order to realize the "speed operation mode", a separately prepared control algorithm dedicated to the speed operation mode is still required, and the same problems as in the conventional branching process occur.

【００１１】[0011]

【発明が解決しようとする課題】以上のように、従来の
多機能を備えたマスタスレーブの制御装置では、各機能
毎に処理アルゴリズムを設けて分岐処理しるため、プロ
グラムが巨大化して複雑となる欠点があった。また、各
操作モードをどのように切り替えても、滑らかにマスタ
スレーブを動作させるためのオフセット処理が複雑にな
る欠点があった。As described above, in the conventional master-slave control device having multiple functions, a processing algorithm is provided for each function to perform branch processing, so that the program becomes huge and complicated. There was a drawback. In addition, there is a drawback that the offset process for smoothly operating the master-slave becomes complicated no matter how the operation modes are switched.

【００１２】よって、本発明は、少ない制御アルゴリズ
ムで各操作モードを実現でき、どのように操作モードを
切替えてもスレーブアームが滑らかで連続的に動作する
マスタスレーブの制御装置を提供することである。Accordingly, the present invention is to provide a master-slave control device which can realize each operation mode with a small number of control algorithms and which has a slave arm which operates smoothly and continuously no matter how the operation mode is switched. .

【００１３】[0013]

【課題を解決するための手段】上記目的を達成するため
本発明のマスタスレーブマニピュレータの制御装置は、
マスタアーム及びスレーブアーム各々において検出され
た力検出力を入力として上記マスタアーム及び上記スレ
ーブアーム各々の目標運動値を生成する内部モデルと、
与えられる目標運動値を追従するように上記マスタアー
ム及び上記スレーブアームを夫々動作させる第１及び第
２の位置追従手段と、上記マスタアームの目標運動値を
アームの操作モードに対応してオフセット補正して上記
第１の位置追従手段に与える第１オフセット補正手段
と、上記スレーブアームの目標運動値をアームの操作モ
ードに対応してオフセット補正して上記第２の位置追従
手段に与える第２オフセット補正手段と、を備えたこと
を特徴とする。In order to achieve the above object, a controller for a master-slave manipulator according to the present invention comprises:
An internal model that generates a target motion value of each of the master arm and the slave arm by inputting the force detection force detected in each of the master arm and the slave arm,
First and second position tracking means for respectively operating the master arm and the slave arm so as to follow a given target motion value, and the target motion value of the master arm is offset-corrected in accordance with the operation mode of the arm. And a first offset correcting means for giving the first position following means to the first position following means, and a second offset given to the second position following means for offset correcting the target motion value of the slave arm in accordance with the operation mode of the arm. And a correction means.

【００１４】[0014]

【作用】内部モデルを用いるマスタスレーブマニピュレ
ータのバイラテラル制御アルゴリズムを基本とし、その
内部モデル出力とこの出力にアームを追従させる位置追
従制御手段の入力との間に、マスタスレーブマニピュレ
ータの操作モードに対応してオフセット量を設定するオ
フセット補正手段を配置する。The operation is based on the bilateral control algorithm of the master-slave manipulator using the internal model, and corresponds to the operation mode of the master-slave manipulator between the output of the internal model and the input of the position tracking control means for making the arm follow the output. Then, offset correction means for setting the offset amount is arranged.

【００１５】この結果、バイラテラル制御アルゴリズム
を活用して他の操作モードを実現でき、同じ数の操作モ
ードを実現する制御プログラムに比べて全体の制御プロ
グラム量を少なくすることが可能となる。As a result, it is possible to realize another operation mode by utilizing the bilateral control algorithm, and it is possible to reduce the entire control program amount as compared with a control program that realizes the same number of operation modes.

【００１６】[0016]

【実施例】まず、内部モデルによるバイラテラル制御の
概念を説明する。図４は、本発明の基礎となる内部モデ
ル（仮想モデル）及びオフセット補正概念を示してい
る。内部モデルは、例えば、マスタスレーブマニピュレ
ータの力学的な仮想モデルであり、計算機上に構築され
る。図示の内部モデルはマス・ダンパからなる単純化さ
れた仮想モデルの例を示しており、マスタ側・スプレー
ブ側の内部モデル位置Ｐｖｍ，Ｐｖｓは一致している
（Ｐｖｍ＝Ｐｖｓ＝Ｐｖ）。質量Ｍｖの質点は固定ベー
スと粘性係数Ｃｖのダンパで接合され、マスタ側力検出
手段により検出されたマスタ操作力Ｆｍとスレーブ側力
検出手段により検出されたスレーブ反力Ｆｓとの合力が
加わり運動する。マスタ操作力Ｆｍ及びスレーブ反力Ｆ
ｓは共に力検出器に作用する方向で図示されている。操
作力Ｆｍと反力Ｆｓが釣り合えば、すなわち、Ｆｍ＝−
Ｆｓであればその運動を停止し、反力Ｆｓがなければ、
すなわち、スレーブが外部環境と干渉していなければ操
作力Ｆｍにより自在に運動する。このとき、実マスタア
ーム・実スレーブアームの設定点（通常手先）を、この
内部モデルの運動に一致させれば、操作者はスレーブア
ームを自在に操作していることになり、かつ、スレーブ
に加わる作業時反力Ｆｓを感じ取れることになる。そこ
で、内部モデル追従型バイラテラル制御は、マスタ側と
スレーブ側のアームが、夫々のモデル位置を追従するよ
うに実アームを制御し、「バイラテラルモード」を実現
している。First, the concept of bilateral control by an internal model will be described. FIG. 4 shows an internal model (virtual model) and an offset correction concept which are the basis of the present invention. The internal model is, for example, a dynamic virtual model of a master-slave manipulator, and is built on a computer. The illustrated internal model shows an example of a simplified virtual model including a mass damper, and the internal model positions Pvm and Pvs on the master side and the splay side are the same (Pvm = Pvs = Pv). The mass point of the mass Mv is joined to a fixed base by a damper having a viscosity coefficient Cv, and a resultant force of the master operation force Fm detected by the master side force detection means and the slave reaction force Fs detected by the slave side force detection means is added to the movement. To do. Master operating force Fm and slave reaction force F
Both s are shown in the direction acting on the force detector. If the operating force Fm and the reaction force Fs are balanced, that is, Fm =-
If it is Fs, the motion is stopped, and if there is no reaction force Fs,
That is, if the slave does not interfere with the external environment, it freely moves by the operating force Fm. At this time, if the set points (normal hands) of the real master arm and the real slave arm match the movement of this internal model, the operator is free to operate the slave arm, and The reaction force Fs can be felt during the applied work. Therefore, in the internal model following type bilateral control, the master side arm and the slave side arm control the actual arms so as to follow the respective model positions, thereby realizing the “bilateral mode”.

【００１７】更に、図４に示される第１スイッチＳＷ１
は、マスタアームのオフセット位置Ｐｏｍが内部モデル
と連動するか否かを決める。第２スイッチＳＷ２は、ス
レーブアームのオフセット位置Ｐｏｓが内部モデルと連
動するか否かを決める。スイッチがオフのときのオフセ
ット位置は、内部モデルの運動とは無関係に一定値を取
る。また、スイッチがオンのときのオフセット位置は、
内部モデルの運動に合わせて変化する。Further, the first switch SW1 shown in FIG.
Determines whether the offset position Pom of the master arm is interlocked with the internal model. The second switch SW2 determines whether the offset position Pos of the slave arm is interlocked with the internal model. The offset position when the switch is off has a constant value regardless of the movement of the internal model. The offset position when the switch is on is
It changes according to the movement of the internal model.

【００１８】このモデルの従来例との相違点は、従来例
が内部モデル位置Ｐｖそのものを目標位置Ｒとしていた
のに対し、本発明では、オフセット位置Ｐｏから見た内
部モデル位置Ｐｖが、実アームの追従すべき目標位置Ｒ
となる点にある。The difference between this model and the conventional example is that the internal model position Pv itself is the target position R in the conventional example, but in the present invention, the internal model position Pv viewed from the offset position Po is the actual arm. Target position R to follow
There is a point.

【００１９】２つのスイッチＳＷ１及びＳＷ２が共にオ
フであれば、マスタ及びスレーブアームの初期位置が異
なるだけで、従来の内部モデルを使ったバイラテラル制
御と変らないが、どちらかのスイッチがオンとなると、
状態は大きく異なってくる。If the two switches SW1 and SW2 are both off, only the initial positions of the master and slave arms are different and the bilateral control using the conventional internal model is the same, but either switch is on. Then,
The conditions are very different.

【００２０】スイッチＳＷ１がオンのとき、マスタ側オ
フセット位置Ｐｏｍと内部モデルＰｖは連動するので、
マスタアームの目標位置Ｒｍは、スイッチＳＷ１がオン
になる直前の目標位置を保持し続け、その結果、マスタ
アームはサーボロックされて動かない。同様に、スイッ
チＳＷ２がオンのときは、スレーブアームの目標位置Ｒ
ｓがスイッチＳＷ２がオンになる直前の目標位置に保持
され、スレーブアームが動かない。When the switch SW1 is turned on, the master side offset position Pom and the internal model Pv are interlocked.
The target position Rm of the master arm continues to hold the target position immediately before the switch SW1 is turned on, and as a result, the master arm is servo-locked and does not move. Similarly, when the switch SW2 is on, the target position R of the slave arm is
s is held at the target position immediately before the switch SW2 is turned on, and the slave arm does not move.

【００２１】しかし、内部モデルは、常に、マスタ操作
力Ｆｍとスレーブ反力Ｆｓの合力で運動しているため、
スイッチがオフ状態である側のアームは内部モデルの運
動に合わせて動く。例えば、スイッチＳＷ１がオン、ス
イッチＳＷ２がオフのとき、マスタアームはサーボロッ
クされて動かないが、内部モデルは合力により運動して
おり、スレーブアームは内部モデルの運動を追従するよ
うに動作する。操作者がマスタに操作力Ｆｍを与えてお
り、スレーブが拘束されていない（スレーブ反力Ｆｓが
“０”）状態であれば、内部モデルは操作力Ｆｍにより
モデル粘性摩擦力と釣り合った速度で運動するので、結
果として、スレーブアームのみが、操作力に比例した速
度で動く「速度操作モード」を実現できる。However, since the internal model always moves by the resultant force of the master operating force Fm and the slave reaction force Fs,
The arm on the side where the switch is off moves in accordance with the movement of the internal model. For example, when the switch SW1 is on and the switch SW2 is off, the master arm is servo-locked and does not move, but the internal model is moving due to the resultant force, and the slave arm operates so as to follow the motion of the internal model. When the operator applies the operation force Fm to the master and the slave is not restrained (slave reaction force Fs is “0”), the internal model operates at a speed balanced with the model viscous friction force by the operation force Fm. As a result, since only the slave arm moves at a speed proportional to the operation force, a “speed operation mode” can be realized.

【００２２】表１に示されるように動作モードとスイッ
チの関係を選定すると、「待機モード」で、マスタアー
ム及びスレーブアーム共にサーボロック状態となる。
「マスタシフトモード」で、マスタアームだけを自在に
動かせる。「速度操作モード」で、マスタアームをジョ
イスティック操作しているかのようにしてスレーブアー
ムを動かせる。When the relationship between the operation mode and the switch is selected as shown in Table 1, both the master arm and the slave arm are in the servo lock state in the "standby mode".
In the "master shift mode", you can freely move only the master arm. In "speed operation mode", you can move the slave arm as if you were operating the joystick on the master arm.

【００２３】以上のように、基本として「バイラテラル
モード」を実現するアルゴリズムだけ調整・準備等すれ
ば、操作者の要求する操作モードに連動するスイッチ手
段を備えたオフセット補正手段により、各種多機能を実
現することが可能となる。As described above, basically, if only the algorithm for realizing the "bilateral mode" is adjusted and prepared, the offset correction means provided with the switch means linked with the operation mode requested by the operator enables various multifunctional functions. Can be realized.

【００２４】次に、本発明の理解を容易にするために、
特公平４−４５３０４号公報により提案されているマニ
ピュレータの制御装置について説明する。この装置の構
成は、図６に示されるように、マスタアーム１、スレー
ブアーム２、内部モデル３、マスタ側追従装置１０ａ及
びスレーブ側追従装置１０ｂによって示される。Next, in order to facilitate understanding of the present invention,
A control device for a manipulator proposed by Japanese Patent Publication No. 4-45304 will be described. The configuration of this device is shown by a master arm 1, a slave arm 2, an internal model 3, a master side follower 10a and a slave side follower 10b, as shown in FIG.

【００２５】マスタアーム１及びスレーブアーム２は、
共にバイラテラル制御を行うのに必要な公知の構成要素
を含んでいる。マスタアーム１は、マスタの位置追従制
御手段１０ａの出力である各軸駆動指令Ｔｍを入力とし
て動作し、各軸関節角度情報θｍ及び共通座標に変換し
た外力情報Ｆｍを出力する。スレーブアーム２はスレー
ブの位置追従制御手段１０ｂの出力である各軸駆動指令
Ｔｓを入力として動作し、各軸関節角度情報θｓ及び共
通座標に変換した外力情報Ｆｓを出力する。位置追従制
御手段１０ａは、運動学に基づいた座標変換を備えた公
知の位置制御手段で、共通座標系表記したアーム設定点
（例えば手先）の位置・姿勢が、共通座標系で夫々与え
られた目標位置情報Ｒm （位置・姿勢）と一致するよう
に、フィードバック制御する。位置追従制御手段１０ｂ
も、運動学に基づいた座標変換を備えた公知の位置制御
手段で、共通座標系表記したアーム設定点（例えば手
先）の位置・姿勢が、共通座標系で夫々与えられた目標
位置情報Ｒｓ（位置・姿勢）と一致するように、フィー
ドバック制御する。The master arm 1 and the slave arm 2 are
Both include the well-known components necessary to provide bilateral control. The master arm 1 operates by inputting each axis drive command Tm output from the master position tracking control means 10a, and outputs each axis joint angle information θm and external force information Fm converted into common coordinates. The slave arm 2 operates by inputting each axis drive command Ts which is the output of the position tracking control means 10b of the slave, and outputs each axis joint angle information θs and external force information Fs converted into common coordinates. The position tracking control means 10a is a known position control means provided with coordinate conversion based on kinematics, and the position / orientation of the arm set point (for example, the hand) described in the common coordinate system is given in the common coordinate system. Feedback control is performed so as to match the target position information Rm (position / posture). Position tracking control means 10b
Also, the position / orientation of the arm set point (for example, the hand) expressed in the common coordinate system is a known position control means having coordinate conversion based on kinematics, and the target position information Rs ( Feedback control is performed to match the position and orientation.

【００２６】内部モデル３は種々のものが考えられ、例
えば、図７乃至図９に示される公知の構成要素を含むブ
ロックで表現される。図７は、マス・ダンパ力学モデル
を示している。図８は、ＰＤ動作型力制御モデルを示し
ている。図９は、マス・ダンパからなるマスタ側・スレ
ーブ側の力学モデルをバネ・ダンパ接合した力学モデル
を示している。各図において、ｓはラプラス演算子、Ｍ
ｖは仮想質量、Ｃｖは粘性抵抗、ｋｆは力反射比、ｋｐ
は変位拡大係数を表している。図８において、ｇｐは比
例成分のゲインを、ｇｉは積分成分のゲインを表す。図
９において、Ｋｃは２つのマス・ダンパモデルを接続す
るばね定数を、Ｄｃは２つのマス・ダンパモデルを接続
するダンピング係数を表す。Various types of internal models 3 are conceivable, and are represented by blocks including known components shown in FIGS. 7 to 9, for example. FIG. 7 shows a mass damper dynamic model. FIG. 8 shows a PD motion type force control model. FIG. 9 shows a dynamic model in which a master / slave dynamic model composed of a mass damper is joined to a spring / damper. In each figure, s is the Laplace operator, M
v is virtual mass, Cv is viscous resistance, kf is force reflection ratio, kp
Represents the displacement magnification factor. In FIG. 8, gp represents the gain of the proportional component, and gi represents the gain of the integral component. In FIG. 9, Kc represents a spring constant connecting two mass damper models, and Dc represents a damping coefficient connecting two mass damper models.

【００２７】上記の如くに選定された内部モデル３は、
マスタ１の力情報Ｆｍ及びスレーブ２の力情報Ｆｓの２
つが入力されると、内部モデルによる演算が実行されて
マスタ側内部モデル出力Ｐｖｍ及びスレーブ側内部モデ
ルＰｖｓの２つを出力する。モデル出力Ｐｖｍは出力Ｒ
ｍとして位置追従制御手段１０ａに供給され、モデル出
力Ｐｖｓは出力Ｒs として位置追従制御手段１０ｂに供
給される。The internal model 3 selected as described above is
2 of force information Fm of master 1 and force information Fs of slave 2
When one of them is input, the calculation by the internal model is executed to output two of the master side internal model output Pvm and the slave side internal model Pvs. Model output Pvm is output R
m is supplied to the position tracking control means 10a, and the model output Pvs is supplied to the position tracking control means 10b as an output Rs.

【００２８】このような内部モデルを用いる構成とする
ことの利点は、例えば、力帰還型制御における力制御時
の位相差に起因する発振不安定の問題を解決できること
である。An advantage of using such an internal model is that the problem of oscillation instability due to the phase difference during force control in force feedback control can be solved.

【００２９】本発明の実施例を図１を参照して説明す
る。同図において図６と対応する部分には同一符号を付
し、かかる部分の説明を省略する。図１に示される構成
では、内部モデルのマスタ側位置情報Ｐｖｍは、操作モ
ードによって制御される第１オフセット補正手段２０ａ
を介して出力Ｒｍとなり、位置追従制御手段１０ａに入
力される。また、内部モデルのスレーブ側位置情報Ｐｖ
ｓは、操作モードによって制御される第２オフセット補
正手段２０ｂを介して出力Ｒｓとなり、位置追従制御手
段１０ｂに入力される。他の構成は同様である。An embodiment of the present invention will be described with reference to FIG. In the figure, parts corresponding to those in FIG. In the configuration shown in FIG. 1, the master-side position information Pvm of the internal model has the first offset correction means 20a controlled by the operation mode.
It becomes an output Rm via and is input to the position tracking control means 10a. Also, the slave side position information Pv of the internal model
s becomes an output Rs via the second offset correction means 20b controlled by the operation mode, and is input to the position tracking control means 10b. Other configurations are the same.

【００３０】前述した図６に示される仮想内部モデル追
従型バイラテラル制御では、内部モデルの位置情報Ｐｖ
（Ｐｖｍ及びＰｖｓ) が、そのままマスタ及びスレーブ
夫々の位置制御手段への目標位置情報Ｒ（Ｒｍ及びＲ
ｓ）となっているが、本発明の図１に示される構成では
オフセット補正手段が間に介在するために、これらの内
部モデル位置情報Ｐｖ（Ｐｖｍ及びＰｖｓ) と、位置制
御手段への目標位置情報Ｒ（Ｒｍ及びＲｓ）とは分けて
定義されていることに注意すべきである。In the above-described virtual internal model following type bilateral control shown in FIG. 6, position information Pv of the internal model is obtained.
(Pvm and Pvs) are the target position information R (Rm and Rm) to the position control means of the master and the slave, respectively.
1) of the present invention, the internal model position information Pv (Pvm and Pvs) and the target position to the position control means. It should be noted that the information R (Rm and Rs) is defined separately.

【００３１】図１において、内部モデルのマスタ側位置
情報Ｐｖｍは、第１のオフセット補正手段２０ａに入力
される。また、内部モデルのスレーブ側位置情報Ｐｖｓ
は、第２のオフセット補正手段２０ｂに入力される。オ
フセット補正手段２０ａ及び２０ｂは、オフセット補正
装置２０によって同様に構成される。図２は，オフセッ
ト補正装置２０の構成例を示しており、差分器２１、ス
イッチ２２、オフセット量更新回路２３及び目標位置補
正回路２４によって構成されている。オフセット補正装
置２０に入力された内部モデル位置情報Ｐｖは、オフセ
ット補正装置の目標位置補正回路２４及び差分器２１に
入力される。In FIG. 1, the master side position information Pvm of the internal model is input to the first offset correction means 20a. Also, the slave side position information Pvs of the internal model
Is input to the second offset correction means 20b. The offset correction means 20a and 20b are similarly configured by the offset correction device 20. FIG. 2 shows an example of the configuration of the offset correction device 20, which includes a difference unit 21, a switch 22, an offset amount update circuit 23, and a target position correction circuit 24. The internal model position information Pv input to the offset correction device 20 is input to the target position correction circuit 24 and the difference unit 21 of the offset correction device.

【００３２】目標位置補正回路２４は、オフセット位置
Ｐｏから見た内部モデル位置Ｐｖを位置制御手段１０へ
の入力である目標位置情報Ｒとして生成するものであ
り、次式の減算処理を行う。 Ｒ [k]＝Ｐｖ [k]−Ｐｏ [k] …（１） ここで、オフセット位置Ｐｏは、差分器２１、スイッチ
２２及びオフセット量更新回路２３を経由して生成され
る。The target position correction circuit 24 generates the internal model position Pv viewed from the offset position Po as the target position information R which is an input to the position control means 10, and performs a subtraction process of the following equation. R [k] = Pv [k] -Po [k] (1) Here, the offset position Po is generated via the difference unit 21, the switch 22, and the offset amount update circuit 23.

【００３３】差分器２１は、減算器とレベル保持器によ
って構成されて、１制御周期以前の内部モデル位置Ｐｖ
［ｋ−１］と現時点での内部モデル位置Ｐｖ［ｋ］との
変化量を算出し、その差分量Ｄｖを操作モードに対応し
て導通するスイッチング回路２２に送る。差分量Ｄｖ
は、 Ｄｖ[k] ＝Ｐｖ[k] −Ｐｖ[k-1] …（２） となる。The differencer 21 is composed of a subtractor and a level holder, and has an internal model position Pv of one control cycle or earlier.
The amount of change between [k-1] and the current internal model position Pv [k] is calculated, and the difference amount Dv is sent to the switching circuit 22 that is conductive in accordance with the operation mode. Difference amount Dv
Becomes Dv [k] = Pv [k] -Pv [k-1] (2).

【００３４】オフセット量更新回路２３は、加算器とレ
ベル保持器によって構成される積算器であり、スイッチ
２２から送られた値Ｄｖと１制御周期以前のオフセット
量Ｐｏ[k-1] とを加算する。スイッチの状態により、次
の出力を発生する。 オフの時、 Ｐｏ[k] ＝Ｐｏ[k-1] オンの時、 Ｐｏ[k] ＝Ｐｏ[k-1] ＋Ｄｖ[k] …（３） と、オフセット位置を更新する。The offset amount updating circuit 23 is an integrator composed of an adder and a level holder, and adds the value Dv sent from the switch 22 and the offset amount Po [k-1] before one control cycle. To do. The following outputs are generated depending on the state of the switch. When off, Po [k] = Po [k-1] When on, Po [k] = Po [k-1] + Dv [k] (3) The offset position is updated.

【００３５】すなわち、スイッチ２２がオフであれば、
オフセット量は更新されないが、スイッチがオンになる
と、Ｐｏ[k] −Ｐｏ[k-1] ＝Ｐｖ[k] −Ｐｖ[k-1] とな
るように更新される。That is, if the switch 22 is off,
The offset amount is not updated, but when the switch is turned on, it is updated so that Po [k] -Po [k-1] = Pv [k] -Pv [k-1].

【００３６】式（１）乃至（３）より、スイッチがオフ
であれば、 Ｒ[k] ＝Ｐｖ[k] ＋ｃｏｎｓｔ． となり、目標位置は内部モデルの位置を一定量シフトし
ただけで、内部モデルの運動と一致し目標運動値を逐次
生成する。スイッチがオンであれば、 Ｒ[k] ＝Ｐｖ[k] −（Ｐｏ[k-1] ＋Ｐｖ[k] −Ｐｖ[k-
1] ） ＝Ｐｖ[k-1] −Ｐｏ[k-1] ＝Ｒ[k-1] ＝ｃｏｎｓｔ． となって、目標位置Ｒは変化せず、スイッチがオンに切
替えられた時点での目標位置情報を保持し続ける。From equations (1) to (3), if the switch is off, R [k] = Pv [k] + const. Therefore, the target position coincides with the motion of the internal model and the target motion value is sequentially generated only by shifting the position of the internal model by a certain amount. If the switch is on, R [k] = Pv [k]-(Po [k-1] + Pv [k] -Pv [k-
1]) = Pv [k-1] -Po [k-1] = R [k-1] = const. Therefore, the target position R does not change, and the target position information at the time when the switch is turned on is maintained.

【００３７】すなわち、スイッチ２２がオン状態である
限り、オフセット補正装置２０の出力である目標位置情
報Ｒは変化せず、内部モデル位置Ｐｖと目標位置Ｒとの
差が常にオフセット量Ｐｏとなるように処理されるので
ある。That is, as long as the switch 22 is on, the target position information R which is the output of the offset correction device 20 does not change, and the difference between the internal model position Pv and the target position R is always the offset amount Po. Is processed.

【００３８】次に、マニピュレータの操作モードとスイ
ッチ２２のオン・オフ状態との対応関係について説明す
る。Next, the correspondence between the operation mode of the manipulator and the on / off state of the switch 22 will be described.

【００３９】既述したように、上記オフセット補正回路
２０は、マスタ側のオフセット補正手段２０ａ及びスレ
ーブ側のオフセット補正手段２０ｂとして用いられる。
従って、図１に示される構成では２つのスイッチが用い
られている。オフセット補正手段２０ａのスイッチ２２
を第１スイッチ、オフセット補正手段２０ｂのスイッチ
２２を第２スイッチとしたとき、操作モードと各スイッ
チのオン／オフ状態の関係を表１に示す。各スイッチ
は、図示しない制御回路によって制御される。As described above, the offset correction circuit 20 is used as the master side offset correction means 20a and the slave side offset correction means 20b.
Therefore, two switches are used in the configuration shown in FIG. Switch 22 of offset correction means 20a
Table 1 shows the relationship between the operation mode and the on / off state of each switch, where is the first switch and the switch 22 of the offset correction means 20b is the second switch. Each switch is controlled by a control circuit (not shown).

【００４０】同表に示されるように、待機モードでは、
第１及び第２スイッチは共にオンに設定される。このモ
ードでは、操作者はマスタスレーブ操作を要求していな
いので、マスタスレーブとも動作しないようにすること
が必要である。このモードで第１スイッチはオンである
ため、先述した第１オフセット補正手段は目標位置情報
を変更しない。すなわち、マスタ側の目標位置は常に一
定値を保持し続け、マスタはサーボロック状態となる。
また第２スイッチもオンであるため、スレーブ側も一定
位置を保持するように位置制御される。As shown in the table, in the standby mode,
Both the first and second switches are set to ON. In this mode, the operator has not requested master-slave operation, so it is necessary not to operate with the master-slave. Since the first switch is on in this mode, the above-described first offset correction means does not change the target position information. That is, the target position on the master side always keeps a constant value, and the master is in the servo lock state.
Further, since the second switch is also on, the slave side is also position-controlled so as to maintain a fixed position.

【００４１】バイラテラルモードでは、第１スイッチ及
び第２スイッチは共にオフに設定される。内部モデル
は、マスタの力情報Ｆｍとスレーブの力情報Ｆｓとを受
け、夫々の内部モデルによる位置出力値Ｐｖｍ、Ｐｖｓ
を更新する。オフセット補正手段２０ａ及び２０ｂは、
第１及び第２スイッチがオフであるため、オフセット量
更新回路２３に保持しているオフセット量は更新され
ず、内部モデルの出力Ｐｖの変化量と目標位置情報Ｒの
変化量は全く同一となる。In the bilateral mode, both the first switch and the second switch are set off. The internal model receives the force information Fm of the master and the force information Fs of the slave, and outputs the position output values Pvm and Pvs by the respective internal models.
To update. The offset correction means 20a and 20b are
Since the first and second switches are off, the offset amount held in the offset amount updating circuit 23 is not updated, and the change amount of the output Pv of the internal model and the change amount of the target position information R are exactly the same. .

【００４２】従って、内部モデルの運動と実アームの運
動とが一致するように制御され、バイラテラルモードを
実現できる。また、マスタ側及びスレーブ側の各オフセ
ット量は、動作モードを切替えた時点での、内部モデル
位置と目標位置（位置制御手段により実アームの設定点
位置と一致している）との差に相当した一定値であり、
その結果、切替えた時点でのマスタスレーブ位置からバ
イラテラルモードが実現されることになる。Therefore, the movement of the internal model and the movement of the real arm are controlled so as to coincide with each other, and the bilateral mode can be realized. Further, each offset amount on the master side and the slave side is equivalent to the difference between the internal model position and the target position (which matches the set point position of the actual arm by the position control means) at the time of switching the operation mode. Is a constant value,
As a result, the bilateral mode is realized from the master slave position at the time of switching.

【００４３】マスタシフトモードは、スレーブを現在位
置に保持しつつ、マスタを任意位置に移動することがで
きるモードである。マスタシフトモードでは、第１スイ
ッチはオフ、第２スイッチはオンに設定される。第１ス
イッチがオフであるため、マスタに加えられる操作力
（マスタの力情報）により、内部モデルの出力が変化
し、それに伴いマスタは移動する。しかし、第２スイッ
チがオンであるため、スレーブは目標位置情報が更新さ
れないため動かない。The master shift mode is a mode in which the master can be moved to an arbitrary position while holding the slave at the current position. In the master shift mode, the first switch is set off and the second switch is set on. Since the first switch is off, the output of the internal model changes due to the operation force applied to the master (force force information), and the master moves accordingly. However, since the second switch is on, the slave does not move because the target position information is not updated.

【００４４】速度操作モードは、ジョイスティックのよ
うにスレーブを指令速度で指令方向に運動させるモード
である。ここでは、マスタに加えた力情報の大きさを指
令速度とみなし、加えた力の方向にスレーブを運動させ
ることを考えている。この速度操作モードでは、第１ス
イッチがオンに、第２スイッチがオフに設定される。第
１スイッチはオンであるため、操作力に関わらずマスタ
は移動しない。The speed operation mode is a mode in which the slave is moved in the command direction at the command speed like a joystick. Here, it is considered that the magnitude of the force information applied to the master is regarded as the command speed and the slave is moved in the direction of the applied force. In this speed operation mode, the first switch is set to ON and the second switch is set to OFF. Since the first switch is on, the master does not move regardless of the operating force.

【００４５】しかし、内部モデルには、マスタの力情報
及びスレーブの力情報が入力され、その合力の方向に移
動する。通常、スレーブアームは外部環境に拘束されて
いないので、スレーブ反力は“０”であり、内部モデル
は操作力により、自在に運動することになる。また、ス
レーブのオフセット補正手段２０ｂの第２スイッチはオ
フであるため、内部のオフセット位置は変わらず、スレ
ーブアームのみが操作モードを切り替えた時点のスレー
ブ位置から内部モデルの運動に合わせ動くことになる。
すなわち、操作者が動かした方向に加えた力に比例した
速度で、スレーブアームを自在に動かせる速度操作モー
ドが実現できる。However, the force information of the master and the force information of the slave are input to the internal model, and the internal model moves in the direction of the resultant force. Normally, the slave arm is not constrained by the external environment, so the slave reaction force is “0”, and the internal model is free to move due to the operating force. Since the second switch of the slave offset correction means 20b is off, the internal offset position does not change, and only the slave arm moves from the slave position at the time of switching the operation mode according to the movement of the internal model. .
That is, it is possible to realize a speed operation mode in which the slave arm can be freely moved at a speed proportional to the force applied in the moving direction by the operator.

【００４６】以上のように、操作モードに対応してマス
タ側・スレーブ側の２つのオフセット補正装置２０の動
作を切替えるだけで、制御アルゴリズムを分岐選択しな
くとも、マスタスレーブに要求される多機能を容易に実
現できる。As described above, only by switching the operation of the two offset correction devices 20 on the master side and the slave side in accordance with the operation mode, the multi-function required for the master-slave without branching the control algorithm is selected. Can be easily realized.

【００４７】次に、本発明の第２の実施例について図３
を参照して説明する。同図において図１の構成と対応す
る部分には同一符号を付し、かかる部分の説明は省略す
る。この実施例では、内部モデルへの入力である力情報
を操作モードに応じて補正することができる。Next, a second embodiment of the present invention will be described with reference to FIG.
Will be described with reference to. In the figure, parts corresponding to those in the configuration of FIG. 1 are designated by the same reference numerals, and description of such parts will be omitted. In this embodiment, the force information input to the internal model can be corrected according to the operation mode.

【００４８】図３において、マスタアーム１及び内部モ
デル３間の力情報Ｆｍの経路にマスタ側力情報補正手段
４０ａが設けられている。また、スレーブアーム２及び
内部モデル３間の力情報Ｆｓの経路にスレーブ側力情報
補正手段４０ｂが設けられている。他の構成は、図１の
構成と同様である。In FIG. 3, master side force information correction means 40a is provided in the path of force information Fm between the master arm 1 and the internal model 3. Further, a slave side force information correction means 40b is provided in the path of the force information Fs between the slave arm 2 and the internal model 3. Other configurations are the same as those in FIG.

【００４９】マスタ側力情報補正手段４０ａ及びスレー
ブ側力情報補正手段４０ｂは、力情報補正装置４０とし
て構成される。力情報補正装置４０は、例えば、図５に
示されるように処理切替スイッチ４１、不感帯処理回路
４２及び力情報増幅回路４３によって形成される。処理
切替スイッチ４１のＢ側選択動作によって不感帯処理回
路４２を経由した力情報が力情報増幅回路４３に供給さ
れ、Ａ側選択動作によって不感帯処理回路４２を経由し
ない力情報が力情報増幅回路４３に供給される。不感帯
処理回路４２は、力検出時に混入する外乱の影響を減少
する。力情報増幅回路４３は、操作モードに対応して増
幅率Ｇｆを設定し、この増幅率Ｇｆで力情報を増幅して
内部モデル３に供給する。本実施例における操作モード
と力情報補正手段の動作状態との対応関係を表２に示
す。The master side force information correction means 40a and the slave side force information correction means 40b are configured as a force information correction device 40. The force information correction device 40 is formed of, for example, a process changeover switch 41, a dead zone processing circuit 42, and a force information amplification circuit 43 as shown in FIG. The force information that has passed through the dead zone processing circuit 42 is supplied to the force information amplifying circuit 43 by the B side selection operation of the process changeover switch 41, and the force information that does not pass through the dead zone processing circuit 42 is supplied to the force information amplification circuit 43 by the A side selection operation. Supplied. The dead zone processing circuit 42 reduces the influence of disturbance mixed during force detection. The force information amplification circuit 43 sets the amplification factor Gf in accordance with the operation mode, amplifies the force information with this amplification factor Gf, and supplies it to the internal model 3. Table 2 shows the correspondence relationship between the operation mode and the operation state of the force information correction means in this embodiment.

【００５０】待機モードでは、マスタ側及びスレーブ側
共に、処理切替スイッチ４１はＡ側、力情報増幅回路４
３の利得はＧｆ＝０に設定される。第１の実施例で述べ
たオフセット補正手段の採用でアームはサーボロックさ
れているため、力情報補正手段を用いる必要はない。し
かし、力センサ出力に有色性の外乱が混入すると、内部
モデルはその外乱により運動し続け、長時間放置してい
ると、計算機のオーバーフローを生ずる可能性が考えら
れる。このような事態を回避する目的で、待機モード時
の力情報増幅率Ｇｆを強制的に“０”にして、待機モー
ド時には内部モデルに力を加えないようにしている。In the standby mode, the process changeover switch 41 is on the A side and the force information amplifying circuit 4 is on both the master side and the slave side.
The gain of 3 is set to Gf = 0. Since the arm is servo-locked by adopting the offset correction means described in the first embodiment, it is not necessary to use the force information correction means. However, if a chromatic disturbance is mixed in the force sensor output, the internal model may continue to move due to the disturbance, and if left unattended for a long time, a computer overflow may occur. For the purpose of avoiding such a situation, the force information amplification factor Gf in the standby mode is forcibly set to "0" so that no force is applied to the internal model in the standby mode.

【００５１】バイラテラルモードでは、マスタ側及びス
レーブ側共に、処理切替スイッチ４１はＡ側、力情報増
幅回路４３の利得はＧｆ＝１に設定される。このモード
では、マスタ側及びスレーブ側共に力補正の必要がない
ため、力情報補正装置４０は検出操作力をそのまま位置
検出手段に送る。In the bilateral mode, the process changeover switch 41 is set to the A side and the gain of the force information amplifying circuit 43 is set to Gf = 1 on both the master side and the slave side. In this mode, since there is no need for force correction on both the master side and the slave side, the force information correction device 40 sends the detected operating force as it is to the position detection means.

【００５２】マスタシフトモードでは、マスタ側の処理
切替スイッチ４１はＡ側、力情報増幅回路４３の利得は
Ｇｆ＝１、スレーブ側の処理切替スイッチ４１はＡ側、
力情報増幅回路４３の利得はＧｆ＝０に設定される。こ
のモードでは、マスタ側力情報補正手段は検出操作力を
そのまま位置検出手段に送る。スレーブ側力情報補正手
段は、バイラテラルモードと同じで良いが、マスタを自
在に動かせるようにするために、スレーブ側反力を除去
するのが適当である。そこで、力情報増幅手段の増幅率
を０にしている。In the master shift mode, the processing switch 41 on the master side is on the A side, the gain of the force information amplification circuit 43 is Gf = 1, and the processing switch 41 on the slave side is on the A side.
The gain of the force information amplifier circuit 43 is set to Gf = 0. In this mode, the master side force information correction means sends the detected operating force as it is to the position detection means. The slave side force information correcting means may be the same as in the bilateral mode, but it is appropriate to remove the slave side reaction force in order to allow the master to move freely. Therefore, the amplification factor of the force information amplification means is set to zero.

【００５３】速度操作モードでは、マスタ側の処理切替
スイッチ４１はＢ側、力情報増幅回路４３の利得は、図
７に示される内部モデルを使用した場合、Ｇｆ＝ｋｖＣ
ｖ／ｋｐ、スレーブ側の処理切替スイッチ４１はＡ側、
力情報増幅回路４３の利得はＧｆ＝０に設定される。こ
の場合の動作について説明する。このモードにおける力
情報補正手段の主目的は、良好な速度操作モードの実現
にある。上述したようにオフセット補正手段を内部モデ
ルを使うバイラテラル制御方式に付加することにより、
速度操作モードを実現できる。しかしながら、内部モデ
ルを使うバイラテラル制御方式は、もともと良好な力制
御を実現することを指向しており、速度操作を行うこと
を前提にしたものではない。内部モデルのパラメータ調
整はバイラテラルモードにおける良好な力制御を実現す
る見地から行われる。このため、良好な速度操作を実現
する重要なファクタの一つである操作力と指令速度の関
係は上記パラメータ調整で決まり、速度操作モードにお
いて、操作力と指令速度の関係を任意に設定する自由度
がない。第２の実施例で付加された力情報補正手段は、
以下に述べるようにかかる点の改善を図ることをも可能
とする。In the speed operation mode, the processing switch 41 on the master side is on the B side, and the gain of the force information amplifying circuit 43 is Gf = kvC when the internal model shown in FIG. 7 is used.
v / kp, the process switch 41 on the slave side is the A side,
The gain of the force information amplifier circuit 43 is set to Gf = 0. The operation in this case will be described. The main purpose of the force information correction means in this mode is to realize a good speed operation mode. By adding the offset correction means to the bilateral control method using the internal model as described above,
A speed operation mode can be realized. However, the bilateral control method using the internal model is originally intended to realize good force control, and is not premised on performing speed operation. Parameter adjustment of the internal model is done from the standpoint of achieving good force control in the bilateral mode. Therefore, the relationship between the operating force and the command speed, which is one of the important factors for achieving good speed operation, is determined by the above parameter adjustment, and in the speed operation mode, the relationship between the operating force and the command speed can be freely set. There is no degree. The force information correction means added in the second embodiment is
It is possible to improve such points as described below.

【００５４】例えば、図７の内部モデルが用いられ、ス
レーブアームが物体に接触していない状態（スレーブ反
力Ｆｓが０である）で、操作力Ｆｍと指令速度Ｖの関係
を考えると、内部モデルのスレーブ側速度出力は、 Ｖ＝ｋｐ・（１／Ｃｖ）・Ｆｍ により、スレーブアームの指令速度Ｖが決定される。そ
こで、力情報補正手段では、マスタ側力情報増幅回路４
３の増幅率をＣｖ・ｋｖ／ｋｐに設定し、 Ｖ＝ｋｐ・（１／Ｃｖ）・（Ｃｖ・ｋｖ／ｋｐ）・Ｆｍ ＝ｋｖ・Ｆｍ の関係が得られるようにする。係数Ｋｖをマスタ側の力
情報増幅回路４３の利得で調整することによって、操作
力Ｆｍと指令速度Ｖの関係を設定することが可能とな
る。For example, when the internal model of FIG. 7 is used and the slave arm is not in contact with the object (slave reaction force Fs is 0), the relationship between the operating force Fm and the commanded speed V is considered. For the speed output on the slave side of the model, the command speed V of the slave arm is determined by V = kp · (1 / Cv) · Fm. Therefore, in the force information correction means, the master side force information amplifier circuit 4
The amplification factor of 3 is set to Cv · kv / kp so that the relationship of V = kp · (1 / Cv) · (Cv · kv / kp) · Fm = kv · Fm can be obtained. By adjusting the coefficient Kv with the gain of the force information amplification circuit 43 on the master side, it becomes possible to set the relationship between the operating force Fm and the command speed V.

【００５５】また、力検出時の外乱影響でスレーブアー
ム２が動作するのは好ましくないため、処理切り替えス
イッチ４３を不感帯処理手段４２のＢ側に切替え、力情
報増幅手段４３の全般で不感帯処理している。このモー
ドでは、スレーブ側力情報補正手段４０ｂは意味を持た
ないため、この実施例では増幅率を“０”としている。Further, since it is not preferable that the slave arm 2 operates due to the influence of disturbance at the time of force detection, the processing changeover switch 43 is changed over to the B side of the dead zone processing means 42, and the dead zone processing is carried out by the force information amplifying means 43 as a whole. ing. In this mode, since the slave side force information correction means 40b has no meaning, the amplification factor is set to "0" in this embodiment.

【００５６】しかしながら、通常の速度操作モードで
は、作業対象に過大な力を加える危険があるため、スレ
ーブがコンプライアンスを持つ速度操作モードが望まれ
る場合がある。表２には示していないが、この速度操作
モードにおいて、スレーブ側の力情報補正手段４０ｂの
処理切替スイッチ４３を不感帯処理のＢ側とし、力情報
増幅手段４３の増幅率を図７のモデルに対応してＣｖ・
ｋｖ／ｋｐに設定する。こうすると、スレーブアームが
作業対象に接触した際に反力が発生し、操作力が作業対
象への作用力の力反射比ｋｆ倍した値と一致するため、
速度操作モードのままで、かなりのマスタスレーブ作業
を複合的に行える。つまり、４０ｂの処理を若干変更す
るだけで、コンプライアンスを持つ速度操作モードを容
易に実現できる。However, in the normal speed operation mode, there is a risk that an excessive force is applied to the work target, and therefore there are cases where the speed operation mode in which the slave has compliance is desired. Although not shown in Table 2, in this speed operation mode, the process changeover switch 43 of the force information correction means 40b on the slave side is set to the dead zone processing side B, and the amplification factor of the force information amplification means 43 is set to the model of FIG. Correspondingly Cv
Set to kv / kp. In this case, a reaction force is generated when the slave arm contacts the work target, and the operating force matches the value obtained by multiplying the force reflection ratio kf of the acting force on the work target.
You can do a lot of master-slave work compoundly in the speed operation mode. That is, the speed operation mode having compliance can be easily realized by slightly changing the process of 40b.

【００５７】[0057]

【表１】 [Table 1]

【００５８】[0058]

【表２】 [Table 2]

【００５９】[0059]

【発明の効果】以上説明したように本発明のマスタスレ
ーブ制御装置は、オフセット補正手段を備えて、バイラ
テラルモードのアルゴリズムによる出力を要求される操
作モードに対応して適宜に補正して出力を得るようにし
ているので、従来のように、マスタスレーブの複数の操
作モード毎に制御処理アルゴリズムを設ける必要がな
く、全体の処理手段（制御プログラム）をコンパクトに
することが可能である。また、制御処理の流れが各操作
モードにおいて共通にオフセット補正手段を経由するた
め、従来のような各操作モード相互間の切替の際に必要
とされた処理アルゴリズム間の同期プロセスが不要で、
滑らかな操作モード切替を実現可能である。更に、力情
報補正手段を含む速度操作モードでは、スレーブが作業
対象に過負荷を与えないコンプライアンスをもつ動作を
容易に実現することが可能になる。As described above, the master-slave control device of the present invention is provided with the offset correction means, and the output by the bilateral mode algorithm is appropriately corrected in accordance with the required operation mode to output. Since it is obtained, it is not necessary to provide a control processing algorithm for each of the plurality of operation modes of the master slave as in the conventional case, and the entire processing means (control program) can be made compact. In addition, since the flow of control processing commonly goes through the offset correction means in each operation mode, a synchronization process between processing algorithms required when switching between operation modes as in the past is not required,
Smooth operation mode switching can be realized. Further, in the speed operation mode including the force information correction means, it is possible to easily realize a compliant operation in which the slave does not overload the work target.

【図面の簡単な説明】[Brief description of drawings]

【図１】本発明の第１の実施例を示すブロック図。FIG. 1 is a block diagram showing a first embodiment of the present invention.

【図２】図１に示されるオフセット補正手段の構成例を
示すブロック図。FIG. 2 is a block diagram showing a configuration example of an offset correction means shown in FIG.

【図３】本発明の第２の実施例を示すブロック図。FIG. 3 is a block diagram showing a second embodiment of the present invention.

【図４】本発明の概念のモデルを示す説明図。FIG. 4 is an explanatory view showing a model of the concept of the present invention.

【図５】図３に示される力補正手段の構成例を示すブロ
ック図。5 is a block diagram showing a configuration example of the force correction means shown in FIG.

【図６】内部モデル追従型制御の構成例を示すブロック
図。FIG. 6 is a block diagram showing a configuration example of internal model following control.

【図７】内部モデルの第１の構成例を示すブロック図。FIG. 7 is a block diagram showing a first configuration example of an internal model.

【図８】内部モデルの第２の構成例を示すブロック図。FIG. 8 is a block diagram showing a second configuration example of an internal model.

【図９】内部モデルの第３の構成例を示すブロック図。FIG. 9 is a block diagram showing a third configuration example of the internal model.

【符号の説明】[Explanation of symbols]

１ マスタアーム ２ スレーブアーム ３ 内部モデル ２１ 差分器 ２２ スイッチ ２３ オフセット量更新回路 ４１ 増幅器 ４２ 不感帯処理回路 ４３ 処理切替スイッチ ５１ 力反射比用増幅器 ５２ 変位拡大比用増幅器 １０ａ マスタ側追従制御手段 １０ｂ スレーブ側追従制御手段 ２０ａ マスタ側オフセット補正手段 ２０ｂ スレーブ側オフセット補正手段 ４０ａ マスタ側力補正手段 ４０ｂ スレーブ側力補正手段 1 Master Arm 2 Slave Arm 3 Internal Model 21 Differencer 22 Switch 23 Offset Amount Update Circuit 41 Amplifier 42 Dead Zone Processing Circuit 43 Processing Changeover Switch 51 Force Reflection Ratio Amplifier 52 Displacement Enlargement Ratio Amplifier 10a Master Side Tracking Control Means 10b Slave Side Tracking control means 20a Master side offset correction means 20b Slave side offset correction means 40a Master side force correction means 40b Slave side force correction means

Claims (1)

【特許請求の範囲】[Claims] 【請求項１】マスタアーム及びスレーブアーム各々にお
いて検出された力検出力を入力として前記マスタアーム
及び前記スレーブアーム各々の目標運動値を生成する内
部モデルと、 与えられる目標運動値を追従するように前記マスタアー
ム及び前記スレーブアームを夫々動作させる第１及び第
２の位置追従手段と、 前記マスタアームの目標運動値をアームの操作モードに
対応してオフセット補正して前記第１の位置追従手段に
与える第１オフセット補正手段と、 前記スレーブアームの目標運動値をアームの操作モード
に対応してオフセット補正して前記第２の位置追従手段
に与える第２オフセット補正手段と、 を備えたことを特徴とするマスタスレーブマニピュレー
タの制御装置。1. An internal model for generating a target motion value of each of the master arm and the slave arm by inputting a force detection force detected in each of the master arm and the slave arm, and to follow a given target motion value. First and second position tracking means for operating the master arm and the slave arm respectively, and offset correction of the target motion value of the master arm corresponding to the operation mode of the arm to the first position tracking means. First offset correction means for giving the target motion value of the slave arm, and second offset correction means for giving the second target position tracking means with offset correction corresponding to the operation mode of the arm. Control device for master-slave manipulator.