JPH0969005A - Servo system control device and control method - Google Patents

Abstract

(57) 【Abstract】 PROBLEM TO BE SOLVED: To change a route between two line segments for an arbitrary line segment combination, reduce takt time, and simplify programming. SOLUTION: A straight line including a line segment connecting a second point and a third point which is a transit target point is defined as an X axis, and a straight line on a plane including a first point, a second point and a third point, wherein X A path coordinate system having a straight line orthogonal to the axis as the Y axis is assumed with respect to the reference coordinate system, the first point is the initial position, the third point is the target position, and the first point and the second point are connected. A time function is generated using the initial speed, target speed, and target acceleration at the branch point when moving from the initial position to the target position to draw a circular arc from the branch point on the straight line and move the path on the X axis. Since the controlled device is servo-driven by the linear trajectory control from the first point to the third point based on the function,
The controlled device can be driven by drawing a substantially L-shaped trajectory without stopping in the middle of the route, the takt time can be reduced, and the program can be simplified.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a servo system control device and control method, and more particularly to a servo system control device and control method for performing linear trajectory control.

In a device having a servo control mechanism such as a robot or an NC (Numerical Control) machine tool, accurately generating a target position (motion control position) of a servo control system directly affects assembly and machining accuracy. To exert
It is an important issue.

However, if the target position is accurately generated, the working time (tact time) becomes long, which is not necessarily efficient. Therefore, in an apparatus including such a servo control mechanism, it is desired not only to accurately generate the target position but also to reduce the takt time.

[0004]

2. Description of the Related Art In a conventional device having a servo control mechanism, linear trajectory control is the most frequently used control method.

In an operation generally called arch motion, it is possible to continuously connect a combination of relatively simple line segments (movement loci) used for a pick and place operation.

For example, as shown in FIG. 12, when the control is performed by the combination of the linear loci used for the pick and place operation, the operation is temporarily stopped at each of the connecting points P ₁ , ..., P ₈ . Was there.

[0007]

When control is performed using the above-described conventional arch motion operation, in order to perform complicated assembly work, it is possible to change the route in two line segments for an arbitrary line segment combination. Need to

Regarding the locus of such a route changing portion, its shape does not matter except for special work such as welding. Further, in the teaching operation, it is necessary to instruct a via point with a linear locus, so that it is convenient from the viewpoint of control programming if it can be executed as a combination of linear loci even in the reproducing operation.

Therefore, the objects of the present invention are: 1) It is possible to change the route in two line segments for any combination of line segments, 2) the tact time can be reduced, and 3) programming can be simplified. To provide a control device and a control method for a servo system.

[0010]

Means for Solving the Problems To solve the above problems,
Therefore, the invention of claim 1 is the first point (x₁, Y₁, Z
₁) To the third point (x_Three, Y_Three, Z_Three) Towards non-direct
Servo system controller that linearly servo-drives the controlled device
And the second point (x₂, Y₂,
z₂) And the line segment connecting the third point to the X-axis
Including the first point, the second point, and the third point.
Which is a straight line on a flat plane that is orthogonal to the X axis.
Assuming the path coordinate system as the axis with respect to the reference coordinate system,
The first point is the initial position and the third point is the target position.
And on the straight line connecting the first point and the second point
Draw an arc from the branch point and move the path on the X axis.
Therefore, the amount when moving from the initial position to the target position
Parameters such as initial speed, target speed, and target acceleration at various points
A time function generating means for generating a time function as
From the first point to the third point based on a time function
Drive control that servo-drives the controlled device by linear trajectory control
And a control means.

According to the first aspect of the invention, the time function generating means includes the second point (x ₂ , y ₂ ,
z ₂ ) and a straight line including a line segment connecting the third point are X-axis, and a straight line on a plane including the first point, the second point and the third point, which is orthogonal to the X-axis is Y-axis. Assuming that the route coordinate system is relative to the reference coordinate system, the first point is the initial position, the third point is the target position, and an arc is drawn from the branch point on the straight line connecting the first point and the second point. A time function is generated using the initial velocity, the target velocity, and the target acceleration at the branch point when moving from the initial position to the target position in order to shift the path on the X axis.

As a result, the drive control means servo-drives the controlled device from the first point to the third point by linear trajectory control based on the generated time function. According to a second aspect of the present invention, a servo that non-linearly servo-drives the controlled device from the first point (x ₁ , y ₁ , z ₁ ) to the fourth point (x ₄ , y ₄ , z ₄ ). A system control device, which is a third target point (x ₃ , y ₃ , z ₃ ) which is a target point for transit and the fourth target point.
A straight line including a line segment connecting points is defined as an X-axis, and a straight line on a plane including a second point (x ₂ , y ₂ , z ₂ ), the third point and the fourth point, and the X-axis Assuming a path coordinate system with a Y axis as an orthogonal straight line with respect to a reference coordinate system,
The first point is the initial position, the fourth point is the target position, and an arc is drawn from a first branch point on a straight line connecting the first point and the second point, and the second point and the second point. The first branch point at the time of going from the initial position to the target position in order to draw a circular arc from the second branch point on the locus going on the straight line connecting the third points and shift the path on the X axis; A time function generating means for generating a time function using the initial speed, the target speed, and the target acceleration at the second branch point as parameters, and the controlled device is linear from the first point to the fourth point based on the time function. Drive control means for servo-driving by trajectory control.

According to the second aspect of the present invention, the time function generating means uses a straight line including a line segment connecting the third point and the fourth point, which are the target points, as the X axis, and the second point and the third point. And a straight line on a plane including the fourth point, where a straight line that is orthogonal to the X-axis and has the Y-axis is assumed to be the reference coordinate system, the first point is the initial position, and the fourth point is From the second branch point on the trajectory that goes to the target position and draws an arc from the first branch point on the straight line connecting the first point and the second point, and goes on the straight line connecting the second point and the third point A time function is generated using the initial velocity, the target velocity, and the target acceleration at the first branch point and the second branch point when moving from the initial position to the target position in order to shift the path on the X axis while drawing an arc. .

The drive control means servo-drives the controlled device from the first point to the fourth point by linear trajectory control based on the generated time function. The invention according to claim 3 is from the first point (x ₁ , y ₁ , z ₁ ) to the third point (x ₃ ,
y ₃ , z ₃ ), which is a servo system control method for non-linearly servo-driving the controlled device, wherein a second target point (x ₂ , y ₂ , z ₂ ) which is a via target point and the second target A straight line including a line segment connecting three points is defined as the X-axis, and the first point and the second point
A straight line on a plane including a point and the third point, assuming a path coordinate system having a straight line orthogonal to the X axis as the Y axis with respect to a reference coordinate system, the first point as an initial position, The third point is set as a target position, and an arc is drawn from a branch point on a straight line connecting the first point and the second point to form the X
Based on the time function, a time function generating step of generating a time function using the initial velocity, the target velocity, and the target acceleration at the branch point when moving from the initial position to the target position to shift the path on the axis; The first
A drive control step of servo-driving the controlled device from a point to the third point by linear trajectory control.

According to the third aspect of the present invention, in the time function generating step, a straight line including a line segment connecting the second point and the third point which are the target points of the passage is set as the X axis, and the first point and the second point are set. And a straight line on a plane including the third point, where a path coordinate system having a straight line orthogonal to the X axis as the Y axis is assumed with respect to the reference coordinate system, the first point is the initial position, and the third point is Initial velocity and target at the branch point when going from the initial position to the target position in order to shift the route on the X axis by drawing an arc from the branch point on the straight line connecting the first point and the second point A time function is generated using the velocity and target acceleration as parameters.

As a result, the drive control step servo-drives the controlled device from the first point to the third point by linear trajectory control based on the time function. The invention according to claim 4 is from the first point (x ₁ , y ₁ , z ₁ ) to the fourth point (x ₄ ,
y ₄ , z ₄ ), which is a servo system control method for non-linearly servo-driving the controlled device, wherein a third target point (x ₃ , y ₃ , z ₃ ) and A straight line including the line segment connecting the four points is defined as the X axis, and the second point (x ₂ , y ₂ ,
z ₂ ), a straight line on a plane including the third point and the fourth point, and a path coordinate system having a Y axis as a straight line orthogonal to the X axis is assumed to be the reference coordinate system, and One point is an initial position, the fourth point is a target position, and an arc is drawn from a first branch point on a straight line connecting the first point and the second point to form the second point and the third point. The first branch point and the first branch point when moving from the initial position to the target position in order to draw a circular arc from the second branch point on the locus going on the straight line connecting the points and shift the path on the X axis. A time function generating step of generating a time function using the initial speed, the target speed, and the target acceleration at the bifurcation point 2 as parameters, and linear trajectory control of the controlled device from the first point to the fourth point based on the time function. Drive control process of servo drive by Constitute Te.

According to the fourth aspect of the invention, in the time function generating step, the straight line including the line segment connecting the third point and the fourth point, which is the route target point, is used as the X-axis, and the second point and the third point. And a straight line on a plane including the fourth point, where a straight line that is orthogonal to the X-axis and has the Y-axis is assumed to be the reference coordinate system, the first point is the initial position, and the fourth point is From the second branch point on the trajectory that goes to the target position and draws an arc from the first branch point on the straight line connecting the first point and the second point, and goes on the straight line connecting the second point and the third point A time function is generated using the initial velocity, the target velocity, and the target acceleration at the first branch point and the second branch point when moving from the initial position to the target position in order to shift the path on the X axis while drawing an arc. .

As a result, the drive control step servo-drives the controlled device from the first point to the fourth point by linear trajectory control based on the time function.

[0019]

BEST MODE FOR CARRYING OUT THE INVENTION Prior to the description of the embodiments,
1 point P₁To second point P₂Straight gauge to get to
For the principle of drawing a trace using a trapezoidal velocity curve
Will be explained. Mode 1 operation As shown in FIG. 1 (a), the first point P₁To second point P₂
The trapezoidal velocity is calculated by simply decomposing the linear trajectory toward
When a curve is generated, the curves shown in FIG.
Will be

By the way, the acceleration / deceleration time needs to be the same for each coordinate component, that is, t _1x = t _1y = t _1z and t _2x = t _2y = t _2z .

Therefore, in general, linear interpolation is performed as shown in FIG. 1 (c) for the length l of the line segment along the direction vector n of the line segment connecting the first point P ₁ and the second point P ₂ .
A method of obtaining the point P on the line segment connecting the first point P ₁ and the second point P ₂ from the equation (1) is used.

P = P ₁ + ^l P · n (1) In this way, the control method of performing the trajectory calculation by paying attention to the length of the line segment cannot sufficiently cope with the route change.

That is, as shown in FIG.
During movement from the point P ₁ at a constant speed toward the second point P _2, in the case of a positioning straight line of the target point P connecting the first point P ₁ and the second point P _2, shown in FIG. 3 like,
Along the planned trajectory on the phase plane l-vl (see FIG.
(See (a)), by generating a time function depending on the length component (see FIG. 3 (b)), the first point P ₁ and the second point P ₁
It is possible to change the route on the straight line connecting the points P ₂ .

However, as shown in FIG. 2 (b),
When changing the route from the point P on the straight line connecting the first point P ₁ and the second point P ₂ to the straight line trajectory from the second point P ₂ to the point P ₃ , a time function depending on the length component is used. Therefore, the route cannot be changed.

Therefore, in the present embodiment, the problem is solved by introducing a separate coordinate system at any time in accordance with the direction of the linear trajectory that is the drive path of the controlled device. Hereinafter, the coordinate system thus introduced will be referred to as a route coordinate system, and a fixed coordinate system regardless of the route coordinate system will be referred to as a reference coordinate system.

As shown in FIG. 4A, the first state in the stopped state
Point P₁Departure from the second point P₂A straight path that stops at
think of. 2 points P₁, P₂When given,
As shown in equation (2), the first point P ₁And the second point P₂To
A straight line including connecting line segments is x-axis (first point P₁To second point
P₂The path coordinate system Σ with the direction toward
l is determined by the following equation (3) with respect to the reference coordinate system Σb.
Confuse.

N = (P ₂ −P ₁ ) / ‖P ₂ −P ₁ ‖ (2)

[0028]

[Equation 1] In this case, the y-axis and the z-axis are set to be orthogonal to each other and to the direction orthogonal to the x-axis.

Coordinates ^l P ₁ and ^l P ₂ in the route coordinate system Σl corresponding to the first point P ₁ and the second point P ₂ are expressed by equation (4). _{^{l P 1 = R t P 1}} , l P 2 = R t P 2 ... (4) In addition the velocity vector in the path coordinate system Σl along the line trajectory ^l v _m and the acceleration vector ^l a _m, with respect to the reference coordinate system Σb The velocity vector V _m and the acceleration vector a _m given in advance are given by equations (5) and (6), respectively.

[0030] ^{_{l v m = R t v m}} ... (5) l a m = R t a m ... (6) At this time, point P on the line segment connecting the first point P ₁ and the second point P ₂ The x coordinate can be obtained as shown in FIG. The y coordinate and the z coordinate of the point P are the route coordinate system Σ
Since it does not change on l, the coordinates of the point P in the reference coordinate system Σb are expressed by equation (7) based on the point ^l P in the route coordinate system Σl.

P = R^lP where^lP = (^lP_x ^lP_2y ^lP_2z) '... (7) If the route is not changed in this way,
I will call it 1. Mode 2 operation Next, the first point P₁To second point P₂Point P while moving to
When changing the route from a straight path to the third point (from
Below, it is called mode 2. ) Will be described with reference to FIG.
You.

The x-axis of the path coordinate system Σl is the same as in the case of mode 1.
Similarly, the second point P₂To third point P _ThreeIncluding the line segment connecting
The straight line is the x-axis (the second point P₂To third point P_ThreeThose heading for
Direction is the positive direction), and the y-axis is the first point P₁And the second place
Point P₂A straight line including the line segment connecting the lines and a plane s including the x axis₂Up,
Moreover, the axis is orthogonal to the x-axis, and the z-axis is the plane s.₂Perpendicular to
The axis.

From this, it is understood that the route from the first point P ₁ to the third point P ₃ via the point P can be treated as a problem on the xy plane in the route coordinate system Σl.
That is, when three points P ₁ , P ₂ and P ₃ are given, a straight line including a line segment connecting the second point P ₂ to the third point P ₃ is given as the x-axis as shown in the following formula (8). The y-axis is a straight line including a line segment connecting the first point P ₁ and the second point P ₂ and a plane s ₂ including the x-axis, and a path coordinate system Σl that is an axis orthogonal to the x-axis is a reference coordinate. For the system Σb, the following equations (9) to (1
1) Set as shown in equation.

N = (P ₃ −P ₂ ) / ‖P ₃ −P ₂ ‖ (8) a = n × (P ₂ −P ₁ ) / ‖n × (P ₂ −P ₁ ) ‖ ... (9) ) O = a × n (10) Σb → Σl: R = (n o a) (11) Further, the point P which is the route change start point and the third point which is the end point.
The point P ₃ is given by the equation (12) in the route coordinate system Σl.

^L P = R ^t P, ^l P ₃ = R ^t P ₃ (12) Furthermore, in the route coordinate system of mode 1 from the first point P ₁ to the second point P ₂ , it corresponds to mode 1. Using the magnitude v0 of the velocity at the point P in the x-axis direction (the axial direction from the first point P ₁ to the second point P ₂ ), the velocity components ^l v _x0 , ^l in the x-axis and y-axis directions in the mode 2 are used. V _y0 is represented by the equations (13) and (14), respectively.

[0036]^lv_x0= V₀・ (P₂−P₁) / ‖P₂−P₁‖ ・ N… (13)^lv_y0= V₀・ (P₂−P₁) / ‖P₂−P₁‖ ・ O… (14) However, v₀Is the velocity at the point P in mode 1 Furthermore, the velocity vector in the path coordinate system Σl^lv
_mAnd acceleration vector ^la_mIs the reference coordinate system Σb
Velocity vector V given in advance_mAnd acceleration
Tor a_mAccording to equation (15) and equation (16),
Given.

[0037] ^l v _m = at _{^{R t v m ... (15)}} l a m = R t a m ... (16) where, from the point P to the straight line connecting the second point P ₂ and the third point P ₃ The route change problem is equivalent to the following two time function generation problems.

1) The time function for starting the movement from the point ^l P _{x at the} initial velocity ^l v _x0 and reaching the point ^l P _3x and stopping is obtained (see FIG. 6 (a)). 2) Start moving from the point ^l P _{y at the} initial velocity ^l v _y0 ,
_Find the time function to stop at ^l P _3y (Fig. 6
(B)).

