JPS603010A - Control system of robot - Google Patents

Abstract

PURPOSE:To realize the flexible control system of a robot which performs various operations without tool replacement by providing a means which increases the flexibility of the robot in terms of software. CONSTITUTION:The means which increases the flexibility of the robot in terms of software is provided. For example, a force sensor 13 is fitted atop of the wrist 12d of the robot consisting of a swivel table 12a, upper arm 12b, front arm 12c, wrist 12d, etc. to detect the force applied to the robot 12, and the current position of the robot 12 is detected through an encoder, etc., coupled with motors, etc., for driving respective arm members 12a-12d of the robot 12. The detected position theta, force Fv, etc., of the robot 12 are inputted to a controller consisting of a microprocessor, arithmetic part, etc., and command values of a position xr, force Fc, etc., are inputted from a console panel, etc., to the controller. Then, the controller outputs the command value of the articulation angular velocity theta of the robot 12 to a servoamplifier to operate the robot 12.

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、ロボットを実際の作業に応用する際に有用な
ロボットのフレキシビリテイヲ増スようにしたロボット
の制御方式に関するものである。DETAILED DESCRIPTION OF THE INVENTION [Field of Application of the Invention] The present invention relates to a control method for a robot that increases the flexibility of the robot, which is useful when the robot is applied to actual work.

〔発明の背景］ ロボットヲ現場の力Ｏ工又は組立ラインで使用する場合
、その１寸の形で使用される例は少なく、ロボットの先
端にあるハンド部に、あるいはアーム部とハンド部の間
にツールを設置して初めて現」＾での稼働が実現するり
Ｎが多（ハ。ぞのツールば、精を横曲に見て、自由度が
Ｏ（動かない）の場合より、１，２あるい（ζ１それ以
上の場合が各（ハ、この傾向は１作業が複雑、高度化す
る程高才る１頃向にある。また、同じ作業でも。[Background of the Invention] When robots are used in manual labor at the site or on an assembly line, there are few examples of robots being used in their one-size form; It is only after the tool is installed that it can be operated in its current state. Or (ζ1 or more) (c) This tendency is towards 1, where the more complex and sophisticated the task, the higher the level of skill.Also, even in the same task.

サイズの異る対象物を扱う時（ｄ、異るツールと交換す
ることも必要である。さらに−異る作票を、ロボットに
させる場合に１．ツール又＋！、！ｌ−の必要性は、一
層高する。このツール交換ｄ５手間がかかり、生＃時間
、コストの面から見て無駄な要因である点１は明白であ
り、その他に、これから派生する信頼性を保持するプｊ
めのツールの保守、管理等、多くの問題を持つ。When handling objects of different sizes (d), it is also necessary to exchange them with different tools.Furthermore, when having a robot create different forms, 1. Tools +!, !l- are necessary. Point 1 is clear that this tool replacement is time consuming and wasteful in terms of time and cost.
There are many problems such as maintenance and management of the tools used.

寸だ、別の見方をすれば、ロボットとは、従来の自動機
械が単一の部品１作業しか対象とできなかった点と異り
、多品麺の部品全扱い、異る作業を同一の機械で可能で
ある点に、その大きな意義がある。このため、ロボット
が１品種。From a different point of view, robots can handle all parts of multi-product noodles and perform different tasks in the same way, unlike conventional automatic machines that can only handle one part and one task. Its great significance lies in the fact that it is possible with machines. For this reason, there is only one type of robot.

作業が変わる度に、ツールの交換が必要であるとすると
、汎用性あるいはフレキシビリティの面から、ロボット
としての特徴を誇示しにくい。If tools need to be replaced every time the task changes, it will be difficult to show off the features of a robot in terms of versatility or flexibility.

しかし、このようなツール交換或いはこれと同等の事を
、６在のロボット技術では行なゎさる′ｆＫ：得ないの
が」実であり、汎用性、フレキシビリティが期待されて
いる程高くない所に問題がある。これに対処するために
、ツールに汎用性金持たせることが考えられるが、汎用
性を持つツールとけ、往々にして１機構が複雑であり、
不必要に部分を含む場合が多く、ツール交換を行なう場
合よシ、むしろ無駄が多くなることがある。このように
、ｋ幼￥機構すだけで解決しようとすると、逆に、この
フレキシビリティとｂう問題は、より難しくなるという
性質を持つ。However, the reality is that it is not possible to perform such tool exchange or something similar with existing robot technologies, and the versatility and flexibility are not as high as expected. There is a problem here. To deal with this, it is possible to make the tool more versatile, but tools that are versatile often have a complex mechanism.
In many cases, parts are included unnecessarily, which can lead to more waste than when exchanging tools. In this way, if you try to solve the problem by just using the mechanism, the problem of flexibility will become more difficult.

