JP4598720B2 - Control device for numerical control machine tool - Google Patents

Description

  The present invention relates to a position and force control device applied to a numerically controlled machine tool.

  FIG. 5 is a block diagram showing a configuration example of a conventional position control device in a machine tool which is one of numerical control machines. The position control device 100 is included in the NC device, and the operation trajectory of the cutting tool expressed in the reference coordinate system (x, y, z, φ, θ, ψ) is a position command value PD for every certain period. Are input from a host device (not shown). The coordinate value (x, y, z) indicates the position of the tool in the space in the three-dimensional orthogonal coordinate system, and the coordinate value (φ, θ, ψ) indicates the coordinate value indicating the posture of the tool in space. For convenience, it is assumed that the well-known Z → Y ′ → Z ″ Euler angle is used.

In the acceleration / deceleration calculation unit 104 of the position command calculation unit 102, the position command value PD is subjected to interpolation acceleration / deceleration processing, and the position command vector r = [x, y, z, φ, θ, ψ] ^T (hereinafter, symbols indicating vectors and matrices are represented by bold characters, and [] ^T indicates transposition). Next, the vector r is converted into a position command vector q = [q ₁ , q ₂ ,..., Q _n ] ^T for each axis by the equation (1) by the drive axis position command calculation unit 106.

  Vector q = vector q (vector r) (1)

  Equation (1) is a solution to the inverse kinematic problem determined by the axis configuration of the machine tool, and the vector q that is the solution is sent to each of the drive axis position control units 108-1 to 108-n having the same independent configuration. .

Next, the function of the shaft position control unit 108-1 will be described using the first drive shaft as an example. The position detector 58 fastened to the servo motor 57 is a position detector that detects the shaft position of the first drive shaft. The position detection value q _d1 that is the output is subtracted from the position command value q ₁ by the subtractor 50. The output of the subtracter 50 is amplified by the position deviation amplifier Gp. On the other hand, the position command value q ₁ is differentiated by the differentiator 53 and added to the output of the position deviation amplifier Gp by the adder 51 to become the speed command value V _c1 of the first drive shaft.

The position detection value q _d1 is time-differentiated by the differentiator 55 and becomes the speed detection value v _d1 . The subtractor 52 subtracts the speed detection value v _d1 from the speed command value V _c1 . The output of the subtracter 52 is normally proportional-integral amplified by the speed deviation amplifier Gv. On the other hand, the output of the differentiator 53 is differentiated again by the differentiator 54 and multiplied by Cb by the amplifier Cb. Cb is a constant for calculating the deceleration driving force for acceleration component of the position command value q _1. The output of the amplifier Cb is added to the output of the speed deviation amplifier Gv by the adder 56, and becomes the driving force command value τ _{c1 of} the first driving shaft.

The power amplifying unit amplifies the driving force command value τ _c1 and supplies it to the servo motor 57. As a result, the servo motor 57 generates driving forces corresponding to the driving force command value τ _c1 . The position detection value q _d1 is controlled so as to coincide with the position command value q ₁ by the operation of the shaft position control unit described above, and as a result, each shaft position control unit independently performs the same operation. The position / posture motion locus of the cutting tool coincides with the position command vector r.

  As described above, the conventional position control device 100 cuts the workpiece (workpiece) into a desired shape by controlling the position and orientation of the cutting tool on the target motion trajectory. However, since only the position and orientation are controlled, it is not possible to apply force control to the workpiece.

  In view of the above-described problems of the prior art, the present invention is to provide a position control device including a force control system that applies a desired force to a workpiece according to the shape of the workpiece.

  In a numerical control machine such as a machine tool, the present invention estimates the surface shape of a workpiece from the position command trajectory of the cutting tool, thereby determining the direction of the force or moment applied to the workpiece according to the shape. The above-mentioned problems are solved by adding a force control system to the position control device that determines and makes these values desired.

  That is, a position control device that drives a drive unit of a mechanism of a numerically controlled machine tool by a servo motor, detects a position of the drive unit by a position detector, and controls the position of the drive unit according to a position command value from a host device A block for detecting a step vector between trajectories from a position command value vector indicating an operation trajectory of the cutting tool expressed in a reference coordinate system, a block for detecting a normal vector on the surface of the workpiece, A position command calculation unit including a force command calculation unit including a block that performs coordinate conversion of a force target value indicated by a constraint coordinate system determined from the direction of a normal vector into a force target value indicated by a drive axis coordinate system, By adding the force target value shown in the drive axis coordinate system and the drive force command value for position control of the drive unit, the drive force command value for position and force control of the drive unit is obtained. Be formed is a numerically controlled machine tool control apparatus characterized.

  According to the present invention, a desired force or moment can be applied to the surface of the workpiece simultaneously with position control along the surface of the workpiece processed by the cutting tool without using a force detector or the like. Applying force control can be achieved. For this reason, if work tools such as cloth, scissors, and sandpaper are mounted at the tool position instead of the cutting tool, operations such as wiping, sweeping, and polishing on the surface of the workpiece can be realized by the machine tool.

  In the present embodiment, the position command calculation unit detects the step vector between the trajectories from adjacent position command trajectories, and detects the normal vector for the workpiece from the relationship with the traveling direction vector on the trajectory. Further, a force command calculation unit is added which sets a force target value in a restrained coordinate system determined from the normal vector direction, and converts and outputs the force target value in each drive axis coordinate system. The force target value of each drive shaft takes a form added to the drive force command value of each drive shaft position control unit.

  The machine tool described below has a configuration in which the position / posture of the cutting tool in the space can be freely controlled, and the work tool mounted at the tool position is not only limited to the position but also the posture constraint on the workpiece. It shall be a shape.

  FIG. 1 is a block diagram showing a configuration of a position control device 200 to which a force control system according to the present invention is added. In this block diagram, the same parts as those in FIG. 5 (conventional example) are given the same names and numbers, and the description thereof is omitted. FIG. 2 is a diagram illustrating an example of a machined object that has been cut. X, Y, and Z (not shown) indicate a reference coordinate system, and a straight line that runs in the X-axis direction while shifting in the Y-axis direction indicates an operation locus of the cutting tool.

A method for deriving the normal vector n toward the inside of the workpiece at the point Pb on the motion locus will be described. First, the operation of the trajectory step vector detection unit 202 in FIG. 1 will be described. Here, attention is paid to the upper three-dimensional vector p = [x, y, z] ^T indicating the position in the position command vector r indicating the motion trajectory. First, an appropriate step detection line passing through the XY coordinate value of the point Pb shown in FIG. 2 is set on the XY plane, and a two-dimensional direction vector having an arbitrary length is set to l _d = [x _d , y _d ] ^T. Now, when the motion trajectory (vector) p advances on the path b in the X-axis increasing direction, the passage detection of the point Pb can be detected by the sign inversion of Expression (2). [X ₀ , y ₀ ] ^T is an arbitrary point coordinate of the step detection line.

y _d (x−x ₀ ) + x _d (y−y ₀ ) (2)

That is, the vector p at the time of sign reversal in Equation (2) is the coordinate value of the point Pb. Since the coordinate value of the point Pa on the step a on the path a can also be detected by the same method, the step vector p _ab between the trajectory between the point Pa and the point Pb can be detected from both coordinate values. The above is the operation of the inter-trajectory step vector detection unit 202 in FIG.

Next, the operation of the normal vector detection unit 204 in FIG. 1 will be described. Since the motion trajectory (vector) p is updated every minute time ΔT, the minute displacement vector Δp in the traveling direction at the point Pb can be detected from the difference between the vector p just before passing the point Pb and the vector p just after passing. . Here, since both the minute displacement vector Δp and the step vector p _ab between the trajectories can be regarded as vectors on the tangent plane of the workpiece upper point Pb, the orthogonal vector with respect to the tangent plane is calculated by the outer product calculation of Expression (3). I want.

Δp × p _ab (3)

