JPH06187026A - Locus controller - Google Patents

Abstract

PURPOSE:To provide a locus controller in which a locus can be controlled so that the same locus as the locus of an inputted circular-arc can be drawn at the connecting part of the circular-arcs. CONSTITUTION:A CPU calculates the directional components of the end point of a preceding section formed by the first teaching point and the second teaching point following the first teaching point, and the directional components of the start point of a succeeding section formed by the second teaching point and the third teaching point following the second teaching point (S6). Then, the difference of the tangent directions of the preceding section and the succeeding section at the second teaching point being the connecting part of the preceding section and the succeeding section is calculated from the calculated directional components of the preceding section and succeeding section (S7). Next, the calculated difference of the tangent directions is compared with a preliminarily set value, and whether or not an object to be carried can be carried on the second teaching point without reducing a speed is judged. Then, when the difference of the tangent directions of the preceding section and the succeeding section is small, and the validity of the carriage of the object to be carried on the second teaching point without reducing the speed is judged, a moving locus is controlled so that the object to be carried can be carried on the second teaching point at the speed equal to that in the preceding section (S8).

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a trajectory control device for controlling the trajectory of an object such as a hand of a robot.

[0002]

2. Description of the Related Art As a trajectory control technique of this type, the C described in the first technical meeting of the Robotics Society of Japan, Proc.
P (Continuous Pass) control is known. In this technique, as illustrated in FIG. 12A, first, each point along the trajectory along which the object is to be moved is set to P
_i-1 , P _i , P _{i + 1} ... And the mode of the section between these points is designated. In the example shown in FIG. 12A, the point P
_The straight line mode is designated for both the section between _i-1 and the point P _{i and} the section between the point P _i and the point P _{i + 1} . Based on this, first, the velocity pattern shown in FIG. 11A in the direction from the point P _i-1 to the point P _i (indicated as P _i-1 → P _i direction; the same applies to others) is determined. . That is, the acceleration is first accelerated by the acceleration α _O in the P _i−1 → P _i direction, and when the constant velocity V _O is reached, the vehicle is moved at the constant velocity V _O , and the acceleration of −α _O (the deceleration is α _O
When decelerating and is decelerated to zero speed at become) (determined this point a timing t 'of zero speed) just point P _i reaches such a point A _O (timing t), this point A _O A speed pattern for starting deceleration from (timing t) is created. Here, the constant velocity V _O is
It is a predetermined maximum velocity of the object whose trajectory is to be controlled, and the acceleration α _O is the maximum acceleration of the controlled object.

When the timing t is determined, the velocity component V _{i + 1 in the} P _i → P _{i + 1} direction will now be described with reference to FIG.
From the timing t as shown in (b), the speed is increased by the acceleration α _O , and a speed pattern is calculated that is a steady speed after reaching the speed V _O. When these two speed patterns are determined, vector addition of the respective speed components is performed to calculate the actual moving direction and the speed component. Note that the vector-added speed never exceeds the maximum speed V _O. As is clear from FIGS. 11A and 11B, P
At the same time as the velocity component V _{i in the} direction of _i-1 → P _i decreases, the point P
_The velocity component V _{i + 1} from _i to the point P _{i + 1} increases, and at timings t and t ′, one of the velocity components becomes the maximum velocity V _O and the other velocity component becomes zero.

Therefore, when the object is moved in accordance with the vector-added direction and speed, the object moves at a constant speed V _o in the direction P _i-1 → P _i before the timing t. From timing t to t ′, the velocity component V _{i in} the P _i−1 → P _i direction decreases and the velocity component in the P _i → P _{i + 1} direction increases. Here, since the velocity component of P _i-1 → P _i becomes zero at the timing t ′, the object moves to the point P at the timing t ′.
That is, it is on a straight line connecting _i and the point P _{i + 1} . When the moving direction and the velocity of the object are determined in this way, the object moves from the point P _i-1 to the point P as shown in the locus of FIG.
_i → along a straight line connecting points P _{i + 1} ··· and points P _i-1
With respect to the point P _i, which is a connecting portion between the section between the point P _i and the point P _i and the section between the point P _i and the point P _{i + 1} , the point P _i moves on the curve M _O while smoothly inward.

[0005]

As described above, when CP control is used for the portion connecting the straight line section and the straight line section, a smooth locus can be drawn at the connecting section. However, conventionally, when connecting arcs to arcs, straight lines to arcs, or arcs to straight lines, there is a problem that a desired trajectory cannot be drawn if the CP control is performed. For example, FIG.
As shown in, the locus where the object is to be moved has points P _i−1 , P
_i and P _{i + 1} , and the section between the point P _i-1 and the point P _i and the section between the point P _i and the point P _{i + 1} are both arcs. When the above CP control is performed on such a locus, the velocity component in the arc direction of the points P _i-1 and P _i is reduced from the point A _O , and the arc in the directions of the points P _i and P _{i + 1} is reduced. The addition of velocity components in the directions is started. Therefore, even when it is desired to draw a locus passing through the point P _i , the locus passes through the curve M _O and cannot pass through the point P _i, and the locus of the arc is distorted.

The present invention has been made in order to solve the above-mentioned problems, and its object is to perform trajectory control capable of controlling so as to draw the same trajectory as the trajectory of an arc input at a connecting portion with the arc. To provide a device.

[0007]

The structure of the invention for solving the above-mentioned problems is a locus control device for creating a locus of movement of an object to be conveyed based on a plurality of input teaching points. Direction component on the side of the second teaching point in the previous section formed by a point and a second teaching point following the first teaching point, and a third teaching following the second teaching point and the second teaching point. A directional component calculating means for calculating a directional component on the second teaching point side of a rear section formed by a point and a difference in tangential direction between the front section and the rear section at the second teaching point, A tangential direction difference calculation means for calculating from the direction component of the front section and the direction component of the rear section calculated by the direction component calculation means, and a tangential direction difference calculated by the tangential direction difference calculation means with a preset value. Comparing means for comparing and the difference in the tangential direction compared by the comparing means When: Because the value set, characterized by comprising a movement locus control means for controlling the movement trajectory to send the transported object on the second teaching point equal to said front section speed.

[0008]

According to the above-mentioned means, the directional component calculating means has the second teaching point side of the previous section formed by the first teaching point and the second teaching point following the first teaching point (on the second teaching point side). The direction component of the end point side) and the direction component of the second teaching point side of the rear section (start point side of the rear section) formed by the second teaching point and the third teaching point following the second teaching point are calculated. To do. From the calculated direction component of the front section and the direction component of the rear section, the tangential difference calculation means causes the tangential direction of the front section and the rear section at the second teaching point, which is a connecting portion between the front section and the rear section. Calculate the difference between. Then, the comparison means compares the calculated difference in the tangential direction with a preset value to determine whether or not the conveyed object can be sent on the second teaching point without decelerating. When it is determined by the comparison means that the difference in the tangential direction between the front section and the rear section is small and the conveyed object can be sent on the second teaching point without deceleration, the movement locus control means performs the second teaching. The movement locus is controlled so as to send the transported object on the point at the same speed as the preceding section.