ここで従来よシ使用されているツールについて説明する
。第１図は、ロボットの外形を示したものであるが、ア
ーム部１のさらに先にツール（含ハンド）２を投信する
場合が多い。このツール２を自由度別に分けて説明する
。第２図（（＋１　、　（Ａｌは、１自由度のツールを
示しである。第２図（ａｌＫ示すハンド３ａｌ−Ｊベー
ス６に対して並進する。−！た第２図（ｂｌに示すハン
ド３ｂけベース６に対して回転をする。ここで各ハンド
３σ、　３Ａにバネ４．及びダンパ５を加える。バネ４
は、一般に知られるように復元力を生じ、ダンパ５は粘
性力を生じ運動の安定化に寄与する。尚、ここで必ずし
も両者を共に加える必要は無い、第３図は２自由度のツ
ールを示１．であるが、ハンド３ｃは部拐８に対してガ
イドレールＺ上を並進する（Ｘ軸方向）６寸た部材８は
、ベース６に対して並進する（ｙ軸方向）。第４図は、
３自由度のツールを示してあシ、同様に、ハンド３ｄは
部材１０に対して並進しく２軸方向）１部材１゜け部材
１１に対して並進しく　、−ｎＭｔ＋方向）、オだ部材
１１はベースに対して並進する（ｙ軸方向）。Here, we will explain the tools that have traditionally been used. Although FIG. 1 shows the external shape of the robot, a tool (including a hand) 2 is often placed further ahead of the arm portion 1. This tool 2 will be explained in terms of degrees of freedom. Figure 2 ((+1, (Al indicates a tool with one degree of freedom. 3b rotates with respect to the base 6.Here, add spring 4 and damper 5 to each hand 3σ and 3A.Spring 4
generates a restoring force as is generally known, and the damper 5 generates a viscous force that contributes to stabilizing the motion. Note that it is not necessary to add both of them together; FIG. 3 shows a tool with two degrees of freedom. However, the hand 3c translates on the guide rail Z with respect to the part 8 (in the X-axis direction), and the member 8, which is 6 inches apart, translates with respect to the base 6 (in the y-axis direction). Figure 4 shows
Similarly, the hand 3d is translated with respect to the member 10 in two axial directions), and the hand 3d is translated with respect to the member 11 by 1 degree (-nMt+ direction), and the other member 11. is translated relative to the base (in the y-axis direction).

上記ベース６として示した所が、実際にロボットに取付
ける時には、第１図に示すロボソトのアーム部１と寿る
。When the part shown as the base 6 is actually attached to a robot, it serves as the arm part 1 of the robot robot shown in FIG.

更にＲＣＣ（遠隔中心コンプライアンス）機構について
考察する。Furthermore, the RCC (remote center compliance) mechanism will be considered.

ＦＣＣ機構は、以上の所までに述べたロボット用ツール
として組立、はめあい作柴に適する代表的ガ例である。The FCC mechanism is a typical example of the robot tool described above that is suitable for assembly and fitting.

実際のＲＣＣ機構は空間機構であるが、その原理を説明
するために平面楼構（２自由度）として第５図として表
示した。ＦＣＣ機構を２自由度の平面４幾構とみなして
も、原理の上から、伺らＲＣＣ機構の原理な損なうもの
で無いため、ここでは、　ＲＣＣ機構とは第５図に示す
機構として説明する。The actual RCC mechanism is a spatial mechanism, but in order to explain its principle, it is shown as a planar tower structure (two degrees of freedom) as shown in Fig. 5. Even if the FCC mechanism is considered to have 4 planes with 2 degrees of freedom, it does not undermine the principle of the RCC mechanism, so here, the RCC mechanism will be explained as the mechanism shown in Figure 5. .

ここでワーク９はハンド６ｅに把握されているために、
ベース６に対して２自由度機構に持つ。Here, since the workpiece 9 is grasped by the hand 6e,
It has a two-degree-of-freedom mechanism with respect to the base 6.

ベース６、部拐１０α９部＠１０Ｃ及び部側１０ｂより
構成される平行四辺形リンク機構により、ワーク９はべ
〜ス乙に対し変位が微小であれば。Due to the parallelogram link mechanism composed of the base 6, the part 10α9 part @10C, and the part side 10b, the workpiece 9 can be moved only if the displacement with respect to the base B is minute.

並進連動を行ない、部材ＩＱＣ，部材１０ｄ、ノ−ンド
３ｅ及び部材１０１１により構成される台形リンク機構
によりＱ点を瞬間回転中心（遠隔中心）として、ワーク
９は変位が微小であれば回転運動を行な、う。また、こ
れらの並進及び回転運動がはめあいに有効となる様姥、
第５図のように各部材が配置されており、そこには、各
バネ４（複数）、各ダンパ５（複数）が第５図のように
配置されている。Translational interlocking is performed, and the workpiece 9 performs rotational movement with the Q point as the instantaneous center of rotation (remote center) using the trapezoidal link mechanism constituted by member IQC, member 10d, node 3e, and member 1011, if the displacement is minute. Let's go. In addition, the manner in which these translational and rotational movements are effective for fitting,
Each member is arranged as shown in FIG. 5, and each spring 4 (plurality) and each damper 5 (plurality) are arranged there as shown in FIG.

ＲＣＣ機構を以上のような機構であると邑い換えること
ができる理由は１機能の上から両者はほぼ同一であると
言うことができるからである。The reason why the RCC mechanism can be replaced with the above mechanism is that the two can be said to be almost the same in terms of one function.

〔発明の目的〕[Purpose of the invention]

本発明の目的は上記従来技術の問題点に外み。 The purpose of the present invention is to overcome the problems of the prior art described above.

ソフトウェアの面からロホットのフレキシプリティを増
加させ、ツール変換をし７なくても様々ガ作業を施行で
きる汎用性のあるロボットの制御方式全提供するにある
。The goal is to increase the flexibility of robots from the software perspective and provide a versatile robot control system that can perform a variety of tasks without changing tools.

〔発明の概要〕[Summary of the invention]

即ち本発明は、ロボットに加えられプと力井たはモーメ
ント−またはその両者（’ｎ　−１）　を検出する第１
の検出手段と、該第１の検出手段によって検出された方
寸フではモーメンｌ−ｄだけその両者である第１の力信
号（Ｆ、、−＋）　と予め定められた方寸たはモーメン
トｔたはその両者の基準と々る基準力信号（Ｆ２．−１
）との差（７ｎ　−Ｉ　Ｊｅ、　ｎ　−１）を引算する
第１の計算手段と、ロボットの現在の位置または姿勢ま
たはその両者（：ｒ：ｎ−１）を検出する第２の検出手
段と、該第２の検出手段によって検出された埃、在の位
置または姿勢またはその両者である第１の位置信号（ｘ
、、−１）と予め定められた位ｆ？、寸たけ姿勢または
その両者の基準となる位置信号（”ｒ、ｎ−１）　との
差（：ｔｎ−１−、？、、、−Ｏを計算する第２の計算
手段と、該第２の計算手段で計算された位置信号を線形
変換して力信号（Ｌｌ（ブｎ　−１’：ｒ、　ｎ　−１
）　）を計算する第３の計算手段と、上記第１の計算手
段で計算された力信号（ｐ、、−Ｈ−Ｆｅ、　ｎ−１）
と第３の計算手段で計算された力信号（Ｌ＋　（＝ｎ−
１”ｒ、−１））との差ＣＣＦｎ−ｔ　−ｐｃ、　＊−
’１）−Ｌ＋　（”ａ−ｊ　”ｒ、　ｎ−１）］を計算
ｆ　ルＦ　４　ｏ　計算−Ｊ［と、該第４の計算手段で
計算された力信号を線形変換して第１の速度信号（Ｌ２
ＣＣＦｎ−１−Ｆｅ、　ｎ−１）−４，Ｃｘｎ−＋　−
ｘ、、　、−＋）〕）　を計算する第５の計算手段と、
前回の積分効果信号（υ７−１）を線形変換して第２の
速度信号（ＬａＦ３−１）を計算する第６の計算手段と
、第５の計や１手段で計算される第１の速度信号と第６
の計算手段で計算さ１Ｌる第２の速度信号との差（Ｌｚ
　［（７，−＋−ＦＣ９ｎ−１　）−Ｌｌ（”ｎ−１’
ｒ、　ｎ−１）］−Ｌ３　ｖｎ−１）を次回の積分効果
信号（ｖｌ）として引算する第７の計算手段と金備え付
け。That is, the present invention provides a first method for detecting force or moment applied to the robot, or both ('n -1).
and a first force signal (F, , -+) which is both a moment l-d and a predetermined square or moment t. is the reference force signal (F2.-1
), and a second calculation means for detecting the current position or orientation of the robot, or both (:r:n-1). and a first position signal (x
,,-1) and the predetermined place f? a second calculation means for calculating a difference (:tn-1-, ?, , -O) between the position signal ("r, n-1) serving as a reference for the , diagonal posture, or both; The position signal calculated by the calculating means of
)) and a force signal (p, , -H-Fe, n-1) calculated by the first calculation means.
and the force signal (L+ (=n-) calculated by the third calculation means
1"r, -1)) CCFn-t -pc, *-
'1)-L+("a-j"r, n-1)] is calculated f le F4 o calculation-J[, and the force signal calculated by the fourth calculation means is linearly transformed to calculate the first speed signal (L2
CCFn-1-Fe, n-1)-4, Cxn-+ −
a fifth calculation means for calculating x, , , −+)]);
a sixth calculation means for calculating a second speed signal (LaF3-1) by linearly converting the previous integral effect signal (υ7-1); and a first speed calculated by a fifth sum or one means. Signal and 6th
The difference (Lz
[(7,-+-FC9n-1)-Ll("n-1'
r, n-1)]-L3 vn-1) as the next integral effect signal (vl).

第７の計算手段で計算される次回の積分効果信号を速度
指令とすることを特徴とするロボットの制御方式である
。This robot control method is characterized in that the next integral effect signal calculated by the seventh calculation means is used as a speed command.

