JPH0146275B2 - - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、工業用ロボツト等の作業機械のなら
い制御方法および装置に係り、特に、ならい動作
の対象物の曲率が変化しても常に円滑なならい動
作を行うに好適なならい制御方法および装置に関
する。[Detailed explanation of the invention] [In industrial use field] The present invention is always smooth even if the curvature of the object of the outbreak changes, especially for the control method and device of working machinery such as Robotto for industrial industries. The present invention relates to a tracing control method and device suitable for performing contouring operations.

〔従来の技術〕[Conventional technology]

従来から行われているならい制御方式は、なら
い動作の基本となる動作方向（以下、「基本動作
方向」という）を常に一定に保ち、これと直角な
方向の対象物の凹凸をセンサにより検出し、その
凹凸の形状に応じて作業機械に取付けられた作業
工具またはセンサを上記センサの検出方向に駆動
し、対象物形状にならわせる方式である。この場
合、対象物の形状の接線方向がセンサの検出方向
と一致するかほぼ一致する個所においては、なら
い動作が不可能となる欠点があつた。また、この
方式では、対象物に対する作業工具やセンサの姿
勢と対象物に沿つた動作速度とを一定に保つこと
が困難であるという欠点かあつた。この欠点は、
例えば塗装ロボツトでは、塗装むらを生じる原因
となる。 The conventional tracing control method keeps the basic movement direction of the tracing operation (hereinafter referred to as the "basic movement direction") constant, and uses sensors to detect irregularities on the object in a direction perpendicular to this direction. In this method, a working tool or a sensor attached to a working machine is driven in the detection direction of the sensor according to the shape of the unevenness, and is made to follow the shape of the object. In this case, there was a drawback that the tracing operation was impossible at a location where the tangential direction of the shape of the object coincided with or almost coincided with the detection direction of the sensor. Furthermore, this method has the disadvantage that it is difficult to maintain constant the posture of the power tool or sensor relative to the object and the speed of movement along the object. This drawback is
For example, in painting robots, this can cause uneven painting.

これらの欠点を解消する方策として、出願人は
既に特開昭55−18376号および特開昭58−34781号
に記載の方法を提案している。これらの方法で
は、センサの検出結果として得られる対象物の接
線方向情報に着目し、ならい動作の基本動作方向
を上記接線方向に一致させるように逐時変更する
という新しい考え方が導入されている。 As a measure to overcome these drawbacks, the applicant has already proposed methods described in Japanese Patent Application Laid-Open Nos. 55-18376 and 58-34781. These methods introduce a new concept of focusing on information on the tangential direction of the object obtained as a detection result of the sensor, and changing the basic motion direction of the tracing motion from time to time so that it matches the tangential direction.

〔発明が解決しようとする課題〕[Problem to be solved by the invention]

しかし、この方式は、対象物の接線方向の変化
量すなわち対象物表面の曲率を一定タイミングま
たはならい動作移動距離ごとに検出し、その検出
ごとにならい動作の方向を変更するものであつ
て、対象物表面の曲率そのものまでも考慮したも
のではなかつた。言い換えると、上記提案の方法
は、対象物表面の曲率にかかわらず、一定長さ
ΔLの線分で対象物表面形状を折れ線近似するこ
とに相当するならい制御方法であつた。 However, this method detects the amount of change in the tangential direction of the object, that is, the curvature of the object's surface, at a fixed timing or every distance traveled by the tracing motion, and changes the direction of the tracing motion each time it is detected. Even the curvature of the object's surface itself was not taken into consideration. In other words, the method proposed above is a tracing control method equivalent to approximating the object surface shape with a line segment of constant length ΔL, regardless of the curvature of the object surface.

ところが、対象物表面の形状を折れ線近似する
にあたり常に同一長さの線分を用いると、対象物
表面の曲率半径が小さい個所すなわち対象物表面
の接線方向の変化が激しい個所においては、なら
い動作方向と実際の接線方向との誤差が大きくな
るという問題が依然として残つている。 However, if line segments of the same length are always used when approximating the shape of the object surface with a polygonal line, the direction of the profiling movement will change in areas where the radius of curvature of the object surface is small, that is, in areas where the tangential direction of the object surface changes drastically. There still remains the problem that the error between the angle and the actual tangential direction becomes large.

また、このように対象物表面の接線方向の変化
が激しい部分においてもゆるやかな部分において
も、一定移動距離ごとに曲率を検出すると、はな
はだしい場合はならい動作中に対象物がセンサの
検出範囲を外れてしまい、ならい動作の継続に必
要なセンサ出力が得られなくなつたり、センサと
対象物との干渉のおそれがあるなど大きな問題が
残つていた。 In addition, if the curvature is detected every fixed distance of movement, whether in areas where the tangential direction of the object surface changes rapidly or slowly, if the curvature is extremely changed, the object may move out of the detection range of the sensor during the movement. As a result, major problems remained, such as not being able to obtain the sensor output necessary to continue the tracing operation, and the risk of interference between the sensor and the object.

本発明の目的は、対象物表面の接線方向の変化
量すなわち対象物表面の曲率がならい動作中に大
きく変化しても、これに適切に対処しながらなら
い動作を円滑に継続できるならい制御方法および
装置を提供することである。 An object of the present invention is to provide a tracing control method and method capable of smoothly continuing the tracing operation while appropriately coping with the change in the amount of change in the tangential direction of the object surface, that is, the curvature of the object surface, during the tracing operation. The purpose is to provide equipment.

［課題を解決するための手段］ 本発明は、上記目的を達成するために、移動体
の先端に取付けられ対象物との相対距離を検出す
るセンサを対象物近傍で指定面内の接線の指定方
向に設定移動距離だけ移動させ、センサと対象物
との相対距離および移動体の３次元空間内の位置
に基づき対象物の位置およびこの対象物の位置に
おける指定面内の接線を演算し、その接線の指定
方向の延長上で前記設定移動距離だけ離れた点に
次の動作目標点を設定し、センサをその動作目標
点に向かつて移動させるならい制御方法におい
て、前記接線方向の変化量に応じて前記設定移動
距離を変更するならい制御方法を提案するもので
ある。[Means for Solving the Problems] In order to achieve the above object, the present invention uses a sensor attached to the tip of a moving object to detect the relative distance to the object to specify a tangent in a specified plane near the object. The position of the object and the tangent in the specified plane at the position of this object are calculated based on the relative distance between the sensor and the object and the position of the moving object in the three-dimensional space. In a tracing control method in which the next operation target point is set at a point separated by the set movement distance on the extension of the tangential line in the designated direction, and the sensor is moved toward the operation target point, This paper proposes a tracing control method for changing the set moving distance.

前記指定面と交わる面内に前記センサを複数個
設け、これらセンサの出力に基づき３次元形状の
対象物の前記交わる面内における法線方向を演算
し、前記３次元形状の対象物上の点における接平
面の法線を求め、この法線に対して一定の角度以
内となるように前記センサの方向を規制すること
もできる。 A plurality of the sensors are provided in a plane that intersects with the designated plane, and based on the outputs of these sensors, the normal direction of the three-dimensional object in the intersecting plane is calculated, and a point on the three-dimensional object is calculated. It is also possible to determine the normal line of the tangential plane at and regulate the direction of the sensor so that it is within a certain angle with respect to this normal line.

前記センサは、前記接線に対する法線方向に正
確に向いている必要はなく、距離測定に所定値以
上の誤差を招かない範囲で一定の角度を保ちつつ
移動するようにしてもよい。 The sensor does not need to be oriented exactly in the direction normal to the tangent line, and may be moved while maintaining a constant angle within a range that does not cause an error of more than a predetermined value in distance measurement.

本発明は、また、移動体の先端に取付けられ対
象物との相対距離を検出するセンサを対象物近傍
で接線方向に設定移動距離だけ移動させる駆動手
段と、前記センサと対象物との相対距離および移
動体の３次元空間内の位置に基づき対象物の位置
およびこの対象物の位置における指定面内の接線
を求め前記駆動手段に動作目標点を出力する第１
演算手段と、前記移動前後の位置における接線方
向の変化量に応じて前記設定移動距離を変更し前
記第１演算手段に出力する第２演算手段とを含む
ならい制御装置を提案するものである。 The present invention also provides a driving means for moving a sensor attached to the tip of a moving body to detect a relative distance to a target object by a set travel distance in a tangential direction near the target object, and a relative distance between the sensor and the target object. and a first step that calculates the position of the object and the tangent in the designated plane at the position of the object based on the position of the moving object in the three-dimensional space and outputs the target operation point to the driving means.
A profiling control device is proposed that includes a calculation means and a second calculation means that changes the set moving distance according to the amount of change in the tangential direction at the position before and after the movement and outputs it to the first calculation means.

