TRAJECTORY GENERATION DEVICE AND AUTOMATIC POSITION CONTROL DEVICE

Abstract

A trajectory generation device configured to generate a trajectory along which a control target passes, the device including a storage unit configured to store a plurality of points, and a processor. the processor is configured to perform receiving process of receiving designated path information about a path designated by a user in a partial section between two points in the plurality of points, and trajectory generation process of generating a trajectory in the partial section by using the designated path information, a first path passing through the two points in the plurality of points and at least one anterior passing point through which the control target passes before passing through the two points, and a second path passing through the two points in the plurality of points and at least one posterior passing point through which the control target passes after passing through the two points.

Claims

1. A trajectory generation device configured to generate a trajectory along which a control target passes, the trajectory generation device comprising: a storage unit configured to store a plurality of points through which the control target passes; and a processor configured to perform: receiving process of receiving designated path information about a path designated by a user in a partial section between two points in the plurality of points, and trajectory generation process of generating, upon reception of the designated path information, a trajectory in the partial section by using the designated path information, a first path passing through the two points in the plurality of points and at least one anterior passing point through which the control target passes before passing through the two points, and a second path passing through the two points in the plurality of points and at least one posterior passing point through which the control target passes after passing through the two points.

2. The trajectory generation device according to claim 1, wherein the designated path information indicates that a type of the path in the partial section is any one of a linear path, an arc path, an elliptic are path, a spiral curved path, a quadratic-function curved path, and a cubic-function curved path.

3. The trajectory generation device according to claim 1, wherein the anterior passing point, the two points, and the posterior passing point are four continuous points through which the control target passes in this order.

4. The trajectory generation device according to claim 1, wherein the processor is configured to perform calculation process of calculating a first section path passing through the anterior passing point and the two points by using the first path and the designated path information, and a second section path passing through the two points and the posterior passing point by using the second path and the designated path information.

5. The trajectory generation device according to claim 4, wherein the path between the two points in the first section path derived in the calculation process is a path of the type indicated by the designated path information.

6. An automatic position control device configured to control a position of a control target, the automatic position control device comprising: the trajectory generation device according to claim 1; and a controller configured to move the control target along the trajectory generated by the trajectory generation device.

7. The automatic position control device according to claim 6, wherein the controller configured to control motion of a robot arm, and the control target is a distal end of the robot arm or an end effector that is attached to the distal end of the robot arm.

Description

BRIEF DESCRIPTION OF DRAWINGS

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

[0007] FIG. 2 is a schematic diagram of a control target to be controlled in this embodiment.

[0008] FIG. 3 is a diagram showing a trajectory generation method according to this embodiment.

[0009] FIG. 4 is a diagram showing the trajectory generation method according to this embodiment.

[0010] FIG. 5 is a diagram showing the trajectory generation method according to this embodiment.

[0011] FIG. 6 is a diagram showing the trajectory generation method according to this embodiment.

DESCRIPTION OF EMBODIMENTS

[0012] In a partial section between points P.sub.m and P.sub.m+1, in order to connect S.sub.m(t) and S.sub.m+1(t) as smoothly as possible, control of a control target in this section is gradually changed from S.sub.m(t) to S.sub.m+1(t) in accordance with the progress of t. The trajectory at this time depends on S.sub.m(t) and S.sub.m+1(t), and, when, for example, they are different types of functions, it is difficult for a user to estimate the trajectory in this partial section. Therefore, it is desirable to make it easy for a user to estimate the trajectory in this section while smoothly connecting S.sub.m(t) and S.sub.m+1(t).

[0013] An aspect of the present disclosure is a trajectory generation device configured to generate a trajectory along which a control target passes, the trajectory generation device including: a storage unit configured to store a plurality of points through which the control target passes; and a processor, the processor is configured to perform: receiving process of receiving designated path information about a path designated by a user in a partial section between two points in the plurality of points, and trajectory generation process of generating, upon reception of the designated path information, a trajectory in the partial section by using the designated path information, a first path passing through the two points in the plurality of points and at least one anterior passing point through which the control target passes before passing through the two points, and a second path passing through the two points in the plurality of points and at least one posterior passing point through which the control target passes after passing through the two points.

[0014] A trajectory generation device, an automatic position control device, and a trajectory generation method according to an embodiment will be described below with reference to the drawings.

[0015] An automatic position control device 2 controls the position of a control target. As shown in FIG. 2, examples of a control target 4a includes the position of a distal end 5a of an articulated robot arm 5 of a robot 4 for industrial use or the like, and a predetermined position 6a of an end effector 6, such as a hand or a tool, attached to the distal end 5a of the robot arm 5. Another example of the control target 4a is a tool of a numerical control (NC) machine tool. Accordingly, 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.

[0016] As shown in FIG. 1, the automatic position control device 2 includes: a trajectory generation device 1 that generates a trajectory passing through n discrete points P.sub.1, P.sub.2, . . . , P.sub.i−1, P.sub.i, P.sub.i+1, P.sub.i+2, . . . that are given as a sequence of points; a controller 3 that moves the control target 4a along the trajectory generated by the trajectory generation device 1; and an input device 10. Examples of the input device 10 include a keyboard, a mouse, a display device with a touchscreen function, and a portable input device for setting an motion program. When the control target 4a is the distal end 5a of the robot arm 5 or the predetermined position 6a of the end effector 6, the controller 3 is a robot controller that controls the motion of the robot arm 5, and the automatic position control device 2 is the robot control device. In this case, the controller 3 includes a processor, a storage device storing an motion program, and the like. Note that the automatic position control device 2 may be a computer that generates an motion program for the robot off-line.

[0017] The trajectory generation device 1 includes a processor 1a, such as a central processing unit, and a storage unit 1b including a RAM, a ROM, a non-volatile storage, a hard disk, or the like. The above-described discrete n points are input by using the input device 10 or are received by the trajectory generation device 1 from another computer, and are stored in the storage unit 1b.

[0018] The point P.sub.i is a teaching point set by, for example, an operator. In the case where the control target 4a moves in a two-dimensional plane, the position of the point P.sub.i is represented by two-dimensional coordinates (x.sub.i, y.sub.i), whereas, in the case where the control target 4a moves in a three-dimensional space, the position of the point P.sub.i is represented by three-dimensional coordinates (x.sub.i, y.sub.i, z.sub.i).

[0019] Furthermore, the storage unit 1b stores a trajectory generation program 1c and a path calculation program 1d. The calculation by the trajectory generation device 1, which will be described below, is achieved by the processor 1a executing processing in accordance with the trajectory generation program 1c.

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

[0021] As shown in FIG. 3, the trajectory generation device 1 calculates a curve S.sub.i passing through two adjoining points P.sub.i and P.sub.i+1 in the sequence of points and interpolating between the two points P.sub.i and P.sub.i+1, and calculates a partial trajectory between the two points P.sub.i and P.sub.i+1 represented by the curve S.sub.i. The point P.sub.i is the i-th point in the sequence of points. By calculating the curve S.sub.i on the basis of four points P.sub.i−1, P.sub.i, P.sub.i+1, and P.sub.i+2, which are the (i−1)-th to the (i+2)-th points, the trajectory generation device 1 calculates a partial trajectory between the points P.sub.i and P.sub.i+1 defined by the four points P.sub.i−1, P.sub.i, P.sub.i+1, and P.sub.i+2 or multiple points including them. The trajectory generation device 1 obtains n−1 partial trajectories by calculating a partial trajectory for each section between adjoining two points P.sub.i and P.sub.i+1.

[0022] Next, the trajectory generation device 1 connects the n−1 partial trajectories to one another to generate a trajectory passing through all n points P.sub.1, P.sub.2, . . . , P.sub.n.

[0023] Here, the trajectory generation device 1 calculates a curve S.sub.i (where 1≤i≤n−2) that satisfies the two conditions below, which are: at the point P.sub.i+1, the first-order derivative values of the partial trajectory (the first curve) S.sub.i(u.sub.i) and the partial trajectory (the second curve) S.sub.i+1(u.sub.i+i) are equal to each other; and at the point P.sub.i+1, the second-order derivative values of the partial trajectory S.sub.i(u.sub.i) and the partial trajectory S.sub.i+1(u.sub.i+1) are equal to each other.

[0024] The first-order and second-order derivative values of the partial trajectory S.sub.i(u.sub.i) respectively represent the speed and acceleration of the control target 4a moving along the partial trajectory S.sub.i(u.sub.i). Accordingly, the position, speed, and acceleration of the control target 4a moving along the trajectory generated by the trajectory generation device 1 are continuous at all the points P.sub.2, P.sub.3, . . . , P.sub.n−1, which are connection points of the partial trajectories.

[0025] Next, a method for calculating the partial trajectory S.sub.i(u.sub.i) will be described below.

[0026] As shown in FIG. 3, a function F.sub.i(u.sub.i) of a curve passing through three continuous points P.sub.i−1, P.sub.i, and P.sub.+1 is calculated for i=2, 3, . . . , and n−1, and, for example, a function F.sub.i(u.sub.i) passing through two points P.sub.1 and P.sub.2 is calculated for i=1. At this time, u.sub.i is a variable representing the progress of interpolation between the point P.sub.i and the point P.sub.i+1 with a value from 0 to 1. Note that u.sub.i+1=u.sub.i−1, u.sub.i+2=u.sub.i−2, and the same applies to the rest. Specifically, in FIG. 3, u.sub.i=0 at the point P.sub.i, u.sub.i=−1 at the point P.sub.i−1, u.sub.i=1 at the point P.sub.i+1, and the variable u.sub.i+1 of the expression F.sub.i+1(u.sub.i+1) at the point P.sub.i+1 is 0. The function F.sub.i(u.sub.i) passes through the point P.sub.i−1 at u.sub.i=−1, passes through the point P.sub.i at u.sub.i=0, and passes through the point P.sub.i+1 at u.sub.i=1. Similarly, the function F.sub.i+1(u.sub.i+1) passes through the point P.sub.i at u.sub.i=0, passes through the point P.sub.i+1 at u.sub.i=1, and passes through the point P.sub.i+2 at u.sub.i=2.

[0027] FIG. 3 shows an example of the control of the control target 4a in the X-axis direction. FIG. 3 shows a sequence of points P.sub.i−1, P.sub.i, P.sub.i+1, P.sub.i+2, . . . on a u-x plane expressed by using a u axis representing the variable u.sub.i and an x axis representing x.sub.i. In this plane, the function F.sub.i(u.sub.i) is, for example, a function for a quadratic curve, such as an arc, an elliptic arc, or a parabola. The function F.sub.i(u.sub.i) may be a function for another quadratic curve, a primary linear function, or a function for a cubic or higher-order curve.

[0028] As in this case, when the sequence of points P.sub.1, P.sub.2, . . . , and P.sub.n is considered by decomposing the points into the x dimension, the y dimension, and the z dimension, similarly to the sequence of points P.sub.1, P.sub.2, . . . , and P.sub.n on the u-x plane, the function F.sub.i(u.sub.i) is calculated for the sequence of points P.sub.1, P.sub.2, . . . , and P.sub.n on the u-y plane, and the function F.sub.i(u.sub.i) is calculated for the sequence of points P.sub.1, P.sub.2, . . . , and P.sub.n on the u-z plane.

[0029] Next, for the four points P.sub.i−1, P.sub.i, P.sub.i+1, and P.sub.i+2 arbitrarily set in the sequence of points P.sub.1, P.sub.2, . . . , and P.sub.n, the partial trajectory S.sub.i(u.sub.i) in the partial section is calculated from Expression (1) below. Note that, in this embodiment, the four points are four continuous points.

[0030] Calculation of the partial trajectory S.sub.i(u.sub.i) in the next or previous partial section is performed by using the point P.sub.i−1, P.sub.i, P.sub.+1, and P.sub.i+2 corresponding thereto.

S.sub.i(u.sub.i)=(1−K(u.sub.i))×F.sub.i(u.sub.i)+K(u.sub.i)×F.sub.i+1(u.sub.i+1)  (1)

where K(u.sub.i) is a function satisfying Condition 1 below. (Condition 1) when u.sub.i changes from 0 to 1, the value of K(u.sub.i) monotonically increases from 0 to 1.

[0031] Note that K(u.sub.i) may be any function whose value monotonically increases from 0 to 1 when u.sub.i changes from 0 to 1, and, in an example, K(u.sub.i) is a function defined by Expression (2) below. In this case, the values of the partial trajectories S.sub.i(u.sub.i) and S.sub.i−1(u.sub.i−1) at the point P.sub.i are equal, and the primary derivative values, the secondary derivative values, and the tertiary derivative values thereof are equal to each other.

K(u.sub.i)=u.sup.3(10−15u+6u.sup.2)  (2)

[0032] Next, using FIGS. 4 and 5, an example trajectory generation method of the present disclosure will be described.

[0033] FIG. 4 shows another example of control of the control target 4a in the axial direction. In FIG. 4, the function (the first path) F0.sub.i(u.sub.i) defining the path in the section from the point P.sub.i−1 to the point P.sub.i+1 is a primary function that draws a straight line, the function (the second path) F0.sub.i+1(u.sub.i+1) defining the path in the section from the point P.sub.i to the point P.sub.i+2 is a function that draws a quadratic curve, such as an arc, an elliptic arc, or a parabola, or as a cubic curve or the like. Note that the function F.sub.i(u.sub.i) may be a quadratic function or a cubic function that draws a substantially straight line. In one example, the processor 1a automatically calculates the first path and the second path passing through their corresponding target point groups on the basis of a known program.

[0034] By using Expression (1) in the case of FIG. 4, the connection of the paths at the point P.sub.i+1 becomes smooth. However, as in the partial trajectory S.sub.i(u.sub.i) shown in FIG. 4, the trajectory between the point P.sub.i and the point P.sub.i+1 is non-linear. This situation may be unwanted by a user, and, for example, a user may want a linear trajectory between the point P.sub.i and the point P.sub.i+1.

[0035] In this embodiment, as shown in FIG. 5, the processor 1a derives a first section path F.sub.i(u.sub.i) as Expression (3) below and a second section path F.sub.i+1(u.sub.i+1) as Expression (4) below on the basis of the path calculation program 1d.

[00001]$\begin{array}{cc}\left\{\mathrm{Formula}1\right\}& \\ F_i\left(u_i\right)=\{\begin{array}{c}{f}_i\left(u_i\right)u_i<0\\ X\left(u_i\right)u_i\geqq 0\end{array}& \left(3\right)\end{array}$$\begin{array}{cc}\left\{\mathrm{Formula}2\right\}& \\ F_{i+1}\left(u_{i+1}\right)=\{\begin{array}{c}X\left(u_{i+1}\right)u_{i+1}<0\\ f_{i+1}\left(u_{i+1}\right)u_{i+1}\geqq 0\end{array}& \left(4\right)\end{array}$

[0036] X(u.sub.i) in Expression (3) and X(u.sub.i+1) in Expression (4) are functions that are equal to each other in the section between the point P.sub.i and the point P.sub.i+1, and, in the case of FIG. 5, are functions that draw straight lines. The partial trajectory S.sub.i(u.sub.i) generated by applying the first section path F.sub.i(u.sub.i) derived by Expression (3) and the second section path F.sub.i+1(u.sub.i+) derived by Expression (4) to Expression (1) above is a linear trajectory extending along X(u.sub.i).

[0037] Because X(u.sub.i) and X(u.sub.i+1) are equal in the section between the point P.sub.i and the point P.sub.i+1, it is possible to connect the first section path F.sub.i(u.sub.i) and the second section path F.sub.i+1(u.sub.i+1) by using an expression other than Expression (1).

[0038] Here, f.sub.i(u.sub.i) in Expression (3) is a function smoothly connected to X(u.sub.i) at the point P.sub.i, or when u.sub.i=0. This function f.sub.i(u.sub.i) is derived by the processor 1a by using, for example, the first path F0.sub.i(u.sub.i) and X(u.sub.i) on the basis of the path calculation program 1d. For example, in the section where u.sub.i is from −1 to 0, f.sub.i(u.sub.i) is calculated from Expression (5) below. Note that u.sub.i−1=u.sub.i+1, and u.sub.i−2=u.sub.i+2, and the same applies to the rest. Also in the section where u.sub.i is from 1 to 2, similarly to the above, a function f.sub.i+1(u.sub.i+1) that is smoothly connected to X(u.sub.i+1) is calculated.

f.sub.i(u.sub.i)=(1−K(u.sub.i−1))×F0.sub.i(u.sub.i)+K(u.sub.i−1)×X(u.sub.i)  (5)

[0039] X(u.sub.i) and X(u.sub.i+1) are set on the basis of the input by a user to the input device 10 of the automatic position control device 2. For example, the user inputs designated path information by using the input device 10. The designated path information includes at least a section in which the trajectory is to be designated, and the type of the path in that section. Examples of the type of the path include a linear path, an arc path, and an elliptic arc path. The type of the path may also be a spiral curved path, a quadratic-function curved path, a cubic-function curved path, or the like. In the case of FIG. 5, a point group shown in FIG. 5 is displayed on a screen of the input device 10, and, when a user instructs the point P.sub.i+1, the section between the point P.sub.i and the point P.sub.i+1, or the like in the point group with the input device 10, section designated information designating the section between the point P.sub.i and the point P.sub.i+1 is input.

[0040] Furthermore, multiple types of the path, such as a linear path, an arc path, and an elliptic arc path, are displayed on the screen, and, when a user selects one of them with the input device 10, the information about the type of the path is input. Note that a user may input, to the input device 10, that the first path or the second path is selected as the trajectory between the point P.sub.i and the point P.sub.i+1. Also when the first path is selected, X(u.sub.i) and X(u.sub.i+1) are linear.

[0041] Note that X(u.sub.i) and X(u.sub.i+1) do not need to be functions that are completely equal to each other in the section between the point P.sub.i and the point P.sub.i+1. Also in that case, X(u.sub.i+1) is a function different from f.sub.i+1(u.sub.i+1). Preferably, X(u.sub.i+1) is a function that draws the same type of path as that of X(u.sub.i), and, when X(u.sub.i) is a function that draws a straight line, X(u.sub.i+1) is also a function that draws a straight line, and, when X(u.sub.i) is a function that draws an arc, X(u.sub.i+1) is also a function that draws an arc.

[0042] When X(u.sub.i) and X(u.sub.i+1) are not the functions that are completely equal to each other in the section between the point P.sub.i and the point P.sub.i+1, it is possible to smoothly connect X(u.sub.i) of F.sub.i(u.sub.i) and X(u.sub.i+1) of F.sub.i+1(u.sub.i+1) in the partial section by using Expression (1). Also when X(u.sub.i) and X(u.sub.i+1) are completely equal to each other in the section between the point P.sub.i and the point P.sub.i+1, it is possible to connect F.sub.i(u.sub.i) and F.sub.i+1(u.sub.i+1) in the partial section by using Expression (1).

[0043] Furthermore, as shown in FIG. 6, the function F0.sub.i(u.sub.i) defining the first path and the function F0.sub.i+1(u.sub.i+1) defining the second path may both be functions that draw an arc. In this case, by instructing the section in which the trajectory is to be designated and selecting, for example, the first path as the trajectory in that section with the input device 10, the designated path information is also input to the trajectory generation device 1. Note that, instead of selecting the first path as the trajectory between the point P.sub.i and the point P.sub.i+1, it is also possible to input, with the input device 10, that the path is an arc path, and the radius, the center, and the like of the arc.

[0044] In the above-described embodiment, although the partial trajectory S.sub.i(u.sub.i) is calculated by using four continuous points, the calculation may be performed by using four non-continuous points. For example, in FIG. 5, f.sub.i+1(u.sub.i+1) of the second section path may be set as a function representing the path between the point P.sub.i+1 and the point P.sub.i+3. In this case, for example, X(u.sub.i) of the first section path and X(u.sub.i+1) of the second section path are set as functions representing the path between the point P.sub.i and the point P.sub.i+1. Furthermore, f.sub.i(u.sub.i) of the first section path is set as a function representing the path between the point P.sub.i−1 and the point P.sub.i. Furthermore, the function F0.sub.i+1(u.sub.i+1) defining the second path corresponds to the section from the point P.sub.i+1 to the point P.sub.i+3. Also in this case, the same advantageous effect as above is obtained.

[0045] Furthermore, in the above-described embodiment, the section between the point P.sub.i and the point P.sub.i+1 in FIG. 5 is instructed by the input device 10. Instead, it is possible to instruct, for example, the section between the point P.sub.i+1 and the point P.sub.i+3 in FIG. 5 as a trajectory designating section. In that case, the point P.sub.i+4 beyond the point P.sub.i+3 is needed.

[0046] In the above-described embodiment, for example, in FIG. 5, a trajectory in the partial section between the point P.sub.i and the point P.sub.i+1 is formed by using the first path F0.sub.i(u.sub.i) passing through two points P.sub.i and P.sub.i+1 and an anterior passing point P.sub.i−1 through which the control target 4a passes before passing through the two points P.sub.i and P.sub.i+1, a second path F0.sub.i+1(u.sub.i+1) passing through the two points P.sub.i and P.sub.i+1 and a posterior passing point P.sub.i+2 through which the control target 4a passes after passing through the two points P.sub.i and P.sub.i+1, and designated path information input to the input device 10 by a user.

[0047] For example, in FIG. 5, when the partial section between the point P.sub.i and the point P.sub.i+1 is generated without designated path information, the trajectory in the partial section deviates from a linear trajectory, as shown in FIG. 4, which is not intended by a user. When the control target 4a is a tool like a welding tool, it is essential to make the machining trajectory thereof accord with that intended by the user, in order to ensure the strength, durability, and the like of products.

[0048] In robots, the positions of passing points (teaching points) of the control target 4a are set depending on the shape of objects to be worked, and sometimes the passing points are arranged substantially linearly or substantially in an arc. In such a case, the trajectory generation device 1 generates a quadratic curve, cubic curve, or similar trajectory that is not linear or arc-shaped such that the trajectory mathematically passes through all the points. This often results in deviation of the trajectory between the passing points from that assumed by the user. In contrast, in the above-described embodiment, it is possible to input, to the input device 10, the type of the path in the partial section as the designated path information. For example, as in the examples shown in FIGS. 5 and 6, the type of the path, such as a linear path or an arc path, is input. This configuration realizes generation of a trajectory for the control target 4a intended by a user while reliably passing through the points P.sub.i and P.sub.i+1.

[0049] Note that the present invention is not limited to the configuration in which a user inputs, to the input device 10, designated path information for all partial sections. In one example, a user inputs designated path information for an important partial section, and the processor 1a generates partial trajectories for the other partial sections by applying the first path F0.sub.i(u.sub.i) and the second path F0.sub.i+1(u.sub.i+1) to, for example, Expression (1).

[0050] Note that the trajectory generation device 1 does not necessarily have to generate a trajectory passing through all n points P.sub.1, P.sub.2, . . . , and P.sub.n preliminarily stored in the storage unit 1b, but may create a new point P.sub.j′ near a stored point P.sub.j as necessary and generate a trajectory passing through the point P.sub.j′ instead of the point P.sub.j.

[0051] For example, when n points P.sub.1, P.sub.2, . . . , and P.sub.n are teaching points, sometimes the control target 4a does not need to exactly pass through some of the teaching points. In such a case, for example, in order to generate an optimum trajectory for moving the control target 4a smoothly, the teaching point P.sub.j may be changed to a nearby point P.sub.j′, and the point P.sub.j′ may be stored in the storage unit 1b as a changed teaching point.