壕だ本発明は第３の計算手段に卦ける線形変換り、が第
１の対角行列［１〕であることを特徴とするものである
。また本発明は第５の引算手段における線形変換り、が
第１のスカシ（−ｌ）と第２のスカシ（ｍ）との比（星
）であることを特徴とするものである。また本発明は第
６の計算手段における線形変換り、が、第１のスカシ（
△ｔ）と第２のスカシ（−）との比（”−！−）に第３
の対角行列を掛けた値（−［Ｃ］）から単位行列（１）
を減じた値と々る第２の対角行列（”（’］−Ｉ）であ
ることを特徴とするものである。The present invention is characterized in that the linear transformation in the third calculation means is the first diagonal matrix [1]. Further, the present invention is characterized in that the linear transformation in the fifth subtraction means is a ratio (star) between the first scale (-l) and the second scale (m). Further, the present invention provides that the linear transformation in the sixth calculation means is
△t) and the second square (-) ("-!-) has a third
The identity matrix (1) is obtained from the value (-[C]) multiplied by the diagonal matrix of
It is characterized by being a second diagonal matrix ("(']-I), which is the value obtained by subtracting .

即ち、本発明は第１の検出手段でロボットの・・ンドに
加わる力またはモーメン）（７，−１）　を検出し、こ
の検出された方寸たけモーメント等の第１の力信号Ｃ’
１．−１＞　Ｋ基いて、ツール力するかのようにハンド
を動かすようにしたことにある。従って本発明では、ソ
フト上のパラメータを変更するだけで、そのパラメータ
が意味するツールを取付けて動いているかのような運動
奢ハンド′部にさせることができる。即ち第１の対角行
列（［＆］）と、第６の対角行列（〔ｃ〕）と−第２の
スカシ（ンｎ）を作業に応じて定義させればよく、オた
第１のスカシ（Δｌ）はサンプリングタイムに応じて定
義させればよい６す！にロボットの応答性は第２のスカ
シ（Ｔｎ、）と゛第３の対角行列（〔Ｃ〕）とによって
調整でき、捷たロボットの制御の安定性は第２のスカシ
（ｍ、）と第１の対角行列（［ｋＤと、第３の対角行列
（〔Ｃ〕）とによって調整できるものである。That is, in the present invention, the first detection means detects the force or moment (7, -1) applied to the robot's... and the first force signal C' such as the detected horizontal moment.
1. -1> Based on K, the hand is moved as if using a tool force. Therefore, in the present invention, by simply changing the parameters on the software, it is possible to attach the tool defined by the parameters and make the hand part move as if it were moving. That is, it is sufficient to define the first diagonal matrix ([&]), the sixth diagonal matrix ([c]), and the -second matrix (n) according to the work, and It is only necessary to define the distance (Δl) according to the sampling time6! The responsiveness of the robot can be adjusted by the second scale (Tn,) and the third diagonal matrix ([C]), and the stability of control of the robot can be adjusted by the second scale (m,) and the third diagonal matrix ([C]). 1 diagonal matrix ([kD]) and a third diagonal matrix ([C]).

なお、」＝記第１の検出手段はロボットになくてもよい
ことは明らかである。Note that it is clear that the robot does not need to include the first detection means.

〔発明の実施例〕[Embodiments of the invention]

以下本発明を図忙示す実施例に基いて具体的に説明する
。即ち全体構成は、氾６図に示すように例えばベース上
を垂直軸寸わりにｇＱ回する旋回台１２αと、該旋回台
１２１Ｚ上を水平ｌ１１１１を中ルに回転する上腕１２
６と、該上肺１２ｂの先処回転自在に連結された前腕１
２Ｃと、該前腕１２Ｃの先帰に図示の如く３自由度でも
って回転出来る」。The present invention will be specifically explained below based on the illustrated embodiments. That is, as shown in Fig. 6, the overall configuration includes, for example, a swivel table 12α that rotates gQ about the vertical axis on the base, and an upper arm 12 that rotates horizontally l1111 on the swivel table 121Z.
6, and a forearm 1 rotatably connected to the upper lung 12b.
2C and the forearm 12C can rotate with three degrees of freedom as shown in the figure.

うに構成された手首１２ｄとを有するｒ″Ａ節形ロボ・
ソト１２であって−この関節形ロボット１２の手葭１２
ｄの先に力センサ１３ｆ：取付＋７−ｊ−この力センサ
１３によりロボットに加えられる力を検出し１才だロボ
ット１２の各腕部材１２σ、１２ｂ、１２Ｃ，１２ｄの
各々を駆動するモータＩｆ等に連結されたエンコーダｌ
）Ｅ等からロボットの現在位Ｖ？、に検出するようにな
っている。なおモータＭにけ各腕部材の目映速度を検出
できるようにタコジェネＩＧ等が連結されている。捷た
ロボットの子猫１％に対象物との距離を検出する近接セ
ンナ等を取付けてもよい。r'' A-joint robot with a wrist 12d configured as
Soto 12 - the handshake 12 of this articulated robot 12
A force sensor 13f is installed at the end of d. The force sensor 13 detects the force applied to the robot by the force sensor 13. encoder l coupled to
) From E etc., the robot's current position V? , it is designed to be detected. Note that a tachogenerator IG or the like is connected to the motor M so that the visual speed of each arm member can be detected. A proximity sensor or the like that detects the distance to an object may be attached to the 1% of the robot kittens that have been deformed.

このようにしてロボットの位置Ｔ、力γＶ等をマイコン
で構成された制御装置１５に取込み、更に位Ｍｒ、力乙
等の指令値を操作卓４３等から制御装置１５に入力して
与えられる。即ち制御装置１５は第６図に示す如く、マ
イクロプロセッサ６７と、ＲＡＭ５Ｂと−ＲＯＭ５９と
、演算部４ｏと、操作卓４３に接続するインターフェー
ス回路（１７Ｆ）　４１　ト。In this way, the robot's position T, force γV, etc. are input to the control device 15 comprised of a microcomputer, and command values such as position Mr, force V, etc. are inputted to the control device 15 from the console 43 and the like. That is, as shown in FIG. 6, the control device 15 includes a microprocessor 67, a RAM 5B, a ROM 59, an arithmetic unit 4o, and an interface circuit (17F) connected to the console 43.

Ｄ／Ａ変換回路６５と、エンコーダＰＥからの出力を計
数するカウンタ３４と、アンプ１８から出力されるアナ
ログ力信号をディジタル信号に変換するＡ／Ｄ変換回路
と、それらを接続するパスライン４２とから構成されて
いる。A D/A conversion circuit 65, a counter 34 that counts the output from the encoder PE, an A/D conversion circuit that converts the analog force signal output from the amplifier 18 into a digital signal, and a pass line 42 that connects them. It consists of

ここで位ｆ￥、フ７．カフ、等の指令値は０でもよいが
、このときは、ロボットはハンド部１４に左えられる力
尋によってのみ動作する。即ち受動的な機構となる。一
方ハンド部１４に力に力が加わらず（或いはセンサ類１
３を取付けずに）位置フ、。Here, place f￥, f7. The command values for the cuff, etc. may be 0, but in this case, the robot operates only by the force exerted on the hand section 14. In other words, it becomes a passive mechanism. On the other hand, no force is applied to the hand portion 14 (or the sensors 1
3) position F, without installing.

力ｒｏ等の指令値のみが与えられたときはその指令値に
よって動きを定められる。When only a command value such as force ro is given, the movement is determined by the command value.

いずれにしても制御装置１５は、ザーボアンブ１６に対
し、ロボットの関節角速度ａの指令値を出力し、ロボッ
ト１２を動作させる。In any case, the control device 15 outputs a command value for the joint angular velocity a of the robot to the servo amplifier 16, and causes the robot 12 to operate.

ここで、第６図の制御装置１５をやや詳しく。Here, the control device 15 in FIG. 6 will be explained in a little more detail.

第８図に示す。It is shown in FIG.

制御装置１５の座標変換２１は、ロボットの各関節のエ
ンコーダＰＲからの信号をカウンタ３４で引数して現在
位置１を受け、関節角度７／ハンド系の直交座樟マなる
座標変換を行在う。実際にはマイクロプロセッサ６７が
らの指令で演−０部４゜が行う６オたロボットにカロゎ
る外カフ、金、力センサ１３が検出し、力センサ１６を
第１♂を成している歪ゲージの電圧７．として、制御装
置１５は、Ａ／〃変換回路３６を介して取込む。カ算出
部２ｏけ−７ｙ。The coordinate transformation 21 of the control device 15 receives the current position 1 by using the counter 34 as an argument for the signal from the encoder PR of each joint of the robot, and performs the coordinate transformation of the joint angle 7/the orthogonal coordinates of the hand system. . In reality, the outer cuff is detected by the force sensor 13, and the force sensor 16 constitutes the first male. Strain gauge voltage7. The control device 15 takes in the signal through the A/〃conversion circuit 36. Calculation unit 2o-7y.

マを取込み、壕ず歪ゲージの電圧７．を歪／カの変換マ
トリックス等により力に変換する。実際には、マイクロ
プロセッサ３７からのＪ旨令で演算部４０が行う。次に
ハンド１４及びハンドが把持する物の重量が大きい場合
には、ハンドの姿勢によって力センサ１６に与える値が
異る。このため。Take in the voltage of the trenchless strain gauge 7. is converted into force using a distortion/force conversion matrix, etc. Actually, the calculation unit 40 performs this operation in response to the J command from the microprocessor 37. Next, when the weight of the hand 14 and the object it grips is large, the value given to the force sensor 16 differs depending on the posture of the hand. For this reason.

ハンド１４の姿勢を１より算出し、これによる影ＩＰ？
を取り除き、実際にロボットに加ったカア。を推定する
。こうして推定された値がアであり、制御主要部２２に
与えられる。制御主要部２２によって、ノ・ンド１４が
動作すべき速度マが鎧、出される。次に１はノ・ンドに
固定された座標系での値であるので、これを座標変換２
６によって一ベースの座標系での速度マ。に変゛換する
。The posture of the hand 14 is calculated from 1, and the shadow IP?
Kaa actually joined the robot after getting rid of it. Estimate. The value estimated in this way is A, and is given to the main control section 22. The control main section 22 determines the speed at which the engine 14 should operate. Next, since 1 is a value in the coordinate system fixed to No.
6. Velocity ma in one-base coordinate system by 6. Convert to .

この後、直交座標系での速度−から、関節角にＪ−１は
逆ヤコビアン行列と呼ばれ、その要素はロボットの各関
節角度Ｔの関数である。）これに基づき、サーボアンプ
２５が作動し、ロボットの各関節用のモータが動作する
。After this, from the velocity in the orthogonal coordinate system, the joint angle J-1 is called an inverse Jacobian matrix, and its elements are functions of each joint angle T of the robot. ) Based on this, the servo amplifier 25 is activated, and the motors for each joint of the robot are activated.

制御主要部２２には、上記のロボットからの力γ、ロボ
ットの位置プ以外に、力９位を凶の指令値Ｆ−，？、が
入力され、・・ンド部の速度マが出力される。In addition to the above-mentioned force γ from the robot and robot position P, the control main unit 22 has command values F-, ? , is input, and the speed ma of the ... end part is output.

この制御主要部２２の信号処理が１本発明の原理をなす
もので、第９図と下記の式を用いて。This signal processing of the main control section 22 constitutes one of the principles of the present invention, using FIG. 9 and the following equation.

概要の項で述べた内容（即ちツール代替機能）が、制御
主要部２２によって如何に実現されるか説明する。A description will be given of how the content described in the overview section (ie, the tool replacement function) is realized by the main control section 22.

ところで第２図（α）て示しプｊ１自由度のツールのモ
デルを斂式で示すと次のｆｉ１式の如くとなる。By the way, if the model of the tool with 1 degree of freedom shown in FIG.

ｍ−＋　Ｃｖ＋ｋ　（ｘ−ｘ　、）−Ｆ−Ｆ−、−−−
・（ＩＩｔ ここでハンド６のｅ量をｍ、バネ４のバネ係数ｆｋ、ダ
ンパ５の粘性係Ｂ−，ｃとし、ｘｌは）々ネのバランス
時の位置を示し−７”、け力の基準値である。m-+ Cv+k (x-x,)-F-F-, ---
・(IIt Here, the e amount of the hand 6 is m, the spring coefficient fk of the spring 4, the viscosity coefficient B-, c of the damper 5, and xl) represents the position of each spring in balance -7'', and the This is the standard value.

あるツールのＪ　ｉｉ／Ｉが一上記（１）式で表現され
るとすると、（１）式は（ｐ−ｐ、）　なる力がツール
に加えられたときのツールの位１１り、及び速度がｒ−
及びνとなることを示す７ ここでｖ、　：Ｅ、Ｆは時間ｌの関数であるが。If J ii/I of a certain tool is expressed by the above equation (1), then equation (1) is expressed as is r-
and ν7 where v, :E and F are functions of time l.

Ｆｅ、”、、ｍ、Ｃ，にも口１間ｔの関りとすることが
できる。しかし以後は主にｍ、Ｃ，には定斂として考え
る。Fe,'', m, C, can also be considered as a relationship between 1 and t.However, from now on, m, C, will mainly be considered as a constant.

もしロボットのハンドに（Ｆ−Ｆ、）　なる力を加えた
とき、ロボットのハンドがＸ、及びυなる位置、及び速
度を持ち、これらの関係が上記ｆｌ１式で表現されるな
らば、ロボットのハンド１４Ｆｉ。If a force (F-F,) is applied to the robot's hand, the robot's hand has positions and velocities of X and υ, and these relationships are expressed by the fl1 formula above, then Hand 14Fi.

実際にはツールを装着していなりにも拘らず。Even though I was actually wearing a tool.

ツールを装着したときと同様の運動をさせることができ
る。It is possible to perform the same exercise as when wearing the tool.

以上が本発明の詳細な説明である。The above is a detailed description of the present invention.

即ち前記の如くロボットに装着させたいツールを相定し
てロボットを制御させることによって、そのツールをロ
ボットに組込んだときにするであろうハンド部の動きを
ロボットにさせることができる。That is, by determining the tool to be attached to the robot and controlling the robot as described above, it is possible to cause the robot to make the movements of the hand portion that would occur when the tool is installed in the robot.

更に上記ｉｉ１式は次の（２）式のように変形される。Further, the above equation ii1 is transformed as shown in the following equation (2).

Ｕ＝−ｆｃｃＦ−Ｆｅ）−ｋｃｘ−ｘｒ）−ＣＵ］ｄｔ
　−・−、、、、ｆ２１この（２）式の変形させると以
下の特ａｔ有する。U=-fccF-Fe)-kcx-xr)-CU]dt
−・−, , , f21 When this equation (2) is transformed, it has the following characteristics at.

第１の特徴は、第８図にも示しである如く、実際にロボ
ットの各関節を駆動させるモータＭを動作させるときに
は、速度あるいは速度に相当する指令値をサーボアンプ
に与えるため一速度υを計算手段で算出することが有用
だからである。第２の特徴は、計算に微分処理が含オれ
ず。The first feature is that, as shown in Fig. 8, when actually operating the motor M that drives each joint of the robot, one speed υ is given to the servo amplifier to provide the speed or a command value corresponding to the speed. This is because it is useful to calculate using calculation means. The second feature is that the calculation does not include differential processing.

ノイズに対して強く、誤動作がなくｆｘｌ：ｒ、ロボッ
トのハンド部に安定した動きをさせることができる。It is resistant to noise, has no malfunctions, and can make the robot's hand move stably.

ところで上記（２）式を離散値系で示すと１次の（３）
式となる。By the way, if the above equation (2) is expressed as a discrete value system, the linear equation (3)
The formula becomes

ここでΔｔはヤンプリング時間、ザフィックスを壕だは
れはｉｉたけｒＬ番目のイ直である。Here, Δt is the dumping time, and the distance from which the Zafix is removed is the ii times the rLth straight line.

上記（３）式は更に次の（４）式となる。The above equation (3) further becomes the following equation (4).

υｎ−［（Ｆｎ−１’ｅ、ｎ−１）−ｋ（”ｎ−１”ｒ
、ｎ−１）’：１−〔可Ｃ−１〕’ｎ−１・・・・・・
・・（４）第３の特徴は、ｌｌ！Ｉｌ散値系に変形ｌ−
た上記（４）式を用いて説明できる。ＩＩＩち（４）式
の右辺にｖ、、−１（ｎの１つ前のＶ）があり、左辺に
ＩＩｎがある。つ寸り、ｎ番目のロボットのハンド部の
ｌＩ　（速度）を計算手段で算出するために、１つ前の
Ｖ（速度）さえあればよく、毎回タコジエネＴＧ等から
実際のＶ（速度）を検出し、フィードバックする必要が
無い。これは実際にロボットの駆動系に取付けであるタ
コジェネＴＧ等の速度検出器を用いてロボットのハンド
部の速ｊ尼ヲ検出したい場合、同じようにロボットの）
駆動系に敗付けであるエンコーダＩ’Ｅ等の位置検出器
で検出されるロボットのハンド部の位ＩＦｆ　ｌ＋て比
較して、信号の信♀ｎ、　ｕｔかや−や低いことを考慮
すると有用ｆｒ、特徴である。υn-[(Fn-1'e, n-1)-k("n-1"r
, n-1)': 1-[possible C-1]'n-1...
...(4) The third feature is ll! Transformed into Il distributed system l-
This can be explained using the above equation (4). III. The right side of equation (4) has v, , -1 (V one before n), and the left side has IIn. In order to calculate the lI (velocity) of the hand section of the n-th robot using a calculation means, it is only necessary to have the previous V (velocity), and each time the actual V (velocity) is calculated from the tachogene TG etc. No need for detection and feedback. This is true if you want to detect the speed of the robot's hand using a speed detector such as a tachogenerator that is actually attached to the robot's drive system.
It is useful to consider that the signal reliability ♀n, ut is rather low compared to the position of the robot's hand detected by a position detector such as an encoder I'E that is inferior to the drive system. fr is a feature.

勿論、（４）式にかいてＸに関するものとして”ｎ−１
が唯１つしか無い月１からも解かるように。Of course, as for X in equation (4), "n-1
As can be seen from the fact that there is only one month 1.

Ｘ（位置）は毎回検出する必要がある。このＸがロボッ
トの関部角度から算出される過程は。X (position) needs to be detected every time. What is the process by which this X is calculated from the angle of the robot's joint?

第６図の１．第８図のＴ、７．第９図のＸによって示さ
れる。以上のように（２）式あるいは（３）式の特徴が
３点ある。1 in Figure 6. T in Figure 8, 7. Indicated by X in FIG. As mentioned above, there are three characteristics of equation (2) or equation (3).

ところで実際組立作苗または加工作業を行うロボットは
多自由度で構成されてｂる６従ってロボットを制御する
速度制御信号ｖ７はベクトルを用いることによって次の
（５）式の関係で表わされる。Incidentally, a robot that actually performs assembly or processing work is constructed with multiple degrees of freedom.Therefore, the speed control signal v7 for controlling the robot is expressed by the following equation (5) using vectors.

７、−ｔｈ、、ｔ　〔（７，、７，、ｎ−１）、ｋ〕、
７．−１２．　ａ−１）〕位行列を示す７切にベクトル
は６次元、行列は６行、６列である。（２）式をブロッ
ク図で示したものが第９図の制御主要部２６であり、り
５際に計算機で多自由度のロボットをｆｌ制御すること
になるので（５）式の離散値系の式が用いられることに
なる。7,-th,,t [(7,,7,,n-1),k],
7. -12. a-1)] The 7-section vector indicating the rank matrix is 6-dimensional, and the matrix has 6 rows and 6 columns. The block diagram of equation (2) is the main control section 26 in FIG. The formula will be used.

この中でｍ、Ｃｋ〕、［Ｃ’］はツールを想定する際に
、決める値である。ツール？想定したならば、そのツー
ルの機構を解析し−ロボットに動作させる前にこ、＋］
らの値をオフラインで法定しておき、この値をそのツー
ルに固有のデータ（定数であっても変ｐであっても良い
。）とし７て操作卓４３等から入力して制御装置１５の
メモリ３８にストックしておく。つまり、想定し／こツ
ールが複数であれば、それに応じてス　トソクすべきデ
ータも複数となる。また＋１１で示される系が安定する
条件は、常微分方程式に関する基礎理論で既知である。Among these, m, Ck], and [C'] are values determined when considering the tool. tool? If it is assumed, analyze the mechanism of the tool - before making it work on the robot.
These values are determined off-line, and these values are input from the console 43 or the like as data specific to the tool (which may be a constant or a variable p) to the control device 15. Store it in the memory 38. In other words, if there are multiple assumptions/tools, there will also be multiple data to be stocked. Furthermore, the conditions for the stability of the system indicated by +11 are known from the basic theory of ordinary differential equations.

それに基いてｍ、ｃ、ｋｆ定めれば、安定させたり、発
散させたりすることも可能である。これと同様に（２）
式を離散値系に変換した（４）式で安定させるためには
、上記の条件の他に、ｚｔの値も十分小さくする必要が
ある。If m, c, and kf are determined based on this, it is possible to stabilize or diverge. Similarly to this (2)
In order to stabilize the equation (4) obtained by converting the equation into a discrete value system, in addition to the above conditions, it is also necessary to make the value of zt sufficiently small.

１−−１＜１　・・・・・・・・・・・（６）たたし、
△ｔはザンブリングタイムであるから、正価を取り、ｍ
は質量に和尚する値であるから。1--1<1 ・・・・・・・・・・・・(6) Tatami,
Since △t is the zumbling time, take the net price and m
Because it is a value that agrees with mass.

正値を取る。寸だ安定させる目的で用いるならば、Ｃは
正値とする。Takes a positive value. If it is used for the purpose of making it more stable, C should be a positive value.

以上寸で所で、本発明により、ツールの動作をロボット
にさせる方式について述べたが、これは主に前記（１１
式中のｍ、Ｃ，にの決定に関するものである。In the above, we have described the method of making a robot operate a tool according to the present invention, but this mainly applies to the above (11).
This concerns the determination of m and C in the formula.

次に前記（＋１式中の別の変数であるロボットに加えら
れる力Ｆ、この力の指令値Ｆ。、ツール位置の指令値ら
と、それに基づく動作について述べる。ν１１ちこのＦ
Ｉ　Ｆｅ　、　”ｆの３値を指令値として用いたｊμ合
、多くの組合せが考えられ、必要に応じてそれらの値を
決定すればよい。ここでは具体例として、以下の条件の
場合について考える。Next, we will discuss the force F applied to the robot, which is another variable in the formula (+1), the command value F of this force, the command value of the tool position, etc., and the operations based on them.
Many combinations are possible when the three values of I Fe and f are used as command values, and these values can be determined as necessary.Here, as a specific example, consider the following conditions. .

条件（ａＪ ４ｍｋ　−Ｃ２）０．　Ｃ＞０．　ｒｎ）Ｏ，ｘｒ＝０
．　Ｆト０　でＦが第１０図（α）に示す如くステップ
入力ｐ、、２７−＜取ったとき、次の（７）式の関係と
なる。Conditions (aJ 4mk-C2) 0. C>0. rn)O, xr=0
．． When F takes the step input p, , 27-< as shown in FIG.

たたし、ＴＩ＝０＝＝Ｑ　Ｉ　ｖｔ＝０＝０とする。However, TI=0==Q　I　vt=0=0.

この場合のロボットの動作Ｉ′ｉ第１０図（Ａｌに示す
如く出力２８となる。ｐｓステップ人力２７の大きさに
比例した値に位置が収束し、その時の立上り時間や振動
について１ｊ−ｒｎ、Ｃ，にで決する。Operation of the robot in this case I'i The output is 28 as shown in FIG. C. Decide.

条件（ｂｌ ４ｍｋ−Ｃ２：）Ｏ，Ｃ＞０．　ｍ）０．　Ｆ＝＝［］
、　Ｆ　、＝Ｏでｘｒに第１１図１ｚｌに示すようにス
テップ入力、？、、２９を馬えたとき５次の（８）式の
関係となる。Condition (bl 4mk-C2:) O, C>0. m) 0. F==[]
, F ,=O and step input to xr as shown in FIG. 11 1zl, ? , , 29, the relationship of equation (8) of fifth order is obtained.

ただし”ｔ＝Ｏ＝Ｏ＋　ｖｔ　＝０＝ａ　とする。However, it is assumed that "t=O=O+vt=0=a".

この場合のロボットの動作は第１１図（ｂｌに示す出力
６０となる。The operation of the robot in this case becomes the output 60 shown in FIG. 11 (bl).

