JPS58172717A - Control system of master and slave manipulators - Google Patents

Abstract

PURPOSE:To improve operativity by giving an operator only force which operates on the body of a slave arm in the form a sence of force. CONSTITUTION:A speed detector 100 for a joint angle converts a speed from the joint angle thetaS of the slave arm 2 into an electric signal, which is converted into a position deviation signal by a signal converter 101; and signal conversion 102 to a torque signal TthetaS is performed. The signal TthetaS is compared by a comparator 103 with the output of a slave-side torque detector 12 to send a signal TS1 to a comparator 13. The comparator 13 inputs the signal TS1 and an output signal TM from a master-side torque detector 11 differentially to output an output signal DELTAT0 as TS1-TM, giving the operator only the torque of the arm 2 which operates on the body in the form of a sense of force.

Description

【発明の詳細な説明】 本発明はカフィードバック制御により駆動するマスター
スレーブマニプレータの制御方式に関する。DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a control system for a master-slave manipulator driven by feedback control.

人間に代ってパルプの開閉中ボルトの増締などの軽作業
を、例えば原子力発電の原子炉格納容器内でロボッ）Ｋ
行なわせる場合、スレーブアームを持つロボットを格納
容器内に置き、容器外で人間（オペレータ）がマスター
アームを操縦することによってスレーブアームを駆動す
せる方法が採られている。Robots can perform light work such as retightening bolts during opening and closing of pulp in place of humans, for example inside the reactor containment vessel of a nuclear power plant)
When performing this, a method is adopted in which a robot with a slave arm is placed inside a containment vessel, and a human (operator) outside the vessel operates the master arm to drive the slave arm.

このようなマスタースレーブ方式の従来のマニプレータ
の制御方式に係るブロック構成図を＄１１１図に示す。A block diagram of a conventional manipulator control system using such a master-slave system is shown in Figure $111.

この例は、カミｔ形と呼ばれるカフィードバック制御方
式（パイラテラルす一ボとも言う）である。第１図にお
いて、１＃ｉマスターアーム、２はスレーブアームであ
シ、ソれぞれ手賃、肘、Ｗの回転成いはねじりの関節を
有す會多関節のアームであって、両アーム１゜２Ｆｉ電
気的に結合されて以下の如く駆動する。An example of this is a feedback control system called a cam type (also called a pirate control system). In Fig. 1, 1#i is a master arm, and 2 is a slave arm, which is a multi-jointed arm with joints for rotation, elbow, and W, respectively. Arms 1°2Fi are electrically coupled and driven as follows.

第１図において、オペレータ（作業者）Ｋよって操縦さ
れるマスターアーム１の関節角度ＩＭが位置検出器３に
よって電気信号に変換され、他方、スレーブアーム２の
関節角度ＩＢも位冒検小器４によって電気信号Ｋｍ換さ
れる０両検出器３．４からの電気信号は比験器９の差動
入力とな）、その出力で参る偏差信号ΔＥ−＃Ｍ−＃ｇ
が増幅器８を介してスレーブアル五個の駆動装置６に４
晃られる。仁の駆動装置＠＃ｉｌＢを〜に一致させるよ
うにスレーブアーム２を駆動する。In FIG. 1, a joint angle IM of a master arm 1 operated by an operator (worker) K is converted into an electric signal by a position detector 3, and a joint angle IB of a slave arm 2 is also converted into an electric signal by a position detector 4. The electrical signal from the zero detector 3.4, which is converted into an electrical signal Km, is the differential input of the comparator 9), and its output is the deviation signal ΔE-#M-#g.
is sent to the five slave drives 6 via the amplifier 8.
I am blessed. Drive the slave arm 2 so that the driver @#ilB matches ~.