As a result, the time function in the route changing process is given by the equation (17). P = R ^l P (17) Here, ^l P = ( ^l P _x ^l P _y ^l P _3z ).

When t> t _c , the path coordinate system Σ
Since the generation of the locus in the y-axis direction is completed at l, the state becomes equivalent to the mode 1. Then, if the third point P ₃ is reached after t> t _c , the state is stopped, and if a new route change designation is given before reaching the third point P ₃ , the mode 2 is entered again. Mode 3 operation On the other hand, if a new route change designation is given when t ≦ t _c , the mode shifts to the mode 3 described below.

Here, the route changing operation in the mode 3 will be described with reference to FIG. From the first point P ₁ to the second
A case where the route is changed to a straight line trajectory from the point P ′ to the third point while moving to the point P ₂ and further to a straight line trajectory from the point P to the fourth point while moving to the third point will be described. To do.

The x-axis of the path coordinate system Σl is the same as in mode 2
Similarly, the third point P_ThreeTo 4th point P _FourIncluding the line segment connecting
The straight line is the x-axis (the third point P_ThreeTo 4th point P_FourThose heading for
And the y-axis is the second point P.₂, Third place
Point P_ThreeAnd 4th point P_FourPlane s containing_ThreeTop and x-axis
And the z-axis is the plane s_ThreeAxis perpendicular to.

In this case, as in the second mode, the path coordinate system Σl is changed to the following (18) with respect to the reference coordinate system Σb.
It is defined as in Expressions (21). n = (P ₄ −P ₃ ) / ‖P ₄ −P ₃ ‖ ... (18) a = n × (P ₃ −P ₂ ) / ‖n × (P ₃ −P ₂ ) ‖ ... (19) o = a × n (20) Σb → Σl: R = (n o a) (21) Further, a point P that is a route change start point and a fourth point that is an end point.
The point P ₄ is given by the equation (22) in the route coordinate system Σ _l .

^L P = R ^t P, ^l P ₄ = R ^t P ₄ (22) Further, in the path coordinate system of mode ₁ from the first point P ₁ to the second point P ₂ , the direction vector in mode 2 Using velocity magnitudes v _2x and v _2y at point P corresponding to n ₂ (x-axis direction) and direction vector o ₂ (y-direction), velocity components in the x-axis, y-axis, and z-axis directions in mode 3
Representing ^l v _x0 , ^l v _y0 , and ^l v _z0 , respectively (23)
Expressions (24) and (25) are used.

[0045]^lv_x0= (V_2x・ N₂+ V_2y・ O₂) ・ N… (23)^lv_y0= (V_2x・ N₂+ V_2y・ O₂) ・ O… (24)^lv_z0= (V_2x・ N₂+ V_2y・ O₂) · A (25) Furthermore, the velocity vector in the path coordinate system Σl^lv
_mAnd acceleration vector ^lam refers to the reference coordinate system Σb
Velocity vector V given in advance_mAnd acceleration vector
Ru a_mIs given by equation (26) and equation (27) respectively.
available.

^L v _m = R ^t v _m (26) ^la _m = R ^t a _m (27) Here, from the point P to the straight line connecting the third point P ₃ and the fourth point P ₄ . The route change problem is equivalent to the following three time function generation problems.

1) Obtain a time function for starting the movement from the point ^l P _{x at the} initial velocity ^l v _x0 and reaching the point ^l P _4x and stopping (see FIG. 8 (a)). 2) Start moving from the point ^l P _{y at the} initial velocity ^l v _y0 ,
_Find the time function to stop at ^l P _4y (Fig. 8
(B)).

