JPH03184786A - Track formation system for robot - Google Patents

Abstract

PURPOSE:To dispense with any instruction to a robot with the change of an objective body and the dislocation of the objective body so far performed by an operator of this robot and thereby reduce the extent of a burden on the operator by performing a contouring operation as adding a constant force to a surface of the objective body with an unknown-form curved surface on the basis of a calculated contouring coordinate system, with a force control part. CONSTITUTION:In this system, there are provided with a position control part 3, controlling a robot position on the basis of position coordinates in a position detecting element 5, and a force control part 4 controlling a constant force given to the robot on the basis of the force detected by a force detecting element 6. Likewise, there are provided with a control command generating part 7, performing the transfer of force and position commands and various parameters to the robot, a normal vector calculating part 8, calculating the normal vector of a contact point between the robot and an objective body and also calculating a contour coordinate system, and a shifting vector calculating part 9 calculating a shifting vector of the robot along the contour coordinate system, through which a contouring operation as adding a constant force to a surface of the objective body with an unknown-form curved surface by the force control part, on the basis of the calculated contour coordinates system.

Description

[Detailed Description of the Invention] [Summary] Regarding the trajectory generation method of a copying control device for copying work that performs machining etc. along the surface of an object in force control of a robot, The aim is to easily generate a trajectory for a force-controlled robot that performs a tracing operation while applying a certain force to a surface. a force detection unit that detects the force acting on the robot; a position detection unit that detects the current position of the robot; a position control unit that controls the position of the robot based on the position coordinates of the position detection unit; A force control unit that controls the force applied to the robot based on the detected force, a control command generation unit that transfers force/position commands and various parameters to the robot, and a control command generation unit that calculates the normal vector of the contact point between the robot and the object. ,
It also includes a normal vector calculation unit that calculates a scanning coordinate system, and a movement direction hector calculation unit that calculates a moving direction vector of the robot along the scanning coordinate system, and calculates a curved surface of an unknown shape based on the calculated scanning coordinate system. The device is configured to perform a tracing operation while applying a constant force to the surface of the object held by the force control section.

[Industrial Application Field] The present invention relates to a trajectory generation method for a tracing control device for a tracing operation in which machining or the like is performed along the surface of a target object in a force-controlled robot.

The scanning control device basically consists of an operation section that operates the controlled object (robot), a position detection section that detects the position, orientation, etc. of the controlled object, and a force detection section that detects the force applied to the controlled object. This is a device that moves a controlled object by tracing along the surface of a target object having a curved surface. Therefore, it is necessary to quickly and accurately generate the trajectory of the controlled object using data from each of the above devices.

[Prior art and problems to be solved by the invention] When a force-controlled robot performs tracing work, a pressing direction n that coincides with the normal direction of the object surface at a contact point where the tip of the robot and the object come into contact; The scanning coordinate system (Ow
Y h Z w ) must be set in the robot control device.

Conventionally, as a method for setting this tracing coordinate system, a method using instruction by an operator has been proposed.

However, while the operator's teaching to the robot does not require very complicated operations if the object surface is relatively flat, it becomes extremely troublesome if the surface is composed of complex curved surfaces. Therefore, it takes time. Therefore, there has been a need for an efficient method for setting this tracing coordinate system.

An object of the present invention is to provide a trajectory generation method for force control of a robot, which makes it possible to easily set this tracing coordinate system and easily generate a trajectory of a controlled object.

[Means to solve the problem]

FIG. 1 is an fJIff diagram of the principle of the present invention. The present invention
A trajectory generation method during force control of a robot (1) to be controlled, which includes a force detection section (6) that detects the force acting on the robot and the object (11), and a position that detects the current position of the robot. a detection unit (5); a position control unit (5) that controls the position of the robot based on the position coordinates of the position detection unit;
3), a force control unit (4) that controls the force applied to the robot based on the force detected by the force detection unit, and a control command generation unit (7) that transfers force/position commands and various parameters to the robot. , a normal vector calculation unit (8) that calculates the normal hector of the point of contact between the robot and the object and also calculates the scanning coordinate system, and a movement direction vector that calculates the movement direction vector of the robot along the scanning coordinate system. A calculation unit (9) is provided, and the scanning operation is performed while applying a constant force by the force control unit to the surface of the object having a curved surface of unknown shape based on the calculated scanning coordinate system. shall be.

[For production]

Figure 2 shows the tracing coordinate system (0w X w Y w Z h
) is an explanatory diagram. This coordinate system is determined by the position and orientation of the robot tip 12 (hand) with respect to the object 11. Note that 13 is a force sensor, and 14 is a manipulator. Vectors n, o, and a are unit vectors for the coordinate axes Xw, Ytn, and Zu of the scanning coordinate system, respectively.

Hector n indicates the pressing direction when applying force to the object, and is the same direction as the normal vector of the surface of the object.

0 is orthogonal to n and indicates the moving direction of the robot tip during the copying operation. Further, a is determined to be orthogonal to n and o, and is given by a=nXo.

On the other hand, the coordinates (OXo, Yo, Zo) are the reference coordinate system. Therefore, the vectors n, a.

Ingredients are displayed for the seed yarn. That is, n = (nxnyng)' o = (o, oyo,)' 0 is the reference point. Therefore, from the copying coordinate system (0, -X, 4Y, Zl, l) to the reference coordinate system (0-X,, yo, When converted to ZO), n
a and o can be expressed as in the above formula.

FIG. 3 is an explanatory diagram of the positional relationship of the robot in the reference coordinate system and the tracing coordinate system. The robot (hand 12) is performing a tracing operation on the surface of the object 11, and P, , P are the positions of the robot. In Pl, vector n is the pressing direction perpendicular to the surface, and o and a are on the tangential plane. When the robot performs a tracing operation and moves in the direction of vector 0, the normal direction of the surface at the point of contact between the robot and the object at P2 no longer matches the pressing direction n at P. If the normal direction and the pressing direction do not match, the pressing force generated against the object will deviate from the set value. Therefore, in order to generate accurate force, it is necessary to switch the tracing coordinate system of Pl to the tracing coordinate system of P2. The conditions for determining whether or not to change the tracing coordinate system are: (1) when a certain period of time has elapsed, (2) when the reaction force received from the object deviates from the set force by more than a certain amount, (3) when the robot When moves a certain distance, etc.

In the following description, it is assumed that the scanning coordinates are changed according to the condition (1). Furthermore, in cases (2) and (3), the basic principle is the same as in (1).

In the present invention, the moving direction vector at each point during the copying operation is determined by two vectors: the normal vector of the starting point of the copying operation (hereinafter referred to as the starting point) and the moving direction vector given by the operator at the start of the operation. Control so that it always exists on a plane.

FIG. 4 is an explanatory diagram of the scanning coordinate system and changes in the trajectory of the robot. Moreover, it is a principle block diagram of the control system for performing the copying motion shown in FIGS. 1.4 and 5 by a force-controlled robot. A control method and trajectory generation method for performing a tracing operation on an object having a curved surface will be explained with reference to FIG. 3.4.

First, calculation of the copying coordinate system at the starting point pH shown in FIG. 4 will be explained.

(1) Calculation starting point Pl of normal vector n at the starting point,
The normal vectors n and l at are determined as follows. Fifth
Figure (a) is a diagram showing the relationship between the reaction force F that the hand receives from the object and the force sensor coordinate system (0, -XSYsZS). The force sensor 13 detects the reaction force F x, y, z,
Component forces fX+rv, rz in each direction are detected.

(Hereinafter, a description of the torque bone detection detected by the force sensor will be omitted.) When the reaction force SF is expressed in hectors, (here, S is the force sensor coordinate system (Os X s Y s
Z s ). ) SF= (fKfYfz)” −(
1). Normal vector S expressed in force sensor coordinate system
nB is a vector in the opposite direction to SF, and when expressed as components: (2) Here, 13F1 is the magnitude of the vector.

Let us express the normal vector Sn in the reference coordinate system.

Let the unit vector of each coordinate axis X, , Y, , Z of the force sensor coordinate system with respect to the reference coordinate system be 0n, . -;30a
, (O indicates that it is written in the reference coordinate system). When the ingredients are displayed, it is as follows. At this time, the coordinate transformation matrix 0A from the force sensor coordinate system to the reference coordinate system is OAS = ('n, s'os'as) −
It is given by (4). Normal vector expressed in reference coordinate system On! l is 'n,B using the coordinate transformation matrix 0A,
= oAssns”・(5) The normal vector is calculated by the normal vector calculation unit 8 in FIG.
It is calculated by Further, calculation of normal vectors at contact points other than the starting point is also performed in a similar manner.

(2) The moving direction vector O6 at the starting point Pl+ is calculated by using the moving direction vector 0°P given by the operator and the obtained normal vector On8 in the moving direction vector calculation section at the starting point. It shall not satisfy the following. The moving direction vectors O and I are vectors that are perpendicular to the normal vector On and are on a certain plane between the moving direction vector 0° and the normal vector On. At this time, all, which is one of the unit vectors representing the coordinate axes of the copying coordinate system, is On
Using B and OOP, it is expressed as (8). The moving direction vector On is the vector 'n
Due to the orthogonal relationship with 1'am, Oo, = 'a, X'ns ・= (
9). The moving direction vector is calculated by the moving direction vector calculating section 9 shown in FIG.

Next, a method of calculating the tracing coordinate system at point Pi in FIG. 4 will be explained.

(3) Calculation of normal vector at point P The method for calculating the normal vector n at point P is the starting point PIl
This is the same as the method of calculating the normal vector in , and is expressed by the following formula. However, the reaction force SF at point P8 is set as 'Ft=(fxtfvtL+)''.

... (]0) On, = 'Ast'ni
... (11)0A5. is a coordinate transformation matrix from the force sensor coordinate system of point P to the reference coordinate system.

(4) Calculation of the movement direction vector at point P. Since the tip position of the robot is also controlled in the hectorum direction, the tip position is controlled so that it is always on the plane affected by vectors n and o. . Therefore, the locus drawn on the surface of the object becomes a curve on the plane formed by the vector n108 given at the starting point. From this, it can be seen that it is sufficient to apply the hexagonal force directly in the moving direction at point P8. Also, since it moves, the vector 00. is expressed as (12). ai, which is one of the unit vectors representing the coordinate axes of the tracing coordinate system, is the vector On.

Due to the orthogonal relationship with Oo, Oa, = On, ×Oo, ... (
13).

A method of controlling the copying operation at the starting point P shown in FIG. 4 will be explained.

(1) When generating the set force F1 to the object at the target force Fr generation starting point P8, it is sufficient to generate a force with a force magnitude of F and a direction of n. Therefore, the total configuration is given by .

(2) Setting the target position The direction of movement of the robot beyond the starting point P is the starting point P.

Give it as a relative position command from . The movement direction from the starting point P is given by the movement direction vector Go8 in equation (9), and when the relative position is expressed using 0oll, OXI = α・008 ... (1
5). α is an appropriate constant determined from conditions (1) to (3) for modifying the scanning coordinate system on page 2.

The target force and target position are set by the control command output section 7 shown in FIG.

(3) End of copying operation The copying operation ends when the following conditions are met.

(1) When the operator gives a command to finish (2) When a certain period of time has passed (3) When there is a collision or when the robot leaves the object (4) When the robot exceeds its movable range (12) is 0a, l-00, or 0all--On
.

It does not hold when . At this time, the moving direction can be maintained by using the moving direction vector of 0° given by the operator as the moving direction vector of point P1.

[Embodiment] FIG. 6 is a block diagram of a copying control device according to an embodiment of the present invention. The control device according to the embodiment, as shown in the figure,
It includes an operation section 22 that controls the manipulator 21. This operation section 22 includes a servo motor 22a and a power
Amplifier 22b, D/A converter 22c, and compensator 2
2d.

The control device also includes a position detecting section 26 that detects the tip position of a hand section (not shown) of the manipulator 21, and this position detecting section 26 has a counter and an encoder 26.
a and a tachometer 26b.

Furthermore, the control device includes a force detection section 23 that detects the force applied to the hand section of the manipulator 21.

This force detection section 23 includes a force sensor 23a similar to the above and a coordinate conversion section 2 from the hand coordinate system to the robot reference coordinate system.
3b.

Furthermore, the control device also detects the force F detected by the force detection section 23 during the copying operation. , Speed command signal in the force control direction based on the set force (force command F,) and the force control parameter ■,
The force control unit 24 that emits the force and the position X detected by the position detection unit 26. , target position

FIG. 7 is a specific configuration diagram of the position control section 27 shown in FIG. 6. As shown in the figure, a transposed orthogonal transformation matrix (RT) calculation unit 31
, the selection matrix (r-sr) calculation unit 32, and the orthogonal matrix (R
) calculation unit 33 and position feedback gain (c= )
It has an arithmetic unit 34. On the other hand, the force control section 24 includes a transposed orthogonal transformation matrix (R'') calculation section 38 and a selection matrix (S,).
It includes a calculation section 37, an orthogonal matrix (R) i calculation section 36, and a CF calculation IE section 35. still,
Symbols 24a and 27a are deviation parts.

The robot reference coordinate system (Xo
, yo, zo) to the tracing coordinate system (X w Y , IZ
An orthogonal coordinate transformation matrix R for coordinate transformation to w ) is expressed as follows.

As shown in FIG. 2, the Xo direction of the scanning coordinate system (Xl, IY, IZw) is the force control direction, yl, 1. When the direction zt, , is the position control direction, the selection matrix Sf of the selection matrix clearing units 32 and 37 is given by (16).

feedback gain C2 teeth, to the reference coordinate system regarding, (17) is given by

Also, Position feedback gain C1 is similarly do, (18) is given by

Furthermore, the control device includes a force control section 24 and the position control section 2.
Adding unit 30 that adds the speeds output from 7.
b, and an inverse Jacobi transform unit 30a that converts the added velocity into an angular velocity θ of each joint of the manipulator 2I.

The coordinate conversion unit 20 converts the joint angle θ of the manipulator detected by the position detection unit 26 into a position Xo in the reference coordinate system.

The host computer 40 provides (1) control commands for the target value WXr and force command F1, [2] position control associated with switching of the scanning coordinate system, transmission of force control parameters, (3) normal vector calculation unit, and movement direction vector. A control command generation section 40a that generates a signal for controlling the state of the calculation section, a normal hector calculation section 40b that calculates the normal vector of the contact point between the manipulator and the object, and a control command generation section 40b that generates a signal for controlling the state of the calculation section, and a normal hector calculation section 40b that calculates the normal vector at the contact point between the manipulator and the object, and the movement at the contact point between the manipulator and the object. Movement direction vector calculation unit 40c that calculates a direction vector
Equipped with Parameters for the copying operation are set in the control command generating section 40a by the operator.

FIG. 8 shows a hardware system configuration according to the embodiment. The host computer 40 includes a 9F11 J8 command generation section 40a, a normal vector calculation section 40b, a movement direction vector calculation section 40c, and a communication control section 40d. The robot controller 10 includes a memory 10a, a communication control section 10b, a control section, a coordinate conversion section, a deviation section, etc., and controls the manipulator 21 via an operation section 22 and a position detection section 26. The host computer 40 and the robot controller 10 are connected by a communication interface IF such as a bus, and are controlled by communication control units 10b and 40d that manage the timing of sending and receiving respective signals.
Data is transferred between the memory 10a, the normal vector calculation unit 40b, and the movement direction vector calculation unit 40c.

FIG. 9 shows the flow of processing when performing a tracing operation on an object having a curved surface when the system configuration is as shown in FIG. 8.

FIG. 9 is a processing flowchart.
0a, a normal vector calculation section 40b, a moving direction hector calculation section 40c, and a flow of processing performed by the robot controller IO.

It is assumed that the parameters for the copying operation are set by the operator. In the figure, the subscript B indicates the starting point, and the subscript i
represents the i-th contact point between the robot and the object.

First, the operator calculates the moving direction vector OOP, the setting force Fr, the switching time T of the scanning coordinate system with an appropriate value, the coefficient α specifying the relative position, and the force vector FB generated to calculate the normal vector at the starting point. Set. The control command generation unit 40a generates the force command Fl+ and the force control parameters in order to generate an appropriate force F at the starting point, and transfers them to the robot controller 10. The robot controller 10 performs a pressing operation against the object 11 based on the command. At this time, when the detected reaction force is 0 (when the robot and the object are separated), the force hector FII is set again.

Next, the normal hector calculation unit 40b calculates 5nlI from the detected reaction force F using equation (2).

Also, calculate the force sensor coordinate system using the Jacobian matrix, and (4)
The coordinate transformation matrix ' A g is calculated from 2. The calculated O
A, the normal vector On8 is obtained, and On6 is transferred to the control command generation section 40a and the movement direction vector calculation section.

Here, the normal vector On, and the movement direction vector 'OO
If the directions of F match, set the parameters again.

The moving direction hector calculation unit 40c calculates the vector Oal+ using equation (8), and obtains the moving direction vector Go8 from OnB and Oa6 (formula (9)).

The control command generating unit 40a generates a target force vector 0Fr, a target relative position hector 0X, from equation (14L (15), a normal vector 0n8, a moving direction hector Go), and transfers it to the robot controller. The controller 10 receives a speed command v0 from the control command generation unit 40a.
When received, the copying motion starts.

After a certain period of time τ has elapsed, it is checked whether the robot is within the movable range, and if it is within the movable range, the reaction force F at the first contact point is detected.

At the first contact point, use the same operation as the starting point to make the normal -
A normal vector is calculated by the vector calculation unit 40b. Also,
The normal vector calculation unit 40b calculates the moving direction hector using (12) 2.

The control command generation unit 40a generates a target force hector 0Fr and a target relative position hector 0X from the normal hector On, the movement direction hector Oo, and transfers them to the robot controller. The robot controller receives the speed command ■ from the control command generation unit 40a.

When received, the copying motion starts.

After a certain period of time τ has elapsed, it is checked whether there is a termination command from the operator, and if there is a command, the copying operation is stopped and the next operation is performed. If there is no command, calculate the opposite. The above operations are repeated until the operator issues a termination command.

〔Effect of the invention〕

As explained above, according to the present invention, it is possible to automatically perform a copying operation on a target object having a curved surface, and it is possible to automatically perform a copying operation on a target object having a curved surface. There is no need to teach the accompanying robot, and the burden on the operator is reduced. Furthermore, since the moving direction vectors are always on the same plane, the deviation between the target force applied to the object and the force generated on the object can be made minute.

[Brief explanation of drawings]

Fig. 1 is a diagram showing the principle configuration of the control system of the present invention, Fig. 2 is an explanatory diagram of the scanning coordinate system, Fig. 3 is an explanatory diagram of the positional relationship between the object and the manipulator, and Fig. 4 is an illustration of the scanning coordinate system at each point. Explanatory diagram, FIG. 5 is an explanatory diagram of the calculation direction of the setting force, FIG. 6 is a configuration diagram of an embodiment of the present invention, FIG. 7 is a configuration diagram of a position control section and a force control section, and FIG. 8 is a diagram of the present invention The system configuration diagram and FIGS. 9(a) to 9(C) are processing procedure diagrams of the present invention. (Explanation of symbols) l...Controlled object, 2...Operation unit, 3...
Position control unit, 4... Force control unit, 5... Position detection unit, 6... Force detection unit, 7... Control command generation unit, 8... Normal hector calculation unit, 9...・Movement direction hector calculation unit, 11...Target, 12...Hunt, 13.
... Force sensor, 14... Manipulator. Patent application agent Fujitsu Co., Ltd.

Claims (1)

[Claims] 1. In a trajectory generation method during force control of a robot (1) to be controlled, a force detection section (6) that detects a force acting on the robot and the object (11); a position detection unit (5) that detects the current position; a position control unit (3) that controls the position of the robot based on the position coordinates of the position detection unit; and a position control unit (3) that controls the robot position based on the force detected by the force detection unit. A force control unit (4) that controls the applied force, a control command generation unit (7) that transfers force/position commands and various parameters to the robot, and a control command generation unit (7) that calculates the normal vector of the contact point between the robot and the object A normal vector calculation unit (8) that calculates a scanning coordinate system; and a movement direction vector calculation unit (9) that calculates a movement direction vector of the robot along the scanning coordinate system, and based on the calculated scanning coordinate system. A trajectory generation method for force control of a robot, characterized in that the force control section performs a tracing operation while applying a constant force to the surface of an object having a curved surface of unknown shape.