JP3156877B2 - Robot controller - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot controller for moving a hand with respect to a work object.

2. Description of the Related Art In recent years, it has been practiced to perform a predetermined repetitive motion on a work object by a robot to save labor. When the hand connected to the tip of the robot comes into contact with the work target and the robot receives some constraint from the environment, it is desired that the robot can automatically perform an operation according to the movable direction in which the robot can move and the constraint direction in which the constraint is received. ing.
Therefore, it is necessary to give a command in the constraint direction based on the contact force between the robot and the member so that the work object or the robot itself is not damaged.

[0003]

2. Description of the Related Art FIG. 5 is a diagram for explaining an object to be worked by a conventional robot.

FIG. 5A shows a state in which a member 11 to be worked is placed on a member 11 to be worked.
In the case of performing a coating operation or a polishing operation while contacting the hand 13 attached to the tip of the robot via the force sensor 12, position control is performed in the X direction, which is the movable direction, and force control is performed in the Y direction, which is the constraint direction. , The robot can be moved while applying a constant force to the member. In other words, a command based on the contact force in the constraint direction is generated by hybrid control of the position and force in the position control in the movable direction and the force control in the constraint direction.

FIG. 5B shows an OA device 1 such as a display of a laptop personal computer.
When the opening / closing operation of the opening / closing portion 14a is performed by the hand 13 to which the force sensor 12 of the robot is attached, the movable direction is a tangent to a circle drawn by the hand, and the restraining directions are two directions (circular (The main normal direction and the subordinate normal direction), and by performing position control and force control respectively, the opening / closing operation of the opening / closing section 14a can be executed.

FIG. 6 shows a configuration diagram of a conventional robot control device. In FIG. 6, the robot controller 21
The operator 22a sends a command of the restraining direction and the movable direction to the operation sequence generation unit 23 via the terminal 22, the operation sequence generation unit 23 generates and sends a position signal to the position control unit 24, and the force control unit 25 Generate and send a force signal to

On the other hand, the position of the control object 26 is detected by the position detecting section 27 and sent to the position controlling section 24, and the force (contact force) is detected by the force detecting section 28 and sent to the force controlling section 25.

Then, a position control signal from the position control unit 24 and a force control signal from the force control unit 25 are sent to the operation unit 29, and the operation unit 29 controls and drives the control object 26.

That is, when performing an operation using hybrid position and force control, the movable direction and the constraint direction expressed in the reference coordinate system (or hand coordinate system) of the robot are as follows:
The calculation is performed by the operator 22a from the installation posture of the work target 26. For example, in the copying operation of FIG. 5A, when the reference coordinate system of the robot is represented by XY axes in the drawing,
The operator 22 determines that the movable direction is the X-axis direction and the constraint direction is the Y-axis direction.

[0010]

However, as in the opening / closing operation of the opening / closing portion 14a shown in FIG. 5B, the moving direction and the restraining direction are complicated, and the moving direction and the restraining direction are defined by the posture of the hand. When using a hand coordinate system, the relationship between the coordinate system and the installation orientation of the members is difficult to intuitively grasp, so that there is a problem that errors are likely to occur and it is difficult to teach the robot.

The present invention has been made in view of the above problems, and has as its object to provide a robot control device that automatically detects a moving direction and a restraining direction of a robot.

[0012]

The object of the present invention is to detect a position of a control object and a force of contact with the work object when driving the control object for operating the work object in a predetermined direction.
In the robot control device for controlling the control target, the position and the force of the control target are detected by a detection signal of the movable or movable state of the control target in a predetermined direction.
A solution is provided by including a constraint detection unit and an operation sequence generation unit that instructs the movable / constraint detection unit to start detection and that issues an operation command in the detection direction of the movable / constrained state of the controlled object. Is done.

[0013]

As described above, the position at which the controlled object is driven,
And the force of contact with the work object is detected, and the movable / constrained detection unit detects the movable direction or the constrained direction of the control object from the detected position in response to the detection start command of the operation sequence generation unit.

Then, the operation sequence generation unit operates
An operation command for driving the controlled object is issued based on the detection direction of the constraint detection unit.

This makes it possible to automatically detect the moving direction and the restraining direction of the robot.

[0016]

FIG. 1 is a block diagram showing an embodiment of the present invention.
FIG. 1 shows a robot controller 31. A control object 32 is an arm joint drive motor (arm joint part) for operating a hand attached via a force sensor to a robot tip as shown in FIG. 5, for example. ). The target that the control target 32 operates is, for example, the opening and closing of an opening and closing mechanism as shown in FIG.

In FIG. 1, an operator 33 and an operation sequence generator 34 are connected to each other via a terminal 35, and a command sequence file 36 is connected to the operation sequence generator 34. In addition, the operation sequence generation unit 34 is connected to the position control unit 37 and the force control unit 38, and is also connected to the movable / restriction detection unit 39. The movable / restriction detecting section 39 is connected to a position control section 37 and a force control section 38.

On the other hand, the position of the control target 32 is
It is connected so as to be detected as 0, and is connected to the movable / restriction detecting section 39 and the position control section 37 so as to transmit a position signal from the position detecting section 40.

The contact force of the controlled object 32 is
1 and is connected to the movable / restriction detecting section 39 and the force control section 38 so as to transmit a force signal from the force detecting section 41.

The position controller 37 and the force controller 38
Are connected to the operation unit 42 so that the combined signals are transmitted, and the operation unit 42 is connected so as to control and drive the control target 32.

Here, the operation of each unit will be described. The operation sequence generation unit 34, the movable / restriction detection unit 39, the position control unit 37, and the force control unit 38 will be described later in detail.

A position detecting unit 37 in the figure includes an encoder and a counter (not shown) for detecting the rotation angle of the motor of the joint of the robot arm which is the control target 32, and the coordinates for converting the coordinate value from the rotation angle to the reference coordinate system. It consists of a converter and detects the position of the tip of the robot in the reference coordinate system.

The force detector 38 includes a force sensor (not shown) connected to the tip of the robot arm and a force signal calculator for calculating a contact force in a reference coordinate system from an electric signal detected by the force sensor. And detects the contact force in the reference coordinate system.

The operation unit 42 drives the arm joint of the robot so as to follow the speed command in the reference coordinate system expression generated by the position control unit 37 and the force control unit 38. It is composed of an amplifier, a D / A converter, and a compensator.

The position control unit 37 calculates the tip position X of the robot (hand) detected by the position detection unit 40, the target position X ₀ given by the operation sequence generation unit 34 or the movable / restriction detection unit 39, the operation time, position control gain, the selection matrix on the basis of the position control parameters such as, emit position control direction of the velocity command signal v _p to zero a deviation between X and X _0.

Further, the force control unit 38 includes a force signal F detected by the force detection unit 41, a target force F ₀ given from the operation sequence generation unit 34 or the movable / restriction detection unit 39, a force control parameter, and a selection matrix. emits F and the speed command signal of the force control direction to zero the deviation F ₀ v _f based on.

The operation sequence generating section 34 executes a movable / restriction detection operation execution command, a target force F ₀ , a force control parameter, a transfer of a selection matrix, a target position X ₀ , a position control parameter, a transfer of a selection matrix, a position control, and a force. The control is switched, the operation command input by the operator is interpreted, and the operation result is displayed on the terminal 35 and transmitted to the operator 32. Then, an operation procedure for the work is set by the operator 33. For example, when continuously detecting the movable / restricted state in each axis direction of the reference coordinate system, the movable / restricted state is returned to the position where the operation started after the movable / restricted state detection operation is completed, and the movable / restricted state is detected in the next movable / restricted state detection direction. Generate a detection operation command.

The movable / restricted state detecting section 39 performs a movable / restricted state detecting operation and detects a movable / restricted state. At the time of executing the movement / restriction detection operation, the selection matrix of the position control unit 37 and the force control unit 38 is set to an appropriate value in order to set the control state in all directions of the robot to the force control state. Movable ・
For detection of the constraint state, an appropriate target force command is generated in the direction of detecting the movable / constrained state, and either "whether the contact force matches the target force" or "whether the contact force has moved a predetermined distance" is selected. This is performed by determining whether the condition is satisfied first.
That is, if the direction being searched is in a constrained state, a contact force occurs because a reaction force returns from the member to the robot,
In the movable state, the robot tip moves in the movable / restricted state detection direction. When the movable / restricted state is detected in the movable / restricted state detection direction, a flag corresponding to the determination result is sent to the operation sequence generator 34.

The command sequence file 36 describes an operation command sequence for executing a task.

Next, the operation of each part when detecting the movable / restricted state will be described in detail.

