JPH0543122B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Application of the Invention] The present invention relates to a feedback control method for a deburring robot that requires force feedback and position feedback.

[Background of the invention]

Until now, the force feedback control method for robots was developed by Hans Colleen.
Giving Robots The Power Two
Giving Robots the Power to Cope
(spring1980/ROBOTICS TODAY (published by IFS)) is known. According to this, a simple on/off sensor, a three to four level level detection sensor, or a combination of these sensors is used as a force sensor. At best, the force feedback signal obtained from this force sensor has been applied to control systems for escape motions and push motions in one axis direction, or at most three orthogonal axes directions.

However, in actual deburring, it is necessary to control the reaction force in one direction while keeping it constant while moving it on a trajectory in the other direction, or in some cases following the same reaction force. In some cases, it is necessary to consider not only translational motion but also rotational motion.

[Purpose of the invention]

An object of the present invention is to provide a feedback control method for a deburring robot that can perform deburring in good condition by feedback of force and position.

[Summary of the invention]

For this purpose, the present invention provides a method in which, when a deburring tool is attached to the tip of an arm via a force/torque sensor and a hand, the deburring tool is gripped to the tip of the arm via a force/torque sensor, and the tool is This is a feedback control method for a deburring robot that scrapes off burrs on an object by contact tracing and moving over the object, and the speed of the deburring tool is later converted into the joint angle between each arm. The command is set to an initial value of zero, and from the equation expressing the movement of the copying spring mechanism required for deburring,
This is calculated in a predetermined manner.

[Embodiments of the invention]

The present invention will be explained below with reference to FIGS. 1 to 8.

First, an overview of the mechanism of the deburring robot according to the present invention will be explained. FIG. 2 shows an outline of the mechanism together with the object to be deburred. As shown in the figure, the tip of arm 1 of the robot has a force
A hand 2 is attached via a torque sensor 4, and a deburring tool 3 (generally using a rotating grindstone or the like) 3 is also attached to the hand 2. Deburring is performed with the tool 3 in contact with the object 5 while detecting the reaction force from the object 5 via the tool 3.

FIG. 3 shows the basic concept of the force/position feedback control method according to the present invention. The hand 2 is actually attached to the arm via the force/torque sensor as described above, but since both the force/torque sensor and the arm have high rigidity, it is difficult to use the hand 2 or the deburring device attached to it. Even if force or torque is applied to the tool from the outside,
Hand 2 remains stationary. This is the same as in the case of position control so far. In other words, the robot's hand moves on a specified trajectory, but
At that time, even if an external force acts on the hand, the hand will not move due to this action itself, but
It simply moves on a designated orbit.

However, position control alone is not sufficient to perform deburring. This is because simply moving along a designated trajectory cannot deal with irregularly protruding burrs. Therefore, while moving on a trajectory specified in advance by position control (this trajectory is determined from the predicted location of burrs in advance), the deburring tool simultaneously removes unexpected protruding burrs. Attempt to remove burrs while applying a moderate pressing force to the object; if necessary, move the deburring tool away while following the protruding burrs, or reduce the feed speed to scrape off large protrusive burrs. , etc., a high degree of control is required. That is, force feedback control and position feedback control must be performed in parallel. Of these, position control has already been performed and is sufficient, so no further explanation is required.
However, some kind of countermeasure is required for force feedback control. This is because, as described above, in deburring, when an unexpected burr shape such as a protruding burr appears, the deburring tool needs to follow the shape. The present invention attempts to realize this copying operation using software.

As described in FIG. 2, because of its rigidity, even if force or moment is applied to the hand or deburring tool, it will not move, and from this point of view, rigidity will work advantageously in improving the accuracy of position control. However, this works against the copying motion. Copying requires a spring mechanism as shown in Figure 3, and such a mechanism makes it possible to perform copying operations by applying force or moment to the hand or deburring tool. This is the result. However, since such a spring mechanism cannot be realized only by the mechanism shown in FIG. 2, the present invention attempts to realize the function of the spring mechanism using software. To briefly explain the mechanism shown in FIG. 3, the hand 2 is suspended from a frame 10 via a translation spring 6 and a rotation spring 8. Since the object is the position and orientation in a three-dimensional space, three independent translational springs and three rotational springs are required, and in order to stabilize these springs, a pair of translational dampers 7, A rotary damper 9 is incorporated.