[0009]

DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the trajectory control device of the present invention will be described below with reference to the drawings. First, a robot 10 having six joints controlled by the trajectory control device of this embodiment will be described with reference to FIG. The robot 10 includes a column 14 rotatably attached to a pedestal 12 fixed to a base 13, a first arm 15, and a second arm 16.
And a third arm 17 and a sealer nozzle 19 for sealing the coating agent. The first joint a, the second joint b, the third joint c, the fourth joint d, the fifth joint e, and the sixth joint f are configured to freely send the nozzle 19 with 6 degrees of freedom.

Next, the configuration of the trajectory control device 50 of this embodiment will be described with reference to FIG. The robot 10 shown in FIG.
The CPU 11 that performs the calculation for the position control of the
(Continuous Pass) Control information for controlling and ROM 20 and RAM 30 storing control information for performing constant velocity passage control of the present embodiment, operating box 27 and operation panel 26 for inputting control commands, and An external storage device 29 that holds control information created by the CPU 11 and the servo control unit 40 are connected. The servo control unit 40 is the first of the robot 10.
The servo motor M1 for driving the joint a to the servo motor M6 for driving the sixth joint f are connected, and the feedback signals from the encoders E1 to E6 attached to the servo motors M1 to M6 are fed back. Is configured. In the locus control device 50, the CPU 11 calculates a locus of movement based on the input teaching point and sends an output to the servo control unit 40, and the servo control unit 40 controls the servo motors M1 to M6 to control the robot 10. The nozzle 19 that is driven and performs sealing is configured to draw a desired movement trajectory.

