JP2016055404A - Locus generation method, locus generation device, robot device, program, and recording medium - Google Patents

Abstract

PROBLEM TO BE SOLVED: To generate an optimal locus abiding by physical restrictions on which a robot following a route combining two interpolation routes is not stopped.SOLUTION: An articulation interpolation route directed from a teaching point Qto a teaching point Qin which changes in differential value differentiated to a predetermined grade via a parameter s are continuous is generated by linear interpolation. A linear interpolation route directed from a teaching point Qto a teaching point Qin which the changes in differential value differentiated to the predetermined grade via the parameter are continuous is generated by linear interpolation. A combined route directed from a teaching point Qto a teaching point Qin which the changes in differential value differentiated to the predetermined grade via the parameter s are continuous is generated. A robot locus is obtained by optimization calculation for optimizing the operation time of the robot so as to abide by predetermined physical restrictions for a route combining a route between the teaching point Qand the teaching point Qand a route between the teaching point Qand the teaching point Qvia the combined route.SELECTED DRAWING: Figure 6

Description

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

  There are three known types of robot interpolation operations: linear interpolation, circular interpolation, and joint interpolation. Linear interpolation is an interpolation method in which a tool center point (TCP) goes straight to a target point. Circular interpolation is an interpolation method in which a reference point is intentionally deviated from a straight path between two points, and TCP is moved along a unique circular arc passing through the three points. This circular interpolation and linear interpolation are essentially the same because they are performed in task space.

  Joint interpolation is an interpolation method in which the axes start moving simultaneously toward the target joint angle at the same time and reach the target point at the same time, regardless of the linearity or arc shape of TCP. That is, joint interpolation is a method for generating a route in a joint space (also referred to as a configuration space). According to joint interpolation, it is easy to control 100% of each joint, and there is no singularity problem, which is a problem in linear interpolation or circular interpolation. Often used in However, since speed control is performed for each joint, the TCP path and speed cannot be easily predicted by the user.

  As the movement of the robot, an interpolation method suitable for the movement and purpose during that movement is adopted. For example, there are cases where a robot grips a cylindrical part, moves the cylindrical part over a cylindrical hole, and inserts the cylindrical part into the hole. In this case, it is desirable that the movement of the robot after gripping operates at high speed by joint interpolation, and that the operation of fitting the cylindrical part into the hole from above the cylindrical hole is fitted straight using linear interpolation. .

  By the way, the interpolation operation in each movement section is given by a speed function that determines 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 movement speed after the movement starts, and the operation is decelerated and stopped before the movement ends.

  Here, as a method of moving two different routes in succession, it is conceivable to stop at the joint of the routes. For example, in the case of inserting a cylindrical part into a hole, acceleration is performed by the joint interpolation path, movement is performed, the speed is decelerated before the end of the joint interpolation path, and the speed is gradually stopped by the linear interpolation path. If it is made to raise, it can operate on two interpolation paths continuously. However, the strict stop of the robot between the two interpolation paths is not only unnecessary, but the stagnation behavior in this part prolongs the operation time of the robot. Of course, if the vehicle is suddenly decelerated or accelerated in order to save work time, it is not preferable because it promotes the vibration of the robot.

  So, for example, when linear interpolation and linear interpolation are operated continuously, after deriving the speed function on each linear interpolation path first, the deceleration area of the previous movement section and the acceleration of the subsequent movement section Connect the area. Then, the connecting point of both sections or the vicinity thereof may be passed through the sum of the speed during deceleration of the previous movement section and the speed during acceleration of the subsequent movement section. Thereby, the time to reach the next teaching point can be shortened. Such speed superimposition processing is easily achieved in a sense by connecting the speed functions themselves when the interpolation method for the previous movement section and the interpolation method for the subsequent movement section are the same type. In the case of linear interpolation and linear interpolation, on the task space, the previous translation speed function and the subsequent translation speed function, the previous rotation speed function and the subsequent rotation speed function, respectively, the deceleration area and the acceleration area, respectively. And all or a part of each may be overlapped.

  When operating by joint interpolation, a velocity function is given to each joint (axis) in the case of a 6-axis robot. Even in this case, the speed overlapping process between the joint interpolations is performed by obtaining the speed function on the respective joint interpolation paths, and then decelerating the deceleration area of the speed function ahead of the first joint and the acceleration area of the subsequent speed function, What is necessary is just to perform it for every joint like the speed function of the tip of the 2nd joint, and the speed function of the back. Therefore, if the manipulator has six degrees of freedom, all six combinations are overlapped.

  However, when the movement of the robot shifts from joint interpolation to linear interpolation, the interpolation space is different between the joint space and the task space, respectively, and the joint angular velocity of joint interpolation, the translation speed and rotational speed of TCP of linear interpolation are different. The splicing process cannot be performed as it is.

  Therefore, in Patent Document 1, when shifting from joint interpolation to linear interpolation, linear interpolation is performed to obtain the translational speed and rotational speed of TCP, and then the inverse kinematics calculation is performed to correct the angular velocity of the joint of each axis. The speed is connected with the joint interpolation trajectory. Since the linear interpolation speed is changed to the joint angular speed to join the speeds, the joint angular speeds can be joined.

  On the other hand, Non-Patent Document 1 describes an optimal speed calculation method for calculating an optimal operation speed of a robot that observes various physical constraints and minimizes an operation time on a given route. To adjust the robot's motion speed and create a motion trajectory along a given route, the user usually adjusts the speed value many times so that the robot's motion time is shortened while observing various physical constraints. There was a need to do. However, if a person adjusts the speed value, the trajectory is not optimal, and the operation time becomes long, and at the same time, the trajectory development man-hours are required. Therefore, it is desirable to automatically calculate the optimal operation speed of the robot that observes various physical constraints and minimizes the operation time.

  Non-Patent Document 1 describes an optimal speed calculation method for generating a trajectory having as short an operation time as possible for a given route while keeping the joint torque constraint of the robot.

JP 2004-252814 A

Verscheure, Dieterik, et al. “Time-optimal path tracking for robots: a convex optimization approach.” Automatic Controls, IEEE Transactions on 54.10 (2009): 2318-2327.

  However, even with the above-mentioned Patent Document 1 and Non-Patent Document 1, it is impossible to generate an optimal trajectory that takes physical constraints into consideration when two interpolation paths are connected.

  Non-Patent Document 1 does not describe a method of connecting two generated interpolation paths while maintaining smoothness with respect to parameters. Also, Cited Document 1 discloses a method for connecting two interpolation paths, but the smoothness of the two interpolation paths with respect to the parameter s (smoothness in which the differential value obtained by differentiating to a predetermined rank is continuous). There is no description about tying it up.

  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 smooth with respect to the parametric variable s (smooth with a continuous change in the differential value obtained by differentiating to a predetermined rank). It has never been done before. For example, it is possible to connect two interpolation paths having different spaces for interpolating joint interpolation and linear interpolation paths, and two interpolation paths having different shapes such as circular interpolation and linear interpolation while maintaining smoothness with respect to the parameter s. It was not done.

  Conventionally, since two interpolation paths were not connected with a 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 the trajectory of the robot is generated, This caused a large error in the optimality.

  For example, the joint jerk of the joint i-axis is expressed as s

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