これら第１０図（ｈｌに示す出力２８．第１１図（Ａｌ
に示す出力ろＯかられかるように、捷たこれらの出力と
、前記（１）式から推定できるように、ある条件の下で
はステップ入力をＦに与えても−ｒ、に与えても一１？
、に与えても、同等な出力が得られる。Output 28 shown in these Figures 10 (hl) and Figure 11 (Al
As can be seen from the output filter O shown in , as can be estimated from these shunted outputs and equation (1) above, under certain conditions, whether a step input is applied to F or −r, the output is the same. 1?
, the same output can be obtained.

これらの特徴を順次調べると、同じ出力を得るのに、変
化させる値をＦにしたり、ｒｒにしたり１選択すること
ができ、必要に応じて多種多様の制御が可１ヒとなる。If these characteristics are examined one after another, one can choose to change the value to F or rr to obtain the same output, and a wide variety of controls can be performed as needed.

条件（Ｃｌ ｋ　＝Ｑ、　ｎｒ）０．　Ｃ〉ｏ、　、ｒ、＝０．　Ｆ
、＝＝Ｑ　で第１２図（ａｒに示す如くＦにステップ人
力／”　ｓ３１を与えたとき、次の（９）式の関係とな
る。Condition (Cl k =Q, nr) 0. C〉o, ,r,=0. F
, ==Q, and when F is given a step force/''s31 as shown in FIG. 12 (ar), the following equation (9) is established.

ただし”　ｆ−〇−ＤＩ　”Ｚ＝Ｏ−０とする。However, "f-〇-DI" Z=O-0.

この場合、ロボットのハンドの速開は第１２図（ｂｌに
示す３２のようになシ、争件（α］、　（ＡＩと異り、
速度が収束する。つまり力によって、ロボットのハンド
の速度を制御することができる。In this case, the rapid opening of the robot's hand is similar to 32 shown in Figure 12 (bl).
The speed converges. In other words, the speed of the robot's hand can be controlled by force.

上記の如く簡単な実施例を３つ示したが、本発明はこの
他例も有用々多数の制御を実行できる可能性を有する。Although three simple embodiments have been shown above, the present invention has the possibility that other embodiments can also be usefully used to perform a large number of controls.

以上のように上記実施例として主に第２図（σ１の１自
由度のツールを想定し、ロボットにさせた場合について
、その原理や、動作例について述べた。As mentioned above, as the above-mentioned embodiment, the principle and operation example of the case where the tool is mainly assumed to be a robot with one degree of freedom of σ1 as shown in FIG. 2 (σ1) have been described.

しかし、実際には、１自由度以外に多自由度のツールを
想定することもでき、その場合は（５）式を用いる。However, in reality, a tool with multiple degrees of freedom in addition to one degree of freedom can be assumed, and in that case, equation (5) is used.

ただし、この場合は、ロボットの自由度と同じか、また
は、それより少ない自由度を持つ。However, in this case, the degree of freedom is the same as or less than that of the robot.

ツールを想定することが必要である、 ところで力センサ１６は第１６図、第１４図、第１５図
に示すように、ロボット１２の先瑞（手首）１２αとハ
ンド１４との間に設けられ、ハンド１４に加えられた力
ｆ　（Ｆｚ、ｐ′ｘ、　ＦＪ、　、ＡＩ２．　Ｊ／占Ｍ
ｙ　）を検出できるようになっている。即ち薄板ばね１
３　ｅ、　１５ｆ、　１釘。By the way, as shown in FIGS. 16, 14, and 15, the force sensor 16 is installed between the tip (wrist) 12α of the robot 12 and the hand 14. Force f (Fz, p′x, FJ, , AI2. J/Zan M
y) can be detected. That is, thin plate spring 1
3e, 15f, 1 nail.

１３ｈに歪ゲージが取付けられ、この歪ゲージから得ら
れる信号によってｆ（Ｆ、、　Ｆ、、　７”、、　Ｍ２
．　ＡＩ、、、　Ｍ、）が検出できるように力っている
。平行薄板ばね１３ｅはブロック１３（ｌの下つ：ｍと
ブロック１３ｈの上端との間に接続され、Ｚ方向の力Ｆ
ｚによって歪むように４１り成されている。平行薄板ば
ね１ｓｆ（ｒｉプロ、り１０ｂの上端とブロック１３Ｃ
の下端との間に接続され、Ｘ方向の力Ｆ工によって歪む
ように構成されている。十字状の薄板ばね１０りはブロ
ック１３ｃの中央の軸とブロック１３ＬＬの内婦との間
で接続され、ｙ　１ｉｌ１回りのモーメントＭ、によっ
て歪むように構成されている。÷状の薄板ばね１＋Ａｉ
ブロツク１６ｄの内端とハンド１４の上端の軸部材との
間に接続され、Ｘ方向の力Ｆｙ１．ｒ軸回りのモーメン
）　Ｍｒ：、Ｚ軸回りのモーメントＭ２によって歪むよ
うに構成されている。即ち第１５図（α１に示すように
Ｚ軸方向に力Ｆｚ、−４たけＸ軸方向に力ＦＴを加えた
とき、薄板ばね１３ｅ、−−ｊたは１３／が歪み、この
薄板ばね１３ｇまたは１５ｆに貼付けたφゲニジから力
Ｆｚ−５たはＦｘの信号が検出されるようになっている
。第１５図（ＡＩに示すようにｙ　ｆｆｌ＋方向に力Ｆ
ｙを加えたとき、薄板ばね１３Ａが歪み、この薄板ばね
１３ｋに貼付けた歪ゲージから力Ｆｙの信号が検出され
るようになっている。第１５図（Ｃ）に示すように、τ
輔寸たはｚ　ｅｈ回りにモーメントＭｘまたはＭｚをカ
ロえたとき、　′ｆｊ板はね、１３Ａが歪み、この薄板
はね１３Ａに貼付けた歪ゲージからモーメントＭｘ、寸
たはＨｚの信号が検出されるようになっている。第１４
図（ｄ＋に示すようにｙ軸回りにモーメン）　Ｊ（ｙ　
２加えたとき。A strain gauge is attached to 13h, and the signal obtained from this strain gauge allows f(F,, F,, 7”,, M2
．． AI,..., M,) is working to enable detection. The parallel thin plate spring 13e is connected between the lower end of the block 13 (l) and the upper end of the block 13h, and is
41 is formed so as to be distorted by z. Parallel thin plate spring 1sf (ri pro, upper end of ri 10b and block 13C
It is connected between the lower end of and is configured to be distorted by force F in the X direction. The cross-shaped thin plate spring 10 is connected between the central axis of the block 13c and the inner ring of the block 13LL, and is configured to be distorted by a moment M about y1il1. ÷-shaped thin leaf spring 1+Ai
It is connected between the inner end of the block 16d and the shaft member at the upper end of the hand 14, and applies the force Fy1. in the X direction. (moment around the r-axis) Mr:, is configured to be distorted by a moment M2 around the Z-axis. That is, when a force Fz is applied in the Z-axis direction and a force FT is applied in the X-axis direction by -4 as shown in FIG. A signal of force Fz-5 or Fx is detected from the φ gauge attached to 15f.
When y is applied, the thin plate spring 13A is distorted, and a signal of force Fy is detected from a strain gauge attached to the thin plate spring 13k. As shown in FIG. 15(C), τ
When the moment Mx or Mz is increased around the thin plate 13A, the 'fj plate 13A is distorted, and the signal of moment Mx, size or Hz is detected from the strain gauge attached to this thin plate 13A. It has become so. 14th
Figure (moment around the y-axis as shown in d+) J (y
When adding 2.

薄板ばね１３Ｆが歪み、この薄板はね１■に貼付けた歪
ゲージからモーメン）Ｍ、が検出できるようになってい
る。The thin plate spring 13F is distorted, and the moment) M can be detected from the strain gauge attached to the thin plate spring 1■.

以上説明した如く、主に式（１）〜（５）及び第９図を
用いて一つの実施例を示した。しかしく１）式を以下の
式ａ１のように一般化することによってより多くの変形
例を考えることができる。As explained above, one embodiment has been shown mainly using equations (1) to (5) and FIG. 9. However, by generalizing equation 1) as shown in equation a1 below, more variations can be considered.

〔Ｍ〕剣＋［ＣＦ４−〔Ｋ〕（￥、’Ｔ’）＝７−．７
”　−＋１１ｉｔ ただし７．位置ベクトル（直交座標系でのロボットの並
進位置及び回転角度） ７：速度ベクトル（直り座標系でのロボットの並進速度
及び回転角速度） ア：カセンサ等から検出される力信号のベクトル ７７：ソフトによる力指令値のベクトル・ア：ソフトに
よる位置指令値のベクトル［Ｊｆ〕：ツールの仮想ｆ１
’　Ｒを示すマトリックス 〔Ｃ〕：ツールの仮想粘性係数を示すマトリックス 〔Ｋ〕：ツールのバネ定係ムヲ示すマトリックス である。[M] Sword + [CF4-[K] (¥, 'T') = 7-. 7
” -+11it However, 7. Position vector (translational position and rotational angle of the robot in the orthogonal coordinate system) 7: Velocity vector (translational velocity and rotational angular velocity of the robot in the orthogonal coordinate system) A: Force signal detected from a force sensor, etc. Vector 77: Vector of force command value by software A: Vector of position command value by software [Jf]: Virtual f1 of tool
' Matrix [C] showing R: Matrix [K] showing the virtual viscosity coefficient of the tool: Matrix showing the spring constant of the tool.

弐〇旬を変形すると弐〇Ｄとなる。If you transform Ni〇Shun, it becomes Ni〇D.

７＝（ｚｆ〕−１／［（７’−，７）−（ＣＣ丁、＋（
ｘ〕（コ；　、７））〕、、・・・　α力四位僅帰還を
エンコーダＩ）Ｅからのノ・−ドで行い。7=(zf]-1/[(7'-,7)-(CC ding, +(
x〕(ko; ,7))〕,,... α force 4-position slight return is performed at the node from encoder I)E.

速度帰還を演算結果マを用いる場合式Ｑ１１を第１５図
に示す。Equation Q11 is shown in FIG. 15 when the calculation result is used for speed feedback.

ここで７・７の代りに・７・Ｉと表示したのは。Here, ・7・I was displayed instead of 7・7.

次の意味を持たぜるためである。This is to give it the following meaning.

１．マ：エンコーダ（・・−ド）からの位置帰還信号 ５７−演算結果（ソフト）による速度帰還信号つ−１）
、式α力の右辺に於いてＩはＨｚ即ちエンコーダによっ
て検出された値を用いる。寸た式ａカの右辺に、ｌ、−
いて、２ノ１７ｆ　５　＋＋即ちハードウェアによって
検出された値ではなく、演算手段によって算出された値
Ｖをその寸ま用いる。1. M: Position feedback signal 57 from encoder (...-) Speed feedback signal based on calculation result (software) 1)
, on the right side of the equation α force, I uses Hz, that is, the value detected by the encoder. On the right side of the formula a, l, -
2 no 17f 5 ++, that is, the value V calculated by the arithmetic means is used to that extent instead of the value detected by the hardware.

これによって、第１６図に示したようにロホノトに取付
けであるエンコーダＰＥ（あるいはそれに和尚する位置
信号を検出する機器）からの信号を必要とし、検出信号
精度の恋いタコジェネＴＧからの信号をフィードバック
するのではなく算出手段によって算出された＋ｊを用い
る。As a result, as shown in Fig. 16, it is necessary to receive a signal from the encoder PE (or a device that detects a position signal that corresponds to it) attached to the Rohonoto, and to feed back the signal from the tachogenerator TG, which has high detection signal accuracy. +j calculated by the calculation means is used instead of +j.

ここで第１６図に於けるＴからθへの変換は１ 〔Ｊ〕　なる行列（〔Ｊ〕はヤニ１フフ１１列と呼ばれ
る。）を用いて行なわれ、ＴからＨＴ＋への変換はロボ
ットの関節座標系から、ｊハ交ＩＬ＋’標系への座標変
換によって行々われる。Here, the conversion from T to θ in Figure 16 is performed using a matrix of 1 [J] ([J] is called the Yani 1 Fufu 11 column), and the conversion from T to HT+ is performed by the robot. This is performed by coordinate transformation from the joint coordinate system to the j-cross IL+' reference system.

ここで式ＯＤによる計算は、実際には計算機では次の（
６）式及び（１３式の如く離散値系によって行なわれる
。Here, the calculation using the formula OD is actually performed on a computer using the following (
6) and (13) using a discrete value system.

ヰ；を１弓〔Ｍ〕−”テ１〔（Ｋ−工）−（［Ｋ］（マ
、−５□）十［Ｃ：］＋＋、）］　・川・曲用α埠ただ
しｎ−１，２，・・曲 マ、−Δｔ　θ→［Ｍ丁１〔（アｎ−＋　ｒ／ｎ−＋）
−［Ｋ：］］σｎ−４ｒ’ｎ−４〕−〔Δｌ←す［Ｍ］
　［Ｃ〕−（７丁筺、−１　・・曲・・・・αｊただし
ｎ＝１．２．・・・・ 式０オ、０埠に於いて Δｔ：サンプリング時間を示すスカシ 〔Ｉ〕：単位マトリックス ｘ、、ｎ番目の（サンプリング時間後の）マｖｎ：ｎ番
目の（サンプリング時間後の）Ｖｒｎ　、ｎ番目の（サ
ン、プリング時間後の）ア、、Ｔ、　、ｎ番目の（サン
プリング時間後の）マ。ヰ; 1 bow [M] - "Te 1 [(K - Technique) - ([K] (Ma, -5□) 10 [C:] ++,)] ・Kawa/Kyuyo α-bashi Butadashi n-1 , 2,... Song ma, -Δt θ → [M-d1 [(An-n-+ r/n-+)
-[K:]]σn-4r'n-4]-[Δl←su[M]
[C] - (7-cho, -1... Song... αj However, n = 1.2.... Formula 0, 0, Δt: Squash [I] indicating sampling time: Unit matrix time later) Ma.