First, the trajectory control device 50 of this embodiment.
The outline of the constant velocity passage control of will be described. As shown in FIG. 3A, the locus control device 50 of the present embodiment has a teaching point P.
The section i between _i-1 and the teaching point P _i is an arc, and the next teaching point P _i
If the section _{i + 1} between the point and the teaching point P _{i + 1} is an arc, section i
And the teaching point P _i to the connection portion of the interval i + 1, is smaller than the angle the angle of the tangential component is set between the arc of the arc of the start point of the end point and the section i + 1 interval i in the connection portion, front The CP control for synthesizing the directional component is not performed between the section i of and the subsequent section i + 1. Conventional case without CP control point P from _i-1 when towards the point P _i, the velocity components to zero at point P _i performs deceleration from synthetic starting point A _O, and again the point P from the point P _i It was accelerating towards _{i + 1} . At a constant speed passage control of this embodiment, however, the points P _i-1 when going to the point P _i from is passed through the point P _i at a constant speed without deceleration of the following: P _{i + 1} Control towards. The trajectory control device 50 of the present embodiment performs the constant velocity passage control not only in the case of the circular arcs of FIG. 3A, but also in the section i as shown in FIG. This is also performed when the section i + 1 is a straight line, or when the section i is a straight line and the next section i + 1 is a circular arc as shown in FIG. This constant velocity passage control will be described in more detail later.

Next, the outline of the input of teaching points and the like to the trajectory control device 50 according to the embodiment of the present invention and the operation according to the input will be described with reference to the flowchart shown in FIG. Here, the operator shows the sealing portion shown in FIG. 4 (here, the sealing portion is an arc connecting the points P _i-1 and P _i , a straight line from the point P _i to the point P _{i + 1} , and the point P _i. The nozzle 19 of the sealer shall be sent along a straight line from ₊₁ to the point P _{i + 2} ).

