JPH03240104A - Robot copying controller - Google Patents

Abstract

PURPOSE:To automatically set a work coordinate system in an on-line method by providing a copying route setting means which defines the intersecting line between a work subject surface and a plane decided by the start point, the end point, and the bypass point of the copying work as a copying work route. CONSTITUTION:A copying route setting means 18 sets an intersecting line lbetween the surface of a subject 4 and a plane phi decided by a start point T1, a bypass point T3, and an end point T2 of the copying work as a copying work route. Then a robot arm 1 is moved. A unit vector (n) of a work coordinate system is decided from the direction of the contact force which is detected by a force sensor 2 attached to a probe 3. Meanwhile a unit vector (a) of the work coordinate system is decided in parallel with a direction vector (m) of the intersecting line l between the plane phi and a contact plane II set at the point T1. Finally a 3rd unit vector (o) is decided as an external product of both vectors (a) and (n). Thus the work coordinate system is automatically set in an on-line method and the surface of the work subject having an unknown shape is smoothly copied.

Description

[Detailed Description of the Invention] [Summary] This invention relates to a robot tracing control device that automatically sets a work coordinate system online in order to smoothly trace the surface of a workpiece of unknown shape. The purpose is to automatically set the working coordinate system online in order to smoothly trace the surface of the robot. Cross product of le■×■
Using a transformation matrix from the work coordinate system to the reference coordinate system with =■ as the second column, calculate the deviation between the target value l of the robot's hand and the current position, the target force value of the robot, and the object of the hand. In a system equipped with a position and force hybrid control device that controls the position and force of the robot based on a deviation from the force acting on the object, a starting point of the tracing operation specified on the surface of the workpiece of the robot. The three points T
+, Tz, and T3 and a line of intersection between the surface of the object to be worked on and the surface of the object to be worked on as the path of the tracing operation.

[Industrial application field]

The present invention relates to a robot control system, and more particularly to a robot tracing control device that automatically sets a work coordinate system online in order to smoothly trace the surface of an unknown-shaped workpiece.

[Conventional technology]

In copying work in which the robot's hand is moved along the surface of a workpiece while being pressed with a certain force, a force is applied in the normal direction to the tangential plane at the contact point between the probe attached to the robot hand and the workpiece. control, and is also set to perform position control in the tangential plane.

FIG. 12 is an example of setting a work coordinate system in hybrid control of position and force such as copying work. In the figure, at the point of contact between the probe 3 attached to the robot arm 1 via the force sensor 2 and the workpiece 4, the force control direction is a unit vector n in the normal direction of the surface of the workpiece 4. Further, the position control direction is indicated by a unit vector OVa. In the present invention, the moving direction of the probe 3 is made to coincide with the direction of the unit vector a.

FIG. 13 is a block diagram of a configuration example of a robot position and force hybrid control device. In the same figure, Pl.

is the current position of the robot's hand calculated from the displacement of each joint of the robot, p or is the target position of the hand, P- is the origin of the work coordinate system, and F is detected by the force sensor attached to the robot. The acting force between the robot hand and the workpiece, For is the target force, and e is the joint speed command. Here, for example, the subscript 1r1 of p sr indicates that it is a reference coordinate system whose origin is the fixed position of the robot body.

The positional deviation described in the reference coordinate system of the robot is generated by the positional deviation generator 5, and the force deviation is generated by the force deviation generator 6, and each of them is generated by a transformation matrix RT, for example, with the robot hand position as the origin, and the hand and work object are It is converted to a work coordinate system defined by constraint relationships such as pressing between objects. Here, "To" of RT indicates a position matrix,
n and 0°a indicate unit vectors that are right-handed and orthogonal to each other.

A speed command is generated by a position compensator 7 and a force compensator 8 from the position deviation and force deviation converted into the work coordinate system, respectively, and the output of the position compensator 7 includes position selection matrices S and 9. The output of the force compensator 8 has a force selection matrix 5rl.
By multiplying by O, Vpw and ■1 are generated as a speed command in which the position control direction and the force control direction are separated. For example, in the case of force control in the translation n direction and position control in the translation 0 and a directions, the position selection matrix S
, and the force selection matrix Sf are given as follows.

S, = dt--(011) (2) S
t-dt--(100) (3) Here “
d Lag' indicates a diagonal matrix whose main diagonal elements are in parentheses.

When moving the hand in space, in order to speed up the movement, for example, a trapezoidal speed function whose height is the maximum speed that the robot can achieve ■□8 is generated by the speed generator 11 as a feedforward speed command ■ . generated as And this ■. 0 is also input to the position error generator 5 via the integrator 12. This is the output I (V
. , (a)), that is, the desired position of the hand at every moment is determined by the amount of movement from the movement start position PC, , and is set as the current position P1. ．． This is for comparison.

The speed V of the work coordinate system description that the hand of the robot should output as the sum of three speed commands by the adder 13. w+V,,+
Vf is generated and transformed into a reference coordinate system description by the transformation matrix R14. And from that speed, the inverse Jacobian matrix J
-'15 is used to generate the joint velocity command e. Here, the inverse Jacobian matrix J-1 expresses the minute angular displacement (joint velocity) 6 of each joint of the robot over a minute time and the minute displacement (velocity) ■ of the robot hand as V = J e (
4) is an inverse matrix of the Jacobian matrix J, and each element of the Jacobian matrix J is determined by the structure of the robot's joints.

[Problem to be solved by the invention]

In hybrid control of position and force as shown in Figure 12.13, if there is a discrepancy in the relationship between the actual tip restraint state of the probe 3 and the set work coordinate system, the contact force will not be controlled according to the target value. There is a point. For example, if the vector a points inward from the tangent to the object 4, an excessive contact force will occur until the misalignment in the direction of the vector n is corrected, and if the vector a points outward, contact will occur. Either the force becomes too low or the probe 3 floats off the surface of the object 4.

Therefore, when the robot smoothly traces the surface of the object 4 with a stable contact force, it is necessary to change the vectors n, o, and a online according to the state of restraint of the tip of the probe 3, and to adjust the shape of the object surface. The problem is that the work coordinate system cannot be set unless it is known, and even if it is known, the tangential plane and its normal line, which constantly change during the copying operation, must be prepared offline. was there.

An object of the present invention is to automatically set a work coordinate system online in order to smoothly trace the surface of a workpiece of unknown shape.

[Means to solve the problem]

FIG. 1 is a block diagram of the principle of the present invention. From the work coordinate system in which the vector n in the figure is in the first column, the unit vector a in the hand movement direction of the robot 16 is in the third column, and the cross product 0 of a and n is in the second column, the reference coordinate is Using the transformation matrix to the system, the robot 16 is calculated based on the deviation between the target position of the robot 16's hand and the current position, and the deviation between the target force value of the robot 16 and the force acting on the workpiece of the hand. FIG. 2 is a principle block diagram of a tracing control device in a robot system including a position/force hybrid control device 17 that controls position and force.

In FIG. 1, the scanning route setting means 18 is the robot 16.
The starting point TI of the copying work specified on the surface of the workpiece
, its end point T2, and the via point T3 of the copying operation between points TI and Tz, copy the line of intersection between the plane Φ determined by the three points T+, Tz, and T3 and the surface of the workpiece. Set as the route. By finding the normal vector Φ to the plane Φ from the three points T'+, Tz, and T3, we can conclude that the tracing path has the normal vector Φ and exists on the plane Φ that includes the point T+. Become.

[For production]

In the present invention, the starting point TI and the transit point T3 of the copying operation are
, and the intersection line between the plane Φ determined by the end point T2 and the surface of the object is set by the copying route setting means 18 as the route for the copying operation, and then the tip of the probe is brought into contact with the starting point TI of the copying operation. The robot arm is moved.

Next, for example, a unit vector n of the work coordinate system is determined from the direction of the contact force detected by a force sensor attached to the probe, and a unit vector a of the work coordinate system is, for example, the starting point T! of the copying work! It slips in the direction of the intersection line l between the tangent plane ■ and the plane Φ.

In the present invention, in order to perform copying work along the surface of a workpiece of unknown shape, suitability of the set copying work coordinate system is appropriately determined, and the already set coordinate system is determined to be unsuitable. The work coordinate system is set again at the point where the work coordinate system is set again.0 The suitability determination of the work coordinate system is already set, for example, as a vector from the origin of the work coordinate system (initially the starting point TI) to the current position of the probe tip. This is done depending on whether the angle of force with respect to the unit vector a in the moving direction exceeds a certain threshold value.

