JP5656189B2 - Trajectory information generation device for articulated robot arm type transfer device - Google Patents

Description

  The present invention relates to an apparatus for generating trajectory information related to a trajectory for controlling an articulated robot arm type transport apparatus and a speed on the trajectory, and in particular, trajectory information generation for obtaining trajectory information for realizing proper driving of the transport apparatus. Relates to the device.

  The transfer device is a device that moves a transfer object using a transfer mechanism of an articulated robot arm in which a plurality of links are rotatably connected, as exemplified in Patent Document 1, and includes a preset trajectory and trajectory. Generally, the conveyance control is performed using the trajectory information regarding the upper speed. The trajectory information used for the transport control is generated in advance by the trajectory information generation device in advance of the transport control. Many of the trajectory information generation devices, as exemplified in Patent Document 2, use an optimization method based on a dynamic model of a robot arm to provide an appropriate trajectory that minimizes the travel time from a desired start point to an end point. It is common to generate information.

JP 2003-145461 A JP-A-7-200030

  However, the trajectory information obtained by the conventional trajectory information generation device is intended to increase the moving speed as much as possible to minimize the moving time. Along with this, vibration that cannot be ignored may occur in the movement control, and it is necessary to take measures such as reducing the movement speed to such an extent that the generated vibration does not cause a problem, which may lead to a decrease in movement efficiency.

  Moreover, in the articulated robot arm type transfer device as described above, it is common to use a dynamic model that can cope with various operations including linear and curved operations as a dynamic model of the transfer device. In such a conventional general dynamic model, if the linear motion that cannot be realized unless the rotational motion of multiple links is performed simultaneously and appropriately is set as the constraint, the constraint becomes strict for the optimization calculation. There is a case where the convergence time of the calculation is deteriorated and the time required for generating the trajectory information is increased.

  The present invention has been made paying attention to such a problem, and its purpose is to reduce the time required for generating trajectory information when moving a conveyance object from a desired start point to an end point in a straight line. Another object of the present invention is to provide a new trajectory information generation device that generates trajectory information that achieves both reduction in travel time and reduction in vibration caused by movement.

  In order to achieve the above object, the present invention takes the following measures.

That is, the trajectory information generation device of the articulated robot arm type transport device according to the present invention is configured to linearly move the object to be transported from a desired start point to an end point with respect to the articulated robot arm type transport device in which a plurality of links are rotatably connected. An apparatus for generating trajectory information related to a trajectory to be moved and a speed on the trajectory, including receiving means for receiving start point information and end point information, information received by the receiving means, and a dynamic model of the transfer device And a trajectory information generating means for generating the trajectory information using an optimization method so that the value of the evaluation function including the time required for moving from the start point to the end point as a parameter is minimized. The trajectory information generation means sets in advance a model in which the motion of the tip link for transporting the transport object is limited to a linear motion as the dynamic model. Together with Ku, single mass only displaced in a direction of the linear motion as the vibration characteristic model of the transport device - previously set the spring model, the size of the simulated vibration caused by the movement of the end link using the vibration characteristic model Calculating the length, including the calculated magnitude of the simulated vibration as one of the parameters in the evaluation function, and generating the trajectory information so that the value of the evaluation function including at least the two parameters is minimized. Features.