3) _Obtain a time function for starting the movement from the point ^l P _{z at the} initial velocity ^l v _z0 and reaching the point ^l P _4z and stopping (see FIG. 8 (c)). As a result, the time function in the route change process is (2
It will be given by the equation (8).

P = R ¹ P (28) Here, ¹ P = ( ¹ P _X ¹ P _Y ¹ P _Z ) ′.

Note that when t _c1 <t <t _c2 , the trajectory generation in the y-axis direction is completed, so that a state equivalent to mode 2 is obtained. Further, at t> t _c2 , the z-axis in the path coordinate system Σ _l is obtained. Since the generation of the trajectory in the direction is completed, the state becomes equivalent to the mode 1.

If a new route change designation to the fifth point P ₅ is given at t <t _c1 , the above equations (23) to (25) are changed to the following three equations (23) in the mode 3 state. '
It is possible to control by substituting the expression (25) '.

^L v _x0 = (v _3x · n ₃ + v _3y · o ₃ + v _3z · a ₃ ) · n (23) ′ ^l v _y0 = (v _3x · n ₃ + v _3y · o ₃ + v _3z · a ₃₎ · o ... (24) shown in _{^{'l v z0 = (v 3x}} · n 3 + v 3y · o 3 + v 3z · a 3) · a ... (25)' 9 mode transition state in the above.

For example, at t <t _c1 described above, the fifth
The state transition when a new route change designation toward the point P ₅ is given corresponds to the transition state of C ₈ in FIG. As described above, according to the present embodiment, by combining the operations of modes 1 to 3, it is possible to change the route in an arbitrary direction using the linear movement command.

In this case, the route can be automatically changed without giving a parameter other than the new target point, and continuous operation can be performed without stopping from the operation start point to the final target point. It is possible to shorten the tact time.

Further, since a special instruction for changing the route is unnecessary, the programming can be simplified and the man-hour can be reduced.

[0056]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS FIGS. 10 and 11 show a programming example and a path locus for actually performing an arch motion operation. In this case, in order to realize it concretely, a robot capable of linear trajectory control is set as the controlled device,
A drive device for actually driving the robot and a control device as a time function generating means are required.

FIG. 10 (a) is a locus when the route is not changed, and FIG. 10 (b) is an example of a program for the corresponding control device. In FIG. 10 (b), MOV A is an instruction for performing a linear movement from the current position to point A, and WAIT UNTIL (B) is an instruction for keeping a wait state (Wait) until condition B is satisfied.

As a result, according to the embodiment of FIG. 10, the first
It departs from the point P _1, passes through the second point P ₂ and the third point P ₃ , and stops at the fourth point P ₄ . FIG.
FIG. 11A is a trajectory when a route is changed, and FIG.
(B) is an example program for the corresponding control device.

As a result, according to the embodiment of FIG. 11, the first
It departs from the point P ₁ , reaches the fourth point P ₄ without passing through the second point P ₂ and the third point P ₃ , and stops. As described above, according to the present embodiment, even when the route change shown in FIG. 11 is performed, no special command is required, and the start timing of the linear movement command (= MOV) is simply changed, and the continuous route is changed. It is possible to draw a trajectory.

[0060]

As described above, according to the invention described in claim 1, the time function generating means separates the second point (x ₂ , y ₂ , z ₂ ) and the third point, which are the target points of the route. A straight line including a connecting line segment is defined as an X axis, and a straight line on a plane including the first point, the second point, and the third point, which is orthogonal to the X axis is Y.
Assuming the path coordinate system as the axis with respect to the reference coordinate system,
The point is the initial position, the third point is the target position, and
The initial speed, target speed, and target acceleration at the branch point when moving from the initial position to the target position to draw a circular arc from the branch point on the straight line connecting the first point and the second point and move the route on the X axis are parameters. As a result, the drive control means servo-drives the controlled device from the first point to the third point by linear trajectory control based on the generated time function. Since the controlled device can be driven by drawing a substantially L-shaped trajectory without stopping halfway, the tact time can be reduced, and control can be performed only by the linear trajectory control command. It is possible to simplify and reduce the programming process.

According to the second aspect of the present invention, the time function generating means uses the straight line including the line segment connecting the third point and the fourth point, which are the target points, as the X axis, and the second point and the third point. And a straight line on a plane including the fourth point, where a straight line that is orthogonal to the X-axis and has the Y-axis is assumed to be the reference coordinate system, the first point is the initial position, and the fourth point is From the second branch point on the trajectory that goes to the target position and draws an arc from the first branch point on the straight line connecting the first point and the second point, and goes on the straight line connecting the second point and the third point A time function is generated using the initial speed, the target speed, and the target acceleration at the first and second branch points when moving from the initial position to the target position in order to shift the path on the X axis while drawing an arc. , The drive control means, based on the generated time function, from the first point to the fourth point Since the controlled device is servo-driven by the linear trajectory control, it is possible to drive the controlled device by drawing an arbitrary trajectory without stopping in the middle of the route, and it is possible to reduce the tact time and control the linear trajectory. Since the control can be performed only by the instruction, the program can be simplified and the programming process can be reduced.

According to the third aspect of the present invention, in the time function generating step, a straight line including a line segment connecting the second point and the third point, which is the via target point, is set as the X axis, and the first point and the second point are set. And a straight line on a plane including the third point, where a path coordinate system having a straight line orthogonal to the X axis as the Y axis is assumed with respect to the reference coordinate system, the first point is the initial position, and the third point is Initial velocity and target at the branch point when going from the initial position to the target position in order to shift the route on the X axis by drawing an arc from the branch point on the straight line connecting the first point and the second point A time function is generated using the speed and the target acceleration as parameters, and the drive control step servo-drives the controlled device from the first point to the third point by linear trajectory control based on the time function, so stop in the middle of the route. Controlled device without drawing L-shaped locus Can be driven, it is possible to reduce the tact time, since it is possible to control only in a straight line trajectory control command, it is possible to reduce the programming steps to simplify the program. According to the invention described in claim 4, in the time function generating step, a straight line including a line segment connecting the third point and the fourth point, which are the transit target points, is used as the X axis, and the second point, the third point, and the fourth point. A straight line on a plane including points, where a path coordinate system having a Y axis as a straight line orthogonal to the X axis is assumed with respect to the reference coordinate system, the first point is the initial position, and the fourth point is the target position. , And draw a circular arc from the first branch point on the straight line connecting the first point and the second point, and draw a circular arc from the second branch point on the path going on the straight line connecting the second point and the third point The initial speed, the target speed, and the target acceleration at the first branch point and the second branch point when moving from the initial position to the target position in order to shift the path on the X axis, and a drive function is generated. The process is based on the time function and the controlled device is linearly tracked from the first point to the fourth point. Since the servo drive is performed by the control, the controlled device can be driven by drawing an arbitrary trajectory without stopping in the middle of the route, the tact time can be reduced, and the control can be performed only by the linear trajectory control command. Therefore, the program can be simplified and the number of programming steps can be reduced.

[Brief description of drawings]

FIG. 1 is an explanatory diagram of the principle of linear trajectory control.

FIG. 2 is an explanatory diagram of linear trajectory control.

FIG. 3 is an explanatory diagram of linear trajectory control depending on a length component.

FIG. 4 is an explanatory diagram (mode 1) of linear trajectory control.

FIG. 5 is an explanatory diagram (mode 2) of linear trajectory control.

FIG. 6 is an explanatory diagram (mode 2) of linear trajectory control.

FIG. 7 is an explanatory diagram (mode 3) of linear trajectory control.

FIG. 8 is an explanatory diagram (mode 3) of linear trajectory control.

FIG. 9 is an explanatory diagram of an operation mode transition state.

FIG. 10 is an explanatory diagram of an example (no route change).

FIG. 11 is an explanatory diagram of an example (with a route change).

FIG. 12 is a diagram for explaining a conventional operation.

[Explanation of symbols]

P, P '... Point P ₁ ... 1st point P ₂ ... 2nd point P ₃ ... 3rd point P ₄ ... 4th point

Claims (4)

[Claims] 1. From the first point (x ₁ , y ₁ , z ₁ ) to the third point
Point A _{(x 3, y 3, z} 3) non-linear servo system control device for servo-driving the controlled device toward the second point is through the target point (x _2, y _2, z ₂ ) and the line segment connecting the third point to the X-axis,
A path coordinate system that is a straight line on a plane including a point, the second point, and the third point, and that is a straight line orthogonal to the X axis is the Y axis, is assumed to be a reference coordinate system, and the first point is As an initial position, the third point as a target position, and the initial position for drawing a circular arc from a branch point on a straight line connecting the first point and the second point to move the route on the X axis. From the first point to the third point based on the time function, the time function generating means generating a time function using the initial velocity, the target velocity, and the target acceleration at the branch point when moving from the vehicle to the target position. A servo system control device comprising: a drive control unit that servo-drives a controlled device by linear trajectory control. 2. From the first point (x ₁ , y ₁ , z ₁ ) to the fourth point
A servo-system control device that non-linearly servo-drives a controlled device toward a point (x ₄ , y ₄ , z ₄ ), and is a third target point (x ₃ , y ₃ , z) that is a via target point. ₃ ) and the straight line including the line segment connecting the fourth point are defined as the X-axis, and are straight lines on the plane including the second point (x ₂ , y ₂ , z ₂ ), the third point and the fourth point. Then, a path coordinate system having a straight line orthogonal to the X axis as the Y axis is assumed with respect to the reference coordinate system,
The first point is the initial position, the fourth point is the target position, and an arc is drawn from a first branch point on a straight line connecting the first point and the second point, and the second point and the second point. The first branch point at the time of going from the initial position to the target position in order to draw a circular arc from the second branch point on the locus going on the straight line connecting the third points and shift the path on the X axis; Time function generating means for generating a time function using the initial speed, the target speed, and the target acceleration at the second branch point as parameters, and the controlled device is linearly moved from the first point to the fourth point based on the time function. A servo-system control device comprising: a drive control unit that servo-drives by trajectory control. 3. From the first point (x ₁ , y ₁ , z ₁ ) to the third point
A method for controlling a servo system, which non-linearly servo-drives a controlled device toward a point (x ₃ , y ₃ , z ₃ ), which is a second target point (x ₂ , y ₂ , z) ₂ ) and the line segment connecting the third point to the X-axis,
A path coordinate system that is a straight line on a plane including a point, the second point, and the third point, and that is a straight line orthogonal to the X axis is the Y axis, is assumed to be a reference coordinate system, and the first point is As an initial position, the third point as a target position, and the initial position for drawing a circular arc from a branch point on a straight line connecting the first point and the second point to move the route on the X axis. From the first point to the third point based on the time function, the time function generating step of generating a time function using the initial speed, the target speed, and the target acceleration at the branch point as a parameter when moving from the first position to the target position. A drive control step of servo-driving a controlled device by linear trajectory control, and a servo system control method comprising: 4. From the first point (x ₁ , y ₁ , z ₁ ) to the fourth point
A method for controlling a servo system, which non-linearly servo-drives a controlled device toward a point (x ₄ , y ₄ , z ₄ ), which is a third target point (x ₃ , y ₃ , z) which is a via target point. ₃ ) and the straight line including the line segment connecting the fourth point are defined as the X-axis, and are straight lines on the plane including the second point (x ₂ , y ₂ , z ₂ ), the third point and the fourth point. Then, a path coordinate system having a straight line orthogonal to the X axis as the Y axis is assumed with respect to the reference coordinate system,
The first point is the initial position, the fourth point is the target position, and an arc is drawn from a first branch point on a straight line connecting the first point and the second point, and the second point and the second point. The first branch point at the time of going from the initial position to the target position in order to draw a circular arc from the second branch point on the locus going on the straight line connecting the third points and shift the path on the X axis; A time function generating step of generating a time function using the initial speed, the target speed, and the target acceleration at the second branch point as parameters, and the controlled device is linearly moved from the first point to the fourth point based on the time function. A drive control step of servo-driving by trajectory control, and a servo system control method comprising: