JP2682977B2 - Robot control method and device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

TECHNICAL FIELD The present invention relates to an industrial robot,
In particular, the present invention relates to a robot control method and apparatus for performing a compliance operation under the influence of an external force.

[0002]

2. Description of the Related Art Conventionally, industrial robots have been subjected to control for increasing position rigidity in order to realize position control at high speed and with high accuracy. In such a robot having a high position rigidity, when an external force is applied, the robot tries to maintain a predetermined command position by repulsing the external force. For example, if an industrial robot with high positional rigidity is used to perform assembly work that involves fitting parts together, even if assembly can be done easily by slightly changing the position of one of the parts, such flexibility Cannot perform certain actions.

A control method that alleviates the positional rigidity when the external force is applied to the robot and enables the escape follow-up operation in a predetermined fixed direction is realized as a compliance operation. One of the prior arts concerning the compliance operation is disclosed in, for example, JP-A-58-45891. In this prior art, on the wrist of the robot,
A dedicated compliance tool can be attached, and its elasticity can be used to achieve the operation of smoothly fitting the two parts together without causing kinking.

Another prior art is disclosed in, for example, JP-A-63-1.
39678. In this prior art, the force sensor detects the contact force of one component gripped by the hand of the robot with the other assembly component, and the compliance operation is achieved by the output from the force sensor. Still another prior art is, for example, Japanese Patent Laid-Open No. 60-132213.
Is disclosed. In this prior art, program control is performed to change the apparent stiffness of the device by adjusting parameters of a control system of the robot.

[0005]

Problems to be Solved by the Invention JP-A-58-4589
In a configuration in which a dedicated compliance tool is mounted as in the prior art 1 or a compliance operation is achieved based on detection of a contact force by a force sensor as in the prior art in JP-A-63-139678, the original configuration of the robot is used. It is necessary to add a new configuration to the robot, which impairs versatility as a robot.

In the technique of adjusting the parameters of the robot control system to realize the compliance operation as in the prior art of Japanese Patent Application Laid-Open No. Sho 60-132213, the compliance operation cannot be performed by accurately detecting the external force. Therefore, it is difficult to incorporate a force control system into the control system. Moreover, this is a passive operation due to external force, and cannot be operated by actively utilizing the functions of the robot.

An object of the present invention is to make it possible to perform a compliance operation by effectively utilizing the original configuration of the robot, and also to perform an active operation different from an external force during the compliance operation. A robot control method and apparatus are provided.

[0008]

SUMMARY OF THE INVENTION The present invention is a method of controlling a robot according to a movement target position Pe of a wrist of a robot designated in advance and a movement width w per unit processing step. The initial posture S of the tool attached to the robot and the initial position Ps of the wrist are calculated, and the movement vector seen from the base coordinate system of the robot is
A transformation matrix Q that is calculated from the motion target position Pe and the initial position Ps and that makes a direction predetermined in the base coordinate system parallel to the calculated movement vector is calculated, and each axis angle of the robot during the compliance motion is calculated. θ1 is detected,
The current position Pr of the wrist is calculated from each axis angle θ1, the current position Pr is multiplied by the conversion matrix Q, and the position ^q Pr in the Q coordinate system =
The step of calculating Q · Pr and the position ^q Pr ′ in which the movement width w is added to the component of the position ^q Pr in the predetermined direction are calculated, and the wrist position Pr ′ = Q ⁻¹ · ^q in the base coordinate system. The robot includes a step of calculating a position command value T including an attitude matrix based on Pr ′ and an initial attitude S, and a step of inversely converting the position command value T to calculate each axis command value and then driving each axis. Is repeated until the wrist of the robot reaches the operation target position Pe. According to the present invention, the initial posture S and the initial position Ps of the tool attached to the operation designated axis of the robot are calculated immediately before the compliance operation of the robot that moves along the external force by the compliance operation is started. Further, the movement vector viewed from the base coordinate system of the robot is calculated from the previously designated motion target position Pe and the initial position Ps, and the direction predetermined in the base coordinate system is made parallel to the calculated movement vector. The conversion matrix Q is calculated in advance. Each axis angle of the robot during compliance operation θ
1 is detected, the current position Pr of the wrist is calculated from each axis angle θ1, the current position Pr is multiplied by the conversion matrix Q, and the position ^q Pr = Q · Pr in the Q coordinate system is calculated. Position in Q coordinate system
position ^q Pr in Q coordinate system the movement width w only to a component of the moving direction predetermined by adding the ^q Pr 'movement position of the wrist in the base coordinate system corresponding to ^{the, Pr' = Q -1 · q} Pr ' Is calculated as A position command value T that enables the compliance operation is calculated based on the moving position ^q Pr ′ and the initial attitude S. By inversely converting the position command value T and calculating each axis command value, it is possible to perform the compliance operation while actively operating in a predetermined direction.