According to this configuration, the trajectory information is generated using the optimization method so that the value of the evaluation function that combines the magnitude of the simulated vibration caused by the movement and the movement time is minimized. The movement time is minimized and the movement efficiency can be improved. Moreover, the operation of the distal end link carrying the transport object as dynamic model of the transport device is preset models is limited to linear operation, Rutotomoni using this model, the linear motion as the vibration characteristic model of the transport device Since a single mass-spring model that displaces only in the direction of is set in advance and the simulated vibration is calculated using this vibration characteristic model , the constraint condition of linear motion becomes stricter for the optimization process. Therefore, it is possible to improve the convergence of the optimization calculation and reduce the calculation time required for generating the trajectory information.

  In particular, in order to reduce the calculation time required to generate the trajectory information and realize the loading / unloading operation to / from the load port or the like, the trajectory information generation means is imposed with a constraint condition that makes the direction of the tip link constant. It is preferable that

  In order to be able to easily change which of vibration suppression or travel time reduction is prioritized, a change operation is accepted, and the weight of the simulated vibration magnitude and the travel time for the evaluation function is changed according to the change operation It is desirable to have weight change means for

  As described above, the present invention generates the trajectory information using the optimization method so that the value of the evaluation function that combines the magnitude of the simulated vibration caused by the movement and the movement time is minimized. Considering this, the travel time is minimized and the travel efficiency can be improved. In addition, as a dynamic model of the transfer device, a model in which the movement of the tip link for transferring the object to be transferred is limited to linear movement is used in advance, and this model is used. On the other hand, it is possible to improve the convergence of the optimization calculation and reduce the calculation time required to generate the trajectory information. Therefore, it is possible to provide transport that reduces both the travel time and the vibration caused by the movement while reducing the time required to generate the trajectory information.

The block diagram which shows typically the track | orbit information generation apparatus of the conveying apparatus which concerns on one Embodiment of this invention. The top view which shows the conveyance mechanism of the conveyance apparatus. The figure which shows a vibration characteristic model typically. The figure which shows the movable area | region of the conveying apparatus. The figure regarding the orbit information produced | generated. The figure which shows the comparison result with the case where vibration suppression is considered in addition to the case where only the shortest time is pursued.

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings.

  As shown in FIG. 1, a transfer device 103 that transfers a transfer object W such as a semiconductor wafer by driving a transfer mechanism 131 such as a robot arm is preliminarily stored in a memory and has track information on the speed on the track. Based on 132, the conveyance control of the conveyance target object W is performed. The transfer apparatus 103 according to the present embodiment uses the movement area of the transfer object W in the rectangular passage ac shown in FIG. 4 and in the loading / unloading section Lo such as a plurality of load ports arranged adjacent to the passage ac. .

  As shown in FIGS. 1 and 4, the trajectory information generation device 2 of the transport device according to the present embodiment is a device that generates trajectory information 132 used for transport control of the transport device 103. Track information generating means 22 for generating track information 132 for linearly moving between the entrance p2 (start point S) and the interior p3 (end point E) of the exit Lo.

  The accepting means 21 accepts input of various information such as a starting point and an ending point of the trajectory necessary for generating the trajectory information 132, and positional information serving as a constraint condition using a known operation unit such as a display, a keyboard, and a mouse. Is. Of course, the receiving means may be configured as a teaching pendant (teaching device), or may be configured to receive settings by communication from a host system or a support tool.

  The trajectory information generation unit 22 includes an optimization processing unit 23 that generates optimal trajectory information from the start point S to the end point E based on the information received by the reception unit 21. The optimization processing unit 23 includes: In an information processing apparatus such as a personal computer equipped with a CPU, memory, various interfaces, etc., software and hardware are realized in cooperation by executing a trajectory information generation processing routine (not shown) stored in advance by the CPU. is there.

  The optimization processing unit 23 is an appropriate trajectory information that can be derived based on a dynamic model, a vibration characteristic model, and a constraint condition that are stored in advance in the memory. Orbital information is generated by an optimization method.

Specifically, the transport mechanism 131 to be driven is a so-called articulated robot arm in which a plurality of links 134, 135, and 136 are rotatably connected in series as shown in FIG. The object to be conveyed is changed by changing the angles θ ₁ , θ _2, and θ ₃ between the links by driving a motor (not shown) in each joint in a state where the object to be conveyed W is placed on the holding portion 136b set at the tip of the link. The object W is moved to a desired position. The transport mechanism 131 of the present embodiment includes a first link 134 having a base end 134a connected to the base 133, a second link 135 having a base end 135a connected to the tip 134b of the first link 134, And a third link 136 in which a base 136a is connected to the distal end 135b of the second link 135 and a holding portion 136b for placing the object to be conveyed W is set at the distal end. The links 134 to 136 are each horizontal. A three-axis horizontal articulated robot is configured so as to be rotatable in the direction. The lengths of the links 134 to 136 are set to L ₁ , L ₂ , and L ₃ , respectively.

Ｊ＝ｄｉａｇ｛Ｊ_ｉ｝， Ｊ_ｉ＞０
Ｇ＝ｄｉａｇ｛Ｇ_ｉ｝， Ｇ_ｉ＞０
ｉ＝１，２，３
ｚ＝［ｘ，ｙ，φ］^Ｔ
τ＝［τ_１，τ_２，τ_３］^Ｔ
上部に二つの点を付記した変数は、加速度を示す。
上付き添え字の「−１」は逆行列を意味し、上付き添え字の「Ｔ」は行列またはベクトルの転置を意味する。
ｘ_ａは、エンドエフェクタ（保持部１３６ｂ）の変位を示す。
Ｍ_ａは、エンドエフェクタ（保持部１３６ｂ）の質量を示す。
Ｊ_ｉは、第ｉ関節のモータの慣性モーメントである。
Ｇ_ｉは、第ｉ関節を駆動するモータに取り付けられた減速器のギア比である。
τ_ｉは、第ｉ関節のモータの駆動トルクである。
ｘ，ｙは、エンドエフェクタ（保持部１３６ｂ）の位置を示す。
φは、エンドエフェクタ（保持部１３６ｂ）の姿勢（向き）を示す。
Ｓ₁＝ｓｉｎ（θ₁₀）
Ｓ₁₂＝ｓｉｎ（θ₁₀＋θ₂₀）
Ｓ₁₂₃＝ｓｉｎ（θ₁₀＋θ₂₀＋θ₃₀）
Ｃ₁＝ｃｏｓ（θ₁₀）
Ｃ₁₂＝ｃｏｓ（θ₁₀＋θ₂₀）
Ｃ₁₂₃＝ｃｏｓ（θ₁₀＋θ₂₀＋θ₃₀）
θ₁₀，θ₂₀，θ₃₀は、それぞれ各関節の直線動作開始位置における角度であり、これらは定数として設定される。なお、上記の動力学モデルや振動特性モデルに係る情報は、図１に示すように、予め軌道情報生成手段２２のメモリに記憶されているが、受付手段２１を通じて設定可能に構成してもよい。 In the dynamic model of the transport mechanism 131, as shown in FIG. 4, the end effector (holding part 136b) moves only in the direction of the arrow X, and the Y direction and the posture (the direction of the holding part 136b) are fixed. Paying attention, the dynamic model of a general robot is converted into one limited to one end (X direction) motion of the end effector (tip link 136), and the mass distribution of each link 134, 135, 136 is linked. As a concentrated load at the tip, it is simply expressed by the following equation. Note that i indicates a link number. For example, i = 1 is the first link 134, i = 2 is the second link 135, and i = 3 is the third link 136. Here, the formula is constructed assuming that the mass distribution of each link is a concentrated load at the link tip, but the formula may of course be constructed assuming that the mass distribution of each link is around the center of gravity of the link. Further, as a vibration characteristic model for considering vibration caused by movement, a virtual mass-spring model that is displaced only in the X direction as shown in FIG. 3 is applied, and the spring constant K is an end measured by experiment. It is calculated based on the natural frequency f of the effector (holding part 136b).

J = diag {J _i }, J _i > 0
G = diag {G _i }, G _i > 0
i = 1, 2, 3
z = [x, y, φ] ^T
τ = [τ ₁ , τ ₂ , τ ₃ ] ^T
Variables with two dots at the top indicate acceleration.
The superscript “−1” means an inverse matrix, and the superscript “T” means a matrix or vector transpose.
x _a denotes the displacement of the end effector (holding portion 136 b).
M _a represents the mass of the end effector (holding portion 136b).
J _i is the moment of inertia of the i-th joint motor.
G _i is the gear ratio of the speed reducer attached to the motor that drives the i-th joint.
τ _i is the driving torque of the i-th joint motor.
x and y indicate the position of the end effector (holding portion 136b).
φ indicates the posture (direction) of the end effector (holding portion 136b).
S ₁ = sin (θ ₁₀ )
S ₁₂ = sin (θ ₁₀ + θ ₂₀ )
S ₁₂₃ = sin (θ ₁₀ + θ ₂₀ + θ ₃₀ )
C ₁ = cos (θ ₁₀ )
C ₁₂ = cos (θ ₁₀ + θ ₂₀ )
C ₁₂₃ = cos (θ ₁₀ + θ ₂₀ + θ ₃₀ )
θ ₁₀ , θ ₂₀ , and θ ₃₀ are angles at the linear motion start positions of the respective joints, and these are set as constants. In addition, although the information regarding said dynamic model and vibration characteristic model is previously memorize | stored in the memory of the track information generation means 22, as shown in FIG. .

As one of the restriction conditions of the trajectory information, the drive restriction of the motor that rotates each link 134, 135, 136 is preset in the memory shown in FIG. This motor drive constraint uses the maximum torque that the motor can output, and is expressed by the following equation. This maximum torque is set to a value obtained by subtracting the friction torque from the rated torque of the actuator. τ _li is the maximum torque in the case of reverse rotation (attempts to drive θ _i in a negative direction), and τ _ui represents the maximum torque in the case of forward rotation (attempts to drive θ _i in a positive direction). Of course, it may be expressed by speed or acceleration.

In the present embodiment, as shown in FIG. 4, since the transport mechanism 131 operates linearly, the position change amount and the posture change amount in the Y direction of the end effector (holding portion 136b) must always be zero. is there. Therefore, the following constraints are imposed to limit the position of the trajectory in a straight line.

The two pieces of position information of the start point and the end point received by the receiving unit 21 are a start point position and an end point position indicated by XY coordinates, and a start point speed and an end point speed, which are one of the constraint conditions. In the present embodiment, the end point speed is treated as zero.

The above represents the start point position, end point position, start point speed, and end point speed in order from left to right. t _e represents the travel time. The start point velocity and the end point velocity are velocity vectors having a velocity and a direction. However, since only the linear motion is performed, the direction is fixed in the X direction. Note that the information related to the Y direction, the posture of the end effector, the constraint conditions regarding the start point and the end point are stored in advance in the memory of the trajectory information generation unit 22 as shown in FIG. It is configured.

Under the above model and constraint conditions, the trajectory information on the trajectory and the velocity on the trajectory is obtained by an optimization method so that the value of the evaluation function represented by the following formula is minimized.

The evaluation function, the magnitude of the simulated vibrations calculated by the vibration characteristics model (x-x _a) as one of the parameters, as one of the travel time t _e parameters, the function including these two parameters at least is there. Specifically, those bound to travel time t _e is multiplied by the simulation weighting information on the size (x-x _a) is the weighting factor of the vibration alpha. In addition, since the general method is used about the optimization method, description is abbreviate | omitted here. Further, as shown in FIG. 1, there is provided a weight changing means 24 for changing the weighting coefficient α in accordance with a changing operation performed by the receiving means 21. By changing the weighting coefficient α, vibrations can be suppressed or minimized. It is possible to change whether priority is given to time reduction. In the present embodiment, since the size of the simulated vibration (x-x _a) comprises a negative value, which was square but in the positive value, the expression taking the absolute value of (x-x _a) May be.

Here, discretization is considered in order to reduce the calculation time in solving the above optimization problem, and the trajectory information 132 is also discretized and generated accordingly. Specifically, the trajectory information 132 is represented by a plurality of points P ₀ to N including a start point S and an end point E, and the moving trajectory Pt is equally divided into N by a time interval T as schematically shown in FIG. The xy coordinates (x _k , y _k ) of each point P _k are expressed by the angles θ _1k · θ _2k · θ _3k of the links 134 to 136 using the following equations.
x _k = L ₁ cos θ _1k + L ₂ cos (θ _1k + θ _2k ) + L ₃ cos (θ _1k + θ _2k + θ _3k )
y _k = L ₁ sin θ _1k + L ₂ sin (θ _1k + θ _2k ) + L ₃ sin (θ _1k + θ _2k + θ _3k )

始点及び終点における位置や速度の制約条件：

アクチュエータトルクの制約条件：

ただし、τ_ｉ［ｋ］＝τ_ｉ（ｋＴ）である。
ロボット手先（エンドエフェクタ１３６、先端リンク１３６）のＹ方向動作及び姿勢（回転動作）の制約条件：

評価関数：

Then, when the model and the constraint condition described above are discretized using the central difference method, it is expressed by the following expression. Hereinafter, the position of the point _{P k} of the k time th abbreviated as x [k] = x (kT ).
Model considering vibration:

Position and velocity constraints at the start and end points:

Actuator torque constraints:

However, τ _i [k] = τ _i (kT).
Restrictions on Y-direction motion and posture (rotational motion) of robot hand (end effector 136, tip link 136):

Evaluation function:

  When the trajectory information 132 is generated using the above model and constraint conditions, specifically, trajectory information connected as a straight line from the start point S to the end point E is generated as an initial value, and the model is based on the initial trajectory. Then, an optimization calculation using the existing sequential quadratic programming method is performed based on the constraint condition, and x [k], τ [k] and the time interval T are obtained.

  Here, the weighting coefficient α is set to 0 in order to perform a comparison experiment between the conventional movement control based on the trajectory information pursuing only the shortest time and the movement control based on the trajectory information considering the vibration suppression of the present embodiment. The trajectory information pursuing only the shortest time and the trajectory information considering vibration suppression according to the present embodiment are generated, the respective trajectory information is converted into drive commands to drive the robot arm, and vibrations generated by movement and As a result of measuring the motor drive pulse, although there was almost no change in the time required for movement, as shown in FIG. 6, the vibration suppression of this embodiment was also considered compared to the conventional orbit information pursuing only the shortest time. The trajectory information has a smoother motor drive pulse, which is preferable for pursuing power saving, and the maximum vibration value is suppressed by at least about 20%. Togawakaru. In the figure, the maximum value of vibration when only the shortest time is pursued is shown as a reference value 1.

  In addition, using a strict motion characteristic such as defining a vibration model for each joint of a robot is not preferable for actual operation because the calculation amount becomes enormous, but a single mass − limited to linear motion as in this embodiment − By simplifying to the spring model, the convergence of the optimization calculation is improved, and the amount of calculation is reduced within a range that maintains the practicality. The time required to generate the trajectory information varies depending on the environment, but is only an example, but in some environments, the time can be reduced to about several tens to several hundreds of times compared to the conventional case.

As described above, the trajectory information generation device of the articulated robot arm type transport device according to the present embodiment is a transport target for the articulated robot arm type transport device 103 in which a plurality of links 134, 135, and 136 are rotatably connected. An apparatus for generating trajectory information 132 relating to a trajectory and a speed on the trajectory for linearly moving the object W from a desired start point S to an end point E. The accepting unit 21 accepts the start point information and the end point information, and optimization techniques so that the value of the evaluation function including time t _e it takes to move from the start point S to the end point E, based on the received information and the dynamic model of the conveying device 103 as one of the parameters is minimized And orbit information generating means 22 for generating orbit information 132 using the orbit information generating means 22. The orbit information generating means 22 conveys the conveyance object W as a dynamic model. It has set up model for limiting the linear movement of the operation of the end link 136 in advance, the simulated vibration caused by the movement of the end link 136 using the vibration characteristics of a predetermined conveying device sized to (x-x _a) calculates, included in the evaluation function the size of the calculated simulated vibrated (x-x _a) as one of the parameters, at least the travel time is the above two parameters t _e and the simulated vibration magnitude (x-x _The trajectory information 132 is generated so that the value of the evaluation function including _a ) is minimized.

Thus, the optimization techniques which the value of the simulated vibration magnitude evaluation function obtained by combining the (x-x _a) and travel time t _e caused by movement including sequential quadratic programming to minimize Since the trajectory information 132 is generated by using it, the movement time is minimized in consideration of vibration suppression, and the movement efficiency can be improved. Moreover, as a dynamic model of the transport apparatus 103, a model in which the operation of the tip link 136 that transports the transport object W is limited to a linear motion is set in advance, and this model is used. It is not strict with the optimization process, and it is possible to improve the convergence of the optimization calculation and reduce the calculation time required for generating the trajectory information 132.

  Furthermore, in the present embodiment, the trajectory information generation means 22 is imposed with a constraint that makes the direction φ of the distal link 136 constant, so that the calculation time required to generate the trajectory information 132 is reduced, while the load port and the like are reduced. It is possible to implement the loading / unloading operation.

In addition, in the present embodiment, the weighting coefficients representing the weights of the magnitude of the simulated vibration on the evaluation function in response to the change operation accepting changed via the reception unit 21 operation (x-x _a) and travel time t _e because it has a weighting changing means 24 for changing the alpha, by performing the change operation, the weighting coefficients representing the weights of the simulated vibration (x-x _a) and travel time t _e for the evaluation function alpha is changed, the vibration suppression or It is possible to change which of the travel time reductions is prioritized. Moreover, since the weighting coefficient α is a one parameter, the parameter setting is easy to handle.

  As mentioned above, although embodiment of this invention was described based on drawing, it should be thought that a specific structure is not limited to these embodiment. The scope of the present invention is shown not only by the above description of the embodiments but also by the scope of claims for patent, and further includes all modifications within the meaning and scope equivalent to the scope of claims for patent.

  For example, in this embodiment, in defining the vibration characteristic model of the robot arm, a single mass-spring model is formed by focusing on the tip of the arm. -You may define as a spring model and may employ | adopt another model as a model which considers a vibration. Further, in this embodiment, the start point speed and the end point speed are handled as 0, but the present invention can also be applied when these speeds are not 0.

  In addition, the trajectory information generation described above is applied to the transport apparatus 103 that moves the transport target W using the robot arm-type transport mechanism 131 in which a plurality of links 134, 135, and 136 are rotatably connected in series. However, it can also be applied to a transport device such as a parallel manipulator in which a plurality of links are connected in parallel. Each functional unit shown in FIG. 1 is realized by executing a predetermined program by a processor, but each functional unit may be configured by a dedicated circuit.

  The specific configuration of each unit is not limited to the above-described embodiment, and various modifications can be made without departing from the spirit of the present invention.

103 ... Conveying device 131 ... Conveying mechanism (articulated robot arm)
132 ... orbit information 134, 135, 136 ... link 21 ... receiving unit 22 ... orbit information generating means 24 ... weighting changing unit S ... starting E ... end point W ... object to be transported Pt ... trajectory _{t e} ... travel time _{(x-x a} )… Size of simulated vibration

Claims (3)

A device that generates trajectory information about the trajectory and the speed on the trajectory for linearly moving the object to be transported from a desired start point to an end point for an articulated robot arm type transport device in which a plurality of links are rotatably connected. ,
Receiving means for receiving start point information and end point information;
An optimization method that minimizes the value of the evaluation function including, as one of the parameters, the time required to move from the start point to the end point based on the information received by the reception unit and the dynamic model of the transfer device Using orbit information generating means for generating the orbit information using,
The orbit information generating means, operates in conjunction with our ku preset models are limited to linear movement of end link for conveying the conveyance object as the dynamic model, the linear motion as the vibration characteristic model of the transport device A single mass-spring model that is displaced only in the direction of is set in advance , the magnitude of the simulated vibration generated by the movement of the tip link is calculated using the vibration characteristic model , and the calculated magnitude of the simulated vibration is a parameter. Generation of trajectory information for an articulated robot arm type transport apparatus, wherein the trajectory information is generated so that the value of the evaluation function including at least the two parameters is minimized as one of the evaluation functions apparatus.   The trajectory information generation device of the articulated robot arm type transfer device according to claim 1, wherein the trajectory information generation means is imposed with a constraint condition that makes a direction of the tip link constant. The trajectory of the articulated robot arm type transfer device according to claim 1 or 2, further comprising weight changing means for accepting a change operation and changing a weight between a magnitude of simulated vibration and a moving time for the evaluation function according to the change operation. Information generator.

Images