First, the operator operates a plurality of teaching points (point P _i-1 , point P _i , point P _{i + 1} , point P _{i + 2.}
By inputting (), the movement locus along the sealing position is instructed and the mode between each teaching point is specified. Here, the arc mode is in the section _i between the points P _i-1 and P _i , the linear mode is in the section i + 1 between the points P _i and P _{i + 1} , and the points P _{i + 1} and P _{i are} Section i between ₊₂
The linear mode is specified to +2. Further, the operator sets a designated speed in each section. Here, it is assumed that the speeds equal to all the sections are designated (step 1). In response to this, the CPU 11 of the trajectory control device 50
Performs a decoding process on the input information (step 2) and stores it in the RAM 30. Unless an operation command is input (determination step 3 is N
o) Perform non-operation command processing (step 4), and wait for input of operation command.

When the locus control device 50 is instructed to start the operation (Yes in judgment step 3), first, the CPU 11
The decoded input information in the RAM 30 is called and interpreted, and the data required for interpolation is created (step 5). Here, the required movement amount (moving distance) Li or the like between the teaching points is calculated, and a buffer to be described later is created.

Next, in steps 6 and 7, data for determining whether or not the constant velocity passing control of the present embodiment described above with reference to FIG. 3 is performed is calculated. That is, in FIG.
As shown in (b) and (c), when the section and the next section are circular arcs, circular arcs, circular arcs and straight lines, and straight lines and circular arcs, the point is a connecting position between the previous section and the subsequent section. At points (P _i , P _{i + 1} , P _{i + 2} , ...) In this case, if the direction component does not change abruptly, constant velocity passage control is performed. Then, when the direction component greatly changes between the previous section and the subsequent section, the conventional CP control is performed instead of the constant velocity passage control because the speed of the previous section cannot be sent to the subsequent section. That is, data for determining whether to perform constant velocity passing control or CP control is calculated.

First, in step 6, the tangential components of the start point and end point of each section are obtained. This is referred to as the start point and end point of the section i between the point P _i-1 and the point P _i in FIG. 4 described above, the start point and end point of the section i + 1 between the next point P _i and the point P _{i + 1} , and so on. To obtain each section. Then, in the next step 7, based on the tangential direction component obtained in step 6, each connecting point, that is, the end point of the previous section at each teaching point (point P _i , point P _{i + 1} , ...). And the tangential difference θ from the start point of the subsequent section. Then, it is compared with an angle (for example, 3 ° or 4 °) set to determine whether or not the difference in the tangential direction allows constant velocity passage. Then, in step 8, based on the data calculated in steps 1 to 7, the trajectory control task is activated to cause the robot 10 to send the sealer. The processing of these steps 6, 7 and 8 will be described in more detail below.

FIG. 6 is a flow chart for explaining the processing in step 6. First, in decision step 61,
Whether or not the mode designated for each section is a straight line is determined.
Here, since the section _i between the point P _i-1 and the point P _i shown in FIG. 4 is a circular arc, the judgment step 61 is No, and the routine proceeds to the next judgment step 62, and it is judged whether or not the mode is a circular arc. To be done. In this case, the judgment step 62 becomes Yes, and in the next step 63, the direction component of the starting point of the section (arc) i is calculated by the following equation 1.

[Equation 1] In this equation 1, the center of the start point P _i-1 and the end point P _i is O,
The angle between the point P _i-1 and the point P _i is θc, and the transformation matrix from the center O to the arcs P _i-1 and P _i is A. A is a matrix with 3 rows and 3 columns. The calculated direction component value ρ of the starting point of the section _i between the point P _i-1 and the point P _i is stored in the starting point iρ92 of the i buffer for the section i shown in FIG. 9A. Then, in step 64, the direction component of the end point of the section i is calculated by the following equation 2.

[Equation 2]

The direction component ρ'of the end point of the section i between the calculated points P _i-1 and P _i is _i in the section shown in FIG. 9A.
It is stored at the end point iρ'93 of Buffer. Here,
The target value 91 of the i buffer stores, as data necessary for interpolation in step 5 described above with reference to FIG. 5, that the operation mode is the arc, the specified operation speed, and the like.

Then, the section i between the points P _i and P _{i + 1}
The processing for +1 (straight line) is performed. First, the judgment step 61 becomes Yes because the mode of the section is a straight line here, and in step 65, the X, Y and Z direction components ρ of the straight line connecting the point P _i and the point P _{i + 1} are given by the following formula 3 Calculate by

[Equation 3] Here, the coordinates of the point P _i are (x ₁ , y ₁ , z ₁ ) and the coordinates of the point P _{i + 1} are (x ₂ , y ₂ , z ₂ ). In the case of the straight line mode, since the direction components of the start point and the end point are the same, the calculated value of ρ is the start point i + 1ρ95 of the i + 1 buffer and the end point i + 1ρ′96 of this section shown in FIG. 9B. Be accommodated. If the mode specified for the section is other than the straight line or the arc, for example, the pulse mode, the decision step 61 and the decision step 62 are both No for that section, and the process proceeds to the next process without obtaining any direction component. And proceed.

Next, the processing in step 7 described above for obtaining the difference in the tangential direction will be described with reference to the flowchart of FIG. 7 showing this processing in detail. Here, in step 71, the difference in the tangential direction at the connecting portion (here, point P _i ) between the end point of the previous section and the start point of the subsequent section is the end point i of the i buffer shown in FIG. 9A.
ρ'93 and the starting point i + of the i + 1 buffer shown in FIG.
Based on 1ρ95, the following equation 4 is obtained.

[Equation 4] θ = cos ⁻¹ [(iρ ′ · i + 1ρ) / {| iρ ′ | * | i + 1ρ |}] (4) where (iρ ′ · i + 1ρ) is the inner product of iρ ′ and i + 1ρ Represents

Then, in decision step 72,
It is determined whether or not the tangential difference angle θ calculated in step 71 is equal to or smaller than a preset θs. That is, if the angle θ of the difference in the tangential direction is small, it is determined that the constant velocity passage control is possible (Yes in the determination step 72), and step 7
As shown in FIG. 9C, the process proceeds to the process of No. 3, and the flag 97 of the constant velocity passage OK is set in the i + 1 buffer. On the contrary, if the angle θ of the difference in the tangential direction is large, it is determined that the constant velocity passage control is impossible (No in the determination step 72), and the step 7
The process proceeds to the process of No. 4 and the flag for passing the uniform velocity is not set. Based on the processing of steps 6 and 7 described above, the trajectory control device 50 determines whether or not constant speed passage control is to be performed at the connecting portion of each section.

Next, the process of operating the robot 10 in step 8 described above will be described in more detail with reference to the flowchart of FIG. First, the CPU 11 is a RAM
A trajectory control task is started based on the program stored in 30 (step 801). Next, the interpolation pattern variable is calculated (step 802). Here, the movement amount Lt,
The remaining movement amount Lr, the synthesis start movement amount L _O, etc. are calculated.
This movement amount Lt means, for example, when the nozzle 19 is located on the point A ′ starting from the point P _i−1 shown in FIG.
_i-1 means the distance traveled by the nozzle 19 to the point A ′,
The remaining movement amount Lr means the nozzle 1 from the point A ′ to the next point P _i.
Means the distance 9 must be moved (note that
The CPU 11 stores this remaining movement amount Lr in the movement distance Li from the teaching point P _i-1 to the next teaching point P _i (i-buffer target value 91 shown in FIG. 9A). }
It is obtained by subtracting the moving amount Lt from). Then, the combined start movement amount L _O refers to the distance from the point A _{O at} which the composition of the speed component is started when CP control is performed to the point P _i . Incidentally, the synthetic starting point A _O is the feed at a designated speed V _O, the point at point P _i is decelerated by the acceleration-.alpha. _O P _i-1
It means the point where deceleration is started in order to zero the velocity component from the point P _i to the point P _i .