First, the operation sequence generator 34 will be described. When the movable / restriction detection operation command is given from the operator 33 or the command sequence file 36 in which the operation command sequence is described, the operation sequence generation unit 34 sends a movable / restriction detection operation execution command to the movable / restriction detection unit 39. At this time, the target force F ₀ , the movement detection distance L, and the movable / restricted state detection direction vector t required for the movable / restricted state detection operation are transferred together. Appropriate values of the target force F ₀ , the movement detection distance L, and the movable / restricted state detection direction vector t are set in advance by an operator in a storage unit in the operation sequence generation unit.

For example, the target force is 1 kg, the movement detection distance is 5
mm, and the movable / restricted state detection direction + X direction, the parameters of the storage unit are set as shown in Table 1. Note that the vector t is (X, Y, Z, α, β, γ), (X, Y, Z) represents a position, and (α, β, γ) represents their rotation.

[0033]

[Table 1]

Successively, + X, -X, + Y, -Y, (+
X) (+ Y), (-X) (+ Y), (+ X) (-Y),
When continuously detecting the movable / restricted state in the (−X) and (−Y) directions, the parameters of the storage unit are set as shown in Table 2.

[0035]

[Table 2]

The movable / restricted state is transmitted to the operator 33 by outputting the movable / restricted state detection direction and the operation result to the terminal 35.

Next, FIG. 2 is a diagram illustrating the movable and restraint detection of FIG.
The flowchart is shown, and the movable / restriction detection unit 39 is explained.
I will tell. In the detection operation of the movable / restriction detection unit 39,
Set an appropriate target force command in the direction to check the movable / restricted state.
Occurs, and in the other direction, a force control of zero target force is performed. here
Work coordinates to describe the control direction of the robot tip
Define the system. Each of the X, Y, and Z axes of the work coordinate system
Let the unit vector be n_w, O _w, A_wToki, the movement of the robot
The control state in each axis direction is set according to the operation state. Also,
As described above, rotation around the X, Y, and Z axes of the working coordinate system is performed.
The axis of rotation is represented by α, β, γ. Operation during movable / restriction detection
The work coordinate system is based on the grip center (C
And the detection direction of the movable / restricted state
X axis, the direction orthogonal to the X axis is defined as Y axis, Z axis
You. And n in the working coordinate system_w, O _w, A_wCalculate
(Step (ST) 1).

First, the unit vector of the movable / restricted state detection direction vector t _n given from the operation sequence generator 34 is set to the vector n _w .

[0039] _{n w = t n / | t} n | ... (1) vector o _w is perpendicular to the vector n _w, inner product n _w
· O _w is set to be _{n w · o w = 0 ...} (2). Also, the vector a _w is
n _w, is represented by the cross product of o _w, it is _{a w = n w × o w} ... (3).

[0040] Then, the conversion matrix for converting a vector that is described in the working coordinate system to the reference coordinate system representation, vector n _w, o _w, using a _w, a transformation matrix ^o R _w, the ^w R ₀ It is calculated (ST2). In this case, ^o R _w is

[0041]

(Equation 1)

Is given by Further, a conversion matrix ^w _Ro for converting the vector of the reference coordinate system expression to the working coordinate system expression is
Coincides with the transposed matrix of the transformation matrix ^o R _w, the _{^{w R o = o R w T}} ... (5). Note that o indicates a reference coordinate system, w indicates a work coordinate system, and T indicates transposition.

Subsequently, in order to set all axes in the working coordinate system in the force control state and generate a force control command of the target force F ₀ in the X-axis direction, the movable / restriction detecting unit sets the following parameters to the position. The control unit and the force control unit are set (ST3).

Target force vector ^w F = (F ₀ 0 0 0 0 0) ^T Force control selection matrix S _f = diag (1 1 1 1 1 1) Position control selection matrix S _p = diag (0 0 0 0 0 0 0) When the force command is generated, the movable / restricted state in the movable / restricted state detection direction is determined as follows (ST4, ST4).
5).

The decision of the moving state ^{_{| w P xa - w P xi}} |> L ... (6) binding decision of the state ^{_{| w F a | = F 0}} ... (7) However, ^w P _xa is, X of the work coordinate system current position in the axial direction of the robot tip, ^w P _xi, the operation start position of the X-axis direction of the robot tip of the work coordinate system, | ^w F _a | is the magnitude of the contact force vector ^w F _a. “Diag” indicates a diagonal matrix.