、Ｕ７．ｎ番目の（サンプリング時間後の）７ｒ、７：
、ｒＬ番目の（サンプリング時間後の）ｒアオた式θ１
において初期値マ０は別途定める必要がある。, U7. nth (after sampling time) 7r, 7:
, rLth (after sampling time) rAota expression θ1
In this case, the initial value Ma0 needs to be determined separately.

（２）位置帰還も、速度帰還もハードによる場合。(2) When position return and speed return are both hardware-based.

さて式α］ｌを第１７図のよう外ブロック図として記載
することもできる。これが第１６図と異なる点けＺ、ｖ
の代りにＨ”＋Ｈｖを用いたことにある。Now, the formula α]l can also be written as an external block diagram as shown in FIG. This is different from Fig. 16 at points Z and v.
The reason is that H''+Hv was used instead of .

〃ｘは第１６図で用いたものと同じであるため説明を省
く。〃Since x is the same as that used in FIG. 16, its explanation will be omitted.

これによってＳマの離散値系での値マ、は例えば０４式
でまる。As a result, the value of S in the discrete value system can be calculated using the formula 04, for example.

ｘｎ瓢を一弓ｖｎ−１”ｎ−１・・・　・・ａ→ただし
ル＝１．２．・・・・・・ 以上のようにＯ〕、■、（５）の場合について述べたが
、各々次のような長所、知所を持つ。A bow of xn gourd vn-1"n-1... ・・a → However, le = 1.2... As above, we have described the cases of O), ■, and (5). , each has the following advantages and knowledge.

即ち、帰還の目的からすると上記（２）の方式（第１７
図の方式）の如く１位置も、速度も実際の値を検出して
帰還させるのがよい。しかし現在の技術レベルから判断
するとロボットに取付けられた位置の検出器ＰＥはかな
り良い精度を持つが、速度の検出器ＴＧＩＪそれに比べ
て劣る。このため両者を折衷した０〕の方式（第１６図
の方式）が現実的に最も優れている。In other words, from the purpose of return, method (2) above (17th
It is preferable to detect and feed back the actual values of both the position and velocity, as in the method shown in the figure. However, judging from the current state of technology, the position detector PE installed on the robot has fairly good accuracy, but it is inferior to the velocity detector TGIJ. For this reason, the method 0] (the method shown in FIG. 16), which is a compromise between the two, is actually the best.

また（支）の方式（第１８図の方式）は全く検出器を必
要としない点に特徴を有する。しかし現状のロボットに
は第６図に示す如く位置の検出器ＰＥも速度検出器ＴＧ
も付いているのでこれを用いればよく、ここでの特徴は
ＨＵである。Furthermore, the (supporting) method (the method shown in FIG. 18) is characterized in that it does not require any detector at all. However, in the current robot, as shown in Figure 6, the position detector PE and the speed detector TG are
It also comes with one, so you can use that, and the feature here is HU.

、、？、タコジエネＴＧからの速度帰還信号つまり、こ
の場合５位置、及び速度の両帰還をハードとして実際に
検出される信号を用いることにある。即ち前記式α力の
右辺にお込てＸとしてｚｆ：’ＩＫ用い、υとしてＨｕ
を用いることにある。,,? , the speed feedback signal from the tachogenete TG, that is, in this case, a signal that is actually detected with both position and speed feedback as hard. That is, put it on the right side of the above formula α force, use zf:'IK as X, and Hu as υ.
The purpose is to use

（５）位置帰還も、速度帰還もソフトによる場合。(5) When both position feedback and velocity feedback are performed by software.

式０２を第１８図のようなブロック図で記載することも
できる。ただしｓ７については、第１６図のところで示
したものと同じものである。Formula 02 can also be written in a block diagram as shown in FIG. However, s7 is the same as shown in FIG. 16.

ここの特徴は５２である。There are 52 features here.

Ｓマ：ソフトによる位置帰還信号 つまり、ここ！け位置、速度の両帰還をソフトによる。Sma: Position feedback signal by software I mean, here! Both position and speed feedback is done by software.

′またここでは弐０］に於いて初期値Ｖ。１．τ。は別
途穴める必要がある。'Also, here 20] is the initial value V. 1. τ. requires a separate hole.

あえて本方式（計算結果を帰還させる方式）を用いると
多少計算量が増加し、ソフトに負担がかかると共に開ル
ープの制御になり収れんしにくくなる。If this method (method of feeding back calculation results) is used, the amount of calculation will increase somewhat, putting a burden on the software, and will result in open-loop control, making it difficult to converge.

ところでツールは最大に６自由度を有することからして
前記マトリックスはる×６のマトリックスとなる。しか
し作業に応じて例えば第２図〜第５図に示すようにツー
ルの自由度。By the way, since the tool has a maximum of six degrees of freedom, the matrix is a x6 matrix. However, depending on the work, the degree of freedom of the tool can be adjusted as shown in FIGS. 2 to 5, for example.

寸法等が変わるのでそのツールに合わせて〔ｋ〕。Dimensions etc. will change, so select [k] according to the tool.

〔ＣＬ　ＣＭ〕　のマ）　ＩＪソックス値を実験等によ
ってめて次表のように制御装置１５の１？０ｋｆ３９に
ツールや作条内容に対応させて各データに記憶させてか
くのがよい、甘だツールの寸法はベクトル量に関係する
のでツールや作業内容に合せて上記ＲＯＭ６９の各テー
ブルに記憶させておく必要がある。[CL CM] Ma) It is a good idea to find the IJ sock values through experiments, etc., and store each data in the 1-0kf 39 of the control device 15 in accordance with the tool and cutting content as shown in the following table. Since the dimensions of the tool are related to the vector quantity, it is necessary to store them in each table of the ROM 69 according to the tool and work content.

特に第５図に示すＲＣＣ機措に対応させて〔ｋ〕。In particular, it corresponds to the RCC mechanism shown in Figure 5 [k].

［Ｃ〕、［Ｊｔ〕　等のマトリックスをめることが可能
である故、制御によってＲＣＣの機能を以上に説明１〜
たように本発明によれば、ロボットのｍＩノ御によって
フレキシビリティを持たせることが可能となシ１次のよ
うな作用効果を有する。Since it is possible to set matrices such as [C] and [Jt], the functions of RCC are explained above by control.
As described above, according to the present invention, it is possible to provide flexibility by controlling the mI of the robot, and the present invention has the following effects.

印　ツール代替機能 この具体的目的として、必要とする動作を行なうツール
を想定し、そのツールと同等の動作をロボットにさせる
ことができ、かつ想定するツールを変換できることを実
現することである。Mark Tool Substitution Function The specific purpose of this is to imagine a tool that performs the required action, make the robot perform the same action as that tool, and realize the ability to convert the assumed tool.

従来は、逐次ツールの交換をしていたが。Previously, tools were replaced one after another.

本発明によれば、このグ換を単なるソフト上のパラメー
タの交換でできるようになった。According to the present invention, it has become possible to perform this change simply by exchanging parameters on software.

また、同一のツールを用いではいるが、若干特性を変え
た動作をロボットにさせる時にも、従来ガらば、逐次ツ
ールの一部を調整しないとでき庁かったが１本発明によ
ればオンラインで、ツールの特性を必要に応じてソフト
の面だけで変換できるという便利な機能を持たせること
ができる。Additionally, when making a robot perform an action with slightly different characteristics using the same tool, in the past it was possible to do so without sequentially adjusting some of the tools, but with the present invention, it can be done online. This allows for the convenient function of converting the characteristics of a tool using just the software as needed.

特に本発明によればロボットの制御によってＲＣＣの機
能を達成することが出来る７■　シミュレーション機能 以上の如く一貫して１本発明をツール代替機能の面から
述べてきたが１本発明によればこの他にシミュレーショ
ン機能を持たせることができる。In particular, according to the present invention, the RCC function can be achieved by controlling the robot. It can also have other simulation functions.

これは、例えば、第４図のような機構（ツールとしてで
はない）を設計するに際し、実際に物を作る前にそのイ
μ描の挙ｒｆＩｂを見たい時に本発明の手法を適用して
、ロボットにその挙動を実際にさせることができる幼果
を奏する。For example, when designing a mechanism (not as a tool) like the one shown in Figure 4, you can apply the method of the present invention when you want to see the diagram of the mechanism rfIb before actually making it. It is a young fruit that can actually make a robot behave that way.

