Profile Control System for Robots

The present invention relates to a robot control system and, more specifically, to a profile control system for a robot which permits the robot to perform a profiling operation along a surface of a work object having curved surfaces of unknown contours on the basis of force acting between the end of the robot and the work object that is sensed by, for example, an inner force sensor while resetting a coordinate system for the profiling operation.

With a force control robot system, in order to perform a profiling operation to move the end of a robot from a work start point to an end point while pressing it on a surface of a work object, the direction n in which the end of the robot is pressed which coincides with the direction of the normal line of the work object at a point of contact between the end of the robot and the work object and a profile coordinate system Ow -Xw, Yw, Zw defined by the direction o in which the robot moves while performing the profiling operation need to be entered into a robot controller.

FIG. 1 is a diagram illustrating the profile coordinate system. As shown, reference coordinate axes Xo, Yo and Zo are taken with, for example, the center O of the bottom surface of the robot body at the origin 1 of a reference coordinate system. Provided are joints 2 and 3 on coordinate axis Zo, a manipulator 5 having an arm between joints 3 and 4, and an inner force sensor 8 between a joint 6 in front of joint 4 and a hand 7. The hand 7 is shown in contact with a work object 9. The point of contact between hand 7 and work object 9, the direction of the inward normal line of the work object at the point of contact between the hand and the work object and the direction of movement of the hand are used as the origin Ow, the coordinate axes Xw and Yw of the profile coordinate system, respectively. The direction normal to the coordinate axes Xw and Yw is used as the coordinate axis Zw. Unit vectors in the directions of the coordinate axes Xw, Yw and Zw are given by n, o and a, respectively.

As described above, the profile coordinate system is determined by the position and attitude of the tip (the hand) of the robot relative to the work object. Unit vector n represents the direction in which the robot is pressed on the work object in applying force to it, and unit vectors o and n representing the directions of movement of the robot's tip are normal to each other. The other unit vector a is given as an outer product of n and o as follows.

Here the three unit vectors n, o and a may be represented in the reference coordinate system O -Xo, Yo, Zo and is generally given by ##EQU1## where T represents a transposed matrix.

FIG. 2 illustrates a relation between a work object with curved surfaces and the profile coordinate system. It is assumed that the robot performs a profiling operation on a side surface of work object 9 and the origin of the profile coordinate system is first placed at point P1. At this time the vector n is normal to the side surface of work object 9 and the vectors o and a are on the tangent plane. When the robot performs a profiling operation to move in the direction of vector o, the direction n of the normal line on the surface will not coincide with the direction in which the robot is pressed on the work object. Then the force applied to the work object by the robot will deviate from a preset value. In order to perform the profiling operation with an accurate force, therefore, it is required to change the profile coordinate system set at point p1 to a profile coordinate system for a point P2.

As described above, in order to perform the profile control by a force control robot, it is required to enter profile coordinate systems into a robot controller. In prior art systems the profile coordinate systems have usually been set by teaching the robot by an operator. In such a case, when a work object has curved surfaces as shown in FIG. 2, the teaching of the profile coordinate systems to the robot will inevitably become troublesome.

Another method of entering working coordinate systems into the robot controller involves the prediction of the direction of movement of the robot by the use of information on the current position of the robot. In this case, since the vectors in the direction of movement of the robot will be represented by estimated values, a deviation will be produced between an estimated position and a position to which the robot is actually moved.

It is a first object of the present invention to provide a robot profile control system which permits a robot to perform a profiling operation while applying a predetermined constant force to a work object having curved surfaces of unknown contours and automatically generating profile coordinate systems utilizing detected force by an inner force sensor mounted, for example, between a wrist and a hand of the robot.

It is a second object of the present invention to provide a robot profile control system which permits a robot to return to its original trajectory quickly and to perform a profiling operation again on a surface of a work object having discontinuous surfaces of any contours when its hand separates from the surface of the object.

It is a third object of the present invention to provide a robot profile control system which permits position error correction to keep force applied to a work object constant by generating a force velocity command calculated from an estimated position error of a robot's hand at a point of contact between the robot and the work object at the time of setting of the next profile coordinate system which is obtained by estimating from the curvature of a surface of the object when the robot performs a profiling operation on the work object having surfaces of unknown contours.

It is a fourth object of the present invention to provide a robot profile control system which permits a robot to perform a profiling operation while applying a constant force to a side surface of an unknown work object rectangular in shape without teaching from an operator.

It is a fifth object of the present invention to provide a robot profile control system which permits a robot to perform a profiling operation while applying a constant force to a work object having a surface which is in the process of being profiled and a surface for the next profile operation which makes an acute angle therebetween.

A feature of the present invention resides in a profile control system for a force control robot system including a robot, a force detecting section for detecting a force acting between the end of said robot and a work object and a position detecting section for detecting the position of the end of said robot and permitting said robot to perform a profiling operation on a surface of said work object, comprising normal vector calculating means responsive to an output of said force detecting section for calculating a normal vector at a point of contact between the end of said robot and said work object, movement calculating means responsive to outputs of said normal vector calculating means and a vector in the direction of movement of the end of said robot at an end point or a start point of an profile operation provided from an operator for calculating a vector in the direction of movement of the end of said robot, control command generating means responsive to outputs of said movement calculating means and said normal vector calculating means for generating control commands to said robot, and force and position control means responsive to outputs of said control command generating section, said force detecting section and said position detecting section for controlling the force of said robot and the position of the end of said robot, thereby permitting said robot to perform an profiling operation on a surface of said work object having an unknown contour while resetting a profile coordinate system defined by said normal vector and said vector in direction of movement.

FIG. 1 is a diagram for use in explanation of a profile coordinate system;

FIG. 2 is a diagram illustrating a relation between a work object having curved surfaces and a profile coordinate system;

FIG. 3 is a basic block diagram of a robot profile control system;

FIG. 4 is a functional block diagram illustrating a robot profile control method;

FIG. 5 is a basic block diagram of a first embodiment of the present invention;

FIG. 6 is a diagram illustrating a method of setting a profile coordinate system in the first embodiment;

FIGS. 7A and 7B are diagrams illustrating method of calculating reaction force and set force Fr at a point of contact between the robot and the work object;

FIG. 8 is a diagram illustrating a method of setting a vector a when vectors n and L coincide with each other;

FIG. 9 is a block diagram of a robot profile control apparatus according to the first embodiment;

FIG. 10 is a detailed block diagram of the position controller and the force controller of FIG. 9;

FIG. 11 is a block diagram illustrating a system configuration of the profile control apparatus according to the first embodiment;

FIGS. 12A, 12B and 12C are flowcharts of the profile operation in the first embodiment;

FIG. 13 is a diagram for use in explanation of a method of setting a profile coordinate system when a vector Oop in direction of movement is provided by an operator;

FIGS. 14A, 14B and 14C are flowcharts of the profile operation in the first embodiment when the vector in direction of movement is provided by an operator;

FIG. 15 is a basic block diagram of a second embodiment of the present invention;

FIG. 16 is a diagram for use in explanation of a method of setting a profile coordinate system when a profile operation is performed on a work object having surfaces which are discontinuous;

FIGS. 17A to 17B are diagrams illustrating a search for a robot's trajectory;

FIGS. 18A to 18B illustrate time response of pressing force F at the time of the search for the trajectory;

FIGS. 19A, 19B and 19C are flowcharts of a process when the robot's end contacts the work object in the second embodiment;

FIGS. 20A and 20B are flowcharts of a process when the robot's end separates from the work object in the second embodiment;

FIG. 21 is a basic block diagram of a third embodiment of the present invention;

FIG. 22 is a diagram for use in explanation of a deviation of contact force from a set force;

FIG. 23 is a diagram for use in explanation of a method of calculating the curvature of a surface of a work object;

FIG. 24 is a diagram for use in explanation of a method of calculating a force velocity command;

FIGS. 25A, 25B and 25C are flowcharts of a profiling operation in the third embodiment;

FIG. 26 is a basic block diagram of a fourth embodiment of the present invention;

FIG. 27 is a diagram illustrating a relation between a work object and a manipulator in the fourth embodiment;

FIG. 28 is a diagram for use in explanation of a profile operation in the fourth embodiment;

FIGS. 29A, 29B and 29C illustrate time responses of the force and position in the profile operation in the fourth embodiment;

FIG. 30 is a block diagram of the robot profile control apparatus in the fourth embodiment;

FIG. 31 illustrates a system configuration of the profile control apparatus in the fourth embodiment;

FIGS. 32A, 32B and 32C are flowcharts of the profiling operation in the fourth embodiment;

FIG. 33 is a basic block diagram of a fifth embodiment of the present invention;

FIG. 34 is a diagram illustrating a position relation between a work object and a manipulator in the fifth embodiment;

FIG. 35 is a diagram for use in explanation of a profile operation in the fifth embodiment;

FIGS. 36A, 36B and 36C illustrate time responses of the force and position in the profile operation in the fifth embodiment;

FIG. 37 illustrates a relation between the profile coordinate system in position P1 and the profile coordinate system position P3;

FIGS. 38A, 38B, 38C, 38D and 38E illustrate profiling operation in the fifth embodiment; and

FIGS. 39A, 39B and 39C illustrate the industrial applicability of the present invention.

FIG. 3 is a basic block diagram of a profile control system for a force control robot which includes a force detector 11, e.g., an inner force sensor, for detecting the force applied to a work object by a robot 10 and a position detector 12 for detecting the position of the robot, e.g., the position of its hand by use of an encoder mounted on a motor for driving a manipulator of the robot.