を九、マスター側の駆動装置ＳＫより発生するトルクＴ
Ｍがトルク検出器１１（例えば、トルクモータの電流値
を検出）Ｋよって電気信号に変換され、他方スレーブ側
の駆動装置６により発生するトルクＴＢがトルク検出器
１２によって電気信号に変換される。仁の両トルク検出
器１１．１２によって変換され良電気信号は比較ｆｙ１
３の差動入力とをや、その出力である偏差信号ＪＴ　＝
Ｔｓ　−ＴＭ　　が増幅器７を介して駆動装置５に与え
られる。この駆動装置ＳによってＴＭ七ＴｓＫ一致させ
るようにマ、、スターアームｉｔ駆動させ為。つまり、
マス１ターアームＩｔ操縦するオペレータに′ｒＭをＴ
Ｓ　Ｋ−散させる崗龜の反力を与えることｋなる。9. Torque T generated by the master side drive device SK
M is converted into an electrical signal by a torque detector 11 (for example, detects the current value of a torque motor) K, and torque TB generated by the slave drive device 6 is converted into an electrical signal by a torque detector 12. The good electric signal converted by both torque detectors 11 and 12 of Jin is compared fy1
3 differential input and its output deviation signal JT =
Ts −TM is applied to the drive device 5 via the amplifier 7 . This drive device S drives the star arm it so that TM7TsK matches. In other words,
T to the operator controlling the master arm It.
S K - Gives the reaction force of the dispersing grommet.

このようにカｆＩＩｌｌ形のカフィードバック制御方式
ＫｊＩＰいては、位置検出のサーボ系とトルク検出のサ
ーボ系とを設叶るヒとＫよって、スレーブアーム２が作
用しているカ或いは保持しているトルクをオペレータに
力感覚として容易に、　　感じさせる仁とができ、より
確実な操縦を行なうことができる。In this way, in the motor feedback control system KjIP, the position detection servo system and the torque detection servo system are installed. This allows the operator to easily feel the torque as a force sensation, allowing for more reliable operation.

しかしながら、スレーブ側のトルク検出器１２によって
検出されるトルク’ｒｓＦｉスレーブアーム２が対象物
体に作用している成分”Ｓｓ　と、スレーブアーム２を
動作させて込る成分子Ｓｓ（九りｌ、、、スレーブアー
ム２の自重はカラｙ）ウェイト等でバランスさ−ている
とする）とからなるため、オペレータｔ・操縦する際に
力感覚として必要なトルクがＴＳ１のみであるに４かか
わらず、スレーブアーム２を動作させてｉる不必要な成
分のトルクＴｓ、が含まれてしまう、そのため、速い操
縦をする際には、トルクＴＳｍが大きくなってしまう良
め、力感覚が不正Ｊｉｆをものとをタオペレータの操縦
性を・損なわれてしまう。However, the torque 'rsFi detected by the torque detector 12 on the slave side is the component "Ss" acting on the target object by the slave arm 2, and the component "Ss" acting on the slave arm 2 , the slave arm 2's own weight consists of (assuming that it is balanced with weights, etc.), so even though the torque required for force sensation when operating the operator t is only TS1, the slave arm 2 An unnecessary component of torque Ts is included when operating arm 2. Therefore, when performing a fast maneuver, the torque TSm becomes large and the force sensation becomes incorrect. This will impair the operator's maneuverability.

本発明は上記従来技術の欠点に鑑み、スレーブアームの
物体に作用するカだけをオペレータに力感覚として与え
ることによ〉、操縦性をいつ−ｔ５向上させたマスター
スレーブマニプレータの制御方式を提供することを目的
とする。In view of the above drawbacks of the prior art, the present invention provides a control system for a master-slave manipulator that improves maneuverability by giving the operator only the force acting on the object of the slave arm as a force sensation. The purpose is to

かかる目的を達成する本発明としては、力帰還形のカフ
ィードバック制御機能を有するマスタースレーブマニプ
レータの制御方式において、スレーブマニプレータの速
度を検出して電気的な速度信号に変換し、との適度信号
を位置偏差信号に変換しさらＫこの位置偏差信号をトル
ク信号に変換し、仁のトルク信号を用すて一し−プ側駆
動装曾の発生トルクを検出し九トルク信号を修正し、こ
の修正後のトルク信号でマスターマニプレータ管動作さ
せ力感覚をオペレータに与えることを特徴とする。The present invention achieves this object by detecting the speed of a slave manipulator and converting it into an electrical speed signal in a control method for a master-slave manipulator having a force feedback control function. Convert this position deviation signal to a position deviation signal, convert this position deviation signal to a torque signal, use the second torque signal to detect the torque generated by the side drive unit, correct the torque signal, and make this correction. It is characterized in that the master manipulator tube is operated by the subsequent torque signal, giving the operator a sense of force.

以下に本発明の一笑麹例を第２図に基づき詳細Ｋｍ明す
る。尚、プＵツク構成を示すｊ１２１］１中、第１１％
Ｑと同−機能管有する部分ＫＦｉ同一符号を附して重複
する説ｌｌ１４Ｆ！省略する。An example of Iksho koji according to the present invention will be explained in detail below based on FIG. 2. In addition, the 11th percentile of j121]1, which shows the push-pull configuration.
The theory that the part KFi having the same function as Q is given the same code and overlaps ll14F! Omitted.

第２図において、１００ｔｌ関節角の速度検出器で６転
スレーブアーム２の関節角ＩＢから関節角の速度１８　
　を電気信号に変換して出力する。In Fig. 2, the joint angle speed 18 is determined from the joint angle IB of the 6-turn slave arm 2 using a 100 tl joint angle speed detector.
is converted into an electrical signal and output.

１０１　Ｆｉ信号変換器であり、速度検出器１０Ｇから
の出力信号−８を、無負荷状態でのスレーブアーム２が
入力され喪速度を得るに必要な等簡約な位置偏差信号Ｅ
ａｓＫ変換して出方する。１０２　Ｆｉ信号彎換器であ
り、前記信号変換器１０１からの出力信号を入力し、こ
の入力されえ位置偏差信号によりスレーブ側の駆動装＃
１６が発生する大きさのトルクと等価なトルク信号Ｔ＃
ｓを出力するものである。１０３ｔｊ比験器てあり、信
号変換器１０２からの出力信号Ｔ＃ｓと、スレーブ側の
トルク検出器１２からの出力信号Ｔｓ　（＝丁８＊”Ｓ
ｔ）とを差動入力とし、出力信号Ｔ・を出力する。101 is a Fi signal converter, which converts the output signal -8 from the speed detector 10G into a simple position error signal E necessary for the slave arm 2 in the no-load state to obtain the dead speed.
Convert asK and output. 102 is a Fi signal converter, which inputs the output signal from the signal converter 101, and converts the drive device # on the slave side according to the input position error signal.
The torque signal T# is equivalent to the torque that generates 16.
It outputs s. The output signal T#s from the signal converter 102 and the output signal Ts from the torque detector 12 on the slave side (=8*"S
t) as differential inputs and outputs an output signal T.

つｔり、速度検出器ｌｏｏにおりてスレーブアーム２の
動作速度−８を検出し、信号変換器１０１において動炸
速ＩＩ　Ｊ８からこの速度を得るに必要な位置偏差信号
Ｅ＃ｓに変換し、さらに信号変換器１０２において位置
偏差信号ＥＩＢを、スレーブ儒の駆動装置６が発生する
トルクに等価なトルク信号”＃Ｓ　（”　ＴＳｍ　）と
して変換して出力する。In other words, the speed detector loo detects the operating speed -8 of the slave arm 2, and the signal converter 101 converts the operating speed II J8 into a position error signal E#s necessary to obtain this speed. Further, the signal converter 102 converts the position error signal EIB into a torque signal "#S ("TSm) equivalent to the torque generated by the slave drive device 6 and outputs the signal.

したがって、信号変換器１０２かもはスレーブアーム２
を動作させる−に必要なトルクの電気信号ＴＳｓが出力
されることになる。さらに比較器１０８においてはトル
ク検出器！２からの出力Ｔｇ、＋ＴＳｍからスレーブア
ーム２の駆動に必要なトルク信号”Ｓｓが減算され、比
較器１０１からはスレーブアーム２が物体に作用してい
るトルクに修正されたトルク信号”８１だけが出力され
ることになる。その結果、比較器１３においては前記比
較ａ１０３の出力信号Ｔｓｌとマスター偶のトルク検出
器１１からの出力信号ＴＭとが差動入力されるため、そ
の出力信号ノＴ・が”Ｓｌ−”Ｍ　として出力されるこ
とＫなり、物体に作用しているスレーブアーム２のトル
クＴ５．７Ｌ’ケｔオペレータに力感覚として与える仁
とができる。Therefore, the signal converter 102 may be connected to the slave arm 2.
An electric signal TSs of the torque necessary for operating the motor is output. Furthermore, in the comparator 108, there is a torque detector! The torque signal "Ss" necessary for driving the slave arm 2 is subtracted from the output Tg, +TSm from the comparator 101, and only the torque signal "81" corrected to the torque acting on the object by the slave arm 2 is output from the comparator 101. It will be output. As a result, in the comparator 13, the output signal Tsl of the comparator a103 and the output signal TM from the master torque detector 11 are differentially inputted, so that the output signal T is expressed as "Sl-"M. As a result, the torque of the slave arm 2 acting on the object is T5.7L' which is given to the operator as a force sensation.