Then, by comparing the calculated remaining movement amount Lr with the synthesis start movement amount L _O, it is determined whether or not the section for feeding at a constant speed is exceeded (the synthesis start point A in FIG. 4).
_{It is} determined whether or not it has reached _O (decision step 803). That is, in this determination step 803, for example, at the time of feeding from the point P _i-1 to the point P _i, from the point P _i-1 to the synthesis start point A _O , the speed component is independently processed after step 804, Then, from the synthesis start point A _O, the velocity component synthesis control (CP control) or constant velocity passage control after step 806 is performed. Since the combination start point A _O has not been reached at the start of operation, the remaining movement amount Lr is larger than the combination start movement amount L _O , and the determination step 803 becomes No, and the processing proceeds to step 804 and subsequent independent processing.

Regarding the independent processing of step 804,
This will be described with reference to FIG. 10 showing a pattern of changes in the velocity component V _i from the point P _i-1 to the point P _i in this embodiment. First, in step 804, the velocity component V from the point P _{i -1} toward the point P _i
In order to perform the independent processing for _i , the target value to be sent to the nozzle 19 is calculated for each independent interpolation cycle, and an output is issued to the servo control unit 40 based on this target value to control the robot 10 (step 805). From this robot 10, accelerated by the acceleration alpha _O on an arc toward the nozzle 19 from the point P _i-1 to the point P _i, continue to feed at the constant velocity V _O the specified reach a constant speed V _O .

When the nozzle 19 reaches the synthesis start point A _O and the remaining movement amount Lr becomes equal to the synthesis start movement amount L _O , the determination step 803 becomes Yes and the step 806 is performed.
Subsequent synthesis control (CP control) or constant velocity passing control is started. Here, first, determination steps 806, 807, 80
8, 809 and 810 determine whether or not to perform constant velocity passage control. In the trajectory control device 50 of the present embodiment, constant velocity passage control is performed only when all of the following five conditions are satisfied. The five conditions are: The operator has input a constant velocity passage command. 2. The connecting portions that perform constant velocity passage are formed by arcs and arcs as shown in FIG. 3A, arcs and straight lines as shown in FIG. 3B, or straight lines and arcs as shown in FIG. 3C. It must be the connecting part of. 3. The difference in the tangential direction of the connecting portion between the previous section and the subsequent section that follows is not more than a predetermined angle. That is, if there is a large difference in the tangential direction of the connecting portion between the front section and the rear section, constant velocity passage cannot be performed. 4. This is because there is no difference in speed between sections where constant speed passage is performed, that is, constant speed feed cannot be performed unless the speeds specified in the previous section and the subsequent section are equal. 5. The section after being connected at a constant speed must have a distance that allows deceleration. That is, when the movement distance of the section after passing the constant velocity is smaller than the above-mentioned combined start movement amount L _O , deceleration of the speed component cannot be performed, so that the CP between the subsequent section and the section subsequent thereto is not possible. This is because it cannot be controlled.

Here, the above-mentioned five conditions are successively judged by judgment steps 806, 807, 808, 809 and 810. First, at decision step 806, it is determined whether the operator of Condition 1 has input a constant velocity passage command. Here, since the constant velocity passage command is input, the determination step 806 becomes Yes, and in the next determination step 807, the connecting portion that performs the constant velocity passage of the condition 2 is an arc and an arc, an arc and a straight line, or It is determined whether or not it is a connecting portion between a straight line and a circular arc. Here, since the section i of the arc is connected to the section i + 1 of the straight line, the judgment step 807 becomes Yes and the next judgment step 808 is performed.
Go to. In the judgment step 808, it is judged whether or not the difference in the tangential direction of the connecting portion between the section of Condition 3 and the section is equal to or less than a predetermined angle, depending on whether or not the flag of the constant velocity passage OK is set in the buffer. Here, FIG.
Flag 9 for passing the uniform velocity in the i + 1 buffer shown in (c)
Since No. 7 is set, the determination is Yes, and the determination is made in the next determination step 809. In the judgment step 809, it is determined whether or not the speeds in the section for performing the constant speed passage of the above condition 4 are equal to each other, by the specified speed stored in the target value of the buffer in the front section and the target value of the buffer in the rear section. Judge by comparing with the specified speed. Here, FIG. 9 (a)
Since the designated speed stored in the target value 91 of the buffer in the i section shown in FIG. 9 is equal to the designated speed stored in the target value 94 of the buffer in the i + 1 section shown in FIG. 9C, the determination in step 809 is made. Yes, and the process proceeds to the next judgment step 810. In the judgment step 810, it is judged whether or not the required movement amount Li (movement distance between teaching points) stored in the target value of the buffer in the rear section is larger than the combination start movement amount L _O calculated in the above step 802. By doing so, for the section after the constant speed connection of Condition 5 above,
It is determined whether or not there is a sufficient distance for deceleration in this section. Here, since the required movement amount Li stored in the target value 94 of the i + 1 buffer is larger than the combined start movement amount L _O , this judgment step 810 also becomes Yes, and the routine proceeds to step 811 and constant velocity passage control is performed.

