JPH03240105A - Robot copying controller - Google Patents

Abstract

PURPOSE:To automatically set a work coordinate system in an on-line method in order to smoothly copy the surface of a work subject having an unknown shape by providing a press control means. CONSTITUTION:A press control means 19A controls the press of the robot fingers to a work subject at the start point of the copying work. A tentative target vector having the value ¦Forl¦t is commanded from the start point in the direction where the work subject exists, and the press control is performed in a state where the freedom degree of translation is stricted to '1' at the tip of a probe. This press operation ends when the component of the contact force Far detected by a force sensor is equal to ¦Forl¦t in the tentative target vector direction. IN this case, a subscript (t) shows the value of the translation vector. As a result, a work coordinate system is automatically set in an on-line method and the surface of a 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. Using the transformation matrix from the robot's hand to the reference coordinate system, using the target position of the robot's hand and the current position transformation matrix, and the deviation between the target force value of the robot and the force acting on the object by the hand, In a system equipped with a hybrid control device for the position and hand of the robot, after spatial movement of the hand of the robot by positioning control to a start point of the copying work specified on the surface of the workpiece, the robot moves the hand to the start point. A temporary target force vector is commanded in the direction of the target object, and pressing is controlled with one degree of freedom of translation in the direction of the temporary target force vector being constrained, and the hand of the robot detected at the starting point is controlled. The press is configured to have a control means that completes the control when the translational degree-of-freedom constraint direction component of the force acting on the object becomes equal to the magnitude of the tentative target force vector.

[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 unit vectors 0 and ■.

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, P□ is the current position of the robot's hand calculated from the displacement of each joint of the robot,
P, , 7 is the target position of the hand, p cr is the origin of the work coordinate system, Fo is the acting force between the robot hand and the workpiece detected by the force sensor attached to the robot, F or
is the target force, and e is the joint velocity command. For example, Pl
The subscript "r" indicates that the reference coordinate system is expressed, for example, with the fixed position of the robot body as the origin.

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 R=(n o a)
(1) Here, “T” of RT indicates a position matrix, n, o.

a indicates 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. The output of the device 8 is a force selection matrix 5rl.
By multiplying by O, V pw and Vfw are generated as speed commands in which the position control direction and the force control direction are separated.

Here is an example. For example, force control in the n-direction of translation,
When controlling the position in the translation 0 and a directions, the position selection matrix S and the force selection matrix St are given as follows.

S,=d,□ (011) (2) St=
dose (100) (3) Here, °d tmg' indicates a diagonal matrix whose main diagonal elements are in parentheses.

When moving a hand in space, in order to speed up the movement, for example, the maximum speed that the robot can achieve ■. A trapezoidal velocity function whose height is X is generated by the velocity generator 11 as the feedforward velocity command Vow. And this ■. 0 is also input manually to the position deviation generator 5 via the integrator 12. This is the output I (
V. This is to obtain the desired position of the hand from time to time based on the amount of movement from the movement start position pcr, w(a)), and compare it with the current position Rp.

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. 8+ Vp
−+V t w is generated and transformed into a reference coordinate system description by the transformation matrix R14. Then, a joint velocity command e is generated from the velocity using an inverse Jacobian matrix J-'15. Here, the inverse Jacobian matrix J-1 expresses the minute angular displacement (joint velocity) e 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 working coordinate system, the contact force will not be controlled to the desired precision. There is a problem. 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. The vector n in the figure is placed in the first column, orthogonal to the vector n, and using the transformation matrix from the thread to the reference coordinate system, 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 calculated. FIG. 2 is a principle block diagram of a copying control device in a robot system including a hybrid control device 17 for the position of a robot 16 and a hand, which is determined by a deviation between a target value and a force acting on a workpiece by a hand.

FIG. 1(a) is a block diagram of the principle of the first invention.

In the figure, the pressing control means 19A is the robot 16.
the starting point of the tracing operation specified on the surface of the workpiece,
For example, after spatial movement of the robot hand by positioning control to T1, a temporary target vector is commanded in the direction of the workpiece at the starting point of the copying work. Here, the direction of the temporary target force vector does not need to be the normal direction of the surface of the workpiece at the contact point, and the target force vector is later corrected using the normal direction vector. The pressing control means 19A is
Next, the pressing is controlled with one translational degree of freedom in the direction of the temporary target force vector being constrained, and the constraint direction component of the force acting on the object of the robot hand detected at point TI is the temporary target force vector. When it becomes equal to the size of , it is assumed that the contact is completed and the control is terminated.

FIG. 1(b) is a block diagram of the principle of the second invention.

In the same figure, the copying route setting means 18 sets the starting point TI of the copying operation specified on the surface of the workpiece of the robot 16, the end point T2 thereof, and the intermediate point T3 of the copying operation between the points T+ and T2. On the other hand, the line of intersection between the plane Φ determined by the three points TI % Tz and T□ and the surface of the workpiece is set as the route of the copying work. By finding the normal vector φ to the plane Φ from the three points T'+, T2 and T3, the tracing path has the normal vector φ and the point TI
It exists on the plane Φ containing .

Next, the action of the pressing control means 19A is the same as in the first invention whose principle is shown in FIG. 1(a).

The copying work system setting means 19B in Fig. 1 (c) is a robot) 1
When the hand of No. 6 comes into contact with the workpiece and stops, the point of contact is set as the origin of the work coordinate system, for example p cr, and the direction of the force acting on the workpiece from the hand detected at the contact point is opposite. Let the unit vector in the direction be the vector n in the first column of the transformation matrix from the work coordinate system to the reference coordinate system. Next, the vector n and the normal direction vector φ of the plane Φ where the copying work path exists
A straight line that has the cross product n×φ=m as a direction vector and passes through the contact point, for example, the legs Ptl and pt of perpendicular lines drawn from the start and end points TI and T2 of the copying operation to l, respectively.
Find z, set the unit vector in the direction from Ptl to ptz as the vector in the third column of the above-mentioned transformation matrix, and then set the target force vector to be equal in magnitude to the above-mentioned temporary target force vector and in the same direction as vector n. Correct its direction as a vector.