以下、具体的な動作例について説明する。A specific example of operation will be described below.

〔動作例　１〕 オペレータが１スターアーム１を操縦し、スレーブアー
ム２をｆｉｆｆ静止状静止状１１対Ｋ物体にトルク”８
ｓで作用している状態での動作について述べる。[Operation example 1] The operator operates 1 star arm 1 and applies a torque of 8 to slave arm 2 to the fiff stationary stationary 11 to K object.
The operation in the state where s is acting will be described.

この場合には、オペレータの操縦によ砂、マスターアー
ム１とスレーブアーム２の位置の偏差信号ノーが比較器
９から出方され、この信号ノＥｘ　Ｋ基づいて増幅器８
を介し駆動装置６を動作させ、スレーブアーム２が対象
物体にトルクＴｓ、で作用する。その間、マスターアー
ムＩにおいて駆動装置Ｓは、この駆動装置５０発生する
トルクＴＭとスレーブ側の駆動装置６の発生するトルク
ＴＳｌ（伝動効率を１とする）とが、方向が反対で絶対
値がＴｓ。In this case, a deviation signal between the positions of the master arm 1 and the slave arm 2 is outputted from the comparator 9 by the operation of the operator, and based on this signal Ex K, the amplifier 8
The drive device 6 is operated via the slave arm 2 to act on the target object with a torque Ts. Meanwhile, in the master arm I, the drive device S is configured such that the torque TM generated by the drive device 50 and the torque TSL (assuming the transmission efficiency is 1) generated by the drive device 6 on the slave side are in opposite directions and have an absolute value of Ts. .

Ｋ等しくなるように動作して、オペレータにはトルクＴ
ｓ１と等しいトルクの力感覚が与えられる。The operator is given a torque T.
A force sensation of torque equal to s1 is provided.

他方、スレーブアーム２はほぼ静止状態であるので、ス
レ」プ乎−人２の動ｆｎｆＪｇｔｊＰ１は零となる九め
、信号変換器１０２から出力されるトルク信号ＴＩｓは
零となる。これＫより従来と同様な動作をして正確な力
感覚を得ながら確爽な操作ができる。On the other hand, since the slave arm 2 is substantially stationary, the movement fnfJgtjP1 of the slave arm 2 becomes zero, and the torque signal TIs output from the signal converter 102 becomes zero. With this K, you can operate in the same way as before and get a more accurate sense of force while still being able to operate with confidence.

〔動作１１２〕 オペレータが！スタースレーブ１を操縦し、スレーブア
ーム２を成る動作速度）Ｓで動作させながら、対象物体
にトルク丁ｓｌて作用させた状態での動作について述べ
る。[Operation 112] Operator! The operation will be described in which the star slave 1 is operated and the slave arm 2 is operated at the operating speed )S while a torque of 1 sl is applied to the target object.

この場合には、オペレータの操縦により、マスターアー
ム１とスレーブアーム冨の位置の偏差信号ノＥ朧（なお
、）Ｅｍ　＞ノＥ１である。）が比較器９から出力され
、この信号ＩＢ、に基づいて増幅器８を介してスレーブ
側の駆動装置６を動作させる。この際には、駆動装置６
においては、スレーブアーム２を成る動作適度−８で動
作させるに必要なトルクＴＳｍとスレーブアーム２を対
象物体に作用させるに必　。In this case, due to the operation of the operator, the deviation signal between the positions of the master arm 1 and the slave arm 1 is such that Em is larger than E1. ) is output from the comparator 9, and the slave side drive device 6 is operated via the amplifier 8 based on this signal IB. At this time, the drive device 6
In this case, the torque TSm required to operate the slave arm 2 at an operating mode of -8 and the torque TSm required to cause the slave arm 2 to act on the target object.

要なトルクＴＳｓを発生している。The necessary torque TSs is generated.