Can be written. If q _i (s) is not smooth with respect to s and is simply continuous, q _i ′ (s), q _i ″ (s), q _i ′ ″ (s) are discontinuous, so q _i (...) (s) becomes discontinuous with respect to the parameter s.

  When the speed optimization calculation is performed on such a path that is not smooth (the change in the differential value differentiated to a predetermined rank is discontinuous), a command value for the joint angle to the robot is generated at regular intervals. There was a big error. As a result, for example, the joint jerk is greatly deviated from the restriction. In addition, this is not limited only to joint jerk, but physical constraints such as joint torque constraint, joint angular velocity constraint, joint angular acceleration constraint, TCP speed constraint or TCP acceleration constraint also cause a large error, It was the cause of a significant departure from the constraints.

  Therefore, an object of the present invention is to generate an optimal trajectory that observes physical constraints and does not stop a robot that follows a path obtained by connecting two interpolation paths.

  The present invention is a trajectory generation method in which the arithmetic unit generates a trajectory of an articulated robot, wherein the arithmetic unit is a function of a parametric variable, and a differential value differentiated by the parametric variable up to a predetermined rank. A first interpolation path generating step of generating a first interpolation path from the first teaching point to the first auxiliary point with a continuous change by a first interpolation method; and the computing unit is a function of the parameter A second interpolation path generating step of generating a second interpolation path from the second auxiliary point to the second teaching point, in which a change in the differential value differentiated by the parameter is continuous up to the predetermined rank, by the second interpolation method And the calculation unit is a function of the parametric variable, and the change of the differential value differentiated by the parametric variable up to the predetermined rank is continuous from the first intermediate point on the first interpolation path to the second intermediate point. Generate a connection path toward the second halfway point on the interpolation path A joining step; and the calculation unit includes: a first local path between the first teaching point and the first intermediate point in the first interpolation path; a second intermediate point in the second interpolation path; By the optimization calculation for optimizing the operation time of the robot so as to keep a predetermined physical constraint with respect to the route obtained by joining the second local route between the second teaching point and the second joining point by the joining route, And an optimization calculation step for obtaining the trajectory of the robot.

  According to the present invention, the robot path that is smooth with respect to the parametric variable (the change in the differential value differentiated to a predetermined rank is continuous) is generated. Therefore, the robot that follows the path obtained by connecting the two interpolation paths It is possible to generate a trajectory that observes physical constraints without stopping.

It is explanatory drawing which shows schematic structure of the robot apparatus which concerns on 1st Embodiment of this invention. It is a schematic diagram which shows the robot of the robot apparatus which concerns on 1st Embodiment of this invention. 1 is a block diagram illustrating a configuration of a trajectory generation device according to a first embodiment of the present invention. It is a graph which shows the weight 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 explanatory drawing which shows the path | route of the robot in the joint space in 1st Embodiment of this invention. It is a flowchart which shows the main steps of the track | orbit 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 method which concerns on 1st Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the task space in 2nd Embodiment of this invention. It is explanatory drawing which shows the path | route of the robot in the joint space in 2nd Embodiment of this invention. It is a flowchart which shows the path | route production | generation process of the track | orbit production 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 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 method which concerns on 4th Embodiment of this invention.

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

[First Embodiment]
FIG. 1 is an explanatory diagram showing a schematic configuration of the robot apparatus according to the first embodiment of the present invention. The robot apparatus 500 includes an articulated robot (two or more joints) 200, a trajectory generation apparatus 100 that generates a trajectory of the robot 200, and a robot controller 300 that controls the operation of the robot 200 based on the generated trajectory. ing. In addition, the robot apparatus 500 includes an operation unit 410 that inputs data 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 displays an image. The operating device 400 is provided.

  The trajectory generation device 100 is configured by a computer including a CPU (Central Processing Unit) 101 that is a calculation unit and a storage unit 106 that 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 worker can visually recognize the generated motion trajectory. In addition, although the case where the operation part 410 and the display part 420 are integrated in one operating device 400 is demonstrated, the operation part 410 and the display part 420 may be comprised with the independent apparatus. Further, the case where the operation unit 410 and the display unit 420 are configured separately will be described. However, 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. Also good.

  The CPU 101 functions as a kinematics calculation unit 31, a route generation unit 32, a route joining 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 diagram showing a robot of the robot apparatus according to the first embodiment of the present invention. The robot 200 has a multi-joint (two or more joints, two joints in the first embodiment) robot arm 210 and an end effector, for example, a robot hand 220 attached to the tip of the robot arm 210.

The robot arm 210 is a two-degree-of-freedom robot arm composed of, for example, two joints (axes) J ₁ and J ₂ and links 201 and 202. There are various types of joints such as a rotary joint, a linear motion joint, and a spherical joint. In the first embodiment, the joints J ₁ and J ₂ are rotational (turning) joints. The joints may be operated actively with a power source such as a motor, or may be passively operated without a power source. In the first embodiment, the joints J ₁ and J ₂ are power sources. Have The robot arm includes a serial link type in which each joint is connected by a link, and the joint and the link are alternately connected in series, and a parallel link type in which the combination of the joint and the link is in parallel. In the first embodiment, The serial link type has a rotating joint. That is, the robot arm 210 is not particularly limited as long as it is widely considered as an operating body in which the robot arm itself operates. For example, a 6-axis articulated robot arm or a direct-acting robot arm may be used.

