JP2014050902A - Calculation unit, control device, and calculation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To break through a situation that interference occurs or a cycle time is prolonged as a result of a failure in appropriately designating a beginning point of an inward turning trajectory.SOLUTION: A calculation unit 11 includes: a teaching point information memory 21 in which teaching information representing two or more teaching points for a manipulator is stored; a shape information memory 22 in which shape information representing a position calculation shape that is a three-dimensional shape having a reference point designated and having a length in a first direction and a length in a second direction, which is a direction different from the first direction, designated independently of each other is stored; and a calculation block 23 that calculates an inward turning beginning position that is an intersection between a segment between two teaching points, which are represented by the teaching information and adjacent to each other, and the position calculation shape represented by the shape information. Owing to the components, prevention of interference and shortening of a movement time can be realized by designating appropriate shape information.

Description

  The present invention relates to a calculation device that calculates an inward start position using a three-dimensional shape.

  2. Description of the Related Art Conventionally, in an industrial robot, two trajectories composed of a teaching point and teaching points before and after the teaching point are synthesized to generate an inward trajectory (see, for example, Patent Document 1). The advantages of using such an inner track are as follows. When the moving direction and speed change discontinuously at a certain teaching point, a large acceleration is generated at the teaching point. As a result, vibration is caused, and required accuracy cannot be obtained, or an excessive force is applied, which causes damage to a conveyance target such as a wafer or a glass substrate or a machine body. In order to avoid such damage or the like, if the speed is set to zero at the teaching point, it takes extra time for movement. On the other hand, these problems can be solved by using the inner track.

Japanese Patent Laid-Open No. 8-123531

  However, there is a problem that the starting point of the inner track cannot be set appropriately. For example, a linear trajectory from point A to point B and a linear trajectory from point B to point C are taught, and when an inward trajectory is used for point B, the distance from point B to the inward starting position is A certain inward start distance was set. In such a case, if the same inward starting distance is set in all inward trajectories, interference may occur. For example, a slot serving as a transfer destination of a transfer target such as a wafer or a glass substrate has a vertical width smaller than a horizontal width of the entrance. Therefore, as shown in FIG. 10A, when moving in the vertical direction and the inward turning distance is long, the conveyance target 50 placed on the hand 3 a may interfere with the slot 52. On the other hand, if the inward starting distance is shortened so that such interference does not occur, as shown in FIG. 10B, when moving in the lateral direction, the inward trajectory becomes smaller than necessary, and the tact time is shortened by the inward turning. The effect cannot be obtained. That is, there has been a problem that interference may occur when a uniform inward start distance is set, and there is a problem that the movement time becomes long in order to prevent interference.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to provide an apparatus and the like that can prevent interference and prolong the travel time when using an inner track.

In order to achieve the above object, a calculation apparatus according to the present invention includes a teaching information storage unit that stores teaching information indicating two or more teaching points of a manipulator having a plurality of arms connected by a joint driven by a motor; A position that is a three-dimensional shape in which a reference point is set and in which the length in the first direction and the length in the second direction, which is different from the first direction, can be set independently For the shape information storage unit that stores shape information indicating the shape for calculation, and the two adjacent teaching points indicated by the teaching information, the line segment between the two teaching points and the teaching point on the end point side are used as reference points. And a calculation unit that calculates an inward start position that is an intersection with the position calculation shape indicated by the shape information.
With such a configuration, the inward starting distance can be changed according to the moving direction by appropriately setting the shape information. As a result, interference can be prevented and travel time can be shortened.

Further, in the calculation device according to the present invention, the teaching information is information indicating the posture of the tip of the manipulator at the teaching point, and a reference direction which is a reference direction is set in the position calculation shape, and the calculation is performed. Is the intersection of the shape for position calculation that combines the teaching point on the end point side as the reference point, the posture of the manipulator at the teaching point on the end point side and the reference direction, and the line segment between the two teaching points. A certain inward start position may be calculated.
With such a configuration, since the posture of the tip of the manipulator is also taken into consideration, a more appropriate inner turning start position can be determined.

In the calculation device according to the present invention, the first direction may be a vertical direction, and the second direction may be a horizontal direction.
With such a configuration, the inward starting distance can be made different between the vertical direction and the horizontal direction. For example, this is useful when the transfer robot accesses the slot.

  In the calculation device according to the present invention, the position calculation shape may be a rectangular parallelepiped having different vertical and horizontal lengths.

Further, the control device according to the present invention uses the calculation device, the teaching information, and the inner rotation start position calculated by the calculation unit to perform servo control for controlling the motor so that the inner track starts from the inner rotation start position. And a trajectory control unit for controlling the controller.
With such a configuration, it is possible to perform control using an appropriate inner turning start position by appropriately setting the shape information.

  According to the calculation device or the like according to the present invention, it is possible to achieve both prevention of interference and reduction of travel time by appropriately setting the shape information used for calculation of the inward start position.

The block diagram which shows the structure of the industrial robot by Embodiment 1 of this invention. The figure which shows the positional relationship of the manipulator and cassette in the same embodiment The figure which shows an example of the track | orbit and slot in the embodiment The figure which shows the track | orbit, a slot, etc. in the embodiment The figure for demonstrating the shape for position calculation in the embodiment The figure for demonstrating the shape for position calculation in the embodiment The figure which shows an example of the shape for position calculation in the embodiment The figure which shows an example of the inner periphery track | orbit in the embodiment The figure which shows an example of the inner periphery track | orbit in the embodiment The figure which shows an example of the time change of the speed according to the linear track | orbit in the embodiment The flowchart which shows operation | movement of the calculation apparatus by the embodiment A diagram showing an example of a conventional inner track A diagram showing an example of a conventional inner track

  Hereinafter, a control device according to the present invention will be described using embodiments. In the following embodiments, components and steps denoted by the same reference numerals are the same or equivalent, and repetitive description may be omitted.

(Embodiment 1)
A manipulator control apparatus according to Embodiment 1 of the present invention will be described with reference to the drawings. The manipulator control device according to the present embodiment calculates an inward start position using a three-dimensional shape.

  FIG. 1 is a block diagram showing a configuration of an industrial robot according to the present embodiment. The industrial robot according to the present embodiment includes a control device 1, a servo controller 2, and a manipulator 3. The manipulator 3 has a plurality of arms connected by joints driven by a motor. In the manipulator 3, each motor and the arm are connected via a speed reducer. A hand effector (end effector) may be provided at the tip of the arms connected in series. The manipulator 3 may be, for example, a manipulator of a horizontal articulated robot or a manipulator of a vertical articulated robot. The servo controller 2 controls the motor of each axis of the manipulator 3 according to control by the control device 1. The control device 1 controls the servo controller 2 so that the manipulator 3 operates according to teaching information to be described later by the teaching playback method. The industrial robot according to the present embodiment may be, for example, a transfer robot, a welding robot, an assembly robot, a painting robot, or other applications. It may be a robot. In this embodiment, an industrial robot that is a horizontal articulated robot and a transfer robot will be mainly described. That is, as shown in FIG. 2, the hand effector is a hand 3 a that conveys a conveyance target, and a conveyance target such as a wafer or a glass substrate is installed in each slot of the cassettes 41 to 43, or from each slot The case where the conveyance target is acquired will be mainly described.

  As shown in FIG. 1, the control device 1 according to the present embodiment includes a calculation device 11 that calculates an inward start position, and a trajectory control unit 12 that controls the servo controller 2 using the calculated inward start position. Is provided. The calculation device 11 includes a teaching information storage unit 21, a shape information storage unit 22, and a calculation unit 23.

  The teaching information storage unit 21 stores teaching information. The teaching information is information indicating two or more teaching points of the manipulator 3 having a plurality of arms connected by a joint driven by a motor. The teaching point may be, for example, a teaching point for each axis of the manipulator 3 or a teaching point at the tip of the manipulator 3. Further, the teaching information may be information indicating the posture of the tip of the manipulator 3 at the teaching point. That is, information indicating the posture of the hand effector of the manipulator 3 may be included in the teaching information. The information indicating the posture may be, for example, an angle indicating the direction of the hand effector in the horizontal direction (for example, an azimuth angle), or an angle indicating the direction of the hand effector in the spherical coordinate system (for example, an azimuth angle and The angle indicating the orientation of the hand effector in the spherical coordinate system and the twist angle that is the angle of rotation about the direction of the hand effector (for example, the angle corresponding to the twist of the wrist) It may be. For example, when the industrial robot is a transfer robot and the tilt of the hand, which is a hand effector, is not considered, the information indicating the posture may be only the azimuth indicating the direction of the hand. Each teaching point may be set with information such as a flag indicating whether or not the teaching point is inward. In addition, information indicating the speed corresponding to the trajectory connecting two teaching points adjacent in time may be included in the teaching information. The information indicating the speed may be indicated by a ratio to the maximum speed, such as 50% or 100%, for example.

  The process in which teaching information is memorize | stored in the teaching information storage part 21 is not ask | required. For example, teaching information may be stored in the teaching information storage unit 21 via a recording medium, and teaching information transmitted via a communication line or the like is stored in the teaching information storage unit 21. Alternatively, the teaching information input via the input device may be stored in the teaching information storage unit 21. Storage in the teaching information storage unit 21 may be temporary storage in a RAM or the like, or may be long-term storage. The teaching information storage unit 21 can be realized by a predetermined recording medium (for example, a semiconductor memory, a magnetic disk, an optical disk, etc.).

  The shape information storage unit 22 stores shape information which is information indicating the position calculation shape. The position calculation shape is a three-dimensional shape in which the length in the first direction and the length in the second direction, which is a direction different from the first direction, can be set independently. That is, the position calculation shape may be any shape that can be independently set in length in at least two directions. Therefore, a sphere or a cube whose length in one direction is determined if the length in one direction is determined is not included in the position calculation shape. The position calculation shape is a three-dimensional shape in which a reference point is set. The reference point is used for positioning the position calculation shape with respect to the teaching point that performs the inward rotation. In addition, a reference direction that is a reference direction may be set in the position calculation shape. The reference direction is used to determine the orientation of the position calculation shape with respect to the posture of the tip of the manipulator 3 at the teaching point that performs inward rotation. The reference direction may be a local coordinate system (for example, a three-dimensional orthogonal coordinate system) set in the position calculation shape, or may be other. In the present embodiment, a case will be mainly described in which both a reference point and a reference direction that is a local coordinate system are set in the position calculation shape. Using this shape information, the inward start position is calculated. The data format of the shape information is not limited. In the shape information, the information indicating the position calculation shape may be information indicating the vertex of the position calculation shape, may be information indicating the surface of the position calculation shape, or may be positioned by other methods. Information indicating a calculation shape may be used. In the shape information, the reference point may be indicated by coordinates, or the position of the reference point may be determined in advance. In the shape information, the reference direction may be determined in advance, or the reference direction may be indicated by a vector or the like.

  Here, a method for setting the shape information will be described. First, an operation in which the transfer robot sets a transfer target in each slot of the cassette and acquires a transfer target from each slot will be described. FIG. 3 is a diagram illustrating the movement of the hand 3a of the transfer robot. When the conveyance target is installed in the slot 52, the hand 3a moves from the position RA1 of the other slot to the installation start position RT, and thereafter, the top position ET, the middle position EM, the bottom position EB, and the acquisition start position RB. It moves in order and moves to the position RA2 of another slot. When acquiring the conveyance target from the slot 52, the hand 3a moves in the reverse order. In the trajectory, the position from the installation start position RT to the acquisition start position RB is fixed, but there are various paths from the installation start position RT to the acquisition start position RB. For example, as shown in FIG. 4, there are cases where approach is made from the upper and lower slots of the target slot 52, and cases where approach is made laterally or obliquely from another cassette. Then, it is necessary to set shape information for achieving an appropriate inward start position for such an approach from each direction. As shown in FIG. 4, the slots 51 to 53 in which the conveyance target is installed or acquired have a feature that the vertical width of the entrance is smaller than the horizontal width. Therefore, it is necessary to shorten the vertical inward starting distance so that the conveyance target and the hand 3a do not interfere with the slots 51 to 53. On the other hand, since there is no such limitation in the horizontal direction, it is preferable that the inward starting distance in the horizontal direction is increased in order to shorten the tact time. Therefore, a position calculation shape is set that is a rectangular parallelepiped that is short in the vertical direction and long in the horizontal direction compared to the vertical direction. Specifically, as shown in FIGS. 5A and 5B, in the vertical direction, the distance z11 between the height of the installation start position RT and the height of the bottom surface of the upper slot 53, and the height of the installation start position RT And the distance z12 from the height of the upper surface of the target slot 52. Among the horizontal directions, for the depth direction of the slot 52, the installation start position RT, the distance x11 between the inlet of the cassette 41, a straight line extending perpendicularly to the depth direction from the turning center of the manipulator 3, and the installation start position It sets using distance x12 with RT. Further, of the horizontal directions, the horizontal direction perpendicular to the depth direction is set using the installation start position and the distances y11 and y12 to the inner wall surface of the cassette 41. Note that x11, x12, y11, y12, z11, and z12 are all positive real numbers. FIG. 6 is a diagram illustrating an example of the position calculation shape 55 set using x11 or the like. In FIG. 6, the position calculation shape 55 is a rectangular parallelepiped having different lengths in the vertical direction and the horizontal direction when the xy plane of the local coordinate system is the horizontal direction. The position calculation shape 55 shown in FIG. 6 has a plane of x = x1, a plane of x = −x2, a plane of y = y1, and a plane of y = −y2 in a three-dimensional orthogonal coordinate system that is a local coordinate system. A rectangular parallelepiped surrounded by a plane, a plane with z = z1, and a plane with z = −z2. In FIG. 6, the reference point is set in advance to the origin in the local coordinate system, that is, (x, y, z) = (0, 0, 0), and the reference direction is the local coordinate system itself. And Therefore, in this case, the shape information may be, for example, information indicating each surface, that is, x1, −x2, y1, −y2, z1, and −z2. Note that x1, x2, y1, y2, z1, and z2 are numbers obtained by multiplying x11, x12, y11, y12, z11, and z12 by a number smaller than 1 and close to 1 (for example, 0.9). Suppose there is. The shape information is information indicating each vertex, that is, (x1, y1, z1), (x1, y1, -z2), (x1, -y2, z1), (x1, -y2, -z2), (-X2, y1, z1), (-x2, y1, -z2), (-x2, -y2, z1), (-x2, -y2, -z2) may be used. The reference position in the shape information is the position of the teaching point when measuring x11 or the like, that is, the installation start position RT. The reference direction is set according to the posture of the tip of the manipulator 3 at the teaching point. In the present embodiment, it is assumed that the local coordinate system is also set for the tip of the manipulator 3 (for example, the hand 3a). The local coordinate system is assumed to be a three-dimensional orthogonal coordinate system in which the xy plane is the mounting surface direction of the hand 3a, as shown in FIG. 5B. Since the local coordinate system is a relative coordinate system based on the posture of the hand 3a, the direction of the x-axis or the like changes according to the posture of the hand 3a. The shape information setting method described with reference to FIGS. 5A and 5B is merely an example, and needless to say, the method is not limited to this. For example, x12 may be determined by a method other than that described above.

  The process in which shape information is memorize | stored in the shape information storage part 22 is not ask | required. For example, shape information may be stored in the shape information storage unit 22 via a recording medium, and shape information transmitted via a communication line or the like is stored in the shape information storage unit 22. Alternatively, the shape information input via the input device may be stored in the shape information storage unit 22. The storage in the shape information storage unit 22 may be temporary storage in a RAM or the like, or may be long-term storage. The shape information storage unit 22 can be realized by a predetermined recording medium (for example, a semiconductor memory, a magnetic disk, an optical disk, etc.).

  When the calculation unit 23 performs an inward rotation at the teaching point on the end point side of two teaching points that are adjacent in time indicated by the teaching information, the calculating unit 23 determines the two teaching points between the two teaching points. An inward start position that is the intersection of the line segment and the position calculation shape with the teaching point on the end point side as a reference point is calculated. The position calculation shape is indicated by the shape information stored in the shape information storage unit 22. When the shape information includes the reference direction and calculates the inward rotation start position using the posture of the tip of the manipulator 3, the calculation unit 23 uses the teaching point on the end point side as the reference point, and teaches on the end point side. An inward start position that is an intersection of a position calculation shape that combines the posture of the manipulator 3 at the point and the reference direction and a line segment that connects the two teaching points may be calculated. As described above, when the local coordinate system that is the reference direction is set for the position calculation shape and the local coordinate system is also set for the tip of the manipulator 3, the calculation unit 23 calculates the position calculation shape. The reference point is located at the teaching point on the end point side, and the orientation of each axis of the local coordinate system set in the position calculation shape is a local coordinate according to the posture of the tip of the manipulator 3 at the teaching point on the end point side. The position calculation shape may be arranged so as to match the direction of each axis of the system. Then, the calculation unit 23 may calculate an inward start position that is an intersection of the arranged position calculation shape and a line segment connecting two teaching points. Of the two teaching points, the teaching point on the end point side is the teaching point on the rear side in time in the trajectory indicated by the teaching point. Even when the teaching information includes a teaching point other than the tip of the manipulator 3, the teaching point used in the calculation of the inward rotation start position is usually the teaching point at the tip of the manipulator 3. Further, the calculated coordinates of the inward start position are usually the coordinates of the global coordinate system (absolute coordinate system). The calculated inner loop start position may be stored in a recording medium (not shown).

  Here, the method by which the calculation unit 23 calculates the inward start position will be described with reference to FIGS. 7A and 7B. 7A and 7B, it is assumed that the orientation of the hand 3a at the taught installation start position RT is a vector orientation starting from the installation start position RT and ending at the top position ET. Then, the calculation unit 23 reads shape information such as x1, x2 from the shape information storage unit 22, and sets the position calculation shape 55 so that the installation start position RT becomes the reference point and the direction of the hand 3a becomes the reference direction. Deploy. Specifically, the calculation unit 23 calculates a coordinate value or the like in the global coordinate system indicating the position calculation shape 55. Then, the calculation unit 23 calculates the intersection of the line segment connecting the position RA1 of the other slot and the installation start position RT and the installed position calculation shape 55. The calculated intersection is the inward start position SP. Thus, since the inward start position SP is calculated using the position calculating shape 55, interference in the vertical direction can be avoided by setting the inward direction to a small inward direction (FIG. 7A). With respect to, the tact time can be shortened by setting it to a large inner diameter (FIG. 7B). Note that when the calculation unit 23 calculates the coordinate value or the like of the position calculation shape 55 in the global coordinate system, the global coordinates are only applied to the plane that intersects the line segment connecting the position RA1 of the other slot and the installation start position RT. Conversion to a system may be performed.

  The trajectory control unit 12 uses the teaching information stored in the teaching information storage unit 21 and the inner loop start position calculated by the calculation unit 23 to cause the servo controller 2 to start the inner loop from the inner loop start position. Control. When performing the inward rotation, the trajectory controller 12 may start the inward trajectory by superimposing the speeds from the inward rotation start position, or generate the inward trajectory from the inward rotation start position. Each axis of the manipulator 3 may pass through the inner track. In the present embodiment, the former case will be mainly described. Specifically, the trajectory control unit 12 calculates the distance from the inward start position SP to the installation start position RT in FIGS. 7A and 7B. And the time when the hand 3a becomes the position of the inward start position SP is calculated. For example, it is assumed that the movement is to be performed with the time change of the speed shown in FIG. The rising position of the speed corresponds to the position RA1 of the other slot, and the position of time “T2” corresponds to the installation start position RT. In such a case, the trajectory control unit 12 obtains a time “T1” when the area of the hatched line in FIG. 8 is equal to the distance from the inward start position SP to the installation start position RT. Then, since the time “T1” is the time when the inward rotation starts, the superposition with the speed from the installation start position RT to the top position ET starts from that time, and the inward trajectory starts. The time change of the speed used in the superposition of the speeds (for example, one shown in FIG. 8) may be included in the teaching information in advance, or generated by the trajectory control unit 12 using the teaching information. Also good. The trajectory control unit 12 may control the servo controller 2 so that each axis of the manipulator 3 passes through the trajectory interpolated between the teaching points indicated by the teaching information. If the teaching information includes a teaching point only at the tip of the manipulator 3, the trajectory control unit 12 may calculate the position of each axis using the teaching point. The processing by the trajectory control unit 12 is the same as the control of the servo controller in the conventional robot that performs the inward rotation, and detailed description thereof is omitted.

  Note that the teaching information storage unit 21 and the shape information storage unit 22 may be realized by the same recording medium or may be realized by separate recording media. In the former case, the area storing the teaching information becomes the teaching information storage unit 21, and the area storing the shape information becomes the shape information storage unit 22.

  Here, the servo controller 2 will be briefly described. The servo controller 2 receives a command for the position of each axis of the manipulator 3 from the control device 1. The servo controller 2 generates a torque command value corresponding to the motor of each axis by feedback control in accordance with the command of the position of each axis. Each motor of the manipulator 3 operates according to this torque command value. The feedback control by the servo controller 2 is already known, and detailed description thereof is omitted.

Next, operation | movement of the control apparatus 1 is demonstrated using the flowchart of FIG.
(Step S <b> 101) The trajectory control unit 12 reads the information of the next teaching point from the teaching information storage unit 21. At the start of movement, the trajectory control unit 12 may read both information on the teaching point of the movement source and information on the teaching point of the movement destination.

  (Step S <b> 102) The trajectory control unit 12 determines whether or not to perform inward rotation at the teaching point read in Step S <b> 101, that is, the teaching point on the end point side in the trajectory. Then, if the inward rotation is performed, the process proceeds to step S103, and if not, the process proceeds to step S105.

  (Step S103) The calculation unit 23 calculates an inward start position for the teaching point read in step S101. Specifically, the calculation unit 23 reads the shape information from the shape information storage unit 22, and the reference point and reference direction of the position calculation shape indicated by the shape information are the position of the teaching point read in step S101 and its reference point. A position calculation shape is set so that the posture of the tip of the manipulator 3 is at the teaching point. Then, the calculation unit 23 calculates an inner turn start position that is an intersection of the teaching point read in step S101 and a line segment connecting the teaching point immediately before it and the position calculation shape.

  (Step S104) The trajectory control unit 12 calculates the time to start the inner loop using the inner loop start position calculated in Step S103.

  (Step S105) The trajectory control unit 12 generates each position between the teaching point read out in step S101 and the teaching point one before the teaching point by interpolation. When the position exceeds the inward rotation start position, the positions are generated so as to pass through the inward trajectory by superimposing the velocities. This position generation may be performed, for example, at predetermined time intervals (for example, 5 ms).

  (Step S106) The trajectory control unit 12 determines whether to read information on the next teaching point. And when reading, it returns to step S101, and when that is not right, it returns to step S105. For example, when the trajectory control unit 12 performs an inward rotation and the position to be calculated exceeds the inward rotation start position, information on the next trajectory is necessary. You may judge that it reads. The trajectory control unit 12 may determine that the next teaching point information is read when the position calculation is completed up to the teaching point on the end point side, for example, when the inward rotation is not performed. Until it is determined that the process returns to step S101, the trajectory control unit 12 repeats the process of step S105 to repeat the interpolation between the teaching points and the calculation of the position according to the inner trajectory.

  In the flowchart of FIG. 9, the process is terminated by power-off or a process termination interrupt. In the flowchart of FIG. 9, as long as at least the calculation of the inward start position in step S103 is performed, the method for controlling the tip of the manipulator 3 to pass through the inward trajectory is not limited. For example, for each teaching point that performs an inward turn indicated by the teaching information, an inner turn start position may be calculated in advance by batch processing, and control may be performed using the inner turn start position when the manipulator 3 is controlled.

Next, the operation of the control device 1 according to the present embodiment will be described using a specific example. In this specific example, it is assumed that the following teaching information is stored in the teaching information storage unit 21.
Position (1), Pose (1), Flag (1)
Position (2), Pose (2), Flag (2)
Position (3), Pose (3), Flag (3)
:
:
Position (10), Pose (10), Flag (10)
Position (11), Pose (11), Flag (11)
Position (12), Pose (12), Flag (12)
:
:

  In this teaching information, Position (N) is information indicating the position of each axis of the manipulator 3. Here, N is an integer of 1 or more. It is assumed that the position (N) includes the position of the hand 3 a that is the tip of the manipulator 3. Pose (N) is information indicating the angle of the hand 3 a that is the tip of the manipulator 3. Flag (N) is a flag indicating whether or not to perform inward rotation at the Nth teaching point. When Flag (N) is “1”, the inner loop is performed, and when Flag (N) is “0”, the inner loop is not performed. That is, when Flag (N) is “0”, the manipulator 3 passes the position indicated by Position (N) corresponding thereto. Further, Position (10) corresponds to the position RA1 of the other slot in FIG. 7B, Position (11) corresponds to the installation start position RT in FIG. 7B, and Position (12) corresponds to the top position ET in FIG. 7B. It shall correspond to. Further, it is assumed that Flag (10) = Flag (12) = 0 and Flag (11) = 1.

  Further, in this specific example, it is assumed that shape information “x1, x2, y1, y2, z1, z2” indicating the position calculation shape 55 in FIG. 6 is stored in the shape information storage unit 22. In the position calculation shape 55, the reference direction is the direction of each axis of the local coordinate system, that is, (x, y, z) = (1, 0, 0), (0, 1, 0), It is assumed that (0, 0, 1) is set.

  The trajectory control unit 12 sequentially reads the teaching point information from Position (1), Pose (1), and Flag (1), calculates the inward starting position and the like as appropriate, and calculates the trajectory (steps S101 to S106). ). It is assumed that Position (11), Pose (11), and Flag (11) are read in this way (step S101). The trajectory control unit 12 calculates the speed from Position (10) to Position (11) in response to a new readout. The speed is, for example, as shown in FIG. Further, the trajectory control unit 12 determines whether Flag (11) is “1” (step S102). In this case, since Flag (11) = 1, the trajectory control unit 12 determines that the inner loop is to be performed, and the calculation device 11 determines that the inner loop is N = 11, which is information for identifying the teaching point performing the inner loop. An instruction to calculate the start position is passed. Then, the calculation unit 23 reads Position (11) and Pose (11) where N = 11 from the teaching information stored in the teaching information storage unit 21, and the shape information “ x1, x2, y1, y2, z1, z2 "are read out. Then, the calculation unit 23 determines that the position indicated by the read Position (11) is the origin (reference point) of the local coordinate system of the position calculation shape 55 and corresponds to the posture of the hand 3a indicated by Pose (11). The position calculation shape 55 is arranged in the global coordinate system so that the direction of each axis of the local coordinate system matches the direction of the local coordinate system of the position calculation shape 55. Further, the calculation unit 23 reads Position (10) and, as shown in FIG. 7B, the position (RA1) of the hand 3a indicated by Position (10) and the position of the hand 3a indicated by Position (11) ( RT) and the coordinates of the inward start position SP that is the intersection of the position calculation shape 55 arranged as described above are calculated and passed to the trajectory control unit 12 (step S103). Note that the calculated coordinates of the inward start position SP are coordinates in the global coordinate system.

  When receiving the inward start position SP from the calculation device 11, the trajectory control unit 12 calculates an inward start distance that is a distance from the position of the hand 3a indicated by Position (11) to the inward start position SP. Then, the trajectory control unit 12 uses the speed change from Position (10) to Position (11) that has been calculated in advance, and at the time when the distance to Position (11) becomes equal to the calculated inward start distance. A certain inner loop start time is calculated (step S104). Thereafter, the trajectory control unit 12 calculates each position from Position (10) to Position (11) according to a predetermined interpolation interval, and the servo controller 2 moves the manipulator 3 to each position. Is instructed (steps S105 and S106). When the trajectory control unit 12 calculates the position to correspond to the inner start time calculated in step S104, the trajectory control unit 12 determines to read the next teaching point, and Position (12), Position (12), Flag (12) is read (step S101). Then, the trajectory controller 12 calculates the speed from Position (11) to Position (12). In this case, since Flag (12) = 0 and no inward rotation is performed, the trajectory control unit 12 superimposes the speeds from the inward rotation start time and sets the position of each trajectory up to Position (12). Calculation is performed (steps S105 and S106). In this way, the position of the manipulator 3 is calculated sequentially according to the teaching information, and the manipulator 3 is controlled. Further, in the case of performing the inward rotation, as shown in FIGS. 7A and 7B, the calculation of the inward start position SP using the position calculating shape 55 and the superposition of the speeds from the calculated inward start position SP are performed. And the hand 3a, which is the tip of the manipulator 3, is controlled so as to move along the inner track.

  As described above, according to the control device 1 according to the present embodiment, by appropriately setting the shape information, for example, when setting the conveyance target in the slot or acquiring the conveyance target from the slot, It is possible to prevent the object to be transported or the hand and the slot from interfering with each other, and it is also possible to avoid restricting the inner circle unnecessarily, and to prevent the movement time from being prolonged. That is, the control device 1 can calculate an appropriate inward turning start position corresponding to approaches from various directions. Also, by calculating the inward start position using the position calculation shape that combines the posture of the tip of the manipulator 3 and the reference direction, it is possible to calculate an appropriate inward start position in consideration of the posture of the tip of the manipulator 3. It becomes. That is, in the case of using a position calculation shape that does not consider the posture of the tip of the manipulator 3, in order to realize appropriate interference avoidance regardless of the orientation, the position calculation shape is, for example, a horizontal base. May have to be a square column with a square shape, a cylinder with a horizontal base in a circle, or the like, and as a result, the inner circumference in a certain direction may be reduced. On the other hand, by using the position calculation shape arranged in accordance with the posture of the tip of the manipulator 3, it is not necessary to limit the inner circumference unnecessarily, and appropriate inner circumference control can be performed.

In the present embodiment, the case where the position calculation shape is a rectangular parallelepiped having different lengths in the vertical direction and the horizontal direction has been mainly described, but this need not be the case. The position calculation shape may be, for example, a three-dimensional shape in which the length in the first direction that is the vertical direction and the length in the second direction that is the horizontal direction can be set independently. Note that the position calculation shape may be one in which the lengths in the first and second directions can be set independently, and the length in three or more directions can be set independently. Good. Specific examples of the position calculation shape in which the lengths in two or more directions can be set independently include a cylinder, a polygonal column, an elliptical column, a cone, a pyramid, and an ellipsoid. When the position calculation shape is a cylinder or a cone, the height and the diameter of the bottom surface can be set independently. When the position calculation shape is a polygonal column or a pyramid, the height and the shape of the bottom surface can be set independently. For example, if the bottom surface of a position calculation shape cylinder or polygonal cylinder, ellipse, cone, or pyramid is in the horizontal direction, the position calculation shape has two independent lengths, the vertical direction and the horizontal direction. Can be set. Moreover, when the shape for position calculation is an ellipsoid, it may be a shape represented by the equation x ² / a ² + y ² / b ² + z ² / c ² = 1. In the equation, a, b, and c may be coefficients that can be set independently, or a = b, and a and c may be coefficients that can be set independently. When a = b, the lengths in the two directions of the vertical direction and the horizontal direction can be set independently. However, it is assumed that the xy plane is the horizontal direction and the z-axis is the vertical direction.

  Moreover, the method in which the shape information memorize | stored in the shape information storage part 22 shows the shape of the shape for position calculation is not ask | required. For example, as described above, the position calculation shape may be indicated by the position of the vertex or the surface, or the position calculation shape may be indicated by an equation.

  Moreover, although the case where only one position calculation shape is used has been described in the present embodiment, this need not be the case. The calculation unit 23 may calculate the inward start position using a plurality of position calculation shapes. In this case, in the teaching information, a shape information identifier for identifying the shape information used for calculating the inward start position may be associated with the instructed teaching point. Then, when calculating the inward start position for a certain teaching point, the calculation unit 23 reads the shape information identified by the shape information identifier corresponding to the teaching point, and calculates the position calculation shape indicated by the read shape information. It may be used to calculate the inward start position. Specifically, shape information indicating a position calculation shape with the installation start position RT as a teaching point as a reference point, and shape information indicating a position calculation shape with the acquisition start position RB as a teaching point as a reference point; If they are different, the inner start position may be calculated using shape information corresponding to those teaching points. Further, when the height between slots differs for each cassette, the inner start position may be calculated for each cassette using shape information corresponding to the height between the slots.

  In the present embodiment, the reference direction is also set in the position calculation shape, and the case where the inward start position is calculated using the reference direction has been mainly described, but this need not be the case. The reference direction is not set in the position calculation shape, and the inward start position may be calculated without using the reference direction. In that case, for example, the position calculation shape may be the same shape regardless of the posture of the tip of the manipulator 3, for example, a cylinder having a horizontal bottom surface or the like. .

  Moreover, although this Embodiment demonstrated the case where the control apparatus 1 included the calculation apparatus 11, it may not be so. The calculation device 11 and the control device that controls the manipulator 3 may be different devices. In this case, for example, the calculation device 11 calculates an inner circle start position corresponding to each of the teaching points that perform the inner rotation among the teaching points indicated by the teaching information, and adds the inner rotation start position to the teaching information. 3 may be passed to a control device that controls the control unit 3. Therefore, when the calculation device 11 and the control device that controls the manipulator 3 are different devices, the calculation device 11 stores an output unit (not shown) that accumulates the calculated inner rotation start position or passes it to the control device. You may have. Further, when the calculation device 11 and the control device that controls the manipulator 3 are different devices, the teaching information storage unit 21 included in the calculation device 11 temporarily stores the teaching information stored in the control device. The storage unit may or may not be.

  Further, in this embodiment, since the position calculation shape with the posture of the tip of the manipulator 3 as the reference direction is arranged, each axis of the local coordinate system corresponding to the posture of the tip of the manipulator 3 and the position calculation shape Although the case where each axis of the local coordinate system is matched has been mainly described, this need not be the case. For example, when the orientation of the tip of the manipulator 3 indicated in the teaching information is only an azimuth angle, the orientation corresponding to the orientation of the tip of the manipulator 3 and the reference direction set in the position calculation shape are one. The position calculation shape may be arranged so that the direction (for example, the x-axis) matches. In this case, since there is a degree of freedom regarding rotation around the reference direction when the position calculation shape is arranged, a surface that is set in the horizontal direction in advance in the position calculation shape (for example, an xy plane) May be determined.

  In the present embodiment, the case where the industrial robot is a transport robot and the tip of the manipulator 3 is the hand 3a has been mainly described, but it is needless to say that it is not necessary. The manipulator 3 may be a manipulator such as a painting robot or a welding robot other than the transport robot. Even in such a case, for example, when performing an operation of inserting a hand effector into a recess having the same shape as a slot, the interference is prevented by calculating the inward start position as in the present embodiment. However, the effect that the traveling time can be shortened can be obtained.

  In the above embodiment, each process or each function may be realized by centralized processing by a single device or a single system, or may be distributedly processed by a plurality of devices or a plurality of systems. It may be realized by doing.

  In the above embodiment, the information exchange between the components is performed by one component when, for example, the two components that exchange the information are physically different from each other. It may be performed by outputting information and receiving information by the other component, or when two components that exchange information are physically the same, one component May be performed by moving from the phase of the process corresponding to to the phase of the process corresponding to the other component.

  In the above embodiment, information related to processing executed by each component, for example, information received, acquired, selected, generated, transmitted, or received by each component In addition, information such as threshold values, mathematical formulas, addresses, etc. used by each component in processing is retained temporarily or over a long period of time on a recording medium (not shown) even when not explicitly stated in the above description. It may be. Further, the storage of information in the recording medium (not shown) may be performed by each component or a storage unit (not shown). Further, reading of information from the recording medium (not shown) may be performed by each component or a reading unit (not shown).

  In the above embodiment, when information used by each component, for example, information such as a threshold value, an address, and various setting values used by each component may be changed by the user Even if it is not specified in the above description, the user may be able to change the information as appropriate, or it may not be. If the information can be changed by the user, the change is realized by, for example, a not-shown receiving unit that receives a change instruction from the user and a changing unit (not shown) that changes the information in accordance with the change instruction. May be. The change instruction received by the receiving unit (not shown) may be received from an input device, information received via a communication line, or information read from a predetermined recording medium, for example. .

  In the above embodiment, each component may be configured by dedicated hardware, or a component that can be realized by software may be realized by executing a program. For example, each component can be realized by a program execution unit such as a CPU reading and executing a software program recorded on a recording medium such as a hard disk or a semiconductor memory. At the time of execution, the program execution unit may execute the program while accessing the storage unit or the recording medium. The program may be executed by being downloaded from a server or the like, or may be executed by reading a program recorded on a predetermined recording medium (for example, an optical disk, a magnetic disk, a semiconductor memory, or the like). Good. The program may be used as a program constituting a program product. Further, the computer that executes the program may be singular or plural. That is, centralized processing may be performed, or distributed processing may be performed.

  Further, the present invention is not limited to the above-described embodiment, and various modifications are possible, and it goes without saying that these are also included in the scope of the present invention.

  As described above, according to the calculation device or the like according to the present invention, an effect that the inner-circulation start distance can be changed according to the moving direction is obtained, and it is useful as a device that calculates the inner-circulation start position of the manipulator.

DESCRIPTION OF SYMBOLS 1 Control apparatus 2 Servo controller 3 Manipulator 3a Hand 11 Calculation apparatus 12 Orbit control part 21 Teaching information storage part 22 Shape information storage part 23 Calculation part

Claims (6)

A teaching information storage unit for storing teaching information indicating two or more teaching points of a manipulator having a plurality of arms connected by a joint driven by a motor;
It is a three-dimensional shape in which a reference point is set, and is a three-dimensional shape in which the length in the first direction and the length in the second direction, which is a direction different from the first direction, can be set independently. A shape information storage unit that stores shape information indicating a position calculation shape;
With respect to two adjacent teaching points indicated by the teaching information, a line segment between the two teaching points and a position calculation shape indicated by the shape information with the teaching point on the end point side as a reference point And a calculation unit that calculates an inner circle start position that is an intersection. The teaching information is information indicating the posture of the tip of the manipulator at the teaching point,
In the position calculation shape, a reference direction which is a reference direction is also set.
The calculation unit uses a teaching point on the end point side as a reference point, and a position calculation shape that combines the attitude of the tip of the manipulator at the teaching point on the end point side and a reference direction, and between the two teaching points. The calculation device according to claim 1, wherein an inward start position that is an intersection with a line segment is calculated. The calculation device according to claim 1, wherein the first direction is a vertical direction and the second direction is a horizontal direction. The calculation device according to claim 3, wherein the position calculating shape is a rectangular parallelepiped having different vertical and horizontal lengths. A calculation device according to any one of claims 1 to 4,
A trajectory control unit that controls a servo controller that controls the motor so that an inward trajectory is started from the inward rotation start position using the teaching information and the inward rotation start position calculated by the calculation unit, Control device with. Teaching information stored in a teaching information storage unit storing teaching information indicating two or more teaching points of a manipulator having a plurality of arms connected by a joint driven by a motor, and a reference point set 3 A shape that is a three-dimensional shape that is a three-dimensional shape that is a three-dimensional shape that can be independently set with a length in the first direction and a length in a second direction that is different from the first direction. Using the shape information stored in the shape information storage unit in which information is stored, regarding the two adjacent teaching points indicated by the teaching information, the line segment between the two teaching points and the end point side A calculation method comprising a calculation step of calculating an inward start position that is an intersection with the position calculation shape indicated by the shape information, using the taught point as a reference point.

Images