FIG. 1(C) is a block diagram of the principle of the third invention.

In the figure, the coordinate system compatibility determination means 19C determines that there is a sufficient component of the unit vector n direction of the contact force at the contact point between the hand of the robot 16 and the work object of the robot 16, and the origin of the work coordinate system, For example, from pcr to the current position of the robot hand, such as P, .

When the angle formed by the vector leading to and the unit vector {circle around (2)} exceeds a preset threshold, the set working system is determined to be inappropriate.

[For production]

In FIG. 1(a) showing the principle of the first invention, the pressing of the robot hand against the workpiece at the starting point of the copying work is controlled by the control means 19A. Ideally, this pressing force should be applied in the normal direction of the surface of the workpiece at the starting point of the copying operation, but since this direction is generally unknown, at the starting point of the copying operation, a temporary target force vector is applied to the object surface. The contact is determined to be complete when the component of the detected acting force, for example, F□ in the translation constraint direction becomes equal to the magnitude of the tentative target force vector. Then, as will be described later, the target force is corrected when the normal direction vector n is also determined.

In FIG. 1(b) illustrating the principle of the second invention, after the intersection line between the plane Φ and the surface of the object is set as a route for the copying operation by the copying route setting means 18, the robot hand, for example, the tip of the probe, The robot arm is moved so that it contacts the starting point TI of the copying operation. The pressing is performed using a temporary target force vector, for example F, at the starting point T+ by the control means 19A. Pressing is controlled using r+,
The pressing control ends when the component in the direction of the temporary target force vector of the force F1 in the normal direction acting on the object becomes equal to the magnitude of the temporary target force vector.

Next, the copying work system setting means 19B determines the unit vector n of the work coordinate system from the direction of the contact force detected by a force sensor attached to the probe, and also determines, for example, the tangent plane {circle around (2)} at the copying work start point TI. The direction of the intersection line l between and the plane Φ→

→ is determined, and the third unit vector 0 is obtained as the cross product with ■n. Then, the correction of the target force vector is
This is performed using the obtained unit vector n.

In FIG. 1(C) showing the third invention, the condition is determined by the coordinate system compatibility determination means 19C when the n-direction component of the contact force is sufficiently large, that is, the probe is pushing the workpiece with sufficient force. The suitability of the coordinate systems that have already been set is determined under 16 continues.

As described above, according to the present invention, the pressing at the starting point of the copying work is controlled, the setting of the copying work system, and the resetting of the work system during the copying work are automatically performed.

〔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 start point TI and the end point T2 of the copying operation, then 3 points T+.

T2. T3. If the line of intersection between the plane Φ1 and the surface of the workpiece 4, which is determined by 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 FIG. 3, the tip of the probe traces, that is, the trace path exists on the plane Φ with Φ as the normal vector and including the starting point T+ of the tracing operation.

FIG. 3 shows an example of controlling the pressing onto 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 T+, and then a temporary target force vector with magnitudes IF and □ h 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. The target vector at this time does not need to coincide with the normal direction at the contact point T1.

As shown in Figure 3, the pressing at the first contact point is
Contact force F, detected by the force sensor.

The component in the direction of the tentative target force vector is IF, 1. l 1
The process ends when t is reached. Here, the subscript "to" indicates the magnitude of the translation vector.

The contact force while the probe tip is moving is the contact force F1 detected by the force sensor when the probe tip stops at the starting point TI of the friction work. ．． represents the normal direction of the tangent plane H at the contact point, and since it can be regarded as a reaction force to the force acting on the object, the working coordinate system in Fig. 4 is the plane for setting the scanning route Φ
This is an example of the intersection line l between and the tangent plane n. In the same figure,
Assume that the tip of the probe has moved from the starting point T+ to an arbitrary point P1r on the copying route, and use this point as the contact point Pcr (origin) for setting the copying work coordinate system.About how to set the unit vector a of the work coordinate system. explain. 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 ■. Here, m is given by the following formula.

m=nxΦ (7) (7
) The vector m given by the formula can be the unit vector (2) of the copying work coordinate system as shown in FIG.

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. It is in the opposite direction, and it is not possible to calculate the unit vector in that direction.

FIG. 5 shows an example of a method for setting the unit vector a of the work coordinate system. In the same figure, the starting point T and the ending point T of the copying work
Ptl and Pt2 to the straight line l of This is an example of determination.

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, from the origin p cr of the work coordinate system to the current position P1 of the hand
When the angle θ between the vector as leading to r and the vector a exceeds the direction angle error dark value θthd, the existing work coordinate system is inappropriate, and the unit vector a is the unit as in the direction from ptt to Ptz. In vectors, the third unit vector of the work coordinate system is 0. When the unit vector 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.

F,,'n≧ξ(Forl −n), (0<ξ〈
1)・・・・05) Only when the following are 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. Generate r, generate the transformation matrix R from the work coordinate system to the reference coordinate system, generate the selection matrix S,, S, and generate the feedforward movement speed VIIIX1
A real-time control device 22 including a motion control device 21, a hybrid position and force control device shown in FIG. 13, 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,
Keyboard 25, starting point TI for copying work.

Memory 26 for storing end point T2, way point T3, etc.
, an A/D converter 27 that digitally converts the signal from the encoder attached to each joint, and a D/A that converts the signal to analog to the power amplifier that drives the motor of each joint.
Converter 28, CRT 29, interface 3o of magnetic disk 24, interface 31 of keyboard 25, interface 32 of force sensor (not shown), CRT 29
interface 33, encoder 11034, and power amplifier 11035.

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 vector and torque 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 pressing with the temporary target force vector F orl at the starting point T1 is performed using the 1-movement control system shown in FIG. &21, each step is executed by the macro instruction shown in the figure. Incidentally, °TIME' in the macro command in S37 is a designation of the movement time in the space movement command.

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
The feedforward movement speed V
By replacing □8 with the variable 5peed, the real-time control device 22 is used to execute the copying operation by hybrid control of position and force.

Then, in S41, the probe tip position fP, , is the target position P0.
1, that is, whether the copying work has reached the end point T2 is determined by, for example, monitoring a 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 step 341, the compatibility determination process of the copying work coordinate system explained in FIG. 11 is performed in step S42. The process from then on is repeated.

If it is determined in S42 that the copying coordinate system is inappropriate, 34
3 is the feedforward movement speed V. Both X and the integral value for determining the target position for each sampling period are set to O, and the tracing operation based on the hybrid control of position and force by 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 S39, and the processes from 340 onwards are repeated.

When it is determined in S41 that the probe tip position psr has reached the end point T2 of the copying operation, the spatial movement commands 345 to 347 move the point E2 and the points T1 and Ez.
The tip of the probe is moved to the point Efi, for example, the home position, through . . . , and the process ends.

FIG. 9 is a flow chart of 336 of FIG. 8, that is, an embodiment of preprocessing. In the figure, first, at 34B, the starting point T2, point T1, end point T2, and transit point T3 of the copying operation are determined by direct teaching, input of coordinate values from the keyboard 25, or input of coordinate values from the disk device 24. is set and stored in memory 26.

Next, in S49, 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 transit points in the 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,
It is stored in the memory 26.

Thereafter, in S51, the initial contact force F at the starting point T1 of the copying operation is determined. 2. is the keyboard 2 as a tentative large target vector.
5 or by inputting coordinate component values from the disk device F24, and is stored in the memory 26. For example, in the case of the restraint state shown in FIG. 3, the workpiece can be contacted by pressing in the Zr direction, so the following equation is input as a temporary large target vector.

Forl=[o O0, 5EdIFo-+lt at this time is set as the magnitude of the contact force in the copying operation.

After that, at 352, the speed of the copying operation is set to 5peed, and again at S5
3.354, 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 probe tip position P1r is
is set as the origin of the coordinate system, a unit vector n is set according to equation (6) in S56, a vector m in the direction of the intersection line l between the plane Φ and n is set in 357, and 35B, 35
9, using formulas (8) and (9), calculate the legs Ptl and Pt of the starting point TI and ending point T2 of the copying operation to the straight line l.
g is required.

Subsequently, in S60, the target force vector is set in the direction of the unit vector n according to the formula 02), and in 361, the moving direction unit vector of the probe tip ■ the third unit vector 0 of the work coordinate system is set using the formulas 00 and 00. In step 362, the force selection matrix Sr and the position selection matrix S2 are set. Here, force selection matrix Sr? ;! Only the n direction is assumed to be 1,
Position selection matrix S.

■The 0 direction and the 0 direction are assumed to be 1.

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, the vector as from the origin PC, , of the work coordinate system to the current position fP, , of the probe tip is determined by equation 03), and in S64, 04) and 05
) equations are both established, and if both are established, the copying work coordinate system is determined to be inappropriate and the 8th
In the figure, the process moves to step 343, and unless both equations (b) and 09 are satisfied, the work coordinate system is determined to be appropriate, and the steps from step 341 in FIG. 8 are repeated.

In this embodiment, if only equation (I5) does not hold at 364 in FIG. It is also possible to include a process of stopping the probe by setting the moving speed to zero when the equation ) does not hold, and waiting for the probe to come into contact with the object 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]

Figure 1(a) is a block diagram of the principle of the first invention;
B) is a block diagram of the principle of the second invention, Figure 1 (C) is a block diagram of the principle of the third invention, Figure 2 is a diagram showing an example of setting a tracing path, and Figure 3 is a workpiece. 4 is a diagram showing an example of the intersection line l between the scanning route setting plane Φ and the tangent plane ■. FIG. 5 is an example of the method for setting the vector a. FIG. 6 is a diagram showing an embodiment of compatibility determination of a scanning coordinate system. 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. The figure is a flowchart of an example of the overall processing of copying control, Figure 9 is a flowchart of an example of pre-processing, Figure 10 is a flowchart of an example of copying work coordinate system setting processing, and Figure 11 is an adaptation of the copying work coordinate system. FIG. 12 is a diagram showing an example of coordinate system setting in copying work; FIG. 13 is a block diagram showing an example of the configuration of a position I and force hybrid control device. 16... Robot, 17... Position and force hybrid control device, 18... Copying route setting means, 19A... Pressing control means, 19B... Copying work system setting means, 19C... Coordinate system compatibility determination means.

Claims (1)

