JPH079608B2 - Robot control method and apparatus - Google Patents

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、ロボットの制御方法及びその装置に係り、特
に多自由度に動作するロボットに種々の作業をさせる
際、好適な制御方法及びその装置に関するものである。Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot control method and apparatus, and particularly to a control method and apparatus suitable for causing a robot operating in multiple degrees of freedom to perform various tasks. It is about.

〔発明の背景〕[Background of the Invention]

近年、ロボットは様々な分野に広く導入されているが、
これはロボットが一般の専用機械と異なり汎用性に富
み、多種多様な作業に適用し得ることが大きな特徴の１
つであるからである。In recent years, robots have been widely introduced in various fields,
One of the major characteristics of this is that the robot is versatile and can be applied to a wide variety of tasks, unlike general dedicated machines.
Because it is one.

ロボットをある作業に適用する場合、そのままの形で使
用される例は少なく、取り扱う対象物に応じたツールを
ロボットに備え付けて使用するのが一般的である。When the robot is applied to a certain work, it is rarely used as it is, and it is common to equip the robot with a tool corresponding to an object to be handled.

例えば、ロボットを現場の加工又は組立ラインに導入し
作業させる場合、対象物の形や大きさ、あるいは作業の
内容に応じたツールをロボットの先端にあるハンド部や
アーム部とハンド部の間に設け、組立作業や塗装作業等
を行なわせている。この場合、ツール自体にある程度の
柔軟性を持たせて、対象物の精度上のバラツキや位置決
め誤差等作業不良の要因を吸収・排除することが行なわ
れている。For example, when introducing a robot into a processing or assembly line at a work site to perform work, a tool according to the shape and size of the object or the content of the work is provided between the hand part at the tip of the robot or the arm part and the hand part. It is installed, and assembly work and painting work are performed. In this case, the tool itself is made to have a certain degree of flexibility so as to absorb and eliminate factors such as variations in accuracy of the target object and work errors such as positioning errors.

しかしながら、作業内容が複雑かつ高度なものになるに
伴い、ツール自体も自由度が高く高機能を要求され、従
ってその構造も複雑で高価なものになる傾向にある。さ
らに１つのツール自体では対処し切れず、同じ作業でも
対象物の形状や作業条件により、ツール交換も必要とな
ることも少なくない。However, as the contents of work become complicated and sophisticated, the tools themselves are required to have a high degree of freedom and high functions, and therefore their structure tends to be complicated and expensive. Further, it is not uncommon for one tool itself to deal with it, and even in the same work, it is often necessary to replace the tool depending on the shape of the object and the working conditions.

この結果、ツール交換及びその調整に時間がかかりロボ
ットの稼動率を低下させるだけでなく、作業の信頼性を
保持するためにツールの保守・管理が生ずる等幾多の問
題点を派生する。As a result, not only is it time-consuming to change and adjust the tool, which reduces the operating rate of the robot, but also causes many problems such as maintenance and management of the tool in order to maintain the reliability of the work.

柔軟性を有するツールの例として、USP 4,098,001に開
示されているRemote Center Compliance（以下RCCとい
う）機構がある。このRCC機構は、把持された対象物の
先端に力やモーメントが加わると、平行移動や回転して
対処し、ロボットハンドを単に直線的に移行させるだけ
で、例えば軸状部品を挿入穴にはめ合わせることができ
るものである。しかし、このRCC機構も対象物である軸
状部品の長さやはめ合い条件の変化に対する柔軟性は充
分ではない。An example of a flexible tool is the Remote Center Compliance (RCC) mechanism disclosed in USP 4,098,001. With this RCC mechanism, if a force or moment is applied to the tip of the grasped object, it will move in parallel or rotate to handle it, and simply move the robot hand in a straight line. It can be combined. However, even this RCC mechanism is not sufficiently flexible with respect to changes in the length and fitting condition of the shaft-shaped part which is the object.

以上述べた様に、ロボットとは、従来の専用機械が単一
の部品、作業しか対象とできなかった点と異なり、多品
種の部品を扱い、異なる作業を同一の機械で可能である
点に、その大きな意義がある。このため、ロボットが品
種、作業が変わる度にツールの交換が必要であるとする
と、汎用性あるいはフレキシビリティの面からロボット
としての特徴を誇示しにくい。As described above, the robot is different from the conventional dedicated machine in that it can handle only a single part and work, but it handles a wide variety of parts, and different work can be done with the same machine. , Has a great significance. For this reason, if it is necessary to replace the tool every time the type and work of the robot change, it is difficult to show off the characteristics of the robot in terms of versatility or flexibility.

しかし、このようなツール交換或いはこれと同等の事を
現在のロボット技術では行なわざるを得ないのが実情で
あり、これに対処するために、ツールに汎用性を持たせ
ることが考えられるが、汎用性を持つツールとは、往々
にして機構が複雑であり、不必要な部分を含む場合が多
く、ツール交換を行なう場合より、むしろ無駄が多くな
ることがある。However, the current situation is that there is no choice but to carry out such a tool exchange or something equivalent to this with the current robot technology, and in order to deal with this, it is possible to make the tool versatile, A tool having general versatility often has a complicated mechanism and often includes an unnecessary part, which may be more wasteful than a case of exchanging tools.

このように、機構だけで解決しようとすると逆に、この
フレキシビリティという問題は、より難しくなるという
性質を持つ。In this way, on the contrary, if the problem is solved only by the mechanism, the problem of flexibility has a property that it becomes more difficult.

〔発明の目的〕[Object of the Invention]

本発明の目的は、種々の作業条件に対してツール交換等
の機構的によらず、制御上の特性パラメータを変更する
だけで対処し得るロボットの制御方法及びその装置を提
供するにある。It is an object of the present invention to provide a robot control method and its apparatus that can cope with various work conditions without changing mechanically such as tool exchange and the like by simply changing control characteristic parameters.

〔発明の概要〕[Outline of Invention]

本発明は、サンプリングタイム毎にロボットを制御する
ロボットの制御方法であって、前サンプリングタイム
（ｎ−１）におけるロボットのハンドに加えられた実際
の力を検知して力信号を算出し、この信号と前サンプリ
ングタイム（ｎ−１）における目標値であった力基準値
との差である第１の値を算出し、前記ハンドの前サンプ
リングタイム（ｎ−１）の実際の位置及び姿勢を表わす
位置信号と前サンプリングタイム（ｎ−１）における位
置及び姿勢の目標値であった位置基準値との差にバネ定
数を掛けた第２の値を算出し、前記ハンドの前サンプリ
ングタイム（ｎ−１）における速度信号としてロボット
に設けられた位置検出器からの位置検出値を微分した値
を用い、これに粘性定数を掛けた第３の値を算出し、前
記第1,第2,第３の値と前記ハンドの質量係数から前記ハ
ンドの次サンプリングタイム（ｎ）の指令速度を算出す
るようにしたものである。The present invention is a robot control method for controlling a robot at each sampling time, in which a force signal is calculated by detecting an actual force applied to a robot hand at a previous sampling time (n-1). The first value, which is the difference between the signal and the force reference value that was the target value at the previous sampling time (n-1), is calculated, and the actual position and posture of the hand at the previous sampling time (n-1) are calculated. A second value is calculated by multiplying the spring constant by the difference between the position signal represented and the position reference value which was the target value of the position and orientation at the pre-sampling time (n-1) to calculate the pre-sampling time (n The value obtained by differentiating the position detection value from the position detector provided in the robot is used as the velocity signal in -1), and the third value is calculated by multiplying this value by the viscosity constant, and the first, second, and With a value of 3 The command speed of the next sampling time (n) of the hand is calculated from the mass coefficient of the hand.

また、位置信号として、前記ロボットに設けられた位置
検出器からの位置検出値を使用するようにしたものであ
る。Further, a position detection value from a position detector provided in the robot is used as the position signal.

また、位置検出信号として、前記ロボットに設けられた
非接触センサからの位置情報を使用するようにしたもの
である。Further, position information from a non-contact sensor provided in the robot is used as the position detection signal.

さらに、ロボットの制御装置をロボットのハンドに加え
られえた力に基づいて力信号を算出する第１の算出手段
と、前記ロボットに設けられた位置検出器からの情報に
基づいて前記ハンドの位置及び姿勢を表わす位置信号を
算出する第２の算出手段と、前記力信号及び前記位置信
号に基づいて、前記ハンドの指令速度から前記ロボット
の関節角速度を算出する第４の算出手段から構成したも
のである。Further, the robot control device calculates a force signal based on a force applied to the robot hand, and a position of the hand based on information from a position detector provided in the robot. It is configured by second calculating means for calculating a position signal representing a posture, and fourth calculating means for calculating a joint angular velocity of the robot from a command velocity of the hand based on the force signal and the position signal. is there.

〔発明の実施例〕Example of Invention

以下、本発明を図に基づいて具体的に説明する。 Hereinafter, the present invention will be specifically described with reference to the drawings.

第１図は、関節形ロボットと制御系の全体構成の概略を
示す構成図である。ここで、制御対象のロボットの一例
として示された関節形ロボット10は、ベース上を垂直軸
まわりに旋回可能な旋回台11と、この旋回台11上に水平
軸を中心に回動可能に設けられた上腕12と、この上腕12
の先端部に位置する他の水平軸を中心に回動可能に設け
られた前腕13と、この前腕13の先端部に図示の如く３自
由度に動作する手首部14を有する計６自由度の関節形ロ
ボットである。この手首部14には取り扱う対象物を把持
するハンド15及びロボットに加えられる力を検出する力
センサ16を有する。FIG. 1 is a configuration diagram showing an outline of the overall configuration of an articulated robot and a control system. Here, the articulated robot 10 shown as an example of the robot to be controlled is provided with a swivel base 11 capable of swiveling on a base around a vertical axis, and provided on the swivel base 11 so as to be rotatable about a horizontal axis. Upper arm 12 and this upper arm 12
There are a total of 6 degrees of freedom including a forearm 13 rotatably provided around another horizontal axis located at the tip of the forearm, and a wrist 14 that operates in 3 degrees of freedom at the tip of the forearm 13 as shown in the figure. It is an articulated robot. The wrist portion 14 has a hand 15 for gripping an object to be handled and a force sensor 16 for detecting a force applied to the robot.

なお、前記旋回台11,上腕12,前腕13及び手首部14を各々
駆動するモータＭには、各々の回転角度を検出するため
のエンコーダPE及び回転速度を検出するためのタコジェ
ネレータTGが連結されている。An encoder PE for detecting each rotation angle and a tacho generator TG for detecting each rotation speed are connected to a motor M for driving each of the swivel base 11, the upper arm 12, the forearm 13, and the wrist portion 14. ing.

制御装置17はエンコーダPE及び力センサ16からそれぞれ
得られるロボットの位置信号及びロボットに加えられ
た力信号ｖを取り込み、予め入力された信号基準値
ｒ及び力基準値ｃとからロボットの を算出してサーボアンプ18に対して出力し、関節形ロボ
ット10を作動させる。The control device 17 takes in the position signal of the robot and the force signal v applied to the robot, which are respectively obtained from the encoder PE and the force sensor 16, and determines the robot's position from the signal reference value r and the force reference value c input in advance. Is calculated and output to the servo amplifier 18 to operate the articulated robot 10.

第２図は、この制御装置とロボットの駆動部の各構成要
素及びその関連を示する説明図である。制御装置17は各
種の演算及び制御を行なうマイクロプロセッサ19と、RA
M20と、ROM21と、定型の関数演算を行なう演算部22と、
操作卓23に接続するインターフェース回路（I/F）24
と、これらの各構成要素を接続するバスライン25を有
し、さらにバスライン25にはサーボアンプ18を介してロ
ボット駆動部26にあるモータＭに出力するため、角速度
のディジタル信号をアナログ信号に交換するD/A変換回
路27と、モータＭに連結されたエンコーダPEからの出力
を計数するカウンタ28,力センサ16からアンプ29を介し
て出力された力に関するアナログ信号をディジタル信号
に変換するためのA/D変換回路30とが接続されている。FIG. 2 is an explanatory diagram showing the components of the control device and the drive unit of the robot and their relationships. The controller 17 includes a microprocessor 19 for performing various calculations and controls, and an RA
M20, ROM21, operation unit 22 for performing routine function operation,
Interface circuit (I / F) 24 connected to console 23
And a bus line 25 for connecting each of these components. Further, the bus line 25 is output to the motor M in the robot drive unit 26 via the servo amplifier 18, so that the angular velocity digital signal is converted into an analog signal. A D / A conversion circuit 27 to be exchanged, a counter 28 for counting the output from the encoder PE connected to the motor M, and for converting an analog signal related to the force output from the force sensor 16 via the amplifier 29 into a digital signal. Is connected to the A / D conversion circuit 30.

ここで、位置基準値ｒ及び力基準値ｃは０でもよい
が、この場合関節形ロボット10はハンド15に与えられる
力ｖによってのみ動作することになる。即ち、受動的
な機構となる。Here, the position reference value r and the force reference value c may be 0, but in this case, the articulated robot 10 operates only by the force v given to the hand 15. That is, it becomes a passive mechanism.

一方、位置基準値ｒ及び力基準値ｃのみが与えられ
たときは、これらの基準値によって関節形ロボット10の
動きが定められる。On the other hand, when only the position reference value r and the force reference value c are given, the movement of the articulated robot 10 is determined by these reference values.

以上の構成により制御装置17は、次の処理動作を行な
う。With the above configuration, control device 17 performs the following processing operation.

第３図は制御装置17の処理動作を示す概略ブロック線図
である。FIG. 3 is a schematic block diagram showing the processing operation of the controller 17.

制御装置17は、位置・姿勢算出部40においてロボットの
各関節のエンコーダPEからの信号をカウンタ28で計数し
てロボットの位置信号を受け、この各関節の角度から
ハンド系の直交座標に座標変換して、ハンドの位置・
姿勢を算出する。実際には第２図におけるマイクロプロ
セッサ19からの指令で演算部22等が行なう。また、ロボ
ットに加わる外力ａを力センサ16が検出し、力センサ
16を構成している歪ゲージの電圧ｖとして、制御装置
17は、アンプ29及びA/D変換回路30を介して取り込む。
力算出部50は、v,を取り込み、まず歪ゲージの電圧
ｖを歪／力の変換マトリックス等により力に変換す
る。実際には、上記と同様第２図におけるマイクロプロ
セッサ19からの指令で演算部22等が行なう。The controller 17 receives the position signal of the robot by counting the signal from the encoder PE of each joint of the robot in the position / orientation calculation unit 40 by the counter 28, and converts the angle of each joint into the orthogonal coordinate of the hand system. Then, the position of the hand
Calculate the posture. Actually, the operation unit 22 and the like perform the command from the microprocessor 19 in FIG. In addition, the force sensor 16 detects the external force a applied to the robot,
Control device as the voltage v of the strain gauge that composes 16
17 is taken in via an amplifier 29 and an A / D conversion circuit 30.
The force calculation unit 50 takes in v, and first converts the strain gauge voltage v into a force by a strain / force conversion matrix or the like. Actually, as in the case described above, the arithmetic unit 22 and the like perform the command from the microprocessor 19 in FIG.

次に、ハンド15及びハンド15が把持する物の重量が大き
い場合には、ハンドの姿勢によって力センサ16に与える
値が異なる。このため、ハンド15の姿勢をより算出
し、これによる影響を取り除き、実際にロボットに加っ
た力ａを推定する。こうして推定された値がであ
り、制御主要部60に与えられる。制御主要部60は、この
算出された力及びロボットの位置と予め入力される
力及び位置の基準値c,ｒからハンド15が動作すべき
速度を算出する。この速度は、ハンドに固定された座標
系での値であるので、これを座標変換によってベースの
座標系での速度に変換されて出力される。Next, when the weight of the hand 15 and the object gripped by the hand 15 is large, the value given to the force sensor 16 differs depending on the posture of the hand. Therefore, the posture of the hand 15 is calculated more, the influence due to this is removed, and the force a actually applied to the robot is estimated. The value thus estimated is and is given to the control main part 60. The main control unit 60 calculates the speed at which the hand 15 should operate from the calculated force and the position of the robot and the force and position reference values c and r input in advance. Since this speed is a value in the coordinate system fixed to the hand, it is converted into a speed in the base coordinate system by coordinate conversion and is output.

この後、直交座標系での速度から関節角速度算出部70
によってロボット各軸が取るべき が出力される。ただし、 （ここにJ^-1は逆ヤコビアン行列と呼ばれ、その要素は
ロボットの位置信号の関数である。）この はD/A変換回路27でアナログ信号に変換され、サーボア
ンプ18を介してロボットの各関節用のモータＭに出力さ
れ、ロボットを動作させる。After this, the joint angular velocity calculation unit 70 calculates from the velocity in the Cartesian coordinate system.
Each axis of the robot should take Is output. However, (Here J ^-1 is called the inverse Jacobian matrix, whose elements are functions of the robot's position signal.) Is converted into an analog signal by the D / A conversion circuit 27 and output to the motor M for each joint of the robot via the servo amplifier 18 to operate the robot.

以上、制御装置17の動作を述べたが、このうち制御主要
部60の処理方法が主に本発明の特徴を成すもので、以下
にその原理的な説明を行なう。The operation of the control device 17 has been described above. Of these, the processing method of the control main part 60 mainly constitutes the feature of the present invention, and the principle thereof will be described below.

一般に、柔軟性を有するツールの動きは慣性力、バネに
よる復元力ダンパによる粘性力と加えられる力によって
関係づけられる。ここでは説明を簡単にするため１自由
度について述べる。In general, the movement of a flexible tool is related to the inertial force, the restoring force of a spring, the viscous force of a damper, and the applied force. Here, one degree of freedom will be described to simplify the description.

１自由度のツールのモデルを数式で示すと次の（１）式
の如くとなる。When the model of the tool having one degree of freedom is expressed by a mathematical expression, the following expression (1) is obtained.

ここで、ｍはハンドの質量,Kはバネのバネ係数,Cはダン
パの粘性係数であり、xr,Fcはそれぞれ位置と力の基準
値である。 Here, m is the mass of the hand, K is the spring coefficient of the spring, C is the viscosity coefficient of the damper, and xr and Fc are the reference values of the position and force, respectively.

（１）式は（Ｆ−Fc）なる力がツールに加えられたとき
のツールの位置及び速度がそれぞれｘ及びｖとなること
を示す。言い換えると、もしロボットのハンドに（Ｆ−
Fc）なる力を加えたとき、ロボットのハンドがｘ及びｖ
なる位置、及び速度を持ち、ロボットのハンドは実際に
はツールを装着していないにも拘らず、ツールを装着し
たときと同様の運動をさせることができることを示すも
のである。Equation (1) shows that the position and velocity of the tool when the force (F-Fc) is applied to the tool are x and v, respectively. In other words, if the robot hand (F-
When the force Fc) is applied, the robot's hand is x and v
It shows that the robot hand can perform the same movement as when the tool is attached, though it has a position and a velocity as follows, although the robot hand is not actually attached.

即ち、ロボットに装着させたいツールを想定してロボッ
トを制御させることによって、そのツールをロボットに
組込んだときにするであろうハンドの動きをロボットに
させることができるもので、このことが本発明の大きな
特徴の１つである。In other words, by controlling the robot assuming the tool you want to attach to the robot, you can make the robot move the hand that you would do when you incorporated the tool into the robot. This is one of the major features of the invention.

実際にロボットの各関節を駆動させるモータＭを動作さ
せるときには、速度あるいは速度に相当する指令値をサ
ーボアンプに与えるため、速度ｖを計算手段で算出する
ことが有用であるから、上記（１）式は次の（２）式の
ように変形される。When actually operating the motor M that drives each joint of the robot, it is useful to calculate the speed v by the calculation means because the speed or a command value corresponding to the speed is given to the servo amplifier. The equation is transformed into the following equation (2).

更に、上記（２）式を離散値系で示すと、次の（３）式
及び（４）式となる。 Furthermore, when the above equation (2) is represented by a discrete value system, the following equations (3) and (4) are obtained.

ここで、Δｔはサンプリング時間、サフィックスｉまた
はｎはｉまたはｎ番目の値である。 Here, Δt is the sampling time, and the suffix i or n is the i or n-th value.

上記の式から明らかなように、本発明による処理方法に
よれば、速度の計算に微分処理が含まれず、ノイズに対
して強く、誤動作がなくなり、ロボットのハンドに安定
した動きをさせることができる。As is clear from the above formula, according to the processing method of the present invention, the differential calculation is not included in the calculation of speed, it is resistant to noise, malfunction does not occur, and the robot hand can move stably. .

更に、離散値系に変形した上記（４）式の右辺にｖ
_ｎ−１（ｎの１つ前のｖ）があり、左辺にｖ_ｎがある。
つまり、ｎ番目のロボットのハンドの速度ｖを計算手段
で算出するために、１つ前の速度ｖさえあればよく、毎
回タコジェネTJ等から実際の速度ｖを検出し、フィード
バックする必要が無い。これは実際にロボットの駆動系
に取付けてあるタコジェネTG等の速度検出器を用いてロ
ボットのハンドの速度を検出することは、同じようにロ
ボットの駆動系に取付けてあるエンコーダPE等の位置検
出器で検出されるロボットのハンドの位置の検出に比較
して、信号の信頼度がやや低いことを考慮すると有用な
特徴である。Further, v is added to the right side of the above equation (4) transformed into a discrete value system.
_{There is n-1} (v before n), and v _n is on the left side.
That is, in order to calculate the speed v of the hand of the n-th robot by the calculation means, only the previous speed v is required, and it is not necessary to detect and feed back the actual speed v from the tachogenerator TJ etc. every time. This is to detect the speed of the robot's hand using a speed detector such as a tachogenerator TG actually attached to the drive system of the robot, to detect the position of the encoder PE etc. also attached to the drive system of the robot. This is a useful feature considering that the reliability of the signal is a little lower than the detection of the position of the robot hand detected by the instrument.

なお、（４）式において、位置ｘに関するものとしてｘ
_ｎ−１が唯１つしか無い事からも解かるように、ｘ位置
は毎回検出する必要があるが、この位置ｘは、エンコー
ダPEからのロボットの位置信号（関節角度）から算出
される。In the equation (4), x is related to the position x.
_As can be understood from the fact that there is only one _n-1 , the x position needs to be detected every time, but this position x is calculated from the position signal (joint angle) of the robot from the encoder PE.

ここで、m,k,Cはツールの特性を想定する際に決める値
である。ツールを想定したならば、そのツールの機構を
解析し、ロボットに動作させる前にこれらの値をオフラ
インで決定しておき、この値をそのツールに固有のデー
タ（定数であっても変数であっても良い。）として、操
作卓23から入力して制御装置17のRAM20にストックして
おく。つまり、想定したツールが複数であれば、それに
応じてストックすべきデータも複数となる。Here, m, k, and C are values determined when assuming the characteristics of the tool. Assuming a tool, analyze the mechanism of the tool and determine these values off-line before operating the robot, and use these values as data specific to the tool (even if it is a constant May be input from the console 23 and stored in the RAM 20 of the control device 17. In other words, if there are multiple assumed tools, there will be multiple data to be stocked accordingly.

また、上記（１）式で示される系が安定する条件は、常
微分方程式に関する基礎理論で既知である。それに基い
てm,c,kを定めれば安定させたり、発散させたりするこ
とも可能である。Further, the condition for stabilizing the system represented by the above formula (1) is known in the basic theory regarding ordinary differential equations. It is possible to stabilize or diverge by setting m, c, and k based on it.

これと同様に、上記（２）式を離散値系に変換した上記
（４）式で安定させるためには、上記の条件の他にΔｔ
の値も十分小さくする必要がある。Similarly, in order to stabilize Equation (2) above by Equation (4) converted to a discrete value system, in addition to the above conditions, Δt
The value of must also be sufficiently small.

ただし、Δｔはサンプリングタイムであるから、正値を
取り、ｍは質量に相当する値であるから、正値を取る。
また、安定させる目的で用いるならばＣは正値とする。 However, since Δt is the sampling time, it takes a positive value, and since m is a value corresponding to the mass, it takes a positive value.
If used for the purpose of stabilization, C has a positive value.

以上まで所で、本発明により、ツールの動作特性をロボ
ットにさせる方法について述べたが、これは主に前記m,
C,kの値に基づく方法に関するものである。In the above, according to the present invention, the method of making the operation characteristic of the tool to the robot has been described.
It relates to a method based on the values of C, k.

本発明は、上記の方法以外にも別の変数であるロボット
に加えられる力Ｆ、この力の基準値Fc,ツール位置の基
準値xrに基づいてツールの特作特性を定めることができ
る特徴がある。In addition to the above method, the present invention is characterized in that the special characteristic of the tool can be determined based on the force F applied to the robot, which is another variable, the reference value Fc of this force, and the reference value xr of the tool position. is there.

即ち、このF,Fc,xrの３値を指令値として用いた場合、
多くの組合せが考えられ、ツールに様々な動作特性を持
たせることができる。ここでは具体例として、以下の動
作例について述べる。That is, when the three values of F, Fc, and xr are used as command values,
Many combinations are possible and the tools can have different operating characteristics. Here, the following operation example will be described as a specific example.

第４図は第１の動作例を示すもので、その条件は4mk−C
²＞0,C＞0,m＞0,xr＝0,Fc＝０であり、Ｆが第４図に示
す如くステップ入力Fs27を取ったとき、位置ｘは時間ｔ
に対して次の（６）式の関係となる。Fig. 4 shows the first operation example, and the condition is 4mk-C.
² > 0, C> 0, m> 0, xr = 0, Fc = 0, and when F takes the step input Fs27 as shown in FIG. 4, position x is time t
In contrast, the relationship of the following expression (6) is established.

ただしｘ_ｔ＝０＝0,v_ｔ＝０＝０とする。However, it is assumed that x _{t = 0} = 0 and v _{t = 0} = 0.

この場合のロボットの動作は第４図に示す出力28とな
る。Fsステップ入力27の大きさに比例した値に位置が収
束し、その時の立上り時間や振動については、m,C,kで
決まる。 The operation of the robot in this case is output 28 shown in FIG. The position converges to a value proportional to the size of the Fs step input 27, and the rising time and vibration at that time are determined by m, C, and k.

第５図は、第２の動作例を示すものでその条件は4mk−C
²＞0,C＞0,m＞0,F＝0,Fc＝０であり、xrに第５図に示す
ようにステップ入力ｘ_rs29を与えたとき、位置ｘは時間
ｔに対して次の（７）式の関係となる。FIG. 5 shows the second operation example, and the condition is 4 mk-C.
² > 0, C> 0, m> 0, F = 0, Fc = 0, and when xr is given a step input x _rs 29 as shown in FIG. Equation (7) is obtained.

ただし、xt＝_０＝0,vt＝_０＝０とする。However, it is assumed that xt = ₀ and vt = ₀ .

この場合のロボットの動作は第５図に示す出力30とな
る。 The operation of the robot in this case is the output 30 shown in FIG.

これら第４図に示す出力28、第５図に示す出力30からわ
かるように、またこれらの出力と前記（１）式から推定
できるように、ある条件の下ではステップ入力をＦに与
えても、xrに与えても同等な出力が得られる。As can be seen from the output 28 shown in FIG. 4 and the output 30 shown in FIG. 5, and as can be estimated from these outputs and the equation (1), even if a step input is given to F under certain conditions. , Xr gives the same output.

即ち、同じ出力を得るのに変化させる値をＦにしたり、
xrにしたり、選択することができ、必要に応じて多種多
様の制御が可能となる。That is, change the value to F to get the same output,
It can be set to xr or selected, and various types of control are possible as required.

第６図は、第３の動作例を示すもので、その条件はｋ＝
0,m＞0,C＞0,xr＝0,Fc＝０であり、第６図に示す如くＦ
にステップ入力Fs31を与えたとき速度ｖは時間ｔに対し
て次の（８）式の関係となる。FIG. 6 shows a third operation example, the condition of which is k =
0, m> 0, C> 0, xr = 0, Fc = 0, and F as shown in FIG.
When the step input Fs31 is applied to the speed v, the speed v has the relationship of the following expression (8) with respect to the time t.

ただしｘ_ｔ＝０＝0,v_ｔ＝０＝０とする。However, it is assumed that x _{t = 0} = 0 and v _{t = 0} = 0.

この場合、ロボットのハンドの速度は第６図に示す出力
32のようになり、上記の第1,第２の動作例と異なり、速
度が収束する。つまり、力によってロボットのハンドの
速度を制御することができる。 In this case, the speed of the robot hand is the output shown in FIG.
As shown in 32, the speed converges unlike the first and second operation examples described above. That is, the force can control the speed of the robot hand.

上記の如く簡単な３つの動作例を示したが、本発明はこ
の他にも有用な多数の制御を実行できる可能性を有す
る。Although three simple operation examples have been shown as described above, the present invention has a possibility of executing a number of other useful controls.

以上、１自由度のツールを想定し、ロボットを制御する
場合について、その原理や動作例について述べた。The principle and operation example of the case of controlling a robot assuming a tool having one degree of freedom have been described above.

しかし、実際にはツールが１自由度である場合は少な
く、多自由度のツールを使用されることが多い。However, in reality, the tool is rarely provided with one degree of freedom, and a tool with multiple degrees of freedom is often used.

多自由度の場合は、ベクトルを用いて式（１）を一般化
することにより、次の式を得ることができる。In the case of multiple degrees of freedom, the following equation can be obtained by generalizing the equation (1) using a vector.

ただし：位置ベクトル（直交座標系でのロボットの並
進位置及び回転角度） ：速度ベクトル（直交座標系でのロボットの並進速度
及び回転角度） ：力センサ等から検出される力信号（並進力と回転力
を含む）のベクトル r:力基準値のベクトル c:位置基準値のベクトル 〔Ｍ〕：ツールの仮想質量を示すマトリックス 〔Ｃ〕：ツールの仮想粘性係数を示すマトリックス 〔Ｋ〕：ツールのバネ定係数を示すマトリックス である。ここで、〔Ｍ〕，〔Ｋ〕，〔Ｃ〕は一般には対
角行列になる。 Where: Position vector (translation position and rotation angle of robot in Cartesian coordinate system): Velocity vector (translation speed and rotation angle of robot in Cartesian coordinate system): Force signal detected by force sensor (translation force and rotation) (Including force) r: Vector of force reference value c: Vector of position reference value [M]: Matrix showing virtual mass of tool [C]: Matrix showing virtual viscosity coefficient of tool [K]: Spring of tool This is a matrix showing constant coefficients. Here, [M], [K], and [C] are generally diagonal matrices.

式（９）を変形すると式（10）となる。The formula (9) is transformed into the formula (10).

＝〔Ｍ〕^-1∫〔−ｒ）−（〔Ｃ〕＋〔Ｋ〕（
−ｒ））〕dt …（10） ここで式（10）による計算は、実際には計算機では次の
如く離散値係によって行なわれる。= [M] ^-1 ∫ [-r)-([C] + [K] (
-R))] dt (10) Here, the calculation by the equation (10) is actually carried out by a discrete value calculator in a computer as follows.

ただしｎ＝1,2…… 式（11），（12）に於いて Δt:サンプリング時間を示すスカラ 〔Ｉ〕：単位マトリックス なお、式（12）において初期値_０は別途定める必要が
あり、また多自由度の場合はロボットの自由度と同じ
か、または、それより少ない自由度を持つ、ツールを想
定することが必要である。 However, n = 1,2 In formulas (11) and (12), Δt is a scalar indicating the sampling time [I]: unit matrix Note that the initial value ₀ in formula (12) must be determined separately, and In the case of multiple degrees of freedom, it is necessary to assume a tool with the same or lesser degrees of freedom as the robot.

以上、多自由度の場合についても述べたが、１自由度の
説明において触れたように、速度を計算する上で必要な
位置情報や速度情報をハード的に検出器から得る方法と
ソフト的に計算結果の値をその都度使用していく方法が
あり、それぞれ様々な組合せが考えられる。以下にその
例を示す。The case of multiple degrees of freedom has been described above. However, as mentioned in the explanation of one degree of freedom, the method of obtaining the position information and the speed information necessary for calculating the speed from the detector by hardware and the software There is a method of using the value of the calculation result each time, and various combinations are possible. An example is shown below.

第７図は位置帰還をエンコーダPEからの位置情報を用
い、速度換還を演算結果の値を用いる第１の方法を示
すブロック線図である。FIG. 7 is a block diagram showing a first method of using position information from the encoder PE for position feedback and using a value of a calculation result for speed conversion.

つまり、式（10）の右辺に於いてはエンコーダによっ
て検出された値を用い、また式（10）の右辺において、
はタコジェネレータTGによって検出された値ではな
く、演算手段によって算出された値をそのまま用いる
方法である。In other words, the value detected by the encoder is used on the right side of equation (10), and on the right side of equation (10),
Is a method of directly using the value calculated by the calculating means, not the value detected by the tachogenerator TG.

ここで、から への変換は〔Ｊ〕^-1なる行列（〔Ｊ〕はヤコビアン行列
と呼ばれる。）を用いて行なわれ、からへの変換は
ロボットの関節座標系から、直交座標系への座標変換に
よって行なわれる。Where from Is converted using a matrix [J] ^-1 ([J] is called a Jacobian matrix), and conversion from to is performed by coordinate conversion from the robot joint coordinate system to the orthogonal coordinate system. .

次に、第８図は位置帰還も、速度帰還も検出器からの情
報を用いる第２の方法を示すブロック線図である。Next, FIG. 8 is a block diagram showing a second method that uses information from the detector for both position feedback and velocity feedback.

ここで、離散値系での値ｎは例えば次式で求まる。Here, the value n in the discrete value system is obtained by the following equation, for example.

ｎ＝Δｔｎ−１＋ｘ_ｎ−１ …………（13） ただしｎ＝1,2,…… 更に第９図は、位置帰還も、速度帰還も計算結果の値を
用いる第３の方法を示すブロック線図である。n = Δtn-1 + x _n -1 (13) However, n = 1,2, ... Further, FIG. 9 is a block showing a third method in which the position feedback and the velocity feedback use the calculated values. It is a diagram.

つまり、この方法は位置、速度の両帰還を計算結果
の値を用いて帰還させるものである。In other words, this method is to feed back both position and velocity feedback using the value of the calculation result.

なお、ここでは式（12）に於いて初期値₀,_０は別途
定める必要がある。In this case, the initial values ₀ and ₀ in the equation (12) need to be determined separately.

以上、３つの方法について述べたが、各々次のような特
徴を持つ。The three methods have been described above, but each has the following characteristics.

即ち、帰還の目的からすると上記第２の方法（第８図の
方法）の如く、位置も速度も実際の値を検出して帰還さ
せるのがよい。しかし現在の技術レベルから判断すると
ロボットに取付けられた位置の検出器PEはかなり良い精
度を持つが、速度の検出器TGはそれに比べて劣る。この
ため、両者を折衷した第１の方法（第７図の方法）が現
実的に最も優れている。That is, for the purpose of feedback, it is preferable to detect the actual values of both the position and the velocity, and return the feedback, as in the second method (the method of FIG. 8). However, judging from the current technical level, the position detector PE attached to the robot has a fairly good accuracy, but the speed detector TG is inferior to that. For this reason, the first method (the method of FIG. 7) that is an eclectic combination of both is actually the best.

また第３の方法（第９図の方法）は、計算量が増加する
ものの、全く検出器を必要としない点に特徴を有する。The third method (the method shown in FIG. 9) is characterized in that it requires no detector at all, although the amount of calculation increases.

なお、ツールは最大に６自由度を有することからして前
記マトリックスは６×６のマトリックスとなる。しか
し、作業に応じてツールの自由度、寸法等が変わるので
そのツールに合わせて〔Ｋ〕，〔Ｃ〕，〔Ｍ〕^-1のマト
リックスの値を実験等によって求めておき、制御装置17
のROM21にツールや作業内容に対応させて各データに記
憶させておくのがよい。Since the tool has maximum 6 degrees of freedom, the matrix is a 6 × 6 matrix. However, since the degree of freedom, size, etc. of the tool change depending on the work, the matrix values of [K], [C], and [M] ⁻¹ are found by experiments according to the tool, and the controller 17
It is advisable to store each data in the ROM 21 in correspondence with tools and work contents.

また、ツールの寸法はベクトル量に関係するのでツール
や作業内容に合せてROM21の各テーブルに記憶させてお
く必要がある。Further, since the tool size is related to the vector amount, it is necessary to store it in each table of the ROM 21 according to the tool and the work content.

以上に説明したように本発明は、ロボットの制御によっ
てツールに様々なフレキシビリティを持たせ得ることが
大きな特徴である。As described above, the present invention is characterized in that the tool can have various flexibility by controlling the robot.

次に、本発明における角速度の算出過程をさらに具体的
に説明するが、その前にモデルとなるロボットの構造，
設定する各座標系及び記号について述べる。Next, the calculation process of the angular velocity in the present invention will be described more specifically. Before that, the structure of the model robot,
Each coordinate system and symbol to be set will be described.

第10図に示すように、ロボットの構造として剛体の空間
内の位置決めには通常６要素が必要であり、またその据
付面積に比べ可動範囲が広いことから６自由度の関節形
ロボットのものとした。この関節形ロボットは、各関節
の回転角度θ_ｉ（ｉ＝１〜６）及び各アーム長さd₁,
a₁₂,a₂₃,a₃₄,d₅（）で表わされ、次に述べる各座標系
が設定されている。まず、ロボットが据付けられた場所
に固定のベース座標系（以下Ｂ系という）、ロボットの
先端部に設けられたハンドに固定のハンド座標系（以下
Ｈ系という）、センサ部に固定のセンサ座標系（以下Ｓ
系という）、運動の回転モーメントの基準点と原点と
し、動作条件を設定したり、指令速度を算出したりする
補助座標系（以下Ａ系という）、バネの平衡位置・姿勢
により定まり、上記のＨ系に対応するリファレンス座標
系（以下RH系という）及びバネの平衡位置・姿勢により
定まり、上記Ａ系に対応するリファレンス座標系（以下
RA系という）の各座標系がある。As shown in Fig. 10, the robot structure usually requires 6 elements for positioning in the space of the rigid body, and since the movable range is wider than the installation area, it is assumed that the robot has 6 degrees of freedom. did. This articulated robot has a rotation angle θ _i (i = 1 to 6) of each joint and each arm length d ₁ ,
It is represented by a ₁₂ , a ₂₃ , a ₃₄ , d ₅ (), and each coordinate system described below is set. First, a base coordinate system fixed to the place where the robot is installed (hereinafter referred to as B system), a hand coordinate system fixed to the hand provided at the tip of the robot (hereinafter referred to as H system), and sensor coordinates fixed to the sensor unit. System (hereinafter S
System), an auxiliary coordinate system (hereinafter referred to as system A) that sets the operating conditions and calculates the command speed by using the reference point and the origin of the rotational moment of motion, and is determined by the equilibrium position / posture of the spring. It is determined by the reference coordinate system corresponding to the H system (hereinafter referred to as the RH system) and the equilibrium position / posture of the spring.
There is each coordinate system of RA system).