This state determination is performed in parallel during the movable / restriction detection operation, and when any of the above determination conditions is satisfied, the corresponding flag is transferred to the operation sequence generation unit. That is, when the condition for determining the movable state is satisfied, en
dflgP is transferred (ST6), and when the determination condition of the constraint state is satisfied, endflgF is transferred (ST7). As described above, there are two cases of the end of the movable / restriction detection operation: the end judgment based on the position and the end judgment based on the force.

The condition for detecting the movable / restricted state may have a range as shown in the following equation in order to increase the accuracy of detection.

The judgment of the movable state ^{L + a · L> | w} P xa - w P xi |> L + b · L ... (8) judgment of the restraining state _{F 0 + c · F 0>} | w F a |> F 0 + d · F ₀ (9) a, b, c, and d are positive or negative coefficients given in advance.

In the above description, the case where the movement in the movable / restricted state detection direction is performed by the force control for generating the target force is described. However, the position control and the speed control may be used. For example, when moving by the position control in the X-axis direction of the work coordinate system, assuming that the start position vector of the movable / restriction detection operation is ^o P _or , the target position vector and the selection matrix are the target force vector ^w R _o · ^o P _or + (L 0 0 0 0 0) ^T position control selection matrix S _p = diag (1 1 1 1 1 1) Force control selection matrix S _f = diag (0 0 0 0 0)

When force control is performed in a direction other than the X-axis direction of the work coordinate system, the target force vector and the selection matrix are set as follows.

Target force vector ^w F = (0 0 0 0 0 0) ^T position control selection matrix S _p = diag (1 1 1 1 1 1) Force control selection matrix S _f = diag (0 0 0 0 0 0) On the other hand, when moving by speed control (plane only) in the X-axis direction of the work coordinate system, the target speed vector and the selection matrix are expressed as follows: target force vector ^w V = (V ₀ 0 0 0 0 0) ^T speed control selection matrix S _V = set as diag (1 1 1 1 1 1 ) power control selection matrix _{S f = diag (0 0 0} 0 0 0). When force control is performed in a direction other than the X-axis direction of the work coordinate system, a target force vector and a selection matrix are set as follows.

Target force vector ^w F = (0 0 0 0 0 0) ^T speed control selection matrix S _V = diag (1 0 0 0 0 0) Force control selection matrix S _f = diag (0 1 1 1 1 1) The target speed V ₀ is given to the operation sequence generation unit 34 in advance, and is sent to the movable / constrained detection unit 39 together with a movable / constrained detection operation execution command.

When the position control and the speed control are performed,
The method of detecting the movable / restricted state is the same as the case where the movement is performed by force control.

Next, FIG. 3 is a diagram for explaining the force control of FIG. 1, and the processing of the force control unit 38 will be described.
In FIG. 3, the force control unit 38 includes a movable / restriction detection unit 3.
9. Generate a speed command to make the deviation between the target force commanded by the operation sequence generating unit 34 and the contact force between the robot and the member zero. First, the force control unit 38 calculates a difference between the target force and the detected force on the working coordinate system.

Contact force vector ^o F described in reference coordinate system
Convert _a to a working coordinate system description. With a transformation matrix ^w R ₀ from the reference coordinate system to the working coordinate system, the contact force vector ^w F _a work coordinate system description is given by _{^{w F a = o R w T}} · o F a ... (10) . Accordingly, the deviation vector ^w dF between the target force vector ^w F ₀ and the detection vector ^w F _a is as follows: ^w dF = ^w F ₀ -(- ^w F _a ) = ^w F ₀ + ^w F _a ... (11) In this case, the detection vector ^w F _a is negative, due to the contact-force vector ^o F _a is movable-constrained state detection direction vector t _n and opposite.

Further, the force control section 38 generates a speed command by a force compensator (not shown). First, the force control speed instruction vector ^w v _f0, using the gain G _f, a _{^{w v f0 = G f · w}} dF ... (12).

[0057] In the movable-lock detection operation, the power control, X-axis of the work coordinate system, Y-axis, performed in the rotational direction of the Z-axis direction and in the axial, selection matrix _{^{w S f = diag (1 1}} 1 1 1
According to 1), the speed command component in the force control direction over the entire width is extracted. Note that S represents selection by a switch.