（５）　補償機能 上記のＯ′１ツール代替−印］シミーレーションの各機
能は、ロボットの粘度が高い程、その効果も高い。しか
し、現実には、ロボットはアーム部の弾性や減速山車の
剛性不足等による振動側の機械的な原因や寸だ電気回路
その他の原因で、不要な振動、遅れ１等が生じている場
合がある７ このような時に、ロボッ）・のこの振動、遅れ等を解析
して、予想できたならば、それを少くするための補償動
作を本発明によれば逆に加えることも可能となる。(5) Compensation Function The above O'1 Tool Alternative - mark] Each function of the simulation is more effective as the viscosity of the robot is higher. However, in reality, unnecessary vibrations and delays may occur in robots due to mechanical causes of vibration such as the elasticity of the arms and insufficient rigidity of the deceleration floats, as well as large electrical circuits and other causes. In such a case, if the robot's vibration, delay, etc. can be analyzed and predicted, according to the present invention, it is also possible to take a compensatory action to reduce it.

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

第１図は関節形ロボットの外観を示す図、第２図（ａｌ
　、　（ｂｌけ各ｈ１自由度機措を示す図、第３図は２
自由度機構を示す図、第４図はろ自由度機構を示す図、
第５図けＲＣＣ’の機構を示す図。 第６図は本発明のロボットの制御方式の一実施例を示す
棚略構成図、第７図は第６図に示す制御装置を具体的に
示した図、第８図は第６図に示す制御装置を機能的に示
（７た図、第９図けｆｌｉｌＪ御主要部の一実施例を演
算のフローで示１〜だ図。 第１０図（α１．　（ｈｌ＋ｄ糸件（α）におけるステ
ップ入力とロボットの手首の動きとを示した図、第１１
図（ａ）。 （ｈｌけ条件（ｈｌにおけるステップ入力とロボットの
手首の動きとを示した図、第１２図（Ｌ′Ｌｌ、　（ｈ
ｌけ条件（Ｃｌにおけろステップ入力とロボットの手首
の動へを示した図、第１５図はロボットの手心とツール
との間に設偽、された力センサを示した図、第１４図（
σ）は力センサの正面断面図−第１４図ＦＡＩは第１４
図（α１の底面図、第１５図（ａ、）〜（ｄｌけ各々力
センサの動きを示した図、第１６図は第９図と同様知制
御主要部の一実施例を演算のフローで示した図、第１７
図は第１６図と異なる他の実施例を演算のフローで示し
た図、第１８図は更に第１６図と異なる他の実施例を演
算のフローで示した図である。 １２・・・（関節形）ロボット １６・・・力センサ　１４・・・ハンド部１５・・・制
御装置　１６・・・ザーボアンプ１７・・ロボットの駆
動部　２ｏ・・・カ算出部２１・・・座標変換　２２・
・・制御主要部２３・・・座標変換 ２４・・・関節角度速度算出手段 １　１　図 ／１ 宅　２　喝 ら ′）ノ （））） ７〜′ 電　３　口 Ｉ、■ ）８ 第　４　図 １ ５５− −５　図 ら ／ ＼ Ｑ４、 皐　ｑ　図 ２２ 篤　１０　口 皐　１１　図 克　１２　日Figure 1 is a diagram showing the external appearance of the articulated robot, Figure 2 (al
(Figure 3 is a diagram showing the mechanism of each h1 degree of freedom.
A diagram showing a degree of freedom mechanism, FIG. 4 is a diagram showing a degree of freedom mechanism,
Figure 5 is a diagram showing the mechanism of RCC'. FIG. 6 is a schematic diagram of the shelf configuration showing one embodiment of the robot control system of the present invention, FIG. 7 is a diagram specifically showing the control device shown in FIG. 6, and FIG. 8 is the diagram shown in FIG. 6. The control device is functionally shown (Figure 7, Figure 9 shows an example of the main part of flilJ in the flow of calculations. Figure 10 (α1. Steps in (hl+d) Diagram showing input and robot wrist movement, No. 11
Figure (a). (hl condition (Figure 12 showing the step input in hl and the movement of the robot's wrist) (L'Ll, (h
Fig. 15 is a diagram showing the force sensor set up between the robot's hand center and the tool, Fig. 14 (
σ) is a front sectional view of the force sensor - Figure 14 FAI is the 14th
Figure (bottom view of α1, Figure 15 (a,) to (dl) are diagrams showing the movement of each force sensor, Figure 16 is the same as Figure 9, showing the calculation flow of an example of the main part of the intelligent control. Figure shown, No. 17
16 is a diagram showing a calculation flow of another embodiment different from FIG. 16, and FIG. 18 is a diagram showing a calculation flow of another embodiment different from FIG. 16. 12... (articulated) robot 16... Force sensor 14... Hand section 15... Control device 16... Servo amplifier 17... Drive section of robot 2o... Power calculation section 21... Coordinate transformation 22.
... Control main part 23 ... Coordinate transformation 24 ... Joint angular velocity calculation means 1 1 Fig. 1 55- -5 Figure 22 Atsushi 10 Kuchigo 11 Zukatsu 12th

Claims (1)

【特許請求の範囲】 １、　ロボットに加えられた力またはモーメント寸たは
その両者を検出する第１の検出手段と。 該第１の検出手段によって検出された力またはモーメン
トまたはその両者である第１の力信号と予め定められた
力またはモーメント寸たはその両者の指令となる指令力
信号との差を計算する第１の汁１算手段と、ロボットの
現在の位置または姿勢またはその両者を検出する第２の
検出手段と、該第２の検出手段によって検出された現在
の位置、または姿勢オたはその両者である第１の位置信
号と予め定められた位ｒｔｔたは姿勢捷たけその両者の
指令となる位置信号との差を計算する第２の計算手段と
、該第２の計算手段で計算された位置信号を線形変換し
て力信号を計算する第３の計算手段と、上記第１の計算
手段で計算された力信号と第３の計算手段で計算された
力信号との差を計算する第４のｉｔ　１．手段と、該第
４の言１算手段で引算されたカイへ号を線形変換して第
１の速度信号を計算する第５の計算手段と、前回の積分
効果信号を線形変換して第２の速度イハ号を計算する第
６の計ａ手段と、第５の計ａ１手段で計算される第１の
速度信号と第６の計算手段で引算される第２の速度信号
との差を次回の積分効果信号として引算する第７の計算
手段とを備えイ］け、第７の１９手段で計算される次回
の積分効果信号を速度指令とすることを特徴とするロボ
ットの制御方式。 ２、　上記第１〜第７の計算手段の計９処理を各サンプ
リングタイム＠Ｖｃ１゛テうことを特徴とする特許請求
の範囲第１項記載のロボットの制御方式。 ３、　第３の計算手段における線形変換を第１の対角行
列を掛けることによって行なうことを特徴とする特許請
求の範囲第１項または第２項記載のロボットのｉｌｉ制
御方式。 ４　第５の＝ｔａ手段における線形変換を第１のスカシ
と第２のスカシとの比を川けることによって行うことを
特徴とする特許請求の範囲第１項寸だに第２項才たけ第
ろ項記載のロボットの制御方式。 ５　第６の計算手段における？が形変換を、第１のスカ
シと第２のスカシとの比に第６の対角行列を掛けた値か
ら即位行列を減じた値となる第２の対角行列を掛Ｈるこ
とによって行うことを特徴とする特許請求の範囲第１項
寸たけ第２項またけ第６項才たけ第４項記載のロボット
の制御方式。[Claims] 1. A first detection means for detecting force or moment applied to the robot, or both. a first force signal that is a force or moment or both detected by the first detection means and a command force signal that is a command for a predetermined force or moment or both; a second detection means for detecting the current position and/or posture of the robot; and the current position and/or posture detected by the second detection means; a second calculation means for calculating the difference between a certain first position signal and a position signal serving as a command for either a predetermined position rtt or attitude maneuver; and a position calculated by the second calculation means. a third calculation means for calculating a force signal by linearly converting the signal; and a fourth calculation means for calculating the difference between the force signal calculated by the first calculation means and the force signal calculated by the third calculation means. It 1. a fifth calculation means for calculating a first speed signal by linearly converting the chi subtracted by the fourth calculation means; the difference between the first speed signal calculated by the fifth calculation a1 means and the second speed signal subtracted by the sixth calculation means; a seventh calculation means for subtracting the next integral effect signal from the next integral effect signal, and the next integral effect signal calculated by the seventh nineteen means is used as a speed command. . 2. The robot control system according to claim 1, wherein a total of nine processes of the first to seventh calculation means are carried out at each sampling time @Vc1. 3. The robot ili control system according to claim 1 or 2, wherein the linear transformation in the third calculation means is performed by multiplying by the first diagonal matrix. 4. The linear transformation in the fifth =ta means is performed by changing the ratio between the first scale and the second scale. The robot control method described in Section 3. 5 In the sixth calculation means? The shape transformation is performed by multiplying the ratio of the first and second scales by the second diagonal matrix, which is the value obtained by subtracting the coronation matrix from the value obtained by multiplying the sixth diagonal matrix. A control system for a robot as set forth in claim 1, paragraph 2, paragraph 6, and paragraph 4, characterized in that: