JP6501470B2 - Track generation method, track generation apparatus, robot apparatus, program and recording medium - Google Patents

Description

  The present invention relates to a trajectory generation method for generating a trajectory of a robot, a trajectory generation device, a robot device, a program, and a recording medium.

  There are mainly three known types of interpolation operation of a robot: linear interpolation, circular interpolation and joint interpolation. Linear interpolation is an interpolation method that makes the tool center point (TCP) go straight to the target point. Arc interpolation is an interpolation method in which a reference point is intentionally provided at a position deviated from a straight path between two points, and TCP is moved along a unique arc passing through three points. The circular interpolation and the linear interpolation are essentially the same because they interpolate in the task space.

  Joint interpolation is an interpolation method in which each axis basically simultaneously starts moving toward the target joint angle and reaches the target point at the same time regardless of the linearity or circularity of TCP. That is, joint interpolation is a method of generating a path in joint space (also referred to as configuration space). According to the joint interpolation, it is easy to perform control to exhibit 100% operation performance of each joint, and there is no singular point problem which becomes a problem in linear interpolation or circular interpolation, so a moving section requiring high speed operation Often used. However, the path and speed of TCP can not be easily predicted by the user because of the speed control for each joint.

  Each time the robot moves, an interpolation method appropriate to the movement and purpose is adopted. For example, a robot may grip a cylindrical part, move the cylindrical part over a cylindrical hole, and fit the cylindrical part into the hole. In this case, it is desirable that the movement of the robot after gripping be operated at high speed by joint interpolation, and the operation of inserting the cylindrical part into the hole from above the cylindrical hole be fitted straight using linear interpolation. .

  By the way, the interpolation operation in each movement section is given by a speed function which determines at what speed to move on the interpolation path. Assuming that the robot is stopped at the start of movement and at the end of movement, the movement is accelerated to increase the moving speed after the start of movement, and decelerated to stop before the end of movement.

  Here, as a method of moving two different paths in succession, it is conceivable to stop at the joint of the paths. For example, in the case of inserting a cylindrical part into a hole, acceleration is performed in the joint interpolation path to move, deceleration is performed before the end of the joint interpolation path, and stops at the joint of the path, and speed is gradually increased in the linear interpolation path. If it is raised, it is possible to operate continuously on two interpolation paths. However, not only is the robot strictly stopping between the two interpolation paths not only useless, but stagnant behavior in this part will prolong the robot's operation time. Of course, if rapid deceleration or rapid acceleration is performed to save working time, it is not preferable because it promotes the vibration of the robot.

  Therefore, for example, in the case where linear interpolation and linear interpolation are continuously operated, the velocity function on each linear interpolation path is first derived, and then acceleration of the deceleration zone of the previous movement zone and acceleration of the later movement zone Connect with the area. Then, the connection point between both sections or the vicinity thereof may be passed by the sum of the speed during deceleration of the previous movement section and the speed during acceleration of the subsequent movement section. This can reduce the time to reach the next teaching point. Such speed overlapping processing can be achieved in a sense in a sense by combining the speed functions themselves if the interpolation method of the previous movement section and the interpolation method of the later movement section are the same. In the case of linear interpolation and linear interpolation, in the task space, the preceding translational velocity function and the later translational velocity function are compared with the preceding rotational velocity function and the later rotational velocity function, respectively. All or part of each may be overlapped.

  In the case of operation by joint interpolation, in the case of a six-axis robot, a velocity function is given for each joint (axis). Even in this case, the overlapping process of the velocity between the joint interpolations obtains the velocity function on each joint interpolation path, and then the deceleration region of the first velocity function of the first joint and the acceleration region of the later velocity function, It may be performed for each joint as in the velocity function of the second joint and the velocity function of the second joint. Therefore, if it is a manipulator with six degrees of freedom, all six combinations will be superimposed.

  However, when the motion of the robot shifts from joint interpolation to linear interpolation, the space to be interpolated differs between the joint space and the task space, and the joint angular velocity of the joint interpolation and the translational velocity and rotational velocity of the linear interpolation TCP are different. The joining process can not be done as it is.

  Therefore, in Patent Document 1, when moving from joint interpolation to linear interpolation, linear interpolation is performed to obtain translational speed and rotational speed of TCP, and then inverse kinematics calculation is performed to convert to angular velocity of joint of each axis, The connection with the joint interpolation trajectory is performed. Since the speed of linear interpolation is converted to the joint angular velocity and the speeds are joined, joint angular velocity can be joined.

  On the other hand, Non-Patent Document 1 describes a method of calculating an optimum speed for calculating an optimum operating speed of a robot which obeys various physical constraints and minimizes the operation time on a given route. In order to adjust the motion speed of the robot and create the motion trajectory on a given route, the user usually adjusts the speed value many times so that the motion time of the robot is shortened while observing various physical constraints. I needed to. However, if a human adjusts the velocity value, the trajectory will not be optimal, and the operation time will be long, and at the same time, the number of trajectory development steps will be increased. Therefore, it is desirable to automatically calculate the optimum operating speed of the robot which obeys various physical constraints and minimizes the operating time.

  Non-Patent Document 1 describes an optimal velocity calculation method for generating a trajectory with a short operation time as much as possible for a given route while maintaining robot joint torque constraints.

Unexamined-Japanese-Patent No. 2004-252814

Verscheure, Diederik, et al. "Time-optimal path tracking for robots: a convex optimization approach." Automatic Control, IEEE Transactions on 54.10 (2009): 2318-2327.

  However, even according to Patent Document 1 and Non-Patent Document 1 described above, it is not possible to generate an optimal trajectory taking physical constraints into consideration when joining two interpolation paths.

  Non-Patent Document 1 does not describe a method of connecting two generated interpolation paths with smoothness with respect to parameters. Further, in the reference document 1 as well, although a method of connecting two interpolation paths is disclosed, the smoothness of the two interpolation paths with respect to the parameter s (smoothness in which the change of differential value differentiated to a predetermined rank is continuous) There is no mention of linking while leaving it in place.

  That is, conventionally, the first interpolation path generated by the first interpolation method and the second interpolation path generated by the second interpolation method are smoothed with respect to the parameter s (smoothness in which changes in derivative values differentiated to a predetermined rank are continuous) It has not been done until now while connecting them. For example, it is possible to connect two interpolation paths having different spaces for interpolating a path such as joint interpolation and linear interpolation, or two interpolation paths having different shapes such as arc interpolation and linear interpolation while maintaining smoothness with respect to the parameter s It was not done.

  Conventionally, since two interpolation paths are not connected with smoothness with respect to the parameter s, the command value of the joint angle for each time to the robot is generated, that is, when generating the robot trajectory, It has caused a large error in the optimality.

  For example, the joint acceleration of the joint i axis is expressed by using s

と書ける。もしｑ_ｉ（ｓ）がｓに関して滑らかでなく、単に連続である場合、ｑ_ｉ’（ｓ）、ｑ_ｉ’’（ｓ）、ｑ_ｉ’’’（ｓ）は不連続になるため、ｑ_ｉ（…）（ｓ）は、媒介変数ｓに関して不連続になる。

I can write. If q _i (s) is not smooth with respect to s, but is simply continuous, then q _i ′ (s), q _i ′ ′ (s), q _i ′ ′ ′ (s) are discontinuous, so q _i (...) (s) becomes discontinuous with respect to parameter s.

  When speed optimization calculation is performed on a path that is not smooth (the change in derivative value differentiated to a predetermined rank is discontinuous), a command value of joint angles to the robot for each fixed cycle is generated At the time, a big error occurred. This causes, for example, the joint acceleration to be largely deviated from the constraint. Also, this is not limited to only joint acceleration, and physical constraints such as joint torque constraint, joint angular velocity constraint, joint angular acceleration constraint, TCP velocity constraint or TCP acceleration constraint also produce large errors as well. It has become a cause of being largely out of the constraints.

  Therefore, an object of the present invention is to generate an optimal trajectory in accordance with physical constraints, in which a robot following a path obtained by combining two interpolation paths does not stop.

The present invention is the trajectory generation method in which the operation unit generates the trajectory of an articulated robot, and the operation unit is a function of a parameter, and is a derivative value differentiated with the parameter to a predetermined rank. A first interpolation path generation step of generating a first interpolation path from the first teaching point to the first auxiliary point, the change being continuous, by the first interpolation method, and the operation unit is a function of the parameter; A second interpolation path generation step of generating a second interpolation path from the second auxiliary point to the second teaching point, in which the change of the differential value differentiated by the parameter to the predetermined rank is continuous, by the second interpolation method And the calculation unit is a function of the parameter, the change of the differentiated value differentiated by the parameter to the predetermined rank being continuous, from the first halfway point on the first interpolation path, the second Generate a spliced path towards the second waypoint on the interpolated path Connecting step, the operation unit, a first local path between the first teaching point and the first halfway point in the first interpolation path, the second halfway point in the second interpolation path, and the second intermediate point The optimization calculation which optimizes the operation time of the robot to meet a predetermined physical constraint with respect to a path where a second local path to a second teaching point and a second local path with the second path are connected. And an optimization calculation step of obtaining the trajectory of the robot, and in the connection step, the operation unit is configured to calculate the first interpolation point from the first halfway point to the first auxiliary point. Applying a first weighting coefficient sequentially transitioning from 1 to 0 sequentially from a first interpolation point closer to one teaching point, and for each second interpolation point from the second auxiliary point to the second halfway point, From the first to the first in order from the second interpolation point closer to the second auxiliary point A second weighting coefficient that sequentially transitions from 0 to 1 so as to be a value obtained by subtracting the weighting coefficient, and a point obtained by multiplying the first interpolation point by the first weighting coefficient, and the second interpolation point to the second The connection path is generated by adding points obtained by multiplying two weighting factors to generate the connection path, and in the connection step, the operation unit is a function of the parameter, and the intermediate to the predetermined rank The first weighting coefficient is determined using a first weighting function in which the change in derivative value due to the variable is continuous, and the second weighting function using a second change in derivative value due to the parameter is continuous up to the predetermined rank And calculating the second weighting factor .
Further, according to the present invention, there is provided a trajectory generation method in which an operation unit generates a trajectory of a multijoint robot, wherein the operation unit is a function of a parameter and the derivative is differentiated by the parameter to a predetermined rank. A first interpolation path generation step of generating a first interpolation path from the first teaching point to the first auxiliary point where the change in value is continuous by the first interpolation method; and the operation unit is a function of the parameter A second interpolation route for generating a second interpolation route from the second auxiliary point to the second teaching point, in which the change of the differentiated value differentiated by the parameter to the predetermined rank is continuous, by the second interpolation method A generation step, and the calculation unit is a function of the parameter, the change of the differentiated value differentiated by the parameter to the predetermined rank being continuous from a first halfway point on the first interpolation path The connection route toward the second halfway point on the second interpolation route is Connecting step, the operation unit generates a first local path between the first teaching point and the first waypoint in the first interpolation path, and the second waypoint in the second interpolation path And the second local path between the second teaching point and the connection path by the optimization calculation which optimizes the operation time of the robot so as to keep a predetermined physical constraint with respect to the path connected by the connection path. And an optimization calculation step of determining the trajectory of the robot, wherein the first interpolation method and the second interpolation method respectively generate an interpolation path in a task space and a method of generating an interpolation path in a joint space In the connection step, the operation unit unifies the first interpolation path and the second interpolation path into paths in any one of the task space and the joint space, Said line And performing the generation of stitching combined path in the space.

  According to the present invention, since a robot path that is smooth with respect to a parameter (a change in differential value differentiated to a predetermined rank is continuous) is generated, a robot that traces a path combining two interpolation paths is It is possible to generate trajectories that do not stop and follow physical constraints.

It is an explanatory view showing a schematic structure of a robot apparatus concerning a 1st embodiment of the present invention. It is a schematic diagram which shows the robot of the robot apparatus which concerns on 1st Embodiment of this invention. It is a block diagram showing composition of a trajectory generating device concerning a 1st embodiment of the present invention. It is a graph which shows the weighting function in 1st Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the task space in 1st Embodiment of this invention. It is an explanatory view showing a course of a robot in joint space in a 1st embodiment of the present invention. It is a flowchart which shows the main steps of the track | orbit production | generation method which concerns on 1st Embodiment of this invention. It is a flowchart which shows the path | route production | generation process of the track | orbit production | generation method which concerns on 1st Embodiment of this invention. It is an explanatory view showing a course of a robot in task space in a 2nd embodiment of the present invention. It is an explanatory view showing a course of a robot in joint space in a 2nd embodiment of the present invention. It is a flowchart which shows the path | route production | generation process of the track | orbit production | generation method which concerns on 2nd Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the task space in 3rd Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the joint space in 3rd Embodiment of this invention. It is a flowchart which shows the path | route production | generation process of the track | orbit production | generation method which concerns on 3rd Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the task space in 4th Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the joint space in 4th Embodiment of this invention. It is a flowchart which shows the path | route production | generation process of the track | orbit production | generation method which concerns on 4th Embodiment of this invention.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

First Embodiment
FIG. 1 is an explanatory view showing a schematic configuration of a robot apparatus according to a first embodiment of the present invention. The robot apparatus 500 includes a multi-joint (two or more joints) robot 200, a trajectory generation apparatus 100 for generating a trajectory of the robot 200, and a robot controller 300 for controlling the operation of the robot 200 based on the generated trajectory. ing. In addition, the robot apparatus 500 includes an operation unit 410 that performs data input and the like to the trajectory generation apparatus 100 by an operation of a worker (user), and a display unit 420 that inputs display data from the trajectory generation apparatus 100 and performs image display. The operation device 400 is provided.

  The trajectory generation device 100 is configured by a computer including a central processing unit (CPU) 101 which is an operation unit, and a storage unit 106 which stores various data. The display unit 420 of the operation device 400 is a display, and is configured to display the robot arm 210 and the work space in 3D (three-dimensional) graphics so that the operator can visually recognize the generated motion trajectory. Although the case where the operation unit 410 and the display unit 420 are incorporated in one operation device 400 will be described, the operation unit 410 and the display unit 420 may be configured as independent devices. In addition, although the case where the operation unit 410 and the display unit 420 are separately configured will be described, the operation unit 410 and the display unit 420 are configured by an operation display unit capable of operation and display such as a touch panel display. It is also good.

  The CPU 101 functions as a kinematics calculation unit 31, a path generation unit 32, a path connection unit 33, a teaching point calculation unit 34, and an optimum speed calculation unit 35 by reading and executing a program described later.

  FIG. 2 is a schematic view showing a robot of the robot apparatus according to the first embodiment of the present invention. The robot 200 has a multijoint (two or more joints, two joints in the first embodiment) robot arm 210 and an end effector such as a robot hand 220 attached to the tip of the robot arm 210.

The robot arm 210 is, for example, a robot arm of two degrees of freedom configured by two joints (axes) J ₁ and J ₂ and links 201 and 202. There are various types of joints, such as a spherical joint as well as a rotary joint and a linear joint, but in the first embodiment, the joints J ₁ and J ₂ are rotation (turning) joints. The motion of the joints may be active with a power source such as a motor or passive with no power source, but in the first embodiment, the joints J ₁ and J ₂ are power sources Have. Robot arms are classified into serial link types in which joints and links are alternately connected in series, and parallel link types in which combinations of joints and links are parallel. , Serial link type with revolute joints. That is, the robot arm 210 is not particularly limited as long as it can be widely regarded as an operating body in which the robot arm itself operates. For example, a six-axis articulated robot arm or a direct acting robot arm may be used.

The joint angle of the two axes J ₁ and J ₂ of the robot arm 210 is θ ₁ and θ ₂ , respectively, with the parameter representing the degree of freedom of the robot arm 210 (robot 200) as the joint angle. The configuration of the robot arm 210 is represented by (θ ₁ , θ ₂ ), and can be regarded as one point on the joint space. As described above, when a parameter (for example, joint angle or expansion / contraction length) representing the degree of freedom of the robot arm 210 is a value of the coordinate axis, the configuration of the robot arm 210 can be expressed as a point on the joint space. That is, the joint space is a space having the joint angle of the robot arm 210 (robot 200) as a coordinate axis.

  In addition, a tool center point (TCP) is set at the tip of the robot 200, that is, the robot hand 220. TCP has three parameters (x, y, z) representing position and three parameters (α, β, γ) representing posture (rotation), ie, six parameters (x, y, z, α, β, It can be represented as γ) and can be regarded as one point on the task space. That is, the task space is a space defined by these six coordinate axes.

  FIG. 3 is a block diagram showing the configuration of a trajectory generation device 100 according to the first embodiment of the present invention. As shown in FIG. 3, a trajectory generation device 100 configured by a computer includes a CPU 101 as an arithmetic unit. The trajectory generation apparatus 100 further includes a read only memory (ROM) 102, a random access memory (RAM) 103, and a hard disk drive (HDD) 104 as the storage unit 106. Further, the trajectory generating apparatus 100 is provided with a recording disk drive 105 and various interfaces 111 to 113.

  The ROM 102, the RAM 103, the HDD 104, the recording disk drive 105, and the interfaces 111 to 113 are connected to the CPU 101 via the bus 120.

  The ROM 102 stores a boot program such as a BIOS. The RAM 103 temporarily stores various data such as the calculation processing result of the CPU 101. A program 140 is stored (recorded) in the HDD 104. Then, the CPU 101 reads out and executes the program 140 to execute each process of a trajectory generation method described later. That is, the CPU 101 functions as the units 31 to 35 shown in FIG. 1 by reading and executing the program 140. The recording disk drive 105 can read various data and programs recorded on the recording disk 141.

  The operating device 400 described above is connected to the interface 111. The interface 112 is connected to a rewritable non-volatile memory such as a USB memory or an external storage device 600 such as an external HDD. The robot controller 300 described above is connected to the interface 113.

  In FIG. 1, the kinematics calculation unit 31 calculates forward kinematics for obtaining the position and rotation (posture) of the TCP of the robot 200 from the joint angle information of the robot 200 (robot arm 210). Further, the kinematics calculation unit 31 calculates inverse kinematics for calculating each joint angle of the robot 200 (robot arm 210) from the position and rotation (posture) of the TCP of the robot 200.

For example, when performing linear interpolation on the basis of configurations (θ ₁ , θ ₂ ) given as teaching points, the kinematics calculation unit 31 sets the configuration to the position and rotation of TCP by forward kinematics. . Conversely, the kinematics calculation unit 31 is given the position and rotation of the TCP as the teaching point, and in the case of joint interpolation, the position and rotation of the TCP are configured by inverse kinematics.

  The path generation unit 32 generates a path (interpolated path) of the robot 200 that interpolates between the teaching points. The path of the robot 200 is the locus of the configuration of the robot 200 in the joint space or the locus of the TCP of the robot 200 in the task space. In other words, the path of the robot 200 is an ordered set of points (poses) in the joint space or task space. The trajectory of the robot 200 represents a pose (path) using time as a parameter. In the first embodiment, it is a set of joint command values (angle command values) of joints of the robot 200 at each time. Since there are a plurality of joints of the robot 200, the joint command values are synchronized at each joint.

  Interpolation methods to interpolate between teaching points include Spline interpolation, B-Spline interpolation, Bezier curve, linear interpolation, circular interpolation, joint interpolation and the like, and the position of robot 200 is indicated by the value of parameter s. . That is, the position (interpolation point) of the robot 200 is associated with the value of the parameter s, and a series of paths is formed by a set of these points. By these interpolation methods, an interpolation function using the parameter s as a parameter is obtained as a path (interpolation path) of the robot 200.

When taking into account the constraint of joint acceleration, it is necessary that the change of the derivative value obtained by differentiating the path up to the third order with respect to the parameter s be continuous. For example, in the case of a cubic B-Spline curve, the C ³ class (the value differentiated to the third order becomes continuous) for the parameter s.

  The optimal speed calculation unit 35 solves the optimization problem of minimizing the operation time of the robot 200 while keeping predetermined physical restrictions on the calculated path of the robot 200, and calculates the optimal speed of the robot 200. . In the first embodiment, the predetermined physical constraints are the constraint of joint torque of the robot 200, the constraint of joint angular velocity, the constraint of joint angular acceleration, the constraint of joint angular acceleration, the constraint of TCP velocity and the constraint of TCP acceleration. is there. The restriction of the TCP speed of the robot 200 is the restriction of the speed of the tip of the robot 200. The restriction of the TCP acceleration of the robot 200 is the restriction of the acceleration of the tip of the robot 200.

  In the first embodiment, although a case where all of these constraints are included in the predetermined physical constraints will be described, it is not limited thereto. The predetermined physical constraints include at least at least one constraint of joint angular velocity and TCP velocity. In addition to this, the predetermined physical constraints may include at least one constraint of joint torque constraint, joint angular acceleration constraint, joint angular jerk constraint and TCP acceleration constraint. If a path having a rank higher than or equal to the highest rank among the constraint values is generated, the model for calculating the computational constraint value becomes continuous, and the constraint values can be accurately calculated.

When obtaining the trajectory of the robot 200 on the condition of joint torque restriction, joint angular acceleration restriction, joint angular jerk restriction and TCP acceleration restriction, the derivative value obtained by differentiating the path by the first to third orders with the parameter s Use. Therefore, the path describes an example in which at least C ³ grade (change of third derivative was differentiated value continuous) needs to be to parametric s. That is, the predetermined physical constraint includes a constraint that is evaluated using a derivative value obtained by differentiating the path by the first to third orders with the parameter s, and in the first embodiment, the constraint of joint torque, joint angular acceleration Constraints, joint angle jerk constraints, and TCP acceleration constraints.

  As a method of solving the optimization problem, a known method may be used, for example, a method of substituting parameters to make the problem convex and using a log barrier method and a Newton method to rapidly converge on a global convergence solution is used. Just do it.

The path connection unit 33 provides connection sections (overlap sections) for each of the two interpolation paths, and multiplies points of the connection sections of the two interpolation paths by the weighting factor and adds up the two interpolation paths. smoothness (in this case C ³ grade smoothness) generating a path pieced together with.

An example of combining two interpolation paths of C ³ class with respect to the parameter s, which are respectively generated by joint interpolation and linear interpolation, while maintaining the smoothness of the C ³ class with respect to the parameter s, is described below. explain.

  In order to connect the interpolation path interpolated in the joint space smoothly and the interpolation path interpolated in the joint space with respect to the parameter s, a connection section is provided in each interpolation path, and the joint space interpolation path is gradually added to the task space interpolation path Need to change to When gradually changing, gradually changing the weight of the connection is essential for connecting smoothly.

As a condition of the weighting function, the first to third derivatives in the parameter s become 0 (including those that can be regarded as 0) at the start point and the end point of the interval of connection, and at least C of the parameter s Smoothness in grade ³ is required. Therefore, considering that the weighting by applying a sigmoid function which is a C ^∞ class.

  The sigmoid function is

である。しかし、このままでは、定義域が−∞≦ｘ≦∞となり扱いにくい。そこで、シグモイド関数とｔａｎ（ｘ）との合成関数を以下のように考える。

It is. However, as it is, the domain of definition is −− ≦ x ≦ ∞, which is difficult to handle. Therefore, the composite function of the sigmoid function and tan (x) is considered as follows.

  Furthermore,

とする。但し、ａは０より大きい実数である。

I assume. However, a is a real number greater than 0.

In this weighting function, assuming that a is sufficiently large, the first to third derivative values can be regarded as almost zero at the start point and the end point of the domain, so interpolation paths created by different interpolation methods are almost C ^It can be used for weighting that keeps grade ³ and connects smoothly.

FIG. 4 is a graph of the first weighting function f _sig1 (x) and the second weighting function f _sig2 (x) when a = 5, and FIG. 4 (a) is a graph of the first weighting function f _sig1 (x) FIG. 4B is a graph of the second weighting function f _sig2 (x).

  The first weighting function is a function that changes sequentially from 0 to 1 according to the increase of x, and the second weighting function is a function that changes sequentially from 1 to 0 according to the increase of x, and the same x value The sum of the first weighting factor of the first weighting function and the second weighting factor of the second weighting function may be 1. If it is such a function, the first weight function and the second weight function are not limited to the above functions, and polynomials or other functions may be used. In this case, the first weight function may be a function of the parameter s, and may be a continuous function differentiated by the parameter s up to a predetermined rank (for example, third rank). Also, the second weighting function may be a function of the parameter s, and may be a continuous function differentiated by the parameter s up to a predetermined rank (for example, third rank).

  FIG. 5 is an explanatory view showing the path of the robot in the task space in the first embodiment, and FIG. 6 is an explanatory view showing the path of the robot in the joint space in the first embodiment.

However, P ₂₁ and P ₂₂ in FIG. 5 are teaching points for generating a linear interpolation path, and Q ₁₁ and Q _{12 in} FIG. 6 are teaching points for generating a joint interpolation path. Further, the position and rotation of the TCP on the task space of Q ₁₁ and Q ₁₂ in FIG. 6 are respectively P ₁₁ and P ₁₂ in FIG. The configuration of the robot 200 (robot arm 210) when the TCP is in the position and rotation of P ₂₁ and P _{22 in} FIG. 5 is denoted by Q ₂₁ and Q _{22 in} FIG. 6, respectively.

Also, let q ₁ (s ₁ ) (0 ≦ s ₁ ≦ 1) in FIG. 6 be the configuration of the robot 200 on the joint interpolation path that is subjected to joint interpolation by the teaching points Q ₁₁ and Q ₁₂ on the joint space. . The configuration of the robot 200 for positioning the TCP on the linear interpolation path linearly interpolated by the teaching points P ₂₁ and P _{22 in the} task space is q ₂ (s ₂ ) (0 ≦ s ₂ ≦ 1) in FIG. Do. However, s ₁ and s ₂ are parameters of the joint interpolation path and the linear interpolation path, respectively.

Further, a vector representing the position and rotation of the TCP when the configuration of the robot 200 is q ₁ (s ₁ ) is p ₁ (s ₁ ) in FIG. A vector representing the position and rotation of TCP when the configuration of the robot 200 is q ₂ (s ₂ ) is p ₂ (s ₂ ) in FIG.

A teaching point P ₁₁ (or teaching point Q ₁₁ ) which is a first teaching point and a teaching point P ₂₂ (or teaching point Q ₂₂ ) which is a second teaching point are points which pass the robot 200. These two teaching points P ₁₁ and P ₂₂ (or teaching points Q ₁₁ and Q ₂₂ ) are stored directly in the storage unit 106 by the user operating the operation unit 410 or by the user operating the operation unit 410. Designate (set) the CPU 101.

On the other hand, the teaching point P ₁₂ (or teaching point Q ₁₂ ) which is the first auxiliary point and the teaching point P ₂₁ (or teaching point Q ₂₁ ) which is the second auxiliary point are points at which the robot 200 does not pass It is an important point. In the first embodiment, the teaching point P ₁₂ (or teaching point Q ₁₂ ) is a point set in advance, and the teaching point P ₂₁ (or teaching point Q ₂₁ ) is a point obtained by calculation.

Now, it is assumed that a joint interpolation path which is a first interpolation path obtained by joint interpolation is connected to a linear interpolation path which is a second interpolation path obtained by linear interpolation. Therefore, _let s ₁ = s _1s on the joint interpolation path (first interpolation path) be the start point of the joining section. When s ₁ = s ₁ s, the point is a halfway point (first halfway point) on the joint interpolation path. Also, let s ₁ = s _{1 e} on the joint interpolation path be the end point of the joining section.

Also, let s ₂ = s _2s on the linear interpolation path (second interpolation path) be the start point of the joining section, and let s ₂ = s _2e on the linear interpolation path be the end point of the joining section. When s ₂ = s _{2 e,} the point is a halfway point (second halfway point) on the linear interpolation path. The constants for simplicity _now, and _{s 1e = 1, s 2s =} 0, s 2e -s 2s = s 1e -s 1s clogging _s 2e = _{1s 1s.}

The user selects at least one of teaching points P _1a and P _2a (or teaching points Q _1a and Q _2a ) which are first and second waypoints which are boundaries between joining sections of the interpolation paths and sections other than the interpolation paths. Set (specify) parameter values of two halfway points. Based on the point, the teaching point calculation unit 34 backcalculates the teaching point and the connection interval used for path interpolation.

  The joint section in the first embodiment is a joint path in which joint interpolation and linear interpolation are jointed, and therefore it may not be a straight line and may not be a movement intended by the user.

  Therefore, the user designates (sets) a point on the linear interpolation path from which the user desires to secure straightness from here on the linear interpolation path. From that point, the teaching point calculation unit 34 calculates an appropriate teaching point used for path interpolation and a joining section.

In the first embodiment, the user designates (sets) the teaching point P ₂ a in FIG. 5 which can ensure the straightness, and performs back calculation from that point to calculate the teaching point P ₂₁ and the constant s _{2 e} by the teaching point calculation unit 34. Invert. Specifically, the teaching point P ₂₁ and the constant s _{2 e} are back-calculated so that the position and rotation p ₁ (s) of the TCP when the linear interpolation is the parameter s = s _{2 e} overlap the teaching point P ₂ a.

If the constant s _{2 e} is obtained, s _{2 e} = 1-s ₁ s, so the constant s ₁ s can be obtained.

In the first embodiment, only the adjustment of the teaching point and the joining section of the linear interpolation path has been described, but it is not limited thereto. Similarly, in the joint interpolation portion, the user can designate (set) the teaching point P _1a (teaching point Q _1a ) which is the boundary between the joining section and the section that is not so, and the user can designate the section for which the characteristic of joint interpolation is desired. You may do so.

s is a parameter of the path of the robot 200 in which two interpolation paths are connected. A point on the path of the robot 200 corresponds to the value of the parameter s. That is, when the value of the parameter s is determined, a point on the path of the robot 200 corresponding to the value is determined. In a first embodiment, the parameter s is a value ranging from 0 to 1 + s ₁ . By changing the value of the parameter s to increase according to the time from s = 0 to s = 1 + s ₁ , the TCP of the robot 200 moves on the path from the teaching point P ₁₁ to the teaching point P ₂₂ It becomes.

A local interval of 0 ≦ s <s _1s is a joint interpolation interval, a section of s _1s ≦ s <1 is a joining interval, and a local interval of 1 ≦ s <1 + s _1s is a linear interpolation interval. The path q (s) on the joint space when two interpolation paths at this time are connected is as follows.

Here, f _sig1 ((s−s _1s ) / (1−s _1s )) is a first weighting function, and f _sig2 ((s−s _1s ) / s _2e ) is a second weighting function. By substituting numerical values into s of these functions, the first weighting factor and the second weighting factor can be obtained.

By using q (s) which maintains smoothness in C ³ class with respect to the parameter s, it is possible to obtain continuous q ′ (s), q ′ ′ (s), q ′ ′ ′ (s). In addition, using q (s), after performing an optimal velocity calculation (optimization calculation to optimize the operation time of the robot 200), based on it, a joint command value (that is, a trajectory) for each fixed time can be generated It becomes possible.

  According to the equation (5), the equation for calculating the point on the joining route in the joining section in FIG. 6 can be written as follows using the value on the graph of FIG.

Here, q ₁₁ , q ₁₂ , q ₁₃ , q ₁₄ and q ₁₅ are from teaching point Q _{1 a} to teaching point Q ₁₂ on the joint interpolation path q ₁ (s ₁ ) which is the first interpolation path in FIG. Of the plurality of interpolation points (first interpolation points). Further, q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ and q ₂₅ are the second interpolation path from the teaching point Q ₂₁ on the linear interpolation path q ₂ (s ₂ ) to the teaching point Q ₂ a in FIG. It is a plurality of interpolation points (second interpolation points).

  FIG. 7 is a flowchart showing main steps of the trajectory generation method according to the first embodiment. Hereinafter, each process of the trajectory generation method of the robot 200 performed by the CPU 101 of the trajectory generation apparatus 100 will be described.

  First, the CPU 101 reads from the storage unit 106 (for example, the HDD 104) a model of the robot 200 and constraint conditions (predetermined physical constraints) in a work space in which the robot 200 performs work (S101).

  The model of the robot 200 is design information of parts constituting the robot 200, and includes the moment of inertia, mass, position, rotation, number of axes, etc. of each link. Constraint conditions define physical constraints of the robot 200, such as constraint of joint torque of the robot 200, constraint of joint angular velocity, constraint of joint angular acceleration, constraint of joint angular acceleration, constraint of TCP velocity, and TCP acceleration. It is a constraint.

  Next, the CPU 101 acquires the teaching point of the robot 200 and the interpolation method (S102). For example, the teaching point of the robot 200 can be determined by the position and rotation of the TCP, or the joint angle. At the same time as the teaching point, information on how to move to the teaching point is read by what interpolation method.

The teaching points acquired by the CPU 101 in the first embodiment are a teaching point P ₁₁ (or teaching point Q ₁₁ ) which is a first teaching point, a teaching point P ₁₂ (or teaching point Q ₁₂ ) which is a first auxiliary point, a first 2 teaching point P ₂₂ (or teaching point Q ₂₂ ) which is a teaching point.

  In the first embodiment, since the user designates joint interpolation and linear interpolation, the interpolation method acquired by the CPU 101 in the first embodiment is joint interpolation as the first interpolation method, and linear interpolation as the second interpolation method. It is. The teaching points and the interpolation method are designated (set) by a robot program stored in the storage unit 106 (for example, the HDD 104).

Next, the CPU 101 sets (designates) at least one of the teaching points P _1a and P _2a , which are halfway points, in the first embodiment, the teaching point P _2a in the first embodiment, and sets a joining section (S103: Setting process). In the first embodiment, a section of s ₁ s s ₁ ≦ ₁ of joint interpolation and a section of 0 <s ₂ <s _{2 e} of linear interpolation are combined sections. The joining section is optional, but if straightness is ensured from the teaching point specified by the user, the teaching point calculation unit 34 adjusts the teaching point and the joining section used for linear interpolation path generation accordingly .

Specifically, the constant s 1 _e is set to 1, and the constant s ₂ s is set to 0. That is, the constants s 1 _e and s ₂ s are stored in advance in the storage unit 106, and the CPU 101 reads out the settings from the storage unit 106.

The user sets (designates) a teaching point P _2a (parameter value) which is a second halfway point on the second interpolation path from the second auxiliary point to the second teaching point. Thus, the CPU 101 calculates the teaching point P ₂₁ which is the second auxiliary point and the constants s _{2 e} and s ₁ s. In the first embodiment, the teaching point P ₁ a, which is the first halfway point on the first interpolation route from the first teaching point to the first auxiliary point, is stored in advance in the storage unit 106 and stored by the CPU 101. Read from the unit 106 and set.

As described above, in step S103, connection sections, that is, constants s _1s and s _2e are set (initialized).

Next, the CPU 101 executes path generation processing for generating a path q (s) of the robot 200 in which the joint interpolation path, which is the first interpolation path, and the linear interpolation path, which is the second interpolation path, are joined (S104) . Specifically, in FIG. 6, first between the teaching points Q ₁₁ in the joint interpolation path and the first local path between teaching points Q _1a, the teaching point Q _2a in the linear interpolation path and teaching point Q ₂₂ A path q (s) is generated in which two local paths are connected by a connection path of connection sections. Here, the first local path is q ₁ (s) (where 0 ≦ s <s _1s ). The second local path is q ₂ (s−s _1s ), where 1 ≦ s <1 + s _1s . The connection path is q ₁ (s) × f _{sig 1} ((s−s _1s ) / (1−s _1s )) + q ₂ (s−s _1s ) × f _{sig 2} ((s−s _1s ) / s _2e ) (However, s ₁ s ≦ s <1) (refer to equation (5)).