他方、スレーブアーム２の動作速度）Ｓは速度検出器ｌ
ＯＯで検出され、信号変換９１０１にて位置偏差信号Ｅ
ＩＢ　（なお、”＃Ｓ　＝−ΔＢ５−４ｇ１）Ｋ変換さ
れ、さらに信号変換器１０２　Ｋてトルク信号”＃８　
（”　ＴＳｓ　）　Ｋ変換される。し九がって、比較器
１０３　においては、スレーブ側のトルク検出器１２か
らのトルク信号ＴＢ　−ＴＢ　、＋”ｆｈと、前記信号
変換器１０２がらのトルク信号”＃８　（耽”８１　）
とが減算されて、その出力としてトルク信号ＴＩ　（＆
お”ｅ　社ＴＢ　＊　＋　ＴＢ　ｍ−ＴＢ　ｍ　；”８
１）＆る出力信号が得られ、さらに比較器１３に入力さ
れる。こうしてマスターアームＩＫはトルク信号Ｔ、−
，Ｔｓ、が指令信号となるため、スレーブアーム２が対
象物体に作用しているトルク信号”８ｓと等しい大きさ
のトルクだけが、駆動装置５によって！スターアームＩ
Ｋ作用するととＫなり、スレーブアーム２が物体に作用
しているトルクに等しい反力が力感覚としてオペレータ
に４見られる。したがって、上述し九動炸例１と同様に
オペレータは正確な力感覚を得なから＊＊なる操縦をす
ることができる。On the other hand, the operating speed of slave arm 2) S is the speed detector l
OO is detected, and the position deviation signal E is detected in the signal conversion 9101.
IB (In addition, "#S = -ΔB5-4g1) K is converted, and then the signal converter 102 K is converted into a torque signal"#8
("TSs) K is converted. Therefore, in the comparator 103, the torque signals TB-TB, +"fh from the torque detector 12 on the slave side and the torque signal from the signal converter 102 are combined. "#8 (Indulgence" 81)
is subtracted, and the output is the torque signal TI (&
O”e company TB * + TB m-TB m ;”8
1) An output signal of & is obtained and further input to the comparator 13. In this way, the master arm IK outputs a torque signal T, -
, Ts, are the command signals, so that only the torque equal to the torque signal "8s" acting on the target object by the slave arm 2 is transmitted by the drive device 5 to the !star arm I.
When K acts on the object, the reaction force equal to the torque exerted by the slave arm 2 on the object is seen by the operator as a force sensation. Therefore, as in the above-mentioned Nine Motion Explosive Example 1, the operator can perform ** maneuvers because he/she does not get an accurate sense of force.

以上、具体例を挙げて説明したように、特に上述した動
作例２の場合において、従来方式では、スレーブ側のト
ルク検出器にて検出されるトルク信号がそのｉまマスタ
ー側の駆動装置働 への指令となる丸め、スレーブアームを成る動作速度で
動作させるに必要なトルクと物体に作用するに必要なト
ルクの和がオペレータに力感覚として与えられるととに
なって、オペレータの操縦性は損なわれてしまうが、こ
れに対し本発明方式によれば、スレーブ側のトルク検出
器にて検出されるトルク信号を、スレーブアームが対象
物体に作用しているトルク成分のトルク信号に修正し、
仁のトルク信号だけでマスター側の駆動装置へ指令され
る九め、オペレータはスレーブアームが物体に作用する
トルクだけ【力感覚として得ることになり、正確な力感
覚にて確実なる操縦ｔすることができる。As explained above with specific examples, especially in the case of the above-mentioned operation example 2, in the conventional method, the torque signal detected by the torque detector on the slave side is transferred to the drive device on the master side. When the sum of the torque required to operate the slave arm at the operating speed and the torque required to act on the object is given to the operator as a force sensation, the operator's maneuverability is impaired. However, according to the method of the present invention, the torque signal detected by the torque detector on the slave side is corrected to a torque signal of the torque component of the slave arm acting on the target object.
The master side drive device is commanded by the torque signal alone, and the operator receives only the torque that the slave arm acts on the object as a force sensation, allowing reliable control with an accurate force sensation. Can be done.

ここで付言するに、本発明を適用する機器の若干の例′
ｔ−あげておくと、放射線、爆発物、有害物質など危険
物を取扱う作業用マニプレータ、放射線や有害ガスなど
の悪秦件の雰囲気で作業する作業用マニプレータ、水中
や海底などで作業する作業用マニプレータ、宇宙などで
作業する作業用マニプレータ等がある。Here, I would like to add some examples of devices to which the present invention is applied.
To name a few, there are manipulators for work that handles dangerous materials such as radiation, explosives, and harmful substances, manipulators for work that involves working in a hostile atmosphere such as radiation and toxic gases, and manipulators for work that involves working underwater or on the seabed. There are manipulators, manipulators for work in space, etc.

【図面の簡単な説明】[Brief explanation of the drawing]

第１図は従来技術に係るブロック構成図、總２１１は本
発明の一実施例を示すブロック構成図である。 図　画　中、 １はマスターアーム、 ２Ｆｉスレーブアーム、 ３．４Ｆｉ位置検出器、 ５．６は駆動装置、 ９、　１３．　１０３は比較器、 １１．１！＃ｉ）ルク検出器、 １００は速度検出器、 １０１、　１０２は信号変換器、 −Ｍはマスターアームの関節角、 ＩＢ　、　ＺＢ−はスレーブアームの関節角およびその
適度、 ΔＥ、ΔＥ□、ΔＥ黛　は偏差信号、 Ｅ−８は位置偏差信号、 ＴＭはマスター７−ム儒トルクの電気信号、Ｔｓはスレ
ーブアーム側トルクの電気信号、ＴＳｔはＴｓ中、スレ
ーブアームが物体に作用するトルクの電気信号、 Ｔｓ、はＴｓ中、スレーブアームを動作させるトルクの
電気信号、 ＴＪＳ　ＦｉＥＩｉ８を等簡約なトルクに変換しぇ電気
信号、　　　□・ Ｔ＠は比較器１０３から出力される電気信号、ノＴ、ノ
Ｔ・は比較ａ１３から出力される電気信号である。 特許出願人 三菱重工業株式命社 書代理人 弁壜士光石士部 （−１名） 第１図FIG. 1 is a block diagram of a conventional technique, and reference numeral 211 is a block diagram of an embodiment of the present invention. In the figure, 1 is a master arm, 2Fi slave arm, 3.4Fi position detector, 5.6 is a drive device, 9, 13. 103 is a comparator, 11.1! #i) Lux detector, 100 is a speed detector, 101, 102 are signal converters, -M is the joint angle of the master arm, IB, ZB- is the joint angle of the slave arm and its moderation, ΔE, ΔE□, ΔE雛 is the deviation signal, E-8 is the position deviation signal, TM is the electric signal of the master torque, Ts is the electric signal of the slave arm side torque, TSt is the electric torque of the slave arm acting on the object during Ts. The signal, Ts, is the electric signal of the torque that operates the slave arm during Ts, the electric signal that converts the TJS FiEI8 into an equally simple torque, □・T@ is the electric signal output from the comparator 103, ノT, T. is an electrical signal output from the comparison a13. Patent Applicant: Mitsubishi Heavy Industries Co., Ltd. Representative, Benz Attorney Shibe Mitsuishi (-1 person) Figure 1

Claims (1)

【特許請求の範囲】[Claims] 力帰還形のカフィードバック制御機能を有するマスター
スレーブマニプレータの制御方式において、スレーブマ
ニプレータの速度を検出して電気的な速度信号に変換し
、仁の速度信号を位置偏差信号に変換しさらにこの位置
偏差信号をトルク信号に変換し、このトルク信号を用い
てスレーブ側駆動装置の発生トルクを検出し九トルク信
号を修正し、この修正俵のトルク信号でｉスターマニプ
レークを動作させ力感覚をオペレータに与えることｔ−
特徴とするマスタースレーブマニプレータの制御方式。In a control method for a master-slave manipulator that has a force feedback type feedback control function, the speed of the slave manipulator is detected and converted into an electrical speed signal, and the speed signal of the slave manipulator is converted into a position deviation signal. The signal is converted into a torque signal, this torque signal is used to detect the torque generated by the slave side drive device, the nine torque signals are corrected, and the i-star manipulator is operated using this corrected bale torque signal to provide a force sensation to the operator. giving t-
Features a master-slave manipulator control system.