さらにハンドの位置及び姿勢を表わすため次の４個の３
次元ベクトルを定める。Furthermore, in order to show the position and posture of the hand, the following four 3
Determine the dimension vector.

:B系で表示したＨ系の原点 :B系で表示したＨ系のｘ軸方向の単位ベクトル :B系で表示したＨ系のｙ軸方向の単位ベクトル :B系で表示したＨ系のＺ軸方向の単位ベクトル 以上のロボットの構造，座標系及び記号を用いて、本発
明による の算出方法の一実施例を具体的に述べる。: Origin of H system displayed in B system: Unit vector in x axis direction of H system displayed in B system: Unit vector in y axis direction of H system displayed in B system: Z of H system displayed in B system Axial unit vector According to the present invention, the robot structure, coordinate system and symbols above are used. An example of the calculation method of is specifically described.

第11図は、第３図のブロック線図をさらに詳細に示した
ものである。前述した様に、制御装置17は、位置・姿勢
算出部40、力算出部50、制御主要部60及び関節角速度算
出部70の４つの主なブロックに分けられる。FIG. 11 shows the block diagram of FIG. 3 in more detail. As described above, the control device 17 is divided into four main blocks of the position / orientation calculation unit 40, the force calculation unit 50, the main control unit 60, and the joint angular velocity calculation unit 70.

力算出部50は、さらに変換部51、座標変換部52、重力分
算出部53及び座標変換部54から構成される。ロボットの
ハンドに外力ａが加えられると、ロボットに設けられ
た力センサ16の感知部例えば歪ゲージはこれを感知し、
加えられた力及びモーメントを電圧としてアンプ29及
びA/D変換回路30を介して変換部51に出力する。変換部5
1は送られてきた電圧に電圧／力変換マトリックス
〔Ａ〕^＋を掛け合わせ、まずＳ系における力ｓを算出
する。この変換マトリックス〔Ａ〕^＋は、予め力センサ
16に各軸方向に基準荷重を与えてキャリブレーションを
行ない、＝〔Ａ〕ｓの関係を有するマトリックス
〔Ａ〕を求め、〔Ａ〕^＋＝（〔Ａ〕^Ｔ〔Ａ〕）^-1〔Ａ〕
^Ｔの式から得られるものである。The force calculation unit 50 further includes a conversion unit 51, a coordinate conversion unit 52, a gravity component calculation unit 53, and a coordinate conversion unit 54. When an external force a is applied to the hand of the robot, the sensing part of the force sensor 16 provided in the robot, for example, the strain gauge, senses this.
The applied force and moment are output as voltage to the conversion unit 51 via the amplifier 29 and the A / D conversion circuit 30. Converter 5
1 is to multiply the transmitted voltage by the voltage / force conversion matrix [A] ⁺ , and first calculate the force s in the S system. This conversion matrix [A] ⁺ is the force sensor
Calibration is performed by applying standard loads to 16 in each axial direction to obtain a matrix [A] having a relation of = [A] s, and [A] ⁺ = ([A] ^T [A]) ^-1 [A ]
It is obtained from the equation of ^T.

この様に変換マトリックス〔Ａ〕^＋は予め算出されて記
憶され、制御演算中は定数として扱われるものであり、
１つの力センサは１つの変換マトリックス〔Ａ〕^＋に対
応し、他の力センサに変える場合は、その力センサに対
応した変換マトリックス〔Ａ〕^＋を求め、対処すること
ができる。この算出された力ｓはＳ系における値であ
るので、これをＨ系に座標変換するため次の座標変換部
52に出力される。In this way, the conversion matrix [A] ⁺ is calculated and stored in advance, and is treated as a constant during the control calculation.
One force sensor corresponds to one conversion matrix [A] ⁺ , and when changing to another force sensor, the conversion matrix [A] ⁺ corresponding to that force sensor can be obtained and dealt with. Since the calculated force s is a value in the S system, the following coordinate conversion unit is used to coordinate-convert it into the H system.
It is output to 52.

即ち、座標変換部52は、Ｈ系におけるＳ系の原点位置及
び姿勢（各軸の単位ベクトル） をパラメータとして用い、Ｓ系の力ｓをＨ系の力_Ｈ
に変換する。この変換された力_Ｈは次の補正部54に出
力される。That is, the coordinate conversion unit 52 determines the origin position and orientation of the S system in the H system (unit vector of each axis). Is used as a parameter, the force s of the S system is the force _H of the H system
Convert to. The converted force _H is output to the next correction unit 54.

一方、位置・姿勢算出部40において後述する様に算出さ
れたＢ系におけるハンドの位置・姿勢，（，，
）から重力分算出部53はハンド及びハンドが把持する
対象物の重量Ｗと重心位置をパラメータとして、ハン
ドや対象物自体の自重によって力センサ16に影響を与え
ている力、即ちハンドや対象物の重力分の力ｇを算出
し、補正部54に出力する。補正部54は、このｇと前述
の_Ｈからハンド及び対象物の重力による影響を取り除
いて、真にハンドに加わった力を、＝_Ｈ−_ｇの
式より求める。この算出された補正後の力はＨ系であ
り、速度算出の行なわれるＡ系に変換する必要があるの
で、座標変換部55において、Ｈ系におけるＡ系の原点位
置及び姿勢（各軸の単位ベクトル） をパラメータとして補正後の力はＡ系に座標変換さ
れ、次に制御主要部60に出力される。On the other hand, the position / orientation of the hand in the B system calculated by the position / orientation calculation unit 40 as described later, (,,
), The gravity component calculation unit 53 uses the weight W of the hand and the object gripped by the hand and the position of the center of gravity as parameters, and the force exerted on the force sensor 16 by the own weight of the hand and the object itself, that is, the hand and the object. The force g corresponding to the gravity is calculated and output to the correction unit 54. The correction unit 54 removes the influence of the gravity of the hand and the object from this g and the above-mentioned _H , and obtains the force actually applied to the hand from the formula = _H - _g . The calculated force after correction is in the H system and needs to be converted into the A system in which the speed is calculated. Therefore, in the coordinate conversion unit 55, the origin position and posture of the A system in the H system (unit of each axis). vector) The corrected force is coordinate-converted to the A system by using as a parameter, and then output to the main control unit 60.

なお、ハンドの動作制御の態様により速度算出をＡ系で
行なう必要はなくＨ系の場合は、当然にこの座標変換部
55における座標変換は不用である。Note that it is not necessary to perform speed calculation in the A system depending on the mode of operation control of the hand.
Coordinate transformation at 55 is unnecessary.

位置・姿勢算出部40は、演算部41、Ａ系の位置・姿勢算
出部42、RA系の位置・姿勢算出部43、Ａ系とRA系の位置
・姿勢偏差算出部44及び座標変換部45から構成される。The position / orientation calculation unit 40 includes a calculation unit 41, an A system position / orientation calculation unit 42, an RA system position / orientation calculation unit 43, an A system and RA system position / orientation deviation calculation unit 44, and a coordinate conversion unit 45. Composed of.

まず、演算部41は、モータＭに連結されたエンコーダPE
の位置情報からカウンタ28によって得られた位置信号
に基づいて、ロボットの各アーム長さとアーム先端部
に対するハンドの位置・姿勢_Ｅ，（_Ｅ，_Ｅ，
_Ｅ）をパラメータとしてＢ系におけるハンドの位置・
姿勢，（，，）を算出する、即ち の各式からそれぞれ，，，が求まる。First, the calculation unit 41 determines the encoder PE connected to the motor M.
Based on the position signal obtained by the counter 28 from the position information of the robot, each arm length of the robot and the position / posture _E , ( _E , _E , of the hand with respect to the arm tip portion)
Position of hand in B system with _E ) as a parameter
Calculate the posture, (,,), that is, From each expression of ,,,, are obtained respectively.

ここで、〔Ａ_1h〕は第１の座標系からＨ座標系への変換
行列を示し、，，_Ｅ，_Ｅ，_Ｅ，_Ｅの関数で
ある。算出されたハンドの位置・姿勢，（，，
）は前述した力算出部50内の重力分算出部53と後述す
る角速度演算部70とＡ系の位置・姿勢算出部42に出力さ
れる。RA系の位置・姿勢算出部43では、Ｈ系（ハンド）
に対応する並進バネ及び回転バネの平衡位置・姿勢r
h,（rh,rh,rh）かから を用いてＡ系に対応する。並進バネ及び回転バネの平衡
位置・姿勢ra,（ra,ra,ra）を算出する。Here, [A _1h ] represents a transformation matrix from the first coordinate system to the H coordinate system, and is a function of ,, _E , _E , _E , _E. Calculated hand position / orientation, (,,
) Is output to the gravity component calculator 53 in the force calculator 50, the angular velocity calculator 70, which will be described later, and the position / orientation calculator 42 of the A system. In the RA system position / orientation calculation unit 43, the H system (hand)
Equilibrium position / posture r of translation and rotation springs corresponding to
From h, (rh, rh, rh) Is used to correspond to system A. Calculate the equilibrium position / posture ra, (ra, ra, ra) of the translation spring and the rotation spring.

さらに、Ａ系とRA系の位置・姿勢偏差算出部44ではr
a,（ra,ra,）とa,（a,a,ａ）から位置の
偏差Δ及び姿勢の偏差，を次式により算出する。Furthermore, in the position / orientation deviation calculation unit 44 of the A system and the RA system, r
From a, (ra, ra,) and a, (a, a, a), the position deviation Δ and the attitude deviation are calculated by the following equations.

ここでは、（ra,ra,ra）なる姿勢から、（a,
a,ａ）なる姿勢への回転を行なう軸方向の単位ベク
トルを、は前記軸回わりの回転角をそれぞれ意味す
る。 Here, from the posture (ra, ra, ra), (a,
The unit vector in the axial direction for rotating to the posture a, a) means the rotation angle about the axis.

さらに、これらの演算結果に基づき、Ｂ系における位置
・姿勢偏差Δを次式により求める。Further, based on these calculation results, the position / orientation deviation Δ in the B system is calculated by the following equation.

この式から明らかなように、偏差Δｘは、パラメータ
ra,（ra,ra,ra）の変更により変化させることが
できる。この算出された偏差Δｘは、座標変換部45にお
いてＢ系から速度算出の行なわれるＡ系に変換される。 As is clear from this equation, the deviation Δx is
It can be changed by changing ra, (ra, ra, ra). The calculated deviation Δx is converted in the coordinate conversion unit 45 from the B system to the A system in which the speed is calculated.

なお、ハンドの動作制御の態様により速度算出をＨ系で
行なう場合は、偏差ΔｘはＨ系に変換されるが、ここで
は速度算出がＡ系で行なわれる場合について述べる。When the speed is calculated in the H system according to the mode of operation control of the hand, the deviation Δx is converted into the H system, but here, the case where the speed is calculated in the A system will be described.

次には動作条件設定部61、指令速度演算部62、及び座標
変換部63から構成される。Next, it comprises an operating condition setting unit 61, a command speed calculation unit 62, and a coordinate conversion unit 63.

まず、動作条件設定部61において、Ａ系における様々な
ハンドの動作条件を加味してハンドに加わった外力＊
を算出することによりハンドに高度な運動を実行させる
ことができる。First, in the operation condition setting unit 61, the external force applied to the hand in consideration of the operation conditions of various hands in the A system *
By calculating, the hand can be made to perform a high degree of exercise.

なお、具体的な応用例は後述する。A specific application example will be described later.

指令速度演算部62は、力算出部50で算出された力（動
作条件設定部を経てとなる）及び位置・姿勢算出部40
で算出された位置・姿勢の偏差Δから力基準値ｃ及
び質量マトリックス〔Ｍ〕，粘性定数マトリックス
〔Ｃ〕，バネ定数マトリックス〔Ｋ〕をパラメータとし
てＡ系におけるハンドの指令速度を次式により算出す
る。The command speed calculation unit 62 includes a force calculated by the force calculation unit 50 (through the operation condition setting unit) and a position / orientation calculation unit 40.
Using the force reference value c, the mass matrix [M], the viscosity constant matrix [C], and the spring constant matrix [K] as parameters from the position / orientation deviation Δ calculated in step 2, the hand command speed in the A system is calculated by the following equation. To do.

＝〔Ｍ〕^-1∫（−ｃ−〔Ｋ〕Δ〔Ｃ〕ｖ）dt…
…（19） 前述の原理的説明で述べた様に、本式によれば〔Ｍ〕，
〔Ｋ〕，〔Ｃ〕によって力を加えられたときのハンドの
挙動が定まるため、〔Ｍ〕，〔Ｋ〕，〔Ｃ〕の各パラメ
ータを適切に定めることによりハンドの動作特性を設定
することができる。= [M] ^-1 ∫ (-c- [K] Δ [C] v) dt ...
(19) As described in the above-mentioned principle explanation, according to this formula, [M],
Since the behavior of the hand when a force is applied by [K] and [C] is determined, the operating characteristics of the hand should be set by appropriately setting the parameters [M], [K], and [C]. You can

また、力基準値ｃを適切に定めることによっても押付
力を一定に保持したり変化させたり様々なハンドの動作
特性を得ることができる。Further, by appropriately setting the force reference value c, the pressing force can be held constant or changed, and various hand movement characteristics can be obtained.

以上算出された指令速度はＡ系における値であるの
で、座標変換部63において前述したパラメータ を用いてＢ系に変換され、次の角速度演算部70に出力さ
れる。Since the command speed calculated above is the value in the A system, the above-mentioned parameter in the coordinate conversion unit 63 is used. Is converted to the B system by using and is output to the next angular velocity calculation unit 70.

実際にロボットは、各関節の角速度で駆動・制御される
ため、指令速度は角速度演算部70において、ロボット
の各関節の回転角度を示す位置信号、ロボットの各ア
ーム長さ及び前述した変換行列〔Ａ_1h〕をパラメータ
として用い、各関節の に変換される。この はD/A変換回路27及びサーボアンプ18を介して各アーム
駆動用のモータＭに入力され、ロボットのハンドは所定
の速度で動作することになる。Since the robot is actually driven and controlled by the angular velocity of each joint, the command velocity is calculated in the angular velocity calculation unit 70 by a position signal indicating the rotation angle of each joint of the robot, each arm length of the robot, and the conversion matrix (described above). A _1h ] as a parameter, Is converted to. this Is input to the motor M for driving each arm through the D / A conversion circuit 27 and the servo amplifier 18, and the robot hand operates at a predetermined speed.

次に本発明による制御方法の具体的な応用例を次に述べ
る。Next, a specific application example of the control method according to the present invention will be described below.

まず、第１の応用例はRA系が移動せずＢ系に固定であ
り、Ａ系をＨ系に一致させ従ってRA系とRHが一致し、Ａ
系の初期の位置・姿勢がRA系と一致した例である。First, in the first application example, the RA system does not move and is fixed to the B system, so that the A system matches the H system, so that the RA system matches the RH,
This is an example in which the initial position and orientation of the system match the RA system.

この場合のハンドの動作を第12図を用いて説明する。第
12図（ａ）に示すように、Ｈ系におけるハンド15にｙ軸
方向になる力が加わると、Ｈ系における外力は、 と示される。ここで、式（19）においてｃ＝〇とし とすると、外力を受けたハンド15は第11図（ｂ）に示
す位置・姿勢で静止する。ここで Δｙ＝ｆ_ｙ/h₂₂ …………（24） Δθｘ＝mx/h₄₄ …………（25） となり、本発明の制御方法によれば、パラメータ〔Ｋ〕
の適切な設定によってロボットの手首部にバネを付加し
たときと同等の動作を得ることを意味している。The operation of the hand in this case will be described with reference to FIG. First
As shown in FIG. 12A, when a force in the y-axis direction is applied to the hand 15 in the H system, the external force in the H system becomes Is shown. Here, in the equation (19), c = 0 Then, the hand 15 receiving the external force stands still in the position / posture shown in FIG. 11 (b). Here, Δy = _fy / h ₂₂ (24) Δθx = mx / h ₄₄ (25), and according to the control method of the present invention, the parameter [K]
It means that the same operation as when a spring is added to the wrist of the robot is obtained by appropriate setting of.

即ち、力を瞬時に取り除くとハンドの原点Ｏ_Ｈは位置
の基準値ｒの原点即ちハンドの原点Ｏ_Ｈの初期位置を
中心として振幅Δｙなる単振動をし、またハンド15の姿
勢は第12図（ｂ）に示すように初期姿勢を中心として振
幅Δθｘとなる角度で示される回転の単振動をする。That is, when the force is removed instantaneously origin O _H hand is a simple harmonic motion comprising amplitude Δy around the initial position of the origin O _H origin i.e. the hand reference value r of the position and orientation of gripper 15 Figure 12 As shown in (b), a simple oscillation of rotation indicated by an angle having an amplitude Δθx with the initial posture as the center is performed.

つまり、ハンドの原点Ｏ_Ｈの振動とハンドの姿勢の振動
が合成された振動をハンド15は行なう、具体的には式
（６）で示す減衰になるのでハンド15の位置と は次の様になる。That is, the vibration oscillations in the attitude of the vibration and the hand of the origin O _H hand were synthesized hand 15 is performed, in particular the position of the hand 15 since the attenuation represented by the formula (6) Is as follows.

つまり、ハンド15に任意の並進力・回転力が加わると、
ハンド15の先端点は初期位置へ、またハンド15の姿勢は
初期姿勢に戻ろうとする復元力と減衰力が働く。 In other words, if any translational force / rotational force is applied to the hand 15,
The tip of the hand 15 is at the initial position, and the posture of the hand 15 is a restoring force and a damping force for returning to the initial posture.

よって、実際にはバネがないにもかかわらず、あたかも
ハンド15が空中に上記の条件を満たすような６自由度の
バネでつるされているような動きをすることになる。Therefore, although there is actually no spring, the hand 15 moves as if it were suspended in the air by a spring having 6 degrees of freedom that satisfies the above conditions.

次に、〔Ｋ〕＝〔〇〕とおくと、質的に異なった動作を
ハンドにさせることができる。この場合は、バネの力即
ち復元力が無いのでハンドは元の位置や姿勢に戻ろうと
することはなく、Ｈ系に関して加えられた並進力の方向
に並進し、回転力の中心となる軸回りに回転する。つま
り空中間において、加えられた並進力及び回転力の方
向、大きさでなすがままに動き、力を加えることをやめ
れば静止するという運動が実現できる。Next, by setting [K] = [◯], it is possible to cause the hand to perform qualitatively different actions. In this case, since there is no spring force, that is, the restoring force, the hand does not try to return to the original position or posture, and the hand translates in the direction of the translational force applied to the H system and rotates around the axis that is the center of the rotational force. Rotate to. That is, in the midair, it is possible to realize a motion in which the translational force and the rotational force are applied in the direction and magnitude as they are, and when the application of force is stopped, the motion is stationary.

例えば、第12図（ａ）に示すような力を加えると、こ
の時の速度は式（８）より となる。For example, if a force as shown in Fig. 12 (a) is applied, the speed at this time is calculated from equation (8). Becomes

これまでの説明では、バネ定数マトリックス〔Ｋ〕の６
個の対角要素が全て〇でない全ての場合を考えたが、こ
の６個の対角要素の一部を〇とし、一部を〇としないよ
うに設定すると、例えば、力を加えるとハンドの姿勢に
関しては復元力が働き、常に元の姿勢に戻ろうとするが
位置に関しては押されるがままに動くという上述した２
つの動作特性とは異なった動作特性をハンドに持たせる
ことができる。In the above description, the spring constant matrix [K] of 6
We considered all the cases where the diagonal elements are not all 〇, but if you set some of these six diagonal elements to 〇 and not to some 〇, for example, if you apply force, the hand As for the posture, the restoring force works, and it always tries to return to the original posture, but the position moves as it is pushed.
The hand can have a motion characteristic different from one motion characteristic.

次に第２の応用例を述べる。Next, a second application example will be described.

第２の応用例はRA系が移動する場合であり、Ａ系はＨ系
に一致させ、従ってRA系とRH系が一致し、RA系が移動す
る例である。The second application example is a case where the RA system moves, and the A system is made to match the H system, so that the RA system and the RH system match and the RA system moves.

この場合のハンドの動作を第13図を用いて説明する。The operation of the hand in this case will be described with reference to FIG.

前述した第１の応用例では、RA系がＢ系に固定であっ
た。このため、ハンドにはRA系が示すところの位置・姿
勢への復元力が働き、また復元力はRA系からの偏差に比
例していたために、実際にはハンドはRA系の近傍でしか
作業することはできない。In the first application example described above, the RA system is fixed to the B system. For this reason, the hand exerts a restoring force to the position / posture indicated by the RA system, and since the restoring force was proportional to the deviation from the RA system, the hand actually works only near the RA system. You cannot do it.

しかし本例では、このRA系の位置・姿勢をＢ系上で移動
するところに特徴がある。RA系を移動させるとは、復元
力の平衡位置・姿勢を移動させることであるので、ハン
ドがその移動に追従する形で移動する。However, this example is characterized in that the position / orientation of this RA system is moved on the B system. To move the RA system means to move the equilibrium position / posture of the restoring force, so the hand moves in a manner that follows the movement.

例えば第13図に示すごとく、RA系の位置・姿勢を移動さ
せると、これに追従してハンドの位置・姿勢も移動し、
同時に前述した第１の応用例で示した動作特性も合わせ
持つ。For example, as shown in FIG. 13, when the RA system position / orientation is moved, the hand position / orientation also moves,
At the same time, it also has the operating characteristics shown in the first application example.

即ち、ハンドの移動中に力が加われば、RA系の移動径路
をその力に応じて外れるが、その瞬間に同時にRA系への
復元力が働くので、また移動径路に戻る。That is, if a force is applied while the hand is moving, the RA system moves out of the moving path according to the force, but at that moment, a restoring force to the RA system works at the same time, so the hand returns to the moving path.

つまり、手首部にバネが付加された機構を持つロボット
に於て、そのハンドが移動しているような運動を制御の
パラメータを与えるだけで実現できる。このときの動作
は、式（７）で示されるが、これをRA系が静止している
ときに力を加えたときの動作を示す式（６）と比較する
と基本的には同一であることがかわる。これは、ロボッ
トに加える力（RA系が静止時）とRA系の移動量を適切に
定めれば、この両者を置き換えてもハンドが同一の動作
を行い得ることを示唆するものである。That is, in a robot having a mechanism in which a spring is added to the wrist, a motion as if the hand is moving can be realized simply by giving a control parameter. The operation at this time is shown by the equation (7), but it is basically the same when comparing this with the equation (6) showing the operation when a force is applied when the RA system is at rest. Change. This suggests that if the force applied to the robot (when the RA system is stationary) and the amount of movement of the RA system are appropriately determined, the hand can perform the same operation even if both are replaced.

第３の応用例として、ハンドの動作条件を設定した場合
の例を示す。As a third application example, an example in which the operating condition of the hand is set will be shown.

最初の例は、動作条件を回転速度のみとし、Ａ系はＨ系
に一致させず、従ってRA系はRH系に一致せず、RA系はＢ
系に固定で、Ａ系の初期位置・姿勢と一致させるもので
ある。In the first example, the operating conditions are only rotation speed, A system does not match H system, therefore RA system does not match RH system and RA system is B
It is fixed to the system and matches the initial position and orientation of the A system.

この場合のハンドの動作を第14図を用いて説明する。The operation of the hand in this case will be described with reference to FIG.

ハンド15が持つシャフト80の先端点をＡ系の原点Ｏ_Ａと
する。基本的な動作は、第12図で示したものと同一であ
るが、Ａ系はＨ系と違う位置にしてある点が異なる。The tip of the shaft 80 of the hand 15 is the origin O _{A of the} A system. The basic operation is the same as that shown in FIG. 12, except that the A system is at a different position from the H system.

また動作の拘束条件として、並進力及びＺ軸回りの回転
力をＯとした。Further, the translational force and the rotational force about the Z axis were set to O as the constraint condition of the operation.

即ち、Ａ系での並進力をＯとし、従ってＡ系の原点Ｏ_Ａ
回りの回転力のみ許される。但しＺ軸回りの回転力はＯ
とした。このため力センサが検出できる範囲内で、ハン
ド及びシャフトのどの点にどのような方向に力（ただし
作用線が原点を通過しないものとする）を働かせても、
ハンド及びシャフトはｘ軸回りとｙ軸回りとの合成され
た回転運動しか行なわない。そして力を除けば元の姿勢
に復帰する。 That is, the translational force in the A system is O, and therefore the origin O _{A of the} A system is
Only rotational force around is allowed. However, the rotational force around the Z axis is O
And Therefore, within the range that can be detected by the force sensor, no matter which point of the hand and shaft is applied in any direction (however, the line of action does not pass the origin),
The hand and shaft perform only a combined rotational movement about the x-axis and the y-axis. And if the force is removed, it will return to the original posture.

この動作は前述したRCC機構の本質をなすシャフト（ワ
ーク）の先端点回りの回転を実現するものであり、自動
組立、特に精密嵌合作業に有用な動作である。This operation realizes the rotation of the shaft (workpiece) around the tip point, which is the essence of the RCC mechanism described above, and is useful for automatic assembly, particularly for precision fitting work.

次の例はハンドの動作条件を並進力のみとし、Ａ系はＨ
系に一致させ、従ってRA系はRH系に一致し、かつRA系は
Ｂ系に固定でＡ系の初期位置・姿勢と一致させるもので
ある。In the following example, the motion condition of the hand is only translational force, and the A system is H
Therefore, the RA system matches the RH system, and the RA system is fixed to the B system to match the initial position and attitude of the A system.

この場合のハンドの動作を第15図を用いて説明する。The operation of the hand in this case will be described with reference to FIG.

基本的な動作は第12図に示したものと同じであるが、動
作の拘束条件として、回転力をＯとした。Although the basic motion is the same as that shown in FIG. 12, the rotational force is set to O as a constraint condition of the motion.

つまり、Ａ系での回転力をＯとした。従って並進運動の
み許される。このため、どのような力をどの点に加えて
もハンドは姿勢が一定のまま、並進運動をする。そして
力を除けば元の位置に復帰する。 That is, the rotational force in the A system was set to O. Therefore, only translational movements are allowed. Therefore, no matter what force is applied to any point, the hand makes a translational motion with the posture kept constant. Then, if the force is removed, it returns to the original position.

また、式（23）に於いて、h₁₁,h₂₂を大きく、h₃₃を小さ
く定めた。Further, in the equation (23), h ₁₁ and h ₂₂ are set to be large and h ₃₃ is set to be small.

つまり、Ｈ系のｘ軸,y軸方向には柔いバネとして働き、
Ｚ方向には硬いバネとして働く。このため第15図で示す
ように、同じ力を加えれば、Ｚ軸方向に大きく振れ、xy
平面上では振れは小さい。In other words, it works as a soft spring in the x-axis and y-axis directions of H system,
It works as a hard spring in the Z direction. Therefore, as shown in Fig. 15, if the same force is applied, it will swing greatly in the Z-axis direction, and xy
The shake is small on the plane.

またここでは、Ａ系をＨ系に固定にしたので第15図で示
したように、この性質（Ｚ軸方向に大きく振れる）は、
ハンドの姿勢が変わっても常にハンドの中心軸方向（即
ちＨ系のＺ軸方向）に大きく振れることに変わりはな
い。Further, here, since the A system is fixed to the H system, as shown in FIG. 15, this property (great swing in the Z axis direction) is
Even if the posture of the hand changes, the hand always swings largely in the central axis direction (that is, the Z axis direction of the H system).

最後の例は、同じくハンドの動作条件を並進条件のみと
し、Ａ系の原点はＨ系に一致させるが、その軸方向はＢ
系に一致させるものである。In the last example, the motion condition of the hand is the translation condition only, and the origin of the A system is made to coincide with the H system, but its axial direction is B
Match the system.

この場合のハンドの動作を第16図を用いて説明する。The operation of the hand in this case will be described with reference to FIG.

この例においてはＡ系の方向がＨ系と異るため、ハンド
の姿勢が変われば大きく振れる方向は、ハンド系から見
て変わる。ただし、Ｂ系から見て不変である。つまり大
きく振れる方向は常にＢ系のＺ軸方向となる。In this example, since the direction of the A system is different from that of the H system, the direction in which the hand swings greatly changes when viewed from the hand system. However, it is unchanged from the B system. That is, the direction of large swing is always the Z axis direction of the B system.

従って上記２例によって、ハンドの動作条件をＨ系にお
いてでも、Ｂ系においてでもＡ系の定め方一つで変えら
れることがわかる。Therefore, it can be seen from the above two examples that the operating condition of the hand can be changed in either the H system or the B system by one method of defining the A system.

次に、本発明による制御方法に非接触センサからの情報
を取り入れた応用例を述べる。Next, an application example in which information from a non-contact sensor is incorporated in the control method according to the present invention will be described.

第17図はロボットをある作業領域空間内にて位置制御し
ている最中に、万一危険領域内に入るような場合には危
険領域内への侵入を非接触センサによって回避せんとす
るものである。既述のように、ロボットの位置制御は位
置基準値raを操作することによって行なわれるが、こ
れよりすればraを作業領域空間内にとれば、如何に操
作しようともロボットはほぼ作業領域空間内に留まるこ
とが予想される。Fig. 17 shows that if a robot enters the danger area while it is being position-controlled in a certain work area space, it will avoid entry into the danger area by a non-contact sensor. Is. As described above, the position control of the robot is performed by operating the position reference value ra, but if this is done, if ra is in the work area space, the robot will be in the work area space no matter how it is operated. Is expected to stay in.

しかしながら、実際には設定されたパラメータ〔Ｍ〕，
〔Ｃ〕，〔Ｋ〕やロボット駆動系の遅れなどによって、
あるいは誤動作や外乱などによってロボットが作業領域
外、即ち、危険領域内に入ることがあり得るというもの
である。第17図に示す例ではｘ軸の正、負方向に壁116,
117があって、壁116,117からの距離がΔlc以内の領域が
危険領域として設定されている。したがって、ロボット
としてはｘ軸の正方向、負方向における壁117,116との
間の距離を測定すべく少なくとも２個の非接触センサ11
8を必要とし、これら非接触センサ118によってロボット
ハンドが危険領域の何れかに侵入したことが知れるもの
である。However, in reality, the set parameter [M],
Due to [C], [K] and delay of robot drive system,
Alternatively, the robot may enter the work area, that is, the dangerous area due to malfunction or disturbance. In the example shown in FIG. 17, the walls 116,
There is 117, and a region within a distance of Δlc from the walls 116 and 117 is set as a dangerous region. Therefore, as a robot, at least two non-contact sensors 11 are required to measure the distance between the walls 117 and 116 in the positive and negative directions of the x-axis.
8 is required, and it is known by these non-contact sensors 118 that the robot hand has entered any of the dangerous areas.

例えば、ロボットハンドが壁117に近づきつつある場合
を想定すれば、壁117との距離Δlx＋がΔlx＋＜Δlcと
なったことを以て危険領域に侵入したことが知れるもの
である。もしも危険領域にロボットのハンド15が侵入し
た場合にはΔlx＋がΔlcとなるべくロボットのハンド15
を押し戻すことが必要である。For example, assuming that the robot hand is approaching the wall 117, it is known that the distance Δlx + from the wall 117 has reached Δlx + <Δlc, and that the robot hand has entered the dangerous area. If the robot's hand 15 enters the dangerous area, Δlx + should be Δlc so that the robot's hand 15
It is necessary to push back.

第18図に示すように、ある時点tcで危険領域に侵入した
ことが検知された場合には、直ちに第５図に示した如く
の挙動をとらせ円滑に危険領域より脱しさせるものであ
る。勿論従来より行なわれているように、その時点tcで
非常停止せしめたり、あるいは円滑にではなく直ちに大
きな加速度を以て危険領域を脱しさせることも可能であ
る。As shown in FIG. 18, when it is detected that the dangerous area is invaded at a certain time point tc, it immediately takes the action as shown in FIG. 5 and smoothly exits the dangerous area. Of course, as has been done conventionally, it is possible to make an emergency stop at that time tc, or to move out of the dangerous area with a large acceleration immediately instead of smoothly.

ところで、非接触センサによる上記のような制御はこれ
までに既に述べた本発明による制御方法に非接触センサ
からのフイードバック信号を加味することによって容易
に達成可能である。By the way, the above-mentioned control by the non-contact sensor can be easily achieved by adding the feedback signal from the non-contact sensor to the control method according to the present invention described above.

また、以上のような制御以外に第19図に示すような制御
も考えられている。第19図（ａ），（ｂ）はロボットが
人間に近づきその間の距離がΔlc,x＋よりも小さくなっ
た場合は、ロボットが人間から離れΔlc,x＋の位置まで
戻るべく制御され、また、それとは逆に人間がロボット
に近づいた場合も同様に制御されることが示されてい
る。第19図に示す制御はいわば壁が移動可とされた場合
に相当するものであり、このような制御によって非接触
直接教示が可能となるものである。In addition to the control described above, control shown in FIG. 19 is also considered. 19 (a) and 19 (b), when the robot approaches a human and the distance between them becomes smaller than Δlc, x +, the robot is controlled to return from the human to the position of Δlc, x +, and On the contrary, it is shown that when a human approaches a robot, the same control is performed. The control shown in FIG. 19 corresponds to a case where the wall can be moved, so to speak, and non-contact direct teaching is possible by such control.

以上２つの応用例は何れもロボットと対象物との間の距
離を検出したうえ、ある基準値との関係にもとづいてロ
ボットを制御するようにしたものである。このような制
御を拡張したものとして第20図に示す如くのものが考え
られている。第20図に示すものにおいては非接触センサ
118以外に接触センサとしての力センサ16もロボットに
装着され、ハイブリットな制御が可能となっている。In each of the above two application examples, the distance between the robot and the object is detected, and the robot is controlled based on the relationship with a certain reference value. As an extension of such control, the one shown in FIG. 20 is considered. In the case shown in Fig. 20, a non-contact sensor
In addition to 118, a force sensor 16 as a contact sensor is attached to the robot, and hybrid control is possible.

例えば軸状ワーク119を穴が穿たれているワーク120に組
付する場合を想定すれば、ロボットはワーク119把持し
た後はワーク120近傍まで搬送することが必要である。
ワーク120への搬送は高速に行なわれることが望ましい
が、高速搬送の場合はワーク120に接触した時点で大き
な衝撃を受けることから、接触直前に搬送速度を小さく
することが必要となる。一般に接触センサのみしか装着
されていない場合は、大きな衝撃で初めて接触が検知さ
れるか、または大きな衝撃を回避すべくワークは低速に
て搬送されることになるが、何れによるとしても大きな
衝撃または低速といった不具合を生じることになる。For example, assuming the case where the shaft-shaped work 119 is assembled to the work 120 having a hole, the robot needs to convey the work 119 to the vicinity of the work 120 after gripping the work 119.
It is desirable that the conveyance to the work 120 is performed at high speed, but in the case of high-speed conveyance, a large impact is applied at the time of contact with the work 120, so it is necessary to reduce the conveyance speed immediately before the contact. Generally, when only the contact sensor is attached, the contact is detected for the first time with a large impact, or the work is conveyed at low speed to avoid a large impact, but in any case, a large impact or This will cause problems such as low speed.

しかしながら、非接触センサを併用する場合には大きな
衝撃を回避し得、しかも高速にワークを搬送することが
可能となるものである。However, when the non-contact sensor is also used, a large impact can be avoided and the work can be conveyed at high speed.

即ち、ワーク120にワーク119が接触する直前を非接触セ
ンサ118によって検出したうえで搬送速度を小さくし、
ワーク119が低速搬送の状態でワーク120に接触した後は
接触センサ118によってワーク119のワーク120への組付
を行なわんとするものである。That is, the contact speed is reduced immediately after the contact of the work 119 with the work 120 is detected by the non-contact sensor 118,
After the work 119 comes into contact with the work 120 in the low-speed conveyance state, the work 119 is to be attached to the work 120 by the contact sensor 118.

以上説明した応用例は、非接触センサによってロボット
ハンドと障害物などの対象物との間の距離が一定以内で
あることが検出された場合にはその検出時点でロボット
に対する制御モードを変更するようにしたものである。In the application example described above, if the non-contact sensor detects that the distance between the robot hand and an object such as an obstacle is within a certain range, the control mode for the robot is changed at the time of detection. It is the one.

したがって、対象物との関係でロボットは適切に制御さ
れ、更に接触センサを併用する場合はそれとの協調制御
も可能となっており、多種の制御動作特性が制御上での
パラメータを変更するだけで容易に実現し得るなどの効
果がある。Therefore, the robot is appropriately controlled in relation to the target object, and when a contact sensor is also used, it is possible to perform coordinated control with it, and various control operation characteristics can be obtained by simply changing the control parameters. There is an effect that it can be easily realized.

〔発明の効果〕〔The invention's effect〕

以上に説明したように本発明によれば、ロボットの制御
によってフレキシビリティを持たせることが可能とな
り、次のような作用効果を有する。As described above, according to the present invention, flexibility can be provided by controlling the robot, and the following operational effects can be obtained.

即ち、必要とする動作を行なうツールを想定し、そのツ
ールと同等の動作をロボットにさせることができ、かつ
想定するツールを変換できることを実現することであ
る。That is, it is possible to assume that a tool that performs a required operation is assumed, the robot can be caused to perform an operation equivalent to that tool, and the assumed tool can be converted.

従来は、遂次ツールの交換をしていたが、本発明によれ
ば、この交換を単なる制御上のバラメータの交換ででき
るようになった。Conventionally, the tool was replaced successively, but according to the present invention, this replacement can be performed by simply replacing the parameter for control.

また、同一のツールを用いてはいるが、若干特性を変え
た動作をロボットにさせる時にも、従来ならば、遂次ツ
ールの一部を調整しないとできなかったが、本発明によ
ればオンラインでツールの特性を必要に応じて制御の面
だけで変換できるという便利な機能を持たせることがで
きる。Further, although the same tool is used, even when the robot is caused to perform an operation with slightly different characteristics, conventionally, it was necessary to adjust a part of the sequential tool, but according to the present invention, online With, it is possible to add a convenient function that can convert the characteristics of the tool only in terms of control.

以上の如く一貫して、本発明をツール代替機能の面から
述べてきたが、本発明によればこの他にシミュレーショ
ン機能を持たせることができる。As described above, the present invention has been consistently described in terms of the tool substitute function. However, according to the present invention, a simulation function can be provided in addition to this.

これは、例えばロボットに取付ける手首機構を設計する
に際し、実際に物を作る前にその機構の挙動を見たい時
に本発明の手法を適用してロボットにその動作を実際に
させることができる。For example, when designing a wrist mechanism to be attached to a robot, and when it is desired to see the behavior of the mechanism before actually making an object, the method of the present invention can be applied to cause the robot to actually perform the operation.

上記のツール代替、シミュレーションの各機能は、ロボ
ットの精度が高い程、その効果も高い。しかし、現実に
はロボットはアーム部の弾性や減速歯車の剛性不足等に
よる振動他の機械的な原因やまた電気回路その他の原因
で、不要な振動、遅れ等が生じている場合がある。The higher the accuracy of the robot, the higher the effects of the above-mentioned tool substitution and simulation functions. However, in reality, in a robot, unnecessary vibration and delay may occur due to mechanical factors such as vibration due to elasticity of the arm portion and insufficient rigidity of the reduction gear, and due to other reasons such as electric circuits.

このような時に、ロボットのこの振動、遅れ等を解析し
て予想できたならば、それを少くするための補償動作を
本発明によれば逆に加えることも可能となる。In such a case, if the vibration and delay of the robot can be analyzed and predicted, a compensating operation for reducing the vibration can be reversely added according to the present invention.

以上述べた様に、本発明を有用な様々な効果を奏するも
のである。As described above, the present invention has various useful effects.

【図面の簡単な説明】 第１図は本発明のロボット制御方法の全体構成を示す構
成概略図、第２図は本発明のロボット制御装置の構成要
素を示す構成図、第３図は本発明のロボット制御装置を
示す概略ブロック線図、第４図は本発明の制御方法によ
るロボットの第１の動作例を示す説明図、第５図は本発
明の制御方法によるロボットの第２の動作例を示す説明
図、第６図は本発明の制御方法によるロボットの第３の
動作例を示す説明図、第７図は本発明の制御方法におけ
る演算フローの第１の実施例を示すブロック線図、第８
図は本発明の制御方法における演算フローの第２の実施
例を示すブロック線図、第９図は本発明の制御方法にお
ける演算フローの第３の実施例を示すブロック線図、第
10図はロボットの構成及び座標系を示す説明図、第11図
は本発明のロボット制御装置を示す詳細なブロック線
図、第12図（ａ），（ｂ）は本発明の制御方法の具体的
な第１の応用例を示す説明図、第13図は本発明の制御方
法の具体的な第２の応用例を示す説明図、第14図，第15
図，第16図は本発明の制御方法の具体的な第３の応用例
を示す説明図、第17図，第18図，第19図，第20図は本発
明による制御方法と非接触センサを組合せた実施例を示
す説明図である。 10……（関節形）ロボット、15……ハンド、16……力セ
ンサ、17……制御装置、18……サーボアンプ、40……位
置・姿勢算出部、50……力算出部、60……制御主要部、
70……関節角速度算出部。BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a schematic configuration diagram showing an overall configuration of a robot control method of the present invention, FIG. 2 is a configuration diagram showing components of a robot control device of the present invention, and FIG. FIG. 4 is a schematic block diagram showing the robot controller of FIG. 4, FIG. 4 is an explanatory view showing a first operation example of the robot by the control method of the present invention, and FIG. 5 is a second operation example of the robot by the control method of the present invention. 6 is an explanatory view showing a third operation example of the robot according to the control method of the present invention, and FIG. 7 is a block diagram showing a first embodiment of a calculation flow in the control method of the present invention. , 8th
FIG. 9 is a block diagram showing a second embodiment of the operation flow in the control method of the present invention, and FIG. 9 is a block diagram showing a third embodiment of the operation flow in the control method of the present invention.
FIG. 10 is an explanatory view showing the configuration and coordinate system of the robot, FIG. 11 is a detailed block diagram showing the robot control device of the present invention, and FIGS. 12 (a) and 12 (b) are specific examples of the control method of the present invention. FIG. 13 is an explanatory view showing a first specific application example, FIG. 13 is an explanatory view showing a second specific application example of the control method of the present invention, FIG. 14, FIG.
FIG. 16 is an explanatory view showing a third specific application example of the control method of the present invention, and FIGS. 17, 18, 19, 19 and 20 are the control method and non-contact sensor according to the present invention. It is explanatory drawing which shows the Example which combined. 10 …… (joint type) robot, 15 …… hand, 16 …… force sensor, 17 …… control device, 18 …… servo amplifier, 40 …… position / orientation calculation unit, 50 …… force calculation unit, 60… … Control main part,
70 ... Joint angular velocity calculator.

───────────────────────────────────────────────────── フロントページの続き (72)発明者 坂上 志之 神奈川県横浜市戸塚区吉田町292番地 株 式会社日立製作所生産技術研究所内 (56)参考文献 特開 昭60−254209（ＪＰ，Ａ) 特開 昭60−205716（ＪＰ，Ａ) ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Shiyuki Sakagami 292 Yoshida-cho, Totsuka-ku, Yokohama-shi, Kanagawa Inside the Hitachi, Ltd. Institute of Industrial Science (56) References JP-A-60-254209 (JP, A) JP-A-60-205716 (JP, A)

Claims (4)

【特許請求の範囲】[Claims] 【請求項１】サンプリングタイム毎にロボットを制御す
るロボットの制御方法であって、 前サンプリングタイム（ｎ−１）におけるロボットのハ
ンドに加えられた実際の力を検知して力信号を算出し、
この信号と前サンプリングタイム（ｎ−１）における目
標値であった力基準値との差である第１の値を算出し、 前記ハンドの前サンプリングタイム（ｎ−１）の実際の
位置及び姿勢を表わす位置信号と前サンプリングタイム
（ｎ−１）における位置及び姿勢の目標値であった位置
基準値との差にバネ定数を掛けた第２の値を算出し、 前記ハンドの前サンプリングタイム（ｎ−１）における
速度信号としてロボットに設けられた位置検出器からの
位置検出値を微分した値を用い、これに粘性定数を掛け
た第３の値を算出し、 前記第１、第２、第３の値と前記ハンドの質量係数から
前記ハンドの次サンプリングタイム（ｎ）の指令速度を
算出することを特徴とするロボットの制御方法。1. A robot control method for controlling a robot at every sampling time, wherein a force signal is calculated by detecting an actual force applied to a robot hand at a previous sampling time (n-1),
The first value, which is the difference between this signal and the force reference value that was the target value at the previous sampling time (n-1), is calculated, and the actual position and posture of the hand at the previous sampling time (n-1) are calculated. A second value is calculated by multiplying the difference between the position signal that represents the position signal and the position reference value that was the target value of the position and orientation at the pre-sampling time (n-1) by the spring constant, and the pre-sampling time of the hand ( As a velocity signal in n-1), a value obtained by differentiating a position detection value from a position detector provided in the robot is used, and a third value is calculated by multiplying this by a viscosity constant, and the first, second, A method for controlling a robot, comprising calculating a command speed of a next sampling time (n) of the hand from a third value and a mass coefficient of the hand. 【請求項２】前記位置信号として、前記ロボットに設け
られた位置検出器からの位置検出値を使用することを特
徴とする特許請求の範囲第１項記載のロボット制御方
法。2. The robot control method according to claim 1, wherein a position detection value from a position detector provided in the robot is used as the position signal. 【請求項３】前記位置検出信号として、前記ロボットに
設けられた非接触センサからの位置情報を使用すること
を特徴とする特許請求の範囲第１項記載のロボットの制
御方法。3. A robot control method according to claim 1, wherein position information from a non-contact sensor provided in the robot is used as the position detection signal. 【請求項４】サンプリングタイム毎にロボットを制御す
るロボットの制御装置であって、 前サンプリングタイム（ｎ−１）におけるロボットのハ
ンドに加えられた実際の力を検知して力信号を算出し、
この信号と前サンプリングタイム（ｎ−１）における目
標値であった力基準値との差である第１の値を算出する
算出手段と、 前記ハンドの前サンプリングタイム（ｎ−１）の実際の
位置及び姿勢を表わす位置信号と前サンプリングタイム
（ｎ−１）における位置及び姿勢の目標値であった位置
基準値との差にバネ定数を掛けた第２の値を算出する算
出手段と、 前記ハンドの前サンプリングタイム（ｎ−１）における
速度信号としてロボットに設けられた位置検出器からの
位置検出値を微分した値を用い、これに粘性定数を掛け
た第３の値を算出する算出手段と、 前記第１、第２、第３の値と前記ハンドの質量係数から
前記ハンドの次サンプリングタイム（ｎ）の指令速度を
算出する算出手段とを備えたことを特徴とするロボット
の制御装置。4. A robot controller for controlling a robot at every sampling time, wherein a force signal is calculated by detecting an actual force applied to a robot hand at a previous sampling time (n-1),
Calculating means for calculating a first value which is the difference between this signal and the force reference value which was the target value at the pre-sampling time (n-1), and the actual value of the pre-sampling time (n-1) of the hand. Calculating means for calculating a second value by multiplying a spring constant by a difference between a position signal representing the position and the attitude and a position reference value which was the target value of the position and the attitude at the previous sampling time (n-1); A calculating means for calculating a third value by using a value obtained by differentiating a position detection value from a position detector provided in the robot as a velocity signal at the hand pre-sampling time (n-1) and multiplying this by a viscosity constant. And a calculation means for calculating a command speed for the next sampling time (n) of the hand from the first, second and third values and the mass coefficient of the hand.