Next, the CPU 101 determines an evaluation point on a path q (s) connecting two interpolation paths to be subjected to speed optimization (S105). For example, a row of s equally divided by a predetermined Δs from s = 0 to s = 1 + s _1s is used as an evaluation point.

  Next, the CPU 101 uses the interpolation function, that is, the path q (s) to perform q '(s), q' '(s), q' '' (s) at the evaluation point by numerical differentiation with respect to the parameter s. (S106).

  Next, the CPU 101 uses q '(s), q' '(s), q' '' (s) to perform joint torque constraint, joint angular velocity constraint, joint angular acceleration constraint, joint angular jerk constraint, TCP velocity constraint And the optimum speed for moving in the shortest time on the condition of the constraints of the TCP acceleration constraint (S107).

  Next, the CPU 101 calculates s (t) from s (·) (s) calculated by the optimum velocity calculation as in equation (14), and calculates a joint command value for every constant time Δt from q (s) The (angle command value) is determined (S108). However, s (0) = 0.

  The above steps S105 to S108 are the optimization calculation process. That is, the value of the parameter s corresponding to the time (ie, s) is calculated by the optimization calculation in which the operation time of the robot 200 is optimized to keep the predetermined physical constraints on the route q (s) in steps S105 to S108. The trajectory of the robot 200 is obtained by obtaining (t).

If q (s) having C ³ class smoothness with respect to parameter s is used, q ′ (s), q ′ ′ (s), q ′ ′ ′ (s) become continuous, and the joint command to the robot 200 Errors in generating values can be significantly reduced.

  Next, the route generation process of step S104 will be specifically described. FIG. 8 is a flow chart showing a route generation process in step S104 of FIG. 7 for inputting the parameter s and outputting the configuration q (s).

First, the CPU 101 acquires (inputs) the numerical value of the parameter s (S201). The CPU 101 carries out the classification by the numerical value of the input parameter s (S202). That is, the CPU 201 determines the numerical value of the parameter s in this step S202. In the case of 0 ≦ s <s _1s , it is a joint interpolation interval, in the case of s _1s ≦ s <1, it is a joint interval of joint interpolation and linear interpolation, and in the case of 1 ≦ s <1 + s _1s , it is a linear interpolation interval.

In the case where 0 ≦ s <s _{1s in} the determination of step S202 (when the interpolation method is joint interpolation), the CPU 101 performs joint interpolation with the teaching points Q ₁₁ and Q _{12 and} uses a joint interpolation path as a function of the parameter s. A certain q ₁ (s) is obtained (S203). That is, the CPU 101 is a function of the parameter s, and the joint interpolation path q ₁ (s) from the teaching point Q ₁₁ to the teaching point Q ₁₂ is continuous, with the change of the differentiated value differentiated by a predetermined order (for example, third order) being continuous. , And by joint interpolation (first interpolation path generation step). In the section of 0 ≦ s <s _1s , the first local path q ₁ (s) between the teaching point Q ₁₁ and the teaching point Q _1a in the first interpolation path is the path of the robot 200, and q (s) ) = Q ₁ (s). The CPU 101 outputs the configuration q (s) (S204).

If it is determined in step S202 that s ₁ s ≦ s <1 (in the case of a joint section of joint interpolation and linear interpolation), the CPU 101 performs joint interpolation using the teaching points Q ₁₁ and Q ₁₂ to obtain q ₁ (s) (S205: first interpolation path generation step).

The CPU 101 linearly interpolates TCP using the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S 206). That is, the CPU 101 generates a linear interpolation path p ₂ (s-s _1s ) which is a function of the parameter s and is a second interpolation path from the teaching point P ₂₁ to the teaching point P ₂₂ by joint interpolation (second Interpolation path generation process). The linear interpolation path p ₂ (s-s _1s ) generated in this step S206 is a function of the parameter s in which the change of the derivative value differentiated by the parameter s is continuous up to a predetermined rank (for example, third rank).

The CPU 101 performs inverse kinematics calculation on p ₂ (s−s _1s ) by the kinematics calculation unit 31 to obtain configuration q ₂ (s−s _1s ) (S 207). That is, the CPU 101 unifies the joint interpolation path and the linear interpolation path into paths in one of the task space and the joint space (the joint space in the first embodiment).

Next, the CPU 101 calculates the first weighting factor from the first weighting function f _sig1 ((s−s _1s ) / (1−s _1s )) for weighting the configuration q ₁ (s) _subjected to joint interpolation. It asks for (S208). Further, the CPU 101 obtains a second weighting factor from a second weighting function f _sig2 ((s−s _1s ) / s _2e ) for weighting the configuration q ₂ (s) _subjected to linear interpolation (S 209).

The CPU 101 _multiplies q ₁ (s) by f _{sig 1} ((s−s _1s ) / (1−s _1s )), and q ₂ (s−s _1s ) to f _{sig 2} ((s−s _1s ) / s _2e ) And sums to obtain a joint path q (s) (where s ₁ s s s ₁ ) (S 210).

q ₁ (s) (where s ₁ s s <1) is a set of first interpolation points q ₁₁ , q ₁₂ , q ₁₃ , q ₁₄ and q ₁₅ shown in equations (6) to (10). The CPU 101 multiplies the respective interpolation points q ₁₁ , q ₁₂ , q ₁₃ , q ₁₄ and q ₁₅ by the respective first weight coefficients r ₁₁ , r ₁₂ , r ₁₃ , r ₁₄ and r ₁₅ .

Similarly, q ₂ (s−s _1s ) (where s _1s ≦ s <1) is the second interpolation point q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ , q shown in equations (6) to (10). It is a set of ₂₅ . The CPU 101 multiplies the respective interpolation points q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ and q ₂₅ by the respective second weighting factors r ₂₁ , r ₂₂ , r ₂₃ , r ₂₄ and r ₂₅ . _Here, an _{r 11 + r 21 = 1,} r 12 + r 22 = 1, r 13 + r 23 = 1, r 14 + r 24 = 1, r 15 + r 25 = 1.

The first weight coefficients r ₁₁ , r ₁₂ , r ₁₃ , r ₁₄ and r ₁₅ sequentially transition from 1 to 0 as the parameter s increases. That is, r ₁₁ (= 1)> r ₁₂ > r ₁₃ > r ₁₄ > r ₁₅ (= 0).

In addition, each second weighting factor r ₂₁ , r ₂₂ , r ₂₃ , r ₂₄ , r ₂₅ is a value obtained by subtracting one first weighting factor r ₁₁ , r ₁₂ , r ₁₃ , r ₁₄ , r ₁₅ from one. As the parameter s increases, the transition from 0 to 1 sequentially occurs. That is, r ₂₁ (= 0) <r ₂₂ <r ₂₃ <r ₂₄ <r ₂₅ (= 1).

That is, the weight CPU101, in step S210, the relative interpolation point _{q 11, q 12, q 13} , q 14, q 15, in this order from the side of the interpolation point close to the teaching point _{Q 11,} sequentially changes from 1 to 0 The coefficients r ₁₁ , r ₁₂ , r ₁₃ , r ₁₄ and r ₁₅ are multiplied. In addition, the CPU 101 changes the weighting factor r ₂₁ , which sequentially transitions from 0 to 1 sequentially from the interpolation point closer to the teaching point Q ₂₁ with respect to the interpolation points q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ , and q ₂₅ . Multiply r ₂₂ , r ₂₃ , r ₂₄ and r ₂₅ . The CPU 101 adds the point obtained by multiplying the interpolation point q _1n (n = 1 to 5, the same applies hereinafter) by the weighting factor r _1n and the point obtained by multiplying the interpolation point q _2n by the weighting factor r _2n , And generates a connection path q ₁ , q ₂ , q ₃ , q ₄ , q ₅ (FIG. 7, formulas (6) to (10)).

By the above steps S206 to S210, the CPU 101 is a function of the parameter s, and from the teaching point Q _1a , the change of the differentiated value differentiated by the parameter s to the predetermined rank (for example, third rank) is continuous _Create a joining route towards Q _2a (joining process). The CPU 101 outputs the configuration q (s) (S211).

Incidentally, the weighting coefficient _{r 1n, r 2n,} the step S208, the description has been given of the case of obtaining the weighting function at S209, is not limited to this, pre-weighting factors _{r 1n,} sure you calculate the _{r 2n} storage unit It may be stored in 106. At that time, the CPU 101 only needs to read out the data of the weight coefficients r _1n and r _2n from the storage unit 106 without performing the calculation in steps S 208 and S 209.

Next, in the case where 1 ≦ s <1 + s _{1s in} the determination of step S202 (in the case where the interpolation method is linear interpolation), the CPU 101 linearly interpolates TCP by the teaching points P ₂₁ and P ₂₂ and p ₂ (s−s _1s ) (S212: second interpolation path generation step).

