The embodiments of the invention will be described below with reference to the accompanying drawings. In the drawings, similar parts are identified by the similar reference characters. The reduction of the drawings have been appropriately made for the sake of clarity.
As shown in
As shown in
The number of the shared RAMs #1-#n and the number of the digital servo circuits C1-Cn correspond to the number of motors M1-Mn of the robot mechanism section RM of the robot. The motors M1-Mn are used to drive articulated shafts of the robot respectively. A typical hexaxial vertical articulated robot includes six articulated shafts J1-J6. Therefore, the number of motors, shared RAMs, digital servo circuits are six respectively.
As shown in the drawing, the teaching control panel TP includes a liquid crystal display 15 and a keyboard 16, which are used to input and change data in the above described program 3 and to input and change relevant setting values for the above-described programs.
Then, in the item “Softness” of the setting screen, the operator makes settings to place a “virtual spring” or “virtual damper” for X, Y and Z directions. On the actual screen, the operator may simply enter “spring” or “damper” through the keyboard 16. X, Y and Z directions correspond to directions of coordinate axes of a predetermined orthogonal coordinate system Σ0 (see
The operator then enters a proportionality constant in association with the set spring or damper. In the setting screen shown in
The numeric values, when “spring” is set, correspond to proportionality constants Kx, Ky and Kz of a virtual spring on the orthogonal coordinate system Σ0 respectively. The numeric values to be entered when “damper” is set correspond to proportionality constants Dx, Dy and Dz of a virtual damper on the orthogonal coordinate system Σ0. In the setting screen shown in the drawing, a virtual spring or virtual damper may be set around X, Y and Z axis to enter numeric values likewise.
In the setting screen shown in
When the operator enters a starting position in an item “Starting Position” of the setting screen shown in
Specifically, when the operator enters a starting position, the robot controller RC calculates Jacobian matrix J as shown below.
In the above formula, the first row corresponds to soft X direction, and the second row corresponds to soft Y direction. Then, the robot controller RC selects, from the first row (∂x/∂J1, ∂x/∂J2, ∂x/∂J3, ∂x/∂J4, ∂x/∂J5, ∂x/∂J6) of Jacobian matrix J, a column having the maximum absolute value. When such column is ∂x/∂J3, the robot controller RC sets the gain reduction setting of a corresponding shaft J3 to be “Enabled” automatically (refer to the item “Gain Reduction Setting” in
Likewise, the robot controller RC selects a column having the maximum absolute value from the second row (∂Y/∂J1, ∂Y/∂J2, ∂Y/∂J3, ∂Y/∂J4, ∂Y/∂J5, ∂Y/∂J6) of Jacobian matrix J. When the appropriate column is ∂Y/∂J1, the robot controller RC sets the gain reduction setting of a corresponding shaft J1 to be “Enabled” automatically (refer to the item “Gain Reduction Setting” in
The articulated shaft's selection as described above may be done by an apparatus different from the robot controller RC shown in
After the articulated shaft has been set, the percentage of reduction in position gain and/or speed gain of the articulated shaft is set. Specifically, the Position gain ratio αp (=after-reduction position gain Kps/normal position gain Kp) and Speed Gain Ratio αv (=after-reduction speed gain Kvs/normal speed gain Kv), which are each an item showing the ratio of a before-reduction gain and an after-reduction gain, are entered (0≦αp≦100, and 0≦αv≦100). In
Also, the percentage at (0≦αt≦100) to be added to a torque command to (described later), from the correction torque Ti (described later) for each of the articulated shafts J1 and J3 is entered. In
In the case where the robot controller RC has the function of switching between soft control gains Kp′ and Kv′ and normal control gains Kp and Kv, ratios of the after-reduction gains Kps and Kvs and the soft control gains Kp′ and Kv′ may be entered as αp and αv, respectively. In this connection, it should be noted that there are the following relationships between the soft control gains Kp′ and Kv′ and the normal control gains Kp and Kv:
Kp>Kp′, and
Kv>Kv′.
The operator may enter the settings of the ratios and percentages through the keyboard 16. However, the robot controller RC may decide the settings based on a predetermined rule and display only the result of the decision on the liquid crystal display 15. For example, predetermined constants e.g. 10%, 10%, and 0% may be set automatically as the position gain ratio αp and the speed gain ratio αv, and the percentage at respectively ((αp, αv, αt)=(10%, 10%, 0%)). In this case, it should be understood that even an inexperienced operator can adequately operate the robot.
The robot R is normally operated by the robot controller RC. When the orthogonal soft float is desired, the operator sets the orthogonal soft float function Enabled through the teaching control panel TP. The case where the orthogonal soft float is performed refers to the case where the robot R has to operate softly following the movement of the molded article W1 in X direction and operate rigidly in Z direction for the purpose of taking out the molded article W1 as shown in
In the case where the robot controller RC has the function of switching between the soft control gains Kp′ and Kv′ and the normal control gains Kp and Kv, at the time of starting the orthogonal soft float, the position gain Kp and speed gain Kv of all the articulated shafts are changed to the position gain Kp′ and the speed gain Kv′ for soft control respectively.
In the host CPU 11, the process according to the flowchart shown in
Subsequently, the positional deviations are multiplied by parameters (Kx, Ky, Kz) of a virtual spring entered on the setting screen shown in
Then, at Step 102, orientation data of the robot is used to transform forces (Fx, Fy, Fz) on the orthogonal coordinate system Σ0 into forces (Fx(n+1), Fy(n+1), Fz(n+1)) on the tool coordinate system Σn+1.
After that, at Step 103, a correction torque Ti (i=1-n) for each shaft is calculated from the current position of the robot and forces (Fx(n+1), Fy(n+1), Fz(n+1)) on the tool coordinate system Σn+1 according to the Newton-Euler method (refer to the block 24 in
Then, at Step 104, the correction torques Ti thus obtained for the articulated shaft are stored in the respective shared RAMs #1-#n. The above process is performed by the host CPU 11 of the robot controller RC.
Then, the process according to the flowchart shown in
Then, it is judged at Step 112 whether or not the articulated shaft Ji corresponding to one digital servo circuit Ci is an articulated shaft, which has been subjected to “Gain Reduction Setting” in
At Step 113, the position gain reducing means 41 multiplies the normal position gain Kp or Kp′ by the position gain ratio αp (see
The block 31 shown in
At Step 113, the above-described after-reduction gains Kps and Kvs are used in the blocks 31 and 32 respectively. As can be seen from
Then, as shown in Step 114 the correction torque reducing means 43 multiplies the correction torque Ti by the above-described percentage αt. In
Then, the correction torque after alteration (zero in this case) is added to the torque command tc to make a new torque command tc′. A driving current corresponding to the new torque command tc′ is supplied to the motor Mi, and then the processing is terminated.
As described above, in the invention, the position gain reducing means 41 decreases the position gain Kp for a certain articulated shaft to the after-reduction gain Kps, and the speed gain reducing means 42 decreases the speed gain Kv for the same articulated shaft to the after-reduction speed gain Kvs. Further, in the invention, the correction torque reducing means 43 decreases the correction torque Ti for performing a control to realize a virtual spring. Due to the foregoing, the invention can suppress an impediment for a smooth action in direction in which the virtual spring is made to act. In other words, according to the invention, an operation in a softening direction can be performed with a small force.
As described with reference to
However, according to the invention, the gain reducing means 41 and 42 can suppress the torque command tc, and additionally the correction torque reducing means 43 can reduce the correction torque Ti to be added. Thus, in the invention, the torque T3 itself can be suppressed by the gain reducing means 41 and 42 and the correction torque reducing means 43. Therefore, the invention can overcome obstacles to the softness in X direction, and therefore a small amount of force is adequate for an operation in the −X direction.
Referring to
Then, at Step 116 the correction torque Ti is added to the torque command tc directly to create a new torque command tc′. In other words, in this case, the gain ratios αp and αv and the percentage at are not used. After that, a driving current corresponding to the new torque command tc′ is supplied to the motor Mi, and then the processing is terminated.
Meanwhile, in the case of correcting the torque command on the orthogonal coordinate system Σ0, there is a problem that the friction of the articulated shafts at a point of time is different from each other, and therefore one articulated shaft can be operated immediately, while a slight delay arises for another articulated shaft before the articulated shaft begins to be operated actually, and consequently the operating direction gradually deviates from the orthogonal direction which has been set.
To solve the problem, it is preferable that a preliminary operation, in which the articulated shaft is slightly operated in its soft direction according to the normal control, is performed to accumulate the resultant torque in the integrator 33 of the speed loop. Thus, in the integrator 33, the torque in a direction for compensating the friction, is accumulated. Specifically, before the soft control is started, an operator orders a position control operation for performing the preliminary operation. The friction depending on the articulated shafts can be compensated by such preliminary operation. In other words, the friction depending on the articulated axis is made substantially uniform. Therefore, the deviation of the operating direction from the orthogonal direction can be suppressed by performing such preliminary operation.
Such a preliminary operation for compensating for the friction is not necessarily ordered by an operator. Even if an operator does not order the position control operation, in the case where a soft control starting order is issued, an interpreter in the controller is made to create and execute the preliminary operation, then start the soft control. However, in this case, the direction and distance for the preliminary operation must be decided in advance.
Meanwhile, there may be cases where the distance required for the preliminary operation cannot ensured because of limitations of the layout of the robot and peripheral apparatuses. In such cases, a quantity corresponding to the torque accumulated in the integrator 33 during the preliminary operation has been set in advance. Then, the torque may be added to an output of the speed loop when the soft control is executed. In other words, the preliminary operation does not have to be actually performed, and even if the distance required for the preliminary operation cannot be ensured, the similar effect as described above can be achieved.
In this connection, such method to compensate the friction is limited to the case where the direction of an operation in the soft control is known in advance. In the case where the direction of an operation in the soft control is unknown, a periodic torque in the form of a sine wave, a triangular wave, a rectangular wave or the like, which has an amplitude as large as that of static friction approximately is added. Under such a condition, in the case where an external force is applied from one direction, when the direction of the external force coincides with the direction of the periodic torque, a torque resulting from a combination of the external force and periodic torque is larger than the static friction, so that it becomes possible to start an operation in that direction. In other words, even in the case where the direction of an operation is unknown during the soft control, the friction depending on the articulated shaft can be compensated to suppress the deviation of the direction of an operation from an orthogonal direction.
In the case where the orthogonal soft float is performed on a working coordinate system, or in the case where the direction of the preliminary operation is specified on a coordinate system, it is necessary to set the coordinate system in advance. For that reason, it is expected that a parameter on the coordinate system is input directly. Otherwise, two positions concerning the tool front end point TCP may be taught, whereby the direction along a straight line connecting the two points may be set as one axis (e.g. Z axis) of the working coordinate system of the orthogonal soft float, or as the direction of the preliminary operation. Thus, it is possible for an operator to specify a direction on the coordinate system intuitively and clearly.
While it is not described in detail, in the case where a virtual damper is set using damping parameters Dx, Dy and Dz instead of the virtual spring on the setting screen shown in
While various kinds of parameters are set by entering the screen in the case shown in
The invention has been described in the typical embodiments. However, it should be understood to those skilled in the art that the above-described and other various changes, omissions and additions may be made without departing from the scope of the invention.
Number | Date | Country | Kind |
---|---|---|---|
2006-259268 | Sep 2006 | JP | national |