［作用］ 本発明においては、センサを対象物表面の接線
方向に設定移動距離だけ移動させる際に、移動前
後の接線方向の変化量が所定値を超えて増加する
に従い、前記動作目標点を設定する距離を前記設
定移動距離よりも短く変更するので、接線の方向
が徐々に変わる比較的なだらかな対象物表面は勿
論のこと、接線の方向の変化量が大きいすなわち
曲率半径が小さいコーナ部等においても、常に安
定して正確なならい制御が可能となる。[Operation] In the present invention, when the sensor is moved by a set movement distance in the tangential direction of the object surface, the operation target point is set as the amount of change in the tangential direction before and after the movement increases beyond a predetermined value. Since the distance to be moved is changed to be shorter than the set travel distance, it can be used not only on a relatively gentle object surface where the direction of the tangent changes gradually, but also on corners where the amount of change in the direction of the tangent is large, that is, the radius of curvature is small. Also, stable and accurate tracing control is always possible.

また、このならい動作の指定面と交わる（例え
ば直交する）面内に複数のセンサを配置し、これ
らセンサの出力に基づき３次元形状の対象物の前
記交わる面内における法線方向を演算し、前記３
次元形状の対象物上の点における接平面の法線を
求め、この法線に対して一定の角度以内となるよ
うに前記センサの方向を規制すると、前記交わる
面方向にも曲面となつている３次元対象物のなら
い動作がより正確になされる。 Further, a plurality of sensors are arranged in a plane that intersects (for example, perpendicularly intersects) with the specified plane of this tracing operation, and based on the outputs of these sensors, the normal direction of the three-dimensional object in the intersecting plane is calculated, Said 3
If the normal to the tangential plane at a point on the dimensional object is determined and the direction of the sensor is regulated to be within a certain angle with respect to this normal, the surface will also be curved in the direction of the intersecting surface. Tracing the three-dimensional object can be performed more accurately.

〔実施例〕〔Example〕

以下、図面を参照して本発明の実施例を具体的
に説明する。 Embodiments of the present invention will be specifically described below with reference to the drawings.

第１図は、本発明によるならい制御装置の一実
施例を備えた作業機械として、曲面Ｗの形状を計
測するロボツト１を示す斜視図である。ロボツト
１は、油圧シリンダ２により第１コラム３の上を
水平方向（以下「Ｘ軸方向」という）に往復移動
する第１移動体４と、第１移動体４上に直立に設
けられた第２コラム５に沿つて油圧シリンダ６に
より上下方向（以下「Ｚ軸方向」という）に移動
する第２移動体７と、第２移動体７に取付けられ
油圧シリンダ８によりＸ軸と直交する水平方向
（以下「Ｙ軸方向」という）に往復移動する第３
移動体９と、第３移動体９の先端に取り付けられ
た手首機構１０とからなる。 FIG. 1 is a perspective view showing a robot 1 for measuring the shape of a curved surface W as a working machine equipped with an embodiment of a contouring control device according to the present invention. The robot 1 includes a first moving body 4 that reciprocates in the horizontal direction (hereinafter referred to as "X-axis direction") on a first column 3 by a hydraulic cylinder 2, and a first moving body 4 that is installed upright on the first moving body 4. 2. A second moving body 7 that moves in the vertical direction (hereinafter referred to as "Z-axis direction") along the second column 5 by a hydraulic cylinder 6, and a horizontal direction perpendicular to the X-axis by a hydraulic cylinder 8 attached to the second moving body 7. (hereinafter referred to as the “Y-axis direction”)
It consists of a moving body 9 and a wrist mechanism 10 attached to the tip of the third moving body 9.

手首機構１０は第３移動体９に固定された垂直
軸１１のまわりに回転する振りブロツク１２と、
振りブロツク１２に取付けられ水平軸１３のまわ
りに回転する曲げブロツク１４とからなる。 The wrist mechanism 10 includes a swinging block 12 that rotates around a vertical shaft 11 fixed to the third moving body 9;
A bending block 14 is attached to a swinging block 12 and rotates about a horizontal axis 13.

曲げブロツク１４には、作業対象物との近接距
離を検出する渦電流式センサ２１が取付けられ、
その主軸線２１０が水平軸１３と常に直交するよ
うになつている。曲げブロツク１４にはまた、セ
ンサ２１と同一型式のセンサ２２がセンサ２１と
平行に軸間距離Lsをもつて配置されている。セ
ンサ２２は、センサ２１の主軸線２１０が水平と
なるように曲げブロツク１４を駆動して位置決め
した場合、センサ２１の上方に位置するように配
置されている。 An eddy current sensor 21 is attached to the bending block 14 to detect the proximity distance to the workpiece.
Its main axis 210 is always perpendicular to the horizontal axis 13. Also on the bending block 14, a sensor 22 of the same type as the sensor 21 is arranged parallel to the sensor 21 with an interaxial distance Ls. The sensor 22 is located above the sensor 21 when the bending block 14 is driven and positioned so that the main axis 210 of the sensor 21 is horizontal.

第２図は、第１図のならい制御装置およびロボ
ツト１の制御系のブロツク図である。マイクロコ
ンピユータ等からなる制御用計算機５１からの位
置指令は、インターフエース５２においてロボツ
ト１の各軸にふり分けられ、各軸の位置決め装置
５３〜５７に送り出される。例えばＸ軸の位置決
め装置５３に送られた位置指令は、減算回路５３
１が位置検出器５３４から得られたロボツト１の
Ｘ軸の現在位置との差を算出する際に用いられ、
その算出結果はサーボ回路５３２に送られる。サ
ーボ回路５３２の出力は、油圧サーボ弁や油圧シ
リンダ２などからなるロボツトＸ軸機構５３３を
駆動させる。Ｘ軸機構５３３の位置は位置検出器
５３４により検出され、減算回路５３１に送られ
ると共に、計算機５１にも読み込まれる。 FIG. 2 is a block diagram of the profiling control device and the control system of the robot 1 shown in FIG. Position commands from a control computer 51, such as a microcomputer, are distributed to each axis of the robot 1 at an interface 52, and sent to positioning devices 53-57 for each axis. For example, the position command sent to the X-axis positioning device 53 is transmitted to the subtraction circuit 53.
1 is used when calculating the difference from the current position of the robot 1 on the X axis obtained from the position detector 534,
The calculation result is sent to the servo circuit 532. The output of the servo circuit 532 drives a robot X-axis mechanism 533 consisting of a hydraulic servo valve, hydraulic cylinder 2, and the like. The position of the X-axis mechanism 533 is detected by a position detector 534, sent to the subtraction circuit 531, and also read into the computer 51.

また、センサ２１，２２の信号はセンサインタ
ーフエース５８により線形化して増幅され、計算
機５１に読み込まれる。 Further, the signals from the sensors 21 and 22 are linearized and amplified by the sensor interface 58, and then read into the computer 51.

次に、第３図〜第１０図を用いて、本発明のな
らい制御装置のならい制御アルゴリズムを説明す
る。ここでは説明を容易にするために、ならい動
作は水平面内で行われるものとし、第３図〜第５
図においては水平面内での制御方法の原理を、第
６図〜第１０図においては垂直平面内での制御方
法の原理を示しているが、ならい動作は水平面内
に限定されるものではなく、任意の平面内更には
曲面内において実行できるということはいうまで
もない。 Next, the profile control algorithm of the profile control device of the present invention will be explained using FIGS. 3 to 10. Here, for ease of explanation, it is assumed that the tracing operation is performed in a horizontal plane, and Figures 3 to 5.
Although the figure shows the principle of the control method in the horizontal plane, and FIGS. 6 to 10 show the principle of the control method in the vertical plane, the tracing operation is not limited to the horizontal plane. It goes without saying that it can be carried out within any plane or even within a curved surface.

なお、以下の説明では、ロボツト１の手先に取
付けられたセンサ２１の主軸２１０上で、センサ
２１の先端から距離Ssだけ離れた点Ｐをロボツ
トの手先位置とし、この手先位置に着目して動作
を説明する。また、センサ２１の検出可能な近接
距離の最小値をS_nioとし同じく最大値をS_naxとす
れば、S_nio＜Ss＜S_naxなるように距離Ssを選び、
この距離Ssをセンサ２１の標準検出距離と呼ぶ
ものとする。通常、距離SsはS_nioとS_naxとの平均
値をとればよい。 In the following explanation, a point P on the main shaft 210 of the sensor 21 attached to the hand of the robot 1, which is a distance Ss away from the tip of the sensor 21, is taken as the hand position of the robot, and the robot's hand position is focused on the operation. Explain. Further, if the minimum value of the detectable proximity distance of the sensor 21 is S _nio and the maximum value is S _nax , select the distance Ss so that S _nio < Ss < S _nax ,
This distance Ss shall be referred to as the standard detection distance of the sensor 21. Normally, the distance Ss may be determined by taking the average value of S _nio and S _nax .

ロボツト１の手先位置Ｐの座標（X_P、Y_P、Z_P）
を、ロボツト１の基台に固定された座標系により
表わせば、Ｘ、Ｙ、Ｚ軸の移動量をｘ、ｙ、ｚ、
また、Ｙ軸方向から測つた手首振りブロツク１２
の回転量をα、水平面から測つた手首曲げブロツ
ク１４の回転量をβとし、垂直軸１１と水平軸１
３の軸間距離をl_A、水平軸１３からセンサ２１先
端までの距離をl_Pとすると、 X_P＝ｘ−｛l_A＋（l_P＋Ss） cosβ｝sinα Y_P＝ｙ＋｛l_A＋（l_P＋Ss） cosβ｝cosα］ Z_P＝ｚ＋（l_P＋Ss）sinβ ……(1) と表わすことができる。 Coordinates of the hand position P of robot 1 (X _P , Y _P , Z _P )
If expressed in a coordinate system fixed to the base of the robot 1, the amount of movement on the X, Y, and Z axes can be expressed as x, y, z,
In addition, wrist swing block 12 measured from the Y-axis direction
α is the amount of rotation of the wrist bending block 14 measured from the horizontal plane, and β is the amount of rotation of the wrist bending block 14 measured from the horizontal plane.
3 _{, the} distance between the axes is l _A , and the distance from the horizontal _axis 13 to the tip of the sensor ₂₁ is _{l P.} (l _P +Ss) cosβ}cosα] Z _P = z + (l _P +Ss) sinβ ...(1).

まず、第３図〜第５図により水平面内でのなら
い制御について説明する。ロボツト１は最初にワ
ークＷのならい開始点Q₀の近傍に位置決めされ
ている。このとき、操作者が点Q₀におけるワー
クＷの接線とほぼ直角方向となるように、センサ
２１を予め位置決めしておく。ロボツト１はここ
から動作を開始し、センサ２１の出力がSsとな
つたところで一旦動作を停止する。このときのロ
ボツト１の手先位置ＰはQ₀点と一致している。 First, tracing control in a horizontal plane will be explained with reference to FIGS. 3 to 5. The robot 1 is initially positioned near the tracing start point _Q0 of the workpiece W. At this time, the operator positions the sensor 21 in advance so that it is substantially perpendicular to the tangent to the workpiece W at the point _Q0 . The robot 1 starts its operation from this point, and once the output of the sensor 21 reaches Ss, it stops its operation. At this time, the hand position P of the robot 1 coincides with the _Q0 point.

ならい動作は、センサ２１の主軸線２１０と直
角方向すなわちワークＷのおおよその接線方向に
ロボツト１を駆動することにより、開始される。
動作を開始し、移動距離L₀となつた時のロボツ
トの手先点をR₁とすると、ロボツトの手先がR₁
点に到達した時点で再度センサ２１のデータを入
力する。このときの測定距離をS₁とすれば、ワー
クＷ上の点Q₁の座標（X₁、Y₁、Z₁）は、ロボツ
ト各軸の移動量x₁、y₁、z₁、α₁、β₁を用いて X₁＝x₁−｛l_A＋（l_P＋S₁） cosβ₁｝sinα₁ Y₁＝y₁＋｛l_A＋（l_P＋S₁） cosβ₁｝cosβ₁］ Z₁＝z₁＋（l_P＋S₁）sinβ₁ ……(2) として求められる。 The tracing operation is started by driving the robot 1 in a direction perpendicular to the main axis 210 of the sensor 21, that is, in a direction roughly tangential to the workpiece W.
If the robot's hand point is R ₁ when it starts moving and reaches the moving distance L ₀ , then the robot's hand point is R _1.
When the point is reached, the data from the sensor 21 is input again. If the measurement distance at this time is S ₁ , the coordinates (X ₁ , Y ₁ , Z ₁ ) of point Q ₁ on the workpiece W are the movement amount of each axis of the robot x ₁ , y ₁ , z ₁ , α ₁ , using β ₁ , X ₁ = x ₁ − {l _A + (l _P + S ₁ ) cosβ ₁ } sinα ₁ Y ₁ = y ₁ + {l _A + (l _P + S ₁ ) cosβ ₁ } cosβ ₁ ] Z ₁ = z ₁ + (l _P + S ₁ ) sinβ ₁ ...(2).

なお、以下では上記移動距離L₀を基準サンプ
リング距離と呼ぶことにする。また、点Q₀から
点R₁への基準サンプリング距離L₀分のロボツト
１の動きは、これらの点間を結ぶ直線上に微細な
距離ごとの中間点を設け、これらの点の座標に対
応するロボツト１各軸の位置を順次指令し、直線
上を手先Ｐが一定速度で移動するように制御す
る。以下の説明においても、ロボツト１の動きま
たはロボツト１を駆動するという表現は、上記の
ような直線上の一定速度での動作またはそれらの
組合わせ動作を意味する。 Note that, hereinafter, the moving distance L ₀ will be referred to as a reference sampling distance. In addition, the movement of the robot 1 from point Q ₀ to point R ₁ over a standard sampling distance L ₀ is determined by setting intermediate points at each minute distance on the straight line connecting these points, and corresponding to the coordinates of these points. The position of each axis of the robot 1 is sequentially commanded, and the hand P is controlled to move at a constant speed on a straight line. In the following description as well, the expressions ``movement of the robot 1'' or ``driving the robot 1'' refer to movement in a straight line at a constant speed as described above, or a combination thereof.

さて、ロボツト１が点R₁に到着した時点で、
(2)式によりワークＷ上の点Q₁が明らかとなる。
そこでワークＷの近似的な接線が点Q₀と点Q₁と
を結ぶ直線として決まる。なお、このときの接線
の傾きは点Q₀、点Q₁の座標から容易に求められ
る。 Now, when robot 1 arrives at point _R1 ,
The point Q ₁ on the workpiece W becomes clear from equation (2).
Therefore, the approximate tangent to the workpiece W is determined as the straight line connecting the point Q ₀ and the point Q ₁ . Note that the slope of the tangent at this time can be easily determined from the coordinates of point Q ₀ and point Q ₁ .

いま、ロボツト１のＸ軸正方向から測つた角度
を用いて接線の傾きをθ₁と表わせば、Q₀点の座
標を（X₀、Y₀、Z₀）として、角度θ₁は次のよう
になる。 Now, if we express the slope of the tangent line as θ ₁ using the angle measured from the positive direction of the X-axis of robot 1, then if the coordinates of point Q ₀ are (X ₀ , Y ₀ , Z ₀ ), the angle θ ₁ is as follows. It becomes like this.

θ₁＝tan^-1Y₁−Y₀／X₁−X₀；X₀≠X₁のとき ＝０ ；X₀＝X₁のとき 本発明の制御方式は、このようにして得られた
接線の延長上にならい動作の目標点を設定しなが
ら動作していくことが基本となつている。すなわ
ち、点Q₀、点Q₁とを結ぶ直線の延長上に、点Q₁
から距離L₀の点R₂を定め、これを目標として次
のサンプリング区間のならい動作が行われる。θ ₁ = tan ^-1 Y ₁ −Y ₀ /X ₁ −X ₀ ; When X ₀ ≠ X ₁ = 0 ; When X _{0 =} The basic idea is to move while setting a target point for the movement following the extension of the above. In other words, on the extension of the straight line connecting point Q ₀ and point Q ₁ , point Q ₁
A point R ₂ at a distance L ₀ from the point R 2 is determined, and the tracing operation for the next sampling section is performed with this as the target.

なお、この場合、センサ２１によるワークＷの
位置測定のために、センサ２１が点R₂において
直線Q₀R₂と直角になるように駆動する。ここで
は、手首振りブロツク１２の回転角αを用いてセ
ンサ２１の姿勢を示し、例えばR₂点における姿
勢をα₂と表す。 In this case, in order to measure the position of the work W by the sensor 21, the sensor 21 is driven to be perpendicular to the straight line Q ₀ R ₂ at the point R ₂ . Here, the rotation angle α of the wrist swing block 12 is used to indicate the attitude of the sensor 21, and for example, the attitude at point _R2 is expressed as _α2 .

ロボツトの手先が点R₂に到達した時点でセン
サ２１の出力を測定し、これをS₂とすれば、(2)式
と同様にR₂点におけるロボツト１の各軸の位置
をx₂、y₂、z₂、α₂、β₂とすると、ワークＷ上の点
Q₂の座標（X₂、Y₂、Z₂）は、 X₂＝x₂−｛l_A＋（l_P＋S₂） cosβ₂｝sinα₂ Y₂＝y₂＋｛l_A＋（l_P＋S₂） cosβ₂｝cosβ₂］ Z₂＝z₂＋（l_P＋S₂）sinβ₂ ……(3) と表せられる。直線Q₁Q₂の傾きθ₂は θ₂＝tan^-1Y₂−Y₁／X₂−X₁；X₂≠X₁のとき ＝０ ；X₂＝X₁のとき である。 If the output of the sensor 21 is measured when the robot's hand reaches point R ₂ and this is set as S ₂ , the position of each axis of the robot 1 at point R ₂ is x ₂ , as in equation (2), Let y ₂ , z ₂ , α ₂ , β ₂ be the point on the work W
_{The coordinates of} Q _{2 (} X _{2 , Y 2 , Z 2 ) are :} +S ₂ ) cosβ ₂ }cosβ ₂ ] Z ₂ =z ₂ + (l _P +S ₂ ) sinβ ₂ ...(3) The slope θ ₂ of the straight line Q ₁ Q ₂ is θ ₂ =tan ⁻¹ Y ₂ −Y ₁ /X ₂ −X ₁ ; when X ₂ ≠X ₁ = 0; when X ₂ = X ₁ .

近似的な接線は直線Q₀Q₁から直線Q₁Q₂に変化
し、その傾きはθ₁からθ₂となつた。この傾きの変
化は、実はワーク表面形状の曲率を表わしてい
る。すなわち、接線方向の変化量は曲率半径に対
応している。 The approximate tangent changed from the straight line Q ₀ Q ₁ to the straight line Q ₁ Q ₂ , and its slope changed from θ ₁ to θ ₂ . This change in slope actually represents the curvature of the workpiece surface shape. That is, the amount of change in the tangential direction corresponds to the radius of curvature.

本発明のならい動作は、ワーク表面の曲線を接
線により折れ線近似している。したがつて、同一
の精度で曲線を折れ線近似するためには、曲線半
径が小さいところでは、折れ線の長さを短くする
必要がある。すなわち、近似的なに得られた接線
の延長上に、サンプリング距離Ｌだけ離れた点を
ならい動作の動作目標点とするが、このサンプリ
ング距離Ｌを上記接線の傾きの変化量に応じて変
更する必要がある。 In the tracing operation of the present invention, the curve of the workpiece surface is approximated by a polygonal line using tangents. Therefore, in order to approximate a curve to a polygonal line with the same accuracy, it is necessary to shorten the length of the polygonal line where the radius of the curve is small. That is, a point on the extension of the approximated tangent line, separated by a sampling distance L, is set as the movement target point of the tracing movement, but this sampling distance L is changed according to the amount of change in the slope of the tangent line. There is a need.

ここでは、接線の傾きの変化θ₂−θ₁の値に応じ
て、上記サンプリング距離Ｌを次のように変化さ
せる。 Here, the sampling distance L is changed as follows depending on the value of change θ ₂ −θ ₁ in the slope of the tangent.

Δθ＝θ₂−θ₁ とし、サンプリング距離Ｌを Ｌ＝ΔS₀／tanΔθ；｜Δθ｜ ≧Δθcのとき Ｌ＝L₀ ；｜Δθ｜＜Δθcのとき ……(4) とする。ここで、ΔS₀、L₀、Δθcは定数であり、
L₀は基準サンプリング距離、ΔS₀はSs−Sminと
Smax−Ssのうちの小さい方よりも小さい値とす
る。一般的には(4)の連続性から、ΔS₀＝
L₀tanΔθcとする方が良い。また、Δθcはならい
精度の所要値から定める。 Let Δθ=θ ₂ −θ ₁ , and let the sampling distance L be L=ΔS ₀ /tanΔθ; when |Δθ| ≧Δθc, L=L ₀ ; when |Δθ|<Δθc... (4). Here, ΔS ₀ , L ₀ , and Δθc are constants,
L ₀ is the reference sampling distance, ΔS ₀ is Ss−Smin
The value shall be smaller than the smaller of Smax−Ss. Generally, from the continuity of (4), ΔS ₀ =
It is better to set L ₀ tanΔθc. Further, Δθc is determined from the required value of tracing accuracy.

このようにして次のサンプリング距離Ｌを決
め、ならい目標点を定める。目標点R₃の決定は
(4)式により求めたＬをL₃とし、直線Q₁Q₂の延長
上にQ₂から距離L₃だけ離れた点として決定する。
点R₃にロボツト１が到達すれば、点R₃において
センサ２１の出力からワーク上の点Q₃の位置を
求め、今度は点Q₂とQ₃を結ぶ直線の延長上に次
の目標点R₄を定める。直線Q₂Q₃の傾きをθ₃とす
れば、R₄点における目標センサ姿勢を直線Q₂Q₃
に直角な姿勢とするための手首振りブロツク１２
の回転角α₄が得られる。このような処理を繰返し
ながら、ならい動作を行うのが、本発明の制御方
法の特徴である。 In this way, the next sampling distance L is determined, and the tracing target point is determined. Determination of target point _R3 is
The L obtained by equation (4) is defined as L ₃ , and is determined as a point on the extension of the straight line Q ₁ Q ₂ that is a distance L ₃ from Q ₂ .
When the robot 1 reaches point _R3 , the position of point _Q3 on the workpiece is determined from the output of the sensor 21 at point _R3 , and the next target point is found on the extension of the straight line connecting points _Q2 and _Q3 . Define R ₄ . If the slope of the straight line Q ₂ Q ₃ is θ ₃ , the target sensor posture at the R ₄ points is the straight line Q ₂ Q ₃
Wrist swing block 12 to maintain a posture perpendicular to
The rotation angle α ₄ is obtained. A feature of the control method of the present invention is that the tracing operation is performed while repeating such processing.

これを一般化すると、第４図のようになる。い
ま、点R_i+1にロボツト１の手先が到達している時
点において、直線Q_i-1Q_iの傾きをθ_iとし、このと
きのセンサ姿勢をα_i+1とする。点R_i+1においてワ
ークＷ上の位置を測定し、センサ２１の出力S_i+1
からワークＷ上の点Q_i+1の座標を求め、直線Q_i
Q_i+1の傾きθ_i+1を決定する。これより、 Δθ_i+1＝θ_i+1−θ_i が得られる。一方、点Q₁、R_i+1間の距離すなわち
前回のサンプリング距離をL_i+1とすれば、L_i+1の
距離間に接線の傾きがΔθ_i+1変化したから、基準
サンプリング距離L₀間の接線の傾き変化Δθ^* _i+1は、
曲率半径を一定と仮定して近似的に Δθ^* _i+1＝2sin^-1（L₀／L_i+1sinΔθ_i+1／２） ……(5) となる。そこで、次のサンプリング距離L_i+2を、
(4)式にならつて L_i+2＝ΔS₀／tanΔθ^* _i+1；｜Δθ^* _i+1｜≧Δθcのとき ……(6) L_i+2＝L₀ ；｜Δθ^* _i+1｜＜Δθcのとき として、次の区間のならい動作目標点R_i+2を直線
Q_iQ_i+1の延長上、点Q_i+1から距離L_i+2だけ離れた
点に設定し、かつR_i+2においてセンサ姿勢が直線
Q_iQ_i+1と直角な姿勢となるような角度α_i+2に設定
し、ならい動作を続ける。(6)式の関係を第５図に
示す。 If this is generalized, it will become as shown in Figure 4. Now, when the hand of the robot 1 has reached the point R _i+1 , the slope of the straight line Q _i-1 Q _i is set to θ _i , and the sensor attitude at this time is set to α _i+1 . The position on the workpiece W is measured at point R _i+1 , and the output S _i+1 of the sensor 21 is
Find the coordinates of the point Q _i+1 on the workpiece W from the line Q _i
Determine the slope θ _{i+ 1 of Q i+} 1. From this, Δθ _i+1 =θ _i+1 −θ _i is obtained. On the other hand, if the distance between points Q ₁ and R _i+1 , that is, the previous sampling distance, is L _i+1 , the slope of the tangent has changed by Δθ i _{+ 1} between the distance L i+1, so the standard sampling distance is The change in the slope of the tangent between L ₀ Δθ ^* _i+1 is
Assuming that the radius of curvature is constant, approximately Δθ ^* _i+1 =2sin ^-1 (L ₀ /L _i+1 sinΔθ _i+1 /2) ...(5). Therefore, the next sampling distance L _i+2 is
Following equation (4), when L _i+2 = ΔS ₀ /tanΔθ ^* _i+1 ; |Δθ ^* _i+1 | ≧Δθc...(6) L _i+2 = L ₀ ; |Δθ ^* _{i+ 1} When ｜<Δθc, set the target point R _i+2 of the next section on a straight line.
Due to the extension of Q _i Q _i+1 , set the point at a distance L _i+2 from point Q _i+1 , and the sensor attitude is straight at R _i+2.
Set the angle α _i+2 so that the posture is perpendicular to Q _i Q _i+1 , and continue the tracing motion. The relationship of equation (6) is shown in Figure 5.

次に、第６図〜第１０図により、本発明の垂直
面内での姿勢制御について説明する。まず、第６
図に示すように、ロボツト１はワークＷのならい
開始点Q₀の近傍に位置決めされており、センサ
２１，２２は、点Q₀におけるワークＷの垂直面
内での接線とほぼ直角方向となるように、操作者
により予めセツトされる。ロボツト１はここから
動作を開始し、センサ２１，２２の出力がともに
一致するように姿勢を制御して、一旦動作を停止
する。なお、姿勢制御にあたつては、ロボツト１
の手先Ｐ点の位置が変化しないように(1)式に基づ
いてロボツト１の各軸を駆動する。また、このと
きのセンサ２１の主軸２１１の傾きを水平面から
の仰角βで表わしてこれをβ₀とし、ワーク表面の
接線の傾きを鉛直面からの角度φで表わしてQ₀
点での傾きをφ₀とする。このときはφ₀＝β₀であ
る。ここでは、前述した水平面内におけるならい
制御方法から、センサ２１の出力はSsとなるよ
うに制御される。したがつて、センサ２１，２２
の出力はともにSsとなつている。 Next, posture control in a vertical plane according to the present invention will be explained with reference to FIGS. 6 to 10. First, the 6th
As shown in the figure, the robot 1 is positioned near the tracing start point Q ₀ of the workpiece W, and the sensors 21 and 22 are in a direction almost perpendicular to the tangent in the vertical plane of the workpiece W at the point Q ₀ . This is set in advance by the operator. The robot 1 starts its operation from this point, controls its posture so that the outputs of the sensors 21 and 22 match, and then temporarily stops its operation. In addition, for posture control, robot 1
Each axis of the robot 1 is driven based on equation (1) so that the position of point P of the hand does not change. Also, the inclination of the main axis 211 of the sensor 21 at this time is expressed as an elevation angle β from the horizontal plane, which is defined as β ₀ , and the inclination of the tangent to the workpiece surface is expressed as an angle φ from the vertical plane, Q ₀
Let the slope at the point be φ ₀ . At this time, φ ₀ =β ₀ . Here, the output of the sensor 21 is controlled to be Ss using the above-described profile control method in the horizontal plane. Therefore, the sensors 21, 22
Both outputs are Ss.

ならい動作が開始され、水平方向のならいが進
作して行くに従つて、センサ２１，２２の出力が
変化する。水平移動距離が第７図に示すように
L₀′となつたところを点R₁′とし、センサ２１の出
力からワーク上の点Q₁′の位置を求める。２つの
センサ２１，２２の出力をそれぞれS_A1、S_B1とす
れば、Q₁′点におけるワークＷの接線の傾きφ₁は、
２つのセンサ２１，２２間の軸間距離Lsを用い
て表わすと、 φ₁＝φ₀＋tan^-1S_B1−S_A1／Ls ……(7) となる。ここで、２つのセンサ２１，２２の出力
差をΔS_H1＝S_B1−S_A1とおき、tan^-1ΔS_H1／Ls＝Δφ₁と おく。 The tracing operation is started, and as the horizontal tracing progresses, the outputs of the sensors 21 and 22 change. The horizontal movement distance is as shown in Figure 7.
The point where L ₀ ' is reached is defined as point R ₁ ', and the position of point Q ₁ ' on the workpiece is determined from the output of the sensor 21. If the outputs of the two sensors 21 and 22 are S _A1 and S _B1 , respectively, then the slope φ ₁ of the tangent to the workpiece W at point Q ₁ ' is as follows:
When expressed using the center distance Ls between the two sensors 21 and 22, φ ₁ =φ ₀ +tan ⁻¹ S _B1 −S _A1 /Ls (7). Here, the output difference between the two sensors 21 and 22 is set as ΔS _H1 =S _B1 -S _A1 , and tan ^-1 ΔS _H1 /Ls = Δφ ₁ .

なお、以下の説明においては上記水平方向のな
らい動作距離L₀′を基準姿勢サンプリング距離と
呼ぶことにする。 In the following description, the horizontal tracing movement distance L ₀ ' will be referred to as the reference posture sampling distance.

さて、ロボツト１が点R₁′に到着した時点では
センサ仰角はβ₀である。次に、ならい動作を継続
する。いま、垂直面内のワーク表面曲率変化も、
前回のならい動作区間の曲率変化とほぼ同じであ
るとすれば、点Q₁′からならい動作距離L′だけ移
動した点で、センサ２１，２２の出力差はΔS_H1
になると予想される。垂直面内のならい姿勢制御
とは、このような予想にもとづいて次の姿勢サン
プリング距離L′を選択決定することである。ここ
では、 L′＝Δφc×L₀′／Δφ₁；｜Δφ₁｜≧Δφcのとき L′＝L₀′；｜Δφ₁｜＜Δφcのとき ……(8) となるように、すなわちセンサ出力差から求めた
姿勢サンプリング距離間の角度変化の値に反比例
させて、次の姿勢サンプリング距離L′を定める。 Now, when the robot 1 arrives at point R ₁ ', the sensor elevation angle is β ₀ . Next, continue the tracing motion. Now, the change in workpiece surface curvature in the vertical plane is also
Assuming that the change in curvature is almost the same as that of the previous tracing operation section, the output difference between the sensors 21 and 22 at the point moved by the tracing operation distance L' from point _Q1 ' is ΔS _H1
It is expected that Tracing attitude control in the vertical plane means selecting and determining the next attitude sampling distance L' based on such predictions. Here, L′=Δφc×L ₀ ′／Δφ ₁ ; When |Δφ ₁ |≧Δφc, L′=L ₀ ′; |Δφ ₁ |When <Δφc...(8) In other words, the sensor The next posture sampling distance L' is determined in inverse proportion to the value of the angle change between the posture sampling distances determined from the output difference.

ここに、Δφcは定数であり、１／２tan^-1 Smax−Smin／Lsまたはtan^-1Smax−Smin／2Lsよりも 小さくなるようにその値を定める。 Here, Δφc is a constant, and its value is determined to be smaller than 1/2tan ⁻¹ Smax−Smin/Ls or tan ⁻¹ Smax−Smin/2Ls.

このようにして次の姿勢サンプリング距離L′を
決め、次の姿勢サンプリング点R₂′を定める。
R₂′点は、(8)式により求めた距離L′をL₂′として、
水平方向のならい動作の接線上にとつた道のりが
L₂′になる点として決定する。こうしてロボツト
１が第８図に示すように点R₂′に到達すれば、２
つのセンサデータを再度読み込み、垂直面内での
ワークＷの傾きφ₂及びセンサ仰角β₂（＝φ₂）を計
算し、次の姿勢サンプリング点R₃′を求め、R₃′点
に到着したときにセンサ仰角がβ₂となるようにな
らい動作を行う。このような処理を繰返して行く
ことが、本発明のもう一つの特徴である。 In this way, the next attitude sampling distance L' is determined, and the next attitude sampling point R ₂ ' is determined.
The R ₂ ′ point is calculated using the distance L ′ obtained by equation (8) as L ₂ ′.
The path taken on the tangent of the horizontal tracing motion is
Determine the point where L ₂ ′. In this way, when robot 1 reaches point R ₂ ' as shown in FIG.
2 sensor data is read again, the inclination φ ₂ of the workpiece W in the vertical plane and the sensor elevation angle β ₂ (=φ ₂ ) are calculated, the next posture sampling point R ₃ ′ is determined, and the next posture sampling point R ₃ ′ is reached. Sometimes, the movement is performed so that the sensor elevation angle becomes β ₂ . Another feature of the present invention is that such processing is repeated.

これを一般化すると、第９図のようになる。点
R₁′にロボツト１の手先が到着している時点にお
いて、ワークＷの表面の垂直面内接線の傾きを
φ₁とすれば、この傾φ₁は φ_i＝φ_i-1＋tan^-1ΔS_Hi／Ls＝φ_i-1＋Δφ_i ……(9) である。ΔS_Hi＝S_Bi−S_Aiであり、センサ姿勢の仰
角はβ_i-1＝φ_i-1となつている。また、点Q′_i-1から
点R₁′までの水平方向ならい動作における接線上
にとつた道のりをL_i′とすれば、この道のりが
L₀′である場合の垂直面内ワーク接線の傾きφ_i ^*
は、曲率の変化が均一であると仮定して、近似的
に φ_i ^*＝φ_i-1＋L₀′／L₁′・Δφ_i ……(10) となる。よつて Δφ_i ^*＝φ_i ^*−φ_i-1＝L₀／L_i′・Δφ_i……(11) とおけば、次の姿勢サンプリング区間の道のり
L′_i+1は、(8)式にならつて、 L′_i+1＝Δφc×L₀′／Δφ_i ^* ＝Δφc×L_i′／Δφ_i ；｜Δφ_i ^*｜≧Δφcのとき L′_i+1＝L₀′ ；｜Δφ_i ^*｜＜Δφcのとき ……(12) として求めることができる。これにより、次の姿
勢サンプリング点R′_i+1の位置が得られる。そこ
で点R′_i+1において、センサ仰角β_iがβ_i＝φ_iとなる
ように設定し、ならい動作の姿勢制御を行なう。
(12)式の関係を第１０図に示す。 If this is generalized, it will become as shown in Fig. 9. point
If the slope of the tangent in the vertical plane of the surface of the workpiece W is φ ₁ at the time when the hand of the robot 1 has arrived at R ₁ ′, then this slope φ ₁ is φ _i =φ _i-1 + tan ^-1 ΔS _Hi /Ls=φ _i-1 +Δφ _i ...(9). ΔS _Hi =S _Bi −S _Ai , and the elevation angle of the sensor attitude is β _i-1 =φ _i-1 . Also, if the path taken on the tangent line in the horizontal tracing motion from point Q′ _i-1 to point R ₁ ′ is L _i ′, then this path is
The slope of the workpiece tangent in the vertical plane when L ₀ ′ is φ _i ^*
Assuming that the change in curvature is uniform, approximately φ _i ^* = φ _i-1 + L ₀ ′/L ₁ ′・Δφ _i ……(10). Therefore, if we set Δφ _i ^* = φ _i ^* −φ _i-1 = L ₀ /L _i ′・Δφ _i ……(11), the path of the next posture sampling section is
Following equation (8), L′ _i+1 is L when L′ _i+1 =Δφc×L ₀ ′/Δφ _i ^* =Δφc×L _i ′/Δφ _i ; |Δφ _i ^* |≧Δφc ′ _i+1 =L ₀ ′ ; |Δφ _i ^* | < Δφc ...(12) It can be obtained as follows. As a result, the position of the next posture sampling point R′ _i+1 is obtained. Therefore, at point R′ _i+1 , the sensor elevation angle β _i is set so that β _i =φ _i , and posture control of the tracing operation is performed.
The relationship of equation (12) is shown in FIG.

なお、上記実施例では、ならい動作方向の接線
決定のためのサンプリング距離L_iの決定にあた
り、一般式として(5)式、(6)式を用いたがΔθ_iが微
小角度であるとすれば、近似的に(5)式は Δθ^* _i+1≒L₀／L_i・Δθ_i+1 ……(13) と簡略化してもよく、さらに(6)式においても、 L_i+2≒ΔS₀／Δθ^* _i+1 ；｜Δθ^* _i+1｜≧Δφc L_i+2≒L₀ ；｜Δθ^* _i+1｜＜Δθc ……(14) と、tanΔθ_i+1≒Δθ_i+1の関係を用いて簡略化する
などの方法が考えられる。 In addition, in the above embodiment, formulas (5) and (6) were used as general formulas to determine the sampling distance L _i for determining the tangent line of the tracing motion direction, but if Δθ _i is a small angle, then , approximately, equation (5) can be simplified as Δθ ^* _i+1 ≒L ₀ /L _i・Δθ _i+1 ...(13), and furthermore, in equation (6), L _i+2 ≒ ΔS ₀ /Δθ ^* _i+1 ; |Δθ ^* _i+1 |≧Δφc L _i+2 ≒L ₀ ; |Δθ ^* _i+1 |<Δθc ...(14) and tanΔθ _i+1 ≒Δθ _i+ One possible method is to simplify using the relationship ₁ .

これらの式（13）、（14）において、Δθ^* _i+1、
Δφ^* _iなどを計算しないで、Δθ_i+1、Δφ_iの値から直
接的に求める簡略化も考えられる。例えば、前区
間のサンプリング距離をL_iとして、 L_i+1＝L_i−ｋΔθ_i／Δθc 同様に前区間の姿勢サンプリング距離をL′_iとし
て L′_i+1＝L′_i−k′Δθ_i／Δθc を求めることなどは、ｋ、k′を定数とした大まか
な線型近似であるが、これらによつても従来方式
よりはきめの細かい制御が可能であり、一つの簡
略法として有効である。さらに、(6)式において、
｜Δθ^* _i+1｜の値にかかわらずL_i+2＝ΔS₀／tanΔθ^* _i
＋１とし、また(2)式において、｜Δφ^* _i｜の値にかか
わらずL′_i+1＝Δφc・L′₀／Δφ_i ^*とするような方法
であつてもよい。 In these equations (13) and (14), Δθ ^* _i+1 ,
It is also conceivable to simplify the equation by directly finding the values of Δθ _i+1 and Δφ _i without calculating Δφ ^* _i , etc. For example, if the sampling distance of the previous section is L _i , L _i+1 = L _i −kΔθ _i /Δθc Similarly, if the posture sampling distance of the previous section is L′ _i , L′ _i+1 = L′ _i −k′Δθ Determining _i /Δθc is a rough linear approximation with k and k′ as constants, but these also allow finer control than conventional methods and are effective as a simplified method. be. Furthermore, in equation (6),
Regardless of the value of |Δθ ^* _i+1 |, L _i+2 =ΔS ₀ /tanΔθ ^* _i
+1 , and in equation (2), L′ _i+1 =Δφc·L′ ₀ /Δφ _i ^* regardless of the value of |Δφ ^* _i |.

前記実施例においては、水平方向のならい動作
のならい動作方向決定のための接線演算を行うサ
ンプリング距離と、垂直面内での姿勢決定のため
の姿勢サンプリング距離とをそれぞれL_i、L′_iのよ
うに区別して示したが、これらはL_i、L_i′のうち
の小さい方を両者に共通したサンプリング距離と
して選び、センサ２１，２２のデータを読み出す
回数を削減することも可能である。また、例えば
垂直面内での姿勢を制御することが、作業の仕様
から見てそれほど重要でない場合は、水平方向の
サンプリング距離のみを前述の方法によつて決定
し、これに合わせて垂直面内での姿勢制御のため
の計算を行つたり、あるいは逆の選択方法をとる
などの方法を用いてもよい。 In the embodiment described above, the sampling distance for performing the tangent calculation for determining the tracing motion direction of the horizontal tracing motion and the posture sampling distance for determining the posture in the vertical plane are defined as L _i and L′ _i, respectively. Although these are shown separately, it is also possible to select the smaller of Li _and Li _' as a common sampling distance for both, thereby reducing the number of times data from the sensors 21 and 22 is read. For example, if controlling the posture in the vertical plane is not so important from the work specifications, only the sampling distance in the horizontal direction can be determined using the method described above, and the sampling distance in the vertical plane can be determined accordingly. You may also use a method such as performing calculations for attitude control at , or using a reverse selection method.

さらに、前述の実施例においては、水平面内に
おけるならい動作と、垂直面内における姿勢制御
の例を示したが、ならい動作は水平面内に限定さ
れるものではなく、任意の平面または曲面内にお
いて行うものであつても、本発明は有効である。
ただし、ならい動作を行う平面または曲面内に、
２本のセンサ２１，２２が同時に含まれないよう
にセンサを配置する必要がある。このために、ロ
ボツト１の手先にセンサ２１，２２をセンサ２１
の主軸まわりに回転させるためのサーボ機構など
を追加する方法をとつてもよい。 Furthermore, in the above-mentioned embodiment, an example of a tracing motion in a horizontal plane and an attitude control in a vertical plane was shown, but the tracing motion is not limited to a horizontal plane, but can be performed in any plane or curved surface. The present invention is effective even if the
However, within the plane or curved surface where the tracing operation is performed,
It is necessary to arrange the sensors so that the two sensors 21 and 22 are not included at the same time. For this purpose, sensors 21 and 22 are installed at the hands of the robot 1.
It is also possible to add a servo mechanism or the like for rotation around the main axis.

上記実施例では、ロボツト１は直角座標形のロ
ボツトを例示しているが、多関節形、極座標形な
どのロボツトであつてもよく、またはレール上を
走行する走行台車などのような作業機械であつて
もよい。 In the above embodiment, the robot 1 is a rectangular coordinate type robot, but it may also be a multi-joint type robot, a polar coordinate type robot, or a working machine such as a traveling trolley that runs on rails. It's okay to be hot.

また、センサとして非接触式の近接距離センサ
を用いた例を示したが、センサとしては、ポテン
シヨメータ等をばねにより付勢した触針式セン
サ、超音波式距離センサ、レーザ光や赤外光など
によるスポツト光やスリツト光などをワーク表面
に照射しこれをイメージセンサやテレビカメラな
どでとらえて距離を検出する方式のセンサ、また
は画像処理によりワーク表面の位置を検出する方
式のセンサなど、種々の形式のセンサも用いるこ
とができる。 In addition, although we have shown an example of using a non-contact proximity distance sensor as a sensor, other sensors include a stylus sensor with a potentiometer etc. biased by a spring, an ultrasonic distance sensor, a laser beam or infrared sensor. Sensors that detect the distance by illuminating the work surface with spot light or slit light, which is captured by an image sensor or television camera, or sensors that detect the position of the work surface through image processing. Various types of sensors can also be used.

また、前述の実施例においては、センサ２１，
２２の主軸２１０，２２０がワークＷの表面に対
して常に垂直となる例を示したが、センサの姿勢
はワークの表面に対して一定の角度をなしても良
く、必ずしも完全に垂直な方向から距離検出を行
う必要はない。また、このようにセンサが一定の
姿勢を持つていても、本発明のならい動作制御は
有効である。 Further, in the above-described embodiment, the sensor 21,
Although the example in which the main axes 210 and 220 of No. 22 are always perpendicular to the surface of the workpiece W has been shown, the attitude of the sensor may be at a fixed angle with respect to the surface of the workpiece, and is not necessarily completely perpendicular to the surface of the workpiece. There is no need to perform distance detection. Further, even if the sensor has a fixed posture as described above, the tracing operation control of the present invention is effective.

以上、本発明の一実施例と、そのいくつかの変
形例について述べたが、本発明によれば、曲率が
複雑に変化する対象物に対して、その曲率に応じ
てならい動作のサンプリング距離を変えながらな
らい動作を行うため、曲率半径の小さいワーク部
分についても、その曲率半径の値に対応した円滑
かつ高精度のならい動作を実現できる。また、接
線の傾きが変化しなくとも、(6)式、(12)式に見られ
るように、サンプリング距離の最大値をそれぞれ
L₀、L₀′としているが、これはならい動作におい
て、一定の長さ以下の折れ線により対象物を近似
するものであり、曲率半径が大きい個所において
も、このようなならい制御により、きめの細かい
高精度のならい動作が可能である。一方、常に微
小距離ごとに接線計算を行う従来の方式では、そ
の接線計算の精度に対するセンサの計測精度の誤
差をまぬがれず誤差が蓄積されるのに対し、本発
明の方法では安定したならい動作制御が可能であ
る。 An embodiment of the present invention and some variations thereof have been described above. According to the present invention, for an object whose curvature changes in a complicated manner, the sampling distance of the tracing operation is determined according to the curvature. Since the tracing operation is performed while changing the radius of curvature, smooth and highly accurate tracing operations corresponding to the value of the radius of curvature can be achieved even for workpiece parts with a small radius of curvature. In addition, even if the slope of the tangent line does not change, the maximum value of the sampling distance can be calculated as shown in equations (6) and (12).
L ₀ and L ₀ ′, which approximates the object with a polygonal line of a certain length or less during the tracing operation, and even in areas with a large radius of curvature, this type of tracing control makes it possible to improve the texture. Fine, high-precision tracing operations are possible. On the other hand, with the conventional method that always calculates tangents for every minute distance, errors accumulate due to errors in the measurement accuracy of the sensor relative to the accuracy of tangent calculations, whereas the method of the present invention provides stable tracing motion control. is possible.

前述の実施例においては、その機能として、な
らい動作面内における接線検出およびならい動作
面と異なる面内（実施例では垂直面内）における
接線検出の２つが含まれているが、これらのうち
のならい動作平面内における機能のみを実現する
ものであつても、本発明の範囲内にあり、本発明
の簡略化例として十分実用になるものである。 In the above-mentioned embodiment, the functions include two functions: tangent detection within the tracing operation plane and tangent detection in a plane different from the tracing operation plane (in the embodiment, perpendicular plane). Even if the function is realized only within the tracing motion plane, it is still within the scope of the present invention and can be put into practical use as a simplified example of the present invention.

上記各実施例においては、それぞれの演算式か
らなる制御アルゴリズムを第３図計算機５１のソ
フトウエアとして具体化してもよいし、専用の電
子回路を付加して演算処理時間を短縮することも
できる。 In each of the embodiments described above, the control algorithm consisting of each calculation formula may be implemented as software of the computer 51 in FIG. 3, or a dedicated electronic circuit may be added to shorten the calculation processing time.