CPU101 _is, p 2 to _{(s-s 1s),} performs an inverse kinematics calculation, obtaining a _q 2 _{(s-s 1s)} is a configuration of the joint space (S213). In the section of 1 ≦ s <1 + s _1s , the second local path q ₂ (s−s _1s ) between the teaching point Q _{2 a} and the teaching point Q ₂₂ in the second interpolation path is the path of the robot 200, a _{q (s) = q 2 (} s-s 1s). The CPU 101 outputs the configuration q (s) (S214).

The above steps S201 to S214 are repeatedly performed while changing the numerical value of the parameter s from 0 to 1 + s _1s . These steps S201～S214, shown in Equation (5), ^{C 3} class path q (s) is obtained in the joint space of the robot 200.

As described above, according to the first embodiment, when the robot 200 is combined connecting the two interpolation paths differ extensively joint interpolation and linear interpolation path in a smooth joint space of the robot 200 in C ³ tertiary respect parametric s q ( s) is generated. As a result, it is possible to generate a trajectory that complies with a physical constraint without the robot 200 stopping following a route q (s) obtained by combining two interpolation routes.

In particular, in step S210, CPU 101 _may calculate the _{q 1n × r 1n + q 2n} × r 2n (n = 1~5), and generates a tie combined path _{q n.} By this, the connection path is generated as a smooth path of C ³ class with respect to the parameter s. In the first embodiment, n is set to 1 to 5 for convenience of explanation, but the present invention is not limited to this. The greater the number of n, the better the accuracy, so it is better to generate as many points as joining paths. In the first embodiment, since the first and second weighting factors are calculated based on the first and second weighting functions, it is not necessary to store a large amount of weighting factor data in the storage unit 106 in advance. Therefore, the memory area of the storage unit 106 is not compressed, and the memory area can be effectively used.

Further, according to the first embodiment, since the user sets (designates) the teaching point P _2a (Q _2a ), the user needs a section that requires straightness without being aware of the connection section of the route. It can be specified and has the effect of improving usability.

Second Embodiment
A trajectory generation method according to a second embodiment of the present invention will be described. The configuration of the trajectory generation device, that is, the robot device is the same as that of the first embodiment, and the description thereof will be omitted. In the second embodiment, a combination of interpolation methods different from the first embodiment, that is, two types of interpolation methods, arc interpolation and linear interpolation, are specified.

  FIG. 9 is an explanatory view showing the path of the robot arm in the task space in the second embodiment, and FIG. 10 is an explanatory view showing the path of the robot arm in the joint space in the second embodiment.

In the second embodiment, the first interpolation method is circular interpolation, and the second interpolation method is linear interpolation as in the first embodiment. Assuming that the configuration when circular interpolation is performed is q ₁ (s), as in the first embodiment, connection can be performed while maintaining the smoothness of the path according to equation (5). According to the equation (5), the equation for calculating the points q _{1 to} q ₅ on the joining route in the joining section in FIG. 10 is the same as the first embodiment, the equation (6) to the equation (10) become.

Unlike the first embodiment, since the interpolation path subjected to the circular interpolation becomes the TCP path on the task space, reverse kinematics is performed in order to join them on the joint space, and the configuration is obtained to obtain the circular interpolation path. q Calculate ₁ (s).

  Taking the above into consideration, the flow for obtaining the configuration q (s) from the parameter s will be described. FIG. 11 is a flowchart of a path generation process which inputs the parameter s and outputs the configuration q (s). In the second embodiment, since the first interpolation method is a method of generating an interpolation path in the task space, the arc interpolation path which is the first interpolation path obtained is converted to a path in the joint space by inverse kinematics calculation. The second embodiment differs from the first embodiment in the point. Except for this point, the process is the same as that of the first embodiment, and hence the respective steps relating to the route generation process will be briefly described.

First, the CPU 101 acquires (inputs) the parameter s (S301). The CPU 101 performs case division in the section of the input parameter s (S302). In the case of 0 ≦ s <s _1s , it is an arc interpolation section, in the case of s _1s ≦ s <1, it is a joining section of arc interpolation and linear interpolation, and in the case of 1 ≦ s <1 + s _1s , it is a linear interpolation section.

In the case where 0 ≦ s <s _{1s in} the determination of step S302, the CPU 101 performs circular interpolation of the TCP by the teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S303). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S304)} . In the section of 0 ≦ s <s _1s , the first local path q ₁ (s) is the path of the robot 200, and q (s) = q ₁ (s). The CPU 101 outputs the configuration q (s) (S305).

Next, when s ₁ s s ≦ _{1 in} the determination of step S 302, the CPU 101 first interpolates TCP by using teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S 306). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S307)} .

The CPU 101 linearly interpolates TCP by the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S 308). CPU101 _is, p 2 to _{(s-s 1s),} it performs an inverse kinematics calculation, obtaining the configuration _{q 2 (s-s 1s)} (S309).

The CPU 101 _obtains a weighting function f _sig1 ((s−s _1s ) / (1−s _1s )) for weighting the circularly interpolated configuration q ₁ (s) (S 310). The CPU 101 _obtains a weighting function f _sig2 ((s−s _1s ) / s _{2 e} ) for weighting the configuration q ₂ (s) _subjected to linear interpolation (S ₃₁₁ ).

The CPU 101 _multiplies q ₁ (s) by f _{sig 1} ((s−s _1s ) / (1−s _1s )), and q ₂ (s−s _1s ) to f _{sig 2} ((s−s _1s ) / s _2e ) And sum to obtain q (s) (S312). The CPU 101 outputs the configuration q (s) (S313).

If 1 ≦ s <1 + s ₁ s in the determination of step S302, the CPU 101 linearly interpolates TCP by the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S314). CPU101 _is, p 2 to _{(s-s 1s),} performs an inverse kinematics calculation, obtaining a _q 2 _{(s-s 1s)} is a configuration of the joint space (S315). In the section of 1 ≦ s <1 + s _1s , the second local path q ₂ (s−s _1s ) is the path of the robot 200, and q (s) = q ₂ (s−s _1s ). The CPU 101 outputs the configuration q (s) (S316).

As described above, since the configuration q (s) obtained by joining the circular interpolation and the linear interpolation while maintaining the C ³ class can be obtained, it is possible to calculate the optimum velocity by the flow of FIG. 7 as in the first embodiment. it can.

Further, in step S103 in FIG. 7, the user designates (sets) points P _1a and P _2a (or points Q _1a and Q _2a ) for which it is desired to secure the arc nature of the arc interpolation path and the straightness of the linear interpolation path. . The CPU 101 functioning as the teaching point calculation unit 34 back-calculates the teaching points and joining sections of the circular arc interpolation and the linear interpolation based on the points P _1a and P _2a (or the points Q _1a and Q _2a ).

Since the joining section is a path in which the circular interpolation and the linear interpolation are connected, it may not be a pure circular arc or a straight line, and may not be a movement intended by the user. Therefore, the user secures the circularity so far and designates the point that he wants to secure the straightness, and then the teaching point calculation unit 34 selects the appropriate teaching points P ₁₂ and P ₂₁ (or teaching points). Calculate Q ₁₂ and Q ₂₁ ).

That is, in the second embodiment, the user designates a point P _1a on FIG. 9 for securing the circularity and a point P _2a on FIG. 9 for securing the straightness. The CPU 101 performs an inverse calculation therefrom to obtain appropriate P ₁₂ , P ₂₁ , s _1s , s _2e . Specifically, circular interpolation _path, s 1 _{= s} position and rotation of the TCP in the case of _{1s p} 1 is _(s), it overlaps the _{P 1a,} the position of the TCP at the time of the linear interpolation path _s 2 _{= s 2e} and rotation _{p 2 (s} 2) _{is, P} 12 so as to overlap with the _{P 21, P 21, s 1s} , calculated back to _{s 2e.}

  As described above, according to the second embodiment, in addition to the effects of the first embodiment, even when the interpolation method attached to the teaching point is switched from circular interpolation to linear interpolation, it is stopped at the time of switching the route. It is possible to generate an optimal trajectory that complies with various physical constraints of the robot 200 without the need to do so. Further, according to the second embodiment, since the user can designate a section requiring arcing and rectilinearity without being aware of the connection section of the route, there is an effect that usability is improved.

Third Embodiment
A trajectory generation method according to a third embodiment of the present invention will be described. The configuration of the trajectory generation device, that is, the robot device is the same as that of the first and second embodiments, and the description will be omitted. In the third embodiment, a combination of interpolation methods different from the first and second embodiments, that is, two types of interpolation methods, arc interpolation and joint interpolation, are specified.

  FIG. 12 is an explanatory view showing the path of the robot arm in the task space in the third embodiment, and FIG. 13 is an explanatory view showing the path of the robot arm in the joint space in the third embodiment.

In the third embodiment, the first interpolation method is circular interpolation, and the second interpolation method is joint interpolation. Assuming that the configuration (interpolation path) when performing the circular arc interpolation is q ₁ (s) and the configuration (interpolation path) when performing the joint interpolation is q ₂ (s), the equation (5) is obtained as in the first embodiment. ) Enables connection that maintains the smoothness of the path. According to the equation (5), the equations for calculating the points q _{1 to} q ₅ on the path in the joining section in FIG. 13 are the equations (6) to (10) as in the first embodiment. .

As in the second embodiment, since the arc-interpolated path becomes a TCP path on the task space, reverse kinematics is performed in order to connect them on the joint space, and the configuration is determined to obtain the arc-interpolated path q. Ask for ₁ (s). Taking the above into consideration, the flow for obtaining the configuration q (s) from the parameter s will be described.

  FIG. 14 is a flowchart of a path generation process which inputs the parameter s and outputs the configuration q (s). In the third embodiment, the second interpolation method is a method of generating an interpolation path in a joint space, and thus differs from the second embodiment in that a joint interpolation path which is a second interpolation path is determined in the joint space. . Other than that is the same as that of the second embodiment, and therefore, the respective steps relating to the route generation processing will be briefly described below.

First, the CPU 101 acquires (inputs) the parameter s (S401). The CPU 101 performs case division in the section of the input parameter s (S402). In the case of 0 ≦ s <s _1s , it is an arc interpolation interval, in the case of s _1s ≦ s <1, it is a joint interval of arc interpolation and joint interpolation, and in the case of 1 ≦ s <1 + s _1s , it is a joint interpolation interval.

In the case where 0 ≦ s <s _{1s in} the determination of step S402, the CPU 101 performs circular interpolation of the TCP by the teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S403). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S404)} . In the section of 0 ≦ s <s _1s , the first local path q ₁ (s) is the path of the robot 200, and q (s) = q ₁ (s). The CPU 101 outputs the configuration q (s) (S405).

Next, in the case where s ₁ s s ≦ _{1 in} the determination of step S 402, the CPU 101 first interpolates TCP by using teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S 406). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S407)} .

The CPU 101 performs joint interpolation based on the teaching points Q ₂₁ and Q ₂₂ to obtain q ₂ (s−s _1s ) (S 408).

The CPU 101 _obtains a weighting function f _sig1 ((s−s _1s ) / (1−s _1s )) for weighting the circularly interpolated configuration q ₁ (s) (S 409). The CPU 101 _obtains a weighting function f _sig2 ((s−s _1s ) / s _{2 e} ) for weighting the configuration q ₂ (s) _subjected to linear interpolation (S 410).

The CPU 101 _multiplies q ₁ (s) by f _{sig 1} ((s−s _1s ) / (1−s _1s )), and q ₂ (s−s _1s ) to f _{sig 2} ((s−s _1s ) / s _2e ) And sum to obtain q (s) (S411). The CPU 101 outputs the configuration q (s) (S412).

In the case where 1 ≦ s <1 + s ₁ s in the determination of step S402, the CPU 101 performs joint interpolation based on the teaching points Q ₂₁ and Q ₂₂ to obtain q ₂ (s−s _1s ) (S413). In the section of 1 ≦ s <1 + s _1s , the second local path q ₂ (s−s _1s ) is the path of the robot 200, and q (s) = q ₂ (s−s _1s ). The CPU 101 outputs the configuration q (s) (S414).

As described above, since the configuration q (s) obtained by joining the circular interpolation and the joint interpolation while maintaining the C ³ class can be obtained, it is possible to calculate the optimum velocity by the flow of FIG. 7 as in the first embodiment. it can.

Further, in step S103 of FIG. 7, the user designates (sets) the point P _1a (or the point Q _1a ) for which it is desired to secure the circularity of the circular arc interpolation path. The CPU 101 functioning as the teaching point calculation unit 34 performs back calculation on the teaching point and the joining section of the arc interpolation and the joint interpolation based on the point P _1a (or the point Q _1a ).

Since the joining section is a path in which the arc interpolation and the joint interpolation are joined, it may not be a pure arc and may not be the movement intended by the user. Therefore, the user specifies that the user wants to secure circularity so far. From there, the teaching point calculation unit 34 calculates an appropriate teaching point P ₁₂ (or teaching point Q ₁₂ ).

That is, in the third embodiment, the user designates a point P _{1 a} on FIG. 12 for securing the circularity. The CPU 101 performs an inverse operation to obtain appropriate P ₁₂ and s _1s . Specifically, P ₁₂ and s _1s are back-calculated so that the position and rotation p ₁ (s ₁ ) of the TCP when s ₁ = s _1s overlap the circular interpolation path with P _1a .

  As described above, according to the third embodiment, in addition to the effects of the first embodiment, even when the interpolation method attached to the teaching point switches from arc interpolation to joint interpolation, stopping at the time of path switching is halted. It is possible to generate an optimal trajectory that complies with various physical constraints of the robot 200 without the need to do so. Further, according to the third embodiment, since the user can specify a section requiring the circularity without being aware of the connection section of the route, there is an effect that usability is enhanced.

Fourth Embodiment
A trajectory generation method according to a fourth embodiment of the present invention will be described. The configuration of the trajectory generation device, that is, the robot device is the same as that of the first, second, and third embodiments, and the description thereof will be omitted. In the fourth embodiment, a combination of interpolation methods different from the first, second, and third embodiments, that is, linear interpolation and an interpolation method of linear interpolation are specified.

  FIG. 15 is an explanatory view showing the path of the robot arm in the task space in the fourth embodiment, and FIG. 16 is an explanatory view showing the path of the robot arm in the joint space in the fourth embodiment.

In the fourth embodiment, the first interpolation method is a first linear interpolation, and the second interpolation method is a second linear interpolation. Assuming that the configuration when the first linear interpolation is performed is q ₁ (s) and the configuration when the second linear interpolation is performed is q ₂ (s), as in the first embodiment, according to Equation (5), It is possible to perform connection while maintaining the smoothness of the path. According to the equation (5), the equations for calculating the points q _{1 to} q ₅ on the path in the joining section in FIG. 16 are the equations (6) to (10) as in the first embodiment. .

As in the first embodiment, since the linearly interpolated route is the TCP route in the task space, reverse kinematics is performed in order to join them in the joint space, and the configuration is determined to obtain the linear interpolated route q. Find ₁ (s), q ₂ (s−s _1s ).

  Taking the above into consideration, the flow for obtaining the configuration q (s) from the parameter s will be described. FIG. 17 is a flowchart of a path generation process which inputs the parameter s and outputs the configuration q (s). In the fourth embodiment, since the first interpolation method is a method of generating an interpolation path in task space, the linear interpolation path which is the first interpolation path obtained is converted to a path in joint space by inverse kinematics calculation. The second embodiment differs from the first embodiment in the point. Except for this point, the process is the same as that of the first embodiment, and hence the respective steps relating to the route generation process will be briefly described.

First, the CPU 101 acquires (inputs) the parameter s (S501). The CPU 101 performs case division in the section of the input parameter s (S502). In the case of 0 ≦ s <s _1s , the first linear interpolation section, in the case of s _1s ≦ s <1, the joining section of the first linear interpolation and the second linear interpolation, in the case of 1 ≦ s <1 + s _1s This is a second linear interpolation interval.

In the case where 0 ≦ s <s _{1s in} the determination of step S502, the CPU 101 linearly interpolates TCP by the teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S503). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S504)} . In the section of 0 ≦ s <s _1s , the first local path q ₁ (s) is the path of the robot 200, and q (s) = q ₁ (s). The CPU 101 outputs the configuration q (s) (S505).

Next, in the case where s ₁ s ≦ s <1 in the determination of step S 502, the CPU 101 first interpolates TCP by teaching points P ₁₁ and P ₁₂ linearly to obtain p ₁ (s) (S 506). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S507)} .

The CPU 101 linearly interpolates TCP using the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S 508). CPU101 _is, p 2 to _{(s-s 1s),} it performs an inverse kinematics calculation, obtaining the configuration _{q 2 (s-s 1s)} (S509).

The CPU 101 _obtains a weighting function f _sig1 ((s−s _1s ) / (1−s _1s )) for weighting the linearly interpolated configuration q ₁ (s) (S 510). The CPU 101 _obtains a weighting function f _sig2 ((s−s _1s ) / s _{2 e} ) for weighting the configuration q ₂ (s) linearly interpolated (S 511).

The CPU 101 _multiplies q ₁ (s) by f _{sig 1} ((s−s _1s ) / (1−s _1s )), and q ₂ (s−s _1s ) to f _{sig 2} ((s−s _1s ) / s _2e ) And sum to obtain q (s) (S512). The CPU 101 outputs the configuration q (s) (S513).