A parameter indicating the degree of freedom of the robot arm 210 (robot 200) is a joint angle, and the joint angles of the two axes J ₁ and J ₂ of the robot arm 210 are θ ₁ and θ ₂ , respectively. 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 the parameters representing the degree of freedom of the robot arm 210 (for example, joint angle and expansion / contraction length) are coordinate axis values, 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.

  A tool center point (TCP) is set at the tip of the robot 200, that is, the robot hand 220. The TCP has three parameters (x, y, z) representing the position and three parameters (α, β, γ) representing the posture (rotation), that is, six parameters (x, y, z, α, β, It 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 a configuration of the trajectory generation device 100 according to the first embodiment of the present invention. As illustrated in FIG. 3, the trajectory generation device 100 configured by a computer includes a CPU 101 as a calculation unit. The trajectory generation device 100 includes a ROM (Read Only Memory) 102, a RAM (Random Access Memory) 103, and an HDD (Hard Disk Drive) 104 as the storage unit 106. Further, the trajectory generating apparatus 100 includes a recording disk drive 105 and various interfaces 111 to 113.

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

  The ROM 102 stores a boot program such as BIOS. The RAM 103 temporarily stores various data such as arithmetic processing results 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, thereby executing each step of the trajectory generation method described later. That is, the CPU 101 functions as the units 31 to 35 illustrated 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 operation device 400 described above is connected to the interface 111. The interface 112 is connected to a rewritable nonvolatile memory such as a USB memory or an external storage device 600 such as an external HDD. The above-described robot controller 300 is connected to the interface 113.

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

For example, the kinematics calculation unit 31 is given a configuration (θ ₁ , θ ₂ ) as a teaching point, and when performing linear interpolation based on the configuration (θ ₁ , θ ₂ ), the configuration is set to the position and rotation of the TCP by forward kinematics. . Conversely, the kinematics calculation unit 31 is given a TCP position and rotation as a teaching point, and when performing joint interpolation, configures the TCP position and rotation by inverse kinematics.

  The path generation unit 32 generates a path (interpolation path) of the robot 200 that interpolates between teaching points. The path of the robot 200 is a configuration locus of the robot 200 in the joint space or a TCP locus 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, and in the first embodiment, is a set of joint command values (angle command values) of the 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.

  As interpolation methods for interpolating between teaching points, there are Spline interpolation, B-Spline interpolation, Bezier curve, linear interpolation, circular interpolation, joint interpolation, and the like, and the position of the robot 200 on the path is indicated by the value of the 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 constraints of joint jerk, it is necessary that the change of the differential value obtained by differentiating the path to the third order with respect to the parameter s is continuous. For example, if the third-order B-Spline curve, to parametric s ^{C 3} grade (value obtained by differentiating to the third floor is continuous) it becomes.

  The optimum 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 optimum speed of the robot 200. . In the first embodiment, the predetermined physical constraint is a joint torque constraint, a joint angular velocity constraint, a joint angular acceleration constraint, a joint angular jerk constraint, a TCP velocity constraint, and a TCP acceleration constraint in the robot 200. is there. The restriction on the TCP speed of the robot 200 is a restriction on the speed of the tip of the robot 200. The restriction on the TCP acceleration of the robot 200 is a restriction on the acceleration at the tip of the robot 200.

  In the first embodiment, a case where all of these restrictions are included in the predetermined physical restrictions will be described, but the present invention is not limited to this. The predetermined physical constraint includes at least one of a joint angular velocity constraint and a TCP velocity constraint. In addition to this, the predetermined physical constraint may include at least one constraint among a joint torque constraint, a joint angular acceleration constraint, a joint angular jerk constraint, and a TCP acceleration constraint. If a route having a rank greater than or equal to the highest rank among the respective constraint values is generated, the model for calculating the calculation constraint value becomes continuous, and the constraint value can be calculated accurately.

When obtaining the trajectory of the robot 200 under the conditions of joint torque restriction, joint angular acceleration restriction, joint angular jerk restriction, and TCP acceleration restriction, a differential value obtained by differentiating the path from the first to the third order with a parameter s is obtained. Use. For this reason, an example will be described in which the path needs to be at least class C ³ with respect to the parameter s (continuous change in the differential value obtained by third-order differentiation). That is, the predetermined physical constraint includes a constraint for evaluating the path using a differential value obtained by first-order differentiation of the path by the parameter s. In the first embodiment, the joint torque constraint, the joint angular acceleration are included. , Joint angle jerk constraint, and TCP acceleration constraint.

  As a method for solving the optimization problem, a known method may be used.For example, a method is used in which parameters are replaced to make the problem convex, and the log barrier method and Newton method are used to rapidly converge to a global convergence solution. That's fine.

The path joining unit 33 provides joining sections (overlapping sections) for each of the two interpolation paths, adds points by multiplying the points on the joining sections of the two interpolation paths by a weighting factor, and sets the two interpolation paths as predetermined. smoothness (in this case C ³ grade smoothness) generating a path pieced together with.

An example in which the path linking unit 33 connects the two interpolation paths of the class C ^{3 with} respect to the parameter s generated by the joint interpolation and the linear interpolation while maintaining the class C ³ smoothness with respect to the parameter s as follows. explain.

  In order to smoothly connect the interpolation path interpolated in the joint space and the interpolation path interpolated in the task space with respect to the parameter s, a connection section is provided in each interpolation path, and the task space interpolation path is gradually changed from the joint space interpolation path. It is necessary to change to. When changing gradually, it is essential to gradually change the weight of the connection in order to connect smoothly.

As a condition of the weight function, the first to third differential values of the parameter s are 0 (including those that can be regarded as 0) at the start point and end point of the joining interval, and at least C with respect to the parameter s. Smoothness at level ³ is required. Therefore, considering that the weighting by applying a sigmoid function which is a C ^∞ class.

  The sigmoid function is

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

It is. However, in this state, the domain is -∞ ≦ x ≦ ∞, which is difficult to handle. Therefore, a synthesis function of the sigmoid function and tan (x) is considered as follows.

  Furthermore,

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

And However, a is a real number larger than 0.

In this weight function, if a is sufficiently large, the first to third differential values can be regarded as almost zero at the start and end points of the domain. Therefore, an interpolation path created by a different interpolation method is approximately C with respect to the parameter s. ^It can be used for weighting that keeps the ^third 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. FIG. 4A is a graph of the first weighting function f _sig1 (x). FIG. 4B is a graph of the second weight function f _sig2 (x).

  The first weight function is a function that sequentially changes from 0 to 1 as x increases, and the second weight function is a function that sequentially changes from 1 to 0 as x increases, and has the same x value. In this case, the sum of the first weighting coefficient of the first weighting function and the second weighting coefficient of the second weighting function may be 1. If it is such a function, a 1st weight function and a 2nd weight function are not limited to said function, You may use a polynomial and another function. In this case, the first weight function may be a function of the parameter s, and may be a continuous function that is differentiated by the parameter s up to a predetermined rank (for example, the third floor). Further, the second weighting function may be a function of the parameter s, and may be a function that is continuously differentiated by the parameter s up to a predetermined rank (for example, the third floor).

  FIG. 5 is an explanatory diagram showing a route of the robot in the task space in the first embodiment, and FIG. 6 is an explanatory diagram showing a route 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 positions and rotations of the TCPs in the task space of Q ₁₁ and Q ₁₂ in FIG. 6 are set as P ₁₁ and P _{12 in} FIG. 5, respectively. The configurations of the robot 200 (robot arm 210) when the TCP is at the positions and rotations of P ₂₁ and P _{22 in} FIG. 5 are respectively Q ₂₁ and Q ₂₂ in FIG.

In addition, the configuration of the robot 200 on the joint interpolation path in which joint interpolation is performed using the teaching points Q ₁₁ and Q ₁₂ on the joint space is q ₁ (s ₁ ) (0 ≦ s ₁ ≦ 1) in FIG. . The configuration of the robot 200 that performs the linear interpolation using the teaching points P ₂₁ and P ₂₂ on the task space and positions the TCP on the linear interpolation path is represented by q ₂ (s ₂ ) (0 ≦ s ₂ ≦ 1) in FIG. To do. However, s ₁ and s ₂ are parameters of the joint interpolation path and the linear interpolation path, respectively.

Also, let p ₁ (s ₁ ) in FIG. 5 be a vector representing the position and rotation of the TCP when the configuration of the robot 200 is q ₁ (s ₁ ). 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.

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

On the other hand, the teaching point P ₁₂ (or teaching point Q ₁₂ ) that is the first auxiliary point and the teaching point P ₂₁ (or teaching point Q ₂₁ ) that is the second auxiliary point are points where the robot 200 does not pass, It is a point. In the first embodiment, the teaching point P ₁₂ (or teaching point Q ₁₂ ) is a preset point, and the teaching point P ₂₁ (or teaching point Q ₂₁ ) is a point obtained by calculation.

Now, consider joining the joint interpolation path, which is the first interpolation path obtained by joint interpolation, with the linear interpolation path, which is the second interpolation path, obtained by linear interpolation. Therefore, s ₁ = s _1s on the joint interpolation path (first interpolation path) is set as the start point of the joining section. When s ₁ = s _1s, the point is an intermediate point (first intermediate point) on the joint interpolation path. Further, s ₁ = s _1e on the joint interpolation path is set as the end point of the joining section.

Further, s ₂ = s _2s on the linear interpolation path (second interpolation path) is set as the start point of the joining section, and s ₂ = s _2e on the linear interpolation path is set as the end point of the joining section. When s ₂ = s _2e, the point is an intermediate point (second intermediate point) on the linear interpolation path. For the sake of simplicity, each constant is set to s _1e = 1, s _2s = 0, s _2e −s _2s = s _1e −s _1s, that is, s _2e = 1−s _1s .

The user selects at least one of the teaching points P _1a and P _2a (or teaching points Q _1a and Q _2a ) that are the first and second intermediate points that are the boundaries between the joining sections of the interpolation paths and the other sections. Set (specify) the parameter value of one halfway point. Based on the point, the teaching point calculation unit 34 performs a reverse calculation of the teaching point used for the path interpolation and the connecting section.

  Since the joint section of the first embodiment is a joint path in which joint interpolation and linear interpolation are joined, there is a possibility that the joint section does not become a straight line and does not become a movement intended by the user.

  Therefore, the user designates (sets) on the linear interpolation path that the straight interpolation path is desired to be secured from here. From this 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 _2a in FIG. 5 in which straightness can be ensured, and calculates backward from the teaching point P ₂₁ and the constant s _2e by the teaching point calculation unit 34. Perform reverse calculation. Specifically, the teaching point P ₂₁ and the constant s _2e are calculated back so that the TCP position and rotation p ₁ (s) when the linear interpolation is the parameter s = s _2e overlaps the teaching point P _2a .

If the constant s _2e is obtained, s _2e = 1−s _1s , and therefore the constant s _1s is obtained.

In the first embodiment, only the adjustment of the teaching point of the linear interpolation path and the joining section has been described, but the present invention is not limited to this. Similarly, in the joint interpolation portion, the user can specify (set) the teaching point P _1a (teaching point Q _1a ) that is the boundary between the joining section and the non-joining section, and the user can specify the section in which joint interpolation characteristics are desired. You may do it.

s is a parameter of the path of the robot 200 obtained by connecting two interpolation paths. 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 the first embodiment, the parameter s is a value ranging from 0 to 1 + s ₁ . By the value of the parametric s depending on the time from s = 0 to s = 1 + _{s 1} is varied to increase, TCP of the robot 200 is to move on the path from the teaching point _{P 11} to the teaching point _{P 22} It becomes.

A local section of 0 ≦ s <s _1s is a joint interpolation section, a section of s _1s ≦ s <1 is connected, and a local section of 1 ≦ s <1 + s _1s is a linear interpolation section. The path q (s) on the joint space when the 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 for s of these functions, the first weighting coefficient and the second weighting coefficient are obtained.

By using q (s) is to maintain the smoothness in C ³ tertiary respect parametric s, continuous q '(s), q''(s),q''' it is possible to obtain (s) is. Further, if q (s) is used, after performing optimum speed calculation (optimization calculation for optimizing the operation time of the robot 200), a joint command value (that is, a trajectory) for every predetermined time can be generated based on the calculation. It becomes possible.

  In accordance with equation (5), the equation for calculating the points on the joining path in the joining section in FIG. 6 can be written as follows using the values on the graph of FIG.

Here, q ₁₁ , q ₁₂ , q ₁₃ , q ₁₄ , and q ₁₅ are the teaching points Q _1a to Q ₁₂ on the joint interpolation path q ₁ (s ₁ ) that is the first interpolation path in FIG. Are a plurality of interpolation points (first interpolation points). In addition, q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ , and q ₂₅ are the points from the teaching point Q ₂₁ to the teaching point Q _2a on the linear interpolation path q ₂ (s ₂ ) that is the second interpolation path in FIG. 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 the model of the robot 200 and the constraint conditions (predetermined physical constraints) in the work space where the robot 200 is working from the storage unit 106 (for example, the HDD 104) (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, and the like of each link. The constraint conditions define the physical constraints of the robot 200. The joint torque constraint, the joint angular velocity constraint, the joint angular acceleration constraint, the joint angular jerk constraint, the TCP speed constraint, and the TCP acceleration constraint of the robot 200 are defined. It is a constraint.

  Next, the CPU 101 acquires the teaching point and the interpolation method of the robot 200 (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 by the interpolation method is read.

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

  In the first embodiment, since the user specifies joint interpolation and linear interpolation, the interpolation method acquired by the CPU 101 in the first embodiment is joint interpolation that is the first interpolation method and linear interpolation that is the second interpolation method. It is. These 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) the teaching point P _{2a of} at least one of the teaching points P _1a and P _2a that are halfway points in the first embodiment, and sets a joining section (S103: Setting process). In the first embodiment, a section of s _1s ≦ s ₁ <1 of joint interpolation and a section of 0 <s ₂ <s _2e of linear interpolation are set as a joining section. The joining section is arbitrary, but when the straightness is ensured from the teaching point designated by the user, the teaching point calculation unit 34 adjusts the joining section and the joining section used for linear interpolation path generation accordingly. .

More specifically, the constant s _1e is set to 1 and the constant s _2s is set to 0. That is, these constants s _1e and s _2s are stored in the storage unit 106 in advance, and the CPU 101 reads out and sets them from the storage unit 106.

The teaching point P _2a (parameter value), which is the second intermediate point on the second interpolation path from the second auxiliary point toward the second teaching point, is set (designated) by the user. Thus, CPU 101 is taught points is a second auxiliary point _{P 21} and the constant _{s 2e,} calculates the _{s 1s.} In the first embodiment, the teaching point P _1a that is the first intermediate point on the first interpolation path from the first teaching point toward the first auxiliary point is stored in the storage unit 106 in advance, and is stored by the CPU 101. This is read from the unit 106 and set.

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

Next, the CPU 101 executes a route generation process for generating the route q (s) of the robot 200 by connecting the joint interpolation route that is the first interpolation route and the linear interpolation route that is the second interpolation route (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 route q (s) in which two local routes are joined by a joining route in a joining section is generated. Here, the first local route is q ₁ (s) (where 0 ≦ s <s _1s ). The second local route is q ₂ (s−s _1s ) (where 1 ≦ s <1 + s _1s ). The joining path is q ₁ (s) × f _sig1 ((s−s _1s ) / (1−s _1s )) + q ₂ (s−s _1s ) × f _sig2 ((s−s _1s ) / s _2e ). (However, s _1s ≦ s <1) (see formula (5)).

Next, the CPU 101 determines an evaluation point on a route q (s) connecting two interpolation routes that are targets of speed optimization (S105). For example, the evaluation score is a column of s equally divided by a certain Δs from s = 0 to s = 1 + s _1s .

  Next, the CPU 101 performs q ′ (s), q ″ (s), q ′ ″ (s) at the evaluation points by numerical differentiation with respect to the parameter s using an interpolation function, that is, the path q (s). Is obtained (S106).

  Next, the CPU 101 uses q ′ (s), q ″ (s), and q ′ ″ (s), joint torque constraint, joint angular velocity constraint, joint angular acceleration constraint, joint angle jerk constraint, TCP speed constraint. Then, the optimum speed for moving in the shortest time is calculated on the condition of the constraint of the TCP acceleration constraint (S107).

  Next, the CPU 101 calculates s (t) from s (•) (s) calculated by the optimum speed calculation as shown in the equation (14), and the joint command value for each predetermined time Δt from q (s). (Angle command value) is obtained (S108). However, s (0) = 0.

  The above steps S105 to S108 are the optimization calculation process. In other words, in steps S105 to S108, the value of the parameter s corresponding to the time (that is, s) is determined by the optimization calculation that optimizes the operation time of the robot 200 so as to keep predetermined physical constraints on the route q (s). (T)) is obtained, and the trajectory of the robot 200 is obtained.

If q (s) having C ³ class smoothness is used for the parameter s, q ′ (s), q ″ (s), and q ′ ″ (s) are continuous, and the joint command to the robot 200 is given. Errors in generating values can be greatly reduced.

  Next, the route generation processing in step S104 will be specifically described. FIG. 8 is a flowchart showing the route generation process in step S104 of FIG. 7 in which the parameter s is input and the configuration q (s) is output.

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

In the case of 0 ≦ s <s _1s (when the interpolation method is joint interpolation) in the determination in step S202, the CPU 101 performs joint interpolation using the teaching points Q ₁₁ and Q ₁₂ , and uses the joint interpolation path that is a function of the parameter s. A certain q ₁ (s) is obtained (S203). In other words, 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 ₁₂ where the change in the differential value obtained by differentiating the predetermined rank (for example, the third order) is continuous. And generated 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).

When s _1s ≦ s <1 in the determination in step S202 (in the case of a joint interval between 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 the TCP with the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S206). 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 step). The linear interpolation path p ₂ (s−s _1s ) generated in step S206 is a function of the parameter s in which the differential value differentiated by the parameter s is continuous up to a predetermined rank (for example, the third floor).

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

