JPH08123531A - Control method for track - Google Patents

Abstract

PURPOSE: To provide stable control, to generate a path not depending on the highest speed and the level of acceleration on a teaching path and to facilitate the prediction of the path by composing the moving track of a pre-decelerating zone, moving zone and post-accelerating zone and independently performing acceleration/deceleration for each zone. CONSTITUTION: The moving track to smoothly connect two teaching paths is composed of pre-decelerating zones S1 S2 for deceleration on the first teaching path, moving zones S2 S3 for move separately from two teaching paths and post-accelerating zones S3 S4 for acceleration on the second teaching zone. By performing interpolation coefficient arithmetic processing and interpolation execution arithmetic processing, the processing of a velocity arithmetic part is switched based on a position 1d1 of the deceleration start point S1 , position ldc1 of the move start point S2 , velocity Vdc1 , position 1ac2 of the move end point S3 , velocity 1va2 and velocity Vm2 at the acceleration end point S4 calculated by an interpolation coefficient arithmetic part, the moving track is composed of the pre-decelerating zone S1 S2 , moving zone S2 S3 and post-accelerating zone S3 S4 and the acceleration / deceleration can be performed for each zone.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method for controlling a transitional trajectory smoothly connecting two continuous trajectories in trajectory control of an automatic work machine and an apparatus for realizing the method.

[0002]

2. Description of the Related Art In an automatic work machine such as a robot, there is a trajectory control for moving a working point set on a tool or a work attached near the tip of a movable part such as a hand according to a taught path and a taught speed. Done. By the way, when the work point moves from a certain teaching route (first teaching route) to the next teaching route (second teaching route) starting from the end point of the teaching route When the teaching in which the moving direction and the speed change discontinuously is performed, a large acceleration occurs at the connection point of the two teaching paths. As a result, vibrations occur, the required accuracy cannot be obtained, and unreasonable force is applied to break the work, the tool, and the machine body. As a method for solving this problem, for example, as disclosed in Japanese Patent Laid-Open No. 64-26911, the acceleration in the second teaching path direction is started at the same time when the deceleration in the first teaching path direction is started. A method is known in which a trajectory smoothly connecting two teaching paths is generated by synthesizing an operation in the first teaching path direction and an operation in the second teaching path direction.

[0003]

In the above prior art, since the deceleration in the first teaching path direction is started at the same time as the acceleration in the second teaching path direction is started, the maximum speed on the first teaching path is increased. , When the magnitude of the acceleration in the first teaching route direction, the maximum speed on the second teaching route, or the magnitude of the acceleration in the second teaching route direction changes, the robot moves away from the first teaching route. The start position and the position where the movement on the second teaching route is started change, and the generated route changes. Furthermore, when the maximum speed and the magnitude of acceleration on the two teaching paths are different, the generated paths are asymmetric with respect to the bisector of the angle formed by the two teaching paths, and thus the generated paths are generated. It is difficult to predict the shape of the route. This is because the generated route is different between the low speed test operation for confirming the operation and the high speed continuous operation for actually performing the work.
In the test driving, the obstacle that could be avoided is interfered in the continuous driving, which makes it difficult to teach the route to avoid the obstacle appropriately.

The object of the present invention is to realize stable control,
Another object of the present invention is to provide a trajectory control method that generates a route that does not depend on the maximum speed and the magnitude of acceleration on a taught route, and that can easily predict the route, and a device that implements the method.

[0005]

In order to achieve the above object, the present invention has been taught as a tool attached to the vicinity of the tip of a movable part of an automatic work machine or a working path set on a work as a taught path. A method of controlling a trajectory to be moved according to speed, wherein two teaching paths are connected between two consecutive teaching paths in which the end point of the first teaching path and the start point of the second teaching path following it are the same. In a method of generating a transition trajectory that smoothly connects in the vicinity of a point and controlling the movement of a work point according to the generated transition trajectory, the generated transition trajectory is decelerated on a first teaching path and a pre-length of 0 or more is used. Deceleration section,
It is composed of a transition section having a length of 0 or more that moves away from the two teaching paths and a post-acceleration section having a length of 0 or more that accelerates on the second teaching path, and acceleration / deceleration is independently performed for each section.

In this method, acceleration / deceleration is performed so that the magnitude of acceleration in the pre-deceleration section, the magnitude of acceleration in the transition section, and the magnitude of acceleration in the rear acceleration section are the same.

Further, in this method, when the acceleration vector in the transition section is decomposed into the first teaching path direction and the second teaching path direction, the magnitudes of the acceleration vectors in the respective teaching path directions are the same, and The distance from the connection point of two teaching routes to the point departing from the first teaching route (transition start point) and the distance from the two teaching routes entering the second teaching route (transition end point) are the same. Alternatively, the acceleration / deceleration is performed so that the speed at the transition start point and the speed at the transition end point are the same. At this time, the speed at the transition start point and the speed at the transition end point are the smaller of the maximum speed on the first teaching path or the maximum speed on the second teaching path. It is desirable to set to the maximum value of acceleration (maximum allowable acceleration) that can realize stable control of the magnitude of acceleration in the pre-deceleration section, transition section, and rear acceleration section.

A control device for performing arithmetic processing based on the above idea is based on the start point and the end point of the teaching route, the shape of the route, the maximum speed on the teaching route, the magnitude of acceleration, the condition for the transition trajectory, etc. Trajectory control means that performs interpolation processing to calculate the interpolation position, which is the position command value for each work point, and calculates the drive amount of the actuator of the movable part from the obtained interpolation position is necessary in order to perform the interpolation processing. Interpolation coefficient calculator for calculating computable interpolation coefficient, at least one speed calculator for calculating speed at interpolation position, at least one moving distance for calculating moving distance by sequentially adding speed at interpolation position Calculation unit, at least one deviation vector calculating unit that calculates a deviation vector corresponding to the vector from the starting point of the taught path to the interpolation position from the moving distance, deviation at the starting point of the teaching path It consists of at least one position calculation unit that calculates the interpolated position by adding the torque, and a coordinate conversion unit that calculates the drive amount of the actuator of the movable unit from the interpolated position, and the speed calculation unit moves away from the two teaching paths. Transition acceleration processing unit that calculates the speed when accelerating in the teaching path direction, the post-acceleration processing unit that calculates the speed when accelerating on the teaching path, and the speed when moving at a constant speed on the teaching path. A constant speed processing unit that calculates the speed, a pre-deceleration processing unit that calculates the speed when decelerating on the teaching path, and a speed when decelerating in the teaching path direction when moving away from two teaching paths And a process switching unit that switches these processes.

Further, the speed calculation unit calculates the speed when accelerating on the teaching path, the deceleration processing unit that calculates the speed when decelerating on the teaching path, and the like on the teaching path. Consists of a constant velocity processing unit for calculating the speed when moving at high speed, a transition processing unit for calculating the speed in each teaching route direction when moving away from two teaching routes, and a process switching unit for switching these processes It may be done.

[0010]

According to the present invention, the transition trajectory smoothly connecting the two teaching paths is composed of the pre-deceleration section, the transition section and the rear acceleration section. At this time, the acceleration / deceleration method in each section is adjusted so as to cancel the amount of change in the positions of the transition start point and the transition end point due to the change in the maximum speed and the magnitude of the acceleration on the teaching route. By adjusting the length, it is possible to realize the maximum speed on the taught route and the generation of the route independent of the magnitude of acceleration.

Further, by making the magnitude of the acceleration in the transition section, the magnitude of the acceleration in the pre-deceleration section, and the magnitude of the acceleration in the rear acceleration section all the same, the magnitude of the acceleration can be made in the entire transition trajectory. Achieve constant and stable control.

Further, when the acceleration vector in the transition section is decomposed into the first teaching path direction and the second teaching path direction, the magnitudes of the acceleration vectors in the respective teaching path directions are the same,
By making the distance from the connection point of the two teaching paths to the transition start point and the distance to the transition end point the same, or by making the speed at the transition start point and the transition end point the same, the first teaching The deceleration in the direction of the path and the acceleration in the direction of the second teaching path are performed symmetrically with respect to the bisector of the angle formed by the two teaching paths. A path that is symmetrical with respect to the path is generated, and the path can be easily predicted.

At this time, the speed at the transition start point and the transition end point is set to the smaller one of the maximum speed on the first teaching path and the maximum speed on the second teaching path. As a result, the length of either the pre-deceleration section or the post-acceleration section becomes 0, and a path with short acceleration / deceleration on the taught path can be generated. Further, when the maximum allowable acceleration is the magnitude of acceleration in the pre-deceleration section, the transition section, and the post-acceleration section, the acceleration / deceleration time in each section is the shortest when performing uniform acceleration motion, and it is symmetric and takt. The orbit with the shortest time can be generated.

[0014]

Embodiments of the present invention will be described with reference to the drawings.

FIG. 2 shows an example of the construction of an automatic work machine to which the present invention is applied. The teaching operation means 1, the storage means 2, the operation procedure control means 5, the trajectory control means 6, the actuator control means 7, and the movable portion 8 are included. The movable portion 8 is composed of an actuator and a link connecting the actuator, and a tool or a work is attached to a link (hand) at the tip, and a working point is set on the tool or the work.

The operator teaches the trajectory of the work point and operates the automatic work machine through the teaching operation means 1. Teaching the trajectory of the work point is performed by teaching the route and speed. The position information of the starting point, the ending point, the passing point, the characteristic point, the representative point, and the like (these are called teaching points) of the taught route is stored in the position storage means 3 in the storage means 2 via the teaching operation means 1. To be done. For teaching the position of the teaching point, in addition to directly inputting the position information from the teaching operation means 1, the teaching operation means 1
Alternatively, the work point may be moved to a position to be taught by a manual operation via the, and the position may be stored. Information such as the passing order of teaching points, the shape of the route (straight line, arc, etc.), the parameters required to determine the shape of the route, the maximum speed on the teaching route, the magnitude of acceleration, and the conditions for the transition trajectory are used for teaching operations. It is stored in the operating procedure storage means 4 in the storage means 2 via the means 1. The position information is stored in the position storage means 3 and the operation procedure information is stored in the operation procedure storage means 4. However, these do not necessarily have to be stored separately, but are stored in the storage means 2 collectively. May be.

When the teaching of the working point trajectory is completed, an operation start command is issued to the operation procedure control means 5 via the teaching operation means 1. The operation procedure control means 5 includes the operation procedure storage means 4
The orbit control means 6 reads out and interprets the information stored in
An operation command including the start point and end point of the teaching route, the shape of the route, the maximum speed on the teaching route, the magnitude of acceleration, and conditions for the transition trajectory is issued. The trajectory control means 6 is
Interpolation processing for calculating an interpolation position, which is a position command value of a work point for each interpolation cycle, is performed based on the operation command, and the obtained interpolation position is converted into a drive amount of the actuator. The actuator control means 7 moves the actuator of the movable part 8 according to the drive amount of the actuator output from the trajectory control means 6, and controls the position of the working point attached to the tip of the movable part.

FIG. 3 shows an example of the structure of the trajectory control means 6 for realizing the embodiment of the present invention. Orbit control means 6
Is an interpolation coefficient calculation unit 21, a speed calculation unit 22, a movement distance calculation unit 23, a deviation vector calculation unit 24, a position calculation unit 25,
The coordinate conversion unit 26 is included. Interpolation coefficient calculator 21
Is interpolated from an operation command issued from the operation procedure control means 5, which includes start and end points of the teaching path, the shape of the path, the maximum speed on the teaching path, the magnitude of acceleration, conditions for the transition trajectory, and the like. A coefficient (interpolation coefficient) that is necessary to perform and can be calculated in advance is calculated. The speed calculation unit 22 outputs a speed according to the motion state of the current working point (acceleration, deceleration / constant speed, shift, operation end). Moving distance calculation unit 23
Then, the result of the speed calculation unit 22 is sequentially added to obtain the moving distance (distance). The deviation vector calculation unit 24 calculates the deviation vector corresponding to the vector from the start point of the teaching route to the interpolation position from the moving distance. The position calculation unit 25
The interpolation position is calculated by adding the deviation vector to the starting point of the taught route. The coordinate conversion means 26 converts the interpolation position calculated by the position calculation unit 25 into the drive amount of the actuator.

The feature of the apparatus according to the present invention is that the speed calculation unit 22 controls the acceleration in the rear acceleration section in order to independently set the acceleration / deceleration method in the pre-deceleration section, the transition section and the rear acceleration section for each section. Acceleration processing unit 30 for calculating the speed during movement, constant speed processing unit 31 for calculating the speed during constant velocity movement, deceleration processing unit 32 for calculating the speed during deceleration movement in the pre-deceleration section, transition section The shift processing unit 33 that calculates the speed in step S1 and the process switching unit 34 that switches these processes.

In FIG. 1, a point P ₀ to a point P ₁ are straight lines as a first teaching path, and a points P ₁ to P _{2 are a} second teaching path.
FIG. 8 is a diagram showing a motion pattern and a temporal change in speed in two teaching route directions, that is, a velocity pattern, when the present invention is applied when being taught to move in a straight line. The transition trajectory smoothly connecting the two teaching paths includes a pre-deceleration section S ₁ S ₂ that decelerates on the first teaching path, a transition section S ₂ S ₃ that moves away from the two teaching paths, and a _second section. It is composed of a post-acceleration section S ₃ S ₄ that accelerates on the teaching path. Here, S ₁ is a point at which deceleration is started on the first teaching path (deceleration starting point), S ₂ is a point at which deceleration is started from the first teaching path (transition starting point), and S ₃ is a second teaching path. Is a point (rush end point) where S. ₄ enters, and S ₄ is a point (acceleration end point) at which acceleration on the second teaching path ends. The contents of the arithmetic processing performed to realize the present invention will be described based on the operation of FIG.

First, the contents of the interpolation coefficient calculation processing performed by the interpolation coefficient calculation unit 21 to determine the timing of switching the processing will be described with reference to FIG. It should be noted that in the present embodiment, the transition trajectory is obtained by combining the motions in the two teaching route directions. That is, the position vector P _(i) , the velocity vector v _(i) , and the acceleration vector α _(i) of the work point at time i are obtained by the following equation. However, the present invention is not limited to the method of generating the transition trajectory.

[0022]

[Equation 1] P _(i) = P _s1 + Δq _{1 (i)} + Δq _{2 (i) (} Equation 1)

[0023]

[Number 2] _{v (i) = v 1 (} i) d 1 + v 2 (i) d 2 ... ( number 2)

[0024]

[Formula 3] α _(i) = α _{1 (i)} d ₁ + α _{2 (i)} d ₂ (Formula 3) where P _s1 is the position vector of the start point of the first teaching path.
(Position vector of P ₀ ), Δq _{1 (i)} is the deviation vector for the first trajectory moving from P ₀ to P ₁ , and Δq _{2 (i)} is P ₁
Deviation vector for the second orbit moving from P to P ₂ ,
v _{1 (i)} is the speed in the first teaching path direction, d ₁ is the direction vector in the first teaching path direction, v _{2 (i)} is the speed in the second teaching path direction, d ₂ is the second Direction vector of the teaching route direction of
_{1 (i)} is the magnitude of acceleration in the first teaching path direction, and α _{2 (i)} is the magnitude of acceleration in the second teaching path direction.

In this embodiment, as a condition for the transition trajectory, the distance l _dc1 from the connection point P ₁ of the two teaching paths to the transition start point S ₂ and the distance l from P ₁ to the transition end point S _3
The case where _ac2 is given will be described. However, the present invention is not limited by the method of setting the conditions for this transition trajectory.

Further, in this embodiment, the maximum speed on the first teaching path is v _m1 and the maximum speed on the second teaching path is v _m1 .
_m2,予減speed section S ₁ S _2, transition section S ₂ S _3, the magnitude of the acceleration at the rear acceleration section S ₃ S ₄ and alpha _max.

FIG. 4 is a flowchart of the interpolation coefficient calculation process. When an operation command is issued from the operation procedure control means 5, the interpolation coefficient calculation state determination flag smooth_mode is determined, and if the interpolation coefficient is not calculated (smooth_mode = 0), P is set.
The interpolation coefficient of the first trajectory moving from ₀ to P ₁ is calculated (block 100), and when the calculation of the interpolation coefficient of the first trajectory is completed (smooth_mode = 1), it moves from P ₁ to P ₂ . The interpolation coefficient of the second trajectory is calculated (block 2
00).

Calculation of interpolation coefficient of the first trajectory (block 10
Details of the processing 0) will be described.

In block 101, from the position vector P ₀ of the start point and the position vector P _{1 of the} end point of the first teaching path,
The distance (interpolation distance) l ₁ from the start point to the end point is calculated.

[0030]

[Equation 4] ΔP ₁ = P ₁ −P ₀ (Equation 4)

[0031]

[Formula 5] l ₁ = | ΔP ₁ | (Formula 5) In the block 102, the direction vector d ₁ in the first teaching path direction is calculated.

[0032]

[Equation 6] d ₁ = ΔP ₁ / l ₁ (Equation 6) In the block 103, the distance required for acceleration to the maximum speed v _m1 on the first teaching path at the specified acceleration α _max (acceleration required distance). l _a1, calculates the maximum velocity v distance required to decelerate from the _m1 (deceleration required distance) l _d1.

The method of calculating l _a1 and l _d1 differs depending on the speed control method (speed pattern). Although a uniform acceleration motion is assumed here, the present invention is not limited thereto. In this case, l _a1 and l _d1 are calculated by the following equation.

[0034]

(Equation 7)

[0035]

(Equation 8)

When the calculation of the interpolation coefficient of the first trajectory is completed by the above processing, the completion of the interpolation coefficient calculation of the first trajectory is set in the interpolation coefficient calculation state determination flag (smooth_mode
= 1, block 104).

Next, the details of the processing of the interpolation coefficient calculation (block 200) of the second trajectory will be described.

In block 201, from the position vector P _{1 at} the start point and the position vector P _{2 at the} end point of the second teaching path,
The distance (interpolation distance) l ₂ from the start point to the end point is calculated.

[0039]

[Formula 9] ΔP ₂ = P ₂ −P ₁ (Formula 9)

[0040]

[Equation 10] l ₂ = | ΔP ₂ | (Equation 10) In the block 202, the direction vector d ₁ of the first teaching path direction is calculated.

[0041]

D ₂ = ΔP ₂ / l ₂ ( _Equation 11) In block 203, the acceleration α _{dc1 in} the first teaching path direction and the acceleration α _ac2 in the second teaching path direction in the transition section are calculated.

Magnitude of acceleration vector in transition section | α
_(i) | is calculated from the equation 3 as follows.

[0043]

(Equation 12)

Here, θ is an angle formed by the first teaching path direction and the second teaching path direction.

Since constant acceleration motion is assumed in this embodiment, α _{1 (i)} and α _{2 (i)} are as in the following equation.

[0046]

[ _Formula 13] α _{1 (i)} = −α _dc1 ( _Formula 13)

[0047]

[ _{Mathematical formula} -see _{original document} ] α _{2 (i)} = α _ac2 ( _Equation 14) When the time from when the work point reaches the transition start point S ₂ to the transition end point S ₃ is t ₂₃ , the connection point of the two teaching paths To the transition start point, l _dc1 , the distance to the transition end point, l _ac2 , the magnitude of acceleration in each teaching path direction α _dc1 , α _ac2
The following relation holds for.

[0048]

(Equation 15)

[0049]

[Equation 16]

Therefore,

[0051]

[Equation 17]

[0052]

( _Equation 18) α _ac2 = nα _dc1 ( _Equation 18) Substituting this into Equation 12,

[0053]

[Formula 19]

Therefore, from | α _(i) | = α _max , α
_dc1 is obtained by the following equation, and α _ac2 is obtained by the equation 18 from the obtained α _dc1 .

[0055]

(Equation 20)

In block 204, the speed (transfer start speed) v _dc1 at the transfer start point S ₂ is calculated. In the transition section, the speed is changed from v _dc1 to 0 at a distance l _dc1 toward the first teaching route direction.
The deceleration rate is α _dc1 . Therefore, v
_dc1 is _calculated by the following equation.

[0057]

[Equation 21]

In block 205, the maximum speed v _{m1 is} decelerated to the speed v _dc1 at the deceleration α _max , and then the distance required for deceleration (deceleration required distance) l _d1 is calculated at the deceleration α _dc1 .

[0059]

[Equation 22]

In block 206, the speed at the transition end point S ₃ (transition end speed) v _ac2 is calculated. In the transition section, the speed is changed from 0 to v _ac2 at the distance l _ac2 in the second teaching path direction.
Acceleration up to α _ac2 . Therefore, v
_ac2 is _calculated by the following equation.

[0061]

(Equation 23)

In block 207, the speed v is _obtained with the acceleration α _ac2.
After accelerating to _ac2 , the distance required to accelerate to the maximum speed v _m2 on the second teaching path with acceleration α _max (acceleration required distance) l _a2 and the acceleration α _max to decelerate from v _m2. The distance (required deceleration distance) l _d2 is calculated. l _a2 and l _d2 are obtained by the following equation.

[0063]

[Equation 24]

[0064]

(Equation 25)

When the calculation of the interpolation coefficient of the second trajectory is completed by the above processing, the completion of the calculation of the interpolation coefficient of the second trajectory is set in the interpolation coefficient calculation state determination flag (smooth_mode
= 2, block 208).

Using the interpolation coefficient calculated as described above, interpolation execution calculation processing is performed by the speed calculation section 22, the moving distance calculation section 23, the deviation vector calculation section 24, the position calculation section 25, and the coordinate conversion section 26. Be seen. This process will be described with reference to FIGS. 5 is a flowchart of the interpolation execution calculation process, FIG. 6 is a speed pattern used to realize the present embodiment, and FIG. 7 is a flowchart of the speed calculation process performed by the speed calculation unit 22 based on the speed pattern of FIG. .

First, in block 301, initialization processing is performed. In the initialization processing, the motion state determination flag ac is set to the acceleration state (ac = 1).

From block 302 to block 307, until the interpolation execution processing of the first trajectory is completed, that is,
This is repeated until the movement distance m ₁ of the first trajectory becomes the interpolation distance l ₁ .

In block 302, the speed calculator 22 calculates the speed v _{1 (i)} in the _first teaching path direction and the speed v _{2 (i)} in the second teaching path direction. This speed calculation process will be described in detail with reference to FIGS. 6 and 7.

The motion state of the working point is the acceleration state (ac = 1)
In the case of, the process switching unit 34 selects the acceleration processing unit 30, and the speed v in the first teaching route direction in the post-acceleration section
Acceleration with α _max of _{1 (i)} is performed (block 402).
When v _{1 (i)} ≧ v _m1 , the motion state determination flag is set to the deceleration / constant speed state (ac = 2, block 401).

In the deceleration / constant speed state (ac = 2), the deceleration is required until the working point reaches the deceleration start point S ₁ , that is, the difference between the interpolation distance l ₁ of the first trajectory and the movement distance m _1. Until the distance l _d1 is reached (l ₁ −m ₁ > l _d1 ), the process switching unit 34
The constant-speed processing unit 31 is selected by this, and constant-speed processing is performed (block 405). When the work point reaches the deceleration start point S ₁ (l ₁ −m ₁ ≦ l _d1 ), the deceleration processing unit 32 is selected by the process switching unit 34, and the speed v in the first teaching route direction in the pre-deceleration section v Deceleration is performed at α _max of _{1 (i)} (block 403). When the work point reaches the transition start point S ₂ , (l ₁ −
m ₁ ≦ l _dc1 ), and sets the motion state determination flag to the transition state (ac = 3, block 404). Transition state (ac =
In 3), the transition switching unit 33 is selected by the process switching unit 34, and α _dc1 of the speed v _{1 (i)} in the first teaching path direction is set so that the magnitude of the acceleration in the transition section becomes α _max. Deceleration of
Acceleration with the speed v _{2 (i)} in the second teaching path direction at α _ac2 is performed (block 406). Working point when it reaches the end of the migration point _{S 3 (l1-m 1 ≦} 0), it sets the motion condition determination flag to the end state (ac = 4, block 407).

In block 303, the moving distance calculation unit 23
The moving distance m _{1 (i) of} the first trajectory and the moving distance m _{2 (i)} of the second trajectory are calculated. In the moving distance calculation unit 23, time i
Distances m _{1 (i-1)} and m of each trajectory to -1
_{2 (i-1)} , the speeds v _{1 (i)} and v _{2 (i)} at time i calculated in block 302 are multiplied by the interpolation cycle t _s to obtain the increment at time i, and the moving distance m _{1 (i)} and m _{2 (i)} are calculated by the following equation.

[0073]

[Number 26] _{m 1 (i) = m 1} (i-1) + v 1 (i) t s ... ( number 26)

[0074]

M _{2 (i)} = m _{2 (i−1)} + v _{2 (i)} t _s (Equation 27) In block 304, time i is calculated from the start point of the first teaching route in deviation vector calculation unit 24. Vector of the first teaching path direction when the vector up to the interpolation position which is the position command value of the working point is distributed to the first teaching path direction and the second teaching path direction (the deviation vector of the first trajectory) ) Δq _{1 (i)} , a vector (second deviation vector of the second trajectory) Δq _{2 (i)} in the second teaching path direction.

[0075]

Δq _{1 (i)} = m _{1 (i)} d _{1 (} Equation 28)

[0076]

In Equation 29] _{Δq 2 (i) = m 2} (i) d 2 ... ( number 29) block 305 is performed by the number 1 the calculation of the position vector P of the interpolation point in time i at the position calculating section 25 _(i) .

In block 306, the drive amount of the actuator for moving the work point to the position vector P _(i) of the interpolation point at time i is calculated. At block 307, output processing to the actuator control means 7 is performed.

At block 308, a switching process is performed to perform the next interpolation process. Here, the interpolation coefficient of the second trajectory is updated as the interpolation coefficient of the first trajectory, and the interpolation coefficient calculated by the interpolation coefficient calculator 21 by the operation command newly issued from the operation procedure control unit 5 is set to the second coefficient. It is updated as the interpolation coefficient of the trajectory of.

By executing the interpolation coefficient calculation processing and the interpolation execution calculation processing as described above, the position (distance from P ₁ ) l of the deceleration start point S ₁ obtained by the interpolation coefficient calculation unit 21.
_d1, (the distance from P ₁₎ position of the transition starting point S ₂ l _dc1, velocity v _dc1, (Distance from P ₁₎ position of the transition end point S ₃ l
_{Based on ac2} , speed v _ac2 , and speed v _m2 at the acceleration end point S ₄ , the processing of the speed calculation unit 22 is switched to change the transition trajectory to the pre-deceleration section S ₁ S ₂ , the transition section S ₂ S ₃ , and the rear. Acceleration section S ₃ S
_It consists of ₄ units, and it is possible to perform acceleration / deceleration for each section.

At this time, as can be seen from the equations 21 to 24, even if the values of the maximum speeds v _m1 , v _m2 and the magnitude α _max of the acceleration on the teaching path are changed, the transition start point S ₂ Position l
_dc1 , the length of the pre-deceleration section (l _d1 −l _dc1 ), the length of the rear acceleration section (l _a2 −) without changing the position l _ac2 of the transition end point S _3.
l _ac2 ), transition start speed v _dc1 , transition end speed v _ac2 v
The effect of changing _m1 , v _m2 , and α _max is absorbed. As a result, a constant route that does not depend on the maximum speed on the taught route and the magnitude of acceleration can be obtained.

Furthermore, the magnitude of the acceleration in the transition section is
By setting the same magnitude of acceleration in the pre-deceleration section and the post-acceleration section, it is possible to realize stable control in which the magnitude of acceleration is constant over the entire transition trajectory.

By the way, as a condition for the transition trajectory, the acceleration magnitude α _{dc1 in} the first teaching route direction and the acceleration magnitude α _ac2 in the second teaching route direction in the transition section are set to the same value α _c . . Further, the distance l _dc1 from the connection point P ₁ of the two teaching paths to the transition start point S ₂ and the distance l _ac2 from P ₁ to the transition end point S ₃ are set to the same value l _c .
At this time, the speed v _dc1 at the transition start point S ₂ and the speed v _ac2 at the transition end point are obtained by the _equations 21 and 23, and v _dc1
And v _ac2 have the same value. Let that value be v _c . If the time from the transition start point S ₂ to the transition end point S ₃ is t ₂₃ , v _c is given by the following equation.

[0083]

V _c = α _c t ₂₃ (Equation 30) At this time, the speed v in the first teaching route direction in the transition section
_{1 (t)} and the speed v _{2 (t)} in the second teaching path direction are as follows.

[0084]

V _{1 (t)} = v _c −α _c (t−T ₂ ) = α _c {t ₂₃ − (t−T ₂ )} (Equation 31)

[0085]

Equation 32] _{v 2 (t) = α c} (t-T 2) ... ( number 32) where, T ₂ is the time it reaches the transfer start point S ₂ working point. From this equation, it is clear that deceleration in the first teaching path direction and acceleration in the second teaching path direction in the transition section are performed symmetrically with respect to the bisector of the angle formed by the two teaching paths. Is. Therefore, in this case, in the transition section, a parabolic trajectory that is symmetrical with respect to the bisector of the angle formed by the two taught routes can be obtained, and the route can be easily predicted.

As a condition for the transition trajectory, the magnitude of acceleration α _{dc1 in} the first teaching route direction and the magnitude of acceleration α _ac2 in the second teaching route direction are set to the same value α _c ,
Speed v _dc1 at transition start point S ₂ , speed v at transition end point
_{Set ac2} to the same value v _c . In this case, the distances l _dc1 , P ₁ from the connection point P ₁ of the two teaching paths to the transition start point S ₂
The distance l _ac2 from the transition end point S ₃ to the transition end point S ₃ is _calculated by the following equation and has the same value. Let that value be l _c .

[0087]

[Expression 33]

[0088]

(Equation 34)

The speed v in the first teaching path direction at this time
_{1 (t)} , the speed in the second teaching path direction v _{2 (t)} is
Equation 32 is obtained, and similarly to the above case, the deceleration in the first teaching route direction and the acceleration in the second teaching route direction in the transition section are
It is performed symmetrically with respect to the bisector of the angle formed by the two teaching paths, and in the transition section, a parabolic trajectory that is symmetrical with respect to the bisector of the angle formed by the two teaching paths is obtained. Can be easily predicted.

Further, the speed at the transition start point and the speed at the transition end point are defined as the maximum speed on the first teaching path, or
If it is set to the smaller one of the maximum speeds on the second teaching path, S ₁ and S ₂ will be the same, or S ₃ and S ₄ will be the same. That is, the length of either the pre-deceleration section or the post-acceleration section becomes 0, and a transition trajectory in which the acceleration / deceleration on the teaching route is minimized is generated. Further, assuming that the magnitude of acceleration in the pre-deceleration section, the transition section, and the post-acceleration section is the maximum allowable acceleration, which is the maximum value of acceleration that can realize stable control, when performing uniform acceleration motion, The acceleration / deceleration time becomes the shortest, and the route with the shortest tact time can be generated.

FIG. 8 shows another structure of the trajectory control means 6 for realizing the embodiment of the present invention. The difference between the case shown in FIG. 8 and the case shown in FIG. 3 is that the speed calculation unit 22, the movement distance calculation unit 23, and the deviation vector calculation unit 24 are separately prepared for each of the first trajectory and the second trajectory. . Further, at this time, the motion state of the work point is divided into transition acceleration, post acceleration, pre-deceleration / constant speed, transition deceleration / constant speed, and operation end, and the speed calculation unit 22 accelerates in the teaching path direction in the transition section. A transition acceleration processing unit 40 that calculates the speed when moving, a post-acceleration processing unit 41 that calculates the speed when accelerating on the teaching route, a constant speed that calculates the speed when moving at a constant speed on the teaching route. Processing unit 42,
A pre-deceleration processing unit 43 that calculates the speed when decelerating on the teaching path, a transition deceleration processing unit 44 that calculates the speed when decelerating in the teaching path direction in the transition section, and a process switching unit that switches these processes. It consists of 45.

When the configuration of FIG. 8 is adopted, the interpolation execution calculation process is also different from that shown in FIGS. The interpolation execution calculation in this case will be described with reference to FIGS. 9 to 11. 9 is a flowchart of interpolation execution calculation processing, FIG. 10 is a speed pattern used in this example, and FIG. 11 is a flowchart of processing in a speed calculation unit based on the speed pattern shown in FIG.

First, in block 501, initialization processing is performed.

Blocks 502 to 509 are repeated until the moving distance m _{1 (i)} of the first trajectory reaches the interpolation distance l ₁ .

In block 502, the speed calculator 22-1
The calculation of the speed v _{1 (i)} in the first teaching path direction is performed. Details of the speed calculation process will be described with reference to FIGS. 10 and 11. When the work point reaches the transition start point of the previous trajectory, the motion state determination flag is set to the transition acceleration state (ac = 1, block 601). Transition acceleration state (ac
= 1), the process switching unit 45-1 selects the transition acceleration processing unit 40-1, and the speed v _{1 (i)} of the first trajectory is α _ac1
Is accelerated (block 603). When v _{1 (i)} ≧ v _ac1 , the motion state determination flag is set to the post-acceleration state (ac = 2, block 602). Post-acceleration state (ac =
In 2), the process switching unit 45-1 causes the post-acceleration processing unit 41.
-1 is selected and v _{1 (i)} is accelerated by α _max (block 605). When v _{1 (i)} ≧ v _m1 , the motion state determination flag is set to the pre-deceleration / constant speed state (ac = 3, block 6
04). In the pre-deceleration / constant speed state (ac = 3), until the work point reaches the deceleration start point S ₁ (l ₁ −m _{1 (i)} > l _d1 ),
The constant speed processing unit 42-1 is selected by the processing switching unit 45-1, and constant speed processing is performed (block 608). When the work point reaches the deceleration start point S ₁ (l ₁ −m _{1 (i)} ≦ l _d1 ), the process switching unit 45-1 selects the pre-deceleration processing unit 43-1 and v _{1 (i)} is α. _{The vehicle} is decelerated by _max (block 606).
When the work point reaches the transition start point S ₂ , (l ₁ −m _{1 (i)} ≦
l _dc1 ), the motion state determination flag is set to the transition deceleration / constant speed state (ac = 4, block 607), and the motion state determination flag of the second trajectory is set to the transition acceleration state. Transition deceleration /
In the constant speed state (ac = 4), the process switching unit 45-1 selects the transition deceleration processing unit 44-1 and v _{1 (i)} is decelerated by α _dc1 (block 609). The work point is the transition end point S
_{When 3} is reached (l ₁ −m _{1 (i)} ≦ 0), the motion state determination flag is set to the end state (ac = 5, block 61).
0).

In block 503, the moving distance calculation unit 23
The movement distance m _{1 (i)} of the first trajectory at −1 is calculated. In the moving distance calculation unit 23-1, the moving distance m up to time i-1
_{1 (i-1)} , the speed v _{1 (i)} at time i calculated in block 502 is multiplied by the interpolation cycle t _s to obtain the increment at time i,
The moving distance m _{1 (i)} at time i is calculated by the equation 26.

In block 504, the number 2 of the deviation vector Δq _{1 (i)} of the first trajectory in the deviation vector calculator 24-1 is calculated.
Calculation is performed by 8.

In the processing from block 505 to block 507, the speed calculation processing, the movement distance calculation processing, and the deviation vector calculation performed in block 502 to block 504 are performed on the second trajectory, and the speed with respect to the second trajectory is calculated. v _{2 (i)} , moving distance m _{2 (i)} , and deviation vector Δq _{2 (i)} are obtained.

A block 508 performs the calculation of the interpolation point P _(i) at the time i in the position calculation section 25 by the formula 1.

In block 509, the interpolation point P at time i
Calculate the drive amount of the actuator to move the work point to _(i) . At block 510, output processing to the actuator control means 7 is performed.

When the movement distance m _{1 (i)} with respect to the first trajectory reaches the interpolation distance l ₁ of the first trajectory, that is, the working point reaches the transition end point S ₃ , the interpolation with respect to the first trajectory is completed. To do. In block 511, preparation (switching process) for executing the next interpolation process is performed. Here, the interpolation coefficient of the second trajectory is updated as the interpolation coefficient of the first trajectory, and the interpolation coefficient calculated by the interpolation coefficient calculator according to the operation command newly issued from the operation procedure control means 5 is changed to the second interpolation coefficient. It is updated as the interpolation coefficient of the trajectory.

By performing the interpolation execution calculation processing as described above, the speed v _ac1 , the maximum speed v _m1 , and the deceleration start point S ₁ of the transition end point of the previous trajectory obtained by the interpolation coefficient calculation unit 21 are also obtained. Position (distance from P ₁ ) l _d1 , transition start point S ₂
Based on the position (distance from P ₁ ) l _dc1 and speed v _dc1 ,
The processing of the speed calculation unit 22 is switched, the transition trajectory is composed of a pre-deceleration section, a transition section, and a post-acceleration section, and acceleration / deceleration can be performed for each section.

In the above embodiments, the connection between the straight route and the straight route has been described, but the present invention can be applied to routes other than the straight route. FIG. 12 is an operation diagram in the case where this processing is applied to the connection between the straight line path and the circular arc path. When realizing FIG. 12, in the interpolation coefficient calculation process shown in FIG. 4, calculation of an interpolation distance (blocks 101 and 201) and calculation of an interpolation direction vector (block 1)
02, block 202) for circular interpolation, and the calculation of the deviation vector (block 304) in the interpolation execution calculation process shown in FIG. 5 may be changed for circular interpolation.

[0104]

According to the present invention, a smooth transition trajectory connecting two continuous teaching paths is moved away from the pre-deceleration section for decelerating on the first teaching path and the two teaching paths. It consists of a transition section and a post-acceleration section that accelerates on the second teaching path.

By adjusting the acceleration / deceleration method in each section, the influence of changes in the maximum speed on the teaching path and the magnitude of acceleration is adjusted to adjust the length of the pre-deceleration section, the length of the rear acceleration section, and the start of transition. By absorbing the speed and the transition end speed, it is possible to generate a fixed path.

Further, by making the magnitudes of acceleration in the pre-deceleration section, the transition section and the rear acceleration section the same, it is possible to realize stable control in which the magnitude of the acceleration is constant over the entire transition trajectory.

Furthermore, when the acceleration vector in the transition section is decomposed into the first teaching route direction and the second teaching route direction, the magnitudes of the acceleration vectors in the respective teaching route directions are made the same, and the two teaching routes are connected. Two teachings are made by making the distance from the transition point to the transition start point and the distance from the connection point to the transition end point the same, or by making the speed at the transition start point and the speed at the transition end point the same. A trajectory that is symmetrical with respect to the bisector of the angle formed by the path can be generated, and the path can be easily predicted. Further, by making the generated paths symmetrical, even when the same teaching path moves in the opposite direction, it is possible to move on the same path as when moving in the positive direction.

As a result, the same route can be generated in the low-speed test operation and the high-speed continuous operation, and appropriate teaching can be performed without considering the change in the route due to the change in speed.

[Brief description of drawings]

FIG. 1 is an explanatory diagram showing a motion path and a velocity pattern when the present invention is applied to generation of a trajectory smoothly connecting two continuous straight line paths.

FIG. 2 is a block diagram of an automatic work machine targeted by the present invention.

FIG. 3 is a block diagram showing an example of the configuration of a trajectory control means for realizing the present invention.

FIG. 4 is a flowchart of interpolation coefficient calculation.

5 is a flowchart of an interpolation execution calculation when the trajectory control means has the configuration shown in FIG.

6 is an explanatory diagram of a speed pattern when the trajectory control means has the configuration shown in FIG.

FIG. 7 is a flowchart of processing of a speed calculation unit based on the speed pattern shown in FIG.

FIG. 8 is a block diagram showing another example of the configuration of the trajectory control means for realizing the present invention.

9 is a flowchart of an interpolation execution calculation when the trajectory control means has the configuration shown in FIG.

10 is an explanatory diagram showing a speed pattern when the trajectory control means has the configuration shown in FIG.

FIG. 11 is a flowchart of processing of a speed calculation unit based on the speed pattern shown in FIG.

FIG. 12 is an explanatory diagram showing an operation path when the present invention is applied to the generation of a trajectory smoothly connecting a continuous straight line path and a circular arc path.

[Explanation of symbols]

1 ... teaching operation means, 2 ... storage means, 3 ... position storage means,
4 ... Operation procedure storage means, 5 ... Operation procedure control means, 6 ... Orbit control means, 7 ... Actuator control means, 8 ... Movable part.

Claims (1)

[Claims] 1. A trajectory control method for moving a tool attached to the vicinity of the tip of a movable part of an automatic work machine or a work point set on a work according to a taught path and a taught speed. A transition trajectory that smoothly connects between two continuous teaching paths having the same end point of the teaching path and the starting point of the second teaching path in the vicinity of the connection point of the two teaching paths is generated. In the method for controlling the movement of the work point according to the above, a pre-deceleration section having a length of 0 or more for decelerating the generated transition trajectory on the first teaching path, and a length of 0 or more for moving away from the two teaching paths. A trajectory control method comprising a transition section and a post-acceleration section having a length of 0 or more for accelerating on a second teaching path, and setting an acceleration / deceleration method independently for each section.