Therefore, the final speed command vector ^w v _f
Is a _{^{w v f = w S f ·}} w v f0 ... (13).

[0059] It should be noted that, when you convert a force control speed instruction ^w v _f to the reference coordinate system description, the _{^{o v f = o R w ·}} w v f ... (14). The gain _Gf is provided from the operation sequence generator 34.

Next, FIG. 4 is a block diagram for explaining the position control of FIG. 1, and the processing of the position control unit 37 will be described. By the way, when the movable / restriction detecting operation is continuously performed in each direction, it is necessary to return the tip end position of the robot to the operation start position. Therefore, the position control unit 37 performs position control to set the target position as the operation start position P _or .

In FIG. 4, in the position control operation, the selection matrix is as follows: Force control selection matrix S _f = diag (0 0 0 0 0 0) Position control selection matrix S _p = diag (1 1 1 1 1 1) .

[0062] In this case, the position deviation vector ^o dP described in the reference coordinate system, the target position vector ^o P _or, and, from the position vector ^o P _ar robot _{^{tip, o dP = o P or -}} o P ar ... Given by (15). The speed command in the working coordinate system is the gain G _p
Using a, a _{^{w v p0 = G p · w}} R o · o dP ... (16).

[0063] In addition, the selection matrix, ^w S _p = diag (1
According to 1 1 1 1), a speed command component in the position control direction of all axes is extracted.

Therefore, the final speed command vector ^w v _p
Is a _{^{w v p = w S p ·}} w v p0 ... (17).

[0065] When converting the position control speed command ^w v _p to the reference coordinate system description, the _{^{o v p = o R w ·}} w v p ... (18). The gain G _p is provided from the operation sequence generator 34.

[0066] Therefore, in the case of performing the speed control, the speed command of the task coordinate system, given by ^{_{w v v = w S v ·}} w v ... (19). Incidentally, converting the speed control speed instruction ^w v _v to the reference coordinate system description, the ^{_{o v v = o R w ·}} w v v ... (20).

As described above, the movable direction and the restraint direction of the robot under the restraint environment can be automatically calculated.
As a result, the burden on the operator can be reduced.

[0068]

As described above, according to the present invention, the movable and restricted state directions are detected from a detection signal corresponding to the state of the controlled object, and an operation command for driving the controlled object is issued based on the detected direction. Accordingly, the moving direction and the restraining direction of the robot can be automatically detected, and the burden on the operator can be reduced.

[Brief description of the drawings]

FIG. 1 is a configuration diagram of an embodiment of the present invention.

FIG. 2 is a flowchart of detection of movable and restraint in FIG.

FIG. 3 is a block diagram for explaining the force control of FIG. 1;

FIG. 4 is a block diagram for explaining the position control of FIG. 1;

FIG. 5 is a diagram for explaining an operation target of a conventional robot control.

FIG. 6 is a configuration diagram of a conventional robot control device.

[Explanation of symbols]

 Reference Signs List 31 robot control device 32 controlled object 33 operator 34 operation sequence generation unit 35 terminal 36 command sequence file 37 position control unit 38 force control unit 39 movable / restriction detection unit 40 position detection unit 41 force detection unit 42 operation unit

Continued on the front page (58) Fields surveyed (Int. Cl. ⁷ , DB name) B25J 3/00-3/10 B25J 9/10-9/22 B25J 13/00-13/08 B25J 19/02-19 / 06 G05B 19/18-19/46

Claims (1)

(57) [Claims] 1. A control system for operating a work object in a predetermined direction.
When driving the elephant (32), the position of the controlled object (32)
And the force of contact with the work object
In the robot control device for performing the control of (32), the position and the force of the control target (32) are detected by a detection signal.
The state of movement or restriction of the control target (32) in a predetermined direction
A movable / restriction detecting section (39) for detecting the state, and instructing the movable / restrictive detecting section (39) to start detection.
The detection direction of the movable and restrained state of the control target (32)
And an operation sequence generation unit (34) for issuing an operation command.See The control target in the movable / restriction detection section (39)
(32) Detection of movable or restricted state in a predetermined direction
Indicates the detection direction as the force control using the control state of all control directions.
The target (32) is generated by generating a target force to
, The condition for judging the end of operation by position, or operation by force
Judge whether any of the termination judgment conditions is satisfied.
And a robot control device.