Next, the CPU 101 calculates a 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. Obtain (S208). Further, the CPU 101 obtains a second weighting factor from the second weighting function f _sig2 ((s−s _1s ) / s _2e ) for weighting the configuration q ₂ (s) _subjected to linear interpolation (S209).

The CPU 101 _multiplies q ₁ (s) by f _sig1 ((s−s _1s ) / (1−s _1s )) and q ₂ (s−s _1s ) by f _sig2 ((s−s _1s ) / s _2e. ) To obtain the sum to obtain a joining route q (s) (where s _1s ≦ s <1) (S210).

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

Similarly, q ₂ (s−s _1s ) (where s _1s ≦ s <1) is the second interpolation points q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ , q shown in the equations (6) to (10). It is a set of ₂₅ . The CPU 101 multiplies each interpolation point q ₂₁ , q ₂₂ , q ₂₃ , q ₂₄ , q ₂₅ by each second weight coefficient r ₂₁ , r ₂₂ , r ₂₃ , r ₂₄ , r ₂₅ . Here, r ₁₁ + r ₂₁ = 1, r ₁₂ + r ₂₂ = 1, r ₁₃ + r ₂₃ = 1, r ₁₄ + r ₂₄ = 1, r ₁₅ + r ₂₅ = 1.

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

Each of the second weight coefficients r ₂₁ , r ₂₂ , r ₂₃ , r ₂₄ , r ₂₅ is a value obtained by subtracting the first weight coefficients r ₁₁ , r ₁₂ , r ₁₃ , r ₁₄ , r ₁₅ from 1. As the parameter s increases, the transition sequentially proceeds from 0 to 1. That is, r ₂₁ (= 0) <r ₂₂ <r ₂₃ <r ₂₄ <r ₂₅ (= 1).

That is, in step S210, the CPU 101 sequentially weights the interpolation points q ₁₁ , q ₁₂ , q ₁₃ , q ₁₄ , q ₁₅ from 1 to 0 sequentially from the interpolation point closer to the teaching point Q _11. Multiply coefficients r ₁₁ , r ₁₂ , r ₁₃ , r ₁₄ , r ₁₅ . Further, CPU 101, the interpolation point _{q 21, q 22, q 23} , q 24, relative to _{q 25,} in this order from the side of the interpolation point close to the teaching point _{Q 21,} the weighting coefficient _{r 21} sequentially changes from 0 to 1, Multiply r ₂₂ , r ₂₃ , r ₂₄ , r ₂₅ . The CPU 101 adds the points obtained by multiplying the interpolation point q _1n (n = 1 to 5, the same applies hereinafter) by the weighting factor r _1n and the points obtained by multiplying the interpolation point q _2n by the weighting factor r _2n. Then, the joining routes q ₁ , q ₂ , q ₃ , q ₄ , and q ₅ are generated (FIG. 7, Expressions (6) to (10)).

Through the above steps S206 to S210, the CPU 101 is a function of the parameter s, and the teaching point _Q1a from the teaching point Q1a is continuously changed in the differential value differentiated by the parameter s up to a predetermined rank (for example, the third floor). A joining route toward Q _2a is generated (joining step). The CPU 101 outputs the configuration q (s) (S211).

The weight coefficients r _1n and r _2n have been described with reference to the case where they are obtained from the weight function in steps S208 and S209. However, the present invention is not limited to this, and the weight coefficients r _1n and r _2n are calculated and stored in advance. It may be stored in 106. At that time, in steps S208 and S209, the CPU 101 only needs to read the data of the weight coefficients r _1n and r _2n from the storage unit 106 without performing the calculation.

Next, when 1 ≦ s <1 + s _1s (when the interpolation method is linear interpolation) in the determination of step S202, the CPU 101 linearly interpolates the TCP with 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 _2a and the teaching point Q ₂₂ in the second interpolation path is the path of the robot 200, q (s) = q ₂ (s−s _1s ). The CPU 101 outputs the configuration q (s) (S214).

The above steps S201 to S214 are repeated by changing the 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). As a result, it is possible to generate a trajectory that observes physical constraints without stopping the robot 200 that follows the path q (s) obtained by connecting two interpolation paths.

In particular, in step S210, the CPU 101 calculates q _1n × r _1n + q _2n × r _2n (n = 1 to 5) to generate a joining route q _n . Thus, of joining path it is generated as a smooth path C ³ tertiary respect parametric s. In the first embodiment, n = 1 to 5 for convenience of explanation, but the present invention is not limited to this. As the number of n increases, the accuracy improves. Therefore, it is preferable to generate a large number of points as a connection path. In the first embodiment, since the first and second weight coefficients are calculated based on the first and second weight functions, it is not necessary to store a large amount of weight coefficient 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 used effectively.

In addition, according to the first embodiment, since the user has set (designated) the teaching point P _2a (Q _2a ), the user can select a section that requires straight travel without being aware of the connecting section of the route. It can be specified and has the effect of improving usability.

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

  FIG. 9 is an explanatory diagram showing the path of the robot arm in the task space in the second embodiment, and FIG. 10 is an explanatory diagram 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. If the configuration at the time of circular interpolation is q ₁ (s), as in the first embodiment, it is possible to join together while maintaining the smoothness of the path by the equation (5). In accordance with the equation (5), the equations for calculating the points q _{1 to} q ₅ on the joining path in the joining section in FIG. 10 are the same as the equations in the first embodiment. become.

Unlike the first embodiment, since the circular interpolation path is a TCP path on the task space, in order to connect on the joint space, inverse kinematics is performed, and the configuration is obtained by performing the circular interpolation path. Find q ₁ (s).

  Considering the above, 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 in which the parameter s is input and the configuration q (s) is output. In the second embodiment, since the first interpolation method is a method of generating an interpolation path in the task space, the obtained circular interpolation path as the first interpolation path is converted into a path in the joint space by inverse kinematics calculation. This is different from the first embodiment. Other than that, the process is the same as in the first embodiment, and each process related to the path generation process will be briefly described below.

First, the CPU 101 acquires (inputs) the parameter s (S301). The CPU 101 classifies cases in the interval of the input parameter s (S302). In the case of 0 ≦ s <s _1s , a circular interpolation section, in the case of s _1s ≦ s <1, the connecting section of circular interpolation and linear interpolation, and in the case of 1 ≦ s <1 + s _1s , the linear interpolation section.

If it is determined in step S302 that 0 ≦ s <s _1s , the CPU 101 circularly interpolates the TCP using 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 route q ₁ (s) is the route of the robot 200, and q (s) = q ₁ (s). The CPU 101 outputs the configuration q (s) (S305).

Next, when s _1s ≦ s <1 in the determination in step S302, the CPU 101 first circularly interpolates the TCP with the teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S306). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S307)} .

The CPU 101 linearly interpolates the TCP using the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S308). 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) (S310). The CPU 101 _obtains a weight function f _sig2 ((s−s _1s ) / s _2e ) for weighting the linearly interpolated configuration q ₂ (s) (S311).

The CPU 101 _multiplies q ₁ (s) by f _sig1 ((s−s _1s ) / (1−s _1s )) and q ₂ (s−s _1s ) by f _sig2 ((s−s _1s ) / s _2e. ) To obtain q (s) (S312). The CPU 101 outputs the configuration q (s) (S313).

When it is determined in step S302 that 1 ≦ s <1 + s _1s , the CPU 101 linearly interpolates TCP with 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 route q ₂ (s−s _1s ) is the route of the robot 200, and q (s) = q ₂ (s−s _1s ). The CPU 101 outputs the configuration q (s) (S316).

As described above, the configuration q (s) obtained by connecting the circular interpolation and the linear interpolation while maintaining the class C ³ is obtained. Therefore, as in the first embodiment, the optimum speed can be calculated according to the flow of FIG. it can.

In step S103 in FIG. 7, the user designates (sets) points P _1a and P _2a (or points Q _1a and Q _2a ) at which the circularity of the circular interpolation path and the straightness of the linear interpolation path are to be ensured. . The CPU 101 functioning as the teaching point calculation unit 34 back-calculates the teaching points and connecting sections of circular interpolation and linear interpolation based on the points P _1a and P _2a (or points Q _1a and Q _2a ).

Since the joining section is a path in which circular interpolation and linear interpolation are joined together, it may not be a pure arc or straight line and may not be a movement intended by the user. Therefore, the user designates the point that the arc property is secured so far and the straightness is desired to be secured, and then the teaching point calculation unit 34 uses the teaching point P ₁₂ , P ₂₁ (or the teaching point). Q ₁₂ , Q ₂₁ ) are calculated.

That is, in the second embodiment, the user designates a point P _1a on FIG. 9 that ensures _arcuateness and a point P _2a on FIG. 9 that can ensure straightness. The CPU 101 calculates backward from there to obtain appropriate P ₁₂ , P ₂₁ , s _1s , and s _2e , respectively. Specifically, the position and rotation p ₁ (s) of TCP when the circular interpolation path is s ₁ = s _1s overlap with P _1a, and the position of TCP when the linear interpolation path is s ₂ = 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 associated with the teaching point is switched from circular interpolation to linear interpolation, it is stopped when the path is switched. Therefore, an optimal trajectory that satisfies various physical constraints of the robot 200 can be generated. In addition, according to the second embodiment, since the user can specify a section that requires arcuateness and straightness without being aware of the connecting section of the route, there is an effect that usability is improved.

[Third Embodiment]
A trajectory generation method according to the third embodiment of the present invention will be described. Note that the configuration of the trajectory generation device, that is, the robot device is the same as that of the first and second embodiments, and a description thereof will be omitted. In the third embodiment, a combination of interpolation methods different from those in the first and second embodiments, that is, two types of interpolation methods of circular interpolation and joint interpolation are designated.

  FIG. 12 is an explanatory diagram showing the path of the robot arm in the task space in the third embodiment, and FIG. 13 is an explanatory diagram 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 circular interpolation is q ₁ (s) and the configuration (interpolation path) when performing joint interpolation is q ₂ (s), as in the first embodiment, the equation (5) ) Makes it possible to join together while maintaining the smoothness of the paths. In accordance with the equation (5), the equations for calculating the points q _{1 to} q ₅ on the route 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 circularly interpolated path is a TCP path on the task space, reverse kinematics is performed to obtain a configuration and the circularly interpolated path q is obtained in order to connect the joint space. ₁ (s) is obtained. Considering the above, 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 in which the parameter s is input and the configuration q (s) is output. In the third embodiment, since the second interpolation method is a method of generating an interpolation path in the joint space, the second interpolation method is different from the second embodiment in that the joint interpolation path that is the second interpolation path is obtained in the joint space. . Other than that, the process is the same as in the second embodiment, and therefore, each process related to the route generation process will be briefly described below.

First, the CPU 101 acquires (inputs) the parameter s (S401). The CPU 101 classifies cases in the interval of the input parameter s (S402). In the case of 0 ≦ s <s _1s , a circular interpolation section, in the case of s _1s ≦ s <1, a joint section of circular interpolation and joint interpolation, and in the case of 1 ≦ s <1 + s _1s , a joint interpolation section.

If it is determined in step S402 that 0 ≦ s <s _1s , the CPU 101 circularly interpolates the TCP using 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 route q ₁ (s) is the route of the robot 200, and q (s) = q ₁ (s). The CPU 101 outputs the configuration q (s) (S405).

Next, when s _1s ≦ s <1 in the determination in step S402, the CPU 101 first circularly interpolates the TCP with the teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S406). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S407)} .

The CPU 101 interpolates joints using the teaching points Q ₂₁ and Q ₂₂ to obtain q ₂ (s−s _1s ) (S408).

The CPU 101 _obtains a weight function f _sig1 ((s−s _1s ) / (1−s _1s )) for weighting the circularly interpolated configuration q ₁ (s) (S409). The CPU 101 _obtains a weight function f _sig2 ((s−s _1s ) / s _2e ) for weighting the configuration q ₂ (s) _subjected to linear interpolation (S410).

The CPU 101 _multiplies q ₁ (s) by f _sig1 ((s−s _1s ) / (1−s _1s )) and q ₂ (s−s _1s ) by f _sig2 ((s−s _1s ) / s _2e. ) To obtain q (s) (S411). The CPU 101 outputs the configuration q (s) (S412).

In the case of 1 ≦ s <1 + s _{1s in} the determination in step S402, the CPU 101 interpolates joints using 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 route q ₂ (s−s _1s ) is the route 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 connecting the circular interpolation and the joint interpolation while maintaining the class C ³ can be obtained, the optimum speed can be calculated according to the flow of FIG. 7 as in the first embodiment. it can.

In step S103 in FIG. 7, the user designates (sets) each of the points P _1a (or points Q _1a ) at which the circularity of the circular interpolation path is desired. Based on the point P _1a (or point Q _1a ), the CPU 101 functioning as the teaching point calculation unit 34 performs a reverse calculation of the teaching point and the joining interval of circular interpolation and joint interpolation.

Since the connecting section is a path in which circular interpolation and joint interpolation are connected, there is a possibility that the connecting section does not become a pure circular arc and does not become a movement intended by the user. Therefore, the user designates a point that he wants to secure the circularity so far. From there, the teaching point calculator 34 calculates an appropriate teaching point P ₁₂ (or teaching point Q ₁₂ ).

That is, in the third embodiment, the user designates the point P _1a on FIG. The CPU 101 calculates backward from there and obtains appropriate P ₁₂ and s _1s . Specifically, P ₁₂ and s _1s are back calculated so that the position and rotation p ₁ (s ₁ ) of TCP when the circular interpolation path is s ₁ = s _1s overlap with P _1a .

  As described above, according to the third embodiment, in addition to the effect of the first embodiment, even when the interpolation method associated with the teaching point is switched from circular interpolation to joint interpolation, the stop is performed at the time of path switching. Therefore, an optimal trajectory that satisfies various physical constraints of the robot 200 can be generated. Further, according to the third embodiment, since the user can designate a section that requires arcuateness without being aware of the joining section of the route, there is an effect that usability is improved.

[Fourth Embodiment]
A trajectory generation method according to the fourth embodiment of the present invention will be described. Note that 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 a description thereof is omitted. In the fourth embodiment, a combination of interpolation methods different from those in the first, second and third embodiments, that is, an interpolation method of linear interpolation and linear interpolation is specified.

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

In the fourth embodiment, the first interpolation method is the first linear interpolation, and the second interpolation method is the second linear interpolation. When the configuration at the time of the first linear interpolation is q ₁ (s) and the configuration at the time of the second linear interpolation is q ₂ (s), as in the first embodiment, It is possible to join together while maintaining the smoothness of the route. In accordance with the equation (5), the equations for calculating the points q _{1 to} q ₅ on the route 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 path becomes a TCP path on the task space, in order to connect on the joint space, inverse kinematics is performed, the configuration is obtained, and the linearly interpolated path q ₁ (s), q ₂ (s−s _1s ) is obtained.

  Considering the above, 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 in which the parameter s is input and the configuration q (s) is output. In the fourth embodiment, since the first interpolation method is a method of generating an interpolation path in the task space, the obtained linear interpolation path that is the first interpolation path is converted into a path in the joint space by inverse kinematics calculation. This is different from the first embodiment. Other than that, the process is the same as in the first embodiment, and each process related to the path generation process will be briefly described below.

First, the CPU 101 acquires (inputs) the parameter s (S501). The CPU 101 performs case classification in 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 joint section of the first linear interpolation and the second linear interpolation, and in the case of 1 ≦ s <1 + s _1s , This is the second linear interpolation section.

If 0 ≦ s <s _{1s in} the determination in step S502, the CPU 101 linearly interpolates TCP with 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 route q ₁ (s) is the route of the robot 200, and q (s) = q ₁ (s). The CPU 101 outputs the configuration q (s) (S505).

Next, when s _1s ≦ s <1 in the determination in step S502, the CPU 101 first linearly interpolates TCP with the teaching points P ₁₁ and P ₁₂ to obtain p ₁ (s) (S506). CPU101 _{is, p} 1 to (s), performs an inverse kinematics calculation, obtaining the configuration _{q 1 (s) (S507)} .

The CPU 101 linearly interpolates the TCP with the teaching points P ₂₁ and P ₂₂ to obtain p ₂ (s−s _1s ) (S508). 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 weight function f _sig1 ((s−s _1s ) / (1−s _1s )) for weighting the linearly interpolated configuration q ₁ (s) (S510). The CPU 101 _obtains a weight function f _sig2 ((s−s _1s ) / s _2e ) for weighting the configuration q ₂ (s) _subjected to linear interpolation (S511).

The CPU 101 _multiplies q ₁ (s) by f _sig1 ((s−s _1s ) / (1−s _1s )) and q ₂ (s−s _1s ) by f _sig2 ((s−s _1s ) / s _2e. ) To obtain q (s) (S512). The CPU 101 outputs the configuration q (s) (S513).

