JPH06214631A - Locus controller - Google Patents

Abstract

PURPOSE:To provide a locus controller which can control a mobile locus to set it at a target speed despite a short space secured between the teaching points. CONSTITUTION:A moved distance adder means adds together the moved distances covering plural teaching points of short spaces. Then a speed control means controls the transfer speed of an object to be transferred over plural teaching points based on the moved distances added together by the adder means. A speed judging means judges whether the speed controlled by the speed control means reached a target level or not. Then the transfer speed of the object is continuously increased by the speed control means until the speed judging means decides that the speed reached the target level. Thus the object is transferred at the target speed after the speed judging means judges that the transfer speed reached the target level. In such a way, the moved distances are added together over plural teaching points without controlling the transfer speed for each space among the teaching points despite a short space. Thus the transfer speed is controlled at a target level.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a locus control device for controlling the locus of movement of an object such as a hand part of a robot, and in particular, reaches a target speed even when a teaching point with a fine interval is given. The present invention relates to a trajectory control device that controls a movement trajectory.

[0002]

2. Description of the Related Art Conventional control (hereinafter referred to as the first control method)
In FIG. 8 (a), first, points P0, P1, P2.
.., the direction from the point P0 to the point P1 (indicated as P0 → P1 direction. The same applies to other points). Next, the velocity pattern V1 shown in FIG. 8B from the point P0 to P1 is determined. That is, the acceleration is first increased in the P0 → P1 direction by the acceleration α, when the target speed V is reached, the target speed V is moved, and the target acceleration V is decelerated by α (the deceleration is α). A deceleration start point A _O that decelerates to zero speed and reaches point P1 is obtained, and a speed pattern for starting deceleration from this deceleration start point A _O is obtained. Here, the target speed V is a predetermined maximum speed of an object whose trajectory is to be controlled, such as a robot, and a target acceleration α
Is the maximum acceleration of the transported object. According to this velocity pattern, the object is sent from the point P0 to the point P1 and the velocity is changed to the point P1.
At the same time, it was once set to zero, and then similarly sent from point P1 to point P2. In this technique, since the speed becomes zero at each point, the feeding speed of the transported object becomes slow and the efficiency deteriorates.

As an improved technique of this first control method, the first
The control described on page 165 of the proceedings of the Technical Meeting of the Robotics Society of Japan (hereinafter referred to as the second control method) is known. In the second control method, the point P shown in FIG.
Given 0, point P1 and point P2, the direction from the point P0 to the point P1 (shown as P0 → P1 direction) is first determined. Next, the velocity pattern V1 'shown in FIG. 8 (c) from the point P0 to P1 is determined. That is, similarly to the above-mentioned first control method, the acceleration α is first increased in the P0 → P1 direction,
When the target speed V is reached, the vehicle is moved at the target speed V, and then decelerated at a target acceleration of -α to obtain a deceleration start point A _O that exactly reaches point P1 when decelerated to zero (at this time The timing is t), and a speed pattern for starting deceleration from this deceleration start point A _O is obtained.

When the timing t is determined, when the speed component V2 'in the P1 → P2 direction is increased by the acceleration α from the timing t as shown in FIG. 8C, the speed V is reached. After that, the speed pattern to be the steady speed is calculated.
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. That is, in the first control method, deceleration is actually performed from the deceleration start point A _O , while in the second control method, as is apparent from FIG. 8C, the speed in the P0 → P1 direction is increased. At the same time when the decrease of the component V1 'is started, the velocity component V2' from the point P1 to the point P2 is increased, and the synthesized velocity is kept at the target velocity V at the timings t and t '.

Therefore, when the object is moved in accordance with the vector-added direction and speed, it moves at the target speed V in the P0 → P1 direction before the timing t. Timing t
From t to t ′, the velocity component V1 ′ in the P0 → P1 direction decreases and the velocity component V2 ′ in the P1 → P2 direction increases. Here, since the velocity component of P0 → P1 becomes zero at the timing t ', the object moves to the points P1 and P at the timing t'.
You are on the straight line connecting 2 and. Movement trajectory Therefore at the timing t and t ', will be on the curve M0 connecting the point A ₀ and the point B _0. In addition, the above calculation is point P
0 → After point P1, point P1 → point P2 → point P3 ...
(Note that point P3 is not shown).

[0006]

Here, when the interval between the teaching points is long as shown in FIG. 8A, if the second control method is used, for example, the section from the point P0 to the point P1 described above is used. As shown in the speed pattern of FIG. 8 (c), once the target acceleration α is reached and the target speed V is reached, the deceleration start point A is reached.
Even after reaching _O , the target velocity V can be maintained even in the section where the velocity components are synthesized from the point A _O to the point B _O because the velocity component V1 'is decreased and the next velocity component V2' is increased. . Then, the target speed V can be maintained in the section from the next point P1 to the point P2 and also in the section after the point P3 (not shown).

On the other hand, when the interval between the teaching points is short as shown in FIG. 9 (a), in the first conventional control method, acceleration from one point to the next is accelerated by the acceleration α. to initiate the deceleration reaches the deceleration start point a _O in the speed pattern, as shown in FIG. 9 (b), a continuous triangular pattern. Similarly, using the second control method, for example, the point P
As can be seen from the speed pattern V1 ′ shown in FIG. 9C for the section from 0 to the point P1, the deceleration start point A _O must be reached during acceleration at the target acceleration α, and deceleration must be started at the acceleration −α. Can't reach V. Therefore, even if the triangular speed pattern V1 ′ that does not reach the target speed V and the subsequent triangular speed pattern V2 ′ (speed component in the direction from the point P1 to the point P2) are combined from the deceleration start point A _O , they are also combined. The velocities are on the line connecting the vertices of the triangles of these velocity patterns, and even after the section of the velocity component V3 'from the next point P2 to the point P3, the synthesized velocity is on the line connecting the vertices of these triangles (two The value V0 (indicated by the dashed line) remains at V0. Therefore, when the interval between the teaching points is short, the feeding speed of the conveyed object is slowed down even if the second control method is used, the cycle time becomes long, and the efficiency deteriorates.

The present invention has been made to solve the above problems, and an object of the present invention is to make a locus that can control a locus of movement so as to reach a target speed even when the interval between teaching points is short. It is to provide a control device.

[0009]

The structure of the invention for solving the above-mentioned problems is, as shown in FIG. 1, a moving amount adding means for adding moving amounts across a plurality of teaching points, and the moving amount adding means. The speed control means for controlling the feeding speed of the conveyed object over a plurality of teaching points based on the movement amount added by the speed control means, and the speed judgment for judging whether the speed controlled by the speed control means has reached the target speed or not. The speed control means continues accelerating until the speed judgment means judges that the target speed has been reached, and when the speed judgment means judges that the target speed has been reached, the feed is carried out at the target speed. It is characterized by performing.

[0010]

According to the above means, the movement amount adding means adds the movement amounts across the plurality of teaching points with short intervals, and the speed control means adds the movement amount based on the movement amount added by the movement amount adding means. The feed speed is controlled over a plurality of teaching points, and the speed judgment means judges whether or not the speed controlled by the speed control means has reached the target speed. Then, the speed control means continues accelerating until the speed determining means determines that the target speed is reached, and when the speed determining means determines that the target speed is reached, the feed is performed at the target speed.
In this way, even if the teaching point interval is short, the speed is controlled so as to reach the target speed by adding the movement amount across a plurality of teaching points without performing acceleration / deceleration for each teaching point interval. .

[0011]

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 structure 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 ROM 20 stores the control information for performing the second control method and the control information for performing the high-speed trajectory tracking control of the present embodiment.
A RAM 30, an operating box 27 and a control panel 26 for inputting control commands, an external storage device 29 for holding control information created by the CPU 11, and a servo control section 40 are connected. This servo control unit 40
Are connected to the servomotors M1 to M6 for driving the first joint a of the robot 10 to the servomotors M6 to drive the sixth joint f, and from the encoders E1 to E6 attached to the respective servomotors M1 to M6. The feedback signal is configured to be fed back. The trajectory control device 50 uses the CPU 11 based on the input teaching points.
Calculates the locus of movement and sends the output to the servo control unit 40,
The servo control unit 40 controls the servo motors M1 to M6 to drive the robot 10 and seal the nozzle 1.
9 is configured to draw a desired movement trajectory.

Next, the concept of the high-speed trajectory tracking control of this embodiment will be described. For example, when using the data created by CAD, when a teaching point sequence P0, P1, P2, ... P10 with short intervals is given as shown in FIG. 4, the conventional first control method or the second control method is used. If the robot is controlled based on these teaching points by the control method of No. 2, the target speed V cannot be reached as described above with reference to FIGS. 9B and 9C. Therefore, in this high-speed trajectory tracking control, each teaching point (P
0, P1, P2, ... P10)
As in the velocity pattern shown in FIG. 5, the entire movement amount is controlled to be treated as one interval in a pseudo manner. The details of this high-speed trajectory tracking control will be further described below.

First, here, the teaching point sequence P0 shown in FIG. 4 is used.
, P1, P2, ... P10 are given, the processing of the trajectory control device 50 of this embodiment according to these teaching points will be described with reference to the flowchart of FIG. 7 and the speed pattern of FIG.

A train of teaching points P0, P1, P by the operator
2, when the program including P10 is input (step 1), the CPU 11 starts the decoding process of the program (step 2). Then, when the decoded instruction is not the operation end instruction (decision step 3 is No), it is judged whether the instruction is an operation instruction or a non-operation instruction (decision step 4). Here, if the instruction is a non-operation instruction (determination step 4 is No), the process proceeds to step 5 to perform processing according to the non-operation instruction. If the command is an operation command (determination step 4 is Yes), the process proceeds to step 6 to create data necessary for interpolation. Here, first, it is determined whether the target speed V can be reached by performing the second control method based on the input teaching point sequence, and if the target speed V can be reached, the second control method is performed. If the target speed V cannot be reached, it is determined that the high-speed trajectory follow-up control of this embodiment is performed. And further,
When performing high-speed trajectory follow-up control, the teaching point is passed to the teaching point of the teaching point whose interval is smaller than the minimum resolution (distance that can be moved in one interpolation cycle) and the next teaching point is set as the control target. Make a selection. In this example, since the interval of the teaching point sequence from the point P1 to the point P10 is short, high-speed trajectory follow-up control is selected, and the intervals of each teaching point are all longer than one interpolation cycle. The following description will be continued assuming that the teaching point is not passed.

Next, at step 7, the trajectory control task is started and first the process of sending the nozzle 19 of the robot 10 to the teaching point P1 is started. Then, in step 8, the command speed, the acceleration, and the minimum deceleration distance S are calculated. This minimum deceleration distance S
Is calculated from the target speed V to the target acceleration −α as shown in FIG.
Is a movement amount that can be decelerated by and stopped at a designated teaching point, that is, a minimum movement distance that can be decelerated from the target speed V, and is expressed as an area corresponding to a shaded portion of the speed pattern shown in FIG. It The CPU 11 uses the following equation 1
Calculate based on

[Equation 1] S = V ² / 2α (Equation 1)

Next, in a judgment step 9, it is judged whether or not this teaching point is the final data (point P10). Here, the determination step 9 is No, and the process proceeds to the next determination step 10.
In the judgment step 10, it is judged whether or not the feed speed has reached the target speed V. Since it is the start of movement here,
The determination step 10 becomes No, and the acceleration processing in the next acceleration section processing step 11 and the interpolation data creation step 12 is performed. In these steps, data for accelerating with the acceleration α toward the target position (point P1) is created for each interpolation cycle, and at the same time, the movement amount moved while performing the acceleration is added. Next, at step 16, a control command is sent to the servo control section 40 shown in FIG. 3 based on the created data to accelerate the nozzle 19 toward the teaching point P1 by acceleration α. Then, in the judgment step 17, it is judged whether or not one interval has ended, that is, whether or not the target teaching point (here, the point P1) has been reached, and the point P1 has not been reached yet, and it is necessary to continue the interpolation. In the case (No in judgment step 17),
Returning to step 10, the processing after step 11 is repeated for each interpolation cycle. Then, when the target teaching point P1 is reached (determination step 17 is Yes), the process returns to step 2 and the program decoding process is performed to start the process for moving to the next target teaching point P2.

The processing from step 3 to step 9 is similarly performed for the next teaching point P2. Then, the judgment step 10 is No because the target speed V has not yet been reached when the processing to the teaching point P2 is started as shown in the speed pattern of FIG.
Then, in addition to the movement amount up to the teaching point P1, a new movement amount is added, and based on this movement amount, an acceleration process is performed in which the acceleration is calculated and the acceleration is performed. When the target teaching point P2 is reached by these processes, the determination step 17 becomes Yes, the process returns to step 2 again, the program decoding process is performed, the process toward the next target teaching point P3 is started, and the steps 3 to 9 are performed. Process up to. And decision step 1
0 indicates that the target speed V has not been reached at the beginning of the process toward the teaching point P3 as shown in the speed pattern of FIG.
Then, in steps 11, 12, and 16, a new movement amount is added in addition to the movement amount up to the teaching point P2, and the acceleration processing is performed to obtain the acceleration based on the movement amount and accelerate the speed. Then, the acceleration is continued in the interpolation cycle unit, and the determination step 10 is Yes when the target speed V is reached at the timing t1.
Becomes

When the target speed V is reached, the judgment step 10 is Y
When it becomes es, the constant velocity processing in step 13 of the constant velocity section processing and the interpolation data creation step 12 is started. In these steps, data for feeding at a constant speed (target speed) is created for each interpolation cycle, and at the same time, the movement amount moved at the target speed is added. Then, in step 16, a control command is sent to the servo control section 40 shown in FIG.
Is transmitted at the target speed V. Then, when the target teaching point P3 is reached, the judgment step 17 becomes Yes and the processing returns to the step 2 and the teaching points P4 P5 ... P9 shown in FIG. Perform constant speed processing.

When the nozzle 19 is sent to the teaching point P9 and the processing for the point P10, which is the final teaching point, is started, the decision step 9 as to whether or not it is the final data becomes Yes, and the next decision step 18 is made. It is determined whether or not the amount of movement (interval) to the final teaching point P10 is less than or equal to the minimum deceleration distance S. When this interval is smaller than the minimum deceleration distance S (decision step 1
If 8 is No), go to Step 19. In step 19, the buzzer is sounded because deceleration cannot be performed at the input teaching point, and the operator is prompted to change the teaching point. As a result, the operator changes the final teaching point P10 to a position further away from P9. On the other hand, when the interval is larger than the minimum deceleration distance S (Yes in determination step 18), the process proceeds to determination step 10 and subsequent steps.

Since the target speed V has been reached at the start of the process toward the teaching point P10, the determination in step 10 is Yes.
Similarly, the determination step 13 also becomes Yes, and the process proceeds to step 14 where constant velocity section processing is performed and the nozzle 19 is continuously sent at the target velocity V. When the remaining movement amount to the teaching point P10 becomes equal to the minimum deceleration distance S at the timing t2, the determination step 13 becomes No and the deceleration processing in the final point processing step 15 and the interpolation data creation step 12 is started. Here, data for deceleration is created for each interpolation cycle, and at the same time, the amount of movement moved while decelerating is added. And in step 16,
Based on the created data, a control command is sent to the servo control unit 40 to decelerate the nozzle 19 of the robot 10 with acceleration -α. When the final teaching point P10 is reached, the operation end instruction is decoded by the decoding processing in step 2 (Yes in determination step 3), and all processing is ended.

In this embodiment, only the processing of teaching points with short intervals has been described, but the trajectory control device 50 of this embodiment creates the data necessary for the interpolation in step 6 as described above. At this time, it is determined whether the target moving speed V can be reached by performing the second control method based on the input interval of the teaching point sequence, and if the target speed can be reached, the second control method is selected. If the target speed cannot be reached, it is decided to perform the above-described high-speed trajectory tracking control. Therefore, when a section of the teaching point sequence having a short interval exists in a part of the teaching point sequence having a long interval, the second control method is automatically performed in the teaching point sequence having a long interval, and
For a train of taught points having a short interval, high-speed trajectory follow-up control is performed to always maintain the target speed V.

According to this embodiment, since the moving amounts of the teaching points having short intervals are added and the speed is controlled based on the obtained long moving amount, the target speed V can be reached and the designated teaching point can be reached. Therefore, the nozzle 19 can be faithfully sent.
That is, as another method of increasing the feed rate when a teaching point with a short interval is given, the present inventor has passed (ignoring) the teaching point with a short interval, and some of the teaching point sequences input. Although a method has been devised in which a long movement amount is obtained and the feed speed is increased by selecting only a teaching point, this method differs from the method of the present invention in that a teaching point with a short interval is passed, so that a trajectory faithful to the teaching point is obtained. I couldn't draw. On the other hand, the method of the present embodiment has an advantage that the trajectory can be drawn faithfully on the teaching point. Further, according to this embodiment, the nozzle 19 can be fed at a constant speed V. For example, when causing the robot to perform sealing, it is difficult to apply the coating agent uniformly if there is a change in speed, but the trajectory control device of the present embodiment can always send the nozzle 19 at the target speed V. Since it is possible, it can be preferably used in an application place where it is desired to reduce the speed change such as sealing work.

In this embodiment, the robot is controlled in accordance with the teaching points while the program decoding process is being performed. However, it is also possible to calculate the velocity pattern as a pre-process before starting the robot operation. Is. In this case, after the processing for the teaching point P9 in the above example is finished, it is judged whether or not the nozzle 19 can be stopped at the final teaching point P10, whereas the speed pattern is calculated by the preprocessing. By doing so, it is possible to start deceleration before reaching the teaching point P9. Further, in the above-described embodiment, the trajectory control device 50 is used as the sealer robot 1.
Although the example applied to No. 0 has been described, it goes without saying that the trajectory control device of the present invention can be applied to a general device that controls the movement trajectory of an object such as a hand of a robot.

[0025]

The present invention is configured as described above, and the target speed can be reached even if the interval between the teaching points is short, so that the cycle time can be shortened and the trajectory control can be performed at the optimum time. It will be possible.

[Brief description of drawings]

FIG. 1 is an overall configuration diagram for clarifying the present invention.

FIG. 2 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. 3 is a block diagram showing a configuration of a trajectory control device according to an embodiment of the present invention.

FIG. 4 is a diagram showing teaching points for performing trajectory control by high-speed trajectory tracking control according to the embodiment of the present invention.

FIG. 5 is a diagram showing patterns of velocity components in the high-speed trajectory tracking control of the present embodiment.

FIG. 6 is a diagram showing a pattern of deceleration of a speed component in the high speed trajectory tracking control of the present embodiment.

FIG. 7 is a flowchart showing an operation of the trajectory control device according to the present embodiment.

FIG. 8 is a diagram showing a locus in the case where the interval between teaching points is long and a trapezoidal pattern of velocity components V1 and V2 in the conventional locus control method.

FIG. 9 is a diagram showing a locus when the interval between teaching points is short and a triangular pattern of the velocity component thereof in the conventional locus control method.

[Explanation of symbols]

 10 Robot 11 CPU 19 Nozzle 30 RAM V Target Speed α Target Acceleration S Minimum Moving Distance

Claims (1)

[Claims] 1. A moving amount adding means for adding a moving amount across a plurality of teaching points, and a feed speed of a conveyed object is controlled over a plurality of teaching points based on the moving amount added by the moving amount adding means. It has speed control means and speed judgment means for judging whether or not the speed controlled by the speed control means reaches the target speed, and the speed control means judges by the speed judgment means that the speed has reached the target speed. The trajectory control device is characterized in that acceleration is continued until the speed is determined, and when the speed judgment means judges that the target speed has been reached, the feed is carried out at the target speed.