In FIG. 3, a normal vector calculating means 13 calculates a normal vector at a point of contact between the end (hand) of the robot and the work object from an output of force detector 11. Movement calculating means 14 calculates a vector in direction of movement of robot 10 from an output of normal vector calculating means 13, the current position of the robot's end output from position detector 12 and, for example, the end position of a profile work which has been entered in advance by an operator.

A control command generating means 15 is responsive to the outputs of normal vector calculating means 13 and movement calculating means 14 to provide desired values of the force and position of robot 10 and a feedback gain serving as a control parameter. Normal vector calculating means 13, movement calculating means 14 and control command generating means 15 may be implemented by a computer.

A force and position control means 16 is responsive to a deviation of a force detect output of force detector 11 from a force desired value output from control command generating means 15 and a deviation of a robot's end position from position detector 12 from a position desired value output from control command generating means 12 to control the force and position of robot 10.

FIG. 4 is a functional block diagram illustrating a profile control method of a force control robot. From the force detected between the robot and the work object a unit vector in the opposite direction to the force is first obtained in block 20. The opposite direction unit vector is transformed from a profile coordinate system to a reference coordinate system to obtain a normal vector n at the point of contact between the robot and the work object in block 21.

In block 22 a unit vector L which points in the direction of the end position of a profile operation from the current position of the robot's end is obtained. From the outer product of the normal vector n and the unit vector L in the direction of the end position a unit vector a on the tangent plane is obtained in block 23 as a unit vector in the same direction as the outer product. From the outer product of a and n a unit vector o pointing in the direction of the outer product, that is, in the direction in which the robot's end moves is obtained in block 24. In block 25, assuming that the work object for the profile operation has surfaces of unknown contours, the normal vector n and the unit vector o which points in the direction in which the robot's end moves are reset at regular intervals to continue the profiling operation until its end position is reached.

Namely, in the present invention, the normal vector n is calculated by normal vector calculating means 13 by transforming the unit vector, which points in the opposite direction to the force acting between robot 10 and the work object detected by force detector 11, to the reference coordinate system. Next, the unit vector a on the tangent plane between the robot's end and the work object is obtained by movement calculating means 14 from the normal vector n and the unit vector L which points in the direction of the end position. Also, the unit vector o pointing in the direction in which the robot's end moves is obtained from a and n. Using those vectors n and o the force and position control commands are entered into force and position control means 16 by control command generating means 15 as desired values for controlling the force and position of robot 10. Assuming that the surface of the work object is unknown, the normal vector n and the unit vector o which points in the direction in which the robot's end moves are reset at regular intervals, thereby performing the profiling operation.

As can be seen, according to the present invention, a profiling operation for a work object having curved surfaces can be performed automatically.

A first embodiment of the present invention will be described first, which corresponds to a first object of the present invention, that is, a robot profile control system which permits a robot to perform a profiling operation on a work object having curved surfaces of unknown contours while automatically generating profile coordinate systems by use of detected force by an inner force sensor.

FIG. 5 is a basic block diagram of the first embodiment of the present invention. In comparison with FIG. 3, FIG. 5 is the same except that normal vector calculating section 13a, movement calculating section 14a and control command generating section 15a correspond to normal vector calculating means 13, movement calculating means 14 and control command generating means 15, respectively, and force and position control means 16 is divided into an operation section 17, a position control section 18 and a force control section 19.

FIG. 6 illustrates a method of setting profile coordinate systems according to the present invention. It is assumed that a profiling operation is performed from a starting point Pb to an end point Pe on a surface of a work object as shown. A description will first be given of the method of calculating the profile coordinate system at the start position of the profile operation, that is, the starting point Pb. In the present invention, when the starting and end points of a profiling operation is given, control is performed such that constant force is applied to the work object, and a vector o in direction of movement of the robot's end always lies on the plane formed by a straight line connecting a point of contact between the robot and the work object with the end point of the profile operation and a normal vector n at the point of contact.

The normal vector n.sub.B at the starting point is found from reaction force F that the robot's hand receives from the work object at the starting point. FIGS. 7A and 7B illustrates a relation between the reaction force F that the robot's hand receives from the work object and an inner force sensor coordinate system Os -Xs, Ys, Zs. The inner force sensor detects components fx, fy and fz in the directions of the sensor coordinate axes Xs, Ys and Zs. Thus, the reaction force .sup.S F will be given in vector representation as follows.

Here s indicates that the reaction force F is described in the inner force sensor coordinate system Os -Xs, Ys and Zs. A description of torque components detected by the inner force sensor is omitted herein.

The normal vector .sup.S n.sub.B represented in the inner force sensor coordinate system is a vector opposite in direction to .sup.S F and given in component representation as follows. ##EQU2## The magnitude of vector F is given by

The representation of the normal vector .sup.S n.sub.B, which is represented in the inner force coordinate system, in a reference coordinate system needs a coordinate transformation matrix for transformation from the inner force sensor coordinate system to the reference coordinate system. It is assumed that the component representation in the reference coordinate system of unit vectors in the directions of the coordinate axes Xs, Ys and Zs in the sensor coordinate system is given by ##EQU3## Note that, for example, the superscript o of the unit vector .sup.o ns in the direction of the coordinate axis Xs in the sensor coordinate system indicates that the unit vector is described in the reference coordinate system. The transformation matrix .sup.o As for transformation from the sensor coordinate system to the reference coordinate system is given by

By the use of the transformation matrix the normal vector .sup.o nB in reference coordinate system representation will be given by

Next, the calculation of a vector in direction of movement of the robot's hand at the starting point PB using the above normal vector .sup.o nB will be described. The vector in direction of movement oB is a vector that is normal to the normal vector nB and exists on the plane made by a straight line connecting the starting point PB with the end point PE and the normal vector nB. A unit vector LB in the direction of the straight line connecting the starting point Pb with the end point PE is given, using position vectors .sup.o PB and .sup.o PE of the starting and end points in the reference coordinate system, by

At this time .sup.o aB, which is on the tangent plane contacting a surface of the work object at the starting point PB and one of the unit vectors in the directions of coordinate axes of the profile coordinate system, is given, using .sup.o nB and .sup.o LB, by

Moreover, .sup.o OB, the vector in direction of movement of the robot's hand, is also given, as a vector on the tangent plane, by

In the profiling operation of the force control robot, control commands for force and position, namely, a desired force and a desired position, output from control command generating section 15a of FIG. 5 are applied to force control section 19 and position control section 18. The generation of the desired force Fr will be described first. To generate a force Fr (scalar quantity) set for the work object at the starting point Ps of the profiling operation, it is only required to generate a force which has a magnitude of Fr and a direction of nB as shown in FIG. 7B. Thus, the set force vector .sup.o Fr is given, using equation (8), by

Next, a desired position to which the robot is to move, which is given at the starting point PB, is given as a command for a position relative to the starting point PB. The direction of movement from the starting point PB is given by the vector .sup.o O.sub.B in direction of movement in equation (9). The relative position command is given, using equation (8), by

where .alpha. is an appropriate constant corresponding to a distance to be corrected in the profile coordinate system as described in connection with FIG. 2. The correction of the profile coordinate system may be made:

(1) when a fixed time elapses;

(2) when the reaction force of the work object is deviated from the set force to some extent or more; or

(3) when the robot moves a fixed distance.

In the present embodiment, it is assumed that the correction of the profile coordinate system is made under the condition (1), namely, when a fixed time elapses. Under the conditions (2) and (3) as well, the principle of the present invention remains unchanged as under the condition (1).

In FIG. 6, the setting of a normal vector n, a vector o in direction of movement, a desired force Fr and a desired position X at a midway point, e.g., point Pi in the profiling operation is made in exactly the same way as at the starting point PB. That is, the normal vector ni and the vector oi in direction of movement are found using equations (1) through (9) and the set force vector and the relative position command are found using equations (10) and (11).

The calculation of the normal vector is performed, using .sup.S Fi as the reaction force at point Pi and the coordinate transformation matrix oAsi for transformation from the sensor coordinate system to the reference coordinate system, such that ##EQU4##

In the present embodiment, as described above, the vector a on the tangent plane is found from the normal vector a at a point of contact between the robot's hand and the work object and the unit vector L in the direction of the end point of the profile work seen from the point of contact. In a special case the vectors n and L may correspond to each other. FIG. 8 shows such a case. In the case of FIG. 8, the normal vector n2 at the point P2 of contact and the unit vector L2 in the direction of the end point PE correspond with each other. In such a case, the outer product of vectors n2 and L2 cannot be found. Thus, the vector a2 on the tangent plane cannot be found by the use of equation (8). It is therefore decided to find the vector a2 on the tangent plane, using the vector o1 in direction of movement of the robot's hand at the previous point P1 of contact near to the contact point P2, as follows:

It is assumed here that the point P1 is a set point of a profile coordinate system immediately before the correction of the profile coordinate system at the point P2. The vector .sup.o o2 in direction of movement is given, as the outer product of two unit vectors .sup.o a2 and .sup.o n2, by

When .sup.o n2=.sup.o o1, a vector on the tangent plane cannot be found using equation (14). Such a case, which will arise from a collision of the robot with the work object, will not be taken into consideration in the present invention.

FIG. 9 is a block diagram of a robot profile control apparatus in the first embodiment of the present invention. The control apparatus comprises a manipulator 26 of the robot, an operation section 27 of the manipulator, a position detecting section 28 of the manipulator, a force detecting section 29 of the manipulator, a coordinate transforming section 30 for transforming an angle .theta..sub.s of a drive motor of manipulator 26, which is an output of position detecting section 28, to the position Xo of the end of the robot's hand, a deviation section 31 for obtaining a difference between a desired position supplied from a host computer 37 and an output of coordinate transforming section 30, a position control section 32 for providing a velocity command signal Vp specifying a velocity in the direction of position control using an output of deviation section 31 and a position control parameter from host computer 37, a deviation section 33 for obtaining a difference between a force command supplied from host computer 37 and an output Fo of force detector 29, a force control section 34 for providing a velocity command Vf specifying a velocity in the direction of force control using an output of deviation section 33 and a force control parameter from host computer 37, an adder section 35 for computing the sum of an output Vp of position control section 32, an output Vf of force control section 34 and a speed command Vo from host computer 37, and an inverse Jacobi transformation section 36 for transforming an output Vk of adder section 35 to a rotating speed .theta. of each joint of the manipulator. The inverse Jacobian matrix J-1 refers to an inverse matrix of a Jacobian matrix J which is defined by the structure of each joint in an equation relating a small displacement (velocity V) of the robot's hand and a small-angle displacement (angular velocity .theta.) with respect to a little time, which is given by

Deviation section 26a on the output side of manipulator 26 is adapted to apply a displacement to inner force sensor 29a, for example, a strain gauge. That is, the instant the end of the robot's hand of manipulator 26 contacts the work object, no displacement is applied to sensor 29a so that its output is 0. When the end of the robot's hand is further pressed against the work object, a difference, .DELTA.XE=X.sub.a -XE, is produced between the position X.sub.a of the hand's end (no deformation of the sensor is involved) which is held in the controller and the position XE of the work object. Consequently a reaction force Fh proportional to the difference will be output form sensor 29a.

In FIG. 9, operation section 27 comprises a servo motor for driving manipulator 26, a power amplifier 27b, a D/A converter 27c and a compensator 27d. Position detecting section 28 comprises an encoder and counter 28a and a tachometer 28b. In addition, force detecting section 29 comprises an inner force sensor 29a and a coordinate transforming section 29b for transforming an output of sensor 29a represented in the sensor coordinate system to the reference coordinate representation. Encoder and counter 28a and tachometer 28b in position detecting section 28 provide signals for compensatory calculation to compensator 27 in operation section 27. The output of encoder and counter 28a, i.e., an angle .theta..sub.S of rotation of each joint, is entered into coordinate transforming section 29b of force detecting section 29 and inverse Jacobi transforming section 36.

Host computer 37 comprises a normal vector calculating section 38, a movement calculating section 39 and a control command generating section 40. Normal vector calculating section 38 calculates the normal vector an at a point of contact between the manipulator and the work object from an output of force detecting section 29, that is, a detected force Fo subjected to transformation to the reference coordinate system, the resultant normal vector being applied to movement calculating section 39 and control command generating section 40.

Movement calculating section 39 calculates the vector in direction of movement .sup.o o using the current position Xo of the robot's hand from coordinate transforming section 30 and the normal vector .sup.o n from normal vector calculating section 38 and applies it to control command generating section 40.

Control command generating section 40 transmits a desired position Xr, a force command Fr and position and force control parameters used in switching between profile coordinate systems to the robot controller using the outputs of normal vector calculating section 33 and movement calculating section 39, the detected force Fo output from force detecting section 29 and the hand current position Xo output from coordinate transforming section 30. In addition, control command generating section 40 generates status control signals to vector calculating sections 38 and 39.

The start and end points PB and PE of an profiling operation are specified by an operator to movement calculating section 39. PB and PE may be specified via control command generating section 40.

The control signals supplied from control command generating section 40 to the robot controller include desired position Xr applied to deviation section 31, unit vectors in the profile coordinate system and position feedback gain Cp to be described later which are applied to position control section 32 as the position control parameters, force command Fr applied to deviation section 33, unit vectors in the profile coordinate system and force feedback gain Cr to be described later which are applied to force control section 34 as the force control parameters and velocity command Vo applied to adder section 35 to move the hand's end at a high speed. Here the value of Vo may be provided directly from the operator or may be given by Vo=l/t using a distance l to move and a time t required to move the distance which are set by the operator.

The aim of entering the detected force Fo output from force detecting section 29 into control command generating section 40 is to detect the reaction force of the work object so as to decide whether or not the pressing of the robot against the work object has succeeded. Also, the aim of entering the hand current position Xo output from coordinate transforming section 30 into control command generating section 40 is to decide whether or not the end point of the profile work is reached.

FIG. 10 is a detailed block diagram of position control section 32 and force control section 34 of FIG. 9. Position control section 32 comprises a transposed orthogonal transformation matrix (RT) calculating section 32a for transforming an output of deviation section 31, that is, a position deviation represented in the reference coordinate system to the profile coordinate system, a selection matrix (I-Sf) calculating section 32b, an orthogonal matrix (R) calculating section 32c and a position feedback gain (Cp) calculating section 32d. Force control section 34 comprises a transposed orthogonal transformation matrix (RT) calculating section 34d for transforming a force deviation represented in the reference coordinate system output from deviation section 33 to the profile coordinate system, a selection matrix (Sf) calculating section 34c, an orthogonal matrix (R) calculating section 34b and a force feedback gain (Cf) calculating section 34a.

The orthogonal coordinate transformation matrix R used for coordinate transformation from the profile coordinate system (Xw, Yw, Zw) to the robot reference coordinate system (Xo, Yo, Zo) is given, using vectors n, o and a, by ##EQU5## The matrix R is a matrix with 6 rows and 6 columns. The elements in the first to third columns, the first to third rows of the matrix correspond to the transformation of coordinates x, y and z on X, Y and Z axes, and the elements in the fourth to sixth columns, the fourth to sixth rows of the matrix correspond to the transformation of angles of rotation .alpha., .beta. and .tau. about X, Y and Z axes. The other elements of the matrix are all 0.

If force is controlled in the direction of Xw and position is controlled in the directions of Yw and Zw in the profile coordinate system as shown in FIG. 1, then the selection matrix Sf of selection matrix calculating section 34c will be given by ##EQU6## If, in this case, torque is applied to an axis in the direction of press like thread fastening, the element S in the fourth row, the fourth column is 1, otherwise S=0. The selection matrix I-Sf used in selection matrix calculating section 32b of position control section 32 is obtained by subtracting Sf in equation (18) from a sixth order unit matrix I.

The position feedback gain Cp used in position feedback gain calculating section 32d of position control section 32 is generally given, in terms of the reference coordinate system, by ##EQU7##

Also, the force feedback gain Cf used in force feedback gain calculating section 34a of force control section 34 is given, in terms of the reference coordinate system, by ##EQU8##

As described above, in position control section 32, a position deviation, which is an output of deviation section 31, is subjected to coordinate transformation from the reference coordinate system to the profile coordinate system by transposed orthogonal matrix calculating section 32a and then multiplied by the selection matrix I-Sf to extract only a component in the direction of position control. The component is again transformed to the reference coordinate system by orthogonal matrix calculating section 32c and then multiplied by position feedback gain Cp to be sent to adder 35 as a velocity command Vp in the direction of position control.

FIG. 11 illustrates a system configuration of the profile control apparatus in the first embodiment. Host computer 37 has normal vector calculating section 38, movement calculating section 39, control command generating section 40 and communication control section 41. Robot controller 42 has memory 42a, communication control section 42b and force control, position control, coordinate transformation, deviation sections 42c. The robot controller controls manipulator 26 through operation section 27, position detecting section 28 and inner force sensor 29a. Host computer 37 and robot controller 42 are interconnected by a communication interface, such as a bus. Data are transferred between memory 42a and normal vector calculating section 38, movement calculating section 39 under the control of communication control sections 41 and 42 for controlling the timing of signal transmission and reception.

FIGS. 12A to 12C are flowcharts of the profiling operation in the first embodiment. The Figure is a flowchart of the profiling operation for a work object having curved surfaces of unknown contours in particular and illustrates the flow of processes performed by control command generating section 40, normal vector calculating section 38, movement calculating section 39 in host computer 37 and the robot controller. It is assumed here that the start and end points of the profile operation shown in FIG. 6 have already been given. The description of a method of moving the manipulator to the end point of the profile operation is omitted. In the Figure a subscript B corresponds to a process at the start point and a subscript i corresponds to a process at the time of setting of the i-th profile coordinate system.

In step S43 operator sets a start point PB, an end point PE, a set force Fr, a switching time .tau. of profile coordinate systems and a coefficient .alpha. for specifying a desired position. The operator further sets a force vector FB for calculating a normal vector at the start point in step S44. The control command generating section generates a force command FB and force control parameters for generating a force FB at the start point and transfers them to the robot controller in step S45. The robot controller responds to the command to generate force FB and presses the manipulator against the work object in step S46. If a reaction force F to be detected in step S47 is found to be 0 by the control command generating section in step S48, then the operation returns to step S44 so that force vector FB is set again. Note that a reaction force of 0 means that the robot is separated from the work object.

When reaction force F is not 0 in step S48, the normal vector calculating section calculates a normal vector .sup.o nB in sensor coordinate system in step S49, calculates a coordinate transformation matrix .sup.o As in step S50 and calculates a normal vector .sup.o nB represented in reference coordinate system in step S51. The normal vector nB is transferred to the control command generating section and the movement calculating section.

The movement calculating section calculates a unit vector pointed in the direction of the end point using equation (7) represented by position vectors at the start and end points of the profiling work in step S52, calculates a vector .sup.0 aB on the tangent plane using equation (8) in step S53 and calculates a vector .sup.0 oB pointing in the direction of movement in step S54. The vector .sup.0 oB is transferred to the control command generating section.

The control command generating section generates a desired force vector .sup.0 Fr using equation (10) and transfers it to the robot controller in step S55. Moreover, the control command generating section generates a desired relative position vector .sup.0 Xr using equation (11) and transfers it to the robot controller in step S56. The control command generating section applies a velocity command Vo for moving the robot's hand at high speed to the robot controller in step S57 so that the controller starts the profiling operation in step S58. The control command generating section monitors in step S59 whether or not a fixed time (T) has elapsed after the start of the profiling operation. The profiling operation is continued in step S58 until the time elapses. When it is decided in step S59 that the fixed time has elapsed, the position X of the robot's end is detected by the control command generating section in step S60 and a decision is made in step S61 as to whether or not the end point of the profiling operation is reached, that is, whether or not X=PE. When the end point is reached, the profiling operation comes to an end and subsequent operations will be performed.

When the end point is not reached in step S61, the setting of a new profile coordinate system and a profiling operation using the new coordinate system are repeated in steps following step S61. That is, first, a reaction force F is detected in step S62 and then a decision is made in step S62 by the control command generating section as to whether or not F=0. In case F=0, the operation is stopped because the robot has been separated from the work object for some cause.

When F.noteq.0 in step S63, a normal vector is calculated by the normal vector calculating section in steps S64 through S66 as in steps S49 through S51 and a vector pointed in the direction of movement is calculated by the movement calculating section in steps beginning with step S67. In this case, however, the current position of the robot's end is detected in step S67 before the same processes as in steps S52 to S54 are performed and the vector in direction of movement is calculated on the basis of the detection of the robot current position. Subsequently the control command generating section a desired force vector, a desired relative position vector and a velocity command in steps S71 to S73 as in steps S55 to S57. A profiling operation is performed by the robot controller in step S74.

The control command generating section monitors whether or not the fixed time .tau. has elapsed after the start of the profiling operation in step S75. If the time has not elapsed, the profiling operation is continued in step S74. When it is decided in step S75 that the fixed time has elapsed, the position of the robot's end is detected by step S76 and a decision is made in step S77 as to whether or not the detected position corresponds with the end point PE. When the detected position corresponds with the end point, the profiling operation is stopped and subsequent operations are performed. When the end point is not reached in step S77, a new profile coordinate system is set again and the profiling operation is continued under the new profile coordinate system from step S62. The above operations are repeated until it is decided in step S77 that the robot has arrived at the end point of the profiling operation.

In the first embodiment described in detail, the vector in direction of movement Oi of the robot's hand is obtained on the basis of the unit vector Li pointed from the current point Pi of contact on the work object in the direction of the end point PE of the profile operation as depicted in FIGS. 4 and 6. In a case where the end point PE of the profile operation is not given definitely as coordinates, the operator may provide a rough direction of movement of the robot's hand from the start point PB as a vector in direction of movement Oop.

FIG. 13 illustrates a method of setting a profile coordinate system when a vector in direction of movement Oop at the start point PB of the profiling operation is set by operator. To obtain the vector in direction of movement Oop at the start point PB, aB, which is one of unit vectors representing the coordinate axes of the profile coordinate system, is found, using .sup.o nB and Oop, to be

The vector in direction .of movement .sup.o OB can be obtained in exactly the same way as equation (9).

Where the vector in direction of movement is provided from the operator at the time of starting of the profiling operation, vectors in direction of movement at points of the profiling operation are controlled such that they always exist on the plane made by the normal vector at the start point PB of the profile operation and the vector in direction of movement provided from the operator.

In FIG. 13, a locus that the robot's end draws on a surface of a work object forms a curved line on the plane made by the normal vector nB and the vector in direction of movement OB at the start point. Thus, a vector in direction of movement Oi at each point Pi of the profiling operation will be normal to a third unit vector aB found at the start point. The vector in direction of movement Oi is also normal to the normal vector ni obtained by equation (13) and will be given by

ai, which is one of unit vectors representing the coordinate axes of the profile coordinate system, is given, using equation (22), by

When .sup.o aB=.sup.o ni or .sup.o aB=-.sup.o ni, equation (22) will not hold. In this case, the direction of movement of the robot's hand can be maintained by using the vector in direction of movement Oop given by the operator at the start point as a vector in direction of movement at point Pi.

The profile control apparatus when the vector in direction of movement Oop at the start point of the profiling operation is provided from operator may be the same as that in FIG. 9 except that the vector in direction of movement Oop is applied to vector in direction of movement calculating section 39 or control command generating section 40 in place of the end point PE of the profiling operation.

FIGS. 14A to 14C are flowcharts of the profile operation in the first embodiment where a vector in direction of movement at a start point is provided from operator. The flowchart of FIG. 14 is substantially the same as that of FIG. 12. Basic differences between the flowchart of FIG. 14 and the flowchart of FIG. 12 are that: a vector in direction of movement .sup.o Oop is given in step S78 in place of position vectors .sup.o PB and .sup.o PE at the start and end points in step S43 of FIG. 12; a decision is made as to whether or not a normal vector nB and a vector in direction of movement .+-..sup.o Oop coincide with each other in step S87 after step S86 corresponding to step S51, and when a coincidence occurs, steps beginning with step S78 are repeated; a vector .sup.o aB is calculated in step S 88 without calculating .sup.o LB in step S52 of FIG. 12; and a decision is made as to whether or not the end position X of the robot is located within an operation range in step S96 in place of deciding whether or not the end position X has arrived at the end point PE in step S61 of FIG. 12.

Note that when reaction force F is much greater than 1 in step S98, the operation is stopped. This is to stop the operation, for example, when the robot collides with a wall and the reaction force thus becomes great. Also, there is a slight difference between the flowcharts of FIGS. 12 and 14 in steps following step S99 because of a difference in the methods of calculating the vector in direction of movement .sup.o Oi. Furthermore, there is also a difference between the flowcharts in that the setting of a coordinate system is repeated in response to the decision that the robot end position X has not arrived at the end point in step S77 of FIG. 12, while it is repeated in response to the absence of a termination instruction from operator in step S112.

A second object of the present invention, that is, a robot's trajectory searching system for returning a robot to an original trajectory quickly when the robot' end is separated from a surface of a work object and permitting the robot to perform an profiling operation again will be described as a second embodiment of the present invention.

FIG. 15 is a basic block diagram of the second embodiment. Distinct from the basic block diagram of the first embodiment shown in FIG. 5, the second embodiment is provided with a trajectory searching section 120 responsive to outputs of force detecting section 11, position detecting section 12, normal vector calculating section 13a and movement calculating section 14a for searching a trajectory when the robot's hand is separated from the work object, the output of trajectory searching section 120 being applied to control command generating section 15a.

FIG. 16 illustrates a method of setting a profile coordinate system in a profile operation on a work object with discontinuous surfaces. The Figure corresponds to the case where the direction of movement at start point PB is provided from operator as a vector in direction of movement Oop in the first embodiment.

In general, in a profiling operation on a work object having discontinuous surfaces, there are a case where the robot's end keeps contacting a surface of the work object after it has passed a discontinuous point and a case where it separates from the work object. When the robot keeps contacting the work object, the profiling operation can be continued in exactly the same way as in the first embodiment.

Although FIG. 16 illustrates the case where vector in direction of movement Oop at the start point PB of the profiling operation is provided from the operator, the same control as in the first embodiment is also possible in the case where the end point PE is provided as in FIG. 6.

When the robot separates from the work object, on the other hand, a search for a trajectory is made by trajectory searching section 120 of FIG. 15.

FIGS. 17A and 17B are diagrams illustrating a trajectory search, in which a section of a work object is seen from above and the operation of the robot in the vicinity of a discontinuous point is depicted. As may be seen from FIGS. 17A, 17B, the method of searching for a trajectory varies with the form of a work object.

FIGS. 18A and 18B illustrate time responses of pressing force at the time of searching for a trajectory. FIGS. 18A and 18B correspond to FIGS. 17A and 17B, respectively. t0, t1, t2, t3, t4, t5, t6 represent arrival times of the robot in positions Pi0, Pi1, Pi2, Pi3, Pi4, Pi5, Pi6, respectively. The position of the hand itself is expressed by the position of its leftmost end as shown. The trajectory searching method will be described in conjunction with FIGS. 17, 18 and 15.

(1) The case of FIG. 17A

It is assumed that the profiling operation is performed in accordance with the control method described in the first embodiment at the i-th profile coordinate system set point Pi (=Pio). It is further assumed that the robot performs the profiling operation from Pio to Pi1 and arrives in position Pi1 at time t1 as shown in FIG. 17A. There is no work object on the right hand side of position Pi1. At time t1 a control command is not altered. Thus, the robot will move in the direction of ni so as to generate a pressing force in the direction of ni. Since there is also a component of movement in the direction of oi, the robot will advance from Pi1 to Pi2 in the end. In this way the robot may separate from the work object, depending on a relation between the form of the work object and the velocity at which the robot moves. In this case pressing force Fni in the direction of ni detected by force detecting section 11 of FIG. 15 will exhibit such a time response as shown in FIG. 18A. Namely, although a fixed pressing force continues to be generated during a time interval (t0-t1) when the robot contacts the work object, the pressing force is reduced to 0 at discontinuous point Pi1 where there is no reaction force because of the absence of the work object. Upon detection of such a variation in force as shown in FIG. 18A the control command generating section of FIG. 15 stores the coordinates of the hand in position Pi1 (discontinuous point) and commands the robot to stop. Upon receipt of the stop command from the control command generating section, force control section 19 of position control section 18 of FIG. 15 stops the robot. At this point the position of the robot is Pi2.

Next, the hand is rotated through an angle of 90.degree. so that the hand moves to a position Pi3 in which its leftmost end is located on an extension of vector oi. To change the attitude of the robot from Pi2 to Pi3, it is required that the hand be rotated clockwise by -.pi./2 about the ZH axis of the current hand coordinate system OH -XH, YH, ZH. The axis ZH is pointed downward from the drawing paper. The hand coordinate system is a coordinate system which is determined by the position and attitude of the hand relative to the manipulator. Unit vectors nH, oH and aH are provided for coordinate axes XH, YH and ZH, respectively. A transformation matrix for transforming the hand coordinate system nH2, oH2, aH2 at point Pi2 to the hand coordinate system nH3, oH3, aH3 at point Pi3 is expresses, using a rotatory coordinate matrix E.sup.k(-.pi./2), by ##EQU9## Assuming Pi1, Pi2, Pi3 to be position vectors at points Pi1, Pi2, Pi3, since point Pi3 is a point on vector oi, point Pi3 is given by

After the position and attitude of the hand is changed to Pi3, the robot is pressed in the direction opposite to vector oi. The robot is pressed against the work object until the set force is reached. The robot contacts the work object in position Pi4. At time t4 the set force is generated as shown in FIG. 18(a). When the contact with the work object is restored, the profiling operation according to the control method described above becomes possible. Thus, the same control as in the first embodiment is performed again.

(2) The case of FIG. 17B

In the case where the work object has such a form as shown in FIG. 18B, if a pressing force is merely generated in the direction opposite to the vector oi after the position and attitude of the hand have been changed to Pi5 as performed in the case (1), the hand will not come into contact with a surface B. Thus, a profile operation cannot be performed on surface B. For this reason, to perform the profile operation on surface B, the following control is performed after the pressing operation is performed from position Pi5.

As can be seen from FIG. 17B, it is when surface B exists in the quadrant IV of a coordinate system having a discontinuous point as its origin, ni as its X axis and oi as its Y axis that a force pressed against surface B has a component in the opposite direction to vector ni. It is when the robot's end passes a ni-ai plane that it enters the quadrant IV during profiling operation from Pi5 to Pi6. Thus, when any pressing force is not sensed after the robot's end has passed the ni-ai plane, the surface B may be considered to be in the quadrant IV.

To restart the profiling operation from the discontinuous point, the instant the robot's end passes the ni-ai plane, the pressing operation which has been performed from Pi5 is stopped. The position of the robot's end at this point is Pi5. Point Pi5 coincides with point Pi1. The hand is pressed while being rotated in the direction of a left-hand screw of vector ai (counterclockwise in FIG. 17) with the robot's end fixed at Pi5. The robot is pressed against the work object until the set force is reached. The robot contacts the work object in position Pi6(=Pi+1). As shown in FIG. 18B the set force is generated at time t6. When contact with the work object is restored, the profiling operation can be made possible in accordance with the abovedescribed control method as with the case (1).

The profile control apparatus according to the second embodiment is substantially the same as that of FIG. 9, inclusive of the detailed arrangements of the position control section and the force control section of FIG. 10, except that host computer 37 involves trajectory searching section 120 which is responsive to coordinate transformation sections 29b, 30, normal vector calculating section 38, movement calculating section 39 and control command generating section 40 for making a search for a trajectory and outputs its result to control command generating section 40.

The system configuration of the profile control apparatus is also the same as that of FIG. 11 according to the first embodiment except that host computer 37 involves trajectory searching section 120 which communicates data with control command generating section 40 and the communication interface and is connected to communication control section 41, vector in direction of movement calculating section 39 and so on.

FIGS. 19A to 20B are flowcharts of the profiling operation according to the second embodiment. More specifically, FIGS. 19A to 19C are flowcharts of the profiling operation when the robot keeps in contact with the work object and FIGS. 20A and 20B are flowcharts when the robot separates from the work object. The flowchart of FIG. 19 is substantially the same as that of FIG. 14 of the first embodiment in which the vector Oop in direction of movement of the robot end at the start point of the profiling operation is provided from operator and thus corresponding parts are shown by like numerals. A difference between the flowcharts of FIGS. 19 and 14 is that the robot is stopped when reaction force F equals 0 in step S98 of FIG. 14, while, in FIG. 19, when F=0 in step S98a, it is decided that the robot's hand passed a discontinuous point of the work object and separated from the object and then processes of FIG. 20A and 20B are performed (the same in S111a).

A flowchart in which the end point PE of the profiling operation is provided from the operator as in FIG. 6 may be the same as that of FIG. 12 except that when reaction force F is 0 in step S63, the processes of FIG. 20A and 20B are performed.

In FIG. 20A and 20B, when the robot passes the discontinuous point and reaction force F is detected to be 0, the position Pi1 of the robot's end is detected and stored by the trajectory searching section in step S121, a robot stop command is issued from the control command generating section in step S122, and the robot is stopped by the robot controller in step S123. The stopped position Pi2 of the robot's end is detected and transferred to the trajectory searching section in step S124.

The hand coordinate system at the end stopped position Pi2 is transformed to the hand coordinate system at point Pi3 using equation (24) by trajectory searching section in step S125. Using equation (25) position Pi3 is calculated in step S126. Data on the position and attitude at Pi3 are transferred to the control command generating section in step S127. The control command generating section applies a command to move to position Pi3 to the robot controller in step S128 so that the robot's end is moved to position Pi3 in step S129. In step S130, the control command generating section generates a command of force in the opposite direction to vector oi and force control parameters, which are, in turn, transferred to the robot controller.

Upon receipt of the command the robot controller presses the robot in the direction opposite to vector oi. The robot's end position is detected by the control command generating section in step S132. In step S133 a decision is made by the trajectory searching section as to whether or not the robot's end has passed ni-ai plane. When the plane is not reached by the robot's end, reaction force F is detected by the control command generating section in step S134. In step S135 the trajectory searching section decides whether its absolute value is positive or not. When it is not positive, the robot's end has not come in contact with the work object yet. For this reason, the processes are repeated from step S131. When it is positive, the robot's end has been pressed against the work object. Thus, the operation returns to step S112 in which a decision is made as to whether or not the operator has instructed the robot to stop. When such an instruction is not issued yet, the processes are repeated from step S99.

When it is decided in step S133 that the robot's end has passed ni-ai plane, on the other hand, the situation of FIG. 17B has been brought about. As a result, a robot stop command is issued from the control command generating section in step S136 so that the robot is stopped by the robot controller in step S137. In step S138 a command for torque in the direction of left-hand screw of vector ai and force control commands are generated by the control command generating section for transmission to the robot controller.

Upon receipt of the command the robot controller rotates the robot's end in step S139. Reaction force F is detected by the control command generating section in step S140 and a decision is made in step S141 by the trajectory searching section as to whether or not the absolute value of the reaction force F is positive. When it is not positive, the processes are repeated from step S139. When it is positive, on the other hand, the operation returns to the process in step S112.

It is, of course, possible to terminate the profiling operation when the end point is reached so as to perform the next operation in place of a termination instruction by the operator in step S112 of FIG. 19.

A third object of the present invention, that is, a position deviation compensating system for estimating, at a profile coordinate system setting point, a position deviation of the robot's end which may occur at the next coordinate system setting point from the curvature of a work object to keep a force applied to the work object constant will be described as a third embodiment of the present invention.

FIG. 21 is a basic block diagram of the third embodiment of the present invention. In comparison with the basic block diagram of the first embodiment shown in FIG. 5, the third embodiment is substantially the same as the first embodiment except that control command generating section 15a involves a curvature calculating section 150 for calculating the curvature of a work object so as to estimate a position deviation of the robot's end from the curvature and a position deviation compensating section 151 for generating a force velocity command corresponding to the position deviation estimated from the curvature.

In the third embodiment, the setting of a profile coordinate system and the control of a profile operation are performed in the same manner as in the first embodiment. In the second and third embodiments, however, the position command and the force command are generated merely from a profile coordinate system at the current point to control the profiling operation. Therefore, a problem with the first and second embodiments is that, when profiling a surface with a large curvature, the robot will not follow a variation in the surface and a set force will not be held from a time of setting the profile coordinate system at the current point to a time of setting the next profile coordinate system.

FIG. 22 illustrates situations in profiling a surface A with a large curvature and a surface B with a small curvature at a constant velocity of movement. It is assumed that, as in FIG. 22, a profile coordinate system n, o is set a point Pi on a work object and a force F is applied at point Pi. The robot's end moves in the direction of vector o up to vo .tau. at a constant velocity vo and is displaced in the direction of vector n up to a position which satisfies the balance of a spring system formed of the robot and the work object until the next sampling time .tau.. At this time, the robot's end moves up to point PA,i+1 on surface A and up to point PB,i+1 on surface B. The amounts of displacement to points PA,i+1 and PB,i+1 in the direction of vector n are assumed to be .delta.A and .delta.B, respectively. However, since a force velocity command v to compensate for a difference between a desired force and a contact force at point Pi is generated in the direction of vector n, the effective amounts of displacement will be .delta.A-vr and .delta.B-vr. Assuming a spring constant of the spring system formed of the robot and the work object to be k, forces acting on the work object at points PA,i+1 and PB,i+1 in the direction of vector n will be F-k(.delta.A-vr.tau.) and F-k(.delta.B-vr.tau.), respectively. Since .delta.A>.delta.B, if the curvature of a surface is large, a deviation of a pressing force on the work object from a set force will also become large. Thus, if a work object having a curvature is profiled without taking a position deviation at the next contact point into consideration, a difference will be produced between a pressing force on the work object and a set force, depending on the magnitude of the curvature.

That is, if a profile coordinate system is set at a point at which the robot comes in contact with the work object at sampling intervals so as to instruct a correct direction of control, it becomes possible to hold a mechanical force between the robot and the work object constant at each of sampling points. If, however, a work surface varies to a large extent between sampling points as shown in FIG. 22, the contact force between the robot and the work object will deviate from a set force. In the third embodiment, therefore, a position deviation at the next sampling point is estimated from the curvature of a work surface and a force velocity command is generated which takes the estimated position deviation into account, to thereby control a mechanical force so that its deviation from the set force may be small.

FIG. 23 illustrates a method of calculating the curvature at point Pi. FIG. 24 illustrates a method of setting a force velocity command. Hereinafter a position deviation compensating apparatus for keeping contact force between a robot and a work object constant will be described with reference to FIGS. 21, 23 and 24.

(1) Calculation of the curvature

For a work object having an unknown contour, the curvature up to a point at which the next sampling is made is estimated using information on the current point Pi and the point Pi-1 of one sampling step before. It is assumed here that the contour of the work object in the vicinity of points Pi and Pi-1 can be approximated by a sphere. Assuming the radius of the sphere to be r, the curvature k is given by 1/r.

When the position vectors at points Pi and Pi-1 are .sup.o Pi and .sup.o Pi-1 and the normal vector at point Pi is .sup.o ni as shown in FIG. 23, the curvature at point Pi will be found. Since it is assumed that the contour in the vicinity of point Pi can be expressed by a sphere, the center of a circle of curvature will be the intersection of normal vector .sup.o ni and the perpendicular bisector of a line segment PiPi-1. The vector pointed from point Pi to center R of the circle of curvature will be represented, using normal vector .sup.o ni, to be ri, .sup.o ni (ri is the radius of the circle of curvature at point Pi). It will be understood from FIG. 23 that the following relation hold.

Thus, the radius ri of the circle of curvature will be found to be ##EQU10## where .sup.o ni(.sup.o Pi-.sup.o Pi-1) represents the inner product of vectors.

From equation (27) the curvature .kappa. will be found to be ##EQU11## (2) Position deviation compensating apparatus

As indicated in FIG. 22, when the contact force at the current point agrees with the desired force, a force velocity command generated by a difference between the contact force and the desired force is zero. Thus, a deviation of the contact force in the direction of vector n from the set force at the next sampling point depends only on a displacement .delta. in the direction of vector n and the contact force varies from the set force by k.delta.. (k is a spring constant of the spring system formed of the robot and the work object.) It is therefore required to generate a force velocity command in the direction of the normal in such a way to cancel the estimated value .delta. of a displacement of force occurring after sampling time .tau., which has been found on the assumption that the contour of the work object in the vicinity of points Pi, Pi-1 can be approximated by a sphere.

Assuming that .delta. is a force velocity command to be generated, .tau. is a sampling time and .delta. is a displacement in the direction of vector n after the sampling time, it is required to satisfy

A method of calculating a force velocity command to satisfy the above condition will be described using FIG. 24. In the Figure, the current position of the robot end is point Pi. If the robot travels from point Pi in the direction of vector o at a command velocity .vertline.Vo.vertline., then the robot end will arrive at point Pi+1' on the circle of curvature (the estimated position at the time of the next sampling) after the next sampling time .tau.. A displacement .delta. in the direction of vector n which occurs when the robot arrives at Pi+1' will be found, using ri obtained in equation (27), to be ##EQU12## From equations (29) and (30) the force velocity command is given by ##EQU13## The force command velocity v is represented in vector form by

If a profile operation is performed using the command velocity .sup.o V.sub.n found by equation (32), it can be controlled in such a way that a deviation of a contact force working between the robot and the work object from a set force will not become large even during a time interval between sampling times.

The above calculation is performed by curvature calculating section 150 and position deviation compensating section 15 using the position of the robot end detected by position detecting section 12 of FIG. 21, the normal vector calculated by normal vector calculating section 13a, and the vector in direction of movement calculated by movement calculating section 14a.

The profile control apparatus according to the third embodiment is the same as that shown in FIG. 9 according to the first embodiment, inclusive of the position control section and the force control section of FIG. 10, except that control command generating section 40 incorporates curvature calculating section 150 for calculating the curvature at the current robot end position and position deviation compensating section 151 for calculating a force velocity command used to compensate for a position deviation estimated by the curvature. The system configuration of the profile control apparatus is also the same as that of FIG. 11 except that control command generating section 40 in host computer 37 contains the curvature calculating section and the position deviation compensating section.

FIGS. 25A to 25C are flowcharts of a profile operation according to the third embodiment of the present invention. The flowchart is similar to that of the first embodiment shown in FIG. 14 and thus their differences will be described mainly. In FIG. 25A to 25C, first, parameters are set in step S152 as in step S78 of FIG. 14 and then the same processes up to step S157 as those up to S78 in FIG. 14 are performed. In FIG. 25, to calculate the curvature, the robot end position PB is stored in a memory by the control command generating section prior to step S159 corresponding to step S84 in FIG. 14. In step S174 as well, the robot end position .sup.o Pi is stored in the memory prior to the step corresponding to step S99 of FIG. 14. In step S177 corresponding to step S101 of FIG. 14, in addition to that normal vector .sup.o ni is calculated and transferred to the control command generating section, it is stored in the memory.

In FIGS. 25A to 25C, the compensation for position deviation is performed by the curvature calculating section and the position deviation compensating section after step S180 corresponding to step S104 in FIG. 14. That is, in step S181, the radius of a circle of curvature is calculated in accordance with equation (27) using normal vector .sup.o ni, the robot end position of one sampling step before and the current robot end position. In step S183, a position deviation is calculated by the position deviation calculating section using equation (30). In step S183, a force velocity command .sup.o Vn is set using equation (32), and, in step S184, .sup.o Vn is commanded by the control command generating section in addition to Vo in step S105 of FIG. 14. The subsequent processes are the same as those in FIG. 14. The processes are repeated from step S174 until a termination instruction by the operator is detected in step S191.

It is, of course, possible to decide whether or not the end point of the profile operation is reached in place of the decision of the termination instruction by the operator in step S191.

A fourth object of the present invention, that is, a profile control system for performing a profiling operation while exerting a fixed force on a side surface of a work object having an unknown rectangular shape will be described as a fourth embodiment.

FIG. 26 is a basic block diagram of the fourth embodiment of the present invention. In comparison with the first embodiment of FIG. 5, the fourth embodiment is provided with a position storing section 200 which is responsive to a command generated by control command generating section 15a when force detected by force detecting section 11 becomes approximately 0 to store a robot end position detected by position detecting section 12 in place of normal vector calculating section 13a and vector in direction of movement calculating section 14a.

FIG. 27 illustrates a relation in position between a work object and a robot's manipulator. In the Figure P1, P2 and P3 designate respective positions of the robot's end. In performing a profile operation on a side surface A of a rectangular object in FIG. 27, a profile coordinate system is defined by a vector n perpendicular to the side surface and vectors o and a parallel to the side surface.

The profile coordinate system set in the position P1 in FIG. 27 needs not be altered so long as the profile operation is performed on the side surface A. However, since the profiled surface is changed from surface A to surface B when the robot moves from position P1 to position P2, the profile coordinate system also needs to be changed to a coordinate system for position P3.

FIG. 28 illustrates a profile operation. In the Figure, a is pointed downward in the direction perpendicular to the drawing paper. The Figure is a view of the work object seen from above (from the direction perpendicular to surfaces A and B) and illustrates how the robot operates in the vicinity of position P2.

FIGS. 29A to 29C illustrate time responses of force and position at the time of a profile operation. In the Figure, t(P1), t(P2) and t(P4) represent times at which the robot arrives at positions P1, P2 and P4. As opposed to FIG. 17 of the second embodiment, in FIG. 28, the position of the robot's end is defined by the center of the hand holder.

FIGS. 29A to 29C illustrate the time hysteresis of a pressing force Fn in the direction of n and the robot end positions (Xow, Yow) at the time of the profile operation on surface A. (Coordinates of Zw axis are omitted.)

FIG. 26 is a block diagram of a control system of a force control robot for changing the profile coordinate system described in position P1 to the profile coordinate system described in position P3 and changing the robot attitude from P2 to P3. A control method of permitting the robot to perform a profiling operation on a rectangular object will be described below in conjunction with FIGS. 26, 28 and 29.

As shown in FIG. 28, it is assumed that the robot arrives at position P2 at time t(P2) while performing profiling operation on surface A from P1 to P2. There is no object on the right-hand side of position P2. At this point the control command is not altered. Thus, the robot is going to move in the direction of n so as to generate the pressing force in the direction of n. Since there is also a component of movement in the direction of o, the robot will move from position P2 to position P4 in the end. At this point, the pressing force Fn in the direction of n detected by force detecting section 11 of FIG. 26 exhibits such a time response as shown in FIG. 29A. That is, the pressing force has a fixed magnitude during a period of time when the robot profiles surface A while moving from P1 to P2 and it becomes 0 due to the absence of the reaction force when the robot arrives in position P2 in which there is no object. The time responses of the robot end position Ow (Xow, Yow) detected by position detecting section 12 of FIG. 26 are shown in FIGS. 29A, 29B. As seen, the robot also moves in the direction of Xw upon arrival in position P2. Note that the robot is assumed to move at a constant velocity in the directions of Xw and Yw.

Control command generating section 15a of FIG. 26 stores the coordinates of position P2 (Xow, Yow indicated by dots in FIGS. 29B, 29C ) and issues a command to stop the robot immediately upon detection of such a variation in force as shown in FIG. 29A. Upon receipt of the stop command, position control section 18 and force control section 19 stop the robot. At this point the position of the robot is P4.

To change the profile coordinate system from the one at P1 to the one at P3 and change the robot attitude from P2 to P5, the robot has only to be rotated clockwise through -90 degrees around vector a. Thus, by multiplying the profile coordinate system [np1, op1, ap1] by a rotatory coordinate system E for the profile coordinate system Xw, Yw and Zw, the profile coordinate system [np3, op3, ap3] at P3 will be found to be ##EQU14## The change of the robot attitude from P2 to P5 will also obtained by performing the same operation on the hand coordinate system describing the robot end.

When the profile coordinate system at P3 and the robot attitude at P5 are obtained, control command generating section 15a of FIG. 26 sets necessary parameters in position control section 18 and force control section 19. Next, the robot attitude is changed so as to correspond with the newly set coordinate system and the robot is moved until its position in the direction of Xw comes to correspond with Xow at the dot in FIG. 29B stored in position storage section 200 (position P6). Next, the robot is pressed in the direction of -Yw and, when the set force is reached (position P5), the profile operation on surface B is resumed.

By performing the above operation on the other corners, the four surfaces of the object can be profiled. For simplicity, the above description was made on the basis of the profile coordinate system Ow -Xw, Yw, Zw. However, if the profile coordinate system is transformed to the reference coordinate system as described above, the description can be made likewise on the basis of the reference coordinate system.

FIG. 30 is a block diagram of a control apparatus according to the fourth embodiment of the present invention. In comparison with the control apparatus according to the first embodiment shown in FIG. 9, in the fourth embodiment, host computer 37 is provided with position storage section 200 for storing the robot end position using the output of coordinate transformation section 30 in place of normal vector calculating section 38 and vector in direction of movement calculating section 39. Position control section and force control section 34 of FIG. 30 are also the same as those in FIG. 10 in the first embodiment.

FIG. 31 illustrates a system configuration of the profile control apparatus according to the fourth embodiment. In comparison with FIG. 11 of the first embodiment, the host computer of the fourth embodiment is provided with memory 201 having position storage section 200 in place of normal vector calculating section 38 and movement calculating section 39.

FIGS. 32A to 32C illustrate, in flowchart form, the flow of processes performed by control command generating section 40, position storage section 200 and robot controller 42 in host computer 37 of the fourth embodiment. In the following description, the method of changing the robot from position P2 to position P5 of FIG. 28 will be detailed. The description of how to move the robot end to position P1 for the profiling operation on surface A is omitted.

When the hand attached to the manipulator performs profile operation on surface A in position P1 as shown in FIG. 28, the process in step S202 has already been terminated and steps S203 and S204 are being processed by the robot controller.

Control command generating section 40 monitors pressing force Fo of the hand, which is transmitted from the robot controller in step S204, in step S205 and decides whether the reaction force is zero or not, namely, whether or not the hand arrived in position P2 in which there is no reaction force from the work object in step S206. In view of an error this decision may be made depending on whether or not the absolute value Fo of Fo is smaller than Fset (<.vertline.). When it is decided that the robot arrived in position P2 because of the absence of the reaction force (Fo=0), control command generating section 40 transfers the coordinates of position P2 detected by position control section 28 and coordinate transformation section 30 in the robot controller to position storage section 200 in step S207. Position storage section 200 accepts and stores the coordinates of the position P2 in step S209. Next, control command generating section 40 issues a force command Fr=0, a velocity command Vo=0 and a desired position command Xr=the current position in step S210 and the manipulator is stopped by robot controller 42 in step S211. After the stop of the manipulator control command generating section 40 changes the coordinate transformation matrix R in accordance with equation (33) and transmits it to robot controller 42 in step S212. Robot controller 42 alters R, RT in position control section 32 and force control section 34 in step S213. The control command generating section calculates the attitude of the hand in according with the newly set profile coordinate system and sends a desired position Xr and a velocity command Vo to the robot controller in step S214. Responsive to the desired position Xr and the velocity command Vo the robot controller 42 changes the attitude of the hand and manipulator in step S215. Next, control command generating section 40 receives the coordinates (Xow, Yow, Zow) of position P2, which have been stored in position storage section 200 in step S209, in step S216 and issues a command to robot controller 42 to move to position P6 in step S217. After the stop of movement in step S218 control command generating section 40 issues a force command Fr and a velocity command Vo to robot controller 42 to press surface B at the set force Fr in step S219. After the commands have been issued, in step S222, control command generating section 40 monitors a force supplied from robot controller 42 in step S221 after the pressing in step S220. When it is decided in step S223 that the pressing force Fo has reached the set force Fr, the profiling operation command is issued again in step S224. Consequently a profiling operation is performed on surface B by robot controller 42 in step S225.

By repeating the above operation, the profiling operation can be performed on other surfaces.

A fifth object of the present invention, that is, a profiling operation control system for a work object in which a surface for the next profiling operation makes an acute angle with another surface, will be described as a fifth embodiment of the present invention.

FIG. 33 is a basic block diagram of the fifth embodiment. In comparison with the fourth embodiment in FIG. 26, the fifth embodiment is provided with a coordinate transformation matrix generating section 230 in control command generating section 15a.

FIG. 34 illustrates a relation in position between a work object having an acute angle and the manipulator. In the Figure, the angle made by surface A and surface B is an acute angle and P1, P2 and P3 represent robot end positions.

The profile coordinate system when the robot performs an profiling operation on a surface (for example, surface A) is set as shown in position P1 of FIG. 34. As shown, vector n is perpendicular to the surface, while vectors o and a are parallel to the surface.

The profile coordinate system set in position P1 of FIG. 34 needs not be altered so long as surface A is profiled. However, when the robot moves from position P1 to position P2, the profile coordinate system needs to be changed to the one set in position P2 because the profiling surface is changed from surface A to surface B.

FIG. 35 illustrates the profiling operation. FIG. 35 is a view of the work object seen from above (from the direction normal to surfaces A and B) and illustrates how the robot operates in the vicinity of position P2.

FIG. 36 illustrates time responses of force and position at the time of profiling operation. FIG. 36 illustrates time hysteresis of the pressing force Fn in the direction of n and the robot's hand position (Xh, Yh) at the time of profiling operation on surface A. t(P1), t(P2), t(P4), t(P5) and t(P6) are times of arrival of the robot in positions P1, P2, P4, P5 and P6. The hand position corresponds with that of FIG. 17 in the second embodiment.

FIG. 33 is a block diagram of a force control robot control system for changing the profile coordinate system described in position P1 to the profile coordinate system in position P3 and changing the attitude of the robot from P2 through P7 to P3. A control method for performing an profiling operation on a work object having an acute angle will be described below in conjunction with FIGS. 33, 35 and 36.

First, the position (Xh, Yh) of the hand at start point P1 of the profiling operation is stored in position storage section 200 of FIG. 33. It is assumed that the robot arrived in position P2 at time t(P2) as the result of profiling operation from P1 to P2 as shown in FIG. 35. There is no object on the right-hand side of position P2. At this time the control command is not altered so that the robot keeps moving in the direction of n so as to generate pressing force in the direction of n. Since there is also a component of movement in the direction of o, the robot will move from position P2 to position P4 in the end. At this point, the pressing force Fn in the direction of n detected by force detecting section 11 of FIG. 33 exhibits such a time response as shown in FIG. 36(a). That is, the pressing force has a fixed magnitude during a period of time (t(P1)-t(P2)) when the robot profiles surface A while moving from P1 to P2 and it becomes 0 due to the absence of the reaction force when the robot arrives in position P2 in which there is no object. The time responses of the robot hand position (Xh, Yh) detected by position detecting section 12 of FIG. 33 are shown in FIGS. 36A, 36B. As seen, the robot will also move in the direction of Xh upon arrival in position P2. Note that the robot is assumed to move at a constant velocity in the directions of Xh and Yh.

Control command generating section 15a of FIG. 33 stores the coordinates of hand position P2 (Xh, Yh indicated by dots in FIGS. 36B, 36C) and issues a command to stop the robot immediately upon detection of such a variation in force (Fn=0) as shown in FIG. 36A. Upon receipt of the stop command, position control section 18 and force control section 19 stop the robot. At this point the position of the robot is P4.

To change the robot attitude from P2 to P5, the hand has only to be rotated clockwise through -.pi./2 around Zh axis of the hand coordinate system OH-Xh, Yh, Zh. The hand coordinate system is defined by the position and attitude of the hand relative to the manipulator and unit vectors nH, oH, aH are defined for coordinate axes XH, YH, ZH. In FIG. 35, the hand coordinate system in position P4 is represented by nHP4, oHP4, aHP4 and the hand coordinate system in position P3 is represented by nHP5, oHP5, aHP5. Note that vectors aHP4 and aHP5 are pointed downward in the direction normal to the drawing paper.

A transformation equation for changing the hand attitude from P4 to P5 is expressed, using a rotatory coordinate transformation matrix E.sup.k(-.pi./2) for the hand coordinate system OH --XH, YH, ZH, by ##EQU15##

After the hand coordinate system has been changed to P5, the robot is pressed in the direction of -Yh until the set force is reached. The set force is reached in position P6. The response of force in the direction of n.sub.H becomes as shown in FIG. 36(a) and the force Fn is held constant after time t(P6).

Upon detection that the set force is reached at time t(P6), control command generating section of FIG. 33 issues a command to store the hand coordinates (Xh, Yh indicated by black triangles in FIGS. 35, 36(b), 36(c)) of position P6 to position storage section 200 of FIG. 33 and thus position storage section 200 stores the position of P6.

A method of finding the profile coordinate system for surface B from the coordinates of positions P1, P2, P6 which are stored in position storage section 200 of FIG. 33 in that way will be described. Let the position vector and the coordinates of the hand position be

.vertline.P.sub.1 (P.sub.1x, P.sub.1y, P.sub.1z),

.vertline.P.sub.2 (P.sub.2x, P.sub.2y, P.sub.2z),

.vertline.P.sub.6 (P.sub.6x, P.sub.6y, P.sub.6z)

In FIGS. 35 and 36, no Z coordinates are given because only the Xh-Yh plane is considered. To deal with generally, the Z coordinates are contained in the following description.

To change from the profile coordinate system in position P1 to the one in position P3 in FIG. 35, an angle .phi. made by surface A and surface B has to be found.

The angle .phi. is represented, using position vectors .vertline.P1, .vertline.P2 and .vertline.P6, by ##EQU16## where .multidot. indicates the inner product and .vertline..vertline. indicates the absolute value.

To change the profile coordinate system [nP1, oP1, aP1] at P1 to the coordinate system [nP3, oP3, aP3] at P3, [nP1, oP1, aP1] has only to be rotated through .pi.-.phi. about axis aP1 as can be seen from the relation between the coordinate systems at P1 and P3 of FIG. 37. From the profile coordinate system at P1 the profile coordinate system is found, using rotatory coordinate transformation matrix E.sup.K() for the profile coordinate system at P1, to be ##EQU17##

To change the attitude of the robot's hand from the status in position P2 to the status in position P7 as well, the hand coordinate system [nMP2 oMP2 aMP2] may be multiplied by the rotatory coordinate transformation matrix E.sup.(.pi.-.phi.) in equation (36) to obtain the hand coordinate system at P7. At the time of movement of the robot's hand from p6 to P7, the hand is moved displaced by l in the direction of the extension of surface B in view of the size of the hand (indicated by l in FIG. 35). At this point the coordinates and attitude of the hand are expressed by ##EQU18## The calculation of the coordinate transformation matrix and the vectors used in equations (34) to (38) is performed by coordinate transformation matrix generating section 230 of FIG. 33.

When the profile coordinate system at P3, the position of the robot and the hand coordinate system at P7 are obtained, control command parameters are set in position control section 18 and force control section 19 by control command generating section of FIG. 33. The robot moves from P4 to P7 while altering its attitude to perform pressing operation on surface B. The direction of the pressing operation at this point follows the profile coordinate system indicated in position P3. When the set force is reached in position P7, the profiling operation on surface B is resumed.

For simplicity the above description was given in terms of the Xh-Yh plane. By the use of the general equations (34) to (38) and the above-described coordinate transformation of the profile coordinate system to the reference coordinate system, the description can be given likewise in terms of the reference coordinate system.

The control apparatus according to the fifth embodiment is the same as that of FIG. 30 according to the fourth embodiment, inclusive of position control section 32 and force control section 34, except that control command generating section 40 contains coordinate transformation matrix generating section 230. The system configuration is also the same as that of FIG. 31 according to the fourth embodiment.

FIGS. 38A to 38E are flowcharts of the profiling operation according to the fifth embodiment, which illustrates the flow of processes performed by control command generating section 40, position storage section 200 and coordinate transformation matrix generating section 230 in host computer 37 and robot controller 42. In the following description a method of changing from position P2 to position P7 of FIG. 35 will be detailed, but a description of how to move the hand to position P1 to perform the profiling operation on surface A will be omitted.

In FIGS. 38A to 38E, control command generating section 40 issues a position storage command to robot controller 42 in step S231 when the hand comes in contact with surface A of the object. Robot controller 42 detects the coordinates of position P1 in step S232 and position storage section 200 stores the position in step S233. Control command generating section 40 issues a command for profiling operation on surface A in step S234 and robot controller 42 performs the profiling operation on surface A in step S235. The detailed description of the profiling operation on surface A is omitted.

In step S237, control command generating section 40 monitors hand pressing force F sent from robot controller 42 in step S236 while the hand profiles surface A. A decision is made in step S238 as to whether or not Fn is zero, that is, whether or not the reaction force is lost as a result of arrival of the hand in position P2. This decision may be made depending on whether or not the absolute value .vertline.F.sub.n .vertline. of F.sub.n is smaller than Fs.sub.e t (.ltoreq.1) in view of an error. When deciding that the hand has arrived in position P2 (Fn =0), control command generating section 40 transfers the coordinates (Xh, Yh) of the position P2 detected by position detecting section 28 and coordinate transformation section 30 in the robot controller to position storage section 200. In step S241 position storage section 200 receives and stores the coordinates of position P2 transmitted from robot controller 42 in step S240. Next, control command generating section 40 issues a force command Fr=0, a velocity command Vo=0 and a desired position command Xr (=current position) in step S242 and robot controller 42 stops the manipulator in step S243. After the manipulator has been stopped, control command generating section 40 calculates the attitude of the hand after being rotated through -.pi./2 around the Z axis of the hand coordinate system using equation (34) and sends a desired position command Xr and a velocity command Vo to robot controller 42 in step S244. Upon receipt of Xr and Vo, robot controller 42 changes the attitude of the manipulator and the hand in step S245. After the attitude has been changed, control command generating section 40 generates a pressing command in the negative direction of the Yh axis shown in FIG. 35 in step S246 and sends a force command Fr and a velocity command Vo to robot controller 42, which, in turn, performs a pressing operation in the -Yh direction in step S247. To monitor a force, control command generating section 40 receives the force, which is detected by robot controller 42 in step S248, in step S249. When deciding, in step S250, that the pressing force Fyh reached the set force Fr, control command generating section 40 issues to robot controller 42 a command to store position P6 in which the hand contacts surface B in step S251. Robot controller 42 detects the coordinates of P6 and sends them to position storage section 200 in step S252. Position storage section 200 receives and stores the coordinates of P6 in step S253. Control command generating section 40 issues a command to generate a matrix for coordinate transformation of the profile coordinate system and a command to transmit the coordinates of positions P1, P2 and P6 stored in position storage section 200 to coordinate transformation matrix generating section 230 in step S254. Coordinate transformation matrix generating section 230 calculates an angle .phi. made by surface A and surface B in accordance with equation (35) on the basis of the coordinates of positions P1, P2 and P6 sent from position storage section 200. Also, coordinate transformation matrix generating section 230 calculates the profile coordinate system at position P2 in step S257 and the coordinates of position P7 in the hand coordinate system and the attitude in position P7 in step S260. In steps S257 and S260, the results of calculation and a calculation termination flag is sent to control command generating section 40 at each calculation. Upon receipt of the termination flag, control command generating section 40 performs subsequent steps S258, S259, S261 and S262. Next, to contact surface B with a correct attitude, control command generating section 40 issues to robot controller a command to move to position P7 in step S263. Robot controller 42 moves the robot from P6 to P7 and changes its attitude in accordance with the received desired position, attitude Xr and velocity command Vo in step S264. After movement the profile coordinate system is switched to the coordinate system obtained in step S257 (steps S265, S266) and the pressing is performed in the direction of n in the new coordinate system (steps S267, S268). Control command generating section 40 decides whether the force Fn sent from the robot controller in step S269 has reached the set force (steps S270, S271). When the set force is reached, control command generating section 40 sends a profiling operation command again to robot controller 42 in step S272. Robot controller 42 permits the robot to profile surface B while generating a constant force Fr using the new coordinate system in step S273. The industrial applicability of the invention

The present invention detailed above can be applied to various works using robots. The works to which the present invention can be applied will be described in conjunction with FIGS. 39A to 39C. FIG. 39A illustrates works, which can be done by prior art systems, including fitting and cutting of an object whose contour is recognized. FIG. 39B illustrates works, which can be done by the prior art systems when a work object has a contour known in advance, including trimming, coating, polishing and rubbing. However, when the contour of a work object is unknown, the works of FIG. 39A are impossible. By the use of the present invention, the works of FIG. 39B can be done to a work object having an unknown contour. Furthermore, FIG. 39C illustrates works which cannot be done by any prior art system. The works includes recognition of an object's contour, assembly, and writing on a curved surface. Such works cannot be done until the present invention is used.