If 1 ≦ s <1 + s ₁ s in the determination of step S502, the CPU 101 linearly interpolates TCP by the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S514). CPU101 _is, p 2 to _{(s-s 1s),} performs an inverse kinematics calculation, obtaining a _q 2 _{(s-s 1s)} is a configuration of the joint space (S515). In the section of 1 ≦ s <1 + s _1s , the second local path q ₂ (s−s _1s ) is the path of the robot 200, and q (s) = q ₂ (s−s _1s ). The CPU 101 outputs the configuration q (s) (S516).

  According to the fourth embodiment, in addition to the effect of the first embodiment, even when the interpolation method attached to the teaching point is switched from the first linear interpolation to the second linear interpolation, it is not stopped at the switching of the path. In addition, it is possible to generate an optimal trajectory that complies with various physical constraints of the robot 200. This is advantageous in that optimal trajectories can be generated that comply with various physical constraints, as compared to the linear interpolation to linear interpolation method described in the background art. Further, according to the fourth embodiment, since the user can specify a section requiring straightness without being aware of the connection section of the route, there is an effect that usability is increased.

  The present invention is not limited to the embodiments described above, and many modifications can be made within the technical concept of the present invention.

  In the above embodiment, although the connection in the joint space has been described, the present invention is not limited thereto, and the same connection may be performed in the task space. For example, similar weighting functions may be used to join the paths for TCP positions and rotations. In this case, finally, the path on the task space may be converted into a path on the joint space to generate a trajectory.

  Moreover, although the case where the track | orbit production | generation apparatus 100 was comprised by one computer was demonstrated in the said embodiment, this invention is applicable also when it is comprised by several computer. In this case, the computing unit of the present invention may be configured by a plurality of CPUs. Moreover, although the case where the track | orbit production | generation apparatus 100 and the robot controller 300 were comprised by another computer was demonstrated, you may be comprised by one computer. In this case, the CPU of the computer generates a path of the robot and controls the operation of the robot according to the generated path.

  Specifically, each processing operation of the above embodiment is executed by the CPU 101. Therefore, the recording medium recording the program for realizing the functions described above is supplied to the track generation device 100, and the computer (CPU or MPU) that configures the track generation device 100 reads out and executes the program stored in the recording medium. It may be done. In this case, the program itself read from the recording medium implements the functions of the above-described embodiments, and the program itself and the recording medium recording the program constitute the present invention.

  In the above embodiment, the case where the computer readable recording medium is the HDD 104 and the program 140 is stored in the HDD 104 has been described. However, the present invention is not limited to this. The program 140 may be recorded on any recording medium as long as it is a computer readable recording medium. For example, as a recording medium for supplying a program, the ROM 102, the external storage device 600, the recording disk 141, etc. shown in FIG. 3 may be used. A specific example will be described. A flexible disk, a hard disk, an optical disk, a magneto-optical disk, a CD-ROM, a CD-R, a magnetic tape, a non-volatile memory card, a ROM or the like can be used as a recording medium. Further, the program in the above embodiment may be downloaded via a network and executed by a computer.

  Further, the present invention is not limited to the implementation of the functions of the above embodiments by executing the program code read by the computer. The case where the OS (Operating System) or the like operating on the computer performs a part or all of the actual processing based on the instruction of the program code and the processing of the above-described embodiment is realized by the processing is also included. .

  Furthermore, the program code read out from the recording medium may be written to a memory provided to a function expansion board inserted into the computer or a function expansion unit connected to the computer. Based on the instruction of the program code, the CPU included in the function expansion board or the function expansion unit performs a part or all of the actual processing, and the processing of the above embodiment is realized.

  In the above embodiment, the computer executes the program recorded in the recording medium or the storage device to perform the processing. However, the present invention is not limited to this. Some or all of the functions of the operation unit that operates based on a program may be configured by a dedicated LSI such as an ASIC or an FPGA. ASIC is an acronym of Application Specific Integrated Circuit, and FPGA is an acronym of Field-Programmable Gate Array.

100 ... trajectory generation unit, 101 ... CPU (arithmetic unit), 140 ... program, 200 ... robot, 500 ... robot _{apparatus, P} 11, Q _{11 ...} first taught _{point, P} 12, Q _{12 ...} first auxiliary point, P _1a , Q _1a ... 1st halfway point, P ₂₁ , Q ₂₁ ... 2nd auxiliary point, P ₂₂ , Q ₂₂ ... 2nd teaching point, P _2a , Q _2a ... 2nd halfway point, s ... parameter variable

Claims (11)

The operation unit is a trajectory generation method for generating a trajectory of an articulated robot,
The first interpolation path from the first teaching point to the first auxiliary point, in which the operation unit is a function of a parameter and the change of the derivative differentiated with the parameter to a predetermined rank is continuous, A first interpolation path generation step of generating by an interpolation method;
The second interpolation path from the second auxiliary point to the second teaching point, in which the operation unit is a function of the parameter and the change of the differentiated value differentiated by the parameter to the predetermined rank is continuous, A second interpolation path generation step of generating by a second interpolation method;
The calculation unit is a function of the parameter, and the change of the differentiated value differentiated by the parameter to the predetermined rank is continuous, the first interpolation point on the first interpolation path from the first interpolation point to the second interpolation path A joining step of creating a joining path towards the second halfway point above;
The calculation unit is configured to calculate a first local path between the first teaching point and the first halfway point in the first interpolation path, the second halfway point in the second interpolation path, and the second teaching point. The trajectory of the robot is determined by optimization calculation that optimizes the operation time of the robot so as to meet predetermined physical constraints with respect to a path connecting the second local path between Optimization calculation process to be determined;
In the joining step, the computing unit is configured to, for each first interpolation point from the first halfway point to the first auxiliary point, sequentially from the first interpolation point closer to the first teaching point, to the first interpolation point. The first weighting coefficient, which sequentially transitions from 0 to 0, is applied to each second interpolation point from the second auxiliary point to the second halfway point in order from the second interpolation point closer to the second auxiliary point A point obtained by multiplying a second weighting factor sequentially transitioning from 0 to 1 so as to be a value obtained by subtracting the first weighting factor from 1, and the first interpolation point being multiplied by the first weighting factor; (2) adding the points obtained by multiplying the two interpolation points by the second weighting factor to generate the connection path;
In the joining step, the first weight coefficient is a function of the parameter, and the first weighting function is used to continuously change the derivative value due to the parameter up to the predetermined rank. And determining the second weighting factor by using a second weighting function in which changes of differential values due to the parameters are continuous up to the predetermined rank. The first interpolation method and the second interpolation method are respectively one of a method of generating an interpolation path in a task space and a method of generating an interpolation path in a joint space,
In the connection step, the operation unit unifies the first interpolation path and the second interpolation path into a path in any one of the task space and the joint space, and connects them in the unified space. The trajectory generation method according to claim 1, wherein generation of a combined route is performed. The operation unit is a trajectory generation method for generating a trajectory of an articulated robot,
The first interpolation path from the first teaching point to the first auxiliary point, in which the operation unit is a function of a parameter and the change of the derivative differentiated with the parameter to a predetermined rank is continuous, A first interpolation path generation step of generating by an interpolation method;
The second interpolation path from the second auxiliary point to the second teaching point, in which the operation unit is a function of the parameter and the change of the differentiated value differentiated by the parameter to the predetermined rank is continuous, A second interpolation path generation step of generating by a second interpolation method;
The calculation unit is a function of the parameter, and the change of the differentiated value differentiated by the parameter to the predetermined rank is continuous, the first interpolation point on the first interpolation path from the first interpolation point to the second interpolation path A joining step of creating a joining path towards the second halfway point above;
The calculation unit is configured to calculate a first local path between the first teaching point and the first halfway point in the first interpolation path, the second halfway point in the second interpolation path, and the second teaching point. The trajectory of the robot is determined by optimization calculation that optimizes the operation time of the robot so as to meet predetermined physical constraints with respect to a path connecting the second local path between Optimization calculation process to be determined;
The first interpolation method and the second interpolation method are respectively one of a method of generating an interpolation path in a task space and a method of generating an interpolation path in a joint space,
In the connection step, the operation unit unifies the first interpolation path and the second interpolation path into a path in any one of the task space and the joint space, and connects them in the unified space. A trajectory generation method characterized in that a combined route is generated.   The trajectory generation method according to any one of claims 1 to 3, wherein the predetermined rank is three. In the joining together process, the calculation unit, by using a sigmoid function, obtains a first weight factor, using a sigmoid function, according to claim 1 or 2, characterized in that determining said second weighting factor Trajectory generation method.   The said operation part was further provided with the setting process by which the at least one waypoint is set by the user among the said 1st waypoint and the said 2nd waypoint, It is characterized by the above-mentioned. The trajectory generation method described in the section.   The predetermined physical constraint includes at least one of a constraint on joint angular velocity of the robot and a constraint on velocity of a tip of the robot, a constraint on joint torque of the robot, a constraint on joint angular acceleration of the robot, The trajectory generation method according to any one of claims 1 to 6, further comprising at least one of a constraint on joint angle jerk of the robot and a constraint on acceleration on a tip of the robot.   A trajectory generation device comprising the operation unit that executes each step of the trajectory generation method according to any one of claims 1 to 7.   A robot apparatus comprising: the articulated robot; and a trajectory generation device according to claim 8, wherein an operation of the robot is controlled based on a trajectory generated by the trajectory generation device.   The program for making a computer perform each process of the track | orbit production | generation method of any one of Claims 1 thru | or 7.   The computer-readable recording medium which recorded the program of Claim 10.

Images