前述の各実施例においては、作業工具を図示し
ていないが、例えば塗布用弾性体、塗装ガン、溶
接トーチ、グラインダ、シーリング工具などの作
業工具をロボツトの手先に配置し、これら工具の
作業点位置をロボツト１の手先位置Ｐと一致させ
るか、手先位置Ｐに対して一定の位置関係を持つ
ように配置し、工具の作業点位置をロボツト１の
手先位置とみなして演算制御すると、作業工具に
よる自動ならい作業が可能である。もちろん、作
業工具を特に備えず、センサによる検出データか
らワークＷの形状を計算し、計測用ロボツトとし
て用いる用途に、または、このようにしてワーク
Ｗの形状をワーク上に沿つた点群の位置データと
して求め、産業用ロボツトにおけるテイーチング
作業に代え、このならい動作結果として得られた
動作経路に沿いプレイバツク動作制御によりロボ
ツトに作業を行わせる自動テイーチング方法に本
方法を応用することも可能である。 Although the working tools are not shown in each of the above-mentioned embodiments, working tools such as a coating elastic body, a painting gun, a welding torch, a grinder, and a sealing tool are arranged at the hands of the robot, and the working points of these tools are If the position of the tool is made to match the hand position P of the robot 1, or if it is arranged so that it has a certain positional relationship with the hand position P, and the work point position of the tool is calculated and controlled as the hand position of the robot 1, the work tool Automatic tracing work is possible. Of course, it is possible to calculate the shape of the workpiece W from the data detected by the sensor and use it as a measurement robot without having a working tool, or in this way, the shape of the workpiece W can be calculated from the position of the point cloud along the workpiece. It is also possible to apply this method to an automatic teaching method in which the robot performs a task by playback motion control along the motion path obtained as a result of the tracing motion, instead of obtaining the data as data and teaching the industrial robot.

〔発明の効果〕〔Effect of the invention〕

本発明によれば、指定面内方向ではセンサの移
動前後の出力から接線方向を検出するので、セン
サがひとつで済み、曲率が急激に変化する部分を
含めて、複雑な３次元形状を有する対象物を正確
にならい制御でき、対象物の最初の微小部分につ
いてならい動作を行なわせれば、そのならい動作
で得られた対象物の形状に関する情報に基づいて
形状を予測しながら自動的に測定するロボツトに
よる形状ならい制御方法が得られる。 According to the present invention, since the tangential direction is detected from the outputs before and after movement of the sensor in the specified in-plane direction, only one sensor is required, and the object has a complex three-dimensional shape, including a portion where the curvature changes rapidly. A robot that can accurately trace and control an object, and if it performs a tracing operation on the first minute part of the object, it can automatically measure and predict the shape of the object based on the information about the shape of the object obtained through the tracing operation. A shape tracing control method can be obtained.

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

第１図は本発明のならい制御装置の一実施例を
備えた作業機械としてのロボツトの一具体例を示
す斜視図、第２図は第１図ならい制御装置および
ロボツトの制御系の構成の一例を示すブロツク
図、第３図は第１図実施例における水平面内のな
らい動作制御方法を示す図、第４図は第３図方法
を一般化して示す図、第５図は本発明のならいサ
ンプリング距離L_i+1の選択にあたつて用いる
tanΔθ_i ^*とL_i+1の関係を示す図、第６図〜第８図
は第１図実施例における垂直面内の姿勢制御方法
を示す図、第９図は第６図〜第８図方法を一般化
して示す図、第１０図は同じく姿勢サンプリング
距離L′_i+1の選択に用いるΔφ_i ^*とL′_i+1との関係を
示す図である。 Ｗ…対象物、Ｐ…ロボツトの手先位置、１…ロ
ボツト、２１，２２，３１，３２，４１，４２…
センサ、５１…制御用計算機、５８…センサ用イ
ンターフエース。 Fig. 1 is a perspective view showing a specific example of a robot as a working machine equipped with an embodiment of the profiling control device of the present invention, and Fig. 2 is an example of the configuration of the profiling control device and robot control system shown in Fig. 1. FIG. 3 is a diagram showing the method of controlling the tracing motion in the horizontal plane in the embodiment of FIG. 1, FIG. 4 is a generalized diagram of the method shown in FIG. 3, and FIG. Used when selecting distance L _i+1
A diagram showing the relationship between tanΔθ _i ^* and L _i+1 , FIGS. 6 to 8 are diagrams showing the posture control method in the vertical plane in the embodiment of FIG. 1, and FIG. FIG. 10, which is a diagram generalizing the method, is a diagram showing the relationship between Δφ _i ^* and L' _{i+1, which are also used to select the posture sampling distance L' i+ 1} . W...Object, P...Robot hand position, 1...Robot, 21, 22, 31, 32, 41, 42...
Sensor, 51...Control computer, 58...Sensor interface.

Claims (1)

【特許請求の範囲】 １ 移動体の先端に取付けられ対象物表面との相
対距離を検出するセンサを指定面内における対象
物表面の接線に沿つて指定方向に設定移動距離だ
け移動させ、センサと対象物表面との相対距離お
よび移動体の３次元空間内の位置に基づいて対象
物表面の位置を求め、 前記設定移動距離両端に対応する対象物表面の
位置から当該対象物表面の指定面内における接線
を演算し直し、当該接線の指定方向の延長上で前
記設定移動距離だけ離れた点に次の動作目標点を
設定し、センサを当該動作目標点に向けて移動さ
せるならい制御方法において、 演算し直した接線とその前に演算された接線と
の方向の変化量に応じて前記設定移動距離を変更
することを特徴とするならい制御方法。 ２ 特許請求の範囲第１項に記載のならい制御方
法において、 前記指定面と交わる面内に前記センサを複数個
設け、当該センサの出力に基づき３次元形状の対
象物表面の前記交わる面内における法線方向を演
算し、前記３次元形状の対象物表面の法線に対し
て一定の角度以内となるように前記センサの方向
を規制することを特徴とするならい制御方法。 ３ 特許請求の範囲第１項に記載のならい制御方
法において、 前記センサの姿勢を前記指定面内における前記
接線に対して一定の角度となるように規制するこ
とを特徴とするならい制御方法。 ４ 移動体の先端に取付けられ対象物表面との相
対距離を検出するセンサと、 前記移動体を制御して前記センサを指定面内に
おける対象物表面の接線に沿つて動作目標点まで
移動させる制御手段と、 前記センサと対象物表面との相対距離および移
動体の３次元空間内の位置に基づき動作目標点に
対応した対象物表面の位置を求め、相続く２つの
動作目標点に対応する対象物表面の位置から指定
面内における対象物表面の接線を求め、当該接線
の指定方向の延長上で設定移動距離だけ離れた点
を次の動作目標点に設定して前記制御手段に出力
する第１演算手段と、 新たに演算する接線とその前に演算された接線
との方向の変化量に応じて前記設定移動距離を変
更し前記第１演算手段に出力する第２演算手段と を含むならい制御装置。[Claims] 1. A sensor attached to the tip of a moving body that detects the relative distance to the object surface is moved in a specified direction along a tangent to the object surface within a specified plane by a set moving distance, and the sensor The position of the object surface is determined based on the relative distance to the object surface and the position of the moving body in three-dimensional space, and the position within the designated plane of the object surface is determined from the position of the object surface corresponding to both ends of the set movement distance. In the tracing control method, the tangent line is recalculated, the next operation target point is set at a point separated by the set movement distance on the extension of the tangent line in the specified direction, and the sensor is moved toward the operation target point, A tracing control method characterized in that the set moving distance is changed according to the amount of change in direction between the recalculated tangent and the previously calculated tangent. 2. In the tracing control method according to claim 1, a plurality of the sensors are provided in a plane that intersects with the designated plane, and based on the outputs of the sensors, the surface of the three-dimensional object is controlled in the plane that intersects with the specified plane. A tracing control method comprising calculating a normal direction and regulating the direction of the sensor so that the direction is within a certain angle with respect to the normal to the surface of the three-dimensional object. 3. The tracing control method according to claim 1, characterized in that the attitude of the sensor is regulated to be at a constant angle with respect to the tangent in the specified plane. 4. A sensor attached to the tip of a moving object to detect the relative distance to the object surface; and a control for controlling the moving object to move the sensor along a tangent to the object surface within a specified plane to an operation target point. means, determining the position of the object surface corresponding to the movement target point based on the relative distance between the sensor and the object surface and the position of the moving object in the three-dimensional space; A tangent to the object surface within the specified plane is determined from the position of the object surface, and a point separated by a set travel distance on an extension of the tangent in the specified direction is set as the next operation target point and outputted to the control means. and a second calculation means that changes the set travel distance according to the amount of change in direction between a newly calculated tangent and a previously calculated tangent and outputs it to the first calculation means. Control device.