Further, according to the present invention, the axis gain value during the compliance operation can be arbitrarily set. According to the present invention, since the axis gain value during the compliance operation can be arbitrarily specified, the compliance can be adjusted by reducing the gain to increase the compliance or increasing the gain to increase the stiffness. Becomes possible.

Further, according to the present invention, the movement during the compliance movement is such that the detected axis angle θ1 exceeds a predetermined movement limit angle, or the movement amount in the base coordinate system and the tool coordinate system exceeds the predetermined movement range. Characterized by being sometimes restricted. According to the present invention, the movement range during the compliance operation can be set as each axis angle θ1, or the movement amount in the xyz direction in the base coordinate system and the xyz direction in the tool coordinate system. If the movement amount exceeds the preset movement range, the movement is restricted, so that the safety function provided in the robot can be utilized even during the compliance operation.

Further, the present invention is characterized in that gravity compensation calculation is performed during the compliance operation, and the calculation result is added to the torque command value of each axis. According to the present invention, the gravity compensation calculation is performed during the compliance operation, and the calculation result is added to the torque command value of each axis. Therefore, even if the posture of each axis of the robot changes during the compliance operation, the posture is not changed. The corresponding gravity compensation can be accurately performed. Since it is not necessary to use an integrator in the control system for gravity compensation, it is possible to eliminate the possibility of performing a sudden operation when switching between compliance control and normal control.

Further, according to the present invention, a tool is mounted on the wrist of the robot, and each axis angle θ1 of the robot driven by the drive source is detected by the axis angle detecting means, and the detected axis angle θ1 and the command value of the axis angle are detected. In the robot controller that controls the drive source in a negative feedback manner so that the deviation becomes zero, the initial attitude S and the initial position Ps of the tool are calculated immediately before the start of the compliance operation, and the movement vector viewed from the base coordinate system of the robot is calculated. From the operation target position Pe and the initial position Ps, and a direction predetermined in the base coordinate system is calculated as
Transformation matrix Q that is parallel to the calculated movement vector
Detecting each axis angle θ1 of the robot during compliance operation, calculating the current position Pr of the wrist from each axis angle θ1, and converting the current position Pr into the conversion matrix Q.
And a second calculation means for calculating a position ^q Pr = Q · Pr in the Q coordinate system, and a movement width specified in advance by a component of the position ^q Pr from the second calculation means in a predetermined direction. The position ^q Pr 'is calculated by adding w, and the wrist position P in the base coordinate system is calculated.
Third computing means for calculating a position command value T including a posture matrix based on r ′ = Q ⁻¹ · ^q Pr ′ and the initial posture S, and based on the position command value T in response to the output of the third computing means. A controller for a robot, comprising: a fourth calculation means for performing the negative feedback control of the drive source until the wrist of the robot reaches the operation target position Pe. According to the present invention, the initial posture S and the initial position Ps of the tool are set by the first computing means immediately before the start of the compliance operation.
, And the transformation matrix Q are calculated. The second calculation means is
Each axis angle θ1 of the robot in compliance operation is detected, and the moving width w is added only to the predetermined moving direction in the Q coordinate system to calculate the corresponding moving position Pr ′ in the base coordinate system, based on the initial posture S. The position command value T including the attitude matrix is calculated. The third calculation means inversely transforms the position command value T from the second calculation means, calculates each axis command value, and then drives each axis. The fourth calculating means responds to the output of the third calculating means so that the deviation between the designated value of the shaft angle included in the position command value T and the shaft angle θ1 detected by the shaft angle detecting means becomes zero. Negative feedback control of the drive source. By the negative feedback control of the fourth calculating means, it is possible to perform an active work in a predetermined moving direction while performing a compliance operation along an external force.

[0013]

1 shows the configuration of an industrial robot 1 used in an embodiment of the present invention. The industrial robot 1 automatically inserts the component 3 into the work 4 by using the hand 2 at the tip. The robot 1 has an arm 7 and an arm 8 on a base 6 installed on the floor surface 5, and a tool mounting arm portion 10 is connected to a flange 9 at the tip of the arm 8. The base 6 can be angularly displaced around a first axis 11 that is perpendicular to the floor surface 5. The arm 7 and the arm 8 can be angularly displaced with respect to the base 6 and the arm 7 around a second axis 12 and a third axis 13 which are parallel to the floor surface 5, respectively. The tool mounting arm unit 10 can be angularly displaced around a fourth axis 14 passing through the center of the arm 8. The tool mounting arm portion 10 has rotary joints that are displaceable around the fifth shaft 15 and the sixth shaft 16, and has arms 17, 18, and 19 connected by these joints.
A gear mechanism is provided in the arm 19, and the hand 2, which is a fingertip, is mounted on the extension of the axis 2a parallel to the sixth shaft 16, and the component 3 can be gripped. The component 3 gripped by the hand 2 is inserted into and fitted into a hole 21 provided in the work 4 held on the work table 20.

FIG. 2 shows a process of inserting the component 3 into the hole 21 of the work 4. For convenience of description, the hand 2 is not shown. FIG. 2A shows a state in which the component 3 is brought close to the hole 21. In FIG. 2 (2), since the component 3 and the hole 21 are eccentric, the tip of the component 3 contacts the slope of the hole 21, and the eccentricity different from the insertion direction is corrected in accordance with the compliance operation by the reaction force. Indicates a moving state. FIG. 2C shows a state where the tip of the component 3 is guided by the slope of the hole 21 and is correctly inserted into the hole 21 of the work 4. At the contact surface between the component 3 and the work 4, the pressing force and the reaction force due to the insertion of the component 3 stop at a position where they balance each other.

FIG. 3 shows a simplified structure of a control system for performing the position control operation and the compliance operation of the robot 1. In the control circuit 22 for each axis of the robot 1,
The servo control circuit 23 is connected, and further the host control circuit 24 is connected. The control circuit 22 for each axis is provided for each of the first to sixth axes 11 to 16 and has a similar configuration. Therefore, one control circuit 22 will be described as a representative. A drive current is supplied to a servo motor 25, which is a drive source that drives each axis by angular displacement, from an amplifier circuit 26 that can be called a base amplifier, and the servo motor 25 drives each corresponding axis. The rotation angle of each axis is detected by the encoder 27.

FIG. 4 shows a specific electrical configuration of the servo control circuit 23. A position command value for each axis is given to one input of the subtraction means 29 from the host control circuit 24 via a line 28. The signal representing the rotation angle of each axis from the encoder 27 is given from the line 30 to the other input of the subtraction means 29. The output of the subtracting means 29 is given to the coefficient unit 31, where it is amplified by the position gain Kp and added by the adding means 32.
Given to. By setting the position gain Kp to 0 or a value close to 0, the compliance operation can be performed. The output of the encoder 27 is also differentiated by a differentiating circuit 33, amplified by a coefficient gain 34 by a speed gain Kv, and given to an adding means 32. The output of the adding means 32 is the integrator 3
5 and the integral calculation is performed. The gain G1 of the integrator 35 is represented by the following first equation.

[0017]

(Equation 1)

Here, Kx is a constant and s is an operator. The output of the integrator 35 is applied to a phase compensator 36, which performs a phase compensation operation during the phase control operation.
2 is expressed by the following second equation.

[0019]

(Equation 2)

Where α is a constant. The output of the phase compensator 36 is provided to another adding means 37. The output from the gravity compensation calculation circuit 38 responsive to the output of the encoder 27 is given to the addition means 37 via a line 38 a and added, and the added output is supplied as a torque command value for each axis from the line 39 to the control circuit 22. Are input to the amplifier circuit 26. By performing gravity compensation, even if the function of the integrator 35 is stopped during the compliance operation, it is possible to prevent each arm of the robot from falling. By stopping the function of the integrator 35, it is possible to eliminate the risk of causing an unexpected and sudden movement of the arm when switching between the compliance operation and the normal operation.

FIG. 5 shows the configuration and operation of the control circuit 22. A signal indicating the torque command value of each axis from the servo control circuit 23 via the line 39 is given to the amplifier circuit 26 via the line 39, and the servo motor 25 is driven. Gravity acts on the arm, which is the load of the motor 25, as indicated by reference numeral 41. Therefore, a load such as an arm driven by the motor 25 is affected by the gravity 41. The influence of the gravity 41 is equivalently expressed as a subtraction by the subtraction means 42, but is compensated so as to be canceled by the output of the gravity compensation calculation circuit 38, and the arm or the like is moved. The angular acceleration of the inertia force 43 of the arm or the like is calculated by the integrating means 4
The angular velocity is obtained by integrating by 4. Further, by integrating the angular velocity by the integrating means 45, the rotation angle of the arm or the like is obtained. This rotation angle is detected by the encoder 27 as described above.

Next, the principle of performing the compliance operation while moving in the z-axis direction in the base coordinate system with the movement width w will be described. The origin position of the tool coordinate system when the compliance operation is started is tPΣt (θ0). When an external force is applied to the robot, the origin position of the tool coordinate system
If it moves to tPΣt (θ1), the following third equation is obtained. In addition, "T" of the right shoulder of a parenthesis shows a transposition symbol.

^T PΣ _t (θ1) = (d ^t PΣ _tx d ^t PΣ _ty d ^t PΣ _tz ) ^T (3) At this time, since the moving direction is the base z direction, coordinate conversion by the conversion matrix Q is omitted. By changing ^t PΣ _t , it is moved only in the z-axis direction. When the position command values of the x and y axis components are set to zero and the z axis component moved by an external force is given to the position command value of the z axis component, the following formula (4) is obtained.

^T PΣ _t.com = (0 0 d ^t PΣ _tz + w) ^T (4) The posture command value gives the value of the initial posture at the start of the compliance operation. By inverting the position and orientation command values, the angle command value θ _com of each axis can be obtained. By issuing this angle command value to the control circuit 22 for the servomotor 25, the origin of the tool coordinate system can be moved only in the z-axis direction, and its posture can be maintained.

FIG. 6 shows a compliance operation according to this embodiment for a more general case. Step a
The operation starts from 1 and moves to step a2 where each axis angle θ
Detects 0. In step a3, the tool initial position Ps and the tool initial attitude S shown in the fifth and sixth equations are detected, and the R matrix as shown in the seventh equation is obtained.

[0026]

(Equation 3)

At step a4, gravity compensation for the control circuits 22, 23, 24 of the robot 1 of the present invention is started. Further, the coefficient units 31,
34 and the gain of the amplifier circuit and the like are changed and set small. For example, the position gain Kp and the velocity gain Kv of the sixth axis 16 on which the tool 10 is mounted are set small,
Further, the gain Ki of the integrator 35 is set to zero, and this integrator 35 is
Is cleared to zero and the function of the phase compensator 36 is stopped. In other words, it is assumed that the output of the subtracting means 32 is directly supplied to the adding means 37. Step a5
Now, set each axis movement range. In step a6, the moving range for linear movement is set. In step a7, the movement width w is designated. Next, the compliance operation is started in step a8, the operation target position Pe is designated in step a9, and the movement vector Rse is calculated in step a10.
Furthermore, the movement vector Rse
When the coordinate conversion is performed, a conversion matrix Q representing a conversion in which the movement vector is parallel to one of the coordinate axes x′y′z ′, for example, the x ′ axis in the converted Q coordinate system is calculated.

[0028] Set ^{q Pse = Q · Pse ... (} 8) Step a11 end time when the in, when it is not determined that the set time has elapsed in step a12, step a13, the first shaft 11 Each axis angle θ1 with respect to the sixth axis 16 is detected by the following Expression 9.

_Θ1 = (θ _JT1.1 θ _JT2.1 θ _JT3.1 θ _JT4.1 θ _JT5.1 θ _JT6.1 ) ^T (9) In step a14, it is determined that each axis value is within the set range. If so, the present position Pr of the wrist is calculated in accordance with each axis angle θ1 in step a15. Next, step a1
6, the current position Pr is converted into the Q coordinate system according to the following formula (10).

[0030] ^{q Pr = Q · Pr ... (} 10) determines whether the host vehicle has reached the step a17 target position. That is, together with step a12, the operation end is determined by whether or not any of the following conditions is satisfied. Does the current position Pr of the wrist reach the target movement position Pe? Has the preset time elapsed since the operation started?

When it is determined in step a18 that the amount of movement is within the set range, the process proceeds to step a19, and the position moved by the movement width w in the Q coordinate system, for example, in the x'direction.
^{Find q} Pr '. In step a20, the moving position ^q Pr 'in the Q coordinate system is changed to the base coordinate system in accordance with the following eleventh equation, and the moving position Pr' in the base coordinate system is calculated.

Pr ′ = Q ⁻¹ · ^q Pr ′ (11) At step a21, the R matrix corresponding to the moving position Pr ′ and the posture S is obtained, and the position command value T of each axis is generated by inverse transformation. At step a22, each axis is driven based on the generated position command value T for each axis. Next, the process returns to step a13, and the operations up to step a22 are repeated.

When it is determined in step a12 that the set time has elapsed, the compliance operation is ended in step a23. If it is determined in step a14 that each axis value is not within the set range, the operation ends in step a24.
When it is determined in step a17 that the target position has been reached, the operation ends in step a25. If it is determined in step a18 that the movement amount is not within the set range, step a
The operation ends at 26.

FIG. 7 shows a simplified state of coordinate conversion by the conversion matrix Q. From the initial position Ps to the operation target position Pe
The transformation matrix Q is obtained as a transformation such that the x ′ axis obtained by transforming the x axis into the Q coordinate system becomes parallel to the movement vector Pse up to. By converting any one of x'y'z 'to be parallel to one axis, the process of adding the movement width w becomes easy. The transformation matrix Q is represented by 4 rows × 4 columns as shown in the seventh equation.
Of these, if the portion excluding the fourth row is Q ′, the following twelfth expression is obtained.

Q ′ = [n, o, a, p] ^T (12) where p is a zero vector and n, o, a are represented by the following thirteenth, fourteenth and fifteenth expressions, respectively. Is a vector.

[0036]

(Equation 4)

FIG. 8 schematically shows another embodiment of the present invention. In this embodiment, the hand 52 of the industrial robot 51 holds the material 53 to be polished and polishes it by the polishing device 54. In the polishing device 54, the polishing belt 55 is the pulley 5
It is stretched between 6 and 57 and transported at high speed. The industrial robot 51 realizes the operation of pressing the polishing target material 53 against the polishing belt 55 with a constant pressing force proportional to the set movement width while performing the compliance operation. The configuration of the control system of the industrial robot 51 is the same as that shown in FIGS. 3 to 5, and the integrator 3 is operated during the compliance operation.
Cut off 5. Even if the belt reaction force from the polishing belt 55 fluctuates, the position is fed back so that the deviation becomes constant, and the deviation amount is proportional to the pressing amount, so that stable polishing operation is possible.

The above-described operation can be similarly applied to a multi-axis robot such as a 6-axis robot and other types of robots.

[0039]

As described above, according to the present invention, an active operation in a predetermined moving direction and a follow-up operation by a compliance operation for releasing an external force can be easily performed.

Further, according to the present invention, since the moving direction by the active motion is opposite to the direction in which the external force acts,
According to the magnitude of the external force, it is possible to perform an operation such that the movement in the movement direction is continued, stopped, or returned in the opposite direction to the movement direction.

Further, according to the present invention, since the axis gain value during the compliance operation can be arbitrarily specified, the position rigidity and the compliance can be adjusted according to the state of generation of the external force.

Further, according to the present invention, even during the compliance operation, each axis angle θ1 and the amount of movement in the base coordinate system and the tool coordinate system are limited within predetermined ranges. It is possible to easily ensure safety and reliability without performing an operation that exceeds the operation range of.

Further, according to the present invention, since the gravity compensation calculation result is added to the torque command value of each axis, the gravity compensation can be accurately performed even if the posture changes during the compliance operation.

Further, according to the present invention, immediately before the start of the compliance operation, the initial posture S of the tool, the initial position Ps,
And the transformation matrix Q is calculated. During the compliance operation, each axis angle θ1 of the robot is detected, and the movement width w is added only to the predetermined movement direction in the Q coordinate system with respect to the current position Pr based on each axis angle θ1. The moving position Pr ′ in the system is calculated, and the position command value T including the attitude matrix based on the initial attitude S is calculated. Since the drive source is negatively feedback controlled so that the deviation between the designated value of the shaft angle included in the position command value T and the detected shaft angle θ1 becomes zero, while performing the compliance operation in accordance with the external force, It is possible to perform active work in a defined moving direction.

[Brief description of the drawings]

FIG. 1 is a front view of an industrial robot 1 according to an embodiment of the present invention.

FIG. 2 is a simplified cross-sectional view showing the relationship between the component 3 and the work 4 in FIG.

3 is a block diagram showing a simplified electrical configuration of the industrial robot 1 of FIG.

4 is an electrical circuit diagram showing a specific configuration of the servo control circuit 23 of FIG.

5 is a block diagram for explaining a function of a control circuit 22 of FIG.

FIG. 6 is a flowchart showing operations of the servo control circuit 21 and the host control circuit 22 of FIG.

FIG. 7 is a schematic diagram showing the concept of conversion to the Q coordinate system in the operation of FIG.

FIG. 8 is a simplified front view showing another embodiment of the present invention.

[Explanation of symbols]

 1,51 Industrial Robot 2,52 Hand 3 Protector 4 Pipe 10 Tool 11 1st Axis 12 2nd Axis 13 3rd Axis 14 4th Axis 15 5th Axis 16 6th Axis 21 Hole 22 Control Circuit 23 Servo Control Circuit 24 Host control circuit 25 Servo motor 26 Amplifying circuit 27 Encoder 29 Subtracting means 31,34 Coefficient unit 32,37 Adding means 38 Gravity compensation circuit 53 Grinding material 54 Polishing device 55 Polishing belt

Claims (5)

(57) [Claims] 1. A method of controlling a robot according to a movement target position Pe of a wrist of a robot designated in advance and a movement width w per unit processing stage, wherein an initial posture of a tool attached to a wrist is provided immediately before a compliance operation is started. S and the initial position Ps of the wrist are calculated, the movement vector viewed from the base coordinate system of the robot is calculated from the motion target position Pe and the initial position Ps, and the direction predetermined in the base coordinate system is set to the calculated movement vector. A conversion matrix Q for parallelization is calculated in advance, each axis angle θ1 of the robot in compliance operation is detected, and the wrist current position Pr is calculated from each axis angle θ1.
The current position Pr is multiplied by the conversion matrix Q, and the position in the Q coordinate system is calculated.
^The step of calculating ^q Pr = Q · Pr, and the position ^q Pr ′ in which the movement width w is added to the component of the position ^q Pr in the predetermined direction are calculated, and the wrist position Pr ′ = Q ⁻ in the base coordinate system. A step of calculating a position command value T including a posture matrix based on ¹ · ^q Pr ′ and an initial posture S, and a step of inversely converting the position command value T, calculating each axis command value, and then driving each axis The robot's wrist moves to the target position P
A robot control method characterized by repeating until e is reached. 2. The robot control method according to claim 1, wherein the axis gain value during the compliance operation can be arbitrarily set. 3. The movement during compliance operation is limited when the detected axis angle θ1 exceeds a predetermined movement limit angle or when the movement amount in the base coordinate system and the tool coordinate system exceeds a predetermined movement range. The method for controlling a robot according to claim 1 or 2, characterized in that: 4. The robot control method according to claim 1, wherein a gravity compensation calculation is performed during the compliance operation, and the calculation result is added to the torque command value of each axis. 5. A tool is attached to a wrist of a robot, and each axis angle θ1 of the robot driven by a drive source is detected by an axis angle detecting means, and a deviation between the detected axis angle θ1 and a command value of the axis angle is determined. In a robot controller that negatively feedback controls a drive source so that the drive source becomes zero, an initial posture S of the tool is set immediately before the compliance operation is started.
And the initial position Ps are calculated, the movement vector viewed from the base coordinate system of the robot is calculated from the motion target position Pe and the initial position Ps, and the direction predetermined in the base coordinate system is made parallel to the calculated movement vector. The first calculation means for calculating such a conversion matrix Q and each axis angle θ1 of the robot during compliance operation are detected, and the present position Pr of the wrist is calculated from each axis angle θ1.
The current position Pr is multiplied by the conversion matrix Q, and the position in the Q coordinate system is calculated.
Second calculation means for calculating ^q Pr = Q · Pr, and a position ^q Pr 'is calculated by adding a predetermined movement width w to a component of the position ^q Pr from the second calculation means in a predetermined direction. Then, the wrist position Pr 'in the base coordinate system = Q ^-1 · ^q Pr'
And a third computing means for calculating a position command value T including a posture matrix based on the initial posture S and a negative feedback control of the drive source based on the position command value T in response to the output of the third computing means. Controller for performing the operation until the wrist of the robot reaches the operation target position Pe.