JP2592636B2 - Copying teaching control method - Google Patents
Copying teaching control methodInfo
- Publication number
- JP2592636B2 JP2592636B2 JP2135788A JP2135788A JP2592636B2 JP 2592636 B2 JP2592636 B2 JP 2592636B2 JP 2135788 A JP2135788 A JP 2135788A JP 2135788 A JP2135788 A JP 2135788A JP 2592636 B2 JP2592636 B2 JP 2592636B2
- Authority
- JP
- Japan
- Prior art keywords
- control
- force
- unit
- plane
- target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Numerical Control (AREA)
- Control Of Position Or Direction (AREA)
Description
【発明の詳細な説明】 〔目次〕 概要 産業上の利用分野 従来の技術 第４図，第８図，第９図 発明が解決しようとする問題点 第５図 問題点を解決するための手段 第１図 作用 第１図 実施例 第２図〜第８図 発明の効果 〔概要〕 制御対象と、当該制御対象について操作を行う操作部
と、前記制御対象の位置を検出する位置検出部と、前記
制御対象が受ける力の検出を行う力検出部とを有する系
で、前記制御対象を対象物の表面に沿って倣い動作をさ
せる場合に当該対象物の表面の教示を行う倣い教示制御
方式に関し、 倣い動作を行う場合に自動的に倣い動作を行う面の法
線方向とこれに垂直な移動方向を得ることができる倣い
教示制御方式を提供することを目的とし、 前記制御対象を前記対象物の表面で接触させた３点以
上の接触点の位置に基づいて当該接触点またはその近傍
を通る平面を認識して制御方向を算出する制御方向算出
部と、算出された制御方向、前記位置検出部により検出
された位置及び設定位置に基づいて倣い方向に速度指令
を行う位置制御手段と、算出された制御方向、検出され
た力及び設定力とに基づいて前記平面の法線方向に速度
指令を行う力制御手段とを有する構成である。Detailed Description of the Invention [Table of Contents] Overview Industrial application field Conventional technology FIGS. 4, 8, 9 Problems to be solved by the invention FIG. 5 Means for solving the problems 1 FIG. 1 FIG. 1 Embodiment Example FIGS. 2 to 8 Advantages of the Invention [Overview] A control target, an operation unit for performing an operation on the control target, a position detection unit for detecting the position of the control target, In a system having a force detection unit that performs detection of the force received by the controlled object, a copying teaching control method for teaching the surface of the object when the controlled object performs a copying operation along the surface of the object, An object of the present invention is to provide a scanning teaching control method capable of automatically obtaining a normal direction of a surface on which a copying operation is performed and a moving direction perpendicular to the normal direction when the copying operation is performed. Position of three or more contact points touched on the surface A control direction calculation unit that calculates a control direction by recognizing a plane passing through the contact point or in the vicinity thereof based on the calculated control direction, a position detected by the position detection unit, and a scanning direction based on the set position. It is configured to include a position control unit that issues a speed command, and a force control unit that issues a speed command in a direction normal to the plane based on the calculated control direction, the detected force, and the set force.
本発明はロボットが対象物表面に沿って加工等の動作
を行う倣い動作の倣い教示制御方式に係り、特に制御対
象と、当該制御対象について操作を行う操作部と、前記
制御対象の位置を検出する位置検出部と、前記制御対象
が受ける力の検出を行う力検出部とを有する系で、前記
制御対象を対象物の表面に沿って倣い動作をさせる場合
に当該対象物の表面に関する教示を行う倣い教示制御方
式に関する。The present invention relates to a copying teaching control method of a copying operation in which a robot performs an operation such as processing along a surface of an object, and in particular, detects a control object, an operation unit that performs an operation on the control object, and a position of the control object. In a system having a position detection unit to perform, and a force detection unit that detects a force received by the control target, when the control target performs a copying operation along the surface of the target object, teaching is performed on the surface of the target object. The present invention relates to a copying teaching control method.
従来、対象物の表面に沿っての加工、例えばバリ取
り、拭き取り、等の作業に力感覚を有するロボットを使
用することはすでに知られており、このときに用いる倣
い制御方式もすでに提案されている。Conventionally, it is already known to use a robot having a force sensation for operations such as deburring, wiping, etc. along a surface of an object, and a copying control method used at this time has already been proposed. I have.
第４図及び第８図に示すように平面S1上で倣い動作の
制御を行わせるには制御対象としてのマニピュレータ11
が設置されているロボット基準座標系（Xr,Yr,Zr）とハ
ンド座標系（Xh,Yh,Zh）とでの倣い動作の開始地点Ａと
終了地点Ｂとの座標値、移動方向、当該平面と接触を保
つために平面に加える押し付け力の大きさ及び当該平面
の法線方向を指示する必要がある。Figure 4 and 8 manipulator 11 as a control target to carry out the control of the copying operation on the plane S 1 as shown in FIG.
There robot reference coordinate system is installed (X r, Y r, Z r) and the hand coordinate system (X h, Y h, Z h) coordinate value of the end point B and the starting point A of the copying operation of the de and , The direction of movement, the magnitude of the pressing force applied to the plane to maintain contact with the plane, and the direction of the normal to the plane.
従来、このような移動方向及び平面の法線方向は、例
えば水平方向に広がった平面S1等の単純な平面で行う
か、対象物の表面を計測することにより予め当該表面の
法線ベクトルや移動方向のベクトルを調べて与えるよう
にしていた。Conventionally, the normal direction of such movement direction and plane, for example, whether to perform a simple plane, such as plane S 1 spread horizontally, Ya normal vector in advance the surface by measuring the surface of the object The vector of the moving direction was checked and given.
すなわち、従来の倣い教示制御方式にあっては第９図
に示すように倣い動作等の動作を行うマニピュレータ81
と、当該マニピュレータ81の操作を行う操作部82と、マ
ニピュレータ81の現在位置を検出する位置検出部85と、
マニピュレータ81が受ける力の検出を行う力検出部86
と、移動方向に基づいて前記マニピュレータ81の移動方
向の速度指令を行う位置制御手段83と、表面の法線方向
への速度指令を行う力制御手段84とを有するものであ
る。That is, in the conventional copying teaching control method, as shown in FIG.
An operation unit 82 that operates the manipulator 81, a position detection unit 85 that detects the current position of the manipulator 81,
Force detector 86 for detecting the force received by manipulator 81
And a position control means 83 for giving a speed command in the movement direction of the manipulator 81 based on the movement direction, and a force control means 84 for giving a speed command in the normal direction of the surface.
本例により所定の対象物の表面について倣い動作を行
うには前記位置制御手段85に前記位置検出部85が検出し
た位置及び設定位置を送出するとともに予め調べた倣い
動作を行う平面上の移動方向を設定しておき、前記力制
御手段84に対しても前記力検出部86が検出した力及び設
定力を送出するとともに、予め調べた前記倣い動作の行
われる平面の法線方向を設定して、所定対象物の表面に
対して倣い動作の教示の制御を行うようにしていた。In order to perform the copying operation on the surface of the predetermined object according to this example, the position detected by the position detection unit 85 and the set position are sent to the position control unit 85 and the movement direction on the plane where the copying operation checked in advance is performed. Is set, and the force and the set force detected by the force detection unit 86 are also sent to the force control unit 84, and the normal direction of the plane on which the copying operation, which is checked in advance, is performed is set. In addition, the teaching of the copying operation is controlled on the surface of the predetermined object.
ところで、従来の倣い動作教示制御方式にあっては、
操作員が倣い動作を行う対象物の表面の平面方程式や当
該平面の法線ベクトル等を調べて前記制御系に倣い動作
の教示を行う必要がある。そのため、例えば第５図に示
すようなロボット上に設定されたロボット基準座標系の
中で任意に置かれた平面や複雑な形状を有する曲面に対
して倣い動作を行うような場合には単純な平面の場合と
異なり複雑な計測を行う必要があり、手間がかかるとい
う問題点を有していた。By the way, in the conventional copying operation teaching control method,
It is necessary for the operator to teach the control system the copying operation by examining the plane equation of the surface of the object on which the copying operation is performed, the normal vector of the plane, and the like. Therefore, for example, when performing a copying operation on a plane or a curved surface having a complicated shape arbitrarily placed in a robot reference coordinate system set on the robot as shown in FIG. Unlike the case of a flat surface, it is necessary to perform complicated measurement, and there is a problem that it takes time and effort.
そこで、本発明は以上の問題点を解決するためになさ
れたものであり、倣い動作を行う場合に自動的に倣い動
作を行う面の法線方向とこれに垂直な移動方向を得るこ
とができる倣い教示制御方式を提供することを目的とし
てなされたものである。Therefore, the present invention has been made to solve the above problem, and when performing a copying operation, a normal direction of a surface on which the copying operation is automatically performed and a moving direction perpendicular to the normal direction can be obtained. The object of the present invention is to provide a copying teaching control method.
以上の問題点を解決するため本発明は第１図に示すよ
うに対象物に対する加工等の動作を行う制御対象１と、
当該制御対象１について操作を行う操作部２と、前記制
御対象１の位置を検出する位置検出部５と、前記制御対
象１が受ける力の検出を行う力検出部６とを有する系
で、前記制御対象１が対象物の表面に沿って倣い動作を
させる場合に当該対象物の表面に関する教示を行う倣い
教示制御方式において、前記制御対象１を前記対象物の
表面で接触させた３点以上の接触点の位置に基づいて当
該接触点またはその近傍を通る平面を認識して前記制御
対象１の制御方向を算出する制御方向算出部９と、算出
された制御方向、前記位置検出部５により検出された位
置及び設定位置に基づいて前記平面に沿った倣い方向に
速度指令を行う位置制御手段３と、算出された制御方
向、検出された力及び設定力とに基づいて前記平面の法
線方向に速度指令を行う力制御手段４とを有するもので
ある。In order to solve the above problems, the present invention provides a control target 1 for performing operations such as processing on an object as shown in FIG.
A system including an operation unit 2 for performing an operation on the control target 1, a position detection unit 5 for detecting a position of the control target 1, and a force detection unit 6 for detecting a force received by the control target 1; In the copying teaching control method for performing teaching related to the surface of the target object when the control target 1 performs the copying operation along the surface of the target object, three or more points where the control target 1 is brought into contact with the surface of the target object A control direction calculating section for recognizing a plane passing through or near the contact point based on the position of the contact point and calculating a control direction of the control target; a calculated control direction, detected by the position detecting section; Position control means 3 for giving a speed command in the scanning direction along the plane based on the set position and the set position, and a normal direction of the plane based on the calculated control direction, the detected force and the set force. Speed command Those having a force control unit 4.
本発明により対象物の表面に対して制御対象１に倣い
動作をさせる場合には、操作部２により当該制御対象１
に対して所定の操作量を送出して当該対象物の前記表面
に接触させる。When the control object 1 is caused to follow the surface of the object according to the present invention, the control unit 1
And sends a predetermined operation amount to the object to make contact with the surface of the object.
当該制御対象１が前記対象物に接触すると前記力検出
部６により当該制御対象１が受けた力が検出され、力制
御手段４により接触が認識されると、前記位置検出部５
は当該接触した際の前記制御対象１の位置を検出して当
該位置座標を出力する。この動作を倣い動作を行おうと
する表面上の少なくとも３箇所の異なる点で繰り返し
て、最低三つの接触点の位置座標を得る。When the control target 1 contacts the object, the force detection unit 6 detects the force received by the control target 1, and when the contact is recognized by the force control unit 4, the position detection unit 5
Detects the position of the control target 1 at the time of the contact and outputs the position coordinates. This operation is repeated at least at three different points on the surface on which the copying operation is to be performed to obtain the position coordinates of at least three contact points.
すると、前記制御方向算出部９は三つの当該接触点に
基づいて当該接触点またはその近傍点が通る平面を認識
し、前記制御対象１の制御方向（平面の法線方向及び当
該方向に垂直な移動方向）を算出し、前記位置制御手段
３及び前記力制御手段４に送出する。Then, the control direction calculation unit 9 recognizes a plane through which the contact point or a point near the contact point is based on the three contact points, and determines the control direction of the control target 1 (the normal direction of the plane and the direction perpendicular to the direction). The moving direction is calculated and sent to the position control means 3 and the force control means 4.
当該位置検出手段３は当該制御方向及び前記位置検出
部５が検出した現在の位置及び設定位置に基づいて前記
制御対象１の平面に沿った方向に速度指令を行い、前記
力制御手段４は前記力検出部６が検出した現在受けてい
る力、設定力及び前記制御方向算出部９が算出した制御
方向に基づいて表面の法線方向に対する速度指令を行う
ことになる。The position detection means 3 issues a speed command in a direction along the plane of the control target 1 based on the control direction and the current position and the set position detected by the position detection unit 5, and the force control means 4 Based on the force currently being detected by the force detector 6, the set force, and the control direction calculated by the control direction calculator 9, a speed command is issued in the direction normal to the surface.
次に本実施例について説明する。 Next, this embodiment will be described.
本実施例は第２図に示すように制御対象１としてのマ
ニピュレータ11と、当該マニピュレータ11の制御を行う
操作部12と、前記マニピュレータ11の位置の検出を行う
位置検出部15と、前記マニピュレータ11のハンド部11a
（第４図及び第５図に示す）が受ける力の検出を行う力
検出部16と、前記位置検出部15が検出した位置に基づい
て制御方向の算出を行う制御方向算出部19と、当該算出
部19により算出された制御方向、前記位置検出部15によ
り検出された位置及び設定位置に基づいて前記マニピュ
レータ11の倣い動作方向への速度指令を行う位置制御手
段13と、前記力検出部16により検出された力、設定力及
び前記算出部19により算出された制御方向としての平面
の法線方向に基づいて前記マニピュレータ11に対して法
線方向への速度指令を行う力制御手段14とを有してい
る。In the present embodiment, as shown in FIG. 2, a manipulator 11 as a control target 1, an operation unit 12 for controlling the manipulator 11, a position detection unit 15 for detecting the position of the manipulator 11, and the manipulator 11 Hand part 11a
A force detection unit 16 for detecting a force received by the control unit (shown in FIGS. 4 and 5), a control direction calculation unit 19 for calculating a control direction based on the position detected by the position detection unit 15, A position control unit 13 for issuing a speed command to the copying operation direction of the manipulator 11 based on the control direction calculated by the calculation unit 19, the position detected by the position detection unit 15 and the set position, and the force detection unit 16 And force control means 14 for issuing a speed command in the normal direction to the manipulator 11 based on the normal direction of the plane as the control direction calculated by the calculation unit 19 and the force detected by the control unit. Have.
前記制御方向算出部19は前記位置検出部15が検出した
各関節部の相対角度で表示された座標をロボット基準系
の座標に変換する位置姿勢演算部19aと、前記位置検出
部15が検出した前記対象物の表面上で接触した３点の位
置座標を一時保持する位置座標記憶部19aと、当該３点
の接触点に基づいて平面方程式を決定して制御方向を算
出する制御方向算出部19Cとを有する。The control direction calculation unit 19 is a position and orientation calculation unit 19a that converts coordinates represented by the relative angle of each joint detected by the position detection unit 15 into coordinates of a robot reference system, and the position detection unit 15 detects A position coordinate storage unit 19a for temporarily storing position coordinates of three points touched on the surface of the object, and a control direction calculation unit 19C for determining a plane equation based on the three contact points and calculating a control direction And
さらに前記操作部12はサーボ・モータ12aと、パワー
・アンプ12bと、D/Aコンバータ12cと、補償器12dとを有
している。Further, the operation unit 12 has a servo motor 12a, a power amplifier 12b, a D / A converter 12c, and a compensator 12d.
また、前記位置検出部15はカウンタ及びエンコーダ15
aとタコ・メータ15bとを有している。Further, the position detection unit 15 includes a counter and an encoder 15.
a and a tachometer 15b.
続いて前記力検出部16は力覚センサ16aとハンド座標
系からロボット基準座標系への座標変換部16bとを有し
ている。Subsequently, the force detector 16 includes a force sensor 16a and a coordinate converter 16b for converting a hand coordinate system to a robot reference coordinate system.
さらに、前記力制御手段14及び前記位置制御手段13か
ら出力された速度についての加算を行う加算部10bと、
加算された速度をマニピュレータの各関節の角速度θに
変換する逆ヤコビ変換部10aとを有している。Further, an addition unit 10b that performs addition about the speed output from the force control unit 14 and the position control unit 13,
An inverse Jacobian converter 10a for converting the added velocity into an angular velocity θ of each joint of the manipulator.
力制御手段14は力制御部14bと偏差部14aとから成り、
位置制御手段13は偏差部13aと位置制御部13bとから成
る。The force control means 14 includes a force control unit 14b and a deviation unit 14a,
The position control means 13 includes a deviation section 13a and a position control section 13b.
次に、前記位置制御部13b及び力制御部14bの詳細を第
３図に示す。Next, details of the position control unit 13b and the force control unit 14b are shown in FIG.
位置制御部13bは転置直交変換行列（RT）演算部31
と、選択行列（Ｉ−Sf）演算部32と、直交行列（Ｒ）演
算部33と、位置フィードバックゲイン（Cp）演算部34と
を有する。The position control unit 13b includes a transposed orthogonal transformation matrix (R T ) calculation unit 31
And a selection matrix (I- Sf ) operation unit 32, an orthogonal matrix (R) operation unit 33, and a position feedback gain ( Cp ) operation unit 34.
また、力制御部14bは転置直交行列（RT）演算部38
と、選択行列（Sf）演算部37と、他の選択行列（Ｉ−
Sf）演算部40と、不感帯演算部39と、直交行列演算部36
と、位置フィードバックゲイン演算部35とを有する。Further, the force control unit 14b includes a transposed orthogonal matrix (R T ) calculation unit 38
, A selection matrix (S f ) operation unit 37, and another selection matrix (I−
S f ) Operation section 40, dead zone operation section 39, orthogonal matrix operation section 36
And a position feedback gain calculator 35.
ここで、第６図に示すように対象物への押し付け力を
示す力指定FOで与えられる力制御方向の単位ベクトル
（倣い動作がわれる対象物の表面の法線ベクトル）を
“”、倣い方向の移動速度指令VOで与えられる位置制
御方向の単位ベクトル“”で表したとき、位置制御さ
れるさらにもう１つの直交ベクトル“”は ＝× で与えられる。Here, the designated force F O at given force control direction of the unit vector indicating the pressing force to the object as shown in FIG. 6 (the normal vector of the surface of the object copying operation is cracked) "", following When represented by a unit vector “” in the position control direction given by the direction moving speed command V O , another orthogonal vector “” to be position-controlled is given by:
これを各々ロボット基準座標系による成分表示を行う
と、 ＝〔nx,ny,nz〕Ｔ ＝〔ox,oy,oz〕Ｔ となる。ここで、Ｔは転置行列を示す。Doing each component displayed by the robot reference coordinate system to this, = [n x, n y, n z] T = [o x, o y, o z] T Becomes Here, T indicates a transposed matrix.
これらのベクトルを用いて当該ロボット基準座標系
（x,y,z）から倣い座標系（ｘ′,y′,z′）への座標変
換を表示する直交座標変換行列Ｒは次のように表される
ことになる。Using these vectors, an orthogonal coordinate transformation matrix R indicating the coordinate transformation from the robot reference coordinate system (x, y, z) to the scanning coordinate system (x ′, y ′, z ′) is expressed as follows. Will be done.
第６図において、当該倣い座標系（ｘ′,y′,z′）の
ｘ′方向を力制御方向とし、ｙ′及びｚ′方向を位置制
御方向とすることで選択行列演算部32,37,40の選択行列
は次の式に表示される。 In FIG. 6, the x 'direction of the scanning coordinate system (x', y ', z') is set as the force control direction, and the y 'and z' directions are set as the position control directions. , 40 are shown in the following equation.
で与えられる。この場合、一般に例えばねじ締め、缶の
蓋締め等押し付け方向の軸まわりにトルクを与えるので
トルクを与える場合にはＳ＝１とし、与えない場合には
Ｓ＝０とする。 Given by In this case, in general, torque is applied around the axis in the pressing direction such as screwing or can lid tightening, so that S = 1 when applying torque, and S = 0 when not applying torque.
力フィードバックゲイン演算部cfは基準座標系に関し
て、 で与えられる。The force feedback gain calculation unit c f Given by
また、位置フィードバックゲインcpは同様にして で与えられる。Similarly, the position feedback gain c p Given by
選択行列のＩは単位行列を表している。 I in the selection matrix represents a unit matrix.
さらに、本実施例では不感帯演算部39を設けている。
当該演算部39は倣い動作中に位置制御方向が対象物接触
点における接平面に一致していない場合に対象物表面に
多大な摩擦力が生じて表面をかじり、対象物表面あるい
はワークに損傷を与えるのを防止することができるもの
である。すなわち、対象物の接触点に許容範囲以上の力
が作用したときには位置制御方向にも力フィードバック
を作用させるためである。Further, in this embodiment, a dead zone calculation section 39 is provided.
When the position control direction does not coincide with the tangent plane at the object contact point during the copying operation, the arithmetic unit 39 generates a large frictional force on the surface of the object and bites the surface, thereby damaging the object surface or the work. It can prevent giving. That is, when a force exceeding the allowable range acts on the contact point of the object, force feedback is also exerted in the position control direction.
本実施例の作用を説明する。 The operation of the present embodiment will be described.
例えば第５図のように置かれた平面に対して倣い動作
を行う場合について述べる。For example, a case where a copying operation is performed on a plane placed as shown in FIG. 5 will be described.
マニピュレータ11のハンド部11aを対象物の表面に接
触するまで近づける。ハンド部11aが点Ａに接触する
と、第８図に示すように前記力覚センサ16aは接触した
際に受けた力をハンド座標系（Xh,Yh,Zh）の成分Fhとし
て検出し、座標変換部16bに送出して力の成分をロボッ
ト基準座標系（Xr,Yr,Zr）で表示した成分Frに変換す
る。The hand section 11a of the manipulator 11 is brought close to contact with the surface of the object. When the hand portion 11a is in contact with the point A, detected the force sensor 16a as shown in FIG. 8 is a force received when contacted hand coordinate system (X h, Y h, Z h) as component F h of Then, the force component is sent to the coordinate conversion unit 16b to convert the force component into a component Fr displayed in the robot reference coordinate system ( Xr , Yr , Zr ).
当該Frは偏差部14aにより、力指令FOとの偏差が取ら
れ力制御部14bに送出される。The F r is the deviation portion 14a, the deviation between the force command F O is sent to the taken force control unit 14b.
その接触された際のハンド部11aの位置姿勢はエンコ
ーダ及びカウンタ15aにより検出された各関節角θＳか
ら位置姿勢演算部18により算出される。The position and orientation of the hand portion 11a when the contacted is calculated by the position and orientation calculation unit 18 from the joint angle theta S detected by the encoder and counter 15a.
こうして得られた接触位置は前記制御方向算出部19の
位置座標記憶部19aに記憶される。The contact position thus obtained is stored in the position coordinate storage unit 19a of the control direction calculation unit 19.
同様にして前記対象物の表面の位置座標が３点得られ
ると、当該位置座標は制御方向演算部19bに送出される
ことになる。Similarly, when three position coordinates on the surface of the object are obtained, the position coordinates are sent to the control direction calculation unit 19b.
当該制御方向算出部19bにおいては、次のようにして
自動的に前記３点により定まる平面方程式を得ることに
なる。The control direction calculation section 19b automatically obtains a plane equation determined by the three points as follows.
すなわち、第５図に示すように、当該３点A,B,Cの位
置座標が各々、ａ＝（a1,a2,a3）、ｂ＝（b1,b2,b3）、
ｃ＝（c1,c2,c3）のように検出されると、各点の位置ベ
クトル＝（ax,ay,az）、＝（bx,by,bz）、＝
（cx,cy,cz）が決まる。That is, as shown in FIG. 5, the position coordinates of the three points A, B, and C are respectively a = (a 1 , a 2 , a 3 ), b = (b 1 , b 2 , b 3 ),
When detected as c = (c 1 , c 2 , c 3 ), the position vector of each point = (a x , a y , a z ), = (b x , b y , b z ), =
(C x , c y , c z ) is determined.
これにより３点A,B,Cを通る平面S2平面方程式 ＝ｒ＋ｓ＋ｔ（ただし、ｒ＋ｓ＋ｔ＝１） 及び平面S2の垂直単位ベクトル ｎ＝（−）×（−）/|（−）×（−）
｜が算出されることになる。Thus three points A, B, plane S 2 plane equation passing through the C = r + s + t (provided that, r + s + t = 1 ) and vertical unit vectors in the plane S 2 n = (-) × (-) / | (-) × (- )
| Is calculated.
さらに移動速度指令VOで与えられる位置制御方向の単
位ベクトルを指定することにより前記直交変換行列Ｒ
及び転置直交変換行列RTが式から求められることにな
る。Further, by specifying a unit vector in the position control direction given by the moving speed command V O , the orthogonal transformation matrix R
And the transposed orthogonal transformation matrix RT is obtained from the equation.
こうして得られたR,RTは前述した位置制御部13a及び
力制御部14bの（転置）直交変換行列演算部33,31,36,38
に送出され、前記対象物の表面に沿って倣い動作が行わ
れることになる。The R and R T obtained in this manner are used as the (transposed) orthogonal transformation matrix operation units 33, 31, 36, and 38 of the position control unit 13a and the force control unit 14b.
And the copying operation is performed along the surface of the object.
すなわち、力制御部14bから出力される速度指令Vfは
式，，及び出力Frと力指令FOとの偏差により Vf＝cfRSfRT（FO−Fr） … また、位置制御部13bから出力される速度指令Vpは式
，，及び現在位置xrと目標位置xOとの偏差によっ
て、 vp＝cpR（Ｉ−Sf）RT（xO−xr） … で与えられる。 That, V f = c f RS f R T (F O -F r) is the speed command V f output from the force control unit 14b according to a deviation between the expression ,, and the output F r and the force command F O ... Also, speed command V p which is output from the position control section 13b by the deviation between the expression ,, and the current position x r and the target position x O, v p = c p R (I-S f) R T (x O -x r )… is given by
この場合に、力制御部14bにおいて、位置制御方向に
作用する力も監視するため、選択行列演算部40と不感帯
演算部39を設ける。これらの手段によって、倣い動作中
に位置制御方向が対象物接触点における接平面に一致し
ていない場合に、対象物表面あるいはワークに損傷を与
えるのを防止することができるものである。すなわち、
対象物の接触点に許容範囲以上の力が作用した場合には
位置制御方向にも力フィードバックを作用させるためで
ある。In this case, in order to monitor the force acting in the position control direction in the force control unit 14b, a selection matrix calculation unit 40 and a dead zone calculation unit 39 are provided. By these means, it is possible to prevent the surface of the object or the work from being damaged when the position control direction does not coincide with the tangent plane at the object contact point during the copying operation. That is,
This is because when a force exceeding the allowable range acts on the contact point of the object, force feedback is also applied in the position control direction.
第３図において、位置制御方向に作用する力としての
監視力Fcrは Fcr＝（Ｉ−Sf）RT（FO−Fr） で与えられ、倣い座標系で表示した力の許容範囲Ur（＞
０）を不感帯巾と設定した不感帯演算部39において、 |Fcr|≦Ur のとき、即ち、監視力が力の許容範囲より小さいとき
は、式及びを加算した速度指令Ｖが得られ、 |Fcr|＞Ur のとき、即ち、監視力が力の許容範囲より大なるとき
は、力制御部14bの速度指令Vfは Vf＝cfR｛SfRT（FO−Fr）＋Fcra｝ で与えられる。In FIG. 3, a monitoring force F cr as a force acting in the position control direction is given by F cr = (I−S f ) R T (F O −F r ), and the allowable force expressed in the scanning coordinate system is given. Range U r (>
0) is set as the dead band width, and when | F cr | ≦ U r , that is, when the monitoring force is smaller than the allowable range of the force, a speed command V obtained by adding the expression and When | F cr |> U r , that is, when the monitoring force is larger than the allowable range of the force, the speed command V f of the force control unit 14b is V f = c f R ｛S f R T (F O − F r ) + F cra ｝.
この場合Fcraは力の許容範囲と監視力との大小に応じ
て Fcra＝Fcr−Uf（Fcr＞０） ＝Fcr＋Uf（Fcr＜０） で与えられる。当然、位置制御部13bにおける速度指令
Ｖp;は式で良い。In this case, F cra is given by F cra = F cr −U f (F cr > 0) = F cr + U f (F cr <0) according to the magnitude of the allowable range of the force and the monitoring force. Of course, the speed command Vp; in the position control unit 13b may be an expression.
こうして、得られた速度指令Ｖは逆ヤコビ行列演算部
10aでθ＝J-1・Ｖに基づいて、関節速度θが求められ、
この関節速度は補償器12dに入力されD/A変換された後、
増幅されてサーボ・モータ12aを駆動する。The speed command V obtained in this manner is converted into an inverse Jacobi matrix operation unit.
At 10a, the joint velocity θ is obtained based on θ = J −1 · V,
This joint speed is input to the compensator 12d and D / A converted.
It is amplified and drives the servo motor 12a.
尚、第７図に示すように対象物の表面が任意の曲面S3
の場合には、当該曲面S3を分割して得た微小曲面S3′を
取り出してS3′について上述した任意平面S2に対して行
った操作と同様の操作を行って他の分割した微小曲面と
合わせることによって、曲面S3全体への倣いを行うよう
にすると良い。つまり、S3の微小曲面S3′を近似的に平
面とみなし、前述した平面の場合と同様に法線方向及び
移動方向を得て、点A,B,Cで決まる平面に対する倣い動
作を行う。Incidentally, as shown in FIG. 7, the surface of the object has an arbitrary curved surface S 3.
In the case of, by performing the same operation as operation performed on any plane S 2 described above with reference to the curved surface S 3 a 'is taken out S 3' micro curved surface S 3 obtained by dividing the other divisional by combining a small curved surface, the curved surface S 3 may be to perform the copying of the whole. That is, regarded as approximately planar micro curved surface S 3 of S 3 ', performed to give a normal direction and the moving direction as in the case of the plane mentioned above, the points A, B, the copying operation for the plane defined by the C .
実際には面S3′は曲面なので点A,B,Cで決る平面と面S
3′との差に応じて面S3′に加わる力にむらが生じる。
従って、力のむらが生じても面S3′と常に接触を保つよ
うに押し付け力の指示を行うことになる。Actually, since the surface S 3 ′ is a curved surface, the plane determined by the points A, B, and C and the surface S
The force applied to the surface S 3 ′ varies according to the difference from 3 ′.
Therefore, the pressing force is instructed so as to always keep contact with the surface S 3 ′ even if the unevenness of the force occurs.
同様の操作を他の微小曲面について行い、各々の移動
方向、法線方向につなげることにより曲面S3全体の倣い
を行うことができることになる。The same operation is performed for other micro curved surface, each of the moving direction, so that it is possible to perform the copying of the entire curved surface S 3 by connecting the normal direction.
以上説明したように本発明では倣い動作を行う場合
に、その対象物表面で３点以上の位置座標を求め、当該
座標から対象物表面の平面を自動的に教示させて倣い動
作を行うようにしているため、任意の位置または形状を
している面に対しても容易かつ信頼性の高い倣い動作を
行うことができる。As described above, according to the present invention, when performing a copying operation, three or more position coordinates are obtained on the surface of the object, and the surface of the object surface is automatically taught from the coordinates to perform the copying operation. Therefore, an easy and highly reliable copying operation can be performed even on a surface having an arbitrary position or an arbitrary shape.
第１図は本発明の原理ブロック図、第２図は実施例に係
るブロック図、第３図は実施例に係る位置制御部及び力
制御部を示す図、第４図は倣い動作の軌跡を示す図、第
５図は倣い動作の行われる平面を示す図、第６図はロボ
ット基準座標系における単位ベクトルを説明する図、第
７図は曲面の場合の説明図、第８図はロボット基準座標
系とハンド座標系とを示す図、第９図は従来例に係るブ
ロック図である。 1,11……マニピュレータ 3,13……位置制御手段 4,14……力制御手段 5,15……位置検出部 6,16……力検出部 9,19……制御方向算出部1 is a block diagram illustrating the principle of the present invention, FIG. 2 is a block diagram according to an embodiment, FIG. 3 is a diagram illustrating a position control unit and a force control unit according to the embodiment, and FIG. FIG. 5, FIG. 5 is a diagram showing a plane on which a copying operation is performed, FIG. 6 is a diagram for explaining a unit vector in a robot reference coordinate system, FIG. 7 is an explanatory diagram for a curved surface, and FIG. FIG. 9 shows a coordinate system and a hand coordinate system, and FIG. 9 is a block diagram according to a conventional example. 1,11 manipulator 3,13 position control means 4,14 force control means 5,15 position detection unit 6,16 force detection unit 9,19 control direction calculation unit
───────────────────────────────────────────────────── フロントページの続き (72)発明者 西本 克史 神奈川県川崎市中原区上小田中1015番地 富士通株式会社内 (56)参考文献 特開 昭54−72380（ＪＰ，Ａ) 特開 昭59−49607（ＪＰ，Ａ) ────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Katsushi Nishimoto 1015 Uedanaka, Nakahara-ku, Kawasaki City, Kanagawa Prefecture Inside Fujitsu Limited (56) References JP-A-54-72380 (JP, A) JP-A-59-49607 (JP, A)
Claims (1)
象（１）と、当該制御対象（１）について操作を行う操
作部（２）と、前記制御対象（１）の位置を検出する位
置検出部（５）と、前記制御対象（１）が受ける力の検
出を行う力検出部（６）とを有する系で、前記制御対象
（１）を対象物の表面に沿って倣い動作をさせる場合に
当該対象物の表面に関する教示を行う倣い教示制御方式
において、 前記制御対象（１）を前記対象物の表面で接触させた３
点以上の接触点の位置に基づいて当該接触点またはその
近傍を通る平面を認識して前記制御対象（１）の制御方
向を算出する制御方向算出部（９）と、 算出された制御方向、前記位置検出部（５）により検出
された位置及び設定位置に基づいて前記平面に沿った倣
い方向に速度指令を行う位置制御手段（３）と、 算出された制御方向、検出された力及び設定力とに基づ
いて前記平面の法線方向に速度指令を行う力制御手段
（４）とを有することを特徴とする倣い教示制御方式。1. A control object (1) for performing an operation such as processing on an object, an operation unit (2) for operating the control object (1), and a position for detecting a position of the control object (1). A system having a detection unit (5) and a force detection unit (6) for detecting a force received by the control target (1) causes the control target (1) to follow the surface of the target object. In the copying teaching control method for performing teaching on the surface of the object in the case, the control object (1) is brought into contact with the surface of the object.
A control direction calculating unit (9) for recognizing a plane passing through or near the contact point based on the position of the contact point at or above the point and calculating a control direction of the control target (1); A position control means (3) for issuing a speed command in a scanning direction along the plane based on the position detected by the position detection section (5) and the set position, a calculated control direction, a detected force and a setting And a force control means (4) for giving a speed command in a direction normal to the plane based on the force.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2135788A JP2592636B2 (en) | 1988-02-02 | 1988-02-02 | Copying teaching control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2135788A JP2592636B2 (en) | 1988-02-02 | 1988-02-02 | Copying teaching control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH01197811A JPH01197811A (en) | 1989-08-09 |
| JP2592636B2 true JP2592636B2 (en) | 1997-03-19 |
Family
ID=12052839
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2135788A Expired - Fee Related JP2592636B2 (en) | 1988-02-02 | 1988-02-02 | Copying teaching control method |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2592636B2 (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2791030B2 (en) * | 1988-03-01 | 1998-08-27 | 日立建機株式会社 | Curved copying controller for multi-degree-of-freedom work machine |
-
1988
- 1988-02-02 JP JP2135788A patent/JP2592636B2/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH01197811A (en) | 1989-08-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4054984B2 (en) | Robot control apparatus and control method | |
| JP2592636B2 (en) | Copying teaching control method | |
| JP2613076B2 (en) | Copying teaching control method | |
| JPH01146645A (en) | Profile control system | |
| JP2791030B2 (en) | Curved copying controller for multi-degree-of-freedom work machine | |
| JP4134369B2 (en) | Robot control device | |
| JP2737325B2 (en) | Robot trajectory generation method | |
| JP2739764B2 (en) | Robot scanning speed control method | |
| JP2691591B2 (en) | Control system of force control robot | |
| JP2773778B2 (en) | Copying device for force control robot | |
| JP2736925B2 (en) | Robot force control method | |
| JP2751967B2 (en) | Copying control device | |
| JP3078884B2 (en) | Copying control device | |
| JP3089027B2 (en) | Robot profiling control device | |
| JP2718687B2 (en) | Control device for position and force of multi-degree-of-freedom work machine | |
| JP2830205B2 (en) | Robot control device | |
| JP2832903B2 (en) | Coordinate switching method for scanning control | |
| JP2737326B2 (en) | Robot trajectory search method | |
| JP3350687B2 (en) | Robot control method and robot control device | |
| JP3196145B2 (en) | Contact point and contact normal detection method for machining / assembly equipment | |
| JP2628846B2 (en) | Industrial robot control method and apparatus | |
| KR0155281B1 (en) | Interpolation method of multi-robot | |
| JPH07205068A (en) | Coordinate system setting method of robot | |
| JP2650889B2 (en) | Articulated robot teaching method | |
| JP3001211B2 (en) | Copying control device for arbitrary surface |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |