Track generation device and automatic position control device

Abstract

The orbit generation device (1) is used to generate an orbit for a control object (4a) to pass through, and includes: a storage unit (1b) that stores a plurality of points through which the control object (4a) passes; and a processor (1a), and the processor (1a) performs the following processes: an acceptance process that accepts designated trajectory information, where the designated trajectory information is information on a user-specified trajectory in a partial interval between two points among the plurality of points; and an orbit generation process that, after receiving the designated trajectory information, generates the orbit in the partial interval by using a first trajectory, a second trajectory, and the designated trajectory information. The first trajectory passes through at least one anterior passing point that the control object passed through before the two points among the plurality of points, and the two points; the second trajectory passes through at least one posterior passing point that the control object passed through after the two points among the plurality of points, and the two points.

Description

Technical Field

[0001] The present invention relates to an orbit generation device and an automatic position control device. Background Art

[0002] Conventionally, in the fields of numerically controlled (NC) machine tools or industrial robots, etc., the following method has been known: an orbit of a tool is generated by interpolating between discrete points provided as passing points of the tool (for example, refer to Patent Document 1).

[0003] Generally, an orbit is generated by calculating interpolation curves connecting a plurality of adjacent points and connecting the interpolation curves to each other. In order for the tool to pass through the passing points without decelerating, it is necessary to smoothly connect the interpolation curves to each other at the passing points.

[0004] In Patent Document 1, in the calculation of the interpolation curve S m , P m+1 in the interval between, in order to connect the interpolation curves of adjacent intervals to each other, the points P m of the adjacent intervals are also used. m-1 , P m+2 . Specifically, in Patent Document 1, a quadratic curve S m-1 passing through three points P m , P m+1 is derived, and a quadratic curve S m (t), and a quadratic curve S m passing through three points P m+1 , P m+2 is derived, and the interpolation curve S m+1 (t) is calculated based on the first differential coefficient and the second differential coefficient of the quadratic curve at the points P m , P m+1 . m (t).

[0005] Prior Art Documents

[0006] Patent Documents

[0007] Patent Document 1: Japanese Patent Publication No. 06-058603 Summary of the Invention

[0008] Problems to be Solved by the Invention

[0009] In Patent Document 1, in a partial interval between the points P m and P m+1 , in order to connect S m (t) and S m+1 (t) as smoothly as possible, the control of the controlled object in this interval is made to progress from S m (t) to Sm+1 (t) Gradually transition. At this time, the trajectory depends on S m (t) and S m+1 (t). In the case where the two are different types of functions, etc., it is difficult for the user to predict the trajectory of this partial interval. Therefore, it is desired to smoothly connect S m (t) and S m+1 (t), and at the same time, it is easy for the user to predict the trajectory of this interval.

[0010] Solution to the problem

[0011] One solution of the present invention is a trajectory generation device that generates a trajectory for a control object to pass through. The trajectory generation device includes: a storage unit that stores a plurality of points passed by the control object; and a processor that executes the following processes: an acceptance process that accepts specified trajectory information, where the specified trajectory information is information on a trajectory specified by the user in a partial interval between two points among the plurality of points; and a trajectory generation process that, after receiving the specified trajectory information, generates the trajectory in the partial interval by using a first trajectory, a second trajectory, and the specified trajectory information. The first trajectory passes through at least one front passing point passed by the control object before the two points among the plurality of points, and the two points; the second trajectory passes through at least one rear passing point passed by the control object after the two points among the plurality of points, and the two points.

[0012] According to the above configuration, it is possible to smoothly connect the first trajectory and the second trajectory, and at the same time, it is easy for the user to predict the trajectory of this interval. Brief description of the drawings

[0013] Figure 1 is a block diagram of an automatic position control device and a trajectory generation device according to an embodiment.

[0014] Figure 2 is a schematic diagram of a control object controlled by this embodiment.

[0015] Figure 3 is a diagram for explaining the trajectory generation method of this embodiment.

[0016] Figure 4 is a diagram for explaining the trajectory generation method of this embodiment.

[0017] Figure 5 is a diagram for explaining the trajectory generation method of this embodiment.

[0018] Figure 6 is a diagram for explaining the trajectory generation method of this embodiment.

[0019] Explanation of reference numerals:

[0020] 1: Orbit generation device

[0021] 2: Automatic position control device

[0022] 3: Control unit

[0023] 4a: Controlled object

[0024] S i (u i ): Partial orbit

[0025] F0 i (u i ): Function defining the first trajectory

[0026] F0 i+1 (u i+1 ): Function defining the second trajectory Detailed implementation method

[0027] Hereinafter, an orbit generation device, an automatic position control device, and an orbit generation method according to an embodiment will be described with reference to the accompanying drawings.

[0028] The automatic position control device 2 is used to control the position of the controlled object. As Figure 2 shown, an example of the controlled object 4a is the position of the front end 5a of the multi-joint robot arm 5 of an industrial robot 4 or the like, or a predetermined position 6a of an end effector 6 such as a hand or a tool mounted on the front end 5a of the robot arm 5. Another example of the controlled object 4a is a tool of a numerically controlled (NC) machine tool. Therefore, the automatic position control device 2 can be a robot control device for controlling a robot or a numerical control device for controlling an NC machine tool.

[0029] As Figure 1 shown, the automatic position control device 2 includes: an orbit generation device 1 that generates an orbit passing through discrete n points P 1 、P 2 、…、P i-1 、P i 、P i+1 、P i+2, an orbit of...; a control unit 3 that moves a control target 4a along an orbit generated by an orbit generation device 1; and an input device 10. The input device 10 is a keyboard, a mouse, a display device with a touch screen function, a portable input device for setting an operation program, or the like. When the control target 4a is a predetermined position 5a at the front end of a robot arm 5 or an end effector 6, the control unit 3 is a robot control unit that controls the operation of the robot arm 5, and the automatic position control device 2 is a robot control device. In this case, the control unit 3 includes a processor and a storage device that stores an operation program. In addition, the automatic position control device 2 may be a computer that generates an operation program for a robot offline.

[0030] The orbit generation device 1 includes: a processor 1a like a central processing unit; and a storage unit 1b having a RAM, a ROM, a non-volatile memory, a hard disk, and the like. The discrete n points are input using the input device 10, or the discrete n points are received by the orbit generation device 1 from another computer and stored in the storage unit 1b.

[0031] Point P i For example, it is a teaching point set by an operator. For a control target 4a that moves in a two-dimensional plane, the position of point P i is represented by two-dimensional coordinates (x i , y i ). For a control target 4a that moves in a three-dimensional space, the position of point P i is represented by three-dimensional coordinates (x i , y i , z i ).

[0032] In addition, an orbit generation program 1c and a trajectory derivation program 1d are stored in the storage unit 1b. The following calculations performed by the orbit generation device 1 are realized by the processor 1a executing processing according to the orbit generation program 1c.

[0033] Next, an example of a basic orbit generation method performed by the orbit generation device 1 will be described.

[0034] As Figure 3 shown, the orbit generation device 1 calculates a curve S i , P i+1 that passes through two adjacent points P in the point sequence and interpolates between the two points P i , P i+1 and calculates a partial orbit between the two points P i represented by the curve S i . Point P i , P i+1 ; Point P iis the i-th point in the point sequence. Among them, the orbit generation device 1 calculates the curve Si based on four points P from the (i - 1)-th to the (i + 2)-th i-1 , P i , P i+1 , P i+2 and thus calculates the partial orbit between two points P i-1 , P i , P i+1 , P i+2 or multiple points including these points. The orbit generation device 1 calculates the partial orbit for each interval of two adjacent points P i , P i+1 and obtains n - 1 partial orbits. i , P i+1

[0035] Next, the orbit generation device 1 generates an orbit passing through all n points P 1 , P 2 , …, P n by connecting the n - 1 partial orbits to each other.

[0036] Among them, the orbit generation device 1 calculates a curve S i (where 1 ≤ i ≤ n - 2) that satisfies the following two conditions. That is, at the point P i+1 , the first derivative values of the partial orbit (the first curve) S i (u i ) and the partial orbit (the second curve) S i+1 (u i+1 ) are consistent with each other. In addition, at the point P i+1 , the second derivative values of the partial orbit S i (u i ) and the partial orbit S i+1 (u i+1 ) are consistent with each other.

[0037] The first and second derivative values of the partial orbit S i (u i ) respectively represent the speed and acceleration of the control object 4a moving along the partial orbit S i (u i ). Therefore, the position, speed, and acceleration of the control object 4a moving along the orbit generated by the orbit generation device 1 are continuous at all points P 2 , P 3 , …, P n-1 that are the connection points of the partial orbits.

[0038] Next, regarding the partial orbit S i (u iDescribe the specific calculation method.

[0039] As Figure 3 shown, for i = 2, 3, …, n - 1, calculate the function F i-1 of the curve passing through three consecutive points P i , P i+1 , P i (u i ). For example, for i = 1, calculate the function F 1 of the curve passing through two points P 2 , P 1 (u 1 ). u i is a variable representing the interpolation progress between point P i and point P i+1 with values from 0 to 1. Among them, u i+1 = u i - 1, u i+2 = u i - 2, and so on. That is, in Figure 3 , at point P i , u i = 0, at point P i-1 , u i = -1, at point P i+1 , u i = 1, and at point P i+1 , the variable u i+1( in the formula F i+1) u i+1 is 0. The function F i (u i ) passes through point P i-1 when u i = -1, passes through point P i when u i = 0, and passes through point P i+1 when u i = 1. Similarly, the function F i+1 (u i+1 ) passes through point P i when u i = 0, passes through point P i+1 when u i = 1, and passes through point P i+2 when u i = 2.

[0040] Figure 3 As an example, describe the control of the control object 4a in the X-axis direction. In Figure 3 , a point sequence P i on the u - x plane represented by the u-axis representing the variable u i and the x-axis representing x i-1 , Pi , P i+1 , P i+2 , …. In the said plane, as an example, the function F i (u i ) is a function of a conic section such as a circular arc, an elliptical arc, a parabola, etc. The function F i (u i ) can also be a function of other conic sections, a linear function, or a function of a curve of degree three or higher.

[0041] Thus, when decomposing the point sequence P 1 , P 2 , …, P n into x-dimension, y-dimension, and z-dimension for consideration, similar to the point sequence P 1 , P 2 , …, P n on the u-x plane, calculate the function F 1 , P 2 , …, P n i (u(u i ). For the point sequence P 1 , P 2 , …, P n on the u-z plane, calculate the function F i (u i ).

[0042] Next, for any four arbitrarily set points P 1 , P 2 , …, P n in the point sequence P i-1 , P i , P i+1 , P i+2 , according to the following formula (1), calculate the partial orbit S i (u i ). In addition, in this embodiment, these four points are consecutive points. For the calculation of the partial orbit S i (u i ) of the following or previous partial interval, use the corresponding points P i-1 , P i , P i+1 , P i+2 to perform.

[0043] S i (u i ) = (1 - K(u i )) × F i (u i ) + K(u i ) × F i+1 (ui+1 )…(1)

[0044] K(u i ) is a function that satisfies the following condition 1.

[0045] (Condition 1) When u i varies from 0 to 1, the value of K(u i ) monotonically increases from 0 to 1.

[0046] K(u i ) only needs to be a function whose value monotonically increases from 0 to 1 when u i varies from 0 to 1. However, as an example, K(u i ) is a function defined by the following formula (2). In this case, at point P i of the partial orbit S i (u i ), S i-1 (u i-1 ) have equal values, and their first derivative values, second derivative values, and third derivative values are equal to each other.

[0047] K(u i ) = u 3 (10 - 15u + 6u 2 )…(2)

[0048] Next, an example of the orbit generation method of the present invention will be described using Figure 4 and Figure 5 . Figure 4 Another example of the control of the control object 4a in the axial direction will be described. In Figure 4 , a function (first trajectory) F0 i-1 defining the trajectory of the interval from point P i+1 to point P i (u i ) is a linear function depicting a straight line, and a function (second trajectory) F0 i defining the trajectory of the interval from point P i+2 to point P i+1 (u i+1 ) is a function depicting a quadratic curve such as an arc, an elliptical arc, a parabola, or a cubic curve. In addition, the function F i (u i ) can also be a quadratic function or a cubic function that roughly depicts a straight line. In one example, the processor 1a automatically calculates the first trajectory and the second trajectory passing through each object point group according to a known program.

[0049] In the case of Figure 4 , if the formula (1) is used, then point P i+1The connection of the locus at [[]] becomes smooth, but like Figure 4 the partial orbit S shown i (u i ), at the point P i and the point P i+1 the orbit between them is not a straight line. Sometimes the user does not expect this situation. For example, sometimes the user wants the orbit between the point P i and the point P i+1 to be a straight line.

[0050] In this embodiment, as Figure 5 shown, the processor 1a derives the first interval locus F as follows according to the locus derivation program 1d as in the following formula (3) i (u i ), and derives the second interval locus F as in the following formula (4) i+1 (u i+1 ).

[0051] [Equation 1]

[0052]

[0053] [Equation 2]

[0054]

[0055] X(u i ) in formula (3) and X(u i+1 ) in formula (4) are functions that are consistent with each other in the interval between the point P i and the point P i+1 , and are functions that depict a straight line in the case of Figure 5 . The partial orbit S i (u i ) generated by applying the first interval locus F i+1 (u i+1 ) of formula (3) and the second interval locus F i (u i ) of formula (4) to the said formula (1) becomes a straight-line orbit along X(u i ).

[0056] Since X(u i ) and X(u i+1 ) are consistent in the interval between the point P i and the point P i+1 , it is also possible to connect the first interval locus F i (u i ) and the second interval locus F i+1 (u i+1 ) using a formula other than formula (1).

[0057] Among them, at point P i , that is, when u i = 0, f in formula (3) i (u i ) is a function that smoothly connects with X(u i ). This function f i (u i ) is derived by the processor 1a according to the trajectory derivation program 1d, using, for example, the first trajectory F0 i (u i ) and X(u i ). For example, f i (u i ) is calculated according to the following formula (5) in the interval where u i is from -1 to 0. Among them, u i-1 = u i + 1, u i－2 = u i + 2, and so on for the rest. In the interval where u i is from 1 to 2, the function f i+1 (u i+1 ) that smoothly connects with X(u i+1 ) is also calculated in the same way.

[0058] f i (u i ) = (1 - K(u i-1 )) × F0 i (u i ) + K(u i-1 ) × X(u i )…(5)

[0059] X(u i ) and X(u i+1 ) are set according to the input of the user to the input device 10 of the automatic position control device 2. For example, the user inputs specified trajectory information using the input device 10. The specified trajectory information includes at least the interval of the specified orbit and the type of the trajectory in this interval. Examples of the type of the trajectory are a straight-line trajectory, an arc trajectory, an elliptical arc trajectory, etc. Sometimes the type of the trajectory is also set as a spiral curve trajectory, a quadratic function curve trajectory, a cubic function curve trajectory, etc. In Figure 5 's case, the point group shown in Figure 5 is displayed on the screen of the input device 10, and the user uses the input device 10 to indicate points P i+1 , P i and between points P i+1 , etc., so as to input the interval specification information between the specified points P i and P i+1 .

[0060] In addition, the screen displays a plurality of trajectory types such as a straight trajectory, a circular arc trajectory, and an elliptical arc trajectory, and the user selects any one of them using the input device 10 to input information on the trajectory type. In addition, the user can also input the first trajectory or the second trajectory as the point P into the input device 10. i With point P i+1 In the case of selecting the first trajectory, X(u i ) and X(u i+1 ) also becomes a straight line.

[0061] In addition, X(u i ) and X(u i+1 ) You can also choose not to click P i With point P i+1 The intervals between them are completely consistent. Even in this case, X(u i+1 ) is also related to f i+1 (u i+1 ) different functions. Preferably, X(u i+1 ) is the same as X(u i ) describes the same type of trajectory, in X(u i ) is a function that describes a straight line, X(u i+1 ) is also a function that draws a straight line. i ) is a function that describes an arc, X(u i+1 ) is also a function for drawing circular arcs.

[0062] In X(u i ) and X(u i+1 ) is not at point P i With point P i+1 If the intervals between them are completely consistent, the formula (1) can be used to smoothly connect F in some intervals. i (u i ) of X(u i ) and F i+1 (u i+1 ) of X(u i+1 ). Even in X(u i ) and X(u i+1 ) at point P i With point P i+1 When the intervals between them are completely consistent, we can also use formula (1) to connect F in some intervals. i (u i ) and F i+1 (u i+1 ).

[0063] In addition, if Figure 6 As shown, the function F0 defining the first trajectoryi (u i ) and a function F0 defining a second locus i+1 (u i+1 ) can also both be functions depicting an arc. In this case, by using the input device 10 to indicate an interval of a specified locus and, for example, selecting the first locus as the locus of the interval, the specified locus information is input into the locus generation device 1. In addition, instead of selecting the first locus as the locus between point P i and point P i+1 , an arc locus, the radius of the arc, the center, etc. can also be input using the input device 10.

[0064] In addition, in the above-described embodiment, a partial locus S i (u i ) is calculated using four consecutive points, but it can also be performed using four non-consecutive points. For example, in Figure 5 , f of the second interval locus i+1 (u i+1 ) can also be set as a function representing the locus between point P i+1 and point P i+3 . In this case, for example, X(u i ) of the first interval locus and X(u i+1 ) of the second interval locus are set as functions representing the locus between point P i and point P i+1 . In addition, f of the first interval locus i (u i ) is set as a function representing the locus between point P i-1 and point P i . In addition, a function F0 defining a second locus i+1 (u i+1 ) corresponds to the interval from point P i+1 to point P i+3 . Even in this case, it has the same effect as described above.

[0065] In addition, in the above-described embodiment, the input device 10 is used to indicate Figure 5 the point P i and point P i+1 therein. Instead, it is also possible to indicate, for example, the interval between point P Figure 5 and point P i+1 in i+3 as the locus specification interval. In this case, a point P i+3 before point P i+4 is also required.

[0066] In the above-described embodiment, for example, in Figure 5In this case, by using the first trajectory F0 i (u i ), the second trajectory F0 i+1 (u i+1 ), and the specified trajectory information input by the user to the input device 10, a point P i and the part of the interval between the point P i+1 are used to generate an orbit. The first trajectory F0 i (u i ) passes through two points Pi and Pi+1 and the front passing point P i-1 that the previous control object 4a passed through. The second trajectory F0 i+1 (u i+1 ) passes through two points Pi and Pi+1 and the rear passing point P i+2 that the subsequent control object 4a passes through.

[0067] For example, in Figure 5 , if a part of the interval between the point P i and the point P i+1 is generated without specifying trajectory information, as shown in Figure 4 , it is different from the user's intention, and the orbit of the part of the interval deviates from the straight-line orbit. When the control object 4a is a tool such as a welding tool, making its processing orbit consistent with what the user desires is extremely important for ensuring the strength, durability, etc. of the product.

[0068] In a robot, the positions of the passing points (teaching points) of the control object 4a are set according to the shape of the work object. There are cases where the passing points are roughly arranged in a straight line and cases where they are roughly arranged on an arc. In these cases, the orbit generation device 1 generates an orbit such as a straight line or a non-circular quadratic curve, cubic curve, etc. in a way that the orbit passes through all points computationally, so there are few cases where the orbit between the passing points deviates from the user's prediction. In contrast, in the above-described embodiment, the type of the trajectory in a part of the interval can be input as the specified trajectory information to the input device 10. For example, as shown in the examples of Figure 5 , Figure 6 , the types of trajectories such as a straight-line trajectory and an arc trajectory are input. This configuration enables reliable passing through the point P i and P i+1 , and at the same time, realizes the generation of the orbit of the control object 4a according to the user's intention.

[0069] In addition, it is not limited to a configuration where the user inputs specified trajectory information to the input device 10 for all parts of the interval. In one example, the user inputs specified trajectory information for important parts of the interval, and for other parts of the interval, the processor 1a uses the first trajectory F0 i (u i ) and the second trajectory F0i+1 (u i+1 ) is applied to, for example, formula (1) to generate a partial orbit.

[0070] In addition, the orbit generation device 1 does not necessarily need to pass through all the n points P 1 、P 2 、…、P n pre-stored in the storage unit 1b. Instead, new points P j ’ can be generated near the stored points P j to replace the points P j and the orbit can be generated in such a way that it passes through the points P j ’.

[0071] For example, when the n points P 1 、P 2 、…、P n are teaching points, for some of the teaching points, the control object 4a sometimes does not need to pass through them strictly. In this case, for example, in order to make the control object 4a move smoothly to generate an optimal orbit, the teaching point P j can be changed to a nearby point P j ’ and the point P j ’ can be stored in the storage unit 1b as the changed teaching point.

Claims

1. An orbit generation device that generates an orbit for a control object to pass through. Characterized in that: It includes: A storage unit that stores multiple points passed by the control object; and A processor, The processor performs the following processing: An acceptance process that accepts specified trajectory information, where the specified trajectory information is information on a user-specified trajectory in a partial interval between two points among the multiple points; And An orbit generation process that, after receiving the specified trajectory information, uses a first trajectory, a second trajectory, and the specified trajectory information to generate a first curve of the orbit in the partial interval and a second curve of the orbit in a subsequent partial interval between the partial interval and a subsequent passed point. The first trajectory passes through at least one front passed point that the control object passed before the two points among the multiple points and the two points, and the second trajectory passes through at least one of the subsequent passed points that the control object passed after the two points among the multiple points and the two points. The processor performs the orbit generation process as follows: Set the trajectory in the partial interval to a trajectory corresponding to the type of trajectory represented by the specified trajectory information, and at a point between the partial interval and the subsequent partial interval, the values of the first derivatives of the first curve and the second curve are mutually consistent, and the control object can move smoothly without stopping at this point.

2. The orbit generation device according to claim 1, Characterized in that: The specified trajectory information represents that the type of trajectory in the partial interval is any one of a straight-line trajectory, a circular arc trajectory, an elliptical arc trajectory, a spiral curve trajectory, a quadratic function curve trajectory, and a cubic function curve trajectory.

3. The orbit generation device according to claim 1 or 2, Characterized in that: The front passed point, the two points, and the subsequent passed point are four consecutive points that the control object passes through in this order.

4. The orbit generation device according to claim 1 or 2, Characterized in that: The processor performs the following derivation process: Use the first trajectory and the specified trajectory information to derive a first interval trajectory passing through the front passed point and the two points, and use the second trajectory and the specified trajectory information to derive a second interval trajectory passing through the two points and the subsequent passed point.

5. The orbit generation device according to claim 3, Characterized in that: The processor performs the following derivation process: Use the first trajectory and the specified trajectory information to derive a first interval trajectory passing through the front passed point and the two points, and use the second trajectory and the specified trajectory information to derive a second interval trajectory passing through the two points and the subsequent passed point.

6. An automatic position control device that controls the position of a control object. Characterized in that: It includes: The orbit generation device according to any one of claims 1 to 5; and A control unit that causes the control object to move along the orbit generated by the orbit generation device.

7. The automatic position control device according to claim 6, Characterized in that, the control unit is used to control the movement of the robotic arm, and the controlled object is the front end of the robotic arm or an end effector mounted on the front end of the robotic arm.

Images