In step 811, interpolation data for constant velocity passage control is created. Then, based on this interpolation data, a control command to the servo is output in step 805 to control the robot 10. For this, again velocity component V _i of the point P _i from the point P _i-1 of FIG. 10, the point from the point P _i P _{i + 1}
This will be described with reference to the pattern of changes in the velocity component V _{i + 1} of. In the conventional CP control, it exceeds a synthesis initiation point A _O in FIG. 4, as indicated by a dotted line in FIG. 10, the point from the point P _i-1 P
with decelerating the _i velocity component V _i of the point from the point P _i P
increasing the velocity component V _{i + 1} of the _{i + 1} were synthesized them. On the other hand, in the constant velocity passage control of the present embodiment, such velocity component synthesis is not performed, and the point P is set for the velocity component.
while the _i-1 velocity component V _i of the point P _i sends a specified speed V _O, and reaches the point P _i, turn, from the point P _i at the point P _{i + 1} of the velocity component V _{i + 1} Switch to feed.

From the point P _i at which the velocity component is switched,
The determination step 803 is No, and the trajectory in the direction of the point P _{i + 1} is drawn at the designated speed V _O by the independent processing of step 804. Then, the nozzle 19 causes the next synthesis start point _A'O.
When the remaining movement amount Lr becomes equal to the combination start movement amount L _O , the determination step 803 becomes Yes again and the process proceeds to the determination step 806. In the judgment step 806, the judgment is Yes because the constant velocity passage command is inputted, and in the next judgment step 807, the judgment about the relationship between the front section and the rear section is made. Here, the section i + 1 between the points P _i and P _{i +} 1 is a straight line, and the section i + between the next point P _{i + 1} and the point P _{i + 2.}
Since 2 is also a straight line, the determination in step 807 is No, and the combining processing by the CP control method in step 812 and thereafter is started. First, in step 812, the CPU 11 creates a composite pattern variable. That is, as shown in FIG. 10, the velocity component V _{i + 1} from the point P _i to the point P _{i + 1} is reduced by the set acceleration −α _O , and the velocity component V _{i
At i + 1} , the velocity component V _{i + 2} from the point P _{i + 1} to the point P _{i + 2}
Is added to the vector with the acceleration α _O to calculate the moving direction and the velocity component. Then, in step 813, a target value for each synthetic interpolation cycle is calculated in accordance with the calculated synthetic pattern variable, and an output is issued to the servo control unit 40 based on this target value to control the robot 10 (step 814). On the curve M ₁ connecting the points A ′ _O and B ′ _O.
Synthesis and nozzle 19 has reached the point B _'O (point velocity component V _{i + 1} going from point P _i to the point P _{i + 1} is zero is decelerated by the acceleration-.alpha. _O) is completed (decision 815
Yes), the process returns to step 802. After that, through the judgment step 803, the velocity component V _{i + 2} traveling from the point P _{i + 1} to the point P _{i + 2 in} the step 804 and thereafter is independently processed to move from the point P _{i + 1} to the point P _{i + 2} . Nozzle 19 on a straight line
To send. Then, the above processing is performed at the point P _{i + 2} → point P _{i + 3} → P
_{When i + 4} → ... is repeated and the output to the servo becomes the final output, that is, when the input teaching point reaches the final output, determination step 816 becomes Yes and all the processing is completed.

According to this embodiment, the nozzle 19 can be fed at a constant speed while drawing a desired locus. To simply pass the nozzle 19 over the teaching point P _{i at} the connecting portion of the arc and the straight line, the conventional method is used without CP control, and the point P _i-1 is sent to the point P _i and the point P _i is set. It could be done by stopping once and then moving to the next point P _{i + 1} . However, in this case, there is an inconvenience because the speed is reduced from the designated speed V _O to zero. For example, when the robot is made to perform the sealing, the speed is decreased from V _O to zero, so that the speed change becomes large, and it becomes difficult to uniformly apply the coating agent to the sealing portion.
On the other hand, the structure of the present embodiment can feed the connecting portion at a constant speed so that the change in speed can be made small and can be suitably used for a robot or the like that performs sealing or the like.

In the above embodiment, the trajectory control device 5 is used.
The example in which 0 is applied to the robot 10 for the sealer has been described, but it goes without saying that the present invention can be applied to a general device that controls the movement trajectory of an object such as a robot hand. In this embodiment, the CP control or the constant velocity passage control is switched. However, in the conventional trajectory control method in which the CP control is not performed, the conventional trajectory control and the constant velocity passage control of the present embodiment are performed. It can also be configured to switch between and. Further, in the above-described embodiment, the constant velocity passage control is performed at the connecting portion of the circular arc and the circular arc, the circular arc and the straight line, and the straight line and the circular arc. However, it can also be used for other curves, and also for connecting portions between straight lines when the difference in the tangential direction is less than or equal to a predetermined angle.

[0031]

The present invention is configured as described above and can be controlled so as to draw the same locus as the locus of the arc input at the connecting portion with the arc. Therefore, it is possible to draw a smooth and beautiful locus in the connection of arcs.

[Brief description of drawings]

FIG. 1 is a configuration diagram showing a mechanical configuration of a robot controlled by a trajectory control device according to an embodiment of the present invention.

FIG. 2 is a block diagram showing a configuration of a trajectory control device according to an embodiment of the present invention.

FIG. 3 is a diagram showing a trajectory that is a target of constant velocity passage control in the present embodiment.

FIG. 4 is a diagram showing an example of a locus in the locus control method of the present embodiment.

FIG. 5 is a flowchart showing an outline of the operation of the trajectory control device of the present embodiment.

6 is a flowchart for explaining in more detail the steps of calculating a direction component in the flowchart shown in FIG.

FIG. 7 is a flowchart for explaining the step of calculating the difference in the tangential direction of the flowchart shown in FIG. 5 in more detail.

FIG. 8 is a flowchart for explaining in more detail the steps of starting the trajectory control task of the flowchart shown in FIG.

FIG. 9 is a configuration diagram of a buffer created by the trajectory control device according to the present embodiment.

FIG. 10 is a diagram showing the magnitude of each velocity component V _i , V _{i + 1,} and V _{i + 2} in the trajectory control device of the present embodiment.

FIG. 11 is a diagram showing the magnitude of each velocity component V _i and V _{i + 1 in} the conventional trajectory control system.

FIG. 12 is a diagram showing a trajectory in a conventional trajectory control method.

[Explanation of symbols]

10 Robot 11 CPU 19 Nozzle 26 Operation Panel 30 RAM A _O Composite Start Point θ Difference in tangential direction

Claims (1)

[Claims] 1. A locus control device for creating a movement locus of a transported object based on a plurality of input teaching points, the method comprising: a first teaching point and a second teaching point following the first teaching point. On the second teaching point side of the rear section formed by the direction component on the second teaching point side of the front section to be formed, and the second teaching point and the third teaching point following the second teaching point. Direction component calculating means for calculating a direction component, and a directional component and a rear area of the front section calculated by the direction component calculating means for calculating a tangential difference between the front section and the rear section at the second teaching point. Tangential direction difference calculating means for calculating the tangential direction difference calculated by the directional component, a comparing means for comparing the tangential direction difference calculated by the tangential direction difference calculating means with a preset value, and the tangential direction compared by the comparing means. If the difference between is less than or equal to a preset value, Locus control system being characterized in that a moving locus control means for controlling the movement trajectory to send the transported object between equal speed.