[Claims] 1) Unit vector ■ in the direction of the target force vector in the first column,
From the work coordinate system to the reference coordinate system, which is perpendicular to the vector ■ and has the unit vector ■ in the hand movement direction of the robot (16) in the third column, and the cross product of the two vectors ■×■=■ in the second column. Using a transformation matrix, the robot (16)'s hand is calculated based on the deviation between the target position and the current position, and the deviation between the target force value of the robot (16) and the force acting on the hand on the object. In a system equipped with a hybrid position and force control device (17) that controls the position and force of a robot (16), the robot (16) is positioned at a starting point of a copying operation specified on the surface of a workpiece. The robot by control (
16) After the spatial movement of the hand, a temporary target force vector is commanded in the direction of the object at the starting point, and pressing control is performed with one degree of freedom of translation in the direction of the temporary target force vector being constrained; The robot (1) detected at the starting point
6) is characterized by having a pressing control means (19A) which completes the control when the translational one-degree-of-freedom constraint direction component of the force acting on the object by the hand becomes equal to the magnitude of the tentative target force vector. robot tracing control device. 2) The unit vector ■ in the direction of the target force vector is in the first column,
From the work coordinate system to the reference coordinate system, which is perpendicular to the vector ■ and has the unit vector ■ in the hand movement direction of the robot (16) in the third column, and the cross product of the two vectors ■×■=■ in the second column. Using a transformation matrix, the robot (16)'s hand is calculated based on the deviation between the target position and the current position, and the deviation between the target force value of the robot (16) and the force acting on the hand on the object. In a system equipped with a hybrid position and force control device (17) that controls the position and force of a robot (16), a start point T_1 and an end point of a copying operation specified on the surface of the workpiece of the robot (16) are provided. Point T_2 and point T_
For the via point T_3 of the copying work between 1 and T_2,
a copying route setting means (18) that sets the intersection line between the plane Φ determined by the three points T_1, T_2, and T_3 and the surface of the workpiece as a route for the copying work; and the workpiece of the robot (16). After spatial movement of the hand of the robot (16) by positioning control to the starting point of the copying work specified on the surface, a temporary target force vector is commanded in the direction of the object at the starting point, and the temporary target is Pressing control is performed with one translational degree of freedom in the force vector direction constrained, and the component of the force acting on the object of the robot (16) hand detected at the starting point in the translational one degree of freedom constraint direction is a pressing control means (19A) that completes the control when the magnitude of the temporary target force vector becomes equal to the magnitude of the tentative target force vector; The point is the origin of the work coordinate system, the unit vector in the direction opposite to the direction of the force acting on the object of the hand detected at the contact point is the vector ■, and the modulus of the vector ■ and the plane Φ The starting point T_1 to the straight line that has the cross product of the forward direction vector ■×■=■ as a direction vector and passes through the contact point
The unit vector in the direction from the drawn perpendicular leg P_t_1 to the perpendicular leg P_t_2 drawn from the end point T_2 is the vector ■, and the target force vector is the vector ■ whose magnitude is equal to the tentative target force vector. A copying control device for a robot, comprising copying work system setting means (19B) for setting a vector in the same direction as the vector. 3) The unit vector ■ in the direction of the target force vector is the first column,
From the work coordinate system to the reference coordinate system, which is perpendicular to the vector ■ and has the unit vector ■ in the hand movement direction of the robot (16) in the third column, and the cross product of the two vectors ■×■=■ in the second column. Using a transformation matrix, the robot (16)'s hand is calculated based on the deviation between the target position and the current position, and the deviation between the target force value of the robot (16) and the force acting on the hand on the object. In a system equipped with a hybrid position and force control device (17) that controls the position and force of a robot (16), the contact force at the point of contact between the hand of the robot (16) and the workpiece of the robot (16). The unit vector of ■
The existing work system is determined to be inappropriate when there is a directional component of a coordinate system compatibility determining means (19C) for determining the coordinate system compatibility determining means (19C);
C), when the existing coordinate system is determined to be inappropriate, stops the movement of the hand, resets the work coordinate system and the target force vector, and then causes the robot (16) to continue the work again by imitating the robot (16). robot tracing control device.