JP3196145B2 - Contact point and contact normal detection method for machining / assembly equipment - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a tool or a part to be assembled in a machining and assembling apparatus used for various operations such as a copying operation for processing along the surface of an object to be processed and an assembly involving contact between parts. And a method for detecting a contact point and a contact normal.

[0002]

2. Description of the Related Art As a conventional technique for detecting a contact point between a tool / assembly target part of a machining / assembly apparatus and the outside, a case where a robot arm is considered as an apparatus is described in, for example, "Journal of the Robotics Society of Japan Vol. 4, No. 2,". On page 27.
This is a method of detecting a contact point using reaction force information, in which it is necessary to obtain the friction coefficient between the tool and the object to be processed in advance, and cannot cope with a change in the friction coefficient during the operation of the robot arm. There are drawbacks.

[0003] The load is changed by control such as changing the posture without changing the contact point, described on page 395 of the 8th Annual Meeting of the Robotics Society of Japan (1).
The method of estimating the contact point based on the deviation force vector measured at that time cannot be applied to copying operations that require a constant posture or acting force, and furthermore, if the contact point changes during the deviation force vector measurement Also has the disadvantage that it cannot be applied.

On the other hand, in the method described in Japanese Patent Application Laid-Open No. 3-256103, in which a contact normal is obtained from force components in three directions of a force detector, it is assumed that a contact point with the periphery of the robot arm is invariable. However, there is a drawback that it is impossible to cope with friction at the contact point or a change in the contact point of the robot arm during operation.

[0005]

In addition to the above, an error always occurs in the measured value of the force detector due to the weight of the object to be gripped or the tool or the acceleration motion. For this reason, attempts have been made to correct these errors based on the mass, center of gravity position, moment of inertia, and hand posture of the arm of the gripped object or tool. Because of the difficulty in the first place, errors in the force detector due to this are inevitable.

Further, considering detection errors due to temperature change, hysteresis, non-linearity, etc., error factors in the force detector are various. The above-mentioned prior arts have a drawback that they do not essentially have a function of reducing a force detector error caused by such factors.

In view of the above-mentioned problems, the present invention can cope with fluctuations in frictional force and friction coefficient at a contact point and fluctuations in a contact point with an external environment in a working / assembly apparatus during work. It is an object of the present invention to provide a method of detecting a contact point and a contact normal of a machining / assembly apparatus having a function of correcting an error of a vessel.

[0008]

The features for solving the above-mentioned problems are achieved by the present invention employing the following novel characteristic construction method. That is, the feature of the present invention is that it has a function of detecting a three-dimensional position / posture or a velocity / angular velocity of a tool or a component to be held in a space in which work is performed, and an acting force applied to the tool / component to be assembled. A method for detecting a contact point and a contact normal in a machining / assembly device having a function of detecting a tool / assembly component, wherein the contact point and the contact normal are detected between the machining / assembly device and the tool / assembly component. A force action calculation processing step of calculating, by a computer, a reaction force acting on the tool / assembly target component from the output signal of the force detector, and calculating a force action line from the reaction force; An intersection calculation processing step of calculating, by using the computer, an intersection of the tool / assembly target part and the line of action of the force using the geometric shape data of the target part; A plurality of candidate points are set in the vicinity of the intersection, a normal vector at each candidate point, and a translation speed of each candidate point by the position or speed detection function are calculated by the computer, and an inner product calculation is performed to obtain an inner product of the normal vector and the translation speed. Processing step, determine the distance from the candidate point to the intersection of the tool and the assembly target part,
A function calculation processing step of calculating, by the computer, a function consisting of the inner product and the distance at each candidate point, the function is used as an evaluation function, and a point at which the evaluation function is minimized among the candidate points and a method at that point A line vector is calculated by the computer as a contact point and a contact normal between the tool / assembly target part and a part to be processed or a part arranged in the vicinity. is there.

[0009]

Since the present invention employs the above-described method, the value of the intersection between the geometrical shape of the tool / part to be assembled and the line of action of the force, which is not affected by the frictional force, is used. The present invention is also applicable to fluctuations and fluctuations of contact points with surroundings in a processing / assembly apparatus such as a robot arm during work.

Further, a plurality of candidates are provided near the intersection on the surface of the tool / part to be assembled by using a control signal of a displacement or speed detector which does not increase an error with respect to an acceleration motion or a change in surrounding temperature. A point is set, an inner product calculation processing step for obtaining an inner product of the normal vector and the translation speed at each candidate point, and a distance from the candidate point to the intersection of the tool / part to be assembled is obtained. And a function calculation processing step of calculating a function consisting of the distance.

[0011] Since the above function is used as an evaluation function and the point and the normal vector of the candidate points which minimize the evaluation function are the contact points and the contact normals, the tool
Solves the problem of the conventional technology that the error of the force detector that cannot be reduced due to its own weight and acceleration motion of the parts to be assembled, temperature change of the force detector, hysteresis, nonlinearity, etc.,
It realizes versatility that can be applied as it is to copying operations that require constant posture and acting force.

[0012]

BRIEF DESCRIPTION OF THE DRAWINGS FIG. FIG.
Is a schematic diagram showing the freedom of each joint and each link length of a robot arm used directly in the implementation of this embodiment, FIG. 2 is a diagram of the same control system, and FIG. FIG. 4 is a view showing an example of setting a candidate point near the intersection of lines, FIG. 4 is a view showing an example of a tool attached to the tip of the robot arm, and FIG. FIG. 6 is a flow chart of the present embodiment.

In the drawing, A is a robot arm having a total of six degrees of freedom, three for each of a pivot axis and a shaft rotation, B is a tip end effector of the robot arm A, C1 is a tool, C2
Is a part to be assembled, D is a processing object, E is a peripheral part, 1
6 are joints from the base of the robot arm A sequentially, and L1, L2 and L3 are, for example, 500 mm and 4 respectively.
A link having a length of 00 mm and 47 mm, X, Y, and Z indicate directions of a horizontal axis, a vertical axis, and a height axis, respectively.

7 is an encoder attached to each of the joints 1 to 6 of the robot arm A, 8 is an up / down counter, 9 is a six-axis force detector attached to the hand of the robot arm A, 1
0 is an A / D converter, 11 is a D / A converter, 12
Is a servo amplifier, 13 is an actuator, and 14 is a computer for controlling the overall operation of the robot arm A and performing calculations necessary for the contact point and contact normal detection method of this embodiment.

In the following, an object to be processed by a tool attached to the tip of the robot arm A as shown in FIG.
The parts gripped by the end effector B of the robot arm A as shown in FIG.
In this example, the surface shape of the part where the tool C1 and the component C2 to be assembled come into contact with the workpiece D or the component E fixed to the periphery can be described by a quadratic curve such as a cylinder, an ellipsoid, or a sphere. A detailed detection method will be described with reference to FIG.

First, immediately after the start step, a reference hand coordinate meter is set on a target axis of the surface shape of the tool C1 or the component C2 to be assembled (hereinafter, tool C1 or the like), and a three-row three-column matrix M representing the surface shape is set. Is stored in the computer 14 (see step [1]). At this time, an equation representing the surface shape is shown in equation (1). Equation (2) shows the equation representing the surface normal vector.

X ^T Mx = 1 (1) n (x) = Mx / {Mx} (2) x: three-dimensional position vector of the surface in the reference hand coordinate system M: three-row, three-column symmetric matrix n (x): Outward normal unit vector at x in the reference hand coordinate system

In the above equation, ‖ · ‖ represents the length of the vector. At this time, when the tool C1 or the like is in contact at one point with the workpiece D or the component E fixed to the periphery, the tool C1 is taken into the computer 14 from the force detector 9 via the A / D converter 10 (step [ 2]) Detection signal S
Using the reaction force value in the reference hand coordinate system obtained from 1 (see step [3]), an equation representing the line of action of the force is calculated by equation (3) (see step [4]).

L = u × uO + tu (3) where u = f / {f} uO = (mO− (mO · u) u) / {f} L: three-dimensional on the force action line in the reference hand coordinate system Position vector t: Parameter f: Reaction force vector in reference hand coordinate system mO: Reaction force moment vector in reference hand coordinate system

Here, if {f} is zero or smaller than a predetermined area Flim, it is assumed that no contact point exists (see step [5]). Also, it is determined whether equation (3) representing the line of action of the force is equal to or greater than zero (see step [6]). Next, if YES, the intersection of the surface shape and the line of action of force is calculated by equation (4).

PO = u × uO + t′u (4) where pO is a three-dimensional position vector of an intersection on the line of action between the surface shape and the force in the reference hand coordinate system t ′ = (− b ± √ (b ² -4ac) ) / (2a) a = u ^T Mu b = u ^T M (u × uO) + (u × uO) ^T Muc = (u × uO) ^T M (u × uO) −1

Here, two t 'are usually obtained, but since the force due to contact acts in the direction of pushing the robot arm A, t' that satisfies equation (5) is used (see step [7]). u · n (u × uO + t′u) ≦ 0 (5)

The pO and n (pO) obtained as described above are independent of the weight and acceleration motion of the grasped object or the tool / part to be assembled, regardless of the fluctuation of the frictional force and the frictional coefficient, respectively. When there is no error of the force detector 9 caused by temperature change, hysteresis, non-linearity, etc., the contact point and the contact normal are represented, and in such an ideal state, the discriminant b ² − 4ac also always takes a positive value.

Usually, force detector 9 is used due to various factors.
Therefore, a measure is taken to reduce the influence of such an error of the force detector 9 by the following procedure. However, if the discriminant b ² -4ac takes a negative value, the point on the surface of the tool C1 or the like closest to the force line L is set as pO (see step [8]), and the following processing proceeds.

First, based on the joint angular displacement taken into the computer 14 via the up / down counter 8 from the encoder 7 attached to each of the joints 1 to 6 of the robot arm A, the joint is operated by a difference or a pseudo-differential filter. The angular velocity is calculated, and the velocity of the origin of the reference hand-end coordinate system viewed from the absolute coordinate system is calculated by equation (6) (see step [9]).

[0026] _{· · · · · · (V} T ω T) T = J (θ1 θ2 θ3 θ4 θ5 θ6) T (6) V: translational velocity vector of the standard hand coordinate system origin as seen from the absolute coordinate system ω: absolute coordinates the angular velocity vector J of the standard hand coordinate system origin as seen from the system: Jacobi matrix _{· · θ1 ~θ6:} the first joint to sixth joint angular velocity

Next, an intersection point pO between the surface state and the line of action of the force.
For example, n × m candidate points as shown in FIG. 3 are set on the surface near (see step [10]), and the translation speed vector at each candidate point is calculated by equation (7). Similarly, a normal vector at each candidate point is obtained using equation (8) (see step [11]).

Vi = V− (R (pi−pO)) × ω (7) Vi: translation speed vector viewed from the absolute coordinates of the candidate point i (1 to n × m) pi: reference hand end coordinate system of the candidate point i R: a coordinate conversion matrix from the reference hand coordinate system to the absolute coordinate system nw (pi) = Rn (pi) (8) nw (pi): normal vector of the candidate point i as viewed from the absolute coordinate system

Based on this, the value of the linear evaluation function H shown in equation (9) is calculated for each candidate point. H = α | Vi · nw (pi) | + β {pi−pO} (9) α, β: weighting factors | · | represent absolute values.

Here, a normal vector pi that minimizes the evaluation function H is obtained (see step [12]).
To the processing target D fixed around the tool C1 and the like
Alternatively, a contact point with the component E is set, and a normal at this contact point is set as a contact normal (see step [13]). Thereafter, it is determined whether or not the process is to be ended (see step [14]). If NO, the process returns to step [2] and is repeated. If YES, the process completely shifts to the end step.

The contact point and the contact normal obtained by the above processing procedure are the error of the force detector 9 due to its own weight or acceleration motion of the grasped object or tool, temperature change of the force detector 9, hysteresis, non-linearity, etc. Also exists if
Using the information on the joint angle of the robot arm A, which does not increase the error due to the acceleration motion or the temperature change, it is possible to reduce the influence of the detection error of the contact point and the contact normal.

In the above description, the case where the surface shape of the tool C1 or the like can be described by a quadratic surface such as a cylinder, an ellipsoid, or a sphere has been described. This can be applied to the case of a polyhedron, a shape composed of a combination of a polyhedron and a quadratic curve, and a shape composed of a plurality of quadratic surfaces. Further, the present embodiment can be applied without any problem even when the degree of freedom of the processing / assembly apparatus such as the robot arm A is less than six or seven or more.

(Experimental Example) Next, a specific experimental example according to this embodiment will be described with reference to the drawings. FIG. 7 is a diagram showing an intersection of a surface shape and an action line of force and a normal vector thereof in an experimental example, and FIG. 8 is a diagram showing a trajectory and a contact normal vector of the same detected contact point.

The tip of the force detector 9 attached to the hand of the robot arm A has a tip of 15 [mm] and a length of 120 [m].
m], an aluminum cylinder having a radius of 200 [mm] is fixed horizontally on the peripheral side, and a human moves the base side from the position where the force detector 9 of the robot arm A is mounted. The detection signals S1 and S2 of the force detector 9 and the encoder 7 when the cylinder was brought into contact with the edge of the vinyl chloride plate at one point and imitated were measured at a sampling interval of 0.05 [sec].

FIG. 7 shows the trajectory of the intersection point of the line of force and the line of action of the force, which is manually calculated using equations (4) and (5), as viewed from the absolute coordinate system, and the normal vector of the cylinder surface at the intersection. Is illustrated. Here, the trajectory alpha ₁ viewed from the absolute coordinate system at the intersection of the line of action of the cylinder surface and the force represents the edge of the vinyl chloride plate, normal vectors beta ₁ of the cylinder surface represents a normal vector of the vinyl chloride plate.

As shown in FIG. 7, the normal vector β ₁ clearly has an error as compared with the normal to the circumference of a radius of 200 [mm]. This is the acceleration motion, force detector 9
This is caused by a detection error of the force detector 9 due to a temperature change, hysteresis, non-linearity, and the like.

FIG. 8 shows the result of detection using the same measurement data as in the prior art for this experimental example, as in FIG. FIG.
In FIG. 7, the normal vector β ₂ and the trajectory α ₂ of the circumference are correctly arranged at regular intervals in a fan shape, and the detection result of the normal vector β ₁ is greatly improved as compared with FIG. Therefore, the effectiveness of the function of reducing the error of the force detector 9 according to this experimental example was proved.

Further, in this experimental example, a complicated operation such as previously obtaining a friction coefficient required in the prior art is not required. Furthermore, in the experiment, since the control of the robot arm A such as keeping the contact point and the contact force constant is not performed at all, the method of detecting the contact point and the contact normal in the machining / assembly apparatus according to the present experimental example is the control of the robot arm A. Independent of the method. In this experimental example, whether the control method of the robot arm A is a method of directly controlling a force, a method of controlling a force indirectly, or a case without control at all, Applicable without any changes.

[0039]

As described above, according to the present invention, it is possible to cope with the fluctuation of the frictional force and the friction coefficient at the contact point and the fluctuation of the contact point with the periphery of the working robot arm, which cannot be coped with the conventional technology. In addition, it has a function to correct the error of the force detector, which is not available in the conventional technology, and it can be applied to copying operations that require constant posture and acting force. And an excellent effect such as the detection of the contact point and the contact normal line without depending on the image.

[Brief description of the drawings]

FIG. 1 is a schematic diagram showing the freedom of each joint and each link length of a robot arm to which an embodiment of the present invention is applied.

FIG. 2 is a control system configuration diagram according to the first embodiment;

FIG. 3 is a diagram showing an example of setting candidate points near an intersection of a surface shape and a line of action of a force in the embodiment of the present invention.

FIG. 4 is a diagram showing an example of a tool attached to the tip of the robot arm.

FIG. 5 is a diagram showing an example of a component held by the end effector of the robot arm.

FIG. 6 is a flowchart of the above.

FIG. 7 is a diagram showing an intersection of a surface shape and a line of action of a force and a normal vector thereof in an experimental example.

FIG. 8 is a diagram showing a trajectory of a contact point and a contact normal vector detected according to the present invention.

[Explanation of symbols]

A: Robot arm B: End effector C1: Tool C2: Assembly target part D: Processing target E: Part (peripheral side) L1: Link of 500 mm length L2: Link of 400 mm length L3: Link of 47 mm length 1: Robot arm 2 joints 3 joints 3 joints 4 joints 4 joints 5 joints 5 joints 6 joints 6 joints 7 encoder 8 up-down counter 9 ... force detector 10 ... a / D converter 11 ... D / a converter 12 ... servo amplifier 13 ... actuator 14 ... computer α _1, α ₂ ... trajectory beta ₁ at the intersection of the line of action of the cylinder surface and the force, beta ₂ … Normal vector

──────────────────────────────────────────────────続 き Continued on front page (58) Field surveyed (Int.Cl. ⁷ , DB name) G05B 19/18 B25J 9/10

Claims (1)

(57) [Claims] 1. A function for detecting a three-dimensional position / posture or a speed / angular velocity of a tool or a component to be held in a space in which work is performed, and detecting an acting force applied to the tool / component to be assembled. What is claimed is: 1. A method for detecting a contact point and a contact normal in a machining / assembly device having a function, comprising: a force detector disposed between the machining / assembly device and the tool / assembly target component; From the output signal of the above, the reaction force acting on the tool / assembly target component is obtained by a computer, and a force action calculation processing step of calculating a line of action of the force from the reaction force; An intersection calculation processing step of calculating, using the computer, an intersection of the tool / assembly target component surface and the line of action of the force using the shape data; and a vicinity of the intersection on the tool / assembly target component surface A plurality of candidate points are set, a normal vector at each candidate point, an inner product calculation processing step of calculating a translation speed of each candidate point by the computer by a position or speed detection function to obtain an inner product of the normal vector and the translation speed, Finding the distance from the candidate point to the intersection of the tool / assembly target part, sequentially performing a function calculation processing step of calculating the function consisting of the inner product and the distance at each candidate point by the computer, As an evaluation function, a point at which the evaluation function is minimized among the candidate points and a normal vector at that point are
A method for detecting a contact point and a contact normal in a machining / assembly apparatus, wherein the computer calculates a contact point and a contact normal between the tool / assembly target component and a component disposed on or around the machining target.