FIG. 4 shows an outline of a control system for performing position feedback control and force feedback control. As shown in the figure, a reference value (described later) and a feedback signal regarding force and position from the robot are input to the control section 11. The force feedback signal is provided by a force/torque sensor 4 installed between the arm 1 and hand 2 of the robot.
Furthermore, position feedback signals are obtained from position sensors (for example, rotary encoders) attached to each joint of the robot. The control section 11 calculates a speed command value based on these feedback signals and reference values, and each servo motor in the robot is driven and controlled by this command value.

The details of this control section 11 will be described later, but first, a coordinate system required for the explanation will be explained.

FIG. 5 shows a coordinate system in the case of an articulated robot having six degrees of freedom. According to this, the coordinate system fixed to the base on which the robot is installed is the base B coordinate system, and the coordinates are expressed by three orthogonal axes x _B , Y _B , and Z _B. Also, the coordinate system fixed to the hand is hand H
It is called a coordinate system, and the coordinates are three orthogonal axes x _H , Y _H ,
It has come to be expressed as _ZH . The origin of this H coordinate system is mainly determined at a certain point on the hand, so that the position and posture of the hand can be known intuitively. However, since the hand often grasps tools and performs important work as a robot near a certain point on the tool,
In consideration of ease of control when the robot actually performs an action, the auxiliary A coordinate system is determined to have a fixed relative positional relationship with respect to the H coordinate system. A
The coordinate system may be the same as the H coordinate system depending on the case, but x _A , Y _A , and Z _A are three orthogonal axes. Furthermore, in the spring mechanism shown in Fig. 3, a reference R coordinate system is determined so that the position and posture in a state of equilibrium can be intuitively known, and the coordinates are three orthogonal axes x _r ,
It is expressed by Y _r and Z _r . Third
In the spring mechanism shown in the figure, if the hand does not exist at the equilibrium point, the A coordinate system is different from the R coordinate system, and if the hand exists at the equilibrium point, the A coordinate system matches the R coordinate system. ing. In addition,
_r is the position vector of the origin of the R coordinate system viewed from the B coordinate system, and _r , _r , _{and r} are unit vectors representing the axial directions of x _r , Y _r , and Z _r .

Next, to explain how to display the above coordinate system, we can express the position vector of the origin of the H coordinate system as seen from the B coordinate system, and the unit vectors representing the axial directions of each of its three orthogonal axes x _H , Y _H , and Z _H. ,,given that,
p, , are both three-dimensional vectors, and these represent the angles θ ₁ , θ ₂ ,
..., is determined by θ ₆ and the inter-articular length.

Also, if the position vector of the origin of the A coordinate system viewed from the B coordinate system _{is a,} and the unit vectors representing the axial directions of x _A , Y _A , and Z _A are _a , _a , and _a , these vectors are three-dimensional vectors. , which can be obtained from equation (1).

However, ^h _a ; position vector of the origin of the A coordinate system as seen from the H coordinate system ^ha _a , ^ha _, ^ha ; x _{a ,} Y _a , as seen from the H coordinate system
Z is a unit vector representing each axis direction of _a .

Next, in order to describe the work performed by the robot, the A coordinate system is determined at an appropriate position and posture, but in this example, the 6th
As shown in Fig. 7, in order to perform deburring work, the origin of the A coordinate system is set at a point on the central axis of the deburring tool 3 and close to the cutting point, and the axis x _A , Y _A and Z _A can now be determined appropriately.

Now, if the motion of the spring mechanism shown in FIG. 3 is described using this A coordinate system, it will be as follows.

[M _a ] (dv _a /dt) = _a − [K _a ]Δ _a − [Ca] _a …(2) However, _a ; External force (translational force and rotational force) applied to the hand (measured by the 6-axis sensor) Detection)...6-dimensional vector Δ _a ; Deviation of A coordinate system from R coordinate system (deviation in position and orientation)...6-dimensional vector _a ; Speed of tool (translational and rotational speed)...6-dimensional vector [M _a ] ;Virtual mass...6 rows and 6 columns matrix [K _a ];Virtual spring constant...6 rows and 6 columns matrix [C _a ];Virtual viscosity constant...6 rows and 6 columns matrix. The present invention is intended to realize the functions of the mechanism shown in FIG. 3 through control based on force and position/orientation feedback signals. here,
[M _a ], [K _a ], and [C _a ] are not characteristic values of the robot, but are control parameters with values set appropriately for the work to be performed.

By the way, equation (2) can be transformed into equation (3), and equation (3) can be further transformed into equation (4) using discrete value system representation. _a = [M _a ] ^-1 ∫ ( _a − [K _a ] Δ _a − [C _a ] _a ) dt …(3) _a , _o = Δt [M _a ] ^-1 ( _a,o-1 − [K _a ] Δ _a , _o-1 ) + ([I] − Δt [M _a ] ^-1 [C _a ]) _a,o-1 …(4) However, Δt; sampling period (sampling control period) [I] is a unit matrix, and the suffix n indicates a value one sampling period Δt after the suffix n-1. In addition,
Generally, the initial value _a of velocity, ₀ , is considered to be a zero vector.

In this equation (4), the value of each term on the right side is determined as follows.

That is, Δt is determined appropriately based on the calculation time required, and [M _a ], [K _a ], and [C _a ] are determined based on the conditions of the work to be performed. Furthermore, for f _a,o-1, the signal obtained from the force/torque sensor one sampling ago is used, and for _a , _o-1, the speed command value one sampling before is used as is.
Similarly, for _a and _o-1, Δ before one sampling
_a is now used as is.

If we explain Δ _a here, it is expressed as Equation (5)
It is becoming more and more sought after.

However, ,Θ is as follows.

= (f _a − f _r )×(g _a − g _r )/｜(f _a − f _r )×(
g _a −g _r )｜…(6) Θ＝2tan ^-1 E・(f _a ×f _r )/f _a・f _r −2(E・f _a
) ² + 1...(7) From equations (5) to (7), Δ _a and _o-1 can be found, but in the end, the robot hand (more precisely, the robot hand on the left side of equation (4) The speed command values _a and _o of the tool) are determined by predetermined constants and force and position feedback signals. The speed command values _{a and o} obtained in this way are converted into speed command values _b in the B coordinate system by equation (8), and then further calculated by equation (9) for each joint θ and (=[θ· ₁ , θ· ₂ , ..., θ· ₆ ] ^T ), and each motor is driven and controlled by this.

θ・=[J] ^-1 _b …(9) However, [J] is a Jacobian matrix.

FIG. 1 shows the block configuration of arithmetic control in a control section 11 according to the present invention together with a robot. The figure shows the strain gauge voltages from the force/torque sensors 20 and 4, and as can be seen, [M _a ], [K _a ], and [C _a ], ^h _a ,
In principle, f ^h _a ^h _a and ^h _a are given as constants, and _r , _r , _r and _r are given from the outside as reference values, which are target positions and orientations at each sampling time. This reference value can be obtained in advance if the design data of the object and the relative positional relationship with the robot are known, and is discrete and updatable and can be obtained externally at every sampling period for calculation control. It has become more of a gift.

Now, to explain the case in which the feedback control method described above is actually applied to deburring work, for convenience, it is important to note that the H coordinate system changes even if the tool is replaced or even if the tool is not installed. The point where there is no point can be defined as the vertex. Regarding the A coordinate system, the origin and axes x _A , Y _{AY , and Z} directions are determined as shown in Figures 6 and 7, taking into account that the tool cuts the object in contact with it. ing. Regarding the R coordinate system, the sixth
As shown in Figure 7, the origin and axes x _r , Y _r ,
The Z _r direction is determined, so that the axes x _A and x _r are located on the same straight line, and the straight line is P ₀ → P ₁ → P ₂ → → P ₇ → as shown in Figure 6. It intersects with the deburring trajectory indicated by P ₀ .

After this, the software control parameters [M _a ], [K _a ], and [C _a ] are determined, among which [K _a ] is the distance between points P ₁ and P _r as well as the tool This is an important factor regarding the pressing force against the object. The larger [K _a ] is, the more
Contact point _P1 of the tool to the target object and the origin of the R coordinate system
This is because the greater the distance between P _r , the greater the pressing force. Therefore, [K _a ] is set to be large when deburring with a large force, and small otherwise. In this case, by setting a large value in the advancing direction and a small value in the pressing direction, it becomes possible to have rigidity in the advancing direction and to imitate the shape of the opponent in the pressing direction. In addition, [C _a ] has the effect of stabilizing the system, and furthermore, [M _a ] can adjust the responsiveness of the system. If the tool is rotated in this state, the A coordinate system will approach the R coordinate system, so the tool will perform deburring while in contact with the object with a certain pressing force. In this state, the tool can be moved in the P2 direction by moving the origin of the R coordinate system parallel to _P1 and _P2 at a certain speed in _{the P2} direction. If the R coordinate system is appropriately set and changed appropriately, the tool can move on the object with a certain pressing force while following the shape of the object. For example, the 8th object
Assuming what is shown in the figure, the burr is P ₁₁ →P ₁₂ →
It exists on a curved trajectory of P ₁₃ →P ₁₄ →P ₁₁ , but on the other hand, if the R coordinate system is changed to a curved trajectory of Q ₁₁ →Q ₁₂ →Q ₁₃ →Q ₁₄ →Q ₁₁ , its origin and attitude change. If you move the tool, the tool will remain in contact with the object at almost all times in the posture shown in Figure 7.P ₁₁ →P ₁₂ →P ₁₃ →P ₁₄ →
By moving in the same way as P ₁₁ ,
It is possible to remove burrs. In this case, even if a protrusion exists on the tracing trajectory, the tool can copy the shape of the protrusion by increasing the pressing force while avoiding the protrusion. It should be noted that while the tool is tracing, the hand is not rotated around the center axis of the tool, and from the hand's point of view, the tool is simply swung back and forth, left and right. Furthermore, movement of the R coordinate system is basically performed as described above, but depending on the case, the movement speed may need to be adjusted appropriately.
That is, when a large force is applied in the direction of movement, the speed is reduced and large burrs can be scraped off. It is also effective to increase the speed while tracing a straight section and reduce the speed at a corner. Furthermore, if necessary, R
It is also possible to give a wavy shape such as a sine wave to the movement locus of the origin of the coordinate system, and to change the value of [K _a ] while the tool is moving. Furthermore, the values of [M _a ] and [C _a ] are determined according to the information from the force sensor during deburring, for example, if the signal is vibrational, [C _a ] is increased. By making changes during robot operation, it is also possible to change the responsiveness and gain within a certain range to perform optimal work.

Although the present invention has been described above, the present invention is not limited to deburring, but may also include polishing, copying,
It can be widely applied to work that receives force from the other party, such as fitting. Moreover, the present invention can handle each task simply by changing the control parameters on the software.

〔Effect of the invention〕

As explained above, according to the present invention, the tool can be moved to follow the object in good condition, and even if a protrusion exists, it can be deburred in good condition.

[Brief explanation of the drawing]

FIG. 1 is a diagram showing a block configuration of arithmetic control according to the present invention, FIG. 2 is a diagram showing an overview of the mechanism of a deburring robot according to the present invention, and FIG. 3 is a diagram showing the basics of the control method according to the present invention. 4 is a diagram showing the outline of the control system according to the present invention; FIG. 5 is a diagram showing each coordinate system necessary for explaining the present invention; FIG. 6 is a diagram showing the concept mechanically; Figure 7 is a diagram showing the relationship between the deburring tool and the object to explain how deburring is performed, and Figure 8 is a diagram showing the specific coordinates for tracing and moving the tool. FIG. 3 is a diagram for explaining a system movement setting method. 1... Arm, 2... Hand, 3... Deburring tool, 4, 20... Force/torque sensor, 5... Target object,
11...Control unit, 12-14...Calculation unit, 15, 16
...Feedback signal converter, 17...Motor, 1
8...Counter, 19...Encoder, 21...Servo amplifier.

Claims (1)

[Claims] 1. A deburring tool is held at the tip of the arm of the robot via a force/torque sensor, and the tool moves along the object to remove burrs on the object. This is a feedback control method for a deburring robot that performs deburring.The speed command value _a of the deburring tool, which is later converted to the joint angle between each arm, is set to an initial value of zero, and the movement of the copying spring mechanism required for deburring is controlled. _{Feedback control method for deburring robot (} However, in the above equation, _a represents the force/torque sensor signal, and Δ
x _a is the deviation between the reference position and orientation command from the outside and the position and orientation of the actual hand or deburring tool, [M _a ] is the virtual mass, [K _a ] is the virtual spring constant, [C _a ] indicate virtual viscosity constants). 2. The deburring robot according to claim 1, wherein during deburring, the virtual mass, spring constant, and viscosity constant as control parameters are variable according to the magnitude of the force/torque feedback signal. Feedback control method.