Furthermore, in this case, since the orthogonal vector in which the Z component takes a negative value is directed toward the inside of the workpiece, when the Z component in the formula (3) takes a positive value, the formula (3) is multiplied by −1, The normal vector n = [n _x , n _y , n _z ] ^T can be detected. The above is the operation of the normal vector detection unit 204 in FIG. In FIG. 2, the step detection line is set as a single straight line, but a broken line may be used, and a plurality of step detection lines may be used depending on how the step difference between the trajectories of the workpiece is changed. If set in advance, the normal vector can be accurately detected at each part on the motion trajectory.

FIG. 3 illustrates the position and orientation relationship between the work tool and the workpiece in the first embodiment. Here, the posture constraint that the work tool receives from the workpiece is assumed to be the posture of the normal vector n for convenience. Then, u ₂ (r) and u ₃ (r) can be set using u ₁ (r) in the normal vector direction as position constraint coordinates and Equations (4) and (5) as posture constraint coordinates. Φ _S and θ _S represent the restraint posture of the work tool in terms of Euler angles.

u ₂ (r) = φ−φ _S (4)
u ₃ (r) = θ−θ _S (5)

However, the posture in which the work tool is restrained when the normal vector is in the −Z-axis direction is set as the reference posture of the work tool. The above three-dimensional constraint coordinate system u (r) = [u ₁ (r), u ₂ (r), u ₃ (r)] In the ^T direction, the corresponding one-dimensional force: f _F1 and two-dimensional moment : F _F2 and f _F3 can be added to the workpiece.

Here, the relationship of Formula (6) is established between a small displacement (vector) Δr at an arbitrary point on a certain locus and a virtual displacement (vector) Δu (r) of the corresponding constraint coordinate system.

Therefore, the three-dimensional force target value (vector) f _F = [f _F1 , f _F2 , f _F3 ] ^T given to the workpiece shown in the constraint coordinate system is expressed by the following equation (7) from the principle of virtual work. It is converted into a force target value (vector) f expressed in a 6-dimensional reference coordinate system.

f = T− ^T · E _F ^T · f _F (7)

Note that the matrix T is a transformation matrix shown in Equation (8) for compensating for the Euler angle gradient.

Further, the force target value (vector) f is converted into a force target value (vector) τ expressed in the drive axis coordinate system. This is converted by Equation (10) using the solution of the forward kinematics problem of Equation (9), which is the inverse problem of Equation (1). However, J _r (q) = {∂r (q) / ∂q} ^T is a Jacobian matrix.

r = r (q) (9)
τ = J _r (q) ^T · T ^T · f (10)

Therefore, as a result, the three-dimensional force target value (vector) f _F given to the workpiece is calculated from the mathematical expression (7) and the mathematical expression (10) according to the mathematical expression (11). Vector) τ = [τ _F1 , τ _F2 ,..., Τ _Fn ] ^T.

τ = J _r (q) ^T · E _F ^T · f _F (11)

Above-described vector E _F from the normal vector n in step, the drive shaft position command vector q from the Jacobian matrix J _r a (q) asking each three-dimensional force target value (vector) f _F of the drive axis coordinate system Conversion to the force target value (vector) τ is the action of the constraint / drive axis coordinate conversion unit 206 in FIG. As described above, in the position and force control apparatus of the present invention, the inter-trajectory step vector detection unit 202, the normal vector detection unit 204, and the constraint / drive axis coordinate conversion unit 206 are added to the conventional position command calculation unit. Thus, not only the position command value but also the force target value for the restraining direction is output to each of the drive shaft position control units 208-1 to 208-n.

The drive axial force target values τ _F1 , τ _F2 ,..., Τ _Fn , which are outputs of the constraint / drive axis coordinate conversion unit 206, are for position control of the drive axis position control units 208-1 to 208-n. Is added by the adder 59 to be calculated as a final driving force command value τ _c1 for position and force control.

  The machine tool according to the second embodiment has a three-dimensional orthogonal axis configuration along the reference coordinate system, and controls the X, Y, and Z axes with the first, second, and third drive axes. Each axis is driven by a ball screw drive system using a rotary servo motor. Further, the posture of the cutting tool and the work tool is assumed to have a structure that is mechanically constrained to the reference posture of the work tool. That is, only the position of the cutting tool and the work tool in space can be freely controlled.

FIG. 4 shows the positional relationship between the work tool and the workpiece in the second embodiment. In this example, the force vector that the work tool can apply to the workpiece is only the one-dimensional force f _{F in} the normal vector direction.

Hereinafter, this example will be described. In this example, since there is no degree of freedom regarding the posture, only the three-dimensional vector p = [x, y, z] ^T related to the position needs to be considered. First, detection from the step vector between trajectories to the normal vector n is the same as in the first embodiment. Next, consider the coordinate conversion of the one-dimensional force target value f _F given to the workpiece to the force target value τ in the drive axis coordinate system.

The solution to the forward kinematics problem is given by equation (12). However, L _x , L _y , and L _z are feed amounts per motor rotation of each axis.

p = p (q) = [(L _x / 2π) q ₁ , (L _y / 2π) q ₂ , (L _z / 2π) q ₃ ] ^T (12)

Therefore, the Jacobian matrix J _r (q) in Expression (10) is a constant matrix of Expression (13).

J _r (q) = diag {(L _x / 2π), (L _y / 2π), (L _z / 2π)} (13)

Furthermore, the vector _{E F} since the can show in equation (14) to the equation (6),

E _F = [(n _x / | n |), ( _ny / | n |), ( _nz / | n |)] (14)

  The force target value (vector) τ of the drive axis coordinate system can be coordinate-transformed using Equation (11) to Equation (15).

τ = [L _x n _x , L _y n _y , L _z n _z ] ^T {f _F / (2π | n |)} (15)

The force target value (vector) τ is added by the adder 59 to the driving force command value for position control of each driving shaft position control unit 208-1 to 208-n to obtain the final position and force control. Driving force command values τ _c1 , τ _c2 , and τ _c3 are configured.

It is a block diagram which shows the structure of the position and force control apparatus by this invention. It is explanatory drawing of this invention which showed an example of the processed target object cut. It is a figure which shows the positional relationship of the working tool in Example 1, and a process target object. It is a figure which shows the positional relationship of the working tool in Example 2, and a process target object. It is a block diagram which shows the structural example of the conventional position control apparatus.

Explanation of symbols

50, 52 subtractor, 51, 56, 59 adder, 53, 54, 55 differentiator, 57 servo motor, 58 position detector, 100 position control device, 102 position command calculation unit, 104 acceleration / deceleration calculation unit, 106 drive Axis position command calculation unit, 108 Drive axis position control unit, 200 Position control device, 202 Inter-trajectory step vector detection unit, 204 Normal vector detection unit, 206 Constraint / drive axis coordinate conversion unit, 208 Drive axis position control unit, PD Position command value, vector r position command vector, vector p three-dimensional position command vector, point on Pa motion trajectory (path a), point on Pb motion trajectory (path b), step vector between vector p _ab trajectory, vector n Normal vector, vector l _d step detection line direction vector, Δp minute displacement vector in the traveling direction, f _F three-dimensional force target value shown in constraint coordinate system, vector τ Force target value vector expressed in the drive axis coordinate system, q Position command vector for each axis, V _c1 speed command value, v _d1 speed detection value, τ _c1 drive force command value, q _d1 position detection value.

Claims (1)

A position control device that drives a drive unit of a mechanism of a numerically controlled machine tool by a servo motor, detects the position of the drive unit by a position detector, and controls the position of the drive unit according to a position command value from a host device. And
A block for detecting a step vector between trajectories from a position command value vector indicating an operation trajectory of the cutting tool expressed in a reference coordinate system;
A block for detecting a normal vector on the surface of the workpiece,
A block for coordinate-converting a force target value indicated by a constraint coordinate system determined from the direction of the normal vector to a force target value indicated by a drive axis coordinate system;
A position command calculation unit including a force command calculation unit comprising:
A drive force command value for controlling the position and force of the drive unit is configured by adding the force target value indicated in the drive axis coordinate system and the drive force command value for controlling the position of the drive unit. A control device for a numerically controlled machine tool characterized by

Images