As described above, in the present invention, the work coordinate system is reset at the time when the set work coordinate system is determined to be inappropriate.

〔Example〕

FIG. 2 shows an example of tracing route setting. In the same figure,
If, for example, a transit point T31 is set between the starting point T+ and the ending point T2 of the copying operation, three points T+.

T2. T: Plane ΦI determined by II and workpiece 4
The line of intersection with the surface of T2. When is specified, the line of intersection between the plane Φ2 and the surface of the workpiece 4 is set as the tracing route 2. Here, the normal vector Φ with respect to the plane Φ is given by the following equation, with the passing point of the copying operation being T3.

In other words, the copying path exists on the plane Φ which has Φ as the normal vector and includes the starting point T+ of the copying operation.

FIG. 3 shows an example of controlling the pressing on the workpiece.

This figure shows the initial contact state for the copying operation at the starting point T1 of the copying operation. First, the probe tip is spatially moved to the starting point T1, and then a temporary target force vector of magnitude IF and □ 1t is commanded from point T1 in the direction where the workpiece exists, and the probe tip is free to translate. When the degree of restraint is 1, pressing is controlled. At this time, the target vector of the plate does not need to coincide with the normal direction at the contact point T+.

As shown in Figure 3, the pressing at the first contact point is
The process ends when the component of the contact force Fllr detected by the force sensor in the tentative target force vector direction becomes IF, , lt. Here, the subscript °t'' indicates the magnitude of the translation vector.

The contact force while the probe tip is moving includes frictional force, but in Figure 3, the probe tip is at the starting point T of the copying operation.
When stopped at +, the contact force F0 detected by the force sensor represents the normal direction of the tangential plane H at the contact point, and can be regarded as a reaction force to the force acting on the object. Figure 4 shows the scanning route setting plane Φ and the tangent plane n.
This is an example of the intersection line l with . In the figure, the tip of the probe traces from the starting point T+ to an arbitrary point P on the tracing path.

A method for setting the unit vector a of the work coordinate system will be explained by using this point as the contact point PC, (origin) at which the work coordinate system is set. As shown in the figure, the unit vector a is parallel to the vector m in the direction of the intersection line l between the plane Φ and the plane ■. If the vector m given by equation (7) follows as shown in Figure 4, the stage vector m/I
m l- can be directly used as the unit vector a of the work coordinate system.

However, if the surface of the object is concave rather than convex as shown in Figure 2, the normal vector Φ determined by equation (5) will be in the opposite direction, and as a result, the vector m will also be affected by the probe movement. The direction is opposite, and the unit vector in that direction cannot be set to a.

FIG. 5 shows an example of a method for setting the unit vector a of the work coordinate system. In the same figure, Pt1. ptz is the following formula % Formula % (8) (9) Unit vector a is the third unit vector 0 of the unit work coordinate system in the direction from Ptl to Pt2 is the unit vector n and a
It is given by the following formula using .

o = a X n 0
0 Also, the target force vector is calculated using the magnitude of the temporary target vector as a vector in the direction of the unit vector n.For=
l F orl t n
Q7J Figure 6 is an example of compatibility determination of the scanning coordinate system.

If the object is a continuous curved surface, the work coordinate system should, in principle, be changed continuously, but this may result in oscillatory motion. In the present invention, as shown in FIG. 6, the current position P0 of the hand is
When the angle θ1 between the vectors as and the vectors a exceeds the direction angle error threshold θtha, it is determined that the existing work coordinate system is inappropriate. That is, a S= (Par Pcr) / l Par
In the as vector Pcrl Q'l), as-a≦cos (θtha) 04
) is true, the existing work coordinate system is inappropriate.

In addition, the tip of the probe may separate from the object due to unstable contact for some reason or a delay in force control in the n direction, so the n The condition is that there is sufficient contact force in the direction.

Fmr・n≧ξ (F, rl ・n), (0<ξ〈
1) ・ ・ ・Limited to when one of a is satisfied.

FIG. 7 is a block diagram of the overall configuration of an embodiment of a robot control system using the scanning control device of the present invention. In the figure, the system includes CPU 20. Pushing works with the target force,
Or a target vector P for performing one action such as movement to a target position. 1, the conversion matrix R from the work coordinate system to the reference coordinate system, the selection matrices Sr and S-, and the feedforward movement speed ■□8. A real-time control device 22 including a position and force hybrid control device and a joint servo control device, a copying motion control device 23 of the present invention, a storage device for storing operation commands to the robot, etc., such as a magnetic disk 24, a keyboard 25, and a copying operation. A memory 26 that stores the start point TI, end point T2, transit point T3, etc., and an A/D that digitally converts signals from encoders attached to each joint.
Converter 27, D/A converter 28 that converts the signal to the power amplifier that drives the motor of each joint into analog, and CRT 2
9. Interface 30 of the magnetic disk 24. An interface 31 of the keyboard 25, an interface 32 of a force sensor (not shown), and an interface 33 of the CRT 29
, an encoder l1034, and a power amplifier l1035.

FIG. 8 is a flowchart of an overall processing example of copying operation control in the present invention. In the figure, when the process starts, first, in step (S) 36, preprocessing explained in FIG. 9 is performed. In the preprocessing, a copying route for copying work is set, and various condition values such as a temporary target force vector at the first contact point are set. Next, in S37, the spatial movement to the starting point T of the copying operation is performed, and in 338, the spatial movement is performed again to the starting point T.
Temporary target force vector F at +. Pressing with □ is the seventh
Each step is executed by the illustrated macro instruction by the operation control device 21 in the figure. Note that "TIME" in the macro instruction in S37 is a designation of the movement time in the space movement instruction.

Subsequently, in S39, a process for setting a copying work coordinate system, which will be explained with reference to FIG. 10, is performed. In this process, the above-mentioned settings of n, a, and 0 are performed. When the processing in S39 is completed, the setting of the work coordinate system at the time of starting the copying work is completed, and the copying work is started in S40. That is, the seventh
Target position P stored in the memory 26 in the figure. Tracing r to the end point T2 of the work, the feedforward moving speed v
By replacing .

Then, at 541, the probe tip position P, , is the target position p
or, that is, whether or not the copying operation end point T2 has been reached is determined by, for example, monitoring the movement end flag in the real-time control device 22. If the tip of the probe has not arrived at the end point T2 of the copying work in 341, the compatibility determination process of the copying work coordinate system, which will be explained in FIG. The process from then on is repeated.

If the scanning coordinate system is determined to be inappropriate in S42,
At step 3, the feedforward moving speed V□8 and the integral value for determining the target position for each sampling period are both set to 0, and the tracing operation by the hybrid control of position and force of the real-time control device 22 is stopped. At the point where the tip of the probe stops in S44, the process of setting the copying work coordinate system is performed in the same manner as in 339, and the processes from S40 onwards are repeated.

At 341, the probe tip position P0 is the end point T2 of the copying operation.
When it is determined that the probe has reached the point En, the probe tip is moved to the point En, for example, the home position, via the points E+, Ez, .

FIG. 9 is a flow chart of 336 of FIG. 8, that is, an embodiment of preprocessing. In the figure, first, at 348, the start point TI, end point T2, and transit point T3 of the copying operation are set by direct teaching, input of coordinate values from the keyboard 25, or input of coordinate values from the disk device 24, and are stored in the memory. 26. followed by S
In step 49, the normal vector Φ of the plane Φ is calculated using equation (5) and stored in the memory 26. In S50, after the tracing operation is completed, the coordinates of an arbitrary number of points are set as waypoints in spatial movement to the home position, for example, by direct teaching or by inputting coordinate values from the keyboard 25 or the disk device 24, and the coordinates are set in the memory 2.
6.

Thereafter, at 351, the initial contact force F at the starting point T1 of the copying operation is determined. Keyboard 2 with rl as a temporary large target vector
5 or by inputting coordinate component values from the disk device 24 and stored in the memory 26. For example, the third
In the case of the restrained state shown in the figure, 2. Since contact with the workpiece can be made by pressing in the direction, the following equation is manually calculated as a temporary large target vector.

Fo, = (OOO, 5)” IF, □ lt at this time is set as the magnitude of the contact force in the copying operation.

After that, in S52, the speed of the copying operation is set to 5peed, and again in S5
3. In S54, the direction angle error threshold θthd % and the contact determination coefficient ξ are used for the coordinate system compatibility determination process, respectively.
is set by input from the keyboard 25 or the disk device 24, is stored in the memory 26, and the process ends.

FIG. 10 is a flowchart of 339 in FIG. 8, that is, an embodiment of the copying work coordinate system setting process.

In the same figure, first, in S55, the position P s of the probe tip is determined.
r is set as the origin p cr of the coordinate system, a unit vector n is set by equation (6) in 356, and the plane Φ and ■
A vector m in the direction of the intersection line l is set, 358, S
In step 59, the legs Ptl and ptz of the starting point T+ and ending point T2 of the copying operation to the straight line l are determined using equations (8) and (9).

Next, in S60, the target force vector is set in the direction of unit vector n according to formula 02), and in S61, the moving direction unit vector a of the probe tip and the third unit vector 0 of the work coordinate system are set using formulas 00 and 00. is set, S62
The force selection matrix Sf and the position selection matrix S9 are set in step. Here, the force selection matrix St is set to 1 only in the n direction, and the position selection matrix S2 is set to 1 in the a and 0 directions.

FIG. 11 is a flowchart of an embodiment of S42 in FIG. 8, that is, the compatibility determination process of the copying work coordinate system. In the same figure, in S63, a vector aS from the origin P cr of the work coordinate system to the current position p sr of the probe tip is calculated using equation 031, and in S64, (2) and 0
It is determined whether or not both of the two equations hold true, and if both hold true, the copying work coordinate system is determined to be inappropriate and the eighth
The process moves to step 343 in the figure, and unless both the circle and equation 05) are satisfied, it is determined that the work coordinate system is normal, and the process from step 341 in FIG. 8 is repeated.

In this example, if only the 09 equation does not hold at 564 in FIG. It is also possible to include a process of stopping the probe by reducing the moving speed to zero when this is not true, and waiting for contact between the probe and the object to be made again by force control in the n direction.

〔Effect of the invention〕

As explained in detail above, according to the present invention, the work coordinate system is automatically set online, making it possible to smoothly trace the surface of a workpiece of unknown shape, and This greatly contributes to improving the controllability of robots.

[Brief explanation of drawings]

Fig. 1 is a block diagram of the principle of the present invention, Fig. 2 is a drawing showing an example of setting the tracing path, Fig. 3 is a drawing showing an example of controlling the pressing on the workpiece, and Fig. 4 is a drawing showing an example of controlling the pressing on the workpiece. Figure 5 is a diagram showing an example of the method of setting the vector a, and Figure 6 is a diagram showing an example of the method of setting the vector a. FIG. 7 is a block diagram showing the overall configuration of an embodiment of a robot control system using the scanning control device of the present invention; FIG. 8 is a flow chart of an overall processing example of scanning control; FIG. FIG. 10 is a flowchart of an embodiment of copying work coordinate system setting processing; FIG. 11 is a flowchart of an embodiment of copying work coordinate system compatibility determination processing; FIG. 12 is a flowchart of an embodiment of copying work coordinate system setting processing; FIG. 13 is a block diagram showing an example of the configuration of a hybrid position and force control device. 1 ・ 2 ・ 3 ・ 4 ・ 21 ・ 22 ・ 23 ・ 24 ・ 25 ・ 26 ・ Robot arm, force sensor, probe, work object, 1 motion control device, real-time control device, copying motion control device, disk device, keyboard, memory.

Claims (1)

[Claims] The unit vector ■ in the direction of the target force vector is in the first column, the unit vector ■ perpendicular to the vector ■, and the unit vector ■ in the hand movement direction of the robot (16) is in the third column, the cross product of these two vectors ■×
Using the transformation matrix from the work coordinate system to the reference coordinate system with ■=■ as the second column, the deviation between the target position and the current position of the hand of the robot (16) and the force of the robot (16) are determined. In a system comprising a position and force hybrid control device (17) that controls the position and force of the robot (16) according to a deviation between a target value and a force acting on the object at the hand, the robot (16) The start point T_1 and end point T_2 of the copying work specified on the surface of the workpiece, and the points T_1 and T_
For the via point T_3 of the copying operation between 2 and 2, the 3 points T
A robot scanning control device comprising a scanning path setting means (18) that sets the intersection line between the plane determined by T_1, T_2, and T_3 and the surface of the workpiece as a path for the scanning operation.