When 1 ≦ s <1 + s _1s is determined in step S502, the CPU 101 linearly interpolates TCP with 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 route q ₂ (s−s _1s ) is the route 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 associated with the teaching point is switched from the first linear interpolation to the second linear interpolation, it does not stop at the time of switching the path. In addition, it is possible to generate an optimal trajectory that observes various physical constraints of the robot 200. This is advantageous in that an optimal trajectory that satisfies various physical constraints can be generated as compared with the method of moving from linear interpolation to linear interpolation described in the background art. In addition, according to the fourth embodiment, the user can designate a section that requires straight travel without being aware of the connecting section of the route, and thus there is an effect that usability is improved.

  The present invention is not limited to the embodiment described above, and many modifications are possible within the technical idea of the present invention.

  In the embodiment described above, the connection in the joint space has been described. However, the present invention is not limited to this, and the same connection may be performed in the task space. For example, the route may be connected to the TCP position and rotation using the same weight function. In this case, finally, a path in the task space may be converted into a path in the joint space to generate a trajectory.

  Moreover, in the said embodiment, although the case where the track | orbit production | generation apparatus 100 was comprised by one computer was demonstrated, this invention is applicable also when comprised by a some computer. In this case, the arithmetic unit of the present invention may be configured by a plurality of CPUs. Further, although the case where the trajectory generating device 100 and the robot controller 300 are configured by different computers has been described, the trajectory generating device 100 and the robot controller 300 may be configured by one computer. In this case, the CPU of the computer generates a robot path and controls the operation of the robot according to the generated path.

  Each processing operation of the above embodiment is specifically executed by the CPU 101. Accordingly, the recording medium storing the program for realizing the functions described above is supplied to the trajectory generating apparatus 100, and the computer (CPU or MPU) constituting the trajectory generating apparatus 100 reads and executes the program stored in the recording medium. You may be made to do. In this case, the program itself read from the recording medium realizes 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 computer-readable recording medium is the HDD 104, and the program 140 is stored in the HDD 104. 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, the ROM 102, the external storage device 600, the recording disk 141, or the like shown in FIG. 3 may be used as a recording medium for supplying the program. As a specific example, a flexible disk, hard disk, optical disk, magneto-optical disk, CD-ROM, CD-R, magnetic tape, nonvolatile memory card, 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-described embodiment by executing the program code read by the computer. This includes a case where an OS (operating system) or the like running on the computer performs part or all of the actual processing based on the instruction of the program code, and the functions of the above-described embodiments are realized by the processing. .

  Furthermore, the program code read from the recording medium may be written in a memory provided in a function expansion board inserted into the computer or a function expansion unit connected to the computer. This includes a case where the CPU of the function expansion board or function expansion unit performs part or all of the actual processing based on the instruction of the program code, and the functions of the above embodiments are realized by the processing.

  Moreover, although the said embodiment demonstrated the case where a computer performed a process recorded on the recording medium or the memory | storage device, it demonstrated, but it does not limit to this. A part or all of the functions of the arithmetic unit that operates based on the program may be configured by a dedicated LSI such as an ASIC or FPGA. Note that ASIC is an acronym for Application Specific Integrated Circuit, and FPGA is an acronym for 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 ...} first middle _{point, P 21,} Q 21 _... second auxiliary _{point, P 22,} Q 22 _... second teaching _{point, P 2a,} Q 2a _... second middle point, s ... parametric

Claims (12)

The arithmetic unit is a trajectory generation method for generating a trajectory of an articulated robot,
A first interpolation path from the first teaching point to the first auxiliary point, in which the arithmetic unit is a function of a parametric variable, and the change of the differential value differentiated by the parametric variable to a predetermined rank is continuous, A first interpolation path generation step generated by an interpolation method;
A second interpolation path from the second auxiliary point to the second teaching point, wherein the arithmetic unit is a function of the parameter, and the change of the differential value differentiated by the parameter to the predetermined rank is continuous; A second interpolation path generation step generated by the second interpolation method;
The calculation unit is a function of the parametric variable, and the change of the differential value differentiated by the parametric variable up to the predetermined rank is continuous from the first intermediate point on the first interpolation path to the second interpolation path. A joining step for generating a joining path toward the second middle point above;
The computing unit includes a first local path between the first teaching point and the first intermediate point in the first interpolation path, a second intermediate point and the second teaching point in the second interpolation path, and The trajectory of the robot is determined by an optimization calculation that optimizes the operation time of the robot so that a predetermined physical constraint is observed with respect to the path that is connected to the second local path between the two by the connecting path. A trajectory generation method characterized by comprising an optimization calculation step to be obtained.   The trajectory generation method according to claim 1, wherein the predetermined rank is three. In the splicing step, the calculation unit sequentially sets the first interpolation point from the first intermediate point to the first auxiliary point in order from the first interpolation point closer to the first teaching point. Multiply the first weighting factor that sequentially transitions from 0 to 0,
For each second interpolation point from the second auxiliary point to the second intermediate point, a value obtained by subtracting the first weighting factor from 1 in order from the second interpolation point closer to the second auxiliary point; Multiply a second weighting factor that sequentially transitions from 0 to 1,
Adding the point obtained by multiplying the first interpolation point by the first weighting factor and the point obtained by multiplying the second interpolation point by the second weighting factor to generate the joining path; The trajectory generation method according to claim 1 or 2.   In the splicing step, the arithmetic unit is a function of the parametric variable, and the first weighting factor is a first weighting function using a first weighting function in which a change in the differential value due to the parametric variable is continuous up to the predetermined rank. The trajectory generation method according to claim 3, wherein the second weighting coefficient is obtained using a second weighting function in which a change in the differential value by the parametric variable is continuous up to the predetermined rank.   The said connection part WHEREIN: The said calculating part calculates | requires a said 1st weighting coefficient using a sigmoid function, and calculates | requires the said 2nd weighting coefficient using a sigmoid function, It is characterized by the above-mentioned. Orbit generation method.   6. The calculation unit according to claim 1, further comprising a setting step in which at least one of the first intermediate point and the second intermediate point is set by a user. The trajectory generation method according to the item. The first interpolation method and the second interpolation method are any one of a method for generating an interpolation path in a task space and a method for generating an interpolation path in a joint space, respectively.
In the joining step, the calculation unit unifies the first interpolation route and the second interpolation route into a route in any one of the task space and the joint space, and connects them in the unified space. The trajectory generation method according to any one of claims 1 to 6, wherein an alignment path is generated.   The predetermined physical constraint includes at least one of a constraint on the joint angular velocity of the robot and a constraint on the speed of the tip of the robot, a constraint on the joint torque of the robot, a constraint on the joint angular acceleration of the robot, The trajectory generation method according to any one of claims 1 to 7, further comprising at least one of a restriction on the joint angle jerk of the robot and a restriction on the acceleration of the tip of the robot.   A trajectory generation apparatus comprising the calculation unit that executes each step of the trajectory generation method according to claim 1.   A robot apparatus comprising: the articulated robot; and the trajectory generation apparatus according to claim 9, wherein the operation of the robot is controlled based on the trajectory generated by the trajectory generation apparatus.   A program for causing a computer to execute each step of the trajectory generation method according to any one of claims 1 to 8.   The computer-readable recording medium which recorded the program of Claim 11.

Images